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

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

0 引 言

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

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

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

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

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

1 點(diǎn)云匹配矩陣獲取

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

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

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

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

點(diǎn)云的參考系采用主成分分析法獲?。豪肞CA(主成分分析)建立參考系，最小的特征值對應(yīng)的特征向量作為坐標(biāo)系的Z軸，為了保證Z軸指向觀察點(diǎn)，如果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給出。

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

(2)

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

圖1 粗匹配結(jié)果

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

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

(3)

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

(4)

(5)

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

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

(6)

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

圖2 精匹配結(jié)果

2 機(jī)器人抓取位姿獲取

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

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

圖3 機(jī)器人位姿獲取流程

過程解釋如下：

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

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

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

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

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

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

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

(7)

(8)

(9)

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

(10)

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

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

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

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

設(shè)A(x1,y1,z1),B(x2,y2,z2),C(x3,y3,z3)是已知平面上的三個(gè)點(diǎn)。A,B,C可以形成三個(gè)向量:向量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 實(shí) 驗(yàn)

3.1 機(jī)器人抓取點(diǎn)位姿獲取實(shí)驗(yàn)

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

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

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

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

圖6 模板點(diǎn)云抓取點(diǎn)位姿設(shè)計(jì)

(4)測試新的采集點(diǎn)云進(jìn)行抓取點(diǎn)進(jìn)行實(shí)驗(yàn)：

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

圖7 新采集點(diǎn)云抓取點(diǎn)位姿

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

圖8 新采集點(diǎn)云抓取點(diǎn)位姿

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

3.2 基于ABB雙臂機(jī)器人的抓取實(shí)驗(yàn)

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

圖9 抓取工件

圖10 裝配工作站

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

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

圖11 裝配示意圖

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

圖12 工作站裝配效果

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

4 結(jié) 語

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