單永志，尹健，劉少坤，孫立偉，許河川

(1.北京理工大學 機電工程學院，北京100081;2.哈爾濱建成集團有限公司，黑龍江 哈爾濱150030;3.空軍裝備研究院 總體所，北京100076;4.駐624 廠軍事代表室，黑龍江 哈爾濱150030)

0 引言

組合導航模型一般都采用非線性模型。常用的濾波算法有擴展卡爾曼濾波(EKF)，Unscented 卡爾曼濾波(UKF)等算法［1］。但這些算法一般適用于觀測噪聲和過程噪聲為獨立或者相關(guān)的高斯白噪聲。在實際應(yīng)用中一般對過程和初始條件有著較高的要求，而且對已經(jīng)發(fā)散的通道數(shù)據(jù)的抑制能力不足［2-4］。如果對已發(fā)散通道的數(shù)據(jù)不能有效修正，就會影響全系統(tǒng)的數(shù)據(jù)解算。文中首先介紹了粒子濾波的基本思想，并且在貝葉斯判據(jù)應(yīng)用的條件下對算法模型做一定修改補充，旨在抑制組合導航系統(tǒng)單通道受干擾發(fā)散的數(shù)據(jù)。

1 粒子濾波和貝葉斯理論

1.1 貝葉斯理論

假定動態(tài)系統(tǒng)工作時可以表示為如下公式:

式中:xk為系統(tǒng)狀態(tài)變量;zk為系統(tǒng)量測向量;ωk為系統(tǒng)隨機噪聲;vk為狀態(tài)噪聲。同時設(shè)定狀態(tài)的初始概率密度函數(shù)為p(x0|z0)=p(x0)，所以系統(tǒng)的狀態(tài)預(yù)測方程和狀態(tài)更新方程分別為

這兩個公式表示了最優(yōu)貝葉斯估計的基本思想［6］。為了利用貝葉斯公式確定模型合適的解，必須計算其合適的后驗分布統(tǒng)計，采用合適的算法模型使連續(xù)積分離散化是計算步驟的一個關(guān)鍵，粒子濾波就是基于這樣一種近似數(shù)值解的方法。

1.2 粒子濾波

粒子濾波的關(guān)鍵思想是:要有一組有關(guān)聯(lián)權(quán)值的隨機樣本去表示要求的后驗概率密度，并且基于這樣的一些樣本和權(quán)值去計算估計值［7］。

對于非線性系統(tǒng)而言，粒子濾波一般采用如下計算步驟:

1)初始化:設(shè)k = 0 時，xi0∝p(x0)i = 1，2，…N，從p(xk| xk-1，yk)中抽取N 個樣本。

2)逐點計算對應(yīng)的p(xk| xk-1)和p(yk| xk).

3)利用公式:

計算對應(yīng)的樣本初值，并對其進行歸一化，即:

4)計算新的粒子集:

5)輸出結(jié)果:

其中L(xk+1)為濾波誤差的方差陣。

6)k 變成k+1，返回步驟3).

2 濾波算法

在非線性系統(tǒng)中，當系統(tǒng)受到干擾時，考慮以下離散模型表示的非線性系統(tǒng):

式中:xk∈Rn是系統(tǒng)狀態(tài)變量;uk∈Rn為系統(tǒng)輸入向量;zk∈Rn是觀測向量;e 為觀測噪聲。

對于非高斯噪聲下的非線性系統(tǒng)而言，采用EKF 算法時殘差加權(quán)平方和不再服從χ2分布，為此，采用負對數(shù)似然檢測辦法。

假設(shè)在計算權(quán)值的過程中，得到各個粒子在j-1時刻的一步轉(zhuǎn)移為預(yù)測為則可以得到由粒子表示的似然函數(shù)為

聯(lián)合密度為

則負對數(shù)似然函數(shù)表示為

所以數(shù)據(jù)檢測算法如下:

式中ε 為數(shù)據(jù)誤差允許發(fā)生的閥值大小。對于每一步而言，在使用貝葉斯粒子濾波計算數(shù)據(jù)得到ωik后，都要計算l(θ)大小，從而判別數(shù)據(jù)是否符合系統(tǒng)誤差要求。

1)當l(θ)≥ε 時，說明數(shù)據(jù)處于干擾狀態(tài)。設(shè)xi∝q(x)，i=1，2，…N，是容易由合適的q(·)產(chǎn)生的樣本，q(·)稱為重要性密度，因此是第i 個粒子的歸一化權(quán)值。

重要性函數(shù)可以分解為

將(13)式帶入(4)式中計算，修正權(quán)值公式為

先驗的重要性函數(shù)選擇為

后驗濾波密度有如下近似:

可以得到ωik∝ωik-1p(zk|xik).此時在粒子濾波的第3 步和第4 步計算中采用如上方法。觀測值發(fā)現(xiàn)有嚴重干擾時，通過改變權(quán)值大小的辦法消除誤差，集中精力在有大權(quán)值的粒子上，然后再重采樣，保證系統(tǒng)數(shù)據(jù)使用。

2)l(θ)＜ε 時，說明數(shù)據(jù)未受干擾，這時采用正常方法，在重要性重采樣中由粒子(xik，ωik)重采樣。得到N 個等權(quán)值的粒子(xik，1/N)，設(shè)定重采樣后的粒子權(quán)值為1/N.

3 仿真計算

對于飛行器的GPS/INS 組合導航系統(tǒng)采用改進的粒子濾波方法進行導航計算。設(shè)飛行器初始狀態(tài)為:北、東、天3 向速度初始偏差均為0，速度測量誤差為0.01 m/s，加速度計初始偏差為:東向初始誤差為1.0 ×10-4m/s2，北向和天向初始誤差均為0。隨機偏差為1.0 × 10-5，刻度誤差為0.000 1，經(jīng)度λ 的初始誤差量為1'，緯度L 的初始誤差量為1'，高度初始誤差為100 m.飛行器組合導航系統(tǒng)開始工作時的初始位置為:北緯30°，東經(jīng)120°，高度為5 000 m;飛行器處于初始狀態(tài)時其初始速度表示為:北向速度和天向速度為0，東向速度為300 m/s.εL= ελ= 0.01'，εh= 10 m，ενN=ενE=ενD=0.5 m/s.

則在濾波解算中，系統(tǒng)狀態(tài)方程的初始矩陣數(shù)據(jù)P0、Q 和R 分別為P0=diag{(1')2(1')210020 0 0}，Q = diag{0 0 0 (1.0 × 10-5)2(1.0 × 10-5)2(1.0 × 10-5)2}，R = diag {(0.01)2(0.01)2(0.01)2}.

在飛行器工作過程中，對組合導航系統(tǒng)進行干擾，使衛(wèi)星導航系統(tǒng)提供的數(shù)據(jù)進行單通道發(fā)散，這時引入改進粒子濾波算法并將其應(yīng)用在組合導航系統(tǒng)中。利用其他通道的正常信息消除因單通道數(shù)據(jù)發(fā)散而引起的誤差。

在組合導航系統(tǒng)中，當衛(wèi)星GPS 信號正常時，對衛(wèi)星的動態(tài)模型采用常速模型。狀態(tài)轉(zhuǎn)移矩陣Φk，k-1和干擾矩陣Γk-1定義如下:

圖1 加速度方向誤差對比Fig.1 Comparison of the acceleration errors

圖2 速度方向誤差對比Fig.2 Comparison of the speed errors

從仿真結(jié)果來看，在系統(tǒng)的單一通道發(fā)生誤差發(fā)散后，系統(tǒng)將粒子濾波中的衰減因子應(yīng)用在改進的粒子濾波算法和貝葉斯判斷中，可以通過減小經(jīng)度通道干擾數(shù)據(jù)的權(quán)值使得重要性權(quán)值分布變得傾斜而抑制系統(tǒng)單個通道發(fā)散干擾而帶來的影響。這樣可以利用粒子濾波的衰減性抑制系統(tǒng)的誤差發(fā)散并使系統(tǒng)誤差在允許的范圍內(nèi)進行調(diào)節(jié)，恢復允許誤差的工作狀態(tài)。缺點是當粒子濾波向其他通道進行權(quán)值重新分配時，有時衰減因子會向正常通道內(nèi)部傾斜，與正常情況相比，其他穩(wěn)定通道相對應(yīng)的誤差值會有所增加。但是，只要把誤差控制在系統(tǒng)誤差允許范圍內(nèi)，還是可以保證系統(tǒng)正常工作的。

圖3 位置方向誤差對比Fig.3 Comparison of the position errors

4 結(jié)論

針對導航系統(tǒng)中單通道數(shù)據(jù)受干擾發(fā)散的情況，本文使用了負對數(shù)似然方法判斷數(shù)據(jù)是否受到干擾。當數(shù)據(jù)正常時，引入UKF 重采樣解決粒子衰退現(xiàn)象;當數(shù)據(jù)受到干擾時，提出了基于貝葉斯理論的粒子濾波算法，利用粒子濾波中的“退化”現(xiàn)象消除了干擾數(shù)據(jù)帶來的影響。仿真結(jié)果表明，當單一通道受到干擾而嚴重發(fā)散時，該算法可有效抑制干擾，提高精度，從而使得系統(tǒng)仍能保持較好的工作狀態(tài)。文中的研究結(jié)果為復雜噪聲下組合導航系統(tǒng)單通道干擾的抑制提供了一種可行的解決方法。

References)

［1］ Sheen J Bishop R.Adaptive nonlinear control of spaceraft［J］.The Journal of the Astronautical Sciences，1994，42(4):451 -472.

［2］ 潘泉，楊峰，葉亮，等.一類非線性濾波器——UKF 綜述［J］.控制與決策，2005，20(5):481 -489.PAN Quan，YANG Feng，YE Liang，et al.Survey of a kind of nonlinear filters—UKF［J］.Control and Decision，2005，20(5):481 -489.(in Chinese)

［3］ Julier S，Unlmann J.Unscented filtering and nonlinear estimation［J］.Proceedings of the IEEE，2004，92(3):401 -422.

［4］ 賈英紅.航天器姿態(tài)與能量一體化控制研究［D］.哈爾濱:哈爾濱工業(yè)大學，2004.JIA Ying-hong.Research on the spacecraft attitude and energy integration control［D］.Harbin:Harbin Institute of Technology，2004.(in Chinese)

［5］ Rudolph V M，Arnaud D.The unscented particle filter CU ED/FInfeng/Tr380［R］.UK:Cambridge University Engineering Department，2001.

［6］ 張磊，李行善，于勁松，等.一種基于高斯混合模型粒子濾波的故障預(yù)測算法［J］.航空學報，2009，30(2):319 -324.ZHANG Lei，LI Xing-shan，YU Jin-song，et al.A fault prognostic algorithm based on gaussian mixture model particle filter［J］.Acta Aeronautica et Astronautica Sinica，2009，30(2):319 -324.(in Chinese)

［7］ 胡士強，敬忠良.粒子濾波原理及其應(yīng)用［M］.北京:科學出版社，2010.HU Shi-qiang，JING Zhong-liang.Principle and application of particle filtering［M］.Beijing:Science Press，2010.(in Chinese)