|
1 前言 S形加减速的最重要特征是该算法的加速度/减速度曲线的形状如字母 S。S形加减速的速度曲线平滑 ,从而能够减少对控制过程中的冲击，并使插补过程具有柔性 1。
由于T形曲线在加速到匀速的切换过程中，实际中存在较大过冲，因此这里对比一下T曲线和7段S曲线的实际过程； - T形：加速 -> 匀速 -> 减速
- S形：加加速(T 1 T_1T1) -> 匀加速(T 2 T_2T2) -> 减加速(T 3 T_3T3)-> 匀速(T 4 T_4T4)-> 加减速(T 5 T_5T5)-> 匀减速(T 6 T_6T6)-> 减减速(T 7 T_7T7)
上文在加速这块的文字描述可能读起来起来有点绕，下面看图：
2 理论分析由于S曲线在加减速的过程中，其加速度是变化的，因此这里引入了新的一个变量 J JJ，即加加速度。
J = d a d t J = \cfrac{d_a}{d_t}J=dtda
因此对应上图的7段S速度曲线中，规定最大加速为a m a x a_{max}amax，最小加速度为− a m a x -a_{max}−amax，则加速度的关系； - 加加速(T 1 T_1T1)：a aa逐渐增大，此时a = J T 1 , 0 < t ≤ t 1 ; a = JT_1,\qquad 0<t\le t_1;a=JT1,0<t≤t1;
- 匀加速(T 2 T_2T2)：a aa达到最大，此时a = a m a x , t 1 < t ≤ t 2 ; a = a_{max}, \qquad t_1<t\le t_2;a=amax,t1<t≤t2;
- 减加速(T 3 T_3T3)：a aa逐渐减小，此时a = a m a x − J T 3 , t 2 < t ≤ t 3 ; a = a_{max}-JT_3, \qquad t_2<t\le t_3;a=amax−JT3,t2<t≤t3;
- 匀速(T 4 T_4T4)：a aa不变化，此时a = 0 , t 3 < t ≤ t 4 ; a = 0, \qquad t_3<t\le t_4;a=0,t3<t≤t4;
- 加减速(T 5 T_5T5)：∣ a ∣ |a|∣a∣ 逐渐增大，此时a = − J T 5 , t 4 < t ≤ t 5 ; a = -JT_5, \qquad t_4<t\le t_5;a=−JT5,t4<t≤t5;
- 匀减速(T 6 T_6T6)：∣ a ∣ |a|∣a∣ 达到最大，此时a = − a m a x , t 5 < t ≤ t 6 ; a = -a_{max}, \qquad t_5<t\le t_6;a=−amax,t5<t≤t6;
- 减减速(T 7 T_7T7)：∣ a ∣ |a|∣a∣ 逐渐减小，此时a = − a m a x + − J T 7 , t 6 < t ≤ t 7 ; a = -a_{max}+-JT_7, \qquad t_6<t\le t_7;a=−amax+−JT7,t6<t≤t7;
∣ a ∣ |a|∣a∣ 为加速度的绝对值；
其中 T k = t k − t k − 1 ( k = 1 , . . . , 7 ) T_k =t_k - t_{k -1} (k =1 , ..., 7)Tk=tk−tk−1(k=1,...,7)
所以通常需要确定三个最基本的系统参数 :系统最大速度 v m a x v_{max}vmax ，最大加速度a_{max} ，加加速度J JJ，就可以可确定整个运行过程2 ； - 最大速度：反映了系统的最大运行能力 ；
- 最大加速度：反映了系统的最大加减速能力 ；
- 加加速度：反映了系统的柔性；
- 柔性越大，过冲越大，运行时间越短；
- 柔性越小，过冲越小，运行时间越长；
2.1 加速度时间关系方程整个加速度变化的过程具体如下图所示；
再次强调一下 T TT 和 t tt 的关系，T k = t k − t k − 1 ( k = 1 , . . . , 7 ) T_k =t_k - t_{k -1} (k =1 , ..., 7)Tk=tk−tk−1(k=1,...,7)
另外这里再引入变量 τ \tauτ，
τ k = t − t k − 1 ⋯ ① \tau_k = t - t_{k-1}\quad \cdots \quad①τk=t−tk−1⋯①
比如，当前时刻 t 2 < t ≤ t 3 t_2<t\le t_3t2<t≤t3 ,即 t tt 位于区间 T 3 T_3T3，则如果将 t 2 t_2t2 作为初始点，则 τ 3 \tau_3τ3为 t tt 相对于t 2 t_2t2时刻的时间，则有：
τ 3 = t − t 2 \tau_3 = t - t_2τ3=t−t2
下面可以得到加速度与时间的关系函数，具体如下：
a ( t ) = { J t , 0 < t ≤ t 1 a m a x , t 1 < t ≤ t 2 a m a x − J ( t − t 2 ) , t 2 < t ≤ t 3 0 , t 3 < t ≤ t 4 − J ( t − t 4 ) , t 4 < t ≤ t 5 − a m a x , t 5 < t ≤ t 6 − a m a x − J ( t − t 6 ) , t 6 < t ≤ t 7 ⋯ ② a(t)=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪Jt,amax,amax−J(t−t2),0,−J(t−t4),−amax,−amax−J(t−t6),0<t≤t1t1<t≤t2t2<t≤t3t3<t≤t4t4<t≤t5t5<t≤t6t6<t≤t7a(t)={Jt,0<t≤t1amax,t1<t≤t2amax−J(t−t2),t2<t≤t30,t3<t≤t4−J(t−t4),t4<t≤t5−amax,t5<t≤t6−amax−J(t−t6),t6<t≤t7
\quad \cdots \quad②a(t)=⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧Jt,amax,amax−J(t−t2),0,−J(t−t4),−amax,−amax−J(t−t6),0<t≤t1t1<t≤t2t2<t≤t3t3<t≤t4t4<t≤t5t5<t≤t6t6<t≤t7⋯②
根据 ① 式，将 τ \tauτ 代入 ② 式可以得到：
a ( t ) = { J τ 1 , 0 < t ≤ t 1 a m a x , t 1 < t ≤ t 2 a m a x − J τ 3 , t 2 < t ≤ t 3 0 , t 3 < t ≤ t 4 − J τ 3 , t 4 < t ≤ t 5 − a m a x , t 5 < t ≤ t 6 − a m a x + J τ 7 , t 6 < t ≤ t 7 a(t)=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪Jτ1,amax,amax−Jτ3,0,−Jτ3,−amax,−amax+Jτ7,0<t≤t1t1<t≤t2t2<t≤t3t3<t≤t4t4<t≤t5t5<t≤t6t6<t≤t7a(t)={Jτ1,0<t≤t1amax,t1<t≤t2amax−Jτ3,t2<t≤t30,t3<t≤t4−Jτ3,t4<t≤t5−amax,t5<t≤t6−amax+Jτ7,t6<t≤t7
a(t)=⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧Jτ1,amax,amax−Jτ3,0,−Jτ3,−amax,−amax+Jτ7,0<t≤t1t1<t≤t2t2<t≤t3t3<t≤t4t4<t≤t5t5<t≤t6t6<t≤t7
上式中 J > 0 J > 0J>0；
2.2 速度时间关系方程速度和加速度满足 v = a t v=atv=at ；加加速度和速度的关系满足：
v = 1 2 J t 2 v = \cfrac{1}{2} Jt^2v=21Jt2 结合加速度时间关系并结合② 式可以得到速度曲线关系，具体关系如下图所示；
进一步简化可以得到：
2.3 位移时间关系方程位移 S SS 和加加速度 J JJ 直接满足关系如下：
S = 1 6 J t 3 S = \cfrac{1}{6} Jt^3S=61Jt3 简单推导
{ S = ∫ 0 t v d t v = 1 2 J t 2 ⎧⎩⎨⎪⎪S=∫t0vdtv=12Jt2{S=∫0tvdtv=12Jt2
⎩⎨⎧S=∫0tvdtv=21Jt2
因此可以得到：
S = ∫ 0 t 1 2 J t 2 d t = 1 6 J t 3 ∣ 0 t S = \int_{0}^{t}\cfrac{1}{2}Jt^2dt = \cfrac{1}{6}Jt^3|_0^t\\S=∫0t21Jt2dt=61Jt3∣0t
积分忘的差不多了，回去再复习一下；
最终位移的方程如下所示；
3 程序实现的思路正如前面所提到的，S曲线规划需要确定三个最基本的系统参数 ：系统最大速度 v m a x v_{max}vmax ，最大加速度a_{max} ，加加速度J JJ，这样就可以确定这个运行过程。
这里有一个隐性的条件，就是在运行的过程中可以达到最大速度，这样才是完整的7段S曲线，另外这里还有一些中间参数： - t m = v m a x a m a x t_m = \cfrac{v_{max}}{a_{max}}tm=amaxvmax，因此有 T 1 = T 3 = T 5 = T 7 T_1=T_3=T_5=T_7T1=T3=T5=T7；
- 加加速度 J = J 1 = J 3 = J 5 = J 7 = a m a x t m J = J_1= J_3= J_5= J_7=\cfrac{a_{max}}{t_m}J=J1=J3=J5=J7=tmamax；
- T 2 = T 6 T_2 = T_6T2=T6；
- T f T_fTf，用户给定整个运行过程所需要的时间；
但是通常实际过程中关心a m a x a_{max}amax，v m a x v_{max}vmax，T f T_fTf； 3.1 T k T_kTk 推导理想状态假设存在 T 2 T_2T2和T 6 T_6T6，则推导过程如下： { v 1 = 1 2 J T 1 2 v 2 = v 1 + a m a x T 2 v 3 = v 2 + 1 2 J T 3 2 v 3 = v m a x T 1 = T 3 ⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪v1=12JT21v2=v1+amaxT2v3=v2+12JT23v3=vmaxT1=T3{v1=12JT12v2=v1+amaxT2v3=v2+12JT32v3=vmaxT1=T3
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧v1=21JT12v2=v1+amaxT2v3=v2+21JT32v3=vmaxT1=T3
因此可以得到：
v m a x = 1 2 J T 1 2 + a m a x T 2 + 1 2 J T 1 2 v_{max} = \cfrac{1}{2}JT_1^2 + a_{max}T_2 +\cfrac{1}{2}JT_1^2vmax=21JT12+amaxT2+21JT12 简化之后得到：
v m a x = J T 1 2 + a m a x T 2 v_{max} = JT_1^2 + a_{max}T_2vmax=JT12+amaxT2 根据②式可知：a m a x = J T 1 a_{max} = JT_1amax=JT1 最终得到：
T 2 = T 6 = V m a x − V s a m a x − T 1 T_2 = T_6 = \cfrac{V_{max} - V_s}{a_{max}} - T_1T2=T6=amaxVmax−Vs−T1 V s 为 初 始 速 度 ； V_s为初始速度；Vs为初始速度；
下面可以根据位移时间关系方程进行离散化的程序编写。 假设可以到达最大速度，且用户给定了整个过程运行时间T f T_fTf，则 T 4 T_4T4 的推导如下：
{ T f = T 1 + T 2 + T 3 + T 4 + T 5 + T 6 + T 7 T 2 = T 6 T 1 = T 3 = T 5 = T 7 ⎧⎩⎨Tf=T1+T2+T3+T4+T5+T6+T7T2=T6T1=T3=T5=T7{Tf=T1+T2+T3+T4+T5+T6+T7T2=T6T1=T3=T5=T7
⎩⎪⎨⎪⎧Tf=T1+T2+T3+T4+T5+T6+T7T2=T6T1=T3=T5=T7
简化上式可以得到：
T 4 = T f − 2 T 2 − 4 T 1 T_4 = T_f - 2T_2 - 4T_1T4=Tf−2T2−4T1
根据 T 1 = a m a x J T_1 = \cfrac{a_{max}}{J}T1=Jamax代入上式可得：
T 4 = T f − 2 v m a x a m a x − 2 a m a x J T_4 = T_f - 2\cfrac{v_{max}}{a_{max}}-2\cfrac{a_{max}}{J}T4=Tf−2amaxvmax−2Jamax
3.2 J JJ 的推导这时候还剩下J JJ需要计算，通过已量 T f , v m a x , a m a x T_f,v_{max},a_{max}Tf,vmax,amax可以推导出来；
首先位移之间满足关系如下：
S r e f = S a + S 4 + S d S_{ref} = S_a + S_4 + S_dSref=Sa+S4+Sd
其中加速区长度为 S a S_aSa；
其中减速区长度为 S d S_dSd；
{ S a = v s ( 2 T 1 + T 2 ) + 1 2 J T 1 ( 2 T 1 2 + 3 T 1 T 2 + T 2 2 ) S d = v 3 ( 2 T 5 + T 6 ) − 1 2 J T 5 ( 2 T 5 2 + 3 T 5 T 6 + T 6 2 ) ⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪Sa=vs(2T1+T2)+12JT1(2T21+3T1T2+T22)Sd=v3(2T5+T6)−12JT5(2T25+3T5T6+T26){Sa=vs(2T1+T2)+12JT1(2T12+3T1T2+T22)Sd=v3(2T5+T6)−12JT5(2T52+3T5T6+T62)
⎩⎪⎨⎪⎧Sa=vs(2T1+T2)+21JT1(2T12+3T1T2+T22)Sd=v3(2T5+T6)−21JT5(2T52+3T5T6+T62)
具体推导；2
前面提到过T 1 = T 3 = T 5 = T 7 T_1 = T_3 = T_5 = T_7T1=T3=T5=T7，T 2 = T 6 T_2 = T_6T2=T6，因此在V s V_sVs=0的时候，则
S a + S d = v 3 ( 2 T 5 + T 6 ) = v 3 ( 2 T 1 + T 2 ) ⋯ ④ S_a + S_d = v_3(2T_5 + T_6) = v_3(2T_1 + T_2) \cdots ④Sa+Sd=v3(2T5+T6)=v3(2T1+T2)⋯④
这里简单推导一下：
{ S 4 = v 4 T 4 T 4 = T f − ( T 1 + T 2 + T 3 + T 4 + T 5 + T 6 ) T 1 = T 3 = T 5 = T 7 = a m a x J T 2 = T 6 = V m a x a m a x − T 1 v 3 = V m a x S r e f = S a + S 4 + S d ⋯ ⑤ ⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪S4=v4T4T4=Tf−(T1+T2+T3+T4+T5+T6)T1=T3=T5=T7=amaxJT2=T6=Vmaxamax−T1v3=VmaxSref=Sa+S4+Sd{S4=v4T4T4=Tf−(T1+T2+T3+T4+T5+T6)T1=T3=T5=T7=amaxJT2=T6=Vmaxamax−T1v3=VmaxSref=Sa+S4+Sd
\cdots ⑤⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧S4=v4T4T4=Tf−(T1+T2+T3+T4+T5+T6)T1=T3=T5=T7=JamaxT2=T6=amaxVmax−T1v3=VmaxSref=Sa+S4+Sd⋯⑤
根据④，⑤最终简化得到：
J = a m a x 2 v m a x v m a x a m a x T f − v m a x 2 − S r e f a m a x J = \cfrac{a_{max}^2v_{max}}{v_{max}a_{max}T_f-v_{max}^2-S_{ref}a_{max}}J=vmaxamaxTf−vmax2−Srefamaxamax2vmax
T f T_fTf：为运行的总时间
S r e f S_{ref}Sref：为运行的总路程
详细推导过程如下：
S r e f = S a + S 4 + S d = S 4 + v 3 ( 2 T 1 + T 2 ) S_{ref} = S_a + S_4 + S_d = S_4 + v_3(2T_1 + T_2)Sref=Sa+S4+Sd=S4+v3(2T1+T2)
因为：
v 3 ( 2 T 1 + T 2 ) = v m a x ( 2 T 1 + T 2 ) v_3(2T_1 + T_2) =v_{max}(2T_1 + T_2)v3(2T1+T2)=vmax(2T1+T2)
因为：
T 2 = T 6 = V m a x − V s a m a x − T 1 T 1 = T 3 = T 5 = T 7 = a m a x J V s = 0 T_2 = T_6 = \cfrac{V_{max} - V_s}{a_{max}} - T_1 \\ T_1 = T_3 = T_5 = T_7 = \cfrac{a_{max}}{J} \\ V_s = 0T2=T6=amaxVmax−Vs−T1T1=T3=T5=T7=JamaxVs=0
所以，简化得到：
v 3 ( 2 T 1 + T 2 ) = v m a x ( v m a x a m a x + a m a x J ) v_3(2T_1 + T_2) =v_{max}(\cfrac{v_{max}}{a_{max}} + \cfrac{a_{max}}{J})v3(2T1+T2)=vmax(amaxvmax+Jamax)
所以可以得到：
S r e f = S 4 + v m a x ( v m a x a m a x + a m a x J ) S r e f = v m a x T 4 + v m a x ( v m a x a m a x + a m a x J ) S_{ref} = S_4 + v_{max}(\cfrac{v_{max}}{a_{max}} + \cfrac{a_{max}}{J}) \\ \\ S_{ref} = v_{max}T_4 + v_{max}(\cfrac{v_{max}}{a_{max}} + \cfrac{a_{max}}{J})Sref=S4+vmax(amaxvmax+Jamax)Sref=vmaxT4+vmax(amaxvmax+Jamax)
因为：
T 4 = T f − 2 v m a x a m a x − 2 a m a x J T_4 = T_f - 2\cfrac{v_{max}}{a_{max}}-2\cfrac{a_{max}}{J}T4=Tf−2amaxvmax−2Jamax
将其代入可以得到：
S r e f = v m a x ( T f − 2 v m a x a m a x − 2 a m a x J ) + v m a x ( v m a x a m a x + a m a x J ) S_{ref} = v_{max}( T_f - 2\cfrac{v_{max}}{a_{max}}-2\cfrac{a_{max}}{J}) + v_{max}(\cfrac{v_{max}}{a_{max}} + \cfrac{a_{max}}{J})Sref=vmax(Tf−2amaxvmax−2Jamax)+vmax(amaxvmax+Jamax)
简化得到最终结果：
J = a m a x 2 v m a x v m a x a m a x T f − v m a x 2 − S r e f a m a x J = \cfrac{a_{max}^2v_{max}}{v_{max}a_{max}T_f-v_{max}^2-S_{ref}a_{max}}J=vmaxamaxTf−vmax2−Srefamaxamax2vmax
| 
楼主
标题置顶
标题高亮
……
