基于NVIDIA JetSon TX2的無人機(jī)雙目測距算法設(shè)計(jì)

樊建昌， 余 粟, 尚鉦歷

(1 上海工程技術(shù)大學(xué) 機(jī)械與汽車工程學(xué)院， 上海 201620； 2 上海工程技術(shù)大學(xué) 工程實(shí)訓(xùn)中心， 上海 201620； 3 上海星河灣雙語學(xué)校， 上海 201108)

0 引 言

隨著無人機(jī)技術(shù)和計(jì)算機(jī)科學(xué)技術(shù)的不斷發(fā)展，無人機(jī)在商業(yè)上的應(yīng)用場景越來越廣泛，從最初的視頻業(yè)務(wù)如AR/VR、無人機(jī)航拍、物流無人機(jī)到后來的智能落地物聯(lián)網(wǎng)、基礎(chǔ)設(shè)施巡檢、無人安防等領(lǐng)域再到如今AI時(shí)代正大力發(fā)展的互聯(lián)網(wǎng)產(chǎn)品，所有的信息都表明了無人機(jī)巨大的商用價(jià)值。但是隨著無人機(jī)應(yīng)用領(lǐng)域的逐漸擴(kuò)大，各個(gè)應(yīng)用場景對(duì)無人機(jī)的性能要求越來越高，主要包括無人機(jī)的續(xù)航能力、拍攝、避障等關(guān)鍵技術(shù)。其中無人機(jī)的避障技術(shù)由于直接關(guān)乎到無人機(jī)的安全性，因此一直以來都是科研人員在無人機(jī)研究領(lǐng)域的重點(diǎn)和熱點(diǎn)問題。潘梟等人[1]提出了一種基于毫米波雷達(dá)的多旋翼無人機(jī)避障技術(shù)的設(shè)計(jì)方案。此方案通過機(jī)載避障傳感器毫米波雷達(dá)來實(shí)現(xiàn)多旋翼無人機(jī)避障功能；劉躍波等人[2]針對(duì)無人機(jī)電力巡檢的應(yīng)用場景，提出了基于T - S 模糊神經(jīng)網(wǎng)絡(luò)，建立模糊控制規(guī)則來對(duì)無人機(jī)的實(shí)時(shí)避障進(jìn)行仿真；金立軍等人[3]結(jié)合定位技術(shù)和激光測距技術(shù)，先通過慣性測量系統(tǒng)進(jìn)行初步定位，得到無人機(jī)的經(jīng)緯度坐標(biāo)，然后利用無人機(jī)機(jī)載激光測距模塊，測量故障點(diǎn)到無人機(jī)的距離來達(dá)到無人機(jī)避障的目的；饒俊[4]利用了單載波頻域均衡(SCFDE)技術(shù)以及優(yōu)化遙控幀與偽碼速率關(guān)系及發(fā)送位置的方法,降低了機(jī)上測距數(shù)據(jù)下傳誤碼率,提高了無人機(jī)系統(tǒng)的測距精度。與前述研究不同的是，本文利用被動(dòng)測距技術(shù)中的雙目視覺測距技術(shù)，以NVIDIA Jetson TX2智能AI計(jì)算模塊為計(jì)算平臺(tái)，以SGBM立體匹配算法為基礎(chǔ)，設(shè)計(jì)了無人機(jī)測距系統(tǒng)，在圖像分辨率為376*240、平均誤差為4.87%的情況下，測距長度上達(dá)到了7 m，幀率達(dá)到了8.9幀，基本上滿足了無人機(jī)飛行過程中的實(shí)時(shí)性要求。

1 攝像機(jī)成像模型

常見的理想狀態(tài)下攝像機(jī)成像模型如圖1[5]所示。模型中包含了4個(gè)坐標(biāo)系，分別是：世界坐標(biāo)系、相機(jī)坐標(biāo)系、圖像坐標(biāo)系以及圖像像素坐標(biāo)系。其中，物體上的任意一點(diǎn)P在世界坐標(biāo)系中的位置記為(Xw，Yw，Zw)，在攝像機(jī)坐標(biāo)系xoy中的坐標(biāo)為(X，Y，Z)，在圖像像素坐標(biāo)系XfOYf中的坐標(biāo)為(u,v)，結(jié)合三角形相似推得點(diǎn)P由世界坐標(biāo)系到圖像像素坐標(biāo)系的轉(zhuǎn)化過程可由式(1)表示[6]：

M*N*Pw.

(1)

其中，s為縮放因子；M為攝像機(jī)的內(nèi)參數(shù)矩陣，描述了點(diǎn)P由相機(jī)坐標(biāo)系到圖像坐標(biāo)系的轉(zhuǎn)化關(guān)系，M由相機(jī)自身的安裝精度及透鏡特性決定；N為點(diǎn)P由世界坐標(biāo)系到相機(jī)坐標(biāo)系的轉(zhuǎn)換矩陣，稱為攝像機(jī)模型的外參數(shù)矩陣；R3*3=[r1,r2,r3]是一個(gè)正交矩陣，T3*1=t3*1。二者分別為點(diǎn)P由世界坐標(biāo)系轉(zhuǎn)換到相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣和平移矩陣。內(nèi)參數(shù)矩陣M以及外參數(shù)矩陣N可以由單目相機(jī)標(biāo)定求得。

圖1 攝像機(jī)成像模型

2 雙目測距的基本原理

雙目立體視覺測距主要是利用雙目相機(jī)從不同視角拍攝同一物體時(shí)在圖像中得到的不同位置信息來計(jì)算圖像的視差信息，進(jìn)一步利用三角形相似的原理求解出物體的三維坐標(biāo)信息來達(dá)到求解物體的距離。其原理圖如圖2所示。

圖2 雙目成像原理

在圖2的平行雙目成像系統(tǒng)中，P表示待測目標(biāo)上的某一點(diǎn),Ol、Or分別表示左右攝像機(jī)坐標(biāo)系的原點(diǎn)(左右相機(jī)的光心),Pl、Pr分別表示P在左右相機(jī)CCD芯片上的投影點(diǎn)，由圖2的攝像機(jī)成像模型易得，Ol、Or與左右相機(jī)的CCD芯片的中心連線垂直于Pl、Pr所形成雙目基準(zhǔn)平面。xl、xr分別為物體上的P在左右相機(jī)成像平面的投影點(diǎn)到成像平面左邊界的物理距離，f為相機(jī)焦距，T為雙目相機(jī)左右光心之間的物理距離，Z為物體距離攝像機(jī)水平面的真實(shí)的物理距離[7]。

觀察圖2，由ΔPPlPr～ΔPOlOr得：

(2)

將PlPr=T-(Xl-Xr) 代入式(2)得：

(3)

由相機(jī)坐標(biāo)系和圖像坐標(biāo)系的轉(zhuǎn)換關(guān)系可得：

xr-xl=(ul-ur)*dx,

(4)

其中，dx表示在圖像坐標(biāo)系中，每個(gè)像素在CCD芯片上所占的實(shí)際物理尺寸的大小。將式(4)代入式(3)得：

(5)

(6)

2.1 雙目標(biāo)定原理

雙目標(biāo)定是雙目系統(tǒng)中求取左右相機(jī)之間的旋轉(zhuǎn)矩陣R以及平移矩陣T的過程。 由圖1～圖2可得，在不考慮攝像頭畸變和相機(jī)水平擺放的誤差的前提下，物體點(diǎn)P由世界坐標(biāo)系投影到相機(jī)坐標(biāo)系可以用式(7)來表示：

(7)

進(jìn)一步可得右相機(jī)到左相機(jī)的旋轉(zhuǎn)矩陣及平移矩陣如下[8]：

(8)

其中，R、T即為右相機(jī)到左相機(jī)的旋轉(zhuǎn)矩陣和平移矩陣。

2.2 立體校正

在實(shí)際的雙目測距項(xiàng)目中，雙目相機(jī)由于安裝工藝的缺陷常常導(dǎo)致實(shí)際的安裝情況如圖3所示[9],這會(huì)使得后期的立體匹配需要在二維方向上進(jìn)行。所以，在實(shí)際的測距過程中，在立體匹配之前常常使用Bouguet立體校正使雙目相機(jī)的左右成像平面平行，并且使得物體在左右圖像上的投影點(diǎn)位于同一水平線上。這樣做的目的是通過運(yùn)用極限約束原理使得同一物體在左右兩幅圖像中的搜索范圍從二維降到了一維。

圖3 雙目成像系統(tǒng)

Bouguet校正矩陣的求解主要分為2步，對(duì)此可得闡釋分述如下。

(1)求解左右相機(jī)共面校正矩陣。由于R是正交實(shí)矩陣，則R是正規(guī)矩陣[10]，所以有：

R=S*D*S-1,

(9)

其中，D為主對(duì)角元素為R的特征值組成的矩陣，S為R的特征向量組成的矩陣。進(jìn)一步拆分式(9)可得：

(10)

令：

(11)

則將2個(gè)相機(jī)之間的旋轉(zhuǎn)矩陣R劃分為左右相機(jī)的合成矩陣Rl、Rr，可以實(shí)現(xiàn)左右圖像平面共面。但是行沒有對(duì)齊。

(2)求解行對(duì)校正矩陣。 根據(jù)式(12)建立行對(duì)準(zhǔn)矩陣Rrect。其中,T為由右相機(jī)到左相機(jī)的平移矩陣。

綜合以上2步分別計(jì)算左右相機(jī)的行對(duì)準(zhǔn)變換矩陣,即：

(12)

(13)

2.3 立體匹配

立體匹配是雙目測距中最關(guān)鍵的一部分，本文選用立體匹配中最常用的SGBM算法作為雙目測距中的立體匹配算法，其原理為[11]：確定左右圖像的匹配基元；構(gòu)建基于多個(gè)方向掃描線的代價(jià)能量和函數(shù)；求取能量代價(jià)和函數(shù)的最優(yōu)解。該算法在OpenCV中的實(shí)現(xiàn)流程具體如下。

(1)預(yù)處理。SGBM采用水平Sobel算子，把圖像做處理，再用一個(gè)函數(shù)將經(jīng)過水平Sobel算子處理后的圖像上每個(gè)像素點(diǎn)(P表示其像素值)映射成一個(gè)新的圖像，Pnew表示新圖像上的像素值。映射函數(shù)如下：

(14)

其中，preFilterCap是一個(gè)常數(shù)參數(shù)，OpenCV缺省情況下取15。

(2)代價(jià)計(jì)算。由2部分組成：

① 經(jīng)過預(yù)處理后得到的圖像的梯度信息利用基于采樣[12]的方法得到的梯度代價(jià)。

② 原圖像經(jīng)過基于采樣的方法得到的SAD代價(jià)，其計(jì)算公式如下：

(15)

(3)動(dòng)態(tài)規(guī)劃。SGBM算法試圖通過圖像上多個(gè)方向上一維路徑的約束，來建立一個(gè)全局的馬爾科夫能量方程，每個(gè)像素最終的匹配代價(jià)是所有路徑信息的疊加，每個(gè)像素的視差選擇都只是簡單通過 WTA(Winner Takes All)決定的。多方向能量聚集如圖4所示。其中，每個(gè)方向都按照動(dòng)態(tài)規(guī)劃的思想進(jìn)行能量累積，再將所有方向的匹配代價(jià)相加得到總的匹配代價(jià)，其計(jì)算公式可表示為：

圖4 能量聚集方向

Lr(p,d)=c(p,d)+

其中，L為當(dāng)前路徑累積的代價(jià)函數(shù)；P1、P2為像素點(diǎn)與相鄰點(diǎn)視差存在較小和較大差異情況下的平滑懲罰，P1

(16)

(4)后處理。主要包括如下內(nèi)容：匹配窗口內(nèi)的視差唯一性檢測；亞像素插值；左右一致性檢測；連通區(qū)域的檢測。

3 實(shí)驗(yàn)流程及結(jié)果

本文以NIVIDIA JetSon TX2 為處理平臺(tái)和小覓S1030(入門版)雙目攝像機(jī)搭建實(shí)驗(yàn)平臺(tái)，詳見圖5。這里，給出研究實(shí)驗(yàn)流程及結(jié)果的分析表述如下。

(1)本文以單元格大小為5 m*5 m的12*9高精度棋盤格作為標(biāo)定板，左右相機(jī)分別采集29張標(biāo)定板圖像，經(jīng)過預(yù)處理后利用張氏標(biāo)定法[13]對(duì)其進(jìn)行標(biāo)定，得到左右相機(jī)的內(nèi)參數(shù)矩陣以及各自的畸變參數(shù)，標(biāo)定結(jié)果界面如圖6所示，標(biāo)定結(jié)果數(shù)值具體如下：

Dl=[-250.357 6,505.793 9,-705.366 3,

-851.954 5,0.000 0]

Dr=[-303.763 8,799.294 3,515.475 4,

-672.489 0,0.000 0]

其中，Ml、Mr分別為左右相機(jī)的內(nèi)參數(shù)矩陣，Dl、Dr分別為左右相機(jī)的畸變參數(shù)矩陣。

圖5 實(shí)驗(yàn)平臺(tái)

圖6 標(biāo)定結(jié)果

(2)選取29組左右相機(jī)的圖片，標(biāo)定板圖片的分辨率為752*480。利用OpenCV視覺庫編寫雙目標(biāo)定程序，以左相機(jī)坐標(biāo)系作為雙目標(biāo)定的基準(zhǔn)坐標(biāo)系，通過OpenCV中的stereoCalibrate()函數(shù)計(jì)算得到旋轉(zhuǎn)矩陣R和平移矩陣T結(jié)果如下：

T=[-1.200 248 976 4e+02,-1.178 263 74e+00,-5.205 820 51e+00]

(3)利用(1)中得到的左右相機(jī)的內(nèi)參數(shù)和畸變參數(shù)，以及(2)中的得到的左右相機(jī)之間的外參數(shù)，再利用通過OpenCV視覺庫中的cvStereoRectify()和initUndistortRectifyMap()函數(shù)計(jì)算出左右相機(jī)的立體校正和畸變校正參數(shù)矩陣。

(4)利用OpenCV視覺庫中的remap()函數(shù)對(duì)左右圖像進(jìn)行立體校正和畸變校正，使得左右相機(jī)圖像處于行對(duì)準(zhǔn)的理想狀態(tài)。

(5)以SGBM立體匹配算法為基礎(chǔ)，利用OpenCV視覺庫編寫雙目測距的運(yùn)行算法，計(jì)算出左右圖像的視差圖。在此基礎(chǔ)上，進(jìn)一步利用OpenCV視覺庫中的cvReprojectImageTo3D()函數(shù)計(jì)算得到障礙物距離雙目相機(jī)的距離,其中SGBM算法中的相關(guān)參數(shù)設(shè)置及含義如圖7所示。

圖7 SGBM算法及參數(shù)

本文在以NVIDIA JetSon TX2為計(jì)算平臺(tái)上，選取3 m、5 m、7 m作為雙目測距的基準(zhǔn)距離，在近似于無人機(jī)空中飛行的簡單場景下，采用20次測量結(jié)果取平均值，得到的實(shí)驗(yàn)結(jié)果見表1。

表1 雙目測距實(shí)驗(yàn)結(jié)果

4 結(jié)束語

由實(shí)驗(yàn)結(jié)果可得，對(duì)于SGBM算法，在752*480的分辨率下，本文設(shè)計(jì)的雙目系統(tǒng)測距的準(zhǔn)確度要高于376*240的準(zhǔn)確率，但是在實(shí)時(shí)性上，752*480的分辨率在4.6幀左右，很難滿足無人機(jī)的高速飛行，在376*240的分辨率下，當(dāng)測量距離在7 m時(shí)，測距誤差在4.8%左右，其測距的準(zhǔn)確性上不如752*480的分辨率，但是其幀率達(dá)到了8.9幀，基本滿足了無人機(jī)在高速飛行時(shí)的實(shí)時(shí)性要求。

與此同時(shí)，本系統(tǒng)也仍存在一定的問題，例如在單目標(biāo)定時(shí)，由于所用的標(biāo)定板過小，導(dǎo)致標(biāo)定得到的相機(jī)的內(nèi)參數(shù)和外參數(shù)的精度還有待提高。測距的準(zhǔn)確度也亟需改善，實(shí)時(shí)性上還可以基于CUDA加速做進(jìn)一步的加速計(jì)算，本系統(tǒng)仍有很大的提升空間。