一種聲速-位置聯(lián)合估計的水聲導航定位方法

李瑨瑤，王海斌，汪 俊，徐 鵬，陳 曦

(1.中國科學院聲學研究所，北京100190；2.中國科學院大學，北京100190)

0 引 言

水下航行器是一種可以憑借自身的動力系統(tǒng)和導航系統(tǒng)來完成水下工作任務的集成系統(tǒng)[1]，在軍事應用和民用方面均有廣泛的需求。軍事方面的應用包括：水下探測[2]、水下巡航以及水下中繼通信等[3]；民用方面包括：海洋資源開發(fā)、海洋環(huán)境監(jiān)測以及海洋救險與打撈等。無論軍用還是民用需求，一般都需要已知水下航行器的位置狀態(tài)，故獲取水下航行器的高精度位置信息成為需要解決的關鍵問題。目前水下航行器導航技術主要包括：航位推算與慣性導航、地球物理屬性導航、聲學導航。

航位推算與慣性導航方法分別利用速度傳感器及加速度傳感器得到水下航行器的速度及加速度，并利用其速度或加速度進行時間積分，從而推算出水下航行器的位置。但這種方法極易受到水流速度的影響，造成結果不準確。地球物理屬性導航利用水下磁場強度特征、重力場特征、地形環(huán)境圖等數據進行匹配定位，但需進行大規(guī)模的復雜調研及測繪工作，故至今實際應用較少。聲學導航利用聲音信號在水下傳播衰減較小、傳播距離較遠的特點，將水面或水下已知位置的信標作為參考位置，通過測量信標發(fā)射的脈沖信號到達水下航行器的時間，計算信標到水下航行器的距離，從而得到水下航行器的位置信息[4-7]。聲學導航主要包括超短基線導航[8]、短基線導航[9]和長基線導航等方式[10-12]。水下航行器的聲學導航定位誤差主要由水下等效聲速的測量誤差產生[13-16]：當水下航行器與信標位置距離較遠時，由于信道傳輸環(huán)境復雜，準確估計等效聲速較為困難，產生一定的距離估計誤差，影響估計精度。為解決這一問題，本文引入粒子濾波算法將等效聲速作為待估計量進行狀態(tài)跟蹤。粒子濾波算法是一種基于序貫重要性采樣的序貫蒙特卡洛方法，不受系統(tǒng)線性化誤差和高斯假設的限制，常用于解決狀態(tài)跟蹤問題。經過改進的粒子濾波算法，通過隨機產生大量粒子的方式對目標狀態(tài)進行模擬，根據觀測信息判斷粒子的擬合程度并賦予粒子相應權值，可以更高效地在先驗信息未知的情況下對目標進行狀態(tài)跟蹤。針對聲學導航定位由于等效聲速估計誤差帶來的定位誤差較大的問題，本文提出了一種基于粒子濾波的水聲導航定位方法，將等效聲速作為未知的估計參量加入狀態(tài)方程中，建立以水下航行器的位置及各信標到水下航行器的等效聲速為核心估計參量的粒子濾波算法模型，對水下航行器的位置進行估計。對上述方法的性能進行了仿真分析，并與常規(guī)長基線導航定位方法進行了性能對比。

1 問題描述及解決思路

常規(guī)的長基線水聲導航定位利用信標發(fā)送的脈沖信號到水下航行器的傳播時間作為觀測量信息，將估計或測量得到的等效聲速作為已知的參數，將兩者相乘計算出各個信標與水下航行器的距離，在已知信標位置的前提下，利用距離的幾何關系，計算出水下航行器的位置信息，其框圖如圖1所示。但無論是工程實測還是理論計算，由于水聲信道的復雜性，將得到的等效聲速作為已知參量都存在誤差。這一誤差會導致水下航行器的定位誤差隨著導航距離的增加而變大，因此，如何高精度地獲取等效聲速成為提高水聲導航定位精度的關鍵問題。

圖1 傳統(tǒng)水聲導航定位方法框圖Fig.1 Traditional underwater acoustic positioning method

針對這一問題，本文提出了一種基于粒子濾波的水聲導航定位方法(如圖2所示)，將等效聲速和水下航行器的位置作為未知的狀態(tài)估計參量，結合觀測期間航行器自身的速度分量，組成運動狀態(tài)方程。將各個信標信號的傳播時間作為測量值，后驗概率密度函數會在較為準確的等效聲速和水下航行器的位置處出現(xiàn)峰值。建立粒子濾波模型，利用估計得到的后驗概率密度峰值獲得水下航行器的位置及等效聲速的最優(yōu)解，從而得到每個時刻水下航行器的位置及等效聲速的估計值。將粗估的等效聲速值作為粒子濾波算法中等效聲速估計的初始值，將等效聲速作為未知狀態(tài)參量，通過狀態(tài)跟蹤，不斷修正等效聲速的估計誤差，從而獲得更高精度的定位。

圖2 本文所提基于粒子濾波的水聲導航定位方法框圖Fig.2 The method of underwater acoustic navigation and positioning based on particle filter proposed in this paper

2 基于粒子濾波水聲導航定位方法

2.1 粒子濾波基本原理

式中：δ為單位沖激函數，為k時刻每個粒子對應的權重，并且。但目標概率密度函數p(X)非常復雜，無法直接產生符合概率密度函數的粒子，因此利用重要性采樣(Importance Sampling)方法，設是較之容易實現(xiàn)采樣的概率分布密度函數，稱為重要性函數，若重要性密度只取決于Xk-1和Zk，定義重要性權重wk為

但粒子濾波存在退化問題，因此，需要利用重采樣來解決粒子退化問題。重采樣的步驟包括對的離散近似表達進行重采樣N次后得到一個新的集合：

2.2 基于粒子濾波算法的水聲導航定位模型

信標數目設置為m，在導航過程中每個信標的位置坐標不變，為(xos,yos)(s=1,???,m,下同)，航行器在k時刻的位置為(xk,yk)。k時刻第s個信標到航行器的等效聲速為csk，從第k時刻到k+1時刻、第s個信標到航行器的等效聲速改變量為。k時刻第s個信標到航行器信號的傳播時間為tsk，其測量誤差分別為nsk，并假設nsk滿足零均值高斯分布。通過航行器慣導系統(tǒng)得到k時刻航行器的速度(vxk,vyk)(即從k時刻到k+1時刻航行器位置的變化量)，但由于速度測量存在誤差，假設航行器速度測量誤差服從零均值高斯分布。

k時刻待估計的狀態(tài)參量為

首先隨機產生M個粒子，每個粒子包含的預估狀態(tài)參量，對每個粒子根據觀測方程給出粒子對應的權值，采樣-重要性重采樣(Sampling-Importance Resampling,SIR)濾波器將重要性函數選擇為先驗函數，代入式(2)中可以得到重要性權值為

該濾波器每個時刻都進行重采樣，因而：

重要性權值在重采樣步驟前需要進行歸一化，之后的每個時刻都進行重采樣操作。因此粒子的權值可以表示為

對每個粒子加權平均估計當前時刻目標的運動狀態(tài)，并進行對當前時刻粒子重采樣，得到重采樣后的粒子。在下一時刻，將重采樣后的粒子代入粒子的狀態(tài)方程中：

式中，A為根據目標運動狀態(tài)得到的逆推矩陣。由式(16)可得到下一時刻的粒子并重新為粒子分配新的權值，繼而得到下一時刻估計得到的目標運動狀態(tài)。

2.3 基于粒子濾波算法的水聲導航定位方法

算法輸入：每個信標的發(fā)射信號到航行器的傳播時間；由航行器慣性導航得到的航行器每個時刻的運動速度。

(1) 在每個時刻隨機產生大量粒子模擬的航行器位置以及各個信標信號傳播到航行器的等效聲速，對應得到這一時刻每個粒子的權值；

(2) 對每個時刻的粒子加權估計航行器位置及各個等效聲速；

(3) 進行重采樣處理重新得到粒子并代入狀態(tài)方程中獲取下一時刻的粒子；

(4) 循環(huán)此過程得到每個時刻狀態(tài)估計值。

算法輸出：每個時刻航行器位置分量、各個標信號傳播到航行器的等效聲速估計值。

基于粒子濾波的導航定位算法流程圖如圖3所示。

圖3 基于粒子濾波的導航定位算法流程圖Fig.3 Flow chart of navigation and positioning algorithm based on particle filter

3 仿真分析

取正北方向為0°，設置3個導航信標B1、B2和B3。三者構成一個邊長r=60 km的等邊三角形，B1位于坐標原點(0,0)處，B2位于坐標(r,0)處，B3位于坐標處。航行器初始位置位于S點，坐標為(3r/4,r/5)，做勻速直線運動，速度為2.5 m·s-1，航向為75°。在實際應用中，航行器即使做勻速直線運動也會受到水流影響，假設水流速度在x和y方向都存在擾動，且其速度擾動均滿足均值為0、標準差為0.71 m·s-1的高斯分布。k時刻與k+1時刻的時間間隔為120 s，時間采樣次數為100次。假設3個信標的信號同時收到。信標信號傳播時間的測量誤差均服從均值為0、標準差為31.6 ms的高斯分布。3個信標到航行器的等效聲速真值分別為c1=1 510 m·s-1、c2=1 519 m·s-1，c3=1 503 m·s-1(聲速真值在后續(xù)處理中未知)，且由于航行器位置逐漸變化，等效聲速緩慢變化，假設其變化量服從均值為0、標準差為0.1 m·s-1的高斯分布，等效聲速的變化是為了模擬實際應用中由于環(huán)境變化以及航行器運動導致的等效聲速改變。粒子濾波算法假設航行器初始位置未知，設定為距離航行器真實位置較遠的坐標(r,5r/2)，設置等效聲速初始估計值分別為c1=1 502 m·s-1、c2=1 521 m·s-1、c3=1 496 m·s-1，與常規(guī)幾何算法的等效聲速估計初始值大小相同，標準差均取40 m·s-1；利用粒子濾波算法估計航行器位置須給出與其真實位置誤差較大的初始位置，將航行器位置的初始值設置為(r,r/4)，x軸與y軸坐標的標準差均取20 000 m。其中，c1和c3兩個等效聲速初始估計值誤差較大，在航行器位置完全未知以及等效聲速誤差較大的情況下進行水聲導航定位。常規(guī)導航算法利用圓相交的幾何關系，等效聲速取初始估計值對航行器定位，并將粒子濾波算法與常規(guī)方法的定位結果及誤差進行對比。

圖4為水下航行器與3個信標每時刻的真實位置。圖5分別為3個信標信號到水下航行器的等效聲速真值。圖6、7分別為水下航行器的真實速度及真實航向角。圖8、9分別為常規(guī)方法和基于粒子濾波對航行器進行導航定位的軌跡估計及誤差對比。圖10為粒子濾波算法對各信標的等效聲速估計與等效聲速真值的對比。

圖4 水下航行器、信標位置及運動軌跡Fig.4 Position and trajectory of underwater vehicle and beacon

圖5 各信標到水下航行器等效聲速真值Fig.5 The true value of equivalent sound velocity from each beacon to the underwater vehicle

圖6 水下航行器速度真值Fig.6 The true value of underwater vehicle speed

圖7 水下航行器真實航向角Fig.7 The true value of underwater vehicle true heading angle

圖8 常規(guī)算法及粒子濾波算法時水下航行器軌跡估計結果Fig.8 Trajectory estimation using conventional algorithm and particle filter algorithm

圖9 常規(guī)算法及粒子濾波算法誤差對比Fig.9 Error contrast of conventional algorithm and particle filter algorithm

圖10 粒子濾波算法對各信標的等效聲速估計Fig.10 Estimation of equivalent sound velocity for each beacon using particle filter algorithm

由圖8可得：粒子濾波算法估計的軌跡與真實軌跡更加吻合。由圖9可知：常規(guī)方法在采樣時間的末尾時刻誤差大于初始值產生的誤差，定位誤差由200 m增加至400 m以上，而粒子濾波算法將定位誤差控制在100 m左右，遠小于初始定位誤差，對水下航行器的定位更加準確。由圖10可知：粒子濾波算法將等效聲速作為未知狀態(tài)量進行估計，在遞推處理過程中估計值逐漸趨向真值，這是粒子濾波算法比常規(guī)方法定位更加準確的原因。

4 結 論

本文提出了一種基于粒子濾波算法的水聲導航定位方法。該算法在每一時刻將大量不同狀態(tài)的粒子代入信標信號到水下航行器的傳播時間的非線性觀測方程中，通過采樣-重要性重采樣濾波器分配每個粒子的權值比重，得到最優(yōu)狀態(tài)估計值。該方法基于粒子濾波模型，將等效聲速和水下航行器位置作為未知估計狀態(tài)，不僅減小了常規(guī)幾何算法因等效聲速誤差產生的定位誤差，而且隨著時間推移，精度逐漸提高，在水下航行器位置完全未知的狀況下能更準確地估計其運動狀態(tài)，定位精度提高到原來的4倍左右，具備較高的應用價值。