机器人 / 系统课时
贝叶斯滤波怎样融合预测与观测
贝叶斯滤波怎样融合预测与观测
机器人仅靠里程计积分会积累漂移,仅凭一次激光扫描又可能出现多个匹配位置。贝叶斯滤波先根据运动模型预测位置概率,再利用传感器观测修正预测结果,形成可供下一时刻继续递推的后验分布。本篇通过三格地图算例完成一次预测与更新。
学习目标
- 区分上一时刻后验、当前预测先验、观测似然与当前后验。
- 使用运动转移概率和观测似然,计算一次完整的贝叶斯滤波。
- 用 Python 验证计算结果,并理解该过程如何用于二维机器人定位。
用概率分布描述机器人位置
假设机器人只能位于一条已知走廊的三个离散位置 A、B、C,每格宽 1 m。上一时刻的定位结果不是单个确定位置,而是一组概率:
| 位置 | A | B | C |
|---|---|---|---|
| 上一时刻后验 | 0.60 | 0.30 | 0.10 |
这表示机器人位于 A 的概率为 60%,位于 B 的概率为 30%,位于 C 的概率为 10%。三个位置互斥且覆盖全部候选状态,所以概率之和等于 1。
用 表示当前机器人状态, 表示运动输入, 表示传感器观测。贝叶斯滤波中的信念分布 ,就是综合截至时刻 的运动与观测信息后,对当前状态的概率估计。
一次递推包含两个阶段:预测使用上一时刻后验与运动模型,产生当前时刻的先验;更新使用当前观测的似然重新加权,形成当前时刻后验。这个后验将在下一时刻成为新的递推起点。
预测:让运动模型传播不确定性
现在机器人收到向右移动一格的控制输入。考虑打滑与运动误差,假设位于 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。当前状态的预测概率,需要把所有可能的上一状态及对应转移概率加权求和:
其中 是从旧状态 转移到新状态 的概率;横线表示当前分布尚未融合新观测。
将上一时刻概率代入:
- A:。
- B:。
- C:。
因此预测先验为 。概率最大的候选位置从 A 变为 B,但 C 仍保留 34% 的可能性。预测没有消除运动误差,而是按照运动模型将不确定性传播到了新的位置。
真实二维机器人中的运动模型还要考虑前进距离、旋转角度以及轮速里程计噪声。误差越大,预测分布通常越分散。
更新:利用观测似然修正先验
假设机器人获得一帧激光观测 ,其回波特征与 B 位置最吻合。根据已知地图及传感器模型,计算该观测在三个候选位置下出现的似然:
- 。
- 。
- 。
这些数值是为演算设定的观测模型结果,并非实际激光雷达直接输出的三个概率。它们不要求相加为 1,因为似然描述的是固定观测在不同候选状态下出现的可能性,而不是状态本身的概率分布。
根据贝叶斯更新公式:
分子是预测先验与观测似然的乘积,分母是所有候选状态权重之和,用于归一化。本例的归一化分母为 。
| 位置 | 预测先验 | 观测似然 | 未归一化权重 | 更新后验 |
|---|---|---|---|---|
| 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 或更新版本即可执行下面的完整示例,无需安装第三方库。程序用转移矩阵计算预测分布,再按观测似然加权并归一化。
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))预期输出:
预测: [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 中理解
真实机器人在二维地图中的位姿通常表示为 ,其中 是航向角。运动输入可以来自轮速里程计,观测来自激光雷达。对已知地图的定位滤波而言,预测阶段使用里程计和运动噪声模型传播位姿分布;更新阶段使用地图与激光回波计算观测似然,修正当前位置估计。
这里的激光似然实际还依赖地图 ,即 。同一束激光测得 2 m 距离,在不同地图或不同位姿下可能具有完全不同的解释。因此,不能把某个激光距离直接当作机器人位姿概率。
本例只有三个离散位置,能够显式枚举所有状态。若扩展到 个离散状态,使用稠密转移矩阵直接预测通常需要 时间和 存储空间。面对连续二维位姿,可采用扩展卡尔曼滤波等高斯近似方法,或用粒子滤波表示多峰位置分布;它们对分布的表达能力和计算代价不同。
实际调试时,如果预测持续偏向错误方向,应先检查里程计的坐标轴、运动增量和噪声设定;如果激光更新反而让定位跳变,应检查观测时间戳、坐标变换、地图匹配及异常回波。贝叶斯公式能够正确融合输入概率,却无法自动修复错误的运动模型或观测模型。