041｜Monte Carlo Localization：让位置假设跟着证据聚拢

总览 · 目录 · 上一篇 · 下一篇

论文：Frank Dellaert、Dieter Fox、Wolfram Burgard、Sebastian Thrun，Monte Carlo Localization for Mobile Robots，1999，ICRA，1322–1328。DOI · 作者机构保存的原稿。同年 Fox 等人的 AAAI 论文题名带有 Efficient Position Estimation，是相关但不同的文章；这里使用 ICRA 版本的实验和作者顺序。

先接住前作的问题

Bootstrap Filter提供分布近似工具，本篇把它放进已有地图中的机器人位姿估计。地图已知是重要边界。

五个前置概念

五个前置概念是：后验分布表达当前位置知识；运动模型传播里程计不确定性；测量似然比较观测与地图；重采样将样本资源集中到高概率区域；可辨识性决定不同位置能否被现有传感区分。

一、问题设定：已知地图中怎样从多个位置假设逐步定位

机器人刚开机时不知道自己位于地图哪里。若建筑里有两条相似走廊，一次测距可能同时支持两个位置。马上挑选其中一个会有风险，而在整张地图上保存高分辨率位置概率网格又很昂贵。

背景与问题：定位已有地图，不是同时建图

状态为 $x_t=(p_x,p_y,\theta)$，即平面位置和朝向。地图 $m$ 已知或此前已经建立，输入包括控制/里程计 $u_t$ 与激光、超声等观测 $z_t$。目标是维护

$$bel_t(x)=p(x_t=x\mid z_{1:t},u_{1:t},m).$$

这篇论文研究定位。虽然实验地图可能来自建图系统，但 MCL 本身没有在这里联合估计未知地图。将它写成 SLAM，会混淆“地图作为条件”和“地图作为未知量”的根本区别。

局部跟踪从一个已知附近的位置开始，只需维持小范围后验；全局定位则从全地图分布开始，需要保留多个相似位置。绑架机器人问题又不同：已收敛的粒子云突然远离真实位置，算法需要重新产生正确区域的假设。能够从均匀先验全局定位，并不自动保证任意绑架后恢复。

早期 Kalman 型定位在近似单峰情况下紧凑高效，但不能用单一高斯完整表达走廊对称性；网格 Markov 定位能表达多个位置，却要选择空间和角度分辨率。网格足够细时内存和更新量很大，网格太粗又限制精度。MCL 改变的是概率表示方式。

二、用运动传播与地图似然更新位姿粒子

方法：同一套贝叶斯递推，换一种表示

预测步骤为

$$\overline{bel}_t(x)=\int p(x\mid x',u_t)bel_{t-1}(x')dx',$$

测量更新为

$$bel_t(x)=\eta\,p(z_t\mid x,m)\overline{bel}_t(x).$$

$\eta$ 是归一化因子。运动模型表达里程计误差和运动不确定性，观测似然表达某个位置在地图中预期看到的测距与真实测距的相容程度。似然不是后验概率；只看“当前位置能解释观测”还不够，也要结合历史运动。

MCL 不把 $bel$ 存成每个格子的数值，而是保存等权粒子集合。预测时从每个粒子的运动模型采一次新状态；更新时按观测计算临时权重；重采样后再得到新的等权集合。

原文 §4.2 用重要性采样解释为什么该形式的权重只含测量似然：提议分布就是预测分布，目标后验与提议的比值中，预测项相消。若使用额外的观测引导提议，权重则需要重新推导。

原稿式 (3) 把经验预测写成混合分布的求和。规范化的教学表达应带等权系数 $1/N$；该常数不改变后续归一化权重，却是把求和称作概率密度时必须明确的因素。它不是算法的额外改进，而是记号层面的说明。

为什么粒子没有固定网格精度，却仍会不准确

粒子坐标可取连续值，所以不像固定单元格那样把所有位置限制在预定分辨率上。当前最可能位置附近获得更多粒子时，表示会自然变细；远离高概率区域的计算投入减少。

但“连续坐标”不等于无限精度。有限粒子可能没有覆盖正确模式，重采样可能过早复制错误峰，观测模型也可能系统性偏差。地图的分辨率和几何误差仍然存在；粒子表示只取消了后验必须与固定位置格一一对应的约束。

数学任务是近似后验，而不是优化轨迹回报；机制改动是用采样和重采样分配表示资源；实验指标包括参考点定位误差、全局定位时间和长时间是否丢失跟踪。把这三个层次分开，就能看懂为什么增加粒子数不会让每项系统指标单调改善。

“连续位置”与实时精度之间隔着更新频率

若地图有两个相似走廊，初始100个粒子各分50个；一次观测对两走廊的似然比为3，后验质量变为0.75与0.25。下一步里程计只移动这些假设，并不自动消除歧义；要遇到能区分两走廊的观测，才会真正破除对称。这是Bootstrap Filter的直接任务化，不是给每个地图格设一个独立概率。

教学实时预算：假设一次粒子观测评分需10微秒，2000粒子用20毫秒，20000粒子用200毫秒。若机器人每秒走1米，忽略排队时后者单次计算期间已走0.2米。离线相同时间戳上粒子更多可能更精确，在线若不能及时处理观测，实际误差却可能变大。这里数字是构造的成本示例，不是原文计时。

原文引言把全局定位与已定位后的跟踪分开；因此实验要分别看初始化成功、持续跟踪和处理吞吐。FastSLAM之后把轨迹粒子与条件地图组合，才进入同时建图，不能把MCL在已有地图的性能直接当成SLAM性能。

同一观测流中增加粒子与实时处理更多数据是两回事。下面先落实运动与测量更新，再用日志和实时预算拆开这两种收益。

三、实现全局到局部定位，检查实时丢帧

输入：已知地图、运动模型、测量模型、粒子数 N
从初始先验采样 N 个位置/朝向粒子
对每个新的运动与测量周期：
    对每个旧粒子 xi：
        从 p(xnew | xi, u) 采一个预测粒子 yi
        计算 wi = p(z | yi, map)
    归一化所有 wi
    按 wi 重采样 N 个粒子，成为新的等权集合
    判断粒子是否仍有多个分离的位置模式
    输出位置分布；在适合时输出单一位置估计

一个小例子与五个前置概念

自构例子，未运行。 两条走廊完全对称，初始粒子分别占一半。机器人向前走两米，两组假设都能解释里程计，因此仍应保留两组。随后左侧出现只有第一条走廊才有的房门，两组似然分别为 0.9 和 0.1，新后验比例约变为 9:1。如果没有出现这扇门，不能仅凭“走了一段时间”宣称定位已经唯一。

四、对照后验图、误差曲线与长日志

读图：横轴粒子数采用对数尺度，纵轴是平均定位误差；粒子减少节省计算，却可能损害估计质量。声呐与激光曲线的差异也表明，地图和观测信息量会改变预算需求。

实验设计：日志重放也必须计算实时成本

参考点误差图与博物馆长日志分属不同评价：前者比较定位精度，后者检验持续运行。粒子数变化还会改变实时系统能处理多少测量。

作者使用先前采集的真实机器人日志，而不是让每个配置都重新在现场独立跑一次。重放保留时间戳以体现实时传感流；它支持定位计算的对比，但不会模拟一个定位错误如何改变后续机器人动作、继而改变未来观测。

MINERVA 运行发生时馆内没有参观者，机器人由网络远程用户控制。虽然平台用于导览，不能把这段结果改写成“拥挤博物馆中自主导览 75 分钟”。场景名称不等于全部实验条件。

五处证据：后验图、误差曲线和长日志分别说明什么

1．Fig. 1，PDF p.4。 已知大致位置但朝向未知，移动后预测形成环状，再用地标观测重新加权。它直观解释了为什么一个均值协方差不总能充分描述状态；这是教学示意，不是现场误差测量。

读图：A到D依次对照先验、运动预测、观测加权和重采样；上排连续密度与下排粒子云对应，粒子向观测兼容的两个区域集中。

2．Fig. 3–5，PDF p.5。 20000 粒子先遍布可行区域，随后聚在两个由走廊对称性产生的位置，进入可区分的房间后才收敛到真位置。它支持保留歧义的必要性；若地图永远完全对称，更多计算也不一定能创造可辨识信息。

读图：后续观测消除先前对称歧义，粒子收敛到正确房间；这组帧展示全局定位过程，不是所有布局的成功率。

读图：环境的对称结构造成两个相互分离的假设，单一高斯不足以表达这种歧义。

读图：初始粒子在多个走廊与房间中分布，体现尚未知道全局位姿时保留多种假设。

3．Fig. 6–7，PDF p.5。 22 个参考点用于位置精度比较，网格单元变大时平均误差增加。95% 区间属于所述评估设置，并非跨任意建筑的泛化保证。

读图：编号参考位置沿路径分布，用来测量实际定位误差；这里是评价所依赖的地图与路线，不是粒子数量的性能曲线。

读图：横轴是网格单元尺寸，纵轴是平均估计误差；两种传感器的误差都随网格变粗而增大，可与本文已有图8的粒子数实验分开比较。

4．Fig. 8，PDF p.6。 MCL 误差最初随粒子数增加而下降，随后因为处理变慢、丢掉观测而变差。作者说网格需要约 4 cm 单元才能达到相应精度；这不是说 MCL 的误差固定等于 4 cm。横轴是粒子数的对数尺度，不是运行时间。

5．Fig. 9 与 §5.3，PDF p.6。 5000 粒子的一次跟踪运行持续 75 分钟、累计超过 2200 m，最高速度约 1.6 m/s，作者报告期间未丢失跟踪；同环境的全局定位报告少于 10 s。最高速度、累计距离与定位耗时描述不同实验量，不能相互替代。

尚未内嵌：图 9。三阶段多峰定位过程及定量误差已经补齐；剩余是大型博物馆场景地图。当前评论使用运行里程、速度和时间，地图本身不能直接验证这些数字，故不继续扩展场景图像的累计展示。

这组证据支持“在这些数据和计算安排中，粒子表示提供了有效的精度与资源折中”。它没有给出任意地图上的最优粒子数，也没有证明其内存、速度、精度对每种网格优化版本都严格占优。

五、保留粒子贫化、绑架恢复与计算预算限制

贡献是把成熟的粒子滤波思想落实为机器人全局定位与跟踪的统一框架：无需先决定每个位置单元的精细程度，后验自己决定样本集中在哪里。它与前两篇采样估计在统计结构上相通，但状态和传感模型转向机器人与地图。

作者在 §6 承认 sample impoverishment，即重采样复制高权重粒子使多样性下降。运动噪声可以重新扩散样本，但不能保证迅速重新覆盖远处的真实位置。自动调整粒子数在这篇论文中也是后续研究方向，不能把后来 AMCL 的所有自适应机制都写成原版已经具备。

本文进一步分析：地图重复结构、动态遮挡、错误传感器似然和里程计突变都可能让后验自信地集中到错误位置。下游导航应保留不确定性与多峰信息；只输出一个最可能点会隐藏“另一个走廊也解释得通”的事实。

六、追问：两个相似走廊对应的粒子峰，什么时候会被同一观测进一步区分？

两个相似走廊对应的粒子峰，什么时候会被同一观测进一步区分？

参考答案：对两个候选位置A、B，更新后的权重比满足 $w_A^+/w_B^+=(w_A^-/w_B^-)(L_A/L_B)$。若当前扫描在两处的地图似然相同，观测不会改变它们的相对可信度；必须等待能产生不同预测的运动或观测。这个两假设验算沿Särkkä的贝叶斯更新结构，不是声称一次测距就能消除全局定位歧义。

七、从已知地图定位走向轨迹条件化建图

• 模块组合：FastSLAM：不删除相关性，而是把它放进轨迹假设。把位姿粒子扩展为轨迹假设，条件于轨迹估计未知地图。

八、自测与参考答案

问题 1：MCL 为什么不是 SLAM？

参考答案：本文把地图作为已知条件，只估计机器人位置与朝向。SLAM 要同时推断地图及轨迹，两者相互耦合。使用一张由其他系统建出的地图，不会使后续定位算法自动变成 SLAM。

问题 2：固定观测序列下更多粒子近似更好，为什么实时实验可能相反？

参考答案：实时系统有计算期限。粒子更多使每次更新变慢，可能无法处理后续测量；此时不同设置实际融合的观测并不相同。表示误差减少与信息丢失增加同时发生，所以系统误差可能呈非单调关系。

问题 3：从均匀先验能全局定位，是否意味着机器人被搬走后也会自动恢复？

参考答案：不一定。收敛后的粒子大多集中在旧位置，如果运动模型不允许大跳跃，新真实位置可能没有粒子。全局初始化和故障后重新扩散是两个机制；后者需要额外恢复策略或合适的提议设计，不能从前者直接推出。