機器人運動中加減速效果實現

有的時候，我們在規劃機器人的參考軌跡過程中，沒有考慮機器人的速度，而機器人實際運動過程中一般啓動和停止過程中有一個加減速過程，爲了實現這一效果，我們可以採用緩動函數來對曲線進行離散化。

(1)緩動函數1

其對應的位置曲線和速度曲線如下

Matlab:

last_t=0;
for j=0:0.01:1
    t=0.5*(sin(pi*j-pi/2)+1);
    v=(t-last_t)/0.01;
    last_t=t;
    subplot(2,1,1)
    plot(j,t,'k.');
    title('位置曲線')
    hold on
    subplot(2,1,2)
    plot(j,v,'k.');
    title('速度曲線')
    hold on
end

(2)緩動函數2

其對應的位置曲線和速度曲線如下

Matlab:

last_t=0;
for j=0:0.01:1
    if j<0.5
        t=(1-sin((1-pow(2*j,2))*pi/2))/2;
    else
        t=0.5+0.5*sin((1-pow(1-2*(j-0.5),2))*pi/2);
    end
    
    v=(t-last_t)/0.01;
    last_t=t;
    subplot(2,1,1)
    plot(j,t,'k.');
    title('位置曲線')
    hold on
    subplot(2,1,2)
    plot(j,v,'k.');
    title('速度曲線')
    hold on
end

具體應用

有了上述緩動函數的公式後，我們可以利用參數t來對規劃的軌跡曲線進行離散化，這裏以貝塞爾曲線的離散進行舉例。我們取上述表達式中的參數j爲勻速遞增(取值在[0,1])，通過將j代入上述公式可求得相應的t。這裏通過改造t來實現最終的加減速效果。下圖爲緩動函數2的具體效果

Matlab:


% 貝塞爾曲線(參考曲線)
start_X=0;
start_Y=0;
end_X=1.5;
end_Y=2.25;
Cx=[start_X,start_X+(end_X-start_X)*0.1,start_X+(end_X-start_X)*0.6,end_X];
Cy=[start_Y,start_Y+(end_Y-start_Y)*0.5,start_Y+(end_Y-start_Y)*0.9,end_Y];

last_Px1=Cx(1);
last_Py1=Cy(1);
last_Px2=Cx(1);
last_Py2=Cy(1);

for j=0:0.01:1

    
    
   
    % 不帶緩動函數
    t=j;
    Pt1=pow(1-t,3);
    Pt2=3*t*pow(1-t,2);
    Pt3=3*pow(t,2)*(1-t);
    Pt4=pow(t,3);
    
    Px1=Pt1*Cx(1)+Pt2*Cx(2)+Pt3*Cx(3)+Pt4*Cx(4);
    Py1=Pt1*Cy(1)+Pt2*Cy(2)+Pt3*Cy(3)+Pt4*Cy(4);
    v1=sqrt(pow(Px1-last_Px1,2)+pow(Py1-last_Py1,2))/0.01;
    last_Px1=Px1;
    last_Py1=Py1;
    
    
    % 帶緩動函數
    %     t=0.5*(sin(pi*j-pi/2)+1);
    
    if j<0.5
        t=(1-sin((1-pow(2*j,2))*pi/2))/2;
    else
        t=0.5+0.5*sin((1-pow(1-2*(j-0.5),2))*pi/2);
    end
    
    Pt1=pow(1-t,3);
    Pt2=3*t*pow(1-t,2);
    Pt3=3*pow(t,2)*(1-t);
    Pt4=pow(t,3);
    
    Px2=Pt1*Cx(1)+Pt2*Cx(2)+Pt3*Cx(3)+Pt4*Cx(4);
    Py2=Pt1*Cy(1)+Pt2*Cy(2)+Pt3*Cy(3)+Pt4*Cy(4);
    
    v2=sqrt(pow(Px2-last_Px2,2)+pow(Py2-last_Py2,2))/0.01;
    last_Px2=Px2;
    last_Py2=Py2;

    subplot(2,1,1)
    plot(Px2,Py2,'k.',Px1,Py1,'r.');
    title('位置曲線')
    legend('加緩動函數','未加緩動函數');
    hold on
    subplot(2,1,2)
    plot(t,v2,'k.',j,v1,'r.');
    title('速度曲線')
    legend('加緩動函數','未加緩動函數');
    hold on
end