机器人 / 系统课时
从航向误差计算纠偏控制量
从航向误差计算纠偏控制量
给定差速底盘的当前位姿和参考路径,本篇把横向偏差与航向偏差转换为角速度指令,再用圆弧跟踪仿真观察纠偏过程中误差如何变化。示例使用 Python 标准库即可运行,并说明在 ROS 2 Jazzy 中如何核对真实反馈。
学习目标
- 按统一符号约定计算带正负号的横向偏差与航向偏差。
- 将路径曲率前馈和两种误差反馈合成为有界角速度。
- 运行 20 Hz 运动学仿真,并用位置、航向和实际角速度判断控制效果。
在路径切线处定义误差
使用二维坐标系,机器人朝向角 逆时针为正,车体前进方向为 base_link 的正 X 轴。设参考路径上最近投影点为 ,该点切线朝向为 ;从投影点指向机器人位置 的向量,投到切线的左法线上,得到横向偏差:
因此机器人处于路径左侧时 ,右侧时 。航向偏差规定为「期望减实际」:
wrap 将角度约束到 ,避免从 到 被误判为转动 。例如沿半径 m 的逆时针圆弧前进,机器人在 m 时位于圆外侧, m;当前航向为 rad,最近点的切向航向约为 rad,故 rad。

用偏差求角速度指令
对以恒定前进速度 跟踪曲率为 的路径,可先用 产生转弯所需的前馈角速度,再加入反馈:
这里正角速度代表逆时针转动;航向落后于参考切线时 ,应增大左转量;机器人位于路径右侧时 ,负号也会增加左转量。不能把 直接当作航向角相加,必须乘上具有合适单位的增益。
本次试验设置 m/s、 m⁻¹、 s⁻¹、 (m·s)⁻¹、 rad/s。初始前馈量为 rad/s,代入上述两项误差得到 rad/s。增益是仿真起点而非通用实车参数;实际底盘还受最大轮速、加速度和打滑约束。
运行圆弧跟踪仿真
下面使用 Python 3 标准库模拟半径 2 m 的逆时针圆弧。每隔 s 从机器人位置重新找圆上的最近点,计算误差并更新位姿;每 2 s 输出一次数据。将代码保存为 track_feedback.py,执行 python3 track_feedback.py。
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)预期输出:
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/s8 s 后横向偏差绝对值从约 m 降为 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 控制器文档。
- 从定位或里程计获取
base_link位姿,将参考路径变换到相同坐标系、相近时间戳后,求最近投影点和切线;不同坐标系的数据不能直接相减。 - 每个周期计算 、 和 ,分别记录命令角速度及
nav_msgs/msg/Odometry的twist.twist.angular.z反馈值,并检查控制器速度限幅是否生效。 - 在低速空旷环境测试:误差绝对值应整体下降;若持续放大,先核对横向误差符号、航向角方向和 TF 变换,再调整增益。若指令持续卡在 rad/s,应降速、减小增益或检查机械限制。
若轨迹曲率突变、参考点跳跃或定位时间戳过期,应先处理路径连续性和数据有效性,而不是盲目增大反馈增益。