基于VisionPro的蒸汽發生器堵板定位抓取系統設計

趙林燁，王湘江，馮棟彥

(南華大學 機械工程學院，湖南 衡陽 421001)

0 引 言

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

基于特定蒸汽發生器的環境，要完全實現對堵板的自動化安裝，第一步需要進行的就是對堵板的定位抓取。筆者提出了一種基于Cognex 公司的 VisionPro 視覺軟件二次開發的三維視覺定位系統。VisionPro是一套基于 PC 架構的視覺系統軟件開發包[6-7]，在復雜的機器視覺領域應用廣泛。該文以機械臂為客戶端，視覺工控機為服務端建立TCP/IP通訊[8-9]，發送拍照指令，控制3D相機采集蒸汽發生器堵板點云數據，基于 VisionPro ，使用C#等進行二次開發，對目標進行檢測、定位，并將定位數據返還機械臂，控制機械臂完成堵板抓取工作。

1 視覺系統的硬件組成及處理流程

視覺定位系統是一套有著完整輸入、處理及輸出結構構成的整體[10]。其中輸入結構一般通過相機鏡頭采集目標對象,從而將實物轉換成機器語言。輸入結構將采集圖像進一步傳輸給處理結構，通常處理結構是具備圖像處理硬件要求和處理軟件的計算機，計算機通過處理軟件將輸入圖像進行一系列處理，包括圖像優化，目標定位，圖像計算等[11]。視覺系統硬件設計原理圖如圖1所示。

圖1 視覺抓取系統的硬件

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

圖2 相機和機械臂及其控制柜

3D相機通過BCM萬兆網口與工控機進行連接，保證圖像的傳送速率與傳送質量。機械臂控制柜與工控機通過網口進行TCP/IP通訊，機械臂控制柜作上位機，通過機械臂發送拍照指令給工控機，再由工控機控制相機完成拍照，采集到的圖像數據由工控機中的程序進行處理，再將計算出的數據轉換成信號并發送給機械臂，由機械臂執行對應的動作。工作流程如圖3所示。

2 視覺定位系統軟件設計

2.1 圖像采集

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

圖3 工作流程圖

2.2 點云相機手眼標定

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

圖4 手眼標定示意圖

此處采用的是眼在手上的手眼標定系統，將3D相機固定在機械臂末端，圖5為手眼標定坐標系變換的示意圖。

圖5 坐標變換示意圖

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

標定方法是將標定板固定在機械臂外某處不動,機械臂帶著相機運動多個姿態,拍攝每個姿態下標定板的圖像，并記錄機械臂末端位姿。定義坐標系W～O的變換矩陣為TO-in-W，那么根據坐標系之間的變換關系，可得：

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

(1)

在標定過程中，標定板坐標系O相對于機械臂基坐標系W是靜止不動的，因此當機械臂處于不同姿態時，TO-in-W均不變。故令Ai=TE-in-W，X=TC-in-E，Bi=TO-in-C。在每個姿態下X均不變，Ai、Bi，均不同，Ai與機械臂相關，Bi與相機相關。有:

TO-in-W=AiX-1Bi

(2)

有：

AiXBi=AjXBj(i≠j)

(3)

進行變換后,得到手眼矩陣：

AX=XB

(4)

(5)

手眼標定的本質就是求解形如AX=XB的矩陣。

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

(6)

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

(7)

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

2.3 圖像定位及坐標變換

工業機器人視覺定位系統的目的是在工業機器人的基坐標系上找出目標的坐標。通過手眼標定獲得了相機相對機械臂基座標的位置。現在只要相機找到目標所在位置，就可以知道它在機器人基坐標系下的位置。

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

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

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

(8)

為提高抓取機械臂抓取精度，因視覺抓取目標產品的多樣性，簡化抓取頻繁設定工具坐標系，視覺定位抓取方案設計采用示教方式，人為設定一個當前目標的標準抓取位姿，定為抓取坐標系G，得到抓取坐標系相對于基座標系的位姿HG-in-W，通過坐標轉換得到抓取位置相對于視覺模板訓練原點的位姿：

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

(9)

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

(10)

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

圖6 坐標轉換流程

3 實驗與精度分析

3.1 手眼標定試驗

此試驗在Win7系統下利用VisionPro 基于C#語言進行開發。手眼標定的目標需具有明顯的三維特征，此次試驗選用的是康耐視校準標定塊。

首先將標定塊放置于凳子上的任意位置，保證其穩定不發生移動，調整機械臂姿態使攝像頭盡量垂直向下對準標定塊，并使標定塊處于相機取像范圍內，調整攝像頭的曝光等數據，使采集到的點云圖像完整清晰，接下來采集第一份點云數據，定為標準點云。并保持標定塊不動，調整機械臂位姿，共計采集6幀點云，圖7所示為同時記錄機械臂末端工具坐標系位姿。

文中在拍攝6幀點云的同時記錄了機械臂末端執行器在機器人坐標系下的位置，如表1所列。表中X，Y，Z分別代表末端執行器中心位置在機器人坐標系下的位置，Rx，Ry，Rz分別代表繞X，Y，Z軸旋轉角度。

圖7 標定點云圖

表1 機械臂末端位姿數據

將上述第一個pose點相機所采集到的點云設置為標準點云，其對應的機械臂位姿設為A1，剩下的5幀點云對應機械臂位姿依次為A2，按照式(5)可得5個A矩陣。

B矩陣的獲取采用點云匹配算法，通過矩陣變換，將其他對應5幀點云變換到標準點云下。設置標準點云為B1，剩下的5幀依次為B2。

文中采用康耐視VisionPro視覺處理軟件中的3DPatMax算法來實現點云匹配及變換。

在離線狀態進行模板設置，以康耐視標定塊為3D手眼標定目標，以第一幀標準點云中的標定塊為特征訓練對象進行框選，設置訓練框中心為原點，進行模板訓練，如圖8所示。

圖8 初始模板訓練

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

將數據導入到VisionPro中后，得到標定結果如圖9所示,RMS及匹配組合誤差為0.746 mm。

圖9 手眼標定結果

在VisionPro中調用Cog3DHandEyeCalibrator函數。通過機械臂末端執行器位置信息與相機外參矩陣，對方程AX=XB求解，最終得到手眼關系矩陣X，如下所示：

3.2 目標定位抓取試驗

試驗要抓取的堵板為拆分設計，將一個整圓的堵板分成三塊，由機器人分別完成三次抓取以及蓋板以達成完整的堵板安裝工作。此次試驗要抓取的目標物是低水位中間堵板。為了驗證系統抓取的可行性及算法的有效性，搭建了堵板抓取工況模型。

調整機械臂到圖10所示姿態，使相機大致垂直向下拍攝堵板，在滿足相機800 mm的點云成像距離要求下采集此時的堵板點云。

通過Cog3DPatMaxTool工具對采集到的堵板點云數據中特有的中間兩片夾板3D特征框選并訓練見圖11。

圖10 堵板視覺定位 圖11 模型訓練 抓取工況

訓練完成后，運行工具，對當前序列點云進行識別定位，利用C#調用該工具下的Results.Item[0].GetPose()。這個值就是該序列點云的訓練特征在相機坐標空間中的位姿。

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

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

圖12 抓取模型訓練

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

將計算出來的機器人末端的位姿矩陣HG-in-W′轉換成坐標形式發送給機器人，即可到達抓手抓取物體時的位置和姿態，進而對物體實現抓取等動作。圖13為定位成功顯示頁面。

圖13 定位成功頁面

再移動堵板位置，重復進行多次試驗，定位準確率100%，3DPatMax定位平均得分0.98。結果表明，每次都可以定位到堵板對應訓練特征的位置，并且成功抓取堵板。

4 結 語

針對高劑量輻射環境下，核電站蒸汽發生器檢修時低水位堵板拆裝問題，提出了一種基于VisionPro的視覺引導的蒸汽發生器堵板抓取機器人系統。首先，使用VisionPro的工具采集點云圖像，相機手眼標定，使用3DPatMax算法進行目標識別和定位，示教抓取，然后使用C#編寫人機交互界面，可以更方便快捷地獲得所需的目標抓取的機械臂位姿。基于成熟的可視化開發軟件進行二次開發，保證了系統的可靠性。實驗證明Vision Pro的視覺引導的蒸汽發生器堵板抓取機器人系統能快速地進行應用程序開發，可以快速定位到堵板對應訓練特征的位置，并且成功抓取堵板，具有實用意義。