胡建玥

(福建省特種設(shè)備檢驗(yàn)研究院,福建 福州 350007)

0 引言

工業(yè)機(jī)器人在自由曲面進(jìn)行磨拋?zhàn)鳂I(yè)時(shí),工業(yè)機(jī)器人的運(yùn)動(dòng)軌跡往往會(huì)受到一些附加約束條件的限制,需要控制末端工具中心點(diǎn)TCP(tool center position)精確跟蹤曲面上規(guī)劃的既定軌跡來(lái)保證磨拋工件的表面加工質(zhì)量。然而在實(shí)際應(yīng)用中,工件在磨拋前往往存在尺寸誤差,工業(yè)機(jī)器人末端工具需要隨著磨拋曲面實(shí)時(shí)調(diào)整工業(yè)機(jī)器人運(yùn)動(dòng)軌跡,使末端工具與工件之間的作用力始終保持在一定范圍,以保證工業(yè)機(jī)器人磨拋過(guò)程的磨拋質(zhì)量[1]。

在工業(yè)機(jī)器人磨拋應(yīng)用中基于STL模型生成加工軌跡的方法已被廣泛使用[2]。當(dāng)前,對(duì)自由曲面的加工軌跡的規(guī)劃主要包括投影法、等參數(shù)法、截平面法等[3]。WALCZYK等[4]針對(duì)STL曲面模型,采用截面法生成加工軌跡,探討了軌跡的處理算法并生成連續(xù)的加工軌跡。徐金亭等[5]在STL曲面模型中,對(duì)模型曲面進(jìn)行局部擬合并利用等殘留高度法生成連續(xù)的加工軌跡。趙萍等[6]則面向數(shù)控機(jī)床加工,重點(diǎn)研究了STL模型的刀具加工軌跡、刀具路徑規(guī)劃等問(wèn)題。

上述方法對(duì)于曲面模型的軌跡規(guī)劃問(wèn)題有一定的借鑒意義,但是無(wú)法避免產(chǎn)生曲線(xiàn)偏置的干涉情況,并且未結(jié)合軌跡運(yùn)動(dòng)控制對(duì)工業(yè)機(jī)器人磨拋運(yùn)動(dòng)系統(tǒng)進(jìn)行全面系統(tǒng)研討。本文基于工件的STL模型,擬合生成了工業(yè)機(jī)器人的磨拋軌跡,另外結(jié)合力/位置控制方法,對(duì)工業(yè)機(jī)器人磨拋?zhàn)鳂I(yè)進(jìn)行自由曲面跟蹤控制研究,并通過(guò)實(shí)驗(yàn)平臺(tái)進(jìn)一步驗(yàn)證了該方法的準(zhǔn)確性和穩(wěn)定性。

1 實(shí)驗(yàn)方法

本文采用的工業(yè)機(jī)器人磨拋系統(tǒng)包括一臺(tái)鏈?zhǔn)酱?lián)工業(yè)機(jī)器人、多維力傳感器、電動(dòng)主軸、磨拋工具、待磨拋工件以及相應(yīng)的固定底座(圖1)。此外,工業(yè)機(jī)器人磨拋?zhàn)鳂I(yè)平臺(tái)還包括主機(jī)控制系統(tǒng)和力信號(hào)采集裝置。其中,6D力傳感器采集的力信息利用數(shù)據(jù)采集卡傳輸至控制主機(jī),控制主機(jī)與工業(yè)機(jī)器人雙向連接;工業(yè)機(jī)器人的工作狀態(tài)實(shí)時(shí)反饋給控制主機(jī),控制主機(jī)綜合處理工業(yè)機(jī)器人的狀態(tài)信息和末端受力信息,計(jì)算出工業(yè)機(jī)器人的位姿調(diào)整值并實(shí)時(shí)將軌跡運(yùn)動(dòng)指令傳輸至工業(yè)機(jī)器人,控制工作臺(tái)沿運(yùn)動(dòng)軌跡作業(yè)。

圖1 磨拋系統(tǒng)平臺(tái)及原理

1.1 自由曲面軌跡規(guī)劃

對(duì)曲面擬合的方法主要包括Bezier曲面、B樣條曲面、NURBS曲面、Coons曲面等[7]?？紤]到單個(gè)三角平面片中信息有限,僅有3個(gè)頂點(diǎn)的位置及所在位置的法向矢量估計(jì)。因此本文將采用多種Bezier三角自由曲面無(wú)限擬合的方法對(duì)三角平面片進(jìn)行自由曲面擬合,如圖2所示。

圖2 Bezier三角曲面擬合

圖2中,三角區(qū)域內(nèi)任意一點(diǎn)都可以利用面積坐標(biāo)來(lái)表示,G為自由三角區(qū)域ABC任一點(diǎn),其面積坐標(biāo)(u,v,w)與面積值s,求得三角形GBC面積為

(1)

由多個(gè)三角平面片擬合成的三角自由曲面方程為

(2)

考慮到STL三角平面片中的原始數(shù)據(jù)包括頂點(diǎn)坐標(biāo)和法向矢量,如圖3所示;通過(guò)對(duì)曲線(xiàn)Hermite插值生成許多中間點(diǎn),進(jìn)而構(gòu)造二次曲面片的擬合點(diǎn)P1,1,0、P0,1,1和P1,0,1。對(duì)于曲線(xiàn)上的兩個(gè)端點(diǎn)及其切向量P0、P1,則曲線(xiàn)方程的三次Hermite形式為

圖3 Bezier曲面的擬合點(diǎn)

P(t)=(2t3-3t2+1)P0+(-2t3+3t2)P1+(t3-2t2+t)P′0+(t3-t2)P′1,t∈[0,1]

(3)

位置點(diǎn)的切向量估計(jì)可以通過(guò)兩點(diǎn)之間的連線(xiàn)及法向矢量進(jìn)行求解,已知軌跡上的兩點(diǎn)P0和P1及其法向量n0和n1,v12為P0指向P1的單位向量,則P0和P1的切向量為

(4)

將式(4)代入式(3)后,求解出曲線(xiàn)的中點(diǎn)P1,1,0、P0,1,1、P1,0,1,將曲線(xiàn)的端點(diǎn)及中點(diǎn)代入式(2),求解出擬合的三角曲面的曲面方程,實(shí)現(xiàn)對(duì)三角平面片的曲面擬合。

通過(guò)Beizer三角曲面擬合方法對(duì)STL模型進(jìn)行曲面擬合獲得對(duì)原始曲面的近似擬合,進(jìn)而在擬合曲面上生成磨拋軌跡。常用的加工自由曲面軌跡生成方法包括等截面法和等參數(shù)法等,其中,等參數(shù)法由于計(jì)算簡(jiǎn)單、軌跡生成速度快等優(yōu)點(diǎn)被廣泛應(yīng)用于加工軌跡的生成過(guò)程[8]。如圖4所示,等參數(shù)法的本質(zhì)是:在已知曲面上的一個(gè)點(diǎn)Pi沿某一方向ti,在曲面上找到一點(diǎn)Pi+1,使得與Pi的Pi+1空間直線(xiàn)距離為S。

圖4 軌跡生成原理

在傳統(tǒng)的曲面軌跡規(guī)劃中,一般采用軌跡偏置的方法生成全部軌跡[8],但這種方法常會(huì)出現(xiàn)偏置曲線(xiàn)相互干涉的問(wèn)題,處理復(fù)雜。針對(duì)曲線(xiàn)偏置方法的不足,本文根據(jù)曲線(xiàn)路徑曲率的變化,自適應(yīng)調(diào)節(jié)控制點(diǎn)的間距,同時(shí)避免曲線(xiàn)偏置時(shí)的干涉問(wèn)題,具體實(shí)現(xiàn)方式如下。

在曲線(xiàn)偏置中,曲線(xiàn)的局部形狀可以近似為圓弧,相鄰曲線(xiàn)近似為同心圓。如圖5所示,已知曲線(xiàn)Ci偏置獲得曲線(xiàn)Ci+1,曲線(xiàn)Ci上一點(diǎn)Ci(tj)偏置設(shè)定距離l獲得曲線(xiàn)Ci+1的曲線(xiàn)控制點(diǎn)Ci+1(tj)。由Ci+1(tj)沿切向偏置控制點(diǎn)距s獲得Ci+1(tj+1)′,由Ci+1(tj+1)′尋找Ci上的最近點(diǎn)Ci(tj+1),再由Ci(tj+1)偏置l獲得Ci+1(tj+1)。

圖5 偏置曲線(xiàn)生成機(jī)理

為便于利用計(jì)算機(jī)的計(jì)算優(yōu)勢(shì),對(duì)參考曲線(xiàn)Ci進(jìn)行曲線(xiàn)離散化,由曲線(xiàn)上的密集點(diǎn)集代替曲線(xiàn)作為有序參考曲線(xiàn)點(diǎn)集,Ci(tj)生成Ci+1(tj)后可將Ci(tj)之前的子點(diǎn)集刪除,簡(jiǎn)化計(jì)算。由于軌跡生成過(guò)程是按點(diǎn)生成的,每個(gè)點(diǎn)生成過(guò)程中都已經(jīng)確保不與其他軌跡產(chǎn)生干涉,進(jìn)而生成無(wú)干涉的軌跡。本文基于螺旋式軌跡生成磨拋軌跡,如圖6所示,螺旋式磨拋軌跡實(shí)現(xiàn)了自由曲面軌跡規(guī)劃,加工完成僅需抬刀處理,加工軌跡連續(xù)且滿(mǎn)足軌跡規(guī)劃要求,通過(guò)三維重構(gòu)減小了模型誤差。

圖6 螺旋式磨拋軌跡

1.2 工業(yè)機(jī)器人曲面跟蹤控制

為了使工業(yè)機(jī)器人磨拋時(shí)保持一個(gè)恒定的接觸力,選擇圖7所示的間接力/位置混合控制結(jié)構(gòu)[9]。

圖7 力/位置混合控制

為了獲得更快的工業(yè)機(jī)器人磨拋系統(tǒng)響應(yīng)速度,位置控制環(huán)采用PD控制,同時(shí)為進(jìn)一步有效地消除穩(wěn)態(tài)誤差,力控制環(huán)采用PI控制,因此:

(5)

當(dāng)工業(yè)機(jī)器人采用速度控制模式,可得:

(6)

設(shè)工業(yè)機(jī)器人當(dāng)前末端的位置和速度分別為P0和v0,工業(yè)機(jī)器人跟蹤的目標(biāo)點(diǎn)位置為P1(圖8),則期望的速度矢量為v01、verr為v0與v01的速度矢量偏差。若磨拋位置控制器采用PD控制,將速度矢量偏差verr作為PD控制器的輸入信號(hào),輸出的偏差信號(hào)v′err與期望速度矢量v01合成為位置速度控制量v;通過(guò)每個(gè)控制周期重新計(jì)算v,工業(yè)機(jī)器人末端將沿圖8中的虛線(xiàn)移動(dòng)至目標(biāo)點(diǎn)。

圖8 工業(yè)機(jī)器人末端位置的速度控制示意

工業(yè)機(jī)器人末端點(diǎn)實(shí)時(shí)姿態(tài)可通過(guò)固連在末端點(diǎn)的坐標(biāo)系來(lái)表示。本文討論單個(gè)坐標(biāo)軸的姿態(tài)跟蹤控制算法,如圖9所示,工業(yè)機(jī)器人末端TCP當(dāng)前的姿態(tài)坐標(biāo)軸為n0,目標(biāo)姿態(tài)的坐標(biāo)軸為n1,等效轉(zhuǎn)軸矢量為f0,則工業(yè)機(jī)器人末端姿態(tài)運(yùn)動(dòng)的期望等效轉(zhuǎn)軸矢量為f01。等效轉(zhuǎn)軸矢量為姿態(tài)坐標(biāo)軸當(dāng)前的轉(zhuǎn)動(dòng)軸和轉(zhuǎn)動(dòng)速度合成:

圖9 末端姿態(tài)的速度控制

(7)

為實(shí)現(xiàn)磨拋機(jī)器人的力/位置混合控制,需要對(duì)磨拋機(jī)器人末端的運(yùn)動(dòng)進(jìn)行分解,在作用力控制空間上采用力的作用位置控制方式,而在位置控制空間仍采用位置控制的方式,通過(guò)速度控制方式實(shí)現(xiàn)工業(yè)機(jī)器人末端的力/位置混合控制。如圖10所示,在工業(yè)機(jī)器人末端TCP坐標(biāo)系下,z軸設(shè)為力控制空間,其余空間為位置控制空間。當(dāng)前位置跟蹤控制系統(tǒng)的計(jì)算速度矢量為v,將速度矢量向力控制空間z軸和其余空間xOy平面分解為vz和vxy;由于z軸為力控制空間,根據(jù)當(dāng)前接觸力信息計(jì)算出TCP軌跡的修正速度vFz。因此z軸的速度矢量被替換為速度矢量vFz;將z軸矢量vFz和矢量vxy合成為力/位置混合控制,矢量v′,實(shí)現(xiàn)工業(yè)機(jī)器人末端的力/位置混合控制,其中力控制速度矢量vFz可由式(7)求得。

圖10 位置速度在力空間的分解與合成

類(lèi)似于位置速度在力空間的分解與合成,在工業(yè)機(jī)器人末端TCP坐標(biāo)系中,設(shè)y軸為力控制空間。將姿態(tài)跟蹤控制器計(jì)算的矢量f向力控制空間的y軸分解為fy和fxz,用實(shí)時(shí)接觸力信息計(jì)算的姿態(tài)速度矢量fFy與位置控制的姿態(tài)速度矢量合成為姿態(tài)的力/位置混合控制矢量f′,如圖11所示。

圖11 姿態(tài)速度在力空間的分解與合成

因此,本文通過(guò)設(shè)計(jì)位置跟蹤控制器和姿態(tài)跟蹤控制器,在引入力控制后的姿態(tài)與位置控制器上實(shí)現(xiàn)對(duì)磨拋軌跡上點(diǎn)的跟蹤,針對(duì)磨拋過(guò)程中的恒力控制要求,采用間接力/位置混合控制算法,保證機(jī)器人的軌跡跟蹤。針對(duì)機(jī)器人控制器一般不對(duì)外開(kāi)放的特殊性,采用上位機(jī)+機(jī)器人控制器的控制架構(gòu),利用機(jī)器人速度控制指令實(shí)現(xiàn)機(jī)器人的軌跡跟蹤控制。

2 實(shí)驗(yàn)結(jié)果與討論

2.1 軌跡跟蹤實(shí)驗(yàn)驗(yàn)證

在工業(yè)機(jī)器人磨拋?zhàn)鳂I(yè)的腳本指令中,提供一種通過(guò)實(shí)時(shí)規(guī)劃?rùn)C(jī)器人末端的運(yùn)動(dòng)速度,保證工業(yè)機(jī)器人末端速度恒定的一種控制指令speedl,speedl運(yùn)動(dòng)控制指令格式為“speedl(xd,a,t)”。其中:“xd”為工業(yè)機(jī)器人末端TCP的速度值,包括末端空間位置移動(dòng)速度“xdp(m/s)”和姿態(tài)調(diào)節(jié)速度“xda(rad/s)”;“a”為加速度;“t”為指令運(yùn)行時(shí)間。實(shí)際跟蹤的軌跡一般是非規(guī)則曲線(xiàn),對(duì)于非規(guī)則曲線(xiàn)可以通過(guò)分段圓弧對(duì)曲線(xiàn)進(jìn)行近似[10-11]。為此,對(duì)圓弧軌跡進(jìn)行軌跡跟蹤實(shí)驗(yàn),實(shí)驗(yàn)的圓弧軌跡參數(shù)如表1所示,設(shè)置跟蹤進(jìn)給速度為10mm/s。實(shí)驗(yàn)結(jié)果如圖12所示,工業(yè)機(jī)器人的運(yùn)動(dòng)速度控制在10mm/s,跟蹤誤差只有不到0.3mm,控制效果與精度良好。

表1 圓弧軌跡的參數(shù)

圖12 速度與跟蹤誤差

2.2 力/位置混合控制實(shí)驗(yàn)驗(yàn)證

根據(jù)力/位置混合控制方法,將工業(yè)機(jī)器人的加工運(yùn)動(dòng)軌跡空間分解成位置控制和力控制兩個(gè)空間。工業(yè)機(jī)器人在力控制空間上對(duì)接觸摩擦力、平臺(tái)抖動(dòng)等外力誤差的響應(yīng)直接影響工業(yè)機(jī)器人磨拋加工過(guò)程中的恒力控制性能。通過(guò)對(duì)外界力誤差反饋為工業(yè)機(jī)器人末端工具的速度修正值,實(shí)現(xiàn)工業(yè)機(jī)器人的對(duì)運(yùn)動(dòng)軌跡和作用力誤差的修正。實(shí)驗(yàn)過(guò)程中,將工業(yè)機(jī)器人工具坐標(biāo)系的z軸設(shè)為力控制軸,目標(biāo)接觸摩擦力設(shè)置為20N。對(duì)工業(yè)機(jī)器人磨拋?zhàn)鳂I(yè)進(jìn)行單軸的力控制實(shí)驗(yàn),工業(yè)機(jī)器人在單軸力控制模式下,工業(yè)機(jī)器人末端工具與工件接觸并保持接觸力穩(wěn)定的響應(yīng)時(shí)間在0.5s以?xún)?nèi)。穩(wěn)定后的接觸摩擦力值大部分集中在目標(biāo)期望接觸摩擦力的±3N區(qū)間內(nèi),實(shí)驗(yàn)表明力控制方法具有穩(wěn)定性,如圖13所示。

圖13 目標(biāo)接觸力為20N的力響應(yīng)

2.3 曲面跟蹤控制實(shí)驗(yàn)驗(yàn)證

通過(guò)規(guī)劃出工業(yè)機(jī)器人加工軌跡,使控制機(jī)器人跟蹤規(guī)劃的軌跡,進(jìn)而實(shí)現(xiàn)加工自由曲面跟蹤控制,如圖14所示。圖中黑色軌跡是加工過(guò)程的軌跡位置,而藍(lán)色的是軌跡上的指向向量,即工具的主軸軸向;綠色和紅色軌跡分別為開(kāi)始及結(jié)束時(shí)的軌跡引導(dǎo)點(diǎn),防止工業(yè)機(jī)器人與工件產(chǎn)生不必要的干涉接觸(本刊黑白印刷,相關(guān)疑問(wèn)咨詢(xún)作者)。

圖14 離線(xiàn)規(guī)劃的部分加工軌跡

在加工過(guò)程中,通過(guò)沿軌跡點(diǎn)的法向方向偏置一定距離,使得工業(yè)機(jī)器人加工軌跡與工件產(chǎn)生接觸,進(jìn)而產(chǎn)生加工過(guò)程的接觸力。設(shè)加工的目標(biāo)接觸力F=20N,工具的彈性模量為k=5 000N/m2則軌跡的偏置量為D=F/k=0.004m=4mm。

將軌跡點(diǎn)沿各自的法向負(fù)方向偏置4mm獲得新的軌跡,作為工業(yè)機(jī)器人預(yù)期的加工軌跡。為了保證加工過(guò)程中的接觸力恒定,引入力控制技術(shù)對(duì)工業(yè)機(jī)器人加工軌跡進(jìn)行修正,同時(shí)保證工業(yè)機(jī)器人進(jìn)給速度的恒定。在工業(yè)機(jī)器人加工軌跡的z軸上(即電動(dòng)主軸方向)采用力控制,而在其余自由度上采用位置控制,實(shí)驗(yàn)結(jié)果如圖15所示。加入工業(yè)機(jī)器人力控制技術(shù)后,z軸末端的受力能夠控制在±4N范圍內(nèi),但是加工過(guò)程中仍有幾個(gè)波峰。對(duì)加工軌跡進(jìn)行分析,可知這幾處的波峰主要是加工軌跡有6處曲率變化較大軌跡,工業(yè)機(jī)器人在恒定速率的加工過(guò)程中,作用力響應(yīng)延遲使得機(jī)器人末端受力變小。

圖15 工業(yè)機(jī)器人末端z軸力

3 結(jié)語(yǔ)

本文就工業(yè)機(jī)器人提出一種自由曲面磨拋軌跡自動(dòng)生成方法,重點(diǎn)對(duì)工件STL模型進(jìn)行Bezier三角曲面擬合,進(jìn)而得到擬合曲面方程。其次,利用等參數(shù)法和曲線(xiàn)偏置的方法,得到工業(yè)機(jī)器人進(jìn)行跟蹤控制的全部軌跡,并對(duì)加工工件的自由曲面生成了螺旋式磨拋軌跡。同時(shí),補(bǔ)充了曲面偏置的方法,通過(guò)軌跡的轉(zhuǎn)換算法生成了機(jī)器人加工過(guò)程中磨拋軌跡位移曲線(xiàn),解決了曲線(xiàn)偏置時(shí)的干涉問(wèn)題,并通過(guò)間接力/位置的控制方法進(jìn)行曲面跟蹤控制。最后通過(guò)磨拋實(shí)驗(yàn)平臺(tái)對(duì)機(jī)器人力覺(jué)伺服軌跡規(guī)劃方法進(jìn)行實(shí)驗(yàn)驗(yàn)證,一是在跟蹤進(jìn)給速度為10mm/s的情況下曲面跟蹤誤差只有不到0.3mm,證明了利用機(jī)器人速度控制指令實(shí)現(xiàn)機(jī)器人的軌跡跟蹤控制效果與精度良好;二是工業(yè)機(jī)器人在單軸力控制模式下,目標(biāo)接觸摩擦力為20N時(shí),工業(yè)機(jī)器人末端工具與工件接觸并保持接觸力穩(wěn)定的響應(yīng)時(shí)間在0.5s以?xún)?nèi),穩(wěn)定后接觸摩擦力值在目標(biāo)接觸摩擦力的±3N區(qū)間內(nèi),證明了力控制方法具有穩(wěn)定性;三是加入工業(yè)機(jī)器人力控制技術(shù)后,z軸末端的受力能夠控制在±4N范圍內(nèi),證明了本文方法的有效性。本文對(duì)機(jī)器人力控制的軌跡規(guī)劃展開(kāi)的研究,對(duì)于曲面跟蹤控制的研究具有一定意義。