基于雙主交替領航的多AUV協同導航方法

高偉，劉亞龍，徐博，唐李軍

（1.哈爾濱工程大學自動化學院，黑龍江哈爾濱150001；2.中國艦船研究設計中心，湖北武漢430000）

基于雙主交替領航的多AUV協同導航方法

高偉1，劉亞龍1，徐博1，唐李軍2

（1.哈爾濱工程大學自動化學院，黑龍江哈爾濱150001；2.中國艦船研究設計中心，湖北武漢430000）

基于單主模式的AUV協同導航系統的可觀測性弱，對主AUV的機動性要求較高，實現較為困難，針對這一問題，提出一種基于雙主交替領航的多AUV協同導航方法。在相鄰量測時刻，從AUV分別利用不同主AUV的距離觀測進行導航誤差的協同校正。首先建立了AUV協同導航系統的數學模型；然后利用譜條件數法對AUV協同導航系統的可觀測性進行了定量分析，明確了系統可觀測度大小與相鄰時刻主從AUV機動狀態的對應關系，為利用雙主交替領航提高系統可觀測性提供了理論依據。通過與單主方案進行對比試驗，驗證了雙主交替領航方案的有效性和可行性。

AUV；協同導航；非線性系統；可觀測性；條件數；機動性

多AUV協作系統是近年來AUV應用領域的一種主流趨勢和發展方向。利用多AUV協同作業不僅能夠承擔單體AUV難以勝任的諸多復雜任務，而且具有高效率、高可靠性的優點，實現了1+1＞2的效果，具有廣闊的應用前景。基于水聲通信網絡的AUV協同導航技術是伴隨AUV協作系統發展而來的一種新的AUV水下導航方案，相比傳統的慣性導航、聲學基線導航，協同導航不僅精度高，而且成本低、魯棒性好，可以大大增強多AUV系統的協同作業能力，具有重要的研究價值［1-3］。理論上，通過融合多個已知參考AUV的距離觀測信息可以實現自身位置信息的精確估計。然而由于水聲傳輸速率低、時間延遲大且信號易阻塞，導致無法實現觀測信息的時鐘同步性，對處于運動狀態的AUV協作系統來說，無論系統中有幾個主AUV，相當于每次只能利用單個主AUV的距離觀測信息進行協同導航。系統可觀測是實現AUV協同導航的前提條件，由于單領航系統的觀測信息量不足、系統可觀測性弱，因而如何增強系統的可觀測性，進而提高系統的協同導航效果對于AUV協同導航系統來說至關重要［4-5］。通過設計復雜的航路規劃方案可以滿足協同導航系統的可觀測性［6-7］，然而對于航速慢，機動性差的AUV系統來說，為了提高系統的可觀測性，在實際執行任務過程中進行復雜的航路機動往往是不現實的。針對以上問題，本文提出一種基于雙主交替領航的AUV協同導航方法，通過位于不同方位的2個主AUV交替領航，等效于AUV相鄰量測時刻的航路機動，增強了系統的可觀測性，提高了AUV的協同定位效果。

1 雙艇交替協同方案

采用雙領航艇，每條領航艇均配備高精度慣性測量單元，結合多普勒計程儀（DVL）、深度傳感器等構成組合導航系統，實現相對精確導航。為降低成本，從艇僅利用低精度航向基準系統、DVL以及深度傳感器進行航位推算導航。雙主交替協同的本質就是通過對位于不同方位的2個領航艇進行交替觀測，近似等效于單艇協同方案中在相鄰量測時間段領航艇的航路機動，進而增強系統的可觀測性，并最終提高系統的協同導航性能。為了實現AUV間的協同導航，主從AUV均配備水聲通信Modem，構成水聲移動通信網絡，用以實現AUV間相對距離的測量以及參考信息的傳遞。基于水聲雙程測距的雙主交替協同過程如圖1所示。其中三角形代表主AUV，五角星代表從AUV，五角星周圍陰影區域代表從AUV的位置狀態的協方差橢圓。

圖1 雙主交替協同導航方案Fig.1 Cooperative navigation scheme with two-leader alternation

主AUV1首先向從AUV發射ping脈沖測距請求信號進行水聲測距，當主AUV1接收到從AUV發送的測距應答信號后，根據水聲信號往返傳播時間計算出二者之間的相對距離d1；緊接著，主AUV1將獲得的水聲距離信息d1連同自身位置信息廣播發送給從AUV，從AUV利用該參考信息進行協同導航，實現對自身航位推算誤差沿觀測距離方向的一次校正。一次協同過程完成后，主AUV2重復主AUV1協同過程，即首先對從AUV進行水聲測距，獲得相對距離信息d2后向從AUV廣播發送自身位置信息以及觀測到的水聲距離信息，進而實現對從AUV的二次協同校正過程。可以看出，每經歷一次協同校正過程，從AUV沿觀測距離方向的位置誤差不確定度就會明顯減小。因此，在整個AUV系統運行過程中，按照圖中箭頭所示，通過位于不同方位的2個主AUV對從AUV進行交替協同，可以更有效地實現AUV航位推算誤差的協同校正。

2 雙艇交替協同信息融合算法

2.1 系統模型建立

AUV的3個狀態xk、yk、zk分別表示經度、緯度和深度信息。由于深度信息可以直接由深度傳感器精確獲得，為了分析問題簡便，接下來只考慮二維平面下的協同導航問題［8］。AUV的運動方程可以表示為

式中：xk-1、yk-1、vk-1、θk-1分別為tk-1時刻，從AUV的東北向位置、速度和航向信息，Δt為采樣周期。

由式（1）得到系統狀態方程：

式中：Xk＝（xk，yk，θk）T表示AUV在tk時刻的狀態向量；Φk，k-1為狀態轉移矩陣；Γ（uk+wk）為非線性項，其中uk-1＝（vk-1，θk-1）T表示控制輸入量；wk＝（wvk，wθk）T為系統過程噪聲，包括速度量測噪聲和方位量測噪聲協方差矩陣Qk：

由于系統噪聲的存在，導致從AUV航位推算誤差及其不確定度不斷增加。多AUV協同導航的本質就是利用具有高精度定位信息的主AUV對從AUV的航位推算誤差進行校正，提高從AVU的定位精度。

水聲測距得到主從AUV三維空間下的距離z3D，k，已知主從AUV深度信息dC、dA條件下，二維平面下的主從AUV量測距離可以表示為

式中：wdk為水聲測距噪聲，為相互獨立不相關的高斯白噪聲，協方差矩陣為

距離量測方程為非線性，已知k時刻主AUV位置信息(xmk，ymk)，則觀測方程對于狀態Xk的雅可比矩陣可以表示為

2.2 EKF濾波算法

觀測方程為非線性，本文采用EKF濾波算法進行信息融合（如圖2所示），對從AUV位置狀態進行估計。由于水聲通信頻率低（假設周期為M），在未接收到水聲信號時，從AUV以較高的頻率進行航位推算（假設周期為T）。由于系統誤差存在，協同周期越長，由推位產生的系統誤差方差也越大。

圖2 信息融合流程圖Fig.2 Information fusion flow chart

因此，每對系統狀態按照航位推算執行一次狀態預測過程之后，相應的要對系統誤差協方差矩陣進行一次誤差預測，以便為后續的狀態更新過程提供正確的系統誤差分布。整個信息融合過程可以分為2部分：未接收到主AUV參考信息時，系統執行開環估計過程，對系統狀態以及相應的誤差協方差矩陣進行預測：

當成功接收到主AUV參考信息后，系統執行狀態更新過程，對系統誤差進行校正：

3 可觀測性分析

所謂量測系統的能觀性，是指通過有限時間內的一系列量測量確定系統狀態的能力。針對線性離散時變系統

能否由一組觀測值Z＝［Z1Z2… Zk］T最終確定系統狀態Xk是系統是否可觀測的實質。根據給定條件，將量測和狀態的對應關系寫成矩陣形式：

則有

式中：

由式（15）可以看出，要想根據觀測序列Z1，Z2，…，Zk確定狀態Xk，充分必要條件是矩陣Wo（0，k）非奇異，這里Wo（0，k）稱為系統的能觀測性格拉姆判別矩陣。

對于本文所研究的離散時變觀測系統，可以進一步給出系統可觀測判別矩陣的等價形式：

式中：Γ（k-N+1，k）為離散時變系統的可觀測秩判別矩陣，n為系統狀態向量維數。

對于一維觀測信息Z，需要至少進行N＝2次觀測才可能完全估計出系統的二維位置狀態。已知相鄰量測時刻2條主AUV的位置信息，根據觀測距離得到系統的可觀測矩陣：

根據系統可觀測性判別條件，系統可觀測當且僅當系統可觀測矩陣滿秩，即系統的可觀測矩陣行列式不為零：

即可觀測。對于雙艇交替領航系統來說，當且僅當2條主艇相對從艇距離向量方向相同時系統不可觀測，在其他條件下均可觀測。為了進一步明確系統可觀測度大小與系統狀態的量化對應關系，下面采用基于譜條件數的可觀測性分析理論，進一步對系統的可觀測性進行定量分析［9］。條件數反應了線性方程組

的解對b的誤差或不確定度的敏感性的度量。A的條件數越大，b的微小改變引起解X的改變越大，數值穩定性越差。A的條件數越小，b的微小改變引起X的改變也越小，數值穩定性也越好。

設A∈Cn×n，‖·‖是一種算子范數：

稱cond（A）為矩陣A（關于求逆或求解線性方程組）的關于算子范數‖·‖的條件數。常用的關于p范數‖·‖p的條件數記為cond（A）p，稱cond（A）2為譜條件數［10］。

矩陣的條件數越大，數值穩定性越差，反之，數值穩定性越好。因此，通過觀測矩陣條件數的大小即可以反應出系統的可觀測性強弱。由于矩陣條件數是一個大于等于1的正數，定義系統可觀測度為可觀測矩陣譜條件數的倒數：

令θk為k時刻主從AUV距離向量方位角，則觀測矩陣可以進一步表示為

其中，定義相鄰時刻主從AUV距離向量方位變化Δθ＝θk-θk-1，得到系統可觀測度解析表達式：

通過對式（25）進行仿真分析可以看出，系統可觀測度大小與相鄰時刻從AUV相對2個主AUV的觀測距離向量夾角有關，向量夾角越接近于90°，系統可觀測度越大。對于采用單領航方案來說，根據上述可觀測條件，要想得到較好的系統可觀測性，需要主從AUV在相鄰量測時刻發生較大的相對位置狀態變化，而這在實際應用過程中往往是比較困難的。由于在協同導航系統中，從AUV只需要根據接收到的參考位置信息以及相對于該參考位置的距離信息進行協同定位，而對于該參考位置信息到底來自于哪個主AUV效果是一樣的。因此，通過采用雙領航交替協同方案可以很容易滿足系統的可觀測性要求。只要保證主從AUV一定的位置關系，在相鄰時刻對不同主AUV交替進行信息觀測，等效于主從AUV在相鄰時刻發生較大的狀態機動，系統獲得較高的可觀測性，如圖3所示。需要說明的是，采用分步式濾波方案，系統的定位精度與相鄰量測時間間隔有關。由于每一次量測更新只是對一個方向上的定位誤差進行修正，如果量測時間間隔過長，由于推算誤差的累積，系統的協同精度會受到影響。

圖3 系統可觀測度與相鄰時刻主從AUV距離向量關系Fig.3 Relationship between the observability and the master-slave AUV distance vector at adjacent time

4 試驗驗證

4.1 試驗條件

2012年10月份，于無錫太湖水域進行了基于水聲通信網絡的多AUV協同導航驗證性試驗。利用水面艇模擬AUV分別對單主協同和雙主交替協同方案進行了驗證。試驗水面艇及相關試驗設備如圖4所示。

圖4 試驗用水面艇及相關試驗設備Fig.4 Vessel used in our work and the on-board equipments

水聲通信設備采用Teledyne Benthos公司的ATM-885型低頻水聲通信設備，該設備具有全向及定向水聲信號發射的能力，同時還可以實現設備間一對一的水聲雙程測距。試驗過程中，利用該設備構建水聲通信網絡（如圖5所示），實現主從艇間距離的量測和位置信息的傳送。

主艇的位置參考信息由艇上GPS提供，從艇利用磁羅經提供的航向信息以及由GPS獲得的速度信息進行初步的航位推算，當接收到主艇發送的位置和距離參考信息后，采用擴展卡爾曼濾波（EKF）技術對自身的航位推算誤差進行協同校正。試驗過程中，主艇每隔30 s對從艇進行一次水聲測距，成功測距后主艇將該距離信息連同自身當前時刻的位置信息一同發送給從艇。從艇成功接收主艇的水聲信號后，融合該信息進行位置狀態的估計，實現航位推算誤差的協同校正。

圖5 水聲通信網絡示意圖Fig.5 Underwater acoustic communication network

4.2 試驗結果

4.2.1 水聲測距誤差分布

水聲測距誤差直接影響AUV的協同導航效果，試驗過程中首先通過靜態條件下選取不同基準距離對水聲測距精度進行了評估。從水聲測距誤差分布直方圖6可以看出，測距誤差滿足高斯分布（均值0.21 m，方差0.528 m），最大測距誤差不超過2 m。

圖6 靜態條件下水聲測距誤差分布Fig.6 Acoustic ranging error distribution in static

4.2.2 協同導航

試驗1 采用單主協同模式，主從AUV運行軌跡如圖7所示，試驗共進行了50 min，試驗過程中主從AUV成功進行了70次水聲通信，通信成功率約為70%，平均協同周期約為42 s，AUV運行速度2.1 m／s。通過比對圖8定位誤差曲線與圖9距離方位變化曲線可以看出，前30 min，由于載體機動不明顯，協同定位效果較差，30 min以后，由于相鄰量測時刻主從AUV距離向量發生較大的方向變化，協同定位效果較好。證明了采用單領航方案需要主從AUV保持較高的航路機動，否則協同效果會較差。

圖7 單主協同AUV運行軌跡Fig.7 AUVs’trajectory based on single leader

圖8 單主協同定位誤差Fig.8 Positioning error based on single leader

圖9 相鄰量測時刻觀測方位變化量（單主）Fig.9 The relative azimuth variation of leader and follower between adjacent measurement time（single leader）

試驗2 采用雙主交替協同模式，試驗過程中，主從AUV基本未做較大的狀態機動，運行軌跡如圖10所示。試驗時間約為13 min，共成功進行了20次水聲通信，平均協同周期約為39 s，AUV運行速度2.5 m／s。由誤差曲線圖可以看出，整個過程中，協同定位效果比較穩定。對比單主協同模式，之所以最后協同定位精度會比單主協同精度低，這是由于單主協同過程后一階段主AUV做圓周運動，本身起到了對航推算誤差的校正作用，若不做類似的機動運動定位效果會很差。

圖10 雙主協同AUV運行軌跡Fig.10 AUVs’trajectory based on two leaders

圖11 雙主協同定位誤差Fig.11 Positioning error based on two leaders

圖12 相鄰量測時刻觀測方位變化量（單主）Fig.12 The relative azimuth variation of leader and follower between adjacent measurement time（two leaders）

5 結束語

本文采用雙主交替領航，近似等效于單主模式中相鄰量測時刻主從AUV相對觀測方位的變化，有效克服了協同導航系統對主AUV的機動性要求，增強了協同導航性能。通過采用譜條件數法對協同導航系統的可觀測性進行定量分析，明確了系統可觀測度大小與相鄰量測時刻主從AUV距離向量夾角之間的關系，向量夾角越接近于90°，系統可觀測度越大。理論上證明了，只要2個主AUV相對從AUV保持一定的幾何位置關系，在系統運行過程中不需要進行航路機動即可保證系統的可觀測性。通過湖上驗證性試驗，對單主協同以及雙主交替協同模式進行了比較分析，得出了雙主交替領航可以在不需要主AUV機動情況下實現較高協同導航效果的結論，進一步驗證了雙主交替領航模式的有效性。本文提出的協同導航方法簡單、有效，具有重要的實際應用價值。由于試驗條件所限，本文的驗證性試驗是采用水面艇代替進行的，由于近水面水聲環境復雜，影響水聲通信效果，后續的研究工作將逐步采用水下AUV進行協同導航效果驗證。

［1］AKYILDIZ I F，POMPILI D，MELODIA T.Underwater acoustic sensor networks：research challenges［J］.Ad Hoc Networks，2005，3（3）：257-279.

［2］HEIDEMANN J，YE W，WILLS J et al.Research challenges and applications for underwater sensor networking［C］／／Proceedings of Wireless Communications and Networking Conference.Las Vegas，USA，2006：228-235.

［3］WEBSTER S E，EUSTICE R M，MURPHY C，et al.Toward a platform-independent acoustic communications and navigation system forunderwatervehicles［C］／／Proceedingsof OCEANS 2009，MTS／IEEE Biloxi-Marine Technology for Our Future：Global and Local Challenges.［S.l.］，2009：1-7.

［4］GADRE A.Observability analysis in navigation systems with an underwater vehicle application［D］.［S.l.］：Virginia Polytechnic Institute and State University，2007：12-23.

［5］FALLON M F，PAPADOPOULOS G，LEONARD J J，et al.Cooperative AUV navigation using a single maneuvering surface craft［J］.The International Journal of Robotics Research，2010，29（12）：1461-1474.

［6］CHIRE M.Path planning for cooperative underwater rangeonly navigation using a single beacon［C］／／Proceedings of International Conference on Autonomous and Intelligent Systems（AIS）.［S.l.］，2010：1-6.

［7］TECK T Y，CHITRE M.Single beacon cooperative path planning using Cross-Entropy method［C］／／Proceedings of OCEANS 2011.［S.l.］，2011：1-6.

［8］張立川，徐德民，劉明雍，等.基于移動長基線的多AUV協同導航［J］.機器人，2009，31（06）：581-585.ZHANG Lichuan，XU Demin，LIU Mingyong，et al.Cooperative navigation of multiple AUVs using moving long baseline［J］.Robot，2009，31（06）：581-585.

［9］錢偉行，劉建業，趙偉，等.基于轉動基座的SINS初始對準方法研究［J］.宇航學報，2008，29（3）：928-932.QIAN Weixing，LIU Jianye，ZHAO Wei，et al.SINS initial alignment based on revolving base［J］.Journal of Astronautics，2008，29（3）：928-932.

［10］戴華.矩陣論［M］.北京：科學出版社，2001：189-193.

Multiple-AUV cooperative navigation based on two-leader alternated navigation

GAO Wei1，LIU Yalong1，XU Bo1，TANG Lijun2
（1.School of Automation，Harbin Engineering University，Harbin 150001，China；2.China Ship Development and Design Center，Wuhan 430000，China）

Due to the weak observability of autonomous underwater vehicles（AUVs）cooperative navigation with a single leader，high maneuverability is required for the main AUV as leader.However，it is always difficult to implement in practice.A cooperative navigation method based on two-leader alternated navigation has been proposed，and the range information observed from different leaders in adjacent measuring time was used for the AUVs＇cooperation.The mathematical model of the cooperative navigation system was first established，and then the observability was analyzed quantitatively by using the spectral condition number method.The relationship between the observability and the maneuvering state of the leader-follower in adjacent time was confirmed and the theoretical basis of the alternated support with the two-leader AUVs was provided.In comparison with the traditional method with only one leader，this proposed method was verified to be more effective and feasible.

autonomous underwater vehicles；cooperative navigation；nonlinear system；observability；spectral condition number；maneuverability

10.3969／j.issn.1006-7043.201307022

http：／／www.cnki.net／kcms／doi／10.3969／j.issn.1006-7043.201307022.html

U666.11

A

1006-7043（2014）06-0735-06

2013-07-10.網絡出版時間：2014-05-15 16：46：34.

國家自然科學基金資助項目（61203225）；國家博士后基金資助項目（2012M510083）.

高偉（1977-），男，教授，博士生導師；劉亞龍（1986-），男，博士研究生.

劉亞龍，E-mail：yalong＠163.com