基于UT變換改進(jìn)多模型濾波的水下航行器導(dǎo)航方法

王澤元，許昭霞

(1．中國人民解放軍61818 部隊，北京102102;2．中國人民解放軍61081 部隊，北京100094)

0 引 言

由于組合導(dǎo)航技術(shù)具有導(dǎo)航精度高，可靠性強(qiáng)等優(yōu)點，被廣泛應(yīng)用于水下航行器導(dǎo)航系統(tǒng)［1］。對于組合導(dǎo)航系統(tǒng)的狀態(tài)估計，目前多采用卡爾曼濾波或擴(kuò)展卡爾曼濾波對非線性模型進(jìn)行線性化處理，這種處理方法存在2 個問題:一是由于忽略了模型的高階導(dǎo)數(shù)，估計精度隨著時間增長難以保證;二是濾波過程中需要反復(fù)計算系統(tǒng)方程的雅可比矩陣，濾波計算量偏大。

為克服卡爾曼濾波算法精度較低，易于發(fā)散，計算量大等缺點，Julier 針對非線性高斯模型提出了無跡卡爾曼濾波方法(UKF)［2］。已有學(xué)者運用無跡卡爾曼濾波方法解決水下航行器導(dǎo)航系統(tǒng)濾波問題［3－4］，文獻(xiàn)［3］針對復(fù)雜加性噪聲，采用基于球面分布單形采樣，簡化了水下航行器組合導(dǎo)航的UKF 算法;文獻(xiàn)［4］針對無人水下航行器目標(biāo)跟蹤控制需求，仿真驗證了UKF 估計方法的有效性。隨著多傳感器獲取信息融合提高導(dǎo)航精度，文獻(xiàn)［5］將UKF 引入到多模型濾波算法中，既能提高濾波算法的精度，也可并行求解，但是此方法的總體計算量偏大。此外，上述方法沒有考慮利用輔助信息對估計結(jié)果進(jìn)行修正。

本文針對上述問題，將UKF 引入水下航行器組合導(dǎo)航系統(tǒng)濾波中，提出基于UT 變換的簡化多模型算法，同時設(shè)計融合算法，利用輔助信息對估計結(jié)果進(jìn)行修正，提高整體估計精度。首先對非線性觀測方程進(jìn)行UT 變化，利用Sigma 點構(gòu)建偽觀測量，更新模型概率，得到偏差估計結(jié)果并代入到多模型濾波中，得到狀態(tài)估計結(jié)果。其次利用環(huán)境變量分析輔助信息可用度，提高系統(tǒng)魯棒性，并在此基礎(chǔ)上設(shè)計加權(quán)因子融合算法，合理利用輔助信息，提高系統(tǒng)整體估計精度。最后通過仿真驗證測試算法的有效性。

1 組合導(dǎo)航系統(tǒng)濾波模型的UT 變換

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

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

水下航行器組合導(dǎo)航系統(tǒng)建模為式(1)時，存在模型的近似和理想化假設(shè)，直接用于濾波會導(dǎo)致估計精度降低?？紤]到系統(tǒng)非線性對觀測方程的影響，將式(1)的觀測方程表示為:

式中，h(·)為觀測函數(shù);ρvk和ρsk為速度偏差和刻度系數(shù)偏差。

組合導(dǎo)航濾波系統(tǒng)中非線性特性表現(xiàn)在觀測方程，因此只需要對預(yù)測量進(jìn)行采樣，然后利用觀測函數(shù)h(·)對采樣點進(jìn)行處理，得到觀測更新后的系統(tǒng)統(tǒng)計特性。下面通過UT 變化產(chǎn)生Sigma 點對觀測方程進(jìn)行近似，重新構(gòu)建偽觀測量用于估計。

預(yù)測方程如下:

通過以下方法選取2nx+1 個sigma 點χi和相對應(yīng)的權(quán)值wi，進(jìn)行Sigma 點采樣:

為了避免UT 轉(zhuǎn)換后的方差陣為非正定陣，采用Julier［8］提出的尺度轉(zhuǎn)換方法對原始的sigma 點及其權(quán)值進(jìn)行如下轉(zhuǎn)換:

其中，λ=α2(n+κ)- n，α 決定sigma 的散布程度，通常取α ≤1，κ 為比例因子，通常nx+κ=3，β 描述協(xié)方差計算χ0的權(quán)重，為求一階統(tǒng)計特性的權(quán)系數(shù)，為求二階統(tǒng)計特性的權(quán)系數(shù)。

觀測量更新如下:

觀測量和狀態(tài)量的交互協(xié)方差為:

若k 時刻觀測量為zk，則狀態(tài)估計結(jié)果及其協(xié)方差為:

2 輔助信息修正的改進(jìn)多模型濾波算法

對組合導(dǎo)航系統(tǒng)濾波模型的UT 變換及狀態(tài)估計過程中，偏差值未知，下面首先基于sigma 點的偽觀測量進(jìn)行偏差值的估計;然后利用輔助信息對估計結(jié)果進(jìn)行修正，消除累積誤差，提高整體估計精度。

2.1 基于sigma 點的偽觀測量與偏差值估計

由式(8)可以看出，在偏差值ρvk，ρsk已知情況下，可通過UKF 方法得到系統(tǒng)狀態(tài)估計結(jié)果。下面利用式(5)生成的Sigma 點構(gòu)建偽觀測量，然后通過改進(jìn)多模型原理對偏差值進(jìn)行估計。

假設(shè)ρvk，ρsk服從半馬爾科夫假設(shè)，利用離散值對其進(jìn)行建模:

式中二維參數(shù)ρj的總數(shù)N=n·m。

在觀測量zk已知情況下，構(gòu)建偽觀測量的量測信息及其協(xié)方差陣:

式中，p(ρk-1=ρα| Z0:k-1)為tk-1時刻偏差為ρα的概率，由遞推計算得到;θαi(tk)為tk時刻偏差ρα到ρj的馬爾可夫轉(zhuǎn)移概率，由概率轉(zhuǎn)移矩陣求得。

在ρj和p(ρk=ρj| Z0:k)的基礎(chǔ)上計算兩類偏差的估計值和:

2.2 輔助信息修正的改進(jìn)多模型濾波算法

1)輔助信息的可信度分析

接收到的輔助信號z2觀測方程可表示為:

圖1 基于UT 變化的多模型濾波改進(jìn)算法流程圖Fig.1 The flow chart of improved multiple model kalman filtering algorithm based on UT

式中:H2=［I，0］;υ2k為零均值高斯白噪聲。采用卡爾曼濾波對輔助信息進(jìn)行處理，得到狀態(tài)估計量和估計方差P2k。

環(huán)境信息變量描述傳感器數(shù)據(jù)的可用性，通過引入環(huán)境信息變量對傳感器進(jìn)行分析，可以判斷傳感器信息的可行度，確保估計系統(tǒng)的整體精度。定義環(huán)境變量

式中，γk，Sk為觀測新息及其協(xié)方差陣，表達(dá)式為

2)輔助信息的融合算法

在融合過程中需設(shè)置啟動閥λF，當(dāng)λF=0，表示沒有獲得輔助信號，直接輸出多模濾波結(jié)果;當(dāng)λF=1，表示得到輔助信號時，利用環(huán)境變量進(jìn)行檢測，如果輔助信息可信度低，濾波過程中不考慮輔助信息，即

若輔助信號可用，則可以利用如式(28)所提加權(quán)因子法提高估計精度。

其中α1，α2為標(biāo)量加權(quán)因子，表達(dá)式為

融合后狀態(tài)估計方差為

輔助信息修正的改進(jìn)多模型濾波算法結(jié)構(gòu)圖如圖2所示。

圖2 輔助信息修正的改進(jìn)多模型濾波算法流程圖Fig.2 Improved multiple model kalman filtering algorithm used the auxiliary information

2.3 算法實現(xiàn)

輔助信息修正的改進(jìn)多模型濾波算法的計算步驟如下:

第1 步:初始化

確定Sigma 點采樣的參數(shù)，計算初始狀態(tài)估計值、協(xié)方差陣。

第2 步:k 時刻濾波一步預(yù)測值

第4 步:觀測值更新

第5 步:輔助信息修正

判斷閥值λF，通過式(27)或式(28)得到融合后的狀態(tài)和狀態(tài)估計方差。

通過式(10)～式(12)計算狀態(tài)估計結(jié)果及其協(xié)方差。

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

僅考慮在固定水深下的航行，水下航行器組合導(dǎo)航系統(tǒng)的仿真模型和條件同文獻(xiàn)［9］，仿真時間取600 s。為了驗證本文提出方法的有效性，采用了本文方法和常規(guī)的多模型濾波算法進(jìn)行了仿真，2 種方法在緯度和經(jīng)度方向的位置誤差仿真結(jié)果如圖3和圖4所示。

由仿真曲線可知，采用本文算法可以有效地降低系統(tǒng)狀態(tài)誤差，加快濾波狀態(tài)收斂速度。

在估計精度方面，定義性能指標(biāo)函數(shù)Jρv，Jρs為:

采用了本文方法和常規(guī)多模型濾波算法對偏差估計精度進(jìn)行比較，表1 給出估計精度統(tǒng)計指標(biāo)。

圖3 本文改進(jìn)算法的位置誤差曲線Fig.3 Errors of position of improved algorithm

圖4 多模型濾波算法的位置誤差曲線Fig.4 Errors of position of traditional multiple model filtering algorithm

從表1 中的數(shù)據(jù)結(jié)果可知，本文算法通過UT變換對多模型算法進(jìn)行修正，提高了估計精度。

表1 濾波算法估計精度Tab.1 Estimated accuracy of filtering algorithm

4 結(jié) 語

本文提出了一種基于UT 變換的簡化多模型算法，并利用輔助信息對估計結(jié)果進(jìn)行修正。針對非線性觀測方程進(jìn)行UT 變化，利用Sigma 點構(gòu)建偽觀測量，進(jìn)而更新模型概率，得到偏差估計結(jié)果并代入到多模型濾波中，得到狀態(tài)估計結(jié)果;然后利用環(huán)境變量分析輔助信息的可用度，設(shè)計加權(quán)因子融合算法，合理利用輔助信息，提高系統(tǒng)整體估計精度。

仿真結(jié)果表明所提方法有效地降低系統(tǒng)狀態(tài)誤差，加快濾波狀態(tài)收斂速度，同時對多模型算法進(jìn)行修正，提高了估計精度。

［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］JULIER S J，UHHNANN J K．Unscented filtering and nonlinear estimation［J］．Proceedings of the IEEE，2004，92(3):401－422．

［3］劉明雍，胡俊偉，李聞白．一種基于改進(jìn)無跡卡爾曼濾波的自主水下航行器組合導(dǎo)航方法研究［J］．兵工學(xué)報，2011，32(2):252－256．

［4］王征．基于PF和UKF 的水下目標(biāo)運動估計方法研究［J］．自動化技術(shù)與應(yīng)用，2013，32(12):12－17．

［5］朱安福，景占榮，高田，等．基于UKF 的交互多模型算法［J］．系統(tǒng)仿真學(xué)報，2009，21(2):655－657．

［6］王其，徐曉蘇，張濤，等．模糊自適應(yīng)濾波在水下航行器組合導(dǎo)航系統(tǒng)中的應(yīng)用［J］．中國慣性技術(shù)學(xué)報，2008，16(3):320－325．

［7］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．

［8］JULIER S J，UHLMANN J K，Durrant－Whyte H F．A new approach for filtering nonlinear systems［C］．Proceedings of the American Control Conference，Evanston，USA，1995:1628－1632．

［9］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．