第三节：多项式轨迹——三次与五次多项式轨迹生成及边界条件
各位好，我是你们的老朋友。今天我们来聊聊多项式轨迹。
说实话，在机器人控制里，轨迹平滑是个绕不开的话题。你想想看，如果让电机直接从A点跳到B点，那冲击力能把机械结构震散架。我早年做六轴机器人调试时，就吃过这个亏——电机一启动，末端直接抖成筛子。后来才发现，问题出在轨迹规划上。
多项式轨迹，说白了就是用数学函数把路径“捋顺”。三次和五次多项式，是最常用的两种。咱们一个一个说。
3.1 三次多项式轨迹
三次多项式长这样：
q(t) = a0 + a1*t + a2*t^2 + a3*t^3它有四个未知系数：a0、a1、a2、a3。要解出这四个数，我们需要四个边界条件。
典型的边界条件是什么？
- 起始位置 q(0) = q_start
- 终止位置 q(T) = q_end
- 起始速度 v(0) = v_start
- 终止速度 v(T) = v_end
把位置和速度条件代进去，就能解出系数。我习惯写成矩阵形式：
| 1 0 0 0 | | a0 | | q_start |
| 1 T T^2 T^3 | * | a1 | = | q_end |
| 0 1 0 0 | | a2 | | v_start |
| 0 1 2*T 3*T^2 | | a3 | | v_end |解这个方程组，就能得到轨迹。三次多项式的特点是：位置和速度连续，但加速度不连续。什么意思？就是电机跑起来速度变化是平滑的，但加速度会突变。这在一些对冲击敏感的场合（比如精密装配）就不太够用。
核心要点：三次多项式只能保证位置和速度连续，加速度在起点和终点处可能跳变。
我的经验：如果只是做简单的点到点运动，比如AGV小车从一个工位到另一个工位，三次多项式完全够用。我做过一个仓储机器人项目，就是用三次多项式规划的路径，跑了两年没出过问题。
3.2 五次多项式轨迹
五次多项式长这样：
q(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5六个未知系数，需要六个边界条件。除了位置和速度，我们还可以约束加速度：
- 起始位置 q(0) = q_start
- 终止位置 q(T) = q_end
- 起始速度 v(0) = v_start
- 终止速度 v(T) = v_end
- 起始加速度 a(0) = a_start
- 终止加速度 a(T) = a_end
写成矩阵：
| 1 0 0 0 0 0 | | a0 | | q_start |
| 1 T T^2 T^3 T^4 T^5 | | a1 | | q_end |
| 0 1 0 0 0 0 | * | a2 | = | v_start |
| 0 1 2*T 3*T^2 4*T^3 5*T^4 | | a3 | | v_end |
| 0 0 2 0 0 0 | | a4 | | a_start |
| 0 0 2 6*T 12*T^2 20*T^3| | a5 | | a_end |五次多项式的优势很明显：位置、速度、加速度全部连续。这意味着电机运行起来极其平滑，没有冲击感。
核心要点：五次多项式能保证位置、速度、加速度三阶连续，适合对运动品质要求高的场景。
我曾经踩过的坑：有一次做高精度点胶机器人，我图省事用了三次多项式。结果点胶头在拐弯处加速度突变，导致胶水甩得到处都是。后来换成五次多项式，问题立刻解决。所以，如果对运动平滑度有要求，别犹豫，直接上五次多项式。
3.3 边界条件的选择策略
边界条件不是随便设的。我总结了几条经验：
| 场景 | 推荐多项式 | 边界条件设置 |
|---|---|---|
| 简单点到点运动 | 三次 | 起止速度设为0 |
| 连续路径跟踪 | 五次 | 起止加速度也设为0 |
| 高速高精度 | 五次或更高阶 | 考虑加加速度约束 |
| 实时性要求高 | 三次 | 计算量小，适合嵌入式 |
你可能会问：为什么起止加速度要设为0？嗯，这是为了让电机启动和停止时没有冲击。我见过有人把加速度设成非零值，结果电机一启动就“咯噔”一下，听着都心疼。
3.4 代码实现示例
下面给一个Python实现，生成五次多项式轨迹：
import numpy as np
def quintic_trajectory(q_start, q_end, v_start, v_end, a_start, a_end, T, num_points=100):
"""
生成五次多项式轨迹
"""
# 构建矩阵
A = np.array([
[1, 0, 0, 0, 0, 0],
[1, T, T**2, T**3, T**4, T**5],
[0, 1, 0, 0, 0, 0],
[0, 1, 2*T, 3*T**2, 4*T**3, 5*T**4],
[0, 0, 2, 0, 0, 0],
[0, 0, 2, 6*T, 12*T**2, 20*T**3]
])
b = np.array([q_start, q_end, v_start, v_end, a_start, a_end])
# 解系数
coeffs = np.linalg.solve(A, b)
# 生成轨迹点
t = np.linspace(0, T, num_points)
q = np.zeros_like(t)
for i in range(6):
q += coeffs[i] * t**i
return t, q, coeffs
# 使用示例
t, q, coeffs = quintic_trajectory(0, 10, 0, 0, 0, 0, 2.0)
print("系数:", coeffs)小技巧：实际项目中，我通常会把轨迹生成封装成一个类，方便复用。另外，如果T（运动时间）太短，加速度会很大，容易超出电机能力。记得做加速度校验。
3.5 知识体系结构图
下面这张图，是我自己梳理的多项式轨迹知识体系，你看一眼就能明白整体脉络：
嗯，以上就是多项式轨迹的核心内容。三次和五次，说白了就是“够用”和“好用”的区别。你根据实际需求选就行。
可可精品资料库