4. S形速度规划：七段式S曲线原理与参数计算
大家好，我是你们的老朋友。今天我们来聊聊机器人轨迹规划里一个绕不开的话题——S形速度规划。
说实话，我刚入行那会儿，用的最多的就是梯形速度规划。简单、直观、好算。但有一次做高速码垛项目，机器人一启动就抖得跟筛糠似的，末端执行器上的工件差点飞出去。我查了半天，最后发现是加速度突变惹的祸。从那以后，我对S形曲线就再也不敢马虎了。
4.1 为什么需要S形曲线？
梯形速度规划的问题在哪？说白了，就是加速度不连续。启动瞬间加速度从0跳到最大值，结束瞬间又从最大值跳回0。这种突变会产生无穷大的加加速度（Jerk），物理上不可能实现，实际表现就是机械振动和冲击。
你想想看，电机和减速器都是有惯性的。突然要求它们瞬间改变加速度，就像你开车时一脚油门踩到底又一脚刹车踩到底——车上的乘客肯定受不了。机器人也是一样，尤其是精密装配或高速搬运场景，这种冲击轻则影响定位精度，重则损坏机械结构。
S形曲线的核心思想，就是让加速度也平滑变化。它把整个运动过程分成七段，每一段都有明确的物理意义。这样加加速度就是有限值，机械系统受到的冲击大大减小。
核心结论：S形曲线通过控制加加速度（Jerk）为常数，实现了加速度的连续变化，从而消除冲击。这是高性能机器人运动控制的基石。
4.2 七段式S曲线的结构
七段式S曲线，顾名思义，整个运动过程被分成7个阶段。我习惯用一张图来理解它，这样最直观。
这七段分别是：
- 加加速段（t₀→t₁）：加速度从0开始，以恒定加加速度J增加。速度曲线呈抛物线上升。
- 匀加速段（t₁→t₂）：加速度达到最大值A_max并保持。速度线性增加。
- 减加速段（t₂→t₃）：加速度从A_max以恒定-J减小到0。速度曲线呈抛物线上升，但曲率方向改变。
- 匀速段（t₃→t₄）：加速度为0，速度保持最大值V_max。
- 加减速段（t₄→t₅）：加速度从0以恒定-J减小（即反向加速）。速度开始下降，呈抛物线。
- 匀减速段（t₅→t₆）：加速度达到反向最大值-A_max并保持。速度线性下降。
- 减减速段（t₆→t₇）：加速度从-A_max以恒定J增加到0。速度曲线呈抛物线下降至0。
我的小技巧：记不住七段顺序？你只需要记住「加速三兄弟」和「减速三兄弟」，中间夹个匀速段。加速三兄弟分别是：加加速、匀加速、减加速。减速同理。这样就好记多了。
4.3 参数计算的核心公式
搞清楚了结构，接下来就是算参数了。七段式S曲线涉及几个关键参数：
| 参数 | 符号 | 单位 | 说明 |
|---|---|---|---|
| 总位移 | S | mm 或 rad | 需要运动的距离或角度 |
| 最大速度 | V_max | mm/s 或 rad/s | 系统允许的最高速度 |
| 最大加速度 | A_max | mm/s² 或 rad/s² | 系统允许的最大加速度 |
| 加加速度 | J | mm/s³ 或 rad/s³ | 加速度的变化率，S曲线的核心 |
| 各段时间 | T₁~T₇ | s | 每段持续的时间 |
嗯，这里要注意：并不是每次运动都能跑满七段。如果位移S太小，可能还没加速到V_max就要开始减速了。这时候就会退化成五段甚至三段S曲线。我待会儿会讲怎么判断。
4.4 完整七段式的参数计算
假设我们已知S、V_max、A_max、J，且位移足够大，能跑满七段。那么计算步骤如下：
第一步：计算加速段和减速段的时间
由于对称性，加速三段时间之和等于减速三段时间之和。每段时间为：
T₁ = T₃ = T₅ = T₇ = A_max / J
T₂ = T₆ = (V_max / A_max) - (A_max / J)这里T₂必须大于等于0。如果T₂ < 0，说明V_max太高，实际跑不到，需要重新计算。
第二步：计算加速段和减速段的总时间
T_acc = T₁ + T₂ + T₃ = (V_max / A_max) + (A_max / J)
T_dec = T_acc // 对称第三步：计算加速段和减速段的总位移
这个公式稍微复杂一点，我直接给结论：
S_acc = V_max * (V_max / A_max + A_max / J) / 2
S_dec = S_acc // 对称第四步：计算匀速段时间
T₄ = (S - S_acc - S_dec) / V_max如果T₄ >= 0，恭喜你，七段完整。如果T₄ < 0，说明位移不够，跑不到V_max，需要降速处理。
关键判断条件：
- T₂ >= 0 且 T₄ >= 0 → 完整七段
- T₂ >= 0 且 T₄ < 0 → 五段S曲线（无匀速段）
- T₂ < 0 → 三段S曲线（无匀加速/匀减速段）
4.5 代码实现示例
纸上得来终觉浅。我写了一个简单的Python函数，用来计算七段式S曲线的参数。你可以在自己的项目里直接套用这个框架。
def s_curve_plan(S, V_max, A_max, J):
"""
七段式S曲线参数计算
:param S: 总位移 (mm)
:param V_max: 最大速度 (mm/s)
:param A_max: 最大加速度 (mm/s²)
:param J: 加加速度 (mm/s³)
:return: 各段时间列表 [T1, T2, T3, T4, T5, T6, T7]
"""
# 1. 计算加加速段时间
T1 = A_max / J
# 2. 计算匀加速段时间
T2 = (V_max / A_max) - (A_max / J)
# 3. 判断是否降速
if T2 < 0:
# 三段式情况：达不到最大加速度
V_actual = A_max * (A_max / J) # 实际能达到的速度
T1 = A_max / J
T2 = 0
T3 = T1
T4 = 0
T5 = T1
T6 = 0
T7 = T1
# 这里需要重新计算实际位移，略
return [T1, T2, T3, T4, T5, T6, T7]
# 4. 计算加速段总位移
S_acc = V_max * (V_max / A_max + A_max / J) / 2
S_dec = S_acc
# 5. 计算匀速段时间
T4 = (S - S_acc - S_dec) / V_max
if T4 < 0:
# 五段式情况：达不到最大速度
# 需要求解实际能达到的速度V_actual
# 这里用二分法或解析法求解，略
pass
# 6. 对称赋值
T3 = T1
T5 = T1
T6 = T2
T7 = T1
return [T1, T2, T3, T4, T5, T6, T7]我曾经踩过的坑：有一次我直接套用七段公式，没检查T₂和T₄的正负，结果算出来的时间有负数。机器人执行时直接报错，差点把末端执行器撞坏。所以一定要先判断运动类型，再选择对应的公式。
4.6 实际项目中的经验
最后分享几个我在项目中积累的经验：
- J值怎么选？我一般取J = A_max / (0.1~0.2)。也就是说，加加速段占加速总时间的10%~20%。太短了冲击大，太长了运动效率低。
- 对称性不是必须的。有些场景加速和减速的A_max可以不同。比如抓取时加速可以快一点，放置时减速要慢一点。这时候七段就不对称了，但计算原理一样。
- 实时性要求高怎么办？如果是在嵌入式系统里跑，建议把公式离线算好，查表或者用多项式拟合。在线实时计算三角函数或开方，对MCU压力很大。
- 别忘了单位一致性。这个看起来是废话，但我真见过有人把mm和m混着用，结果算出来的速度是光速级别的。
好了，关于七段式S曲线的原理和参数计算，今天就聊到这里。记住一句话：S形规划的本质，是用加加速度的有限性换取加速度的连续性，从而消除冲击。理解了这个，你就能灵活应对各种轨迹平滑的需求。