基于力覺伺服軌跡規(guī)劃的磨拋工業(yè)機器人曲面跟蹤控制研究

胡建玥

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

0 引言

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

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

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

1 實驗方法

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

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

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

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

圖2 Bezier三角曲面擬合

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

(1)

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

(2)

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

圖3 Bezier曲面的擬合點

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

(3)

位置點的切向量估計可以通過兩點之間的連線及法向矢量進行求解,已知軌跡上的兩點P0和P1及其法向量n0和n1,v12為P0指向P1的單位向量,則P0和P1的切向量為

(4)

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

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

圖4 軌跡生成原理

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

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

圖5 偏置曲線生成機理

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

圖6 螺旋式磨拋軌跡

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

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

圖7 力/位置混合控制

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

(5)

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

(6)

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

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

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

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

(7)

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

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

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

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

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

2 實驗結(jié)果與討論

2.1 軌跡跟蹤實驗驗證

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

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

圖12 速度與跟蹤誤差

2.2 力/位置混合控制實驗驗證

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

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

2.3 曲面跟蹤控制實驗驗證

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

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

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

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

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

3 結(jié)語

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