slam-gmapping 源码阅读2.md

最新推荐文章于 2025-07-23 15:41:49 发布

Tomas_yuan

最新推荐文章于 2025-07-23 15:41:49 发布

阅读量1.5k

点赞数

CC 4.0 BY-SA版权

文章标签： slam ros gmapping

本文链接：https://blog.youkuaiyun.com/m0_37350344/article/details/80329928

本文深入探讨了基于栅格地图的FastSLAM算法(RBPF)实现细节，包括粒子结构、地图更新机制、重采样原理等内容。通过具体步骤解析算法流程，并介绍了关键函数如score计算、scanMatch匹配及optimize优化等。

```

Todo: 1）理解路径存储结构，以及粒子结构？

2）map格式？map.cell.update怎么实现的

3）重采样的原理？

4）NDT 计算概率（得分）

接下来涉及到底层:

GridSlamProcessor

功能: 实现了基本的基于栅格地图的FastSlam算法,RBPF．每个粒子携带地图以及机器人位姿
实现过程：每次收到激光数据，里程计数据，粒子的机器人位姿根据运动模型更新．接下来机器人的位姿用于初始化扫描匹配算法,并根据每个粒子地图进行校正. scanMatcher对每个粒子作局部的优化.
tip: 当然滤波器只有当机器人运动超过一定阈值才进行更新过程

成员	内容	功能
TNode	位姿,父节点,子节点数,权值	定义了一个节点树,存储轨迹
Particle	地图,位姿,权值,在树中的节点	定义了滤波器的粒子
初始化,设置参数函数	运动模型参数,更新距离,更新周期等.匹配参数
核心处理函数	processTruePos() processScan()
私有函数	scanMatch normalize resample updateTreeWeights	归一化粒子权重是否进行重采样更新树的权重

processScan()

1.对于每一个粒子，根据里程计模型推算机器人当前时刻位姿（t-1位姿，t里程计位姿，t-1里程计位姿） drawFromMotion
2.判断机器人的平移和旋转是否合理
3.处理激光数据（机器人运动一段距离或者到达一定时间）  scanMatch  
4.resample，满足条件重采样，删除旧粒子，产生新粒子,register_scan。不满足重采样条件，就register_scan

scanMatch(const double* plainReading)
对于每一个粒子都进行以下操作

           
          for every particle Pk in particles
         
              do
         
                  optimize(Pk) 局部优化粒子位姿 it->pose
         
                  likelihoodAndScore(Pk) 计算粒子权重
         
                  更新粒子权重                       it->weight
         
                  computeActiveArea (Pk) 计算有效区域，更新地图 it->map
         
           end

optimize 计算最优的粒子，粒子进行局部优化。
optimize 调用了 score 这个函数（计算粒子得分）。原理就参考《Probabilistic Robot》一书的P130 页
- score 函数:首先计算障碍物的坐标phit，然后将phit转换成网格坐标iPhit 。计算光束上与障碍物相邻的非障碍物网格坐标pfree,pfree由phit沿激光束方向移动一个网格的距离得到，将pfree转换成网格坐标ipfree（增量，并不是实际值）在iphit 及其附近8个（m_kernelSize:default=1）栅格（pr,对应自由栅格为pf）搜索最优可能是障碍物的栅格。
- 最优准则： pr 大于某一阈值，pf小于该阈值，且pr栅格的phit的平均坐标与phit的距离bestMu最小。 (算法第七步)
- 得分计算： s+=exp(-1.0/m_gaussianSigma* bestMu* besMu) 参考NDT算法,距离越大，分数越小，分数的较大值集中在距离最小值处，符合正态分布模型至此 score 函数结束并返回粒子（currentPose）得分，（算法第八步）
- 然后回到optimize函数： optimize 干的事就是 currentPose 的位姿进行微调，前、后、左、右、左转、右转共6次，然后选取得分最高的位姿，返回最终的得分
  如果得分小于最小值，则认为匹配失败，使用里程计位姿
likelihoodAndSocre 计算粒子的权重（l）

与score函数基本相似，除了返回s外，还考虑l值。当忽略部分激光束后，则利用m_likelihoodSigma计算f，若匹配成功，l为f,否则 l=noHit
invalidateActiveArea computeActiveArea 计算可活动区域
//set up the selective copy of the active area //by detaching the areas that will be updated computeActiveArea 用于计算每个粒子相应的位姿所扫描到的区域计算过程首先考虑了每个粒子的扫描范围会不会超过子地图的大小，如果会，则resize地图的大小然后定义了一个activeArea 用于设置可活动区域，调用了gridLine() 函数，Bresenham算法来计算非障碍物格点的集合。

normalize()
- 归一化
- 求有效粒子数
resample(const double* plainReading, int adaptSize, const RangeReading* reading)

1.当有效粒子数小于阈值进行重采样
2.删除旧粒子，加入新粒子，将粒子位姿以及激光数据更新到局部地图？`registerScan()`
3.若是无需重采样，则建立一个新节点，并加入到树中

registerScan()
根据Bresenham算法确定激光束扫描过的单元格，所以扫描过的单元格被扫描过的次数加1，激光束末端对应的单元格障碍物标记次数加1，并累加障碍物坐标acc.x和acc.y，则障碍物的。最后单元格是障碍物的概率p = n / visits。

---

bresenham算法
bresenham直线算法是用来描绘由两点所决定的直线的算法，它会算出一条线段在n维光栅上最接近的点。源码中的incr2不是很能理解。
- 算法原理：由直线的斜率选择在x方向或y方向上每次递增（减）1个单位，决定另一个变量递增（减）量为0或1. 取决于实际直线于最近栅格点的距离，这个距离的最大误差为0.5;

实现方法：

           
              x=x1,y=y1;
         
              dx=x2-x1;dy=y2-y1;
         
              error=dy/dx-0.5;
         
              for i=1 to dx
         
                   write(x,y,value);
         
                  if (error>=0) then
         
                         y=y+1;
         
                         error=error-1;
         
                  end if
         
                  x=x+1;
         
                  error=error+dy/dx;
         
                  i=i+1;
         
              end

因为只用到error的符号，可作以下替换Nerror=2* error *dx,这样所有的计算均为整数运算，避免了除法

           
          %%start point and end point ,in first xiangxian
         
          x1=0;y1=0;
         
          x2=9;y2=6;
         
          %%plot set
         
          axis([x1 x2 y1 y2]);
         
          axis equal; 
         
          figure(1);
         
          line([x1,x2],[y1,y2],'color','g');
         
          hold on;
         
          grid on;
         
          %%inital state
         
          x=x1;y=y1;
         
          dx=x2-x1;dy=y2-y1;
         
          Nerror=2*dy-dx;
         
          myline=zeros(2,dx);
         
          %%decide the next point
         
          for i=1 : dx
         
              myline(1,i)=x;myline(2,i)=y;
         
              plot(x,y,'ro');
         
              pause(1);
         
              if (Nerror>=0) 
         
                     y=y+1;
         
                     Nerror=Nerror+2*(dy-dx);
         
              end 
         
              x=x+1;
         
              Nerror=Nerror+2*dy;
         
          end