水下航行器導航系統的模糊自適應多模型濾波方法

許昭霞，王澤元，吳振東

(1．中國人民解放軍61081 部隊，北京100094;2．中國人民解放軍61618 部隊，北京102102;3．中國船舶重工集團公司 第七一四研究所，北京100012)

0 引 言

高精度導航系統可保障水下航行器順利完成任務，由于組合導航技術具有導航精度高，可靠性強等優點，被廣泛應用于高精度導航系統［1］。對于組合導航系統模型為精確已知情況，可以用卡爾曼濾波方法進行狀態估計，獲得狀態的精確估計值。但對于實際的水下航行器組合導航系統而言，因處于復雜的工作環境，不易獲得精確量測噪聲的統計特性，影響系統對狀態誤差的估計［2］。

為在線估計量測噪聲統計特性，文獻［3－4］采用自適應濾波方法，通過分析實時量測信息實際方差與理論方差的差異，設計推理模型在線實時調整量測噪聲矩陣。文獻［5］采用模糊系統調整濾波器的增益系數，在線自適應調整子濾波器，并對聯邦濾波器信息分配系數進行模糊自適應調整。但是這些方法均假設系統模型固定不變，降低了估計算法的抗干擾能力。

本文針對上述問題，為提高估計精度和抗干擾特性，將模糊邏輯與多模濾波算法相結合，設計了基于模糊自適應的改進多模型濾波算法，減少模型誤差對狀態估計的影響，利用濾波信息的統計方差和計算方差的偏差作為模糊系統輸入，設計模糊推理系統調節量測噪聲方差。所提方法可以增強系統濾波模型對外部環境的適應性，有效提高水下航行器組合導航系統的精度。

1 組合導航系統濾波模型分析

對于水下組合導航系統，線性化后離散系統數學模型如下［5－6］:

式中:xk為狀態向量;zk為觀測向量;Φk為狀態轉移矩陣;Γk為噪聲干擾矩陣;Hk為觀測矩陣;ωk，υk為零均值高斯白噪聲，且。

由于水下航行器處于復雜的工作環境，當其濾波模型建模為式(1)的形式，對外部環境的適應性不強，因此建模為含有時變參數的卡爾曼濾波模型［7］，考慮到水下航行器濾波模型在狀態方程中不包含時變參數，時變參數只影響觀測方程，針對此特點，對多模型方法進行改進，建模為如下形式:

式中:sk為時變參數向量，其取值為有限的一系列真實值，sk∈{s1，s2，…sn}，n 為參數向量可能取值的個數，sk只影響系統觀測方程，且其變化符合半馬爾科夫假設，離散系統式(2)可以依據參數sk的不同分為n 個子系統。

基于貝葉斯估計理論和全概率公式，在估計方差最小條件下，sk的最優估計為:

式中:模型匹配概率p(sk=si| Z0:k)計算公式如下:

式中:分母p(zk| Z0:k－1)為歸一化系數，由全概率公式計算。式(4)中分子第一項似然函數p(zk|Z0:k－1，sk=si)與最新觀測量zk有關，體現觀測更新后的模型概率更新，利用子濾波器的觀測信息γk(si)及其協方差陣Sk(si)求得，即

式中γk(si)和Sk(si)為觀測新息及其協方差陣。

式(4)中分子第二項p(sk=si| Z0:k－1)體現先驗知識，利用馬爾科夫狀態轉移概率遞推求得，即

通過式(3)～式(5)和式(8)，可得到參數sk的估計值，將代入式(2)，消除濾波模型的不確定性，然后可采用擴展卡爾曼濾波算法得到狀態的估計值。

2 模糊自適應多模型濾波算法

從上述分析可知，求解式(5)過程中需要得到準確的觀測噪聲統計特性，然而，在實際應用中，由于環境干擾等多方面因素，很難先驗給出觀測噪聲方差的精確值，降低似然函數的估計精度，進而影響多模算法整體估計性能，下面給出本文提出的模糊自適應擴展卡爾曼濾波結構，應用協方差匹配技術設計模糊推理方法，在線調整傳感器觀測噪聲協方差矩陣。

2.1 模糊自適應擴展卡爾曼濾波結構

觀測信息的實際協方差通過在1 個大小為N 的移動估計窗口中取平均值近似得到:

式中:i0=k－N+1 是在估計窗中的第一次采樣，這表明只有最后的N 個采樣時刻的γi值被用于估計其協方差，N 根據經驗選取使得協方差的計算具有較好的統計性(N=10 或15)。當信息序列的實際協方差與其理論值有偏差時，通過模糊推理系統根據這個偏差調整Rk。為了描述與Sk之間的偏差，定義新的變量匹配度:

理論上說，如果濾波過程中設定的Rk與真實值相符，則Mk應該為一小量，因此可以通過判斷Mk的大小來自適應調整Rk的大小，進而改變Sk的大小，使Mk趨于0。由于Rk為對角陣，可以設計單輸入單輸出的自適應算法調整Rk的第i 行上的對角線元素Rk(i)。為了調節系統特性，設定相對于Rk(i)閥值μk(i)＞0，具體調節方式如下:

2)如果Mk(i)＞μk(i)，表明減小Rk(i)值。

3)如果Mk(i)＜－μk(i)，表明，增大Rk(i)值。

式中:ΔRk(i)為每個瞬時時刻從Rk(i)中增加或減去的因子;ΔRk(i)為模糊推理系統的輸出;Mk(i)為模糊推理系統的輸入。系統結構如圖1所示。

圖1 觀測噪聲方差模糊自適應濾波結構圖Fig.1 Block diagram of fuzzy adaptive filtering on measurement noise

2.2 觀測噪聲方差模糊自適應算法

由圖1 可見，模糊推理系統的輸入為新息序列的實際協方差與其理論值的匹配度Mk，輸出為觀測噪聲估計值的增量。模糊推理系統的主要包括:數據預處理、隸屬度函數定義、模糊規則和輸出數據反模糊化。

1)數據預處理

為了實現算法的通用性，首先對Mk(i)進行歸一化處理:

傳感器的測量噪聲調整量ΔRk(i)的范圍定義與傳感器特性當關，定義歸一化的測量噪聲增量。將的范圍定義為［－1，1］，則測量噪聲的增量由式(4)得到:

式中:m0為一正常數，m0的選取影響Rk(i)的收斂速度和收斂精度。m0值選取較大，Rk(i)可以迅速逼近真實的量測噪聲矩陣，Rk(i)振蕩嚴重;m0值選取較小，Rk(i)收斂速度較慢，估計方差較小。

2)隸屬函數定義

模糊推理需要利用輸入、輸出變量的模糊集、論域和隸屬度函數，將變量的精確值映射到模糊集上。定義的模糊集為P(正)，Z(0)，N(負)。~Mk(i)的隸屬度函數采用三角形函數。輸出變量是歸一化的測量噪聲增量，通過它實現測量噪聲的自調節。定義的模糊集為I(增大)，M(保持)，D(減小)。輸入輸出變量的隸屬度函數采用三角形函數，如圖2和圖3所示。

圖2 觀測噪聲方差輸入變量的隸屬度函數Fig.2 Membership grade of measurement noise covariance input variable

圖3 觀測噪聲方差輸出變量的隸屬度函數Fig.3 Membership grade of measurement noise covariance output variable

3)模糊規則

定義輸入輸出變量后，可以利用以下模糊規則進行模糊推理。每個模糊推理系統規則庫中包括3個模糊規則:

4)輸出數據反模糊化

2.3 算法實現

基于模糊自適應改進多模型濾波算法的計算步驟如下:

1)初始化

確定參數集合{s1，s2，…sn}和模型轉換概率θαi(tk)，i=1，2，…n，α=1，2，…n，計算初始狀態估計值、協方差陣。

3)更新模型概率

其中p(zk| ρk=ρi，Z0:k－1)由式(5)得到。

4)估算導引頭觀測方程陣

若k 小于設定觀測窗口值設定值，則

若k 大于窗口值設定值，利用前面提出的模糊推理方法，得到ΔRk，則

利用式(4)和式(3)計算模型概率p(sk=si|Z0:k)，得到參數s 的估計結果。

3 仿真結果與分析

僅考慮在固定水深下的航行，水下航行器組合導航系統的仿真模型和條件同文獻［2］，仿真時間取1 000 s。從仿真時間600 s 開始，將系統噪聲干擾的方差陣參數取為600 s 前的100 倍。

為驗證本文提出方法的有效性，采用本文的方法和卡爾曼濾波算法進行仿真，2 種方法在緯度和經度方向的位置誤差仿真結果如圖4和圖5所示。

由仿真曲線可知，采用模糊自適應改進多模型濾波算法可以有效地降低系統狀態誤差，加快濾波狀態收斂速度;仿真時間在600 s 后，單一卡爾曼濾波器由于噪聲矢量和方差陣固定不變，導致濾波效果不好，而模糊自適應改進多模型濾波算法仿真結果不受影響。

因此，在系統噪聲和噪聲模型不準確的情況下，模糊自適應改進多模型濾波算法可以有效地抑制系統濾波發散，提高系統估計精度。

圖4 模糊自適應多模型濾波算法的位置誤差曲線Fig.4 Position error curve of fuzzy adaptive multiple model Kalman filtering algorithm

圖5 卡爾曼濾波算法的位置誤差曲線Fig.5 Position error curve of Kalman filtering algorithm

4 結 語

本文提出了一種基于模糊自適應改進多模型濾波的算法，可提高水下航行器組合導航系統的精度。針對觀測噪聲，設計采樣窗口，在線統計濾波觀測新息方差，然后應用協方差匹配技術設計模糊推理方法，在線調整傳感器觀測噪聲協方差矩陣。仿真結果表明所提方法可增強估計算法的抗干擾能力，提高了估計精度。

［1］LI Kang-hua，WANG Jin-ling，LI Wan-li，et al．A noval INS and doppler sensors calibration method for long range underwater vehicle navigation［J］．Sensors，2013，13(14):583－600．

［2］HAO Yan-ling，MU Hong-wei．Application of integrated alignment in AUV based on GP－SRCDKF［J］．Advances in Information Sciences and Service Sciences，2012，4(21):598－605．

［3］LOEBIS D，SUTTON B，CHUDLEY J，et al．Adaptive tuning of a Kalman filter via fuzzy logic for an intelligent AUV navigation System［J］．Control Engineering Practice，2004，12(12):1531－1539．

［4］ZHU Feng-chun，JU Yan-bing，WANG Ai-hua．Research on integrated navigation technology of field robot［C］．2006 IEEE International Conference on Information Acquisition，2006:59－64．

［5］王其，徐曉蘇，張濤，等．模糊自適應濾波在水下航行器組合導航系統中的應用［J］．中國慣性技術學報，2008，16(3):320－325．

［6］KIM H S，KIM I H．Design of adaptive fuzzy IMM algorithm for tracking the maneuvering target with time-varying measurement noise［J］．International Journal of Control，Automation，and Systems，2007，5(3):307－316．

［7］LI Wen-ling，JIA Ying-min．Consensus-based distributed multiple model UKF for jump markov nonlinear systems［J］．IEEE Transactions on Automatic Control，2012，57(1):230－236．