速度曲线规划之S形曲线

速度曲线规划之S形曲线

前言

S形曲线重要特征是其加速度/减速度曲线形如字母S。S形加减速的速度曲线平滑，从而能够减少对控制过程中的冲击，并使插补过程具有柔性。

由于T形曲线在加速到匀速的切换过程中，实际中存在较大过冲，因此这里对比一下T曲线和7段S曲线的实际过程：

• T形速度曲线包括匀加速、匀速和匀减速，类似于上一节中带有抛物线过渡的线性函数

  

• S形：加加速->匀加速->减加速->匀速->加减速->匀减速->减减速

  

理论分析

引入==加加速度==的概念，表示加速度的变化，用$J$表示，$J=\frac{da}{dt}$

在上图中7段S速度曲线中，规定最大加速为$a_{max}$，最小加速度为$-a_{max}$，加速度的关系为：

• 加加速($T_1$)：a逐渐增大，此时$a=Jt$，$0<t\le t_1$
• 匀加速($T_2$)：a达到最大，此时$a=a_{max}$，$t_1<t\le t_2$
• 减加速($T_3$)：a逐渐减小，此时$a=a_{max}-J(t-t_2)$，$t_2<t\le t_3$
• 匀速($T_4$)：a保持不变，此时$a=0$，$t_3<t\le t_4$
• 加减速($T_5$)：$|a|$逐渐增大，此时$a=-J(t-t_4)$，t_4<t≤t_5$
• 匀减速($T_6$)：a达到最小，此时$a=-a_{max}$，$t_5<t\le t_6$
• 减减速($T_7$)：$|a|$逐渐增大，此时$a=-a_{max}+J(t-t_6)$，$t_6<t\le t_7$

其中$|a|$是加速度的绝对值，$T_k=t_k-t_{k-1},k=1.2,...,7$

确定整个运动过程需要确定三个最基本的系统参数：系统最大速度$v_{max}$，最大加速度$a_{max}$，加加速度$J$

• 最大速度：反映了系统的最大运行能力；
• 最大加速度：反映了系统的最大加减速能力
• 加加速度：反映了系统的柔性；
  • 柔性越大，过冲越大，运行时间越短；
  • 柔性越小，过冲越小，运行时间越长；

加速度时间关系方程

整个加速度变化的过程具体如下图所示：

$$\mathrm{a}(\mathrm{t})=\{\begin{array}{ll}\mathrm{Jt},& 0<\mathrm{t}\le {\mathrm{t}}_1\\ {\mathrm{a}}_{\max }=\mathrm{J}{\mathrm{T}}_1,& {\mathrm{t}}_1<\mathrm{t}\le {\mathrm{t}}_2\\ {\mathrm{a}}_{\max }-\mathrm{J}\left(\mathrm{t}-{\mathrm{t}}_2\right),& {\mathrm{t}}_2<\mathrm{t}\le {\mathrm{t}}_3\\ 0,& {\mathrm{t}}_3<\mathrm{t}\le {\mathrm{t}}_4\\ -\mathrm{J}\left(\mathrm{t}-{\mathrm{t}}_4\right),& {\mathrm{t}}_4<\mathrm{t}\le {\mathrm{t}}_5\\ -{\mathrm{a}}_{\max }=-\mathrm{J}{\mathrm{T}}_5,& {\mathrm{t}}_5<\mathrm{t}\le {\mathrm{t}}_6\\ -{\mathrm{a}}_{\max }+\mathrm{J}\left(\mathrm{t}-{\mathrm{t}}_6\right),& {\mathrm{t}}_6<\mathrm{t}\le {\mathrm{t}}_7\end{array}$$

上式中，$J>0$，引入变量$\tau$，令${\tau }_k=t-t_{k-1}$，上式可写为

$$\mathrm{a}(\mathrm{t})=\{\begin{array}{ll}\mathrm{J}{\tau }_1,& 0<{\tau }_1\le {\mathrm{t}}_1\\ {\mathrm{a}}_{\max },& {\mathrm{t}}_1<\mathrm{t}\le {\mathrm{t}}_2\\ {\mathrm{a}}_{\max }-\mathrm{J}{\tau }_3,& 0<\mathrm{t}\le {\mathrm{t}}_3-{\mathrm{t}}_2\\ 0,& {\mathrm{t}}_3<\mathrm{t}\le {\mathrm{t}}_4\\ -\mathrm{J}{\tau }_5,& 0<\mathrm{t}\le {\mathrm{t}}_5-{\mathrm{t}}_4\\ -{\mathrm{a}}_{\max },& {\mathrm{t}}_5<\mathrm{t}\le {\mathrm{t}}_6\\ -{\mathrm{a}}_{\max }+\mathrm{J}{\tau }_7,& 0<\mathrm{t}\le {\mathrm{t}}_7-{\mathrm{t}}_6\end{array}$$

速度时间关系方程

速度和加速度满足$v=at$，加加速度和速度的关系满足$v=\frac{1}{2}J{t}^2$；由匀加速直线运动位移为$x=v_{0}t+\frac{1}{2}a{t}^2$，类比推导出恒定加加速度的运动，其速度变化为$v=a_{0}t+\frac{1}{2}J{t}^2$

速度与时间的关系如下：

KaTeX parse error: Undefined control sequence: \placeholder at position 120: …frac12JT_1^2 & \̲p̲l̲a̲c̲e̲h̲o̲l̲d̲e̲r̲{} \\ & & \pl…

进一步化简得到($v_S=0$)：

$$v\left(t\right)=\{\begin{array}{lll}\frac{1}{2}J{\tau }_1^2& 0\le t<t_1& \\ & & \\ J{T}_1\left(T_1+2{\tau }_2\right)& t_1\le t<t_2& \\ & & \\ J(T_1{T}_2+\frac{1}{2}{T}_1^2+T_1{\tau }_3-\frac{1}{2}{\tau }_3^2)& t_2\le t<t_3& \\ & & \\ & & \\ & & \\ J{T}_1\left(T_1+T_2\right)& t_3\le t<t_4& \\ & & \\ J(T_1^2+T_1{T}_2-\frac{1}{2}{\tau }_3^2)& t_4\le t<t_5& \\ & & \\ J{T}_1\left(T_2+\frac{1}{2}{T}_1-t_6\right)& t_5\le t<t_6& \\ & & \\ J(T_1{T}_2+\frac{1}{2}{T}_1^2-T_1{\tau }_7+\frac{1}{2}{\tau }_7^2)& t_6\le t<t_7& \end{array}$$

位移时间关系方程

位移$S$和加加速度$J$直接满足关系$\mathrm{S}={\int }_{{\mathrm{t}}_1}^{{\mathrm{t}}_2}\frac{1}{2}\mathrm{J}{\mathrm{t}}^2\mathrm{dt}=\frac{1}{6}\mathrm{J}{\mathrm{t}}^3{|}_{{\mathrm{t}}_1}^{{\mathrm{t}}_2}$

位移方程如下

$$S\left(t\right)=\{\begin{array}{ll}{v}_s{\tau }_1+J{\tau }_1^3/6& 0\le t<t_1\\ S_1+v_1{\tau }_2+J{T}_1{\tau }_2^2/2& t_1\le t<t_2\\ S_2+v_2{\tau }_3+J{T}_1{\tau }_3^2/2-J{\tau }_3^3/6& t_2\le t<t_3\\ S_3+v_3{\tau }_4& t_3\le t<t_4\\ S_4+v_4{\tau }_5-J{\tau }_3^3/6& t_4\le t<t_5\\ S_8+v_5{\tau }_6-J{T}_1{\tau }_6^2/2& t_5\le t<t_6\\ S_6+v_6{\tau }_7-J{T}_1{\tau }_7^2/2+J{\tau }_7^3/6& t_6\le t<t_7\end{array}$$

程序实现

S曲线规划需要确定三个最基本的系统参数：系统最大速度$v_{max}$，最大加速度$a_{max}$，加加速度$J$，在运行的过程中达到最大速度才是完整的7段S曲线。

• $t_m=\frac{v_{max}}{a_{max}}$，变加速度的每一段时间相同,$T_1=T_3=T_5=T_7=\frac{a_{max}}{J}=\frac{v_{max}-v_s}{a_{max}}$；
• 加加速度恒定$J={\mathrm{J}}_1={\mathrm{J}}_3={\mathrm{J}}_5={\mathrm{J}}_7=\frac{{\mathrm{a}}_{\max }}{{\mathrm{t}}_{\mathrm{m}}}$；
• 匀加速运动时间相等，$T_2=T_6$；
• 匀速运动时间$T_4=T_f-2T_2-4T_4$
• $T_f$是用户给定整个运动过程所需要的时间

$T_k$推导

理想状态下假设存在匀加速运动过程，即存在$T_2$和$T_6$

• $T_2=T_6=\frac{v_{max}-v_s}{a_{max}}-T_1$，$v_s$为$t_0$时刻初始速度;
• ${\mathrm{T}}_4={\mathrm{T}}_{\mathrm{f}}-2\frac{v_{\max }-v_s}{a_{\max }}-2\frac{{\mathrm{a}}_{\max }}{\mathrm{J}}$

$J$推导

利用v-t时间图像计算

位移关系满足：$S_{ref}=S_a+S_4+S_d$，其中$S_{ref}$是总位移，$S_a$为加速区位移，$S_b$为减速区位移。

当$v_s=0$时，即从静止开始运动，可得：

$$\mathrm{J}=\frac{{\mathrm{a}}_{\max }^2{\mathrm{v}}_{\max }}{{\mathrm{v}}_{\max }{\mathrm{a}}_{\max }{\mathrm{T}}_{\mathrm{f}}-{\mathrm{v}}_{\max }^2-{\mathrm{S}}_{\mathrm{ref}}{\mathrm{a}}_{\max }}$$