李小霞，汪木蘭，殷紅梅，許云飛

(1．江蘇食品藥品職業(yè)技術(shù)學(xué)院 機電工程系，江蘇 淮安 223003)(2．南京工程學(xué)院 先進數(shù)控技術(shù)江蘇省高校重點建設(shè)實驗室，江蘇 南京 211167)(3．淮安信息職業(yè)技術(shù)學(xué)院 機電工程系，江蘇 淮安 223003)

基于矩陣分解的四關(guān)節(jié)機械手運動學(xué)逆解研究

李小霞1，汪木蘭2，殷紅梅3，許云飛1

(1．江蘇食品藥品職業(yè)技術(shù)學(xué)院 機電工程系，江蘇 淮安 223003)(2．南京工程學(xué)院 先進數(shù)控技術(shù)江蘇省高校重點建設(shè)實驗室，江蘇 南京 211167)(3．淮安信息職業(yè)技術(shù)學(xué)院 機電工程系，江蘇 淮安 223003)

以自主研發(fā)的NGR01型四關(guān)節(jié)機械手為研究對象，基于機械手運動學(xué)方程，提出一種實時高精度逆運動學(xué)算法。該算法首先通過符號運算將矩陣方程從8階降到4階，在消除增根的同時還提高了計算速度。然后通過矩陣分解特征值求解關(guān)節(jié)變量，保證了逆運動學(xué)解的準(zhǔn)確性和精度。最后借助Maple數(shù)學(xué)計算軟件驗證逆運動學(xué)算法的正確性，采用VC++語言和CLAPACK數(shù)學(xué)運算庫實現(xiàn)運動學(xué)算法的仿真。驗證結(jié)果表明，利用該算法求解運動學(xué)逆解僅需0.67ms，相對傳統(tǒng)的反變換法具有更優(yōu)的實時高精度性能。

四關(guān)節(jié)機械手；逆運動學(xué)；矩陣分解；實時高精度

機械手位姿的變化通過各關(guān)節(jié)的運動來實現(xiàn)，因此任務(wù)空間的運動必須轉(zhuǎn)換到各關(guān)節(jié)空間，而逆運動學(xué)算法是關(guān)節(jié)角與三維位姿之間的轉(zhuǎn)換紐帶，所以逆運動學(xué)算法的實時精確性是機械手軌跡規(guī)劃與運動優(yōu)化控制的基礎(chǔ)。

王戰(zhàn)中[1]等人提出了用MATLAB GUI編程來自動求解機器人的多組逆解，并采用最短行程的原則自動尋找機器人的最優(yōu)逆運動學(xué)解。李憲華[2]等人針對純代數(shù)法找不到六自由度模塊化串聯(lián)機械臂的獨立不相關(guān)變量方程的問題，采用幾何方法求解機械臂前3個關(guān)節(jié)和反變換法求解后3個關(guān)節(jié)，并通過給出解的組合原則，得到了該機械臂逆運動學(xué)的完整解析解。程永倫[3]和劉華山[4]等人基于變換矩陣中旋轉(zhuǎn)子矩陣正交的特性，提出了一種6R機器人運動學(xué)逆解算法。S. KUMAR[5]等人將冗余機械臂逆運動學(xué)求解問題轉(zhuǎn)化為非線性優(yōu)化問題。Van Henten[6]等人將混合數(shù)值解析法引入到黃瓜采摘機器人逆運動學(xué)求解問題中，提高了算法的穩(wěn)定性和效率。

本文針對自主研發(fā)的NGR01型四關(guān)節(jié)機械手[7]，通過符號運算和矩陣分解求運動學(xué)逆解，不僅提高了計算運動學(xué)逆解的速度，同時還降低了浮點數(shù)運算帶來的累積誤差。四關(guān)節(jié)機械手的實數(shù)運動學(xué)逆解最多有4組，但考慮到機構(gòu)設(shè)計及實際工程應(yīng)用中各關(guān)節(jié)角的有效范圍，最終滿足要求的機械手逆運動學(xué)解最多有2組。

1 逆運動學(xué)方程演算

機械手逆運動學(xué)含義為：已知三維空間中機械手關(guān)節(jié)末端的位姿矩陣，要求出對應(yīng)的各關(guān)節(jié)變量。根據(jù)D-H矩陣坐標(biāo)變換原則，四關(guān)節(jié)機械手運動學(xué)方程可以描述為：

TEnd=T1T2T3T4

變形可得：

(1)

Ti=Rz(θi)Tz(di)Tx(ai)Rx(αi)

(i=1,2,3,4)

式中：Ti為第i個連桿相對于第(i-1)個連桿的位姿轉(zhuǎn)換矩陣；TEnd為機械手末端坐標(biāo)系相對于基坐標(biāo)系的位置和姿態(tài)；Rz和Rx為姿態(tài)變換矩陣；Tz和Tx為位置變換矩陣；di，ai和αi為機械手結(jié)構(gòu)參數(shù)，由具體機械手結(jié)構(gòu)確定；θi為關(guān)節(jié)變量。

現(xiàn)將D-H參數(shù)作為符號變量，采用以符號運算著稱的數(shù)學(xué)軟件Maple對式(1)左右兩邊分別進行計算，可知第3列和第4列元素與θ4無關(guān)，從而得到關(guān)于θi(i=1,2,3)的6個方程，以矩陣形式表示為：

(2)

(3)

式中：f1=g1+g2+a3，f2=λ3(g2-g1)，f3=μ3(g1-g2)+d3，r1=m1+m2，r2=λ3(m2-m1)，r3=μ3(m1-m2)，g1=c3a3，g2=λ3a3，m1=s3λ3，m2=λ3μ3，h1=c1a4+s1d4-a1，h2=-μ1d1-λ1(s1a4-c1d4)，h3=μ1(s1a4-c1d4)+λ1d1，n1=c1μ4-s1λ4，n2=-λ1(s1μ4-c1λ4)，n3=μ1(s1μ4-c1λ4);μi=sinαi，λi=cosαi，si=sinθi，ci=cosθi，i=1,2,3,4。

進一步將式(2)和式(3)進行變換，構(gòu)造出兩個新的矢量等式P和L，有：

式中：hv=FSC(c1,s1,c2,s2)，fv=FSC(c3,s3)，nv=FSC(c1,s1,c2,s2)，rv=FSC(c3,s3)，其中v=1,2,3，F(xiàn)SC(·)表示關(guān)節(jié)變量正余弦函數(shù)的組合。

采用符號運算對P和L做乘法，由P，L，P·P，P·L，得到8個逆運動學(xué)方程：

PL1(θ1,θ2)=PR1(θ3)

(4)

PL2(θ1,θ2)=PR2(θ3)

(5)

PL3(θ1,θ2)=PR3

(6)

LL1(θ1,θ2)=LR1(θ3)

(7)

LL2(θ1,θ2)=LR2(θ3)

(8)

LL3(θ1,θ2)=LR3

(9)

(P·P)L1(θ1,θ2)=(P·P)R1

(10)

(P·L)L2(θ1,θ2)=(P·L)R2

(11)

式中：下標(biāo)L和R分別表示矢量等式的左式和右式，1，2，3是矢量中元素的標(biāo)號。

可見，式(6)、式(9)～(11)共4個等式與θ3無關(guān)，將這4個等式合并后組合成矩陣形式：

(12)

代入含有θ3的4個等式，線性變換后得到4個新的等式：

PL1(θ1,θ2)+x3(PL2(θ1,θ2))=

PR1(θ3)+x3(PR2(θ3))

(13)

PL2(θ1,θ2)-x3(PL1(θ1,θ2))=

PR2(θ3)-x3(PR1(θ3))

(14)

LL1(θ1,θ2)+x3(LL2(θ1,θ2))=

LR1(θ3)+x3(LR2(θ3))

(15)

LL2(θ1,θ2)-x3(LL1(θ1,θ2))=

LR2(θ3)-x3(LR1(θ3))

(16)

同理，將式(13)～(16)這4個等式合并，用矩陣形式表示：

(17)

(18)

令xi=tan(θi/2)，i=4,5,則有：

(19)

為方便表述，將式(19)簡化為：

L5(x3)(V12)=0

(20)

式中：L5(x3)=Ax3+B，為常系數(shù)矩陣，其中A∈R4×4，B∈R4×7；V12為包含θ1和θ2的半角正切以及正余弦函數(shù)的向量。

2 矩陣分解求根

式(20)有解的必要條件是：

det(L5(x3))=0

(21)

式(21)是關(guān)于x3的一元四次方程，通過求方程的解可得到x3的4個根，從而解出θ3。求得x3后,將其帶入式(19)可求出x1和x2的解,由于求解一元四次方程的計算量較大,且最終解的精度會受運算過程中累積誤差的影響,為克服該問題,采用矩陣分解法化解式(21)。

設(shè)A為非奇異矩陣，由方程det(L5(x3))=0可以得到：

Ix3+A-1B=0

(22)

式中：I為4階單位矩陣。

令M=A-1B，由矩陣?yán)碚摽芍?，矩陣M的特征值即為x3的解，對應(yīng)的特征矢量即為V12。由V12中的相關(guān)元素可以計算出x1和x2，從而解出θ1和θ2，繼續(xù)將求出的3個關(guān)節(jié)變量代入式(1)，即可求得θ4。為降低累積誤差對關(guān)節(jié)變量解的影響,運算的過程數(shù)據(jù)需要保留較高數(shù)量級。

3 基于VC++的算法實時求解

分解矩陣R4×4和A的奇異值，求解矩陣M的特征矢量和特征值，均具有較高的運算復(fù)雜度。為了實現(xiàn)NGR01型機械手運動學(xué)逆解的實時計算，采用C++將CLAPACK編譯成blas.lib、libF77.lib、libI77.lib和clapack.lib 4個庫文件，調(diào)用其中的相關(guān)函數(shù)進行矩陣計算。CLAPACK是由f2c編譯成Fortran77 LAPACK線性代數(shù)庫的C語言版本，以目前最有效的線性運算內(nèi)核blas做底層計算，具有簡潔、高效和可移植性等優(yōu)點。

a.頭文件包含處理。

因為程序是C++，而CLAPACK是f2c程序轉(zhuǎn)換的C語言版本，所以用extern關(guān)鍵字調(diào)用。

extern"C"

{

#include

#include

}

b.關(guān)鍵函數(shù)的調(diào)用。

dgesvd_的功能是對一個實矩陣進行矩陣奇異值分解。gesvd_的函數(shù)聲明如下:

GLfloat dgesvd_(char *jobu, char *jobvt, integer *m, integer *n, doublereal *a, integer *lda, doublereal *s, doublereal *u, integer * ldu, doublereal *vt, integer *ldvt, doublereal *work, integer *lwork, integer *info)

Dgeev_的功能是對一個實矩陣進行矩陣特征值分解。dgeev_的函數(shù)聲明如下:

GLfloat dgeev_(char *jobvl, char *jobvr, integer*n, doublereal*a, integer*lda, doublereal* wr, doublereal *wi, doublereal *vl, integer *ldvl, doublereal *vr, integer *ldvr, doublereal *work, integer *lwork, integer *info)

4 實例求解與數(shù)字仿真

以自主開發(fā)的NGR01[7]型四自由度機械手為對象開展逆運動學(xué)驗算實驗。機械手連桿坐標(biāo)系如圖1所示，D-H參數(shù)見表1。仿真計算機平臺配置為：Intel酷睿雙核處理器，2GB DDR3內(nèi)存，Windows XP操作系統(tǒng)。

圖1 NGR01型機械手連桿坐標(biāo)系

表1 NGR01型機械手連桿參數(shù)

設(shè)四關(guān)節(jié)機械手末端位姿矩陣為：

設(shè)置Maple的Digits環(huán)境參數(shù)為15時，求出與Tend0對應(yīng)的滿足要求的2組運動學(xué)逆解，結(jié)果見表2。

表2 2組運動學(xué)逆解

現(xiàn)將每組關(guān)節(jié)變量代入Tend=T1T2T3T4中，得到2個末端位姿矩陣Tend_i(i=1,2)，令誤差矩陣為e，e(r,c)(r,c=1,2,3,4)為e的第r行第c列元素，其計算表達式為：

則有：

在VC++ 6.0環(huán)境中，基于MFC框架類，通過CLAPACK數(shù)學(xué)運算庫實現(xiàn)逆運動學(xué)算法的仿真。以1 500次測試計算關(guān)鍵數(shù)值所需時間，得到如表3所示的結(jié)果，可見，算法的平均耗時為0.671 1ms。在相同仿真條件下，用反變換法求解逆運動學(xué)的平均耗時為1.13ms，因此提出的算法比反變換法具有更好的實時性。

表3 數(shù)值計算所需時間

基于VC++仿真環(huán)境及OpenGL圖形庫仿真出機械手的運動學(xué)過程，NGR01型機械手的逆運動學(xué)解對應(yīng)的位姿仿真結(jié)果如圖2所示。在工程實際應(yīng)用中，為了縮短機械手運動時間，并降低機械手能量的消耗，最優(yōu)逆運動學(xué)解通常選取距當(dāng)前關(guān)節(jié)變量距離最小的一組。

5 結(jié)束語

本文采用矩陣特征分解方法計算關(guān)節(jié)變量，從特征值和特征向量中快速求出3個關(guān)節(jié)變量，進一步得到四關(guān)節(jié)機械手最多4組逆運動學(xué)解。借助Maple數(shù)學(xué)計算軟件、VC++語言和CLAPACK數(shù)學(xué)運算庫實現(xiàn)整套運動學(xué)算法的仿真和驗證，基于OpenGL圖形庫仿真出機械手的運動學(xué)過程。文中提出的整套算法具有實時高精度特性，完全能夠滿足NGR01型四關(guān)節(jié)機械手的實時控制需要，是一種比較理想的逆運動學(xué)求解方案，本論文提出的算法將應(yīng)用于智能型花卉采摘機械手控制系統(tǒng)中。

圖2 NGR01型機械手的2種姿態(tài)

[1] 王戰(zhàn)中，楊長建，劉超穎，等．MATLAB環(huán)境下六自由度焊接機器人運動學(xué)逆解及優(yōu)化[J]．機械設(shè)計與制造，2013(7)：182-184.

[2] 李憲華，郭永存，張軍，等．基于MATLAB的模塊化機器人手臂運動學(xué)算法驗證及運動仿真[J]．計算機應(yīng)用研究，2013，30(6)：1682-1684，1704.

[3] 程永倫，朱世強，劉松國．基于旋轉(zhuǎn)子矩陣正交的6R機器人運動學(xué)逆解研究[J]．機器人，2008，30(2)：160-164.

[4] 劉華山，朱世強，吳劍波，等．基于向量內(nèi)積的機器人實時逆解算法[J]．農(nóng)業(yè)機械學(xué)報，2009，40(6)：212-216.

[5] Kumar S，Sukavanam N．An optimization approach to slove the inverse kinematics of redundant manipulator[J]．Information and System Sciences, 2010，6(4)：414-423.

[6] Van Henten，Schenk E. J．Collision-free inverse kinematics of the redundant seven-link manipulator used in a cucumber picking robot[J]．Biosystems Engineering，2010，106(2)：223-330.[7] 汪木蘭，徐開蕓，饒華球，等．虛實結(jié)合的多關(guān)節(jié)機器人開放式仿真系統(tǒng)研究[J]．系統(tǒng)仿真學(xué)報，2007，19(21)：4965-4969.

InverseKinematicsAlgorithmforFourJointManipulatorBasedonMatrixDecomposition

LI Xiaoxia1, WANG Mulan2, YIN Hongmei3, XU Yunfei1

(1.Jiangsu Food & Pharmaceutical Science College, Jiangsu Huaian, 223003, China)(2.Nanjing Institute of Technology, Jiangsu Nanjing, 211167, China)(3.Huaian College of Information Technology, Jiangsu Huaian, 223003, China)

It proposes a real-time inverse kinematics algorithm with high accuracy based on the kinematics equation of manipulator with the research object of self-developed NGR01 type four joints manipulator. This reduces the order of matrix equation from 4 to 2 through symbolic calculating, thus eliminates extraneous roots while improving the algorithmic speed. Eigen-decomposition is adopted to get roots from target matrix, which ensures the accuracy of the solutions. The correctness of the algorithm is verified by Matlab and Maple mathematical software. It uses C++ language and CLAPACK math library to realize the simulation of kinematics algorithm. Experimental results indicate that the presented algorithm can acquire 2 inverse kinematics solutions in a time of 0.67 ms, so the proposed algorithm is faster and higher accuracy than the traditional inverse transformation method with the same experiment platform.

Four Joint Manipulator; Inverse Kinematics; Matrix Decomposition; Real-time & High Accuracy

10.3969/j.issn.2095-509X.2014.09.007

2014-05-10

淮安市科技項目(SN13064)；江蘇省高校自然科學(xué)研究基金項目(13KJB120003)

李小霞(1987—)，女，江蘇淮安人，江蘇食品藥品職業(yè)技術(shù)學(xué)院助教，工學(xué)碩士，主要研究領(lǐng)域為機器人技術(shù)和先進制造技術(shù)。

TP241.2

A

2095-509X(2014)09-0027-04