抗差卡爾曼濾波及其在超短基線水下定位中的應(yīng)用

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

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

水下目標(biāo)定位導(dǎo)航是海洋開發(fā)活動與海洋技術(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)的方位和距離，實現(xiàn)對目標(biāo)的跟蹤定位[4]。相較于其他幾種水下定位方法，USBL 定位系統(tǒng)由于只需要一個換能器，因此其集成系統(tǒng)價格更低廉，且有構(gòu)成簡單、操作維護方便的優(yōu)點[5]。

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

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

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

1 USBL 定位原理

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

圖1 USBL 定位示意圖

通過測量往返時間和聲速，計算得到斜距R為：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

式中：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)計量[18]將其分為正常段、可疑段和淘汰段，分別對處于三種情況的觀測值進行維持正常值、降權(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ǔ)上改進了降權(quán)方法。

4 實測實驗與分析

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

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

圖2 實驗設(shè)備工作流程

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

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

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

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

5 仿真實驗與分析

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

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

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

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

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

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

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

對實驗結(jié)果分析可知：

（1）由圖7 ～圖9 可見，經(jīng)典卡爾曼濾波方法只能夠?qū)崿F(xiàn)小部分的粗差剔除，對較大粗差處的濾波不徹底，連續(xù)粗差處的濾波結(jié)果較濾波前變化不大；抗差Huber 方法對于隨機不連續(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ù)進行濾波處理，對比Huber、IGG1 和IGG3等價權(quán)函數(shù)的抗差卡爾曼濾波結(jié)果，并分析經(jīng)典卡爾曼對實測數(shù)據(jù)中的連續(xù)粗差的剔除效果。相比經(jīng)典卡爾曼濾波、Huber 等價權(quán)函數(shù)的抗差卡爾曼濾波方法，利用IGG 系列權(quán)函數(shù)將誤差分為3 段，分別采取不同抗差處理的方法，對USBL數(shù)據(jù)中的離散及連續(xù)粗差剔除效果較好，在一定剔除閾值下，該方法能夠?qū)崿F(xiàn)各方向80%以上的粗差剔除率，對實際USBL 定位數(shù)據(jù)的擬合處理具有一定參考價值。