雙臂機(jī)器人的軌跡規(guī)劃研究與仿真1

劉子貴

（江門職業(yè)技術(shù)學(xué)院 廣東省江門市 529090）

0 引言

工業(yè)機(jī)器人能夠代替人工完成枯燥、重復(fù)性較高的工作，大幅提高生產(chǎn)效率，甚至可以完成一些人工無法完成的危險(xiǎn)性任務(wù)。雖然單臂機(jī)器人已經(jīng)得到了廣泛應(yīng)用，但隨著生產(chǎn)作業(yè)越來越復(fù)雜，單臂機(jī)器人在某些場(chǎng)合下已經(jīng)無法勝任，例如較為復(fù)雜的搬運(yùn)、柔性裝配等應(yīng)用，而雙臂機(jī)器人則能完成這些工作任務(wù)。因此，研究雙臂機(jī)器人的運(yùn)動(dòng)軌跡規(guī)劃，可以提高機(jī)器人的雙臂協(xié)作、協(xié)調(diào)能力，擴(kuò)展其應(yīng)用領(lǐng)域[1]。

1 軌跡規(guī)劃方法

關(guān)節(jié)變量空間軌跡規(guī)劃的常見方法有三次多項(xiàng)式插值和五階多項(xiàng)式插值等方法。在關(guān)節(jié)變量空間中研究雙臂機(jī)器人的軌跡規(guī)劃，需要先根據(jù)機(jī)器人的逆運(yùn)動(dòng)學(xué)方程，算出機(jī)器人末端在運(yùn)動(dòng)軌跡上各個(gè)點(diǎn)的關(guān)節(jié)變量值，再利用這些關(guān)節(jié)變量值擬合出每個(gè)關(guān)節(jié)的運(yùn)動(dòng)軌跡。最后必須要保證運(yùn)動(dòng)軌跡、速度曲線、加速度曲線的連續(xù)性，以達(dá)到機(jī)器人運(yùn)動(dòng)平穩(wěn)的目標(biāo)[2]。

1.1 三次多項(xiàng)式插值法

設(shè)已知機(jī)器人的起點(diǎn)和終點(diǎn)位姿，根據(jù)機(jī)器人逆運(yùn)動(dòng)學(xué)方程，求出每個(gè)關(guān)節(jié)在初始時(shí)刻t0的關(guān)節(jié)值為θ0，終止時(shí)刻tf的關(guān)節(jié)值為θf。為了保證關(guān)節(jié)運(yùn)動(dòng)速度的平穩(wěn)性，設(shè)定起點(diǎn)和終點(diǎn)的關(guān)節(jié)角速度為0，即：。根據(jù)這4個(gè)約束條件，可以確定該關(guān)節(jié)變量的三次多項(xiàng)式表達(dá)式，即：

對(duì)公式1求一階和二階導(dǎo)數(shù)，可以獲取關(guān)節(jié)角速度和加速度：

設(shè)初始時(shí)刻t0=0，并將4個(gè)約束條件代入公式1和公式2可得：

求解以上方程組可得：

將公式4代入公式1得公式5，可求得該關(guān)節(jié)的軌跡函數(shù)。根據(jù)公式5可以獲取從θ0到θf任意時(shí)刻的關(guān)節(jié)位置。求導(dǎo)后獲得該關(guān)節(jié)的角速度和加速度函數(shù)，如公式6所示，速度曲線為拋物線軌跡，加速度曲線為直線軌跡。

1.2 五次多項(xiàng)式插值法

采用三次多項(xiàng)式插值法，機(jī)器人關(guān)節(jié)變量的軌跡曲線和速度曲線能夠平滑過渡，但加速度曲線為直線，運(yùn)動(dòng)并不平穩(wěn)。對(duì)于運(yùn)動(dòng)軌跡的平穩(wěn)性要求較高的應(yīng)用場(chǎng)合，可采用五次多項(xiàng)式來規(guī)劃運(yùn)動(dòng)軌跡[3]，約束條件增至6個(gè)：初始時(shí)刻t0和終止時(shí)刻tf的關(guān)節(jié)角度值、角速度值和角加速度值。即：

根據(jù)以上6個(gè)約束條件可以求得五次多項(xiàng)式表達(dá)式，如公式7所示：

對(duì)公式7求一階二階導(dǎo)數(shù)，可得關(guān)節(jié)角速度和角加速度，如公式8所示：

設(shè)t0=0，并將6個(gè)約束條件代入公式7、公式8可得公式9：

求解以上方程組可得：

將公式11帶入公式7可得：

公式12就是該關(guān)節(jié)的軌跡函數(shù)，它確定了從θ_0到θ_f任意時(shí)刻的關(guān)節(jié)位置。該關(guān)節(jié)的角速度和角加速度如公式13所示，角速度、角加速度曲線均為拋物線軌跡。

2 雙臂機(jī)器人的運(yùn)動(dòng)學(xué)模型

Baxter雙臂機(jī)器人是美國(guó)Rethink公司研發(fā)一款仿人雙臂機(jī)器人，如圖1所示，由頭部、臉部、雙臂、可移動(dòng)底座等部件構(gòu)成。每條機(jī)臂具有7個(gè)自由度，而且每個(gè)關(guān)節(jié)都是串聯(lián)彈性關(guān)節(jié)，其剛度能夠隨著工作任務(wù)的不同而改變，這種彈性關(guān)節(jié)讓機(jī)器人可以更加靈活地完成工作任務(wù)，而且能在一定程度上保證人、機(jī)交互時(shí)更加地安全、可靠。

圖1 Baxter 雙臂機(jī)器人

圖2 Baxter左臂連桿坐標(biāo)系

Baxter機(jī)器人屬于典型的開鏈串聯(lián)關(guān)節(jié)機(jī)械臂，前4個(gè)關(guān)節(jié)可以控制機(jī)械臂末端位置的狀態(tài)，后3個(gè)關(guān)節(jié)能夠控制機(jī)械臂端的姿態(tài)。

如圖2所示，以Baxter機(jī)器人左臂為研究對(duì)象，采用D-H法建立機(jī)器人的連桿坐標(biāo)系，相關(guān)參數(shù)如表1所示。

表1 Baxter左臂D-H參數(shù)表

運(yùn)用Robotics Toolbox 編制如下程序，建立雙臂機(jī)器人運(yùn)動(dòng)學(xué)模型[4]，如圖3所示。

L1=Link('d',270,'a',0,'alpha',0);%定義機(jī)器人左臂連桿1

L2=Link('d',0,'a',69,'alpha',-pi/2);%定義左臂連桿2

L3=Link('d',364,'a',0,'alpha',pi/2);%定義左臂連桿3

L4=Link('d',0,'a',69,'alpha',-pi/2);%定義左臂連桿4

L5=Link('d',375,'a',0,'alpha',pi/2);%定義左臂連桿5

L6=Link('d',0,'a',10,'alpha',-pi/2);%定義左臂連桿6

L7=Link('d',280,'a',0,'alpha',pi/2);%定義左臂連桿7

robotL=SerialLink([L1 L2 L3 L4 L5 L6 L7]);%建立機(jī)器人左臂模型

robotL.display();%顯示機(jī)器人左臂DH參數(shù)

robotL.name='左臂'

thetaL=[0 0 0 0 0 0 0];%設(shè)定機(jī)器人左臂7個(gè)關(guān)節(jié)的初始關(guān)節(jié)角

robotL.plot(thetaL);%顯示機(jī)器人左臂模型

plot(robotL, thetaL);

hold on;

L8=Link('d',270,'a',0,'alpha',0);%定義機(jī)器人右臂連桿1

L9=Link('d',0,'a',69,'alpha',-pi/2);%定義機(jī)器人右臂連桿2

L10=Link('d',-364,'a',0,'alpha',pi/2);%定義機(jī)器人右臂連桿3

L11=Link('d',0,'a',69,'alpha',-pi/2);%定義機(jī)器人右臂連桿4

L12=Link('d',-375,'a',0,'alpha',pi/2);%定義機(jī)器人右臂連桿5

L13=Link('d',0,'a',10,'alpha',-pi/2);%定義機(jī)器人右臂連桿6

L14=Link('d',-280,'a',0,'alpha',pi/2);%定義機(jī)器人右臂連桿7

robotR=SerialLink([L8 L9 L10 L11 L12 L13 L14]);%建立機(jī)器人右臂模型

robotR.display();%顯示機(jī)器人右臂DH參數(shù)

robotR.name='右臂'

thetaR=[0 0 0 0 0 0 0];%設(shè)定機(jī)器人右臂7個(gè)關(guān)節(jié)的初始關(guān)節(jié)角

robotR.plot(thetaR); %顯示機(jī)器人右臂模型

plot(robotR, thetaR);

hold on;

圖3 雙臂機(jī)器人模型

3 雙臂機(jī)器人的運(yùn)動(dòng)軌跡仿真

利用Robotics Toolbox工具箱中的關(guān)節(jié)變量空間軌跡規(guī)劃函數(shù)jtraj ，對(duì)Baxter機(jī)器人的左臂進(jìn)行仿真研究。設(shè)定7個(gè)關(guān)節(jié)角初始值qz=[0 0 0 0 0 0 0]，終點(diǎn)關(guān)節(jié)角值qr=[pi/4 -pi/3 -pi/4 pi/5 pi/3 pi/6 pi/10]，執(zhí)行下列程序代碼，可獲得各個(gè)關(guān)節(jié)的角度、角速度和角加速度曲線[5]，如圖4、圖5和圖6所示。

圖4 Baxter機(jī)器人的左臂關(guān)節(jié)空間軌跡曲線

t=[0:0.1:5];%設(shè)定仿真時(shí)間為5秒

qz=[0 0 0 0 0 0 0];%設(shè)定7個(gè)關(guān)節(jié)角的初始值

qr=[pi/4 -pi/3 -pi/4 pi/5 pi/3 pi/6 pi/10];%設(shè)定7個(gè)關(guān)節(jié)角的終點(diǎn)值

[q,qd,qdd]=jtraj(qz,qr,t);%利用軌跡規(guī)劃函數(shù)求出各個(gè)關(guān)節(jié)角度、速度和加速度

figure(1)

plot(t,q(:,1),'x');%用“十字”符號(hào)繪制關(guān)節(jié)1的角度曲線

hold on;

plot(t,q(:,2),'p');%用“五角星”符號(hào)繪制關(guān)節(jié)2的角度曲線

hold on;

plot(t,q(:,3),'*');%用“星號(hào)”符號(hào)繪制關(guān)節(jié)3的角度曲線

hold on;

plot(t,q(:,4),'-');%用“實(shí)線”繪制關(guān)節(jié)4的角度曲線

hold on;

plot(t,q(:,5),'--');%用“虛線”繪制關(guān)節(jié)5的角度曲線

hold on;

plot(t,q(:,6),'-.');%用“點(diǎn)劃線”繪制關(guān)節(jié)6的角度曲線

hold on;

plot(t,q(:,7),'^');%用“三角形”繪制關(guān)節(jié)7的角度曲線

xlabel('時(shí)間(s)');ylabel('關(guān)節(jié)角度(rad)');

title('關(guān)節(jié)空間軌跡規(guī)劃');

figure(2)

plot(t,qd(:,1),'x');%用“十字”符號(hào)繪制關(guān)節(jié)1的角速度曲線

hold on;

plot(t,qd(:,2),'p');%用“五角星”符號(hào)繪制關(guān)節(jié)2的角速度曲線

hold on;

plot(t,qd(:,3),'*');%用“星號(hào)”符號(hào)繪制關(guān)節(jié)3的角速度曲線

hold on;

圖5 Baxter機(jī)器人的左臂關(guān)節(jié)角速度曲線

圖6 Baxter機(jī)器人的左臂關(guān)節(jié)角加速度曲線

plot(t,qd(:,4),'-');%用“實(shí)線”繪制關(guān)節(jié)4的角速度曲線

hold on;

plot(t,qd(:,5),'--');%用“虛線”繪制關(guān)節(jié)5的角速度曲線

hold on;

plot(t,qd(:,6),'-.');%用“點(diǎn)劃線”繪制關(guān)節(jié)6的角速度曲線

hold on;

plot(t,qd(:,7),'^');%用“三角形”繪制關(guān)節(jié)7的角速度曲線

xlabel('時(shí)間(s)');ylabel('關(guān)節(jié)角速度');

title('關(guān)節(jié)角速度');

figure(3)

plot(t,qdd(:,1),'x');%用“十字”符號(hào)繪制關(guān)節(jié)1的角加速度曲線

hold on;

plot(t,qdd(:,2),'p');%用“五角星”符號(hào)繪制關(guān)節(jié)2的角加速度曲線

hold on;

plot(t,qdd(:,3),'*');%用“星號(hào)”符號(hào)繪制關(guān)節(jié)3的角加速度曲線

hold on;

plot(t,qdd(:,4),'-');%用“實(shí)線”繪制關(guān)節(jié)4的角加速度曲線

hold on;

plot(t,qdd(:,5),'--');%用“虛 線”繪 制 關(guān) 節(jié)5的 角加速度曲線

hold on;

plot(t,qdd(:,6),'-.');%用“點(diǎn) 劃 線”繪 制 關(guān) 節(jié)6的角加速度曲線

hold on;

plot(t,qdd(:,7),'^');%用“三角形”繪制關(guān)節(jié)7的角加速度曲線

xlabel('時(shí)間(s)');ylabel('關(guān)節(jié)角加速度');

title('關(guān)節(jié)角加速度');

4 結(jié)束語(yǔ)

Robotics Toolbox工具箱中的函數(shù)jtraj采用了五次多項(xiàng)式插值方法，利用它仿真雙臂機(jī)器人的角度軌跡曲線、速度曲線和加速度曲線，這些曲線均為拋物線。三次多項(xiàng)式插值方法的約束條件只有4個(gè)：起點(diǎn)和終點(diǎn)的角位移、角速度，而五次多項(xiàng)式插值的約束條件有6個(gè)，包括了起點(diǎn)和終點(diǎn)的角加速度，生成的加速度曲線不存在跳變，雙臂機(jī)器人7個(gè)關(guān)節(jié)角的加速度變化平緩，各關(guān)節(jié)運(yùn)動(dòng)軌跡平穩(wěn)、連續(xù)。

（文責(zé)自負(fù)）