一種用于機(jī)器人抓取的單目實(shí)時(shí)三維重建系統(tǒng)

吉白冰，曹其新

（上海交通大學(xué)機(jī)械與動(dòng)力工程學(xué)院，上海200240）

1 引言

三維重建系統(tǒng)在機(jī)器人、生物醫(yī)學(xué)工程、建筑測(cè)繪等領(lǐng)域均有廣泛的應(yīng)用。在機(jī)器人領(lǐng)域當(dāng)中，三維重建主要有兩類應(yīng)用，一類是移動(dòng)機(jī)器人的定位導(dǎo)航地圖構(gòu)建，另一類是機(jī)器人的抓取任務(wù)中，待抓取物體的三維模型構(gòu)建。在機(jī)器人抓取任務(wù)中，無論是傳統(tǒng)的PPF方法［2］還是基于深度學(xué)習(xí)的物體姿態(tài)估計(jì)方法［3］都將待抓取物體的三維模型作為已知信息。目前基于結(jié)構(gòu)光原理［4-5］的三維掃描儀雖然可以重建較高精度的物體模型，但此類三維掃描儀價(jià)格昂貴且操作繁瑣，在家庭環(huán)境中難以被普通用戶接受和使用。視覺傳感器成本低且通用性強(qiáng)的傳感器。目前已有應(yīng)用單目傳感器解決工業(yè)機(jī)器人識(shí)別問題的研究［9］，基于視覺的三維重建系統(tǒng)，對(duì)家用服務(wù)機(jī)器人抓取任務(wù)也有很大意義。

針對(duì)基于視覺傳感器的三維重建問題，文獻(xiàn)［10］提出了一種基于運(yùn)動(dòng)恢復(fù)的雙目視覺三維重建系統(tǒng)，將原有拋物線擬合的方法加入到三維重建系統(tǒng)，提高了三維重建系統(tǒng)精度。但并未考慮到雙目圖像錯(cuò)誤立體匹配產(chǎn)生的三維結(jié)構(gòu)，重建結(jié)果中噪點(diǎn)較多，因此上述方法僅在形狀規(guī)則的物體中取得較好的效果。面向家庭環(huán)境的機(jī)器人視覺抓取的三維重建系統(tǒng)具有以下幾個(gè)特點(diǎn)：一是涉及到的場(chǎng)景較小，一般為桌面上玩偶、書本等物體，太大或太小的物體都難于使用抓手抓??；二是重建使用的硬件和三維重建軟件系統(tǒng)需簡(jiǎn)單易用，方便家庭用戶的使用；三是對(duì)精度與實(shí)時(shí)性有一定要求。

針對(duì)上述特點(diǎn)，提出了一種應(yīng)用于家庭環(huán)境中機(jī)器人抓取任務(wù)的單目三維重建系統(tǒng)，可以基于目前消費(fèi)電子產(chǎn)品中主流的單目視覺傳感器實(shí)時(shí)運(yùn)行，降低了傳統(tǒng)三維重建系統(tǒng)對(duì)傳感器的要求。同時(shí)，在傳統(tǒng)單目三維重建系統(tǒng)上增加了基于連通區(qū)域?yàn)V波器等后處理模塊，用于消除立體匹配中錯(cuò)誤恢復(fù)的三維結(jié)構(gòu)，保證重建效果和精度。此外，算法基于GPU并行處理技術(shù)［11］設(shè)計(jì)和實(shí)現(xiàn)，加速立體匹配過程，確保三維重建系統(tǒng)實(shí)時(shí)性。

2 單目三維重建系統(tǒng)算法框架

雙目三維重建系統(tǒng)建立在的理想雙目相機(jī)成像模型下，即左右兩個(gè)相機(jī)位于同一平面（光軸平行），且相機(jī)固有參數(shù)（如內(nèi)參矩陣K）一致。可推出如下公式，如圖1所示。

圖1 理想雙目模型Fig.1 Ideal Stereo Model

式中：x2-x1—視差，b—相機(jī)左右目的光心O1和O2間的距離，p1—空間點(diǎn)P在左相機(jī)成像的像素，p2—空間點(diǎn)P在左相機(jī)成像的像素。空間點(diǎn)P的深度Z與視差成反比，即深度圖的計(jì)算與視差圖的計(jì)算具有反比對(duì)應(yīng)關(guān)系。

對(duì)于圍繞物體運(yùn)動(dòng)并采集圖像的單目相機(jī)而言，雖然缺少物理意義上的雙目結(jié)構(gòu)，但可以組合當(dāng)前相機(jī)所拍攝圖像與歷史某時(shí)刻相機(jī)所拍攝的圖像組成從運(yùn)動(dòng)中構(gòu)造雙目（motion stereo）。如圖2所示，公式（1）在從運(yùn)動(dòng)中構(gòu)成雙目的相機(jī)配置下不再成立，且從運(yùn)動(dòng)中構(gòu)造雙目的相機(jī)相對(duì)位置關(guān)系未知且并不固定。因此不能簡(jiǎn)單應(yīng)用雙目三維重建方法解決單目三維重建問題。

圖2 從運(yùn)動(dòng)中構(gòu)造雙目的雙目相機(jī)位置關(guān)系Fig.2 Motion from Stereo

針對(duì)單目三維重建問題的上述特點(diǎn)，提出了的三維重建系統(tǒng)算法框架。在單目三維重建系統(tǒng)中，首先根據(jù)SFM算法［7-8］恢復(fù)不同時(shí)刻相機(jī)間的相對(duì)運(yùn)動(dòng)關(guān)系，根據(jù)幾何關(guān)系選擇相機(jī)當(dāng)前幀和過去某一幀組成虛擬雙目。根據(jù)虛擬雙目間的幾何關(guān)系和圖像像素相似性，計(jì)算當(dāng)前幀視場(chǎng)內(nèi)的場(chǎng)景點(diǎn)云。經(jīng)過坐標(biāo)系變換將不同時(shí)刻相機(jī)幀重建點(diǎn)云拼接為完整的物體模型。系統(tǒng)基于SFM算法恢復(fù)相機(jī)運(yùn)動(dòng)，后應(yīng)用TSDF方法［6］完成坐標(biāo)系對(duì)齊功能。上述方法已有成熟的研究結(jié)果，因此不再贅述相應(yīng)原理。下文中將詳細(xì)說明基于虛擬雙目構(gòu)造、立體匹配及后處理過程，如圖3所示。

圖3 算法流程圖Fig.3 Flow Chart of Proposed Algorithm

3 單目三維重建系統(tǒng)的三維結(jié)構(gòu)恢復(fù)

在單目三維重建系統(tǒng)中，三維結(jié)構(gòu)恢復(fù)指根據(jù)從運(yùn)動(dòng)中構(gòu)造的雙目計(jì)算相機(jī)視場(chǎng)內(nèi)的場(chǎng)景三維結(jié)構(gòu)。下面將從虛擬雙目構(gòu)造，虛擬雙目立體匹配，后處理三個(gè)部分介紹基于單目相機(jī)的三維結(jié)構(gòu)恢復(fù)方法。

3.1 虛擬雙目構(gòu)造

基于SFM方法恢復(fù)的相機(jī)相對(duì)位置關(guān)系存在一定誤差，因此要保證最終場(chǎng)景三維結(jié)構(gòu)的準(zhǔn)確性，需要合理選擇歷史相機(jī)幀與當(dāng)前幀配對(duì)，使得誤差對(duì)深度圖質(zhì)量的影響較小。在配對(duì)選擇的過程中，歷史相機(jī)幀與當(dāng)前相機(jī)幀間要保持較大的重疊和相似的朝向，這樣深度圖可以覆蓋圖像的大部分范圍，同時(shí)兩個(gè)相機(jī)狀態(tài)間要有合理的基線，較長(zhǎng)的基線會(huì)降低圖像的重疊度，減少深度圖的有效區(qū)域，較短的基線會(huì)導(dǎo)致深度圖易受到噪聲的干擾，降低深度估計(jì)的準(zhǔn)確性。基于上述分析，提出了如下評(píng)價(jià)函數(shù)：

3.2 虛擬雙目的立體匹配

由章2所述，從運(yùn)動(dòng)中恢復(fù)雙目得到雙目圖像對(duì)基本不滿足理想雙目模型，因此其立體匹配過程分為如下幾步：

像素匹配和匹配代價(jià)計(jì)算：首先基于先驗(yàn)信息確定待計(jì)算深度圖的圖像所對(duì)應(yīng)場(chǎng)景的深度分布區(qū)間，在得到的分布區(qū)間內(nèi)確定N個(gè)深度假設(shè)，每個(gè)深度假設(shè)對(duì)應(yīng)于一個(gè)代表深度值d的平面。為增強(qiáng)深度估計(jì)結(jié)果對(duì)于場(chǎng)景光照變化和圖像噪聲的魯棒性，在以像素p為中心的方形領(lǐng)域內(nèi)采樣計(jì)算代價(jià)函數(shù)C（p，d）。應(yīng)用ZNCC算子構(gòu)造像素對(duì)應(yīng)代價(jià)函數(shù)C（p，d）。

式中：q—右圖像中與像素p對(duì)應(yīng)的像素，K—內(nèi)參矩陣，R—相機(jī)旋轉(zhuǎn)矩陣，t—相機(jī)平移向量，n—平面法向量。q代表像素p在由圖像上的投影，如圖4所示。

圖4 像素點(diǎn)投影示意圖Fig.4 Illustration of Pixel Reprojection

代價(jià)聚合與深度值求精：計(jì)算得到每個(gè)深度假設(shè)d所對(duì)應(yīng)的代價(jià)函數(shù)C(p,d)后，按WTA規(guī)則估計(jì)每個(gè)像素p所對(duì)應(yīng)的深度：

為補(bǔ)償深度假設(shè)離散化造成的精度損失，采用拋物線擬合的方式獲取亞像素級(jí)深度估計(jì)精度。則拋物線函數(shù)極小值點(diǎn)對(duì)應(yīng)的深度值dmin滿足公式（5）：

式中：di—最小C(p,d)所對(duì)應(yīng)的深度假設(shè)，di-1di+1—與di相鄰的深度假設(shè)，Δd—深度假設(shè)間距。由于間中每個(gè)像素的深度估計(jì)過程具有獨(dú)立性，因此每個(gè)像素的深度估計(jì)過程可以并行進(jìn)行，得到初始深度圖。

3.3 后處理

立體匹配計(jì)算的深度圖中存在大量錯(cuò)誤深度估計(jì)，使最終的三維模型產(chǎn)生大量噪點(diǎn)，降低三維模型質(zhì)量。為解決這一問題，提出了如下的后處理過程。

左右一致性檢查：由于立體匹配過程具有計(jì)算對(duì)稱性，因此估計(jì)正確的深度應(yīng)滿足如下所示的重投影誤差公式：

式中：Dc—當(dāng)前幀深度圖，Dk—?dú)v史幀深度圖，δ—閾值，其余符號(hào)同公式（3）。

獨(dú)特性比率濾波器：在被圖像噪聲干擾或相對(duì)運(yùn)動(dòng)關(guān)系的誤差而產(chǎn)生錯(cuò)誤深度估計(jì)的區(qū)域，其最小代價(jià)函數(shù)與次小代價(jià)函數(shù)會(huì)比較接近。根據(jù)上述觀察，定義如下獨(dú)特性比率過濾噪聲。當(dāng)像素p的獨(dú)特性比率小于閾值時(shí)過濾對(duì)應(yīng)深度：

式中：runiq(p)—p像素的獨(dú)特性比率，dsec—次小代價(jià)函數(shù)對(duì)應(yīng)的深度假設(shè)，dmin—最小代價(jià)函數(shù)對(duì)應(yīng)的深度假設(shè)。rthres—獨(dú)特性比率的閾值。

連通區(qū)域?yàn)V波器：真實(shí)場(chǎng)景中大部分區(qū)域在空間是連續(xù)的，因此體現(xiàn)在深度圖上相鄰區(qū)域的深度值也具有一定的連續(xù)性。錯(cuò)誤匹配的深度值一般與其周邊區(qū)域的深度值相比具有較大的不連續(xù)性，且在深度圖中零散的分布與各個(gè)區(qū)域。根據(jù)如下公式判斷深度圖上某像素與相鄰區(qū)域在深度圖上的連續(xù)性關(guān)系。

式中：d—某像素對(duì)應(yīng)深度，dr某像素領(lǐng)域的對(duì)應(yīng)深度，Δd—深度假設(shè)間距。應(yīng)用上述公式將深度圖的深度連續(xù)區(qū)域的劃分問題轉(zhuǎn)化為一個(gè)二值圖像連通區(qū)域標(biāo)記問題，使用兩步法將所有的深度圖連通區(qū)域分類并標(biāo)記。根據(jù)標(biāo)記的連通區(qū)域中像素的數(shù)量，將像素?cái)?shù)量小于一定值的連通區(qū)域從原始深度圖中去除。經(jīng)過上述后處理后得到的深度圖經(jīng)過坐標(biāo)系對(duì)齊后得到最終的物體模型。

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

在家庭環(huán)境中常用且易于機(jī)器人抓取的三種物體上進(jìn)行了實(shí)驗(yàn)。實(shí)驗(yàn)圖像分辨率為640×480，程序運(yùn)行于配備i7-4710MQ處理器和GTX880M顯卡的筆記本電腦上。以Realsense SR300傳感器測(cè)量的結(jié)果作為真值。

對(duì)比了應(yīng)用后處理和不應(yīng)用后處理方法的重建結(jié)果。在實(shí)際環(huán)境中由于采集的圖像中圖像噪聲及場(chǎng)景光照變化，一些在理想條件下正確匹配的像素在噪聲干擾下匹配錯(cuò)誤，進(jìn)而物體模型表面不完整且中存在大量噪點(diǎn)。通過節(jié)3.3介紹的后處理流程可以過濾單幀深度圖中的錯(cuò)誤匹配結(jié)果。在基于運(yùn)動(dòng)的單目三維重建系統(tǒng)中，不同幀圖像恢復(fù)的三維結(jié)構(gòu)存在一定冗余，某一幀中被過濾的錯(cuò)誤三維結(jié)構(gòu)會(huì)被其他幀中正確估計(jì)的三維結(jié)構(gòu)代替，因此后處理模塊不會(huì)對(duì)完整性造成影響，如圖5所示。

圖5 實(shí)驗(yàn)結(jié)果對(duì)比Fig.5 Comparison of Experiment Result

基于結(jié)構(gòu)光傳感器的測(cè)量結(jié)果作為真值計(jì)算三維重建結(jié)果物體模型的精度。根據(jù)計(jì)算結(jié)果繪制了誤差色溫圖，誤差表示范圍從0%到100%，且色溫越冷代表誤差越小。上述三個(gè)實(shí)驗(yàn)物體的模型與結(jié)構(gòu)光傳感器測(cè)量真值的中位數(shù)相對(duì)誤差（MAE）均在1.0%左右，從相對(duì)誤差色溫圖中也可以看出重建物體模型的大部分區(qū)域誤差非常小。對(duì)于長(zhǎng)寬高在20cm范圍內(nèi)的家庭中常見物體而言，生成的模型可以滿足服務(wù)機(jī)器人構(gòu)建抓取物體數(shù)據(jù)庫(kù)的模型精度要求，如圖6所示。

圖6 三種不同物體重建模型（上）與誤差色溫圖（下）Fig.6 Models of Three different objects 3D Reconstruction（Up）and Corresponding Error Color Map（Down）

為驗(yàn)證三維重建系統(tǒng)實(shí)時(shí)性，記錄了圖6中玩偶熊模型建模過程中每一幀的處理時(shí)間并繪制了的處理時(shí)間隨拍攝時(shí)間變化的曲線。幀平均處理時(shí)間為33.95ms，即29.45幀/秒的處理速度，滿足實(shí)時(shí)運(yùn)行的要求，如圖7所示。

圖7 三維重建系統(tǒng)單幀處理時(shí)間曲線圖Fig.7 Graph of Running Time of 3D Reconstruction

5 結(jié)論

介紹了一種基于運(yùn)動(dòng)恢復(fù)深度的單目實(shí)時(shí)三維重建系統(tǒng)，針對(duì)運(yùn)動(dòng)中單目相機(jī)采集的圖像序列，提出了一種虛擬雙目相機(jī)的構(gòu)造準(zhǔn)則。并推導(dǎo)了虛擬雙目相機(jī)的立體匹配公式。針對(duì)實(shí)際場(chǎng)景中圖像噪聲對(duì)三維重建結(jié)果產(chǎn)生較大影響的問題，在傳統(tǒng)算法框架基礎(chǔ)上增加了后處理模塊。實(shí)驗(yàn)結(jié)果表明應(yīng)用后處理模塊，取得了質(zhì)量更好的物體模型。針對(duì)三種不同物體的重建實(shí)驗(yàn)說明了應(yīng)用三維重建系統(tǒng)的精度。系統(tǒng)并行處理圖像數(shù)據(jù)并生成三維模型。平均處理速度為29.45幀每秒，滿足服務(wù)機(jī)器人抓取任務(wù)對(duì)建模系統(tǒng)速度的要求。未來將在本文實(shí)驗(yàn)得到的模型基礎(chǔ)上，設(shè)計(jì)基于物體模型服務(wù)機(jī)器人抓取系統(tǒng)，進(jìn)一步驗(yàn)證本文模型對(duì)于服務(wù)機(jī)器人抓取任務(wù)的適用性。