一種基于自適應神經模糊推理系統的多AUV協同定位方法

徐 博，李盛新，王連釗，段騰輝，姚 賀

（1.哈爾濱工程大學 自動化學院，哈爾濱 150001；2.湖南航天機電設備與特種材料研究所， 長沙 410205；3.中船航海科技有限責任公司，北京 100071）

自主水下航行器（Autonomous Underwater Vehi- cles,AUV）是一種集人工智能、計算機軟件、傳感器等先進技術于一體的現代技術產品。隨著人類在海洋探索和開發作業的不斷深入，單一AUV的使用已經不能滿足當前的需求。因此，研究人員將目光投入到由多個AUV組成的協作系統上。然而復雜的水下環境限制了協同系統的應用，在環境探測和監測等大多數水下任務中，需要多AUV協作系統在部分工作區間沒有或基本沒有外界輔助位置信息的情況下完成，導航定位問題成為發展AUV所面臨的重要挑戰[1-3]。陀螺儀和加速度計組成的慣性導航系統（Inertial Navigation System,INS）是目前最常用的水下導航方法。然而，其固有的導航誤差隨時間積累的特性又不利于長航時的高精度導航[4]。隨著水聲通信技術的進步，基于水聲通信網絡的多AUV協同定位技術得到了越來越多的關注和發展。AUV間通過水聲通信實現信息共享，通過水聲測距實現相互觀測，進而通過信息融合技術實現導航定位誤差的協同校正。在協同定位系統中，少數領航AUV配備高精度INS，領航AUV可以通過水聲調制解調器輔助其他配備低精度傳感器的跟隨AUV，這種定位方法不僅能提高系統整體的導航性能，而且具有成本低、實現簡單、無區域限制等優點，是近年來海洋工程領域的研究熱點之一。

對AUV的位置進行估計是水下協同作業中一個至關重要的問題，基于狀態空間模型的濾波技術是目前應用最廣泛的一種方法，它可以實現統計意義上的最優估計，獲得高精度的狀態估計信息。然而，考慮到水下環境的不確定性和水聲通信在實際應用中的復雜性，多AUV協同定位系統的定位性能受到系統內部和外部多種因素的制約[5]。此外，惡劣的水下環境也可能導致水下機器人在一段時間內無法進行通信。在這種情況下，位置估計一般可以僅通過濾波的時間更新進行一步預測得到，但是位置誤差在不進行測量更新的情況下會迅速發散。相關學者提出了一些基于間歇觀測信息的卡爾曼濾波方法[6-7]。然而，所提出的方法計算復雜，在通信包丟失數分鐘情況下，狀態估計的精度無法保證，因此有必要探討一種合適的方法，保證系統在長時間通信包丟失情況下多AUV協同定位精度。

自適應模糊推理系統（Adaptive Neuro-Fuzzy Infe- rence System,ANFIS）是模糊推理系統（FIS）和神經網絡（NN）的結合，在許多領域得到了廣泛的應用[8]。文獻[9]將ANFIS用于無線傳感器網絡的精確定位中，試驗結果表明相對于對數正態陰影模型法的粗略估計，采用ANFIS優化，室內/外距離估計精度均有明顯提高；文獻[10]在ANFIS的基礎上進行了改進，提出了一種基于人工智能的綜合INS/GPS測量方法，并利用實際數據對其性能進行了評價，結果表明與擴展卡爾曼濾波相比，在沒有GPS信號的情況下，僅使用低成本慣性傳感器在GPS信號丟失情況下定位性能有了很大改進。ANFIS以其處理高度非線性問題的效率而聞名，它是對傳統卡爾曼濾波器的一種改進，特別是對動態環境下輸入數據的復雜性和不確定性的有效處理，適用于解決水下環境復雜情況下的多AUV協同定位問題。

基于以上分析，考慮到系統狀態方程和量測方程的非線性及濾波運算復雜度，為了提高水下機器人在惡劣水下環境下的定位精度，本文提出了基于ANFIS輔助非線性濾波的多AUV協同定位方法。在協同定位系統正常通信時，跟隨AUV利用領航AUV位置信息和相對距離信息進行信息融合估計位置，同時對ANFIS模型進行訓練。當協同系統通信數據包丟失時，利用訓練好的ANFIS預測跟隨AUV的位置。實驗結果表明，本文所提方法在多AUV協同定位中具有很好的定位精度和穩定性。

1 多AUV 協同定位系統描述

本文研究了基于領航-跟隨編隊形式的多AUV協同定位問題。領航AUV配備高精度慣性導航設備、深度傳感器（即壓力傳感器）和多普勒速度計程儀（Doppler Velocity Log,DVL），可獲得高精度的位置信息；跟隨AUV裝備低精度的慣性導航設備，可以依靠DVL、羅經和深度傳感器進行粗略的航位推算。在實際應用中，利用深度傳感器可以獲取AUV的精確深度信息。因此，水平位置的計算不受深度信息的影響，三維定位問題可以簡化為二維[11]。

定義跟隨AUV在tk時刻的位置向量為Xk= [λk,Lk]T，基于聲學測距的協同定位系統狀態方程為：

式中，λk、Lk分別為AUV 在tk時刻的經度位置與緯度位置；Δt為采樣時間；vk、ωk分別為由DVL 測得的跟隨AUV 的前向速度與右舷速度；θk為羅經所測AUV 航向，這里航向角為AUV 的前向與北向的夾角，范圍為(-180°,180°)，順時針方向為正；h k為跟隨AUV的深度信息，由深度傳感器獲得；wk=[wλ ,k,wL,k]T為過程噪聲向量；RM,k和RN,k分別是子午線的曲率半徑和卯酉線的曲率半徑，定義為：

式中，Re=6 378 137 m，e=1/298.257。

領航AUV 與跟隨AUV 都安裝了水聲調制解調器，領航AUV 通過水聲調制解調器向跟隨AUV 周期性的廣播自身位置。領航AUV 和跟隨AUV 之間的坐標位置關系為：

式中：rk為領航AUV 與跟隨AUV 之間的相對距離；hkm為領航AUV 的深度；ak、b k、a km和bkm定義如下：

由于壓力傳感器可以準確地獲取深度信息，因此可以將三維距離量測簡化為二維距離量測，如下所示：

式中，δk為量測噪聲。

基于式(1)(9)，建立多AUV 協同定位系統的離散時間狀態空間模型（狀態方程和量測方程）：

式中，f(X k-1,vk,ωk,θk)和分別為狀態轉移函數和量測函數，定義為：

2 自適應神經模糊推理系統

ANFIS 借鑒NN 的學習機制，將其應用于FIS 中，使得FIS 也能夠通過收集數據進行訓練[12]。在FIS 中，一些隸屬函數是通過輸入來給出的，輸入函數相互組合或交互的方式稱為規則，這些規則分為前件參數和后件參數[13]。本文所采用的ANFIS 技術中，考慮了三個輸入、一個輸出和一階Takagi-Sugeno 方法。輸入、輸出和規則描述如下：

輸入：時間(T)、航推位置(L)、速度(V)；

輸出：濾波輸出的位置；

規則1：

規則2：

ANFIS 結構由五層組成，如圖1所示。圓圈和正方形分別代表固定節點和自適應節點。

Layer 1:該層稱為模糊層，該層中的所有節點都被視為自適應節點。在此層中輸出值O1,i可以表示為：

式中，T、L、V是節點的輸入，Ai、Bi、C i分別為與節點T、L、V有關的模糊集合。這里Ai、Bi、C i的隸屬函數被認為是廣義鐘形隸屬度函數（gbellmf），定義如下：

式中，ai、b i、c i為可以改變gbellmf形狀的參數，稱為前件參數，參數數值的變化影響隸屬度函數。

Layer 2:該層稱為規則層，節點標記為Π。該層實現了模糊推理過程，每個節點的輸出表示某一條規則的可信度。該層的輸出值O2,i是通過所有輸入成員函數相乘來計算：

式中，wi表示第i條規則的激勵強度。

Layer 3:該層節點被標記為N。將前一層的輸出做歸一化處理，輸出為處理后的強度值：

式中，為第三層的輸出值。

Layer 4:這一層稱為去模糊層，該層中的所有節點都是自適應節點。該層在歸一化的激勵強度和結果函數之間創建一個自適應關聯函數，輸出值是第三層和第一層的值的乘積：

式中，m i、pi、q i、r i是結果參數，通常稱為后件參數。

Layer 5:該層中唯一的節點是固定節點，標記為Σ。計算ANFIS 的總輸出：

ANFIS中的模糊隸屬度函數和模糊規則是通過大量已知數據進行學習訓練而獲得的，而不是根據經驗或直覺任意給出的，這對于尚未完全理解或具有復雜特征的系統尤為重要[12]。利用該學習過程，對FIS 的前件和后件參數進行優化調整，以準確地表達輸入空間和輸出空間之間的實際數學關系。在每次迭代訓練中，可以減少實際輸出與預期輸出之間的誤差。當達到預定的訓練次數或錯誤率時停止訓練。梯度下降法用于非線性前件參數的優化，而均值最小二乘法用于線性后件參數的調整[14]。

3 基于ANFIS 輔助EKF 的協同定位

考慮到系統狀態方程和量測方程的非線性及濾波運算復雜度，引入擴展卡爾曼濾波。對于非線性系統，EKF 是一種行之有效的濾波方法，但是在量測信息長時間丟失的情況下，EKF 的狀態估計性能會降低，故在EKF 中引入ANFIS 提出基于ANFIS-EKF 的協同定位方法，保證通信包丟失時跟隨AUV 的估計位置精度。

3.1 基于EKF 的協同定位算法

由于式(1)(9)組成的系統模型為非線性系統，因此需采用非線性濾波方法進行狀態估計，這里采用EKF 來實現協同定位算法。假定tk-1時刻的后驗概率估計和后驗概率密度函數已知，算法過程如下：

時間更新：

量測更新：

式中： (·)-1表示矩陣的逆運算； (·)T表示矩陣的轉置運算；和Pk|k-1分別為預測狀態向量和相應的預測誤差協方差陣；Kk為卡爾曼濾波增益；和Pk|k分別為估計狀態向量和相應的狀態誤差協方差陣；F k為f(X k,v k,wk,θk)關于的雅克比矩陣；Gk為f(X k,v k,wk,θk)關于uk=(v k,wk,θk)T的雅克比矩陣；Hk為關于的雅克比矩陣。

式中，Ak=(RN,k+hk)cosLk，Bk=RM,k+hk。

3.2 基于ANFIS-EKF 的協同定位方法

基于水聲通信的協同定位系統，利用EKF 可以實時估計位置。然而，由于水下環境的不確定性和復雜性，水聲通信信道具有很高的不可靠性。跟隨AUV 可能無法接收領航AUV 的測量和位置信息，即通信數據包丟失，這將導致多AUV 協作系統的定位誤差迅速發散。

針對上述問題，提出了一種基于ANFIS 的實時數據融合方法。當水聲通信系統正常工作時建立ANFIS 模型，ANFIS 的輸入包括時間、經度速度和緯度速度、航位推算位置，利用濾波估計的位置信息用作輸出數據。通過對ANFIS 模型參數的訓練，得到了AUV 位置隨時間、速度和DR 位置的變化規律。ANFIS 利用“歷史數據”實現對未來結果的預測，該方法不增加濾波的計算量。模型更新結構和數據預測結構如圖2所示。

該協同定位方法根據測量信息的狀態，分為模型訓練過程和預測過程兩部分。當領航AUV 的位置信息及與跟隨AUV 間的相對距離信息可以通過水聲通信正常傳輸到跟隨AUV 時，ANFIS 模型處于更新學習過程中，此時AUV 的位置由濾波估計得到。同時，利用該階段收集的數據對ANFIS 模型進行訓練，收集的數據為：時間tk、通過航位推算獲得的經緯度位置信息、EKF 估算的經/緯度位置信息測量的速度信息、羅經測量的航向信息。經度和緯度方向的速度信息分別為vλ,k和vL,k：

ANFIS 是一個多輸入單輸出系統，經度位置和緯度位置可分別由兩個模型預測。和為模型1 的輸入和輸出信息，和為模型2的輸入和輸出信息。在此過程中，通過收集的數據對ANFIS 進行訓練，得到非線性前件參數和線性后件參數的最優值，從而獲得位置隨時間、速度和航位推算位置的變換規律。

圖2 基于ANFIS-EKF 的協同定位方法原理圖 Fig.2 Schematic diagram of cooperative localization method based on ANFIS-EKF

當通信包丟失時，系統進入ANFIS 預測過程。在此過程中，ANFIS 可以根據時間、航位推算位置以及經度方向和緯度方向的速度來預測跟隨AUV 的位置。當通信丟包的時間較短時，協同定位系統可以通過濾波的時間更新而不進行測量更新得到跟隨AUV 的位置估計，短時間內定位誤差不會有明顯變化。故本文通過設置閾值的方式，當通信丟包時間小于5 s 時，將濾波的一步預測位置作為實際估計位置；當通信丟包時間大于5 s 時，系統進入預測過程，以ANFIS 預測的位置作為實際估計位置。通過以上兩種過程的切換，ANFIS 可以有效地處理通信包丟失狀態下的定位問題，這為協同定位系統通信包丟失的問題提供了一種有效的輔助定位方案。

4 基于ANFIS-EKF的協同定位方法半實物仿真驗證

4.1 半實物仿真情況概述

本文所提出協同定位方法的有效性和穩定性通過湖試采集的后處理數據驗證，湖水試驗中使用了3 艘勘探船模擬AUV，其中兩艘充當領航AUV，一艘充當跟隨AUV。跟隨AUV 利用水聲調制解調器周期性地獲取領航AUV 的位置及領航AUV 與跟隨AUV 之間的相對距離。基于領航-跟隨模式的多AUV 協同定位如圖3所示，圖中橢圓面積表示定位誤差協方差，紅色虛線橢圓表示AUV 只依據自身裝備慣性導航系統進行定位的誤差協方差，藍色實線表示跟隨AUV通過水聲設備進行協同定位的定位誤差協方差，這種周期性協同定位方法有效地減小了定位誤差。

圖3 基于交替量測的協同定位示意圖 Fig.3 Schematic diagram of cooperative location based on alternating measurement

領航AUV 和跟隨AUV 都配備了GPS 和水聲調 制解調器。GPS 用來提供基準參考位置，AUV 之間通過水下聲學調制解調器進行通信。此外，跟隨AUV配備了DVL 和羅經，傳感器性能參數如表1所示。

表1 傳感器的性能參數 Tab.1 Performance parameters of sensors

領航AUV 和跟隨AUV 的實際軌跡如圖4所示，試驗總時間為1730 s，采樣周期為Δt=1 s。圖4中粗線型表示協同定位系統在1150～1350 s 時水聲通信包丟失。初始狀態估計由GPS 提供，初始狀態估 計誤差協方差矩陣設為P0=diag[1 m,1 m]2，過程噪聲協方差矩陣為Qk=diag[1 m,1 m]2，測量噪聲協方差為Rk=diag[5 m]2。

圖4 領航AUV 和跟隨AUV 的真實軌跡 Fig.4 True trajectory of leader AUV and follower AUV

圖5 兩個領航AUV 的測量交替情況說明 Fig.5 Illustration of the measurements received from two leader AUVs

在每一采樣時刻，兩個領航AUV 中只有一個與跟隨AUV 通信，跟隨AUV 與兩個領航AUV 的量測交替情況如圖5所示。標志位等于1 時，跟隨AUV與領航AUV-1 進行通信；標志位等于2 時，跟隨AUV與領航AUV-2 進行通信，標志位等于0 時，跟隨AUV不與任何領航AUV 進行通信。

由圖5可以看出，在1150～1350 s 時，跟隨AUV與領航AUV 之間的通信包連續丟失201 s。在接下來的實驗描述中，本文將使用上述通信包丟失的數據作為量測信息進行算法驗證。

4.2 協同定位算法驗證

為驗證本文提出方法的有效性，采用定位誤差（Localization Error,LE）、平均定位誤差（Averaged Localization Error,ALE）、均方根誤差（Root Mean Square Error,RMSE）和最大誤差（Maximum error,ME）作為多AUV 協同定位性能指標，定義如下：

式中，(xk,yk)為GPS 提供的tk時刻跟隨AUV 的基準參考位置；為tk時刻協同定位估計位置；T為試驗數據長度。

MATLAB 的神經網絡工具箱為ANFIS 提供了封裝的訓練函數，為了得到ANFIS 模型結構，我們使用網格劃分（Grid Partition,GP）方法來訓練ANFIS，從而確定每個輸入變量的隸屬度函數個數和規則個數，仿真計算機配置為Intel Core i5-8500@3.00Hz。考慮的ANFIS 有三個輸入，分別為時間、航位推算的位置和跟隨AUV 的速度，每個輸入變量都有3 個鐘形隸屬度函數，故規則的數量為27 條。ANFIS 的主要任務是通信包丟失時實時預測跟隨AUV 的位置信息。

圖6 跟隨AUV 經度估計位置的對比 Fig.6 Comparison on estimated values of longitude position of follower AUV

圖7 跟隨AUV 緯度估計位置的對比 Fig.7 Comparison on estimated values of latitude position of follower AUV

圖8 跟隨AUV 軌跡對比圖 Fig.8 Comparison on follower AUV trajectories obtained by different methods

圖9 跟隨AUV 定位誤差對比圖 Fig.9 Comparison on positioning errors of follower AUV obtained by different methods

實驗結果如圖6～9 所示。圖6和圖7分別為不同 定位方法估計的跟隨AUV 經度位置和緯度位置的比較圖，圖8為不同定位方法估計得到的跟隨AUV 軌跡對比，圖9為不同定位方法估計位置與基準位置的距離誤差。

從圖6～9 可以看出，航位推算位置逐漸偏離真實軌跡，這是因為航位推算只依賴DVL 和羅經提供的速度和航向信息計算位置，累積誤差隨時間而增大。在1150～1350 s 時間段內，由于跟隨AUV 與領航AUV 之間的通信包丟失，EKF 量測更新過程無法進行，濾波估計的定位誤差迅速增加。但由于ANFIS 具有利用“歷史數據”預測未來結果的能力，所以通過收集1～1149 s之間的傳感器及濾波輸出的數據對ANFIS 進行訓練，即得到跟隨AUV 位置隨時間、速度及航位推算位置之間的變換規律，使得在通信包丟失期間ANFIS-EKF 方法可以有效抑制定位誤差的發散，實現更準確的位置預測。在1150～1350 s 期間（即通信包丟失的時間段內），不同協同定位方法的定位性能比較如表2所示。

表2 不同協同定位方法在通信包丟失時定位性能比較 Tab.2 Cooperative location performance of different methods when communication packet loss

由表2協同定位性能比較結果可以看出，多AUV協同定位系統在跟隨AUV 與領航AUV 之間通信包丟失時，基于ANFIS-EKF 比僅基于EKF 的協同定位方法平均定位誤差減少78%，均方根誤差減少77%，最大定位誤差減小62 m，具有更好的協同定位精度。基于ANFIS-EKF 的協同定位方法在通信包丟失時可替代EKF 濾波估計，更準確地預測跟隨AUV 的位置。

5 結 論

本文針對多AUV 協同系統通信包丟失的情況，結合傳統的卡爾曼濾波估計，提出了一種基于ANFIS- EKF 的協同定位方法，并通過湖試數據進行了仿真實驗驗證。主要貢獻如下：

1）建立了基于地球坐標系的多AUV 協同導航算法的系統定位模型，提出基于ANFIS-EKF 的多AUV協同定位方法，給出了基于ANFIS-EKF 協同定位方法的詳細流程；

2）對基于EKF 和ANFIS-EKF 的多AUV 協同定位方法通過湖試數據進行了仿真實驗對比，實驗結果表明基于ANFIS-EKF 的多AUV 協同定位方法定位精度明顯提高，協同定位性能更加穩定。

本文提出的基于ANFIS-EKF 協同定位方法可有效保證量測丟失情況下跟隨AUV 的定位性能，為進一步研究多AUV 協同定位系統、提高系統協同定位性能提供了可行性思路和方法。但在實際應用過程中，即使跟隨AUV 接收到了通信包，也極有可能由于水下環境的不確定性使得量測數據受野值、水聲延遲以及洋流等影響，導致濾波估計定位誤差變大，這將間接影響ANFIS 模型訓練的準確性。這些問題仍需要深入研究與分析，接下來的工作將著重探索在惡劣水下環境中如何保證ANFIS 模型訓練的準確性。