AMCL介绍

AMCL介绍

AMCL( Adaptive Mentcarto Localization)，自适应蒙特卡洛定位，基于粒子滤波器的定位算法，与gmapping的定位方法类似，但主要实现定位功能。

MCL：蒙特卡洛定位介绍
MCL算法流程：

1. 随机生成粒子，粒子权重相等。更常用是位置初始化可以设为(0,0,0)。
2. 根据运动模型估计新时刻位姿
3. 根据观测模型及观测值，更新该时候粒子携带位姿的权重
4. 重采样，调整粒子分布，收紧粒子(去掉可信度低的粒子，增加新的)
5. 计算粒子权重的均值和方差，得到最佳位置估计。
6. 回到第2步，进行下一帧的扫描匹配。
   
   MCL问题：

• 权值退化：如果任由粒子权值增长，只有少数粒子的权值较大，其余粒子的权值可以忽略不计，变成无效粒子，因此需要引入重采样。需要衡量粒子权值的退化程度。
• 粒子多样性丧失：通常我们会舍弃权值较小的粒子，代之以权值较大的粒子。这样会导致权值小的粒子逐渐绝种，粒子群多样性减弱，从而不足以近似表征后验密度。从而出现正确粒子被去掉的可能性，而后出现粒子的奇异性丧失，总体表现就是粒子的权重均值偏低。

改进算法：AMCL
AMCL 结合自适应（Augmented_MCL）和库尔贝克-莱不勒散度采样（KLD_Sampling_MCL）算法

• Augmented_MCL：
  在机器人遭到绑架的时候，它会在发现粒子们的平均分数突然降低了，这意味着正确的粒子在某次迭代中被抛弃了，此时会随机的全局注入粒子（injection of random particles）。
  
  算法流程上看，augmented_MCL算法最显著的区别就是引入了四个参数用于失效恢复：
  $w_{slow}$wslow​：长期似然平均估计
  $w_{fast}$wfast​：短期似然平均估计
  $alpha_{slow}$alphaslow​：长期指数滤波器衰减率
  $alpha_{fast}$alphafast​：短期指数滤波器衰减率
  失效恢复的核心思想是：测量似然的一个突然衰减（短期似然劣于长期似然）象征着粒子质量的下降，这将引起随机采样数目的增加。
  $w_{avg}$wavg​计算了粒子的平均权重，当粒子质量下降时，平均权重随之下降，$w_{slow}、w_{fast}$wslow​、wfast​也会随之下降，但是显然$w_{fast}$wfast​下降的速度要快于$w_{slow}$wslow​——这由衰减率决定，因此随机概率$p = 1 - frac{w_{fast}}{w_{slow}}$p=1−fracwfast​wslow​会增大，随机粒子数目增加。而当粒子质量提高时，粒子短期权重要好于长期，随机概率小于0，不生成随机粒子。

• KLD_Sampling_MCL：
  动态调整粒子数，当机器人定位精确时(粒子都集中在一块)，减少粒子数，当定位不精确时，增加粒子数。
  在栅格地图中，看粒子占了多少栅格。占得多，说明粒子很分散，在每次迭代重采样的时候，允许粒子数量的上限高一些。占得少，说明粒子都已经集中了，那就将上限设低。bin应该是指栅格直方。一个bin对应一个栅格，用来统计有多少粒子在这个栅格中。k则是粒子占有的网格数，根据k值调整粒子数。
  

参考

《Probabilistic_Robotics》
http://wiki.ros.org/amcl
amcl介绍