萬振塬楊功流涂勇強(1.北京航空航天大學(xué)儀器科學(xué)與光電工程學(xué)院北京1001912.慣性技術(shù)國防重點實驗室北京100191)

一種新型模糊自適應(yīng)Kalman濾波器在組合導(dǎo)航中的應(yīng)用

萬振塬1，2，楊功流1，2，涂勇強1，2
(1.北京航空航天大學(xué)儀器科學(xué)與光電工程學(xué)院，北京100191；2.慣性技術(shù)國防重點實驗室，北京100191)

針對光纖陀螺捷聯(lián)慣導(dǎo)(FOG SINS)／GPS組合導(dǎo)航系統(tǒng)實際工作環(huán)境中，由于系統(tǒng)噪聲與量測噪聲模型發(fā)生變化而帶來的濾波器發(fā)散的問題，提出一種新型模糊自適應(yīng)Kalman濾波器(FSHAKF)。通過引入IMU精度因子與GPS水平精度因子，構(gòu)造模糊推理系統(tǒng)(FIS)，實時更新自適應(yīng)參數(shù)，有效地解決了傳統(tǒng)Sage?Husa自適應(yīng)濾波器(SHAKF)估計模型不準(zhǔn)確、系統(tǒng)噪聲與量測噪聲無法同時估計以及濾波器長時間易發(fā)散的問題。仿真實驗表明，本文提出的FSHAKF算法相較于SHAKF算法，估計精度得到明顯提高，且避免了濾波器的發(fā)散。

捷聯(lián)慣導(dǎo)系統(tǒng)；組合導(dǎo)航；模糊推理系統(tǒng)；自適應(yīng)Kalman濾波

0 引言

Kalman濾波器已經(jīng)廣泛應(yīng)用于光纖陀螺捷聯(lián)慣導(dǎo)(FOG SINS)／GPS組合導(dǎo)航系統(tǒng)中，但是在實際的工程應(yīng)用中，大多數(shù)系統(tǒng)的量測噪聲和系統(tǒng)噪聲預(yù)先都是未知的或者不確定的，即使建立了正確的系統(tǒng)模型，在工作過程中也會存在一些不確切因素可能導(dǎo)致數(shù)學(xué)模型攝動，例如環(huán)境溫度變化繁復(fù)、GPS信號不穩(wěn)定導(dǎo)致光纖陀螺器件噪聲與GPS量測噪聲發(fā)生變化。在此情況下進行濾波很有可能導(dǎo)致濾波發(fā)散［1］。

自適應(yīng)濾波是一種具有抑制濾波器發(fā)散的濾波方法。往往通過對系統(tǒng)噪聲方差陣Q及量測噪聲方差陣R進行實時估計，然后反饋給濾波器增益，進而適應(yīng)復(fù)雜的噪聲環(huán)境。目前最常用的濾波方法是SHAKF算法，相關(guān)研究發(fā)現(xiàn)該算法不能同時對系統(tǒng)噪聲與量測噪聲進行估計，否則容易造成濾波器發(fā)散［2］。針對這一問題，一些學(xué)者提出了簡化的SHAKF算法，但是并未從根本上解決噪聲方差陣Q、R在新息中的復(fù)雜耦合關(guān)系，且易造成濾波模型不準(zhǔn)確，最終影響組合導(dǎo)航精度［3?5］。另一方面，一些學(xué)者通過監(jiān)測新息理論值與實測值，應(yīng)用模糊推理系統(tǒng)不斷調(diào)整量測噪聲的加權(quán)來修正濾波估計結(jié)果［6?7］，但并未深入討論模糊推理系統(tǒng)在系統(tǒng)噪聲與量測噪聲模型同時變化的復(fù)雜情況下的使用方法。

在以上理論研究的基礎(chǔ)上，本文提出了一種新型的模糊自適應(yīng)Kalman濾波器(FSHAKF)。首先對SHAKF進行簡化處理，保證濾波器不發(fā)散。然后，通過構(gòu)造IMU精度因子與GPS水平精度因子，建立模糊推理系統(tǒng)，實時調(diào)整自適應(yīng)參數(shù)。FSHAKF準(zhǔn)確地發(fā)揮了新息的作用，解決了簡化處理帶來的模型不準(zhǔn)確問題，提高了組合導(dǎo)航系統(tǒng)狀態(tài)估計精度。

1 Sage-Husa自適應(yīng)Kalman濾波

1.1 SHAKF基本原理

利用時變噪聲統(tǒng)計估計器對量測噪聲和系統(tǒng)噪聲的統(tǒng)計特性進行實時的在線修正和估計，與此同時通過量測信息進行迭代濾波，這種方法被稱為Sage?Husa自適應(yīng)濾波算法。

假設(shè)系統(tǒng)狀態(tài)方程和量測方程可離散化成如下形式：

其中，Xk、Zk分別為狀態(tài)量和量測量；Φk，k－1為k－1時刻到k時刻系統(tǒng)狀態(tài)一步轉(zhuǎn)移矩陣；Hk為k時刻量測矩陣；Wk和Vk為k時刻離散型Gauss白噪聲：Wk～WN(qk，Qk)，Vk～WN(rk，Rk)，且假設(shè)這兩種噪聲互不相關(guān)。此處不加證明地給出時變噪聲統(tǒng)計估計方程：

式(2)中給出的是其遞推形式，b為遺忘因子。Sage?Husa自適應(yīng)濾波理論上能在線同時估計系統(tǒng)噪聲和量測噪聲，但從式(2)可知，它們都依賴新息向量ek，即ek的異常變化同時影響和的計算，不能保證濾波估計的準(zhǔn)確性，只能在已知時估計出，或者在已知時估計出。另外，由于式(2)中、中的減號項存在，不能保證和的半正定性和正定性，而從算法分析中可看出，由于減法的存在會使估計結(jié)果失去這種性質(zhì)，從而造成濾波發(fā)散。

1.2 簡化的SHAKF

通過上文分析可知，為了抑制濾波發(fā)散，必須保證和的半正定性和正定性。因此，將式(2)中和的迭代部分改寫為：

即對迭代算式進行簡化，去除了影響和的半正定性和正定性的減號項－Φk／k－1Px，k－1ΦTk／k－1、－HkPx，k／k－1HTk。

由式(2)可知，和的值主要取決于上一時刻的系統(tǒng)噪聲和量測噪聲以及當(dāng)前時刻新息向量ek，而減號項對和的取值影響不大。因此，去除減號項只會對濾波精度產(chǎn)生一定影響。反而，如果和不能保證半正定性，則會使濾波發(fā)散。實際應(yīng)用中，濾波算法的穩(wěn)定性與可靠性更有價值。鑒于此，相關(guān)學(xué)者選擇犧牲一定濾波精度以保證濾波器的穩(wěn)定性。

2 模糊自適應(yīng)Kalman濾波

2.1 FSHAKF基本原理

為了保證和的正定性，上一小節(jié)中將其迭代算式做了簡化處理。但是這樣會導(dǎo)致估計不準(zhǔn)確，最終影響濾波精度。另外可以推出信息均方誤差理論值滿足：

即新息向量ek中耦合著系統(tǒng)噪聲及量測噪聲的影響，若采用此新息向量同時對和進行估計，其結(jié)果是不準(zhǔn)確的。目前這方面的研究集中在如何抑制濾波器發(fā)散上，對于濾波精度方面，往往默認其受系統(tǒng)噪聲影響較小，不再做進一步處理或者直接省去對的更新。

為了實現(xiàn)在抑制濾波器發(fā)散的同時保證估計精度，首先引入衛(wèi)星導(dǎo)航系統(tǒng)水平精度因子(PDOP)及IMU精度因子(FT)。

衛(wèi)星導(dǎo)航系統(tǒng)水平精度因子(PDOP)可由GPS輸出直接獲得，PDOP的大小直接反映出GPS此時量測噪聲的水平。參考文獻[8]，GPS的測距誤差會引起用戶的位置誤差?P，用戶的位置誤差協(xié)方差為：

其中，σ為GPS距離測量的方差，x、y、z代表衛(wèi)星在地球坐標(biāo)系下的幾何位置，t為系統(tǒng)時間。

在FOG SINS／GPS組合導(dǎo)航過程中，光纖陀螺、石英加速度計噪聲受溫度影響較大。盡管可以事前對其進行溫度補償，但并不能完全保證系統(tǒng)噪聲模型不發(fā)生變化。通過分析慣性器件的溫度補償結(jié)果，建立溫補殘差與溫度及溫度相關(guān)量之間的關(guān)系，對溫補殘差的均方差Δtemp二次建模如下：

采用最小二乘擬合方法求解出溫度T，溫度變化率及乘積項的權(quán)值系數(shù)，對溫補殘差均方差做歸一化處理得到IMU精度因子FT∈(0，1)。

2.2 模糊推理系統(tǒng)構(gòu)建

本文采用Mamdani型模糊邏輯系統(tǒng)，其規(guī)則只有4條，便于實現(xiàn)。常用的設(shè)計過程可分為3部分：模糊化過程、模糊控制規(guī)則生成過程、反模糊化的過程。

本文設(shè)計的模糊推理系統(tǒng)(FIS)的輸入為FT、PDOP，輸出為，為多輸入單輸出系統(tǒng)。輸入輸出模糊集分別為：(GPS精度高，GPS精度低)；(IMU精度高，IMU精度低)；(Increase，Maintain，De?crease)。

依據(jù)IMU溫度特性及GPS精度分布規(guī)律設(shè)計隸屬度函數(shù)；解模糊化方法選用重心法，即輸出取隸屬函數(shù)曲線圍成區(qū)域面積的重心。此FIS系統(tǒng)輸入輸出特性曲線如圖2所示。將FIS每次運算得到的帶入式(3)，對、進行自適應(yīng)調(diào)整。

2.3 FSHAKF在組合導(dǎo)航系統(tǒng)中的應(yīng)用

本文以FOG SINS／GPS組合導(dǎo)航系統(tǒng)為研究對象，采用FOG SINS為主導(dǎo)航系統(tǒng)，利用GPS提供的速度和位置信息作為外觀測量。

系統(tǒng)狀態(tài)方程可以寫成以下形式：

式中，X(t)＝ ［?E?N?UδVEδVNδLδλ εxεyεzxyz］T為13維狀態(tài)向量，E、N、U分別代表東、北、天三個軸向，?E、?N、?U為姿態(tài)誤差，δVE、δVN為速度誤差，δL、δλ為緯度誤差和經(jīng)度誤差，εx、εy、εz為陀螺隨機常值漂移，x、y、z為加速度計隨機常值零偏。F(t)為系數(shù)矩陣，W(t)為系統(tǒng)噪聲矩陣。

量測方程為：

式中，H(t)為系數(shù)矩陣，υ(t)為量測噪聲［8］。圖3為模糊自適應(yīng)濾波器在組合導(dǎo)航系統(tǒng)中的應(yīng)用原理圖。有量測更新時，IMU精度因子與GPS水平精度因子被送入模糊控制器，經(jīng)過模糊處理得到自適應(yīng)調(diào)節(jié)因子，并以此參數(shù)對SHAKF進行自適應(yīng)修正。經(jīng)過修正的SHAKF對新息進行處理估計出狀態(tài)量，以此校正組合導(dǎo)航系統(tǒng)的姿態(tài)、速度、位置輸出。

3 仿真驗證與結(jié)果分析

假設(shè)載體的初始位置為北緯40.120662°，東經(jīng)116.344311°，高度為50m，導(dǎo)航時間為29min，SINS采樣頻率為100Hz，GPS輸出頻率(濾波頻率) 為1Hz。在Matlab仿真環(huán)境下設(shè)定載體運動狀態(tài)，包括轉(zhuǎn)彎、加減速、勻速直行，具體的載體運動軌跡如圖4所示。

設(shè)置組合導(dǎo)航系統(tǒng)初始參數(shù)為：SINS慣性器件陀螺儀的常值漂移為0.05(°)／h，隨機漂移為0.01(°)／h ，加速度計的常值零偏為150μg，隨機漂移為30μg·s；GPS速度測量誤差0.1m／s，位置測量誤差水平2m。分別采用傳統(tǒng)SHAKF算法、簡化的SHAKF算法和本文提出的FSHAKF算法，對以下兩種噪聲條件進行仿真實驗。

(1)量測噪聲改變

在仿真進行到5000s時，GPS量測誤差逐漸變?yōu)槌踔档?倍；11000s時，GPS量測誤差逐漸變?yōu)槌踔档?倍。SINS系統(tǒng)噪聲保持不變。仿真結(jié)果如圖5所示。

(2)系統(tǒng)噪聲、量測噪聲同時改變

仿真進行到5000s時，設(shè)置的SINS系統(tǒng)噪聲緩慢變?yōu)槌踔档?倍，GPS量測誤差逐漸變?yōu)槌踔档?倍；11000s時，GPS量測誤差逐漸變?yōu)槌踔档?倍；14000s時，SINS系統(tǒng)噪聲緩慢變?yōu)槌踔档?倍。仿真結(jié)果如圖6所示。

分析兩組仿真結(jié)果發(fā)現(xiàn)：在系統(tǒng)噪聲和量測噪聲發(fā)生較大變化時，采用本文提出的FSHAKF算法可較快地收斂到相應(yīng)量測噪聲對應(yīng)的狀態(tài)估計精度，且多次實驗未出現(xiàn)濾波器發(fā)散情況；而采用簡化的SHAKF算法的估計精度較低，穩(wěn)定性較差，甚至有發(fā)散趨勢。

另外，采用傳統(tǒng)SHAKF濾波器完成上述仿真條件實驗時，發(fā)現(xiàn)P陣經(jīng)常會快速增大，濾波器發(fā)散嚴(yán)重。因篇幅所限，本文未給出該算法濾波曲線。

4 結(jié)論

[1] Crisan D，Obanubi O.Particle filters with random resampling times[J].Stochastic Processes and their Applications，2011，122(4):1332?1368.

[2] 李旦，秦永元，梅春波.組合導(dǎo)航自適應(yīng)卡爾曼濾波改進算法研究[J].測控技術(shù)，2011，30(3):114?116. LI Dan，QIN Yong?yuan，MEI Chun?bo.An improved a?daptive Kalman filter algorithm for SINS／GPS integrated navigation system[J].Measurement＆Control Technology，2011，30(3):114?116.

[3] 趙龍，吳康.新型自適應(yīng)Kalman濾波算法及其應(yīng)用[J].壓電與聲光，2009，31(6):908?911. ZHAO Long，WU Kang.A new type of adaptive Kalman filtering algorithm and its application[J].Piezoelectrics＆Acoustooptics，2009，31(6):908?911.

[4] 宋杰，晉帆.組合導(dǎo)航中抑制自適應(yīng)濾波發(fā)散的研究[J].彈箭與制導(dǎo)學(xué)報，2012，32(2):46?48. SONG Jie，JIN Fan.The study on restrain of adaptive fil?tering divergence for integrated navigation[J].Journal of Projectiles Rockets Missiles and Guidance，2012，32(2): 46?48.

[5] Venkataraman V，F(xiàn)an G，Havlicek J P，et al. Adaptive Kalman filtering for histogram?based appearance learning in infrared imagery[J].IEEE Transactions on Image Pro?[6] cessing，2012，21(11):4622?4635.

[6] Gao X D，You D Y，Katayama S. Seam tracking moni?toring based on adaptive Kalman filter embedded Elman neural network during high?power fiber laser welding[J]. IEEE Transactions on Industrial Electronics，2012，59 (11):4315?4325.

[7] 聶浩翔，關(guān)勁，卞鴻巍.模糊自適應(yīng)Kalman濾波在INS／GPS組合導(dǎo)航系統(tǒng)中的應(yīng)用[J].艦船電子工程，2011，31(1):67?70. NIE Hao?xiang，GUAN Jin，BIAN Hong?wei.Fuzzy adap?tive Kalman filtering in INS／GPS integrated navigation system[J].Ship Electronic Engineering，2011，31(1): 67?70.

[8] 袁信，余濟祥，陳哲.導(dǎo)航系統(tǒng)[M].北京:航空工業(yè)出版社，1993. YUAN Xin，YU Ji?xiang，CHEN Zhe.Navigation system [M].Beijing:Aviation Industry Press，1993.

Application of a New Fuzzy Adaptive Kalman Filter to Integrated Navigation

WAN Zhen?yuan1，2，YANG Gong?liu1，2，TU Yong?qiang1，2
(1.School of Instrumentation Science and Opto?electronics Engineering，Beihang University，Beijing 100191; 2.Science and Technology on Inertial Laboratory，Beijing 100191)

In order to solve the filter divergence phenomenon brought by changes of system noise model and measure?ment noise model，while FOG SINS／GPS integrated navigation system is working in actually environment，this paper presents a new fuzzy adaptive Kalman filter.By constructing fuzzy inference system(FIS)to update adaptive parameter in real time with the IMU accuracy factor and GPS horizontal accuracy factor，F(xiàn)SHAKF solve these problems of the traditional SHAKF，which contains inaccurate estimation model，system noise and measurement noise immeasurable at the same time and easy di?vergent filter while working for a long time.Simulation results show that，F(xiàn)SHAKF algorithm proposed by this paper compared to SHAKF algorithm，obtains higher estimation accuracy significantly and avoids the divergence of the filter effectively.

strapdown inertial navigation system(SINS);integrated navigation;fuzzy inference system(FIS); adaptive Kalman filter

U<666.1 文獻標(biāo)志碼：A class="emphasis_bold">666.1 文獻標(biāo)志碼：A 文章編號：1674?5558(2017)03?01294666.1 文獻標(biāo)志碼：A

1674?5558(2017)03?01294

A 文章編號：1674?5558(2017)03?01294

10.3969／j.issn.1674?5558.2017.02.015

萬振塬，男，碩士，研究方向為導(dǎo)航、制導(dǎo)與控制。

2016?07?21

國家自然科學(xué)基金(編號：61340044，11202010)

本文提出的FSHAKF算法從一個全新的角度解決了噪聲方差陣Q、R在新息ek中的復(fù)雜耦合關(guān)系，實現(xiàn)了二者同時在線估計，在抑制常規(guī)SHAKF算法及其簡化算法易發(fā)散的前提下，提高了濾波估計精度，進而提高組合導(dǎo)航精度。

需要特別指出的是，本文設(shè)計模糊自適應(yīng)Kalman濾波器是針對系統(tǒng)噪聲受溫度影響較大的FOG SINS／GPS組合導(dǎo)航系統(tǒng)。而仿真條件A情況下的實驗結(jié)果表明，F(xiàn)SHAKF同樣適用于系統(tǒng)噪聲較為穩(wěn)定的組合導(dǎo)航系統(tǒng)，具有一定的普適性。