摘 要：

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

關鍵詞：動態(tài)物體；語義概率；位姿估計；視覺慣性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 引言

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

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

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

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

a）將實時語義網(wǎng)絡與前端模塊融合，篩選提取到的特征點，避免動態(tài)特征點參與后端優(yōu)化。

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

差的后端優(yōu)化方法，從而提升算法在靜態(tài)環(huán)境和動態(tài)環(huán)境中的準確性和魯棒性。

c）為每個特征點分配動態(tài)權(quán)重，并優(yōu)化了外參估計策略，能有效降低離群點對狀態(tài)估計的影響。

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

1 視覺慣性SLAM算法框架

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

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

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

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

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

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

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

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

d）剔除先驗動態(tài)物體上的特征點匹配對；

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

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

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

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

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

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

3 后端非線性優(yōu)化

3.1 語義概率誤差

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

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

假設經(jīng)過位姿轉(zhuǎn)換后，特征點的語義概率不變，則定義語義概率誤差為

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

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

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

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

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

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

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

u=fxX′Z′+cx

v=fyY′Z′+cy（3）

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

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

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

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

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區(qū)域的語義概率為

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，即在圖像外圈進行“零填充”，確保雅可比計算的正確性。

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

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

而估計空間點坐標P^kj對各個優(yōu)化變量的雅可比如式（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在世界系下旋轉(zhuǎn)向量和平移向量的偏導數(shù)；第三和四項分別為第j幀IMU在世界系下旋轉(zhuǎn)向量和平移向量的偏導數(shù)；最后一項為估計空間點坐標對該特征點在第i幀相機系下逆深度的偏導數(shù)；［·］×表示將括號中的向量反對稱化，假設有某向量v=［v1 v2 v3］，則

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

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

3.2 特征點加權(quán)

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

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

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

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

R（wi）=1－wi（14）

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

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

其中：ti為該特征點權(quán)重之前的優(yōu)化次數(shù)，該特征點權(quán)重優(yōu)化次數(shù)越多，證明它被更多的相機幀觀測到，因此該特征點的權(quán)重要處于一個相對平穩(wěn)的狀態(tài)；i為該特征點上一次的權(quán)重。

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

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

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

3.3 緊耦合的視覺慣性語義聯(lián)合優(yōu)化算法

后端模塊優(yōu)化的是整個滑動窗口里面所有的狀態(tài)量，狀態(tài)向量為

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

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

xbc=［pbc，qbc］（17）

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

本文后端采用基于滑動窗口的非線性優(yōu)化方法，固定窗口大小，當新的一幀圖像到來時會邊緣化舊的圖像幀，因此還需要考慮先驗誤差。通過最小化各個模塊的誤差總和，來優(yōu)化滑動窗口中所有的狀態(tài)量，從而融合了視覺信息、語義信息和IMU信息。綜上，本文構(gòu)造的誤差函數(shù)如式（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觀測；λ表示語義概率誤差的優(yōu)化權(quán)重；rs表示語義概率誤差；ρ（·）表示魯棒核函數(shù)；cij表示語義概率觀測；rf表示視覺誤差；cij表示視覺觀測。

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

4 實驗與分析

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

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

4.1 VIODE數(shù)據(jù)集

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

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

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

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

4.2 NTU VIRAL數(shù)據(jù)集

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

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

4.3 消融實驗

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

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

4.4 實時性評估

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

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

5 結(jié)束語

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

隨著人工智能的發(fā)展，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］高興波，史旭華，葛群峰，等. 面向動態(tài)物體場景的視覺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(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.

［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］王富強，王強，李敏，等. 基于動態(tài)分級的自適應運動目標處理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，F(xiàn)lores 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］崔林飛，黃丹丹，王祎旻，等. 面向動態(tài)環(huán)境的視覺慣性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.