3. 多项式轨迹规划：三次多项式、五次多项式插值，以及梯形速度规划的实现
轨迹规划，说白了就是让机器人“怎么动”的问题。你给机器人一个起点和终点，它不能“嗖”一下瞬移过去——物理上不允许，电机也受不了。所以我们需要一条平滑的路径，让位置、速度、加速度都连续变化。
这一章，我们聊三种最基础的规划方法：三次多项式、五次多项式，还有梯形速度规划。这三种方法我几乎在每个项目里都用过，从简单的单轴点到点运动，到复杂的多轴协调，都离不开它们。
3.1 三次多项式插值：最简单的平滑曲线
三次多项式长这样：
q(t) = a0 + a1*t + a2*t² + a3*t³它有四个未知系数。我们需要四个约束条件来解它。通常我们给定起点和终点的位置，以及起点和终点的速度。
假设起点时间 t=0，终点时间 t=T。那么：
- q(0) = q_start
- q(T) = q_end
- q'(0) = v_start
- q'(T) = v_end
把这四个条件代进去，解一个四元一次方程组，就能得到 a0, a1, a2, a3。我习惯直接写出解析解，省得每次都要解矩阵。
三次多项式系数解析解：
a0 = q_start
a1 = v_start
a2 = (3*(q_end - q_start) - (2*v_start + v_end)*T) / T²
a3 = (2*(q_start - q_end) + (v_start + v_end)*T) / T³嗯，这里要注意：三次多项式只能保证位置和速度连续，加速度是线性的，在起点和终点处会有跳变。如果你的机器人对加速度突变比较敏感（比如高精度装配），三次多项式可能不够用。
我的经验： 在码垛、搬运这类对启停冲击要求不高的场景，三次多项式完全够用。我曾经在一个码垛项目里，所有轴都用三次多项式规划，跑了两年没出过问题。代码简单，计算量小，实时性很好。
3.2 五次多项式插值：加速度也连续
如果三次多项式不满足要求，那就上五次多项式。它多了两个自由度，可以同时约束加速度。
q(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵六个未知系数，需要六个约束条件：位置、速度、加速度在起点和终点都给定。
- q(0), q(T), q'(0), q'(T), q''(0), q''(T)
解这个方程组稍微麻烦一点，但同样有解析解。我直接给代码实现吧，你拿去就能用。
def quintic_poly(q0, q1, v0, v1, a0, a1, T):
"""
计算五次多项式系数
q0, q1: 起点/终点位置
v0, v1: 起点/终点速度
a0, a1: 起点/终点加速度
T: 总时间
"""
t = T
t2 = t * t
t3 = t2 * t
t4 = t3 * t
t5 = t4 * t
a = q0
b = v0
c = a0 / 2.0
d = (20*(q1 - q0) - (8*v1 + 12*v0)*t - (3*a0 - a1)*t2) / (2*t3)
e = (30*(q0 - q1) + (14*v1 + 16*v0)*t + (3*a0 - 2*a1)*t2) / (2*t4)
f = (12*(q1 - q0) - (6*v1 + 6*v0)*t - (a0 - a1)*t2) / (2*t5)
return [a, b, c, d, e, f]避坑指南： 我曾经在一个焊接机器人项目里，直接用五次多项式规划所有轨迹。结果发现，虽然加速度连续了，但加加速度（jerk）在某些路径点非常大，导致机械臂末端抖动。后来我加了加加速度约束，才解决问题。所以，五次多项式不是万能的，关键看你的应用对平滑度的要求有多高。
3.3 梯形速度规划：简单又实用
梯形速度规划，也叫T型速度曲线。它把运动分成三个阶段：匀加速、匀速、匀减速。速度曲线看起来像个梯形，所以叫这个名字。
为什么说它实用？因为很多工业机器人、CNC机床都在用。它简单、直观、容易调参。你想想看，一个电机从静止加速到目标速度，然后匀速跑一段，最后减速停下——这不就是最自然的运动方式吗？
梯形速度规划的核心参数有三个：
| 参数 | 含义 | 典型值 |
|---|---|---|
| v_max | 最大速度 | 电机额定转速的80% |
| a_max | 最大加速度 | 电机额定扭矩对应的加速度 |
| d_max | 最大减速度 | 通常等于a_max |
给定总位移S，我们需要判断能不能达到最大速度。如果S太小，还没加速到v_max就得开始减速了——这种情况叫“三角形速度曲线”。
判断条件：
# 如果位移足够大，能到达v_max
if v_max * v_max / a_max + v_max * v_max / d_max <= 2 * S:
# 有匀速段
t_acc = v_max / a_max
t_dec = v_max / d_max
t_const = (S - 0.5 * a_max * t_acc² - 0.5 * d_max * t_dec²) / v_max
else:
# 没有匀速段，三角形曲线
# 需要重新计算实际能达到的最大速度
v_peak = sqrt(2 * S * a_max * d_max / (a_max + d_max))
t_acc = v_peak / a_max
t_dec = v_peak / d_max
t_const = 0我个人习惯把梯形速度规划封装成一个类，这样在多个轴之间复用很方便。每个轴有自己的v_max和a_max，但时间同步需要额外处理——这个我们后面章节再聊。
3.4 三种方法的对比与选择
我把这三种方法放在一起对比一下，方便你选型：
| 方法 | 连续阶数 | 计算量 | 适用场景 |
|---|---|---|---|
| 三次多项式 | 速度连续 | 低 | 码垛、搬运、简单点对点 |
| 五次多项式 | 加速度连续 | 中 | 焊接、喷涂、高精度装配 |
| 梯形速度 | 速度连续（加速度分段常数） | 低 | 工业机器人、CNC、通用运动控制 |
你可能会问：那到底选哪个？我的建议是：
- 如果项目时间紧、要求不高，直接用梯形速度规划。它最稳定，也最容易调试。
- 如果对平滑度有要求，但计算资源有限，用三次多项式。
- 如果对加速度冲击敏感，或者需要高精度轨迹跟踪，用五次多项式。
一个小技巧： 我经常在同一个项目里混用这三种方法。比如，粗定位用梯形速度，精定位用五次多项式。这样既保证了效率，又保证了精度。你试试看，效果很好。
3.5 知识体系总览
下面这张图总结了本章的核心内容，帮你理清思路：
这张图把三种方法并列展示，你可以看到它们各自的特性、适用场景，以及它们之间的层级关系。在实际项目中，我经常根据需求在这三种方法之间切换，甚至组合使用。
3.6 实现中的常见问题
最后，分享几个我在实现过程中踩过的坑：
- 时间参数T的选择： T太小，速度加速度会很大，可能超出电机能力；T太大，运动太慢，影响效率。我一般先根据最大速度和加速度估算一个最小T，然后留10%-20%的余量。
- 数值稳定性： 当T很小时，多项式系数计算中会出现T的高次幂在分母上，容易产生数值误差。我建议使用double类型，并避免T小于1毫秒。
- 多轴同步： 如果多个轴同时运动，每个轴的T必须相同，否则轨迹会不同步。我通常以最慢的轴为准，其他轴跟着它走。
曾经踩过的坑： 有一次我在一个六轴机器人上做轨迹规划，所有轴都用五次多项式，T设得一样。结果运行时发现，某个轴在路径中间突然抖动了一下。排查了半天，发现是那个轴的T太小，导致加速度超过了电机额定值。后来我加了一个安全检查：在生成轨迹前，先校验所有轴的加速度是否在允许范围内。从那以后，再也没出过类似问题。
好了，这一章的内容就到这里。三种方法各有优劣，关键是根据你的应用场景选择合适的方案。代码实现不难，难的是理解每种方法背后的物理意义和适用边界。多动手写代码，多在实际机器人上跑一跑，你很快就能掌握它们。