基于矩陣分解的四關(guān)節(jié)機(jī)械手運(yùn)動(dòng)學(xué)逆解研究

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

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

基于矩陣分解的四關(guān)節(jié)機(jī)械手運(yùn)動(dòng)學(xué)逆解研究

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

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

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

四關(guān)節(jié)機(jī)械手；逆運(yùn)動(dòng)學(xué)；矩陣分解；實(shí)時(shí)高精度

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

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

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

1 逆運(yùn)動(dòng)學(xué)方程演算

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

TEnd=T1T2T3T4

變形可得：

(1)

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

(i=1,2,3,4)

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

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

(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。

進(jìn)一步將式(2)和式(3)進(jìn)行變換，構(gòu)造出兩個(gè)新的矢量等式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ù)的組合。

采用符號(hào)運(yùn)算對(duì)P和L做乘法，由P，L，P·P，P·L，得到8個(gè)逆運(yùn)動(dòng)學(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)號(hào)。

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

(12)

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

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個(gè)等式合并，用矩陣形式表示：

(17)

(18)

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

(19)

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

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的一元四次方程，通過(guò)求方程的解可得到x3的4個(gè)根，從而解出θ3。求得x3后,將其帶入式(19)可求出x1和x2的解,由于求解一元四次方程的計(jì)算量較大,且最終解的精度會(huì)受運(yùn)算過(guò)程中累積誤差的影響,為克服該問(wèn)題,采用矩陣分解法化解式(21)。

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

Ix3+A-1B=0

(22)

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

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

3 基于VC++的算法實(shí)時(shí)求解

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

a.頭文件包含處理。

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

extern"C"

{

#include

#include

}

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

dgesvd_的功能是對(duì)一個(gè)實(shí)矩陣進(jìn)行矩陣奇異值分解。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_的功能是對(duì)一個(gè)實(shí)矩陣進(jìn)行矩陣特征值分解。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í)例求解與數(shù)字仿真

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

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

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

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

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

表2 2組運(yùn)動(dòng)學(xué)逆解

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

則有：

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

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

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

5 結(jié)束語(yǔ)

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

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

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

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

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

[4] 劉華山，朱世強(qiáng)，吳劍波，等．基于向量?jī)?nèi)積的機(jī)器人實(shí)時(shí)逆解算法[J]．農(nóng)業(yè)機(jī)械學(xué)報(bào)，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] 汪木蘭，徐開蕓，饒華球，等．虛實(shí)結(jié)合的多關(guān)節(jié)機(jī)器人開放式仿真系統(tǒng)研究[J]．系統(tǒng)仿真學(xué)報(bào)，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

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

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

TP241.2

A

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