Part I · 第 4 章 概率 · 4.10

粒子滤波

卡尔曼滤波只会用一个高斯表示"我在哪"，可真实的信念常常有好几个峰。粒子滤波 (particle filter) 干脆放弃用公式描述信念：撒一群带权重的样本，每个样本是一个"可能的机器人"。预测时让每个样本自己走一步，加权时按传感器读数给每个样本打分，重采样时让高分者繁殖、低分者淘汰。用它做的机器人定位叫蒙特卡洛定位 (MCL)。

约 30 分钟 前置：4.7 贝叶斯滤波 · 4.9 采样方法 · 4.8 卡尔曼滤波（对比） 3 个交互

    1为什么机器人需要它

    卡尔曼滤波 (4.8) 又快又准，可它有两个硬伤：信念只能是单峰的高斯，模型最好是线性的。现实不总这样：

    • 走廊里有几扇一模一样的门，机器人看到一扇门，它可能在任何一扇门前：信念天然是多峰的。
    • 机器人被人突然抱到别的房间（绑架问题，kidnapped robot）：它必须放弃原来的自信，在全图上重新搜索。
    • 激光对地图的匹配、里程计的转弯，都是强非线性的，线性化 (EKF) 会失真。

    4.7 节的网格（直方图）滤波能处理这些，但网格数随维度指数增长：三维位姿 \((x,y,\theta)\) 在一张大地图上要几百万个格子。粒子滤波用样本代替网格：只在"概率大的地方"放样本，所以算力集中在真正重要的区域，实现简单，还能直接使用任何你能仿真的运动模型和传感器模型。这也是 ROS 里最常用的室内定位器 AMCL 的核心。

    2直觉：预测、加权、重采样

    回到 4.7 节那条有五扇门的走廊，这次位置是连续的。我们用 \(N\) 个粒子表示信念，每个粒子是走廊上的一个位置 \(x^{[i]}\)，带一个权重 \(w^{[i]}\)。每一轮三步：

    1. 预测：下令"向右走 1 m"，每个粒子独立地走 \(1\) m 再加一点随机噪声（就是从运动模型中采样，4.9 节）。粒子群整体平移、散开。
    2. 加权：传感器说"我看到一扇门"。门里的粒子得到高分（似然 \(a\)），墙边的粒子得到低分（\(1-a\)）。这一步粒子不动，只改权重（FIG 1 里圆点变大、竖线变长）。
    3. 重采样：权重是"繁殖概率"。把带权重的粒子集合替换成 \(N\) 个等权重的新粒子：高权重的被复制多份，低权重的多半消失。这样粒子就重新集中到可能性大的地方。

    重采样的做法就是 4.9 节的逆 CDF 采样：把权重首尾相接铺成一条 [0,1] 的线段（图中紫色长条），按等间距撒 \(N\) 根指针（系统重采样），每根指针落在谁的线段上，就复制谁。

    FIG 1走廊里的粒子滤波：三步循环
    顶部是走廊（蓝块 = 门，可拖动的大圆点 = 真实机器人）。中间一行是粒子：青 = 预测后，紫 = 加权后（点越大、竖线越长，权重越高），黄 = 重采样后（等权重）。紫色长条是累积权重，竖直的刻度是重采样的指针，细线连到被选中的粒子。底部柱状图是"由粒子估计的每格概率"，可以和 4.7 节的直方图滤波对比。
    试一试
    • 多峰：先把机器人拖到一扇门（蓝块）上，"重置"，再"加权"（应读到"门"）、"重采样"：粒子集中到几扇门前，出现至少两个相距 5 m 以上、各占 18% 以上质量的簇（簇 = 一格加左右相邻两格）。如果读到的是"墙"，重置后再试一次。
    • 定位成功：交替"移动 → 加权 → 重采样"，让真实位置 ±1 m 内的粒子占 90% 以上。
    • 粒子太少：把粒子数调到 12 个以下，切到"多项式重采样"，不移动，反复"加权 → 重采样"：看看会不会"一个粒子也没落在真实位置附近"，滤波器还一脸自信（一格里聚了 60% 以上的粒子）。
    核心洞见

    粒子滤波就是 4.7 节贝叶斯滤波的"蒙特卡洛版"：粒子的疏密就是概率密度。预测 = 让每个粒子按运动模型采样；更新 = 按似然给粒子加权；重采样 = 把权重"兑现"成粒子的疏密。它不需要线性、不需要高斯，代价是随机性：样本不够时，估计会抖，甚至把真值整个漏掉。

    3动手玩：重采样本身也会"伤"粒子

    重采样有一个副作用：每次都是有放回抽样，高权重的粒子被复制，别的粒子消失，粒子群里不同的"祖先"越来越少。即使没有任何新信息（所有权重相等），只要你每一步都重采样，多样性也会像"遗传漂变"那样一代代流失，最后所有粒子都是同一个祖先的拷贝。这叫粒子贫化 / 粒子耗尽 (sample impoverishment / particle depletion)。注意它和"权重退化"（少数粒子占去全部权重）不是一回事：权重退化靠重采样来治，粒子贫化却是重采样造成的。

    FIG 2 把每一代粒子画成一行，从上往下是时间。起初粒子均匀铺在 \([0,10]\) 上，没有运动、没有观测，只做重采样，看看有多少个不同的祖先能活下来。

    FIG 2祖先的消失：多样性随代数下降
    每一行是一代粒子（纵轴向下是时间），每个点是一个粒子，画在它最初祖先的位置上，相邻的祖先颜色不同，所以一列同色的点就是同一个祖先的后代。右侧曲线是"不同祖先数 / N"。"总是重采样"每代都做多项式重采样；"仅当 N_eff < N/2"只在权重严重不均时才重采样。"弱观测"模式每代加一次很弱的观测（一个宽高斯），权重略不均匀。
    试一试
    • 取 N = 50，"总是重采样"、无观测，跑 60 代：看看还剩几个不同的祖先（通常不到 10 个）。
    • 切换到"仅当 N_eff < N/2"，同样设置再跑 60 代：多样性保住了吗？为什么？
    一个反直觉的事实

    无信息时，"总是重采样"让粒子群像一个无记忆的随机游走：期望 \(O(N)\) 代之内，所有粒子就会合并成同一祖先。这意味着：重采样是必须付出的代价（它让计算资源别浪费在低权重粒子上），但是不该每一步都做。常用的规则是只在有效粒子数 \(N_{\mathrm{eff}}=1/\sum_i (w^{[i]})^2\) 低于 \(N/2\) 时才重采样；系统重采样（等间距指针）方差比多项式重采样小，而且权重全相等时一个粒子都不会丢。

    4再看数学

    提示：悬停下面带底色的符号，FIG 1 与 FIG 3 中对应的元素会高亮。

    算法

    信念用 \(N\) 个带权样本表示：\(\mathrm{bel}(x_k)\approx\sum_{i=1}^N w_k^{[i]}\,\delta\big(x_k - x_k^{[i]}\big)\)。每个时刻：

    $$ \text{1. predict:}\quad \hl{part}{x_k^{[i]}} \sim p\big(x_k \mid x_{k-1}^{[i]},\,u_k\big) $$ $$ \text{2. weight:}\quad \hl{wt}{w_k^{[i]}} \;\propto\; w_{k-1}^{[i]}\;p\big(z_k \mid x_k^{[i]}\big),\qquad \sum_i w_k^{[i]}=1 $$ $$ \text{3. resample (if } N_{\mathrm{eff}}=1\Big/\textstyle\sum_i (w_k^{[i]})^2 < \tfrac N2 \text{):}\quad \text{draw } N \text{ indices } \propto w_k^{[i]};\;\; w_k^{[i]}\leftarrow 1/N $$

    这个版本叫自举粒子滤波 (bootstrap filter)，是最简单的一种。

    为什么权重是 \(p(z\mid x)\)？重要性采样的视角

    我们想从 \(\mathrm{bel}(x_k)\propto p(z_k\mid x_k)\int p(x_k\mid x_{k-1},u_k)\,\mathrm{bel}(x_{k-1})\,dx_{k-1}\) 里抽样，它直接抽不出来。但"预测"步骤恰好提供了一个容易抽的提议分布：先按旧信念抽 \(x_{k-1}\)，再按运动模型抽 \(x_k\)，得到的分布就是 \(\mathrm{bel}^-(x_k)\)。按 4.9 节的重要性采样，权重 = 目标 ÷ 提议：

    $$ w \;=\; \frac{\text{target}}{\text{proposal}} \;=\; \frac{p(z_k\mid x_k)\,\mathrm{bel}^-(x_k)}{\mathrm{bel}^-(x_k)} \;=\; \hl{wt}{p(z_k\mid x_k)}. $$

    于是"加权"的就是传感器模型。重采样则是把一组带权样本转成等权样本，防止权重退化（少数粒子占去全部权重）。当 \(N\to\infty\)，估计收敛到真实后验，误差按 \(1/\sqrt N\) 下降。

    系统重采样

    取一个随机数 \(u_0\sim\mathrm{U}(0,1/N)\)，令指针 \(u_j=u_0+j/N\)（\(j=0,\dots,N-1\)），选中满足 \(\sum_{i'\lt i}w^{[i']}\lt u_j\le\sum_{i'\le i}w^{[i']}\) 的那个粒子 \(i\)。只用一个随机数，复杂度 \(O(N)\)，粒子 \(i\) 被复制的次数是 \(\lfloor Nw^{[i]}\rfloor\) 或 \(\lceil Nw^{[i]}\rceil\)，比 \(N\) 次独立抽样（多项式重采样，次数服从二项分布）的方差小得多。

    从粒子读出估计

    位置的估计是加权平均 \(\hat{\mathbf p}=\sum_i w^{[i]}\mathbf p^{[i]}\)。角度不能直接平均：358° 和 2° 的平均应是 0°，不是 180°。要用圆周平均 \(\hat\theta=\operatorname{atan2}\big(\sum_i w^{[i]}\sin\theta^{[i]},\,\sum_i w^{[i]}\cos\theta^{[i]}\big)\)。更要警惕：多峰分布的均值可能落在没有任何粒子的地方（FIG 3 里的"镜像"就是例子），这时应对粒子聚类，取最大的簇，或者干脆把整个粒子集交给下游。

    5机器人中的它：对称房间里的蒙特卡洛定位

    一台带 8 束测距的机器人在 10 m × 6 m 的房间里。房间几乎中心对称：两个方块、两段隔墙关于房间中心互为镜像，只有右下角那个小方块是"独一份"。机器人知道地图，但一开始不知道自己在哪、朝哪：全局定位，粒子均匀铺满整张地图的位置和朝向。

    点"前进 / 左转 / 右转"，或打开"自动巡航"，看粒子怎样聚拢。大多数时候你会看到两团：一团在真实位置，一团在镜像位置，直到机器人看见那个独一份的小方块才能排除。接着拖动机器人把它"绑架"到别处，看粒子滤波能不能醒悟；再打开"随机注入"救它。

    FIG 3蒙特卡洛定位 (MCL)：全局定位、镜像歧义与被绑架
    黄色小点是粒子（带朝向刻度），带朝向短线的大圆点是真实机器人（可拖动，等于绑架），紫线是它的 8 束测距，青色是加权平均得到的估计位姿，红线是估计与真值的误差。灰色是已知地图，右下角的小方块打破了对称。"随机注入"每次重采样时把 5% 的粒子换成随机位姿。
    试一试
    • 镜像歧义：重置后驱动机器人，等到粒子分成两团：真实位姿和它的点对称镜像各占 25% 以上。这时加权平均落在哪里？
    • 全局定位成功：让粒子的位置散布 < 0.5 m，估计误差 < 0.4 m。（提示：绕到右下角，让机器人看到那个独一份的小方块。）
    • 绑架并救援：定位成功后把机器人拖到 3 m 以外，打开"随机注入"，让滤波器重新收敛（误差 < 0.5 m）。
    • 粒子不够：把粒子数降到 40 以下并重置，驱动 8 步以上：滤波器常常自信地收敛到错误的位置（误差 > 1.5 m 而散布 < 0.4 m）。
    真实系统里

    ROS 导航栈里的 AMCL 就是这样的滤波器：状态 \((x,y,\theta)\)，运动模型来自里程计，观测模型来自激光对占据栅格地图的匹配（似然场模型）。它用 KLD 采样自适应地决定粒子数（粒子占据的状态空间格子越多，就用越多粒子），并在短期平均似然相对长期平均似然突然下降时注入随机粒子（这就是 FIG 3 的"随机注入"，对应增广 MCL）。仓库 AGV 常常遇到"整排货架长得一样"的歧义，靠多峰信念和多走几步来消除。

    6常见误区

    误区 1：粒子数太少

    全局定位时，粒子要足够密才能"碰巧"有一个落在真值附近。粒子太少时，真值附近一个粒子也没有，别的粒子在错误的地方越聚越紧，滤波器自信地走向错误（FIG 3 的最后一个任务）。需要的粒子数随状态维度和地图大小快速增长；跟踪（已知大致位置）时几十到几百个就够，全局定位可能要几千个。

    误区 2：传感器太"准"，权重一下塌了

    如果似然很尖（传感器噪声设得很小，或者束数很多），几乎所有粒子的权重都接近 0，只有一两个例外，\(N_{\mathrm{eff}}\) 塌到 1 到 2，一次更新就丢掉绝大多数粒子。工程办法是：故意把传感器噪声设得比实际大（相当于对似然做"退火"），只取一部分束，或给似然加一个均匀的底噪混合（对付野值）。

    误区 3：绑架后没有注入随机粒子

    跟 4.7 节的"零概率"同一个病：粒子群已经收缩到旧位置，新位置周围没有任何粒子，重采样不会凭空造出来。要么定期混入一小撮全图随机粒子，要么监测平均似然、在它突然下降时扩散粒子。代价是注入过多会让稳定的估计变得抖动，要调。

    误区 4：用加权平均当"答案"，且角度直接平均

    多峰时的平均值可能是一个没有任何粒子、甚至在墙里的点；角度用算术平均会在 ±180° 附近翻车。读估计时，先看分布（聚类或直方图），再决定怎么报告。

    误区 5：每一步都重采样

    FIG 2 已经演示：重采样会损失多样性。用 \(N_{\mathrm{eff}}\) 阈值触发，并且用系统重采样代替多项式重采样。

    7小练习

    1. 四个粒子的归一化权重为 \((0.5,\,0.5,\,0,\,0)\)。有效粒子数 \(N_{\mathrm{eff}}\) 是多少？
    \(N_{\mathrm{eff}}=1/\sum w^2=1/(0.25+0.25)=2\)：只有两个粒子在起作用，另外两个权重为 0，等于白占位置。
    2. 用 \(N=10\) 个粒子做多项式重采样，某个粒子的归一化权重是 0.4。它的期望被复制几份？
    每次抽样选中它的概率是 0.4，抽 10 次，期望 \(10\times0.4=4\) 份（多项式重采样下实际份数服从二项分布，可能是 2 或 6）。系统重采样会把它稳定地复制 4 份。
    3. 机器人被"绑架"后，一个已经收敛的粒子滤波为什么恢复不过来？
    粒子滤波的"搜索空间"只有现有的粒子。新位置周围没有粒子时，没有任何一个能得到高权重，权重也只能在旧位置附近的错误粒子间分配。对策是注入随机粒子，或在平均似然骤降时重新全局撒粒子。
    4. 一半粒子（权重各相等）在 \(x=2\)，另一半在 \(x=8\)。粒子的加权平均是 5。这个 5 的含义是？
    均值只在单峰时代表"位置"。双峰时它落在空地上，概率密度为 0。要么聚类后分别报告，要么用最大簇的均值，要么把整个粒子集交给下游。

    8延伸阅读

    • S. Thrun, W. Burgard & D. Fox, Probabilistic Robotics, MIT Press, 2005, Ch. 4.3 & Ch. 8.3 — 粒子滤波与蒙特卡洛定位（含 KLD 采样与随机注入）
    • F. Dellaert, D. Fox, W. Burgard & S. Thrun, "Monte Carlo Localization for Mobile Robots", ICRA, 1999 — MCL 的原始论文
    • N. J. Gordon, D. J. Salmond & A. F. M. Smith, "Novel approach to nonlinear/non-Gaussian Bayesian state estimation", IEE Proceedings F, 1993 — 自举粒子滤波
    • T. D. Barfoot, State Estimation for Robotics, Cambridge University Press, 2017, Ch. 4 — 非线性非高斯估计，包括粒子滤波
    • R. Douc & O. Cappé, "Comparison of resampling schemes for particle filtering", ISPA, 2005 — 多项式、分层与系统重采样的比较