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 & 0\leq t \leq t_1\\ 0 & t_1\leq t \leq t_2\\ -J & t_2\leq t \leq t_3\\ 0 & t_3\leq t \leq t_4\\ J & t_4\leq t \leq t_5\\ 0 & t_5\leq t \leq t_6\\ -J & t_6\leq t \leq t_7\\ \end{cases} j(t)=⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧J0−J0J0−J0≤t≤t1t1≤t≤t2t2≤t≤t3t3≤t≤t4t4≤t≤t5t5≤t≤t6t6≤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 & 0\leq t \leq t_1\\ J*T_1 & t_1\leq t \leq t_2\\ J*T_1 - J*(t-t_1) & t_2\leq t \leq t_3\\ 0 & t_3\leq t \leq t_4\\ -J*(t-t_4) & t_4\leq t \leq t_5\\ -J*T_4 & t_5\leq t \leq t_6\\ -J*T_4 + J*(t-t_6) & t_6\leq t \leq t_7\\ \end{cases} a(t)=⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧J∗tJ∗T1J∗T1−J∗(t−t1)0−J∗(t−t4)−J∗T4−J∗T4+J∗(t−t6)0≤t≤t1t1≤t≤t2t2≤t≤t3t3≤t≤t4t4≤t≤t5t5≤t≤t6t6≤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 & 0\leq t \leq t_1\\ v_1 + J*T_1*(t-t_1) & t_1\leq t \leq t_2\\ v_2 + J*T_1*(t-t_2) - J*(t-t_2)^2/2 & t_2\leq t \leq t_3\\ v_3 & t_3\leq t \leq t_4\\ v_4 - J*(t-t_4)^2/2 & t_4\leq t \leq t_5\\ v_5 - J*T_5*(t-t_5) & t_5\leq t \leq t_6\\ v_6 - J*T_5*(t-t_6) + J*(t-t_6)^2/2 & t_6\leq 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/2v3v4−J∗(t−t4)2/2v5−J∗T5∗(t−t5)v6−J∗T5∗(t−t6)+J∗(t−t6)2/20≤t≤t1t1≤t≤t2t2≤t≤t3t3≤t≤t4t4≤t≤t5t5≤t≤t6t6≤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 & 0\leq t \leq t_1\\ s_1 + v_1*(t-t_1) + J*T_1*(t-t_1)^2/2 & t_1\leq 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_2\leq t \leq t_3\\ s_3 + v_3*(t-t_3) & t_3\leq t \leq t_4\\ s_4 + v_4*(t-t_4) - J*(t-t_4)^3/6 & t_4\leq t \leq t_5\\ s_5 + v_5*(t-t_5) - J*T_5*(t-t_5)^2/2 & t_5\leq 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_6\leq 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/60≤t≤t1t1≤t≤t2t2≤t≤t3t3≤t≤t4t4≤t≤t5t5≤t≤t6t6≤t≤t7