机器人 / 系统课时
检查轨迹时间参数与运动限速
检查轨迹时间参数与运动限速
规划器找到了避障路径,并不代表这条路径已经满足执行时序要求。本课以 ROS 2 Jazzy、MoveIt 2 的 Panda 演示为例,检查 JointTrajectory 的位置、速度、加速度和累计时间,再用实际轨迹验证限速设置是否生效。
学习目标
- 识别
JointTrajectory的关节顺序及每个轨迹点的四类关键字段。 - 解释时间参数化、关节限速与速度/加速度缩放之间的关系。
- 在 RViz 只规划不执行,读取轨迹并完成执行前检查。
读懂轨迹点和时间轴
MoveIt 2 的规划结果通常放在 moveit_msgs/msg/RobotTrajectory 中,其中 joint_trajectory 是 trajectory_msgs/msg/JointTrajectory。它不是单个目标角度,而是多个关节在一系列相对时间点上的目标状态。
| 字段 | 含义与检查方式 |
|---|---|
joint_names |
有序关节名列表;Panda 手臂通常为 panda_joint1 至 panda_joint7 |
points[i].positions |
与关节名逐项对应的位置,转动关节单位 rad |
points[i].velocities |
对应速度,转动关节单位 rad/s |
points[i].accelerations |
对应加速度,转动关节单位 rad/s² |
points[i].time_from_start |
距轨迹起点的时长,含 sec 和 nanosec |
将时间换算成秒:t = sec + nanosec × 1e-9。时间应非负并随轨迹点严格递增;末点的 time_from_start 就是计划时长。例如同一关节从 0 rad 到 0.10 rad,耗时 1 s,只能算出区间平均速度 0.10 rad/s,不能据此断定整个过程中峰值速度也只有 0.10 rad/s。
时间参数化如何应用限速
OMPL 等规划器首先求出满足几何与碰撞约束的路径。MoveIt 2 再通过时间参数化(常见为 TOTG)为路径点安排 time_from_start、速度和加速度;有配置时还可接 Ruckig 做加加速度平滑。TOTG 可能重新采样并略微改变中间路径点,因此重新检查碰撞在高要求场景中仍有必要;算法细节见 MoveIt 2 时间参数化说明。

有效速度、加速度上限通常来自机器人的 URDF 和 MoveIt 配置包中的 config/joint_limits.yaml。规划请求的缩放系数取 (0, 1]:假设某转动关节生效上限分别为 2.0 rad/s 与 4.0 rad/s²,两个系数均为 0.20,那么该次规划对应的上限分别是 0.40 rad/s 与 0.80 rad/s²。这是限幅,不是要求关节持续以 0.40 rad/s 运动;由于加减速阶段存在,时长也不会固定变成原来的五倍。
在 RViz 的 MotionPlanning → Planning 中设置速度与加速度缩放为 0.20,重新点击 Plan;C++ 客户端可在已有 MoveGroupInterface move_group 对象、调用 plan() 之前这样写。
片段示例(依赖已创建的 move_group,不能独立运行):
move_group.setMaxVelocityScalingFactor(0.20);
move_group.setMaxAccelerationScalingFactor(0.20);修改系数后必须重新规划。不要以为修改已生成轨迹的缩放系数变量,就能自动改变已有轨迹;也不要用 0 表示停止,API 会将非正数按默认值处理。
读取一次真实规划的轨迹
先启动可正常规划的 Panda MoveIt 2 演示,确认 RViz 的 MotionPlanning 已选 panda_arm。以下程序订阅可视化用的 /display_planned_path,读取其中最后一条 RobotTrajectory 的 joint_trajectory,逐项核对时间和数组长度,并打印每个关节已提供采样点的速度、加速度绝对值峰值。将程序保存为 inspect_trajectory.py。
import rclpy
from rclpy.node import Node
from moveit_msgs.msg import DisplayTrajectory
class TrajectoryInspector(Node):
def __init__(self):
super().__init__("trajectory_inspector")
# 这是规划显示话题,不是控制器的实际执行目标。
self.subscription = self.create_subscription(
DisplayTrajectory, "/display_planned_path", self.inspect, 10
)
print("等待规划消息,请在 RViz 点击 Plan", flush=True)
def inspect(self, msg):
if not msg.trajectory:
print("消息不含轨迹")
return
jt = msg.trajectory[-1].joint_trajectory
names, points = jt.joint_names, jt.points
if not names or len(points) < 2:
print("关节列表为空或轨迹点不足")
return
n = len(names)
times = [
p.time_from_start.sec + p.time_from_start.nanosec * 1e-9
for p in points
]
# 空速度/加速度字段可以存在,但位置必须完整、时间必须递增。
sizes_ok = all(
len(p.positions) == n
and len(p.velocities) in (0, n)
and len(p.accelerations) in (0, n)
for p in points
)
times_ok = times[0] >= 0 and all(
times[i] > times[i - 1] for i in range(1, len(times))
)
print(f"关节数={n} 点数={len(points)} 总时长={times[-1]:.3f}s")
print(f"数组长度合法={sizes_ok} 时间递增={times_ok}")
if not (sizes_ok and times_ok):
print("结构异常,不应执行")
return
if not all(
len(p.velocities) == n and len(p.accelerations) == n
for p in points
):
print("速度或加速度缺失,不能按零处理")
return
for j, name in enumerate(names):
vmax = max(abs(p.velocities[j]) for p in points)
amax = max(abs(p.accelerations[j]) for p in points)
print(f"{name}: vmax={vmax:.3f} amax={amax:.3f}")
def main():
rclpy.init()
node = TrajectoryInspector()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()在另一个终端运行下面的命令;如果 MoveIt 2 由源码构建,还须先加载该工作区的 install/setup.bash:
source /opt/ros/jazzy/setup.bash
python3 inspect_trajectory.py预期输出(点击 Plan 后;<N>、<TIME>、<VALUE> 为动态数值,以下仅展示前一个关节的结果):
等待规划消息,请在 RViz 点击 Plan
关节数=7 点数=<N> 总时长=<TIME>s
数组长度合法=True 时间递增=True
panda_joint1: vmax=<VALUE> amax=<VALUE>程序会继续打印其余关节,按 Ctrl+C 退出。若一直没有数据,先运行 ros2 topic info /display_planned_path 检查消息类型、发布者数和命名空间,再确认你按的是 Plan 而非 Execute。如果配置使用了其他显示话题,应同步修改订阅路径。
用实际限值验收而不是只看动画
固定同一组起点与终点,在 RViz 分别用 0.50/0.50 和 0.20/0.20 规划两次,记录末点时间及各关节采样峰值。低限速方案通常时长更长、峰值不高于其对应上限,但不同路径与时间参数化算法会导致结果不严格成比例。
- 查阅当前已加载的
joint_limits.yaml和 URDF,按关节名将每个vmax、amax与“生效上限 × 当前缩放系数”对照。数值需保留一定浮点误差容差;超过限制就停止执行并排查配置和时间参数化链路。 - 确认时间单调、数组长度匹配,末点时间大于零,且速度、加速度非空;若为空,检查规划后处理是否启用,不要补零冒充有效轨迹。
- 再检查碰撞、关节位置边界及控制器接受的关节集合。
/display_planned_path只是可视化消息;实际执行需核对FollowJointTrajectory目标和/joint_states的跟踪反馈,不能用 RViz 回放速度当成实机速度。
只有时间参数、关节约束、碰撞检查与控制器反馈都通过,规划轨迹才具备进入实际执行验证的基础。