動態場景下移動機器人視覺SLAM

仉新，鄭飂默，譚振華，張雷，李鎖

(1.中國科學院沈陽計算技術研究所有限公司，遼寧沈陽 110168；2.東北大學軟件學院，遼寧沈陽110169；3.沈陽理工大學機械工程學院，遼寧沈陽 110159；4.63867部隊，吉林白城 137000)

0 前言

同時定位與地圖構建(Simultaneous Localization and Mapping, SLAM) 是移動機器人在未知環境中，利用自身傳感器獲取環境信息，構建環境地圖，同時估計自身在地圖中的位姿，實現機器人自主定位及導航[1]。SLAM廣泛應用于自動駕駛、智能家居、工業自動化以及移動機器人等領域。視覺SLAM獲取場景信息豐富，成本低廉，成為機器人領域研究熱點問題[2]。

視覺SLAM分為特征點法和直接法。特征點法通過特征提取和匹配實現位姿估計，ORB-SLAM2是經典的基于特征點的VSLAM系統，適用于單目、雙目及多目等視覺傳感器[3]。直接法基于灰度不變假設，通過計算最小化灰度誤差實現位姿估計，它對曝光和相機內參非常敏感，光照變化場景下容易出現失效或退化問題。視覺SLAM大多基于外部靜態環境假設，然而實際環境包含車輛和行人等動態信息，嚴重影響移動機器人定位和建圖的準確性，動態場景下SLAM的準確性和魯棒性問題亟待解決[4]。

動態場景下SLAM的準確性和魯棒性問題受到研究者們廣泛關注。根據動態場景下SLAM系統的目標檢測方法分為兩類，分別為基于幾何信息的動態目標檢測和基于深度學習的動態目標檢測。基于幾何信息的動態目標檢測方法大多無法獲取場景語義信息，缺乏對場景的高層語義信息理解[5-7]。

隨著深度學習的發展，學者們將深度學習引入SLAM系統。基于深度學習的動態目標檢測方法中，ZHONG等[8]采用單幀多框檢測器(Single Shot Multibox Detector, SSD)檢測運動物體，剔除相關區域的全部特征。阮曉鋼等[9]采用Mask R-CNN網絡進行圖像語義分割，剔除場景動態特征，依靠靜態特征進行位姿估計，提升了SLAM系統的準確性[10]。YU等[11]采用光流法進行運動一致性檢測，通過SegNet (Segmentation Network)[12]網絡進行語義分割，實現位姿估計和地圖構建。BESCOS等[5]采用Mask R-CNN(Region-based Convolutional Neural Network)方法進行目標檢測，提升ORB-SLAM2系統定位和建圖的準確性，但實時性較差。相比Mask R-CNN網絡，目標檢測模型YOLO(You Look Only Once)[13]檢測速度提高8～30倍，有利于保障SLAM系統的實時性需求。

本文作者提出了融合目標檢測和點線特征的SLAM系統：首先，通過四叉樹算法均勻化特征分布，解決動態場景特征聚集問題；然后，采用融合語義信息的目標檢測網絡識別場景的動態特征，剔除動態物體對位姿估計的干擾；最后，結合點特征和線特征進行特征提取及閉環檢測，充分提取場景空間結構信息，構建無動態物體影響的環境地圖，實現動態場景下移動機器人定位及建圖。

1 基礎理論

1.1 ORB-SLAM2系統框架

ORB-SLAM2包含多線程結構，分別為特征跟蹤線程、閉環檢測及局部建圖線程。跟蹤線程提取圖像ORB特征，通過幀間特征匹配計算相機位姿。閉環檢測線程通過閉環約束實現全局圖優化，消除系統累計誤差。局部建圖線程包含局部BA(Bundle Adjustment)優化、關鍵幀管理以及地圖點過濾，通過BA可以實現對局部空間特征及相機位姿優化。

圖1 ORB-SLAM2系統框架Fig.1 The ORB-SLIM2 system framework

1.2 YOLO目標檢測

目標檢測可自主識別圖像中的各類物體，主流算法包含基于回歸的單階目標檢測和基于候選區域的雙階目標檢測。單階目標檢測通過網絡預測物體的位置和類別，如YOLO、SSD、Retina Net等，網絡訓練及識別速度較高，小目標物體檢測難，漏檢率較高，準確性一般；雙階目標檢測包含區域提取和類別預測兩部分，如R-CNN、SPP-Net、Fast R-CNN、Faster R-CNN、R-FCN以及 NAS-FPN等，準確度有所提升但訓練及識別速度較低[14]。

2016年REDMON等提出基于深度學習的目標檢測YOLO v1算法[13]，采用卷積神經網絡實現目標類別判定和邊界預測，REDMON等采用Darknet53作為特征提取網絡，將YOLO算法性能提升為YOLO v3。BOCHKOVSKIY等基于YOLO v3，2020年提出了YOLO v4[15]，包含主干、頸部和頭部網絡，采用CSPDarknet53作為特征提取網絡，引入空間金字塔池化。YOLO v4提升了模型精度，廣泛應用于目標檢測領域，但存在網絡結構復雜及計算量大等問題。

2 動態SLAM系統框架

2.1 四叉樹算法

以SLAM系統中廣泛采用的ORB(Oriented FAST and Rotated BRIEF)特征為例，ORB分為oFAST(Oriented FAST)特征點提取和rBRIEF(Rotated BRIEF)特征描述符計算。圖像外觀、視角、紋理變化及遮擋等動態場景下，提取的ORB特征通常比較集中，影響SLAM前端特征提取和位姿估計的準確性。四叉樹算法廣泛應用于圖像處理、空間數據索引等方向，文中選用四叉樹算法實現特征均勻分布。

四叉樹算法流程如下：

(1)初始化，頂點分別是TL、TR、BL和BR，將當前節點特征劃分至n1、n2、n3、n4區域；

(2)判斷節點內特征數量是否等于設定的數量或者達到設定的深度，是則不再生成新的子節點，否則繼續劃分，執行步驟(1)；

(3)計算節點Harris響應值，保留響應值最大的特征。

圖2為傳統ORB特征提取，圖3為基于四叉樹的特征提取。如圖2和圖3所示：四叉樹算法均勻化了特征分布，解決了動態場景特征聚集問題，為SLAM前端運動估計提供穩定的特征。

圖2 ORB特征提取Fig.2 Feature extraction of ORB

圖3 基于四叉樹的特征提取Fig.3 Feature extraction based on quadtree

2.2 PL-SLAM系統

傳統點特征方法難以在動態場景下追蹤充足的特征，依賴于場景中的紋理信息，容易造成初始化失敗以及相機跟蹤失敗問題，影響SLAM系統定位的準確性。線特征可以表示圖像邊緣信息，體現了結構化空間幾何約束關系，提高數據關聯的準確性。本文作者采用融合點特征和線特征的PL-SLAM(Points and Line segments SLAM, PL-SLAM)系統，最小化點特征和線特征約束函數，實現相機位姿估計。

(1)點特征重投影誤差

(1)

點特征約束函數為

fp(x,u)=‖π(Tcw·x)-u‖Σp

(2)

式中：‖x‖Σ=xTΣ-1x表示馬氏距離；Σ為相應的協方差矩陣，可以體現數據間的相似度。

(2)線特征重投影誤差

假設空間直線Lw由世界坐標系轉換成相機坐標系直線Lc，直線Lc的Plucker坐標表示為

(3)

(4)

其中：(fu,fv,cu,cv)為相機內參，fu和fv分別表示相機在x軸及y軸焦距，cu和cv分別表示相機的光圈中心；K′表示直線投影矩陣。

空間直線Lw與相機位姿重投影誤差為

(5)

第k幀觀測第j條空間直線Lw,j重投影誤差為

(6)

因此，線特征的約束函數為

(7)

(3)PL-SLAM系統的重投影誤差模型

Huber核函數為

(8)

采用Huber函數降低代價函數異常項，當誤差e大于閾值δ時，目標函數化簡為一次函數，有效抑制誤匹配對，實現優化結果穩定性。最小化目標函數(9)，求解相機位姿，實現SLAM系統位姿估計。

(9)

2.3 融合目標檢測的動態SLAM系統

本文作者在ORB-SLAM2系統基礎上增加了四部分內容，分別為YOLO v4目標檢測模塊、線特征提取與匹配跟蹤及閉環檢測以及地圖構建模塊。圖4為融合目標檢測的動態SLAM系統。

圖4 融合目標檢測的動態SLAM系統Fig.4 Dynamic SLAM system with fused target detection

YOLO v4采用CSPDarknet53作為特征提取網絡，存在網絡結構復雜及計算量大等問題。考慮到SLAM系統實時性需求，文中引入深度可分離卷積對主干網絡進行改進，深度可分離卷積包含逐點和逐通道卷積，有效減小計算量，提升目標檢測效率。

(1)目標檢測模塊：通過YOLO v4網絡檢測場景中動態目標，剔除場景動態信息，將結果輸入跟蹤以及地圖構建模塊。

(2)特征跟蹤模塊：結合ORB點特征和LSD線特征，建立點特征和線特征的重投影誤差模型，通過非線性優化估計相機位姿。

(3)閉環檢測模塊：構建基于ORB點特征和LSD線特征的視覺詞袋模型，度量圖像相似性。

(4)地圖構建模塊：剔除場景動態信息，構建關鍵幀，構建外部環境靜態點云地圖。

3 實驗結果與分析

文中選用的實驗平臺為履帶式機器人和Kinect v1相機傳感器。運行環境為64位Ubuntu16.04系統、處理器為i7-8750H、主頻2.2 GHz和16 GB內存的便攜式計算機，不使用GPU加速。

3.1 相機標定

傳統相機標定法準確性高，但標定過程復雜，所需成本高。張正友標定法利用棋盤格模型進行相機標定，通過拍攝不同視角下的棋盤格，提取圖像特征，計算單應性矩陣得出相機內參。具有成本低、準確性高和易于操作等優點，被廣泛應用于相機標定。張正友標定法步驟如下：首先，將Kinect v1相機位置固定；其次，打印8×6棋盤格，將其固定于標定板；最后，調整標定板的角度和距離，獲取多組彩色圖像與深度圖像。標定結果如表1所示。

表1 Kinect v1標定參數Tab.1 Calibration parameters of Kinect v1

3.2 動態物體檢測

實驗數據集選擇COCO數據集，包含了行人等動態目標，如圖5所示。數據集分為訓練集、驗證集和測試集。原始網絡在檢測動態物體的同時，會檢測出圖5所示的靜態物體。通過對YOLO v4網絡進行訓練，如圖6所示，可以有效檢測出場景中的動態物體，準確識別出場景中動態物體信息。

圖5 動態目標檢測結果(訓練前)

圖6 動態目標檢測結果(訓練后)

3.3 動態場景下特征提取及匹配

SLAM前端加入優化后的YOLO v4網絡，實現動態場景下特征的有效識別，通過傳感器獲取彩色圖像和深度圖像，一分支采用優化后的YOLO v4網絡實現動態目標檢測，同時對動態物體進行掩碼處理，二分支對圖像進行點特征和線特征提取。

對圖像提取ORB點特征和LSD線特征，采用隨機采樣一致性算法RANSAC(Random Sample Consensus)剔除誤匹配，點特征和線特征匹配結果分別如圖7和圖8所示?？梢钥闯鲇行蕹藙討B物體的干擾，去除了行人的特征，實現靜態特征提取和匹配，避免動態場景對位姿估計的影響。

圖7 特征點匹配Fig.7 Feature point matching

圖8 特征線匹配Fig.8 Feature line matching

3.4 SLAM系統實驗

采用SLAM常用TUM數據集驗證文中方法(IPL-SLAM)的有效性。TUM數據集為慕尼黑工業大學通過Kinect傳感器采集的RGB-D數據集，包含彩色和深度圖像，分為低動態sitting序列和高動態walking序列，sitting序列包含肢體或小物體的輕微移動，walking序列包含行走及大范圍運動[16]。數據集參數如表2所示。

表2 TUM數據集參數Tab.2 Parameters of TUM dataset

與經典的ORB-SLAM2系統進行對比，采用絕對軌跡誤差(Absolute Trajectory Error, ATE)和相對位姿誤差(Relative Pose Error, RPE)評估SLAM系統的準確性。ATE表示真實軌跡坐標與估計坐標之間的誤差，其均方根誤差為

(10)

RPE表示真實位姿變換與估計位姿變換之前的誤差，其均方根誤差為

(11)

圖9為針對TUM數據集的低動態場景及高動態場景下，IPL-SLAM與經典ORB-SLAM2絕對軌跡誤差對比。TUM數據集提供了真實軌跡(Ground Truth)，用于評估SLAM系統定位準確性。

圖9 TUM數據集下ORB-SLAM2和IPL-SLAM絕對軌跡誤差對比

如圖9(a)和圖9(b)所示：在低動態場景下IPL-SLAM與ORB-SLAM2位姿估計結果與真實值均較為接近，定位準確性較高。如圖9(c)和圖9(d)所示：在高動態場景下，ORB-SLAM2位姿估計結果與真實值軌跡誤差較大，ORB-SLAM2系統受動態場景影響；IPL-SLAM仍然保持著較高的定位準確率，是由于文中采用YOLO v4剔除了場景中的動態特征，采用穩定的ORB特征點和LSD線特征共同進行SLAM前端及閉環檢測模塊，點特征和線特征相互輔助，實現了動態場景下SLAM系統的準確定位。表3為ORB-SLAM2和IPL-SLAM在數據集上的ATE和RPE均方根誤差對比，結果表明IPL-SLAM在低動態及高動態場景均保持較高的定位準確性。

表3 ATE和RPE匯總 單位：m

3.5 真實場景實驗

實驗平臺為搭載Kinect傳感器的履帶式機器人，如圖10所示，處理器i7-8750H，內存16 GB，系統Ubuntu16.04。控制機器人在學校走廊內運動采集環境信息，光線較差的走廊內有行人隨意行走，場景相似且紋理單一。

利用文中IPL-SLAM方法得到走廊點云地圖如圖11所示。移動機器人運動軌跡與實際機器人運動軌跡基本一致，紋理單一的走廊場景下，建立的場景環境地圖不包含動態點無重影，剔除了動態行人特征，且與走廊實際情況相一致，表明文中方法在動態場景下魯棒性較好。

圖10 履帶式機器人 圖11 點云地圖

4 結論

為提升動態場景下移動機器人SLAM的準確性和魯棒性，提出了融合目標檢測的IPL-SLAM方法。采用YOLO v4提取檢測場景動態信息，剔除動態特征；利用四叉樹算法實現特征均勻分布；在經典的ORB-SLAM2系統基礎上，結合圖像靜態特征點和特征線進行移動機器人位姿估計，建立無動態特征的環境地圖，適用于結構化及低紋理等復雜場景。實驗結果表明：相比ORB-SLAM2系統，改進方法解決了動態場景下SLAM系統位姿估計準確性低和魯棒性差的問題，且滿足SLAM系統的實時性需求。