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

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

0 引言

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

1 粒子濾波和貝葉斯理論

1.1 貝葉斯理論

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

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

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

1.2 粒子濾波

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

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

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

2)逐點(diǎn)計(jì)算對(duì)應(yīng)的p(xk| xk-1)和p(yk| xk).

3)利用公式:

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

4)計(jì)算新的粒子集:

5)輸出結(jié)果:

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

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

2 濾波算法

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

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

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

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

聯(lián)合密度為

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

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

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

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

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

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

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

后驗(yàn)濾波密度有如下近似:

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

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

3 仿真計(jì)算

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

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

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

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

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

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

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

4 結(jié)論

針對(duì)導(dǎo)航系統(tǒng)中單通道數(shù)據(jù)受干擾發(fā)散的情況，本文使用了負(fù)對(duì)數(shù)似然方法判斷數(shù)據(jù)是否受到干擾。當(dāng)數(shù)據(jù)正常時(shí)，引入U(xiǎn)KF 重采樣解決粒子衰退現(xiàn)象;當(dāng)數(shù)據(jù)受到干擾時(shí)，提出了基于貝葉斯理論的粒子濾波算法，利用粒子濾波中的“退化”現(xiàn)象消除了干擾數(shù)據(jù)帶來的影響。仿真結(jié)果表明，當(dāng)單一通道受到干擾而嚴(yán)重發(fā)散時(shí)，該算法可有效抑制干擾，提高精度，從而使得系統(tǒng)仍能保持較好的工作狀態(tài)。文中的研究結(jié)果為復(fù)雜噪聲下組合導(dǎo)航系統(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è)大學(xué)，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ù)測(cè)算法［J］.航空學(xué)報(bào)，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］ 胡士強(qiáng)，敬忠良.粒子濾波原理及其應(yīng)用［M］.北京:科學(xué)出版社，2010.HU Shi-qiang，JING Zhong-liang.Principle and application of particle filtering［M］.Beijing:Science Press，2010.(in Chinese)