文章目录
 
一、7种速度规划对比
1、梯形速度规划
1.1 只有一段
1.1.1 加速段
1.1.2匀速段
1.1.3减速段
    
1.2 只有两段
1.2.1 加速段+匀速段
1.2.2匀速段 +减速段
1.2.3加速段+减速段
    
1.3 有三段(加速段+匀速段 +减速段)
   
2、余弦速度规划
2.1 只有一段
2.1.1 加速段
2.1.2 匀速段
2.1.3 减速段
    
2.2 只有两段
2.2.1 加速段+匀速段
2.2.2 匀速段+减速段
2.2.2 加速段+减速段
    
2.3 有三段(加速段+匀速段 +减速段)
   
3、多项式速度规划
3.1 只有一段
3.1.1 加速段
3.1.2 匀速段
3.1.3 减速段
    
2.2 只有两段
3.2.1 加速段+匀速段
3.2.2 匀速段+减速段
3.2.2 加速段+减速段
    
3.3 有三段(加速段+匀速段 +减速段)
   
4、7段S型速度规划
4.1 只有加速段
4.1.1 无匀加速段
4.1.2 有匀加速段
    
4.2 只有匀速段
4.3 只有减速段
4.3.1 无匀减速段
4.3.2 有匀减速段
    
4.4 只有加速段+匀速段
4.4.1 无匀加速段
4.4.2 有匀加速段
    
4.5 只有匀速段+减速段
4.5.1 无匀减速段
4.5.2 有匀减速段
    
4.6 只有加速段+减速段
4.6.1 无匀加速段、无匀减速段
4.6.2 有匀加速段、无匀减速段
4.6.3 无匀加速段、有匀减速段
4.6.4有匀加速段、有匀减速段
    
4.7 加速段+匀速段+减速段
4.7.1 无匀加速段、无匀减速段
4.7.2 有匀加速段、无匀减速段
4.7.3 无匀加速段、有匀减速段
4.7.4有匀加速段、有匀减速段
   
   
5、7段修正S型速度规划
5.1 只有加速段
5.1.1 无匀加速段
5.1.2 有匀加速段
    
5.2 只有匀速段
5.3 只有减速段
5.3.1 无匀减速段
5.3.2 有匀减速段
    
5.4 只有加速段+匀速段
5.4.1 无匀加速段
5.4.2 有匀加速段
    
5.5 只有匀速段+减速段
5.5.1 无匀减速段
5.5.2 有匀减速段
    
5.6 只有加速段+减速段
5.6.1 无匀加速段、无匀减速段
5.6.2 有匀加速段、无匀减速段
5.6.3 无匀加速段、有匀减速段
5.6.4有匀加速段、有匀减速段
    
5.7 加速段+匀速段+减速段
5.7.1 无匀加速段、无匀减速段
5.7.2 有匀加速段、无匀减速段
5.7.3 无匀加速段、有匀减速段
5.7.4有匀加速段、有匀减速段
   
   
6、15段S型速度规划
7、31段S型速度规划
  
二、7种速度规划示例演示

 
一、7种速度规划对比
 
  一般地，速度规划输入参数为：长度$L$，起始速度$v_s$，结束速度$v_e$，最大速度$v_c$，加速度$acc$，减速度$dec$，二次加速度$jerk$，三次加速度$snap$，四次加速度$crackle$。下面通过对比7种速度规划生成的位置、速度、加速度、二次加速度、三次加速度及四次加速度曲线的连续性与平滑性，可控性(是否可以限制在给定值以内)，规划总时间长短，算法计算量四个方面来比较7种速度规划算法。
 
1、梯形速度规划
 
  (1) 位置平滑，速度连续但不平滑，加速度跳变，二次加速度出现无穷大跳变;
   (2) 速度、加速度可控，更高阶不可控；
   (3) 所有速度规划算法中规划时间最短;
   (4) 算法最简单、计算量最小。
 
1.1 只有一段
 
1.1.1 加速段
 
  当$L=1,v_s=0,v_c=10,v_e=10,acc=50,dec=123(任意)$时，只有加速段。
 
 
1.1.2匀速段
 
  当$L=1,v_s=1,v_c=1,v_e=1,acc=123(任意),dec=123(任意)$时，只有匀速段。
 
 
1.1.3减速段
 
  当$L=1,v_s=10,v_c=10,v_e=0,acc=123(任意),dec=50$时，只有减速段。
 
 
1.2 只有两段
 
1.2.1 加速段+匀速段
 
  当$L=1,v_s=0,v_c=10,v_e=10,acc=80,dec=123(任意)$时，只有加速段+匀速段 。
 
 
1.2.2匀速段 +减速段
 
  当$L=1,v_s=10,v_c=10,v_e=0,acc=123(任意),dec=80$时，只有匀速段 +减速段。
 
 
1.2.3加速段+减速段
 
  当$L=1,v_s=1,v_c=6,v_e=2,acc=20,dec=30$时，只有加速段+减速段 。
 
 
1.3 有三段(加速段+匀速段 +减速段)
 
  当$L=1,v_s=1,v_c=3,v_e=2,acc=40,dec=30$时，有加速段+匀速段 +减速段。
 
 
2、余弦速度规划
 
  (1) 位置平滑，速度平滑，加速度连续但不够平滑，二次加速度跳变;
   (2) 速度、加速度可控，更高阶不可控；
   (3) 规划时间较长;
   (4) 算法较简单、计算量较小。
 
2.1 只有一段
 
2.1.1 加速段
 
  当$L=1,v_s=0,v_c=10,v_e=10,acc=78.5398,dec=123(任意)$时，只有加速段。
 
 
2.1.2 匀速段
 
  当$L=1,v_s=1,v_c=1,v_e=1,acc=123(任意),dec=123(任意)$时，只有匀速段。
 
 
2.1.3 减速段
 
  当$L=1, v_s=10, v_c=10, v_e=0, acc =123(任意), dec=78.5398时，只有减速段。
 
 
2.2 只有两段
 
2.2.1 加速段+匀速段
 
  当$L=1,v_s=0,v_c=10,v_e=10,acc=100,dec=123(任意)$时，只有加速段+匀速段 。
 
 
2.2.2 匀速段+减速段
 
  当$L=1,v_s=10,v_c=10,v_e=0,acc=123(任意),dec=100$时，只有匀速段 +减速段。
 
 
2.2.2 加速段+减速段
 
  当$L=1,v_s=1,v_c=6,v_e=2,acc=20,dec=30$时，只有加速段+减速段 。
 
 
2.3 有三段(加速段+匀速段 +减速段)
 
  当$L=1,v_s=1,v_c=3,v_e=2,acc=40,dec=30$时，有加速段+匀速段 +减速段。
 
 
3、多项式速度规划
 
  (1) 位置平滑，速度平滑，加速度平滑，二次加速度连续但不够平滑，三次加速度跳变;
   (2) 速度、加速度、二次加速度可控，更高阶不可控；
   (3) 规划时间长;
   (4) 算法较简单、计算量较小。
 
3.1 只有一段
 
3.1.1 加速段
 
  当$L=1,v_s=0,v_c=10,v_e=10,acc=93.75,dec=123(任意),jerk=2000$时，只有加速段。
 
 
3.1.2 匀速段
 
  当$L=1,v_s=1,v_c=1,v_e=1,acc=123(任意),dec=123(任意),jerk=123(任意)$时，只有匀速段。
 
 
3.1.3 减速段
 
  当$L=1,v_s=10,v_c=10,v_e=0,acc=123(任意),dec=93.75,jerk=2000$时，只有减速段。
 
 
2.2 只有两段
 
3.2.1 加速段+匀速段
 
  当$L=1,v_s=0,v_c=10,v_e=10,acc=150,dec=123(任意),jerk=2000$时，只有加速段+匀速段 。
 
 
3.2.2 匀速段+减速段
 
  当$L=1,v_s=10,v_c=10,v_e=0,acc=123(任意),dec=150,jerk=2000$时，只有匀速段 +减速段。
 
 
3.2.2 加速段+减速段
 
  当$L=1,v_s=1,v_c=6,v_e=2,acc=20,dec=30,jerk=2000$时，只有加速段+减速段 。
 
 
3.3 有三段(加速段+匀速段 +减速段)
 
  当$L=1,v_s=1,v_c=3,v_e=2,acc=40,dec=30,jerk=2000$时，有加速段+匀速段 +减速段。
 
 
4、7段S型速度规划
 
  (1) 位置平滑，速度平滑，加速度连续但不平滑，二次加速度跳变;
   (2) 速度、加速度、二次加速度可控，更高阶不可控；
   (3) 规划时间较短;
   (4) 算法稍复杂、计算量稍大。
 
4.1 只有加速段
 
4.1.1 无匀加速段
 
$L=0.36742346,v_s=0,v_c=3,v_e=3,acc=40,dec=123(任意),jerk=200$
 
 
4.1.2 有匀加速段
 
$L=0.24,v_s=0,v_c=3,v_e=3,acc=30,dec=123(任意),jerk=500$
 
 
4.2 只有匀速段
 
$L=1,v_s=1,v_c=1,v_e=1,acc=123(任意),dec=123(任意),jerk=123(任意)$
 
 
4.3 只有减速段
 
4.3.1 无匀减速段
 
$L=0.36742346,v_s=3,v_c=3,v_e=0,acc=123(任意),dec=40,jerk=200$
 
 
4.3.2 有匀减速段
 
$L=0.24,v_s=3,v_c=3,v_e=0,acc=123(任意),dec=30,jerk=500$
 
 
4.4 只有加速段+匀速段
 
4.4.1 无匀加速段
 
$L=0.36742346,v_s=0,v_c=2,v_e=2,acc=40,dec=123(任意),jerk=200$
 
 
4.4.2 有匀加速段
 
$L=0.24,v_s=0,v_c=3,v_e=3,acc=30,dec=123(任意),jerk=1000$
 
 
4.5 只有匀速段+减速段
 
4.5.1 无匀减速段
 
$L=0.36742346,v_s=2,v_c=2,v_e=0,acc=123(任意),dec=40,jerk=200$
 
 
4.5.2 有匀减速段
 
$L=0.24,v_s=3,v_c=3,v_e=0,acc=123(任意),dec=30,jerk=1000$
 
 
4.6 只有加速段+减速段
 
4.6.1 无匀加速段、无匀减速段
 
$L=1,v_s=1,v_c=6,v_e=2,acc=60,dec=80,jerk=500$
 
 
4.6.2 有匀加速段、无匀减速段
 
$L=1,v_s=1,v_c=6,v_e=2,acc=50,dec=80,jerk=800$
 
 
4.6.3 无匀加速段、有匀减速段
 
$L=1,v_s=2,v_c=6,v_e=1,acc=80,dec=60,jerk=800$
 
 
4.6.4有匀加速段、有匀减速段
 
$L=1,v_s=2,v_c=6,v_e=1,acc=40,dec=50,jerk=800$
 
 
4.7 加速段+匀速段+减速段
 
4.7.1 无匀加速段、无匀减速段
 
$L=1,v_s=2,v_c=4,v_e=1,acc=40,dec=50,jerk=400$
 
 
4.7.2 有匀加速段、无匀减速段
 
$L=1,v_s=2,v_c=4,v_e=1,acc=20,dec=40,jerk=400$
 
 
4.7.3 无匀加速段、有匀减速段
 
$L=1,v_s=2,v_c=4,v_e=1,acc=40,dec=30,jerk=400$
 
 
4.7.4有匀加速段、有匀减速段
 
$L=1,v_s=2,v_c=4,v_e=1,acc=30,dec=40,jerk=800$
 
 
5、7段修正S型速度规划
 
  (1) 位置平滑，速度平滑，加速度连续但不够平滑，二次加速度跳变;
   (2) 速度、加速度、二次加速度可控，更高阶不可控；
   (3) 规划时间较短;
   (4) 算法稍复杂、计算量稍大。
 
5.1 只有加速段
 
5.1.1 无匀加速段
 
$L=0.496729413289805,v_s=0,v_c=10,v_e=10,acc=200,dec=123(任意),jerk=5000$
 
 
5.1.2 有匀加速段
 
$L=0.614159265358979,v_s=0,v_c=10,v_e=10,acc=100,dec=123(任意),jerk=5000$
 
 
5.2 只有匀速段
 
$L=1,v_s=1,v_c=1,v_e=1,acc=123(任意),dec=123(任意),jerk=123(任意)$
 
 
5.3 只有减速段
 
5.3.1 无匀减速段
 
$L=0.496729413289805,v_s=10,v_c=10,v_e=0,acc=123(任意),dec=200,jerk=5000$
 
 
5.3.2 有匀减速段
 
$L=0.614159265358979,v_s=10,v_c=10,v_e=0,acc=123(任意),dec=100,jerk=5000$
 
 
5.4 只有加速段+匀速段
 
5.4.1 无匀加速段
 
$L=0.61415926,v_s=0,v_c=10,v_e=10,acc=300,dec=123(任意),jerk=5000$
 
 
5.4.2 有匀加速段
 
$L=0.61415926,v_s=0,v_c=10,v_e=10,acc=150,dec=123(任意),jerk=8000$
 
 
5.5 只有匀速段+减速段
 
5.5.1 无匀减速段
 
$L=0.61415926,v_s=10,v_c=10,v_e=0,acc=123(任意),dec=300,jerk=5000$
 
 
5.5.2 有匀减速段
 
$L=0.61415926,v_s=10,v_c=10,v_e=0,acc=123(任意),dec=150,jerk=8000$
 
 
5.6 只有加速段+减速段
 
5.6.1 无匀加速段、无匀减速段
 
$L=1,v_s=1,v_c=6,v_e=2,acc=60,dec=80,jerk=500$
 
 
5.6.2 有匀加速段、无匀减速段
 
$L=1,v_s=1,v_c=6,v_e=2,acc=40,dec=80,jerk=1200$
 
 
5.6.3 无匀加速段、有匀减速段
 
$L=1,v_s=2,v_c=6,v_e=1,acc=80,dec=40,jerk=1200$
 
 
5.6.4有匀加速段、有匀减速段
 
$L=1,v_s=2,v_c=6,v_e=1,acc=40,dec=30,jerk=1200$
 
 
5.7 加速段+匀速段+减速段
 
5.7.1 无匀加速段、无匀减速段
 
$L=1,v_s=2,v_c=3,v_e=1,acc=40,dec=50,jerk=400$
 
 
5.7.2 有匀加速段、无匀减速段
 
$L=1,v_s=2,v_c=3,v_e=1,acc=20,dec=40,jerk=1000$
 
 
5.7.3 无匀加速段、有匀减速段
 
$L=1,v_s=2,v_c=3,v_e=1,acc=40,dec=20,jerk=1000$
 
 
5.7.4有匀加速段、有匀减速段
 
$L=1,v_s=2,v_c=4,v_e=1,acc=30,dec=40,jerk=3000$
 
 
6、15段S型速度规划
 
 
  (1) 位置平滑，速度平滑，加速度平滑，二次加速度连续但不平滑，三次加速度跳变;
   (2) 速度、加速度、二次加速度、三次加速度可控，四次加速度不可控；
   (3) 规划时间稍长;
   (4) 算法较复杂、计算量较大。
 
7、31段S型速度规划
 
 
  (1) 位置平滑，速度平滑，加速度平滑，二次加速度平滑，三次加速度连续但不平滑，四次加速度跳变;
   (2) 位置的各阶导皆可控；
   (3) 规划时间稍长;
   (4) 算法较复杂、计算量较大。
 
   总体来说：
   连续性与平滑性：31段S型 $>$ 多项式 $\approx$ 15段S型 $>$ 7段修正S型 $>$ 余弦 $>$ 7段S型 $>$ 梯形；
   可控性：31段S型$>$ 15段S型 $>$ 7段修正S型 $=$ 7段S型 $=$ 多项式 $>$ 余弦 $=$ 梯形；
   规划总时间：多项式 $>$ 余弦 $>$ 31段S型 $>$ 15段S型 $>$ 7段修正S型 $>$ 7段S型 $>$ 梯形；
   算法计算量：31段S型 $>$ 15段S型 $>$ 7段修正S型 $>$ 7段S型 $>$ 多项式 $\approx$ 余弦 $>$ 梯形。
   根据以上的对比，7种速度规划算法大致可分为低中高端三档。
   低档：梯形、余弦(普通学者，自身“阶数”较低，专业技能浮于表面，自律能力较差)
   中档：多项式、7段S型、7段修正S型(专家，自身“阶数”较高，专业技能深入，需要深入了解才能知道其学术造诣之深，自律能力强)
   高档：15段S型、31段S型(大师，自身“阶数”高，专业技能出神入化，需要一定造诣的人深入洞悉才能领悟其所思所为之精妙，自律能力非常人所能及)
 
二、7种速度规划示例演示
 
  笔者采用统一的框架实现了以上7种速度规划算法，所有核心算法均用c语言实现，不调用第三方算法库，采用QT实现图形用户界面。下载链接:robot_velocity_planning _V2.0.rar。下面是示例演示，取长度$L=300$，起始速度$v_s=100$，结束速度$v_e=200$，最大速度$v_c=500$，加速度$acc=2000$，减速度$dec=3000$，二次加速度$jerk=50000$，三次加速度$snap=3000000$，四次加速度$crackle=500000000$。
 

 
 
S型曲线主函数
 
目录
 
S型曲线主函数
 
S1段曲线
 
S2段曲线
 
S3段曲线末端速度倒推型
 
S3段曲线末端速度正推型
 
S4段曲线
 
S5段曲线
 
S6段曲线
 
S7段曲线倒推型
 
S7段曲线正推型
 

clc;clear;close;
 
GoalV=100;%%单位mm/s
 GoalS=100;%%单位mm
 Acc=400;
 Jerk=30;
 %%%%%%%%%%%%%%%%可以修改
 InitV=0;
 InitS=0;
 %%%%%%%%%%%%%%%%不可以修改
 Vs2=InitV;
 n=1000;
 T1=Acc/Jerk;
 Vtemp=Acc^2/Jerk;%%这个速度变化量包括了加加速度段和加减速度段的速度变化
 
Vmax=abs(GoalV-InitV);
 
Vs=InitV;%%所以不用担心速度跟不上的情况，只要考虑好如何优化分配位移的情况就行了。
 
J=Jerk;
 %T1=sqrt((((GoalV+Vs)/2-Vs)*2)/J);
 
[s(1),v(1),a(1)]=SProfileT1(0,Jerk,InitV,T1);
 T3=T1;
 t=0;
 J=-Jerk;
 Vc=2*(v(1)-InitV)+InitV;
 
s(3)=0+(double(1)/6)*J*T3.^3-(double(1)/2)*J*T3.^2*t+(double(1)/2)*J*T3*t.^2-(double(1)/6)*J*t.^3+Vc*T3-Vc*t;
 
J=Jerk;
 
s(7)=0+(double(1)/6)*J*T3.^3-(double(1)/2)*J*T3.^2*t+(double(1)/2)*J*T3*t.^2-(double(1)/6)*J*t.^3+Vs*T3-Vs*t;%%这部分只是计算相对T7段的位移变化量
 J=-Jerk;
 s(5)=0+Vc*T1+(double(1)/6)*J*T1.^3+(double(1)/2)*0*T1.^2;
 
S(4)=s(3)+s(5)+s(1)+s(7);%%这是四段曲线位移的分界点
 J=Jerk;
 S(4)=4*InitV*T1+2*J*T1.^3;%%这个没加初始的位移变化量，所以前面的计算的s(1)、s(3)、s(5)、s(7)都没有加初始的位移
     
 if Vmax<=Vtemp%%速度最大变化量小于加加速度段和减减速度段的速度变化量，此时速度曲线分为四段或五段，此时看位移的情况当前的情况下速度都是可以跟的上的
     
     if S(4)<abs(GoalS-InitS) %&& InitV==Vs2  %%就用五段曲线 else是用四段曲线
           
             t1=linspace(0,T1,n);
             t=T1;
             [St{1},Vt{1},At{1}]=SProfileT1(InitS,Jerk,InitV,t1);
 
          
             t3=linspace(t,2*t,n);
             [St{3},Vt{3},At{3}]=SProfileT3(T1,-Jerk,GoalV,t3-t,S(4)/2+InitS,0);
             
             %[St{3},Vt{3},At{3}]=SProfileT32(At{1}(n),-Jerk,Vt{1}(n),t3-t,St{1}(n));
             t=2*t;
           
             T4=abs(S(4)-abs(GoalS-InitS))/GoalV;
             t4=linspace(t,t+T4,n);
             [St{4},Vt{4},At{4}]=SProfileT4(St{3}(n),GoalV,t4-t);
             t=t+T4;
            
             t5=linspace(t,t+T1,n);
             [St{5},Vt{5},At{5}]=SProfileT5(-Jerk,GoalV,t5-t,St{4}(n),At{4}(n));
             t=t+T1;
            
             t7=linspace(t,t+T1,n);
             [St{7},Vt{7},At{7}]=SProfileT7(T1,Jerk,InitV,t7-t,GoalS );
             
             %[St{7},Vt{7},At{7}]= SProfileT72(At{5}(n),J,Vt{5}(n),t7-t,St{5}(n));
             t=t+T1;
            
             
             figure('name','五段位置曲线');
             hold on
             plot(t1,St{1});
             plot(t3,St{3});
             plot(t4,St{4});
             plot(t5,St{5},'y');
             plot(t7,St{7},'b');
             figure('name','五段速度曲线');
             hold on
             plot(t1,Vt{1});
             plot(t3,Vt{3});
             plot(t4,Vt{4},'r');
             plot(t5,Vt{5},'y');
             plot(t7,Vt{7},'b');
             
             figure('name','五段加速度曲线');
             hold on
             plot(t1,At{1});
             plot(t3,At{3});
             plot(t4,At{4});
             plot(t5,At{5},'y');
             plot(t7,At{7},'b');
     elseif S(4)>=abs(GoalS-InitS)
            
             sc=abs(GoalS-InitS);
             J=Jerk;
             fun=@(T)(sc-4*InitV*T-2*J*T.^3);
             T=fsolve(fun,0.1);
             
             Vs=InitV;
             J=Jerk;
 
            t1=linspace(0,T,n);
             t=T;
             [St{1},Vt{1},At{1}]=SProfileT1(InitS,Jerk,Vs,t1);
             
          
             J=-Jerk;
             
             %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            
             sm=0
             vm=Vt{1}(n);
             am=At{1}(n);
 
            t3=linspace(t,t+T,n);
             [St{3},Vt{3},At{3}]=SProfileT32(am,J,vm,t3-t,St{1}(n));
             t=T+T;
            
             
             J=-Jerk;
             Vc=Vt{3}(n);
             a4=At{3}(n);
             s4=St{3}(n);
 
            t5=linspace(t,t+T,n);
             [St{5},Vt{5},At{5}]=SProfileT5(J,Vc,t5-t,s4,a4);
             t=t+T;
 
            J=Jerk;
 
            t7=linspace(t,t+T,n);
             [St{7},Vt{7},At{7}]= SProfileT72(At{5}(n),J,Vt{5}(n),t7-t,St{5}(n));
             t=t+T;
             figure('name','四段位置曲线');
             hold on
             plot(t1,St{1});
             plot(t3,St{3});
             plot(t5,St{5},'y');
             plot(t7,St{7},'b');
             figure('name','四段速度曲线');
             hold on
             plot(t1,Vt{1});
             plot(t3,Vt{3});
             plot(t5,Vt{5},'y');
             plot(t7,Vt{7},'b');
             
             figure('name','四段加速度曲线');
             hold on
             plot(t1,At{1});
             plot(t3,At{3});
             plot(t5,At{5},'y');
             plot(t7,At{7},'b');
     end
 else
                  if S(4)<abs(GoalS-InitS) %%可分为六段和七段大于四段的位移量了
                     %%先计算六段的位移总量，采用的时间间隔都是一样
                         t=T1;
                         J=Jerk;
                         s(1)=0+(double(1)/6)*J*t.^3+Vs*t;
                         [s(1),v(1),a(1)]=SProfileT1(0,J,InitV,T1);
                         [s(2),v(2),a(2)]=SProfileT2(T1,J,v(1),T1,0);
                         J=-Jerk;
                         [s(3),v(3),a(3)]=SProfileT32(a(2),J,v(2),T1,0);
                         [s(5),v(5),a(5)]=SProfileT5(J,v(3),T1,0,0);
                         [s(6),v(6),a(6)]=SProfileT6( T1,J,v(5),T1,0);
                         J=Jerk;
                         [s(7),v(7),a(7)]=SProfileT72(a(6),J,v(6),T1,0);
                         S(6)=s(1)+s(2)+s(3)+s(5)+s(6)+s(7);
                         if S(6)<=abs(GoalS-InitS);%%大于六段的时候的位移
 
                                    J=Jerk;
                                     t1=linspace(0,T1,n);
                                     [St{1},Vt{1},At{1}]=SProfileT1(InitS,J,InitV,t1);
                                     t=T1;
 
                                    t2=linspace(t,t+T1,n);
                                     [St{2},Vt{2},At{2}]=SProfileT2(T1,J,Vt{1}(n),t2-t,St{1}(n));
                                     t=t+T1;
 
                                    J=-Jerk;
                                     t3=linspace(t,t+T1,n);
                                     [St{3},Vt{3},At{3}]=SProfileT32(At{2}(n),J,Vt{2}(n),t3-t,St{2}(n));
                                     t=t+T1;
 
                                    T4=abs(S(6)-abs(GoalS-InitS))/Vt{3}(n);
                                     t4=linspace(t,t+T4,n);
                                     [St{4},Vt{4},At{4}]=SProfileT4(St{3}(n),Vt{3}(n),t4-t);
                                     t=t+T4;
 

                                     t5=linspace(t,t+T1,n);
                                     [St{5},Vt{5},At{5}]=SProfileT5(J,Vt{4}(n),t5-t,St{4}(n),0);
                                     t=t+T1;
 
                                    t6=linspace(t,t+T1,n);
                                     [St{6},Vt{6},At{6}]=SProfileT6( T1,J,Vt{5}(n),t6-t,St{5}(n));
                                     t=t+T1;
 
                                    t7=linspace(t,t+T1,n);
                                     J=Jerk;
                                     [St{7},Vt{7},At{7}]=SProfileT72(At{6}(n),J,Vt{6}(n),t7-t,St{6}(n));
 
                                    t=t+T1;
                                 figure('name','七段位置曲线');
                                 hold on
                                 plot(t1,St{1});
                                 plot(t2,St{2});
                                 plot(t3,St{3});
                                 plot(t4,St{4});
                                 plot(t5,St{5},'y');
                                 plot(t6,St{6},'r');
                                 plot(t7,St{7},'b');
                                 figure('name','七段速度曲线');
                                 hold on
                                 plot(t1,Vt{1});
                                 plot(t2,Vt{2});
                                 plot(t3,Vt{3});
                                 plot(t4,Vt{4});
                                 plot(t5,Vt{5},'y');
                                 plot(t6,Vt{6},'r');
                                 plot(t7,Vt{7},'b');
 
                                figure('name','七段加速度曲线');
                                 hold on
                                 plot(t1,At{1});
                                 plot(t2,At{2});
                                 plot(t3,At{3});
                                 plot(t4,At{4});
                                 plot(t5,At{5},'y');
                                 plot(t6,At{6});
                                 plot(t7,At{7},'b');
                      else 
                                 
                                 st=abs(GoalS-InitS)/2;
                                 
                                 T1=Acc/Jerk;
                                 J=Jerk;
                                 t1=linspace(0,T1,n);
                                 [St{1},Vt{1},At{1}]=SProfileT1(InitS,J,InitV,t1);
                                 t=T1;
                                 
                                 
                                 
                                 fun=@(tp)(-st+(double(1)/6)*J*T1.^3+InitV*T1+0+Vt{1}(n)*tp+(double(1)/2)*J*T1*tp.^2+(double(1)/6)*(-1*J)*T1.^3+(double(1)/2)*(J*T1+0*tp)*T1.^2+(Vt{1}(n)+J*T1*tp)*T1);
                                 T2=fsolve(fun,0.1);
                                 
                                 
                                 t2=linspace(t,t+T2,n);
                                 [St{2},Vt{2},At{2}]=SProfileT2(T1,J,Vt{1}(n),t2-t,St{1}(n));
                                 t=t+T2;
                                 
                                  J=-Jerk;
                                 t3=linspace(t,t+T1,n);
                                 [St{3},Vt{3},At{3}]=SProfileT32(At{2}(n),J,Vt{2}(n),t3-t,St{2}(n));
                                 t=t+T1;
                                 
                                 t5=linspace(t,t+T1,n);
                                 [St{5},Vt{5},At{5}]=SProfileT5(J,Vt{3}(n),t5-t,St{3}(n),0);
                                 t=t+T1;
                                 
                                 t6=linspace(t,t+T2,n);
                                 [St{6},Vt{6},At{6}]=SProfileT6( T1,J,Vt{5}(n),t6-t,St{5}(n));
                                 t=t+T2;
                                 
                                 t7=linspace(t,t+T1,n);
                                 J=Jerk;
                                 [St{7},Vt{7},At{7}]=SProfileT72(At{6}(n),J,Vt{6}(n),t7-t,St{6}(n));
 
                                t=t+T1;
                                 
                                 figure('name','六段位置曲线');
                                 hold on
                                 plot(t1,St{1});
                                 plot(t2,St{2});
                                 plot(t3,St{3});
                                
                                 plot(t5,St{5},'y');
                                 plot(t6,St{6},'r');
                                 plot(t7,St{7},'b');
                                 figure('name','六段速度曲线');
                                 hold on
                                 plot(t1,Vt{1});
                                 plot(t2,Vt{2});
                                 plot(t3,Vt{3});
                                
                                 plot(t5,Vt{5},'y');
                                 plot(t6,Vt{6},'r');
                                 plot(t7,Vt{7},'b');
 
                                figure('name','六段加速度曲线');
                                 hold on
                                 plot(t1,At{1});
                                 plot(t2,At{2});
                                 plot(t3,At{3});
                                
                                 plot(t5,At{5},'y');
                                 plot(t6,At{6});
                                 plot(t7,At{7},'b');
                         end
                  else
                     %%%%%%%%%%%%%%%%%%%%四段曲线    
                     sc=abs(GoalS-InitS);
                     J=Jerk;
                     fun=@(T)(sc-4*InitV*T-2*J*T.^3);
                     T=fsolve(fun,0.1);
 
                    Vs=InitV;
                     J=Jerk;
 
                    t1=linspace(0,T,n);
                     t=T;
                     [St{1},Vt{1},At{1}]=SProfileT1(InitS,Jerk,Vs,t1);
 

                     J=-Jerk;
 
                    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
                    sm=0
                     vm=Vt{1}(n);
                     am=At{1}(n);
 
                    t3=linspace(t,t+T,n);
                     [St{3},Vt{3},At{3}]=SProfileT32(am,J,vm,t3-t,St{1}(n));
                     t=T+T;
 

                     J=-Jerk;
                     Vc=Vt{3}(n);
                     a4=At{3}(n);
                     s4=St{3}(n);
 
                    t5=linspace(t,t+T,n);
                     [St{5},Vt{5},At{5}]=SProfileT5(J,Vc,t5-t,s4,a4);
                     t=t+T;
 
                    J=Jerk;
 
                    t7=linspace(t,t+T,n);
                     [St{7},Vt{7},At{7}]= SProfileT72(At{5}(n),J,Vt{5}(n),t7-t,St{5}(n));
                     t=t+T;
                     figure('name','四段位置曲线');
                     hold on
                     plot(t1,St{1});
                     plot(t3,St{3});
                     plot(t5,St{5},'y');
                     plot(t7,St{7},'b');
                     figure('name','四段速度曲线');
                     hold on
                     plot(t1,Vt{1});
                     plot(t3,Vt{3});
                     plot(t5,Vt{5},'y');
                     plot(t7,Vt{7},'b');
 
                    figure('name','四段加速度曲线');
                     hold on
                     plot(t1,At{1});
                     plot(t3,At{3});
                     plot(t5,At{5},'y');
                     plot(t7,At{7},'b');
             end
 end
 
S1段曲线
 
function [ s,v,a ] = SProfileT1(S0,J,Vs,t)
 %UNTITLED2 Summary of this function goes here
 %   Detailed explanation goes here
 s=S0+(double(1)/6)*J*t.^3+Vs*t;
 v=(double(1)/2)*J*t.^2+Vs;
 a=J*t;
 end
 
S2段曲线
 
function [ s,v,a] = SProfileT2(T1,J,Vl,t,S0)%%备注一下这里的T2时间必须等于T1时间
 %UNTITLED3 Summary of this function goes here
 %   Detailed explanation goes here
 s=S0+Vl*t+(double(1)/2)*J*T1*t.^2;
 v=Vl+J*T1*t;
 a=J*T1+0*t;
 end
 
S3段曲线末端速度倒推型
 
function [ s,v,a] = SProfileT3( T3,J,Vc,t,St3,a3)
 %UNTITLED Summary of this function goes here
 %   Detailed explanation goes here
 v=Vc+(double(1)/2)*J*(T3-t).^2-a3*(T3-t);%%因为现在根据时间去求速度Vm，而速度V2的速度求解是按绝对时间来的，即可以按It的间隔来此时时间的总长就是T3即是It
 s=St3-(double(1)/6)*J*(T3-t).^3+(double(1)/2)*a3*(T3-t).^2-Vc*(T3-t);
 a=a3-J*(T3-t);
 end
 
S3段曲线末端速度正推型
 
function [ s,v,a ] = SProfileT32( am,J,vm,t,sm)
 %UNTITLED Summary of this function goes here
 %   Detailed explanation goes here
 a=am+J*t;
 v=vm+am*t+(double(1)/2)*J*t.^2;
 s=sm+(double(1)/6)*J*t.^3+(double(1)/2)*am*t.^2+vm*t;
 end
 
S4段曲线
 
function [ s,v,a ] = SProfileT4(S0,Vc,t)
 %UNTITLED Summary of this function goes here
 %   Detailed explanation goes here
 a=t*0;
 v=Vc+0*t;
 s=Vc*t+S0;
 end
 
S5段曲线
 
function [ s,v,a ] = SProfileT5(J,Vc,t,s4,a4)
 %UNTITLED2 Summary of this function goes here
 %   Detailed explanation goes here
 v=Vc+(double(1)/2)*J*t.^2+a4*t;
 s=s4+Vc*t+(double(1)/6)*J*t.^3+(double(1)/2)*a4*t.^2;
 a=a4+J*t;
 end
 
S6段曲线
 
function [ s,v,a] = SProfileT6( T5,J,Vm2,t,S5)
 %UNTITLED Summary of this function goes here
 %   Detailed explanation goes here
 v=Vm2+J*T5*t;
 s=S5+Vm2*t+(double(1)/2)*J*T5*t.^2;
 a=J*T5+0*t;
 end
 
S7段曲线倒推型
 
function [ s,v,a] = SProfileT7( T7,J,Vs2,t,St7 )
 %UNTITLED2 Summary of this function goes here
 %   Detailed explanation goes here
 v=Vs2+(double(1)/2)*J*(T7-t).^2;
 s=St7-(double(1)/6)*J*T7.^3+(double(1)/2)*J*T7.^2*t-(double(1)/2)*J*T7*t.^2+(double(1)/6)*J*t.^3-Vs2*T7+Vs2*t;
 a=-J*(T7-t);
 end
 
S7段曲线正推型
 
function [ s,v,a ] = SProfileT72( al2,J,vl2,t,sl2)
 %UNTITLED Summary of this function goes here
 %   Detailed explanation goes here
 a=al2+J*t;
 v=vl2+al2*t+(double(1)/2)*J*t.^2;
 s=sl2+(double(1)/6)*J*t.^3+(double(1)/2)*al2*t.^2+vl2*t;
 end

写在前面
 
学习代码都记录在个人github上，欢迎关注~
 
前面的博客已经写了关于空间直线与空间圆弧的常用插补算法，而这些都是单一路径，实际中并不实用。对于连续多段路径，传统方法是将多段路径细分，然后对每段路径采用首尾速度为0的加减速算法（S型曲线或梯形曲线），这就带来了频繁启停的问题，容易对机械臂造成冲击，同时运行时间较长。
 
下面我把前面博客中提到的非对称S型加减速算法与空间中多段路径相结合，实现多段路径平滑过渡，减少运行时间。简单画一个轮廓状的“S”字符：
 
本文方法：
 
 
速度与加速度曲线：
 
 
采用分段路径首尾速度为0的加减速控制方法：
 
 
速度与加速度曲线：
 
 
对比结果还是很明显滴~本文的方法运行时间更短了，没有频繁启停，轮廓曲线更圆滑。如果在加上动力学方面的约束，就更好了。这个以后再讨论。
 
预处理
 
“预处理”，顾名思义就是拿到了一个轮廓轨迹，先想想这个轨迹如何分段，分段后如何衔接。
 
一般工业机器人任务空间的运动轨迹都是由圆弧段和直线段组成的，对于多段连续直线路径而言，任意两段相连直线段的连接处需要平滑处理，这里选择建立圆弧模型进行路径拐角过渡。下面给出圆弧模型的建立过程：
 
 
$P_i$,$v_i,$$r_i$分别为用户设定的拾放路径目标点、速度和过渡半径；$P_{Ti}$,$v_{Ti}$,$C_i$分别为圆弧转接点、转接点的速度和圆心； $M_i$为两圆弧转接点的中点； ${\theta }_i$,$l_i$ 分别为拐角大小和角平分线； $d_i$为分割后的小路径段长度。
 
针对由 $P_0$,$P_1$,$P_2$构成的路径段转接模型，设计求解步骤为：
 
 
 
Matlab函数如下：
 
% 拾取放置操作分段路径转接模型
% 参数： 用户设定的三个拾放路径点P0,P1,P2的位置
%       过渡半径r
%       最大加速度Amax
%       插补周期t
% 返回值： 转接点Pt1，Pt2，分割后的小路径长度d1,d2,圆心C，圆心角theta
%         
function [Pt1, Pt2, d1, d2, C, theta] = PathSegmentTrans(P0, P1, P2, r, t)
% 求拐角theta
P1P0 = sqrt(power(P0(1) - P1(1), 2) + power(P0(2) - P1(2), 2) + power(P0(3) - P1(3), 2));
P1P2 = sqrt(power(P2(1) - P1(1), 2) + power(P2(2) - P1(2), 2) + power(P2(3) - P1(3), 2));
vec_P1P0 = [P0(1) - P1(1), P0(2) - P1(2), P0(3) - P1(3)];
vec_P1P2 = [P2(1) - P1(1), P2(2) - P1(2), P2(3) - P1(3)];
theta = acos((dot(vec_P1P0, vec_P1P2)) / (P1P0*P1P2));
% 求转接点Pt1、Pt2
vec_P1Pt1 = (r/tan(theta/2)/P1P0) * vec_P1P0;
vec_P1Pt2 = (r/tan(theta/2)/P1P2) * vec_P1P2;
Pt1 = P1 + vec_P1Pt1;
Pt2 = P1 + vec_P1Pt2;
% 求路径长度d1、弧长d2
d1 = sqrt(power(Pt1(1) - P0(1), 2) + power(Pt1(2) - P0(2), 2) + power(Pt1(3) - P0(3), 2));
d2 = (pi - theta) * r;
% % 求转接速度vt
% 这是考虑机械系统动力学匀速因素得到的转接速度
% a = sqrt(Amax * r);
% b = d2 / t;
% if (a > b)
%     vt  = b;
% else
%     vt = a;
% end
% 求圆心C
vec_Pt1M = (1/2) * (Pt2 - Pt1);
M = Pt1 + vec_Pt1M;
vec_P1M = M - P1;
P1M = sqrt(power(M(1) - P1(1), 2) + power(M(2) - P1(2), 2) + power(M(3) - P1(3), 2));
P1C = r / sin(theta/2);
vec_P1C = (P1C / P1M) * vec_P1M;
C = P1 + vec_P1C;
end
 
基于非对称S曲线的轨迹规划
 
核心方法：机器人学回炉重造（6）：关节空间规划方法——梯形加减速（与抛物线拟合的线性函数）、S型曲线规划
 
一些文献中给S型曲线加了很多修饰词，比如非对称、带约束啥的，其实就是我上面那篇博客的S曲线，首尾速度可以不一样，带有位移、速度以及加速度约束。
 
下面这幅图中，$T_1$ ~$T_3$对应路径段$P_0{P}_{T1}$ ,分为加加速段、匀加速段、减加速段和匀速段; $T_4{T}_5$ 对应过渡圆弧$P_{T1}{P}_{T2}$,为匀速运动; $T_6$ ~$T_9$对应路径段$P_{T2}{P}_{T3}$,分为减加速段、匀减速段、减减速段和匀速段。把下面这幅图和上面那篇博客中的S曲线对照着看，容易理解直线段$d_i$（i为奇数，1,3,5，……）均为加速段或者减速段，圆弧段$d_i$（i为偶数，2,4,6，……）均为匀速段，并且对于直线段而言，其最大速度为圆弧连接点的速度$v_{Ti}(v_{Ti}=v_{Ti+1})$。
 
 
对每段$P_i{P}_{i+1}{P}_{i+2}$进行S曲线加减速规划，得到转接点速度，也就是规划得到的曲线实际能够达到的最大速度vlim，Matlab函数如下：
 
% 获取转接运动参数（速度）
% 参数： 分段路径长度d1,d2,d3
%       P0、P1、P2的速度v0, v1, v2（P1为中间点）
%       最大加速度amax,最大加加速度jmax
% 返回值： 转接点速度vt（即为路径上能够达到的最大速度）
function [vt] = TransMotionPara(d1, d2, d3, v0, v1, v2, amax, jmax)
% 得到规划参数Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin
q0 = 0; q1 = d1 + d2 + d3
para = STrajectoryPara(q0, q1, v0, v2, v1, amax, jmax);
vt = para(10);
end
 
插补
 
这里先只讲位置插补
 
预处理过后，轨迹就分段成为了一大堆直线段和一大堆圆弧段，这里要做的就是对这些空间直线和空间圆弧进行插补，于是又回到老话题了……
 
这里直线段插补不多说，和前面博客中提到的一样，空间单一直线位置插补+ S型加减速曲线归一化处理，具体方法见基于带约束S型加减速曲线的空间直线插补与空间圆弧插补算法（Matlab）
 
圆弧段插补也大同小异，方法有所不同，但基本是采用局部坐标系的空间圆弧插补方法和齐次坐标变换原理。新建立的局部坐标系UVW如下图所示，下面图中点的下标和我上面写的有些不一样，忽略所有$P_{bi}$并将$P_{ei}$看成$P_{Ti}$就行了，我就没有重新画图了。以第一段轨迹为例，新坐标系建立过程如下：
 
以$P_{T1}$为原点，$\overrightarrow{P_0{P}_1}$为V轴，$\overrightarrow{P_2{P}_1}\times \overrightarrow{P_1{P}_0}$为W，
 
V轴单位向量为：$V=\frac{\overrightarrow{P_0{P}_1}}{\left|\overrightarrow{P_0{P}_1}\right|}$
 
W轴单位向量为：$W=\frac{\overrightarrow{P_2{P}_1}\times \overrightarrow{P_1{P}_0}}{\left|\overrightarrow{P_2{P}_1}\times \overrightarrow{P_1{P}_0}\right|}$
 
U轴单位向量为：$U=V\times W$
 
在新坐标系UVW上圆弧过渡段的插补点坐标$(x_i,y_i,z_i)$坐标值为：
 $$\begin{array}{c}{x}_i=r_i-r_i\cdot \cos \left(\frac{v{t}_i\cdot t}{r_i}\right)\\ y_i=r_i\cdot \sin \left(\frac{v{t}_i\cdot t}{r_i}\right)\\ z_i=0\end{array}$$
 其中，$r_i$为设定的该段圆弧的半径参数，$v{t}_i$为规划得到的过渡圆弧速度.
 
然后就是老生常谈的话题，把局部新坐标系中的圆弧通过左乘该坐标系相对基座标系的齐次变换矩阵从而变换回原来的XYZ值。
 
 
Matlab函数如下，我在函数中留了姿态插补四元数参数的位置，留着以后加姿态插补用：
 
% 过渡圆弧的插补算法，搭配ContinueSpaceLine使用
% 参数： Pt1,Pt2,P1(Pt1和Pt2的中间点),Qt1,Qt2
%       过渡圆弧弧长d、过渡半径r、插补周期t
%       圆弧插补速度vt
% 返回值： 插补点数N，插补时间Tt
function [x y z qk N Tt] = Transition_arc(Pt1, Pt2, P1, Qt1, Qt2, d, r, t, vt)
% 建立新坐标系UVW
% 新坐标轴V
vec_Pt1P1 = P1 - Pt1;
Pt1P1 = sqrt(power(P1(1) - Pt1(1), 2) + power(P1(2) - Pt1(2), 2) + power(P1(3) - Pt1(3), 2));
V = (1/Pt1P1) * vec_Pt1P1;
ox = V(1); oy = V(2); oz = V(3);
% 新坐标系W
vec_Pt2P1 = P1 - Pt2;
vec_W_ = cross(vec_Pt1P1, vec_Pt2P1);
W_ = sqrt(power(vec_W_(1), 2) + power(vec_W_(2), 2) + power(vec_W_(3), 2));
W = (1/W_) * vec_W_;
ax = W(1); ay = W(2); az = W(3);
% 新坐标系U
U = cross(V, W);
nx = U(1); ny = U(2); nz = U(3);
% 相对于基座标系{O-XYZ}， 新坐标系{C-UVW}的坐标变换矩阵
T = [nx ox ax Pt1(1);
     ny oy ay Pt1(2);
     nz oz az Pt1(3);
      0  0  0  1];

% 计算两个四元数之间的夹角
dot_q = Qt1.s*Qt2.s + Qt1.v(1)*Qt2.v(1) + Qt1.v(2)*Qt2.v(2) + Qt1.v(3)*Qt2.v(3);
if (dot_q < 0)
    dot_q = -dot_q;
end
% 插补时长
Tt = d / vt;
i = 1;
for j = 0: t: Tt
    % 位置插补
    x_(i) = r -  r * cos(vt*j/r);
    y_(i) = r * sin(vt*j/r);
    P = T*[x_(i); y_(i); 0; 1];
    x(i) = P(1);
    y(i) = P(2);
    z(i) = P(3);
    % 单位四元数球面线性姿态插补
    % 插值点四元数
    if (dot_q > 0.9995)
        k0 = 1-t;
        k1 = t;
    else
        sin_t = sqrt(1 - power(dot_q, 2));
        omega = atan2(sin_t, dot_q);
        k0 = sin((1-t)*omega) / sin(omega);
        k1 = sin(t*omega) / sin(omega);
    end
    qk(i) = Qt1 * k0 + Qt2 * k1;
    N = i;
    i = i + 1;
end

end
 
字符轮廓提取
 
CAD中画50mm*50mm的栅格图，在栅格图内绘制字体轮廓，然后提取关键点坐标。
 
以栅格左下角的点作为坐标原点，以水平方向为X轴，以垂直方向为Y轴建立平面直角坐标系，字符“S”的关键点坐标如图所示
 
 
提取坐标原点O的坐标值，然后用其他关键点的坐标值减去原点的，就能得到这些关键点在当前栅格坐标系下的坐标值，如下表
 
关键点
A
B
C
D
E
F
G
H
I
J
K
L
X
5
45
45
12
12
46
46
5
5
38
38
5
Y
0
0
22
28
39
39
45
46
22
16
7
7
ok完事儿~
 
拼起来
 
是的，最后总的程序就是把上面分段规划得到的一堆直线插补点和一堆圆弧插补点，按照时间顺序拼起来。总程序我就不放了，仁者见仁智者见智。
 
参考文献
 
[1]王斌锐,王涛,李正刚,陈立,陈迪剑.多路径段平滑过渡的自适应前瞻位姿插补算法[J].控制与决策,2019,34(06):1211-1218.
 [2]陈伟华. 工业机器人笛卡尔空间轨迹规划的研究[D].华南理工大学,2010.
 [3]类延超. 五自由度写字机器人系统研究[D].山东大学,2012.
 [4]王涛,陈立,郭振武,王斌锐,李振娜.基于圆弧转接和跨段前瞻的拾放操作轨迹规划[J/OL].计算机集成制造系统:1-13[2019-08-09]
 
 
2019.12.8
 发现一处问题：
 子程序Transition_arc中原 k0 = sin((1-t*omega)) / sin(omega);修改为 k0 = sin((1-t)*omega) / sin(omega);
 
 
写算法不易，要点积分ok把~
 很多朋友需要代码，每次发邮件也挺费劲，所以我就上传了文档供大家下载，仅供思路参考哈~
 这只是matlab理论代码，还是有很多bug的，实际运用写成c还是要根据实际情况修改很多很多。我的邮箱是neuxuyan@163.com，欢迎大家和我交流！
 下载链接如下：
 https://download.csdn.net/download/qq_26565435/12630819

 
 
 
   
   
  
梯形加减速（与抛物线拟合的线性函数）
 
  

 梯形曲线在电机控制中经常被用到，因为其速度曲线形状为梯形，因此被称为梯形速度曲线。
 一般情况下，用户给定起始速度、终止速度、加速度、减速度、最大速度以及起始时刻和终止时刻的位移参数，需要计算出加速段、匀速段以及减速段对应的时间Ta、Tv、Td，然后再根据位移、速度以及加速度公式计算轨迹。
 （1）首先需要计算用户给定参数下所能达到的最大速度。要使能够达到的速度最大，则只有加速段和减速段，没有匀速段。此时h = p1 - p0，有如下公式
 
 根据上述公式，可得到给定参数下所能达到的最大速度：
 
 （2）比较理论最大速度vf与用户限定最大速度vmax之间的大小。当vf<vmax时，说明系统能够达到的最大速度无法满足用户要求，此时匀速段的最大速度为vv = vf；当vf>vmax时，说明给定的参数可以达到用户限定的最大速度，但是又不能超过限定的最大速度，因此匀速段的最大速度为vv = vmax。
 （3）计算加速段、匀速段以及减速段的时间和位移，计算公式如下：
 
 （4）计算各时刻下的位移、速度以及加速度，公式如下：
 
 
  
%% 与抛物线拟合的线性函数（线性轨迹）
% 梯形加减速，可限制最大速度和最大加速度
% 用户给定起始速度、终止速度、加速度、减速度、最大速度及位移参数
% 该算法需要计算出加速段、匀速段以及减速段对应的时间Ta Tv Td
% t0 = 0, p0 = 5, p1 = 30, v0 = 50, vmax = 150, v1 = 20, aa = 1000
% ad = -1500
t0 = 2;
p0 = 5; p1 = 30;
v0 = 50; vmax = 150; v1 = 20;
aa = 1000; ad = -1500;
h = p1 - p0;
% 可达到的最大速度
vf = sqrt((2.0*aa*ad*h - aa*v1^2 + ad*v0^2) / (ad - aa));
% 确定匀速阶段速度
if (vf < vmax)
    vv = vf;
else
    vv = vmax;
end
% 计算加速阶段的时间和位移
Ta = (vv - v0) / aa;
La = v0*Ta + (1.0/2.0)*aa*Ta^2;
% 计算匀速阶段的时间和位移
Tv = (h - (vv^2 - v0^2)/(2.0*aa) - (v1^2 - vv^2)/(2.0*ad)) / vv;
Lv = vv*Tv;
% 计算减速阶段的时间和位移
Td = (v1 - vv) / ad;
Ld = vv*Td + (1.0/2.0)*ad*Td^2;
k = 1;
ts = 0.001;
% 计算轨迹的离散点
for t = t0: ts: (t0+Ta+Tv+Td)
    time(k) = t;
    t = t - t0;
    if (t >= 0 && t < Ta)
        p(k) = p0 + v0*t + (1.0/2.0)*aa*t^2;
        pd(k) = v0 + aa*t;
        pdd(k) = aa;
    elseif (t >= Ta && t < Ta+Tv)
        p(k) = p0 + La + vv*(t - Ta);
        pd(k) = vv;
        pdd(k) = 0;
    elseif (t >= Ta+Tv && t <= Ta+Tv+Td)
        p(k) = p0 + La + Lv + vv*(t - Ta - Tv) + (1.0/2.0)*ad*power(t - Ta - Tv, 2);
        pd(k) = vv + ad*(t - Ta - Tv);
        pdd(k) = ad;
    end
    k = k + 1;
end
figure(1)
subplot(3, 1, 1)
plot(time, p, 'r', 'LineWidth', 1.5)
ylabel('position')
grid on
subplot(3, 1, 2)
plot(time, pd, 'b', 'LineWidth', 1.5)
ylabel('velocity')
grid on
subplot(3, 1, 3)
plot(time, pdd, 'g', 'LineWidth', 1.5)
ylabel('acceleration')
grid on

 


 由上图可以看出，梯形曲线在t0、Ta、Ta+Tv以及Ta+Tv+Td时刻时加速度不连续，速度过渡不平滑，存在冲击，这是梯形曲线的缺点，因此该加减速曲线常用于低速、低成本的运动控制过程。下面的S型曲线能解决这个问题。
 
S型曲线
 

 定义：
 
加速段 0 <= t < Ta： 
  
以恒定的jerk J（加速度的导数）使加速度从0增加到预先设定的a；
以恒定的加速度加速；
已恒定的负的jerk J（加速度的导数）使加速度从预先设定的a减到0；
 
最大速度段 Ta <= t < Ta + Tv： 
  
以恒定的速度v匀速运动；
 
减速段 Ta + Tv <= t <= Ta + Tv + Td： 
  
已恒定的负的jerk J（加速度的导数）使加速度从0减到预先设定的-a；
以恒定的加速度-a减速；
以恒定的jerk J（加速度的导数）使加速度从预先设定的-a增加到0；
 
 
总的运行段数为七段，因此S型曲线也成为七段式曲线。
 
下面是S曲线的算法步骤：
 
第一步：由于初始时刻和终止时刻的位置存在谁大谁小的问题，即q0<q1或者q0>q1，这会对最后的轨迹曲线造成影响。需经如下处理：
 
 然后进行第二步；
 第二步：判断是否能够达到最大速度，步骤如下：
 
 
 此时得到匀速时间Tv，进行第三步；
 
第三步：如果Tv > 0，则跳转到第四步；反之，则跳转到第五步；
 
第四步：此时满足条件Tv > 0，说明系统能够达到的最大速度与设定的最大速度相等，即vlim = vmax，存在匀速时间，整理计算得到Ta、Tv、Td、Tj1、Tj2直接跳转到第十一步计算轨迹；
 
第五步：此时满足Tv <= 0，说明系统不能达到指定的最大速度vlim < vmax，系统不存在匀速段。此时Tv = 0。此时先假设系统能够达到最大加速度和最小加速度，进行如下计算：
 
 得到Ta、Td，对两者进行讨论，进行第六步；
 
第六步：判断Ta <0 or Td < 0?，如果Ta < 0 || Td < 0，则跳转第七步；反之，则跳转至第八步；
 
第七步：满足Ta < 0 || Td < 0的条件。如果此时Ta < 0，令Ta = 0，说明系统没有加速度段，只有减速段，减速段参数利用如下公式计算：
 
 此时得到Ta、Tj，跳转至第十一步直接计算轨迹；
 如果Td < 0，令Td = 0，说明系统没有减速段，只有加速段，加速段参数利用如下公式计算：
 
 此时得到Td、Tj，跳转至第十一步直接计算轨迹；
 
第八步：判断Ta >= 2Tj && Td >= 2Tj是否成立，如果成立则跳转至第九步；反之，则跳转至第十步；
 
第九步：此时Ta >= 2Tj && Td >= 2Tj，说明系统的加速段和减速段均能达到最大加速度，整理（3.26a）、（3.26b）、（3.26c）得到的Ta、Td和Tj，直接跳转至第十一步计算轨迹；
 
第十步：系统加速和减速阶段至少有一段不能达到最大加速度，这种情况很少见，处理方法为逐渐减小amax，令amax = lambda*amax(0<lambda<1)（这种方法不是最优，但是从计算的角度可以接受），然后跳转至第第五步进行循环，循环结束条件为Ta >= 2Tj && Td >= 2Tj成立。这一步也体现了本文S曲线规划的缺点，一旦出现第十步的情况，则无法获得最优规划曲线，也就是在指定的参数范围内，无法获得时间最短的轨迹。
 
第十一步：计算位置、速度、加速度以及加加速度轨迹曲线，公式如下，此时假设t0 = 0：
 
 
 
 
 
 
 
S曲线流程图
 
配合流程图，更容易理解上述计算步骤：
 
 
Matlab实现程序
 
% 单关节空间规划
%% S曲线参数计算（S型速度规划，又称七段式轨迹）
function para = STrajectoryPara(q0, q1, v0, v1, vmax, amax, jmax)
% 得到规划参数Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin
% 用户给定参数
% 边界条件q0 = 10, q1 = 0, v0 = -7, v1 = 0
% 约束条件vmax = 10, amax = 10, jamx = 30
% q0 = 0; q1 = 10; 
% v0 = 7.5; v1 = 0;
% vmax = 10; amax = 10; jmax = 30;
vmin = -vmax; amin = -amax; jmin = -jmax;

%% 利用公式（3.31）（3.32）转化得到实际的q_0、q_1、v_max、a_max
 sigma = sign(q1 - q0);
 q_0 = sigmaq0;
 q_1 = sigmaq1;
 v_0 = sigmav0;
 v_1 = sigmav1;
 v_max = ((sigma+1)/2)*vmax + ((sigma-1)/2)*vmin;
 v_min = ((sigma+1)/2)*vmin + ((sigma-1)/2)*vmax;
 a_max = ((sigma+1)/2)*amax + ((sigma-1)/2)*amin;
 a_min = ((sigma+1)/2)*amin + ((sigma-1)/2)*amax;
 j_max = ((sigma+1)/2)*jmax + ((sigma-1)/2)*jmin;
 j_min = ((sigma+1)/2)*jmin + ((sigma-1)/2)*jmax;
 
%% 判断是否达到最大速度
 if ((v_max - v_0)j_max < a_max^2)
 Tj1 = sqrt((v_max - v_0) / j_max); % 达不到a_max
 Ta = 2Tj1;
 a_lima = j_max * Tj1;
 else
 Tj1 = a_max / j_max; % 能够达到a_max
 Ta = Tj1 + (v_max - v_0) / a_max;
 a_lima = a_max;
 end
 if ((v_max - v_1)j_max < a_max^2)
 Tj2 = sqrt((v_max - v_1) / j_max); % 达不到a_min
 Td = 2Tj2;
 a_limd = -j_max * Tj2;
 else
 Tj2 = a_max / j_max; % 能够达到a_min
 Td = Tj2 + (v_max - v_1) / a_max;
 a_limd = -a_max;
 end
 % 根据（3.25）计算匀速段时间
 Tv = (q_1 - q_0)/v_max - (Ta/2)(1 + v_0/v_max) - (Td/2)(1 + v_1/v_max);
 
%% 对Tv进行讨论
 if (Tv > 0)
 % 达到最大速度v_max，即存在匀速阶段
 vlim = v_max;
 T = Ta + Tv + Td;
 para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
 return;
 else
 % 达不到最大速度，即匀速阶段Tv=0
 % 假设最大加速度和最小加速度均能达到
 Tv = 0;
 Tj = a_max / j_max;
 Tj1 = Tj;
 Tj2 = Tj;
 delta = (a_max4/j_max2) + 2*(v_0^2 + v_1^2) + a_max*(4*(q_1 - q_0) - 2*(a_max/j_max)(v_0 + v_1));
 Ta = ((power(a_max, 2)/j_max) - 2.0v_0 + sqrt(delta)) / (2.0a_max);
 Td = ((power(a_max, 2)/j_max) - 2.0v_1 + sqrt(delta)) / (2.0a_max);
 % 对Ta和Td进行讨论
 if (Ta < 0 || Td < 0)
 if (Ta < 0)
 % 没有加速段，只有减速段
 Ta = 0; Tj1 = 0;
 Td = 2(q_1 - q_0) / (v_0 + v_1);
 Tj2 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_maxpower(q_1 - q_0, 2) + power(v_1 + v_0, 2)(v_1 - v_0)))) / (j_max*(v_1 + v_0));
 a_lima = 0;
 a_limd = -j_maxTj2;
 vlim = v0;
 para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
 return;
 elseif (Td < 0)
 % 没有减速段，只有加速段
 Td = 0; Tj2 = 0;
 Ta = 2(q_1 - q_0) / (v_0 + v_1);
 Tj1 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_maxpower(q_1 - q_0, 2)) - power(v_1 + v_0, 2)(v_1 - v_0))) / (j_max*(v_1 + v_0));
 a_lima = j_maxTj1;
 a_limd = 0;
 vlim = v_0 + a_lima(Ta - Tj1);
 para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
 return;
 end
 elseif (Ta >= 2Tj && Td >= 2Tj)
 % 加速段和减速段都能达到最大加速度
 a_lima = a_max;
 a_limd = -a_max;
 vlim = v0 + a_lima*(Ta - Tj);
 para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
 return;
 else
 % 加速和减速阶段至少有一段不能达到最大加速度
 lambda = 0.99; % 系统取0<lambda<1
 while (Ta < 2Tj || Td < 2Tj)
 % 循环
 a_max = lambdaa_max;
 Tv = 0;
 Tj = a_max / j_max;
 Tj1 = Tj;
 Tj2 = Tj;
 delta = (a_max4/j_max2) + 2(v_0^2 + v_1^2) + a_max*(4*(q_1 - q_0) - 2*(a_max/j_max)(v_0 + v_1));
 Ta = ((power(a_max, 2)/j_max) - 2.0v_0 + sqrt(delta)) / (2.0a_max);
 Td = ((power(a_max, 2)/j_max) - 2.0v_1 + sqrt(delta)) / (2.0a_max);
 if (Ta < 0 || Td < 0)
 if (Ta < 0)
 % 没有加速段，只有减速段
 Ta = 0; Tj1 = 0;
 Td = 2(q_1 - q_0) / (v_0 + v_1);
 Tj2 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_maxpower(q_1 - q_0, 2) + power(v_1 + v_0, 2)(v_1 - v_0)))) / (j_max*(v_1 + v_0));
 a_lima = 0;
 a_limd = -j_maxTj2;
 vlim = v0;
 para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
 return;
 elseif (Td < 0)
 % 没有减速段，只有加速段
 Td = 0; Tj2 = 0;
 Ta = 2(q_1 - q_0) / (v_0 + v_1);
 Tj1 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_maxpower(q_1 - q_0, 2)) - power(v_1 + v_0, 2)(v_1 - v_0))) / (j_max*(v_1 + v_0));
 a_lima = j_maxTj1;
 a_limd = 0;
 vlim = v_0 + a_lima(Ta - Tj1);
 para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
 return;
 end
 elseif (Ta >= 2Tj && Td >= 2Tj)
 % 加速段和减速段都能达到最大加速度
 a_lima = a_max;
 a_limd = -a_max;
 vlim = v0 + a_lima*(Ta - Tj);
 para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
 return;
 end
 end
 end
 end
 end
 
%% 计算位移
 function q = S_position(t, Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin)
 T = Ta + Tv + Td;
 % 加速段
 if (t >= 0 && t < Tj1)
 q = q0 + v0t + jmaxt^3/6;
 elseif (t >= Tj1 && t < Ta - Tj1)
 q = q0 + v0t +(alima/6)(3t^2 - 3Tj1t + Tj1^2);
 elseif (t >= Ta - Tj1 && t < Ta)
 q = q0 + (vlim + v0)(Ta/2) - vlim*(Ta - t) - jmin*((Ta - t)^3/6);
 % 匀速段
 elseif (t >= Ta && t < Ta + Tv)
 q = q0 + (vlim + v0)(Ta/2) + vlim(t - Ta);
 % 减速段
 elseif (t >= Ta + Tv && t < T - Td + Tj2)
 q = q1 - (vlim + v1)(Td/2) + vlim(t - T + Td) - jmax*(power(t - T + Td, 3)/6);
 elseif (t >= T - Td + Tj2 && t < T - Tj2)
 q = q1 - (vlim + v1)(Td/2) + vlim(t - T + Td) + (alimd/6)(3power(t - T + Td, 2) - 3Tj2(t - T + Td) + Tj2^2);
 elseif (t >= T - Tj2 && t <= T)
 q = q1 - v1*(T - t) - jmax*(power(T - t, 3)/6);
 end
 end
 
%% 计算速度
 function qd = S_velocity(t, Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin)
 T = Ta + Tv + Td;
 if (t >= 0 && t < Tj1)
 qd = v0 + jmax*(t^2/2);
 elseif (t >= Tj1 && t < Ta - Tj1)
 qd = v0 + alima*(t - Tj1/2);
 elseif (t >= Ta - Tj1 && t < Ta)
 qd = vlim + jmin*(power(Ta - t, 2)/2);
 % 匀速段
 elseif (t >= Ta && t < Ta + Tv)
 qd = vlim;
 % 减速段
 elseif (t >= Ta + Tv && t < T - Td + Tj2)
 qd = vlim - jmax*(power(t - T + Td, 2)/2);
 elseif (t >= T - Td + Tj2 && t < T - Tj2)
 qd = vlim + alimd*(t - T + Td - Tj2/2);
 elseif (t >= T - Tj2 && t <= T)
 qd = v1 + jmax*(power(t - T, 2)/2);
 end
 end
 
%% 计算加速度
 function qdd = S_acceleration(t, Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin)
 T = Ta + Tv + Td;
 if (t >= 0 && t < Tj1)
 qdd = jmaxt;
 elseif (t >= Tj1 && t < Ta - Tj1)
 qdd = alima;
 elseif (t >= Ta - Tj1 && t < Ta)
 qdd = -jmin(Ta - t);
 % 匀速段
 elseif (t >= Ta && t < Ta + Tv)
 qdd = 0;
 % 减速段
 elseif (t >= Ta + Tv && t < T - Td + Tj2)
 qdd = -jmax*(t - T + Td);
 elseif (t >= T - Td + Tj2 && t < T - Tj2)
 qdd = alimd;
 elseif (t >= T - Tj2 && t <= T)
 qdd = -jmax*(T - t);
 end
 end
 
%% 计算加加速度
 function qddd = S_jerk(t, Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin)
 T = Ta + Tv + Td;
 if (t >= 0 && t < Tj1)
 qddd = jmax;
 elseif (t >= Tj1 && t < Ta - Tj1)
 qddd = 0;
 elseif (t >= Ta - Tj1 && t < Ta)
 qddd = jmin;
 % 匀速段
 elseif (t >= Ta && t < Ta + Tv)
 qddd = 0;
 % 减速段
 elseif (t >= Ta + Tv && t < T - Td + Tj2)
 qddd = -jmax;
 elseif (t >= T - Td + Tj2 && t < T - Tj2)
 qddd = 0;
 elseif (t >= T - Tj2 && t <= T)
 qddd = jmax;
 end
 end
 
测试程序
 
%% S曲线规划
% 边界条件
q0 = 0; q1 = 10; 
v0 = 1; v1 = 0;
vmax = 10; amax = 10; jmax = 30;
sigma = sign(q1 - q0);
% 得到规划参数Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin
para = STrajectoryPara(q0, q1, v0, v1, vmax, amax, jmax)
i = 1; 
T = para(1) + para(2) + para(3)
for t = 0: 0.001: T
   time(i) = 0.001*i;
   q(i) = S_position(t, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
   qd(i) = S_velocity(t, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
   qdd(i) = S_acceleration(t, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
   qddd(i) = S_jerk(t, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
   i = i + 1;
end
q = sigma*q;
qd = sigma*qd;
qdd = sigma*qdd;
qddd = sigma*qddd;
figure(1)
subplot(4, 1, 1)
plot(time, q, 'r', 'LineWidth', 1.5)
grid on
subplot(4, 1, 2)
plot(time, qd, 'b', 'LineWidth', 1.5)
grid on
subplot(4, 1, 3)
plot(time, qdd, 'g', 'LineWidth', 1.5)
grid on
subplot(4, 1, 4)
plot(time, qddd, 'LineWidth', 1.5)
grid on