6、多项式插值：三次多项式、五次多项式轨迹规划，边界条件与求解

好，咱们今天聊聊多项式插值。说实话，这是轨迹规划里最基础、也最实用的方法之一。我刚开始做机器人控制那会儿，第一个上手的项目就是拿三次多项式做关节空间的点到点运动。那时候觉得这东西简单，不就是解个方程组嘛。后来踩的坑多了，才发现里面的门道还真不少。

6.1 为什么需要多项式轨迹？

你想想看，让一个电机从A点转到B点，最简单的办法是什么？直接给个阶跃信号？不行，那会带来巨大的冲击。给个梯形速度曲线？可以，但加速度不连续，末端会抖。

说白了，我们需要一条平滑的曲线。平滑意味着位置连续、速度连续，最好加速度也连续。多项式天然就是光滑函数，而且求导容易，计算量小。这就是它被广泛使用的原因。

我在项目中遇到过一个问题：用梯形速度规划时，机器人末端在拐点处有明显的振动。后来换成五次多项式，问题就解决了。嗯，这里要注意，不是所有场合都需要高阶多项式，关键看你的约束条件有多少。

6.2 三次多项式轨迹规划

三次多项式是最简单的连续轨迹形式。它的表达式长这样：

θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³

这里有四个未知系数：a₀、a₁、a₂、a₃。要解出它们，我们需要四个边界条件。

6.2.1 边界条件怎么定？

典型的点到点运动，我们通常知道起点和终点的位置和速度。假设：

• 起始时刻 t=0：位置 θ(0)=θ₀，速度 θ̇(0)=v₀
• 终止时刻 t=T：位置 θ(T)=θ₁，速度 θ̇(T)=v₁

把这四个条件代进去，就能列出一个方程组。我习惯写成矩阵形式：

θ₀ = a₀
v₀ = a₁
θ₁ = a₀ + a₁·T + a₂·T² + a₃·T³
v₁ = a₁ + 2·a₂·T + 3·a₃·T²

解这个方程组，得到：

a₀ = θ₀
a₁ = v₀
a₂ = (3(θ₁-θ₀)/T²) - (2v₀+v₁)/T
a₃ = (2(θ₀-θ₁)/T³) + (v₀+v₁)/T²

如果起点和终点速度都为零（这是最常见的情况），公式会简化很多：

a₀ = θ₀
a₁ = 0
a₂ = 3(θ₁-θ₀)/T²
a₃ = -2(θ₁-θ₀)/T³

6.2.2 三次多项式的局限

三次多项式有个硬伤：加速度是线性的，在起点和终点处加速度不连续。这意味着什么？意味着电机在启动和停止的瞬间会有冲击。

我曾经在一个高速分拣项目上吃过这个亏。用三次多项式规划，机器人跑起来倒是挺快，但每次启停时末端都会抖一下。后来一测加速度曲线，果然在边界处有跳变。

6.3 五次多项式轨迹规划

五次多项式比三次多了两个自由度。它的形式是：

θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵

六个未知数，需要六个边界条件。除了位置和速度，我们还可以约束加速度。

6.3.1 完整的边界条件

典型的边界条件设置如下：

时刻	位置	速度	加速度
t=0	θ₀	v₀	α₀
t=T	θ₁	v₁	α₁

把六个条件代入五次多项式及其一阶、二阶导数，得到方程组。我直接给出解吧，省得大家手算：

a₀ = θ₀
a₁ = v₀
a₂ = α₀/2
a₃ = (20(θ₁-θ₀) - (8v₁+12v₀)T - (3α₀-α₁)T²) / (2T³)
a₄ = (30(θ₀-θ₁) + (14v₁+16v₀)T + (3α₀-2α₁)T²) / (2T⁴)
a₅ = (12(θ₁-θ₀) - (6v₁+6v₀)T - (α₀-α₁)T²) / (2T⁵)

如果起点和终点的速度、加速度都为零，公式会简化成：

a₀ = θ₀
a₁ = 0
a₂ = 0
a₃ = 10(θ₁-θ₀)/T³
a₄ = -15(θ₁-θ₀)/T⁴
a₅ = 6(θ₁-θ₀)/T⁵

6.4 三次 vs 五次：怎么选？

我个人的经验是这么判断的：

• 选三次的情况： 对速度要求不高，或者控制器算力有限。比如一些简单的物料搬运、码垛，三次完全够用。
• 选五次的情况： 精密操作、高速运动、或者对振动敏感的场景。比如焊接、涂胶、3D打印。

另外还有个折中方案：用三次多项式，但在起点和终点加一段S形加减速。不过说实话，这还不如直接用五次来得干净利落。

6.5 代码实现示例

下面是我常用的一个五次多项式计算函数。C语言风格，可以直接移植到嵌入式系统上：

// 计算五次多项式系数
// 输入：起始位置q0, 终点位置q1, 运动时间T
// 输出：系数数组coeff[6]
void calcQuinticCoeff(float q0, float q1, float T, float coeff[6]) {
    float T2 = T * T;
    float T3 = T2 * T;
    float T4 = T3 * T;
    float T5 = T4 * T;
    
    // 假设起点终点速度加速度均为0
    coeff[0] = q0;
    coeff[1] = 0.0f;
    coeff[2] = 0.0f;
    coeff[3] = 10.0f * (q1 - q0) / T3;
    coeff[4] = -15.0f * (q1 - q0) / T4;
    coeff[5] = 6.0f * (q1 - q0) / T5;
}

// 根据系数计算t时刻的位置
float calcQuinticPos(float coeff[6], float t) {
    return coeff[0] + coeff[1]*t + coeff[2]*t*t + 
           coeff[3]*t*t*t + coeff[4]*t*t*t*t + 
           coeff[5]*t*t*t*t*t;
}

6.6 实际项目中的注意事项

最后聊几个我在项目中踩过的坑：

1. 时间T的选择： T太小，速度加速度会很大，可能超出电机能力。T太大，效率低。我一般先根据最大速度和最大加速度反推一个最小T，再留20%的余量。
2. 数值稳定性： 当T很小时，T³、T⁵这些项会变得非常小，导致系数计算时出现浮点下溢。解决办法是用归一化时间 τ = t/T，把时间缩放到[0,1]区间。
3. 多段轨迹衔接： 如果有多段轨迹，要保证衔接点的位置、速度、加速度连续。用五次多项式时，把上一段的终点速度、加速度作为下一段的起点条件传进去就行。

嗯，多项式插值这块内容就讲到这里。说白了，三次和五次是两种最常用的工具。选哪个，取决于你对平滑性的要求。下次遇到轨迹规划问题，不妨先想想：我的边界条件有几个？然后就知道该用几次多项式了。