机器人 / 系统课时

检查轨迹时间参数与运动限速

检查轨迹时间参数与运动限速

规划器找到了避障路径,并不代表这条路径已经满足执行时序要求。本课以 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,不能独立运行):

cpp
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。

python
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:

bash
source /opt/ros/jazzy/setup.bash
python3 inspect_trajectory.py

预期输出(点击 Plan 后;<N>、<TIME>、<VALUE> 为动态数值,以下仅展示前一个关节的结果):

text
等待规划消息,请在 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 规划两次,记录末点时间及各关节采样峰值。低限速方案通常时长更长、峰值不高于其对应上限,但不同路径与时间参数化算法会导致结果不严格成比例。

  1. 查阅当前已加载的 joint_limits.yaml 和 URDF,按关节名将每个 vmax、amax 与“生效上限 × 当前缩放系数”对照。数值需保留一定浮点误差容差;超过限制就停止执行并排查配置和时间参数化链路。
  2. 确认时间单调、数组长度匹配,末点时间大于零,且速度、加速度非空;若为空,检查规划后处理是否启用,不要补零冒充有效轨迹。
  3. 再检查碰撞、关节位置边界及控制器接受的关节集合。/display_planned_path 只是可视化消息;实际执行需核对 FollowJointTrajectory 目标和 /joint_states 的跟踪反馈,不能用 RViz 回放速度当成实机速度。

只有时间参数、关节约束、碰撞检查与控制器反馈都通过,规划轨迹才具备进入实际执行验证的基础。

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

目录与阅读设置

阅读设置

17

记录一个想法

检查轨迹时间参数与运动限速

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

登录哪都通