一種三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人控制研究

，

(1.陜西工業(yè)職業(yè)技術(shù)學(xué)院，陜西 咸陽 712000； 2.深圳市元?jiǎng)?chuàng)興科技有限公司，廣東 深圳 518055)

0 引言

并聯(lián)機(jī)器人作為近年來機(jī)器人研究熱點(diǎn)之一，目前其應(yīng)用幾乎涉及現(xiàn)代尖端技術(shù)的各個(gè)領(lǐng)域[1]，如應(yīng)用于航空航天的運(yùn)載工具模擬器、衛(wèi)星跟蹤系統(tǒng)及飛船對(duì)接器等，從機(jī)構(gòu)選型角度看，凡是需要把轉(zhuǎn)動(dòng)、移動(dòng)或其復(fù)合運(yùn)動(dòng)轉(zhuǎn)化成空間復(fù)雜運(yùn)動(dòng)的場(chǎng)合都有應(yīng)用并聯(lián)機(jī)器人機(jī)構(gòu)的潛在可能性。從這一角度講，并聯(lián)機(jī)器人在未來獲得更為廣泛的應(yīng)用是毋庸置疑的。

三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人作為一種創(chuàng)新型并聯(lián)機(jī)器人，是一個(gè)由三個(gè)并行鏈構(gòu)成的閉鏈運(yùn)動(dòng)系統(tǒng)，即末端執(zhí)行器通過三個(gè)獨(dú)立運(yùn)動(dòng)鏈與機(jī)座相連。其結(jié)構(gòu)上具有低慣量、大負(fù)載能力、高速高精度等優(yōu)點(diǎn)，而且在工作空間中沒有奇異位形，運(yùn)動(dòng)學(xué)性能和動(dòng)力學(xué)性能均優(yōu)于非冗余機(jī)構(gòu)，在精密儀器、現(xiàn)代機(jī)床、高速自動(dòng)化生產(chǎn)線等領(lǐng)域有著廣闊的應(yīng)用前景。

1 系統(tǒng)運(yùn)動(dòng)學(xué)分析

機(jī)器人控制就是控制機(jī)器人各連桿、各關(guān)節(jié)等彼此之間的相對(duì)位置和各連桿、各關(guān)節(jié)的運(yùn)動(dòng)速度以及輸出力的大小，因此下面進(jìn)行三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人的運(yùn)動(dòng)學(xué)分析[2-4]。

并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)分析主要包括運(yùn)動(dòng)學(xué)正解分析、運(yùn)動(dòng)學(xué)反解分析及工作空間確定，運(yùn)動(dòng)學(xué)分析是并聯(lián)機(jī)器人軌跡規(guī)劃控制中的研究重點(diǎn)，由于并聯(lián)機(jī)器人比較特殊的機(jī)械結(jié)構(gòu)(相對(duì)于串聯(lián)型機(jī)器人)，使得其每個(gè)運(yùn)動(dòng)軸之間存在相互耦合關(guān)系，這也直接導(dǎo)致了并聯(lián)機(jī)器人在某些工作區(qū)域內(nèi)存在著奇異位形，所以并聯(lián)機(jī)器人相對(duì)于串聯(lián)機(jī)器人的運(yùn)動(dòng)學(xué)分析更為復(fù)雜。

1.1 幾何參數(shù)

如圖1所示，三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人平臺(tái)主要由三對(duì)連桿組成，每一對(duì)連桿(AA’和A’E，BB’和B’E，CC’和C’E)構(gòu)成一只手臂，每一對(duì)連桿(手臂)中的第一根連桿為驅(qū)動(dòng)連桿(AA’，BB’，CC’)，第二根連桿為從動(dòng)連桿(A’E，B’E，C’E)，三套驅(qū)動(dòng)電機(jī)安裝在驅(qū)動(dòng)連桿的起始端(A，B，C)，三套驅(qū)動(dòng)電機(jī)分布在一個(gè)正三角形頂點(diǎn)上(邊長(zhǎng)為500 mm)，三根從動(dòng)連桿末端連接為一個(gè)活動(dòng)關(guān)節(jié)點(diǎn)E(稱為末端E)，末端E上安裝有工具(筆架)，連桿長(zhǎng):L=|AA’|=|A’E|=|BB’|=|B’E|=|CC’|=|C’E|=244 mm，驅(qū)動(dòng)電機(jī)間距離:D=|AB|=|BC|=|CA|=500 mm。

圖1 三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人機(jī)構(gòu)示意圖

1.2 坐標(biāo)系建立

建立如上圖1所示坐標(biāo)系: 取與AB平行且過ABC中心方向?yàn)樽鴺?biāo)系X軸，AB的中垂線為Y軸(C點(diǎn)在Y軸上，Y軸經(jīng)過三角形△ABC的重心)；三根驅(qū)動(dòng)連桿AA’ ，BB’ ，CC’ 與坐標(biāo)系X軸方向夾角(驅(qū)動(dòng)電機(jī)的轉(zhuǎn)角)分別記為q1 ，q2 ，q3；根據(jù)以上坐標(biāo)系，驅(qū)動(dòng)電機(jī)關(guān)節(jié)A，B，C點(diǎn)的直角坐標(biāo)位置固定：

(1)

1.3 運(yùn)動(dòng)學(xué)正解

已知三個(gè)驅(qū)動(dòng)電機(jī)的角度q1，q2，q3，求解工具末端E在直角坐標(biāo)系中的坐標(biāo)(xE,yE)。

1)驅(qū)動(dòng)連桿末端A’，B’，C’在直角坐標(biāo)系中的坐標(biāo)：

(2)

2)根據(jù)兩點(diǎn)間距離公式和|A’E|=|B’E|=|C’E|=D得到聯(lián)立方程(沒有利用連桿長(zhǎng)度信息)：

(3)

3)令A(yù)’，B’，C’點(diǎn)到坐標(biāo)系原點(diǎn)距離的平方記為：

(4)

4)最后求解得到運(yùn)動(dòng)學(xué)正解方程：

(5)

1.4 運(yùn)動(dòng)學(xué)反解

已知工具末端E在直角坐標(biāo)系中的坐標(biāo)，求解三個(gè)驅(qū)動(dòng)電機(jī)的角度q1，q2，q3，根據(jù)三角關(guān)系可以推出(角度轉(zhuǎn)化到[0，360°])：

(6)

其中：

(7)

計(jì)算得到當(dāng)并聯(lián)機(jī)器人工具末端處于直角坐標(biāo)系中(0，0)時(shí)通過運(yùn)動(dòng)學(xué)反解計(jì)算[5]得到此時(shí)三個(gè)驅(qū)動(dòng)關(guān)節(jié)角度：

q1=-23.7331,q2=96.2669,q3=216.2669

可以看出三個(gè)關(guān)節(jié)角度間相差均為120度，此時(shí)并聯(lián)機(jī)器人三對(duì)連桿處于對(duì)稱分布結(jié)構(gòu)，我們一般將此時(shí)末端所處的位置作為機(jī)器人機(jī)械零點(diǎn)標(biāo)定位置，亦是絕對(duì)原點(diǎn)位置。

1.5 工作空間確定

工作空間的確定是并聯(lián)機(jī)器人反向運(yùn)動(dòng)學(xué)中的一個(gè)關(guān)鍵問題，要確保并聯(lián)機(jī)構(gòu)連桿末端規(guī)劃點(diǎn)在有效工作范圍，就必須先確定工作空間(即反解存在的區(qū)域)。

圖2 三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人工作空間圖

以主動(dòng)桿起始端點(diǎn)A(xA,yA)、B(xB,yB)、C(xC,yC)三點(diǎn)為圓心，連桿手臂長(zhǎng)為半徑畫圓，分別得到圓g1、圓g2、圓g3，如圖2所示，圓g1、圓g2、圓g3可以視為三個(gè)軸的獨(dú)立運(yùn)動(dòng)范圍。W1是圓g2和圓g3的一個(gè)交點(diǎn)；W2是圓g1和圓g3的一個(gè)交點(diǎn)；W3是圓g1和圓g2的一個(gè)交點(diǎn)；弧W1W2在圓g3上；弧W2W3在圓g1上；弧W3W1在圓g2上；所以可以確定弧W1W2、弧W2W3、弧W3W1包括的范圍即為三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人有效工作范圍(圖2中的藍(lán)色區(qū)域)。圓g1、圓g2、圓g3表達(dá)式如式(8)所示，其中半徑R長(zhǎng)度等于連桿手臂長(zhǎng)度，R= 244+244=488 mm。

(8)

1.6 Matlab運(yùn)動(dòng)仿真

Matlab是一款可視化的具有極為強(qiáng)大矩陣計(jì)算能力的軟件，它包含了上百個(gè)預(yù)先定義好的命令和函數(shù)，這些函數(shù)能通過用戶自定義函數(shù)進(jìn)一步擴(kuò)展。Matlab除了具有強(qiáng)大的矩陣運(yùn)算能力外，同時(shí)還有著較強(qiáng)的二維、三維繪圖能力，利用Matlab進(jìn)行仿真[6]能夠大大縮短機(jī)器人的開發(fā)周期。

前面我們已經(jīng)對(duì)三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人的運(yùn)動(dòng)學(xué)正反解和工作空間進(jìn)行了分析，接下來將在Matlab環(huán)境下利用M語言建立其仿真模型，并進(jìn)行運(yùn)動(dòng)仿真實(shí)驗(yàn)，最后分析仿真結(jié)果。仿真模型如圖3所示，部分仿真代碼如下：

s = 500; %基座電機(jī)間距

a = 244; %主動(dòng)桿長(zhǎng)度

b = 244; %從動(dòng)桿長(zhǎng)度

x0=0; %原點(diǎn)初始X坐標(biāo)

y0=0; %原點(diǎn)初始Y坐標(biāo)

theta= pi*1/6; %動(dòng)平臺(tái)轉(zhuǎn)角

DXc = [Xc(1) Xc(2) Xc(3) Xc(1)];

DYc = [Yc(1) Yc(2) Yc(3) Yc(1)];

plot(DXc,DYc)

hold on

text(sum(Xc)/3-15,sum(Yc)/3-15,[' (' num2str(theta*180/pi) ')'])

plot(sum(Xc)/3,sum(Yc)/3,'pb')

hold on

f1 = 2*a*(Ya-Yc);

f2 = 2*a*(Xa-Xc);

f3=Xc.*Xc+Yc.*Yc+Xa.*Xa+Ya.*Ya-2*Xc.*Xa-2*Yc.*Ya+a^2-b^2;

ThetaO = 2*atan((-f1 + sqrt(f1.^2+f2.^2-f3.^2))./(f3 -f2));

Thata1 = 2*atan((-f1 - sqrt(f1.^2+f2.^2-f3.^2))./(f3 -f2))。

圖3 三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人Matlab仿真模型

2 系統(tǒng)控制架構(gòu)

三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人控制系統(tǒng)主要由機(jī)器人本體和電氣控制箱兩大部分組成[7]，二者之間通過帶屏蔽功能的通訊線纜連接，機(jī)器人本體包括工作臺(tái)面、連桿手臂、伺服電機(jī)及電磁鐵筆架。

工作臺(tái)面主要用于安裝三個(gè)連桿手臂及其電機(jī)，同時(shí)也是機(jī)器人的工作空間臺(tái)面，機(jī)器人末端繪圖筆將在工作臺(tái)面上繪制出末端運(yùn)動(dòng)軌跡，每個(gè)連桿手臂都由一根主動(dòng)桿和一根從動(dòng)桿組成，工作臺(tái)面的一個(gè)等邊三角形的三個(gè)頂點(diǎn)安裝有三套交流伺服電機(jī)，用于驅(qū)動(dòng)對(duì)應(yīng)連桿手臂運(yùn)動(dòng)。運(yùn)動(dòng)控制器、伺服驅(qū)動(dòng)器、開關(guān)電源等則安裝于電氣控制箱內(nèi)部。系統(tǒng)采用PC機(jī)+運(yùn)動(dòng)控制器的控制方案，三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人控制框圖如圖4所示。

圖4 三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人控制框圖

3 控制軟件設(shè)計(jì)

三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人軟件是機(jī)器人控制系統(tǒng)設(shè)計(jì)的核心，是控制代碼及控制算法等資源的統(tǒng)稱，機(jī)器人軟件架構(gòu)的設(shè)計(jì)好壞直接關(guān)系到機(jī)器人系統(tǒng)的成敗。三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人軟件具體設(shè)計(jì)過程應(yīng)該包括以下三個(gè)方面:

1)軟件系統(tǒng)架構(gòu)設(shè)計(jì)；

2)具體功能模塊的程序?qū)崿F(xiàn)；

3)系統(tǒng)性集成；

三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人的主要功能是繪圖，即并聯(lián)機(jī)器人末端繪圖筆需要在工作臺(tái)面上繪制出用戶編輯的軌跡，為防止軌跡范圍超出并聯(lián)機(jī)器人有效工作區(qū)域，編輯好的軌跡需要先進(jìn)行繪制求解，然后再進(jìn)行運(yùn)動(dòng)仿真，如果不進(jìn)行運(yùn)動(dòng)仿真直接實(shí)際控制則有可能損壞機(jī)器人，運(yùn)動(dòng)仿真通過以后才能開始實(shí)際控制并聯(lián)機(jī)器人繪圖，運(yùn)動(dòng)控制指令及編碼器反饋指令均由運(yùn)動(dòng)控制器完成，在此過程中涉及的模塊包括軌跡編輯、繪制求解、運(yùn)動(dòng)仿真、反饋采集、零點(diǎn)標(biāo)定、運(yùn)動(dòng)控制等，每個(gè)模塊功能如下描述：

1)軌跡編輯：用戶手動(dòng)繪制軌跡

2)繪制求解：軌跡的運(yùn)動(dòng)學(xué)正向求解與反向求解

3)運(yùn)動(dòng)仿真：軌跡的動(dòng)態(tài)模擬仿真

4)反饋采集：相對(duì)式及絕對(duì)式編碼器數(shù)據(jù)采集

5)零點(diǎn)標(biāo)定：機(jī)器人絕對(duì)零點(diǎn)位置設(shè)定

6)運(yùn)動(dòng)控制：電機(jī)控制指令接口

三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人軟件系統(tǒng)架構(gòu)如表1所示。

表1 三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人軟件系統(tǒng)架構(gòu)

三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人控制主流程如圖5所示。

圖5 三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人控制主流程圖

運(yùn)動(dòng)控制器提供了Windows下的動(dòng)態(tài)鏈接庫DLL，只要調(diào)用函數(shù)庫中的指令，就可以實(shí)現(xiàn)運(yùn)動(dòng)控制器的各種功能。在Visual C++中調(diào)用運(yùn)動(dòng)控制器提供的函數(shù)庫中的運(yùn)動(dòng)函數(shù)，開始采用MFC編寫三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人控制軟件。

實(shí)現(xiàn)并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)正解函數(shù)核心代碼如下：

bool ForwardKinematics(double t1,double t2,double t3,double &Cx,double& Cy)

{

//[輸入]:t1,t2,t3 驅(qū)動(dòng)關(guān)節(jié)電機(jī)角度,單位為rad

//[輸出]:Cx,Cy 末端位置,單位為mm

double xa1,ya1,xb1,yb1,xc1,yc1;

double U,V,W;

xa1=XA+ARM_LENGTH*cos(t1);ya1=YA+ARM_LENGTH*sin(t1);

xb1=XB+ARM_LENGTH*cos(t2);yb1=YB+ARM_LENGTH*sin(t2);

xc1=XC+ARM_LENGTH*cos(t3);yc1=YC+ARM_LENGTH*sin(t3);

U=xa1*xa1+ya1*ya1;

V=xb1*xb1+yb1*yb1;

W=xc1*xc1+yc1*yc1;

Cx=0.5*(U*(yb1-yc1)+V*(yc1-ya1)+W*(ya1-yb1))/(xa1*(yb1-yc1)+xb1*(yc1-ya1)+xc1*(ya1-yb1));

Cy=-0.5*(U*(xb1-xc1)+V*(xc1-xa1)+W*(xa1-xb1))/(xa1*(yb1-yc1)+xb1*(yc1-ya1)+xc1*(ya1-yb1));

return true;

}

實(shí)現(xiàn)并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)反解函數(shù)核心代碼如下：

bool InverseKinematics(double Cx,double Cy,double &t1,double &t2,double &t3)

{

//[輸入]:Cx,Cy 末端位置,單位為mm

//[輸出]:t1,t2,t3 驅(qū)動(dòng)關(guān)節(jié)電機(jī)角度,單位為rad

//判斷輸入點(diǎn)是否在工作空間內(nèi)

if(IsPointInWorkspace(Cx,Cy)==false)

return false;

double DAE,DBE,DCE;

DAE=sqrt((Cy-YA)*(Cy-YA)+(Cx-XA)*(Cx-XA)); DBE=sqrt((Cy-YB)*(Cy-YB)+(Cx-XB)*(Cx-XB)); DCE=sqrt((Cy-YC)*(Cy-YC)+(Cx-XC)*(Cx-XC)); t1=atan2((Cy-YA),(Cx-XA))-acos(DAE/(2.*ARM_LENGTH)); t2=atan2((Cy-YB),(Cx-XB))-acos(DBE/(2.*ARM_LENGTH)); t3=2*pi+atan2((Cy-YC),(Cx-XC))-acos(DCE/(2.*ARM_LENGTH));

return true;

}

根據(jù)控制主流程圖編寫完成后的控制軟件運(yùn)行結(jié)果如圖6所示。

圖6 三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人控制軟件界面

4 控制效果及分析

控制軟件設(shè)計(jì)完成以后，就可以在軌跡編輯中開始編輯軌跡。軌跡的鼠標(biāo)輸入方法：移動(dòng)鼠標(biāo)到繪圖區(qū)，光標(biāo)會(huì)變成繪圖筆形狀，此時(shí)可以按下鼠標(biāo)左鍵開始在繪圖區(qū)繪圖，松開鼠標(biāo)左鍵結(jié)束當(dāng)前繪圖筆畫，按下鼠標(biāo)左鍵開始下一個(gè)繪圖筆畫。編輯好的繪圖軌跡可以通過軌跡編輯子界面中的“打開文件”按鈕打開，然后進(jìn)行軌跡求解，若求解出的目標(biāo)點(diǎn)不在并聯(lián)機(jī)器人有效工作區(qū)域，則會(huì)彈出警告提示框，如圖7所示。

圖7 目標(biāo)點(diǎn)不在工作空間內(nèi)

求解完成以后通過“仿真運(yùn)動(dòng)”模擬運(yùn)行繪圖軌跡文件，仿真試驗(yàn)滿意的軌跡可以執(zhí)行實(shí)際控制，選擇速度檔中的“低速”、“中速” 、“高速”，點(diǎn)擊“實(shí)際控制”將在工作臺(tái)面上(鋪上紙張)繪制出并聯(lián)機(jī)器人末端運(yùn)動(dòng)軌跡。

實(shí)際運(yùn)行中的三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人如圖8所示。

圖8 運(yùn)行中的三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人

三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人運(yùn)動(dòng)過程中，由運(yùn)動(dòng)學(xué)函數(shù)理論計(jì)算得到的三個(gè)主動(dòng)關(guān)節(jié)轉(zhuǎn)動(dòng)角度(紅色曲線表示)和編碼器實(shí)際測(cè)量得到的三個(gè)主動(dòng)關(guān)節(jié)轉(zhuǎn)動(dòng)角度(藍(lán)色曲線表示)之間的誤差曲線如圖9所示。

圖9 三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人運(yùn)動(dòng)誤差曲線圖

誤差曲線圖表明三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人在實(shí)際運(yùn)動(dòng)過程中雖然存在一定的誤差，但最大誤差也只有0.001弧度，此誤差很小，系統(tǒng)中基本可以忽略，實(shí)際繪制軌跡基本能和仿真軌跡一致，說明了此控制系統(tǒng)具有較高的控制精度。

5 結(jié)語

本文提出一種對(duì)稱結(jié)構(gòu)的三自由度冗余驅(qū)動(dòng)并聯(lián)機(jī)器人機(jī)構(gòu)，采用冗余驅(qū)動(dòng)和結(jié)構(gòu)對(duì)稱使得該并聯(lián)機(jī)器人具有更為優(yōu)良的機(jī)構(gòu)性能，首先詳細(xì)闡述了該并聯(lián)機(jī)器人的運(yùn)動(dòng)學(xué)基礎(chǔ)知識(shí)，包括幾何參數(shù)、坐標(biāo)系建立、運(yùn)動(dòng)學(xué)正反解等，并利用MATLAB 工具進(jìn)行了機(jī)器人運(yùn)動(dòng)仿真，縮短了開發(fā)周期，并以控制框圖的形式展現(xiàn)了系統(tǒng)的整個(gè)控制架構(gòu)，最后采用MFC編程實(shí)現(xiàn)了機(jī)器人人機(jī)交互，實(shí)現(xiàn)了較好的控制效果。

并聯(lián)機(jī)器人作為一種全新的機(jī)器人，它具有剛度大、承載能力強(qiáng)、誤差小、精度高、動(dòng)力性能好等一系列優(yōu)點(diǎn)，其運(yùn)用涉及到諸多方面，但并聯(lián)機(jī)器人尤其是多自由度并聯(lián)機(jī)器人的理論和其相應(yīng)的控制技術(shù)還不成熟，所以對(duì)此作出研究是必要而且大有意義的。