Description：Probabilistic Robotics 第 4 章读书笔记 — 重要性采样、SIS 粒子退化、SIR 重采样修复、重采样策略、MCL 定位、Rao-Blackwellized PF (FastSLAM)
My Notion Note ID：K2E-B-B1-4
Created：2026-06-06
Updated：2026-06-06
License：转载欢迎 — 请署名 Yu Zhang 并链回 yuzhang.io 原文

1. 非参数方法的动机

高斯假设在以下情况失效：

多峰分布：全局定位（初始位姿完全未知）、数据关联歧义
非线性程度高：单峰高斯传播后严重失形

非参数滤波用有限粒子集近似任意分布：

$\text{bel}(x_t) \approx \{x_t^{[m]}, w_t^{[m]}\}_{m=1}^M$

$M$ 个粒子 $x_t^{[m]}$ 带权重 $w_t^{[m]}$ ， $\sum_m w_t^{[m]} = 1$ 。 $M$ 越大近似越精确，计算量越大。

2. 重要性采样

从提议分布 $\pi(x)$ 采样，用权重 $w \propto p(x)/\pi(x)$ 修正偏差，使加权样本集等效逼近目标分布 $p(x)$ 。

关键： $\pi(x)$ 的支撑必须包含 $p(x)$ 的支撑（否则高概率区域无样本）。

3. SIS — Sequential Importance Sampling

递推权重更新（不重采样）：

$w_t^{[m]} \propto \frac{p(z_t \mid x_t^{[m]}) \, p(x_t^{[m]} \mid x_{t-1}^{[m]}, u_t)}{\pi(x_t^{[m]} \mid x_{t-1}^{[m]}, u_t)}$

粒子退化 (particle degeneracy)：多步之后大多数粒子权重趋近于零，只有极少数粒子有效。根本原因：提议分布与后验分布不匹配。

有效样本数量度： $N_{eff} = \frac{1}{\sum_m (w_t^{[m]})^2}$ ， $N_{eff} \ll M$ 时退化严重。

4. SIR — Sequential Importance Resampling

修复退化：在每步（或 $N_{eff}$ 低于阈值时）加入重采样：

按权重 $\{w_t^{[m]}\}$ 从当前粒子集中有放回地采样 $M$ 个新粒子
重置所有粒子权重为 $1/M$

常用提议分布： $\pi(x_t^{[m]} \mid x_{t-1}^{[m]}, u_t) = p(x_t \mid x_{t-1}^{[m]}, u_t)$ （先验分布）→ 权重简化为： $w_t^{[m]} \propto p(z_t \mid x_t^{[m]})$ （只与观测似然有关）。

粒子贫化 (particle impoverishment)：重采样后粒子多样性下降（低权重粒子被丢弃，高权重粒子被多次复制）。解法：加入随机噪声、用更好的提议分布（如测量增强采样）。

5. 重采样策略

策略	描述	特点
多项式重采样	按多项式分布采样 $M$ 次	简单，方差最大
系统重采样	用均匀间隔的随机起点遍历累积和	方差小，实现简单，SLAM 常用
残差重采样	先确定整数部分，再随机采样小数部分	方差较小
分层重采样	把 $[0,1]$ 分成 $M$ 层，每层采一个	方差小，与系统重采样类似

触发条件：每步重采样（计算量大）or $N_{eff} < M/2$ 时触发（自适应）。

6. MCL — Monte Carlo Localization

SIR 粒子滤波用于已知地图条件下的机器人定位。

初始分布：全局定位时用均匀分布初始化粒子；已知初始位姿时用小范围高斯
运动更新：从 $p(x_t \mid x_{t-1}^{[m]}, u_t)$ 采样新粒子位姿
权重更新： $w_t^{[m]} = p(z_t \mid x_t^{[m]}, \text{map})$ （激光/相机观测似然）
重采样：系统重采样，低权重粒子死亡，高权重粒子繁殖

优势：自然处理全局定位（多峰）；可处理绑架问题（添加随机粒子比例）；精度随 $M$ 增大。

7. Rao-Blackwellized PF (FastSLAM)

将 SLAM 联合分布分解：

$p(\mathbf{x}_{0:t}, \mathbf{m} \mid z_{1:t}, u_{1:t}) = p(\mathbf{m} \mid \mathbf{x}_{0:t}, z_{1:t}) \cdot p(\mathbf{x}_{0:t} \mid z_{1:t}, u_{1:t})$

用粒子代表机器人轨迹 $\mathbf{x}_{0:t}$ （路径不确定性）
给定路径，每个路标独立 → 每个粒子维护 $N$ 个独立路标 EKF

FastSLAM 每粒子有 $N$ 个一维 EKF（路标坐标），计算复杂度 $O(M \log N)$ （vs 标准 EKF-SLAM 的 $O(N^2)$ ）。

局限：仍存在粒子贫化（轨迹维数随时间增长）；路标 EKF 仍是线性化。

References

Thrun, S., Burgard, W., & Fox, D. Probabilistic Robotics. MIT Press, 2005. 第 4 章 — 本笔记内容来源

非参数滤波 — 粒子滤波与 MCL

Table of Contents