張新宇, 王 燕,2,3, 鄒 男,2,3, 梁國龍,2,3, 付 進,2,3

(1. 哈爾濱工程大學(xué)水聲技術(shù)全國重點實驗室, 哈爾濱 150001;2. 海洋信息獲取與安全工信部重點實驗室(哈爾濱工程大學(xué)),工業(yè)和信息化部,哈爾濱 150001;3. 哈爾濱工程大學(xué)水聲工程學(xué)院,哈爾濱 150001)

0 引言

當(dāng)今,自主水下航行器(autonomous underwater vehicle,AUV)在海洋環(huán)境監(jiān)測、海底繪圖、海底電纜鋪設(shè)等民用領(lǐng)域中發(fā)揮著重要作用,同時在雷區(qū)探測和水下目標(biāo)打擊等軍事領(lǐng)域也備受關(guān)注,而精確導(dǎo)航是AUV完成這些任務(wù)的先決條件[1-4]。由于聲信號在水下的衰減遠小于電磁信號,聲學(xué)導(dǎo)航方式被廣泛使用。

AUV的聲學(xué)導(dǎo)航方法可以近似分為三類:長基線(long baseline,LBL)、超短基線(ultra short baseline,USBL)以及單信標(biāo)導(dǎo)航。LBL由三個及以上的聲信標(biāo)構(gòu)成,信標(biāo)之間的基線長度在千米量級,AUV接收多個聲信標(biāo)的測距信號并估計信號傳播時延,進而解算AUV自身位置。針對AUV運動所導(dǎo)致的時延測量時空不匹配問題,陳偉等[5]提出了一種聯(lián)合使用濾波和平滑的實時LBL導(dǎo)航算法。針對時鐘異步情況下導(dǎo)航精度低這一問題,Bartista[6]提出了一種將時鐘偏差增補為狀態(tài)量的新型LBL濾波算法。Fu等[7]構(gòu)建了考慮水聲信道影響的LBL導(dǎo)航精度預(yù)測模型。針對水下體目標(biāo)幾何尺度大和運動姿態(tài)未知等因素導(dǎo)致傳統(tǒng)LBL定位模型失配這一問題,孫大軍等[8]通過引入體目標(biāo)姿態(tài)、位置聯(lián)合估計,消除了傳統(tǒng)質(zhì)點算法的模型誤差,理論分析及數(shù)值仿真結(jié)果表明,在體目標(biāo)半徑5 m、測距精度0.2 m的條件下,較傳統(tǒng)LBL定位方法,所提方法將垂直定位精度從32 m提升至0.5 m,實現(xiàn)亞米級精度定位。針對傳統(tǒng)LBL水聲定位模型在以飛機黑匣子等周期偏移聲信標(biāo)為目標(biāo)時定位精度顯著下降的問題,孫思博等[9]提出了一種不依賴于信號周期信息的新型LBL定位模型,所提出新模型的仿真試驗及湖上試驗的定位誤差分別為3.14 m和1.19 m。LBL導(dǎo)航精度高且作用范圍大,但布放和回收多個聲信標(biāo)使得成本較高。

USBL的基線長度一般小于1m,通過測距及測向?qū)崿F(xiàn)AUV位置的解算。針對時延野值點所導(dǎo)致的噪聲非高斯這一問題,Xu等[10]提出了一種適用于USBL和捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system,SINS)的組合導(dǎo)航系統(tǒng)的魯棒濾波算法。為了改善原始測量數(shù)據(jù)的質(zhì)量,Liu等[11]使用在線支持向量回歸(online support vector regression,OSVR)來消除測量野點和填補間斷點。針對海水聲速不均勻?qū)е侣暰€彎曲這一問題,羅宇等[12]提出了一種基于二次項擬合的聲線跟蹤算法,顯著提高了USBL的定位精度。針對USBL安裝偏差校準(zhǔn)問題,趙俊波等[13]提出了一種帶約束最小二乘校準(zhǔn)算法,并進一步給出了考慮聲速測量誤差影響的校準(zhǔn)算法。針對基于單模型卡爾曼濾波無法全程適應(yīng)水下目標(biāo)的所有運動狀態(tài)的問題,張曉飛等[14]采用交互式多模型卡爾曼濾波方法處理AUV的USBL跟蹤數(shù)據(jù),運動模型之間通過概率矩陣轉(zhuǎn)移來增強運動狀態(tài)的適應(yīng)性。針對復(fù)雜水下聲場環(huán)境下高精度、長航時導(dǎo)航與定位的需求,王健等[15]提出了一種融合USBL、SINS以及多普勒速度計程儀(doppler velocity log,DVL)多源信息的組合定位算法。針對SINS/USBL一體化組合樣機系統(tǒng)需要提前進行精確標(biāo)定的問題,張濤等[16]提出了基于量測信息濾波估計的誤差標(biāo)定方法,精確校正后無需重復(fù)標(biāo)定。USBL體積小但對安裝的精度要求較高,使用前需要花費大量的精力進行陣元校準(zhǔn)。

單信標(biāo)導(dǎo)航是近20年發(fā)展起來的一種新型導(dǎo)航方式,它不僅需要測量AUV與信標(biāo)之間的距離信息,還需利用AUV自身搭載的傳感器獲取AUV的狀態(tài)信息,如深度和速度等。單信標(biāo)導(dǎo)航是傳統(tǒng)水聲導(dǎo)航系統(tǒng)組合化和簡約化的結(jié)果。簡約化是因為它只需要布放一個聲信標(biāo),提高了使用便捷性和作業(yè)效率;組合化是因為它將聲學(xué)測距定位設(shè)備與載體運動傳感器組合使用。De Palma等[17]對單信標(biāo)導(dǎo)航雙積分系統(tǒng)的局部可觀測性和全局可觀測性進行了分析。Indiveri等[18]討論了單信標(biāo)導(dǎo)航在3D情況下的可觀測性問題。針對有效聲速未知的問題,Qin等[19]將有效聲速視為數(shù)字特征未知的隨機變量并提出了一種基于變分貝葉斯(varia-tional bayesian,VB)近似的自適應(yīng)單信標(biāo)導(dǎo)航方法。針對時鐘異步情況下導(dǎo)航精度差的問題,Sun等[20]提出了一種聯(lián)合利用時延差和相位差的單信標(biāo)異步導(dǎo)航算法。陳允鋒等[21]提出了融合遺忘因子和多新息的改進擴展卡爾曼濾波算法(extended Kalman filter,EKF),改進算法的定位準(zhǔn)確性和穩(wěn)定性顯著提升,相比于標(biāo)準(zhǔn)EKF,最大定位誤差減少了2.352 2 m,定位誤差平均值減少了0.856 4 m。針對信標(biāo)稀疏或者信標(biāo)通信信息缺失的情況下定位精度低這一問題,孫大軍等[22]提出了基于概率圖模型的單信標(biāo)定位算法,通過聯(lián)合目標(biāo)時域上其他位置時刻的所有量測信息,實現(xiàn)目標(biāo)定位,測試結(jié)果表明其平均定位精度可達到1.203 5 m。

上述的研究證明了單信標(biāo)導(dǎo)航的可行性和有效性,為了進一步提高單信標(biāo)導(dǎo)航精度,本文提出了單信標(biāo)導(dǎo)航的航路規(guī)劃方案。本文首先建立了單信標(biāo)導(dǎo)航模型,基于泰勒級數(shù)展開推導(dǎo)了水平位置精度因子的表達式,分析了聲信標(biāo)和AUV之間的相對幾何位置關(guān)系對導(dǎo)航精度的影響,在此基礎(chǔ)上提出了單信標(biāo)導(dǎo)航的航路規(guī)劃方案。此外,為了將AUV引導(dǎo)進入所預(yù)設(shè)的最優(yōu)航路,本文基于可觀測度分析結(jié)果設(shè)計了最優(yōu)航路接近軌跡,使得AUV高效率且高精度地進入預(yù)設(shè)的最優(yōu)航路。

1 導(dǎo)航模型

如圖1所示,一個典型的單信標(biāo)導(dǎo)航系統(tǒng)由一個靜止的聲信標(biāo)和一個運動的AUV組成。聲信標(biāo)浮于海面且周期發(fā)射測距聲信號,AUV接收聲信號并估計信號傳播時延,進而進行自身位置的解算。以聲信標(biāo)為原點建立三維直角坐標(biāo)系,x軸指向正東方向,y軸指向正北方向,z軸豎直向上。在該系統(tǒng)中,可以獲得如下的信息:

1)AUV分別在A和B兩點接收聲信標(biāo)發(fā)射的測距聲信號,相應(yīng)的信號傳播時延分別為tA和tB;

2)AUV搭載慣性導(dǎo)航系統(tǒng),進而可以獲得從A點到B點的虛擬基線矢量LAB=[xAB,yAB]T;

3)聲速c可以通過AUV搭載的聲速儀進行測量;

4)AUV搭載了壓力傳感器,可以獲得自身的入水深度zA和zB;

5)聲信標(biāo)搭載了全球定位系統(tǒng)(global positioning system,GPS),可以實時獲取自身在大地坐標(biāo)系下的位置信息。

設(shè)O、A和B三點的坐標(biāo)分別為[xO,yO,zO]T、[xA,yA,zA]T和[xB,yB,zB]T,利用上述信息可以在x-y-z坐標(biāo)系建立如下的導(dǎo)航方程組以解算B點的坐標(biāo)

(1)

(a) 正視圖

(b) 俯視圖圖1 導(dǎo)航模型Fig.1 Geometrical model for a typical single beacon navigation system

2 導(dǎo)航精度

在實際的導(dǎo)航環(huán)境中,由于時延測量誤差、聲信標(biāo)位置誤差、聲速測量誤差、深度測量誤差以及慣導(dǎo)誤差的存在,式(1)所解算的B點坐標(biāo)也存在誤差。本文用水平位置精度因子(horizontal dilution of precision,HDOP)來描述單信標(biāo)導(dǎo)航精度,HDOP越小,導(dǎo)航精度越高,HDOP的計算表達式如下

(2)

上式中,Δx和Δy分別表示xB和yB的估計誤差,E(*)表示期望運算。接下來基于泰勒級數(shù)展開推導(dǎo)IHDOP的近似解析解。

對式(1)等號左側(cè)部分在各變量真值點處進行泰勒級數(shù)展開并忽略二階及以上的高階項可得

(3)

上式中,ΔxO、ΔyO和ΔzO表示聲信標(biāo)位置誤差,ΔtA和ΔtB表示時延測量誤差,Δc表示聲速測量誤差,ΔzA和ΔzB表示AUV入水深度測量誤差,Δ|LAB|表示慣導(dǎo)測量誤差。為了便于觀察,將上式改寫成矩陣形式

(4)

其中,Μ、ΜO、Μt、Μc、Μz和ΜINS的細節(jié)如下

(5)

(6)

從式(4)可得

(7)

假設(shè)各測量誤差皆服從均值為零的高斯分布且相互獨立,令上式乘以該式的轉(zhuǎn)置并取期望可得

(8)

其中

利用式(2)和(8)可得

(9)

上式中,IHDOP,*(*∈{t,O,c,z,INS})為僅考慮某一誤差δ*時的水平位置精度因子,tr(*)表示矩陣的跡運算。

接下來給出HDOP的仿真結(jié)果,仿真參數(shù)如表1所示,圖2(a)給出了在LAB=[50,0]Tm且B點處于不同位置時HDOP的計算結(jié)果,圖2(b)給出了當(dāng)A點固定于[0,200,-60]Tm且B點處于不同位置時HDOP的計算結(jié)果,圖中O′點是聲信標(biāo)O點在水平面上的投影。從圖2中可以得出以下結(jié)論:1)保持LAB和夾角∠BO′X不變,B點和聲信標(biāo)之間的水平距離越遠,導(dǎo)航誤差越大,當(dāng)圖2(a)中B點從B1移動至B2,HDOP隨之從5.68 m增大至13.24 m;2)當(dāng)A和B兩點近似與O′點共線時,導(dǎo)航誤差不可接受(如圖2(b)中的點B3);3)當(dāng)A、B和O′三點所組成的夾角∠BO′A近似為直角時,導(dǎo)航誤差較小(如圖2(b)中的點B4)。

(a) LAB=[50,0]T m

(b) xA=[0,200,-60]T m圖2 B點處于不同位置時的HDOPFig.2 Distribution of HDOP with the variation of B’s coordinate

表1 仿真參數(shù)

接下來通過仿真分析單信標(biāo)導(dǎo)航系統(tǒng)對各測量誤差的魯棒性。A和B兩點分別固定于[150,250,-60]Tm和[200,250,-60]Tm。當(dāng)研究HDOP對某一種測量誤差的魯棒性時,將其他測量誤差設(shè)置為0。計算結(jié)果如圖3所示,可以看出:1)單信標(biāo)導(dǎo)航系統(tǒng)對時延測量誤差(0～5 ms)、聲信標(biāo)位置誤差(0～5 m)和深度測量誤差(0～5 m)的魯棒性差,時延測量誤差每增大0.1 ms,HDOP隨之增大1.6 m;2)單信標(biāo)導(dǎo)航系統(tǒng)對聲速測量誤差(0～5 m/s)和慣導(dǎo)測量誤差(0～5 ‰)的魯棒性強;3)為了獲得更高精度的AUV位置,應(yīng)該注重降低時延測量誤差、聲信標(biāo)位置誤差以及深度測量誤差。

圖3 魯棒性分析Fig.3 Analysis of the robustness of the system to several kinds of errors

3 航路規(guī)劃

(10)

(11)

(a) 俯視圖

(b) 側(cè)視圖圖4 圖1模型的簡化Fig.4 Simplified geometric configuration

在此基礎(chǔ)上,式(9)中的I*的化簡結(jié)果如下

(12)

從上式可以看出,HDOP的大小依賴于圓弧半徑ρ和夾角∠AO′B,接下來分別分析ρ和β對HDOP大小的影響。首先,保持夾角β不變,可以得出如下的結(jié)論:1)IHDOP,t、IHDOP,O和IHDOP,z隨著半徑ρ的增大而增大;2)IHDOP,INS隨著ρ的增加而減小;3)當(dāng)ρ遠大于z時,IHDOP,c是一個關(guān)于ρ的凹函數(shù),存在一個極小值;4)綜合考慮五種測量誤差的影響且各測量誤差取值合理時,IHDOP是一個關(guān)于半徑ρ的凹函數(shù),存在一個半徑取值使得IHDOP最小。

保持半徑ρ不變,分析夾角β對HDOP大小的影響,可以得出如下的結(jié)論:1)IHDOP,t是一個關(guān)于β的凹函數(shù),當(dāng)β=45°(或∠AO′B=90°)時,It取值最小;2)IHDOP,O、IHDOP,c、IHDOP,z和IHDOP,INS隨著β的增大而增大;3)綜合考慮五種測量誤差的影響且各測量誤差取值合理時,IHDOP是一個關(guān)于β的凹函數(shù),存在一個夾角取值使得IHDOP最小。

從上述的分析可以看出存在某一最優(yōu)的半徑和夾角組合(ρopt,βopt)使得HDOP最小,單信標(biāo)導(dǎo)航精度最高

(ρopt,βopt)=argminIHDOP(ρ,β)

(13)

接下來通過一個仿真來證明(ρopt,βopt)的存在,仿真參數(shù)見表1。圖5(a)給出了在半徑和夾角取不同值時HDOP的計算結(jié)果,可以看出當(dāng)ρ=276.5 m且∠AO′B=87°時HDOP取得最小值,圖5(b)給出了當(dāng)∠AO′B=87°時HDOP隨半徑ρ的變化曲線,圖5(c)給出了當(dāng)ρ=276.5 m時HDOP隨夾角∠AO′B的變化曲線。

(a) HDOP在ρ-∠AO′B域的分布

(b) ∠AO′B=87°

(c) ρ=276.5 m圖5 未考慮信號傳播損失時HDOP在ρ-∠AO′B域的取值分布Fig.5 Distribution of HDOP on ρ-∠AO′B domain without considering TL

圖5的仿真是在各測量誤差的標(biāo)準(zhǔn)差取值固定的條件下進行的,但在實際的水聲環(huán)境中,隨著AUV位置發(fā)生改變,信噪比發(fā)生改變,時延估計誤差的標(biāo)準(zhǔn)差也隨之改變。因此,為了更準(zhǔn)確地計算水平位置精度因子,需要考慮信號傳播損失(transmission loss,TL)的影響。某一位置的信噪比JSNR及時延測量誤差標(biāo)準(zhǔn)差δt可通過下式計算[23]

(14)

上式中,JSL表示聲源級(sound level,SL),JTL表示傳播損失,其包含兩部分,一是由球面擴展導(dǎo)致的傳播損失,二是由海水聲吸收導(dǎo)致的傳播損失,α表示海水的聲吸收系數(shù),r表示AUV和聲信標(biāo)之間的斜距;JNL表示噪聲級(noise level,NL);m是一個固定系數(shù)。圖6(a)是在JSL、α、JNL和m分別為220 dB、0.003 dB/m、120 dB和0.03的情況下δt隨ρ的變化曲線,可以看出δt隨著半徑ρ的增大而增大。使用圖6(a)和表1重新進行HDOP的計算,結(jié)果如圖6(b)所示,HDOP在ρ=186.7 m且∠AO′B=83.9°時取得最小值,圖6(c)給出了當(dāng)∠AO′B=83.9°時HDOP隨半徑ρ的變化曲線,圖6(d)給出了當(dāng)ρ=186.7 m時HDOP隨夾角∠AO′B的變化曲線。

基于上述的分析,為了提高單信標(biāo)導(dǎo)航精度,本文提出了單信標(biāo)導(dǎo)航的航路規(guī)劃方案,其流程如下:

步驟1:尋找使得HDOP最小的(ρopt,βopt);

步驟2:令A(yù)UV沿著以O(shè)′為圓心并以ρopt為半徑的圓形軌跡進行行駛;

(a) δt隨ρ的變化曲線

(b) HDOP在ρ-∠AO′B域的分布

(c) ∠AO′B=83.9°

(d) ρ=186.7 m圖6 考慮信號傳播損失后HDOP在ρ-∠AO′B域的分布Fig.6 Distribution of HDOP on ρ-∠AO′B domain when considering TL

步驟3:在圓形軌跡上選擇夾角∠AO′B接近2βopt的兩個導(dǎo)航點的相應(yīng)測量數(shù)據(jù)參與AUV位置的解算。

4 最優(yōu)航路接近方法

最優(yōu)航路接近方法包含兩部分:一是實時估計AUV自身位置的濾波算法;二是根據(jù)實時坐標(biāo)調(diào)整AUV速度方向的算法。

4.1 濾波模型和可觀測度分析

本文所涉及的非線性運動系統(tǒng)可以描述為

(15)

上式中,x=[x,y]T表示AUV的水平坐標(biāo),v=[vx,vy]T表示AUV的水平速度,t是測距聲信號從聲信標(biāo)傳播至AUV所需花費的時間。系統(tǒng)是否可觀測決定了是否可估計目標(biāo)狀態(tài),接下來基于Lie導(dǎo)數(shù)進行系統(tǒng)可觀測性分析[24-25]。為了便于分析,將式(15)改寫為

(16)

(17)

(18)

可以看出只有前兩列元素非零,故O可以簡寫為

(19)

(20)

(21)

其中,U和V是矩陣O奇異值分解后的酉矩陣,σmax和σmin分別是相應(yīng)的最大奇異值和最小奇異值。

(22)

對矩陣O進行奇異值分解后,可得σmax和σmin:

(23)

則條件數(shù)cond(O)為

(24)

接下來分析夾角θ對條件數(shù)大小的影響。下式是條件數(shù)cond(O)對|sinθ|的偏導(dǎo)

(25)

偏導(dǎo)小于0意味著cond(O)隨著|sinθ|的增大而減小,當(dāng)夾角θ=π/2時條件數(shù)最小,即可觀測度最大,換言之,當(dāng)x⊥v時(圓形軌跡),系統(tǒng)可觀測度最大。上述的分析會為下一小節(jié)的最優(yōu)航路接近軌跡的設(shè)計提供理論支撐。

由于實際中時延測量結(jié)果是離散的,故也需要將式(15)改寫為如下的離散形式

(26)

其中,xk=[xk,yk]T是AUV接收到第k個測距信號時的位置,Lk,k+1是從xk指向xk+1的虛擬基線矢量,tk+1是第k個測距信號的傳播時延。本文利用無跡卡爾曼濾波器(unscented Kalman filter,UKF)來實現(xiàn)AUV位置的實時估計[26-28]。不同于EKF,UKF避免了非線性函數(shù)的線性化,可以更準(zhǔn)確地計算經(jīng)過非線性函數(shù)傳遞的均值和協(xié)方差矩陣。

4.2 最優(yōu)航路接近軌跡的設(shè)計

首先考慮兩種特殊的軌跡,如圖7所示,分別是圓形軌跡和徑向直線軌跡,O′表示聲信標(biāo)在水平面上的投影點,圖中的藍色實線代表最優(yōu)航路,綠色圓點代表AUV的初始位置,黑色實線表示AUV接近最優(yōu)航路的軌跡。通過圖7以及4.1節(jié)的分析可得到以下的結(jié)論:1)徑向直線軌跡的效率最高(花費時間最少)但不可觀測(濾波誤差無法收斂);2)圓形軌跡無法接近最優(yōu)航路但可觀測度最大。

(a) 徑向直線軌跡

(b) 圓形軌跡圖7 兩種特殊的軌跡Fig.7 Two special trajectories

綜合考慮兩種軌跡的優(yōu)缺點并在效率和可觀測度之間做出權(quán)衡,本文希望所設(shè)計的最優(yōu)航路接近軌跡具有以下的特點(如圖8所示):1)隨著AUV與最優(yōu)航路之間徑向距離的減小,θ的取值也隨之減小并趨近于π/2;2)當(dāng)AUV離最優(yōu)航路較遠時,令θ的取值略小于π,主要目的是在保證系統(tǒng)可觀測的條件下高效率地接近最優(yōu)航路;2)當(dāng)AUV靠近最優(yōu)航路時,令θ的取值略大于π/2,主要目的是增大系統(tǒng)可觀測度,減少濾波誤差。

圖8 最優(yōu)航路接近軌跡的示意圖Fig.8 Designed trajectory

為了獲得如圖8所示的最優(yōu)航路接近軌跡,需要根據(jù)UKF給出的AUV位置實時調(diào)整AUV航速的方向。設(shè)AUV的初始位置坐標(biāo)為x0=[x0,y0]T,AUV以速度vk從xk運動至xk+1,vk與x軸正方向的夾角為γk,本文設(shè)計了如下的實時調(diào)整γk的方法。

1)如圖9所示,將x-y坐標(biāo)系繞原點順時針旋轉(zhuǎn)角度χ得到w-u坐標(biāo)系,并使得w軸經(jīng)過點x0,x-y坐標(biāo)系下的xk對應(yīng)w-u坐標(biāo)系下的wk,二者滿足如下的關(guān)系式

(27)

圖9 調(diào)整vk與x軸正方向夾角γk的示意圖Fig.9 Process of changing AUV’s velocity direction

2)在w-u坐標(biāo)系下,以u軸上某點為圓心作圓弧Ck并保證該圓弧經(jīng)過點wk和點(0,ρopt),在點wk作圓弧Ck的切線,該切線與w軸正方向的夾角φk可以通過下式計算

(28)

3)令A(yù)UV速度vk與x軸的夾角為γk=φk+χ。

接下來給出一個最優(yōu)航路接近軌跡的計算例子,最優(yōu)圓形軌跡的半徑為ρopt=200 m,AUV初始位置為x0=[-800,0]Tm,圖10(a)的黑色實線畫出了最優(yōu)航路接近軌跡,圖10(b)則給出了在AUV接近最優(yōu)航路過程中θ的變化曲線,可以看出所設(shè)計的軌跡完全滿足前文所提的三點要求。

(a) 最優(yōu)航路的接近軌跡

(b) θ隨距離的變化曲線圖10 當(dāng)ρopt=200 m且x0=[-800,0]T m時的最優(yōu)航路接近軌跡Fig.10 Trajectory of approaching the optimal route when ρopt=200 m and x0=[-800,0]T m

5 仿真

5.1 航路規(guī)劃方案的驗證

本章通過蒙特卡羅方法來驗證所尋得的(ρopt,βopt)的準(zhǔn)確性。仿真條件與第3章相同,對于每個半徑和夾角的組合,執(zhí)行3 000次蒙特卡羅仿真試驗,每次蒙特卡羅仿真利用牛頓迭代法來求解式(1)的方程組,進而獲得AUV位置的估計結(jié)果,最后基于這些位置估計結(jié)果統(tǒng)計單信標(biāo)導(dǎo)航的均方根誤差(root mean squared error,RMSE)。RMSE的計算見式(29),式中[xAUV,yAUV]T是AUV位置的真值,[xi,yi]T是第i次蒙特卡羅仿真的AUV位置解算結(jié)果。圖11(a)給出了在半徑和夾角取不同值時RMSE的計算結(jié)果,可以看出當(dāng)ρ=186.8 m且∠AO′B=84.25°時RMSE取得最小值,圖11(b)給出了當(dāng)∠AO′B=84.25°時RMSE隨半徑ρ的變化曲線,圖11(c)給出了當(dāng)ρ=186.8 m時RMSE隨夾角∠AO′B的變化曲線。比較第3章和本章的計算結(jié)果,可以看出兩者近似一致,這即證明了第2和3章公式化簡的正確性,也證明了第3章航路規(guī)劃方法的有效性。

(a) RMSE在ρ-∠AO′B域的分布

(b) ∠AO′B=84.25°

(c) ρ=186.8 m圖11 RMSE在ρ-∠AO′B域的分布Fig.11 Distribution of RMSE on ρ-∠AO′B domain

(29)

5.2 最優(yōu)航路接近方法的驗證

仿真參數(shù)見表1和圖6(a),最優(yōu)半徑設(shè)置為186.7 m,AUV的初始位置真值為[-600,700]Tm且其估計值為[-625,730]Tm,AUV的速度大小為5 m·s-1,測距信號發(fā)射周期為5 s,蒙特卡羅仿真次數(shù)為3000并利用式(29)統(tǒng)計濾波結(jié)果的RMSE。圖12(a)給出一次蒙特卡羅試驗的結(jié)果,其中黑色曲線代表最優(yōu)航路,藍色曲線代表真實的接近軌跡,紅色曲線代表UKF的濾波估計結(jié)果,圖12(b)給出了UKF的RMSE隨時間的變化曲線,圖12(c)給出了AUV和聲信標(biāo)之間的水平距離隨時間的變化曲線,圖12(d)給出了θ(vk和xk之間的夾角)隨著時間的變化曲線?？梢钥闯?在前70 s時間內(nèi),θ是一個大鈍角(154°左右),此時的可觀測度小,AUV高效率地向最優(yōu)航路靠近但是濾波誤差仍然較大,濾波軌跡和真實軌跡存在明顯的差別;在后半段時間內(nèi),θ開始快速下降,其可觀測度逐漸增大,濾波誤差開始大幅度下降,濾波軌跡和真實軌跡逐漸重合;在最后時刻,θ的取值近似為92°,AUV近似以切線形式進入預(yù)設(shè)的圓形軌跡,濾波誤差小于2 m,AUV與最優(yōu)航路的徑向距離(相對于聲信標(biāo))約為1 m,AUV準(zhǔn)確地進入了預(yù)設(shè)的最優(yōu)航路。上述仿真證明了所提最優(yōu)航路接近方法的有效性。

(a) 最優(yōu)航路接近軌跡

(c) AUV與聲信標(biāo)之間的水平距離

(d) θ隨時間的變化曲線圖12 最優(yōu)航路接近軌跡的仿真結(jié)果Fig.12 Simulation results of the path approaching method

6 結(jié)論

通過研究,得到如下結(jié)論:

1)本文基于泰勒級數(shù)展開對單信標(biāo)導(dǎo)航進行了精度分析以及魯棒性分析。當(dāng)兩個導(dǎo)航點和聲信標(biāo)構(gòu)成的夾角近似為直角時導(dǎo)航精度高,此外,單信標(biāo)導(dǎo)航系統(tǒng)對時延測量誤差、聲信標(biāo)位置誤差以及深度測量誤差的魯棒性較弱。

2)在精度分析的基礎(chǔ)上,以降低導(dǎo)航誤差為目的提出了航路規(guī)劃方案,包括最優(yōu)圓形軌跡半徑的計算以及最優(yōu)導(dǎo)航點的選取,航路規(guī)劃方案使得AUV位置估計的均方根誤差約為2 m。

3)對單信標(biāo)導(dǎo)航系統(tǒng)的可觀測性和可觀測度進行了分析,指出AUV相對于聲信標(biāo)的徑向直線軌跡不可觀測,且以聲信標(biāo)為圓心的圓形軌跡的可觀測度最強?；诳捎^測度的分析結(jié)果設(shè)計了最優(yōu)航路接近軌跡,使得AUV高效率且高精度地逼近預(yù)設(shè)的最優(yōu)航路。