機(jī)器人針刺成型異形預(yù)制體的路徑規(guī)劃①

邵建娜,張雅秀,曾欣怡,蔣 云*,李嘉祿

(1.江南大學(xué) 紡織科學(xué)與工程學(xué)院,無(wú)錫 214122;2.天津工業(yè)大學(xué),天津 300387)

0 引言

纖維預(yù)制體是采用定型劑或紡織方法將增強(qiáng)纖維預(yù)成型為特定形狀的纖維復(fù)合材料增強(qiáng)體。針刺作為一種工藝簡(jiǎn)單、低成本的織物增強(qiáng)成型制備技術(shù)而被航空航天領(lǐng)域廣泛應(yīng)用,使用該技術(shù)制備的纖維復(fù)合材料比強(qiáng)度、比剛度高,抗疲勞性能良好[1-2]。三維針刺成型技術(shù)的原理是使用帶倒鉤的刺針?lè)磸?fù)刺入纖維網(wǎng),當(dāng)刺針的倒鉤進(jìn)入纖維網(wǎng)時(shí),由于纖維之間摩擦力的作用,使得原本有序的纖維交叉纏繞在一起,經(jīng)過(guò)大量反復(fù)的針刺,最終得到具有良好力學(xué)特性的針刺復(fù)合材料預(yù)制體。

三維針刺成型技術(shù)適用于制備平板預(yù)制體、圓環(huán)預(yù)制體、圓柱和圓錐預(yù)制體,以及具有復(fù)雜曲面形狀的預(yù)制體。在過(guò)去的幾十年時(shí)間里,國(guó)內(nèi)外的研究單位及科研工作者對(duì)三維針刺預(yù)制體的成型裝備開(kāi)展了廣泛的研究[3]。OLRY[4]發(fā)明了一種可以制備高厚三維針刺預(yù)制體的針刺機(jī),該設(shè)備在厚度方向具有較大的工作范圍,可以用于制備不同厚度的平板預(yù)制體。為了進(jìn)一步擴(kuò)展針刺機(jī)的適用性,RENAUD等[5]公開(kāi)了一種環(huán)形纖維預(yù)成型構(gòu)件的制造方法,該方法可以實(shí)現(xiàn)環(huán)向纖維的自動(dòng)鋪放和針刺,自動(dòng)化程度更高。航空航天用構(gòu)件通常具有較為復(fù)雜的曲面形狀。OLRY和RENAUD發(fā)明的平板和圓環(huán)預(yù)制體針刺機(jī)不能很好的滿足近凈成型的要求,限制了三維針刺預(yù)制體及其復(fù)合材料的開(kāi)發(fā)和應(yīng)用[3]。OLRY[6]在之前的基礎(chǔ)上發(fā)明了用于異形纖維預(yù)制體的針刺設(shè)備,由四自由度機(jī)械臂拖動(dòng)針刺頭完成針刺,針刺頭角度可在0～90°范圍內(nèi)進(jìn)行調(diào)整,特別適合橢球、圓錐等特定類型的非封閉殼體制品的針刺工作[7]。后續(xù),FRANCOIS等[8]設(shè)計(jì)了一款數(shù)控復(fù)合材料纖維增強(qiáng)針刺設(shè)備,設(shè)備采用龍門(mén)架式結(jié)構(gòu)并配有數(shù)控系統(tǒng),工作范圍更大,自動(dòng)化程度更高,可進(jìn)行平面、曲面的針刺任務(wù),缺點(diǎn)是針刺角度不可調(diào)節(jié)[7]。由于技術(shù)封鎖的原因,國(guó)外在機(jī)器人復(fù)合材料三維針刺成型領(lǐng)域應(yīng)用研究的資料寥寥無(wú)幾。國(guó)內(nèi)有天津工業(yè)大學(xué)的陳小明[3]借助CATIA軟件開(kāi)發(fā)了機(jī)器人針刺路徑規(guī)劃算法,缺點(diǎn)是該算法依賴其他第三方軟件。童寧[9]對(duì)PCA算法進(jìn)行改進(jìn)得到一種具有廣泛適應(yīng)性的噴槍姿態(tài)獲取方法。周仁義[10]采用基于Zernike矩的亞像素輪廓提取方法,通過(guò)NURBS曲線擬合的方法得到機(jī)器人運(yùn)動(dòng)路徑。

在機(jī)器人快速發(fā)展的今天[11-15],本文在此背景下對(duì)異形預(yù)制體機(jī)器人針刺成型路徑規(guī)劃進(jìn)行研究,解決異形變截面預(yù)制體變密度針刺,棱邊和大面不同植針數(shù)量末端執(zhí)行器無(wú)縫銜接針刺,實(shí)現(xiàn)輪廓形狀可變,纖維體積含量的精確控制,推動(dòng)三維針刺裝備快速發(fā)展,助力中國(guó)航天事業(yè)。

1 異形預(yù)制體機(jī)器人針刺路徑生成方法

由于自由曲面的靈活性使得異型預(yù)制體針刺路徑生成較為困難,主流的等參數(shù)法具有較大的局限性。為此,本團(tuán)隊(duì)提出了基于切片法的異型預(yù)制體機(jī)器人針刺路徑生成方法[7],將預(yù)制體模型保存為STL格式并提取其中的三角面幾何信息,采用多個(gè)平行切平面與STL模型文件三角面求交得到異型預(yù)制體針刺路徑型值點(diǎn)集合,如圖1所示。然后利用交點(diǎn)冗余信息對(duì)針刺路徑點(diǎn)集進(jìn)行優(yōu)化排序。最后,使用3次NURBS方法對(duì)針刺路徑型值點(diǎn)進(jìn)行擬合[7],從而得到機(jī)器人針刺路徑以及路徑表達(dá)式。圖中,T1、T2、…、T9為STL文件包含的三角面;i-2、i-1、…、i+2為多個(gè)切平面;a、b、…、e為i+2層切平面與三角面的交點(diǎn),后文統(tǒng)稱為針刺路徑型值點(diǎn)。

圖1 異形預(yù)制體機(jī)器人針刺路徑生成示意圖Fig.1 Schematic diagram of needling path generation for special-shaped preform robot

1.1 STL模型文件特點(diǎn)

STL文件是計(jì)算機(jī)圖形應(yīng)用系統(tǒng)中使用三角面集合去描述物體的一種文件格式,該格式文件可以根據(jù)不同的精度要求對(duì)模型表進(jìn)行網(wǎng)格劃分,將模型表面離散為許多三角面的集合,每個(gè)三角面記錄著三角面的三個(gè)頂點(diǎn)坐標(biāo)和外法向矢量[9]。不同于其他模型文件(STEP、IGES等),該格式文件只限于幾何信息數(shù)據(jù)在3D物體的表面,而沒(méi)有描述三角面之間的拓?fù)潢P(guān)系。文本格式的STL文件數(shù)據(jù)結(jié)構(gòu)如下:

solid film name.stl//模型文件名稱

facet normalnxnynz//法向量的3個(gè)分量值

outer loop

vertexx1y1z1//第一個(gè)頂點(diǎn)坐標(biāo)

vertexx2y2z2//第二個(gè)頂點(diǎn)坐標(biāo)

vertexx3y3z3//第三個(gè)頂點(diǎn)坐標(biāo)

endloop

endfacet //結(jié)束第一個(gè)三角面定義

……//其余三角面定義

end solid //文件結(jié)束聲明

1.2 三角面與切平面交點(diǎn)計(jì)算

三角面與切平面交點(diǎn)計(jì)算的基本算法是當(dāng)前切平面與一個(gè)三角面相交,以Z軸為切片方向進(jìn)行說(shuō)明。經(jīng)過(guò)分析,切平面與三角形面片的相交情況存在以下4種(見(jiàn)圖2):

(1)X-Y切平面與三角面的一個(gè)頂點(diǎn)相交;

(2)X-Y切平面與三角形面片的兩個(gè)頂點(diǎn)相交;

(3)X-Y切平面與三角形面片的一條邊和一個(gè)頂點(diǎn)相交;

(4)X-Y切平面與三角形面片的兩條邊相交。

設(shè)三角面的三個(gè)頂點(diǎn)坐標(biāo)分別為v1(x1,y1,z1)、v2(x2,y2,z2)、v3(x3,y3,z3),th為切片間距,z0為Z軸方向切片的起始高度。假設(shè)當(dāng)前位于第i個(gè)切片層,在Z軸方向高度為

zi=i·th+z0

(1)

當(dāng)X-Y切平面與小三角形面片的一個(gè)頂點(diǎn)相交時(shí),說(shuō)明該頂點(diǎn)就是在當(dāng)前切平面上的一個(gè)針刺路徑點(diǎn),如圖2(a)所示,二者相交于v1點(diǎn),則針刺路徑型值位置信息為

圖2 三角面相交的情況分析Fig.2 Analysis of triangular patch intersection

(2)

當(dāng)X-Y切平面與小三角形面片的兩個(gè)頂點(diǎn)相交時(shí),說(shuō)明三角面的一條邊與切平面重合,此時(shí)該條邊的兩個(gè)頂點(diǎn)就是在當(dāng)前切平面的兩個(gè)針刺路徑點(diǎn),如圖2(b)所示,二者相交于v2和v3點(diǎn),則針刺路徑型值點(diǎn)位置信息分別為

(3)

當(dāng)X-Y切平面與小三角形面片的一條邊和一個(gè)頂點(diǎn)相交時(shí),此時(shí)產(chǎn)生兩個(gè)針刺路徑點(diǎn)。其中一個(gè)為三角形的一個(gè)頂點(diǎn),二者相交于v3點(diǎn);另一個(gè)位于三角形邊v1v2上,可以使用相似三角形原理求解三角形邊v1v2上的針刺路徑點(diǎn)位置信息,如圖2(c)所示,則針刺路徑點(diǎn)位置信息分別為

(4)

和

(5)

當(dāng)X-Y切平面與三角形面片的兩個(gè)邊相交時(shí),說(shuō)明該切片層與三角形面片相交產(chǎn)生兩個(gè)針刺路徑點(diǎn),兩個(gè)針刺路徑點(diǎn)分別位于三角形面片的兩條邊上[9],如圖2(d)所示,此時(shí)兩個(gè)針刺路徑點(diǎn)分別為

(6)

和

(7)

1.3 基于交點(diǎn)冗余信息的針刺路徑型值點(diǎn)排序

由于STL模型丟失了原物理模型之間的拓?fù)潢P(guān)系,三角形的記錄順序雜亂無(wú)章,為了使交點(diǎn)坐標(biāo)有序排列,需要對(duì)其重建三角形間的拓?fù)潢P(guān)系。由于STL模型三角形之間存在共邊原則,因此在切片過(guò)程中存在著冗余的交點(diǎn)信息,本文基于此信息利用逐點(diǎn)識(shí)別與剔除的思想重建拓?fù)潢P(guān)系,進(jìn)而對(duì)針刺路徑型值點(diǎn)進(jìn)行排序[10]。

根據(jù)STL文件共邊規(guī)則,三角形面片共用一條邊,T1、T2三角形與切平面的交點(diǎn)記錄形式[ab]、[cd][10],分別為面片T1和面片T2的針刺路徑型值點(diǎn),坐標(biāo)值b=c,在順時(shí)針記錄有序點(diǎn)坐標(biāo)時(shí)可將點(diǎn)坐標(biāo)視為點(diǎn)坐標(biāo)的冗余交點(diǎn)。因此,利用三角形與切平面的交點(diǎn)坐標(biāo)可快速查到其冗余交點(diǎn)坐標(biāo),得到三角形的鄰接三角形,從而得到針刺路徑點(diǎn)型值點(diǎn)順序?yàn)閍、b、d或a、c、d,冗余交點(diǎn)如圖3所示。

圖3 交點(diǎn)冗余信息示意圖Fig.3 Schematic diagram of intersection redundancy information

1.4 基于NURBS曲線擬合的針刺路徑生成

經(jīng)過(guò)1.2和1.3節(jié)的運(yùn)算,將排序后的針刺路徑型值點(diǎn)擬合就形成了針刺路徑,針刺路徑型值點(diǎn)是后續(xù)規(guī)劃針刺點(diǎn)位置和姿態(tài)的基礎(chǔ)數(shù)據(jù)。由于NURBS方法靈活性和連續(xù)性好的優(yōu)點(diǎn)目前已在CAD/CAM領(lǐng)域獲得廣泛應(yīng)用,且這種方法已經(jīng)成為產(chǎn)品數(shù)據(jù)交換的國(guó)際標(biāo)準(zhǔn)。因此,本文采用NURBS方法進(jìn)行針刺路徑點(diǎn)擬合。一條k次的NURBS曲線可以由以下有理分式表示[7]:

(7)

其中,di(i=0,1,…,n)為控制頂點(diǎn);ωi(i=0,1,…,n)為權(quán)因子,與控制頂點(diǎn)di一一對(duì)應(yīng);Ni,k(u)是由節(jié)點(diǎn)矢量U=[u0,u1,…,un+k+1]通過(guò)德布爾-考克斯遞推公式?jīng)Q定的k次B樣條基函數(shù),其遞推公式為[11]

(8)

設(shè)1.3節(jié)獲取的針刺路徑型值點(diǎn)序列為pi(i=0,1,…,n),由于3次NURBS曲線完全滿足實(shí)際應(yīng)用中的要求,所以本文應(yīng)用3次NURBS曲線進(jìn)行針刺路徑型值點(diǎn)擬合,擬合流程圖如圖4所示[7]。

圖4 針刺路徑型值點(diǎn)NURBS曲線擬合流程圖Fig.4 Flow chart of NURBS curve fitting for needling path type value points

(1)型值點(diǎn)參數(shù)化

型值點(diǎn)參數(shù)化實(shí)際上是為每一個(gè)型值點(diǎn)pi賦予參數(shù)值ui,確定節(jié)點(diǎn)矢量U。積累弦長(zhǎng)參數(shù)化方法是目前公認(rèn)的最佳參數(shù)化法,該方法可以如實(shí)地表現(xiàn)出型值點(diǎn)的分布情況,更符合切片法得到的數(shù)據(jù)點(diǎn)特點(diǎn),則使用積累弦長(zhǎng)參數(shù)化方法得到的節(jié)點(diǎn)矢量U[12]:

(9)

(2)控制頂點(diǎn)反算

將控制頂點(diǎn)di與權(quán)因子ωi重新組合,定義帶權(quán)控制頂點(diǎn)Di[ωidi,ωi](i=0,1,…,n),轉(zhuǎn)換為齊次坐標(biāo)形式,它們之間的關(guān)系為

(10)

對(duì)于C2連續(xù)的3次NURBS閉曲線,式(10)中包含n+1個(gè)方程,因?yàn)槭啄┬椭迭c(diǎn)重合p0=pn,不計(jì)重復(fù),方程數(shù)減少1個(gè),剩余n個(gè)。又首末3個(gè)控制頂點(diǎn)依次相重Dn=D0,Dn+1=D1,Dn+2=D2,未知控制頂點(diǎn)數(shù)少了3個(gè),剩余n個(gè)。因此,就可從n個(gè)方程構(gòu)成的線性方程組求解出個(gè)n個(gè)未知不重復(fù)帶權(quán)控制頂點(diǎn)[13]。將式(10)改寫(xiě)為矩陣形式:

(11)

其中,

(12)

對(duì)于3次NURBS開(kāi)曲線,式(10)的n+1個(gè)方程不足以解決其中包含的n+3個(gè)未知控制頂點(diǎn),還必須增加兩個(gè)通常由邊界條件給定的附加方程[14]。此時(shí)求解3次B樣條插值曲線未知控制頂點(diǎn)的線性方程組可寫(xiě)成如下矩陣形式:

(13)

其中,系數(shù)矩陣中首行非零元素a1、b1、c1與右端列陣中矢量e1表示了首端點(diǎn)邊界條件;系數(shù)矩陣中末行非零元素an+1、bn+1、cn+1與右端列陣中矢量en+1表示了末端點(diǎn)邊界條件[15]。其余各行元素ai、bi、ci(i=1,2,3,…,n)與C2連續(xù)閉曲線的情況相同。

邊界條件通常采用端點(diǎn)切矢條件,切矢條件相當(dāng)于力學(xué)中的梁的端部固支的情況,該條件具有固定的切線方向。采用端點(diǎn)切矢條件時(shí),首、末端滿足下列附加方程[16]:

(14)

(15)

2 針刺點(diǎn)位置和姿態(tài)信息確定

在獲得針刺路徑的表達(dá)式后,根據(jù)針刺工藝對(duì)針刺路徑進(jìn)行等步長(zhǎng)插補(bǔ),進(jìn)而得到一系列針刺點(diǎn)位置和姿態(tài)信息,將生成的一系列針刺點(diǎn)位姿進(jìn)行逆運(yùn)動(dòng)學(xué)求解,運(yùn)動(dòng)控制器對(duì)各個(gè)電機(jī)進(jìn)行脈沖分配,驅(qū)動(dòng)機(jī)器人TCP點(diǎn)沿著針刺軌跡移動(dòng)到針刺點(diǎn)完成針刺。

2.1 針刺點(diǎn)位置信息確定

由于插補(bǔ)節(jié)點(diǎn)參數(shù)集合{u1,u2,…,un}與插補(bǔ)時(shí)間集合{t1,t2,tn}相互對(duì)應(yīng),以插補(bǔ)時(shí)間t為自變量,節(jié)點(diǎn)參數(shù)u為因變量建立函數(shù)。將參數(shù)u對(duì)時(shí)間t的函數(shù)在ti時(shí)刻進(jìn)行泰勒展開(kāi),則有

(16)

記T=ti+1-ti,則在t=ti+1時(shí)刻可得

(17)

略去式(17)中高階項(xiàng),只保留到二階項(xiàng),記u(ti)=ui,u(ti+1)=ui+1,即可得插補(bǔ)參數(shù)u的遞推公式為

(18)

由式(18)可知,插補(bǔ)參數(shù)的計(jì)算問(wèn)題轉(zhuǎn)換為了節(jié)點(diǎn)參數(shù)u對(duì)時(shí)間t的求導(dǎo)問(wèn)題。若機(jī)器人末端TCP點(diǎn)始終沿著曲線移動(dòng),則理想插補(bǔ)速度Varc(ui)為

(19)

用給定進(jìn)給速度V(ui)近似代替理想進(jìn)給速度Vace(u)并將式(19)變形得

(20)

對(duì)式(20)求導(dǎo)得

(21)

因此,基于泰勒展開(kāi)法的插補(bǔ)參數(shù)計(jì)算公式為

(22)

為了統(tǒng)一描述插補(bǔ)參數(shù)計(jì)算誤差的大小,引入速度波動(dòng)率δ作為評(píng)價(jià)標(biāo)準(zhǔn):

(23)

速度波動(dòng)率δ值越小,插補(bǔ)參數(shù)越接近理想值。

二階泰勒展開(kāi)法計(jì)算插補(bǔ)參數(shù)均要進(jìn)行復(fù)雜的求導(dǎo)運(yùn)算,不利于算法的實(shí)時(shí)性,同時(shí)算法截?cái)嗾`差較大。為此本文提出了基于四階Runge-Kutta的插補(bǔ)參數(shù)預(yù)估校正法,該方法首先運(yùn)用Runge-Kutta法對(duì)插補(bǔ)參數(shù)進(jìn)行預(yù)估計(jì)算,之后使用校正公式對(duì)得到的插補(bǔ)參數(shù)進(jìn)行修正,使速度波動(dòng)率保持在限定的范圍內(nèi),提高插補(bǔ)參數(shù)的計(jì)算效率和計(jì)算精度,確保針刺點(diǎn)間距一致。

將四階Runge-Kutta法運(yùn)用到NURBS曲線插補(bǔ)中用以計(jì)算插補(bǔ)參數(shù)。插補(bǔ)參數(shù)計(jì)算過(guò)程中,時(shí)間t為自變量,參數(shù)u為因變量,T為時(shí)間間隔(插補(bǔ)周期),T=ti+1-ti,已知在ti時(shí)刻對(duì)應(yīng)插補(bǔ)參數(shù)為ui,給定進(jìn)給速度V(ui),目標(biāo)是求解處時(shí)刻ti+1的插補(bǔ)參數(shù)ui+1,因此建立非線性微分方程:

(24)

由二階泰勒展開(kāi)法可得

(25)

由四階Runge-Kutta法可得

(26)

其中,

(27)

四階Runge-Kutta法與二階泰勒展開(kāi)法相比,只需要進(jìn)行一階導(dǎo)數(shù)的運(yùn)算,避免了二階導(dǎo)數(shù)的運(yùn)算,同時(shí)提高了計(jì)算精度,但是相較于理想的插補(bǔ)參數(shù)還是有一定的偏差,主要原因是用給定的插補(bǔ)進(jìn)給速度V(ui)替代理想進(jìn)給速度Varc(ui)。對(duì)此以速度波動(dòng)率作為優(yōu)化目標(biāo)設(shè)計(jì)校正算法對(duì)四階Runge-Kutta法計(jì)算出的插補(bǔ)參數(shù)初值進(jìn)行修正,提高插補(bǔ)參數(shù)的計(jì)算精度,保證各個(gè)周期插補(bǔ)步長(zhǎng)一致。

圖5 基于速度波動(dòng)率的插補(bǔ)參數(shù)校正示意圖Fig.5 Schematic diagram of interpolation parameter correction based on velocity fluctuation rate

本文設(shè)計(jì)的修正策略如下:

(28)

基于四階Runge-Kutta法的預(yù)估校正法插補(bǔ)參數(shù)計(jì)算流程如圖6所示。

圖6 基于四階Runge-Kutta的預(yù)估校正 插補(bǔ)參數(shù)計(jì)算流程圖Fig.6 Calculation flow chart of predictive correction interpolation parameters based on fourth order Runge Kutta

具體計(jì)算步驟如下:

Step1:初始化i=0,ui=0;

Step3:采用本文設(shè)計(jì)的四階Runge-Kutta法計(jì)算式(26),計(jì)算初始插補(bǔ)參數(shù)ui+1;

Step4:計(jì)算出插補(bǔ)參數(shù)ui+1處的插補(bǔ)點(diǎn)坐標(biāo)C(ui+1),得到當(dāng)前插補(bǔ)參數(shù)ui+1插補(bǔ)時(shí)的實(shí)際進(jìn)給步長(zhǎng)ΔLi;

Step5:根據(jù)式(23)計(jì)算速度波動(dòng)率,若δi=δmax,則跳轉(zhuǎn)到Step6;否則,采用式(28)對(duì)插補(bǔ)參數(shù)進(jìn)行校正,返回Step4;

Step6:判斷是否滿足ui+1≥1,如果滿足,表明此時(shí)已經(jīng)到達(dá)NURBS曲線的終點(diǎn),令ui+1=1,結(jié)束插補(bǔ);否則返回Step2。

將基于四階Runge-Kutta的預(yù)估校正得到的插補(bǔ)參數(shù)u代入式(7)中即可得到針刺點(diǎn)位置坐標(biāo)信息。2.2 針刺點(diǎn)姿態(tài)信息確定

假設(shè)基于四階Runge-Kutta的預(yù)估校正法求得的某個(gè)針刺點(diǎn)對(duì)應(yīng)的節(jié)點(diǎn)參數(shù)為u,從而確定節(jié)點(diǎn)參數(shù)u位于的節(jié)點(diǎn)區(qū)間,即u∈[u3+i,u4+i]。采用二分搜索法在節(jié)點(diǎn)矢量U的子區(qū)間{u3,u4,…,u2+n,u3+n}中搜索節(jié)點(diǎn)區(qū)間的下標(biāo)i,即可確定針刺點(diǎn)在型值點(diǎn)pi與pi+1之間[7]。

圖7 姿態(tài)笛卡爾坐標(biāo)系建立Fig.7 Establishment of attitude Cartesian coordinate system

則所有針刺點(diǎn)與其相鄰兩個(gè)型值點(diǎn)連線組成的邊的集合為

(29)

針刺點(diǎn)相鄰兩個(gè)型值點(diǎn)所在三角面的法向量集合可表示為[7]

(30)

(31)

由上述可知,機(jī)器人第i個(gè)針刺點(diǎn)的姿態(tài)信息可以表示為[9]

(32)

由于機(jī)器人RPY角姿態(tài)描述方式較為常用,將姿態(tài)矩陣轉(zhuǎn)換為RPY方式。機(jī)器人姿態(tài)的RPY描述方法如圖8所示,首先繞a軸旋轉(zhuǎn)α角,再繞o軸旋轉(zhuǎn)β角,最后繞n軸旋轉(zhuǎn)γ角,α、β、γ分別為其滾動(dòng)角、俯仰角和偏航角[7]。

圖8 機(jī)器人姿態(tài)RPY描述方式Fig.8 RPY description mode of robot attitude

機(jī)器人RPY姿態(tài)轉(zhuǎn)換矩陣表示為

Rot(a,α)Rot(o,β)Rot(n,γ)=

(33)

式中 cα、cβ、cγ分別為代表cosα、cosβ、cosγ; sα,sβ,sγ分別代表sinα、sinβ、sinγ。

式(33)中包含α、β、γ三個(gè)耦合角,為了求解α、β、γ的具體值,需要對(duì)矩陣進(jìn)行解耦。假設(shè)機(jī)器人期望姿態(tài)為

(34)

將式(34)左乘Rot(a,α)-1可得

(35)

將式(35)展開(kāi):

(36)

令式(36)中(2,1)元素對(duì)應(yīng)相等:

nycα-nxsα=0

(37)

解得

α=arctan 2(ny,nx)

(38)

令式(36)中(1,1)和(3,1)項(xiàng)分別對(duì)應(yīng)相等:

(39)

解得

β=arctan2[-nz,(nxcα+nysα)]

(40)

令式(36)中(2,2)和(2,3)項(xiàng)分別對(duì)應(yīng)相等得

γ=arctan 2[(-aycα+axsα),(oycα-oxsα)]

(41)

3 仿真驗(yàn)證

為了驗(yàn)證上述算法的有效性和穩(wěn)定性,選取某導(dǎo)彈天線罩預(yù)制體三維模型進(jìn)行仿真實(shí)驗(yàn),異型預(yù)制體三維模型如圖9所示。

圖9 某導(dǎo)彈天線罩預(yù)制體三維模型圖Fig.9 3D model drawing of a missile radome preform

選取X軸為切片方向,切片間距設(shè)置為100 mm,得到每一個(gè)切片層的交點(diǎn)坐標(biāo)數(shù)據(jù)并基于交點(diǎn)冗余信息的針刺路徑型值點(diǎn)排序,仿真結(jié)果如圖10所示。

圖10 針刺路徑點(diǎn)Fig.10 Needling route point

由于每個(gè)切片層切片形狀基本一致,為了簡(jiǎn)化仿真流程,選取單個(gè)切片層驗(yàn)證針刺路徑點(diǎn)NURBS曲線插補(bǔ)算法,仿真結(jié)果如圖11所示。圖11中,各個(gè)坐標(biāo)原點(diǎn)為針刺點(diǎn)位置,綠色為針刺點(diǎn)法向矢量,紅色為針刺點(diǎn)切向矢量,藍(lán)色為針刺點(diǎn)軸向矢量。三個(gè)矢量構(gòu)成了針刺點(diǎn)的姿態(tài)信息。

圖11 機(jī)器人針刺點(diǎn)位置和姿態(tài)生成Fig.11 Generation of robot needling position and attitude

速度波動(dòng)率描述了插補(bǔ)誤差的大小,進(jìn)而反映了針刺的均勻性。在此基礎(chǔ)上,運(yùn)用二階泰勒展開(kāi)法和基于四階Runge-Kutta的預(yù)估校正法對(duì)機(jī)器人針刺路徑進(jìn)行插值仿真。仿真插補(bǔ)參數(shù)如下:給定進(jìn)給速度V=100 mm/s,插補(bǔ)周期為T(mén)=0.04 s,即插補(bǔ)步長(zhǎng)為4 mm,最大速度波動(dòng)率δmax=10-2%,以速度波動(dòng)率為判定標(biāo)準(zhǔn)比較二階泰勒展開(kāi)法以及基于四階Runge-Kutta的預(yù)估校正兩種插補(bǔ)參數(shù)計(jì)算方法。

采用二階泰勒展開(kāi)法進(jìn)行仿真,結(jié)果如圖12所示。仿真結(jié)果表明,速度波動(dòng)率最大為6.4%,在u=0.2、u=0.6處出現(xiàn)突變,此時(shí)針刺點(diǎn)剛好位于針刺軌跡最大曲率處,原因在于插補(bǔ)節(jié)點(diǎn)參數(shù)u計(jì)算時(shí)是用給定的弦長(zhǎng)進(jìn)給速度代替理想的切線方向進(jìn)給速度,這時(shí)兩者差距較大,因此出現(xiàn)劇烈變化,說(shuō)明此時(shí)針刺點(diǎn)間距與理想值偏差較大。該算法由于沒(méi)有考慮對(duì)速度波動(dòng)率的限制,所以導(dǎo)致速度波動(dòng)率遠(yuǎn)遠(yuǎn)超過(guò)最大允許速度波動(dòng)率[17]。

圖12 二階泰勒展開(kāi)法的速度波動(dòng)率Fig.12 Velocity fluctuation rate of second order taylor expansion method

如圖13所示,采用基于四階Runge-Kutta的預(yù)估校正法使得最大速度波動(dòng)率限制在給定10-2%內(nèi),計(jì)算精度明顯提高,原因在于四階Runge-Kutta算法精度比二階泰勒展開(kāi)法高,同時(shí)對(duì)計(jì)算得到的插補(bǔ)參數(shù)進(jìn)行校正,保證速度波動(dòng)率限制在預(yù)設(shè)的范圍內(nèi)。

圖13 基于四階Runge-Kutta的預(yù)估校正法速度波動(dòng)率Fig.13 Velocity volatility of predictor corrector based on fourth order Runge Kutta

綜合以上分析,基于四階Runge-Kutta的預(yù)估校正法在速度波動(dòng)率方面明顯優(yōu)于二階泰勒展開(kāi)法,插補(bǔ)參數(shù)的計(jì)算更加準(zhǔn)確,保證了針刺點(diǎn)的均勻性,且不需要計(jì)算高階導(dǎo)數(shù),計(jì)算速度也有所提升,充分體現(xiàn)了算法的有效性[17]。

仿真結(jié)果表明,該算法靈活性好,穩(wěn)定性強(qiáng)。可以設(shè)置切片間距和切片方向以滿足不同針刺工藝要求的異型預(yù)制體針刺點(diǎn)位置和姿態(tài)生成,對(duì)于實(shí)際生產(chǎn)具有一定的意義。

機(jī)器人實(shí)物驗(yàn)證驗(yàn)證在本課題搭建的機(jī)器人針刺成形單元中進(jìn)行,如圖14所示。針刺機(jī)器人采用廣州數(shù)控(RB50),針刺工具為自主研制的氣動(dòng)針刺頭,如圖15所示。其余部件還包括空氣壓縮機(jī)、電磁閥、工件工裝等。

圖14 機(jī)器人針刺單元實(shí)驗(yàn)平臺(tái)Fig.14 Robot needling unit experiment platform

圖15 針刺工具三維模型Fig.15 3D model of needling tool

異形預(yù)制體針刺加工驗(yàn)證如圖15所示,針刺點(diǎn)位姿與規(guī)劃值基本一致,圖16為針刺效果的局部放大圖,經(jīng)過(guò)測(cè)量,針刺間距和針刺深度滿足針刺要求。

圖16 實(shí)際加工驗(yàn)證Fig.16 Actual processing verification

圖17 針刺后的異形預(yù)制體Fig.17 Special-shaped preform after needling

4 結(jié)論

(1)本文對(duì)異型預(yù)制體機(jī)器人針刺成型路徑規(guī)劃進(jìn)行研究,提出了一種基于切片法的機(jī)器人針刺路徑規(guī)劃方法。將多個(gè)平行切平面與預(yù)制體STL模型文件中包含的三角面進(jìn)行求交運(yùn)算,從而實(shí)現(xiàn)了異形預(yù)制體CAD模型到機(jī)器人針刺路徑的自動(dòng)生成。

(2)采用基于四階Runge-Kutta的預(yù)估校正法,用于NURBS曲線等步長(zhǎng)插補(bǔ)參數(shù)的計(jì)算,得到針刺點(diǎn)位置;選取針刺點(diǎn)相鄰路徑型值點(diǎn)的法向量和位置坐標(biāo),結(jié)合機(jī)器人RPY姿態(tài)描述方式,得到了針刺點(diǎn)的姿態(tài)。

(3)仿真結(jié)果表明,該路徑規(guī)劃算法靈活有效,為異型預(yù)制體機(jī)器人針刺成型路徑規(guī)劃提供了一種簡(jiǎn)便有效的方法。

后續(xù),結(jié)合此路徑規(guī)劃方法進(jìn)一步提高效率還可采用電動(dòng)凸輪驅(qū)動(dòng)提高針刺頻率和針刺力,可制作更厚、密度更高的預(yù)制體。