4、多项式轨迹表示：多项式函数、最小加加速度轨迹、最小加加加速度轨迹、边界条件设定
各位同学，今天我们来聊聊轨迹优化里最基础、也最核心的一块——多项式轨迹表示。
说实话，我刚入行那会儿，觉得轨迹规划不就是画条曲线嘛，随便搞个三次多项式就完事了。后来在项目里吃过亏，才明白这里面的门道有多深。你想想看，机器人动起来，如果轨迹不平滑，轻则抖动，重则撞机。所以，选对多项式形式、设对边界条件，是后面所有优化的地基。
4.1 为什么是多项式？
多项式函数在轨迹规划里这么受欢迎，说白了就三个原因：
- 计算简单：求导、求积分都是线性操作，闭式解好算
- 连续性好：多项式是无限可微的，天然保证平滑
- 边界可控：通过系数调整，可以精确满足起点和终点的位置、速度、加速度
我个人习惯把多项式轨迹比作「搭积木」。你给定起点和终点的状态（位置、速度、加速度等），多项式就是那个刚好能把这些点串起来的平滑曲线。积木块数越多（多项式阶数越高），你能控制的细节就越多。
4.2 多项式函数的基本形式
一个 n 次多项式长这样：
p(t) = c₀ + c₁·t + c₂·t² + ... + cₙ·tⁿ其中 t 是时间，c₀ 到 cₙ 是待求的系数。我们通常把时间归一化到 [0, T] 区间，T 是轨迹的总时长。
它的各阶导数就是：
速度： v(t) = c₁ + 2c₂·t + 3c₃·t² + ... + n·cₙ·tⁿ⁻¹
加速度： a(t) = 2c₂ + 6c₃·t + ... + n·(n-1)·cₙ·tⁿ⁻²
加加速度： j(t) = 6c₃ + ... + n·(n-1)·(n-2)·cₙ·tⁿ⁻³嗯，这里要注意：加加速度（Jerk）是加速度的变化率，它直接决定了机器人运动时的冲击感。我在做协作机器人项目时，如果 Jerk 太大，末端执行器上的工件就会晃动，甚至掉落。所以后来我特别关注 Jerk 的约束。
4.3 最小加加速度轨迹（Minimum Jerk Trajectory）
什么叫「最小加加速度」？就是让 Jerk 在整个运动过程中尽可能小。为什么要这么做？因为 Jerk 小，意味着加速度变化平缓，运动就柔和，对机械结构的冲击也小。
数学上，我们最小化这个代价函数：
J = ∫₀ᵀ (j(t))² dt对于一段从起点到终点的轨迹，如果我们只约束位置、速度、加速度（共 6 个边界条件），那么一个 5 次多项式就能搞定。为什么是 5 次？因为 6 个未知系数对应 6 个方程，刚好有唯一解。
我记得有一次做码垛机器人的轨迹优化，客户要求末端轨迹必须平滑到「感觉不到换向」。我试了三次多项式，结果加速度在起点和终点有跳变，机器人启动时「咯噔」一下。换成五次多项式后，加速度连续了，但 Jerk 还是有点大。最后用了七次多项式，才把 Jerk 也约束住。
4.4 最小加加加速度轨迹（Minimum Snap Trajectory）
加加加速度（Snap）是 Jerk 的导数，也就是位置的四阶导数。最小化 Snap 有什么用？它能让 Jerk 的变化也平缓，进一步减少高频振动。
代价函数变成：
J = ∫₀ᵀ (s(t))² dt如果你要同时约束位置、速度、加速度、加加速度（共 8 个边界条件），那就需要 7 次多项式。我在做无人机航迹规划时，就特别喜欢用最小 Snap 轨迹。因为无人机对高频振动特别敏感，Snap 一大，飞控就容易震荡。
4.5 边界条件设定
边界条件就是轨迹在起点和终点必须满足的状态。常见的设定方式有：
| 边界条件 | 物理含义 | 典型值 |
|---|---|---|
| p(0) = p₀, p(T) = p₁ | 起点和终点位置 | 关节角度或笛卡尔坐标 |
| v(0) = v₀, v(T) = v₁ | 起点和终点速度 | 通常为 0（静止到静止） |
| a(0) = a₀, a(T) = a₁ | 起点和终点加速度 | 通常为 0 |
| j(0) = j₀, j(T) = j₁ | 起点和终点加加速度 | 通常为 0（可选） |
我曾经犯过一个错误：在做多段轨迹拼接时，只保证了位置和速度连续，忘了加速度连续。结果轨迹在拼接点处加速度突变，机器人直接抖了一下。从那以后，我给自己定了个规矩：至少保证加速度连续，除非你明确知道不需要。
4.6 知识体系总览
下面这张图是我自己整理的，把本章的核心逻辑串起来了：
从这张图可以看得很清楚：从多项式函数出发，根据你的优化目标（最小 Jerk 还是最小 Snap），选择对应的多项式阶数，然后设定好边界条件，最后落地到实际应用中。每一步都有明确的物理含义和数学对应。
4.7 代码实战：求解五次多项式系数
光说不练假把式。下面我给出一段 Python 代码，演示如何求解五次多项式的系数。这段代码我在好几个项目里复用过了，很稳定。
import numpy as np
def quintic_poly_coeffs(p0, p1, v0, v1, a0, a1, T):
"""
求解五次多项式系数
边界条件：位置、速度、加速度在起点和终点
T: 总时间
返回：c0~c5 共6个系数
"""
# 构建约束矩阵
A = np.array([
[1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 2, 0, 0, 0],
[1, T, T**2, T**3, T**4, T**5],
[0, 1, 2*T, 3*T**2, 4*T**3, 5*T**4],
[0, 0, 2, 6*T, 12*T**2, 20*T**3]
])
b = np.array([p0, v0, a0, p1, v1, a1])
# 解线性方程组
coeffs = np.linalg.solve(A, b)
return coeffs
# 示例：从静止到静止，移动1米，用时2秒
p0, p1 = 0.0, 1.0
v0, v1 = 0.0, 0.0
a0, a1 = 0.0, 0.0
T = 2.0
c = quintic_poly_coeffs(p0, p1, v0, v1, a0, a1, T)
print("五次多项式系数：", c)
这段代码的核心就是构建一个 6x6 的矩阵 A，把边界条件写成线性方程组，然后求解。你想想看，如果我要加 Jerk 约束，矩阵就变成 8x8，系数变成 7 次多项式。道理是一样的，只是矩阵大了一圈。
好了，关于多项式轨迹表示，我就讲这么多。记住一句话：选对阶数、设好边界、算对系数，你的轨迹就成功了一半。剩下的，就是根据实际约束去优化了。