这一算法已经比较落后了， 因为滤波是用于估计当前的状态，如果我们之前的数据出现错误，是没有办法进行修正的，所以这个方法的整体效果并不是特别好。但是这个方法在整个激光SLAM的发展中还是占有很重要的地位的，而且这里蕴含的很多数学知识是非常重要的，所以我通过这篇文章详细说明一下吧。
这一方法生成的地图叫Grid-based，也就是栅格地图。还有另一种地图叫feature-based，也就是特征地图。一般依靠视觉生成的地图就是feature-based，激光雷达生成的地图一般是Grid-based，一般用激光（尤其是二维激光雷达）生成的Grid-based是可以用来导航的，但是用视觉生成的地图是很难用于导航的。基本介绍做完了就进入正片。
目录
贝叶斯滤波
我们目前大部分状态估计用到的滤波基本都是基于贝叶斯滤波的。熟悉数理统计的应该知道数理统计领域分为两大流派：频率学派（古典学派）和贝叶斯学派。一般我们认为在实际生活中，贝叶斯学派更符合实际情况，所以我们生活中常见的各种估计其实是基于贝叶斯的。所以贝叶斯滤波其实是一大类方法的统称。
我们开始进入正题学习贝叶斯滤波，首先就是贝叶斯滤波的基本数学概念：
上面这些都是比较基础的知识，就不做过多的解释，接下来我们看一下贝叶斯滤波是怎么一回事。
1.1贝叶斯滤波特性
- 估计的是概率分布，不是具体的数值。这一点就是和频率学派不同的地方之一，频率学派估计的典型方法是MLE（极大似然估计），估计的是具体的数值；而贝叶斯估计的典型方法是MAP（最大后验估计），估计的是概率分布。这里顺便说一下我对量大学派的理解，频率学派认为，有些参数它仅仅就是一些参数，我们要想办法估计这些参数；而对于贝叶斯来说，没有任何的参数，一切都是概率分布。
- 是一大类方法的统称。
- 是一个抽象的表达形式——对于不同问题有不同的实现方式（卡尔曼家族、粒子滤波）。
- 迭代估计形式。这个形式可以减少很多的计算量。
1.2贝叶斯滤波的推导
所有的贝叶斯滤波最终都能推导成下图的形式：
我们来具体分析一下怎么得到这个式子：
我们用
来表示一个量的后验概率分布，用
来表示proposal分布，
等于
，表示一个预测的过程，意思就是我们已经知道xt-1的状态了，然后根据一个预测量来预测xt的状态得到一个新的分布，然后用
（观测量）对这个新的分布进行校正。
以上就是贝叶斯滤波的大致过程。
回到上一条所说的第二个特性，凡是能推导成这种形式的滤波，都称为贝叶斯滤波。所以熟悉卡尔曼滤波的人可以看出来，卡尔曼滤波是贝叶斯滤波在线性高斯情况下的特例。
现在开始正式对贝叶斯滤波进行推导。
首先根据刚才分析的表达式，我们确定一个目标：已知
、𝑢𝑡、𝑧t的情况下，得到
的表达式。
根据条件贝叶斯公式:
其中：
这个式子其实就是机器人的运动模型。
令：
表示xt的后验概率分布；
表示xt的proposal概率分布。
则：
粒子滤波
粒子滤波其实是贝叶斯滤波的一种特例，换句话说，粒子滤波是贝叶斯滤波的一种实现方式。粒子滤波用的应用场景就很多了，比如说动态物体跟踪、机器人定位、SLAM等等。当在视觉中用滤波器做SLAM往往会出现很多问题，原因就是滤波器要同时估计机器人的位姿以及feature-based，随着运行feature-based会越来越大，但越慢。所以视觉中用滤波器做SLAM只适用于小环境。说到这里也许有人会问了，为什么激光雷是运用卡尔曼滤波法要对状态向量的立方求逆，随着feature-based的增大，状态向量也会越来越大，这就会导致程序运行越来达用滤波器做SLAM就可以了？ 答案是激光雷达的SLAM算法里面，把估计位姿和地图的问题分成了两个问题单独讨论（RB Particle-filter）。但是由于之前提到的错误没办法修正的，所以这个方法也不适用于大的环境。所以随着环境的增大，出错的可能性也就越来越大，我们称主要造成这个情况的问题叫粒子耗散问题，很可惜这个问题是没办法解决的。
综上所述，粒子滤波法因为粒子耗散问题的存在，也不适用于大环境的SLAM，但是它在小环境也是有不错的效果的。
现在我们开始具体分析一下什么是粒子滤波。
2.1粒子滤波特性
- 是贝叶斯滤波的实现方式。
- 能处理非线性情况。
- 能处理多峰分布的情况。（多峰分布的典型就是全局定位）
- 用一系列的粒子（particle）近似概率分布。
- 非参滤波器。这个的意思就是粒子滤波是用一系列的粒子表示的，没有参数。
示意图：
这些特性非常适合用于机器人，比如说机器人所涉及的很多问题就是非线性情况，还有机器人的全局定位就是利用了多峰分布的特性。
2.2粒子滤波的流程
以下面的式子说明：
表示一个状态的假设——在定位中可以表示为机器人的位姿信息。
表示假设的权重——在定位中可以表示为跟地图的匹配度。
粒子滤波工作的大致流程为：
- 用粒子进行状态传播。
- 评估每一个粒子的权重。
- 根据权重进行重采样——去除权重小的粒子。以
的概率接受
，权重清零。
示意图：
下面将对各个流程进行详细解释。
2.2.1状态传播
传播模型：
根据之前提到的
可以把t-1时刻的概率分布（粒子分布）表示成：
根据数据ut预测t-1时刻的概率分布（粒子分布）：
因为这里是用粒子表示分布，那我们就可以对每个粒子进行单独的预测。所以这个传播模型就是对每个粒子进行传播，那这个传播模型就涉及到了机器人的运动模型的知识了。
运动学模型：
从预测分布即为运动学模型中进行采样：
设t-1时刻第i个粒子的位姿：
设t-1时刻的
设t时刻第i个粒子的位姿
噪声为0均值的高斯分布的方差分别为Nx，Ny，Nz，则这个传播模型为：
如果用图像来表示的话：
可以看到随着传播 ，粒子扩散越明显，如果放任不管的话，这些粒子会逐渐趋向均匀分布，所以当我们有观测值的时候，我们就可以通过重采样把扩散的粒子（即权重小的粒子）除去，这样就可以使得粒子重新聚集。
2.2.2权重评估
以定位为例，权重就是地图和机器人位姿的匹配程度。这里有几点需要知道：
- 我们无法知道机器人位姿的实际分布。
- 从机器人的预测分布进行采样，联合权重一起近似机器人的后验概率分布。
- 权重用来评估实际人的预测分布和实际分布的差，差越大，权重越小。
- 权重的定义：
对于某一个粒子：
因此权重为：
——这个是观测模型
——这个是不重采样的更新方式，每一次重采样过后都会进行一次权重更新。
下面用一组图片来说明一下：
图中的g表示proposal分布为先验概率分布，f表示后验概率分布进行权重评估后：
可以看到权重大小的值跟真实的数值变化趋势很相似。但是大量的例子分布在了低权重区域，所以我们需要进行后续操作。
2.2.3重采样
- 到目前为止，新的粒子群是根据 proposal分布进行采样的，并且用观测模型计算权重，而最终的目的是用粒子群来近似后验概率分布。
- 对粒子群进行重采样，对于某一个粒子xi来说，以wi的概率接受这个粒子。
- 生成一个随机数，根据其落在的区间决定接受的粒子，重复N次。
我们可以借助下面的图像理解一下重采样的过程：
最终，权重更大的粒子会被多次接受，权重小的粒子很可能没有被接受。
所以重采样的目的就是用proposal分布的粒子和观测模型的权重，生成符合后验分布的粒子群。
那么上面的例子的重采样的效果图：
可以看到评估的效果已经很好了。
2.3粒子滤波总结
2.3.1算法流程
2.3.2存在的问题
- 粒子耗散问题，简单来说就是粒子多样性的散失，这是重采样我们复制了一些权重大粒子，丢掉了一些其他的粒子带来的问题。这个问题对定位的影响不大，但是对建图的影响是灾难性的。
- 维数灾难，这个就是粒子数会随着维数的增加呈指数增长，这也是在视觉SLAM中不会用到粒子滤波的原因之一。
- 当proposal比较差的时候，需要用很多的粒子才能较好的表示机器人的后验概率分布
FastSLAM的原理及优化
FastSLAM算法是基于粒子滤波的，也是Gmapping算法的基本原理。
3.1FastSLAM介绍
- SLAM：在给定传感器数据的情况下，同时估计机器人的位姿和环境地图：
- SLAM可以分解成两个问题（RB Particle-filter）：
- 机器人的定位。
- 基于已知机器人位姿的构图。因为激光雷达的测距很精准，所以这个是可以很好做出来的。可以认为这个问题是有close-form解的。
上面的这个分解用数学公式来表示：
其中
为估计机器人的路径（位姿）；
为给定机器人位姿和传感器观测数据的情况，进行地图构建。
所以这个算法就是用粒子滤波来估计机器人的位姿，然后分别为每一个粒子计算地图即可。因此一个粒子包含以下数据：机器人的轨迹
和对应环境的地图。
3.2算法流程
我们回顾一下贝叶斯滤波：
描述：
上式把对
的估计转换成一个增量估计的问题。
用粒子群表示，每个粒子用运动学模型
进行传播。 对于传播之后的粒子，用观测模型进行权重计算，并且根据估计的位姿构建地图。
3.3存在的问题及优化
因为FastSLAM算法是基于粒子滤波的，所以前面提到的粒子滤波存在的问题同样存在于FastSLAM算法中，我们现在研究一下怎么去优化。
3.3.1问题一
问题：每一个粒子都包含自己的栅格地图。对于稍微大一点的环境来说，每一个粒子都会占用比较大的内存。如果机器人的里程计误差比较大，即proposal分布跟实际分布相差较大，则需要较多的粒子才能比较好的表示机器人位姿的后延概率分布，会造成内存爆炸。
目的：要保持粒子的数量在一个比较小的数值。
方法：提升proposal分布采样的位姿质量，即：
这里加了一个scan-match的过程来减少粒子数。这个优化方式也同样运用在了Gmapping算法，后续可能会详细讲解。
3.3.2问题二
问题：粒子耗散问题，因此每一次进行重采样都有一定的随机性。随着重采样次数的加多，粒子的多样性会耗散掉，即最终的所有粒子都来自同一个粒子或者少数的几个粒子的复制。
目的：尽量缓解粒子耗散的问题。
方法：减少重采样的次数，用一个量来表示当前估计和真实分布的差异性：
式子中的w是权重估计之后的，不是重采样之后的。当𝑁eff较大时，说明差异性很小，不进行重采样；当𝑁eff较小于时，说明差异性很大，因此进行重采样。 这样可以极大的减少了重采样的次数，缓解了粒子耗散问题。所以粒子耗散问题只能缓解，不能去除。这个优化方式也同样运用在了Gmapping算法，后续可能会详细讲解。
3.3.3进一步优化proposal分布
这个优化和前几个不一样，没有运用在Gmapping算法。但是我在尝试在Gmapping算法中加入，后续可能会写一篇关于Gmapping算法的文章来着重分析一下。
之前的两个优化方式都是从proposal分布进行采样，然后进行极大似然估计提升采样的质量。而这一次，我们用的方法是考虑最近一帧的观测，把proposal分布限制在一个狭小的有效区域。然后在正常的对proposal分布进行采样。
假设激光雷达的匹配比里程计的测量精确很多，从分布上来说，激光雷达匹配的方差要比里 程计模型的方差小很多。
如图所示，激光匹配的方差比里程计要小很多，如果proposal分布用激光匹配来表示，则可以把采样范围限制在一个比较小的区域，因此可以用更少的粒子即覆盖机器人的概率分布，所以我们可以认为这段数据中激光雷达占主导。
proposal分布：
在自己的区域
占主导地位，此时
的值可以看成常数，因此：
即proposal分布从里程计观测模型变换到了激光雷达观测模型。
因为激光雷达观测模型的方差较小，假设其服从高斯分布：
对其求解：
极大似然估计得到局部极值：
我们认为 
离高斯分布的均值比较近，因此在
附近采样得到K个位姿：
对这K个位姿进行打分
，并认为这K个位姿服从高斯分布，即可求解得到高斯分布的表达式：
Proposal分布变为高斯分布由(𝜇, Σ)表示的高斯分布，因此粒子传播由从运动学模型采样修改为对该高斯分布进行采样。则权重的计算方式为：
最终算法流程：
已知数据：
Scan-match和采样：
计算高斯分布：
从高斯分布中采样并更新权重：
已知位姿的情况下进行地图更新：
已知位姿的情况下进行地图更新：
总结
这篇文章到这里也算是结束了，写出这篇文章也是看了很多资料，后续可能会写一篇对Gmapping算法的解读和优化。
1478
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
