基于3維點(diǎn)云深度信息和質(zhì)心距相結(jié)合的機(jī)器人抓取控制方法①

鄒 遇 熊禾根 陶 永③* 任 帆 陳超勇 江 山

(*武漢科技大學(xué)冶金裝備及其控制教育部重點(diǎn)實(shí)驗(yàn)室 武漢 430081)(**武漢科技大學(xué)機(jī)械傳動(dòng)與制造工程湖北省重點(diǎn)實(shí)驗(yàn)室 武漢 430081)(***北京航空航天大學(xué)機(jī)械工程及自動(dòng)化學(xué)院 北京 100191)(****北京航空航天大學(xué)生物醫(yī)學(xué)工程高精尖創(chuàng)新中心 北京 100191)

0 引 言

近幾十年來(lái)，機(jī)器人技術(shù)得到了快速的發(fā)展，機(jī)器人已經(jīng)在各行各業(yè)中得到了廣泛應(yīng)用[1]。機(jī)器人最主要的工作之一是進(jìn)行搬運(yùn)裝配工作，而完成這些工作的前提是需要機(jī)器人對(duì)物體進(jìn)行很好的識(shí)別和抓取。隨著計(jì)算機(jī)技術(shù)和傳感器技術(shù)的發(fā)展，機(jī)器視覺(jué)成為人工智能重要的研究領(lǐng)域，其中基于視覺(jué)的機(jī)器人抓取是當(dāng)前機(jī)器視覺(jué)領(lǐng)域的一個(gè)研究熱點(diǎn)。

目前，機(jī)器人通過(guò)視覺(jué)完成對(duì)物體的識(shí)別和跟蹤已經(jīng)有很多的研究成果，但傳統(tǒng)的視覺(jué)檢測(cè)識(shí)別技術(shù)都是基于2維圖像，2維圖像在成像的過(guò)程中會(huì)丟失物體大量的信息。隨著Kinect等廉價(jià)深度傳感器的普及，物體3維數(shù)據(jù)獲取的成本也越來(lái)越低，大量研究者開(kāi)始利用深度傳感器獲取物體的3維點(diǎn)云，基于物體3維點(diǎn)云深度信息來(lái)實(shí)現(xiàn)抓取工作[2]。Hugh等人[3]針對(duì)未知抓取對(duì)象堆疊的情況下，提出了一種滿足機(jī)器人手與物體接觸約束的最優(yōu)抓取方法，計(jì)算如何從幾乎無(wú)限多的抓取可能中選擇最合適的抓取位置。Marcus等人[4]使用卷積神經(jīng)網(wǎng)絡(luò)(convolutional neural metwork，CNN)來(lái)訓(xùn)練抓握部分的點(diǎn)云，不需要手動(dòng)標(biāo)記，但它在很大程度上依賴于點(diǎn)云的重建，這種方法不靈活且需要額外的成本。Lin 等人[5]搭建了一個(gè)RGB-D相機(jī)固定在機(jī)器人手上的系統(tǒng)，在不需要預(yù)先知道物體信息的情況下，只需要場(chǎng)景下的深度信息就可以確定抓取方向。Joe等人[6]利用卷積神經(jīng)網(wǎng)絡(luò)(CNN)從視覺(jué)和深度兩方面推斷抓取坐標(biāo)，該方法使用Kinect傳感器，并使用回歸推斷出視覺(jué)中的抓取點(diǎn)，再現(xiàn)Redmon和Angelova[7]的工作。Kensuke等人[8]提出了一種基于學(xué)習(xí)的隨機(jī)迭代視覺(jué)識(shí)別系統(tǒng)，不需要在每次挑選時(shí)都嘗試檢測(cè)堆中包含的所有物體的姿態(tài)。朱海波[9]對(duì)具有規(guī)則幾何形狀的工件提出了一種基于Hough鏈碼確定工件形狀和基于幾何不變矩來(lái)計(jì)算工件表面中心和長(zhǎng)軸朝向的形狀識(shí)別算法，從而實(shí)現(xiàn)了工件定位，但這種方法不適合復(fù)雜環(huán)境的工件識(shí)別。

物體抓取位置的確定對(duì)物體抓取工作具有決定性作用。抓取工具分為吸盤(pán)式和夾持型，吸盤(pán)式抓取工具對(duì)物體形狀和表面特性都有要求，而夾持型抓取工具運(yùn)用更廣。本文提出一種基于物體3維點(diǎn)云深度信息，利用物體邊界輪廓上抓取點(diǎn)云對(duì)所在直線與法向量的夾角、抓取點(diǎn)云對(duì)所在直線到物體質(zhì)心的距離結(jié)合的機(jī)器人抓取控制方法，運(yùn)用二指夾持器作為抓取工具對(duì)物體進(jìn)行抓取工作，該方法能夠很好地完成物體的測(cè)量、識(shí)別、定位等系統(tǒng)的物體抓取工作。

1 抓取點(diǎn)對(duì)初步篩選

1.1 點(diǎn)云前期處理

使用深度相機(jī)獲取物體表面的點(diǎn)云數(shù)據(jù)，受外界環(huán)境和相機(jī)自身等因素的影響，所得到的3維點(diǎn)云數(shù)據(jù)存在背景點(diǎn)、離群點(diǎn)等大量噪聲點(diǎn)，噪聲點(diǎn)會(huì)對(duì)目標(biāo)的定位精度及識(shí)別速度產(chǎn)生影響，所以需要對(duì)獲取的場(chǎng)景點(diǎn)云進(jìn)行處理；此外，為了提高目標(biāo)物位姿估算的精度，需要將目標(biāo)物從場(chǎng)景中準(zhǔn)確地分割出來(lái)；分割后得到的點(diǎn)云數(shù)一般有幾萬(wàn)個(gè)甚至更多，點(diǎn)云數(shù)太多會(huì)增大計(jì)算量，需要對(duì)物體進(jìn)行降采樣，從而降低點(diǎn)云數(shù)量、減小計(jì)算量；一般提取的物體點(diǎn)云的邊界輪廓信息會(huì)發(fā)生丟失或輪廓不清晰，為了得到較好的物體點(diǎn)云深度信息，將分離出來(lái)的目標(biāo)點(diǎn)云與已建立好的物體3維模型點(diǎn)云數(shù)據(jù)庫(kù)進(jìn)行匹配[10]，采用采樣一致性初始配準(zhǔn)算法(sample consensus initial aligment, SACIA)對(duì)分割出的點(diǎn)云和模型數(shù)據(jù)庫(kù)進(jìn)行初始匹配，然后再通過(guò)迭代最近鄰點(diǎn)算法(iterative cloest point, ICP)進(jìn)行精確配準(zhǔn)，得到與目標(biāo)點(diǎn)云對(duì)應(yīng)的深度信息清晰的點(diǎn)云模型[11]。可由如下步驟確定點(diǎn)云前期處理。

INPUT: 深度相機(jī)獲取的初始場(chǎng)景點(diǎn)云OUTPUT: 數(shù)據(jù)庫(kù)與目標(biāo)點(diǎn)云對(duì)應(yīng)的點(diǎn)云模板步驟1: 獲取目標(biāo)物場(chǎng)景點(diǎn)云步驟2: 基于歐式聚類(lèi)分割從場(chǎng)景中分割出目標(biāo)物點(diǎn)云步驟3: 點(diǎn)云去噪步驟4: 點(diǎn)云降采樣步驟5: 物體識(shí)別與位姿估計(jì) (1) 基于SAC-IA算法初始匹配 (2) 基于ICP算法精確配準(zhǔn)步驟6: 輸出3維點(diǎn)云數(shù)據(jù)庫(kù)與目標(biāo)點(diǎn)云相匹配的模板點(diǎn)云

1.2 目標(biāo)物邊界點(diǎn)云提取

為了降低計(jì)算量，提高計(jì)算效率，將匹配好的模板點(diǎn)云進(jìn)行邊界提取。對(duì)匹配得到與數(shù)據(jù)庫(kù)相對(duì)應(yīng)的點(diǎn)云模板，進(jìn)行主成分分析(principal component analysis，PCA)，逼近物體的質(zhì)心和主軸，建立本地對(duì)象坐標(biāo)系如圖1所示。

圖1 主平面投影

如圖2所示，提取出模板點(diǎn)云輪廓，模板點(diǎn)云首先投影到XOY主平面，以獲得目標(biāo)對(duì)象的主輪廓顯示。求出投影點(diǎn)云的輪廓有2種方法，即凹?xì)ぽ喞屯拱喞谶@種情況下，凹?xì)ぽ喞梢愿玫乇硎灸繕?biāo)的幾何形狀。

圖2 點(diǎn)云邊界

1.3 獲取抓取候選點(diǎn)對(duì)

機(jī)器人的機(jī)械臂采用二指夾持器。在抓取目標(biāo)物的過(guò)程中，要成功抓取目標(biāo)物，首先必須保證夾持器所選擇的抓取點(diǎn)對(duì)之間的距離比夾持器的最大張開(kāi)寬度還要小[12]，夾持器和物體的關(guān)系如圖3所示。

圖3 夾持器張度與目標(biāo)物關(guān)系

圖中，u表示夾持器的最大張度，w表示所選擇抓取點(diǎn)對(duì)之間的距離。Pi、Pj為目標(biāo)物點(diǎn)云邊界上的任意2點(diǎn)，其中Pi的坐標(biāo)為(xi,yi,zi)、Pj的坐標(biāo)為(xj,yj,zj)，則：

(1)

那么Pi、Pj2點(diǎn)云之間的歐式距離表示為

(2)

為了使夾持器能夠準(zhǔn)確地夾住物體，要給夾持器留有一定空間余量，空間余量為夾持器最大張開(kāi)寬度的10%(即0.1u)，經(jīng)過(guò)初步篩選把間距大于0.9u的點(diǎn)云對(duì)剔除掉，初步篩選方案如下。

輸入: 夾持器寬度u;目標(biāo)物邊界點(diǎn)云集P(共有n個(gè)點(diǎn)云)1: for (i=1, i

2 機(jī)器人抓取單一控制方法

在抓取對(duì)象邊界點(diǎn)云中已經(jīng)初步篩選出可抓取候選點(diǎn)云對(duì)，但可抓取點(diǎn)對(duì)數(shù)還是過(guò)多，下一步將對(duì)可抓取候選點(diǎn)云對(duì)進(jìn)行評(píng)價(jià)，可從2個(gè)方面入手：一方面可以考慮機(jī)器人夾持器抓取物體時(shí)物體的受力情況，決定能否抓取物體，即考慮抓取點(diǎn)對(duì)的法向量和抓取點(diǎn)對(duì)兩點(diǎn)云所在直線的位置關(guān)系作為一個(gè)評(píng)價(jià)方法；另一方面考慮抓取點(diǎn)對(duì)和目標(biāo)物質(zhì)心的關(guān)系，決定能否抓穩(wěn)，作為第二個(gè)評(píng)價(jià)方法。

2.1 法向量夾角抓取控制方法

2.1.1 點(diǎn)云法向量

法向量是點(diǎn)云數(shù)據(jù)的一個(gè)基本屬性，在許多點(diǎn)云處理中是必不可少的信息。本文的抓取方法基于目標(biāo)物邊界點(diǎn)云法向量的幾何關(guān)系進(jìn)行求取抓取候選點(diǎn)對(duì)，因此邊界輪廓點(diǎn)云法向量的求取和定向至關(guān)重要。切平面、切線、法線都是針對(duì)連續(xù)的曲面，而點(diǎn)云表面特征是一個(gè)個(gè)離散點(diǎn)，點(diǎn)云數(shù)據(jù)所記錄的信息是每個(gè)獨(dú)立的3維坐標(biāo)，所以為了求得每個(gè)點(diǎn)對(duì)應(yīng)的法向量，取一定半徑內(nèi)的點(diǎn)進(jìn)行曲面擬合，在擬合的基礎(chǔ)上求對(duì)應(yīng)的法向量。最小二乘法平面擬合估計(jì)是求取法向量較好的方法，解決表面法線估計(jì)的問(wèn)題可以最終化簡(jiǎn)為對(duì)一個(gè)協(xié)方差矩陣的特征向量和特征值的分析(或稱PCA，主成分分析)[13]，這個(gè)協(xié)方差矩陣從查詢點(diǎn)的近鄰元素中產(chǎn)生，對(duì)于每一個(gè)點(diǎn)Pi，對(duì)應(yīng)點(diǎn)的協(xié)方差矩陣C如下：

(3)

2.1.2 法向量夾角評(píng)價(jià)方法

機(jī)器人夾持器夾取物體時(shí)，物體所受的力是否平衡、是否產(chǎn)生扭矩都會(huì)影響到抓取是否成功。物體抓取點(diǎn)的受力方向和此點(diǎn)的點(diǎn)云法向量方向在同

圖4 目標(biāo)物邊界點(diǎn)云法向量

一直線上，所以候選抓取點(diǎn)處的受力方向可轉(zhuǎn)化為抓取點(diǎn)對(duì)應(yīng)的點(diǎn)云法向量方向[14，15]。為了同時(shí)兼顧抓取點(diǎn)力和力矩2個(gè)因素對(duì)抓取的影響，以抓取點(diǎn)處的點(diǎn)云法向量和抓取點(diǎn)對(duì)兩點(diǎn)所在直線的夾角大小為評(píng)判依據(jù)，如圖5所示。

圖5 邊界點(diǎn)云法向量夾角

(4)

(5)

(6)

(7)

(8)

2.2 質(zhì)心距抓取控制方法

抓取點(diǎn)對(duì)所在直線與目標(biāo)物質(zhì)心的距離也是影響抓取質(zhì)量的一個(gè)重要因素，在滿足其他抓取要求時(shí)，抓取點(diǎn)對(duì)連線距離目標(biāo)物質(zhì)心越近抓取質(zhì)量越好，如圖6所示。

圖6 質(zhì)心距

(9)

則質(zhì)心O(x,y,z)到Pi、Pj所在直線的質(zhì)心距為

(10)

其中，

(11)

質(zhì)心距越小越好，采用百分制打分，以候選抓取點(diǎn)對(duì)中質(zhì)心距最大的為0分，質(zhì)心距為0時(shí)為100分，設(shè)最大質(zhì)心距為D，通過(guò)搜索算法找出最大的質(zhì)心距。算法如下：

輸入: 質(zhì)心距dij(共有n個(gè)點(diǎn)云)1: D=d12;2: for (i=2, iD)5: D=dij; } }6: print D;輸出: 最大質(zhì)心距D

(12)

即：

(13)

3 基于法向量夾角和質(zhì)心距的機(jī)器人抓取綜合控制方法

第2節(jié)中提出的2個(gè)抓取點(diǎn)對(duì)質(zhì)量評(píng)價(jià)方法是：(1)法向量夾角評(píng)價(jià)方法；(2) 質(zhì)心距評(píng)價(jià)方法。當(dāng)抓取點(diǎn)對(duì)確定后，這2種評(píng)價(jià)方法候選點(diǎn)云對(duì)的法向量夾角和質(zhì)心距也就隨之確定，法向量夾角和質(zhì)心距是同時(shí)成對(duì)出現(xiàn)的，法向量夾角越小抓取成功率越大，質(zhì)心距越小抓取后越穩(wěn)定。但法向量夾角和質(zhì)心距并不是都同時(shí)處于最優(yōu)(最小)狀態(tài)，大多數(shù)情況下法向量夾角和質(zhì)心距一個(gè)達(dá)到最優(yōu)時(shí)另一個(gè)處于欠優(yōu)狀態(tài)(甚至更差)，或者2個(gè)評(píng)價(jià)方法都不能達(dá)到最優(yōu)狀態(tài)，此時(shí)就需要一個(gè)同時(shí)兼顧2種評(píng)價(jià)方法的綜合評(píng)價(jià)方法來(lái)選取較好的抓取位置對(duì)物體實(shí)施抓取，如圖7所示。

圖7 綜合評(píng)價(jià)方法

(14)

即：

×0.25

(15)

輸入:點(diǎn)云對(duì)(Pi, Pj)對(duì)應(yīng)的法向量夾角Aij,質(zhì)心距 Bij,設(shè)C得分最高1:C=C12=0.75A12+0.25B12;2: for (i=2, iC)6: { C=Cij;7: a=i, b=j;} } }8: print(Pa, Pb);輸出: 最優(yōu)抓取點(diǎn)云對(duì)(Pa, Pb)

綜合前文所述完整的物體識(shí)別抓取過(guò)程包括目標(biāo)物點(diǎn)云拾取、抓取候選點(diǎn)對(duì)初步篩選、最優(yōu)抓取點(diǎn)對(duì)選取、機(jī)器人執(zhí)行抓取等過(guò)程。本文所述的一個(gè)完整的物體識(shí)別抓取流程如圖8所示。

圖8 物體識(shí)別抓取流程圖

4 實(shí)驗(yàn)仿真

為了驗(yàn)證本文提出的基于3維點(diǎn)云深度信息和質(zhì)心距相結(jié)合的機(jī)器人抓取控制方法的可行性，在Linux系統(tǒng)ROS(robot operating system)環(huán)境下綜合Gazebo仿真平臺(tái)以及Moveit!運(yùn)動(dòng)規(guī)劃插件進(jìn)行仿真實(shí)驗(yàn)[16，17]。圖9說(shuō)明了抓取系統(tǒng)的體系結(jié)構(gòu)，它包括3個(gè)部分：3D傳感器(Microsoft Kinect雙目相機(jī))、機(jī)械臂和工作臺(tái)。Kinect雙目相機(jī)采取固定方式(eye to hand)配置捕獲物體3D點(diǎn)云數(shù)據(jù)，引導(dǎo)機(jī)器人抓取物體，工作臺(tái)模擬裝配臺(tái)，如圖9所示。

圖9 抓取系統(tǒng)的體系結(jié)構(gòu)

4.1 抓取評(píng)價(jià)方法對(duì)比實(shí)驗(yàn)與分析

本文在第2節(jié)機(jī)器人抓取單一控制方法中研究了法向量夾角和質(zhì)心距2種評(píng)價(jià)方法，就其中一種單獨(dú)作為評(píng)價(jià)方法對(duì)物體進(jìn)行抓取時(shí)，單獨(dú)取法向量夾角或質(zhì)心距的最優(yōu)值時(shí)無(wú)法保證可以對(duì)物體實(shí)施最佳抓取，于是在第3節(jié)中提出了點(diǎn)云對(duì)法向量夾角評(píng)價(jià)方法和質(zhì)心距評(píng)價(jià)方法相結(jié)合的物體抓取綜合評(píng)價(jià)方法，仿真實(shí)驗(yàn)證明其可以有效地對(duì)物體實(shí)施抓取。圖10所示為抓取對(duì)象邊界點(diǎn)云和法向量。經(jīng)過(guò)降采樣和邊界提取，最后邊界剩余205個(gè)點(diǎn)云反映物體邊界信息，各點(diǎn)云位置3維坐標(biāo)和各點(diǎn)云對(duì)應(yīng)的法向量分量數(shù)據(jù)如表1所示。

質(zhì)心坐標(biāo)O(-0.31649, 0.0684358, 9.51443)

將實(shí)驗(yàn)數(shù)據(jù)分為3組：(1) 法向量夾角單獨(dú)作為評(píng)價(jià)方法；(2) 質(zhì)心距單獨(dú)作為評(píng)價(jià)方法；(3) 綜合評(píng)價(jià)方法。

分別取3種評(píng)價(jià)方法得分最高的前5組點(diǎn)云對(duì)，通過(guò)對(duì)法向量夾角單獨(dú)最優(yōu)、質(zhì)心距單獨(dú)最優(yōu)、綜合評(píng)價(jià)方法最優(yōu)時(shí)3種評(píng)價(jià)方法得分?jǐn)?shù)據(jù)對(duì)比分析，相關(guān)數(shù)據(jù)如表2所示。

表1 點(diǎn)云位置和各點(diǎn)云對(duì)應(yīng)法向量分量數(shù)據(jù)

表2 抓取評(píng)價(jià)表

圖11是法向量夾角最小、得分最高時(shí)前5組點(diǎn)云對(duì)的3種評(píng)價(jià)方法得分對(duì)比，這5組點(diǎn)云對(duì)法向量夾角評(píng)價(jià)方法都接近100分滿分，但此時(shí)與之相對(duì)應(yīng)的質(zhì)心距并不理想，只有1組點(diǎn)云對(duì)質(zhì)心距得分超過(guò)60分，另外4組點(diǎn)云對(duì)質(zhì)心距得分太低，這4組點(diǎn)云對(duì)中質(zhì)心距最小也有4.9 cm，質(zhì)心距過(guò)大意味著二指夾持器的兩指距離質(zhì)心太遠(yuǎn)不利于抓取的穩(wěn)定，在夾取物體之后可能會(huì)發(fā)生掉落。

圖11 前5組法向量夾角最優(yōu)點(diǎn)云對(duì)

圖12 前5組質(zhì)心距最優(yōu)點(diǎn)云對(duì)

如圖13所示，法向量夾角和質(zhì)心距相結(jié)合的綜合評(píng)價(jià)方法得分最高的前5組綜合評(píng)價(jià)方法都超過(guò)90分，相對(duì)應(yīng)的點(diǎn)云對(duì)法向量夾角評(píng)價(jià)方法得分都有98分，最大法向量夾角只有2.73 °(0.0478×57.29 °)，可以保證機(jī)器人很好地抓取物體，與此同時(shí)相對(duì)應(yīng)質(zhì)心距都有75分，質(zhì)心距最大只有2.8 cm，距離較小，保證了成功抓取之后的穩(wěn)定，如圖14所示，選取這5組點(diǎn)云對(duì)中得分最高的一組(94，97)為物體的抓取位置對(duì)物體實(shí)施抓取，機(jī)器人對(duì)抓取對(duì)象成功抓取和移動(dòng)。

圖13 前5組綜合評(píng)價(jià)方法最優(yōu)點(diǎn)云對(duì)

圖14 綜合評(píng)價(jià)機(jī)制實(shí)驗(yàn)驗(yàn)證

4.2 不同物體抓取實(shí)驗(yàn)

本文最后通過(guò)在Linux系統(tǒng)的ROS平臺(tái)下對(duì)不同形狀的生產(chǎn)工件進(jìn)行抓取仿真實(shí)驗(yàn)。圖15展示了3種不同形狀工件的抓取實(shí)驗(yàn)。

圖15 工件抓取實(shí)驗(yàn)

仿真實(shí)驗(yàn)中機(jī)器人對(duì)不同形狀的工件可以成功進(jìn)行抓取并且抓取位置較好，無(wú)論對(duì)于4.1節(jié)中形狀簡(jiǎn)單的回轉(zhuǎn)體，還是對(duì)于形狀復(fù)雜的生產(chǎn)工件該綜合評(píng)價(jià)方法都有很好的抓取效果。實(shí)驗(yàn)表明，基于點(diǎn)云對(duì)法向量夾角和質(zhì)心距相結(jié)合的綜合評(píng)價(jià)方法可以有效地選取二指夾持器的抓取位置，從而成功地對(duì)抓取對(duì)象實(shí)施抓取工作。

5 結(jié) 論

基于視覺(jué)的物體抓取工作涉及到機(jī)器視覺(jué)、機(jī)械臂軌跡規(guī)劃等技術(shù)，依賴視覺(jué)的機(jī)器人抓取工作在工業(yè)生產(chǎn)中具有廣泛的應(yīng)用。本文基于物體3維點(diǎn)云深度信息，提出了一種綜合物體邊界點(diǎn)云對(duì)法向量夾角和質(zhì)心距兩者結(jié)合的機(jī)器人控制方法，避免單一抓取評(píng)價(jià)方法對(duì)抓取因素的判斷不足，能夠有效地對(duì)抓取對(duì)象進(jìn)行識(shí)別和選取抓取位置，保證了對(duì)物體成功抓取，通過(guò)仿真實(shí)驗(yàn)驗(yàn)證了該方法的有效性。