机器人 / 系统课时

从航向误差计算纠偏控制量

从航向误差计算纠偏控制量

给定差速底盘的当前位姿和参考路径,本篇把横向偏差与航向偏差转换为角速度指令,再用圆弧跟踪仿真观察纠偏过程中误差如何变化。示例使用 Python 标准库即可运行,并说明在 ROS 2 Jazzy 中如何核对真实反馈。

学习目标

  • 按统一符号约定计算带正负号的横向偏差与航向偏差。
  • 将路径曲率前馈和两种误差反馈合成为有界角速度。
  • 运行 20 Hz 运动学仿真,并用位置、航向和实际角速度判断控制效果。

在路径切线处定义误差

使用二维坐标系,机器人朝向角 ψ\psi 逆时针为正,车体前进方向为 base_link 的正 X 轴。设参考路径上最近投影点为 prefp_{ref},该点切线朝向为 ψref\psi_{ref};从投影点指向机器人位置 pp 的向量,投到切线的左法线上,得到横向偏差:

ey=(p−pref)⋅(−sin⁡ψref,cos⁡ψref)e_y=(p-p_{ref})\cdot(-\sin\psi_{ref},\cos\psi_{ref})

因此机器人处于路径左侧时 ey>0e_y>0,右侧时 ey<0e_y<0。航向偏差规定为「期望减实际」:

eψ=wrap(ψref−ψ)e_\psi=\mathrm{wrap}(\psi_{ref}-\psi)

wrap 将角度约束到 [−π,π)[-\pi,\pi),避免从 179∘179^\circ 到 −179∘-179^\circ 被误判为转动 358∘358^\circ。例如沿半径 R=2.0R=2.0 m 的逆时针圆弧前进,机器人在 (2.2,−0.1)(2.2,-0.1) m 时位于圆外侧,ey=2−2.22+(−0.1)2≈−0.202e_y=2-\sqrt{2.2^2+(-0.1)^2}\approx-0.202 m;当前航向为 1.421.42 rad,最近点的切向航向约为 1.5251.525 rad,故 eψ≈+0.105e_\psi\approx+0.105 rad。

圆弧路径上的横向偏差、航向偏差与纠偏过程

用偏差求角速度指令

对以恒定前进速度 vv 跟踪曲率为 κ\kappa 的路径,可先用 vκv\kappa 产生转弯所需的前馈角速度,再加入反馈:

ωcmd=clip(vκ+kψeψ−kyey,−ωmax,ωmax)\omega_{cmd}=\mathrm{clip}(v\kappa+k_\psi e_\psi-k_y e_y,-\omega_{max},\omega_{max})

这里正角速度代表逆时针转动;航向落后于参考切线时 eψ>0e_\psi>0,应增大左转量;机器人位于路径右侧时 ey<0e_y<0,负号也会增加左转量。不能把 eye_y 直接当作航向角相加,必须乘上具有合适单位的增益。

本次试验设置 v=0.30v=0.30 m/s、κ=1/R=0.50\kappa=1/R=0.50 m⁻¹、kψ=1.8k_\psi=1.8 s⁻¹、ky=1.2k_y=1.2 (m·s)⁻¹、ωmax=0.8\omega_{max}=0.8 rad/s。初始前馈量为 0.150.15 rad/s,代入上述两项误差得到 ωcmd≈0.582\omega_{cmd}\approx0.582 rad/s。增益是仿真起点而非通用实车参数;实际底盘还受最大轮速、加速度和打滑约束。

运行圆弧跟踪仿真

下面使用 Python 3 标准库模拟半径 2 m 的逆时针圆弧。每隔 0.050.05 s 从机器人位置重新找圆上的最近点,计算误差并更新位姿;每 2 s 输出一次数据。将代码保存为 track_feedback.py,执行 python3 track_feedback.py。

python
import math

R, v, dt = 2.0, 0.30, 0.05
x, y, yaw = 2.2, -0.1, 1.42

def wrap(angle):
    return (angle + math.pi) % (2.0 * math.pi) - math.pi

def errors(x, y, yaw):
    # 逆时针圆弧的左法线朝向圆心
    tangent = math.atan2(y, x) + math.pi / 2.0
    return R - math.hypot(x, y), wrap(tangent - yaw)

for step in range(161):
    ey, epsi = errors(x, y, yaw)
    # 曲率前馈 + 航向反馈 - 横向偏差反馈
    omega = v / R + 1.8 * epsi - 1.2 * ey
    omega = max(-0.8, min(0.8, omega))
    if step % 40 == 0:
        print(f"t={step*dt:4.1f}s e_y={ey:+.3f}m "
              f"e_psi={epsi:+.3f}rad omega={omega:+.3f}rad/s")
    if step == 160:
        break
    # 仅模拟二维运动学,不含电机响应延迟和打滑
    x += v * math.cos(yaw) * dt
    y += v * math.sin(yaw) * dt
    yaw = wrap(yaw + omega * dt)

预期输出:

text
t= 0.0s e_y=-0.202m e_psi=+0.105rad omega=+0.582rad/s
t= 2.0s e_y=-0.161m e_psi=-0.121rad omega=+0.126rad/s
t= 4.0s e_y=-0.101m e_psi=-0.082rad omega=+0.124rad/s
t= 6.0s e_y=-0.064m e_psi=-0.052rad omega=+0.133rad/s
t= 8.0s e_y=-0.041m e_psi=-0.033rad omega=+0.140rad/s

8 s 后横向偏差绝对值从约 0.2020.202 m 降为 0.0410.041 m。注意航向误差会先越过零点再回落:控制器先主动朝路径内侧转,再逐渐与圆弧切线对齐,不能只凭某一时刻的航向误差正负判断控制失效。

接入 ROS 2 并验证反馈

在 ROS 2 Jazzy 使用 ros2_control 的 diff_drive_controller 时,控制器的 ~/cmd_vel 接口接收 geometry_msgs/msg/TwistStamped,使用 twist.linear.x 和 twist.angular.z 两个分量;填入当前 header.stamp,控制频率可先设为 20 Hz。实际完整话题名取决于控制器命名空间,参见 Jazzy 控制器文档。

  1. 从定位或里程计获取 base_link 位姿,将参考路径变换到相同坐标系、相近时间戳后,求最近投影点和切线;不同坐标系的数据不能直接相减。
  2. 每个周期计算 eye_y、eψe_\psi 和 ωcmd\omega_{cmd},分别记录命令角速度及 nav_msgs/msg/Odometry 的 twist.twist.angular.z 反馈值,并检查控制器速度限幅是否生效。
  3. 在低速空旷环境测试:误差绝对值应整体下降;若持续放大,先核对横向误差符号、航向角方向和 TF 变换,再调整增益。若指令持续卡在 ±0.8\pm0.8 rad/s,应降速、减小增益或检查机械限制。

若轨迹曲率突变、参考点跳跃或定位时间戳过期,应先处理路径连续性和数据有效性,而不是盲目增大反馈增益。

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

目录与阅读设置

阅读设置

17

记录一个想法

从航向误差计算纠偏控制量

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

登录哪都通