羅才智，楊 鯤，辛明真，衛(wèi)進(jìn)進(jìn)，張 凱，陽凡林*

（1. 山東科技大學(xué) 測繪科學(xué)與工程學(xué)院，山東 青島 266590；2. 交通運(yùn)輸部天津水運(yùn)工程科學(xué)研究所，天津 300456）

水下目標(biāo)定位導(dǎo)航是海洋開發(fā)活動(dòng)與海洋技術(shù)發(fā)展的基礎(chǔ)，水下聲學(xué)定位系統(tǒng)包括長基線（Long Baseline，LBL）定位系統(tǒng)、短基線（Short Baseline，SBL）定位系統(tǒng)、超短基線（Ultra-Short Baseline，USBL）定位系統(tǒng)和GNSS智能浮標(biāo)（GNSS Intelligent Buoys，GIB）系統(tǒng)[1-3]。其中，USBL 定位系統(tǒng)利用各基元信號間的相位差來解算目標(biāo)的方位和距離，實(shí)現(xiàn)對目標(biāo)的跟蹤定位[4]。相較于其他幾種水下定位方法，USBL 定位系統(tǒng)由于只需要一個(gè)換能器，因此其集成系統(tǒng)價(jià)格更低廉，且有構(gòu)成簡單、操作維護(hù)方便的優(yōu)點(diǎn)[5]。

受海洋環(huán)境動(dòng)態(tài)變化、測量儀器安裝校準(zhǔn)偏差、外圍設(shè)備測量可靠性、聲速測量與改正精度等因素的影響，USBL 定位會(huì)出現(xiàn)較大粗差和連續(xù)異常誤差。經(jīng)典卡爾曼濾波的前提假設(shè)是濾波的狀態(tài)模型和觀測模型準(zhǔn)確，且狀態(tài)噪聲和觀測噪聲均為互不相關(guān)的零均值高斯白噪聲[6]，因此經(jīng)典卡爾曼濾波無法有效消除USBL 定位數(shù)據(jù)中的較大粗差?？共羁柭鼮V波作為經(jīng)典卡爾曼濾波的一種改進(jìn)方法，是通過調(diào)整觀測噪聲協(xié)方差陣提高濾波的抗差性，因而更適于消除實(shí)際動(dòng)態(tài)定位中的連續(xù)誤差。

此前，對于面向不同實(shí)際工程應(yīng)用的粗差數(shù)據(jù)處理，相關(guān)學(xué)者提出了多種抗差濾波方法。王潤英等[7]提出基于M 估計(jì)的抗差卡爾曼濾波算法，利用抗差估計(jì)方法識別粗差并修正其觀測方差，通過IGG3 等價(jià)權(quán)函數(shù)構(gòu)造等價(jià)權(quán)來減弱參數(shù)估計(jì)中異常值的作用，從而減弱變形監(jiān)測中粗差對參數(shù)解的影響。李雅梅等[8]針對移動(dòng)機(jī)器人定位的測量誤差和位姿誤差問題，引入多新息向量來提高非線性系統(tǒng)的精度，并利用Huber 權(quán)函數(shù)改變測量值的權(quán)重，引入自適應(yīng)因子調(diào)整狀態(tài)協(xié)方差矩陣大小，從而實(shí)現(xiàn)對測量更新估計(jì)值的精確估計(jì)。黃玨等[9]提出一種集中式非線性抗差卡爾曼濾波算法，通過設(shè)計(jì)代價(jià)函數(shù)重構(gòu)系統(tǒng)測量噪聲方差，并利用UKF 處理非線性觀測方程，解決了傳感器測量異常、數(shù)據(jù)傳輸錯(cuò)誤和融合數(shù)據(jù)異常的目標(biāo)跟蹤問題。彭月[10]以卡爾曼濾波中目標(biāo)位置參數(shù)的預(yù)測狀態(tài)不符值的標(biāo)準(zhǔn)化殘差作為抗差判斷量，自適應(yīng)地判斷當(dāng)前運(yùn)動(dòng)模型的準(zhǔn)確程度，解決了目標(biāo)導(dǎo)航定位中經(jīng)典卡爾曼濾波結(jié)果易受運(yùn)動(dòng)模型誤差影響的問題。Zhang Kai 等[11]為了解決反向散射數(shù)據(jù)異常而難以獲得K 分布參數(shù)穩(wěn)健估計(jì)解的問題，提出一種基于穩(wěn)健統(tǒng)計(jì)的K 分布參數(shù)估計(jì)算法，將DI 統(tǒng)計(jì)量替換為魯棒統(tǒng)計(jì)量，以無異常情況下的效率損失為代價(jià)，提高M(jìn)oM 方法的魯棒性；以增加計(jì)算時(shí)間為代價(jià)，自適應(yīng)裁剪異常值，提高計(jì)算效率。在存在異常值時(shí)，應(yīng)用魯棒統(tǒng)計(jì)和自適應(yīng)微調(diào)算法，可獲得更精確、穩(wěn)健的估計(jì)解。

盡管針對抗差卡爾曼濾波的理論研究日益深入，當(dāng)面向不同的實(shí)際工程應(yīng)用時(shí)，受測量誤差類型、觀測模型等影響，仍須結(jié)合實(shí)際情況對濾波參數(shù)進(jìn)行調(diào)整。本文主要面向抗差卡爾曼濾波在USBL 定位數(shù)據(jù)處理中的應(yīng)用，針對定位中的數(shù)據(jù)擾動(dòng)，選擇目前國際上經(jīng)典的Huber、IGG1、IGG3 等價(jià)權(quán)函數(shù)，通過調(diào)整更新后狀態(tài)向量估計(jì)值中觀測噪聲的協(xié)方差矩陣，重點(diǎn)對比分析不同等價(jià)權(quán)函數(shù)對USBL 定位數(shù)據(jù)的抗差濾波效果，為水下定位的實(shí)際應(yīng)用提供參考。

1 USBL 定位原理

USBL 定位系統(tǒng)是通過測量應(yīng)答器與聲基陣中各聲單元的相位差來確定應(yīng)答器到聲基陣的方位，并測量應(yīng)答器到聲基陣之間的距離來確定目標(biāo)位置的一種定位技術(shù)[12-13]。USBL 的聲基陣至少需要3 個(gè)聲單元，并構(gòu)成聲單元坐標(biāo)系，如圖1 所示。假設(shè)3 個(gè)聲單元位于2 個(gè)相互垂直的基線上（x 和y 軸上），各方向上基元間距（基線）均為d，應(yīng)答器發(fā)出的信號到基陣原點(diǎn)的聲線與x 軸及y 軸的夾角分別為θmx, θmy[14]。

圖1 USBL 定位示意圖

通過測量往返時(shí)間和聲速，計(jì)算得到斜距R為：

式中：c 為聲波在水中的速度；T 為往返時(shí)間。

根據(jù)空間幾何關(guān)系可得

式中：θm為聲線與聲單元坐標(biāo)系各坐標(biāo)軸之間的夾角；λ 為波長；為相位差。

兩聲單元接收信號的相位差與信號入射角的關(guān)系可表示為：

應(yīng)答器S 在聲單元坐標(biāo)系位置可以表示為：

結(jié)合公式（3）可進(jìn)一步表示為

2 抗差卡爾曼濾波的USBL 定位

在經(jīng)典卡爾曼濾波中，觀測噪聲通常假設(shè)為系統(tǒng)誤差、功率譜密度分布均勻的白噪聲以及隨機(jī)的高斯—馬爾可夫過程，且誤差表現(xiàn)出隨機(jī)性和獨(dú)立性。但在實(shí)際觀測過程中，觀測噪聲并不表現(xiàn)為隨機(jī)分布，其功率頻譜密度也并不一定為常數(shù)，數(shù)據(jù)中存在連續(xù)且大的粗差，在這些情況下，經(jīng)典的卡爾曼濾波不能夠很好的消除和調(diào)整連續(xù)粗差帶來的連續(xù)數(shù)據(jù)波動(dòng)問題，從而會(huì)導(dǎo)致濾波結(jié)果出現(xiàn)大的偏離。

當(dāng)數(shù)據(jù)中出現(xiàn)偏離較大的連續(xù)粗差時(shí)，抗差卡爾曼濾波可以通過調(diào)整觀測噪聲的協(xié)方差矩陣，利用觀測協(xié)方差矩陣代替原來的觀測噪聲協(xié)方差矩陣，來削弱粗差對濾波整體的影響。

對于超短基線定位的離散時(shí)間卡爾曼濾波，其狀態(tài)方程及觀測方程可以表示為

抗差卡爾曼濾波的估計(jì)準(zhǔn)則為：

式中：P 為觀測向量Zt的權(quán)矩陣，即觀測噪聲協(xié)方差矩陣的逆矩陣，取值6 階單位矩陣；Pt為預(yù)測狀態(tài)估計(jì)向量的權(quán)矩陣，即系統(tǒng)噪聲協(xié)方差矩陣的逆矩陣，取值系數(shù)為10的6階單位矩陣。

對上式求極值得到[16]：

觀測噪聲協(xié)方差矩陣等價(jià)替換后得到的等價(jià)卡爾曼增益矩陣為

即狀態(tài)更新方程可以等價(jià)替換為：

最后，更新系統(tǒng)噪聲協(xié)方差矩陣為：

式中：Pt為更新后的系統(tǒng)噪聲協(xié)方差矩陣；I為單位矩陣。

3 抗差卡爾曼濾波的USBL 定位

抗差卡爾曼對于數(shù)據(jù)的濾波處理重點(diǎn)在于對等價(jià)權(quán)的選擇使用上，不同的等價(jià)權(quán)函數(shù)會(huì)產(chǎn)生不同的等價(jià)觀測協(xié)方差矩陣，對于粗差的濾波效果也不同?？紤]到抗差卡爾曼濾波能夠滿足對USBL 數(shù)據(jù)的抗差性和有效性的要求，選取目前實(shí)際應(yīng)用中常見的Huber、IGG1、IGG3 三種等價(jià)權(quán)函數(shù)，它們的共同特點(diǎn)是利用觀測值殘差與殘差的標(biāo)準(zhǔn)差的關(guān)系，求取標(biāo)準(zhǔn)化殘差，并以經(jīng)驗(yàn)獲得的臨界值界定觀測值權(quán)值。對三種等價(jià)權(quán)函數(shù)的介紹如下：

（1）Huber 權(quán)函數(shù)

Huber 權(quán)函數(shù)劃分為兩段，包含正常域和可疑域，正常域的觀測值仍采用原始權(quán)，其它的觀測值則一律降權(quán)，顯然該權(quán)函數(shù)缺少淘汰段，可能會(huì)減弱估值的抗差能力。

式中：v 為標(biāo)準(zhǔn)化殘差[6]；k0取值1.0～2.0。

（2）IGG 系列權(quán)函數(shù)

IGG 系列權(quán)函數(shù)[17]主要基于測量誤差的有界性提出的，選取標(biāo)準(zhǔn)化殘差作為誤差判別統(tǒng)計(jì)量[18]將其分為正常段、可疑段和淘汰段，分別對處于三種情況的觀測值進(jìn)行維持正常值、降權(quán)處理和淘汰處理。當(dāng)觀測值處于粗差較多的情況下，該方法較為有效[7]。

1）IGG1 權(quán)函數(shù)

式中：k0一般取1.0～1.5；k1一般取3.0～4.5[18]。

可以看出，IGG1 權(quán)函數(shù)沿用了Huber 權(quán)函數(shù)在可疑域的降權(quán)方法，只是增加了淘汰域。

2）IGG3 權(quán)函數(shù)

式中：k0，k1取值同上。

可見，IGG3 權(quán)函數(shù)是在IGG1 權(quán)函數(shù)的基礎(chǔ)上改進(jìn)了降權(quán)方法。

4 實(shí)測實(shí)驗(yàn)與分析

超短基線實(shí)際測量過程中，由于受測量環(huán)境、信號反射等因素影響，經(jīng)常會(huì)出現(xiàn)較大粗差，對于測量數(shù)據(jù)整體精度會(huì)有很大影響。為了對比分析不同等價(jià)權(quán)函數(shù)對USBL 定位實(shí)測數(shù)據(jù)的粗差濾波效果，采用在中國南海為水下Swif 01 型ROV定位的USBL 實(shí)測數(shù)據(jù)作為實(shí)驗(yàn)數(shù)據(jù)，實(shí)驗(yàn)設(shè)備主要包括英國Sonardyne 公司的Ranger 2 USBL 定位系統(tǒng)、Syrinx 多普勒速度計(jì)程儀、英國SubSea7公司的Veripos LD4 接收機(jī)、法國Ixsea 公司的Octans 3000 型光纖羅經(jīng)及運(yùn)動(dòng)傳感器等，實(shí)驗(yàn)設(shè)備安裝工作流程見圖2。其中，測量船位置、超短基線換能器位置由星站差分GPS 接收機(jī)提供，利用USBL 測量的位置數(shù)據(jù)結(jié)合DVL、光纖羅經(jīng)確定的速度信息，來精確推算水下ROV 的準(zhǔn)確位置。

選取抗差卡爾曼濾波中的Huber 函數(shù)、IGG系列權(quán)函數(shù)作為求取等價(jià)權(quán)的方法，分析這幾種等價(jià)權(quán)函數(shù)的抗差性，并與經(jīng)典卡爾曼濾波進(jìn)行對比，觀察其抗差效果。圖3～圖5 分別為X,Y,Z方向各濾波方法的結(jié)果對比。

圖2 實(shí)驗(yàn)設(shè)備工作流程

圖3 X 方向的濾波結(jié)果對比

圖4 Y 方向的濾波結(jié)果對比

圖5 Z 方向的濾波結(jié)果對比

通過圖3～圖5 各濾波方法結(jié)果對比圖可以發(fā)現(xiàn)，實(shí)際測量中的某些較大粗差，經(jīng)典卡爾曼濾波可以削弱，但不能有效剔除；對于抗差Huber 方法，其削弱程度略遜于抗差I(lǐng)GG 系列方法。由于實(shí)測數(shù)據(jù)僅能對各種等價(jià)權(quán)函數(shù)的抗差濾波效果進(jìn)行定性分析，因此，設(shè)計(jì)仿真實(shí)驗(yàn)展開定量分析。

5 仿真實(shí)驗(yàn)與分析

將經(jīng)過精細(xì)處理后的USBL 實(shí)測數(shù)據(jù)作為標(biāo)準(zhǔn)值，在此基礎(chǔ)上，隨機(jī)增加121 個(gè)異常值模擬粗差，其中根據(jù)實(shí)際工程情況模擬了一段由30 個(gè)異常值組成的連續(xù)粗差。模擬粗差的位置和大小見圖6。圖7～圖9 分別為X,Y,Z方向上各方法的濾波結(jié)果對比曲線。

圖6 原始及模擬定位數(shù)據(jù)

圖7 X 方向的濾波結(jié)果

圖8 Y 方向的濾波結(jié)果

圖9 Z 方向的濾波結(jié)果

為衡量濾波值與USBL 實(shí)測數(shù)據(jù)標(biāo)準(zhǔn)值之間的偏差，統(tǒng)計(jì)了各方法在不同方向上的RMSE結(jié)果，見表1。

根據(jù)上述各個(gè)方法計(jì)算得到的粗差結(jié)果，以0.5 m 作為判別X,Y,Z方向粗差剔除的閾值，并計(jì)算各個(gè)方法對X,Y,Z三個(gè)方向的粗差剔除率，各方法的粗差剔除率，見表2。

對實(shí)驗(yàn)結(jié)果分析可知：

（1）由圖7 ～圖9 可見，經(jīng)典卡爾曼濾波方法只能夠?qū)崿F(xiàn)小部分的粗差剔除，對較大粗差處的濾波不徹底，連續(xù)粗差處的濾波結(jié)果較濾波前變化不大；抗差Huber 方法對于隨機(jī)不連續(xù)粗差的剔除效果明顯優(yōu)于經(jīng)典卡爾曼濾波，但對于連續(xù)粗差沒有較大改善，且總體精度明顯低于抗差I(lǐng)GG 系列方法；抗差I(lǐng)GG 系列方法明顯消除了不連續(xù)粗差和連續(xù)粗差對USBL數(shù)據(jù)的影響，且兩者效果相似。

表1 各方法濾波RMSE 結(jié)果

表2 各方法粗差剔除率

（2）由表1 分析，三種抗差卡爾曼濾波方法較真值的偏差明顯小于經(jīng)典卡爾曼濾波方法，采用經(jīng)典卡爾曼濾波處理后的USBL 數(shù)據(jù)仍存在較大粗差?？共頗uber 方法在各方向上的RMSE 值總體有所改善但仍偏高，相比之下，抗差I(lǐng)GG 系列方法優(yōu)于抗差Huber 方法，RMSE 值均可控制在0.3 m 以內(nèi)。

（3）由表2 分析，經(jīng)典卡爾曼濾波對各方向誤差剔除效果較差，對粗差的改善效果不理想；結(jié)合圖7 ～圖9 可知，抗差Huber 方法在各方向上僅能剔除不到一半的粗差，且大部分未被剔除的粗差存在于連續(xù)粗差段處；相比前兩種方法，將誤差分為三段式處理的抗差I(lǐng)GG 系列在各方向上的粗差剔除率較高，均超過80%，總體剔除效果明顯。

6 結(jié) 論

本文主要利用抗差卡爾曼濾波對USBL 定位數(shù)據(jù)進(jìn)行濾波處理，對比Huber、IGG1 和IGG3等價(jià)權(quán)函數(shù)的抗差卡爾曼濾波結(jié)果，并分析經(jīng)典卡爾曼對實(shí)測數(shù)據(jù)中的連續(xù)粗差的剔除效果。相比經(jīng)典卡爾曼濾波、Huber 等價(jià)權(quán)函數(shù)的抗差卡爾曼濾波方法，利用IGG 系列權(quán)函數(shù)將誤差分為3 段，分別采取不同抗差處理的方法，對USBL數(shù)據(jù)中的離散及連續(xù)粗差剔除效果較好，在一定剔除閾值下，該方法能夠?qū)崿F(xiàn)各方向80%以上的粗差剔除率，對實(shí)際USBL 定位數(shù)據(jù)的擬合處理具有一定參考價(jià)值。