S形曲线速度规划 |
您所在的位置：网站首页 › t-s曲线 › S形曲线速度规划 |
S形曲线速度规划
|
为了减小机器人加速过程的冲击，这里梳理一下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)=\begin{cases} J & 0\leq t \leq t_1\\ -J & t_1\leq t \leq t_2\\ 0 & t_2\leq t \leq t_3\\ -J & t_3\leq t \leq t_4\\ J & t_4\leq t \leq t_5\\ \end{cases} j(t)=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧J−J0−JJ0≤t≤t1t1≤t≤t2t2≤t≤t3t3≤t≤t4t4≤t≤t5 加速度：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)=\begin{cases} J*t & 0\leq t \leq t_1\\ J*T_1 - J*(t-t_1) & t_1\leq t \leq t_2\\ 0 & t_2\leq t \leq t_3\\ -J*(t-t_3) & t_3\leq t \leq t_4\\ -J*T_4 + J*(t-t_4) & t_4\leq t \leq t_5\\ \end{cases} a(t)=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧J∗tJ∗T1−J∗(t−t1)0−J∗(t−t3)−J∗T4+J∗(t−t4)0≤t≤t1t1≤t≤t2t2≤t≤t3t3≤t≤t4t4≤t≤t5 速度：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)=\begin{cases} v_s + J*t^2/2 & 0\leq t \leq t_1\\ v_1 + J*T_1*(t-t_1) - J*(t-t_1)^2/2 & t_1\leq t \leq t_2\\ v_2 & t_2\leq t \leq t_3\\ v_3 - J*(t-t_3)^2/2 & t_3\leq t \leq t_4\\ v_4 - J*T_4*(t-t_4) + J*(t-t_4)^2/2 & t_4\leq t \leq t_5\\ \end{cases} v(t)=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧vs+J∗t2/2v1+J∗T1∗(t−t1)−J∗(t−t1)2/2v2v3−J∗(t−t3)2/2v4−J∗T4∗(t−t4)+J∗(t−t4)2/20≤t≤t1t1≤t≤t2t2≤t≤t3t3≤t≤t4t4≤t≤t5 位移： 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)=\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 - J*(t-t_1)^3/6 & t_1\leq t \leq t_2\\ s_2 + v_2*(t-t_2) & t_2\leq t \leq t_3\\ s_3 + v_3*(t-t_3) - J*(t-t_3)^3/6 & t_3\leq t \leq 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\leq t \leq t_5\\ \end{cases} s(t)=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧vs∗t+J∗t3/6s1+v1∗(t−t1)+J∗T1∗(t−t1)2/2−J∗(t−t1)3/6s2+v2∗(t−t2)s3+v3∗(t−t3)−J∗(t−t3)3/6s4+v4∗(t−t4)−J∗T4∗(t−t4)2/2+J∗(t−t4)3/60≤t≤t1t1≤t≤t2t2≤t≤t3t3≤t≤t4t4≤t≤t5 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=V∗JV 第二步，比较位移关系如 果 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 \\ 则：\\ 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>S则：V,=(2S∗J)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)∗JV−Vs Sd=(V+Ve)∗JV−Ve 第二步，比较位移关系如 果 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 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+Sd则：v2=V如果S |
【本文地址】
今日新闻 |
推荐新闻 |