科普SLAM之粒子滤波建图

本讲重点

• RBPF建图方法的基本定义（什么是RBPF？）

• RBPF建图方法的研究意义（为什么需要RBPF？）

• RBPF建图方法的基本原理（RBPF如何产生的？）

• RBPF建图方法的算法流程（RBPF如何工作的？）

• 栅格地图构建（RBPF如何绘图的？）

• RBPF建图方法的性能改善（如何改善RBPF？）

 

1. RBPF建图方法的基本定义（什么是RBPF？）

RBPF (Rao-Blackwellized Particle Filter)是基于粒子滤波的定位建图方法。它使用粒子群来描述估算机器人位姿与地图的可能性；其中每个粒子中包含了机器人一种可能的历史轨迹，以及所关联的相应地图。这些粒子集合在不断的更新与重采样过程中，能够收敛到少数几个权重系数较高的粒子上。图【1】描述了多条轨迹的情形，其中每条轨迹就是一个粒子中所包含的机器人所经过的中途点信息。在经历了权重系数评估和重采样过程后，原来的多条轨迹能够收敛重合到权重系数较高的轨迹上。

图【1】粒子群所描述的多条轨迹在重采样后收敛到更多重合的轨迹上

对于建图而言，由于地图是可以根据原始激光点云数据按照相关联的位姿来画到同一个地图坐标系中，这样只要使用共同的内存单元来存储一组共同的原始激光点云数据，再按照粒子中所包含的历史轨迹，就可以绘制地图。在算法迭代的过程中，会选出来最优的一个粒子，使用激光雷达数据和所关联的历史轨迹来绘制地图。图【2】直观展示了基于RBPF粒子滤波建图所绘制地图的样子。

 

图【2】基于RBPF粒子滤波建图所绘制地图

2. RBPF建图方法的研究意义（为什么需要RBPF？）

在使用贝叶斯框架来定位建图的方法中，比较常用的是卡尔曼滤波及其衍生体，粒子滤波，以及位姿图优化这三类方法。历史上基于粒子滤波的RBPF定位建图方法出现于卡尔曼滤波及其衍生方法之后，是因为基于粒子滤波的定位建图方法本身存在相对的优点。

卡尔曼滤波及其衍生方法(Extened Kalman Filter & variants)对于所估算变量（机器人位姿）使用了高斯分布(Gaussian Distribution)的假设，也就是只有单一模态(Uni-Modal)的均值与协方差矩阵来描述位置与朝向的数值与不确定度；并且在迭代的预测与更新过程中，都需要使用一阶泰勒展开来对于本来非线性的运动模型与观测模型进行线性化。这样卡尔曼滤波及其衍生体方法就存在先天不足：所使用的数学工具无法反映出物理过程的非线性特性，所使用的线性化手段存在近似偏差，偏差积累到一定程度容易导致系统的状态估计发散；同时单一模态的特性，对于实际场景中由于某种原因（比如场景的结构对称性）导致机器人可能存在多模态的情形，很难进行描述。当然，这两种可能的位姿会随着机器人不断运动和获取到更多的环境信息会收敛到唯一正确的位姿上。对于此种场景，如果选用基于粒子滤波的定位建图方法，在粒子群的迭代过程中，可以直接使用非线性运动模型与非线性观测模型，从而避免线性化所引入的偏差。同时，粒子群中的每一个粒子都代表机器人位姿的一种可能性，这样就是多模态(Multi-Modal)的状态估计，可以避免单模态状态估计的局限。

3. RBPF建图方法的基本原理（RBPF如何产生的？）

RBPF是贝叶斯估计框架的一个具体实例方法。所以说起RBPF，就要先从贝叶斯估计框架说起。对于定位建图过程，可以先将机器人运动过程描述成贝叶斯网络，如图【3】所示，其中机器人的位姿用​来表示，轮速里程计的信号输入为​，外部传感器如激光雷达的观测量用​来表示，相应的地图数据用​来表示。其中每一次变换，都会引入一个运动变量​ 和一个观测变量​，这里时间戳​的取值为​。在运动变量​ 进行预测的基础上，观测量​进行一次更新。在机器人状态更新的过程中，一个马可夫假设(Markov Assumption)就是下一个时刻的位姿状态只和上一个时刻的位姿状态有关，而和上一个时刻之前的位姿没有直接联系。

 

图【3】贝叶斯估计用于建图定位的框架流程

数学上来讲，贝叶斯估计描述了后验概率与似然概率以及先验概率的关系，用数学表达式描述就是​，其中​ 代表所要估计的变量，​代表观测数据，​代表后验概率，​代表似然概率，​代表先验概率。通过贝叶斯框架来将机器人的位姿状态估计和地图状态估计的联合概率分布​分解成两个独立的概率分布乘积的形式，其中​代表里程计数据，​代表激光观测数据，​是机器人的轨迹，​描述了地图特征点位置。

​。

这样分解处理可以先估算机器人的轨迹​，再基于此机器人轨迹来估算地图​。使用图表描述了机器人的位姿状态​ 随时间变化的情况。RBPF就是使用粒子滤波来估算机器人轨迹的可能性，有了轨迹后，再将雷达原始数据按照轨迹上面机器人的位姿来绘制构建地图。

对于已知机器人位姿的地图构建，需要先将激光雷达数据按照所关联的位姿来转换到统一的地图坐标系中，然后可以根据激光束寻迹(Laser RayTracing)来绘制地图。对于地图而言，其中每个栅格位置是相互独立的，这样整个地图的概率分布又可以转换为每个栅格概率分布的乘积​，如图【4】所示，可以将多维栅格地图分解成多个单个栅格地图来求解。其中每个栅格来进行地图建模的细节，可以参见 第【5】节【栅格地图构建】。

 

图【4】占据栅格地图的独立求解方法

在使用粒子滤波来估算机器人位姿的可能性过程中，需要针对粒子群的所有粒子进行不断迭代的更新和重采样。更新过程中，需要使用提议分布proposal distribution来推算机器人位姿在下一个时刻的可能位姿，比较常用的是机器人的里程计运动模型：通过里程计运动学模型推算出机器人在下个时刻最可能的位姿，然后以此为中心，在刚刚所计算出来的位姿附近来采样得到多个备选位姿(candiate poses)；然后对于这些位姿根据观测模型进行评分，从而计算出有权重的均值与协方差矩阵，也就是恢复出了以里程计所推算的机器人下时刻可能位姿周围的正态分布。有了下个时刻机器人可能位姿所服从的正态分布，就可以通过采样来得到机器人下个时刻与观测模型比较一致的位姿。重采样过程中，会根据观测模型来对于此刻的所有粒子进行权重系数的评分，然后根据权重系数的大小来进行采样；这个过程可以从图【5】来直观看到。

 

图【5】采样与权重系数评估过程

采样过程使用的是有放回的采样，粒子被采样出来的概率大小与粒子所对应的权重系数成正比。权重系数较大的粒子，能够有更多存留下来的几率。这与达尔文的生物进化中优胜劣汰(survival of fittest)的原则是一致的。采样后与采样前粒子数目一样，只是采样前权重系数较小的粒子被权重系数较大的粒子所代替。由于重采样过程会导致粒子消失问题，所以只有必须的时候才可以进行重采样。具体细节可以参考【5】节【避免粒子消失Particle Deprivation的重采样策略】。

4. RBPF建图方法的算法流程（RBPF如何工作的？）

RBPF建图过程中，需要经历不断迭代的预测与更新等步骤。

1. 遍历描述机器人位姿粒子群中的每一个粒子​:

   a. 【更新位姿过程】：通过运动模型来预测下一个时刻的可能位姿​，并通过激光匹配来得到取得【最大似然】的位姿​。在这个最大似然位姿附近进行随机采样，可以得到【多个位姿点采样点】​，以及相关联的【权重系数】​。这样就可以通过计算有权重的均值和协方差矩阵，来恢复出【正态分布】的参数特性​。这个正态分布上再进行【采样单个点】，就可以作为在时刻​的位姿​。同时，计算出该位姿所对应的权重系数​;

   b. 【更新地图过程】：单个粒子进行更新后，就可以将该时刻的【雷达数据】按照【更新后的位姿】集成到现有的地图中​，从而得到此时刻的拓展地图​;

2. 计算粒子群中每一个粒子所关联的【权重系数的平方和的倒数】，并且和参考阈值进行比较。如果符合重采样的条件，就触发重采样过程：

3. 【重采样】过程会按照权重系数的大小，进行【有放回地重新采样】(Re-sampling)：权重系数越大的粒子，越容易被选出；【权重系数小的粒子】，经过重采样过程就会【被权重系数大的粒子所取代】。这样确保重采样可以得到【相同数目的粒子】，其中权重系数较大粒子数目会变多。

   整个算法流程的细节如图【6】所示【改进的RBPF建图算法流程图】。

   

   
    

   

    

 

 

图【6】改进的RBPF建图算法流程图

5. 栅格地图构建（RBPF如何绘图的？）

构建地图的过程，会使用占据栅格Occupancy Grid Map来对于环境进行建模；也就是将平面的二维空间划分成大小相等的一系列格子Grid，其中每个格子里面的数值代表了该格子被障碍物所占据的概率的大小(Probability of Occupancy)。形象描述的话，墙壁所对应的格子就是被占据的状态(Occupied State)，走廊过道所对应的格子就是自由的状态(Free State)。

根据公式​，整张地图的建模可以分解成各个栅格进行独立的建模，于是建模问题的基础就是各个栅格的建模。简要来讲，使用激光束的穿透特性，来统计该栅格激光束所打到的次数与穿透的次数，按照公式， ​,可以得到该格子被障碍物所占据的概率大小。所构建的占据栅格地图的示意图如下图所示，其中黑色代表的是障碍物所在区域，白色代表的是自由空间所在区域。

 

图【8】占据栅格地图的激光束扫描更新过程

也可以使用复杂的激光束模型来为栅格进行建模。由于本课程主要以基于科普的实践为主要目标，所以在此不再赘述。

 

6. RBPF建图方法的性能改善（如何改善RBPF？）

a. 适应型粒子更新策略Improved proposal distribution

RBPF中使用粒子滤波来描述机器人位姿的可能性。每一次粒子更新的过程中，都需要使用里程计数据根据假设的正态分布(Proposal Distribution)来预测粒子可能的位置，再根据所采集的雷达数据与所建地图的匹配程度来为粒子的权重系数进行评估。但是对于里程计运动模型不准确的情况，该种方法容易出现问题，导致更新后的粒子比较多分布在状态空间错误的区域。改进型的粒子更新策略，将里程计运动模型与雷达数据的观测模型来放到一起进行粒子更新，

 

 

图【9】改进型的粒子更新策略

这里的图表描述了改进型的粒子更新策略，其中引入了里程计运动模型与雷达观测模型两方面的数据，粒子更新后只分布在与观测模型一致的范围内。图【a】为作为对比只使用里程计运动模型所产生的粒子，可以看到比较零散。图【b】为引入了带有走廊两侧墙壁特征的观测数据，由于在走廊的延伸方向缺少显著特征是不可观的，导致粒子沿走廊方向径向分布。图【c】为引入了带有走廊两侧墙壁和堵头的观测数据，此时在走廊的延伸方向具备显著特征可观，导致粒子分布密集到一个有效点附近。通过三幅图的对比显示，可见观测数据在粒子的更新过程中可以起到非常重要的密集化作用。

 

图【10】改进型的粒子更新策略

这里另外一张图表对比显示了使用原始的里程计运动模型（左图）与综合使用了里程计运动模型与激光匹配（右图）所得到的机器人位姿粒子分布。可以看到综合使用了激光模型后，粒子分布更加密集。

b. 避免粒子消失Particle Deprivation的重采样策略

在根据粒子的权重系数进行重采样的过程中，需要确保权重系数大的粒子不会丢失；不然，就会出现匹配度较好的粒子消失的情况。在必要的时候触发重采样，以便使得权重大的粒子能够在有选择采样中存活下来。​ 描述了粒子权重系数的分布情况。对于观测数据与地图匹配得较好的情况，相应粒子的权重系数​会大些，这样​会小些。相反，对于观测数据与地图匹配得效果不好的情况，相应粒子的权重系数​会小些，这样​会大些。对于判断小于阈值相应进行重采样的情形，学界验证的一个有效值是粒子数目的一半​。当满足​，就会进行重采样，来确保粒子群可以聚集到权重系数较大的粒子附近。

 

6. 参考文献

[1] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. Cambridge, MA: MIT Press, 2006

[2] D. Hähnel, W. Burgard, D. Fox, and S. Thrun, “An efficient FastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Las Vegas, NV, 2003, pp. 206–211

[3] Giorgio Grisettiyz, Cyrill Stachniss, Wolfram Burgard, Improving grid-based slam with rao-blackwellized particle filters by adaptive proposals and selective resampling, Proceedings of the 2005 IEEE international conference on robotics and automation, 2005

[4] Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard, Improved techniques for grid mapping with rao-blackwellized particle filters, IEEE transactions on Robotics, 2007

本讲总结

本讲描述了RBPF粒子滤波来进行定位建图的基本原理以及算法流程，并对其中可以改进提效的地方进行阐述：改进型粒子更新策略和重采样策略。感兴趣的读者可以自行阅读参考文献中原文来探索更详细的全貌。