結(jié)合旋量理論和代數(shù)方法的六自由度機械臂逆運動學(xué)求解算法

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

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

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

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

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

1 預(yù)備知識

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

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

運動旋量Si由旋轉(zhuǎn)角速度ωi和平移線速度vi組成

(1)

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

vi=-wi×qi

(2)

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

(3)

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

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

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

(4)

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

(5)

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

(6)

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

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 子問題

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

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

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

(9)

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

圖1 第1類Paden-Kahan子問題Fig.1 The subproblems of the first kind Paden-Kahan

2 6R機械臂逆運動學(xué)

2.1 運動學(xué)建模

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

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

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

(10)

(11)

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

(12)

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

(13)

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

(14)

(15)

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

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

(16)

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

2.2 求解θ1

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

(17)

(18)

式(18)等號右側(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

因為運動旋量S2、S3和S4相互平行，所以這3個運動旋量對于姿態(tài)的影響可以看做一個整體，即θ234=θ2+θ3+θ4。由圖2可知，運動旋量S2～S6帶來的姿態(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ù)計算，將式(17)左乘e-[S1]θ1得

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

(29)

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

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

(30)

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

(31)

(32)

那么θ6可以被計算為

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

θ6=±atan2(s6,c6)

(33)

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

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

(34)

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

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

(35)

所以θ234可以被計算為

θ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)

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

(39)

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

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

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

(40)

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

(41)

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

(42)

(43)

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

(44)

最后，θ4可以被表示為

θ4=θ234-θ3-θ2

(45)

3 實例與驗證

UR5協(xié)作機械臂滿足所述機器人的構(gòu)型要求，為了驗證提出的逆運動學(xué)求解算法的正確性，將該機械臂作為實例進行逆運動學(xué)求解。UR5機械臂各結(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。具體過程如下。

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

(46)

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

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

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

(47)

表1 逆運動學(xué)求解的8 組解Table 1 The 8 groups of solutions for inverse kinematics

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

ΔTmax=Te-Te7≈

(48)

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

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

(49)

4 結(jié)論

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

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