融合語義信息的視覺慣性SLAM算法

摘 要：

針對傳統SLAM算法在動態環境中會受到動態特征點的影響，導致算法定位精度下降的問題，提出了一種融合語義信息的視覺慣性SLAM算法SF-VINS（visual inertial navigation system based on semantics fusion）。首先基于VINS-Mono算法框架，將語義分割網絡PP-LiteSeg集成到系統前端，并根據語義分割結果去除動態特征點；其次，在后端利用像素語義概率構建語義概率誤差約束項，并使用特征點自適應權重，提出了新的BA代價函數和相機外參優化策略，提高了狀態估計的準確度；最后，為驗證該算法的有效性，在VIODE和NTU VIRAL數據集上進行實驗。實驗結果表明，與目前先進的視覺慣性SLAM算法相比，該算法在動態場景和靜態場景的定位精度和魯棒性均有一定優勢。

關鍵詞：動態物體；語義概率；位姿估計；視覺慣性SLAM

中圖分類號：TP242 文獻標志碼：A 文章編號：1001-3695（2024）08-041-2533-07

doi： 10.19734/j.issn.1001-3695.2023.09.0556

Visual inertial SLAM algorithm with semantic information fusion

He Mingzhen， He Yuanlie， Hu Tao

（School of Computer， Guangdong University of Technology， Guangzhou 510006， China）

Abstract：

Aiming at the problem that the positioning accuracy of traditional SLAM algorithm decreases due to the influence of dynamic feature points in dynamic environment， this paper proposed SF-VINS which fused semantic information. Firstly， it incorporated the PP-LiteSeg semantic segmentation network into the front-end of system based on the VINS-Mono algorithm and removed the dynamic feature points according to the results of semantic segmentation. Then， it constructed the semantic probability error constraint with the pixel semantic probabilities and employed adaptive weights for feature points in the backend. Based on these modifications， this paper proposed a new BA cost function and an optimization strategy for camera extrinsic para-meters to improve the accuracy of state estimation. Finally， this paper conducted experiments on the VIODE and NTU VIRAL datasets to validate its effectiveness. The experimental results show that the proposed algorithm has advantages in terms of positioning accuracy and robustness in both dynamic and static scenes compared with state-of-the-art visual-inertial SLAM algorithms.

Key words：dynamic objects; semantic probability; pose estimation; visual inertial SLAM

0 引言

隨著視覺技術和硬件水平的發展，同步定位和建圖（simul-taneous localization and mapping，SLAM）技術已經在機器人上廣泛應用［1］，特別是在無GPS信號的環境中尤為重要［2］。傳統的視覺SLAM算法在靜態環境中已有較好的定位和建圖效果，但在面臨動態環境時，物體的運動會導致圖像特征點的跟蹤失敗或誤匹配，從而影響SLAM算法的表現［3］。而慣性傳感器不受動態環境的影響，可與視覺傳感器結合使用，獲得更可靠的位姿［4］。如Bloesch等人［5］提出ROVIO算法，其前端利用圖像塊像素強度誤差進行特征跟蹤，后端使用EKF融合慣性信息和視覺信息進行優化。Leutenegger等人［6］提出的OKVIS算法的后端使用滑動窗口非線性緊耦合優化，實現了雙目慣性里程計。Qin等人［7］提出的VINS-Mono具有回環檢測功能，是一個基于滑動窗口優化的緊耦合單目視覺慣性SLAM系統。Campos等人［8］提出的ORB-SLAM3在ORB-SLAM2［9］的基礎上融合了慣性傳感器信息，并且適用于單目、雙目、RGB-D相機、視覺慣性多種模式的數據輸入，突破了傳統視覺SLAM算法的定位精度。但如果在高動態環境中，視覺信息的誤差會變大，加入慣性傳感器的效果就會大打折扣。

隨著深度學習和神經網絡的發展，研究人員試圖結合神經網絡對SLAM算法進行優化。如DS-SLAM［10］配備了一個在獨立線程中運行的實時語義分割網絡，再加上移動的一致性檢查方法，使其能夠過濾出場景中的動態部分。DynaSLAM［11］用多視圖幾何和深度學習來檢測運動物體，并進行了背景重繪。DM-SLAM［12］結合了Mask R-CNN［13］、光流和對極約束來判斷異常值，并檢測先驗動態對象的特征點是否有大量移動，避免出現因刪除所有先驗動態對象特征點而導致沒有特征點的情況。AD-SLAM［14］利用對極約束來判斷當前環境的動態級別，并使用累積運動概率來應對低動態場景和使用語義分割網來應對高動態場景，從而提高系統的魯棒性和定位精度。STDyn-SLAM［15］同樣使用了語義分割網絡、光流法和幾何約束來消除動態物體，并且能夠重建剔除動態物體后的三維場景。

但上述這些方法都僅僅只是簡單地剔除掉動態特征點，當動態物體占據相機大部分視場，則無法被神經網絡正確識別，導致系統無法剔除掉這些特征點。目前大多數SLAM算法都會采用RANSAC［16］算法或魯棒核函數［17］來降低異常值的影響，如果環境中僅有少數的動態特征點，這些方法確實能起到一定作用。但如果圖像中大多數特征點都屬于移動物體，這些方法所起的作用就十分有限，從而極大地影響了狀態估計的準確性。因此一些研究人員嘗試結合IMU并降低相機部分的優化權重進行位姿估計，以提高算法在動態環境中的魯棒性。如DVI-SLAM［18］根據對極幾何約束檢測動態點，并添加視覺信息自適應權重因子，提高了系統的魯棒性。Song等人［19］提出了一個魯棒的BA函數，它可以利用IMU預積分估計的先驗姿態來消除動態對象特征點的影響。但這類方法始終會將動態點加入到后端優化中，并且直接使用視覺權重因子會影響實時外參估計的準確性。此外，一些研究嘗試將語義信息融合到后端優化中來提升定位精度，如Lianos等人［20］提出了語義重投影誤差，該誤差對齊兩幀之間特征點的語義標簽，增強了數據關聯性，從而減少軌跡漂移。SDVO［21］結合了直接法的光度誤差和語義概率誤差，構造出新的誤差函數，該函數具有更好的凸性，在優化迭代中有著更穩定的性能。但是這類方法如果直接應用在動態場景中，同樣會受到動態物體的影響，魯棒性不強。

綜上所述，僅在前端剔除動態點或者僅在后端利用語義信息形成數據關聯來提升定位精度的方法依然存在一些缺陷，并且不能夠充分地利用場景中的語義信息；而如果直接采用降低相機部分優化權重的策略來提高算法在動態環境中的魯棒性，會影響實時外參估計的準確性。因此，為充分地利用語義分割網絡得到的信息，本文針對室外的動態場景和靜態場景，使用單目相機和慣性傳感器，在VINS-Mono算法的基礎上進行改進，提出了一種將語義信息融入前后端的視覺慣性SLAM算法SF-VINS。本文的主要工作如下：

a）將實時語義網絡與前端模塊融合，篩選提取到的特征點，避免動態特征點參與后端優化。

b）提出了一種結合光流法的重投影誤差和語義概率誤

差的后端優化方法，從而提升算法在靜態環境和動態環境中的準確性和魯棒性。

c）為每個特征點分配動態權重，并優化了外參估計策略，能有效降低離群點對狀態估計的影響。

本文使用VIODE［4］和NTU VIRAL［22］數據集對SF-VINS算法進行測試，并與STDyn-SLAM、ORB-SLAM3和DynaVINS算法作比較。

1 視覺慣性SLAM算法框架

SF-VINS的算法框架是基于VINS-Mono進行構建的，如圖1所示，共包含特征點提取和跟蹤模塊、系統初始化模塊和后端優化模塊三大模塊。本文著重對特征點提取和跟蹤模塊和后端優化模塊進行了優化改進。在特征點提取和跟蹤模塊中，提取特征點時設置語義掩膜，以避免在動態物體上提取特征點，并在跟蹤過程中根據語義分割結果剔除動態特征點匹配對。在后端優化模塊中，利用語義網絡輸出的語義概率來構造語義概

率誤差約束項，以限制狀態估計，并根據每個特征點的誤差總和大小對特征點進行加權，最終結合先驗誤差、視覺重投影誤差、IMU誤差和語義概率誤差進行系統狀態估計，得到每一幀的位姿信息和每一個特征點的空間位置。

2 動態特征點剔除

對于VINS-Mono算法，動態特征點的匹配會導致錯誤的數據關聯，從而影響位姿估計的準確度。而靜態特征點相比動態特征點對位姿估計具有著更強的魯棒性，因此本文在前端根據語義分割網絡輸出的語義標簽形成語義掩模，進而在靜態物體上提取特征點，用于后端優化中的位姿估計。本文使用在Cityscapes數據集［23］上訓練的實時語義分割網絡PP-LiteSeg［24］作為語義信息的來源，該網絡是一個輕量級的圖像語義分割模型，由PaddlePaddle深度學習框架開發團隊提出，該網絡采用了編解碼器架構，使用靈活和輕量級解碼器、統一注意力融合模塊和簡單金字塔池化等輕量級網絡結構，并對模型進行了多個細節優化，兼顧了網絡推理的實時性和準確性，并有一定的泛化能力，適合與SLAM算法一起使用。訓練后的網絡能夠對室外場景出現的道路、建筑、植被、天空、汽車、行人等34類物體進行像素級語義識別。特征點的提取和跟蹤流程如圖2所示。

以下是需要被推送到后端圖像的前端處理流程：

a）特征點提取和跟蹤模塊讀入一幀新圖像后，送進語義分割網絡獲得像素級語義圖像；

b）使用該幀的原圖像結合上一幀圖像的特征點進行光流跟蹤；

c）剔除跟蹤后落在圖像外的特征點匹配對；

d）剔除先驗動態物體上的特征點匹配對；

e）利用對極約束剔除外點；

f）根據配置文件中的特征點最小距離設置特征點均勻分布掩膜；

g）結合語義掩膜和特征點均勻分布掩膜生成感興趣區域；

h）利用上一步生成的感興趣區域，使用good_feature_to_track算法［25］提取新的特征點，供下一幀的光流跟蹤使用。

這樣便實現了特征點的均勻分布以及防止在動態物體上提取特征點。最后再計算該幀提取和跟蹤到的特征點速度以及對其進行去畸變的操作。通過這些處理步驟，特征點提取和跟蹤模塊能夠更加穩定地跟蹤靜態目標并提高算法的魯棒性。圖3（a）為原始圖像幀；圖3（b）是經語義分割網絡預測得到的動態區域圖，白色區域即為動態區域；圖3（c）為原本提取和跟蹤到的特征點分布圖，三角形特征點表明該特征點落在靜態區域，圓形特征點表明該特征點落在動態區域；圖3（d）為使用語義掩膜后的特征點分布圖，可看出結合語義掩膜來提取特征點后，并沒有特征點落在行駛車輛上。

大部分語義SLAM算法采用先提取特征點再剔除的策略，這種策略會導致特征點數量的下降，而本文算法結合語義掩膜來避免在動態區域上提取特征點，能讓每幀圖像都擁有足夠數量的靜態特征點用于位姿估計，從而保證位姿估計的準確性。

3 后端非線性優化

3.1 語義概率誤差

由于特征點會隨著相機視角、尺度、光照的變化而發生變化，相比之下，特征點的語義信息不會受上述因素的影響，例如上一幀標記為A類的點投影到當前幀后，應該落在當前幀中對應的A類區域。因此，可以將這種語義不變性應用到數據關聯中，建立起連續圖像幀之間的語義信息約束［20］。本文構建語義概率誤差約束項的假設是：某特征點經過位姿變換后，該特征點在前后兩幀的語義概率保持不變。從PP-LiteSeg網絡的推理結果中，可以得到每一幀關于不同物體的語義概率圖，而由于建筑類物體在實際環境中相對固定不動，且在室外場景中常見，可作為語義概率對齊的參照對象，所以本文選擇關于建筑類的語義概率圖來構造語義概率誤差約束項。

如圖4所示，相機在兩個位置對同一空間點P進行觀察，假設空間點P被相機幀i觀測到并落在了相機幀i的第k個特征點，對應的像素坐標為pki，該特征點經過相機的估計位姿轉換后，落在相機幀j的像素坐標為kj。

假設經過位姿轉換后，特征點的語義概率不變，則定義語義概率誤差為

esemi， j，k=S（kj）－S（pki）（1）

其中：S（·）表示某個像素坐標在對應相機幀中的語義概率。

為了得到空間點P在第j幀相機系下的估計像素坐標，首先需要獲得空間點P在第j幀相機系下的空間坐標kj。為了說明，先進行一些符號定義，記世界坐標系下第i幀的旋轉和平移為Rwbi和twbi，第j幀的旋轉和平移為Rwbj和twbj，外參為Rbc和tbc，空間點P在第i幀相機系下的坐標為Pki。

由于本文算法使用IMU坐標系作為基準，所以需要經過多次轉換才能將空間點P從第i幀的相機系重新投影到第j幀的相機系。具體的轉換過程如下：首先通過外參Rbc和tbc將空間點坐標從第i幀的相機系轉換到第i幀的IMU系；再用第i幀的估計位姿Rwbi和twbi，從第i幀的IMU系轉換到世界系下；之后利用第j幀估計位姿的逆旋轉RTwbj和逆平移tbjw，從世界系轉換到第j幀的IMU系；最后再通過外參轉換得到空間點P在第j幀相機系下的空間坐標P^kj?；喓蟮霓D換公式如下：

P^kj=X′Y′Z′=RTbcRTwbjRwbiRbcPki+

RTbc（RTwbj（（Rwbitbc+twbi）－twbj）－tbc）（2）

得到空間點P在第j幀相機系下的空間坐標后，再通過相機內參將空間點投影到像素平面上即可獲得像素坐標，投影關系為

u=fxX′Z′+cx

v=fyY′Z′+cy（3）

其中： fx和fy表示相機焦距；cx和cy表示相機主點位置。

為最小化語義概率誤差，即最小化式（1），需要提供語義概率誤差對各個優化變量的雅可比。定義優化變量為

Ψki， j=［Rwbi，twbi，Rwbj，twbj，λk］T（4）

其中：λk為特征點pki在第i幀的逆深度。則語義概率誤差對優化變量的雅可比可通過鏈式求導法則得到

esemi， j，kΨki， j=S（kj）kj·kjP^kj·P^kjΨki， j（5）

假設估計像素坐標（u，v）處的語義概率值對像素坐標的雅可比為

S（kj）kj=［nx ny］（6）

其中：nx為估計像素點在語義概率圖中x方向上的梯度；ny為估計像素點在語義概率圖中y方向上的梯度。

假設估計像素點附近3×3區域的語義概率為

Au，v=S（u－1，v－1）S（u，v－1）S（u+1，v－1）S（u－1，v）S（u，v）S（u+1，v）S（u－1，v+1）S（u，v+1）S（u+1，v+1）（7）

則可采用算子式（8）與Au，v進行卷積運算，得到估計像素點（u，v）處的語義概率值對像素坐標的雅可比。

Nx=－303－909－303，Ny=－3－9－3000393（8）

即

S（kj）kj=［Au，vNx Au，vNy］（9）

其中：表示卷積運算。如果遇到語義概率圖像的邊緣沒有左右像素的情況，本文將超出邊界的像素值設置為0，即在圖像外圈進行“零填充”，確保雅可比計算的正確性。

根據式（3），易得第j幀的估計像素坐標kj對估計空間點坐標P^kj的雅可比，如式（10）所示。

kjP^kj=fxZ′0－fxX′Z′20fyZ′－fyY′Z′2（10）

而估計空間點坐標P^kj對各個優化變量的雅可比如式（11）所示。

P^kjΨki，j=P^kjRwbiP^kjtwbiP^kjRwbj

P^kjtwbj

P^kjλk=RTbcRTwbjRwbi［Pki］×RTbcRTwbj

RTbc［Pbj］×

－RTbcRTwbj－1λkRTbcRTwbjRwbiRbcPki（11）

其中：前兩項分別為估計空間點坐標對第i幀IMU在世界系下旋轉向量和平移向量的偏導數；第三和四項分別為第j幀IMU在世界系下旋轉向量和平移向量的偏導數；最后一項為估計空間點坐標對該特征點在第i幀相機系下逆深度的偏導數；［·］×表示將括號中的向量反對稱化，假設有某向量v=［v1 v2 v3］，則

［v］×=0－v3v2v30－v1－v2v10（12）

通過鏈式求導法則，將原本需要求解的雅可比矩陣分解成三個不同的雅可比矩陣，分別求出這三個雅可比矩陣后再進行連乘，即可得到原本的雅可比矩陣，從而可以使用非線性優化算法來進行狀態估計。

3.2 特征點加權

當某個動態物體在相機圖像中不完整，或者占據了相機圖像大部分面積時，語義網絡將無法正確地識別該動態物體，因此僅在特征點提取和跟蹤模塊中剔除先驗動態特征點，不足以降低動態物體給算法所帶來的影響，盡管魯棒核函數同樣可以抑制外點，但其效果有限。而慣性傳感器的測量值不受動態物體的影響，如果使用特征點自適應權重，當動態物體出現時，動態特征點的權重會降低，能夠使估計結果更偏向于IMU。因此本文方法使用特征點自適應權重，除了在動態場景下能夠讓估計結果更偏向于IMU，還能降低語義分割網絡無法識別的動態點的權重，從而使狀態估計的結果更加精確。

本文對每個特征點賦予不同的權重，如果特征點的匹配誤差越大，權重越低，從而降低匹配誤差大的特征點在后端優化中的影響。根據文獻［19，26］提出的BR對偶性，通過最小化式（13）的誤差，可得到每個特征點的最優權重。

ρ（wi，rvisi）=w2i∑j∈f（pi）‖rvisi， j‖2+αR2（wi）+βM2（wi）（13）

其中：rvisi， j為重投影誤差，在此優化問題中為常數；α和β為常系數；R（wi）為正則化項，如式（14）所示。正則化項可令誤差越高的特征點，權重越接近于0，反之權重越接近于1。

R（wi）=1－wi（14）

M（wi）為動量項，如式（15）所示，如果不設置動量項，當相機快速運動時，重投影誤差會突然增大而造成特征點權重快速降低，設置動量項能使權重的變化更加平緩。

M（wi）=ti（i－wi）（15）

其中：ti為該特征點權重之前的優化次數，該特征點權重優化次數越多，證明它被更多的相機幀觀測到，因此該特征點的權重要處于一個相對平穩的狀態；i為該特征點上一次的權重。

在設置權重時，本文先將特征點的重投影誤差固定住，當作常數因子，因此上述問題可轉換成一個一元二次方程。為簡要說明，記某個特征點的重投影誤差總和為Ri，易得當式（13）取最小值時，權重應為

wi=α+2itiβRi+α+tiβ，i∈（0，1］（16）

從式（16）可得，特征點的權重被控制在0和1之間，當特征點的重投影誤差越小，則權重越接近于1，表明越信賴該特征點。相反地，當特征點的重投影誤差越大，則權重越接近于0，間接抑制了該特征點在后端優化中的負面效果。而通常動態特征點的重投影誤差較大，因此該方法能有效消除離群點對狀態估計帶來的影響。

3.3 緊耦合的視覺慣性語義聯合優化算法

后端模塊優化的是整個滑動窗口里面所有的狀態量，狀態向量為

χ=［x0，x1，…，xn，xbc，λ0，…，λm］

xk=［pwbk，vwbk，qwbk，bbka，bbkg］T k∈［0，n］

xbc=［pbc，qbc］（17）

其中：xk為第k個圖像幀對應的IMU狀態，包括在世界系下IMU的平移pwbk、速度vwbk、旋轉qwbk、加速度計零偏bbka和陀螺儀零偏bbkg；xbc包括了相機到IMU的旋轉qbc和平移pbc；λi為第i個特征點的逆深度；n為關鍵幀數量；m為滑動窗口中觀測到的特征點總數。

本文后端采用基于滑動窗口的非線性優化方法，固定窗口大小，當新的一幀圖像到來時會邊緣化舊的圖像幀，因此還需要考慮先驗誤差。通過最小化各個模塊的誤差總和，來優化滑動窗口中所有的狀態量，從而融合了視覺信息、語義信息和IMU信息。綜上，本文構造的誤差函數如式（18）所示，包含了先驗誤差、IMU預積分誤差、視覺誤差和語義概率誤差四部分。

minχ‖rp－Jpχ‖2+∑i∈B‖rb（bibi+1，χ）‖2+

λ∑（i， j）∈F‖rs（cij，χ）‖2+∑（i， j）∈Fρ（‖ωirf（cij，χ）‖2）

（18）

其中：rp表示先驗誤差；Jp為邊緣化后留下的先驗信息；rb表示IMU預積分誤差；bibi+1表示IMU觀測；λ表示語義概率誤差的優化權重；rs表示語義概率誤差；ρ（·）表示魯棒核函數；cij表示語義概率觀測；rf表示視覺誤差；cij表示視覺觀測。

值得注意的是，在沒有準確外參或外參可能實時變化的情況下，需要實時地將視覺信息和IMU信息進行對齊來估計外參。由于使用了特征點自適應權重，會使視覺權重整體下降，而實時外參估計需要完整的視覺信息和IMU信息，降低視覺權重會導致實時外參估計受到較大的影響，使得軌跡發生嚴重漂移。因此本文在需要實時估計外參xbc時，假設滑動窗口內所有幀的外參保持不變，先不改變特征點的權重，使用式（18）對外參進行一次估計得到固定外參T，然后使用固定外參T和動態特征點權重再進行迭代優化。一次完整的后端優化流程如圖5所示。首先恢復視覺權重，即將特征點權重設置為1，再聯合IMU數據估計得到本次滑動窗口優化的固定外參，然后根據上一輪的視覺誤差最小化式（13）后，獲得每個特征點的權重，接著再根據式（18）進行優化，如此迭代k次后獲得最終的狀態估計結果。這樣便可以避免出現因降低視覺權重而影響外參估計的問題，同時也保證了動態特征點權重的效果，提高了算法的魯棒性。

4 實驗與分析

本文的實驗環境為裝有INTEL I7-7700K CPU、 NVIDIA GTX 1080TI GPU、16 GB內存和Ubuntu 18.04系統的臺式主機。語義分割網絡采用經過Cityscapes數據集訓練的PP-LiteSeg模型，并使用paddle deployment inference工具將PP-LiteSeg模型集成到SF-VINS算法中。

本文實驗具體分為三部分。首先在具有動態環境的VIODE數據集上測試，用來評估算法在動態環境中的定位精度；其次在具有靜態校園環境的NTU VIRAL數據集上測試，驗證算法在不經過網絡訓練且靜態的場景下的魯棒性；最后對算法進行消融實驗，更好地體現出不同模塊所起的作用。為了保證公平，所有對比算法和SF-VINS的實驗結果均在同一平臺上進行測試，且每個序列至少運行三次并取中位數作為最終結果。實驗使用絕對軌跡誤差（absolute trajectory error， ATE）來評估算法效果，即比較估計位姿和真實位姿的歐幾里德距離的均方根差、中位數、平均數以及標準差。

4.1 VIODE數據集

VIODE數據集使用了一個四旋翼無人機作為載體，其搭載了兩個攝像頭和一個慣性傳感器。city_day包括四個有著城市白天環境的圖像序列，其場景包含了高大的建筑和樹木，city_night包括了與city_day相同環境下的四個夜間圖像序列，但無人機的軌跡和動態物體的軌跡不同于city_day。各序列的名稱后綴none到high表示序列中包含動態物體的程度。

本文僅使用左攝像頭數據和慣性傳感器數據進行實驗。表1為SF-VINS與STDyn-SLAM雙目、VINS-Mono單目慣性、ORB-SLAM3雙目慣性、DynaVINS單目慣性四個算法的對比實驗結果。

從表1的八個序列對比結果可得，SF-VINS僅使用單目相機和慣性傳感器作為輸入，但大部分的估計軌跡均比上述四個算法的定位精度更高、誤差更低。對比不同動態物體程度的序列，其他對比算法的軌跡誤差會隨著動態物體的增多而會有顯著的上升，但SF-VINS的誤差變化處于相對平穩的趨勢，STDyn-SLAM雖然同樣使用了動態點剔除算法，但由于缺失慣性傳感器的輔助，僅使用了雙目攝像頭作為數據來源，導致算法受環境的影響較大，這從側面證明了慣性傳感器在動態環境中的重要性。DynaVINS由于沒有在前端剔除動態點，僅在后端降低了離群點的影響，這樣依然會將有噪聲的數據納入到后端優化中，因此DynaVINS在最高動態序列中所受的影響較大。而且注意到，即使在靜態序列city_night_none和city_day_none中，得益于新的BA優化函數，SF-VINS相比于VINS-Mono同樣有不小幅度的提升，說明SF-VINS在動態環境和靜態環境中均表現出了良好的性能，具有更好的魯棒性。

圖6是上述五種算法在city_day_high、city_day_none、city_night_high、city_night_none四個序列上的估計軌跡與真實軌跡的對比，為方便對比，所有軌跡投影在xy平面上并進行局部放大?？煽闯鯯F-VINS估計的軌跡更接近于真實軌跡，并且優于其他四種算法。而且注意到在圖6（a）的city_day_high序列中，VINS-Mono和ORB-SLAM3估計的軌跡偏移較大，這是因為在序列的后半段，有一輛行駛中的汽車占據了相機的大部分視場，導致該路段中幾乎所有特征點都落在了動態物體上，嚴重影響了視覺信息的準確性。VINS-Mono和ORB-SLAM3算法并未能夠有效地處理這種情況，因此軌跡發生了較大的漂移。而SF-VINS在剔除掉先驗動態點的同時，降低了離群特征點的權重，彌補了在前端模塊中僅刪除先驗動態點的缺陷，并將語義概率誤差納入后端優化中，為狀態估計提供了更強的約束，因此沒有發生較大的軌跡漂移，即使在高動態環境下依然有較好的定位精度。

4.2 NTU VIRAL數據集

NTU VIRAL數據集是南洋理工大學使用多傳感器采集的一個SLAM數據集，包括兩個三維激光雷達、兩個相機、多個慣性傳感器，數據采集于南洋理工大學校園內的某些場景。該數據集同樣僅使用左攝像頭數據和單個慣性傳感器數據進行實驗。本文在NTU VIRAL數據集中選取具有較多建筑場景的三個序列進行測試，都為靜態序列。其中Sbs_01序列是在校內的一個開放廣場上采集的，附近有一些低層建筑，Eee_01和Eee_02序列同樣是在校內采集，該區域四面都是高大的建筑。需要注意的是，由于NTU VIRAL數據集使用無人機采集數據，快速運動過程中可能會出現外參變化的情況，所以該數據集要求算法能夠對外參進行實時估計，否則無法穩定跟蹤。而由于STDyn-SLAM算法在NTU VIRAL數據集上尺度估計不準確，無法穩定跟蹤，所以在該數據集上不加入該算法進行測試。表2為SF-VINS與VINS-Mono單目慣性、ORB-SLAM3雙目慣性、DynaVINS單目慣性三種算法的對比實驗結果。

從表2對比結果可得，SF-VINS的實驗結果在三個序列上的定位精度都要優于其他對比算法。在Sbs_01序列中，由于大部分特征點都處于較遠的位置，ORB-SLAM3無法穩定跟蹤特征點，導致軌跡發生嚴重漂移，而另外兩個序列也存在小部分特征點處于較遠位置的片段，所以ORB-SLAM3在三個序列中的軌跡誤差都比較高。DynaVINS降低了視覺權重，影響了外參的實時估計，導致定位精度下降。而SF-VINS在三個序列上都很好地估計了變化的外參，并沒有發生定位精度下降的問題，且在沒有訓練過的校園建筑場景中，相比VINS-Mono依然有13%～40%的性能提升，表明SF-VINS具有一定的泛化能力。從圖7各個軌跡的對比可看出，SF-VINS的估計軌跡最接近真實軌跡。

4.3 消融實驗

本文在兩個數據集中挑選三個具有代表性的序列進行消融實驗，以驗證各個模塊在不同場景下所起的作用。在表3中VINS+SEG表示只加入前端剔除先驗動態特征點模塊，VINS+NBA表示只使用新的BA代價函數，VINS+SEG+NBA表示同時使用前端剔除先驗動態特征點模塊和新的BA代價函數。

從表3中city_day_high和city_night_high序列的實驗結果可知，僅加入SEG模塊雖然能夠提高一些定位精度，但如果場景中存在無法識別的動態物體或者動態物體占據了相機大部分視場時，此時SEG模塊的作用相對有限；而如果僅使用新的BA代價函數，雖然也能一定程度地提高定位精度，但無可避免地將動態點納入了后端優化中，影響了狀態估計的準確性；同時使用SEG模塊和新的BA代價函數，能夠將特征點集中在先驗靜態物體上，并同時能夠為狀態估計提供更強的約束以及降低離群點對狀態估計的負面影響。而在Sbs_01序列中動態特征點模塊并沒有起到正向作用，反而因為語義網絡的誤分類而誤刪了某些特征點，導致定位精度有略微下降，因此在靜態環境中，起主要作用的是新的BA代價函數。

4.4 實時性評估

為了驗證SF-VINS的實時性，并且為排除機器差異的影響，以下算法均在同一平臺進行測試。以VIODE數據集中的city_night_high序列為例，該序列的圖像分辨率為752×480，而本文測試的所有序列的圖像分辨率均為752×480，且該序列包含高動態場景。因此，選擇這個序列來比較SF-VINS和其他算法的實時性和精度具有一定的代表性。各算法每部分的平均耗時如表4所示。

SF-VINS由于添加了語義分割網絡部分，并在提取和跟蹤特征點時構建語義掩膜，所以相比VINS-Mono算法的總耗時有所增加，但依然有約29 fps的跟蹤性能，基本滿足實時性要求。對比同樣使用語義分割網絡的STDyn-SLAM，本文算法在各個部分的耗時都要低，因為本文使用的分割網絡比STDyn-SLAM使用的SegNet的推理速度更快，而且SF-VINS采用光流法跟蹤特征點，每張圖像只需提取150～250個特征點即可，而STDyn-SLAM采用特征點法進行跟蹤，每張圖像需要提取上千個特征點，增加了不少耗時。結合表1定位精度的測試結果，并綜合速度上的考慮，SF-VINS在動態環境和靜態環境中的定位精度和魯棒性均有一定優勢，并且滿足了實時性的要求。

5 結束語

本文提出了一種融合語義信息的單目視覺慣性SLAM算法SF-VINS，相比一些僅在前端剔除動態特征點的算法，SF-VINS將語義信息融合到后端優化，提供了更強的約束，并且采取特征點加權的策略降低了潛在動態點對系統狀態估計的影響。而通過動態數據集VIODE和靜態數據集NTU VIRAL驗證了SF-VINS在動態環境和靜態環境下具有更好的魯棒性，且對比最新的視覺慣性SLAM算法，SF-VINS的定位精度更高。但所提算法仍有一些不足：由于加入了語義分割網絡和更多的優化約束項，算法的實時性相比VINS-Mono有所降低；語義概率誤差的準確性依賴于前端語義分割網絡的效果，若語義分割網絡的效果不佳，會使分割連續幀的效果不穩定，導致語義概率誤差不準確，從而影響系統的狀態估計。因此在未來的工作中，可采取更輕量級且泛化性能更強的語義分割網絡，并改進后端優化策略，保證算法精度的同時提高實時性。更進一步地，可通過語義信息，構造出語義地圖，給系統提供更精確的定位信息和場景理解。

隨著人工智能的發展，SLAM算法不應只是針對場景的表面計算，更應該像人類一樣試圖理解當前場景的意義，這有助于SLAM算法具有更高的可靠性并應用于更廣泛的場景。

參考文獻：

［1］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-1332.

［2］Urzua S，Munguia R，Grau A. Vision-based SLAM system for MAVs in GPS-denied environments［J］. International Journal of Micro Air Vehicles，2017，9（4）： 283-296.

［3］高興波，史旭華，葛群峰，等. 面向動態物體場景的視覺SLAM綜述［J］. 機器人，2021，43（6）： 733-750. （Gao Xingbo，Shi Xuhua，Ge Qunfeng，et al. A survey of visual SLAM for scenes with dynamic objects［J］. Robot，2021，43（6）： 733-750.）

［4］Minoda K，Schilling F，Wuest V，et al. VIODE： a simulated dataset to address the challenges of visual-inertial odometry in dynamic environments［J］. IEEE Robotics and Automation Letters，2021，6（2）： 1343-1350.

［5］Bloesch M，Omari S，Hutter M，et al. Robust visual inertial odometry using a direct EKF-based approach［C］// Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway，NJ： IEEE Press，2015： 298-304.

［6］Leutenegger S，Lynen S，Bosse M，et al. Keyframe-based visual-inertial odometry using nonlinear optimization［J］. International Journal of Robotics Research，2014，34（3）： 314-334.

［7］Qin Tong，Li Peiliang，Shen Shaojie. VINS-Mono： a robust and versatile monocular visual-inertial state estimator［J］. IEEE Trans on Robotics，2018，34（4）： 1004-1020.

［8］Campos C，Elvira R，Rodríguez 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）： 1874-1890.

［9］Mur-Artal R，Tardós 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-1262.

［10］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.

［11］Bescos 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.

［12］Cheng Junhao，Wang Zhi，Zhou Hongyan，et al. DM-SLAM： a feature-based SLAM system for rigid dynamic scenes［J］. ISPRS International Journal of Geo-Information，2020，9（4）： 202.

［13］He Kaiming，Gkioxari G，Dollár P，et al. Mask R-CNN［C］// Proc of International Conference on Computer Vision. Piscataway，NJ： IEEE Press，2017： 2961-2969.

［14］王富強，王強，李敏，等. 基于動態分級的自適應運動目標處理SLAM算法［J］. 計算機應用研究，2023，40（8）： 2361-2366. （Wang Fuqiang，Wang Qiang，Li Min，et al. Adaptive moving object processing SLAM algorithm based on scene dynamic classification［J］. Application Research of Computers，2023，40（8）： 2361-2366.）

［15］Esparza D，Flores G. The STDyn-SLAM： a stereo vision and semantic segmentation approach for VSLAM in dynamic outdoor environments［J］. IEEE Access，2022，10： 18201-18209.

［16］Fischler M A，Bolles R C. Random sample consensus： a paradigm for model fitting with applications to image analysis and automated carto-graphy［J］. Communications of the ACM，1981，24（6）： 381-395.

［17］Li Xuelong，Lu Quanmao，Dong Yongsheng，et al. Robust subspace clustering by Cauchy loss function［J］. IEEE Trans on Neural Networks and Learning Systems，2019，30（7）： 2067-2078.

［18］崔林飛，黃丹丹，王祎旻，等. 面向動態環境的視覺慣性SLAM算法［J］. 計算機工程與設計，2022，43（3）： 713-720. （Cui Linfei，Huang Dandan，Wang Yimin，et al. Visual inertial SLAM algorithm for dynamic environment［J］. Computer Engineering and Design，2022，43（3）： 713-720.）

［19］Song S，Lim H，Lee A J，et al. DynaVINS： a visual-inertial SLAM for dynamic environments［J］. IEEE Robotics and Automation Letters，2022，7（4）： 11523-11530.

［20］Lianos K N，Schonberger J L，Pollefeys M，et al. VSO： visual semantic odometry［C］// Proc of European Conference on Computer Vision. Cham： Springer， 2018： 234-250.

［21］Bao Yaoqi，Yang Zhe，Pan Yun，et al. Semantic-direct visual odometry［J］. IEEE Robotics and Automation Letters，2022，7（3）： 6718-6725.

［22］Nguyen T M，Yuan Shenghai，Cao Muqing，et al. NTU VIRAL： a visual-inertial-ranging-lidar dataset，from an aerial vehicle viewpoint［J］. The International Journal of Robotics Research，2022，41（3）： 270-280.

［23］Cordts M，Omran M，Ramos S，et al. The cityscapes dataset for semantic urban scene understanding［C］// Proc of IEEE Conference on Computer Vision and Pattern Recognition. Piscataway，NJ： IEEE Press，2016： 3213-3223.

［24］Peng Juncai，Liu Yi，Tang Shiyu，et al. PP-LiteSeg： a superior real-time semantic segmentation model［EB/OL］. （2022-04-06）. https：//arxiv.org/abs/2204.02681.

［25］Shi Jianbo，Tomasi C. Good features to track［C］// Proc of IEEE Conference on Computer Vision and Pattern Recognition. Piscataway，NJ： IEEE Press，1994： 593-600.

［26］Black M J，Rangarajan A. On the unification of line processes，outlier rejection，and robust statistics with applications in early vision［J］. International Journal of Computer Vision，1996，19（1）： 57-91.