跳到主要内容

4. 轨迹跟踪

定点控制只要求机器人到达一个目标；轨迹跟踪要求机器人在每个时刻都跟上一个随时间变化的目标。机械臂执行 MoveIt 2 轨迹、四足跟踪步态、移动机器人沿路径行驶，本质上都是轨迹跟踪问题。

4.1 轨迹内容​

一条合格的轨迹通常包含：

量含义为什么重要
位置 每个时刻应该在哪里决定几何路径
速度 每个时刻应该多快决定运动是否平滑
加速度 速度如何变化用于前馈力矩或加速度控制
时间戳 每个点何时到达决定节奏和同步

如果只有位置点，没有时间和速度，控制器只能“追点”。追点会带来两个问题：

  • 速度不连续，执行器容易抖。
  • 机器人可能在某些点停顿，动作不像规划器预期的轨迹。

4.2 前馈反馈​

轨迹跟踪常用结构是：

期望轨迹 -> 前馈项 ----+
                      +-> 执行器
当前状态 -> 反馈项 ----+

反馈项根据误差修正：

前馈项根据轨迹本身提前给力：

如果系统模型已知，前馈可以来自动力学：

其中 是质量矩阵， 是科氏 / 离心项， 是重力项。入门阶段不一定要完整实现逆动力学，但要理解：前馈负责“按计划用力”，反馈负责“纠正偏差”。

4.3 高增益限制​

有些人会想：既然误差大就加大 ，为什么还要前馈？

因为高增益反馈有代价：

  • 会放大传感器噪声。
  • 会激发结构柔性和传动间隙。
  • 会让接触任务变硬，容易撞击环境。
  • 遇到执行器限幅时，继续加增益没有意义。

前馈的价值是：在误差变大之前，就根据轨迹需求给出合理控制量。这样反馈项不用“拼命救火”，系统更平滑。

4.4 关节轨迹​

机械臂和腿式机器人常在关节空间跟踪：

def track_joint_trajectory(q_des, dq_des, ddq_des, q, dq):
    tau_ff = inverse_dynamics(q_des, dq_des, ddq_des)
    tau_fb = kp * (q_des - q) + kd * (dq_des - dq)
    return tau_ff + tau_fb

如果没有逆动力学，也可以先用：

tau = kp * (q_des - q) + kd * (dq_des - dq)

这就是带目标速度的 PD 跟踪。它比只用目标位置更稳，因为速度误差也参与了控制。

4.5 末端轨迹​

机械臂末端任务通常在笛卡尔空间描述：末端应该到达某个位置和姿态。

常见流程：

  1. 规划器给出末端轨迹。
  2. 用 IK 转成关节轨迹。
  3. 关节控制器跟踪关节轨迹。

也可以直接做任务空间控制：

然后通过雅可比矩阵转成关节力矩：

任务空间控制更贴近任务目标，但需要处理雅可比、奇异位形、关节限位和冗余自由度。入门阶段先掌握“末端轨迹 -> IK -> 关节轨迹 -> 关节控制器”的链路即可。

4.6 时间同步​

轨迹跟踪最容易被忽略的是时间。

控制器必须知道当前应该跟踪轨迹上的哪一个点：

elapsed = current_time - trajectory_start_time
q_des, dq_des, ddq_des = sample_trajectory(elapsed)

如果控制器循环有延迟，或者轨迹起始时间和执行器时间不同步，就会出现：

  • 机器人总是落后轨迹。
  • 某些段突然加速。
  • 轨迹执行结束后还在追旧目标。

在 ROS2 / MoveIt 2 里，轨迹消息通常带有 time_from_start。控制器需要根据这个时间戳插值，而不是把每个点当成普通列表逐个执行。

4.7 排错表​

现象可能原因
轨迹整体跟得慢控制增益低，前馈不足，执行器限速
每个路径点停顿把轨迹点当离散目标，没有按时间插值
速度突然跳变轨迹不连续，时间戳错误
末端路径对，但关节抖IK 解不连续，关节空间轨迹不平滑
仿真跟得好，真机落后通信延迟、控制频率低、负载模型不准

轨迹跟踪是连接规划器和执行器的桥。下一章的 MPC，可以看作在轨迹跟踪的基础上，把约束和未来预测一起放进优化问题里。

最后由 johnjim0816 于 更新