肖俊明, 韓 偉, 王璦琿, 李繼朋, 梁 旺

(中原工學(xué)院 電子信息學(xué)院，河南 鄭州 450007)

20世紀(jì)60年代初機(jī)器人問世以來，機(jī)器人技術(shù)逐步取得了實(shí)質(zhì)性的進(jìn)步與成果[1]，目前已被應(yīng)用于很多行業(yè)，在航空航天、汽車工業(yè)、海洋探索、醫(yī)療手術(shù)等行業(yè)的高科技領(lǐng)域發(fā)揮著重要作用，促進(jìn)了人們生活的改善。機(jī)械臂是最早應(yīng)用于實(shí)際生產(chǎn)領(lǐng)域的一種機(jī)器人[2]。其中，串聯(lián)機(jī)械臂應(yīng)用最為廣泛，它是由若干通過關(guān)節(jié)連接在一起的連桿組成的具有多個(gè)自由度的開鏈型連桿機(jī)構(gòu)。它的一端固定在基座上，另一端為末端執(zhí)行器，主要進(jìn)行抓取操作，中間部分是由若干帶驅(qū)動(dòng)的轉(zhuǎn)動(dòng)或移動(dòng)關(guān)節(jié)串聯(lián)而成的機(jī)構(gòu)體。機(jī)械臂運(yùn)動(dòng)學(xué)研究的是各連桿關(guān)節(jié)的運(yùn)動(dòng)與機(jī)械臂各部分(一般為末端執(zhí)行器)的位置、姿態(tài)之間的數(shù)學(xué)關(guān)系。研究這種關(guān)系是設(shè)計(jì)機(jī)械臂運(yùn)動(dòng)控制器所必需的。機(jī)械臂運(yùn)動(dòng)學(xué)的研究主要有正運(yùn)動(dòng)學(xué)問題和逆運(yùn)動(dòng)學(xué)問題兩類。由已知機(jī)械臂各關(guān)節(jié)的角度位置和各連桿的長(zhǎng)度，計(jì)算末端執(zhí)行器相對(duì)基坐標(biāo)系原點(diǎn)的位置與姿態(tài)，即為正運(yùn)動(dòng)學(xué)問題。要使末端執(zhí)行器運(yùn)動(dòng)到期望的位置與姿態(tài)，就必須計(jì)算出各關(guān)節(jié)此時(shí)此刻的位置角度，即為逆運(yùn)動(dòng)學(xué)問題。

傳統(tǒng)物理實(shí)體設(shè)計(jì)方法不但費(fèi)時(shí)費(fèi)力，而且很難達(dá)到理想效果。 隨著計(jì)算機(jī)技術(shù)的不斷發(fā)展，通過計(jì)算機(jī)仿真技術(shù)完全能夠設(shè)計(jì)出接近實(shí)物且精度較高的機(jī)械臂運(yùn)動(dòng)模型，經(jīng)過實(shí)際調(diào)試達(dá)到理想效果，這不僅可縮短設(shè)計(jì)周期，同時(shí)能節(jié)省大量生產(chǎn)成本。運(yùn)用Matlab軟件的Robotics Toolbox(即機(jī)器人工具箱)，可便捷地完成機(jī)械臂的空間運(yùn)動(dòng)學(xué)建模、分析與仿真及軌跡規(guī)劃等任務(wù)。通過計(jì)算機(jī)對(duì)機(jī)械臂建模與仿真，不僅能夠得到機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)曲線，而且可直觀地顯示出機(jī)械臂的運(yùn)動(dòng)情況，為機(jī)械臂的設(shè)計(jì)提供理論支撐。

1 六自由度機(jī)械臂運(yùn)動(dòng)學(xué)分析

1.1 機(jī)械臂D-H坐標(biāo)系及連桿參數(shù)

D-H參數(shù)法是Denavit和Hartenberg在1955年提出的，它是為機(jī)械臂關(guān)節(jié)鏈中每個(gè)連桿坐標(biāo)系建立關(guān)系矩陣的一種方法[3-4]。

本文研究對(duì)象為六自由度串聯(lián)型機(jī)械臂，該機(jī)械臂有6個(gè)關(guān)節(jié)，而且均為轉(zhuǎn)動(dòng)型。圖1所示為六自由度機(jī)械臂。圖2所示為六自由度機(jī)械臂的D-H坐標(biāo)系。以各關(guān)節(jié)的中心為坐標(biāo)系原點(diǎn)建立坐標(biāo)系，使坐標(biāo)系{O4}的原點(diǎn)和坐標(biāo)系{O5}的原點(diǎn)重合，以末端執(zhí)行器中心為坐標(biāo)系{O6}的原點(diǎn)，按照D-H參數(shù)法確定的機(jī)械臂連桿參數(shù)如表1所示。

1.2 機(jī)械臂正運(yùn)動(dòng)學(xué)方程

確定全部連桿參數(shù)之后，可按照以下步驟建立相鄰兩連桿i與i-1之間的關(guān)系：

(1) 以第i連桿與第i-1連桿的連接軸作為zi-1坐標(biāo)軸；

(2)xi坐標(biāo)軸沿zi-1軸和zi軸的公垂線，且指向zi軸而背離zi-1軸；

圖1 六自由度機(jī)械臂

圖2 六自由度機(jī)械臂的D-H坐標(biāo)系

表1 按D-H參數(shù)法確定的機(jī)械臂連桿參數(shù)

(3) 坐標(biāo)yi軸雖不關(guān)鍵，但其方向必須滿足與xi軸、zi軸構(gòu)成xiyizi右手直角坐標(biāo)系的條件；

(4) 將xi-1軸繞zi-1軸旋轉(zhuǎn)θi角，使xi-1軸與xi軸同向；

(5) 將坐標(biāo)系{Oi-1}沿zi-1軸平移距離di，使xi-1軸與xi軸在同一條直線上；

(6) 將坐標(biāo)系{Oi-1}沿xi軸平移距離ai，使其坐標(biāo)原點(diǎn)與坐標(biāo)系{Oi}的原點(diǎn)重合；

(7) 將坐標(biāo)系{Oi-1}繞xi軸旋轉(zhuǎn)αi角，使zi-1軸與zi軸在同一條直線上。

經(jīng)上述坐標(biāo)系變換，得到的齊次變換矩陣為：

i-1Ti=Rot(zi-1,θi)Trans(0,0,di)

Trans(ai,0,0)Rot(xi,αi)

(1)

式中：i-1Ti是坐標(biāo)系{Oi-1}變換到坐標(biāo)系{Oi}的齊次變換矩陣；Rot(zi-1,θi)是xi-1軸繞zi-1軸旋轉(zhuǎn)θi角的齊次變換矩陣；Trans(0,0,di)是坐標(biāo)系{Oi-1}沿zi-1軸平移距離di的齊次變換矩陣；Trans(ai,0,0)是坐標(biāo)系{Oi-1}沿xi軸平移距離ai的齊次變換矩陣；Rot(xi,αi)是坐標(biāo)系{Oi-1}繞xi軸旋轉(zhuǎn)αi角的齊次變換矩陣。

由式(1)可推導(dǎo)出：

(2)

將表1中連桿參數(shù)代入式(2)，可得到機(jī)械臂各坐標(biāo)系之間轉(zhuǎn)換的齊次變換矩陣。坐標(biāo)系{O0}到坐標(biāo)系{O1}的變換矩陣為：

(3)

坐標(biāo)系{O1}到坐標(biāo)系{O2}的變換矩陣為：

(4)

坐標(biāo)系{O2}到坐標(biāo)系{O3}的變換矩陣為：

(5)

坐標(biāo)系{O3}到坐標(biāo)系{O4}的變換矩陣為：

(6)

坐標(biāo)系{O4}到坐標(biāo)系{O5}的變換矩陣為：

(7)

坐標(biāo)系{O5}到坐標(biāo)系{O6}的變換矩陣為：

(8)

因此，機(jī)械臂的正運(yùn)動(dòng)學(xué)方程為：

(9)

式中：n=[nxnynz0]T是坐標(biāo)系{O0}相對(duì)于坐標(biāo)系{O6}在x軸上的方向矢量；o=[oxoyoz0]T

是坐標(biāo)系{O0}相對(duì)于坐標(biāo)系{O6}在y軸上的方向矢量；a=[axayaz0]T是坐標(biāo)系{O0}相對(duì)于坐標(biāo)系{O6}在z軸上的方向矢量；p=[pzpypz1]T是坐標(biāo)系{O0}相對(duì)于坐標(biāo)系{O6}的位置矢量。

式(9)表示的是末端執(zhí)行器的位姿與基坐標(biāo)系原點(diǎn)之間的變換關(guān)系。取初始位姿：d1=375.5 mm,d4=268.0 mm,d6=165.5 mm,a2=230.0 mm,a3=107.0 mm,θ1=θ3=θ4=θ5=θ6=0，θ2=-90°，代入式(9)后得到的機(jī)械臂初始位姿矩陣為：

(10)

這與機(jī)械臂處于初始位姿的實(shí)際情況完全一致，說明所得機(jī)械臂正運(yùn)動(dòng)學(xué)方程是正確的。

1.3 機(jī)械臂逆運(yùn)動(dòng)學(xué)方程

根據(jù)機(jī)械臂正運(yùn)動(dòng)學(xué)方程，可對(duì)機(jī)械臂求逆解。

關(guān)節(jié)1轉(zhuǎn)角為：

θ1=arctg-1[(-px+d6·ay)/(-px+d6·ax)]

(11)

關(guān)節(jié)2轉(zhuǎn)角為：

θ2=arctg-1{[(px12+py12)1/2·(-d4c3-a3s3)-pz1·(-d4s3+a3c3+a2)]/[(px12+py12)1/2·(-d4s3+

a3c3+a2)+pz1·(-d4c3-a3s3)]}

(12)

關(guān)節(jié)3轉(zhuǎn)角為：

(13)

關(guān)節(jié)4轉(zhuǎn)角為：

θ4=arctg-1{[-(ax·s1-ay·c1)]/[-(ax·c1c23+ay·s1c23-az·s23)]}

(14)

關(guān)節(jié)5轉(zhuǎn)角為：

θ5=arctg-1{[az·s23c4-ax·(c1c23c4+s1s4)-ay·(s1c23c4-c1s4)]/(-ax·c1s23-ay·s1s23-az·c23)}

(15)

關(guān)節(jié)6轉(zhuǎn)角為：

θ6=arctg-1{[nz·s23s4-nx·(c1c23s4-s1c4)-ny·(s1c23s4+c1c4)]/[oz·s23s4-ox·(c1c23s4-s1c4)-

oy(s1c23s4+c1c4)]}

(16)

式中：px1=c1·(-d4·s23+a3c23+a2c2)；

py1=s1·(-d4s23+a3c23+a2c2);

pz1=-d4c23-a3s23-a2s2；

sij=sin(θi+θj)；

cij=cos(θi+θj)；

si=sinθi；

ci=cosθi。

2 機(jī)械臂工作空間仿真分析

機(jī)械臂的工作空間是指機(jī)械臂的特定部位(一般為末端關(guān)節(jié)坐標(biāo)系原點(diǎn))在一定條件下所能達(dá)到的空間位置集合[5-6]。工作空間的大小反映了機(jī)械臂的活動(dòng)范圍。機(jī)械臂的設(shè)計(jì)工作空間是一個(gè)重要的運(yùn)動(dòng)學(xué)指標(biāo)。本文運(yùn)用蒙特卡羅法[7-8]對(duì)機(jī)械臂的工作空間進(jìn)行仿真分析。

蒙特卡羅法的應(yīng)用步驟和算法流程如圖3所示。

圖3 機(jī)械臂工作空間的仿真算法流程

首先初始化，讀取連桿參數(shù)和坐標(biāo)系，求解機(jī)械臂的正解，并根據(jù)正解得出機(jī)械臂末端在基坐標(biāo)系中的位置向量；然后利用Matlab軟件中的Rand函數(shù)產(chǎn)生[0，1]間的均勻隨機(jī)數(shù)Rand(N,1)，并讀取一列隨機(jī)數(shù)。

則各關(guān)節(jié)的隨機(jī)值為：

(17)

本文結(jié)合該機(jī)械臂模型的實(shí)際參數(shù)，取N=12 000，打印出了機(jī)械臂工作空間及其在空間3個(gè)坐標(biāo)平面上的投影(見圖4)。

為了對(duì)計(jì)算出的機(jī)械臂工作空間進(jìn)行驗(yàn)證，可任取一關(guān)節(jié)角度向量q，并令：

q=[-pi/2-pi/2pi/3pi/2pi/3pi/3]

(18)

代入式(9)可得：

px=-143.327 4 mm,py=-268.039 7 mm,pz=367.241 6 mm。顯然，所得坐標(biāo)在機(jī)械臂末端工作空間內(nèi)。

同理，可取規(guī)定范圍內(nèi)不同的θi值，再次驗(yàn)證機(jī)械臂末端位置，所得位置也在此工作空間內(nèi)。因此，利用蒙特卡羅法計(jì)算機(jī)械臂末端工作空間是可行的。

(a) 機(jī)械臂末端運(yùn)動(dòng)空間的三維空間范圍

(b) 末端運(yùn)動(dòng)空間在xOy坐標(biāo)平面上的投影

(c) 末端運(yùn)動(dòng)空間在xOz坐標(biāo)平面上的投影

(d) 末端運(yùn)動(dòng)空間在yOz坐標(biāo)平面上的投影圖4 機(jī)械臂末端工作空間及其在坐標(biāo)平面上的投影

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

3.1 仿真模型的建立

機(jī)器人工具箱是由澳大利亞科學(xué)家Peter Corke開發(fā)的一套基于Matlab軟件環(huán)境的機(jī)器人學(xué)工具箱[9]。在Matlab系統(tǒng)環(huán)境下，借助Robotics Toolbox可以便捷地建立機(jī)械臂仿真模型(見圖5)。

建立機(jī)械臂仿真模型時(shí)調(diào)用link函數(shù)的格式為：

Li=link([αiaiθidisign],‘standard’)。

式中:αi為連桿扭角;ai為連桿長(zhǎng)度;θi為關(guān)節(jié)轉(zhuǎn)角;di為偏置距離;sign=0；standard為標(biāo)準(zhǔn)D-H參數(shù)。

在圖5(a)中，拖動(dòng)橫條上的小方塊，則相應(yīng)的關(guān)節(jié)轉(zhuǎn)動(dòng)。

機(jī)械臂建模程序如下：

L1=link([-pi/200387.50],‘standard’);

L2=link([0230-pi/200],‘standard’);

L3=link([-pi/2 107000],‘standard’);

L4=link([-pi/2002680],‘standard’);

L5=link([pi/20000],‘standard’);

L6=link([000165.50],‘standard’);

r=robot({L1L2L3L4L5L6},‘六自由度機(jī)械臂’);

plot(r,[0-pi/20000])；

drivebot(r)；

(a) 模型的驅(qū)動(dòng)器

(b) 模型的初始狀態(tài)圖5 機(jī)械臂仿真模型

3.2 機(jī)械臂正、逆運(yùn)動(dòng)學(xué)方程驗(yàn)證

在Matlab的Robotics Toolbox中，利用fkine函數(shù)可實(shí)現(xiàn)機(jī)械臂正運(yùn)動(dòng)學(xué)方程求解。調(diào)用fkine函數(shù)的格式為：T=fikine(robot,q)。其中:robot為所建機(jī)械臂模型；T為q所定義的每個(gè)前向運(yùn)動(dòng)的正解變換矩陣。

為驗(yàn)證運(yùn)動(dòng)學(xué)方程式(9)的正確性，現(xiàn)結(jié)合該機(jī)械臂的實(shí)際情況，隨機(jī)取工作空間中一點(diǎn)Q1的關(guān)節(jié)角度向量q=[-pi/2-pi/2pi/3pi/2pi/3pi/3],通過fkine(robot，q)函數(shù)得到該點(diǎn)相對(duì)基坐標(biāo)原點(diǎn)的變換矩陣如下：

(19)

將關(guān)節(jié)角度向量q代入式(9)，其計(jì)算結(jié)果與式(19)結(jié)果一致，從而驗(yàn)證了機(jī)械臂正運(yùn)動(dòng)學(xué)方程的正確性。

同理,可利用Robotics toolbox中的ikine函數(shù)實(shí)現(xiàn)機(jī)械臂逆運(yùn)動(dòng)學(xué)的求解。調(diào)用ikine函數(shù)的格式為：q′=ikine(robot，T′，q0，M)。其中：robot為所建機(jī)械臂模型；T′為要求反解的變換矩陣；q0為該機(jī)械臂的初始位置向量；M為機(jī)械臂的自由度標(biāo)稱向量，即當(dāng)自由度為6時(shí),M=[111111],若其中一個(gè)元素為0，則表示忽略該關(guān)節(jié)的自由度。結(jié)合該機(jī)械臂仿真模型，取初始位置向量q0=[0-pi/20000]，若機(jī)械臂末端點(diǎn)的坐標(biāo)位置為Q2，則其變換矩陣為：

(20)

再調(diào)用函數(shù)qi=ikine(robot，T′，q0，M)，可得機(jī)械臂在Q2點(diǎn)的關(guān)節(jié)位置向量qi=[0 -1.570 80000]。

由上述正、逆運(yùn)動(dòng)學(xué)方程計(jì)算結(jié)果可知，所建仿真模型是正確的，可用于機(jī)械臂運(yùn)動(dòng)軌跡仿真。

3.3 機(jī)械臂運(yùn)動(dòng)軌跡仿真

在Robotics Toolbox中，可利用jtraj函數(shù)進(jìn)行機(jī)械臂關(guān)節(jié)空間規(guī)劃。調(diào)用jtraj函數(shù)的格式為：[qqdqdd]=jtraj(q0,q1,t)。這里，可將q視為狀態(tài)q0到狀態(tài)q1的關(guān)節(jié)空間規(guī)劃軌跡，qd、qdd分別為該規(guī)劃軌跡的速度和加速度，t為給定的時(shí)間長(zhǎng)度向量。

針對(duì)正運(yùn)動(dòng)學(xué)方程和逆運(yùn)動(dòng)學(xué)方程，在軌跡規(guī)劃后，分別利用fkine函數(shù)進(jìn)行前向求解，可得到關(guān)節(jié)的位置、速度和加速度。定義該機(jī)械臂的起始位置為A點(diǎn)，終止位置為B點(diǎn)，可利用已建立的模型進(jìn)行機(jī)械臂運(yùn)動(dòng)軌跡仿真。其程序如下：

t=0:0.056:2;時(shí)間范圍，0.056為步長(zhǎng)

q0=[0-pi/20000];初始位置

q1=[-pi/2-pi/2pi/3-pi/2pi/3pi/3];要到達(dá)的位置

[q,qd,qdd]=jtraj(q0,q1,t);軌跡規(guī)劃

plot(r,q);畫出機(jī)械臂運(yùn)動(dòng)圖

T=fkine(r,q);計(jì)算變換矩陣

m=squeezeT(i,4,:);提取動(dòng)態(tài)變換矩陣(i=1,2,3)

plot3(x,y,z);畫出空間軌跡

plot(t,q(:,i));畫出角位移隨時(shí)間變化曲線(i=1，2，3，4，5，6)

plot(t,qd(:,i));畫出角速度隨時(shí)間變化曲線

plot(t,qdd(:,i));畫出角加速度隨時(shí)間變化曲線

通過軌跡規(guī)劃，不僅可得到機(jī)械臂末端執(zhí)行器的空間軌跡，而且還能得到各關(guān)節(jié)角位移、角速度和角加速度隨時(shí)間的變化曲線。圖6所示為機(jī)械臂的初始位置。圖7所示為機(jī)械臂的終點(diǎn)位置。圖8所示為機(jī)械臂末端的空間移動(dòng)軌跡。從圖8可看出，機(jī)械臂末端執(zhí)行器達(dá)到了指定的位置，這證明機(jī)械臂運(yùn)動(dòng)模型的設(shè)計(jì)正確。圖9所示為機(jī)械臂末端在空間各平面上的位置變化曲線。

圖6 機(jī)械臂在起點(diǎn)的位置狀態(tài)

圖7 機(jī)械臂到達(dá)終點(diǎn)的位置狀態(tài)

關(guān)節(jié)1的角位移時(shí)間曲線、角速度時(shí)間曲線及角加速度時(shí)間曲線都是連續(xù)平穩(wěn)的(見圖10)。受篇幅所限，這里省略了關(guān)節(jié)2～6的角位移、角速度和角加速度時(shí)間變化曲線。綜上所述，在理論上，該機(jī)械臂各關(guān)節(jié)和各運(yùn)動(dòng)部件的運(yùn)行是平穩(wěn)的。

圖8 機(jī)械臂末端的空間移動(dòng)軌跡

圖9 機(jī)械臂末端在空間各平面上的位置變化曲線

圖10 關(guān)節(jié)1角位移、角速度和角加速度隨時(shí)間的變化曲線

4 結(jié) 語

利用D-H參數(shù)法建立機(jī)械臂的空間坐標(biāo)系，并且建立了機(jī)械臂運(yùn)動(dòng)學(xué)方程。運(yùn)用蒙特卡羅法在Matlab軟件中對(duì)機(jī)械臂的工作空間進(jìn)行仿真分析，得到機(jī)械臂的空間工作范圍，并驗(yàn)證了其可行性和正確性。利用Matlab中的Robotics Toolbox建立機(jī)械臂仿真模型，對(duì)機(jī)械臂的正、逆運(yùn)動(dòng)學(xué)方程進(jìn)行了驗(yàn)證。通過運(yùn)動(dòng)軌跡規(guī)劃仿真，得到了機(jī)械臂末端執(zhí)行器的空間軌跡及各關(guān)節(jié)的角位移、角速度和角加速度隨時(shí)間變化的曲線。本文能夠?yàn)闄C(jī)械臂控制系統(tǒng)設(shè)計(jì)、動(dòng)力學(xué)分析和軌跡規(guī)劃提供借鑒。