蔣坤, 劉承君, 戴義祥, 張峰

(陸軍軍官學(xué)院　a.裝甲兵系;　b.訓(xùn)練部, 安徽 合肥　230031)

?

一般耦合6R機器人實時高精度逆運動學(xué)算法及應(yīng)用

蔣坤a, 劉承君a, 戴義祥b, 張峰a

(陸軍軍官學(xué)院a.裝甲兵系;b.訓(xùn)練部, 安徽 合肥230031)

對于一般6R機器人，包括耦合和解耦機械臂，提出一種通用的與常見工業(yè)機器人有區(qū)別的能夠?qū)崿F(xiàn)實時高精度控制的機械臂逆運動學(xué)算法，并且應(yīng)用在特種多關(guān)節(jié)機械臂上。基于矩陣多項式理論及其性質(zhì)，從12個等式方程中重新提取6個獨立的等式方程。對多變量等式方程組進(jìn)行消元后轉(zhuǎn)換成單變量方程，將問題轉(zhuǎn)換為求解單變量方程根的問題，最后利用矩陣多項式的性質(zhì)將多項式的求根問題轉(zhuǎn)化成為矩陣的特征值求解問題，由此大大降低了求解難度，并且保證了算法的穩(wěn)定性和精度問題。求解實例表明，提出的該算法能在毫秒級別內(nèi)得到某種耦合機械臂的所有運動學(xué)逆解，并且得到的解能夠精確到小數(shù)點后11位，從而實現(xiàn)實時高精度控制。

耦合機械臂；矩陣多項式；逆運動學(xué)；實時；高精度

0　引　言

自上世紀(jì)70年代開始機器人領(lǐng)域的相關(guān)專家和學(xué)者一直在密切關(guān)注和研究一般6R機器人的逆運動學(xué)算法問題，研究逆解算法在機器人學(xué)研究領(lǐng)域中的意義非常重大。Pieper[1]在1968年最早嘗試了將6自由度機械臂逆運動學(xué)求解問題轉(zhuǎn)換為單變量多項式問題，提出了解耦機械臂能夠得到封閉解。之后Duffy和Derby等[1]相繼嘗試從6自由多機械臂方程組中提取最簡化的特征多項式方程，但是只能達(dá)到32次單變量多項式，并且猜想到次數(shù)仍可降低。1985年Tsai和Morgan[2]使用多項式連續(xù)方法在數(shù)值上對非線性方程進(jìn)行替換，得到了二次多項式方程組的形式。上述研究者也都實踐證明了機械臂最多可以得到16組實數(shù)逆解問題。在1988年Lee和Liang[1]提出將相同的問題轉(zhuǎn)換成16次單變量多項式問題。1990年和1993年Li以及Raghavan和Roth[3]相繼提出了不同的單變量多項式系數(shù)計算的過程。1990年Manocha[3]提出了多項式方程的求解方法，將單變量方程組根的求解問題轉(zhuǎn)換成求解矩陣特征值的問題。至此逆運動學(xué)問題雖然可以認(rèn)為基本解決，但是關(guān)于如何更加穩(wěn)定和快速地求解出其所有逆解這一問題至今一直在研究中。

本文中采用以符號推演和數(shù)值計算方法為基礎(chǔ)的逆解算法，從多項式方程組中消除相關(guān)變量，將非線性方程組進(jìn)行線性組合，將多項式方程等式以矩陣的形式表達(dá)出來，最后將求解多項式方程根問題轉(zhuǎn)化成矩陣特征值問題，并且利用特征向量和方程組其他未知數(shù)之間的對應(yīng)關(guān)系求解出其它未知數(shù)。為了更加方便該算法的應(yīng)用，依托于MATLAB平臺將求解代碼程序封裝在GUI(圖形交互界面)，輸入已知量，直觀快速的解出未知量。

1　一般6R機械臂運動學(xué)方程的建立

基于D-H模型，一般6R機器人運動學(xué)方程[4]可以改寫成如下形式：

(1)

式中Ai(i=1～6)表示一個連桿相對與下一個連桿間相對關(guān)系的齊次變換矩陣，T6表示機器人末端執(zhí)行器相對于基坐標(biāo)系之間的相對關(guān)系，Ai和T6可分別表示如下：

(2)

ci=cosθi,si=sinθi,μi=sinαi,λi=cosαi，其中θi，di，ai，αi分別表示關(guān)節(jié)兩連桿法線夾角，兩連桿相對位置、公共法線距離和扭角參數(shù)。di，ai，αi表示機械臂的關(guān)節(jié)參數(shù)，一般6R機器人運動學(xué)逆解問題就是在已知di，ai，αi和T6的條件下求解關(guān)節(jié)變量θi。

由于Dinesh Manocha和John F. Canny[3]構(gòu)建等式方程的過程復(fù)雜，且計算方式繁瑣，本文基于位姿矩陣的特征，通過將位姿矩陣分解為位置和姿態(tài)分矩陣來重新構(gòu)建方程中的所需的矢量U,V,W和Y：

(3)

(4)

W=X2Z3X3Q4Q5e

(5)

(6)

式中:

且為正交陣。

由此可以先得到如下兩個等式:

U=V

(7)

W=Y

(8)

根據(jù)空間向量的投影性質(zhì)和幾何意義，尋找和上述兩個矢量式相同的冪乘積(單變量消去法的核心思路)，采用符號運算可以得到以下14個運動學(xué)方程。

Ux(θ3,θ4,θ5)=Vx(θ1,θ2)

(9a)

Uy(θ3,θ4,θ5)=Vy(θ1,θ2)

(9b)

Uz(θ3,θ4,θ5)=Vz(θ1,θ2)

(9c)

Wx(θ3,θ4,θ5)=Yx(θ1,θ2)

(9d)

Wy(θ3,θ4,θ5)=Yy(θ1,θ2)

(9e)

Wz(θ3,θ4,θ5)=Yz(θ1,θ2)

(9f)

U·U(θ3,θ4,θ5)=V·V(θ1,θ2)

(9g)

U·W(θ3,θ4,θ5)=V·Y(θ1,θ2)

(9h)

(U×W)x(θ3,θ4,θ5)=(V×Y)x(θ1,θ2)

(9i)

(U×W)y(θ3,θ4,θ5)=(V×Y)y(θ1,θ2)

(9j)

(U×W)z(θ3,θ4,θ5)=(V×Y)z(θ1,θ2)

(9k)

[(U·U)W-2(U·W)U]x(θ3,θ4,θ5)=

[(V·V)Y-2(V·Y)V]x(θ1,θ2)

(9l)

[(U·U)W-2(U·W)U]y(θ3,θ4,θ5)=

[(V·V)Y-2(V·Y)V]y(θ1,θ2)

(9m)

[(U·U)W-2(U·W)U]z(θ3,θ4,θ5)=

[(V·V)Y-2(V·Y)V]z(θ1,θ2)

(9n)

式中下標(biāo)x,y和z分別表示矢量等式的第一部分、第二部分以及第三部分。為了消去變量，首先應(yīng)進(jìn)行變量分離，將等式左邊和右邊分別提取冪乘積x45、x12，可以得到：

Px45=Qx12

(10)

其中x45=[s4s5s4c5c4s5c4c5s4c4s5c51]T，

x12=[s1s2s1c2c1s2c1c2s1c1s2c2]T。

式中P為元素是θ3線性組合的14×9矩陣，而Q則為14×8常系數(shù)矩陣。消元θ1,θ2：

從Q矩陣較為特殊的結(jié)構(gòu)可知，式(10)中的6個等式右邊只含有S1和C1，將其中任意兩個等式帶入到其余四個等式中，如下：

C1x1=n1該式表示上述任意兩個等式，其中C1為2×2矩陣，且要求可逆。

C2x1=n2表示余下的四個等式，x1=[cosθ1sinθ1]T。

通過帶入消元法消去S1，C1，S2和C2，即可得到只與θ3,θ4,θ5有關(guān)的4個等式，寫成矩陣形式：

(11)

式中T1表示元素為與θ3有關(guān)的4×9矩陣。P2為與上式(10)對應(yīng)的P矩陣的2×9分塊矩陣，同樣P4為對應(yīng)的4×9矩陣，01為2×1零矩陣。

除此之外，對上述剩余的8個等式進(jìn)行線性變換得到兩個等式右邊恒為0的等式，通過線性組合消元法將θ1,θ2消去，在Raghavan和Roth[2]構(gòu)建等式的基礎(chǔ)上進(jìn)一步進(jìn)行簡化修改和驗證，重新構(gòu)造了只和θ3,θ4,θ5有關(guān)的兩個等式，即：

-λi(U×W)x+ωUy-(r-d1)Wy=0

(12a)

-λi(U×W)y+ωUx-(r-d1)Wx=0

(12b)

其中ω=ozμ6+azλ6，r=-L6az+pz。至此總共得到6個只和變量θ3,θ4,θ5有關(guān)的等式方程，列成矩陣形式，得:

T2x45=O2

(13)

式中T2表示元素為與C3或S3線性相關(guān)有關(guān)多項式的矩陣，O2為6×1零矩陣。

參數(shù)代換：

T3(x3)x45=02

(14)

15)

T(x3)x″45(x4,x5)=03

(16)

(17)

式中O63為6×3階0矩陣，同時矩陣T(x3)可表示為:

(18)

A、B、C皆為12×12階常系數(shù)矩陣。

2　高精度求解

2.1符號推演和數(shù)值計算

一般6R機器人方程組求解過程運算量非常之大，且數(shù)值計算大多是借助機器浮點運算完成的，由于存在浮點計算的累計誤差問題，浮點計算誤差不僅僅是位數(shù)的問題，且在方程組求解過程中往往存在矩陣條件數(shù)大小問題，累計誤差可能會直接誤導(dǎo)判斷的正確性。利用MATLAB軟件對上述方程組或者矩陣進(jìn)行符號預(yù)處理，避免了浮點舍入誤差的問題。符號運算預(yù)處理只需執(zhí)行一次，每次計算時只需要將數(shù)值帶入符號運算結(jié)果中即可。

2.2矩陣特征值求根

根據(jù)線性方程組解性質(zhì)，可知若式子(16)有解，則有rank(T(x3))<12，即：

det(T(x3))=0

(19)

式子(19)是關(guān)于x3的一元16次方程，就單解變量x3的運算量就相當(dāng)大，計算出x3之后，還需帶入線性方程組求解其他變量值，首先浮點計算的累計誤差會很大程度上影響解的精度，其次很容易出現(xiàn)增根、漏解的情況，更重要的是該計算方式只能在線下求解，且計算時間較長，總言之無法實現(xiàn)實時高精度。與Raghavan和Roth等[2]采用的較為復(fù)雜繁瑣的計算方法不同，利用矩陣特征值求解的方法能較好的解決上述問題。

若A矩陣可逆，將式(18)兩邊同時左乘A-1，得：

(20)

其中E為單位陣，根據(jù)矩陣多項式理論可知式(1.19)的根對應(yīng)如下矩陣M的特征值：

該矩陣的特征值x3所對應(yīng)的特征向量即為x″45(x4,x5)，即可求解出x4和x5，將求解出來的三個未知數(shù)帶入式(1)求解出x1和x2，最終再求解出x6。

然而在很多情況下，A矩陣的條件數(shù)是非常大的，即A矩陣接近不可逆，如此就無法轉(zhuǎn)換為可以求解的莫尼矩陣多項式，因此利用簡單的元素線性代換法[5]，將A矩陣調(diào)整為可逆。將式(18)中的x3線性替換成：

(21)

2.3提高求解精度

每個特征值對應(yīng)的矩陣條件數(shù)的大小各不相同，即求解精度也各不相同。如果需要進(jìn)一步提高求解精度，將最初始計算出來的數(shù)值利用牛頓迭代法進(jìn)行一到兩次迭代可以進(jìn)一步提高求解精度(前提是要形成局部收斂情況)。

3　算法應(yīng)用于某種型號耦合機械臂

依托于MATLAB數(shù)學(xué)計算軟件，選擇具有代表性的6個旋轉(zhuǎn)自由度關(guān)節(jié)的某種內(nèi)窺機械臂作為求解實例，驗證算法的可行性和有效性。關(guān)節(jié)參數(shù)如表1與圖1所示。

表1　機械臂D-H參數(shù)

表1中θ為兩連桿法線夾角，d為兩連桿相對位置，a為公共法線距離，α為扭角參數(shù)。

圖1　運動學(xué)研究軟件界面

將機械臂模型導(dǎo)入至托卡馬克真空室內(nèi)部，將每個關(guān)節(jié)旋轉(zhuǎn)至一定的角度(例θ1=60°，θ2=15°，θ3=20°，θ4=25°，θ5=25°，θ6=20°θ1=60°，θ2=15°，θ3=20°，θ4=25°，θ5=25°，θ6=20°)，測量機械臂末端執(zhí)行器末端位姿矩陣(假設(shè)為目標(biāo)點位姿)，可表示為:

(22)

在MATLAB軟件中以符號運算為基礎(chǔ)進(jìn)行運算，之后帶入12位精度的各關(guān)節(jié)參數(shù)和目標(biāo)點的位姿，最終求解出各關(guān)節(jié)轉(zhuǎn)角。

表2　求解結(jié)果

實驗數(shù)據(jù)表明，該算法解出的第三組角度值相對理論值能夠精確到小數(shù)點后11位，求解全過程充分驗證了算法的穩(wěn)定性、實時性和精度，能夠完全滿足一般6R機器人的實時高精度控制的需要，滿足了對某些必須使用耦合機械臂上的應(yīng)用。

4　結(jié)束語

(1) 根據(jù)矩陣多項式理論和矩陣性質(zhì)構(gòu)建6個相互獨立的等式方程，簡化了計算過程的復(fù)雜程度。

(2) 通過符號運算避免了一般浮點運算帶來的累計誤差從

而提高算法的精度，并且通過轉(zhuǎn)換成求解矩陣特征值的途徑提高了算法的穩(wěn)定性和效率。

(3) 通過MATLAB自帶的GUI界面，將算法過程轉(zhuǎn)換成程序語言并將其封裝在其內(nèi)部，將已知量輸入之后可以在毫秒級別快速得到高精度解，將一般6R機器人逆運動學(xué)算法成功應(yīng)用于AIA多關(guān)節(jié)機械臂，滿足了實時高精度控制的需要。

[1] JORGE ANGELES.Fundamentals of robotic mechanical system. theory, methods and algorithms, second edition[M].New York:Springer,2003.

[2] MADHUSUDAN RAGHAVAN, BERNARD ROTH.Kinematic analysis of the 6R manipulator of general geometry[J].Journal of Mechanical Design,1990,115(3):502-508.

[3] DINESH MANOCHA,JOHN F CANNY.Efficient inverse kinematics for general 6R manipulators[J]. IEEE Transactions on Robotics and Automation, 1994,5(10):648-657.

[4] 蔡自興.機器人學(xué)[M].北京：清華大學(xué)出版社，2000.

[5] 劉松國,朱世強,王宣銀. 基于矩陣分解的一般6R機器人實時高精度逆運動學(xué)算法[J].機械工程學(xué)報，2008，44(11):304-309.

Algorithm and Application of Real-time Highly-accurate Inverse Dynamics of General Coupling 6R Robots

JIANG Kuna, LIU Cheng-juna, DAI Yi-xiangb, ZHANG Fenga

(a. Department of Armored Reconnaissance, b. Training Division, Academy of Army Officer, Hefei Anhui 230031, China)

For general 6R robots including coupling and decoupling robot arms, this paper presents an inverse dynamics algorithm of the robot arm with real-time and highly accurate control which differs from common industrial robots, and applies it to special robot arms with multiple joints. Based on the theory and property of matrix polynomials, 6 independent equations are drawn from 12 equations. Then, multi-variable equation groups are transferred into a single-variable equation through element elimination, so that we are now faced with the problem of finding the root of the single-variable equation. Finally, using the properties of the matrix polynomial, we transform the problem of extracting the root of a polynomial to finding the proper value of a matrix, thus making the solution much easier and ensuring the stability and accuracy of the algorithm. Solution examples indicate that the proposed algorithm can obtain all inverse kinematics of a specific coupling robot arm on the ms level, and the solution can be correct up to 11 decimal places, thus realizing real-time and highly accurate control.

coupling robot arm;matrix polynomial;inverse kinematics; real time;high accuracy

10.3969/j.issn.1000-3886.2016.02.031

TP242

A

1000-3886(2016)02-0102-04

蔣坤(1981-)，男，安徽渦陽人，講師，碩士，電氣工程專業(yè)方向。

定稿日期: 2015-09-16