ROS小车（SLAM+物体追踪）

属于交通运输工程设计的论文，包含SLAM与物体追踪这两个方向

---

ROS智能车设计

---

一. 系统描述

1. 车轮子

单轮平衡式结构，能量利用率高，但转弯的时候需要倾角高速运动，很难控制

差速转向平衡两轮式：利用率高，上下坡的时候智能车可以保持垂直，
高速情况下转向很难控制。

全向三轮结构，可以全向移动原地转向，轮胎利用率不高，成本高。

阿克曼转向式：一般的汽车转向方式，转向摩擦力小，但转弯半径大不能原地转向。
全向四轮：比全向三轮结构多加一个轮子后动力得到增强，但能量利用率不高、成本高。
麦克纳姆四轮：可以全向移动，原地转向，摩擦小移动精度高，成本贵。

2.硬件布局

下位机：底盘运动控制系统主要组成部分有 STM32、IMU 、电机、编码器等，智能车左右轮的移动速度是通过编码器来测量，IMU 模块主要作用是采集智能车在三维空间当中移动的姿态、方向和加速度等信息，通过 STM32 进行 PID 控制。实现智能车的精准移动。
上位机：嵌入式计算系统，主要由 Raspberry Pi 3+、kinect 深度摄像头、激光雷达组成，Raspberry Pi 3B+ 搭载主频为 1.4GHz、内含 ARM Cortex-A53 核心的处理器，配备 1G 内存和多种外设。通过上层嵌入式计算系统完成外部传感器信息获取、图像处理、数据处理、运动决策、实现视觉跟踪算法和 SLAM 算法并且根据获取信息发送运动控制指令。远程的笔记电脑主要作用是可以实现智能车远程控制与监测。

3.基于ROS系统，小车的节点设计

节点说明：
智能车底盘（底盘运动控制节点）：作为运动执行器，根据系统经过处理的数据，订阅发布的速度控制消息控制底盘运动；
视觉跟踪算法：主要是将深度相机获取的数据经过处理，发布到话题中由底盘运动控制系统订阅后控制底盘运动；
同步建图节点（SLAM）：将激光雷达、底盘中 IMU 和编码器里程计等传感器的信息经过处理实时建立环境地图；
深度相机：采集外部环境中图像的 RGB 和深度信息；
激光雷达：作为环境感知的传感器同样需要在 ROS系统中安装驱动；
键盘控制：主要作用是通过笔记本电脑远程控制智能车运动。

---

二.小车的SLAM

1.算法整体设计

激光 SLAM :使用的传感器和建立的地图类型
激光 SLAM 主要的传感器:轮式里程计、惯性测量单元（IMU）、激光雷达，其中轮式里程计主要算距离，惯性测量单元（IMU）计算角度，最终和激光数据合在一起进行 SLAM 的过程。
地图类型:覆盖栅格地图和点云地图，覆盖栅格地图的优点是他对环境进行了很鲜明的区分，可以清楚的知道什么地方是障碍物，什么地方是可以通行的区域。
SLAM拆分：一个是前端一个是后端，前端是构图的过程，包含帧间匹配和回环检测，当形成一个图后，如果地图中出现误差后地图一定是不一致的；图优化要使得这个误差最小化。里程计测量的位姿可以认为是预测位姿，回环后扫描匹配的位姿是观测的位姿，使得预测和观测之间的差最小化，得到一个位姿来解释观测值，实现了误差的去除。

图优化 SLAM 算法 Cartographer 由数据获取（Input Sensor Data）、局部建图（Local SLAM）、全局优化（Gloab SLAM）三部分组成。传感器输入数据包含激光雷达数据、底盘里程计数据、IMU 数据。激光雷达原生数据经过体素滤波器降采样后，减少了数据数量并且保证了障碍物特征，再经过自适应体素滤波器后进行下一步的扫描匹配，作为观测位姿。上一次前端匹配完成的位姿、里程计数据和经过预处理后的 IMU数据输入到姿势外推器中，通过里程计增量得到线速度和角速度，对 IMU 经过预积分后得到线速度和角速度，通过位姿变化增量得到线速度和角速度。对下一次前端匹配的初始位姿估算方法为，对位移位姿的变化如果有里程计的数据就选择里程计的数据，如果没有里程计的数据就选择位姿变化增量，对于角度姿态变化如果有 IMU 的数据就选择 IMU 的数据。根据传感器特性，在短时间内 IMU 的角度增量更准，里程计的位移增量更准。在局部建图部分，外推器初始位姿经过扫描匹配得到最优位姿，扫描匹配就是非线性优化问题，通过建立最小二乘问题计算出当前时刻相对于之前一段时间存在的最优位姿。得到最优位姿后经过运动滤波器后插入子图中，运动滤波器的作用是为了防止每个子图中插入太多扫描帧数据。一旦扫描匹配器生成了新的位姿后，比较位姿和上次位姿的变化量，调用运动滤波器，如果变化很小则删除本次的扫描。当变化大于某个距离、角度或时间阈值后，才会将扫描帧插入到当前的子图中。每当获得一次扫描帧的数据后，便与当前最近建立的子图进行扫描匹配，使这一帧的扫描帧数据插入到子图中的最优位置。在不断插入新数据帧的同时该子图也得到了更新。一定量的数据组合成为一个子图，当不再有新的数据帧插入到子图时，就认为这个子图已经创建完成，接着会去创建下一个子图。Cartographer 理论认为连续的多帧扫描具有很小的累积误差，因此这样构建子图作为地图的一个小单元。通过扫描匹配得到的位姿估计在短时间内是可靠的，但是时经过很长时间后会有累积误差。因此 Cartographer 应用了回环检测对累积误差进行全局优化。插入子图的扫描帧与之前已创建完成的子图中所有扫描帧都会用来做回环检测的匹配，如果当前的扫描帧和所有已创建完成的子图在距离上足够近，那么通过某种匹配策略就会找到该闭环。如果得到一个足够好的匹配，则表示回环检测已经结束了。接下来要根据当前扫描帧的位姿和匹配到得最接近的子图中的某一个位姿，对所有的子图中的位姿进行优化，最终得到地图。

2.本车SLAM:cartographer的结构

Cartographer 由数据获取（Input Sensor Data）、局部建图（LocalSLAM）、全局优化（Gloab SLAM）三部分组成。
传感器输入：激光雷达数据、底盘里程计数据、IMU 数据。
激光雷达原生数据经过体素滤波器降采样后，减少了数据数量并且保证了障碍物特征，再经过自适应体素滤波器后进行下一步的扫描匹配，作为观测位姿。
上一次前端匹配完成的位姿、里程计数据和经过预处理后的 IMU数据输入到姿势外推器中，通过里程计增量得到线速度和角速度，对 IMU 经过预积分后得到线速度和角速度，通过位姿变化增量得到线速度和角速度。
对下一次前端匹配的初始位姿估算方法为，对位移位姿的变化如果有里程计的数据就选择里程计的数据，如果没有里程计的数据就选择位姿变化增量，对于角度姿态变化如果有 IMU 的数据就选择 IMU 的数据。根据传感器特性，在短时间内 IMU 的角度增量更准，里程计的位移增量更准。
局部建图部分，外推器初始位姿经过扫描匹配得到最优位姿，扫描匹配就是非线性优化问题，通过建立最小二乘问题计算出当前时刻相对于之前一段时间存在的最优位姿。得到最优位姿后经过运动滤波器后插入子图中，运动滤波器的作用是为了防止每个子图中插入太多扫描帧数据。一旦扫描匹配器生成了新的位姿后，计算这次位姿和上次位姿的变化量，调用运动滤波器，如果变化很小则删除本次的扫描。当变化
大于某个距离、角度或时间阈值后，才会将扫描帧插入到当前的子图中。每当获得一次扫描帧的数据后，便与当前最近建立的子图进行扫描匹配，使这一帧的扫描帧数据插入到子图中的最优位置。在不断插入新数据帧的同时该子图也得到了更新。一定量的数据组合成为一个子图，当不再有新的数据帧插入到子图时，就认为这个子图已经创建完成，接着会去创建下一个子图。Cartographer 理论认为连续的多帧扫描具有很小的累积误差，因此这样构建子图作为地图的一个小单元。
通过扫描匹配得到的位姿估计在短时间内是可靠的，但是时经过很长时间后会有累积误差。因此 Cartographer 应用了回环检测对累积误差进行全局优化。插入子图的扫描帧与之前已创建完成的子图中所有扫描帧都会用来做回环检测的匹配，如果当前的扫描帧和所有已创建完成的子图在距离上足够近，那么通过某种匹配策略就会找到该闭环。如果得到一个足够好的匹配，则表示回环检测已经结束了。接下来要根据当前扫描帧的位姿和匹配到得最接近的子图中的某一个位姿，对所有的子图中的位姿进行优化，最终得到地图。

3.SLAM解决的技术问题

1.前提：非线性最小二乘的原理

f(x)=z
给定系统，描述系统的状态方程， x 是该系统的状态向量表示智能车的位姿,z 是一个非线性函数, 是系统的观测值可以通过传感器直接观测，可以通过函数得到一个预测值，在得到预测值和观测值后最小化他们之间的差。给定系统中 n 个混有噪声的传感器真实的观测
值，估计状态向量 x，使得其经过非线性函数，映射后的预测值与观测值的误差最小。

2. 里程计的标定

里程计存在系统误差和随机误差。
随机误差无法消除
系统误差可以通过标定进行消除。
系统误差是确定的误差，比如当设计智能车的时候，根据智能车的图纸进行装配后，轮子的间距和轴距会存在装配误差，智能车的车轮理论上的设计值与实际值存在误差，所以需要对系统误差进行标定。（里程计的标定，对于可以消除的误差进行处理）
在标定的时候采用比较通用的线性最小二乘法，线性最小二乘标定不需要关心底层的细节就可以进行标定。线性方程组 Ax=b。

3.运动去畸变

4.激光的前端配准

---

三 、导航模块设计