王艷東， 韓智華， 蔡君亮， 陳曉強， 吉慶昌

(北京航空航天大學(xué)自動化科學(xué)與電氣工程學(xué)院，北京 100191)

0 引言

相對導(dǎo)航是無人機利用其導(dǎo)航測量設(shè)備在一定條件下實時地確定編隊飛行中無人機之間的相對運動參數(shù)。通常采用GPS/SINS組合相對導(dǎo)航［1］，應(yīng)用卡爾曼濾波方法最優(yōu)地估計出導(dǎo)航系統(tǒng)的誤差狀態(tài)，再用誤差狀態(tài)的最優(yōu)估計值校正系統(tǒng)。但是，有時系統(tǒng)的狀態(tài)方程是時變的，而且狀態(tài)轉(zhuǎn)移矩陣中含有導(dǎo)航信息及慣性元件測量值，這些含有誤差的參數(shù)使得濾波器模型不準(zhǔn)確。另外，系統(tǒng)噪聲與觀測噪聲的統(tǒng)計特性等很難精確地估計或測定，所以采用常規(guī)卡爾曼濾波器時常常會發(fā)散［2］。為了解決此問題，通常采用自適應(yīng)濾波技術(shù)，在進行濾波的同時，利用觀測數(shù)據(jù)帶來的信息，不斷地在線估計和修正模型參數(shù)、噪聲統(tǒng)計特性和狀態(tài)增益矩陣，以提高濾波精度，得到系統(tǒng)狀態(tài)的最優(yōu)估計值。在以往的文獻中通常只是對系統(tǒng)噪聲與觀測噪聲方差陣進行調(diào)整，如果系統(tǒng)的狀態(tài)方程不準(zhǔn)確就會使濾波器發(fā)散，則不能滿足濾波要求。

針對上述問題，本文采用一種新的模糊自適應(yīng)卡爾曼濾波技術(shù)，即調(diào)整系統(tǒng)噪聲與觀測噪聲的方差陣或強跟蹤濾波方法進行自適應(yīng)的濾波方法，與以往文獻的方法相比，本文結(jié)合了兩種濾波方法的優(yōu)點，逐步調(diào)整系統(tǒng)中的參數(shù)，保證濾波器能實時處理新的量測值，以解決系統(tǒng)噪聲和觀測噪聲不準(zhǔn)確或是濾波器的發(fā)散問題。

1 相對導(dǎo)航模型系統(tǒng)模型

采用四元數(shù)作為相對導(dǎo)航系統(tǒng)狀態(tài)變量，因為應(yīng)用四元數(shù)法時，四元數(shù)姿態(tài)矩陣微分方程式只需要解4個一階微分方程式組就可以了［3］，比計算方向余弦姿態(tài)需要更少的時間和較小的傳遞誤差。

1.1 相對導(dǎo)航系統(tǒng)狀態(tài)方程

相對導(dǎo)航系統(tǒng)狀態(tài)變量為 X=［δP，δV，δq，δw，δf］T。其中:δP、δV 分別為地球坐標(biāo)系下的位置、速度誤差;δq為姿態(tài)角對應(yīng)的四元數(shù)誤差;δw、δf分別為陀螺儀和加速度計的輸出值［4］。

假定單個無人機系統(tǒng)的狀態(tài)方程為

引導(dǎo)機和跟隨機合并得到兩無人機導(dǎo)航狀態(tài)方程為

其中:下標(biāo)l為引導(dǎo)機;f為跟隨機。

式(2)經(jīng)過轉(zhuǎn)換得到兩無人機的相對狀態(tài)方程為

其中:ΔX=Xl－ Xf;ΔF=Fl－ Ff;ΔW=Wl－ Wf。

1.2 相對導(dǎo)航系統(tǒng)觀測方程

定義相對導(dǎo)航系統(tǒng)的觀測輸出值為Z=［PINSPGPS，VINS－ VGPS］T。PINS、VINS分別為無人機 INS 輸出的地球系下的位置、速度矢量;PGPS、VGPS分別為無人機GPS輸出的地球系下的位置、速度矢量。

單個無人機系統(tǒng)的觀測模型為

引導(dǎo)機和跟隨機結(jié)合得到兩無人機導(dǎo)航測量方程為

引導(dǎo)機和跟跟機相減得到兩無人機的相對測量方程為

其中:ΔZ=Zl－ Zf;ΔV=Vl－ Vf。

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

對于INS/GPS組合導(dǎo)航系統(tǒng)，一般的線性離散系統(tǒng)數(shù)學(xué)模型［5－6］如下:

其中:Wk～N(0，Q);Vk～N(0，R)。

文獻［7］給出了常規(guī)卡爾曼濾波的算法為

定義測量殘差值(新息)

從式(8)可以看出，預(yù)測狀態(tài)協(xié)方差陣Pk|k－1和量測噪聲方差陣Rk的變化都能影響Kk的大小，從而改變?yōu)V波方程中新息的權(quán)重，影響新息在濾波中的作用。具體選用哪種方法，可見下面的判斷不等式。

式中，γ為參數(shù)變量，一般按系統(tǒng)模型選取;tr表示求矩陣的跡。如果已知系統(tǒng)噪聲與觀測噪聲而系統(tǒng)狀態(tài)方程不準(zhǔn)確，不等式(10)成立，選用強跟蹤濾波;如果系統(tǒng)噪聲和觀測噪聲未知，不滿足不等式(10)，則選擇調(diào)整Q和R。

下面逐一對兩種方法進行分析。

2.1 調(diào)整系統(tǒng)噪聲與觀測噪聲方差陣

如果數(shù)學(xué)模型足夠準(zhǔn)確，則殘差序列應(yīng)為零均值的白噪聲，殘差方差實測值與經(jīng)過卡爾曼濾波得到的殘差方差的理論值的差值應(yīng)在0附近。如果此差值長期偏離0，則說明量測噪聲水平已經(jīng)發(fā)生了變化，需要對Q和R調(diào)整，調(diào)整的準(zhǔn)則是使此差值回到0附近［8－9］。根據(jù)上述想法，設(shè)計自適應(yīng)卡爾曼濾波器。

定義殘差方差的理論值為pr，

其中:cr為對最新的M各殘差向量方差求平均值;i0=k－M+1，M為一經(jīng)驗參數(shù)，表示用來計算實際協(xié)方差陣的窗口大小，一般取20左右。

定義Qk、Rk分別為系統(tǒng)噪聲方差陣和量測噪聲方差陣變量:

式中:Q和R是初始常值;α、β是調(diào)整值，均為時變。根據(jù)Kalman濾波器的殘差特性自適應(yīng)地調(diào)整系統(tǒng)噪聲和觀測噪聲誤差方差陣，當(dāng)α=β=1，就是常規(guī)的Kalman濾波。

定義殘差實測方差與理論方差的差值為

通過在濾波的每一采樣點監(jiān)控新息，得到均值和方差。根據(jù)式(7)，殘差方差實測值與經(jīng)過卡爾曼濾波得到的殘差方差的理論值的差值應(yīng)在0附近，均值也漸漸遠(yuǎn)離0點，則相應(yīng)地做出調(diào)整:需改變Qk或Rk值，即若DROR＞0，則減小 Qk或Rk值;若DROR＜0，則增大Qk或Rk值。這樣，濾波器增益矩陣也會相應(yīng)減小，系統(tǒng)對量測值的信賴和利用程度減小，濾波性能趨于穩(wěn)定。

2.2 強跟蹤濾波方法

強跟蹤濾波的初衷是為了使濾波器收斂，通過使用STKF降低精度來換取濾波的穩(wěn)定性，即增大系統(tǒng)的噪聲方差陣，這相當(dāng)于將許多未建模的誤差包含進來，使算法更簡單可靠。

在式(8)中，系統(tǒng)的噪聲方差陣Pk|k－1乘以一個系數(shù)λk

在STKF中關(guān)鍵的參數(shù)為λk，要得到λk還要求取4個參數(shù) αi，ρ，d，β，其他參數(shù)通常根據(jù)經(jīng)驗獲得，取 α1=α2=...=αm=1，0＜ρ≤1，d根據(jù)模糊自適應(yīng)系統(tǒng)獲取。

3 模糊自適應(yīng)濾波實現(xiàn)

模糊推理原理如圖1所示［10］。

圖1 模糊原理圖Fig.1 Schematic of fuzzy principle

圖1 中，模糊化指將輸入的物理量通過隸屬度函數(shù)轉(zhuǎn)換成模糊量;模糊推理指所有規(guī)則的前提條件與控制輸入進行比較來確定哪一規(guī)則可用于當(dāng)前的狀態(tài)。匹配的過程包括每個規(guī)則應(yīng)用的可能性。解模糊化指將模糊推理得到的模糊量轉(zhuǎn)換為實際物理量。本文模糊推理系統(tǒng)(FIS)選擇的類型為Mamdani，選擇的模糊推理規(guī)則見表1。

表1 模糊推理規(guī)則Table 1 Fuzzy inference rule

輸入1為殘差的實測均值，輸入2為殘差的實測方差與理論方差的差值DROR，輸出則為α、β、d。解模糊化方法選用重心法，即輸出取為隸屬函數(shù)曲線圍成區(qū)域面積的重心。采用形狀為三角形的隸屬度函數(shù)，速度和位置的輸入輸出隸屬函數(shù)如圖2所示。

圖2 輸入輸出隸屬函數(shù)Fig.2 Input and output membership function

4 相對導(dǎo)航系統(tǒng)仿真

仿真通過Matlab和Fuzzy工具箱來實現(xiàn)。在基于模糊自適應(yīng)卡爾曼濾波的系統(tǒng)仿真中，計算出每一采樣時刻的濾波器的殘差方差和均值，通過上述規(guī)則和方法，分別得到濾波器的系統(tǒng)噪聲和量測噪聲的調(diào)整值或是狀態(tài)協(xié)方差陣，從而不斷地修正濾波器，使濾波器趨于穩(wěn)定，估計性能最優(yōu)。

為了比較，在相同的條件下用標(biāo)準(zhǔn)卡爾曼濾波進行了仿真。表2是在其他條件相同的情況下分別改變Q和R的值，常規(guī)卡爾曼濾波和新自適應(yīng)濾波(FLKF)的對比。

表2 FLKF和KF的比較Table 2 Comparison of KF and FAKF by changed Q/R

圖3a、圖3b分別是改變系統(tǒng)方程中的參數(shù)后得到的X軸、Y軸、Z軸方向相對位置誤差和相對速度誤差的仿真結(jié)果。圖中，藍(lán)線表示新自適應(yīng)(FLKF)算法得到的結(jié)果，紅線表示標(biāo)準(zhǔn)卡爾曼濾波算法(KF)得到的結(jié)果。

圖3 FLKF與KF對比精度Fig.3 Comparison of KF and FAKF

從仿真結(jié)果來看，新的自適應(yīng)濾波(FLKF)的誤差均低于常規(guī)卡爾曼濾波器估計的誤差，這說明采用此種自適應(yīng)濾波(FLKF)的GPS/INS系統(tǒng)比用普通卡爾曼濾波技術(shù)具有更高的精度，模糊系統(tǒng)能適當(dāng)降低卡爾曼濾波器的發(fā)散程度，從而達到了應(yīng)用模糊自適應(yīng)系統(tǒng)提高系統(tǒng)定位精度的目的。

但在仿真過程中由于要進行模糊系統(tǒng)運算，增加了判斷語句和模糊算法，算法結(jié)構(gòu)比較復(fù)雜，所以計算效率不高，在此可以進行一些簡化。

當(dāng)編隊飛行中的兩架無人機狀態(tài)相似，兩者之間相對線加速度和相對角速度較小時，可以假定:Ff≈Fl，對模型進行簡化，代入全維濾波模型式(3)和式(6)后，可把相對導(dǎo)航系統(tǒng)狀態(tài)模型降維為原來的一半，得到

降維相對導(dǎo)航系統(tǒng)的觀測方程為

進行計算仿真，由于模型維數(shù)減半，計算量下降，運算速度加快;但要求是兩架無人機飛行狀態(tài)相似，如果進行較大的機動飛行時，濾波的精度可能較差。

總結(jié)為:在無人機機動幅度不大、模型較為準(zhǔn)確的情況下，采用降維濾波模型、常規(guī)卡爾曼濾波以節(jié)省定位時間;在無人機機動形式大、模型不準(zhǔn)確的情況下，采用全維濾波模型、模糊自適應(yīng)卡爾曼濾波來提高系統(tǒng)容錯性。

5 結(jié)論

文章提出了一種新的模糊自適應(yīng)濾波算法，并將其應(yīng)用在INS/GPS相對導(dǎo)航系統(tǒng)中，同常規(guī)卡爾曼濾波算法進行了仿真比較，其算法簡單，意義明確，收斂時間短，運算效率高;并且，它不受系統(tǒng)噪聲方差和觀測噪聲方差改變的影響，對于系統(tǒng)模型不準(zhǔn)確情況有較好的自適應(yīng)效果。因此，它是一種較為理想的自適應(yīng)濾波算法。

［1］ 徐俊，張珩.編隊飛行GPS/INS相對導(dǎo)航的分布濾波算法［J］.電光與控制，2008，15(8):40－44.

［2］ 孟焱，孫增圻.INS/GPS組合導(dǎo)航中實時模糊自適應(yīng)濾波技術(shù)的研究［J］.計算機工程與應(yīng)用，2002，38(8):63－65.

［3］ FOSBURY A M，CRASSIDIS J L.Relative navigation of air vehicles ［J］.Journal of Guidance，Control，and Dynamics，2008，31(4):824－834.

［4］ 楊泗智.衛(wèi)星組合導(dǎo)航與相對定位方法研究［D］.哈爾濱:哈爾濱工業(yè)大學(xué)，2006.

［5］ 徐田來，游文虎，崔平遠(yuǎn).基于模糊自適應(yīng)卡爾曼濾波的INS/GPS組合導(dǎo)航系統(tǒng)算法研究［J］.宇航學(xué)報，2005，26(9):571－575.

［6］ 柏菁，劉建業(yè)，袁信.模糊自適應(yīng)卡爾曼濾波技術(shù)研究［J］.信息與控制，2002，31(6):193－197.

［7］ GENG Yanrui，CUI Zhongxing.Adaptive fading Kalman filter with applications in integrated navigation system［J］.Journal of Beijing University of Aeronautics and Astronautics，2004，30(5):434－437.

［8］ 徐光晨，翁澤宇，丁紅鋼，等.模糊自適應(yīng)卡爾曼濾波在慣性/地磁導(dǎo)航中的應(yīng)用［J］.艦船科學(xué)技術(shù)，2010，32(5):68－72.

［9］ 馬忠孝，劉宗玉，陳明.基于模糊邏輯的自適應(yīng)卡爾曼濾波在GPS/INS組合導(dǎo)航中的應(yīng)用［J］.信息與控制，2006，35(8):457－461.

［10］ SASIADEKT J Z，WANG Q，ZEREMBA M B.Fuzzy adaptive Kalman filtering for INS/GPS［C］//Data Fusion Proceedings the 15th IEEE International Symposium on Intelligent Control.Rio，PatFas.GREECE，July，17－19，2000:181－l86.