机器人 / 系统课时

轮式里程计怎样累计位姿漂移

轮式里程计怎样累计位姿漂移

差速机器人看似沿走廊走直线,里程计却可能把它画成弧线。这里从左右轮编码器计算每次位姿增量,再让右轮的测量距离仅偏大 1%,观察误差如何在 10 米行驶中累积,并学会在 ROS 2 中核查原因。

学习目标

  • 根据轮半径、轮距和编码器增量更新二维位姿。
  • 运行数值实验,量化 1% 单侧测量偏差造成的航向和横向漂移。
  • 区分系统性偏差、打滑与积分误差,检查 ROS 2 里程计的坐标系和参数。

从轮子位移推算位姿

设差速底盘左、右轮半径为 rL,rRr_L,r_R,轮中心间距为 bb;车体坐标系 base_link 的前方是 +x,左方是 +y,逆时针转动为正航向。编码器测得轮子转角增量 ΔϕL,ΔϕR\Delta\phi_L,\Delta\phi_R(弧度),在纯滚动且不侧滑的假设下:

ΔsL=rLΔϕL,ΔsR=rRΔϕR\Delta s_L=r_L\Delta\phi_L,\qquad \Delta s_R=r_R\Delta\phi_R

车体中心前进距离和航向变化分别为:

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

以 odom 坐标系中的 (xk,yk,θk)(x_k,y_k,\theta_k) 为上一步位姿,用中点航向法更新:

xk+1=xk+Δscos⁡(θk+Δθ/2)x_{k+1}=x_k+\Delta s\cos(\theta_k+\Delta\theta/2)
yk+1=yk+Δssin⁡(θk+Δθ/2),θk+1=θk+Δθy_{k+1}=y_k+\Delta s\sin(\theta_k+\Delta\theta/2),\qquad \theta_{k+1}=\theta_k+\Delta\theta

例如 b=0.30 mb=0.30\,\text{m}、左右轮半径均为 0.05 m0.05\,\text{m},若左轮转动 2.02.0 rad、右轮转动 2.22.2 rad,则两轮分别走 0.1000.100 m、0.1100.110 m;中心前进 0.1050.105 m,同时左转约 1.91∘1.91^\circ。中点法适合短采样步长;单步转角较大时可改用精确圆弧积分。

差速底盘轮距与真实、估计轨迹的偏离

用代码复现一百分点误差

假定机器人真实沿直线前进,每步左右轮实际各走 0.1000.100 m,共 100 步;但右轮编码器换算值被系统性高估为 0.1010.101 m。用 Python 3 标准库分别积分真实值与错误测量值:

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

预期输出:

text
真实: x=10.000 m, y=0.000 m
里程计: x=9.865 m, y=1.660 m
横向误差: 1.660 m
航向误差: 19.10 deg

误差并非简单地把 1% 乘以 10 米:右轮每步多计 0.0010.001 m,会不断产生正的 Δθ\Delta\theta;后续的前进距离又被投影到逐渐偏斜的航向上,所以横向偏移最终达到约 1.66 米。试着把 0.101 改成 0.100,预期横向和航向误差都变为零。

找到漂移的具体来源

误差来源 典型现象 可执行的检查
两轮半径换算比例同向偏差 方向基本正确,直线距离整体偏长或偏短 地面量取 5 m,比较里程计前进距离
左右轮有效半径或编码器比例不一致 实际走直线,odom 轨迹逐渐弯曲 低速直行 5 m,检查终点横向偏差和航向
轮距 bb 配置不准 直行可能正常,原地旋转角度却不准 原地旋转一周,与外部测角结果比较
加速、转弯或湿滑地面打滑 编码器仍有位移,但车体实际位移不足 在不同地面重复同一路线,对比漂移突变
速度采样时间错误或积分步长过大 快速转弯时误差更明显 如果输入是轮速,用真实时间戳求 Δt\Delta t 并缩短积分步长

其中,固定比例偏差会反复向同一方向积累;打滑的误差常随地面和动作而变化。编码器刻度量化通常还会引入小幅随机误差。仅靠轮编码器无法知道自己已经偏离真实轨迹,因此反复积分不会自动纠正先前的偏差。

在 ROS 2 中核查与修正

在 ROS 2 Jazzy 的 diff_drive_controller 中,wheel_radius 控制轮角位移与线位移换算,wheel_separation 控制转角换算;open_loop 为 true 时使用指令速度估算里程计,而非真实关节反馈。真实机器人应在硬件反馈有效的前提下采用 open_loop: false,并依据直行与旋转试验标定尺寸,相关参数以 控制器文档 为准。

  1. 用 ros2 topic list 找到里程计话题;根据实际命名运行 ros2 topic echo /odom --once,或将 /odom 替换为控制器发布的话题。预期能读到 header、child_frame_id、pose、twist 等字段,数值取决于机器人当前运动。
  2. 检查 header.frame_id 是否为 odom、child_frame_id 是否为 base_link(如有命名空间前缀,以实际 TF 配置为准)。pose 表示 odom 下的位姿,twist 表示子坐标系中的速度;不要把二者当成相同坐标系的数据。
  3. 在防碰撞的空旷场地做 5 m 直线与原地一周试验,记录实际终点和 /odom 差值。先修正轮半径与左右轮比例,再通过转角误差校准轮距;每修改一项后重复相同试验。
  4. 如果偏差集中出现在打滑场景,尺寸校准无法消除它,应借助 IMU、激光扫描匹配或 SLAM 的外部观测约束漂移。

轮式里程计通常负责提供短时间内连续的 odom → base_link 运动估计,而 SLAM 可通过调整 map → odom 来修正机器人相对地图的长期位置。odom 可以连续,但不保证长期全局准确;地图校正也不应该伪装成编码器从未发生过误差。

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

目录与阅读设置

阅读设置

17

记录一个想法

轮式里程计怎样累计位姿漂移

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

登录哪都通