S形曲线加减速

S形曲线加减速

S加减速–7段

接前文5段S形速度规划，这里推导一下7段S形曲线的计算公式

1. S加减速曲线

定义：
上图中，令t1-t7为各个位置的绝对时间节点，T1-T7为每一段的增量时间节点。
这里的定义和图中有些出入，线下面的计算全部按照此定义为准.

计算公式：
加加速度

j ( t ) = { J 0 ≤ t ≤ t 1 0 t 1 ≤ t ≤ t 2 − J t 2 ≤ t ≤ t 3 0 t 3 ≤ t ≤ t 4 J t 4 ≤ t ≤ t 5 0 t 5 ≤ t ≤ t 6 − J t 6 ≤ t ≤ t 7 j(t)=begin{cases} J & 0leq t leq t_1\ 0 & t_1leq t leq t_2\ -J & t_2leq t leq t_3\ 0 & t_3leq t leq t_4\ J & t_4leq t leq t_5\ 0 & t_5leq t leq t_6\ -J & t_6leq t leq t_7\ end{cases} j(t)=⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧​J0−J0J0−J​0≤t≤t1​t1​≤t≤t2​t2​≤t≤t3​t3​≤t≤t4​t4​≤t≤t5​t5​≤t≤t6​t6​≤t≤t7​​

加速度：

a ( t ) = { J ∗ t 0 ≤ t ≤ t 1 J ∗ T 1 t 1 ≤ t ≤ t 2 J ∗ T 1 − J ∗ ( t − t 1 ) t 2 ≤ t ≤ t 3 0 t 3 ≤ t ≤ t 4 − J ∗ ( t − t 4 ) t 4 ≤ t ≤ t 5 − J ∗ T 4 t 5 ≤ t ≤ t 6 − J ∗ T 4 + J ∗ ( t − t 6 ) t 6 ≤ t ≤ t 7 a(t)=begin{cases} J*t & 0leq t leq t_1\ J*T_1 & t_1leq t leq t_2\ J*T_1 - J*(t-t_1) & t_2leq t leq t_3\ 0 & t_3leq t leq t_4\ -J*(t-t_4) & t_4leq t leq t_5\ -J*T_4 & t_5leq t leq t_6\ -J*T_4 + J*(t-t_6) & t_6leq t leq t_7\ end{cases} a(t)=⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧​J∗tJ∗T1​J∗T1​−J∗(t−t1​)0−J∗(t−t4​)−J∗T4​−J∗T4​+J∗(t−t6​)​0≤t≤t1​t1​≤t≤t2​t2​≤t≤t3​t3​≤t≤t4​t4​≤t≤t5​t5​≤t≤t6​t6​≤t≤t7​​

速度：

v ( t ) = { v s + J ∗ t 2 / 2 0 ≤ t ≤ t 1 v 1 + J ∗ T 1 ∗ ( t − t 1 ) t 1 ≤ t ≤ t 2 v 2 + J ∗ T 1 ∗ ( t − t 2 ) − J ∗ ( t − t 2 ) 2 / 2 t 2 ≤ t ≤ t 3 v 3 t 3 ≤ t ≤ t 4 v 4 − J ∗ ( t − t 4 ) 2 / 2 t 4 ≤ t ≤ t 5 v 5 − J ∗ T 5 ∗ ( t − t 5 ) t 5 ≤ t ≤ t 6 v 6 − J ∗ T 5 ∗ ( t − t 6 ) + J ∗ ( t − t 6 ) 2 / 2 t 6 ≤ t ≤ t 7 v(t)=begin{cases} v_s + J*t^2/2 & 0leq t leq t_1\ v_1 + J*T_1*(t-t_1) & t_1leq t leq t_2\ v_2 + J*T_1*(t-t_2) - J*(t-t_2)^2/2 & t_2leq t leq t_3\ v_3 & t_3leq t leq t_4\ v_4 - J*(t-t_4)^2/2 & t_4leq t leq t_5\ v_5 - J*T_5*(t-t_5) & t_5leq t leq t_6\ v_6 - J*T_5*(t-t_6) + J*(t-t_6)^2/2 & t_6leq t leq t_7\ end{cases} v(t)=⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧​vs​+J∗t2/2v1​+J∗T1​∗(t−t1​)v2​+J∗T1​∗(t−t2​)−J∗(t−t2​)2/2v3​v4​−J∗(t−t4​)2/2v5​−J∗T5​∗(t−t5​)v6​−J∗T5​∗(t−t6​)+J∗(t−t6​)2/2​0≤t≤t1​t1​≤t≤t2​t2​≤t≤t3​t3​≤t≤t4​t4​≤t≤t5​t5​≤t≤t6​t6​≤t≤t7​​

位移：

​
s ( t ) = { v s ∗ t + J ∗ t 3 / 6 0 ≤ t ≤ t 1 s 1 + v 1 ∗ ( t − t 1 ) + J ∗ T 1 ∗ ( t − t 1 ) 2 / 2 t 1 ≤ t ≤ t 2 s 2 + v 2 ∗ ( t − t 2 ) + J ∗ T 1 ∗ ( t − t 2 ) 2 / 2 − J ∗ ( t − t 2 ) 3 / 6 t 2 ≤ t ≤ t 3 s 3 + v 3 ∗ ( t − t 3 ) t 3 ≤ t ≤ t 4 s 4 + v 4 ∗ ( t − t 4 ) − J ∗ ( t − t 4 ) 3 / 6 t 4 ≤ t ≤ t 5 s 5 + v 5 ∗ ( t − t 5 ) − J ∗ T 5 ∗ ( t − t 5 ) 2 / 2 t 5 ≤ t ≤ t 6 s 6 + v 6 ∗ ( t − t 6 ) − J ∗ T 5 ∗ ( t − t 6 ) 2 / 2 + J ∗ ( t − t 6 ) 3 / 6 t 6 ≤ t ≤ t 7 s(t)=begin{cases} v_s*t + J*t^3/6 & 0leq t leq t_1\ s_1 + v_1*(t-t_1) + J*T_1*(t-t_1)^2/2 & t_1leq t leq t_2\ s_2 + v_2*(t-t_2) + J*T_1*(t-t_2)^2/2 - J*(t-t_2)^3/6 & t_2leq t leq t_3\ s_3 + v_3*(t-t_3) & t_3leq t leq t_4\ s_4 + v_4*(t-t_4) - J*(t-t_4)^3/6 & t_4leq t leq t_5\ s_5 + v_5*(t-t_5) - J*T_5*(t-t_5)^2/2 & t_5leq t leq t_6\ s_6 + v_6*(t-t_6) - J*T_5*(t-t_6)^2/2 + J*(t-t_6)^3/6 & t_6leq t leq t_7\ end{cases} s(t)=⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧​vs​∗t+J∗t3/6s1​+v1​∗(t−t1​)+J∗T1​∗(t−t1​)2/2s2​+v2​∗(t−t2​)+J∗T1​∗(t−t2​)2/2−J∗(t−t2​)3/6s3​+v3​∗(t−t3​)s4​+v4​∗(t−t4​)−J∗(t−t4​)3/6s5​+v5​∗(t−t5​)−J∗T5​∗(t−t5​)2/2s6​+v6​∗(t−t6​)−J∗T5​∗(t−t6​)2/2+J∗(t−t6​)3/6​0≤t≤t1​t1​≤t≤t2​t2​≤t≤t3​t3​≤t≤t4​t4​≤t≤t5​t5​≤t≤t6​t6​≤t≤t7​​