AMCL源码架构讲解与详细分析

AMCL算法简介

AMCL(adaptive Monte Carlo Localization)自适应蒙特卡洛定位 ，源于MCL算法的一种增强，本章简要介绍AMCL的算法原理并着重讲解源码包（详细算法原理请见ROS及SLAM进阶教程（二）AMCL算法原理讲解）
AMCL是2D的概率定位系统，这个方法是在已知地图中使用粒子滤波方法得到位姿的。输入激光雷达数据、里程计数据，输出机器人在地图中的位姿。如果里程计没有误差，完美的情况下，我们可以直接使用里程计信息推算出机器人（base_frame）相对里程计坐标系的位置。但现实情况，里程计存在漂移以及无法忽略的累计误差，所以AMCL采用先根据里程计信息初步定位base_frame，然后通过测量模型得到base_frame相对于map_frame（全局地图坐标系）的偏移，也就知道了机器人在地图中的位姿。（注意，这里虽然估计的是base到map的转换，但最后发布的是map到odom的转换，可以理解为里程计的漂移。）
与MCL不同的地方是AMCL算法在机器人遭到绑架的时候，会随机注入粒子（injection of random particles），增加粒子的方法引起两个问题，一是每次算法迭代中应该增加多少粒子，二是从哪种分布产生这些粒子。
解决第一个问题可通过监控传感器测量的概率来评估增加粒子，即式$(1.1)$
$$p(z_t|z_{1:t-1},u_{1:t},m)$$
并将其与平均测量概率联系起来，在粒子滤波中这个数量的近似容易根据重要性因子获取，因为重要性权重是这个概率的随机估计，其平均值为式$(2.2)$
$$\frac{1}{M}\sum _{m=1}^M{w}_t^{[m]}\approx p(z_t|z_{1:t-1},u_{1:t},m)$$
这个接近式$(1.1)$中的期望概率。
解决第二个问题可以根据均匀分布在位姿空间产生粒子，用当前观测值加权得到这些粒子。如下给出增加随机粒子的蒙特卡洛定位算法自适应变种（AMCL）：

算法第5行使用运动模型采样，以当前置信度为起点使用粒子，第6行使用测量模型以确定粒子的重要性权值。这个算法跟踪式(1.1)的似然值的短期与长期均值，在第8行中给出了经验测量似然，并在第10、11行维持短期和长期似然平均，算法要求 0 ≤ α s l o w ≤ α f a s t 0\leq\alpha_{slow}\leq\alpha_{fast}