4、三次多项式插值：单关节三次多项式轨迹、边界条件设定、位置与速度连续性

各位同学，今天我们来聊聊轨迹规划里最基础、也最常用的一个方法——三次多项式插值。

说实话，我刚入行那会儿，第一次做机器人轨迹规划，领导丢给我一句话：“让机械臂从A点走到B点，别抖。”我当时心想，这还不简单？直接给位置指令不就行了？结果一跑，机械臂在起点和终点猛地一冲一停，差点把工件甩飞出去。嗯，从那以后，我就老老实实学三次多项式了。

4.1 为什么是三次多项式？

你想想看，我们控制一个关节，最朴素的需求是什么？

• 起点位置已知，终点位置已知
• 起点速度已知（通常是0），终点速度已知（通常也是0）

四个条件，对应四个未知系数。三次多项式正好有四个系数：

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

说白了，这就是一个“刚刚好”的数学工具。少一次不够灵活，多一次又没必要。我个人习惯，能用三次解决的，绝不用五次——计算量小，调试也方便。

4.2 边界条件怎么设？

边界条件，就是轨迹的“起止约束”。我们设：

• 起始时间 t = 0，关节位置 θ(0) = θ₀，速度 θ̇(0) = v₀
• 终止时间 t = t_f，关节位置 θ(t_f) = θ_f，速度 θ̇(t_f) = v_f

把四个条件代入多项式，得到方程组：

θ₀ = a₀
v₀ = a₁
θ_f = a₀ + a₁·t_f + a₂·t_f² + a₃·t_f³
v_f = a₁ + 2·a₂·t_f + 3·a₃·t_f²

解这个方程组，就能得到四个系数。我在项目中遇到过最典型的场景：上下料机器人，起点和终点速度都设为0。这样机械臂启动和停止时最平稳，不会冲击减速机。

a₀ = θ₀
a₁ = v₀
a₂ = (3·(θ_f - θ₀) - (2·v₀ + v_f)·t_f) / t_f²
a₃ = (2·(θ₀ - θ_f) + (v₀ + v_f)·t_f) / t_f³

4.3 位置与速度连续性——避坑指南

为什么强调连续性？我举个例子。有一次我做多段轨迹拼接，第一段用三次多项式，第二段也用三次多项式，但两段在衔接点的速度没对齐。结果呢？机械臂在拼接点“咯噔”一下，像开车时猛踩了一脚刹车又立刻加油。工件上的液体都洒了。

我曾经踩过这个坑，后来总结出经验：

• 位置连续：前一段的终点位置 = 后一段的起点位置。这个一般不会忘。
• 速度连续：前一段的终点速度 = 后一段的起点速度。这个最容易忽略！

4.4 一个完整的计算示例

我们来看一个实际算例。假设：

• θ₀ = 30°，θ_f = 75°
• v₀ = 0°/s，v_f = 0°/s
• t_f = 3秒

代入公式：

a₀ = 30
a₁ = 0
a₂ = (3·(75-30) - (0+0)·3) / 9 = 135 / 9 = 15
a₃ = (2·(30-75) + (0+0)·3) / 27 = (-90) / 27 = -3.333

所以轨迹方程为：

θ(t) = 30 + 0·t + 15·t² - 3.333·t³

你看，速度从0开始，到0结束，中间平滑变化。这就是三次多项式的魅力。

4.5 知识体系图

下面这张图，是我自己梳理的三次多项式插值的核心逻辑。你把它记住了，这一章就算吃透了。

好了，这一章就到这里。三次多项式插值，说白了就是“四个条件定四个系数”。你只要记住边界条件怎么设，剩下的就是解方程。下次遇到单关节轨迹规划，先试试三次多项式，90%的场景都够用。

可可精品资料库