閆敬 張立 羅小元 濮彬 關(guān)新平

水下信息物理融合系統(tǒng)是集控制、通信和計算于一體的水下智能系統(tǒng),包括探測與采集、傳播與組網(wǎng)、控制與融合等過程.為構(gòu)建水下信息物理融合系統(tǒng),需要在特定水域部署靜態(tài)傳感器和動態(tài)潛器,其中潛器不僅可作為移動錨節(jié)點對傳感器數(shù)據(jù)進行中繼轉(zhuǎn)發(fā),也可根據(jù)任務(wù)需要動態(tài)調(diào)整姿態(tài)以達到網(wǎng)絡(luò)靈活性的提升.為確保潛器上述功能的實現(xiàn),需通過傳感器的協(xié)同來確定潛器的精確位置.

現(xiàn)有定位技術(shù)大致分為距離相關(guān)技術(shù)和距離無關(guān)技術(shù)兩類[1?2].前者定位精度較高、受制因素較少,是本文著力研究的定位技術(shù).目前,一些學者已經(jīng)對距離相關(guān)定位技術(shù)進行了研究,并從不同角度設(shè)計了協(xié)同定位算法.這些算法大多利用到達時間[3]、到達時間差[4]、到達角度[5]和接收信號強度[6]進行距離測量.例如Liu等[7]基于到達時間差設(shè)計了多潛器協(xié)作的定位算法,實現(xiàn)了移動潛器群的精確定位.Zhou等[8]考慮時空相關(guān)性,提出了基于移動預測和到達時間的協(xié)同定位算法.Luo等[9]對傳感器節(jié)點的被動移動進行分析,提出了混合網(wǎng)絡(luò)下的協(xié)同定位算法.賀華成等[10]研究了鉆井平臺在工作海況下的定位精度和功率消耗.文獻[11]考慮區(qū)域無解問題,設(shè)計了兩類傳感器節(jié)點協(xié)作的水下定位算法.文獻[12]考慮傳感器不能直接獲取潛器距離信息約束,設(shè)計了直接與間接測量下的融合定位算法.上述定位算法假設(shè)節(jié)點間的時鐘是同步傳播的.然而,相比于陸地無線電波通信,聲波仍是水下通信最主要的載體[13?14].水聲通信具有典型的弱通信特性,例如北斗與全球定位系統(tǒng)并不能直接應(yīng)用于水下,且受高噪聲以及多徑干擾等不穩(wěn)定因素影響,水下節(jié)點間的時鐘同步難以精確實現(xiàn).上述異步時鐘等弱通信特性使得陸地良好通信條件下的定位算法并不能直接應(yīng)用于水下.此外,潛器的動力學模型相對于陸地機器人更加復雜.

針對異步時鐘下的定位問題,Carroll等[15]提出了基于窮舉法的水下請求式異步定位算法.上述算法雖能消除時鐘偏移的影響,但是在定位過程中需要不斷窮舉,使得算法復雜度較高.此外,Liu等[16]設(shè)計了水下移動環(huán)境下的同步定位算法.Mortazavi等[17?18]對時鐘同步與定位進行聯(lián)合求解,提出了多階段協(xié)同定位算法.同時,文獻[19]采用移動預測的方式,設(shè)計了異步時鐘下的定位算法.根據(jù)對象的不同,文獻[15?19]分別構(gòu)建了定位優(yōu)化問題,為求解上述優(yōu)化問題,需對非線性測量方程進行近似線性化處理.然而,這種近似線性化易引入模型誤差,使其定位精度受限.

本文提出一種基于信息物理融合的水下潛器協(xié)同定位算法.設(shè)計基于傳感器與潛器交互通信的異步定位策略,并分別提出基于擴展卡爾曼濾波(Extended Kalman filter,EKF)與無跡卡爾曼濾波(Unscented Kalman filter,UKF)的協(xié)同定位算法.與文獻[15,19]類似,本文忽略時鐘漂移的影響,僅考慮時鐘偏移對定位的影響.主要創(chuàng)新點如下:1)在異步時鐘下,建立通信時延與位置的關(guān)系,進而定義了潛器協(xié)同定位問題;與文獻[7?11]不同,本文所提的異步定位策略可以有效消除異步時鐘對水下定位的影響.2)提出基于擴展卡爾曼與無跡卡爾曼濾波的協(xié)同定位算法.同時,對上述定位算法的有界性以及克拉美羅下界(Cramr-Rao lower bound,CRLB)進行了分析.與文獻[15]中的窮舉算法相比,本文所提算法復雜度更低,同時與文獻[15?19]相比,基于無跡卡爾曼濾波的協(xié)同定位算法,不需要將非線性測量方程進行近似線性化處理,其定位精度更高.

1 系統(tǒng)架構(gòu)與定位策略概述

1.1 水下信息物理融合系統(tǒng)架構(gòu)

考慮如圖1所示的系統(tǒng)架構(gòu),其各部分功能如下:

圖1 水下信息物理融合系統(tǒng)架構(gòu)Fig.1 Architecture of underwater cyber-physical system

1)浮標.漂浮在水面,可通過北斗導航或全球定位系統(tǒng)獲取自身位置及時鐘同步信息,主要作用是協(xié)助水下傳感器節(jié)點進行自定位.

2)傳感器.懸浮于水下,可通過浮標的協(xié)助獲取自身位置,但受噪聲等因素影響,時鐘并不同步,主要作用是通過信息物理融合獲取水下潛器的位置信息.

3)潛器.可看作是合作目標,可通過向傳感器廣播請求來啟動整個定位過程.假設(shè)水下潛器配備壓力傳感器,以直接獲取自身深度(即z軸)信息.

1.2 定位策略概述

文獻[20]指出,潛器在滾動和俯仰自由度上較穩(wěn)定.為此,將動力學簡化為4自由度方程,其中u,v,w和r分別表示潛器進退、橫移、潛浮、偏航角的速度.潛器動力學模型[14]表示如下:

其中,mu,mv,mw分別表示潛器進退、橫移、潛浮的質(zhì)量;Tr表示偏航的慣性力;Fu,Fv,Fw分別表示進退、橫移、潛浮作用在潛器上的外力;Ir表示潛器偏航的力矩;W表示潛浮的浮力;為相關(guān)系數(shù).

水下潛器的運動軌跡可表示為X1,k=[x(k),y(k),z(k),?(k)],其運動學方程描述如下:

其中,x,y和z分別表示潛器在慣性坐標系北、東和下三個方位的位置;?表示偏航旋轉(zhuǎn)角度;ρ1∈R+為更新步長;慣性坐標系和載體坐標系之間的映射關(guān)系可通過歐拉角變換進行轉(zhuǎn)換.定義Xk=[X1,k,X2,k]T,其中X2,k=[u(k),v(k),w(k),r(k)].進一步,結(jié)合式(1)和式(2),可得潛器的整體模型.

其中,f表示k?1與k時刻狀態(tài)的非線性關(guān)系;為高斯白噪聲,噪聲協(xié)方差為.

在定位過程中,每個傳感器節(jié)點有對應(yīng)的ID號,且彼此知道感知范圍內(nèi)節(jié)點的ID.傳感器節(jié)點可編號為1,2,···,N,且第n個傳感器節(jié)點的位置可表示為(xn,yn,zn),其中潛器的位置可表示為(x,y,z).為此,定位過程(見圖2)可以歸納為如下4步:

圖2 傳感器與潛器的信息協(xié)同過程Fig.2 Cooperation process for the sensors and vehicle

步驟1.在初始時刻ts,s,潛器向傳感器節(jié)點發(fā)送定位請求,其中發(fā)起消息包含傳感器節(jié)點的發(fā)送順序,由n=1,2,···,N表示.隨后,潛器進入監(jiān)聽模式,等待來自傳感器節(jié)點的消息有序傳回.

步驟2.在時刻,傳感器節(jié)點n接收到潛器消息后,節(jié)點n進入監(jiān)聽模式并對來自節(jié)點1,2,···,n?1的所有消息進行解碼.記錄到達時間為,其中m=1,2,···,n?1.當節(jié)點n接收完節(jié)點n?1的消息后,進入發(fā)送模式并在時間tn,n發(fā)出它自己的消息.節(jié)點n發(fā)出的時間戳信息包含,以及tn,n.

步驟3.潛器在時刻接收到來自傳感器節(jié)點n的回復,其中n=1,2,···,N.在接收到傳感器節(jié)點N消息后,潛器完成本次定位交互過程,其中收集到的時間戳可表示為,.

步驟4.基于上述時間戳測量,潛器結(jié)合水下聲速,將時間戳信息轉(zhuǎn)化為距離信息,進而利用協(xié)同定位算法確定水平位置(x,y).最后,將水下水平位置與深度位置進行結(jié)合,確定潛器的位置(x,y,z),整個定位過程結(jié)束.

考慮如下的異步時鐘模型[15]:T=t+a,其中T表示測量時間,t表示真實時間,a為時鐘偏移.

算法目的:構(gòu)建傳播時延與位置之間的交互關(guān)聯(lián);定義潛器協(xié)同定位問題,并根據(jù)潛器整體模型(3),提出基于擴展卡爾曼與無跡卡爾曼濾波的協(xié)同定位算法,以實現(xiàn)潛器(x,y)位置的準確獲取.

2 水下潛器協(xié)同定位

2.1 傳播時間與位置關(guān)系構(gòu)建

為抵消時鐘異步的影響,獲取時間差如下:

其中,n=1,2,···,N,m=1,2,···,n?1. 可看出上述時間戳差值都是在相同的節(jié)點時鐘上做差的,因此上述計算可以消去時鐘偏差的影響.

水下潛器與傳感器節(jié)點n、傳感器節(jié)點n與傳感器節(jié)點m之間的距離可分別表示為

假設(shè)測量值具有相同精度,且方差為δ2mea.為此,結(jié)合式(4)～(6)可獲得相應(yīng)的定位位置與時間測量值的關(guān)系.

其中,γs,n=ls,n/c,γm,n=lm,n/c,c為水聲傳播速度.vs,n和vm,n為測量噪聲.根據(jù)圖2,在定位過程中獲取的對應(yīng)時間戳的傳播次數(shù)及式(4)和式(5)可以推導出vs,n與vm,n的協(xié)方差可以分別為與.

水下潛器異步時鐘定位問題可以表示為

注1.相比于超短基線定位方法,本文的定位方法有如下不同:在超短基線定位中,潛器與傳感器(或浮標)直接交互,沒有考慮傳感器間的信息交互,在上述思想下定位問題可定義為,而本文給出的定位問題也考慮了錨節(jié)點間的協(xié)作,可通過錨節(jié)點的協(xié)作增加定位的可靠性與精度.

2.2 協(xié)同定位算法設(shè)計

2.2.1 基于擴展卡爾曼濾波的協(xié)同定位算法

基于式(4)～(6),定位時間差可轉(zhuǎn)化為如下距離測量:ZZZk={cTs,n,cTm,n|n=1,···,N.m=1,···,n?1}.由于Zk與潛器的位置(x,y,z)T有關(guān),再根據(jù)式(6)～(8)將位置與距離測量構(gòu)建關(guān)聯(lián)如下:

其中,h(Xk)= [2ls,1(k),···,2ls,n(k),l1,2(k)+ls,1(k)?ls,2(k),···,ls,m(k)+lm,n(k)?ls,n(k)]T;假設(shè)vk是方差為Rk的零均值測量噪聲.

考慮到窮舉法在求解水下時鐘異步協(xié)同定位時存在算法復雜度較高的問題,本部分用擴展卡爾曼濾波[21]進行定位迭代,進而估計出優(yōu)化問題的最優(yōu)解,其中k時刻潛器的位置估計可由表示.

步驟1.在k=1時進行初始化,得E[X0],.

步驟2.基于式(3)對潛器位置進行狀態(tài)預測

步驟3.基于式(6)和式(9),可知在k時刻測量方程關(guān)于狀態(tài)位置Xk的雅克比矩陣為.與其類似,狀態(tài)方程f關(guān)于Xk?1的雅克比矩陣可定義為.進而,在k時刻估計協(xié)方差矩陣表示為Pe,k,同時狀態(tài)預測協(xié)方差矩陣Pe,k|k?1更新為

步驟4.估計協(xié)方差更新為

卡爾曼增益Ke,k為

狀態(tài)更新方程為

算法1.擴展卡爾曼協(xié)同定位算法

2.2.2 基于無跡卡爾曼濾波的協(xié)同定位算法

上述算法需求解雅克比矩陣,易引入模型誤差且求解繁瑣.為此,采用無跡卡爾曼濾波[22]進行協(xié)同定位,其中k時刻潛器的位置估計可由表示.

步驟1.在k=1時,進行如下初始化:.

步驟2.基于狀態(tài)估計Xu,k?1和誤差協(xié)方差矩陣Pu,k?1,選擇Sigma點如下:

其中,χj,k?1是Xu,k?1的第j+1 列,j=0,···,2L;;其中L=8;表示矩陣的第列.

步驟3.根據(jù)狀態(tài)方程(3)預測下一時刻狀態(tài)χj,k|k?1=f(χj,k?1),并加權(quán)處理后得

協(xié)方差矩陣更新為

然后由式(9)計算自身位置測量值的無跡變換.

步驟4.測量協(xié)方差矩陣和狀態(tài)值測量交互協(xié)方差矩陣更新為

步驟5.通過以下更新規(guī)則來更新卡爾曼增益、協(xié)方差和估計狀態(tài).

算法2.無跡卡爾曼協(xié)同定位算法

3 性能分析

3.1 算法有界性分析

雖然一般卡爾曼估計均方誤差(Mean squared error,MSE)已被證明有界[23],但在對水下異步時鐘定位進行穩(wěn)定性分析上有所不同.由文獻[24]可知,濾波算法有界性分析需要在線性化后的系統(tǒng)矩陣Fe,k?1和測量矩陣He,k的基礎(chǔ)上進行.基于此,可導出偽系統(tǒng)矩陣Fu,k?1和測量矩陣Hu,k.考慮到擴展卡爾曼濾波與無跡卡爾曼濾波的證明類似,下文以無跡卡爾曼濾波為例證明.Hu,k可以直接計算得到,.進一步,偽系統(tǒng)矩陣Fu,k?1可定義為Fu,k?1=,其中,.

與文獻 [23]類似,引入對角矩陣αk=diag(α1,k,α2,k,···,αn,k) 和βk=diag(β1,k,β2,k,···,βn,k)對狀態(tài)和觀測進行補償,則式(3)和式(9)可更新為

進而,可得出預測誤差協(xié)方差、更新誤差協(xié)方差和無跡卡爾曼濾波增益如下:

為給出估計有界性證明,參考如下引理[25].

引理1.假設(shè)ζk是隨機過程,且存在V(ζk)和正實數(shù)vmin,vmax且0<λ≤1,這樣對于?k,有

若滿足上式則該隨機過程以均方為界表示為

引理2.如果矩陣A>0且C>0,可知A?1>B(BTAB+C)?1BT.

基于上述引理,給出如下有界性定理.

定理1.基于狀態(tài)方程(3)和測量方程(9),并結(jié)合算法2,可得出結(jié)論如下:

1)存在不為零的實數(shù)αmin,βmin,fmin,hmin,αmax,βmax,fmax和hmax,使得對于任何k≥0滿足如下關(guān)系:以及;

證明.由文獻[26]得,預測和估計誤差分別定義為.進而,預選李亞普洛夫函數(shù)如下：

綜合式(27)和式(28)得到

其中

由式(24)知,

結(jié)合式(26),(27),(29),可得

由引理2可知,

從式(31)可知,0<λ<1,根據(jù)定理的兩個條件可得

則式(30)可改寫為

最終可知

由式(33)推導,根據(jù)引理1,可知Xu,k+1|k均方根有界,根據(jù)式(22)可變形為

兩邊同時求期望,可得

3.2 克拉美羅下界(CRLB)

令Mk={X0,X1,···,Xk},Nk={ ZZZ0, ZZ1,···,ZZk}.P(Mk,Nk)對應(yīng)的聯(lián)合概率密度為(Mk,Nk),則克拉美羅估計誤差的形式為

進一步,Jk可定義為

其中,

在高斯白噪聲假設(shè)下,由式(3)和式(9)可知

進而可得

綜上,式(35)可更新為

4 仿真分析

使用4個傳感器對潛器定位.設(shè)定狀態(tài)方差為Q=0.01I,初始協(xié)方差矩陣為P=0.2I,測量方差為R=diag(3,3,3,3,4.5,4.5,4.5,4.5,4.5,4.5).潛器初始位置為(500m,200m,?100m).傳感器位置分別為(600m,100m,?100m),(400m,300m,?110m),(600m,300m,?90m)以及 (550m,150m,?100m).

1)傳感器與潛器信息物理交互過程分析

圖3給出了同步測量(文獻[7?8,11])與異步測量(本文)方式下的距離比較.為更加直觀顯示距離精度,圖4給出了同步與異步測量下的距離誤差.從圖3和圖4可以看出,本文所提的異步定位策略可以有效消除異步時鐘對水下定位的影響.

2)協(xié)同定位算法精度比較

從圖5可以看出,基于無跡卡爾曼濾波的定位算法精度更高.為更清晰地表述上述判斷,圖6給出了上述3種定位算法下的均方誤差(MSE),可以看出擴展卡爾曼與無跡卡爾曼濾波下的定位精度比窮舉法下定位精度高,基于擴展卡爾曼濾波的定位算法精度沒有無跡卡爾曼濾波下定位精度高.圖7給出了兩種濾波下的MSE,可看出無跡卡爾曼濾波更接近CRLB,其定位精度更高.

3)協(xié)同定位算法仿真時間比較

圖8給出了三種方法下的定位過程持續(xù)時間.可以看出,相比于擴展卡爾曼與無跡卡爾曼濾波算法,基于窮舉法的定位算法需要更長的時間達到所需要的定位精度.同時,基于擴展卡爾曼與無跡卡爾曼濾波的定位算法仿真時間相差不大,但是基于無跡卡爾曼濾波的定位算法定位精度更高(見圖6和圖7).

4)誤差統(tǒng)計分析

以無跡卡爾曼濾波定位算法為例,設(shè)定三種干擾場景.場景2下的干擾強度大于場景1下的干擾強度,而場景3下的干擾強度大于場景2下的干擾強度.考慮上述場景,圖9和圖10分別給出了潛器的定位軌跡與定位誤差.可以看出,在上述場景下潛器盡管可以完成定位任務(wù),然而場景1下的定位精度最高,而場景3下的定位精度最差,即干擾強度的增大降低了定位的精度.圖11給出了測量距離誤差的統(tǒng)計特性,可以看出干擾越小距離誤差分布越接近于零.

5 結(jié)論

針對異步時鐘下的潛器定位問題,提出了一種基于信息物理融合的水下潛器協(xié)同定位算法.建立了通信時延與位置的關(guān)系,并定義了潛器協(xié)同定位問題.為求解上述優(yōu)化問題,提出了基于擴展卡爾曼與無跡卡爾曼濾波的協(xié)同定位算法,并對上述定位算法的有界性以及克拉美羅下界進行了分析.最后,通過仿真對比驗證了本文所提方法的有效性.

圖3 同步測量與異步測量距離比較Fig.3 Distance comparison with synchronous and asynchronous measurements

圖4 同步測量與異步測量距離誤差比較Fig.4 Distance error comparison with synchronous and asynchronous measurements

圖5 窮舉法、擴展卡爾曼和無跡卡爾曼定位軌跡Fig.5 Trajectories with exhaustive,EKF-based and UKF-based methods

圖6 窮舉法、擴展卡爾曼與無跡卡爾曼MSE比較Fig.6 Comparison of MSE for exhaustive,EKF and UKF based methods

圖7 擴展卡爾曼與無跡卡爾曼MSE與CRLB比較Fig.7 Comparison of MSE and CRLB for UKF and EKF-based methods

圖8 算法仿真定位用時比較Fig.8 Localized trajectories of underwater vehicle different interference intensities

圖9 不同干擾強度下潛器定位軌跡Fig.9 Positioning trajectories of submersible under different interference intensities

圖10 不同干擾強度下潛器定位誤差Fig.10 Positioning errors of underwater vehicle under different interference intensities

圖11 誤差統(tǒng)計分析Fig.11 Analysis of statistical error