六自由度機(jī)械臂運(yùn)動(dòng)學(xué)旋量逆解及簡(jiǎn)化算法*

劉國(guó)平，楊先永，鐘飛飛，王大海

(南昌大學(xué)機(jī)電工程學(xué)院，南昌 330031)

0 引言

機(jī)器人的運(yùn)動(dòng)學(xué)是機(jī)器人控制的基礎(chǔ)，其包括正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)兩個(gè)部分；當(dāng)前國(guó)內(nèi)外運(yùn)動(dòng)學(xué)研究主要采用D-H參數(shù)法和旋量法[1-2]。相比于傳統(tǒng)的D-H參數(shù)法，旋量法具有不需要建立局部坐標(biāo)系因此避開了局部坐標(biāo)系帶來(lái)的奇異性問(wèn)題，并且能夠很好的區(qū)分多解和具有明顯的幾何意義等優(yōu)點(diǎn)[3-5]，所以越來(lái)越多的學(xué)者將旋量引用到機(jī)器人的研究中。

文獻(xiàn)[6]將paden-kahan子問(wèn)題和消元法相結(jié)合解決了后3關(guān)節(jié)交于一點(diǎn)的六自由度逆解問(wèn)題。文獻(xiàn)[7]將吳方法(吳方法是我國(guó)數(shù)學(xué)家吳文俊于20世紀(jì)70年代提出的處理多項(xiàng)式代數(shù)問(wèn)題的一種數(shù)學(xué)方法)引入6R機(jī)器人的逆運(yùn)動(dòng)學(xué)問(wèn)題中來(lái)，解決旋量算法子問(wèn)題的限制。文獻(xiàn)[8]將希爾維斯特(sylvester)結(jié)式法與旋量理論相結(jié)合來(lái)求取6自由度機(jī)器人的逆解，擺脫了對(duì)paden-kahan子問(wèn)題的依賴性。文獻(xiàn)[9]針對(duì)前3關(guān)節(jié)軸線均不相交，后3關(guān)節(jié)相交于一點(diǎn)的六自由度機(jī)器人，提出了幾何法和旋量法相結(jié)合的運(yùn)動(dòng)學(xué)逆解求解算法。文獻(xiàn)[10]針對(duì)前3關(guān)節(jié)不相交，后3關(guān)節(jié)相交構(gòu)型的“錢江一號(hào)”機(jī)器人提出了一種新的運(yùn)動(dòng)學(xué)逆解子問(wèn)題算法。文獻(xiàn)[11]針對(duì)工業(yè)機(jī)器人提出了一類子問(wèn)題，并以IRB1400弧焊機(jī)器人為例驗(yàn)證算法的正確性。文獻(xiàn)[12]提出了新的旋量子問(wèn)題改進(jìn)一類前3關(guān)節(jié)均不相交的六自由度運(yùn)動(dòng)學(xué)逆解求解方法。

綜合上述文獻(xiàn)，當(dāng)前旋量理論在逆運(yùn)動(dòng)學(xué)求解上的研究還存在以下問(wèn)題：①多數(shù)研究主要針對(duì)后3關(guān)節(jié)交于一點(diǎn)構(gòu)型的六自由度機(jī)械臂，缺少旋量理論解決前3關(guān)節(jié)交于一點(diǎn)的六自由度機(jī)械臂逆解研究。②僅僅依靠傳統(tǒng)的旋量理論和panden-kahan子問(wèn)題來(lái)求取逆解，其過(guò)程和結(jié)果還不夠簡(jiǎn)潔。

本文針對(duì)前3關(guān)節(jié)交于一點(diǎn)的六自由度機(jī)械臂提出一種旋量逆解算法，并采用幾何法及歐拉角對(duì)算法進(jìn)行簡(jiǎn)化改進(jìn)。

1 基于旋量理論的運(yùn)動(dòng)學(xué)模型

1.1 旋量描述剛體運(yùn)動(dòng)

歐拉定理說(shuō)明，任何一個(gè)剛體的旋轉(zhuǎn)運(yùn)動(dòng)都可以用一個(gè)旋轉(zhuǎn)矩陣R來(lái)描述，該旋轉(zhuǎn)矩陣R可以表示成轉(zhuǎn)角θ和轉(zhuǎn)軸的單位矢量ω的函數(shù)：

(1)

(2)

根據(jù)Chasles定理，任何剛體運(yùn)動(dòng)都可以拆分為沿著某直線的移動(dòng)和旋轉(zhuǎn)來(lái)表示，可以用如下的指數(shù)積形式來(lái)表示。

(3)

當(dāng)工具坐標(biāo)系{T}相對(duì)于參考坐標(biāo)系{S}的初始位形為gST(0)時(shí)，繞某軸螺旋運(yùn)動(dòng)的剛體，{T}相對(duì)于{S}的位姿可以表示為gST(θ)：

(4)

1.2 機(jī)械臂正運(yùn)動(dòng)學(xué)建模

圖1和圖2分別為六自由度機(jī)械臂的三維模型圖和結(jié)構(gòu)示意圖，其6個(gè)關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)，前3關(guān)節(jié)和5、6關(guān)節(jié)軸線分別交于一點(diǎn)，第1和第2關(guān)節(jié)、第2和第3關(guān)節(jié)、第5和第6關(guān)節(jié)始終垂直，第4和第5關(guān)節(jié)異面垂直。

圖1 三維模型圖

圖2 機(jī)械臂結(jié)構(gòu)示意圖

選取初始位形如圖2所示，此時(shí)第1與第3關(guān)節(jié)垂直。在此初始位形下末端工具坐標(biāo)系{T}在慣性坐標(biāo)系{S}中的變換為：

(5)

根據(jù)POE公式，可以得到六自由度機(jī)械臂的正運(yùn)動(dòng)方程：

(6)

2 機(jī)械臂逆運(yùn)動(dòng)學(xué)建模及簡(jiǎn)化算法

2.1 基于panden-kahan子問(wèn)題的逆解

機(jī)械臂的運(yùn)動(dòng)學(xué)逆解是已知末端的期望位置gST(θ)，求出該位置下對(duì)應(yīng)的各關(guān)節(jié)角度。旋量逆解的基本思想就是將復(fù)雜的機(jī)器人運(yùn)動(dòng)學(xué)逆解，拆分成多個(gè)具有幾何意義明確的逆解子問(wèn)題，當(dāng)前公認(rèn)的3個(gè)最基本子問(wèn)題為paden-kahan的3個(gè)子問(wèn)題[13]。

該六自由度機(jī)械臂是前3關(guān)節(jié)和5、6軸線分別交于一點(diǎn)，第4關(guān)節(jié)偏移中心軸線一定距離并與第5關(guān)節(jié)異面垂直；這樣的結(jié)構(gòu)不能做簡(jiǎn)單的拆分，所以不能直接利用paden-kahan的子問(wèn)題求得逆解；這里我們需要利用前3關(guān)節(jié)和5、6關(guān)節(jié)分別相交一點(diǎn)的條件來(lái)消去其中的部分變量，才能將其轉(zhuǎn)化成相應(yīng)的子問(wèn)題，該結(jié)構(gòu)機(jī)械臂的旋量運(yùn)動(dòng)學(xué)逆解具體求解如下。

(1)求解θ4

因?yàn)?、2、3關(guān)節(jié)和5、6關(guān)節(jié)軸線分別相交于一點(diǎn)，利用這一條件我們可以將除θ4以外的變量消去，這樣可以優(yōu)先求得θ4。

記機(jī)械臂的前3關(guān)節(jié)交于點(diǎn)p，p=q2=q3，第5和第6關(guān)節(jié)交于點(diǎn)q5。

(7)

第5、6關(guān)節(jié)軸線交于q5，轉(zhuǎn)動(dòng)后位置不變，

(8)

式(7)兩端同時(shí)右乘q5有：

(9)

前3關(guān)節(jié)交于點(diǎn)p，有：

(10)

上兩式相減有：

(11)

在剛體旋轉(zhuǎn)的過(guò)程中，其上兩點(diǎn)之間的距離始終保持不變，并對(duì)上式兩端取范數(shù)：

(12)

那么θ4求解變成旋轉(zhuǎn)到指定距離的子問(wèn)題3。

(13)

(2)求解θ5、θ6

由式(7)可得：

(14)

(15)

所以有：

(16)

以上問(wèn)題轉(zhuǎn)變成繞兩相交軸旋轉(zhuǎn)的子問(wèn)題2。

(17)

可以得到：

(18)

以上結(jié)果可以應(yīng)用子問(wèn)題1求得θ5、θ6：

(19)

(3)求解θ1、θ2，

(20)

以上結(jié)果可以應(yīng)用子問(wèn)題2進(jìn)行求得θ1、θ2，

(21)

可以得到：

(22)

以上結(jié)果可以應(yīng)用子問(wèn)題1求得θ1、θ2，

(23)

(4)求解θ3，

由式(7)可得下式，并記已知量為g3：

(24)

(25)

以上結(jié)果可以應(yīng)用子問(wèn)題1求解得θ3：

(26)

至此，已經(jīng)求解得到基于旋量六自由度機(jī)械臂的所有運(yùn)動(dòng)學(xué)封閉逆解。其中關(guān)節(jié)4有2組解，關(guān)節(jié)5與關(guān)節(jié)6組合有2組解，關(guān)節(jié)1與關(guān)節(jié)2組合也有2組解，所以該構(gòu)型的機(jī)械臂共有8組解。

2.2 結(jié)合幾何法及歐拉角簡(jiǎn)化前4關(guān)節(jié)角求解

2.2.1 幾何法簡(jiǎn)化θ4的求解

圖3 第4關(guān)節(jié)幾何關(guān)系圖

LBD2=LBC2+LCD2-2LBCLCDcos(π-β)

(27)

(28)

2.2.2 歐拉角簡(jiǎn)化θ1,θ2,θ3的求解

在求得θ4,θ5,θ6的基礎(chǔ)上，可以通過(guò)以下關(guān)系求解θ1,θ2,θ3。

(29)

由于前3關(guān)節(jié)交于一點(diǎn)，第1關(guān)節(jié)與第2關(guān)節(jié)、第2關(guān)節(jié)與第3關(guān)節(jié)始終垂直，在初始位形下3個(gè)關(guān)節(jié)兩兩垂直，且3個(gè)軸都是純旋轉(zhuǎn)關(guān)節(jié)，在工具坐標(biāo)系{S}下，θ1、θ2、θ3分別依次繞工具坐標(biāo)系{S}的Z軸、X軸、Y軸轉(zhuǎn)動(dòng)后的結(jié)果，可以等價(jià)為繞Z-X-Y順序的歐拉角變換結(jié)果：

(30)

其中，c=cos,s=sin,1=θ1,2=θ2,3=θ3。

由式(30)、式(31)，通過(guò)以下關(guān)系可以求得θ1,θ2,θ3。

(31)

如果利用旋量理論和幾何法及歐拉角相結(jié)合來(lái)求解機(jī)械臂逆解，從上面可以看到，運(yùn)動(dòng)學(xué)逆解的求解過(guò)程和求解結(jié)果將得到很大簡(jiǎn)化。

3 逆運(yùn)動(dòng)學(xué)實(shí)例驗(yàn)證

六自由度機(jī)械臂的各個(gè)關(guān)節(jié)桿件參數(shù)如下：

d1=129.5 mm，l1=260.5 mm，a1=44 mm，l2=256.12 mm，l3=0 mm。

(1)任取一組不在奇異位形上的一組關(guān)節(jié)角，[θ1θ2…θ6]=[-66°55°-13°25°4°-10°]，將其帶入機(jī)械臂的正解公式中去，得到該組關(guān)節(jié)角對(duì)應(yīng)末端姿態(tài)矩陣如下：

(32)

(2)利用前面推導(dǎo)的旋量逆解算法進(jìn)行求解，得到機(jī)械臂各組關(guān)節(jié)角如表1所示，各組逆解位形如圖4所示。

表1 8組運(yùn)動(dòng)學(xué)逆解

續(xù)表

(a) 第1、2組解位形 (b) 第3、4組解位形

(c) 第5、6組解位形 (d) 第7、8組解位形圖4 8組逆解位形圖

表1的結(jié)果與前面分析一致，一共8組解，從上表可以看出，第8組解與我們?nèi)《ǖ年P(guān)節(jié)角一模一樣，1和2、3和4、5和6、7和8分別對(duì)應(yīng)同一位形，8組解總共對(duì)應(yīng)4種位形。

(3)再將以上8組解的結(jié)果再次帶入機(jī)械臂的正運(yùn)動(dòng)學(xué)公式中去，得到各組解對(duì)應(yīng)的末端姿態(tài)矩陣，可以看到以上8組解對(duì)應(yīng)著幾乎相同的末端姿態(tài)矩陣，這里再一次證明該算法的正確性；將得到的8組解對(duì)應(yīng)的末端矩陣與給定角度對(duì)應(yīng)的末端矩陣做差，結(jié)果看作各組解的誤差，其中第4組解的誤差最大，為：

(33)

最大誤差在10-13數(shù)量級(jí)，該算法具有非常高的精度。實(shí)際應(yīng)用中，需要對(duì)求得的多組解進(jìn)行篩選，要求各個(gè)關(guān)節(jié)要在機(jī)械臂的運(yùn)動(dòng)范圍內(nèi)、避開機(jī)械臂的各個(gè)奇異位形和工作范圍內(nèi)的障礙物等，再根據(jù)運(yùn)動(dòng)行程最短等尋優(yōu)條件去選擇一組最優(yōu)解。

(4)同樣的，取式θ5=4°,θ6=-10°和式(32)的末端姿態(tài)，帶入簡(jiǎn)化算法中可以得到：[θ1θ2θ3θ4]=[-66°55°-13°24.992 2°]，該結(jié)果與給定關(guān)節(jié)角幾乎一樣，驗(yàn)證了簡(jiǎn)化算法的正確性。

4 結(jié)束語(yǔ)

(1)結(jié)合旋量理論和已知的paden-kahan子問(wèn)題，推導(dǎo)出前3關(guān)節(jié)、第5、6關(guān)節(jié)分別相交于一點(diǎn)，第4關(guān)節(jié)偏移的六自由度機(jī)械臂運(yùn)動(dòng)學(xué)逆解。該方法相對(duì)于傳統(tǒng)的D-H法，其幾何意義明顯，過(guò)程和結(jié)果都更加簡(jiǎn)潔，并且算法精度能夠達(dá)到10-13數(shù)量級(jí)，有非常高的精度，能夠很好的滿足各種應(yīng)用的控制需求。

(2)采用歐拉角及幾何法對(duì)前3關(guān)節(jié)和第4關(guān)節(jié)運(yùn)動(dòng)學(xué)逆解算法進(jìn)行簡(jiǎn)化改進(jìn)，應(yīng)用旋量理論、歐拉角和幾何法相結(jié)合求取運(yùn)動(dòng)學(xué)逆解，能夠極大的簡(jiǎn)化過(guò)程和結(jié)果。

(3)本文提出的方法適用于其他類似構(gòu)型，特別是前3關(guān)節(jié)交于一點(diǎn)構(gòu)型機(jī)械臂的運(yùn)動(dòng)學(xué)逆解，具有一定通用性。