一種新型模糊自適應Kalman濾波器在組合導航中的應用

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

一種新型模糊自適應Kalman濾波器在組合導航中的應用

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

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

捷聯慣導系統；組合導航；模糊推理系統；自適應Kalman濾波

0 引言

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

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

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

1 Sage-Husa自適應Kalman濾波

1.1 SHAKF基本原理

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

假設系統狀態方程和量測方程可離散化成如下形式：

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

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

1.2 簡化的SHAKF

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

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

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

2 模糊自適應Kalman濾波

2.1 FSHAKF基本原理

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

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

為了實現在抑制濾波器發散的同時保證估計精度，首先引入衛星導航系統水平精度因子(PDOP)及IMU精度因子(FT)。

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

其中，σ為GPS距離測量的方差，x、y、z代表衛星在地球坐標系下的幾何位置，t為系統時間。

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

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

2.2 模糊推理系統構建

本文采用Mamdani型模糊邏輯系統，其規則只有4條，便于實現。常用的設計過程可分為3部分：模糊化過程、模糊控制規則生成過程、反模糊化的過程。

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

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

2.3 FSHAKF在組合導航系統中的應用

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

系統狀態方程可以寫成以下形式：

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

量測方程為：

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

3 仿真驗證與結果分析

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

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

(1)量測噪聲改變

在仿真進行到5000s時，GPS量測誤差逐漸變為初值的6倍；11000s時，GPS量測誤差逐漸變為初值的4倍。SINS系統噪聲保持不變。仿真結果如圖5所示。

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

仿真進行到5000s時，設置的SINS系統噪聲緩慢變為初值的7倍，GPS量測誤差逐漸變為初值的6倍；11000s時，GPS量測誤差逐漸變為初值的4倍；14000s時，SINS系統噪聲緩慢變為初值的5倍。仿真結果如圖6所示。

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

另外，采用傳統SHAKF濾波器完成上述仿真條件實驗時，發現P陣經常會快速增大，濾波器發散嚴重。因篇幅所限，本文未給出該算法濾波曲線。

4 結論

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

[2] 李旦，秦永元，梅春波.組合導航自適應卡爾曼濾波改進算法研究[J].測控技術，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] 趙龍，吳康.新型自適應Kalman濾波算法及其應用[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] 宋杰，晉帆.組合導航中抑制自適應濾波發散的研究[J].彈箭與制導學報，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，Fan 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] 聶浩翔，關勁，卞鴻巍.模糊自適應Kalman濾波在INS／GPS組合導航系統中的應用[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] 袁信，余濟祥，陳哲.導航系統[M].北京:航空工業出版社，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，FSHAKF 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，FSHAKF 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 文獻標志碼：A class="emphasis_bold">666.1 文獻標志碼：A 文章編號：1674?5558(2017)03?01294666.1 文獻標志碼：A

1674?5558(2017)03?01294

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

10.3969／j.issn.1674?5558.2017.02.015

萬振塬，男，碩士，研究方向為導航、制導與控制。

2016?07?21

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

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

需要特別指出的是，本文設計模糊自適應Kalman濾波器是針對系統噪聲受溫度影響較大的FOG SINS／GPS組合導航系統。而仿真條件A情況下的實驗結果表明，FSHAKF同樣適用于系統噪聲較為穩定的組合導航系統，具有一定的普適性。