王振杰，劉慧敏，楊慧良，賀凱飛，單 瑞

（1.中國石油大學(xué)（華東），青島 266580；2.青島海洋科學(xué)與技術(shù)國家實(shí)驗(yàn)室 海洋礦產(chǎn)資源評(píng)價(jià)與探測(cè)技術(shù)功能實(shí)驗(yàn)室，青島 266071；3.中國地質(zhì)調(diào)查局青島海洋地質(zhì)研究所，青島 266071）

深海拖曳系統(tǒng)（Underwater Towed System,UTS）由母船通過長纜提供動(dòng)力，廣泛應(yīng)用于深?？茖W(xué)研究以及海底調(diào)查，例如，測(cè)量海底地形測(cè)量及海底底質(zhì)調(diào)查等[1]。實(shí)時(shí)高精度的動(dòng)態(tài)導(dǎo)航定位是其進(jìn)行近海底、遠(yuǎn)距離、長時(shí)間工作的重要保障[2]。常見的深海拖曳系統(tǒng)導(dǎo)航傳感器包括光纖羅經(jīng)、壓力傳感器、多普勒計(jì)程儀（Doppler Velocity Log,DVL）以及超短基線定位系統(tǒng)（Ultra-Short Baseline System,USBL）等，可提供運(yùn)載體的姿態(tài)和方位信息、深度信息、速度信息和位置信息等。超短基線定位系統(tǒng)可為拖曳系統(tǒng)在淺水區(qū)域提供高精度位置信息，但受姿態(tài)測(cè)量精度、時(shí)延和聲速誤差的影響，其精度隨著拖曳系統(tǒng)與母船的距離增加而降低。拖曳體下潛至5000 m左右的深水中時(shí)，超短基線定位系統(tǒng)的定位精度甚至不足10 m[2]。拖曳系統(tǒng)貼近海底進(jìn)行工作時(shí)，多普勒計(jì)程儀可提供載體相對(duì)海底的速度信息，結(jié)合載體姿態(tài)傳感器以航跡推算的方式求解地理坐標(biāo)，因測(cè)速誤差的影響，其位置誤差隨時(shí)間累積。拖曳系統(tǒng)在深海的工作環(huán)境復(fù)雜，單一的導(dǎo)航系統(tǒng)通常存在各種各樣的局限性，需要多種傳感器的導(dǎo)航信息密切配合。

采用RDOA（Range Difference of Arrival）模式定位的USBL定位誤差隨斜距增長而增加，在深海中USBL定位系統(tǒng)的測(cè)距精度要明顯優(yōu)于其定位精度[3]。校準(zhǔn)后的壓力傳感器的測(cè)深精度為米級(jí)，因此可利用拖曳系統(tǒng)上的壓力傳感器測(cè)得的深度信息、USBL換能器實(shí)時(shí)吃水信息與超短基線測(cè)距信息進(jìn)行耦合，以提高超短基線定位系統(tǒng)的水平定位精度。

為合理利用多源導(dǎo)航信息，常采用線性的卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合；對(duì)于非線性模型，則多采用基于線性近似的擴(kuò)展卡爾曼濾波算法和基于確定性采樣的無跡卡爾曼濾波算法[4]。本文基于自適應(yīng)抗差濾波算法思想[5]和深度約束信息設(shè)計(jì)了一種適于深海拖體長時(shí)間、長距離觀測(cè)的自適應(yīng)抗差無跡卡爾曼濾波算法。該濾波算法可以利用深度約束信息提高濾波精度，同時(shí)通過新息向量判別定位數(shù)據(jù)中的粗差觀測(cè)，提高了濾波算法的魯棒性。

1 深海拖曳定位系統(tǒng)的深度傳感器信息獲取

如圖1 所示，深海拖曳系統(tǒng)的導(dǎo)航傳感器包括安裝在母船的 GPS及姿態(tài)傳感器和安裝在拖體上的超短基線定位系統(tǒng)、深度計(jì)、多普勒計(jì)程儀、光纖羅經(jīng)等。GPS天線可以獲得母船的絕對(duì)坐標(biāo)，并通過姿態(tài)傳感器得到USBL換能器基陣坐標(biāo)。換能器基陣使用多個(gè)（3個(gè)以上）測(cè)量單元，測(cè)量信標(biāo)聲信號(hào)到達(dá)各單元的時(shí)間差及聲信號(hào)延遲，得到信標(biāo)相對(duì)于換能器的相對(duì)坐標(biāo)。

壓力傳感器測(cè)量拖曳系統(tǒng)到瞬時(shí)平均海面的垂直距離，由于其性能穩(wěn)定且精度較高，可視為同GPS一樣穩(wěn)定可靠的導(dǎo)航信息源。如果拖曳系統(tǒng)接近海底，多普勒計(jì)程儀可以測(cè)量拖體相對(duì)海底的速度信息，結(jié)合姿態(tài)信息可得到其航跡推算位置。多普勒測(cè)速信息以及壓力傳感器測(cè)得的深度信息都可以有效地提高深海超短基線的定位精度。下面將詳細(xì)介紹這幾種導(dǎo)航數(shù)據(jù)的組合導(dǎo)航原理。

圖1 基于USBL/DVL/GPS的拖曳導(dǎo)航系統(tǒng)示意圖Fig.1 Deployment of USBL and geometrical relationship with GPS and DVL

1.1 無波浪影響的深度信息獲取

由于海面波浪、潮汐及其他因素的影響，實(shí)時(shí)海平面在不斷的變化。在淺水區(qū)，可假設(shè)由壓力傳感器測(cè)量得到的到海平面的垂直距離Di為[6]：

定義換能器瞬時(shí)高程為Hi可由 GPS天線求得，瞬時(shí)海面高程為Zi，且有Zi=Hi+Hd，其中，應(yīng)答器的動(dòng)態(tài)吃水改正為Hd。通常，潮汐的周期長達(dá)數(shù)小時(shí)，短時(shí)間內(nèi)潮汐對(duì)動(dòng)態(tài)吃水的高程影響可以暫不考慮。波浪的周期僅有數(shù)秒至幾分鐘，因而波浪對(duì)USBL換能器造成的動(dòng)態(tài)吃水影響可以通過取多個(gè)歷元的平均值或其他平滑算法盡量消除，此時(shí)的瞬時(shí)海面高程可表示為：

如圖2(a) 所示，采用2000s的船測(cè)GPS數(shù)據(jù)，并通過姿態(tài)改正和吃水改正后得到瞬時(shí)海平面的高程。利用所有的觀測(cè)數(shù)據(jù)并使用經(jīng)驗(yàn)?zāi)B(tài)分解（Empirical Mode Decomposition,EMD）去除高頻分量，并作為離線解算方案。與式(2)計(jì)算的瞬時(shí)海面高程對(duì)比，二者差距僅為數(shù)厘米。由圖2(b)所知，深海中載體溫鹽深（Conductivity Temperature Depth,CTD）傳感器測(cè)量得到的深度值比較平滑。此外 DVL測(cè)量的速度值在垂向上的積分在短時(shí)間內(nèi)有較好的結(jié)果，但隨著時(shí)間的累積容易發(fā)散。壓力傳感器可得到平滑的瞬時(shí)平均海平面，校準(zhǔn)后精度達(dá)數(shù)厘米；而USBL換能器到瞬時(shí)平均海平面的距離可以通過式(2)粗略估計(jì)，二者之間的差值即為本文討論的垂直約束信息。

圖2 (a) 換能器到大地水準(zhǔn)面的距離Fig.2 (a) Vertical distance from USBL transducer to the mean sea level

圖2 (b) CTD測(cè)得及DVL垂向積分的深度Fig.2 (b) Depth from CTD and DVL

1.2 深度約束提高USBL定位精度的幾何解釋

超短基線定位精度多與斜距有關(guān)，USBL的測(cè)距誤差精度優(yōu)于米級(jí)，測(cè)角精度不夠往往是水平定位誤差的主因。假設(shè)其精度為 1‰～5‰，斜距為 6000 m時(shí)，USBL的定位精度為6～30 m。可結(jié)合垂直深度約束信息和高精度斜距信息給出可靠的水平約束。本節(jié)給出了二者耦合的幾何原理。

為研究垂直約束對(duì)水平方向精度的影響，先不考慮測(cè)距誤差對(duì)垂直約束的影響。如圖3所示，在X-Z平面中，點(diǎn)A為換能器基陣的位置，點(diǎn)C和D分別為測(cè)量和真實(shí)信標(biāo)位置，則由測(cè)角誤差dθ引起的定位誤差在X-Z平面的投影為CD，深海中其值可達(dá)數(shù)十米。此時(shí)USBL得到的高程誤差值為dh，深度約束改正即將C投影到E點(diǎn)。易得在X-Z方向上提高的精度為CD減DE。

圖3 垂直約束條件下超短基線信標(biāo)與基陣的幾何在垂直面的關(guān)系Fig.3 Geometrical relationship between USBL and target in X-Z

母船沿直線航行時(shí)，水下拖體通常在水平方向上近似沿直線運(yùn)動(dòng)。拖體在拖體坐標(biāo)系和導(dǎo)航坐標(biāo)系下的速度可表示為

其中，[X,Y]和[εx,εy]分別是載體坐標(biāo)系下的位置和誤差。因此在水平直線航行的條件下，根據(jù)航向信息，通過式(3)可轉(zhuǎn)化成載體坐標(biāo)系下的坐標(biāo)，通過選取一段時(shí)間的觀測(cè)值，對(duì)拖體的垂直運(yùn)動(dòng)方向上的位置進(jìn)行平滑，可明顯提高其定位精度。上述方法更可作為無DVL觀測(cè)信息時(shí)的重要補(bǔ)充。

2 自適應(yīng)抗差無跡卡爾曼濾波

由于聲信號(hào)在海水中的不穩(wěn)定性，USBL和DVL觀測(cè)值中往往存在大量的粗差觀測(cè)。本節(jié)使用自適應(yīng)抗差無跡卡爾曼濾波（Adaptive Robust Unscented Kalman Filtering,ARUKF）對(duì)USBL、DVL和深度信息進(jìn)行融合。采用抗差I(lǐng)GG-3方案對(duì)粗差觀測(cè)進(jìn)行判定和處理，保證濾波算法在長時(shí)間觀測(cè)下的穩(wěn)定性。

2.1 狀態(tài)方程和量測(cè)方程定義

定義狀態(tài)模型參數(shù)：

其中，xn(k)、xe(k)、ψ分別為北、東方向的位置和航向角，vf(k)和vs(k)為載體坐標(biāo)系下的速度。非機(jī)動(dòng)情況下的狀態(tài)方程可簡化為[7]：

其中，ε表示過程噪聲，ΔT為采樣間隔。觀測(cè)向量定義為且

Ya(k)由 USBL在導(dǎo)航坐標(biāo)系下的位置測(cè)量值(xusbl,n(k),xusbl,e(k))、DVL在載體坐標(biāo)系下的速度測(cè)量值(vdvl,f(k),vdvl,s(k))以及航向角ψgro組成。深度約束信息可寫為：

2.2 自適應(yīng)抗差無跡卡爾曼濾波（ARUKF）

無跡卡爾曼濾波通過無跡變換（Unscented Transform,UT）給出的確定性采樣，得到非線性變換后變量的均值和方差信息，理論上可以達(dá)到二階以上的擴(kuò)展卡爾曼濾波（EKF）精度。假設(shè)非線性方程為y=g(x)，如果已知變量x的均值和協(xié)方差矩陣Px，無跡變換過程如下[8-12]：

其中，χi表示采樣點(diǎn)，n最小必要采樣數(shù)，而λ可影響采樣點(diǎn)到x的距離。點(diǎn)γi=g(χi)(i= 1,,2n)被依次計(jì)算，要估計(jì)的g(x)的均值及協(xié)方差矩陣Py和相應(yīng)的權(quán)值及可快速求出。

自適應(yīng)無跡卡爾曼濾波主要分為以下四個(gè)步驟：

1）初始化（計(jì)算初始狀態(tài)及其協(xié)方差陣）

2）生成采樣點(diǎn)

3）時(shí)間更新

4）量測(cè)更新

本文主要通過新息序列和等價(jià)權(quán)函數(shù)來消除粗差觀測(cè)值帶來的影響。通過粗差對(duì)新息序列的異常影響對(duì)粗差進(jìn)行識(shí)別，并通過量測(cè)矩陣的膨脹因子處理粗差觀測(cè)值。具體過程如下：

根據(jù)式(28)，膨脹因子?k被分為正常、可能正常、異常三種情況，其中c0和c1可根據(jù)正常穩(wěn)定的觀測(cè)情況下的標(biāo)準(zhǔn)化殘差數(shù)量級(jí)給出。修正后的觀測(cè)值的協(xié)方差矩陣為：

在深海中，非線性情況并不突出，但拖體上浮過程中，垂直約束方程的非線性程度會(huì)變強(qiáng)。而 UKF算法并不需要直接解算雅克比矩陣，能方便得到非線性方程的統(tǒng)計(jì)信息，在非線性較強(qiáng)的系統(tǒng)中明顯優(yōu)于一階EKF濾波算法。當(dāng)觀測(cè)值中存在粗差觀測(cè)值時(shí)，信息向量一般會(huì)出現(xiàn)異常，此時(shí)通過增大觀測(cè)噪聲矩陣使得濾波器拒絕該觀測(cè)信息，僅從狀態(tài)方程和其他觀測(cè)信息預(yù)測(cè)出該時(shí)刻的狀態(tài)信息，從而提高濾波器的容錯(cuò)性。

3 實(shí)驗(yàn)分析

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

首先通過仿真實(shí)驗(yàn)對(duì)基于深度約束的 ARUKF算法的定位精度和穩(wěn)健性進(jìn)行分析。仿真水深為5000 m，拖體以3 m/s 的常速度航行。導(dǎo)航傳感器測(cè)量誤差見表1。

表1 USBL/DVL/Depth/Attitude仿真參數(shù)Tab.1 Simulation parameters of USBL/DVL/Depth/attitude

表1中，R表示隨機(jī)噪聲，S表示系統(tǒng)偏差。USBL基陣到信標(biāo)的斜距為Ra，其測(cè)量精度為Ra/5000。DVL測(cè)量拖體在載體坐標(biāo)系下的速度。母船和拖曳系統(tǒng)按圖5的整齊的巡回模型航行。

使用30000個(gè)歷元的直線觀測(cè)值對(duì)平滑算法的性能進(jìn)行測(cè)試，給出原始USBL結(jié)果、深度約束（USBL&DEPTH）的結(jié)果和MEDFILT平滑方案。MEDFILT方案是采用10個(gè)歷元數(shù)據(jù)的開窗平滑算法（MEDFILT

圖4 水下拖曳系統(tǒng)的軌跡示意圖Fig.4 Trajectory of underwater towed system

在本文指中值濾波方法）。MEDFILT方案首先將載體深度約束后的位置轉(zhuǎn)換成載體運(yùn)動(dòng)坐標(biāo)系，并在垂直于運(yùn)動(dòng)方向上進(jìn)行平滑處理。

如圖5所示，經(jīng)深度約束（USBL & DEPTH）后的水平位置精度明顯優(yōu)于USBL原始觀測(cè)值。為研究深度約束與拖體和母船運(yùn)動(dòng)狀態(tài)之間的關(guān)系，繪制了縮小10倍的二者航向角之差YAW。垂直約束后精度變化的趨勢(shì)同 YAW 的趨勢(shì)明顯相關(guān)，當(dāng)存在較大定位誤差的情況時(shí)，拖體水平位置雜亂無序，但經(jīng)過深度約束改正后沿船航行方向上的定位精度明顯提高，與上文分析的結(jié)論相符。因考慮到載體水平運(yùn)動(dòng)狀態(tài)模型，MEDFILT 方案精度較 USBL&DEPTH 算法進(jìn)一步提高。仿真實(shí)驗(yàn)采用平均絕對(duì)誤差（ Mean Absolute Error,MAE） 來評(píng)定算法的平均精度，其定義為：

圖5 開窗平滑算法定位誤差圖Fig.5 Positioning residuals of MEDFILT

表2給出前1000個(gè)歷元的直線運(yùn)動(dòng)狀態(tài)的解算結(jié)果。仿真實(shí)驗(yàn)中船沿北方向上航行，深度約束算法在北方向上的深度約束效果明顯優(yōu)于東方向。結(jié)合圖5和表2 可得，MEDFILT 算法通過深度約束信息和開窗平滑處理，明顯提高了直線狀態(tài)下垂直于運(yùn)動(dòng)方向上的定位精度，當(dāng)載體運(yùn)動(dòng)方向垂直的速度為 0時(shí)，MEDFILT方案在東方向精度較深度約束方法提高了4 m左右。

表2 直線狀態(tài)下的平均絕對(duì)誤差Tab.2 Mean absolute errors of straight line

為驗(yàn)證自適應(yīng)無跡抗差卡爾曼濾波算法的精度和容錯(cuò)性能，本文分別設(shè)置了不含粗差和含粗差的兩種實(shí)驗(yàn)，并采用標(biāo)準(zhǔn)無約束卡爾曼濾波（KF）算法作為對(duì)照。如圖6所示，不含粗差情況下，ARUKF的定位精度最高，其精度明顯優(yōu)于KF濾波算法，證明了深度約束對(duì)濾波算法的提高。

圖6 不含粗差情況下的定位誤差圖Fig.6 Positioning residuals of four approaches without outliers

由表3可知，不含粗差的情況下，融合多普勒觀測(cè)的KF和ARUKF濾波都可以提高載體水平定位的精度。由于垂直約束的影響，ARUKF算法在北方向上精度較KF濾波算法提高了接近2 m。

表3 不含粗差情況下的所有歷元的平均絕對(duì)誤差Tab.3 Comparison on Mean Absolute Errors of the epoch without outliers

含粗差情況時(shí)，在每100個(gè)USBL觀測(cè)值中和每150個(gè)DVL觀測(cè)中添加一個(gè)隨機(jī)的粗差（10倍觀測(cè)噪聲），通過標(biāo)準(zhǔn)無約束卡爾曼濾波（KF）和有深度約束的無跡卡爾曼濾波（UKF）作為對(duì)照來驗(yàn)證算法的有效性。

如圖7所示，當(dāng)USBL和DVL數(shù)據(jù)中同時(shí)含有粗差時(shí)，此時(shí)的定位精度較其他時(shí)刻要差。ARUKF濾波效果要明顯優(yōu)于UKF和KF算法，證明了新算法的穩(wěn)健性。

圖7 含粗差情況下的定位誤差圖Fig.7 Position residuals of the three approaches with outliers

3.2 實(shí)測(cè)實(shí)驗(yàn)

圖8(a)是采集的4500 m深的USBL數(shù)據(jù)，紅綠藍(lán)三種顏色的點(diǎn)分別表示早、中、后三階段的位置。由圖可見，USBL觀測(cè)值的觀測(cè)精度和采樣率都比較低，并且含有一些粗差值。聲學(xué)斜距信息如圖8(b) 所示，其測(cè)距精度要明顯優(yōu)于其定位精度，這是本文深度約束的基礎(chǔ)。

圖8 實(shí)測(cè)USBL水平位置和斜距觀測(cè)值Fig.8 Position and slant range of underwater towed vehicle by USBL

DVL傳感器測(cè)得的速度信息如圖9所示，由載體坐標(biāo)系下的速度觀測(cè)值的可看出，X方向上的速度值在0值附近波動(dòng)且變化幅度較小，說明拖體沿直線運(yùn)動(dòng)且左右擺動(dòng)較小。較USBL觀測(cè)值，DVL和姿態(tài)傳感器的觀測(cè)值具有高精度和高采樣率的特點(diǎn)。本文采用拉格朗日插值保證采用時(shí)刻的一致。

圖9 水下拖體的多普勒速度信息Fig.9 Velocity of towed vehicle measured by DVL

圖10 基于ARUKF算法的水下拖體平面位置Fig.10 Position of towed vehicle by ARUKF method

由于USBL精度較差，且粗差較多，KF算法不適合作為對(duì)照，而深度約束后的點(diǎn)在垂直于前進(jìn)方向上的離散程度依然很大，本文使用 DVL數(shù)據(jù)進(jìn)行航跡推算（DR）作為對(duì)照。如圖10所示，DR方法較USBL的位置更加平滑，因精度和采樣率不夠的問題，其在后期發(fā)散。使用深度約束的ARUKF算法定位精度在保證比較平滑的情況下，精度要高于DR算法，同時(shí)具有一定的抗差性能。

4 結(jié) 論

本文結(jié)合實(shí)測(cè)的深海拖體導(dǎo)航數(shù)據(jù)分析了導(dǎo)航傳感器的性能，采用仿真數(shù)據(jù)和實(shí)測(cè)數(shù)據(jù)驗(yàn)證了新濾波算法的有效性。實(shí)驗(yàn)結(jié)果表明：基于深度約束的開窗平滑算法能有效利用USBL聲學(xué)測(cè)距信息及深度約束信息提高水下拖體的定位精度；自適應(yīng)無跡抗差卡爾曼濾波可以有效識(shí)別USBL數(shù)據(jù)和DVL數(shù)據(jù)存在粗差，提高算法的容錯(cuò)性；該算法能有效利用深度約束信息和拖曳系統(tǒng)直線狀態(tài)模型，提高動(dòng)態(tài)定位的精度。