基于雙目視覺的物體識別定位與抓取

高智偉， 譚曉東， 劉 客

(大連交通大學(xué)機(jī)械工程學(xué)院，大連 116028)

目前在自動化的工件加工、產(chǎn)品分類、零件裝配等領(lǐng)域,均已大規(guī)模使用工業(yè)機(jī)器人。而為實(shí)現(xiàn)生產(chǎn)的高效率、智能化,基于機(jī)器視覺的機(jī)器人控制成為目前研究的重點(diǎn)。雙目視覺由于具有高效、高精度、 非接觸式及能感知深度信息等優(yōu)點(diǎn),得以在目標(biāo)識別與定位中廣泛應(yīng)用[1]。

基于機(jī)器視覺的目標(biāo)抓取一般分為3個步驟:首先對目標(biāo)進(jìn)行識別,然后進(jìn)行目標(biāo)定位,最后機(jī)器人實(shí)施抓取。近年來,研究者們提出了許多方法。梁俊杰[2]提出了采用Harris角點(diǎn)檢測、尺度不變特征變換[3](scale-invariant feature transform，SIFT)計算角點(diǎn)的幅值和方向并用DAISY描述符對特征點(diǎn)進(jìn)行描述的方法,但SIFT計算速度相對較慢[4]且并沒有給出姿態(tài)的估計方法以及手眼標(biāo)定、機(jī)器人抓取的相關(guān)計算。李勝利[5]采用幾何特征對堆疊的工件進(jìn)行識別與定位,使用基于梯度方向橢圓檢測方法實(shí)現(xiàn)目標(biāo)識別,這種方法適用于一種特定形狀或紋理單一的物體識別,對特征復(fù)雜的物體無法做到很好的識別。王德海等[1]采用快速魯棒特征(speeded up robust features，SURF)算法進(jìn)行特征匹配,但并未對錯誤匹配點(diǎn)進(jìn)行過濾,導(dǎo)致物體識別的不準(zhǔn)確,且識別物體后僅僅通過三維重建計算了物體位置,并沒有計算姿態(tài),使得無法得到物體位姿從而實(shí)現(xiàn)抓取。高如新等[6]把特征點(diǎn)的平均值作為目標(biāo)的定位點(diǎn),但在許多情況下特征點(diǎn)的平均值并不是目標(biāo)輪廓的中心,這種方法定位誤差較大。以上方法均未對姿態(tài)的估計提出明確的解決方法。

針對存在的問題,采用SURF[7]特征匹配及雙重過濾的方法保證目標(biāo)識別的精度,并提出一種準(zhǔn)確的姿態(tài)估計方法,最終通過計算得到目標(biāo)位姿后對機(jī)器人進(jìn)行手眼標(biāo)定完成抓取,希望達(dá)到識別定位更加準(zhǔn)確迅速,抓取精準(zhǔn)可靠。

1 目標(biāo)識別

1.1 關(guān)鍵點(diǎn)匹配

采用SURF算法配合快速近似最近鄰搜索庫(fast library for approximate nearest neighbors，F(xiàn)LANN)匹配達(dá)到更快速的物體識別。SURF算法是基于SIFT算法的一種更高效的替換,它具有尺度不變性,旋轉(zhuǎn)不變性以及對視角變化、光照變化、噪聲、仿射變換也具有很強(qiáng)的魯棒性[8]。SURF算法作為核心關(guān)鍵點(diǎn)檢測的一種方法，其步驟如下。

(1)關(guān)鍵點(diǎn)檢測:SURF通過局部Hessian再給定的點(diǎn)上定義關(guān)鍵點(diǎn),SURF檢測器使用與近似兩個高斯內(nèi)核差的盒式濾波器進(jìn)行卷積來計算局部Hessian。從對這些盒式濾波器的響應(yīng)中,關(guān)鍵點(diǎn)特征在SURF中也定義為該Hessian的行列式超過某個閾值的局部極值。

(2)創(chuàng)建描述符:關(guān)于SURF方向,使用一對簡單的Haar小波來近似局部梯度,然后通過滑動窗口來聚合這些梯度估計。通過對該方向窗口中的所有梯度進(jìn)行求和,選擇最大方向作為特征的方向。關(guān)于尺度方面,以特征為形心區(qū)域由網(wǎng)格劃分為單元格,網(wǎng)格根據(jù)剛剛計算的方向給出的角度相對于特征旋轉(zhuǎn)。再經(jīng)過Haar小波計算,得到64維特征向量。

(3)描述符集匹配:目前的匹配方法有兩種,一種是暴力匹配,另一種稱為FLANN。FLANN匹配是一種近似法,它的精度雖然不如暴力匹配，但算法的速度卻快得多,而且也可以調(diào)節(jié)其參數(shù)提高準(zhǔn)確度或速度。

1.2 匹配點(diǎn)過濾

因?yàn)椴煌e誤匹配點(diǎn)對需要用不同方法過濾,所以本文算法采用雙重匹配點(diǎn)過濾,算法流程為:得到匹配點(diǎn),使用匹配點(diǎn)距離過濾錯誤匹配點(diǎn),然后采用隨機(jī)抽樣一致RANSAC[9](random sample consensus)算法進(jìn)一步過濾錯誤匹配。

初步過濾通過距離篩選匹配點(diǎn)中首先需計算出匹配點(diǎn)對之間的最大距離與最小距離,然后篩選掉距離過大的錯誤匹配點(diǎn)對,經(jīng)過反復(fù)實(shí)驗(yàn),發(fā)現(xiàn)當(dāng)篩選掉距離大于4倍最小距離的匹配點(diǎn)對時,可以達(dá)到匹配過濾的最好效果。

在進(jìn)一步匹配過濾中使用的RANSAC算法解決了最小二乘法的缺陷。在最小二乘法擬合模型中求取擬合直線運(yùn)用到了盡可能多的數(shù)據(jù)點(diǎn),期望通過“平均”消除偏差,但其中卻包括了重大誤差點(diǎn),這些重大誤差點(diǎn)就會造成擬合直線的巨大偏差。RANSAC算法可避免這一問題,其步驟如下。

(1)隨機(jī)抽取一小組內(nèi)點(diǎn)擬合模型。

(2)用上一步擬合的模型檢驗(yàn)其他數(shù)據(jù)點(diǎn),若適用于模型，則將其添加到內(nèi)點(diǎn)。

(3)如果得到的內(nèi)點(diǎn)數(shù)目大于給定閾值則認(rèn)為擬合的模型是合理的；否則重新選取模型,重復(fù)以上步驟。

(4)用擴(kuò)展后的內(nèi)點(diǎn)重新擬合模型。若迭代次數(shù)大于k算法終止；否則迭代次數(shù)加1,并重復(fù)以上步驟。

(5)算法終止后,選取內(nèi)點(diǎn)數(shù)最多的模型或者算法失效。

實(shí)驗(yàn)采用cv::findHomography函數(shù)調(diào)用RANSAC算法實(shí)現(xiàn)匹配點(diǎn)篩選。在這里RANSAC算法初始數(shù)據(jù)點(diǎn)為4個,通過這4點(diǎn)計算得到變換矩陣H,即為擬合模型M。迭代次數(shù)k是不斷更新的。對公式1-p=(1-wn)k兩邊取對數(shù)得

式(1)中：n為初始數(shù)據(jù)點(diǎn)個數(shù)；p為置信度；w為內(nèi)點(diǎn)比例；m為擬合模型最少初始點(diǎn)。

1.3 圖像分割

Grabcut算法是一種用戶導(dǎo)向的圖像分割算法,它是迭代的Graphcut[10],Grabcut通過局部標(biāo)記法,用矩形框?qū)⑶熬澳繕?biāo)物體包圍,矩形框以外的所有像素都被標(biāo)記為背景；然后對前景和背景顏色空間建立高斯混合模型；最后通過迭代完成能量函數(shù)最小化,實(shí)現(xiàn)前景目標(biāo)與背景的分離[11]。

但Grabcut算法需要用戶在待分割對象周圍定義一個矩形框,使用標(biāo)志GC_INIT_WITH_RECT,使得矩形框外為確定背景,矩形框內(nèi)為疑似前景。采用在SURF算法得到匹配目標(biāo)后,獲取4個角點(diǎn),繪制得到目標(biāo)區(qū)域矩形框,通過這一過程使Grabcut算法在此研究中成為了一種自動圖像分割算法。

1.4 目標(biāo)識別實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)中通過雙目相機(jī)采集左右圖像,對左圖像進(jìn)行處理,右圖像加以輔助,實(shí)現(xiàn)目標(biāo)的定位。不同場景下的目標(biāo)識別結(jié)果如圖1所示,后續(xù)實(shí)驗(yàn)針對圖1(a)進(jìn)行。

圖1 目標(biāo)識別效果圖Fig.1 Photograph of target recognition

目標(biāo)識別后進(jìn)行圖像分割,圖2所示為分割后的結(jié)果。

由于目標(biāo)物體是黑色包圍白色形式,Grabcut會將某些白色區(qū)域處理為背景,增加了目標(biāo)分割的難度,因?yàn)槲矬w內(nèi)部并不影響形心位置,所以采取輪廓填充的方法解決這一問題,結(jié)果如圖3所示。

圖2 圖像分割效果圖Fig.2 Photograph of image segmentation

圖3 輪廓填充圖Fig.3 Photograph of contour fill

2 三維重建

2.1 三維重建算法

在機(jī)器視覺中圖像到三維世界一共有4個坐標(biāo)系,分別為像素坐標(biāo)系、圖像坐標(biāo)系、相機(jī)坐標(biāo)系和世界坐標(biāo)系[12]。實(shí)驗(yàn)通過三維重建目標(biāo)物體輪廓形心來確定目標(biāo)物體位置。設(shè)空間中某一點(diǎn)P=(xc,yc,zc)在左圖的圖像坐標(biāo)為PxL=(xL,yL),像素坐標(biāo)為PuL=(uL,vL)，右圖的圖像坐標(biāo)PxR=(xR,yR)，像素坐標(biāo)為PuR=(uR,vR)，圖像坐標(biāo)系原點(diǎn)(u0,v0)，因?yàn)槭褂玫氖枪庑脑谕凰骄€的雙目相機(jī)，所以yL=yR=y，在相機(jī)標(biāo)定中將相機(jī)坐標(biāo)系視為世界坐標(biāo)系，則由相機(jī)成像原理及像素坐標(biāo)系與圖像坐標(biāo)系的轉(zhuǎn)換關(guān)系可得到：

式(1)中：f為焦距；B為兩相機(jī)的基線距；d=uL-uR為視差；fx為f在像素坐標(biāo)系u軸上的尺度因子。

實(shí)驗(yàn)三維重建算法流程如下。

(1)圖像二值化。

(2)目標(biāo)物體輪廓提取及填充使用函數(shù)cv::findContours及cv::drawContours。

(3)計算左圖輪廓形心像素坐標(biāo),如圖3所示,認(rèn)為白色為1,黑色為0,根據(jù)式(3)求出輪廓形心點(diǎn)坐標(biāo)：

式(4)中：(x0,y0)為形心坐標(biāo)；f(x,y)是在點(diǎn)(x,y)的像素值。通過遍歷左圖中的所有像素點(diǎn)，最終求解形心坐標(biāo)。

(4)在左圖像中以目標(biāo)形心為形心截取一個40×40的矩形模板圖像塊,使用歸一化的互相關(guān)模板匹配方法得到右圖對應(yīng)形心像素坐標(biāo)。

(5)將左右圖形心像素坐標(biāo)代入式(2)計算得到形心空間坐標(biāo)。

2.2 三維重建實(shí)驗(yàn)結(jié)果

標(biāo)定得到左相機(jī)的內(nèi)參矩陣：

可以得到fx=719.883 9，u0=325.761 2，v0=246.649 0，B=40.4。

由圖3得到目標(biāo)左圖中的形心像素坐標(biāo)為OL=(391,224)，模板匹配得到右圖像素坐標(biāo)OR=(311,230)。代入式(2)得到O點(diǎn)三維坐標(biāo)為O=(32.946,-11.438,363.541)。

2.3 三維重建誤差分析

在計算誤差時,由于O點(diǎn)實(shí)際坐標(biāo)難以準(zhǔn)確測量,所以實(shí)驗(yàn)采取間接方法測量誤差。因?yàn)闃?biāo)定板上每格邊長是精準(zhǔn)確定的,所以通過測量每格邊長再與實(shí)際邊長比較得到誤差率。標(biāo)定板如圖4所示。

圖4 標(biāo)定板角點(diǎn)圖Fig.4 Photograph of calibration plate corner

求得圖4(a)角點(diǎn)A～I(xiàn)點(diǎn)的三維坐標(biāo),計算點(diǎn)之間距離,結(jié)果如表1所示。

表1 三維重建誤差Table 1 Error of three-dimensional reconstruction

3 姿態(tài)估計

3.1 姿態(tài)估計算法

想要實(shí)現(xiàn)物體抓取還需得到目標(biāo)姿態(tài)。姿態(tài)估計的算法步驟如下。

(1)在模板圖中找到兩個特征點(diǎn)A1、B1,在左圖中找到對應(yīng)匹配點(diǎn)AL、BL。因?yàn)樗R別目標(biāo)物體為圓柱體,為了使線段A1B1更逼近直線,找橫坐標(biāo)最為接近的A1、B1兩點(diǎn)。

(2)計算直線A1B1和直線ALBL斜率k1、kL，求得A1B1與目標(biāo)物體母線的夾角θ1。

(3)左圖中ALBL與母線夾角為θL，可以近似認(rèn)為θL=θ1。根據(jù)公式：

求出左圖中母線斜率k。

(4)在左圖目標(biāo)物體區(qū)域取兩點(diǎn)dL=(xdL,ydL)和eL=(xeL,yeL)，使得dLeL所連直線斜率等于左圖母線斜率k。使用模板匹配找到右圖對應(yīng)兩點(diǎn)dR=(xdR,ydR)和eR=(xeR,yeR)，然后通過三維重建計算出d、e三維坐標(biāo)d=(xd,yd,zd)、e=(xe,ye,ze)

(5)根據(jù)公式：

分別求得物體母線與世界坐標(biāo)系X軸夾角θx，Y軸夾角θy，Z軸夾角θz。空間向量即為n=(cosθx,cosθy,cosθz)。

(6)建立工件坐標(biāo)系，將θx、θy、θz代入式(8)得到姿態(tài)矩陣R。

至此得到了目標(biāo)物體的位置及姿態(tài)，將其組合為4×4的位姿齊次矩陣T。

3.2 姿態(tài)估計實(shí)驗(yàn)結(jié)果

找到兩特征點(diǎn)在模板圖坐標(biāo)分別為A1=(100.367,190.356)，B1=(100.988,225.269)，左圖匹配點(diǎn)坐標(biāo)分別為AL=(376.837,271.843)，BL=(371.361,309.486)，求得tanθ1=0.018，代入式(7)得到母線斜率k=-6.101。從左圖取兩點(diǎn)CL=(386,219)，DL=(396,158)，對應(yīng)右圖坐標(biāo)CR=(306,225)，DR=(316,164)，代入式(2)得兩點(diǎn)三維坐標(biāo)C=(30.421,-13.963,363.541)，D=(35.471,-44.768,363.541)，將兩點(diǎn)代入式(8)可以得到cosθx=0.167 75，cosθy=-0.986 828，cosθz=0，即θx=-80°41′24″，θy=-9°18′36″，θz=0°。

以形心為原點(diǎn)，母線方向上建立工件坐標(biāo)系object，如圖5所示。

圖5 工件坐標(biāo)系及相機(jī)坐標(biāo)系Fig.5 Photograph of workpiece coordinate system and camera coordinate system

得到位姿矩陣為

3.3 姿態(tài)估計誤差分析

對于姿態(tài)的誤差,首先將目標(biāo)物體豎直放置在水平平面,按姿態(tài)的求取方法求取其空間向量,再與垂直放置的物體的標(biāo)準(zhǔn)空間向量n=(0,1,0)作比較,得到誤差角,實(shí)驗(yàn)場景如圖6所示。

圖6 實(shí)驗(yàn)場景Fig.6 Experimental scene

得到的誤差結(jié)果如表2所示。由表2可知,誤差角在2°以內(nèi),證明姿態(tài)估計結(jié)果準(zhǔn)確可靠。

表2 位姿估計誤差Table 2 Error of pose estimation

4 目標(biāo)抓取實(shí)驗(yàn)

4.1 手眼標(biāo)定

手眼標(biāo)定要求得到相機(jī)坐標(biāo)系到機(jī)器人基坐標(biāo)系的坐標(biāo)轉(zhuǎn)換關(guān)系。實(shí)驗(yàn)所采取的手眼關(guān)系為eye-in-hand。標(biāo)定過程中機(jī)器人末端需移動兩個位置,且標(biāo)定板與機(jī)器人底座保持不變。則可以建立以下坐標(biāo)系關(guān)系等式：

經(jīng)過計算得到：

4.2 目標(biāo)抓取

抓取所用機(jī)器人選用kukaKR5，圖7所示為實(shí)驗(yàn)中各坐標(biāo)系關(guān)系圖，為實(shí)現(xiàn)抓取，需將求得位姿從相機(jī)坐標(biāo)系轉(zhuǎn)化到機(jī)器人基坐標(biāo)系。當(dāng)拍攝目標(biāo)時工具坐標(biāo)系到基坐標(biāo)系的變換矩陣為

圖7 實(shí)驗(yàn)坐標(biāo)系關(guān)系圖Fig.7 Experimental coordinate system diagram

則相機(jī)坐標(biāo)系到基坐標(biāo)系變換關(guān)系為

求得其一組合理的逆解為

q=[0.004 8,0.637 9,-0.199 2,-0.217 3,0.849 7,1.413]。

將其用MATLAB仿真得到圖8所示結(jié)果,這里的逆解規(guī)定機(jī)器人為右旋、肘部向上、腕部不翻轉(zhuǎn),所以可以得到唯一解。證明手眼標(biāo)定正確,且機(jī)器人能夠?qū)崿F(xiàn)在工作范圍內(nèi)的目標(biāo)抓取。

圖8 機(jī)器人抓取位置圖 Fig.8 Photograph of robot grab position

5 結(jié)論

(1)目標(biāo)識別采用SURF算法配合雙重匹配點(diǎn)過濾算法,通過Grabcut算法將目標(biāo)從圖像中分割,達(dá)到目標(biāo)識別的目的。實(shí)驗(yàn)結(jié)果表明:目標(biāo)識別準(zhǔn)確,且不受場景影響,圖像分割效果良好。

(2)三維重建中通過尋找及填充目標(biāo)輪廓找到左圖像中目標(biāo)形心,模板匹配得到形心在右圖像上對應(yīng)坐標(biāo),根據(jù)公式求出形心三維坐標(biāo),實(shí)現(xiàn)二維重建三維。誤差分析證明實(shí)驗(yàn)結(jié)果正確、可靠。

(3)姿態(tài)估計中通過物體母線在圖像中的斜率求得母線的空間向量,繼而求出母線與各坐標(biāo)軸的夾角,之后建立工件坐標(biāo)系得到物體姿態(tài)矩陣。實(shí)驗(yàn)結(jié)果證明，不同場景、不同視角均可得到相對準(zhǔn)確的姿態(tài)。

(4)目標(biāo)抓取中,手眼標(biāo)定確定相機(jī)坐標(biāo)系到機(jī)器人基坐標(biāo)系的關(guān)系；根據(jù)kukaKR5參數(shù)求運(yùn)動學(xué)逆解,得到一組能夠抓取目標(biāo)的關(guān)節(jié)角。得到的實(shí)驗(yàn)結(jié)果證明目標(biāo)物體在機(jī)器人工作范圍內(nèi),且可實(shí)現(xiàn)準(zhǔn)確抓取。