一種基于證據推理的慣性/視覺里程計深組合導航方法

孫 偉，苗宏勝，李海軍，徐海剛，裴玉鋒

（北京自動化控制設備研究所，北京100074）

隨著飛行器長時間、遠程航行需求的不斷增強，對導航系統的精度和自主性要求不斷提高。純慣性導航系統的導航誤差隨時間積累而不斷增大，不能完全滿足實際應用的需要。慣性/視覺里程計組合導航可在不提高慣性元件精度的情況下，僅通過增加視覺傳感設備，獲得載體運動的速度，提升導航系統的導航精度，節約成本。

視覺里程計技術的基本內涵為：單目相機通過前后幀圖像的差異，在已知距離的情況下可以測量出載體的速度[1-4]。而雙目視覺里程計技術利用雙目相機，通過左右兩個相機圖像差異測出距離，通過前后幀圖像的差異測量出載體的速度，但是如果飛行器的高度越高，雙目視覺的測距精度越低，因此視覺里程計應用于飛行器上時，需要利用其他手段獲得距離信息。

視覺里程計的圖像匹配算法分為兩大類[5-8]：基于區域的算法和基于特征的算法?；谔卣鞯钠ヅ渌惴ㄔ诔叨壬暇哂幸欢ǖ牟蛔冃?，對相機的姿態要求不很高。但在實時圖與基準圖差異較大的情況下會產生大量的誤匹配點，或因匹配點數過少而無法對匹配結果進行判斷?；趨^域的匹配算法包括灰度相關算法、相位相關算法等，在這類算法中，圖像的像素點陣直接參與匹配運算，灰度相關算法參與匹配的是圖像灰度強度點陣，并可計算出圖像匹配的置信度，由置信度來對匹配結果進行判斷。

傳統圖像匹配置信度研究存在缺陷，使用單一特征值來衡量匹配置信度，極有可能出現誤報；使用多個特征值來衡量匹配置信度，可能出現相互矛盾的情況。

傳統的視覺里程計技術存在缺陷，受相機參數、測量距離的范圍等影響，視覺里程計輸出的速度信息與真實值將會存在一定的差異；并且，傳統視覺里程技術在實時性和精確性方面難以同時滿足組合導航的需求。

本文針對視覺里程計的技術特點，在進行圖像匹配的過程中，使用衡量匹配置信度的兩個特征值，基于證據推理理論，將兩個特征值進行融合計算，獲得綜合置信度，在減少誤報概率的同時增強置信度計算的準確性；在慣性/視覺里程計深組合導航過程中，利用慣性信息輔助提升視覺里程計計算的實時性和準確性，并對視覺里程計的誤差進行深入建模與分析，利用卡爾曼濾波器對慣性、視覺里程計的誤差進行精確估計，實現高精度組合導航。

1 圖像匹配算法與特征值分析

1.1 歸一化積相關亞像素匹配算法

1.1.1 歸一化積相關匹配算法

歸一化積相關匹配算法的本質是衡量兩幅圖像相似程度的度量準則。其過程是，實時圖像在基準圖中從起始位置（基準圖左上角）逐行掃過整個基準圖；在每一個位置將實時圖像中每個像素點與它所對應基準圖區域像素點進行相關性運算，將結果記錄在相應的位置處；最后選取相關性最高像素點處，作為匹配結果。

相關性的計算公式為：

式中，(x,y)為實時圖像中的坐標點；(u,v)為基準圖像中的坐標點；T(x,y)為實時圖像在點(x,y)處的灰度值，范圍為0 到255；為實時圖像在基準圖像(u,v)處的均值；I(x+u,y+v)為基準圖像點(u,v)處，與實時圖像相對應位置處的灰度值；為基準圖像(u,v)處與實時圖像相同區域的均值。

1.1.2 歸一化積相關亞像素匹配

歸一化積相關法得到的結果是實時圖的整像素位置，實際目標位置不一定在整像素點上，將會導致亞像素級的匹配誤差，為降低亞像素級的匹配誤差，提出歸一化積相關亞像素匹配。

由于相關函數矩陣在以最大值為中心的一個單峰值區域上通過近似滿足二次多項式，因此可以通過擬合方法得到該區域相關系數的解析曲面函數，取曲面極值點為匹配結果的亞像素位置。該曲面的方程可由式(2)表達：

式中，x、y為圖像平面坐標系中的橫、縱坐標；f(x,y)為指定坐標點的匹配相關度。

對式(2)中x、y分別求偏導，且偏導為零時，f(x,y)取得極值，如式(3)所示：

式中，xm、ym即為求得的亞像素匹配結果。

可見，計算亞像素匹配結果轉化為求取曲面參數，而曲面參數的求取可通過取以整像素最大值點為中心取3×3 的小窗口，將該窗口中所有數據作為最小二乘擬合方法的輸入值，構建擬合方程即可求得曲面參數。

通過上述兩個步驟，即可求得兩幅圖像相關性最高點，但是由于實際圖像獲取過程中，姿態變化、光照變化等因素，歸一化積相關亞像素匹配結果會產生誤匹配，需要一些特征值來評估景象匹配結果的準確性及剔除誤匹配結果。

1.2 特征值算法

1.2.1 信噪比SNR

實時圖和基準圖的相似度是體現景象匹配系統性能的重要因素之一。信噪比SNR(Signal Noise Ratio)是用來衡量兩圖之間的相似程度的重要指標。

實時圖是基準圖累加一定噪聲構成的，將基準圖定義成信號，則由天氣、圖像采集等因素對基準圖的影響就是噪聲。通過大量試驗數據可獲得SNR與圖像匹配正確率的關系，結果如圖1所示。

圖1 SNR 與正確率關系圖Fig.1 The relationship between SNR and accurate rate

可見SNR與匹配正確率為正相關，與上述分析相符。當SNR大于0.85，匹配正確率基本為100%。

1.2.2 次高峰與最高峰相關性之比SMR

次高峰與最高峰相關性之比(Sub Max Ratio,SMR)定義為：

式中，CCSubMax表示次高峰的相關性，CCMax表示最高峰的相關性。該特征值表征的是基準圖中兩幅實時圖同時存在的概率，越大說明越容易發生誤匹配。

通過大量試驗數據可獲得SMR與圖像匹配正確率的關系。以0.01 個SMR作為間隔進行統計，結果如圖2所示。

圖2 SMR 與正確率關系圖Fig.2 The relationship between SMR and accurate rate

可見SMR與匹配正確率是負相關，當SMR小于0.7 時，匹配正確率為100%，說明此時實時圖在基準圖中是獨特的。

用單一特征值評估圖像匹配結果存在較大的風險，如圖1中SNR大于0.85 時，正確率基本為100%，但是此時仍有可能出現誤匹配；而若條件過于苛刻，會將一些正確的匹配結果判定為錯誤的，即誤報概率較大。因此需要將特征值進行融合，以增加特征值評估的可靠性，降低誤報概率。因此，本文基于證據推理理論，將兩種特征值的圖像匹配置信度進行數據融合，求取綜合置信度。

2 基于證據推理的綜合信度計算方法

證據推理理論將對特征值的單一處理轉化為融合處理，它即強調特征值的客觀性又稱靜態性，又強調了特征值對圖像匹配正確率估計的主觀性又稱動態性，一定程度上解決了可能發生的特征值估計圖像匹配正確率高度沖突的融合問題。

特征值對圖像匹配正確率估計的可靠度主要來自于特征值對應的圖像匹配置信度的靜態權重及動態權重[9]。

靜態權重作為先驗信息體現了不同特征值對圖像匹配正確率估計具有不同的可靠性和權威性，在本文中體現的是特征值自身的可靠性。一般認為特征值具有相同的靜態權重，如有L個特征值時，每個特征值的靜態權重ωi=1/L，i∈L。

動態權重指一些特征值應用于圖像匹配時所表現出的相互支持度，它以非直接的方式表現了特定特征值對應的圖像匹配置信度的可靠性。當某一特征值被其它特征值支持的程度大，說明該特征值具有更大的可靠度，反之亦然。采用經靜態權重折扣后的特征值圖像匹配置信度與加權平均圖像匹配置信度的距離計算相似度、支持度和動態權重。

設SMR 特征值的序號為1，SNR 特征值的序號為2?，F給出基于證據推理的融合計算步驟：

1）給定各特征值的靜態權重ωi，這里設ω1=0.5,ω2=0.5。

式中，βij為每個證據ei（i=1…L）在各個命題Hj（j=1…N）下的初始信度。

3）計算動態權重si′：

式中，si=1-di，L為特征值個數。

4）依據飛行試驗條件和對特征值的認知確定靜態權重比例λ，并計算各特征值對應圖像匹配置信度的綜合權重αi：

5）用綜合權重對各特征值的mass 進行折扣，并計算其加權平均值；

6）則第j次圖像匹配的綜合信度為βj=，j=1…N。

對圖像匹配試驗數據進行統計，經過上述步驟的數據融合計算，可得出綜合信度與正確率的關系曲線，如圖3。

圖3 綜合信度與正確率的關系曲線Fig.3 The relationship between apprehensive credibility and the accurate rate

可見經過數據融合，綜合信度大于0.9 時，匹配正確率高于95%。

3 基于慣性信息的視覺里程計技術

3.1 基本原理

基于慣性信息的視覺里程計的基本原理是視覺里程計通過慣導獲取載體姿態角、載體速度等信息，利用慣導的速度、姿態信息預測（計算）實時圖在基準圖中的坐標，將該坐標作為搜索區域的中心點，搜索區域的大小則由慣導的速度、姿態誤差決定；利用水平姿態角信息修正由于飛行姿態問題帶來的視覺里程計測速誤差，提升視覺里程計測速精度；利用慣導輸出的當前姿態角、速度信息，將速度信息進行積分，即可得到在兩次采集圖像的時間間隔內載體運動的位移，將運動位移結合姿態角信息經過坐標變換轉換到圖像坐標系下的像素值，利用計算出來的像素值，對下一時刻的匹配區位置進行預測，提高匹配搜索的速度及精度，增強視覺里程計的實時性和精確性。

3.2 視覺里程計實時圖截取

設圖像采樣時間間隔為dt，慣導輸出的水平載體坐標系下的dt時間內平均速度變量為θΔ，相機焦距（以像素為單位）為Fu，無線電高度表輸出高度為Hw，圖像沿OU 軸移動的像素值為du，圖像沿OV軸移動的像素值為dv，則利用慣導輸出的載體坐標系下速度信息可預測圖像移動的像素值，如式(8)所示。

式中，duimu、dvimu為慣導速度信息計算得到的移動像素值；、為慣導輸出的水平載體坐標系下，X、Z 軸dt時間的平均速度。

為了保證視覺里程計能夠測量的速度達到最大，希望截取的實時圖像在當前時刻圖像中的位置坐標點與在上一時刻圖像中的位置坐標點的連線的中點位于基準圖的中心位置坐標。

3.3 視覺里程計搜索區預測

傳統視覺里程計在圖像匹配的過程中，往往將整張基準圖作為匹配搜索的區域，造成系統資源的浪費以及實時性的降低，本文利用慣性信息對上一幀圖像中的匹配區位置進行預測，提高匹配搜索的速度及精度，增強視覺里程計的實時性和精確性。

本方法主要給出利用慣性速度信息來確定搜索區中心坐標，其中 (us_m,vs_m)表示搜索區的中心坐標?？紤]兩幀圖像之間的姿態角變化分別為Δφ、Δγ、Δθ，進一步推導可得搜索區中心坐標如下：

式中，（ureal_m,vreal_m）為當前幀圖像中的中心坐標。

3.4 視覺里程計輸出速度校正

傳統的視覺里程計在計算速度時，若載體姿態角發生變化，將影響視覺里程計測速精度，且影響程度與姿態角變化幅度、相機到特征點的距離成正比。本文利用慣性的姿態角信息，對視覺里程計輸出的速度信息進行高精度補償，能夠極大提升視覺里程計的測速精度。

式中，uodo_real_m,vodo_real_m為視覺里程計中實時圖在當前時刻圖像的中心坐標；duodo、dvodo為視覺里程計得到的在圖像平面坐標系下沿OU、OV軸的像素偏移值。

獲得慣性導航系統姿態角信息后，通過分析可以對視覺里程計速度進行補償，進一步提高視覺里程計測速精度，其計算如式(11)所示：

式中，Vodo_x、Vodo_z為視覺里程計輸出的水平載體坐標系下沿Ob′Xb′、Ob′Zb′速度；Fu為相機焦距（像素）；為無線電高度輸出的上一濾波周期時間更新時備份的dt時間內原始高度平均值并經過慣導信息補償后的值；為無線電高度輸出的上一濾波周期時間更新時備份的dt時間內原始高度平均值；、為慣導輸出的上一濾波周期時間更新時備份的滾轉、俯仰角；、為慣導輸出的當前時刻濾波周期時間更新時備份的滾轉、俯仰角；dt為圖像采樣時間間隔。

3.5 視覺里程計有效性判斷

本文通過計算視覺里程計卡方和視覺里程計的綜合信度來對視覺里程計有效性進行判斷，下面介紹這兩個指標的計算方法。

3.5.1 視覺里程計置信度計算算法

經過圖像匹配后，計算圖像匹配的特征值——信噪比SNR、次高峰與最高峰相關性之比SMR，使用基于證據推理的綜合圖像匹配置信度算法計算出視覺里程計的綜合信度，當綜合置信度大于一定閾值后，認為圖像匹配正確。

該算法僅通過圖像信息來對視覺里程計有效性進行判斷，進一步表明視覺里程計測速的自主性，為視覺里程計有效性判斷提供了強有力的依據。

3.5.2 視覺里程計卡方計算算法

先獲得組合濾波的新息值：

式中相關變量均與組合濾波相關，Z(k+1)為量測量，H(k+1)為量測矩陣，X(k+1,k)為一步狀態預測值，具體的計算方法將會在后續章節進行描述。

因γ(k)包含其它觀測量，故將γ(k)中與視覺里程計無關的值置零，得到γOdo(k)，將此作為視覺里程計的新息值。

則視覺里程計卡方為：

式中，PBak(k+1,k)為一步預測誤差方差矩陣在時間更新時進行的備份。

當視覺里程計卡方小于某一閾值時，認為視覺里程計輸出的速度可用于組合導航。該算法依賴于慣性導航信息，將慣性導航信息作為視覺里程計有效性判斷的依據之一，進一步提高了視覺里程計有效性判斷的可靠性。

4 慣性/視覺里程計深組合導航

4.1 慣性/視覺里程計深組合導航基本原理

本文描述的深組合導航算法如圖4所示。

深組合設計思想主要體現在以下兩方面：

（1）視覺里程計中，使用慣導速度和姿態信息進行實時圖截??；慣導速度和姿態信息預測搜索區位置；參考慣導速度信息進行視覺里程計有效性判斷；慣導姿態信息參與視覺里程計運動狀態估計；

（2）慣導、視覺里程計通過組合濾波實現對慣導誤差校正，且在組合濾波器中引入視覺導航誤差量，實現慣性導航與視覺導航在導航狀態估計的深度組合。

圖4 慣性/視覺里程計深組合算法原理Fig.4 The algorithm principle of INS/ visual odometry deeply integrated navigation

4.2 視覺里程計系統誤差模型

慣導系統誤差模型可參考相應書籍與文獻，這里不再贅述，本節主要對視覺里程計系統誤差進行建模，為深組合方案的提出打下基礎。

本文中，視覺里程計與慣性導航系統固聯在同一基座上，而且在視覺里程計計算過程中會使用慣性導航系統與無線電高度表的信息。經分析，視覺里程計主要包括以下誤差源：

（1）視覺里程計與慣性導航系統的安裝誤差；

（2）無線電高度表測高誤差；

（3）相機焦距誤差；

（4）視覺里程計圖像匹配像素誤差。

視覺里程計誤差源較多，現從兩個方面考慮，對視覺里程計誤差進行建模：

（1）視覺里程計輸出速度公式

首先分析視覺里程計輸出速度公式。由式(11)可知，當圖像匹配、無線電高度表測高、相機焦距出現誤差時會導致視覺里程計測速誤差，且認為慣性導航系統的姿態精度很高，故可得視覺里程計測速誤差的方程：

式中，、為視覺里程計輸出速度公式中相關誤差源導致的水平載體坐標系下沿x、z軸的視覺里程計測速誤差；δduodo、δdvodo為視覺里程計在圖像平面坐標系下沿OU、OV軸圖像匹配像素誤差；為視覺里程計高度誤差；δFu為相機焦距誤差；為無線電高度表測高誤差。

經過整理，去掉二階小量，且考慮到δFu遠小于Fu，故可略去δFu，可得下式：

將式(15)與視覺里程計輸出速度公式相結合，可得出視覺里程計測速誤差為：

（2）安裝誤差

在安裝的過程中，應該使得視覺里程計與慣性導航系統盡量重合，但由于安裝工藝的問題，不可避免地存在安裝誤差，這個安裝誤差雖然對視覺里程計測速不會造成影響，但是當需要將視覺里程計與慣性導航系統進行組合時，會導致一定的誤差，故需要進一步分析由于安裝誤差導致的視覺里程計測速誤差。

設機器視覺與慣性導航系統在載體坐標系下沿x、y、z軸的安裝誤差角為：

已知δaΦ為小角度向量，故含安裝誤差的慣導對應水平載體坐標系到視覺里程計對應水平載體坐標系的變換矩陣Codo可表示為：

式中：

由安裝誤差角導致的視覺里程計測速誤差計算公式為：

整理可得：

經過上述分析，可得出視覺里程計測速誤差為：

4.3 慣性/視覺里程計深組合濾波器設計

慣性/視覺里程計深組合導航系統采用速度匹配的組合濾波方案，組合濾波方程如下：

4.3.1 狀態方程建模

在常規卡爾曼濾波中加入視覺里程計對應的誤差狀態量，共選取21 個系統狀態：δVn、δVu、δVe、δL、δh、δλ、φn、φu、φe、?x、?y、?z、εx、εy、εz、φax、φay、φaz、δH0、δduodo、δdvodo。

其中，δVn、δVu、δVe分別表示捷聯慣導系統北向、天向、東向的速度誤差；δL、δh、δλ分別表示微慣導系統的緯度誤差、高度誤差、經度誤差；φn、φu、φe分別表示微慣導系統導航坐標系內北、天、東三個方向的失準角；?x、?y、?z分別表示慣導系統載體坐標系內X、Y、Z 三個方向的加速度計零偏；εx、εy、εz分別表示微慣導系統載體坐標系內X、Y、Z三個方向的陀螺漂移；φax、φay、φaz慣導與視覺里程計的x、y、z軸的安裝誤差；δH0表示無線電高度表測高誤差；δduodo、δdvodo分別表示視覺里程計圖像匹配在OU 和OV 軸的像素誤差。

系統矩陣為：

式中，Fbase為9 個基本導航參數的系統矩陣；FD為慣性器件的誤差項；FM為加速度計和陀螺的一階馬爾可夫漂移誤差項。

Fbase由慣性導航系統誤差方程構成，而慣性器件的一階馬爾可夫漂移誤差相關系統矩陣可表示為：

式中，Trx、Try、Trz為加速度計在x、y、z三個方向的一階馬爾柯夫過程的相關時間；TGx、TGy、TGz為陀螺儀在x、y、z三個方向的一階馬爾柯夫過程的相關時間。

4.3.2 觀測方程建模

選擇視覺里程計輸出的速度信息作為觀測，因此觀測量為：

確定好觀測量后，組合濾波器觀測矩陣的求取直接決定了組合濾波最優估計的準確性。因為本文使用的是間接法的組合濾波，組合濾波器的輸入是慣性導航系統和機器視覺導航系統的誤差量，而慣性導航系統、機器視覺導航系統輸出的信息可看作真值與誤差量之和，故式(26)可表示為：

式中，、為慣導輸出前向和側向的速度誤差；δvx_ov、δvz_ov為視覺里程計輸出前向和側向的速度誤差。

組合濾波器觀測矩陣反應了觀測量與系統狀態變量的關系，因為間接法組合濾波中，系統狀態變量與誤差量相關，故通過慣性導航系統和視覺里程計系統的誤差模型，由式(19)(24)即可求取出組合濾波器觀測矩陣。同時，在進行組合導航時，需要利用基于證據推理和卡方對視覺里程計有效性進行判斷，如果視覺里程計無效，則觀測矩陣置零，卡爾曼濾波僅進行時間更新，不進行量程更新。

4.4 組合導航步驟

基于證據推理的慣性/視覺里程計深組合導航的基本步驟如下：

1）相關變量初始化，卡爾曼濾波器初始化；

2）開始導航解算，并計算系統狀態轉移矩陣

3）到達卡爾曼濾波周期時，進行時間更新：

4）進入視覺里程計匹配區域，將當前幀圖像作為實時圖，上一幀圖像作為基準圖，利用慣性信息輔助視覺里程計計算，獲得實時圖在基準圖中的位置，并計算當次匹配的特征值SMR和SNR。

5）由試驗數據統計得到的特征值與視覺里程計匹配置信度關系并計算兩個特征值所表征的視覺里程計匹配置信度Bpasmr和Bpasnr。計算公式如下：

式中，a1、b1、c1、a2、b2、c2為多項式擬合參數。

6）計算經靜態權重折扣后的證據與其加權平均證據的距離di，計算公式見式(5)。

7)計算各證據的綜合權重ai，使用綜合權重對各證據源的mass 進行折扣，并計算其加權平均值。加權平均值表示的是第j次視覺里程計匹配經證據推理后得到的綜合信度。當綜合信度表征的視覺里程計匹配正確率低于95%時，不進行量測更新，即Kk+1=0；否則進行量測更新：

8）求取最優估計狀態向量和最優估計協方差矩陣Pk+1：

9）若進行量測更新，對估計出的系統誤差進行閉環修正，轉入步驟（2）；若未進行量測更新，直接轉入步驟（2）。

5 慣性/視覺里程計深組合導航

5.1 試驗條件

本文針對慣性/視覺里程計深組合導航，研制了演示樣機，并安裝于運12 飛機內部開展了機載試驗，安裝情況如圖5所示。

試驗設備：

1）慣性導航系統，采用光纖捷聯慣導系統，陀螺漂移0.05 °/h，加表零偏：100μg；

2）可見光相機：ICL-B2520 相機（2456×2058）；

3）距離獲取設備：衛星高度通道。

在機載試驗中，可見光相機拍攝運12 飛機下方的景象，利用視覺里程計測量載體的前向、側向速度，并與慣導系統進行深度組合，實現高精度導航定位。

圖5 機載試驗安裝示意圖Fig.5 The installation of airborne test

飛行軌跡如圖6所示。

圖6 飛行軌跡Fig.6 Flight path

飛行高度如圖7所示。

圖7 飛行高度Fig.7 Flight altitude

5.2 試驗結果

圖8(a)表示試驗結果，分別使用傳統的慣性/視覺里程計組合導航和本文提出的基于證據推理的慣性/視覺里程計深組合導航。圖8(b)為圖8(a)速度誤差曲線的局部放大圖。

由圖8(b)可知，在視覺里程計匹配過程中，傳統方法對視覺里程計匹配結果的正確率判斷不準確，導致速度誤差變大?；谧C據推理的慣性/視覺里程計深組合導航，首先對視覺里程計匹配結果進行更加可靠的正確率判斷，認為視覺里程計匹配正確后，才進行慣性/視覺里程計深組合導航，組合導航結果更為平滑、準確。

圖8(a) 速度誤差曲線Fig.8(a) The curve of velocity error

圖8(b) 速度誤差曲線局部放大圖Fig.8(b) The partial enlarged curve of velocity error

徑向位置誤差對比如圖9所示。經計算，采用新方法后組合導航精度由0.4%D提升到0.3%D，進一步驗證了新方法的有效性。

圖9 徑向位置誤差對比Fig.9 The comparison of radial position

6 結 論

本文基于證據推理理論，將與視覺里程計匹配置信度相關的特征量進行數據融合，獲得綜合信度與匹配正確率的關系曲線。使用綜合信度作為組合導航量測更新的依據，提高了卡爾曼濾波器的誤差估計精度以及組合導航精度。

通過機載試驗表明，基于證據推理的慣性/視覺里程計深組合導航相比傳統慣性/視覺里程計組合導航，導航精度提升到了0.3%D，且計算復雜度低，易于工程實現。