S曲线规划停止运动

S曲线停止运动的实现在运动控制系统中简单的急停会带来冲击和振动。S曲线停止运动S-Curve Stop通过在减速阶段平滑过渡加速度让电机或运动部件优雅地停下来。本文将拆解核心算法并给出代码框架。1. S曲线停止原理与梯形速度曲线不同S曲线在减速阶段的加速度不是突变的而是呈线性变化加加速度 Jerk 恒定。这样可以减少机械冲击降低定位过冲提升运动平顺性典型的 S 曲线减速过程分为三个阶段减加速度段— 加速度从 0 线性降至最大负加速度恒减速段— 以最大负加速度匀速减速加速度回升段— 加速度从最大负加速度线性回升至 0匀速运动减加速度段Jerk -J恒减速段Acc -Amax加速度回升段Jerk J停止2. 核心参数参数含义单位v0当前速度count/sAmax最大加速度count/s²J加加速度 (Jerk)count/s³T1减加速度段时间sT2恒减速段时间sT3加速度回升段时间s3. 公式推导假设从速度v0v_0v0​减速到 0加加速度恒定为JJJ减加速度段0≤tT10 \le t T_10≤tT1​a(t)−J⋅ta(t) -J \cdot ta(t)−J⋅tv(t)v0−12Jt2v(t) v_0 - \frac{1}{2} J t^2v(t)v0​−21​Jt2恒减速段T1≤tT1T2T_1 \le t T_1 T_2T1​≤tT1​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​)加速度回升段T1T2≤tT1T2T3T_1 T_2 \le t T_1 T_2 T_3T1​T2​≤tT1​T2​T3​a(t)−AmaxJ⋅(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,step0.0001):T_trans0.0sign_a0.0v1v q1qifabs(a)1e-6:T_transabs(a)/j_max sign_a1.0ifa0else-1.0v1va*T_trans-sign_a*j_max*T_trans**2/2q1qv*T_transa*T_trans**2/2-sign_a*j_max*T_trans**3/6v_absabs(v1)sign_v1.0ifv10else(-1.0ifv10else0.0)ifv_abs1e-6:T_decel0.0Tj0.0reached_a_maxFalseq_stopq1elifv_abs*j_maxa_max**2:Tjnp.sqrt(v_abs/j_max)T_decel2*Tj reached_a_maxFalseq_stopq1sign_v*v_abs*Tjelse:Tja_max/j_max T_decelTjv_abs/a_max reached_a_maxTrueq_stopq1sign_v*v_abs*(v_abs/(2*a_max)Tj/2)T_totalT_transT_decelifT_total1e-6:return[q],[v],[0.0],[0.0],q t_listnp.arange(0,T_totalstep,step)q_list,v_list,a_list[],[],[]fortint_list:ifT_trans0andtT_trans:a_ta-sign_a*j_max*t v_tva*t-sign_a*j_max*t**2/2q_tqv*ta*t**2/2-sign_a*j_max*t**3/6elifT_decel1e-6:a_t0.0v_tv1 q_tq1else:tdt-T_transiftdT_decel:a_t0.0v_t0.0q_tq_stopelifnotreached_a_max:iftdTj:a_t-sign_v*j_max*td v_tv1-sign_v*j_max*td**2/2q_tq1v1*td-sign_v*j_max*td**3/6else:tauT_decel-td a_t-sign_v*j_max*tau v_tsign_v*j_max*tau**2/2q_tq_stop-sign_v*j_max*tau**3/6else:iftdTj:a_t-sign_v*j_max*td v_tv1-sign_v*j_max*td**2/2q_tq1v1*td-sign_v*j_max*td**3/6eliftdT_decel-Tj:t_consttd-Tj v_constv1-sign_v*a_max*Tj/2q_constq1v1*Tj-sign_v*j_max*Tj**3/6a_t-sign_v*a_max v_tv_const-sign_v*a_max*t_const q_tq_constv_const*t_const-sign_v*a_max*t_const**2/2else:tauT_decel-td a_t-sign_v*j_max*tau v_tsign_v*j_max*tau**2/2q_tq_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_max200000j_max2000000test_cases[(0,10000,0,v10000, a0),(1000,20000,10000,v20000, a10000),(0,30000,0,v30000, a0),(0,10000,100000,v10000, a100000),(0,10000,-100000,v10000, 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,fontsize14)fori,(q,v,a,label)inenumerate(test_cases):ql,vl,al,tl,q_stopstop(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,i1)plt.plot(tl,ql,r,linewidth1.2,labelpos)plt.plot(tl,vl,g,linewidth1.2,labelvel)plt.axhline(y0,colorgray,linestyle--,alpha0.4)plt.title(label)plt.xlabel(Time (s))plt.legend(locbest,fontsize8)plt.grid(True,alpha0.3)print(*65)plt.tight_layout()plt.show()5. 总结S曲线停止运动通过控制加加速度实现了平滑减速是工业运动控制中的常用策略。你可以在上方代码块中填入自己的实现结合具体硬件平台进行调试。