机器人 / 系统课时
给速度指令加入加速度限幅
给速度指令加入加速度限幅
速度控制器突然收到较大的线速度或角速度指令时,电机可能产生急加速、轮胎打滑或轨迹超调。本课使用 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,写入以下完整配置。
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 文件的目录,运行平滑节点。
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 执行:
ros2 lifecycle set /velocity_smoother configure
ros2 lifecycle set /velocity_smoother activate
ros2 lifecycle get /velocity_smoother预期输出中,最后一条命令应显示:
active [3]接下来在终端 C 连续发布测试指令。下面的 header: auto 是 ROS 2 命令行对标准消息头的自动时间戳填充方式。
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 停止发布,再观察输出停止的过程。
在另一个终端检查控制话题:
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 的类型不匹配问题。
对照验证安全性和跟踪误差
仅看到速度曲线平滑还不够,还应核对底盘里程计是否真正跟上指令。测试时记录原始目标、平滑输出和轮速反馈对应的里程计,确保比较发生在同一时间范围内。
在发布测试指令之前,新开终端执行录包命令:
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 输出,以区分上游平滑限制和底层控制器限制。
最终应同时通过三项检查:平滑指令在正常阶段满足速度与加减速约束、里程计跟踪误差处于底盘可接受范围、停止和失联情况下能够可靠进入安全状态。调参时先保证制动能力,再提高加速度,不要只以达到目标速度的快慢作为优化标准。