一、S型速度规划：从原理到代码实现
大家好，我是你们的老朋友。今天我们来聊聊S型速度规划。
说实话，我刚入行那会儿，用的最多的就是梯形速度规划。简单、粗暴、有效。但有一次做高精度焊接机器人项目，梯形曲线在加减速切换点产生的冲击，直接把焊枪抖偏了零点几毫米。嗯，从那以后，我就彻底转向了S型曲线。
1.1 为什么需要S型曲线？
梯形速度规划的问题在哪？说白了，就是加速度会突变。加速度从0瞬间跳到最大值，又从最大值瞬间跳回0。这种突变会产生无穷大的加加速度（Jerk），在物理系统中表现为冲击和振动。
你想想看，一个机械臂末端带着几公斤的负载，加速度突然变化，那冲击力可不是闹着玩的。我在做半导体封装设备时，就遇到过因为加速度突变导致芯片贴装偏移的案例。
S型曲线解决了这个问题——它让加速度平滑变化，而不是跳变。
1.2 加加速度（Jerk）约束
加加速度，英文叫Jerk，是加速度的变化率。数学表达式很简单：
Jerk = da/dt = d²v/dt² = d³s/dt³单位是 m/s³ 或者 rad/s³。
为什么要约束Jerk？我举个例子你就明白了。假设你坐电梯，梯形速度规划就像电梯突然启动、突然停止——你会感到明显的「推背感」和「失重感」。而S型曲线就像高级电梯，启动和停止都特别平缓，你几乎感觉不到。
核心思想： 约束Jerk，就是约束加速度的变化率，从而让运动更平滑。
在实际项目中，Jerk约束通常由电机驱动器或机械结构的物理限制决定。我记得有一次调试六轴机器人，电机驱动器手册上明确写着最大Jerk不能超过500 rad/s³，否则会触发过流保护。
1.3 七段式S曲线详解
标准的S型速度规划分为七段。我画了一张图，帮你直观理解：
七段分别是：
- 加加速度段（t₀→t₁）：加速度从0线性增加到最大值，Jerk为正常数
- 匀加速段（t₁→t₂）：加速度保持最大值，Jerk为0
- 减加速度段（t₂→t₃）：加速度从最大值线性减小到0，Jerk为负常数
- 匀速段（t₃→t₄）：速度保持最大值，加速度和Jerk均为0
- 加加速度段（t₄→t₅）：加速度从0线性减小到负最大值，Jerk为负常数
- 匀减速段（t₅→t₆）：加速度保持负最大值，Jerk为0
- 减加速度段（t₆→t₇）：加速度从负最大值线性增加到0，Jerk为正常数
个人经验： 并不是所有运动都需要完整的七段。如果位移距离较短，可能没有匀速段（第4段），变成六段。如果距离更短，可能连匀加速段都没有，变成四段。我在做桌面级机械臂时，经常遇到这种情况。
1.4 数学描述
七段式S曲线的数学描述其实不复杂。我们以第一段（加加速度段）为例：
// 第一段：加加速度段 (0 ≤ t ≤ T₁)
// Jerk = J_max (常数)
// 加速度：a(t) = J_max * t
// 速度：v(t) = v₀ + ½ * J_max * t²
// 位置：s(t) = s₀ + v₀ * t + ⅙ * J_max * t³其他段落的公式类似，只是初始条件和Jerk符号不同。我把完整的公式整理成表格：
| 段落 | 时间区间 | Jerk | 加速度 a(t) | 速度 v(t) |
|---|---|---|---|---|
| ①加加速度 | [t₀, t₁] | +J_max | J_max·t | v₀ + ½J_max·t² |
| ②匀加速 | [t₁, t₂] | 0 | a_max | v₁ + a_max·t |
| ③减加速度 | [t₂, t₃] | -J_max | a_max - J_max·t | v₂ + a_max·t - ½J_max·t² |
| ④匀速 | [t₃, t₄] | 0 | 0 | v_max |
| ⑤加加速度 | [t₄, t₅] | -J_max | -J_max·t | v_max - ½J_max·t² |
| ⑥匀减速 | [t₅, t₆] | 0 | -a_max | v₅ - a_max·t |
| ⑦减加速度 | [t₆, t₇] | +J_max | -a_max + J_max·t | v₆ - a_max·t + ½J_max·t² |
注意： 上表中的t是相对于各段起始时间的相对时间，不是绝对时间。写代码时一定要小心这个细节，我曾经在这里吃过亏——用绝对时间代入公式，结果位置曲线出现了不连续。
1.5 代码实现
好了，理论讲完了，咱们来点实际的。下面是一个完整的S型速度规划C语言实现：
#include <stdio.h>
#include <math.h>
typedef struct {
double t0, t1, t2, t3, t4, t5, t6, t7; // 各段时间节点
double v0, v_max; // 初速度和最大速度
double a_max; // 最大加速度
double j_max; // 最大加加速度
double s0; // 初始位置
} SCurveParams;
// 计算S型曲线在时间t时的位置、速度、加速度
void s_curve_calc(SCurveParams *p, double t,
double *s, double *v, double *a) {
double dt;
if (t <= p->t1) {
// 第1段：加加速度段
dt = t - p->t0;
*a = p->j_max * dt;
*v = p->v0 + 0.5 * p->j_max * dt * dt;
*s = p->s0 + p->v0 * dt + (1.0/6.0) * p->j_max * dt * dt * dt;
}
else if (t <= p->t2) {
// 第2段：匀加速段
dt = t - p->t1;
*a = p->a_max;
*v = p->v0 + 0.5 * p->j_max * (p->t1 - p->t0) * (p->t1 - p->t0)
+ p->a_max * dt;
// 位置计算略...
}
// ... 其他段落类似
// 实际项目中，我会把七段全部展开
// 这里为了简洁只展示前两段
}我的习惯： 实际项目中，我更喜欢用查表法来实现S型曲线。预先计算好每个时间点的位置、速度、加速度，存成数组。运行时直接查表插值，效率高很多。特别是对于嵌入式系统，能省不少CPU算力。
1.6 避坑指南
做S型曲线规划，有几个坑我踩过，分享给你：
- 时间参数计算错误：各段时间必须满足约束条件，比如加加速度段时间T₁ = a_max / j_max。如果算错了，曲线就不连续。
- 边界条件处理：当位移距离太短时，可能无法达到最大速度或最大加速度。这时候需要降阶处理，变成六段或四段S曲线。
- 浮点数精度：在嵌入式平台上，float精度可能不够。我建议用double，或者做定点数处理。
我曾经踩过的坑： 有一次在STM32上实现S型曲线，用了float类型。结果在长时间运行时，累积误差导致位置偏差达到了毫米级。后来换成double，问题就解决了。所以，精度问题真的不能忽视。
好了，S型速度规划的原理、数学描述和代码实现就讲到这里。这东西看着复杂，其实核心就三点：约束Jerk、分段计算、边界处理。掌握了这三点，你就能写出平滑的运动轨迹了。