基于雙臂機器人和三維視覺的物體抓取和軸孔裝配

徐友法，王衛(wèi)軍，王兆廣，陳寶

(中國電子科技集團公司第二十一研究所，上海 200233)

0 引 言

隨著三維物體抓取需求的增加，基于三維視覺的抓取得到了廣泛研究。目前，研究領域主要集中在三維抓取標定、姿態(tài)估算、抓取路徑規(guī)劃等領域。

姿態(tài)估算中，由于采集的圖象已經轉換成點云圖，因此姿態(tài)估算主要研究為點云的匹配技術。點云匹配一般分為粗匹配和精匹配，粗匹配技術主要有隨機抽樣一致性[1-3](RANSAC，Random Sample Consensus)、主成分分析[4](PCA，Principal Component Analysis)等，精匹配有近點迭代法[5-6](ICP，Iterative Closest Points)等。

點云匹配時，需要有一個模板作為標準，即模板建立。點云模型建立主要有二種方式獲取： 基于三維CAD建圖軟件建立物體的三維模型[7-8]，基于視覺處理軟件獲得三維模型[9-10]。視覺處理方式采用深度相機或雙目相機對物體沿不同方向拍攝，通過多圖象合成物體的三維模型。攝象機拍攝主要是抓取物體表面特征，或者通過幾次拍攝獲取多個表面特征，雖然也能改善之前的一些特殊情況的局限性，但仍然存在產品自身拍攝的遺漏。

在理論研究匹配算法得到廣泛研究時，基于匹配算法的實際抓取研究則主要集中在二維視覺領域，其主要原因為理論匹配矩陣和實際抓取時機器人的姿態(tài)和抓取點無法方便地建立聯系。對于平面物體的抓取，我們可以通過旋轉角度和中心點的坐標直接進行抓取[11-12]。對于三維物體定位，物體的抓取點確認和抓取姿態(tài)確認則無法通過簡單的模板匹配矩陣直接獲取。現在常用的方法是通過深度學習方法直接獲取抓取位置和姿態(tài)[13-14]，但是此方法要求較高，需要深度學習算法的訓練，并且深度學習算法會引入一定的誤差，對于固定物體抓取不適用。

本文基于目前研究比較成熟的模板匹配矩陣，通過機器人示教的方式獲取物體的抓取位置和抓取位姿。整個抓取過程中，只有匹配的精度誤差，基于匹配矩陣獲取抓取位置和抓取位姿的方式不會引入新的誤差。假設在匹配精度零誤差的情況下，示教獲取的抓取位置和抓取姿態(tài)，在新的抓取過程中是完全復制的過程。

1 點云匹配矩陣獲取

基于點云模板的加工工件自動識別算法，其算法的核心是先利用基于點云模板的匹配算法RANSAC進行點云粗匹配，將工件點云數據和數字模型庫中的數模轉換到較近的位置，再使用經典的點云最近點迭代法(ICP)進行精匹配，完成目標文件的數字模型自動識別。

在點云匹配之前，需要對3D傳感器獲取的點云進行分割獲取目標物體的點云。本文采用區(qū)域分割的方法對采集的點云進行分割，基本原理：先對需要分割的區(qū)域找出一個曲率最小的種子作為生長的起點，然后將種子領域中具有相同性質的點歸并到種子點中，新的種子點繼續(xù)作為種子尋找下一個種子點，直到沒有新種子點出現，所有的種子點就形成一個分割的區(qū)域。分割完成后，根據點云規(guī)模獲取目標物體的點云，即可進行模板匹配獲取匹配矩陣。

1.1 粗匹配獲取旋轉矩陣

本文采用RANSAC預匹配方法，其主要過程：估計點云的參考系，計算點云參考系與正則坐標系對齊的變換，然后將點云通過坐標轉換移動到正則坐標系下，使采集點云和模板點云都移動到正則坐標系下的位置，作為精匹配的初始位姿。此方法實施簡單，可靠性較高，可以作為分割效果較好的點云預匹配方法。

點云的參考系采用主成分分析法獲取：利用PCA(主成分分析)建立參考系，最小的特征值對應的特征向量作為坐標系的Z軸，為了保證Z軸指向觀察點，如果Z軸和觀察方向不同(根據夾角范圍判斷)，則舍棄。最大的特征值對應的特征向量作為坐標系的X軸，Y軸通過X軸和Z軸叉乘得到。

(1)

然后，可以得到C的特征值λj和對應的特征向量vj，j∈{1,2,3}。特征值按升序排列，采用與最小特征值相關的特征向量v1作為參照系的z軸。參考坐標系的x軸是與最大特征值相關的特征向量v3。y軸由v2=v3×v1給出。

從參考系中，可以得到一個轉換矩陣[R|t]，使它與正則坐標系對齊。然后來用[R|t]矩陣轉換點云，使所有點云轉換到正則坐標系下，定義如下：

(2)

通過預匹配，效果如圖1所示，其中深色為采集點云，淺色為模板點云。

圖1 粗匹配結果

1.2 精匹配獲取高精度旋轉矩陣

采集點云和模板點云都移動到正則坐標系下，基于ICP算法進行匹配，通過剛性矩陣變化，實現采集點云和模板點云的精匹配。剛性匹配矩陣如下:

(3)

式中：R3×3是旋轉矩陣；T3×1是平移矩陣，表達式如下：

(4)

(5)

式中：α，β，γ分別為繞X，Y，Z軸的旋轉角度；tx，ty，tz分別表示點沿著X，Y，Z軸的平移量。

ICP算法基本原理：分別在待匹配的目標點云PX和源點云Q中，按照一定的約束條件，找到最臨近點(pi,qi)，然后計算出最優(yōu)匹配參數R和t，使得誤差函數最小。誤差函數定義：

(6)

通過ICP算法匹配效果如圖2所示，其中深色為采集點云，淺色為模板點云。

圖2 精匹配結果

2 機器人抓取位姿獲取

2.1 機器人抓取姿態(tài)獲取方法設計

機器人姿態(tài)獲取方法需要提前獲取模板點云和采集點云的模板匹配矩陣，通過矩陣轉換，將提前設定好的機器人抓取點和機器人抓取位姿轉換成新采集物體點云的抓取點和抓取位置，流程如圖3所示。

圖3 機器人位姿獲取流程

過程解釋如下：

(1)將工件按標準姿態(tài)擺放

將工件的正面朝上，使方框內(方框如圖4所示)平面與相機保持水平，同時也要和機器人的Z坐標水平，即平面的Z坐標在相機下和機器人下都是保持不變的；工件的箭頭方向和機器人的X軸向相同。

圖4 設置機器人抓取位姿

(2)在點云上面獲取整個工件的中心點P0(x0,y0,z0),作為后面的抓取點；取平面的三個標記點P1(x1,y1,z1)，P2(x2,y2,z2)，P3(x3,y3,z3)作為判定工件方向的基準點，其中P1,P2兩個點和機器人的X軸相同，P2,P3與機器人的Y軸相同，點的設置如圖4所示。

(3)采集點云，獲取所有點的點云坐標

(4)通過模板匹配，得到模板點云到采集點云的旋轉平移矩陣和逆矩陣。

(6)采集新的點云，獲取模板到采集點云新的旋轉平移矩陣。

(7)

(8)

(9)

式中：α1為機器人繞X軸旋轉角度；β1為機器人繞Y軸旋轉角度；γ1為機器人繞Z軸旋轉角度。

(10)

則機器人的抓取姿態(tài)為(xreal,yreal,zreal,α,β,γ)。

2.2 機器人預抓取位置設置

在機器人進行抓取時，機器人到達抓取位置姿態(tài)要保持不變，否則抓手運動軌跡可能會和物體本身、桌面產生干涉。

具體的預抓取位置設定方法如下：

設A(x1,y1,z1),B(x2,y2,z2),C(x3,y3,z3)是已知平面上的三個點。A,B,C可以形成三個向量:向量AB,向量AC和向量BC,則AB(x2-x1,y2-y1,z2-z1),AC(x3-x1,y3-y1,z3-z1),BC(x3-x2,y3-y2,z3-z2),設平面的法向量為(x,y,z)則有：

(11)

3 實 驗

3.1 機器人抓取點位姿獲取實驗

本文采用三維相機直接獲取點云的形式采集物體的三維點云數據集。通過CAD方式畫出物體的標準三維圖，通過模型轉化的方式，獲取物體的三維點云模型。

(1)設定機器人抓取點：在采集的點云上面取4個點，P0為抓取點，P1，P2，P3為姿勢估計點，點的坐標為P0(34.445 8；-33.446 4；724.207)，P1(56.385 7；-30.631 7；724.446)，P2(41.570 2 ；-23.418；724.288)，P3(36.856 3；-32.024 6；723.995)，效果如圖5所示。

圖5 采集點云抓取點位姿設計

(2)創(chuàng)建物體的點云模板，并通過模板匹配，獲取采集點云到模板的轉換矩陣：

(3)通過矩陣旋轉平移，標志點在模板上的坐標為P′0(-0.455 947 4;-1.197 418 2;0.058 593 75)，P′1(11.896 341 ;-19.549 126 ;-0.015 991 211)，P′2(11.702 364 ; -3.070 591 ;-0.024 963 379 )，P′3(1.892 770 3 ;-2.712 799 1; -0.195 495 61)，則標記點在模板上的位置如圖6所示。

圖6 模板點云抓取點位姿設計

(4)測試新的采集點云進行抓取點進行實驗：

采集新的點云1：獲取模板到點云的旋轉平移矩陣，將模板點云上的4個標記點也通過旋轉矩陣平移，得到如圖7所示，其中淺色為模板點云，深色為采集點云，4個白色點為采集點云的抓取點。由于抓取點超出了采集點云范圍，模板點云在此處顯示的是抓取點的位置。

圖7 新采集點云抓取點位姿

采集新的點云2：獲取模板到點云的旋轉平移矩陣，將模板點云上的4個標記點也通過旋轉矩陣平移，得到如圖8所示。

圖8 新采集點云抓取點位姿

通過兩次測試，測試效果較好。

3.2 基于ABB雙臂機器人的抓取實驗

本實驗完成了抓取工件的制作和加工，采用軸孔裝配的方式設計了如圖9所示的裝配工件，模擬電機轉子向定子中裝配的過程。

圖9 抓取工件

圖10 裝配工作站

實驗平臺框架如圖10所示，組成部件主要包括三維相機、雙臂機器人、機械抓手、待抓取工件。各部件共同合作實現工件的智能抓取。三維相機和機械臂的標定方法采用常用的6點標定，即使用機器人的6個點和三維相機采集到的6個對應點獲取轉換矩陣即可。

為了測試抓取效果，本文設計了兩個抓取工件，左機械臂設定為定點抓取，右機械臂通過三維視覺定位并抓取，然后兩個物體進行裝配，兩個物體內外徑直徑差為2 mm，裝配示意如圖11所示。

圖11 裝配示意圖

裝配過程效果如圖12所示。

圖12 工作站裝配效果

實驗結果顯示，實際抓取時右側機械臂能夠較準確地完成抓取。100組實驗的抓取中，模板匹配成功率為80%，在模板匹配成功的條件下，抓取成功率100%，并能順利完成裝配，取得了較好的實驗效果。

4 結 語

本文基于RANSAC和ICP模板匹配算法獲取矩陣，研究了一種機器人抓取點確定和機器人末端姿態(tài)獲取的方法；同時為機器人設計了一個預抓取位置，機器人在抓取時，調整抓取姿態(tài)到達預抓取位置后，保持抓取姿態(tài)不變，到達抓取點對物體進行抓取。抓取精度取決于模板匹配精度和示教抓取點精度，沒有引入其他誤差，具有較高的重復性精度。通過基于ABB雙臂機器人的抓取實驗驗證了方法的可行性，解決了匹配矩陣在實際生產和抓取應用中難落地的情況。