基于目標檢測的室內動態場景定位與建圖

席志紅，溫家旭

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

0 引言

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

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

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

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

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

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

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

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

1 本文系統

1.1 SLAM系統框架

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

圖1 本文SLAM系統框架Fig.1 Framework of SLAM system in this paper

1.2 動態目標檢測

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

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

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

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

1.3 GMS算法剔除誤匹配

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

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

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

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

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

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

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

本文系統在ORB 特征點匹配后利用GMS 算法來剔除誤匹配，這樣不僅可以使后續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 靜態稠密點云地圖

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

1.4.2 八叉樹地圖

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

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

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

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

2 實驗與分析

2.1 數據集與測試平臺

本文選用TUM RGB-D 公開數據集中的動態序列freiburg3_walking_xyz、freiburg3_walking_halfsphere和freiburg3_walking_static 進行測試，測試數據集均為高動態圖像序列，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 系統、GCNv2_SLAM 系統、YOLOv4+ORB-SLAM2 系統與本文系統在上述3 個數據集序列中的實驗結果如表1～3 所示，利用絕對軌跡誤差（Absolute Trajectory Error，ATE）和相對位姿誤差（Relative Pose Error，RPE）作為評價指標，ATE 代表軌跡的全局一致性，RPE 測量平移和旋轉漂移，其中均方根誤差（Root Mean Square Error，RMSE）反映估計值與真實值之間的偏差，平均誤差（Mean）反映所有估計誤差的平均水平。這幾種客觀評價指標表現出系統估計的軌跡與真實值之間的差距，反映了系統的穩定性和可靠性。

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

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

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

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

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

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

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

2.2.2 運行時間

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

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

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

2.3 地圖創建

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

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

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

3 結語

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