一種室內動態場景下RGB-D SLAM的前端優化方法

作者簡介：李泳（1996－），男，碩士研究生，主要研究方向為SLAM、路徑規劃；劉宏杰（1972－），男（通信作者），高級工程師，碩導，碩士，主要研究方向為操作系統、無人機應用技術（89993662@qq.com）；周永錄（1965－），男，高級工程師，主要研究方向為操作系統；余映（1977－）男，副教授，博士，主要研究方向為計算機視覺、圖像處理與識別.

摘 要：針對動態物體會導致位姿估計誤差過大的問題，提出一種適用于動態場景下的RGB-D SLAM算法。首先，將目標檢測的結果根據物體是否具有自主移動能力劃分為動態物體與潛在動態物體，并使用大津法在對應的深度圖中完成檢測框內區域的前、背景分割；然后，結合檢測框的位置坐標與深度信息確定潛在動態物體與動態物體在三維空間內是否關聯，初步篩選出具有一定運動概率的潛在動態物體；最后，通過運動一致性檢測算法來最終確定是否將潛在動態物體作為動態物體處理。實驗部分選用公開的TUM數據集，結果表明，該算法在保持實時性的同時具有較高的定位精度和魯棒性。

關鍵詞：同時定位與地圖構建；動態場景；目標檢測；圖像分割；位姿估計

中圖分類號：TP391.4 文獻標志碼：A 文章編號：1001－3695（2023）04－005－0991－05doi： 10.19734/j.issn.1001－3695.2022.09.0459

Abstract：Aiming at the problem that dynamic objects will lead to excessive pose estimation error， this paper proposed a RGB-D SLAM algorithm for dynamic scenes. Firstly， the method divided the object detection results into dynamic objects and potential dynamic objects according to whether the object had the ability of autonomous movement， and used the Otsu method to complete the front and background segmentation of the area in the detection frame in the corresponding depth map. Then， combining with the position coordinates and depth information of the detection frame， it determined whether the potential dynamic object was related to the dynamic object in three-dimensional space， and screened out the potential dynamic object with a certain motion probability. Finally， it used the motion consistency detection algorithm to determine whether to treat potential dynamic objects as dynamic objects. The experiment selected the open TUM dataset. The results show that the proposed algorithm has high positioning accuracy and robustness while maintaining real-time.

Key words：SLAM; dynamic scene; object detection; image segmentation; pose estimation

0 引言

通過外部傳感器，如激光雷達、相機等進行同時定位與地圖構建的過程被稱為SLAM（simultaneous localization and mapping）。隨著近幾年移動機器人、無人機等領域的快速發展，對于未知環境中的自身定位與周邊環境構建問題變得尤為重要，SLAM正是解決此類問題的關鍵。相較于復雜的激光雷達，相機低廉的成本和豐富的圖像信息使其成為目前SLAM領域的熱門研究方向［1～5］。

經典的VSLAM算法根據其視覺里程計（visual odometry，VO）的不同，大體分為特征點法、直接法以及半直接法三類。特征點法通過最小化投影誤差來優化相機運動，Mur-Artal等人［6］提出的ORB-SLAM2使用ORB（oriented FAST and rotated BRIEF）［7］特征點進行當前幀與上一幀的特征匹配，完成位姿初始估計。ORB特征點由改進的FAST關鍵點和描述子構成，相較于SIFT［8］、SURF［9］等特征點，ORB特征點的計算量更小，適用于對實時性要求很高的SLAM系統。直接法是光流法的延伸，在直接法中無須知道點與點之間的對應關系，而是根據最小化光度誤差來求得相機位姿。Engel等人［10］提出完全基于直接法的DSO，考慮到圖像信息易受光照干擾，DSO提出光度標定，使得算法更具魯棒性。半直接法是特征點法與直接法的折中，Forster等人［11］提出SVO只計算關鍵點而不計算描述子，通過灰度值進行匹配，非常適用于無人機、小型手持設備等計算受限的平臺。

上述算法在靜態場景下具有非常不錯的表現，但考慮到現實世界，無論是室外場景下移動的車輛還是室內場景下運動的人，這些運動物體都會導致SLAM算法的精度變低，隨著機器學習的發展，研究人員提出了結合深度學習技術的SLAM算法［12］。

Bescos等人［13］提出DynaSLAM算法，該算法主要有結合Mask R-CNN［14］神經網絡對潛在運動物體進行實例分割以及利用多視圖幾何方法對沒有先驗動態標記的物體進行分割兩個特點，最后結合兩者結果，只要任意一方被檢測出來就認為是動態物體進行特征點剔除。Mask R-CNN實現的像素級實例分割在剔除動態物體上的特征點時具有較高的精度，但由于其計算量較大，很難在計算力受限的平臺上實時運行SLAM系統。Xiao等人［15］提出Dynamic-SLAM算法，使用SSD神經網絡［16］對圖像進行目標檢測，根據相鄰幀速度不變的特性使用一種補償算法對目標檢測的缺漏進行處理。該算法簡要地剔除掉目標檢測框內所有特征點，在動態目標較多的情況下會使檢測框內大部分靜態物體上的特征點同樣被剔除，容易導致地圖追蹤丟失。方寶富等人［17］提出聯合深度圖聚類和目標檢測以實現像素級實例分割的SLAM算法，該算法的主要特點在于對RGB-D相機的深度信息進行深度修復，通過對RGB圖像進行目標檢測，使用K-means聚類算法［18］將對應的修復后的深度圖進行實例分割，最終完成動態物體上的特征點剔除。K-means算法需要預先設定K（簇的個數）值，同一組數據由于K值的不同得到的聚類情況不同，最終得到的前景與背景也不同。Yu等人［19］提出基于語義的DS-SLAM算法，該算法使用SegNet［20］實現圖像的像素級語義分割，使用運動一致性檢測算法確定動態特征點并進行剔除。該算法的語義分割和運動一致性檢測只針對于室內場景下的人，并沒有考慮到與人相關聯的物體也存在運動的可能性，如人手上的書、人坐的椅子等。

在動態場景中，動態特征點對定位精度會造成極大的影響。針對上述方法出現的問題，本文結合深度學習技術與RGB-D相機的深度信息，在保證實時性的同時以期剔除所有動態特征點并保留足夠的靜態特征點參與位姿估計，在ORB-SLAM3的算法框架上主要進行如下改進：a）使用YOLOv5（you only look once）對RGB圖像進行推理，結合相機深度信息對檢測框內區域完成前、背景分割；b）提出一種物體相關聯算法對潛在動態物體進行初步運動概率判斷，結合運動一致性算法最終確定潛在動態物體是否發生運動。

1 算法框架

Campos等人［21］于2021年發表了ORB-SLAM3，是第一個基于特征的緊耦合VIO系統，對于位姿的估計僅依賴于最大后驗估計，同時引入Atlas多地圖系統［22］，對于地圖追蹤失敗的情況，Altas會重新構建地圖并對原來的地圖進行補齊。ORB-SLAM3作為典型的使用特征點匹配，對于不同的點的類型使用對極幾何約束、PnP、ICP等方法完成初始相機位姿估計。本文算法在ORB-SLAM3上進行改進，其具體算法流程如圖1所示，其中白色部分為本文主要工作。

2 目標檢測與前、背景分割

目前深度學習主要分為一階段方法和兩階段方法。一階段方法有YOLO系列、SSD等；兩階段方法有Faster R-CNN［23］、Mask R-CNN等。相較于一階段方法，兩階段方法在精度及準確度上都有較大的提升，但其檢測速度要慢于一階段方法。考慮到SLAM系統對實時性的要求，本文選用一階段YOLOv5神經網絡對每一幀圖片進行推理，并使用NMS非極大值抑制進行篩選。訓練集選用COCO2017中的一部分，訓練的類別選取室內場景下常見的自主運動物體與可能產生運動的部分靜態物體，如人、椅子、書、鍵盤、茶杯、鼠標等。

由于只依賴目標檢測技術所獲取的檢測框并不能完全擬合目標的邊界，若對檢測框內所有的特征點進行剔除，當檢測框數量較多且占據面積較大時可能會使匹配的特征點數量過少，導致追蹤失敗。基于深度學習的實例分割雖然對目標分割的效果較好，但其速度較慢，不適用于對實時性有要求的SLAM算法，因此需要一種較為快速的方法來完成檢測框區域內的前、背景分割。

大津法（Otsu）［24］是一種自適應的閾值算法，按照圖像的灰度特性將圖像分為前景與背景兩部分。若兩者之間的類間方差越大，表明構成圖像的兩部分的差別越大，若部分前景錯分為背景或部分背景錯分為前景都會導致兩部分的差別變小，本文結合深度圖像使用改進的大津法進行檢測框內前、背景分割。

深度相機所獲得的深度圖會出現深度為0的情況，其具體原因主要有以下三點：深度超出相機量程范圍，被測物體的材質問題以及相機自身硬件的老化、損壞等。在進行前、背景分割之前，需要剔除檢測框內深度為0的像素點。改進后的算法原理如下：

設目標區域圖像大小為W×H，將進行前、背景分割的閾值記為T，其中深度值D（x，y）小于閾值T的像素個數計為N0，深度值小于閾值T的計為N1。深度圖像中點深度為0的總數記為M，前景的像素點數占圖像像素點總數的比值記為ω0，背景的像素點數占圖像像素總數的比值記為ω1，則

3 部分靜態物體運動判斷

經過觀察發現，不具備自主移動能力的物體在多數情況下都保持靜止狀態，只有在受到具備自主移動能力的物體的外力作用下才會發生移動，如放在桌面上的水杯，只有當人端起它時會離開桌面發生運動。基于此，本文根據物體是否具有自主移動能力將目標檢測的結果人為地劃分為兩類。對于具備自主移動能力的物體，由于其運動的不確定性且發生運動的概率較大，將其視做動態物體處理。而對于不具備自主移動能力的物體，首先通過提出的相關聯判斷算法來檢測該物體是否與自主移動物體在三維空間內關聯，完成初步的篩選；其次，即使在三維空間內關聯的潛在動態物體也有不發生運動的可能性，因此需要使用基于稀疏光流的運動一致性檢測算法來最終確定潛在動態物體是否發生運動，若發生運動，則視做動態物體處理。為便于討論，以下將具備自主移動能力的物體稱為動態物體，不具備自主移動能力的物體稱為潛在動態物體。

3.1 物體相關聯判斷算法

在室內場景中，本文將“人”作為動態物體的代表，考慮潛在動態物體與其關聯情況。算法流程如圖2所示。

則認為該潛在動態物體與動態物體在三維空間內相關聯，此時該潛在動態物體由于受動態物體的影響，有極大概率發生運動，結合運動一致性檢測以最終確定該潛在動態物體是否會發生運動。關聯效果如圖3所示，圖（a）為原始RGB圖片，圖（b）為目標檢測結果，在圖（c）中，紅色線框內區域為動態物體，藍色線框內區域經物體相關聯算法可以確認為靜態物體，綠色線框內區域由于與動態物體關聯，其運動概率較大，通過運動一致性算法最終確定是否作為動態物體來處理（參見電子版）。對實驗部分所采用的數據集進行了測試，發現當K取值為0.2 m時可以取得較好的關聯效果。

3.2 運動一致性檢測算法

利用物體相關聯判斷算法可以篩選出有概率發生運動的潛在動態物體，但這些物體并非一定會隨著與之關聯的動態物體的運動而運動。由于這些潛在動態物體通常所占面積較小，可以借助運動物體檢測算法來最終確定該潛在動態物體的運動情況，考慮是否作為動態物體處理。

基于圖像差分的運動物體檢測算法有背景減除法和幀間差分法［25］，但這兩種方法均不適用于相機發生運動的情況。由于SLAM算法對實時性的要求，基于稠密光流的運動物體檢測算法計算量過大，本文使用基于稀疏光流的運動一致性檢測算法來判斷相關聯的靜態物體的運動情況。

若距離D大于閾值，則認為該點為動態特征點，對于附著在相關聯的潛在動態物體上的所有特征點中，動態特征點個數若超過一半，則認為該關聯的靜態物體會發生運動，視做動態物體處理。

4 實驗結果及分析

本文實驗采用公開的TUM RGB-D數據集。該數據集由在不同的室內場景使用Microsoft Kinect傳感器記錄的39個序列組成，包含了testing and debugging（測試）、structure vs. texture（結構 vs 紋理）、dynamic objects（動態物體）等。本文分別在該數據集上的高動態序列（fr3/walking_…）、低動態序列（fr3/sitting_…）及靜態場景（fr2/）進行實驗，以驗證算法在一般場景下的精度與魯棒性。實驗使用的計算機的硬件參數CPU為銳龍R9，GPU為RTX3060，8 GB內存；操作系統為Ubuntu 20.04。

4.1 定位誤差實驗對比及分析

絕對軌跡誤差（ATE）是指算法估計的位姿與真實位姿的差值，適合評估整體SLAM算法的性能；相對位姿誤差（RPE）是指真實位姿與估計位姿之間的差值每隔一段時間相對上次計算的差值的變化量，適合評估SLAM系統的漂移誤差。本文使用絕對路徑誤差（absolute trajectory error，ATE）與相對位姿誤差（relative pose error，RPE）對實驗結果進行評估。

為便于實驗對比，將本文算法命名為DLOA-SLAM。首先與傳統SLAM算法ORB-SLAM3在高動態序列walking_halfsphere上進行絕對軌跡誤差對比，以驗證本文算法在高動態場景中定位精度的提升效果。使用可視化工具evo進行軌跡繪制，如圖4～5所示。

其中褐色實線表示ORB-SLAM3絕對軌跡，藍色實線表示本文算法絕對軌跡，粉色虛線表示真實絕對軌跡（參見電子版）。可以看出，本文算法估計的軌跡能很好地貼合真實軌跡，與真實軌跡的誤差較小；而ORB-SLAM3算法不僅平均誤差大，且在部分時刻誤差較為嚴重。說明本文算法在動態場景下對于定位準確性有明顯的提升。

為進一步驗證本文算法的精度及魯棒性，本文選用均方根誤差（RMSE）、均值誤差（mean）、標準差（S.D）及中值誤差（median）四個衡量標準對ORB-SLAM3、DS-SLAM與本文算法在高動態序列、低動態序列及靜態序列下進行對比，對比結果如表1～3所示。其中表1為絕對軌跡誤差，表2為平移漂移誤差，表3為旋轉漂移誤差。從表中可以看出，本文算法在四個高動態序列中，其中三個取得最優結果，一個取得次優結果。而在低動態和靜態序列中，由于移動物體較少，動態特征點對于位姿估計影響有限，三種算法的定位誤差基本相同，表明本文算法同樣適用于低動態及靜態場景。

4.2 算法實時性實驗對比及分析

由于SLAM算法對于實時性有較高的要求，本文分別對ORB-SLAM3、Dyna-SLAM、DS-SLAM及本文算法進行時間對比實驗，選用每幀耗時時間的平均值（mean）與中值（median）作為衡量標準。對比結果如表4所示，可以看出，結合了深度學習技術的動態場景下的SLAM由于需要對圖片進行推理而消耗了大量時間，由于本文算法選用一階段網絡YOLOv5，相較于Dyna-SLAM及DS-SLAM則耗時較少。

5 結束語

本文結合深度學習技術對傳統的SLAM算法進行改進，通過對相機深度信息的利用完成目標區域內的前、背景分割及物體相關聯的判斷，最終在最大程度上剔除動態特征點，同時保留足夠的靜態特征點進行位姿估計。通過對傳統的ORB-SLAM3及同類的DS-SLAM算法進行實驗對比可以看出，本文算法不僅對于定位精度有所提高，同時實時性也較好。然而視覺SLAM中圖像易受光照、天氣影響，從而導致初始的位姿估計不夠準確，未來考慮結合其他傳感器，如激光雷達、里程計等，對SLAM算法作進一步研究。

參考文獻：

［1］Huang Shoudong，Dissanayake G. A critique of current developments in simultaneous localization and mapping ［J/OL］. International Journal of Advanced Robotic Systems，2016，13（5）： DOI：10.1177/1729881416669482.

［2］Cadena C，Carlone L，Carrillo H，et al. Past，present，and future of simultaneous localization and mapping： toward the robust-perception age ［J］. IEEE Trans on Robotics，2016，32（6）： 1309－1322.

［3］Pacheco J F，Ascencio J R，Mancha J M R，et al. Visual simultaneous localization and mapping： a survey ［J］. Artificial Intelligence Review，2015，43（1）： 55－81.

［4］Takafumi T，Hideaki U，Sei I. Visual SLAM algorithms： a survey from 2010 to 2016 ［J］. IPSJ Trans on Computer Vision and Applications，2017，9（6）：article No.16.

［5］權美香，樸松昊，李國. 視覺SLAM綜述 ［J］. 智能系統學報，2016，11（6）： 768－776. （Quan Meixiang，Piao Songhao，Li Guo. An overview of visual SLAM ［J］. CAAI Trans on Intelligent Systems，2016，11（6）： 768－776.）

［6］Mur-Artal R，Tardos J D. ORB-SLAM2： an open-source SLAM system for monocular，stereo and RGB-D cameras ［J］. IEEE Trans on Robotics，2017，33（5）： 1255－1265.

［7］Rublee E，Rabaud V，Konolige K，et al. ORB： an efficient alternative to SIFT or SURF ［C］// Proc of International Conference on Computer Vision.Washington DC： IEEE Computer Society，2011： 2564－2571.

［8］Lowe D G. Distinctive image feature from scale-invariant keypoints ［J］. International Journal of Computer Vision，2017，60（2）： 91－110.

［9］Bay H，Ess A，Tuytelaars T，et al. SURF： speeded up robust features ［C］// Proc of the 9th European Conference Computer Vision. Berlin： Springer，2006： 404－417.

［10］Engel J，Koltun V，Cremers D. Direct sparse odometry ［J］. IEEE Trans on Pattern Analysis and Machine Intelligence，2018，40（3）： 611－625.

［11］Forster C，Pizzoli M，Scaramuzza D. SVO： FAST semi-direct monocular visual odometry ［C］// Proc of IEEE International Conference on Robotics and Automation. Piscataway，NJ： IEEE Press，2014： 15－22.

［12］張順，龔怡宏，王進軍. 深度卷積神經網絡的發展及其在計算機視覺領域的應用 ［J］. 計算機學報，2019，42（3）： 453－482. （Zhang Shun，Gong Yihong，Wang Jinjun. Development of deep convolution neural network and its applications on computer vision ［J］. Chinese Journal of Computer，2019，42（3）： 453－482.）

［13］Bescos B，Facil J M，Civera J，et al. DynaSLAM： tracking，mapping，and inpainting in dynamic scenes ［J］. IEEE Robotics and Automation Letters，2018，3（4）： 4076－4083.

［14］He Kaiming，Gkioxari G，Dollar P，et al. Mask R-CNN ［C］// Proc of International Conference on Computer Vision. Washington DC： IEEE Computer Society，2017： 2980－2988.

［15］Xiao Linhui，Wang Jinge，Qiu Xiaosong，et al. Dynamic-SLAM： semantic monocular visual localization and mapping based on deep learning in dynamic environment ［J］. Robotics and Autonomous Systems，2019，117（7）： 1－16.

［16］Liu Wei，Anguelov D，Erhan D，et al. SSD： single shot multibox detector ［C］// Proc of the 14th European Conference on Computer Vision. Cham： Springer，2016： 21－37.

［17］方寶富，張旭，王浩. 聯合深度圖聚類與目標檢測的像素級分割算法 ［J］. 模式識別與人工智能，2022，35（2）： 130－140. （Fang Baofu，Zhang Xu，Wang Hao. Pixel-level segmentation algorithm combining depth map clustering and object detection ［J］. Pattern Re-cognition and Artificial Intelligence，2022，35（2）： 130－140.）

［18］楊俊闖，趙超. K-means聚類算法研究綜述［J］. 計算機工程與應用，2019，55（23）： 7－14. （Yang Junchuang，Zhao Chao. Survey on K-means clustering algorithm ［J］. Computer Engineering and Applications，2019，55（23）： 7－14.）

［19］Yu Chao，Liu Zuxin，Liu Xinjun，et al. DS-SLAM： a semantic visual SLAM towards dynamic environments ［C］// Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway，NJ： IEEE Press，2018： 1168－1174.

［20］Badrinarayanan V，Kendall A，Cipolla R，et al. SegNet： a deep convolutional encoder-decoder architecture for image segmentation ［J］. IEEE Trans on Pattern Analysis and Machine Intelligence，2017，39（12）： 2481－2495.

［21］Campos C，Elvira R，Rodriguez J J G，et al. ORB-SLAM3： an accurate open-source library for visual，visual-inertial，and multi-map SLAM ［J］. IEEE Trans on Robotics，2021，37（6）： 1873－1890.

［22］Elvira R，Tardos J D，Montiel J M M. ORBSLAM-Atlas： a robust and accurate multi-map systems ［C］// Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway，NJ： IEEE Press，2019： 6253－6259.

［23］Xiao Xinshuai，Tian Xiuxia. Research on reference target detection of deep learning framework Faster-RCNN ［C］// Proc of the 5th Annual International Conference on Data Science and Business Analytics. Piscataway，NJ： IEEE Press，2021： 41－44.

［24］Qu Zhong，Zhang Li. Research on image segmentation based on the improved Otsu algorithm ［C］// Proc of the 2nd International Confe-rence on Intelligent Human-Machine Systems and Cybernetics. Pisca-taway，NJ： IEEE Press，2010： 228－231.

［25］陳媛，胡娜，余秋月. 融合背景差分法和幀間差分法的運動目標檢測 ［J］. 現代計算機，2019（34）： 50－53. （Chen Yuan，Hu Na，Yu Qiuyue. Moving target detection using background difference method and inter-frame difference method ［J］. Modern Computer，2019（34）： 50－53.）