特龙智慧-GMapping

Posted on Edited on In Views: Symbols count in article: 8.6k Reading time ≈ 8 mins.

特龙智慧

​ 我在大二上学期购买了《概率机器人》一书，当时还没有学概率论，所以我看不懂（但是我大受震撼）。大二下的暑假曾经学了一段时间，但是没有深入，到第五章就结束了。直到现在，大四了，项目有这方面的需求了，才重新开始看特龙(Sebstian Thrun)这本口碑很好的书。尽管这本书是2006年出版的，其中的很多思想在现在的我看来，都还很有指导意义。非常后悔，自己没能在之前花精力啃下这本SLAM以及移动机器人著作。

​ GMapping（以及相关的粒子滤波SLAM方法）或多或少都有他的参与，最近也刚好读了相关的论文，并且仔细研读了其代码（OpenSLAM上的💩山，literally），故我把这些笔记整理成了一篇文章：

Figure 1. 这人有一个以自己名字命名的实验室，还曾拒绝过出任Google副总裁...

II. GMapping 论文分析

2.1 大致思想

​ gmapping是一篇基于粒子滤波的配准工作，其主要创新点只有一点：就是得到了一个改进的粒子滤波建议分布（improved proposal distribution），这篇文章既没有涉及到回环检测、后端优化，也没有深入分析前端的配准算法。其主要贡献就是：改进粒子滤波在某些传感器上的表现。

​ 我们考虑激光雷达，一般来说，激光雷达的测距精度很高，在配准算法具有很好的表现情况下时，可以认为配准也有比较好，配准结果噪声较小。因为粒子滤波的主要思想是：

  • 我先建模一个易于获取的建议分布（proposal）
  • 再建模一个【近似的】建议分布与目标分布之间的差别w（权重）（根据贝叶斯理论，一般是乘性的因子）

​ 可见，如果目标分布（贝叶斯后验）很好获得，那么我们可以直接将目标分布当作建议分布，并且使得这差别权重都相等，为1。但是一般来说，需要粒子滤波解决的问题都不会有简单的目标后验，故建模一个好的建议分布是非常必要的：

  • 一个好的建议分布可以使得【近似的】差别权重对于其近似性的要求降低
  • 好的建议分布可以提高采样的质量

​ 传统意义上，建议分布一般会使用odometry的运动模型分布，比如沿着运动方向的与垂直运动方向两个方向为主轴的高斯分布。在粒子的更新阶段（也即从建议分布采样阶段），我们根据控制\(u_t\)，确定\(x_t^{(i)}=u_t *x_{t-1}^{(i)}\)，并且根据\(u_t\)，确定一个带噪声的\(x_t^{(i)}\)，人工加噪相当于是从运动模型分布中采样了。但是，gmapping这篇论文说：odometry较之于LiDAR配准来说，一般都大了一些，如果我们可以在建议分布中也用上观测的值，说不定可以得到更加符合目标分布的建议分布。

When using the odometry model as the proposal distribution in such a case, the importance weights of the individual samples can differ signifificantly from each other since only a fraction of the drawn samples cover the regions of state space that have a high likelihood under the observation model

​ 举一个例子：假设我们有一条长走廊，长走廊较为理想。那么对于配准来说，长走廊情况下的配准协方差沿着走廊方向的分量更大，但是垂直走廊方向很小，而另一方面，odometry在两个方向上可能都有较大的协方差。如果我们使用odometry的协方差，在更新采样环节可能会采到次优的位置，但如果此时结合配准误差、里程计协方差，可以将：

  • 垂直长走廊方向的协方差降下来，沿着长走廊方向的协方差则可以由odometry主导。

​ 从这样的分布中采样，可以得到更好的结果。

​ 此外，还有另一个问题。为什么你在实现PF时，使用完全随机的采样策略效果并不好？这篇论文中的一个解释我觉得非常有道理：

However, if the observation likelihood is peaked the number of pose samples \(x_j\) that has to be sampled from the motion model is high, since a dense sampling is needed for sufficiently capturing the typically small areas of high likelihood. This results in a similar problem than using the motion model as the proposal: a high number of samples is needed to sufficiently cover the meaningful region of the distribution.

​ 之前也没有想得太深，只是觉得角度上的采样不足，于是增大了角度采样。现在想想，整个后验分布确实就是一个具有突出峰值的分布，在实验过程中也发现了这一点（实验是上一篇博客中的粒子滤波，更新过程中发现，收敛时的粒子分布的确呈尖峰形状）。

2.2 如何结合观测

​ PF更新公式如下： \[ \begin{equation} w_t=w_{t-1}\frac{ \eta p(z_t|x_{1:t}, z_{1:t-1})p(x_t|x_{t-1},u_{t-1}) }{ \pi(x_t|x_{1:t-1},z_{1:t},u_{1:t-1}) } \end{equation} \] ​ 朴素情况下，建议分布就是运动状态转移分布：\(p(x_t|x_{t-1},u_{t-1})\)，我们将其替换：考虑到配准信息也可以使用，那么gmapping使用了一个这样的策略：

  • 首先我根据scan-matcher确定一个ROM（region of meaning，有意义的区域），也即是配准认为可能的分布位置
  • 在这个有意义的区域内进行多次采样。多次采样的结果，使用scan-matcher得到\(p(x_t|x_{1:t},z_{1:t-1})\)（似然），而由于控制已知（均值已知），参数一般也已知，那么分布就已知，则在选取的点可以计算\(p(x_t^{(j)}|x_{t-1},u_{t-1})\)
  • 我们需要计算的improved proposal是：\(p(x_t|m^{(i)}_{t-1},x_{t-1},z_t,u_{t-1})\)。这个的物理意义是：在第i个粒子对应的map \(m^{(i)}_{t-1}\)下，由\(x_{t-1}\)由控制量\(u_{t-1}\)转移，观测到\(z_t\)的条件下，在\(x_t\)位置的概率。看起来这个非常像后验，但是其计算是近似的。

​ 但是实际上，在RBPF中，更新公式有所不同。因为RBPF是专门针对SLAM设计的一种粒子滤波，在滤波过程中，各种概率分布需要包含地图信息。针对RBPF的权重更新公式如下： \[ \begin{equation}\label{rbpf} w_t=w_{t-1}\frac{ \eta p(z_t|x_{t}, m_{t-1})p(x_t|x_{t-1},u_{t-1}) }{ p(x_t|x_{t-1},z_t,u_{t-1},m_{t-1}) } \end{equation} \] ​ 上式的分母可以使用贝叶斯理论展开： \[ \begin{align} &p(x_t|x_{t-1},z_t,u_{t-1},m_{t-1})=\frac{p(z_t|x_t,x_{t-1},u_{t-1},m_{t-1})p(x_t,x_{t-1},u_{t-1},m_{t-1})} {p(z_t,x_{t-1},u_{t-1},m_{t-1})}\\ &p(x_t,x_{t-1},u_{t-1},m_{t-1})=p(x_t|u_{t-1},m_{t-1},x_{t-1})p(u_{t-1},m_{t-1},x_{t-1})=p(u_{t-1},m_{t-1},x_{t-1})p(x_t|x_{t-1},u_{t-1})\tag{条件独立性-1}\\ &p(z_t|x_t,x_{t-1},u_{t-1},m_{t-1})=p(z_t|x_{t},m_{t-1})\tag{条件独立性-2} \end{align} \] ​ 故，分母可以展开成为： \[ \begin{equation} \frac{p(z_t|x_t,m_{t-1})p(x_t|x_{t-1},u_{t-1})} {p(z_t|x_{t-1},u_{t-1},m_{t-1})} \end{equation} \] ​ 带入公式\(\eqref{rbpf}\)中可以得到\(w_t=w_{t-1}p(z_t|x_{t-1},u_{t-1},m_{t-1})\)。则进一步可以写为（别看公式很长，其实完全就是贝叶斯，“很好”推的！）： \[ \begin{align} &p(z_t|x_{t-1},u_{t-1},m_{t-1})=\int p(z_t,x'|x_{t-1},m_{t-1},u_{t-1})dx'=\\ &\int \frac {p(x',z_t,x_{t-1},m_{t-1},u_{t-1})}{p(x'|x_{t-1},m_{t-1},u_{t-1})p(x_{t-1},u_{t-1},m_{t-1})}p(x'|x_{t-1},m_{t-1},u_{t-1})dx'=\\ &\int p(z_t|m_{t-1},x',x_{t-1},u_{t-1})p(x'|x_{t-1},m_{t-1},u_{t-1})dx'\stackrel{\text{独立性}}{\longrightarrow}\\ &\int p(z_t|x',m_{t-1})p(x'|x_{t-1},u_{t-1})dx'\propto\\ &\sum_{j=1}^Kp(z_t|m_{t-1}^{(i)},x_j)p(x_j|x_{t-1}^{(i)},u_{t-1})\label{approx} \end{align} \] ​ 公式\(\eqref{approx}\)将最后的转换完成了，这个公式使得我们不必计算复杂的条件概率，而可以使用：机器人运动学模型分布与观测模型分布的加权组合，更新每一个粒子（运用好独立性假设以及边缘化，可以推出很多有意思的公式）。

​ 论文中的更新迭代策略是这样的：

  • 根据里程计信息，作为初值，进行配准，得到带有观测信息的\(u_{t-1}^*\)
  • \(x_{t-1}+u_{t-1}^*\)周围（一个球内）采样，每个采样点\(x_s\)都可以根据里程计的高斯分布以及配准的似然域计算点权（\(p(z|x)p(x|x',u)\)），注意，所有点的点权之和，就是更新公式的\(p(z_t|x_{t-1},u_{t-1},m_{t-1})\)。
  • 根据采样的点可以计算高斯分布，最后得到的粒子从这个高斯分布中采样。

​ 非常奇怪的是，网上所有GMapping的实现，都没有实现多点采样生成高斯分布再进行重新采样的步骤。

III. GMapping 代码流程

3.1 processScan

​ processScan是核心函数：

3.1.1 运动模型

​ 首先从运动模型中采样（drawFromMotion函数，此函数的原理在《概率机器人上》）

3.1.2 计算移动距离

​ 计算移动距离：m_linearDistance, m_angularDistance

  • 保证平移距离不过大，过大则认为位置发生了突变
  • 当移动距离过大，或是长时间没有更新，那么就需要进行一次配准
3.1.3 scanMatch

​ scanMatch首先根据plainReading（也就是点云原始数据）进行配准，配准使用搜索算法，在optimize函数中。注意代码里有bug：

1
if (bestScore>=currentScore)

​ 六个方向移动，直到达到最大分数或者迭代次数，每次减小步长

​ score的计算,在之后的函数中说. 配准函数针对了每一个粒子,每个粒子都进行了一次优化

​ likelihoodAndAScore函数 进行似然计算：大概做了这样的事情：

  • skip是跳过处理的一种实现，指的是：激光线太多，信息冗余，可以跳过一个激光线计算似然域
  • 首先把激光器的位姿投影到世界坐标系下，此后对于每一条需要处理的激光线，也转到世界坐标系下，并且确定一个位于空域的点位置（激光线截短一截）
  • 在一个领域内，查找最适合的障碍物点（因为激光测距会存在噪声），障碍物点需要满足：该点占用概率大于阈值，对应空域点占用概率小于阈值
  • 求均值，并求激光点世界坐标位置与该均值的距离（越小说明越应该是这个障碍点）
  • 求似然。这样求出的似然域是较为平滑的，故在scanMatch的optimize中，计算score也是用了类似的平滑函数

​ computeActiveArea：是一个更新地图操作，因为地图是基于占用栅格的，未知区域也是需要表征的，限制范围可以节约内存资源。computeActiveArea更新了地图大小（每个粒子）：

1
2
3
//this allocates the unallocated cells in the active area of the map
//cout << "activeArea::size() " << activeArea.size() << endl;
map.storage().setActiveArea(activeArea, true);

​ 在此前，先根据scanMatch计算的位姿，计算了占用栅格（每条激光线经过的每个点），将占用栅格加入到map中，等待对地图进行更新。

3.2 updateTreeWeights

​ updateTreeWeights只做了三件事：

  • 归一化权重
  • 轨迹树重置
  • 权重沿着树传播

​ 轨迹树会不会做的是这样一件事情呢？初始时粒子都在同一位置，在迭代过程中，也可能出现两个粒子位姿一致的情况吗？会有一个parent对应了很多child的情况吗，轨迹树的child会怎么来？

​ propagateWeights就是一层一层将叶子节点的weight向上传播（因为在scanMatch中会重新衡量叶子节点的weight，而上层weight（相当于之前的位姿）是逐层累加的）。

​ 但是resample计算使用的是当前叶子节点的权重，与上层的权重无关。那么这样逐层累加的weight，其作用是什么？weightSum在另一个类中被使用了。看代码时没有注意继承关系，以为只有GridSlamProcessor是主要模块，最后发现openslam版本中，还有一个类叫做：GridSlamProcessorThread，内部实现了一些父类没有实现的回调函数，在这个类中定义的onScanmatchUpdate函数中，用到了weightSum：

1
2
3
4
if(part->weightSum>bestWeight){
    bestIdx=idx;
    bestWeight=part->weightSum;
}

​ 算法选择权重综合最大的一个粒子（故与历史信息相关了），取该粒子对应的map作为当前地图，根据最大权重的粒子更新地图，并且选择最优粒子的位姿当作自身当前位姿。

​ 不过根据算法的逻辑，每次计算了底层叶子节点的weight之后，上层需要根据底层来的值修改权重累加，但是非叶子节点的weightSum并没有用（weightSum本身就没有被用过多少次），只有叶子节点（当前最新的particles）对应的weightSum才有价值，故propagateWeights以及相应函数其实是没有作用的。

3.3 重采样

​ resmapleIndexes函数：这里的重采样策略使用的也是《概率机器人》上的低方差重采样。故可能出现：两个粒子重复的情况。返回的是被采样到的粒子的下标。在这个函数的内部，由于使用的策略是低方差采样，下标是有顺序的，故采样的结果也是有顺序的。 ​而new TNode在整个项目中出现在：

  • gridslamprocessor_tree.cpp 的integrateScanSequence中，没有实际调用
  • gridslamprocessor_tree.cpp 的 getTrajectory中，在GSP构造时使用，所以对代码逻辑的影响应该不大
  • gridslamprocessor.hxx 的resample函数中用到过，此处的使用对代码逻辑有实际的影响

​ 建立以及更新权重轨迹树的流程与作用：

  • 根据权重进行低方差采样，结果被保存在m_indexes之中，注意m_indexes是顺序化的，比如：原来有8个粒子，id从0-7，重采样之后成为：1 1 1 4 4 4 7 7，那么在轨迹树中，0，2，3，5，6均会被删除（假设没有其他关联，整条与这几个节点有关的树枝均会被切除）
  • 轨迹树实际上就是重采样树，0 1 2 3 4 5 6 7 这8个节点，按照上面的重采样例子会生成如下图所示的树：
graph TD

A(root)
A1(0)
A2(1)
A3(2)
A4(3)
A5(4)
A6(5)
A7(6)
A8(7)
B(1-1)
C(1-2)
D(1-3)
E(4-1)
F(4-2)
G(4-3)
H(7-1)
I(7-2)
A---A1
A---A2
A---A3
A---A4
A---A5
A---A6
A---A7
A---A8
A2---B
A2---C
A2---D
A5---E
A5---F
A5---G
A8---H
A8---I
Figure 2. GMapping权重树结构示例

​ 重采样得到点就相当于子节点，重采样前的节点是父节点。

  • 删除所有重采样后没有结果的点，比如假设上述例子又发生了一次重采样，(7-1)(7-2)权重过小，那么(7-1)(7-2)(7)都会被删除
  • 对重采样后的新particles，根据当前的点云信息，更新地图（activeArea重新计算），但是权重会被重置为0。那么这个为0的权重，将会在scanMatch函数中被修改（计算完likelihood之后，权重会被设置为likelihood），并且在weightSum中也累加一份。

IV. 流程总结

​ 我已经发现了：openslam的代码又臭又长，还不能编译，令人根本不知道processScan这个函数究竟在什么情况下被使用，貌似因为下载的是最原始的版本。

​ addScan函数（貌似是ROS封装），在转换scan信息之后，调用processScan函数。此函数首先从motion model中采样，此后判定是否需要进行配准。

​ 如果需要配准，通常情况下是超时或者超出距离阈值，则需要调用scanMatch函数，此函数内部使用被称之为【爬山法】的搜索方法进行匹配，得到修正后的位姿（但是貌似没有实现GMapping中的生成高斯分布）。

​ 此后需要计算似然，使用似然域法，在领域内搜点获得一个平滑似然。计算得到的似然作为新的权重，并且将此权重累加到weightSum上去。

​ 计算地图更新，也就是重新计算activeArea，需要向栅格图原始数据（PointAccumulator）的容器中增加新的栅格（或者对原有栅格进行访问）。

​ 重采样步骤，重采样的过程中，会对轨迹树进行重建。方法在上文已经阐述过了，注意propagateWeights是没有太大意义的操作，我直接忽略了。

V. 效果

​ 2007啊！他们太强了。不过也不能说完美，有如下几个问题：

  • 5cm grid 精度可以适应大多数的需求，所以2D SLAM方向做的人越来越少了（基本满足需求了），而我尝试使用更高精度grid的时候（比如2.5cm），配准将发生错误（我猜是grid太小了，导致了激光器模型的稀疏性问题）
  • 同时，grid变小将会占用过多的内存（1cm大小的格子跑Intel数据集貌似可以跑到10GB内存占用，真的无法想象当年他们那个条件怎么做实验的）
  • 丢帧将会导致非常严重的问题，当点云频率很高的时候，丢帧将直接导致配飞（很奇怪，里程计不是用上了吗？）
  • 貌似没有实现2007年论文的improved proposal思想。

​ 以下测试的地图均为5cm大小grid，intel数据包由于点云帧率低，我的i5上可以开8倍速播放包，而我自己做的仿真数据集只能2倍速（否则丢帧直接死掉），粒子个数均为15个，其他参数都是默认参数。

Figure 3. Intel 实验室数据集

Figure 4. hqy仿真数据集【1】

Figure 5. hqy仿真数据集【2】