Part I · 第 4 章 概率 · 4.7

贝叶斯滤波

机器人看不到自己的真实状态，只能一边动、一边看。贝叶斯滤波 (Bayes filter) 用一个概率分布表示"我可能在哪"，然后反复做两件事：预测（动作把分布挪开、抹宽）和更新（传感器读数乘上去、再归一化）。卡尔曼滤波、粒子滤波，都只是这套递推在不同"信念表示"下的具体实现。

约 25 分钟 前置：4.2 条件概率与贝叶斯 · 4.5 不确定性传播 · 4.6 最大似然与最大后验 3 个交互

    1为什么机器人需要它

    一台机器人被放进一条很长的走廊，走廊里有几扇门，它手里有一张写着门位置的地图，但不知道自己在哪一格。它能做的只有两件事：向前或向后走（走得不一定准：轮子打滑，可能少走或多走一格），以及看一眼（传感器说"我面前是扇门"，但这个传感器也会看错）。

    怎么办？如果只相信一次读数，一次错误就毁掉一切。如果只相信里程计，误差会无限累积。更好的办法是：别急着下结论，把每个位置的可信程度都记下来，每走一步、每看一眼就把这份"可信程度表"更新一遍。这张表就是信念 (belief)，更新它的规则就是贝叶斯滤波。

    它是整个状态估计的母体。4.8 节的卡尔曼滤波是它在"高斯、线性"假设下的精确闭式解，4.10 节的粒子滤波是它用一堆随机样本做的近似，SLAM、视觉里程计、机器人定位里的"滤波"类方法，都建立在这同一条递推上。

    2直觉：一排柱子，先挪动、再相乘

    把走廊切成 20 格，给每格一根柱子，柱子的高度就是"我在这一格的概率"。开始时一无所知：20 根柱子一样高（均匀分布）。

    • 预测 (predict)：机器人下令"向右走一格"。每根柱子里的概率大部分挪到右边一格，一小部分留在原地（少走），一小部分跑到右边两格（多走）。整个柱状图向右平移，同时被抹平：不确定性变大了。
    • 更新 (update)：传感器说"看到一扇门"。对每个格子问：如果我在这里，看到门的可能性有多大？在门前这个数大（比如 0.8），在墙前这个数小（0.2）。把每根柱子乘以自己的数，再让所有柱子的总和重新等于 1（归一化）。门前的柱子长高，墙前的柱子变矮：不确定性变小了。

    下面是这套规则的实验台。注意走廊里有几处"长得一模一样"的局部（比如"门·墙·门"出现了两次）：一次读数分不清我在哪一处，信念于是出现多个峰。只有多走几步、多看几眼，才能排除。

    FIG 1走廊里的直方图滤波器
    上方是走廊：蓝色方块是门，圆点机器人是真实位置（滤波器不知道，可以拖动它来"绑架"机器人）。下方柱子是信念：青色 = 刚预测完，黄色 = 刚更新完，灰色轮廓 = 上一步的信念。紫色小条是上一次读数给每格的似然 p(z | x)。按"左移 / 右移"做预测，按"感知"做更新。
    试一试
    • 歧义：重置后点一次"感知"：信念出现好几个一样高、相距 5 格以上的峰（读到"门"时最明显：5 扇门各一个）。一次读数分不清是哪一处。
    • 定位成功：通过移动和感知，让真实位置那一格的概率超过 0.9。需要几步？
    • 绑架机器人：定位成功后把机器人拖到 5 格以外，然后连续"感知"。滤波器多久才醒悟？（提示：它可能永远不会。）
    核心洞见

    动作让你更糊涂，观测让你更清楚。预测把分布抹开（在开阔处熵增加；例外是边界：撞到尽头的墙会把概率堆在一起，见 FIG 3），更新把它压尖（平均而言熵减少）。滤波器的工作就是在这两股力量之间保持诚实：既不假装知道得比实际多，也不把已经知道的东西忘掉。注意"归一化"不是装饰：没有它，相乘之后柱子的总和就不再是 1，也就不再是概率。

    3动手玩：同一条递推，三种信念表示

    走廊是离散的，所以我们可以用 20 根柱子精确地做。如果位置是连续的（比如 0 到 12 米的一维轨道）呢？同一条递推可以用三种方式实现：

    • 细网格（直方图）：把轨道切成 240 个小格，当作很多根很细的柱子。这是"几乎精确"的参照答案。
    • 一个高斯：只记均值和方差。当运动和测量都是线性高斯时，这就是卡尔曼滤波 (4.8)，并且和网格的结果完全一致。
    • 一堆粒子：撒 400 个随机样本，预测时每个样本自己走一步（带噪声），更新时按似然重新抽样 (4.10)。

    用 GPS 式的传感器（读数 = 位置 + 高斯噪声）时，三者几乎重合。把传感器换成"门探测器"（只在门附近报"门"），似然是多峰的，高斯就无能为力了。

    FIG 2三种信念表示：网格、高斯、粒子
    黄色填充 = 细网格上的精确信念；青色曲线 = 高斯（GPS 模式下是真正的卡尔曼滤波，门模式下是"均值与方差相同的最佳单个高斯"）；紫色折线与刻度 = 400 个粒子的直方图；灰色虚线 = 真实位置。每次"右移 / 左移"走 1.5 m（噪声 0.35 m），"感知"做一次测量。
    试一试
    • GPS 模式下交替移动与感知至少 3 次：看右边读数，卡尔曼高斯与网格的总变差距离应小于 0.02。卡尔曼滤波就是贝叶斯滤波。
    • 切到门探测器，点「感知」直到读到「门」（读到「墙」就重置再试）：精确信念有三个峰，最佳单个高斯却把质量堆在两扇门之间的空地上（两者的总变差距离大于 0.3）。

    4再看数学

    提示：悬停下面公式中带底色的符号，FIG 1 中对应的部分会高亮（预测后的信念 \(\mathrm{bel}^-\)、更新后的信念 \(\mathrm{bel}\)、似然条、运动模型的转移概率）；FIG 2 中精确信念对应 \(\mathrm{bel}\)。

    信念与两条假设

    记 \(x_k\) 为 \(k\) 时刻的状态，\(u_k\) 为控制，\(z_k\) 为测量。信念是"已知到目前为止所有控制和测量时，状态的分布"，更新前的那一版（只用到 \(k-1\) 时刻的测量）记作 \(\mathrm{bel}^-\)：

    $$ \mathrm{bel}(x_k) = p(x_k \mid z_{1:k},\,u_{1:k}),\qquad \mathrm{bel}^-(x_k) = p(x_k \mid z_{1:k-1},\,u_{1:k}). $$

    递推能成立，靠的是马尔可夫假设：状态是"完备"的，即只要知道当前状态，过去就不再提供额外信息。

    $$ p(x_k \mid x_{0:k-1},\,z_{1:k-1},\,u_{1:k}) = \hl{motion}{p(x_k \mid x_{k-1},\,u_k)},\qquad p(z_k \mid x_{0:k},\,z_{1:k-1},\,u_{1:k}) = \hl{sensor}{p(z_k \mid x_k)}. $$

    两个因子分别叫运动模型和传感器模型 (观测模型)。它们是你在做滤波之前要从物理和标定中得到的全部输入。

    递推：预测与更新

    预测用全概率公式：下一个状态可以从任何一个旧状态来，按"旧状态有多可信 × 转移概率"加权求和。更新用贝叶斯公式（4.2 节）：后验 ∝ 似然 × 先验。

    $$ \hl{prior}{\mathrm{bel}^-(x_k)} = \int \hl{motion}{p(x_k \mid x_{k-1},\,u_k)}\;\hl{post}{\mathrm{bel}(x_{k-1})}\;dx_{k-1} \qquad\text{(predict)} $$ $$ \hl{post}{\mathrm{bel}(x_k)} = \eta\;\hl{sensor}{p(z_k \mid x_k)}\;\hl{prior}{\mathrm{bel}^-(x_k)}, \qquad \eta = \frac{1}{p(z_k\mid z_{1:k-1},u_{1:k})} \qquad\text{(update)} $$

    \(\eta\) 只是让总和为 1 的归一化常数，它还有一个含义：这个读数在滤波器看来有多"意外"（\(\eta\) 越大，读数越出乎意料）。在连续状态下积分，在离散状态下就是求和。

    离散情形就是矩阵乘法

    把信念写成列向量 \(\mathbf b\)（\(N\) 个格子）。转移矩阵 \(T\) 的第 \((j,i)\) 项是"从格 \(i\) 走到格 \(j\) 的概率"，每一列的和为 1。设 \(\boldsymbol\ell\) 的第 \(j\) 项是 \(p(z_k\mid x_k = j)\)。于是

    $$ \mathbf b^- = T\,\mathbf b,\qquad \mathbf b^{\text{new}} = \frac{\operatorname{diag}(\boldsymbol\ell)\,\mathbf b^-}{\mathbf 1^\top \operatorname{diag}(\boldsymbol\ell)\,\mathbf b^-}. $$

    预测是一次矩阵乘（1.4 节：把"分布"当向量，让转移矩阵作用在上面），更新是逐元素相乘再归一化。FIG 1 里右移一格的 \(T\) 近似为：对角线上方一条 0.8 的带，两侧各有一条 0.1 的带，边界处被墙截断后把超出去的概率累加到最后一格。

    算一遍：三个格子

    先验 \(\mathbf b^-=(0.5,\,0.3,\,0.2)\)，读数的似然 \(\boldsymbol\ell=(0.9,\,0.1,\,0.5)\)。逐项相乘得 \((0.45,\,0.03,\,0.10)\)，总和 \(0.58\)，所以 \(\eta = 1/0.58\)，后验 \(=(0.776,\,0.052,\,0.172)\)。第 1 格从 0.5 涨到 0.78，第 2 格从 0.3 跌到 0.05：读数"投票"支持了第 1 格。

    一个骨架，三个肌肉

    信念表示预测更新代价与限制
    直方图 / 网格 (本节)卷积或矩阵乘 \(T\mathbf b\)逐格相乘，归一化任意形状的信念；格子数随维度指数增长
    高斯 (4.8 卡尔曼 / EKF)\(F P F^\top + Q\)\(K = P^-H^\top S^{-1}\)极快、极省；只能表示单峰，需要线性（或线性化）
    粒子 (4.10)每个粒子按运动模型采样按似然加权，重采样任意形状；精度随粒子数增长，维度高时需要大量粒子

    5机器人中的它：下一步该往哪走？

    有了信念，机器人就能做一件聪明的事：挑一个能最大程度消除歧义的动作，而不是漫无目的地走。这叫主动定位 (active localization)。

    怎么评估一个动作好不好？对每个候选动作"走 k 格再看一眼"，机器人可以在脑中推演：先用预测步算出走完后的信念，再对两种可能的读数（门 / 墙）分别算出更新后的熵，按各自出现的概率加权，得到期望的后验熵。熵下降得越多（信息增益，4.11 节会详细讲熵），这个动作越值得做。

    FIG 3 里机器人刚在歧义的走廊里看到一扇门。柱子代表每个动作的信息增益（单位：比特）；点一根柱子选择动作，按"执行"真正走一步并感知。

    FIG 3主动定位：按期望信息增益选动作
    上半部分：走廊、当前信念（黄）与真实位置（点）。下半部分：每个"移动 k 格"（k 为负表示向左）的期望信息增益 \(H(\mathrm{bel}) - \mathbb E_z[H(\mathrm{bel}\mid z)]\)，单位比特；绿色是最优动作，紫色边框是你选中的动作。传感器正确率 0.9，运动准确率 0.9。注意朝走廊尽头的墙开过去往往信息量最大：墙会把机器人拦住，位置反而变得确定。
    试一试
    • 只用"执行最优动作"，让真实位置的概率超过 0.9。一般要几步？再重置，改成总选"原地不动 (k = 0)"，比较一下。
    真实系统里

    室内的 AMCL（自适应蒙特卡洛定位，ROS 导航栈里的标准定位器）本质上就是一个跑在二维地图上的贝叶斯滤波：状态是 \((x,y,\theta)\)，运动模型来自里程计，观测模型来自激光对地图的匹配。主动定位和"信息增益驱动的探索"是它的延伸：下一个观测点选在最能消除歧义（或最能揭开未知区域）的地方。仓库里的 AGV 靠门、货架这类重复结构，常常遇到本节演示的"感知混淆 (perceptual aliasing)"。

    6常见误区

    误区 1：把概率写成 0

    贝叶斯更新是乘法：0 乘什么都是 0，而且永远救不回来。如果某格的信念被压成 0，哪怕机器人其实就在那里，后面再多的读数也无法让它复活。FIG 1 里"绑架"机器人就是这个现象：旧信念几乎把真实位置排除了。工程上的办法是给每格留一点底噪（比如运动模型里加一点点均匀的概率，传感器似然不低于 0.01），也就是"永远不要说永远不"。

    误区 2：过早取最大值

    信念有两个同样高的峰时，"取概率最大的格子"只是随机挑一个，一半概率是错的。只在信念已经集中（比如最大值超过 0.9，或熵很低）时才把它当成"位置估计"，否则要把整个分布交给下游（规划器可以选择先观察再行动）。

    误区 3：状态不完备，却假装马尔可夫

    递推依赖"知道当前状态就够了"。如果漏掉了一个会持续影响读数的量（比如陀螺仪的零偏、地图里移动的行人），连续的读数就不再相互独立，滤波器会把同一份信息数了好几次，越来越过度自信。解决办法是把这个量也放进状态里（如 4.8 节的"状态扩维"）。

    误区 4：忘了维度灾难

    20 格的走廊没问题。可是平面位姿 \((x,y,\theta)\)，每个方向 100 格就是 \(10^6\) 格以上，每次预测还要对每格做卷积；三维位姿加速度就更不用说了。网格滤波只适合低维问题，维度高了就要换成高斯（4.8）或粒子（4.10）。

    误区 5：数值下溢

    似然往往是很小的数，连乘几百次就会下溢成 0（然后触发误区 1）。实践里要么每次更新都归一化，要么在对数域里做加法，归一化时用 log-sum-exp 技巧。

    7小练习

    1. 贝叶斯滤波的两步对信念的熵（不确定性）分别有什么影响？
    运动带噪声，预测相当于把分布与噪声核卷积，在开阔处总是更宽（边界例外：墙会把越界的概率堆在最后一格，FIG 3 正是利用这一点）；观测带来信息，更新平均而言让分布更尖。"平均"二字不能省：遇到一个极出乎意料的读数，单次更新也可能让熵上升。
    2. 先验 \((0.5,\,0.3,\,0.2)\)，似然 \((0.9,\,0.1,\,0.5)\)，归一化后的后验是？
    逐项相乘得 (0.45, 0.03, 0.10)，这还不是概率（总和 0.58）。除以 0.58 才是后验：(0.776, 0.052, 0.172)。第一个选项忘了归一化。
    3. 机器人的信念在真实位置那一格是 0（它被"绑架"到了那里）。继续做贝叶斯滤波，会怎样？
    更新是乘法，0 × 任何数 = 0，归一化也救不回来。预测步骤只会把邻居的一点概率抹过来，如果邻居也接近 0，就几乎没有。所以真实系统要给信念设置下限，或者主动注入随机假设（4.10 节的做法）。
    4. 为什么对 \((x,y,\theta)\) 三维位姿，通常不用 FIG 1 那样的网格滤波，而用卡尔曼或粒子滤波？
    网格滤波对任意形状的信念、任意噪声都成立，而且能表示多峰，缺点只有一个：维度灾难。每个方向 100 格，三维就是 \(10^6\) 格。

    8延伸阅读

    • S. Thrun, W. Burgard & D. Fox, Probabilistic Robotics, MIT Press, 2005, Ch. 2 & 4 — 贝叶斯滤波的推导与离散（直方图）滤波；第 7 章讲马尔可夫定位
    • T. D. Barfoot, State Estimation for Robotics, Cambridge University Press, 2017, Ch. 4 — 非线性、非高斯估计：贝叶斯滤波作为统一框架
    • D. Fox, W. Burgard & S. Thrun, "Markov Localization for Mobile Robots in Dynamic Environments", Journal of Artificial Intelligence Research, 1999 — 本节走廊例子的原型：机器人定位上的网格贝叶斯滤波
    • S. Russell & P. Norvig, Artificial Intelligence: A Modern Approach, the chapter on probabilistic reasoning over time — 滤波、预测、平滑的统一视角
    • R. Labbe, Kalman and Bayesian Filters in Python — 免费交互式教材，第 2 章用"走廊里的狗"讲离散贝叶斯滤波