一種改進的水下三陣元輔助慣導系統的定位算法

張 濤，胡賀慶

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

2. 東南大學 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096）

一種改進的水下三陣元輔助慣導系統的定位算法

張 濤，胡賀慶

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

2. 東南大學 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096）

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

水下潛器；慣導系統；三陣元基陣；距離耦合

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

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

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

1 系統原理與結構

1.1 三陣元基陣布放和校準

水下三陣元基陣精確定位的前提是陣元的絕對地理位置已知，且得到準確校準，常利用超短基線測出陣元在聲學基陣坐標系下的位置，通過坐標轉化，可以得到陣元的絕對地理位置。校準原理可用下式表示：

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

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

圖1 陣元校準定位模型Fig.1 Array element calibration positioning model

1.2 三陣元基陣定位原理

水下定位系統模型主要有球面交匯模型和雙曲面交匯模型，在實際情況中，一般很少用雙曲線交匯模型，而采用高精度的同步式球面交匯定位模型，主要通過計算信號的傳播時間和聲速的乘積來獲得水下潛器和陣元的距離，然后解算方程組來對目標進行定位。定位模型如圖2所示。

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

2 距離耦合組合導航模型

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

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

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

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

2.1 距離耦合組合導航模型狀態方程

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

式中：R為距離真值，δR為距離測量誤差，τ、ω分別是一階馬爾科夫過程的相關時間和驅動白噪聲，υ為高斯白噪聲。狀態方程可表示為

即：

式中：X為狀態變量，F為狀態轉移矩陣，W為系統噪聲矩陣。

選擇速度誤差、姿態誤差、位置誤差、加速度計零偏和陀螺漂移作為狀態量：

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

選擇三陣元基陣到每個陣元的距離誤差作為狀態量：

2.2 距離耦合組合導航模型觀測方程

導航系統觀測方程為

式中：VSINS是慣導系統輸出的速度信息，VDVL是DVL系統輸出的速度信息，RSINS是慣導系統解算的位置與每個陣元之間的距離信息，Racou是三陣元基陣定位系統輸出的距離信息。

式中：i=1,2,3。

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

由三陣元基陣定位系統距離誤差模型可知：

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

由以上分析可得觀測矩陣為

3 等效聲速修正方法

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

4 系統仿真及分析

4.1 水下Bellhop仿真模型

一般來說，水下聲信道是時變、空變的隨機信道，其必須用時變、空變的隨機濾波器來描述，但是在大多數實際應用中，聲信道可以看作是緩慢時變的相干多途信道，若觀察或者處理的時間不是過分長，則聲信道可以用時不變濾波器來描述。

水下聲音傳播可以簡化為下式：

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

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

4.2 距離耦合組合導航模型仿真

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

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

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

圖5 北向誤差曲線對比圖Fig.5 North error curves

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

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

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

5 結 論

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

該算法計算過程簡單，能有效減小測距誤差。通過系統仿真結果說明，三陣元基陣距離耦合算法能夠避免傳統三陣元基陣同步定位求解模糊問題，同時在三陣元基陣定位范圍內能夠有效修正慣導誤差，保證水下潛器的導航精度，具有良好的適用性。

（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] 張濤, 徐曉蘇, 李瑤, 等. 基于慣導及水下聲學輔助系統的AUV容錯導航技術[J]. 中國慣性技術學報, 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 組合導航方法[J].魚雷技術, 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

國家自然科學基金資助項目（51375088）；微慣性儀表與先進導航技術教育部重點實驗室基金（201403）；優秀青年教師教學科研資助計劃（2242015R30031）

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