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

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

1 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

為陀螺儀和加速度計的誤差狀態(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表示陀螺隨機白噪聲,[ωaxωayωaz]T代表加速度計隨機白噪聲.

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

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

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

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

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

式中:Lu,λu,hu代表USBL設(shè)備輸出的UUV的位置,分別為緯度、經(jīng)度和高度;Li,λi,hi代表由捷聯(lián)慣導設(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的真實狀態(tài).根據(jù)式(1)選取的USBL輔助SINS導航的狀態(tài)向量X,結(jié)合式(7)可以推導出基于位置的USBL輔助SINS導航的量測方程為

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

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

式中:X(t)為導航系統(tǒng)的狀態(tài),選為SINS的狀態(tài)誤差和慣性儀表誤差;F(t)為狀態(tài)轉(zhuǎn)移矩陣;G(t)是系統(tǒng)的過程噪聲驅(qū)動矩陣;Z(t)是導航系統(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ù),表達式為

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

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

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

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

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

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

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

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

濾波增益計算:

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

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

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

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

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

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

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

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

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

圖1 UUV實際軌跡與導航軌跡Fig.1 UUV actual trajectory and navigation trajectory

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

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

圖2 KF位置估計曲線Fig.2 KF position estimation curve

圖3 AKF位置估計曲線Fig.3 AKF position estimation curve

USBL/SINS組成的組合導航系統(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°,同時在北向、東向和天向位置誤差均穩(wěn)定在0.2m以內(nèi).

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

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

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

5 實驗結(jié)果及分析

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

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

圖8 藍色遙控水下航行器Fig.8 BlueRov2

圖9 湖面實驗場地Fig.9 Experimental site on the lake

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

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

圖11 UUV松組合導航實驗結(jié)果Fig.11 UUV pine integrated navigation results

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

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

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

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

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

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

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

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

6 結(jié)束語

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