4. 多项式速度规划：三次多项式与五次多项式
速度规划里，多项式曲线是最经典的工具之一。说白了，就是用数学公式把速度变化描述成一条光滑的曲线。我个人习惯把多项式规划比作「搭桥」——从起点速度到终点速度，中间怎么过渡，全看多项式阶数怎么选。
今天咱们重点聊两种：三次多项式和五次多项式。它们看起来只是阶数差了两阶，但实际用起来差别很大。我在项目里踩过不少坑，今天一并讲清楚。
4.1 三次多项式：最简单的平滑过渡
三次多项式的形式很简单：
s(t) = a₀ + a₁·t + a₂·t² + a₃·t³这里 s(t) 是位置，t 是时间。它有 4 个未知系数，所以需要 4 个边界条件来求解。通常我们给定起点和终点的位置与速度：
| 边界条件 | 数学表达 |
|---|---|
| 起点位置 | s(0) = s₀ |
| 终点位置 | s(T) = s₁ |
| 起点速度 | v(0) = v₀ |
| 终点速度 | v(T) = v₁ |
有了这 4 个方程，就能唯一解出 a₀~a₃。你想想看，这就像用一根弹簧连接两个点，两端不仅位置固定，连倾斜角度也固定了——这就是三次多项式的本质。
4.2 五次多项式：更柔顺的进阶选择
当三次多项式不够用时——比如你要求加速度也必须连续——那就得上五次多项式了：
s(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵6 个未知系数，需要 6 个边界条件。除了位置和速度，我们还要指定起点和终点的加速度：
| 边界条件 | 数学表达 |
|---|---|
| 起点位置 | s(0) = s₀ |
| 终点位置 | s(T) = s₁ |
| 起点速度 | v(0) = v₀ |
| 终点速度 | v(T) = v₁ |
| 起点加速度 | a(0) = a₀ |
| 终点加速度 | a(T) = a₁ |
多出来的两个条件，让加速度也能平滑变化。嗯，这里要注意：五次多项式虽然更柔顺，但计算量也更大，而且对边界条件更敏感。
4.3 连续性约束：为什么不能只看边界？
边界条件保证的是「端点」连续，但中间过程呢？如果你把多个多项式段拼接起来，就必须考虑段与段之间的连续性约束。
连续性通常分三个等级：
- C⁰ 连续：位置连续。这是最基本的，轨迹不能有断点。
- C¹ 连续：速度连续。运动不会出现「急停急起」。
- C² 连续：加速度连续。力不会突变，机械结构不会受冲击。
我曾经在一个码垛机器人项目里，用了三段三次多项式拼接。结果在段间加速度突变，导致末端抖动，抓取精度直接掉了 0.5mm。后来换成五次多项式，问题就解决了。
4.4 代码实现与对比
下面是我常用的 Python 实现。先看三次多项式：
import numpy as np
def cubic_trajectory(s0, s1, v0, v1, T, t):
"""
三次多项式轨迹规划
s0, s1: 起点/终点位置
v0, v1: 起点/终点速度
T: 总时间
t: 当前时间 (0 <= t <= T)
"""
# 构建系数矩阵
A = np.array([
[1, 0, 0, 0],
[1, T, T**2, T**3],
[0, 1, 0, 0],
[0, 1, 2*T, 3*T**2]
])
b = np.array([s0, s1, v0, v1])
a = np.linalg.solve(A, b)
# 计算当前时刻的位置、速度、加速度
s = a[0] + a[1]*t + a[2]*t**2 + a[3]*t**3
v = a[1] + 2*a[2]*t + 3*a[3]*t**2
acc = 2*a[2] + 6*a[3]*t
return s, v, acc再看五次多项式：
def quintic_trajectory(s0, s1, v0, v1, a0, a1, T, t):
"""
五次多项式轨迹规划
比三次多项式多了起点/终点加速度约束
"""
A = np.array([
[1, 0, 0, 0, 0, 0],
[1, T, T**2, T**3, T**4, T**5],
[0, 1, 0, 0, 0, 0],
[0, 1, 2*T, 3*T**2, 4*T**3, 5*T**4],
[0, 0, 2, 0, 0, 0],
[0, 0, 2, 6*T, 12*T**2, 20*T**3]
])
b = np.array([s0, s1, v0, v1, a0, a1])
a = np.linalg.solve(A, b)
s = a[0] + a[1]*t + a[2]*t**2 + a[3]*t**3 + a[4]*t**4 + a[5]*t**5
v = a[1] + 2*a[2]*t + 3*a[3]*t**2 + 4*a[4]*t**3 + 5*a[5]*t**4
acc = 2*a[2] + 6*a[3]*t + 12*a[4]*t**2 + 20*a[5]*t**3
return s, v, acc4.5 两种方法的对比总结
| 对比项 | 三次多项式 | 五次多项式 |
|---|---|---|
| 未知系数个数 | 4 | 6 |
| 边界条件 | 位置、速度 | 位置、速度、加速度 |
| 加速度连续性 | 线性变化，不保证连续 | 二次变化，保证连续 |
| 计算复杂度 | 低 | 中 |
| 适用场景 | 对加速度不敏感、简单点到点运动 | 高精度、多段拼接、对冲击敏感的系统 |
我个人习惯这样选：如果只是做简单的传送带同步，三次多项式就够了。但如果是做 CNC 插补或者机器人轨迹，我闭着眼睛选五次——省心。
4.6 知识体系总览
下面这张图把本章的核心逻辑串起来了：
从这张图可以看得很清楚：三次和五次的核心差异在于「加速度连续性」。你想想看，如果你的系统对力冲击敏感——比如抓取易碎品——那五次多项式几乎是唯一选择。