粒子滤波器又叫蒙特卡罗定位,是解决机器人定位问题的一种方法.我们来对照马尔可夫定位,来看一下粒子滤波器.

马尔克夫定位及粒子滤波器以及卡尔曼滤波等等滤波都是分为两个主要步骤:
第一步是预测,根据上一时刻的运动状态估计下一时刻的运动状态.
第二步是更新,根据传感器的测量值修正第一步对运动状态的估计.

马尔可夫定位

我们来回顾一下马尔克夫定位,先来看第一步是预测,因为可以根据GPS把地图确定到一个比较小的范围,所以我们假设地图为一个4*6的方格子,马尔克夫定位会根据运动模型计算地图中所有位置的概率:

在来看马尔克夫定位的第二步更新,首先使用人工智能及计算机视觉技术在地图上确定几个地标,通过传感器测量到的车辆与各地标之间的距离与地图中各地标之间的距离进行比对,从而计算出条件概率,与第一步的概率相乘的到后验概率:

最终概率最大那个位置就是马尔克夫定位得出的机器人的位置,然后循环上述步骤.

粒子滤波

OK,我们回顾完马尔克夫定位的原理之后来看一下粒子滤波,首先粒子滤波选取了很多粒子分布在地图中.

第一步预测,假设粒子位置就是机器人位置,根据上一时刻的机器人速度及方向预测下一时刻粒子的位置:

接下来,根据传感器测量得出的机器人位置与上一步根据运动模型得到的机器人位置计算偏差值,偏差值越小权重越大,偏差越大权重越小.

最后重采样,比如之前是100个粒子,现在我在这100个粒子中按照权重的大小重新选取100个,权重大的被选中的可能性大,这样权重较小的粒子会被干掉,权重大的粒子会被多次选择,由于有噪声的存在相同的粒子在接下来的表现也会不同.

如此我们就完成了粒子滤波滤波的一条流程,最后权重最大粒子作为最终的结果.重复以上操作你会发现粒子会变得越来越集中.

Resampling wheel

重采样的算法很容易实现,不过这里介绍一个比较有趣的重采样算法.叫做Resampling wheel.为什么叫wheel呢?因为它和赌桌上的轮盘比较像.

我们把粒子按照权重的大小摆在轮盘上,权重大的所占面积就大,权重小的占面积就小.

第二种情况,的值大于index所指的权重: