基于徑向基函數(shù)神經(jīng)網(wǎng)絡輔助容積卡爾曼濾波的多自主水下航行器協(xié)同定位方法

徐博，李盛新，金坤明，王連釗

(哈爾濱工程大學 自動化學院，黑龍江 哈爾濱 150001)

0 引言

自主水下航行器(AUV)作為海洋探測的倍增器，具有控制靈活、價格低廉等特點，是近年來海洋工程領域的重點研究內(nèi)容之一。多AUV組成的協(xié)作系統(tǒng)不僅可以勝任單AUV能夠承擔的任務，例如海底環(huán)境勘探、收集水下情報等，而且效率高、可靠性好，具有重要的應用前景[1]。在水下環(huán)境中，受海況變化、海底環(huán)境信息缺乏等條件制約，一些環(huán)境探測及監(jiān)測任務執(zhí)行過程中，需要多AUV協(xié)作系統(tǒng)在部分工作區(qū)間沒有或基本沒有外界輔助位置信息的情況下完成，導航問題成為發(fā)展AUV所面臨的重要挑戰(zhàn)。多AUV協(xié)同定位是隨著多AUV協(xié)作系統(tǒng)的應用發(fā)展而開發(fā)的一種新型水下定位技術。它不僅可以實現(xiàn)高精度水下定位，而且成本低、實現(xiàn)簡單、可靠性好、無區(qū)域限制，受到世界各國的廣泛重視[2-3]。

準確的位置信息對AUV實現(xiàn)自主工作，尤其對多AUV組成的協(xié)作系統(tǒng)是至關重要的。人們提出很多基于狀態(tài)空間模型的濾波方法[4-5]，但在實際的協(xié)同導航中，由于水下環(huán)境的隨機性和復雜性，多AUV協(xié)同定位系統(tǒng)的定位性能受到系統(tǒng)內(nèi)部和外部等多種因素的制約[6]，例如洋流漂移、傳感器誤差及水下測距精度等引起的系統(tǒng)傳感器輸入誤差和量測誤差。針對這種情況，一般可以通過采用更高精度的傳感器應用到多AUV協(xié)同定位系統(tǒng)中，或者建立更符合系統(tǒng)實際運動規(guī)律的狀態(tài)空間模型，但這會增加協(xié)同定位系統(tǒng)的成本，增大算法復雜度。故需要確定一種有效的方法，在不增加成本和算法復雜性的前提下保證協(xié)同定位的精度。

神經(jīng)網(wǎng)絡可以實現(xiàn)輸入輸出的任意非線性映射，并且對非線性濾波具有強映射和魯棒性，目前已經(jīng)應用于很多領域。文獻[7]利用反向傳播(BP)神經(jīng)網(wǎng)絡建立了液壓振動臺正弦加速度響應的諧波辨識方法,并在液壓振動臺上進行了實驗，驗證了所提出的方案能有效地識別各次諧波的相位以及幅值，具有較高的識別精度。文獻[8]基于BP神經(jīng)網(wǎng)絡輔助擴展卡爾曼濾波(EKF)和無跡卡爾曼濾波(UKF)，并對兩個運動目標進行了仿真實驗，結果表明使用BP神經(jīng)網(wǎng)絡優(yōu)化非線性濾波可以提高濾波估計性能，但應用BP神經(jīng)網(wǎng)絡進行學習訓練時，是采用負梯度下降法進行的權值調(diào)節(jié)，這是一種收斂速度較慢的權值調(diào)節(jié)方法。文獻[9]將粒子濾波與多層感知器神經(jīng)網(wǎng)絡相結合，實現(xiàn)了很好的訓練效果，但訓練效果理想的前提是需要巨大的粒子數(shù)目，這將導致系統(tǒng)計算復雜度增大，在實際應用中難以實現(xiàn)。文獻[10]將徑向基函數(shù)(RBF)神經(jīng)網(wǎng)絡應用于洪水預報，分別建立4種RBF神經(jīng)網(wǎng)絡洪水預報模型，試驗結果驗證了RBF神經(jīng)網(wǎng)絡運算速度快、預報準確度高。RBF神經(jīng)網(wǎng)絡可以將輸入信息映射得到輸出信息，近似于人腦的功能和結構，其逼近函數(shù)能力、分類識別能力和學習速度等方面都好于BP神經(jīng)網(wǎng)絡，適用于解決水下環(huán)境復雜情況下的多AUV協(xié)同定位問題。

另外，由于容積卡爾曼濾波(CKF)通過點估計的方法，不需要通過線性處理方法將非線性系統(tǒng)線性化，從而提高了非線性函數(shù)的擬合精度，同時也避免了在高維系統(tǒng)中UKF容易出現(xiàn)數(shù)值不穩(wěn)定現(xiàn)象，理論推導更加嚴謹。文獻[11]對比了UKF和CKF方法的精度和適用范圍，仿真結果表明CKF方法性能更好，尤其對于高維非線性系統(tǒng)，CKF的適用性和穩(wěn)定性更強。

基于以上分析，考慮到系統(tǒng)狀態(tài)方程和量測方程的非線性及濾波運算復雜度，本文提出基于RBF神經(jīng)網(wǎng)絡輔助CKF的多AUV協(xié)同定位方法。利用RBF神經(jīng)網(wǎng)絡的輸出補償CKF估計，進而提高存在系統(tǒng)誤差情況下，多AUV協(xié)同定位系統(tǒng)的魯棒性和準確性，并通過湖試數(shù)據(jù)進行了仿真驗證。結果表明，本文所提的定位方法在多AUV協(xié)同定位中具有更好的定位精度和穩(wěn)定性。

1 多AUV協(xié)同定位狀態(tài)空間模型

假設多AUV群體中有N個領航AUV，M個跟隨AUV，為了研究方便先考慮2個領航AUV對1個跟隨AUV進行交替領航協(xié)同定位的情況?；谙鄬ξ恢昧繙y的領航- 跟隨式多協(xié)同定位示意圖如圖1所示，圖1中橢圓面積表示定位誤差協(xié)方差。領航AUV裝備全球定位系統(tǒng)(GPS)或基于慣性導航設備、多普勒速度計程儀(DVL)和壓力傳感器的高精度組合導航系統(tǒng)，跟隨AUV裝備由羅經(jīng)和DVL組成的低精度航位推算系統(tǒng)，以及壓力傳感器。此外領航AUV和跟隨AUV都配備了水聲調(diào)制解調(diào)器，在協(xié)同定位過程中，跟隨AUV通過水聲設備每隔一段時間間隔與領航AUV進行相對距離量測，利用領航AUV的導航位置信息和相對距離量測進行信息融合(協(xié)方差橢圓如圖1中實線橢圓面積明顯減小)，而在非協(xié)同定位時刻，跟隨AUV仍采用航位推算的方法。這種周期性協(xié)同定位有效降低了跟隨AUV的定位誤差，滿足長航時、高精度要求。

圖1 相對位置量測的協(xié)同定位示意圖Fig.1 Cooperative localization in relative position measurement

(1)

式中：xk和yk分別為跟隨AUV在tk時刻的東向和北向位置；δt為航位推算周期；vk為跟隨AUV在tk時刻的前向速度；θk為跟隨AUV在tk時刻的航向角；wx,k和wy,k為零均值且相互獨立的Gauss白噪聲。

(2)

定義Xk=[xk,yk]T為跟隨AUV在tk時刻的狀態(tài)量，wk=[wx,k,wy,k]T為過程噪聲向量，εk=[εk]T為量測噪聲向量，且有

(3)

(4)

Qk和Rk分別為假設已知的系統(tǒng)噪聲協(xié)方差陣和量測噪聲協(xié)方差陣。

基于(1)式和(2)式，建立多AUV協(xié)同定位系統(tǒng)的離散時間狀態(tài)空間模型為

(5)

2 CKF協(xié)同定位方法

考慮到協(xié)同定位系統(tǒng)狀態(tài)方程和量測方程的非線性以及計算的復雜度，引入CKF. CKF方法采用Spherical-Radial準則得到等權值的容積點，再利用統(tǒng)計數(shù)值積分原理計算非線性變換后的隨機變量均值和協(xié)方差。考慮如下求積問題：

(6)

式中：I(f)是所求積分，被積函數(shù)定義在笛卡爾坐標系中；Rn是n維積分域，n為狀態(tài)的維數(shù)；f(x)為非線性函數(shù)，x為狀態(tài)向量。將(6)式變換為球徑求容積的形式，然后運用球徑求積規(guī)則進行求解。

在球面徑向變換中，把笛卡爾向量x∈Rn轉換成徑向標量r和方向向量y. 假設x=ry，yTy=1，得xTx=r2，r∈[0，∞)，(6)式的積分可在球徑坐標系中表示為

(7)

式中：Un為半徑為1的球表面，即Un={y∈Rn|yTy=1}；σ(·)為球面測度。

進一步化簡，將(6)式表示為

(8)

(9)

對(8)式和(9)式采用3階容積原則，n維標準正態(tài)分布與非線性函數(shù)乘積的積分可近似為

(10)

(11)

在得到容積點后，通過這些點經(jīng)過非線性系統(tǒng)結合濾波中的時間與量測更新對系統(tǒng)狀態(tài)進行估計，能夠較為準確地獲得狀態(tài)更新及狀態(tài)協(xié)方差矩陣更新[12-17]。

由上述過程可以看出，CKF利用2n個等權值的容積點，計算復雜積分和所需的統(tǒng)計量，完成濾波周期內(nèi)的時間更新和量測更新。

1)時間更新。假設狀態(tài)向量Xk=[xk,yk]T在tk時刻系統(tǒng)后驗概率狀態(tài)k|k以及概率密度p(Xk|Zk)=N(k|k,Pk|k)是已知的，通過Cholesky將狀態(tài)誤差協(xié)方差Pk|k分解為

(12)

計算Cubature點

Xi,k|k=Sk|kξi+k|k，

(13)

定義以下變量

(14)

通過系統(tǒng)狀態(tài)的轉移矩陣函數(shù)傳遞Cubature點

(15)

tk+1時刻的狀態(tài)預測值通過加權求和得到

(16)

計算tk+1時刻的狀態(tài)誤差協(xié)方差預測值

(17)

2)量測更新。通過Cholesky將狀態(tài)誤差協(xié)方差預測值Pk+1|k分解為

(18)

計算Cubature點

Xi,k+1|k=Sk+1|kξi+k+1|k，

(19)

通過系統(tǒng)的量測函數(shù)傳遞Cubature點

(20)

tk+1時刻的觀測預測值通過加權求和得到

(21)

計算tk+1時刻的量測誤差協(xié)方差預測值為

(22)

互協(xié)方差矩陣為

(23)

估計卡爾曼濾波增益為

(24)

tk+1時刻的狀態(tài)估計k+1|k+1及其誤差協(xié)方差矩陣Pk+1|k+1分別為

k+1|k+1=k+1|k+Kk+1(Zk+1-k+1|k)，

(25)

(26)

3 RBF神經(jīng)網(wǎng)絡輔助CKF

3.1 徑向基神經(jīng)網(wǎng)絡原理

RBF神經(jīng)網(wǎng)絡和多層前向網(wǎng)絡的結構相類似，是3層前向網(wǎng)絡，分別為輸入層、隱含層、輸出層。輸入層到隱含層的變換是非線性的，隱含層到輸出層的變換是線性的[18-20]。其基本思想為：

1)將RBF作為隱單元的“基”，從而構成隱含層空間，并將輸入向量映射到隱含層空間；

2)當確定RBF的中心點后，即可得到映射關系；

3)隱含層空間到輸出層空間線性映射。

RBF神經(jīng)網(wǎng)絡具有緊湊的拓撲結構，單層的隱含層使其自身結構更加簡單且更容易實現(xiàn)，這些優(yōu)點可以讓RBF神經(jīng)網(wǎng)絡更好地逼近非線性函數(shù)。RBF神經(jīng)網(wǎng)絡的結構如圖2所示。

圖2 RBF神經(jīng)網(wǎng)絡結構Fig.2 RBF neural network structure

圖2中：X=(x1x2…xp)T為網(wǎng)絡輸入層的輸入向量；(φ1φ2…φb)為神經(jīng)網(wǎng)絡隱含層的b個任一節(jié)點的激活函數(shù)，即RBF；Y=(y1y2…ys…yd)T為網(wǎng)絡的輸出向量；w為輸出權矩陣，其中wjs為隱含層第j個節(jié)點與輸出層第s個節(jié)點間的連接權值，j=1,2,…,b，s=1,2,…,d.

RBF網(wǎng)絡要訓練的參數(shù)有3個，即基函數(shù)的中心和方差以及隱含層與輸出層之間的連接權值。RBF神經(jīng)網(wǎng)絡中常用的RBF是高斯函數(shù)，因此激活函數(shù)可表示為

(27)

式中：‖·‖ 為歐式范數(shù)；cj為高斯函數(shù)中心；σj為高斯函數(shù)的方差，即神經(jīng)元的寬度。

徑向基神經(jīng)網(wǎng)絡的輸出為

(28)

式中：cj為網(wǎng)絡隱含層節(jié)點的中心；wjs為隱含層到輸出層的連接權值；ys為與輸出樣本對應網(wǎng)絡的第s個輸出節(jié)點的實際輸出。

方差σj可由(28)式求解

(29)

式中：cmax為所選取中心的最大距離。

連接權值通過最小二乘法計算得到，即

(30)

3.2 RBF神經(jīng)網(wǎng)絡輔助CKF的多AUV協(xié)同定位

1)量測值Zk和預測量測值k+1|k之差(Zk-k+1|k)，也稱為新息；

3) CKF增益Kk+1.

CKF狀態(tài)更新估計值與準確狀態(tài)之間的誤差值反映了濾波方法性能的好壞，若能估計出濾波誤差值補償狀態(tài)更新的濾波估計值，便能直接提高CKF方法精度。

RBF神經(jīng)網(wǎng)絡輔助CKF方法的多AUV協(xié)同定位方法的基本思路分為兩個部分：

第2部分為通過神經(jīng)網(wǎng)絡估計濾波誤差補償濾波狀態(tài)更新估計值。利用CKF方法對tk～te時段數(shù)據(jù)進行濾波估計，可得到狀態(tài)更新中的新息(Zk-k+1|k)、預測誤差(k+1|k-k+1|k+1)及濾波增益Kk+1，并將其當做訓練完成后的RBF神經(jīng)網(wǎng)絡輸入值代入輸入層，得到濾波誤差將神經(jīng)網(wǎng)絡輸出的估計濾波誤差與濾波估計值k+1|k+1相加，則可以得到新的更為準確的狀態(tài)估計值。

RBF神經(jīng)網(wǎng)絡輔助CKF方法的原理圖如圖3所示。

圖3 RBF神經(jīng)網(wǎng)絡輔助CKF方法的原理圖Fig.3 Principle diagram of CKF algorithm aided by RBF neural network

這種基于RBF神經(jīng)網(wǎng)絡輔助CKF的方法，利用CKF估計的準確性和穩(wěn)定性以及RBF神經(jīng)網(wǎng)絡的學習能力和速度，可實現(xiàn)更為準確的修正濾波估計值。

4 基于RBF神經(jīng)網(wǎng)絡輔助CKF的多AUV協(xié)同定位實驗

4.1 實驗情況概述

實驗利用湖試采集的后處理數(shù)據(jù)，試驗使用3艘勘探船模擬AUV航行，其中兩艘充當領航AUV，另一艘充當跟隨AUV. 領航AUV和跟隨AUV都配備了美國Teledyne Benthos公司產(chǎn)ATM-885聲學調(diào)制解調(diào)器，進行水下單程測距(OWTT)和廣播信息，獲取速度信息的DVL和提供航向的羅經(jīng)也安裝在AUV上，此外，跟隨AUV還配備了GPS，以便提供參考位置。湖試設備現(xiàn)場如圖4所示，所用傳感器的性能參數(shù)如表1所示。

圖4 湖試設備現(xiàn)場Fig.4 Experimental equipment used in the the lake test

傳感器型號性能參數(shù)指標聲學調(diào)制解調(diào)器ATM-885通信范圍/m8000GPSOEMV-2RT-2單點定位精度/m1.8(均方根值)羅經(jīng)H/H HZ001航向精度/(°)0.3DVLDS-99速度精度/kn0.2

湖試數(shù)據(jù)長度為1 600 s，采樣周期δt=1 s，取0～1 200 s數(shù)據(jù)作為RBF神經(jīng)網(wǎng)絡0～tk時段的訓練數(shù)據(jù)，取1 200～1 600 s數(shù)據(jù)作為tk～te時段的實驗數(shù)據(jù)，系統(tǒng)的狀態(tài)方程初始狀態(tài)0由GPS提供。在每一時刻，跟隨AUV只與兩艘領航AUV中的一艘進行通信。圖5顯示了由GPS提供的兩艘領航AUV和一艘跟隨AUV的真實航行軌跡，其中細線表示AUV前1 200 s的真實航行軌跡，粗線表示1 200 s以后AUV的真實航行軌跡。從圖5中可以看出，跟隨AUV始終位于兩艘領航AUV之間，這樣能夠提高系統(tǒng)的可觀測性。

圖5 領航AUV和跟隨AUV的真實軌跡Fig.5 True trajectories of Leader AUV and Follower AUV

4.2 狀態(tài)空間模型輸入誤差

在實際的多AUV協(xié)同定位中，由于建立的狀態(tài)空間模型不夠準確，系統(tǒng)的定位性能會受到各種不定性因素影響，這些影響因素能夠?qū)е聻V波方法精度下降甚至快速發(fā)散。本文在非線性濾波CKF的基礎上引入RBF神經(jīng)網(wǎng)絡對CKF估計值進一步修正，可有效提高CKF的估計精度及穩(wěn)定性。基于RBF神經(jīng)網(wǎng)絡輔助CKF的多AUV協(xié)同定位方法在下文中簡稱為RBFCKF方法。

基于以上描述，利用湖試數(shù)據(jù)，在建立的狀態(tài)空間模型上分別添加以下3種不同輸入誤差：

圖6 跟隨AUV航向Fig.6 Heading of Follower AUV

圖7 跟隨AUV東向、北向速度Fig.7 The eastward and northerly velocities of Follower AUV

3)領航AUV和跟隨AUV間的量測距離存在均值不為0的高斯噪聲。領航AUV與跟隨AUV之間的量測距離存在誤差時，會嚴重影響CKF估計精度，本實驗在得到的領航AUV與跟隨AUV間距離量測數(shù)據(jù)的基礎上，設置均值為15 m、方差為(10 m)2距離量測誤差，以驗證在跟隨AUV量測距離存在未知誤差時本文所提方法的有效性。跟隨AUV與領航AUV1、領航AUV2之間的量測距離和設置誤差的量測距離如圖8、圖9所示。

圖8 領航AUV1與跟隨AUV之間的量測距離Fig.8 Distance between Leader AUV 1 and Follower AUV

圖9 領航AUV2與跟隨AUV之間的量測距離Fig.9 Distance between Leader AUV 2 and Follower AUV

4.3 協(xié)同定位方法驗證

數(shù)學仿真軟件MATLAB的神經(jīng)網(wǎng)絡工具箱為RBF網(wǎng)絡提供了很多工具箱函數(shù)，本文通過newrbe函數(shù)創(chuàng)建一個準確的RBF網(wǎng)絡。RBF網(wǎng)絡的擴展速度函數(shù)spread越大，擬合出來的函數(shù)會更平滑，但過大的擴展速度會引起數(shù)值問題，本實驗所取徑向基擴展速度的值設為3；RBF隱含層神經(jīng)元數(shù)與輸入矢量數(shù)相等；隱含層神經(jīng)元閾值取0.863 2.

在第1 200 s時利用GPS進行跟隨AUV位置更新，作為多AUV協(xié)同定位實驗濾波的初始位置。初始時刻的狀態(tài)估計誤差協(xié)方差矩陣為P0=diag[1 m,1 m]2；過程噪聲和量測噪聲協(xié)方差矩陣分別為Qk=diag[1 m，1 m]2和Rk=[10 m]2。基于4.2節(jié)中協(xié)同定位系統(tǒng)輸入誤差的設置，模擬多AUV協(xié)同定位系統(tǒng)狀態(tài)方程和量測方程存在誤差情況下的協(xié)同定位實驗，驗證RBFCKF方法的有效性和優(yōu)越性，實驗結果如圖10～圖12所示，其中參考CKF協(xié)同定位誤差為沒有添加航向漂移誤差、洋流干擾、量測誤差情況下對實驗數(shù)據(jù)進行CKF估計的定位誤差。實驗過程中添加了跟隨AUV速度傳感器測量噪聲(0.3 m/s)2的零均值高斯白噪聲，航向角的測量噪聲取為(1°)2的零均值高斯白噪聲，故3組實驗中參考CKF協(xié)同定位誤差有所變化。

圖10 系統(tǒng)輸入航向存在漂移誤差的實驗結果Fig.10 Experimental results of drift error in the input heading

圖11 系統(tǒng)輸入速度存在洋流干擾的實驗結果Fig.11 Experimental results of system input velocity with ocean currents

由圖10～圖12可以看出，多AUV協(xié)同定位系統(tǒng)存在輸入誤差或外部量測誤差時，僅基于CKF協(xié)同定位方法的定位誤差明顯增大，RBFCKF協(xié)同定位方法精度接近無誤差影響時CKF方法估計的定位精度，由RBFCKF方法估計的跟隨AUV定位軌跡更接近跟隨AUV的真實軌跡。

圖12 量測信息存在噪聲干擾的實驗結果Fig.12 Experimental results of noise interference measurement information

為進一步驗證RBFCKF協(xié)同定位方法的定位性能，選擇定位誤差(LE)、平均定位誤差(ALE)、均方根誤差(RMSE)等作為多AUV協(xié)同定位性能指標[21]，定義分別為

多AUV協(xié)同定位系統(tǒng)存在輸入誤差時，對比CKF和RBFCKF方法的協(xié)同定位性能指標如表2所示。

表2 存在誤差時多AUV協(xié)同定位性能比較

由表2協(xié)同定位性能比較結果可以看出，多AUV協(xié)同定位系統(tǒng)狀態(tài)空間模型存在以上3種輸入誤差時，基于RBFCKF方法的ALE和RMSE僅為CKF方法的30%，具有更好的協(xié)同定位精度；協(xié)同定位誤差的方差和標準差相比基于CKF的協(xié)同定位方法均提高一倍左右，協(xié)同定位性能更加穩(wěn)定；多AUV協(xié)同定位系統(tǒng)的最大定位誤差也明顯降低。故當多AUV協(xié)同定位系統(tǒng)狀態(tài)空間模型系統(tǒng)輸入不準確時，基于RBFCKF的多AUV協(xié)同定位方法的各定位性能，均優(yōu)于無RBF神經(jīng)網(wǎng)絡輔助的CKF方法方式。

5 結論

本文針對多AUV系統(tǒng)協(xié)同定位性能受傳感器輸入誤差和外部量測誤差的影響，提出了一種基于RBF神經(jīng)網(wǎng)絡輔助CKF的多AUV協(xié)同定位方法，并通過湖試數(shù)據(jù)進行了仿真實驗驗證。得出主要結論如下：

1)介紹了多AUV協(xié)同導航的背景與研究意義，建立了多AUV協(xié)同導航方法的系統(tǒng)定位模型，給出了基于CKF的多AUV協(xié)同導航方法的基本方程。

2)提出基于RBFCKF的多AUV協(xié)同定位方法，給出了RBF神經(jīng)網(wǎng)絡的基本概念及其輔助CKF的具體流程。

3)對基于CKF和RBFCKF的多AUV協(xié)同定位方法通過湖試數(shù)據(jù)進行了仿真對比。實驗結果表明基于RBFCKF的多AUV協(xié)同定位方法定位精度明顯提高，協(xié)同定位性能更加穩(wěn)定。

本文雖然通過湖試數(shù)據(jù)進行了實驗驗證，可以有效提高多AUV協(xié)同定位系統(tǒng)的協(xié)同定位性能，為進一步研究多AUV協(xié)同定位系統(tǒng)、提高系統(tǒng)協(xié)同定位性能提供了可行性思路和方法。但由于水下環(huán)境的不確定性和水聲通信的復雜性，導致系統(tǒng)噪聲模型時變以及AUV在水下的欠驅(qū)動特性等問題，仍需要深入研究與分析，并在實際操作過程中去探索。