動(dòng)態(tài)場景下基于雙目的無人機(jī)SLAM研究

(廣東工業(yè)大學(xué) 廣東 廣州 510006)

一、引言

近年來隨著微電子技術(shù)和微型控制器性能的提升以及無人機(jī)產(chǎn)鏈的成熟，無人機(jī)在軍事領(lǐng)域和民用領(lǐng)域都得到廣泛的應(yīng)用，并在災(zāi)害探測，測繪，航拍等方面大放異彩。四旋翼飛行器憑借著結(jié)構(gòu)簡單，成本低，操作簡便以及懸停功能等諸多優(yōu)勢[1]，獲得了越來越多的關(guān)注。日益增長的需求和任務(wù)的復(fù)雜程度越來越高，使得無人機(jī)要向自主化智能化方向發(fā)展。要實(shí)現(xiàn)無人機(jī)在未知環(huán)境下的自主飛行需要無人機(jī)具備環(huán)境感知和定位的功能。在室外時(shí)，GPS廣泛應(yīng)用于各種產(chǎn)品的定位，但是GPS精度受環(huán)境影響較大，在建筑群或者是室內(nèi)的時(shí)候GPS信號(hào)弱甚至沒有，導(dǎo)致了無人機(jī)無法在室內(nèi)或者在密閉空間使用GPS做定位。并且單單使用GPS無人機(jī)會(huì)無法獲得環(huán)境的信息從而無法實(shí)現(xiàn)避障路徑規(guī)劃等功能[2]。因此使用視覺傳感器來做定位是一個(gè)必然趨勢，攝像頭具有體積小，功耗低，成本低，可提供豐富的環(huán)境信息,這為機(jī)器視覺的位姿估計(jì)提供了良好的硬件環(huán)境。同時(shí)定位和地圖構(gòu)建SLAM((Simultaneous localizationand mapping)問題在移動(dòng)機(jī)器人領(lǐng)域有著悠久的歷史，根據(jù)傳感器的類型可分為激光SLAM系統(tǒng)[3]，單目SLAM，雙目SLAM和RGB-D SLAM[3]。激光SLAM精度高，但是價(jià)格昂貴；單目SLAM對計(jì)算資源要求不高，但是缺少物體的實(shí)際尺度信息，系統(tǒng)初始化困難，對于無人機(jī)而言存在著很大的隱患。RGB-D SLAM可以直接獲得深度信息但是受光照影響大，而且適用距離有限，不適合用于室外環(huán)境。雙目SLAM系統(tǒng)具有尺度信息而且對于光照等環(huán)境的變化具有一定的魯棒性，所以適合用于無人機(jī)上[4]。目前許多先進(jìn)的算法相繼被提出來，列如ORB-SLAM,LSD-SLAM[9],DSO[10]等。然而這些算法是假設(shè)環(huán)境是絕對靜止的，雖然它們能夠?qū)崿F(xiàn)很好的精度和魯棒性，但是沒有解決場景中的動(dòng)態(tài)對象的問題。上述方法利用了物理先驗(yàn)算法實(shí)現(xiàn)動(dòng)態(tài)場景下的相機(jī)姿態(tài)實(shí)時(shí)估計(jì)，并且具有很好的精度。當(dāng)動(dòng)態(tài)場景中存在快速移動(dòng)的剛性或者非剛性物體對象時(shí)，如移動(dòng)的人，車，以及其他物體時(shí)，VSLAM往往會(huì)產(chǎn)生較大的定位誤差或者導(dǎo)致跟蹤失敗。因此，動(dòng)態(tài)環(huán)境下的位姿估計(jì)一直是一個(gè)難題[5-8]。今年來人工智能的快速發(fā)展，使得目標(biāo)檢測技術(shù)日趨成熟，目標(biāo)檢測可以應(yīng)用于各種應(yīng)用，如車輛識(shí)別，人臉識(shí)別，物體跟蹤等。我們可以充分利用人工智能的成果來加強(qiáng)SLAM系統(tǒng)的性能，目前有的語義分割SLAM系統(tǒng)就是代表性產(chǎn)物。但是語義分割方法的運(yùn)行速度和準(zhǔn)確率成反比，對于無人機(jī)這樣運(yùn)算資源有限而且實(shí)時(shí)性要求比較強(qiáng)的系統(tǒng)并不使用。YOLO[10]目標(biāo)檢測系統(tǒng)具有算法準(zhǔn)確率高而且運(yùn)算速度快。因此我們提出了利用YOLO目標(biāo)檢測系統(tǒng)檢測出潛在的動(dòng)態(tài)物體，然后去掉動(dòng)態(tài)物體再進(jìn)行特征點(diǎn)的匹配來獲取無人機(jī)的姿態(tài)。從而改進(jìn)SLAM算法的魯棒性，使其能夠應(yīng)對動(dòng)態(tài)場景。

二、系統(tǒng)結(jié)構(gòu)

(一)ORB特征點(diǎn)檢測

ORB[9]特征點(diǎn)由關(guān)鍵點(diǎn)和描述子兩部分組成。其關(guān)鍵點(diǎn)稱作“Oriented FAST”,是一種改進(jìn)的FAST角點(diǎn)，使得角點(diǎn)具有方向性。其描述子被稱為BRIEF(binary robust independent elementary feature)，是一種二進(jìn)制描述子，使用了隨機(jī)選點(diǎn)的比較，速度非常快，而且存儲(chǔ)十分方便。提取ORB描述子分為兩個(gè)步驟：

1.FAST角點(diǎn)的提取：在圖像中選取像素p，然后計(jì)算半徑為r的半圓上的像素點(diǎn)的亮度Ip，如果有連續(xù)N個(gè)點(diǎn)亮度大于Ip+τ(閾值)或者Ip-τ，則被認(rèn)為是一個(gè)角點(diǎn)。

在一個(gè)圖像塊B中，定義圖像的像素的矩為：

mpq=∑x,yxpyqI(x,y)

(1)

其中為圖像點(diǎn)(x，y)處的灰度值，通過矩可以得到圖像的質(zhì)心：

(2)

那么幾何中心O與質(zhì)心C的方向角可以定義為：

θ=arctan(m01,m10)

(3)

2.BRIEF描述子，采用隨機(jī)選取點(diǎn)的方法，比較附件兩個(gè)隨機(jī)像素(比如p和q)的大，如果p比q大，則取1，反之取0然后我們?nèi)個(gè)這樣的點(diǎn)，組成的向量變?yōu)锽RIEF描述子。

(二)基于深度學(xué)習(xí)的目標(biāo)檢測算法

YOLO全稱是You Only Look Once，這個(gè)算法來源于Joseph Redmon等人在CVPR 2015上發(fā)表的一篇論文[10]。它不僅解決了目標(biāo)邊界框不精確的問題，而且算法速度也很快。YOLO的基本思路是，把一個(gè)大的圖像分割成更小更精細(xì)的n等份的網(wǎng)格，然后每個(gè)網(wǎng)格都打上標(biāo)簽，這個(gè)格子內(nèi)是否有目標(biāo)，目標(biāo)的位置以及目標(biāo)所屬類別。YOLO算法將目標(biāo)檢測任務(wù)看作目標(biāo)區(qū)域預(yù)測和類別預(yù)測的回歸問題。該方法采用單個(gè)神經(jīng)網(wǎng)絡(luò)直接預(yù)測物品邊界和類別概率，實(shí)現(xiàn)端到端(end to end)的物品檢測。同時(shí)，該方法檢測速非常快，基礎(chǔ)版可以達(dá)到45幀/s的實(shí)時(shí)檢測；FastYOLO[11]可以達(dá)到155幀/s。通過YOLOv3算法把圖像的動(dòng)態(tài)物圖識(shí)別出來，把它們分為動(dòng)態(tài)特征點(diǎn)集合。

(三)雙目相機(jī)檢測原理

雙目相機(jī)是由兩個(gè)基于針孔模型的中心投影的單目相機(jī)組成[12]，通過中心投影可以建立三維點(diǎn)與像平面之間的關(guān)系，由成像關(guān)系可得：

(4)

(5)

d=z·s

(6)

其中，fx,fy指相機(jī)在x,y兩個(gè)軸上的焦距，cx,cy指相機(jī)的光圈中心，s指深度圖的縮放因子。空間點(diǎn)p[x,y,z]和它在圖像中的像素坐標(biāo)[u,v,d](d指深度數(shù)據(jù))，f為焦距。我們會(huì)把fx,fy,cx,cy這四個(gè)參數(shù)定義為相機(jī)的內(nèi)參矩陣，也就是相機(jī)做好之后就不會(huì)變的參數(shù)。相機(jī)的內(nèi)參可以用很多方法來標(biāo)定。雙目相機(jī)深度測量原理[14]如下從理想的情況開始分析:假設(shè)左右兩個(gè)相機(jī)位于同一平面(光軸平行)，且相機(jī)參數(shù)(如焦距f)一致。根據(jù)上述推導(dǎo)，要計(jì)算空間點(diǎn)P離相機(jī)的距離(深度)z，必須要知道：

1.相機(jī)焦距f，左右相機(jī)基線b。這些參數(shù)可以通過先驗(yàn)信息或者相機(jī)標(biāo)定得到。

2.視差d。需要知道左相機(jī)的每個(gè)像素點(diǎn)(xl,yl)和右相機(jī)中對應(yīng)點(diǎn)(xr,yr)的對應(yīng)關(guān)系。這是雙目視覺的核心問題

(四)系統(tǒng)的結(jié)構(gòu)

1.硬件結(jié)構(gòu)

本系統(tǒng)的硬件由大疆飛行器機(jī)架，px4飛控，地面站構(gòu)成，無人機(jī)搭載了機(jī)載算計(jì)(intel up2 CPU N4200 8G+128G 1套)，小覓雙目攝像頭，光流傳感器構(gòu)成。大疆的650機(jī)架能夠提供2kg的載重負(fù)荷，保證了其能夠搭載機(jī)載計(jì)算機(jī)以及攝像頭。機(jī)載計(jì)算機(jī)采用NVDIA的Jetson TX2，其具有256顆的CUDA核4顆ARM A57核以及8GB的內(nèi)存，對圖像處理有強(qiáng)大的優(yōu)勢，而且其體積小，功耗低使用于無人機(jī)的機(jī)載計(jì)算機(jī)。另外采用小覓的ZED雙目相機(jī)，其具有高分辨率，較低的光靈敏度和自帶IMU。飛控采用Pixhawk飛控，不僅擁有穩(wěn)定的飛行控制還提供了豐富的外部接口。攝像頭采集的圖像經(jīng)過Jetson TX2處理后由基于ROS(robot operation system的MavLink協(xié)議與pixhawk進(jìn)行通信。

2.算法流程

本算法在ROS系統(tǒng)中完成，其中軟件架構(gòu)如下所示，通過視覺傳感器讀取第一幀RGB圖像，對圖像進(jìn)行ORB特征點(diǎn)提取并計(jì)算特征點(diǎn)數(shù)量，如果特征點(diǎn)數(shù)量大于閾值，則把圖像作為參考幀，系統(tǒng)初始化。如果特征點(diǎn)數(shù)量小于閾值則讀取下一幀RGB圖片，提取ORB特征點(diǎn)，直到特征點(diǎn)數(shù)量大于閾值，進(jìn)行系統(tǒng)初始化。對于初始化的系統(tǒng)，讀取下一幀圖像，并使用YOLO算法對圖像進(jìn)行目標(biāo)檢測，并將檢測到的動(dòng)態(tài)物體框選出來。然后對剔除動(dòng)態(tài)物體的區(qū)域進(jìn)行特征點(diǎn)提取以及匹配。通過空間幾何原理得到特征點(diǎn)的三維坐標(biāo)。然后對進(jìn)行特征點(diǎn)進(jìn)行跟蹤。

三、實(shí)驗(yàn)結(jié)果與分析

本文對設(shè)計(jì)的方案分別在TUM[2]集上和真實(shí)飛行兩種實(shí)驗(yàn)驗(yàn)證。在數(shù)據(jù)集上我們和ORB-SLAM進(jìn)行了比較，將重點(diǎn)放在具有動(dòng)態(tài)物體的系列上。

圖1 本方法和ORB_SLAM在動(dòng)態(tài)場景下的對比(左ORB_SLAM右本方法)

由圖可知，在一些具有移動(dòng)的行人的環(huán)境中，ORB-SLAM會(huì)對動(dòng)態(tài)物體進(jìn)行檢測，其結(jié)果會(huì)對系統(tǒng)的跟蹤和精度有一定的影響。本系統(tǒng)中對環(huán)境中的車輛和行人作為動(dòng)態(tài)對象并進(jìn)行了檢測和處理。圖1中可以看出本算法能夠避免對行人進(jìn)行特征匹配，從而提高了系統(tǒng)的精度。

圖2是在是測試時(shí)候的真實(shí)場景，為了安全考慮，無人機(jī)在無人的公路旁以及僅有少量車輛的環(huán)境下進(jìn)行了飛行測試。實(shí)驗(yàn)所設(shè)定的圖像采集頻率為15幀/s，分辨率為1280x720。由圖可得，傳統(tǒng)ORB-SLAM算法在圖片進(jìn)行特征點(diǎn)提取的時(shí)候會(huì)把動(dòng)態(tài)物體的特征點(diǎn)計(jì)算進(jìn)去，而本算法可以對動(dòng)態(tài)物體進(jìn)行識(shí)別，在做特征點(diǎn)匹配時(shí)候可以將其剔除。驗(yàn)證了本文算法具有更高的精確度。

圖2 飛行結(jié)果展示

四、結(jié)束語

為了減少動(dòng)態(tài)物體對slam系統(tǒng)的影響，提高無人機(jī)飛行的安全性和魯棒性，本文提出了一種將基于深度學(xué)習(xí)的目標(biāo)檢測的YOLOv3算法引入到基于視覺特征實(shí)時(shí)檢測的SLAM系統(tǒng)中。將潛在的動(dòng)態(tài)物體進(jìn)行識(shí)別，然后對其區(qū)域的特征點(diǎn)進(jìn)行分類處理，然后對非動(dòng)態(tài)特征點(diǎn)進(jìn)行特征點(diǎn)的匹配，減少了動(dòng)態(tài)特征點(diǎn)參與位姿跟蹤和建圖所造成的誤差。與ORB_SLAM相比較，系統(tǒng)的精確性和魯棒性有了不少的提高。但是其處理造成了系統(tǒng)的響應(yīng)速度下降，在物體檢測要消耗不少時(shí)間，因此對機(jī)載計(jì)算機(jī)的性能要求較高。并且在無人機(jī)快速移動(dòng)的時(shí)候容易造成跟蹤失敗。因此在未來的研究中，我們將重點(diǎn)研究如何提高精確度的同時(shí)降低圖像的處理時(shí)間，保證系統(tǒng)的實(shí)時(shí)性能。