4、多项式轨迹规划：三次多项式插值、五次多项式插值、带中间点的多项式轨迹、过约束问题的处理

轨迹规划里，多项式插值是最基础、最常用的方法之一。说白了，就是给机器人关节或末端位置找一条平滑的曲线，让它从A点走到B点，中间不抖、不跳、不超限。

我个人习惯把多项式轨迹分成两类：低阶和高阶。低阶计算快、简单，但约束少；高阶更灵活，能加更多条件，但算起来也重一些。咱们一个一个说。

4.1 三次多项式插值

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

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

它有四个未知系数，所以需要四个约束条件。通常我们给定起点和终点的位置、速度：

解这个方程组，就能得到四个系数。我在项目中遇到过一个问题：三次多项式只能保证位置和速度连续，加速度在起点和终点会突变。如果你做的是高速抓取，加速度突变会产生冲击力，轻则抖动，重则损坏机构。

代码实现很简单，我一般这样写：

def cubic_trajectory(theta_0, theta_f, t_f, t):
    # 解系数
    a0 = theta_0
    a1 = 0.0
    a2 = 3.0 * (theta_f - theta_0) / (t_f ** 2)
    a3 = -2.0 * (theta_f - theta_0) / (t_f ** 3)
    # 计算位置
    return a0 + a1*t + a2*t**2 + a3*t**3

4.2 五次多项式插值

如果你需要更平滑的轨迹，那就得上五次多项式了。它比三次多项式多了两个自由度，可以额外约束起点和终点的加速度：

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

六个系数，六个约束：位置、速度、加速度在起点和终点都给定。这样加速度就是连续的，不会突变。

解这个方程组稍微麻烦一点，但也是解析解，直接算就行：

def quintic_trajectory(theta_0, theta_f, t_f, t):
    # 归一化时间
    tau = t / t_f
    # 五次多项式系数（解析解）
    a0 = theta_0
    a1 = 0.0
    a2 = 0.0
    a3 = 10.0 * (theta_f - theta_0)
    a4 = -15.0 * (theta_f - theta_0)
    a5 = 6.0 * (theta_f - theta_0)
    # 计算位置
    return a0 + a1*tau + a2*tau**2 + a3*tau**3 + a4*tau**4 + a5*tau**5

4.3 带中间点的多项式轨迹

实际应用中，很少只做A到B的单段运动。更多时候，我们需要经过多个中间点，比如避障、抓取路径上的关键位置。

带中间点的轨迹，常见做法有两种：

1. 分段多项式：每两个相邻点之间用一段三次或五次多项式连接。段与段之间在中间点处保证位置、速度连续。
2. 全局多项式：用一个高阶多项式一次性穿过所有点。点数多了，阶数就高，容易出现龙格现象（Runge's phenomenon）——曲线在端点附近剧烈振荡。

我个人更推荐分段多项式。原因很简单：稳定、可控、好调试。每段只负责两个点之间的运动，出了问题只需要改局部，不用动全局。

分段多项式在中间点处的衔接条件是这样的：

嗯，这里要注意：如果中间点处速度不为零，那你就得额外指定中间点的速度值。我一般用两种方式确定中间点速度：

• 平均法：取前后两段平均速度作为中间点速度。
• 用户指定：由工艺要求直接给定，比如在抓取点处速度必须为零。

4.4 过约束问题的处理

什么叫过约束？就是你给的条件太多了，多项式系数不够用。比如你只有四个系数，却给了五个约束条件——这就无解了。

为什么会这样？你想想看，实际项目中经常出现这种情况：既要保证起点终点位置速度加速度，又要保证中间某点的位置，还要限制最大速度……约束条件一多，低阶多项式就扛不住了。

处理过约束，我常用的方法有几种：

我曾经在一个六轴机器人项目中遇到过这种情况：用户要求轨迹经过7个点，每个点都要位置、速度、加速度连续。如果用全局多项式，阶数高达18阶，计算不稳定，曲线振荡严重。最后我用了分段五次多项式+中间点速度优化，把问题拆成6段，每段5次多项式，中间点速度用优化算法自动确定。效果很好，曲线平滑，计算也快。

下面这张图总结了本章的知识体系：

好了，多项式轨迹规划的核心内容就这些。从三次到五次，从单段到多段，再到过约束处理——本质上都是在约束数量和计算复杂度之间找平衡。实际项目中，我建议你从五次多项式起步，遇到中间点就分段，遇到过约束就升阶或放松条件。这套思路，基本能覆盖90%以上的轨迹规划场景。