動態(tài)場景下移動機(jī)器人視覺SLAM

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

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

0 前言

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

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

動態(tài)場景下SLAM的準(zhǔn)確性和魯棒性問題受到研究者們廣泛關(guān)注。根據(jù)動態(tài)場景下SLAM系統(tǒng)的目標(biāo)檢測方法分為兩類，分別為基于幾何信息的動態(tài)目標(biāo)檢測和基于深度學(xué)習(xí)的動態(tài)目標(biāo)檢測。基于幾何信息的動態(tài)目標(biāo)檢測方法大多無法獲取場景語義信息，缺乏對場景的高層語義信息理解[5-7]。

隨著深度學(xué)習(xí)的發(fā)展，學(xué)者們將深度學(xué)習(xí)引入SLAM系統(tǒng)?；谏疃葘W(xué)習(xí)的動態(tài)目標(biāo)檢測方法中，ZHONG等[8]采用單幀多框檢測器(Single Shot Multibox Detector, SSD)檢測運(yùn)動物體，剔除相關(guān)區(qū)域的全部特征。阮曉鋼等[9]采用Mask R-CNN網(wǎng)絡(luò)進(jìn)行圖像語義分割，剔除場景動態(tài)特征，依靠靜態(tài)特征進(jìn)行位姿估計，提升了SLAM系統(tǒng)的準(zhǔn)確性[10]。YU等[11]采用光流法進(jìn)行運(yùn)動一致性檢測，通過SegNet (Segmentation Network)[12]網(wǎng)絡(luò)進(jìn)行語義分割，實現(xiàn)位姿估計和地圖構(gòu)建。BESCOS等[5]采用Mask R-CNN(Region-based Convolutional Neural Network)方法進(jìn)行目標(biāo)檢測，提升ORB-SLAM2系統(tǒng)定位和建圖的準(zhǔn)確性，但實時性較差。相比Mask R-CNN網(wǎng)絡(luò)，目標(biāo)檢測模型YOLO(You Look Only Once)[13]檢測速度提高8～30倍，有利于保障SLAM系統(tǒng)的實時性需求。

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

1 基礎(chǔ)理論

1.1 ORB-SLAM2系統(tǒng)框架

ORB-SLAM2包含多線程結(jié)構(gòu)，分別為特征跟蹤線程、閉環(huán)檢測及局部建圖線程。跟蹤線程提取圖像ORB特征，通過幀間特征匹配計算相機(jī)位姿。閉環(huán)檢測線程通過閉環(huán)約束實現(xiàn)全局圖優(yōu)化，消除系統(tǒng)累計誤差。局部建圖線程包含局部BA(Bundle Adjustment)優(yōu)化、關(guān)鍵幀管理以及地圖點過濾，通過BA可以實現(xiàn)對局部空間特征及相機(jī)位姿優(yōu)化。

圖1 ORB-SLAM2系統(tǒng)框架Fig.1 The ORB-SLIM2 system framework

1.2 YOLO目標(biāo)檢測

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

2016年REDMON等提出基于深度學(xué)習(xí)的目標(biāo)檢測YOLO v1算法[13]，采用卷積神經(jīng)網(wǎng)絡(luò)實現(xiàn)目標(biāo)類別判定和邊界預(yù)測，REDMON等采用Darknet53作為特征提取網(wǎng)絡(luò)，將YOLO算法性能提升為YOLO v3。BOCHKOVSKIY等基于YOLO v3，2020年提出了YOLO v4[15]，包含主干、頸部和頭部網(wǎng)絡(luò)，采用CSPDarknet53作為特征提取網(wǎng)絡(luò)，引入空間金字塔池化。YOLO v4提升了模型精度，廣泛應(yīng)用于目標(biāo)檢測領(lǐng)域，但存在網(wǎng)絡(luò)結(jié)構(gòu)復(fù)雜及計算量大等問題。

2 動態(tài)SLAM系統(tǒng)框架

2.1 四叉樹算法

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

四叉樹算法流程如下：

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

(2)判斷節(jié)點內(nèi)特征數(shù)量是否等于設(shè)定的數(shù)量或者達(dá)到設(shè)定的深度，是則不再生成新的子節(jié)點，否則繼續(xù)劃分，執(zhí)行步驟(1)；

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

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

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

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

2.2 PL-SLAM系統(tǒng)

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

(1)點特征重投影誤差

(1)

點特征約束函數(shù)為

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

(2)

式中：‖x‖Σ=xTΣ-1x表示馬氏距離；Σ為相應(yīng)的協(xié)方差矩陣，可以體現(xiàn)數(shù)據(jù)間的相似度。

(2)線特征重投影誤差

假設(shè)空間直線Lw由世界坐標(biāo)系轉(zhuǎn)換成相機(jī)坐標(biāo)系直線Lc，直線Lc的Plucker坐標(biāo)表示為

(3)

(4)

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

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

(5)

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

(6)

因此，線特征的約束函數(shù)為

(7)

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

Huber核函數(shù)為

(8)

采用Huber函數(shù)降低代價函數(shù)異常項，當(dāng)誤差e大于閾值δ時，目標(biāo)函數(shù)化簡為一次函數(shù)，有效抑制誤匹配對，實現(xiàn)優(yōu)化結(jié)果穩(wěn)定性。最小化目標(biāo)函數(shù)(9)，求解相機(jī)位姿，實現(xiàn)SLAM系統(tǒng)位姿估計。

(9)

2.3 融合目標(biāo)檢測的動態(tài)SLAM系統(tǒng)

本文作者在ORB-SLAM2系統(tǒng)基礎(chǔ)上增加了四部分內(nèi)容，分別為YOLO v4目標(biāo)檢測模塊、線特征提取與匹配跟蹤及閉環(huán)檢測以及地圖構(gòu)建模塊。圖4為融合目標(biāo)檢測的動態(tài)SLAM系統(tǒng)。

圖4 融合目標(biāo)檢測的動態(tài)SLAM系統(tǒng)Fig.4 Dynamic SLAM system with fused target detection

YOLO v4采用CSPDarknet53作為特征提取網(wǎng)絡(luò)，存在網(wǎng)絡(luò)結(jié)構(gòu)復(fù)雜及計算量大等問題?？紤]到SLAM系統(tǒng)實時性需求，文中引入深度可分離卷積對主干網(wǎng)絡(luò)進(jìn)行改進(jìn)，深度可分離卷積包含逐點和逐通道卷積，有效減小計算量，提升目標(biāo)檢測效率。

(1)目標(biāo)檢測模塊：通過YOLO v4網(wǎng)絡(luò)檢測場景中動態(tài)目標(biāo)，剔除場景動態(tài)信息，將結(jié)果輸入跟蹤以及地圖構(gòu)建模塊。

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

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

(4)地圖構(gòu)建模塊：剔除場景動態(tài)信息，構(gòu)建關(guān)鍵幀，構(gòu)建外部環(huán)境靜態(tài)點云地圖。

3 實驗結(jié)果與分析

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

3.1 相機(jī)標(biāo)定

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

表1 Kinect v1標(biāo)定參數(shù)Tab.1 Calibration parameters of Kinect v1

3.2 動態(tài)物體檢測

實驗數(shù)據(jù)集選擇COCO數(shù)據(jù)集，包含了行人等動態(tài)目標(biāo)，如圖5所示。數(shù)據(jù)集分為訓(xùn)練集、驗證集和測試集。原始網(wǎng)絡(luò)在檢測動態(tài)物體的同時，會檢測出圖5所示的靜態(tài)物體。通過對YOLO v4網(wǎng)絡(luò)進(jìn)行訓(xùn)練，如圖6所示，可以有效檢測出場景中的動態(tài)物體，準(zhǔn)確識別出場景中動態(tài)物體信息。

圖5 動態(tài)目標(biāo)檢測結(jié)果(訓(xùn)練前)

圖6 動態(tài)目標(biāo)檢測結(jié)果(訓(xùn)練后)

3.3 動態(tài)場景下特征提取及匹配

SLAM前端加入優(yōu)化后的YOLO v4網(wǎng)絡(luò)，實現(xiàn)動態(tài)場景下特征的有效識別，通過傳感器獲取彩色圖像和深度圖像，一分支采用優(yōu)化后的YOLO v4網(wǎng)絡(luò)實現(xiàn)動態(tài)目標(biāo)檢測，同時對動態(tài)物體進(jìn)行掩碼處理，二分支對圖像進(jìn)行點特征和線特征提取。

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

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

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

3.4 SLAM系統(tǒng)實驗

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

表2 TUM數(shù)據(jù)集參數(shù)Tab.2 Parameters of TUM dataset

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

(10)

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

(11)

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

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

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

表3 ATE和RPE匯總 單位：m

3.5 真實場景實驗

實驗平臺為搭載Kinect傳感器的履帶式機(jī)器人，如圖10所示，處理器i7-8750H，內(nèi)存16 GB，系統(tǒng)Ubuntu16.04?？刂茩C(jī)器人在學(xué)校走廊內(nèi)運(yùn)動采集環(huán)境信息，光線較差的走廊內(nèi)有行人隨意行走，場景相似且紋理單一。

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

圖10 履帶式機(jī)器人 圖11 點云地圖

4 結(jié)論

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