3. 多项式轨迹：三次多项式轨迹、五次多项式轨迹、边界条件与连续性约束

各位同学，今天我们来聊聊多项式轨迹。说实话，这是机器人轨迹规划里最基础、也最实用的工具之一。我刚开始做机器人控制那会儿，第一个上手的项目就是写三次多项式轨迹生成器。那时候踩了不少坑，今天我把这些经验都抖出来，希望能帮你们少走弯路。

3.1 为什么是多项式？

你想想看，机器人从一个点运动到另一个点，中间怎么走？最简单的想法是让位置随时间平滑变化。多项式函数正好能满足这个需求——它连续、可导，而且计算简单。

说白了，多项式轨迹就是用时间 t 的多项式函数来描述关节位置 q(t)。我们最常用的是三次和五次多项式。为什么是这两个次数？嗯，这里头有讲究。

3.2 三次多项式轨迹

三次多项式长这样：

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

它有四个未知系数 a₀~a₃，所以我们需要四个约束条件才能解出来。通常我们给定起点和终点的位置和速度：

边界条件	数学表达
起点位置	q(0) = q₀
终点位置	q(t_f) = q_f
起点速度	q̇(0) = v₀
终点速度	q̇(t_f) = v_f

把这四个条件代进去，解一个四元一次方程组，就能得到系数。我在项目中遇到过一个问题：如果起点和终点的速度都设为零，轨迹在两端会显得有点「愣」，启动和停止的加速度突变很大。

3.3 五次多项式轨迹

五次多项式长这样：

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

六个未知系数，需要六个约束条件。除了位置和速度，我们还可以约束起点和终点的加速度：

边界条件	数学表达
起点位置	q(0) = q₀
终点位置	q(t_f) = q_f
起点速度	q̇(0) = v₀
终点速度	q̇(t_f) = v_f
起点加速度	q̈(0) = a₀
终点加速度	q̈(t_f) = a_f

我个人习惯在大多数场景下用五次多项式。为什么？因为它能保证加速度连续，运动更平滑。我曾经在一个码垛机器人项目里，一开始用三次多项式，结果每次启停时机械臂都在抖。换成五次多项式后，问题立马解决了。

3.4 边界条件与连续性约束

这里我要强调一个概念：连续性约束。它指的是轨迹在连接点处的光滑程度。我们用 Cⁿ 来表示 n 阶导数连续：

• C⁰ 连续：位置连续，但速度可以跳变
• C¹ 连续：位置和速度都连续
• C² 连续：位置、速度、加速度都连续

三次多项式只能做到 C¹ 连续，五次多项式能做到 C² 连续。为什么 C² 重要？因为加速度连续意味着加加速度（jerk）有限，机器人运动更平稳，对机械结构的冲击更小。

避坑指南：我曾经在一个多段轨迹拼接的项目里，只检查了位置和速度连续，忽略了加速度。结果两段轨迹连接处加速度突变，导致电机电流瞬间飙升，差点烧了驱动器。从那以后，我只要做轨迹拼接，一定会检查到加速度连续。

3.5 知识体系总览

下面这张图是我自己画的，把多项式轨迹的核心逻辑串起来了：

3.6 代码实现要点

写代码的时候，我建议你把系数求解封装成一个函数。输入是边界条件，输出是多项式系数。这样不管是三次还是五次，调用起来都很方便。

// 三次多项式系数求解（伪代码）
function solveCubic(q0, qf, v0, vf, tf) {
    a0 = q0;
    a1 = v0;
    a2 = (3*(qf - q0) - (2*v0 + vf)*tf) / (tf*tf);
    a3 = (2*(q0 - qf) + (v0 + vf)*tf) / (tf*tf*tf);
    return [a0, a1, a2, a3];
}

// 五次多项式系数求解（伪代码）
function solveQuintic(q0, qf, v0, vf, a0, af, tf) {
    // 这里用矩阵求解，或者直接套公式
    // 我习惯用矩阵形式，代码更清晰
    // 具体公式略长，建议查标准库
}

好了，关于三次和五次多项式轨迹，核心就是这些。记住：选三次还是五次，取决于你对加速度连续性的要求。如果只是简单的点到点运动，三次就够了；如果对运动平滑度有要求，别犹豫，上五次。

---