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

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

0 前言

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

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

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

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

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

1 基礎(chǔ)理論

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

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

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

1.2 YOLO目標(biāo)檢測(cè)

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

2016年REDMON等提出基于深度學(xué)習(xí)的目標(biāo)檢測(cè)YOLO v1算法[13]，采用卷積神經(jīng)網(wǎng)絡(luò)實(shí)現(xiàn)目標(biāo)類別判定和邊界預(yù)測(cè)，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)檢測(cè)領(lǐng)域，但存在網(wǎng)絡(luò)結(jié)構(gòu)復(fù)雜及計(jì)算量大等問(wèn)題。

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

2.1 四叉樹(shù)算法

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

四叉樹(shù)算法流程如下：

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

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

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

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

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

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

2.2 PL-SLAM系統(tǒng)

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

(1)點(diǎn)特征重投影誤差

(1)

點(diǎn)特征約束函數(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幀觀測(cè)第j條空間直線Lw,j重投影誤差為

(6)

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

(7)

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

Huber核函數(shù)為

(8)

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

(9)

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

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

圖4 融合目標(biāo)檢測(cè)的動(dòng)態(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ù)雜及計(jì)算量大等問(wèn)題?？紤]到SLAM系統(tǒng)實(shí)時(shí)性需求，文中引入深度可分離卷積對(duì)主干網(wǎng)絡(luò)進(jìn)行改進(jìn)，深度可分離卷積包含逐點(diǎn)和逐通道卷積，有效減小計(jì)算量，提升目標(biāo)檢測(cè)效率。

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

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

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

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

3 實(shí)驗(yàn)結(jié)果與分析

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

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

傳統(tǒng)相機(jī)標(biāo)定法準(zhǔn)確性高，但標(biāo)定過(guò)程復(fù)雜，所需成本高。張正友標(biāo)定法利用棋盤(pán)格模型進(jìn)行相機(jī)標(biāo)定，通過(guò)拍攝不同視角下的棋盤(pán)格，提取圖像特征，計(jì)算單應(yīng)性矩陣得出相機(jī)內(nèi)參。具有成本低、準(zhǔn)確性高和易于操作等優(yōu)點(diǎn)，被廣泛應(yīng)用于相機(jī)標(biāo)定。張正友標(biāo)定法步驟如下：首先，將Kinect v1相機(jī)位置固定；其次，打印8×6棋盤(pán)格，將其固定于標(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 動(dòng)態(tài)物體檢測(cè)

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

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

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

3.3 動(dòng)態(tài)場(chǎng)景下特征提取及匹配

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

對(duì)圖像提取ORB點(diǎn)特征和LSD線特征，采用隨機(jī)采樣一致性算法RANSAC(Random Sample Consensus)剔除誤匹配，點(diǎn)特征和線特征匹配結(jié)果分別如圖7和圖8所示。可以看出有效剔除了動(dòng)態(tài)物體的干擾，去除了行人的特征，實(shí)現(xiàn)靜態(tài)特征提取和匹配，避免動(dòng)態(tài)場(chǎng)景對(duì)位姿估計(jì)的影響。

圖7 特征點(diǎn)匹配Fig.7 Feature point matching

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

3.4 SLAM系統(tǒng)實(shí)驗(yàn)

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

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

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

(10)

RPE表示真實(shí)位姿變換與估計(jì)位姿變換之前的誤差，其均方根誤差為

(11)

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

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

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

表3 ATE和RPE匯總 單位：m

3.5 真實(shí)場(chǎng)景實(shí)驗(yàn)

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

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

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

4 結(jié)論

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