王艷東， 韓智華， 蔡君亮， 陳曉強(qiáng)， 吉慶昌

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

單個(gè)無人機(jī)系統(tǒng)的觀測(cè)模型為

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

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

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

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

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

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

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

定義測(cè)量殘差值(新息)

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

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

下面逐一對(duì)兩種方法進(jìn)行分析。

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

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

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

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

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

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

定義殘差實(shí)測(cè)方差與理論方差的差值為

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

2.2 強(qiáng)跟蹤濾波方法

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖3 FLKF與KF對(duì)比精度Fig.3 Comparison of KF and FAKF

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

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

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

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

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

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

5 結(jié)論

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

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

［2］ 孟焱，孫增圻.INS/GPS組合導(dǎo)航中實(shí)時(shí)模糊自適應(yīng)濾波技術(shù)的研究［J］.計(jì)算機(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)航與相對(duì)定位方法研究［D］.哈爾濱:哈爾濱工業(yè)大學(xué)，2006.

［5］ 徐田來，游文虎，崔平遠(yuǎn).基于模糊自適應(yīng)卡爾曼濾波的INS/GPS組合導(dǎo)航系統(tǒng)算法研究［J］.宇航學(xué)報(bào)，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.