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

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

(1.信息工程大學地理空間信息學院， 鄭州 450001； 2.北斗導航應用技術協調創新中心， 鄭州 450001)

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

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

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

1 SINS/DVL組合導航系統

1.1 系統方程

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

(1)

(2)

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

(3)

(4)

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

(5)

則系統的狀態方程可表示為：

(6)

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

1.2 量測方程

DVL的速度觀測值可以通過慣性導航系統姿態轉換到導航坐標系下：

(7)

(8)

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

(9)

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

(10)

由于慣性導航系統的天向通道是發散的，去掉天向通道的觀測值，則觀測方程為：

(11)

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

(12)

(13)

2 SINS/DVL組合導航系統

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

組合導航系統的狀態方程和觀測方程經過離散化，可化為：

(14)

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

新息序列定義為：

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

(15)

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

(16)

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

(17)

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

(18)

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

2.2 自濾波算法流程

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

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

定義新息的協方差矩陣為：

(19)

利用新息可以構造一個統計量：

(20)

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

(21)

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

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

3 數據處理與結果分析

3.1 組合導航性能分析

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

圖2 船載實驗行船軌跡

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

圖3 SINS/DVL組合導航姿態誤差曲線

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

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

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

3.2 量測噪聲在線估計

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

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

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

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

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

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

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

圖9 DVL速度觀測值

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

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

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

4 結論

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

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