移動USBL測距輔助的UUV協(xié)同導(dǎo)航定位方法

王銀濤,賈曉寶,崔榮鑫,嚴(yán)衛(wèi)生

(1.西北工業(yè)大學(xué)航海學(xué)院,陜西西安 710072;2.水下信息與控制國家級重點(diǎn)實(shí)驗(yàn)室,陜西西安 710072;3.中國航空工業(yè)集團(tuán)公司洛陽電光設(shè)備研究所,河南洛陽 471023)

1 引言

精確的導(dǎo)航定位能力是無人水下航行器(unmanned underwater vehicle,UUV)順利完成各種任務(wù)的重要前提與保障.常見的水下導(dǎo)航方法有航位推算導(dǎo)航[1]、慣性導(dǎo)航[2]、地球物理導(dǎo)航[3]、聲學(xué)導(dǎo)航[4]等,雖然都具有各自的特點(diǎn),但又相應(yīng)的具有一些自身難以克服的缺陷.例如:航位推算導(dǎo)航定位精度低而且存在誤差累積;慣性導(dǎo)航雖然可以在短時(shí)間內(nèi)獲得較高的定位精度,但仍然存在誤差累積的問題,不適用于長時(shí)間或遠(yuǎn)距離的水下導(dǎo)航;地球物理導(dǎo)航實(shí)時(shí)性和適用性則受環(huán)境條件限制.

長基線(long base line,LBL)、短基線(short base line,SBL)、超短基線(ultra short base line,USBL)聲學(xué)定位系統(tǒng)是基于聲學(xué)原理,聲信標(biāo)在收到UUV發(fā)來的信號后會在極短延遲后進(jìn)行應(yīng)答,根據(jù)聲信號往返的時(shí)間、聲信標(biāo)位置和傳播速度就可以計(jì)算UUV實(shí)際位置[5].其中,超短基線由于尺寸小、安裝方便等特點(diǎn),比較適合用于水下運(yùn)動載體間的通信和定位[6].文獻(xiàn)[7]結(jié)合捷聯(lián)慣導(dǎo)系統(tǒng)(stapdown inertial navigation system,SINS)短時(shí)間內(nèi)導(dǎo)航精度高和USBL在一定范圍內(nèi)定位精度高的特點(diǎn),提出了一種帶有卡方檢驗(yàn)的SINS/USBL集成導(dǎo)航容錯(cuò)方法,實(shí)驗(yàn)結(jié)果表明該方法可以抑制USBL異常信息對導(dǎo)航精度的影響,并保證了組合導(dǎo)航系統(tǒng)的穩(wěn)定性和高精度.Jalving在文獻(xiàn)[8]中利用卡爾曼濾波器(Kalman filter,KF)技術(shù)把USBL和SINS的輸出信息進(jìn)行數(shù)據(jù)融合和濾波處理得到更加精確的導(dǎo)航信息.文獻(xiàn)[9]為了提高導(dǎo)航精度,采用改進(jìn)的EKF濾波對系統(tǒng)的隨機(jī)誤差進(jìn)行補(bǔ)償.在文獻(xiàn)[10]中,Morgado等人提出了一種緊組合技術(shù),將超短基線系統(tǒng)與慣性導(dǎo)航系統(tǒng)相關(guān)參數(shù)直接組合起來對UUV的導(dǎo)航精度進(jìn)行輔助提升.上述方法的性能通常受聲信標(biāo)布放位置以及工作適用距離限制.為擴(kuò)大信標(biāo)適用距離和工作范圍,針對USV和UUV協(xié)同導(dǎo)航的研究,嚴(yán)浙平等人[11]分析了USBL水聲定位數(shù)據(jù)和GPS數(shù)據(jù),證明兩者可進(jìn)行組合導(dǎo)航.多UUV協(xié)同導(dǎo)航定位[12]是在單體UUV導(dǎo)航定位和多UUV協(xié)同基礎(chǔ)上發(fā)展而來的水下導(dǎo)航定位技術(shù)新方法,其原理是利用與集群中個(gè)別或部分裝配高精度導(dǎo)航設(shè)備的UUV相對位置關(guān)系提高自身導(dǎo)航精度.此類方法雖然拓寬了基線布放位置和區(qū)域的限制,但參考UUV導(dǎo)航誤差累積的問題依然存在.

本文提出一種基于無人水面船(unmanned surface vehicle,USV)輔助的UUV導(dǎo)航新方法,利用USV自身的移動性及布設(shè)在上面的的USBL為UUV提供長距離大范圍的高精度導(dǎo)航定位服務(wù).論文首先建立SINS的動態(tài)誤差模型,在此基礎(chǔ)上設(shè)計(jì)構(gòu)建USBL輔助SINS的組合導(dǎo)航系統(tǒng)狀態(tài)方程和量測方程,并分別利用卡爾曼濾波算法和自適應(yīng)卡爾曼濾波兩種算法對位置信息進(jìn)行融合提升導(dǎo)航精度.論文對提出的算法進(jìn)行了仿真和實(shí)驗(yàn)驗(yàn)證,表明了提出的改進(jìn)型自適應(yīng)卡爾曼濾波算法的有效性和可行性.

2 組合導(dǎo)航系統(tǒng)的模型

2.1 SINS系統(tǒng)誤差模型

UUV狀態(tài)誤差矢量方程中,一般包含位置誤差(包括經(jīng)度誤差、緯度誤差和深度方向的誤差)、速度誤差(包括東向速度誤差、北向速度誤差、天向速度誤差)以及3個(gè)姿態(tài)角的誤差(包括俯仰角、橫滾角、航向角、加速度計(jì)的常值漂移和陀螺儀的常值漂移).

因此,慣性導(dǎo)航系統(tǒng)的完整誤差狀態(tài)矢量表述為

式中:?=[φEφNφU]為姿態(tài)誤差矢量,δv=[δvEδvNδvU]為速度誤差矢量,δp=[δL δλ δh]代表位置誤差矢量,ε=[εxεyεz]為陀螺儀漂移誤差矢量,?=[?x?y?z]為加速度計(jì)漂移誤差矢量.

2.2 組合導(dǎo)航系統(tǒng)狀態(tài)方程

采用USBL與SINS設(shè)計(jì)組合導(dǎo)航系統(tǒng)時(shí),一般選用卡爾曼濾波進(jìn)行組合,通常使用間接法進(jìn)行估計(jì),因此濾波器的狀態(tài)量選為導(dǎo)航參數(shù)的誤差,主要是對導(dǎo)航參數(shù)的誤差進(jìn)行估計(jì),然后通過估計(jì)值去校正SINS的導(dǎo)航參數(shù).

USBL與SINS的組合可以有不同的方式,根據(jù)選取的狀態(tài)參數(shù),可分為兩類,一類是松組合,一類是緊組合.松組合主要是位置和速度信息作為組合量;緊組合主要依據(jù)設(shè)備測量的原始信息進(jìn)行組合,這里主要是斜距和斜距差.由于本文所采用的USBL無法輸出斜距和斜距率信息,本論文提出一種不依賴斜距和斜距率的組合導(dǎo)航方法,并推導(dǎo)出組合導(dǎo)航狀態(tài)方程.

導(dǎo)航系統(tǒng)的狀態(tài)方程主要由慣性導(dǎo)航系統(tǒng)的誤差狀態(tài)組成,將平臺誤差角方程、速度誤差方程、位置誤差方程和慣性儀器的誤差方程綜合到一起,可表示為

同理,Fb可以寫為下述矩陣形式,即

為陀螺儀和加速度計(jì)的誤差狀態(tài)所對應(yīng)的狀態(tài)矩陣,由于文中將兩者的誤差考慮為白噪聲,因此這里Fc取零矩陣,0∈R9×6.G(t)為系統(tǒng)噪聲矩陣,這里取為單位陣.W(t)=[ωgxωgyωgzωaxωayωaz]T表示慣性器件的噪聲,其中:[ωgxωgyωgz]T表示陀螺隨機(jī)白噪聲,[ωaxωayωaz]T代表加速度計(jì)隨機(jī)白噪聲.

2.3 組合導(dǎo)航系統(tǒng)量測方程

由于本文中用到的USBL無法輸出斜距信息,因此這里USBL/SINS組合導(dǎo)航系統(tǒng)的選擇的是基于位置的松組合方法,系統(tǒng)的量測值為USBL和SINS輸出位置的差值.為了便于進(jìn)行組合導(dǎo)航解算,一般選擇將位置信息轉(zhuǎn)化為地球直角坐標(biāo)系下的位置信息,假設(shè)水下航行器UUV在地球空間直角坐標(biāo)系的位置為[x y z],則地球坐標(biāo)系與地球空間直角坐標(biāo)系之間的轉(zhuǎn)換關(guān)系為

式中:RN為地球半徑,h為當(dāng)?shù)睾０胃叨?λ為經(jīng)度,φ代表緯度,e代表偏心率.對式(3)兩端求解全微分得

為了便于后續(xù)測量方程的推導(dǎo),將上式改寫為矩陣形式

USBL定位設(shè)備對水下UUV進(jìn)行定位,并將定位結(jié)果和USV的位置通過通信手段,以一定的時(shí)間間隔同時(shí)發(fā)送給UUV,這樣UUV既可以對其位置進(jìn)行精確估計(jì),同時(shí)也可以對UUV所裝備的SINS設(shè)備進(jìn)行校正.當(dāng)USBL安裝外置GNSS后,通過USBL可以解算出UUV的位置信息,包括經(jīng)度、緯度和UUV的深度信息.而UUV自身安裝的SINS設(shè)備同樣可以解算出經(jīng)緯度和UUV的水深信息,通過兩個(gè)設(shè)備作差,得到量測信息.所以量測值Z可以選為下式

式中:Lu,λu,hu代表USBL設(shè)備輸出的UUV的位置,分別為緯度、經(jīng)度和高度;Li,λi,hi代表由捷聯(lián)慣導(dǎo)設(shè)備輸出的UUV的位置,分別為緯度、經(jīng)度和高度.由于SINS和USBL輸出的位置信息中均存在量測誤差,所以根據(jù)式(6)可以將量測值變?yōu)槿缦滦问?

式中:δLu,δλu,δhu分別代表USBL輸出的緯度、經(jīng)度和深度誤差值;δLi,δλi,δhi分別代表UUV自身安裝的SINS輸出的緯度、經(jīng)度和深度誤差值;L,λ,h代表UUV的真實(shí)狀態(tài).根據(jù)式(1)選取的USBL輔助SINS導(dǎo)航的狀態(tài)向量X,結(jié)合式(7)可以推導(dǎo)出基于位置的USBL輔助SINS導(dǎo)航的量測方程為

式中:量測矩陣H=[03×6I3×303×6],V為系統(tǒng)的量測噪聲,這里設(shè)置為零均值的高斯白噪聲.

結(jié)合式(8)和式(2)可以得到基于USBL輔助的INS導(dǎo)航系統(tǒng)的狀態(tài)方程和量測方程

式中:X(t)為導(dǎo)航系統(tǒng)的狀態(tài),選為SINS的狀態(tài)誤差和慣性儀表誤差;F(t)為狀態(tài)轉(zhuǎn)移矩陣;G(t)是系統(tǒng)的過程噪聲驅(qū)動矩陣;Z(t)是導(dǎo)航系統(tǒng)的量測量,為USBL定位結(jié)果與SINS定位結(jié)果的差值;W(t),V(t)分別為系統(tǒng)噪聲和量測噪聲,其中:E{W(t)}=0,E{W(t)WT(τ)}=Q(t)δ(t-τ);E{V(t)}=0,E{V(t)VT(τ)}=R(t)δ(t-τ);Q(t)為系統(tǒng)的狀態(tài)方差矩陣;R(t)為系統(tǒng)的量測方差矩陣;δkj為克羅內(nèi)克函數(shù),表達(dá)式為

3 自適應(yīng)卡爾曼濾波算法設(shè)計(jì)

自適應(yīng)卡爾曼濾波(adaptive Kalman filter,AKF)是指在利用測量數(shù)據(jù)進(jìn)行濾波的同時(shí),不斷地由濾波本身去判斷系統(tǒng)的動態(tài)是否有變化,對模型參數(shù)和噪聲統(tǒng)計(jì)特性進(jìn)行估計(jì)和修正,以改進(jìn)濾波設(shè)計(jì)、縮小濾波的實(shí)際誤差.卡爾曼濾波算法需已知系統(tǒng)噪聲協(xié)方差和量測噪聲協(xié)方差,并且系統(tǒng)噪聲協(xié)方差W和量測噪聲協(xié)方差V的精度與濾波器的濾波效果有緊密的聯(lián)系.由于水下環(huán)境惡劣,實(shí)際系統(tǒng)中存在各種噪聲影響,難以獲得準(zhǔn)確的W和V,導(dǎo)致濾波器估計(jì)精度下降,因此采用AKF算法對噪聲協(xié)方差進(jìn)行自適應(yīng)估計(jì).

本文對狀態(tài)方程(9)采用AFK算法,其過程如下:

步驟1計(jì)算一步預(yù)測狀態(tài)值及狀態(tài)誤差協(xié)方差矩陣:

一步狀態(tài)預(yù)測:

一步狀態(tài)誤差協(xié)方差預(yù)測:

其中Γk|k-1為過程噪聲矩陣.

步驟2卡爾曼濾波增益計(jì)算,狀態(tài)更新和狀態(tài)誤差協(xié)方差更新:

濾波增益計(jì)算:

其中Hk為量測矩陣.狀態(tài)更新:

式中:令εk=其代表新息或預(yù)測殘差,可以用作檢驗(yàn)卡爾曼濾波的誤差.

狀態(tài)誤差協(xié)方差矩陣更新:

步驟3系統(tǒng)噪聲和量測噪聲的自適應(yīng)調(diào)整:

系統(tǒng)的噪聲估計(jì)公式為

系統(tǒng)的量測噪聲估計(jì)公式為

在式(15)-(16)中:εk代表新息;sk表示加權(quán)系數(shù),給予舊數(shù)據(jù)和新數(shù)據(jù)不同的權(quán)重,使新數(shù)據(jù)在濾波運(yùn)算中發(fā)揮主要作用.加權(quán)系數(shù)一般用公式進(jìn)行計(jì)算,其中0＜a＜1,這里a=0.96.

4 數(shù)值仿真及結(jié)果分析

利用MATLAB對USBL輔助SINS導(dǎo)航系統(tǒng)進(jìn)行仿真,分別對傳統(tǒng)的卡爾曼濾波和改進(jìn)的自適應(yīng)卡爾曼濾波算法進(jìn)行仿真.仿真中,UUV實(shí)際軌跡設(shè)定:X=t,Y=100cos(0.05t),Z=-40;USBL參數(shù)設(shè)定為量測誤差r≤0.01m.UUV的實(shí)際軌跡與導(dǎo)航軌跡如圖1所示.

圖1 UUV實(shí)際軌跡與導(dǎo)航軌跡Fig.1 UUV actual trajectory and navigation trajectory

通過圖1的局部圖可以看出,僅利用SINS進(jìn)行導(dǎo)航,隨著時(shí)間的增加會產(chǎn)生較大的誤差,而通過USBL的輔助,利用濾波算法進(jìn)行位置的融合,可以提高UUV的導(dǎo)航精度.

為了便于對比,分別繪制傳統(tǒng)卡爾曼濾波和改進(jìn)的自適應(yīng)卡爾曼濾波得到的位置估計(jì)曲線,如圖2和圖3所示.由圖可知,通過AKF算法可以得到更高的導(dǎo)航精度.

圖2 KF位置估計(jì)曲線Fig.2 KF position estimation curve

圖3 AKF位置估計(jì)曲線Fig.3 AKF position estimation curve

USBL/SINS組成的組合導(dǎo)航系統(tǒng)解算的姿態(tài)角,速度及位置誤差曲線如圖4-6所示.

圖4 姿態(tài)誤差曲線Fig.4 Attitude error curve

在USBL測距輔助作用下,UUV俯仰和橫滾角度誤差穩(wěn)定在0.1°,最大航向誤差在KF下有1.5°,而在AKF下只有0.4°,同時(shí)在北向、東向和天向位置誤差均穩(wěn)定在0.2m以內(nèi).

通過對誤差曲線的分析可知:若僅利用慣性導(dǎo)航系統(tǒng)進(jìn)行導(dǎo)航解算時(shí),隨著位置誤差的逐漸累積,曲線會發(fā)散;利用USBL/SINS組成的組合導(dǎo)航系統(tǒng)的定位信息進(jìn)行融合后,系統(tǒng)的導(dǎo)航參數(shù)得到較大提高,并且利用KF和AKF均能夠有效的估計(jì)出各個(gè)誤差角,克服純慣導(dǎo)造成的速度和位置隨時(shí)間發(fā)散的問題.同KF估計(jì)方法相比,AKF方法估計(jì)精度更高.

圖5 USBL/SINS組合導(dǎo)航的速度誤差曲線Fig.5 Velocity error curve of USBL/SINS integrated navigation

圖6 USBL/SINS組合導(dǎo)航的位置誤差曲線Fig.6 Position error curve of USBL/SINS integrated navigation

5 實(shí)驗(yàn)結(jié)果及分析

為驗(yàn)證本文所提出的組合導(dǎo)航方法的有效性,本文使用某型雙體無人船(圖7)和BlueROV2(圖8)在西北工業(yè)大學(xué)啟翔湖進(jìn)行了USBL/SINS組合導(dǎo)航實(shí)驗(yàn),其中:無人船用于搭載USBL,ROV用于搭載OEM應(yīng)答器.無人船平臺通過iNet300與地面站進(jìn)行通信,通過USBL與ROV進(jìn)行通信.由于湖面水域以及水深的限制,實(shí)驗(yàn)在80m×20m的水域范圍內(nèi)進(jìn)行,湖上實(shí)驗(yàn)場地如圖9所示.實(shí)驗(yàn)過程中,BlueROV2在水下以直線軌跡航行.

圖7 黃色雙體無人船F(xiàn)ig.7 Yellow catamaran USV

圖8 藍(lán)色遙控水下航行器Fig.8 BlueRov2

圖9 湖面實(shí)驗(yàn)場地Fig.9 Experimental site on the lake

無人船和ROV的航行軌跡如圖10所示.通過USV輔助UUV實(shí)現(xiàn)了較高精度的導(dǎo)航,分析分別利用KF方法和AKF方法導(dǎo)航的效果.組合導(dǎo)航實(shí)驗(yàn)結(jié)果如圖11所示.

圖10 USV靜止、UUV航行實(shí)驗(yàn)結(jié)果Fig.10 USV static and UUV sailing test results

圖11 UUV松組合導(dǎo)航實(shí)驗(yàn)結(jié)果Fig.11 UUV pine integrated navigation results

為了比較兩種濾波方法的組合效果,分別繪制KF濾波和AKF濾波實(shí)現(xiàn)的松組合導(dǎo)航系統(tǒng)的軌跡跟蹤曲線,分別如圖12和圖13所示.對比圖12和圖13可知,利用AKF算法估計(jì)的效果更佳平滑,而且可以避免KF造成的發(fā)散問題.

圖12 KF軌跡跟蹤曲線Fig.12 KF trajectory tracking curve

圖13 AKF軌跡跟蹤曲線Fig.13 AKF trajectory tracking curve

位置誤差曲線和速度誤差曲線分別如圖14和圖15所示.根據(jù)圖14可知,采用KF和AKF均可以實(shí)現(xiàn)對組合導(dǎo)航系統(tǒng)誤差的抑制,但是利用改進(jìn)的AKF算法,當(dāng)存在較大的誤差時(shí),同樣可以實(shí)現(xiàn)對UUV導(dǎo)航系統(tǒng)輸出的位置信息進(jìn)行校正.由圖15可知,采用改進(jìn)的AKF算法較KF估計(jì)效果更好,可以實(shí)現(xiàn)對UUV導(dǎo)航系統(tǒng)輸出的速度信息進(jìn)行校正,避免了KF在較大噪聲時(shí)的發(fā)散問題.

圖14 KF和AKF位置誤差的USBL/SINS組合導(dǎo)航位置誤差曲線對比圖Fig.14 USBL/SINS combined navigation position error curve comparison of position error of KF and AKF

圖15 KF和AKF速度誤差的USBL/SINS組合導(dǎo)航曲線對比圖Fig.15 Comparison of speed error of KF and AKF(USBL and SINS integrated navigation)

姿態(tài)誤差曲線如圖16所示.根據(jù)圖16可知,采用KF算法和AKF算法均可以抑制誤差發(fā)散,但是改進(jìn)的AKF算法相比KF算法,具有更好的抑制效果.

圖16 KF和AKF航向角誤差曲線對比圖Fig.16 Comparison of heading angle error of KF and AKF

6 結(jié)束語

本文設(shè)計(jì)提出一種USV輔助UUV導(dǎo)航定位方法,通過利用USV攜帶的USBL獲得UUV的定位信息,與UUV自身慣導(dǎo)信息融合,得到一種UUV組合導(dǎo)航模型,并基于自適應(yīng)卡爾曼濾波對UUV運(yùn)動狀態(tài)進(jìn)行估計(jì).論文分別基于仿真和現(xiàn)場實(shí)驗(yàn)對提出的方法進(jìn)行了驗(yàn)證,表明了算法的有效性和可行性.論文結(jié)果對UUV的組合導(dǎo)航具有一定的理論指導(dǎo)意義和實(shí)際應(yīng)用價(jià)值.