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

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

0 引 言

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

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

1 問題描述及解決思路

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

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

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

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

2 基于粒子濾波水聲導(dǎo)航定位方法

2.1 粒子濾波基本原理

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

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

2.2 基于粒子濾波算法的水聲導(dǎo)航定位模型

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

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

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

該濾波器每個(gè)時(shí)刻都進(jìn)行重采樣，因而：

重要性權(quán)值在重采樣步驟前需要進(jìn)行歸一化，之后的每個(gè)時(shí)刻都進(jìn)行重采樣操作。因此粒子的權(quán)值可以表示為

對(duì)每個(gè)粒子加權(quán)平均估計(jì)當(dāng)前時(shí)刻目標(biāo)的運(yùn)動(dòng)狀態(tài)，并進(jìn)行對(duì)當(dāng)前時(shí)刻粒子重采樣，得到重采樣后的粒子。在下一時(shí)刻，將重采樣后的粒子代入粒子的狀態(tài)方程中：

式中，A為根據(jù)目標(biāo)運(yùn)動(dòng)狀態(tài)得到的逆推矩陣。由式(16)可得到下一時(shí)刻的粒子并重新為粒子分配新的權(quán)值，繼而得到下一時(shí)刻估計(jì)得到的目標(biāo)運(yùn)動(dòng)狀態(tài)。

2.3 基于粒子濾波算法的水聲導(dǎo)航定位方法

算法輸入：每個(gè)信標(biāo)的發(fā)射信號(hào)到航行器的傳播時(shí)間；由航行器慣性導(dǎo)航得到的航行器每個(gè)時(shí)刻的運(yùn)動(dòng)速度。

(1) 在每個(gè)時(shí)刻隨機(jī)產(chǎn)生大量粒子模擬的航行器位置以及各個(gè)信標(biāo)信號(hào)傳播到航行器的等效聲速，對(duì)應(yīng)得到這一時(shí)刻每個(gè)粒子的權(quán)值；

(2) 對(duì)每個(gè)時(shí)刻的粒子加權(quán)估計(jì)航行器位置及各個(gè)等效聲速；

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

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

算法輸出：每個(gè)時(shí)刻航行器位置分量、各個(gè)標(biāo)信號(hào)傳播到航行器的等效聲速估計(jì)值。

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

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

3 仿真分析

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

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

圖4 水下航行器、信標(biāo)位置及運(yùn)動(dòng)軌跡Fig.4 Position and trajectory of underwater vehicle and beacon

圖5 各信標(biāo)到水下航行器等效聲速真值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 水下航行器真實(shí)航向角Fig.7 The true value of underwater vehicle true heading angle

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

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

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

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

4 結(jié) 論

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