4. RBPF：把”鸡生蛋”拆成两个小问题

上一篇：3. 粒子滤波：一千个分身猜位置｜下一篇：5. gmapping的两大改进

RBPF = Rao-Blackwellized Particle Filter（Rao-Blackwellized 粒子滤波），gmapping 的全称招牌。听起来唬人，其实思想在第 0 篇的链式法则里已经全部讲完了。这一篇把那行公式逐符号拆开，并补上”每个粒子怎么建图”。

一、那个 SLAM 公式，逐符号拆解

gmapping 求解的目标（论文里的第一个公式）：

$$P(x_{1:t}, m \mid z_{1:t}, u_{1:t-1})$$

先看懂每个符号（都是纸老虎）：

符号	人话
$x_{1:t}$	轨迹：从第 1 时刻到现在的全部位姿 $x_1, x_2, …, x_t$（每个位姿 = $x, y, \theta$）
$m$	地图（栅格地图）
$z_{1:t}$	观测：到目前所有激光帧（z = 激光 LaserScan）
$u_{1:t-1}$	控制量/运动：里程计读数（u = 里程计 odometry）
整体	“已知看过的所有激光、走过的所有里程计，同时猜测轨迹和地图”

记忆口诀：x 走过的路，m 手里的图，z 眼睛看的，u 轮子记的。

二、拆解：链式法则出手

2.1 一行变换

$$P(x_{1:t}, m \mid z_{1:t}, u_{1:t-1}) = \underbrace{P(x_{1:t} \mid z_{1:t}, u_{1:t-1})}{\text{① 定位}} \cdot \underbrace{P(m \mid x{1:t}, z_{1:t})}_{\text{② 建图}}$$

这就是第 0 篇的 $P(A,B) = P(A) \cdot P(B|A)$，一行完成”鸡生蛋”拆解：

flowchart TD
    Q["❓ 鸡生蛋: 同时猜 轨迹+地图"] -->|"链式法则<br/>P(A,B)=P(B|A)·P(A)"| S1["① 先猜轨迹<br/>P(轨迹|激光,里程计)<br/>'我走过一条怎样的路?'"]
    S1 --> S2["② 已知轨迹, 再猜地图<br/>P(地图|轨迹,激光)<br/>'沿着这条路, 看到的墙拼成什么图?'"]
    S2 --> OK["✅ 两个问题都能各自解决"]
    style Q fill:#fce8e6,stroke:#d93025
    style OK fill:#e6f4ea,stroke:#188038

2.2 为什么拆开就”能解”了？

问题	拆开前	拆开后
① 定位	轨迹和地图都不知道，无从下手	粒子滤波搞定（第 3 篇）：粒子=候选轨迹，权重=激光吻合度
② 建图	位姿未知，墙不知道画哪	位姿已知时建图变简单：每帧激光”照着画”就行（下面 2.3）

2.3 为什么”已知轨迹后建图”是简单问题？

假如你知道自己每一帧的准确位置，建图退化为逐格子的独立计数问题：

某格子被激光穿过 k 次、命中 j 次 -> 用贝叶斯算它被占用的概率（第 2 篇讲过）；
每个格子独立计算，不需要优化、不需要搜索，纯统计；
所以 ② 根本不需要粒子滤波，一个粒子配一份地图，老老实实计数即可。

RBPF 的分工：难的部分（轨迹不确定性）交给粒子滤波；简单的部分（已知轨迹的建图）解析计算。这就是 “Rao-Blackwellized” 的含义：对能解析算的部分解析算，只对必须采样的部分采样 – 数学上可证明这样方差更小（同样精度需要的粒子更少）。

三、”Rao-Blackwellized” 到底什么意思（进阶框）

⭐ 选读，不影响主线。

Rao-Blackwellization 是统计学技巧：$E[X] = E[E[X|Y]]$ – 先对 Y 条件化精确算一部分，再对 Y 采样；
用在 SLAM：$P(\text{轨迹},\text{地图}) = P(\text{地图}|\text{轨迹}) \cdot P(\text{轨迹})$，地图部分条件化后可解析（每格子独立贝叶斯），只需对轨迹采样；
收益定理（Rao-Blackwell 定理）：这样分解后的估计量方差 ≤ 直接采样的方差 -> 同精度下粒子数更少，这正是 gmapping 能用 30~100 个粒子跑室内 SLAM 的理论底气（朴素做法需要成千上万）。

四、RBPF-SLAM 的完整循环（朴素版）

把第 3 篇 SIR 循环套上地图，得到朴素 RBPF-SLAM 伪代码：

for 每一帧新数据 z_t, u_t:            # 激光帧 + 里程计
    for 每个粒子 i:
        # ① 预测: 从"里程计运动模型"采样新位姿
        x_t[i] = 采样( P(x_t | x_{t-1}[i], u_t) )   # 走一步+加噪声
        # ② 加权: 激光和粒子自带地图的吻合度
        w[i]  = w[i] × P(z_t | x_t[i], m[i])
        # ③ 建图: 用新位姿+新激光更新该粒子的地图
        m[i]  = 更新栅格(m[i], x_t[i], z_t)
    # ④ 重采样(每帧都做!) + 复制时连地图一起复制
    if 重采样:
        (x, m, w) = 按权重抽签重组

但朴素版有两个坑（正是 gmapping 论文的靶子）：

坑	现象	后果
提议分布太烂	① 只用里程计采样：里程计说”在 A”，实际在 A 偏 30cm 处，采样点全撒错地方，激光再准也白搭	需要巨量粒子才能覆盖真实位置
每帧重采样	④ 无脑重采样 -> 第 3 篇说的粒子退化加速	正确粒子被过早淘汰，地图越走越歪

五、gmapping = RBPF + 两个补丁

flowchart LR
    A["朴素 RBPF-SLAM"] -->|"补丁1: 改进提议分布<br/>(采样前先用激光'对齐'一下)"| B["gmapping"]
    A -->|"补丁2: 自适应重采样<br/>(Neff低了才重采样)"| B
    style A fill:#f1f3f4,stroke:#5f6368
    style B fill:#e6f4ea,stroke:#188038

两大补丁的细节（含公式逐项拆解）是下一篇的全部内容，也是 gmapping 论文的两大贡献。

六、本篇小结

目标公式 $P(x_{1:t}, m \mid z_{1:t}, u_{1:t-1})$：已知激光+里程计，猜轨迹+地图；
链式法则拆两半：粒子滤波管轨迹（难），逐格子贝叶斯管地图（易）；
Rao-Blackwellization 的本质：能解析的解析，必须采样的才采样，方差更小、粒子更省；
朴素版两大坑（烂提议分布、频繁重采样）-> gmapping 两大改进 -> 下一篇。

📚 参考：源代码解析（RBPF 分解式）、原理分析（提议分布 vs 目标分布，讲得极好）、ROS1系列（RBPF 章节）