吳繼春，方海國，陽廣興,范大鵬

(1.湘潭大學 機械工程學院，湖南 湘潭 411150；2.國防科技大學 智能科學學院，湖南 長沙 410073)

0 引言

機器人抓取系統(tǒng)的核心就是確定物體的位姿，即確定物體在空間下的6自由度，該自由度由物體XYZ軸的平移及其空間旋轉(zhuǎn)組成。在非結(jié)構(gòu)環(huán)境下，由于物體的位姿易受到物體的種類、光照變化、遮擋等條件的影響，確定物體的位姿十分困難。傳統(tǒng)的抓取方法一般是采用拖動示教的方式，將機器人手臂拖動到某一個指定位置，機器人完成軌跡存儲后再進行抓取，由于缺乏視覺信息，位置稍有移動便無法完成抓取。計算機視覺主要通過手工建立目標模型與圖片的之間的特征關(guān)系[1]，有較廣的應用，不足的是該方法對于背景、光照等變化不夠魯棒。隨著深度學習的發(fā)展，目標檢測在機器人的抓取應用中已有研究[2-4]，傳統(tǒng)的2D目標檢測通常采用卷積神經(jīng)網(wǎng)絡對物體進行識別與定位，通過固定相機以獲取z軸信息，然而該方法有其局限性，如檢測只包含物體的位置、類別和置信度等信息，并不包含物體的旋轉(zhuǎn)角，同時“眼在手外”的安裝方式使得相機不能移動，導致機器人的應用場景受限。

隨著RGBD(RGB depth map)深度相機的興起，在機械臂抓取任務上,用傳感器獲取的深度信息與RGB結(jié)合的方法也開始興起。LENZ等[6]以RGBD作為輸入，將3D問題的抓取點轉(zhuǎn)換成了2D抓取點的問題; CHU等[7]在REDMON等[8]提出以RGBD圖像分析抓取點的基礎上運用了改進的Resnet提升了抓取的準確率，然而兩級串聯(lián)的網(wǎng)絡加之滑動窗口的方法實時性不高。在亞馬遜揀貨大賽中，ZENG 等[9]以多個不同視角的深度相機估計位姿信息，Sun等[10]在mask CNN(mask convolutional neural networks)分割出的深度圖像上將粗估計與微調(diào)結(jié)合進行位姿估計 。然而，由于深度相機在室外或者反光物體上存在失效的情況，無法長期依賴深度信息，同時與單張RGB圖像估計位姿相比，從深度信息中分割出目標物體的點云信息后，還需要與模型對齊，該過程涉及大量的點云計算，運行時間會更長，因此該方法需要較高的硬件支撐。綜上所述，從單張RGB圖像估計位姿有其必要，同時也更具挑戰(zhàn)。

在RGB進行位姿的估計中，PoseCNN[11]、SSD6D[12]、BB8[13]采用神經(jīng)網(wǎng)絡對二維的圖片進行定位，并求出了目標的位姿信息。該算法先得到粗略的估計結(jié)果，雖然對于采用微調(diào)步驟對結(jié)果進行修正，但兩級串聯(lián)微調(diào)的方式會導致用時更長。在前人的基礎上，TEKIN等[14]進行了算法優(yōu)化，采用端到端的方式去掉了微調(diào)部分，取得了較好的結(jié)果。

本文主要從單張RGB圖片對飲料等物體進行檢測并估計6D位姿，避免了傳統(tǒng)方法手工設計特征存在的魯棒性差，以及基于深度學習的二維識別抓取的一些缺陷。本文研究包括：①提出了最小尺寸點模型，不再依賴于物體完整的3D CAD模型，更加便捷；②提出了一種簡單實用的三維算法的數(shù)據(jù)集制作方法，該方法無需復雜的設備，更加實用；③改進了基于3D目標檢測的6D位姿估計算法，且結(jié)合六自由度機臂實現(xiàn)了對于不同待檢測物體、不同角度以及不同擺放方向的6D姿態(tài)估計并抓取，彌補了深度學習2D檢測抓取的不足，實驗結(jié)果驗證了該方法的實用性。

1 3D物體的描述

3D物體可以通過點云、體素和多邊形網(wǎng)格進行描述。近年來也出現(xiàn)了基于特征點描述3D物體以計算6D位姿的方法，該類方法是建立在目標物體的3D CAD模型已知的基礎上，然而物體的3D CAD模型的重建也是一個繁瑣的問題。工業(yè)現(xiàn)場的模型是通過昂貴的專業(yè)儀器進行重建，如三維光掃描儀。隨著經(jīng)濟型的RGBD傳感器的普及，對于目標物體的三維模型，一些新的模型建立方法也被提出。HODAN等[15]首先繪制物體3D的CAD模型，然后對該物體進行三維重建，通過迭代最近算法(Iterative Closest Point,ICP)將兩種模型對齊后再手動微調(diào)，該方法使用設備較多，也較復雜。一些開源的3D重建軟件，雖然可以重建帶紋理的三維模型，但是因為重建模型的大小和實際物體存在差異，仍然需要對模型的比例進行調(diào)整，對于曲面較多的物體，手繪3D的CAD模型也較為復雜。

3D CAD模型按照用途可以分為RGBD型和RGB型，RGBD型主要利用局部點云信息或者完整點云信息與物體3D CAD模型進行剛體變換求解位姿。RGB型則是通過3D模型的空間點與像素點之間的映射關(guān)系完成位姿估計，換言之，3D CAD模型的點云數(shù)據(jù)并沒有全部使用。基于這種思想，本文提出了一種使用最小尺寸點來表示復雜的3D CAD模型的方法。

本文采用物體的最小尺寸信息長Lmin、寬Wmin、高Hmin，并以物體最小長、寬、高尺寸的一半作為中點(Lmin/2,Wmin/2,Hmin/2)建立坐標系，長寬高的物體1/2的信息作為包圍框x、y、z的邊，且各邊長與中點坐標系平行，以物體的最小尺寸的長寬高包圍框的8頂點加1個中點作為空間點集Pi對3D物體進行描述，將點集Pi按順序存貯得到物體的最小尺寸點的3D模型(Model of the Smallest Size Point, MSSP)，如圖1b所示。

2 3D數(shù)據(jù)集制作

有效的數(shù)據(jù)集對訓練神經(jīng)網(wǎng)絡至關(guān)重要，3D目標檢測數(shù)據(jù)集通常包括3部分：目標物體的3D CAD模型、目標物體彩色圖片以及目標物體在彩色圖片中的位姿標注文件。二維目標檢測可以通過手工標注制作標簽，但是對于3D目標檢測來說，純手工標注數(shù)據(jù)集是很困難的事情，因此在6D位姿估計的文獻中，均采用LineMod、YCB-video dataset等公開數(shù)據(jù)集以及模型對算法進行驗證，但對于如何制作自己的數(shù)據(jù)集與模型等卻很少提及。然而對于6D位姿估計的實際應用，制作自動打標的數(shù)據(jù)集尤為重要，為此本文提出了基于用最小尺寸點模型的制作方法。

2.1 基于最小尺寸點模型的數(shù)據(jù)集制作方法

Aruco碼是一個由外部邊框和內(nèi)部漢明碼組成的黑白格子圖(如圖2)，其外部邊框及4個角主要用于標簽定位，內(nèi)部漢明碼主要用于ID識別、信息效驗和誤差校正。

本文主要通過Aruco碼作為視覺檢測的協(xié)助來完成物體位置和位姿確定。首先基于Aruco碼建立一個任務坐標系框架。以Aruco碼的中心點作為世界坐標系，目標物體的基點作為物體坐標系，整個坐標系如圖3所示，最終所有數(shù)據(jù)都將統(tǒng)一到相機坐標系下。由相機成像原理可知，坐標系之間的變換可以通過旋轉(zhuǎn)矩陣與平移矩陣表示。本文將目標物體坐標系的軸線與世界坐標系的軸線依次平行，同時對目標物體的旋轉(zhuǎn)矩陣進行約束，最終可通過世界坐標系與目標坐標系之間平移矩陣求出目標物體的位姿信息。

因單個Aruco碼在數(shù)據(jù)采集時易被遮擋，魯棒性不高，本文設計了12個Aruco碼組成的碼盤，以左上角的碼為第一檢測碼，出現(xiàn)遮擋則順時針對其他進行檢測，下方配有旋轉(zhuǎn)底盤，整體數(shù)據(jù)采集裝置如圖4所示。

自動打標數(shù)據(jù)集的建立方法流程如圖5所示。

當物體至于碼盤平面上時，相機采集物體及碼盤的圖像，圖像中至少有一個Aruco碼不被遮擋。Aruco檢測算法對圖像中指定的Aruco碼進行檢測，用已被識別的二維碼的中心為空間坐標系中的世界坐標，并計算出該點相對于相機坐標系的(Rc,Tc)，Rc和Tc分別表示旋轉(zhuǎn)與平移矩陣。根據(jù)已知的目標物體的尺寸信息、Aruco碼的尺寸信息物體以及物體相對被識別Aruco碼之間的距離，可以計算出目標物體最小尺寸的8個頂點的坐標點集Pi={P1,P2,…，P8}。通過相機的內(nèi)外參數(shù)(r,t)以及8個頂點的世界坐標便可計算出點集的像素點集pi={p1,p2,……..p8}，計算出點集Pi的像素點集pi之后，由于三維模型的點集應與數(shù)據(jù)集的像素點集順序一致，需要對采集到的數(shù)據(jù)點集通過重投影到物體上進行判斷，如圖6所示，如果不一致，則調(diào)整成一致。

同時，將獲取到的二維圖片的8個頂點連接成6個面，且面內(nèi)像素值設置不變，將面外像素值重置為0，通過觀察只包含目標物體面的完整性判斷8個頂點的像素是否精確，剔除不精確的圖片。

經(jīng)過幾輪剔除后，面內(nèi)的像素值設置為255，面外的像素值設置為0，對面內(nèi)的不規(guī)則圖像也可進一步采用Grabcut算法，得到待檢測目標物體對應圖片的掩碼。

3 3D目標檢測算法

由于定義了最小尺寸點集描述3D物體，對于目標物體需要檢測出其最小尺寸的點集Pi，本文CNN選擇TEKIN等[14]提出的基于YOLOv2的模型進行改進。該網(wǎng)絡是一個全卷積神經(jīng)網(wǎng)絡框，共有23層卷積層與5個最大池化層，當輸入為一張416×416×3 的RGB圖像時，經(jīng)過該網(wǎng)絡后會將圖片下采樣32次，輸出維度為13×13×20，即13×13個網(wǎng)格以及20維向量。

在13×13分割成的cell(單元格)中，每個網(wǎng)格包含5個anchor(錨),且每一個網(wǎng)格包含目標物體的二維信息，以及目標物體的三維信息等多個維度的信息；二維信息主要是目標物體的類別預測概率信息和置信度(confidence)，三維信息中主要是目標物體的三維包圍框的控制點在二維圖片中定位的映射。

置信度(confidence)表示每個網(wǎng)格含有物體的概率以及點集的準確度，confidence =P(object)×IOU，模型輸出的張量為20，其中包含9個控制點(x,y)、類別預測概率和置信度。因此，預測單個物體時，模型輸出的維度為13×13×(19+1)。

如圖7所示，進行S×S的分格后，物體種類的識別部分就由單個網(wǎng)格訓練完成，在訓練階段，當目標物體的中心沒有落到網(wǎng)格中時，物體的類別概率Pr(object)=0；當物體的中心落在網(wǎng)格中時Pr(object)=1，同時也給網(wǎng)格打上類別標簽，YOLO通過監(jiān)督學習訓練單個網(wǎng)格對物體分類，為了避免物體落在多個網(wǎng)格區(qū)域，而單個網(wǎng)格因無法從全局提取特征而導致預測不準確等問題，將置信度最大的網(wǎng)格相鄰的所有單個網(wǎng)格的預測類別結(jié)果進行綜合，就能從整體對物體類別進行完整的預測。

由于本文開始就定義了用3D物體最小尺寸信息的點集Pi描述3D物體，在訓練網(wǎng)絡進行定位時，其實是訓練目標物體的最小尺寸的9個點在2D空間的投影點pi，但對于9個坐標點，并不是直接回歸9個坐標點，而是通過預測點相對于網(wǎng)格左上角的偏移，如圖8所示，網(wǎng)格左上角為圓心，Cx，Cy為偏移量。

對于9個坐標點，其中基點一定落在網(wǎng)格內(nèi)，則采用sigmod函數(shù)將網(wǎng)絡的輸出壓縮在0～1之間，其余8個點可能會在網(wǎng)格外，因此沒有進行限制，只采用歸一化函數(shù)，如下：

(1)

不同于2D的YOLO模型，置信度用grounding truth(真實值)與預測的包圍框求重疊面積(IOU)，如果采用2D空間的方法求IOU,計算量會很大，因為在3D空間中相當于是對兩個立方體進行求解,這會極大地降低訓練速度，因此采用一種新的IOU計算方法，即定義一個置信度函數(shù)：

(2)

式中：DT(x)為預測的2D點坐標值與真實值之間的歐式距離；dth為提前設定的閾值；a為超參數(shù)。從式(1)可以得出，當預測值與真實值越接近時候，DT(x)越小，C(x)值越大，表示置信度越大;反之，表示置信度越小。損失函數(shù)如下：

L=λptLpt+λconfLconf+λidLid

(3)

式中：λpt表示坐標系數(shù)，設為1；λconf表示置信度系數(shù)，設為5.0；λid表示分類損失函數(shù)系數(shù)，設為1。對于坐標和置信度誤差項使用均分誤差，對于分類誤差使用交叉熵。

3.1 最小尺寸點模型的位姿估計

3D物體可以用物體最小尺寸信息的點集Pi進行表達，本文改進的最小尺寸點模型方法，對于目標物體位姿的預測可以抽象成對描述物體點集Pi位姿的預測。

網(wǎng)絡輸出13×13單元格矩陣，每一個單元格都會預測最小尺寸信息的點集Pi在二維空間上的投影點集pi，每個類別物體的置信度值由類別概率與置信度函數(shù)的值相乘得到，通過預設的閾值對置信度較低的預測值進行過濾，保留置信度高的預測值。如果物體較大，其投影點會跨越多個單元格，此時以置信度最高的單元格為基準，用置信度值做權(quán)重計算其3×3鄰域中的單元格中單個點，加權(quán)平均后再進行組合。位姿估計如圖9所示。

PnP(perspective-n-poin)[17]是一種已知n個3D空間點及圖像上n個2D投影點時，求解相機位姿的方法。當網(wǎng)絡對目標物體的類別進行識別時，同時預測在3D空間下目標物體長寬高包圍框的頂點在2D平面的投影點pi=[ui,vi]。由于已知目標物體最小尺寸模型點數(shù)據(jù)pi=[xi,yi,zi],其空間關(guān)系如下：

(4)

式中：si表示投影關(guān)系；K為相機內(nèi)參矩陣；T為相機的位姿(RC,TC)的李代數(shù)表示；式(4)的矩陣形式為siui=KTPi。由于存在誤差，求解最優(yōu)相機位姿問題變?yōu)樽钚《朔▎栴}：

(5)

通過求解PnP問題即可求出目標物體相對相機坐標系的3D旋轉(zhuǎn)RC以及3D平移TC。

4 機械臂抓取實驗系統(tǒng)的相關(guān)驗證

目標物體的權(quán)重需要通過離線訓練獲得。通過本文提出的3D目標檢測數(shù)據(jù)集方法構(gòu)建目標物體數(shù)據(jù)集，在訓練階段通過隨機更換背景圖片提高模型的泛化能力，在Ubuntu系統(tǒng)下，使用1 050ti顯卡，圖片目標檢測與位姿估計速度是0.059 s/張，工控機內(nèi)由于沒有GPU檢測，估計識別速度為1.026 s/張，實驗的識別效果如圖10所示，其準確率如表1所示，其中可樂屬于全對稱物。

表1 2D投影誤差

本設計方案采用基于RGB圖像的目標位姿估計，該算法從RGB圖像信息中推理出目標物體相對相機的位姿。由于是應用于移動機械臂的視覺系統(tǒng)，為了使應用場景更加靈活，本方案采用相機安裝在機械臂上的手眼方式，相比于“眼在手外”的固定安裝方式難度更大。

實物圖如圖11所示。圖中：①為華數(shù)機器人Co602a。②為相機，選用的是D435i，該相機主要有兩方面的作用：一是通過該相機采集物體的RGB圖像，用上述章節(jié)的算法對目標物體進行識別定位并估計位姿；二是該感器的深度信息只用于建立障礙物信息，便于軌跡規(guī)劃算法避過障礙物。③末端夾持器為電動二指夾爪，型號為EG2-4B1。④為協(xié)作機器人IR-C100，協(xié)作機器人的導航模塊與機械臂的感知、物體識別、位姿計算、運動規(guī)劃等模塊都在一個工控機上完成，工控機的配置為Intel雙核2.4 Hz CPU，4 GB運行內(nèi)存，1T硬盤，運行系統(tǒng)Linux 版本號為 Ubuntu16.04，工控機放置在移動底盤內(nèi)部。

實驗前，采用張定友標記法對相機的標定獲得相機內(nèi)參矩陣，同時為了建立相機坐標系與機器人坐標系之間的轉(zhuǎn)換關(guān)系，對機器臂進行手眼標定可得手眼關(guān)系矩陣Tm。目標物體的投影點p=(u,v)在相機坐標系下的坐標(xc,yc,zc)坐標轉(zhuǎn)換為機械臂坐標系下的坐標(x,y,z)，坐標關(guān)系矩陣為：

(6)

完成數(shù)據(jù)轉(zhuǎn)化，通過深度信息建立障礙物信息后，就可以建立抓取目標物體的避障軌跡。對于目標物體到機械臂之間的軌跡，使用RRT connect[18]軌跡規(guī)劃算法進行規(guī)劃。對于多個物體時，如果是不同種類物體，通過YOLOv2在多物體中對目標物體進行識別，如果是同種類多物體，識別出目標物體后，會從視野范圍內(nèi)置信度最高的物體抓取，該過程為無序抓取。抓取實驗過程如圖12所示，實驗過程數(shù)據(jù)如表2所示。

表2 實驗結(jié)果表

表2中平均識別時間為3.51 s。該時間包含從圖片檢測目標并預測位姿的時間，以及機械臂運動到拍照點的時間，拍照點即機械臂根據(jù)物體的擺放位姿運動到一個合適的位姿，以便讓機械臂末端相機視野可以看見物體的運動過程。如表3所示為抓取時間的對比。

表3 抓取方法對比表

本文在無GPU加速的情況下，不計算導航及回程，僅計算抓取物體所需要的識別、抓取、放置等平均時間，為23.71 s；而SUN等[10]采用完整點云匹配模型求位姿估計方法采用NVIDIA 1060Ti用時56.67s。相比之下，本文用時更少，對硬件要求更低。

5 結(jié)束語

本文針對機器人抓取提出了基于尺寸點模型的目標檢測與位姿估計方法，無需深度信息即能夠?qū)崿F(xiàn)對不同種類、不同尺寸的物體分類并抓取。通過仿真實驗證實了本方法對物體較好的分類和位姿估計，同時在Co602a機械臂上進行了抓取實驗，實驗結(jié)果證明了該方法對6D位姿估計的有效性及高效性。在未來的研究中，將通過改進網(wǎng)絡結(jié)構(gòu)進一步提高識別的精度，優(yōu)化數(shù)據(jù)集制作方法，提高識別的實時速度。