Monte Carlo 定位：用一群"侦察兵"解决机器人定位

开篇

上一章的马尔可夫定位搭好理论框架，Monte Carlo 定位（MCL）让它真正落地：全局定位、绑架恢复、算力可调，一套粒子滤波搞定。本文拆解 MCL 的三步循环，讲透随机粒子注入、自适应粒子数与混合提议分布等改进，读完你不仅明白 MCL 为何应用最广，还能看懂 ROS amcl 参数背后的原理。

MCL = 粒子滤波 + 定位

把第 4 章的粒子滤波直接套到第 7 章的定位问题上，就得到了 Monte Carlo 定位（MCL）——目前最流行的实用定位算法之一。状态取 $x_t=(x,y,\theta )$，置信度用 $M$ 个带权粒子 {xt[m],wt[m]}\{x_t^{[m]},w_t^{[m]}\}{xt[m]​,wt[m]​} 表示：

$$\mathrm{bel}(x_t)\approx \sum _{m=1}^M{w}_t^{[m]}\delta (x_t-x_t^{[m]})$$

三个标准步骤直接对应第 4 章的 SIR 粒子滤波：

1. 运动采样：$x_t^{[m]}\sim p(x_t\mid x_{t-1}^{[m]},u_t)$，调用第 5 章 sample_motion_model_*；
2. 观测赋权：$w_t^{[m]}=p(z_t\mid x_t^{[m]},m)$，调用第 6 章观测模型；
3. 重采样：按 $w$ 抽 $M$ 个粒子，权重重置为 $1/M$。

主算法一览

算法: MCL(X_{t-1}, u_t, z_t, m)
─────────────────────────────────────────────
输入: 上一时刻粒子集 X_{t-1}={x_{t-1}^{[m]},w_{t-1}^{[m]}},
      控制 u_t, 观测 z_t, 地图 m
1. X̄_t = X_t = ∅
2. 对 m = 1..M:
     x_t^{[m]} = sample_motion_model(u_t, x_{t-1}^{[m]})    // 第5章
     w_t^{[m]} = measurement_model(z_t, x_t^{[m]}, m)       // 第6章
     X̄_t = X̄_t ∪ {(x_t^{[m]}, w_t^{[m]})}
3. X_t = low_variance_resample(X̄_t, M)                     // 第4章
4. 返回 X_t
─────────────────────────────────────────────

初始化方式决定定位类型

• 位姿跟踪：粒子集中在已知初始位姿附近的小高斯内；
• 全局定位：粒子在整个地图上均匀撒开；
• 后续运动 + 观测的迭代会让粒子收敛到真实位姿。

直觉：MCL 像"派一群侦察兵在地图上各自站位，每轮按运动模型走一步、按观测打分；分数低的逐渐被淘汰，分数高的繁殖出更多侦察兵，最终所有人围拢到真实位姿"。

全局定位与收敛性

粒子方法最漂亮的一点，是能自然表达多峰后验——机器人可能在 A、B、C 三处，粒子就同时在三处扎堆。随着观测积累，错误峰上的粒子权重持续偏低，被重采样淘汰，最后只剩一个真峰。收敛速度取决于地图信息量与观测判别力；信息丰富的环境中，几步即可收敛。

遭遇"绑架"：MCL 的软肋与解药

软肋：粒子匮乏

MCL 一旦收敛，所有粒子都挤在真峰附近，多样性极低。此时若机器人突然被搬走（绑架），原有粒子完全失效，且无法自我恢复。

解药一：随机粒子注入

每次迭代以小概率 $\alpha$ 注入均匀分布的随机粒子：

算法: Augmented_MCL（带随机注入）
─────────────────────────────────────────────
1. 以概率 w_fast, w_slow 跟踪长期/短期平均观测概率
2. 常规 MCL 采样赋权后:
     注入粒子数 ≈ M · ε · w_slow / w_fast
     其中新粒子从全图均匀分布采样
3. 重采样
─────────────────────────────────────────────

诀窍在于比值 $w_{\text{slow}}/w_{\text{fast}}$：观测概率持续偏低（被绑架）时它增大，系统自动注入更多随机粒子，重新探索全图。

解药二：自适应粒子数

不确定性大（粒子分散）时用更多粒子保精度；不确定性小（粒子聚集）时减少粒子省算力。可用 $N_{\text{eff}}=1/\sum (w^{[m]}{)}^2$ 监控。

更聪明的采样：混合提议分布

标准 MCL 用运动模型做提议分布，常把粒子撒到观测匹配很差的区域——纯属浪费。改进思路是用观测引导采样：

$$\pi (x_t)\propto p(z_t\mid x_t,m)\overline{\mathrm{bel}}(x_t)$$

即直接从"预测分布 × 观测似然"采样。这通常需要传感器模型支持解析采样（比如从激光端点反推候选位姿）。

直觉：标准 MCL 是"先随便走一步再看像不像"；改进版是"先看一眼观测，从最可能产生这种观测的位姿里挑粒子"。后者效率高得多，但实现更复杂（FastSLAM 2.0 也用了类似思想）。

MCL 的优缺点

优点	缺点
多峰、全局定位、绑架恢复	粒子数与维数正相关，3D 以上位姿成本高
实现简单、并行性好	重采样导致粒子匮乏
任意非线性模型	提议分布次优时效率低

要点回顾

• MCL 把粒子滤波套到定位上：运动采样 → 观测赋权 → 重采样；
• 天然支持全局定位与多峰后验，是工程上最常用的实用算法；
• AMCL 用随机粒子注入应对绑架问题；自适应粒子数在精度与算力间权衡；混合提议分布用观测引导采样提升效率；
• MCL 是第 13 章 FastSLAM 的基础（FastSLAM 用粒子表示轨迹，每条粒子内挂一个地图滤波器）。

MCL 也是 ROS amcl 包的核心；第 9 章是反向问题——已知位姿时用观测建占据栅格地图；第 11~13 章 SLAM 同时未知位姿与地图。

延伸思考

• ROS amcl：开源实现即基于本文的 AMCL，参数 recovery_alpha_*、laser_max_range、粒子数等均对应文中概念。
• 3D 位姿 MCL：六自由度位姿（含 roll/pitch）需要更多粒子；可用 GPU 并行或分层 MCL 缓解。
• 提议分布设计：好的提议分布可以减少一个数量级的粒子。视觉/激光里程计可作为强提议，结合 MCL 做后端验证。
• 重采样频率：并非每步都重采样最优，只在 $N_{\text{eff}}$ 低于阈值时重采样可保留多样性。
• 静态环境假设：MCL 假设地图 $m$ 已知且静态；动态障碍（行人、移动物体）会污染观测概率，需在观测模型中显式建模或时间滤波。

写在最后

回顾全文：MCL 用"运动采样—观测赋权—重采样"三步让贝叶斯滤波落地，天然表达多峰后验，是移动机器人定位的事实标准。随机粒子注入、自适应粒子数与混合提议分布，撑起了 ROS amcl 等工业实现；扩展到同时估计地图，便是 FastSLAM 的起点。理解 MCL，就握住了现代 SLAM 的主干。