茅凌波,史金龍,周志強,佘若水

(江蘇科技大學(xué) 計算機學(xué)院,江蘇 鎮(zhèn)江 212000)

0 引言

機器人視覺抓取在機器人學(xué)和控制理論的基礎(chǔ)上融合了計算機視覺理論、人工智能等學(xué)科,具有重要的科研和應(yīng)用價值[1],目前,在家庭、辦公、醫(yī)護、工業(yè)等環(huán)境中,智能機器人通常需要完成一些自主抓取任務(wù),如傳遞物品、分揀物料、清理桌面等,需要機器人能夠快速準確地完成對環(huán)境中物體的定位與識別,并實現(xiàn)對目標物體的精確抓取。

其中,物體的定位是智能機器人抓取技術(shù)中的關(guān)鍵問題,不僅需要知道物體在三維空間中的位置[X,Y,Z],還需要知道物體的擺放姿態(tài)[Rx,Ry,Rz],即物體的6D位姿信息。在非結(jié)構(gòu)環(huán)境下,由于物體的位姿容易受到物體的種類、光照變化、遮擋等條件的影響,因此確定物體的位姿很具挑戰(zhàn)[2]?，F(xiàn)階段的機器人抓取工作主要分為以下幾種:

(1)基于傳統(tǒng)算法的物體抓取機制 一部分學(xué)者采用尋找物體輪廓,進而判斷物體擺放方向,并根據(jù)形狀估計物體中心位置的方式,如沈博[3]、楊平[4]等人的機器人抓取工作使用吸盤式抓取工具,只需要物體在三維空間中的位置即可。同樣,張亞輝等[5]利用具有特殊結(jié)構(gòu)的機械手爪進行抓取,也無需知道物體的具體擺放姿態(tài)。但對特定的復(fù)雜抓取場景而言,只有三維空間中的位置,沒有抓取的旋轉(zhuǎn)角度是不夠的。同時,在抓手的旋轉(zhuǎn)角度的定義上,如基于掩膜區(qū)域的卷積神經(jīng)網(wǎng)絡(luò)(Mask Region-based Convolutional Neural Network,Mask R-CNN)與主成分分析(Principal Components Analysis,PCA)[6]相結(jié)合的方式,在分割待抓取物體之后,利用PCA算法得到物體在三維空間中的朝向,進而確定機器人抓手的旋轉(zhuǎn)角度。然而,該算法要求相機始終放置物體正上方,限制了抓取工作的應(yīng)用場景。另一部分學(xué)者則是利用物體點云,提取幾何信息,進而確定抓取的正確位姿。如鄒遇[7]等通過提取物體的邊界點云,結(jié)合質(zhì)心距獲取物體表面的最佳抓取點,梁颯等[8]則是通過加速穩(wěn)健特征(Speeded Up Robust Features,SURF)[9]提取特征重建場景點云,然后利用最近點迭代(Iterative Closest Point,ICP)[10]調(diào)整確定物體位姿信息。此類方法精確度高,但是相較于其他方法,算法成本較高且耗時較長。

(2)基于模板匹配的物體抓取機制 首先設(shè)置可成功抓取的物體位姿信息作為模板,然后利用深度神經(jīng)網(wǎng)絡(luò)將圖像與模板匹配,進而得到物體的抓取位姿。現(xiàn)有方法多是在基于二維圖像的目標檢測網(wǎng)絡(luò)基礎(chǔ)上,新增位姿估計的分支,將提取的物體幾何信息與已有模板庫中的幾何信息進行匹配,從而得到物體的6D位姿信息,供機器人實現(xiàn)抓取。如姿態(tài)卷積神經(jīng)網(wǎng)絡(luò)(Pose Convolutional Neural Network,PoseCNN)[11],單類多目標的6維姿態(tài)檢測器(Single Shot MultiBox Detector-6 Dimension,SSD-6D)[12],深層次6維位姿估計(Deep-6 Dimension Pose,Deep-6DPose)[13]等。這類算法在公開數(shù)據(jù)集上表現(xiàn)不俗,然而,算法性能受數(shù)據(jù)集質(zhì)量的影響較大,需要模板庫中可成功抓取的姿勢足夠全面。且對遮擋或截斷場景缺少相應(yīng)的學(xué)習(xí),在實際抓取時,抓取成功率將受到影響。

(3)基于最優(yōu)抓取框的抓取機制 現(xiàn)實場景中的抓取工作,需要傳遞給機器人的是抓手的抓取位置與旋轉(zhuǎn)角度,反映到2D圖像中,就是目標表面上的一個長方形抓取框。抓取某個物體,可能有多種抓取姿勢,因此需要在生成的若干個抓取框中,篩選出最符合認知的抓取姿勢。ASIF等[14]提出的GraspNet,MAHLER等[15]提出的Dex-Net,REDMON[16]等提出的基于卷積的機器人實時抓取工作就是基于上述方式,并且都在Cornell數(shù)據(jù)集上獲取了不錯的效果。但該方法局限于平面抓取,在一些特定的抓取環(huán)境中很難適用。

鑒于以上方法出現(xiàn)的算法成本高,對拍攝角度過于敏感,遮擋、截斷等極端場景魯棒性差等問題,本文設(shè)計了一種面向機器人抓取工作的單視圖兩階段物體位姿估算方法。在自制抓取數(shù)據(jù)集的基礎(chǔ)上,僅利用單張RGB圖像,使用投票機制篩選目標表面關(guān)鍵點,再利用多組點對映射關(guān)系(E-Perspective-n-Point,EPnP)算法與3D模型的關(guān)鍵點一一對應(yīng),從而得到精確的目標位姿信息,供機器人實現(xiàn)抓取。

1 系統(tǒng)框架

整個機器人抓取系統(tǒng)的工作框架如圖1所示。首先,將相機固定在機械手外,利用手眼標定確定相機坐標系與機器人基坐標系之間的轉(zhuǎn)換關(guān)系。其次,拍攝目標場景的RGB圖像,將其輸入位姿估計網(wǎng)絡(luò),輸出目標物體的6D位姿信息,即物體與相機之間的旋轉(zhuǎn)平移矩陣,同時輸出抓取點位置,并將抓取點與三維框在RGB圖像上可視化。最后,計算抓取點在機器人基坐標系下的位置,并將旋轉(zhuǎn)矩陣轉(zhuǎn)化為旋轉(zhuǎn)向量,調(diào)整為正確的機器人抓手方向,引導(dǎo)機器人完成抓取工作。

2 方法描述

2.1 數(shù)據(jù)集制作

考慮到機器人抓取的實際工作環(huán)境,本文使用開源工具Object Dataset Tools(1)https://github.com/F2Wang/ObjectDatasetTools采集自定義數(shù)據(jù)集進行訓(xùn)練與測試。該工具包可以實現(xiàn)自動生成可用于6D目標位姿估計信息的標注工作。由于需要重建數(shù)據(jù)集中物體的三維模型,本文使用Intel RealSense D435i深度相機采集信息。具體制作方法如下:

步驟1在待抓取桌面上散亂貼上定位標簽,放置待抓目標物體;

步驟2將相機環(huán)繞抓取場景一周,生成N個有序RGB圖像和對應(yīng)的深度圖像,假設(shè)第i個深度圖像生成的相應(yīng)點云為Oi,Oi中的點由二維到三維的投影公式(1)計算得到:

(1)

式中:[u,v]為圖像中的像素坐標,Zd為對應(yīng)的深度值,fx、fy分別為相機在x軸與y軸的焦距,[cx,cy]圖像的中心坐標,[OPi,OPi,OPi]為對應(yīng)點的三維坐標。

步驟3通過拼接相鄰點云,生成完整目標點云,作為抓取目標的三維模型M;

步驟4以相機拍攝的第一幀圖片為標準幀,生成每張RGB圖中目標物體相對于拍攝相機的位姿信息[R|t],其中:R是物體坐標系相對于相機坐標系的旋轉(zhuǎn)矩陣,t是物體坐標系相對于相機坐標系的平移矩陣。

步驟5利用已知的相機內(nèi)參矩陣C,抓取目標的三維模型M,以及每張RGB圖中物體對應(yīng)相機的位姿信息[R|t],生成每張RGB圖中目標物體的掩膜,具體計算如三維到二維的投影公式(2)所示:

(2)

式中:[Xw,Yw,Zw]為三維模型上點的坐標,[uc,vc]是對應(yīng)的掩膜圖上的像素坐標,Zc是目標到相機的距離。

以訂書機為例,整個數(shù)據(jù)集生成的標注信息如表1所示。

表1 數(shù)據(jù)集標注信息

數(shù)據(jù)集中圖片與示例模型如圖2所示。

2.2 網(wǎng)絡(luò)結(jié)構(gòu)

本文網(wǎng)絡(luò)總體結(jié)構(gòu)如圖3所示。首先輸入 RGB圖像,經(jīng)過卷積、下采樣、上采樣等操作后,輸出圖像分割結(jié)果與單位向量;然后進行RANSAC投票篩選2D關(guān)鍵點;最后利用 EPnP算法,得到物體的6D位姿信息。本文將在后續(xù)章節(jié)詳細介紹網(wǎng)絡(luò)關(guān)鍵細節(jié)。

2.2.1 主干網(wǎng)設(shè)計

ResNet[17]結(jié)構(gòu)簡單、性能高效,因此,本文選擇預(yù)訓(xùn)練過的 ResNet-18作為主干網(wǎng),并將其修改為全卷積結(jié)構(gòu)。網(wǎng)絡(luò)輸入 RGB 圖像,輸出分割結(jié)果與單位向量。其中,RGB圖像的分辨率為640×480×3;分割結(jié)果表示為 640×480×(7+1)的張量,其中“7”是類別總個數(shù);單位向量表示為640×480×(8×2×7)的張量,其中“8”是 2D 關(guān)鍵點個數(shù),“2”是沿x軸與y軸的兩個子方向。相較于原始的 ResNet-18,本文設(shè)定在特征圖的大小下采樣到 W/8×H/8 時,停止下采樣,放棄后續(xù)的平均池化等操作。接著將最后的全連接層替換為卷積層,然后,在進行上采樣、跳躍連接等操作后,將特征圖還原到原圖尺寸大小,最后經(jīng)過 1×1 的卷積,得到最后的對應(yīng)輸出。

2.2.2 3D關(guān)鍵點篩選

在投票篩選關(guān)鍵點之前,需要在物體的三維模型上定義好關(guān)鍵點位置,作為基準(ground-truth)信息。對于關(guān)鍵點的篩選,在之前的方法中,多選取物體的三維包圍框上的 8 個頂點作為關(guān)鍵點,如Yolo6D[18]。然而,對于一些不規(guī)則物體,三維包圍框上的頂點遠離圖像中的目標像素,距離目標像素越遠,投影定位的誤差影響越大。同時,考慮到后續(xù) EPnP算法的穩(wěn)定性,關(guān)鍵點應(yīng)盡量分散[19]。因此,本文采用最遠點采樣(farthest point sampling)方法,在物體模型表面上選取8個關(guān)鍵點,來代替三維包圍框的8個頂點。具體算法如下:

步驟1分別取目標三維模型M在x,y,z方向上的最大、最小值,平均化后得到M的中心點P0的位置,初始化3D關(guān)鍵點集合S{P0};

步驟2記模型M表面除集合S中的點外的所有點為集合U={Qi|Qi∈M,Qi?S},計算集合U中的每個點Qi到P0的歐氏距離,記d(Qi,P0),存放于數(shù)組L中,從中選取距離最大值對應(yīng)的點,記為P1,更新集合S={P0,P1};

步驟3更新集合U,計算集合U中每個點Qi到P1的歐氏距離,若點Qi到P1的距離小于L[i],則更新L=[i]=d(Qi,P1);

步驟4選取L中距離最大值對應(yīng)的點,記為P2,更新集合S={P0,P1,P2}。

按上述方法不斷迭代更新,直到集合S中出現(xiàn)P8,此時P1～P8即最后選取的8個關(guān)鍵點。

如圖4所示,深色點是在訂書機模型上找到的關(guān)鍵點。

2.2.3 2D關(guān)鍵點投票

本文網(wǎng)絡(luò)的核心是在二維(2D)圖像上的關(guān)鍵點投票機制。在網(wǎng)絡(luò)進行分割任務(wù)后,從屬于某一對象的所有像素都會計算其到該對象關(guān)鍵點的方向,生成該關(guān)鍵點的2D位置假設(shè),以及基于隨機采樣一致(RANdom SAmple Consensus,RANSAC)投票的置信度。相較于傳統(tǒng)的關(guān)鍵點匹配算法,如更快特征點與旋轉(zhuǎn)不變描述子(Oriented fast and Rotated BRIEF,ORB)[20]、尺度不變特征變換(Scale Invariant Feature Transform,SIFT)[21]等,逐像素投票生成的關(guān)鍵點能使網(wǎng)絡(luò)更加關(guān)注圖像中的局部信息,減輕圖像背景的干擾。因為是投票生成的關(guān)鍵點,所以即使在遮擋或截斷場景中,某個關(guān)鍵點不可見,仍然可以通過其他可見部分估計的方向推理其正確定位。每個像素到關(guān)鍵點的向量定義如下:

(3)

式中:Hk表示假設(shè)的第k個關(guān)鍵點,Xk,i表示指向Hk的第i個像素。

基于RANSAC投票生成關(guān)鍵點的具體算法如下:

步驟1根據(jù)圖像分割結(jié)果,確定目標對象對應(yīng)區(qū)域的像素集合Q;

步驟2在集合Q中隨機選取兩個像素X1,X2,取它們的向量的交點,記hk,1作為可能的候補假設(shè)關(guān)鍵點Hk;

步驟3重復(fù)第二步n次,生成由n個可能的候補假設(shè)關(guān)鍵點Hk組成的集合{hk,i|i=1,2,…,n}。

如此,所有像素都經(jīng)過了投票。再對集合中的每個hk,i進行打分,投票的得分wk,i定義[19]如下:

(4)

式中F為指示函數(shù)。

得分越高,說明指向該候補關(guān)鍵點的像素越多,則其越可能是真正的關(guān)鍵點。最后本文篩選出8個得分最高的候補關(guān)鍵點,作為在二維圖像上選取的最終關(guān)鍵點。

2.2.4 物體位姿信息生成

如圖5所示,在得到2D圖像上目標的關(guān)鍵點位置后,與關(guān)鍵點的三維坐標進行匹配,本文使用EPnP[22]計算得到物體的位姿信息[R|t]。具體計算如式(5)所示:

(5)

式中:上標c和上標w分別表示三維點在相機坐標系和世界坐標系下坐標;Cj為空間中選取的4個控制點;αij為引入的參數(shù),任意三維點都可以表示為控制點坐標的線性組合。

2.2.5 損失函數(shù)設(shè)計

根據(jù)網(wǎng)絡(luò)結(jié)構(gòu),本文的損失函數(shù)分為分割損失和投票損失兩部分。分割損失采取交叉熵損失,定義如下:

(6)

式中:I為類別總數(shù);yic為指示變量,若該類別與樣本i類別一致則為1,否則為0;pic為估計的類別概率。

投票損失采取smoothL1函數(shù)格式,定義如下:

(7)

整個網(wǎng)絡(luò)的損失為上述損失函數(shù)之和:

loss=loss_seg+loss_vote。

(8)

3 機器人控制

3.1 手眼標定

為了實現(xiàn)機器人的有效抓取,除了預(yù)測得到的物體相對于相機的位姿信息外,還需要機器人坐標系與相機坐標系的轉(zhuǎn)換關(guān)系,即手眼標定。按照相機擺放位置的不同,可分為眼在手上(eye-in-hand)的隨動視覺系統(tǒng)、手眼分離(eye-to-hand)的固定相機視覺系統(tǒng)和既有固定相機又有隨動相機的混合視覺系統(tǒng)[23]。本文的抓取場景設(shè)置為手眼分離的固定相機視覺系統(tǒng)。手眼標定過程如下:

首先,在機器人抓手末端處固定標定板,調(diào)整抓手位置,拍攝標定板的不同姿勢,利用張正友標定法得到相機與標定板間的轉(zhuǎn)換矩陣TCam2Obj。其次,通過UR5機械臂的示教板,讀取每次調(diào)整時抓手與機器人基坐標系間的轉(zhuǎn)換矩陣TEnd2Base。然后,根據(jù)AX=XB方程[24],求解得到標定板與抓手中心間的轉(zhuǎn)換矩陣TEnd2Base。最后,得到相機坐標系與基坐標系間的轉(zhuǎn)換矩陣,即TCam2Obj×TObj2End×TEnd2Base。

3.2 抓取姿勢定義

物體與相機的位姿信息有多種表達方式,如歐拉角,四元數(shù)等。本文網(wǎng)絡(luò)輸出的6D位姿信息表示為3×4的旋轉(zhuǎn)平移矩陣。然而,在實際抓取場景中,二指夾爪的抓取需要物體在機器人坐標系下的中心點坐標和相應(yīng)的旋轉(zhuǎn)角度,因此還需要將旋轉(zhuǎn)矩陣經(jīng)羅德里格斯(Rodrigues)變換,轉(zhuǎn)為旋轉(zhuǎn)向量。在實際應(yīng)用中,考慮到不規(guī)則物體如杯子,手表等,若選擇物體的質(zhì)心作為抓取中心,顯然不符合日常抓取工作的認識。因此,本文預(yù)先在物體三維模型上設(shè)置兩個抓取點,并選取兩點連線的中點作為抓取中心。

抓取點經(jīng)網(wǎng)絡(luò)輸出的旋轉(zhuǎn)平移矩陣變換,再經(jīng)過手眼標定矩陣變換后,轉(zhuǎn)換為機器人坐標系下物體的實際抓取點,即二指夾爪在實際抓取時的對應(yīng)位置。此時,夾爪的朝向即是所需要的旋轉(zhuǎn)向量。具體計算如旋轉(zhuǎn)矩陣轉(zhuǎn)旋轉(zhuǎn)向量的式(9)所示:

(9)

式中:Rp為估計的6D位姿中的旋轉(zhuǎn)矩陣,RCam2base為手眼標定后得到的相機相對于機器人基座的旋轉(zhuǎn)矩陣。

4 實驗結(jié)果與分析

4.1 評價指標

本文使用3種評價指標評估模型的性能:2D重投影誤差,3D對應(yīng)點平均距離誤差(Average Diameter Distance,ADD)與位姿矩陣誤差。2D重投影誤差[25]的計算,是根據(jù)預(yù)測得到的位姿信息,計算2D圖像上投影點與基準投影點位置之間的誤差,若誤差距離小于5像素,則認為預(yù)測的位姿信息是正確的。具體定義如下:

(10)

式中,C為相機內(nèi)參矩陣;R,t為物體的基準位姿信息;Rp,tp為估計的物體位姿信息;m為模型M上點的總數(shù)量;x為模型M上的三維點。

3D對應(yīng)點平均距離誤差A(yù)DD[11]則是將模型點云分別根據(jù)基準的位姿信息與預(yù)測的位姿信息進行變換,計算兩者對應(yīng)點距離的平均值。當距離平均值小于模型直徑的10%時,認為預(yù)測的位姿信息是正確的。對于數(shù)據(jù)集中出現(xiàn)的對稱物體,則采用ADD-S[11],使用快速搜索算法KNN計算最近距離。具體定義如式(11)和式(12)所示:

(11)

(12)

位姿矩陣誤差[26],則是分別提取基準位姿矩陣與估計位姿矩陣中的旋轉(zhuǎn)矩陣R與平移矩陣t,計算兩者之間的誤差。若旋轉(zhuǎn)矩陣之間誤差在5度以內(nèi),且平移矩陣之間誤差在5cm以內(nèi),則認為預(yù)測的位姿信息是正確的。具體定義分別如式(13)和式(14)所示:

cm5=‖tp-t‖,

(13)

d5=arccos[(Tr(RpR-1)-1)/2]。

(14)

4.2 訓(xùn)練細節(jié)與實驗效果

在訓(xùn)練時,本實驗設(shè)置batch_size=32,learning_rate=0.001,epoch=200,每訓(xùn)練50個epoch,學(xué)習(xí)率將減半;顯卡配置為兩塊GTX-2080Ti。

圖6是實驗中不同物體的檢測結(jié)果可視化圖,其中綠色是按基準位姿信息投影得到的三維框,藍色是按估計位姿信息投影得到的三維框。

為驗證算法的魯棒性,本文在極端場景下試驗了算法預(yù)測位姿的性能。如圖7所示為分別在遮擋、截斷、陌生環(huán)境下物體的位姿估計效果。

實際的抓取工作需要同時對多個目標實現(xiàn)位姿估計。如圖8所示為在真實抓取環(huán)境中,多個目標的位姿估計結(jié)果。圖9所示為抓取實驗的具體過程。本實驗的抓取視頻見鏈接:https://www.bilibili.com/video/BV1mV411x7ZL。

4.3 實驗對比分析

近年來,物體6D位姿信息的獲取主要通過RGB+深度圖的組合或者只通過RGB圖像得到。本文僅利用單張RGB圖像,即可實現(xiàn)精確的物體位姿估計。為比較實驗方法的性能優(yōu)劣,本文選擇Deep-6DPose網(wǎng)絡(luò)、PoseCNN+DeepIM[25]網(wǎng)絡(luò)進行比較。前者使用了RGB+深度圖的組合作為輸入,后者與本文方法一樣,只使用了RGB圖像作為輸入。根據(jù)4.1節(jié)中各評價指標標準,預(yù)測位姿的準確率對比如表2所示。由表中實驗結(jié)果可知,本文方法預(yù)測的位姿信息相比另外兩種方法,在性能上有所提高。

表2 6D位姿信息評估比較

在真實場景的實際抓取工作中,本文使用Intel RealSense D435i相機采集圖像,UR5輕量型協(xié)作式工業(yè)機器人進行抓取。該機器人的有效載荷為5kg,滿足本次抓取實驗要求。抓取結(jié)果如表3所示。

表3 真實場景抓取成功率 %

實驗結(jié)果表明,采用本文提出的位姿估計方法,計算抓取姿勢后,機器人在物體分散擺放的普通場景中平均抓取成功率達到了94%。同時,在物體被遮擋、截斷的場景中,平均抓取成功率達到了88%,滿足實際抓取需求。

5 結(jié)束語

目前,機器人抓取工作仍面臨著成本高,精確度受限于使用場景布置,泛化能力一般等局限性。本文通過投票推理物體表面關(guān)鍵點的方式,再結(jié)合EPnP算法得到物體的位姿信息,經(jīng)實驗,即使在遮擋或截斷場景中也能體現(xiàn)出不錯的性能,增強了機器人抓取工作的魯棒性,提高了抓取工作的任務(wù)質(zhì)量與效率。

受限于三維數(shù)據(jù)集制作的采集工作較為困難,網(wǎng)絡(luò)訓(xùn)練的精確度還有待提高。在今后的研究中,擬采取將合成數(shù)據(jù)集與真實數(shù)據(jù)集混合的方式,并結(jié)合數(shù)據(jù)增量,提高數(shù)據(jù)集的質(zhì)量,進而提升網(wǎng)絡(luò)的魯棒性。同時改進網(wǎng)絡(luò)架構(gòu),實現(xiàn)同類多目標物體的檢測與抓取。