考慮通信延遲的多自治水下航行器協(xié)同定位算法

盧 健,陳 旭,羅毛欣,楊騰飛

(西安工程大學(xué)電子信息學(xué)院,陜西西安 710048)

1 引言

多協(xié)同體編隊(duì)結(jié)構(gòu)規(guī)劃[1–3]、協(xié)同定位模型及相關(guān)算法設(shè)計(jì)[4–5]和環(huán)境約束下誤差建模與補(bǔ)償[6–7]是水下協(xié)同定位(cooperative localization,CL)方法研究的3個(gè)主要方向.多自治水下航行器(autonomous underwater vehicle,AUV)協(xié)同定位主流是采用“n主帶m從”的主從式[8]編隊(duì)結(jié)構(gòu),借助于主AUV高精度的自定位信息和主從AUV間相對(duì)量測(cè)有效改善低自定位水平從AUV的定位精度.相比于基于優(yōu)化理論[9]和基于圖論[10]的算法,基于Bayes估計(jì)的協(xié)同定位算法由于能夠有效地融合被定位體內(nèi)外部定位相關(guān)信息[11],成為了主流方法.作為一種重要的Bayes濾波器,擴(kuò)展卡爾曼濾波(extended Kalman filtering,EKF)具有計(jì)算量小、易于工程實(shí)現(xiàn)等特點(diǎn),特別適宜處理處于水下環(huán)境的AUV的定位問(wèn)題.文獻(xiàn)[12]提出了一種基于EKF的多AUV主從式協(xié)同定位方法,仿真驗(yàn)證了這種基于概率學(xué)方法的有效性.文獻(xiàn)[12]的方法本質(zhì)是一種基于整周期濾波的算法,本文的研究中將其作為基礎(chǔ)算法并進(jìn)行改造,同時(shí)將其作為仿真分析的對(duì)比方法.

在誤差的建模與補(bǔ)償問(wèn)題中,根據(jù)環(huán)境約束條件優(yōu)化協(xié)同定位算法或改善協(xié)同編隊(duì)結(jié)構(gòu)[13]是建構(gòu)有效的誤差在線(xiàn)估計(jì)模型和補(bǔ)償算法[14]重要保證.水下環(huán)境使用聲信號(hào)作為載體進(jìn)行信息傳輸為最有效手段.水聲通信有兩個(gè)基本任務(wù):形成測(cè)量值和傳遞狀態(tài)信息.量測(cè)的生成是利用水聲傳播時(shí)延(time-of-flight,TOF)原理:

聲信號(hào)的發(fā)送端和接收端必須使用同一時(shí)鐘或已同步的不同時(shí)鐘. c為聲波在水中的傳播速度,和tk分別為主AUV發(fā)送探測(cè)脈沖信號(hào)和從AUV接收到此脈沖信號(hào)的時(shí)刻.

圖1 主從AUV間的水聲通信Fig.1 The underwater acoustic communication between the master and the slaver AUV

實(shí)際中,主AUV發(fā)射的探測(cè)脈沖信號(hào)和傳送狀態(tài)信息的通信數(shù)據(jù)包一般由不同裝置產(chǎn)生,并且,為了避免造成相互干擾,將二者進(jìn)行分時(shí)發(fā)射.如圖1,單周期內(nèi)AUV間水聲通訊信號(hào)的發(fā)射過(guò)程須分兩步:第1步發(fā)射用于形成量測(cè)的聲脈沖信號(hào);第2步延遲一個(gè)足夠小的隨機(jī)時(shí)間,發(fā)射含有脈沖信號(hào)發(fā)送時(shí)刻的主AUV運(yùn)動(dòng)狀態(tài)信息的通信數(shù)據(jù)包.除了水聲傳播時(shí)延之外,由于水下環(huán)境復(fù)雜性導(dǎo)致的水聲多徑傳播、傳感器的固有延遲和信號(hào)在器件內(nèi)部的處理與傳輸延遲等原因,脈沖串和通信數(shù)據(jù)包的傳送過(guò)程都存在一定的固有和隨機(jī)延遲,而且通信數(shù)據(jù)包信息由于丟包、數(shù)據(jù)解構(gòu)和傳送帶寬限制等原因,延遲時(shí)間會(huì)更長(zhǎng).

如何減小通信延遲對(duì)提升協(xié)同定位精度的不利影響是環(huán)境約束下誤差建模與補(bǔ)償研究的重點(diǎn)問(wèn)題.文獻(xiàn)[15]提出了在從AUV量測(cè)信息接收時(shí)刻利用新息對(duì)估計(jì)誤差進(jìn)行補(bǔ)償?shù)姆椒?但是該類(lèi)方法需要存儲(chǔ)器對(duì)延遲時(shí)間內(nèi)的估計(jì)狀態(tài)進(jìn)行保存,并要求對(duì)量測(cè)信息進(jìn)行時(shí)間上的排序,增加了定位系統(tǒng)的負(fù)擔(dān).文獻(xiàn)[16]通過(guò)建立一個(gè)主AUV運(yùn)動(dòng)狀態(tài)估計(jì)器,在狀態(tài)信息延遲的條件下,實(shí)時(shí)完成了協(xié)同定位,降低了對(duì)硬件存儲(chǔ)的要求.但是其對(duì)出序的通信數(shù)據(jù)包采取了丟棄的做法,沒(méi)有充分利用信息,損失了精度.

按照信息是否能夠以其所產(chǎn)生的時(shí)間順序到達(dá)定位系統(tǒng),通信延遲分為信息順序到達(dá)和信息出序到達(dá)兩種情景.雖然沒(méi)有出序,但是延遲了的順序信息依然會(huì)造成估計(jì)精度的降低或?qū)崟r(shí)性變差.設(shè)計(jì)一種更適合處理本類(lèi)延遲信息的協(xié)同定位方法是本文所建立的關(guān)于通信延遲協(xié)同定位算法體系的重要一環(huán).信息發(fā)生出序是通信延遲所帶來(lái)的一個(gè)嚴(yán)重后果.信息出序可能造成出序信息不能夠被使用或者高成本被使用.使用Bayes濾波器時(shí),對(duì)于出序信息的處理方法文[17]中有多種:①丟棄滯后數(shù)據(jù)法:簡(jiǎn)單易行且計(jì)算量小,但大量數(shù)據(jù)棄用會(huì)嚴(yán)重降低定位精度;②數(shù)據(jù)緩存法:精度高,但需要大量存儲(chǔ)空間緩存數(shù)據(jù),濾波器輸出滯后;③重新濾波法:最終精度及存儲(chǔ)要求與數(shù)據(jù)緩存法相同,造成重復(fù)濾波,實(shí)時(shí)性差;④直接更新法:利用出序信息直接在最新采樣周期內(nèi)進(jìn)行狀態(tài)更新,存儲(chǔ)量和計(jì)算量都很小,濾波輸出沒(méi)有滯后且精度較高,實(shí)時(shí)性強(qiáng).

在發(fā)生信息出序時(shí),如何平衡高精度估計(jì)值對(duì)數(shù)據(jù)緩存的需求與運(yùn)動(dòng)系統(tǒng)輸出實(shí)時(shí)性要求之間的矛盾,是水下協(xié)同定位一個(gè)亟待解決的難點(diǎn)問(wèn)題.本文建立了水聲探測(cè)和水聲通信的時(shí)延模型,給出了AUV的運(yùn)動(dòng)學(xué)模型和水聲量測(cè)模型,在提出延遲信息順序到達(dá)算法的基礎(chǔ)上,重點(diǎn)研究如何采用直接更新的方法解決信息出序到達(dá)時(shí)的主從式協(xié)同定位問(wèn)題.本文建立了一類(lèi)全新的針對(duì)水聲通信延遲的協(xié)同定位算法,其優(yōu)先保證實(shí)時(shí)性,不緩存量測(cè)數(shù)據(jù)和中間結(jié)果,估計(jì)結(jié)果具有較高的精度.

2 水聲探測(cè)和通信時(shí)延模型

除了水聲傳播時(shí)延,脈沖串和通信數(shù)據(jù)包發(fā)送與接收過(guò)程中的滯后時(shí)間還有:

1) 脈沖串發(fā)送端的固有時(shí)延.即聲脈沖信號(hào)從開(kāi)始啟動(dòng)發(fā)射(kT時(shí)刻)到物理設(shè)備真正發(fā)出信號(hào)的時(shí)間.此時(shí)延極短,且為固定時(shí)延.

2) 脈沖串處理時(shí)延.即聲脈沖被接收后對(duì)其進(jìn)行處理的時(shí)間,包括信息在從AUV內(nèi)部的傳輸和形成量測(cè)值的時(shí)間.此時(shí)延相對(duì)較短,但為隨機(jī)時(shí)延.

3) 通信數(shù)據(jù)包的發(fā)射時(shí)延.在探測(cè)脈沖啟動(dòng)發(fā)送之后,延遲一個(gè)較小的隨機(jī)時(shí)間,才發(fā)射含有脈沖串發(fā)送時(shí)刻的主AUV運(yùn)動(dòng)狀態(tài)信息的水聲通信數(shù)據(jù)包.此時(shí)延相對(duì)較短,為隨機(jī)時(shí)延.

4) 通信數(shù)據(jù)處理時(shí)延.此時(shí)延包括通信數(shù)據(jù)包被發(fā)送、被接收、被解析和被重構(gòu)以及在從AUV內(nèi)傳輸?shù)臅r(shí)間.此時(shí)延相對(duì)較長(zhǎng),為隨機(jī)時(shí)延.本時(shí)延占用較長(zhǎng)時(shí)間的一個(gè)重要原因是需要對(duì)多徑傳播的大信息量的數(shù)據(jù)進(jìn)行處理.

根據(jù)以上分析,建立水聲探測(cè)和水聲通信的時(shí)延模型.

探測(cè)時(shí)延:

其中:τa為探測(cè)脈沖串水聲傳播時(shí)延,τb為脈沖串發(fā)送端的固有時(shí)延,τc為脈沖串處理時(shí)延;τα為通信數(shù)據(jù)水聲傳播時(shí)延,τβ為通信數(shù)據(jù)的發(fā)射時(shí)延,τγ為通信數(shù)據(jù)處理時(shí)延.

用各種時(shí)延對(duì)水聲探測(cè)和水聲通信過(guò)程進(jìn)行分解后,對(duì)于信息的到達(dá)可做如下界定:

1) 脈沖串到達(dá)AUV時(shí)刻.即從AUV接收到脈沖的時(shí)刻.從AUV根據(jù)本時(shí)刻與脈沖發(fā)出時(shí)刻獲取τa,并根據(jù)式(1)計(jì)算出量測(cè)值;

2) 脈沖串到達(dá)濾波器時(shí)刻.此時(shí)接收AUV已解算出量測(cè)值,并可以被協(xié)同定位算法使用.此時(shí)刻與脈沖串到達(dá)AUV時(shí)刻相差一個(gè)τc;

3) 狀態(tài)數(shù)據(jù)到達(dá)濾波器時(shí)刻.此時(shí)通信數(shù)據(jù)包含有的狀態(tài)信息已被解析為可被濾波器直接使用的數(shù)據(jù),一般情況下,通信數(shù)據(jù)包延遲到達(dá)時(shí)間明顯長(zhǎng)于脈沖串延遲到達(dá)時(shí)間.從發(fā)射脈沖串開(kāi)始,到此時(shí)刻正好經(jīng)歷整個(gè)通信延遲時(shí)間τcom.

通信數(shù)據(jù)包中的主AUV的狀態(tài)信息的時(shí)戳界定為真正發(fā)出脈沖串的時(shí)刻;通信數(shù)據(jù)包中含有的關(guān)于主AUV的狀態(tài)信息,主要為主AUV的本體傳感器在此時(shí)戳點(diǎn)對(duì)自身位置的觀測(cè)信息和推算結(jié)果.從AUV的運(yùn)動(dòng)狀態(tài)更新點(diǎn)的時(shí)戳為脈沖串到達(dá)時(shí)刻,由從AUV記錄本時(shí)刻,并于通信數(shù)據(jù)包到達(dá)后在本時(shí)戳點(diǎn)更新運(yùn)動(dòng)狀態(tài)的估計(jì)值.時(shí)延的隨機(jī)性增加了考慮通信時(shí)延的協(xié)同定位問(wèn)題的復(fù)雜度.

由于探測(cè)脈沖的時(shí)延較小,所以本文將研究重點(diǎn)放在通信數(shù)據(jù)包時(shí)延對(duì)定位精度的影響和補(bǔ)償問(wèn)題上.

3 運(yùn)動(dòng)學(xué)和量測(cè)模型

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

由于深度信息可由壓力傳感器直接測(cè)得,則定位問(wèn)題的研究可以由3D空間中轉(zhuǎn)化至2D平面內(nèi).其運(yùn)動(dòng)軌跡可在oxy坐標(biāo)系下進(jìn)行描述,如圖2所示,k時(shí)刻的位置可表示為(xk,yk).

圖2 AUV運(yùn)動(dòng)狀態(tài)量簡(jiǎn)圖Fig.2 The diagram of the motion state of AUV

AUV運(yùn)動(dòng)學(xué)模型可表述為

其中:T為AUV的采樣周期,Vk,?k為AUV在k時(shí)刻的運(yùn)動(dòng)速度和偏航角.

控制向量表述為

由于本文重點(diǎn)考慮延遲時(shí)間與定位精度的關(guān)系問(wèn)題,所以在AUV的運(yùn)動(dòng)學(xué)模型中加入推算時(shí)間T:

3.2 量測(cè)模型

兩AUV每隔時(shí)間T進(jìn)行一次協(xié)同定位,由從(salver)AUV解算出的其與主(master)AUV的相對(duì)距離可表示為

Rk是誤差協(xié)方差.

量測(cè)模型可簡(jiǎn)單地描述為

4 信息順序到達(dá)協(xié)同定位算法

為研究簡(jiǎn)便,設(shè)環(huán)境中只存在一個(gè)主AUV,相鄰時(shí)刻主AUV發(fā)送的脈沖串與通信數(shù)據(jù)包可按照順序到達(dá)從AUV的濾波器.

如圖3所示,盡管第k個(gè)周期內(nèi)的主AUV的通信數(shù)據(jù)包在k+1個(gè)周期才到達(dá)從AUV的濾波器,但還是早于第k+1個(gè)周期主AUV發(fā)送的通信數(shù)據(jù)包到達(dá),通信有延遲但沒(méi)有出序.從AUV的運(yùn)動(dòng)狀態(tài)估計(jì)應(yīng)在k周期內(nèi)tk時(shí)刻和k+1周期內(nèi)tk+1時(shí)刻完成.根據(jù)時(shí)延的定義和模型,圖中時(shí)間含義及關(guān)系為

其中:t1為主AUV發(fā)送出脈沖串的時(shí)刻,tk為從AUV接收到脈沖串的時(shí)刻,t1+τβ為主AUV發(fā)送通信數(shù)據(jù)包的時(shí)刻,為通信數(shù)據(jù)包到達(dá)從AUV濾波器的時(shí)刻.以對(duì)k周期內(nèi)從AUV定位為例,協(xié)同定位算法如下.

圖3 信息順序到達(dá)時(shí)序Fig.3 Information sequential arrival Timing

根據(jù)水聲傳播時(shí)延公式,相對(duì)距離量測(cè)值為

使用tk時(shí)刻傳感器量測(cè)更新系統(tǒng)狀態(tài),利用從AUV的tk時(shí)刻狀態(tài)預(yù)測(cè)和主AUV的t1時(shí)刻狀態(tài)估計(jì)(此值由時(shí)刻從AUV接收到的通信數(shù)據(jù)包所提供)對(duì)量測(cè)值的估計(jì)為

從式(20)–(21)分別得出從AUV的運(yùn)動(dòng)狀態(tài)估計(jì)值和對(duì)估計(jì)結(jié)果的評(píng)價(jià).對(duì)于此類(lèi)有信息延遲但沒(méi)有出序的情景,本文提出的序貫處理算法可以在延遲時(shí)間可預(yù)計(jì)的條件下得到最優(yōu)的估計(jì)精度.

5 信息出序到達(dá)協(xié)同定位算法

如圖4所示,相鄰時(shí)刻主AUV發(fā)送的通信數(shù)據(jù)包未按照順序到達(dá)從AUV的濾波器.第k和k+1個(gè)周期中主AUV在t1和t2時(shí)刻所發(fā)送的脈沖串按照發(fā)送順序依次在tk和tk+1時(shí)刻到達(dá)從AUV;含有主AUV在t1和t2時(shí)刻位置狀態(tài)的通信數(shù)據(jù)包分別于時(shí)刻到達(dá)從AUV的濾波器,時(shí)刻早于時(shí)刻,通信數(shù)據(jù)包出序到達(dá).

5.1 信息出序問(wèn)題分析

為了能夠維持較高的定位精度,協(xié)同定位要求從AUV在通信數(shù)據(jù)包可用時(shí)立即利用主AUV運(yùn)動(dòng)狀態(tài)數(shù)據(jù)和量測(cè),完成對(duì)從AUV位姿狀態(tài)估計(jì)的更新.如圖4,就是要估計(jì)tk和tk+1時(shí)刻從AUV的運(yùn)動(dòng)狀態(tài).按圖4中信息發(fā)生出序到達(dá),第k+1個(gè)周期的協(xié)同定位所需數(shù)據(jù)在時(shí)刻先完備,先對(duì)tk+1時(shí)刻從AUV的運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì);而在時(shí)刻第k個(gè)周期的協(xié)同定位所需數(shù)據(jù)才完備,但此時(shí)從AUV狀態(tài)已經(jīng)更新至相對(duì)于tk時(shí)刻更晚的tk+1時(shí)刻,如果使用常規(guī)的協(xié)同定位算法,將無(wú)法再利用tk時(shí)刻的數(shù)據(jù)對(duì)從AUV的運(yùn)動(dòng)狀態(tài)進(jìn)行更新,致使這部分信息無(wú)益于對(duì)定位精度的改善.

理論上,出序的情景有3種類(lèi)型:1)脈沖串順序到達(dá),通信數(shù)據(jù)包出序到達(dá),如圖4;2)脈沖串出序到達(dá),通信數(shù)據(jù)包順序到達(dá),如圖5;3)脈沖串和通信數(shù)據(jù)包都出序到達(dá),見(jiàn)圖6.情景2與情景1相類(lèi)似,都屬于量測(cè)后形成而數(shù)據(jù)先完備的情況;對(duì)于類(lèi)似情景3的類(lèi)型,由于先形成的量測(cè)對(duì)應(yīng)的數(shù)據(jù)也先完備,可以應(yīng)用第4節(jié)中算法予以應(yīng)對(duì).

圖4 信息出序到達(dá)時(shí)序1Fig.4 Information out-of-sequence arrival Timing 1

圖5 信息出序到達(dá)時(shí)序2Fig.5 Information out-of-sequence arrival Timing 2

圖6 信息出序到達(dá)時(shí)序3Fig.6 Information out-of-sequence arrival Timing 3

相比于脈沖串,通信數(shù)據(jù)包信息量大且傳輸過(guò)程復(fù)雜,通信延遲時(shí)間長(zhǎng),更容易出序,以圖4所展示的情景為背景進(jìn)行分析和提出解決方案將更有現(xiàn)實(shí)意義.信息出序?qū)е螺^晚時(shí)刻產(chǎn)生的信息反而先于較早時(shí)刻產(chǎn)生的信息被使用,而當(dāng)較早的信息可以被使用時(shí),從AUV的狀態(tài)估計(jì)值已經(jīng)被更新至較晚時(shí)刻.出序信息處理的實(shí)質(zhì)就是將這些較早產(chǎn)生的信息有效加以利用來(lái)提高定位精度.在接下來(lái)的工作中,本文將試圖建立一種全新的協(xié)同定位算法,其基本思路為:不存儲(chǔ)任何量測(cè)和濾波中間結(jié)果,在出序量測(cè)可用時(shí),將其用于在最新?tīng)顟B(tài)估計(jì)時(shí)刻對(duì)狀態(tài)估計(jì)值進(jìn)行再更新,優(yōu)化協(xié)同定位精度,從而達(dá)到兼顧系統(tǒng)輸出實(shí)時(shí)性和對(duì)信息的無(wú)損利用以最大限度地提高定位精度的目的.

5.2 AUV運(yùn)動(dòng)模型的線(xiàn)性化

AUV運(yùn)動(dòng)過(guò)程中其本體傳感器測(cè)量的不準(zhǔn)確性反映了過(guò)程噪聲影響.為適應(yīng)后續(xù)算法推導(dǎo)的需求,在通信延遲周期內(nèi),將AUV運(yùn)動(dòng)模型進(jìn)行線(xiàn)性化表示,顯式化明確過(guò)程噪聲.

AUV的航位推算模型如式(6),則有

其中:Ik為單位陣;Vmk和?mk為AUV實(shí)際運(yùn)動(dòng)速度和偏航角,可用延遲周期內(nèi)多次測(cè)量的平均值近似,當(dāng)AUV穩(wěn)定航行時(shí),短時(shí)間內(nèi)可認(rèn)為其為固定值.則設(shè)

5.3 出序問(wèn)題的解決方案

假設(shè)從AUV運(yùn)動(dòng)的初始時(shí)刻、過(guò)程噪聲和量測(cè)噪聲之間互不相關(guān).為了敘述簡(jiǎn)便和后續(xù)推導(dǎo)過(guò)程的清楚,將除了出序量測(cè)外的所有量測(cè)形成時(shí)間點(diǎn)(即各時(shí)刻脈沖串到達(dá)時(shí)刻)設(shè)為t1,t2,···,tk?1,tk,并假設(shè)形成于時(shí)刻td的量測(cè)zd為l步滯后,即

5.4 信息出序到達(dá)的協(xié)同定位算法

5.5 “n主帶m從”模式信息出序問(wèn)題的討論

在多主多從的“n主帶m從”模式下,對(duì)于m從中的每一個(gè)個(gè)體來(lái)說(shuō),只需要接收每一個(gè)主AUV發(fā)送的信息并將其用于自身的狀態(tài)更新即可,不需要與m中其他個(gè)體交互.所以,從信息的角度看,“n主帶m從”模式實(shí)質(zhì)為若干個(gè)“n主帶1從”模式的不相關(guān)組合.對(duì)于“n主帶1從”模式,由于一般情況下n個(gè)主AUV相距有一段距離,與從AUV的距離差別較大,系統(tǒng)設(shè)置的采樣間隔較大,故而多考慮信息在一個(gè)周期內(nèi)出序的情景.當(dāng)某一主AUV發(fā)送的數(shù)據(jù)出序時(shí),按照數(shù)據(jù)后完備后處理的原則,依照第5.4節(jié)算法,利用其在最新?tīng)顟B(tài)估計(jì)時(shí)刻對(duì)狀態(tài)估計(jì)值進(jìn)行再更新.

6 仿真分析

為了更好地比較和分析,設(shè)定主從AUV距離一近一遠(yuǎn)兩種情景,圖7–8顯示了二者沿著各自方向從左向右運(yùn)動(dòng)的真實(shí)軌跡關(guān)系.設(shè)定主AUV擁有較高的自定位精度,其本體傳感器噪聲較小:

從AUV的本體傳感器噪聲較大:

二者運(yùn)動(dòng)速度均為10 kn.主AUV定時(shí)發(fā)送聲信號(hào)(含脈沖串和狀態(tài)數(shù)據(jù)包)給從AUV,設(shè)定脈沖串發(fā)送固定延時(shí)為0.1 s.設(shè)定水聲傳播速度c=1500 m/s.為了表述簡(jiǎn)單,以下將信息順序到達(dá)協(xié)同定位算法簡(jiǎn)稱(chēng)為延遲算法1.0,將融入信息出序到達(dá)處理方法的協(xié)同定位算法簡(jiǎn)稱(chēng)延遲算法2.0.為了說(shuō)明仿真結(jié)果的科學(xué)有效性,進(jìn)行了1000次Monte Carlo仿真.

圖7 兩AUV的實(shí)際航行線(xiàn)路1Fig.7 The actual navigation route 1 of two AUVs

圖8 兩AUV的實(shí)際航行線(xiàn)路2Fig.8 The actual navigation route 2 of two AUVs

6.1 信息順序到達(dá)情景的仿真分析

類(lèi)似圖3情景,設(shè)定量測(cè)和通信數(shù)據(jù)包能夠按順序到達(dá)從AUV.采用3種方法估計(jì)從AUV的航行軌跡:①航位推算,即只采用本體傳感器和運(yùn)動(dòng)模型來(lái)估計(jì)位置;②整周期濾波算法,即利用本周期量測(cè)和數(shù)據(jù)包在整周期時(shí)間點(diǎn)更新從AUV狀態(tài);③延遲算法1.0.

針對(duì)圖7情景,設(shè)定采樣周期為2 s.設(shè)置3種聲納傳感器探測(cè)噪聲水平如下:①σr=5 m;②σr=10 m;③σr=15 m.各噪聲水平下各算法的估計(jì)結(jié)果的均方根誤差(root mean square error,RMSE)比較如表1.相應(yīng)的比較估計(jì)誤差均值和方差如圖9,其中柱狀圖為誤差均值,線(xiàn)棒為估計(jì)誤差的方差.在①的噪聲水平下,各算法從AUV的位置估計(jì)誤差比較如圖10.

表1 主從AUV近距離情景不同噪測(cè)探聲水平下各算法估計(jì)結(jié)果的RMSE(m)Table 1 The RMSE of the estimation results of each algorithm at different tetecdion noise levels in the short-distance scene of the master-slaver AUV(m)

圖9 主從AUV近距離情景不同測(cè)量噪聲水平下的誤差均值和方差比較Fig.9 Comparison of the Mean and Variance of errors at different measurement noise levels in the short-distance scene of the master-slaver AUV

圖10 主從AUV近距離情景噪聲水平①各算法從AUV的位置估計(jì)誤差比較Fig.10 Comparison of location estimation errors in each algorithm at the noise Level ①in the short-distance scene of the master-slaver AUV

針對(duì)圖8情景,設(shè)定采樣周期為4 s.設(shè)置3種聲納傳感器探測(cè)噪聲水平如下:①σr=10 m;②σr=15 m;③σr=20 m.估計(jì)結(jié)果的RMSE比較如表2.相應(yīng)的比較估計(jì)誤差均值和方差如圖11.在①的噪聲水平下,各算法從AUV的位置估計(jì)誤差比較如圖12.

表2 主從AUV遠(yuǎn)距離情景不同探測(cè)噪聲水平下各算法估計(jì)結(jié)果的RMSE(m)Table 2 The RMSE of the estimation results of each algorithm at different detection noise levels in the long-distance scene of the master-slaver AUV(m)

圖11 主從AUV遠(yuǎn)距離情景不同測(cè)量噪聲水平下的誤差均值和方差比較Fig.11 Comparison of the mean and variance of errors at different measurement noise levels in the long-distance scene of the master-slaver AUV

圖12 主從AUV遠(yuǎn)距離情景噪聲水平①各算法從AUV的位置估計(jì)誤差比較Fig.12 Comparison of location estimation errors of the slaver AUV in each algorithm at the noise Level ①in the long-distance scene of the master-slaver AUV

表1–2、圖9和圖11都顯示了兩種不同的距離尺度下,不同探測(cè)噪聲水平下3 種估計(jì)方法對(duì)從AUV位置估計(jì)效果,以EKF為核心算法的協(xié)同定位算法對(duì)于誤差的抑制能力遠(yuǎn)優(yōu)于航位推算方法,本文提出的延遲算法1.0采用序貫量測(cè)處理的方式,在不提高算法復(fù)雜度的基礎(chǔ)上,相比于常規(guī)整周期濾波的協(xié)同定位方式,能更進(jìn)一步提升定位精度.相比于圖10,圖12所反映的是大距離尺度情景各算法的估計(jì)效果,在主從AUV距離較遠(yuǎn)時(shí),量測(cè)延遲時(shí)間會(huì)更久,相比其他方法,本文所提出的方法會(huì)更有效.無(wú)論是圖10或是圖12中,延遲算法1.0與經(jīng)典的整周期濾波算法的估計(jì)誤差曲線(xiàn)變化趨勢(shì)相同,具有較好的一致性.

6.2 信息出序到達(dá)情景的仿真分析

設(shè)定通信數(shù)據(jù)包可能出序到達(dá),出序時(shí)類(lèi)似圖4情景.采用4種方法估計(jì)從AUV的航行軌跡:①航位推算;②量測(cè)丟棄算法;③數(shù)據(jù)緩存算法(由于重新濾波算法的最終濾波精度與本算法相同,估計(jì)結(jié)果等價(jià),則在本實(shí)驗(yàn)中不做重新濾波算法估計(jì)過(guò)程仿真.);④延遲算法2.0.

針對(duì)圖7情景,設(shè)置3種聲納傳感器探測(cè)噪聲水平如 下:①σr=5 m;②σr=10 m;③σr=15 m,采樣周期為2 s.為了更清晰地展示本文算法效果,假設(shè)主從AUV間每2次通信中有一次通信數(shù)據(jù)出序到達(dá)的現(xiàn)象發(fā)生,即設(shè)定2,4,6,···等周期內(nèi)的通信數(shù)據(jù)遲于相鄰的3,5,7,···等周期內(nèi)的通信數(shù)據(jù)到達(dá),并可在3,5,7,···等周期內(nèi)到達(dá),可以利用其進(jìn)一步更新從AUV在3,5,7,···等周期內(nèi)的運(yùn)動(dòng)狀態(tài).估計(jì)結(jié)果的RMSE比較如表3.相應(yīng)的比較估計(jì)誤差均值和方差如圖13.在③的噪聲水平下,各算法從AUV的位置估計(jì)誤差比較如圖14.

表3 不同探測(cè)噪聲水平下各算法估計(jì)結(jié)果的RMSE(m)Table 3 The RMSE of the estimation results of each algorithm at different detection noise levels(m)

表3和圖13顯示了小距離尺度3種不同探測(cè)噪聲水平下4種估計(jì)方法對(duì)從AUV位置估計(jì)的效果.數(shù)據(jù)緩存算法的估計(jì)精度最高,本文提出的延遲算法2.0優(yōu)于量測(cè)丟棄算法和航位推算.數(shù)據(jù)緩存算法(重新濾波算法)雖然有較好的抑制誤差能力,但是需要等待數(shù)據(jù)完備后回到先前時(shí)刻對(duì)估計(jì)狀態(tài)進(jìn)行更新,犧牲了實(shí)時(shí)性,且需要存儲(chǔ)空間緩存數(shù)據(jù);延遲算法2.0屬于直接更新法,雖然在出序時(shí)刻無(wú)法利用外部量測(cè)修正航跡而導(dǎo)致定位精度有所下降,但可在出序量測(cè)可用時(shí),在最近狀態(tài)更新點(diǎn)利用其迅速改善定位精度,有效收斂定位累積誤差,達(dá)到為快速運(yùn)動(dòng)的AUV實(shí)時(shí)提供高精度定位信息支持的目的.圖14比較了小距離尺度下4種算法的位置估計(jì)誤差,本文所提出的算法估計(jì)精度介于數(shù)據(jù)緩存算法和量測(cè)丟棄算法之間,說(shuō)明了通過(guò)設(shè)計(jì)算法,將發(fā)生通信延遲時(shí)的信息加以利用而不是丟棄對(duì)提升定位精度具有重要意義.

圖13 不同測(cè)量噪聲水平下的誤差均值和方差比較Fig.13 Comparison of the mean and variance of errors at different measurement noise levels

圖14 噪聲水平③各算法從AUV的位置估計(jì)誤差比較Fig.14 Comparison of location estimation errors of the slaver AUV in each algorithm at the noise Level ③

表4 不同出序情景下各算法估計(jì)結(jié)果的RMSE(m)Table 4 The RMSE of estimation results of each algorithm in different out-of-sequence scenarios(m)

針對(duì)圖8情景,設(shè)定聲納傳感器探測(cè)噪聲σr=20 m,采樣周期為4 s.設(shè)置3種信息出序情景:①每2次通信中有一次信息出序;②每3次通信中有一次信息出序;③每4次通信中有一次信息出序.估計(jì)結(jié)果的RMSE比較如表4.相應(yīng)的比較估計(jì)誤差均值和方差如圖15.3種出序情景各算法從AUV的位置估計(jì)誤差比較如圖16–18.

圖15 不同出序情景下的誤差均值和方差比較Fig.15 Comparison of the mean and variance of errors in different out-of-sequence scenes

圖16 情景①各算法從AUV的位置估計(jì)誤差比較Fig.16 Comparison of location estimation errors of the slaver AUV in the Scene ①

圖17 情景②各算法從AUV的位置估計(jì)誤差比較Fig.17 Comparison of location estimation errors of the slaver AUV in the Scene ②

圖18 情景③各算法從AUV的位置估計(jì)誤差比較Fig.18 Comparison of location estimation errors of the slaver AUV in the Scene ③

表4和圖15顯示了大距離尺度相同水平探測(cè)噪聲3種信息出序情景條件下4種估計(jì)方法對(duì)從AUV位置估計(jì)的效果.比較每一種出序情景對(duì)應(yīng)延遲算法2.0和量測(cè)丟棄算法的RMSE和誤差均值可以看出,隨著出序頻次的降低,兩種算法的RMSE和誤差均值差值越來(lái)越小,這一點(diǎn)與圖16–18所顯示的曲線(xiàn)差別度一致.比較圖16–18中的各算法的估計(jì)誤差曲線(xiàn),延遲算法2.0的估計(jì)誤差曲線(xiàn)始終介于量測(cè)丟棄和數(shù)據(jù)緩存兩種經(jīng)典算法的曲線(xiàn)之間,且在出序信息加以利用后,延遲算法2.0估計(jì)誤差曲線(xiàn)迅速回歸于數(shù)據(jù)緩存算法估計(jì)誤差曲線(xiàn),曲線(xiàn)的總體變化趨勢(shì)相同,具有較好的一致性.為了更好地說(shuō)明本文算法的有效性,給出與圖16相對(duì)應(yīng)的,在各延遲信息到達(dá)時(shí)刻,融入延遲信息前后的從AUV的位置估計(jì)誤差比較,如圖19所示.

圖19 融入延遲信息前后的位置估計(jì)誤差比較Fig.19 Comparison of estimation errors before and after integrating delay information

圖19中粉色曲線(xiàn)對(duì)應(yīng)著始終放棄使用延遲量測(cè)更新從AUV運(yùn)動(dòng)狀態(tài)的位置估計(jì)誤差,綠色離散點(diǎn)標(biāo)示只利用當(dāng)前時(shí)刻量測(cè)更新從AUV運(yùn)動(dòng)狀態(tài)(按算法程序,延遲算法2.0先利用本時(shí)刻量測(cè)信息)的估計(jì)誤差,而對(duì)應(yīng)的黑色點(diǎn)標(biāo)示將上一時(shí)刻所產(chǎn)生的、于本時(shí)刻才到達(dá)的延遲量測(cè)信息進(jìn)一步用于狀態(tài)更新后(延遲算法2.0后利用出序信息)的估計(jì)誤差.表5與圖19對(duì)應(yīng),給出了各延遲量測(cè)可用時(shí)刻狀態(tài)估計(jì)過(guò)程中融入延遲信息前后的估計(jì)誤差值.從表5中可以看到,在每一時(shí)刻,融合了延遲量測(cè)的延遲算法2.0都進(jìn)一步減小了估計(jì)誤差.以上說(shuō)明了在出序信息可用時(shí),延遲算法2.0可以迅速地以一定幅度提升協(xié)同定位的精度.

表5 融入延遲信息前后估計(jì)誤差值(m)Table 5 Estimation errors before and after integrating delay information(m)

7 結(jié)論

如何減小通信延遲對(duì)協(xié)同定位過(guò)程中定位精度和實(shí)時(shí)性的不利影響是一項(xiàng)重要又富有挑戰(zhàn)性的工作.本文分析了水聲信息傳輸?shù)膹?fù)雜過(guò)程,建立了水聲探測(cè)和水聲通信的時(shí)延模型,對(duì)主從式協(xié)同結(jié)構(gòu)的信息順序到達(dá)和出序到達(dá)情景做出了描述和分析,設(shè)計(jì)了2種適用于處理延遲信息的多AUV協(xié)同定位算法,并進(jìn)行了仿真實(shí)驗(yàn)分析.本文驗(yàn)證并得出了如下結(jié)論:

1) 對(duì)于有延遲但沒(méi)有出序的順序信息,本文提出了一種將信息序貫處理的信息順序到達(dá)協(xié)同定位算法,相比于航位推算、整周期濾波等傳統(tǒng)方法,可以較明顯地提升定位精度.

2) 本文的主要?jiǎng)?chuàng)新點(diǎn)和貢獻(xiàn)是針對(duì)信息出序情景,提出了一種直接使用出序信息優(yōu)化當(dāng)前時(shí)刻狀態(tài)估計(jì)值的方法.相比于量測(cè)丟棄等方法,由于沒(méi)有損失信息而具有更高的定位精度;相比于數(shù)據(jù)緩存等算法,不需要緩存數(shù)據(jù),濾波器輸出無(wú)滯后,實(shí)時(shí)性好.

3) 本文算法與經(jīng)典協(xié)同定位算法具有相同的定位估計(jì)一致性,能夠有效抑制誤差發(fā)散速度.在具有較好的理論意義的同時(shí),也具備工程實(shí)踐參考價(jià)值.