基于自適應濾波的水下長基線導航定位技術

成月，趙俊波，李錦，曹園山

(中國船舶科學研究中心 深海載人裝備國家重點實驗室，江蘇 無錫 214082)

0 引 言

海洋在資源、經濟、安全等方面具有戰略意義，自治式潛水器（Autonomous Underwater Vehicle，AUV）作為一種新型水下無人運載平臺，因其機動性、隱蔽性強，具備大范圍搜索與探測能力，被廣泛應用于海洋科考、海洋探測、海洋安全等領域。為保障AUV 在水下具備長航時作業能力，高精度、高可靠性的導航定位系統至關重要[1-3]。捷聯慣性導航系統（Strapdown Inertial Navigation System,SINS）不受外界干擾，可在任何介質和任何環境條件下實現導航[4]，一般作為AUV 導航定位系統的關鍵子系統。由于SINS 導航定位誤差隨時間累積，長期穩定性差，需采用外部輔助導航手段對其進行補償修正，通過組合導航技術提高導航定位系統的整體性能。

水下主要采用聲波進行導航定位，其中，長基線（Long Baseline, LBL）定位系統能夠在較大的范圍和較深的海水中得到較高的導航定位精度，利用LBL 輔助SINS 的組合導航技術是抑制誤差發散、提高導航定位精度的有效手段[5-6]。LBL 定位通過測量布放在海底的應答器陣與AUV 上換能器之間的距離，求解AUV的位置坐標，再通過Kalman 濾波實現對慣導的校正。但由于水下環境多變，干擾因素復雜，組合導航系統精度難以保證。文獻[7]采用異步量測序貫濾波方法提高了在應答信號缺失情況下組合導航系統的精度；文獻[8]研究了SINS/LBL 緊組合系統量測方程非線性對定位精度的影響，選用UKF 濾波算法進行信息融合；文獻[9]提出一種迭代聲速修正算法，提高了LBL 在同步工作方式下目標定位精度。

這些算法主要集中在提高LBL 自身的定位精度及改進組合導航濾波算法，針對這兩點問題，本文提出一種基于自適應濾波的水下長基線導航定位技術。一方面，研究基于偽距輔助的SINS/LBL 組合算法，解決水聲通信時延導致LBL 定位系統位置坐標計算不準確的問題；另一方面，針對系統噪聲統計特性未知問題，引入自適應濾波算法，通過改進組合導航系統信息融合方式，提高AUV 導航定位精度及系統可靠性。

1 偽距輔助的SINS/LBL 組合導航系統

1.1 系統原理及構成

LBL 定位系統是通過測量AUV 與海底應答器基陣之間的距離解算出目標AUV 位置坐標，其定位原理如圖1 所示。

圖 1 LBL 系統定位原理示意圖Fig. 1Schematic diagram of LBL positioning system

LBL 海底應答器之間距離約100～6 000 m，其絕對位置坐標已知，主要用于接收聲信號并發射不同頻率的應答信號。AUV 上安裝收發器，用于發射詢問信號。當海底應答器基陣收到詢問信號后，以不同頻率應答。AUV 接收應答信號后，通過信號收發時間差即可計算出與相應應答器的距離，并通過球面交匯法或雙曲面交匯法解算出AUV 位置坐標。

為實現水下高精度導航定位，AUV 組合導航系統以SINS 為關鍵子系統，LBL 定位系統為輔助系統。傳統組合導航方式一般以LBL 解算出的位置信息作為量測量，通過Kalman 濾波校正SINS 系統。但在實際應用中，由于聲信號在水中傳播速度慢，隨著AUV 運動將會產生時延誤差，導致通過幾何交匯進行位置解算的LBL 定位系統失效，且水下復雜的環境使得系統噪聲無法精確建模，使用常規kalman 濾波無法得到最優估計，甚至會使組合導航系統誤差發散。針對以上問題，本文設計基于自適應濾波的SINS/LBL 組合導航系統，以SINS 系統推算的偽距ρSINS與LBL 系統量測的偽距ρLBL之差作為組合導航系統量測量，并通過自適應濾波實現信息融合。系統結構如圖2 所示。

圖 2 組合導航系統結構圖Fig. 2Structure diagram of integrated navigation system

本節通過構建偽距輔助的SINS/LBL 組合導航系統解決水下時延誤差對導航定位精度的影響，建立組合導航系統狀態方程及量測方程。

1.2 狀態方程

設AUV 真實位置為(X,Y,Z)，海底應答器坐標為(xi,yi,zi),(i=1,2,3,4) ，為AUV 到第 i個海底應答器水聲信號的傳播時間，為AUV 到第 i個海底應答器的量測水聲偽距，則有：

其中： ti為AUV 到第 i個海底應答器傳播時間真值；δt為AUV 到海底應答器鐘差；為 測量誤差； c為聲波在海水中傳播速度。

δt的等效距離誤差δtu可用一階馬爾科夫過程表示為：

其中： τtu為 相關時間； wtu為白噪聲。

對于SINS 系統，選取15 維狀態量，分別是三維位置誤差δL,δλ,δh ， 三維速度誤差 δ vE,δvN,δvU，三維姿態角誤差 φE,φN,φU，陀螺漂移誤差 εx,εy,εz，加速計偏置誤差 ?x,?y,?z，表示為：

XS INS =[φEφNφUδvEδvNδvUδL δλ δh εxεyεz?x?y?z]T其狀態方程為：

對于LBL 定位系統，選取等效距離誤差 δtu為 狀態量，其狀態方程可表示為：

由式（4）和式（5）可得，偽距輔助的SINS/LBL組合導航系統16 維狀態量表示為：

1.3 量測方程

由于水聲信號時延，AUV 接收到海底應答器發出信號所在的位置并不一致，即采用幾何交匯法無法解算出位置坐標。AUV 由慣導解算出的位置坐標表示為(xSINS,ySINS,zSINS)，設 ti時刻AUV 接收到第 i個應答器的水聲信號，以表示AUV 到第 i個應答器的偽距，則表示為：

其中， ( δxti,δyti,δzti)表示為第 ti時刻起AUV 移動的位置增量，即需先計算得出AUV 在 ti時刻位置，然后進行偽距計算。

對式（8）在AUV 真實位置(X,Y,Z)處進行泰勒展開，偽距方程可簡化為：

其中，Rii為AUV 到第 個應答器的真實距離。

偽距輔助的SINS/LBL 組合導航系統量測量表示為：

偽距輔助的SINS/LBL 組合導航系統量測方程表示為：e

其中，為卯酉圈曲率半徑， 為旋轉橢球扁率。

2 自適應濾波算法

組合導航系統通過濾波算法實現最優估計，傳統的組合導航系統一般采用Kalman 濾波作為信息融合手段，但是Kalman 濾波需要構建噪聲模型，由于水下環境產生的干擾復雜，往往無法對其進行精確建模，一般采用固定值直接帶入濾波方程，這會極大影響Kalman 濾波的穩定性及組合導航精度。為此，本文采用自適應濾波算法估計組合導航系統誤差，對系統量測噪聲的協方差陣 R進行在線辨識，實現對AUV 姿態、速度、位置信息的最優估計。

式（7）、式（13）離散化后系統狀態方程及量測方程如下：

自適應濾波算法主要通過引入遺忘因子b ，不斷強化量測量 Zk的作用，并削弱 Z1···Zk-1的影響，通過新息 εk在線估計協方差陣 Rk，并進一步優化濾波增益陣Kk，從而影響最終濾波結果。自適應濾波算法流程如下：

3 仿真結果分析

3.1 仿真環境及運動軌跡

設4 個海底應答器之間間隔1 000 m，AUV 初始姿態為(ψ,θ,γ)=(0,0,0)，初始速度為 ( vE,vN,vU)=(0,0,0)，初始位置為 ( λ,L,h)=(0,0,0)。采樣頻率100 Hz，總航時3 260 s。采用Matlab 對AUV 運動軌跡進行仿真，其三維運動軌跡如圖3 所示。

圖 3 AUV 三維軌跡圖Fig. 3AUV three-dimensional trajectory

3.2 仿真結果對比

在建立狀態與量測方程的基礎上，用Matlab 對本文提出組合算法進行仿真試驗。考慮到慣導的高度通道發散，AUV 可直接通過深度計測量深度信息，故重點針對二維平面導航定位精度進行對比分析。AUV 依照圖3 所示軌跡運動，在純慣導算法及在偽距輔助的長基線組合導航算法下的二維平面仿真軌跡對比如圖4 所示。

圖 4 二維軌跡對比圖Fig. 4Two-dimensional trajectory contrast diagram

由圖4 可知，在3 260 s 的航時中，隨著時間累積，慣性導航定位誤差出現增長發散情況，導致純慣導算法軌跡與真實軌跡出現千米級偏移量。這說明在無外界信息參考的情況下，僅依靠純慣導無法滿足AUV 水下導航定位精度要求。而在圖4（b）中，偽距輔助的長基線組合導航算法由于提供了位置參考，抑制了慣導誤差累積，其軌跡基本能夠跟隨真實軌跡，沒有出現誤差發散情況，導航定位精度較高。

圖 5 速度誤差對比圖Fig. 5Velocity error contrast diagram

為進一步模擬水下復雜噪聲干擾情況，仿真時增強量測噪聲協方差陣 R，用以驗證自適應濾波算法針對噪聲統計模型未知情況下的修正能力及算法有效性。AUV 依照圖3 所示軌跡運動，采用本文提出的基于自適應濾波的長基線導航定位算法與基于傳統Kalman 濾波的長基線組合導航算法的速度、位置誤差進行對比，仿真與分析結果如圖5 和圖6 所示。

可知，在噪聲出現大幅變化的情況下，組合導航系統的信息融合算法將影響最終的導航定位精度。如圖5（b）和圖6（b）所示，由于量測噪聲協方差陣R未知，仿真初始階段誤差震蕩幅度較大。在航行時間2000 s 后，由于自適應濾波算法對 R進行在線估計，采用自適應濾波算法的長基線組合導航系統誤差基本收斂，其東向、北向速度誤差在±0.05 m/s范圍內波動，東向、北向位置誤差在±10 m范圍內波動。對比圖5（a）和圖6（a），在航行時間2 000 s 后，采用傳統Kalman 濾波算法的長基線組合導航系統的東向、北向速度誤差達到-0.1 m/s，且有發散趨勢，其東向位置誤差達到 - 15 m，北向位置誤差達到 - 27 m。這說明在環境噪聲干擾較強時，即使采用長基線通過位置校正慣導，也無法達到高精度導航定位要求，甚至無法確保系統的導航定位誤差收斂。

圖 6 位置誤差對比圖Fig. 6Position error contrast diagram

4 結 語

本文提出一種基于自適應濾波的水下長基線導航定位技術，通過偽距輔助的長基線組合導航系統對慣導進行校正，防止導航誤差隨時間累積；通過自適應濾波算法對噪聲進行在線辨識，實現對系統導航定位參數的最優估計，提高系統的導航定位精度及可靠性。仿真結果表明，面對水下復雜的噪聲干擾，本文設計的水下組合導航算法具有較強的抗干擾性能，并能夠滿足AUV 水下長航時、高精度的導航定位要求。