IXSea Gaps超短基線定位數(shù)據(jù)卡爾曼濾波研究

王崇明，田春和，隋海琛

（交通部天津水運工程科學(xué)研究所，天津300456）

法國IXSea公司研制的新型超短基線（USBL）定位系統(tǒng)Gaps（Global Acoustic Positioning System）是世界上首個便攜式、即插即用、無需坐標校準的超短基線定位系統(tǒng)，近年來引起了廣泛的關(guān)注。其最大的優(yōu)勢是將GPS定位系統(tǒng)、慣性導(dǎo)航系統(tǒng)、光纖羅經(jīng)/姿態(tài)儀系統(tǒng)同水聲定位系統(tǒng)的傳感器組裝在一起，各傳感器之間的相對偏移量在出廠前已進行內(nèi)部標定并固化在系統(tǒng)的內(nèi)部程序中，因此系統(tǒng)在現(xiàn)場無需標定，不存在各傳感器之間的安裝、測量誤差問題，確保了系統(tǒng)的高精度導(dǎo)航定位。Gaps采用寬頻發(fā)射信號，在增大作用距離的基礎(chǔ)上，提高了定位精度。最大有效距離為4 000 m，定位精度達到斜距的0.2%，角度精度為0.12°，距離精度最高可達0.2 m，能最大限度地滿足近距離水下定位及導(dǎo)航的要求［1-2］。除此之外，系統(tǒng)可以同時追蹤多個水下目標［3］。在海洋工作環(huán)境中，由于噪聲影響產(chǎn)生的USBL定位誤差甚至粗差是不可避免的。Gaps也存在同樣問題，由于信標的運動軌跡是有章可循的，不會產(chǎn)生較大的突起，運動軌跡相對平滑，這樣就可以應(yīng)用卡爾曼濾波剔除錯誤點。本次水下超短基線定位即使用IXSea Gaps系統(tǒng)進行數(shù)據(jù)采集。

1 卡爾曼濾波的基本原理

卡爾曼濾波是一種最優(yōu)化自回歸數(shù)據(jù)處理算法，它通過不斷預(yù)測、修正遞推過程，可隨時計算最新的濾波值，便于實時處理觀測數(shù)據(jù)，提供一種最優(yōu)估計值。

經(jīng)典卡爾曼濾波應(yīng)用的一個先決條件是建立準確的動態(tài)模型和觀測模型，這就要求較清楚地了解物體的運動。偏離理想假設(shè)的觀測向量或動力學(xué)模型必然會給動態(tài)定位結(jié)果帶來偏差，甚至導(dǎo)致卡爾曼濾波發(fā)散［4］。

本次動態(tài)定位試驗采用定常速度模型，狀態(tài)方程和測量方程都是線性的，狀態(tài)向量X（k）選取坐標和速度，加速度為狀態(tài)噪聲W（k）

式中：x（k），y（k）為 k 時刻信標的坐標；vx（k）、vy（k）為 k 時刻信標的速度；ax（k）、ay（k）為 k 時刻信標的加速度。

設(shè)狀態(tài)方程和測量方程都是線性的，則可以建立如下方程

狀態(tài)方程

觀測方程

式中：Φ（k，k-1）為狀態(tài)轉(zhuǎn)移矩陣；Γ（k-1）為動態(tài)噪聲干擾矩陣；H為測量矩陣；P（k）為觀測值矩陣；V為測量誤差。

根據(jù)測量誤差理論及限差要求，設(shè)此線性系統(tǒng)滿足下列條件

在此次超短基線定位試驗中，分別選取

式中：t（k）為數(shù)據(jù)采樣時間間隔。

信標的運動軌跡相對平滑，且每一時刻的位置具有唯一性，因此可以對其進行卡爾曼濾波。根據(jù)最小方差無偏估計原理，導(dǎo)出卡爾曼數(shù)據(jù)濾波的遞推方程如下。

一步預(yù)測值

預(yù)報誤差方差陣

濾波增益矩陣

濾波值

濾波誤差陣

式中：I為單位陣。

首先需要確定狀態(tài)和觀測噪聲的初始方差，即Dw（k，k-1）和Dv，可根據(jù)經(jīng)驗選取。若信標隨時間變化緩慢，則Dw（k，k-1）取值較小，表明模型變化比較緩慢［5］，這樣得到的濾波值結(jié)果就比較平坦；若信標運動速度變化較大，則Dw（k，k-1）取值相應(yīng)要變大。Dv的取值應(yīng)與測量值的方差相對應(yīng)，當測量值有較大的方差時，Dv的取值相應(yīng)較大。初始速度可以由前2個觀測值來確定，狀態(tài)噪聲和觀測噪聲的初始方差分別取0.005和1.5。當濾波對象發(fā)生變化時，模型參數(shù)也應(yīng)該進行相應(yīng)的調(diào)整，以達到最佳的濾波效果。

2 卡爾曼濾波的特點

卡爾曼濾波方程是一組遞推計算公式，其計算過程是一個不斷預(yù)測、修正的過程。求解時不需要貯存大量的觀測數(shù)據(jù)，且當?shù)玫叫碌挠^測數(shù)據(jù)時，可隨時算得新的濾波值，便于實時處理觀測成果。

卡爾曼濾波還具有以下特點：增益矩陣與觀測值無關(guān)，因此可以預(yù)先算出，從而減少實時處理的計算工作量；增益矩陣與Dv成反比，與Dw成正比；卡爾曼濾波方程是根據(jù)廣義最小二乘原理導(dǎo)出的，而按廣義最小二乘原理得到的結(jié)果具有無偏性和方差最小性，因此卡爾曼濾波值是由觀測值得到的最小方差無偏估計，其誤差方差陣就是所有估計中的最小方差陣［6］。

3 野值的判別

測量中的誤差是不可避免的，在海洋測繪中海洋噪聲、工作環(huán)境以及儀器設(shè)備安裝等均會使測量數(shù)據(jù)產(chǎn)生較大誤差，即野值。在進行數(shù)據(jù)處理時必須將其刪除，否則會嚴重影響精度，甚至導(dǎo)致濾波器發(fā)散。

在卡爾曼濾波器中，殘差 Δ（k）=P（k）-X（k/k-1），它是一個零均值的高斯隨機量，其協(xié)方差陣為 S（k），利用殘差的統(tǒng)計特性，采取強約束的方法，可以得到如下野值判定準則

若式（8）成立，則認為觀測值P（k）正確，否則視觀測值為野值，當出現(xiàn)野值時，不考慮實際測量值，將觀測預(yù)報值作為最終結(jié)果。

4 定位實驗與數(shù)據(jù)處理

在此次定位試驗中，將信標放置于運動的船上，Gaps安置在一固定位置進行數(shù)據(jù)采集。根據(jù)上述原理，編寫程序?qū)崿F(xiàn)了適用于超短基線定位數(shù)據(jù)的卡爾曼濾波。

原始觀測數(shù)據(jù)是帶有隨機誤差的數(shù)據(jù)，并且有些數(shù)據(jù)還帶有較大誤差，即所謂的野值，這會嚴重損害測量結(jié)果的精度（圖1）。

圖2為經(jīng)卡爾曼濾波后的數(shù)據(jù)，可以看到濾波數(shù)據(jù)較原始數(shù)據(jù)更平滑。圖3為濾波數(shù)據(jù)與原始數(shù)據(jù)的比較結(jié)果，可以看出兩者較吻合。這說明卡爾曼濾波取得了良好的效果，明顯改善了超短基線定位的結(jié)果。

5 結(jié)論

由于受到各種噪聲的影響，用IXSea Gaps超短基線進行定位所得的觀測數(shù)據(jù)中不可避免的帶有一些誤差。本文編程實現(xiàn)了超短基線定位的卡爾曼濾波，通過對原始觀測數(shù)據(jù)進行卡爾曼濾波，得到了平滑且與原始觀測數(shù)據(jù)較吻合的結(jié)果，說明卡爾曼濾波可以明顯地改善超短基線的定位結(jié)果。

［1］李守軍，包更生，吳水根.水聲定位技術(shù)的發(fā)展現(xiàn)狀與展望［J］.海洋技術(shù)，2005，24（1）：130-135.LI S J，BAO G S，WU S G.A Practical Overview and Prospect of Acoustic Positioning Technology［J］.Ocean Technology，2005，24（1）：130-135.

［2］吳永亭，周興華，楊龍.水下聲學(xué)定位系統(tǒng)及其應(yīng)用［J］.海洋測繪，2003，23（4）：18-21.WU Y T，ZHOU X H，YANG L.Underwater Acoustic Positioning System and Its Application［J］.Hydrographic Surveying and Charting，2003，23（4）：18-21.

［3］楊鯤，吳永亭，趙鐵虎，等.海洋調(diào)查技術(shù)及應(yīng)用［M］.武漢：武漢大學(xué)出版社，2009.

［4］宋迎春.動態(tài)定位中的卡爾曼濾波研究［D］.長沙：中南大學(xué)，2006.

［5］郭紀捷.水下拖體聲學(xué)超短基線定位測量及其卡爾曼濾波技術(shù)［J］.海洋技術(shù)，2002，21（1）：7-11.GUO J J.Location of a Tow body by Ultra-short Base Line Underwater Acoustic Positioning and Kalman Filtering［J］.Ocean Technology，2002，21（1）：7-11.

［6］崔希璋，於宗儔，陶本藻，等.廣義測量平差［M］.武漢：武漢大學(xué)出版社，2001.