簡杰，朱志宇

江蘇科技大學 電子信息學院，江蘇 鎮(zhèn)江 212100

0 引 言

自主水下航行器（AUV）是一種無纜無人水下機器人，其自主作業(yè)能力強、隱蔽性高，擁有廣泛的運用場景和極高的應(yīng)用價值。隨著水下探索任務(wù)日益復(fù)雜，AUV 的任務(wù)難度相應(yīng)提高，單個AUV 受自身限制無法滿足全部工作需求。近年來，集群化、自主化和結(jié)構(gòu)混合化是AUV 融合先進控制方法下的主要發(fā)展方向。多AUV 之間的交互協(xié)作使其能夠超越單個AUV 的單一功能，同時多智能體結(jié)構(gòu)下的AUV 系統(tǒng)能夠獲得更高的容錯性和魯棒性，提高AUV 的作業(yè)效率和適應(yīng)復(fù)雜任務(wù)的能力，具有更加廣闊的應(yīng)用前景[1]。

主流AUV 集群系統(tǒng)一般采用主從式結(jié)構(gòu)，從AUV 無需與主AUV 裝備同級別的傳感器和處理器元件，因而可大大降低系統(tǒng)成本，減少在整個AUV 系統(tǒng)定位過程中，主AUV 上浮矯正定位衛(wèi)星信號的需求，提高系統(tǒng)隱蔽性[2]。對于采用“n主帶m從”的主從式編隊結(jié)構(gòu)的多AUV 系統(tǒng)[3]，其主AUV 裝備有高級別的傳感器，可自主獲取精度相對較高的自定位信息，通過主從AUV 之間的相對測量，完成對缺失自身定位功能的從AUV 全局定位信息的獲取?；趦?yōu)化理論[4]和圖論[5]的算法對通信質(zhì)量及頂層集中處理能力要求較高，難以匹配基于水聲信號系統(tǒng)的嚴苛條件，相較而言，基于貝葉斯濾波的協(xié)同定位算法則因其能穩(wěn)定、有效地融合被測AUV 的相關(guān)定位信息[6]，而成為主流的方法。本文擬采用的消息擴散形式，只需要鄰居節(jié)點之間進行單跳通信[7-9]，因而在理論上對于依靠水聲通信的AUV 集群系統(tǒng)具有很強的適用性[10-11]。

作為一種重要的貝葉斯濾波器，無跡卡爾曼濾波（UKF）適用于非線性系統(tǒng)，適合處理復(fù)雜非線性運動下AUV 的定位問題[12-17]。本文擬將UKF 作為基礎(chǔ)算法并進行改造，同時將其作為仿真分析的對比方法。相比量測均值和以網(wǎng)絡(luò)權(quán)重為加權(quán)指標的信息融合方法，本文將從評價量測品質(zhì)的角度出發(fā)，充分發(fā)揮較高準確度信號源的價值，在不失去整體可靠性的前提下提高全局定位精確度。通過采用局部濾波算法與融合算法并行運行的結(jié)構(gòu)，依靠及時更新機制來保障系統(tǒng)的實時性。

1 系統(tǒng)建模

1.1 運動模型

AUV 坐標系的建模包括大地坐標系和運動坐標系。單平臺AUV 的姿態(tài)以及AUV 之間的觀測都是建立在以自身為原點的運動坐標系上的，使用運動坐標系便于研究AUV 之間的相對位置等運動信息。在研究AUV 在環(huán)境中的運動狀態(tài)時，使用大地坐標系易于直觀地表示AUV 的位置信息，故其被廣泛應(yīng)用于AUV 集群場景。在對由多臺主AUV 觀測得到的同一從AUV 位置信息進行融合時，需要完成從運動坐標系到大地坐標系的轉(zhuǎn)換，以保證多個主AUV 觀測的位置信息能夠使用統(tǒng)一的坐標尺度。大地坐標系與運動坐標系的關(guān)系如圖1 所示[18]。

圖1 大地坐標系與運動坐標系Fig. 1 Geodetic coordinate system and motion coordinate system

如表1 所示， (x，y，z)為AUV 在運動坐標系下的位移， (u，v，w) 為 AUV 沿X，Y，Z軸方向的速度分量， (φ，θ，ψ)分別為AUV 的橫搖角、縱傾角和艏搖角， (p，q，r) 為 沿X，Y，Z軸方向的角速度分量。

表1 AUV 運動參數(shù)定義Table 1 Definition of AUV motion parameters

AUV 之間通過量測獲取相對位置，進而獲得運動坐標系下的位置坐標。為便于計算以及系統(tǒng)濾波的融合，需要將該位置坐標轉(zhuǎn)換到大地坐標系中。假設(shè)兩坐標系的原點位置重合，經(jīng)過坐標旋轉(zhuǎn)，即可得到坐標系的轉(zhuǎn)換矩陣S。

運動坐標系可由轉(zhuǎn)換矩陣轉(zhuǎn)換為大地坐標系：

AUV 個體運動學方程如下：

式中，vx,vy,vz為個體移動速度在運動坐標下的分量。

1.2 量測模型

兩AUV 每隔時間T進行一次協(xié)同定位，k時刻下，由主AUV 坐標解算出其與從AUV 坐標的相對距離，可以表示為

1.3 狀態(tài)與觀測模型

單AUV 的平臺狀態(tài)由平臺的位置、速度、姿態(tài)等元素組成。綜合考慮平臺的動力學特性，確定輸入?yún)⒆兞?，加入高斯噪聲后，建立運動方程。由全體平臺的狀態(tài)、輸入和噪聲可以得到整個協(xié)同定位系統(tǒng)的狀態(tài)方程。任意從AUV 節(jié)點i的狀態(tài)空間模型如下所示。

狀態(tài)方程：

觀測方程：

式中：Xk為被測AUV 的真實狀態(tài)變量，包含位置、速度、姿態(tài)等信息；為觀測變量，為被測AUV 的真實觀測值，考慮到相對距離量測模型的非線性，狀態(tài)方程中的f(x)和 觀測方程中的h(x)均為非線性函數(shù)；Qk-1為 預(yù)測噪聲；Rk為觀測噪聲。假設(shè)Qk-1和Rk均為高斯白噪聲，滿足正態(tài)分布。

2 算法設(shè)計

2.1 閾值加權(quán)改善粗差

圖2 閾值加權(quán)算法流程圖Fig. 2 Flow chart of threshold weighting algorithm

2.2 局部信息濾波

局部信息濾波選用UKF 為底層方法。UKF是在貝葉斯濾波和卡爾曼濾波（KF）的基礎(chǔ)上，利用無跡變換（UT）對函數(shù)的概率密度做近似處理，求出目標事件的期望和方差，然后將非線性問題轉(zhuǎn)變成卡爾曼濾波問題。由于無跡變換具有二階精度，所以UKF 也具有二階精度，這也符合多AUV 系統(tǒng)的復(fù)雜度。對于上文已經(jīng)建立的系統(tǒng)模型（5），（6）（式（5）、式（6）），已知當前時刻被探測AUV 節(jié)點狀態(tài)Xk-1的預(yù)估期望（對于前一時刻，為后驗估計均值）和協(xié)方差，將協(xié)方差矩陣分解LLT=， 其中要求必須為正定。為了將該AUV 的狀態(tài)傳遞函數(shù)f(Xk-1)近似為正態(tài)分布，進行無跡變換，取Sigma 點，狀態(tài)變化得到被探測節(jié)點狀態(tài)先驗值的近似概率密度，該先驗值滿足正態(tài)分布N(,)，上一時刻的后驗期望為=，則有

式中，R為觀測誤差。

觀測到被測AUV 節(jié)點的觀測值Ym，完成更新步，得到k時刻被測AUV 節(jié)點的狀態(tài)后驗期望和后驗方差為：

至此，k時刻的狀態(tài)后驗估計值和協(xié)方差已獲得，可進入下一個循環(huán)，繼續(xù)遞推。

2.3 信息增益加權(quán)融合算法

在信息論與概率統(tǒng)計中，熵（entropy）是表示隨機變量不確定性的度量[19]，設(shè)X是一個取有限個值的離散隨機變量，其概率分布為：

則隨機變量X的熵定義為

通常，式中的對數(shù)是以2 為底或是以e（自然對數(shù)）為底，這時，熵的單位分別稱作比特或是納特。

顯然，由定義式可知，熵值越大，隨機變量的不確定性就越大。

設(shè)有隨機變量 (X,Y)，其聯(lián)合概率分布為

條件熵H(X|Y)表 示在已知隨機變量Y的條件下隨機變量X的不確定性。隨機變量Y給定條件下的隨機變量X的條件熵H(X|Y)，定義為Y給定條件下X的條件概率分布的熵對Y的數(shù)學期望：

條件熵H(X|Y)表 示在已知隨機變量Y的條件下隨機變量X的不確定性。

在UKF 的應(yīng)用情境下，觀測值作為外部觀測所獲得的特征Y，其條件熵H(X|Y)表示在獲得外部觀測值Y的條件下狀態(tài)值X的不確定性。條件熵H(X|Y) 對 狀態(tài)估計值X的信息增益g(X,Y)，為先驗狀態(tài)值X的經(jīng)驗熵H(X)與 觀測值Y給定條件下X的 條件熵H(X|Y)之差，即

一般地，熵H(X)與 條件熵H(X|Y)之差也稱為互信息。

在UKF 算法中，狀態(tài)信息和觀測信息之間存在因果關(guān)系，由于測量誤差的存在，觀測值不能真實地反映狀態(tài)值，該濾波方法的本質(zhì)是利用觀測信息去估計狀態(tài)的真實值，以減小觀測值的不確定度。信息增益表示已得知特征Y的信息而使得X的信息不確定性減少的程度[20]。

在n主m從 AUV 的 系 統(tǒng) 中，設(shè) 定 共 有i個 主AUV，分別對編號為j的從AUV 進行位置觀測，主AUVi獲得一組被測AUV 的狀態(tài)觀測值和一組優(yōu)化后的局部信息濾波值。

由式（20）可知，編號為i的AUV 執(zhí)行一次完整的局部信息濾波后，觀測值Ym作為外部觀測所獲得的特征，對狀態(tài)先驗估計值的信息增益可以寫作

定理1[6]：主AUV 對被測AUV 的狀態(tài)觀測信息X1,X2, ···,Xn服 從n維 高斯分布N(μ,Σ),其 中 期 望為 μ，協(xié)方差矩陣為 Σ，則被測AUV 的狀態(tài)值X的經(jīng)驗熵可以寫為

證明如下：X1,X2, ···,Xn的分布函數(shù)為

則有

為便于表述，式(24)可寫為式(25)與式(26) 的和，其中第1 項為

第2 項為

由式(24)～式(26)可知，

所以，主AUVi預(yù)估的狀態(tài)先驗值和后驗值的條件熵可以分別寫為：

融合i組AUV 測量結(jié)果所得被測從AUV 的位置狀態(tài)信息為

權(quán)值wi由下式給出：

3 并行結(jié)構(gòu)的分布式融合算法

主AUV 平臺可以通過UKF 提高定位精度，但各主AUV 與被測從AUV 的相對位置的狀態(tài)不一致，從而導(dǎo)致一次完整濾波迭代周期不一致，同時，主、從AUV 之間完成一次水聲通信的周期也不一致。若將濾波網(wǎng)絡(luò)設(shè)計成級聯(lián)結(jié)構(gòu)，信息增益加權(quán)融合結(jié)果與UKF 的輸出將作為彼此的輸入在整個結(jié)構(gòu)中循環(huán)。然而，受主、從AUV 相對位置的狀態(tài)不同且動態(tài)變化的影響，各主AUV 的濾波周期不一致且不斷變化，信息增益加權(quán)融合周期隨之變化甚至是拉長，在級聯(lián)結(jié)構(gòu)中往復(fù)循環(huán)，整個算法的周期將被無限制拉長，從而逐漸失去定位實時性。

并行結(jié)構(gòu)的分布式濾波融合算法如圖3 所示。各主AUV 濾波擁有相同的可靠性，即各主AUV 擁有同級別的坐標轉(zhuǎn)換誤差和量測誤差，然后以此為前提，在各主AUV 優(yōu)質(zhì)濾波效果的基礎(chǔ)上，以濾波信息增益為權(quán)值對多條濾波結(jié)果進行加權(quán)融合，進一步改善濾波誤差，融合結(jié)果將更加接近被測AUV 狀態(tài)的真實值。

圖3 并行融合分布式濾波算法Fig. 3 Parallel fusion distributed filtering algorithm

同時，采用UKF 的局部信息濾波將由各主AUV 獨立完成，與系統(tǒng)整體的信息增益加權(quán)算法并行運行，以使加權(quán)融合算法并非一定要在所有參與融合的濾波迭代周期內(nèi)完成融合，即加權(quán)融合可以在任意長度的時間間隔內(nèi)完成，從而使算法免遭因多個濾波器完成時間不一致而導(dǎo)致的困擾。

圖4 給出了一種融合信息數(shù)據(jù)包順序到達的情況。加權(quán)濾波融合和局部無跡濾波的融合周期及迭代周期分別為Tw和TL， 其中TL實質(zhì)上就是被測AUV 的觀測周期，在相對位置狀態(tài)沒有較大變化的短時間內(nèi)可以將其看作是固定的，而Tw則由所有參與觀測的主AUV 濾波信號到達從AUV的時間決定。顯然，Tw隨著變化的通信情況呈現(xiàn)略大于最長TL的情形，基于水聲通信的特點，加權(quán)融合也必定滯后于對應(yīng)時刻的局部濾波。由圖可見，3 臺主AUV 的局部信息濾波的迭代周期TL1，TL2和TL3較為接近。3 臺主AUV 均可在下一觀測周期完成前接收從AUV 發(fā)送的加權(quán)融合結(jié)果，信息包順序到達，作為UKF 的狀態(tài)先驗值參與局部濾波運算。

圖4 信息順序到達Fig. 4 Information arrives in order

圖5 給出了一種融合信息數(shù)據(jù)包亂序到達的情況。主AUV3 可以在下一觀測周期完成前接收從AUV 發(fā)送的加權(quán)融合結(jié)果，信息包順序到達，參與局部濾波運算。然而，主AUV1 和主AUV2在獲取更接近于真實值的融合結(jié)果前，已經(jīng)完成了下一觀測周期，融合結(jié)果無法參與該周期的濾波運算。

圖5 信息亂序到達Fig. 5 Information arrives out of order

針對上述信息增益加權(quán)融合結(jié)果亂序到達的問題，建立了并行融合的即時更新機制，以使加權(quán)融合的對象均為最新的局部后驗估計結(jié)果，如圖6 所示。

圖6 及時更新機制Fig. 6 Timely update mechanism

主AUV1 由于其自身的狀態(tài)特點，一次迭代濾波周期較短，被測從AUV 的狀態(tài)信息更新更為頻繁，相較于t2時 刻的濾波結(jié)果，AUV1 在t3時刻的濾波結(jié)果與AUV2 和AUV3 在t2時刻的測量結(jié)果在時間上更為接近，更能真實地反映當前被測從AUV 的狀態(tài)信息。這種及時更新AUV1 濾波結(jié)果的方法不僅可以保障系統(tǒng)整體融合目標的實時性，也充分利用了系統(tǒng)融合結(jié)果參與各主AUV的局部濾波。同樣，Tw由所有參與觀測的主AUV濾波信號到達從AUV 的時間決定，很顯然，及時更新機制使得融合信息為當前各主AUV 所測得的最新狀態(tài)值，從AUV 完成一次完整的信息獲取即可對結(jié)果進行融合，這樣的融合周期會隨著信息跟隨信息獲取時間呈現(xiàn)略微的動態(tài)變化。動態(tài)變化的加權(quán)融合周期在一定程度上對觀測周期不一的主AUV 量測系統(tǒng)起到了動態(tài)調(diào)節(jié)作用。至此，每一次融合都將針對最新狀態(tài)信息進行計算，而融合結(jié)果也可以被局部信息濾波充分利用。

4 仿真分析

為了直觀地驗證和分析本文所提方法，在三維坐標上進行仿真，設(shè)計2 個仿真案例，分別給出多臺分別按照不同航線運動的主AUV 和1 個按照預(yù)設(shè)路線運動的從AUV。

案例1 中，3 臺主AUV 均在同一高度平面內(nèi)以1 kn 的速度勻速航行，從原點開始，方向分別為正北、正東和東北（北偏東45°）。從AUV 在x∈[0 m,100 m]，y∈[0 m,100 m]，z∈[0 m,500 m]范圍內(nèi)，在x-y平面內(nèi)對AUV3 保持跟隨，做下滑與爬升的變加速度運動。將從AUV 的運動軌跡設(shè)定為理論軌跡，為拋物面中的一條斜截線，其運動軌跡方程具體的表達式為

對于在Z軸方向做變加速度運動的從AUV，表示其部分狀態(tài)信息的狀態(tài)方程為

式中，a為加速度。

狀態(tài)方程整體可以簡寫為

觀測方程為

式中：Xk為被測AUV 在三維坐標下的位置狀態(tài)變量；為觀測變量，為被測AUV 的真實觀測值；運動噪聲被假定為零均值高斯白噪聲，協(xié)方差矩陣Q=5·diag([1,1,1])，測量噪聲的協(xié)方差矩陣R=2.5×107·diag([1,1,1])。

4.1 單臺主AUV 對從AUV 的濾波定位

如圖7 所示，假設(shè)從AUV 理想的運動路徑為真實值，用黑色表示；單臺主AUV 對該從AUV的觀測值以藍色的“+”表示，對應(yīng)產(chǎn)生的局部濾波信息以紅色的“〇”表示。同時，將觀測值與濾波結(jié)果反映到二維坐標上，可見觀測值相對真實值有明顯的偏差，甚至有明顯的觀測值漂移，但濾波結(jié)果卻十分接近于真實值，基于UKF 的局部信息的濾波效果明顯。

圖7 局部信息濾波結(jié)果Fig. 7 Filtering results of partial information

4.2 多臺主AUV 對從AUV 的協(xié)同濾波定位以及加權(quán)融合

3 臺主AUV 和被定位的從AUV 都從起始點開始按照預(yù)設(shè)的方向航行，其中AUV1 朝向正東方向保持水平位置航行，同樣，AUV2 在保持同一水平面位置的情況下，向正北方向航行，AUV3 向東北（北偏東45°）方向航行。從AUV 按照式（32）所給路線對AUV3 保持跟隨。

圖8 所示為多AUV 局部濾波結(jié)果，其中紅色“+”代表AUV1 對從AUV 位置的觀測值，紅色“〇”為AUV1 對觀測值的濾波定位結(jié)果；類似地，藍色“+”和藍色“〇”分別代表AUV2 的觀測值和濾波結(jié)果；黃色“+”和黃色“〇”分別代表AUV3 的觀測值和濾波結(jié)果，品紅色“*”表示3 組濾波結(jié)果的加權(quán)融合值。

圖8 多AUV 局部濾波結(jié)果Fig. 8 Local filtering results of multiple AUVs

與單平臺主AUV 的定位相同，3 臺主AUV分別對從AUV 進行觀測，多臺主AUV 觀測均值與加權(quán)信息增益融合算法優(yōu)化值的對比如圖9 所示。圖9 中的3 幅圖分別為融合結(jié)果在(x，y)，(y，z)，(z，x)平面下的描述。

可見，盡管3 臺主AUV 局部濾波結(jié)果的精確度各不相同，但基于加權(quán)信息增益的融合方法輸出的結(jié)果穩(wěn)定、平滑，且對真實值的跟蹤性能有明顯提高，相比簡單的平均值處理（圖9 中綠色星號標注的觀測均值），該融合方法對濾波結(jié)果有明顯的優(yōu)化作用。

圖9 加權(quán)信息增益融合算法效果對比Fig. 9 Comparison of the effect of weighted information gain fusion algorithm

以第70 次迭代時刻下的濾波結(jié)果為例，各主AUV 的濾波數(shù)據(jù)及融合權(quán)值如表2 所示。

表2 系統(tǒng)濾波與融合結(jié)果Table 2 Filtering and fusion results of AUV system

3 臺主AUV 量測的濾波結(jié)果精度不一，對應(yīng)地，獲取的信息增益也不同。采用本文所述融合方法，以完成一次濾波迭代所獲取的信息增益為評價指標，信息增益越大，消除觀測值的不確定性程度越大，在融合加權(quán)中所占的權(quán)重也就越大，這樣可以提高精度濾波數(shù)據(jù)的可靠性，使多組濾波結(jié)果按照可靠性加權(quán)，從而提高數(shù)據(jù)整體的精度。

為展示濾波效果和信息增益加權(quán)融合的效果，本文引用累計平均誤差為狀態(tài)測量指標來作為對定位測量效果的評價。UKF 對先驗值具有依賴性，隨著迭代次數(shù)的增加，濾波結(jié)果會更加接近于真實值。如圖10 所示，AUV1，AUV2 和AUV3的局部濾波結(jié)果相比真實值的累計平均誤差，能如實地反映這一特性。受AUV3 前期誤差較大的影響，融合結(jié)果也具有較高的測量誤差，但仍優(yōu)于AUV3 的濾波結(jié)果。在呈現(xiàn)非線性較強的運動中期時間段，由于底層的UKF 在解決非線性方面能力較強，故3 臺主AUV 的濾波結(jié)果均較穩(wěn)定，各組的分布式濾波結(jié)果穩(wěn)定且可靠性接近，基于信息增益的加權(quán)融合結(jié)果也呈現(xiàn)出低誤差的優(yōu)質(zhì)表現(xiàn)，最終融合結(jié)果的誤差遠小于局部濾波誤差。

圖10 案例1 的累計平均誤差Fig. 10 Cumulative average error in Case 1

如表3 所示，選取5 個迭代時刻下的絕對誤差作為參考，列出了3 臺主AUV 濾波量測所得結(jié)果以及系統(tǒng)濾波加權(quán)融合結(jié)果。為考察3 組主AUV 的濾波數(shù)據(jù)離散情況，模擬了工程實際測量環(huán)境，并將濾波均值假設(shè)為該數(shù)組的真值。

表3 案例1 的系統(tǒng)濾波與融合絕對誤差對比Table 3 Absolute errors comparison of system filtering and fusion results in Case 1

為了更加直觀地反映各組結(jié)果與真實值之間的擬合程度，以表3 中數(shù)據(jù)為基礎(chǔ)，引入均方誤差和擬合標準差作為量化指標，對各組結(jié)果進一步予以了對比，結(jié)果如表4 所示。

表4 案例1 的系統(tǒng)濾波與融合擬合程度對比Table 4 Fitting precision comparison of system filtering and fusion results in Case 1

由表3 和表4 可以看出，在不同時刻下，3 組主AUV 濾波結(jié)果的精度是無規(guī)律變化的，因而無法確定出一組能夠保證全過程、高質(zhì)量的濾波數(shù)據(jù)。在所選取的5 個迭代時刻下，無論是實時標準誤差，還是反映擬合程度的均方誤差和擬合標準差，濾波融合結(jié)果都明顯小于3 組主AUV 的獨立濾波結(jié)果。而以信息增益加權(quán)的融合結(jié)果，則使得濾波結(jié)果得以平穩(wěn)輸出，且相對于各主AUV 的濾波量測結(jié)果而言，融合濾波結(jié)果與真實值的擬合程度得到了極大提高。如圖11 所示，AUV1 對被測AUV 的濾波定位結(jié)果相較于AUV2 和AUV3，擁有更小的誤差，3 臺主AUV 擁有不同品質(zhì)的測量結(jié)果。從系統(tǒng)3 組濾波定位值的角度考慮，相比濾波平均值，融合結(jié)果更加接近于最優(yōu)結(jié)果，并且能保持平穩(wěn)且精確的濾波效果，完整地傳遞系統(tǒng)最優(yōu)性能。從性能上看，加權(quán)融合也是依據(jù)濾波效果來對系統(tǒng)的多組濾波結(jié)果進行優(yōu)化整合，然后突出選擇其中一組精確度更高的濾波結(jié)果。

圖11 不同精確度的濾波融合效果Fig. 11 Filter fusion results with different accuracy

由式（28）～式（32）可知，AUVi的濾波狀態(tài)先驗值與后驗值的條件熵差值越大，一次濾波的信息增益就越大，其對應(yīng)的在系統(tǒng)中的加權(quán)權(quán)重值wi也就越大。濾波信息增益反映了AUVi由于局部信息濾波使得對被測AUV 狀態(tài)信息獲取的不確定性減少的程度，這在一定程度上直接體現(xiàn)了濾波效果的好壞，也即狀態(tài)信息的不確定性降低得越多，濾波效果越好。以信息增益為權(quán)重指標的加權(quán)融合算法本質(zhì)上是提高優(yōu)質(zhì)濾波結(jié)果的可靠性，從而讓優(yōu)質(zhì)的濾波結(jié)果得以突出。圖12所示為觀測誤差矩陣Q=7.5·diag([1,1,1])時系統(tǒng)濾波融合的累計平均誤差。從中可見，3 組主AUV的局部濾波累計平均誤差有顯著增加，但融合結(jié)果的累計誤差仍維持在較低水平，且融合定位結(jié)果輸出平滑、穩(wěn)定。觀測誤差的提高使得3 組局部濾波結(jié)果在初始階段精確度差異較大，消除定位不確定度的程度較大，但同樣也帶來了濾波精確度高越高，信息增益越大的特點。相比觀測誤差較小的系統(tǒng)，在較大的觀測誤差系統(tǒng)中，局部濾波精確度提高的能力會在信息增益這一具體值上得到更明顯的體現(xiàn)，當進入整體融合加權(quán)時，精確度提高較大的那組局部濾波結(jié)果將得到凸顯。

圖12 提高觀測誤差的系統(tǒng)濾波效果Fig. 12 System filtering effect to improve observation error

案例2 中，參與系統(tǒng)測量的主AUV 數(shù)量增加，部分主AUV 做復(fù)雜運動，并且加入了4 臺主AUV 的運動噪聲。如圖13 所示，主AUV 的運動軌跡已標出，從AUV 保持著與AUV1，AUV2 和AUV3 之間的隊形，始終處于三者的中心，5 臺AUV 沿X軸方向維持隊列前進，其中3 臺主AUV 做勻速運動，主AUV1 和從AUV 做變加速運動。

圖13 多AUV 協(xié)同定位結(jié)果Fig. 13 Cooperative localization results of multiple AUVs

前文已針對主AUV 個體擁有不同精度和提高部分AUV 量測誤差的情況進行討論，為了驗證融合方法的適用性，案例2 給出了更高的定位精度，其中運動噪聲（高斯白噪聲）的協(xié)方差矩陣Q=0.5·diag([1,1,1])，測量噪聲的協(xié)方差矩陣R=1.5·107*diag([1,1,1])。4 臺主AUV 的局部濾波結(jié)果和觀測均值與加權(quán)信息增益融合算法的優(yōu)化值的對比如圖14 所示。為了直觀地展示融合結(jié)果數(shù)據(jù)，在X,Y,Z坐標量測值的基礎(chǔ)上分別予以了作圖。

圖14 加權(quán)信息增益融合算法效果對比Fig. 14 Comparison of the effect of weighted information gain fusion algorithm

可見，在系統(tǒng)整體定位精度提高后，局部濾波與融合結(jié)果的精度也會隨之提高，以信息增益為權(quán)值指標的融合算法對局部濾波融合的效果依然良好，對4 組局部濾波結(jié)果也有明顯的優(yōu)化作用。在增加參與融合的局部濾波個體數(shù)量后，融合結(jié)果對抗誤差較大的局部濾波值的能力更加穩(wěn)定，更多局部樣本的加入使得融合結(jié)果更接近于真實值。

表5 所示為在選取的5 個迭代時刻下AUV系統(tǒng)的局部濾波與融合結(jié)果間的實時誤差。

表5 案例2 的系統(tǒng)濾波與融合絕對誤差對比Table 5 Absolute errors comparison of system filtering and fusion results in Case 2

如表5 所示，進行獨立濾波的個體越多，系統(tǒng)獲得的濾波結(jié)果差異性越大，呈現(xiàn)出定位精度不穩(wěn)定的特點，而濾波融合結(jié)果則可以做到精度相對穩(wěn)定的定位輸出，并且整體保持在高于各組濾波結(jié)果精度的水平。

表6 給出了4 組主AUV 局部濾波和融合結(jié)果的擬合標準差。

表6 案例2 的系統(tǒng)濾波與融合擬合程度對比Table 6 Fitting precision comparison of system filtering and fusion results in Case 2

如表6 所示，融合結(jié)果相比4 組獨立的濾波結(jié)果其擬合標準差維持在較低的水平，其與真實值之間擬合的程度更高。盡管在第75 次迭代時刻因受局部濾波效果的影響，系統(tǒng)擬合效果變差，但該融合方法依然可以明顯降低擬合誤差，提高系統(tǒng)定位精度的穩(wěn)定性。

圖15 給出了4 組主AUV 局部濾波和融合結(jié)果的累計平均誤差。由圖可見，區(qū)別于案例1，案例2 因設(shè)定了更高的定位精度，局部濾波結(jié)果的精度隨之提升，在各組濾波結(jié)果都能夠高質(zhì)量且穩(wěn)定輸出的情況下，融合方法對局部濾波的提升依舊明顯。

圖15 案例2 的累計平均誤差Fig. 15 Cumulative average error in Case 2

以上列出的2 個案例從不同的編隊數(shù)量、編隊方式、行駛路徑和定位精度上驗證了以信息增益為加權(quán)指標的融合方法的有效性，該方法在融合多源濾波信息方面效果較好，能夠提高系統(tǒng)定位精度。融合結(jié)果充分利用了對多組濾波信息的優(yōu)劣選擇能力。

5 結(jié) 語

多主多從模式在AUV 集群系統(tǒng)中十分常見。在本文中，主AUV 平臺通過局部信息濾波產(chǎn)生從AUV 的定位信息，而面對多臺主AUV 產(chǎn)生的定位信息，從AUV 僅需一個準確的自身定位信息即可。本文利用信息論中的信息熵概念，依據(jù)局部信息濾波的信息增益，對多臺主AUV濾波信息進行加權(quán)計算，使得被測從AUV 僅依據(jù)局部信息濾波即可得到更接近于真實值的自身定位信息。信息增益反映了局部信息濾波器對觀測數(shù)據(jù)的優(yōu)化情況，其以對觀測信號的優(yōu)化程度為指標，將多個主AUV 濾波信息進行加權(quán)融合計算從而產(chǎn)生唯一的定位信息。局部信息濾波與信息增益加權(quán)融合算法并行運行，通過利用及時更新機制提高濾波結(jié)果與融合結(jié)果的實時性和運算效率，提高了整個算法的實時性。由于局部信息濾波完全可以做到大幅度地準確優(yōu)化觀測值，且濾波結(jié)果本身已經(jīng)平滑可靠，所以此融合方法以信息增益為加權(quán)指標科學、合理，能起到進一步優(yōu)化局部濾波結(jié)果的作用。