基于旋量理論和代數消元6R機器人逆解算法*

趙榮波， 施智平，關 永，邵振洲，王國輝，吳立峰

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

0 引 言

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

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

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

1 旋量理論數學基礎

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

(1)

設gst(0)為剛體相對于慣性坐標系的初始位姿，當剛體轉動或平移θ后，剛體的最終位姿為gst(θ)=egst(0)。

對于只有旋轉關節的機器人，當給定ξ和θ，則θ的指數形式可表示為

(2)

(3)

根據指數積公式，n自由度串聯機器人末端執行器位姿可表示為

gst(θ)=ee…egst(0)

(4)

式中gst(0)和gst(θ)分別為末端執行器初始和最終位姿。

2 串聯機器人逆運動學

2.1 運動學建模

機器人結構參數和旋量坐標系如圖1所示。

圖1 機器人結構參數和旋量坐標系

可知，機器人的第2，3關節軸線是平行的，均與第1關節軸線異面垂直，且機器人后3關節軸線相交于一點，滿足Piper準則具有封閉形式解。

圖1中，S和T分別為慣性和工具坐標系，點o為慣性坐標系S的原點。ξ1～ξ6分別為各關節的單位運動旋量。r1～r3分別為前3關節軸線上的1點，r4～r6為后3關節軸線交點。各關節運動旋量計算為

(5)

由圖1可得機器人初始位姿為

(6)

6自由度串聯機器人正向運動學可由 (POE)表示為

gst(θ)=eeeeeegst(0)

(7)

2.2 逆運動學求解算法

(8)

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

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

(9)

式(9)表示點r4在前3關節中的螺旋運動，其對應的2種情形如圖2和圖3所示。

圖2 前3關節螺旋運動情形一

圖3 前3關節螺旋運動情形二

2.2.1 求解p21和p22

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

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

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

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

(10)

2.2.2 求解p31,p32,p33和p34

由圖2和圖3可知，ξ2和ξ3軸線平行于慣性坐標系S的x軸，且都垂直于平面β。因點r4,p31,p32,p33,p34,o2和o3都在β內，則這些點的x坐標相同，均為0。根據機器人的特殊結構和所建慣性坐標系之間關系，可確定o2,o3坐標為

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

(11)

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

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

(12)

通過求解可知，p3點坐標有4個解，因此，可以得到對應p31,p32,p33和p34坐標。

2.2.3 求解θ1,θ2和θ3

根據圖2和圖3，前3關節螺旋運動表示為

(13)

其由3個子問題構成，根據Paden-Kahan子問題1[2]，可得前3關節角度分別為

(14)

從圖2和圖3中可以看出,前3關節角度變量的解共有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)

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

用點p6右乘式(15)，則

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

(16)

應用式(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軸線上的一點p7，坐標p7=(0,b+1,0,1)T。

點p7右乘式(19)有gst3·p7=e·p7=[t4,t5,t6,1]T。

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

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

3 實例與驗證

“錢江一號”[18]工業機器人滿足本文所提出的機器人的結構。為了驗證本文提出的逆解算法，將該機器人作為實例進行逆運動學求解。驗證過程如下：

1)機器人工作空間內，任意給出6個關節角度θ1=100°,θ2=50°,θ3= -60°,θ4=180°,θ5=30°,θ6=-110°。

2)應用正向運動學方程求解機器人位姿

(20)

3)利用本文提出的算法和求出的位姿，計算各關節對應角度。共得到8組逆運動學的解，如表1所示。可以看出，第1組解與給定的6個角度完全相同。

表1 逆運動學的8組解

4)計算求出的8組逆解對應的位姿。

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

6)找出最大差矩陣范數及其對應的位姿誤差。

通過計算，第7組逆解對應位姿差矩陣的2范數norm7為6.864 709 873 407 309×10-13為最大。

該范數對應的最大位姿誤差Δgmax為

Δgmax=gst-gst7=

(21)

式中gst為步驟(2)中給定角度對應的位姿，gst7為第7組逆解對應的位姿。

4 結 論

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