改進的抗差UKF在AUV水下聲學導航中的應(yīng)用

王君婷，王振杰，2

(1.中國石油大學(華東)，山東 青島 266580；2.海洋國家實驗室 海洋礦產(chǎn)資源評價與探測技術(shù)功能實驗室，山東 青島 266071)

0 引言

伴隨著水聲通信、能源、控制和導航技術(shù)的發(fā)展，自主式水下航行器(autonomous underwater vehicle，AUV)廣泛應(yīng)用于海上預(yù)警、水下搜索、海洋資源勘探、海底地形地貌調(diào)查與水下環(huán)境監(jiān)測等領(lǐng)域[1]。高精度的導航系統(tǒng)是AUV實現(xiàn)自主導航的關(guān)鍵技術(shù)，已有的AUV導航技術(shù)包括慣性導航、聲學導航以及地球物理導航[2]。隨著近年來聲學定位技術(shù)的迅速發(fā)展，聲學導航克服了慣性導航中誤差會隨著時間積累的缺點，廣泛應(yīng)用于AUV導航系統(tǒng)中。水聲定位技術(shù)可分為長基線定位(long baseline，LBL)、短基線定位(short baseline，SBL)和超短基線定位(ultra short baseline，USBL)。本文基于長基線定位系統(tǒng)進行AUV位置解算的無跡卡爾曼濾波(unscented Kalman filter，UKF)濾波算法研究。

常見的AUV導航濾波方法有拓展卡爾曼濾波算法(extended Kalman filter，EKF)、無跡卡爾曼濾波算法(unscented Kalman filter，UKF)、粒子濾波算法(particle filter，PF)。EKF算法具有實時數(shù)據(jù)處理能力，在非線性強度較弱的動態(tài)數(shù)據(jù)處理時有較大的優(yōu)勢；但是對于強非線性系統(tǒng)，線性化的EKF方法會出現(xiàn)較大的截斷誤差，甚至導致濾波發(fā)散，算法的定位精度嚴重降低。粒子濾波器(PF)在非線性情況下有較好的性能，但是需要大量的粒子樣本才能達到最優(yōu)的濾波效果[3]。UKF濾波算法是用固定量的參數(shù)去近似一個高斯分布，比近似任意的非線性函數(shù)或變換更容易；UKF算法的濾波效果不會受到非線性強度的影響，更加適用于AUV導航系統(tǒng)中非線性的動態(tài)估計。

在實際的觀測過程中，觀測數(shù)據(jù)難免存在觀測粗差。UKF算法無法隨著觀測系統(tǒng)的改變而進行自動調(diào)節(jié)，影響了實時濾波估計結(jié)果，從而大大減少了UKF濾波算法的穩(wěn)定性和濾波解的精度。為了抑制觀測粗差對濾波結(jié)果的影響，提高濾波結(jié)果的穩(wěn)定性，文獻[4]分別提出了多模型自適應(yīng)估計法(multiple model adaptive estimation，MMAE)、采用預(yù)報殘差向量估計當前觀測殘差的協(xié)方差矩陣的自適應(yīng)開窗估計法(innovation-based adaptive estimation，IAE)、采用測量殘差向量估計當前觀測殘差的協(xié)方差矩陣的自適應(yīng)開窗估計法(residual-based adaptive estimation，RAE)。MMAE算法要求幾種平行的卡爾曼濾波器進行估計從而獲得濾波的正確統(tǒng)計信息，導致了估計效率降低，IAE法和RAE法對觀測噪聲協(xié)方差矩陣進行估計，要求各歷元觀測信息不僅同類、同分布，而且同維。文獻[5-6]基于殘差向量，采用觀測信息的等價協(xié)方差矩陣原理，給出了基于M估計的抗差UKF的算法。文獻[7]提出了一種測量噪聲比例因子，將其與觀測噪聲協(xié)方差矩陣相乘以減弱觀測粗差的影響。

本文結(jié)合2段函數(shù)模型和測量噪聲比例因子構(gòu)造抗差因子函數(shù)，進而計算UKF算法中的增益矩陣，同時利用文獻[8]提出的統(tǒng)計信息來進行粗差探測，以期改善僅依據(jù)殘差進行粗差探測而無法完全正確識別粗差的問題。

1 基于長基線的AUV水下聲學導航系統(tǒng)

目前大部分的AUV都裝有全球衛(wèi)星導航系統(tǒng)(global navigation satellite system，GNSS)接收機，用來對水面航行時的AUV定位校準，且方便回收。為解決水下導航的問題，則要結(jié)合水下定位系統(tǒng)和慣性導航系統(tǒng)得到AUV的位置信息和航向信息?；陂L基線的AUV導航的基本原理如圖1所示。

圖1 基于長基線的走航式水下定位

從圖中可知，走航式布設(shè)海底應(yīng)答器的工作過程類似于空間全球定位系統(tǒng)(global positioning system，GPS)的原理[9]。搭載著GPS接收機的測量船可以得到船的實時位置，并利用姿態(tài)儀測量船體的實時姿態(tài)。船的實時位置和姿態(tài)數(shù)據(jù)可換算出船底換能器的實時位置。換能器通過發(fā)射聲脈沖信號并接收水下應(yīng)答器的應(yīng)答信號，得到換能器與應(yīng)答器之間的雙向距離。測量船沿某一航跡航行可以得到不同時刻的多組觀測值，再進行后處理就可以解算出應(yīng)答器在WGS-84坐標系統(tǒng)中的坐標[10]。

通過走航式布設(shè)海底應(yīng)答器同時進行應(yīng)答器位置的求解，可以求得海底應(yīng)答器的位置，原理如圖2所示。

圖2 AUV水下聲學定位原理

已知海底應(yīng)答器的空間三維坐標位置為x，海底AUV的三維坐標位置為xk，k時刻測得的AUV至應(yīng)答器的距離zk(傳播時間乘以聲速)，則有AUV導航的觀測方程和狀態(tài)方程為

觀測方程：zk=h((xk,x),vk)

(1)

狀態(tài)方程：xk=f(xk-1,wk-1)

(2)

式中：坐標系為局部地方坐標系，坐標原點可設(shè)為研究區(qū)域的任意合適位置，坐標軸可設(shè)為北、東、天或者自定義的空間直角坐標系：h(·)為關(guān)聯(lián)k時刻狀態(tài)xk和k時刻測量向量zk的更新方程；f(·)為關(guān)聯(lián)k-1時刻狀態(tài)xk-1和k時刻狀態(tài)xk的差分形式的預(yù)測方程；wk-1為過程噪聲向量；vk為觀測噪聲向量。

2 AUV動態(tài)導航中的UKF濾波

2.1 基于M估計的抗差UKF濾波

UKF的基本原理是在原先狀態(tài)分布中按某一規(guī)則(U變換)取一些點，使這些點的均值和協(xié)方差等于原狀態(tài)分布的均值和協(xié)方差，將這些點代入非線性函數(shù)中，相應(yīng)得到非線性函數(shù)值點集，通過這些點求取變換后的均值和協(xié)方差[6]。UKF濾波算法的步驟如下：

1)初始化

(3)

(4)

2)時間更新

(5)

(6)

(7)

計算預(yù)測σ點，即

(8)

計算預(yù)測估計值和預(yù)測協(xié)方差，即

(9)

(10)

式中Qk表示狀態(tài)噪聲協(xié)方差矩陣。

3)計算預(yù)測量測值，即

(11)

式中h[]表示量測方程函數(shù)。

4)計算信息方差和協(xié)方差，即

(12)

(13)

式中Rk表示量測噪聲協(xié)方差矩陣。

5)計算Kalman增益，即

(14)

6)更新狀態(tài)和協(xié)方差，即

(15)

(16)

基于M估計的抗差UKF的濾波方程只需在標準UKF濾波方程的基礎(chǔ)上，利用穩(wěn)健M估計等價權(quán)原理[11]求逆計算等價協(xié)方差陣，對噪聲協(xié)方差陣Rk進行等價協(xié)方差的替換，從而起到調(diào)節(jié)Kalman濾波增益的作用，以使濾波方程對導航系統(tǒng)中存在的觀測粗差有較強的抵抗作用，進而消除濾波方程發(fā)散的可能性[6]。

根據(jù)標準UKF方程，建立基于M估計的抗差UKF的濾波模型，只需對式(12)進行修改為

(17)

穩(wěn)健M估計目前常用的方法有Huber法[12]、IGGⅢ法等，本文將應(yīng)用IGGⅢ法計算等加權(quán)，求逆獲得等價協(xié)方差矩陣，減小或消除粗差對估計結(jié)構(gòu)的影響。等價權(quán)的計算公式[13]為

(18)

(19)

(20)

2.2 改進的抗差UKF濾波

當觀測值不含有粗差時，標準的UKF算法可以得到較好的濾波結(jié)果；一旦觀測值中含有粗差，必定會影響狀態(tài)參數(shù)的估計，得不到可靠的濾波結(jié)果，甚至導致濾波發(fā)散。由式(21)可以看出粗差通過狀態(tài)增益矩陣K影響到狀態(tài)濾波值；由式(22)可知，通過采用基于預(yù)報殘差的測量噪聲比例因子Sk來調(diào)節(jié)信息方差；同時由式(23)求得正確的濾波增益值，最終得到準確的狀態(tài)濾波結(jié)果。其計算過程為

(21)

(22)

(23)

改進的抗差UKF算法的核心是測量噪聲比例因子Sk的確定。式(24)中，不等式左側(cè)是真實的信息方差值，不等式右側(cè)是預(yù)報信息方差值。當量測值不等于預(yù)測量測值時，真實的信息方差值就會超過預(yù)報信息方差值，因此需要測量尺度因子Sk來調(diào)節(jié)信息方差。

(24)

為了計算測量噪聲比例因子Sk，由式(24)構(gòu)造式(25)，考慮式(26)的情況，得到測量噪聲比例因子Sk即式(27)[8]。

(25)

(26)

(27)

(28)

(29)

式中：

這個值來自χ2分布時的自由度是9，可靠性水平是95 %。

在標準UKF的基礎(chǔ)上，由式(29)計算得到的濾波增益矩陣根據(jù)式(15)更新狀態(tài)和協(xié)方差，從而得到正確的濾波結(jié)果。

3 實驗及結(jié)果分析

模擬海上AUV的定位方式如圖3所示。模擬海底應(yīng)答器陣對AUV進行導航，用三角符號表示應(yīng)答器，4個應(yīng)答器呈中心放射狀布設(shè)在海底，應(yīng)答器的坐標分別為(0 m，500 m，-497 m)，(435 m，-253 m，-486 m)，(-428 m，-232 m，-474 m)，(5 m，7 m，-456 m)。模擬AUV在水下100 m深的位置做勻速轉(zhuǎn)彎運動，AUV的運動參數(shù)分別是X坐標、Y坐標、速度、半徑和轉(zhuǎn)向角，AUV的初始位置是(100 m，0 m，-100 m)，半徑為100 m，轉(zhuǎn)向角為π/100，仿真100個歷元，采樣間隔1 s，采用Munk聲速剖面和射線跟蹤算法模擬觀測量，并對觀測量添加隨機噪聲，同時每隔幾個歷元對其量測值分別添加10～15 m的隨機觀測粗差，然后將濾波結(jié)果分別與已知的坐標值作差，改變AUV的初始位置，模擬20個歷元，每個歷元都是加入的隨機粗差；算法中采用最小二乘估計初始值，重復上述過程。

文章通過仿真算例對標準UKF、粒子濾波、改進的抗差UKF與基于M估計的抗差UKF進行對比分析，結(jié)果如圖4～圖5、表1所示。

圖3 海上AUV曲線運動軌跡和海底應(yīng)答器模擬示意

表1 含粗差時4種濾波算法的X、Y坐標差值統(tǒng)計 m

為了驗證算法的精度和穩(wěn)定性，改變AUV的初始位置，利用標準UKF、粒子濾波、改進的抗差UKF與基于M估計的抗差UKF進行解算，得到結(jié)果如表2所示。圖6為改進的抗差UKF與基于M估計的抗差UKF算法的精度比較結(jié)果。

結(jié)果分析如下：

1)由圖4可以看出：在觀測無粗差時，2種抗差UKF算法和標準UKF算法得到的濾波結(jié)果相當；但是由于觀測值中加入了隨機噪聲，基于M估計的抗差UKF僅僅基于預(yù)報殘差來進行粗差探測，導致濾波結(jié)果在某一歷元采樣時刻出現(xiàn)較小的偏差；粒子濾波算法的定位精度與標準UKF算法的效果相當。

2)由圖5可以看出：在觀測值中存在粗差時，標準UKF算法和粒子濾波算法不能對數(shù)據(jù)中的粗差進行處理，由于粒子權(quán)重錯誤選取導致定位結(jié)果出現(xiàn)錯誤[15]，標準UKF濾波由于不具有抗差性，所以濾波估值存在較大的誤差；2種抗差UKF算法都可以判斷出含粗差的觀測值，并且不同程度上抑制粗差觀測值對濾波結(jié)果的干擾。從表1可知，本文中改進的抗差UKF算法的最大值、平均值和RMS值都要小于基于M估計的抗差UKF算法，當粗差影響較大時，可以得到更為可靠的濾波結(jié)果。

表2 含粗差初始位置不同時4種濾波算法坐標差值的RMS值統(tǒng)計 m

圖5 含粗差時4種濾波算法解算結(jié)果對比

圖6 模擬20次的2種抗差UKF算法的RMS值對比

3)為了驗證算法的有效性和穩(wěn)定性，模擬20個歷元對2種抗差算法進行對比分析。由表2可以看出：標準UKF和粒子濾波不具有抗差性，定位精度較差且結(jié)果相當；另2種抗差濾波定位精度明顯提高。由圖6可以看出，本文中改進的抗差UKF算法不僅具有較高精度的濾波結(jié)果，而且與基于M估計的抗差UKF算法相比，改進的抗差UKF具有更好的穩(wěn)定性。

4 結(jié)束語

1)預(yù)報殘差可以判斷觀測值是否存在粗差；但是由于1個粗差影響所有殘差，使得僅利用預(yù)報殘差進行粗差探測會出現(xiàn)偏差。本文基于預(yù)報殘差和信息方差來進行粗差探測，可以更為準確地判斷觀測值是否存在粗差：結(jié)合2段函數(shù)和測量尺度因子來構(gòu)造抗差因子，利用抗差因子增大觀測協(xié)方差來減小存在觀測粗差的濾波增益，可以降低觀測粗差對濾波估值的影響，提高濾波解的精度和可靠性。

2)由于采用2段函數(shù)模型來構(gòu)造抗差因子，使得濾波算法僅受1個常數(shù)的影響，可以提高算法的穩(wěn)定性，使其在實際觀測數(shù)據(jù)的粗差處理上更具優(yōu)勢。

[1] CHANDRASEKHAR V, SEAH W K, CHOO Y S, et al. Localization in underwater sensor networks-survey and challenges[EB/OL].[2017-07-16].http://vijaychan.github.io/Publications/2006%20-%20Localization%20in%20Underwater%20Sensor%20Networks-%20Survey%20and%20challenges.pdf.

[2] 崔海英,崔鑫山,羅康,等.粒子濾波在AUV組合導航技術(shù)上的應(yīng)用研究[J].聲學技術(shù), 2014,33(5):269-270.

[3] LEFEBVRE T, BRUYNINCKX H, SCHUTTER J D. Kalman filters for nonlinear systems[EB/OL].[2017-07-16].https://link.springer.com/chapter/10.1007%2F11533054_4.

[4] HIDE C, MOORE T, SMITH M. Adaptive Kalman filtering algorithms for integrating GPS and low cost INS [EB/OL].[2017-07-16].https://ieeexplore.ieee.org/document/1308998/.

[5] 李雪鵬,張幼群,包括. UKF的改進算法及其在偽衛(wèi)星定位中的應(yīng)用[J]. 測繪科學技術(shù)學報, 2008, 25(2):108-111.

[6] 汪秋婷.自適應(yīng)抗差UKF在衛(wèi)星組合導航中的理論與應(yīng)用研究[D].武漢:華中科技大學,2010.

[7] HAJIYEV C. Adaptive filtration algorithm with the filter gain correction applied to integrated INS/radar altimeter[J]. Journal of Aerospace Engineering, 2007, 221(5):847-855.

[8] HAJIYEV C, ATA M, DINC M, et al. Fault tolerant estimation of autonomous underwater vehicle dynamics via robust UKF[EB/OL].[2017-07-16].https://innovate.ieee.org/see-if-your-organization-or-institution-qualifies-for-a-free-trial-of-ieee-xplore/?LT=XPLLG_XPL_Bold728x90_2018_FT_Subpage.

[9] 李莉.長基線陣測陣校陣技術(shù)研究[D].哈爾濱：哈爾濱工程大學，2007.

[10] SATO M，F(xiàn)UJITA M，MATSUMOTO Y，et al.Improvement of GPS acoustic seafloor positioning precision through controlling the ship’s track line[J].Journal of Geodesy，2013，87(9)：825-842.

[11] 張凱，趙建虎，張紅梅.一種基于M估計的水下地形抗差匹配算法[J].武漢大學學報(信息科學版)，2015，40(4)：558-562.

[12] 趙全哲，徐愛功，徐宗秋，等.基于穩(wěn)健總體最小二乘的GPS水準擬合[J].導航定位學報，2017，5(1)：95-99.

[13] 楊元喜.等價權(quán)原理：參數(shù)平差模型的抗差最小二乘解[J].測繪通報.1996(6)：33-35.

[14] 楊元喜，任夏，許艷.自適應(yīng)抗差濾波理論及應(yīng)用的主要進展[J].導航定位學報，2013，1(1)：10-15.

[15] 范澎湃，隋立芬，曹軼之.觀測值含有粗差的抗差粒子濾波算法[J].測繪工程，2009，18(2)：24-27.