AUV協同導航定位算法研究

姜朝宇，王喜龍

(海軍駐葫蘆島四三一廠代表室，遼寧葫蘆島125004)

0 引言

隨著地面及空中移動平臺協同導航研究的成熟，人們開始將協同導航擴展到水下平臺，尤其是AUV的協同導航。Hunt首先提出了標準長基線法 (LBL)［1］，Atwood將其用于多 AUV的協同導航［2］，AUV依靠固定在水面或是海底已知位置的信標定位，這種信標至少需要2個，當AUV向信標發出詢問信號后，信標立即返回一個應答信號，AUV便可利用該信標的位置并結合聲波在水中的傳播速率和信號往返的時間差計算出自己相對于信標的可能位置估計，AUV同時相對2個或者2個以上信標的可能位置估計的交點即為AUV實際所處的位置，多AUV之間通過分時詢問共享信標。Vaganay等改進了該方法提出了移動長基線法(MLBL)［3］，這種方法將信標接入GPS，可任意移動，極大地擴展了AUV的可操作區域。Matos和Zhang［4－5］對該方法進行了進一步研究，先后提出了基于雙領航者和單領航者的協同導航定位方法。Liu［6］等對基于單信標測距的協同導航協同進行了能觀性分析。Larsen通過與INS結合，提出了虛擬長基線法 (SLBL)法［7－8］。Rigby［9］等對超短基線法(USLS)進行了研究。為了多AUV協同導航具有更好的可擴展性，Cureio［10］等提出了時鐘同步和單程傳播時間 (OWTT)距離測量的方法并用ASC進行了實驗。Eustice［11－12］等將其擴展到 AUV，使 AUV協同導航系統可以容納的AUV數目得到了擴展，并對該系統進行了實驗。Liu［13－14］等考慮海洋環境影響和AUV在三維空間的動力學特性，提出了未知洋流影響下的協同導航算法并對其能觀性進行了分析。為提高AUV協同導航系統的經濟型和實用性，Liu等將凸優化算法用于AUV協同定位［15］，利用部分廉價傳感器即可取得較好的定位精度。此外，為了提高 LBL的導航性能。Bingham 和 Seering［16］還提出了一種基于假想柵格的方法。Olsen［17］等也進一步研究 LBL提出了 ROBL算法;Ruiz［18］借鑒地面移動平臺的方法提出了針對于AUV的同步定位與構圖法 (SLAM)。同時為了提高AUV導航的準確度和精度引入了許多估計算法，如擴展卡爾曼濾波 (EKF)、粒子濾波 (PF)和無跡卡爾曼濾波(UKF)［19－21］等。

本文首先對近年來多AUV協同導航領域的研究境況進行綜述，然后介紹AUV協同導航的基本結構并分析了其特征;第2部分詳細介紹基于Kullback－Leibler距離的多AUV協同導航算法;第3部分通過數值仿真驗證第2部分中介紹的有效性，對該算法的性能進行分析。最后總結當前研究的熱點并展望AUV協同導航定位的研究方向。

1 AUV協同導航基本結構及特征

多AUV協同導航系統，按照AUV在協同系統中的重要性，可分為主從式和平行式，其中主從式又包括單主、雙主和多主等結構。主從式結構中主AUV配備高性能的導航系統并可以依靠自身的傳感器測量進行有界誤差定位，同時主AUV將承擔協同導航中大量的數據存儲和計算任務;而從AUV通常配備低精度的導航系統。主AUV通過與從AUV通信實現自己高精度的導航信息在整個AUV群體中的共享。從AUV利用該導航信息并通過相對位置測量計算自己的位置估計。通過這種方式提高整個AUV群體的定位準確度和精度，如圖1所示。

平行式結構中，每個AUV配備相同的導航系統和轉發器，通過接受來自信標的聲波信號修正自己的誤差。同時各AUV之間通過相互廣播自己的導航信息實現導航信息的共享，并結合相互的距離測量來更新自身的位置，如圖2所示。

圖1 AUV的主從式結構Fig.1 Master-slave structure AUV

圖2 AUV的平行式結構Fig.2 Parallel structure AUV

多AUV協同導航系統通過信息的共享，可以獲得單AUV導航系統無法具備的優勢:

1)部分具有高性能導航設備的AUV通過通信共享其導航信息可以實現其高性能導航設備在整個AUV群體中的共享。

2)在多AUV協同導航系統中，只要部分AUV具有有界誤差的定位能力，通過協同導航，就可以使系統中的每一個AUV都具有誤差有界的定位能力。例如在水下進行協同導航的AUV群體中，某一個AUV浮出水面使用GPS定位進行誤差修正后，通過共享其導航信息就可以使得群體中其他AUV的定位誤差有界［22］。

3)多AUV協同導航系統具有更強的容錯能力和穩定性。當系統中部分AUV因為傳感器故障或是環境因素而失去獨立導航能力時，協同導航可以在一定程度上恢復這些AUV的導航能力。如果某個AUV自身的導航系統無法正常工作，但是能夠和其他正常工作的平臺相互通信并且進行距離或者方位觀測，那么這個AUV的位置可以通過協同導航來估計。

2 AUV協同導航定位算法原理

AUV的協同導航關鍵是利用其他AUV的導航信息，通過信息融合以維持一個準確的位置估計，因此近幾年針對AUV的協同導航定位提出了多種位置估計方法，如 EKF，PF和 UKF等。本文針對AUV的協同導航系統引入一種新的基于Kullback－Leibler距離的估計算法，該算法通過主從AUV之間的距離量測和導航信息共享可有效提高AUV的定位精度。

2.1 多AUV協同導航的數學模型

針對AUV協同導航問題，由于其深度值可以直接準確測量，故可將三維坐標投影到二維平面，考慮如下二維非線性離散系統，其運動方程為:

式中:xk，yk和θk分別為AUV在二維投影平面中的橫向坐標、縱向坐標和偏航角度;Vk和分別為AUV的前向速度和偏航角速度;T為在將連續系統離散化時傳感器的采樣周期，而其中Vk和可由AUV的本體傳感器測量，為系統的控制輸入，可建模如下:

假設多AUV協同導航系統為主從式單主結構，其觀測方程為:

其中 dxk，k+1和 dyk，，+1分別為 AUV 在采樣周期時間內走過的水平位移和縱向位移，可由航位推算傳感器測量;上標S為從AUV;上標M為主AUV;Rk為k時刻主、從AUV之間的距離。

由此可將該非線性系統表示為:

2.2 基于Kullback－Leibler距離的協同導航方法

在k時刻，從AUV成功接收到一個數據包后，它將獲取以下信息:關于主AUV的位置估計xM(k)=［xM(k)，yM(k)］T，一個表達了xM(k)中各分變量可信度的協方差矩陣PM(k)，一個深度測量值zM(k)和一個主、從AUV之間的距離測量值rS，M(k)。

狀態xM(k)和協方差PM(k)可能是來自于運行在主AUV上的導航濾波器的一個快照，也有可能是來自于GPS的一個快照 (在主AUV位于水面上的時候)。距離測量值rS，M(k)通過PPS同步特性而直接算得。許多實驗表明該距離測量的誤差與距離的遠近只具有弱依賴性而且可以使用具有均值rS，M(k)和固定方差的高斯噪聲建模。

在t(n)時刻從AUV所有的可能的位置都分布在以主AUV為中心 (假設坐標為xM(n))，為半徑的圓上，同理t(m)時刻AUV所有可能的位置都分布在主 AUV為圓心 (設坐標為 xM(m))，r(m)為半徑的圓上，將圓xM(n)按照 t(n)到t(m)這段時間內從AUV運動的距離向量=平移后得xM(n)'。求圓xM(n)'和圓xM(m)的交點，解這兩個圓組成的線性方程，可得到一個解集，這個解集中可能包含1個、2個或者0個解:

如果將n從1到m－1取值，那么最多可計算出2(m－1)個解。在接下來的計算中假設使用了q個解。求交函數相對于測量和傳送參數的雅克比可以用來計算出解xS(m)的協方差矩陣

其中:

同時也定義一個位置矩陣T(m－q)，將過去的從時刻t(m－q)到時刻t(m－1)AUV所有可能的位置存儲在其中，同時也包括該位置的協同矩陣(m－o)和一個累積的轉換代價cu(m－o)。

如果初始時候有一個已知的位置xS(0)(比如在水面的時候從GPS中獲取)，可利用這個位置初始化T(0)=［xS(0)c(0)=0］。如果沒有初始化位置可以利用，就用第一個解的集合S(0)初始化T(0)，只有當接收到連續的信息包后的位置估計才有效。

S(m)中任意一行 (如xSv(m))與T(m－o)的每一行分別代入上式可計算出q2個累積代價，

選取其中的最小值作為cv，然后將其整合到S(m)形成T(m)。

在T(m)中cv最小的那一行所對應的位置xv即為最可能的位置。

3 AUV協同導航定位算法性能分析

為分析算法性能，對上述算法進行仿真研究。在仿真實驗中，根據式(1)的運動模型，不考慮AUV的動力學和水流的干擾，假設主、從AUV位于同一水平面內做直線運動，且初始航向角均為45°，前向航速均為5 m/s，偏航角速度均為0。主AUV配備高性能傳感器，因此具有較高的定位精度，控制輸入噪聲為=(0.5 m/s)2，=(0.03°/s)2，從AUV的速度傳感器測量噪聲取=(2 m/s)2，=(0.05°/s)2，距離測量的噪聲=(10 m)2，系統中的噪聲均假設為零均值高斯白噪聲，更新頻率為0.1 Hz。

基于LKD的算法在剛開始一段時間內位置估計誤差較大 (若初始位置未知則剛開始一段時間內的位置估計無效)，而后位置估計恢復正常，其仿真結果如圖3所示，其誤差如圖4所示。

圖3 KLD估計仿真結果Fig.3 The simulation results of KLD estimation

圖4 KLD估計仿真結果Fig.4 The simulation error of KLD estimation

LKD算法計算一個位置估計的復雜度為o(q3)，其中q為需要考慮的過去的測量值的數目。受限于數據包傳送過程，LKD算法的計算頻率最大為0.1 Hz，且每一個數據包傳輸時間約為10 s。這樣當q≈10時，在主頻為1 GHz的PC上計算一個新位置的時間約為0.1 s。

4 AUV協同導航定位算法的研究展望

雖然近年來對AUV協同導航領域已經展開了許多研究，但仍有許多問題尚未解決或需要進一步研究:

1)目前AUV協同導航中的運動建模通常都進行了極大的簡化，考慮更多因素，如AUV形狀、大小甚至水動力學問題等，與AUV實際運動更相符的建模方法研究;

2)環境因素，如海水的溫度、鹽度、渾濁度和水下固定或移動障礙物等對AUV協同導航性能影響的定性或定量分析;

3)針對協同定位的策略研究，使AUV群的結構具有更好的靈活性和容錯能力性，如主從式結構中若主AUV出現故障的應對策略等;

4)多AUV協同導航系統的智能化研究，特別是借鑒群智能領域的研究成果增加多AUV協同導航協同的智能化水平和應用領域;

5)除對已經提出的估計方法針對特定應用進一步改進外，還需要在非線性理論或其他相關領域中尋找適合于AUV協同導航定位的新方法，特別是借鑒地面和空中移動平臺相關研究成果;

6)結合新的傳感器或環境感知系統提出新的協同導航方法;

7)協同導航的異構研究，包括協同導航方法異構，如同一協同導航系統中的不同AUV采用不同的估計方法，和平臺異構，如水下潛器、水面艦艇和飛行器三方協同。

總之，隨著AUV協同導航系統各項研究的開展，一些問題會被解決，同時許多新的問題將被提出。隨著這些問題的解決和各項研究的成熟，針對特定應用的各種AUV協同導航系統將會出現，這將極大擴展AUV的應用范圍。

［1］ HUNT M，MARQUET W，MOLLER D，et al．An acoustic navigation system［R］．Woods Hole Oceanographic Institution Mass，1974．

［2］ ATWOOD D K，LEONARD J J，BELLINGHAM J G，et al．An acoustic navigation system for multi-vehicle operations［C］International Symposium on Unmanned Uutethered Submersible Technology．University of New Hampshire-Marine Systems，1995:202 －208．

［3］ VAGANAY J，LEONARD J J，CURCIO J A，et al．Experimental validation of the moving long base-line navigation concept［C］//Autonomous Underwater Vehicles，2004 IEEE/OES．IEEE，2004:59 －65．

［4］ MATOS A，CRUZ N．AUV navigation and guidance in a moving acoustic network［C］//Oceans 2005 － Europe．IEEE，2005，1:680 －685．

［5］ ZHANG L，LIU M，XU D，et al．Cooperative localization and navigation for multiple UUVs［J］．Journal of System Simulation，2008，19:065．

［6］ 劉明雍，李聞白，劉富檣，等．基于單信標測距的水下導航系統可觀測性分析［J］．西北工業大學學報，2011，29(1):87－92．

［7］ LARSEN M B．High performance Doppler-inertial navigationexperimental results［C］//Oceans 2000 MTS/IEEE Conference and Exhibition．IEEE，2000，2:1449 －1456．

［8］ LARSEN M B．Synthetic long baseline navigation of underwater vehicles［C］//OCEANS 2000 MTS/IEEE Conference and Exhibition．IEEE，2000，3:2043 －2050．

［9］ RIGBY P，PIZARRO O，WILLIAMS S B．Towards georeferenced AUV navigation through fusion of USBL and DVL measurements［C］//OCEANS 2006．IEEE，2006:1－6．

［10］ CURCIO J，LEONARD J，VAGANAY J，et al．Experiments in moving baseline navigation using autonomous surface craft［C］//OCEANS，2005．Proceedings of MTS/IEEE．IEEE，2005:730 －735．

［11］ GADRE A S，MACZKA D K，SPINELLO D，et al．Cooperative localization of an acoustic source using towed hydrophone arrays［C］//Autonomous Underwater Vehicles，2008．AUV 2008．IEEE/OES．IEEE，2008:1 －8．

［12］ EUSTICE R M，WHITCOMB L L，SINGH H，et al．Experimental results in synchronous-clock one-way-traveltime acoustic navigation for autonomous underwater vehicles［C］//Robotics and Automation，2007 IEEE International Conference on．IEEE，2007:4257 －4264．

［13］李聞白，劉明雍，高俊釵，等．洋流影響下基于單領航者的多AUV 協同導航［J］．魚雷技術，2010，18(004):277－281．

［14］ LI W，LIU M，LEI X，et al．Observability analysis for cooperative navigation system in autonomous underwater vehicles［C］//Intelligent Computing and Intelligent Systems(ICIS)，2010 IEEE International Conference on．IEEE，2010，1:155 －161．

［15］ LI M Y，LI W B，PEI X．Convex optimization algorithms for cooperative localization in autonomous underwater vehicles［J］．Acta Automatica Sinica．v36 i5，704 －710．

［16］ BINGHAM B，SEERING W．Hypothesis grids:Improving long baseline navigation for autonomous underwater vehicles［J］．Oceanic Engineering，IEEE Journal of，2006，31(1):209－218．

［17］ OLSON E，LEONARD J J，TELLER S．Robust range － only beacon localization［J］．Oceanic Engineering，IEEEJournal of，2006，31(4):949 －958．

［18］ TENA Ruiz I，RAUCOURT S，PETILLOT Y，et al．Concurrent mapping and localization using sidescan sonar［J］．Oceanic Engineering，IEEE Journal of，2004，29(2):442－456．

［19］ RUI G，CHITRE M．Cooperative positioning using rangeonly measurements between two AUVs［C］//Proc．Oceans，2010:1－6．

［20］ FOX D，BURGARD W，KRUPPA H，et al．A probabilistic approach to collaborative multi－ robot localization［J］．Autonomous Robots，2000，8(3):325 －344．

［21］劉明雍，沈超，張立川，等．一種基于UKF的AUV移動聲學網絡協同導航方法［J］．西北工業大學學報，2012，29(6):934－938．

［22］ BAHR A，WALTER M，LEONARD J．Consistent cooperative localisation［C］//In IEEE International Conference on Robotics and Automation，2009．