第 8 章 轨迹规划¶
前三章解决"末端能到哪、怎么走"，剩下的是"沿一条路该怎么分时"。轨迹规划（trajectory planning / time scaling）给一条几何路径配上时间规律，使关节位置、速度、加速度都落在执行能力约束内且平滑可跟。这是规划链的最下一环——运动规划（第 9 章）给"无碰的几何路径"，时标化给"这条路径怎么在 \(T\) 秒内走完"；至于让机器人实际跟上轨迹的前馈 + 反馈控制律，归 control-note。
1. 关节空间多项式插值¶
单段点到点用多项式拟合。给两端的位置与速度约束，三次多项式够用；再约束两端加速度（软启动软停止），用五次多项式。rest-to-rest（两端速度、加速度都为零、位移 \(D\)、时长 \(T\)）的五次解即最小加加速度（minimum jerk）轨迹：
其速度 \(\dot q=\frac{30D}{T}\tau^2(1-\tau)^2\)、加速度 \(\ddot q=\frac{60D}{T^2}\tau(1-\tau)(1-2\tau)\)。峰值有闭式：
## D=1, T=2：
## 峰值速度 = 1.875*D/T = 0.9375 （τ=0.5 处）
## 峰值加速度 = (10/√3)*D/T^2 = 1.4434 （τ=0.5∓√3/6 处）
峰值随 \(1/T\)、\(1/T^2\) 缩放：给定 \(D\)，时长减半则速度峰值翻倍、加速度峰值变四倍，很容易顶破限制。所以多项式适合"先定时长再核约束"的场合，或反过来由速度/加速度限反解 \(T\)；要在给定限值下把能力打满，梯形/S 曲线更直接。
多段路径（经中间路点）靠拼接多段多项式：位置、速度连续容易保证，加速度连续要额外约束，jerk 在拼接点仍会跳。段数一多，手凑系数不现实，工程上改用样条（三次样条保加速度连续、五次样条保 jerk 连续）或整体最小 jerk 优化。时间分配也有两种口径：按各段距离比例分（简单，但看不出去约束最紧的那一段），或把段时长当优化变量、以各轴速度/加速度不越界为约束。
给路径配时间律，并回检执行能力约束：
flowchart LR
path["几何路径 q(s)"] --> time["时间律 s(t)<br/>多项式 / 梯形 / S 曲线"]
time --> out["q(t), q̇(t), q̈(t)"]
out --> chk{"速度 / 加速度在限内？"}
chk -->|"否"| time
chk -->|"是"| send["交控制器跟踪"]2. 梯形速度与 S 曲线¶
梯形速度剖面（trapezoidal velocity profile）在位移 \(D\)、限速 \(v_{\max}\)、限加速度 \(a_{\max}\) 下分加速—匀速—减速三段。加速时长 \(t_a=v_{\max}/a_{\max}\)，若加速+减速位移 \(v_{\max}^2/a_{\max}\le D\) 则能进入匀速，总时长 \(T=2t_a+t_c\)：
## D=1, vmax=0.5, amax=1 -> ta=0.5, tc=1.5, T=2.5, 峰值速度=0.5（打满限速）
短距离（\(v_{\max}^2/a_{\max}>D\)）时取不到限速，退化为三角形剖面，\(t_a=\sqrt{D/a_{\max}}\)。梯形的问题是加速度在段界处阶跃（加加速度无穷），冲击机械。七段 S 曲线（S-curve / jerk-limited）用有限 \(j_{\max}\) 把加速度也做成梯形，速度得 S 形：
## D=1, vmax=0.5, amax=1, jmax=5 -> ta=0.2, tv=0.3, tc=1.3, T=2.7
同一组约束下 S 曲线 \(T=2.7\) 比梯形 \(T=2.5\) 慢 \(0.2\) 秒。S 曲线并非总比梯形好：它以牺牲节拍时间为代价换来加加速度连续，在短距、高动态、节拍敏感的取放作业里这点时延未必划算——是否用 S 曲线取决于机构对冲击的容忍度与节拍要求，不是无条件的升级。
把梯形与最小 jerk 的位移/速度/加速度剖面并排画出，直观对照两者在段界处的光滑性差异：
图中对照：中排梯形速度平顶于 \(0.50\)、最小 jerk 速度峰值 \(0.9375=1.875D/T\)；下排梯形的加速度在 \(t_a\) 处从 \(+a_{\max}\) 跳到 \(0\) 再跳到 \(-a_{\max}\)（阶跃不连续、加加速度无穷），最小 jerk 的加速度则连续过零——这正是用 S 曲线消除冲击的动机。
3. 笛卡尔轨迹与姿态同步¶
直线运动类任务（涂胶、去毛刺、焊接）要在笛卡尔空间走直线，而非关节空间点到点。做法是在起点、终点位姿之间对位置线性插值、对姿态做球面插值：
## 位置：p(τ) = (1-τ) p0 + τ p1（笛卡尔直线）
## 姿态：四元数 slerp(q0, q1, τ)，且先确保 q0·q1>0（同半球，走最短弧）
姿态插值须用第 2 章「单位四元数与双覆盖」的 slerp，并注意 \(q\) 与 \(-q\) 同旋转——若两端四元数点积为负，须先翻号再插值，否则绕远。笛卡尔直线与关节插值一般不等价：直线要求逐时刻反解 IK（第 5 章），中途还可能穿越奇异，需检查可操作度。位置与姿态的插值参数 \(\tau\) 是否同步，决定末端工具姿态沿路径的过渡质量。
笛卡尔插值的一个代价：关节空间不再单调。末端沿直线匀速时，某个关节角可能在途中加速冲过极值再返回，若这段弧越到限位外，路径在关节空间就不可行——所以笛卡尔轨迹要按弧长细采样、逐点做 IK 与限位检查，采样密度由曲率与允许的跟随误差决定，而不是随手取几十个点。
4. 时间参数化与速度加速度约束¶
第 9 章运动规划产出的是一条几何路径（只有关节顺序、不带时间）。时间参数化（time scaling / reparameterization）给路径 \(q(s)\)（\(s\) 为弧长参数）找一个单调时间映射 \(s(t)\)，使 \(\dot q,\ddot q\) 满足速度、加速度、加加速度上界。核心是把约束转成对 \(\dot s,\ddot s\) 的逐点界：
在相平面 \((s,\dot s)\) 里，速度约束给一条 \(\dot s\) 上界、加速度约束给一条抛物线界，从 \((0,0)\) 到 \((S,0)\) 贴着界走即得最短时间的时标律（时间最优路径参数化 TOPP，前向–后向扫描）。这一步是"给定路径求最快且可行"的通用工具。轨迹交给控制器后如何用前馈 + 反馈跟住它，归 control-note；这一层止于生成可行的 \(q(t)\)。
数值实现有三处要小心：约束是逐点的，采样步长太粗会让界在两点之间被违反，一般留 3%–5% 的安全裕度；前向扫描给出"能多快"、后向扫描保证"还刹得住"，两者取小才是可行上界，只做前向会得到冲过头、只能急停的假解；加加速度约束在相平面里带二阶信息，需要额外维护加速度状态，实现比速度/加速度两层复杂得多。
5. 时间最优轨迹¶
最快速度到达且两端静止，就是时间最优（time-optimal）问题。对单轴双积分器 \(\ddot q=u\)、约束 \(|u|\le a_{\max}\)、\(|\dot q|\le v_{\max}\)，时间最优解是 bang-bang 控制：加速度全程取 \(\pm a_{\max}\) 切换至多一次——其速度剖面正是梯形（无匀速段时为三角形）。因此，第 2 节的梯形剖面不只是"简单"，它就是纯速度/加速度约束下的时间最优解。一旦再加 jerk 约束，最优结构升为七段 S 曲线。多轴耦合时时间最优解沿 \(C\)-space 的"最大速度流形"走，计算量大增，工程上常用逐轴梯形/S 曲线加缩放近似。
6. 要点¶
- 关节空间点到点：三次拟合端点位置+速度，五次（最小 jerk）再拟合端点加速度；rest-to-rest 峰值速度 \(1.875D/T\)、峰值加速度 \((10/\sqrt3)D/T^2\)。
- 梯形剖面打满 \(v_{\max},a_{\max}\)（示例 \(D=1\to T=2.5\)），是纯速度/加速度约束下的时间最优；S 曲线以有限 jerk 换平滑（同约束 \(T=2.7\)），短高动态未必划算。
- 笛卡尔直线：位置线性插值 + 姿态四元数 slerp（先同半球），直线需逐点 IK 并防奇异。
- 时间参数化把几何路径配时间：相平面里贴速度/加速度约束求最速时标律；跟踪控制律归 control-note。
- 时间最优 = bang-bang，速度剖面即梯形/三角形；加 jerk 约束后升为 S 曲线。