机器人 / 系统课时

给速度指令加入加速度限幅

给速度指令加入加速度限幅

速度控制器突然收到较大的线速度或角速度指令时,电机可能产生急加速、轮胎打滑或轨迹超调。本课使用 ROS 2 Jazzy 的 Nav2 Velocity Smoother,在底盘执行前限制速度变化率,并通过指令和里程计对照,验证限幅是否生效、跟踪误差如何变化。

学习目标

  • 根据控制周期计算线速度和角速度的单次允许变化量。
  • 配置并接入 Nav2 Velocity Smoother,使速度指令满足加减速约束。
  • 对比限幅前后的速度曲线、跟踪误差和停止行为。

从速度阶跃计算限幅

差速底盘通常使用 TwistStamped 表达车体速度,其中 twist.linear.x 为前进速度,单位 m/s;twist.angular.z 为偏航角速度,单位 rad/s。速度限幅限制速度值的大小,加速度限幅则限制相邻控制周期内速度变化的快慢,两者需要同时设置。

假设底盘原本静止,上层控制器突然要求前进速度达到 0.40 m/s、角速度达到 0.80 rad/s。若直接在 50 ms 内执行,相当于要求线加速度达到 8.0 m/s²、角加速度达到 16.0 rad/s²,可能超过电机和轮地接触条件允许的范围。

采用以下教学参数作为起点,实际应用必须根据电机能力、载荷、制动距离和地面附着条件重新标定。

参数 取值 含义
控制频率 20 Hz 每 0.05 s 更新一次
最大前进速度 0.40 m/s 线速度上限
最大角速度 1.00 rad/s 偏航角速度上限
最大线加速度 0.50 m/s² 前进加速约束
最大角加速度 1.00 rad/s² 旋转加速约束
最大线减速度绝对值 0.70 m/s² 常规减速约束
最大角减速度绝对值 1.20 rad/s² 常规旋转减速约束

在 20 Hz 下,线速度每次最多增加 0.025 m/s,角速度每次最多增加 0.05 rad/s。由静止达到本例目标,两个分量理论上均需约 0.8 s。下降速度时使用单独的减速度约束,不应直接复用加速上限。

速度平滑数据流与线速度角速度阶跃响应对比

需要区分两个误差:平滑输出与原始目标之间的差值,是主动限制加速度造成的指令滞后;实际里程计速度与平滑输出之间的差值,才主要反映底盘执行跟踪情况。不能仅因为加速阶段暂时达不到原始目标,就认为限幅导致控制失败。

为 Nav2 配置加减速约束

本例适用于 Ubuntu 24.04、ROS 2 Jazzy 和 Nav2 Jazzy,假设使用差速机器人,并由 diff_drive_controller 接收 geometry_msgs/msg/TwistStamped。如果已有完整 Nav2 系统,应修改现有 velocity_smoother 配置,而不是重复启动第二个平滑节点。

确认环境已安装 nav2_velocity_smoother;缺少时可执行 sudo apt install ros-jazzy-nav2-velocity-smoother。然后在工作目录建立 velocity_limits.yaml,写入以下完整配置。

yaml
velocity_smoother:
  ros__parameters:
    smoothing_frequency: 20.0
    feedback: "OPEN_LOOP"
    scale_velocities: false
    max_velocity: [0.4, 0.0, 1.0]
    min_velocity: [-0.4, 0.0, -1.0]
    max_accel: [0.5, 0.0, 1.0]
    max_decel: [-0.7, 0.0, -1.2]
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 0.5
    enable_stamped_cmd_vel: true
    stamp_smoothed_velocity_with_smoothing_time: true

这里的三个元素按 [x, y, theta] 排列。差速底盘不能横向平移,因此 y 轴速度和加速度均设为 0。max_decel 必须使用负值,例如线减速度设为 -0.7,不能错误地写成 0.7。

OPEN_LOOP 以平滑器上一次发布的速度作为下一周期的参考,不直接使用轮速反馈。它适合先进行可重复的指令级验证;若后续改成 CLOSED_LOOP,应保证里程计更新率和延迟满足平滑器要求,并重新检查低速振荡问题。

enable_stamped_cmd_vel: true 使输入输出使用 TwistStamped,符合 Jazzy 版 diff_drive_controller 的命令接口。stamp_smoothed_velocity_with_smoothing_time 是 Jazzy 后续版本支持的时间戳平滑选项,应确认安装的 Nav2 版本包含该参数。

接入底盘并观察限幅输出

实验应先在仿真环境或安全测试台上进行,确保驱动控制器已经启动、底盘周围无遮挡,并且不存在其他节点同时向最终控制话题发布速度。

本例约定上层命令进入 /cmd_vel_target,平滑器输出直接进入 /diff_drive_controller/cmd_vel。如果实际控制器使用不同命名空间,需要调整最后一项重映射。若系统还包含 Collision Monitor,应将安全监控节点安排在速度处理链路的最后一级,而不是绕过它直接向驱动发送指令。

在终端 A 进入保存 YAML 文件的目录,运行平滑节点。

bash
source /opt/ros/jazzy/setup.bash
ros2 run nav2_velocity_smoother velocity_smoother --ros-args --params-file velocity_limits.yaml -r cmd_vel:=/cmd_vel_target -r cmd_vel_smoothed:=/diff_drive_controller/cmd_vel

预期结果:velocity_smoother 节点启动并加载参数。它是生命周期节点,独立启动后通常还需要执行配置与激活操作。

保持终端 A 运行,在已加载相同 ROS 2 环境的终端 B 执行:

bash
ros2 lifecycle set /velocity_smoother configure
ros2 lifecycle set /velocity_smoother activate
ros2 lifecycle get /velocity_smoother

预期输出中,最后一条命令应显示:

text
active [3]

接下来在终端 C 连续发布测试指令。下面的 header: auto 是 ROS 2 命令行对标准消息头的自动时间戳填充方式。

bash
ros2 topic pub -r 10 /cmd_vel_target geometry_msgs/msg/TwistStamped '{header: auto, twist: {linear: {x: 0.4}, angular: {z: 0.8}}}'

预期结果:上层以约 10 Hz 提交固定目标,平滑节点以约 20 Hz 输出逐步增加的线速度和角速度。运行约 2 s 后用 Ctrl+C 停止发布,再观察输出停止的过程。

在另一个终端检查控制话题:

bash
ros2 topic hz /diff_drive_controller/cmd_vel
ros2 topic echo /diff_drive_controller/cmd_vel --field twist

预期结果:话题存在且接近 20 Hz;开始加速时,线速度由接近 0 逐步增加至 0.40 m/s,角速度逐步增加至 0.80 rad/s。实际消息间隔存在调度抖动,因此应依据时间差计算加速度,不要求逐帧数字严格等于理论值。

如果没有输出,依次确认生命周期状态、重映射后的话题名称、发布与订阅消息类型,以及目标速度是否持续送入。特别要避免上层发送 Twist,而平滑器配置为接收 TwistStamped 的类型不匹配问题。

对照验证安全性和跟踪误差

仅看到速度曲线平滑还不够,还应核对底盘里程计是否真正跟上指令。测试时记录原始目标、平滑输出和轮速反馈对应的里程计,确保比较发生在同一时间范围内。

在发布测试指令之前,新开终端执行录包命令:

bash
ros2 bag record -o limited_run /cmd_vel_target /diff_drive_controller/cmd_vel /diff_drive_controller/odom

预期结果:开始记录三个话题的数据;终止录制后,当前目录产生 limited_run 数据目录。实际消息数量、时间戳和文件大小取决于录制时长。

分析时用消息时间戳对齐数据,分别提取 twist.linear.x、twist.angular.z 与里程计中的 twist.twist.linear.x、twist.twist.angular.z。对于相邻的平滑输出样本,计算速度变化量除以实际时间间隔;对里程计数据则计算它与同时刻平滑指令的绝对误差。

建议按以下指标验收本例参数:

检查指标 验收参考
平滑指令最大线速度 不超过 0.40 m/s
平滑指令最大角速度绝对值 不超过 1.00 rad/s
加速阶段线加速度 约不超过 0.50 m/s²
加速阶段角加速度 约不超过 1.00 rad/s²
达到本例目标的理论时间 约 0.8 s
实际执行跟踪误差 由里程计测量,不预设必然改善

以上加速度检查适用于正常连续平滑阶段。时间戳、调度抖动与测量噪声需要单独考虑;不能将超时归零或紧急制动阶段直接按正常加速斜率判定。

例如,在启动后的 0.20 s,假设平滑指令为 0.10 m/s,里程计实际测得 0.08 m/s,则底盘对平滑指令的瞬时跟踪误差是 0.02 m/s;它与上层原始目标 0.40 m/s 的差值为 0.32 m/s。后者主要包含预期的加速滞后,不能直接用于判定电机跟踪质量。

如需对比未启用限幅的情况,应在相同仿真场景、载荷和目标序列下,停止平滑器输出后重新连接单一命令源,单独录制基线数据。比较峰值加速度、超调和平均绝对跟踪误差,同时注意底盘控制器本身也可能已启用速度或加速度限制。不要在真实运行底盘上为了制造基线而取消必要的硬件保护。

处理停止超时与实际安全边界

限幅提高了常规加减速的平顺性,但也会延长减速距离。按本例线减速度绝对值 0.70 m/s² 计算,机器人从 0.40 m/s 匀减速到零,理论减速时间约 0.57 s、距离约 0.11 m;这还没有计入通信延迟、驱动响应与轮胎打滑。

velocity_timeout: 0.5 用于防止上层长期停止发布后仍沿用旧目标。Nav2 Velocity Smoother 超时会将输出归零并停止持续发布,不能把这一行为等同于受限减速度的正常停车过程。需要平稳停车时,上层应持续发送零速度目标,让平滑器完成正常减速;驱动层必须另有独立的失联保护和紧急停机机制。

还需注意双重限幅:如果 diff_drive_controller 自身也配置了更严格的加减速度约束,那么最终执行速度可能比平滑器输出变化得更慢。可以启用控制器的 publish_limited_velocity,检查其 cmd_vel_out 输出,以区分上游平滑限制和底层控制器限制。

最终应同时通过三项检查:平滑指令在正常阶段满足速度与加减速约束、里程计跟踪误差处于底盘可接受范围、停止和失联情况下能够可靠进入安全状态。调参时先保证制动能力,再提高加速度,不要只以达到目标速度的快慢作为优化标准。

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

目录与阅读设置

阅读设置

17

记录一个想法

给速度指令加入加速度限幅

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

登录哪都通