3. 多项式轨迹规划：三次多项式插值、五次多项式插值、梯形速度规划、S形速度规划
轨迹规划，说白了就是解决「怎么从A点走到B点」的问题。但实际工程里，光走通可不够——你得考虑电机能不能跟上、末端会不会抖、加速度会不会把工件甩出去。这一章我们聊的几种方法，就是解决这些问题的基本功。
我个人习惯把轨迹规划分成两类：多项式插值和速度规划。前者关注位置曲线的平滑性，后者关注运动过程的物理约束。咱们一个一个来。
核心观点：三次多项式保证位置和速度连续，五次多项式额外保证加速度连续。梯形速度规划简单高效，S形速度规划则能彻底消除加加速度冲击。选哪个，取决于你的应用场景对平滑性的要求有多高。
3.1 三次多项式插值
三次多项式是最简单的连续轨迹形式。它的数学表达式是：
q(t) = a₀ + a₁·t + a₂·t² + a₃·t³四个未知系数，需要四个边界条件。通常我们给定起点和终点的位置与速度：
q(0) = q₀, q(T) = q_f
q̇(0) = v₀, q̇(T) = v_f解这个方程组，系数就出来了。我在项目中遇到过不少新手直接手算这个，其实写成矩阵形式更清晰：
| 1 0 0 0 | | a₀ | | q₀ |
| 1 T T² T³ | | a₁ | = | q_f |
| 0 1 0 0 | | a₂ | | v₀ |
| 0 1 2T 3T² | | a₃ | | v_f |解出来之后，位置、速度、加速度曲线都是连续的。但加速度曲线是直线——这意味着加速度在起点和终点会有跳变。嗯，这里要注意：如果电机对加速度突变敏感，三次多项式就不够用了。
工程小技巧：如果你只需要点到点的移动，且起止速度为零，三次多项式的加速度最大值出现在t=0或t=T处。可以用这个特性快速估算电机峰值扭矩需求。
3.2 五次多项式插值
五次多项式比三次多两个自由度：
q(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵六个系数，对应六个边界条件——位置、速度、加速度在起点和终点都给定：
q(0)=q₀, q(T)=q_f
q̇(0)=v₀, q̇(T)=v_f
q̈(0)=a₀, q̈(T)=a_f解这个方程组稍微麻烦一点，但好处很明显：加速度连续，没有跳变。我曾经在一个精密装配项目里用过五次多项式——那个场景下，加速度突变会导致零件表面划伤，换成五次多项式之后问题就解决了。
系数公式可以直接写出来（假设起止速度、加速度都为零）：
a₀ = q₀
a₁ = 0
a₂ = 0
a₃ = 10·(q_f - q₀) / T³
a₄ = -15·(q_f - q₀) / T⁴
a₅ = 6·(q_f - q₀) / T⁵你想想看，这个公式多漂亮——只需要知道位移和时间，就能直接写出轨迹。
避坑指南：我曾经在一个六轴机器人上同时用五次多项式规划六个关节，结果末端轨迹在笛卡尔空间里出现了「绕远路」的现象。原因很简单：关节空间的平滑不代表笛卡尔空间的平滑。如果你需要末端走直线，得在笛卡尔空间做规划，或者用逆解做中间点插值。
3.3 梯形速度规划
梯形速度规划是工程中最常用的方法之一。它的速度曲线像个梯形——匀加速、匀速、匀减速三段。
为什么叫梯形？你看速度曲线形状就知道了：
速度 v
^
| _________
| / \
| / \
| / \
| / \
+-------------------> 时间 t
0 t₁ t₂ t₃三段的时间分配取决于三个参数：最大速度v_max、最大加速度a_max、总位移S。如果位移足够大，三段都能完整出现；如果位移太小，可能只有加速和减速段（三角形速度曲线）。
我习惯先判断能不能达到最大速度：
如果 S > v_max² / a_max，则能达到最大速度（梯形）
否则，达不到最大速度（三角形）梯形规划的优点是计算量小、实现简单。缺点也很明显——加速度在拐点处跳变。你想想看，电机在加速段结束时突然切换到匀速段，加速度从a_max瞬间降到0，这个冲击虽然比三次多项式小，但对精密设备来说还是不够好。
适用场景：梯形速度规划适合大多数工业搬运、码垛、点焊等对轨迹平滑性要求不高的场景。我见过很多AGV小车也用梯形规划，跑起来没问题，就是启停时货物会晃一下。
3.4 S形速度规划
S形速度规划是梯形规划的升级版。它把加速度也做了平滑处理——加速度本身也是连续变化的，不再是阶跃信号。
S形规划通常有七个阶段：
- 加加速段：加速度从0增加到a_max
- 匀加速段：加速度保持a_max
- 减加速段：加速度从a_max减小到0
- 匀速段：速度保持v_max
- 加减速段：加速度从0减小到-a_max
- 匀减速段：加速度保持-a_max
- 减减速段：加速度从-a_max增加到0
说白了，就是把梯形规划的加速度跳变点，用一段斜坡过渡。这样加加速度（jerk）就是有限值，不会出现无穷大的冲击。
S形规划的数学推导比梯形复杂得多。你需要同时满足位置、速度、加速度、加加速度四个约束。我一般在代码里用查表法或者数值积分来实现，很少手推解析解——除非位移特别小，必须精确计算每个阶段的持续时间。
工程实现建议：如果你用的是实时系统，S形规划的在线计算量可能偏大。我常用的做法是离线生成轨迹表，运行时直接查表插值。这样既保证了平滑性，又满足了实时性要求。
3.5 四种方法的对比与选型
四种方法各有优劣，我整理了一个对比表，方便你选型时参考：
| 方法 | 位置连续 | 速度连续 | 加速度连续 | 加加速度有限 | 计算量 | 典型应用 |
|---|---|---|---|---|---|---|
| 三次多项式 | ✓ | ✓ | ✗ | ✗ | 低 | 简单点到点 |
| 五次多项式 | ✓ | ✓ | ✓ | ✗ | 中 | 精密装配 |
| 梯形速度 | ✓ | ✓ | ✗ | ✗ | 极低 | 工业搬运 |
| S形速度 | ✓ | ✓ | ✓ | ✓ | 高 | 高精度加工 |
选型时我一般遵循三条原则：
- 追求简单可靠：选梯形速度规划。80%的工业场景够用。
- 需要加速度连续：选五次多项式或S形。前者适合短距离，后者适合长距离。
- 对冲击极度敏感：必须上S形。比如晶圆搬运、医疗设备、精密光学加工。
避坑指南：我曾经在一个项目里用S形规划做长距离移动，结果因为加加速度限制太严格，导致运动时间比梯形规划长了将近一倍。后来我加了一个自适应策略——位移大时放宽加加速度限制，位移小时收紧。这样既保证了平滑性，又没牺牲效率。
3.6 知识体系总览
下面这张图总结了本章的核心知识结构，帮你理清思路：
这张图把四种方法的关系和特性都串起来了。你可以把它当作选型时的快速参考——从根节点出发，根据你的需求选择分支，再看底部的特性说明做最终决定。
好了，这一章的内容就到这里。多项式插值和速度规划是轨迹规划的基础，后面的章节会在这个基础上继续深入——比如多段轨迹的衔接、笛卡尔空间的规划、以及带约束的优化方法。这些内容，我们后面慢慢聊。