席志紅，溫家旭

（哈爾濱工程大學(xué) 信息與通信工程學(xué)院，哈爾濱 150001）

0 引言

同時(shí)定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）是移動(dòng)機(jī)器人在未知環(huán)境下實(shí)現(xiàn)自主定位導(dǎo)航的核心技術(shù)［1］，視覺SLAM（Visual SALM，VSLAM）主要采用視覺傳感器作為數(shù)據(jù)源，相比激光雷達(dá)等其他SLAM 技術(shù)具有一定價(jià)格優(yōu)勢，近年來成為SLAM 研究領(lǐng)域的熱點(diǎn)方向之一［2］。

室內(nèi)場景中存在運(yùn)動(dòng)物體時(shí)，動(dòng)態(tài)物體內(nèi)不穩(wěn)定的特征點(diǎn)會(huì)嚴(yán)重影響相機(jī)位姿估計(jì)的準(zhǔn)確性，因此視覺SLAM 在室內(nèi)動(dòng)態(tài)場景中仍然具有很大的挑戰(zhàn)性。隨著深度學(xué)習(xí)的快速發(fā)展，基于深度學(xué)習(xí)的室內(nèi)動(dòng)態(tài)場景SLAM 逐漸受到人們的關(guān)注［3］。

ORB-SLAM2［4］被認(rèn)為是目前最完整的SLAM 框架之一，但是在動(dòng)態(tài)場景中ORB-SLAM2 系統(tǒng)定位與建圖效果并不理想，并且僅創(chuàng)建稀疏點(diǎn)云地圖，無法用于移動(dòng)機(jī)器人后續(xù)導(dǎo)航工作。針對(duì)以上問題研究學(xué)者們做了大量嘗試，Yu 等［5］提出的DS-SLAM 將SLAM 與SegNet 語義分割網(wǎng)絡(luò)相結(jié)合，利用語義信息和運(yùn)動(dòng)特征點(diǎn)檢測并濾除每一幀圖像中的動(dòng)態(tài)物體，從而極大提高了動(dòng)態(tài)環(huán)境中相機(jī)的定位精度，同時(shí)建立語義八叉樹地圖。Bescos 等［6］提出的DynaSLAM 將深度學(xué)習(xí)和多視幾何結(jié)合，在ORB-SLAM2 系統(tǒng)的基礎(chǔ)上，增加了動(dòng)態(tài)物體檢測和背景修補(bǔ)功能，同時(shí)建立靜態(tài)場景地圖。房立金等［7］提出一種結(jié)合實(shí)例分割的動(dòng)態(tài)場景SLAM 方法，利用MaskRCNN［8］對(duì)關(guān)鍵幀進(jìn)行實(shí)例分割，計(jì)算特征點(diǎn)光流場篩選出動(dòng)態(tài)特征點(diǎn)，識(shí)別出場景中真正運(yùn)動(dòng)的物體并將屬于動(dòng)態(tài)物體的特征點(diǎn)去除，最后進(jìn)行語義關(guān)聯(lián)，建立無動(dòng)態(tài)物體干擾的語義地圖；Wen 等［9］提出了一種利用MaskRCNN 檢測環(huán)境中動(dòng)態(tài)對(duì)象并構(gòu)建包含語義地圖的SLAM 方法，在該方法中重投影誤差、光度誤差和深度誤差被用來給每個(gè)關(guān)鍵點(diǎn)分配一個(gè)魯棒的權(quán)重，從而將動(dòng)態(tài)點(diǎn)和靜態(tài)點(diǎn)分開，實(shí)現(xiàn)動(dòng)態(tài)對(duì)象的幾何分割；Ren 等［10］提出了一種基于圖像信息熵的視覺SLAM 方法，根據(jù)圖像的信息熵確定信息量，篩選對(duì)比度低、梯度變化小的圖像，對(duì)圖像進(jìn)行增強(qiáng)處理，盡可能多地提取出能夠代表圖像信息的特征點(diǎn)，作為相鄰幀匹配和關(guān)鍵幀匹配的相關(guān)基礎(chǔ)。

本文針對(duì)室內(nèi)場景中動(dòng)態(tài)對(duì)象影響相機(jī)位姿估計(jì)準(zhǔn)確性的問題，提出了一種基于目標(biāo)檢測的室內(nèi)動(dòng)態(tài)場景SLAM系統(tǒng)，主要工作如下：

1）將ORB-SLAM2 系統(tǒng)與YOLOv4 目標(biāo)檢測網(wǎng)絡(luò)［11］相結(jié)合，檢測并濾除動(dòng)態(tài)對(duì)象邊界框的掩膜區(qū)域內(nèi)部的特征點(diǎn)，利用靜態(tài)特征點(diǎn)估計(jì)相機(jī)位姿；

2）在ORB特征匹配時(shí)，結(jié)合GMS（Grid-based Motion Statistics）穩(wěn)健匹配算法［12］進(jìn)一步有效剔除誤匹配，提高特征匹配質(zhì)量；

3）建立靜態(tài)稠密點(diǎn)云地圖［13］以及八叉樹地圖［14］，可用于后續(xù)移動(dòng)機(jī)器人的導(dǎo)航工作；

4）在TUM RGB-D 公開數(shù)據(jù)集［15］上，將本文系統(tǒng)與ORBSLAM2 系統(tǒng)和YOLOv4+ORB-SLAM2［16］系統(tǒng)進(jìn)行對(duì)比測試，評(píng)估本文系統(tǒng)相機(jī)位姿估計(jì)精度。

1 本文系統(tǒng)

1.1 SLAM系統(tǒng)框架

本文SLAM 系統(tǒng)在ORB-SLAM2 系統(tǒng)框架的基礎(chǔ)上進(jìn)行改進(jìn)，采用RGB-D 相機(jī)獲取場景圖像，并行運(yùn)行5 個(gè)線程，分別為目標(biāo)檢測、跟蹤、局部地圖、局部回環(huán)以及地圖構(gòu)建。本文SLAM 系統(tǒng)框架如圖1 所示，首先圖像經(jīng)過目標(biāo)檢測網(wǎng)絡(luò)處理，提取場景中的動(dòng)態(tài)物體信息；跟蹤線程提取圖像ORB特征點(diǎn)，將動(dòng)態(tài)物體檢測框掩膜內(nèi)部的特征點(diǎn)剔除后，再經(jīng)過GMS 特征匹配算法剔除誤匹配［17］；利用質(zhì)量較好的匹配進(jìn)行跟蹤，局部地圖和局部回環(huán)可對(duì)關(guān)鍵幀的位姿和地圖點(diǎn)的位置進(jìn)行優(yōu)化，避免產(chǎn)生尺度漂移問題；最后在地圖構(gòu)建線程中建立靜態(tài)稠密點(diǎn)云地圖以及八叉樹地圖。

圖1 本文SLAM系統(tǒng)框架Fig.1 Framework of SLAM system in this paper

1.2 動(dòng)態(tài)目標(biāo)檢測

本文系統(tǒng)使用YOLOv4 網(wǎng)絡(luò)進(jìn)行動(dòng)態(tài)目標(biāo)檢測，YOLOv4 目標(biāo)檢測網(wǎng)絡(luò)是一種高效且強(qiáng)大的目標(biāo)檢測模型，YOLOv4 算法在實(shí)時(shí)目標(biāo)檢測算法中精度很高，實(shí)現(xiàn)了精度和速度的最佳平衡。

YOLOv4 目標(biāo)檢測網(wǎng)絡(luò)結(jié)構(gòu)采用CSPDarknet53 作為主網(wǎng)絡(luò)，保留了YOLOv3 的head 部分，采用空間金字塔池化思想增加感受野，能夠分離出最重要的上下文特征而且沒有降低網(wǎng)絡(luò)運(yùn)行的速度，同時(shí)使用PANet 中的路徑聚合模塊，將融合的方法由加法改為乘法。

在實(shí)際室內(nèi)場景中，人、狗、貓被認(rèn)為是主要?jiǎng)討B(tài)對(duì)象，本系統(tǒng)利用YOLOv4 網(wǎng)絡(luò)對(duì)室內(nèi)場景進(jìn)行動(dòng)態(tài)目標(biāo)檢測，若目標(biāo)檢測后的結(jié)果中含有上述動(dòng)態(tài)對(duì)象，則生成對(duì)應(yīng)邊界框的掩膜區(qū)域，結(jié)合掩膜區(qū)域和ORB 特征提取后的結(jié)果，將掩膜區(qū)域內(nèi)的ORB 特征點(diǎn)剔除，這樣可以顯著降低動(dòng)態(tài)對(duì)象的影響。本文系統(tǒng)輸入圖像經(jīng)過YOLOv4 目標(biāo)檢測網(wǎng)絡(luò)后的結(jié)果如圖2 所示（測試圖像來源于freiburg3_walking_xyz 數(shù)據(jù)集）。

圖2 YOLOv4目標(biāo)檢測結(jié)果Fig.2 YOLOv4 target detection results

1.3 GMS算法剔除誤匹配

GMS 是一種基于網(wǎng)格運(yùn)動(dòng)統(tǒng)計(jì)的快速魯棒的特征匹配方法，可以快速剔除誤匹配，提高匹配穩(wěn)定性，核心思想是利用相鄰像素之間的一致性約束，并將這種約束封裝成區(qū)域?qū)χg的統(tǒng)計(jì)特性，誤匹配點(diǎn)和正確匹配點(diǎn)在統(tǒng)計(jì)特性上會(huì)服從不同的分布，可通過設(shè)定閾值的方法來區(qū)分正確匹配和誤匹配。

首先將圖片進(jìn)行網(wǎng)格化處理，任意網(wǎng)格中的匹配點(diǎn)為xi，xi所在網(wǎng)格中的匹配點(diǎn)數(shù)量記為Si，則Si服從二項(xiàng)分布，如式（1）所示：

其中：K為xi周圍的網(wǎng)格數(shù)，n為xi所在網(wǎng)格內(nèi)特征點(diǎn)的數(shù)量，pt表示xi為正確匹配的概率，pf表示xi為誤匹配的概率，T代表匹配正確，F(xiàn)代表匹配錯(cuò)誤。該分布的均值和標(biāo)準(zhǔn)差為：

通常來說，正確匹配點(diǎn)要比誤匹配點(diǎn)多，設(shè)定合適的閾值，利用Si的均值和方差就可以劃分誤匹配點(diǎn)和正確匹配點(diǎn)，定義劃分指標(biāo)為P，使用劃分指標(biāo)P來剔除誤匹配點(diǎn)，其數(shù)學(xué)表達(dá)式如式（3）所示：

這可以轉(zhuǎn)化為一個(gè)優(yōu)化問題，即最大化劃分指標(biāo)，由式（3）可得：

從式（4）可以看到，P值和特征點(diǎn)的鄰域格網(wǎng)數(shù)量以及特征點(diǎn)數(shù)量呈正相關(guān)，設(shè)閾值為τ，那么

其中：α設(shè)為6，網(wǎng)格內(nèi)的特征點(diǎn)個(gè)數(shù)若大于閾值則認(rèn)為是正確匹配，否則為誤匹配。

本文系統(tǒng)在ORB 特征點(diǎn)匹配后利用GMS 算法來剔除誤匹配，這樣不僅可以使后續(xù)RANSAC（RANdom SAmple Consensus）算法［18］更快收斂，而且可以得到更加準(zhǔn)確的相機(jī)位姿，利用GMS 算法進(jìn)行ORB 特征點(diǎn)匹配的效果如圖3 所示，其中ORB 特征點(diǎn)提取數(shù)量設(shè)置為1 000，最終得到的匹配數(shù)量為137 對(duì)（注：測試圖像來源于freiburg3_walking_xyz 數(shù)據(jù)集）。

圖3 GMS算法特征點(diǎn)匹配效果Fig.3 Feature point matching effect by GMS algorithm

1.4 地圖構(gòu)建

1.4.1 靜態(tài)稠密點(diǎn)云地圖

點(diǎn)云地圖即一組離散點(diǎn)表示的地圖，包含三維坐標(biāo)以及彩色信息，利用RGB-D 相機(jī)提供的彩色圖和深度圖，結(jié)合相機(jī)內(nèi)參便可計(jì)算點(diǎn)云，根據(jù)相機(jī)位姿將點(diǎn)云進(jìn)行加和，就可以得到全局點(diǎn)云，ORB-SLAM2 系統(tǒng)僅創(chuàng)建稀疏點(diǎn)云地圖，缺乏環(huán)境信息，無法用于移動(dòng)機(jī)器人后續(xù)導(dǎo)航工作。本文系統(tǒng)首先利用目標(biāo)檢測結(jié)果濾除場景中的先驗(yàn)動(dòng)態(tài)對(duì)象，然后構(gòu)建無動(dòng)態(tài)對(duì)象干擾的靜態(tài)稠密點(diǎn)云地圖，能夠更好地展示場景環(huán)境信息，并且在建圖時(shí)加入外點(diǎn)去除濾波器和降采樣濾波器對(duì)點(diǎn)云進(jìn)行濾波處理，利用外點(diǎn)去除濾波器可去除孤立的噪聲點(diǎn)，利用降采樣濾波器對(duì)三維空間進(jìn)行降采樣，從而節(jié)省存儲(chǔ)空間。

1.4.2 八叉樹地圖

八叉樹地圖（octomap）是通過八叉樹結(jié)構(gòu)進(jìn)行信息存儲(chǔ)的地圖，是一種靈活的、壓縮的、又能隨時(shí)更新的地圖形式，可用于移動(dòng)機(jī)器人導(dǎo)航與避障。

八叉樹的節(jié)點(diǎn)存儲(chǔ)該節(jié)點(diǎn)是否被占據(jù)的信息，用概率形式表達(dá)某節(jié)點(diǎn)是否被占據(jù)，定義一個(gè)浮點(diǎn)數(shù)x∈[0，1]，初始值取0.5，如果不斷觀測到該節(jié)點(diǎn)被占據(jù)，那么不斷增大x取值；如果不斷觀測到該節(jié)點(diǎn)是空白，便不斷減小x取值，這樣就可以動(dòng)態(tài)建模地圖中的障礙物信息，設(shè)y∈R 為概率對(duì)數(shù)值，那么

這樣可保證x∈[0，1]，設(shè)某節(jié)點(diǎn)為m，觀測數(shù)據(jù)為z，那么從開始到t時(shí)刻某節(jié)點(diǎn)的概率對(duì)數(shù)值為L(n|z1：t)，t+1 時(shí)刻為：

這樣就可以根據(jù)RGB-D 數(shù)據(jù)更新整個(gè)八叉樹地圖，相對(duì)于稠密點(diǎn)云地圖，八叉樹地圖能夠顯著減小地圖存儲(chǔ)空間，可用于后續(xù)移動(dòng)機(jī)器人的導(dǎo)航工作。

2 實(shí)驗(yàn)與分析

2.1 數(shù)據(jù)集與測試平臺(tái)

本文選用TUM RGB-D 公開數(shù)據(jù)集中的動(dòng)態(tài)序列freiburg3_walking_xyz、freiburg3_walking_halfsphere和freiburg3_walking_static 進(jìn)行測試，測試數(shù)據(jù)集均為高動(dòng)態(tài)圖像序列，freiburg3_walking_xyz 圖像序列相機(jī)沿(x，y，z)三個(gè)方向移動(dòng)，freiburg3_walking_halfsphere 圖像序列中相機(jī)在大約1 m 直徑的小半球上移動(dòng)，freiburg3_walking_static 圖像序列相機(jī)保持在適當(dāng)位置，本實(shí)驗(yàn)測試運(yùn)行平臺(tái)為配備Intel Core i7 處理器、GeForce GTX 1050Ti 型號(hào)GPU，內(nèi)存為8 GB的臺(tái)式電腦。

2.2 相機(jī)位姿估計(jì)

2.2.1 位姿估計(jì)精度

本文進(jìn)行多次實(shí)驗(yàn)測試對(duì)比了ORB-SLAM2 系統(tǒng)、GCNv2_SLAM 系統(tǒng)、YOLOv4+ORB-SLAM2 系統(tǒng)與本文系統(tǒng)在上述3 個(gè)數(shù)據(jù)集序列中的實(shí)驗(yàn)結(jié)果如表1～3 所示，利用絕對(duì)軌跡誤差（Absolute Trajectory Error，ATE）和相對(duì)位姿誤差（Relative Pose Error，RPE）作為評(píng)價(jià)指標(biāo)，ATE 代表軌跡的全局一致性，RPE 測量平移和旋轉(zhuǎn)漂移，其中均方根誤差（Root Mean Square Error，RMSE）反映估計(jì)值與真實(shí)值之間的偏差，平均誤差（Mean）反映所有估計(jì)誤差的平均水平。這幾種客觀評(píng)價(jià)指標(biāo)表現(xiàn)出系統(tǒng)估計(jì)的軌跡與真實(shí)值之間的差距，反映了系統(tǒng)的穩(wěn)定性和可靠性。

由表1、2 可以看出，相對(duì)于ORB-SLAM2 系統(tǒng)和GCNv2_SLAM 系統(tǒng)來說，本文系統(tǒng)相機(jī)位姿估計(jì)的絕對(duì)軌跡誤差和相對(duì)位姿誤差明顯降低；相對(duì)于YOLOv4+ORBSLAM2 系統(tǒng)，本文系統(tǒng)利用GMS 算法剔除誤匹配，同時(shí)也提高了相機(jī)位姿估計(jì)的精度，為后續(xù)地圖創(chuàng)建提供更加準(zhǔn)確的相機(jī)位姿，提高建圖精度與效果。

表1 四個(gè)系統(tǒng)的絕對(duì)軌跡誤差結(jié)果對(duì)比單位：cm Tab.1 Absolute trajectory error results comparison of four systems unit：cm

本文利用evo 工具分別對(duì)ORB-SLAM2 系統(tǒng)、GCNv2_SLAM 系統(tǒng)、YOLOv4+ORB-SLAM2 系統(tǒng)與本文系統(tǒng)繪制相機(jī)的軌跡，并評(píng)估估計(jì)軌跡與真值的誤差，在freiburg3_walking_xyz 數(shù)據(jù)集下的實(shí)驗(yàn)結(jié)果如圖4 所示，其中虛線代表相機(jī)真實(shí)軌跡，實(shí)線代表相機(jī)估計(jì)軌跡，不同顏色代表不同的誤差。

表2 四個(gè)系統(tǒng)的相對(duì)位姿誤差結(jié)果對(duì)比 單位：cmTab.2 Relative pose error results comparison of four systems unit：cm

表3 四個(gè)系統(tǒng)的旋轉(zhuǎn)漂移結(jié)果對(duì)比 單位：（°）Tab.3 Rotational drift results comparison of four systems unit：（°）

圖4 四個(gè)系統(tǒng)的相機(jī)軌跡與誤差對(duì)比Fig.4 Camera trajectory and error comparison of four systems

由以上實(shí)驗(yàn)測試結(jié)果可以發(fā)現(xiàn)，本文系統(tǒng)在相機(jī)位姿估計(jì)準(zhǔn)確性方面表現(xiàn)良好，在freiburg3_walking_xyz、freiburg3_walking_halfsphere 和freiburg3_walking_static 數(shù)據(jù)集中，本文系統(tǒng)相對(duì)于ORB-SLAM2 系統(tǒng)來說，絕對(duì)位姿誤差的均方根誤差分別降低了97.47%、93.13% 和97.21%，相對(duì)于GCNv2_SLAM 系統(tǒng)分別降低了97.42%、93.30%和87.07%，相對(duì)于YOLOv4+ORB-SLAM2 系統(tǒng)分別降低了4.93%、9.26%和10.72%，其中本文系統(tǒng)相機(jī)估計(jì)軌跡與真實(shí)軌跡整體誤差最小，相機(jī)位姿估計(jì)精度最高，其原因在于本文系統(tǒng)增加了對(duì)動(dòng)態(tài)物體的處理，利用目標(biāo)檢測方法篩選出靜態(tài)特征點(diǎn)進(jìn)行位姿估計(jì)，并且利用GMS 特征匹配方法有效剔除誤匹配，從而提升動(dòng)態(tài)場景中相機(jī)位姿估計(jì)精度。

2.2.2 運(yùn)行時(shí)間

本文測試對(duì)比了ORB-SLAM2 系統(tǒng)、DynaSLAM 系統(tǒng)以及本文系統(tǒng)在freiburg3_walking_xyz 數(shù)據(jù)集上的平均跟蹤時(shí)間，結(jié)果如表4 所示。

表4 三種系統(tǒng)的運(yùn)行時(shí)間結(jié)果對(duì)比 單位：sTab.4 Running time results comparison of three systems unit：s

由表4 可以看出本文系統(tǒng)在ORB-SLAM2 系統(tǒng)的基礎(chǔ)上添加目標(biāo)檢測和GMS 特征匹配，同時(shí)也增加了運(yùn)行時(shí)間，但相對(duì)于DynaSLAM 系統(tǒng)來說，運(yùn)行時(shí)間較短，并且能夠保證較高的精度。

2.3 地圖創(chuàng)建

為了濾除動(dòng)態(tài)對(duì)象的干擾以及后續(xù)移動(dòng)機(jī)器人的導(dǎo)航與路徑規(guī)劃，本文在ORB-SLAM2 的基礎(chǔ)上構(gòu)建靜態(tài)稠密點(diǎn)云地圖和八叉樹地圖，在freiburg3_walking_xyz 數(shù)據(jù)集上創(chuàng)建的靜態(tài)稠密點(diǎn)云地圖與八叉樹地圖如圖5 所示。

由圖5 可以看出，靜態(tài)稠密點(diǎn)云地圖可以比較完整地保留環(huán)境信息，較清楚地顯示細(xì)節(jié)。八叉樹地圖所占存儲(chǔ)空間（1.6 MB）約為靜態(tài)稠密點(diǎn)云地圖（11.3 MB）的14%，同時(shí)八叉樹地圖可應(yīng)用于移動(dòng)機(jī)器人導(dǎo)航任務(wù)中，對(duì)于移動(dòng)機(jī)器人避障與路徑規(guī)劃有著重要的意義。

圖5 靜態(tài)稠密點(diǎn)云地圖和八叉樹地圖Fig.5 Static dense point cloud map and octomap

3 結(jié)語

本文針對(duì)室內(nèi)動(dòng)態(tài)場景下動(dòng)態(tài)對(duì)象影響相機(jī)位姿估計(jì)準(zhǔn)確性的問題，在ORB-SLAM2 系統(tǒng)的基礎(chǔ)上，結(jié)合YOLOv4目標(biāo)檢測網(wǎng)絡(luò)檢測場景中的動(dòng)態(tài)對(duì)象，剔除分布在動(dòng)態(tài)對(duì)象檢測框掩膜內(nèi)部的特征點(diǎn)；同時(shí)結(jié)合GMS 特征匹配算法剔除誤匹配，利用質(zhì)量較高的匹配計(jì)算相機(jī)位姿，這樣可以顯著降低動(dòng)態(tài)對(duì)象的影響，提高相機(jī)位姿估計(jì)的準(zhǔn)確性；最后實(shí)現(xiàn)靜態(tài)稠密點(diǎn)云地圖和八叉樹地圖的構(gòu)建。然而本文SLAM 系統(tǒng)在減小誤差的同時(shí)，也增加了運(yùn)行時(shí)間，在接下來的工作中，將進(jìn)一步提升目標(biāo)檢測網(wǎng)絡(luò)性能以減小相機(jī)位姿估計(jì)誤差，并且保證系統(tǒng)的實(shí)時(shí)性。