本發(fā)明涉及一種基于數(shù)控刀位文件的機器人末端軌跡規(guī)劃方法，尤其針對機器人軌跡規(guī)劃中末端執(zhí)行器空間位置和姿態(tài)的準(zhǔn)確控制。

技術(shù)背景

工業(yè)機器人作為現(xiàn)代智能制造業(yè)的重要裝備，其技術(shù)水平代表了一個國家制造業(yè)的發(fā)展水平。雖然利用刀位文件在五軸機床實現(xiàn)零件的加工已得到廣泛應(yīng)用，但對于工業(yè)機器人來說還相對滯后，國內(nèi)的研究水平與國外還有較大差距，而傳統(tǒng)示教方法無法對機器人末端執(zhí)行器姿態(tài)進(jìn)行準(zhǔn)確控制，這就加大了空間曲面加工中機器人末端姿態(tài)控制的難度。一般來說機器人不接受基于CAM軟件的刀位文件，要想實現(xiàn)工業(yè)機器人的加工，必須要尋求一種從刀位文件到機器人軌跡規(guī)劃的轉(zhuǎn)換方式，來提高機器人軌跡規(guī)劃效率，擴大機器人應(yīng)用領(lǐng)域。

對基于數(shù)控刀位文件的機器人末端軌跡規(guī)劃方法，文獻(xiàn)《基于CAM的機器人拋光軌跡規(guī)劃》（韓光超,孫明,張海鷗,等. 基于CAM的機器人拋光軌跡規(guī)劃[J].華中科技大學(xué)學(xué)報（自然科學(xué)版）,2008,36(5):60-62.）利用UG CAM軟件中的多軸銑加工功能模塊獲得型腔表面信息，然后采用輔助區(qū)域驅(qū)動法在復(fù)雜型腔表面映射生成連續(xù)的多軸數(shù)控加工軌跡，然后根據(jù)拋光工藝要求調(diào)節(jié)工藝參數(shù)，并將多軸數(shù)控加工軌跡轉(zhuǎn)化成機器人拋光軌跡。該方法最大的限制在于所加工曲面的CAD模型必須按行和列有序地排列，相鄰曲面必須共享同一條邊緣，不利于普遍應(yīng)用。文獻(xiàn)《數(shù)控加工機器人后置處理的研究及軟件實現(xiàn)》（陳勝奮,謝明紅. 數(shù)控加工機器人后置處理的研究及軟件實現(xiàn)[J].機床與液壓,2015,43(3):29-33.）以pro/E軟件生成的刀位文件，提取其刀位信息，通過坐標(biāo)系的轉(zhuǎn)換推導(dǎo)出機器人坐標(biāo)系下的末端位置，在機器人末端執(zhí)行器空間姿態(tài)上，固定第六軸的轉(zhuǎn)動，利用G代碼中的偏轉(zhuǎn)角來確定機器人末端執(zhí)行器的空間姿態(tài)并去除冗余自由度，該方法將五軸數(shù)控加工的刀位文件和G代碼結(jié)合來實現(xiàn)機器人末端執(zhí)行器空間位姿的確定，利用數(shù)控刀位文件對無法同時對機器人末端執(zhí)行器位置和姿態(tài)的控制，與本發(fā)明所述用數(shù)控刀位文件一次性得到機器人末端執(zhí)行器位置和姿態(tài)相比轉(zhuǎn)換過程較為繁瑣，編程也不易實現(xiàn)。

技術(shù)實現(xiàn)要素：

針對現(xiàn)有技術(shù)中存在的不足，本發(fā)明提供一種基于數(shù)控刀位文件的機器人末端軌跡規(guī)劃方法，該方法可實現(xiàn)由數(shù)控刀位文件同時對機器人末端執(zhí)行器位置和姿態(tài)進(jìn)行準(zhǔn)確控制。

為實現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案如下：一種基于數(shù)控刀位文件的機器人末端軌跡規(guī)劃方法，包括以下步驟：

步驟1）機器人基坐標(biāo)系下末端執(zhí)行器位姿的計算；

步驟2）根據(jù)機器人結(jié)構(gòu)參數(shù)對其逆運動學(xué)分析，建立逆運動學(xué)模型；

步驟3）基于逆運動學(xué)模型計算關(guān)節(jié)角；

步驟4）利用關(guān)節(jié)角輸出機器人末端軌跡。

所述步驟1中，機器人基坐標(biāo)系下末端執(zhí)行器位姿的計算，包括以下步驟：

1）提取數(shù)控加工刀位文件信息，利用刀位文件中同一條加工軌跡上相鄰兩個坐標(biāo)點，計算機器人在工件坐標(biāo)系下的末端執(zhí)行器位姿A；

提取基于UG的數(shù)控刀位文件，其格式為GOTO//x_r,y_r,z_r,i_r,j_r,k_r，根據(jù)刀位文件信息中的空間位置在x方向分量x_r、y方向分量y_r、z方向分量z_r，空間姿態(tài)在x方向分量i_r、y方向分量j_r、z方向分量k_r，可直接得到刀軸向量N和末端位置向量P：

N=(n_x,n_y,n_z)=(i_r, j_r,k_r) (1)

P=(l_x,l_y,l_z)=(x_r,y_r,z_r) (2)

其中，(n_x,n_y,n_z)分別是刀軸向量N在x、y、z方向的分量，(l_x,l_y,l_z)分別是末端位置向量P在x、y、z方向上的分量；

假設(shè)數(shù)控刀位文件中同一條加工軌跡上第一點的坐標(biāo)為(x_r，y_r，z_r)，第二點的坐標(biāo)為(x_r+1，y_r+1，z_r+1)，將N平移到以(x_r，y_r，z_r)為起點的位置，相對應(yīng)的坐標(biāo)值分別加上(x_r，y_r，z_r)的坐標(biāo)值，過點(x_r，y_r，z_r)作與向量(x_r-i_r，y_r-j_r，z_r-k_r)垂直的平面S，過空間點(x_r+1，y_r+1)作平行于z軸的直線L，直線L與平面S的交于點w，向量(x_r-i_r，y_r-j_r，z_r-k_r)與向量(x_r-x_r+1，y_r-y_r+1，z_r-z)相互垂直，相乘為0，可求得通過點(x_r+1，y_r+1)的z方向的值g，將向量（x_r+1-x_r， y_r+1-y_r， g-z_r）簡化為單位向量，可求得切向向量T：

g=(((x_r+1-x_r)i_r+(y_r+1-y_r)j_r)/-k_r)+z_r, (3)

o_x= (x_r+1-x_r)/),

o_y=(y_r+1-y_r)/ ),

o_z=(z_r+1-z_r)/ ),

T=(o_x,o_y,o_z) (4)

其中，(o_x,o_y,o_z)分別是切向向量T在x、y、z方向的分量；

法向向量N和切向向量T叉乘可得到副法向向量B：

B=N×T=(a_x,a_y,a_z) (5)

其中，(a_x, a_y, a_z)分別是副法向向量B在x、y、z方向的分量；

由刀軸向量N、切向向量T、副法向向量B和末端位置向量P可求得在工件坐標(biāo)系下的機器人末端位置姿態(tài)A：

A=[n_x o_x a_xl_x;n_y o_y a_yl_y;n_z o_z a_zl_z;0 0 0 1] (6)

2）對工件坐標(biāo)系進(jìn)行標(biāo)定，將工件坐標(biāo)系下的末端位姿轉(zhuǎn)換為機器人基坐標(biāo)系下的末端位姿，主要包括以下內(nèi)容：

對工件坐標(biāo)系進(jìn)行標(biāo)定，規(guī)定工件坐標(biāo)系的空間姿態(tài)與機器人基坐標(biāo)系空間姿態(tài)一致，在機器人工作空間范圍內(nèi)只需對工件坐標(biāo)系的空間位置進(jìn)行標(biāo)定，假設(shè)工件坐標(biāo)系的標(biāo)定值為（h_x,h_y,h_z），通過相應(yīng)的坐標(biāo)變換就可得到在機器人基坐標(biāo)系下的末端位姿U：

U=[n_x o_x a_x p_x;n_y o_y a_y p_y;n_z o_z a_z p_z;0 0 0 1]

其中（p_x,p_y,p_x）為機器人基坐標(biāo)系下的末端位置在x、y、z方向的分量，p_x=l_x+h_x，p_y=l_y+h_y，p_z=l_z+h_z。

所述步驟2中，根據(jù)機器人結(jié)構(gòu)參數(shù)對其逆運動學(xué)分析，建立逆運動學(xué)模型，包括以下內(nèi)容：

根據(jù)工業(yè)機器人連桿參數(shù)對其逆運動分析，建立機器人六個關(guān)節(jié)角逆運動學(xué)數(shù)學(xué)模型，其數(shù)學(xué)模型如下所示：

q₁=arctan2((p_x-d₆a_x),(p_y-d₆a_y))±π/2, (7)

令 k₁=cos(q₁)a_x+ sin(q₁)a_y,

k₂= cos(q₁)p_x-a₁+ sin(q₁)p_y,

k₃=p_z,

k₃₁=k₂-d₆k₁,

k₃₂=d₆a_z-k₃,

k₄=((a₂)²-(d₄)²+(k₃₁)²+(k₃₂)²)/(2a₂),

r= , 可求得關(guān)節(jié)2的關(guān)節(jié)角q₂，

q₂=arctan2((k4/r),± ), (8)

令 k₅=a₂-cos(q₂)k₃₁- sin(q₂)k₃₂ ,

k₅₁= cos(q₂)k₃₂- sin(q₂)k₃₁, 可求得關(guān)節(jié)3的關(guān)節(jié)角q₃，

q₃=arctan2(k₅,k₅₁) , (9)

令 k₆=sin(q₂₊q₃)k₁+cos(q₂₊q₃)a_z , 可求得關(guān)節(jié)5的關(guān)節(jié)角q₅，

q₅=arctan2(±),-k₆), (10)

令 k₇=(cos(q₁)a_y-sin(q₅)a_x)/sin(q₅),

k₈=(sin(q₂)a_z-cos(q₂)k₁-sin(q₃)cos(q₅))/(cos(q₃)sin(q₅)), 可求得關(guān)節(jié)4的關(guān)節(jié)角q₄，

q₄=arctan2(k₇,k₈), (11)

令 k₁₀=cos(q₁)n_y-sin(q₁)n_x,

k₁₁=sin(q₁)o_x-cos(q₁)o_y,

k₁₂=sin(q₄)cos(q₅)/cos(q₄), 可求得關(guān)節(jié)6的關(guān)節(jié)角q₆，

q₆=arctan2((k₁₀+k₁₁k₁₂),(k₁₂k₁₀-k₁₁))+ π , (12)

其中，a₁為關(guān)節(jié)0與關(guān)節(jié)1沿公垂線的距離，a₂為關(guān)節(jié)1與關(guān)節(jié)2沿公垂線的距離，d₄為關(guān)節(jié)4軸線兩個公垂線的距離，d₆為關(guān)節(jié)6兩個公垂線的距離；

由上述數(shù)學(xué)表達(dá)式可知，q₁、q₂、q₅分別對應(yīng)有兩組不同解，通過數(shù)列組合可知機器人關(guān)節(jié)角共有八組不同解。

所述步驟3中，基于逆運動學(xué)模型計算關(guān)節(jié)角，包括以下內(nèi)容：

1）將機器人基坐標(biāo)系下的末端位姿帶入機器人逆運動學(xué)模型中求解關(guān)節(jié)角；

2）將每組六個關(guān)節(jié)角值分別取絕對值，然后相加求和，利用最短路程法優(yōu)化關(guān)節(jié)角，比較八組值，選取最小那一組作為最優(yōu)關(guān)節(jié)角。

所述步驟4中，利用關(guān)節(jié)角輸出機器人末端軌跡，主要包括：

將得到的一系列優(yōu)化后的關(guān)節(jié)角，利用MATLAB軟件編程，得到機器人末端位置軌跡點，輸出機器人末端軌跡。

本發(fā)明的有益效果是：

1、可對機器人末端軌跡規(guī)劃時末端執(zhí)行器姿態(tài)進(jìn)行準(zhǔn)確控制，可有效避免機器人軌跡規(guī)劃時刀具與工件的碰撞；

2、可實現(xiàn)由刀位文件到工業(yè)機器人關(guān)節(jié)角的轉(zhuǎn)換，實現(xiàn)工業(yè)機器人基于數(shù)控加工刀位文件的軌跡規(guī)劃。

附圖說明

圖1是本發(fā)明的步驟流程圖；

圖2是本發(fā)明的姿態(tài)計算示意圖；

圖3是本發(fā)明所使用的葉輪流道面數(shù)控精加工軌跡圖；

圖4是本發(fā)明的機器人末端軌跡圖。

具體實施方式

下面結(jié)合附圖和具體實施方式對本發(fā)明進(jìn)一步詳細(xì)的說明。本實施例在以本發(fā)明內(nèi)容為前提下進(jìn)行，給出了詳細(xì)的實施步驟，但本發(fā)明的保護(hù)范圍不限于下述的實施例。

本發(fā)明實施對象為葉輪流道面的精加工，其數(shù)控加工軌跡如圖3所示，使用工業(yè)機器人型號為瑞士IRB140，流道面精加工第一點x₁=-40.8181mm，y₁=13.0743mm，z₁=-3.0148mm，i₁=-0.8204，j₁=0.5520，k₁=0.1494，第二點x₂=-41.0437mm，y₂=13.0898mm。關(guān)節(jié)0與關(guān)節(jié)1沿公垂線的距離a₁=70mm，關(guān)節(jié)1與關(guān)節(jié)2沿公垂線的距離a₂=360mm，關(guān)節(jié)4軸線兩個公垂線的距離d₄=380mm，關(guān)節(jié)6兩個公垂線的距離d₆=65mm。根據(jù)圖1步驟流程圖，本發(fā)明一種基于數(shù)控刀位文件的機器人末端軌跡規(guī)劃方法，包括以下步驟：

步驟1）機器人基坐標(biāo)系下末端執(zhí)行器位姿的計算；

步驟2）根據(jù)機器人結(jié)構(gòu)參數(shù)對其逆運動學(xué)分析，建立逆運動學(xué)模型；

步驟3）基于逆運動學(xué)模型計算關(guān)節(jié)角；

步驟4）利用關(guān)節(jié)角輸出機器人末端軌跡。

所述步驟1中，機器人基坐標(biāo)系下末端執(zhí)行器位姿的計算，包括以下內(nèi)容：

1）提取基于UG的數(shù)控加工刀位文件信息，利用刀位文件中同一條加工軌跡上相鄰兩個坐標(biāo)點，計算機器人在工件坐標(biāo)系下的末端執(zhí)行器位姿A；

以同一條加工軌跡上前兩個加工坐標(biāo)點為例介紹在工件坐標(biāo)系下機器人末端位姿的計算過程，根據(jù)刀位文件信息中的空間位置在x方向分量x₁、y方向分量y₁、z方向分量z₁，空間姿態(tài)在x方向分量i₁、y方向分量j₁、z方向分量k₁，可直接得到刀軸向量N和末端位置向量P：

N=(n_x1,n_y1,n_z1)=(i_1, j_1, k₁) =(-0.8204,0.5520,0.1464),

P=(l_x1,l_y1,l_z1)=(x₁,y₁,z₁) =(-0.0408,0.0131,-0.0030),

其中， (n_x1 n_y1 n_z1)分別是刀軸向量N在x、y、z方向的分量， (l_x1,l_y1,l_z1)分別是末端位置向量在x、y、z方向上的分量；

令第一點的坐標(biāo)為(x₁,y₁,z₁)，第二點的坐標(biāo)為（i₁,j₁,k₁），將N平移到以(x₁,y₁,z₁)為起點的位置，相對應(yīng)的坐標(biāo)值分別增加（i₁,j₁,k₁）的坐標(biāo)值，通過點(x₁,y₁,z₁)作與向量(x₁-i₁,y₁-j₁,z₁-k₁)相互垂直的平面S，過空間點(x₂，y₂)作平行于z軸的直線L，直線L與平面S的交與點w，向量(x₁-i₁,y₁-j₁,z₁-k₁)與向量(x₁-x₂，y₁-y₂，z₁-z)相互垂直，相乘為0，可求得通過點(x₂，y₂)的z方向的值g，然后將向量(x₂-x₁，y₂-y₁，g-z₁)簡化為單位向量，切向向量T由此求得：

g=(((x₂-x₁)i_r+(y₂-y₁)j₁)/-k₁)+z₁=-4.3mm,

o_x1= (x₂-x₁)/ )=-0.1715mm,

o_y1=(y₂-y₁)/ )=0.0118mm,

o_z1=(z₂-z₁)/ )=-0.9851mm,

T=(o_x1, o_y1 ,o_z1)= (-0.1715,0.0118,-0.9851)

其中，(o_x1,o_y1,o_z1)分別是切向向量T在x、y、z方向的分量；

法向向量N和切向向量T叉乘可得到副法向向量B：

B=N×T=(a_x1,a_y1,a_z1) =(n_x1,n_y1,n_z1) ×(o_x1, o_y1 ,o_z1)

=(-0.8204,0.5520,0.1464)×(-0.1715,0.0118,-0.9851)=(-0.5455,-0.8338,0.0850),

其中，(a_x1, a_y1, a_z1)分別是副法向向量B在x、y、z方向的分量；

由刀軸向量N、切向向量T、副法向向量B和末端位置向量P可求得在工件坐標(biāo)系下的機器人末端位置姿態(tài)A：

A=[n_x1 o_x1 a_x1 l_x1;n_y1 o_y1 a_y1 l_y1;n_z1 o_z1 a_z1 l_z1;0 0 0 1]

=[-0.8204,-0.1715,-0.5455,-0.0408;0.5520,0.0118,-0.8338,0.0131;-0.1494,-0.9851,0.0850,-0.0030;0,0,0,1]；

2）對工件坐標(biāo)系進(jìn)行標(biāo)定，將工件坐標(biāo)系下的末端位姿轉(zhuǎn)換為機器人基坐標(biāo)系下的末端位姿，主要包括以下內(nèi)容：

對工件坐標(biāo)系進(jìn)行標(biāo)定，規(guī)定工件坐標(biāo)系的空間姿態(tài)與機器人基坐標(biāo)系空間姿態(tài)一致，在機器人工作空間范圍內(nèi)只需對工件坐標(biāo)系的空間位置進(jìn)行標(biāo)定，本發(fā)明對工件坐標(biāo)系的標(biāo)定值為（-0.4,-0.4,-0.4），通過相應(yīng)的坐標(biāo)變換就可以求得在機器人基坐標(biāo)系下的末端位姿U：

U=[n_x1 o_x1 a_x1 p_x1;n_y1 o_y1 a_y1 p_y1;n_z1 o_z1 a_z1 p_z1;0 0 0 1]

=[-0.8204,-0.1715,-0.5455,-0.4408;0.5520,0.0118,-0.8338,-0.3869;-0.1494,-0.9851,0.0850,-0.4030;0,0,0,1];

其中，（p_x1,p_y1,p_x1）為機器人基坐標(biāo)系下的末端位置在x、y、z方向的分量，p_x1=l_x1+h_x1=-0.4408，p_y1=l_y1+h_y1=-0.3869，p_z1=l_z1+h_z1=-0.4030。

所述步驟2中，根據(jù)機器人結(jié)構(gòu)參數(shù)對其逆運動學(xué)分析，建立逆運動學(xué)模型，其數(shù)學(xué)表達(dá)式如下所示：

q₁=arctan2((p_x-d₆a_x),(p_y-d₆a_y))±π/2, (1)

令 k₁=cos(q₁)a_x+ sin(q₁)a_y,

k₂= cos(q₁)p_x-a₁+ sin(q₁)p_y,

k₃=p_z,

k₃₁=k₂-d₆k₁,

k₃₂=d₆a_z-k₃,

k₄=((a₂)²-(d₄)²+(k₃₁)²+(k₃₂)²)/(2a₂),

r= , 可求得關(guān)節(jié)2的關(guān)節(jié)角q₂，

q₂=arctan2((k4/r),± ), (2)

令 k₅=a₂-cos(q₂)k₃₁- sin(q₂)k₃₂ ,

k₅₁= cos(q₂)k₃₂- sin(q₂)k₃₁, 可求得關(guān)節(jié)3的關(guān)節(jié)角q₃，

q₃=arctan2(k₅,k₅₁) , (3)

令 k₆=sin(q₂₊q₃)k₁+cos(q₂₊q₃)a_z , 可求得關(guān)節(jié)5的關(guān)節(jié)角q₅，

q₅=arctan2(±),-k₆), (4)

令 k₇=(cos(q₁)a_y-sin(q₅)a_x)/sin(q₅),

k₈=(sin(q₂)a_z-cos(q₂)k₁-sin(q₃)cos(q₅))/(cos(q₃)sin(q₅)), 可求得關(guān)節(jié)4的關(guān)節(jié)角q₄，

q₄=arctan2(k₇,k₈), (5)

令 k₁₀=cos(q₁)n_y-sin(q₁)n_x,

k₁₁=sin(q₁)o_x-cos(q₁)o_y,

k₁₂=sin(q₄)cos(q₅)/cos(q₄), 可求得關(guān)節(jié)6的關(guān)節(jié)角q₆，

q₆=arctan2((k₁₀+k₁₁k₁₂),(k₁₂k₁₀-k₁₁))+ π , (6)

由上述數(shù)學(xué)表達(dá)式可知，q₁、q₂、q₅分別對應(yīng)有兩組不同解，通過數(shù)列組合可知機器人關(guān)節(jié)角共有八組不同解。

所述步驟3中，基于逆運動學(xué)模型計算關(guān)節(jié)角，包括以下內(nèi)容：

1）將標(biāo)定后的機器人基坐標(biāo)系下的末端位姿帶入通過機器人逆運動學(xué)求解出的機器人關(guān)節(jié)角表達(dá)式中計算機器人關(guān)節(jié)角，求解出來的關(guān)節(jié)角共有八組不同解，但求解出來的關(guān)節(jié)角不一定都符合要求，如果求出的關(guān)節(jié)角超出機器人關(guān)節(jié)角運動范圍，則要對該關(guān)節(jié)角轉(zhuǎn)換上下限，即在求解得到的關(guān)節(jié)角的基礎(chǔ)上加減2π，使關(guān)節(jié)角處在合理的范圍內(nèi)，其八組不同解具體如表1所示：

表1 本發(fā)明所計算機器人八組關(guān)節(jié)角

2）將每組六個關(guān)節(jié)角值分別取絕對值，然后相加求和M_x；

其中x=1,2,3,4,5,6,7,8，

通過表1八組不同關(guān)節(jié)角值可求得

M₁=497.4869，M₂=352.6731，M₃=659.4947，M₄=584.8969，

M₅= 595.2634，M₆= 706.2474，M₇= 665.7014，M₈= 802.5171；

利用最短路程法優(yōu)化逆求解算法，比較八組M_x值，選取八組值中最小一組作為最優(yōu)關(guān)節(jié)角；

通過比較可知最小值為M₂，故選取第二組解的關(guān)節(jié)角值為最優(yōu)解。

所述步驟4中，利用關(guān)節(jié)角輸出機器人末端軌跡，主要內(nèi)容為：

將得到的一系列優(yōu)化后的關(guān)節(jié)角，利用MATLAB軟件編程，得到機器人末端位置軌跡點，輸出機器人末端軌跡，如圖4所示。

為進(jìn)一步體現(xiàn)本發(fā)明方法的有效性，特在數(shù)控加工軌跡上取十個點的坐標(biāo)同相對應(yīng)機器人軌跡上的坐標(biāo)相對比，其對比結(jié)果如表2所示：

表2 數(shù)控加工軌跡坐標(biāo)同機器人位置坐標(biāo)對比

從數(shù)據(jù)對比上可以看出，數(shù)控加工軌跡與機器人末端軌跡基本上相同，重合度較高，驗證了利用此方法可以實現(xiàn)從刀位文件到機器人末端執(zhí)行器位姿轉(zhuǎn)換。機器人的虛擬仿真驗證了此方法對機器人末端執(zhí)行器姿態(tài)的良好控制，也證明了本發(fā)明所使用方法的有效性。