轨迹生成：让机器人沿正确的路径，在正确的时间运动

假设一台机械臂夹着工具，当前位姿为 $T_0$，目标位姿为 $T_1$。

逆运动学可以分别求出起点和终点的关节角 $q_0$、$q_1$。但如果下一控制周期直接把命令从 $q_0$ 改成 $q_1$，机器人并不会“立刻到达”，只会收到一次不可能完成的跳变：位置不连续意味着速度趋于无穷，驱动器要么拒绝命令，要么以自己的限幅和插补规则重新解释它。

所以，从 A 到 B 还缺少中间那一整段运动。

控制器在每个周期真正需要的是：

$$q(t),\qquad \dot q(t),\qquad \ddot q(t)$$

必要时还要知道加加速度：

$$\dddot q(t)$$

这些量不仅要在终点准确，还要在整个过程中连续、可达，并且不超过电机、减速器、机构和工艺允许的范围。

这就是轨迹生成要解决的问题。但它不能从“选一条插值公式”开始，因为一段机器人运动其实包含两个彼此独立、又必须重新组合的决定：

先决定机器人从哪里经过，再决定它什么时候经过。

前者是路径，后者是时间参数化。五次多项式、梯形速度、笛卡尔直线、ScLERP 和 S 曲线并不是互相竞争的五种答案；它们分属这两个不同层次，解决的是运动链中不同的问题。

1. 一个目标位姿，为什么还不是一段运动

先把问题缩小到一个关节。若关节从 $q_0$ 移动到 $q_1$，最简单的想法是把两端连起来：

$$q(t)=q_0+\frac{t}{T}(q_1-q_0)$$

位置看起来很连续，但速度为常数：

$$\dot q(t)=\frac{q_1-q_0}{T}$$

在 $t=0$ 前，关节原本静止；在 $t=0$ 后，它却立刻获得非零速度。终点也一样，速度会从非零瞬间跳回零。理想模型中的加速度因此出现脉冲。

这说明“把点连起来”只解决了位置问题，没有解决运动怎样开始和结束。

如果继续要求速度平滑，就要关心加速度；如果希望加速度也不要突然变化，就要继续关心 jerk。三阶导数并不是为了让公式显得完整，而是因为每提高一阶连续性，都在消除执行链中的一种突变：

量	直接描述	突变时意味着什么
$q$	关节位置	机器人被要求瞬移
$\dot q$	关节速度	需要冲击式加速度
$\ddot q$	关节加速度	需要瞬时改变驱动力趋势
$\dddot q$	jerk	加速度变化是否受到限制

但在讨论“怎样把速度变平滑”之前，还有一个更早的选择：从同一个起点到终点，机器人究竟应该沿哪条路走？

2. 第一个选择：任务真正要求保护哪条路径

同一组起点和终点，可以对应很多条合法路径。哪一条更好，取决于任务关心的是关节、工具中心点，还是完整刚体运动。

2.1 自由空间搬运：关节空间直线通常最直接

若机械臂只需要把零件从一个无遮挡位置搬到另一个位置，中间 TCP 是否严格走直线通常并不重要。此时可以直接定义：

$$\begin{aligned} q(s)&=q_0+s(q_1-q_0)\\ s&\in[0,1] \end{aligned}$$

当 $s$ 从 0 增加到 1，所有关节按同一比例完成各自位移。这条路径在关节空间中是直线，优点很实际：

• 不需要沿途反复求逆运动学；
• 只要两端角度选在同一连续分支上，关节路径天然连续；
• 每个关节走过的距离明确，便于计算时间和约束；
• 多关节使用同一个 $s$，可以自然同步起停。

它的代价是：经过正运动学映射后，TCP 通常会在笛卡尔空间中走曲线。对于自由搬运，这往往完全可以接受；对于焊接和插接，却可能直接破坏任务。

2.2 焊接或插接：必须直接定义 TCP 直线

若焊枪尖端必须沿工件上的直线前进，就应直接约束选定的 TCP：

$$p(s)=p_0+s(p_1-p_0)$$

如果姿态保持不变：

$$R(s)=R_0$$

如果位置必须走直线，同时姿态需要从 $R_0$ 旋转到 $R_1$，可以分别定义：

$$R(s)=R_0 \operatorname{Exp} \left( s\operatorname{Log}(R_0^{\mathsf T}R_1) \right)$$

于是：

$$T_{line}(s)= \begin{bmatrix} R(s)&p(s)\\ 0&1 \end{bmatrix}$$

这里保护的是 TCP 位置直线。法兰原点、工具尖端和工具上的其他点并不一定共享同一条路径，因此工程实现前必须先明确被约束的是哪个点、在哪个坐标系下表达。

2.3 转阀门或绕铰链：ScLERP 保护的是螺旋几何

若工具要绕一根固定轴转动，例如开门、转阀门或沿螺旋副运动，位置直线反而不符合物理结构。

给定两个完整位姿 $T_0,T_1\in SE(3)$，先计算相对位姿：

$$\Delta T=T_0^{-1}T_1$$

再沿相对螺旋运动插值：

$$T_{screw}(s) =T_0 \operatorname{Exp} \left( s\operatorname{Log}(\Delta T) \right)$$

这就是 ScLERP 的矩阵形式；用单位对偶四元数也可以写成等价表达。它先从两个端点中恢复一段相对螺旋运动，再沿同一根轴前进。

图中的两组运动具有相同的相对起终位姿：

• 左侧让 TCP 原点走两点之间的直线，同时姿态旋转；
• 右侧表示绕外部固定轴纯转动，TCP 原点自然走四分之一圆弧。

因此，ScLERP 不是“更高级的笛卡尔直线”，也不是“自动更平滑”的同义词。它回答的是完整刚体位姿沿什么几何前进；速度、加速度和 jerk 是否合理，要在下一层决定。

2.4 任务空间路径最终仍要回到关节空间

关节控制器最终需要的是 $q(t)$。所以，笛卡尔直线和 ScLERP 都要沿路径求解：

$$f(q(s))=T(s)$$

这里不能把每个采样点当成互不相关的 IK：

• 以上一个路径点的解作为下一个点的初值；
• 保持肩、肘、腕等逆解分支连续；
• 处理旋转关节的角度展开；
• 检查关节限位、碰撞与奇异性；
• 确认整段路径可达，而不只是两端可达。

经过这一步，三种路径最终都可以写成统一形式：

$$\begin{aligned} q&=q(s)\\ s&\in[0,1] \end{aligned}$$

到这里，我们只知道机器人从哪里经过，还没有给 $s$ 配上时钟。

3. 第二个选择：沿路径的时间怎样流动

$s$ 只是路径刻度。要让机器人真正运动，需要再定义：

$$\begin{aligned} s&=s(t)\\ s(0)&=0\\ s(T)&=1\\ \dot s(t)&\ge0 \end{aligned}$$

最终轨迹是：

$$q(t)=q(s(t))$$

任务空间同样如此：

$$T(t)=T(s(t))$$

这张图把全文的方法放回了各自位置：

• 关节直线、笛卡尔直线和 ScLERP 负责左侧的几何路径；
• 五次多项式、梯形速度和 S 曲线主要负责中间的时间规律；
• 二者复合后，才得到控制器真正执行的轨迹。

为什么要把两层拆开？因为同一条路径可以慢慢走，也可以在约束允许时快速走；同一个时间函数也可以施加到不同几何路径上。拆开后，我们既能保护任务要求的路径，又能单独调整运动节奏。

3.1 链式法则把两层重新接起来

记：

$$\begin{aligned} q_s&=\frac{dq}{ds}\\ q_{ss}&=\frac{d^2q}{ds^2}\\ q_{sss}&=\frac{d^3q}{ds^3} \end{aligned}$$

那么：

$$\dot q=q_s\dot s$$ $$\ddot q=q_{ss}\dot s^2+q_s\ddot s$$ $$\dddot q =q_{sss}\dot s^3 +3q_{ss}\dot s\ddot s +q_s\dddot s$$

这三条式子非常重要。它们说明关节运动由两部分共同塑造：

• $q_s,q_{ss},q_{sss}$ 来自路径本身；
• $\dot s,\ddot s,\dddot s$ 来自时间参数化。

在关节直线 $q(s)=q_0+s(q_1-q_0)$ 上，有 $q_{ss}=q_{sss}=0$，问题很简单；笛卡尔路径经过 IK 后通常是弯曲的，路径导数就会参与加速度和 jerk。也正因为如此，简单时间函数足以处理很多点到点运动，却未必适合复杂工艺路径。

先从最简单的关节直线开始，我们就能自然看到五次多项式为什么出现。

4. 为什么最后常常得到五次多项式

设所有关节沿共同路径参数运动：

$$q(t)=q_0+s(t)(q_1-q_0)$$

现在只需要设计一个从 0 到 1 的 $s(t)$。

4.1 线性时间函数的问题：速度会突然出现

若直接令：

$$s(t)=\frac{t}{T}$$

则 $\dot s=1/T$。轨迹开始时，关节速度会从零瞬间跳到：

$$\dot q(0^+)=\frac{q_1-q_0}{T}$$

这正是开头遇到的问题。于是我们至少要增加两条条件：

$$\dot s(0)=\dot s(T)=0$$

三次多项式拥有四个系数，恰好能满足位置和速度的四个端点条件。但它的端点加速度不为零；若前后连接的是静止段，加速度仍会跳变。

若希望机器人从“位置、速度、加速度都静止”的状态出发，并以同样状态停下，就需要六个条件：

$$\begin{aligned} s(0)&=0\\ s(T)&=1 \end{aligned}$$ $$\dot s(0)=\dot s(T)=0$$ $$\ddot s(0)=\ddot s(T)=0$$

最低需要六个系数，因此最小次数正好是五次。五次多项式不是凭经验随手挑出的，而是这些边界条件自然逼出来的最低阶选择。

4.2 归一化后，答案只有一条

令：

$$\begin{aligned} u&=\frac{t}{T}\\ u&\in[0,1] \end{aligned}$$

满足六个条件的标准时间函数为：

$$h(u)=10u^3-15u^4+6u^5$$

于是：

$$s(t)=h(t/T)$$ $$q(t)=q_0+(q_1-q_0)h(t/T)$$

它的导数为：

$$\dot q(t) =\frac{\Delta q}{T} 30u^2(1-u)^2$$ $$\ddot q(t) =\frac{\Delta q}{T^2} 60u(1-u)(1-2u)$$ $$\dddot q(t) =\frac{\Delta q}{T^3} 60(1-6u+6u^2)$$

其中 $\Delta q=q_1-q_0$。

现在前面的问题依次被消除了：位置连续，速度从零升起再回到零，加速度也从零出发并回到零。

但这并不意味着所有平顺性问题都消失了。端点 jerk 为：

$$\dddot q(0)=\dddot q(T)=\frac{60\Delta q}{T^3}$$

如果前后静止段的 jerk 为零，拼接处仍然会发生 jerk 跳变。五次多项式保证位置具有 $C^2$ 连续性，不等于 jerk 连续，也不等于已经满足某个硬 jerk 上限。

在这些零速度、零加速度端点条件下，这条曲线也是经典的最小平方 jerk 解。但“整体意义下让 jerk 尽量小”与“每一时刻都不超过 $j_{max}$”仍是两个不同问题。

4.3 总时间不是凭感觉填写的

五次曲线的形状固定后，运动快慢全部由 $T$ 控制。峰值为：

$$\max|\dot q| =\frac{15}{8}\frac{|\Delta q|}{T}$$ $$\max|\ddot q| =\frac{10\sqrt3}{3}\frac{|\Delta q|}{T^2}$$ $$\max|\dddot q| =60\frac{|\Delta q|}{T^3}$$

因此每个关节都能给出一个最短时长下界：

$$T_v\ge\frac{15|\Delta q|}{8v_{max}}$$ $$T_a\ge \sqrt{ \frac{10\sqrt3|\Delta q|}{3a_{max}} }$$ $$T_j\ge \sqrt[3]{ \frac{60|\Delta q|}{j_{max}} }$$

多关节共用一个时间函数时，取所有关节、所有约束下界中的最大值：

$$T=\max_i\{T_{v,i},T_{a,i},T_{j,i}\}$$

最慢的那个关节决定全局时长，其他关节随它同步完成。这就是五次多项式特别适合普通点到点运动的原因：路径简单，边界条件清楚，所有关节只需共享一只时钟。

可如果目标是“尽量快”，固定五次曲线并不会主动把速度、加速度能力用到边界上。于是，梯形速度自然出现了。

5. 为什么还需要梯形速度

五次多项式先规定一条平滑曲线，再放大 $T$ 以满足约束。梯形速度从相反方向思考：既然已经知道最大速度和最大加速度，能否尽量使用这些能力？

考虑单关节静止到静止运动，定义：

$$\begin{aligned} D&=|q_1-q_0|\\ d&=\operatorname{sgn}(q_1-q_0) \end{aligned}$$

在只限制速度和加速度的一维理想模型中，最快策略很直观：

1. 先以最大加速度加速；
2. 达到最大速度后保持；
3. 在恰好能够停到终点的位置开始最大减速。

速度曲线因此呈梯形。

5.1 位移决定它是梯形还是三角形

加速到 $v_{max}$ 所需时间为：

$$t_a=\frac{v_{max}}{a_{max}}$$

加速和减速阶段总共需要的位移为：

$$D_a=\frac{v_{max}^2}{a_{max}}$$

如果 $D\ge D_a$，机器人能够真正达到 $v_{max}$，匀速时间为：

$$t_c=\frac{D}{v_{max}}-\frac{v_{max}}{a_{max}}$$

总时间为：

$$T=2t_a+t_c =\frac{D}{v_{max}}+\frac{v_{max}}{a_{max}}$$

如果 $D<D_a$，机器人尚未达到最大速度就必须减速，匀速段消失，梯形退化成三角形：

$$t_a=\sqrt{\frac{D}{a_{max}}}$$ $$v_{peak}=\sqrt{a_{max}D}$$ $$T=2t_a$$

因此，出现负的匀速时间并不是“公式还能凑合用”，而是在提醒我们：这段运动根本没有匀速阶段，必须切换分支。

5.2 梯形速度的代价：加速度被瞬间打开

梯形速度直接利用 $v_{max}$ 和 $a_{max}$，因此在简化模型中很高效。但加速度会在 $0$、$+a_{max}$、$0$、$-a_{max}$ 之间瞬间跳变。

理想数学模型中，这些切换要求无限大的 jerk。真实系统只能依靠驱动器带宽、结构柔性或额外滤波把尖角磨圆，而磨圆后的运动已经不再是原先那条严格梯形速度。

这就逼出了下一步：如果不只限制加速度大小，还限制加速度变化得多快，时间规律必须增加 jerk 受限的过渡段。

6. S 曲线为什么通常有七段

jerk 定义为：

$$j(t)=\dddot q(t)=\frac{d\ddot q}{dt}$$

若 jerk 有限，加速度就不能再瞬间跳到 $a_{max}$，而要用一段斜坡逐渐升起。对称的静止到静止运动因此常被分成七段，jerk 依次为：

$$+j_{max}, 0, -j_{max}, 0, -j_{max}, 0, +j_{max}$$

它们分别完成：

1. 把加速度从零推到正值；
2. 保持正加速度；
3. 把加速度拉回零；
4. 保持匀速；
5. 把加速度推到负值；
6. 保持负加速度；
7. 把加速度拉回零。

这一次，速度和加速度都能连续变化。代价是分支更多：位移缩短时，匀速段会先消失，恒加速度段也可能继续消失；若初末速度、初末加速度不为零，阶段组合还会继续变化。

这也是为什么真实项目经常使用成熟的在线轨迹生成器，而不是只手写一份“标准七段公式”。难点不在画出一条 S 曲线，而在正确处理所有边界状态、退化分支与多自由度同步。

现在可以看清三种时间规律之间的关系：

方法	为什么出现	得到了什么	仍然缺少什么
五次多项式	希望端点速度、加速度都为零	紧凑的 $C^2$ 点到点运动	不直接利用约束边界，jerk 可跳变
梯形/三角速度	希望在速度、加速度限制下更快	直接使用 $v_{max}$、$a_{max}$	理想切换处 jerk 无界
七段 S 曲线	希望加速度也连续转向	显式 jerk 上限	分支与多轴同步更复杂

它们并不是三套互不相关的公式，而是每当上一种时间规律暴露新的执行问题，就多约束一阶导数后自然得到的结果。

7. 为什么复杂路径需要“时间参数化”而不只是一条公共曲线

对于关节直线：

$$q(s)=q_0+s(q_1-q_0)$$

$q_s$ 是常量，$q_{ss}=0$。用五次 $s(t)$ 或 S 曲线给所有关节共享时钟，往往已经足够。

但笛卡尔直线经过连续 IK 后，得到的关节路径 $q(s)$ 通常是弯曲的。即使 TCP 沿路径匀速，不同关节也可能在中间某处突然需要更高速度；接近奇异位形时，这种放大会更加明显。

此时，固定形状的 $s(t)$ 只能整体变快或变慢，却不会主动识别路径中哪个位置是瓶颈。我们需要让时间规律随着路径位置变化，这就是路径时间参数化。

7.1 速度限制变成沿路径的速度上界

由：

$$\dot q_i=q_{i,s}\dot s$$

关节速度约束：

$$|\dot q_i|\le v_{i,max}$$

会给出：

$$\dot s \le \frac{v_{i,max}}{|q_{i,s}|}$$

每个关节都会给路径速度设置上界，而真正允许的 $\dot s$ 由其中最严格者决定。瓶颈关节还会随路径位置变化：前半段可能是肩关节，后半段可能换成腕关节。

7.2 加速度限制要求提前为未来减速

关节加速度为：

$$\ddot q_i =q_{i,ss}\dot s^2 +q_{i,s}\ddot s$$

其中：

• $q_{i,ss}\dot s^2$ 来自几何路径本身的弯曲；
• $q_{i,s}\ddot s$ 来自沿路径加速或减速。

因此，即使 $\dot s$ 保持常数，弯曲路径仍可能需要关节加速度。时间参数化算法不能只看当前位置允许多快，还必须向前看：如果未来即将出现低速瓶颈，现在就要提前减速，否则到达瓶颈时已经来不及满足 $a_{max}$。

这就是图中“向前加速、向后制动”的意义：当前的最快可行速度，由整条未来路径共同决定。

7.3 力矩限制也会进入同一个问题

将：

$$\begin{aligned} \dot q&=q_s\dot s\\ \ddot q&=q_{ss}\dot s^2+q_s\ddot s \end{aligned}$$

代入机器人动力学：

$$M(q)\ddot q+C(q,\dot q)\dot q+g(q)=\tau$$

沿给定路径通常可整理成：

$$\tau =a(s)\ddot s +b(s)\dot s^2 +c(s)$$

于是电机力矩上下限也被投影成沿路径的可行速度和加速度范围。上坡姿态、重负载或较大惯量处会自动形成新的速度瓶颈。

7.4 jerk 约束为什么更难

加入：

$$|\dddot q_i|\le j_{i,max}$$

后，约束会同时包含：

$$\dddot q_i =q_{i,sss}\dot s^3 +3q_{i,ss}\dot s\ddot s +q_{i,s}\dddot s$$

问题从二阶约束进入三阶约束。算法不仅要决定当前能否加速，还要保证加速度能够以有限速率改变。最优段如何连接、怎样穿过奇异点以及怎样离散化都会更复杂。

因此，工程中常见两条路线：

1. 先按速度、加速度或力矩约束完成时间参数化，再做 jerk 受限平滑；
2. 对状态到状态运动，直接使用同时处理位置、速度、加速度和 jerk 上限的在线轨迹生成器。

无论采用哪条路线，平滑后都要重新检查约束、路径误差与碰撞。

8. 把几何路径与时间规律重新组合

现在可以用一个统一模板理解全文：

$$\boxed{ \text{轨迹} = \text{几何路径} \circ \text{时间规律} }$$

先看最简单的关节点到点运动。选择关节空间直线作为几何路径，再用五次时间函数控制沿路径前进的节奏：

$$q(t) =q_0+h(t/T)(q_1-q_0)$$

其中，$q_0$ 到 $q_1$ 的直线负责“从哪里经过”，$h(t/T)$ 负责“何时经过”。如果任务要求 TCP 严格沿直线运动，只需更换左侧的几何路径，而不必更换这只平顺时钟：

$$p(t)=p_0+h(t/T)(p_1-p_0)$$ $$R(t)=R_0 \operatorname{Exp} \left( h(t/T)\operatorname{Log}(R_0^{\mathsf T}R_1) \right)$$

此时位置仍严格落在线段上，只是沿线段的速度不再恒定；经过连续 IK 后，还要检查关节约束是否允许这个 $T$。如果任务保护的不是一条位置直线，而是完整刚体运动的螺旋几何，则再把路径换成 ScLERP：

$$T(t)=T_0 \operatorname{Exp} \left( h(t/T)\operatorname{Log}(T_0^{-1}T_1) \right)$$

这里螺旋几何不变，但不再使用线性的 $s=t/T$。机器人沿同一根相对螺旋轴平顺起步、加速、减速并停下。

这三个式子终于把方法串在了一起：五次多项式并不只属于关节空间，ScLERP 也不自带某种固定速度。一个选择几何，一个选择时钟；它们在 $s(t)$ 处相接。

如果简单 $h(t/T)$ 无法同时满足所有关节在复杂路径上的限制，就保留左边的几何路径，把右边升级为专门时间参数化算法。

9. 三个任务，三条自然的生成路线

回到开头那台机械臂。同样是从 $T_0$ 到 $T_1$，任务不同，正确路线也不同。

9.1 自由空间搬运

任务只要求安全到达，不要求 TCP 走特定曲线：

1. 选择连续、无碰撞的起终关节构型 $q_0,q_1$；
2. 使用关节空间直线 $q(s)$；
3. 普通低速任务可用五次时间标定；
4. 若对节拍和冲击敏感，使用 jerk 受限状态到状态生成器；
5. 检查整段关节路径的碰撞和限位。

这里没必要为了“空间轨迹更高级”而沿途反复求 IK。

9.2 沿工件直线焊接

任务明确要求焊枪 TCP 沿直线、保持工艺速度：

1. 在工件坐标系中定义 TCP 直线与姿态规律；
2. 沿路径连续求 IK，得到 $q(s)$；
3. 检查奇异性、分支、碰撞和关节限位；
4. 根据 $q_s,q_{ss}$ 和各关节限制进行时间参数化；
5. 若工艺要求恒定 TCP 速度，把它作为路径速度约束的一部分；
6. 在关节空间和笛卡尔空间同时复核结果。

这里不能先在关节空间插值，再期待 TCP 恰好变成直线。

9.3 转动阀门

任务要求工具绕阀门轴做固定轴运动：

1. 确定阀门轴与工具抓取关系；
2. 用 ScLERP 或显式螺旋轴生成 $T(s)$；
3. 连续求 IK 得到 $q(s)$；
4. 根据关节速度、加速度、jerk 与力矩限制设计 $s(t)$；
5. 检查工具与阀门之间的几何约束是否在全程保持。

这里笛卡尔直线反而会把工具从正确的圆弧上拉走。

这三个例子说明，轨迹生成的第一问从来不是“用五次还是梯形”，而是：任务要求哪一种几何不被破坏？ 只有路径确定后，时间规律的选择才有意义。

10. 开源软件中的这些步骤分别在哪里

现代机器人软件通常也遵循“先路径、后时间”的分工，只是每一层由不同组件完成。

10.1 MoveIt：规划器先给路径，轨迹处理再加时间

MoveIt 的运动规划器通常先生成没有时间信息的运动学路径。随后由轨迹处理阶段添加时间戳、关节速度和加速度。

当前 MoveIt 2 文档中的默认时间参数化方法是 Time-Optimal Trajectory Generation（TOTG）。它考虑关节速度和加速度限制，通过拟合并重新采样路径生成速度曲线。需要注意的是，MoveIt 的 TOTG 不一定严格只改时间戳：新采样点可能在给定关节空间路径容差内偏离原路径，因此在狭窄碰撞间隙中可能需要重新检查碰撞。

MoveIt 文档也明确指出，当前时间参数化算法本身不支持 jerk 限制；常见做法是让 Ruckig jerk 受限平滑作为最后一道后处理。

10.2 TOPP-RA：固定路径上的二阶约束传播

TOPP-RA 面向的是“路径已经给定，怎样在二阶约束下尽快通过”。它把路径离散化，通过可达性分析向前、向后传播可行集合，求出满足速度、加速度、力矩或接触稳定性等约束的时间参数化。

它与五次多项式的区别很清楚：五次多项式预先选择一个时间函数形状；TOPP-RA 则让速度形状由路径各处的约束主动决定。

10.3 Ruckig：jerk 受限的状态到状态运动

Ruckig 的输入可以包含当前与目标的位置、速度和加速度，以及各自由度的速度、加速度与 jerk 上限。对状态到状态运动，它能够生成多自由度同步、jerk 受限的时间最优轨迹。

但状态到状态生成与“严格沿一条已知笛卡尔路径进行时间参数化”不是同一个问题。若任务必须保护 TCP 直线或螺旋几何，仍应先保护路径，再选择合适的时间参数化或跟踪方法。

三者可以这样放回运动链：

工具	接收到什么	主要负责什么
MoveIt TOTG	离散关节路径	添加满足速度、加速度约束的时间信息并重采样
TOPP-RA	固定路径与二阶约束	计算沿路径的近时间最优速度规律
Ruckig	当前/目标运动状态与三阶上限	生成 jerk 受限、同步的状态转移

11. 工程实现中最容易忽略的几件事

路径尖角不能靠改时间消除。 若几何路径在路点处方向不连续，$q_s$ 本身会跳变。以非零速度通过该点需要无穷大加速度。时间参数化只能让机器人在角点停下或显著降速；若要连续快速通过，必须先对路径做圆角、样条或其他路径混合。

不能只检查采样点。 离散点满足约束，不代表点间多项式也满足。速度、加速度和 jerk 峰值可能出现在区间内部，碰撞也可能发生在两个无碰撞路点之间。应对连续段求极值，或使用带误差依据的连续检查。

jerk 也不能靠三次生硬差分可靠测量。 从离散位置连续做三次差分，会显著放大编码器噪声与时间戳抖动。验证规划 jerk 时优先使用解析导数；分析实测数据时则要结合可靠时间戳、适当滤波和系统带宽。

整体降速有明确的能力边界。 若把总时长乘以 $\lambda>1$，在同一路径和同一归一化时间函数下，速度、加速度与 jerk 会分别按：

$$\frac1\lambda, \frac1{\lambda^2}, \frac1{\lambda^3}$$

缩小。这是有用的保守降速手段，却不能修复 IK 跳支、路径尖角、碰撞或奇异性。

最后，真实限制来自完整执行链。 配置中的 $v_{max}$、$a_{max}$、$j_{max}$ 不应只是一组“能跑起来”的数字。它们要综合考虑电机和驱动器、减速器与机构、末端负载、控制周期、通信抖动、安全要求和工艺条件。

还应确认下层控制器真正接收什么：有的控制器执行带时间戳的 $q,\dot q,\ddot q$，有的只接收位置点并自行插补。如果下层再次用未知规则重规划，上层验证过的 jerk 受限曲线可能根本没有被原样执行。

12. 一套从目标到执行的完整流程

现在可以把整篇文章收束为一条连续工作流：

1. 明确任务真正保护的几何：关节运动、TCP 直线、固定螺旋轴，或其他工艺路径；
2. 构造路径：在关节空间得到 $q(s)$，或先在 $SE(3)$ 中得到 $T(s)$；
3. 回到连续关节路径：对任务空间路径连续求 IK，处理分支、限位、奇异与碰撞；
4. 整理真实约束：速度、加速度、jerk，必要时加入力矩、接触和工艺速度；
5. 选择时间方法：简单点到点使用五次或 S 曲线，速度/加速度优先时考虑梯形，复杂固定路径使用专门时间参数化；
6. 组合成轨迹：计算 $q(s(t))$ 或 $T(s(t))$；
7. 检查整段连续曲线：峰值、碰撞、路径误差和动力学可行性都不能只看端点；
8. 按真实控制周期执行并记录反馈：确认下层没有重新改变运动规律，再逐步提高速度。

这条链中，每种方法都有明确位置：

• 关节直线、笛卡尔直线和 ScLERP 决定几何；
• 五次多项式、梯形速度和 S 曲线决定简单运动的时间规律；
• TOPP 一类方法为复杂既定路径安排受约束的速度；
• 逆运动学、雅可比与拉格朗日动力学把任务路径逐层连接到关节速度和驱动力矩。

轨迹生成因此不是公式菜单，而是一串由任务逐步逼出的决定。目标位姿告诉机器人去哪里，路径告诉它从哪里经过，时间参数化告诉它何时经过，运动约束则决定这一切能否真正发生。

参考与延伸阅读

1. Kevin M. Lynch, Frank C. Park, Modern Robotics: Mechanics, Planning, and Control, Chapter 9: Trajectory Generation, Cambridge University Press, 2017. 官方课程资源
2. James E. Bobrow, Steven Dubowsky, J. S. Gibson, “Time-Optimal Control of Robotic Manipulators Along Specified Paths,” The International Journal of Robotics Research, 4(3), 1985.
3. Kang G. Shin, Neil D. McKay, “Minimum-Time Control of Robotic Manipulators with Geometric Path Constraints,” IEEE Transactions on Automatic Control, 30(6), 1985.
4. Hung Pham, Quang-Cuong Pham, “A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis,” IEEE Transactions on Robotics, 34(3), 2018. arXiv:1707.07239
5. Lars Berscheid, Torsten Kröger, “Jerk-limited Real-time Trajectory Generation with Arbitrary Target States,” Robotics: Science and Systems, 2021. arXiv:2105.04830
6. MoveIt 2, Time Parameterization
7. toppra, Time-Optimal Path Parameterization by Reachability Analysis
8. Ruckig, Online Trajectory Generation documentation