UUVs集群協(xié)同定位的分散式增廣信息濾波方法

杜禎強(qiáng)，柴洪洲，向民志，章 繁，黃紫如，朱華巍

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

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

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

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

1 UUVs集群協(xié)同定位數(shù)學(xué)模型

1.1 協(xié)同定位原理

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

圖1 UUVs協(xié)同定位原理Fig.1 UUVs cooperative localization

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

(1)

(2)

(3)

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

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

(4)

(5)

(6)

(7)

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

1.3 協(xié)同定位的分散式增廣信息濾波

(8)

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

(9)

(10)

Yk+=Yk-+Ik

(11)

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

(12)

(13)

(14)

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

(15)

等式兩邊同時求逆，進(jìn)一步化簡整理得

(16)

(17)

(18)

(19)

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

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

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

(20)

(21)

(22)

(23)

2 理論仿真分析

仿真時間設(shè)置為1800 s，觀測間隔為1 s且每次僅能進(jìn)行兩兩UUV之間的觀測，水聲測距標(biāo)準(zhǔn)差為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，其航位推算的誤差要遠(yuǎn)小于UUV B和UUV C。狀態(tài)添加時，同一時刻各UUV按照預(yù)定的編號(A、B、C)進(jìn)行排序，每個時刻兩個UUV之間進(jìn)行觀測。圖4表示前5個時刻UUVs集群的狀態(tài)添加及觀測更新，實線表示UUV按照運動學(xué)方程進(jìn)行狀態(tài)添加，虛線表示兩個UUV之間進(jìn)行觀測。

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

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

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

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

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

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

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

圖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集群的真實軌跡，航位推算和分散式增廣信息濾波的解算結(jié)果。圖中黑線表示UUV的真實軌跡，綠線表示UUV的航位推算結(jié)果，紅線表示本文方法的計算結(jié)果?？梢钥闯觯褂帽疚姆椒軌蝻@著提高UUVs集群的定位精度。為進(jìn)一步說明所提出的UUVs集群協(xié)同定位分散式增廣信息濾波與集中式濾波的精度一致性，圖12表示分別采用集中式擴(kuò)展Kalman濾波和分散式增廣信息濾波進(jìn)行UUVs集群協(xié)同定位后UUV B和UUV C在X、Y方向的精度。

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

圖12(a)中綠線表示航位推算的結(jié)果，紅線表示集中式Kalman濾波的結(jié)果，圖12(b)紅線表示本文方法的結(jié)果，兩種方法的具體計算結(jié)果見表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)計結(jié)果Tab.1 Decentralized new method and centralized Kalman filter statistical results m

3 結(jié) 論

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

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