萬(wàn)振塬楊功流涂勇強(qiáng)(1.北京航空航天大學(xué)儀器科學(xué)與光電工程學(xué)院北京1001912.慣性技術(shù)國(guó)防重點(diǎn)實(shí)驗(yàn)室北京100191)

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

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

針對(duì)光纖陀螺捷聯(lián)慣導(dǎo)(FOG SINS)／GPS組合導(dǎo)航系統(tǒng)實(shí)際工作環(huán)境中，由于系統(tǒng)噪聲與量測(cè)噪聲模型發(fā)生變化而帶來(lái)的濾波器發(fā)散的問(wèn)題，提出一種新型模糊自適應(yīng)Kalman濾波器(FSHAKF)。通過(guò)引入IMU精度因子與GPS水平精度因子，構(gòu)造模糊推理系統(tǒng)(FIS)，實(shí)時(shí)更新自適應(yīng)參數(shù)，有效地解決了傳統(tǒng)Sage?Husa自適應(yīng)濾波器(SHAKF)估計(jì)模型不準(zhǔn)確、系統(tǒng)噪聲與量測(cè)噪聲無(wú)法同時(shí)估計(jì)以及濾波器長(zhǎng)時(shí)間易發(fā)散的問(wèn)題。仿真實(shí)驗(yàn)表明，本文提出的FSHAKF算法相較于SHAKF算法，估計(jì)精度得到明顯提高，且避免了濾波器的發(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)中，但是在實(shí)際的工程應(yīng)用中，大多數(shù)系統(tǒng)的量測(cè)噪聲和系統(tǒng)噪聲預(yù)先都是未知的或者不確定的，即使建立了正確的系統(tǒng)模型，在工作過(guò)程中也會(huì)存在一些不確切因素可能導(dǎo)致數(shù)學(xué)模型攝動(dòng)，例如環(huán)境溫度變化繁復(fù)、GPS信號(hào)不穩(wěn)定導(dǎo)致光纖陀螺器件噪聲與GPS量測(cè)噪聲發(fā)生變化。在此情況下進(jìn)行濾波很有可能導(dǎo)致濾波發(fā)散［1］。

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

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

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

1.1 SHAKF基本原理

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

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

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

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

1.2 簡(jiǎn)化的SHAKF

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

即對(duì)迭代算式進(jìn)行簡(jiǎn)化，去除了影響和的半正定性和正定性的減號(hào)項(xiàng)－Φk／k－1Px，k－1ΦTk／k－1、－HkPx，k／k－1HTk。

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

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

2.1 FSHAKF基本原理

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

量測(cè)方程為：

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

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

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

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

(1)量測(cè)噪聲改變

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

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

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

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

另外，采用傳統(tǒng)SHAKF濾波器完成上述仿真條件實(shí)驗(yàn)時(shí)，發(fā)現(xiàn)P陣經(jīng)常會(huì)快速增大，濾波器發(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ìn)算法研究[J].測(cè)控技術(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é)報(bào)，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] 袁信，余濟(jì)祥，陳哲.導(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 文獻(xiàn)標(biāo)志碼：A class="emphasis_bold">666.1 文獻(xiàn)標(biāo)志碼：A 文章編號(hào)：1674?5558(2017)03?01294666.1 文獻(xiàn)標(biāo)志碼：A

1674?5558(2017)03?01294

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

10.3969／j.issn.1674?5558.2017.02.015

萬(wàn)振塬，男，碩士，研究方向?yàn)閷?dǎo)航、制導(dǎo)與控制。

2016?07?21

國(guó)家自然科學(xué)基金(編號(hào)：61340044，11202010)

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

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