六軸機器人軌迹規劃之五段位置s曲線插補

1.原理

五段s曲線相較于三段s曲線而言加速度也是連續變化的，能适用于平穩性要求更高的場合。分為加加速、加減速、勻速、減加速、減減速這五段。

設除勻速段以為，其餘四段的時間相等都為 Ta T a ，總時間為 T T ，勻速段速度為vsvs，四個變速段斜率大小都為 A A ，整段軌迹的總位移LL、加加速段位移 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=[,,,];
v_arry=[,,];
A_arry=[,,];
weiyi=[x_arry()];sudu=[];shijian=[];timeall=;jiasudu=[]
for i=::length(x_arry)-;
%清空
    a=[];v=[];s=[];
%計算加減速段的時間和位移
    L=x_arry(i+)-x_arry(i);
    A=A_arry(i);
    vs=v_arry(i);
    Ta=sqrt(vs/A);
    L1=A*(Ta^)/;
    L2=A*(Ta^)*(/); 
%計算整段軌迹的總位移
    T=*Ta+(L-*L1-*L2)/vs;
    for t=:.:T
        if t<=Ta;%加加速度階段
            ad=A*t;
            vd=.*A*t^;
            sd=(/)*A*t^;
            a=[a,ad];v=[v,vd];s=[s,sd];
        elseif t>Ta && t<=*Ta;%加減速階段
            ad=-A*(t-*Ta);
            vd=-.*A*(t-*Ta)^+A*Ta^;
            sd=-(/)*A*(t-*Ta)^+A*Ta^*t-A*Ta^;
            a=[a,ad];v=[v,vd];s=[s,sd];
         elseif t>*Ta && t<=T-*Ta;%勻速階段
            ad=;
            vd=vs;
            sd=A*Ta^*t-A*Ta^;  
            a=[a,ad];v=[v,vd];s=[s,sd];
        elseif t>T-*Ta && t<=T-Ta;%減加度階段
            ad=-A*(t-(T-*Ta));
            vd=-.*A*(t-T+*Ta)^+A*Ta^;
            sd=-(/)*A*(t-T+*Ta)^+A*Ta^*t-A*Ta^;
            a=[a,ad];v=[v,vd];s=[s,sd];
         elseif t>T-Ta && t<=T;%減減階段
            ad=A*(t-T);
            vd=.*A*(t-T)^;
            sd=(/)*A*(t-T)^-*A*Ta^+A*Ta^*T;
            a=[a,ad];v=[v,vd];s=[s,sd];
        end
    end
%時間
    time=[timeall:.:timeall+T];
    timeall=timeall+T;
%連接配接每一段軌迹
    weiyi=[weiyi,s(:end)+x_arry(i)];
    sudu=[sudu,v(:end)];
    jiasudu=[jiasudu,a(:end)];
    shijian=[shijian,time(:end)];
end
subplot(,,),plot(shijian,weiyi,'r');xlabel('t'),ylabel('position');grid on;
subplot(,,),plot(shijian,sudu,'b');xlabel('t'),ylabel('velocity');grid on;
subplot(,,),plot(shijian,jiasudu,'g');xlabel('t'),ylabel('accelerate');grid on;
           

結果如下