3. 关节空间轨迹规划：梯形速度、S形速度与多项式插值

关节空间轨迹规划，说白了就是让机器人每个关节从A点转到B点，中间怎么走的问题。你想想看，如果直接让电机以最大速度冲过去，末端执行器可能会抖得像筛糠一样。所以我们需要给关节设计一条平滑的路径。

我个人习惯把这类规划分成两大类：分段式速度规划和多项式插值。前者直观、计算快，后者平滑、可控性强。今天我们就把这几种方法挨个捋一遍。

3.1 梯形速度规划

梯形速度规划，也叫T型曲线。名字很形象——速度曲线像个梯形。分为三段：匀加速、匀速、匀减速。

我记得第一次在六轴机器人上跑梯形规划时，发现加速度突变的地方会有明显的冲击声。嗯，这就是梯形规划的硬伤——加速度不连续。

梯形规划的数学表达很简单：

// 已知：起始角度 θ₀，目标角度 θ₁，最大速度 v_max，最大加速度 a_max
// 计算加速段距离、匀速段距离、减速段距离

// 加速段距离
d_acc = 0.5 * v_max² / a_max
// 减速段距离
d_dec = d_acc
// 匀速段距离
d_cruise = (θ₁ - θ₀) - d_acc - d_dec

// 如果 d_cruise < 0，说明距离太短，无法达到最大速度
// 此时需要重新计算实际能达到的最大速度
if (d_cruise < 0) {
    v_actual = sqrt(a_max * (θ₁ - θ₀))
    // 变成只有加速和减速的三角形曲线
}

实际项目中，梯形规划最常用的场景是：

• 点到点快速移动——比如焊接机器人空行程移动
• 对冲击不敏感的任务——比如码垛机器人抓取和放置之间的过渡
• 计算资源受限的嵌入式控制器——梯形规划计算量极小

3.2 S形速度规划

S形速度规划，顾名思义，速度曲线像个拉长的S。它把梯形规划的加速段又细分为：加加速、匀加速、减加速三段。减速段同理。这样加速度就连续了，没有突变。

说白了，S形规划就是用加速度的变化率（加加速度，Jerk）来平滑过渡。加加速度越小，运动越柔和，但总时间也会变长。

S形规划的典型分段（7段式）：

阶段	名称	加速度状态	速度状态
1	加加速	从0增加到 a_max	缓慢增加
2	匀加速	保持 a_max	线性增加
3	减加速	从 a_max 降到 0	缓慢增加至 v_max
4	匀速	0	保持 v_max
5	加减速	从0降到 -a_max	缓慢减小
6	匀减速	保持 -a_max	线性减小
7	减减速	从 -a_max 升到 0	缓慢减小至 0

我在项目中遇到过一个问题：S形规划的参数太多，调参很麻烦。加加速度设太大，冲击还是明显；设太小，运动时间又太长。后来我总结了一个经验——加加速度一般设为最大加速度的 5~10 倍，这样既平滑又不至于太慢。

3.3 多项式插值

多项式插值，就是用多项式函数来拟合关节角度随时间的变化。你想想看，只要给定起点和终点的位置、速度、加速度约束，就能解出多项式的系数。

常用的有三种：3次、5次、7次多项式。次数越高，能约束的边界条件越多，曲线越平滑，但计算量也越大。

3.3.1 3次多项式插值

3次多项式是最简单的插值方式。它只约束起点和终点的位置和速度，共4个条件，刚好解出4个系数。

// 3次多项式：θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³
// 约束条件：
// θ(0) = θ₀, θ(T) = θ₁
// θ'(0) = v₀, θ'(T) = v₁

// 系数求解：
a₀ = θ₀
a₁ = v₀
a₂ = (3·(θ₁ - θ₀) - (2·v₀ + v₁)·T) / T²
a₃ = (2·(θ₀ - θ₁) + (v₀ + v₁)·T) / T³

3次多项式的优点是计算简单，实时性好。但缺点也很明显——加速度是线性的，起点和终点的加速度可能不为0，导致冲击。

我记得在早期的一个SCARA机器人项目中，用了3次多项式做关节插补。结果每次启停时末端都会抖一下。后来换成5次多项式，问题就解决了。

3.3.2 5次多项式插值

5次多项式比3次多了两个约束——起点和终点的加速度。共6个条件，解6个系数。

// 5次多项式：θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵
// 约束条件：
// θ(0) = θ₀, θ(T) = θ₁
// θ'(0) = v₀, θ'(T) = v₁
// θ''(0) = a₀, θ''(T) = a₁

// 系数求解（略，建议用矩阵求逆或直接公式）

5次多项式是工业机器人最常用的插值方式。它保证了加速度连续，启停平稳。我个人的经验是：如果对运动平滑性有要求，优先选5次多项式。计算量不大，效果却比3次好很多。

3.3.3 7次多项式插值

7次多项式更进一步，约束了起点和终点的加加速度（Jerk）。共8个条件，解8个系数。

为什么要约束加加速度？因为加速度连续不代表加加速度连续。加加速度突变同样会引起机械振动。7次多项式让加加速度也连续，运动极其平滑。

但代价也很明显：

• 计算量大，实时性差
• 容易出现数值振荡（Runge现象）
• 参数多，调参复杂

3.4 三种方法对比

方法	加速度连续性	加加速度连续性	计算量	适用场景
梯形速度规划	不连续	不连续	极低	快速移动、粗定位
S形速度规划	连续	分段连续	低	通用工业场景
3次多项式	线性变化	不连续	极低	简单插补
5次多项式	连续	不连续	低	工业机器人主流
7次多项式	连续	连续	中	高精度、高平滑场景

3.5 知识体系总览

下面这张图把本章的知识结构串起来了。你可以看到，关节空间轨迹规划从速度规划到多项式插值，各有各的用武之地。

3.6 选型建议

说了这么多，到底该怎么选？我个人的经验是：

1. 先看任务需求——如果只是快速移动，梯形规划就够了。如果要求平滑，至少用S形或5次多项式。
2. 再看硬件能力——控制器算力强，用7次多项式；算力弱，用梯形或3次多项式。
3. 最后看调试成本——梯形和3次多项式参数少，好调。S形和7次多项式参数多，需要花时间。