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

羅才智，楊 鯤，辛明真，衛進進，張 凱，陽凡林*

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

水下目標定位導航是海洋開發活動與海洋技術發展的基礎，水下聲學定位系統包括長基線（Long Baseline，LBL）定位系統、短基線（Short Baseline，SBL）定位系統、超短基線（Ultra-Short Baseline，USBL）定位系統和GNSS智能浮標（GNSS Intelligent Buoys，GIB）系統[1-3]。其中，USBL 定位系統利用各基元信號間的相位差來解算目標的方位和距離，實現對目標的跟蹤定位[4]。相較于其他幾種水下定位方法，USBL 定位系統由于只需要一個換能器，因此其集成系統價格更低廉，且有構成簡單、操作維護方便的優點[5]。

受海洋環境動態變化、測量儀器安裝校準偏差、外圍設備測量可靠性、聲速測量與改正精度等因素的影響，USBL 定位會出現較大粗差和連續異常誤差。經典卡爾曼濾波的前提假設是濾波的狀態模型和觀測模型準確，且狀態噪聲和觀測噪聲均為互不相關的零均值高斯白噪聲[6]，因此經典卡爾曼濾波無法有效消除USBL 定位數據中的較大粗差。抗差卡爾曼濾波作為經典卡爾曼濾波的一種改進方法，是通過調整觀測噪聲協方差陣提高濾波的抗差性，因而更適于消除實際動態定位中的連續誤差。

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

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

1 USBL 定位原理

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

圖1 USBL 定位示意圖

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

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

根據空間幾何關系可得

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

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

應答器S 在聲單元坐標系位置可以表示為：

結合公式（3）可進一步表示為

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

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

當數據中出現偏離較大的連續粗差時，抗差卡爾曼濾波可以通過調整觀測噪聲的協方差矩陣，利用觀測協方差矩陣代替原來的觀測噪聲協方差矩陣，來削弱粗差對濾波整體的影響。

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

抗差卡爾曼濾波的估計準則為：

式中：P 為觀測向量Zt的權矩陣，即觀測噪聲協方差矩陣的逆矩陣，取值6 階單位矩陣；Pt為預測狀態估計向量的權矩陣，即系統噪聲協方差矩陣的逆矩陣，取值系數為10的6階單位矩陣。

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

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

即狀態更新方程可以等價替換為：

最后，更新系統噪聲協方差矩陣為：

式中：Pt為更新后的系統噪聲協方差矩陣；I為單位矩陣。

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

抗差卡爾曼對于數據的濾波處理重點在于對等價權的選擇使用上，不同的等價權函數會產生不同的等價觀測協方差矩陣，對于粗差的濾波效果也不同。考慮到抗差卡爾曼濾波能夠滿足對USBL 數據的抗差性和有效性的要求，選取目前實際應用中常見的Huber、IGG1、IGG3 三種等價權函數，它們的共同特點是利用觀測值殘差與殘差的標準差的關系，求取標準化殘差，并以經驗獲得的臨界值界定觀測值權值。對三種等價權函數的介紹如下：

（1）Huber 權函數

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

式中：v 為標準化殘差[6]；k0取值1.0～2.0。

（2）IGG 系列權函數

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

1）IGG1 權函數

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

可以看出，IGG1 權函數沿用了Huber 權函數在可疑域的降權方法，只是增加了淘汰域。

2）IGG3 權函數

式中：k0，k1取值同上。

可見，IGG3 權函數是在IGG1 權函數的基礎上改進了降權方法。

4 實測實驗與分析

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

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

圖2 實驗設備工作流程

圖3 X 方向的濾波結果對比

圖4 Y 方向的濾波結果對比

圖5 Z 方向的濾波結果對比

通過圖3～圖5 各濾波方法結果對比圖可以發現，實際測量中的某些較大粗差，經典卡爾曼濾波可以削弱，但不能有效剔除；對于抗差Huber 方法，其削弱程度略遜于抗差IGG 系列方法。由于實測數據僅能對各種等價權函數的抗差濾波效果進行定性分析，因此，設計仿真實驗展開定量分析。

5 仿真實驗與分析

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

圖6 原始及模擬定位數據

圖7 X 方向的濾波結果

圖8 Y 方向的濾波結果

圖9 Z 方向的濾波結果

為衡量濾波值與USBL 實測數據標準值之間的偏差，統計了各方法在不同方向上的RMSE結果，見表1。

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

對實驗結果分析可知：

（1）由圖7 ～圖9 可見，經典卡爾曼濾波方法只能夠實現小部分的粗差剔除，對較大粗差處的濾波不徹底，連續粗差處的濾波結果較濾波前變化不大；抗差Huber 方法對于隨機不連續粗差的剔除效果明顯優于經典卡爾曼濾波，但對于連續粗差沒有較大改善，且總體精度明顯低于抗差IGG 系列方法；抗差IGG 系列方法明顯消除了不連續粗差和連續粗差對USBL數據的影響，且兩者效果相似。

表1 各方法濾波RMSE 結果

表2 各方法粗差剔除率

（2）由表1 分析，三種抗差卡爾曼濾波方法較真值的偏差明顯小于經典卡爾曼濾波方法，采用經典卡爾曼濾波處理后的USBL 數據仍存在較大粗差。抗差Huber 方法在各方向上的RMSE 值總體有所改善但仍偏高，相比之下，抗差IGG 系列方法優于抗差Huber 方法，RMSE 值均可控制在0.3 m 以內。

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

6 結 論

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