分段式S形速度規劃算法

1 典型七段式S形速度曲線

典型7段式S形曲線位置q、速度v、加速度a、加加速度j的波形如圖所示：

位置q和時間t的關係式：

2 分段式S形曲線速度規劃

已知初速度V0,末速度Ve,最大速度Vmax，路程距離s，最大加速度amax,最大jerk（加加速度）jmax,要規劃位置、速度、加速度、加加速度隨時間變化的曲線。具體步驟如下：

1） 假設能夠達到最大速度Vmax，此時計算加速時間Ta和減速時間Td

此時需要判斷最大加速度amax是否能夠達到

在加速階段，有關係式：

推導出：

由於勻加速和勻減速階段是對稱的,所以在臨界情況下有：

則當 (Vmax-V0)j<amax*amax時，不能達到最大加速度，否則能夠達到最大加速度amax。

當(Vmax-V0)j<amax*amax時,沒有勻加速階段，此時有關係式：

此時最大加速度爲 alim=j*Tj1

否則，存在勻加速階段，勻加速度階段加速度爲amax，此時有關係式：

這裏T2爲勻加速階段的時間。上式可以推導出Ta=Tj1+(Vmax-V0)/amax

同理，可以求出減速階段的時間Tj2和Td。

加速段時間Ta和減速度時間Td都求出以後，可以求勻速段時間Tv。由對稱性，加速段平均速度爲(V0+Vmax)/2，減速度時間平均速度爲(Ve+Vmax)/2，則有關係式：

Tv*Vmax+Ta*(v0+Vmax)/2+Td*(Ve+Vmax)/2=s

求出Tv=s/Vmax-Ta(V0/Vmax+1)/2+Td*(Ve/Vmax+1)/2

若Tv>0，則有勻速階段，勻速度Vmax，此時可以按照第1節的公式求出7段式軌跡各段的位置、速度、加速度和加加速度。

若Tv<=0,則證明不能達到最大速度Vmax。

2） 不能達到最大速度Vmax

此時不存在勻速階段。首先假設此時能夠達到最大加速度Vmax，此時有：

其中 

這裏具體delta的推導見論文《數控系統S型曲線加減速快速規劃研究》。

若計算得到的Ta<2*Tj1或Td<2*Tj1，那麼最大加速度amax沒有達到，此時可以通過減少amax,直到滿足Ta>2*Tj1或Td>2*Tj2爲止。

3 Matlab實現代碼

%通過直接確定軌跡參數的方法，計算並且畫出雙S曲線。
%定義變量 , 現在的初始值都按找書中的例子給定
q0 = 0  
q1 = 10
vmax = 10
max = 10
v0 = 0
v1 = 0
amax = 20
jmax = 30
count=0

%根據軌跡規劃的流程，首先根據初始調節與限制條件判斷是否存在雲加速段
%首先按照能夠達到最大速度vmax，來計算Ta,Tb,Tj1 Tj2, Tv
if (vmax -v0)*jmax < amax^2
    if v0>vmax
        Tj1 = 0; 
        Ta = 0;
        alima = 0;
    else
        Tj1 = ((vmax - v0)/jmax)^0.5;
        Ta = 2*Tj1;
        alima = Tj1 * jmax;
    end
else
    Tj1 = amax/jmax;
    Ta = Tj1 + (vmax-v0)/amax;
    alima = amax;
end

if (vmax -v1)*jmax < amax^2
    Tj2 = ((vmax - v1)/jmax)^0.5;
    Td = 2*Tj1;
    alimd = Tj2* jmax;
else
    Tj2 = amax/jmax;
    Td = Tj2 + (vmax-v1)/amax;
    alimd = amax;
end
    
Tv = (q1 - q0)/vmax - Ta/2*(1+v0/vmax) - Td/2*(1+v1/vmax)
T = Tv + Ta + Td
%Tv>0 說明可以達到最大速度，進入計算軌跡，並畫圖
p  = [];
vc = [];
ac = [];
jc = [];
if Tv > 0
    vlim = vmax;
    T = Tv + Ta + Td
else
    Tv = 0;
    %說明沒有辦法達到最大速度，判斷是否能夠達到最大加速度
    %首先重新計算Ta,Tb
    amax_org = amax;
    delta = (amax^4)/(jmax^2) + 2*(v0^2+v1^2) + amax*(4*(q1 - q0) - 2*amax/jmax*(v0+v1));
    Tj1 = amax/jmax
    Ta = (amax^2/jmax - 2*v0 + delta^0.5)/2/amax
    Tj2 = amax/jmax
    Td = (amax^2/jmax - 2*v1 + delta^0.5)/2/amax
    vlim = v0 + (Ta - Tj1)*alima
    while Ta < 2*Tj1||Td < 2*Tj2
        count = count+1;
        amax = amax - amax_org*0.1;
        alima = amax;
        alimd = amax;
        if amax>0
            delta = (amax^4)/(jmax^2) + 2*(v0^2+v1^2) + amax*(4*(q1 - q0) - 2*amax/jmax*(v0+v1));
        else
            delta = (amax^4)/(jmax^2) + 2*(v0^2+v1^2) - amax*(4*(q1 - q0) - 2*amax/jmax*(v0+v1));
        end
        Tj1 = amax/jmax;
        Ta = (amax^2/jmax - 2*v0 + delta^0.5)/2/amax;
        Tj2 = amax/jmax;
        Td = (amax^2/jmax - 2*v1 + delta^0.5)/2/amax;
        vlim = v0 + (Ta - Tj1)*alima;
        vlima = vlim;
        vlimb = v1 - (Td - Tj2)*alimd;
    end
    disp('這裏打印出計算出來的Ta,Tb,')
    Tj1
    Ta
    Td
    amax
    if Ta <0||Td<0
        if v0>v1
            %這裏只需要一個減速段
            Ta =0;
            Tj1 = 0;
            alima = 0;
            Td = 2*(q1 - q0)/(v1 + v0);
            Tj2 = (jmax*(q1 - q0) - (jmax*(jmax*(q1 - q0)^2 + (v1 + v0)^2*(v1 - v0)))^0.5)/jmax/(v1 + v0);
            alimd = -jmax*Tj2
            vlim = v1 - (Td - Tj2)*alimd
            alimd = -alimd
        else
            Td =0;
            Tj2 = 0;
        
            Ta = 2*(q1 - q0)/(v1 + v0);
            Tj1 = (jmax*(q1 - q0) - (jmax*(jmax*(q1 - q0)^2 - (v1 + v0)^2*(v1 - v0)))^0.5)/jmax/(v1 + v0);
            alima = jmax*Tj1;
            vlim = v0 + (Ta - Tj1)*alima
        end
    end

%     amax = amax_org;
%     while Td < 2*Tj2
%         amax = amax - amax_org*0.1;
%         alimb = amax;
%         Tj2 = amax/jmax;
%         Td = (amax^2/jmax - 2*v1 + delta^0.5)/2/amax;     
%     end
    
%     if Td <0
%         Td =0;
%         Tj2 = 0;
%         
%         Ta = 2*(q1 - q0)/(v1 + v0);
%         Tj1 = (jmax*(q1 - q0) - (jmax*(jmax*(q1 - q0)^2 - (v1 + v0)^2*(v1 - v0)^0.5)))/jmax/(v1 + v0);
%         alima = jmax*Tj1;
%         vlim = v0 + (Ta - Tj1)*alima
%     end
    Tj1
    Tj2
    Ta
    Td
    alima
    alimd
    T = Tv + Ta + Td
%     vlim = v0 + alima * Tj1^2 + (Ta - 2*Tj1)*alima
     
end
for t = 0:0.001:T
%         加速段軌跡
        if t>=0&&t<Tj1
            q = q0 + v0*t + jmax*t^3/6;
            p = [p q];
            v = v0 + jmax*t^2/2;
            vc = [vc v];
            a = jmax*t;
            ac = [ac a];
            jc = [jc jmax];
        elseif t>= Tj1&&t<(Ta - Tj1)
            q = q0 + v0*t + alima/6*(3*t^2 - 3*Tj1*t + Tj1^2);
            p = [p q];
            v = v0 + alima*(t - Tj1/2);
            vc = [vc v];
            a = alima;
            ac = [ac a];
            jc = [jc 0];
        elseif t>=(Ta - Tj1)&&t < Ta
            q = q0 + (vlim + v0)*Ta/2 - vlim*(Ta - t) + jmax*(Ta - t)^3/6;
            p = [p q];
            v = vlim - jmax*(Ta - t)^2/2;
            vc = [vc v];
            a = jmax*(Ta - t);
            ac = [ac a];
            jc = [jc -jmax];
         %勻速段   
        elseif t>=Ta&&t<(Ta + Tv)
            q = q0 + (vlim + v0)*Ta/2 + vlim*(t - Ta);
            p = [p q];
            v = vlim;
            vc = [vc v];
            a = 0;
            ac = [ac 0];
            jc = [jc 0];           
        %減速段    
        elseif t>=(T - Td)&&t <(T - Td + Tj2)
            q = q1 - (vlim + v1)*Td/2 + vlim*(t - T + Td) - jmax*(t - T + Td)^3/6;
            p = [p q];
            v = vlim - jmax*(t - T + Td)^2/2;
            vc = [vc v];
            a = -jmax*(t - T + Td);
            ac = [ac a];
            jc = [jc -jmax];
        elseif t>=(T - Td + Tj2)&&t<(T - Tj2)
            q = q1 - (vlim + v1)*Td/2 + vlim*(t - T + Td) - alimd/6*(3*(t - T + Td)^2 -3*Tj2*(t - T + Td) + Tj2^2);
            p = [p q];
            v = vlim - alimd*(t - T + Td - Tj2/2);
            vc = [vc v];
            a = -alimd;
            ac = [ac a];
            jc = [jc 0];
        elseif t>=(T - Tj2)&& t<T
            q = q1 - v1*(T - t) -jmax*(T - t)^3/6;
            p = [p q];
            v = v1 + jmax*(T - t)^2/2;
            vc = [vc v];
            a = -jmax*(T - t);
            ac = [ac a];
            jc = [jc jmax];           
         end
end
t =  0:0.001:T;
plot(p)
figure
plot(vc)
figure
plot(ac)
figure
plot(jc)

當q0 = 0, q1 = 10, v0 = 1, v1 = 0,vmax = 5, amax = 10, jmax = 30時，爲典型的7段式S曲線，

當q0 = 0, q1 = 10, v0 = 1, v1 = 0,vmax = 10, amax = 10, jmax = 30時，爲5段式S曲線：

當q0 = 0, q1 = 10, v0 = 7, v1 = 0,vmax = 10, amax = 10, jmax = 30時，爲4段式S曲線：

當q0 = 0, q1 = 10, v0 = 7.5, v1 = 0,vmax = 10, amax = 10, jmax = 30時，爲3段式S曲線：