机器人 / 系统课时

贝叶斯滤波怎样融合预测与观测

贝叶斯滤波怎样融合预测与观测

机器人仅靠里程计积分会积累漂移,仅凭一次激光扫描又可能出现多个匹配位置。贝叶斯滤波先根据运动模型预测位置概率,再利用传感器观测修正预测结果,形成可供下一时刻继续递推的后验分布。本篇通过三格地图算例完成一次预测与更新。

学习目标

  • 区分上一时刻后验、当前预测先验、观测似然与当前后验。
  • 使用运动转移概率和观测似然,计算一次完整的贝叶斯滤波。
  • 用 Python 验证计算结果,并理解该过程如何用于二维机器人定位。

用概率分布描述机器人位置

假设机器人只能位于一条已知走廊的三个离散位置 A、B、C,每格宽 1 m。上一时刻的定位结果不是单个确定位置,而是一组概率:

位置 A B C
上一时刻后验 0.60 0.30 0.10

这表示机器人位于 A 的概率为 60%,位于 B 的概率为 30%,位于 C 的概率为 10%。三个位置互斥且覆盖全部候选状态,所以概率之和等于 1。

用 xtx_t 表示当前机器人状态,utu_t 表示运动输入,ztz_t 表示传感器观测。贝叶斯滤波中的信念分布 belt(xt)\mathrm{bel}_t(x_t),就是综合截至时刻 tt 的运动与观测信息后,对当前状态的概率估计。

一次递推包含两个阶段:预测使用上一时刻后验与运动模型,产生当前时刻的先验;更新使用当前观测的似然重新加权,形成当前时刻后验。这个后验将在下一时刻成为新的递推起点。

预测:让运动模型传播不确定性

现在机器人收到向右移动一格的控制输入。考虑打滑与运动误差,假设位于 A 或 B 时有 80% 的概率成功向右移动一格、20% 的概率留在原格;位于最右端 C 时受边界限制,仍留在 C。

运动模型可以写成状态转移概率表。每一行表示从某个旧状态出发,转移到各个新状态的概率。

原位置 到 A 到 B 到 C
A 0.20 0.80 0.00
B 0.00 0.20 0.80
C 0.00 0.00 1.00

每一行的概率之和必须等于 1。当前状态的预测概率,需要把所有可能的上一状态及对应转移概率加权求和:

bel‾t(xj)=∑ip(xj∣xi,ut) belt−1(xi)\overline{\mathrm{bel}}_t(x_j)=\sum_i p(x_j\mid x_i,u_t)\,\mathrm{bel}_{t-1}(x_i)

其中 p(xj∣xi,ut)p(x_j\mid x_i,u_t) 是从旧状态 xix_i 转移到新状态 xjx_j 的概率;横线表示当前分布尚未融合新观测。

将上一时刻概率代入:

  • A:0.60×0.20=0.120.60\times0.20=0.12。
  • B:0.60×0.80+0.30×0.20=0.540.60\times0.80+0.30\times0.20=0.54。
  • C:0.30×0.80+0.10×1.00=0.340.30\times0.80+0.10\times1.00=0.34。

因此预测先验为 [0.12,0.54,0.34][0.12,0.54,0.34]。概率最大的候选位置从 A 变为 B,但 C 仍保留 34% 的可能性。预测没有消除运动误差,而是按照运动模型将不确定性传播到了新的位置。

真实二维机器人中的运动模型还要考虑前进距离、旋转角度以及轮速里程计噪声。误差越大,预测分布通常越分散。

更新:利用观测似然修正先验

假设机器人获得一帧激光观测 ztz_t,其回波特征与 B 位置最吻合。根据已知地图及传感器模型,计算该观测在三个候选位置下出现的似然:

  • p(zt∣A)=0.10p(z_t\mid A)=0.10。
  • p(zt∣B)=0.80p(z_t\mid B)=0.80。
  • p(zt∣C)=0.20p(z_t\mid C)=0.20。

这些数值是为演算设定的观测模型结果,并非实际激光雷达直接输出的三个概率。它们不要求相加为 1,因为似然描述的是固定观测在不同候选状态下出现的可能性,而不是状态本身的概率分布。

根据贝叶斯更新公式:

belt(xj)=p(zt∣xj) bel‾t(xj)∑kp(zt∣xk) bel‾t(xk)\mathrm{bel}_t(x_j)=\frac{p(z_t\mid x_j)\,\overline{\mathrm{bel}}_t(x_j)}{\sum_k p(z_t\mid x_k)\,\overline{\mathrm{bel}}_t(x_k)}

分子是预测先验与观测似然的乘积,分母是所有候选状态权重之和,用于归一化。本例的归一化分母为 0.012+0.432+0.068=0.5120.012+0.432+0.068=0.512。

位置 预测先验 观测似然 未归一化权重 更新后验
A 0.12 0.10 0.012 0.0234
B 0.54 0.80 0.432 0.8438
C 0.34 0.20 0.068 0.1328

更新后,机器人位于 B 的概率从预测时的 54% 提高到约 84.38%。A 因为既不符合运动预测,也不符合激光观测,概率降至约 2.34%。后验仍是完整概率分布,而不只是位置 B 这个单点答案。

贝叶斯滤波中预测先验与观测更新的概率演化

这张图揭示了递归估计的关键关系:运动模型决定概率如何从旧位置传播到新位置,观测模型决定当前哪些候选位置更可信。两者都存在误差,所以最终结果仍保留不确定性。

用 Python 复算并验证结果

使用 Python 3.10 或更新版本即可执行下面的完整示例,无需安装第三方库。程序用转移矩阵计算预测分布,再按观测似然加权并归一化。

python
prior = [0.6, 0.3, 0.1]
transition = [
    [0.2, 0.8, 0.0],
    [0.0, 0.2, 0.8],
    [0.0, 0.0, 1.0],
]
likelihood = [0.1, 0.8, 0.2]

# transition[i][j] 表示从旧位置 i 到新位置 j 的概率
assert abs(sum(prior) - 1.0) < 1e-9
assert all(abs(sum(row) - 1.0) < 1e-9 for row in transition)
predicted = [
    sum(prior[i] * transition[i][j] for i in range(3))
    for j in range(3)
]

# 用固定观测下的似然加权,并统一归一化
weights = [predicted[j] * likelihood[j] for j in range(3)]
normalizer = sum(weights)
if normalizer <= 0:
    raise ValueError("观测与全部候选状态不相容")
posterior = [w / normalizer for w in weights]

print("预测:", [round(v, 2) for v in predicted])
print("后验:", [round(v, 4) for v in posterior])
print("概率和:", round(sum(posterior), 6))

预期输出:

text
预测: [0.12, 0.54, 0.34]
后验: [0.0234, 0.8438, 0.1328]
概率和: 1.0

验证时重点检查三个条件:上一时刻分布的概率和为 1;每个运动状态的转移概率行之和为 1;更新后的后验概率和为 1。如果归一化分母为 0,说明当前观测在所有候选状态下都被模型判为不可能,不能继续执行除法。

还可以把 B 的观测似然从 0.80 改为 0.10,重新运行并观察后验变化。这样可以直接验证:当观测不再支持 B 时,即使运动预测偏向 B,更新结果也会随之改变。

放到真实二维 SLAM 中理解

真实机器人在二维地图中的位姿通常表示为 (x,y,θ)(x,y,\theta),其中 θ\theta 是航向角。运动输入可以来自轮速里程计,观测来自激光雷达。对已知地图的定位滤波而言,预测阶段使用里程计和运动噪声模型传播位姿分布;更新阶段使用地图与激光回波计算观测似然,修正当前位置估计。

这里的激光似然实际还依赖地图 mm,即 p(zt∣xt,m)p(z_t\mid x_t,m)。同一束激光测得 2 m 距离,在不同地图或不同位姿下可能具有完全不同的解释。因此,不能把某个激光距离直接当作机器人位姿概率。

本例只有三个离散位置,能够显式枚举所有状态。若扩展到 NN 个离散状态,使用稠密转移矩阵直接预测通常需要 O(N2)O(N^2) 时间和 O(N2)O(N^2) 存储空间。面对连续二维位姿,可采用扩展卡尔曼滤波等高斯近似方法,或用粒子滤波表示多峰位置分布;它们对分布的表达能力和计算代价不同。

实际调试时,如果预测持续偏向错误方向,应先检查里程计的坐标轴、运动增量和噪声设定;如果激光更新反而让定位跳变,应检查观测时间戳、坐标变换、地图匹配及异常回波。贝叶斯公式能够正确融合输入概率,却无法自动修复错误的运动模型或观测模型。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

贝叶斯滤波怎样融合预测与观测

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通