AMCL（Adaptive Monte Carlo Localization）算法的原理详解

1. AMCL 的背景与定位任务

在移动机器人中，我们常遇到 定位问题：
已知机器人的地图（Map）和传感器观测数据（激光雷达、里程计等），推断机器人在地图中的位置与姿态（$x,y,\theta$）。

这个任务属于 全局定位问题 或 跟踪定位问题：

• 全局定位（Global Localization）：机器人初始位置未知
• 跟踪定位（Tracking）：已知初始位置，大部分时候是位置跟踪

AMCL 是 蒙特卡洛定位（MCL） 的改进版，本质是 基于粒子滤波的贝叶斯定位方法，并引入 KLD-采样 动态调整粒子数量，减少计算量。

---

2. 粒子滤波（MCL）的基本原理

粒子滤波是通过一组粒子 { x t [ m ] } m = 1 M \{x_t^{[m]}\}_{m=1}^M { xt[m]​}m=1M​ 来近似位置的概率分布 $p(x_t|z_{1:t},u_{1:t})$。

粒子滤波步骤：

1. 初始化（Initialize）：在地图上均匀采样粒子，或围绕初始估计采样

2. 预测（Prediction / Motion Update）：根据运动模型更新粒子位置

   $$x_t^{[m]}\sim p(x_t|u_t,x_{t-1}^{[m]})$$

3. 更新（Update / Measurement Update）：根据传感器观测更新粒子权重

   $$w_t^{[m]}\propto p(z_t|x_t^{[m]},m)$$

4. 重采样（Resampling）：根据权重采样新粒子，避免权重退化

---

3. AMCL 的核心改进：KLD-Sampling

标准 MCL 缺点：

• 粒子数固定，计算量大
• 如果粒子数太少 → 定位