武永強(qiáng)，于 濤，紀(jì) 俐

(1.遼寧工業(yè)大學(xué)機(jī)械工程與自動(dòng)化學(xué)院，錦州 121001；2.沈陽航空航天大學(xué)機(jī)電工程學(xué)院，沈陽 110000)

0 引言

目前，雙機(jī)器人協(xié)調(diào)作業(yè)被越來越多的企業(yè)應(yīng)用。雙機(jī)器人協(xié)調(diào)作業(yè)的研究和開發(fā)已成為機(jī)器人研究領(lǐng)域中重要且具有活力的一個(gè)分支[1]。

單個(gè)機(jī)器人運(yùn)動(dòng)學(xué)建模已經(jīng)十分成熟，運(yùn)動(dòng)學(xué)模型均根據(jù)D-H參數(shù)法來建立。曹啟賀等[2]用齊次坐標(biāo)變換推出理論上的坐標(biāo)變換方程，結(jié)合D-H參數(shù)法，創(chuàng)建KUKA-KR16機(jī)器人的結(jié)構(gòu)模型；趙文強(qiáng)、郝建豹等[3-4]用D-H參數(shù)法建立機(jī)械臂運(yùn)動(dòng)學(xué)模型，分析其正、逆運(yùn)動(dòng)學(xué)問題；上述均用MATLAB驗(yàn)證建立的模型是否正確，建立的模型沒有與真實(shí)的機(jī)器人進(jìn)行比對(duì)。本文將用改進(jìn)D-H參數(shù)法建立ABB-IRB120型機(jī)器人的運(yùn)動(dòng)學(xué)模型并且與實(shí)際機(jī)器人進(jìn)行比對(duì)，驗(yàn)證運(yùn)動(dòng)學(xué)建模的正確性。

雙機(jī)器人系統(tǒng)也不乏有學(xué)者研究，于廣東等[5]將雙機(jī)器人協(xié)調(diào)分為兩類，分析了其運(yùn)動(dòng)學(xué)關(guān)系；史帥剛、楊國(guó)等[6-7]從主從機(jī)器人不同的末端位姿約束關(guān)系出發(fā)，分析了主從機(jī)器人運(yùn)動(dòng)學(xué)關(guān)系。本文將在單個(gè)機(jī)器人運(yùn)動(dòng)學(xué)建模的基礎(chǔ)上，提出基于標(biāo)定模型的基坐標(biāo)系標(biāo)定方法進(jìn)行雙機(jī)器人基坐標(biāo)系標(biāo)定，進(jìn)而推出雙機(jī)器人運(yùn)動(dòng)學(xué)模型。

1 單機(jī)器人建模與驗(yàn)證

1.1 機(jī)器人坐標(biāo)系

根據(jù)文獻(xiàn)[8-9]，建立ABB-IRB120型機(jī)器人D-H坐標(biāo)系如圖1所示。進(jìn)一步得到ABB-IRB120型機(jī)器人的改進(jìn)D-H參數(shù)[10]如表1所示。

圖1 ABB-IRB120型機(jī)器人D-H坐標(biāo)系

表1 ABB-IRB120型機(jī)器人的改進(jìn)D-H參數(shù)

表1中，θi表示第i關(guān)節(jié)的關(guān)節(jié)轉(zhuǎn)角；di表示第i連桿的關(guān)節(jié)距離；ai-1表示第i-1連桿的連桿長(zhǎng)度；αi-1表示第i-1連桿的連桿扭角；βi表示第i關(guān)節(jié)的關(guān)節(jié)角偏移量；α1=90；α3=-90；α4=90；α5=90；a2=270；a3=70；d4=302；d6=72；β2=90；β5=-180；θ1=(-165°～165°)；θ2=(-110°～110°)；θ3=(-90°～70°)；θ4=(-160°～160°)；θ5=(-120°～120°)；θ6=(-400°～400°)。

由于機(jī)器人的0關(guān)節(jié)角位姿狀態(tài)往往是不規(guī)則的位形，為了使其變?yōu)橐恍└鼮槊黠@的位形，表1中引入關(guān)節(jié)角偏移量β。

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

使用改進(jìn)的D-H算法[11]建立機(jī)器人的運(yùn)動(dòng)學(xué)模型，坐標(biāo)系{Oi-1}到坐標(biāo)系{Oi}之間的坐標(biāo)變換，可由式(1)得到：

(1)

將表1中的參數(shù)代入式(1)，可得各連桿的變換矩陣如下：

式中，Ti表示連桿坐標(biāo)系i相對(duì)于連桿坐標(biāo)系i-1的變換矩陣；i=1,2,3,4,5,6。

上述變換認(rèn)為機(jī)器人的基坐標(biāo)原點(diǎn)位于機(jī)器人內(nèi)部結(jié)構(gòu)的腰關(guān)節(jié)和肩關(guān)節(jié)軸線的交點(diǎn)上。ABB-IRB120型機(jī)器人有一個(gè)290 mm的底座結(jié)構(gòu)，需要通過坐標(biāo)變換將機(jī)器人基坐標(biāo)系的原點(diǎn)轉(zhuǎn)移到機(jī)器人底座的下端面的中心[15]。該變換矩陣為：

將基坐標(biāo)系的變換矩陣和各連桿的變換矩陣依次相乘，得機(jī)器人正運(yùn)動(dòng)學(xué)方程[12-14]：

(2)

nx=c6(c5(s1s4-c4c1c23))+s5c1s23-s6(c4s1+s4c1c23)

ny=s6(c1c4-s4s1c23)-c6(c5(c1s4+c4s1c23)-s5s1s23)

nz=s6(c1c4-s4s1c23)-c6(c5(c1s4+c4s1c23)-s5s1s23)

ox=-c6(c4s1+s4c1c23)-s6(c5(s1s4-c4c1c23)+s5c1s23)

oz=-s5(c1s4+c4s1c23)-c5s1s23

ax=s5(s1s4-c4c1c23)-c5c1s23

ay=-s5(c1s4+c4s1c23)-c5s1s23

az=c4s5s23-c5c23

px=d6(s5(s1s4-c4c1c23)-c5c1s23)-a3c1c23+d4c1s23-a2c1c2

py=-a3s1c23+d4s1s23-d6(s5(c1s4+c4s1c23)+

c5s1s23)-a2c2s1

pz=a3s23+d4c23-d6(c5c23-c4s5s23)+a2s2+290

式中，s1=sinθ1；c1=cosθ1；s23=sin(θ2+θ3)；c23=cos(θ2+θ3)，以此類推，后文相同。

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

(1)求解θ1、θ2、θ3。根據(jù)位姿分離的方法，連桿坐標(biāo)系的{4}、{5}、{6}坐標(biāo)系的坐標(biāo)原點(diǎn)相對(duì)于基坐標(biāo)系的位置是相同的，所以利用連桿{4}坐標(biāo)系的坐標(biāo)原點(diǎn)即機(jī)器人末端的坐標(biāo)原點(diǎn)[16]。

由變換矩陣T4易得坐標(biāo)系{4}的坐標(biāo)原點(diǎn)在坐標(biāo)系{3}中的表述為：

3p4=[a3-d4sα3d4cα31]T

(3)

結(jié)合式(2)，進(jìn)而可得機(jī)器人末端在基坐標(biāo)系下的表述為：

(4)

(5)

將T0、T1、T2、T3和式(3)代入式(5)并展開，令等式左右兩邊矩陣(2,1)元素相等，可得：

pxs1-pyc1=0

(6)

根據(jù)式(6)，進(jìn)一步可得：

θ1=arctan2(py,px)

或者

θ1=arctan2(-py,-px)

(7)

將T0、T1、T2和T3以及式(3)代入式(7)并展開，令等式左右兩邊的矩陣中的元素(1,1)和元素(2,1)分別對(duì)應(yīng)相等，可得：

As2-Bc2=C

(8)

Ac2+Bs2=D

(9)

式中，A、B、C、D分別為：

A=pz-290

B=pxc1+pys1

C=a2+a3c3-d4s3

(10)

D=d4c3+a3s3

(11)

A2+B2=C2+D2

(12)

將式(10)、式(11)帶入式(12)得：

a3c3-d4s3=k

(13)

利用三角代換可以解得：

解得以后，代入式(10)和式(11)，得到C和D的值。最后，將式(9)利用三角代換，得：

至此，θ1、θ2、θ3求解完畢。

(2)求解θ4、θ5、θ6。求解θ4、θ5、θ6必須依據(jù)姿態(tài)矩陣來建立有效的方程求解，上述已經(jīng)求得θ1、θ2、θ3，后面將T1、T2、T3視為已知量。根據(jù)式(2)可以得到：

(14)

基于遙感的岸線變遷及其在電力工程的應(yīng)用……………………………………胡進(jìn)寶，劉海成，王曉霞，等（3.89）

(15)

式(15)中等式兩邊的矩陣元素(2,3)相等，得：

c5= -azc23-s23(axc1+ays1)

(16)

式(16)進(jìn)一步變形可得：

式(15)中，根據(jù)等式左右兩邊的矩陣中的元素(1,3)、(3,3)、(2,1)、(2,2)分別對(duì)應(yīng)相等，可得以下兩組方程：

(17)

(18)

根據(jù)式(17)、式(18)可得：

當(dāng)s5>0時(shí)，

θ4=arctan2(axs1-ayc1,azs23-(axc1+ays1)c23)

θ6=arctan2(-ozc23-(oxc1+oys1)s23,nzc23+(nxc1+nys1)s23)

當(dāng)s5<0時(shí)，

θ4=arctan2(ayc1-axs1,(axc1+ays1)c23-azs23)

θ6=arctan2(ozc23+(oxc1+oys1)s23,-nzc23-(nxc1+nys1)s23)

至此，θ4、θ5、θ6求解完畢。

1.4 機(jī)器人模型的驗(yàn)證

(1)模型建立。根據(jù)表1所示的ABB-IRB120型機(jī)器人的D-H參數(shù)，使用MATLAB中的Robotics Toolbox建立機(jī)器人運(yùn)動(dòng)學(xué)模型[17]，得到IRB120機(jī)器人模型如圖2所示。

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

使用命令：IRB120_R1.fkine([0 0 0 0 0 0])，在命令框中可以得到如圖3所示的位姿矩陣。

>>IRB120_R1.fkine([0 0 0 0 0 0])ans=-0.0000-0.00001.0000374.00000.00001.00000.00000.0000-1.00000.0000-0.0000630.00000001.0000

(2)實(shí)物驗(yàn)證。在RobotStudio中建立ABB-IRB120機(jī)器人系統(tǒng)，使各個(gè)關(guān)節(jié)角度歸零，通過機(jī)器人示教器手動(dòng)操作界面可以看到機(jī)器人末端(不含工具)相對(duì)于機(jī)器人基坐標(biāo)系的位移以及歐拉角如圖4所示。

圖4 ABB-IRB120工業(yè)機(jī)器人關(guān)于基坐標(biāo)的末端位姿

為了方便和圖4比較，圖3的位姿矩陣拆分為位置矩陣和姿態(tài)矩陣。

位置矩陣為：

P=[374,0,630]T

姿態(tài)矩陣為：

圖4示教器中的位置矩陣：

P′=[374,0,630]T

姿態(tài)是用歐拉角表示的，將其轉(zhuǎn)換為相對(duì)于基坐標(biāo)系的位姿矩陣形式，變化如下：

R′=RY(90°)

(19)

式中，表示機(jī)器人的基坐標(biāo)系：

(20)

RY(θ)表示繞Y軸旋轉(zhuǎn)θ角度的正交旋轉(zhuǎn)矩陣：

(21)

結(jié)合式(19)、式(20)和式(21)，可以得到圖4示教器中的姿態(tài)矩陣：

機(jī)器人模型得到的位置矩陣P和姿態(tài)矩陣R與仿真得到的位置矩陣P′和姿態(tài)矩陣R′進(jìn)行比較，發(fā)現(xiàn)：

P=P′
R=R′

顯然，使用MATLAB建立的模型和實(shí)際機(jī)器人完全一致，所以該模型建立是正確的。

2 雙機(jī)器人系統(tǒng)運(yùn)動(dòng)學(xué)建模

2.1 雙機(jī)器人系統(tǒng)基坐標(biāo)系標(biāo)定

(1)坐標(biāo)系的建立及標(biāo)定原理。如圖5標(biāo)定原理圖所示，主從機(jī)器人基坐標(biāo)分別為{O1}和{O2}。

圖5 標(biāo)定原理圖

雙機(jī)器人基坐標(biāo)系的標(biāo)定目的在于確定坐標(biāo)系2相對(duì)于坐標(biāo)系1之間的變換矩陣。通過圖5所示的標(biāo)定原理圖可得：

1T2=1T3·3T4·(2T4)-1

(22)

(2)具體標(biāo)定。首先，示教機(jī)器人R1，使標(biāo)定針針尖接觸標(biāo)定模型上的坐標(biāo)系1的坐標(biāo)原點(diǎn)，記該點(diǎn)為p11，記錄此時(shí)機(jī)器人R1各關(guān)節(jié)的角位移(記為ψ11)以及p11在基坐標(biāo)系下的坐標(biāo)值；然后，繼續(xù)示教機(jī)器人R1，使其接觸標(biāo)定模型上的坐標(biāo)系3的X軸和Y軸上任意一點(diǎn)，分別記點(diǎn)p12和p13，同樣記錄在p12和p13點(diǎn)時(shí)機(jī)器人1各關(guān)節(jié)的角位移(分別記為ψ12和ψ13)以及在基坐標(biāo)系下的坐標(biāo)值；最后，可以得到以下數(shù)據(jù)：

p11=(x11y11z11)
p12=(x12y12z12)
p13=(x13y13z13)
ψ11=(φ111φ112φ113φ114φ115φ116)
ψ12=(φ121φ122φ123φ124φ125φ126)
ψ13=(φ131φ132φ133φ134φ135φ136)

式中，ψij為第pij點(diǎn)機(jī)器人i的關(guān)節(jié)角度；φijm為第pij點(diǎn)機(jī)器人i的m關(guān)節(jié)的角位移。

同理，使用相同的方法，示教機(jī)器人R2標(biāo)定針接觸標(biāo)定模型上的坐標(biāo)系4，得到以下參數(shù)：

p21=(x21y21z21)
p22=(x22y22z22)
p23=(x23y23z23
ψ21=(φ211φ212φ213φ214φ215φ216)
ψ22=(φ221φ222φ223φ224φ225φ226)
ψ23=(φ231φ232φ233φ234φ235φ236)

由于p11、p12、p13是標(biāo)定模型上的坐標(biāo)系3上取的三點(diǎn)，利用三點(diǎn)建立一個(gè)坐標(biāo)系：以p11點(diǎn)為坐標(biāo)原點(diǎn)，以p11p12方向?yàn)閄軸方向，以p11p13方向?yàn)閅軸方向，Z軸方向通過X軸方向的方向向量和Y軸方向的方向向量差乘運(yùn)算獲得，如此便完成了圖5標(biāo)定原理圖中的坐標(biāo)系3的構(gòu)建。

同理，用相同的辦法，完成坐標(biāo)系4的構(gòu)建。

根據(jù)上述坐標(biāo)系3的構(gòu)建方法，取：

(23)

(24)

式中，u和v分別為坐標(biāo)系3的X軸和Y軸的方向向量，則Z軸的方向向量為：

w1=u1×v

(25)

得到坐標(biāo)系3相對(duì)于坐標(biāo)系1 的變換矩陣：

(26)

同理，坐標(biāo)系4相對(duì)于坐標(biāo)系2的變換矩陣為：

(27)

標(biāo)定模型上的坐標(biāo)系3和坐標(biāo)系4的坐標(biāo)變換3T4是已知的，將式(26)、式(27)以及3T4代入式(22)便可確定機(jī)器人R2的基坐標(biāo)系相對(duì)于機(jī)器人R1的基坐標(biāo)系的變換矩陣為：

(28)

至此雙機(jī)器人系統(tǒng)的基坐標(biāo)系標(biāo)定任務(wù)完成。

2.2 雙機(jī)器人系統(tǒng)運(yùn)動(dòng)學(xué)模型

如圖6所示，R1和R2分別為主、從機(jī)器人；{O1}為R1的基坐標(biāo)系；{O2}為R2的基坐標(biāo)系；{O}為世界坐標(biāo)系。

圖6 主從機(jī)器人運(yùn)動(dòng)學(xué)模型簡(jiǎn)圖

已知主從機(jī)器人基坐標(biāo)系之間的位姿關(guān)系1T2，主(從)機(jī)器人末端坐標(biāo)系相對(duì)于主(從)機(jī)器人基坐標(biāo)系之間的位姿關(guān)系TR1(TR2)。若以主機(jī)器人的基坐標(biāo)系作為雙機(jī)器人系統(tǒng)的基坐標(biāo)系，主從機(jī)器人末端到達(dá)空間中的某一位置，則從機(jī)器人的運(yùn)動(dòng)學(xué)方程為：

TR2=(1T2)-1·TR1

(29)

若主從機(jī)器人的末端沒有到達(dá)同一位置，則有：

R1TE2=1T2·TR2

(30)

式中，E2為從機(jī)器人的末端坐標(biāo)；R1TE2為從機(jī)器人末端坐標(biāo)系相對(duì)于主機(jī)器人基坐標(biāo)系的位姿變換。

同理，可得從機(jī)器人R2的基坐標(biāo)系和主機(jī)器人末端坐標(biāo)系之間的關(guān)系為：

R2TE1=(1T2)-1·TR1

(31)

3 仿真驗(yàn)證

3.1 基坐標(biāo)系標(biāo)定理論驗(yàn)證

在理想狀態(tài)下，如圖7所示，假設(shè)主從機(jī)器人同平面相對(duì)而立，在X軸方向相距800 mm。

圖7 雙機(jī)器人標(biāo)定系統(tǒng)

主從機(jī)器人基坐標(biāo)系之間的位姿變換矩陣為：

(32)

姿態(tài)矩陣R′為：

(33)

位置矩陣T′為：

(34)

標(biāo)定模型上坐標(biāo)系4相對(duì)于坐標(biāo)系3的變換矩陣為：

(35)

在MATLAB的機(jī)器人工具箱中建立如圖9所示的雙機(jī)器人標(biāo)定系統(tǒng)，然后按照本文提出的標(biāo)定法方進(jìn)行標(biāo)定，得到的數(shù)據(jù)如表2所示。

表2 機(jī)器人R1和R2在3個(gè)接觸點(diǎn)的各關(guān)節(jié)角度 (°)

表2中的數(shù)據(jù)代入式(23)～式(27)，得到坐標(biāo)系3相對(duì)于坐標(biāo)系1的變換矩陣為：

(36)

坐標(biāo)系4相對(duì)于坐標(biāo)系2的變換矩陣為：

(37)

結(jié)合式(28)、式(35)～式(37)，最終可以得到機(jī)器人R2的基坐標(biāo)系相對(duì)于主機(jī)器人R1的基坐標(biāo)系變換矩陣為：

(38)

3.2 運(yùn)動(dòng)學(xué)模型理論驗(yàn)證

假設(shè)主從機(jī)器人同平面相對(duì)而立，在X軸方向相距800 mm，如圖8所示。

圖8 雙機(jī)器人系統(tǒng)模型

圖8可知，主機(jī)器人R1的基坐標(biāo)系的坐標(biāo)原點(diǎn)和雙機(jī)器人系統(tǒng)的世界坐標(biāo)系的原點(diǎn)重合，主從機(jī)器人基坐標(biāo)系之間的位姿變換矩陣和式(32)相同：

(39)

從機(jī)器人R2的末端坐標(biāo)系相對(duì)于主機(jī)器人R1的基坐標(biāo)系的位姿變換為：

(40)

根據(jù)式(2)從機(jī)器人的運(yùn)動(dòng)學(xué)方程得到從機(jī)器人末端坐標(biāo)系和基坐標(biāo)系之間的位姿關(guān)系為：

(41)

由于主從機(jī)器人的末端沒重合，故將式(39)和式(40)帶入式(30)，得從機(jī)器人R2的末端坐標(biāo)系相對(duì)于主機(jī)器人R1的基坐標(biāo)系的位姿變換為：

(42)

對(duì)比式(40)和式(42)，可見該模型理論上是正確的。

3.3 運(yùn)動(dòng)學(xué)模型仿真驗(yàn)證

如圖9所示，建立雙機(jī)器人系統(tǒng)工作站。為方便主從機(jī)器人在空間中的某一點(diǎn)精準(zhǔn)“握手”，主從機(jī)器人均加上相同的標(biāo)定針。

圖9 雙機(jī)器人系統(tǒng)工作站

主從機(jī)器人的標(biāo)定針末端到達(dá)空間中的同一位置，從示教器中得到的數(shù)據(jù)如表3所示。

表3 機(jī)器人R1和R2的末端位置和姿態(tài)

表中的數(shù)據(jù)顯示，主從機(jī)器人的末端(標(biāo)定針針尖)位置相同，但是姿態(tài)不同，利用坐標(biāo)變換讓主從機(jī)器人末端姿態(tài)相同，得到：

(43)

由于主從機(jī)器人末端執(zhí)行器均加上了標(biāo)定針，可將式(29)變形為：

(44)

根據(jù)表3中的數(shù)據(jù)，可得：

(45)

(46)

將式(39)、式(43)、式(45)代入式(44)得：

(47)

4 結(jié)論

使用改進(jìn)D-H參數(shù)法建立了ABB IRB-120工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)模型，并驗(yàn)證了建立的機(jī)器人運(yùn)動(dòng)學(xué)模型的正確性。提出一種基于標(biāo)定模型的基坐標(biāo)系標(biāo)定方法對(duì)雙機(jī)器人系統(tǒng)進(jìn)行了標(biāo)定，在雙機(jī)器人系統(tǒng)標(biāo)定的基礎(chǔ)上建立了雙機(jī)器人運(yùn)動(dòng)學(xué)模型，使用MATLAB和RobotStudio驗(yàn)證了雙機(jī)器人運(yùn)動(dòng)學(xué)模型的正確性。研究表明，本文建立的雙機(jī)器人運(yùn)動(dòng)學(xué)模型是正確的。