潘紹華，徐曉蘇，張 亮

（1.微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2.東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

自主水下航行器（Autonomous Underwater Vehicle,AUV）是水下資源勘探作業(yè)的基礎(chǔ)，其核心是導(dǎo)航定位技術(shù)[1]。慣性導(dǎo)航系統(tǒng)憑借其自主性、隱蔽性特點(diǎn)，成為了AUV導(dǎo)航定位的主要實(shí)現(xiàn)手段[2]，在短時(shí)間內(nèi)，慣性導(dǎo)航可以保持良好的導(dǎo)航精度，然而，隨著時(shí)間的延長，慣性導(dǎo)航的誤差積累非常明顯，導(dǎo)航精度迅速下降[2,3]。在水下電磁波快速衰減，無法長距離傳輸[4,5]，利用電磁波作為能量載體的全球?qū)Ш叫l(wèi)星系統(tǒng)和雷達(dá)等導(dǎo)航系統(tǒng)在水下使用困難。地球物理導(dǎo)航系統(tǒng)需要預(yù)先采集并建立相應(yīng)的導(dǎo)航數(shù)據(jù)庫[6,7]。而水聲定位具有傳播距離長、衰減損失小的特點(diǎn)?；诼暡ǘ嗥绽招?yīng)的原理的多普勒計(jì)程儀（Doppler Velocity Log,DVL）可以提供高精度的載體速度信息，其誤差不會(huì)隨著時(shí)間積累[1]。由此，捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System,SINS）與DVL的組合導(dǎo)航系統(tǒng)能夠完成高精度、長航時(shí)的導(dǎo)航任務(wù)，成為AUV的主流導(dǎo)航方式。

對(duì)于SINS/DVL組合系統(tǒng)，影響組合導(dǎo)航精度的因素很多，如安裝角誤差、桿臂誤差、DVL誤差等[8,9]。SINS和DVL固定后，安裝偏差角和桿臂相對(duì)穩(wěn)定，可以在航行前進(jìn)行校準(zhǔn)[10,11]。DVL有兩種工作模式，對(duì)水工作模式和對(duì)地工作模式，工作在水下未知、復(fù)雜環(huán)境中易受到干擾，測(cè)速不穩(wěn)定可能會(huì)發(fā)生數(shù)據(jù)跳變或數(shù)據(jù)丟失，部分波束失效容易出現(xiàn)野值；并且當(dāng)AUV航行過程中遇到深溝、淤泥等情況，即海底超過測(cè)量范圍或存在吸收信號(hào)的物質(zhì)時(shí)，DVL可能難以獲取反射信號(hào)，會(huì)導(dǎo)致短時(shí)失效[12]。在這些情況下，DVL的速度測(cè)量誤差可能很大。因此，在復(fù)雜海洋環(huán)境下，DVL誤差是影響組合導(dǎo)航精度的重要因素。

近年來，很多學(xué)者圍繞著DVL信息異常相關(guān)方面進(jìn)行了大量的研究。針對(duì)野值問題，朱兵[13]等提出一種基于馬氏距離算法的Huber魯棒自適應(yīng)卡爾曼濾波算法，根據(jù)量測(cè)噪聲特性對(duì)調(diào)節(jié)因子進(jìn)行實(shí)時(shí)的自適應(yīng)調(diào)整；黃玉龍[14]等提出一種基于學(xué)生t分布的魯棒卡爾曼濾波算法，將量測(cè)噪聲建模為學(xué)生t分布消除野值影響。針對(duì)DVL短時(shí)失效問題，Bo Wang[15]等提出一種基于支持向量機(jī)回歸算法建立DVL速度預(yù)測(cè)模型的方法，應(yīng)用優(yōu)化的網(wǎng)格搜索遺傳算法選擇支持向量回歸的最佳參數(shù)；Di Wang[1]等提出一種基于DS理論和最小二乘支持向量機(jī)的方法，通過DS理論對(duì)SINS和DVL進(jìn)行融合，通過最小二乘支持向量機(jī)進(jìn)行DVL速度預(yù)測(cè)。注意上述方法都是從單一方面考慮了DVL信息異常情況。事實(shí)上，由于水下環(huán)境未知、多變，DVL可能存在多種類型的異常信息，不具有普適性。因此，有必要建立能夠同時(shí)應(yīng)對(duì)多種DVL異常情況的處理方案。

針對(duì)上述問題，本文提出一種用于SINS/DVL組合導(dǎo)航系統(tǒng)的DVL異常信息處理機(jī)制。一方面，采用較為容易實(shí)現(xiàn)的滑動(dòng)窗卡方檢測(cè)方法，在檢測(cè)出野值的時(shí)候剔除此時(shí)觀測(cè)量，并采用固定長度的滑動(dòng)數(shù)據(jù)窗中存儲(chǔ)的觀測(cè)量序列平滑值替代此時(shí)刻的觀測(cè)量，繼續(xù)進(jìn)行組合濾波，消除野值對(duì)導(dǎo)航精度的影響；另一方面，采用訓(xùn)練效率更高的相關(guān)向量機(jī)算法建立了DVL速度回歸預(yù)測(cè)模型，采用具有良好局部插值能力的高斯核函數(shù)作為模型核函數(shù)，通過貝葉斯聯(lián)合后驗(yàn)估計(jì)來選擇相關(guān)向量機(jī)的最佳參數(shù)，在DVL短時(shí)失效時(shí)依舊保持較高精度導(dǎo)航。

本文建立了SINS/DVL組合導(dǎo)航系統(tǒng)濾波模型；分別提出了基于滑動(dòng)窗卡方檢測(cè)的野值處理方法和基于相關(guān)向量機(jī)的虛擬DVL構(gòu)建方法以及DVL異常信息處理機(jī)制工作方式；進(jìn)行了仿真和長江試驗(yàn)，驗(yàn)證所提出機(jī)制的有效性。

1 SINS/DVL組合導(dǎo)航系統(tǒng)濾波模型

在AUV水下導(dǎo)航定位中，SINS為主導(dǎo)航系統(tǒng)，DVL測(cè)量的速度信息為外部輔助信息。采用間接濾波法，將SINS解算得到的導(dǎo)航坐標(biāo)系n系速度轉(zhuǎn)換到載體坐標(biāo)系b系下，與DVL測(cè)量的b系速度（已進(jìn)行標(biāo)定補(bǔ)償，下文也是同樣表述）做差作為量測(cè)量進(jìn)行導(dǎo)航濾波，實(shí)現(xiàn)對(duì)SINS解算的導(dǎo)航信息實(shí)時(shí)修正，保證AUV的長航時(shí)、高精度導(dǎo)航。

2 DVL異常信息處理機(jī)制

2.1 基于滑動(dòng)窗卡方檢測(cè)的野值處理方法

圖1 滑動(dòng)窗卡方檢測(cè)示意圖Fig.1 Chi-square detection of sliding window

2.2 基于相關(guān)向量機(jī)的虛擬DVL構(gòu)建方法

目前，DVL失效的應(yīng)對(duì)方法主要有隔離法和替代法[16]。本文采用替代法，考慮到傳統(tǒng)的預(yù)測(cè)方法，如支持向量機(jī)，核函數(shù)必須滿足正定，使用核函數(shù)數(shù)量隨著訓(xùn)練集的增長急劇增大，并且訓(xùn)練結(jié)果對(duì)于邊界參數(shù)敏感，需要大量的測(cè)試以達(dá)到較好結(jié)果[17]；神經(jīng)網(wǎng)絡(luò)容易學(xué)習(xí)過度，無法實(shí)現(xiàn)全局最優(yōu)[18]等，本文選擇基于稀疏貝葉斯理論的相關(guān)向量機(jī)回歸模型建立DVL速度預(yù)測(cè)模型，作為虛擬DVL應(yīng)對(duì)DVL短時(shí)失效?？紤]到SINS解算的速度與DVL輸出的速度有一定的關(guān)系，并且SINS可靠性相對(duì)高于DVL，本文在SINS解算速度與DVL輸出速度之間建立相關(guān)向量機(jī)（Relevance Vector Machine,RVM）模型。

當(dāng)DVL正常工作時(shí)，將SINS解算速度作為預(yù)測(cè)模型的訓(xùn)練輸入，訓(xùn)練目標(biāo)輸出為同時(shí)刻DVL的速度測(cè)量值，采用RVM回歸算法建立DVL輸出的預(yù)測(cè)模型。而在DVL異常時(shí)，此虛擬DVL可以基于SINS解算的速度信息輸出預(yù)測(cè)的DVL速度信息，實(shí)現(xiàn)連續(xù)導(dǎo)航，避免定位誤差積累。

如果直接用最大化似然值的方法，可能的問題是會(huì)產(chǎn)生過多的支持向量從而導(dǎo)致過擬合（Over-Fitting）。為避免該RVM模型出現(xiàn)過擬合現(xiàn)象，通過在RVM預(yù)測(cè)模型中加入超參數(shù)，為權(quán)重向量ω引入一個(gè)先驗(yàn)概率分布[20]。超參數(shù)滿足：

重復(fù)迭代計(jì)算直至達(dá)到收斂標(biāo)準(zhǔn)或達(dá)到最大循環(huán)次數(shù)而停止，得到基于稀疏貝葉斯理論的RVM模型。如果*x為已知的新的輸入時(shí)間序列，為與之相對(duì)應(yīng)的時(shí)間序列預(yù)測(cè)目標(biāo)，那么相應(yīng)的預(yù)測(cè)函數(shù)為：

2.3 DVL異常信息處理機(jī)制實(shí)現(xiàn)方案

在本節(jié)中，將給出本文所提出方案的具體實(shí)施過程，如圖2所示。

圖2 DVL異常信息處理機(jī)制工作流程圖Fig.2 The working flow chart of DVL exception information processing mechanism

首先進(jìn)行初始對(duì)準(zhǔn)[21]，完成后通過IMU數(shù)據(jù)和DVL數(shù)據(jù)進(jìn)行導(dǎo)航過程。此時(shí)保存SINS解算速度信息與DVL輸出信息進(jìn)行RVM模型訓(xùn)練。當(dāng)DVL檢測(cè)到失效時(shí)用虛擬DVL模型預(yù)測(cè)結(jié)果代替失效DVL信息進(jìn)行組合導(dǎo)航，沒有失效則正常導(dǎo)航。將DVL測(cè)量信息進(jìn)行卡方檢測(cè)，判定為野值則剔除并通過量測(cè)滑動(dòng)窗處理，沒有野值則正常導(dǎo)航。通過濾波結(jié)果校正SINS輸出，保持組合導(dǎo)航系統(tǒng)定位精度，直至完成導(dǎo)航過程。

3 仿真與試驗(yàn)驗(yàn)證

為了驗(yàn)證本文所提出的DVL異常信息處理機(jī)制的有效性，本節(jié)將所提出算法（簡寫為X2-RVM-kf，下文同樣這樣做）與失效時(shí)僅隔離DVL（kf）、通過RVM構(gòu)建虛擬DVL但未處理野值（RVM-kf）、通過SVR構(gòu)建虛擬DVL并處理野值（X2-SVR-kf）這三種算法進(jìn)行對(duì)比，并采用RMSE作為性能評(píng)價(jià)指標(biāo)。分別進(jìn)行了仿真實(shí)驗(yàn)和長江試驗(yàn)驗(yàn)證。

3.1 仿真實(shí)驗(yàn)與分析

為了實(shí)施本文所提出的算法，在接下來的仿真中以MATLAB R2020b作為實(shí)現(xiàn)該技術(shù)方案的軟件工具，將含有野值的量測(cè)噪聲建模為：

表1 傳感器參數(shù)Tab.1 Sensor parameters

考慮到RVM和SVR模型都是單輸出的工作方式，將三維速度信息分別訓(xùn)練為三個(gè)不相關(guān)的模型，進(jìn)行每一維的速度預(yù)測(cè)。RVM模型設(shè)置核函數(shù)寬度因子為0.4、0.1、0.02。SVR模型設(shè)置懲罰因子和核函數(shù)參數(shù)搜索范圍分別為[2-10，210]、[2-10，210]，其不敏感系數(shù)設(shè)置為0.015，交叉驗(yàn)證折數(shù)5V=，不進(jìn)行歸一化處理。

通過停止DVL的輸出僅采用純慣性系統(tǒng)進(jìn)行導(dǎo)航來模擬DVL短時(shí)失效的情況，失效時(shí)間設(shè)置為500 s~650 s共150 s的時(shí)間。導(dǎo)航過程中收集200 s~500 s時(shí)SINS解算的速度與DVL輸出的速度作為訓(xùn)練樣本，在500 s~650 s DVL異常時(shí)使用SINS解算的速度利用RVM構(gòu)建的虛擬DVL進(jìn)行速度預(yù)測(cè)。仿真軌跡如圖3所示，星標(biāo)為初始位置。

圖3 仿真軌跡Fig.3 Trajectory of simulation

圖4-5給出了DVL失效時(shí)僅隔離DVL（kf、藍(lán)色）、DVL失效時(shí)采用RVM模型但未處理DVL野值（RVM-kf、綠色）、通過SVR構(gòu)建虛擬DVL并處理野值（X2-SVR-kf、黑色）和本文所提出的DVL異常信息處理機(jī)制（X2-RVM-kf、紅色）四種方法的速度誤差和位置誤差，可以看出，本文所提出的DVL異常信息處理機(jī)制能夠獲得相對(duì)最好的定位精度，通過SVR構(gòu)建虛擬DVL并處理野值次之，采用RVM模型但未處理DVL野值方法有較大誤差，僅隔離DVL方法效果最差。

圖4 仿真速度誤差Fig.4 Velocity error of simulation

圖5 仿真位置誤差Fig.5 Position error of simulation

在500 s~650 s這段時(shí)間，由于DVL失效，僅隔離DVL的方法相當(dāng)于只采用純慣性工作模式進(jìn)行導(dǎo)航，會(huì)導(dǎo)致SINS誤差積累，精度下降嚴(yán)重。而DVL失效時(shí)采用RVM模型但未處理DVL野值方法、通過支持向量機(jī)（Support Vector Regression,SVR）構(gòu)建虛擬DVL并處理野值方法和本文提出方法都保持了較高的精度，說明構(gòu)建虛擬DVL模型進(jìn)行速度輸出可以有效應(yīng)對(duì)DVL失效的情況。其中可以明顯看出，在速度預(yù)測(cè)的準(zhǔn)確度上，本文所采用的RVM模型有更好的效果。但是在650 s以后，DVL失效時(shí)采用RVM模型但未處理野值的方法在使用原始包含野值的DVL數(shù)據(jù)情況下和僅隔離DVL呈現(xiàn)一樣的定位誤差變大的特性，說明僅考慮虛擬DVL進(jìn)行速度預(yù)測(cè)無法實(shí)現(xiàn)對(duì)野值的抗差特性，伴隨全程的DVL測(cè)量野值也會(huì)使系統(tǒng)的量測(cè)噪聲呈現(xiàn)厚尾分布，不再滿足卡爾曼濾波量測(cè)噪聲為零均值高斯白噪聲的要求，從而導(dǎo)致濾波精度下降，而本文所提算法通過滑動(dòng)窗卡方檢測(cè)，可以實(shí)現(xiàn)對(duì)野值的有效處理。

求取四種方法進(jìn)行定位的RMSE，得到圖6?？梢钥闯?，本文所提出方法RMSE最小，能有效應(yīng)對(duì)DVL異常問題。通過求取每種方法x、y、z方向的RMSE的和進(jìn)行比較來衡量定位精度好壞，其中本文所提出方法較DVL失效時(shí)通過SVR構(gòu)建虛擬DVL并處理野值提升了22.8%的定位精度，較DVL失效時(shí)采用RVM模型但未處理DVL野值提升了14.1%的定位精度，較僅隔離DVL提升了36.9%的定位精度。綜上，本文所提出的DVL異常信息處理機(jī)制全方位考慮到了DVL可能遇到的短時(shí)失效和伴隨野值的異常情況。

圖6 仿真定位RMSEFig.6 RMSE of the simulated position errors

3.2 長江試驗(yàn)分析

為了驗(yàn)證所提出的DVL異常信息處理機(jī)制的有效性，在長江進(jìn)行了試驗(yàn)。

測(cè)試傳感器包括RTK GPS、IMU、DVL和PHINS。IMU和RTK固定在船上，如圖7左側(cè)所示。RTK的精度達(dá)到厘米級(jí)，因此采用RTK GPS測(cè)量船舶的航跡作為算法驗(yàn)證的地面真值。DVL實(shí)驗(yàn)裝置如圖7右側(cè)所示，其中，SINS與DVL設(shè)備的安裝誤差角、桿臂和DVL刻度因子誤差已經(jīng)提前標(biāo)定好，SINS已經(jīng)完成初始對(duì)準(zhǔn)，傳感器參數(shù)同仿真，SINS和DVL的更新頻率為200 Hz和1 Hz，設(shè)置DVL失效時(shí)間為500 s-650 s共150 s的時(shí)間。根據(jù)得到的IMU數(shù)據(jù)和DVL實(shí)測(cè)數(shù)據(jù)，分別通過四種方法進(jìn)行導(dǎo)航解算與濾波過程。RVM模型設(shè)置核函數(shù)寬度因子為0.6、0.6、0.6，SVR模型設(shè)置不敏感系數(shù)為0.1，其他相關(guān)參數(shù)設(shè)置同仿真。試驗(yàn)軌跡如圖8所示，得到如圖9-10所示的速度誤差和位置誤差，測(cè)量船前進(jìn)方向的DVL測(cè)量值經(jīng)過滑動(dòng)窗卡方檢測(cè)處理后的效果如圖11所示，測(cè)量船前進(jìn)方向的虛擬DVL預(yù)測(cè)結(jié)果如圖12所示，四種方法的定位RMSE在圖13中列出。

圖7 長江試驗(yàn)裝置Fig.7 Device for the Yangtze River test

圖8 長江試驗(yàn)軌跡Fig.8 The trajectory of the Yangtze River test

圖9 長江試驗(yàn)速度誤差Fig.9 Velocity error of the Yangtze River test

圖11 虛擬DVL預(yù)測(cè)結(jié)果Fig.11 The prediction results of virtual DVL

圖12 卡方檢測(cè)效果圖Fig.12 The effect of the Chi-square test

圖13 長江試驗(yàn)定位RMSEFig.13 RMSE of the Yangtze River test

圖10 長江試驗(yàn)位置誤差Fig.10 Position error of the Yangtze River test

由圖9-10可知，在DVL出現(xiàn)故障短時(shí)失效期間，同仿真一樣，RVM預(yù)測(cè)模型和SVR預(yù)測(cè)模型所構(gòu)建的虛擬DVL能夠有效替代真實(shí)DVL輸出較為準(zhǔn)確的速度信息參加導(dǎo)航濾波，保持組合系統(tǒng)正常工作，抑制SINS的誤差積累。

圖11將y方向的DVL實(shí)際輸出（藍(lán)色）、SVR預(yù)測(cè)輸出（黑色）與本文采用的RVM預(yù)測(cè)輸出（紅色）進(jìn)行了對(duì)比，可以看出RVM模型速度輸出與DVL實(shí)際輸出更為接近，預(yù)測(cè)效果更好。另外，SVR預(yù)測(cè)時(shí)間約2 min/次，而RVM模型預(yù)測(cè)時(shí)間約2 s/次，RVM預(yù)測(cè)模型耗時(shí)極短，預(yù)測(cè)效率相對(duì)SVR模型提高約98.3%。當(dāng)實(shí)際工程中需對(duì)x、y、z多個(gè)方向進(jìn)行及時(shí)預(yù)測(cè)時(shí)，短時(shí)高效的預(yù)測(cè)模型可以大大節(jié)省時(shí)間成本，提高組合導(dǎo)航效率和精度。

另外，在試驗(yàn)中，野值對(duì)于系統(tǒng)定位精度的影響非常大，特別是在800 s之后，野值較為密集，由此未對(duì)野值進(jìn)行處理的僅隔離DVL方法、DVL失效時(shí)采用RVM模型但未處理DVL野值方法在這段時(shí)間內(nèi)定位誤差迅速增大。圖12展示了y方向的DVL原始數(shù)據(jù)與經(jīng)過卡方檢測(cè)處理后的DVL數(shù)據(jù)的對(duì)比，可以看到，本文所提出的野值處理方法能夠基本過濾掉異常值。綜上，本文所提出的DVL異常信息處理機(jī)制，綜合地考慮了AUV航行過程中的野值問題和短時(shí)失效問題，較DVL失效時(shí)通過SVR構(gòu)建虛擬DVL并處理野值提升了22.6%的定位精度，較DVL失效時(shí)采用RVM模型但未處理DVL野值提升了67.3%的定位精度，較僅隔離DVL提升了68.8%的定位精度。

目前針對(duì)DVL短時(shí)失效時(shí)通過人工智能算法構(gòu)建虛擬DVL的研究還不是很多，并且本文中通過尋求SINS解算速度與DVL輸出速度之間的聯(lián)系的方法，可以進(jìn)行訓(xùn)練的樣本特征過少，由此本文仍有一定局限性：當(dāng)AUV運(yùn)動(dòng)狀態(tài)發(fā)生改變時(shí)，RVM預(yù)測(cè)模型的預(yù)測(cè)精度將會(huì)降低。這對(duì)于1-5 min短時(shí)失效來說，影響是很小的。但是當(dāng)DVL輸出長時(shí)間沒有恢復(fù)時(shí)，虛擬DVL的有效性就需要進(jìn)行考量。由此考慮通過IMU的輸出來判斷預(yù)測(cè)結(jié)果是否有效：當(dāng)加速度或角速度輸出連續(xù)超過設(shè)定的閾值時(shí)，判斷AUV發(fā)生較大的運(yùn)動(dòng)狀態(tài)改變。此時(shí)建議AUV僅采用純慣性模式進(jìn)行工作，并采用動(dòng)態(tài)零速修正的方法重新獲取準(zhǔn)確的定位信息。

4 結(jié)論

SINS/DVL組合導(dǎo)航算法是水下航行器的主要定位算法之一，對(duì)DVL的定位精度要求較高。針對(duì)復(fù)雜、未知環(huán)境下DVL測(cè)量信息可靠性降低的問題，本文提出了一種DVL異常信息處理機(jī)制。采用滑動(dòng)窗卡方檢測(cè)算法，有效消除了野值對(duì)導(dǎo)航精度的影響。為了避免由于預(yù)測(cè)精度不足造成的錯(cuò)誤補(bǔ)償，采用了高精度、高效率的RVM算法，建立了DVL速度預(yù)測(cè)模型應(yīng)對(duì)DVL短時(shí)失效問題。將所提出的方法與DVL失效時(shí)僅隔離DVL、采用RVM模型但未處理DVL野值、采用SVR模型并處理野值進(jìn)行了對(duì)比，仿真結(jié)果表明提出的DVL異常信息處理機(jī)制能夠在DVL短時(shí)失效時(shí)準(zhǔn)確預(yù)測(cè)速度信息并且基本消除野值對(duì)定位精度的影響。最后通過長江試驗(yàn)驗(yàn)證，該方法在惡劣的水下環(huán)境下能保持較高的定位精度。綜上所述，該定位方法具有良好的容錯(cuò)性和定位精度，具有一定的應(yīng)用價(jià)值。