動態(tài)場景下基于視覺特征的SLAM方法

張金鳳,石朝俠,王燕清

(1.南京理工大學 計算機科學與工程學院,南京 210094; 2.南京曉莊學院 信息工程學院,南京 211171)

0 概述

同時定位與地圖構建(Simultaneous Localization and Mapping,SLAM)問題是機器人和計算機視覺領域的研究熱點。研究人員已設計了不同的傳感器模式,包括2D激光掃描儀、3D掃描儀、單目相機、雙目相機和RGB-D傳感器?，F(xiàn)有SLAM方法按照輸入數(shù)據(jù)類型的不同,可分為基于深度相機的方法和基于單目相機的方法,按照采用方法的不同,也可以分為依靠特征點進行匹配并構建稀疏地圖的特征點法和最小化光度誤差并構建稠密地圖的直接法。本文的研究對象是基于ORB(ORiented Brief)特征點的SLAM方法[1-2]。ORB特征點[3]可以在CPU上進行實時計算,相比Harris等簡單角點特征,其具有良好的旋轉和縮放不變性。此外,ORB定義的描述子在運動范圍較大時也可以實現(xiàn)良好的回環(huán)檢測和重定位效果。為了消除應用場景中移動目標和潛在移動目標的影響,本文引入基于深度學習的對象檢測算法以檢測場景中的移動目標和潛在移動目標?；诜菨撛趧討B(tài)目標的運動模型對特征點全集進行篩選,利用其中的靜態(tài)特征點實現(xiàn)位姿跟蹤,通過靜態(tài)特征點中的非潛在動態(tài)特征點進行地圖構建。

1 相關工作

目前,多數(shù)SLAM方法的一個基本假設是環(huán)境為靜態(tài)的。然而,像人類這樣的活動對象存在于許多真實場景中。因此,最初設計用于在靜態(tài)環(huán)境中執(zhí)行SLAM的方法都無法處理復雜的動態(tài)場景。為解決該問題,需要從環(huán)境中識別出移動目標,然后在進行姿態(tài)估計之前丟棄它們。在稠密SLAM中,許多運動目標檢測方法往往基于光流方法。如果圖像中存在運動目標就會產(chǎn)生光流,通過計算光流的不一致性可以區(qū)分靜態(tài)背景和運動對象。例如,文獻[4]使用最優(yōu)估計和均勻采樣的方法來檢測動態(tài)對象,與其他導數(shù)光流方法相比,該方法具有較高的時效性,但精度較低且計算量較大,尤其是在采樣量較大的情況下。文獻[5]基于點軌跡對圖像進行聚類,將動態(tài)對象排除在最小化能量函數(shù)之外,該方法具有健壯性但不具備實時性。近年來,在動態(tài)場景中出現(xiàn)了許多SLAM方法,它們大多使用語義分割或目標檢測算法來識別環(huán)境中的動態(tài)目標。

dynaSLAM[6]方法中設計了動態(tài)對象檢測系統(tǒng),其使用MASK-RCNN[7]實現(xiàn)實例分割,并對具有移動性的物體進行分割,將之前的20個關鍵幀的RGB信息和深度信息映射投影到當前幀上,在沒有動態(tài)對象的情況下完成背景修復。該系統(tǒng)能夠較好地處理動態(tài)場景,但無法實現(xiàn)實時操作,而實時性能對于SLAM相關研究而言不容忽視。在DS-SLAM[8]方法中,通過將語義分割網(wǎng)絡與光流法相結合以提供八叉樹地圖的語義表示,從而降低基于視覺的SLAM中動態(tài)對象的影響。VSO[9]的主要思想是使用語義作為不變的場景表示,其假設視角、尺度和光照等方面的變化只影響物體的低層外觀而不影響其語義。基于這一思想,文獻[9]提出了一種將語義約束集成到姿態(tài)優(yōu)化和地圖優(yōu)化中的視覺語義測距方法。

近年來,駕駛環(huán)境中的目標檢測問題得到廣泛關注。自動檢測道路上的車輛和行人等物體有助于駕駛員了解道路狀況、交通信息等,因此,目標檢測可以用于各種應用,如自動車輛[10]和自動監(jiān)視系統(tǒng)[11]等。然而,在開發(fā)駕駛環(huán)境下可靠目標檢測方法的過程中,面臨著如目標遮擋[12]、尺度的大方差[13]等問題,尤其是由停放的汽車、過往車輛和行人引起的道路阻塞,這種遮擋會使道路上的目標檢測難度升高。在文獻[14-15]中,提出了一種基于HOG和線性SVM的行人檢測方法,其主要思想是利用HOG特征和線性SVM學習正、負樣本模板。文獻[16]提出了一種基于多尺度變形分量檢測模型的可變形零件模型。DPM是除深度學習之外的最優(yōu)對象檢測模型,其通常采用滑動窗口檢測方法,通過構造一個尺度金字塔對每個尺度進行搜索。區(qū)域卷積神經(jīng)網(wǎng)絡(RCNN)[17-19]是較早的CNN之一,在檢測模型中應用網(wǎng)絡進行目標檢測。RCNN在傳統(tǒng)選擇性搜索的基礎上選擇候選集,建立CNN特征提取網(wǎng)絡從而檢測目標。YOLO是一種基于神經(jīng)網(wǎng)絡的目標檢測系統(tǒng)[20],其將整個圖像作為網(wǎng)絡模型的輸入,將圖像劃分為S×S網(wǎng)格,如果一個物體的中心落在網(wǎng)格上,網(wǎng)絡將檢測這個物體然后輸出該區(qū)域的物體以及置信度。

2 體系架構

2.1 主要框架

在現(xiàn)有面向動態(tài)環(huán)境的SLAM改進方法中,實時性是一種重要的評價指標。首先,本文考慮將基于像素的語義分割方法與SLAM相結合,但實驗發(fā)現(xiàn)基于像素的語義分割方法的運行速度和準確率成反比。因此,本文引入基于深度學習的目標檢測方法來檢測環(huán)境中潛在的動態(tài)對象,該方法較高的運行速度滿足了SLAM系統(tǒng)的實時性要求。如圖1所示,本文在基于特征點的SLAM方法中增加了基于深度學習的目標檢測方法,將輸入圖像中提取出的特征點分為兩類,一類是潛在動態(tài)特征點,這一類特征點具有可移動性,在場景中并非長時間固定存在(如車、人等目標上提取的特征點),在重定位和閉環(huán)檢測時可能會導致場景匹配失敗;另一類是非潛在動態(tài)特征點,這一類特征點在場景中往往不能移動或者在足夠長的時間內(nèi)不會移動(如建筑物、樹木等目標上提取的特征點)。

圖1 本文方法系統(tǒng)框架Fig.1 System framework of the method in this paper

(1)

2.2 ORB特征點提取

ORB特征點由關鍵點和描述符2個部分組成。ORB特征點提取主要分為2個步驟:

1)FAST角點提取:找到圖像中的角點,計算特征點的主方向并為后續(xù)的簡要描述符添加旋轉不變特征。

2)BRIEF描述子:描述上一步提取的特征點周圍的圖像區(qū)域。

FAST角點提取如圖2所示,其主要依據(jù)是:如果一個像素與其周圍像素顯著不同(太亮或太暗),則它可能就是角點。

圖2 FAST角點提取示意圖Fig.2 Schematic diagram of FAST corner extraction

ORB添加了尺度和旋轉的描述。對于任意一個特征點p而言,其鄰域像素的矩為:

(2)

其中,I(x,y)為點(x,y)處的灰度值?？梢缘玫綀D像的質(zhì)心為:

(3)

則特征點與質(zhì)心的夾角定義為FAST特征點的方向:

θ=arctan(m01,m10)

(4)

為了提高方法的旋轉不變性,需要確保x和y在半徑為r的圓形區(qū)域內(nèi),即x,y∈[-r,r],r為鄰域半徑。在提取有向FAST關鍵點后,計算每個點的描述符。ORB選擇BRIEF作為特征描述方法,BRIEF采用隨機選取點的方法,選擇特征點周圍S×S大小的像素塊,隨機選取n對像素點,定義如下:

(5)

其中,p(x)是點x處的灰度值,則特征點p的描述子定義為:

(6)

2.3 基于深度學習的目標檢測算法

隨著深度學習技術的快速發(fā)展,目標檢測從基于手工特征的傳統(tǒng)算法轉化為基于深度神經(jīng)網(wǎng)絡的檢測技術。本文采用由殘差塊構成的全卷積網(wǎng)絡作為網(wǎng)絡主體,YOLOv3中將其命名為Darknet-53網(wǎng)絡,結構如圖1所示,在不同尺度上進行特征提取獲得最終的目標檢測結果。如圖3所示,定義檢測框集合為R={r1,r2,…},單個檢測框定義為r(x,y,w,h)∈R,其中,(x,y)為檢測框左上角在幀中的坐標,(w,h)為檢測框的寬和高。對特征點集合U中的每個特征點p(u,v)進行如下判斷:

圖3 特征點分類示意圖Fig.3 Schematic diagram of feature point classification

(7)

2.4 特征點匹配

圖4 特征點匹配算法框架Fig.4 Framework of feature point matching algorithm

特征點匹配算法具體分為以下4個步驟:

1)根據(jù)特征點的描述子對U中的特征點進行粗匹配,比較特征點描述向量之間的Hamming距離,距離越小表明2個特征點之間相似度越高,若Hamming距離小于一定的閾值,則表示2個特征點匹配成功。記匹配點對集合為Um:

(8)

(9)

(10)

若更新了模型M1,則M3被刪除,模型M1、M2變成新的M2和M3。對M2和M3的更新策略可依此類推。

3)在順序抽取完匹配點對中的所有點對并更新η值后,根據(jù)各個點對的η值進行重新排序,重復執(zhí)行第2步的操作,直到在某次操作后對匹配點對重排序時并未改變匹配點對的順序。分別計算M1、M2和M3相對應的運動模型,如下:

f(Mk)=(qk,tk),k={1,2,3}

(11)

其中,qk、tk為模型Mk對應的旋轉四元數(shù)和平移距離,定義:

(12)

其中,q、t為上一幀的旋轉四元數(shù)和平移距離,λ1、λ2為常數(shù)。選擇diff值最小的模型作為最優(yōu)模型M的輸出,對應的匹配點對集合作為I的輸出。

4)根據(jù)最優(yōu)模型M,計算出相機運動的本質(zhì)矩陣。設匹配點對{ic,ir}屬于集合I,其在當前幀中的特征點為ic,在參考幀中的匹配特征點為ir。ic和ir的歸一化坐標為:

ic=(ac,bc,1)

ir=(ar,br,1)

{ic,ir}∈S

(13)

根據(jù)對極約束:

(14)

求得本質(zhì)矩陣:

根據(jù)E可以計算出兩幀之間的旋轉矩陣R和平移向量t:

E=t＾R

(15)

其中,t＾是t的反對稱矩陣。

2.5 位姿跟蹤與建圖

對于集合U中的特征點篩選分為以下2個步驟:

1)如圖5所示,當前幀中提取出的潛在動態(tài)特征點pc(pc∈U),在參考幀中的對應匹配點為pr,根據(jù)上文計算出的本質(zhì)矩陣E,可以獲得空間點P(X,Y,Z)在參考幀中的投影點為p,p和pr的齊次坐標為:

圖5 特征點篩選示意圖Fig.5 Schematic diagram of feature point screening

p=(u,v,1)

pr=(ur,vr,1)

(16)

若滿足式(17)則保留特征點p并加入到集合S中;否則,丟棄特征點p。

(17)

其中,d為設置的距離閾值。

2)對集合S中的特征點進行跟蹤。

對于建圖模塊的操作分為以下2個步驟:

1)對跟蹤模塊輸入的關鍵幀中的特征點集合進行篩選,基于特征點集合L實現(xiàn)建圖:

(18)

2)插入該關鍵幀到地圖中,具體操作本文不做贅述。

圖6所示為本文方法和ORB_SLAM2系統(tǒng)的特征點提取效果對比,圖6(a)為輸入圖像,圖6(b)為ORB_SLAM2系統(tǒng)提取特征點后的圖像幀,圖6(c)為本文方法對特征點進行篩選后傳入跟蹤模塊的圖像幀。

圖6 本文方法和ORB_SLAM2系統(tǒng)在動態(tài)場景下的特征點提取效果Fig.6 Feature point extraction effect of the proposed method and ORB_SLAM2 system in dynamic scene

3 實驗結果與分析

3.1 KITTI數(shù)據(jù)集

在KITTI數(shù)據(jù)集中,一些移動的車輛和行人對跟蹤和定位精度產(chǎn)生影響,本文方法將環(huán)境中的車輛和行人作為動態(tài)對象進行檢測和處理。由于數(shù)據(jù)集中動態(tài)對象較少,跟蹤模塊性能提升不明顯,但是可以有效減小沒有閉環(huán)情況下的累積誤差。從圖7可以看出,在沒有閉環(huán)的情況下,因為本文方法在地圖中濾除了所有移動的物體,所以其軌跡誤差相對ORB_SLAM2系統(tǒng)明顯降低。

圖7 2種方法繪制路徑與實際路徑的對比情況Fig.7 Comparison of drawing path and actual path of two methods

3.2 TUM數(shù)據(jù)集

如圖8所示,在TUM數(shù)據(jù)集上,本文方法的性能明顯優(yōu)于ORB_SLAM2系統(tǒng)。ORB_SLAM2在序列walking_xyz和walking_halfsphere上的軌跡跟蹤誤差較高,原因是該方法主要針對的是場景中的動態(tài)對象。

圖8 2種方法的絕對路徑誤差對比Fig.8 Comparison of absolute path errors of two methods

表1所示為TUM數(shù)據(jù)集中本文方法和ORB_SLAM2系統(tǒng)的絕對路徑誤差(ATE)對比結果,表2、表3所示分別為2種方法相對位姿誤差(RPE)在平移和旋轉方面的對比結果。其中,RMSE為均方根誤差,MEAN為平均誤差,STD為標準差,Improvement定義為:

(19)

其中,our為本文方法的運行結果,ori為ORB_SLAM2系統(tǒng)的運行結果。從表1～表3可以看出,相對ORB_SLAM2系統(tǒng),本文方法性能提升明顯。

表1 2種方法的絕對路徑誤差對比Table 1 Comparison of absolute path errors of two methods

表2 2種方法的相對位姿誤差對比(平移)Table 2 Comparison of relative pose errors of two methods(translation)

表3 2種方法的相對位姿誤差對比(旋轉)Table 3 Comparison of relative pose errors of two methods(rotation)

4 結束語

為了降低動態(tài)對象對跟蹤定位結果的影響,本文提出一種基于視覺特征的實時SLAM方法。該方法引入基于深度學習的目標檢測算法,將特征點進行分類處理,降低動態(tài)特征點對位姿跟蹤與建圖造成的誤差。實驗結果表明,與ORB_SLAM2系統(tǒng)相比,該方法的跟蹤性能得到明顯提升,其運行速度可以滿足實時性的要求,且在多數(shù)情況下能達到較高的精度。下一步將使用像素級語義分割算法處理動態(tài)對象區(qū)域,以在保證圖像實時性的前提下提高其處理效率。