翟敬梅, 黃 樂
(華南理工大學(xué) 機(jī)械與汽車工程學(xué)院, 廣州 510641)
機(jī)器人的自動(dòng)化分揀大大提高了生產(chǎn)力,在零件裝配、搬運(yùn)碼垛以及物流分揀等領(lǐng)域中具有廣泛的應(yīng)用前景。然而,在一些復(fù)雜場(chǎng)景中目標(biāo)任意堆放、相互遮擋等問題,給機(jī)器人的自動(dòng)化分揀帶來了巨大的挑戰(zhàn),其中獲取目標(biāo)的6D位姿是機(jī)器人成功分揀的關(guān)鍵。目前,已經(jīng)有許多學(xué)者針對(duì)目標(biāo)位姿估計(jì)研究提出了可行的方法。Rad等[1]基于卷積神經(jīng)網(wǎng)絡(luò)的方法對(duì)圖像中的目標(biāo)進(jìn)行定位和分割,以2D投影點(diǎn)預(yù)測(cè)目標(biāo)的位姿,但該方法對(duì)旋轉(zhuǎn)對(duì)稱目標(biāo)效果較差。Hu等[2]引入一個(gè)分割驅(qū)動(dòng)的位姿估計(jì)框架,以目標(biāo)可見部分的關(guān)鍵點(diǎn)預(yù)測(cè)整體位姿,不足之處在于對(duì)多同類物體的情況表現(xiàn)不好。Pavlakos等[3]利用卷積神經(jīng)網(wǎng)絡(luò)從單一RGB圖像中預(yù)測(cè)一組關(guān)鍵點(diǎn),結(jié)合可變形狀模型估計(jì)目標(biāo)的姿態(tài),無法應(yīng)對(duì)目標(biāo)存在遮擋的情況。在2D-3D對(duì)應(yīng)點(diǎn)匹配的基礎(chǔ)上,為了提高搜索效率,基于投票思想產(chǎn)生了新的位姿估計(jì)方法。Drost等[4]提出了將點(diǎn)對(duì)特征PPF(Point pair features)作為模型的整體描述,對(duì)參數(shù)空間投票求解出目標(biāo)位姿。Wang等[5]提出了一個(gè)端到端的網(wǎng)絡(luò)框架DenseFusion,將RGB數(shù)據(jù)和點(diǎn)云數(shù)據(jù)融合估計(jì)目標(biāo)姿態(tài)。Peng等[6]提出采用PVNet(Pixel-wise voting network)對(duì)投影的2D關(guān)鍵點(diǎn)位置進(jìn)行投票,然后轉(zhuǎn)換為PnP(Perspective-n-point)問題來估算位姿[7]。然而2D關(guān)鍵點(diǎn)的預(yù)測(cè)依賴于強(qiáng)紋理特征,對(duì)于弱紋理目標(biāo)的效果較差,通常采用CAD模型配準(zhǔn)的方式解決此類問題。Hinterstoisser等[8]提出了基于圖像梯度信息和點(diǎn)云法向特征的模板匹配方法來實(shí)時(shí)獲取目標(biāo)位姿,對(duì)于相互重疊等復(fù)雜情況下的物體位姿估計(jì)仍存在不足。Mellado等[9]和Yang等[10]將目標(biāo)點(diǎn)云與模型點(diǎn)云進(jìn)行配準(zhǔn)對(duì)齊獲取目標(biāo)位姿,在含有噪聲情況下具有一定的魯棒性,但是運(yùn)行效率較低,無法達(dá)到實(shí)時(shí)估計(jì)。Kehl等[11]基于擴(kuò)展的SSD范式從RGB圖像中檢測(cè)三維模型實(shí)例以估計(jì)目標(biāo)6D位姿,該方法可以實(shí)時(shí)獲取目標(biāo)位姿,但對(duì)目標(biāo)和模型之間的顏色偏差比較敏感。
現(xiàn)有研究中基于CAD模型配準(zhǔn)的方式獲取目標(biāo)位姿存在著耗時(shí)長、精度低以及色差要求等問題。針對(duì)上述問題,本文先通過點(diǎn)云分割的方式獲取單個(gè)目標(biāo)的點(diǎn)云數(shù)據(jù)進(jìn)行處理,提取3D特征描述子進(jìn)行識(shí)別,避免了顏色偏差的干擾;融合截?cái)嘧钚《?半定松弛算法和迭代最近點(diǎn)算法建立目標(biāo)6D位姿估計(jì)模型,在運(yùn)行效率和精度上均具有一定優(yōu)勢(shì),并完成機(jī)器人的無序分揀實(shí)驗(yàn)。
針對(duì)多目標(biāo)散亂堆疊情況下地機(jī)器人分揀作業(yè),提出了分揀系統(tǒng)框架,如圖1所示。
圖1 堆疊散亂目標(biāo)的無序分揀系統(tǒng)
系統(tǒng)包括3D視覺處理模塊、控制模塊以及機(jī)器人運(yùn)動(dòng)模塊3個(gè)部分,其中3D視覺處理模塊是系統(tǒng)的核心部分。3D視覺處理模塊主要實(shí)現(xiàn)場(chǎng)景點(diǎn)云采集、目標(biāo)分割與識(shí)別以及目標(biāo)6D位姿估計(jì),控制模塊將目標(biāo)的6D位姿信息與機(jī)器人手眼標(biāo)定結(jié)果進(jìn)行位姿轉(zhuǎn)換,獲取目標(biāo)在機(jī)器人基坐標(biāo)系下的6D位姿,最后由機(jī)器人運(yùn)動(dòng)模塊完成目標(biāo)的抓取動(dòng)作。
針對(duì)目標(biāo)堆疊的特性,為便于對(duì)單個(gè)目標(biāo)進(jìn)行處理,先從場(chǎng)景點(diǎn)云中提取出堆疊目標(biāo)點(diǎn)云,并進(jìn)行點(diǎn)云分割,得到分離的各個(gè)目標(biāo)點(diǎn)云。
場(chǎng)景點(diǎn)云為深度相機(jī)采集彩色圖像和深度圖像進(jìn)行融合得到,如圖2(a)所示為深度相機(jī)獲取場(chǎng)景點(diǎn)云的一個(gè)實(shí)例,對(duì)場(chǎng)景點(diǎn)云進(jìn)行預(yù)處理,過濾掉堆疊目標(biāo)周圍的點(diǎn)云,再擬合實(shí)驗(yàn)平臺(tái)平面的點(diǎn)云并去除,得到如圖2(b)所示的堆疊目標(biāo)點(diǎn)云。
(a) 場(chǎng)景點(diǎn)云 (b) 堆疊目標(biāo)點(diǎn)云
由于目標(biāo)之間相互堆疊,處于下層的目標(biāo)往往會(huì)被遮擋。對(duì)于該類目標(biāo),一方面機(jī)械臂執(zhí)行器很難到達(dá)抓取點(diǎn)位置,即使抓取到目標(biāo)點(diǎn),也會(huì)在運(yùn)動(dòng)中帶動(dòng)上層目標(biāo),很可能導(dǎo)致目標(biāo)抓飛。同時(shí)考慮到盡量避免抓取目標(biāo)周邊環(huán)境的干擾,故確定每一輪抓取目標(biāo)是從上至下進(jìn)行的。因此首先采用局部凸性連接方法對(duì)堆疊目標(biāo)點(diǎn)云進(jìn)行分割[12-13],該算法可以無監(jiān)督地將堆疊散亂的目標(biāo)分割成一個(gè)個(gè)獨(dú)立的點(diǎn)云子集,雖然被遮擋的目標(biāo)分割成了多個(gè)部分,但最上面一層的目標(biāo)是完整的。圖3為目標(biāo)點(diǎn)云分割的一個(gè)實(shí)例,其中不同顏色的點(diǎn)云代表著分割出的各個(gè)目標(biāo)。
圖3 點(diǎn)云分割實(shí)例
結(jié)合實(shí)際點(diǎn)云分割的情況,上層目標(biāo)相對(duì)下層目標(biāo)距離相機(jī)的距離更近,該距離可以衡量為目標(biāo)點(diǎn)云質(zhì)心到深度相機(jī)坐標(biāo)系原點(diǎn)的距離;此外由于上層目標(biāo)沒有被其他目標(biāo)所遮擋,其點(diǎn)云也會(huì)相對(duì)完整,因此點(diǎn)云中點(diǎn)的數(shù)量比下層目標(biāo)多。綜合以上兩個(gè)因素,為了衡量分割出的各個(gè)目標(biāo)之間的抓取優(yōu)先級(jí),定義了抓取分?jǐn)?shù)為
(1)
式中:w1和w2分別為點(diǎn)云質(zhì)心位置和點(diǎn)云數(shù)量的權(quán)重系數(shù),Z表示點(diǎn)云質(zhì)心到深度相機(jī)坐標(biāo)系原點(diǎn)的距離,N表示點(diǎn)云中點(diǎn)的數(shù)量,Nt是一個(gè)常數(shù)。因?yàn)閷?duì)于分揀而言點(diǎn)云質(zhì)心位置更為重要,所以分別設(shè)置權(quán)重系數(shù)w1=0.8和w2=0.2。
根據(jù)抓取分?jǐn)?shù)從圖3中篩選出候選抓取目標(biāo),其中深紫色、淺紫色和棕色點(diǎn)云分別代表抓取分?jǐn)?shù)最高、次高和第3高的目標(biāo)。
不同形狀目標(biāo)進(jìn)行抓取時(shí)的位姿各不相同,目標(biāo)識(shí)別的目的在于找到與待抓取目標(biāo)點(diǎn)云所匹配的模型點(diǎn)云,以此獲取其6D位姿。針對(duì)目標(biāo)已知的CAD模型,設(shè)計(jì)了一種基于匹配相似度函數(shù)的目標(biāo)識(shí)別方法。
模型點(diǎn)云是通過建模軟件生成CAD模型后,采樣其表面的點(diǎn)形成,并經(jīng)過體素濾波的方式減少點(diǎn)的數(shù)量以提高點(diǎn)云識(shí)別過程的運(yùn)行效率。從目標(biāo)點(diǎn)云和各個(gè)模型點(diǎn)云中提取SIFT3D(3D Scale Invariant Feature Transform)關(guān)鍵點(diǎn)[14],根據(jù)這些關(guān)鍵點(diǎn)計(jì)算FPFH(Fast Point Feature Histograms)特征[15]。FPFH特征是基于查詢點(diǎn)與其近鄰點(diǎn)之間的空間差異形成的一個(gè)多維直方圖對(duì)點(diǎn)的近鄰進(jìn)行幾何描述。直方圖提供的信息對(duì)于點(diǎn)云具有平移旋轉(zhuǎn)不變性,對(duì)采樣密度和噪聲點(diǎn)具有穩(wěn)健性,因此一定程度上可以描述點(diǎn)云的局部特征,其計(jì)算公式為
(2)
式中pq為提取出的一個(gè)關(guān)鍵點(diǎn),pk為pq的近鄰點(diǎn),k為近鄰點(diǎn)的數(shù)目,wk為距離權(quán)重,S(·)表示只計(jì)算查詢點(diǎn)與近鄰點(diǎn)之間的特征元素。
對(duì)每一個(gè)關(guān)鍵點(diǎn)與其近鄰點(diǎn)計(jì)算FPFH特征,將目標(biāo)點(diǎn)云的FPFH特征與各個(gè)模型點(diǎn)云的FPFH特征進(jìn)行匹配,根據(jù)每一對(duì)目標(biāo)與模型的特征匹配點(diǎn)對(duì)數(shù)量設(shè)計(jì)了匹配相似度函數(shù)來衡量目標(biāo)與模型的匹配程度,其定義為
(3)
式中:ci為目標(biāo)與第i個(gè)模型的匹配點(diǎn)對(duì)數(shù)量,t為模型的數(shù)目,M所對(duì)應(yīng)的模型為目標(biāo)識(shí)別的結(jié)果。目標(biāo)識(shí)別方法的具體流程如圖4所示。
圖4 目標(biāo)識(shí)別流程
精確的目標(biāo)6D位姿估計(jì)是保證機(jī)械臂準(zhǔn)確抓取目標(biāo)的重要前提。針對(duì)目標(biāo)點(diǎn)云和模型點(diǎn)云重合率低的問題,融合截?cái)嘧钚《?半定松弛算法TEASER(Truncated least squares Estimation And SEmidefinite Relaxation)[16]和迭代最近點(diǎn)算法ICP(Iterative Closest Point)[17]解決點(diǎn)云配準(zhǔn)的問題,實(shí)現(xiàn)目標(biāo)在機(jī)器人基坐標(biāo)系下的6D位姿估計(jì)。
(4)
式中:βi為給定的噪聲邊界,R為待求的旋轉(zhuǎn)變換,t為待求的平移變換。
窮舉搜索式(4)全局最優(yōu)解的時(shí)間復(fù)雜度為指數(shù)時(shí)間O(2N),為了提高算法的運(yùn)行效率,對(duì)問題解耦形式化,轉(zhuǎn)換問題為旋轉(zhuǎn)和平移不變量的測(cè)量。旋轉(zhuǎn)不變量的估計(jì)為
(5)
根據(jù)估計(jì)出的旋轉(zhuǎn)不變量,再對(duì)平移不變量進(jìn)行估計(jì):
(6)
TEASER算法可以快速獲取目標(biāo)點(diǎn)云和模型點(diǎn)云之間的位姿轉(zhuǎn)換關(guān)系,但轉(zhuǎn)換精度不高。為了得到更精確的位姿轉(zhuǎn)換關(guān)系,基于ICP算法進(jìn)行局部位姿修正。將估計(jì)出的旋轉(zhuǎn)和平移不變量作為ICP算法的初始估計(jì)位姿,使式(7)中的目標(biāo)函數(shù)取得最小值。
(7)
(8)
式中:n為目標(biāo)點(diǎn)云與模型點(diǎn)云匹配點(diǎn)對(duì)數(shù)量,R*為所求的旋轉(zhuǎn)變換,t*為所求的平移變換,二者組合成4×4齊次變換矩陣
(9)
通過手眼標(biāo)定,獲取相機(jī)坐標(biāo)系與機(jī)器人基坐標(biāo)系之間的轉(zhuǎn)換關(guān)系,其結(jié)果表示為
(10)
目標(biāo)在機(jī)器人基坐標(biāo)系下的6D位姿估計(jì)Hp表示為
(11)
圖5 目標(biāo)6D位姿估計(jì)過程
實(shí)驗(yàn)中測(cè)試的對(duì)象采用凹型塊(長125 mm、寬50 mm、高30 mm)、圓柱(直徑50 mm、高125 mm)以及四棱柱(長100 mm、寬50 mm、高50 mm)3類目標(biāo)組成。將12個(gè)目標(biāo)(凹型塊8個(gè),圓柱2個(gè)以及四棱柱2個(gè))隨意散亂堆疊在實(shí)驗(yàn)平臺(tái)上,采集點(diǎn)云數(shù)據(jù)提取堆疊目標(biāo)點(diǎn)云并分割,獲取如圖6所示的目標(biāo)點(diǎn)云分割結(jié)果。從圖6的分割點(diǎn)云中選取序號(hào)為1、2、3、4、5和6的6個(gè)目標(biāo)進(jìn)行6D位姿估計(jì)實(shí)驗(yàn),根據(jù)2.4節(jié)介紹的方法將這6個(gè)目標(biāo)分別與對(duì)應(yīng)的模型點(diǎn)云配準(zhǔn),獲取各個(gè)目標(biāo)在機(jī)器人基坐標(biāo)系下的6D位姿。為了評(píng)估文中6D位姿估計(jì)方法的性能,與現(xiàn)有流行的3種方法(基于RANSAC算法[18],基于NDT算法[19],以及基于3DSC算法[20])進(jìn)行比較。
圖6 目標(biāo)點(diǎn)云分割結(jié)果
為了定量衡量實(shí)驗(yàn)結(jié)果的精度,定義了均方根距離誤差De和均方根角度誤差Ne兩個(gè)評(píng)價(jià)指標(biāo)對(duì)結(jié)果進(jìn)行分析
(12)
(13)
式中m表示目標(biāo)點(diǎn)云和模型點(diǎn)云配準(zhǔn)后匹配點(diǎn)對(duì)的數(shù)量,Xpi和Xqi分別表示目標(biāo)點(diǎn)云和模型點(diǎn)云配準(zhǔn)后對(duì)應(yīng)點(diǎn)的空間三維坐標(biāo)值,npi和nqi分別表示目標(biāo)點(diǎn)云和模型點(diǎn)云配準(zhǔn)后對(duì)應(yīng)點(diǎn)的法向量。因此評(píng)價(jià)指標(biāo)De和Ne分別衡量的是目標(biāo)點(diǎn)云中所有的點(diǎn)通過估計(jì)出的6D位姿變換后與模型點(diǎn)云中的對(duì)應(yīng)點(diǎn)的距離和法向量夾角的平均值,這兩個(gè)指標(biāo)的值越小,則目標(biāo)點(diǎn)云與模型點(diǎn)云的匹配程度越高,即6D位姿估計(jì)越準(zhǔn)確。
序號(hào)為2、3、6目標(biāo)在機(jī)器人基坐標(biāo)系下的6D位姿估計(jì)結(jié)果如圖7所示。統(tǒng)計(jì)文中算法和另外3種算法的位姿估計(jì)誤差和平均運(yùn)行時(shí)間,計(jì)算出的De和Ne如表1所示,平均運(yùn)行時(shí)間Ta如表2所示。
(a) 序號(hào)2目標(biāo) (b) 序號(hào)3目標(biāo) (c) 序號(hào)6目標(biāo)
表1 不同方法的位姿估計(jì)誤差對(duì)比
表2 不同方法的平均運(yùn)行時(shí)間對(duì)比
從表1結(jié)果可以看出,對(duì)于不同目標(biāo)的6D位姿估計(jì),文中算法的De在3.3 mm之內(nèi),Ne在5.6°之內(nèi),誤差在無序分揀系統(tǒng)允許的范圍之內(nèi)。與其他3種方法相比,本文方法的位姿估計(jì)誤差都是最小的,說明本文6D位姿估計(jì)方法具有較高的精度和魯棒性。序號(hào)2和5目標(biāo)的De比較大,這是由于這兩個(gè)目標(biāo)表面為曲面,而測(cè)試的目標(biāo)均由3D打印零件組成,其表面紋路凹凸不平,因此采集的點(diǎn)云數(shù)據(jù)與CAD模型有較大誤差,導(dǎo)致點(diǎn)云配準(zhǔn)的誤差較大。從表2可以看出,本文方法的平均運(yùn)行時(shí)間為0.23 s,在這幾種方法中耗時(shí)最短,可以滿足系統(tǒng)分揀的實(shí)時(shí)性要求。
搭建了無序分揀系統(tǒng),在實(shí)際場(chǎng)景中,用機(jī)器人分揀堆疊散亂的目標(biāo)。整個(gè)分揀系統(tǒng)主要由Kinect V2深度相機(jī)、UR3e機(jī)器人、機(jī)器人控制器、真空吸盤、真空泵以及工控機(jī)組成。
實(shí)驗(yàn)以3.1節(jié)中的實(shí)驗(yàn)對(duì)象作為分揀對(duì)象,其中凹型塊和四棱柱零件的抓取位置為平面,而圓柱零件的抓取位置為曲面。系統(tǒng)抓取目標(biāo)后,將目標(biāo)放置到對(duì)應(yīng)的位置釋放后,機(jī)械臂返回到初始位置,等待下一個(gè)目標(biāo)的抓取。重復(fù)上述步驟,直至將工作臺(tái)上的目標(biāo)全部分揀完成。然后再次隨機(jī)散亂放置零件在實(shí)驗(yàn)臺(tái)上,重復(fù)完成5組實(shí)驗(yàn),統(tǒng)計(jì)5組實(shí)驗(yàn)中目標(biāo)的分揀結(jié)果來評(píng)估分揀系統(tǒng)的性能。
圖8為無序分揀系統(tǒng)在第1組實(shí)驗(yàn)過程中前3個(gè)目標(biāo)的抓取過程圖。圖8(a)為第一個(gè)目標(biāo)(序號(hào)1)抓取,抓取完并放置目標(biāo)后,機(jī)械臂返回初始位置,進(jìn)行下一個(gè)目標(biāo)的抓取。從圖8可以看出,前3個(gè)目標(biāo)(序號(hào)1、2、3)都被順利分揀出來,證明該系統(tǒng)可以應(yīng)用于堆疊目標(biāo)的分揀場(chǎng)景中。
圖8 無序分揀系統(tǒng)工作過程圖(從左至右依次為:堆疊目標(biāo)圖片、最上層目標(biāo)篩選、目標(biāo)6D位姿估計(jì)、機(jī)器人抓取運(yùn)動(dòng)軌跡以及目標(biāo)實(shí)際抓取圖)
為了定量評(píng)價(jià)該分揀系統(tǒng)的性能,從抓取成功率和抓取時(shí)間兩個(gè)方面進(jìn)行評(píng)估。抓取成功率Gp的定義為
Gp=ns/ng×100%
(14)
式中ng為每組實(shí)驗(yàn)中總的抓取目標(biāo)次數(shù),ns表示每組實(shí)驗(yàn)中正確分揀目標(biāo)的次數(shù),正確分揀目標(biāo)表示成功將目標(biāo)抓取并放置到特定區(qū)域中。
系統(tǒng)的抓取時(shí)間Tg的定義為
Tg=T1+T2
(15 )
式中T1為從采集點(diǎn)云數(shù)據(jù)到估計(jì)目標(biāo)6D位姿所花費(fèi)的時(shí)間,T2為機(jī)器人從抓取目標(biāo)到放置目標(biāo)并返回抓取起始點(diǎn)所花費(fèi)的時(shí)間。因此,Tg衡量的是分揀系統(tǒng)工作一個(gè)周期所需要的時(shí)間。
對(duì)5組實(shí)驗(yàn)數(shù)據(jù)進(jìn)行統(tǒng)計(jì)分析,每組實(shí)驗(yàn)的抓取成功率如表3所示。從表3可以看出,兩組實(shí)驗(yàn)的抓取成功率達(dá)到100%,3組實(shí)驗(yàn)的抓取成功率為91.76%。實(shí)驗(yàn)中抓取失敗的對(duì)象主要是圓柱體,原因在于圓柱的抓取位置為曲面,只有當(dāng)橢圓吸盤長軸方向與圓柱體表面完全貼合,才會(huì)抓取成功,因此對(duì)圓柱體位姿精度要求較高,誤差較大時(shí)容易導(dǎo)致抓取失敗。表4從時(shí)間性能上對(duì)分揀系統(tǒng)進(jìn)行分析,不同分揀對(duì)象的平均抓取時(shí)間在14 s以內(nèi),其中視覺處理的時(shí)間在3 s以內(nèi),遠(yuǎn)小于機(jī)械臂運(yùn)動(dòng)的時(shí)間,可以保證機(jī)器人分揀過程的實(shí)時(shí)性要求。
表3 5組實(shí)驗(yàn)的抓取成功率
表4 不同分揀對(duì)象的平均抓取時(shí)間
針對(duì)堆疊散亂目標(biāo)的無序分揀系統(tǒng),開展了目標(biāo)的點(diǎn)云識(shí)別與位姿估計(jì)研究,根據(jù)目標(biāo)的6D位姿完成機(jī)器人的自動(dòng)化抓取,主要結(jié)論如下:
1) 提出了抓取分?jǐn)?shù)來判斷堆疊目標(biāo)中的最上層目標(biāo),保證系統(tǒng)可以從上至下進(jìn)行抓取目標(biāo),避免出現(xiàn)抓飛的情況。
2) 提出了基于匹配相似度函數(shù)的目標(biāo)識(shí)別方法,并融合TEASER算法和ICP算法實(shí)現(xiàn)精確的目標(biāo)6D位姿估計(jì)
3)在自采數(shù)據(jù)上進(jìn)行實(shí)驗(yàn)驗(yàn)證,本文方法均方根距離誤差<3.3 mm,均方根角度誤差<5.6°,與現(xiàn)有普遍采用的3種方法相比具有更高的位姿估計(jì)精度;在實(shí)際堆疊散亂目標(biāo)的場(chǎng)景下進(jìn)行分揀實(shí)驗(yàn),結(jié)果表明搭建的系統(tǒng)可以實(shí)現(xiàn)目標(biāo)的無序分揀。
該系統(tǒng)適用于多個(gè)目標(biāo)尺寸相近情形下的無序分揀,其性能可以適應(yīng)實(shí)際生產(chǎn)需要。