3. 粒子滤波：用一千个”分身”猜位置

上一篇：2. 机器人眼中的世界：传感器与栅格地图｜下一篇：4. RBPF：把鸡蛋问题拆开

gmapping 的”滤波”两个字，指的就是粒子滤波。这一篇不出现任何 SLAM 公式，只讲清楚一件事：为什么撒一把随机点，就能算出一个概率分布。

一、核心思想：算不出来的分布，就”撒豆子”采样出来

回到 0. 基本数学：机器人对”我在哪”的答案是一个概率分布。理想的分布长这样（真实位置附近可能性高）：

$$P(x) = \text{某个极其复杂、写不出解析式的函数}$$

问题：这个函数在计算机里根本没法直接表示和计算（里程计误差+激光匹配，非高斯、非线性、多维）。

粒子滤波的暴力解法：管它什么函数，撒 N 个随机样本点（粒子），用这堆点的密度来近似分布：

flowchart LR
    subgraph hard["无法计算的真实分布"]
        A["🗺️ 复杂概率云<br/>(真值附近高, 其他地方低)"]
    end
    subgraph pf["粒子滤波近似"]
        B["· ·•····<br/>••••🤖••••<br/>····•··<br/>(点越密=可能性越大)"]
    end
    hard ==>|"撒 N 个粒子<br/>密度 ≈ 分布"| pf
    style A fill:#fce8e6,stroke:#d93025
    style B fill:#e6f4ea,stroke:#188038

类比：想知道湖里鱼群分布，不用解流体力学方程，撒 1000 个浮标看哪里浮标晃动多，哪里鱼就多。

一个粒子 = 一个”可能的我”：它包含一个假设的位姿 $(x, y, \theta)$ 和一个可信度（权重 $w$）。

二、SIR 三步循环：撒 -> 打分 -> 淘汰

粒子滤波每来一次新数据，跑一轮循环：

flowchart TD
    P["① 预测 Prediction<br/>每个粒子按运动模型'走一步'<br/>(加上随机噪声, 分身各自演化)"] --> W["② 加权 Weighting<br/>用新观测给每个分身打分<br/>'如果你在这, 看到当前激光的可能性多大?'"]
    W --> R["③ 重采样 Resampling<br/>按权重'抽签'繁衍后代<br/>高分粒子多复制, 低分粒子淘汰"]
    R -->|"下一帧数据到来"| P
    style P fill:#e8f0fe,stroke:#1a73e8
    style W fill:#fef7e0,stroke:#b06000
    style R fill:#e6f4ea,stroke:#188038

用一个具体例子走一遍（机器人蒙眼在走廊里）：

① 预测：分身各自”走一步”

里程计说”我向前走了 1 米”。但里程计有噪声，于是每个粒子按”1 米 + 随机扰动”各自走：

分身 A 走了 0.98m、分身 B 走了 1.05m、分身 C 走了 1.01m 还歪了 2°……
一千个分身 = 一千个稍微不同的未来 -> 不确定度被”演”出来了；
这一步的数学：从提议分布（proposal distribution）中采样，gmapping 里就是里程计运动模型加噪声。

② 加权：用激光给分身打分

关键问题：”如果我真的站在分身 A 的位置，能看到现在这帧激光吗？“

分身 A 站的位置，按它携带的地图推测：”我正前方 2 米应该是墙”；
实际激光说：正前方 2.05 米是墙 – 很符合！权重 ↑；
分身 B 站的位置推测”前方应该是一片空地”，激光却看到墙 – 矛盾！权重 ↓。

这一步的数学就是 0. 基本数学里的似然：$P(\text{激光观测} \mid \text{粒子位姿})$。权重 = 分身与观测的吻合程度。

③ 重采样：优胜劣汰的”抽签”

按权重比例抽签产生下一代（权重 0.5 的粒子平均被抽中 5 次，权重 0.01 的基本出局）：

像不像自然选择？与环境证据吻合的猜想存活繁衍，矛盾的被淘汰；
几轮之后，粒子云自动收缩聚集到真实位置附近 – 不确定度被打掉了。

⚠️ 注意：重采样是”有损压缩”，引出下面的大问题。

三、粒子退化：粒子滤波的阿喀琉斯之踵

3.1 什么叫退化

重采样久了会出事：

正确但暂时低分的粒子被淘汰：环境相似（两条一样的走廊）或测量噪声，可能让”对的分身”这轮得分反而低，被抽签淘汰；
粒子多样性骤减：所有粒子都是某个高分粒子的复制品（”近亲繁殖”），万一它其实是错的，全军覆没，再无翻身机会。

3.2 退化的度量：有效粒子数 Neff

怎么知道”粒子群体还健康吗”？看权重分布是否均衡：

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

拆解这个公式（不背，理解）：

情形	权重分布	$\sum (w^{(i)})^2$	$N_{eff}$	状态
N 个粒子权重全相等（$w=1/N$）	均匀	$N \cdot \frac{1}{N^2} = \frac{1}{N}$	$N$（满分）	✅ 多样性满格
一个粒子权重≈1，其余≈0	极端集中	≈ 1	≈ 1	❌ 名存实亡

一句话：Neff = “表面有 N 个粒子，实际相当于几个独立粒子”。 Neff 越小，说明粒子越”近亲”，越需要警惕。

gmapping 的对策（第 5 篇）：Neff 低于阈值才重采样，平时不动 – 这就是”选择性/自适应重采样”。

四、朴素粒子滤波做 SLAM 的两大烧钱点

把上面循环直接套到 SLAM 上（每个粒子除了位姿还带一张地图）：

烧钱点	说明
钱袋子（内存）	每个粒子带一张地图！粒子越多地图越大越吃内存（第 9 篇算账：一粒子 16M × 100 粒子 = 1.6G）
准确性 vs 粒子数	大场景/强噪声需要更多粒子才不丢解，但粒子多 = 计算和内存线性涨

gmapping 论文的全部贡献，就是回答”怎么用更少的粒子达到同样的精度”：

让每个粒子”生得更准”（改进提议分布，第 5 篇）；
让粒子群体”死得更慢”（自适应重采样，第 5 篇）。

五、本篇小结

粒子滤波 = 撒点近似分布：粒子是”可能的我”，权重是可信度；
SIR 循环：预测（撒）-> 加权（打分）-> 重采样（淘汰），不确定度逐轮收缩；
退化是命门，Neff 是体检指标；
SLAM 场景下每粒子带地图 -> 粒子数就是钱 -> gmapping 的目标是省粒子。

📚 参考：源代码解析（SIR 流程）、原理分析（粒子退化成因）、ROS1系列（粒子滤波基础）