水下單信標導航航路規劃和最優航路接近方法

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

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

0 引言

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

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

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

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

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

1 導航模型

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

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

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

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

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

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

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

(1)

(a) 正視圖

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

2 導航精度

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

(2)

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

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

(3)

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

(4)

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

(5)

(6)

從式(4)可得

(7)

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

(8)

其中

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

(9)

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

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

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

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

3 航路規劃

(10)

(11)

(a) 俯視圖

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

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

(12)

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

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

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

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

(13)

接下來通過一個仿真來證明(ρopt,βopt)的存在,仿真參數見表1。圖5(a)給出了在半徑和夾角取不同值時HDOP的計算結果,可以看出當ρ=276.5 m且∠AO′B=87°時HDOP取得最小值,圖5(b)給出了當∠AO′B=87°時HDOP隨半徑ρ的變化曲線,圖5(c)給出了當ρ=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的仿真是在各測量誤差的標準差取值固定的條件下進行的,但在實際的水聲環境中,隨著AUV位置發生改變,信噪比發生改變,時延估計誤差的標準差也隨之改變。因此,為了更準確地計算水平位置精度因子,需要考慮信號傳播損失(transmission loss,TL)的影響。某一位置的信噪比JSNR及時延測量誤差標準差δt可通過下式計算[23]

(14)

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

基于上述的分析,為了提高單信標導航精度,本文提出了單信標導航的航路規劃方案,其流程如下:

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

步驟2:令AUV沿著以O′為圓心并以ρ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的兩個導航點的相應測量數據參與AUV位置的解算。

4 最優航路接近方法

最優航路接近方法包含兩部分:一是實時估計AUV自身位置的濾波算法;二是根據實時坐標調整AUV速度方向的算法。

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

本文所涉及的非線性運動系統可以描述為

(15)

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

(16)

(17)

(18)

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

(19)

(20)

(21)

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

(22)

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

(23)

則條件數cond(O)為

(24)

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

(25)

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

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

(26)

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

4.2 最優航路接近軌跡的設計

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

(a) 徑向直線軌跡

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

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

圖8 最優航路接近軌跡的示意圖Fig.8 Designed trajectory

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

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

(27)

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

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

(28)

3)令AUV速度vk與x軸的夾角為γk=φk+χ。

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

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

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

5 仿真

5.1 航路規劃方案的驗證

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

(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 最優航路接近方法的驗證

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

(a) 最優航路接近軌跡

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

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

6 結論

通過研究,得到如下結論:

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

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

3)對單信標導航系統的可觀測性和可觀測度進行了分析,指出AUV相對于聲信標的徑向直線軌跡不可觀測,且以聲信標為圓心的圓形軌跡的可觀測度最強。基于可觀測度的分析結果設計了最優航路接近軌跡,使得AUV高效率且高精度地逼近預設的最優航路。