基于SINS/LBL緊組合的AUV水下導航定位技術

張 濤，石宏飛，徐曉蘇

（1. 東南大學 儀器科學與工程學院，南京 210096；2. 東南大學 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096）

基于SINS/LBL緊組合的AUV水下導航定位技術

張 濤1,2，石宏飛1,2，徐曉蘇1,2

（1. 東南大學 儀器科學與工程學院，南京 210096；2. 東南大學 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096）

針對因水下環境的特殊性AUV難以實現精準導航的問題，設計了一種基于SINS/LBL緊組合的AUV水下導航定位系統。該系統具有定位精度高、魯棒性好等優點，系統由SINS、LBL、DVL和MCP組成，根據LBL的TDOA定位原理建立了LBL斜距差模型，給出了SINS/LBL/DVL/MCP的狀態方程和量測方程，利用集中kalman濾波器對組合導航系統進行最優估計。在相同的仿真條件下，對SINS/LBL松組合、緊組合進行了軟件仿真，仿真結果表明：相對于松組合系統，基于SINS/LBL的緊組合系統導航精度更高，尤其是在由于AUV運動或受到外界干擾導致可用信號的水聽器不足四個時，緊組合系統的可靠性和容錯性更高。

AUV；捷聯慣導；長基線；緊組合；松組合

AUV（Autonomous Underwater Vehicle，自主式水下航行器）是一種自主式、智能化、能完成水下探測、運載、打撈等多種功能的水下工具，由于水下環境的特殊性，實現AUV的精準導航仍很困難，導航問題是AUV所面臨的主要技術挑戰之一[1]。由于單一的導航系統自身存在不足，因而組合導航技術已成為導航技術發展的重要方向之一，目前國內外應用于AUV的水下自主導航技術主要以SINS(Strapdown Inertial Navigation Systems，捷聯式慣性導航系統)和DVL（Doppler Velocity Log，多普勒計程儀）的組合導航主，輔以GPS(Global Positioning System，全球定位系統)、水聲定位系統、MCP(Magnetic compass，磁羅經）等修正，能有效提高系統的導航精度[2-6]。

LBL（Long Base Line，長基線）水聲定位系統利用AUV上的聲源與海底水聽器陣之間的距離信息來求解AUV位置，因其作用范圍廣、定位精度高已廣泛應用于水下潛器[7]。根據參與組合物理量的不同，SINS與LBL的組合方式可分為松組合和緊組合。基于SINS與LBL松組合的系統是根據LBL計算得到的位置來輔助修正SINS導航系統，它需要四個以上的水聽器來確定AUV的位置信息[8]。當由于AUV運動或外界干擾使可用水聽器數量少于四個時，LBL將無法進行定位，從而無法進行松組合，將導致AUV無法完成準確定位。因此，有必要對AUV水下組合導航技術進行進一步的研究。

針對上述SINS/LBL松組合的缺陷，本文提出了一種基于SINS/LBL緊組合的AUV水下導航定位系統，該系統由SINS、LBL、DVL、MCP組成。在LBL水聲定位原理的基礎上建立了LBL斜距差模型，并詳細推導了SINS/LBL緊組合的狀態方程和量測方程，利用卡爾曼濾波技術對組合系統進行最優估計。在相同條件下對松、緊組合系統進行了仿真，仿真結果表明，緊組合系統的導航精度更高，誤差曲線平滑，且當可用水聽器不足四個時，緊組合系統具有更好的導航性能。

1 基于SINS/LBL緊組合水下組合導航系統原理

針對水下實際使用環境，僅SINS/LBL緊組合系統難以提供全面的導航信息，考慮到DVL(Doppler Velocity Log，多普勒計程儀)能夠完全自主導航，隱蔽性好，在淺海和低速時測得AUV相對海底的航行速度精度高，MCP（Magnetic Compass Pilot）成本低，可靠性高，可提供航向信息，因此可選取SINS、LBL、DVL和MCP四個子系統來組成AUV組合導航系統。

圖1 SINS/LBL緊組合水下組合導航系統結構示意圖Fig.1 Structure of underwater integrated navigation system based on SINS/LBL tightly coupled system

圖1所示為基于SINS/LBL緊組合水下組合導航系統結構示意圖，組合系統由SINS、LBL、DVL和MCP四個子系統組成。其中，SINS作為主導航系統，根據SINS解算的AUV位置和多個水聽器位置得到基于SINS的斜距差，利用TOA(Time of Arrival，到達時間)或TDOA(Time Difference of Arrival，到達時間差)方法得到基于LBL的斜距差，以上兩個斜距差之差作為觀測量之一輸入至Kalman濾波器。另外，DVL提供速度信息，MCP提供航向信息，分別與SINS解算得到的速度和航向值之差也作為觀測量，經Kalman信息融合后得到各導航參數誤差反饋校正SINS從而提高導航精度。

1.1 基于 LBL的斜距差模型的建立

LBL水聲定位系統是由安裝在AUV的聲源和布放在海底的水聽器陣構成的定位系統，海底水聽器之間的距離一般為100～6000 m，通過測量聲源到達水聽器的時延解算出聲源與水聽器之間的距離，再由其幾何關系解算出AUV的位置。常用定位算法有TOA和TDOA。與TOA不同，TDOA是通過檢測聲信號到達兩個水聽器的時間差，而不是到達的絕對時間來確定AUV的位置，對海底水聽器的時間同步不作要求，因此，本文采用基于TDOA方法輔助SINS進行定位。

圖2 LBL水聲定位示意圖Fig.2 Schematic LBL acoustic positioning

圖2為LBL水聲定位示意圖。LBL水聲定位系統由安裝在AUV上的聲源和布放在海底的四個水聽器組成。以地球直角坐標系為全局坐標系，假設AUV位置坐標為(x,y,z)，四個水聽器的位置坐標為(xi,yi,zi)，則AUV與水聽器之間的斜距可表示為

在TDOA方法下，通過對各水聽器接收聲源信號做廣義互相關得到聲源信號傳播至各水聽器的時間差，再乘以聲速則得到對應的斜距差Ri-R0。

由于時延差測量、聲傳播的多途徑效應等將引起斜距差測量有誤差，為簡化模型，可將斜距差誤差用一階馬爾科夫過程表示，則斜距差[9-10]可表示為

1.2 SINS/LBL緊組合狀態方程和量測方程

緊組合系統狀態方程描述為

式中，XSINS、XLBL分別為SINS、LBL的狀態變量，FSINS、FLBL分別為SINS、LBL系統矩陣，WSINS、WLBL分別為SINS與LBL的系統噪聲。

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

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

式中，δRi(i=1,2,3)斜距差誤差。

量測方程描述為

式中，ρSINSi-ρLBLi為SINS解算的各水聽器與聲源的斜距差與LBL測量得到的斜距差的差值，ηLBL為量測噪聲。

設SINS解算的AUV位置為（xS,yS,zS），則有

相對AUV真實位置（x,y,z）將上式線性化得

式中：

LBL斜距差為

于是：

當系統采用地球直角系（Oxeyeze）作為導航坐標系時，可用式(15)構造系統量測方程。實際應用中是以經緯度和高度定位的，因此要把dx、dy、dz用dl、dλ、dh 表示。

其非零元素如下：

式中，i=1,2,3，RN為參考橢球體子午面內的曲率半徑，RE為垂直子午面的法線平面內的曲率半徑，e為橢球的橢圓度。

2 組合系統模型的建立

組合系統包括SINS、LBL、DVL和MCP四個子系統，Kalman濾波器的狀態量依次選取SINS選擇速度誤差、姿態誤差、位置誤差、加速度計零偏和陀螺漂移，LBL斜距差的隨機誤差，DVL的速度誤差，MCP的航向角誤差，共22個變量。SINS/LBL緊組合狀態方程和量測方程如上節所述。

2.1 SINS/DVL狀態方程和量測方程

選取多普勒測速儀的測速誤差作為狀態變量：

式中：δVDi(i=x,y,z)為地形變化引起的測速誤差，可用一階馬爾科夫過程近似描述。

式中，Cij(i=1,2,3;j =1,2,3)為姿態轉移矩陣的元素，ηDVL為DVL量測噪聲。

2.2 SINS/MCP狀態方程和量測方程

選取磁羅經輸出的航向角誤差作為狀態變量：

δψMCP可設為隨機常值，于是：

式中，ηMCP為MCP量測噪聲。

3 系統仿真及分析

為驗證基于SINS/LBL緊組合的水下組合導航系統的性能，在C++ 環境下進行了1 h的仿真，仿真條件如下：AUV運動模型為三軸正弦搖擺運動，縱搖幅度分別為9°，搖擺周期為8 s，橫搖幅度分別為12°，搖擺周期為10 s，航向幅度分別為14°，搖擺周期為6 s，初始相位均為零。SINS慣性器件的性能參數如下：陀螺儀隨機漂移均為0.06 (°)/h，常值漂移均為0.06 (°)/h，加速度計隨機漂移均為50 μg，常值偏置均為100 μg；初始失準角：縱搖角1.5°，橫搖角1.5°，航向角1.5°；LBL斜距差隨機漂移為5 m，常值偏置為5 m，DVL速度誤差為0.2 m/s，MCP精度優于3°。

圖3所示為AUV理想運動軌跡與海底水聽器陣位置圖。AUV在深度為100 m的水下以初始航向角45°做勻速直線運動，航速為4 m/s，姿態更新周期為10 ms；AUV的初始位置為：緯度L=32°，經度λ=118°；海底布放兩個水聽器陣列，水聽器接收信號距離均為2.5 km，為簡化分析，假設所有的水聽器都在同一平面內，水聽器陣列1（圖3左下方藍色圓圈所示）的四個水聽器的位置分別為(117.995°, 32°)、(118°, 31.995°)、(118.005°, 32°)、(118°, 32.005°)，水聽器陣列2（圖3右上方藍色圓圈所示）的四個水聽器的位置分別為(118.100°, 32.090°)、(118.105°, 32.085°)、(118.110°, 32.090°)、(118.105°, 32.095°)。仿真結果如圖4～圖7所示。

圖3 AUV理想運動軌跡與海底水聽器陣位置圖Fig.3 Ideal trajectory of AUV and the seabed hydrophone array locations.

圖4 SINS/LBL緊組合姿態誤差曲線Fig.4 Curves of attitude error of SINS/LBL tightly coupled system

圖5 SINS/LBL緊組合速度誤差曲線Fig.5 Curves of velocity error of SINS/LBL tightly coupled system

圖4～圖6為SINS/LBL緊組合姿態、速度和位置誤差曲線，由圖可知：在0～625 s內AUV行駛在水聽器陣列1接收信號范圍內，LBL提供三個斜距差觀測量與SINS作緊組合，位置誤差小于5 m，其他誤差小且穩定；在625～800 s時，AUV逐漸駛離長基線陣列1的距離范圍，此時仍有兩個水聽器可以接收到聲源信號，提供一個斜距差觀測量與SINS進行緊組合，此時導航誤差仍較小；在800～2800 s內AUV進入LBL信號盲區，由于失去位置信息校正，各導航誤差增大，并且位置誤差有發散的趨勢；在2800～2975 s內，AUV逐步靠近水聽器陣列2信號區，有兩個水聽器可以提前接收到聲源信號，使得導航誤差逐漸減少；在2800～3600 s內，AUV完全駛入水聽器陣列2，四個水聽器均能接收到聲源信號，導航誤差保持較小且穩定，且位置誤差恢復至5 m以內。

圖6 SINS/LBL緊組合位置誤差曲線Fig.6 Curves of position error of SINS/LBL tightly coupled system

圖7 SINS/LBL松組合位置誤差曲線Fig.7 Curves of position error of SINS/LBL loosely coupled system

圖7為松組合誤差曲線，只有接四個以上的水聽器接收到聲源信號時，松組合算法才可以正常執行，位置誤差為10 m，一旦接收到信號的水聽器數量小于四個時無法定位，因此松組合失效，位置誤差發散。

上述分析表明，相對于松組合系統，SINS/LBL緊組合系統導航精度更高，尤其在由于AUV運動或受到外界干擾導致可用信號的水聽器不足四個時，緊組合系統的可靠性和容錯性更高。

4 結 論

本文針對SINS與LBL松組合的缺陷，設計了一種基于SINS/LBL緊組合的AUV水下導航定位系統。整個組合系統由SINS、DVL、LBL和MCP四個子系統組成，在建立LBL斜距差模型的基礎上詳細推導了SINS/LBL緊組合的狀態方程和量測方程，利用集中Kalman濾波技術進行最優估計補償組合系統。

通過系統仿真結果可以看出，相對于松組合系統，基于SINS/LBL緊組合的AUV水下導航定位系統能有效提高導航精度，具有較好的抗干擾性。

（References）：

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

[2] 張濤, 徐曉蘇, 李瑤, 等. 基于慣導及水下聲學輔助系統的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.

[3] Morgado M, Paulo O, Carlos S, et al. Embedded vehicle dynamics aiding for USBL/INS underwater navigation system[J]. IEEE Transactions on Control Systems Technology, 2014, 22(1): 322-330.

[4] Miller P A, Farrell J A, Zhao Yuan-yuan, et al. Autonomous underwater vehicle navigation[J]. IEEE Journal of Oceanic Engineering, 2010, 35(3): 663-678.

[5] Marco D B, Healey A J. Command, control, and navigation experimental results with the NPS ARIES AUV[J]. IEEE Journal of Oceanic Engineering, 2001, 26(4): 466-476.

[6] Liu Yu, Li X R. Aided strapdown inertial navigation for autonomous underwater vehicles[C]//Proc. SPIE of Signal and Data Processing of Small Targets. 2010: 2654-2662.

[7] 田坦. 水下定位與導航技術[M]. 北京: 國防工業出版社, 2007.

[8] 范欣, 張福斌, 張永清, 等. 多傳感器信息融合的水下航行器組合導航方法[J]. 火力與指揮控制, 2011, 36(3): 78-81. Fan Xin, Zhang Fu-bing, Zhang Yong-qing, et al. Information fusion technology and its application to multi-sensor integrated navigation system for autonomous underwater vehicle[J]. Fire Control & Command Control, 2011, 36(3): 78-81.

[9] Pan-Mook Lee, Jun Bong-Huan, Kim Kihun, et al. Simulation of an inertial acoustic navigation system with range aiding for an autonomous underwater vehicle[J]. IEEE Journal of Oceanic Engineering, 2007, 32(2): 327-345.

[10] Pan-Mook Lee, Jun Bong-Huan, Choi Hyun Taek, et al. An integrated navigation systems for underwater vehicles based on inertial sensors and pseudo LBL acoustic transponders[C]//Proceedings of OCEANS 2005 MTS/IEEE. 2005: 555-562.

Positioning technology based on SINS / LBL tightly coupling for AUV underwater navigation system

ZHANG Tao1,2, SHI Hong-fei1,2, XU Xiao-su1,2
( 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)

AUVs(Autonomous Underwater Vehicle) are hardly to achieve precise navigation due to the particularity of the underwater environment. In this paper, an underwater navigation of AUV based on SINS/LBL tightly coupled system was designed. The system is composed of SINS, LBL, DVL and MCP, and has such advantages as high precision and good robustness, etc.. The LBL slant-range difference model was established according to the TDOA location principle of LBL. The SINS/LBL/DVL/MCP state equation and measurement equation were given. The centralized Kalman filter was applied to the optimal estimation of the integrated navigation system. The software simulations of SINS/LBL loosely coupled and tightly coupled systems were conducted in the same simulation conditions. The results show that, compared with the loosely coupled system, the navigation precision of the SINS/LBL tightly coupled system is higher, and the reliability of the SINS/LBL tightly coupled system is better especially when the number of available hydrophones is less than four caused by AUV movement or outside disturbance.

autonomous underwater vehicle; SINS; long base line; tightly coupled; loosely coupled

V249.3

A

1005-6734(2015)04-0500-05

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

2015-03-09；

2015-06-30

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

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