六自由度機(jī)器人的運(yùn)動(dòng)學(xué)分析

王夢(mèng)濤，張良安

(安徽工業(yè)大學(xué)機(jī)械工程學(xué)院，安徽馬鞍山243032)

六自由度機(jī)器人的運(yùn)動(dòng)學(xué)分析

王夢(mèng)濤，張良安

(安徽工業(yè)大學(xué)機(jī)械工程學(xué)院，安徽馬鞍山243032)

針對(duì)一種自主研發(fā)的二三四軸相互平行的六自由度機(jī)器人，通過(guò)D-H(Denavit-Hartenberg)法建立機(jī)器人連桿坐標(biāo)系和變換矩陣，求得正運(yùn)動(dòng)學(xué)方程，基于矩陣逆乘的思路進(jìn)行逆運(yùn)動(dòng)學(xué)的求解，提出此類六自由度機(jī)器人逆運(yùn)動(dòng)學(xué)求解的方法，求出逆運(yùn)動(dòng)學(xué)的全部解析解。針對(duì)該機(jī)器人的奇異性，提出奇異位置逆解的處理方法，并用MATLAB機(jī)器人工具箱Robotics Toolbox驗(yàn)證了正解模型和逆解算法的正確性。研究結(jié)果為該機(jī)器人后續(xù)的軌跡規(guī)劃和控制提供理論依據(jù)，對(duì)于具有相似結(jié)構(gòu)的機(jī)器人運(yùn)動(dòng)學(xué)正逆解問(wèn)題具有借鑒意義。

六自由度機(jī)器人；運(yùn)動(dòng)學(xué)；機(jī)器人工具箱；奇異性

在2015上海、蘇州的工業(yè)機(jī)器人展覽會(huì)上，眾多機(jī)器人廠商紛紛推出其3～10 kg輕載機(jī)器人產(chǎn)品，如日本那智不二越的MZ系列六自由度機(jī)器人、德國(guó)KUKA的LBR iiwa機(jī)器人、瑞士ABB的YuMi機(jī)器人等。一種新的被稱為“協(xié)作機(jī)器人”的輕載機(jī)器人正越來(lái)越多地出現(xiàn)在人機(jī)協(xié)作的場(chǎng)合，有望填補(bǔ)全手動(dòng)裝配與全自動(dòng)生產(chǎn)線之間的差距[1]，由此看出輕載機(jī)器人有較好的應(yīng)用前景。

機(jī)器人的正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)是機(jī)器人研究中的關(guān)鍵問(wèn)題，其中逆解的研究廣受關(guān)注。求正解中常采用D-H(Denavit-Hartenberg)法得到連桿變換矩陣，將連桿變換矩陣相乘得到正解方程[2]。求逆解常見(jiàn)的方法有解析解法和數(shù)值解法，其中解析解法進(jìn)一步劃分為幾何解法與代數(shù)解法[3]，而數(shù)值解法由于需要迭代，一般比解析解法慢。Paul等[4]提出的反變換解析法對(duì)于多自由度機(jī)器人求逆解十分有效；Pieper等[5]針對(duì)末端三軸交于一點(diǎn)的機(jī)器人提出Pieper法；王英石[6]采用旋量理論求解冗余機(jī)器人的運(yùn)動(dòng)學(xué)問(wèn)題；姜宏超等[7]針對(duì)自主研發(fā)的六自由度模塊化機(jī)器人，采用矩陣逆乘求出了16組逆解；邱寧佳等[8]針對(duì)六自由度機(jī)器人空間劃線的具體應(yīng)用，限定關(guān)節(jié)角度，并由運(yùn)動(dòng)時(shí)關(guān)節(jié)約束簡(jiǎn)化求逆解過(guò)程，避免了使用矩陣求逆解或高斯消去法等相對(duì)繁瑣的算法；曾劍等[9]針對(duì)六自由度噴涂機(jī)器人，采用臂、腕分離法(即Pieper法)求出該機(jī)器人正逆運(yùn)動(dòng)學(xué)的解析表達(dá)式；耿磊[10]采用局部消元法求得庫(kù)卡六自由度工業(yè)機(jī)器人逆運(yùn)動(dòng)學(xué)的解析解。

以上逆解算法多針對(duì)末端三軸相交的機(jī)器人，該機(jī)器人與二三四軸相互平行的六自由度機(jī)器人結(jié)構(gòu)不同?；贒-H法求解二三四軸相互平行與末端三軸相交的機(jī)器人的運(yùn)動(dòng)學(xué)問(wèn)題，其求解方法不能通用，而目前較少學(xué)者研究二三四軸相互平行機(jī)器人的運(yùn)動(dòng)學(xué)問(wèn)題。為此，筆者針對(duì)二三四軸相互平行的六自由度機(jī)器人，采用D-H法求解機(jī)器人的運(yùn)動(dòng)學(xué)正逆解問(wèn)題，且對(duì)其奇異性進(jìn)行分析，使用Robotics Toolbox工具箱驗(yàn)證所求正逆解模型的正確性。

1 運(yùn)動(dòng)學(xué)模型的建立

1.1 機(jī)器人結(jié)構(gòu)設(shè)計(jì)

六自由度機(jī)器人存在封閉解有兩個(gè)充分條件[11]：3個(gè)相鄰關(guān)節(jié)交于一點(diǎn)；3個(gè)相鄰關(guān)節(jié)軸相互平行。多數(shù)焊接機(jī)器人滿足第一個(gè)條件，而丹麥優(yōu)傲機(jī)器人UR系列和北京遨博智能的OUR系列機(jī)器人則滿足第二個(gè)條件。本文研究的六自由度機(jī)器人為自主研發(fā)的機(jī)器人，滿足第二個(gè)條件，其三維模型如圖1。圖中數(shù)字1，…，6表示關(guān)節(jié)軸線，由圖1可以看出，該機(jī)器人二三四軸相互平行。

圖1 六自由度機(jī)器人整體結(jié)構(gòu)及轉(zhuǎn)動(dòng)關(guān)節(jié)Fig.1 Structure and rotation joints of 6 degree-of-freedom robot

1.2 連桿坐標(biāo)系的建立

采用D-H法[12]建立連桿坐標(biāo)系，在每個(gè)連桿上固接一個(gè)坐標(biāo)系，通過(guò)坐標(biāo)系之間的齊次變換矩陣相乘，可得到機(jī)器人末端相對(duì)于基坐標(biāo)系的位姿矩陣。針對(duì)二三四軸相互平行的六自由度機(jī)器人，其連桿坐標(biāo)系Ok-XkYkZk的建立過(guò)程為：坐標(biāo)軸Zk為圖1所示關(guān)節(jié)軸線j(j=1,…,6)，方向任意；找出相鄰兩軸線j和j+1的公垂線，坐標(biāo)軸Xk與公垂線重合，方向指向軸j+1，關(guān)節(jié)公垂線與軸j的交點(diǎn)為坐標(biāo)系Ok-XkYkZk的原點(diǎn)；若軸j與j+1相交，則取交點(diǎn)為坐標(biāo)系Ok-XkYkZk的原點(diǎn)，坐標(biāo)軸Xk為Zk與Zk+1的法線，方向任意；若軸j與軸j+1平行，原點(diǎn)取在使Xk到Xk+1沿Zk+1方向距離為零的位置，坐標(biāo)軸Xk為相鄰兩關(guān)節(jié)軸線j和j+1的公垂線，方向指向軸j+1；基坐標(biāo)系O0-X0Y0Z0與基座固接，可任意規(guī)定，一般使坐標(biāo)軸Z0與Z1重合；末端連桿坐標(biāo)系O6-X6Y6Z6原點(diǎn)可任意選取，坐標(biāo)軸X6選取方法為當(dāng)關(guān)節(jié)變量為零時(shí)，X6與X5重合；坐標(biāo)系的Y軸按右手法則確定。

建立的二三四軸相互平行的六軸機(jī)器人連桿坐標(biāo)系如圖2。其中基坐標(biāo)系原點(diǎn)O0與第一連桿坐標(biāo)系原點(diǎn)O1重合，第五連桿坐標(biāo)系原點(diǎn)O5與第六連桿坐標(biāo)系原點(diǎn)O6重合。表1為六自由度機(jī)器人連桿參數(shù)。

表中：ai-1表示連桿長(zhǎng)度(i=1,…,6)，即從Zk-1到Zk沿坐標(biāo)軸Xk-1方向的距離；αi-1表示連桿扭角，即從Zk-1到Zk繞坐標(biāo)軸Xk-1旋轉(zhuǎn)的角度；di表示從Xk-1到Xk沿坐標(biāo)軸Zk方向的距離；θi表示從Xk-1到Xk繞坐標(biāo)軸Zk旋轉(zhuǎn)的角度；a2=582 mm;a3=424 mm；d2=240 mm；d5=329 mm。

2 運(yùn)動(dòng)學(xué)分析

機(jī)器人的運(yùn)動(dòng)學(xué)包括正運(yùn)動(dòng)學(xué)問(wèn)題和逆運(yùn)動(dòng)學(xué)問(wèn)題，由關(guān)節(jié)角度確定末端位姿是正運(yùn)動(dòng)學(xué)問(wèn)題，也稱運(yùn)動(dòng)學(xué)正解。由末端位姿確定各關(guān)節(jié)的角度是逆運(yùn)動(dòng)學(xué)問(wèn)題，也稱運(yùn)動(dòng)學(xué)逆解。正解是逆解的基礎(chǔ)，可用來(lái)驗(yàn)證逆解的正確性。在工業(yè)應(yīng)用時(shí)，常常給定一系列末端位置，通過(guò)逆解求得每個(gè)位置對(duì)應(yīng)的關(guān)節(jié)角度，控制電機(jī)轉(zhuǎn)動(dòng)使每個(gè)關(guān)節(jié)到達(dá)求得的位置，從而使末端到達(dá)給定的位姿。

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

連桿i相對(duì)于連桿i-1的變換矩陣為

式(1)由4個(gè)參數(shù)ai-1、αi-1、di、θi決定，只有變量θi不確定，即變換矩陣隨θi變化而變化。文中六自由度機(jī)器人連桿坐標(biāo)系O6-X6Y6Z6相對(duì)于基坐標(biāo)系O0-X0Y0Z0，其變換矩陣為各變換矩陣相乘，為

式中：nx，ny，nz分別表示坐標(biāo)系O6-X6Y6Z6的坐標(biāo)軸X6相對(duì)于基坐標(biāo)系O0-X0Y0Z0在X，Y，Z軸的方向余弦；ox，oy，oz分別表示坐標(biāo)系O6-X6Y6Z6的坐標(biāo)軸Y6相對(duì)于基坐標(biāo)系O0-X0Y0Z0在X，Y，Z軸的方向余弦；ax，ay，az分別表示坐標(biāo)系O6-X6Y6Z6的坐標(biāo)軸Z6相對(duì)于基坐標(biāo)系O0-X0Y0Z0在X，Y，Z軸的方向余弦；px，py，pz表示坐標(biāo)系O6-X6Y6Z6相對(duì)于基坐標(biāo)系O0-X0Y0Z0的位置坐標(biāo)。

2.2 逆運(yùn)動(dòng)學(xué)求解

六自由度機(jī)器人的逆運(yùn)動(dòng)學(xué)求解是給定末端位置和姿態(tài)，求出各關(guān)節(jié)變量，其解可能存在多重解，也可能無(wú)解。文中基于矩陣逆乘的方法，即Paul等[4]提出的反變換方法，對(duì)機(jī)器人關(guān)節(jié)變量進(jìn)行求解。

二三四軸相互平行的六自由度機(jī)器人，不同于常見(jiàn)的三軸相交于一點(diǎn)的六自由度機(jī)器人，采用矩陣逆乘的方法求逆解時(shí)每個(gè)關(guān)節(jié)角求解的順序很重要。求平行關(guān)節(jié)角度時(shí)，需先求平行關(guān)節(jié)的關(guān)節(jié)角度和，再用和減去單個(gè)關(guān)節(jié)角度得到其余關(guān)節(jié)角度，即求逆解的過(guò)程中先求θ1,θ5,θ6，后求θ2,θ3,θ4。求θ2,θ3,θ4時(shí)，先求θ2,θ3,θ4這3個(gè)平行關(guān)節(jié)的一個(gè)角度θ2，然后求θ23，通過(guò)(θ23-θ2)求得θ3，再求θ234，用(θ234-θ23)求解θ4。其中θ23=θ2+θ3，θ234=θ2+θ3+θ4。

2.2.1 求解θ1,θ5,θ6

令式(3)兩邊矩陣第二行第四、三、二、一列元素相等，可得下面4個(gè)方程：

其中s1=sinθ1,c1=cosθ1，其余類推。由式(4)得

由式(5)得

進(jìn)而得到θ5的解，因?yàn)棣?有2個(gè)解，所以對(duì)應(yīng)θ5有4個(gè)解。

由式(6)，(7)得θ6與θ5一一對(duì)應(yīng)，有4個(gè)解。

2.2.2 求解θ2,θ3,θ4

已求出θ1，θ5，θ6，將式(2)改寫(xiě)成

由式(11)兩邊第一、三行第四列的元素相等，得

式中：c23=cosθ23=cos(θ2+θ3)；s23=sinθ23=sin(θ2+θ3)；m1,m2分別為式(11)右邊第一行第四列和第三行第四列的元素，m1,m2對(duì)應(yīng)θ1、θ5、θ6各有4個(gè)值。

將式(12)含θ23項(xiàng)移到一邊，得

式(13)兩邊取平方后相加，得

則可解得θ2的兩組解，因?yàn)閙1,m2有4個(gè)值，所以θ2有8個(gè)解，即

求出θ2，則由式(13)得

求得θ23，減去θ2可得θ3，θ3有8個(gè)解，且與θ2一一對(duì)應(yīng)。

由式(11)兩邊第三行第一列、第三行第二列元素相等，得

則可求得

由式(15)，(18)求得

θ234有4個(gè)值，且與θ23的8個(gè)值對(duì)應(yīng)，故θ4有8個(gè)解。

2.3 奇異性分析

機(jī)器人在奇異位置時(shí)運(yùn)動(dòng)學(xué)逆解會(huì)發(fā)生跳變，這對(duì)后期的軌跡規(guī)劃造成潛在危險(xiǎn)。如果機(jī)器人的逆運(yùn)動(dòng)學(xué)具有解析形式的解，根據(jù)文獻(xiàn)[5]，可通過(guò)判斷該解的代數(shù)式分母為零等異常情況，找到機(jī)器人的可能奇異點(diǎn)。在這些可能奇異點(diǎn)位置，如果機(jī)器人雅可比矩陣行列式的值為零，則機(jī)器人處于奇異位置。

針對(duì)二三四軸相互平行的六自由度機(jī)器人，逆運(yùn)動(dòng)學(xué)存在解析形式的解，即式(10)。當(dāng)s5=sinθ5=0時(shí)，式(10)出現(xiàn)分母為零的情況，通過(guò)計(jì)算可得機(jī)器人的雅可比矩陣行列式的值為零，所以sinθ5=0時(shí)機(jī)器人處于奇異位置。此時(shí)θ6可取任意值均滿足給定的約束，對(duì)此需對(duì)奇異點(diǎn)附近的θ6作平滑處理，避免θ6發(fā)生跳變，同時(shí)保證機(jī)器人的運(yùn)動(dòng)精度。

3 運(yùn)動(dòng)學(xué)正逆解的仿真驗(yàn)證

比較采用機(jī)器人工具箱Robotics Toolbox[13]與正解模型式(2)求得的正解，若兩者相差小于10-4，則式(2)得到驗(yàn)證。將求得的8個(gè)逆解代入Robotics Toolbox求得位姿矩陣與最初給定的位姿矩陣且進(jìn)行比較，若兩矩陣前三行三列相差小于10-4，第四列相差小于10-2，則逆解得到驗(yàn)證。

3.1 運(yùn)動(dòng)學(xué)正解的驗(yàn)證

采用機(jī)器人工具箱Robotics Toolbox中提供的函數(shù)對(duì)六自由度機(jī)器人建模，并進(jìn)行正運(yùn)動(dòng)學(xué)仿真。主要MATLAB代碼為：

L1，…，L6表示工具箱Robotics Toolbox中l(wèi)ink函數(shù)，‘link’是Robotics Toolbox中的函數(shù)，用來(lái)創(chuàng)建桿件坐標(biāo)系，調(diào)用方法為

其中：參數(shù)α,a,θ,d為表1中的連桿參數(shù)。‘sigma’表示關(guān)節(jié)類型，0表示旋轉(zhuǎn)關(guān)節(jié)，非0表示平動(dòng)關(guān)節(jié)，這里取0；CONVENTION可以取‘standard’或‘modified’，‘standard’表示采用標(biāo)準(zhǔn)D-H法得到的連桿參數(shù)[15]，‘modified’表示采用改進(jìn)的D-H法得到的連桿參數(shù)[16]，這里取‘standard’；q表示6個(gè)關(guān)節(jié)的變量θi；函數(shù)‘fkine’用來(lái)求正解。采用相同的關(guān)節(jié)變量θi由式(2)求正解。通過(guò)機(jī)器人工具箱與正解模型式(2)求得的正解均為

2次求得的結(jié)果保留小數(shù)點(diǎn)后4位相等，隨機(jī)取多個(gè)不同的q同樣滿足正解結(jié)果相等，證明運(yùn)動(dòng)學(xué)正解模型正確。

3.2 運(yùn)動(dòng)學(xué)逆解的驗(yàn)證

設(shè)定目標(biāo)位姿矩陣為式(20)，采用2.2節(jié)的求逆解過(guò)程編寫(xiě)程序求得8個(gè)逆解，表2中序號(hào)1～8為所求的逆解。使用Robotics Toolbox工具箱的‘fkine’函數(shù)求逆解對(duì)應(yīng)的位姿矩陣，如B1,…,B8。

表2 8個(gè)逆解Tab.2 Eight solutions of inverse kinematics

從表2可以看到，逆解3與4相同，逆解5與6相同，實(shí)際只有6組逆解。比較B1～B8可以看出，B1、B2、B7、B8與式(20)前三行三列相同，相差小于10-4，第四列（位置坐標(biāo)）相差小于10-2，偏差在工程應(yīng)用允許范圍內(nèi)；B3、B4、B5、B6與式(20)相差較大，認(rèn)為逆解3、4、5、6是錯(cuò)誤解，逆解1、2、7、8是正確解。只求出4組正確逆解并不代表逆解模型錯(cuò)誤，而是表明此時(shí)機(jī)器人只有4組逆解。通過(guò)計(jì)算發(fā)現(xiàn)大部分位姿時(shí)機(jī)器人有8組逆解，而有些位姿時(shí)機(jī)器人逆解少于8組，所以在計(jì)算逆解時(shí)需代回正解模型中驗(yàn)證，剔除多余的逆解。

4 結(jié) 論

針對(duì)自主設(shè)計(jì)的一種二三四軸相互平行的六自由度機(jī)器人，對(duì)其進(jìn)行運(yùn)動(dòng)學(xué)分析，得出以下結(jié)論：

1)運(yùn)用D-H法求得該機(jī)器人運(yùn)動(dòng)學(xué)正解，基于矩陣逆乘的思路求得運(yùn)動(dòng)學(xué)逆解，進(jìn)行逆運(yùn)動(dòng)學(xué)求解時(shí)，關(guān)節(jié)角的求解順序很關(guān)鍵，求平行關(guān)節(jié)角度時(shí)，需先求平行關(guān)節(jié)的角度和，再相減得到其余關(guān)節(jié)角度；

2)當(dāng)六自由度機(jī)器人處于奇異位置即sinθ5=0時(shí)，求逆解時(shí)，θ6取任意值均滿足給定的約束；

3)運(yùn)用機(jī)器人工具箱Robotics Toolbox對(duì)正逆解進(jìn)行驗(yàn)證，結(jié)果表明所求正逆解是正確的，由此進(jìn)一步表明該正逆解模型可以用于此類機(jī)器人后續(xù)軌跡規(guī)劃和控制。

[1]艾斯本·奧斯特加.你的新同事：走出牢籠的機(jī)器人[J].辦公自動(dòng)化，2014(15):25-27.

[2]余曉流，劉進(jìn)福，汪麗芳.基于ADAMS的六自由度焊接機(jī)器人運(yùn)動(dòng)學(xué)分析及仿真[J].安徽工業(yè)大學(xué)學(xué)報(bào)(自然科學(xué)版)，2012,29(1):49-51.

[3]CRAIG J J.機(jī)器人學(xué)導(dǎo)論[M].贠超，譯.北京：機(jī)械工業(yè)出版社，2004:83-88.

[4]PAUL R P,SHIMAO B E,MAYER G.Kinematics control equations for simple manipulators[J].IEEE Transactions on Systems, Man and Cybernetics,1981,11(6):449-445.

[5]PIEPER D,ROTH B.The kinematics of manipulators under computer control[C]//Proc of the 2ND International Congress on Theory of Machines and Mechanisms.Zakopane:Springer,1969:159-169.

[6]王英石.冗余機(jī)器人的運(yùn)動(dòng)學(xué)及軌跡規(guī)劃的研究[D].天津：南開(kāi)大學(xué)，2014:35-40.

[7]姜宏超，劉士榮，張波濤.六自由度模塊化機(jī)器人的逆運(yùn)動(dòng)學(xué)分析[J].浙江大學(xué)學(xué)報(bào)(工學(xué)版)，2010,44(7):1348-1354.

[8]邱寧佳，隋振，李明哲，等.六自由度機(jī)器人空間劃線軌跡規(guī)劃算法[J].吉林大學(xué)學(xué)報(bào)(工學(xué)版)，2013,43(5):1307-1313.

[9]曾劍，林義忠，廖小平，等.6R型噴涂機(jī)器人運(yùn)動(dòng)學(xué)分析及仿真[J].機(jī)械設(shè)計(jì)與制造，2010(6):145-147.

[10]耿磊.六自由度工業(yè)機(jī)器人的建模與仿真研究[D].沈陽(yáng)：東北大學(xué)，2013:20-23.

[11]熊有倫，丁漢，流恩滄.機(jī)器人學(xué)[M].北京：機(jī)械工業(yè)出版社，1993:74-75.

[12]DENAVIT J,HARTENBERG R S.A kinematic notation for lower pair mechanisms based on matrices[J].ASME Journal of Applied Mechanics,1955,77(6):215-221.

[13]CORKE P.Arobotics toolbox for MATLAB[J].IEEE Robotics andAutomation Magazine,1996,3(1):24-32.

[14]謝斌，蔡自興.基于MATLAB Robotics Toolbox的機(jī)器人學(xué)仿真實(shí)驗(yàn)教學(xué)[J].計(jì)算機(jī)教育，2010(19):140-143.

[15]PAUL R P.Robot Manipulators:Mathematics,Programming and Control[M].Cambridge,Massachusetts:MIT Press,1981: 98-100.

[16]CRAIG J J.Introduction to Robotics:Mechanics and Control[M].3rd ed.New York:Prentice Hall,2005:21-30.

責(zé)任編輯：何莉

KinematicsAnalysis of 6 Degree-of-freedom Robot

WANG Mentao，ZHANG Liang''an
(School of Mechanical Engineering,Anhui University of Technology,Ma'anshan 243032,China)

For a developed 6 degree-of-freedom robot with 2,3 and 4-axis parallelled to each other,a forward kinematics equation of the robot was acquired by establishing the link coordinate system and transformation matrix with D-H(Denavit-Hartenberg)method.Based on the idea of matrix inversion,the inverse kinematics calculation method of 6 degree-of-freedom robot was proposed,and all analytical solutions of the inverse kinematics were obtained.According to the singularity of the robot,the processing method of the inverse solution of the singular position was proposed,the forward kinematics model and inverse kinematics solution were verified by Robotics Toolbox in MATLAB software.The research results provide a basis for later trajectory planning and control of the robot,and it is a reference for the kinematics of the robot with similar structure.

6 degree-of-freedom robot；kinematics；robotics toolbox；singularity

TP 242.2

：A

10.3969/j.issn.1671-7872.2016.04.011

1671-7872(2016)04-0365-07

2015-07-14

國(guó)家自然科學(xué)基金項(xiàng)目(51375014)

王夢(mèng)濤(1992-)，男，安徽靈璧人，碩士生，研究方向?yàn)闄C(jī)器人機(jī)構(gòu)學(xué)及工業(yè)機(jī)器人控制技術(shù)。

張良安(1981-)，男，浙江杭州人，博士，副教授，主要研究方向?yàn)樽詣?dòng)化生產(chǎn)線和機(jī)器人機(jī)構(gòu)學(xué)。