4. 时间最优轨迹规划：梯形速度规划、S形速度规划原理与实现
时间最优，说白了就是让机器人跑得最快。但你不能让它猛冲猛停——电机受不了，末端还会抖。我早年做六轴机器人时，就吃过这个亏：梯形速度规划写得太糙，加减速段直接拉满，结果电机电流过冲，驱动器报警。从那以后，我对速度规划就特别较真。
这一章，我们聚焦两种最经典的规划方法：梯形速度规划和S形速度规划。它们不复杂，但很实用。你想想看，工业机器人里90%的点到点运动，底层用的就是这两套东西。
4.1 梯形速度规划：简单但够用
梯形速度曲线，顾名思义，速度-时间图是个梯形。加速段、匀速段、减速段，三段搞定。我习惯叫它「T型曲线」，因为形状像字母T。
4.1.1 核心参数与约束
做梯形规划前，你得先定好三个东西：
- 最大速度 v_max —— 电机或关节能跑的上限
- 最大加速度 a_max —— 电机力矩和结构刚度决定的
- 总位移 s —— 起点到终点的距离
有了这三个，就能算出加速时间和减速时间。但有个坑：如果位移太短，速度还没到 v_max 就得开始减速。这种情况叫「三角型曲线」——没有匀速段。
关键判断条件：
如果 s ≥ v_max² / a_max，梯形成立；否则，退化为三角形。
4.1.2 分段公式与实现
梯形规划的分段公式其实很直观。我直接给代码，你一看就懂：
// 梯形速度规划：位置、速度、加速度计算
// 输入：总位移 s，最大速度 v_max，最大加速度 a_max，当前时间 t
// 输出：位置 pos，速度 vel，加速度 acc
double ta = v_max / a_max; // 加速时间
double td = v_max / a_max; // 减速时间
double sa = 0.5 * a_max * ta * ta; // 加速段位移
double sd = sa; // 减速段位移
// 检查是否退化为三角形
if (sa + sd > s) {
// 三角形规划：没有匀速段
ta = sqrt(s / a_max);
td = ta;
v_max = a_max * ta;
}
double tv = (s - sa - sd) / v_max; // 匀速段时间
double T = ta + tv + td; // 总时间
if (t <= ta) {
// 加速段
acc = a_max;
vel = a_max * t;
pos = 0.5 * a_max * t * t;
} else if (t <= ta + tv) {
// 匀速段
acc = 0;
vel = v_max;
pos = sa + v_max * (t - ta);
} else if (t <= T) {
// 减速段
double t_dec = t - ta - tv;
acc = -a_max;
vel = v_max - a_max * t_dec;
pos = sa + v_max * tv + v_max * t_dec - 0.5 * a_max * t_dec * t_dec;
} else {
// 结束
acc = 0;
vel = 0;
pos = s;
}我的习惯：实际项目中，我会在梯形规划前加一个「最小位移检查」。如果 s 小于某个阈值（比如 0.001 rad），直接跳过规划，输出零速。这样可以避免除零和数值震荡。
4.1.3 梯形规划的局限性
梯形规划最大的问题：加速度在切换点突变。加速段结束瞬间，加速度从 a_max 跳到 0；减速段开始瞬间，又从 0 跳到 -a_max。这种突变会产生冲击力，轻则抖动，重则损坏机械结构。
我记得有一次做高速分拣机器人，梯形规划跑 2m/s 的传送带跟踪，结果末端执行器在加减速切换点有明显的「咯噔」声。后来换成 S 形规划，声音就没了。
4.2 S形速度规划：平滑但复杂
S形速度曲线，本质上是给加速度加了一个「缓变」过程。加速度不再跳变，而是线性变化。这样速度曲线就从折线变成了 S 形。
4.2.1 七段式结构
完整的 S 形规划有七段：
- 加加速段：加速度从 0 增加到 a_max
- 匀加速段：加速度保持 a_max
- 减加速段：加速度从 a_max 降到 0
- 匀速段：速度保持 v_max
- 加减速段：加速度从 0 降到 -a_max
- 匀减速段：加速度保持 -a_max
- 减减速段：加速度从 -a_max 升到 0
你想想看，七段听起来多，但核心就三个参数：最大速度 v_max、最大加速度 a_max、最大加加速度 j_max（也叫 jerk）。
注意：加加速度 j_max 是 S 形规划的灵魂。它决定了加速度变化的快慢。j_max 越大，曲线越接近梯形；j_max 越小，曲线越「软」。我一般建议 j_max 取 a_max 的 5~10 倍，具体看机械刚度。
4.2.2 参数计算与退化解
S 形规划的退化情况比梯形多得多。我整理了一张表，方便你对照：
| 条件 | 退化形态 | 说明 |
|---|---|---|
| 位移足够大 | 完整七段 | 所有段都存在 |
| 位移中等 | 五段（无匀加速/匀减速） | 加速度刚升到 a_max 就得降 |
| 位移较小 | 三段（只有加/减加速段） | 速度都到不了 v_max |
| 位移极小 | 两段（直接加减速） | 加速度还没升到 a_max 就开始降 |
我曾经在调试一个短行程（5mm）的直线模组时，忘了处理退化情况，结果规划出来的时间居然是负数。嗯，从那以后我就在代码里加了一个「退化检测」函数，先判断位移属于哪种情况，再分配段时间。
4.2.3 核心实现框架
S 形规划的代码比梯形长不少，但逻辑是清晰的。我给出核心框架：
// S形速度规划：核心参数计算
// 输入：s, v_max, a_max, j_max
// 输出：各段时间 ta1, ta2, ta3, tv, td1, td2, td3
// 1. 计算加/减速段能达到的最大加速度
double ta1 = a_max / j_max; // 加加速段时间
double sa1 = (1.0/6.0) * j_max * ta1 * ta1 * ta1;
// 2. 检查是否能达到 a_max
if (sa1 * 2 > s) {
// 退化：加速度达不到 a_max
// 重新计算 ta1，使位移匹配
ta1 = pow(s / (2.0 * j_max), 1.0/3.0);
// 此时没有匀加速/匀减速段
ta2 = 0;
tv = 0;
td2 = 0;
td1 = ta1;
td3 = ta1;
} else {
// 3. 检查是否能达到 v_max
double v_ramp = 0.5 * a_max * ta1 + a_max * ta1; // 近似
// 实际计算更复杂，这里简化
// ...
}避坑指南：我曾经在 S 形规划里用浮点数直接比较相等，结果因为精度问题，匀速段时间算出来是 -1e-12。后来我统一用「if (tv < 1e-9) tv = 0」来处理。浮点数比较，一定要给容差。
4.3 梯形 vs S形：怎么选？
很多初学者会问：到底用哪个？我的建议是：
- 对冲击不敏感的场景（如大型重载机器人、低速搬运）：梯形就够了，计算量小，实时性好。
- 对平滑性要求高的场景（如高速分拣、精密装配、医疗机器人）：必须上 S 形，否则抖动会毁掉精度。
- 实时性受限的嵌入式系统：梯形更友好。S 形在退化判断上需要更多分支，容易出 bug。
我个人习惯是：默认用 S 形，但把 j_max 设大一点。这样既保留了平滑性，又不会让曲线太「软」导致时间过长。如果发现计算超时，再切回梯形。
4.4 知识体系总览
下面这张图，把本章的核心逻辑串起来了。你可以把它当作一个速查地图：
4.5 工程落地要点
最后，我总结几个工程中容易踩的坑：
- 浮点数精度：规划中大量使用除法、开方、三次方，浮点误差会累积。建议所有中间变量用 double，最终输出前做一次限幅。
- 时间同步：多轴联动时，每个轴独立规划，但总时间必须对齐。我习惯先算出所有轴的时间，取最大值，然后让短轴「等」长轴。
- 实时性保障：S 形规划的退化判断分支多，建议在离线阶段预计算好各段时间，在线只做插值查表。
一句话总结：梯形规划是「快」，S形规划是「稳」。选哪个，看你的机器人是去搬砖还是去绣花。