基于因子圖的AUV集群速度估計(jì)和協(xié)同定位

李鑫濱 ,袁蕊霞 ,閆 磊 ,韓 松,3

(1.燕山大學(xué)河北省工業(yè)計(jì)算機(jī)控制工程重點(diǎn)實(shí)驗(yàn)室,河北秦皇島 066004;2.東北大學(xué)秦皇島分校計(jì)算機(jī)與通信工程學(xué)院,河北秦皇島 066004;3.河北省智能康復(fù)及神經(jīng)調(diào)控重點(diǎn)實(shí)驗(yàn)室,河北秦皇島 066004)

1 引言

隨著裝備制造技術(shù)發(fā)展,海洋勘探技術(shù)成為許多國家的戰(zhàn)略重點(diǎn)[1-2].多AUV(autonomous underwater vehicle)協(xié)作系統(tǒng)由于可以有效的執(zhí)行復(fù)雜的海上任務(wù)被得到廣泛關(guān)注[3-4].準(zhǔn)確的定位是實(shí)現(xiàn)多AUV協(xié)同操作的技術(shù)保證.由于陀螺儀、加速度計(jì)等慣性傳感器的測量漂移[5],使得定位誤差累積增長.為了實(shí)現(xiàn)AUV集群中更好的定位,基于聲學(xué)距離測量及信息共享的協(xié)同定位方法被提出,用于實(shí)現(xiàn)定位誤差的協(xié)同修正[6-9].

然而,在復(fù)雜的水下環(huán)境中,傳統(tǒng)的協(xié)同定位方法仍然存在一些挑戰(zhàn).在使用距離測量信息時,時間信息是一個重要的信息,輕微時鐘偏差也會導(dǎo)致顯著的性能損失.在水下環(huán)境中,AUV僅依靠自身的本地時鐘信息,但由于本地器件和環(huán)境差異,本地時鐘存在不同的時間傾斜和漂移問題.對于具有時間漂移的協(xié)同定位系統(tǒng),文獻(xiàn)[10]重新推導(dǎo)濾波過程,得到了最優(yōu)狀態(tài)估計(jì),文獻(xiàn)[11]推導(dǎo)出隨機(jī)兩步時延濾波器.文獻(xiàn)[12]將時延轉(zhuǎn)化為測量偏置,重構(gòu)了測量方程,但其并未解決時鐘傾斜問題.文獻(xiàn)[13]雖然考慮了時鐘傾斜,但其需要有時鐘同步的錨節(jié)點(diǎn)作為輔助,這在沒有錨節(jié)點(diǎn)的AUV協(xié)作系統(tǒng)中難以實(shí)現(xiàn).因此,仍需要考慮AUV間存在的異步時鐘問題.

在傳統(tǒng)定位中,由于陀螺儀、加速度計(jì)等慣性傳感器的漂移,速度信息往往存在較大偏差.多普勒計(jì)程儀可以通過測量相對于水底的相對速度推得自身速度,缺點(diǎn)是其過于昂貴,并且在深海中可能無效.文獻(xiàn)[14]提出從寬帶聲通信信號中提取傳播時間(time of arrival,TOA)和多普勒頻移,將其與慣性測量融合進(jìn)行定位,然而,其使用的方法需要發(fā)射機(jī)的位置固定.文獻(xiàn)[15]提出了一種速度估計(jì)和位置預(yù)測方法,該方法使用鄰居AUV輔助自身進(jìn)行速度估計(jì).然而,參考AUV的速度信息可能不準(zhǔn)確,直接利用參考AUV的錯誤速度信息會導(dǎo)致較大的估計(jì)誤差.因此,如何在不能提供準(zhǔn)確參考AUV速度時,充分利用測量信息進(jìn)行狀態(tài)估計(jì)需要深入研究.

其中擴(kuò)展卡爾曼濾波(extended Kalman filter,EKF)以其計(jì)算簡便的優(yōu)勢常用于協(xié)同定位中[12,16-17].然而,EKF對非線性方程進(jìn)行泰勒展開,由于忽略高階項(xiàng),導(dǎo)致了建模誤差.因子圖提供了一個全局函數(shù)分解成局部函數(shù)的乘積的自然圖形描述[18-19],將大規(guī)模的全局運(yùn)算劃分為簡單的局部運(yùn)算,解決了多源融合應(yīng)用中實(shí)現(xiàn)邊緣后驗(yàn)概率分布的問題.一些文獻(xiàn)將基于因子圖的高斯消息傳遞算法用于協(xié)同定位[20-21].高斯消息傳遞算法具有自然分布和良好的可伸縮性,這使得高斯消息傳遞算法非常適合于多AUV問題.然而,由于距離測量與位置關(guān)系呈非線性,線性化會帶來階段性的誤差.因此,對于AUV協(xié)同定位系統(tǒng)而言,如何降低系統(tǒng)在線性化過程導(dǎo)致的定位誤差,仍然是需要解決的關(guān)鍵問題.

綜上所述,在AUV集群系統(tǒng)中協(xié)同定位中的時鐘異步、不準(zhǔn)確的速度以及協(xié)同定位中線性化過程帶來的影響仍然是定位的難點(diǎn).針對上述問題,首先,本文建立了時延與位置的關(guān)系,提出了基于泰勒迭代方法來實(shí)現(xiàn)AUV時鐘參數(shù)估計(jì),解決了AUV間各不相同的時間傾斜和偏移對距離測量帶來的誤差影響.然后,針對由慣性導(dǎo)航系統(tǒng)測量的速度具有誤差累積的問題,將每個AUV的位置和速度作為變量節(jié)點(diǎn),建立一個整體的協(xié)同定位與速度估計(jì)因子圖模型,通過因子圖的消息傳遞計(jì)算因子變量的邊緣分布,得到了位置和速度變量的估計(jì),有效降低了速度和位置測量的誤差.最后,針對測量函數(shù)線性化過程具有較大誤差的問題,提出了根據(jù)變量的協(xié)方差矩陣構(gòu)造一個自適應(yīng)因子的方法,在因子圖的消息傳遞過程中計(jì)算邊緣分布時,用自適應(yīng)因子調(diào)整了變量的置信度,降低了線性化過程帶來的誤差影響.

2 系統(tǒng)建立與問題描述

2.1 系統(tǒng)建立

在協(xié)作系統(tǒng)中包含n個主AUV,m個從AUV,主AUV配備了高精度的慣性導(dǎo)航系統(tǒng)(inertial measurement unit,IMU),從AUV配備了低精度的慣性導(dǎo)航系統(tǒng),主AUV相對于從AUV具有較精確的位置信息.正常情況下,AUV的航向和速度信息用慣性導(dǎo)航系統(tǒng)進(jìn)行測量,通過壓力傳感器可以準(zhǔn)確地獲得深度信息.每個AUV配備了接收機(jī)和發(fā)射機(jī),用于測量外部時延信息和多普勒頻移信息.AUV在沒有錨節(jié)點(diǎn)輔助的環(huán)境運(yùn)動,依靠自身航向、速度和深度信息粗略地獲得航行器的水下位置,然后利用外部測量的時延信息和多普勒頻移信息及其他AUV的位置信息對自身AUV進(jìn)行位置和速度糾正.系統(tǒng)環(huán)境圖如圖1所示.

圖1 AUV集群協(xié)同定位場景Fig.1 Cooperative localization scenario of AUV swarm

主AUV 的集合為ΩM={1,2,···,n},從AUV的集合為ΩS={n+1,n+2,···,m+n},所有AUV的集合為ΩA={1,2,···,m+n}.運(yùn)動過程表示為離散測量,記為tk,k=0,1,2,···,K.第i個AUV在tk時刻的位置和速度分別定義為xk,i=[xk,i yk,i zk,i]T和vk,i=[vxk,i vyk,i vzk,i]T.假設(shè)AUV運(yùn)動符合隨機(jī)游走模型,AUVi在tk時刻的狀態(tài)xk,i可以從tk-1時刻的狀態(tài)xk-1,i轉(zhuǎn)移過來,更新過程表示如下:

其中:ts為采樣時間間隔;vk-1,i表示時間tk處的速度;表示時間tk-1處的加速度,由IMU的加速度計(jì)測量,即

2)AUVi測量的與AUVj,j ∈ΩA{i}距離為

3)多普勒效應(yīng)造成的發(fā)射和接收信號的頻率之差稱為多普勒頻移,表示為Δf,用fc表示信號的中心頻率,AUVi測量的與AUVj,j ∈ΩA{i}多普勒尺度因子為:D=Δf/(Δf+fc),多普勒尺度因子與位置和速度變量的對應(yīng)關(guān)系為

AUV間通過接收機(jī)與發(fā)射機(jī)進(jìn)行通信,周期性通過信息交互獲取信息,k時刻為完成一個交互周期,需要接收的信息及具體交互過程為:AUV1首先廣播信號給其他AUV,此信號攜帶信息有自身的先驗(yàn)位置和速度信息ut,1、時間信息t及發(fā)出者信息1,信號包發(fā)出的信息,AUV2,···,AUVn接收信號,AUV2接收到AUV1的信號并進(jìn)行測量后,立即向除AUV1以外的AUV廣播信號,并且增加接收到的其他AUV的信息包.AUV3向除AUV2以外的AUV廣播信號,其余AUV與AUV3同理,AUVj廣播信號:AUV(j+1),···,AUVn接收信號.當(dāng)AUVj接收到AUVi廣播的信號,解析信號得到測距信息和多普勒因子測量AUV1在k時得到的全部信息,包含所有AUV的先驗(yàn)位置信息、測距信息及多普勒頻移信息,這些信息用于算法中計(jì)算,得到各個AUV的位置估計(jì),最后把位置估計(jì)廣播給各AUV.以上過程為一次交互過程,在一個定位周期內(nèi)有一次通信交互.完成交互過程后,等待下一個定位周期到達(dá)后再次進(jìn)行交互.

2.2 問題陳述

3 基于因子圖的AUV集群速度估計(jì)和協(xié)同定位

本節(jié)為集群中的AUV設(shè)計(jì)速度估計(jì)與位置估計(jì)的協(xié)同定位算法,在使用外部測量進(jìn)行位置估計(jì)時,需要先進(jìn)行時鐘參數(shù)估計(jì),整個算法結(jié)構(gòu)框圖如圖2所示.其中: 黑色虛線框外上方的部分為測量信息,由慣性測量、距離測量和多普勒因子測量組成,為整個結(jié)構(gòu)的輸入;左側(cè)黑色虛線框?yàn)闋顟B(tài)的先驗(yàn)信息,由慣性信息通過航位推算所得,所得結(jié)果作為算法結(jié)構(gòu)部分的初值輸入;右側(cè)黑色虛線框?yàn)樗惴ńY(jié)構(gòu)部分,輸入為結(jié)構(gòu)圖左側(cè)所得變量的均值與協(xié)方差,輸出為此部分所重新計(jì)算的變量均值與協(xié)方差;黑色虛線框外下方的的部分表示算法結(jié)構(gòu)部分的輸出替換掉之前的先驗(yàn)信息,作為算法結(jié)構(gòu)部分新的輸入,進(jìn)行迭代計(jì)算.

圖2 系統(tǒng)結(jié)構(gòu)框圖Fig.2 System structure diagram

圖2中,算法結(jié)構(gòu)主要分為兩部分,圖中左上紅色虛線框代表第1部分,另一個紅色虛線框代表第2部分.第1部分內(nèi)容為基于泰勒算法的異步時鐘參數(shù)估計(jì),文章將在第3.1節(jié)進(jìn)行介紹.第2部分內(nèi)容為基于因子圖的協(xié)同定位和速度估計(jì),文章將在第3.2節(jié)進(jìn)行介紹.

3.1 基于泰勒算法的異步時鐘參數(shù)估計(jì)

在本小節(jié)中,考慮時鐘異步環(huán)境,將AUV的時鐘等效為具有時鐘傾斜和時鐘偏移的模型.AUVi的本地時鐘表示為

其中:t0表示基準(zhǔn)時間;bi表示AUVi時間與基準(zhǔn)時間的偏差;ai表示AUVi收到信號時的時鐘傾斜.

AUVj傳輸信號到AUVi的到達(dá)時間表示為

則傳播時間可表示為

其中Δτi,j表示測量噪聲.

未知參數(shù)有[ai bi]T,i∈ΩA和[xk,i yk,i]T,i∈ΩS,式(11)左側(cè)的測量時延構(gòu)成測量向量,右側(cè)進(jìn)行泰勒展開,可通過泰勒迭代法進(jìn)行估計(jì).由τi,j組成的測量向量為T={τi,j},i,j ∈ΩA,待估參數(shù)有[ai bi]T,i ∈ΩA.用f(φ)表示測量和變量的函數(shù)關(guān)系,測量模型可表示成如下向量行式:

其 中:φ=[a1···am b1···bm xk,1···xk,m yk,1···yk,m]T;e=[Δτ1,2··· Δτm,m+n-1]T表示測量噪聲向量;T=[τ1,2τ2,1···τ1,m+n τm+n,1···τi,m+n τm+n,i···τm+n-1,m+n τm+n,m+n-1]T.

將式(12)在φ-Δφ處進(jìn)行泰勒展開,化成參數(shù)φ的線性函數(shù).將f(φ)在φ-Δφ處展開,可得

可構(gòu)造如下方程:

由已知信息(時間測量和主AUV的位置信息)及未知參數(shù)(從AUV的位置及時鐘參數(shù))根據(jù)AUV間位置與傳播時間的關(guān)系構(gòu)成式(14)的方程,通過泰勒迭代算法,可得到集群中每個AUV的時鐘傾斜和時鐘偏移參數(shù)的初始估計(jì)

3.2 基于因子圖的協(xié)同定位和速度估計(jì)

得到時鐘參數(shù)后,可以利用測距信息計(jì)算AUV的位置和速度變量的邊緣概率分布.如第2.2節(jié)所述,由于直接計(jì)算邊緣概率分布較復(fù)雜,本文采用計(jì)算簡便的基于因子圖的消息傳遞算法.AUV可以通過消息傳遞利用外部測量信息來協(xié)同推斷其位置和速度.當(dāng)分解后的因子圖與網(wǎng)絡(luò)拓?fù)湎嗥ヅ鋾r,可以使用有效的分布式推理消息傳遞[19],通過消息傳遞算法應(yīng)用于因子圖,得到了變量的近似邊緣分布(“belief”),即b(x)?p(x|z).在消息傳遞算法中,信念(b(x))和消息(μg→x(x))是重要的傳輸信息.可以將任何變量和函數(shù)的關(guān)系用因子圖表示,在本文中,將每個AUV的位置和速度作為變量節(jié)點(diǎn),將距離測量及多普勒因子測量及前一時刻和后一時刻變量轉(zhuǎn)移關(guān)系作為函數(shù)節(jié)點(diǎn).以此建立協(xié)同速度和位置估計(jì)的因子圖模型,利用因子圖模型描述以慣性測量、距離測量、多普勒因子為觀測量的AUV協(xié)同定位與速度估計(jì)過程,以因子圖的信息傳遞用于AUV的位置和速度估計(jì).將式(8)進(jìn)行中因子分解,分解后的因子圖如圖3所示,圖3中函數(shù)節(jié)點(diǎn)分別為

圖3 因子圖模型Fig.3 Factor graph model

1)在貝葉斯框架下的位置和速度信念.

為求得AUV的位置和速度信念,可通過圖4中因子圖模型及貝葉斯框架得,第i個AUV,i ∈ΩA的位置和速度信念表示為

圖4 時鐘參數(shù)誤差圖Fig.4 Clock parameter error

2)對測量函數(shù)進(jìn)行泰勒展開.

得到變量的信念在貝葉斯框架下的表示后,本文致力于變量的信念計(jì)算.通過消息傳遞,變量間利用函數(shù)關(guān)系可以進(jìn)行信念的更新,達(dá)到計(jì)算邊緣分布的目的.在計(jì)算過程中,測量和變量間轉(zhuǎn)換關(guān)系是一個關(guān)鍵點(diǎn).常規(guī)做法是將其進(jìn)行線性化處理,鑒于循環(huán)消息傳遞的迭代性質(zhì),在時刻k和第L次迭代中,根據(jù)估計(jì)位置對測量函數(shù)進(jìn)行一階泰勒展開,即

與距離測量相似,根據(jù)之前的估計(jì)位置對多普勒因子測量函數(shù)同樣進(jìn)行一階泰勒展開,即

3)自適應(yīng)因子.

符號N(x,mx,Vx)表示x服從均值mx,方差為Vx的正態(tài)分布,符號mx表示變量x的均值,符號Vx表示變量x的方差.由于因子圖中的所有信息和信念都是高斯形式,它們可以通過相應(yīng)的均值和協(xié)方差進(jìn)行參數(shù)化.由于一階泰勒展開進(jìn)行線性化會導(dǎo)致近似結(jié)果與實(shí)際有一定偏差,從而在參數(shù)化的計(jì)算過程中引入了誤差.為了抑制線性化的影響,提出了根據(jù)變量的協(xié)方差矩陣構(gòu)造自適應(yīng)因子β.該自適應(yīng)因子構(gòu)造為

其中tr(·)表示矩陣的跡.

在信念的均值和協(xié)方差計(jì)算中引入的自適應(yīng)因子β,β隨當(dāng)前計(jì)算的第i個AUV信念和觀測的第j個AUV信念的協(xié)方差變化,當(dāng)?shù)趈個AUV信念的協(xié)方差增大時,β也增大,先驗(yàn)和觀測融合后協(xié)方差變化減小,均值變化也減小;在消息傳遞迭代過程中,隨著信念接近于實(shí)際值時,第j個AUV信念的協(xié)方差減小,β也減小,先驗(yàn)和觀測融合后協(xié)方差變化增大,均值變化也增大.因此,在觀測可信度低時,利用自適應(yīng)因子β降低均值變化量,在觀測可信度高時,利用自適應(yīng)因子β提高均值變化量,實(shí)現(xiàn)調(diào)整線性化過程的影響.

4)位置信念的均值和協(xié)方差計(jì)算.

根據(jù)文獻(xiàn)[19]的消息傳遞算法,通過參數(shù)化計(jì)算,可以得到第i個AUV,i ∈ΩA位置信念的均值和協(xié)方差為

5)速度信念的均值和協(xié)方差計(jì)算.

與位置信念參數(shù)化相似,速度信念可做參數(shù)化.第i個AUV,i ∈ΩA速度信念的均值和協(xié)方差為

基于迭代消息傳遞,確定了AUV在時刻k的位置和速度信念,然后通過MMSE得到位置和速度估計(jì).綜合上述步驟,文章將基于因子圖的AUV速度估計(jì)和協(xié)同定位算法具體步驟總結(jié)為表1所示.

表1 基于因子圖的AUV集群速度估計(jì)和協(xié)同定位Table 1 Velocity estimation and cooperative localization of AUV swarm based on factor graph

4 仿真與分析

在本文中,使用MATLAB進(jìn)行了數(shù)值模擬.在本實(shí)驗(yàn)中,主AUV個數(shù)設(shè)置為2個,從AUV個數(shù)設(shè)置為4個,初始位置在600×600×50 m3范圍內(nèi)隨機(jī)產(chǎn)生.設(shè)每個AUV的x軸、y軸、z軸初始速度分別為1 m/s,0.4 m/s,0.25 m/s,初始加速度為-0.001 m/s2,-0.001 m/s2,-0.00015 m/s2,總共行駛600 s,每10 s進(jìn)行一次外部信息測量.

為了模擬主AUV和從AUV不同精度的慣性導(dǎo)航設(shè)備,每一步(指進(jìn)行外部測量的10 s)主AUV加速度噪聲設(shè)置為N(0,0.0001),每一步從AUV加速度噪聲設(shè)置為N(0,0.01).在本文中,設(shè)深度信息由深度計(jì)提供,深度計(jì)誤差服從均值為深度的5%、方差為1的正態(tài)分布,AUV深度運(yùn)動范圍在[0 m,150 m]之間.根據(jù)文獻(xiàn)[12]與文獻(xiàn)[23]所述的測量設(shè)備,在600×600×50 m3范圍內(nèi)將距離測量噪聲設(shè)置為N(0,1)較為合理.根據(jù)文獻(xiàn)[14]所述多普勒因子誤差,將多普勒因子測量噪聲設(shè)為N(0,1×10-3).設(shè)2個主AUV,4個從AUV編號依次為1～6,則時鐘參數(shù)設(shè)置為:a=[1.05 1.05 1.05 1.05 1.05 1.05],b=[0 0.12-0.1 0.5 0.1-0.02],消息傳遞迭代次數(shù)為5.最后,利用上述信息對AUV的位置和速度進(jìn)行估計(jì),仿真通過50次蒙特卡羅隨機(jī)實(shí)驗(yàn)來計(jì)算目標(biāo)位置和速度估計(jì)值的誤差.

1)時鐘參數(shù)估計(jì)結(jié)果.

本文評估了時鐘參數(shù)估計(jì)的均方誤差(mean square error,MSE).首先,本文給出了所估計(jì)時鐘參數(shù)與真實(shí)值的誤差值.然后,分析了時鐘異步對位置估計(jì)的影響.對于時鐘傾斜參數(shù),以估計(jì)參數(shù)和實(shí)際參數(shù)差值的絕對值作為評價(jià)指標(biāo);對于時鐘漂移參數(shù),以AUV1 為基準(zhǔn)時鐘,計(jì)算其余AUV與AUV1的時鐘漂移,并以估計(jì)參數(shù)和實(shí)際參數(shù)差值的絕對值作為評價(jià)指標(biāo),即

結(jié)果如圖4,時鐘傾斜的誤差均在1.2×10-3以內(nèi),時鐘漂移的誤差均在1.0×10-3以內(nèi).2)定位和速度估計(jì)結(jié)果.

本文分析了存在時鐘異步和不存在時鐘異步時,所提算法的位置和速度估計(jì)誤差.其中,位置估計(jì)結(jié)果與文獻(xiàn)[20]的(posterior linearization belief propagation,PLBP)算法、EKF、無跡卡爾曼濾波(unscented Kalman filter,UKF)、航位推算(dead reckoning,DR)做對比,其中PLBP算法中L參數(shù)為5,M參數(shù)為5,在與對比算法深度測量信息一致的條件下,通過仿真對比了x和y方向的位置和速度估計(jì)結(jié)果.以實(shí)際位置(速度)與估計(jì)位置(速度)之間的均方根誤差(root mean square error,RMSE)作為評價(jià)指標(biāo).

首先,文章為了驗(yàn)證第3.2節(jié)的算法,對于不存在時鐘異步情況下,本文所提算法和DR,EKF,UKF,PLBP進(jìn)行了對比.AUVs運(yùn)動軌跡如圖5所示,AUV在區(qū)域內(nèi)運(yùn)動,圓標(biāo)記表示從AUV軌跡的起點(diǎn),星標(biāo)記表示主AUV軌跡的起點(diǎn),圖中藍(lán)色曲線代表真實(shí)軌跡,紅色曲線代表本文所估計(jì)軌跡.

圖5 不存在異步時鐘時的AUV軌跡Fig.5 Trajectories of AUVs when there is no asynchronous clock

圖6為位置誤差結(jié)果,在600 s內(nèi),DR方法、PLBP,EKF,UKF,本文所提算法平均誤差如表2的情況1所示,將平均誤差以及所提算法較其他算法提升比例進(jìn)行了總結(jié).因?yàn)殡S著AUV的運(yùn)動,誤差會逐漸累積,導(dǎo)致DR方法的結(jié)果為沒有修正時AUV的誤差,而EKF,UKF和PLBP有一定的修正作用,本文所提算法引入的速度估計(jì)使位置估計(jì)時有更準(zhǔn)確的先驗(yàn)位置信息,因此位置誤差更低.

表2 位置平均誤差Table 2 The average error of the position

圖6 不存在異步時鐘時位置誤差Fig.6 Position error when there is no asynchronous clock

為了驗(yàn)證速度估計(jì)部分的效果,對本文方法、最小二乘法(least sqaure,LS)、EKF及DR的速度估計(jì)進(jìn)行了仿真,仿真結(jié)果如圖7所示.在600 s內(nèi),DR,LS算法,EKF算法.本文所提算法所求速度的平均誤差如表3中情況1所示.本文算法相對于DR,LS算法和EKF算法精度分別提升了96%,89%,53%,上述數(shù)據(jù)在表3中情況1進(jìn)行了總結(jié).這是由于DR方法對速度誤差沒有降低作用,而LS算法依賴于較準(zhǔn)確的主AUV速度,使得LS算法對誤差有一定抑制效果;EKF算法不像LS算法需要盡量多的測量方程,并且根據(jù)測量及前一時刻狀態(tài)進(jìn)行濾波,其比LS算法誤差小,而本文的算法不僅將主AUV的速度用于從AUV進(jìn)行校準(zhǔn),并且通過迭代進(jìn)行相互校準(zhǔn),因此比EKF算法效果好.由結(jié)果可知,本文方法EKF,LS算法均對速度誤差有抑制作用,本文方法效果最明顯.

表3 速度平均誤差Table 3 The average error of the velocity

圖7 不存在異步時鐘時速度誤差Fig.7 Speed error when there is no asynchronous clock

同時,本文比較了存在時鐘異步時,文章的算法和DR,EKF,UKF,PLBP的位置誤差.如圖8所示,在600 s內(nèi),DR方法,PLBP,EKF,UKF,本文所提算法平均誤差如表2的情況2所示.5種方法中,本文所提方法誤差最小,比PLBP提升了86%,比EKF提升了74%,比UKF提升了75%,上述數(shù)據(jù)在表2的情況2中進(jìn)行了總結(jié).由于DR不使用測距信息,時鐘異步影響的測距誤差對DR無影響,DR與無時鐘異步時基本一致,EKF,UKF由于沒有考慮時鐘異步,在不準(zhǔn)確的測距信息影響下,導(dǎo)致效果變差,而文章考慮了時鐘異步后,效果優(yōu)于其他算法.由于解決了異步時鐘的影響,圖8中本文算法誤差與圖7中不存在異步時鐘時誤差相比基本一致,表明解決時鐘異步問題對定位精度有所提升.對本文方法LS,EKF及DR的速度估計(jì)進(jìn)行了仿真.在600 s內(nèi),DR,LS算法、EKF算法,本文所提算法所求速度的平均誤差如表3的情況2所示.由于時鐘異步不影響多普勒頻移信息,表3中情況2結(jié)果與不存在時鐘異步時表3中情況1的結(jié)果基本一致.

圖8 存在異步時鐘時位置誤差Fig.8 Position error when asynchronous clock exists

3)不同參數(shù)下估計(jì)結(jié)果.

本節(jié)將進(jìn)行數(shù)值模擬,以評估如何計(jì)算這個迭代次數(shù)的影響,此外,還考慮了各種環(huán)境參數(shù)來驗(yàn)證算法的性能.本文分析了不同噪聲對算法影響,其中,測距噪聲為零均值,方差為0.5～2.5的高斯噪聲,多普勒因子噪聲為零均值,方差為0.5×10-3～2.5×10-3的高斯噪聲,本實(shí)驗(yàn)對不同噪聲下的結(jié)果進(jìn)行分析,以所有時刻實(shí)際位置與估計(jì)位置之間的均方根誤差之和作為評價(jià)指標(biāo),即

圖9為不同測距噪聲下對位置和速度估計(jì)影響的實(shí)驗(yàn),圖9(a)為位置估計(jì)誤差,如表6所示,本文所提方法比DR,EKF,UKF,PLBP的位置誤差小.圖9(b)為速度估計(jì)誤差,DR,LS,EKF,本文算法平均誤差如表7所示,在不同測距噪聲下速度估計(jì)誤差仍然是本文所提方法最小.如表6-7相關(guān)數(shù)據(jù)所示,本文所提方法適應(yīng)不同測距噪聲,在不同測距噪聲時仍能有良好效果.

表6 位置平均誤差比較Table 6 The average error of the position m

表7 速度平均誤差比較Table 7 The average error of the velocity m/s

圖9 不同測距噪聲影響Fig.9 Effects of different ranging noise

圖10為不同多普勒因子噪聲下對位置和速度估計(jì)影響的實(shí)驗(yàn),與圖9相似,在不同多普勒因子噪聲下,DR,PLBP,EKF,UKF,本文算法的平均位置估計(jì)誤差和平均速度估計(jì)誤差在表6-7中進(jìn)行了總結(jié).位置估計(jì)誤差和速度估計(jì)誤差中本文所提方法最小.因此,仿真結(jié)果證明隨著噪聲變化,本文所提算法的估計(jì)誤差呈平穩(wěn)趨勢,所提算法在不同噪聲下有最低的位置和速度誤差.

圖10 不同多普勒因子噪聲影響Fig.10 Effects of different doppler factor noise

5 結(jié)論

本文提出了一種基于因子圖的速度估計(jì)和協(xié)同定位算法,該方法在AUV協(xié)作系統(tǒng)中利用距離測量和多普勒頻移測量進(jìn)行誤差修正.首先,文章建立了時鐘傾斜和時延與位置的關(guān)系,通過當(dāng)前的后驗(yàn)信息利用泰勒算法近似計(jì)算時鐘參數(shù).針對由慣性導(dǎo)航系統(tǒng)測量的速度具有誤差累積問題,本文建立了利用距離測量和多普勒頻移測量的因子圖模型,利用因子圖模型描述以距離測量和多普勒因子為觀測量的AUV協(xié)同速度和位置估計(jì)過程,提出基于因子圖的速度估計(jì)和協(xié)同定位算法,通過因子圖的消息傳遞,計(jì)算因子變量的邊緣分布,有效降低了速度和位置測量的誤差.最后,在消息傳遞過程中設(shè)計(jì)一自適應(yīng)因子,采用自適應(yīng)因子來調(diào)整觀測的置信度,降低線性化過程帶來的誤差影響.仿真結(jié)果表明,該算法估計(jì)的時鐘參數(shù)誤差在0.001 以內(nèi),在不存在時鐘異步時,該算法比DR,EKF,UKF,PLBP有更低的位置誤差,比DR,LS,EKF有更低的速度誤差.在存在時鐘異步時,該算法通過準(zhǔn)確估計(jì)時鐘參數(shù),其位置誤差和速度誤差與不存在時鐘異步時基本一致,該算法實(shí)現(xiàn)了準(zhǔn)確、高效的定位.