自适应MCL(KLD采样)

之前介绍过粒子滤波器(传送门),在全局地图环境下,传统粒子滤波器在sample数量过大的情况下,计算效率会非常的低。但是如果在非常大的地图环境下,少量的sample数又会导致定位的发散。这样,在早期阶段可能需要有数十万的样本来完成初期的定位,维持一个如此大的样本集合是有必要的,一旦机器人确定了自己的位置之后只需要一小部分粒子就足以跟踪机器人的位置了。

KLD采样是MCL定位的一种变形,它根据时间自适应的调整粒子数量。这里我们不提供KLD采样的数学推导,只是陈述算法过程并展示一些实验结果。 KLD采样的全称是库尔贝克-莱布勒散度Kullback-Leibler Divergence,它是一种计算两个概率分布之间差异的方法。KLD采样背后的思想就是根据基于采样近似质量的统计界限来确定粒子数量。 更特殊的,在粒子滤波器的每次迭代过程中,KLD采样以概率1−σ来确定样本数量,真实的后验概率与基于采样的近似分布之间的差异小于ε。 还有几个假设可以让我们有效的实现这一思想,这里先不详细解释这些假设了。

KLD采样会不断的生成粒子直到满足了第16行中的统计边界为止。Mx表示的就是样本的数量,我们可以注意到,样本数量的计算依赖于K的大小,KLD的核心算法就在这。判断每一个样本是不是在一个样本分布bin内。整个地图可以想象成是一个很大的环境,然后我们划分出指定分布的b个区域。当采样获取到的sample发现是在其中一个b中的时候,我们会标注这个b是占的,这样下次再有想吐地域的sample被挑选中,就不会增加k的大小,这样就可以在有很多集中的sample的情况下,减少总量,也就是可以排除掉很多发散的sample。

该算法不断地生成粒子,直到粒子数量M超过了Mχ或者用户定义的上限Mχmin。可以看出Mχ是M的一个运动目标(moving target)。生成越多的样本, 就有更多的直方非空,阈值Mχ就越高。

实际上,该算法是由于如下的原因才停止的。在采样的早期阶段,基本上每生成一个新样本k都要增加,因为此时基本上所有的直方都是空的。k的增加将导致阈值Mχ上升。 但是,随着时间的推移,越来越多的直方都是非空的了,Mχ偶尔才会增加一次。而每次产生新样本的时候M都会增加的,最终M会达到阈值Mχ算法停止。 计算置信度的时候,粒子分布越分散,就填充了越多的直方,因而需要更大的阈值Mχ。在跟踪定位过程中,KLD采样会产生较少的粒子,这是因为粒子集中在少数几个不同的直方中。 我们需要注意到,直方图对于粒子的分布并没有什么影响。它的唯一目的就是测量置信度的复杂度,或者说是状态空间的体积。在每次迭代之后这个直方图或者栅格就会被抛弃掉。

看下KLD的实现效果:

我们发现,在一开始的时候sample数非常的多,但是经过200秒不到以后,一旦确定了机器人的位置, sample数急剧下降,还不到原来的1%。看淡sonar没有laser下降的早,这是因为从全局定位到位置跟踪转变的时机和速度依赖于环境的类型和实际使用的传感器。这里高精度的激光传感器就更早的进行了切换。

真实实现:

Ros提供了AMCL功能包,wiki.ros.org/amcl

amcl is a probabilistic localization system for a robot moving in 2D. It implements the adaptive (or KLD-sampling) Monte Carlo localization approach (as described by Dieter Fox), which uses a particle filter to track the pose of a robot against a known map.

从介绍中看到使用的就是KLD采样的自适应MCL算法进行定位。

目前AMCL用的都是默认的设置,来看下具体的效果:

这是我创建的一个小车运动环境,由于环境比较简单,所以定位并不是太困难,可以看到初始状态下,AMCL过滤出的sample都在小车的附近,这样基本就确定次奥车具体所在的位置了。

我们在来看下小车移动起来的效果: