面向動態環境的視覺慣性SLAM算法

崔林飛，黃丹丹，王祎旻，耿振野，劉 智

(長春理工大學 電子信息工程學院，吉林 長春 130000)

0 引 言

動態環境中的自我運動估計是移動設備的基本問題，SLAM技術就是有效解決這個問題的常用方法[1]。視覺傳感器因成本低廉和使用方便等優點，視覺SLAM得到了快速發展[2]，目前開源視覺SLAM具有代表性的有ORB-SLAM[3]、DSO[4]等。為了彌補單目視覺SLAM在快速移動下容易失效的缺點，添加了慣性測量單元(IMU)，以增強單目視覺SLAM系統的定位精度和魯棒性。目前流行的開源視覺慣性SLAM解決方案包括：VINS-MONO[5]、VINS-Fusion[6]等。然而，絕大多數SLAM方法以及數據集都是基于靜態環境下進行研究，而如何消除動態環境對SLAM算法性能影響的相關研究較少。同時視覺SLAM主要是根據兩幀圖像之間匹配的特征點估計出自身的旋轉和平移(即相機的位姿)。在動態環境中由于存在大量移動對象，會造成提取的特征中包含部分動態特征點，使用這些動態特征點進行特征匹配，會降低系統位姿估計的準確性。

針對SLAM系統受到動態環境影響的問題，提出一種基于對極幾何的特征檢測算法，由于動態環境對視覺和慣性信息影響程度不同，設計了一種自適應調整因子用于后端調整視覺信息權重。改進后的視覺慣性SLAM系統可以提高相機的位姿估計精度以及系統魯棒性。

1 相關研究

大多數基于視覺慣性SLAM的移動設備，通常應用在動態環境，經常會遇到移動的對象，如行人、汽車等。因此，研究SLAM系統如何在動態環境下穩定、精確地定位導航，對智能機器人以及增強現實、虛擬現實的普遍應用具有重要意義。

根據SLAM系統在動態環境中受到干擾的分析可知，去除提取特征中的動態特征，僅保留靜態特征可以提高SLAM系統在高動態環境中位姿估計的準確性。現有針對動態環境的SLAM解決方案，主要可以分為：基于幾何信息的動態特征檢測；基于語義信息的動態目標檢測；將幾何信息和語義信息相結合的動態目標檢測。

幾何信息處理方案主要有Bescos等[7]提出的Dyna-SLAM通過結合多視圖幾何和深度學習的方法來檢測動態目標。魏彤和李緒等[8]根據立體視覺幾何約束判別場景中的稀疏特征點，然后根據場景深度和顏色進行場景分割。高成強等[9]通過建立圖像塊實時更新高斯模型，依據方差變化分割出動態對象，通過最小化重投影誤差對相機位姿進行優化。張慧娟等[10]采取了直線特征的靜態權重剔除動態直線特征的方法。

基于語義信息處理的方案主要有房立金等[11]通過深度學習進行語義信息分割，結合特征點光流對語義分割進一步優化，檢測動態目標并去除不可靠的動態特征。Zhang等[12]使用光流殘差來突出RGB-D點云中的動態特征。Brasch等[13]提出一種基于CNN(convolution neural networks)預測的語義先驗信息結合概率模型以及時間運動信息判斷動態特征。利用語義信息來判斷動態目標主要是根據常識和經驗對目標進行判斷，但是可能會將處于靜止的人、汽車等先驗動態目標的全部特征去除，而運動的椅子、桌子等先驗為靜態的目標無法判斷為移動物體。

幾何信息與語義信息相融合的方案包括Cui和Ma等[14]提出的將語義分割和對極幾何信息以緊耦合的方式融合在一起有效檢測并去除動態特征。Yu和Liu等[15]通過將語義分割和運動一致性檢測相結合檢測動態特征。Bescos等結合多視圖幾何和MaskR-CNN[16]檢測動態目標。雖然將幾何信息和語義信息相結合會提升SLAM系統對動態目標的檢測，但由于語義信息的獲取需要算力較高的硬件支持，這不適合移動設備輕便的設計要求。

綜上，現有的動態SLAM算法大多結合語義信息進行動態目標檢測，但要實現精確的動態目標檢測需要構建高性能的目標檢測網絡，但這需要大量的算力，難以應用在嵌入式移動平臺上；同時這些解決方案大多只采用單一的視覺傳感器，已知單一的視覺傳感器容易受到環境的干擾。綜合考慮后，本文提出了一種適用于動態環境的DVI-SLAM系統，它通過在VINS-MONO前端特征提取階段添加動態特征檢測器，同時根據當前幀動態點的數量，自適應調整后端優化中視覺信息的權重，從而提高視覺慣性SLAM系統在動態環境中的位姿的準確性以及系統的魯棒性。

2 DVI-SLAM概述

在真實環境中，精確的位姿估計以及系統的魯棒性是評價智能機器人的一個重要標準。VINS-MONO在目前公開的視覺慣性數據集和真實環境中均有良好表現，因此本文將VINS-MONO作為DVI-SLAM的基本框架，在此基礎上添加了動態目標檢測并改進了后端優化方式，以更好適應動態環境的挑戰。

DVI-SLAM系統的動態特征檢測器總覽如圖1所示，對先前圖像幀和當前幀的特征點進行KLT(Kanade-Lucas-Tomasi)稀疏光流[17]跟蹤，然后，通過對極幾何約束檢測并去除動態特征點，只保留靜態特征點用于位姿估計。

圖1 DVI-SLAM動態檢測器總覽

本文提出的DVI-SLAM系統的整體結構如圖2所示。其中測量預處理模塊對新采集的圖像幀提取亞像素級特征點，然后，通過動態特征檢測器將特征點進行分類，去除動態特征，僅保留真正的靜態特征，采用KLT稀疏光流法對靜態特征點進行跟蹤；其中IMU數據的處理與VINS-MONO相同。在后端非線性優化部分，通過添加自適應視覺信息權重因子，來降低動態環境對系統的影響。初始化模塊、重定位模塊以及全局位姿圖優化模塊與原始VINS-MONO相同。位姿圖優化模塊接受幾何驗證的重定位結果，并進行全局優化以消除漂移。回環檢測模塊是為了判斷是否到達過以前經過的位置，消除軌跡漂移。

圖2 DVI-SLAM系統的整體結構

3 動態點檢測與后端優化

3.1 動態特征點檢測

本文采用對極幾何約束來判斷當前關鍵幀中的特征是否為動態。圖3為靜態特征點在相鄰圖像幀的對極幾何約束，其中X是靜態三維特征點，x1是空間點X在I1幀上的投影，x2是空間點X在I2幀上的投影。C1和C2是相機的光學中心，連接C1和C2的線稱為基線，與兩個關鍵幀相交的點稱為極點e1、e2。C1和C2以及空間點X組成的平面稱為對極平面，對極平面與幀I1、I2相交的直線L1，L2稱為極線。

假設在靜態環境中，已知像素點的像素坐標p1，三維空間點X位于從x1的反投影線C1x上，通過對極幾何約束得到X在關鍵幀I2中的投影點x2的像素坐標范圍，如圖3所示。若三維空間點X為靜態特征點，在已知相機的旋轉和平移后，可以通過對極幾何約束判斷x2的像素坐標。兩幅圖像的對極幾何約束可用相鄰兩圖像間的基礎矩陣來表示

(1)

式中：p1和p2是對應投影x1和x2的像素坐標。I2幀中的極線L2可以通過幀I1中相對應的p1和基礎矩陣F計算得到

L2=Fp1

(2)

圖3 對極幾何約束

如果三維點X是靜態的，根據對極幾何約束，x2投影點一定會在極線L2上。如果三維點X是動態點，X可能會在對極平面之外運動如圖4(a)所示，會導致投影點x′2和極線L2之間有一定的距離d；若是在對極平面內運動如圖4(b)所示，則像素點x′2依然會落在極線L2上。由此可以看出，只有特征點在對極平面之外運動才不滿足對極幾何約束。

圖4 空間三維點的運動可能性

在實際操作中由于特征點的提取和基礎矩陣F的計算均存在誤差，即使三維點X是靜態的，投影點x′2也不一定位于對應的極線L2上，而是距離相應極線有一定距離(即使距離非常小)，如圖4(a)所示。因此可以通過這個約束來判斷特征點是動態點還是靜態點。通過判斷投影點x2到對應極線L2的距離d是否大于預定義的閾值，從而判斷該特征點是否為動態特征點。在具體的實現過程中，本文選擇1個像素作為閾值，即當前幀中的投影點距離相應極線的距離超過1個像素則認為此特征點為動態特征。

假設有

P1=(x1,y1,1)P2=(x2,y2,1)

(3)

式中：P1和P2是投影點的歸一化坐標。極線方程可以通過以下公式計算

(4)

式中：X,Y,Z是極線方程的系數向量，F代表基礎矩陣。投影點到對應的極線的距離方程如下

(5)

通過式(5)可以看出，基礎矩陣F的選取對于距離d的精度非常重要。在相鄰兩幀之間通常可以通過5點法[18]、8點法[19]以及RANSAC(random sample consensus)[20]計算基礎矩陣。最常用的是8點法推導基礎矩陣F。假設

(6)

根據式(1)和式(3)可以得出

Af=0

(7)

式中：A=(x1x2,x1y2,x1,y1x2,y1y2,y1,x2,y2,1),f=(f11,f12,f13,f21,f22,f23,f31,f32,f33)T， 此時的A矩陣只是由一對匹配點組成的矩陣，想要計算出基礎矩陣F則只需找到兩幀之間8對匹配特征點。8點法是計算基礎矩陣F最簡單的方法，為了增強其計算的精度和穩定性，一般先對輸入的匹配點進行歸一化處理，具體過程在本文不作討論。

為了得到兩幀間正確匹配的特征點，采用可以跟蹤特征點運動方向的光流法，在一定程度上也可以對動態物體進行判斷，但考慮到一般光流法計算量較大，因此采取了KLT稀疏光流法對特征點進行跟蹤。采用RANSAC方法簡單去除跟蹤過程中誤匹配的特征點，以得到更加精確的基礎矩陣。因此系統需要在一個相對靜態的環境下完成初始化，并通過RANSAC去除誤匹配的點，得到一個相對精確的初始基礎矩陣。

特征點的提取、動態特征點檢測以及動態特征分離的算法如算法1所示。

算法1： 動態特征點檢測算法

輸入： 先前關鍵幀I1； 當前關鍵幀I2；

輸出： 動態特征點集合D；

(1)先前幀的特征點P1=goodFeaturesToTrack(I1);

(2)亞像素級特征點P1=cornerSubPix(I1,P1);

(3)當前幀特征點P2=CalcOpticakFlowPyrLK(I1,I2,P1);

(4)去除P2中的外點；

(5)F=FindFundamentalMatrix(P1,P2);

(6)forP1,P2中每對匹配的點p1,p2do

L1=ComputeCorrespondEpilines(p1,F);

d=getDist_P2L(p2,L1);

ifd> 1 then

Appendp2to D;

end if

end for

算法1中用到的表達式均來源于OPENCV庫中的相關函數。

圖5表示原始VINS-MONO在3個動態序列中提取的特征點的結果。圖6顯示了本文提出的DVI-SLAM系統去除動態特征后的效果。可以看到與原始VINS-MONO相比DVI-SLAM提取的特征幾乎沒有來自于移動的行人身上。

圖5 VINS-MONO在動態環境中提取的特征點

圖6 DVI-SLAM系統在3個動態序列中的特征提取

3.2 后端非線性優化

當SLAM系統在動態環境中運行時，打破了傳統SLAM靜態環境的基本假設，影響了SLAM系統位姿估計的準確性。經過分析發現，移動目標對視覺慣性系統的影響主要是發生在視覺前端，即在圖像幀提取關鍵特征的過程中使用動態特征點進行特征點匹配。慣性傳感器是由陀螺儀和加速度計構成，因此測量數據并不會受到移動對象的影響，因此可以調整視覺信息的權重來提高系統在動態環境中的魯棒性。

通過設置一個比率M，即當前幀中去除動態特征后剩余靜態特征點的數量與當前幀中特征點的總數的比值，作為調整視覺信息權重的參考條件

(8)

式中：當前幀的特征點的數量，本文采用VINS-MONO中采用的150個特征點。

在滑動窗口內n個IMU幀以及m個特征點的狀態向量為

(9)

通過最小二乘非線性優化方法，最小化滑動窗口內得到的邊緣化先驗信息、視覺殘差以及慣性殘差的平方和。目標函數如下

(10)

(11)

其中自適應調整因子α被定義為

(12)

θ為確定是否舍棄當前幀的視覺測量信息的閾值。閾值θ在實驗過程中根據環境中動態對象的數量進行自適應調整。若當前關鍵幀中的靜態特征點占的比重M小于閾值θ時，則舍棄當前的視覺信息，只保留慣性信息進行位姿估計，以此來保證SLAM系統在動態環境中的魯棒性。

4 實驗評估

在這一部分，本文在現有的公共視覺慣性數據集TUM-VI[21]上對提出的DVI-SLAM系統進行相關實驗。TUM-VI數據集中包含動態的運動場景，這有利于對提出的DVI-SLAM系統在動態環境中的性能進行評估。為了實驗的公平比較，將提出的DVI-SLAM系統與原始VINS-MONO系統在多個動態場景序列中進行了比較，以驗證其在動態環境中的改進效果。算法的實驗平臺為：Intel i5 5200U處理器(2.2 GHz)，8 GB RAM，64位Ububtu16.04操作系統。

4.1 在TUM-VI數據集的評估

TUM-VI數據集提供了許多序列，這些序列提供了20 Hz幀率，分辨率為1024×1024的高動態范圍光度較準圖像。此外還提供了頻率為200 Hz的IMU慣性單元，測量3個軸上的加速度和角速度。相機和IMU在硬件中進行時間同步。同時在數據序列的開始和結束處，給出使用運動捕捉系統以120 Hz的高頻率獲取的精確標準位姿數據，并與相機和IMU測量值對準作為參考標準的地面真實值。為了驗證提出的DVI-SLAM系統在動態環境中的性能，本文選擇具有挑戰性并且包含動態元素的序列來進行相關實驗，即序列Slides1、Slides2、Slides3，這3個序列是在學校的中央大廳采集的，光照明暗變化較大，并且有突然進入的行人，這樣的環境會對SLAM系統構成巨大的挑戰。在TUM-VI數據集的這3個動態序列中，人是主要的動態對象。其中Slides1序列動態對象相對較少稱為低動態環境，其余兩個序列動態對象較多稱為高動態環境。

在選定的3個動態序列上分別運行VINS-MONO和本文提出的DVI-SLAM系統。并把這兩個系統運行估計的運動軌跡與地面真實值繪制在一個圖中。為了更清晰直觀展示對比效果，將三維軌跡投影到二維平面中，以定性和直觀的方式展示兩個系統的定位精度。兩個系統估計的運動軌跡對比結果如圖7所示。通過圖7可以看出，在低動態的場景下VINS-MONO和DVI-SLAM都可以回到出發原點；而在高動態的場景下，VINS-MONO的軌跡會發生偏移而無法正常回到出發原點；DVI-SLAM則偏移較小。

圖7 在3個動態序列中兩個系統的測試結果對比

由于TUM-VI數據集中的Slides1、Slides2和Slides3這3個動態序列，只給出了開始和結束的地面真實軌跡，位置如圖7中的虛線框處，兩個系統的軌跡與真實軌跡對比如圖8所示，其中粗虛線表示真實軌跡，點虛線表示

圖8 3個序列開始和結束階段地面真實軌跡的對比

VINS-MONO的軌跡，實線表示DVI-SLAM的軌跡。若軌跡與給出的地面真實軌跡重合度越高，表示SLAM系統的位姿估計更準確。通過對圖8分析可知，在低動態序列Slides1中兩個系統的軌跡都非常接近地面真實值。而在高動態的Slides2和Slides3序列中，本文提出的DVI-SLAM系統與地面真實值更接近；而VINS-MONO估計的軌跡與地面真實值之間的差距較大，軌跡的偏移較大。這是因為 VINS-MONO僅僅通過使用RANSAC方法粗略的將誤匹配特征點去除，并沒有針對性的考慮移動對象對SLAM系統的影響。相反，新提出的DVI-SLAM系統針對性的對動態特征進行檢測與去除，并改進了后端非線性優化，提高了系統的定位精度，減小了軌跡的偏移。

為了對提出的DVI-SLAM系統做進一步的定性比較，以驗證在動態環境中的性能。本文使用絕對軌跡誤差ATE(absolute trajectory error)的RMSE(root mean square error)作為本文實驗的定性評估指標。絕對軌跡誤差直接計算系統所估計的相機位姿于數據集提供的真實軌跡之間的差。首先，通過時間戳將估計位置與真實位置進行對齊，然后，分別計算每對位姿的差值。絕對軌跡誤差的定義如下

(13)

式中：Qi為真實的位姿，Pi為估計位姿，并通過變換矩陣T將兩者對齊。獲得每幀的絕對軌跡誤差之后，求其均方根誤差

(14)

對于每種方法，都會在相應序列運行5次，以獲得RMSE的中位數、均值、最小和最大值，以降低偶然因素對實驗定性測試的影響。結果見表1。為了更直觀展示DVI-SLAM較VINS-MONO在相應序列的改進情況，在表2中展現了提升的百分比。

根據表1和表2，可以看到在低動態序列Slides1中，這兩種方法的結果非常接近，VINS-MONO的性能要稍好于DVI-SLAM。而在高動態的兩個序列中，DVI-SLAM系統與原始的VINS-MONO相比大大提高了系統的準確性，測試的所有參數，中位數、均值、最小和最大RMSE都有很大幅度得提升。這說明了本文改進后的視覺慣性SLAM算法，在動態環境中展現了更好的性能。因此驗證了提出的DVI-SLAM系統在高動態的環境中實現了較高的定位精度。

4.2 與主流視覺慣性SLAM的對比

為了更全面驗證本文提出系統在動態環境中的性能，將本文的方法與已經發表論文中的實驗結果進行比較。進行對比的系統均在TUM-VI動態序列中測試過的視覺慣性SLAM：OKVIS、ROVIO[22]，這兩個系統沒有對動態物體進行針對處理。選擇DVI-SLAM在測試實驗中RMSE的中位數作為對比值。比較結果見表3。

通過表3，可以看出本文提出的系統在動態環境中的軌跡的誤差遠優于OKVIS、ROVIO。可以驗證提出的DVI-SLAM系統在動態環境中的性能要優于上述兩種視覺慣性SLAM系統。

5 結束語

本文提出一個適用于動態場景的視覺慣性SLAM——DVI-SLAM。通過在視覺前端添加動態檢測模塊，用于檢測動態特征，同時統計每幀靜態特征點所占的比重，用于后端非線性優化過程中自適應調整視覺信息的權重。在高動態的環境中，由于添加了慣性傳感器，當相機提取的有效特征點不足的情況下，也可以通過IMU的測量信息在短時間內進行有效定位，這提高了系統在動態環境中的魯棒性。為了驗證提出系統的有效性，在公開的TUM-VI數據集中進行了相關實驗，并與最新的視覺慣性SLMA做了對比分析。實驗結果表明，在高動態序列中，DVI-SLAM相對于VINS-MONO的定位精度提升了平均47.34%。

表1 RMSE實驗數據對比

表2 較VINS-MONO提升的百分比

表3 與最新視覺慣性SLAM系統的RMSE數據對比