激光SLAM导航系列(二)SLAM与导航系统框架
激光SLAM系统框架
在激光SLAM系统中，Gmapping获取扫描的激光雷达信息以及里程计数据可动态的生成2D栅格地图。导航包则利用这个栅格地图，里程计数据和激光雷达数据做出适合的路径规划和定位，最后转换为机器人的速度指令。下图为激光SLAM系统的框架，方框里为传感器获得的数据或者生成的数据，椭圆里为ROS节点。
1、建图系统框架
（1） 数据输入
在ROS GMapping包中，获取激光和里程计数据传入openslam GMapping包中，为新一时刻的建图做准备。
（2）运动模型
根据t-1时刻的粒子位姿以及里程计数据，预测t时刻的粒子位姿，在初始值的基础上增加高斯采样的noisypoint。
（3）扫描匹配
对每个粒子执行扫描匹配算法，GMapping默认采用30个采样粒子。扫描匹配的作用是找到每个粒子在t时刻位姿的最佳坐标。为后面每个粒子权重更新做准备。如果此处扫描匹配失败，则粒子权重更新则采用默认的似然估计。
（4）建议分布
混合了运动模型和观测模型的建议分布，根据上一步扫描匹配获得的最佳坐标，围绕该坐标取若干位置样本（距离差值小于某阈值）计算均值与方差，使得当前粒子位置满足该均值方差的高斯分布。
（5）权重计算
对各个粒子的权重进行更新，更新之后还需进行归一化操作。注意：重采样前更新过一次，重采样后又更新过一次。
（6）重采样
使用Neff判断是否进行重采样（重采样频率越高，粒子退化越严重，即粒子多样性降低，导致建图精确度降低，有必要设定一个判定值改善粒子退化问题）。
（7）粒子维护地图
每个粒子都维护了属于自己的地图，即运动轨迹。该步骤执行的操作是更新每个粒子维护的地图。
（8）地图更新
在ros中进行地图更新。先得到最优的粒子（使用权重和 weightSum判断 ），得到机器人最优轨迹，地图膨胀更新。
2、导航系统框架
导航系统总结来看可以分为数据收集层（传感器数据收集）、全局规划层（global_planner）、局部规划层（local_planner）、行为层（结合机器人状态和上层指令给出机器人当前行为）、控制器层（与下位机通信）。
机器人使用navigation栈导航时，move_base这个模块负责整个navigation 行为的调度, 包括初始化costmap与planner, 监视导航状态适时更换导航策略等[监视导航状态、适时更换导航策略等]. 涉及到行为的控制, move_base 具体实现就是有限状态机, 定义了若干recovery_behavior , 指定机器人导航过程中出问题后的行为. 具体的逻辑流程如下：
（1）move_base首先启动了global_planner和local_planner两个规划器，负责全局路径规划和局部路径规划，通过costmap组件生成自己的代价地图(global_costmap和local_costmap)。
（2）通过全局路径规划，计算出机器人到目标位置的全局路线，实现的方法是基于栅格地图的cost搜索找最优。
（3）然后通过局部规划，负责做局部避障的规划，具体navigation中实现的算法是Dynamic Windows Approach，具体流程是：
1.由移动底盘的运动学模型得到速度的采样空间
2.在采样空间中, 计算每个样本的目标函数
3.得到期望速度, 插值成轨迹输出
（4）在机器人运行过程中，根据机器人的状态做出规划（唤醒规划器），操作（计算合法速度并发布），清理（recovery_behavior）等操作。