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

朱 楠， 方 偉

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

0 引 言

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

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

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

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

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

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

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

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

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

圖1 輸出校正Fig. 1 Output correction

圖2 反饋校正Fig. 2 Feedback correction

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

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

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

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

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

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

式中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é)果分析

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

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

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

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

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

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

當(dāng)北斗失效時(shí)，僅由慣性導(dǎo)航輸出的速度、位置誤差會(huì)越來(lái)越大。 因此，為了進(jìn)行驗(yà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)北斗失效時(shí)，無(wú)法利用北斗輸出的速度、位置信息對(duì)慣性導(dǎo)航進(jìn)行校正，速度誤差和位置誤差在三個(gè)方向上的分量從零刻度線處迅速上升或下降。 而當(dāng)北斗正常工作時(shí)，慣性導(dǎo)航經(jīng)過(guò)北斗校正后提供的導(dǎo)航信息的誤差逐漸回到零刻度線處。 因此，通過(guò)誤差仿真實(shí)驗(yàn)說(shuō)明，單一的慣性導(dǎo)航系統(tǒng)無(wú)法滿足運(yùn)動(dòng)體長(zhǎng)時(shí)間運(yùn)動(dòng)狀態(tài)下的定位要求，組合導(dǎo)航的優(yōu)點(diǎn)得到體現(xiàn)。

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

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