一種基于輪廓特征匹配的零件位姿估計方法

孫長江, 段旭洋, 王 皓, 陳智超, 徐 鵬

(1.上海交通大學(xué) 上海市復(fù)雜薄板結(jié)構(gòu)數(shù)字化制造重點實驗室,上海 200240,E-mail:scj0013@sjtu.edu.cn;2.上海交通大學(xué) 弗勞恩霍夫協(xié)會智能制造創(chuàng)新中心,上海 201306; 3.上海交通大學(xué) 機械系統(tǒng)與振動國家重點實驗室,上海 200240; 4.上海飛機制造有限公司5G工業(yè)創(chuàng)新中心,上海 201202)

采用機器人進行工業(yè)零件的自動化分揀是制造業(yè)企業(yè)智能化升級的一個重要內(nèi)容,其中目標識別和位姿估計算法的速度和準確性是影響機器人抓取效率和成功率的關(guān)鍵因素[1-2]。由于零件散亂堆疊場景下物體相互遮擋、背景雜亂、傳感器噪聲干擾等問題,準確的零件六維位姿估計仍然面臨著較大的挑戰(zhàn)。

近年來,針對工業(yè)零件的位姿估計方法主要采用基于圖像或三維點云的匹配方法[3-4]。Hinterstoisser等[5-6]提出LINEMOD方法從多視角渲染目標物體模板,根據(jù)場景圖像的邊緣梯度和物體表面法向建立特征進行模板匹配,搜索與場景目標匹配的圖像模板并獲得目標位姿。Rios-Cabrera等[7]在LINEMOD的基礎(chǔ)上使用背景圖片構(gòu)建負樣本集,采用支持向量機對模板進行學(xué)習和聚類從而減少模板數(shù)量提高匹配算法速度。Ulrich等[8]利用CAD模型的邊緣輪廓特征建立分層級模板金字塔,實現(xiàn)單個圖像中三維物體的快速識別和位姿估計。模板匹配的方法通過查找與場景相似的模板可以快速估計物體位姿,但無法應(yīng)對遮擋情況且匹配精度受尺度變化影響較大。Drost等[9]提出基于點對特征(Point Pair Feature, PPF)的匹配方法,通過對模型和場景的點對特征進行匹配來估計目標位姿并聚類獲得最優(yōu)結(jié)果,該方法針對噪聲和部分遮擋仍有較好的識別性能,但算法耗時長。魯榮榮等[10]提出增強型點對特征,減少了點對數(shù)目且區(qū)分性更強。陸軍等[11]使用ISS算法在場景點云中提取關(guān)鍵點,并直接對關(guān)鍵點建立點對特征。Zhou等[12]提出BOLD3D描述子,在遮擋和雜亂場景下實現(xiàn)較高的匹配精度。

綜上所述,采用圖像模板匹配的方法對零件遮擋情況魯棒性差且匹配精度受模板尺度變化影響較大,而采用點云的特征匹配方法存在計算效率低、對點云噪聲敏感等問題。針對上述問題,本文提出了一種基于輪廓特征匹配的位姿估計方法,該方法通過將場景三維點云轉(zhuǎn)換為深度圖來提取場景點云輪廓,采用基于局部點云主方向的區(qū)域生長方法對輪廓點云進行聚類分割,通過采樣輪廓點云提取特征點并建立輪廓點對特征,通過特征點對匹配實現(xiàn)場景中目標的識別和位姿估計,相比于原始點對特征匹配算法在識別效率和識別準確率方面均有所提升。

1 點云輪廓特征提取

1.1 點云輪廓提取

點云輪廓提取需要根據(jù)輸入的點云數(shù)據(jù)提取出場景點云中零件的邊緣輪廓點。通過將場景點云變換為深度圖像后,采用Sobel濾波器進行邊緣檢測,根據(jù)圖像灰度梯度提取出場景輪廓。

(1) 生成場景深度圖像

深度圖像是單通道灰度圖,其中像素的灰度值表示點云中每個點距離相機的深度。已知用于拍攝場景點云的三維結(jié)構(gòu)光相機的內(nèi)參矩陣:

(1)

式中:f表示相機焦距,dx、dy表示像元尺寸,cx、cy表示相機成像中心的像素坐標。對點云中的每個點P(X,Y,Z),根據(jù)相機成像原理可將其映射到二維圖像中,對應(yīng)像素坐標:

(2)

并將點P到相機的深度距離Z線性映射為該像素點的灰度值:

(3)

式中:Zmax和Zmin是場景點云中所有點到相機的最大深度和最小深度,grmax是灰度圖像的最大灰度值,通過設(shè)置場景的最大深度和最小深度,可以擴大灰度范圍提高灰度精度,提升深度圖邊緣檢測的準確度。

(2) 邊緣提取

采用Sobel濾波器對深度圖像進行邊緣檢測,Sobel算子是用于邊緣檢測的離散微分算子,采用兩個卷積核:

(4)

分別從橫縱兩個方向?qū)D像進行卷積,其卷積結(jié)果為兩個方向灰度的一階梯度:

(5)

根據(jù)梯度的范數(shù):

(6)

進行閾值分割和區(qū)域篩選最終提取深度圖像的邊緣結(jié)果。

(3) 生成場景點云邊緣輪廓

對深度圖像中的邊緣區(qū)域,根據(jù)區(qū)域中像素點的灰度值和像素坐標,重新映射得到場景點云的邊緣輪廓。

(7)

(8)

(9)

圖1是對場景點云進行輪廓提取的各步驟結(jié)果,其中目標零件為塑料材質(zhì)的型材端蓋。采用深度圖像提取場景輪廓特征,不會受到環(huán)境光照和零件顏色紋理的影響,穩(wěn)定性更強,且計算效率更高。

1.2 基于主方向一致性的輪廓點云聚類算法

點云輪廓邊緣聚類的目的是對輪廓點云進行分割從而提取出輪廓中的直線邊緣。為了準確的分割出在同一條直線段上的邊緣點云,本文提出了基于主方向一致性的點云區(qū)域生長算法,主要步驟如下:

step 1: 對邊緣點云進行預(yù)處理,剔除其中的離群點,得到初始點集Sorigin;

step 2: 設(shè)置搜索半徑δ,對于每個點pi∈Sorigin,查找其鄰域半徑內(nèi)的所有點記為點集Si,通過主成分分析,計算得到點集的主方向并作為點pi的方向向量ni=PCA(Si)(|ni|=1),并將pi狀態(tài)標記為“未訪問”;

▲圖1 點云輪廓提取過程

step 5: 記錄點集Lj;如果所有點狀態(tài)均為已訪問則區(qū)域生長結(jié)束,得到完成聚類的全部點集L1,L2,…,Lj,否則跳轉(zhuǎn)到step 3。

采用該方法,可以準確的對輪廓點云進行聚類得到每個直線邊緣,同時計算得到直線邊緣的方向向量,具體實驗結(jié)果如圖2所示,其中圖2(a)是輪廓點云,圖2(b)是通過聚類算法得到的直線邊緣點云。

▲圖2 邊緣點聚類結(jié)果

2 輪廓特征描述方法

對提取得到的點云輪廓特征,建立輪廓點對特征描述方法,并構(gòu)建模型全局特征描述。

2.1 輪廓點對特征

輪廓點對特征使用兩個點之間的距離關(guān)系和方向關(guān)系描述不同直線輪廓上的任意兩個點之間的空間關(guān)系。如圖3所示,對直線輪廓L1,L2上的兩個點ml,mr,定義其點對特征:

(10)

▲圖3 輪廓點對特征示意圖

2.2 全局特征描述構(gòu)建

全局特征描述用于表示模型輪廓特征點與點對特征空間的映射關(guān)系。通過構(gòu)建全局特征描述,可以在點對特征匹配階段根據(jù)場景輪廓的點對快速查找到具有相同特征值的模型輪廓點對。

全局特征描述采用哈希表數(shù)據(jù)結(jié)構(gòu)進行存儲,將具有相同特征值的模型輪廓特征點對存儲在同一個索引項中,并能夠根據(jù)特征值快速檢索到對應(yīng)的特征點對。對于模型表面輪廓采樣點集合L中的任意點對(mi,mj)∈L2(i≠j),計算特征向量FB2B(mi,mj)。為降低點對特征的數(shù)量并提高特征匹配的魯棒性,設(shè)定距離采樣步長ddist與角度采樣步長θ,將特征向量中的值離散為采樣步長的整數(shù)倍:

(11)

▲圖4 全局特征描述

3 三維目標位姿估計

基于輪廓特征的三維目標位姿估計算法分為離線和在線兩個階段。如圖5所示。

在離線階段,根據(jù)目標三維模型提取模型輪廓點云,建立模型全局特征描述;在在線階段,分割并聚類得到場景點云的直線輪廓點云,在輪廓點云中選取采樣點建立點對特征并從模型全局特征描述中匹配具有相同特征值的點對,并將場景點對和模型點對進行匹配得到一系列候選模型位姿,最終通過位姿投票和位姿聚類得到場景點云中的一組最佳位姿。

▲圖5 三維目標位姿估計過程

3.1 輪廓點對特征匹配

▲圖6 點對匹配示意圖

(12)

3.2 位姿投票

對于場景輪廓的任意參考點sr,在輪廓點對特征匹配階段會得到多組可能與之對應(yīng)的模型輪廓關(guān)鍵點和旋轉(zhuǎn)角(mr,α),因此在位姿投票階段通過廣義霍夫投票得到參考點sr的最佳提議位姿。

如圖7所示,構(gòu)建用于位姿投票的二維累加數(shù)組,其中每一行對應(yīng)模型輪廓關(guān)鍵點,每一列對應(yīng)離散的旋轉(zhuǎn)角,數(shù)組的行數(shù)為模型輪廓采樣點的數(shù)目,列數(shù)為離散旋轉(zhuǎn)角的數(shù)目。

▲圖7 位姿投票示意圖

3.3 位姿聚類

通過廣義霍夫投票,可以獲得單個場景采樣點sr到模型點云的最佳變換位姿。對于場景中的不同采樣點,需要通過位姿聚類的方式將相似的位姿結(jié)果聚為一類,從而減少位姿數(shù)量,提高位姿精度。

首先按照投票數(shù)由高到低對位姿結(jié)果進行排序,將投票數(shù)最高的位姿創(chuàng)建一個位姿類別;從未聚類的位姿結(jié)果中找到屬于該類別的位姿,其判斷方式為計算未聚類位姿與最高票數(shù)位姿之間的距離和角度偏差,如式(13)所示:

(13)

式中:P*為聚類中心的位姿,P為候選位姿,tr(·)為跡運算,t*和t為P*和P中的平移向量,ε、δ分別為旋轉(zhuǎn)矩陣的角度偏差閾值和平移向量的距離閾值。當偏差小于指定閾值時將該位姿標記為同一類。之后根據(jù)位姿的投票數(shù)采用加權(quán)平均的方式對聚類位姿進行平均,從而得到最終位姿。聚類位姿的加權(quán)平均方法如式(14)、(15)、(16)所示:

(14)

(15)

(16)

通過位姿聚類獲得場景點云中目標物體的一組位姿結(jié)果,根據(jù)位姿結(jié)果的最終投票數(shù)可以篩選出最佳的一個或多個結(jié)果。同時為了優(yōu)化位姿結(jié)果,可以采用迭代最近點算法(Iterative Closest Point, ICP)對位姿結(jié)果進行進一步的精配準。在本文中為了評價位姿估計算法的精度,并未采用ICP算法對位姿結(jié)果進行優(yōu)化。

4 實驗與分析

本文算法采用開源點云庫PCL1.11.1和開源視覺算法庫OpenCV4.5,使用C++語言實現(xiàn),且未采用OpenMP、Cuda等并行加速工具。本文實驗使用的計算機配置為AMD R5 4600H 3.0 GHz處理器,16 GB內(nèi)存,64位Win10操作系統(tǒng)。本文實驗使用開源數(shù)據(jù)集和實驗室采集場景數(shù)據(jù)對所提算法和已有的基于點對特征的匹配算法進行算法運行效率和識別準確率的對比測試。

4.1 實驗數(shù)據(jù)集

為測試算法的運行效率和目標位姿估計準確率,本實驗采用MVTec ITODD[14]數(shù)據(jù)集和實驗室自采數(shù)據(jù)進行算法測試。

ITODD數(shù)據(jù)集是MVTec公司開源的用于工業(yè)零件3D物體檢測和位姿估計數(shù)據(jù)集,該數(shù)據(jù)集由28個工業(yè)場景對象和3 500個標記場景組成。本實驗從ITODD測試數(shù)據(jù)集中選擇三種零件作為待識別的目標物體如圖8(a)、8(b)、8(c)所示。同時,利用實驗室的三維結(jié)構(gòu)光相機拍攝采集了兩種零件型材端蓋如圖8(d)及連接件如圖8(e)的多個場景點云。部分場景如圖9所示,圖9(a)、9(b)為端蓋和連接件的場景圖像,圖9(c)、9(d)為對應(yīng)的場景點云。上述目標零件的尺寸如表1所示。

表1 待識別目標物體尺寸/mm

▲圖8 數(shù)據(jù)集零件實物圖

▲圖9 實際采集的場景數(shù)據(jù)

針對上述目標零件和場景數(shù)據(jù),為評估位姿估計算法的準確率,還需要標定場景中目標零件的真實位置和姿態(tài)。本文采用BOP工具箱[15]中的手動標注工具進行場景目標的位姿標定。首先使用手動方式調(diào)整模型點云的位置和姿態(tài)將點云與場景中目標大致對齊,進一步通過ICP迭代優(yōu)化位姿從而獲得場景目標的真實位置和姿態(tài),最后將位姿結(jié)果記錄保存用于算法識別目標的準確率評估。

4.2 算法識別結(jié)果分析

在機器人零件抓取場景中,針對散亂堆疊零件的自動分揀往往在每次機器人抓取前都需要進行場景點云的拍攝并重新進行位姿估計,因此位姿估計算法的運行效率對整個抓取分揀系統(tǒng)的運行效率有較大影響。同時由于機器人每次只會抓取一個零件,因此對場景中零件的識別數(shù)量無較高要求,而是更關(guān)注識別零件的準確率。本小節(jié)從算法的運行效率和識別準確率兩方面對所提出算法和原始基于點對特征的匹配算法進行對比分析。

(1) 算法運行效率分析

在點對特征匹配算法中,場景點云的點云數(shù)量和選取的關(guān)鍵點的數(shù)量對算法在在線匹配階段的運行速度有很大的影響。關(guān)鍵點數(shù)量越多,在線匹配階段需要匹配的場景點對和模型點對數(shù)量也會更多,從而降低算法運行效率。表2對比了在五個不同零件場景下本文算法和原始PPF算法的關(guān)鍵點數(shù)目和在線匹配階段的識別時間,其中場景I到場景Ⅴ分別對應(yīng)了圖9(a)-圖9(e)的5種零件。

表2 兩種算法在不同場景下的運行速度對比

通過表2可以得出,本文算法相比于原始PPF算法,需要的場景關(guān)鍵點數(shù)目更少,且在時間方面本文算法也有明顯的優(yōu)勢。這是因為本文算法對場景點云進行輪廓提取,并從輪廓點云中采樣得到關(guān)鍵點,相比與原有算法從整個場景點云中采樣關(guān)鍵點需要的關(guān)鍵點數(shù)目更少。

(2) 算法準確率分析

為評估位姿估計算法的準確率,分別測試本文算法和原始PPF算法在場景Ⅰ～Ⅴ中識別到的目標零件數(shù)目和其中正確位姿的數(shù)目并計算對應(yīng)的識別準確率。識別準確率是算法在場景點云中識別到的目標位姿中正確位姿的占比,且當目標位姿與真實位姿的旋轉(zhuǎn)誤差<5°,平移誤差<模型直徑的10%則認為該位姿為正確位姿,即:

(17)

式中:[R,t]和[R*,t*]為目標估計位姿和對應(yīng)的真實位姿,ε是旋轉(zhuǎn)誤差閾值設(shè)為5°,σ是平移誤差閾值設(shè)為0.1Dm。圖10為本文算法在場景Ⅴ中的識別結(jié)果,其中綠色模型為正確位姿,紅色為誤識別位姿。

▲圖10 場景Ⅴ識別結(jié)果

表3列出了兩個算法在不同場景下的目標零件識別準確率對比結(jié)果,從結(jié)果中可以看出本文算法的平均識別準確率為76%,而原始PPF算法的平均識別準確率為58.5%。

表3 兩種算法在不同場景下的識別準確率對比

一方面,由于原始的PPF算法對點云的法向量變化較為敏感,對于表面曲率變化不明顯或者存在較大平面的零件算法效果較差,而本文算法采用場景點云輪廓建立點對特征對表面曲率變化不明顯的零件仍然能夠取得較好的識別效果;另一方面,原始的PPF算法可能會將場景的背景點云誤識別為目標零件從而導(dǎo)致識別的準確率降低。因此,相比于原始的PPF算法,本文算法在識別準確率方面效果更優(yōu)。

5 結(jié)論

本文針對工業(yè)零件的自動化分揀場景中的零件六維位姿估計問題,在點對特征匹配算法的基礎(chǔ)上進行改進,提出了一種基于輪廓特征匹配的目標位姿估計方法。通過將場景點云轉(zhuǎn)換為深度圖并進行邊緣檢測,實現(xiàn)快速的輪廓點云提取,并提出基于主方向一致性的點云聚類算法,對同一邊緣的輪廓點云進行聚類。基于輪廓點對特征的匹配算法所需要的場景關(guān)鍵點數(shù)目更少,算法識別時間也更快。使用ITODD數(shù)據(jù)集和自采集場景數(shù)據(jù)進行算法測試,證明本文算法相比于原始PPF算法在運行速度和識別準確率方面均有提升。