基于VisionPro的蒸汽發(fā)生器堵板定位抓取系統(tǒng)設(shè)計(jì)

趙林燁，王湘江，馮棟彥

(南華大學(xué) 機(jī)械工程學(xué)院，湖南 衡陽(yáng) 421001)

0 引 言

為了保證核電站反應(yīng)堆安全運(yùn)行，提高核電站應(yīng)急救災(zāi)能力，各國(guó)核電站對(duì)機(jī)器人的需求將日益迫切。核電站主系統(tǒng)的其中一個(gè)關(guān)鍵設(shè)備就是蒸汽發(fā)生器，為確保核安全，需要定時(shí)對(duì)蒸汽發(fā)生器進(jìn)行檢修工作[1-3]，蒸汽發(fā)生器堵板的拆裝是核電站停堆檢修中的必要操作。在現(xiàn)有的檢修作業(yè)中，蒸汽發(fā)生器低水位堵板的密封作業(yè)要求工作人員將可折疊的堵板帶入蒸汽發(fā)生器內(nèi)部，蓋在對(duì)應(yīng)的堵口上，然后檢修人員可以站在堵板上面以進(jìn)行后續(xù)的檢修操作，這些都是高強(qiáng)度輻射下的長(zhǎng)時(shí)間作業(yè)[4-5]。根據(jù)核電工業(yè)發(fā)展趨勢(shì)，未來機(jī)器人將會(huì)取代人工完成堵板安裝工作。

基于特定蒸汽發(fā)生器的環(huán)境，要完全實(shí)現(xiàn)對(duì)堵板的自動(dòng)化安裝，第一步需要進(jìn)行的就是對(duì)堵板的定位抓取。筆者提出了一種基于Cognex 公司的 VisionPro 視覺軟件二次開發(fā)的三維視覺定位系統(tǒng)。VisionPro是一套基于 PC 架構(gòu)的視覺系統(tǒng)軟件開發(fā)包[6-7]，在復(fù)雜的機(jī)器視覺領(lǐng)域應(yīng)用廣泛。該文以機(jī)械臂為客戶端，視覺工控機(jī)為服務(wù)端建立TCP/IP通訊[8-9]，發(fā)送拍照指令，控制3D相機(jī)采集蒸汽發(fā)生器堵板點(diǎn)云數(shù)據(jù)，基于 VisionPro ，使用C#等進(jìn)行二次開發(fā)，對(duì)目標(biāo)進(jìn)行檢測(cè)、定位，并將定位數(shù)據(jù)返還機(jī)械臂，控制機(jī)械臂完成堵板抓取工作。

1 視覺系統(tǒng)的硬件組成及處理流程

視覺定位系統(tǒng)是一套有著完整輸入、處理及輸出結(jié)構(gòu)構(gòu)成的整體[10]。其中輸入結(jié)構(gòu)一般通過相機(jī)鏡頭采集目標(biāo)對(duì)象,從而將實(shí)物轉(zhuǎn)換成機(jī)器語(yǔ)言。輸入結(jié)構(gòu)將采集圖像進(jìn)一步傳輸給處理結(jié)構(gòu)，通常處理結(jié)構(gòu)是具備圖像處理硬件要求和處理軟件的計(jì)算機(jī)，計(jì)算機(jī)通過處理軟件將輸入圖像進(jìn)行一系列處理，包括圖像優(yōu)化，目標(biāo)定位，圖像計(jì)算等[11]。視覺系統(tǒng)硬件設(shè)計(jì)原理圖如圖1所示。

圖1 視覺抓取系統(tǒng)的硬件

考慮到實(shí)際應(yīng)用中輻射的影響，在選擇攝像機(jī)時(shí)，為了避免電磁干擾，選用抗輻射纖維材料的攝像機(jī)。此外，機(jī)械設(shè)計(jì)部分還將考慮電磁屏蔽和輻射防護(hù)設(shè)計(jì)。此次實(shí)驗(yàn)選用的相機(jī)為康耐視3D-A5120掃描點(diǎn)云相機(jī)，機(jī)械臂為新松GCR14-1400系列協(xié)作機(jī)器人，以及1臺(tái)配備視覺處理的工控機(jī)，工控機(jī)配備RTX-2080顯卡、10 KM以太網(wǎng)適配器，快速采集高分辨率 3D 圖像。部分硬件實(shí)物如圖2所示。

圖2 相機(jī)和機(jī)械臂及其控制柜

3D相機(jī)通過BCM萬兆網(wǎng)口與工控機(jī)進(jìn)行連接，保證圖像的傳送速率與傳送質(zhì)量。機(jī)械臂控制柜與工控機(jī)通過網(wǎng)口進(jìn)行TCP/IP通訊，機(jī)械臂控制柜作上位機(jī)，通過機(jī)械臂發(fā)送拍照指令給工控機(jī)，再由工控機(jī)控制相機(jī)完成拍照，采集到的圖像數(shù)據(jù)由工控機(jī)中的程序進(jìn)行處理，再將計(jì)算出的數(shù)據(jù)轉(zhuǎn)換成信號(hào)并發(fā)送給機(jī)械臂，由機(jī)械臂執(zhí)行對(duì)應(yīng)的動(dòng)作。工作流程如圖3所示。

2 視覺定位系統(tǒng)軟件設(shè)計(jì)

2.1 圖像采集

使用VisionPro的CogAcqFifoTool工具來進(jìn)行圖像采集，為相機(jī)建立一個(gè)Acquisition FiFo對(duì)象，該是一個(gè)維持先進(jìn)先出陣列的對(duì)象。在對(duì)象中可以設(shè)置相機(jī)端口、亮度、對(duì)比度、曝光、自定義等參數(shù)[12]，此處需在自定義參數(shù)頁(yè)面將采集的圖像屬性設(shè)置為rangeimage，即可獲取需要的深度圖像，將采集到的深度圖像傳輸給輸入圖像，并進(jìn)行后續(xù)工具處理。

圖3 工作流程圖

2.2 點(diǎn)云相機(jī)手眼標(biāo)定

根據(jù)機(jī)械臂與相機(jī)位置關(guān)系的不同，手眼系統(tǒng)可以分為眼在手上(Eye-in-Hand，Moving camera)和眼在手外(Eye-to-Hand，Stationary camera)兩種[13]，如圖4所示。

圖4 手眼標(biāo)定示意圖

此處采用的是眼在手上的手眼標(biāo)定系統(tǒng)，將3D相機(jī)固定在機(jī)械臂末端，圖5為手眼標(biāo)定坐標(biāo)系變換的示意圖。

圖5 坐標(biāo)變換示意圖

一共有四個(gè)坐標(biāo)系，分別為機(jī)械臂的基坐標(biāo)系(W)；機(jī)械臂末端工具坐標(biāo)系(E)； 相機(jī)坐標(biāo)系(C)；工件坐標(biāo)系(O)。對(duì)應(yīng)有三個(gè)變換矩陣： 坐標(biāo)系W～E的轉(zhuǎn)換用TE-in-W表示，該參數(shù)可直接從機(jī)械臂的操作面板獲??；坐標(biāo)系E～C轉(zhuǎn)換用TC-in-E表示，該參數(shù)是一個(gè)待求取的未知固定矩陣，即手眼標(biāo)定矩陣；坐標(biāo)系C～G的轉(zhuǎn)換用TO-in-C表示，該參數(shù)由相機(jī)在每次機(jī)械臂末端姿態(tài)變化后，通過視覺定位求取。

標(biāo)定方法是將標(biāo)定板固定在機(jī)械臂外某處不動(dòng),機(jī)械臂帶著相機(jī)運(yùn)動(dòng)多個(gè)姿態(tài),拍攝每個(gè)姿態(tài)下標(biāo)定板的圖像，并記錄機(jī)械臂末端位姿。定義坐標(biāo)系W～O的變換矩陣為TO-in-W，那么根據(jù)坐標(biāo)系之間的變換關(guān)系，可得：

TO-in-W=TE-in-WTC-in-ETO-in-C

(1)

在標(biāo)定過程中，標(biāo)定板坐標(biāo)系O相對(duì)于機(jī)械臂基坐標(biāo)系W是靜止不動(dòng)的，因此當(dāng)機(jī)械臂處于不同姿態(tài)時(shí)，TO-in-W均不變。故令A(yù)i=TE-in-W，X=TC-in-E，Bi=TO-in-C。在每個(gè)姿態(tài)下X均不變，Ai、Bi，均不同，Ai與機(jī)械臂相關(guān)，Bi與相機(jī)相關(guān)。有:

TO-in-W=AiX-1Bi

(2)

有：

AiXBi=AjXBj(i≠j)

(3)

進(jìn)行變換后,得到手眼矩陣：

AX=XB

(4)

(5)

手眼標(biāo)定的本質(zhì)就是求解形如AX=XB的矩陣。

用旋轉(zhuǎn)矩陣R和平移向量T表示矩陣方程，則有:

(6)

其中R為3×3的旋轉(zhuǎn)矩陣，T為3×1的平移矢量，由式(6)得到手眼標(biāo)定的基本方程:

(7)

根據(jù)公式(6)可知，要求解X矩陣，機(jī)器人至少需要兩次的相對(duì)位姿變化，并且兩次位置變化的旋轉(zhuǎn)軸要求為不平行。為了提高校準(zhǔn)精度，文中后續(xù)試驗(yàn)將在6個(gè)位置進(jìn)行取像。

2.3 圖像定位及坐標(biāo)變換

工業(yè)機(jī)器人視覺定位系統(tǒng)的目的是在工業(yè)機(jī)器人的基坐標(biāo)系上找出目標(biāo)的坐標(biāo)。通過手眼標(biāo)定獲得了相機(jī)相對(duì)機(jī)械臂基座標(biāo)的位置?，F(xiàn)在只要相機(jī)找到目標(biāo)所在位置，就可以知道它在機(jī)器人基坐標(biāo)系下的位置。

調(diào)用 VisionPro 中的 3DPatMaxTool 工具來完成目標(biāo)物的尋找。3DPatMaxTool 工具是PMAlignTool 工具的3D版本，同樣是模板匹配的工具，其核心算法是 3DPatMax 算法[14]。3DPatMax采用基于幾何的定位方法，通過圖像的三維幾何特征信息和特征之間的空間位置關(guān)系來訓(xùn)練和匹配模板，保證了高精度和抗干擾性，并且通過參數(shù)閾值設(shè)定，能夠定位旋轉(zhuǎn)、縮放的目標(biāo)。首先對(duì)拍照所得三維圖像具體特征進(jìn)行框選作為模板，并對(duì)模板進(jìn)行訓(xùn)練，然后根據(jù)模板對(duì)接下來采集的圖像進(jìn)行三維匹配，實(shí)現(xiàn)定位[15]。

前面通過手眼標(biāo)定得到了得到了相機(jī)坐標(biāo)系相對(duì)于工具坐標(biāo)系的位姿HC-in-E，同時(shí)通過模板匹配定位得到目標(biāo)特征工件坐標(biāo)系相對(duì)與相機(jī)的位姿HO-in-C，工具坐標(biāo)系相對(duì)于機(jī)械臂基座標(biāo)系的位姿HE-in-W可由機(jī)械臂直接讀出，那么工件坐標(biāo)系相對(duì)于基座標(biāo)系的位姿為：

HO-in-W=HE-in-WHC-in-EHO-in-C

(8)

為提高抓取機(jī)械臂抓取精度，因視覺抓取目標(biāo)產(chǎn)品的多樣性，簡(jiǎn)化抓取頻繁設(shè)定工具坐標(biāo)系，視覺定位抓取方案設(shè)計(jì)采用示教方式，人為設(shè)定一個(gè)當(dāng)前目標(biāo)的標(biāo)準(zhǔn)抓取位姿，定為抓取坐標(biāo)系G，得到抓取坐標(biāo)系相對(duì)于基座標(biāo)系的位姿HG-in-W，通過坐標(biāo)轉(zhuǎn)換得到抓取位置相對(duì)于視覺模板訓(xùn)練原點(diǎn)的位姿：

HG-in-O=(HO-in-W)-1HG-in-W

(9)

此位姿為固定常量，那么在下次抓取時(shí)，保證目標(biāo)成像成功的情況下，獲取新的工件坐標(biāo)系位姿HO-in-W′，那么抓取坐標(biāo)系相對(duì)于基坐標(biāo)系的位姿計(jì)算式為:

(10)

式(10)顯示了最終所需的工件抓取位姿。獲得此位姿后，可以向機(jī)器人發(fā)送運(yùn)動(dòng)命令，將手爪移動(dòng)到對(duì)應(yīng)位姿，并控制完成抓取工作。位姿變換的具體流程如圖6所示。

圖6 坐標(biāo)轉(zhuǎn)換流程

3 實(shí)驗(yàn)與精度分析

3.1 手眼標(biāo)定試驗(yàn)

此試驗(yàn)在Win7系統(tǒng)下利用VisionPro 基于C#語(yǔ)言進(jìn)行開發(fā)。手眼標(biāo)定的目標(biāo)需具有明顯的三維特征，此次試驗(yàn)選用的是康耐視校準(zhǔn)標(biāo)定塊。

首先將標(biāo)定塊放置于凳子上的任意位置，保證其穩(wěn)定不發(fā)生移動(dòng)，調(diào)整機(jī)械臂姿態(tài)使攝像頭盡量垂直向下對(duì)準(zhǔn)標(biāo)定塊，并使標(biāo)定塊處于相機(jī)取像范圍內(nèi)，調(diào)整攝像頭的曝光等數(shù)據(jù)，使采集到的點(diǎn)云圖像完整清晰，接下來采集第一份點(diǎn)云數(shù)據(jù)，定為標(biāo)準(zhǔn)點(diǎn)云。并保持標(biāo)定塊不動(dòng)，調(diào)整機(jī)械臂位姿，共計(jì)采集6幀點(diǎn)云，圖7所示為同時(shí)記錄機(jī)械臂末端工具坐標(biāo)系位姿。

文中在拍攝6幀點(diǎn)云的同時(shí)記錄了機(jī)械臂末端執(zhí)行器在機(jī)器人坐標(biāo)系下的位置，如表1所列。表中X，Y，Z分別代表末端執(zhí)行器中心位置在機(jī)器人坐標(biāo)系下的位置，Rx，Ry，Rz分別代表繞X，Y，Z軸旋轉(zhuǎn)角度。

圖7 標(biāo)定點(diǎn)云圖

表1 機(jī)械臂末端位姿數(shù)據(jù)

將上述第一個(gè)pose點(diǎn)相機(jī)所采集到的點(diǎn)云設(shè)置為標(biāo)準(zhǔn)點(diǎn)云，其對(duì)應(yīng)的機(jī)械臂位姿設(shè)為A1，剩下的5幀點(diǎn)云對(duì)應(yīng)機(jī)械臂位姿依次為A2，按照式(5)可得5個(gè)A矩陣。

B矩陣的獲取采用點(diǎn)云匹配算法，通過矩陣變換，將其他對(duì)應(yīng)5幀點(diǎn)云變換到標(biāo)準(zhǔn)點(diǎn)云下。設(shè)置標(biāo)準(zhǔn)點(diǎn)云為B1，剩下的5幀依次為B2。

文中采用康耐視VisionPro視覺處理軟件中的3DPatMax算法來實(shí)現(xiàn)點(diǎn)云匹配及變換。

在離線狀態(tài)進(jìn)行模板設(shè)置，以康耐視標(biāo)定塊為3D手眼標(biāo)定目標(biāo)，以第一幀標(biāo)準(zhǔn)點(diǎn)云中的標(biāo)定塊為特征訓(xùn)練對(duì)象進(jìn)行框選，設(shè)置訓(xùn)練框中心為原點(diǎn)，進(jìn)行模板訓(xùn)練，如圖8所示。

圖8 初始模板訓(xùn)練

在其他5幀點(diǎn)云圖像中，使用3DPatMax快速匹配出標(biāo)定塊的點(diǎn)云特征，按照式(5)可得5個(gè)B矩陣。

將數(shù)據(jù)導(dǎo)入到VisionPro中后，得到標(biāo)定結(jié)果如圖9所示,RMS及匹配組合誤差為0.746 mm。

圖9 手眼標(biāo)定結(jié)果

在VisionPro中調(diào)用Cog3DHandEyeCalibrator函數(shù)。通過機(jī)械臂末端執(zhí)行器位置信息與相機(jī)外參矩陣，對(duì)方程AX=XB求解，最終得到手眼關(guān)系矩陣X，如下所示：

3.2 目標(biāo)定位抓取試驗(yàn)

試驗(yàn)要抓取的堵板為拆分設(shè)計(jì)，將一個(gè)整圓的堵板分成三塊，由機(jī)器人分別完成三次抓取以及蓋板以達(dá)成完整的堵板安裝工作。此次試驗(yàn)要抓取的目標(biāo)物是低水位中間堵板。為了驗(yàn)證系統(tǒng)抓取的可行性及算法的有效性，搭建了堵板抓取工況模型。

調(diào)整機(jī)械臂到圖10所示姿態(tài)，使相機(jī)大致垂直向下拍攝堵板，在滿足相機(jī)800 mm的點(diǎn)云成像距離要求下采集此時(shí)的堵板點(diǎn)云。

通過Cog3DPatMaxTool工具對(duì)采集到的堵板點(diǎn)云數(shù)據(jù)中特有的中間兩片夾板3D特征框選并訓(xùn)練見圖11。

圖10 堵板視覺定位 圖11 模型訓(xùn)練 抓取工況

訓(xùn)練完成后，運(yùn)行工具，對(duì)當(dāng)前序列點(diǎn)云進(jìn)行識(shí)別定位，利用C#調(diào)用該工具下的Results.Item[0].GetPose()。這個(gè)值就是該序列點(diǎn)云的訓(xùn)練特征在相機(jī)坐標(biāo)空間中的位姿。

將該值導(dǎo)入到封裝好的機(jī)械臂抓取工具Cog3DRobotPickTool中，并輸入采集點(diǎn)云時(shí)機(jī)械臂位姿HE-in-W以及示教抓取此時(shí)目標(biāo)堵板時(shí)的機(jī)械臂位姿HG-in-W。如圖12所示。完成基于坐標(biāo)變換的抓取示教模型訓(xùn)練。相應(yīng)的位姿參數(shù)如下(數(shù)據(jù)單位為mm)。

通過式(9)得到抓取位置相對(duì)于視覺模板訓(xùn)練原點(diǎn)的位姿HG-in-O。

圖12 抓取模型訓(xùn)練

在所有訓(xùn)練完成后，進(jìn)行抓取驗(yàn)證試驗(yàn)，改變堵板姿態(tài)并控制相機(jī)回到拍照位置進(jìn)行拍照定位，基于模板匹配得到目標(biāo)特征相對(duì)與機(jī)械臂基座標(biāo)的位姿HO-in-W′，將其轉(zhuǎn)換成矩陣形式，代入式(10)得到此時(shí)堵板的抓取位姿HG-in-W′。

將計(jì)算出來的機(jī)器人末端的位姿矩陣HG-in-W′轉(zhuǎn)換成坐標(biāo)形式發(fā)送給機(jī)器人，即可到達(dá)抓手抓取物體時(shí)的位置和姿態(tài)，進(jìn)而對(duì)物體實(shí)現(xiàn)抓取等動(dòng)作。圖13為定位成功顯示頁(yè)面。

圖13 定位成功頁(yè)面

再移動(dòng)堵板位置，重復(fù)進(jìn)行多次試驗(yàn)，定位準(zhǔn)確率100%，3DPatMax定位平均得分0.98。結(jié)果表明，每次都可以定位到堵板對(duì)應(yīng)訓(xùn)練特征的位置，并且成功抓取堵板。

4 結(jié) 語(yǔ)

針對(duì)高劑量輻射環(huán)境下，核電站蒸汽發(fā)生器檢修時(shí)低水位堵板拆裝問題，提出了一種基于VisionPro的視覺引導(dǎo)的蒸汽發(fā)生器堵板抓取機(jī)器人系統(tǒng)。首先，使用VisionPro的工具采集點(diǎn)云圖像，相機(jī)手眼標(biāo)定，使用3DPatMax算法進(jìn)行目標(biāo)識(shí)別和定位，示教抓取，然后使用C#編寫人機(jī)交互界面，可以更方便快捷地獲得所需的目標(biāo)抓取的機(jī)械臂位姿?；诔墒斓目梢暬_發(fā)軟件進(jìn)行二次開發(fā)，保證了系統(tǒng)的可靠性。實(shí)驗(yàn)證明Vision Pro的視覺引導(dǎo)的蒸汽發(fā)生器堵板抓取機(jī)器人系統(tǒng)能快速地進(jìn)行應(yīng)用程序開發(fā)，可以快速定位到堵板對(duì)應(yīng)訓(xùn)練特征的位置，并且成功抓取堵板，具有實(shí)用意義。