倍四元數(shù)在6R串聯(lián)機(jī)器人逆解中的應(yīng)用*

葛小川, 鄭飂默,吳純赟,鄭國(guó)利

(1.中國(guó)科學(xué)院大學(xué)，北京 100049；2.中國(guó)科學(xué)院沈陽(yáng)計(jì)算技術(shù)研究所 高檔數(shù)控國(guó)家工程研究中心，沈陽(yáng) 110168；3.沈陽(yáng)高精數(shù)控智能技術(shù)股份有限公司，沈陽(yáng) 110168)

倍四元數(shù)在6R串聯(lián)機(jī)器人逆解中的應(yīng)用*

葛小川1,2, 鄭飂默2,3,吳純赟1,2,鄭國(guó)利3

(1.中國(guó)科學(xué)院大學(xué)，北京 100049；2.中國(guó)科學(xué)院沈陽(yáng)計(jì)算技術(shù)研究所 高檔數(shù)控國(guó)家工程研究中心，沈陽(yáng) 110168；3.沈陽(yáng)高精數(shù)控智能技術(shù)股份有限公司，沈陽(yáng) 110168)

在研究了四元數(shù)性質(zhì)的基礎(chǔ)上，采用倍四元數(shù)求解6R串聯(lián)機(jī)器人的逆運(yùn)動(dòng)學(xué)問(wèn)題。將三維空間的位移表示成四維空間的雙旋轉(zhuǎn)，建立了倍四元數(shù)形式的機(jī)器人運(yùn)動(dòng)學(xué)方程，再通過(guò)消元，構(gòu)造Dixon結(jié)式。最終通過(guò)求解Dixon結(jié)式求解出機(jī)器人16組解析解。最后，使用MatlabRobotics進(jìn)行仿真，新方法能夠求出機(jī)器人逆運(yùn)動(dòng)學(xué)全部解析解，而且速度比D-H方法提升了10%左右。

6關(guān)節(jié)機(jī)器人;逆運(yùn)動(dòng)學(xué);倍四元數(shù);Dixon結(jié)式

0 引言

逆運(yùn)動(dòng)學(xué)求解是機(jī)器人路徑規(guī)劃和控制的基礎(chǔ)。逆運(yùn)動(dòng)學(xué)就是已知末端執(zhí)行器的位姿，求解相應(yīng)的關(guān)節(jié)變量的過(guò)程[1]。它實(shí)質(zhì)是求解非線性方程的過(guò)程，求解過(guò)程復(fù)雜。Primrose首次論證了6R機(jī)器人的逆運(yùn)動(dòng)學(xué)最多有16個(gè)實(shí)根[2]，但是沒有給出一般的求解過(guò)程。Lee和Liang利用14個(gè)逆運(yùn)動(dòng)學(xué)方程和解析方法,首次得到一般6R機(jī)器人的16組逆運(yùn)動(dòng)學(xué)解[3]。Manocha和Canny進(jìn)一步將一元16次方程求根問(wèn)題轉(zhuǎn)化為24階矩陣的特征分解問(wèn)題[4],降低了對(duì)計(jì)算精度的要求。但是，這些方法都涉及到大量的矩陣運(yùn)算，計(jì)算復(fù)雜而且存在奇異性的問(wèn)題，不能滿足實(shí)時(shí)在線控制的需求。

本文使用倍四元數(shù)建立機(jī)器人的運(yùn)動(dòng)學(xué)模型，將三維空間的旋轉(zhuǎn)和平移運(yùn)動(dòng)轉(zhuǎn)化成四維空間的雙旋轉(zhuǎn)，通過(guò)消元構(gòu)建出Dixon結(jié)式，最終求解出機(jī)器人所有的關(guān)節(jié)角。新的逆運(yùn)動(dòng)學(xué)求解的方法消除了傳統(tǒng)方法求解奇異性的問(wèn)題，能夠滿足實(shí)時(shí)在線控制的需求。

1 四元數(shù)及倍四元數(shù)

四元數(shù)由一個(gè)實(shí)數(shù)和向量組成，可以空間旋轉(zhuǎn)運(yùn)動(dòng)。四元數(shù)可以表示成：Q=q0+q1i+q2j+q3k=q0+q，表示繞著q的方向旋轉(zhuǎn)q0弧度。

G=cos(θ/2)+sin(θ/2)(s1i+s2j+s3k)
H=cos(φ/2)+sin(φ/2)(s1i+s2j+s3k)

(1)

2 6R機(jī)器人運(yùn)動(dòng)學(xué)方程倍四元數(shù)表示

2.1 連桿變換的倍四元數(shù)表示

6R機(jī)器人的運(yùn)動(dòng)由6個(gè)連桿的平移和旋轉(zhuǎn)運(yùn)動(dòng)構(gòu)成，四維空間球體半徑足夠大，我們就可以將三維空間的位移表示成四維空間的雙旋轉(zhuǎn)，因此將連桿變換用倍四元數(shù)表示。

如圖1表示使用D-H法建立的相鄰關(guān)節(jié)i-1和i之間的關(guān)系。其中，關(guān)節(jié)i-1繞自身z軸旋轉(zhuǎn)θi，平移si得到新的坐標(biāo)系(i-1)′,新的坐標(biāo)(i-1)′繞x軸旋轉(zhuǎn)αi平移ai得到i坐標(biāo)系。

圖1 相鄰坐標(biāo)變換

得到坐標(biāo)系(i-1)′的過(guò)程用倍四元數(shù)可以表示成：

z(θ,γ)=ζG(θ,γ)+ηH(θ,γ)

(2)

G(θ,γ)和H(θ,γ)分別為：

(3)

其中，γ=d/R

同理由坐標(biāo)系(i-1)′向坐標(biāo)i的轉(zhuǎn)換過(guò)程可以標(biāo)稱為：

X(α,ρ)=ζG(α,ρ)+ηH(α,ρ)

(4)

(5)

(6)

(7)

ρ=a/R

R為四維空間球體的半徑，根據(jù)經(jīng)驗(yàn)公式[7]，R=L/δ1/2,L表示機(jī)器人手臂所能達(dá)到了最遠(yuǎn)距離，δ表示所要求的精度。

2.2 運(yùn)動(dòng)學(xué)方程的倍四元數(shù)表示

根據(jù)具有n個(gè)連桿機(jī)器人的運(yùn)動(dòng)學(xué)方程[8]的倍四元數(shù)表示可以得到6R機(jī)器人的一般的情況：

(8)

由于倍四元數(shù)中G和H可以分開計(jì)算，公式(9)也可以寫成：

G=G(θ1,γ1)G(α1,ρ1)G(θ2,γ2)G(α2,ρ2)…
G(θ5,γ5)G(α5,ρ5)G(θ6,γ6)G(α6,ρ6)

(10)

H=H(θ1,γ1)H(α1,ρ1)H(θ2,γ2)H(α2,ρ2)…
H(θ5,γ5)H(α5,ρ5)H(θ6,γ6)H(α6,ρ6)

(11)

3 PUMA機(jī)器人倍四元數(shù)逆運(yùn)動(dòng)學(xué)分析

PUMA機(jī)器人的機(jī)械手臂構(gòu)成一個(gè)開式運(yùn)動(dòng)鏈。它由一系列連桿通過(guò)轉(zhuǎn)動(dòng)關(guān)節(jié)串聯(lián)而成，關(guān)節(jié)的相對(duì)轉(zhuǎn)動(dòng)帶動(dòng)連桿運(yùn)動(dòng)[9]。PUMA機(jī)器人由6個(gè)連桿組成，是一個(gè)典型的6自由度的串聯(lián)關(guān)節(jié)機(jī)器人，它的關(guān)節(jié)結(jié)構(gòu)如圖2所示。D-H參數(shù)能夠很好的描述PUMA機(jī)器人的各關(guān)節(jié)之間的幾何關(guān)系，機(jī)器人逆運(yùn)動(dòng)學(xué)實(shí)質(zhì)上就是通過(guò)運(yùn)動(dòng)學(xué)方程反解出相鄰關(guān)節(jié)關(guān)節(jié)角θi的過(guò)程。PUMA機(jī)器人的D-H參數(shù)如表1所示。

圖2 PUMA機(jī)器人關(guān)節(jié)結(jié)構(gòu)簡(jiǎn)圖

關(guān)節(jié)iθiαiaidi關(guān)節(jié)范圍190-9000-160～160200αid2-225～453909000-45～22540-900d4-110～170509000-100～1006000d6-226～226

3.1 運(yùn)動(dòng)學(xué)建模

由公式(15)和(16)得到6自由度串聯(lián)機(jī)器人的運(yùn)動(dòng)學(xué)方程，令：

(12)

(13)

(14)

(15)

3.2 求解

6R機(jī)器人的運(yùn)動(dòng)學(xué)可以表示成公式(10)和公式(11)分離變量得：

(16)

(17)

對(duì)以上的式子進(jìn)行計(jì)算，對(duì)應(yīng)的項(xiàng)相等得到8個(gè)式子：

bi1y1+bi2y2+bi3y3+bi4y4

(18)

(19)

式中,ci為機(jī)械人結(jié)構(gòu)所決定的已知參數(shù)。

消去θ3,θ4,θ5，將式(19)除以k

ci1+ci2u+ci3v+ci4uv+ci5w+ci6uw+

ci7vw+ci8uvw+ci9x+ci10ux+ci11vx+ci12uvx+

ci13wx+ci14uwx+ci15vwx+ci16uvwx=0

(20)

為了消去v,w,x設(shè)u為常數(shù)，定義方程：

(21)

根據(jù)文獻(xiàn)[11]使用Dixon結(jié)式進(jìn)行消元，首先構(gòu)造如下方程：

(22)

展開然后以矩陣的形式重寫為：

(α2β,αβ,β,α2,α,1)D6×6(x2w,xw,v,x2,x,1)=0

(23)

3.3 求解過(guò)程

(1)求解θ2

矩陣D6×6的第2列和第5列的每一個(gè)元素都能寫成如下形式：

a+bu+cu2+du3+eu4

(24)

其中a,b,c,d為系數(shù)，且發(fā)現(xiàn)有b=d,a+e=c成立。因此(24)可以化簡(jiǎn)為：

(u2+1)(ew2+bw+a)

(25)

這樣第2和第5列可以提取出(u2+1)并把它刪除掉。

然后把矩陣D6×6的第3列減去第1列、第6列減去第4列，重新得到的第3列和第6列同樣具有式(24)的形式且系數(shù)滿足相同的關(guān)系。因此，同樣把兩列分別提取(u2+1)并把它刪除掉。經(jīng)過(guò)以上處理新的矩陣D6×6的每一列關(guān)于u的最高次數(shù)分別為4,2,2,4,2,2。

θ2=2arctanu

(26)

得到θ2的值。

(2)求解θ3,θ4,θ5

將u帶入式子：

D6×6(x2w,xw,w,x2,x,1)=0

(27)

得到6個(gè)關(guān)于x2w,xw,w,x2,x的方程。從中任取5個(gè)方程，把x2w,xw,w,x2,x當(dāng)做該線性方程組的5個(gè)未知數(shù)，求解出w和x。

然后把w和x帶入到(20)中的任意一個(gè)可以得到v。

再用θ3=2arctanv,θ4=2arctanw,θ5=2arctanx求解出θ3,θ4,θ5。

(3)求解θ1,θ6

把θ2,θ3,θ4,θ5帶入到(18)中可以計(jì)算得到y(tǒng)1,y2,y4。

4 實(shí)例

這里采用文獻(xiàn)[12]的數(shù)據(jù)對(duì)上述的求解過(guò)程進(jìn)行對(duì)比驗(yàn)證。6R機(jī)器人的結(jié)構(gòu)參數(shù)和位置參數(shù)為：

通過(guò)正向運(yùn)動(dòng)學(xué)可以計(jì)算出機(jī)械手臂的末端位姿的矩陣表示為：

使用倍四元數(shù)表示運(yùn)動(dòng)學(xué)方程：

通過(guò)以上計(jì)算過(guò)程使用Matlab，最終得到16組關(guān)節(jié)角。反解結(jié)果中2組為實(shí)數(shù)解。第1組解和初值是一樣的。把表中其余結(jié)果正向運(yùn)動(dòng)學(xué)方程進(jìn)行正解驗(yàn)算，結(jié)果是正確的。

為了驗(yàn)證倍四元數(shù)方法對(duì)于速度上的提升，我們構(gòu)造10組關(guān)節(jié)角(di,ai,αi同上)，分別使用D-H方法和本文提出的方法，計(jì)算出末端位姿，然后再反向求出關(guān)節(jié)角，兩種方法的計(jì)算時(shí)間如圖3所示。倍四元數(shù)的方法較D-H法速度上提高了10%左右。

圖3 倍四元數(shù)方法和D-H方法求解速度比較

5 結(jié)論

本文針對(duì)傳統(tǒng)D-H方法求解機(jī)器人逆運(yùn)動(dòng)學(xué)求解復(fù)雜，存在奇異解的問(wèn)題，提出采用倍四元數(shù)表示6R機(jī)器人，并應(yīng)用于PUMA機(jī)器人運(yùn)動(dòng)逆解中，通過(guò)消元和構(gòu)造Dixon結(jié)式最終獲得PUMA機(jī)器人全部的解析解。實(shí)驗(yàn)表明新方法計(jì)算過(guò)程簡(jiǎn)單，速度較D-H方法有很大程度的提高。該方法有存在一定的問(wèn)題，比如，該方法是夠適合用于機(jī)器人參數(shù)辨識(shí)有待驗(yàn)證，速度提升不是特別的明顯等。

[1] 龔星如.六自由度工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)標(biāo)定的研究[D].南京:南京航空航天大學(xué),2012.

[2] 張忠海, 李端玲.串聯(lián)機(jī)構(gòu)運(yùn)動(dòng)學(xué)反解的D-H四元數(shù)方法[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào), 2014, 45(3):299-304.

[3] 倪振松, 廖啟征, 吳莘馨.基于四元數(shù)矩陣與Groebner基的6R機(jī)器人運(yùn)動(dòng)學(xué)逆解算法[J]. 清華大學(xué)學(xué)報(bào)(自然科學(xué)版), 2013,53(5): 683-687.

[4] 宋婧, 方康玲.基于倍四元數(shù)的機(jī)器人逆運(yùn)動(dòng)學(xué)分析[J]. 機(jī)械與電子, 2010(12): 67-69.

[5]NielsenJ,BRoth.OntheKinematicAnalysisofRoboticMechanisms[J].InternationalJournalofRoboticsResearch, 1999,18(18): 1147-1160.

[6] 方俊珊, 劉華山,孫培德.基于分塊矩陣的6R機(jī)器人實(shí)時(shí)逆解算法[J]. 燕山大學(xué)學(xué)報(bào), 2012,36(2): 119-125.

[7]PereiraVF,etal,AQualitative-ProbabilisticApproachtoAutonomousMobileRobotSelfLocalisationandSelfVisionCalibration[C].2013BrazilianConferenceonIntelligentSystems(Bracis), 2013: 157-162.

[8] 陳愛,馮桑,何春,等.基于四元數(shù)的6R串聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)逆解應(yīng)用[J]. 機(jī)電工程技術(shù), 2013,42(9): 5-10.

[9] 董明曉, 周以齊, 張明勤.PUMA機(jī)器人逆運(yùn)動(dòng)學(xué)求解新方法[J]. 組合機(jī)床與自動(dòng)化加工技術(shù), 2000(10):21-23.[10] 喬曙光.6自由度串聯(lián)機(jī)械手位置逆解新方法[D].北京:北京郵電大學(xué),2008.

[11] 喬曙光, 廖啟征, 黃昔光.倍四元數(shù)及其在串聯(lián)機(jī)構(gòu)運(yùn)動(dòng)分析中的應(yīng)用[J]. 機(jī)械設(shè)計(jì)與制造, 2008(2):36-38.

[12]HustyML,MPfurner,HPSchrocker.Anewandefficientalgorithmfortheinversekinematicsofageneralserial6Rmanipulator[J].MechanismandMachineTheory, 2007,42(1): 66-81.

(編輯 李秀敏)

Application of Double Quaternion in the Inverse Kinematics of 6 R Serial Robots

GEXiao-chuan1,2,ZHENGLiao-mo2,3,WUChun-yun1,2,ZHENGGuo-li3

(1.UniversityofChineseAcademyofSciences,Beijing100049，China；2.NationalEngineeringResearchCenterForHigh-EndCNC,ShenyangInstituteofComputingTechnology,ChineseAcademyofSciences,Shenyang110168,China)

Theinversekinematicsofageneral6Rserialrobotshasmuchdifficultytobesolvedandamountofcomputations.Also,itexistssingularsolution.Weusesdoublequaterniontodealwiththeproblem.Thedisplacementinthreedimensionscanbeconvertedtodoublerotation,wecanbuildkinematicsequationofrobotsintheformofdoublequaternion,theneliminateandbuildDixonresultantmatrixandfinallywegettheallthe16closed-formsolutions.Atlast,weverifyournewmeansusingmathroboticstoolbox.Wegetallthesolutionscorrectlyandimproveabouttenpercentcomparingtod-hmeansinspeed.

6Rrobot;inversekinematics;doublequaternion;Dixonresultant

1001-2265(2016)12-0016-04DOI:10.13462/j.cnki.mmtamt.2016.12.005

2016-02-26；

2016-03-31

國(guó)家科技重大專項(xiàng)：國(guó)產(chǎn)高檔數(shù)控機(jī)床、系統(tǒng)及其技術(shù)在航空領(lǐng)域的綜合應(yīng)用驗(yàn)證及工藝研究(2014ZX04001051)

葛小川(1990—)，男，河南商丘人，中國(guó)科學(xué)院大學(xué)碩士研究生，研究方向?yàn)闄C(jī)器人機(jī)構(gòu)學(xué)，(E-mail)gexiaochuan122@126.com。

TH165;TG

A