4. 多项式速度规划：三次、五次、七次多项式的推导与对比
速度规划里，多项式曲线是最基础也最常用的工具。说白了，就是用一条多项式函数来描述位置随时间的变化。你想想看，机器人从A点走到B点，中间怎么走？多项式给了我们一个很优雅的数学框架。
我个人习惯把多项式速度规划分成三个档次：三次、五次、七次。档次越高，能约束的边界条件越多，轨迹也越平滑。但代价呢？计算量上去了，而且高阶多项式容易在中间段出现不必要的抖动。今天我们就来把这三种多项式掰开揉碎了讲清楚。
核心观点：多项式阶数 = 可独立约束的边界条件数 - 1。三次多项式能约束4个条件，五次能约束6个，七次能约束8个。选哪个，取决于你的任务需要多少个边界条件。
4.1 三次多项式：最简方案
三次多项式长这样：
q(t) = a₀ + a₁·t + a₂·t² + a₃·t³它有4个未知系数。所以我们能约束4个边界条件。通常我们约束起点和终点的位置和速度：
- q(0) = q₀, q(T) = q_T
- q̇(0) = v₀, q̇(T) = v_T
把边界条件代进去，解一个4元线性方程组，就能得到a₀~a₃。我在项目中遇到过用三次多项式做传送带上的抓取动作，效果还不错。但要注意——三次多项式没法约束加速度。这意味着起点和终点的加速度是跳变的，电机那边会感受到一个冲击。
避坑指南：我曾经在一个码垛项目里直接用三次多项式做关节空间规划，结果机器人末端在拐点处剧烈抖动。后来一查，是加速度不连续导致的。如果你的应用对平滑性要求高，三次多项式可能不够用。
4.2 五次多项式：工业标配
五次多项式增加了两个自由度：
q(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵6个系数，对应6个边界条件。除了位置和速度，我们还能约束起点和终点的加速度：
- q(0) = q₀, q(T) = q_T
- q̇(0) = v₀, q̇(T) = v_T
- q̈(0) = a₀, q̈(T) = a_T
解这个方程组，我建议用矩阵形式写，方便编程实现：
| 1 0 0 0 0 0 | | a₀ | | q₀ |
| 1 T T² T³ T⁴ T⁵ | | a₁ | | q_T |
| 0 1 0 0 0 0 | × | a₂ | = | v₀ |
| 0 1 2T 3T² 4T³ 5T⁴ | | a₃ | | v_T |
| 0 0 2 0 0 0 | | a₄ | | a₀ |
| 0 0 2 6T 12T² 20T³| | a₅ | | a_T |嗯，这里要注意：矩阵求逆在嵌入式平台上可能有点慢。我一般会预先把解析解算好，写成公式直接代入。五次多项式的解析解其实很规整，网上也能查到现成的系数公式。
个人经验：五次多项式是工业机器人最常用的速度规划方式。它保证了加速度连续，电机不会受到冲击。我做过一个焊接机器人项目，用五次多项式做轨迹平滑，焊道质量明显比三次多项式好一个档次。
4.3 七次多项式：追求极致平滑
七次多项式有8个系数：
q(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵ + a₆·t⁶ + a₇·t⁷除了位置、速度、加速度，我们还能约束加加速度（Jerk）：
- q(0) = q₀, q(T) = q_T
- q̇(0) = v₀, q̇(T) = v_T
- q̈(0) = a₀, q̈(T) = a_T
- q̇̈(0) = j₀, q̇̈(T) = j_T
七次多项式能保证Jerk连续。Jerk是什么？就是加速度的变化率。Jerk不连续，意味着加速度会突变，体现在物理上就是力会突变。对于精密装配、医疗机器人这类场景，Jerk连续是硬要求。
关键理解：三次多项式保证位置和速度连续；五次多项式额外保证加速度连续；七次多项式再额外保证Jerk连续。每提高两次阶数，就能多保证一阶导数的连续性。
4.4 三种多项式对比
| 特性 | 三次多项式 | 五次多项式 | 七次多项式 |
|---|---|---|---|
| 系数个数 | 4 | 6 | 8 |
| 可约束边界条件 | 位置、速度 | 位置、速度、加速度 | 位置、速度、加速度、Jerk |
| 加速度连续性 | 不连续 | 连续 | 连续 |
| Jerk连续性 | 不连续 | 不连续 | 连续 |
| 计算复杂度 | 低 | 中 | 高 |
| 适用场景 | 简单点对点、低精度 | 工业机器人、通用轨迹 | 精密装配、医疗、高速高精度 |
4.5 最小化Jerk的优化目标
为什么要最小化Jerk？你想想看，Jerk大意味着加速度变化剧烈，力变化剧烈。这会导致：
- 机器人结构产生振动
- 电机电流波动大，发热严重
- 末端执行器定位精度下降
- 机械寿命缩短
最小化Jerk的优化目标函数通常写成：
min J = ∫₀ᵀ (q̇̈(t))² dt也就是让整个运动过程中Jerk的平方积分最小。有意思的是，在只约束位置和速度边界条件的情况下，最小化Jerk的最优解恰好就是五次多项式。这可不是巧合——五次多项式在数学上就是Jerk最小的轨迹。
深入一点：如果你约束了位置、速度、加速度，再最小化Jerk的导数（也就是Snap），得到的就是七次多项式。这个规律可以一直推下去。我当年做博士论文时推导过这个结论，挺有意思的。
4.6 知识体系总览
下面这张图把三种多项式的关系和选择逻辑梳理清楚了：
4.7 实际应用建议
说了这么多理论，最后给几条实在的建议：
- 能用五次就别用三次。 五次多项式计算量只比三次多一点点，但平滑性提升明显。除非你的控制器算力实在捉急，否则我建议默认选五次。
- 七次多项式要慎用。 不是所有场景都需要Jerk连续。我曾经在一个高速分拣项目里用了七次多项式，结果轨迹中间段出现了不必要的波动——高阶多项式容易在约束点之间"乱晃"。后来改回五次，效果反而更好。
- 边界条件要合理。 如果你给的速度或加速度边界条件本身就不合理（比如要求1秒内从0加速到10m/s），再高阶的多项式也救不了你。先检查物理可行性。
- 考虑分段多项式。 如果整个轨迹很长，不要用一段高阶多项式去拟合。分段用低阶多项式拼接，效果往往更好。这个我们后面章节会详细讲。
最后提醒：多项式速度规划不是万能的。当轨迹需要经过多个路径点时，单纯用一段多项式就不够了。这时候需要用到样条曲线或者梯形速度规划。别把多项式当锤子，看什么都像钉子。