閔柏成，杜康健，張世超

(中國(guó)船舶集團(tuán)有限公司第八研究院，江蘇 揚(yáng)州 225101)

0 引 言

機(jī)器人是機(jī)電一體化最高成就的體現(xiàn)，處于科技發(fā)展的前沿[1-2]。機(jī)器人技術(shù)包括了多種學(xué)科和領(lǐng)域的最新研究成果，匯集了機(jī)械工程、計(jì)算機(jī)技術(shù)、電子技術(shù)、人工智能及自動(dòng)控制理論等[3]。機(jī)器人運(yùn)動(dòng)學(xué)是分析機(jī)器人控制的基礎(chǔ)。本文以FANUC-M10i六自由度機(jī)器人為研究對(duì)象，利用D-H法建立機(jī)器人的坐標(biāo)系并以一次逆乘法進(jìn)行正逆運(yùn)動(dòng)學(xué)算法的推導(dǎo)。推導(dǎo)出的運(yùn)動(dòng)學(xué)算法與MATLAB機(jī)器人工具箱進(jìn)行對(duì)比以驗(yàn)證其正確性[4]。

1 機(jī)器人D-H坐標(biāo)系的建立

FANUC-M10i型機(jī)器人具有6個(gè)自由度，如圖1所示。根據(jù)其結(jié)構(gòu)建立各連桿坐標(biāo)系，其中4、5、6系均在腕部，如圖2所示[5]。機(jī)器人的6個(gè)關(guān)節(jié)都是旋轉(zhuǎn)關(guān)節(jié)，由D-H法建立連桿坐標(biāo)系，建立連桿參數(shù)表如表1所示，其中連桿參數(shù)可定義如下：

(1)ai-1表示從zi-1到zi沿xi-1測(cè)量的距離；

(2)αi-1表示從zi-1到zi繞xi-1旋轉(zhuǎn)的角度；

(3)di表示從xi-1到xi沿zi測(cè)量的距離；

(4)θi表示從xi-1到xi沿zi旋轉(zhuǎn)的角度。

坐標(biāo)系{i-1}與{i}系之間的位姿關(guān)系代表連桿之間的變換，可以用連桿參數(shù)來表示。一個(gè)參數(shù)可以用一種簡(jiǎn)單的變換來表示，4種簡(jiǎn)單的變換就可以組成連桿坐標(biāo)系之間的變換。得到的連桿變換公式如下：

圖1 M10I型機(jī)器人結(jié)構(gòu)簡(jiǎn)圖

圖2 連桿坐標(biāo)系圖

表1 M-10i型機(jī)器人關(guān)節(jié)坐標(biāo)及參數(shù)

(1)

2 正運(yùn)動(dòng)學(xué)分析

根據(jù)上述FANUC-M10i型機(jī)器人的連桿坐標(biāo)系圖與參數(shù)表，由連桿變換公式可以進(jìn)行連桿坐標(biāo)系{i}相對(duì)于坐標(biāo)系{i-1}之間關(guān)系的推導(dǎo)，推導(dǎo)結(jié)果可以表示如下：

(2)

其相鄰兩連桿的變換結(jié)果可以表示為：

(9)

式中：si=sinθi;ci=cosθi。

根據(jù)上述結(jié)果可得：

(11)

求得表示姿態(tài)的公式：

nx=-s1(c5c6s4+c4s6)+c1(c6c23s5+

c23(c4c5c6-s4s6))

(12)

ny=c6s1s23s5+c1(c5c6s4+c4s6)+

c23s1(c4c5c6-s4s6)

(13)

nz=-c4c5c6s23+c23c6s5+s23s4s6

(14)

ox=c6(c4s1+c1c23s4)+s6(-c5s1s4+

c1(c23c4c5+s23s5)),ox=-ox6

(15)

oy=c1(-c4c6+c5s4s6)+s1(s23s5s6+

c23(c6s4+c4c5s6)),oy=-oy6

(16)

oz=-c6s23s4+s6(-c4c5s23+c23s5),oz=-oz6

(17)

ax=-s1s4s5+c1(-c5s23+c23c4s5),ax=-ax6

(18)

ay=-c5s1s23+(c23c4s1s5+c1s4s5),ay=-ay6

(19)

az=-c23c5-c4s23s5,az=-az6

(20)

求得表示位置的公式：

px=-L7s1s4s5+c1(a1+a2c2-(d4+L7c5)s23+

c23(a3+L7c4s5))，px6=px-L7ax

(21)

py=L7c1s4s5+s1(a1+a2c2-(d4+L7c5)s23+

c23(a3+L7c4s5))，py6=py-L7ay

(22)

pz=-d4c23-s2(a2+L7c5s3+c3(a3+L7c4s5))+

c2(-L7c3c5+s3(a3+L7c4s5))，pz6=pz-L7az

(23)

式中：s23=sin(θ2-θ3)；c23=cos(θ2-θ3)。

3 逆運(yùn)動(dòng)學(xué)分析

逆運(yùn)動(dòng)學(xué)分析是在知道機(jī)器人末端位置和姿態(tài)的情況下，通過一系列的算法推導(dǎo)得出對(duì)應(yīng)的機(jī)器人各個(gè)關(guān)節(jié)角的過程。機(jī)器人逆運(yùn)動(dòng)學(xué)求解是一大難題，通常在機(jī)器人末端運(yùn)動(dòng)空間內(nèi)給定機(jī)器人末端位置和姿態(tài)的情況下，機(jī)器人各個(gè)關(guān)節(jié)轉(zhuǎn)角的求解結(jié)果會(huì)有多種[6]。多解的選擇和優(yōu)化直接關(guān)系到最終機(jī)器人運(yùn)動(dòng)軌跡的可行性。本文以FANUC-M10i機(jī)器人為例，采取逆運(yùn)動(dòng)學(xué)求解方法，這樣避開了繁瑣的多次矩陣逆運(yùn)算，在明確正運(yùn)動(dòng)學(xué)方程的基礎(chǔ)上加以變換，只需要1次求逆就可以得到所對(duì)應(yīng)轉(zhuǎn)角的值。

具體計(jì)算過程：第1步將式(10)兩邊左乘一個(gè)逆矩陣進(jìn)行變換，然后利用變換后等式兩邊對(duì)應(yīng)元素一致建立方程，最后用建立的方程求解，把關(guān)節(jié)變量分離出來。將式(10)兩側(cè)左乘逆矩陣得：

(24)

由式(24)中兩側(cè)第3行第4列對(duì)應(yīng)一致求解得：

θ1=atan2(-ayd7+py，±(axd7-px))

(25)

由式(24)兩側(cè)第1行第4列和第2行第4列元素對(duì)應(yīng)一致求解得：

(26)

u=a1+(axd7-px)c1+ayd7s1-pys1

(27)

(28)

v=azd7-pz

(29)

由等式(24)兩側(cè)第1行第4列和第2行第4列元素對(duì)應(yīng)一致求解得：

θ3=atan2(m1，n1)

(30)

m1=[(-a2-uc2+vs2)d6-

(-vc2-us2)a3]/(a32+d62)

(31)

n1=(-a2-uc2+vs2)a3+

(-vc2-us2)d6/(a32+d62)

(32)

由式(24)兩側(cè)第1行第2列和第2行第2列元素對(duì)應(yīng)一致求解得：

(33)

將式(33)兩側(cè)先同時(shí)乘以c3和s3，其次作差，最后化簡(jiǎn)得：

(34)

式中：h=-s23(axc1+ays1)-azc23，且θ5≠0。

將式(33)兩側(cè)先同時(shí)乘以c3和s3，然后相加求和，最后化簡(jiǎn)得：

θ4=atan(m2,n2)

(35)

m2=(ayc1-axs1)/s5

(36)

n2=[(s2s3+c2c3)(axc1+ays1)+

az(c2s3-s2c3)]/s5

(37)

由式(24)兩側(cè)第3行第1列和第3行第3列元素對(duì)應(yīng)一致求解得：

θ6=atan(m3,n3)

(38)

m3=c5s4(oyc1-oxs1)-c4(nxs1-nyc1)

(39)

n3=-c5s4(nys1-nyc1)+c4(oxs1-oyc1)

(40)

上述解的結(jié)果有多個(gè)，最終實(shí)際上只能選用1組解。首先得排除掉在機(jī)器人各個(gè)關(guān)節(jié)行程范圍外的解，其次需要排除在奇異點(diǎn)位置的解，最后剩余的解需要根據(jù)實(shí)際情況來確定選取唯一解。

4 MATLAB進(jìn)行運(yùn)動(dòng)學(xué)方程驗(yàn)證

MATLAB中Robotics工具包是由Peter Croke團(tuán)隊(duì)開發(fā)的基于MATLAB平臺(tái)的機(jī)器人運(yùn)算、仿真的強(qiáng)大工具，可以作為驗(yàn)證相關(guān)機(jī)器人算法的工具[7]。為了驗(yàn)證推導(dǎo)出的正逆運(yùn)動(dòng)學(xué)算法的正確性，可以先用MATLAB進(jìn)行編程計(jì)算；其次在MATLAB中應(yīng)用Robotics工具包建立機(jī)器人模型，工具包可以自動(dòng)進(jìn)行相關(guān)正逆運(yùn)動(dòng)學(xué)解算并得出正確結(jié)果；最后將推導(dǎo)出來的算法對(duì)比MATLAB中的Robotics工具包，若結(jié)果一致，即可證明推導(dǎo)出來的算法是正確無誤的。

(1) 正運(yùn)動(dòng)學(xué)方程驗(yàn)證

不妨取(θ1,θ2,θ3,θ4,θ5,θ6)=(10,20,30,40,50,60)，單位°。在MATLAB中輸入算法進(jìn)行運(yùn)算得出末端位姿矩陣：

T=

在MATLAB機(jī)器人工具箱中建模后，輸入各個(gè)關(guān)節(jié)轉(zhuǎn)角，得出結(jié)果如圖3所示。與T相對(duì)應(yīng)，即證明了正運(yùn)動(dòng)學(xué)方程的正確性。

圖3 機(jī)器人工具箱仿真

(2) 逆運(yùn)動(dòng)學(xué)方程驗(yàn)證

在MATLAB中寫入逆運(yùn)動(dòng)學(xué)算法，輸入矩陣T，得出表2所示結(jié)果。

表2 MATLAB逆解8組解

逆解有8組，由表2可知逆解3為對(duì)應(yīng)的(10,20,30,40,50,60)逆解。細(xì)微的誤差是因?yàn)門取有效數(shù)字時(shí)會(huì)省略一些位數(shù)造成的，對(duì)應(yīng)同一個(gè)機(jī)器人末端位姿。

5 結(jié)束語

本文提出了用一次逆乘法推導(dǎo)出機(jī)器人的逆運(yùn)動(dòng)學(xué)算法的方法。選擇了日本FANUC-M10I機(jī)器人為研究對(duì)象，用D-H法建立了機(jī)器人的連桿坐標(biāo)系，計(jì)算得出各個(gè)相關(guān)參數(shù)，并用一次逆乘法推出正逆運(yùn)動(dòng)學(xué)算法；將推導(dǎo)出的算法在MATLAB中進(jìn)行編程求解，并與機(jī)器人工具箱結(jié)果進(jìn)行對(duì)比。結(jié)果表明1次逆乘法只需要1次矩陣求逆就可以正確推導(dǎo)出逆運(yùn)動(dòng)學(xué)方程，可以免除以往傳統(tǒng)逆運(yùn)動(dòng)學(xué)推導(dǎo)所需的復(fù)雜多次的逆運(yùn)算。