一種基于移動矢徑的單信標(biāo)測距定位算法

朱子堯，韓樹平，黃 銳，馬 鑫

(海軍潛艇學(xué)院，山東 青島 266044)

0 引 言

近年來，隨著國家對海洋開發(fā)的重視，水下定位服務(wù)的需求日益迫切，以慣性導(dǎo)航為代表的航位推算系統(tǒng)（Dead Reckoning，DR）長時間使用后會引入較大的累積誤差，而聲波是水下唯一可以進(jìn)行遠(yuǎn)距離傳播的信號形式，利用水聲導(dǎo)航系統(tǒng)校正慣導(dǎo)累積誤差成為研究熱點。單信標(biāo)同步式測距定位系統(tǒng)參考了OWTT（one-way travel-time）導(dǎo)航的原理，利用單程傳播時間TOF（time-of- fl ight）進(jìn)行測距，利用單個信標(biāo)可以實現(xiàn)水下航行器隱蔽無源定位，極大地降低應(yīng)用成本，拓展作用范圍，同時可以擴(kuò)展通信指揮功能，是未來發(fā)展的趨勢[1–2]。

文獻(xiàn)[3]給出一種航位推算軌跡平移策略的水下航行器初始位置確定方法，文獻(xiàn)[4]在水下航行器變向機(jī)動的前提下利用最小二乘法解算其水平初始位置，文獻(xiàn)[5]利用粒子濾波和擴(kuò)展卡爾曼濾波實現(xiàn)了單信標(biāo)測距定位。以上算法針對的是信標(biāo)固定的假設(shè)，實際情況中，信標(biāo)受洋流的影響不斷在運(yùn)動。文獻(xiàn)[6]利用移動的母船給水下航行器提供位置信息，水下航行器采用信息濾波方法實現(xiàn)了單信標(biāo)同步式測距定位。文獻(xiàn)[7]將移動矢徑的概念引入水下自主航行器（AUV）的協(xié)同定位中，文獻(xiàn)[8]考慮了水下未知定常洋流的影響，設(shè)計了一種基于單移動領(lǐng)航者僅依靠距離量測的主從式AUV協(xié)同定位方法，文獻(xiàn)[9]分析了移動矢徑對AUV協(xié)同定位精度的影響。

基于移動矢徑的定位方法在AUV協(xié)同定位中得到了廣泛應(yīng)用，本文將移動矢徑的的概念應(yīng)用到移動單信標(biāo)測距定位系統(tǒng)中，基于擴(kuò)展卡爾曼濾波（EKF）算法提出一種信標(biāo)運(yùn)動場景下的定位算法，用于對航位推算系統(tǒng)的累積誤差和定位海域未知聲速誤差進(jìn)行估計和濾除。仿真和試驗結(jié)果表明，該算法可以對未知聲速誤差進(jìn)行估計，明顯減小了初始位置誤差，大大提高了定位精度。

1 基于移動矢徑的單信標(biāo)測距定位模型

基于單信標(biāo)的同步式測距定位系統(tǒng)通過水聲通信測量單程傳播時延OWTT，其定位示意圖如圖1所示，水下航行器需要進(jìn)行定位時，對水聲導(dǎo)航信標(biāo)授時后將其發(fā)射出去，信標(biāo)上浮至水面后接收GNSS（Global Navigation Satellite System）定位信號，信標(biāo)成功捕獲衛(wèi)星信號實現(xiàn)自定位后以固定頻率向外發(fā)送水聲導(dǎo)航信號，通過水聲通信將自身位置和時間發(fā)送給水下航行器，由于初始時刻信標(biāo)和水下航行器時鐘同步，水下航行器接收水聲導(dǎo)航信號后結(jié)合自身時鐘測得單程傳播時延，并基于單程傳播時延進(jìn)行定位。

圖 1 單信標(biāo)同步式測距定位示意圖Fig. 1 Map of single beacon synchronous range measurement location

水下航行器在該定位場景下已知條件為：水聲導(dǎo)航信號單程傳播時延，海區(qū)平均聲速經(jīng)驗信息，信標(biāo)位置（信標(biāo)經(jīng)緯度坐標(biāo)和信標(biāo)發(fā)射換能器深度），自身運(yùn)動信息，航行深度等。

將水下航行器第1次捕獲到的水聲導(dǎo)航信號對應(yīng)的發(fā)射時間記為，以時刻信標(biāo)位置為原點建立笛卡爾坐標(biāo)系，由于洋流作用信標(biāo)的位置在不斷發(fā)生變化，將第次定位時信標(biāo)的坐標(biāo)記為，水下航行器坐標(biāo)記為，則可以建立如下基本定位方程：

假設(shè)水下航行器自身航位推算系統(tǒng)精度較高，短時間內(nèi)運(yùn)動軌跡推算信息作為不含誤差的準(zhǔn)確信息，由此建立基于移動矢徑的單信標(biāo)測距定位系統(tǒng)，其定位模型如圖2所示。

圖 2 基于移動矢徑的單信標(biāo)測距定位模型Fig. 2 Single beacon synchronous range measurement location model based on MRV

參考文獻(xiàn)[7]的定義，移動矢徑（Moving Radius Vector，MRV）指的是接收機(jī)在相鄰采樣時刻經(jīng)由自身航位推算獲得的相對位移向量。記水下航行器從第次定位到第次定位過程的移動矢徑為，則

進(jìn)而可根據(jù)兩次測得的距離信息建立如下定位方程：

2 基于EKF的定位濾波算法

水下航行器拋棄信標(biāo)進(jìn)行同步式測距定位的目的是為了對航位推算系統(tǒng)的累積誤差進(jìn)行濾除，縮小定位誤差。卡爾曼濾波算法根據(jù)建立的系統(tǒng)方程和觀測方程對需要處理的信號做出滿足最小均方誤差的估計，實時性好，魯棒性強(qiáng)，適合應(yīng)用于該定位場景下的定位濾波算法。

同步式測距需要信標(biāo)發(fā)送較多的導(dǎo)航信息，信號時間較長，為了避免水聲信道的多途效應(yīng)造成前后兩次信號發(fā)生互相串?dāng)_，通常將信標(biāo)水聲導(dǎo)航信號的發(fā)送間隔設(shè)置的步長較長。由于水下航行器與信標(biāo)距離的變化，傳播時間在不斷變化，信標(biāo)等時間間隔向外發(fā)送導(dǎo)航信號，水下航行器接收到水聲導(dǎo)航信號的時間間隔并非固定不變，而是隨著距離的變化在連續(xù)變化。以水中聲速1 500 m/s為例，相對距離變化1 500 m，時間間隔即可變化約1 s，誤差不可忽略。

因此，在設(shè)計卡爾曼濾波算法時應(yīng)當(dāng)考慮水聲傳播時間的變化，系統(tǒng)狀態(tài)的更新頻率為非等間隔，避免狀態(tài)方程采樣時間與觀測方程不同步的問題。此外，水下航行器掌握的海區(qū)平均聲速并不一定準(zhǔn)確，通常含有一定的偏差，在建立濾波模型時也要將其考慮進(jìn)去。

建立卡爾曼定位算法的過程如下：

定義狀態(tài)向量為

參考文獻(xiàn)[8]將誤差項分離的建模方法，定義狀態(tài)方程含噪聲的項為

根據(jù)式（4）可得量測方程為

由于深度傳感器測量精度較高，在水平遠(yuǎn)距離條件下，其誤差對測距來說可以忽略。因此定義

從模型上看，狀態(tài)方程是線性的，量測方程是非線性的，因此單信標(biāo)同步式測距定位是一個非線性估計問題，通常的處理方法是將其線性化轉(zhuǎn)化為一個近似的線性濾波問題，常用的非線性卡爾曼濾波算法有擴(kuò)展卡爾曼濾波（EKF）、無跡卡爾曼濾波（UKF）、粒子濾波（PF）、容積卡爾曼濾波（CKF）等。雖然EKF的近似精度只有1階，但沒有引進(jìn)其他額外的高階項，不需要選取任何參數(shù)，比UKF，CKF和PF等確定采樣型濾波算法更容易預(yù)測[10]。因此，本文采用EKF進(jìn)行定位算法設(shè)計。

根據(jù)式（5）確定的狀態(tài)方程和式（7）確定的量測方程，可得系統(tǒng)的濾波方程為

則狀態(tài)轉(zhuǎn)移矩陣為

系統(tǒng)噪聲驅(qū)動矩陣為

量測矩陣為

量測噪聲驅(qū)動矩陣為

其中：

定位算法的迭代更新過程如下：

狀態(tài)一步預(yù)測方程為

狀態(tài)預(yù)測方差陣

量測值預(yù)測

濾波增益

狀態(tài)估計方差陣

狀態(tài)估計

3 仿真分析

針對前文所述的應(yīng)用場景，設(shè)計一道仿真題目用于測試算法的性能。如圖3所示，水下航行器航速2 m/s；信標(biāo)以10 s為間隔向外發(fā)送水聲導(dǎo)航信號，受海流影響以0.3 m/s的速度勻速航行；設(shè)置真實聲速誤差服從均值為30，方差為1的高斯分布，含有[500 m，500 m]的初始位置誤差。仿真時間為1 500 s。濾波參數(shù)設(shè)置為：

圖 3 仿真航跡Fig. 3 Simulation track

圖 4 聲速誤差估計Fig. 4 Error estimation of sound velocity

圖 5 定位誤差Fig. 5 Positioning error

4 試驗驗證

為檢驗算法在實際應(yīng)用中的性能，2017年8月，在丹江口水庫進(jìn)行了水聲導(dǎo)航科學(xué)試驗。試驗平臺為某型UUV（Unmanned Underwater Vehicle），試驗設(shè)備包括：GNSS定位系統(tǒng)、慣性導(dǎo)航系統(tǒng)、深度傳感器和NCART長基線定位系統(tǒng)等。信標(biāo)隨水流漂移，其水面浮體裝備有北斗定位系統(tǒng)，起始坐標(biāo)（0，0）。UUV慣導(dǎo)記錄航跡如圖6所示，航行速度2 m/s，由于UUV攜帶的慣導(dǎo)精度較高，且下潛前在水面利用GNSS定位系統(tǒng)對慣導(dǎo)進(jìn)行了校正，本次試驗以慣導(dǎo)推算航跡作為基準(zhǔn)。試驗中間隔10 s進(jìn)行一次測距定位，整個過程持續(xù)700 s。對采集回來的數(shù)據(jù)進(jìn)行離線處理，并給慣導(dǎo)記錄數(shù)據(jù)增加[500 m，500 m]的誤差作為UUV航跡推算軌跡。

圖 6 試驗航跡Fig. 6 Experiment track

濾波參數(shù)設(shè)置同式（20），應(yīng)用本文算法對增加誤差后的試驗數(shù)據(jù)進(jìn)行處理，定位誤差如圖7所示，聲速誤差估計如圖8所示。由圖7可知，本文算法處理實際數(shù)據(jù)時同樣取得了較好的效果，中間出現(xiàn)了一次定位波動是因為在轉(zhuǎn)向過程中傳播時延觀測量出現(xiàn)了粗差，這也造成圖8中聲速誤差估計并不準(zhǔn)確。

圖 7 定位誤差Fig. 7 Positioning error

圖 8 聲速誤差估計Fig. 8 Error estimation of sound velocity

5 結(jié) 語

本文借助移動矢徑的概念建立了移動單信標(biāo)同步式測距定位模型，并基于EKF設(shè)計了該定位模型下的定位濾波算法，然后通過解算仿真算例和試驗數(shù)據(jù)，驗證了該定位濾波算法的有效性，實現(xiàn)了利用單個移動信標(biāo)為水下航行器提供定位服務(wù)。水下航行器長時間航行后航位推算系統(tǒng)會累積較大的誤差，本文算法可以在未知聲速干擾下有效約束這種初始位置誤差，提高定位精度，并對聲速誤差進(jìn)行估計。算法定位性能會受觀測值粗差的影響，如何提升算法的抗差性能是下一步的研究工作。