机器人 / 系统课时

让里程计估计包含不确定性

让里程计估计包含不确定性

轮式里程计把左右轮运动积分为机器人位姿,但编码器量化、轮胎打滑和轮径偏差会让估计逐渐偏离真实位置。本课用差速底盘建立位姿协方差模型,运行一个十步直行实验,并学会把不确定性正确表达给 ROS 2 的后续定位模块。

学习目标

  • 区分里程计位姿估计、真实位姿与协方差各自代表的含义。
  • 从左右轮距离噪声推导前进和转向的不确定性,并递推位姿协方差。
  • 用 Python 验证误差随路程扩大,理解 ROS 2 Odometry 中的协方差字段。

位姿估计不等于真实位置

在平面中,用 p^=[x,y,θ]T\hat{\mathbf{p}}=[x,y,\theta]^T 表示估计位姿,θ\theta 是朝向角(弧度)。例如里程计报出 (1.00 m,0.00 m,0)(1.00\ \mathrm{m},0.00\ \mathrm{m},0),只表示根据轮子数据推算走到了这里;实际位置可能因为滑动落在别处,需要外部定位或测量才能验证。

用 3×33\times3 协方差矩阵 PP 描述 x,y,θx,y,\theta 的不确定性。对角线 Pxx,Pyy,PθθP_{xx},P_{yy},P_{\theta\theta} 是方差,平方根是各分量的标准差;非对角线表示位置与朝向误差的相关性。它不是实时测得的“真实误差”,更不是绝对误差上限。

从左右轮噪声算出运动噪声

设轮距 b=0.40 mb=0.40\ \mathrm{m},采样周期 Δt=0.10 s\Delta t=0.10\ \mathrm{s},左右轮各前进 0.10 m0.10\ \mathrm{m}。差速模型给出:

Δs=ΔsL+ΔsR2,Δθ=ΔsR−ΔsLb\Delta s=\frac{\Delta s_L+\Delta s_R}{2},\qquad \Delta\theta=\frac{\Delta s_R-\Delta s_L}{b}

因此中心前进 Δs=0.10 m\Delta s=0.10\ \mathrm{m},朝向变化为零。假设每次两轮距离测量各有独立、零均值、标准差 0.005 m0.005\ \mathrm{m} 的随机误差(仅为演示参数),则前进增量的标准差约为 0.00354 m0.00354\ \mathrm{m},转向增量的标准差约为 0.01768 rad0.01768\ \mathrm{rad},即 1.01∘1.01^\circ。

两个轮子都前进时,左右轮差值的噪声仍会引入随机转角。即便平均运动轨迹笔直,朝向的不确定性也会在后续行驶中变成横向位置的不确定性。

每走一步都传播协方差

采用中点航向 ϕ=θk+Δθ/2\phi=\theta_k+\Delta\theta/2,用 Δscos⁡ϕ\Delta s\cos\phi 和 Δssin⁡ϕ\Delta s\sin\phi 更新 x,yx,y,用 Δθ\Delta\theta 更新朝向。先用雅可比 JJ 把左右轮噪声映射到 u=[Δs,Δθ]Tu=[\Delta s,\Delta\theta]^T,再递推位姿协方差:

J=[1/21/2−1/b1/b],Q=J diag(σL2,σR2) JTJ=\begin{bmatrix}1/2&1/2\\-1/b&1/b\end{bmatrix},\qquad Q=J\,\mathrm{diag}(\sigma_L^2,\sigma_R^2)\,J^T
Pk+1=FkPkFkT+GkQGkTP_{k+1}=F_k P_k F_k^T+G_k Q G_k^T

其中 FkF_k 是位姿更新对旧位姿的雅可比,GkG_k 是对 [Δs,Δθ][\Delta s,\Delta\theta] 的雅可比。前一项传播历史不确定性,后一项加入本次轮增量的噪声。特别是 Fyθ=Δscos⁡ϕF_{y\theta}=\Delta s\cos\phi:行驶越远,原有航向误差越容易放大为横向误差。

左右轮增量噪声经位姿积分形成逐渐扩大的位置与朝向不确定性

用 Python 验证十步传播

在安装了 NumPy 的 Python 3 环境运行下面的完整代码。初始化 P0=0P_0=0 只表示演示中起点被假定完全已知;真实系统应使用起始定位的不确定性。

python
import math
import numpy as np

wheelbase = 0.40
sigma_wheel = 0.005
pose = np.zeros(3)        # [x, y, yaw],起点在 odom 原点
cov = np.zeros((3, 3))    # 演示假设初始位姿已知

# 左右轮距离噪声映射到前进增量和航向增量
J = np.array([[0.5, 0.5],
              [-1 / wheelbase, 1 / wheelbase]])
wheel_cov = np.diag([sigma_wheel**2, sigma_wheel**2])
Q = J @ wheel_cov @ J.T

for _ in range(10):
    dl, dr = 0.10, 0.10
    ds = (dl + dr) / 2
    dtheta = (dr - dl) / wheelbase
    mid = pose[2] + dtheta / 2

    # 中点积分对位姿和运动增量的雅可比
    F = np.eye(3)
    F[0, 2] = -ds * math.sin(mid)
    F[1, 2] = ds * math.cos(mid)
    G = np.array([[math.cos(mid), -ds * math.sin(mid) / 2],
                  [math.sin(mid),  ds * math.cos(mid) / 2],
                  [0.0, 1.0]])

    # 先传播旧协方差,再加入这一采样周期的轮噪声
    cov = F @ cov @ F.T + G @ Q @ G.T
    pose += [ds * math.cos(mid), ds * math.sin(mid), dtheta]

std = np.sqrt(np.diag(cov))
print(f"pose: x={pose[0]:.2f} m, y={pose[1]:.2f} m, yaw={pose[2]:.2f} rad")
print(f"std:  x={std[0]:.4f} m, y={std[1]:.4f} m, yaw={math.degrees(std[2]):.2f} deg")

预期输出:

text
pose: x=1.00 m, y=0.00 m, yaw=0.00 rad
std:  x=0.0112 m, y=0.0322 m, yaw=3.20 deg

均值轨迹仍是直线,但横向标准差约 3.22 cm3.22\ \mathrm{cm},大于前进方向的 1.12 cm1.12\ \mathrm{cm}。这不是“机器人实际偏了 3.22 厘米”,而是当前随机噪声模型预测的离散程度。

向 ROS 2 发布可信的不确定性

ROS 2 的 nav_msgs/msg/Odometry 中,header.frame_id 通常设为 odom,child_frame_id 设为 base_link。pose.pose 存估计位姿,pose.covariance 存按 [x,y,z,roll,pitch,yaw][x,y,z,\mathrm{roll},\mathrm{pitch},\mathrm{yaw}] 排列的 6×66\times6 行主序矩阵;平面 PP 的行列对应索引为 0,1,50,1,5,例如 xx、yy、yaw 方差分别落在数组位置 0、7、35。交叉协方差也须对称填写,而 twist.covariance 另行表示速度估计的不确定性。

不要用全零协方差表示“数据不准”:下游融合器可能将零理解为极高确信度。应根据轮编码器标定和实际运动误差设定噪声,不能观测的维度按下游融合配置谨慎处理。可重复进行至少 100 次、每次 1 米的直行实验,用独立测量的终点位置统计误差;若横向偏差持续朝同一侧,优先检查轮径、轮距和打滑,因为零均值随机噪声模型无法覆盖持续性偏差。

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

目录与阅读设置

阅读设置

17

记录一个想法

让里程计估计包含不确定性

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

登录哪都通