机器人 / 系统课时
EKF SLAM 如何联合估计地标和位姿
EKF SLAM 如何联合估计地标和位姿
机器人依靠里程计移动时,位姿估计会逐渐漂移;重新观测固定地标时,机器人的位姿和地图又会互相约束。本篇用二维机器人与一个地标构造 EKF SLAM 的联合状态,执行一次可复现的量测更新,并解释交叉协方差、线性化误差和计算规模。
学习目标
- 写出机器人位姿与地标组成的联合状态,读懂协方差中的交叉项。
- 区分运动预测和地标观测更新,验证一次更新如何同时修改位姿与地图。
- 判断角度残差、数据关联和状态规模带来的风险。
联合状态与相关性
二维机器人位姿是 ,每个点地标是 。若地图中有 个点地标,EKF SLAM 的状态均值为:
状态长度为 。相应的协方差矩阵不仅记录各个量自己的不确定性,还记录它们的联合误差:
其中 为 位姿协方差, 为 地标协方差, 则记录机器人与各地标的交叉协方差。例如机器人向右偏估时,同一机器人观测初始化的地标也可能向右偏估;两者不能被视为独立。

运动预测如何传播不确定性
收到轮式里程计或其他运动输入 后,预测函数 只改变机器人位姿;静态地标的均值保持原样:
协方差必须随运动模型的雅可比 一同传播:
的机器人部分是 对位姿的雅可比,地图部分是单位阵; 仅在机器人位姿对应的块中加入运动噪声。例如一次预测可采用平移标准差 、航向标准差 ,对应的噪声方差分别为 和 。这些是演示取值,实际应通过里程计误差标定。
地标坐标虽没有移动,机器人与地标的交叉协方差仍会随 改变。只更新机器人的 对角块、忽略其他块,会破坏联合误差的描述。
观测更新为何能同时纠偏
设观测是机器人到已匹配地标的距离和相对方位角。定义 、,预测量测为:
在当前预测均值处对 求雅可比 。其机器人位姿列与当前地标列都非零,因此一次测量不应只改其中一方。设测量噪声协方差为 ,计算创新、创新协方差和卡尔曼增益:
把 的方位角分量归一化到 ,再进行联合更新:
这里采用数值上更稳健的 Joseph 协方差更新形式。即使本次只看到一个地标,先前相关的其他地标也可能通过协方差交叉项受到修正。
用一个地标复现更新
下面用 Python 3 和 NumPy 演示二维机器人与单个已入图地标的量测更新。状态顺序固定为 ;预测均值是 ,观测值为距离 、方位 。距离标准差取 ,方位标准差取 。
先安装 NumPy(命令 python -m pip install numpy),再把下列代码保存为 ekf_demo.py 并用 python ekf_demo.py 运行。为突出一次观测如何产生交叉项,本例人为采用对角先验;真实系统从不确定的机器人位姿初始化新地标时,应同时建立相应的交叉协方差。
import math
import numpy as np
# 一个已入图地标;mu、P 为运动预测后的先验
mu = np.array([0., 0., 0., 2., 1.])
P = np.diag([0.20, 0.20, 0.04, 0.50, 0.50])
R = np.diag([0.20**2, 0.05**2])
z = np.array([2.40, 0.53])
dx, dy = mu[3] - mu[0], mu[4] - mu[1]
q = dx**2 + dy**2
r = math.sqrt(q)
z_hat = np.array([r, math.atan2(dy, dx) - mu[2]])
# 对距离与方位观测在先验均值处求雅可比
H = np.array([
[-dx/r, -dy/r, 0., dx/r, dy/r],
[dy/q, -dx/q, -1., -dy/q, dx/q]
])
nu = z - z_hat
nu[1] = (nu[1] + math.pi) % (2 * math.pi) - math.pi
S = H @ P @ H.T + R
K = np.linalg.solve(S, H @ P).T
mu = mu + K @ nu
mu[2] = (mu[2] + math.pi) % (2 * math.pi) - math.pi
# Joseph 形式降低协方差数值失稳风险
I = np.eye(5)
P = (I - K @ H) @ P @ (I - K @ H).T + K @ R @ K.T
print("预测量测:", np.round(z_hat, 4))
print("更新位姿:", np.round(mu[:3], 4))
print("更新地标:", np.round(mu[3:], 4))
print("交叉协方差 P[0,3]:", round(P[0, 3], 4))预期输出:
预测量测: [2.2361 0.4636]
更新位姿: [-0.0251 -0.0489 -0.0145]
更新地标: [2.0627 1.1223]
交叉协方差 P[0,3]: 0.13位姿和地标都发生了修正;原本为零的 变为约 ,说明两者在该观测之后相关。这里只演示更新步骤,不包含实际里程计预测、地标初始化和数据关联。
线性化与地图规模的边界
EKF 在单一均值附近用一阶雅可比近似非线性模型。如果初始朝向误差很大,或长时间没有可靠观测,线性化点可能偏离真实状态,滤波器会表现得过度自信。可用以下检查避免把错误观测直接并入地图:
- 检查关联:对候选地标计算马氏距离 。二维距离—方位量测在高斯假设下可用 作为约 的卡方门限;超出时先拒绝该候选关联,而非强行更新。
- 检查几何与角度:方位角残差必须环绕归一化;当 接近零时,距离—方位雅可比会病态,应跳过该观测或更换量测模型。
- 检查规模: 个二维地标使 占用 内存。 时, 的
float64协方差约占 ; 时约占 ,还不含中间矩阵。利用量测的低维结构,单次联合更新仍通常需要 运算。
当地图地标数持续增长、误匹配频繁或线性化一致性难以维持时,可以改用基于关键帧和稀疏因子图的 SLAM 优化方法;若地图规模较小、地标可稳定关联,EKF SLAM 则提供了直接维护联合位姿—地图不确定性的方案。