基于聯邦濾波算法的組合導航信息融合方法設計

萬曉帆，呂耀文，劉偉

（長春理工大學 光電工程學院，長春 130022）

隨著導航技術的迅猛發展，多傳感器信息融合技術逐漸成為了組合導航系統的關鍵技術［1-2］。目前應用最廣泛、效率最高的融合方法是基于Kalman濾波技術的聯邦濾波算法［3］。賈沛然等［4］通過跑車實驗驗證了聯邦Kalman濾波技術應用于組合導航系統進行數據融合的可行性。解可新等［5］在GPS/SINS/OD組合導航系統中單一傳感器信號缺失時，利用聯邦卡爾曼濾波器增強了系統的容錯性。雷克萊狄斯GV等［6］通過仿真實驗驗證了聯邦卡爾曼濾波技術應用于導航數據融合中，導航精度仍有很大的提升空間。基于聯邦Kalman濾波技術的數據融合方法包括各子濾波器估計相關時的融合方法和各子濾波器估計不相關時的融合方法，吳延昌等［7］采用方差上界方法將各子濾波器的最優估計值的相關狀態轉化成不相關狀態，對于各不相關的子系統的估計值，傳統的聯邦濾波器將時間序列中的每一項數據做等權重處理，而組合導航系統實際運行過程中每個子濾波器對下一時刻的全局最優估計值影響并不相同［8］。

本文設計了基于Carlson聯邦濾波器的混合聯邦濾波器，主濾波器中通過對一個時間序列的每項數據進行加權處理，實現權重值的自適應更新，對全局最優估計值影響較大的子濾波器分配更大的權重值，進而提高了組合導航系統的導航精度、容錯性等指標。

1 數據采集平臺

數據采集平臺以S5PV210為數據處理核心，S5PV210可以實現2000DMIPS的高性能運算能力，能夠滿足數據采集系統高效、實時的要求。設計中采用MPU6050傳感器進行慣導數據（INS）采集；GPS數據采集以T326三星六頻高精度定位定向板卡模塊為核心，通過串行通信協議與S5PV210處理器進行數據傳輸；使用OV5640圖像傳感器進行圖像采集，由于相機不同的拍攝角度會引起復雜的圖像畸變，進而會增大數據處理的難度，所以本文相機采用水平拍攝，即相機和特征點在兩個平行的平面內，利用Harris角點檢測法對特征點進行提取，利用特征點周圍的的灰度信息進行焦點匹配。數據采集平臺如圖1所示。

圖1 組合導航系統數據采集平臺

組合導航數據采集精度是實現高效數據融合的前提，慣導（INS）和GPS采用數字傳感器模塊，進而保證了數據采集精度能夠滿足系統要求。單目視覺傳感器對速度的解算是整個數據采集平臺數據輸出精度的關鍵技術。單目視覺導航系統速度解算示意圖如圖2所示，導航參考系選擇用當地東北天坐標系（地理坐標系），導航系用n表示，導航載體系用b表示，單目系用c表示，a是空間中特征點，At1和At2是空間中特征點a對應的在同一水平面內的兩個像點。

圖2 單目視覺導航系統示意圖

像點隨著載體的運動而變化，由單目系的像點坐標到導航系的轉移方程如式（1）所示。

為提高單目速度解算的精度，簡化數據計算量，單目運動平面為水平二維運動平面，當攝像機水平向前拍攝時，根據式（1）單目系的像點坐標到導航系的轉移方程，導航系下單目解算的速度為：

2 混合聯邦濾波器器模型

Carlson聯邦濾波器由多個子濾波器和一個主濾波器構成，對數據進行并行、分級處理。本設計中混合聯邦濾波器模型將INS作為公共參考系，分別與GPS和單目進行數據融合，主濾波器利用兩個子濾波器的濾波結果對INS信息進行更新。組合導航濾波器結構圖如圖3所示。

圖3 組合導航濾波器結構圖

基于INS構建捷聯慣導系統作為公共參考系，狀態變量為：

式中，δLIEδλIN為緯度、經度誤差；δvIE、δvIN為東向、北向速度誤差；φIE、φIN、φIU為平臺航向角、橫滾角和俯仰角誤差。?E、?N為東向加速度計零偏和北向加速度計零偏；εE、εN、εU為東向陀螺儀漂移，北向陀螺儀漂移和方位陀螺儀漂移。

2.1 INS/GPS組合導航系統

GPS狀態變量為：

式中，δLG為GPS輸出的緯度誤差和經度誤差；δvEG、δvGN為東向速度誤差和北向速度誤差；δψG為航向角誤差，δθG為俯仰角誤差，δγG為橫滾角誤差。

INS/GPS組合導航系統狀態方程為：

式中，W為均值為零、方差為Q的白噪聲，FINS為12×12矩陣［9］，FGPS為7×7矩陣，

其中相關時間τLG,τλG,τνEG,τνNG,τψG,τθG,τγG,在100～200s［10］之間選取。以上假設GPS的位置誤差、速度誤差、姿態誤差均為一階馬爾科夫過程。

傳統的INS/GPS組合導航系統采用位置、速度組合方式，隨著GPS技術的發展，GPS模塊可以實時輸出高精度的姿態數據，在本設計中將姿態也作為系統觀測量。INS/GPS組合導航系統量測方程為：

量測噪聲V是零均值，方差為R的白噪聲，且V、W互不相關。

2.2 INS/單目組合導航系統

INS/單目視覺組合導航采用速度組合模式，INS和單目視覺解算出的載體速度之差作為觀測

式中δLk2,δλk2為載體緯度誤差、經度誤差，δvNk2,δvEk2為載體北向和東向速度，δφNk2,δφEk2和δφEk2分別為載體北東和天向的姿態誤差角度，δk為攝像頭標度因數誤差。δk包括常值誤差和隨機誤差，本系統中用隨機常數代替常值誤差，而隨機誤差用作一階馬爾科夫過程表示，反時間相關常數設為1τc。系統狀態方程為：量。選取狀態量為：

式中FSINS為7×7的矩陣［9］。取INS系統解算的載體速度和單目解算的載體速度的差值為量測向量，系統的量測方程為：

2.3 全局濾波最優估計

采用最小二乘迭代加權算法對兩個子濾波器的最優估計值進行融合。一般最小二乘法將時間序列中的各項數據的重要性同等看待，而事實上時間序列各項數據對未來的影響作用應是不同的，因此比較合理的方法就是使用加權的方法，對相對重要的數據賦以較大的權數。兩個子濾波器更新的局部最優估計值分別為，兩個子濾波器的權重分配系數關系如式（10）所示。

式中，a和b為需要確定的權值。取k個時刻的測量數據進行最小二乘計算，公式如（11）所示。

求出K個時刻內的參數a和b，進而根據式（10），將兩個局部最優估計值Xk1(t)和Xk2(t)發送到主濾波器得到最優估計X(t)。

3 Matlab仿真結果與分析

導航系統數據采集平臺數據更新周期為0.1s，數據更新次數N=500，數據采集平臺靜止GPS初始北緯L0=43°50′12.97″，東經λ0=125°18′17.02″，地球的自轉角速度ωie=7.27×10-5rad/s，INS中陀螺的一階馬爾科夫過程的白噪聲和相關時間分別為ωr=0.1、Tr=0.1s。INS中加速度計的一階馬爾科夫過程白噪聲和相關時間分別為ωa=0.01、Ta=0.01s。OV5640鏡頭分辨率2592×1944，視場角54.1H°×41.9V°。在PC端Matlab環境下，對數據采集平臺采集到數據進行仿真分析，仿真分析流程圖4所示。

圖4 仿真分析流程圖

姿態誤差對比曲線如圖5所示，圖中實線表示應用采用等權處理的Carlson聯邦濾波器得到的誤差曲線，虛線表示采用基于最小二乘加權處理的混合聯邦濾波器的誤差曲線。俯仰角、翻滾角和航向角誤差曲線均收斂于0值。

圖5 Carlson聯邦濾波器與混合聯邦濾波器姿態誤差對比曲線

位置誤差對比曲線如圖6所示，圖中實線表示采用等權處理的誤差曲線，虛線表示采用基于最小二乘加權處理的誤差曲線。等權處理的Carlson聯邦濾波算法位置誤差呈現發散狀態，而采用混合聯邦濾波器位置誤差穩定在±2m范圍內。

圖6 Carlson聯邦濾波器與混合聯邦濾波器位置誤差對比曲線

速度誤差對比曲線如圖7所示，圖中實線表示采用等權處理的誤差曲線，虛線表示采用基于最小二乘加權處理的誤差曲線。采用混合聯邦濾波器速度誤差曲線雖然波動范圍較大，但是相比于等權處理的Carlson聯邦濾波器得到的速度誤差曲線并未出現明顯的發散現象，速度穩定在±0.5m/s范圍內。

圖7 Carlson聯邦濾波器與混合聯邦濾波器速度誤差對比曲線

4 結論

多傳感器信息融合方法是提高組合導航系統導航精度的關鍵技術，本文基于Carlson聯邦濾波器模型設計了混合聯邦濾波器，主濾波器中采用最小二乘加權方法給每個子濾波器分配動態、自適應系統的權重值。Matlab仿真結果表明，相比于等權重值Carlson聯邦濾波器，混合聯邦濾波器導航精度更高。為組合導航數融合方法的研究提供了參考。