3. 多项式轨迹规划基础

各位同学，今天我们来聊聊轨迹规划里最基础、也最常用的一类方法——多项式插值。说白了，就是用一条平滑的多项式曲线，把机器人要经过的点串起来。

我记得刚入行那会儿，带我的老工程师跟我说过一句话：「机器人的运动，本质上就是位置随时间的变化。」而多项式，就是描述这种变化最自然的数学工具。为什么？因为它简单、可导、好算。

3.1 多项式插值的基本思想

先问大家一个问题：给你两个点，你怎么让机器人从A点走到B点？

最简单的想法——直接连一条直线。但机器人不是光速移动，它需要加速、减速。如果直接走直线，起点和终点的速度会突变，产生冲击。这就是为什么我们需要插值。

多项式插值的核心思想，就是用一个多项式函数 q(t) 来描述位置随时间的变化：

q(t) = a₀ + a₁·t + a₂·t² + ... + aₙ·tⁿ

其中 a₀, a₁, ..., aₙ 是待求的系数。我们有多少个约束条件，就决定多项式的阶数。举个例子：

• 约束起点位置和终点位置 → 需要2个条件 → 一次多项式（直线）
• 再加上起点速度和终点速度 → 需要4个条件 → 三次多项式
• 再加上起点加速度和终点加速度 → 需要6个条件 → 五次多项式

我个人习惯把约束条件分成两类：

• 位置约束：机器人必须在某个时刻到达某个位置
• 平滑约束：速度、加速度甚至加加速度在连接点处连续

你想想看，如果只满足位置约束，那速度可能突变，机器人就会「抖」一下。这在工业现场是绝对不能接受的。

3.2 三次多项式轨迹

三次多项式是最常用的轨迹形式。为什么？因为它刚好能同时约束位置和速度。

假设我们要让机器人从 q₀ 运动到 q₁，用时 T。约束条件如下：

时刻	位置约束	速度约束
t = 0	q(0) = q₀	q̇(0) = 0
t = T	q(T) = q₁	q̇(T) = 0

一共4个约束，所以用三次多项式：

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

对时间求导得到速度：

q̇(t) = a₁ + 2·a₂·t + 3·a₃·t²

代入约束条件，解方程组：

q(0) = a₀ = q₀
q̇(0) = a₁ = 0
q(T) = a₀ + a₁·T + a₂·T² + a₃·T³ = q₁
q̇(T) = a₁ + 2·a₂·T + 3·a₃·T² = 0

解出来：

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

3.3 五次多项式轨迹

三次多项式虽然简单，但有个问题：加速度在起点和终点会突变。你想想看，加速度突变意味着什么？——力的突变，也就是冲击。

对于高速运动或者精密装配，这种冲击是不能接受的。这时候就需要五次多项式。

五次多项式可以额外约束加速度：

时刻	位置约束	速度约束	加速度约束
t = 0	q(0) = q₀	q̇(0) = 0	q̈(0) = 0
t = T	q(T) = q₁	q̇(T) = 0	q̈(T) = 0

6个约束，对应五次多项式：

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

解这个方程组稍微复杂一点，但结果很规整：

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

3.4 多项式轨迹的优缺点

做了这么多年轨迹规划，我总结了一下多项式方法的优缺点：

优点	缺点
数学形式简单，计算量小	阶数越高，数值稳定性越差
导数连续，运动平滑	无法精确控制中间点的速度/加速度
容易实现，代码量少	不适合长距离多段轨迹
适合点到点运动	无法避开障碍物

嗯，这里要特别说一下「数值稳定性」的问题。当多项式的阶数超过7次时，系数会变得非常大，稍微有点计算误差，轨迹就会跑偏。我见过有人用10次多项式做轨迹，结果在仿真里看着挺好，一上实际机器人就抖得不行。

最后，给大家看一张我画的对比图，直观感受一下不同阶数多项式的区别：

从图上可以清楚看到：一次多项式（直线）在起点和终点有尖角，速度不连续；三次多项式平滑了很多，但加速度有突变；五次多项式则是最平滑的，加速度也连续变化。

好了，这一节的内容就到这里。多项式轨迹是轨迹规划的基础，理解透了，后面学样条曲线、B样条就会轻松很多。