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

趙錦陽

北京信息科技大學(xué)高動態(tài)導(dǎo)航技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室，北京 100101

0 前言

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

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

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

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

1 前端圖像信息處理

1.1 SURF算法

1.1.1 構(gòu)建Hessian矩陣

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

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

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

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

其中，0.9——權(quán)重系數(shù)。

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

1.1.2 確定特征點(diǎn)方向

以特征點(diǎn)為中心，在尺度為σ的范圍內(nèi)構(gòu)建以6σ為半徑的圓，統(tǒng)計在60°扇形內(nèi)，所有點(diǎn)的x軸方向上與y軸方向上haar小波特征總和。將扇形每經(jīng)過一定的時間間隔進(jìn)行旋轉(zhuǎn)，直到最后得到一個最大值，即為特征點(diǎn)的主方向。

1.1.3 特征點(diǎn)描述與匹配

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

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

1.2 LK光流

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

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

對左邊進(jìn)行泰勒展開，保留一階項(xiàng)，得：

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

兩邊除以dt，得：

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

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

令：

則方程變?yōu)椋?/p>

求得其最小二乘解為：

即可得到像素在圖像間的運(yùn)動速度u、v。

2 信息融合框架

2.1 信息擴(kuò)增

由四元數(shù)Iq表示慣性坐標(biāo)系中的轉(zhuǎn)動。向量 GvI∈R3和GPI∈R3表示慣性坐標(biāo)系下的速度和位置。向量bg∈R3和ba∈R3分別表示陀螺儀和加速度計的測量偏差。則IMU的狀態(tài)參數(shù)表示為：

由于使用實(shí)模態(tài)會導(dǎo)致協(xié)方差矩陣的奇異性，從而導(dǎo)致IMU的狀態(tài)誤差可定義為：

其中，表示誤差四元數(shù)。將相機(jī)的狀態(tài)加入狀態(tài)向量中，誤差狀態(tài)可以寫作：

其中，——每個相機(jī)的位姿誤差。

IMU狀態(tài)的連續(xù)動態(tài)數(shù)學(xué)模型為：

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

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

為了解決慣性測量單元測量時間的離散問題，采用龍格-庫塔數(shù)值積分進(jìn)行狀態(tài)轉(zhuǎn)移。由于慣性測量單元狀態(tài)的不確定性，轉(zhuǎn)換矩陣和噪聲協(xié)方差矩陣為：

狀態(tài)的協(xié)方差矩陣可分為：

完全不確定性轉(zhuǎn)移Qk+1/k可以表示為：

當(dāng)接收到新的相機(jī)圖像時，狀態(tài)進(jìn)行擴(kuò)增，增廣協(xié)方差矩陣為：

2.2 觀測模型

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

根據(jù)估計的相機(jī)姿態(tài)，使用給定的最小二乘法計算相機(jī)在世界坐標(biāo)系中的位置。為了更新殘差的協(xié)方差，對觀測函數(shù)在當(dāng)前狀態(tài)進(jìn)行線性化，測得的殘差rij的近似值可寫成：

其中，——測量噪聲；

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

2.3 濾波更新

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

3 實(shí)驗(yàn)分析

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)束語

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