趙阿朋,王雪竹,張美玲, 康林清,樊繼壯

(1.中廣核研究院有限公司, 廣東 深圳 518000;2.哈爾濱工業(yè)大學(xué) 機電工程學(xué)院 哈爾濱 150008)

蒸汽發(fā)生器作為壓水堆核電站一回路設(shè)備中的核心裝置之一,在核電站發(fā)電過程中起到一、二回路間的能量傳輸作用.由于其內(nèi)部一回路側(cè)的傳熱管在工作中長期承受高溫、高壓和硼酸水腐蝕,容易導(dǎo)致管板、傳熱管以及水室發(fā)生泄漏、破裂和生銹等異常狀況,給核電設(shè)備安全運行帶來隱患[1-2].因此為減少安全風(fēng)險,避免非計劃停堆,本課題組研制了一種新型串聯(lián)作業(yè)機械臂,使用時可將其倒掛安裝在蒸汽發(fā)生器的管板上,在其末端可搭載多種作業(yè)工具對蒸汽發(fā)生器進行視頻檢測、打磨、拋光等修復(fù)作業(yè).

目前,國內(nèi)外學(xué)者對于機械臂軌跡規(guī)劃算法進行了深入研究并取得了眾多成果.Bailón W P[3]使用八次多項式插值法進行機械臂末端軌跡規(guī)劃,雖然能夠得到更為光滑的運動參數(shù)曲線,但是計算過程較為復(fù)雜且軌跡精度難以保證.Altabey W A[4]利用五次多項式插值法得到了連續(xù)且光滑的運動參數(shù)曲線,在降低計算量的同時保證了軌跡精度.Gasparetto A[5]采用五次B樣條插值法進行規(guī)劃,得到了加速度曲線不存在拐點的運動參數(shù)曲線,但該方法的計算量較大,可能會影響軌跡精度.喬亮[6]通過總結(jié)現(xiàn)有方法的不足,提出了三階參數(shù)樣條插值,通過參數(shù)選取解決軌跡曲率以及拐點問題,并進行仿真驗證了算法的可行性.

本文介紹了核電站蒸汽發(fā)生器的檢修任務(wù)工況和作業(yè)機械臂的作業(yè)過程,在此基礎(chǔ)上設(shè)計了作業(yè)機械臂的控制系統(tǒng),根據(jù)檢修任務(wù)需求,研究了作業(yè)機械臂在關(guān)節(jié)空間和笛卡爾坐標系空間下的軌跡規(guī)劃算法,并基于霍夫變換圓原理實現(xiàn)了作業(yè)機械臂對管板圓孔的識別定位.通過實驗驗證了軌跡規(guī)劃算法和圓孔識別定位的有效性.

1 蒸汽發(fā)生器檢修工況

蒸汽發(fā)生器的整體結(jié)構(gòu)由下方的半球形水室和數(shù)千根倒置的U形傳熱管共同組成.半球形水室與一回路側(cè)U形傳熱管相連,中間有隔板隔開,形成兩個四分之一的水室,上方有約厚557 mm的管板,管板上有數(shù)千個直徑約為20 mm的通孔,倒置的U形管脹緊在通孔內(nèi),如圖1(A)所示.在蒸汽發(fā)生器長期運行的過程中,需要定期停堆對其內(nèi)部的管板、傳熱管以及水室進行視頻檢測、打磨、拋光等修復(fù)作業(yè),由于蒸汽發(fā)生器內(nèi)部具有較強核輻照劑量,采用人工作業(yè)具有極高的安全風(fēng)險,因此課題組研制了一種作業(yè)機械臂,代替人工完成此項檢修任務(wù),提升作業(yè)安全性和自動化作業(yè)水平.

圖1 蒸汽發(fā)生器和作業(yè)機械臂結(jié)構(gòu)示意圖

檢修作業(yè)機械臂結(jié)構(gòu)設(shè)計如圖1(B)所示,采用六自由度串聯(lián)結(jié)構(gòu)設(shè)計方案,各個自由度均采用驅(qū)控一體的模塊化關(guān)節(jié)驅(qū)動,以便于機械臂的維護和檢修.

作業(yè)機械臂在執(zhí)行檢修任務(wù)前,如圖2所示,先由地面小車抓持機械臂的末端,通過蒸汽發(fā)生器上的人孔送入水室,并將機械臂的底座吊掛安裝固定在管板上,然后通過遠程控制軟件操控,配合多種檢修工具,完成對U形管、管板以及整個水室的打磨拋光、無損檢測、脹管、傳熱管清潔、傳熱管內(nèi)徑測量、襯管激光焊接等作業(yè).

圖2 作業(yè)機械安裝、檢修過程

2 作業(yè)機械臂控制系統(tǒng)設(shè)計

2.1 控制系統(tǒng)硬件設(shè)計

本文的控制系統(tǒng)采用分布式,將上位機和下位機分別進行控制,作業(yè)機械臂控制器為主站,進行運動學(xué)、動力學(xué)等算法計算,電機驅(qū)動器為從站,安裝在各個模塊關(guān)節(jié)的內(nèi)部,以驅(qū)動關(guān)節(jié)轉(zhuǎn)動,并實時反饋關(guān)節(jié)信息.采用EtherCAT總線來實現(xiàn)作業(yè)機械臂主站和從站之間的通訊,進行實時的數(shù)據(jù)傳輸.作業(yè)機械臂的控制系統(tǒng),需要完成機械臂在蒸汽發(fā)生器水室內(nèi)的位置姿態(tài)、軌跡規(guī)劃以及視覺圓孔點位識別等計算,作業(yè)機械臂硬件系統(tǒng)框圖及作業(yè)機械臂控制系統(tǒng)方案如圖3、4所示.

圖3 作業(yè)機械臂硬件系統(tǒng)框圖

圖4 作業(yè)機械臂控制系統(tǒng)框圖

主站是整個作業(yè)機械臂控制系統(tǒng)的核心,需要進行大量的數(shù)據(jù)計算,包括位姿、軌跡規(guī)劃、圓孔定位等一些重要的算法計算.其通過EtherCAT總線接受來自上位機傳遞的控制命令,進行計算求解之后,通過EtherCAT總線將控制指令發(fā)送給各個模塊關(guān)節(jié)的驅(qū)動器,從而驅(qū)動各個關(guān)節(jié)完成相對應(yīng)的指令,同時將各個模塊關(guān)節(jié)所處的轉(zhuǎn)動角度等狀態(tài)帶回給上位機,得到一個實時的反饋.因此對主站的計算能力要求很高,同時還需要豐富的接口,來滿足實際的需求.本文采用納博特科技研發(fā)的NRC系列控制器,尺寸為45 mm×230 mm×165 mm.該控制器配備了Intel下的Bay Trail Celeron處理器,8GB系統(tǒng)內(nèi)存,以及豐富的外設(shè)接口.

2.2 控制系統(tǒng)上位機設(shè)計

由前文可知,作業(yè)機械臂由六個模塊關(guān)節(jié)、連接件、控制器和上位機組成,機械臂的運動模式由上位機軟件進行控制,因此上位機軟件在作業(yè)機械臂控制系統(tǒng)中占重要位置.

本文上位機軟件系統(tǒng)采用Qt編寫,主要用于為操作者提供友好全面的作業(yè)機械臂軟件操作界面,用戶通過該軟件對作業(yè)機械臂進行遠程操作和實時控制,從而實現(xiàn)的對蒸發(fā)發(fā)生器管板及水室的檢查和修復(fù),如圖5所示.

圖5 上位機軟件控制界面

上位機軟件界面主要包括了通過IP連接機械臂、設(shè)置運動速度、點動、多關(guān)節(jié)聯(lián)動、圓孔視覺識別與定位、圓弧與直線規(guī)劃運動、獲取當前關(guān)節(jié)位置與緊急下電等功能模塊.操作上位機軟件時,首先通過EtherCAT連接到各個模塊關(guān)節(jié),點擊連接,等待響應(yīng),連接成功之后,上位機與主站建立了聯(lián)系,可以通過上位機發(fā)送指令,對機械臂進行運動控制.本文設(shè)計的上位機包含以下功能模塊:

1)回原點

回原點操作即是復(fù)位的操作,作業(yè)機械臂在搭建過程中,關(guān)節(jié)之間有一個相對位置.為了操作方便,在每次執(zhí)行完任務(wù)之后,可以執(zhí)行回原點的操作,方便下一次作業(yè),同時也方便觀察機械臂的狀態(tài),防止出現(xiàn)碰撞.

2)單關(guān)節(jié)點動

單關(guān)節(jié)點動是作業(yè)機械臂運動控制的基本功能.選擇點動的關(guān)節(jié),輸入轉(zhuǎn)動的關(guān)節(jié)角度,然后點擊動作按鈕即可.通過修改速度,可以改變關(guān)節(jié)點動的速度,速度變化范圍為0～10°/s.點擊獲取點位按鈕,可以實時得到關(guān)節(jié)的變化角度.

3)多關(guān)節(jié)聯(lián)動

多關(guān)節(jié)聯(lián)動功能是同時控制六個模塊關(guān)節(jié)運動,可以實現(xiàn)作業(yè)機械臂末端點到點的運動.首先給定各個關(guān)節(jié)轉(zhuǎn)動的角度,并設(shè)置目標速度,然后點擊運動按鈕,即可實現(xiàn)關(guān)節(jié)聯(lián)動功能.同時,點擊當前位置按鈕,上位機軟件還能通過讀取絕對值編碼器值,反饋出各個關(guān)節(jié)目前所處的位置狀態(tài).

4)逆運動學(xué)求解

在該功能模塊中,輸入作業(yè)機械臂末端在基坐標系下的位置坐標和姿態(tài)角度,即橫滾、俯仰與偏轉(zhuǎn)角度,運用機器人的逆運動學(xué)公式求解出各個關(guān)節(jié)需要轉(zhuǎn)動的角度,在軟件中顯示出來.點擊開始運動按鈕之后,作業(yè)機械臂即從當前位置運動到所給定的位姿.

5)圓孔識別定位

作業(yè)機械臂末端運動到管板正下方,通過相機抓拍一張管板管孔深度彩色圖片,基于霍夫變換圓檢測技術(shù),可得到各個圓孔圓心在相機坐標系下的位置,通過位姿矩陣變換,得到管孔圓心在作業(yè)機械臂基坐標系下的位置坐標和姿態(tài),運用所推導(dǎo)的逆運動學(xué)求解公式得到機械臂末端工具運動到管孔圓心的各個關(guān)節(jié)角度變量,然后點擊圓孔定位.

6)軌跡規(guī)劃運動

在使用前需要轉(zhuǎn)到連續(xù)運動模式,該功能模塊分成兩部分,一部分是機械臂軌跡規(guī)劃仿真,另一部分是樣機軌跡規(guī)劃運動.首先,通過RemoteAPI將Qt與仿真軟件Vrep進行通訊,將求解得到軌跡規(guī)劃數(shù)據(jù)傳給Vrep,控制作業(yè)機械臂模型進行運動,通過觀察機械臂在水室中的運動狀態(tài),主要觀察是否發(fā)生碰撞、關(guān)節(jié)運動是否異常、末端狀態(tài)等.在保證仿真驗證不出問題的前提下,再將軌跡規(guī)劃數(shù)據(jù)通過EtherCAT總線傳給機械臂進行運動控制.

3 作業(yè)機械臂運動軌跡規(guī)劃

根據(jù)檢修任務(wù)需求,可以將作業(yè)機械臂軌跡運動形式分為點對點運動和連續(xù)路徑運動.點對點運動用于管板圓孔的檢修作業(yè),通過關(guān)節(jié)空間軌跡規(guī)劃算法實現(xiàn);連續(xù)路徑運動用于管板和水室內(nèi)壁加工面的檢測、焊接等作業(yè),通過笛卡爾坐標系空間軌跡規(guī)劃算法實現(xiàn)[7-9].

3.1 關(guān)節(jié)空間軌跡規(guī)劃算法

關(guān)節(jié)空間軌跡規(guī)劃算法需要給定作業(yè)機械臂末端作業(yè)工具的起始位姿和目標位姿,利用逆運動學(xué)求解公式得到各個關(guān)節(jié)起點和終點的角度,對各個關(guān)節(jié)起點和終點的角度進行插補,得到關(guān)節(jié)變化的連續(xù)軌跡.由于需要對速度和加速度進行約束,本文選擇五次多項式插值規(guī)劃算法.

五次多項式的通式為式(1),有6個待定系數(shù).給定作業(yè)機械臂末端工具在起始點和目標點下各個關(guān)節(jié)的角度、速度以及加速度,根據(jù)五次多項式的一階導(dǎo)、二階導(dǎo),如式(2)所示,可列出六個方程,如式(3)所示,從而求解得到五次多項式.

q(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(1)

(2)

(3)

聯(lián)立式(1)～(3)可求解待定系數(shù)分別為:

同樣的,得到各個關(guān)節(jié)的角度、速度和加速度對時間的函數(shù)之后,通過仿真驗證五次多項式插值軌跡規(guī)劃在關(guān)節(jié)空間的結(jié)果,如圖6所示.

圖6 五次多項式插值關(guān)節(jié)角度、速度、加速度變化曲線

從五次多項式插值的結(jié)果來看,作業(yè)機械臂的關(guān)節(jié)角度變化較為平滑,速度與加速度的變化也較為連續(xù),沒有出現(xiàn)突變現(xiàn)象,有利于作業(yè)機械臂的運動平穩(wěn)進行.

3.2 笛卡爾坐標系空間軌跡規(guī)劃算法

在關(guān)節(jié)空間下的軌跡規(guī)劃無法保證作業(yè)機械臂末端作業(yè)工具按照特定路徑進行運動,因此,若要對作業(yè)機械臂末端作業(yè)工具的位姿進行絕對的約束,需要在笛卡爾坐標系空間內(nèi)實現(xiàn)軌跡規(guī)劃.

3.2.1 直線軌跡規(guī)劃

圖7 空間直線軌跡

設(shè)作業(yè)機械臂末端工具沿直線插補的速度為v,直線插補周期為Ts,總運動時長為T,根據(jù)直線長度L,求解總的直線插補步數(shù)N,如式(4)～(6)所示:

(4)

T=L/v

(5)

N=round(T/Ts)+1

(6)

其中:round表示對內(nèi)部的數(shù)值進行取整.

規(guī)定λ為歸一化位置變量,取值范圍為[0,1],當λ取0和1時,分別對應(yīng)為作業(yè)機械臂末端工具在起始點和目標點位置.F函數(shù)表示λ在時間上的映射,如式(7)所示,對應(yīng)的變化曲線如圖8所示.

圖8 歸一化變化曲線

(7)

3.2.2 圓弧軌跡規(guī)劃

在一個三維空間內(nèi),已知三個不共線的坐標點,可以求得經(jīng)過這三個坐標點的圓弧軌跡,并采用插補方法計算出在圓弧上各個插值點的三維坐標值.給定空間中的三個坐標點分別為Q1=[x1y1z1]、Q2=[x2y2z2]、Q3=[x3y3z3],其在空間中的位置如圖9所示.

圖9 空間圓弧軌跡

給定的三個坐標點在平面o1x1y1內(nèi)確定了一個外接圓,根據(jù)外接圓公式,得到圓的方程:

(8)

圓心的位置可以通過平面o1x1y1與線段Q1Q2的垂直平分面和與線段Q1Q3的垂直平分面的交點確定,線段Q1Q2的垂直平分面的方程如式(9).

(9)

線段Q1Q3的垂直平分面的方程如式(10):

(10)

聯(lián)立式(8)～(10)可求解圓弧圓心坐標和半徑為:

(11)

(12)

根據(jù)圓弧圓心坐標及其半徑值,運用插值算法,可計算出圓弧上各個插值點在o1x1y1z1坐標系中的坐標P(1).通過齊次坐標變換,得出插值點相對于基坐標系下的三維坐標值P(0),如式(13).

P(0)=0T1P(1)

(13)

其中:0T1為o1x1y1z1坐標系相對于o0x0y0z0坐標系的變換矩陣.

4 管板圓孔識別定位方法

管板位于水室上方,表面均布有上千個直徑約為20 mm的通孔.根據(jù)檢修任務(wù)要求,需要作業(yè)機械臂對管板圓孔進行識別定位,進而執(zhí)行檢修作業(yè).首先利用深度相機對管板進行拍攝,根據(jù)霍夫圓環(huán)變換識別圖像中的圓孔,通過坐標變換得到基坐標系下的圓孔圓心坐標.

4.1 霍夫變換圓環(huán)檢測

圖像中的圓孔識別是利用霍夫變換圓環(huán)檢測實現(xiàn)的,該算法是將識別到圖像中的每一個像素點作為圓上的一個點,進行投票方式來得到該圖像中的形狀集合,并根據(jù)實際情況設(shè)定一個權(quán)重從而定位圓的位置[10],如圖10所示.

圖10 霍夫變換圓環(huán)檢測原理

假設(shè)在X-Y平面內(nèi)有一個圓,其圓心為(a,b),半徑為r,則該圓可用(x-a)2+(y-b)2=r2方程來表示,即為二元一次函數(shù)f(x,y)=r2.圓方程也可寫成為(a-x)2+(b-y)2-r2=0,其表示a-b-r空間中函數(shù),此時x、y為常量,也可以寫成三元一次函數(shù)f(a,b,r)=0.此變換的含義就是將X-Y平面內(nèi)圓環(huán)上的一個點對應(yīng)為a-b-r三維空間中的一條曲線,如圖8所示.圓上各點對應(yīng)的曲線在a-b-r三維空間中會相交于一點(a,b,r),該點就是X-Y平面上所定義的圓的圓心(a,b)和半徑r.前面提到的權(quán)重就是在a-b-r三維空間內(nèi)相交曲線的數(shù)量,當累加的相交曲線數(shù)量達到該權(quán)重的比例時,就認為相交的那個點對應(yīng)X-Y平面上的一個圓.

4.2 圓心三維坐標獲取

使用深度相機拍取一張管板的深度/彩色圖并進行灰度處理,利用霍夫變換圓檢測算法得到圓孔的位置信息.由于此位置信息是相對于像素坐標系而言的,需要通過坐標變換,得到圓孔圓心在基坐標系下的位置坐標[11].

由深度相機得到的深度/彩色圖,因為彩色圖像數(shù)據(jù)和深度圖像數(shù)據(jù)是相對于不同的坐標系生成的,彩色圖像數(shù)據(jù)的原點在RGB攝像頭中心,深度圖像數(shù)據(jù)的原點在紅外攝像頭中心,兩攝像頭存在一定距離,兩者的坐標系會有一定的誤差.因此需要進行深度圖像的匹配,將彩色圖像與深度圖像進行對齊,使其在同一個坐標系下進行位置計算.

由像素坐標系下的位置轉(zhuǎn)換成相機坐標系下的位置這個過程,運用小孔成像原理,如圖11所示,為了理解清晰,將實際點與成像點放置在同一方向上,實際應(yīng)是關(guān)于Oc對稱的.f為相機坐標系下的焦距,m為在焦平面上的成像點,其坐標為Px(-x,-y,-z),M為相機坐標系下的一個點,坐標為Pc(xc,yc,zc).由相似三角形關(guān)系可得:

圖11 小孔成像模型

(14)

一般像素坐標系的原點位于圖像左上角,為了計算方便,將像素坐標系(Px)相對于相機坐標系(Pc)進行了縮放和原點平移,像素坐標系為Px(u,v):

(15)

其中:fx,fy,cx,cy的單位為像素,cx,cy是像素坐標系平移的尺寸.f為像距,dX,dY分別表示X和Y方向上的一個像素點在相機感光板上的物理長度,即fx=f/dX,fy=f/dY.則(u,v)可以寫成矩陣方程為:

(16)

通過矩陣運算,點在相機坐標系下的位置為:

(17)

同樣的,該點位置只是相對于相機坐標系而言的,需要將相機坐標系(Pc)變換到基坐標系(P0)下,從而得到該點在基坐標系下的位置為:

(18)

其中:R為一個3×3的旋轉(zhuǎn)矩陣,T是一個3×1的平移矩陣.聯(lián)立兩式即可得到像素坐標系下m點的位置相對于基坐標系下的位置為:

(19)

5 實驗測試

為了驗證軌跡規(guī)劃算法和圓孔識別定位方法的有效性, 搭建了實驗平臺進行實驗驗證. 利用EtherCAT信號線借口串聯(lián)各個模塊,電纜及信號線均為中空走線,作業(yè)機械臂樣機如圖12所示.

圖12 作業(yè)機械臂樣機

5.1 軌跡規(guī)劃運動實驗

通過上位機發(fā)送指令,機械臂各個關(guān)節(jié)之間進行配合實現(xiàn)聯(lián)動控制,分別完成在笛卡爾坐標系下的直線和圓弧軌跡規(guī)劃,如圖13所示.

圖13 作業(yè)機械臂軌跡規(guī)劃實驗

作業(yè)機械臂末端繪制的軌跡如圖14所示.繪制結(jié)果沒有明顯的抖動痕跡,且運動精度較為精確,運動過程比較穩(wěn)定.給了確定作業(yè)機械臂末端軌跡規(guī)劃的精度,直線與圓弧進行了多組實驗,每組進行十次測試,求取平均值,結(jié)果如表1、2所示.經(jīng)過計算,作業(yè)機械臂末端直線軌跡規(guī)劃誤差不超過0.5%,圓弧軌跡規(guī)劃誤差不超過1%.

表1 直線理論尺寸與實際尺寸

表2 圓弧理論尺寸與實際尺寸

圖14 機械臂末端軌跡

5.2 管板圓孔識別定位實驗

實驗所用的管板模型如圖15(A),圓孔直徑為19.28 mm.相機位于管板平面的正上方,相機到管板的垂直距離為300 mm.經(jīng)過霍夫變換圓檢測處理,得到如圖15(B)所示的圓孔處理圖像.從霍夫圓檢測識別到的圓心位置與實際的圓心位置進行對比發(fā)現(xiàn),識別得到的圓心位置誤差不超過1.38 mm,并能夠準確將末端作業(yè)工具插入到管板圓孔中,如圖15(C)所示.

圖15 管板圓孔識別定位實驗

在實驗的過程中,誤差有多個方面原因:1)光源與識別背景,光線太強或背景過于復(fù)雜,會造成較大的識別誤差,甚至導(dǎo)致識別失敗;2)相機視平面應(yīng)與管板平行,若成傾斜狀態(tài),會導(dǎo)致識別到的圓孔為橢圓形,降低識別到圓心個數(shù).

6 結(jié) 論

1)根據(jù)核電站蒸汽發(fā)生器內(nèi)部結(jié)構(gòu)及檢修任務(wù)要求,分別在關(guān)節(jié)空間和笛卡爾坐標系空間下對自研的六自由度機械臂進行了軌跡規(guī)劃,并完成了機械臂末端工具對管板圓孔的識別定位.

2)搭建了作業(yè)機械臂樣機并進行性能測試,機械臂能夠完成直線和圓弧軌跡規(guī)劃,誤差分別不超過0.5%和1%;能夠識別管板圓孔坐標并定位,識別的圓心誤差不超過1.38 mm.