六軸機器人軌跡規劃之五段位置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$$\begin{array}{r}\{\begin{array}{l}{T}_a=\sqrt{\frac{v_s}{A}}\\ L_1=\frac{1}{6}A{T}_a^3\\ L_2=\frac{5}{6}A{T}_a^3\\ T=4T_a+\frac{L-2L_1-2L_2}{v_s}\end{array}\end{array}$$

則加速度分段函數爲

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)$$\begin{array}{r}a=\{\begin{array}{l}At,(0\le t\le T_a)\\ -A(t-2T_a),(T_a\le t\le 2T_a)\\ 0,(2T_a\le t\le T-2T_a)\\ -A[t-(T-2T_a)],(T-2T_a\le t\le T-T_a)\\ A(t-T),(T-T_a\le t\le T)\end{array}\end{array}$$

對加速度積分可得

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)$$\begin{array}{r}v=\{\begin{array}{l}\frac{1}{2}A{t}^2,(0\le t\le T_a)\\ -\frac{1}{2}A(t-2T_a{)}^2+A{T}_a^2,(T_a\le t\le 2T_a)\\ v_s,(2T_a\le t\le T-2T_a)\\ -\frac{1}{2}A（t-T+2T_a{)}^2+A{T}_a^2,(T-2T_a\le t\le T-T_a)\\ \frac{1}{2}A(t-T{)}^2,(T-T_a\le t\le T)\end{array}\end{array}$$

對速度積分可得到位移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)$$\begin{array}{r}s=\{\begin{array}{l}\frac{1}{6}A{t}^3,(0\le t\le T_a)\\ -\frac{1}{6}A(t-2T_a{)}^3+A{T}_a^{2}t-A{T}_a^3,(T_a\le t\le 2T_a)\\ A{T}_a^{2}t-A{T}_a^3,(2T_a\le t\le T-2T_a)\\ -\frac{1}{6}A(t-T+2T_a{)}^3+A{T}_a^{2}t-A{T}_a^2,(T-2T_a\le t\le T-T_a)\\ \frac{1}{6}A(t-T{)}^3-2A{T}_a^3+A{T}_b^{2}T,(T-T_a\le t\le T)\end{array}\end{array}$$

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;

結果如下