基于目標檢測和多視圖幾何的動態SLAM算法

摘" 要： 目前大多數的SLAM系統主要針對靜態場景，然而，在實際環境中不可避免地存在許多動態對象，這將大大降低算法的魯棒性和相機的定位精度。針對動態對象造成的軌跡偏差問題，文中提出一種結合目標檢測網絡和多視圖幾何結構的動態SLAM算法。首先，基于YOLOv5算法框架，將骨干網絡CSPDarkNet?53替換為輕量型L?FPN（Lite?FPN）結構，并使用VOC2007數據集進行預訓練。與YOLOv5s原始模型相比，新網絡的計算量減少了45.73%，檢測速率提高了31.90%；然后，將檢測物體劃分為高動態對象、中動態對象以及低動態對象，利用多視圖幾何方法計算閾值，并根據閾值對中高動態對象進行二次檢測，以決定是否剔除預測框中的特征點；最后，在TUM數據集上的實驗結果顯示，該方法在定位精度上平均提升了82.08%，證明了其在準確性方面的顯著改進。

關鍵詞： 同步定位與地圖構建； 動態環境； 多視圖幾何結構； 目標檢測； 特征點； 輕量型

中圖分類號： TN911.73?34； TP391.41" " " " " " " " "文獻標識碼： A" " " " " " " " "文章編號： 1004?373X（2025）01?0135?09

Dynamic SLAM algorithm based on object detection and multi?view geometry

YU Qingcang， DONG Genyang， FANG Caiwei， SUN Shusen

（School of Computer Science and Technology （School of Artificial Intelligence）， Zhejiang Sci?Tech University， Hangzhou 310018， China）

Abstract： Nowadays， most SLAM （simultaneous localization and mapping） systems mainly focus on static scenes. However， there are many dynamic objects inevitably in the real environment， which will greatly reduce the robustness of the algorithm and the positioning accuracy of the camera. Therefore， a dynamic SLAM algorithm combining object detection network and multi?view geometric structure is proposed to get rid of the trajectory deviation caused by dynamic objects. On the basis of the framework of YOLOv5 algorithm， the backbone network CSPDarkNet?53 is replaced with a lightweight L?FPN （lightweight feature pyramid network） structure， and the dataset VOC2007 is used for pre?training. The parameters of the proposed network is reduced by 45.73%， and its detection rate is increased by 31.90% in comparison with those of the original model YOLOv5s. Then， the detected objects are categorized into high dynamic objects， medium dynamic objects and low dynamic objects. The multi?view geometric method is used to calculate the threshold value， and the medium and high dynamic objects are detected twice based on the threshold value， so as to decide whether to eliminate the feature points in the prediction frame. The experimental results on the dataset TUM show that the positioning accuracy of the proposed method is improved by 82.08% on average， demonstrating significant improvement in accuracy.

Keywords： SLAM; dynamic environment; multi?view geometry; object detection; feature point; lightweight

0" 引" 言

隨著研究的不斷深入，同步定位與建圖（Simultaneous Localization And Mapping， SLAM）的應用場景變得愈加廣泛[1]。在復雜的環境中，例如室內、隧道等GPS信號弱的地方，SLAM技術可以利用自身傳感器獲得感知信息，遞增地創建一個與周圍環境相一致的地圖，并利用創建的地圖實現自主定位[2?3]。SLAM的實現途徑根據使用的傳感器不同，主要分為聲吶SLAM、激光SLAM和視覺SLAM（VSLAM），相較于激光雷達，相機具有信息量大、靈活性高、成本低等優點[4]。雖然VSLAM在靜態場景中已經表現出了優異的性能，例如ORB?SLAM2[5]、ORB?SLAM3[6]。但由于靜態場景這一假設，當有大量動態對象存在于目標場景中時，視覺SLAM算法的性能會顯著下降，這也限制了其在實際動態環境中的應用[7]。因此，如何在保證實時性的前提下，對處于場景中的動態目標進行檢測和剔除是當前的主要挑戰。為了識別環境中的動態目標，研究主要分為三類。

1） 基于多視圖幾何來實現動態點的剔除，文獻[8]提出了一種稀疏運動去除（Sparse Motion Removal， SMR）模型，利用當前幀與參考幀之間的差異以及連續幀之間的相似性過濾動態特征，但當幀之間的差異過大時，容易出現跟蹤丟失的情況。文獻[9]提出了一種利用相鄰幀間差異來檢測運動對象的在線運動去除方法。

2） 通過深度學習對輸入的每幀圖像進行處理，獲得其語義信息并剔除其中的動態物體。文獻[10]基于ORB?SLAM3，提出使用SparseInst網絡[11]對圖像幀進行語義分割來獲取語義標簽和動態對象掩膜，實現動態對象的去除。文獻[12]提出了將ORB?SLAM2和SegNet相結合起來的DS?SLAM。該系統首先使用移動一致性檢驗來判斷潛在的離群值，然后結合語義分割結果剔除離群值。文獻[13]提出了YOLO?SLAM，利用YOLOv3檢測可移動目標，然后根據深度RANSAC方法將特征細分為動態特征和靜態特征。但當相機范圍內存在大量動態對象時，定位精度會顯著下降。

3） 基于多視圖幾何和深度學習融合的方法。文獻[14]設計了一種自適應語義分割算法對ORB?SLAM2的跟蹤線程進行改進，并使用全局BA算法優化相機位姿。文獻[15]提出了Blitz?SLAM，利用BlitzNet獲得原始掩膜，然后結合深度信息和幾何信息對動態特征點進行過濾。同時，他們還提出了一種去除點云圖中動態對象創建的噪聲塊的方法，但在實時性方面相對較差。

為解決實際場景中的動態對象問題，本文采用多視圖幾何和深度學習結合的思想，提出了一種輕量級的動態實時SLAM系統。

本文的主要貢獻如下。

1） 提出了一種基于ORB?SLAM2的動態SLAM系統，驗證了在TUM數據集下YG?SLAM（YOLO and Multi?view Geometry SLAM）算法的有效性。

2） 將目標檢測線程作為一個單獨的線程，為基于對象運動層次劃分的輕量級目標檢測網絡提供先驗知識，同時引入多視圖幾何方法，對特征點進行二次判斷，增加其可靠性。

3） YOLOv5原本的CSPDarkNet?53網絡被輕量級的L?FPN網絡取代，用于減少目標識別的時間開銷，從而提高整個系統的實時性能。

1" YG?SLAM系統

1.1" 算法流程

在ORB?SLAM2的基礎上，加入目標檢測線程和多視圖幾何方法，圖1描述了該系統的整體框架圖。首先，相機對圖像數據進行收集；然后，傳遞給跟蹤線程進行預處理，通過輕量級的LG?YOLOv5s算法識別所有的先驗動態對象，同時利用多視圖幾何線程對圖像中的動態特征點進行再識別；其次，將LG?YOLOv5s算法的識別結果與基于多視圖幾何方法得到的運動狀態信息相結合，用于提取動態對象區域；最后，去除動態對象區域的特征點，將只有靜態特征的幀輸入到跟蹤線程中和映射構造線程中，以提高SLAM系統的精度和魯棒性。

1.2" 目標分類

為了識別圖像中物體的狀態，將其分為高動態對象、中動態對象、低動態對象三類。

高動態對象主要是人和動物，他們能進行自主的移動；中動態對象主要是椅子、書、鍵盤等小型可以輕易移動的物體；低動態對象主要是冰箱、貨物柜、電視這些大型不易移動的物體。對于低動態對象，在進行處理時不加入多視圖幾何方法的判斷，以減少時間開銷。

1.3" 輕量級LG?YOLOv5s網絡

YOLOv5是一種基于深度學習的目標檢測算法，網絡結構主要由四個部分組成：輸入（Input）、主干網絡（Backbone）、頸部網絡（Neck）和頭部網絡（Head）。數據在輸入主干網絡之前先進行預處理，包括Mosaic數據增強。接著從主干網絡處理后的圖像中提取相關特征，然后頸部網絡對提取的特征進行融合，產生三種不同的特征大?。捍螅?6×76×255）、中（38×38×255）和?。?9×19×255）。最后將這些融合的特征指向檢測頭，并通過檢測過程生成輸出結果。YOLOv5設計了兩種CSP結構，分別應用于主干網絡（Backbone）和頸部網絡（Neck）中，CSP結構的核心思想是將主干網絡的特征圖分為兩個路徑，并跨階段進行部分特征融合，以此來提高特征表達的準確性和有效性。但由于CSP結構需要額外的參數來建立主路徑和跨階段路徑之間的連接和融合操作，造成了計算復雜度的增加和模型參數量的增加，存在過擬合的風險。為保證系統在參數減少的情況下，仍具有較高的準確性，本文設計了一種輕量級的L?FPN（Lightweight Feature Pyramid Network）進行代替。

L?FPN基于FPN網絡結構進行實現，使用Ghost模塊（GhostConv+C3Ghost）代替FPN中普通的[3×3]卷積，減少計算量，實現同時檢測圖像中的多個物體并提供它們的位置和類別信息，將其稱為LG?YOLOv5s（Light Ghost YOLOv5），其結構圖如圖2所示。

自底向上進行特征提取，逐漸減小特征圖的尺寸，同時提取更加抽象的語義信息。接著自頂向下將較低分辨率的特征圖進行上采樣操作，使其與較高分辨率的特征圖具有相同的尺寸，用于恢復丟失的細節。在自頂向下的過程中，FPN通過特征融合操作將上采樣的特征圖與對應分辨率自底向上的特征圖進行融合，以綜合利用不同尺度上的特征信息，圖3為特征提取過程。

GhostConv模塊將普通卷積分為兩部分，分別是一個普通的1×1卷積和一個深度可分離卷積。其中：1×1卷積的作用是進行特征整合，生成輸入特征層的特征濃縮；深度可分離卷積是利用上面的這一結果生成Ghost特征圖，之后將特征圖與特征層進行堆疊，獲得輸出特征層，完成Ghost模塊的構建，如圖4所示。

使用VOC2007數據集對模型進行訓練，數據集主要包含人、鍵盤、顯示器、鼠標等80個類別。重要超參數設置如下：[lr0]（初始學習率）為0.01，momentum（學習率動量）為0.937，weight_decay（權重衰減系數）為0.000 5，box（IoU損失系數）為0.05，cls（cls損失系數）為0.5，IoU_[t]（IoU訓練時間閾值）為0.20，batch?size（批處理大?。?，訓練周期是100輪。

YOLOv5s和LG?YOLOv5s在訓練后得到的結果如表1所示。

在參數量上，LG?YOLOv5s比傳統YOLOv5s減少46.06%，計算量減少45.73%，通過犧牲部分精度來提高系統的運行速度。

1.4" 多視圖幾何方法

多視圖幾何方法描述了來自同一個場景的不同圖像如何從多個角度相互投影到彼此。本文通過多幀之間圖像的位置約束去除具有較大誤差的特征點。根據3D點[X]，對每一個關鍵點[x]計算前一個關鍵幀到當前關鍵幀的投影點[x']和投影深度[dh]。為此，需要給定前一個關鍵幀的位姿[Tcwprev]和當前關鍵幀的位姿[Tcwcurr]，計算從前一個關鍵幀到當前關鍵幀的相對變換[Tprev_curr]。

[Tprev_curr=Tcwcurr?T-1cwprev，" " Tcw=Rt0T1] （1）

式中：[Tcw]是相機位姿（世界坐標系到相機坐標系的變換）；[R]是3×3旋轉矩陣；[t]是3×1平移向量。

將[X]從前一個關鍵幀的相機坐標系變換到當前關鍵幀的相機坐標系。設[x]在關鍵幀[kf]的相機坐標系中的坐標為[Pprev]，在當前關鍵幀的相機坐標系中的坐標為[Pcurr]。

[Pcurr=Rprev_currPprev+tprev_curr] （2）

式中：[Pcurr=[Xcurr，Ycurr，Zcurr]T]；[Rprev_curr]和[tprev_curr]分別是相對變換的旋轉矩陣和平移向量。

將變換后的[Pcurr]投影到當前關鍵幀平面上[x']，設相機內參矩陣為[J]，則有：

[x'=JXcurr ZcurrYcurr Zcurr1] （3）

[J=fx0cx0fycy001] （4）

式中：[fx]、[fy]、[cx]和[cy]是相機的內參系數。

接著計算由關鍵點[x]、投影點[x']以及3D點[X]形成的角度[p]。如果[p]大于某個閾值，則認為該點被阻塞，不做處理。在本文中設置閾值為25°。在TUM數據集中，對于視差角大于25°的對象則判定為是動態的。[x']與深度值[d']相對應，在誤差允許的范圍內，將其與[dh]進行比較，如果[x']超過某個閾值也被認為是動態對象。深度值[dh]是通過將關鍵點[x]從圖像坐標系投影到相機坐標系中的值。投影深度用于檢測不同幀之間物體的位置變化。對于靜態對象，相鄰幀之間的變化小，相對穩定，而動態對象的投影深度由于位置的不同會發生明顯的變化，具體如圖5所示。

通過比較關鍵點的投影深度，確定一個點是否對應于一個動態對象。

[Δd=dh-d'] （5）

如果[Δd]超過設定閾值[δd]，則認為[x']區域為動態對象。本文設定的[δd]=0.4 m，通過手動標記TUM數據集中50幅圖像的動態對象，評估在不同閾值下的精度（[precision]）和召回率（[recall]）。

[measuremax=0.7×precision+0.3×recall] （6）

根據式（6）得到最佳閾值[δd]=0.4 m。

1.5" 融合去除

使用LG?YOLOv5s網絡對目標檢測框中的動態特征點進行標記，并采用多視圖幾何法進行二次驗證。如果多視圖幾何方法判斷位于目標檢測框中的特征點也為動態特征點，則最終確定為動態特征點，否則為靜態特征點。對于目標檢測框中沒有特征點的區域，不做處理。

LG?YOLOv5目標檢測網絡對圖像進行預測后，生成的邊界框位置參數分別為[x]、[y]、[w]、[h]，[x]和[y]表示相對于原始圖像的中心坐標，[w]和[h]表示相對于原始圖像的長度和寬度。以檢測框[K]為例，將位置參數轉換為原始圖像下的左上角和右下角坐標[XK1，YK1]，[XK2，YK2]，在圖6中，黑點為靜態特征點，白點為動態特征點，多視圖幾何線程用于識別和消除動態特征點，[XK1，YK1]和[XK2，YK2]依據目標檢測和FAST角點檢測共同獲得。

預測框中包含的ORB特征點是由先驗知識判斷的未定性動態特征點，預測框外的特征點均為靜態特征點。假設所有特征點集合[R={r1，r2，…，rn}]，未定性動態特征點集合[P={p1，p2，…，pn}]，靜態特征點集合[Q={q1，q2，…，qn}]，則[R=P∪Q]，其中集合[P]中的所有特征點將參與ORB?SLAM2前端的動態特征點篩選以及坐標位置計算。

具體判斷流程如圖7所示。

實驗結果如圖8所示，圖像中的中高動態物體被框出，由于中動態物體在相鄰兩幀圖像上的幾何位置未超出設定閾值，保留預測框內的特征點。

2" 實" 驗

2.1" 實驗設備

實驗采用的電腦配置為Intel[?] CoreTM i5?8300H CPU@2.3 GHz，NVIDIA GeForce GTX 1050Ti，16 GB內存，系統版本Ubuntu 20.04，深度學習框架采用YOLOv5。

2.2" 衡量指標

采用EVO軌跡評價工具繪制相機的軌跡，并使用估計軌跡與真實軌跡的絕對軌跡誤差（Absolute Trajectory Error， ATE）和相對軌跡誤差（Relative Trajectory Error， RTE）對旋轉漂移進行全面評估。評估指標有均方根誤差（RMSE）、標準差（STD）、誤差平均數（MEAN）、最小值（MIN）、最大值（MAX）、中位數（MEDIAN）。

[Ei=（R-1tRt+1）-1（P-1tPt+1）] （7）

式中：[Ei]表示[i]時刻的相對軌跡估計誤差；[Rt]表示[t]時刻的真實位姿；[Pt]表示[t]時刻的估計軌跡；1表示時間間隔為1 s。

均方根誤差：計算不同時間戳下真實位姿和估計位姿之間的均方根誤差，以此來評估運行前后的姿態差，反映系統的魯棒性，計算公式為：

[RMSE=1ni=1ntrans（Fi）212] （8）

式中：[n]表示軌跡點的數量；[Fi]表示[i]時刻相對位姿誤差估計；[transFi]表示位姿誤差分量，該值越小，代表擬合效果越好。

中位數：反映系統整體的中心位置，衡量估計位姿的離群值。當圖像幀總數為奇數時，中位數計算如式（9）所示：

[MEAN=trans（Fi）212n+12] （9）

當圖像幀總數為偶數時，中位數計算如式（10）所示：

[MEAN=12trans（Fi）212n2+trans（Fi）212n2+1] （10）

2.3" 數據集

數據集使用德國慕尼黑工業大學的TUM RGB?D SLAM數據集來評估上述系統在動態環境下的性能。該數據集被廣泛認可為衡量SLAM系統表現的基準，其由不同室內場景中的39個序列組成，每個數據集都包含多個數據，可用于多個任務的性能測試。

TUM序列中所有圖像尺寸為[640×480]。動態對象數據集包含3個序列，其中fr1、fr2為靜態場景數據集，fr3為動態場景數據集。本文利用fr3序列下的4個高動態數據集，即fr3_w_[xyz]、fr3_w_static、fr3_w_rpy、fr3_w_halfsphere進行實驗，其中w表示行走的狀態。

動態對象序列展示了如下四種相機運動類型：

1） [xyz]，相機沿著[x]、[y]和[z]軸移動，同時保持其初始方向。

2） [rpy]，表示相機在相同位置繞固定軸進行旋轉。

3） static，相機保持靜止。

4） halfsphere，相機在直徑約1 m的小半球體上運動。

2.4" 實驗結果

為了比較YG?SLAM系統與原始ORB?SLAM2系統的性能，在上述動態數據集上進行實驗，并根據衡量指標客觀評價YG?SLAM系統在高動態環境下的魯棒性和準確性。

利用ORB?SLAM2、DynaSLAM與YG?SLAM進行對比，得到的結果如表2所示。

圖9為ORB?SLAM2算法和YG?SLAM算法在fr3_w_[ xyz]序列下的相對軌跡誤差，對比可以發現，本文提出的YG?SLAM算法在波動性上遠小于ORB?SLAM2算法，具有更小的RTE。

圖10～圖13為在上述四個動態序列下，ORB?SLAM2與YG?SLAM的三維軌跡圖，虛線代表真實軌跡，實線代表相機的估計軌跡。

圖14～圖17是四個動態序列下，ATE各指標之間的對比。實驗結果表明YG?SLAM算法遠遠優于ORB?SLAM2算法。

表2是ORB?SLAM2、DynaSLAM與YG?SLAM算法在四個動態序列下的RMSE（均方根誤差）和MEAN（平均偏差）。

在“fr3_w_[xyz]”序列中，YG?SLAM算法的RMSE為0.012 7 m，MEAN為0.010 8 m，相比于ORB?SLAM2分別提高了95.47%和95.64%，精度與DynaSLAM算法在同一水平。這一實驗結果表明在位置方式變化時，YG?SLAM提供了高精度的定位，誤差變化小。

在“fr3_w_rpy”序列中，RMSE和MEAN的值分別為0.045 2 m和0.037 1 m，與ORB?SLAM2對比分別提高了72.06%和73.80%。

表明YG?SLAM算法在處理姿態變化方面的任務時，具有較高精度。

3" 結" 論

YG?SLAM系統在ORB?SLAM2的基礎上加入目標檢測網絡，用于檢測動態特征點，使用多視圖幾何方法盡可能保留靜態點，剔除高動態特征點。在公開數據集TUM上進行實驗，精度方面相比于ORB?SLAM2算法，提高71.08%～95.64%；實時性方面，相比于DynaSLAM算法平均每幀減少228.03～354.04 ms。雖然在精度和實時性能上取得了一些進展，但仍有提升的空間，未來將進一步進行相關研究。

注：本文通訊作者為孫樹森。

參考文獻

[1] 張小勇，張洪，高清源，等.室內動態場景下基于稀疏光流與實例分割的視覺SLAM算法[J].東華大學學報（自然科學版），2023，49（6）：111?119.

[2] 鐘興軍，吳俊琦.一種基于目標檢測的動態環境下視覺定位系統[J].現代電子技術，2024，47（2）：160?164.

[3] 林子祥，張斌，王嘉盛，等.激光雷達IMU緊耦合的室內大范圍SLAM方法[J].現代電子技術，2023，46（20）：135?141.

[4] 李炯，趙凱，張志超，等.一種融合密度聚類與區域生長算法的快速障礙物檢測方法[J].機器人，2020，42（1）：60?70．

[5] MUR?ARTAL R， TARDOS 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.

[6] CAMPOS C， ELVIRA R， RODRíGUEZ 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.

[7] MIN F Y， WU Z B， LI D P， et al. COEB?SLAM： A robust VSLAM in dynamic environments combined object detection， epipolar geometry constraint， and blur filtering [J]. IEEE sensors journal， 2023， 23（21）： 1?20.

[8] CHENG J Y， WANG C Q， MENG M Q H. Robust visual localization in dynamic environments based on sparse motion removal [J]. IEEE transactions on automation science and engineering， 2020， 17（2）： 658?669.

[9] SUN Y X， LIU M， MENG M Q H. Improving RGB?D SLAM in dynamic environments： A motion removal approach [J]. Robotics and autonomous systems， 2017， 89： 110?122.

[10] JIN J， JIANG X F， YU C H， et al. Dynamic visual simultaneous localization and mapping based on semantic segmentation module [J]. Applied intelligence， 2023， 53（16）： 19418?19432.

[11] CHENG T H， WANG X G， CHEN S Y， et al. Sparse instance activation for real?time instance segmentation [C]// Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition. New York： IEEE， 2022： 4423?4432.

[12] YU C， LIU Z X， LIU X J， et al. DS?SLAM： A semantic visual SLAM towards dynamic environments [C]// 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. New York： IEEE， 2018： 1168?1174.

[13] 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.

[14] SONG W， ZHANG L. An RGB?D SLAM algorithm based on adaptive semantic segmentation in dynamic environment [J]. Journal of real?time image processing， 2023， 20（5）： 85.

[15] FAN Y C， ZHANG Q C， TANG Y L， et al. Blitz?SLAM： A semantic SLAM in dynamic environments [J]. Pattern recognition， 2022， 121： 108225.

作者簡介：喻擎蒼（1967—），男，浙江臺州人，博士研究生，教授，主要研究方向為機器視覺、機器人技術、智能裝備、異形作業無人機技術。

董根陽（1999—），男，浙江湖州人，在讀碩士研究生，主要研究方向為無人機技術、視覺SLAM。

方才威（1999—），男，湖北咸寧人，在讀碩士研究生，主要研究方向為機器視覺。

孫樹森（1974—），男，內蒙古寧城人，博士研究生，副教授，主要研究方向為圖像圖形處理。