3. S形速度规划：原理与数学公式
做运动控制这些年，我见过太多工程师一上来就怼梯形速度曲线。梯形简单，但启动和停止那一下的冲击，真的让人头疼。我记得有一次调试一台高速贴片机，梯形加减速导致末端抖动，贴片精度死活上不去。后来换成S形曲线，问题立马解决。
S形速度规划，说白了就是让速度变化本身也变得平滑。加速度不再突变，而是连续变化。你想想看，电机启动时电流不会瞬间拉满，机械间隙也不会被猛冲撞开——这就是S曲线的价值。
3.1 为什么需要S形曲线？
梯形速度规划有个硬伤：加速度在切换点处跳变。从0加速到最大加速度，中间没有过渡。这会导致什么？
- 冲击力大：加速度突变产生无限大的加加速度（Jerk），机械结构承受冲击
- 定位精度差：末端容易过冲，需要更长的稳定时间
- 电机寿命短：电流突变，驱动器负担重，电机发热严重
S形曲线通过限制加加速度（Jerk），让加速度连续变化。嗯，这里要注意：加加速度是加速度的变化率，单位是 m/s³。说白了就是「加速度变化得有多快」。
核心思想：S形曲线把梯形曲线的「加速度突变」变成了「加速度渐变」。代价是总运动时间变长，但换来了平滑性和精度。
3.2 七段式S曲线详解
标准的S形速度规划分为7个阶段。我习惯叫它「七段式S曲线」。为什么是七段？因为加加速度有正、负、零三种状态，组合起来刚好七段。
先看这张图，把七段结构刻在脑子里：
七段分别是：
- 加加速段：加加速度为正，加速度从0线性增加到最大值
- 匀加速段：加加速度为0，加速度保持最大值
- 减加速段：加加速度为负，加速度从最大值线性减小到0
- 匀速段：加速度为0，速度保持最大值
- 加减速段：加加速度为负，加速度从0线性减小到负最大值
- 匀减速段：加加速度为0，加速度保持负最大值
- 减减速段：加加速度为正，加速度从负最大值线性增加到0
我的经验：实际项目中，匀加速段和匀减速段不一定存在。如果位移距离不够，S曲线会退化成「五段式」甚至「三段式」。我调试一台小型机械臂时就遇到过这种情况——行程太短，匀加速段直接被压缩没了。
3.3 数学公式推导
搞懂了七段结构，接下来就是数学公式。别怕，其实不复杂。我们定义几个参数：
| 符号 | 含义 | 单位 |
|---|---|---|
| J | 加加速度（Jerk） | m/s³ |
| A | 最大加速度 | m/s² |
| V | 最大速度 | m/s |
| T₁~T₇ | 各段持续时间 | s |
| S | 总位移 | m |
以加加速段（第1段）为例，假设加加速度为常数 J：
// 加加速段 (0 ≤ t ≤ T₁)
j(t) = J
a(t) = J · t
v(t) = v₀ + ½ · J · t²
s(t) = s₀ + v₀ · t + ⅙ · J · t³匀加速段（第2段）就简单了，加加速度为0：
// 匀加速段 (T₁ ≤ t ≤ T₁+T₂)
j(t) = 0
a(t) = A
v(t) = v₁ + A · t
s(t) = s₁ + v₁ · t + ½ · A · t²减加速段（第3段）加加速度为 -J：
// 减加速段 (T₁+T₂ ≤ t ≤ T₁+T₂+T₃)
j(t) = -J
a(t) = A - J · t
v(t) = v₂ + A · t - ½ · J · t²
s(t) = s₂ + v₂ · t + ½ · A · t² - ⅙ · J · t³后面的减速三段（第5~7段）是对称的，只是加速度方向相反。我这里就不全写了，你照着对称关系推导就行。
关键约束条件：
- 速度不能超过 V_max
- 加速度不能超过 A_max
- 加加速度不能超过 J_max
- 各段时间必须 ≥ 0
3.4 代码实现
理论讲完了，上代码。我习惯用C语言写运动控制库，这里给一个简化版的S曲线规划函数：
typedef struct {
double J; // 加加速度
double A; // 最大加速度
double V; // 最大速度
double T[7]; // 七段时间
double t_total;// 总时间
} SCurveProfile;
int SCurve_Plan(double s_total, double v_max, double a_max, double jerk, SCurveProfile *profile) {
// 参数检查
if (s_total <= 0 || v_max <= 0 || a_max <= 0 || jerk <= 0) return -1;
// 计算加加速段时间（假设对称，T1=T3=T5=T7）
double T1 = a_max / jerk;
double T2 = 0, T4 = 0;
// 计算加加速段位移
double s_acc = 2 * T1 * v_max - (a_max * T1 * T1) / jerk;
// 判断是否能达到最大速度
if (s_total >= 2 * s_acc) {
// 能达到最大速度，有匀速段
T2 = (s_total - 2 * s_acc) / v_max;
T4 = T2; // 匀速段时间
} else {
// 达不到最大速度，需要重新计算
// 这里简化处理，实际需要迭代求解
T2 = 0;
T4 = 0;
}
// 填充profile
profile->J = jerk;
profile->A = a_max;
profile->V = v_max;
profile->T[0] = T1; // 加加速
profile->T[1] = T2; // 匀加速
profile->T[2] = T1; // 减加速
profile->T[3] = T4; // 匀速
profile->T[4] = T1; // 加减速
profile->T[5] = T2; // 匀减速
profile->T[6] = T1; // 减减速
profile->t_total = 4*T1 + 2*T2 + T4;
return 0;
}注意：上面的代码是简化版，只处理了「能达到最大速度」的情况。实际项目中，当位移距离不足时，需要做迭代求解。我曾经在写一个通用运动库时，花了整整两天才把边界情况全部覆盖——特别是那种「刚好卡在临界点」的情况，最容易出bug。
3.5 平滑性分析
S形曲线到底有多平滑？我们对比一下梯形和S形的关键指标：
| 指标 | 梯形曲线 | S形曲线 | 改善效果 |
|---|---|---|---|
| 加速度连续性 | 不连续（跳变） | 连续 | 冲击力大幅降低 |
| 加加速度 | 无穷大（理论） | 有限值（可控） | 机械振动减少 |
| 最大冲击力 | 高 | 低 | 降低50%~80% |
| 运动时间 | 短 | 略长（约10%~20%） | 平滑性换时间 |
| 定位精度 | 一般 | 高 | 过冲减少 |
从数学上看，S形曲线的速度函数是二阶连续可导的（C²连续），而梯形曲线只有一阶连续（C¹连续）。这意味着什么？
- C¹连续：速度连续，加速度有跳变
- C²连续：速度和加速度都连续，加加速度有跳变
实际应用中，C²连续已经足够平滑。如果你需要更高阶的平滑（比如C³连续），那就得用更高阶的多项式曲线了——不过说实话，我做了十几年运动控制，C²连续在99%的场景下都够用。
避坑指南：我曾经在一个项目中把加加速度设得太小，结果S曲线变得「太软」，运动时间拉长了一倍。后来我总结了一个经验：加加速度一般设为最大加速度的5~10倍。比如最大加速度是10 m/s²，加加速度设在50~100 m/s³之间比较合理。
3.6 小结
S形速度规划的核心就三点：
- 七段结构：加加速→匀加速→减加速→匀速→加减速→匀减速→减减速
- 数学公式：每段都是关于时间的多项式，最高到三次
- 平滑性：C²连续，冲击力可控，定位精度高
下功夫把S曲线吃透，后面讲轨迹规划联动时你会轻松很多。毕竟，速度规划是轨迹规划的「内功」，内功练好了，招式才能打得漂亮。