一種基于自適應(yīng)神經(jīng)模糊推理系統(tǒng)的多AUV協(xié)同定位方法

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

（1.哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，哈爾濱 150001；2.湖南航天機(jī)電設(shè)備與特種材料研究所， 長(zhǎng)沙 410205；3.中船航海科技有限責(zé)任公司，北京 100071）

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

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

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

基于以上分析，考慮到系統(tǒng)狀態(tài)方程和量測(cè)方程的非線性及濾波運(yùn)算復(fù)雜度，為了提高水下機(jī)器人在惡劣水下環(huán)境下的定位精度，本文提出了基于ANFIS輔助非線性濾波的多AUV協(xié)同定位方法。在協(xié)同定位系統(tǒng)正常通信時(shí)，跟隨AUV利用領(lǐng)航AUV位置信息和相對(duì)距離信息進(jìn)行信息融合估計(jì)位置，同時(shí)對(duì)ANFIS模型進(jìn)行訓(xùn)練。當(dāng)協(xié)同系統(tǒng)通信數(shù)據(jù)包丟失時(shí)，利用訓(xùn)練好的ANFIS預(yù)測(cè)跟隨AUV的位置。實(shí)驗(yàn)結(jié)果表明，本文所提方法在多AUV協(xié)同定位中具有很好的定位精度和穩(wěn)定性。

1 多AUV 協(xié)同定位系統(tǒng)描述

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

定義跟隨AUV在tk時(shí)刻的位置向量為Xk= [λk,Lk]T，基于聲學(xué)測(cè)距的協(xié)同定位系統(tǒng)狀態(tài)方程為：

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

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

領(lǐng)航AUV 與跟隨AUV 都安裝了水聲調(diào)制解調(diào)器，領(lǐng)航AUV 通過水聲調(diào)制解調(diào)器向跟隨AUV 周期性的廣播自身位置。領(lǐng)航AUV 和跟隨AUV 之間的坐標(biāo)位置關(guān)系為：

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

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

式中，δk為量測(cè)噪聲。

基于式(1)(9)，建立多AUV 協(xié)同定位系統(tǒng)的離散時(shí)間狀態(tài)空間模型（狀態(tài)方程和量測(cè)方程）：

式中，f(X k-1,vk,ωk,θk)和分別為狀態(tài)轉(zhuǎn)移函數(shù)和量測(cè)函數(shù)，定義為：

2 自適應(yīng)神經(jīng)模糊推理系統(tǒng)

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

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

輸出：濾波輸出的位置；

規(guī)則1：

規(guī)則2：

ANFIS 結(jié)構(gòu)由五層組成，如圖1所示。圓圈和正方形分別代表固定節(jié)點(diǎn)和自適應(yīng)節(jié)點(diǎn)。

Layer 1:該層稱為模糊層，該層中的所有節(jié)點(diǎn)都被視為自適應(yīng)節(jié)點(diǎn)。在此層中輸出值O1,i可以表示為：

式中，T、L、V是節(jié)點(diǎn)的輸入，Ai、Bi、C i分別為與節(jié)點(diǎn)T、L、V有關(guān)的模糊集合。這里Ai、Bi、C i的隸屬函數(shù)被認(rèn)為是廣義鐘形隸屬度函數(shù)（gbellmf），定義如下：

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

Layer 2:該層稱為規(guī)則層，節(jié)點(diǎn)標(biāo)記為Π。該層實(shí)現(xiàn)了模糊推理過程，每個(gè)節(jié)點(diǎn)的輸出表示某一條規(guī)則的可信度。該層的輸出值O2,i是通過所有輸入成員函數(shù)相乘來計(jì)算：

式中，wi表示第i條規(guī)則的激勵(lì)強(qiáng)度。

Layer 3:該層節(jié)點(diǎn)被標(biāo)記為N。將前一層的輸出做歸一化處理，輸出為處理后的強(qiáng)度值：

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

Layer 4:這一層稱為去模糊層，該層中的所有節(jié)點(diǎn)都是自適應(yīng)節(jié)點(diǎn)。該層在歸一化的激勵(lì)強(qiáng)度和結(jié)果函數(shù)之間創(chuàng)建一個(gè)自適應(yīng)關(guān)聯(lián)函數(shù)，輸出值是第三層和第一層的值的乘積：

式中，m i、pi、q i、r i是結(jié)果參數(shù)，通常稱為后件參數(shù)。

Layer 5:該層中唯一的節(jié)點(diǎn)是固定節(jié)點(diǎn)，標(biāo)記為Σ。計(jì)算ANFIS 的總輸出：

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

3 基于ANFIS 輔助EKF 的協(xié)同定位

考慮到系統(tǒng)狀態(tài)方程和量測(cè)方程的非線性及濾波運(yùn)算復(fù)雜度，引入擴(kuò)展卡爾曼濾波。對(duì)于非線性系統(tǒng)，EKF 是一種行之有效的濾波方法，但是在量測(cè)信息長(zhǎng)時(shí)間丟失的情況下，EKF 的狀態(tài)估計(jì)性能會(huì)降低，故在EKF 中引入ANFIS 提出基于ANFIS-EKF 的協(xié)同定位方法，保證通信包丟失時(shí)跟隨AUV 的估計(jì)位置精度。

3.1 基于EKF 的協(xié)同定位算法

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

時(shí)間更新：

量測(cè)更新：

式中： (·)-1表示矩陣的逆運(yùn)算； (·)T表示矩陣的轉(zhuǎn)置運(yùn)算；和Pk|k-1分別為預(yù)測(cè)狀態(tài)向量和相應(yīng)的預(yù)測(cè)誤差協(xié)方差陣；Kk為卡爾曼濾波增益；和Pk|k分別為估計(jì)狀態(tài)向量和相應(yīng)的狀態(tài)誤差協(xié)方差陣；F k為f(X k,v k,wk,θk)關(guān)于的雅克比矩陣；Gk為f(X k,v k,wk,θk)關(guān)于uk=(v k,wk,θk)T的雅克比矩陣；Hk為關(guān)于的雅克比矩陣。

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

3.2 基于ANFIS-EKF 的協(xié)同定位方法

基于水聲通信的協(xié)同定位系統(tǒng)，利用EKF 可以實(shí)時(shí)估計(jì)位置。然而，由于水下環(huán)境的不確定性和復(fù)雜性，水聲通信信道具有很高的不可靠性。跟隨AUV 可能無法接收領(lǐng)航AUV 的測(cè)量和位置信息，即通信數(shù)據(jù)包丟失，這將導(dǎo)致多AUV 協(xié)作系統(tǒng)的定位誤差迅速發(fā)散。

針對(duì)上述問題，提出了一種基于ANFIS 的實(shí)時(shí)數(shù)據(jù)融合方法。當(dāng)水聲通信系統(tǒng)正常工作時(shí)建立ANFIS 模型，ANFIS 的輸入包括時(shí)間、經(jīng)度速度和緯度速度、航位推算位置，利用濾波估計(jì)的位置信息用作輸出數(shù)據(jù)。通過對(duì)ANFIS 模型參數(shù)的訓(xùn)練，得到了AUV 位置隨時(shí)間、速度和DR 位置的變化規(guī)律。ANFIS 利用“歷史數(shù)據(jù)”實(shí)現(xiàn)對(duì)未來結(jié)果的預(yù)測(cè)，該方法不增加濾波的計(jì)算量。模型更新結(jié)構(gòu)和數(shù)據(jù)預(yù)測(cè)結(jié)構(gòu)如圖2所示。

該協(xié)同定位方法根據(jù)測(cè)量信息的狀態(tài)，分為模型訓(xùn)練過程和預(yù)測(cè)過程兩部分。當(dāng)領(lǐng)航AUV 的位置信息及與跟隨AUV 間的相對(duì)距離信息可以通過水聲通信正常傳輸?shù)礁SAUV 時(shí)，ANFIS 模型處于更新學(xué)習(xí)過程中，此時(shí)AUV 的位置由濾波估計(jì)得到。同時(shí)，利用該階段收集的數(shù)據(jù)對(duì)ANFIS 模型進(jìn)行訓(xùn)練，收集的數(shù)據(jù)為：時(shí)間tk、通過航位推算獲得的經(jīng)緯度位置信息、EKF 估算的經(jīng)/緯度位置信息測(cè)量的速度信息、羅經(jīng)測(cè)量的航向信息。經(jīng)度和緯度方向的速度信息分別為vλ,k和vL,k：

ANFIS 是一個(gè)多輸入單輸出系統(tǒng)，經(jīng)度位置和緯度位置可分別由兩個(gè)模型預(yù)測(cè)。和為模型1 的輸入和輸出信息，和為模型2的輸入和輸出信息。在此過程中，通過收集的數(shù)據(jù)對(duì)ANFIS 進(jìn)行訓(xùn)練，得到非線性前件參數(shù)和線性后件參數(shù)的最優(yōu)值，從而獲得位置隨時(shí)間、速度和航位推算位置的變換規(guī)律。

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

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

4 基于ANFIS-EKF的協(xié)同定位方法半實(shí)物仿真驗(yàn)證

4.1 半實(shí)物仿真情況概述

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

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

領(lǐng)航AUV 和跟隨AUV 都配備了GPS 和水聲調(diào) 制解調(diào)器。GPS 用來提供基準(zhǔn)參考位置，AUV 之間通過水下聲學(xué)調(diào)制解調(diào)器進(jìn)行通信。此外，跟隨AUV配備了DVL 和羅經(jīng)，傳感器性能參數(shù)如表1所示。

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

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

圖4 領(lǐng)航AUV 和跟隨AUV 的真實(shí)軌跡 Fig.4 True trajectory of leader AUV and follower AUV

圖5 兩個(gè)領(lǐng)航AUV 的測(cè)量交替情況說明 Fig.5 Illustration of the measurements received from two leader AUVs

在每一采樣時(shí)刻，兩個(gè)領(lǐng)航AUV 中只有一個(gè)與跟隨AUV 通信，跟隨AUV 與兩個(gè)領(lǐng)航AUV 的量測(cè)交替情況如圖5所示。標(biāo)志位等于1 時(shí)，跟隨AUV與領(lǐng)航AUV-1 進(jìn)行通信；標(biāo)志位等于2 時(shí)，跟隨AUV與領(lǐng)航AUV-2 進(jìn)行通信，標(biāo)志位等于0 時(shí)，跟隨AUV不與任何領(lǐng)航AUV 進(jìn)行通信。

由圖5可以看出，在1150～1350 s 時(shí)，跟隨AUV與領(lǐng)航AUV 之間的通信包連續(xù)丟失201 s。在接下來的實(shí)驗(yàn)描述中，本文將使用上述通信包丟失的數(shù)據(jù)作為量測(cè)信息進(jìn)行算法驗(yàn)證。

4.2 協(xié)同定位算法驗(yàn)證

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

式中，(xk,yk)為GPS 提供的tk時(shí)刻跟隨AUV 的基準(zhǔn)參考位置；為tk時(shí)刻協(xié)同定位估計(jì)位置；T為試驗(yàn)數(shù)據(jù)長(zhǎng)度。

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

圖6 跟隨AUV 經(jīng)度估計(jì)位置的對(duì)比 Fig.6 Comparison on estimated values of longitude position of follower AUV

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

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

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

實(shí)驗(yàn)結(jié)果如圖6～9 所示。圖6和圖7分別為不同 定位方法估計(jì)的跟隨AUV 經(jīng)度位置和緯度位置的比較圖，圖8為不同定位方法估計(jì)得到的跟隨AUV 軌跡對(duì)比，圖9為不同定位方法估計(jì)位置與基準(zhǔn)位置的距離誤差。

從圖6～9 可以看出，航位推算位置逐漸偏離真實(shí)軌跡，這是因?yàn)楹轿煌扑阒灰蕾嘍VL 和羅經(jīng)提供的速度和航向信息計(jì)算位置，累積誤差隨時(shí)間而增大。在1150～1350 s 時(shí)間段內(nèi)，由于跟隨AUV 與領(lǐng)航AUV 之間的通信包丟失，EKF 量測(cè)更新過程無法進(jìn)行，濾波估計(jì)的定位誤差迅速增加。但由于ANFIS 具有利用“歷史數(shù)據(jù)”預(yù)測(cè)未來結(jié)果的能力，所以通過收集1～1149 s之間的傳感器及濾波輸出的數(shù)據(jù)對(duì)ANFIS 進(jìn)行訓(xùn)練，即得到跟隨AUV 位置隨時(shí)間、速度及航位推算位置之間的變換規(guī)律，使得在通信包丟失期間ANFIS-EKF 方法可以有效抑制定位誤差的發(fā)散，實(shí)現(xiàn)更準(zhǔn)確的位置預(yù)測(cè)。在1150～1350 s 期間（即通信包丟失的時(shí)間段內(nèi)），不同協(xié)同定位方法的定位性能比較如表2所示。

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

由表2協(xié)同定位性能比較結(jié)果可以看出，多AUV協(xié)同定位系統(tǒng)在跟隨AUV 與領(lǐng)航AUV 之間通信包丟失時(shí)，基于ANFIS-EKF 比僅基于EKF 的協(xié)同定位方法平均定位誤差減少78%，均方根誤差減少77%，最大定位誤差減小62 m，具有更好的協(xié)同定位精度。基于ANFIS-EKF 的協(xié)同定位方法在通信包丟失時(shí)可替代EKF 濾波估計(jì)，更準(zhǔn)確地預(yù)測(cè)跟隨AUV 的位置。

5 結(jié) 論

本文針對(duì)多AUV 協(xié)同系統(tǒng)通信包丟失的情況，結(jié)合傳統(tǒng)的卡爾曼濾波估計(jì)，提出了一種基于ANFIS- EKF 的協(xié)同定位方法，并通過湖試數(shù)據(jù)進(jìn)行了仿真實(shí)驗(yàn)驗(yàn)證。主要貢獻(xiàn)如下：

1）建立了基于地球坐標(biāo)系的多AUV 協(xié)同導(dǎo)航算法的系統(tǒng)定位模型，提出基于ANFIS-EKF 的多AUV協(xié)同定位方法，給出了基于ANFIS-EKF 協(xié)同定位方法的詳細(xì)流程；

2）對(duì)基于EKF 和ANFIS-EKF 的多AUV 協(xié)同定位方法通過湖試數(shù)據(jù)進(jìn)行了仿真實(shí)驗(yàn)對(duì)比，實(shí)驗(yàn)結(jié)果表明基于ANFIS-EKF 的多AUV 協(xié)同定位方法定位精度明顯提高，協(xié)同定位性能更加穩(wěn)定。

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