陳禹含，韓寶玲*，王善達(dá)，許仕杰，劉 楊

(1.北京理工大學(xué)機(jī)械與車(chē)輛學(xué)院，北京 100081；2.北京理工大學(xué)機(jī)電學(xué)院，北京 100081)

實(shí)現(xiàn)軌跡規(guī)劃[1]、運(yùn)動(dòng)控制[2]等典型機(jī)器人功能的前提是解決逆運(yùn)動(dòng)學(xué)問(wèn)題。該問(wèn)題是已知末端執(zhí)行器位姿，求解關(guān)節(jié)變量值。因?yàn)槟孢\(yùn)動(dòng)學(xué)求解存在非線(xiàn)性和多解的問(wèn)題，所以很難找到一種適用于不同構(gòu)型機(jī)器人的逆運(yùn)動(dòng)學(xué)解析的求解算法[3]。

對(duì)機(jī)器人運(yùn)動(dòng)學(xué)的描述方法有Denavit-Hartenberg參數(shù)法[4]和指數(shù)積方法[5]。傘紅軍等[6]、寧祎等[7]和張震等[8]利用前者的描述方法對(duì)機(jī)器人進(jìn)行建模并完成逆運(yùn)動(dòng)學(xué)求解。但是該描述方法存在描述參數(shù)復(fù)雜且模型表述不唯一等缺點(diǎn)。而后者描述方法具有建模過(guò)程簡(jiǎn)單、描述參數(shù)少等優(yōu)點(diǎn)，且該方法在進(jìn)行逆運(yùn)動(dòng)學(xué)求解時(shí)具有明確的幾何意義，所以指數(shù)積方法越來(lái)越受到歡迎。趙榮波等[9]及敖天翔等[10]利用旋量理論和代數(shù)消元相結(jié)合提出了6-DOF機(jī)器人逆解算法，該方法適用于末端3個(gè)關(guān)節(jié)軸線(xiàn)相交于一點(diǎn)的串聯(lián)機(jī)器人。李立君等[11]基于旋量理論對(duì)6-DOF混聯(lián)機(jī)械臂進(jìn)行逆運(yùn)動(dòng)學(xué)求解，該方法可以穩(wěn)定得出10組工作目標(biāo)位置信息對(duì)應(yīng)的關(guān)節(jié)值。盧喆等[12]將幾何法與旋量理論相結(jié)合推導(dǎo)6-DOF串聯(lián)機(jī)器人逆運(yùn)動(dòng)學(xué)，該方法是采用幾何法解決前3個(gè)關(guān)節(jié)軸線(xiàn)不相交的問(wèn)題，采用Paden-Kahan子問(wèn)題的方法求解后3個(gè)關(guān)節(jié)軸線(xiàn)交于一點(diǎn)的問(wèn)題。Sung等[13]和Wang等[14]將3類(lèi)Paden-Kahan子問(wèn)題不同組合，設(shè)計(jì)出幾種新的逆運(yùn)動(dòng)學(xué)子問(wèn)題，并利用旋量理論進(jìn)行分析求解，該結(jié)果具有一定的通用性。

目前各位學(xué)者的求解逆運(yùn)動(dòng)學(xué)的方法大多是適用于末端3個(gè)關(guān)節(jié)軸線(xiàn)相交于一點(diǎn)的構(gòu)型，該構(gòu)型常用于工業(yè)機(jī)械臂中，但是另一種滿(mǎn)足Pieper準(zhǔn)則[3]的構(gòu)型，即具有相互平行的3個(gè)相鄰關(guān)節(jié)軸的機(jī)械臂，利用旋量理論求解該逆運(yùn)動(dòng)學(xué)卻少有研究。針對(duì)這一問(wèn)題，現(xiàn)使用旋量理論與代數(shù)相結(jié)合的方法求解具有相互平行的3個(gè)相鄰關(guān)節(jié)軸的串聯(lián)式機(jī)械臂逆運(yùn)動(dòng)學(xué)問(wèn)題。

1 預(yù)備知識(shí)

1.1 旋量理論數(shù)學(xué)基礎(chǔ)

剛體運(yùn)動(dòng)可由4×4的齊次變換矩陣T表示，該變換矩陣由旋轉(zhuǎn)變換R和平移變換P組成。根據(jù)Chasles定理[15]，任意剛體移動(dòng)都可以表示為繞某一轉(zhuǎn)軸的旋轉(zhuǎn)和沿著該軸平移的組合運(yùn)動(dòng)，該運(yùn)動(dòng)稱(chēng)為螺旋運(yùn)動(dòng)，且其無(wú)窮小量為運(yùn)動(dòng)旋量[9]。

運(yùn)動(dòng)旋量Si由旋轉(zhuǎn)角速度ωi和平移線(xiàn)速度vi組成

(1)

式(1)中：vi可以表示為

vi=-wi×qi

(2)

式(2)中：qi為Si轉(zhuǎn)軸上的任意一點(diǎn)。Si的矩陣形式為

(3)

式(3)中：[wi]為wi的斜對(duì)稱(chēng)矩陣；0為1×3維零向量。

基于旋量理論，計(jì)算n自由度串聯(lián)式機(jī)械臂末端執(zhí)行器位姿Te的公式[5]可表示為

Te(Θ)=e[S1]θ1…e[Sn-1]θn-1e[Sn]θnM

(4)

式(4)中：M為末端執(zhí)行器的初始位姿；θi為機(jī)械臂各關(guān)節(jié)角度；Θ為由機(jī)械臂各關(guān)節(jié)角度組成的向量，表達(dá)式為

(5)

[Si]θi的指數(shù)形式可以表示為

(6)

式(6)中：0為1×3的零向量；Ri(θi)和Pi(θi)的表達(dá)式為

Ri(θi)=e[wi]θi=

I+[wi]sinθi+[wi]2(1-cosθi)

(7)

Pi(θi)=[(I+[wi]2)θi+[wi]×

(1-cosθi)-[wi]2sinθi]vi

(8)

式中：I為3×3的單位矩陣。

1.2 Paden-Kahan 子問(wèn)題

Paden-Kahan子問(wèn)題是基于旋量理論的指數(shù)積公式，用幾何方法求出繞軸的旋轉(zhuǎn)角度[13]?；谛坷碚摰倪\(yùn)動(dòng)求解可以分為3 類(lèi)Paden-Kahan子問(wèn)題，Murray等[15]對(duì)這3 類(lèi)子問(wèn)題做了詳細(xì)的介紹。這里僅介紹求解過(guò)程中所用到的第1類(lèi)子問(wèn)題。

第1類(lèi)Paden-Kahan子問(wèn)題是關(guān)于單一轉(zhuǎn)軸旋轉(zhuǎn)，如圖1所示。具體過(guò)程是繞運(yùn)動(dòng)旋量S旋轉(zhuǎn)θ角度，使點(diǎn)q1旋轉(zhuǎn)至點(diǎn)q2。S中向量w為單位向量，且代表旋轉(zhuǎn)方向。那么在已知運(yùn)動(dòng)旋量S和點(diǎn)q1、q2坐標(biāo)的前提下，旋轉(zhuǎn)角度θ[15]可以表示為

θ=atan2[w(uq×νq),uq·νq]

(9)

式(9)中：uq和νq分別為向量u和ν在w的法平面的投影。

圖1 第1類(lèi)Paden-Kahan子問(wèn)題Fig.1 The subproblems of the first kind Paden-Kahan

2 6R機(jī)械臂逆運(yùn)動(dòng)學(xué)

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

機(jī)械臂結(jié)構(gòu)參數(shù)和旋量坐標(biāo)系如圖2所示。從圖2可以看出，該機(jī)械臂具有6個(gè)自由度，且均為轉(zhuǎn)動(dòng)關(guān)節(jié)，其中關(guān)節(jié)2、3、4互相平行，這種構(gòu)型滿(mǎn)足 Piper 準(zhǔn)則,所以該機(jī)械臂逆運(yùn)動(dòng)學(xué)具有封閉形式解。

從圖2中可以得出各關(guān)節(jié)方向向量wi和點(diǎn)坐標(biāo)qi分別為

坐標(biāo)系s和坐標(biāo)系b分別為機(jī)械臂的基坐標(biāo)系和末端執(zhí)行器坐標(biāo)系；S1～S6為各轉(zhuǎn)動(dòng)關(guān)節(jié)的運(yùn)動(dòng)旋量；q0為基坐標(biāo)系s的原點(diǎn)；q1～q6為各關(guān)節(jié)旋量上一點(diǎn)，其中點(diǎn)q1、q4和q5分別在S1與S2交點(diǎn)、S4與S5交點(diǎn)、S5與S6交點(diǎn)處，點(diǎn)q6位于坐標(biāo)系b原點(diǎn)處；W1、W2、L1、L2、H1、H2為上述各相鄰點(diǎn)之間距離；xs、ys、zs為坐標(biāo)系s的方向向量；xb、yb、zb為坐標(biāo)系b的方向向量圖2 機(jī)械臂的結(jié)構(gòu)參數(shù)和旋量坐標(biāo)系Fig.2 The configuration parameters and screw coordinate system of the manipulator

(10)

(11)

那么vi可以根據(jù)式(2)計(jì)算得

(12)

為了后文對(duì)點(diǎn)qi進(jìn)行位置變換，定義其次坐標(biāo)形式為

(13)

最后末端執(zhí)行器初始位姿M和根據(jù)式(1)計(jì)算的Si為

(14)

(15)

根據(jù)式(4)，圖2中的六自由度串聯(lián)式機(jī)械臂的正運(yùn)動(dòng)學(xué)可以表示為

Te(Θ)=e[S1]θ1e[S2]θ2e[S3]θ3e[S4]θ4e[S5]θ5e[S6]θ6M

(16)

有了機(jī)械臂運(yùn)動(dòng)學(xué)參數(shù)就可以應(yīng)用旋量理論與代數(shù)相結(jié)合的方法求解上述構(gòu)型機(jī)械臂的逆運(yùn)動(dòng)學(xué)。

2.2 求解θ1

在式(16)的左右兩側(cè)同時(shí)右乘M-1可得

(17)

(18)

式(18)等號(hào)右側(cè)均為已知量，為了便于后續(xù)推導(dǎo)，設(shè)

(19)

(20)

(21)

設(shè)

(22)

有

(23)

解得

(24)

將式(22)和式(19)代入式(18)得

(25)

θ1=atan2[w1(up1×νp1),up1νp1]

(26)

式(26)中：

(27)

2.3 求解θ5和θ6

因?yàn)檫\(yùn)動(dòng)旋量S2、S3和S4相互平行，所以這3個(gè)運(yùn)動(dòng)旋量對(duì)于姿態(tài)的影響可以看做一個(gè)整體，即θ234=θ2+θ3+θ4。由圖2可知，運(yùn)動(dòng)旋量S2～S6帶來(lái)的姿態(tài)變化可以等效為Y-Z-Y歐拉角變換

(28)

式(28)中：c234=cosθ234，s234=sinθ234，c5=cosθ5，s5=sinθ5，c6=cosθ6，s6=sinθ6。該Y-Z-Y歐拉角變換Ryzy可以根據(jù)已知參數(shù)計(jì)算，將式(17)左乘e-[S1]θ1得

e[S2]θ2e[S3]θ3e[S4]θ4e[S5]θ5e[S6]θ6=e-[S1]θ1Ta=

(29)

這里采用式(28)和式(29)對(duì)應(yīng)元素相等的代數(shù)方法求解θ5和θ6。根據(jù)式(28)第2行第2列元素對(duì)應(yīng)相等可以求得θ5，即

θ5=±arccos[Tb(2,2)]

(30)

根據(jù)式(28)第2行第1列和第2行第3列元素對(duì)應(yīng)相等可以求得

(31)

(32)

那么θ6可以被計(jì)算為

1977年起，他參加了國(guó)內(nèi)外舉辦的400多場(chǎng)團(tuán)體展覽，獲得了150多個(gè)獎(jiǎng)項(xiàng)。至今布蘭尼斯拉夫布爾基在國(guó)內(nèi)外已經(jīng)舉辦了31場(chǎng)個(gè)人影展。在國(guó)際攝影藝術(shù)聯(lián)合會(huì)，他擁有MFIAP、HonEFIAP、EFIAP、EFIAP/Silver等榮譽(yù)。在塞爾維亞攝影協(xié)會(huì)，他享有攝影大師的最高藝術(shù)頭銜。自2003年以來(lái)，他一直擔(dān)任塞爾維亞攝影協(xié)會(huì)主席。

θ6=±atan2(s6,c6)

(33)

由于θ5和θ6已知，那么根據(jù)式(28)第1行第1列和第1行第3列元素對(duì)應(yīng)相等可以求得

s234=Tb(1,3)c6-Tb(1,1)s6

(34)

根據(jù)式(28)第3 行第1 列和第3 行第3 列元素對(duì)應(yīng)相等可以求得

c234=Tb(3,3)c6-Tb(3,1)s6

(35)

所以θ234可以被計(jì)算為

θ234=±atan2(s234,c234)

(36)

2.4 求解θ2、θ3和θ4

式(29)右乘e-[S6]θ6e-[S5]θ5得

e[S2]θ2e[S3]θ3e[S4]θ4=Tbe-[S6]θ6e-[S5]θ5=Tc

(37)

(38)

因?yàn)镾2與S3互相平行，點(diǎn)q4的軌跡投影圖如圖3所示。圖中點(diǎn)O2和O3分別為點(diǎn)q2和q3，α、β、θ′2、θ′3分別為各向量間的夾角。向量uq、vq、O32和O23可表示為

(39)

圖3 軌跡投影圖Fig.3 The trajectory projection

根據(jù)第1類(lèi)Paden-Kahan子問(wèn)題的求解公式計(jì)算θ′3和θ′2得

θ′3=atan2[w3(uq×O32),uqO32]

(40)

θ′2=atan2[w2(O23×vq),O23vq]

(41)

根據(jù)三角形余弦定理，角度α和β可以表示為

(42)

(43)

有了上述參數(shù)，θ2和θ3可以計(jì)算為

(44)

最后，θ4可以被表示為

θ4=θ234-θ3-θ2

(45)

3 實(shí)例與驗(yàn)證

UR5協(xié)作機(jī)械臂滿(mǎn)足所述機(jī)器人的構(gòu)型要求，為了驗(yàn)證提出的逆運(yùn)動(dòng)學(xué)求解算法的正確性，將該機(jī)械臂作為實(shí)例進(jìn)行逆運(yùn)動(dòng)學(xué)求解。UR5機(jī)械臂各結(jié)構(gòu)參數(shù)的數(shù)值是W1=0.109 m，W2=0.082 m，L1=0.425 m，L2=0.392 m，H1=0.089 m，H2=0.095 m。具體過(guò)程如下。

(1)任意給定一組關(guān)節(jié)角度。

(46)

(2)根據(jù)正運(yùn)動(dòng)學(xué)計(jì)算公式[式(16)]計(jì)算的末端執(zhí)行器位姿如式(47)所示。

(3)由逆運(yùn)動(dòng)學(xué)計(jì)算公式可以求出逆運(yùn)動(dòng)學(xué)的8組解，如表1所示，該8組解的位姿如圖4所示。

(4)步驟(3)中8組逆解的位姿與步驟(2)中的位姿作差得出誤差矩陣并計(jì)算該矩陣的2范數(shù)。通過(guò)計(jì)算，第7組解對(duì)應(yīng)的位姿誤差矩陣的2范數(shù)最大，為1.072 782 134 37×10-15，該位姿誤差矩陣ΔTmax如式(48)所示。

(47)

表1 逆運(yùn)動(dòng)學(xué)求解的8 組解Table 1 The 8 groups of solutions for inverse kinematics

圖4 逆運(yùn)動(dòng)學(xué)求解的8組解的位姿Fig.4 The poses of 8 sets of solutions solved by inverse kinematics

ΔTmax=Te-Te7≈

(48)

式(48)中：Te7為步驟(3)中第7組逆解對(duì)應(yīng)的位姿。

(5)為了更進(jìn)一步說(shuō)明提出算法的魯棒性和準(zhǔn)確性，將步驟(1)～步驟(4)循環(huán)執(zhí)行1 000次，然后對(duì)1 000組ΔTmax取平均，計(jì)算結(jié)果為

(49)

4 結(jié)論

(1)利用旋量理論與代數(shù)相結(jié)合的方法推導(dǎo)了六自由度且具有相互平行的3個(gè)相鄰關(guān)節(jié)軸的串聯(lián)式機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解公式，該方法具有幾何意義明確的優(yōu)點(diǎn)，并以UR5型號(hào)機(jī)械臂為例進(jìn)行逆運(yùn)動(dòng)學(xué)求解，計(jì)算得出逆解的最大位姿誤差為10-15數(shù)量級(jí)，與文獻(xiàn)[9]相比，其最大位姿誤差為10-12數(shù)量級(jí)，因此，該精度足以驗(yàn)證算法的準(zhǔn)確性和優(yōu)越性。

(2)提出的逆運(yùn)動(dòng)學(xué)求解算法針對(duì)一類(lèi)具有相互平行的3個(gè)相鄰關(guān)節(jié)軸構(gòu)型的機(jī)械臂，該構(gòu)型常見(jiàn)于協(xié)作式機(jī)器人。因此，提出的逆運(yùn)動(dòng)學(xué)算法具有一定通用性。