許昭霞，王澤元，吳振東

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

0 引 言

高精度導(dǎo)航系統(tǒng)可保障水下航行器順利完成任務(wù)，由于組合導(dǎo)航技術(shù)具有導(dǎo)航精度高，可靠性強(qiáng)等優(yōu)點(diǎn)，被廣泛應(yīng)用于高精度導(dǎo)航系統(tǒng)［1］。對(duì)于組合導(dǎo)航系統(tǒng)模型為精確已知情況，可以用卡爾曼濾波方法進(jìn)行狀態(tài)估計(jì)，獲得狀態(tài)的精確估計(jì)值。但對(duì)于實(shí)際的水下航行器組合導(dǎo)航系統(tǒng)而言，因處于復(fù)雜的工作環(huán)境，不易獲得精確量測噪聲的統(tǒng)計(jì)特性，影響系統(tǒng)對(duì)狀態(tài)誤差的估計(jì)［2］。

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

本文針對(duì)上述問題，為提高估計(jì)精度和抗干擾特性，將模糊邏輯與多模濾波算法相結(jié)合，設(shè)計(jì)了基于模糊自適應(yīng)的改進(jìn)多模型濾波算法，減少模型誤差對(duì)狀態(tài)估計(jì)的影響，利用濾波信息的統(tǒng)計(jì)方差和計(jì)算方差的偏差作為模糊系統(tǒng)輸入，設(shè)計(jì)模糊推理系統(tǒng)調(diào)節(jié)量測噪聲方差。所提方法可以增強(qiáng)系統(tǒng)濾波模型對(duì)外部環(huán)境的適應(yīng)性，有效提高水下航行器組合導(dǎo)航系統(tǒng)的精度。

1 組合導(dǎo)航系統(tǒng)濾波模型分析

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

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

由于水下航行器處于復(fù)雜的工作環(huán)境，當(dāng)其濾波模型建模為式(1)的形式，對(duì)外部環(huán)境的適應(yīng)性不強(qiáng)，因此建模為含有時(shí)變參數(shù)的卡爾曼濾波模型［7］，考慮到水下航行器濾波模型在狀態(tài)方程中不包含時(shí)變參數(shù)，時(shí)變參數(shù)只影響觀測方程，針對(duì)此特點(diǎn)，對(duì)多模型方法進(jìn)行改進(jìn)，建模為如下形式:

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

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

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

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

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

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

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

2 模糊自適應(yīng)多模型濾波算法

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

2.1 模糊自適應(yīng)擴(kuò)展卡爾曼濾波結(jié)構(gòu)

觀測信息的實(shí)際協(xié)方差通過在1 個(gè)大小為N 的移動(dòng)估計(jì)窗口中取平均值近似得到:

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

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

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

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

式中:ΔRk(i)為每個(gè)瞬時(shí)時(shí)刻從Rk(i)中增加或減去的因子;ΔRk(i)為模糊推理系統(tǒng)的輸出;Mk(i)為模糊推理系統(tǒng)的輸入。系統(tǒng)結(jié)構(gòu)如圖1所示。

圖1 觀測噪聲方差模糊自適應(yīng)濾波結(jié)構(gòu)圖Fig.1 Block diagram of fuzzy adaptive filtering on measurement noise

2.2 觀測噪聲方差模糊自適應(yīng)算法

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

1)數(shù)據(jù)預(yù)處理

為了實(shí)現(xiàn)算法的通用性，首先對(duì)Mk(i)進(jìn)行歸一化處理:

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

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

2)隸屬函數(shù)定義

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

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

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

3)模糊規(guī)則

定義輸入輸出變量后，可以利用以下模糊規(guī)則進(jìn)行模糊推理。每個(gè)模糊推理系統(tǒng)規(guī)則庫中包括3個(gè)模糊規(guī)則:

4)輸出數(shù)據(jù)反模糊化

2.3 算法實(shí)現(xiàn)

基于模糊自適應(yīng)改進(jìn)多模型濾波算法的計(jì)算步驟如下:

1)初始化

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

3)更新模型概率

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

4)估算導(dǎo)引頭觀測方程陣

若k 小于設(shè)定觀測窗口值設(shè)定值，則

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

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

3 仿真結(jié)果與分析

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

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

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

因此，在系統(tǒng)噪聲和噪聲模型不準(zhǔn)確的情況下，模糊自適應(yīng)改進(jìn)多模型濾波算法可以有效地抑制系統(tǒng)濾波發(fā)散，提高系統(tǒng)估計(jì)精度。

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

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

4 結(jié) 語

本文提出了一種基于模糊自適應(yīng)改進(jìn)多模型濾波的算法，可提高水下航行器組合導(dǎo)航系統(tǒng)的精度。針對(duì)觀測噪聲，設(shè)計(jì)采樣窗口，在線統(tǒng)計(jì)濾波觀測新息方差，然后應(yīng)用協(xié)方差匹配技術(shù)設(shè)計(jì)模糊推理方法，在線調(diào)整傳感器觀測噪聲協(xié)方差矩陣。仿真結(jié)果表明所提方法可增強(qiáng)估計(jì)算法的抗干擾能力，提高了估計(jì)精度。

［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］王其，徐曉蘇，張濤，等．模糊自適應(yīng)濾波在水下航行器組合導(dǎo)航系統(tǒng)中的應(yīng)用［J］．中國慣性技術(shù)學(xué)報(bào)，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．