杜禎強，柴洪洲，向民志，章 繁，黃紫如，朱華巍

1. 自然資源部海洋測繪重點實驗室，山東 青島 266590； 2. 信息工程大學地理空間信息學院，河南 鄭州 450001； 3. 中國電子科技集團公司第二十八研究所，江蘇 南京 210007

隨著水下勘探偵察、海洋工程、水下作業(yè)和水下作戰(zhàn)等任務難度和復雜度的不斷提升，水下無人航行器(unmanned underwater vehicle,UUV)以集群的形式互相協作成為UUV發(fā)展的必然方向[1-4]。自20世紀90年代中期開始，UUV協同定位就引起了許多西方國家的重視，美國、歐盟等開展了“The European GREX Project”[5]、“Autonomous Ocean Sampling Network”[6]等一系列項目。迄今為止，UUVs協同定位技術已經有了長足發(fā)展，并在海洋監(jiān)測、資源探查及海軍防御領域有著廣泛的應用[7-9]。

協同定位與單UUV定位的最大區(qū)別是多個UUV之間可進行協調、合作與信息交互，如何用合理的數據融合算法有效地融合UUV內、外部傳感器信息以及UUV之間的聲學測距信息是實現UUVs集群協同定位的關鍵[10-11]。文獻[12]首次提出了基于“移動長基線”的協同定位方法，并利用有人駕駛的母船通過聲學通信設備實現了UUV的協同定位。文獻[13]利用擴展Kalman濾波方法融合領航艇按時廣播發(fā)送的參考位置坐標、協方差矩陣以及觀測數據實現了UUVs協同定位。文獻[14]和文獻[15]分別提出容積卡爾曼濾波和無跡卡爾曼濾波的UUVs協同定位方法，解決協同定位中的非線性濾波問題。文獻[16]提出了基于學生t-分布(Student 's t-distribution)的穩(wěn)健濾波協同定位算法處理聲速測量誤差。高精度定位需要有符合系統(tǒng)特性的隨機模型，但是由于觀測受環(huán)境、信號等多種因素影響，先驗權隨機模型精度有限[17-18]。為了處理多徑效應引起的量測厚尾噪聲，文獻[19]將最大相關熵應用到協同定位中。文獻[20]在無多普勒測速儀(Doppler velocity log，DVL)或DVL受限情況下建立了協同定位的動態(tài)過程模型，通過海上試驗驗證了所建立的動態(tài)過程模型相比傳統(tǒng)運動學模型在速度測量受限情況下的優(yōu)勢。文獻[21]添加載體運動學約束，提出了適應復雜場景下的抗差漸消Kalman濾波。文獻[22]和文獻[23]分別提出顧及聲線入射角的水下隨機模型、顧及觀測值時空相關性估計當前歷元殘差的方差協方差的隨機模型。文獻[24]采用自適應Huber濾波算法處理非高斯噪聲的異常觀測量。然而，以上研究主要致力于觀測模型的精化和觀測量噪聲的處理，由于集群中各UUV互相觀測后，狀態(tài)參數相關所需要的龐大實時通信在實際中難以實現，均采用近似求解得到不嚴密的結果。

信息濾波作為Kalman濾波的對稱形式，同屬于高斯濾波，近年來在SLAM領域已得到廣泛應用[25]。本文提出一種UUVs集群協同定位的分散式濾波(decentralized extend information filter,DEIF)方法，在顧及算法嚴密性的基礎上有效地降低了通信載荷，實現了UUVs集群的分散式協同定位。

1 UUVs集群協同定位數學模型

1.1 協同定位原理

UUVs集群協同定位中主UUV通過搭載高精度的傳感器實現自身高精度的定位，搭載低精度傳感器的從UUV通過獲取與主UUV之間的觀測信息，實現觀測資源的共享，從而對自身行位推算(dead reckoning，DR)位置進行校正，其原理示意圖如圖1所示。圖1中橢圓表示誤差橢圓，橢圓面積表示位置誤差的不確定度，圓環(huán)的寬窄表示觀測噪聲的大小，從UUV根據主UUV的觀測信息對自身行位推算誤差進行修正，降低自身位置的不確定度。

圖1 UUVs協同定位原理Fig.1 UUVs cooperative localization

假定UUVs集群由N個UUV構成，其運動狀態(tài)的空間模型可表示為

(1)

(2)

(3)

1.2 傳統(tǒng)分散式Kalman濾波

對于UUVs集群，k時刻單平臺的觀測更新為

(4)

(5)

(6)

(7)

按照嚴密的理論要求，首先傳統(tǒng)分散式Kalman濾波需要所有UUVs集群平臺保持時間同步，不斷發(fā)送/接收來自集群中每一個平臺的信息，這導致龐大的實時通信量。其次觀測更新只能依次進行，對于同一個時刻的觀測，UUV集群在完成一個觀測量的更新后，才能啟動另一個觀測量的更新。當UUVs集群規(guī)模增大，同一時刻的觀測量增多，傳統(tǒng)的分散式Kalman濾波在實際中難以實現。因此傳統(tǒng)方法只能忽略平臺之間的相關性，即觀測更新只考慮觀測量涉及的UUV平臺，但這樣的計算結果顯然不是嚴密的。

1.3 協同定位的分散式增廣信息濾波

(8)

采用矩參數的高斯濾波即為Kalman濾波，采用信息參數的高斯濾波即為信息濾波，Kalman濾波與信息濾波僅為形式上的不同，但由于參數形式的不同使得處理不同問題會有不一樣的復雜度。將k時刻單平臺觀測更新的Kalman濾波形式替換為信息濾波形式，即將式(8)代入式(5)

(9)

(10)

Yk+=Yk-+Ik

(11)

同理將k時刻UUVi單平臺觀測更新的Kalman參數替換為信息參數，即將式(8)代入式(4)，即

(12)

(13)

(14)

由式(13)和式(11)可以看出，在進行UUVi的單平臺觀測更新時，僅需要更新UUVi的信息參數。UUV單平臺的觀測更新僅與其自身有關，而不像傳統(tǒng)方法單平臺觀測更新需要更新所有平臺的狀態(tài)參數。同理將k時刻UUVi對UUVj觀測更新的Kalman濾波參數表達替換為信息濾波表達，即將式(8)代入式(7)，即

(15)

等式兩邊同時求逆，進一步化簡整理得

(16)

(17)

(18)

(19)

由式(18)可以看出，進行UUVi與UUVj的平臺間觀測更新時，僅需要對UUVi和UUVj的信息參數更新，而無須像傳統(tǒng)方法對UUVs集群整體進行信息參數更新。對于UUVs集群協同定位，信息濾波的觀測更新僅改變觀測量直接涉及狀態(tài)的信息參數，相比于傳統(tǒng)方法的觀測更新需要改變所有與觀測相關的狀態(tài)參數，信息濾波的觀測更新具有局部性。

考慮到水下環(huán)境的復雜性和水聲通信引起的時間延遲，交換信息濾波觀測更新和時間更新的順序，存儲部分歷史信息，即可得到增廣信息濾波。UUVs集群進行狀態(tài)添加即每個UUV平臺各自進行狀態(tài)添加，不同時刻UUV狀態(tài)參數之間存在協方差，可按協方差公式推導得到。體現在馬爾可夫隨機場上，狀態(tài)添加即為增加一個新的UUV狀態(tài)結點，如圖2所示。將新結點Ak+1連接到一個已有的結點Ak上，并且改變被連結點Ak的數值。

圖2 UUVs狀態(tài)添加Fig.2 State augmentation of UUVs

(20)

(21)

(22)

(23)

2 理論仿真分析

仿真時間設置為1800 s，觀測間隔為1 s且每次僅能進行兩兩UUV之間的觀測，水聲測距標準差為1 m。海域水流速度為2 m/s，UUV航行速度為4節(jié)。3個UUV的軌跡路線如圖3所示，黑線表示UUV的實際運動軌跡，綠線表示UUV航位推算軌跡。

圖3 UUVs的實際軌跡及航位推算軌跡Fig.3 Trajectory and dead reckoning trajectory of UUVs

對于搭載高精度INS和DVL的UUV A，其航位推算的誤差要遠小于UUV B和UUV C。狀態(tài)添加時，同一時刻各UUV按照預定的編號(A、B、C)進行排序，每個時刻兩個UUV之間進行觀測。圖4表示前5個時刻UUVs集群的狀態(tài)添加及觀測更新，實線表示UUV按照運動學方程進行狀態(tài)添加，虛線表示兩個UUV之間進行觀測。

圖4 前5個時刻UUVs的狀態(tài)添加和觀測更新Fig.4 UUVs state augmentation and observation update in the fifth epochs

對UUVs集群進行狀態(tài)添加時，對應的信息矩陣Y的變化體現在兩方面，一是信息矩陣維數的增大，且新增元素位于原矩陣的右側和下方；二是原信息矩陣內的部分區(qū)域會發(fā)生改變。圖5表示t=5時UUVs集群狀態(tài)添加引起的信息矩陣變化。狀態(tài)A5、B5和C5添加到已有的聯合狀態(tài)中，其中A5連接到A4,B5連接到B4,C5連接到C4。信息矩陣維數增加的部分來自新狀態(tài)的添加，原有信息矩陣的變化位置為狀態(tài)添加連接到節(jié)點的相應位置，紅色區(qū)域和藍色區(qū)域重疊的部分為信息矩陣需要修正的序列。

圖5 t=5時狀態(tài)添加引起信息矩陣Y的變化Fig.5 Change of information matrix Y caused by state augmentation in t=5

為進一步顯示信息矩陣的變化，圖6給出了對應上述狀態(tài)添加的信息矩陣(a)及其Cholesky分解矩陣(b)。灰色表示該處矩陣值不為零，白色表示該處矩陣值為零。信息矩陣中5個紅色方框分別對應5個時刻，方框內表示該時刻UUV所對應狀態(tài)的信息矩陣，紅方框外表示不同時刻間UUV所對應狀態(tài)的信息矩陣。對于圖6(a)第一個紅色方框，UUV A和UUV B進行了相互觀測，其信息矩陣相對應位置的元素不為零；UUV C狀態(tài)與UUV A、B都不相關，其信息矩陣相應位置元素為零。

由Cholesky分解的性質，帶狀稀疏矩陣的Cholesky因子L保持帶狀性，僅原來帶狀區(qū)域內的部分零元素會變成非零元素。如圖6所示，信息矩陣分解后的Cholesky因子矩陣依舊滿足帶狀性。帶狀矩陣Cholesky分解具有局部性，使得UUVs集群的信息矩陣可以實現遞增分解，并且每一步的計算量不隨矩陣規(guī)模的增大而增大。圖7表示UUVs集群信息矩陣的狀態(tài)添加過程，圖7(a)、圖7(b)、圖7(c)分別表示第3、4、5個時刻的UUVs的信息矩陣，圖8表示與圖7相對應的Cholesky分解矩陣。

圖6 UUVs集群信息矩陣及其Cholesky因子Fig.6 UUVs information matrix and Cholesky factor

將上述UUVs集群狀態(tài)添加過程展開，圖7(a)中橙色方框為t=3時刻的狀態(tài)添加，圖8(a)中藍色方框表示圖7(a)中信息矩陣相應位置的Cholesky分解。t=4時刻，UUVs狀態(tài)添加如圖7(b)所示，狀態(tài)添加僅增加了橙色方框右側和下側的區(qū)域，其Cholesky分解后的結果如圖8(b)所示，狀態(tài)更新后的Cholesky分解并未改變原有位置的分布。信息矩陣中元素影響Cholesky分解的區(qū)域僅為其右側和下方，即意味著每次狀態(tài)更新僅需改變紅色方框內的Cholesky因子即可，進而實現信息矩陣的遞推分解。

圖7 UUVs集群信息矩陣狀態(tài)添加Fig.7 State augmentation of UUVs information matrix

圖8 UUVs集群信息矩陣Cholesky因子Fig.8 The Choresky factor of the UUVs information matrix

圖9 傳統(tǒng)分散式Kalman濾波的通信策略Fig.9 Communication strategy of traditional decentralized Kalman filter

圖10 UUVs增廣信息濾波的通信策略Fig.10 Communication strategy of UUVs extend information filter

圖11為UUVs集群的真實軌跡，航位推算和分散式增廣信息濾波的解算結果。圖中黑線表示UUV的真實軌跡，綠線表示UUV的航位推算結果，紅線表示本文方法的計算結果?？梢钥闯?，使用本文方法能夠顯著提高UUVs集群的定位精度。為進一步說明所提出的UUVs集群協同定位分散式增廣信息濾波與集中式濾波的精度一致性，圖12表示分別采用集中式擴展Kalman濾波和分散式增廣信息濾波進行UUVs集群協同定位后UUV B和UUV C在X、Y方向的精度。

圖11 UUVs的軌跡、航位推算及本文方法解算結果Fig.11 The trajectory and dead reckoning of UUVs and the results of the proposed method

圖12(a)中綠線表示航位推算的結果，紅線表示集中式Kalman濾波的結果，圖12(b)紅線表示本文方法的結果，兩種方法的具體計算結果見表1。由表1可以看出，UUV2采用航位推算在X、Y方向上的誤差RMS為54.82、35.27 m，采用集中式Kalman在X、Y方向上的誤差RMS為5.67、4.92 m，采用本文方法在X、Y方向上的誤差RMS為5.40、4.74 m。對于UUV3采用航位推算在X、Y方向上的誤差RMS分別為59.07、31.95 m，采用集中式Kalman在X、Y方向上的誤差RMS為6.10、4.87 m，采用本文方法在X、Y方向上的誤差RMS為5.22、4.64 m。傳統(tǒng)集中式Kalman濾波RMS和STD的平均值分別為5.39 m和3.28 m，分散式本文方法的RMS和STD分別為5.00 m和3.07 m?？梢钥闯?，分散式本文方法與集中式Kalman濾波保持精度上的一致性。

圖12 傳統(tǒng)集中式Kalman濾波與本文方法一致性對比Fig.12 Consistency comparison between traditional centralized Kalman filter and the proposed method

表1 分散式方法與集中式的Kalman濾波統(tǒng)計結果Tab.1 Decentralized new method and centralized Kalman filter statistical results m

3 結 論

針對水下復雜環(huán)境及UUV搭載水聲傳感器的特性，從高斯分布的角度構建了一種基于增廣信息濾波的UUVs集群協同定位方法，解決了傳統(tǒng)分散式Kalman濾波由于龐大通信限制無法得到嚴密結果的問題。每個UUV平臺根據本地的傳感器數據建立自己的狀態(tài)鏈，同時廣播自己的測距狀態(tài)信息，各個平臺協同完成修正序列的Cholesky修正?；趪烂艿臄道砝碚撟C明了所提出的分散式濾波與集中式濾波的誤差一致性，并與傳統(tǒng)Kalman濾波進行對比分析。

理論仿真分析表明，分散式方法與集中式Kalman濾波保持精度上的一致性，且本文方法使得觀測更新僅與觀測直接涉及的UUV相關，有效地降低了通信載荷，實現觀測信息的即插即用。