📚 课程目录第3章运动规划算法

3. 运动规划算法：梯形速度曲线、S形速度曲线、T型曲线、多项式插值、样条插值

运动规划，说白了就是让机器人知道「怎么动」。不是简单的从A到B，而是要考虑速度、加速度、冲击度，甚至路径的光滑程度。我做了十几年运动控制，见过太多因为规划不当导致的抖动、过冲、甚至撞机事故。这一章，咱们就把几种最核心的算法掰开揉碎了讲清楚。

  核心观点： 没有万能的规划算法。梯形简单粗暴，S形平滑舒适，多项式灵活多变，样条则适合复杂路径。选对算法，项目就成功了一半。

3.1 梯形速度曲线

梯形速度曲线，也叫Trapezoidal Profile。它把运动分成三段：匀加速、匀速、匀减速。速度曲线看起来像个梯形，因此得名。

为什么它这么流行？因为简单。我早期做的一个码垛项目，用的就是梯形曲线。PLC里写几行代码就能实现，计算量小，响应快。

适用场景： 对启停冲击不敏感、负载较轻、速度不高的场合。比如小型传送带、简易搬运、气动辅助运动。

但梯形曲线有个硬伤——加速度在拐点处突变。启动瞬间加速度从0跳到最大值，停止时又瞬间归零。这会产生冲击力，俗称「急动」。负载重了，机构会抖，甚至把工件甩出去。

我曾经在一个玻璃搬运项目里吃过这个亏。梯形曲线启动时，玻璃直接滑落。后来换成S形曲线才解决。

3.2 S形速度曲线

S形曲线，说白了就是给梯形曲线「圆角」。它在加速和减速阶段，让加速度也平滑变化，而不是突变。速度曲线看起来像字母S，所以叫S形。

S形曲线通常分七段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。听着复杂，但核心思想就一个——让加速度的变化率（Jerk，冲击度）可控。

特性	梯形曲线	S形曲线
加速度连续性	不连续（阶跃变化）	连续（线性变化）
冲击度（Jerk）	无穷大（理论）	有限值（可控）
运动时间	最短	略长（约5%~15%）
机械冲击	大	小
实现复杂度	低	中

我个人的习惯是：凡是负载超过额定50%、或者有精密定位要求的场合，一律用S形曲线。虽然运动周期长一点点，但设备寿命和定位精度都上去了。

注意： S形曲线不是万能的。如果Jerk设置过大，它和梯形曲线没区别；如果Jerk设置过小，运动时间会显著增加。需要根据实际负载和工艺节拍来调参。

3.3 T型曲线

T型曲线，其实和梯形曲线是一回事。只是叫法不同。有些资料叫Trapezoidal，有些叫T型。我习惯把「T型」特指一种简化实现——只有加速和减速两段，没有匀速段。

什么时候用T型？短距离点动、回零、微调。比如伺服电机的JOG模式，就是典型的T型曲线。启动就加速，还没到匀速就得减速，干脆省掉匀速段。

嗯，这里要注意：T型曲线因为没有匀速段，峰值速度可能达不到设定值。如果你需要精确控制平均速度，还是用完整的梯形曲线更靠谱。

3.4 多项式插值

多项式插值，是用多项式函数来拟合位置-时间曲线。最常见的是三次多项式和五次多项式。

三次多项式： 可以约束起点和终点的位置、速度。四个约束条件，四个系数，刚好解出来。适合简单的点到点运动。

五次多项式： 可以额外约束起点和终点的加速度。六个约束条件，六个系数。适合需要精确控制加加速度的场合。

// 五次多项式位置公式（C语言风格伪代码）
// t: 归一化时间 [0, 1]
// p0: 起始位置, v0: 起始速度, a0: 起始加速度
// p1: 终点位置, v1: 终点速度, a1: 终点加速度

double t2 = t * t;
double t3 = t2 * t;
double t4 = t3 * t;
double t5 = t4 * t;

double h1 = 1 - 10*t3 + 15*t4 - 6*t5;
double h2 = t - 6*t3 + 8*t4 - 3*t5;
double h3 = 0.5*t2 - 1.5*t3 + 1.5*t4 - 0.5*t5;
double h4 = 10*t3 - 15*t4 + 6*t5;
double h5 = -4*t3 + 7*t4 - 3*t5;
double h6 = 0.5*t3 - t4 + 0.5*t5;

double pos = p0*h1 + v0*h2 + a0*h3 + p1*h4 + v1*h5 + a1*h6;

多项式插值的优点是灵活。你可以任意指定起点和终点的状态。但缺点是：阶数高了容易振荡，尤其是高阶多项式在端点附近可能产生大幅超调。我见过有人用七次多项式做轨迹，结果中间段直接飞出去了。

我的建议： 一般工程应用，五次多项式足够了。除非你有特殊约束（比如加加速度的导数也要连续），否则别轻易上高阶。

3.5 样条插值

样条插值，是把整个路径分成多段，每段用低阶多项式（通常是三次）拼接起来。拼接处保证位置、速度、加速度连续。这样既避免了高阶多项式的振荡，又能实现全局光滑。

样条插值在工业机器人里用得特别多。比如焊接、涂胶、曲面加工——这些路径不是简单的点到点，而是需要沿着复杂曲线运动。

常见的样条类型：

三次样条（Cubic Spline）： 每段三次多项式，二阶连续。最常用。
B样条（B-Spline）： 支持局部修改，不改变全局。适合交互式调整。
NURBS： 非均匀有理B样条。可以精确表示圆锥曲线，高端数控和机器人必备。

我记得有一次做机器人打磨项目，工件表面是自由曲面。用三次样条插值，每段路径都光滑连接，打磨出来的表面几乎没有波纹。如果用梯形曲线分段走，那表面就是一道一道的沟壑。

避坑指南： 样条插值虽然好，但计算量大。尤其是实时插补时，CPU负载会明显上升。我曾经在低端PLC上跑样条插值，结果插补周期从1ms拖到了5ms，电机直接抖成筛子。后来换了带浮点运算单元的控制器才搞定。

3.6 如何选择？一张表说清楚

应用场景	推荐算法	理由
简单点到点，负载轻	梯形/T型曲线	实现简单，计算快
点到点，负载重或精度高	S形曲线	冲击小，定位稳
多段连续轨迹，速度可控	五次多项式	灵活约束起止状态
复杂曲面、自由路径	三次样条/B样条	全局光滑，局部可调
高速高精数控加工	NURBS	精确表示复杂曲线

最后说一句：实际项目中，很少只用一种算法。我常用的套路是：路径用样条插值生成位置点，速度用S形曲线规划。两者结合，既保证了路径光滑，又控制了冲击。这才是工程实战的思维。