为了减小机器人加速过程的冲击,这里梳理一下S形加减速相关知识。

1. S加减速曲线

在这里插入图片描述

计算公式:
加加速度

j ( t ) = { J 0 ≤ t ≤ t 1 − J t 1 ≤ t ≤ t 2 0 t 2 ≤ t ≤ t 3 − J t 3 ≤ t ≤ t 4 J t 4 ≤ t ≤ t 5 j(t)=

j(t)=JJ0JJ0tt1t1tt2t2tt3t3tt4t4tt5

加速度:

a ( t ) = { J ∗ t 0 ≤ t ≤ t 1 J ∗ T 1 − J ∗ ( t − t 1 ) t 1 ≤ t ≤ t 2 0 t 2 ≤ t ≤ t 3 − J ∗ ( t − t 3 ) t 3 ≤ t ≤ t 4 − J ∗ T 4 + J ∗ ( t − t 4 ) t 4 ≤ t ≤ t 5 a(t)=

a(t)=JtJT1J(tt1)0J(tt3)JT4+J(tt4)0tt1t1tt2t2tt3t3tt4t4tt5

速度:

v ( t ) = { v s + J ∗ t 2 / 2 0 ≤ t ≤ t 1 v 1 + J ∗ T 1 ∗ ( t − t 1 ) − J ∗ ( t − t 1 ) 2 / 2 t 1 ≤ t ≤ t 2 v 2 t 2 ≤ t ≤ t 3 v 3 − J ∗ ( t − t 3 ) 2 / 2 t 3 ≤ t ≤ t 4 v 4 − J ∗ T 4 ∗ ( t − t 4 ) + J ∗ ( t − t 4 ) 2 / 2 t 4 ≤ t ≤ t 5 v(t)=

v(t)=vs+Jt2/2v1+JT1(tt1)J(tt1)2/2v2v3J(tt3)2/2v4JT4(tt4)+J(tt4)2/20tt1t1tt2t2tt3t3tt4t4tt5

位移:


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 − J ∗ ( t − t 1 ) 3 / 6 t 1 ≤ t ≤ t 2 s 2 + v 2 ∗ ( t − t 2 ) t 2 ≤ t ≤ t 3 s 3 + v 3 ∗ ( t − t 3 ) − J ∗ ( t − t 3 ) 3 / 6 t 3 ≤ t ≤ t 4 s 4 + v 4 ∗ ( t − t 4 ) − J ∗ T 4 ∗ ( t − t 4 ) 2 / 2 + J ∗ ( t − t 4 ) 3 / 6 t 4 ≤ t ≤ t 5 s(t)=

s(t)=vst+Jt3/6s1+v1(tt1)+JT1(tt1)2/2J(tt1)3/6s2+v2(tt2)s3+v3(tt3)J(tt3)3/6s4+v4(tt4)JT4(tt4)2/2+J(tt4)3/60tt1t1tt2t2tt3t3tt4t4tt5

2. 算法流程(vs=0, ve=0)

已知 位移S,最大速度V 加加速度J,按照如下流程进行速度规划:
第一步,计算加速减速距离:

S a = S d = V ∗ V J S_a = S_d = V*\sqrt\frac{V}{J} Sa=Sd=VJV

第二步,比较位移关系

如 果 S a + S d < = S 则 : t 1 = t 2 = t 4 = t 5 = V J t 3 = S − S a − S d V 如果 S_a + S_d <= S \\ 则:\\ t_1 = t_2 = t_4 = t_5 = \sqrt\frac{V}{J}\\ t_3 = \frac{S-S_a-S_d}{V} Sa+Sd<=St1=t2=t4=t5=JV t3=VSSaSd

如 果 S a + S d > S 则 : V , = ( S ∗ J 2 ) 1 3 t 1 = t 2 = t 4 = t 5 = V , J t 3 = 0 如果 S_a + S_d > S \\ 则:\\ V^, = (\frac{S*J}{2})^\frac{1}{3} \\ t_1 = t_2 = t_4 = t_5 = \sqrt\frac{V^,}{J}\\ t_3 = 0 Sa+Sd>SV,=(2SJ)31t1=t2=t4=t5=JV, t3=0

第三步,即得到每阶段对应的插补时间,即可进行速度插补

3. 算法流程(vs>0或 ve>0)

已知 位移S,起始速度Vs,终止速度Ve,最大速度V 加加速度J,按照如下流程进行速度规划:
第一步,计算加速减速距离:

S a = ( V + V s ) ∗ V − V s J S d = ( V + V e ) ∗ V − V e J S_a = (V+V_s)*\sqrt\frac{V-V_s}{J} \\ S_d = (V+V_e)*\sqrt\frac{V-V_e}{J} Sa=(V+Vs)JVVs Sd=(V+Ve)JVVe

第二步,比较位移关系

如 果 S a + S d < = S 则 : t 1 = t 2 = V − V s J t 4 = t 5 = V − V e J t 3 = S − S a − S d V 如果 S_a + S_d <= S \\ 则:\\ t_1 = t_2 = \sqrt\frac{V-V_s}{J}\\ t_4 = t_5 = \sqrt\frac{V-V_e}{J}\\ t_3 = \frac{S-S_a-S_d}{V} Sa+Sd<=St1=t2=JVVs t4=t5=JVVe t3=VSSaSd

如果 	S_a + S_d > S 
重新计算最大速度V,因为解析解计算复杂,这里通过二分法逼近,方法如下:
(1)设置v1 = V,v2设置为Vs,Ve中较大的数值
(2)二分法计算最大速度

V = v 1 + v 2 2 V = \frac{v1+v2}{2} V=2v1+v2

(3)计算加减速距离

S a = ( V + V s ) ∗ V − V s J S d = ( V + V e ) ∗ V − V e J S_a = (V+V_s)*\sqrt\frac{V-V_s}{J} \\ S_d = (V+V_e)*\sqrt\frac{V-V_e}{J} Sa=(V+Vs)JVVs Sd=(V+Ve)JVVe

(3)比较位移

如 果 S > S a + S d 则 : v 2 = V 如 果 S < S a + S d 则 : v 1 = V 如果 S > S_a + S_d \\ 则:\\ v2 = V 如果 S < S_a + S_d \\ 则:\\ v1 = V S>Sa+Sdv2=VS<Sa+Sdv1=V

(4) 重复步骤(1)(2)(3),直到

∣ S − S a − S d ∣ < ϵ |S-S_a-S_d| < \epsilon SSaSd<ϵ

阈值ε根据实际需求进行设置,通常设置为0.000001,迭代次数不超过10次.