RoboCup中的机器人自主定位——从理论到实践（三）：粒子滤波器

最近虽然闲在家中，但是要开始做毕业设计了，每天光是读论文就花去了大量的时间，因此这个系列更的有点慢，不过我一定会抽空写完的，也算是为以后找工作问项目经历提前做点准备。

我们继续本节的内容~
上一次在RoboCup中的机器人自主定位——从理论到实践（二）中详细介绍了UKF的由来、在什么环境下使用以及如何使用。相信大家应该还记得（不记得的请戳上面链接）卡尔曼滤波系列算法是由贝叶斯滤波方法结合了高斯分布的假设发展而来，而今天要介绍的粒子滤波就是贝叶斯滤波演化出来的另一个分支，很显然，它并不需要高斯分布这个假设。

一、初识粒子滤波

前面我们讲到机器人的位姿是通过概率分布来表示的，卡尔曼滤波意图通过高斯分布的方式将这个概率分布表示出来（用均值和方差），而粒子滤波是另辟蹊径，它想要通过大量的点去逼近这个分布。这就好像求不规则物体的面积一样，既可以用类似形状的面积公式来求，也可以分割成无数个小块，无限逼近来求。

由此我们已基本能猜测出粒子滤波的优缺点：
由于它是用很多粒子去模拟，因此最大的好处便是它能应对各种各样的概率分布模型，尤其是卡尔曼滤波最不擅长的多峰分布(multi-modal distribution)，而多峰分布恰恰在定位问题中经常出现；但是同样，缺点也很明显，那就是计算量会随着粒子点数的增加而剧增，而如果只用少许粒子的话又不能很好地表征该分布。这些特性在实践中都会一一得到验证。

二、算法步骤及理论支撑

网上关于这方面的文章其实很多，在做项目时我也基本都看过一遍。有些写的很通俗易懂的文章会把粒子比作“小狗”啊、“猴毛”啊，在此我就不拾人牙慧了，我会秉持着“用大家都能理解的话解读专业术语同时不失深度”的写作理念把粒子滤波讲清楚。

之所以会用这些形象的东西来比喻粒子，主要还是为了体现粒子的特性——随机的、不确定的。尤其在定位之初，完全不清楚机器人所处的位置，那机器人的位置便有多种可能，它可能出现在中圈附近，也可能出现在左边线亦或是右侧的球门旁，那我们就需要相应的往以上可能的地方撒点，用这个点来表示机器人出现在这个位置的可能性，可能性越大，粒子越密集，可能性越小粒子越稀疏。随着机器人迈开步子一摇一摆的走动起来，它观测到的视野也开始发生变化，它看到地上有一个白点，那极有可能是罚球点了，这时候粒子也开始向罚球点处聚集。像生物进化中的“优胜劣汰”法则一般，每个粒子都会被赋予一个权重，权重高的粒子留存下来，权重低的则会逐渐被淘汰，而这个权重的高低就是由离罚球点的远近决定的。那我们再走两步看看？看看就看看，哟，看到球门框了，这可是不得了的重大情报，粒子们像抢到大新闻的狗仔队一般蜂拥而至，那这时候，机器人的位置基本就确定的八九不离十了，在罚球点附近，并且是往球门方向走去。

以上的自然语言表述成数学理论是什么样的呢？Particle Filter Tutorial 粒子滤波：从推导到应用（一）到（ 四 ）这篇文章将粒子滤波涉及到的几个关键点：蒙特卡洛方法、重（zhong）要性采样、重（chong）采样写的很清晰，我在此将他们梳理出来，方便大家跟着脉络有重点的看。

1.从贝叶斯滤波讲起

一切的一切还是要从概率滤波方法的老祖宗贝叶斯滤波说起。 从贝叶斯理论的观点来看，状态估计问题就是根据之前1到$k$时刻的已有数据$y_{1:k}$递推的计算出当前状态$x_k$的概率，即$p(x_k|y_{1:k})$ 。

这个过程需要分“两步走”来完成，第一步利用已有的先验知识和系统运动模型预测状态的先验概率密度$p(x_k|y_{1:k-1})$，第二步结合$k$时刻的测量和观测模型，对之前的预测结果进行更新，得到后验概率$p(x_k|y_{1:k})$。这个后验概率又会作为$k+1$时刻的先验代入下一时刻进行预测，如此循环反复，最终完成滤波操作。

根据边缘概率公式和无处不在的贝叶斯公式$p(x)=\int p(x,y)dy=\int p(x|y)p(y)dy$可知，预测概率 p ( x k ∣ y 1 : k − 1 ) = ∫ p