S形加减速

• 加加速段
  加加速度为 $j_{max}$，加速度线性增加至设定值或最大值$a_{max}$。
• 匀加速段
  加加速度为0，加速度恒定。
• 减加速段
  当速度接近设定的值或最大值$v_{max}$时，加加速度突变为反向的$j_{max}$，进入加速度线性减小的变减速运动阶段。
• 匀速段
  当速度增至$v_{max}$后，加加速和加速度均变为0，进入匀速运动阶段。
• 加减速段
  加加速度突变为反向的$j_{max}$，加速度反向线性增加至$-a_{max}$。
• 匀减速段
  加加速度和为0，减加速度恒定。
• 减减速段
  加速度突变为$j_{max}$， 加速度由负向的$a_{max}$线性减小至0。

---

1. 加加速度、加速度、速度、位移随时间的变化函数：
   $t_k(k=0,1,...7)$：表示各个阶段的过渡点时刻；
   $\tau_k(k=0,1,...7)$：局部时间坐标，表示以各个阶段的起始点作为时间零点的时间表示，$\tau_k=t_k-t_{k-1}(k=1,...7)$;
   $T_k(k=1,...7)$：各个阶段的持续运行时间。
   $$j(t) = \begin{cases} j_{max} & 0 \le t \le t_1 \\ 0 & t_1 \le t \le t_2 \\ -j_{max} & t_2 \le t \le t_3 \\ 0 & t_3 \le t \le t_4 \\ -j_{max} & t_4 \le t \le t_5 \\ 0 & t_5 \le t \le t_6 \\ j_{max} & t_6 \le t \le t_7 \end{cases}$$
   $$a(t) = \begin{cases} j_{max}\tau_1 & 0 \le t \le t_1 & t=t_1时，a_{max}=j_{max}T_1\\ a_{max} & t_1 \le t \le t_2 \\ a_{max}-j_{max}\tau_3 & t_2 \le t \le t_3 \\ 0 & t_3 \le t \le t_4 \\ -j_{max}\tau_5 & t_4 \le t \le t_5 \\ -a_{max} & t_5 \le t \le t_6 \\ -a_{max}+j_{max}\tau_7 & t_6 \le t \le t_7 \end{cases}$$
   $$v(t) = \begin{cases} v_s+\frac12 j_{max}\tau_1^2 & 0 \le t \le t_1 & v_1=v_s+\frac12j_{max}T_1^2 \\ v_1+a_{max}\tau_2 & t_1 \le t \le t_2 & v_2=v_1+a_{max}T_2 \\ v_2+a_{max}\tau_3-\frac12j_{max}\tau_3^2 & t_2 \le t \le t_3 & v_3=v_2+\frac12j_{max}T_1^2 \\ v_3 & t_3 \le t \le t_4 & v_3=v_4 \\ v_4-\frac12j_{max}\tau_5^2 & t_4 \le t \le t_5 & v_5=v_4-\frac12j_{max}T_5^2 \\ v_5-a_{max}\tau_6 & t_5 \le t \le t_6 & v_6=v_5-a_{max}T_6 \\ v_6-a_{max}\tau_7+\frac12j_{max}\tau_7^2 & t_6 \le t \le t_7 & v_7=v_6-\frac12j_{max}T_5^2 \end{cases}$$
   $$S(t) = \begin{cases} v_s\tau_1+\frac16j_{max}\tau_1^3 & 0 \le t \le t_1 & t=t_1时,S_1=v_sT_1+\frac16j_{max}T_1^3 \\ S_1+v_1\tau_2+\frac12j_{max}T_1\tau_2^2 & t_1 \le t \le t_2 & t=t_2时,S_2=S_1+v_1T_2+\frac12j_{max}T_1T_2^2 \\ S_2+v_2\tau_3+\frac12j_{max}T_1\tau_3^2-\frac16j_{max}\tau_3^3 & t_2 \le t \le t_3 & t=t_3时,S_3=S_2+v_2T_1+\frac13j_{max}T_1^3 \\ S_3+v_3\tau_4 & t_3 \le t \le t_4 & t=t_4时,S_4=S_3+v_3T_4 \\ S_4+v_4\tau_5-\frac16j_{max}\tau_5^3 & t_4 \le t \le t_5 & t=t_5时,S_5=S_4+v_4T_5-\frac16j_{max}T_5^3 \\ S_5+v_5\tau_6-\frac12j_{max}T_5\tau_6^2 & t_5 \le t \le t_6 & t=t_6时,S_6=S_5+v_5T_6-\frac12j_{max}T_5T_6^2 \\ S_6+v_6\tau_7-\frac12j_{max}T_5\tau_7^2+\frac16j_{max}\tau_7^3 & t_6 \le t \le t_7 & t=t_7时,S_7=S_6+v_6T_5-\frac13j_{max}T_5^3 \end{cases}$$

2. 几条关键中间计算公式推导
   

   $$T_1=T_3，T_5=T_7$$
   $$T_2={v_{max}-v_s \over a_{max}}-T_1$$
   $$T_6={v_{max}-v_e \over a_{max}}-T_5$$
   当系统可以达到最大加速度时，有$T_1=T_3=T_5=T_7$。若此时有$v_s=v_e$时，则 $$\begin{cases}T_2=T_6 \\ j_{max}T_1=j_{max}T_5=a_{max} \\ v_3=v_4=v_{max}=v_s+j_{max}T_1T_2+j_{max}T_1^2 \end{cases}$$
   $$v_7=v_s+j_{max}T_1(T_1+T_2)-j_{max}T_5(T_5+T_6)=v_e$$
   加速区长度$S_a=v_s(2T_1+T_2)+\frac12j_{max}T_1(2T_1^2+3T_1T_2+T_2^2)$
   减速区长度$S_d=v_3(2T_5+T_6)-\frac12j_{max}T_5(2T_5^2+3T_5T_6+T_6^2）$
   规划路径的长度$S_7=S_a+v_{max}T_4+S_d$

3. 规划
   系统的最大加加速度为J，进给速度为F，最大加速度为$A_{max}$,并且$v_s=v_e$。

   1. 当系统刚好能够达到最大加速度$A_{max}$，且无匀加减速和匀速段
      
      $T_2=T_6=T_4=0$
      $T_1=T_3=T_5=T_7$
      $T_1=\frac {A_{max}}{J}$
      $v_{max}=v_3=v_s+j_{max}T_1^2$
      系统的规划路径长度
      $S_{ref}=S_7=2v_sT_1+j_{max}T_1^3+2v_3T_5-j_{max}T_5^3 =2v_sT_1+2v_3T_5$。

   2. 当$L < S_{ref},F > v_{max}$，L的长度不足以系统增大至最大加速度$A_{max}$。加速区和减速区各为$L$的一半。
      
      $S_a=JT_1^3+2v_sT_1=\frac L2$，解方程得
      

      $$a={L\sqrt{J}\over4}$$
      $$b={\sqrt{{L^2J\over16}+\frac{8v_s^3}{27}}}$$
      $$T_1=\frac{\sqrt[3]{a+b} +\sqrt[3]{a-b}}{\sqrt{J}}$$
      $$v_{max}=JT_1^2+v_s$$

   3. 当$L<S_{ref},F<v_{max}$，此时有匀速段。
      
      

      $$T_1=\sqrt{F-v_s\over J}$$
      $$S_a=JT_1^3+2v_sT_1$$
      $$T_4=\frac{L-2S_a}F$$

   4. 当$L>S_{ref},F<v_{max}$，此种情形与上同。

   5. 当$L>S_{ref},F>v_{max}$，L足够长以致$a_{max}$有足够的时间来增大至$A_{max}$，并且$v_{max}$没有超过进给速度$F$，此时必定产生匀加减速段，假设$v_{max}$刚好增大至$F$。
      $$T_5=T_1=\frac{A_{max}}J$$
      $$T_2={F-v_s \over A_{max}}-T_1$$
      $$T_6={F-v_e \over A_{max}}-T_5$$
      $$S'_{ref}={F+v_s\over2}(2T_1+T_2)+{F+v_e\over2}(2T_5+T_6)$$
      若$L<S'_{ref}$，$L$不够长以致$v_{max}$无法增大至$F$，重新规划，匀加减速段减小。解方程组 $$\begin{cases}T_5=T_1=\frac{A_{max}}J \\ T_2={v_{max}-v_s \over A_{max}}-T_1 \\ T_6={v_{max}-v_e \over A_{max}}-T_5 \\ L={v_{max}+v_s\over2}(2T_1+T_2)+{v_{max}+v_e\over2}(2T_5+T_6) \end{cases}$$ ，带入化简得
      $$2Jv_{max}^2+2A_{max}^2v_{max}+A_{max}^2(v_s+v_e)-J(v_s^2+v_e^2)-2JA_{max}L=0$$
      $$v_{max}={-A_{max}^2\over2J}+{\sqrt{A_{max}^4-2J[A_{max}^2(v_s+v_e)-J(v_s^2+v_e^2)-2A_{max}JL]}\over2J}$$ 。
      解得$v_{max}$带入方程组求得$T_2,T_6$。
      若$L>S'_{ref}$，L足够长以致$v_{max}$能够增大至$F$，并且产生匀速段，此时有完整的7段S形曲线。
      
      $$T_4={L-S'_{ref}\over v_{max}}$$ 。

基于LinuxCNC的机器人核心控制算法研究
数控系统S形曲线加减速快速规划研究
S曲线加减速算法研究
S曲线加减速控制新方法的研究
Motion Planning for Archwire Bending Robot in Orthodontic Treatments