童水光，王 敏，從飛云,童哲銘

(浙江大學(xué) 機(jī)械工程學(xué)院，杭州 310027)

0 引言

目前機(jī)械臂已廣泛應(yīng)用于生產(chǎn)制造業(yè)，對(duì)機(jī)械臂的精度和實(shí)時(shí)性要求也越來(lái)越高。而機(jī)械臂的逆運(yùn)動(dòng)學(xué)分析作為機(jī)械臂運(yùn)動(dòng)學(xué)的重要組成環(huán)節(jié)，直接影響機(jī)械臂的位置精度、動(dòng)力學(xué)特性和實(shí)時(shí)響應(yīng)性，但目前其求解過(guò)程涉及大量矩陣乘積，矩陣求逆等非線性方程組的計(jì)算，求解復(fù)雜耗時(shí)[1-3]。

針對(duì)這一問(wèn)題，國(guó)內(nèi)外學(xué)者提出一些改進(jìn)的方法，付中濤等提出基于符號(hào)運(yùn)算預(yù)處理和矩陣特征向量提取的算法[4]，通過(guò)矩陣拆分和符號(hào)運(yùn)算消去系數(shù)矩陣中的未知變量，采用矩陣特征值的分解和特征向量的選取得到逆運(yùn)動(dòng)學(xué)的16組解；T Ao等提出結(jié)合旋量和代數(shù)方法的工業(yè)機(jī)械臂逆運(yùn)動(dòng)學(xué)解法[5]，利用工業(yè)機(jī)械臂最后三個(gè)關(guān)節(jié)依次兩兩垂直的結(jié)構(gòu)特點(diǎn)進(jìn)一步簡(jiǎn)化計(jì)算過(guò)程；林陽(yáng)等提出基于多種群遺傳算法的機(jī)械臂逆運(yùn)動(dòng)學(xué)算法[6]，采用動(dòng)態(tài)變異率的變異算子，種群替代和個(gè)體替代相結(jié)合的移民算子來(lái)克服全局收斂的盲目性。

現(xiàn)有的逆運(yùn)動(dòng)學(xué)求解方法大多數(shù)都是利用數(shù)值法或幾何法單獨(dú)實(shí)現(xiàn)[7-9]，需要龐大的計(jì)算量，而一些智能算法程序復(fù)雜，需要高性能的計(jì)算機(jī)配置，且存在穩(wěn)定性不足的問(wèn)題[10]，本文利用旋轉(zhuǎn)子矩陣正交的特性和位姿分離思想對(duì)一般6自由度串聯(lián)機(jī)械臂的逆運(yùn)動(dòng)學(xué)解法進(jìn)行改進(jìn)，利用位置子矩陣用來(lái)求解前3個(gè)關(guān)節(jié)變量，旋轉(zhuǎn)子矩陣用來(lái)求解后3個(gè)關(guān)節(jié)變量，求解過(guò)程簡(jiǎn)單明了。此外，對(duì)一般6自由度串聯(lián)機(jī)械臂存在的奇異點(diǎn)問(wèn)題和多解問(wèn)題也進(jìn)行了詳細(xì)的說(shuō)明。最后在RobotStudio仿真和汽車輪轂打磨的實(shí)際工況中進(jìn)行驗(yàn)證，結(jié)果表明，該算法有效且縮短了求解時(shí)間，滿足機(jī)械臂工作的實(shí)時(shí)性要求

1 算法基本思想

1.1 位姿分離思想

從機(jī)械臂的結(jié)構(gòu)設(shè)計(jì)角度出發(fā)，6自由度機(jī)械臂有3個(gè)自由度用于規(guī)定位置，另外3個(gè)自由度用來(lái)規(guī)定姿態(tài)[1]。利用這一思想，可以從位置和姿態(tài)分別聯(lián)立方程組求解正運(yùn)動(dòng)學(xué)方程。利用Denavit和Hartenberg提出的描述機(jī)械臂各連桿之間運(yùn)動(dòng)關(guān)系的通用方法(以下簡(jiǎn)稱Denavit-Hatenberg法)建立坐標(biāo)系[11]，得到機(jī)械臂末端與基座的關(guān)系為[1]：

(1)

其中，R表示旋轉(zhuǎn)子矩陣，P表示位置子矩陣，根據(jù)位姿分離思想，位置子矩陣用來(lái)求解前3個(gè)關(guān)節(jié)變量θ1～θ3，旋轉(zhuǎn)子矩陣用來(lái)求解后3個(gè)關(guān)節(jié)變量θ4～θ6。

因旋轉(zhuǎn)子矩陣是3階方陣，相比于傳統(tǒng)方法直接用4階方陣T求解，該算法對(duì)矩陣進(jìn)行了降維，可有效簡(jiǎn)化運(yùn)算，縮短運(yùn)算時(shí)間。

1.2 正交矩陣

在傳統(tǒng)解法中，求解后3個(gè)關(guān)節(jié)變量θ4～θ6時(shí)，需要對(duì)3個(gè)變換矩陣T1T3T3求逆矩陣，將4、5、6關(guān)節(jié)進(jìn)行解耦，運(yùn)算復(fù)雜。

經(jīng)證明R-1RT=E，即旋轉(zhuǎn)子矩陣為正交矩陣，因此可以利用正交矩陣的性質(zhì)：矩陣的逆等于矩陣的轉(zhuǎn)置，R-1=RT，避免求逆矩陣的復(fù)雜運(yùn)算，簡(jiǎn)化運(yùn)算過(guò)程，提高機(jī)械臂逆運(yùn)動(dòng)學(xué)求解的運(yùn)算速度。

2 算法實(shí)現(xiàn)

本文以ABB IRB1600機(jī)械臂為例進(jìn)行運(yùn)動(dòng)學(xué)建模，并進(jìn)行運(yùn)動(dòng)學(xué)分析。

2.1 運(yùn)動(dòng)學(xué)建模及正運(yùn)動(dòng)學(xué)分析

ABB IRB1600的平面CAD圖如圖 1所示。自下至上主要由基座、腰部、大臂、小臂、腕部組成。

圖1 ABB-IRB1600機(jī)械臂

由Denavit-Hatenberg法建立連桿坐標(biāo)系進(jìn)行運(yùn)動(dòng)學(xué)建模，給每一個(gè)關(guān)節(jié)指定一個(gè)參考坐標(biāo)系，從基座到第1個(gè)關(guān)節(jié)，再?gòu)牡?個(gè)關(guān)節(jié)到第2個(gè)關(guān)節(jié)直至到最后一個(gè)關(guān)節(jié)的所有變換結(jié)合起來(lái)就得到了機(jī)械臂的總變換矩陣[3]。z軸位于按右手規(guī)則旋轉(zhuǎn)的方向，x軸位于相鄰z軸的公垂線方向，y軸按照笛卡爾坐標(biāo)系右手法則可得，建立好的連桿坐標(biāo)系如圖 2所示。

圖2 機(jī)械臂Denavit-Hatenberg連桿坐標(biāo)系

ABB IRB1600的Denavit-Hatenberg參數(shù)如表1所示。

表1 IRB1600連桿參數(shù)

表中，ai為沿xi軸，從zi移動(dòng)到zi+1的距離；αi為繞xi軸，從zi旋轉(zhuǎn)到zi+1的角度；di為沿zi軸，從xi-1移動(dòng)到xi的距離；θi為繞zi軸，從xi-1旋轉(zhuǎn)到xi的角度。

相鄰兩連桿之間的變換矩陣通式為[1]：

(2)

根據(jù)Denavit-Hatenberg參數(shù)，代入式(2)求得各連桿變換矩陣如下：

其中，sθ1=sinθ1，cθ1=cosθ1，以此類推。

各連桿變換矩陣相乘得總變換矩陣為：

(3)

其中，px=c1(a1-d4s23+a2c2)

(4)

py=s1(a1-d4s23+a2c2)

(5)

pz=d1-d4c23-a2s2

(6)

其中，c1=cosθ1，s1=sinθ1，c23=cos(θ2+θ3)，s23=sin(θ2+θ3)。

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

2.2.1 前三個(gè)關(guān)節(jié)變量的求解

利用位置子矩陣的元素求解。由上式(4)、式(5)聯(lián)立求得：

θ1=arctan2(py，px)

當(dāng)c1≠0時(shí)，把式(4)、式(5)作如下變換：

T1=-d4s23+a2c2

(7)

T3=-d4c23-a2s2

(8)

其中，

式(7)、式(8)兩邊分別平方相加，利用三角函數(shù)關(guān)系式進(jìn)行化簡(jiǎn)可得：

-T1c2+T3s2=M

其中，

所以，可求解θ2：

當(dāng)c1=0時(shí)，聯(lián)立式(5)、式(6)按上述方法求解θ2。

已知θ1，θ2，可聯(lián)立式(7)、式(8)求解θ3，

所以，θ3=arctan2(s23，c23)-θ2

同樣地，當(dāng)c1=0時(shí)，聯(lián)立式(7)、式(8)按上述方法求解。

2.2.2 后三個(gè)關(guān)節(jié)變量的求解

利用旋轉(zhuǎn)子矩陣求解。相鄰兩連桿之間的旋轉(zhuǎn)矩陣通式為[1]：

所以4、5、6連桿之間的旋轉(zhuǎn)變換矩陣為：

其中，

(9)

(10)

(11)

(12)

由上文前3個(gè)關(guān)節(jié)角已求得，又因旋轉(zhuǎn)子矩陣正交，所以可得：

(13)

當(dāng)s5>0時(shí)，θ4=arctan[L(3,3),-L(1,3)]

當(dāng)s5<0時(shí)，θ4=arctan[-L(3,3),L(1,3)]

2.3 奇異點(diǎn)和多解問(wèn)題的處理

2.3.1 奇異點(diǎn)分析

機(jī)械臂的奇異狀態(tài)指的是某種情況下，機(jī)械臂失去了一個(gè)自由度，不按所期望的狀態(tài)運(yùn)動(dòng)。一般來(lái)說(shuō)，機(jī)械臂有兩類奇異點(diǎn)，一類是臂奇異點(diǎn)，一類是腕奇異點(diǎn)[12]。臂奇異點(diǎn)就是腕中心(軸4、5、6的交點(diǎn))正好位于軸1上方，如圖 3所示。腕奇異點(diǎn)就是軸4和軸6處于同一條線上，即軸5角度為0，如圖 4所示。

圖3 腕中心和軸1匯集時(shí)出現(xiàn)臂奇異點(diǎn)

圖4 軸5角度為0時(shí)出現(xiàn)腕奇異點(diǎn)

在運(yùn)動(dòng)學(xué)逆解的過(guò)程中，應(yīng)舍去θ5=0的解。另外，在后續(xù)的軌跡規(guī)劃過(guò)程中，應(yīng)選擇關(guān)節(jié)空間軌跡規(guī)劃的方法而非笛卡爾空間軌跡規(guī)劃的方法，以避免發(fā)生機(jī)構(gòu)的奇異性問(wèn)題[3]。

2.3.2 多解問(wèn)題的處理

對(duì)于機(jī)械臂的逆解過(guò)程，由于解的不唯一性，會(huì)產(chǎn)生8組解，即機(jī)械臂可通過(guò)不同組軸角度獲得相同的機(jī)械臂工具位置和方位。一般來(lái)說(shuō)，解的選取遵循就近原則，利用最接近軸起點(diǎn)位置的軸定位，移動(dòng)機(jī)械臂至設(shè)定位置處[13]。

3 算法驗(yàn)證

3.1 Robotstudio仿真

表2 運(yùn)動(dòng)學(xué)逆解結(jié)果

在RobotStudio中，輸入給定的期望位姿，得到機(jī)械臂各關(guān)節(jié)角如圖 5所示，從上至下分別是θ1～θ6，與算法結(jié)果基本一致，驗(yàn)證了該算法的正確性。并且在Windows Sever 2008 R2，128G RAM的服務(wù)器平臺(tái)上運(yùn)算，該算法實(shí)現(xiàn)的時(shí)間僅為0.057ms，而用一般封閉解法在相同的試驗(yàn)條件下實(shí)現(xiàn)時(shí)間為0.11ms，因此該算法可以更好地滿足實(shí)時(shí)性要求。

圖5 RobotStudio中機(jī)械臂各關(guān)節(jié)角度值

3.2 實(shí)際工況應(yīng)用

上述仿真是針對(duì)單一期望位姿點(diǎn)進(jìn)行的驗(yàn)證，下面將給出一段連續(xù)軌跡曲線進(jìn)行驗(yàn)證。以基于視覺(jué)的汽車輪轂自動(dòng)化打磨工況為例，如圖 6所示，打磨前需提取輪廓軌跡點(diǎn)坐標(biāo)，然后通過(guò)逆運(yùn)動(dòng)學(xué)算法求取機(jī)械臂各關(guān)節(jié)的角度值，交由機(jī)械臂執(zhí)行，實(shí)現(xiàn)打磨動(dòng)作。

按照本文逆運(yùn)動(dòng)學(xué)算法求解機(jī)械臂各關(guān)節(jié)的角度值，傳輸至機(jī)械臂控制器IRC5 Compact，控制機(jī)械臂執(zhí)行各關(guān)節(jié)的目標(biāo)變換角度，得到機(jī)械臂的末端位置，經(jīng)證明實(shí)際加工軌跡與輪轂輪廓吻合，并在MATLAB中繪出連續(xù)軌跡曲線如圖 8所示，與實(shí)際輪轂輪廓曲線段一致，驗(yàn)證了該算法是合理有效的。

圖6 基于視覺(jué)的汽車輪轂自動(dòng)化打磨工況 圖7 工業(yè)相機(jī)采集的汽車輪轂圖

圖8 輪轂加工軌跡結(jié)果圖

4 結(jié)論

針對(duì)符合pieper準(zhǔn)則的一般6自由度串聯(lián)機(jī)械臂[14]，基于封閉解法提出一種改進(jìn)的逆運(yùn)動(dòng)學(xué)解法。利用Denavit-Hatenberg法對(duì)ABB機(jī)械臂運(yùn)動(dòng)學(xué)建模，確定連桿結(jié)構(gòu)參數(shù)，求解機(jī)械臂正運(yùn)動(dòng)學(xué)方程，然后從機(jī)械臂結(jié)構(gòu)設(shè)計(jì)的角度出發(fā)，基于位姿分離思想分別利用位置子矩陣和旋轉(zhuǎn)子矩陣求解6個(gè)關(guān)節(jié)角值，并輔以正交矩陣的性質(zhì)簡(jiǎn)化運(yùn)算。針對(duì)機(jī)械臂的奇異狀態(tài)和多解問(wèn)題進(jìn)行了分析和闡述。最后在RobotStudio中仿真驗(yàn)證了單一期望位姿的求解結(jié)果，在MATLAB中驗(yàn)證了汽車輪轂打磨實(shí)際工況下連續(xù)軌跡點(diǎn)的求解結(jié)果，均證明了該算法的有效性和高效性，對(duì)提高機(jī)械臂的工作效率和實(shí)時(shí)響應(yīng)有著重要的意義。