4、S形速度规划：原理、七段式S曲线、实现方法
速度规划这事儿，说白了就是让机器人「走得稳、停得准」。梯形速度规划虽然简单，但启动和停止时的冲击太大，电机容易抖，末端执行器也容易晃。我当年调试一个高速拾取项目，梯形加减速一启动，整个机架都在共振——从那以后，我就老老实实用S形了。
4.1 为什么要用S形速度规划？
梯形规划的加速度是突变的。启动瞬间，加速度从0直接跳到最大值，这会产生无穷大的加加速度（Jerk）。你想想看，物理世界哪有这么刚性的变化？
S形规划的核心思想：让加速度也平滑变化。这样加加速度就是有限值，运动过程自然柔和。
关键指标对比：
| 指标 | 梯形规划 | S形规划 |
|---|---|---|
| 加速度变化 | 阶跃突变 | 连续平滑 |
| 加加速度 | 无穷大 | 有限值（可设定） |
| 冲击力 | 大 | 小 |
| 适用场景 | 低速、低精度 | 高速、高精度 |
4.2 七段式S曲线原理
标准的S形速度规划分为七段。我习惯叫它「三段加速、一段匀速、三段减速」。为什么是七段？因为加速度的变化需要「上升-保持-下降」三个过程。
来看这七段分别是什么：
- 加加速段（T1）：加速度从0线性增加到最大值。速度曲线是二次抛物线。
- 匀加速段（T2）：加速度保持最大值。速度线性上升。
- 减加速段（T3）：加速度从最大值线性减小到0。速度曲线再次变弯。
- 匀速段（T4）：加速度为0，速度保持最大值。
- 加减速段（T5）：加速度从0反向增加到负最大值。速度开始下降。
- 匀减速段（T6）：加速度保持负最大值。速度线性下降。
- 减减速段（T7）：加速度从负最大值线性回到0。速度平滑归零。
我的经验：实际项目中，T1、T3、T5、T7这四个段的时间通常设为相等（对称S形）。这样参数少，好调。如果非要不对称，那就要分别设定加加速度值，调试起来很痛苦。
4.3 核心公式与参数关系
嗯，这里要上公式了。别怕，其实就三个核心参数：最大速度Vmax、最大加速度Amax、加加速度J。
七段式S曲线的位移公式可以统一写成：
// 加加速段（0 ≤ t ≤ T1）
a(t) = J * t
v(t) = v0 + 0.5 * J * t²
s(t) = s0 + v0*t + (1/6)*J*t³
// 匀加速段（T1 ≤ t ≤ T1+T2）
a(t) = Amax
v(t) = v1 + Amax * (t - T1)
s(t) = s1 + v1*(t-T1) + 0.5*Amax*(t-T1)²
// 减加速段（T1+T2 ≤ t ≤ T1+T2+T3）
a(t) = Amax - J*(t - T1 - T2)
v(t) = v2 + Amax*(t-T1-T2) - 0.5*J*(t-T1-T2)²
s(t) = s2 + v2*(t-T1-T2) + 0.5*Amax*(t-T1-T2)² - (1/6)*J*(t-T1-T2)³
// 匀速段、减速段类似，对称即可
看着复杂？其实你只需要记住：位移是速度的积分，速度是加速度的积分，加速度是加加速度的积分。一层层往上积分，就得到了S形曲线。
注意：当运动距离太短时，可能无法达到最大速度或最大加速度。这时候需要降阶处理——从七段变成五段甚至三段。我曾经在调试一个短行程点焊机器人时踩过这个坑，算出来的轨迹还没加速完就要减速了，结果电机一顿乱抖。
4.4 七段式S曲线的实现方法
实际工程中，我们一般分三步走：
4.4.1 第一步：参数计算
给定总位移S、最大速度Vmax、最大加速度Amax、加加速度J。先判断能否达到Vmax：
// 判断是否能达到最大速度
double T1 = Amax / J; // 加加速段时间
double T2 = (Vmax / Amax) - T1; // 匀加速段时间
if (T2 < 0) {
// 达不到最大速度，需要重新计算
// 这种情况叫「三角型S曲线」
}
4.4.2 第二步：时间分配
我习惯用一个结构体来管理七段时间：
typedef struct {
double T[7]; // 七段时间
double J; // 加加速度
double Amax; // 最大加速度
double Vmax; // 最大速度
double S; // 总位移
} SCurveProfile;
// 对称S形：T1=T3=T5=T7, T2=T6
void CalculateSCurve(SCurveProfile *profile) {
double T1 = profile->Amax / profile->J;
double T2 = (profile->Vmax / profile->Amax) - T1;
// 检查是否有效
if (T2 < 0) {
// 降阶处理：五段式或三段式
HandleShortDistance(profile);
return;
}
profile->T[0] = T1; // 加加速
profile->T[1] = T2; // 匀加速
profile->T[2] = T1; // 减加速
profile->T[3] = (profile->S - 2*T1*profile->Vmax - T2*profile->Vmax) / profile->Vmax;
profile->T[4] = T1; // 加减速
profile->T[5] = T2; // 匀减速
profile->T[6] = T1; // 减减速
}
4.4.3 第三步：实时插补
在运动控制周期（比如1ms）里，根据当前时间t，查表计算位置指令：
double GetPosition(SCurveProfile *profile, double t) {
double totalTime = 0;
for(int i=0; i<7; i++) totalTime += profile->T[i];
if(t <= profile->T[0]) {
// 加加速段
return (1.0/6.0) * profile->J * t * t * t;
}
// ... 其他段类似，用分段函数计算
// 实际工程中建议用查表法，提前算好位置表
}
工程技巧：别在实时中断里算这些三次方！我建议提前把整条轨迹的位置-时间表算好，存成数组。运行时直接查表+线性插值，又快又稳。1ms周期查1000个点的表，CPU占用几乎为零。
4.5 知识体系总览
下面这张图，是我做S形规划时脑子里始终绷着的一根弦：
4.6 避坑指南
我曾经在调试一个六轴机器人时，遇到过几个典型问题，分享给你：
- 短距离振荡：位移太短时，七段式退化成三段式，但加加速度没跟着调整。结果电机在终点附近来回震荡。解决方案是动态检测距离，自动降阶。
- 浮点精度问题：三次方运算累积误差，导致终点位置偏差。我后来改用定点数+查表，精度从0.1mm提升到0.01mm。
- 多轴同步：每个轴独立做S形规划，结果各轴到达时间不一致。必须用主从同步或者时间同步算法。
重要提醒：S形规划不是万能的。对于超高速运动（比如每秒几米），加加速度J如果设得太小，加减速时间会很长，反而影响效率。这时候需要权衡——是追求平滑，还是追求节拍？我一般先按J=Amax*10来试，再根据实测振动微调。
好了，S形速度规划的核心就这些。七段式是最通用的形式，掌握了它，五段式、三段式都是它的特例。实际项目中，建议先从对称S形入手，跑通了再考虑非对称优化。
公众号:蓝海资料掘金营，微信deep3321