AMCL的粒子滤波

概述

粒子滤波，也就是Particle Filter，简写PF。
所有的定位程序被放在激光传感器数据接收的事件中，每次接收到激光雷达的数据，都会判断一下机器人是否移动了给定的距离或旋转了给定的角度（下面简称机器人满足移动调节）。如果机器人满足移动调节，则处理激光数据，否则不处理，直接丢弃。感觉这里如此浪费激光雷达数据，不是一个好的策略。
注意：下文中所说的粒子的概率和粒子的权重，是一回事。

基本流程

PF伪代码（这里没有考虑机器人绑架，所以是MCL，不是AMCL）：

实际的处理流程跟这里上面的伪代码不太一样。稍微解释下上面的伪代码。
函数输入机器人上一个状态，运动，测量和粒子数。利用运动采样模型生成M个服从给定分布的随机粒子X，利用测量模型为每个粒子评估产生测量zt的概率Pzt，并把X和Pzt组合到一起存放。
第8行开始筛选粒子，筛选的规则是，在上面产生的M个粒子中选出M个粒子，每次从中按概率Pzt去选择一个粒子。将产生的M的新粒子作为结果输出。
实际的流程不太一样，但是思想基本一致。流程如下：
1、获取机器人当前位姿，计算移动增量delta，判断机器人移动调节是否满足
2、根据上一个位姿Xt-1和移动增量delta，用运动采样模型产生（在原有粒子基础上按给定分布生成）若干数量（sample-count个）的粒子，也就是生成若干个当前状态的可能位姿。
3、将激光雷达数据从激光坐标系换算到baselink坐标系，使用激光传感器模型为每一个粒子计算在其状态下获得测量Zt的概率，并计算总概率totalweight。
4、上面已经获得了伪代码中的粒子和概率组合，就进行采样（可设定若干次数据更新采样一次）。采样后粒子数量不变，并重置所有粒子的概率为平均值。（1/sample-count）
5、对粒子进行聚类，计算每个cluster的位姿均值、概率（权重）均值、协方差等参数 （实际中此步骤和上一步是同时进行的）
6、获取平均概率最大的那一个聚类，认为是粒子滤波所得的最终机器人位姿。根据此位姿（机器人在MAP坐标系下的位置），修正ODOM坐标系下机器人的位姿（编码器累计值计算得到的机器人位姿）。完成一个PF滤波周期。

运动学模型和传感器模型属于另外两个话题，下面重点分析粒子滤波的重采样，聚类的方法这里也不讨论，后面另开文章叙述。
其实，代码中采用的重采样方法很简单，原理依然是，按照每个粒子的权重对粒子进行采样。在重采样过程中，我们还需要提起机器人绑架问题。由于粒子聚集后，万一机器人定位是错误的，或者机器人被搬动，粒子无法聚集到正确的范围内。所以还需要在定位效果不好的情况下全局随机的粒子，来模拟初始化时的情形。

定位效果的评价

方法描述：粒子X产生测量Zt的概率的含义是：在X的位姿下，能够产生实际测量Zt的概率，其实也就是反演模型产生的虚拟测量与实际测量的吻合程度。显然，定位越不正确，概率越小，定位越准确，概率越大。因此，可以监控全局平均概率，如果概率变小，说明定位越来越不准确了。

Alfa（slow）要远远小于Alfa（fast），Wavg是本次滤波得到的全局平均概率。显然Wfast会很显著地反应滤波效果，而Wslow则变化比较慢，更能反应滤波的长期效果。

如上图公式（称为滤波评价值），当滤波效果越差，Wfast就变得越来越小于Wslow，那么返回值就会越大。反之，滤波效果较好时候，或者比较稳定的时候，Wfast和Wslow比较接近，返回值就接近0。
考虑上述机器人绑架问题的蒙特卡罗滤波被称为增强的蒙特卡罗滤波，也就是AMCL。
第12行开始，筛选粒子的过程也发生了变化，原来仅仅从生成的粒子里面挑选粒子，现在还根据滤波评价值的大小随机地添加一些全局随机粒子。

重采样小技巧

在前面提供的伪代码中，都有一句draw i with probability的伪代码，也就是根据粒子的权重，去挑选粒子。实际如何做呢？
我们知道，所有粒子的权重归一化后求和为1。如果把所有粒子排列成一排，权重越大，占用位置越大，而他们占用的总位置求和是1。那么我们获取一个（0,1）的随机数，显然，这个随机数落在权重大的粒子上的可能性也更大啦。算法：
c[0] = 0.0;
for(i=0;i c[i+1] = c[i]+weight[i]; //用c来储存权重的累加值
foreach(p in all particles)
{
double r;
r = drand48();
for(i=0;i {
if((c[i] <= r) && (r < c[i+1])) //权重越大的粒子，区间就越大，随机数r落在其区间的可能性也越大
break;
}
//TODO: Select particle i
}

总结

粒子滤波的思想很简单，是基于穷举法的一种高效方法。若使用穷举法，那么就是对位姿空间中的每个可能去遍历，当然从计算量上来说这是不可能的。所以粒子滤波就是足够多的随机粒子（位姿假设）放到整个位姿空间中，然后用本文的方法去选出定位效果好的粒子，不停迭代，最终所有的粒子收敛到正确的位姿附近。
优点：能够得到机器人位姿的较优解（蒙特拉罗的），大大降低了穷举法的计算量，能够一定程度上解决机器人绑架问题
缺点：必须要机器人移动了一定距离才能执行一次滤波，机器人不运动时无法定位；粒子需要多次迭代才能收敛；计算量随着粒子数量的增加而直线上升。