4. 多项式轨迹规划：三次、五次、七次插值原理与实现
轨迹规划里最基础、也最常用的方法，就是多项式插值。说白了，就是给机器人每个关节的运动，找一条光滑的曲线。你想想看，电机不能瞬间从0转到1000转，中间必须有个平滑的过渡。多项式就是干这个的。
我个人习惯把多项式轨迹分成三档：三次、五次、七次。档位越高，能约束的条件越多，轨迹越平滑，但计算量也上去了。咱们一个一个聊。
核心思想：用多项式函数拟合关节位置随时间的变化。约束条件越多，多项式阶数越高。
4.1 三次多项式插值
三次多项式是最简单的。它只有4个系数，所以只能约束4个条件。哪4个？起始位置、终止位置、起始速度、终止速度。
公式长这样：
q(t) = a0 + a1*t + a2*t² + a3*t³它的速度是：
v(t) = a1 + 2*a2*t + 3*a3*t²我们设起始时间 t=0，终止时间 t=T。已知 q(0)=q0, q(T)=qf, v(0)=v0, v(T)=vf。代入求解，就能得到四个系数。
我在项目里遇到过一个问题：用三次多项式做点位运动，中间速度曲线是抛物线形状。如果起始和终止速度都是0，那速度会在中间达到峰值，然后降回来。这个峰值有多大？跟位移和时间有关。你想想看，如果位移很大，时间很短，速度峰值就会很高，可能超出电机额定转速。
避坑指南：我曾经在六轴机器人上做点位运动，用三次多项式规划，结果关节4的速度超限了。后来加了速度校验，如果峰值超限，就拉长时间或者分段规划。三次多项式没法控制加速度，这是它的硬伤。
三次多项式的系数求解，其实就是解一个4元线性方程组。我直接给代码：
def cubic_trajectory(q0, qf, v0, vf, T):
"""
三次多项式轨迹规划
q0: 起始位置
qf: 终止位置
v0: 起始速度
vf: 终止速度
T: 总时间
"""
a0 = q0
a1 = v0
a2 = (3*(qf - q0) - (2*v0 + vf)*T) / (T**2)
a3 = (2*(q0 - qf) + (v0 + vf)*T) / (T**3)
return a0, a1, a2, a3
# 使用示例
a0, a1, a2, a3 = cubic_trajectory(0, 90, 0, 0, 3)
print(f"系数: a0={a0:.3f}, a1={a1:.3f}, a2={a2:.3f}, a3={a3:.3f}")4.2 五次多项式插值
三次多项式管不了加速度，那怎么办？上五次多项式。它有6个系数，可以约束位置、速度、加速度，一共6个条件。
公式：
q(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵加速度：
a(t) = 2*a2 + 6*a3*t + 12*a4*t² + 20*a5*t³约束条件多了起始和终止加速度。通常我们设起始和终止加速度为0，这样电机启动和停止时不会产生冲击。
我建议在大多数工业场景里，直接用五次多项式。为什么？因为三次多项式在启停瞬间加速度是跳变的，会产生冲击。五次多项式保证了加速度连续，对机械臂的寿命有好处。
| 对比项 | 三次多项式 | 五次多项式 | 七次多项式 |
|---|---|---|---|
| 约束条件数 | 4 | 6 | 8 |
| 可约束量 | 位置、速度 | 位置、速度、加速度 | 位置、速度、加速度、加加速度 |
| 加速度连续性 | 不连续（阶跃） | 连续 | 连续且平滑 |
| 计算复杂度 | 低 | 中 | 高 |
| 适用场景 | 简单点位、速度不敏感 | 大多数工业场景 | 高精度、高速、低振动 |
五次多项式的系数求解，我直接给代码：
def quintic_trajectory(q0, qf, v0, vf, a0, af, T):
"""
五次多项式轨迹规划
a0: 起始加速度
af: 终止加速度
"""
# 中间变量
delta_q = qf - q0
a0_coeff = q0
a1_coeff = v0
a2_coeff = a0 / 2.0
# 解3x3线性方程组
T2 = T * T
T3 = T2 * T
T4 = T3 * T
T5 = T4 * T
a3_coeff = (20*delta_q - (8*vf + 12*v0)*T - (3*a0 - af)*T2) / (2*T3)
a4_coeff = (30*delta_q + (14*vf + 16*v0)*T + (3*a0 - 2*af)*T2) / (2*T4)
a5_coeff = (12*delta_q - (6*vf + 6*v0)*T - (a0 - af)*T2) / (2*T5)
return a0_coeff, a1_coeff, a2_coeff, a3_coeff, a4_coeff, a5_coeff注意：五次多项式的系数计算涉及T的高次幂。如果T很小（比如0.1秒），T⁵会非常小，导致数值不稳定。我建议T不要小于0.2秒，或者改用归一化时间。
4.3 七次多项式插值
七次多项式，8个系数，能约束加加速度（Jerk）。加加速度是什么？就是加速度的变化率。如果加加速度不连续，加速度曲线会有尖点，电机输出力矩会突变，产生振动。
公式：
q(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵ + a6*t⁶ + a7*t⁷加加速度：
j(t) = 6*a3 + 24*a4*t + 60*a5*t² + 120*a6*t³ + 210*a7*t⁴约束条件：起始和终止的位置、速度、加速度、加加速度，一共8个。
我记得有一次做高速分拣机器人，用五次多项式规划，末端振动很大。后来换成七次多项式，加加速度连续了，振动明显减小。嗯，这里要注意：七次多项式不是万能的，它只是让加加速度连续，但加加速度的峰值可能更大。
七次多项式的系数求解，我建议用矩阵形式，代码更清晰：
import numpy as np
def septic_trajectory(q0, qf, v0, vf, a0, af, j0, jf, T):
"""
七次多项式轨迹规划
j0: 起始加加速度
jf: 终止加加速度
"""
# 构建约束矩阵
# 这里用归一化时间 tau = t/T，避免数值问题
A = np.array([
[1, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 2, 0, 0, 0, 0, 0],
[0, 0, 0, 6, 0, 0, 0, 0],
[1, 1, 1, 1, 1, 1, 1, 1],
[0, 1, 2, 3, 4, 5, 6, 7],
[0, 0, 2, 6, 12, 20, 30, 42],
[0, 0, 0, 6, 24, 60, 120, 210]
])
b = np.array([q0, v0*T, a0*T**2, j0*T**3,
qf, vf*T, af*T**2, jf*T**3])
coeff = np.linalg.solve(A, b)
return coeff经验之谈：七次多项式在T较小时，矩阵条件数会变大。我习惯用归一化时间 tau = t/T，把时间缩放到[0,1]区间。这样矩阵元素都在同一量级，数值稳定性好很多。
4.4 三种多项式的选择策略
怎么选？我总结了一个简单规则：
- 三次多项式：只做简单点位运动，对速度没要求，或者后续有速度规划器做二次处理。别用在高速场景。
- 五次多项式：90%的工业场景够用。加速度连续，冲击小，计算量适中。我个人的默认选择。
- 七次多项式：高速高精度场景，或者对振动敏感的设备（比如精密装配、医疗机器人）。
你想想看，如果只是让机器人从A点走到B点，中间不关心怎么走，三次多项式就够了。但如果要保证末端执行器不抖动，五次是底线。七次嘛，属于锦上添花。
4.5 工程实现中的注意事项
最后聊几个实际工程里容易踩的坑。
第一，时间归一化。我建议所有多项式都用归一化时间 tau = t/T 来计算。这样系数都在同一量级，不会因为T太小导致数值问题。
第二，边界条件校验。我曾经遇到过，起始速度和终止速度设得太大，结果多项式算出来的轨迹中间位置超出了关节限位。所以算完系数后，一定要采样检查位置、速度、加速度是否在安全范围内。
第三，多段轨迹拼接。如果一段轨迹不够，需要分段规划。比如从A到B再到C。这时候要保证段与段之间的位置、速度、加速度连续。我习惯用五次多项式做段间过渡，保证二阶连续。
第四，实时性。三次和五次多项式计算很快，微秒级。七次多项式涉及矩阵求逆，在嵌入式平台上可能耗时几十微秒。如果控制周期是1毫秒，问题不大。但如果控制周期只有100微秒，就要小心了。
重要提醒：多项式轨迹规划只是生成了一条光滑的曲线，但实际执行时还要考虑电机力矩限制、关节摩擦、动力学耦合等因素。多项式规划出来的轨迹，如果加速度太大，电机可能跟丢。所以规划完一定要做动力学校验。
好了，多项式轨迹规划就聊到这儿。三种多项式各有适用场景，我个人推荐默认用五次，特殊场景再考虑三次或七次。记住一点：轨迹规划的目标不是让曲线多漂亮，而是让机器人跑得稳、跑得快、不抖动。