基于因子圖的USV位置估計算法

孫 鑫，姜 靜

(沈陽理工大學(xué) 自動化與電氣工程學(xué)院，沈陽 110159)

隨著各國對海洋的探索，無人水面航行器(unmanned surface vehicle,USV)是近些年伴隨著軍事需求而新興的產(chǎn)物。軍事方面，出于維護安全，在執(zhí)行掃雷、偵查、監(jiān)測等任務(wù)時，USV具有很大優(yōu)勢；民用方面,USV能減少人力資源，提高船舶航行安全[1]，并可以作為連接無人機與水下航行器的移動水面通信節(jié)點，在未來開啟海陸空全方面協(xié)同作戰(zhàn)中起著不可或缺的重要作用。隨著USV在各方面廣泛的應(yīng)用，人們對其性能要求越來越高，如高機動下的自主避障，復(fù)雜海況下的自主航行等；USV自身準(zhǔn)確的位置信息是完成以上各種功能的基礎(chǔ)條件。隨著導(dǎo)航技術(shù)的不斷發(fā)展，多傳感器組合導(dǎo)航已經(jīng)逐漸成為各種復(fù)雜環(huán)境下長時間高精度導(dǎo)航的主要方式[2-3]。文獻[4-5]中以GPS和羅經(jīng)為導(dǎo)航傳感器，基于USV的二維平面運動學(xué)模型,利用擴展卡爾曼濾波(extended Kalman filter,EKF)算法對USV的速度和位置估計。但EKF算法只能對當(dāng)前時刻的狀態(tài)進行估計，當(dāng)系統(tǒng)存在時間延遲，傳感器采樣周期不一致等問題時，需要增加計算量，降低定位精度，且在弱噪聲情況時，濾波效果不佳[6]，所以本文提出一種新的USV定位方法，即基于因子圖的USV位置估算算法，仿真結(jié)果表明本文所提算法可以準(zhǔn)確估計USV的位置信息，而且在弱噪聲環(huán)境下效果更佳。

1 USV二維平面運動模型及其線性化

(1)

式中wk和Vk分別為USV的偏航角速度和行駛速度。

(2)

(3)

圖1 USV二維空間運動模型

所以USV的二維運動學(xué)方程可簡寫為

(4)

式中

(5)

(6)

把式(4)展開可得線性化后的USV運動學(xué)方程為

(7)

由此可得狀態(tài)轉(zhuǎn)移函數(shù)為

(8)

在k+1時刻，GPS觀測的USV位置信息可用觀測方程表示為

(9)

協(xié)方差矩陣為

由此可得觀測過程更新函數(shù)為

p(Zk+1|Xk+1)=N(Zk+1;Hk+1Xk+1,Rk+1)

(10)

2 因子圖與和積算法

20世紀(jì)90年代,因子圖由F R Kschischang等從Wiberg圖、Tanner圖等模型的基礎(chǔ)上推廣而來，其后被用做研究信道估計、解碼及迭代接收機技術(shù)的通用工具[8]。因子圖模型具有很強的靈活性，能夠?qū)崿F(xiàn)傳感器的即插即用，已經(jīng)在導(dǎo)航領(lǐng)域受到廣泛關(guān)注[9]。因子圖是一種二向圖，可將一個復(fù)雜多元的全局函數(shù)問題拆分為多個簡單的子函數(shù)的積。因子圖包括兩種節(jié)點：變量節(jié)點和函數(shù)節(jié)點。函數(shù)節(jié)點代表的是局部函數(shù)，而與其相連的那些變量是其自身相關(guān)的自變量[10]。

f(X1,X2,X3,X4)是包含四個變量的全局函數(shù)，f1、f2、f3是其組成的三個局部函數(shù)，全局函數(shù)與局部函數(shù)之間的關(guān)系為f(X1,X2,X3,X4)=f1(X1)f2(X2)f3(X1,X2,X3,X4),具體的因子圖模型如圖2所示，圖中每一條邊都連接著一個變量節(jié)點(圓圈)和一個函數(shù)節(jié)點(方框)；其中fi為函數(shù)節(jié)點，Xj為變量節(jié)點，BI(fi,Xj)表示函數(shù)節(jié)點fi到變量節(jié)點Xj的置信度信息，BI*(Xj,fi)表示變量節(jié)點Xj到函數(shù)節(jié)點fi的置信度信息，i=1,2,3，j=1,2,3,4。

基于因子圖的問題是通過在因子圖中的變量節(jié)點和函數(shù)節(jié)點之間傳遞“置信度信息BI(Belief Information)”解決的，在經(jīng)過多次迭代后，得到收斂解。置信度信息為隨機變量的均值和方差[11]。整個過程通過和積算法[12]實現(xiàn)。以圖2為例，置信度信息在相鄰節(jié)點間傳遞需滿足以下規(guī)則。

(1)從變量節(jié)點傳遞到函數(shù)節(jié)點的置信度信息是所有來自其它相鄰函數(shù)節(jié)點到達這一變量節(jié)點的置信度信息的乘積。

BI*(X1,f3)=BI(f1,X1)BI(f2,X1)

(11)

圖2 因子圖模型

(2)函數(shù)節(jié)點傳遞到變量節(jié)點的置信度信息是所有相鄰變量節(jié)點傳遞給該函數(shù)節(jié)點置信度信息與包含這些變量的函數(shù)的乘積，然后再對所有除此變量外的相關(guān)變量做積分所得。

BI(f3,X3)=

BI*(X2,f3)BI*(X4,f3)dX1dX2dX4

(12)

(3)由以上兩個規(guī)則可推導(dǎo)出，變量節(jié)點的置信度信息等于所有相鄰的函數(shù)節(jié)點到該變量節(jié)點的置信度信息的乘積。

BI(X1)=BI(f1,X1)BI(f2,X1)BI(f3,X1)

(13)

上述規(guī)則為和積算法的基本思路，通過利用和積算法對變量的計算，實現(xiàn)全局問題的局部求解，顯著降低了算法的復(fù)雜度。

3 基于因子圖的USV位置估計算法

由第2節(jié)中式(7)和式(9)可以得知，USV位置估計的過程是由GPS觀測值來估計的，所以該模型可以寫成條件概率密度函數(shù)f(X1,…,Xk+1|Z1,…,Zk+1)，而且k+1時刻的狀態(tài)向量Xk+1只與k時刻的狀態(tài)變量Xk有關(guān)，而與k時刻以前的狀態(tài)變量無關(guān)；k+1時刻的GPS觀測值只與狀態(tài)變量Xk有關(guān)，滿足隱馬爾科夫模型。所以條件概率密度函數(shù)可以因式分解為

f(X1,…,Xk+1|Z1,…,Zk+1)

(14)

式(14)的因子圖見圖3，其中函數(shù)節(jié)點為f(Xi+1|Xi)、f(Zi|Xi),i=1,2,…,k；變量節(jié)點為Xi、Xi,i=1,2,…,k+1。假定圖3中的所有變量節(jié)點都服從高斯分布，均值為A，協(xié)方差矩陣為B2。

(15)

圖3 USV位置估計算法中的因子圖

由式(12)可知，f(Xk+1|Xk)到Xk+1的消息為

BI(f(Xk+1|Xk),Xk+1)=

(16)

f(Zk+1|Xk+1)到Xk+1的消息為

BI(f(Zk+1|Xk+1),Xk+1)=

(17)

由式(11)和式(13)可知，Xk傳遞到f(Xk+1|Xk)的消息為

(18)

將式(8)和式(18)代入式(17)中，得到預(yù)測消息BI(f(Xk+1|Xk),Xk+1)為

BI(f(Xk+1|Xk),Xk+1)=

(19)

(20)

將式(10)和式(20)代入式(17)中，得到預(yù)測消息BI(f(Zk+1|Xk+1),Xk+1)為

(21)

由式(13)可知，

BI(Xk+1)=BI(f(Zk+1|Xk+1),Xk+1)BI(f(Xk+1|Xk),Xk+1)

(22)

由于多個高斯分布的積也是高斯分布，故可以表示為

(23)

利用式(23)可得出BI(Xk+1)。

4 仿真結(jié)果與分析

常用的定位方法包括有只利用外界信息的GPS定位方法、只利用自身信息的航位推算方法和把二者信息結(jié)合的EKF算法。為驗證基于因子圖模型的USV定位算法的有效性，進行Matlab仿真。將本文所提算法與上述三種常用的定位方法進行比較。仿真中設(shè)USV位置初始條件為X0=[0，1000，45]T,USV以4kn的速度、以±2°/s的角速度做S型曲線，航行時間為360×t，采樣時間間隔為t=1s，速度傳感器量測噪聲是標(biāo)準(zhǔn)差取為σv=2kn的零均值高斯白噪聲，航向角的測量噪聲是標(biāo)準(zhǔn)差取σw=5°的零均值高斯白噪聲，GPS的觀測噪聲是標(biāo)準(zhǔn)差取σv=5m的零均值高斯白噪聲。

仿真結(jié)果如下，圖4是USV理想運動軌跡，圖5是4種定位方法的誤差曲線。

圖4 USV理想運動軌跡

圖5 4種定位方法的誤差曲線

從圖5可以看出利用因子圖的USV定位算法可以有效的估計位置信息；當(dāng)USV在(60，150)時間段內(nèi)作圓周運動時，EKF算法定位誤差增加，而本文所提算法一直都很平穩(wěn)，受USV的運動狀態(tài)影響較小。表1是4種定位方法的均方根誤差表。

表1 4種定位方法均方根誤差

從表1可知，本文所提算法可以有效降低GPS定位結(jié)果的誤差，精度提升約71%；估計精度明顯高于EKF算法的定位精度，精度提升約52%。

圖6為在不同觀測噪聲協(xié)方差情況下，EKF算法與因子圖算法的估計誤差與觀測噪聲協(xié)方差的關(guān)系。

圖6 兩種定位誤差與觀測噪聲協(xié)方差的關(guān)系

從圖6中可以看出，兩種算法的估計誤差隨觀測噪聲協(xié)方差的增加而逐漸增大，針對定位精度這一性能指標(biāo)來說，因子圖算法明顯優(yōu)于EKF算法，因子圖的定位效果明顯優(yōu)于EKF算法。

圖7是因子圖方法的定位精度提升率與觀測噪聲協(xié)方差的關(guān)系。

圖7 因子圖方法的定位精度提升率與觀測噪聲協(xié)方差的關(guān)系

從圖7中可以看出，隨著觀測噪聲協(xié)方差的增加，定位精度提升率從85%下降到30%，因此可得出，在觀測噪聲較小情況下，因子圖位置估計算法估計效果比EKF算法更好，而且隨著觀測噪聲的降低，因子圖算法的優(yōu)勢越來越大，所以本文所提算法更適用于觀測噪聲較小情況下的USV定位估計。

5 結(jié)論

本文提出一種基于因子圖的USV位置估計算法，仿真結(jié)果表示，此算法可以有效的估計USV位置信息；把此算法與EKF算法進行比較，發(fā)現(xiàn)定位精度提升了約52%，而且隨著觀測噪聲的減小，均方根誤差比越來越大，可以更好的估計出USV位置信息。隨著傳感器技術(shù)不斷提升，傳感器的精度也會越來越精確，所以基于因子圖的USV位置估計算法更加適用于未來的應(yīng)用情景。