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

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

0 引 言

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

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

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

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

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

1 點云匹配矩陣獲取

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

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

1.1 粗匹配獲取旋轉(zhuǎn)矩陣

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

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

(1)

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

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

(2)

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

圖1 粗匹配結(jié)果

1.2 精匹配獲取高精度旋轉(zhuǎn)矩陣

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

(3)

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

(4)

(5)

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

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

(6)

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

圖2 精匹配結(jié)果

2 機器人抓取位姿獲取

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

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

圖3 機器人位姿獲取流程

過程解釋如下：

(1)將工件按標(biāo)準(zhǔn)姿態(tài)擺放

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

圖4 設(shè)置機器人抓取位姿

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

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

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

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

(7)

(8)

(9)

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

(10)

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

2.2 機器人預(yù)抓取位置設(shè)置

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

具體的預(yù)抓取位置設(shè)定方法如下：

設(shè)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),設(shè)平面的法向量為(x,y,z)則有：

(11)

3 實 驗

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

本文采用三維相機直接獲取點云的形式采集物體的三維點云數(shù)據(jù)集。通過CAD方式畫出物體的標(biāo)準(zhǔn)三維圖，通過模型轉(zhuǎn)化的方式，獲取物體的三維點云模型。

(1)設(shè)定機器人抓取點：在采集的點云上面取4個點，P0為抓取點，P1，P2，P3為姿勢估計點，點的坐標(biāo)為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 采集點云抓取點位姿設(shè)計

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

(3)通過矩陣旋轉(zhuǎn)平移，標(biāo)志點在模板上的坐標(biāo)為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)，則標(biāo)記點在模板上的位置如圖6所示。

圖6 模板點云抓取點位姿設(shè)計

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

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

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

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

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

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

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

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

圖9 抓取工件

圖10 裝配工作站

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

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

圖11 裝配示意圖

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

圖12 工作站裝配效果

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

4 結(jié) 語

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