劉超群　柯寶中　蔡洪煒

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

關(guān)鍵詞：視覺SLAM；動態(tài)場景；ORB-SLAM3；YOLOv3；絕對軌跡誤差

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

0 引言

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

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

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

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

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

1 算法框架

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

1.1 ORB-SLAM3系統(tǒng)概括

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

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

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

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

1.2 YOLOv3目標檢測原理

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

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

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

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

1.3 對極幾何約束

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

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

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

3）重復執(zhí)行上述步驟[N]次，選擇內(nèi)點數(shù)目[I]最大的一次，將此次求得的基礎(chǔ)矩陣當作最終的基礎(chǔ)矩陣。

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

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

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

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

2 實驗結(jié)果及分析

2.1 實驗數(shù)據(jù)集和環(huán)境

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

2.2 動態(tài)特征點剔除

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

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

2.3 定位精度分析

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

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

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

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

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

2.4 實時性評估

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

3 結(jié)論

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

參考文獻

[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] 李博，段中興. 室內(nèi)動態(tài)環(huán)境下基于深度學習的視覺里程計[J/OL]. 小型微型計算機系統(tǒng)，2022：1-9[2022-11-25]. http：//kns. cnki. net/kcms/detail/21. 1106. TP. 20211019. 1809. 020. html.

[7] BESC?S B，F(xiàn)?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] 吳靜，王智文，王康權(quán)，等. 基于改進CRNN算法的電動自行車車牌識別研究[J]. 廣西科技大學學報，2022，33（4）：63-69.

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

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

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

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

[15] 張杰. 動態(tài)環(huán)境下視覺慣性定位方法研究[D]. 西安：西安工業(yè)大學，2022.

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

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

[18] 伍曉東，張松柏，湯適榮，等. 基于改進關(guān)鍵幀選擇的ORB-SLAM3算法[J/OL]. 計算機應(yīng)用研究，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年度廣西高校中青年教師科研基礎(chǔ)能力提升項目（2022KY1704）；2019年度廣西科技大學鹿山學院科學基金項目（2019LSKY09）；2020年自治區(qū)級大學生創(chuàng)新創(chuàng)業(yè)訓練計劃項目（202013639028）資助

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

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