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

趙錦陽(yáng)

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

0 前言

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

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

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

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

1 前端圖像信息處理

1.1 SURF算法

1.1.1 構(gòu)建Hessian矩陣

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

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

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

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

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

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

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

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

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

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

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

1.2 LK光流

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

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

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

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

兩邊除以dt，得：

將圖像灰度對(duì)時(shí)間的變化量記為It，寫(xiě)成矩陣形式為：

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

令：

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

求得其最小二乘解為：

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

2 信息融合框架

2.1 信息擴(kuò)增

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

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

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

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

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

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

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

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

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

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

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

2.2 觀測(cè)模型

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

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

其中，——測(cè)量噪聲；

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

2.3 濾波更新

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

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

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

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

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

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

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

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

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

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

平均軌跡誤差代表估計(jì)軌跡相較于真實(shí)軌跡的偏離程度，值越低代表估計(jì)軌跡精度越高；均方根誤差代表估計(jì)軌跡的離散程度，值越低表明估計(jì)軌跡離散越小，系統(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)行對(duì)比，軌跡對(duì)比結(jié)果如圖8所示，精度對(duì)比如表3所示。相較于目前主流的ROVIO算法，本文算法有更低的誤差，更高的準(zhǔn)確性，證明了本文算法的優(yōu)越性能。

表3 算法誤差對(duì)比（單位：m）

4 結(jié)束語(yǔ)

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