摘要： 系統(tǒng)地研究了不同波束可用情況下的多普勒計程儀（Doppler velocity logger， DVL）/捷聯(lián)慣性導(dǎo)航系統(tǒng)（strapdown inertial navigation system， SINS）緊組合導(dǎo)航算法。證明了3個或4個波束可用情況下，DVL/SINS波束域緊組合的定位精度與三維速度松組合系統(tǒng)具有等價性，并給出了松組合中量測協(xié)方差陣的確定方法。針對緊組合系統(tǒng)定位精度隨可用波束減少而降低的問題，考慮DVL的誤差參數(shù)，基于載體運(yùn)動約束和壓力深度計提出一種改進(jìn)的虛擬波束輔助緊組合算法。實驗結(jié)果表明，3個或4個波束可用時的松組合與緊組合的定位精度等價;改進(jìn)的虛擬波束輔助緊組合算法可以準(zhǔn)確跟蹤故障的波束速度，有效提升了不同波束可用情況下緊組合的定位精度。

關(guān)鍵詞： 多普勒計程儀; 捷聯(lián)慣性導(dǎo)航系統(tǒng); 組合導(dǎo)航; 等價性分析; 虛擬波束構(gòu)建

中圖分類號： TN 967.2

文獻(xiàn)標(biāo)志碼： A

DOI：10.12305/j.issn.1001-506X.2024.06.29

Equivalence analysis between DVL/SINS loosely and tightly coupled systems and improved virtual beam-aided tightly coupled algorithm

JIN Kaidi¹， CHAI Hongzhou^1，*， SU Chuhan²

（1. Institute of Geospatial Information， Strategic Support Force Information Engineering University， Zhengzhou 450001， China; 2. PIESAT Institute of Applied Beidou Navigation Technologies at Zhengzhou， Zhengzhou 450001， China）

Abstract： This paper systematically studies the Doppler velocity logger （DVL）/strapdown inertial navigation system （SINS） tightly coupled navigation system with different available beams. It is demonstrated that the positioning accuracy of tightly coupled system for DVL/SINS beam domain is equivalent to the 3D velocity-aided loosely coupled system when three or four beams are available， and the determination method of measurement covariance matrix in loosely coupled system is given. To address the problem that the positioning accuracy of tightly coupled system decreases with the reduction of available beams， considering the error parameters of DVL， an improved virtual beam-aided tightly coupled algorithm is proposed based on the motion constraint and pressure sensor （PS） measurements. Experimental results show that the loosely and tightly coupled systems have equivalent positioning accuracy when three or four beams are available， and the improved virtual beam-aided tightly coupled system can accurately track the velocity of outage beams， which effectively improves the positioning accuracy of tightly coupled system when different beams are available.

Keywords： Doppler velocity logger （DVL）; strapdown inertial navigation system （SINS）; integrated navigation; equivalence analysis; virtual beam construction

0 引 言

作為新一代的海洋探索工具，水下無人航行器（underwater unmanned vehicle， UUV）在海洋測繪、水下目標(biāo)探測、海洋資源開發(fā)等任務(wù)中發(fā)揮著愈發(fā)重要的作用^［1-3］。精確的位置信息是UUV順利完成任務(wù)和安全往返的關(guān)鍵。海水對電磁波有很強(qiáng)的吸收作用，廣泛用于陸地和空中導(dǎo)航的全球衛(wèi)星導(dǎo)航系統(tǒng)（global navigation satellite system， GNSS）信號在水下迅速衰減，使UUV水下精確定位成為了研究的重點(diǎn)和難點(diǎn)^［4］。

作為UUV導(dǎo)航的主傳感器，捷聯(lián)慣性導(dǎo)航系統(tǒng)（strapdown inertial navigation system， SINS）具有自主性強(qiáng)、隱蔽性高和導(dǎo)航參數(shù)豐富等優(yōu)勢，但其誤差會隨時間迅速累積^［5］。多普勒計程儀（Doppler velocity logger， DVL）可以提供高精度的測速信息，有效修正SINS的累積誤差。因此，DVL/SINS組合導(dǎo)航系統(tǒng)在UUV上得到了廣泛應(yīng)用^［6］。DVL/SINS組合導(dǎo)航系統(tǒng)常用卡爾曼濾波進(jìn)行數(shù)據(jù)融合，根據(jù)DVL提供的信息種類的不同，DVL/SINS組合導(dǎo)航系統(tǒng)可分為松組合和緊組合^［7-8］。在松組合中，DVL提供載體系三維速度修正SINS誤差。受海洋復(fù)雜環(huán)境影響，DVL的量測值可能包含時變噪聲、非高斯噪聲和粗差，嚴(yán)重影響DVL/SINS組合導(dǎo)航系統(tǒng)精度。諸學(xué)者在自適應(yīng)濾波、非高斯濾波和抗差濾波等方面展開研究，顯著提升了DVL/SINS松組合系統(tǒng)的精度和魯棒性^［9-13］。

在實際應(yīng)用中，DVL常采用四波束詹納斯配置測量載體三維速度。但在復(fù)雜環(huán)境下，如UUV經(jīng)過海溝或魚群時，部分波束可能無法反射，造成波束故障。當(dāng)可用波束數(shù)量小于3個時，由于無法計算三維速度，此時松組合系統(tǒng)將退化為純慣性導(dǎo)航^［14-15］系統(tǒng)。為提升波束故障時組合導(dǎo)航系統(tǒng)的定位精度，直接利用波束信息作為量測值的緊組合系統(tǒng)逐漸得到關(guān)注^［16-17］。文獻(xiàn)［14］介紹了緊組合系統(tǒng)的設(shè)計框圖，但并未給出具體實施細(xì)節(jié)。文獻(xiàn)［18］系統(tǒng)地提出了DVL/SINS緊組合系統(tǒng)的濾波模型和實施流程，在部分波束故障時仍可利用可用的波束信息，顯著提升了組合導(dǎo)航系的統(tǒng)精度。為適應(yīng)時變的波束噪聲，文獻(xiàn)［19］將Sage-Husa自適應(yīng)濾波應(yīng)用于DVL/SINS緊組合系統(tǒng)。進(jìn)一步地，為解決波束粗差和濾波模型不準(zhǔn)確的問題，文獻(xiàn)［20］提出一種基于雙自適應(yīng)因子的緊組合模型。文獻(xiàn)［21］結(jié)合UUV運(yùn)動模型提出一種運(yùn)動約束補(bǔ)償?shù)木o組合算法，并結(jié)合魯棒因子動態(tài)調(diào)節(jié)運(yùn)動約束程度。上述研究雖然提升了定位精度，但緊組合系統(tǒng)精度仍會隨著可用波束數(shù)量的減少而逐漸減低。

為提升緊組合系統(tǒng)在部分波束可用條件下的定位精度，文獻(xiàn)［18］針對單波束可用情況，基于UUV非完整性約束（non-holonomic constraint， NHC）條件提出虛擬波束構(gòu)建算法，但其未對其他波束故障情況給出解決方案。文獻(xiàn)［22］針對不同可用波束數(shù)量情況，提出一種基于波束幾何構(gòu)型的虛擬波束重構(gòu)方案。文獻(xiàn)［23］指出上述方案均忽略了載體運(yùn)動狀態(tài)，同時未考慮DVL存在的安裝偏差角，因此直接重構(gòu)虛擬波束將嚴(yán)重影響波束精度，并提出使用SINS速度作為外部輸入構(gòu)造虛擬波束。然而，基于SINS信息的虛擬波束精度會隨DVL故障時間的延長而逐漸降低。同時，當(dāng)前研究普遍認(rèn)為當(dāng)四波束可用時，利用三維速度作為量測值的松組合系統(tǒng)相當(dāng)于對四維波束信息進(jìn)行了降維處理，因此緊組合的精度比松組合更高，并在試驗中得到了對應(yīng)的結(jié)論^{［16，18-20，22］}。

針對當(dāng)前DVL/SINS緊組合導(dǎo)航研究中存在的問題，本文系統(tǒng)研究了不同可用波束數(shù)量情況下的DVL/SINS組合導(dǎo)航算法。首先，證明了3個或4個波束可用情況下松組合和緊組合定位精度的等價性，并給出松組合量測噪聲協(xié)方差陣的具體確定方法;然后，針對不同波束故障情況，充分考慮DVL的誤差參數(shù)，結(jié)合載體側(cè)向速度約束和壓力深度計（pressure sensor， PS）量測值，提出改進(jìn)的虛擬波束輔助緊組合算法;最后，使用UUV實測數(shù)據(jù)驗證了本文理論的正確性和所提算法的有效性。

1 DVL/SINS組合導(dǎo)航濾波模型

1.1 三維速度松組合濾波模型

考慮SINS的導(dǎo)航參數(shù)誤差，在DVL/SINS組合導(dǎo)航系統(tǒng)中選擇15維狀態(tài)向量：

X=［?^T，（δvⁿ）^T，δp^T，（ε^b）^T，（Δ^b）^T］^T（1）

式中：b系為“右-前-上”載體坐標(biāo)系;n系為“東-北-天”導(dǎo)航坐標(biāo)系;?為SINS姿態(tài)失準(zhǔn)角;δvⁿ，δp分別為SINS的速度和位置誤差;ε^b和Δ^b分別表示建模為隨機(jī)常值的陀螺零偏和加速度計零偏。

由SINS誤差方程，得到Kalman濾波狀態(tài)方程為

X·=F_t0_9×6

0_6×90_6×6X+G_tW_SINS（2）

式中：X·表示對X的微分;F_t為由SINS誤差方程決定的誤差傳遞矩陣;G_t為系統(tǒng)噪聲驅(qū)動矩陣;W_SINS為慣性測量元件（inertial measurement unit， IMU）噪聲。F_t，G_t，W_SINS的具體形式可參考文獻(xiàn)［21］。

DVL/SINS松組合系統(tǒng)使用DVL提供的載體系三維速度作為量測值，其量測方程為

v～^d_SINS-v～^d=［-C～^b_n（v～ⁿ×）C～^b_n 0_3×9］X+w_DVL（3）

式中：“～”表示該值含有誤差;d表示DVL框架坐標(biāo)系;v～ⁿ×表示v～ⁿ的反對稱矩陣;v～^d為DVL三維速度;w_DVL為三維速度噪聲;v～^d_SINS為由經(jīng)過DVL誤差參數(shù)校正的SINS速度^［24-26］，其表達(dá)式為

v～^d_SINS=（1+δk）C^d_b（C～^b_nv～ⁿ+ω^b_eb×l^b）（4）

式中：C～^b_n=（C～ⁿ_b）^T，C～ⁿ_b為SINS姿態(tài)矩陣;v～ⁿ為SINS計算的導(dǎo)航系速度;δk為DVL刻度系數(shù)誤差;l^b為DVL桿臂;C^b_d為DVL安裝偏差角η的方向余弦矩陣;e表示地球坐標(biāo)系;ω^b_eb為b系相對于e系的角速度在b系中的投影。

1.2 波束域緊組合濾波模型

在實際應(yīng)用中，為減弱載體機(jī)動對測速的不利影響，DVL常采用圖1所示的四波束詹納斯配置。緊組合系統(tǒng)使用波束速度v^D=［v^D₁，v^D₂，v^D₃，v^D₄］^T作為量測信息，其中D表示波束坐標(biāo)系，用于指示波束順序，v^D₁～v^D₄分別表示波束1～波束4的波束速度。

根據(jù)波束的幾何構(gòu)型，DVL速度v^d與v^D的關(guān)系為

v^D=Mv^d=cos αsin βcos αcos β－sin α

cos αcos β－cos αsin β－sin α

－cos αsin β－cos αcos β－sin α

－cos αcos βcos αsin β－sin αv^d（5）

式中：α為波束傾角;β與波束幾何構(gòu)型相關(guān)，在“+”型配置中β為0°，“×”型配置中β為45°。

由式（5），得緊組合量測方程為

Mv～^d_SINS-v～^D=［-MC～^b_n（v～ⁿ×） MC～^b_n0_4×9］X+w^D（6）

式中：w^D為波束速度噪聲。

DVL/SINS松組合與緊組合數(shù)據(jù)融合具體流程如圖2所示。

2 松緊組合的等價性分析

當(dāng)前研究普遍認(rèn)為使用三維載體系速度的松組合系統(tǒng)相當(dāng)于對四維波束信息進(jìn)行了降維處理。因此，在四波束可用情況下緊組合的精度比松組合更高^{［16，18-20，22］}。本節(jié)證明3個或4個波束可用情況下松緊組合定位精度的等價性，并給出松組合量測協(xié)方差陣的具體確定方法。

由式（6），將緊組合量測方程改寫為

v～^D=Mv^d+w^D（7）

對矩陣M進(jìn)行初等行變換：

可知，rank（M^TM）=rank（M）=3，rank（·）表示對括號內(nèi)矩陣求秩。一方面，在式（7）兩側(cè)同時左乘（M^TM）^-1M^T，可得

（M^TM）^-1M^Tv～^D=v^d+（M^TM）^-1M^Tw^D（9）

另一方面，將v^b作待求參數(shù)，v～^D作量測值，利用式（5）列最小二乘方程：

V=Mv^d－v～^D（10）

式中：V為最小二乘的殘差向量。

DVL各波束使用獨(dú)立的換能器發(fā)射和接收聲波，因此將v～^D視為等權(quán)獨(dú)立觀測向量，即協(xié)方差陣Σ_w^D=σ²I_4×4，其中σ²為波束噪聲的方差。根據(jù)參數(shù)平差原理，由式（10）解得三維速度^［27］：

v^^d=（M^TM）^－1M^Tv～^D（11）

式中：v^^d為根據(jù)最小二乘原理計算的三維速度。

對比式（9）和式（11）可見，式（9）左側(cè)即為由波束速度計算的三維載體系速度。將式（11）代入式（9），緊組合量測方程轉(zhuǎn)換為

v^^d=v^d+（M^TM）^-1M^Tw^D（12）

整理式（12），緊組合量測方程便轉(zhuǎn)換為松組合量測方程：

v～^d_SINS-v^d=［-C～^b_n（v～ⁿ×） C～^b_n 0_3×9］X+w^d（13）

式中：w^d=－（M^TM）^－1M^Tw^D，根據(jù)協(xié)方差陣傳播定律：

Σ_w^d=σ²（M^TM）^－1M^TM（M^TM）^－1=σ²（M^TM）^－1（14）

因此，在準(zhǔn)確設(shè)置協(xié)方差陣的前提下，3個或4個波束可用情況下的DVL/SINS的緊組合系統(tǒng)與最小二乘速度輔助的松組合系統(tǒng)的定位精度完全等價，下文的實驗也驗證了這一結(jié)論。

受圖3所示的海洋環(huán)境的影響，DVL可能發(fā)生多個波束故障，具體情況包括：① UUV經(jīng)過海溝時，離底深度超過DVL最大測量距離，導(dǎo)致部分波束無法到達(dá)海底;② UUV經(jīng)過淤泥等強(qiáng)吸聲底質(zhì)時，波束被吸收，無法反射;③ 波束傳播中被魚群等海洋生物遮擋^［28］。在實際應(yīng)用中，DVL通過獨(dú)立的換能器發(fā)射和接收波束。當(dāng)波束發(fā)生故障時，對應(yīng)的換能器未接收到反射波，便在DVL數(shù)據(jù)流中標(biāo)記換能器狀態(tài)，以此確定發(fā)生故障的波束位置。當(dāng)可用波束數(shù)量為1或2時，v～^D包含的信息少于3維，無法計算載體系速度，此時松組合導(dǎo)航將退化為純慣性導(dǎo)航，而緊組合仍可利用存在的波束信息進(jìn)行組合導(dǎo)航解算。因此，緊組合的優(yōu)勢在于在1個或2個波束可用的情況下，增強(qiáng)DVL/SINS組合導(dǎo)航系統(tǒng)的魯棒性。

3 虛擬波束輔助的緊組合算法

隨著故障波束數(shù)量的增加，DVL提供的測量信息逐漸減少，導(dǎo)致緊組合導(dǎo)航系統(tǒng)的精度降低。構(gòu)建虛擬波束是提升波束故障條件下緊組合精度的有效措施。文獻(xiàn)［21］指出，基于波束幾何構(gòu)型的直接速度重構(gòu)方法忽略了載體的運(yùn)動狀態(tài)，同時也未考慮DVL的安裝偏差角，并利用SINS速度構(gòu)建虛擬波束。該算法認(rèn)為SINS的側(cè)向速度和垂向速度無誤差，導(dǎo)致波束速度隨時間延長逐漸發(fā)散。本文充分考慮DVL誤差參數(shù)，利用UUV的側(cè)向速度約束和壓力深度計量測值構(gòu)建虛擬波束，有效提升了虛擬波束精度。

根據(jù)圖1所示波束的對稱性，將波束故障情況分為如下4種情形：① 三波束可用;② 兩垂直波束可用;③ 兩平行波束可用;④ 單波束可用。

（1） 三波束可用（以D1，D2，D3可用為例）

當(dāng)三波束可用時，將故障的v～^D₄視為其他3個波束的線性組合^［22-23］：

v～^D₄=v～^D₁+v～^D₃－v～^D₂（15）

（2）兩垂直波束可用（以D1和D2可用為例）

在兩垂直波束可用情況下，由于可用波束信息小于3維，因此需要輸入外部信息，才能構(gòu)造虛擬波束?；赟INS信息的波束重構(gòu)方法認(rèn)為安裝偏差角標(biāo)定準(zhǔn)確，將SINS速度作為輸入，構(gòu)造虛擬波束D3和D4：

式中：v～^b_z，sins為由SINS導(dǎo)航參數(shù)得到的垂向速度。

將式（16）代入式（5），整理可得

A₁［v～^D₁v～^D₂v～^b_z，sins］^T=Mv^b（17）

式中：

由rank（A^T₁A₁）=rank（A₁）=3，式（17）兩側(cè)同時左乘（A^T₁A₁）^-1A^T₁可得

從式（19）可見，其前兩行為可用波束的量測方程，第3行表示v～^b_z，sins作為SINS垂向速度的量測值。而使用v～^b_z，sins修正v～^b_z，sins等價于不修正v～^b_z，sins，因此必然導(dǎo)致SINS垂向速度發(fā)散，進(jìn)而導(dǎo)致波束速度發(fā)散。

本文使用UUV普遍搭載的PS作為外部輸入。對PS相鄰歷元量測值進(jìn)行差分，獲得近似的^［18］天向速度：

v～_PS=h₂－h(huán)₁T_PS（20）

式中：h₁，h₂分別是PS上一歷元和當(dāng)前歷元量測值;T_PS為PS的采樣間隔。

由天向速度和載體系速度的關(guān)系可知：

式中：θ～和γ～分別是SINS解算的俯仰角和橫滾角。

在實際應(yīng)用中，考慮DVL存在安裝偏差角、桿臂和刻度系數(shù)誤差，使用已準(zhǔn)確標(biāo)定的DVL誤差參數(shù)補(bǔ)償v^d，得到載體系速度^［29-30］：

v^b=11+δkC^b_dv^d－ω^b_eb×l^b（22）

若誤差參數(shù)未標(biāo)定，可在濾波模型中將其增廣為狀態(tài)參數(shù)，使用上一時刻誤差參數(shù)的后驗估值補(bǔ)償v^d。

將式（22）代入式（21）并整理可得

v～^b_PS=A_PSC^b_dv^d（23）

式中：v～^b_PS=（1+δk）［v～_PS+A_PS（ω^b_eb×）l^b］。

由可用波束和v～^b_PS列線性方程組可得

由rank（A₂）=3，在式（24）兩側(cè)同時左乘A^－1₂：

v^d=A^－1₂［v～^b_PSv^D₁v^D₂］^T（25）

將式（25）代入式（5）后兩行得到虛擬波束：

式中：M_3，4為矩陣M的第3行和第4行。

（3） 兩平行波束可用

在兩平行波束可用情況下，若DVL為“+”型配置，當(dāng)可用波束為D1和D3時，v～^D₁和v～^D₃僅包含前向和垂向速度信息，需要外部提供側(cè)向速度信息?；赟INS信息的波束構(gòu)建方法將v～^b_x，sins作為輸入，等價于在組合導(dǎo)航中不修正v～^b_x，sins。證明過程同兩垂直波束可用的情況，在此不再贅述。其他波束故障情況同理。

該情況下，考慮UUV巡航時側(cè)向速度較小，令v^b_x=0，將式（22）展開，由v^b_x對應(yīng)式（22）的第一行可得：

0=11+δk［1η_z－η_y］v^d－［0－ω^b_eb，zω^b_eb，y］l^b（27）

整理得到：

v^b_l=［1η_z－η_y］v^d（28）

式中：v^b_l=（1+δk）（ω^b_eb，yl^b_z－ω^b_eb，zl^b_y）。

結(jié)合式（28），和由式（5）的第1行、第3行列線性方程組可得：

在式（29）兩側(cè)同時左乘A^－1₃可得：

v^d=A^－1₃［v^b_lv～^D₁v～^D₃］^T（30）

將式（30）代入式（5）的第2行、第4行中，得到故障波束速度：

若“+”型配置可用波束為D2和D4，可用波束中不含前向速度信息，無法構(gòu)建虛擬波束。對于“×”型配置，任意平行波束均包含前向、側(cè)向、垂向速度信息，只需將v～^b_PS或v^b_l代入便可求解另兩維速度，進(jìn)而通過式（5）計算故障波束。在實際航行中，UUV側(cè)向速度不一定嚴(yán)格為0，為增強(qiáng)虛擬波束的魯棒性，選擇v～^b_PS作為外部輸入，計算過程與兩垂直波束可用時相同。

（4） 單波束可用

當(dāng)單波束可用時需要外部輸入2個維度的速度信息。對于“+”型配置，若可用波束為D1或D3，波束信息僅包含前向速度信息。以D1可用為例，利用v～_PS和側(cè)向速度約束，由式（5）第1行可得：

式（32）兩側(cè)同時左乘A^－1₄得

v^d=A^－1₄［v^b_psv^b_lv^D₁］^T（33）

將式（33）代入式（5），得到故障波束速度為

若可用波束為D2或D4，波束信息中不含前向速度信息，無法構(gòu)建虛擬波束。對于“×”型配置，任意一個波束可用時，虛擬波束構(gòu)建過程與D1或D3可用時的“+”型配置相同。綜上所述，波束故障時的虛擬波束構(gòu)建方案如圖4所示，圖中陰影部分表示“+”型配置。結(jié)合虛擬波束和可用波束，所提算法可在上述波束故障情況下可為SINS提供連續(xù)的四波束信息。

在虛擬波束輔助的DVL/SINS緊組合系統(tǒng)中，由于UUV航行時不一定完全滿足側(cè)向速度等于0的假設(shè)，虛擬波束不能反映準(zhǔn)確的波束速度，同時由PS差分獲得的天向速度與真實速度之間也存在誤差。因此，當(dāng)海況復(fù)雜時可適當(dāng)放大量測噪聲，以降低未知虛擬波束誤差對組合導(dǎo)航的影響。綜上所述，虛擬波束輔助的DVL/SINS緊組合系統(tǒng)流程如圖5所示。

4 試驗與分析

為驗證所提算法的有效性，使用2022年8月某海域的UUV實測數(shù)據(jù)進(jìn)行試驗驗證。UUV平臺及海試場景如圖6所示，試驗軌跡如圖7所示。平臺搭載了華信HX-CH4601A型GNSS天線、中船航海MFG-IIIU型光纖IMU和Nortek 1 MHz DVL，各傳感器的具體參數(shù)如表1所示。試驗中，在岸邊搭設(shè)GNSS基準(zhǔn)站，使用RTK/SINS組合導(dǎo)航結(jié)果為本試驗提供厘米級的參考位置。試驗中，DVL與IMU間的誤差參數(shù)已精確標(biāo)定，其中桿臂為［0，0.188，-0.082］m，刻度系數(shù)誤差為0.36%，三軸安裝偏差角均為0°，DVL采用“×”型配置，得到的波束速度如圖8所示。

根據(jù)波束幾何構(gòu)型的對稱性，假設(shè)DVL軌跡的1 000～1 500 s發(fā)生不同數(shù)量的波束故障，具體情況如表2所示（“√”表示波束可用，“×”表示波束故障），分別設(shè)計如下4種方案進(jìn)行對比驗證：① 松組合算法;② 緊組合算法;③ 文獻(xiàn)［23］提出的基于SINS速度的虛擬波束輔助緊組合（SINS-aided tight couple， SINS-TC）算法;④ 本文提出的基于側(cè)向速度NHC和PS觀測量的虛擬波束輔助緊組合（non-holonomic constraint and PS-aided tight couple， NHC-PS-TC）算法。

首先，設(shè)置松組合方案LC1（Σ_w^d=σ²I_3×3），LC2（Σ_w^d=σ²（M^TM）^－1）和緊組合方案，驗證3個或4個波束可用條件下松組合與緊組合定位精度的等價性。圖9顯示了情況1下3種方案的位置誤差曲線。

如圖9所示，在正確設(shè)置協(xié)方差的情況下，LC2的位置誤差與TC完全一致。由于僅考慮了協(xié)方差陣對角線元素，LC1與TC的位置誤差曲線有明顯差別。在四波束可用的正常區(qū)間內(nèi)，LC1、LC2和TC的位置均方根誤差（root mean square error， RMSE）分別為9.003 m、8.551 m和8.551 m。LC1的位置誤差大于LC2和TC，LC2與TC完全一致。與四波束可用情況相同，在三波束可用的故障區(qū)間內(nèi)，LC1、LC2和TC的位置RMSE分別為8.952 m、8.892 m和8.892 m。因此，在正確設(shè)置松組合中三維速度協(xié)方差陣的前提下，松組合和緊組合的定位精度完全一致，未準(zhǔn)確設(shè)置協(xié)方差陣會降低松組合的定位精度。

為驗證改進(jìn)虛擬波束構(gòu)建算法的有效性，圖10～圖12對比了情況1～情況3中SINS-TC和NHC-PS-TC構(gòu)建的虛擬波束速度。由于情況1中兩種算法的虛擬波束構(gòu)建策略相同，在圖10中僅顯示一條虛擬波束曲線。由圖10可知，與仿真試驗一致，在三波束可用情況下，虛擬波束可以準(zhǔn)確跟蹤真實波束，虛擬波束與DVL波束之差的RMSE為10^-17量級，可忽略不計。

從圖11和圖12可見，在情況2和情況3下，受SINS誤差影響，SINS-TC的虛擬波束的誤差隨時間延長而逐漸累積。尤其在單波束可用的情況3中，波束2和波束4迅速發(fā)散，無法為SINS提供準(zhǔn)確的波束速度參考。本文所提出的NHC-PS-TC基于NHC假設(shè)和PS提供的側(cè)向和天向速度約束計算虛擬波束，由于不依賴SINS的輸出，因此在故障區(qū)間有著一致的波束估計效果。但從圖11和圖12也可以看到，由于UUV運(yùn)動不一定滿足NHC假設(shè)，以及由PS差分得到的天向速度存在誤差，導(dǎo)致所提算法構(gòu)建的虛擬波束存在一定程度的偏差。但與現(xiàn)有算法相比，所提算法已顯著提升了虛擬波束的精度。

進(jìn)一步地，表3給出了虛擬波束與DVL波束之差的統(tǒng)計結(jié)果。在情況2下，NHC-PS-TC的波束3和波束4的RMSE為0.020 m/s，較SINS-TC減少了62%;在情況3下，NHC-PS-TC的波束2～波束4的RMSE分別為0.040 m/s、0.043 m/s和0.022 m/s，較SINS-TC分別減少了61%、31%和83%。因此，所提算法提升了一或兩個波束可用情況下的虛擬波束精度，可為SINS提供更加準(zhǔn)確的波束量測信息。

圖13對比了故障區(qū)間內(nèi)不同波束可用情況下TC、SINS-TC和NHC-PS-TC的位置誤差。從圖中可見，在三波束可用情況中，在相同虛擬波束輔助下，SINS-TC和NHC-PS-TC的定位誤差相同，且較三波束可用時的TC算法精度更高;在二波束可用情況中，SINS-TC算法在不準(zhǔn)確波束的輔助下，提高組合導(dǎo)航系統(tǒng)定位精度的能力受限;在單波束可用情況中，SINS-TC受SINS側(cè)向速度發(fā)散影響，在較大誤差的虛擬波束輔助下，定位誤差迅速發(fā)散。而所提算法在不同波束可用情況下均可有效補(bǔ)償故障波束的影響，顯著提升緊組合系統(tǒng)的定位精度。

表4列出了不同情況下3種算法故障區(qū)間定位誤差的統(tǒng)計結(jié)果。從表中可見，在1～3個波束可用情況下，所提算法的定位誤差RMSE分別為9.016 m、8.065 m和5.622 m，較TC分別減小了67%、26%和37%，較SINS-TC分別減小了75%、20%和0%。因此，所提算法在多個波束故障情況下較SINS-TC有更高的定位精度，實測數(shù)據(jù)驗證了所提算法的有效性和可行性。

5 結(jié) 論

為提高DVL/SINS緊組合導(dǎo)航系統(tǒng)的定位精度，系統(tǒng)研究了不同波束故障情況下的DVL/SINS緊組合算法，證明了3個或4個波束可用情況下松組合的定位精度與緊組合系統(tǒng)具有等價性，并給出松組合系統(tǒng)中量測協(xié)方差陣的具體確定方法。針對波束故障情況，基于UUV非完整性約束條件和PS量測，提出了改進(jìn)的虛擬波束構(gòu)建方案，建立了虛擬波束輔助下的DVL/SINS緊組合系統(tǒng)。通過UUV實測數(shù)據(jù)驗證了本文方法的正確性。試驗結(jié)果表明，3個或4個波束的緊組合與松組合的定位精度完全相同;改進(jìn)的虛擬波束構(gòu)建算法提高了波束預(yù)測精度，有效提升了緊組合系統(tǒng)的定位精度。

參考文獻(xiàn)

［1］KARMOZDI A， HASHEMI M， SALARIEH H， et al. INS-DVL navigation improvement using rotational motion dynamic model of AUV［J］. IEEE Sensors Journal， 2020， 20（23）： 14329-14336.

［2］FU Q W， LIU Y， LIU Z B， et al. Autonomous in-motion alignment for land vehicle strapdown inertial navigation system without the aid of external sensors［J］. Journal of Navigation， 2018， 71（6）： 1312-1328.

［3］PANDA J P， MITRA A， WARRIOR H V. A review on the hydrodynamic characteristics of autonomous underwater vehicles［J］. Proceedings of the Institution of Mechanical Engineers Part M-Journal of Engineering for the Maritime Environment， 2021， 235（1）： 15-29.

［4］STUTTERS L， LIU H， TILTMAN C， et al. Navigation technologies for autonomous underwater vehicles［J］. IEEE Trans.on Systems， Man， and Cybernetics， Part C （Applications and Reviews）， 2008， 38（4）： 581-589.

［5］ZHANG T W， TANG J L， QIN S J， et al. Review of navigation and positioning of deep-sea manned submersibles［J］. Journal of Navigation， 2019， 72（4）： 1021-1034.

［6］MAURELLI F， KRUPIN＇SKI S， XIANG X， et al. AUV locali-sation： a review of passive and active techniques［J］. International Journal of Intelligent Robotics and Applications， 2021， 6（2）： 817-828.

［7］靳凱迪， 柴洪洲， 宿楚涵， 等. DVL/SINS組合導(dǎo)航技術(shù)發(fā)展現(xiàn)狀及趨勢［J］. 導(dǎo)航定位學(xué)報， 2022， 10（2）： 1-12， 92.

JIN K D， CHAI H Z， SU C H， et al. Research status and trends of DVL/SINS integrated navigation technology［J］. Journal of Navigation and Positioning， 2022， 10（2）： 1-12， 92.

［8］SAHOO A， DWIVEDY S， ROBI P. Advancements in the field of autonomous underwater vehicle［J］. Ocean Engineering， 2019， 181： 145-160.

［9］ZHU J P， LI A， QIN F J， et al. A hybrid method for dealing with DVL faults of SINS/DVL integrated navigation system［J］. IEEE Sensors Journal， 2022， 22（16）： 15844-15854.

［10］BEN Y Y， ZANG X L， LI Q， et al. A dual-state filter for a relative velocity aiding strapdown inertial navigation system［J］. IEEE Trans.on Instrumentation and Measurement， 2021， 70： 1000215.

［11］靳凱迪， 柴洪洲， 宿楚涵， 等. 基于狀態(tài)變換卡爾曼濾波的DVL/SINS組合導(dǎo)航算法［J］. 系統(tǒng)工程與電子技術(shù)， 2023， 45（11）： 3624-3631.

JIN K D， CHAI H Z， SU C H， et al. State transformation Kalman filter for DVL/SINS integral navigation system［J］. Systems Engineering and Electronics， 2023， 45（11）： 3624-3631.

［12］MARTINEZ A， HERNANDEZ L， SAHLI H， et al. Model-aided navigation with sea current estimation for an autonomous underwater vehicle［J］. International Journal of Advanced Robotic Systems， 2015， 12（7）： 103-116.

［13］SHI W C， XU J N， HE H Y， et al. Fault-tolerant SINS/HSB/DVL underwater integrated navigation system based on variational Bayesian robust adaptive Kalman filter and adaptive information sharing factor［J］. Measurement， 2022， 196： 11125.

［14］TAL A， KLEIN I， KATZ R. Inertial navigation system/Doppler velocity log （INS/DVL） fusion with partial DVL measurements［J］. Sensors， 2017， 17（2）： 415-434.

［15］ELIAV R， KLEIN I. INS/Partial DVL measurements fusion with correlated process and measurement noise［J］. Proceedings， 2019， 4（1）： 34-39.

［16］馬小艷， 陸明泉， 張輝， 等. 基于波束域信息的SINS/DVL組合導(dǎo)航算法［J］. 中國慣性技術(shù)學(xué)報， 2021， 29（2）： 164-170.

MA X Y， LU M Q， ZHANG H， et al. Algorithm of SINS/DVL integrated navigation based on echo beam［J］. Journal of Chinese Inertial Technology， 2021， 29（2）： 164-170.

［17］YOO T， KIM M， YOON S， et al. Performance enhancement for conventional tightly coupled INS/DVL navigation system using regeneration of partial DVL measurements［J］. Journal of Sensors， 2020， 2020： 5324349.

［18］LIU P J， WANG B， DENG Z H， et al. INS/DVL/PS tightly coupled underwater navigation method with limited DVL measurements［J］. IEEE Sensors Journal， 2018， 18（7）： 2994-3002.

［19］WANG D， XU X S， HOU L H. An improved adaptive Kalman filter for underwater SINS/DVL system［J］. Mathematical Problems in Engineering， 2020， 2020： 5456961.

［20］LIU S D， ZHANG T， ZHANG J Y， et al. A new coupled method of SINS/DVL integrated navigation based on improved dual adaptive factors［J］. IEEE Trans.on Instrumentation and Measurement， 2021， 70： 8504211.

［21］JIN K D， CHAI H Z， SU C H， et al. A compensation algorithm with motion constraint in DVL/SINS tightly coupled positioning［J］. Marine Geodesy， 2022， 45（4）： 380-406.

［22］WANG D， XU X S， YAO Y Q， et al. A novel SINS/DVL tightly integrated navigation method for complex environment［J］. IEEE Trans.on Instrumentation and Measurement， 2020， 69（7）： 5183-5196.

［23］徐博， 王連釗， 李盛新. 量測野值與波束故障條件下SINS/DVL緊組合導(dǎo)航方法［J］. 中國慣性技術(shù)學(xué)報， 2021， 29（6）： 746-755.

XU B， WANG L Z， LI S X. Improved huber robust filter SINS/DVL compact combination fault processing method based on beam reconstruction［J］. Journal of Chinese Inertial Technology， 2021， 29（6）： 746-755.

［24］LI D， XU J N， ZHU B， et al. A calibration method of DVL in integrated navigation system based on particle swarm optimization［J］. Measurement， 2022， 187： 110325.

［25］XU B， WANG L Z， LI S X， et al. A novel calibration method of SINS/DVL integration navigation system based on quaternion［J］. IEEE Sensors Journal， 2020， 20（16）： 9567-9580.

［26］LIU J X， LI H B， MA S L， et al. Simultaneous calibration method for Doppler velocity log errors based on a genetic algorithm［J］. IEEE Sensors Journal， 2022， 22（10）： 9558-9567.

［27］隋立芬， 宋力杰， 柴洪洲， 等. 誤差理論與測量平差基礎(chǔ)［M］. 2版. 北京： 測繪出版社， 2016： 60-65.

SUI L F， SONG L J， CHAI H Z， et al. Error theory and foundation of surveying adjustment［M］. 2nd ed. Beijing： Surveying and Mapping Publishing House， 2016： 60-65.

［28］JIN K D， CHAI H Z， SU C H， et al. A performance-enhanced DVL/SINS integrated navigation system based on data-driven approach［J］. Measurement Science and Technology， 2023， 34（9）： 095120.

［29］KLEIN I， DIAMANT R. Observability analysis of heading aided ins for a maneuvering AUV［J］. Navigation， 2018， 65（1）： 73-82.

［30］嚴(yán)恭敏， 鄧瑀. 傳統(tǒng)組合導(dǎo)航中的實用Kalman濾波技術(shù)評述［J］. 導(dǎo)航定位與授時， 2020， 7（2）： 50-64.

YAN G M， DENG Y. Review on practical Kalman filtering techniques in traditional integrated navigation system［J］. Navigation Positioning and Timing， 2020， 7（2）： 50-64.

作者簡介

靳凱迪（1997—），男，博士研究生，主要研究方向為水下無人航行器自主定位理論與方法。

柴洪洲（1969—），男，教授，博士，主要研究方向為大地測量數(shù)據(jù)處理、水下導(dǎo)航定位。

宿楚涵（1997—），女，助理工程師，碩士，主要研究方向為組合導(dǎo)航數(shù)據(jù)處理。