基于因子圖的協(xié)同定位與誤差估計(jì)算法

范世偉, 張 亞, 郝 強(qiáng), 姜 畔, 于 飛

(哈爾濱工業(yè)大學(xué)儀器科學(xué)與工程學(xué)院, 黑龍江 哈爾濱 150001)

0 引 言

隨著人類對(duì)海洋的深入開發(fā),自主水下航行器(autonomous underwater vehicle,AUV)在軍事應(yīng)用、科學(xué)研究及海洋探索等方面的作用越來(lái)越重要[1-2]。AUV正確感知自身在環(huán)境中的位置和的能力,是成功執(zhí)行任務(wù)的決定因素。如今的導(dǎo)航不僅確定載體當(dāng)前和可能的未來(lái)位置所必需的測(cè)量和計(jì)算過程,還包含了與載體從一個(gè)點(diǎn)移動(dòng)到另一個(gè)點(diǎn)有關(guān)的所有子問題,如感知、狀態(tài)估計(jì)、避障甚至路徑規(guī)劃等[3-4]。在導(dǎo)航過程中定位技術(shù)則是解決狀態(tài)估計(jì)、避障等問題的關(guān)鍵技術(shù),所以研究AUV的高精度定位技術(shù)具有重要意義[5-6]。由于電磁信號(hào)在水下環(huán)境中會(huì)被強(qiáng)烈衰減導(dǎo)致全球定位系統(tǒng)(global positioning system, GPS)技術(shù)在水下無(wú)法正常應(yīng)用[7]。因此,必須采用其他技術(shù)為AUV提供準(zhǔn)確的導(dǎo)航能力,通常情況下AUV的位置是根據(jù)計(jì)程儀配合羅經(jīng)或者利用慣性導(dǎo)航系統(tǒng)(inertial navigation systems, INS)來(lái)確定的,但是當(dāng)AUV運(yùn)動(dòng)時(shí),根據(jù)計(jì)程儀配合羅經(jīng)進(jìn)行航位推算的位置估計(jì)誤差不斷累積,最終可能會(huì)導(dǎo)致AUV丟失,而高精度的INS通常價(jià)格高昂,不利于AUV的大規(guī)模應(yīng)用[8-10]。由于對(duì)海洋的深入開發(fā),單個(gè)AUV難以執(zhí)行復(fù)雜的軍事及民事任務(wù),導(dǎo)致多AUV協(xié)同系統(tǒng)在海洋開發(fā)、科學(xué)研究及軍事應(yīng)用等方面的作用更加重要[11]。因此,基于水聲通信網(wǎng)絡(luò)的多AUV的協(xié)同定位技術(shù)受到了更多的關(guān)注[12-13]。不同AUV利用水聲通信來(lái)共享彼此的信息,利用水聲測(cè)距進(jìn)行相對(duì)距離的測(cè)量作為觀測(cè)量,然后利用數(shù)據(jù)融合技術(shù)來(lái)降低定位誤差,這種方式被稱為“協(xié)同定位”。由于多AUV協(xié)同定位可以提高系統(tǒng)整體的定位能力,而且具有成本低、系統(tǒng)魯棒性好的優(yōu)點(diǎn),可以極大地增強(qiáng)多AUV系統(tǒng)的任務(wù)執(zhí)行能力,具有重要的理論和應(yīng)用價(jià)值。因此,多AUV的協(xié)同定位技術(shù)漸漸發(fā)展為一個(gè)熱門的研究課題,研究能夠提高多AUV協(xié)同定位精度的定位算法具有重要意義[14-15]。

目前,國(guó)內(nèi)外學(xué)者對(duì)協(xié)同定位的研究主要集中在如何利用觀測(cè)信息直接提高定位精度方面,針對(duì)從艇的速度誤差和航向誤差進(jìn)行估計(jì)及補(bǔ)償算法還有待研究[16]。例如,Webster研究了基于擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)的協(xié)同定位算法,研究結(jié)果表明EKF算法能夠有效實(shí)現(xiàn)多AUV系統(tǒng)協(xié)同定位[17]。但是該研究具有以下局限:① 面對(duì)非線性問題時(shí),EKF實(shí)際是將非線性系統(tǒng)取泰勒展開一階項(xiàng)進(jìn)行線性化,由于截?cái)嘁鸬母唠A截?cái)嗾`差會(huì)對(duì)系統(tǒng)估計(jì)結(jié)果產(chǎn)生較大影響,協(xié)同定位精度降低[18];② 算法沒有對(duì)從艇速度誤差及航向誤差進(jìn)行估計(jì)和補(bǔ)償,降低了協(xié)同定位精度。

為便于算法的實(shí)施,本文以高斯噪聲為前提,通過在協(xié)同定位的因子圖模型中計(jì)算不同變量的均值及方差來(lái)得到從艇的后驗(yàn)位置估計(jì)結(jié)果。然后,根據(jù)位置的先驗(yàn)估計(jì)作為觀測(cè)量,在誤差估計(jì)的因子圖模型中計(jì)算從艇的速度誤差及航向誤差。最后,通過將估計(jì)的誤差結(jié)果補(bǔ)償?shù)絽f(xié)同定位算法中,來(lái)提高從艇的定位精度?；谝蜃訄D的協(xié)同定位算法充分利用了因子圖的特點(diǎn),將復(fù)雜的問題分為若干簡(jiǎn)單問題,保障算法精度的同時(shí)降低了計(jì)算量。

本文的主要貢獻(xiàn)包括:① 針對(duì)多AUV協(xié)同定位的數(shù)據(jù)融合問題,設(shè)計(jì)了基于距離測(cè)量的協(xié)同定位系統(tǒng)因子圖模型,提出了基于因子圖模型的多AUV協(xié)同定位算法;② 分析了從艇速度誤差及航向誤差對(duì)其定位的影響,并提出了基于因子圖的誤差估計(jì)算法,大幅提高了從艇的定位能力。

1 協(xié)同定位系統(tǒng)分析

1.1 AUV運(yùn)動(dòng)學(xué)模型

利用AUV搭載的深度計(jì)可以準(zhǔn)確地測(cè)量AUV所處深度,可以將各AUV的位置信息及距離觀測(cè)信息投影到水平面內(nèi),所以本文在二維平面內(nèi)討論多AUV協(xié)同定位問題?？梢詫UV的二維運(yùn)動(dòng)模型寫成如下形式:

(1)

式中,(x,y)表示AUV的位置坐標(biāo);v表示AUV在航行時(shí)的速度;θ表示AUV的航向。

在k時(shí)刻利用計(jì)程儀測(cè)量得到的速度表示為vk,羅經(jīng)測(cè)量得到的航向表示為θk,此時(shí)AUV的位置表示為(xk,yk),根據(jù)式(1)可以得到AUV的離散運(yùn)動(dòng)模型為

(2)

式中,Δt表示采樣周期。

1.2 基于水聲測(cè)距的量測(cè)模型

本文主要研究主從式AUV協(xié)同定位系統(tǒng),主艇配備著高精度的導(dǎo)航設(shè)備,在協(xié)同定位過程中,認(rèn)為主艇的位置準(zhǔn)確,利用主艇位置修正配備低精度導(dǎo)航設(shè)備的從艇,此時(shí)主從艇間相互通信,從艇獲取主艇位置信息與觀測(cè)信息。水下協(xié)同定位系統(tǒng)中的觀測(cè)信息是利用水聲設(shè)備得到的相對(duì)距離,因此協(xié)同定位系統(tǒng)的量測(cè)方程可以表示為

(3)

1.3 定位誤差分析

從式(2)可以看出,影響定位精度的關(guān)鍵因素就是速度及航向誤差。根據(jù)AUV的運(yùn)動(dòng)學(xué)模型,在l時(shí)刻利用測(cè)量得到的速度及航向計(jì)算位置如下:

(4)

(5)

(6)

將式(5)和式(6)代入式(4)可得

(7)

將式(7)中的兩式分別展開，可得

(8)

(9)

假設(shè)速度及航向誤差為小量,那么有

(10)

將式(10)代入式(8)中可得

(11)

將式(10)代入式(9)中可得

(12)

2 協(xié)同定位與誤差估計(jì)算法

(13)

l時(shí)刻的位置誤差可以寫成

(14)

從l時(shí)刻到k時(shí)刻由于沒有接收到量測(cè)信息,所以位置更新采用先驗(yàn)估計(jì)值,對(duì)x的先驗(yàn)估計(jì)如下:

(15)

同理,對(duì)y的先驗(yàn)估計(jì)如下:

(16)

將式(13)代入式(15)中可得

(17)

將式(13)代入式(16)中可得

(18)

那么k時(shí)刻的位置誤差可以表示為

(19)

(20)

式中,

根據(jù)式(19)、式(20)以及AUV的運(yùn)動(dòng)學(xué)模型和量測(cè)模型,可以得到協(xié)同定位與誤差估計(jì)的因子圖模型,如圖1所示。圖1中的紅色實(shí)線框內(nèi)是算法時(shí)間更新部分,紫色虛線框內(nèi)是速度及航向誤差估計(jì)的量測(cè)更新部分,綠色點(diǎn)劃線框內(nèi)是協(xié)同定位的量測(cè)更新部分。

圖1 協(xié)同定位及誤差估計(jì)算法的因子圖模型Fig.1 Factor graph model of cooperative positioning and error estimation algorithm

2.1 協(xié)同定位算法

在執(zhí)行基于因子圖模型的協(xié)同定位算法時(shí),需要在變量節(jié)點(diǎn)與函數(shù)節(jié)點(diǎn)間傳遞均值和方差,具體步驟如下。

步驟 1初始化

在協(xié)同定位開始時(shí)確定初始狀態(tài):

(21)

那么,可以得到如下表達(dá)式:

(22)

(23)

步驟 3xk和yk的第一次更新

首先引入轉(zhuǎn)換矩陣:

(24)

(25)

(26)

其中,

(27)

(28)

(29)

步驟 5Δxk和Δyk的第一次更新

函數(shù)節(jié)點(diǎn)Ck和Dk的作用是轉(zhuǎn)換相對(duì)位置信息與絕對(duì)位置信息。因此,從節(jié)點(diǎn)Ck傳遞到節(jié)點(diǎn)Δxk的概率密度函數(shù)可以表示為

(30)

對(duì)于節(jié)點(diǎn)Dk,從節(jié)點(diǎn)Dk傳遞到節(jié)點(diǎn)Δyk的概率密度函數(shù)可以表示為

(31)

步驟 6Δxk和Δyk的第二次更新

函數(shù)節(jié)點(diǎn)Ek的作用是將x組坐標(biāo)系與y組結(jié)合起來(lái)。根據(jù)勾股定理,變量節(jié)點(diǎn)Δxk與Δyk之間的約束可以描述為

(32)

從函數(shù)節(jié)點(diǎn)Ek傳遞到變量節(jié)點(diǎn)Δyk的概率密度函數(shù)可以表示為

(33)

從函數(shù)節(jié)點(diǎn)Ek傳遞到變量節(jié)點(diǎn)Δxk的概率密度函數(shù)可以表示為

(34)

(35)

(36)

步驟 8xk和yk的第二次更新

經(jīng)過計(jì)算整理后得到xk和yk在更新后的概率密度函數(shù)分別為

(37)

(38)

(39)

其均值和方差的計(jì)算與式(28)和式(29)相似,不再贅述。到此完成了圖1中綠色點(diǎn)劃線內(nèi)協(xié)同定位的部分。

2.2 誤差估計(jì)算法

各國(guó)學(xué)者對(duì)協(xié)同定位的研究主要關(guān)注協(xié)同定位算法方面,如EKF、無(wú)跡卡爾曼濾波等,目前并沒有在協(xié)同定位系統(tǒng)中對(duì)速度及航向誤差進(jìn)行估計(jì)的研究,本文為了提高協(xié)同定位精度,首次開展了誤差估計(jì)算法的研究。

將式(19)和式(20)中的誤差公式重寫為

(40)

(41)

成立，則進(jìn)一步有

(42)

在協(xié)同定位算法中位置的時(shí)間更新時(shí),需要利用速度和航向的測(cè)量值減掉相應(yīng)的估計(jì)誤差后參與計(jì)算,具體如下:

(43)

(44)

(45)

(46)

(47)

(48)

(49)

(50)

將式(49)代入到式(48)中,可以得到

(51)

(52)

(53)

(54)

(55)

(56)

(57)

(58)

(59)

將式(49)和式(58)代入到式(57),可以得到

(60)

(61)

在此可以將式(60)的期望和方差表示為

(62)

(63)

(64)

可以將式(63)的期望和方差表示為

(65)

(66)

若k時(shí)刻沒有接收到觀測(cè)信息,有

(67)

(68)

在從艇接收到觀測(cè)信息后,首先利用協(xié)同定位算法輸出的位置后驗(yàn)估計(jì)和先驗(yàn)估計(jì)求取δxk和δyk的期望和方差:

(69)

(70)

(71)

(72)

(73)

(74)

(75)

(76)

(77)

(78)

(79)

(80)

(81)

至此,完成了基于因子圖模型的速度及航向誤差估計(jì),結(jié)合第2.1節(jié)的協(xié)同定位算法共同構(gòu)成基于因子圖的協(xié)同定位及誤差估計(jì)(factor graph-based cooperative positioning and error estimation, FGCPAEE)算法,為了清晰描述FGCPAEE算法,其偽代碼如下:

算法 1

3 船載試驗(yàn)驗(yàn)證

為了驗(yàn)證本文所提算法在處理實(shí)際數(shù)據(jù)時(shí)的有效性,利用船載湖試試驗(yàn)數(shù)據(jù)進(jìn)行離線處理與分析。由于條件限制,湖試試驗(yàn)利用水面艇代替AUV進(jìn)行。而淺水水域的聲學(xué)通信環(huán)境與深水水域相比更加復(fù)雜,且其系統(tǒng)噪聲特性完全可以表示AUV系統(tǒng)。湖試試驗(yàn)方案中采用兩條主艇、一條從艇進(jìn)行試驗(yàn),試驗(yàn)過程中兩條主艇的位置信息采用高精度的實(shí)時(shí)GPS位置信息,利用水聲通信設(shè)備實(shí)現(xiàn)主從艇之間信息傳遞及觀測(cè)量的測(cè)量。從艇自身的定位是利用多普勒計(jì)程儀提供的速度信息和磁羅經(jīng)提供的航向信息計(jì)算得到的。為提供參考基準(zhǔn),在相對(duì)固定的聯(lián)合安裝基座上還配備位置精度為2～5 cm的高精度組合導(dǎo)航系統(tǒng)。水聲通信是實(shí)現(xiàn)AUV協(xié)同定位的重要環(huán)節(jié),湖試中利用S2CR7/17型水聲通信設(shè)備實(shí)現(xiàn)水聲通信及測(cè)距。在進(jìn)行試驗(yàn)時(shí)兩條主艇各自每隔5 s向從艇發(fā)送帶標(biāo)識(shí)符的GPS位置信息以及主從艇間相對(duì)距離信息，其他傳感器信息的采樣周期為1 s。

設(shè)置試驗(yàn)時(shí)間為2 000 s,試驗(yàn)數(shù)據(jù)僅在前1 000 s進(jìn)行協(xié)同定位和誤差估計(jì),1 000 s后進(jìn)行自主定位,與EKF算法和改進(jìn)的因子圖與和積(improved factor graph and sum-product, IFGS)算法進(jìn)行定位誤差的對(duì)比[19]。試驗(yàn)中各艇軌跡如圖2所示。

圖2 試驗(yàn)中各艇航行軌跡Fig.2 Running track of each boat in the experiment

試驗(yàn)中FGCPAEE算法對(duì)從艇的速度誤差及航向誤差的估計(jì)結(jié)果如圖3所示,由于1 000 s后不再進(jìn)行協(xié)同定位,因此1 000 s后速度誤差及航向誤差不再更新。從圖3中可以看出,試驗(yàn)中從艇的速度誤差估計(jì)值在0.1～0.2 m/s,航向誤差的估計(jì)值在-1°～0°。

對(duì)試驗(yàn)數(shù)據(jù)分別利用3種算法進(jìn)行協(xié)同定位，定位誤差的結(jié)果如圖4所示。從圖4中可以看出,在1 000 s之后不再接收觀測(cè)信息時(shí),EKF算法和IFGS算法由于沒有對(duì)速度及航向誤差進(jìn)行估計(jì)和補(bǔ)償，導(dǎo)致定位誤差增長(zhǎng)很快,而本文所提算法則明顯降低了誤差增長(zhǎng)速度。

圖3 FGCPAEE算法的誤差估計(jì)結(jié)果Fig.3 Error estimation results of FGCPAEE algorithm

圖4 不同算法的定位誤差曲線Fig.4 Positioning error curves of different algorithms

各算法前1 000 s定位誤差的均值和均方根如圖5所示。從圖5可以看出，這段時(shí)間內(nèi),EKF算法的RMSE為17.13 m,IFGS算法的RMSE為12.79 m,FGCPAEE算法的RSME為12.67 m,FGCPAEE算法的定位誤差最小。

圖5 不同算法前1 000 s的定位誤差比較Fig.5 Comparison of positioning errors of different algorithms before 1 000 s

3種算法的處理時(shí)間如表1所示,實(shí)驗(yàn)采用的計(jì)算機(jī)為8 G內(nèi)存,2.3 G主頻,Matlab語(yǔ)言。從表1可以看出,本文提出的FGCPAEE算法在定位誤差最小的同時(shí)運(yùn)行時(shí)間小于EKF算法,說明此算法可以進(jìn)行在線的實(shí)時(shí)計(jì)算。

表1 算法運(yùn)行時(shí)間比較

不同算法后1 000 s定位誤差的均值和RMSE如圖6所示。從圖6可以看出,這段時(shí)間內(nèi),EKF算法的RMSE為99.43 m,IFGS算法的RMSE為96.92 m,FGCPAEE算法的RMSE為35.89 m,此時(shí)顯然FGCPAEE算法的定位性能大幅優(yōu)于另外兩種算法。

圖6 不同算法后1 000 s的定位誤差比較Fig.6 Comparison of positioning errors of different algorithms after 1 000 s

4 結(jié) 論

本文研究了多AUV協(xié)同定位的數(shù)據(jù)融合問題,提出了速度及航向誤差估計(jì)算法的因子圖模型,并推導(dǎo)了速度和航向誤差估計(jì)公式。在高斯噪聲的前提下,因子圖模型利用變量的期望和方差作為因子間傳遞的信息,推導(dǎo)了FGCPAEE算法的估計(jì)過程。船載試驗(yàn)結(jié)果驗(yàn)證了本文所提算法的有效性,在實(shí)驗(yàn)過程中前1 000 s的協(xié)同定位階段與經(jīng)典的EKF算法相比均方根誤差減少了26.04%;在后1 000 s的自主定位階段與經(jīng)典的EKF算法相比均方根誤差減少了63.9%。

本文所提算法具有較大的適用性,當(dāng)主艇無(wú)法提供觀測(cè)信息時(shí),速度誤差及航向誤差對(duì)傳統(tǒng)算法影響很大,而對(duì)本文所提算法影響較小,有利于應(yīng)對(duì)多AUV執(zhí)行任務(wù)時(shí)的突發(fā)情況。