工業(yè)機器人螺旋路徑規(guī)劃的研究與仿真

劉子貴

（江門職業(yè)技術(shù)學(xué)院,廣東,529090)

0 引言

當(dāng)鉆井鉆桿等螺紋有缺陷時，因報廢成本較高，一般都會采用修復(fù)方式：先挖除缺陷，再對缺陷補焊，最后加工去除多余的補焊材料。如若碰到螺紋直徑和螺距較大的情況，其在修復(fù)加工時無法采用常規(guī)的車削、銑削或鏜削，而且成形刀具的制造極其困難。采用工業(yè)機器人補焊或加工螺紋，可以避免螺紋修復(fù)專用設(shè)備的投入，降低企業(yè)運作成本。因此，研究工業(yè)機器人的螺旋路徑規(guī)劃，具有十分重要的應(yīng)用價值。

目前，針對直線、圓弧和規(guī)則曲線的路徑規(guī)劃已經(jīng)非常成熟，但針對螺紋軌跡規(guī)劃尚處于探索和實驗階段。首先必須建立螺旋線模型，然后需要選取數(shù)量合適的節(jié)點和刀具進(jìn)給的步進(jìn)角大小，最后工業(yè)機器人末端目標(biāo)加工位置需要較為復(fù)雜的變換，才能確保機器人能夠走出連續(xù)光滑的螺紋曲線，因此，這比傳統(tǒng)工業(yè)機器人的連續(xù)路徑規(guī)劃更為困難。

1 建立機器人運動學(xué)模型

本文以YASKAWA六自由度串聯(lián)機器人MH5F為研究對象，采用D-H建模法建立機器人坐標(biāo)系，如圖1所示。

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

查詢MH5F機器人的技術(shù)手冊，可以確定圖1中各關(guān)節(jié)連桿長度和連桿距離，a1=88mm，a2=310mm，a3=40mm，d4=305mm，d6=80mm。

MH5F工業(yè)機器人D-H 模型參數(shù)如表1所示。

表1 安川MH5F工業(yè)機器人 D-H 模型參數(shù)

運用Robotics Toolbox 編制如下程序，建立機器人運動學(xué)模型，如圖2所示。

圖2 MH5F機器人在MATLAB的模型

L1=Link('d',0,'a',0,'alpha',pi/2); %定義連桿1

L2=Link('d',0,'a',88,'alpha',0); %定義連桿2

L3=Link('d',0,'a',310,'alpha',pi/2); %定義連桿3

L4=Link('d',305,'a',40,'alpha',-pi/2); %定義連桿4

L5=Link('d',0,'a',0,'alpha',pi/2); %定義連桿5

L6=Link('d',80,'a',0,'alpha',0); %定義連桿6

robot=SerialLink([L1 L2 L3 L4 L5 L6]); %建立機器人模型

robot.display(); %顯示機器人DH參數(shù)

theta=[pi/2 0 0 0 0 0]; %設(shè)定機器人初始關(guān)節(jié)角

robot.plot(theta)。 %顯示機器人模型

2 螺旋線模型建立

圖3 螺旋線參數(shù)方程

根據(jù)傳統(tǒng)的螺紋加工工藝原理，螺紋的加工精度受加工過程中加工刀具每次向前的步進(jìn)角大小影響。因此，將空間螺旋線向平面坐標(biāo)系投影，設(shè)步進(jìn)角為，可獲得半徑為的扇形，如圖4所示。

圖4 螺旋線投影圖

外螺紋有3種螺紋等級，分別為4h、6h和6g，假若取6h等級，查表得最大控制誤差δmax為0.022mm。采用直線段擬合空間螺旋線，根據(jù)圖4可推導(dǎo)出式（1），計算出誤差δ，再經(jīng)過變換得到步進(jìn)角表達(dá)式，即式（2）。為了確保加工誤差不超過δmax，必須讓步進(jìn)角≤0.0593。式（3）中的ceil為MATLAB函數(shù)，向正無窮取整，計算出n為106，n是一個螺距螺旋線上的節(jié)點數(shù)量。步進(jìn)角越小，n越大，擬合精度就越高。

根據(jù)螺旋線參數(shù)方程、式（2）、式（3），利用MATLAB 編寫下列程序，可生成5個螺距的空間螺旋線，如圖5所示。

r=50; %螺紋半徑為50mm

p=6; %螺距為6mm

alpha=2*acos(1-0.022/r); %根據(jù)控制誤差，求得步進(jìn)角

n=ceil(2*pi/alpha); %計算一個螺距的節(jié)點數(shù)量

x0=0; %初始坐標(biāo)x的值

y0=0; %初始坐標(biāo)y的值

z0=0; %初始坐標(biāo)z的值

fi=0:p/n:5*p; %生成參數(shù)變量

x=x0+r*cos(2*pi/p*fi); %生成螺旋線x方向值

y=y0+r*sin(2*pi/p*fi); %生成螺旋線y方向值

z=z0+fi。 %生成螺旋線z方向值

plot3(x,y,z) %繪制空間螺旋線

圖5 空間螺旋線

3 節(jié)點的截取與姿態(tài)

在螺旋線上選取若干個節(jié)點，工業(yè)機器人必須知曉這些節(jié)點的位置和姿態(tài)信息，然后分段執(zhí)行，產(chǎn)生圖5所示的空間螺旋線。

3.1 節(jié)點位置

根據(jù)式（2）、式（3）分割空間螺旋線的依據(jù)和原理，可以通過空間螺旋線的數(shù)學(xué)參數(shù)表達(dá)式計算節(jié)點的位置，運用MATLAB編寫下列程序：

for i=1:5*n+1; %設(shè)置循環(huán)量

To(:,:,i)=transl(x(i),y(i),z(i)); % 生成齊次矩陣

end。 %循結(jié)束

即可得到5n+1個節(jié)點的位置齊次矩陣，也是機器人末端坐標(biāo)原點在機器人坐標(biāo)系中的位置，如下所示:

3.2 節(jié)點姿態(tài)

工業(yè)機器人末端要達(dá)到目標(biāo)加工位置，一般需要經(jīng)過三步變換：

1）工業(yè)機器人的末端坐標(biāo)系先繞參考坐標(biāo)系y軸旋轉(zhuǎn) -90。；

2）再繞z軸旋轉(zhuǎn)180。；

3.3 節(jié)點位姿

由式(4)、式(5)可得機器人末端位姿，即：

利用MATLAB編寫如下程序，求得各節(jié)點位姿，即T(:,:,i)，就是5n+1個節(jié)點位置和姿態(tài)的齊次矩陣序列。

Tp=trotz(pi)*troty(-pi/2); %繞y軸旋轉(zhuǎn)-π/2,再繞z軸旋轉(zhuǎn)π

fio=atan(p/(2*pi*r)); %求螺旋傾角

k=reshape([cos(fio*2*pi/p),sin(fio*2*pi/p),

zeros(1,5*n+1)],5*n+1,3); %生成螺旋軸

for i=1:5*n+1 %生成5n+1個節(jié)點

To(:,:,i)=transl(x(i),y(i),z(i)); %節(jié)點的位置

Tr(:,:,i)=angvec2tr(fi0,k(i,:))*Tp; %節(jié)點的姿態(tài)

RpY(:,:,i)=tr2rpy(Tr(:,:,i)); % 轉(zhuǎn)化為roll、pitch、yaw角

T(:,:,i)=To(:,:,i)*Tr(:,:,i); %節(jié)點位姿

end。

4 螺旋路徑仿真

運行上面的程序，可以獲得各個節(jié)點的齊次坐標(biāo)矩陣。利用逆運動學(xué)方程q(i)=ikine(T,'pinv')，求得每一個節(jié)點的關(guān)節(jié)變量值q。執(zhí)行Robotics Toolbox中的驅(qū)動函數(shù)plot(q)，仿真出工業(yè)機器人的螺旋路徑。程序如下：

q=ikine(T,'pinv'); %求每一個節(jié)點的關(guān)節(jié)變量值

robot.teach; %機器人驅(qū)動

hold on; %保持

plot3(x,y,z); %仿真螺旋線

robot.plot(q)。 %繪制螺旋線

5 結(jié)束語

本文通過Robotics Toolbox建立數(shù)學(xué)模型，獲得螺旋曲線的節(jié)點位置和姿態(tài)以及螺旋線的導(dǎo)程等相關(guān)參數(shù)，令工業(yè)機器人具備了執(zhí)行螺旋線路徑功能，對擴寬其應(yīng)用領(lǐng)域具有重要的應(yīng)用價值。