基于SINS/LBL緊組合的AUV水下導(dǎo)航定位技術(shù)

張 濤，石宏飛，徐曉蘇

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

基于SINS/LBL緊組合的AUV水下導(dǎo)航定位技術(shù)

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

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

針對(duì)因水下環(huán)境的特殊性AUV難以實(shí)現(xiàn)精準(zhǔn)導(dǎo)航的問(wèn)題，設(shè)計(jì)了一種基于SINS/LBL緊組合的AUV水下導(dǎo)航定位系統(tǒng)。該系統(tǒng)具有定位精度高、魯棒性好等優(yōu)點(diǎn)，系統(tǒng)由SINS、LBL、DVL和MCP組成，根據(jù)LBL的TDOA定位原理建立了LBL斜距差模型，給出了SINS/LBL/DVL/MCP的狀態(tài)方程和量測(cè)方程，利用集中kalman濾波器對(duì)組合導(dǎo)航系統(tǒng)進(jìn)行最優(yōu)估計(jì)。在相同的仿真條件下，對(duì)SINS/LBL松組合、緊組合進(jìn)行了軟件仿真，仿真結(jié)果表明：相對(duì)于松組合系統(tǒng)，基于SINS/LBL的緊組合系統(tǒng)導(dǎo)航精度更高，尤其是在由于AUV運(yùn)動(dòng)或受到外界干擾導(dǎo)致可用信號(hào)的水聽(tīng)器不足四個(gè)時(shí)，緊組合系統(tǒng)的可靠性和容錯(cuò)性更高。

AUV；捷聯(lián)慣導(dǎo)；長(zhǎng)基線；緊組合；松組合

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

LBL（Long Base Line，長(zhǎng)基線）水聲定位系統(tǒng)利用AUV上的聲源與海底水聽(tīng)器陣之間的距離信息來(lái)求解AUV位置，因其作用范圍廣、定位精度高已廣泛應(yīng)用于水下潛器[7]。根據(jù)參與組合物理量的不同，SINS與LBL的組合方式可分為松組合和緊組合?；赟INS與LBL松組合的系統(tǒng)是根據(jù)LBL計(jì)算得到的位置來(lái)輔助修正SINS導(dǎo)航系統(tǒng)，它需要四個(gè)以上的水聽(tīng)器來(lái)確定AUV的位置信息[8]。當(dāng)由于AUV運(yùn)動(dòng)或外界干擾使可用水聽(tīng)器數(shù)量少于四個(gè)時(shí)，LBL將無(wú)法進(jìn)行定位，從而無(wú)法進(jìn)行松組合，將導(dǎo)致AUV無(wú)法完成準(zhǔn)確定位。因此，有必要對(duì)AUV水下組合導(dǎo)航技術(shù)進(jìn)行進(jìn)一步的研究。

針對(duì)上述SINS/LBL松組合的缺陷，本文提出了一種基于SINS/LBL緊組合的AUV水下導(dǎo)航定位系統(tǒng)，該系統(tǒng)由SINS、LBL、DVL、MCP組成。在LBL水聲定位原理的基礎(chǔ)上建立了LBL斜距差模型，并詳細(xì)推導(dǎo)了SINS/LBL緊組合的狀態(tài)方程和量測(cè)方程，利用卡爾曼濾波技術(shù)對(duì)組合系統(tǒng)進(jìn)行最優(yōu)估計(jì)。在相同條件下對(duì)松、緊組合系統(tǒng)進(jìn)行了仿真，仿真結(jié)果表明，緊組合系統(tǒng)的導(dǎo)航精度更高，誤差曲線平滑，且當(dāng)可用水聽(tīng)器不足四個(gè)時(shí)，緊組合系統(tǒng)具有更好的導(dǎo)航性能。

1 基于SINS/LBL緊組合水下組合導(dǎo)航系統(tǒng)原理

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

圖1 SINS/LBL緊組合水下組合導(dǎo)航系統(tǒng)結(jié)構(gòu)示意圖Fig.1 Structure of underwater integrated navigation system based on SINS/LBL tightly coupled system

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

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

LBL水聲定位系統(tǒng)是由安裝在AUV的聲源和布放在海底的水聽(tīng)器陣構(gòu)成的定位系統(tǒng)，海底水聽(tīng)器之間的距離一般為100～6000 m，通過(guò)測(cè)量聲源到達(dá)水聽(tīng)器的時(shí)延解算出聲源與水聽(tīng)器之間的距離，再由其幾何關(guān)系解算出AUV的位置。常用定位算法有TOA和TDOA。與TOA不同，TDOA是通過(guò)檢測(cè)聲信號(hào)到達(dá)兩個(gè)水聽(tīng)器的時(shí)間差，而不是到達(dá)的絕對(duì)時(shí)間來(lái)確定AUV的位置，對(duì)海底水聽(tīng)器的時(shí)間同步不作要求，因此，本文采用基于TDOA方法輔助SINS進(jìn)行定位。

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

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

在TDOA方法下，通過(guò)對(duì)各水聽(tīng)器接收聲源信號(hào)做廣義互相關(guān)得到聲源信號(hào)傳播至各水聽(tīng)器的時(shí)間差，再乘以聲速則得到對(duì)應(yīng)的斜距差Ri-R0。

由于時(shí)延差測(cè)量、聲傳播的多途徑效應(yīng)等將引起斜距差測(cè)量有誤差，為簡(jiǎn)化模型，可將斜距差誤差用一階馬爾科夫過(guò)程表示，則斜距差[9-10]可表示為

1.2 SINS/LBL緊組合狀態(tài)方程和量測(cè)方程

緊組合系統(tǒng)狀態(tài)方程描述為

式中，XSINS、XLBL分別為SINS、LBL的狀態(tài)變量，F(xiàn)SINS、FLBL分別為SINS、LBL系統(tǒng)矩陣，WSINS、WLBL分別為SINS與LBL的系統(tǒng)噪聲。

SINS選擇速度誤差、姿態(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è)軸向漂移。FSINS可由SINS誤差方程確定。

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

量測(cè)方程描述為

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

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

相對(duì)AUV真實(shí)位置（x,y,z）將上式線性化得

式中：

LBL斜距差為

于是：

當(dāng)系統(tǒng)采用地球直角系（Oxeyeze）作為導(dǎo)航坐標(biāo)系時(shí)，可用式(15)構(gòu)造系統(tǒng)量測(cè)方程。實(shí)際應(yīng)用中是以經(jīng)緯度和高度定位的，因此要把dx、dy、dz用dl、dλ、dh 表示。

其非零元素如下：

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

2 組合系統(tǒng)模型的建立

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

2.1 SINS/DVL狀態(tài)方程和量測(cè)方程

選取多普勒測(cè)速儀的測(cè)速誤差作為狀態(tài)變量：

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

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

2.2 SINS/MCP狀態(tài)方程和量測(cè)方程

選取磁羅經(jīng)輸出的航向角誤差作為狀態(tài)變量：

δψMCP可設(shè)為隨機(jī)常值，于是：

式中，ηMCP為MCP量測(cè)噪聲。

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

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

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

圖3 AUV理想運(yùn)動(dòng)軌跡與海底水聽(tīng)器陣位置圖Fig.3 Ideal trajectory of AUV and the seabed hydrophone array locations.

圖4 SINS/LBL緊組合姿態(tài)誤差曲線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緊組合姿態(tài)、速度和位置誤差曲線，由圖可知：在0～625 s內(nèi)AUV行駛在水聽(tīng)器陣列1接收信號(hào)范圍內(nèi)，LBL提供三個(gè)斜距差觀測(cè)量與SINS作緊組合，位置誤差小于5 m，其他誤差小且穩(wěn)定；在625～800 s時(shí)，AUV逐漸駛離長(zhǎng)基線陣列1的距離范圍，此時(shí)仍有兩個(gè)水聽(tīng)器可以接收到聲源信號(hào)，提供一個(gè)斜距差觀測(cè)量與SINS進(jìn)行緊組合，此時(shí)導(dǎo)航誤差仍較?。辉?00～2800 s內(nèi)AUV進(jìn)入LBL信號(hào)盲區(qū)，由于失去位置信息校正，各導(dǎo)航誤差增大，并且位置誤差有發(fā)散的趨勢(shì)；在2800～2975 s內(nèi)，AUV逐步靠近水聽(tīng)器陣列2信號(hào)區(qū)，有兩個(gè)水聽(tīng)器可以提前接收到聲源信號(hào)，使得導(dǎo)航誤差逐漸減少；在2800～3600 s內(nèi)，AUV完全駛?cè)胨?tīng)器陣列2，四個(gè)水聽(tīng)器均能接收到聲源信號(hào)，導(dǎo)航誤差保持較小且穩(wěn)定，且位置誤差恢復(fù)至5 m以內(nèi)。

圖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為松組合誤差曲線，只有接四個(gè)以上的水聽(tīng)器接收到聲源信號(hào)時(shí)，松組合算法才可以正常執(zhí)行，位置誤差為10 m，一旦接收到信號(hào)的水聽(tīng)器數(shù)量小于四個(gè)時(shí)無(wú)法定位，因此松組合失效，位置誤差發(fā)散。

上述分析表明，相對(duì)于松組合系統(tǒng)，SINS/LBL緊組合系統(tǒng)導(dǎo)航精度更高，尤其在由于AUV運(yùn)動(dòng)或受到外界干擾導(dǎo)致可用信號(hào)的水聽(tīng)器不足四個(gè)時(shí)，緊組合系統(tǒng)的可靠性和容錯(cuò)性更高。

4 結(jié) 論

本文針對(duì)SINS與LBL松組合的缺陷，設(shè)計(jì)了一種基于SINS/LBL緊組合的AUV水下導(dǎo)航定位系統(tǒng)。整個(gè)組合系統(tǒng)由SINS、DVL、LBL和MCP四個(gè)子系統(tǒng)組成，在建立LBL斜距差模型的基礎(chǔ)上詳細(xì)推導(dǎo)了SINS/LBL緊組合的狀態(tài)方程和量測(cè)方程，利用集中Kalman濾波技術(shù)進(jìn)行最優(yōu)估計(jì)補(bǔ)償組合系統(tǒng)。

通過(guò)系統(tǒng)仿真結(jié)果可以看出，相對(duì)于松組合系統(tǒng)，基于SINS/LBL緊組合的AUV水下導(dǎo)航定位系統(tǒng)能有效提高導(dǎo)航精度，具有較好的抗干擾性。

（References）：

[1] 王小峰. 水下航行器INS/GPS/DVL組合導(dǎo)航方法[J]. 魚(yú)雷技術(shù), 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] 張濤, 徐曉蘇, 李瑤, 等. 基于慣導(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.

[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] 田坦. 水下定位與導(dǎo)航技術(shù)[M]. 北京: 國(guó)防工業(yè)出版社, 2007.

[8] 范欣, 張福斌, 張永清, 等. 多傳感器信息融合的水下航行器組合導(dǎo)航方法[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

國(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