草莓采摘机器人——ROS导航（三）ROS自适应蒙特卡罗定位算法（ACML）理解

草莓采摘机器人——ROS导航（三）

ROS自适应蒙特卡罗定位算法（AMCL）理解

在详细解释ROS工作原理与步骤之前， 我想先Mark一下自适应蒙特卡罗定位算法的原理与思想。 毕竟算法才是王道。

AMCL(adaptive Monte Carlo Localization)自适应性蒙特卡罗定位，根据ROS官网的简介为：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.

翻译过来可以理解为： AMCL是一个关于机器人在2D空间移动的概率定位系统。 AMCL实现了自适应性蒙特卡罗定位方法，其实现方法为：通过粒子滤波来跟踪机器人在已知地图上的位姿。

对于机器人来说， 知道自己实时的位置位姿的精确定位是非常必要的。如果无法正确定位机器人的当前位置与位姿， 机器人就无法规划出正确的目的地目标与移动轨迹， 在很大程度上可以说：位姿位置的定位精度决定了规划路径与导航的精度。

自适应蒙特卡罗定位的思想

在介绍自适应蒙特卡罗之前梳理一下几个重要的几个概念：

（1）蒙特卡罗和粒子滤波
蒙特卡洛：是一种思想或方法。举例：一个矩形里面有个不规则形状，怎么计算不规则形状的面积？不好算。但我们可以近似。拿一堆豆子，均匀的撒在矩形上，然后统计不规则形状里的豆子的个数和剩余地方的豆子个数。矩形面积知道的呀，所以就通过估计得到了不规则形状的面积。拿机器人定位来讲，它处在地图中的任何一个位置都有可能，这种情况我们怎么表达一个位置的置信度呢？我们也使用粒子，哪里的粒子多，就代表机器人在哪里的可能性高。

粒子滤波：粒子数代表某个东西的可能性高低。通过某种评价方法（评价这个东西的可能性），改变粒子的分布情况。比如在机器人定位中，某个粒子A，我觉得这个粒子在这个坐标（比如这个坐标就属于之前说的“这个东西”）的可能性很高，那么我给他打高分。下次重新安排所有的粒子的位置的时候，就在这个位置附近多安排一些。这样多来几轮，粒子就都集中到可能性高的位置去了。

（2）重要性采样
就像转盘抽奖一样，原本分数高（我们给它打分）的粒子，它在转盘上对应的面积就大。原本有100个粒子，那下次我就转100次，转到什么就取个对应的粒子。这样多重复几次，仍然是100个粒子，但是分数高的粒子越来越多了，它们代表的东西（比如位姿）几乎是一样的。

（3）机器人绑架
机器人突然偏离原来的位置或轨迹。 举例，机器人突然被抱走，放到了另外一个新的地址。类似这种情况有：驱动轮打滑， 机器人漂移等等。

（4）自适应蒙特卡罗
自适应体现在：
1.解决了机器人绑架问题，它会在发现粒子们的平均分数突然降低了（意味着正确的粒子在某次迭代中被抛弃了）的时候，在全局再重新的撒一些粒子。

2.解决了粒子数固定的问题，因为有时候当机器人定位差不多得到了的时候，比如这些粒子都集中在一块了，还要维持这么多的粒子没必要，这个时候粒子数可以少一点了。

（5）KLD采样
KLD采样就是为了控制上述粒子数冗余而设计的。比如在栅格地图中，看粒子占了多少栅格。占得多，说明粒子很分散，在每次迭代重采样的时候，允许粒子数量的上限高一些。占得少，说明粒子都已经集中了，那就将上限设低，采样到这个数就行了。

总结来说, amcl(自适应蒙特卡罗)就是升级版的蒙特卡罗定位， amcl通过KLD方法更新粒子的数量以增加自适应性。 而蒙特卡罗定位方法的实现简单来说就是一开始在地图空间很均匀的撒一把粒子，然后通过获取机器人的motion来移动粒子，比如机器人向前移动了一米，所有的粒子也就向前移动一米，不管现在这个粒子的位置对不对。使用每个粒子所处位置模拟一个传感器信息跟观察到的传感器信息（一般是激光）作对比，从而赋给每个粒子一个概率。之后根据生成的概率来重新生成粒子，概率越高的生成的概率越大。这样的迭代之后，所有的粒子会慢慢地收敛到一起，机器人的确切位置也就被推算出来了。

自适应蒙特卡罗定位的实现

在编辑中 待续。。。