趙榮波， 施智平，關(guān) 永，邵振洲，王國輝，吳立峰

(首都師范大學(xué) 信息工程學(xué)院，北京 100048)

0 引 言

機(jī)器人逆向運(yùn)動學(xué)通過目標(biāo)位置確定各關(guān)節(jié)角度。目前，常用的機(jī)器人運(yùn)動學(xué)建模方法主要有D-H(Denavit-Hartenberg)參數(shù)法[1]和旋量法[2]。盡管在機(jī)器人運(yùn)動學(xué)建模中，D-H參數(shù)法廣泛使用，但當(dāng)采用該方法對相鄰關(guān)節(jié)平行或幾乎平行的機(jī)器人進(jìn)行運(yùn)動學(xué)標(biāo)定時(shí)，會導(dǎo)致參數(shù)奇異性問題[3～6]。為此，提出了許多其他運(yùn)動學(xué)模型[7～9]。

為了更好地進(jìn)行運(yùn)動學(xué)標(biāo)定，旋量指數(shù)積(product of exponentials，POE)方法廣泛使用[3,6,10,11]。與D-H方法相比，其只需建立慣性和工具2個(gè)坐標(biāo)系，且其運(yùn)動學(xué)參數(shù)變化光滑，克服了運(yùn)動學(xué)標(biāo)定中存在的奇異性問題[6,10]。機(jī)器人逆運(yùn)動學(xué)求解存在多種方法。Kucuk S等人[12,13]采用D-H方法分別對具有歐拉腕和偏移腕的工業(yè)機(jī)器人進(jìn)行逆運(yùn)動學(xué)求解。王憲等人[14]采用迭代法對6R機(jī)器人進(jìn)行逆運(yùn)動學(xué)求解。呂世增等人[15]將吳方法應(yīng)用到機(jī)器人逆運(yùn)動學(xué)的求解中。Fu Z T等人[16]應(yīng)用幾何代數(shù)理論對6R機(jī)器人進(jìn)行運(yùn)動學(xué)建模和方程求解?；趲缀畏椒?，Murray R M等人[2]介紹了3種基本子問題，且Chen I M等人[17]對子問題進(jìn)行擴(kuò)展，并將其應(yīng)用于機(jī)器人逆運(yùn)動學(xué)的求解。然而，有限的子問題不能滿足所有類型機(jī)器人求解。

本文應(yīng)用旋量指數(shù)積法對6R機(jī)器人進(jìn)行運(yùn)動學(xué)建模，采用幾何、代數(shù)法和Paden-Kahan子問題并借助MATLAB處理機(jī)器人逆運(yùn)動學(xué)問題，該方法為機(jī)器人逆運(yùn)動學(xué)的求解提供了參考。

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

一般來說，剛體運(yùn)動可由特殊歐氏群SE(3)中的4×4齊次變換矩陣來表示。該矩陣由3×3旋轉(zhuǎn)矩陣和3×1平移位置矢量兩部分組成。根據(jù)Chasles定理[2]，剛體繞某一軸的旋轉(zhuǎn)和沿著該軸的平移可以完成其從某一位置到另一位置的運(yùn)動，該運(yùn)動稱為螺旋運(yùn)動，且其無窮小量為運(yùn)動旋量。運(yùn)動旋量的2種表示形式如下

(1)

設(shè)gst(0)為剛體相對于慣性坐標(biāo)系的初始位姿，當(dāng)剛體轉(zhuǎn)動或平移θ后，剛體的最終位姿為gst(θ)=egst(0)。

對于只有旋轉(zhuǎn)關(guān)節(jié)的機(jī)器人，當(dāng)給定ξ和θ，則θ的指數(shù)形式可表示為

(2)

(3)

根據(jù)指數(shù)積公式，n自由度串聯(lián)機(jī)器人末端執(zhí)行器位姿可表示為

gst(θ)=ee…egst(0)

(4)

式中g(shù)st(0)和gst(θ)分別為末端執(zhí)行器初始和最終位姿。

2 串聯(lián)機(jī)器人逆運(yùn)動學(xué)

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

機(jī)器人結(jié)構(gòu)參數(shù)和旋量坐標(biāo)系如圖1所示。

圖1 機(jī)器人結(jié)構(gòu)參數(shù)和旋量坐標(biāo)系

可知，機(jī)器人的第2，3關(guān)節(jié)軸線是平行的，均與第1關(guān)節(jié)軸線異面垂直，且機(jī)器人后3關(guān)節(jié)軸線相交于一點(diǎn)，滿足Piper準(zhǔn)則具有封閉形式解。

圖1中，S和T分別為慣性和工具坐標(biāo)系，點(diǎn)o為慣性坐標(biāo)系S的原點(diǎn)。ξ1～ξ6分別為各關(guān)節(jié)的單位運(yùn)動旋量。r1～r3分別為前3關(guān)節(jié)軸線上的1點(diǎn)，r4～r6為后3關(guān)節(jié)軸線交點(diǎn)。各關(guān)節(jié)運(yùn)動旋量計(jì)算為

(5)

由圖1可得機(jī)器人初始位姿為

(6)

6自由度串聯(lián)機(jī)器人正向運(yùn)動學(xué)可由 (POE)表示為

gst(θ)=eeeeeegst(0)

(7)

2.2 逆運(yùn)動學(xué)求解算法

(8)

用點(diǎn)r4右乘式(8)并化簡得

gst1·r4=eeer4=p1=(px,py,pz,1)T

(9)

式(9)表示點(diǎn)r4在前3關(guān)節(jié)中的螺旋運(yùn)動，其對應(yīng)的2種情形如圖2和圖3所示。

圖2 前3關(guān)節(jié)螺旋運(yùn)動情形一

圖3 前3關(guān)節(jié)螺旋運(yùn)動情形二

2.2.1 求解p21和p22

在圖2和圖3中，點(diǎn)o1,o2和o3分別為圓1、圓2和圓3的圓心。p21和p22分別為圓1和圓2的2個(gè)不同交點(diǎn)。由機(jī)器人幾何結(jié)構(gòu)可知，圓1與圓2不能同時(shí)相交于2點(diǎn)。p31，p32，p33和p34分別為圓2和圓3的交點(diǎn)。圖2和圖3中的x，y和z軸分別對應(yīng)于圖1中慣性坐標(biāo)系S的3個(gè)坐標(biāo)軸。需要說明的是，為了便于理解，在圖2和圖3中將o1點(diǎn)與慣性坐標(biāo)系的原點(diǎn)o重合。實(shí)際上，圓1的圓心o1不一定在o點(diǎn)，其可以是關(guān)節(jié)1軸線上的任意一點(diǎn)，但不會影響點(diǎn)的坐標(biāo)的求解。

如圖2和圖3所示，在初始位姿下，首先，點(diǎn)r4通過繞ξ3軸旋轉(zhuǎn)角度θ31(或θ32,θ33,θ34)運(yùn)動到點(diǎn)p31(或p32,p33,p34)，然后，該點(diǎn)通過繞ξ2軸旋轉(zhuǎn)角度θ21(或θ22,θ23,θ24)運(yùn)動到點(diǎn)p21(或p22)。最終，該點(diǎn)通過繞ξ1軸旋轉(zhuǎn)角度θ11(或θ12)運(yùn)動到點(diǎn)p1。點(diǎn)r4和p1為已知，為了能夠簡單、直觀地求出前三關(guān)節(jié)角度變量，則應(yīng)計(jì)算出各個(gè)軌跡圓的交點(diǎn)。計(jì)算過程如下：

設(shè)圓1與圓3分別在平面α，β內(nèi)，由于關(guān)節(jié)2和3的軸線平行，因此圓 2也在平面β內(nèi)。由圖2和圖3可知，ξ1軸和慣性坐標(biāo)系S的z軸是重合的。因o1和p1兩點(diǎn)都在平面α內(nèi)，且慣性坐標(biāo)系S的z軸垂直于平面α,所以o1和p1的z坐標(biāo)相等，均為pz。又因點(diǎn)o1為慣性坐標(biāo)系S的z軸上的一點(diǎn)，所以o1的x，y坐標(biāo)均為0。綜上，o1點(diǎn)坐標(biāo)為(0,0,pz)。

同理，因慣性坐標(biāo)系S的z軸垂直于平面α，且p1,p21和p223點(diǎn)都在平面α內(nèi)，所以p1,p21和p22點(diǎn)的z坐標(biāo)均為pz。此外，p21和p22都在慣性坐標(biāo)系S的y軸上或兩點(diǎn)的連線平行于y軸，且均在平面β內(nèi)，則該2點(diǎn)的x坐標(biāo)都為0。因p1,p21和p223點(diǎn)都在圓1上，則p21和p22的y坐標(biāo)分別為‖o1-p1‖和- ‖o1-p1‖。綜上，可知p21和p22坐標(biāo)為

(10)

2.2.2 求解p31,p32,p33和p34

由圖2和圖3可知，ξ2和ξ3軸線平行于慣性坐標(biāo)系S的x軸，且都垂直于平面β。因點(diǎn)r4,p31,p32,p33,p34,o2和o3都在β內(nèi)，則這些點(diǎn)的x坐標(biāo)相同，均為0。根據(jù)機(jī)器人的特殊結(jié)構(gòu)和所建慣性坐標(biāo)系之間關(guān)系，可確定o2,o3坐標(biāo)為

o2=[0y2z2]T,o3=[0y2z3]T

(11)

為了描述方便，用p2表示交點(diǎn)p21和p22，p3表示交點(diǎn)p31,p32,p33和p34。根據(jù)圖2、圖3特殊幾何關(guān)系建立方程

‖o3-r4‖=‖o3-p3‖,‖o2-p2‖=‖o2-p3‖

(12)

通過求解可知，p3點(diǎn)坐標(biāo)有4個(gè)解，因此，可以得到對應(yīng)p31,p32,p33和p34坐標(biāo)。

2.2.3 求解θ1,θ2和θ3

根據(jù)圖2和圖3，前3關(guān)節(jié)螺旋運(yùn)動表示為

(13)

其由3個(gè)子問題構(gòu)成，根據(jù)Paden-Kahan子問題1[2]，可得前3關(guān)節(jié)角度分別為

(14)

從圖2和圖3中可以看出,前3關(guān)節(jié)角度變量的解共有4種組合：(θ11,θ21,θ31)，(θ11,θ22,θ32)， (θ12,θ23,θ33)和(θ12,θ24,θ34)。

2.2.4 求解θ4,θ5和θ6

用e,e和e分別左乘式(8)，得到

eeegst1=eee=gst2

(15)

選擇一點(diǎn)p6，使得其在ξ6軸線上但不在ξ4和ξ5軸線上，該點(diǎn)坐標(biāo)為p6=(0,b,0,1)T。

用點(diǎn)p6右乘式(15)，則

gst2·p6=ee·p6=(t1,t2,t3,1)T

(16)

應(yīng)用式(2)和式(3)，并借助MATLAB工具化簡得

-c·sinθ4·cosθ5=t1,c·sinθ5+b=t2,

c-c·cosθ4·cosθ5=t3

(17)

采用消元法求解，可得

(18)

式中m6=-t1/cosθ5,m7=(c-t3)/cosθ5(cosθ5≠0)。

同理，用e，e分別左乘式(15)，則

ee·gst2=e=gst3

(19)

選擇不在ξ6軸線上的一點(diǎn)p7，坐標(biāo)p7=(0,b+1,0,1)T。

點(diǎn)p7右乘式(19)有g(shù)st3·p7=e·p7=[t4,t5,t6,1]T。

根據(jù)Paden-Kahan子問題1[2]可得θ6=atan 2(-t4,t5-b)。

至此，已得到6R串聯(lián)機(jī)器人所有可能的逆運(yùn)動學(xué)的解。幾何方法的巧妙運(yùn)用，簡化了運(yùn)算，加深了對多解組合的理解，有助于選擇合適的逆解。

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

“錢江一號”[18]工業(yè)機(jī)器人滿足本文所提出的機(jī)器人的結(jié)構(gòu)。為了驗(yàn)證本文提出的逆解算法，將該機(jī)器人作為實(shí)例進(jìn)行逆運(yùn)動學(xué)求解。驗(yàn)證過程如下：

1)機(jī)器人工作空間內(nèi)，任意給出6個(gè)關(guān)節(jié)角度θ1=100°,θ2=50°,θ3= -60°,θ4=180°,θ5=30°,θ6=-110°。

2)應(yīng)用正向運(yùn)動學(xué)方程求解機(jī)器人位姿

(20)

3)利用本文提出的算法和求出的位姿，計(jì)算各關(guān)節(jié)對應(yīng)角度。共得到8組逆運(yùn)動學(xué)的解，如表1所示?？梢钥闯?，第1組解與給定的6個(gè)角度完全相同。

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

4)計(jì)算求出的8組逆解對應(yīng)的位姿。

5)計(jì)算步驟(2)中位姿與步驟(4)中位姿差矩陣的2范數(shù)。

6)找出最大差矩陣范數(shù)及其對應(yīng)的位姿誤差。

通過計(jì)算，第7組逆解對應(yīng)位姿差矩陣的2范數(shù)norm7為6.864 709 873 407 309×10-13為最大。

該范數(shù)對應(yīng)的最大位姿誤差Δgmax為

Δgmax=gst-gst7=

(21)

式中g(shù)st為步驟(2)中給定角度對應(yīng)的位姿，gst7為第7組逆解對應(yīng)的位姿。

4 結(jié) 論

本文提出的6R機(jī)器人逆解算法的最大位姿誤差極其地小，為10-12數(shù)量級，從而驗(yàn)證了算法的正確性。與文獻(xiàn)[19]相比，其最大位姿誤差為10-2數(shù)量級，因此，本文提出的算法在精度方面具有很大優(yōu)勢。