机器人学习--粒子滤波及其在定位中的应用

前提基础，先看一下贝叶斯滤波和蒙特卡洛方法

一、什么是粒子滤波？

这里有个基于粒子滤波的物体跟踪案例说明：

参考：基于粒子滤波的物体跟踪 - yangyangcv - 博客园

如果还是看不懂，知乎上有个通俗地解释粒子滤波器的案例说明，感觉工程师做过实际项目有经验的说的要通透一点。

参考：通俗地解释粒子滤波器 - 知乎

先定义一下基本场景，以及理清楚基础概念。

我们有一个小车，可以在2D平面上移动。

我们离散化整个过程，意思是，我们在每个k*T（T是周期）的时候用粒子滤波定位小车的位置。比如，出发后第一秒1s，第二秒2s，第三秒3s。

*************************以下也可以先忽略*******************************

我们认为这个过程可以用马尔可夫链简化，也就是说，这一步k的状态由上一步的状态+转移过程就可以确定。如果想详细了解可以看之前的文章，因为本质上这也是个用蒙特卡洛（瞎试）方法得到概率分布的贝叶斯滤波器。当然如果你不清楚上面这些名词，不重要。因为我之后的讲解用不着！

风光霁月游侠儿：通俗地解释卡尔曼滤波器（一）——从贝叶斯滤波器说起zhuanlan.zhihu.com

*************************以上也可以先忽略*******************************

粒子滤波定位小车的过程分为三步，需要三个信息（就是我们知道三件事）。

哪三个信息呢？

1.车上一秒在哪

2.车的运动是什么

3.测量结果，比如GPS

哪三步呢？

1.Prediction Step

2.Innovation Step

3. Re-sampling Step

这三步是干嘛呢？这三个信息在这三步里怎么用呢？

一步一步来,

第一步，Prediction Step（预测）

用到上一步的结果，和运动。请看下图

我们用“粒子”点来表示车的位置，也就是上图的×。

上一步的位置，我们用红色的x来表示，他们都是车的可能的位置。比如上图的(1,5),(3,5),(3,7),(5,5)

那你说这些位置听谁的呢？车在哪呢？我们可以给每个位置定个权重，然后加权平均一下。在预测这步权重都是一样的，我们有四个粒子，那么每个权重就是0.25。

也就是我们有四个粒子x1，x2，x3，x4，x[(横坐标x，纵坐标y)，权重w] //注释：这部分粒子集合权重，参考《概率机器人》中文版 P74页程序4.3粒子滤波和P191页粒子滤波定位的内容

x1[(1,5),0.25]

x2[(3,5),0.25]

x3[(3,7),0.25]

x4[(5,5),0.25]

然后我们有个运动，这个运动可能是轮速传感器得来的，或者是比如IMU，复杂一点对于无人车来讲，7自由度整车模型的预测结果等等（这就专业一点了）。不管怎么来的吧。

假设我们现在知道了运动是(5,4)。但是还有个误差。

预测这步就是把所有权重不变，在状态（运动）加上这个运动的结果。

x1[(6,9),0.25]

x2[(8,9),0.25]

x3[(8,11),0.25]

x4[(10,9),0.25]

如果你知道噪声的大小，可以在结果上加上一个随机数。结果如下：

x1[(6.1,9.2),0.25]

x2[(7.9,9),0.25]

x3[(8.3,10.8),0.25]

x4[(10.1,8.9),0.25]

预测这步就完啦。 //注释：以上这部分内容参考《概率机器人》这本书中文版的P103页程序5.6，采样里程计运动模型

第二步，Innovation Step（这个咋翻译，更新？）

这步的作用是，嘿我不是还有个GPS吗（或者其他的测量，图像定位之类的）？

我们说测量值是下图那个绿色的点，然后测量是不准的嘛，假设他遵循高斯分布。

*************************以下也可以先忽略*******************************

一般来讲测量结果是可以用随机变量描述的。（根据最新的ISO和德国DIN，并不建议使用测量值加error的方式表述，而是使用随机变量描述）

*************************以上也可以先忽略*******************************

我们怎么把这个测量和那一堆现在有的，经过prediction的粒子的信息融合在一起呢？

是通过之前那个权重值。

按照测量的概率分布来重新分配权重。高斯分布的话当然是离中心点越近权重越大啦。上图举例x2<x3<x1<x4

x1[(6.1,9.2),0.4]

x2[(7.9,9),0.5]

x3[(8.3,10.8),0.35]

x4[(10.1,8.9),0.25]

好现在权重更新了，但是还有个问题！我们稍后希望用概率的方式来使用权重，但是权重加在一起大于1了怎么办？归一化一下就好了嘛。每个新权重=旧权重/求和（旧权重）

x1[(6.1,9.2),0.26]

x2[(7.9,9),0.33]

x3[(8.3,10.8),0.23]

x4[(10.1,8.9),0.16]

然后这步就完了，简单吧

第三步，Resampling step

这一步是干嘛呢？

你上一步得到的那些权重没用上对不对，这步用上了。

我们之前那些粒子不用了，重新摇色子摇几个粒子出来。不过一般来讲，粒子数量是不变的，都是能足够多就足够多。

比如之前的

x1[(6.1,9.2),0.26]

x2[(7.9,9),0.33]

x3[(8.3,10.8),0.23]

x4[(10.1,8.9),0.16]

比如x2的概率是0.33，那么新摇号摇出来的粒子可能有两个x2，因为它概率最大，x4的概率比较小可能一个没有。

然后我们认为摇出来的新的粒子权重相等。

x1[(6.1,9.2),0.25]

x2[(7.9,9),0.25]

x3[(8.3,10.8),0.25]

x4[(7.9,9),0.25]

最终的估计值可以是四个粒子的平均。

二、粒子滤波的案例代码

参考：开发者说｜手把手教你用粒子滤波实现无人车定位感觉更新阶段的代码好像和预测阶段的重复了。

粒子滤波的思想基于蒙特卡洛方法，利用粒子集来表示概率，可以用在任何形式的状态空间模型上。相比于其他滤波算法，粒子滤波在解决非线性、非高斯的问题上，有着较大的优越性。在无人驾驶领域，粒子滤波常被用于解决无人车的定位问题。

本文将以优达学城（Udacity）无人驾驶工程师学位中的定位课程为例讲解粒子滤波如何解决无人驾驶中的定位问题。

参考： opencv实现粒子滤波目标跟踪 OpenCV3学习（12.5） opencv实现粒子滤波目标跟踪_知识搬运工的博客-CSDN博客

参考：机器人粒子滤波定位（蒙特卡罗定位）_weixin_34253539的博客-CSDN博客蒙特卡洛定位

参考：particle filtering---粒子滤波（讲的很通俗易懂） particle filtering---粒子滤波（讲的很通俗易懂）_上善若水-CSDN博客_粒子滤波

三、动画演示

国外网站参考：https://www.cs.utexas.edu/~teammco/misc/particle_filter/

四、基于2D栅格地图的移动机器人全局定位参考和代码

[PR-2] PF 粒子滤波/蒙特卡罗定位 - 知乎