張 濤，胡賀慶

（1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096；

2. 東南大學(xué) 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096）

一種改進(jìn)的水下三陣元輔助慣導(dǎo)系統(tǒng)的定位算法

張 濤，胡賀慶

（1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096；

2. 東南大學(xué) 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096）

針對(duì)水下慣導(dǎo)系統(tǒng)定位誤差隨時(shí)間發(fā)散的缺陷，設(shè)計(jì)了一種基于DVL、聲學(xué)定位輔助慣導(dǎo)的組合導(dǎo)航系統(tǒng)。聲學(xué)定位系統(tǒng)由安裝在水下潛器上的換能器陣和布放在海底的三陣元定位基陣組成，傳統(tǒng)長(zhǎng)基線定位水下至少要布放四個(gè)陣元，三個(gè)陣元無(wú)法直接定位求解。根據(jù)三陣元測(cè)得的距離信息和慣導(dǎo)系統(tǒng)輸出的位置信息建立距離耦合模型，給出了慣導(dǎo)、DVL和聲學(xué)定位系統(tǒng)的狀態(tài)方程和量測(cè)方程；同時(shí)針對(duì)水下聲速垂直分布特性，提出了一種等效聲速的修正方法。仿真結(jié)果表明：在三陣元定位基陣作用范圍內(nèi)，組合導(dǎo)航系統(tǒng)能有效修正慣導(dǎo)位置誤差，定位誤差小于4 m，具有良好的適用性。

水下潛器；慣導(dǎo)系統(tǒng)；三陣元基陣；距離耦合

21世紀(jì)將是海洋的世紀(jì)，隨著對(duì)海洋中生物和礦物資源的需求增大，水下定位導(dǎo)航技術(shù)也成為當(dāng)今研究的重點(diǎn)。水下潛器在海洋數(shù)據(jù)采集、災(zāi)難預(yù)警、戰(zhàn)場(chǎng)監(jiān)視和礦產(chǎn)探測(cè)等方面發(fā)揮著重要的作用，具有活動(dòng)范圍大、潛水深度深、體積小、成本優(yōu)等諸多優(yōu)點(diǎn)[1-2]。為了確保潛器水下任務(wù)的順利完成，就需要通過(guò)一定的技術(shù)對(duì)其進(jìn)行精確定位導(dǎo)航。慣性導(dǎo)航系統(tǒng)因其隱蔽性、自主性強(qiáng)以及能獲取載體完備運(yùn)動(dòng)信息的獨(dú)特優(yōu)點(diǎn)而在水下導(dǎo)航中得到廣泛應(yīng)用，但由于慣性導(dǎo)航系統(tǒng)在長(zhǎng)時(shí)間工作之后，導(dǎo)航誤差會(huì)隨著時(shí)間累積而累積增大，從而使得慣導(dǎo)的長(zhǎng)時(shí)間精確導(dǎo)航成為一個(gè)需要突破的難題。為了實(shí)現(xiàn)長(zhǎng)航時(shí)高精度的導(dǎo)航目標(biāo)，需要進(jìn)行誤差補(bǔ)償或利用外界位置信息對(duì)其進(jìn)行周期的調(diào)整和校正，從而提高其精度[3-5]。

在水下環(huán)境中，電磁波信號(hào)具有嚴(yán)重衰減的特性；在深?；蛘弑嫦?，采用GPS導(dǎo)航及其他無(wú)線電定位方式會(huì)使用較長(zhǎng)的時(shí)間來(lái)浮出水面，甚至可能暴露自己的當(dāng)前位置，極大程度的降低了潛器的隱蔽性以及對(duì)信息采集的實(shí)時(shí)性。上述水下環(huán)境中遭遇的困境嚴(yán)重限制了衛(wèi)星導(dǎo)航系統(tǒng)或其它無(wú)線電導(dǎo)航系統(tǒng)在水下航行器中的應(yīng)用。聲波在海洋中的傳播相比其他任何物理場(chǎng)能夠傳播更遠(yuǎn)的距離，具有明顯的優(yōu)勢(shì)。隨著聲學(xué)定位技術(shù)的進(jìn)步，水聲高精度定位技術(shù)有了長(zhǎng)足的進(jìn)步。水下長(zhǎng)基線系統(tǒng)基線長(zhǎng)，定位精度高，但是一般布放的基陣個(gè)數(shù)較多，因而布放、校準(zhǔn)和回收需要較長(zhǎng)時(shí)間，且這些作業(yè)過(guò)程較為復(fù)雜[6-10]。

針對(duì)水下慣導(dǎo)系統(tǒng)誤差存在隨時(shí)間累積、GPS系統(tǒng)無(wú)法在水下定位以及傳統(tǒng)長(zhǎng)基線系統(tǒng)復(fù)雜等問(wèn)題，本文設(shè)計(jì)了一種基于 DVL和三陣元定位輔助慣導(dǎo)的組合導(dǎo)航系統(tǒng)，利用慣導(dǎo)位置定位信息，建立了慣導(dǎo)系統(tǒng)和三陣元定位基陣的距離耦合模型，同時(shí)針對(duì)水下聲速垂直分布特性，提出了一種等效聲速的修正方法。仿真結(jié)果表明，該方法有效可行，能夠在保證導(dǎo)航精度的前提下，簡(jiǎn)化聲學(xué)定位系統(tǒng)，降低成本，具有一定的實(shí)際可用性。

1 系統(tǒng)原理與結(jié)構(gòu)

1.1 三陣元基陣布放和校準(zhǔn)

水下三陣元基陣精確定位的前提是陣元的絕對(duì)地理位置已知，且得到準(zhǔn)確校準(zhǔn)，常利用超短基線測(cè)出陣元在聲學(xué)基陣坐標(biāo)系下的位置，通過(guò)坐標(biāo)轉(zhuǎn)化，可以得到陣元的絕對(duì)地理位置。校準(zhǔn)原理可用下式表示：

式中：Ps為陣元的絕對(duì)地理坐標(biāo)，為目標(biāo)所求；PGPS為測(cè)量船的位置，可以由GPS精確定位；p為大地坐標(biāo)系下陣元相對(duì)測(cè)量船的位置坐標(biāo)。

如圖1所示，測(cè)量船上安裝有GPS、高精度慣性測(cè)量單元、羅經(jīng)系統(tǒng)，用于提供測(cè)量船的精確地理位置和姿態(tài)角信息。測(cè)量船上安裝有超短基線系統(tǒng)，聲學(xué)基陣接收陣元發(fā)出的信號(hào)，根據(jù)超短基線測(cè)距側(cè)向原理計(jì)算出陣元在聲學(xué)基陣坐標(biāo)系下的位置坐標(biāo)為。設(shè)地理坐標(biāo)系為n系，聲學(xué)基陣坐標(biāo)系為a系，載體坐標(biāo)系為b系，由聲學(xué)基陣坐標(biāo)系到載體坐標(biāo)系的坐標(biāo)轉(zhuǎn)換可以得到陣元在載體坐標(biāo)系下的位置，δp為基陣坐標(biāo)系在載體坐標(biāo)系的安裝誤差，為聲學(xué)基陣坐標(biāo)系到載體坐標(biāo)系的變換矩陣。由載體坐標(biāo)系到地理坐標(biāo)系的坐標(biāo)變換可以得到在大地坐標(biāo)系下陣元相對(duì)測(cè)量船的位置，由式(1)即可得到陣元的絕對(duì)地理位置。

圖1 陣元校準(zhǔn)定位模型Fig.1 Array element calibration positioning model

1.2 三陣元基陣定位原理

水下定位系統(tǒng)模型主要有球面交匯模型和雙曲面交匯模型，在實(shí)際情況中，一般很少用雙曲線交匯模型，而采用高精度的同步式球面交匯定位模型，主要通過(guò)計(jì)算信號(hào)的傳播時(shí)間和聲速的乘積來(lái)獲得水下潛器和陣元的距離，然后解算方程組來(lái)對(duì)目標(biāo)進(jìn)行定位。定位模型如圖2所示。

圖2 三陣元定位模型Fig.2 Three-element array positioning model

2 距離耦合組合導(dǎo)航模型

將式(2)中方程減消去二次項(xiàng)，得到如下線性方程：

由于該方程組有3個(gè)未知數(shù)，可知采用三陣元基陣定位系統(tǒng)，當(dāng)水下潛器深度未知時(shí)，不能得到準(zhǔn)確解，此時(shí)需要增加陣元的個(gè)數(shù)，至少需要4個(gè)陣元，才能得到唯一解。特別需要注意的是，當(dāng)有較大測(cè)距誤差時(shí)，四陣元基陣定位結(jié)果誤差較大，可以通過(guò)增加陣元個(gè)數(shù)，計(jì)算方程組的最小二乘解得到定位結(jié)果。當(dāng)陣元大于4個(gè)時(shí)，將式(3)表示成矩陣形式：

由該方程組根據(jù)最小二乘法得到最佳解為

增加陣元數(shù)量能有效提高聲學(xué)定位的精度，但是水下陣元布放、校準(zhǔn)和回收需要較長(zhǎng)的時(shí)間，耗費(fèi)較大的財(cái)力和物力，同時(shí)陣元數(shù)量的增加使得后端處理系統(tǒng)變得復(fù)雜。本文在在三陣元基陣定位基礎(chǔ)上，利用慣導(dǎo)給出的位置定位信息，建立了慣導(dǎo)系統(tǒng)和三陣元基陣定位的距離緊耦合組合導(dǎo)航模型，避免了三陣元基陣定位求解模糊問(wèn)題，同時(shí)能夠有效修正慣導(dǎo)位置誤差。

2.1 距離耦合組合導(dǎo)航模型狀態(tài)方程

距離耦合組合導(dǎo)航算法誤差狀態(tài)模型主要包括慣導(dǎo)系統(tǒng)的誤差模型和三陣元基陣定位誤差模型。慣導(dǎo)系統(tǒng)誤差模型主要是速度誤差、姿態(tài)誤差、位置誤差、陀螺漂移、加表誤差。三陣元基陣定位主要是距離測(cè)量誤差，在三陣元基陣同步定位模型下，由于時(shí)鐘同步誤差、水下聲速垂直分布等特性將會(huì)引起距離測(cè)量有誤差，為簡(jiǎn)化模型，可將距離測(cè)量誤差用一階馬爾科夫過(guò)程表示，則距離誤差模型可表示為

式中：R為距離真值，δR為距離測(cè)量誤差，τ、ω分別是一階馬爾科夫過(guò)程的相關(guān)時(shí)間和驅(qū)動(dòng)白噪聲，υ為高斯白噪聲。狀態(tài)方程可表示為

即：

式中：X為狀態(tài)變量，F(xiàn)為狀態(tài)轉(zhuǎn)移矩陣，W為系統(tǒng)噪聲矩陣。

選擇速度誤差、姿態(tài)誤差、位置誤差、加速度計(jì)零偏和陀螺漂移作為狀態(tài)量：

式中：δVE、δVN、δVU分別是捷聯(lián)東向、北向、天向的速度誤差，Eφ、Nφ、Uφ分別是捷聯(lián)東向、北向、天向的失準(zhǔn)角，δL、δλ、δh分別是捷聯(lián)緯度、經(jīng)度、高度誤差，?bx、?by、?bz為捷聯(lián)加表三個(gè)軸向的偏置誤差，εbx、εby、εbz是捷聯(lián)陀螺的三個(gè)軸向漂移。F可由SINS誤差方程確定。

選擇三陣元基陣到每個(gè)陣元的距離誤差作為狀態(tài)量：

2.2 距離耦合組合導(dǎo)航模型觀測(cè)方程

導(dǎo)航系統(tǒng)觀測(cè)方程為

式中：VSINS是慣導(dǎo)系統(tǒng)輸出的速度信息，VDVL是DVL系統(tǒng)輸出的速度信息，RSINS是慣導(dǎo)系統(tǒng)解算的位置與每個(gè)陣元之間的距離信息，Racou是三陣元基陣定位系統(tǒng)輸出的距離信息。

式中：i=1,2,3。

相對(duì)水下航行器的真實(shí)位置(x,y,z)，將式(12)進(jìn)行泰勒線性化得：

由三陣元基陣定位系統(tǒng)距離誤差模型可知：

將式(18)中的δx、δy、δz用δL、δλ、δh表示為

由以上分析可得觀測(cè)矩陣為

3 等效聲速修正方法

由水下三陣元基陣定位原理可知，定位誤差不僅與測(cè)量時(shí)間誤差有關(guān)，還與水下聲速有關(guān)。一般假設(shè)水下聲速是恒定的，為1500 m/s，同時(shí)還假設(shè)聲音是沿著直線傳播的，但是這些假設(shè)都有一定的局限性，水下環(huán)境中，聲速隨著水下溫度、壓力以及鹽度不斷變化，從而導(dǎo)致測(cè)距產(chǎn)生誤差。本文提出一種基于卡爾曼濾波的組合導(dǎo)航系統(tǒng)輔助的等效聲速修正方法，該算法計(jì)算過(guò)程簡(jiǎn)單，能有效減小測(cè)距誤差。

4 系統(tǒng)仿真及分析

4.1 水下Bellhop仿真模型

一般來(lái)說(shuō)，水下聲信道是時(shí)變、空變的隨機(jī)信道，其必須用時(shí)變、空變的隨機(jī)濾波器來(lái)描述，但是在大多數(shù)實(shí)際應(yīng)用中，聲信道可以看作是緩慢時(shí)變的相干多途信道，若觀察或者處理的時(shí)間不是過(guò)分長(zhǎng)，則聲信道可以用時(shí)不變?yōu)V波器來(lái)描述。

水下聲音傳播可以簡(jiǎn)化為下式：

Bellhop是一個(gè)描述海洋環(huán)境中聲壓場(chǎng)的射線傳播模型。由于采用了射線傳播模型，使得針對(duì)海洋環(huán)境聲壓場(chǎng)獲得的算法變得簡(jiǎn)單易于實(shí)現(xiàn)，其中具體使用了Gaussian和hat-shaped射線的幾何特性以及物理傳播規(guī)律。Bellhop可以輸出包括聲波傳輸能量損失、本征聲線、到達(dá)時(shí)間序列等海洋中聲速傳播特征。同時(shí)利用環(huán)境文件對(duì)海洋中的聲速、聲源位置、水聽(tīng)器布放數(shù)量及位置等進(jìn)行設(shè)置，可以得到水下信道響應(yīng)。本次仿真實(shí)驗(yàn)中采用的環(huán)境文件為在某湖某區(qū)域采集的數(shù)據(jù)，聲速分布如圖3所示。

圖3 某湖區(qū)域聲速分布圖Fig.3 Sound velocity distribution in a lake area

4.2 距離耦合組合導(dǎo)航模型仿真

為了驗(yàn)證算法定位效果，在Matlab環(huán)境下，分別對(duì)本文的三陣元距離耦合算法和常用的五陣元最小二乘算法進(jìn)行仿真比較。為模擬實(shí)際海情，設(shè)水下航行器做三軸搖擺運(yùn)動(dòng)，搖擺參數(shù)如下：橫搖幅度為 8°，搖擺周期為 8 s；縱搖幅度為8°，搖擺周期為 10 s；航向搖擺幅度為 6°，搖擺周期為 6 s。陀螺儀隨機(jī)漂移和常值漂移均為 0.05 (°)/h，加速度計(jì)隨機(jī)漂移為50μg，常值偏置為50μg；初始失準(zhǔn)角分別為：縱搖角0.01°，橫搖角0.01°，航向角0.01°；DVL速度誤差為0.3 m/s；仿真時(shí)間為1500 s。

假設(shè)潛器初始位置為（119.995°, 32.995°），深度為10 m，以航向角30°且航速為1 m/s做勻速直線航行。由某湖試得到的水下聲速分布數(shù)據(jù)，按照 Bellhop模型模擬陣元接收到的信號(hào)，陣元地理位置為（120.01°, 40.01°）、（120.01°, 40.00°）、（120.02°, 40.01°）、（120.01°, 40.02°）、（120°, 40°），深度均為30 m。分別采用三陣元距離耦合算法和五陣元最小二乘算法輔助慣性導(dǎo)航系統(tǒng)進(jìn)行定位仿真。圖4為水下航行器理想軌跡及陣元基陣布放圖；圖5～7分別為東向誤差曲線對(duì)比圖、北向誤差曲線對(duì)比圖、高度誤差曲線對(duì)比圖。

圖4 水下航行器理想軌跡及陣元基陣布放圖Fig.4 Real track and element array

圖5 北向誤差曲線對(duì)比圖Fig.5 North error curves

圖6 東向誤差曲線比較圖Fig.6 East error curves

圖7 高度誤差曲線比較圖Fig.7 Height error curves

由圖 5～7可知：當(dāng)水下潛器僅采用慣導(dǎo)和 DVL組合導(dǎo)航系統(tǒng)時(shí)，水下潛器的位置誤差會(huì)隨著時(shí)間的推移產(chǎn)生發(fā)散；采用三陣元基陣定位時(shí)，組合導(dǎo)航系統(tǒng)的位置誤差在4 m以內(nèi)。本文提出的三陣元基陣距離耦合定位算法避免了在潛器深度未知時(shí)，位置求解模糊問(wèn)題，能夠在三陣元基陣定位范圍內(nèi)有效修正慣導(dǎo)位置誤差。增加水下陣元個(gè)數(shù)，采用五陣元最小二乘算法的組合導(dǎo)航系統(tǒng)位置誤差在2 m以內(nèi)，可見(jiàn)增加陣元的個(gè)數(shù)對(duì)水下導(dǎo)航系統(tǒng)的定位精度改善明顯，但是陣元個(gè)數(shù)的增加使聲學(xué)定位系統(tǒng)變得復(fù)雜，同時(shí)陣元布放、回收和校準(zhǔn)耗時(shí)耗力，經(jīng)濟(jì)適用性差。本文提出的三陣元基陣距離耦合定位算法，雖然精度不及五陣元定位精度高，但是系統(tǒng)簡(jiǎn)單實(shí)用，在三陣元基陣定位區(qū)域內(nèi)仍能有效修正慣導(dǎo)位置誤差，滿足水下航行要求。

5 結(jié) 論

本文針對(duì)水下三陣元基陣定位技術(shù)的缺陷，建立了慣導(dǎo)、DVL和三陣元基陣定位的距離耦合組合導(dǎo)航模型，詳細(xì)推導(dǎo)了距離耦合組合導(dǎo)航系統(tǒng)的狀態(tài)方程和觀測(cè)方程，同時(shí)，針對(duì)水下聲速垂直分布特性，提出了提出一種基于卡爾曼濾波的組合導(dǎo)航系統(tǒng)輔助的等效聲速修正方法。

該算法計(jì)算過(guò)程簡(jiǎn)單，能有效減小測(cè)距誤差。通過(guò)系統(tǒng)仿真結(jié)果說(shuō)明，三陣元基陣距離耦合算法能夠避免傳統(tǒng)三陣元基陣同步定位求解模糊問(wèn)題，同時(shí)在三陣元基陣定位范圍內(nèi)能夠有效修正慣導(dǎo)誤差，保證水下潛器的導(dǎo)航精度，具有良好的適用性。

（References）：

[1] Gill B, Schipf D, Borth R, et al. Hybrid baseline localization for portable AUV navigation[C]//Oceans ’13 MTS/IEEE San Diego Conference. 2013: 1-10.

[2] Paull L, Saeedi S, Seto M, et al. AUV navigation and localization: A review[J]. IEEE Journal of Oceanic Engineering, 2014, 39(1): 131-149.

[3] 張濤, 徐曉蘇, 李瑤, 等. 基于慣導(dǎo)及水下聲學(xué)輔助系統(tǒng)的AUV容錯(cuò)導(dǎo)航技術(shù)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2013, 21(4): 512-516. Zhang Tao, Xu Xiao-su, Li Yao, et al. AUV fault-tolerant technology based on inertial navigation and underwater acoustics assisted navigation system[J]. Journal of Chinese Inertial Technology, 2013, 21(4): 512-516.

[4] Larsen M B. High performance doppler-inertial navigation experimental results[J]. Journal of System Engineering and Electronics, 2012, 23(2): 256-264.

[5] Zhang T, Chen L, Li Y. AUV underwater positioning algorithm based on interactive assistance of SINS and LBL[J]. Sensors, 2016, 16(1): 1-22.

[6] Miller B S, Barlow J, Calderan S, et al. Validating the reliability of passive acoustic localisation: a novel method for encountering rare and remote Antarctic blue whales[J]. Endanger. Species Res, 2015, 26: 257-269.

[7] Zhang T, Shi H, Chen L, et al. AUV positioning method based on tightly coupled SINS/LBL for underwater acoustic multipath propagation[J]. Sensors, 2016, 16(3): 1-16.

[8] 王小峰. 水下航行器 INS/GPS/DVL 組合導(dǎo)航方法[J].魚雷技術(shù), 2010, 18(4): 287-290. Wang Xiao-feng. New application of fedearted filter in INS/GPS/DVL integrated navigation system for AUV[J]. Torpedo Technology, 2010, 18(4): 287-290.

[9] Chen Y, Zheng D, Miller P A, et al. Underwater inertial navigation with long baseline transceivers: A near-realtime approach[J]. IEEE Transactions on Control Systems Technology, 2016, 24(1): 240-251.

[10] Kashima T, Asada A, Ura T. The positioning system integrated LBL and SSBL using seafloor acoustic mirror transponder[C]//2013 IEEE International Conference on Underwater Technology Symposium. 2013: 1-4.

Improved positioning algorithm of inertial navigation system with aiding of underwater three-element array

ZHANG Tao, HU He-qing
(1. School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China; 2. Key Lab of Micro Inertial Instruments and Advanced Navigation Technology of Education Ministry, Southeast University, Nanjing 210096, China)

Aiming at the error divergence of the inertial navigation system, an integrated navigation system based on INS/DVL and acoustic positioning is designed for underwater vehicle. The acoustic positioning system is made up of transducer array on underwater vehicles and three positioning array elements on seabed. Traditional LBL system should have at least four elements, while the location of three elements can’t be directly calculated. To overcome this problem, a range coupling mode is built based on the distance information measured by the three-element array and the positioning information outputted by the inertial navigation system. The state and measurement equations of inertial navigation, DVL and acoustic positioning system are given. In addition, an equivalent sound velocity correcting method is given based on the vertical distribution characteristics of underwater sound velocity. Simulation results show that the integrated navigation system can effectively correct the position error of the inertial navigation system, the positioning error is less than 4 m, and it has good applicability.

underwater vehicle; SINS; three-element array; range coupling

V249.3

A

1005-6734(2017)02-0192-05

10.13695/j.cnki.12-1222/o3.2017.02.010

2017-01-04；

2017-03-25

國(guó)家自然科學(xué)基金資助項(xiàng)目（51375088）；微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室基金（201403）；優(yōu)秀青年教師教學(xué)科研資助計(jì)劃（2242015R30031）

張濤（1980—），男，博士、副研究員，從事組合導(dǎo)航。E-mail: ztandyy@163.com