Gmapping学习总结

Gmapping学习总结

Gmapping的程序框架是依托Open_slam,该框架主要分成slam_gmapping和openslam_gmapping。在slam_gmapping可以从lasercallback出发,作为整个框架的起点(虽然有main函数,main函数在main.cpp中,从这里出发不影响这个流程的分析)。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;
gmapping是基于FastSlam,FastSlam是将Slam分解成机器人定位和建图的问题,而FastSlam的主要原理是粒子滤波。生动形象的介绍粒子滤波的例子为一群警犬追捕逃犯,网上有很多相关说明,这里不赘述了。主要就是说一下基本原理,粒子滤波是一种非参数滤波,粒子则是样本,利用这些粒子去近似后验分布,粒子越多,状态空间的子区域被样本填充的越密集。在FastSlam里,粒子滤波是对机器人的轨迹进行估计,然后分别为每一个粒子计算地图。粒子包含了机器人轨迹以及与之对应的环境地图,但是不对环境地图进行估计。有时候直接从目标函数提取样本不容易,因此我们会用通过建议分布去近似目标分布
在这里插入图片描述
当I是真为1,是假为0。在这里插入图片描述为x的粒子的权重。这反映了建议分布g(x)与目标分布f(x)的匹配程度。相似度越大,权重越大,重要性采样也就是根据建议分布样本匹配获取f(x)的样本。接下来将权重进行归一化处理,为重采样做准备。归一化之后,权重大的样本说明与目标分布匹配高是下次采样的重点,重新分布样本,在重点地区,重点查,将权重进行了0-1分布。权重大的粒子所占的比例就大,选中的机率也就大,这也就解决如何“查”重点地区的问题。这些就是粒子滤波的重要流程,这些在Processscan函数里有体现。
scanMach:scanMach,该函数在gridslamprocessor.hxx中,对粒子的最优位姿进行计算:optimize,该函数在scanmacher.cpp;计算粒子最优位姿后,重新计算粒子的权重;粒子的权重由粒子的似然表示的,计算出来最优的位姿后,进行了地图的扩充。Optimize首先计算当前位置的得分,调用score函数,score函数在scanmacher.h。当前得分与上一次的得分差,要减少搜索步长,得到周围的方向里面最好的一个位姿和对应的得分。返回最优额位置和得分。计算当前的位姿和初始位姿的区别,区别越大增益越小。增益的计算odo_gain*=exp(-m_angularOdometryReliability*dth),-m_angularOdometryReliability为角度里程计的依赖,就是相信传感器传来的数据程度。dth为角度变化量。线性距离也是这样的。计算当前位姿的角度和初始角度的区别,如果里程计比较可靠的话,那么进行匹配的时候就需要对离初始位姿比较远的位姿施加惩罚
得分=增益得分。
Score:score函数和likelihoodandscore函数都在scanmacher.h。这两个函数比较类似。score函数该函数先将激光雷达的坐标转换到世界坐标,先转换到机器人的坐标系。然后转到世界坐标。激光雷达击中到某一点,沿着激光方向的前一个点必是未击中的,得到击中点的栅格和前一个点的栅格。如果搜索出最优最可能被这个激光束击中的点,计算得分并返回该得分,得分公式为在这里插入图片描述,likelihoodandscore与其类似。
回到scanmatch函数,invalidateActiveArea和computeActiveArea,UpdateTreeweight。UpdateTreeweight在gridslamprocessor_tree.cpp。这里主要是更新权重,在这里调用normalize,然后调用resetTree把所有的粒子的所有轨迹清零,最后调用propagateweights。在propgateweight更新归一化的粒子权重。因为每一个粒子的路径都是从叶子节点开始的,得到了叶子节点,就得到了路径,所以叶子节点的子节点累计权重就等于每个粒子的权重。
Normalize:该函数在gridslamprocessor.hxx。增益为所有的粒子数。求出粒子中最大的权重,权重=在这里插入图片描述
权重=该权重/总和。有效率为 在这里插入图片描述
Resample:该函数在gridslamprocessor.hxx。首先是备份老的粒子的轨迹,即保留叶子的节点。然后是需要重采样还是不需要重采样,如果不需要重采样,则权值不变。只为轨迹创建一个新的节点,每个粒子更新地图。当有效值小于阈值的时候需要重采样,通过resampleIndexes提取到需要删除的粒子。删除粒子后,保留当前的粒子并在保存的粒子的节点里新增一个节点。删除要删除粒子的节点,保留的粒子进行数据更新,将每个粒子的设置同一个权重。最后更新一下地图。
resampleIndexes:该函数在particlefilter.h中,首先计算总的权重,计算平均权重值(interval),根据权值进行采样,target是0-1分布随机选取的一数值,当总权重大于目标权重的,记录该粒子的索引,target在加上一个interval。如果某个粒子的权重比较大的话,那么他肯定会被采样到很多次。
invalidateActiveArea和computeActiveArea在scanmacher.cpp中。computeActiveArea计算有效区域,通过激光雷达的数据计算出来哪个地图栅格应该要被更新了。
InitMapper 激光雷达测到数据是在激光雷达坐标系下测的,在InitMapper设置一个比激光雷达高一米的点用来判断激光雷达是否倾斜,根据激光雷达排放位置对角度进行调整。设置参数和初始化参数。

Gmapping学习总结_第1张图片

你可能感兴趣的:(Gmapping学习总结)