武永強，于 濤，紀 俐

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

0 引言

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

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

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

1 單機器人建模與驗證

1.1 機器人坐標系

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

圖1 ABB-IRB120型機器人D-H坐標系

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

表1中，θi表示第i關節(jié)的關節(jié)轉角；di表示第i連桿的關節(jié)距離；ai-1表示第i-1連桿的連桿長度；αi-1表示第i-1連桿的連桿扭角；βi表示第i關節(jié)的關節(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°)。

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

1.2 正運動學

使用改進的D-H算法[11]建立機器人的運動學模型，坐標系{Oi-1}到坐標系{Oi}之間的坐標變換，可由式(1)得到：

(1)

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

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

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

將基坐標系的變換矩陣和各連桿的變換矩陣依次相乘，得機器人正運動學方程[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 逆運動學

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

由變換矩陣T4易得坐標系{4}的坐標原點在坐標系{3}中的表述為：

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

(3)

結合式(2)，進而可得機器人末端在基坐標系下的表述為：

(4)

(5)

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

pxs1-pyc1=0

(6)

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

θ1=arctan2(py,px)

或者

θ1=arctan2(-py,-px)

(7)

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

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)矩陣來建立有效的方程求解，上述已經求得θ1、θ2、θ3，后面將T1、T2、T3視為已知量。根據(jù)式(2)可以得到：

(14)

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

(15)

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

c5= -azc23-s23(axc1+ays1)

(16)

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

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

(17)

(18)

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

當s5>0時，

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

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

當s5<0時，

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

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

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

1.4 機器人模型的驗證

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

圖2 IRB120機器人運動學模型

使用命令：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)實物驗證。在RobotStudio中建立ABB-IRB120機器人系統(tǒng)，使各個關節(jié)角度歸零，通過機器人示教器手動操作界面可以看到機器人末端(不含工具)相對于機器人基坐標系的位移以及歐拉角如圖4所示。

圖4 ABB-IRB120工業(yè)機器人關于基坐標的末端位姿

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

位置矩陣為：

P=[374,0,630]T

姿態(tài)矩陣為：

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

P′=[374,0,630]T

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

R′=RY(90°)

(19)

式中，表示機器人的基坐標系：

(20)

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

(21)

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

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

P=P′
R=R′

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

2 雙機器人系統(tǒng)運動學建模

2.1 雙機器人系統(tǒng)基坐標系標定

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

圖5 標定原理圖

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

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

(22)

(2)具體標定。首先，示教機器人R1，使標定針針尖接觸標定模型上的坐標系1的坐標原點，記該點為p11，記錄此時機器人R1各關節(jié)的角位移(記為ψ11)以及p11在基坐標系下的坐標值；然后，繼續(xù)示教機器人R1，使其接觸標定模型上的坐標系3的X軸和Y軸上任意一點，分別記點p12和p13，同樣記錄在p12和p13點時機器人1各關節(jié)的角位移(分別記為ψ12和ψ13)以及在基坐標系下的坐標值；最后，可以得到以下數(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點機器人i的關節(jié)角度；φijm為第pij點機器人i的m關節(jié)的角位移。

同理，使用相同的方法，示教機器人R2標定針接觸標定模型上的坐標系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是標定模型上的坐標系3上取的三點，利用三點建立一個坐標系：以p11點為坐標原點，以p11p12方向為X軸方向，以p11p13方向為Y軸方向，Z軸方向通過X軸方向的方向向量和Y軸方向的方向向量差乘運算獲得，如此便完成了圖5標定原理圖中的坐標系3的構建。

同理，用相同的辦法，完成坐標系4的構建。

根據(jù)上述坐標系3的構建方法，?。?/p>

(23)

(24)

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

w1=u1×v

(25)

得到坐標系3相對于坐標系1 的變換矩陣：

(26)

同理，坐標系4相對于坐標系2的變換矩陣為：

(27)

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

(28)

至此雙機器人系統(tǒng)的基坐標系標定任務完成。

2.2 雙機器人系統(tǒng)運動學模型

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

圖6 主從機器人運動學模型簡圖

已知主從機器人基坐標系之間的位姿關系1T2，主(從)機器人末端坐標系相對于主(從)機器人基坐標系之間的位姿關系TR1(TR2)。若以主機器人的基坐標系作為雙機器人系統(tǒng)的基坐標系，主從機器人末端到達空間中的某一位置，則從機器人的運動學方程為：

TR2=(1T2)-1·TR1

(29)

若主從機器人的末端沒有到達同一位置，則有：

R1TE2=1T2·TR2

(30)

式中，E2為從機器人的末端坐標；R1TE2為從機器人末端坐標系相對于主機器人基坐標系的位姿變換。

同理，可得從機器人R2的基坐標系和主機器人末端坐標系之間的關系為：

R2TE1=(1T2)-1·TR1

(31)

3 仿真驗證

3.1 基坐標系標定理論驗證

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

圖7 雙機器人標定系統(tǒng)

主從機器人基坐標系之間的位姿變換矩陣為：

(32)

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

(33)

位置矩陣T′為：

(34)

標定模型上坐標系4相對于坐標系3的變換矩陣為：

(35)

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

表2 機器人R1和R2在3個接觸點的各關節(jié)角度 (°)

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

(36)

坐標系4相對于坐標系2的變換矩陣為：

(37)

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

(38)

3.2 運動學模型理論驗證

假設主從機器人同平面相對而立，在X軸方向相距800 mm，如圖8所示。

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

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

(39)

從機器人R2的末端坐標系相對于主機器人R1的基坐標系的位姿變換為：

(40)

根據(jù)式(2)從機器人的運動學方程得到從機器人末端坐標系和基坐標系之間的位姿關系為：

(41)

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

(42)

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

3.3 運動學模型仿真驗證

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

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

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

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

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

(43)

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

(44)

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

(45)

(46)

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

(47)

4 結論

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