馬浩然，巴 鵬

（沈陽(yáng)理工大學(xué)機(jī)械工程學(xué)院，沈陽(yáng) 110159）

0 引言

隨著中國(guó)制造對(duì)世界的影響越來(lái)越大，中國(guó)的制造業(yè)急速發(fā)展，工業(yè)機(jī)器人在自動(dòng)化生產(chǎn)中越來(lái)越重要。針對(duì)大型壓縮機(jī)氣閥體積、質(zhì)量大不方便更換的問題，需要特定的裝配機(jī)器人代替人來(lái)替換氣閥。機(jī)器人的運(yùn)動(dòng)學(xué)分析是研究機(jī)器人學(xué)的基礎(chǔ)，是確定機(jī)器人在特定工況下動(dòng)力學(xué)分析、軌跡規(guī)劃和運(yùn)動(dòng)控制的前提[1]。因此，要驗(yàn)證氣閥裝配機(jī)器人的結(jié)構(gòu)合理，就需要進(jìn)行運(yùn)動(dòng)學(xué)分析。目前國(guó)內(nèi)外通常都會(huì)以六軸的工業(yè)機(jī)器人為分析對(duì)象，進(jìn)行運(yùn)動(dòng)學(xué)分析和仿真[2]。高東強(qiáng)等[3]對(duì)傳統(tǒng)的SCARA 機(jī)器人進(jìn)行分析與仿真。

通過D-H法，在MATLAB中建立關(guān)于氣閥裝配機(jī)器人的數(shù)學(xué)模型和相對(duì)應(yīng)的運(yùn)動(dòng)學(xué)方程。使用機(jī)器人工具箱建立簡(jiǎn)易的氣閥裝配機(jī)器人模型，再通過蒙特卡羅法得到氣閥裝配機(jī)器人的工作空間和末端點(diǎn)位姿，判斷其是否在工作空間內(nèi)，再通過幾何法求出運(yùn)動(dòng)學(xué)逆解。最后根據(jù)約束條件，對(duì)獲得的解進(jìn)行篩選，排除不在約束條件內(nèi)部的解。

1 機(jī)器人基本數(shù)據(jù)建立

對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模和分析時(shí)，D-H （Denavit-Hartenberg）表示法是一種非常簡(jiǎn)單且廣泛應(yīng)用的方法，為進(jìn)行微分運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)分析等后續(xù)的推導(dǎo)提供了基礎(chǔ)，可用于任何機(jī)器人構(gòu)型，而與機(jī)器人的結(jié)構(gòu)順序和復(fù)雜程度無(wú)關(guān)[4]。用齊次變換來(lái)描述各個(gè)連桿相對(duì)于固定參考坐標(biāo)系的空間幾何關(guān)系，Denavit 和Hartenberg 在1955 年提出了一種為關(guān)節(jié)鏈中的每一桿件建立附體坐標(biāo)系的矩陣方法，即對(duì)每個(gè)關(guān)節(jié)處的桿件坐標(biāo)系建立4×4 齊次變換矩陣，用來(lái)描述機(jī)器人各桿件相對(duì)于參考坐標(biāo)系的空間幾何關(guān)系，從而推導(dǎo)出連桿的位姿[5]。

氣閥裝配機(jī)器人的連桿參數(shù)和關(guān)節(jié)用D-H法計(jì)算出。圖1所示為機(jī)器人各桿件相對(duì)于參考坐標(biāo)系的空間幾何關(guān)系；6個(gè)關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)，得出D-H參數(shù)表，如表1所示。

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

2 氣閥裝配機(jī)器人運(yùn)動(dòng)學(xué)分析

機(jī)器人運(yùn)動(dòng)學(xué)分析包括正、逆運(yùn)動(dòng)學(xué)分析。正運(yùn)動(dòng)學(xué)分析是已知所有的關(guān)節(jié)和連桿變量，推導(dǎo)出與機(jī)器人特定構(gòu)型有關(guān)的方程，計(jì)算出任一瞬間機(jī)器人的位姿；而逆運(yùn)動(dòng)學(xué)分析是使驅(qū)動(dòng)機(jī)器人達(dá)到期望位置，求解機(jī)器人每一關(guān)節(jié)和連桿變量[6-8]。

表1 機(jī)器人連桿參數(shù)

2.1 機(jī)器人正運(yùn)動(dòng)學(xué)分析

建立氣閥裝配機(jī)器人的正運(yùn)動(dòng)學(xué)方程時(shí)，4個(gè)運(yùn)動(dòng)變換矩陣的乘積為機(jī)器人基座與末端執(zhí)行器之間的總變換則。

根據(jù)式（1）可以看出氣閥裝配機(jī)器人的正運(yùn)動(dòng)學(xué)方程是通過機(jī)器人對(duì)應(yīng)6個(gè)變換矩陣的6個(gè)自由度確定的：

2.2 機(jī)器人工作空間計(jì)算

蒙特卡羅法，也稱統(tǒng)計(jì)模擬方法，隨著計(jì)算機(jī)的出現(xiàn)和科技的不斷進(jìn)步，在20世紀(jì)40年代中期提出的一種通過使用偽隨機(jī)數(shù)和隨機(jī)數(shù)解決發(fā)生的問題的數(shù)值計(jì)算方法，其基本思想是概率統(tǒng)計(jì)理論。進(jìn)行模擬時(shí)，首先把得到的概率分布的隨機(jī)變量，通過統(tǒng)計(jì)方法估計(jì)出模型對(duì)應(yīng)的數(shù)字特征，就可以得出與結(jié)果有關(guān)的數(shù)值解。

把事物運(yùn)動(dòng)分為幾何數(shù)量和幾何特征兩類，通過數(shù)學(xué)方法模擬出事物的運(yùn)動(dòng)，即進(jìn)行一種數(shù)字模擬實(shí)驗(yàn)。通過最基本的概率模型，描繪出運(yùn)動(dòng)的過程，再通過模擬實(shí)驗(yàn)結(jié)果，作為問題的近似解。

對(duì)于氣閥裝配機(jī)器人工作空間的求解問題，首先要產(chǎn)生每個(gè)關(guān)節(jié)的隨機(jī)變量，對(duì)其正運(yùn)動(dòng)學(xué)計(jì)算。相比于其他方法，通過蒙特卡洛法進(jìn)行計(jì)算用時(shí)較短可以節(jié)省計(jì)算時(shí)間。

2.3 機(jī)器人逆運(yùn)動(dòng)學(xué)分析

逆運(yùn)動(dòng)學(xué)求解是通過已經(jīng)得知?dú)忾y裝配機(jī)器人的末端位置和姿態(tài)，計(jì)算出這一時(shí)刻末端6個(gè)關(guān)節(jié)軸相對(duì)于初始軸所旋轉(zhuǎn)的角度。由圖1可以看出，氣閥裝配機(jī)器人的后3個(gè)關(guān)節(jié)軸相交于一點(diǎn)，是符合Pieper準(zhǔn)則的。

可以將機(jī)械臂逆運(yùn)動(dòng)學(xué)的求解分為兩部分：因?yàn)闅忾y裝配機(jī)器人符合Pieper準(zhǔn)則，可以先使用幾何法計(jì)算出前3個(gè)關(guān)節(jié)角度，并不涉及到角度4、5 和6 的情況。機(jī)械臂整體的旋轉(zhuǎn)是通過角度1實(shí)現(xiàn)的，計(jì)算時(shí)不需要考慮Z軸的影響，即可投影到XOY坐標(biāo)系計(jì)算。角度2和角度3并不涉及到機(jī)械臂的整體旋轉(zhuǎn)，同理在計(jì)算中可投影到坐標(biāo)系XOZ 或YOZ。完成角度1、2和3計(jì)算后，將其結(jié)果代入機(jī)械臂的DH矩陣中，以便后續(xù)的計(jì)算。

A4和A5的乘積可以通過計(jì)算得出，將算出的角度1、2和3 代入后得到和T5的乘積，這兩個(gè)結(jié)果是相等的矩陣。因此，建立等式，將兩個(gè)矩陣做對(duì)比，獲取一邊為常數(shù)，另一邊為單個(gè)未知數(shù)的兩個(gè)元素。對(duì)方程組進(jìn)行計(jì)算，分別求出角度4、5和6。

3 仿真驗(yàn)證

3.1 氣閥裝配機(jī)器人的正運(yùn)動(dòng)學(xué)仿真

根據(jù)表1 中的關(guān)節(jié)和連桿參數(shù)，利用機(jī)器人工具包中的link和robot函數(shù)來(lái)建立名為“sixlink”的機(jī)器人對(duì)象。得到的簡(jiǎn)易三維建模圖像如圖2所示。

圖2 機(jī)器人三維模型圖

根據(jù)所求的正運(yùn)動(dòng)方程，設(shè)定各關(guān)節(jié)的初始位置q0=[0 pi/2 0 pi pi 0]和終止位置q1=[pi,pi/2,-pi,pi/2,pi/4,pi]。用jtraj（）函數(shù)規(guī)劃氣閥裝配機(jī)器人關(guān)節(jié)空間的關(guān)節(jié)運(yùn)動(dòng)軌跡，結(jié)果如圖3所示。

圖3 各關(guān)節(jié)的位移曲線

圖4 機(jī)器人末端在X、Y、Z方向上的位移曲線

利用fkine（）函數(shù)計(jì)算出氣閥裝配機(jī)器人末端執(zhí)行器的起點(diǎn)與終點(diǎn)位姿，在工作空間中用Ctraj（）函數(shù)規(guī)劃出氣閥裝配機(jī)器人從起點(diǎn)到終點(diǎn)位置的運(yùn)動(dòng)軌跡。其在X、Y、Z 方向的運(yùn)動(dòng)軌跡如圖4所示。

實(shí)現(xiàn)其仿真程序?yàn)?/p>

q0=[0 pi/2 0 pi pi 0]

q1=[pi/4,pi/3,-pi/5,-pi/3,pi/4,pi/6]

t=[0:0.05:3]

q=jtraj(q0,q1,t);

plot(t,q);

T=robot.fkine(q)

x=squeeze(T(1,4,:))

plot(t,x)

Tc=ctraj(p0,p1,step);

Tjtraj=transl(Tc);

subplot(3,2,5);

plot2(Tjtraj,′r′);

title(′p1到p2直線軌跡′);

grid on;

由圖可知，氣閥裝配機(jī)器人的各關(guān)節(jié)與末端執(zhí)行器在起點(diǎn)到終點(diǎn)的位置可以柔滑穩(wěn)定地運(yùn)動(dòng)，證明氣閥裝配機(jī)器人有合理的結(jié)構(gòu)。

3.2 機(jī)器人全工作空間仿真

蒙特卡羅法以概率統(tǒng)計(jì)為指導(dǎo)，使用隨機(jī)數(shù)產(chǎn)生各關(guān)節(jié)的隨機(jī)變量，再用統(tǒng)計(jì)方法把模型的數(shù)字特征估計(jì)出來(lái)，從而計(jì)算正解。在MATLAB中，選取100 000個(gè)點(diǎn)計(jì)算得到的結(jié)果如圖5所示。

圖5 機(jī)器人的工作空間

仿真程序?yàn)椋?/p>

q1_s=-180;q1_end=180;

q2_s=-120; q2_end=40;

q3_s=-60; q3_end=90;

q4_s=-120;q4_end=120;

q5_s=-120; q5_end=120;

q6_s=-360; q6_end=360;

num=100000;

q1_rand=q1_s+rand(num,1)*(q1_end-q1_s);

q2_rand=q2_s+rand(num,1)*(q2_end-q2_s);

q3_rand=q3_s+rand(num,1)*(q3_end-q3_s);

q4_rand=q4_s+rand(num,1)*(q4_end-q4_s);

q5_rand=q5_s+rand(num,1)*(q5_end-q5_s);

q6_rand=rand(num,1)*0;

q=[q1_rand q2_rand q3_rand q4_rand q5_rand q6_rand];

tic;

T_cell=cell(num,1);

[T_cell{:,1}]=six_link.fkine(q).t;

disp([′運(yùn)行時(shí)間′,num2str(toc)]);

t1=clock;

figure(′name′,′機(jī)械臂工作空間′)

hold on

plotopt={′noraise′,′nowrist′,′nojaxes′,′delay′,0};

six_link.plot([0 pi/2 0 pi pi 0],plotopt{:},′tilesize′,2000);

figure_x=zeros(num,1);

figure_y=zeros(num,1);

figure_z=zeros(num,1);

for cout=1:1:num

figure_x(cout,1)=T_cell{cout}(1);

figure_y(cout,1)=T_cell{cout}(2);

figure_z(cout,1)=T_cell{cout}(3);

end

plot3(figure_x,figure_y,figure_z,′r.′,′MarkerSize′,3);

hold off

disp([′繪制工作空間運(yùn)行時(shí)間′,num2str(etime(clock,t1))]);

Point_range=[min(figure_x) max(figure_x) min(figure_y)max(figure_y)min(figure_z)max(figure_z)];

3.3 機(jī)器人逆運(yùn)動(dòng)學(xué)仿真

先對(duì)氣閥裝配機(jī)器人進(jìn)行正運(yùn)動(dòng)學(xué)仿真，獲取其末端執(zhí)行器的位姿，判斷末端執(zhí)行器的坐標(biāo)位置是否超出氣閥裝配機(jī)器人全工作空間的范圍，如果在范圍之內(nèi)才開始進(jìn)行逆解的計(jì)算。然后將6個(gè)關(guān)節(jié)角度分為2組，通過幾何法計(jì)算不需要涉及到角度4、5 和6 的關(guān)節(jié)角度1、2 和3。計(jì)算求出的機(jī)器人末端坐標(biāo)如圖6所示。

圖6 機(jī)器人末端坐標(biāo)

輸入末端坐標(biāo)，判斷是否在工作空間內(nèi)，根據(jù)約束條件對(duì)多個(gè)逆解進(jìn)行篩選，排除不在約束范圍條件內(nèi)部的解，并根據(jù)路徑最短原則選取最優(yōu)解作為最終結(jié)果。逆解所求得的各關(guān)節(jié)角度如圖7所示。

圖7 機(jī)器人逆解

逆解的仿真程序如下

T06=[T_start(1:3,1:3)[X;Y;Z];

0 0 0 1];

T5=T06*inv(A{6});

X_t=double(T5(1,4));

Y_t=double(T5(2,4));

Z_t=double(T5(3,4));

r_squre=X_t^2+Y_t^2;

r_gen=sqrt(r_squre);

S=Z_t-L1;

R_squre=S^2+r_squre;

R_gen=sqrt(R_squre);

theta11=atan2(Y_t,X_t);

theta11=double(real(theta11));

a33=sqrt(L3^2+L4^2);

a33_angle=atan(L4/L3);

cos_theta3_bu=(L2^2+a33^2-R_squre)/(2*L2*a33);

theta33=pi-acos(cos_theta3_bu)-a33_angle;

theta33=double(theta33);

cos_theta2_bu=(L2^2+R_squre-a33^2)/(2*L2*R_gen);

if S＞0

theta22=pi/2-acos(cos_theta2_bu)-atan(S/r_gen);

else

theta22=pi/2-acos(cos_theta2_bu)+atan(-S/r_gen);

end

theta22=double(theta22);

A11=trotz(L(1,1) + theta11)*transl(0,0,L(1,2))*trotx(L(1,4))*transl(L(1,3),0,0);

A22=trotz(L(2,1) + theta22)*transl(0,0,L(2,2))*trotx(L(2,4))*transl(L(2,3),0,0);

A33=trotz(L(3,1) + theta33)*transl(0,0,L(3,2))*trotx(L(3,4))*transl(L(3,3),0,0);

T321=inv(A11*A22*A33);

T45=simplify(A{4}*A{5});

kimejer=simplify(T321*T5);

str_solve=kimejer(3,2);

AA=double(subs(str_solve,[cos(theta6),sin(theta6)],[1,0]));

BB=double(subs(str_solve,[cos(theta6),sin(theta6)],[0,1]));

theta66=atan(-AA/BB);

kimejer2=simplify(subs(kimejer,theta6,theta66));

theta441=double(atan(kimejer2(2,1)/kimejer2(1,1)));

if(theta441*radian)＜q4_end&&(theta441*radian)＞q4_s

theta44=theta441;

end theta551=double(atan2(-kimejer2(3,1),kimejer2(3,3))-pi/2);

if(theta551*radian)＜q5_end&&(theta551*radian)＞q5_s

theta55=theta551;

end

與機(jī)器人初始位置各關(guān)節(jié)角度作比較可以看出基本相同，證明了進(jìn)行運(yùn)動(dòng)學(xué)逆解的方法有效。

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

利用D-H法對(duì)氣閥裝配機(jī)器人進(jìn)行分析，得到正逆運(yùn)動(dòng)學(xué)模型及末端姿態(tài)等。使用MATLAB 對(duì)氣閥裝配機(jī)器人的工作空間進(jìn)行計(jì)算，得到其范圍。利用MATLAB 中的機(jī)器人模塊對(duì)氣閥裝配機(jī)器人進(jìn)行正運(yùn)動(dòng)學(xué)仿真，并通過蒙特卡洛法計(jì)算出機(jī)器人的工作空間，之后排除掉不在工作空間內(nèi)的解更精確地得到運(yùn)動(dòng)學(xué)逆解，簡(jiǎn)便了逆運(yùn)動(dòng)學(xué)的分析過程，為后續(xù)的路徑規(guī)劃、動(dòng)態(tài)控制等研究奠定基礎(chǔ)。