第四章：多项式轨迹规划——从三次到S形，实战中的选择与坑
各位工程师朋友，大家好。今天我们来聊聊轨迹规划里最基础、也最常用的一类方法——多项式轨迹规划。说实话，我入行那会儿，第一次做机器人关节插补，用的就是三次多项式。那时候觉得这东西太简单了，不就是解个方程组嘛。后来在实际产线上跑起来，才发现坑不少。今天我把这些经验掰开揉碎，跟你好好说说。
4.1 三次多项式插值：最朴素的平滑运动
三次多项式长这样：
q(t) = a0 + a1*t + a2*t^2 + a3*t^3它有四个未知系数。所以我们只需要四个边界条件：起始位置、起始速度、终点位置、终点速度。通常我们设起始和终点速度为零。
解这个方程组很简单，我一般直接写矩阵形式：
| 1 t0 t0^2 t0^3 | | a0 | | q0 |
| 0 1 2t0 3t0^2 | * | a1 | = | v0 |
| 1 tf tf^2 tf^3 | | a2 | | qf |
| 0 1 2tf 3tf^2 | | a3 | | vf |嗯，这里要注意：如果起始和终点速度不为零，比如在传送带跟踪场景下，你就要把实际速度填进去。我曾经在一条包装线上吃过这个亏——默认设零速度，结果机械臂在抓取点猛刹了一下，产品全飞了。
核心特点：
- 加速度是线性变化的（一次函数）
- 位置和速度连续，但加速度在起点和终点有跳变
- 计算量极小，适合嵌入式实时系统
避坑指南： 我曾经用三次多项式做高速拾取，结果发现末端抖动明显。原因就是加速度不连续，在启停时刻产生了冲击力。如果你的应用对振动敏感（比如精密装配），建议直接上五次多项式。
4.2 五次多项式插值：更平滑，但代价是什么？
五次多项式有六个系数：
q(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5多出来的两个自由度，让我们可以约束加速度。也就是说，我们可以指定起始加速度和终点加速度都为零。这样加速度曲线就是连续的，没有突变。
解这个方程组需要六个边界条件：位置、速度、加速度各两个。我习惯用下面的代码模板：
function [a0,a1,a2,a3,a4,a5] = quintic_traj(t0,tf,q0,qf,v0,vf,acc0,accf)
T = tf - t0;
% 直接公式解，避免矩阵求逆
a0 = q0;
a1 = v0;
a2 = acc0/2;
a3 = (20*qf - 20*q0 - (8*vf + 12*v0)*T - (3*acc0 - accf)*T^2) / (2*T^3);
a4 = (30*q0 - 30*qf + (14*vf + 16*v0)*T + (3*acc0 - 2*accf)*T^2) / (2*T^4);
a5 = (12*qf - 12*q0 - (6*vf + 6*v0)*T - (acc0 - accf)*T^2) / (2*T^5);
end我的经验： 在半导体晶圆搬运这类高精度场景，我必用五次多项式。虽然计算量比三次大一些，但现在的MCU跑这点运算完全不是问题。说白了，别省那点算力，换来的是设备寿命和良率。
4.3 带中间点约束的多段多项式
实际应用中，我们很少只规划一段轨迹。更多情况是：机器人要从A点经过B、C、D点，最后到E点。这时候就需要多段多项式拼接。
关键问题来了：如何保证段与段之间的平滑过渡？
我常用的方法有两种：
- 速度连续法： 每段终点速度等于下一段起点速度。位置自然连续，但加速度可能跳变。
- 加速度连续法： 在中间点同时约束位置、速度、加速度连续。这需要用到更高阶的多项式，或者用样条插值。
举个例子，假设有三个路径点 P1、P2、P3：
段1: P1 → P2, 时间 [t1, t2]
段2: P2 → P3, 时间 [t2, t3]
约束条件：
段1终点位置 = P2 = 段2起点位置
段1终点速度 = 段2起点速度
（可选）段1终点加速度 = 段2起点加速度我个人习惯在中间点留一个小的“通过速度”，而不是强制速度为零。你想想看，如果每个中间点都刹停再启动，那效率得多低？我在做码垛机器人时，中间点速度通常设为最大速度的30%~50%，既保证平滑，又不损失节拍。
多段多项式规划的核心原则：
- 位置连续是底线，必须保证
- 速度连续是基本要求，否则会有冲击
- 加速度连续是进阶要求，根据应用场景取舍
4.4 梯形速度规划：简单粗暴但有效
梯形速度曲线，说白了就是三段：匀加速、匀速、匀减速。它的速度曲线像个梯形，因此得名。
数学表达很简单：
阶段1 (加速段): v(t) = v0 + a*t
阶段2 (匀速段): v(t) = v_max
阶段3 (减速段): v(t) = v_max - a*(t - t2)梯形规划的优点是计算量极小，适合实时性要求极高的场景。但缺点也很明显——加速度在三个阶段的切换点上有跳变。说白了就是加加速度（jerk）无穷大。
我曾经踩过的坑： 在一条高速分拣线上用梯形规划，结果运行半年后，减速机的齿面出现了明显的冲击痕迹。后来换成S形规划，问题就解决了。所以，如果你的设备设计寿命要求长，或者负载比较大，慎用梯形。
4.5 S形速度规划：工业界的黄金标准
S形速度曲线，本质上是让加速度也平滑变化。它把梯形规划的加速段又分成了三段：加加速、匀加速、减加速。减速段同理。所以整个曲线有七段。
七段分别是：
- 加加速段（jerk为正，加速度从0增加到a_max）
- 匀加速段（加速度保持a_max）
- 减加速段（jerk为负，加速度从a_max降到0）
- 匀速段（速度保持v_max）
- 加减速段（jerk为负，加速度从0降到-a_max）
- 匀减速段（加速度保持-a_max）
- 减减速段（jerk为正，加速度从-a_max升到0）
你看，这七段保证了位置、速度、加速度全部连续，只有加加速度（jerk）是分段常数的。对于大多数工业应用，这已经足够平滑了。
我给出一个简化的S形规划代码框架：
function [t, q, v, a] = s_curve_traj(q0, qf, v_max, a_max, jerk_max)
% 计算各段持续时间
% 这里省略了详细的边界条件判断逻辑
% 实际应用中需要根据位移量判断是否能达到最大速度
% 假设能完整走完七段
T_j = a_max / jerk_max; % 加加速段时间
T_a = v_max / a_max - T_j; % 匀加速段时间
T_v = ... % 匀速段时间（根据剩余位移计算）
T_d = T_a; % 匀减速段时间（对称情况）
% 分段计算位置、速度、加速度
% 每段用五次多项式或直接积分公式
end我的建议： 如果你刚开始做轨迹规划，先别急着写七段完整的S形。可以先从对称的S形开始（加速段和减速段参数相同），跑通了再考虑非对称情况。我在带新人时，都是让他们先调通梯形，再改S形，这样不容易出错。
4.6 知识体系总览
下面这张图是我自己整理的轨迹规划方法选择逻辑，你可以参考：
这张图的核心逻辑很简单：先问自己，我的应用对加速度连续性要求高不高？ 如果高，就往左边走；如果低，就往右边走。然后再根据精度和成本做二次选择。
4.7 实战中的选择建议
| 应用场景 | 推荐方法 | 理由 |
|---|---|---|
| 精密装配（如芯片贴装） | S形速度规划 + 五次多项式 | 振动最小，定位精度高 |
| 高速拾取（如食品分拣） | 梯形速度规划 + 三次多项式 | 节拍优先，冲击可接受 |
| 多路径点焊接 | 多段五次多项式拼接 | 路径平滑，避免急停急启 |
| 低成本SCARA | 三次多项式 + 梯形 | 算力有限，够用就好 |
最后说一句：没有最好的方法，只有最合适的。我在不同项目里用过这四种方法的各种组合。关键是要理解每种方法的数学本质和物理含义，然后根据你的硬件条件、节拍要求、精度指标去做权衡。
好了，这一章的内容就到这里。希望这些实战经验能帮你少走弯路。