六轴机器人轨迹规划之五段位置s曲线插补

1.原理
五段s曲线相较于三段s曲线而言加速度也是连续变化的，能适用于平稳性要求更高的场合。分为加加速、加减速、匀速、减加速、减减速这五段。
设除匀速段以为，其余四段的时间相等都为 Ta T a ，总时间为 T T ，匀速段速度为 vs v s ，四个变速段斜率大小都为 A A ，整段轨迹的总位移 L L 、加加速段位移 L1 L 1 、加减速段位移 L2 L 2

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪Ta=vsA−−√L1=16AT3aL2=56AT3aT=4Ta+L−2L1−2L2vs { T a = v s A L 1 = 1 6 A T a 3 L 2 = 5 6 A T a 3 T = 4 T a + L − 2 L 1 − 2 L 2 v s

则加速度分段函数为

a=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪At,(0≤t≤Ta)−A(t−2Ta),(Ta≤t≤2Ta)0,(2Ta≤t≤T−2Ta)−A[t−(T−2Ta)],(T−2Ta≤t≤T−Ta)A(t−T),(T−Ta≤t≤T) a = { A t , ( 0 ≤ t ≤ T a ) − A ( t − 2 T a ) , ( T a ≤ t ≤ 2 T a ) 0 , ( 2 T a ≤ t ≤ T − 2 T a ) − A [ t − ( T − 2 T a ) ] , ( T − 2 T a ≤ t ≤ T − T a ) A ( t − T ) , ( T − T a ≤ t ≤ T )

对加速度积分可得

v=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪12At2,(0≤t≤Ta)−12A(t−2Ta)2+AT2a,(Ta≤t≤2Ta)vs,(2Ta≤t≤T−2Ta)−12A（t−T+2Ta)2+AT2a,(T−2Ta≤t≤T−Ta)12A(t−T)2,(T−Ta≤t≤T) v = { 1 2 A t 2 , ( 0 ≤ t ≤ T a ) − 1 2 A ( t − 2 T a ) 2 + A T a 2 , ( T a ≤ t ≤ 2 T a ) v s , ( 2 T a ≤ t ≤ T − 2 T a ) − 1 2 A （ t − T + 2 T a ) 2 + A T a 2 , ( T − 2 T a ≤ t ≤ T − T a ) 1 2 A ( t − T ) 2 , ( T − T a ≤ t ≤ T )

对速度积分可得到位移s的分段函数

s=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪16At3,(0≤t≤Ta)−16A(t−2Ta)3+AT2at−AT3a,(Ta≤t≤2Ta)AT2at−AT3a,(2Ta≤t≤T−2Ta)−16A(t−T+2Ta)3+AT2at−AT2a,(T−2Ta≤t≤T−Ta)16A(t−T)3−2AT3a+AT2bT,(T−Ta≤t≤T) s = { 1 6 A t 3 , ( 0 ≤ t ≤ T a ) − 1 6 A ( t − 2 T a ) 3 + A T a 2 t − A T a 3 , ( T a ≤ t ≤ 2 T a ) A T a 2 t − A T a 3 , ( 2 T a ≤ t ≤ T − 2 T a ) − 1 6 A ( t − T + 2 T a ) 3 + A T a 2 t − A T a 2 , ( T − 2 T a ≤ t ≤ T − T a ) 1 6 A ( t − T ) 3 − 2 A T a 3 + A T b 2 T , ( T − T a ≤ t ≤ T )

3.matlab代码实现
指定位置、速度、斜率

clc;
clear;
%初始条件
x_arry=[0,10,20,30];
v_arry=[2,2,2];
A_arry=[3,3,3];
weiyi=[x_arry(1)];sudu=[0];shijian=[0];timeall=0;jiasudu=[0]
for i=1:1:length(x_arry)-1;
%清空
    a=[];v=[];s=[];
%计算加减速段的时间和位移
    L=x_arry(i+1)-x_arry(i);
    A=A_arry(i);
    vs=v_arry(i);
    Ta=sqrt(vs/A);
    L1=A*(Ta^3)/6;
    L2=A*(Ta^3)*(5/6); 
%计算整段轨迹的总位移
    T=4*Ta+(L-2*L1-2*L2)/vs;
    for t=0:0.001:T
        if t<=Ta;%加加速度阶段
            ad=A*t;
            vd=0.5*A*t^2;
            sd=(1/6)*A*t^3;
            a=[a,ad];v=[v,vd];s=[s,sd];
        elseif t>Ta && t<=2*Ta;%加减速阶段
            ad=-A*(t-2*Ta);
            vd=-0.5*A*(t-2*Ta)^2+A*Ta^2;
            sd=-(1/6)*A*(t-2*Ta)^3+A*Ta^2*t-A*Ta^3;
            a=[a,ad];v=[v,vd];s=[s,sd];
         elseif t>2*Ta && t<=T-2*Ta;%匀速阶段
            ad=0;
            vd=vs;
            sd=A*Ta^2*t-A*Ta^3;  
            a=[a,ad];v=[v,vd];s=[s,sd];
        elseif t>T-2*Ta && t<=T-Ta;%减加度阶段
            ad=-A*(t-(T-2*Ta));
            vd=-0.5*A*(t-T+2*Ta)^2+A*Ta^2;
            sd=-(1/6)*A*(t-T+2*Ta)^3+A*Ta^2*t-A*Ta^3;
            a=[a,ad];v=[v,vd];s=[s,sd];
         elseif t>T-Ta && t<=T;%减减阶段
            ad=A*(t-T);
            vd=0.5*A*(t-T)^2;
            sd=(1/6)*A*(t-T)^3-2*A*Ta^3+A*Ta^2*T;
            a=[a,ad];v=[v,vd];s=[s,sd];
        end
    end
%时间
    time=[timeall:0.001:timeall+T];
    timeall=timeall+T;
%连接每一段轨迹
    weiyi=[weiyi,s(2:end)+x_arry(i)];
    sudu=[sudu,v(2:end)];
    jiasudu=[jiasudu,a(2:end)];
    shijian=[shijian,time(2:end)];
end
subplot(3,1,1),plot(shijian,weiyi,'r');xlabel('t'),ylabel('position');grid on;
subplot(3,1,2),plot(shijian,sudu,'b');xlabel('t'),ylabel('velocity');grid on;
subplot(3,1,3),plot(shijian,jiasudu,'g');xlabel('t'),ylabel('accelerate');grid on;

结果如下