面向動態場景的ORB-SLAM3算法研究

劉超群　柯寶中　蔡洪煒

摘 要：傳統的視覺同步定位與地圖構建（simultaneous localization and mapping，SLAM）算法大多是基于靜態場景這一假設進行研究，在動態場景中容易出現定位精度不佳、穩定性差等情況。針對這一問題，本文以ORB-SLAM3算法為基礎，與YOLOv3目標檢測網絡相結合，在提取特征點的同時進行動態目標檢測，根據目標檢測結果剔除動態特征點；再利用對極幾何約束剔除殘留的動態特征點和誤匹配的特征點；最后利用剩余的特征點進行幀間匹配和位姿估計，并在TUM數據集上進行實驗驗證。結果表明：相比較于ORB-SLAM3，本文算法在高動態場景中的系統絕對軌跡誤差減少了85%以上，大大提高了定位精度；同時，本文算法跟蹤線程每幀圖像平均用時為75 ms左右，能夠滿足實時運行的需求。

關鍵詞：視覺SLAM；動態場景；ORB-SLAM3；YOLOv3；絕對軌跡誤差

中圖分類號：TP391.41 DOI：10.16375/j.cnki.cn45-1395/t.2023.04.012

0 引言

隨著人工智能技術的發展和應用，同步定位與地圖構建（simultaneous localization and mapping，SLAM）技術可以使機器人在未知的工作環境中確定自身位置和建立地圖，在智能移動機器人、增強現實（augmented reality，AR）技術和自動駕駛等領域中發揮著重要的作用，逐漸受到科技企業、高校和研究所的關注。根據傳感器類型的不同，可以將SLAM技術分為激光SLAM和視覺SLAM兩大類[1]。視覺SLAM使用相機傳感器，由于其價格低廉，獲取的環境信息更加豐富，逐漸受到研究者的青睞。

在視覺SLAM中，相機成像容易受光照變化及相機穩定性的影響，在復雜場景中僅依靠單一的視覺信息效果不顯著。采用多傳感器融合逐漸成為解決這一問題的較優解，可以提高SLAM系統的魯棒性和精度。Davison等[2]提出的MonoSLAM算法是第一個實時的SLAM算法，但不能在線運行，只能在離線時進行定位和建圖。Klein等[3]提出了使用非線性優化作為后端并多線程運行的PTAM算法，此算法僅在小場景中運行。Mur-Artal等[4]基于特征點具有快速方向性和旋轉不變性的特點提出了ORB-SLAM2算法。Campos等[5]開源了ORB-SLAM3算法，它相較于之前的算法增加了IMU融合算法和多地圖系統，這種算法是目前性能較為優異的算法之一。然而這些傳統的視覺SLAM大部分都是假設外部環境是靜態的，而對于動態場景，系統在進行特征點提取時會存在誤差，從而導致相機位姿估計精度降低。如何減少動態物體對于系統精度的干擾成為視覺SLAM的一個主流研究方向[6]。

伴隨著深度學習理論的快速發展，大量結合深度學習的SLAM算法被提出，基于視覺的同步定位與地圖構建（visual SLAM，VSLAM）進入一個全新的階段，即魯棒性-預測性階段。Bescós等[7]提出了DynaSLAM算法，該算法以ORB-SLAM2算法為基礎，依據語義分割和多視圖幾何的結果，將語義動態或者幾何動態的特征點剔除。Wu等[8]提出了一種YOLO-SLAM動態環境SLAM系統，利用YOLO算法來獲得語義信息，濾除檢測到的動態特征點。Yu等[9]以ORB-SLAM2為基礎提出DS-SLAM算法，選用SegNet網絡和運動一致性檢驗結合的方法進行動態特征點濾除。相較于傳統的VSLAM，結合深度學習理論的算法剔除動態特征點效果更好，定位精度也能大大提高。因此，本文以ORB-SALM3算法為基礎增加語義信息，主要內容如下：

1）增加一條獨立的動態目標檢測線程，結合ORB-SLAM3視覺里程計，使用YOLOv3進行端對端目標檢測，有效提取動態圖像特征。

2）依據動態目標檢測結果剔除動態特征點，殘留的動態特征點和誤匹配的特征點使用對極幾何約束濾除；利用剩下的特征點進行幀間匹配和位姿估計，在TUM數據集上對本文所提算法的定位精度和實時性進行評估。

1 算法框架

本文算法以ORB-SLAM3算法為主體框架，在視覺里程計的前端加入一個動態目標檢測線程，通過動態目標檢測網絡YOLOv3分割圖像中的靜態和動態特征，將動態目標視為離群值，增加系統的魯棒性。根據目標檢測的結果，將動態特征范圍內的特征點剔除，再利用對極幾何約束濾除殘留的動態特征點和誤匹配的點，利用完全過濾后的靜態特征點進行位姿估計和非線性優化操作，減少動態特征點對于系統的影響。改進的視覺里程計框架如圖1所示。

1.1 ORB-SLAM3系統概括

ORB-SLAM3是一種基于特征點法的視覺里程計系統，其充分利用了圖像特征點對光照強度和環境尺度的穩健性，在室內和室外環境中的表現都較為良好。主要由以下幾部分組成[5]：

1）跟蹤線程的主要作用是為活動的地圖提供新的關鍵幀。對輸入系統中的圖像進行ORB特征點提取，對相鄰2幅圖像進行特征點匹配，使用最小化重投影誤差確定當前圖像幀相機的初始位姿，利用初始值進行最小化重投影誤差，盡可能使多的特征點與地圖點相匹配。

2）局部建圖線程主要是在活動的地圖中添加關鍵幀的位姿以及新的地圖點，刪除冗余數據，利用滑動窗口進行BA優化，更新地圖數據。當初始化完成后，若關鍵幀跟蹤成功，則利用預積分誤差和最小化重投影誤差進行關鍵幀的優化，最終剔除冗余的關鍵幀。

3）經過篩選后的關鍵幀會進入回環檢測和地圖合并線程。檢測活動地圖與其他地圖存在的共有區域，檢測是否存在回環或者融合等情況，若存在，則使用相似變換合并地圖和回環矯正。在優化完成后，打開另一個線程進一步更新地圖，通過在回環檢測的同時合并共視地圖，可以保障系統的實時性。

1.2 YOLOv3目標檢測原理

YOLOv3是一個可以實現端對端目標檢測的算法，它的主干網絡由Darknet-53組成，通過調節不同的卷積步長控制特征圖尺寸，借助金字塔思想，用小尺寸特征圖檢測大尺寸物體，大尺寸特征圖用于檢測小尺寸物體[10]。

在前向進程中，YOLOv3網絡依據不同的輸入尺寸，輸出3個不同尺寸的特征圖：[y1、y2、y3]。輸出特征圖的維度為

其中：[m]表示矩形框坐標參數，主要由矩形框中心坐標[（x， y）]以及矩形框寬和高組成；[l]表示預測矩形框坐標的偏移量；[g]表示默認框和與之相匹配的真實框之間的偏移量；[bx、by、bw、bh]表示預測目標矩形框參數；[cx、cy、pw、ph]表示默認矩形框參數；[gx、gy、gw、gh]表示其匹配到的實際目標框參數[12]。

在對數據集完成動態目標檢測后，將物體分為3類，即高動態物體、中動態物體、靜態物體。只有當特征點位于高動態物體框內且不存在于靜態物體框內時，認為此特征點是一個動態特征點，并剔除這些動態特征點[13]。

1.3 對極幾何約束

在經過上述動態特征點濾除后，數據集中可能存在動態物體被大面積遮擋、動態特征點漏檢的情況，需要進一步濾除特征點。對相鄰2幀圖像中的剩余特征點進行特征匹配，計算兩特征點之間的漢明距離，選取距離最小的點為匹配點[14]。為了減小由于暴力匹配產生的誤差，按照已匹配點對之間的漢明距離進行升序排列，選取排序靠前的1/3點對求解基礎矩陣。隨機抽樣一致（random sample consensus，RANSAC）算法的具體操作步驟如下：

1）隨機在匹配成功的點對中選取4組，求得基礎矩陣F。

2）計算剩余特征點到矩陣F所對應極線的距離[a]，并與設定好的閾值[an]進行比較。當[aan]時，認為該特征點是外點，并且記錄內點的數目[I]。

3）重復執行上述步驟[N]次，選擇內點數目[I]最大的一次，將此次求得的基礎矩陣當作最終的基礎矩陣。

在求取基礎矩陣之后對所有的匹配點進行檢驗，判斷是否滿足對極幾何約束。

對極幾何約束示意圖如圖2所示。

在視覺SLAM系統需要用金字塔對圖像進行不同層次的降采樣處理。閾值d的取值與置信度、圖像金字塔層數緊密相關，當距離[d]超過給定的范圍，則認定該特征點是動態的或者誤匹配的點，予以剔除。

在動態場景中，由于存在大量的動態特征點，會造成相機位姿估計偏差[15]。本文所提出的方案使用YOLOv3算法進行目標檢測，將被判定為動態特征點的點進行剔除；再利用對極幾何約束剔除殘留的動態特征點和誤匹配的特征點，進而增加定位的精度。

2 實驗結果及分析

2.1 實驗數據集和環境

本文實驗中使用的設備為ThinkPadE560筆記本電腦，CPU型號為英特爾i5-6200U，主頻2.4 GHz，8 G內存，操作系統為Ubuntu20.04。選用公開的TUM RGB-D動態數據集中的6組數據進行實驗，TUM數據集由德國慕尼黑工業大學采集制作，包含了由深度相機采集到的彩色圖像、深度圖像以及由8個高速追蹤相機獲取到的真實位姿等數據。本次實驗選用的動態場景分別為freiburg2_desk_with_person、freiburg3_sitting_static、freiburg3_sitting_xyz、freiburg3_walking_halfsphere、freiburg3_walking_static、freiburg3_walking_xyz等6組數據集，部分數據集如圖3所示。其中在sitting序列中展示的是桌子前面有2個人，他們的肢體或者桌上的物品存在輕微移動；在walking序列中展示的是2個人在桌子周圍進行大幅度的移動；在static序列中，相機相對靜止；xyz序列中，相機在x、y、z方向上移動。

2.2 動態特征點剔除

選用COCO數據集進行YOLOv3網格的訓練。COCO數據集中包含80個類別，這種檢測算法輸出3個特征圖用以檢測數據集中的大中小型物體，例如數據集中的人、椅子、鼠標等動態物體。當對極幾何約束中給定閾值過小時，會造成剔除的誤匹配和動態特征點過多，余下的靜態特征點過少，不利于后續的相機位姿估計。當給定閾值過大時，動態特征點濾除效果會變差，后續相機位姿估計中有太多的誤匹配和動態特征點，降低定位的精度。本文對極幾何約束的閾值選為金字塔第[n]層，置信度取95%時，閾值[d]的約束條件為[d<3.84×1.22n]，在這個范圍內的特征點被保留，則動態特征點剔除前后效果圖如圖4所示。

該圖例選自TUM數據集中的freiburg3_walking_xyz動態場景，在此場景中2個人圍繞桌子運動。當圖像中存在動態目標時，ORB-SLAM3算法對動態目標不作處理，將特征點一起進行位姿估計，而本文的算法剔除了運動中人邊界框中的特征點，剩下的靜態特征點用于進行位姿估計。圖4（a）是ORB-SLAM3系統特征提取效果圖，圖4（b）是增加了YOLOv3線程之后的特征點提取效果圖。對比圖4（a）、（b）可以看出，處于動態目標范圍內的大部分動態特征點已經被剔除，由于圖中右側人手邊的筆記本屬于中動態物體，因此這部分特征點被保留。

2.3 定位精度分析

本文選擇的是在傳統ORB-SLAM3視覺前端添加一個深度學習模塊YOLOv3，并剔除動態特征點。此次實驗主要對改進方案的定位精度和實時性進行評估。SLAM系統精度的評價指標為絕對軌跡誤差（absolute trajectory error，ATE），能夠較為直觀地反映相機軌跡的精度，通過不同時間戳下真實位姿和估計位姿之間的均方根誤差來表示[8]，計算公式如式（13）所示，

式中：[eATE]為絕對軌跡誤差，[ei]為真實位姿和估計位姿李代數的差，[T-1gt， i]為數據集中真實的軌跡，[Testi， i]為SLAM系統估計的軌跡。本文使用均方根誤差（RMSE）、平均值（mean）和標準差（std）這3個參數來反映系統的性能，平均值反映估計位姿的精度，均方根誤差和標準差體現系統的魯棒性[16-17]。

本文選用目前性能較為優異的ORB-SLAM3算法進行對比。在經過數據集的測試后，兩者的絕對軌跡誤差和性能提升率如表1所示。

從表1中可以看出，在高動態場景中，本文所提方案的均方根誤差、平均值和標準差都有較大程度的提升，在walking_half、walking_static、walking_xyz這3個走動序列中，系統精度至少能提升85%以上。在這3個序列中，由于人走動較為頻繁，動作幅度過大，本文算法在對特征點進行篩選時，剔除了大多數的動態特征點，選用靜態的特征點進行位姿估計。相較于ORB-SLAM3算法，本文算法在動態場景中的適應性更好，定位精度更高。在sitting系列場景中，與ORB-SLAM3算法相比較，本文算法提升效果不明顯，甚至在sitting_xyz序列中，本文算法的精度略微下降，均方根誤差下降了11.11%。這是由于在進行閾值判斷剔除動態特征點時刪除了部分靜態特征點，影響了定位的精度?？傮w而言，本文的方案相較ORB-SLAM3算法仍然有所提升。

圖5和圖6分別為walking_xyz和sitting_xyz序列在ORB-SLAM3和本文算法下的預估相機軌跡和真實軌跡的對比圖。從圖中可以看出，ORB-SLAM3在一個地點旋轉角度過大時會產生較大偏移，受到大量的動態特征干擾時，會產生明顯的漂移，相比較之下，本文的算法在動態場景中有著較好的表現。

2.4 實時性評估

實時性對于SLAM系統是否具有實用性非常重要。為了驗證本文算法的實時性，在本次實驗中選用的是跟蹤線程中處理每幀圖像所花費的時間[18]。與DynaSLAM算法和DS-SLAM算法進行對比，對照實驗在walking_xyz序列中進行，對比結果如表2所示。由表2可知，本文算法的平均跟蹤時間為75 ms，大體上可以滿足系統實時性的要求。雖然這幾種方案都是采用融合深度學習的方式，但由于本文采用快速檢測網絡YOLOv3進行動態目標檢測，相比較于DynaSLAM算法使用Mask-RCNN、DS-SLAM算法使用SegNet網絡，本文算法在速度上有較大優勢[19]。通過對比驗證了本文算法能夠較好地滿足定位精度和實時性要求。

3 結論

本文構造了一種面向動態場景的視覺里程計，以ORB-SLAM3為基礎框架，增加了一條獨立的目標檢測線程，選用YOLOv3網絡檢測動態目標，剔除位于高動態物體框內且不存在于低動態物體框內的動態特征點，以防動態特征點剔除不完全；再次利用對極幾何約束進一步剔除動態特征點和誤匹配的特征點。通過在TUM數據集上的實驗表明，本文算法在定位精度和實時性方面有較大提升。在后續的研究中，將會嘗試加入三維語義地圖，以便此系統能夠應用到機器人導航中。

參考文獻

[1] 李浩東，陶鈞，劉辰宇，等. 基于ORB-SLAM框架的直接法改進與對比[J]. 測繪地理信息，2022，47（S1）：211-215.

[2] DAVISON A J，REID I D，MOLTON N D，et al. MonoSLAM：real-time single camera SLAM[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence，2007，29（6）：1052-1067.

[3] KLEIN G，MURRAY D. Parallel tracking and mapping for small AR workspaces[C]//6th IEEE and ACM International Symposium on Mixed and Augmented Reality，2007：225-234.

[4] MUR-ARTAL R，TARD?S J D. ORB-SLAM2：an open-source SLAM system for monocular，stereo，and RGB-D cameras[J]. IEEE Transactions on Robotics，2017，33（5）：1255-1262.

[5] CAMPOS C，ELVIRA R，RODRIGUEZ J J G，et al. ORB-SLAM3：an accurate open-source library for visual，visual-inertial，and multimap SLAM[J]. IEEE Transactions on Robotics，2021，37（6）：1874-1890.

[6] 李博，段中興. 室內動態環境下基于深度學習的視覺里程計[J/OL]. 小型微型計算機系統，2022：1-9[2022-11-25]. http：//kns. cnki. net/kcms/detail/21. 1106. TP. 20211019. 1809. 020. html.

[7] BESC?S B，F?CIL 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.

[8] WU W X，GUO L，GAO H L，et al. YOLO-SLAM：a semantic SLAM system towards dynamic environment with geometric constraint[J]. Neural Computing and Applications，2022，34（8）：6011-6026.

[9] YU C，LIU Z X，LIU X J，et al. DS-SLAM：a semantic visual SLAM towards dynamic environments[C]//Proceedings of the 25th IEEE/RSJ International Conference on Intelligent Robots and Systems，IEEE，2018：1168-1174.

[10] 吳靜，王智文，王康權，等. 基于改進CRNN算法的電動自行車車牌識別研究[J]. 廣西科技大學學報，2022，33（4）：63-69.

[11] 張政，林志誠. 基于ORB-SLAM2的四足機器人定位和建圖[J]. 工業控制計算機，2022，35（10）：78-80，84.

[12] 徐武，高寒，王欣達，等. 改進ORB-SLAM2算法的關鍵幀選取及地圖構建研究[J]. 電子測量技術，2022，45（20）：143-150.

[13] 陸佳嘉. 面向室內復雜環境的無人車同時定位與建圖研究[D]. 南京：南京信息工程大學，2022.

[14] 喬亞坤，林川，張貞光. 基于深度學習的輪廓檢測模型的交互式解碼網絡[J]. 廣西科技大學學報，2022，33（1）：46-53.

[15] 張杰. 動態環境下視覺慣性定位方法研究[D]. 西安：西安工業大學，2022.

[16] 馮一博，張小俊，王金剛. 適用于室內動態場景的視覺SLAM算法研究[J]. 燕山大學學報，2022，46（4）：319-326.

[17] 張昊宇，柳祥樂，王思山. 基于深度學習特征提取的改進ORB-SLAM3算法[J]. 湖北汽車工業學院學報，2022，36（2）：55-59.

[18] 伍曉東，張松柏，湯適榮，等. 基于改進關鍵幀選擇的ORB-SLAM3算法[J/OL]. 計算機應用研究，2023：1-7[2023-02-04]. https：//doi.org/10.19734/j.issn.1001-3695. 2022. 09. 0480.

[19] 江浩瑋. 基于改進Mask-RCNN的SLAM算法研究[D]. 蕪湖：安徽工程大學，2022.

Research on ORB-SLAM3 algorithm for dynamic scenes

LIU Chaoqun1， KE Baozhong*1， CAI Hongwei2

（1. School of Automation， Guangxi University of Science and Technology， Liuzhou 545616， China; 2. School of Information Science and Engineering， Liuzhou Institute of Technology， Liuzhou 545616， China）

Abstract： Most of the traditional visual SLAM algorithms are based on the assumption of static scenes， which is prone to poor localization accuracy and stability in dynamic scenes. To address these problems， based on the ORB-SLAM3 algorithm， combined with the YOLOv3 target detection network， dynamic target detection is performed while feature points are extracted， and dynamic feature points are rejected according to the target detection results; then the pairwise polar geometric constraints are used to reject the residual dynamic feature points and mismatched feature points; and finally the remaining feature points are used for inter-frame matching and bit pose estimation. The final experimental validation on the TUM dataset shows that compared with ORB-SLAM3， this algorithm reduces the absolute trajectory error of the system by more than 85% in highly dynamic scenes， which greatly improves the localization accuracy. At the same time， the average time spent per image frame by the tracking thread of this algorithm is around 75 ms， which can meet the requirements of real-time operation.

Key words： visual SLAM; dynamic scene; ORB-SLAM3; YOLOv3; absolute trajectory error

（責任編輯：黎 婭）

收稿日期：2022-12-13

基金項目：2022年度廣西高校中青年教師科研基礎能力提升項目（2022KY1704）；2019年度廣西科技大學鹿山學院科學基金項目（2019LSKY09）；2020年自治區級大學生創新創業訓練計劃項目（202013639028）資助

第一作者：劉超群，在讀碩士研究生

*通信作者：柯寶中，學士，高級工程師，研究方向：自動控制，E-mail：64285076@qq.com