基于雙目結(jié)構(gòu)光的工件位姿檢測(cè)方法

宛江洪，曹雛清，高 靖

(1.安徽工程大學(xué)電氣工程學(xué)院，安徽蕪湖 241000；2.安徽工程大學(xué)計(jì)算機(jī)與信息學(xué)院，安徽蕪湖 241000；3.蕪湖哈特機(jī)器人產(chǎn)業(yè)技術(shù)研究院有限公司，安徽蕪湖 241000)

0 引言

工業(yè)機(jī)器人在生產(chǎn)中執(zhí)行抓取、搬運(yùn)任務(wù)時(shí)需要獲取工件位姿，常用的工件位姿檢測(cè)方法是基于雙目視覺(jué)位姿檢測(cè)。王才東等[1]利用雙目視覺(jué)檢測(cè)機(jī)械零件的位姿,Z軸方向重復(fù)誤差達(dá)到了3.42°；江士雄等[2]基于雙目視覺(jué)對(duì)4 cm×2 cm×2 cm大小的矩形塊檢測(cè)位姿，X軸方向平均平移誤差和平均角度誤差為3.1 mm和6°；牛晨等[3]基于雙目視覺(jué)檢測(cè)15 cm×10 cm×4 cm大小的箱體位姿，重心坐標(biāo)誤差為1.57 mm，Z軸方向誤差達(dá)到了3.33°。由于雙目視覺(jué)測(cè)量會(huì)隨著基線距離的增加而誤差增大[4-5]，較少用在工件放置范圍大或大尺寸工件位姿檢測(cè)的應(yīng)用場(chǎng)景。因此，提出了一種面向較大范圍的物體空間位姿檢測(cè)系統(tǒng)，實(shí)現(xiàn)了在標(biāo)準(zhǔn)托盤(pán)空間范圍內(nèi)對(duì)工件的準(zhǔn)確位姿檢測(cè)。

1 位姿檢測(cè)系統(tǒng)方案

雙目結(jié)構(gòu)光的位姿檢測(cè)系統(tǒng)由雙目結(jié)構(gòu)光視覺(jué)采集系統(tǒng)、數(shù)據(jù)處理、點(diǎn)云配準(zhǔn)3個(gè)主要部分組成，其工作流程圖如圖1所示。

圖1 位姿檢測(cè)系統(tǒng)工作流程

圖1中，雙目結(jié)構(gòu)光獲取點(diǎn)云信息后，進(jìn)行點(diǎn)云數(shù)據(jù)處理，通過(guò)直通濾波、分層遍歷尋找、點(diǎn)云數(shù)據(jù)矯正后，將點(diǎn)云信息轉(zhuǎn)換為2D圖像，再進(jìn)行圖像處理，通過(guò)圖像二值化、腐蝕、輪廓檢測(cè)完成多個(gè)工件分割并得到位置信息。在點(diǎn)云配準(zhǔn)中，利用圖像處理中獲得的位置信息進(jìn)行點(diǎn)云粗配準(zhǔn)，再進(jìn)行ICP(iterative closesf point)精配準(zhǔn)[6]，獲得模板與工件的轉(zhuǎn)換矩陣，從而得到工件位姿信息。

2 雙目結(jié)構(gòu)光三維視覺(jué)系統(tǒng)

雙目結(jié)構(gòu)光三維視覺(jué)系統(tǒng)如圖2(a)所示，主要由固定機(jī)架、2個(gè)工業(yè)相機(jī)和1個(gè)投影儀組成，實(shí)驗(yàn)平臺(tái)如圖2(b)所示，雙目結(jié)構(gòu)光三維視覺(jué)系統(tǒng)安裝高度為3 m，實(shí)驗(yàn)在1 200 mm×1 000 mm標(biāo)準(zhǔn)托盤(pán)上進(jìn)行，實(shí)驗(yàn)工件選取600 mm×400 mm×300 mm料框。

(a)雙目結(jié)構(gòu)光三維視覺(jué)系統(tǒng)

(b)位姿檢測(cè)實(shí)驗(yàn)平臺(tái)圖2 雙目結(jié)構(gòu)光和位姿檢測(cè)實(shí)驗(yàn)平臺(tái)

3 數(shù)據(jù)處理

3.1 點(diǎn)云處理

在采集工件點(diǎn)云數(shù)據(jù)時(shí)，利用直通濾波將獲取的點(diǎn)云數(shù)據(jù)限定在需要的范圍內(nèi)。尋找工件最上層，是通過(guò)對(duì)相機(jī)Z軸分層遍歷完成。具體步驟如下：

(1)找到相機(jī)Z軸點(diǎn)云數(shù)據(jù)最小值Zmin并確定分層高度H和閾值S；(2)循環(huán)遍歷(Zmin，Zmin+H)、(Zmin+H，Zmin+2H)…范圍內(nèi)的點(diǎn)云數(shù)目；(3)當(dāng)范圍內(nèi)的點(diǎn)云數(shù)目超過(guò)閾值S時(shí)，循環(huán)停止，此時(shí)認(rèn)為找到了工件的最上層。

安裝雙目結(jié)構(gòu)光三維視覺(jué)系統(tǒng)時(shí)，不能保證相機(jī)垂直于地面，因此得到的點(diǎn)云數(shù)據(jù)會(huì)有一定程度的傾斜。為矯正點(diǎn)云數(shù)據(jù)，利用工件最上層平面法向作為Z軸，重新建立坐標(biāo)系來(lái)矯正原始點(diǎn)云數(shù)據(jù)。具體步驟如下：

(1)對(duì)最上層點(diǎn)云數(shù)據(jù)RANSAC平面擬合，得到平面方程見(jiàn)式(1)，由平面方程知法向量為(A，B，C)，并求得質(zhì)心坐標(biāo)Cm(xm,ym,zm)。

Ax+By+Cz+D=0

(1)

(2)以工件最上層平面法向量為Z軸，保持Y軸主方向不變，利用Y軸和Z軸叉乘得到X軸，通過(guò)X軸和Z軸叉乘得到Y(jié)軸，并以質(zhì)心Cm為坐標(biāo)原點(diǎn)建立坐標(biāo)系。設(shè)原坐標(biāo)系為(e1,e2,e3)，其中e1(1,0,0)T，e2(0,1,0)T,e3(0,0,1)T，新坐標(biāo)系為(e′1,e′2,e′3)，已知e′3=(A,B,C)T，則

e′2=e′3×ei=(0,C,-B)T

(2)

e′1=e′2×e′3=(B2+C2,-AB,-AC)T

(3)

則原坐標(biāo)系到新建坐標(biāo)系的旋轉(zhuǎn)矩陣m為

(4)

(3)已知質(zhì)心Cm在原坐標(biāo)系坐標(biāo)為(xm,ym,zm)且新建坐標(biāo)系原點(diǎn)在質(zhì)心處，從原坐標(biāo)系到新建坐標(biāo)系的轉(zhuǎn)換矩陣為T(mén)，利用轉(zhuǎn)換矩陣T完成點(diǎn)云數(shù)據(jù)的矯正。

(5)

3.2 圖像處理

3.2.1 點(diǎn)云轉(zhuǎn)圖像

矯正點(diǎn)云數(shù)據(jù)后，將上層點(diǎn)云映射到平面，利用KD-tree近鄰檢索[7]，將映射到平面的每個(gè)點(diǎn)云數(shù)據(jù)對(duì)應(yīng)成像素點(diǎn)轉(zhuǎn)成2D圖像如圖3所示。具體步驟如下：

(1)工件上層點(diǎn)云數(shù)據(jù)映射到平面；(2)計(jì)算映射到平面的極值Xmax、Xmin、Ymax、Ymin，并根據(jù)極值創(chuàng)建相應(yīng)大小的Mat；(3)建立kdtree索引，由Xmin開(kāi)始逐行近鄰檢索，有點(diǎn)云數(shù)據(jù)對(duì)應(yīng)Mat存入像素點(diǎn)，直到全部搜索完成，得到圖像數(shù)據(jù)。

圖3 圖像數(shù)據(jù)

3.2.2 輪廓檢測(cè)

圖像輪廓不僅能提供物體的邊緣，還能提供物體邊緣之間的層次關(guān)系以及拓?fù)潢P(guān)系。通過(guò)輪廓檢測(cè)函數(shù)檢測(cè)圖像中的輪廓信息，計(jì)算出各個(gè)輪廓之間的結(jié)構(gòu)信息。最小外接矩形函數(shù)可以根據(jù)輸入的二維點(diǎn)集合計(jì)算最小外接矩形并得到矩形的中心位置、矩形的邊長(zhǎng)以及矩形的旋轉(zhuǎn)角度。通過(guò)輪廓檢測(cè)函數(shù)找到最外層輪廓并判斷是否有下一個(gè)同級(jí)輪廓來(lái)確定料箱個(gè)數(shù)i，用最小外接矩形函數(shù)計(jì)算出最小外接矩形，得到工件的中心位置(Xc,Yc)以及旋轉(zhuǎn)角度angle用于接下來(lái)的點(diǎn)云粗配準(zhǔn)。具體步驟如下：

(1)對(duì)圖像進(jìn)行二值化、腐蝕處理，尋找最外層輪廓；(2)計(jì)算最外層輪廓的最小外接矩形，輸出矩形的中心坐標(biāo)(Xc,Yc)及旋轉(zhuǎn)角度angle；(3)判斷有無(wú)同級(jí)輪廓，有同級(jí)輪廓?jiǎng)t回到第2步，料箱個(gè)數(shù)為i=同級(jí)輪廓數(shù)+1；無(wú)同級(jí)輪廓?jiǎng)t輪廓檢測(cè)結(jié)束，料箱個(gè)數(shù)i=1。

4 點(diǎn)云配準(zhǔn)

通過(guò)配準(zhǔn)獲得源點(diǎn)云和目標(biāo)點(diǎn)云的轉(zhuǎn)換矩陣計(jì)算工件位姿。點(diǎn)云配準(zhǔn)分為粗配準(zhǔn)和精配準(zhǔn)2部分，在精配準(zhǔn)中常用的方法是ICP算法[8]。ICP配準(zhǔn)需要一個(gè)旋轉(zhuǎn)平移矩陣的初值[9]，若初值選取的不好，會(huì)造成局部最優(yōu)配準(zhǔn)，影響配準(zhǔn)效果。

4.1 粗配準(zhǔn)

配準(zhǔn)需要2個(gè)點(diǎn)云數(shù)據(jù)，目標(biāo)點(diǎn)云和源點(diǎn)云。目標(biāo)點(diǎn)云是工件上層點(diǎn)云，源點(diǎn)云是事先做好的模板如圖4所示，并且源點(diǎn)云質(zhì)心在坐標(biāo)原點(diǎn)位置。將源點(diǎn)云與目標(biāo)點(diǎn)云短邊中點(diǎn)位置進(jìn)行配準(zhǔn)。首先對(duì)目標(biāo)點(diǎn)云進(jìn)行處理，將目標(biāo)點(diǎn)云的質(zhì)心移到坐標(biāo)原點(diǎn)位置。在圖像處理中，得到了在圖像信息里工件中心位置(Xc,Yc)，工件長(zhǎng)邊L以及旋轉(zhuǎn)角度angle，則得到轉(zhuǎn)換矩陣T1，通過(guò)轉(zhuǎn)換矩陣T1將目標(biāo)點(diǎn)云的質(zhì)心移到坐標(biāo)原點(diǎn)處。

圖4 源點(diǎn)云

(6)

為使ICP配準(zhǔn)更準(zhǔn)確快速，結(jié)合料框長(zhǎng)邊L得到ICP配準(zhǔn)旋轉(zhuǎn)平移矩陣T2(R,t)初值。

(7)

式中：R為旋轉(zhuǎn)矩陣；t為平移向量。

若先配X軸正方向，則a=1，反之a(chǎn)=-1。

4.2 精配準(zhǔn)

ICP算法的基本原理是：在源點(diǎn)云P和目標(biāo)點(diǎn)云Q中，按照約束條件找到轉(zhuǎn)換矩陣T(R,t)，使得源點(diǎn)云P中的點(diǎn)經(jīng)過(guò)轉(zhuǎn)換矩陣T(R,t)變換，與目標(biāo)點(diǎn)云Q中的點(diǎn)誤差最小，誤差函數(shù)為E(R，t)。

(8)

其中pi∈P，qi為pi在目標(biāo)點(diǎn)云Q中找到的配準(zhǔn)點(diǎn)，配準(zhǔn)結(jié)果如圖5(a)所示。

4.3 真實(shí)場(chǎng)景粗配準(zhǔn)分析

比較使用本文粗配準(zhǔn)方法與常用的粗配準(zhǔn)方法配準(zhǔn)，再進(jìn)行ICP配準(zhǔn)所獲得的結(jié)果，如圖5所示。FPFH(fast point feature histograms)[10]粗配準(zhǔn)方法效果如圖5(b)所示，3DSC(3D shape context)[11]粗配準(zhǔn)方法效果如圖5(c)所示，配準(zhǔn)結(jié)果均不能配準(zhǔn)到短邊中點(diǎn)位置，本文的粗配準(zhǔn)方法配準(zhǔn)(如圖5(a)所示)效果較好。

(a)本文方法

(b)FPFH粗配準(zhǔn)方法

(c)3DSC粗配準(zhǔn)方法圖5 配準(zhǔn)方法實(shí)際效果

5 實(shí)驗(yàn)誤差分析

為驗(yàn)證位姿測(cè)量數(shù)據(jù)的準(zhǔn)確性，實(shí)驗(yàn)先對(duì)1個(gè)料箱位姿重復(fù)檢測(cè)5次，實(shí)驗(yàn)數(shù)據(jù)如表1所示。再同時(shí)對(duì)2個(gè)料箱位姿重復(fù)檢測(cè)5次，實(shí)驗(yàn)數(shù)據(jù)如表2所示。

表1 1個(gè)料箱位姿數(shù)據(jù)表

表2 2個(gè)料箱位姿數(shù)據(jù)表

表1是1個(gè)料箱位姿檢測(cè)結(jié)果，系統(tǒng)對(duì)料箱在繞Z軸、Y軸和X軸方向旋轉(zhuǎn)角度測(cè)量波動(dòng)分別僅有0～0.073°、0～0.049°和0～0.104°，質(zhì)心位置最大波動(dòng)為0.413 mm。表2是同時(shí)放置2個(gè)料箱位姿檢測(cè)結(jié)果，料箱1：在繞Z軸、Y軸和X軸方向旋轉(zhuǎn)角度測(cè)量波動(dòng)分別僅有0～0.092°、0～0.025°和0～0.234°，質(zhì)心位置最大波動(dòng)為0.37 mm。料箱2：在繞Z軸、Y軸和X軸方向旋轉(zhuǎn)角度測(cè)量波動(dòng)分別僅有0～0.118°、0～0.027°和0～0.283°，質(zhì)心位置最大波動(dòng)為0.265 mm。位姿測(cè)量結(jié)果穩(wěn)定，可滿足工業(yè)機(jī)器人上下料抓取需求，抓取實(shí)驗(yàn)如圖6所示。

圖6 抓取實(shí)驗(yàn)

6 結(jié)論

面向機(jī)器人在柔性上下料中工件尺寸大、擺放位姿不確定的應(yīng)用場(chǎng)景，提出了一種基于雙目結(jié)構(gòu)光的框型工件位姿檢測(cè)系統(tǒng)。視覺(jué)采集部分采用了雙目結(jié)構(gòu)光來(lái)獲取點(diǎn)云信息；在數(shù)據(jù)處理部分，采用點(diǎn)云與2D圖像相結(jié)合的方式，完成點(diǎn)云的粗配準(zhǔn)以及工件分割；最終利用ICP配準(zhǔn)，得到工件位姿信息。實(shí)驗(yàn)結(jié)果表明，無(wú)論是對(duì)一個(gè)料框還是多個(gè)料框的位姿檢測(cè)效果都較好，檢測(cè)系統(tǒng)穩(wěn)定，并已經(jīng)應(yīng)用到料箱抓取搬運(yùn)系統(tǒng)。