基于目標檢測的室內動態(tài)場景定位與建圖

席志紅，溫家旭

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

0 引言

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

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

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

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

1）將ORB-SLAM2 系統(tǒng)與YOLOv4 目標檢測網絡［11］相結合，檢測并濾除動態(tài)對象邊界框的掩膜區(qū)域內部的特征點，利用靜態(tài)特征點估計相機位姿；

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

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

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

1 本文系統(tǒng)

1.1 SLAM系統(tǒng)框架

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

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

1.2 動態(tài)目標檢測

本文系統(tǒng)使用YOLOv4 網絡進行動態(tài)目標檢測，YOLOv4 目標檢測網絡是一種高效且強大的目標檢測模型，YOLOv4 算法在實時目標檢測算法中精度很高，實現了精度和速度的最佳平衡。

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

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

圖2 YOLOv4目標檢測結果Fig.2 YOLOv4 target detection results

1.3 GMS算法剔除誤匹配

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

首先將圖片進行網格化處理，任意網格中的匹配點為xi，xi所在網格中的匹配點數量記為Si，則Si服從二項分布，如式（1）所示：

其中：K為xi周圍的網格數，n為xi所在網格內特征點的數量，pt表示xi為正確匹配的概率，pf表示xi為誤匹配的概率，T代表匹配正確，F代表匹配錯誤。該分布的均值和標準差為：

通常來說，正確匹配點要比誤匹配點多，設定合適的閾值，利用Si的均值和方差就可以劃分誤匹配點和正確匹配點，定義劃分指標為P，使用劃分指標P來剔除誤匹配點，其數學表達式如式（3）所示：

這可以轉化為一個優(yōu)化問題，即最大化劃分指標，由式（3）可得：

從式（4）可以看到，P值和特征點的鄰域格網數量以及特征點數量呈正相關，設閾值為τ，那么

其中：α設為6，網格內的特征點個數若大于閾值則認為是正確匹配，否則為誤匹配。

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

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

1.4 地圖構建

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

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

1.4.2 八叉樹地圖

八叉樹地圖（octomap）是通過八叉樹結構進行信息存儲的地圖，是一種靈活的、壓縮的、又能隨時更新的地圖形式，可用于移動機器人導航與避障。

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

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

這樣就可以根據RGB-D 數據更新整個八叉樹地圖，相對于稠密點云地圖，八叉樹地圖能夠顯著減小地圖存儲空間，可用于后續(xù)移動機器人的導航工作。

2 實驗與分析

2.1 數據集與測試平臺

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

2.2 相機位姿估計

2.2.1 位姿估計精度

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

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

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

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

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

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

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

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

2.2.2 運行時間

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

表4 三種系統(tǒng)的運行時間結果對比 單位：sTab.4 Running time results comparison of three systems unit：s

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

2.3 地圖創(chuàng)建

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

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

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

3 結語

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