基于光流融合快速特征提取的視覺/慣性定位方法

趙錦陽

北京信息科技大學高動態導航技術北京市重點實驗室，北京 100101

0 前言

近年來，導航技術有了快速的發展，然而在一些特殊環境下，例如地震后的廢墟、災害后的礦洞、地下車庫等地下遮蔽空間，環境的封閉性導致定位困難。由于慣性傳感器有著成本低、重量輕、易攜帶等優點，受環境的封閉性影響較小，因此，在地下遮蔽空間內，慣性定位技術成為人員定位的首要選擇。由于慣性器件本身存在零漂穩定性、噪聲、隨機游走等誤差，導致其在長時間運行時漂移誤差不斷累積，從而導致定位不準確，而相機采集的數據基本不會有漂移，使用相機輔助慣性傳感器可以有效修正慣性傳感器的漂移誤差[1]。因此，采用由相機與慣性傳感器融合的視覺/慣性定位可以解決在地下遮蔽空間內人員定位的難題。

視覺/慣性定位技術目前較為主流的有V-SLAM（Visual-Simultaneous Localization And Mapping）、視覺慣性里程計等。視覺慣性里程計是一種由相機（如單目、雙目、深度攝像頭等）獲取圖像信息，慣性傳感器（IMU）采集慣性信息，將視覺信息與慣性信息進行融合，對相鄰圖像之間的相對位移與姿態進行估計，從而獲得傳感器運動的方法。按照融合信息的方式，視覺慣性里程計（Visual Inertial Odometry，VIO）分為基于濾波方法與基于優化方法。基于濾波的方法采用濾波技術（如卡爾曼濾波、擴展卡爾曼濾波、無跡卡爾曼濾波等）融合視覺信息與慣性信息；基于優化的方法采用非線性迭代技術融合視覺信息與慣性信息。按照相機狀態與IMU狀態是否進行合并分析，視覺慣性里程計分為基于緊耦合與基于松耦合兩種方案[2-5]。在松耦合方案中，VIO對相機與IMU進行單獨的運動估計，分別計算位姿結果，然后對結果進行融合；在緊耦合方案中，VIO在進行狀態估計時將IMU與相機狀態進行結合，共同構建運動方程和觀測方程。

傳統MSCKF方法[6]在視覺前端采用Two Point Ransac提取算法，通過在前后幀跟蹤特征點時進行特征匹配，去除不滿足雙目極限約束的特征點。此算法優點是篩選過程簡單，算法復雜度較小，運行時占用系統內存小；缺點是在光照較差，相機運動過快導致圖像模糊等弱紋理環境下，簡單的篩選機制無法保證優質的特征點篩選率，從而導致累計誤差的問題。

為解決地下遮蔽空間內，弱紋理環境下視覺/慣性定位方法特征點提取、追蹤困難問題，提出一種基于聯合光流法的視覺/慣性定位方法。本文采用SURF（快速魯棒性特征）提取算法[7-8]，通過在特征提取時構建Hessian矩陣，構建高斯金字塔對特征點進行提取，具有取得更好的特征提取速度與特征提取精度，隨后利用LK光流法對特征信息進行追蹤。信息融合部分：利用IMU信息進行狀態與協方差預測，將相機的位姿信息加入特征向量和協方差矩陣中對狀態信息進行擴增。通過特征信息構建觀測模型對狀態預測進行修正。在特征點信息消失或濾波狀態消失時，對卡爾曼濾波器進行更新。本文算法流程如圖1所示。

1 前端圖像信息處理

1.1 SURF算法

1.1.1 構建Hessian矩陣

SURF算法通過求取特征點的Hessian矩陣，經過濾波求得矩陣行列式，利用行列式圖進行圖像近似。在一個尺度為σ的圖像中，若某一個像素點的橫坐標為x，縱坐標為y，則這個像素點的坐標為f(x, y)，它的Hessian矩陣為：

進行類比，可以求出這幅圖像中每一個像素點的Hessian矩陣，進行高斯濾波后得到：

其中，Lxx(x,σ)、Lxy(x,σ)、Lyy(x,σ)——濾波后圖像在各個方向上的二階導數， 求得矩陣行列式的值為：

為了加速卷積的效率，使用方框濾波代替二階高斯濾波。用方框濾波替代表示之后，令Lxx、Lxy、Lyy記為Dxx、Dxy、Dyy，則Hessian矩陣行列式為：

其中，0.9——權重系數。

如此可得一張近似Hessian矩陣行列式圖，通過改變不同尺度的高斯模板即可構建高斯金字塔。

1.1.2 確定特征點方向

以特征點為中心，在尺度為σ的范圍內構建以6σ為半徑的圓，統計在60°扇形內，所有點的x軸方向上與y軸方向上haar小波特征總和。將扇形每經過一定的時間間隔進行旋轉，直到最后得到一個最大值，即為特征點的主方向。

1.1.3 特征點描述與匹配

在特征點的附近選取一個正方形框，將該框16等分，劃分為16個子區域，分別統計每個子區域內25個像素的haar小波特征：其中各項分別是水平方向之和、水平方向絕對值之和、垂直方向之和與垂直方向絕對值之和。四維描述向量為：因此SURF的描述子共有64維。

SURF算法衡量兩個特征描述子相似度的方法是計算其相隔的歐式距離。例如，在尋找A、B兩幅圖像之間的相似特征點時，將圖像A中的一個特征點與圖像B中所有特征點進行相似度比較，歐氏距離最小的為相似點。

1.2 LK光流

LK光流法認為不同時間點對應著不同的圖像[9]。因此，將圖像看做時間的函數I(t)。在t時刻，某個位于(x, y)處的像素，灰度表達式可以寫成I(x, y, t)。通過使用這種函數表達，圖像成為了關于時間與位置的函數，它的值域就是圖像中像素的灰度。

由于相機在傳輸信息時傳輸的是連續幀的圖像，因此可以認為這些圖像是隨時間連續變化的。對于t時刻位于(x, y)處的像素，設在t+dt時刻它運動到(x+dx, y+dy)處，則：

對左邊進行泰勒展開，保留一階項，得：

由LK光流法中灰度不變假設可得，下個時刻的灰度保持不變，與上一時刻的灰度值相等，從而：

兩邊除以dt，得：

將圖像灰度對時間的變化量記為It，寫成矩陣形式為：

由LK光流法中的空間一致假設可得，同一個窗口內的所有像素具有相似的運動。設一個窗口大小為ω×ω，則其含有ω2個像素，則：

令：

則方程變為：

求得其最小二乘解為：

即可得到像素在圖像間的運動速度u、v。

2 信息融合框架

2.1 信息擴增

由四元數Iq表示慣性坐標系中的轉動。向量 GvI∈R3和GPI∈R3表示慣性坐標系下的速度和位置。向量bg∈R3和ba∈R3分別表示陀螺儀和加速度計的測量偏差。則IMU的狀態參數表示為：

由于使用實模態會導致協方差矩陣的奇異性，從而導致IMU的狀態誤差可定義為：

其中，表示誤差四元數。將相機的狀態加入狀態向量中，誤差狀態可以寫作：

其中，——每個相機的位姿誤差。

IMU狀態的連續動態數學模型為：

其中，∈ R3和∈ R3是 陀螺儀和加速度計的無偏差測量值；R(I) 用 于將四元數轉換為旋轉矩陣；Ω() 可 以寫作：

其中，[] 表 示對應于的斜對稱矩陣。誤差慣性測量單元狀態的線性動態方程為：

為了解決慣性測量單元測量時間的離散問題，采用龍格-庫塔數值積分進行狀態轉移。由于慣性測量單元狀態的不確定性，轉換矩陣和噪聲協方差矩陣為：

狀態的協方差矩陣可分為：

完全不確定性轉移Qk+1/k可以表示為：

當接收到新的相機圖像時，狀態進行擴增，增廣協方差矩陣為：

2.2 觀測模型

由于在狀態預測的過程中，誤差會隨著時間積累，因此，引入相機的觀測值對系統狀態進行修正。若單個特征點fj已經在圖像中被觀測，并且觀測值中包含有特征點在像素坐標系下的坐標由于雙目攝像頭的姿態不同，所以同時左攝像頭和右攝像頭的姿態分別記錄為和。 雙目測量表示為：

根據估計的相機姿態，使用給定的最小二乘法計算相機在世界坐標系中的位置。為了更新殘差的協方差，對觀測函數在當前狀態進行線性化，測得的殘差rij的近似值可寫成：

其中，——測量噪聲；

計算追蹤過程中的殘差，通過疊加相同字符fj的各種觀察值：

2.3 濾波更新

濾波更新是算法中的最后一步，在兩種情況下算法會執行濾波更新步驟：（1）一些特征點丟失；（2）當相機狀態的數量達到滑動窗口的上限。執行濾波更新時，所有的特征點信息被用來約束和修正誤差。對每一個追蹤的特征點計算對應的殘差與特征點位置誤差并添加到總體狀態中，修正累積的誤差。

3 實驗分析

為了驗證算法的整體性能，本文在各數據集上對其進行了測試，實驗數據集采用EuRoC室內數據集，IMU采樣頻率為200 Hz，相機采樣頻率為20 Hz。EuRoC數據集由蘇黎世聯邦理工學院開源，采集自微型飛行器上的視覺慣性傳感器，數據集場景為室內機器人大廳與廠房，較好地還原了地下空間光照較差、圖像模糊等問題。實驗設備為筆記本電腦，i59代芯片，rtx960顯卡。軌跡處理使用EVO評測工具，對算法輸出的軌跡與數據集中真實軌跡進行對比，計算估計軌跡與真值的誤差。

分別計算本文方法與MSCKF在不同數據集的運行過程中的平均單幀處理時間，并記錄進行特征點識別時的特征識別效果，進行對比，其結果如圖2、圖3所示。

由圖2可以看出，相較于MSCKF前端Two Point Ransac提取算法，本文方法在單幀處理時間上平均快了約3 ms，證明本算法具有更快的特征點提取速度。圖3中（a）、（b）兩圖分別是本文方法與MSCKF的特征點識別效果，在視覺信息模糊、弱環境紋理、光線較暗環境下，本文擁有更好的特征點識別精度與效果。

將運行數據集后計算出的位姿使用EVO評估工具進行處理，與真實軌跡對齊，并與MSCKF算法進行對比，對比結果分別如圖4 ～圖7所示。

圖中虛線為數據集中真實軌跡，藍色、綠色線條分別為本文方法與MSCKF算法輸出估計值。相較于MSCKF算法，本文估計軌跡更為精確，與真值偏離程度更小。

通過使用EVO評測工具對算法輸出軌跡與真值間的偏差進行分析，計算出算法輸出位姿與真值間的軌跡誤差與均方根誤差，結果如表1、表2所示。

表1 本文算法誤差（單位：m）

表2 MSCKF算法誤差（單位：m）

平均軌跡誤差代表估計軌跡相較于真實軌跡的偏離程度，值越低代表估計軌跡精度越高；均方根誤差代表估計軌跡的離散程度，值越低表明估計軌跡離散越小，系統穩定性越高。以平均軌跡誤差與均方根誤差作為精度標準，本文算法相較于MSCKF算法，平均軌跡誤差平均減少8%，均方根誤差平均減少21%。證明本文算法相較于MSCKF算法擁有更為精確的位姿輸出。

為了進一步驗證本文算法的性能，采用MH_05數據集，與另一種同為基于濾波的VIO算法ROVIO[10]進行對比，軌跡對比結果如圖8所示，精度對比如表3所示。相較于目前主流的ROVIO算法，本文算法有更低的誤差，更高的準確性，證明了本文算法的優越性能。

表3 算法誤差對比（單位：m）

4 結束語

針對弱紋理環境下，視覺/慣性定位方法特征點提取、追蹤困難問題，本文提出了一種視覺/慣性定位方法，通過SURF提取特征點，LK光流法對特征點進行追蹤，將視覺信息與慣性信息進行緊耦合狀態分析，輸出位姿估計結果。通過對EuRoc數據集中的圖像部分進行仿真分析，與MSCKF算法對比，驗證了本文方法具有更快的單幀處理速度與更高的特征點識別精度，證明了本文方法在前端視覺處理部分在弱紋理環境下具有更好的適應性與魯棒性。對輸出位姿進行保存，與真實軌跡對齊，并與MSCKF、ROVIO進行對比，驗證本文方法能夠實現更為精確的位姿估計，且在弱紋理環境下具有更好的定位精度，證明了本文方法的有效性，相較于現有的視覺慣性里程計算法具有顯著優勢。在工業視覺慣性導航領域具有應用前景，為應急救援、位置服務等領域提供了新的思路。