模糊自適應濾波方法在相對導航系統中的應用

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

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

0 引言

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

針對上述問題，本文采用一種新的模糊自適應卡爾曼濾波技術，即調整系統噪聲與觀測噪聲的方差陣或強跟蹤濾波方法進行自適應的濾波方法，與以往文獻的方法相比，本文結合了兩種濾波方法的優點，逐步調整系統中的參數，保證濾波器能實時處理新的量測值，以解決系統噪聲和觀測噪聲不準確或是濾波器的發散問題。

1 相對導航模型系統模型

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

1.1 相對導航系統狀態方程

相對導航系統狀態變量為 X=［δP，δV，δq，δw，δf］T。其中:δP、δV 分別為地球坐標系下的位置、速度誤差;δq為姿態角對應的四元數誤差;δw、δf分別為陀螺儀和加速度計的輸出值［4］。

假定單個無人機系統的狀態方程為

引導機和跟隨機合并得到兩無人機導航狀態方程為

其中:下標l為引導機;f為跟隨機。

式(2)經過轉換得到兩無人機的相對狀態方程為

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

1.2 相對導航系統觀測方程

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

單個無人機系統的觀測模型為

引導機和跟隨機結合得到兩無人機導航測量方程為

引導機和跟跟機相減得到兩無人機的相對測量方程為

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

2 模糊自適應濾波

對于INS/GPS組合導航系統，一般的線性離散系統數學模型［5－6］如下:

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

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

定義測量殘差值(新息)

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

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

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

2.1 調整系統噪聲與觀測噪聲方差陣

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

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

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

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

式中:Q和R是初始常值;α、β是調整值，均為時變。根據Kalman濾波器的殘差特性自適應地調整系統噪聲和觀測噪聲誤差方差陣，當α=β=1，就是常規的Kalman濾波。

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

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

2.2 強跟蹤濾波方法

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

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

在STKF中關鍵的參數為λk，要得到λk還要求取4個參數 αi，ρ，d，β，其他參數通常根據經驗獲得，取 α1=α2=...=αm=1，0＜ρ≤1，d根據模糊自適應系統獲取。

3 模糊自適應濾波實現

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

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

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

表1 模糊推理規則Table 1 Fuzzy inference rule

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

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

4 相對導航系統仿真

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

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

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

圖3a、圖3b分別是改變系統方程中的參數后得到的X軸、Y軸、Z軸方向相對位置誤差和相對速度誤差的仿真結果。圖中，藍線表示新自適應(FLKF)算法得到的結果，紅線表示標準卡爾曼濾波算法(KF)得到的結果。

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

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

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

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

降維相對導航系統的觀測方程為

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

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

5 結論

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

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

［2］ 孟焱，孫增圻.INS/GPS組合導航中實時模糊自適應濾波技術的研究［J］.計算機工程與應用，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］ 楊泗智.衛星組合導航與相對定位方法研究［D］.哈爾濱:哈爾濱工業大學，2006.

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

［6］ 柏菁，劉建業，袁信.模糊自適應卡爾曼濾波技術研究［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］ 徐光晨，翁澤宇，丁紅鋼，等.模糊自適應卡爾曼濾波在慣性/地磁導航中的應用［J］.艦船科學技術，2010，32(5):68－72.

［9］ 馬忠孝，劉宗玉，陳明.基于模糊邏輯的自適應卡爾曼濾波在GPS/INS組合導航中的應用［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.