5. gmapping 的两大改进：粒子生得更准，死得更慢

上一篇：4. RBPF：把鸡蛋问题拆开 ｜ 下一篇：6. 源码走读：一帧激光的旅程

朴素 RBPF 有两个坑（第 4 篇结尾）：粒子撒不准 + 重采样太勤。gmapping 论文就干了两件事：改进提议分布（撒得准）+ 自适应重采样（死得慢）。这两件事让它用 30 个粒子干出了别人 3000 个粒子的活。

一、改进一：提议分布–先对齐，再撒粒子

1.1 先搞懂”提议分布”是什么（60 秒版）

回顾 3. 粒子滤波：一千个分身猜位置：预测阶段粒子从某个分布里采样，这个”从哪撒”的分布就叫提议分布（proposal distribution），记作 $q(x)$。

理想情况：直接从目标分布（真实的后验，”我到底在哪”的真实概率云）撒粒子–但算不出来（第 3 篇说过）。

所以只能找个好算又尽量接近目标的替身 $q(x)$，再靠权重修正两者差距：

权重的本质 = 目标分布与提议分布的不一致程度。提议分布越接近目标，需要的权重修正越小，粒子利用效率越高。

1.2 朴素做法烂在哪

朴素 RBPF 的提议分布只信里程计：

$$q(x_t) = p(x_t \mid x_{t-1}^{(i)},\ u_t) \quad \text{（运动模型）}$$

拆解读法：粒子 = 老位姿 + 走一步 + 加噪声。里程计说在 A，粒子就全撒在 A 附近–但这帧激光 $z_t$ 的信息完全没用上！

1
2
3
4
5
里程计说: "我在 A" (✕)
真实位置: A 偏右 30cm (★)
朴素粒子: ●●●●●●●●  全撒在 A 附近
                              ★ 真值附近一个粒子都没有!
                              -> 权重修不回来, 这一帧白费

里程计漂移大（打滑、快转）时尤其惨：撒了一堆点，真值附近颗粒无收。

1.3 gmapping 的做法：把激光”对齐”进提议分布

目标分布（理想提议）其实长这样：

$$q(x_t) \propto \underbrace{p(z_t \mid x_t,\ m_{t-1}^{(i)})}{\text{激光有多吻合}} \times \underbrace{p(x_t \mid x{t-1}^{(i)},\ u_t)}_{\text{里程计觉得多合理}}$$

拆解读法：好的采样位置 = “激光看下来像真的” × “里程计也说得通” 的交集。但这个乘积分布仍然没法直接采样。

gmapping 的工程近似（三步）：

1
2
3
4
5
flowchart TD
    A["① 扫描匹配 Scan Matching<br/>在里程计位置附近微调 (x,y,θ),<br/>找到让激光和地图最吻合的位姿 x̂"] --> B["② 局部高斯近似<br/>用 x̂ 为中心的一个小高斯 N(x̂, Σ)<br/>代替那个算不出的乘积分布"]
    B --> C["③ 从这个高斯里撒粒子<br/>粒子全落在'里程计 ∩ 激光'的交集里"]
    style A fill:#e8f0fe,stroke:#1a73e8
    style C fill:#e6f4ea,stroke:#188038

扫描匹配是什么（直觉版）：拿着当前这帧激光的点云，在旧地图上”蹭一蹭、对一对”：

  • 在位姿 $(x, y, \theta)$ 三个自由度上做爬山式微调（往让吻合度变高的方向挪一点，反复迭代）；
  • 吻合度 = 激光点落到地图上”墙的位置”的程度（每个激光端点查地图，落在占据格附近就加分）；
  • 爬到山顶，得到对齐后的位姿 $x̂$。

生活类比：拼图。里程计告诉你”这块大概在这附近”（先验），你拿着拼图块在附近转一转、蹭一蹭找到严丝合缝的位置（扫描匹配），然后在这个位置附近（而不是里程计说的位置附近）撒粒子。

1.4 采完样后，权重怎么算

重要性权重的理论公式（把分子分母逐项摆出来，不需要背，看清逻辑即可）：

$$w_t^{(i)} = w_{t-1}^{(i)} \cdot \frac{p(z_t \mid x_t^{(i)}, m_{t-1}^{(i)}) \cdot p(x_t^{(i)} \mid x_{t-1}^{(i)}, u_t)}{q(x_t^{(i)})}$$

拆解读法：

  • 分子：这个粒子”激光吻合度 × 里程计合理性”（真实目标分布的分数）；
  • 分母：这个粒子”被采到的概率”（提议分布的分数）；
  • 直觉：如果提议分布已经把好位置都撒满了，那么好粒子被采到本身就是大概率事件，权重不用大修（分子分母接近，比值≈1）；只有”侥幸被采中的冷门好粒子”才需要权重翻盘。
  • gmapping 实现里这被简化为：用采样点处的扫描匹配得分均值来近似修正权重（源码细节见第 6 篇）。

1.5 兜底机制：匹配失败怎么办？

扫描匹配有个最低分数线（参数 minimumScore，默认 50）。分数不达标 = 这次对不上（比如刚进全新区域、地图还是空白），此时退回朴素提议分布（只用里程计采样）–稳健性优先。

这就是调参时最常见的日志 "Scan Matching Failed, falling back to odom" 的出处！看到它说明激光和地图对不上（转太快/新区域/雷达太差），第 7 篇讲怎么治。

二、改进二：自适应重采样–平时不动手，Neff 掉线才动手

2.1 朴素做法的病：每帧都重采样

第 3 篇讲过：重采样是”有损压缩”，每次都会丢一点多样性。朴素 RBPF 每帧都重采样 -> 正确但暂时低分的粒子被反复抽杀，几轮下来粒子群全是”近亲”，一旦集体走错，永无翻身之日（地图糊、重影的直接原因）。

2.2 gmapping 的做法：给粒子群体装体检仪

体检指标就是 3. 粒子滤波：一千个分身猜位置 推导过的有效粒子数：

$$N_{eff} = \frac{1}{\sum_{i=1}^{N}\left(w^{(i)}\right)^2}$$

规则一句话：$N_{eff}$ 高于阈值（默认 $0.5 \times N$）就不重采样，只更新权重；掉了线才重采样。

1
2
3
4
5
6
flowchart LR
    A["每帧算一次 Neff"] --> B{"Neff > N/2 ?"}
    B -->|"是: 权重均衡<br/>粒子群体健康"| C["✅ 只更新权重<br/>不动粒子 (保多样性)"]
    B -->|"否: 权重失衡<br/>快近亲繁殖了"| D["⚡ 重采样抽签<br/>淘汰低分粒子"]
    style C fill:#e6f4ea,stroke:#188038
    style D fill:#fce8e6,stroke:#d93025

2.3 两个改进的配合（为什么必须一起上）

只改进提议分布 只自适应重采样 两个一起（gmapping）
粒子撒得准 ✅ ❌ 还是乱撒 ✅
多样性保得住 ❌ 每帧重采样照样退化 ✅ ✅
效果 粒子准但死得快 活得久但撒不准 撒得准 + 活得久 -> 少量粒子长期稳定

改进提议分布还带来一个隐藏福利：粒子都撒在高分区域，权重彼此接近（都差不多好），Neff 自然保持高位，重采样频率进一步下降–两个改进互相成就。

三、完整流程：gmapping 处理一帧的总览

把两大改进装回 4. RBPF：把鸡蛋问题拆开 的循环，得到 gmapping 的完整一帧：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
flowchart TD
    Z["新激光帧 z_t + 里程计 u_t"] --> M["① drawFromMotion<br/>里程计运动模型: 先粗略采一步"]
    M --> S["② scanMatch 扫描匹配<br/>每个粒子在自己地图上爬山对齐<br/>(分数 < minimumScore 则跳过)"]
    S --> P["③ 从对齐位置附近的局部高斯<br/>撒新粒子 (改进提议分布)"]
    P --> W["④ 按匹配得分更新权重"]
    W --> N{"⑤ Neff > N/2 ?"}
    N -->|"是"| U["不重采样"]
    N -->|"否"| R["重采样"]
    U --> MAP["⑥ 每个粒子用新位姿+新激光<br/>更新自己的地图 (射线追踪+投票)"]
    R --> MAP
    MAP --> OUT["最佳粒子的地图 = 输出"]
    style S fill:#e8f0fe,stroke:#1a73e8
    style N fill:#fef7e0,stroke:#b06000
    style OUT fill:#e6f4ea,stroke:#188038

四、本篇小结

改进 治什么病 一句话原理
改进提议分布 粒子撒不准 采样前先扫描匹配”对齐”，把粒子撒在里程计∩激光的交集上；失败退回里程计
自适应重采样 粒子退化快 Neff 体检，高于 N/2 不动手，掉线才抽签
  • 权重 = 目标分布 / 提议分布的修正项；提议越准，修正越小；
  • 扫描匹配 = 拿新激光在旧地图上”蹭一蹭”找严丝合缝的位姿（爬山法）；
  • 两个改进互相成就：撒得准 -> 权重均衡 -> Neff 高 -> 少重采样。

下一篇：这些原理在代码里长什么样？6. 源码走读：一帧激光的旅程 – 从 ROS 回调一路追到 processScan()。

📚 参考：原理分析（提议分布 vs 目标分布，讲得极好）、ROS1系列（扫描匹配与重采样章节）、不可错过的gmapping解释