AMCL算法原理讲解

AMCL算法理解

AMCL(adaptive Monte Carlo Localization)自适应蒙特卡洛定位 ，源于MCL算法的一种增强，那么为什么要从MCL升级为AMCL呢？首先应该了解MCL的算法原理

本教程是对Dr. Sebastian Thrun所著《概率机器人》（Probabilistic Robotics）一书的部分原理讲解，插图均来自该书，如有兴趣深入理解请大家购买原书进行学习

蒙特卡洛定位算法

蒙特卡洛定位适用于局部定位和全局定位两类问题，尽管它相对的年轻，但是已经成为定位领域中的主流算法，如下所示为蒙特卡洛定位算法：

通过把合适的概率运动和感知模型代入到粒子滤波算法中得到，使用M个粒子的集合 χ t = { x t [ 1 ] , x t [ 2 ] , . . . , x t [ M ] } \chi_t=\{x_t^{[1]},x_t^{[2]},...,x_t^{[M]}\} χt​={ xt[1]​,xt[2]​,...,xt[M]​}表示置信度$bel(x_t)$，初始置信度由先验分布随机产生的M个这样的粒子得到。算法第4行使用运动模型采样，以当前置信度为起点使用粒子，第5行使用测量模型以确定粒子的重要性权值（这两个模型介绍见第二、三节）通过增加粒子总数M能提高定位的近似精度。
MCL以目前的形式解决了全局定位问题，但无法从机器人绑架或全局定位失败中恢复过来。当机器人位置被获取时，其他地方的不正确粒子会逐渐消失。在某种程度上，粒子只能“幸存”在一个单一的姿势附近，如果这个姿势恰好不正确，算法就无法恢复。而这个问题可通过相当简单的探索算法解决，其思想是增加随机粒子到粒子集合，从而在运动模型中产生一些随机状态，这便是AMCL算法的由来。

蒙特卡洛定位算法自适应变种

AMCL算法在机器人遭到绑架的时候，会随机注入粒子（injection of random particles），增加粒子的方法引起两个问题，一是每次算法迭代中应该增加多少粒子，二是从那种分布产生这些粒子。
解决第一个问题可通过监控传感器测量的概率来评估增加粒子，即式$(1.1)$
$$p(z_t|z_{1:t-1},u_{1:t},m)$$
并将其与平均测量概率联系起来，在粒子滤波中这个数量的近似容易根据重要性因子获取，因为重要性权重是这个概率的随机估计，其平均值为式$(1.2)$
$$\frac{1}{M}\sum _{m=1}^M{w}_t^{[m]}\approx p(z_t|z_{1:t-1},u_{1:t},m)$$
这个接近上式中的期望概率。
解决第二个问题可以根据均匀分布在位姿空间产生粒子，用当前观测值加权得到这些粒子。如下给出增加随机粒子的蒙特卡洛定位算法自适应变种（AMCL）：

与MCL相比，这个算法跟踪式$(1.1)$的似然值的短期与长期均值，整体框架与MCL相同，但在第八行中给出了经验测量似然，并在第10、11行维持短期和长期似然平均，算法要求$0\le {\alpha }_{slow}\le {\alpha }_{fast}$ ，参数α_slow和α_fast分别估计长期和短期平均的指数滤波器的衰减率。算法的关键在第13行，重采样过程中，随机采样以以下式$(1.3)$概率增加
m a x { 0.0 , 1 − w f a s t w s l o w } max\{0.0, 1-\frac{w_{fast}}{w_{slow}}\} max{ 0.0,1−wslow​wfast​​}
否则重采样以MCL相同的方式进行，即根据式$(1.3)$如果短期似然优于长期似然，则算法将判断不增加随机采样，否则的话则按两者之比的比例增加随机采样，以这种方式可抵消瞬时传感器噪声带来的定位误差。

里程计运动模型

在第一章MCL和AMCL算法中均涉及运动模型采样，本章将介绍所使用的里程计运动模型。里程通常可通过整合轮子的编码信息来得到，许多商业机器人在固定的时间间隔产生这样的积分位姿估计，即里程计运动模型通过距离测量来估计运动。
解决机器人定位问题主要是解决从机器人内部里程计使用的坐标到物理世界坐标之间的变换问题。里程计模型使用相对运动信息，由内部里程计测量，在时间间隔$(t-1,t]$内，机器人从位姿x_t-1运动到x_t，里程计反馈了从<