2021-10-22 ACML概念与应用

一、概念

AMCL(adaptive Monte Carlo Localization)自适应蒙特卡洛定位，官方解释：
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.

MCL蒙特卡洛 ，这是一种基于粒子滤波的思想或者方法，举例：地图里有一个开放的小盒子，如何计算盒子的底面积？我们可以拿一堆豆子，均匀的撒在地图上，然后统计盒子里的豆子的个数和地图上剩余地方的豆子个数。地图面积已知，所以就通过估计得到了盒子的底面积。拿机器人定位来讲，它处在地图中的任何一个位置都有可能，这种情况我们怎么表达一个位置的置信度呢？我们也使用粒子，哪里的粒子多，就代表机器人在哪里的可能性高。我们将粒子均匀撒在地图上以后，通过机器人的感官（传感器）不断去聚集这些粒子的位置（粒子滤波），粒子密集的地方就是机器人的估计位姿了。

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

那蒙特卡洛加上了自适应是什么意思？

1. 当粒子们的平均分数突然降低，说明正确的粒子被抛弃不要了，也就是机器人突然被抱到另外一个位置（机器人绑架），这时，在全局地图重新再撒上一批粒子，重新定位。解决了机器人绑架问题。
2. 当机器人的定位很准确了，比如机器人静止在一个地方，这个位置也估计的比较准确，那么就没有必要在这个位置再维持那么多的粒子了，就可以适当减小粒子数提高计算速度。解决了粒子数固定的问题。用的KLD方法

KLD（kullback-leibler divergence）又是怎么控制粒子数量的呢
在栅格地图中，如果粒子占了很多栅格，说明粒子很分散，在每次迭代重采样的时候，将粒子数量上限调高一些。占得少，说明粒子都已经集中了，那就将上限设低，采样到这个数就行了。

二、应用

放出官方图：

第一个流程图想表示如果里程计没有误差，航迹推算（Dead Reckoning）得到的位姿是准确的，我们可以直接使用里程计信息来推算出机器人（base_frame）相对里程计坐标系（/odom_frame）的位置。但是现实是里程计存在漂移以及无法忽略的累计误差，所以AMCL采用第二个流程图的方法，即先根据里程计的DR信息初步定位base_frame，然后通过测量模型得到base_frame相对于map_frame（全局地图坐标系）的TF，也就知道了机器人在地图中的位姿。（注意，这里虽然估计的是base到map的转换，但最后发布的是map到odom的转换，可以理解为里程计的漂移。）
三个坐标系的关系如下图：

现在就进入到代码中去具体分析。