4. 轨迹规划入门：梯形速度规划、S形速度规划、多项式插值
各位好，我是老张。今天咱们聊聊轨迹规划里最基础、也最实用的三个方法：梯形速度规划、S形速度规划、多项式插值。
说实话，我刚入行那会儿，觉得轨迹规划不就是让电机从A点走到B点嘛，有啥难的？直到第一次做高速贴片机项目，电机一启动就剧烈抖动，我才意识到——轨迹规划的核心，是让运动既快又稳。
好，咱们一个一个来看。
4.1 梯形速度规划
梯形速度规划，名字很形象。速度曲线像个梯形——先匀加速，再匀速，最后匀减速。
为什么叫梯形？ 你想想看，速度-时间图里，加速段是斜线，匀速段是水平线，减速段是斜线，连起来不就是个梯形嘛。
核心公式：
- 加速段：v(t) = v₀ + a·t，位移 s(t) = v₀·t + ½a·t²
- 匀速段：v(t) = v_max，位移 s(t) = v_max·t
- 减速段：v(t) = v_max - a·t，位移 s(t) = v_max·t - ½a·t²
我在项目中遇到过一个问题：梯形规划虽然简单，但加速度在起点和终点处是突变的。什么意思？就是电机在启动瞬间，加速度从0直接跳到a，会产生一个冲击力。这在低速场合还好，高速时就会引起振动。
避坑指南：我曾经在一个码垛机器人项目里，直接用梯形规划做高速搬运。结果每次启动和停止时，机械臂末端都会抖一下，抓取精度完全达不到要求。后来换成S形规划才解决。
梯形规划的代码实现很简单，我一般这样写：
// 梯形速度规划 - 位置计算
float trapezoidal_profile(float t, float t_acc, float t_const, float t_dec, float v_max, float a) {
if (t <= t_acc) {
// 加速段
return 0.5f * a * t * t;
} else if (t <= t_acc + t_const) {
// 匀速段
float t1 = t - t_acc;
return 0.5f * a * t_acc * t_acc + v_max * t1;
} else if (t <= t_acc + t_const + t_dec) {
// 减速段
float t2 = t - t_acc - t_const;
return 0.5f * a * t_acc * t_acc + v_max * t_const
+ v_max * t2 - 0.5f * a * t2 * t2;
}
// 到达终点
return total_distance;
}4.2 S形速度规划
S形规划，说白了就是给梯形规划加了加加速度（Jerk）约束。加速度不再突变，而是平滑变化。速度曲线从梯形变成了S形。
为什么要加Jerk约束？ 嗯，这里要注意：加速度突变会产生冲击力，冲击力会导致振动。而加加速度（Jerk）就是加速度的变化率。限制Jerk，就是限制冲击力的变化速度。
我个人习惯把S形规划分成7段：
- 加加速度段（Jerk为正，加速度从0增加到a_max）
- 匀加速度段（加速度保持a_max）
- 减加速度段（Jerk为负，加速度从a_max降到0）
- 匀速段（加速度为0）
- 加减速度段（Jerk为负，加速度从0降到-a_max）
- 匀减速度段（加速度保持-a_max）
- 减减速度段（Jerk为正，加速度从-a_max升到0）
我的经验：实际项目中，如果行程很短，可能用不到完整的7段。比如只有加速和减速，没有匀速段。这时候要动态调整参数，我一般会先判断能否达到最大速度，再决定分段策略。
S形规划的代码比梯形复杂一些，但核心逻辑很清晰：
// S形速度规划 - 简化版（仅加速段）
float s_curve_accel(float t, float jerk, float a_max, float v_max) {
float t_j = a_max / jerk; // 加加速度段时间
if (t <= t_j) {
// 加加速度段：Jerk为正
return 0.5f * jerk * t * t * t / 3.0f;
} else if (t <= 2 * t_j) {
// 匀加速度段
float t1 = t - t_j;
return 0.5f * a_max * t_j * t_j / 3.0f
+ a_max * t_j * t1 + 0.5f * a_max * t1 * t1;
} else {
// 减加速度段
float t2 = t - 2 * t_j;
return ...; // 篇幅原因，完整代码见课程附件
}
}4.3 多项式插值
梯形和S形规划，本质上都是分段函数。而多项式插值，是用一个连续的多项式函数来描述整个运动轨迹。
最常用的是三次多项式和五次多项式。
| 类型 | 约束条件 | 特点 |
|---|---|---|
| 三次多项式 | 位置、速度（4个约束） | 简单，但加速度不连续 |
| 五次多项式 | 位置、速度、加速度（6个约束） | 加速度连续，运动更平滑 |
三次多项式长这样：
s(t) = a₀ + a₁·t + a₂·t² + a₃·t³
给定起点位置、终点位置、起点速度、终点速度，就能解出四个系数。说白了就是解一个4元一次方程组。
五次多项式呢？
s(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵
多加了加速度约束，所以需要6个系数。我建议在要求高精度的场合，比如数控机床、焊接机器人，直接用五次多项式。虽然计算量大一点，但运动质量好很多。
实际应用建议：
- 简单搬运、低速场合 → 梯形规划就够了
- 高速、中等精度 → S形规划
- 高精度、连续轨迹 → 五次多项式插值
我曾经在一个激光切割项目里，用五次多项式做轨迹插值。因为切割头要沿着复杂曲线运动，速度必须连续变化，加速度也不能突变。梯形规划根本不行，S形规划分段太多，调试起来麻烦。最后用五次多项式，一个函数搞定，效果很好。
4.4 三种方法的对比
| 方法 | 计算量 | 平滑度 | 适用场景 |
|---|---|---|---|
| 梯形规划 | 低 | 差（加速度突变） | 低速、简单运动 |
| S形规划 | 中 | 好（Jerk可控） | 高速、中等精度 |
| 多项式插值 | 高 | 最好（连续可导） | 高精度、复杂轨迹 |
说白了，没有最好的方法，只有最合适的。我个人的选择原则是：够用就好，别过度设计。能用梯形解决的，就别上S形；能用S形解决的，就别上五次多项式。毕竟计算资源也是成本。
好了，以上就是轨迹规划的三种基本方法。梯形规划简单实用，S形规划平滑可控，多项式插值精度最高。实际项目中，我建议你从梯形开始，如果发现振动问题，再升级到S形或多项式。别一上来就搞最复杂的，调试起来会哭的。
一个小技巧：如果你不确定该用哪种方法，可以先在仿真环境里跑一下梯形规划。如果加速度突变引起的振动在可接受范围内，那就用梯形。如果不行，再换S形。我一般用这种方法快速验证，省时省力。
公众号:蓝海资料掘金营，微信deep3321