第 5 章 轨迹与插值¶
前面四章的回路都在跟踪"别人给的参考"。本章负责生成那个参考：从两个端点出发，先决定几何形状（走哪条路），再决定时间分配（多快走），产出一条满足速度/加速度/加加速度约束、且被控对象跟得住的轨迹。主线是关节空间与笛卡尔空间两条路的分工、多项式与梯形/S 曲线两类速度规划、时间参数化，以及落点——轨迹与反馈跟踪的耦合关系。运行环境 python 3.11 + numpy/scipy。运动学层面的正逆解与雅可比归 robotics-note，本章取其"位形空间路径"为输入。
先定几何形状、再配时间律，最后回检执行能力：
flowchart LR
shape["几何形状<br/>关节空间 / 笛卡尔空间"] --> time["时间分配<br/>多项式 / 梯形 / S 曲线"]
time --> out["q(t), q̇, q̈, 加加速度"]
out --> lim["速度 / 加速度 / 加加速度约束"]
lim --> track["交反馈回路跟踪"]1. 多项式轨迹：端点约束定阶数¶
最少约束的点到点轨迹用多项式满足边界条件。三次多项式绑四个条件（起止位置 + 起止速度为 0）：\(q(t)=a_0+a_1t+a_2t^2+a_3t^3\)，rest-to-rest 解出 \(q(\tau)=q_0+(q_f-q_0)(3\xi^2-2\xi^3)\)，\(\xi=t/T\)。五次多项式再多绑起止加速度为 0（六个条件），把加速度也做成平滑起落。同取 \(q_0=0,q_f=1,T=2\)：
三次: 峰值速度=0.750 (=1.5·Δq/T, 在中点) 峰值加速度=1.497 (在两端点, 阶跃)
五次: 峰值速度=0.937 (=15/8·Δq/T, 在中点) 峰值加速度=1.443 (在内部, 平滑)
一句话取舍：三次的加速度在端点是跳变（等效无穷 jerk），起停冲击大；五次把加速度也摊平、峰值更均衡，代价是同样 \(T\) 下峰值速度更高（0.94 vs 0.75，占约 25% 裕度）。所以"给定速度上限反求最短时间"时两者要分开算。多点串联用分段三次样条（每段绑位置与速度连续），整体用自然样条（加速度连续、解三对角带系统）——这些插值的构造与求解在 robotics-note 第 8 章「轨迹规划」展开，本章只用其结论：关节空间的平滑路径。
2. 梯形与 S 曲线速度剖面¶
给一个位移 \(d\) 与上限 \(v_{\max},a_{\max}\)，最省时的标称剖面是梯形速度：匀加速 \(t_r=v_{\max}/a_{\max}\)、匀速、匀减速三段。先判断能否跑到 \(v_{\max}\)：加速段位移 \(d_r=\tfrac12 a_{\max}t_r^2\)，若 \(2d_r\le d\) 则可达，否则退化为三角形（峰值 \(v_p=\sqrt{d\,a_{\max}}<v_{\max}\)）。取 \(d=3,v_{\max}=2,a_{\max}=5\)：\(t_r=0.40\) s、\(d_r=0.40\)、匀速 \(1.10\) s、总时间 \(1.900\) s。
梯形的问题在加速度是方波——起止与两处切换点加速度瞬时跳变（jerk 无穷），机器听得到的"咔"和末端抖振就来自这里。S 曲线（seven-segment）把加速度也做成梯形：jerk 受限下 \(a\) 平滑爬升，峰加速度与峰速度关系为 \(a_{\max}^{s}=\sqrt{j_{\max}v_{\max}}\)（当 jerk 约束先到顶时）。
取 \(j_{\max}=50,v_{\max}=2\)：\(a_{\max}^{s}=10\)，而设定 \(a_{\max}=5<10\)，说明本例加速度约束先binding、jerk 只把跳变沿摊成斜率有限的斜坡。代价一句话：同样 \(a_{\max}\) 下 S 曲线达峰更慢、总时间比梯形长，换来的是力冲击可控——接触附近、载荷易晃、高速龙门都值这个代价。
3. 时间参数化：几何与时间解耦¶
先定几何路径 \(\mathbf{q}(s)\)、再定 \(s(t)\) 的"时间参数化"（time parametrization / re-timing）把两件事解耦。设笛卡尔速度上限 \(v_{\max}\)，则沿路径 \(\|\dot{\mathbf{q}}\|=\|\mathbf{q}'(s)\|\dot s\le v_{\max}\) 给出 \(\dot s\le v_{\max}/\|\mathbf{q}'(s)\|\)：路径曲率大（\(\|\mathbf{q}'(s)\|\) 大）处必须放慢。取 \(\mathbf{q}(s)=(s,\sin 2\pi s)\)：
|dq/ds| 范围 [1.00, 6.36]
按参数等速(ds/dt=1)时最大笛卡尔速度 = 6.36 > vmax=1.0 → 违反约束
按 ds/dt=vmax/|dq/ds| 重定时: 总时间 = 弧长/vmax = 4.188 s（弧长 4.188，直线弦长 1，曲率把弧拉长）
若不重定时、直接令 \(s\) 线性推进，波峰处（路径陡峭）实际速度达 6.36 倍上限，执行器要么饱和要么丢步。重定时后波峰减速、波谷提速，总耗时由弧长而非弦长决定。这是 robotics-note 第 8 章「轨迹规划」时标化路径的回路侧用法：规划器给出 \(\mathbf{q}(s)\)，控制器按 \(\dot s\) 剖面沿线送点，两者接口就是 \((s,\dot s,\ddot s)\) 三元组（\(\ddot s\) 供加速度前馈）。
4. 关节空间还是笛卡尔空间¶
同样两个位形端点，在哪个空间插值决定末端走什么路。关节空间线性插值（\(\mathbf{q}\) 直连）实现简单、避开逐点逆解，但末端走的是弯曲弧；笛卡尔空间规划（末端 \(\mathbf{p}\) 走直线）要求满足工艺（焊接、涂胶、搬运不碰障碍）时必需，代价是每个路径点都要逆解、还可能撞奇异。平面二连杆 \(L_1=L_2=1\)，\((30^\circ,30^\circ)\to(60^\circ,-60^\circ)\)：
起点 [1.366 1.366] 终点 [1.5 0.866] 直线弦长=0.518
关节插值末端偏离笛卡尔直线最大=0.167（占弦长 32%）
一条 0.518 的"直线"搬运，关节插值实际让末端鼓出去 0.167——对自由空间点到点无所谓，对"必须走直线"的工艺就是 32% 的废品。选择判据：路径形状无关紧要（空移、点到点）用关节空间；末端轨迹本身是任务（沿缝、贴面、避障扫描）用笛卡尔空间 + 逐点逆解。骨架期本章以关节空间为主线，笛卡尔直线的逆解细节（雅可比伪逆沿路径选解、奇异规避）归 robotics-note 第 4 章「速度运动学与雅可比」与第 5 章「逆运动学」，需要时再回引。
5. 重试与平滑约束¶
规划出的轨迹要能被跟踪才算数，两处约束常在重试循环里出现。其一是执行器可达：反推出的 \(\ddot{\mathbf{q}}\) 经 \(M\ddot{\mathbf{q}}+C\dot{\mathbf{q}}+G=\tau\) 映射回力矩，若超 \(\tau_{\max}\) 就得回「梯形与 S 曲线速度剖面」一节把 \(a_{\max}\) 调小重算（时间变长）——这是"轨迹↔动力学"往返。
其二是奇异与可逆性：笛卡尔直线穿过奇异位形时逆解关节速度爆炸，重定时须沿路径检查雅可比最小奇异值下限、必要时改走弯绕。机械臂任务级的规划重试（碰撞返回、路点微调）归 manipulation-note 与 ROS2-note 的 MoveIt 侧，本章只界定"回路能吃的轨迹"这条下限。
6. 轨迹与跟踪：前馈把跟随误差压平¶
轨迹生成给的不只是位置参考，理想还应给 \((\mathbf{q}_d,\dot{\mathbf{q}}_d,\ddot{\mathbf{q}}_d)\) 三元组，供"前馈 + 反馈"结构使用。一阶对象 \(\tau\dot x=-x+u\)（\(\tau=0.1\)）、梯形位置参考（巡航速度 1.0）对比纯反馈 P 与 P+精确逆前馈 \(u=r+\tau\dot r\)：
巡航段最大跟踪误差: 纯反馈=0.0488 前馈+反馈=0.0010
纯反馈巡航尾端误差=0.0488（type-0 对斜坡的拖角随时间累积）
纯反馈下跟踪匀速段有恒定滞后（准稳态滞后 \(\approx v\tau/(1+K_p)=0.0048\)，但瞬态与减速段把最大误差抬到 0.049）；前馈把参考的各阶导数直接喂进控制器逆模型，误差塌到 0.001，反馈只需修模型失配而非追参考。
结论：轨迹给几阶导数，决定反馈要还多少债——只给位置，反馈就得自己从误差里"猜"速度和加速度；给足 \((\mathbf{q}_d,\dot{\mathbf{q}}_d,\ddot{\mathbf{q}}_d)\)，反馈回到只需抗扰的小角色。这条正是第 3 章「PID 工程」前馈节"前馈误差进跟踪不进稳定裕度"的量化收口，也是第 10 章「交互控制与模型预测」用轨迹前馈 + 反馈修正做接触跟踪的起点。
7. 要点¶
- 多项式阶数由要绑的边界条件定：三次绑位置+速度（加速度端点跳变）、五次再绑加速度（平滑但峰值速度高 25%）；多点串联用样条保证速度/加速度连续。
- 梯形最省时但加速度方波、jerk 无穷；S 曲线用 jerk 受限把加速度摊成斜坡，同样 \(a_{\max}\) 下更慢但力冲击可控。
- 时间参数化解耦几何与时间：曲率大处 \(\|\mathbf{q}'(s)\|\) 大必须减速，重定时后总时间由弧长决定；不重定时波峰速度可达上限 6 倍。
- 关节空间插值省逆解但末端走弧（本例偏离直线 32%）；要"走直线"必须笛卡尔规划逐点逆解——判据是路径形状是否本身是任务。
- 轨迹要能被跟踪才算数：反推 \(\ddot{\mathbf{q}}\) 超力矩或穿奇异就须重降 \(a_{\max}\) 或改路径。
- 前馈给足 \((\mathbf{q}_d,\dot{\mathbf{q}}_d,\ddot{\mathbf{q}}_d)\) 把巡航跟踪误差从 0.049 压到 0.001：参考给几阶导数决定反馈还多少债。