AUV協(xié)同導航定位算法研究

姜朝宇，王喜龍

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

0 引言

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

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

1 AUV協(xié)同導航基本結(jié)構(gòu)及特征

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

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

圖1 AUV的主從式結(jié)構(gòu)Fig.1 Master-slave structure AUV

圖2 AUV的平行式結(jié)構(gòu)Fig.2 Parallel structure AUV

多AUV協(xié)同導航系統(tǒng)通過信息的共享，可以獲得單AUV導航系統(tǒng)無法具備的優(yōu)勢:

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

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

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

2 AUV協(xié)同導航定位算法原理

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

2.1 多AUV協(xié)同導航的數(shù)學模型

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

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

假設多AUV協(xié)同導航系統(tǒng)為主從式單主結(jié)構(gòu)，其觀測方程為:

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

由此可將該非線性系統(tǒng)表示為:

2.2 基于Kullback－Leibler距離的協(xié)同導航方法

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

狀態(tài)xM(k)和協(xié)方差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)這段時間內(nèi)從AUV運動的距離向量=平移后得xM(n)'。求圓xM(n)'和圓xM(m)的交點，解這兩個圓組成的線性方程，可得到一個解集，這個解集中可能包含1個、2個或者0個解:

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

其中:

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

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

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

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

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

3 AUV協(xié)同導航定位算法性能分析

為分析算法性能，對上述算法進行仿真研究。在仿真實驗中，根據(jù)式(1)的運動模型，不考慮AUV的動力學和水流的干擾，假設主、從AUV位于同一水平面內(nèi)做直線運動，且初始航向角均為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，系統(tǒng)中的噪聲均假設為零均值高斯白噪聲，更新頻率為0.1 Hz。

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

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

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

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

4 AUV協(xié)同導航定位算法的研究展望

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

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

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

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

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

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

6)結(jié)合新的傳感器或環(huán)境感知系統(tǒng)提出新的協(xié)同導航方法;

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

總之，隨著AUV協(xié)同導航系統(tǒng)各項研究的開展，一些問題會被解決，同時許多新的問題將被提出。隨著這些問題的解決和各項研究的成熟，針對特定應用的各種AUV協(xié)同導航系統(tǒng)將會出現(xiàn)，這將極大擴展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］ 劉明雍，李聞白，劉富檣，等．基于單信標測距的水下導航系統(tǒng)可觀測性分析［J］．西北工業(yè)大學學報，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 協(xié)同導航［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移動聲學網(wǎng)絡協(xié)同導航方法［J］．西北工業(yè)大學學報，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．