6R偏置腕關節攝影機器人逆運動學分析

阮德善, 韓 軍

(南京理工大學機械工程學院，江蘇 南京 210094)

目前攝影機器人已廣泛應用于影視行業中，其最大優點是能夠精確再現拍攝軌跡[1-2]。為了使攝像機可以靈活運動從而獲得高質量的拍攝效果，攝影機器人的機械結構多數為多軸聯動[3]。目前，6R攝影機器人使用廣泛，其結構類似于工業機器人。6R攝影機器人腕關節結構一般可分為歐拉腕關節和偏置腕關節。歐拉腕關節的三軸交于一點，針對這種帶有歐拉腕關節結構的6R攝影機器人可采用成熟的工業機器人理論進行相應的研究。在一些特殊情況下，6R攝影機器人末端需要裝載攝像機及其相關配件，因此攝影機器人的末端結構需要承受較大的載荷。此外，6R攝影機器人往往需要較長的水平延伸和末端更大的旋轉角度[4]，然而帶有歐拉腕關節結構的攝影機器人難以滿足上述要求。為此，可設計3個軸互不相交的偏置腕關節結構。但擁有此結構的6R攝影機器人其運動學方程往往變得很復雜，從而使求運動學逆解變得更加困難。

Kucuk等[5]通過研究大量機器人結構指出，求6R偏置腕關節機的運動學逆解非常困難且僅能使用數值法進行求解，并指出在使用牛頓數值算法時易出現由于雅可比矩陣奇異而導致算法不收斂的情況，此外牛頓數值算法需要給出一組精確的初始向量。Zhao等[6]也通過雅可比逆矩陣研究了一種數學算法，但由于雅可比逆矩陣求解困難，而且當出現奇異時算法也無法收斂。Pashkevich[7]研究了一種適用于偏置腕關節機器人的逆運動學算法，該算法不使用雅可比矩陣，但當腕關節接近奇異點時，該算法就無法收斂。

本文以6R偏置腕關節攝影機器人為研究對象，提出一種求解逆運動學問題的方法，然后采用MATLAB 編程并驗證逆解的正確性。此外，為了縮短求運動學逆解的時間，根據6R偏置腕關節攝影機器人的結構縮小第一個關節的搜索范圍。

1 攝影機器人運動學模型

1.1 關節坐標系的建立

建立攝影機器人運動學模型的一種有效方法是，在每一關節建立相應的坐標系，經過一系列坐標轉換求得末端執行器對基坐標系的位姿矩陣，從而得到相應的運動學方程。而坐標系的建立方法會影響最后運動學方程的復雜程度。為了減少參數個數，本文中采用典型的D-H改進法建立攝影機器人各個關節的坐標系。根據D-H改進法得到相鄰兩個坐標系的齊次坐標轉換矩陣[8- 9]如式(1)所示。

(1)

式中：i-1Ti為第i坐標系相對于第(i-1)坐標系的齊次坐標轉換矩陣；θi為xi和xi-1之間的夾角；αi-1為zi和zi-1之間的夾角；ai-1為zi和zi+1之間的距離；di為zi和zi-1之間的距離；sθi，cθi，sαi-1，cαi-1分別表示sinθi，cosθi，sinαi-1，cosαi-1。

1.2 攝影機器人運動學模型

攝影機器人本體如圖1所示，采用D-H改進法建立攝影機器人的各個關節坐標系，如圖 2所示，由此可得攝影機器人的參數，見表 1。

圖1 6R攝影機器人本體

圖2 攝影機器人的各個關節坐標系

表1 攝影機器人D-H改進法參數

根據式(1)以及表 1可得攝影機器人各個齊次坐標轉換矩陣：

2 正運動學分析

將上面所有齊次坐標轉換矩陣相乘可得出末端執行器坐標系相對于基坐標系的轉換矩陣0T6為

0T6=0T11T22T33T44T55T6

(2)

(3)

(4)

式中：R為姿態矩陣。

式(3)即為攝影機器人的運動學方程。正運動學求解的目的就是通過各關節的轉角求出末端執行器坐標系相對于基坐標系的位置矩陣p和姿態矩陣R。

將矩陣0T6展開，然后令矩陣里面的每個元素與式(3)右邊的元素相等就能求出運動學正解：

nx=s6(s1c4-c1s4c23)-c6[s5c1s23-

c5(s1s4+c1c4c23)]

ny=-c6[s1s5s23+c5(c1s4-s1c4c23)]-

s6(c1c4+s1s4c23)

nz=s4s6s23-c6(s5c23+c4c5s23)

ox=s6[c1s5s23-c5(s1s4+c1c4c23)]+

c6(s1c4-c1s4c23)

oy=s6[s1s5s23+c5(c1s4-s1c4c23)]-

c6(c1c4+s1s4c23)

oz=s6(s5c23+c4c5s23)+s4c6s23

ax=-c1c5s23-s5(s1s4+c1c4c23)

ay=s5(c1s4-s1c4c23)-s1c5s23

az=c4c5s23-c5c23

px=a1c1-a5[c1s5s23-c5(s1s4+c1c4c23)]-

d6[c1c5s23+s5(s1s4+c1c4c23)]+a3c1c23-

d4c1s23+a2c1c2

py=a1s1-a5[s1s5s23+c5(c1s4-s1c4c23)]-

d6[s1c5s23-s5(c1s4-c4s1c23)]+a3s1c23-

d4s1s23+a2c2s1

pz=-a2s2-a3s23-d4c23-a5(s5c23+c4c5s23)-

d6(c5c23-c4s5s23)

(5)

式中：si=sinθi,ci=cosθi;sij=sin(θi+θj),cij=cos(θi+θj)。

本文使用MATLAB編寫程序來驗證運動學正解。設置各關節角度如圖3所示，通過計算得出運動學正解如圖4所示。此時，攝影機器人的位姿如圖2所示，從而驗證了運動學正解的正確性。

圖3 各關節的角度

3 逆運動學分析

3.1 逆運動學求解方法

運動學逆解是當給定機器人末端執行器相對于基坐標系的位姿矩陣時，要求出各個關節的轉角。逆運動學對機器人的軌跡規劃和動力學分析具有重要意義。逆運動學的求解特點是計算量大，往往需要花費大量的時間。本文的研究對象為6R偏置腕關節攝影機器人，由于其結構較為特殊，無法用解析法求出運動學逆解，因此必須使用數值法進行求解。本文提出的數值法基本思路如圖5所示。

圖4 運動學正解

圖5 數值法的基本思路

3.2 逆運動學求解過程

假設θ1已知，采用式(2)以及合適的變換將其他關節角變量表示為θ1的函數。基本的變換思路是將要求的變量逐一移到式(2)的左側。首先通過如下變換求出θ6。

(0T1)-1(0T6)(5T6)-1=1T22T33T44T5

(6)

將式(6)展開，在等號兩邊找到僅包含θ1和θ6的元素令其相等可得：

c1py-pxs1+a5s6(c1oy-oxs1)=a5c6(c1ny-nxs1)

(7)

令A=-(c1py-pxs1)/a5,B=c1oy-oxs1,C=c1ny-nxs1,從而可得：

θ6=arctan2(C,B)+arctan2(A,

(8)

觀察式(6)的展開項，可得方程(9)和(10)：

a1+a2c2+a3c23-d4s23=D

(9)

-a2s2-a3s23-d4c23=E

(10)

其中：D=c1px+pys1+a5s6(c1ox+oys1)-a5c6(c1nx+nys1)；E=-a5c6nz+a5ozs6。

將式(9)和式(10)平方后相加得到式(11)，從而求出θ2，如式(12)所示。

Es2-Dc2=F

(11)

θ2=arctan2(D,E)+arctan2(F,

(12)

θ3=θ23-θ2

(13)

為了求出θ4,θ5，將式(6)繼續變換，如式(14)所示。

(0T1)-1(0T6)(5T6)-1(4T5)-1=1T22T33T4

(14)

將式(14)展開得到與θ4,θ5有關的方程：

s5G+c5H=0

(15)

-s4=c5G-s5H

-c4=I

(16)

其中G=c6(c1ny-nxs1)-s6(c1oy-oxs1)；H=ayc1-axs1；I=c6(c1oy-oxs1)+s6(c1ny-nxs1)。從而可得：

θ5=arctan2(H,-G)

θ4=arctan2(c5G-s5H,I)

(17)

由此可知，所有關節變量均能表示成θ1的函數。現在假設其他關節變量已知，通過將式(2)進行如下變換來求θ1的值。

0T6(5T6)-1(4T5)-1(3T4)-1=0T11T22T3

(18)

將式(18)展開得到與θ1相關并且沒使用過的方程組，如式(19)所示：

(19)

令P=s4[axs5-c5(c6nx-oxs6)]-c4(c6ox+nxs6)，Q=s4[ays5-c5(c6ny-oys6)]-c4(c6oy+nys6)，從而可得：

θ1=arctan2(-P,Q)

(20)

逆運動學問題求解流程如圖6所示。逆解計算過程涉及到位置絕對誤差ε和θ1每次循環增加的角度Δθ1。

圖6 逆運動學問題求解流程

3.3 逆運動學算法優化

由3.2節可知，所提出的逆解計算過程要先給出θ1的初始值、位置絕對誤差ε和θ1每次循環增加的角度Δθ1。而ε一般要事先確定下來(本文中設置ε為0.01 mm就能滿足要求)，因此為了提高算法的精度以及滿足誤差要求，Δθ1需要盡量取小，本文設置Δθ1為0.001°，理論上可以在0°～360°任意選擇。但是θ1的初始值的選擇會影響到運動學逆解的計算時間，因此θ1的初始值應該越接近實際角度值越好。此外，每次循環θ1自增微小角度Δθ1，從而θ1的初始值應該要小于實際角度值才能縮短計算時間。為了縮小θ1初始值的范圍，將式(2)進行變換，如式(21)所示：

(0T1)-1(0T6)=1T22T33T44T55T6

(21)

將式(21)展開得到方程如式(22)，從而求出θ1，如式(23)所示。

pxs1-pyc1=a5s4c5

(22)

θ1(k)=arctan2(py,px)+arctan2(a5k,

(23)

在式(23)中只有k為未知變量且其值在[-1,1]。為了求出θ1的最小值，將式(23)對k進行求導得：

(24)

θ1min=arctan2(py,px)+arctan2(-a5,

(25)

分析式(22)可知，當θ4為0°或者θ5為90°時，可以直接求得θ1的值為arctan2(py,px)。

3.4 運動學逆解驗證

首先給出末端執行器對基坐標系的位姿矩陣(見表2)，采用3.2節的計算過程求運動學逆解，然后使用這組逆解通過正運動方程求出末端執行器的位姿矩陣。此時，將通過逆解所求出末端執行器的位姿矩陣與已知末端執行器的位姿矩陣進行比較并分析可判斷算法的正確性。

表2 末端執行器對極坐標的位姿矩陣

表3 運動學逆解

圖7 攝像機坐標系的建立

攝像機坐標系對基坐標系的位姿矩陣為0TC，為了求出攝像機在運動過程中各個關節的變化，需要攝像機在圓弧上每一位置相應的位姿矩陣0T6。0T6位姿矩陣可以通過式(26)求出。

由于0TC=0T66TC，所以有

0T6=0TC(6TC)-1

(26)

式中：6TC為攝影機坐標系對第六關節坐標系的位姿矩陣。由于攝像機固定在攝影機器人的末端，因此6TC位姿矩陣為已知。

將圓弧段插補500個點，采用MATLAB軟件編寫程序得到各關節的角度曲線，如圖8所示。從圖中可以清楚地看出，逆運動學求解方法得到的角度曲線是連續光滑的，進一步表明方法的可行性以及正確性。

4 結束語

本文以6R偏置腕關節攝影機器人為研究對象，采用D-H改進法建模之后，進行了正運動學和逆運動學的求解。由于攝影機器人的機械結構有些特殊，逆運動學問題僅能采用數值法進行求解。經過運動學逆解驗證表明，本文所提出的求解過程不僅收斂速度快、精度高，而且出現奇異時算法也能快速收斂。假設攝像機沿著一條圓弧運動，通過本文的運動學逆解可得各個關節變化曲線連續光滑，進一步驗證了逆解的正確性，從而為攝影機器人的控制以及軌跡規劃提供了重要的依據。

圖8 各關節變化曲線