張 鷺，周繼航，馬向東，張廣娜，劉旭光

基于滑動窗口抗差自適應(yīng)濾波的SINS/DVL組合導航算法

張 鷺，周繼航，馬向東，張廣娜，劉旭光

（中國人民解放軍63892部隊，洛陽 471003）

捷聯(lián)慣性導航系統(tǒng)（SINS）/多普勒測速儀（DVL）組合導航時，需要考慮多普勒速度信息中出現(xiàn)的野值和噪聲特性變化對導航精度的影響。針對上述問題，提出基于滑動窗口抗差自適應(yīng)濾波的SINS/DVL組合導航算法。首先建立狀態(tài)變換漂移誤差角系統(tǒng)誤差模型，在速度誤差方程中用重力常值項代替比力項，減少比力項引起的速度誤差，提高模型的準確性。然后利用新息序列判別野值，若異常則采用滑動窗口數(shù)據(jù)修正錯誤新息，并使用帶遺忘因子的自適應(yīng)濾波在線估計量測噪聲。實驗結(jié)果證明，新的系統(tǒng)誤差模型和滑動窗口抗差自適應(yīng)濾波能有效減緩位置誤差的累積。2 h的SINS/DVL組合導航中誤差航程比優(yōu)于1.05%，相比于傳統(tǒng)卡爾曼濾波性能提升39.66%。

捷聯(lián)慣性導航系統(tǒng)；多普勒測速儀；組合導航；狀態(tài)變換；滑動窗口；抗差自適應(yīng)濾波

0 引言

水下潛航器（Autonomous Undersea Vehicle，AUV）具有隱蔽性強、自主性好的特點，可代替人工進行危險任務(wù)，目前已廣泛應(yīng)用于水下探測、打撈、運載等多種水下作業(yè)中。水下導航精度是決定AUV性能的重要指標之一[1-3]。受水下特殊環(huán)境的影響，全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System，GNSS）信號無法在水中傳播，水下導航方式具有較大的局限性。捷聯(lián)慣性導航系統(tǒng)（Strap-down Inertial Navigation System，SINS），具有自主性好、導航參數(shù)完備、更新頻率高的優(yōu)點，是重要的水下導航方式之一，但其導航誤差會隨時間不斷累積。特別對于小型AUV，考慮體積、重量和成本等因素，普遍采用中低精度SINS，其解算的速度信息通常發(fā)散速度快，單獨作為導航系統(tǒng)難以滿足長航時導航的實際需求。而多普勒測速儀（Doppler Velocity Log，DVL）是利用安裝在載體上的換能器接收向海底發(fā)射的聲波，根據(jù)多普勒效應(yīng)測量載體速度的儀器[4]，速度誤差不隨時間累積，可提供一定精度的速度信息，能有效彌補SINS中速度誤差隨時間不斷累積的缺點。因此SINS/DVL組合導航是目前最常用、技術(shù)相對成熟的水下導航方式，但復(fù)雜的水下環(huán)境易導致DVL測速出現(xiàn)野值。在SINS/DVL組合導航時，噪聲的統(tǒng)計變化情況[5]采用標準卡爾曼濾波方法，該方法無法有效剔除或修正野值，造成濾波器性能惡化，降低導航精度。

針對這一問題，需要對SINS/DVL組合導航算法進行改進。常用的改進方法一是通過載體運動學模型計算出載體速度，代替DVL速度完成組合導航，但這只能實現(xiàn)短時間內(nèi)組合導航，長時間的組合導航結(jié)果較差[6]。二是采用自適應(yīng)濾波，常用的濾波器有Suga_husa自適應(yīng)濾波、帶遺忘因子濾波器和抗差自適應(yīng)濾波等[7-9]，Suga_husa自適應(yīng)濾波和帶遺忘因子濾波器算法通過調(diào)整DVL噪聲的統(tǒng)計特性降低DVL精度下降的影響，抗差自適應(yīng)濾波主要通過殘差辨別野值，降低野值對組合導航的影響。然而，在使用自適應(yīng)濾波過程中需要考慮濾波器的可靠性，否則容易出現(xiàn)濾波器發(fā)散[9]。

基于此，本文同時考慮小型AUV進行SINS/DVL組合導航時，中低精度SINS速度信息易發(fā)散、DVL測速易出現(xiàn)野值、噪聲特性變化的問題，提出基于滑動窗口抗差自適應(yīng)濾波的SINS/DVL組合導航算法。首先構(gòu)建了地理坐標系下狀態(tài)變換漂移誤差角系統(tǒng)誤差模型。相比于傳統(tǒng)誤差模型，狀態(tài)變換系統(tǒng)誤差模型通過對速度誤差方程的狀態(tài)變換，將傳統(tǒng)誤差狀態(tài)方程中的導航系下比力項替換成重力項，有效減少動態(tài)環(huán)境下誤差狀態(tài)協(xié)方差陣計算誤差，實現(xiàn)更高的速度估計精度[10-12]。然后設(shè)計滑動窗口抗差自適應(yīng)濾波，通過分布辨別野值是否出現(xiàn)，若出現(xiàn)則切換到自適應(yīng)濾波算法，降低DVL速度野值和噪聲特性動態(tài)變化對組合導航精度的影響。

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

1.1 狀態(tài)變換漂移誤差角系統(tǒng)誤差方程

等式兩邊同時微分得到

將式（3）和式（9）帶入式（8）中，依據(jù)雅克比恒等式并忽略二階小項可得到

相應(yīng)的位置誤差方程改寫為

忽略高度矢量，則北向和東向位置誤差表達式為

綜上，計算地理坐標系下狀態(tài)變換漂移誤差角系統(tǒng)誤差方程為

1.2 量測方程

DVL速度觀測值在計算坐標系下的投影為

在SINS/DVL組合導航中，以SINS解算得到的速度與多普勒速度在計算坐標系下速度的差值為量測值，構(gòu)建量測方程如式（15）所示

1.3 SINS/DVL組合導航系統(tǒng)誤差模型

SINS/DVL組合導航時忽略高度通道矢量，則系統(tǒng)誤差模型如式（16）所示

綜上得到狀態(tài)變換下漂移誤差角的SINS/DVL組合導航誤差模型，組合導航流程圖如圖1所示。

圖1 SINS/DVL組合導航算法流程圖

2 滑動抗差自適應(yīng)濾波

滑動窗口抗差自適應(yīng)濾波分為滑動窗口抗差濾波和自適應(yīng)濾波兩部分，滑動抗差用于甄別和修正DVL速度中存在的野值，自適應(yīng)濾波用于實時估計變換的測量噪聲統(tǒng)計特性。

2.1 滑動窗口抗差濾波

對1.3節(jié)中系統(tǒng)模型離散化得到離散隨機線性系統(tǒng)

根據(jù)線性濾波器理論定義新息如式（22）所示

2.2 自適應(yīng)濾波

經(jīng)移項可得量測噪聲協(xié)方差矩陣如式（30） 所示

2.3 算法流程

滑動抗差自適應(yīng)濾波算法流程如圖2所示。

圖2 滑動窗口抗差自適應(yīng)濾波流程圖

算法具體實現(xiàn)步驟如下：

3）判斷觀測信息DVL速度是否更新時，無則返回2），有則進入步驟4）；

3 實驗結(jié)果分析

以某次水下的實驗數(shù)據(jù)對算法有效性進行分析，實驗平臺SINS、DVL測速儀和GNSS系統(tǒng)，其性能指標如表1所示。實驗分為兩個部分，一是驗證組合導航模型有效性，二是滑動抗差自適應(yīng)算法有效性。實驗中以SINS/GNSS組合導航結(jié)果作為基準。

表1 實驗平臺傳感器性能

3.1 系統(tǒng)誤差模型有效性驗證

為驗證系統(tǒng)誤差模型的有效性，以SINS/GNSS組合導航結(jié)果作為基準，潛航器漂浮水面進行180 s粗對準后直接進行SINS/DVL組合導航，運行時間為2 h。

圖3 狀態(tài)變換模型與傳統(tǒng)模型姿態(tài)誤差對比圖

圖4 狀態(tài)變換模型與傳統(tǒng)模型速度誤差對比

位置誤差曲線如圖5所示，不同時刻兩誤差模型的位置誤差如表2所示，分析圖5和表2易知，隨著導航時間的增長，狀態(tài)變換誤差模型的位置誤差逐漸小于傳統(tǒng)誤差模型，7 200 s組合導航過程中，狀態(tài)變換模型最大位置誤差為158 m，最大誤差航程比為1.74%，傳統(tǒng)模型最大位置誤差為184.41 m，最大誤差航程比為1.78%。

圖5 狀態(tài)變換模型與傳統(tǒng)模型位置誤差對比

表2 狀態(tài)變換模型和傳統(tǒng)模型位置誤差

綜合上述實驗結(jié)果，本文使用的狀態(tài)變換系統(tǒng)誤差模型相比于傳統(tǒng)模型，更加適用于長航時SINS/DVL組合導航。

3.2 滑動窗口抗差自適應(yīng)濾波效果驗證

實驗中，DVL速度在系下的投影和SINS/GNSS組合導航的速度對比如圖6所示，圖中DVL速度有出現(xiàn)超出其噪聲統(tǒng)計特性的測量值，即野值。因此SINS/DVL組合導航時如采用Kalman濾波，不能有效剔除或校正野值，會降低導航精度。

采用滑動抗差自適應(yīng)濾波狀態(tài)變換SINS/DVL組合導航（State Transformation Adaptive Kalman Filter，STAKF）與標準Kalman濾波狀態(tài)變換SINS/DVL組合導航（State Transformation Kalman Filter，STKF）結(jié)果如圖7～圖9和表3所示。圖7是姿態(tài)誤差曲線，兩種濾波器均能準確估計姿態(tài)誤差，水平誤差角穩(wěn)定在0.001°，方位誤差角優(yōu)于0.1°。從圖8速度誤差曲線中可以看到，STAKF的速度誤差小于STKF相同時刻的速度誤差，STAKF北向和東向速度均方根誤差為0.064 m/s和0.062 m/s，優(yōu)于STKF的0.074 m/s和0.075 m/s。

圖6 DVL速度與SINS/DVL組合導航速度對比圖

圖7 STAKF與STKF姿態(tài)誤差對比圖

圖8 STAKF與STKF速度誤差對比圖

圖9 STAKF與STKF位置誤差對比圖

表3 STAKF與STKF位置誤差

圖9是組合導航位置誤差曲線，表3是不同時刻兩種濾波器的位置誤差，圖9中顯示STAKF的位置誤差小于STKF，且在修正野值影響后其誤差曲線相較于傳統(tǒng)卡爾曼濾波器誤差更加平滑。2 h的組合導航過程中，STAKF最大位置誤差為 78.84 m，航程誤差比為1.05%，最大位置誤差和最大航程誤差比分別提升52.77%和39.66%。

上述實驗中，野值不連續(xù)出現(xiàn)。針對DVL測速儀可能連續(xù)產(chǎn)生野值的情況，本節(jié)通過仿真手段驗證算法的有效性。在已有的DVL測速數(shù)據(jù)600～660 s間疊加隨機數(shù)值仿真DVL測速野值，如圖10所示。

圖10 DVL速度仿真曲線

實驗結(jié)果如圖11和圖12所示，圖11為速度誤差曲線，結(jié)果表明滑動抗差自適應(yīng)濾波能有效抑制連續(xù)野值對速度估計的影響，速度誤差控制在0.2 m/s范圍內(nèi)。圖12為位置誤差曲線，相比于傳統(tǒng)濾波器，本文提出的濾波器能有效抑制連續(xù)野值造成的位置誤差，但隨著連續(xù)野值出現(xiàn)時間的增長，性能有所降低。實驗結(jié)果表明，針對一定時間范圍內(nèi)連續(xù)野值，滑動窗口抗差自適應(yīng)濾波能夠提升濾波器性能，抑制導航定位精度惡化。提高SINS/DVL組合導航精度。

圖11 速度誤差曲線

圖12 位置誤差曲線

4 結(jié)語

[1] Wynn R B，Huvenne V A I，Le Bas T P，et al. Autonomous Underwater Vehicles （AUVs）：Their past，present and future contributions to the advancement of marine geoscience[J]. Marine Geology，2014，352：451-468.

[2] Brown H C，Kim A，Eustice R M. An overview of autonomous underwater vehicle research and Tested at ReRL[J]. Journal of Marine Technology Society，2009，43（2）：33-47.

[3] 徐博，劉德政，張勛. 基于交互式模型的多AUV協(xié)同導航魯棒性濾波算法[J]. 系統(tǒng)工程與電子技術(shù)，2017，39（9）：2087-2093.

[4] Pan X，Wu Y. Underwater Doppler Navigation with self-calibration[J]. Journal of navigation，2016，69（2）：295-312.

[5] Liu P，Wang B，Deng Z，et al. INS/DVL/GPS tightly DVL measurements[J]. IEEE Sensor Journal，2018，18（7）：2994-3002.

[6] Zhao L，Liu X，Wang L，et al. A pretreatment method for the velocity of DVL based on the motion constraint for the interated SINS/DVL[J]. Applied Sciences，2016（6）：79.

[7] 魏偉，秦永元，張曉東，等. 對Suga_Husa算法的改進[J]. 慣性技術(shù)學報，2012，20（6）：768-786.

[8] 曾慶化，趙天鈺，趙賓，等. 基于指數(shù)漸消遺忘因子的組合導航自適應(yīng)濾波算法[J]. 中國慣性技術(shù)學報，2021，29（3）：307-313.

[9] 李萬里，陳明劍，張倫東，等. 基于新息的SINS/DVL組合導航自適應(yīng)濾波算法[J]. 兵器裝備工程學報，2020，41（12）：225-229+252．

[10] Maosong W，Wenqi W，Peiyuan Z，et al. State transformation extended Kalman filter for GPS/SINS tightly coupled integration[J]. GPS Solutions，2018，22（4）：112.

[11] 王茂松，吳文啟，何曉峰，等. 狀態(tài)變換卡爾曼濾波的進一步解釋與應(yīng)用[J]. 中國慣性技術(shù)學報，2019，27（4）：449-504+509.

[12] Wang M，Wu W，He X，et al. Consistent ST-EKF for Long Distance Land Vehicle Navigation Based on SINS/OD Integration[J]. IEEE Transactions on Vehicular Technology，2019，68（11）：10525-10534.

[13] Weston J. Strapdown Inertial Navigation Technology，2nd Edition[M]. IEEE Aerospace & Electronic Systems Magazine，2005.

[14] Eun-Hwan Shin. Estimation Techniques for Low-Cost Inertial Navigation[D]. Calgary：University of Calgary，2015.

[15] 高鐘毓. 慣性導航系統(tǒng)技術(shù)[M]. 北京：清華大學出版社，2012.

SINS/DVL Integrated Navigation Based on Sliding Window Robust Adaptive Kalman Filter

ZHANG Lu, ZHOU Jihang, MA Xiangdong, ZHANG Guangna, LIU Xuguang

It’s necessary to consider the impact of Doppler Velocity Log (DVL) measurement outliers and its time-varying noise characteristic on navigation accuracy in Strap-down Inertial Navigation System (SINS)/DVL integrated navigation. To address the problem, a sliding window robust adaptive Kalman filter for SINS/DVL integrated navigation is proposed. Firstly, the system error model is established based on psi angle state transformation. The force item of its velocity is replaced by constant gravity term, so that the accuracy of the model is improved by reducing the speed error caused by the force item. Then the innovation sequence is used to verified outliers. If the outliers exist, the sliding window data is used to correct the innovation sequence and adaptive Kalman filter algorithm with attenuating factor is utilized for estimate observation noise. The experimental results show that the new error model and sliding window robust adaptive Kalman filter can effectively suppression the position error divergence. The position error can be controlled in 1.05% in two hours’ SINS/DVL integrated navigation. The navigation position accuracy is enhanced by 39.66% compared to Kalman filter.

Strap-down Inertial Navigation System; Doppler Velocity Log; Integrated Navigation; State Transformation; Sliding Window; Robust Adaptive Kalman Filter

TN966

A

1674-7976-(2023)-05-324-09

2023-03-22。

張鷺（1991.02—），江西奉新人，博士，助理研究員，主要研究方向為組合導航、慣性導航。