机器人 / 系统课时

识别打滑和采样造成的轨迹漂移

识别打滑和采样造成的轨迹漂移

差速机器人即使收到恒定的直行指令,里程计轨迹也可能逐步偏离实际位置。本课用可运行的数值实验分别复现车轮打滑、左右轮径偏差和离散积分误差,再给出 ROS 2 Jazzy 上区分和校准这些误差的方法。

学习目标

  • 说明轮位移、轮距、航向积分如何形成位姿估计。
  • 用数值算例比较打滑、轮径偏差与积分步长的误差特征。
  • 根据直线、转弯和不同速度下的轨迹偏差选择检查与修正方法。

从轮位移理解误差来源

差速里程计通过左右轮编码器的转角增量估计车体运动。设左右轮有效半径为 rL,rRr_L,r_R,轮距为 bb,轮转角增量为 ΔϕL,ΔϕR\Delta\phi_L,\Delta\phi_R,则轮缘位移分别为 ΔsL=rLΔϕL\Delta s_L=r_L\Delta\phi_L、ΔsR=rRΔϕR\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}

例如两轮半径均为 0.050 m、轮距 0.30 m,两轮各转 10 rad 时,估计前进 0.50 m,转角为零。这些公式默认车轮与地面纯滚动;编码器只能记录轮子转了多少,无法直接测出轮胎相对地面的滑移。

  • 打滑:编码器累计 5.0 m 的轮缘运动,若有效地面位移仅为其 80%,真实前进只有 4.0 m,直行距离就会高估 1.0 m。
  • 轮径偏差:两侧编码器转速相同,但右轮实际有效半径为 0.051 m、左轮为 0.050 m,机器人会向左走弧线;按两侧均为 0.050 m 计算的里程计却会报告直行。
  • 采样积分:即使轮速完全准确,采用每步起始朝向推进的前向欧拉法,也会把连续圆弧近似成折线;在较大的转角步长下产生明显位置误差。

打滑、左右轮径偏差与离散积分产生的轨迹对比

运行脚本复现三种漂移

下面的三个工况互相独立:前两个比较真实运动与编码器估计,第三个只改变积分步长。保存为 drift_demo.py,使用 Python 3.10 或更新版本运行,无需安装第三方库。

python
from math import cos, degrees, hypot, sin

def arc_pose(v, omega, seconds):
    theta = omega * seconds
    if abs(omega) < 1e-12:
        return v * seconds, 0.0, theta
    return v / omega * sin(theta), v / omega * (1 - cos(theta)), theta

def euler_pose(v, omega, seconds, dt):
    x = y = heading = 0.0
    for _ in range(round(seconds / dt)):
        # 前向欧拉:用每一步开始时的朝向推进位置
        x += v * dt * cos(heading)
        y += v * dt * sin(heading)
        heading += omega * dt
    return x, y, heading

def main():
    radius, wheelbase, speed, seconds = 0.05, 0.30, 10.0, 10.0
    estimated = radius * speed * seconds
    actual = 0.8 * estimated  # 假定打滑使实际地面位移减少20%
    print(f"打滑: 估计 {estimated:.3f} m, 实际 {actual:.3f} m, 误差 {estimated-actual:.3f} m")

    # 右轮有效半径比标称值大2%,里程计仍按左右同半径计算
    v = (0.05 + 0.051) * speed / 2
    omega = (0.051 - 0.05) * speed / wheelbase
    x, y, yaw = arc_pose(v, omega, seconds)
    print(f"轮径偏差: 实际 x={x:.3f} m, y={y:.3f} m, 航向={degrees(yaw):.1f}°; 估计 y=0.000 m")

    reference = arc_pose(0.5, 1.0, 2.0)
    for dt in (1.0, 0.1):
        x, y, _ = euler_pose(0.5, 1.0, 2.0, dt)
        error = hypot(x - reference[0], y - reference[1])
        print(f"欧拉步长 {dt:.1f} s: 位置误差 {error:.3f} m")

if __name__ == "__main__":
    main()

预期输出:

text
打滑: 估计 5.000 m, 实际 4.000 m, 误差 1.000 m
轮径偏差: 实际 x=4.957 m, y=0.834 m, 航向=19.1°; 估计 y=0.000 m
欧拉步长 1.0 s: 位置误差 0.427 m
欧拉步长 0.1 s: 位置误差 0.042 m

轮径偏差的结果说明:仅有 2% 的左右轮半径差,行驶约 5 m 后就可能形成 19.1° 的航向偏差。积分实验则说明步长从 1.0 s 缩短到 0.1 s,可以把该理想恒速圆弧的欧拉位置误差从 0.427 m 降到 0.042 m;这些数字不是所有机器人都会出现的固定误差。

根据轨迹特征检查问题

观察到的现象 优先怀疑 可执行的区分方法
加速、急转或光滑地面上误差突然增加 轮胎纵向打滑或侧滑 同一路段分别用 0.2 m/s 与 0.5 m/s 跑三次,比较误差是否随速度和地面变化
低速直线反复向同一侧偏,偏航有固定方向 左右轮有效半径或轮胎接触条件不一致 记录两轮累计转角,用独立测量的航向检查真实车体是否走弧线
直线距离一直按相近比例高估或低估 标称轮半径或编码器换算系数不准 用卷尺标定 5.00 m 路段,比较里程计距离与实测距离
急弯误差大,降低积分步长后明显减小 时间步长过大或积分方法粗糙 使用相同的轮速数据分别按 1.0 s、0.1 s 步长重算,再与连续圆弧比较

打滑与轮径误差不能只看终点位置:还应同时比较独立测得的航向,并区分“真实机器人走偏”和“机器人没走偏但里程计报偏”。要检测打滑,外部基准必须独立于轮编码器,例如地面标记、运动捕捉或仿真真值;单独拿编码器里程计互相比对无法证明打滑。

在 ROS 2 Jazzy 中校准和验证

使用 ros2_control 的 diff_drive_controller 时,先确认 open_loop 为 false:此时里程计依赖硬件反馈;若为 true,控制器用命令速度估算里程计,更不能据此判断实际是否打滑。检查 nav_msgs/msg/Odometry 的位姿与时间戳,通常 header.frame_id 是 odom,child_frame_id 是 base_link。

  1. 在平整地面标出 5.00 m 的直线和已知转角,先以 0.2 m/s 低速各重复三次,记录起终点的真实位置、航向及 /odom 估计,再用 0.5 m/s 复测。可以先以“5 m 直线终点误差不超过 0.10 m”作为本次实验的自定验收阈值。
  2. 低速仍存在固定距离比例误差时,检查 wheel_radius;左右转向或直行偏航呈固定方向性时,再检查 left_wheel_radius_multiplier、right_wheel_radius_multiplier;转弯角度比例错误时检查 wheel_separation 与 wheel_separation_multiplier。每次只调整一组参数并重新跑相同路线。
  3. 若仅在加速或低附着力地面上出现跳变,应降低驱动加速度、改善轮胎附着,并结合 IMU 航向与外部定位判断漂移;固定的轮径修正系数无法补偿随机打滑。
  4. 若怀疑采样积分,核对反馈消息时间戳与控制器实际更新间隔,优先按真实时间差积分,必要时用步中朝向 θk+Δθ/2\theta_k+\Delta\theta/2 更新位置。publish_rate 控制里程计消息发布频率,不能直接当成积分更新频率。

具体参数名称和反馈模式可对照 ROS 2 Jazzy diff_drive_controller 官方文档。局部 odom 的积分误差通常会随路程积累,即使提高频率也不能消除打滑;需要长期定位时,应再引入独立观测进行校正。

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

目录与阅读设置

阅读设置

17

记录一个想法

识别打滑和采样造成的轨迹漂移

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

登录哪都通