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

王曉超 王春林* 袁成祥

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

0 引 言

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

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

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

1 系統構成

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

圖1 基于YOLOv4改進的SLAM系統框架

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

2.1 YOLOv4

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

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

2.2 跟蹤線程

2.2.1特征提取與特征匹配

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

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

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

(1)

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

(2)

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

θ=arctan(m01,m10)

(3)

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

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

(4)

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

(5)

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

2.2.2YOLOv4去除動態特征

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

(a) 傳統特征提取 (b) YOLOv4改進后特征提取圖2 特征提取結果

(a) 傳統ORB-SLAM2特征匹配結果

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

2.2.3位姿估計

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

(6)

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

(7)

定義一個誤差項ei為:

(8)

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

(9)

(10)

(11)

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

(12)

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

(13)

旋轉矩陣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建圖線程后添加一個構建帶有語義信息的點云地圖線程。以ORB-SLAM2的點云地圖為輸入,采用基于超體素聚類(Supervoxel Clustering)的算法對點云地圖進行初步分割,再與YOLOv4獲得的語義標簽融合構建最終的語義地圖。

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

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

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

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

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

(18)

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

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

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

2.3.2基于YOLOv4獲取語義標簽

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

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

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

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

(19)

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

(20)

3 實驗與分析

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

3.1 數據集

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

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

3.2 實驗結果

3.2.1定量結果

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

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

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

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

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

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

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

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

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

3.2.2語義地圖構建

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

(a) 傳統SLAM系統點云

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

3.2.3實時性分析

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

表4 系統實時性比較

4 結 語

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