孫 鑫，姜 靜

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

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

1 USV二維平面運(yùn)動(dòng)模型及其線性化

(1)

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

(2)

(3)

圖1 USV二維空間運(yùn)動(dòng)模型

所以USV的二維運(yùn)動(dòng)學(xué)方程可簡(jiǎn)寫(xiě)為

(4)

式中

(5)

(6)

把式(4)展開(kāi)可得線性化后的USV運(yùn)動(dòng)學(xué)方程為

(7)

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

(8)

在k+1時(shí)刻，GPS觀測(cè)的USV位置信息可用觀測(cè)方程表示為

(9)

協(xié)方差矩陣為

由此可得觀測(cè)過(guò)程更新函數(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ǔ)上推廣而來(lái)，其后被用做研究信道估計(jì)、解碼及迭代接收機(jī)技術(shù)的通用工具[8]。因子圖模型具有很強(qiáng)的靈活性，能夠?qū)崿F(xiàn)傳感器的即插即用，已經(jīng)在導(dǎo)航領(lǐng)域受到廣泛關(guān)注[9]。因子圖是一種二向圖，可將一個(gè)復(fù)雜多元的全局函數(shù)問(wèn)題拆分為多個(gè)簡(jiǎn)單的子函數(shù)的積。因子圖包括兩種節(jié)點(diǎn)：變量節(jié)點(diǎn)和函數(shù)節(jié)點(diǎn)。函數(shù)節(jié)點(diǎn)代表的是局部函數(shù)，而與其相連的那些變量是其自身相關(guān)的自變量[10]。

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

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

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

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

(11)

圖2 因子圖模型

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

BI(f3,X3)=

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

(12)

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

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

(13)

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

3 基于因子圖的USV位置估計(jì)算法

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

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

(14)

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

(15)

圖3 USV位置估計(jì)算法中的因子圖

由式(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ù)測(cè)消息BI(f(Xk+1|Xk),Xk+1)為

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

(19)

(20)

將式(10)和式(20)代入式(17)中，得到預(yù)測(cè)消息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)

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

(23)

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

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

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

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

圖4 USV理想運(yùn)動(dòng)軌跡

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

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

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

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

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

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

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

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

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

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

5 結(jié)論

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