数学基础：多项式函数、导数与边界条件、五次多项式的6个约束方程
说实话，很多做机器人控制的朋友一听到「数学基础」四个字就想跳过。我当年也是这样，总觉得直接上代码跑起来就完事了。结果呢？调了三天三夜，轨迹就是不平滑，末端执行器抖得像帕金森。后来我才明白——不懂多项式约束，你连问题出在哪都不知道。
好，咱们今天就把这块硬骨头啃下来。我保证，用最直白的方式讲清楚。
1. 多项式函数长什么样？
五次多项式，说白了就是最高次数为5的多项式函数。它的通用形式长这样：
θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵这里 θ(t) 表示关节角度随时间的变化。t 是时间，a₀ 到 a₅ 是六个待定系数。嗯，六个系数，这就是关键——六个未知数，就需要六个方程来解。
你可能会问：「为什么非得是五次？三次不行吗？」
我在项目里试过三次多项式。三次多项式只能保证位置和速度连续，加速度会突变。想象一下，你开车时如果加速度突然跳变，那感觉就像被人从背后猛推一下。机器人关节也是这个道理——加速度不连续，电机就会受到冲击，轻则抖动，重则损坏减速器。
2. 导数——描述运动的语言
咱们得先统一一下语言。在轨迹规划里，我们关心三个量：
- 位置 θ(t)：关节角度，单位 rad
- 速度 ω(t) = θ'(t)：角度对时间的一阶导数，单位 rad/s
- 加速度 α(t) = θ''(t)：角度对时间的二阶导数，单位 rad/s²
对五次多项式求导，得到速度表达式：
θ'(t) = a₁ + 2·a₂·t + 3·a₃·t² + 4·a₄·t³ + 5·a₅·t⁴再求一次导，得到加速度：
θ''(t) = 2·a₂ + 6·a₃·t + 12·a₄·t² + 20·a₅·t³你看，求导其实就是降次。五次多项式求一次导变成四次，再求一次变成三次。这个规律在后面的约束方程里会反复用到。
3. 边界条件——轨迹的「起跑线」和「终点线」
轨迹规划的本质，就是让机器人从 A 点运动到 B 点。但光有起点和终点位置还不够——你还得告诉机器人：
- 出发时速度是多少？（通常为0）
- 到达时速度是多少？（通常也为0）
- 出发时加速度是多少？（通常为0）
- 到达时加速度是多少？（通常也为0）
这就是六个边界条件。我刚开始做的时候犯过一个低级错误——只给了位置和速度约束，没给加速度约束。结果轨迹虽然连续，但加速度在起点和终点处有跳变，导致机器人启动时猛地一冲，停下来时又猛地一抖。
那次教训让我记住了：边界条件一个都不能少。
4. 五次多项式的6个约束方程
好，现在我们把边界条件写成数学形式。假设轨迹从 t=0 开始，到 t=T 结束：
| 约束类型 | 起始时刻 (t=0) | 终止时刻 (t=T) |
|---|---|---|
| 位置 | θ(0) = θ₀ | θ(T) = θ₁ |
| 速度 | θ'(0) = 0 | θ'(T) = 0 |
| 加速度 | θ''(0) = 0 | θ''(T) = 0 |
把多项式代入这些条件，得到六个方程：
- θ(0) = a₀ = θ₀ —— 起始位置
- θ'(0) = a₁ = 0 —— 起始速度为零
- θ''(0) = 2·a₂ = 0 —— 起始加速度为零
- θ(T) = a₀ + a₁·T + a₂·T² + a₃·T³ + a₄·T⁴ + a₅·T⁵ = θ₁ —— 终止位置
- θ'(T) = a₁ + 2·a₂·T + 3·a₃·T² + 4·a₄·T³ + 5·a₅·T⁴ = 0 —— 终止速度为零
- θ''(T) = 2·a₂ + 6·a₃·T + 12·a₄·T² + 20·a₅·T³ = 0 —— 终止加速度为零
你看，前三个方程直接给出了 a₀、a₁、a₂ 的值。后三个方程需要联立求解 a₃、a₄、a₅。这就是为什么五次多项式刚好需要六个约束——不多不少，刚刚好。
5. 知识体系结构图
下面这张图，是我自己梳理这个知识点时画的。它把整个逻辑串起来了：
这张图的核心逻辑很简单：从多项式出发，通过求导得到运动学描述，再施加边界条件形成约束方程，最终解出系数。每一步都是环环相扣的。
6. 一个具体的计算例子
光说不练假把式。咱们来算一个实际的例子。
假设机器人关节要从 30° 转到 60°，用时 2 秒。起始和终止的速度、加速度都为零。
先把角度换算成弧度：θ₀ = 30° = 0.5236 rad，θ₁ = 60° = 1.0472 rad，T = 2 s。
根据前三个方程：
- a₀ = θ₀ = 0.5236
- a₁ = 0
- a₂ = 0
后三个方程联立求解（这里我直接给结果，具体求解过程下一节会讲）：
- a₃ = 10·(θ₁ - θ₀) / T³ = 10 × 0.5236 / 8 = 0.6545
- a₄ = -15·(θ₁ - θ₀) / T⁴ = -15 × 0.5236 / 16 = -0.4909
- a₅ = 6·(θ₁ - θ₀) / T⁵ = 6 × 0.5236 / 32 = 0.0982
所以最终的五次多项式是：
θ(t) = 0.5236 + 0.6545·t³ - 0.4909·t⁴ + 0.0982·t⁵你可以验证一下：t=0 时 θ=0.5236，t=2 时 θ=1.0472。速度加速度在两端都为零。完美！
好了，数学基础就讲到这里。说白了，五次多项式轨迹规划的核心就是「六个约束解六个系数」。你把这个逻辑刻在脑子里，后面的代码实现就是水到渠成的事。
公众号:蓝海资料掘金营，微信deep3321