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

(中國(guó)船舶科學(xué)研究中心 深海載人裝備國(guó)家重點(diǎn)實(shí)驗(yàn)室，江蘇 無(wú)錫 214082)

0 引 言

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

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

這些算法主要集中在提高LBL 自身的定位精度及改進(jìn)組合導(dǎo)航濾波算法，針對(duì)這兩點(diǎn)問(wèn)題，本文提出一種基于自適應(yīng)濾波的水下長(zhǎng)基線導(dǎo)航定位技術(shù)。一方面，研究基于偽距輔助的SINS/LBL 組合算法，解決水聲通信時(shí)延導(dǎo)致LBL 定位系統(tǒng)位置坐標(biāo)計(jì)算不準(zhǔn)確的問(wèn)題；另一方面，針對(duì)系統(tǒng)噪聲統(tǒng)計(jì)特性未知問(wèn)題，引入自適應(yīng)濾波算法，通過(guò)改進(jìn)組合導(dǎo)航系統(tǒng)信息融合方式，提高AUV 導(dǎo)航定位精度及系統(tǒng)可靠性。

1 偽距輔助的SINS/LBL 組合導(dǎo)航系統(tǒng)

1.1 系統(tǒng)原理及構(gòu)成

LBL 定位系統(tǒng)是通過(guò)測(cè)量AUV 與海底應(yīng)答器基陣之間的距離解算出目標(biāo)AUV 位置坐標(biāo)，其定位原理如圖1 所示。

圖 1 LBL 系統(tǒng)定位原理示意圖Fig. 1Schematic diagram of LBL positioning system

LBL 海底應(yīng)答器之間距離約100～6 000 m，其絕對(duì)位置坐標(biāo)已知，主要用于接收聲信號(hào)并發(fā)射不同頻率的應(yīng)答信號(hào)。AUV 上安裝收發(fā)器，用于發(fā)射詢問(wèn)信號(hào)。當(dāng)海底應(yīng)答器基陣收到詢問(wèn)信號(hào)后，以不同頻率應(yīng)答。AUV 接收應(yīng)答信號(hào)后，通過(guò)信號(hào)收發(fā)時(shí)間差即可計(jì)算出與相應(yīng)應(yīng)答器的距離，并通過(guò)球面交匯法或雙曲面交匯法解算出AUV 位置坐標(biāo)。

為實(shí)現(xiàn)水下高精度導(dǎo)航定位，AUV 組合導(dǎo)航系統(tǒng)以SINS 為關(guān)鍵子系統(tǒng)，LBL 定位系統(tǒng)為輔助系統(tǒng)。傳統(tǒng)組合導(dǎo)航方式一般以LBL 解算出的位置信息作為量測(cè)量，通過(guò)Kalman 濾波校正SINS 系統(tǒng)。但在實(shí)際應(yīng)用中，由于聲信號(hào)在水中傳播速度慢，隨著AUV 運(yùn)動(dòng)將會(huì)產(chǎn)生時(shí)延誤差，導(dǎo)致通過(guò)幾何交匯進(jìn)行位置解算的LBL 定位系統(tǒng)失效，且水下復(fù)雜的環(huán)境使得系統(tǒng)噪聲無(wú)法精確建模，使用常規(guī)kalman 濾波無(wú)法得到最優(yōu)估計(jì)，甚至?xí)菇M合導(dǎo)航系統(tǒng)誤差發(fā)散。針對(duì)以上問(wèn)題，本文設(shè)計(jì)基于自適應(yīng)濾波的SINS/LBL 組合導(dǎo)航系統(tǒng)，以SINS 系統(tǒng)推算的偽距ρSINS與LBL 系統(tǒng)量測(cè)的偽距ρLBL之差作為組合導(dǎo)航系統(tǒng)量測(cè)量，并通過(guò)自適應(yīng)濾波實(shí)現(xiàn)信息融合。系統(tǒng)結(jié)構(gòu)如圖2 所示。

圖 2 組合導(dǎo)航系統(tǒng)結(jié)構(gòu)圖Fig. 2Structure diagram of integrated navigation system

本節(jié)通過(guò)構(gòu)建偽距輔助的SINS/LBL 組合導(dǎo)航系統(tǒng)解決水下時(shí)延誤差對(duì)導(dǎo)航定位精度的影響，建立組合導(dǎo)航系統(tǒng)狀態(tài)方程及量測(cè)方程。

1.2 狀態(tài)方程

設(shè)AUV 真實(shí)位置為(X,Y,Z)，海底應(yīng)答器坐標(biāo)為(xi,yi,zi),(i=1,2,3,4) ，為AUV 到第 i個(gè)海底應(yīng)答器水聲信號(hào)的傳播時(shí)間，為AUV 到第 i個(gè)海底應(yīng)答器的量測(cè)水聲偽距，則有：

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

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

其中： τtu為 相關(guān)時(shí)間； wtu為白噪聲。

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

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

對(duì)于LBL 定位系統(tǒng)，選取等效距離誤差 δtu為 狀態(tài)量，其狀態(tài)方程可表示為：

由式（4）和式（5）可得，偽距輔助的SINS/LBL組合導(dǎo)航系統(tǒng)16 維狀態(tài)量表示為：

1.3 量測(cè)方程

由于水聲信號(hào)時(shí)延，AUV 接收到海底應(yīng)答器發(fā)出信號(hào)所在的位置并不一致，即采用幾何交匯法無(wú)法解算出位置坐標(biāo)。AUV 由慣導(dǎo)解算出的位置坐標(biāo)表示為(xSINS,ySINS,zSINS)，設(shè) ti時(shí)刻AUV 接收到第 i個(gè)應(yīng)答器的水聲信號(hào)，以表示AUV 到第 i個(gè)應(yīng)答器的偽距，則表示為：

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

對(duì)式（8）在AUV 真實(shí)位置(X,Y,Z)處進(jìn)行泰勒展開(kāi)，偽距方程可簡(jiǎn)化為：

其中，Rii為AUV 到第 個(gè)應(yīng)答器的真實(shí)距離。

偽距輔助的SINS/LBL 組合導(dǎo)航系統(tǒng)量測(cè)量表示為：

偽距輔助的SINS/LBL 組合導(dǎo)航系統(tǒng)量測(cè)方程表示為：e

其中，為卯酉圈曲率半徑， 為旋轉(zhuǎn)橢球扁率。

2 自適應(yīng)濾波算法

組合導(dǎo)航系統(tǒng)通過(guò)濾波算法實(shí)現(xiàn)最優(yōu)估計(jì)，傳統(tǒng)的組合導(dǎo)航系統(tǒng)一般采用Kalman 濾波作為信息融合手段，但是Kalman 濾波需要構(gòu)建噪聲模型，由于水下環(huán)境產(chǎn)生的干擾復(fù)雜，往往無(wú)法對(duì)其進(jìn)行精確建模，一般采用固定值直接帶入濾波方程，這會(huì)極大影響Kalman 濾波的穩(wěn)定性及組合導(dǎo)航精度。為此，本文采用自適應(yīng)濾波算法估計(jì)組合導(dǎo)航系統(tǒng)誤差，對(duì)系統(tǒng)量測(cè)噪聲的協(xié)方差陣 R進(jìn)行在線辨識(shí)，實(shí)現(xiàn)對(duì)AUV 姿態(tài)、速度、位置信息的最優(yōu)估計(jì)。

式（7）、式（13）離散化后系統(tǒng)狀態(tài)方程及量測(cè)方程如下：

自適應(yīng)濾波算法主要通過(guò)引入遺忘因子b ，不斷強(qiáng)化量測(cè)量 Zk的作用，并削弱 Z1···Zk-1的影響，通過(guò)新息 εk在線估計(jì)協(xié)方差陣 Rk，并進(jìn)一步優(yōu)化濾波增益陣Kk，從而影響最終濾波結(jié)果。自適應(yīng)濾波算法流程如下：

3 仿真結(jié)果分析

3.1 仿真環(huán)境及運(yùn)動(dòng)軌跡

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

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

3.2 仿真結(jié)果對(duì)比

在建立狀態(tài)與量測(cè)方程的基礎(chǔ)上，用Matlab 對(duì)本文提出組合算法進(jìn)行仿真試驗(yàn)。考慮到慣導(dǎo)的高度通道發(fā)散，AUV 可直接通過(guò)深度計(jì)測(cè)量深度信息，故重點(diǎn)針對(duì)二維平面導(dǎo)航定位精度進(jìn)行對(duì)比分析。AUV 依照?qǐng)D3 所示軌跡運(yùn)動(dòng)，在純慣導(dǎo)算法及在偽距輔助的長(zhǎng)基線組合導(dǎo)航算法下的二維平面仿真軌跡對(duì)比如圖4 所示。

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

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

圖 5 速度誤差對(duì)比圖Fig. 5Velocity error contrast diagram

為進(jìn)一步模擬水下復(fù)雜噪聲干擾情況，仿真時(shí)增強(qiáng)量測(cè)噪聲協(xié)方差陣 R，用以驗(yàn)證自適應(yīng)濾波算法針對(duì)噪聲統(tǒng)計(jì)模型未知情況下的修正能力及算法有效性。AUV 依照?qǐng)D3 所示軌跡運(yùn)動(dòng)，采用本文提出的基于自適應(yīng)濾波的長(zhǎng)基線導(dǎo)航定位算法與基于傳統(tǒng)Kalman 濾波的長(zhǎng)基線組合導(dǎo)航算法的速度、位置誤差進(jìn)行對(duì)比，仿真與分析結(jié)果如圖5 和圖6 所示。

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

圖 6 位置誤差對(duì)比圖Fig. 6Position error contrast diagram

4 結(jié) 語(yǔ)

本文提出一種基于自適應(yīng)濾波的水下長(zhǎng)基線導(dǎo)航定位技術(shù)，通過(guò)偽距輔助的長(zhǎng)基線組合導(dǎo)航系統(tǒng)對(duì)慣導(dǎo)進(jìn)行校正，防止導(dǎo)航誤差隨時(shí)間累積；通過(guò)自適應(yīng)濾波算法對(duì)噪聲進(jìn)行在線辨識(shí)，實(shí)現(xiàn)對(duì)系統(tǒng)導(dǎo)航定位參數(shù)的最優(yōu)估計(jì)，提高系統(tǒng)的導(dǎo)航定位精度及可靠性。仿真結(jié)果表明，面對(duì)水下復(fù)雜的噪聲干擾，本文設(shè)計(jì)的水下組合導(dǎo)航算法具有較強(qiáng)的抗干擾性能，并能夠滿足AUV 水下長(zhǎng)航時(shí)、高精度的導(dǎo)航定位要求。