PF、RBPF和Gmapping介绍（笔记）

本人非该专业领域，公式、论文看得一头雾水，只能看一些通俗易懂一点的文章，简单理解一下，在此做个记录。

前言

首先，在解决SLAM时，既需要完成地图评估特征估计，又要完成机器人位姿估计，解决的是一个“先有鸡还是先有蛋”的问题。

其次，基于滤波的SLAM主要经历了卡尔曼滤波器(Kalman Filter, KF)、粒子滤波(Particle Filters, PF)和RBPF这三个阶段。

基于RBPF的SLAM方案（如Gmapping）是先完成定位，再完成建图。RBPF算法的主要方式是用许多粒子去获取准确的地图，因此要对RBPF算法进行优化的主要方向是减少粒子的数量。

Gmapping在RBPF的基础上完成了两个改进：
①为了减小粒子数Gmapping提出了改进提议分布；
②为了减少重采样的次数Gmapping提出了选择性重采样。

1、粒子滤波（PF: Particle Filter）

1.1定义：

通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数，用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程，这些样本被形象的称为“粒子”，故而叫粒子滤波。其核心思想是通过从后验概率中抽取的随机状态粒子来表达其分布。

通俗理解：先在地图上均匀的撒一把沙子，再根据传感器值来给每粒沙子赋予不同的权重，高权重的沙子会被保留，低权重的沙子会被删除。

1.2特点：

粒子滤波的方法一般需要大量的粒子来获取好的结果,但这必会引入计算的复杂度;粒子是一个依据过程的观测逐渐更新权重与收敛的过程,这种重采样的过程必然会代入粒子耗散问题(depletion problem), 大权重粒子显著,小权重粒子会消失(有可能正确的粒子模拟可能在中间的阶段表现权重小而消失).

1.3实现步骤：

(1)初始状态：用大量粒子模拟运动状态，使粒子在空间内均匀分布；

(2)预测阶段：根据状态转移方程，将每一个粒子带入，得到一个预测粒子；

(3)校正阶段：对预测粒子进行评价（计算权重），越接近于真实状态的粒子，其权重越大；

(4)重采样：根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；

(5)滤波：将重采样后的粒子带入状态转移方程得到新的预测粒子。

2 Rao-Blackwellized Particle Filters (RBPF)

2.1定义：

该算法将SLAM问题分解成机器人定位问题和基于位姿估计的环境特征位置估计问题，用粒子滤波算法做整个路径的位置估计，用EKF估计环境特征的位置，每一个EKF对应一个环境特征。
Montemerlo等人在2002年首次将Rao-Blackwellised粒子滤波器应用到机器人SLAM中，并取名为FastSLAM算法。

2.2特点：

在RBPF算法实现中会采样计算大量粒子来拟合目标分布, 频繁的重采样步骤导致粒子逐渐耗散（也会造成粒子多样性减少）, 浪费大量计算资源。主要原因在于计算建议分布时只把运动模型数据考虑在内, 造成建议分布与目标分布差距较大, 需要大量的粒子进行拟合。

2.3基于RBPF实现SLAM的步骤：

(1)采样: 根据建议分布从t–1时刻粒子集合采样获取t时刻粒子的先验分布集合, 建议分布使用运动模型；

(2)计算权重: 计算每个粒子的权重, 权重w反映了建议分布与目标后验分布的差距；

(3)重采样: 根据每个粒子的权重进行重采样, 从临时粒子集合中抽取更换M个粒子, 结果产生的M个粒子形成新的最终粒子集；

(4)更新地图: 对于每个粒子根据传感器观测数据和机器人当前位姿更新地图中的每个特征。

3.基于RBPF的Gmapping-SLAM方案

3.1改进了什么：

Gmapping在RBPF的基础上改进提议分布和选择性重采样，从而减少粒子个数和防止粒子退化。
①改进的提议分布不但考虑运动（里程计）信息还考虑最近的一次观测（激光）信息，这样就可以使提议分布的更加精确从而更加接近目标分布。

②选择性重采样通过设定阈值，只有在粒子权重变化超过阈值时才执行重采样，从而大大减少重采样的次数。

3.2为什么先定位后建图：

SLAM的过程是一个条件联合概率分布，由概率论可知联合概率可以转换成条件概率即：P(x,y) = p(y|x)p(x)。通俗点解释就是我们在同时求两个变量的联合分布不好求时，可以先求其中一个变量，再将这个变量当做条件求解另一个变量。这就是解释了Gmapping为什么要先定位再建图：同时定位和建图是比较困难的，因此我们可以先求解位姿。这也造成了Gmapping很依赖里程计的精度。

参考文章：

1、百科：粒子滤波
2、知乎：粒子滤波在目标跟踪中的作用是什么？（通俗易懂）
3、知乎：怎样从实际场景上理解粒子滤波（Particle Filter）？（挺有趣)
4、书籍：Thrun《Probabilistic Robotics》
5、CSDN:基于粒子滤波的定位算法 ——原理、理解与仿真
6、