本文深入解析Gmapping算法的实现细节，包括主要函数的功能与调用流程。阐述了粒子滤波与扫描匹配等关键技术，并介绍了算法参数及其作用。
激光SLAM之Gmapping（2）算法分析解读
Gmapping的程序框架是依托Open_slam，该框架主要分成slam_gmapping和openslam_gmapping。在slam_gmapping可以从Lasercallback出发，作为整个框架的起点，Lasercallback函数在slam_gmapping.cpp文件中。
文章目录
主要函数及调用关系
废话不多说，先上函数调用图
①InitMapper函数
如果是首次调用lasercallback函数，则进入InitMapper。函数在同样的.cpp文件中。激光雷达测得数据数据，是在激光雷达为坐标系的数据。在InitMapper设置一个比激光雷达Z轴上高一个单位的一个点，用这个点来判断激光雷达是否发生了倾斜。接下来就是根据激光雷达的安装位置（正反安放）。初始化并设置一些参数。
②addScan函数
成功将测量值加入之后，在Lasercallback下面就是两个坐标系的变换。在addscan函数是主要函数的。在该函数了成功获取到里程计位姿后，根据激光雷达的安装方式，对角度进行修改,然后将ROS的激光雷达采集的信息转换成gmapping能看懂的格式,设置和激光数据时间戳匹配的机器人的位姿。调用processscan函数。
③Processscan函数
processscan函数在gridslamprocessor.cpp中，首先获取当前的位姿，然后在从里程计运动模型获取位姿，这里调用了drawFromMotion函数，这个函数在motionmodel.cpp中，drawFromMotion函数中的sample函数是形参作为方差，均值为0的高斯分布。sample函数是数值分析所近似生成的高斯分布，具体的函数实现在stat.cpp中。当前位姿与上一次位姿做差，计算做累计角度偏差和位移偏差。利用激光雷达测得距离做得分处理。非首帧调用scanMatch，upDateTreeWeight ,resample。首帧则调用invalidActiveArea, computeActiveArea, registerScan.
④scanMach
该函数在gridslamprocessor.hxx中，对粒子的最优位姿进行计算：optimize，该函数在scanmacher.cpp；计算粒子最优位姿后，重新计算粒子的权重；粒子的权重由粒子的似然表示的，计算出来最优的位姿后，进行了地图的扩充。Optimize首先计算当前位置的得分，调用score函数，score函数在scanmacher.h。当前得分与上一次的得分差，要减少搜索步长，得到周围的方向里面最好的一个位姿和对应的得分。返回最优额位置和得分。计算当前的位姿和初始位姿的区别，区别越大增益越小。增益的计算odo_gain*=exp(-m_angularOdometryReliability*dth)，-m_angularOdometryReliability为角度里程计的依赖，就是相信传感器传来的数据程度。dth为角度变化量。线性距离也是这样的。计算当前位姿的角度和初始角度的区别，如果里程计比较可靠的话，那么进行匹配的时候就需要对离初始位姿比较远的位姿施加惩罚得分=增益得分。
⑤Score
score函数该函数先将激光雷达的坐标转换到世界坐标，先转换到机器人的坐标系，然后转到世界坐标。激光雷达击中到某一点，沿着激光方向的前一个点必是未击中的，得到击中点的栅格和前一个点的栅格。如果搜索出最优最可能被这个激光束击中的点，计算得分并返回该得分。
⑥Likelihoodandscore
likelihoodandscore与score其类似。回到scanmatch函数，invalidateActiveArea和computeActiveArea，UpdateTreeweight。UpdateTreeweight在gridslamprocessor_tree.cpp。这里主要是更新权重，在这里调用normalize，然后调用resetTree把所有的粒子的所有轨迹清零，最后调用propagateweights。在propgateweight更新归一化的粒子权重。因为每一个粒子的路径都是从叶子节点开始的，得到了叶子节点，就得到了路径，所以叶子节点的子节点累计权重就等于每个粒子的权重。
⑦Resample
该函数在gridslamprocessor.hxx。首先是备份老的粒子的轨迹，即保留叶子的节点。然后是需要重采样还是不需要重采样，如果不需要重采样，则权值不变。只为轨迹创建一个新的节点，每个粒子更新地图。当有效值小于阈值的时候需要重采样，通过resampleIndexes提取到需要删除的粒子。删除粒子后，保留当前的粒子并在保存的粒子的节点里新增一个节点。删除要删除粒子的节点，保留的粒子进行数据更新，将每个粒子的设置同一个权重。最后更新一下地图。
⑧InitMapper
该函数是激光雷达测到数据是在激光雷达坐标系下测的，在Init-Mapper设置一个比激光雷达高一米的点用来判断激光雷达是否倾斜，根据激光雷达排放位置对角度进行调整。设置参数和初始化参数。
算法结构
输入： S t − 1 S_{t-1} St−1前一时刻的粒子集信息
Z t Z_t Zt最新时刻的观测数据
U t U_t Ut最新的里程计数据
求解： S t S_t St当前时刻的粒子集信息
初始化 X V t = XV_t= XVt=初始化 X V t XV_t XVt
For 所有 S t − 1 i ∈ S t − 1 S^i_{t-1}\in S_{t-1} St−1i∈St−1执行
提取相关位姿信息 X t − 1 i X^i_{t-1} Xt−1i，权值信息 W t − 1 i W^i_{t-1} Wt−1i，对应地图信息 m t − 1 i m^i_{t-1} mt−1i
//执行扫描匹配
x v t i = x v t − 1 i ⨁ u t xv^i_t=xv^i_{t-1}\bigoplus u_t xvti=xvt−1i⨁ut根据控制信息估计移动机器人位姿信息
x v ^ t i = a r g m a x p ( x ∣ m t − 1 i , z t , x v t i ) x\hat{v}^i_t=arg\,max p(x\mid m^i_{t-1},z_t,xv^i_t) xv^ti=argmaxp(x∣mt−1i,zt,xvti) 通过估计出的位姿，结合当前观测信息与前一时刻的地图进行扫描匹配，求出匹配程度最高的位姿信息。
if x v ^ t i = f a i l u r e t h e n x\hat{v}^i_t=failure\,then x
最低0.47元/天 解锁文章
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
