夏華佳 章紅平 陳德忠 李 團

（武漢大學衛(wèi)星導航定位技術研究中心 武漢 430079）

0 引 言

近年來，全場景環(huán)境下的高可用、高可靠、高精度的完備導航參數定位需求日益增長，傳統(tǒng)GNSS/INS組合導航在GNSS信號遮擋區(qū)域，INS的導航誤差迅速累積，定位結果精度快速下降[1]。視覺慣性里程計（VIO）是利用圖像和慣導信息，融合計算獲取定位導航信息的熱點技術，能有效解決GNSS失效情況下導航系統(tǒng)誤差快速發(fā)散的問題，因此被廣泛研究與應用。根據其融合框架，可以分為基于濾波器的VIO和基于優(yōu)化的VIO[2]。前者核心思想為慣導測量載體的運動數據用于狀態(tài)傳播，相機測量的信息數據用于狀態(tài)更新估計。而后者需要處理在軌跡上所有的測量，估計和平滑載體狀態(tài)，由于這類方法擁有迭代的運算機制，能夠修正歷史的狀態(tài)估計，因此可達到較高的運動估計精度。但基于優(yōu)化的方法計算量較大，算法設計更復雜，無法在一些算力有限的平臺上運行。而基于濾波器的VIO算法可同時兼顧計算效率和精度，且大多數平臺上均能獲取準確一致的定位結果[3]。

基于濾波的VIO又分為松耦合[4]、緊耦合[5]這2種模式。松耦合模式是將視覺運動估計的位置、速度、姿態(tài)變化的結果作為觀測量構建觀測方程，與慣性導航系統(tǒng)方程進行融合解算；緊耦合模式則是直接將相機觀測到的圖像特征信息作為觀測量，構建觀測方程對慣導誤差進行修正。緊耦合算法中具有代表性的是MSCKF算法，采用了1個滑動窗口維護多個時刻的相機位姿，以多幀圖像共視到同1個特征點為出發(fā)點，特征點與圖像之間的約束關系構建觀測方程，估計并輸出最新時刻的位姿[6]。該方法直接使用了圖像特征點原始觀測值，因此視覺粗差對VIO解算精度影響極大。為了減少視覺粗差，需要提高VIO數據關聯的準確性，即特征點跟蹤匹配的準確度。

目前，特征點跟蹤匹配算法大多采用基于描述符特征點匹配，或光流法匹配[7]。描述符的特征點匹配方法，描述了特征點與周圍像素分布特征，可將其作為目標匹配的依據；光流法則是依據同一特征點灰度不變原理，尋找該點在下一幀圖像中可能出現的位置，從而實現匹配。在Liu等[8]的研究中改善了特征點的局部描述符，采用融合全局環(huán)境描述符，使得特征信息具備一定的空間描述能力；在成怡等[9]的成果中提出了基于Hamming距離和余弦相似度結合的描述符匹配方法，降低了多相似區(qū)域圖像特征點的誤匹配率；而鄒斌等[10]的研究中則采用區(qū)域分塊對圖像進行預處理，挑選最佳匹配子塊預期得到最佳匹配。以上3種方法均屬于對描述符表達能力及匹配策略進行改進，能夠獲取較為準確數據關聯，但匹配過程計算復雜，實時性較差。基于光流的方法中，王亭亭等[11]采用前后雙向光流方法提高了特征點的跟蹤精度，該方法不需要計算特征點描述符，節(jié)約了描述符暴力匹配時間，但在實際環(huán)境中由于光照、載體動態(tài)等因素引入大量錯誤匹配點，導致特征點跟蹤匹配的準確度改善有限。

針對濾波框架下的視覺慣性里程計定位精度易受特征點異常匹配值影響的問題，光流法具備較高的計算速度，但假設條件過于嚴苛導致跟蹤匹配不準確；基于描述符的特征點匹配在準確性上存在一定的優(yōu)勢，但描述符計算和匹配過程復雜。因此，本文結合了快速角點檢測（FAST）、旋轉二值描述符（rotated BRIEF）和Lucas-Kanade（LK）光流算法，提出了rBRIEF描述符輔助光流跟蹤匹配的特征點數據關聯方法，在保留計算效率的同時，可有效剔除光流異常匹配點對，提高特征點跟蹤匹配的準確度，進一步提升MSCKF算法的定位性能。

1 特征點數據關聯方法

基于特征點的數據關聯主要是完成序列圖像中相鄰2幀圖像之間特征點的匹配，本文的視覺數據處理方法見圖1，主要包括FAST特征點提取、LK光流跟蹤匹配以及基于rBRIEF描述符的特征匹配點對優(yōu)選。

圖1 基于特征點的數據關聯方案Fig.1 Data association scheme based on feature points

1.1 FAST角點檢測

FAST角點檢測算法檢測圖像中較為特別的點，基本思想是1個像素的灰度大小與其鄰域內足夠多像素點灰度差異達到某種標準時，則認為該像素為角點。

以圖像內任意1個像素作為候選特征點，建立以該像素點為中心，半徑為3、周長為16像素的圓形檢測模板。如圖2所示，順時針方向對圓上的像素點進行編號，若圓上有連續(xù)N（一般取值為9）個像素點灰度值都比圓心灰度加上設立的閾值大，或者都比圓心灰度減去設立的閾值小，則將該點視為目標點。

圖2 FAST角點檢測模板Fig.2 FAST corner detection template

為了避免檢測到的角點過于集中在1個區(qū)域，導致角點的分布性不好，一般做法是對圖像進行網格化，在網格化后的所有小圖像塊內進行非極大值抑制操作，只保留Shi-Tomas響應值最大的角點作為該圖像提取到的特征點。

1.2 LK光流跟蹤匹配

光流是像素在2幅序列圖像中運動的表現方式，通過估計像素的位移完成特征點的匹配，一般分為稠密光流和稀疏光流。前者指的是計算圖像中所有像素的運動，后者則計算部分像素運動。稀疏光流以LK光流法為代表。

LK光流基于以下3個假設：像素由1幅圖像流向另外1幅圖像，像素的灰度保持不變；相鄰2幀圖像之間運動變化不大；像素及其鄰域像素具有相似的運動[12]。假設圖像I中1個包含紋理信息的特征窗口W=(-wx,wx)×(-wy,wy)，中心點坐標u=(ux,uy)，當圖像發(fā)生變化時，LK光流跟蹤的目的是在圖像J中找到另1個點v=(ux+dx,uy+dy)，使得2處窗口內像素點的灰度值近似相等，定義像素窗口的灰度誤差函數。

此時，LK光流跟蹤的任務求解合適的d，使得誤差函數ε(d)達到最小。其中，I(x,y)與J(x,y)分別代表對應圖像坐標(x,y)處的灰度值。運動量d=(dx,dy)為計算光流，也稱為像素點u的偏移量。式（1）描述的灰度誤差函數，關于變量dx，dy求解雅可比矩陣。

在2幀圖像之間為小運動的前提下，式（2）中，在d=0處對J(x+dx,y+dy)進行泰勒展開，再令灰度函數關于偏移量的雅可比矩陣為0求解光流，進而得到匹配關系。

LK光流法假設2幀圖像之間運動不大，但當這個假設不成立時，跟蹤匹配會出現較大的誤差。為了解決這個問題，通常使用基于金字塔分層的LK光流[13]。對原尺寸圖像進行若干層級的縮放，即可將處于金字塔底端像素的大運動在金字塔頂端被視為小運動，滿足了LK光流的計算條件。

將金字塔最頂層Lm光流初始值傳遞值記為，通過最小化灰度誤差函數，計算的得到Lm層的殘余光流，則最終計算光流為，假設金字塔圖像縮放因子為0.5，利用和可以估計次1層光流的初始傳遞值為

依次遞推，即可計算原始尺寸大小的圖像光流位移d。

1.3 rBRIEF描述符精確匹配

FAST特征點經過金字塔分層的LK光流跟蹤后，得到相鄰2幀圖像特征點匹配結果。在實際情況中，圖像不可避免地受到光照、噪聲以及運動狀態(tài)等因素的影響，光流計算假設并不成立，導致跟蹤匹配結果不準確，即使是基于金字塔分層的光流法改善也很有限。特征點描述符在一定程度上可以反應特征點以及特征點周圍像素的分布特征，以此作為約束條件進一步濾除部分匹配點，可提高光流跟蹤匹配的準確性。

特征描述符可分為2種形式，梯度直方圖描述符和二進制描述符。本文選擇二進制rBRIEF描述符[14]作為異常匹配判斷依據的原因為：①相對梯度直方圖描述符，例如SIFT，SURF特征，在計算和對比匹配時均需要進行多種復雜且耗時的操作，而rBRIEF描述符作為1種二進制字符向量，其描述向量以及相似度判斷均可以較高效地計算；②常規(guī)車載運動連續(xù)圖像之間的旋轉非常輕微，而rBRIEF描述符具備一定的旋轉不變性，足夠滿足該場景下的應用需求。

rBRIEF描述符主要步驟包括FAST特征點方向計算以及進行描述的二進制編碼。對FAST特征點采用灰度質心法計算方向：以特征點為中心，半徑為r的小圖像塊為例，定義圖像塊的矩為

式中：(x,y)為定義圓域內的像素坐標；I(x,y)為像素點(x,y)處的灰度值。則該圓域圖像的質心有

特征點的方向可表示為

為了使得描述子具備一定的旋轉不變性，rBRIEF描述符根據上一步計算得到的FAST特征點方向，以該特征點為原點，x軸正半軸方向為特征方向，y軸垂直x軸建立坐標系，見圖3，在該坐標系下以特定模式選取n對點對，按照一定的準則對比這n對點對的灰度大小，定義比較準則τ見式（7）。

圖3 rBRIEF描述符旋轉不變性Fig.3 Rotation invariance of rBRIEF descriptor

式中：I(a)為點a處的灰度值。將這n對點對的灰度大小比較結果按位編碼即可得到該特征點的rBRIEF描述符。

對光流跟蹤后所有特征點匹配對，重復以上操作計算rBRIEF描述符，通過Hamming距離對每一對匹配點對的描述符進行相似度比較，對沒有通過的特征點匹配對進行濾除，最終再利用RANSAC算法[15]進行幾何校驗，選取算法結果內點作為最終的匹配結果。

2 MSCKF視覺慣性里程計算法

采用本文提出的rBRIEF描述符輔助光流跟蹤匹配的數據關聯方法進行圖像特征點跟蹤匹配，進一步基于MSCKF濾波器對視覺、慣導數據進行融合解算，獲取運動載體的位姿信息。設計視覺慣性里程計定位算法見圖4。

圖4 MSCKF視覺慣性里程計算法框圖Fig.4 MSCKF-based visual-inertial odometer algorithm

利用GNSS/INS松耦合提供位置、速度、姿態(tài)信息，完成VIO的初始化。在融合解算過程中，INS作為主要的傳感器進行航位推算，視覺作為輔助手段對導航誤差進行修正。MSCKF算法維護了1個包含時間序列上連續(xù)的位姿信息窗口，因此，待估計參數包括當前時刻慣性測量單元（inertial measurement unit，IMU）狀態(tài)誤差以及滑動窗口內相機狀態(tài)誤差，定義單個相機狀態(tài)誤差為

當處理下1幀圖像時，MSCKF完成現有誤差參數的狀態(tài)傳播。

式中：Φk,k-1為k-1時刻到k時刻的系統(tǒng)轉移矩陣，其值由狀態(tài)參數的動力學模型來決定；和Pk-1分別為k時刻狀態(tài)參數預測方差陣和k-1時刻狀態(tài)參數方差陣；Qk-1為系統(tǒng)噪聲方差陣，由狀態(tài)參數的動力學模型精度確定。

同時對該時刻IMU位置、旋轉誤差項進行增廣，以及增廣后誤差參數的方差協(xié)方差陣的更新。

濾波觀測更新是建立在多幀圖像共視到同1個特征點基礎之上。當連續(xù)被跟蹤到的特征點從當前相機視野中消失，或窗口內包含歷史IMU位姿個數達最大需要移除部分狀態(tài)時，不再被跟蹤到的特征點以及被移除狀態(tài)時刻相機視野中所有的特征點均被用于構建觀測方程。此時，滑動窗口內多個與相機固連的IMU位姿受特征點fi(i=1,2,…,m)約束。假設特征點fj被連續(xù)的n幀圖像觀測到，這n幀圖像對應的相機位姿可記為，采用逆深度參數化的估計方法恢復該特征點的地固系e坐標，重投影至第i幀圖像對應的相機坐標系Ci，投影后的坐標可表示為

根據相機模型，式表示的特征點在相機坐標系下坐標，經過歸一化投影，在Ci對應的像素平面坐標表示為

式中：fx和fy分別為像素平面x軸、y軸上的歸一化焦距（標定已知）；為圖像噪聲向量。為了方便描述，在第i幀圖像中，特征點fj像素觀測值記為，根據式（15）和式（16）計算得到fj像素估計值記為，像素觀測值與估計值做差即構成了測量殘差。

為了建立測量殘差與待估計量關系，將上式線性化可得

將式（15）擴展至滑動窗口內所有觀測到該特征點的狀態(tài)，得到該特征點構建的觀測方程。

視覺輔助INS解算需要對δx進行估計，然而由于觀測方程中特征點坐標誤差項的存在，需要對觀測方程進行解耦操作，目的是消除待估計狀態(tài)量與特征點3D位置坐標誤差之間的相關性，使得解耦后的觀測方程滿足卡爾曼濾波更新的要求。式投影至的左零空間。

一般而言，在MSCKF某更新時刻，存在多個滿足條件的特征點用于構建觀測方程，每個特征點對窗口內多個狀態(tài)形成約束，構建式（20）類型的觀測方程，堆疊所有點構建的觀測方程完成導航狀態(tài)誤差量的參數估計。

3 實驗與分析

本節(jié)實驗分別從特征匹配效果和定位精度2個方面，對本文方案進行評價。數據采集設備中，視覺/慣性數據采集硬件包括Basler acA1600-20gm型工業(yè)相機、普通MEMS級別慣導M40（內置慣導型號為ADIS16460）。此外，POS320作為1款高精度導航級光纖慣導，與GNSS載波相位差分后處理（PPK）進行組合濾波和反向平滑處理，可為車載定位提供參考真值。2款慣導及Basler acA1600-20gm相機的主要性能參數見表1～2。

表1 慣導設備參數Tab.1 Equipment parameters of inertial-navigation

表2 Basler acA1600-20gm相機參數Tab.2 Parameters of Basler acA1600-20gm camera

3.1 特征點匹配效果分析

分別采用原始金字塔分層的LK光流跟蹤匹配方法和本文提出的加rBRIEF描述符優(yōu)化光流跟蹤匹配方法，對實測車載視覺圖像提取到的FAST特征點進行了跟蹤匹配。圖5描述了上述2類方法處理后特征點匹配點對數量情況。

圖5中，藍色曲線表示每幀圖像均可提取到足夠多的FAST特征點，其個數均在150個以上；光流跟蹤后，絕大多數特征點均被視為跟蹤匹配成功，綠色曲線則對應著匹配點對的數量情況；而結合rBRIEF描述符對光流跟蹤匹配結果作濾除后，留存的匹配點對數量進一步減少，如紅色曲線所示。這是因為圖像灰度本質上是由光照強度、圖像拍攝角度等因素綜合決定，而光流法在求解特征點運動偏移量時，采用運動參數擬合灰度誤差函數，這使得只有在小運動、光照不變等假設前提下，求解過程中擬合才不會引入較大的模型誤差。圖6給出了本次測試中載體行駛的速度以及姿態(tài)信息。

圖5 特征點跟蹤匹配點對數量圖Fig.5 Quantity of feature points tracking matching-point pairs

圖6 載體導航狀態(tài)Fig.6 Navigation status of the carrier

當載體處于平穩(wěn)行駛狀態(tài)時，rBRIEF描述符剔除部分特征不穩(wěn)定的匹配點對，特征數據優(yōu)化后留存的特征匹配點對在100以上；而當載體出現較大機動時，例如轉彎時刻，圖5中紅色曲線總會伴隨1個尖峰跳躍，特征匹配點對數量銳減至100以下，這與上述光流跟蹤適用情況原理分析一致。

抽取其中具有代表性圖像進行視覺效果分析，圖7中給出轉向場景下基于rBRIEF描述符濾除異常匹配點對前后的特征匹配圖。如載體表面輪廓、標識牌、建筑表面以及路面等處，利用rBRIEF描述符對像素及其鄰域像素分布特征進行描述、比對，有效地對這些錯誤匹配點對進行濾除。

圖7 rBRIEF描述符濾除異常匹配前后對比Fig.7 Comparison before and after rBRIEF descriptor filtering exception matching

3.2 定位算法性能分析

從VIO定位精度方面對本文提出的視覺特征點匹配優(yōu)化方案進行檢驗評估。對比算法采用基于原始金字塔LK光流跟蹤匹配的MSCKF，簡記為VIO1.0，基于本文提出的rBRIEF描述符優(yōu)化特征點跟蹤匹配方案的MSCKF在本實驗中簡記為VIO2.0。2種方法的解算軌跡以及典型定位誤差曲線見圖8～9，表3～4給出了相關定位誤差的統(tǒng)計結果。

圖8為視覺慣性組合導航解算軌跡與真值軌跡對比，由于視覺輔助定位缺乏絕對外部觀測，導致VIO解算存在累計誤差，總體呈現發(fā)散趨勢。VIO2.0采用rBRIEF描述符對光流跟蹤匹配結果進行精化，視覺數據關聯準確度更高，定位累計誤差較小。因此，相較于VIO1.0，VIO2.0軌跡跟真值軌跡更加貼合。

圖8 真值與不同算法解算軌跡圖Fig.8 Solving trajectory of the true value and different algorithms

圖9描述了2種算法解算定位結果與參考真值對比后的位置、姿態(tài)誤差曲線。由圖9可知：基于本文視覺特征匹配優(yōu)化方案的MSCKF算法，定位誤差更貼近0誤差標準線，這與圖8分析結果一致。結合圖6所示的載體行駛姿態(tài)變化曲線，每當載體在轉彎時，北向、東向位置誤差改善的尤為明顯，航向誤差發(fā)散速度也更緩慢。

圖9 2種算法解算定位結果誤差曲線圖Fig.9 Two algorithms solving the error curves of positioning results

表3與表4分別統(tǒng)計了2種算法解算位姿的最大誤差值（MAX）和均方根誤差（RMS）。376 s內，VIO1.0定位結果北-東-高3個方向最大發(fā)散誤差為7.74 m，16.80 m，2.63 m，3D位置最大發(fā)散誤差為16.90 m，考慮到載體在該段時間內行駛距離超過2 km，定位誤差漂移約為0.84%；而VIO2.0定位結果在這3個方向最大發(fā)散誤差5.51 m，7.17 m，2.43 m，3D位置最大發(fā)散誤差為7.47 m，定位誤差漂移優(yōu)于0.38%。2種方案的RMS值在垂向方向相當，而在平面方向本文方法具有明顯的改善，尤其是東方向改善達到56.5%。姿態(tài)結果與位置結果類似，VIO2.0方案的3個方向最大誤差值均優(yōu)于VIO1.0，YAW方向的改善達到45.2%，進一步說明本文方法的有效性。

表3 VIO1.0解算誤差統(tǒng)計表Tab.3 Statistics of VIO1.0 solution errors

表4 VIO2.0解算誤差統(tǒng)計表Tab.4 Statistics of VIO2.0 solution errors

本實驗共采集7 191張序列影像，對圖像處理運算時間進行分析。使用基于rBRIEF描述符輔助光流跟蹤匹配的數據關聯方法在計算機上進行處理，各步驟處理時間如下：FAST特征點提取時間花費195.82 s，3層金字塔光流跟蹤時間花費22.00 s，描述符濾除異常匹配點時間花費64.90 s，平均處理單幀圖像時間約為39 ms。傳統(tǒng)金字塔光流跟蹤平均處理單幀圖像時間約為30ms，使用ORBSLAM2中基于描述符匹配方法處理單幀圖像時間約為68 ms。本文提出的算法相較于傳統(tǒng)金字塔光流跟蹤，單幀圖像處理時間增加9 ms，但能夠改善54.7%的位置漂移；而相較于ORBSLAM 2中基于描述符特征點匹配方法，本文提出的方法處理時間縮短了42%。

4 結束語

基于MSCKF的視覺慣性里程計定位方法，具有計算效率高、可移植性強等特點，但其定位精度嚴重依賴于視覺特征點跟蹤匹配的準確性。本文提出了1種rBRIEF描述符輔助光流跟蹤匹配的數據關聯方法，該方法利用描述符具備描述特征點及其周圍像素分布信息的能力，計算每對匹配點的描述符，根據對描述符的相似度判斷是否濾除該對點，從而提升特征點匹配的準確度。實驗表明：當載體存在較大的機動時，該方法能有效地剔除異常匹配點；在實測載體行駛距離超過2 km的大尺度場景下，基于描述符輔助光流跟蹤匹配的MSCKF算法整體位姿均要優(yōu)于原始光流跟蹤的MSCKF，尤其是位置漂移率優(yōu)于0.38%，改善約為54.7%。同時，該算法具有圖像處理速度快的優(yōu)點，與ORBSLAM2[17]圖像特征匹配算法相比，單幀圖像處理時間縮減42%。