机器人 / 系统课时

EKF SLAM 如何联合估计地标和位姿

EKF SLAM 如何联合估计地标和位姿

机器人依靠里程计移动时,位姿估计会逐渐漂移;重新观测固定地标时,机器人的位姿和地图又会互相约束。本篇用二维机器人与一个地标构造 EKF SLAM 的联合状态,执行一次可复现的量测更新,并解释交叉协方差、线性化误差和计算规模。

学习目标

  • 写出机器人位姿与地标组成的联合状态,读懂协方差中的交叉项。
  • 区分运动预测和地标观测更新,验证一次更新如何同时修改位姿与地图。
  • 判断角度残差、数据关联和状态规模带来的风险。

联合状态与相关性

二维机器人位姿是 (xr,yr,θ)(x_r,y_r,\theta),每个点地标是 (lix,liy)(l_{ix},l_{iy})。若地图中有 NN 个点地标,EKF SLAM 的状态均值为:

μ=[xr,yr,θ,l1x,l1y,…,lNx,lNy]T\boldsymbol{\mu}=[x_r,y_r,\theta,l_{1x},l_{1y},\ldots,l_{Nx},l_{Ny}]^T

状态长度为 3+2N3+2N。相应的协方差矩阵不仅记录各个量自己的不确定性,还记录它们的联合误差:

P=[PrrPrmPmrPmm],Pmr=PrmTP=\begin{bmatrix}P_{rr}&P_{rm}\\P_{mr}&P_{mm}\end{bmatrix},\quad P_{mr}=P_{rm}^T

其中 PrrP_{rr} 为 3×33\times3 位姿协方差,PmmP_{mm} 为 2N×2N2N\times2N 地标协方差,PrmP_{rm} 则记录机器人与各地标的交叉协方差。例如机器人向右偏估时,同一机器人观测初始化的地标也可能向右偏估;两者不能被视为独立。

EKF SLAM 的机器人位姿、地标量测与交叉协方差关系

运动预测如何传播不确定性

收到轮式里程计或其他运动输入 uu 后,预测函数 ff 只改变机器人位姿;静态地标的均值保持原样:

μr−=f(μr,u),μm−=μm\boldsymbol{\mu}_r^-=f(\boldsymbol{\mu}_r,u),\quad \boldsymbol{\mu}_m^-=\boldsymbol{\mu}_m

协方差必须随运动模型的雅可比 GG 一同传播:

P−=GPGT+QaugP^-=GPG^T+Q_{\mathrm{aug}}

GG 的机器人部分是 ff 对位姿的雅可比,地图部分是单位阵;QaugQ_{\mathrm{aug}} 仅在机器人位姿对应的块中加入运动噪声。例如一次预测可采用平移标准差 0.10 m0.10\,\mathrm{m}、航向标准差 0.02 rad0.02\,\mathrm{rad},对应的噪声方差分别为 0.01 m20.01\,\mathrm{m}^2 和 0.0004 rad20.0004\,\mathrm{rad}^2。这些是演示取值,实际应通过里程计误差标定。

地标坐标虽没有移动,机器人与地标的交叉协方差仍会随 GG 改变。只更新机器人的 3×33\times3 对角块、忽略其他块,会破坏联合误差的描述。

观测更新为何能同时纠偏

设观测是机器人到已匹配地标的距离和相对方位角。定义 Δx=lx−xr\Delta x=l_x-x_r、Δy=ly−yr\Delta y=l_y-y_r,预测量测为:

h(μ−)=[Δx2+Δy2atan2⁡(Δy,Δx)−θ]h(\boldsymbol{\mu}^-)= \begin{bmatrix} \sqrt{\Delta x^2+\Delta y^2}\\ \operatorname{atan2}(\Delta y,\Delta x)-\theta \end{bmatrix}

在当前预测均值处对 hh 求雅可比 HH。其机器人位姿列与当前地标列都非零,因此一次测量不应只改其中一方。设测量噪声协方差为 RR,计算创新、创新协方差和卡尔曼增益:

ν=z−h(μ−),S=HP−HT+R,K=P−HTS−1\nu=z-h(\boldsymbol{\mu}^-),\quad S=HP^-H^T+R,\quad K=P^-H^TS^{-1}

把 ν\nu 的方位角分量归一化到 [−π,π)[-\pi,\pi),再进行联合更新:

μ+=μ−+Kν\boldsymbol{\mu}^+=\boldsymbol{\mu}^-+K\nu
P+=(I−KH)P−(I−KH)T+KRKTP^+=(I-KH)P^-(I-KH)^T+KRK^T

这里采用数值上更稳健的 Joseph 协方差更新形式。即使本次只看到一个地标,先前相关的其他地标也可能通过协方差交叉项受到修正。

用一个地标复现更新

下面用 Python 3 和 NumPy 演示二维机器人与单个已入图地标的量测更新。状态顺序固定为 [xr,yr,θ,lx,ly][x_r,y_r,\theta,l_x,l_y];预测均值是 [0,0,0,2,1][0,0,0,2,1],观测值为距离 2.40 m2.40\,\mathrm{m}、方位 0.53 rad0.53\,\mathrm{rad}。距离标准差取 0.20 m0.20\,\mathrm{m},方位标准差取 0.05 rad0.05\,\mathrm{rad}。

先安装 NumPy(命令 python -m pip install numpy),再把下列代码保存为 ekf_demo.py 并用 python ekf_demo.py 运行。为突出一次观测如何产生交叉项,本例人为采用对角先验;真实系统从不确定的机器人位姿初始化新地标时,应同时建立相应的交叉协方差。

python
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))

预期输出:

text
预测量测: [2.2361 0.4636]
更新位姿: [-0.0251 -0.0489 -0.0145]
更新地标: [2.0627 1.1223]
交叉协方差 P[0,3]: 0.13

位姿和地标都发生了修正;原本为零的 Pxr,lxP_{x_r,l_x} 变为约 0.1300 m20.1300\,\mathrm{m}^2,说明两者在该观测之后相关。这里只演示更新步骤,不包含实际里程计预测、地标初始化和数据关联。

线性化与地图规模的边界

EKF 在单一均值附近用一阶雅可比近似非线性模型。如果初始朝向误差很大,或长时间没有可靠观测,线性化点可能偏离真实状态,滤波器会表现得过度自信。可用以下检查避免把错误观测直接并入地图:

  1. 检查关联:对候选地标计算马氏距离 d2=νTS−1νd^2=\nu^TS^{-1}\nu。二维距离—方位量测在高斯假设下可用 5.995.99 作为约 95%95\% 的卡方门限;超出时先拒绝该候选关联,而非强行更新。
  2. 检查几何与角度:方位角残差必须环绕归一化;当 q=Δx2+Δy2q=\Delta x^2+\Delta y^2 接近零时,距离—方位雅可比会病态,应跳过该观测或更换量测模型。
  3. 检查规模:NN 个二维地标使 PP 占用 O(N2)O(N^2) 内存。N=1000N=1000 时,2003×20032003\times2003 的 float64 协方差约占 32 MB32\,\mathrm{MB};N=5000N=5000 时约占 800 MB800\,\mathrm{MB},还不含中间矩阵。利用量测的低维结构,单次联合更新仍通常需要 O(N2)O(N^2) 运算。

当地图地标数持续增长、误匹配频繁或线性化一致性难以维持时,可以改用基于关键帧和稀疏因子图的 SLAM 优化方法;若地图规模较小、地标可稳定关联,EKF SLAM 则提供了直接维护联合位姿—地图不确定性的方案。

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

目录与阅读设置

阅读设置

17

记录一个想法

EKF SLAM 如何联合估计地标和位姿

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

登录哪都通