机器人 / 系统课时
轮式里程计怎样累计位姿漂移
轮式里程计怎样累计位姿漂移
差速机器人看似沿走廊走直线,里程计却可能把它画成弧线。这里从左右轮编码器计算每次位姿增量,再让右轮的测量距离仅偏大 1%,观察误差如何在 10 米行驶中累积,并学会在 ROS 2 中核查原因。
学习目标
- 根据轮半径、轮距和编码器增量更新二维位姿。
- 运行数值实验,量化 1% 单侧测量偏差造成的航向和横向漂移。
- 区分系统性偏差、打滑与积分误差,检查 ROS 2 里程计的坐标系和参数。
从轮子位移推算位姿
设差速底盘左、右轮半径为 ,轮中心间距为 ;车体坐标系 base_link 的前方是 +x,左方是 +y,逆时针转动为正航向。编码器测得轮子转角增量 (弧度),在纯滚动且不侧滑的假设下:
车体中心前进距离和航向变化分别为:
以 odom 坐标系中的 为上一步位姿,用中点航向法更新:
例如 、左右轮半径均为 ,若左轮转动 rad、右轮转动 rad,则两轮分别走 m、 m;中心前进 m,同时左转约 。中点法适合短采样步长;单步转角较大时可改用精确圆弧积分。

用代码复现一百分点误差
假定机器人真实沿直线前进,每步左右轮实际各走 m,共 100 步;但右轮编码器换算值被系统性高估为 m。用 Python 3 标准库分别积分真实值与错误测量值:
import math
def integrate(steps, left_m, right_m, wheel_base_m):
x = y = yaw = 0.0
for _ in range(steps):
# 轮距决定左右轮差值会被换算成多大的偏航
distance = (left_m + right_m) / 2.0
delta_yaw = (right_m - left_m) / wheel_base_m
# 用本步中点航向,把车体前进距离投影到 odom 坐标系
x += distance * math.cos(yaw + delta_yaw / 2.0)
y += distance * math.sin(yaw + delta_yaw / 2.0)
yaw += delta_yaw
return x, y, yaw
def main():
actual = integrate(100, 0.100, 0.100, 0.30)
measured = integrate(100, 0.100, 0.101, 0.30)
print(f"真实: x={actual[0]:.3f} m, y={actual[1]:.3f} m")
print(f"里程计: x={measured[0]:.3f} m, y={measured[1]:.3f} m")
print(f"横向误差: {measured[1] - actual[1]:.3f} m")
print(f"航向误差: {math.degrees(measured[2] - actual[2]):.2f} deg")
if __name__ == "__main__":
main()预期输出:
真实: x=10.000 m, y=0.000 m
里程计: x=9.865 m, y=1.660 m
横向误差: 1.660 m
航向误差: 19.10 deg误差并非简单地把 1% 乘以 10 米:右轮每步多计 m,会不断产生正的 ;后续的前进距离又被投影到逐渐偏斜的航向上,所以横向偏移最终达到约 1.66 米。试着把 0.101 改成 0.100,预期横向和航向误差都变为零。
找到漂移的具体来源
| 误差来源 | 典型现象 | 可执行的检查 |
|---|---|---|
| 两轮半径换算比例同向偏差 | 方向基本正确,直线距离整体偏长或偏短 | 地面量取 5 m,比较里程计前进距离 |
| 左右轮有效半径或编码器比例不一致 | 实际走直线,odom 轨迹逐渐弯曲 |
低速直行 5 m,检查终点横向偏差和航向 |
| 轮距 配置不准 | 直行可能正常,原地旋转角度却不准 | 原地旋转一周,与外部测角结果比较 |
| 加速、转弯或湿滑地面打滑 | 编码器仍有位移,但车体实际位移不足 | 在不同地面重复同一路线,对比漂移突变 |
| 速度采样时间错误或积分步长过大 | 快速转弯时误差更明显 | 如果输入是轮速,用真实时间戳求 并缩短积分步长 |
其中,固定比例偏差会反复向同一方向积累;打滑的误差常随地面和动作而变化。编码器刻度量化通常还会引入小幅随机误差。仅靠轮编码器无法知道自己已经偏离真实轨迹,因此反复积分不会自动纠正先前的偏差。
在 ROS 2 中核查与修正
在 ROS 2 Jazzy 的 diff_drive_controller 中,wheel_radius 控制轮角位移与线位移换算,wheel_separation 控制转角换算;open_loop 为 true 时使用指令速度估算里程计,而非真实关节反馈。真实机器人应在硬件反馈有效的前提下采用 open_loop: false,并依据直行与旋转试验标定尺寸,相关参数以 控制器文档 为准。
- 用
ros2 topic list找到里程计话题;根据实际命名运行ros2 topic echo /odom --once,或将/odom替换为控制器发布的话题。预期能读到header、child_frame_id、pose、twist等字段,数值取决于机器人当前运动。 - 检查
header.frame_id是否为odom、child_frame_id是否为base_link(如有命名空间前缀,以实际 TF 配置为准)。pose表示odom下的位姿,twist表示子坐标系中的速度;不要把二者当成相同坐标系的数据。 - 在防碰撞的空旷场地做 5 m 直线与原地一周试验,记录实际终点和
/odom差值。先修正轮半径与左右轮比例,再通过转角误差校准轮距;每修改一项后重复相同试验。 - 如果偏差集中出现在打滑场景,尺寸校准无法消除它,应借助 IMU、激光扫描匹配或 SLAM 的外部观测约束漂移。
轮式里程计通常负责提供短时间内连续的 odom → base_link 运动估计,而 SLAM 可通过调整 map → odom 来修正机器人相对地图的长期位置。odom 可以连续,但不保证长期全局准确;地图校正也不应该伪装成编码器从未发生过误差。