2.5 轨迹生成与跟踪

2.5 轨迹生成与跟踪

本节摘要：逆运动学给出的是端点，控制器需要的是每个周期一个的关节目标值——把它们连续补齐的是轨迹生成。本节讲三次多项式插值、梯形速度曲线与样条，并用数字说明"梯形轨迹 + 动力学前馈"为什么是精度与平滑的组合拳。

控制器吃不到"终点"

上一节的 PID 仿真里，目标是一个恒定的 1 弧度。真实任务里目标是"0.5 秒后到达某角度且中途不撞东西"，如果直接把终点值拍给控制器，会发生什么？纯 P 环输出饱和，电机全速冲刺，到点后带着巨大速度冲过头再拉回来——快是快，撞也是真撞。轨迹生成的职责就是把"什么时候、以什么速度、经过哪里"显式地定义出来，让控制器每个周期收到的都是合理的中间目标。

三次多项式：最简单的光滑轨迹

点到点运动要求起止时刻的位置与速度给定（通常起止速度为零），四个约束恰好确定一条三次多项式。以从 q0 = 0 到 qf = 1 弧度、历时 T = 2 秒为例：

q(t) = q0 + (qf − q0)·(3s² − 2s³)，其中 s = t/T
q̇(t) = (qf − q0)/T·(6s − 6s²)
​

中点 s = 0.5 处速度最大：1.5·(qf−q0)/T = 0.75 弧度每秒；起止加速度最大：6·(qf−q0)/T² = 1.5 弧度每平方秒。代入 2.2 节动力学就能直接算出这条轨迹的力矩需求——这是轨迹生成与动力学第一次握手。

图：三种速度曲线的形态对比

梯形曲线胜在"峰值速度与时间预算可以心算"：给定行程 s 与加速度 a，最短时间为 √(4s/a)（若能到匀速段则再加匀速段时长）。S 形把加速度也变成连续斜坡，消掉了 jerk（加加速度）跳变，代价是峰值加速度上浮。选型口诀：慢而轻用多项式，对时间可预算有要求用梯形，高速高精（凸轮、直驱、激光加工头）用 S 形。

多路径点：样条与过冲

真实轨迹常要穿过一系列路径点（示教点、视觉抓取点）。逐段用三次多项式并让段间速度连续，就得到三次样条；进一步约束加速度连续则用五次样条或 B 样条。常见的坑是路径点过密：样条为了"光滑穿过"每个点，会在点间摆出多余的小波纹，末端画出的实际曲线在路径点附近蛇形——工程解法是增加过渡圆角（路径点只要求"靠近"不要求"穿过"），或用 B 样条的局部可控性只动邻近控制点。

跟踪：反馈之外，前馈定成败

把生成的轨迹喂给第 2.4 节的 PID，会发现一个残酷事实：再好的反馈参数，跟踪误差也不可能为零——反馈本质是"错了再改"，必然滞后。高速轨迹下，滞后误差与速度、加速度成正比。

破局的钥匙是前馈。把 2.2 节的动力学反过来用：由轨迹的 q_d(t)、q̇_d(t)、q̈_d(t) 直接算出所需的力矩 τ_ff(t)，提前喂给电流环，PID 只补残差：

def ff_pd_control(traj, i, q, v, Kp=120, Kd=6):
    """动力学前馈 + PD 反馈的最小骨架"""
    qd, vd, ad = traj.q[i], traj.v[i], traj.a[i]     # 期望轨迹
    tau_ff = M(qd) @ ad + C(qd, vd) @ vd + G(qd)     # 动力学前馈
    tau_fb = Kp * (qd - q) + Kd * (vd - v)           # PD 只补残差
    return tau_ff + tau_fb
​

数字最能说明差距：某三连杆臂做半径 0.3 米的圆弧，周期 1 秒，纯 PD（Kp=120, Kd=6）的最大跟踪误差约 12 毫米；叠加动力学前馈后降到约 1.5 毫米——八倍改善，而反馈参数一个没动。剩下的 1.5 毫米来自摩擦未建模与参数误差，这部分才是反馈环的正当工作。

💡 关键直觉：反馈处理不可预测的误差，前馈处理可预测的部分。凡是能由模型算出来的力矩，都不该让反馈去"追"——追就有滞后，滞后就是误差。

本节要点

• 轨迹生成把端点补成周期级目标序列，三次多项式、梯形、S 形各有适用面：平滑、可预算、低冲击。
• 样条过密路径点会蛇形，工程上用过渡圆角或 B 样条局部可控性解决。
• 跟踪误差的主战场在前馈：动力学前馈把误差砍掉一个数量级，反馈只兜底不可建模部分。
• 轨迹与动力学必须联算：速度曲线的加速度上限就是力矩需求的来源。

笛卡尔空间轨迹与腕部翻转

前面说的插值都发生在关节空间——直接对关节角插值。但焊缝、涂胶这类任务要求末端在笛卡尔空间走直线，于是流程变成：路径点序列 → 每个采样点做一次逆运动学 → 对关节角做时间参数化。这条链上埋着一个著名陷阱：路径穿过奇异位形附近时，逆解的关节速度会瞬时飙升（雅可比接近奇异），机械臂表现为"莫名其妙的猛甩"。工程对策有三：路径预检（离线检查全程的雅可比条件数，条件数超限的区段改道）；冗余自由度（七轴臂多一个自由度，可以"绕开"奇异）；或允许笛卡尔路径在奇异区段让步为关节空间过渡。腕部翻转（末端姿态跨过 180 度不连续点）是另一个高发事故点——欧拉角表示在翻转点跳变，逆解跟着跳，机械臂在半空"换手"。对策是姿态用四元数插值，或在路径里显式插入"过渡小圆弧"。

多层时间同步：为什么轨迹层要打时间戳

轨迹点由规划层低频发出（比如 10 赫兹一段），控制器高频消费（100 赫兹）。两层之间的时间对齐靠什么？靠轨迹段自带的时间参数：每段轨迹携带起始时刻与持续时间，控制器按本地时钟插值出当前时刻的期望值。若没有这套时间戳机制，规划层发送的抖动会被控制器误读为轨迹本身的变化——表现为低速下莫名其妙的"抽动"。这与第 5 章的时序纪律是同一件事在控制层的具体化：凡是跨模块的运动数据，必须携带时间语义。

问题：示教与轨迹复现有误差正常吗

正常但要有边界。示教再现的误差来源包括：示教点的逆解多解选择与示教时实际构型不一致（解的支别跳了）、负载与示教时不同、以及减速器背隙在换向处的虚位。检查次序：先确认逆解支别一致（看各关节角是否落在同一支），再看负载配置是否与示教登记一致，最后才怀疑机械磨损。误差稳定在半个背隙以内可视为正常；误差随行程累积则要查标定。

补跟踪误差的预算分配方法：末端总误差由规划插补误差、伺服跟踪滞后、机械振动三项构成，预算分配的经验比例是四三三。伺服滞后项与速度成正比（比例控制的跟踪误差约等于速度除以速度环增益），所以高速段精度骤降的根源常在伺服而不是规划——前馈补偿（把速度加速度直接前馈给伺服）能把这项压掉大半，是高速高精轨迹的标准配置。机械振动项在加减速拐点激励固有频率时最大，S 曲线加减速的作用就是把激励能量从固有频率挪开。诊断现场精度问题时按这三项依次排查：先看插补周期是否过长、再查跟踪误差曲线是否随速度线性增长、最后用加速度计确认振频——三步定位法比盲调参数快一个数量级。