卡爾曼濾波在組合導(dǎo)航中的仿真與分析

朱 楠， 方 偉

（揚(yáng)州海通電子科技有限公司， 江蘇 揚(yáng)州225000）

0 引 言

目前我國船舶領(lǐng)域多使用單一的導(dǎo)航方式［1］，而單一導(dǎo)航模式都有其不足的地方，慣性導(dǎo)航的誤差會隨著時間的累計而增大［2］，北斗導(dǎo)航系統(tǒng)使用定位服務(wù)時易暴露自身定位，且同一時間段內(nèi)用戶人數(shù)會受限制［3］。 本文選擇以慣性導(dǎo)航為主，北斗系統(tǒng)作為輔助系統(tǒng)的模式。 根據(jù)卡爾曼濾波算法設(shè)計濾波器，估計位置和速度的最小誤差，并利用該誤差對慣性導(dǎo)航進(jìn)行校正，從而得出運(yùn)動體實時的導(dǎo)航信息［4］。 本文首先介紹卡爾曼濾波算法及其作為組合導(dǎo)航算法的優(yōu)勢，對組合導(dǎo)航建立數(shù)學(xué)模型，最后設(shè)置濾波初始值。 通過MATLAB 進(jìn)行仿真實驗，實驗數(shù)據(jù)表明，以卡爾曼濾波作為核心算法的組合導(dǎo)航提供的導(dǎo)航參數(shù)，在精度上高于單個導(dǎo)航系統(tǒng)，且不受導(dǎo)航時間的限制。

1 卡爾曼濾波器在組合導(dǎo)航中的應(yīng)用

數(shù)據(jù)濾波是去除噪聲，還原真實數(shù)據(jù)的一種數(shù)據(jù)處理技術(shù)，卡爾曼濾波在方差已知的情況下能夠從一系列存在噪聲的數(shù)據(jù)中，估計動態(tài)系統(tǒng)的狀態(tài)，且便于計算機(jī)編程實現(xiàn)。

設(shè)被估值系統(tǒng)的離散狀態(tài)方程式為

式中，XK為系統(tǒng)的狀態(tài)變量，UK為系統(tǒng)的控制變量，WK為系統(tǒng)的隨機(jī)干擾輸入，ZK為系統(tǒng)的測量值，VK為測量值的隨機(jī)干擾變量。 狀態(tài)轉(zhuǎn)移陣均為已知的同時，需求驗前統(tǒng)計量

式中，P表示狀態(tài)估計協(xié)方差，Q表示系統(tǒng)噪聲協(xié)方差，R表示測量噪聲協(xié)方差。 假定{WK} 和{VK}是高斯白噪聲序列，它們之間以及和X0之間均互不相關(guān)，并要求RK大于0。 （濾波過程詳見參考文獻(xiàn)［2］）

在卡爾曼濾波算法的迭代過程中，噪聲協(xié)方差陣Q、R 反映了狀態(tài)方程和測量方程的相信程度。 Q值增大，說明對狀態(tài)遞推越來越不相信；R 值增大，說明對測量越來越不相信［5］。

標(biāo)準(zhǔn)的卡爾曼濾波常用于估計線性系統(tǒng)在隨機(jī)噪聲干擾下的狀態(tài)變量，但在組合導(dǎo)航系統(tǒng)中，系統(tǒng)的量測方程是非線性方程，需要對其求偏導(dǎo)以實現(xiàn)線性化，這種濾波方法被稱之為擴(kuò)展卡爾曼濾波（EKF）。 由于卡爾曼濾波算法將誤差作為變量，因此可以作為組合導(dǎo)航的核心算法。

對慣性導(dǎo)航和其他導(dǎo)航系統(tǒng)提供的速度、位置信息之差進(jìn)行最優(yōu)估計后，校正慣性導(dǎo)航系統(tǒng)。 校正方式分為輸出校正和反饋校正。 輸出校正是將經(jīng)過卡爾曼濾波后得到的導(dǎo)航參數(shù)誤差最優(yōu)估計，直接補(bǔ)償慣性導(dǎo)航輸出的導(dǎo)航參數(shù)；反饋校正是將導(dǎo)航參數(shù)誤差最優(yōu)估計，反饋到慣性導(dǎo)航系統(tǒng)的內(nèi)部，對誤差狀態(tài)進(jìn)行校正，得到導(dǎo)航參數(shù)的最優(yōu)估計值。兩種校正原理分別如圖1、圖2 所示。

圖1 輸出校正Fig. 1 Output correction

圖2 反饋校正Fig. 2 Feedback correction

輸出校正與反饋各自都有優(yōu)缺點。 輸出校正常用于慣導(dǎo)系統(tǒng)中的慣性元件精度較高，且工作時間較短的場合。 反饋校正常應(yīng)用于慣性元件精度較低，且需要長時間工作的場合。

2 北斗／慣性組合導(dǎo)航的數(shù)學(xué)模型

選擇導(dǎo)航算法的狀態(tài)方程為慣導(dǎo)系統(tǒng)誤差方程［6］，這里選擇狀態(tài)變量為：

其中，從左到右依次為三個姿態(tài)角、速度誤差在3 個方向上的分量、位置誤差在3 個方向上的分量、陀螺漂移以及加速度計的零偏。 其中，加速度計的零偏同樣可用一階馬爾科夫過程描述，即

式中各元素具體的表達(dá)式見參考文獻(xiàn)6。

量測方程的觀測量選擇北斗和慣性導(dǎo)航分別輸出的位置、速度信息之差，令量測方程為

式中Z＝[Vie-VgeVin-Vgnλi-λgφi-φg]T，Vie，Vin為慣導(dǎo)系統(tǒng)輸出速度信息。Vge，Vgn為北斗輸出速度信息。λi，φi為慣導(dǎo)系統(tǒng)輸出的經(jīng)緯度。λg，φg為北斗輸出的經(jīng)緯度。

3 仿真結(jié)果分析

通過MATLAB 進(jìn)行仿真，驗證算法的可靠性。 濾波器選擇輸出校正方式，濾波初始值如表1所示。

表1 濾波初始值設(shè)置Tab. 4 Filter initial value setting

表中，Tg、Ta分別表示加速度計和陀螺儀的白噪聲激勵時間。 仿真時間為500 s，仿真結(jié)果如圖3和圖4 所示［7］。

圖3 濾波前后速度誤差對比圖Fig. 3 Velocity error contrast diagram before and after filtering

圖4 濾波前后位置誤差對比圖Fig. 4 Position error contrast diagram before and after filtering

由圖可知，北斗／慣性組合導(dǎo)航算法輸出的位置、速度信息能夠很好地跟蹤運(yùn)動體的真實軌跡，濾波后的誤差隨著時間的推移始終趨向于零。

當(dāng)北斗失效時，僅由慣性導(dǎo)航輸出的速度、位置誤差會越來越大。 因此，為了進(jìn)行驗證，本文在仿真中人為設(shè)置3 次北斗失效的情況，得到的仿真效果如圖5、圖6 所示。

圖5 北斗失效后的速度誤差仿真圖Fig. 5 Simulation chart of speed error after Beidou failure

圖6 北斗失效后的位置誤差仿真圖Fig. 6 Simulation chart of position error after Beidou failure

由圖可知，當(dāng)北斗失效時，無法利用北斗輸出的速度、位置信息對慣性導(dǎo)航進(jìn)行校正，速度誤差和位置誤差在三個方向上的分量從零刻度線處迅速上升或下降。 而當(dāng)北斗正常工作時，慣性導(dǎo)航經(jīng)過北斗校正后提供的導(dǎo)航信息的誤差逐漸回到零刻度線處。 因此，通過誤差仿真實驗說明，單一的慣性導(dǎo)航系統(tǒng)無法滿足運(yùn)動體長時間運(yùn)動狀態(tài)下的定位要求，組合導(dǎo)航的優(yōu)點得到體現(xiàn)。

4 結(jié)束語

本文介紹了卡爾曼濾波的基本原理以及其在北斗／慣性組合導(dǎo)航中的應(yīng)用，通過仿真實驗說明了該算法在組合導(dǎo)航中的可靠性，為以后的硬件實現(xiàn)提供理論基礎(chǔ)與支撐。