一種基于移動矢徑的單信標測距定位算法

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

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

0 引 言

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

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

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

1 基于移動矢徑的單信標測距定位模型

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

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

水下航行器在該定位場景下已知條件為：水聲導航信號單程傳播時延，海區平均聲速經驗信息，信標位置（信標經緯度坐標和信標發射換能器深度），自身運動信息，航行深度等。

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

假設水下航行器自身航位推算系統精度較高，短時間內運動軌跡推算信息作為不含誤差的準確信息，由此建立基于移動矢徑的單信標測距定位系統，其定位模型如圖2所示。

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

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

進而可根據兩次測得的距離信息建立如下定位方程：

2 基于EKF的定位濾波算法

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

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

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

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

定義狀態向量為

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

根據式（4）可得量測方程為

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

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

根據式（5）確定的狀態方程和式（7）確定的量測方程，可得系統的濾波方程為

則狀態轉移矩陣為

系統噪聲驅動矩陣為

量測矩陣為

量測噪聲驅動矩陣為

其中：

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

狀態一步預測方程為

狀態預測方差陣

量測值預測

濾波增益

狀態估計方差陣

狀態估計

3 仿真分析

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

圖 3 仿真航跡Fig. 3 Simulation track

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

圖 5 定位誤差Fig. 5 Positioning error

4 試驗驗證

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

圖 6 試驗航跡Fig. 6 Experiment track

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

圖 7 定位誤差Fig. 7 Positioning error

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

5 結 語

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