基于SINS/DVL與LBL交互輔助的AUV水下定位系統

張 濤，陳立平，石宏飛，胡賀慶

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

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

基于SINS/DVL與LBL交互輔助的AUV水下定位系統

張 濤1,2，陳立平1,2，石宏飛1,2，胡賀慶1,2

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

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

針對AUV水下定位系統存在的缺陷，設計了一種基于SINS/DVL與LBL交互輔助的AUV水下導航定位系統。系統由安裝在AUV上的SINS、DVL、發聲源及布放在海底已知位置的LBL水聲定位基陣組成。SINS/DVL估計的位置信息既輔助到達時延差估計，修正聲速，提高LBL定位估計的精度，又作為Taylor級數展開迭代法所需的迭代初始值，輔助LBL估計AUV位置。然后利用LBL系統估計的位置信息校正SINS/DVL的累積誤差。仿真結果表明，當AUV靠近LBL基陣時，與傳統方法相比，該系統能更有效地修正SINS/DVL系統的累積誤差，使定位誤差小于2 m，適用于水下定位導航。

自主式水下航行器；捷聯慣性導航系統；LBL；水下定位系統

AUV（Autonomous Underwater Vehicle，自主式水下航行器）現在被用于執行各種水下任務，包括海洋探測、水下排雷以及收集海洋及河流的水深測量數據[1-3]。為了保證順利完成水下任務，并能得到比較精確的水下測量數據，要求其在水下具有長期自主的高精度定位導航能力，并且具有隱蔽性[4]。在水下環境中，電磁波信號具有嚴重衰減的特性，在深海或者冰面下，采用GPS(Global Positioning System，全球定位系統)及其他無線電定位方式均不能取得理想的定位效果。因此，AUV通常依靠本體傳感器并通過艦位推算的方法來估計位置。為了滿足導航需求，通常會用DVL（Doppler Velocity Log，多普勒計程儀）與SINS（Strap-down Inertial Navigation System，捷聯慣性導航系統）進行組合導航。但是，采用艦位推算方法進行定位時，定位誤差會隨著時間不斷累積[5]。目前，當AUV在淺海中執行任務時，可以采用“潛航-水面校正-潛航”的導航模式進行定位導航，SINS/DVL組合導航系統用于AUV水下定位導航，當AUV在水下潛行一定時間時，為了校正累積誤差，上浮水面，使用GPS系統進行校正[6]。采用這種方案，必須要求AUV不斷往返于水下作業地點與水面之間。這樣做不僅影響工作效率，增加能耗，而且更容易暴露 AUV的位置，特別是當AUV在深海或者冰下作業[7-8]時，這個方案的缺點更加突出。因此，非常有必要研究一種直接在水下進行輔助定位并校正累積誤差的方法。

LBL水聲定位系統通常是由基線長度為幾百米至幾千米的海底應答器基陣和被定為載體的問答機組成，利用水下目標與海底陣元之間的距離或距離差信息來求解目標位置，可在局部區域內對水下載體進行精確定位，不存在累積誤差[9-10]。因此，LBL水聲定位系統非常適用于輔助水下AUV進行定位。

在前人的一些研究中，Liu Yu、Li X Rong提出了一種利用LBL水聲定位系統，ADCP（acoustic Doppler current profiler，聲學多普勒流速剖面儀）及深度計輔助INS的水下AUV定位導航算法[11]，Miller P A等提出了一種基于LBL/DVL/INS緊組合系統[3]。這些系統在使用LBL定位時，使用TOA（Time of Arrival，到達時延）定位方法進行定位，但沒有解決聲線在水下彎曲和多路徑造成的定位誤差。同時，使用TOA定位需要保證基陣及 AUV上導航系統時間同步，需要在水下進行較復雜的通信，這一點在實際應用中比較困難。因此，本文通過研究TDOA（Time Difference of Arrival，到達時延差）水下定位的關鍵技術，提出了一種基于SINS/DVL組合導航系統與LBL水聲定位系統交互輔助的AUV水下定位方法。

1 系統原理與結構

如圖1所示：固定于水底的多個水聽器作為參考導航節點接收固定在 AUV上的聲源發出聲音信號，并通過電纜將信號發送至數據處理器，數據處理器根據各水聽器接收的聲信號來確定各信號接收的時延差，從而計算出距離差；然后根據雙曲定位模型計算出聲源位置，即得到 AUV的絕對位置；然后將該位置信息通過水下通訊方式發送給AUV進行數據融合。

圖1 水下長基線定位Fig.1 LBL acoustic positioning

圖2 系統框圖Fig.2 Block diagram of system

如圖2所示，該定位系統主要由LBL水下定位系統、SINS/DVL組合導航系統和數據處理單元三部分組成。LBL水下定位系統提供水聽器接收信號，進行廣義互相關計算，得到模糊相關峰。SINS/DVL提供當前的導航定位位置信息PSINS，計算時延差值tij′，用于篩選出主峰，得到時延差tij，列出定位解算方程，并且以PSINS為初始迭代位置，采用Taylor級數展開算法解算出位置信息PLBL，然后將PLBL和PSINS的差值作為外部觀測信息輸入到卡爾曼濾波器進行濾波，濾波結果對SINS/DVL進行校正，最終得到精確的AUV位置信息PAUV。

2 系統關鍵性技術

2.1 時延差的計算

假設聲源產生聲波信號x( t)，則第i個水聽器接收到的信號為

第j個水聽器接收到的信號為

其中：αi、αj為聲信號在水中傳播的衰減系數，ni( t)、nj( t)為互不相關的噪聲信號， τi、 τj為傳播時間；

xi( t)與 xj( t)的互相關函數為

其中：τ =τj-τi，表示到達時間差，T表示觀測時間。根據相關函數的性質，只要找出 Rxixj(τ)的峰值，其對應的τ即是估計的時延差。

2.2 尋找主相關峰

由于多路徑效應，互相關函數最終會出現多個相關峰，各峰值相差不大，難以甄別實際用于定位解算的時延差。本文提出一種通過 SINS定位結果輔助選擇主相關峰的方法，實現了時延差的最優估計。

LBL水下聲學定位系統中，第i個水聽器的位置為δVE、δ VN、δ VU，通過 SINS/DVL組合導航系統計算得到的 AUV的位置為 φE、 φN、φU，則水聽器與AUV的距離為

根據式(4)，計算任意兩個水聽器與AUV之間的距離差為

則兩兩水聽器接收信號時延差為

第i、j個水答器與AUV的距離差為

在tk-1時刻篩選得到的時延差為 tij(k - 1)，則當前等效聲速為

2.3 計算位置值

假設基陣中有n個水聽器，根據雙曲定位模型，列出定位解算方程組：

式中，δ、G、h分別如式(12)(13)(15)所示：

MHD方程組的求解難點在于：① 方程組含有非線性項。② MHD方程組是多方程耦合的方程組，導致方程組的計算量偏大。針對這兩個難題，本文基于區域分解技術和兩步有限元方法提出并行兩步有限元方法，并利用數值算例體現它的高效性。

式中，Q是TDOA的協方差矩陣，

算法需要一個位置估計值作為初始值進行迭代，初始值的準確程序對算法的收斂性和迭代次數影響較大。本文選用SINS/DVL的解算結果PSINS作為迭代初始值既可以滿足算法的收斂性，也可以使迭代次數大大減少。

最后，將PLBL和PSINS的差值作為外部觀測信息輸入到卡爾曼濾波器進行濾波，濾波結果對SINS/DVL進行校正，最終得到精確的AUV位置信息PAUV。

2.4 SINS/DVL/LBL組合系統建模

組合系統的狀態方程為

式中，X為狀態變量，F為狀態轉移矩陣，G為過程噪聲轉移矩陣，W為系統噪聲。選擇速度誤差、姿態誤差、位置誤差、加速度計零偏和陀螺漂移作為狀態量X：

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

組合系統的量測方程為

式中：Z為觀測量；PSINS是SINS系統輸出的位置信息；PLBL是LBL系統輸出的位置信息；VSINS是SINS系統輸出的速度信息；VDVL是 DVL系統輸出的速度信息；V為觀測噪聲向量；H為觀測矩陣，且滿足

3 系統仿真及分析

3.1 尋找主相關峰仿真

本項目在前期研究中進行仿真實驗，根據某湖的水下環境數據，利用Bellhop軟件模擬水下聲信道，得到水聽器接收信號，并進行廣義互相關計算不同水聽器接收信號的時延差。本文仿真選用帶寬50 kHz，中心頻率為 25 kHz的調幅信號作為聲源信號，最終通過互相關計算得到時延差。然而由于海洋混響、海底和海面反射等原因，水聲信號必然存在多徑傳播，聲源信號經過不同路徑最終在接收點處相干疊加，從而使得廣義互相關結果出現多個相關峰，如圖3所示。

圖3表明，傳統算法中選用的幅值最高的相關峰對應的時延差（圖3中的A點）與真值（圖3中的紅色圓圈表示）相差比較大，而通過SINS/DVL組合導航解算結果計算得到的時延差（圖3中的叉表示）更接近時延差真值，因此選擇峰值最大的前十個相關峰中與該值最接近的一個（圖3中的B點）作為代入計算的相關峰，從而能夠得到更加精確的時延差的值。

圖3 多相關峰Fig.3 Multi correlation peaks

傳統算法直接將最大峰值對應的相關峰作為主相關峰來計算時延差，在多路徑效應下會有較大誤差。本文采用的改進算法通過SINS/DVL組合導航系統得到的位置來選擇主相關峰，兩種算法的計算結果及誤差對比如表1 、表2所示。

表1、表2表明，改進算法減小了多路徑效應產生的多相關峰對時延差估計的干擾，使最終的時延差計算值精度優于傳統算法的時延差計算值。聲信號在水下傳播時，由于聲速分布不均，會產生聲線彎曲現象，有時必須經過多次在海面或海底的反射才能到達接收點。傳統算法直接采用傳統意義的聲速計算距離差，本文采用的改進算法根據式(9)計算等效聲速，利用等效聲速來計算距離差。傳統算法和改進算法的計算結果對比及其誤差對比如表3、表4 所示。

表3、表4表明，由于改進算法采用等效聲速計算距離差，對聲速進行了修正，在多路徑效應及聲線彎曲的情況下，大大提高了距離差計算值的精度。結合表1、表2、表3及表4來看，采用本文提出的時延差算法和距離差算法能夠大大減小時延差和距離差的誤差，這對提高AUV水下定位精度有重大意義。

表1 時延差計算結果對比Tab.1 Contradistinction of time-delay difference in two ways

表2 時延差計算值的誤差對比Tab.2 Contradistinction of the error of time-delay difference in two ways

表3 距離差計算結果對比Tab.3 Contradistinction of distance difference in two ways

表4 距離差計算值的誤差對比Tab.4 Contradistinction of the error of distance difference in two ways

3.2 定位仿真

為了驗證算法的定位效果，在Matlab環境下進行仿真。在水下布放5個水聽器，其位置用經緯度表示分別為(118.010°, 32.000°)、(118.010°, 32.010°)、(118.010°, 32.020°) 、(118.020°, 32.010°) 、(118.000°, 32.010°)，深度均為30 m。假設AUV當前真實位置為(118°, 32°)，深度為 10 m，由某湖試得到的水下聲速分布數據，采用bellhop模型模擬從LBL基陣處獲得的聲信號，并根據本文介紹算法進行時延差及距離差計算，最后采用SINS/DVL組合導航系統得到的位置作為Taylor級數展開法的迭代初值，進行定位結果解算，得到目標位置的經緯度及深度，并與傳統算法的定位結果進行對比，結果如圖4所示。

圖4中可以看出傳統算法得到的定位結果遠遠偏離了真實位置，而由改進算法計算得到的定位結果與真實值非常接近。這是由于采用傳統算法計算得到的距離差誤差過大，導致定位解算的結果不收斂或者收斂的結果誤差很大，無法得到可以接受的定位結果。而采用改進算法計算的距離差誤差在1 m以下，最終可以得到比較精確的定位結果。

圖4 定位結果對比Fig.4 Contradistinction of positioning result in two ways

3.3 動態仿真

為進一步驗證該算法在AUV處于動態運動狀態中的有效性，對算法進行動態仿真。在水下布放5個水聽器，其位置與定位仿真中的位置相同。假設 AUV從(117.995o, 31.995o)開始，以北偏東 45°沿某軌跡運動，其中，陀螺儀隨機漂移和常值漂移均為0.04 (o)/h，加速度計隨機漂移為50 μg，常值偏置為50 μg；初始失準角分別為：縱搖角0.005o，橫搖角0.005o，航向角0.005o；DVL速度誤差為0.05 m/s；AUV航行速度為1 m/s。

根據某湖試得到的水下聲速分布數據，采用bellhop模型模擬從 LBL基陣處獲得的聲信號，結合接收到信號采用本文算法進行定位計算，并將定位結果與SINS/DVL組合系統再次組合校正累積誤差，仿真時間為 4000 s，特別地，為增強系統的容錯能力，如果定位結果不收斂，則 LBL的定位結果不用于校正。圖5為傳統算法與改進算法的定位誤差曲線對比圖，其中，定位誤差用定位結果與真實位置之間的距離表示；圖6為AUV理想航跡、傳統算法估計的航跡、改進算法估計的航跡及水聽器布放位置示意圖。圖5表明，在0～1500 s內，傳統算法的定位誤差與改進算法的定位誤差都逐漸增大，這是因為在這段時間內，AUV距離LBL基陣還比較遠，水聽器接收聲源信號需要的時間比較長，定位結果會有很大的滯后，因此，這段時間不采用LBL進行校正，只用SINS/DVL組合導航系統進行定位，等AUV靠近LBL基陣時，即1500 s以后，再采用LBL進行校正。由于采用傳統算法計算的距離差誤差過大，導致LBL定位計算的結果不收斂，對 SINS/DVL組合導航系統基本上沒有校正作用，在1500 s以后，定位誤差不斷增大，最高達到10 m左右，而采用了本文提出的改進算法，大大減小了距離差計算值的誤差，LBL定位系統發揮作用，對SINS/DVL組合導航系統的累積誤差起到校正作用，使系統的定位誤差控制在2 m以內，當AUV駛離LBL基陣，失去校正作用時，SINS/DVL組合導航系統的整體誤差也遠遠小于傳統算法。圖 6表明，與傳統算法相比，使用改進算法估計的航跡更接近理想航跡，定位誤差更小。上述分析表明，在實際的動態運行狀態下，當AUV靠近LBL基陣時，可以使用改進算法進行定位計算，并用其結果校正 SINS/DVL組合導航系統，大大提高AUV在水下航行時的定位精度。

圖5 傳統算法與改進算法計算結果的誤差對比Fig.5 Contradistinction of error of positioning result in two ways

圖6 理想航跡及兩種算法的估計航跡Fig.6 Real track and the tracks estimated in two ways

4 結 論

本文針對現有水下定位技術中存在的缺陷，提出一種基于 SINS/DVL與 LBL交互輔助的水下定位系統。整個系統由SINS/DVL組合導航系統和LBL水聲定位系統組成。其中，LBL水聲定位系統采用基于Taylor級數展開的TDOA定位算法，并采用SINS/DVL組合導航系統給出的定位結果輔助計算時延差和距離差。同時，該定位結果還用于定位算法的迭代初值，最終的定位結果用于校正SINS/DVL組合導航系統的累積誤差。最終的仿真結果表明，相對于傳統算法，該算法能夠大大提高 AUV水下定位精度，當僅安裝了SINS/DVL組合導航系統的AUV靠近LBL基陣時，累積誤差能夠得到有效校正，具有很強的實用性。

（References）：

[1] 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.

[2] Tan Y T, Gao R, Chitre M. Cooperative path planning for range-only localization using a single moving beacon[J]. IEEE Journal of Oceanic Engineering, 2014, 39(2): 371-385.

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

[4] 李佩娟, 徐曉蘇, 張濤. 信息融合技術在水下組合導航系統中的應用[J]. 中國慣性技術學報, 2009, 17(3): 344-349. Li Pei-juan, Xu Xiao-su, Zhang Tao. Application of information fusion to integrated navigation system of underwater vehicle[J]. Journal of Chinese Inertial Technology, 2009, 17(3): 344-349.

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

[6] 潘學松. 基于SINS/DVL/GPS的AUV組合導航系統關鍵技術研究[D]. 青島: 中國海洋大學, 2011. Pan Xue-song. Research on key technology of AUV integrated navigation system based on SINS/DVL/GPS [D]. Qing dao:Ocean University of China, 2011.

[7] 朱忠軍. 未知海底環境下AUV組合導航技術研究[D].哈爾濱: 哈爾濱工程大學, 2013. Zhu Zhong-jun. The research of the unknown seabed environment on AUV integrated navigation technology [D]. Harbin: Harbin Engineering University, 2013.

[8] Jakuba M V, Roman C N, Singh H, et al. Long-baseline acoustic navigation for under-ice autonomous underwater vehicle operations[J]. Journal of Field Robotics, 2008, 25(11-12): 861-879.

[9] 寧津生, 吳永亭, 孫大軍. 長基線聲學定位系統發展現狀及其應用[J]. 海洋測繪, 2014(01): 72-75. Ning Jin-sheng, Wu Yong-ting, Sun Da-jun. The development of LBL acoustic positioning system and its application[J]. Hydrographic Surverying and Charting, 2014(1): 72-75.

[10] Batista P, Silvestre C, Oliveira P. Sensor-based long baseline navigation: observability analysis and filter design[J]. Asian Journal of Control, 2014, 16(4): 974-994.

[11] Liu Y, Li X R. Aided strapdown inertial navigation for autonomous underwater vehicles[C]//Signal and Data Processing of Small Targets 2010. Vol.7698(76981H).

Underwater positioning system based on SINS / DVL and LBL interactive auxiliary for AUV

ZHANG Tao1,2, CHEN Li-ping1,2, SHI Hong-fei1,2, HU He-qing1,2
(1. School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China; 2. Key Lab of Micro Inertial Instruments and Advanced Navigation Technology, Ministry of Education, Southeast University, Nanjing 210096, China)

An underwater positioning system based on SINS/DVL and LBL interactive auxiliary for AUV is designed to solve the defects of the underwater positioning system. The system consists of SINS/DVL and sound source installed on the AUV and LBL acoustic positioning array clothed on the known sea-bottom positions. The estimated location information of SINS/DVL can not only assist to calculate the time difference of arrivals and correct the sound velocity under water to improve the accuracy of LBL positioning estimation, but also be taken as the iterative initial value for Taylor series expansion iterative method to assist LBL location. Then the location information estimated by LBL system is used to correct the cumulative error of SINS/DVL. Simulation results show that, compared with conventional approach, the system can more effectively correct the cumulative error of SINS/DVL system when AUV is closer to LBL array,and the location error is less than 2 m, which is applicable to underwater navigation and positioning.

autonomous underwater vehicle; SINS; LBL; underwater positioning system

V249.3

：A

2015-08-23；

：2015-11-11

國家自然科學基金資助項目（51375088）；微慣性儀表與先進導航技術教育部重點實驗室基金（201403）；優秀青年教師教學科研資助計劃（2242015R30031）；基于大數據架構的公安信息化應用公安部重點實驗室（浙江警察學院）開放課題資助項目（2015DSJSYS002）

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

1005-6734(2015)06-0769-06

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