S曲线规划停止运动
S曲线停止运动的实现
在运动控制系统中，简单的急停会带来冲击和振动。S曲线停止运动（S-Curve Stop）通过在减速阶段平滑过渡加速度，让电机或运动部件优雅地停下来。本文将拆解核心算法，并给出代码框架。
1. S曲线停止原理
与梯形速度曲线不同，S曲线在减速阶段的加速度不是突变的，而是呈线性变化（加加速度 Jerk 恒定）。这样可以：
- 减少机械冲击
- 降低定位过冲
- 提升运动平顺性
典型的 S 曲线减速过程分为三个阶段：
- 减加速度段— 加速度从 0 线性降至最大负加速度
- 恒减速段— 以最大负加速度匀速减速
- 加速度回升段— 加速度从最大负加速度线性回升至 0
2. 核心参数
| 参数 | 含义 | 单位 |
|---|---|---|
v0 | 当前速度 | count/s |
Amax | 最大加速度 | count/s² |
J | 加加速度 (Jerk) | count/s³ |
T1 | 减加速度段时间 | s |
T2 | 恒减速段时间 | s |
T3 | 加速度回升段时间 | s |
3. 公式推导
假设从速度v0v_0v0减速到 0，加加速度恒定为JJJ：
减加速度段（0≤t<T10 \le t < T_10≤t<T1）：
a(t)=−J⋅ta(t) = -J \cdot ta(t)=−J⋅t
v(t)=v0−12Jt2v(t) = v_0 - \frac{1}{2} J t^2v(t)=v0−21Jt2恒减速段（T1≤t<T1+T2T_1 \le t < T_1 + T_2T1≤t<T1+T2）：
a(t)=−Amaxa(t) = -A_{max}a(t)=−Amax
v(t)=v(T1)−Amax⋅(t−T1)v(t) = v(T_1) - A_{max} \cdot (t - T_1)v(t)=v(T1)−Amax⋅(t−T1)加速度回升段（T1+T2≤t<T1+T2+T3T_1 + T_2 \le t < T_1 + T_2 + T_3T1+T2≤t<T1+T2+T3）：
a(t)=−Amax+J⋅(t−T1−T2)a(t) = -A_{max} + J \cdot (t - T_1 - T_2)a(t)=−Amax+J⋅(t−T1−T2)
v(t)v(t)v(t)持续减小直至归零
实际实现中，通常用离散的方式在每个控制周期计算当前目标速度。
4. 代码实现
# -*- coding: utf-8 -*-importnumpyasnpfrommatplotlibimportpyplotaspltdefstop(q,v,a,a_max,j_max,step=0.0001):T_trans=0.0sign_a=0.0v1=v q1=qifabs(a)>1e-6:T_trans=abs(a)/j_max sign_a=1.0ifa>0else-1.0v1=v+a*T_trans-sign_a*j_max*T_trans**2/2q1=q+v*T_trans+a*T_trans**2/2-sign_a*j_max*T_trans**3/6v_abs=abs(v1)sign_v=1.0ifv1>0else(-1.0ifv1<0else0.0)ifv_abs<1e-6:T_decel=0.0Tj=0.0reached_a_max=Falseq_stop=q1elifv_abs*j_max<a_max**2:Tj=np.sqrt(v_abs/j_max)T_decel=2*Tj reached_a_max=Falseq_stop=q1+sign_v*v_abs*Tjelse:Tj=a_max/j_max T_decel=Tj+v_abs/a_max reached_a_max=Trueq_stop=q1+sign_v*v_abs*(v_abs/(2*a_max)+Tj/2)T_total=T_trans+T_decelifT_total<1e-6:return[q],[v],[0.0],[0.0],q t_list=np.arange(0,T_total+step,step)q_list,v_list,a_list=[],[],[]fortint_list:ifT_trans>0andt<T_trans:a_t=a-sign_a*j_max*t v_t=v+a*t-sign_a*j_max*t**2/2q_t=q+v*t+a*t**2/2-sign_a*j_max*t**3/6elifT_decel<1e-6:a_t=0.0v_t=v1 q_t=q1else:td=t-T_transiftd>=T_decel:a_t=0.0v_t=0.0q_t=q_stopelifnotreached_a_max:iftd<Tj:a_t=-sign_v*j_max*td v_t=v1-sign_v*j_max*td**2/2q_t=q1+v1*td-sign_v*j_max*td**3/6else:tau=T_decel-td a_t=-sign_v*j_max*tau v_t=sign_v*j_max*tau**2/2q_t=q_stop-sign_v*j_max*tau**3/6else:iftd<Tj:a_t=-sign_v*j_max*td v_t=v1-sign_v*j_max*td**2/2q_t=q1+v1*td-sign_v*j_max*td**3/6eliftd<T_decel-Tj:t_const=td-Tj v_const=v1-sign_v*a_max*Tj/2q_const=q1+v1*Tj-sign_v*j_max*Tj**3/6a_t=-sign_v*a_max v_t=v_const-sign_v*a_max*t_const q_t=q_const+v_const*t_const-sign_v*a_max*t_const**2/2else:tau=T_decel-td a_t=-sign_v*j_max*tau v_t=sign_v*j_max*tau**2/2q_t=q_stop-sign_v*j_max*tau**3/6q_list.append(q_t)v_list.append(v_t)a_list.append(a_t)returnq_list,v_list,a_list,t_list,q_stopif__name__=="__main__":a_max=200000j_max=2000000test_cases=[(0,10000,0,"v=10000, a=0"),(1000,20000,10000,"v=20000, a=10000"),(0,30000,0,"v=30000, a=0"),(0,10000,100000,"v=10000, a=100000"),(0,10000,-100000,"v=10000, a=-100000"),(0,-10000,-100000,"v=-10000, a=-100000"),]print("a_max={:.0f}, j_max={:.0f}".format(a_max,j_max))print("="*65)print("{:<30}{:>8}{:>12}{:>12}".format("场景","T(s)","q_stop","v_end"))print("-"*65)plt.figure(figsize=(14,10))plt.suptitle("Jerk-limited Stop Trajectory",fontsize=14)fori,(q,v,a,label)inenumerate(test_cases):ql,vl,al,tl,q_stop=stop(q,v,a,a_max,j_max)print("{:<30}{:>8.4f}{:>12.2f}{:>12.4f}".format(label,tl[-1],q_stop,vl[-1]))plt.subplot(3,2,i+1)plt.plot(tl,ql,'r',linewidth=1.2,label='pos')plt.plot(tl,vl,'g',linewidth=1.2,label='vel')plt.axhline(y=0,color='gray',linestyle='--',alpha=0.4)plt.title(label)plt.xlabel("Time (s)")plt.legend(loc='best',fontsize=8)plt.grid(True,alpha=0.3)print("="*65)plt.tight_layout()plt.show()5. 总结
S曲线停止运动通过控制加加速度实现了平滑减速，是工业运动控制中的常用策略。你可以在上方代码块中填入自己的实现，结合具体硬件平台进行调试。