4. 关节空间轨迹规划：梯形速度规划、S形速度规划、多项式插值（3-5-7次）
关节空间轨迹规划，说白了就是让机器人每个关节从A点转到B点，怎么转得又快又稳。我刚开始做机器人调试那会儿，总觉得这步很简单——给个速度，让电机转过去不就行了？结果第一次试跑，机器人抖得像筛糠，末端执行器直接撞上了夹具。嗯，从那以后我才明白，轨迹规划不是“能不能到”，而是“怎么到”。
这一章我们重点聊三种最常用的规划方法：梯形速度、S形速度，还有多项式插值。这三种方法覆盖了从简单到高精度的绝大多数工业场景。
核心观点：关节空间规划不关心末端走什么路径，只关心每个关节的角度、速度、加速度随时间的变化。你想想看，如果每个关节都平滑了，末端自然就平滑。
4.1 梯形速度规划
梯形速度规划是最基础、最常用的方法。它的速度曲线像梯形——匀加速、匀速、匀减速三段。我习惯叫它“三段式”。
为什么叫梯形？你想想看，速度从0开始线性增加到最大值，保持一段时间，再线性减到0。画出来就是个梯形。简单粗暴，计算量极小。
我的经验：梯形规划适合搬运、码垛这类对末端冲击不敏感的场合。我曾经在一个码垛项目里全程用梯形规划，一天跑几万次，减速机用了两年都没出问题。但如果是焊接或涂胶，梯形规划就不太行了——加速度突变会导致末端抖动，焊缝质量会受影响。
梯形规划的核心参数有三个：
- 最大速度 v_max —— 由电机额定转速和减速比决定
- 最大加速度 a_max —— 由电机转矩和负载惯量决定
- 总位移 θ_total —— 起点到终点的角度差
计算流程其实很简单：
- 先算加速段和减速段需要的位移：θ_acc = v_max² / (2 * a_max)
- 如果 θ_acc * 2 < θ_total，说明能达到最大速度，有匀速段
- 如果 θ_acc * 2 ≥ θ_total，说明到不了最大速度，就是三角形速度曲线
代码实现也很直接：
// 梯形速度规划 - 位置计算
double trapezoidal_plan(double t, double t_total, double v_max, double a_max, double theta_total) {
double t_acc = v_max / a_max; // 加速时间
double theta_acc = 0.5 * a_max * t_acc * t_acc; // 加速段位移
// 判断是否达到最大速度
if (2 * theta_acc < theta_total) {
// 有匀速段
double t_const = (theta_total - 2 * theta_acc) / v_max;
if (t <= t_acc) {
return 0.5 * a_max * t * t; // 加速段
} else if (t <= t_acc + t_const) {
return theta_acc + v_max * (t - t_acc); // 匀速段
} else {
double t_dec = t - t_acc - t_const;
return theta_acc + v_max * t_const
+ v_max * t_dec - 0.5 * a_max * t_dec * t_dec; // 减速段
}
} else {
// 三角形速度曲线，到不了最大速度
double t_half = sqrt(theta_total / a_max);
if (t <= t_half) {
return 0.5 * a_max * t * t;
} else {
double t_dec = t - t_half;
return theta_total - 0.5 * a_max * (t_half - t_dec) * (t_half - t_dec);
}
}
}注意：梯形规划在加速结束和减速开始的时刻，加速度会突变。这会产生冲击力（Jerk无穷大）。我曾经在一个精密装配项目里用梯形规划，结果每次加减速切换时，末端都会抖一下，导致零件装配失败。后来换成S形规划才解决。
4.2 S形速度规划
S形速度规划，说白了就是给梯形规划“圆角”。它把加速度的变化也做了平滑处理，让加速度从0开始逐渐增加到最大值，再逐渐减到0。这样速度曲线就不是直线了，而是S形。
我个人觉得，S形规划是工业机器人最实用的方案。它比梯形平滑，又比高次多项式计算量小。我做过一个统计：在同样的运动时间和位移下，S形规划的末端残余振动比梯形规划低40%以上。
S形规划有7段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。听着复杂，但核心就是控制加加速度（Jerk）。
| 阶段 | 加速度变化 | 速度特征 | Jerk值 |
|---|---|---|---|
| 1. 加加速 | 0 → a_max | 二次曲线上升 | +J |
| 2. 匀加速 | 保持 a_max | 线性上升 | 0 |
| 3. 减加速 | a_max → 0 | 二次曲线上升 | -J |
| 4. 匀速 | 0 | 恒定 | 0 |
| 5. 加减速 | 0 → -a_max | 二次曲线下降 | -J |
| 6. 匀减速 | 保持 -a_max | 线性下降 | 0 |
| 7. 减减速 | -a_max → 0 | 二次曲线下降 | +J |
这里有个关键点：Jerk值J决定了加速度变化的快慢。J越大，越接近梯形规划；J越小，曲线越平滑，但总时间会变长。
避坑指南：我曾经在一个高速分拣项目里把Jerk设得太小，结果加速段拉得很长，还没加速到最大速度就开始减速了，整个运动变成了“三角形S形”，效率反而比梯形还低。后来我总结了一个经验：Jerk值一般取 a_max / (0.1~0.2 * t_total)，这样既平滑又不损失太多效率。
4.3 多项式插值（3-5-7次）
多项式插值，就是用一个多项式函数来描述位置随时间的变化。阶次越高，能约束的条件越多，曲线越平滑。
为什么需要不同阶次？你想想看：
- 3次多项式：约束位置和速度，加速度不连续
- 5次多项式：约束位置、速度、加速度，加加速度不连续
- 7次多项式：约束位置、速度、加速度、加加速度，全部连续
我一般这样选：
- 简单搬运用3次就够了，计算快，占用CPU少
- 焊接、涂胶用5次，加速度连续，末端抖动小
- 高精度加工或光学检测用7次，所有导数都连续，运动最平滑
5次多项式的公式长这样：
θ(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵
约束条件：
θ(0) = θ_start θ(T) = θ_end
θ'(0) = v_start θ'(T) = v_end
θ''(0) = a_start θ''(T) = a_end
系数求解（矩阵形式）：
[1 0 0 0 0 0 ] [a0] [θ_start]
[0 1 0 0 0 0 ] [a1] [v_start]
[0 0 2 0 0 0 ] [a2] [a_start]
[1 T T² T³ T⁴ T⁵ ] [a3] = [θ_end ]
[0 1 2T 3T² 4T³ 5T⁴ ] [a4] [v_end ]
[0 0 2 6T 12T² 20T³ ] [a5] [a_end ]实际经验：多项式插值虽然平滑，但有个坑——龙格现象。阶次太高（比如10次以上），曲线会在端点附近剧烈振荡。我见过有人用15次多项式规划一个长距离运动，结果中间段直接冲过了目标位置30%。所以工业上一般不超过7次，7次已经能保证所有导数连续了。
4.4 三种方法的对比与选型
说了这么多，到底怎么选？我整理了一个对比表，方便你快速决策：
| 特性 | 梯形速度 | S形速度 | 5次多项式 | 7次多项式 |
|---|---|---|---|---|
| 速度连续性 | 连续 | 连续 | 连续 | 连续 |
| 加速度连续性 | 跳变 | 连续 | 连续 | 连续 |
| 加加速度连续性 | 不连续 | 跳变 | 不连续 | 连续 |
| 计算复杂度 | 极低 | 低 | 中 | 高 |
| 运动时间可控性 | 精确 | 精确 | 固定 | 固定 |
| 典型应用 | 搬运、码垛 | 焊接、涂胶 | 精密装配 | 光学检测 |
重要提醒：多项式插值虽然平滑，但运动时间T是固定的。如果你需要实时调整速度（比如根据视觉反馈减速），梯形和S形更方便——它们可以随时修改参数。我做过一个视觉引导的抓取项目，用的就是S形规划，因为视觉反馈会实时调整目标位置，多项式插值根本来不及重新算系数。
最后说一句：没有最好的方法，只有最合适的。梯形快但糙，S形折中，多项式平滑但慢。实际项目中，我经常混合使用——粗定位用梯形，精定位用多项式。你调试的时候，不妨也试试这种思路。