基于目標檢測的視覺SLAM改進方法

王曉超 王春林* 袁成祥

1(杭州電子科技大學自動化(人工智能)學院 浙江 杭州 310018) 2(浙江工商大學計算機與信息工程學院 浙江 杭州 310018)

0 引 言

同時定位與地圖構(gòu)建(SLAM)技術(shù)是機器人通過傳感器對未知環(huán)境構(gòu)建地圖的同時實現(xiàn)自定位的過程。大多數(shù)視覺SLAM系統(tǒng)是將環(huán)境假設為靜態(tài)場景,而在實際場景中往往會有像人類這樣的活動對象,這些動態(tài)特征被提取后會嚴重影響相機的位姿估計,造成軌跡漂移嚴重,甚至導致系統(tǒng)崩潰。另外,這些SLAM系統(tǒng)主要構(gòu)建的是幾何信息地圖,缺少對物體具體語義層次的理解,不能提供帶語義信息的地圖,制約了移動機器人交互能力和導航能力。

近年來,深度學習在語義信息獲取方面的進展及應用為解決這些問題提供了一個可行的方向。深度學習在圖像分類[1]、識別、圖像分割[2]等幾大領域的表現(xiàn)都遠遠高于傳統(tǒng)人工設計的算法。深度學習與SLAM結(jié)合可以使機器人從幾何和語義兩個層次對場景進行抽象理解,獲得高層次的感知,提高機器人對周圍環(huán)境的理解。Yu等[3]提出了DS-SLAM系統(tǒng),該系統(tǒng)是在ORB-SLAM2[4]的跟蹤線程中加入語義分割SegNet網(wǎng)絡[5],去除每一幀圖像中的動態(tài)特征,從而降低位姿估計的誤差。Bescos等[6]提出DynaSLAM系統(tǒng),采用實例分割網(wǎng)絡MASK-RCNN[7]對當前幀中的動態(tài)物體進行分割,并根據(jù)前20個關(guān)鍵幀對去除的地方進行背景修復。由于MASK-RCNN復雜的網(wǎng)絡結(jié)構(gòu),DynaSLAM系統(tǒng)實時性較差。Sünderhauf等[8]同樣以ORB-SLAM2系統(tǒng)為基礎,在系統(tǒng)中加入SSD目標檢測算法[9]對構(gòu)建的三維點云地圖物體識別和分割,最終構(gòu)建了帶有語義信息的三維點云語義地圖。Mccormac等[10]提出SemanticFusion系統(tǒng),該系統(tǒng)使用卷積神經(jīng)網(wǎng)絡(CNN)進行語義分割,再與ElasticFusion系統(tǒng)結(jié)合,構(gòu)建了稠密的三維語義地圖。

本文針對機器人視覺SLAM系統(tǒng)中存在的對動態(tài)物體難以處理和無法構(gòu)建帶有語義信息地圖的問題,以ORB-SLAM2為基礎,采用基于深度學習的目標檢測算法YOLOv4對系統(tǒng)進行改進,減少系統(tǒng)位姿估計誤差,提高系統(tǒng)的魯棒性,并添加一個點云語義地圖構(gòu)建的線程,構(gòu)建稠密的三維語義地圖,以提高視覺SLAM系統(tǒng)的感知能力。

1 系統(tǒng)構(gòu)成

本文提出的基于目標檢測的視覺SLAM系統(tǒng)改進方法的框架如圖1所示。本文采用YOLOv4對傳統(tǒng)ORB-SLAM2系統(tǒng)進行部分改進。在跟蹤線程中,采用ORB(Oriented FAST and Rotated BRIEF)算法提取出圖像幀中的特征點,然后判斷這些特征點是否在由YOLOv4定位出的動態(tài)物體上,如果是,則去除這些特征點,避免這些動態(tài)特征點對系統(tǒng)的干擾,以提高系統(tǒng)位姿估計的準確率。在建圖線程中,構(gòu)建3D點云地圖,再對點云地圖采用基于圖結(jié)構(gòu)的超體素聚類算法生成初步語義地圖,與YOLOv4提供的語義標簽構(gòu)建最后的語義地圖。

圖1 基于YOLOv4改進的SLAM系統(tǒng)框架

2 基于YOLOv4的視覺SLAM改進方法

2.1 YOLOv4

YOLOv4[11]在以殘差塊結(jié)構(gòu)的Darknet-53為骨干網(wǎng)絡的YOLOv3基礎上作出了全面的提升,在骨干網(wǎng)絡Darknet-53上加入CSPNet(Cross Stage Partial Network)網(wǎng)絡結(jié)構(gòu),減少計算量的同時提高了推理速度和準確性。另外,骨干網(wǎng)絡還加入了SPP(Spatial Pyramid Pooling)模塊,可以提升模型的感受野,分離更重要的上下文信息。與此同時,還采用PANet(Path Aggregation Network)改進骨干網(wǎng)絡結(jié)構(gòu),加強了特征金字塔的結(jié)構(gòu),縮短了高低層特征融合的路徑。

本文利用MS COCO數(shù)據(jù)集來訓練YOLOv4的網(wǎng)絡模型,數(shù)據(jù)集里包含人、茶杯、鍵盤、顯示器、鼠標、玩具熊等80個類別。由于網(wǎng)絡結(jié)構(gòu)的優(yōu)勢,YOLOv4可以在MS COCO數(shù)據(jù)集上AP(Average Precision)為43.5%的同時可以達到65幀/s,是目前最新的快速而高效的目標檢測器。

2.2 跟蹤線程

2.2.1特征提取與特征匹配

ORB[12]是一種快速特征點提取和特征描述的算法,由關(guān)鍵點和描述子兩部分組成,ORB特征提取主要分為以下兩個步驟:(1) 方向FAST特征點檢測;(2) BRIEF特征描述。

FAST是一種角點,主要是比較局部像素灰度與周圍像素點是否變化比較明顯,如果該像素點與周圍的像素點差別較大,那么它可能是角點。為解決傳統(tǒng)的FAST角點不具有尺度和方向性的問題,ORB在原來的基礎上添加了尺度和旋轉(zhuǎn)的描述。為解決尺度不變性,采用構(gòu)建圖像金字塔方式,在每一層金字塔上檢測角點。而特征的旋轉(zhuǎn)則是用灰度質(zhì)心法來實現(xiàn)。

所謂灰度質(zhì)心法指的是將圖像的灰度值作為權(quán)重的中心。例如在一小的圖像塊A中,定義圖像塊的矩為:

(1)

式中:I(x,y)為點(x,y)處的灰度值。得到圖像的質(zhì)心為:

(2)

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

θ=arctan(m01,m10)

(3)

通過以上方法,FAST角點便有了尺度和旋轉(zhuǎn)的描述,提升了特征點的魯棒性,為后續(xù)特征匹配提供了方便。

ORB的特征描述則是選擇BRIEF算法,該算法采用隨機選取點的方式,選擇特征點周圍S×S大小的像素塊,隨機選取n對像素點,定義:

(4)

式中:p(x)是點x處的灰度值;p(y)是點y處的灰度值。則特征點p的描述子定義為:

(5)

特征點提取后,接下來就是特征匹配。特征匹配是SLAM系統(tǒng)中的重要部分,為保證位姿估計的精度,特征點間必須正確地、有效地相互匹配。以上的特征描述子均是二進制描述子,為后續(xù)計算特征相似度減少了計算壓力。這里比較特征相似度采用計算漢明距離的方式,漢明距離是指兩字符串之間對應位置字符不同的總數(shù)量。當兩特征描述子的漢明距離低于設定的閾值時,便認為這兩個特征是同一個點。

2.2.2YOLOv4去除動態(tài)特征

將當前圖像幀輸入YOLOv4網(wǎng)絡模型,經(jīng)過目標檢測算法定位出動態(tài)物體的位置,本文將人類視為動態(tài)物體類別,將人類用矩形框框出來,并將框內(nèi)的特征點視為動態(tài)特征點,全部去除,如圖2所示。當動態(tài)特征剔除后,進行特征匹配時,避免了很多在人身上的特征點的匹配,以提高后續(xù)位姿估計的精度,如圖3所示。

(a) 傳統(tǒng)特征提取 (b) YOLOv4改進后特征提取圖2 特征提取結(jié)果

(a) 傳統(tǒng)ORB-SLAM2特征匹配結(jié)果

(b) YOLOv4改進后特征匹配結(jié)果圖3 特征匹配結(jié)果

2.2.3位姿估計

將特征匹配好的2D點從其對應深度圖像中獲取該點的深度,轉(zhuǎn)換成3D點。采用ICP(Iterative Closest Point)[13]算法對配對好的3D點進行位姿估計。假設有兩組匹配好的3D點:

(6)

式中:P為當前幀特征點的集合;P′為前一幀特征點的集合。尋找一個歐氏變換R、t使得:

(7)

定義一個誤差項ei為:

(8)

然后,構(gòu)建最小二乘問題,求使誤差平方和達到極小值時的R和t:

(9)

(10)

(11)

將式(10)代入式(9)中,可得:

(12)

式(12)的第一項只與旋轉(zhuǎn)矩陣R相關(guān),第二項既有R也有t,將式(11)代入第一項可得:

(13)

旋轉(zhuǎn)矩陣R*具體計算方法是首先定義矩陣H:

(14)

H是一個3×3的矩陣,對H進行SVD分解:

H=UΣVT

(15)

式中:Σ為奇異值組成的對角矩陣,對角線元素從大到小排列;U和V為對角矩陣。當H滿秩時,R*為:

R*=VUT

(16)

利用式(16)求得R后再求解平移量t:

t=p-R*p′

(17)

2.3 建圖線程

本文在ORB-SLAM2建圖線程后添加一個構(gòu)建帶有語義信息的點云地圖線程。以ORB-SLAM2的點云地圖為輸入,采用基于超體素聚類(Supervoxel Clustering)的算法對點云地圖進行初步分割,再與YOLOv4獲得的語義標簽融合構(gòu)建最終的語義地圖。

2.3.1基于超體素聚類的物體分割算法

超體(Supervoxel)[14]是一種集合,集合的元素是“體”,其本質(zhì)是一個個小方塊。超體素聚類并不是分割出某個物體,而是對點云過分割(over segmentation),將場景點云化成許多的小方塊,根據(jù)顏色、法向量方向等進行局部分割。

超體素聚類算法是在空間中選擇一定數(shù)量的種子點作為超體的初始化。首先將空間分割成半徑為Rseed分辨率的體素網(wǎng)格,Rseed的大小要比各像素間的范圍Rvoxel大得多,然后將種子作為每個體素網(wǎng)格的中心。如圖4所示,其中Rsearch表示放置種子空間距離。

圖4 影響超體素聚類的不同半徑

從種子點開始通過計算特征空間如空間范圍、法向量、顏色等進行擴展,通過式(18)計算兩個素體之間的距離:

(18)

式中:Dc為RGB空間中的歸一化歐氏距離;Ds為歸一化的空間的歐氏距離;Dn為歸一化的法向量角度距離;wc、ws、wn分別為顏色、空間、法向量的權(quán)重。

超體是使用迭代的方式增長的。首先從種子最近鄰點開始,如果計算出體素是距離當前種子最近的點,則將該體素加入當前超體中,接著使用近鄰圖繼續(xù)將該體素所有近鄰點加入到搜索隊列中,然后再處理下個種子,一直迭代到超體邊界。這種方式能保證在處理過程中每個像素里中心點的層次水平是相同的。在超體搜索結(jié)束后,接著更新每個超體的中心為其組成成分的重心點,這樣經(jīng)過幾次迭代,直達超體中心穩(wěn)定停止。超體聚類效果如圖5所示。

圖5 超體素聚類后的點云分割地圖

2.3.2基于YOLOv4獲取語義標簽

以上將點云進行超體素聚類屬于幾何結(jié)構(gòu)的分割,雖有不錯的分割效果,但是不能得到物體的語義信息。為獲取圖像中的語義信息,本文采用YOLOv4進行目標檢測獲取圖像幀的語義標簽,目標檢測效果如圖6(a)所示。

(a) YOLOv4目標檢測 (b) 帶有語義標簽的點云圖6 獲取語義標簽

彩色圖片通過YOLOv4將檢測到的物體用不同顏色框出,并獲取框的左頂點位置(xi,yi)和框?qū)抴i、高hi,則類別ci在彩色圖片中的描述為:

Di={(xi,yi),wi,hi,ci}

(19)

在深度圖片中定位到對應位置,并將框內(nèi)的所有深度歸為此類并用某顏色標出,則加入深度信息di和標注顏色colori后,類別ci的描述為:

(20)

3 實驗與分析

本節(jié)中,我們使用開源數(shù)據(jù)集對本文構(gòu)建的系統(tǒng)進行評估,將本文系統(tǒng)、傳統(tǒng)ORB-SLAM2和基于SegNet改進的DS-SLAM三系統(tǒng)進行比較,分析系統(tǒng)在動態(tài)環(huán)境中的性能是否提高,并構(gòu)建帶有語義信息的稠密地圖。所有實驗均在處理器為Intel i5 8400、顯卡GTX1060 6 GB顯存和內(nèi)存16 GB的計算機上進行。

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

為評價系統(tǒng)的綜合能力,實驗采用的數(shù)據(jù)集是德國慕尼黑工業(yè)大學開源的TUM RGB-D數(shù)據(jù)集[15],該數(shù)據(jù)集由39個序列組成,這些序列是由Microsoft Kinect傳感器以30 Hz速率記錄在不同的室內(nèi)場景中,包含RGB圖片、深度圖片和地面實況數(shù)據(jù)。本文實驗主要采用TUM RGB-D數(shù)據(jù)集中的5個序列。其中walking系列的4個序列是兩人在桌前桌后行走或者坐在桌前的場景,都屬于高動態(tài)序列,相機在4種狀態(tài)下拍攝:(1) 沿x-y-z軸移動;(2) 保持靜止;(3) 在翻滾、仰俯和偏航軸上旋轉(zhuǎn);(4) 直徑為一米的半球面移動。freiburg2_xyz序列是相機沿著(x,y,z)方向緩慢移動,確保了數(shù)據(jù)足夠清晰。

此外,TUM RGB-D數(shù)據(jù)集還提供了用于系統(tǒng)評估的兩種方法:(1) 絕對軌跡誤差(Absolute Trajectory Error,ATE),代表運動軌跡的全局一致性;(2) 相對位姿誤差(Relative Pose Error,RPE),測量平移和旋轉(zhuǎn)漂移。

3.2 實驗結(jié)果

3.2.1定量結(jié)果

為對比本文系統(tǒng)、ORB-SLAM2和DS-SLAM在TUM RGB-D數(shù)據(jù)集中的4個高動態(tài)序列的實驗結(jié)果,評價指標為均方根誤差(RMSE)、平均誤差(MEAN)、標準偏差(S.D.)。均方根誤差計算估計值與真實值之間的偏差;平均誤差描述所有估計誤差的平均水平;標準偏差反映系統(tǒng)軌跡估計的離散程度。三種評價指標可以很好地體現(xiàn)SLAM系統(tǒng)的穩(wěn)定性和可靠性。

從表1-表3的比較結(jié)果可以看出,本文系統(tǒng)相比于傳統(tǒng)的ORB-SLAM2系統(tǒng)和DS-SLAM系統(tǒng)在三種評價誤差中均有不同程度的減少。圖7顯示了在freiburg3_walking_halfsphere序列中傳統(tǒng)ORB-SLAM2、DS-SLAM和本文的SLAM系統(tǒng)的ATE和RPE圖。可明顯看出本文系統(tǒng)的絕對軌跡誤差和相對位姿誤差均降低了很多。

表1 絕對路徑誤差(ATE)對比 單位:m

表2 相對位移誤差(RPE)對比 單位:m

表3 相對旋轉(zhuǎn)誤差(RPE)對比 單位:°

(a) ORB-SLAM2絕對軌跡誤差

(b) DS-SLAM絕對軌跡誤差

(c) 本文系統(tǒng)絕對軌跡誤差

(d) 相對位姿誤差對比圖7 絕對軌跡誤差和相對位姿誤差對比

3.2.2語義地圖構(gòu)建

本節(jié)實驗是構(gòu)建freiburg2_xyz序列數(shù)據(jù)集的稠密語義點云地圖,其效果如圖8所示。圖8(a)是傳統(tǒng)SLAM系統(tǒng)所構(gòu)建的點云地圖,雖然可以看出物體的輪廓,但是各物體間分層不夠清晰,并且地圖中只是三維點,并沒有物體的語義信息,這樣的地圖并不能定位到具體的某物體中。而圖8(b)是將帶語義標簽的點云地圖與超體素聚類的點云地圖相融合后的結(jié)果,可以看出實驗桌上物體分層清晰,由YOLOv4算法識別出的物體用不同顏色標出。本文構(gòu)建的帶有語義信息的點云地圖可以容易地定位到某物體的位置,提高SLAM系統(tǒng)的交互能力和感知能力。

(a) 傳統(tǒng)SLAM系統(tǒng)點云

(b) 本文系統(tǒng)的語義點云地圖圖8 傳統(tǒng)SLAM點云地圖和本文語義點云地圖比較

3.2.3實時性分析

如表4所示,本文系統(tǒng)對每幀圖像平均計算時間為68 ms,雖比DS-SLAM計算較慢些,但是系統(tǒng)的位姿估計精度比DS-SLAM系統(tǒng)高20%～40%。另外,本文系統(tǒng)每秒的計算幀數(shù)為15,基本滿足SLAM系統(tǒng)的實時性要求。

表4 系統(tǒng)實時性比較

4 結(jié) 語

為減少動態(tài)環(huán)境對SLAM系統(tǒng)的位姿和軌跡估計的影響和無法建立帶有語義信息的地圖,構(gòu)建一種基于目標檢測的視覺SLAM系統(tǒng)。在ORB-SLAM2的跟蹤線程中采用YOLOv4定位出動態(tài)物體,從而去除動態(tài)特征。與傳統(tǒng)的ORB-SLAM2相比,系統(tǒng)的性能有明顯的提升。在建圖線程中加入構(gòu)建稠密點云語義地圖線程,用超體素聚類對點云地圖初步分割,再與YOLOv4獲取的標簽融合得到最終的語義地圖,地圖構(gòu)建效果良好,且本文SLAM系統(tǒng)計算速度為15幀/s,能很好地滿足實時性的要求。在接下來的工作中,重點研究將深度學習運用到視覺SLAM的閉環(huán)檢測線程中,進一步提高系統(tǒng)的準確性。