王君婷，王振杰，2

(1.中國(guó)石油大學(xué)(華東)，山東 青島 266580；2.海洋國(guó)家實(shí)驗(yàn)室 海洋礦產(chǎn)資源評(píng)價(jià)與探測(cè)技術(shù)功能實(shí)驗(yàn)室，山東 青島 266071)

0 引言

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

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

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

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

1 基于長(zhǎng)基線的AUV水下聲學(xué)導(dǎo)航系統(tǒng)

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

圖1 基于長(zhǎng)基線的走航式水下定位

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

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

圖2 AUV水下聲學(xué)定位原理

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

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

(1)

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

(2)

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

2 AUV動(dòng)態(tài)導(dǎo)航中的UKF濾波

2.1 基于M估計(jì)的抗差UKF濾波

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

1)初始化

(3)

(4)

2)時(shí)間更新

(5)

(6)

(7)

計(jì)算預(yù)測(cè)σ點(diǎn)，即

(8)

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

(9)

(10)

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

3)計(jì)算預(yù)測(cè)量測(cè)值，即

(11)

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

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

(12)

(13)

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

5)計(jì)算Kalman增益，即

(14)

6)更新?tīng)顟B(tài)和協(xié)方差，即

(15)

(16)

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

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

(17)

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

(18)

(19)

(20)

2.2 改進(jìn)的抗差UKF濾波

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

(21)

(22)

(23)

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

(24)

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

(25)

(26)

(27)

(28)

(29)

式中：

這個(gè)值來(lái)自χ2分布時(shí)的自由度是9，可靠性水平是95 %。

在標(biāo)準(zhǔn)UKF的基礎(chǔ)上，由式(29)計(jì)算得到的濾波增益矩陣根據(jù)式(15)更新?tīng)顟B(tài)和協(xié)方差，從而得到正確的濾波結(jié)果。

3 實(shí)驗(yàn)及結(jié)果分析

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

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

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

表1 含粗差時(shí)4種濾波算法的X、Y坐標(biāo)差值統(tǒng)計(jì) m

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

結(jié)果分析如下：

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

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

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

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

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

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

4 結(jié)束語(yǔ)

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

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

[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組合導(dǎo)航技術(shù)上的應(yīng)用研究[J].聲學(xué)技術(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的改進(jìn)算法及其在偽衛(wèi)星定位中的應(yīng)用[J]. 測(cè)繪科學(xué)技術(shù)學(xué)報(bào), 2008, 25(2):108-111.

[6] 汪秋婷.自適應(yīng)抗差UKF在衛(wèi)星組合導(dǎo)航中的理論與應(yīng)用研究[D].武漢:華中科技大學(xué),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] 李莉.長(zhǎng)基線陣測(cè)陣校陣技術(shù)研究[D].哈爾濱：哈爾濱工程大學(xué)，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ì)的水下地形抗差匹配算法[J].武漢大學(xué)學(xué)報(bào)(信息科學(xué)版)，2015，40(4)：558-562.

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

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

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

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