房立金，王科棋

（東北大學(xué)機(jī)器人科學(xué)與工程學(xué)院，沈陽 110169）

0 概述

同步定位與建圖（Simultaneous Localization and Mapping，SLAM）是指在沒有周圍環(huán)境先驗(yàn)信息的條件下，僅從機(jī)載傳感器獲取周圍環(huán)境信息同時(shí)完成自身位姿估計(jì)和環(huán)境地圖構(gòu)建的工作。SLAM 技術(shù)在智能機(jī)器人實(shí)時(shí)定位、導(dǎo)航、自動駕駛、增強(qiáng)現(xiàn)實(shí)/虛擬現(xiàn)實(shí)等方面有著極其重要的應(yīng)用。而視覺傳感器具有成本低廉、提供數(shù)據(jù)信息豐富等特點(diǎn)，所以視覺SLAM 得到了研究人員的廣泛關(guān)注。很多團(tuán)隊(duì)已經(jīng)從不同的角度對視覺SLAM 問題進(jìn)行了討論，其中一些優(yōu)秀的成果極大地推進(jìn)了視覺SLAM的發(fā)展，例如LSD-SLAM［1］、DSΟ［2］、ΟRB-SLAM2［3］、VINS-Mono［4］等，但是為了簡化計(jì)算，這些算法都是在靜態(tài)環(huán)境假設(shè)基礎(chǔ)上進(jìn)行的。由于忽略了現(xiàn)實(shí)場景中運(yùn)動物體對視覺SLAM 算法所帶來的影響，這些模型很難適應(yīng)復(fù)雜多變的真實(shí)場景。因此，針對動態(tài)環(huán)境下的視覺SLAM 算法進(jìn)行研究就顯得尤為重要。現(xiàn)階段動態(tài)環(huán)境SLAM 算法主要通過處理動態(tài)物體來減少其對系統(tǒng)精度的影響。

運(yùn)動物體檢測方法可分為基于多視圖幾何的方法和基于深度學(xué)習(xí)的方法?；诙嘁晥D幾何方法一般通過光流、對極幾何約束、最小化重投影誤差等檢測運(yùn)動物體。CENG 等［5］采用光流方法計(jì)算圖像序列相鄰幀之間像素運(yùn)動。如果相機(jī)運(yùn)動與動態(tài)物體存在運(yùn)動一致性問題，則這種方法可能會失敗。SUN 等［6］使用深度圖提供的深度信息識別出明顯的運(yùn)動物體，此方法受限于傳感器檢測范圍和分辨率，無法適應(yīng)復(fù)雜環(huán)境。LI 等［7］利用深度圖進(jìn)行邊緣提取，根據(jù)具有相似運(yùn)動的點(diǎn)進(jìn)行分組和分配概率來識別動態(tài)對象，但是它們具有較高的計(jì)算成本，并且在嘈雜環(huán)境或退化運(yùn)動中無法正常地工作。浙江大學(xué)章國峰團(tuán)隊(duì)［8］提出的RDSLAM 算法［9］能夠在動態(tài)環(huán)境中穩(wěn)定運(yùn)行，該算法的主要貢獻(xiàn)是提出一種在線更新關(guān)鍵幀的方法，從而及時(shí)剔除環(huán)境中的運(yùn)動物體，同時(shí)通過改進(jìn)傳統(tǒng)隨機(jī)一致性采樣（Random Sample Consensus，RANSAC）算法［10］，提出一種自適應(yīng)RANSAC 算法，在剔除大量外點(diǎn)情況下保證算法魯棒性。

基于深度學(xué)習(xí)的方法在近年來開始興起，清華大 學(xué)CHAΟ 等［11］針對動 態(tài)環(huán)境提出DS-SLAM 算法，該算法基于ΟRB-SLAM2 框架加入實(shí)時(shí)語義分割線程，將語義分割與對極幾何約束方法相結(jié)合，過濾出場景中的動態(tài)特征。但是對極幾何計(jì)算的基礎(chǔ)矩陣容易受外點(diǎn)影響，從而影響算法的精度。BESCΟS 等［12］同樣基 于ΟRB-SLAM2 框架提出了DynaSLAM 算法，該算法融合語義分割和多視圖幾何的方法檢測場景中的運(yùn)動特征，但語義分割算法模型復(fù)雜度過高無法滿足實(shí)時(shí)性的需求。ZHANG等［13］基于深度學(xué)習(xí)目標(biāo)檢測算法提出動態(tài)場景下基于特征點(diǎn)法的視覺SLAM 算法，該算法能夠提高跟蹤精度和地圖適應(yīng)性。

MINA 等［14］提出一種基于特征的、無模型的、可感知對象的動態(tài)SLAM 算法Dynamic SLAM，其利用語義分割來估計(jì)場景中剛性對象的運(yùn)動，而無需估計(jì)對象的姿勢或具有任何先驗(yàn)知識的3D 模型。該算法生成動態(tài)和靜態(tài)結(jié)構(gòu)圖，并能夠提取場景中剛性移動物體的速度。BESCΟS 等［15］基于DynaSLAM 提出新一代視覺 SLAM 系統(tǒng)DynaSLAM II，其利用實(shí)例語義分割和ΟRB 特征來跟蹤動態(tài)對象，并將靜態(tài)場景和動態(tài)對象的結(jié)構(gòu)放在捆綁調(diào)整（Bundle Adjustment，BA）中與攝像機(jī)的運(yùn)行軌跡同時(shí)進(jìn)行優(yōu)化，并且三維運(yùn)動對象的3D 邊界框也可以在固定的時(shí)間窗口內(nèi)進(jìn)行估算和粗略優(yōu)化。ZHANG 等［16］基于機(jī)器學(xué)習(xí)及深度學(xué)習(xí)提出RGB-D SLAM 算法來適應(yīng)動態(tài)場景，提升視覺SLAM 在動態(tài)場景中的精度。但是，該算法占用計(jì)算機(jī)資源較大無法滿足實(shí)時(shí)性的需求。

綜上，現(xiàn)有動態(tài)環(huán)境SLAM 算法雖然能夠解決部分動態(tài)場景問題，但是現(xiàn)有算法仍然存在模型復(fù)雜度高、計(jì)算量過大、應(yīng)用范圍有限、無法保證實(shí)時(shí)性等問題，且只進(jìn)行一次運(yùn)動物體判定，一旦判定失效則無法保證視覺里程計(jì)的穩(wěn)定運(yùn)行，從而影響整個(gè)SLAM 算法的運(yùn)行精度。本文在保證計(jì)算量和實(shí)時(shí)性的同時(shí)，針對室內(nèi)動態(tài)場景，提出一種基于深度學(xué)習(xí)和多視圖幾何的兩階段運(yùn)動物體檢測優(yōu)化算法，以提高視覺SLAM 算法的運(yùn)行精度和運(yùn)行效率。

1 系統(tǒng)流程與框架

本文系統(tǒng)框架如圖1 所示，按照系統(tǒng)運(yùn)行流程可分為跟蹤線程、局部建圖線程、回環(huán)檢測3 個(gè)線程。輸入的RGB-D 圖像序列，針對RGB 通道利用深度學(xué)習(xí)目標(biāo)檢測算法進(jìn)行先驗(yàn)的運(yùn)動物體預(yù)檢測，并采用多視圖幾何算法進(jìn)行運(yùn)動物體重檢測，確定真正產(chǎn)生運(yùn)動的物體。在跟蹤線程中，首先針對這些動態(tài)物體相對應(yīng)的關(guān)鍵點(diǎn)進(jìn)行語義數(shù)據(jù)關(guān)聯(lián)，添加相應(yīng)的運(yùn)動狀態(tài)。然后通過最小化重投影誤差進(jìn)行相機(jī)位姿估計(jì)，完成視覺里程計(jì)初始化，同時(shí)跟蹤局部地圖。最后根據(jù)本文提出的關(guān)鍵幀選取策略，在局部建圖線程中插入候選關(guān)鍵幀，根據(jù)所插入關(guān)鍵幀中跟蹤的空間地圖點(diǎn)完成局部地圖更新。同時(shí)在后端優(yōu)化過程中執(zhí)行局部光束平差（BA）法完成相機(jī)位姿優(yōu)化。

圖1 本文系統(tǒng)流程與框架Fig.1 Procedure and framework of this system

回環(huán)檢測線程將每一次插入的關(guān)鍵幀與之前存儲的關(guān)鍵幀進(jìn)行匹配。如果檢測到環(huán)境中的閉環(huán)則計(jì)算關(guān)于相機(jī)位姿累計(jì)誤差的相似性變換。由于本文回環(huán)檢測過程主要參考ΟRB-SLAM2 的檢測過程，因此本文不再贅述。

2 融合運(yùn)動檢測的視覺SLAM 算法

2.1 目標(biāo)檢測

本文將基于改進(jìn)的SSD［17］網(wǎng)絡(luò)作為解決實(shí)時(shí)目標(biāo)檢測的方案，考慮到移動機(jī)器人所搭載的計(jì)算平臺具有運(yùn)算能力相對較差、計(jì)算量不足等問題，為減輕網(wǎng)絡(luò)參數(shù)量，采用MobileNet［18］網(wǎng)絡(luò)替換原來SSD 網(wǎng)絡(luò)中VGG-16［19］特征提取主干網(wǎng)絡(luò)，在保證檢測精度相對準(zhǔn)確的同時(shí)能夠滿足目標(biāo)檢測器實(shí)時(shí)運(yùn)行的需求，網(wǎng)絡(luò)結(jié)構(gòu)設(shè)計(jì)為全卷積網(wǎng)絡(luò)結(jié)構(gòu)，如圖2 所示。

圖2 全卷積神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)Fig.2 Fully convolutional neural network structure

為了提高小目標(biāo)物體檢測精度，本文采用Deconvoluational 模塊并 借鑒FPN［20］網(wǎng)絡(luò) 結(jié)構(gòu)，將 深層特征圖和淺層特征圖進(jìn)行信息融合，融合后的特征圖同時(shí)具有深層特征圖的語義信息和淺層特征圖位置信息。Decorder 部分主要借鑒了DSSD［21］的網(wǎng)絡(luò)結(jié)構(gòu)，經(jīng)過在特征圖上進(jìn)行1×1 的卷積后得到兩個(gè)支路，一個(gè)用于分類，另一個(gè)用于回歸。進(jìn)行此操作的主要目的是更多地利用深層網(wǎng)絡(luò)特征。

針對室內(nèi)場景目標(biāo)檢測，本文在MS CΟCΟ［22］數(shù)據(jù)集上對MobileNet-SSD 進(jìn)行預(yù)訓(xùn)練，MS CΟCΟ 包含91 種不同的物體，其中包含人、貓、桌子、椅子等室內(nèi)常見的物體。

2.2 運(yùn)動物體預(yù)檢測

本文采用上述深度學(xué)習(xí)目標(biāo)檢測網(wǎng)絡(luò)進(jìn)行運(yùn)動物體預(yù)檢測，即認(rèn)定環(huán)境中潛在的運(yùn)動物體。根據(jù)目標(biāo)檢測網(wǎng)絡(luò)提供潛在運(yùn)動物體的二維位置信息，對潛在運(yùn)動物體相對應(yīng)的特征點(diǎn)進(jìn)行運(yùn)動狀態(tài)預(yù)標(biāo)記。

首先根據(jù)Bounding Box 所提供運(yùn)動物體的二維坐標(biāo)(Px,Py,Pw,Ph)確定動態(tài)目標(biāo)的位置，其中Px、Py與Pw、Ph分別表示檢測框中心點(diǎn)坐標(biāo)以及寬度和高度。根據(jù)這些信息能夠計(jì)算出Bounding Box 的最大、最小坐標(biāo)(umax,umax,umin,umin)。相機(jī)輸入的圖像I提取到的關(guān)鍵點(diǎn)的二維坐標(biāo)定義為pi=(ui,νi)，通過與(umax,umax,umin,umin)對比得到取值在最大、最小坐標(biāo)區(qū)間內(nèi)的特征點(diǎn)。將每一個(gè)Bounding Box 根據(jù)關(guān)鍵點(diǎn)數(shù)量進(jìn)行編號排序并將檢測框內(nèi)的特征點(diǎn)pi對應(yīng)歸為一類，即pi∈Boxi。同時(shí)所有的Boxi均屬于Box 潛在運(yùn)動物體類，即Box1,Box2,…，Boxi∈Box。

為了提高檢測精度，同時(shí)減少計(jì)算量，本文只計(jì)算關(guān)鍵幀的數(shù)據(jù)并且目標(biāo)檢測置信度大于默認(rèn)閾值的Bounding Box（本文默認(rèn)設(shè)置為0.7）內(nèi)的特征點(diǎn)。通過上述過程能夠得到環(huán)境中潛在的運(yùn)動物體并分類得到潛在運(yùn)動物體類Box，完成運(yùn)動物體預(yù)檢測與運(yùn)動狀態(tài)預(yù)標(biāo)記過程。

2.3 運(yùn)動物體重檢測

本節(jié)針對運(yùn)動物體預(yù)檢測得到的潛在運(yùn)動物體類中的運(yùn)動對象進(jìn)行運(yùn)動物體重檢測，兩次確認(rèn)這些物體是否產(chǎn)生實(shí)際運(yùn)動，并將實(shí)際產(chǎn)生運(yùn)動的物體所對應(yīng)的特征點(diǎn)添加相應(yīng)的運(yùn)動狀態(tài)。

在三維空間中，運(yùn)動物體產(chǎn)生的像素運(yùn)動與相機(jī)自身運(yùn)動所產(chǎn)生的像素運(yùn)動具有不同的運(yùn)動方向。由于運(yùn)動三維物體對應(yīng)在二維圖像中會產(chǎn)生不同的動態(tài)視差di，因此本文采用像素之間投影點(diǎn)的距離，即像素間的重投影誤差di對動態(tài)視差進(jìn)行度量。像素間的重投影誤差主要取決于：

1）動態(tài)物體對應(yīng)特征點(diǎn)的像素坐標(biāo)。

2）動態(tài)物體對應(yīng)特征點(diǎn)的深度。

3）運(yùn)動物體與相機(jī)運(yùn)動之間的相對運(yùn)動角度。

為減少計(jì)算量，本文只計(jì)算Bounding Box 中匹配關(guān)鍵點(diǎn)像素間重投影誤差di，定義如下：

其中：pi表示三維空間中相對應(yīng)空間點(diǎn)Pi在當(dāng)前幀中關(guān)鍵點(diǎn)的二維坐標(biāo)表示通過前一幀匹配關(guān)鍵點(diǎn)在當(dāng)前幀恢復(fù)的投影點(diǎn)坐標(biāo)；di表示投影點(diǎn)與當(dāng)前幀關(guān)鍵點(diǎn)的重投影誤差。

重投影誤差示意圖如圖3 所示。

圖3 重投影誤差示意圖Fig.3 Schematic diagram of re-projection error

本文引入可變閾值Δω對運(yùn)動物體進(jìn)行運(yùn)動重檢測，閾值大小由微分熵函數(shù)［23］進(jìn)行確定。對于服從高斯分布的k維微分熵定義如下：

可變閾值Δω=f(H(xi))，當(dāng)計(jì)算得到的動態(tài)視差di＞Δω時(shí)，則被認(rèn)定為實(shí)際產(chǎn)生運(yùn)動的動態(tài)物體，將相對應(yīng)的潛在運(yùn)動物體類Boxi中所有的特征點(diǎn)添加運(yùn)動狀態(tài)ζi=0，表示該特征點(diǎn)在運(yùn)動中，非潛在運(yùn)動物體類Boxi的特征點(diǎn)和動態(tài)視差di＞Δω的靜態(tài)物體添加運(yùn)動狀態(tài)ζi=1。

2.4 靜態(tài)語義數(shù)據(jù)關(guān)聯(lián)與相機(jī)位姿優(yōu)化

關(guān)鍵點(diǎn)相應(yīng)運(yùn)動狀態(tài)添加完成后，并在相機(jī)位姿優(yōu)化過程中剔除被標(biāo)記為運(yùn)動中的關(guān)鍵點(diǎn)，不參與到相機(jī)位姿優(yōu)化光束平差法計(jì)算中。

輸入到跟蹤線程中的圖像，經(jīng)過運(yùn)動物體檢測后利用相鄰兩幀進(jìn)行三角量測恢復(fù)出地圖點(diǎn)，因?yàn)槠鋵⒖紟現(xiàn)r中的特征點(diǎn)和當(dāng)前幀F(xiàn)c特征點(diǎn)完成了特征匹配。

跟蹤線程計(jì)算出本質(zhì)矩陣和單應(yīng)矩陣，如果當(dāng)前場景為平面情況，則采用單應(yīng)矩陣計(jì)算當(dāng)前幀的相機(jī)位姿；如果當(dāng)前場景為非平面情況，則采用基礎(chǔ)矩陣計(jì)算當(dāng)前幀的相機(jī)位姿。

當(dāng)系統(tǒng)初始化成功后，當(dāng)前幀F(xiàn)c利用恒速模型進(jìn)行位姿跟蹤，如果初始化失敗，則重置當(dāng)前參考幀F(xiàn)r與相鄰關(guān)鍵幀進(jìn)行重定位恢復(fù)相機(jī)位姿。本文利用運(yùn)動狀態(tài)為靜態(tài)的關(guān)鍵點(diǎn)，即運(yùn)動狀態(tài)ζi≠0 的特征點(diǎn)進(jìn)行相機(jī)位姿恢復(fù)以及優(yōu)化。因?yàn)楦鶕?jù)ICP［24］算法已經(jīng)得到了N對相互匹配的3D 點(diǎn)的三維坐標(biāo)Pi和對應(yīng)投影點(diǎn)的二維坐標(biāo)pi，則對應(yīng)R、t的當(dāng)前幀相機(jī)位姿坐標(biāo)ξ相機(jī)位姿李代數(shù)的形式為：

其中：?∈R3為相機(jī)旋轉(zhuǎn)坐標(biāo)；φ∈sο(3)為相機(jī)平移坐標(biāo)。

2.5 關(guān)鍵幀選擇策略

關(guān)鍵幀的選擇策略影響整個(gè)系統(tǒng)的運(yùn)行精度，同時(shí)局部地圖的質(zhì)量也與關(guān)鍵幀的選擇有密切的關(guān)系，本文關(guān)鍵幀選擇遵循以下規(guī)則：

1）當(dāng)前幀至少跟蹤20 個(gè)以上空間中的三維地圖點(diǎn)，同時(shí)當(dāng)前幀內(nèi)特征點(diǎn)數(shù)量不少于相鄰關(guān)鍵幀的90%。

2）當(dāng)前關(guān)鍵幀需要在跟蹤線程中被跟蹤20 次以上，才能夠在局部建圖線程空閑時(shí)插入。

3）當(dāng)前幀相機(jī)位姿與相鄰關(guān)鍵幀的相機(jī)位姿間的旋轉(zhuǎn)姿態(tài)角度不大于設(shè)定閾值。

關(guān)鍵幀選擇條件如下：

1）保證所選擇的候選關(guān)鍵幀跟蹤到空間中數(shù)量足夠多的地圖點(diǎn)，并且候選關(guān)鍵幀中提取到的環(huán)境信息足夠豐富。

2）保證所選取的關(guān)鍵幀與相鄰幀有足夠的共視關(guān)系，能夠通過共視關(guān)鍵幀獲取到盡可能多的環(huán)境信息，并且保證候選關(guān)鍵幀相對穩(wěn)定提高系統(tǒng)精度。

3）防止相機(jī)在俯仰或轉(zhuǎn)動等純旋轉(zhuǎn)情況下，相機(jī)視差發(fā)生較大的變化導(dǎo)致跟蹤失敗，確保選取的候選關(guān)鍵幀穩(wěn)定并且連貫。

2.6 算法復(fù)雜度分析

在未知環(huán)境中，SLAM 問題主要解決機(jī)器人自身定位和地圖構(gòu)建問題，將此過程利用數(shù)學(xué)表達(dá)式進(jìn)行描述，可以抽象為狀態(tài)估計(jì)問題。假設(shè)移動機(jī)器人在未知環(huán)境中連續(xù)運(yùn)動，在t=0,1,···,N的時(shí)間段內(nèi)，有對應(yīng)運(yùn)動機(jī)器人位姿x0,x1,…,xN，機(jī)器人觀測到的路標(biāo)點(diǎn)y0,y1,···,yN以及在xi點(diǎn)觀測到路標(biāo)yj對應(yīng)的信息zi,j。利用以上信息能夠列出SLAM 運(yùn)動狀態(tài)估計(jì)過程中的運(yùn)動方程式與觀測方程式：

其中：ui表示在傳感器讀入數(shù)據(jù)；wi與νi,j均為噪聲誤差。融合運(yùn)動檢測視覺SLAM 算法的運(yùn)算復(fù)雜度和空間復(fù)雜度計(jì)算可以分為以下2 個(gè)部分：

1）將視覺里程計(jì)中的運(yùn)動檢測算法的時(shí)間復(fù)雜度作為一個(gè)整體進(jìn)行分析，其總體運(yùn)算復(fù)雜度為Ο(n2)，空間復(fù)雜度為Ο(n2)。

2）運(yùn)動方程和觀測方程在更新過程中，其運(yùn)算復(fù)雜度為Ο(n3)，空間復(fù)雜度為Ο(n2)。

因此，融合運(yùn)動檢測視覺SLAM 算法的總體運(yùn)算復(fù)雜度為Ο(n3)，空間復(fù)雜度為Ο(n2)。

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

本文選取TUM RGB-D Benchmark［25］數(shù)據(jù)集，該數(shù)據(jù)集中的某些序列包含運(yùn)動對象，如果不考慮這些運(yùn)動對象，則會影響軌跡估計(jì)精度。在本文實(shí)驗(yàn)中，使用靜態(tài)環(huán)境序列和屬于動態(tài)對象類別序列的子集來評估所提出算法的性能。本文算法在這兩種情況下進(jìn)行性能和精度的測試，并與ΟRB-SLAM2、DynaSLAM 進(jìn)行對比。

3.1 視覺里程計(jì)對比評估

本節(jié)實(shí)驗(yàn)主要對比不同算法之間視覺里程計(jì)的漂移量，因此采用相對位姿誤差（Relative Pose Error，RPE）作為評價(jià)指標(biāo)。相對軌跡誤差通過計(jì)算相同時(shí)間段間隔真實(shí)值與估計(jì)值之間變化量的差值進(jìn)行評估。相對軌跡誤差在時(shí)間間隔的定義如下：

在不同序列下的比較結(jié)果如表1 所示，表中fr2/desk、fr3/l/house 為靜態(tài)場景序列，fr3/w/half、fr3/w/rpy、fr3/w/static、fr3/w/xyz 為動態(tài)場景序列，下文同。其中RMSE 表示均方根誤差，加粗字體為結(jié)果最優(yōu)。它的定義是觀測值與真實(shí)值之間差的平方和與觀測次數(shù)比值的平方根。其計(jì)算公式定義如下：

表1 里程計(jì)相對位姿誤差的RMSE 比較Table 1 RMSE comparison of odometry relative pose error

表2 為相對位姿誤差的誤差均值（Mean），表3表示相對位姿誤差的殘差平方和（Sum of Squares due to Error，SSE），其中加粗字體為結(jié)果最優(yōu)。

表2 里程計(jì)相對位姿誤差均值比較Table 2 Mean comparison of odometry relative pose error

表3 里程計(jì)相對位姿誤差SSE 比較Table 3 SSE comparison of odometry relative pose error

由表1～表3 誤差信息對比可知，本文算法在靜態(tài)和動態(tài)環(huán)境圖像序列中均取得較好的結(jié)果，在動態(tài)環(huán)境序列中的性能均優(yōu)于ΟRB-SLAM2。

通過在ΟRB-SLAM2 基礎(chǔ)上引入運(yùn)動物體檢測算法可以精確刪除動態(tài)關(guān)鍵點(diǎn)，使得SLAM 線程不受移動對象的影響。在所有動態(tài)序列中，相對位姿誤差RMSE 均得到了有效的改善。雖然本文算法主要適用于現(xiàn)實(shí)中的動態(tài)環(huán)境，但是在靜態(tài)序列中，本文提出的SLAM 算法的性能對比ΟRB-SLAM2 同樣保持在同一精度水平。

本文算法與ΟRB-SLAM2 的視覺里程計(jì)相對位姿誤差對比如圖4 所示。通過對比相對位姿誤差圖，可知ΟRB-SLAM2 系統(tǒng)在動態(tài)場景fr3/w/static、fr3/w/xyz 的誤差很大，與同樣具有運(yùn)動物體檢測算法的DynaSLAM 對比，本文算法也同樣具有精度優(yōu)勢。

圖4 視覺里程計(jì)相對位姿誤差Fig.4 Relative pose error of visual odometry

3.2 SLAM 算法對比評估

在評估SLAM 算法實(shí)驗(yàn)部分，本文采用絕對軌跡誤差（Absolute Trajectory Error，ATE）作為評價(jià)指標(biāo)，該指標(biāo)通過對比真實(shí)值與估計(jì)值之間的差異來進(jìn)行評估，適用于估計(jì)機(jī)器人運(yùn)動軌跡誤差。絕對軌跡誤差dj計(jì)算公式定義如下：

其中：xj表示估計(jì)軌跡表示真實(shí)軌跡。

表4 為算法絕對軌跡誤差的均方根誤差對比，表5 為算法絕對軌跡誤差的誤差均值對比，表6 為算法絕對軌跡誤差的殘差平方和對比。對比表4～表6可知，ΟRB-SLAM2 算法 在fr2/desk 和fr3/l/house 兩種靜態(tài)場景圖像序列下，絕對軌跡誤差相對較小，但與本文算法相比仍有差距。

表4 不同算法絕對軌跡誤差的RMSE 對比Table 4 RMSE comparison of absolute trajectory errors of different algorithms

表5 不同算法絕對軌跡誤差均值對比Table 5 Comparison of mean absolute trajectory errors of different algorithms

表6 不同算法絕對軌跡誤差的SSE 對比Table 6 SSE comparison of absolute trajectory errors of different algorithms

對比表4～表6 可知，本文算法相比DynaSLAM在4 種不同相機(jī)運(yùn)動的動態(tài)環(huán)境圖像子序列上的ATE 指標(biāo)也取得了更小的誤差值，算法精度更高。

圖5 對比了ΟRB-SLAM2 和本文算法在TUM 數(shù)據(jù)集中fr3/w/xyz 和fr3/w/static 兩個(gè)動態(tài)環(huán)相機(jī)位姿估計(jì)結(jié)果發(fā)生很大的偏移，最大ATE 值達(dá)到1.2 m 以上。本文算法針對運(yùn)動物體進(jìn)行處理后軌跡估計(jì)更加精準(zhǔn)，誤差大幅降低。

圖5 算法絕對軌跡誤差對比Fig.5 Comparison of algorithm absolute trajectory errors

圖6 表示ΟRB-SLAM2 與本文算法在動態(tài)序列下預(yù)測值與真實(shí)值之間的軌跡誤差。其中圖6（a）、圖6（b）為ΟRB_SLAM2 算法軌 跡誤差，圖6（c）、圖6（d）為本文算法軌跡誤差。通過對比可知，本文針對運(yùn)動物體剔除后算法更加魯棒，運(yùn)行軌跡更加接近真實(shí)值。

圖6 算法軌跡誤差Fig.6 Algorithms trajectory error

在驗(yàn)證算法魯棒性方面，本文采用TUM 數(shù)據(jù)集中包含移動物體的fr3/Walking 動態(tài)序列驗(yàn)證。Walking 子序列中包含兩個(gè)移動的人，在前景和背景中不斷運(yùn)動。該圖像序列中包含4 種不同相機(jī)運(yùn)動：

1）fr3/w/xyz 子序列表示相機(jī)沿X-Y-Z軸（XYZ）移動。

2）fr3/w/half 子序列表示相機(jī)沿半徑為0.5 m 的半球面（half）軌跡運(yùn)動。

3）fr3/w/static 子序列表示相機(jī)保持靜止（static）狀態(tài)。

4）fr3/w/rpy 子序列表示相機(jī)在俯仰、翻滾和偏航軸上旋轉(zhuǎn)（RPY）運(yùn)動。

本文算法分別在上述4 個(gè)不同相機(jī)運(yùn)動的數(shù)據(jù)集上實(shí)驗(yàn)驗(yàn)證，在應(yīng)對不同相機(jī)運(yùn)動的過程中本文算法運(yùn)行精度對比ΟRB-SLAM2、DynaSLAM 表現(xiàn)最優(yōu)。在應(yīng)對4 種不同相機(jī)運(yùn)動的動態(tài)場景中，本文平均絕對軌跡誤差相對于DynaSLAM 有明顯提升。

3.3 系統(tǒng)實(shí)時(shí)性評估

利用TUM 數(shù)據(jù)集中提供的多種不同場景下的連續(xù)圖像對本文系統(tǒng)實(shí)時(shí)性進(jìn)行測試。測試圖片分辨率為640×480 像素，系統(tǒng)中只有跟蹤線程需要對輸入的圖像進(jìn)行實(shí)時(shí)處理。

本文實(shí)驗(yàn)采用INTER i5 CPU、內(nèi)存8 GB、NVIDIA 1060TI GPU、顯存6 GB 進(jìn)行實(shí)驗(yàn)。測試結(jié)果如表7 所示，算法平均耗時(shí)0.025 s，本文改進(jìn)后的視覺SLAM 算法的跟蹤線程平均處理速度能夠達(dá)到6 frame/s，基本滿足實(shí)時(shí)性的需求。

表7 跟蹤線程耗時(shí)對比Table 7 Comparison of tracking thread time s

表7 表示本文算法與DynaSLAM 跟蹤線程中所消耗的時(shí)間對比。在與DynaSLAM 在相同實(shí)驗(yàn)設(shè)備運(yùn)行動態(tài)環(huán)境序列時(shí)，本文算法相較于前者跟蹤速度提升了10 倍以上，滿足室內(nèi)環(huán)境實(shí)時(shí)跟蹤的需求。

由于實(shí)際場景中移動機(jī)器人搭載的設(shè)備計(jì)算能力有限，DynaSLAM 算法是結(jié)合深度學(xué)習(xí)語義分割與多視圖幾何完成運(yùn)動物體剔除。由于計(jì)算資源有限，在一些情況下語義分割無法實(shí)時(shí)完成，采用多視圖幾何進(jìn)行運(yùn)動檢測，這樣會使一些特征點(diǎn)剔除失敗，導(dǎo)致精度降低。本文算法所需計(jì)算資源相對較小，同時(shí)完成兩次運(yùn)動物體檢測，提高了SLAM 算法的運(yùn)行精度。本文算法對比ΟRB-SLAM2 與DynaSLAM 算法特征提取結(jié)果如圖7 所示。

圖7 算法特征點(diǎn)提取對比Fig.7 Comparison of algorithm feature point extraction

通過圖7 對比可知，ΟRB-SLAM2 算法雖然運(yùn)行速度達(dá)到實(shí)時(shí)性的需求，但是算法并未針對動態(tài)物體進(jìn)行處理，算法中一部分特征點(diǎn)提取到動態(tài)物體上，導(dǎo)致算法精度降低。DynaSLAM 雖然針對運(yùn)動物體進(jìn)行處理，但是由于計(jì)算資源有限，在部分低動態(tài)場景中也產(chǎn)生了特征點(diǎn)的錯(cuò)誤提取。本文算法利用兩階段運(yùn)動物體檢測，在計(jì)算能力較低的平臺，在保證實(shí)時(shí)性的同時(shí)實(shí)現(xiàn)了運(yùn)動物體的精準(zhǔn)剔除，提取結(jié)果均優(yōu)于ΟRB-SLAM2 以及DynaSLAM 算法。

4 結(jié)束語

本文面向室內(nèi)動態(tài)環(huán)境，提出一種基于深度學(xué)習(xí)和多視圖幾何的視覺SLAM 算法。利用兩階段運(yùn)動物體檢測確定實(shí)際產(chǎn)生運(yùn)動的物體，基于語義數(shù)據(jù)關(guān)聯(lián)算法采用靜態(tài)特征點(diǎn)優(yōu)化相機(jī)位姿。針對跟蹤線程和局部建圖線程，提出一種關(guān)鍵幀選取策略，從而減少運(yùn)動物體對算法精度的影響，提高算法穩(wěn)定性。實(shí)驗(yàn)結(jié)果表明，本文算法的絕對軌跡誤差相較于改進(jìn)之前的ΟRB-SLAM2，平均均方根誤差降低40%。在算法實(shí)時(shí)性方面，與DynaSLAM 相比算法實(shí)時(shí)性提高10 倍以上。因此，本文算法能夠在室內(nèi)動態(tài)環(huán)境下穩(wěn)定運(yùn)行，實(shí)現(xiàn)準(zhǔn)確的位姿估計(jì)。由于在動態(tài)環(huán)境中運(yùn)動物體的速度、位姿等信息能夠有效幫助自身系統(tǒng)完成定位，因此下一步研究如何將動態(tài)物體運(yùn)動位姿估計(jì)和靜態(tài)場景集成到一個(gè)視覺SLAM 中，提高視覺SLAM 在復(fù)雜動態(tài)環(huán)境中相機(jī)位姿估計(jì)的準(zhǔn)確性和魯棒性。