基于新息的SINS/DVL組合導航自適應濾波算法

李萬里， 陳明劍， 張倫東， 陳 銳

(1.信息工程大學地理空間信息學院， 鄭州 450001； 2.北斗導航應用技術(shù)協(xié)調(diào)創(chuàng)新中心， 鄭州 450001)

自主水下航行器(Autonomous Underwater Vehicle，AUV)無論在軍事還是民用領(lǐng)域都有廣泛的應用。然而，目前可用于水下導航的技術(shù)仍然十分有限。全球衛(wèi)星導航系統(tǒng)(Global Navigation Satellite System，GNSS)在陸地、空中及水面艦艇導航得到了廣泛的應用，但是GNSS信號在水下很快會被阻隔。捷聯(lián)式慣性導航系統(tǒng)(SINS)可以提供完備的導航參數(shù)，且更新頻率高，是實現(xiàn)水下自主導航的一個重要手段。但是，由于慣性導航系統(tǒng)的誤差會隨時間累積，難以滿足長航時高精度導航的需求，為此，需要額外的輔助傳感器[1-3]。多普勒測速儀(DVL)是根據(jù)多普勒效應測量載體速度的儀器[4]。為實現(xiàn)水下遠程、長航時、高精度自主導航，多采用慣導/多普勒(SINS/DVL)組合，以卡爾曼濾波的方式進行組合導航[5-7]。

DVL利用安裝在載體上的超聲換能器向海底發(fā)射超聲波，并根據(jù)多普勒效應測量載體的速度。DVL的測速精度對組合導航的精度顯然是至關(guān)重要的。然而，多普勒在使用的過程中，由于遇到魚群、水下地形的突變等原因，DVL的測速精度會出現(xiàn)下降的情況[8]。若水深超過測速的工作范圍，DVL只能獲得相對于水流的速度，甚至出現(xiàn)失效的情況。為此，需要對DVL測速精度發(fā)生變化時的組合導航方法進行研究。目前，常用的手段主要有兩種:一是多普勒測速精度下降或者多普勒失效時，通過載體的運動學模型代替DVL輸出速度觀測量[9],該方式可以在DVL失效的條件下短時維持組合導航的精度，但是在一定程度上增加了算法的復雜度，需要增加一個子濾波器實時估計載體的速度;二是采用自適應濾波的方式[10],典型的自適應濾波算法有Sage-Husa自適應濾波、基于遺忘因子的自適應濾波算法、多模卡爾曼自適應濾波算法、過程噪聲自適應濾波算法等[11-14]。自適應濾波通過在線調(diào)整系統(tǒng)噪聲矩陣或者量測噪聲矩陣來降低系統(tǒng)模型或者噪聲統(tǒng)計特性不準確所帶來的影響。可以在一定程度上降低DVL精度下降的影響。然而，自適應濾波算法在選擇和使用的時候需要考慮它的可靠性，否則容易出現(xiàn)濾波發(fā)散的現(xiàn)象[15]。

量測噪聲反映的是組合導航系統(tǒng)量測值的精度特性，若DVL的精度下降，則可以對量測噪聲進行在線調(diào)整，以此來降低對組合導航的精度。基于此，本文提出一種基于新息的抗差自適應濾波算法。算法通過χ2檢驗判斷DVL的測速精度是否發(fā)生變化，DVL的測速精度若發(fā)生明顯變化，組合導航系統(tǒng)則切換到自適應濾波算法,以降低測速精度下降帶來的影響。

1 SINS/DVL組合導航系統(tǒng)

1.1 系統(tǒng)方程

SINS/DVL采用速度組合方式，即用DVL輸出的速度信息輔助慣導，用DVL和慣導輸出的速度信息的差值作為量測值，利用卡爾曼濾波對慣導系統(tǒng)誤差進行估計，然后對慣導系統(tǒng)進行校正。根據(jù)慣性導航系統(tǒng)的誤差方程可以建立組合導航系統(tǒng)的狀態(tài)方程。選取北-天-東(NUE)地理坐標系為導航參考坐標系，記為n系。慣性導航系統(tǒng)的速度誤差方程、姿態(tài)誤差方程可以表示為：

(1)

(2)

加速度計零偏與陀螺零偏建模為常值，則有：

(3)

(4)

組合導航系統(tǒng)的狀態(tài)量選擇如下：北向和東向的速度誤差δVN和δVE；姿態(tài)誤差角φx、φy、φz；加速度計零偏▽x、▽y、▽z；陀螺零偏εx、εy、εz。即系統(tǒng)的狀態(tài)量為：

(5)

則系統(tǒng)的狀態(tài)方程可表示為：

(6)

式(6)中：W(t)為零均值高斯白噪聲，由陀螺儀、加速度計的性能確定；F(t)為系統(tǒng)矩陣，其具體形式可參見文獻[16]。

1.2 量測方程

DVL的速度觀測值可以通過慣性導航系統(tǒng)姿態(tài)轉(zhuǎn)換到導航坐標系下：

(7)

(8)

式(8)中，φ×為姿態(tài)誤差φ的斜對稱矩陣。則式(7)可化為：

(9)

慣性導航系統(tǒng)與DVL的速度之差即為組合導航系統(tǒng)的觀測量，即：

(10)

由于慣性導航系統(tǒng)的天向通道是發(fā)散的，去掉天向通道的觀測值，則觀測方程為：

(11)

式(11)中，η(t)為零均值高斯白噪聲。觀測矩陣H(t)為：

(12)

(13)

2 SINS/DVL組合導航系統(tǒng)

2.1 基于新息的卡爾曼濾波算法

組合導航系統(tǒng)的狀態(tài)方程和觀測方程經(jīng)過離散化，可化為：

(14)

式(14)中：Φk為狀態(tài)轉(zhuǎn)移矩陣；Γk為噪聲分配矩陣；Wk與ηk是互不相關(guān)的高斯白噪聲，它們的噪聲方差分別用Q和R來表示。在卡爾曼濾波中，一般采用固定系統(tǒng)噪聲與量測噪聲方差矩陣的形式。其中，量測噪聲方差代表了測量數(shù)據(jù)可信的精度。量測噪聲方差越大，說明觀測值越不可靠，反之亦然。為保證組合導航的精度，可以在濾波中根據(jù)實時的量測數(shù)據(jù)自動調(diào)整量測噪聲方差。基于新息的自適應濾波算法正是基于這個思想，根據(jù)新息序列，實時估計量測噪聲方差。

新息序列定義為：

εk=Zk-HkXk，k-1=Hk(Xk-Xk，k-1)+ηk

(15)

當新息序列為零均值高斯白噪聲時，卡爾曼濾波為最優(yōu)。對式(15)兩邊求方差，可得：

(16)

式(16)中，Pk,k-1為k時刻卡爾曼濾波協(xié)方差陣矩陣的預測值。因此，利用新息序列就可以求出量測噪聲Rk的值，即：

(17)

計算時僅保留對角線上的元素。在實際使用的過程中，通常用一個滑動數(shù)據(jù)窗來保存量測噪聲的在線估計值，并且用前m個時刻量測噪聲的平均值作為當前噪聲值，即：

(18)

式(18)中，m為滑動平均窗的長度。

2.2 自濾波算法流程

在實際使用的過程中，為了保證卡爾曼濾波的穩(wěn)定性，僅在多普勒觀測值的精度有明顯變化或者異常的時候再切換到自適應濾波。本文采用χ2檢驗法來判斷DVL的測量值是否出現(xiàn)異常[6]。

中華合作時報社編委、中國農(nóng)資傳媒執(zhí)行總編輯孫立新以媒體視角看經(jīng)銷商的轉(zhuǎn)型。他在《新時代農(nóng)資經(jīng)銷商的機遇與使命》專題報告中，以媒體的視角分析了近年來農(nóng)資經(jīng)銷商的轉(zhuǎn)型和未來的發(fā)展方向，以中國農(nóng)資傳媒傳統(tǒng)活動項目——全國百佳（優(yōu)秀）農(nóng)資經(jīng)銷商評選的活動為例，闡述經(jīng)銷商正在發(fā)揮自身的特殊優(yōu)勢，扎根基層、誠信經(jīng)營，悄然向?qū)Α叭r(nóng)”的服務商轉(zhuǎn)變，投身公益事業(yè)，努力成為國家倡導的“一懂兩愛”的群體，承擔起美麗鄉(xiāng)村建設(shè)的特殊中間力量。

定義新息的協(xié)方差矩陣為：

(19)

利用新息可以構(gòu)造一個統(tǒng)計量：

(20)

統(tǒng)計量γk服從χ2分布，通過設(shè)定誤警率可以確定閾值TD，則可以利用以下準則來判斷DVL的測量值是否出現(xiàn)異常，即：

(21)

若DVL數(shù)據(jù)異常，則切換到自適應濾波方式。綜合2.1節(jié)、2.2節(jié)，本文所提出的抗差自適應濾波算法整體流程如圖1所示。DVL數(shù)據(jù)正常時，組合導航系統(tǒng)工作在普通卡爾曼濾波的形式，并利用滑動數(shù)據(jù)窗口實時估計量測噪聲。當通過χ2檢驗判斷DVL數(shù)據(jù)出現(xiàn)異常時，則切換到自適應濾波模式，以降低DVL數(shù)據(jù)異常對組合導航精度的影響。

圖1 抗差自適應濾波算法流程框圖

3 數(shù)據(jù)處理與結(jié)果分析

3.1 組合導航性能分析

為了驗證本文的算法，采用長江某次船載實驗的實測數(shù)據(jù)進行分析。實驗航程約30 km，航行時間約6 600 s。試驗采用某型激光陀螺捷聯(lián)慣導系統(tǒng)，陀螺零偏穩(wěn)定性0.02 deg/h(1σ)，加速度計零偏穩(wěn)定性5×10-5g(1σ)，采樣頻率為200 Hz，姿態(tài)更新頻率為100 Hz。多普勒測試儀的發(fā)射頻率為300 kHz，速度更新頻率為1 Hz，測速精度約為0.5% V±0.5 cm/s。實驗船上裝載GNSS接收機，以SINS/GNSS組合導航的輸出結(jié)果作為基準，評估SINS/DVL組合導航的性能。船載實驗的軌跡如圖2所示。

圖2 船載實驗行船軌跡

圖3為SINS/DVL組合導航姿態(tài)誤差曲線。由于DVL提供的是載體系d下速度觀測值，SINS/DVL方位誤差角的可觀測度較弱。水平誤差角在組合導航模式下快速收斂，而航向誤差角收斂較為緩慢。但是，從圖4的姿態(tài)誤差局部放大曲線可以看出，SINS/DVL組合導航的姿態(tài)誤差能夠隨著時間收斂，與SINS/GNSS組合導航的結(jié)果比較，組合導航最終的方位角誤差穩(wěn)定在0.02°以內(nèi)，水平姿態(tài)角誤差穩(wěn)定在0均值附近。

圖3 SINS/DVL組合導航姿態(tài)誤差曲線

圖4 SINS/DVL姿態(tài)誤差曲線(局部放大)

圖5為SINS/DVL組合導航位置誤差曲線。受方位誤差角收斂緩慢的影響，位置誤差在組合導航的前半段(0～3 000 s)變化較為劇烈，且變化規(guī)律具有一定的隨機性。隨著組合導航的姿態(tài)誤差趨于穩(wěn)定，在組合導航的后半段，位置誤差曲線的變化也較為平穩(wěn)。然而，在無位置觀測的模式下，SINS/DVL組合導航的誤差累積是無法消除的。組合導航最終的定位誤差約為110 m，相當于航程的3.67‰。

圖5 SINS/DVL組合導航位置誤差曲線

3.2 量測噪聲在線估計

為分析量測噪聲自適應濾波算法的性能，采用不同長度的滑動窗口長度(m=25,50,100)、不同的量測噪聲初值估計量測噪聲。圖6為在不同量測噪聲初值條件下利用自適應濾波算法實時估計量測噪聲曲線。量測噪聲的初始值分別設(shè)為0.5 m2·s-2、0.1 m2·s-2、0.01 m2·s-2，滑動窗口的長度設(shè)定為50。由圖6結(jié)果可以看出，采用不同初始值所估計的量測噪聲方差很快收斂到幾乎同一條曲線上。由此可見，基于新息的自適應濾波算法在估計量測噪聲時，對量測噪聲的初始值并不敏感，具有較強的魯棒性。

圖6 不同量測噪聲初值估計量測噪聲曲線

圖7為采用不同長度滑動數(shù)據(jù)窗口估計量測噪聲曲線。量測噪聲的初始定為0.5 m2·s-2。實驗結(jié)果表明，采用不同長度滑動數(shù)據(jù)窗口所估計的量測噪聲方差值很快收斂到同樣的數(shù)值附近。從圖8局部放大曲線可以看出，當用于估計量測噪聲的滑動窗口比較短時，量測噪聲的估計值抖動比較厲害，而滑動窗口較長時，則量測噪聲的估計曲線則比較平穩(wěn)。在實際應用中，若想盡量消除歷史觀測數(shù)據(jù)對濾波的影響，則可以選取較短的滑動數(shù)據(jù)窗。

圖7 不同長度滑動窗口估計量測噪聲曲線

圖8 不同長度滑動窗口估計量測噪聲曲線(局部放大)

3.3 自適應濾波算法性能評估

為評估本文所提出的抗差自適應濾波算法的性能，在DVL的原始觀測值中人為加入噪聲，如圖9所示，在DVL數(shù)據(jù)4 000～4 300 s這個時段，3個方向的速度觀測值中均加入均值為0、標準差為1的正態(tài)分布隨機噪聲。以SINS/GNSS組合導航的結(jié)果作為基準，圖10為SINS/DVL組合導航速度誤差曲線。由圖10中的結(jié)果看出，以普通卡爾曼濾波進行組合導航，無法消除附加觀測噪聲的影響。在速度誤差曲線的4 000～4 300 s附近，組合導航的速度誤差明顯增大。而抗差自適應濾波算法則可以明顯改善量測噪聲所帶來的影響，速度誤差曲線比較平穩(wěn)。

圖9 DVL速度觀測值

圖10 SINS/DVL組合導航速度誤差曲線

由圖11所示的SINS/DVL組合導航位置誤差曲線可以看出，在加入了噪聲的位置(4 000～4 300 s)，普通卡爾曼濾波算法所得到的位置誤差有明顯增大。而基于抗差自適應濾波算法的位置誤差增長程度略低于普通卡爾曼，進一步驗證了自適應濾波算法的優(yōu)越性。值得注意的是，自適應濾波算法組合導航的結(jié)果仍然比不加觀測噪聲的結(jié)果稍差一些。也就是說自適應濾波算法并不能完全消除觀測噪聲增大的影響。

圖11 SINS/DVL組合導航位置誤差曲線

4 結(jié)論

1) 基于信息的自適應濾波算法能有效地在線估計量測噪聲，在不同長度的滑動數(shù)據(jù)窗口、不同的量測噪聲初值條件下，量測噪聲估計值都能收斂。

2) 當DVL數(shù)據(jù)出現(xiàn)異常時，抗差自適應濾波算法能在一定程度上抑制組合導航誤差的增長，性能優(yōu)于普通卡爾曼濾波。