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

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

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

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

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

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

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

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

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

DVL測速誤差主要與聲速誤差、載體姿態(tài)誤差等有關(guān)[12-13]?？紤]到這些誤差在實(shí)際產(chǎn)品中已經(jīng)經(jīng)過補(bǔ)償，現(xiàn)只考慮 DVL刻度因子誤差以及安裝角誤差。由于水平方向上的兩個安裝角誤差對系統(tǒng)組合導(dǎo)航定位精度的影響相對較小，所以可以忽略水平安裝角誤差的影響，并且可將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；北向、東向位置誤差δφ、δλ；載體系各軸向的陀螺漂移；加速度計零偏。選取DVL刻度因子誤差δkD、航向安裝角誤差β以及北向、東向洋流速度誤差分量δVcn、 δVce為子系統(tǒng)DVL的狀態(tài)量?？梢缘玫絊INS/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相對于自身測量坐標(biāo)系的速度在導(dǎo)航系（n系）下的投影為，其為 DVL測得載體在儀器系（d系）下的真實(shí)速度，儀器系到載體系（b系）姿態(tài)轉(zhuǎn)換矩陣，為載體系到導(dǎo)航系的姿態(tài)轉(zhuǎn)換矩陣。

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

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

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

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

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

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

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è)置

仿真總時間： t=2000s；

轉(zhuǎn)彎時間：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°，滾動角γ=0°；初始姿態(tài)誤差：δψ=0.1°，δθ=0.01°，δγ=-0.01°。

②慣性器件設(shè)置

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

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

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

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

DVL測速采樣周期：T=1s；

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

DVL 僅對水有效時間段：200～600s，800～1200s。

200～600s時間段：

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

800～1200s時間段：

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)。從洋流速度估計結(jié)果可以看出，洋流速度無論在勻速直航還是在加速或者轉(zhuǎn)彎條件下都能夠被準(zhǔn)確地估計出來，如圖3所示。

圖3 洋流速度估計曲線Fig.3 Current velocity estimation

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

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

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

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

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

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

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

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

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

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

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

圖11 考慮洋流速度前后系統(tǒng)定位誤差對比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ù)對底無效期間，未考慮洋流速度的情況下，系統(tǒng)定位誤差增大了20m以上，而引入洋流速度估計后，系統(tǒng)定位誤差只增大了5m左右，組合導(dǎo)航精度由0.42%航程提高到0.24%航程。

4 結(jié) 論

針對SINS/DVL工作于水跟蹤模式條件下的高精度定位需求，提出一種考慮洋流影響的SINS/DVL組合導(dǎo)航算法。分析了DVL水跟蹤模式下洋流速度對組合導(dǎo)航精度的影響，基于速度匹配組合導(dǎo)航的方式提出了實(shí)時SINS/DVL組合導(dǎo)航系統(tǒng)設(shè)計方案。湖面試驗(yàn)結(jié)果表明，系統(tǒng)在6min初始對準(zhǔn)條件下SINS/DVL組合導(dǎo)航定位精度優(yōu)于0.24%D，考慮洋流影響的SINS/DVL組合導(dǎo)航算法能夠有效提高DVL僅對水速度有效情況下系統(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 容錯導(dǎo)航技術(shù)[J].中國慣性技術(shù)學(xué)報,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)計模型的 DVL/SIMU 船位推算方法[J].中國慣性技術(shù)學(xué)報,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é)報,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.