机器人 / 系统课时
让里程计估计包含不确定性
让里程计估计包含不确定性
轮式里程计把左右轮运动积分为机器人位姿,但编码器量化、轮胎打滑和轮径偏差会让估计逐渐偏离真实位置。本课用差速底盘建立位姿协方差模型,运行一个十步直行实验,并学会把不确定性正确表达给 ROS 2 的后续定位模块。
学习目标
- 区分里程计位姿估计、真实位姿与协方差各自代表的含义。
- 从左右轮距离噪声推导前进和转向的不确定性,并递推位姿协方差。
- 用 Python 验证误差随路程扩大,理解 ROS 2
Odometry中的协方差字段。
位姿估计不等于真实位置
在平面中,用 表示估计位姿, 是朝向角(弧度)。例如里程计报出 ,只表示根据轮子数据推算走到了这里;实际位置可能因为滑动落在别处,需要外部定位或测量才能验证。
用 协方差矩阵 描述 的不确定性。对角线 是方差,平方根是各分量的标准差;非对角线表示位置与朝向误差的相关性。它不是实时测得的“真实误差”,更不是绝对误差上限。
从左右轮噪声算出运动噪声
设轮距 ,采样周期 ,左右轮各前进 。差速模型给出:
因此中心前进 ,朝向变化为零。假设每次两轮距离测量各有独立、零均值、标准差 的随机误差(仅为演示参数),则前进增量的标准差约为 ,转向增量的标准差约为 ,即 。
两个轮子都前进时,左右轮差值的噪声仍会引入随机转角。即便平均运动轨迹笔直,朝向的不确定性也会在后续行驶中变成横向位置的不确定性。
每走一步都传播协方差
采用中点航向 ,用 和 更新 ,用 更新朝向。先用雅可比 把左右轮噪声映射到 ,再递推位姿协方差:
其中 是位姿更新对旧位姿的雅可比, 是对 的雅可比。前一项传播历史不确定性,后一项加入本次轮增量的噪声。特别是 :行驶越远,原有航向误差越容易放大为横向误差。

用 Python 验证十步传播
在安装了 NumPy 的 Python 3 环境运行下面的完整代码。初始化 只表示演示中起点被假定完全已知;真实系统应使用起始定位的不确定性。
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")预期输出:
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 厘米”,而是当前随机噪声模型预测的离散程度。
向 ROS 2 发布可信的不确定性
ROS 2 的 nav_msgs/msg/Odometry 中,header.frame_id 通常设为 odom,child_frame_id 设为 base_link。pose.pose 存估计位姿,pose.covariance 存按 排列的 行主序矩阵;平面 的行列对应索引为 ,例如 、、yaw 方差分别落在数组位置 0、7、35。交叉协方差也须对称填写,而 twist.covariance 另行表示速度估计的不确定性。
不要用全零协方差表示“数据不准”:下游融合器可能将零理解为极高确信度。应根据轮编码器标定和实际运动误差设定噪声,不能观测的维度按下游融合配置谨慎处理。可重复进行至少 100 次、每次 1 米的直行实验,用独立测量的终点位置统计误差;若横向偏差持续朝同一侧,优先检查轮径、轮距和打滑,因为零均值随机噪声模型无法覆盖持续性偏差。