考慮洋流影響的SINS/DVL組合導(dǎo)航算法

郭玉勝，付夢(mèng)印，鄧志紅，鄧?yán)^權(quán)，艾瀛濤

（北京理工大學(xué)自動(dòng)化學(xué)院，北京100081；北京自動(dòng)化控制設(shè)備研究所，北京100074）

捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)/多普勒計(jì)程儀(DVL)組合導(dǎo)航系統(tǒng)是長(zhǎng)航時(shí)水下導(dǎo)航常用的一種導(dǎo)航方式[1-5]。DVL是利用安裝在載體的超聲換能器向海底發(fā)射超聲波，并根據(jù)多普勒效應(yīng)的原理測(cè)量載體速度的儀器[6-7]。目前使用的 DVL通常都有兩種工作模式：底跟蹤模式和水跟蹤模式。當(dāng)載體相對(duì)于海底距離在 DVL有效打底深度之內(nèi)時(shí)，DVL工作在底跟蹤模式，可以提供精確、穩(wěn)定的載體前向和側(cè)向?qū)Φ姿俣取．?dāng)載體對(duì)底距離超過(guò)了DVL測(cè)速最大射程時(shí)，DVL會(huì)自動(dòng)切換到水跟蹤模式，此時(shí)測(cè)量得到的是載體相對(duì)于DVL設(shè)定水流層的速度[8]。當(dāng)DVL工作于水跟蹤模式時(shí)，還需要知道水流層的水流速度，兩者之和才是載體的對(duì)底速度。因此，當(dāng) DVL由底跟蹤模式切換到水跟蹤模式后，洋流速度將會(huì)對(duì)SINS/DVL組合導(dǎo)航系統(tǒng)的定位精度產(chǎn)生很大的影響。

針對(duì)該問(wèn)題，當(dāng)前研究的主要思路是基于機(jī)動(dòng)目標(biāo)“當(dāng)前”統(tǒng)計(jì)模型的水下組合導(dǎo)航模式，通過(guò)增加加速度觀測(cè)信息，采用加速度均值、方差自適應(yīng)Kalman濾波算法實(shí)現(xiàn)線運(yùn)動(dòng)參數(shù)的估計(jì)和流速修正，進(jìn)而提高了航位推算的精度[9-10]。但是該方法計(jì)算量大，工程實(shí)現(xiàn)較為麻煩，而且該方法不能解決 DVL在不同模式切換下的流速修正問(wèn)題。

在高精度水下導(dǎo)航定位中必須考慮洋流速度的影響。在極端條件下，渤海的表層水流速度可達(dá)3.47m/s，黃海的表層水流速度可達(dá)2.88 m/s，東海的表層水流速度可達(dá)3.26 m/s。此時(shí)，對(duì)于運(yùn)行速度比較慢的載體，水流的速度可能超過(guò)載體自身的行駛速度。因此針對(duì)海底深度變化導(dǎo)致DVL在不同模式下切換的問(wèn)題，開(kāi)展便于工程實(shí)現(xiàn)的載體周?chē)》秶鷥?nèi)的洋流速度估計(jì)對(duì)提高 SINS/DVL組合導(dǎo)航定位精度有著非常重要的意義。

DVL測(cè)速精度越高要求的聲波發(fā)射頻率越高，聲波發(fā)射頻率越高，射程就會(huì)越小[11]。當(dāng)海底深度超過(guò)了DVL射程范圍，DVL測(cè)速精度就會(huì)嚴(yán)重下降。兩者的具體切換情況如圖1所示。當(dāng)載體位于A區(qū)域時(shí)，由于載體對(duì)底距離超過(guò)了DVL測(cè)速最大射程，DVL工作于水跟蹤模式，測(cè)量得到的是載體相對(duì)于DVL設(shè)定水流層的速度，此時(shí)，還需要知道水流層的水流速度，兩者之和才是載體的對(duì)底速度。當(dāng)載體行駛至B區(qū)域時(shí)，由于載體對(duì)底距離在DVL測(cè)速最大射程之內(nèi)，DVL切換到底跟蹤模式，可以提供精確、穩(wěn)定的載體前向和側(cè)向?qū)Φ姿俣取４送猓捎贐區(qū)域海底深度大于DVL設(shè)定的水流層深度，DVL在輸出載體對(duì)底速度的同時(shí)也可以輸出載體相對(duì)于水流層的速度。當(dāng)載體行駛至C區(qū)域時(shí)，由于海底深度小于DVL設(shè)定的水流層的深度，DVL只能輸出載體對(duì)底速度。在實(shí)際工程應(yīng)用中，由于海底深度的變化，DVL可能頻繁地在水跟蹤和底跟蹤模式之間切換。而通常情況下DVL設(shè)定的水流層的水流速度大多是未知的，所以當(dāng)DVL工作于水跟蹤模式時(shí)，DVL測(cè)得的載體速度將會(huì)帶有很大的誤差，導(dǎo)致SINS/DVL組合導(dǎo)航系統(tǒng)存在很大的定位誤差。

SINS/DVL組合導(dǎo)航采用速度組合方式，利用SINS解算得到的導(dǎo)航系下的速度與DVL直接測(cè)得的速度經(jīng)轉(zhuǎn)換修正后作差，將速度差作為 Kalman濾波的觀測(cè)量。當(dāng) DVL工作于底跟蹤模式時(shí)，能夠輸出較為準(zhǔn)確的載體對(duì)底速度信息。然而由于水流速度的存在，當(dāng) DVL工作于水跟蹤模式時(shí)，測(cè)得的載體速度有較大誤差。在實(shí)際工程應(yīng)用中，由于海底深度的變化，DVL可能頻繁地在底跟蹤模式和水跟蹤模式之間切換。針對(duì)這種情況，根據(jù) DVL的工作模式，設(shè)置不同的觀測(cè)量以及量測(cè)矩陣，當(dāng) DVL進(jìn)入水跟蹤模式時(shí)，通過(guò)觀測(cè)量引入洋流速度信息，利用Kalman濾波器對(duì)洋流速度進(jìn)行實(shí)時(shí)估計(jì)。

圖1 DVL工作模式示意圖Fig.1 Schematic diagram of DVL working mode

1 SINS/DVL組合濾波方程及算法

DVL測(cè)速誤差主要與聲速誤差、載體姿態(tài)誤差等有關(guān)[12-13]。考慮到這些誤差在實(shí)際產(chǎn)品中已經(jīng)經(jīng)過(guò)補(bǔ)償，現(xiàn)只考慮 DVL刻度因子誤差以及安裝角誤差。由于水平方向上的兩個(gè)安裝角誤差對(duì)系統(tǒng)組合導(dǎo)航定位精度的影響相對(duì)較小，所以可以忽略水平安裝角誤差的影響，并且可將DVL刻度因子誤差δkD以及航向安裝角誤差β都當(dāng)作常值處理。此外，根據(jù)現(xiàn)有海洋模型可以得知，在小范圍內(nèi)，洋流速度變化較為緩慢，因此可以假定短期內(nèi) DVL設(shè)定水流層的洋流速度為恒定值。令洋流速度在地理坐標(biāo)系東向、天向、北向的分量可分別表示為 Vcn、 Vcu、Vce：

可以得到：

SINS/DVL組合導(dǎo)航系統(tǒng)狀態(tài)變量由主系統(tǒng)SINS的狀態(tài)量和輔助子系統(tǒng) DVL的狀態(tài)量一起構(gòu)成。不考慮天向通道的影響，主系統(tǒng)SINS狀態(tài)量選取如下：北向、東向速度誤差δVN、δVE；北向、天向、東向姿態(tài)誤差φN、φU、φE；北向、東向位置誤差δφ、δλ；載體系各軸向的陀螺漂移；加速度計(jì)零偏。選取DVL刻度因子誤差δkD、航向安裝角誤差β以及北向、東向洋流速度誤差分量δVcn、 δVce為子系統(tǒng)DVL的狀態(tài)量。可以得到SINS/DVL組合導(dǎo)航系統(tǒng)誤差狀態(tài)方程如下：

根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)誤差方程可以得到系統(tǒng)狀態(tài)矩陣 FSINS。又有 XDVL=[δkDβ δVcnδVce]T，根據(jù)上述分析可以得到系統(tǒng)矩陣 FDVL中的元素均為零。

理想情況下，DVL相對(duì)于自身測(cè)量坐標(biāo)系的速度在導(dǎo)航系（n系）下的投影為，其為 DVL測(cè)得載體在儀器系（d系）下的真實(shí)速度，儀器系到載體系（b系）姿態(tài)轉(zhuǎn)換矩陣，為載體系到導(dǎo)航系的姿態(tài)轉(zhuǎn)換矩陣。

考慮刻度因子、安裝誤差等影響因素，DVL實(shí)際輸出的速度為：

SINS/DVL組合導(dǎo)航Kalman濾波采用速度匹配模式。當(dāng)DVL工作于底跟蹤模式時(shí)，無(wú)需考慮洋流速度的影響，直接以捷聯(lián)慣導(dǎo)解算得到的速度和DVL測(cè)得的速度之差值作為Kalman濾波器的觀測(cè)量，即觀測(cè)量：

式中， Cij(i = 1 ,2,3; j = 1 ,2,3)為姿態(tài)轉(zhuǎn)換矩陣 Cnb中相應(yīng)的元素。對(duì)應(yīng)的系統(tǒng)量測(cè)矩陣為：

當(dāng)DVL工作于水跟蹤模式時(shí)，DVL輸出速度有較大的誤差，需考慮洋流速度的影響。即觀測(cè)量，此時(shí)系統(tǒng)量測(cè)矩陣變?yōu)椋?/p>

式中，1H、2H 與 DVL工作于底跟蹤模式時(shí)對(duì)應(yīng)的量測(cè)矩陣相同。

選擇合適的狀態(tài)初始值 X0、初始估計(jì)均方誤差陣P0、系統(tǒng)噪聲初始方差陣 Q0以及量測(cè)噪聲方差陣 Rk，按照離散型 Kalman濾波計(jì)算公式即可進(jìn)行SINS/DVL組合導(dǎo)航Kalman濾波計(jì)算。

2 仿真分析

2.1 仿真參數(shù)設(shè)置

根據(jù)水下環(huán)境特點(diǎn)以及SINS/DVL組合導(dǎo)航系統(tǒng)的實(shí)際使用情況，設(shè)置如圖2所示的仿真運(yùn)行軌跡。

圖2 仿真運(yùn)行軌跡Fig.2 Simulation running track

具體仿真條件設(shè)置如下：

①軌跡設(shè)置

仿真總時(shí)間： t=2000s；

轉(zhuǎn)彎時(shí)間：400s；

初始位置：φ0=39.8°，λ0=116.2°，h0=-80m；

初始速度： VN0=0m/s， VU0=0m/s， VE0=0m/s；

勻速直航速度： Vb=6m/s；

轉(zhuǎn)彎速度： Vb=3m/s；

初始姿態(tài)角：航向角ψ=-90°，俯仰角θ=0°，滾動(dòng)角γ=0°；初始姿態(tài)誤差：δψ=0.1°，δθ=0.01°，δγ=-0.01°。

②慣性器件設(shè)置

慣性器件采樣周期： Tn=5ms；

陀螺漂移：0.01(°)/h的隨機(jī)噪聲；

加速度計(jì)零偏：0.0005m/s2的隨機(jī)噪聲。

③DVL參考信息設(shè)置

DVL測(cè)速采樣周期：T=1s；

DVL測(cè)速誤差：0.02m/s的隨機(jī)噪聲；

DVL 僅對(duì)水有效時(shí)間段：200～600s，800～1200s。

200～600s時(shí)間段：

Vcn=0.3m/s， Vcu=0m/s， Vce=0.3m/s；

800～1200s時(shí)間段：

Vcn=0.5m/s， Vcu=0m/s， Vce=0.5m/s。

2.2 仿真結(jié)果及分析

仿真結(jié)果如圖3～5所示，可以得到如下結(jié)論：

①200～600s期間載體處于勻速直航狀態(tài)，800 ～1200s期間載體處于轉(zhuǎn)彎以及加速狀態(tài)。從洋流速度估計(jì)結(jié)果可以看出，洋流速度無(wú)論在勻速直航還是在加速或者轉(zhuǎn)彎條件下都能夠被準(zhǔn)確地估計(jì)出來(lái)，如圖3所示。

圖3 洋流速度估計(jì)曲線Fig.3 Current velocity estimation

圖4 未考慮洋流速度情況下組合導(dǎo)航定位誤差Fig.4 Navigation error when not taking into account the current velocity

3 湖面試驗(yàn)結(jié)果及分析

選取某水庫(kù)開(kāi)闊水面開(kāi)展船載試驗(yàn)。慣導(dǎo)系統(tǒng)選用零偏穩(wěn)定性 0.02(°)/h的光纖陀螺和零偏穩(wěn)定性80μg的石英撓性加速度計(jì)，選取的 RDI Workhorse Navigator DVL，標(biāo)稱精度為0.4%航程，工作頻率為300kHz，整個(gè)導(dǎo)航過(guò)程中DVL都處于底跟蹤模式，對(duì)準(zhǔn)時(shí)間6min。湖面試驗(yàn)所在水域水深較淺，平均水深只有34m，最大深度不到100m。因此，該試驗(yàn)環(huán)境無(wú)法對(duì)該SINS/DVL組合導(dǎo)航算法的進(jìn)行全面、充分的驗(yàn)證。

考慮到DVL在底跟蹤模式下可以同時(shí)輸出載體對(duì)底以及對(duì)水速度，假設(shè)在指定時(shí)間段內(nèi)DVL連續(xù)對(duì)底無(wú)效，組合導(dǎo)航系統(tǒng)使用DVL輸出的載體對(duì)水速度參與解算，基于該考慮洋流速度的SINS/DVL組合導(dǎo)航算法可以得到北向、東向洋流速度估計(jì)值，如圖6～7所示。

將引入洋流速度估計(jì)前后系統(tǒng)SINS/DVL組合導(dǎo)航的定位誤差進(jìn)行比較，對(duì)比結(jié)果如圖8～11所示。

圖5 考慮洋流速度情況下組合導(dǎo)航定位誤差Fig.5 Navigation error when taking into account the current velocity

圖6 北向洋流速度估計(jì)Fig.6 Northward current velocity estimation

圖7 東向洋流速度估計(jì)Fig.7 Eastward current velocity estimation

圖8 考慮洋流速度前后系統(tǒng)北向定位誤差對(duì)比Fig.8 Comparison on north positioning errors with/without considering the current velocity

圖9 考慮洋流速度前后系統(tǒng)東向定位誤差對(duì)比Fig.9 Comparison east positioning errors with/without considering the current velocity

圖10 系統(tǒng)綜合定位誤差對(duì)比Fig.10 Comparison on positioning errors with/without considering the current velocity

圖11 考慮洋流速度前后系統(tǒng)定位誤差對(duì)比Fig.11 Comparison on percentages of posi-tionning errors with/without considering the current velocity

從上述結(jié)果中可以看出，雖然試驗(yàn)所在水域DVL設(shè)定水流層的洋流速度只有0.02m/s左右，但在考慮洋流速度前后，系統(tǒng)定位誤差仍然有明顯區(qū)別。2500s至3100s連續(xù)對(duì)底無(wú)效期間，未考慮洋流速度的情況下，系統(tǒng)定位誤差增大了20m以上，而引入洋流速度估計(jì)后，系統(tǒng)定位誤差只增大了5m左右，組合導(dǎo)航精度由0.42%航程提高到0.24%航程。

4 結(jié) 論

針對(duì)SINS/DVL工作于水跟蹤模式條件下的高精度定位需求，提出一種考慮洋流影響的SINS/DVL組合導(dǎo)航算法。分析了DVL水跟蹤模式下洋流速度對(duì)組合導(dǎo)航精度的影響，基于速度匹配組合導(dǎo)航的方式提出了實(shí)時(shí)SINS/DVL組合導(dǎo)航系統(tǒng)設(shè)計(jì)方案。湖面試驗(yàn)結(jié)果表明，系統(tǒng)在6min初始對(duì)準(zhǔn)條件下SINS/DVL組合導(dǎo)航定位精度優(yōu)于0.24%D，考慮洋流影響的SINS/DVL組合導(dǎo)航算法能夠有效提高DVL僅對(duì)水速度有效情況下系統(tǒng)組合導(dǎo)航定位精度，該算法具有較好的工程適用性。

（

）：

[1] Paull L,Saeedi S,Seto M,et al.AUV navigation and localization:A review[J].IEEE Journal of Oceanic Engineering,2014,39(1):131-149.

[2] Morgado M,Oliveira P,Silvestre C.Tightly coupled ultrashort baseline and inertial navigation system for underwater vehicles:An experimental validation[J].Journal of Field Robotics,2013,30(1):142-170

[3] 陳俊博.SINS/DVL組合導(dǎo)航濾波算法研究[D].哈爾濱:哈爾濱工程大學(xué),2015.Chen J B.Research on SINS/DVL filtering method[D].Harbin:Harbin Engineering University,2015.

[4] 張濤,徐曉蘇,李瑤,等.基于慣導(dǎo)及水下聲學(xué)輔助系統(tǒng)的 AUV 容錯(cuò)導(dǎo)航技術(shù)[J].中國(guó)慣性技術(shù)學(xué)報(bào),2013,21(4):512-516.Zhang T,Xu X S,Li Y,et al.AUV fault-tolerant technology based on inertial navigation and underwater acoustics assisted navigation system[J].Journal of Chinese Inertial Technology,2013,21(4):512-516.

[5] 王磊.深海AUV多源導(dǎo)航信息融合方法研究[D].南京:東南大學(xué),2015.Wang L.Research on multi-source navigation information fusion method for deep-sea AUV[D].NanJing:Southeast University,2015.

[6] Sahu B K,Subudhi B.The state of art of autonomous underwater vehicles in current and future decades[C]//First International Conference on Auto-mation,Control,Energy and Systems.2014:1-6.

[7] Liu X X,Zhao Y,Liu Z P,et al.A novel self-alignment method for SINS based on parameter recognition and dual-velocity vectors[J].Proceedings of the Institution of Mechanical Engineers,Part G:Journal of Aerospace Engineering,2015,19(1):75-78.

[8] RD Instruments.Workhorse commands and output data format[M].P/N957-6156-00,March2005.

[9] 劉強(qiáng),徐曉蘇,許江寧.一種基于“當(dāng)前”統(tǒng)計(jì)模型的 DVL/SIMU 船位推算方法[J].中國(guó)慣性技術(shù)學(xué)報(bào),2012,20(4):450-454.Liu Q,Xu X S,Xu J N.DVL/SIMU dead-reckoning method based on“current”statistical model[J].Journal of Chinese Inertial Technology,2012,20(4):450-454.

[10] Wang L,Cheng X H,Li S X,et al.Adaptive interacting multiple model filter for AUV integrated navigation[J].Journal of Chinese Inertial Tech-nology,2016,24(4):511-515.

[11] RD Instruments.Navigator Doppler velocity log technical manual[M].P/N957-6023-00,October1997(Change1).

[12] 于玖成,何昆鵬,王曉雪.SINS/DVL 組合導(dǎo)航系統(tǒng)的標(biāo)定[J].智能系統(tǒng)學(xué)報(bào),2015,10(1):143-144.Yu J C,He K P,Wang X X.Calibration for strapdown inertial navigation/Doppler velocity log integrated navigation system[J].CAAI Transactions on Intelligent Systems,2015,10(1):143-144.

[13] Hodges B A,Fratantoni D M.AUV observations of the diurnal surface layer in the north Atlantic salinity maximum[J].Journal of Physical Oceanography,2014,44(6):1595-1604.