一種顧及厄特沃思效應(yīng)的水下重力輔助導(dǎo)航方法

張光俊，張夢得，張忠俠，高端陽

（1.中國人民解放軍92941部隊，葫蘆島 125000；2.中國人民解放軍92330部隊，青島 266000；3.中國人民解放軍91917部隊，北京 100036；4.海軍工程大學(xué)導(dǎo)航工程系，武漢 430033）

1 引 言

在水下環(huán)境中衛(wèi)星導(dǎo)航系統(tǒng)無法正常工作，慣性導(dǎo)航系統(tǒng)成為了更為主流的導(dǎo)航方式，并得到了廣泛應(yīng)用。鑒于慣性導(dǎo)航系統(tǒng)誤差隨時間積累這一固有弊端，純慣性導(dǎo)航方式難以實現(xiàn)水下長航時的導(dǎo)航需求，必須通過其他輔助信息源對慣性導(dǎo)航系統(tǒng)進(jìn)行定期校正，以抑制慣性導(dǎo)航系統(tǒng)的誤差積累效應(yīng)。通過船底搭載計程儀可以測量得到潛航器相對地面的航行速度，利用該速度信息可實現(xiàn)對慣導(dǎo)速度誤差的定期修正。但是慣導(dǎo)／計程儀這種組合模式依然沒有逃離慣導(dǎo)積分定位原理的本質(zhì)，長航時位置信息依然存在誤差積累，只是在誤差積累的快速性上起到了一定的緩解作用。水下聲學(xué)定位系統(tǒng)也是一種外界輔助信息，但是該系統(tǒng)需要布置基站，建設(shè)和維護(hù)工作較為困難，成本較高，也難以實現(xiàn)全海域輔助導(dǎo)航定位。海洋物理場作為地球的本質(zhì)屬性，一是不受空間地域的限制，可實現(xiàn)全海域輔助導(dǎo)航定位的需求，二是基于海洋物理場的匹配導(dǎo)航方式，可實現(xiàn)對慣導(dǎo)位置誤差定期校正，有效解決慣導(dǎo)位置誤差發(fā)散問題。鑒于上述優(yōu)點(diǎn)，加之近年來超導(dǎo)技術(shù)和原子干涉技術(shù)等前沿技術(shù)在重力精密測量領(lǐng)域的應(yīng)用，重力測量精度逐步提高，重力儀器環(huán)境適應(yīng)能力逐步增強(qiáng)，基于重力匹配的輔助導(dǎo)航方式得到了越來越多學(xué)者的關(guān)注。

重力測量、重力參考圖和重力匹配算法是構(gòu)成重力匹配導(dǎo)航系統(tǒng)的三個關(guān)鍵部分。本文重點(diǎn)關(guān)注重力匹配算法領(lǐng)域的研究。ICCP、TERCOM 和SITAN是當(dāng)前最為經(jīng)典的三種重力匹配算法，針對以上三種傳統(tǒng)算法的不足，文獻(xiàn)［10－12］對ICCP匹配算法進(jìn)行了改進(jìn)，文獻(xiàn)［13，14］對TERCOM匹配算法進(jìn)行了改進(jìn)，文獻(xiàn)［15－17］對SITAN匹配算法進(jìn)行了改進(jìn)，以增強(qiáng)算法的快速性或定位精度。但這三種算法的底層思想均源于地形匹配導(dǎo)航研究領(lǐng)域，重力信息與慣性導(dǎo)航系統(tǒng)信息融合不夠緊密?？柭鼮V波和粒子濾波等算法通過建立系統(tǒng)模型，為慣性信息與其他輔助信息源的有效融合提供了手段。文獻(xiàn)［10，18，19］對基于粒子濾波、質(zhì)子濾波算法的重力匹配算法進(jìn)行了較為深入的研究，無論是粒子濾波還是質(zhì)子濾波都存在計算量較大的問題，難以實現(xiàn)實時重力輔助導(dǎo)航。文獻(xiàn)［20，21］提出了采用UKF的非線性濾波框架實現(xiàn)重力匹配，相較于粒子濾波算法減少了計算量。

重力儀隨水下運(yùn)載體與地球產(chǎn)生相對運(yùn)動，進(jìn)而產(chǎn)生科里奧利力作用于重力儀，使得重力測量值發(fā)生變化，這種現(xiàn)象稱為厄特沃思效應(yīng)。厄特沃思效應(yīng)與水下載體的位置和速度信息有關(guān)，特殊情況下厄特沃思效應(yīng)可達(dá)上百厄特沃思，成為了重力測量的關(guān)鍵誤差源。厄特沃思校正是重力儀測量數(shù)據(jù)處理過程中的關(guān)鍵步驟，傳統(tǒng)重力匹配算法研究大都建立在可獲得精確重力信息基礎(chǔ)上展開的，但是對于水下應(yīng)用環(huán)境，重力儀無法利用衛(wèi)星導(dǎo)航系統(tǒng)提供的導(dǎo)航參數(shù)進(jìn)行厄特沃思校正，厄特沃思估計誤差勢必影響重力儀的測量精度，進(jìn)而影響重力輔助導(dǎo)航精度。本文顧及厄特沃思效應(yīng)的影響，構(gòu)建了水下INS／計程儀／重力儀組合導(dǎo)航框架，采用了UKF非線性濾波算法進(jìn)行信息融合，通過半物理仿真分析了不同導(dǎo)航模式下厄特沃思估計誤差特性，以及厄特沃思估計誤差的存在對重力輔助導(dǎo)航性能的影響。

2 重力測量與數(shù)據(jù)處理

2.1 運(yùn)動加速度方程

本文討論的背景是海洋絕對重力儀。絕對重力儀不僅測量重力加速度，還測量載體的運(yùn)動加速度以及由于地球自轉(zhuǎn)（厄特沃思效應(yīng)）耦合而產(chǎn)生的加速度。重力儀測得的加速度為

式中：g——海洋重力儀的測量絕對值；g——重力加速度真值；a——厄特沃思加速度。

因此，海洋絕對重力儀測量的原始數(shù)據(jù)需要經(jīng)過一定處理才能得到準(zhǔn)確的、可用的重力信息，其中厄特沃思校正是數(shù)據(jù)處理最為關(guān)鍵的步驟。測量得到的絕對重力值減去地球正常重力場模型計算值，實現(xiàn)重力異常信號的提取。

2.2 厄特沃思效應(yīng)及誤差分析

厄特沃思效應(yīng)計算公式為

式中：ω——地球的自轉(zhuǎn)角速度；L——緯度；h——高度；v和v——分別為東向速度和北向速度；R和R——分別為地球的本初垂直半徑和地球在子午線的曲率半徑。觀察公式（2）可知，厄特沃思效應(yīng)主要與載體的緯度和東向速度信息有關(guān)，下面分別分析緯度誤差和東向速度誤差對厄特沃思估計誤差的影響。

當(dāng)緯度誤差為δL，速度項不存在誤差時，則厄特沃思估計誤差δa為

當(dāng)東向速度誤差為δv，緯度不存在誤差時，則厄特沃思估計誤差δa為：

為了定量分析不同緯度誤差和東向速度誤差對厄特沃思效應(yīng)的影響，假定水下航行器東向航行速度為5節(jié)，在緯度0°～90°范圍內(nèi)進(jìn)行數(shù)值仿真分析，如圖1所示。在中低緯度區(qū)域，0.1m／s的東向速度誤差會產(chǎn)生1 mGal以上的厄特沃思估計誤差，而緯度誤差超過90 n mile時才會導(dǎo)致1 mGal厄特沃思估計誤差如圖2所示。因此，可認(rèn)為重力儀動態(tài)測量精度對東向速度誤差更為敏感。若要精確對厄特沃思效應(yīng)進(jìn)行補(bǔ)償，需要外界提供較為精確的東向速度信息。

圖1 緯度誤差對厄特沃思效應(yīng)的影響Fig.1 Relationship between latitude error and E?tv?s estimation error

圖2 東向速度誤差對厄特沃思效應(yīng)的影響Fig.2 Relationship between eastward velocity error and E?tv?s estimation error

2.3 重力異常估計

重力異常通過WGS84正常重力模型減去重力測量值獲得，同時考慮高度和緯度影響。正常重力計算公式為

其中，

式中：g——考慮高度和緯度影響的重力加速度；g——赤道處海平面上的重力加速度；R——參考橢球的地球長半徑，e——參考橢球的第一偏心率。

3 水下輔助導(dǎo)航的數(shù)學(xué)公式

水下慣性導(dǎo)航系統(tǒng)／計程儀／重力儀組合導(dǎo)航系統(tǒng)一方面可實現(xiàn)水下重力儀精確測量，另一方面可實現(xiàn)水下長航時導(dǎo)航定位，抑制慣性導(dǎo)航系統(tǒng)的誤差積累，如圖3所示。重力匹配問題是通過噪聲觀測來估計這種隨機(jī)動力系統(tǒng)的狀態(tài)（姿態(tài)、速度和位置），本研究中的噪聲觀測是指重力異常測量。

圖3 水下組合導(dǎo)航框圖Fig.3 Block diagram of underwater aided navigation

3.1 狀態(tài)方程

SINS在導(dǎo)航坐標(biāo)系下的比力方程為

公式（14）表明，在從加速度計的輸出中扣除有害加速度后，速度可以通過積分加速度得到，位置可以通過積分速度得到。因此，比力方程是SINS的基本方程。

導(dǎo)航坐標(biāo)系下的位置微分方程為

本文將慣性導(dǎo)航系統(tǒng)的基本方程作為狀態(tài)方程，水下航行器的姿態(tài)、速度和位置信息作為狀態(tài)量。離散化動力學(xué)方程為

式中：x——k時的狀態(tài)量，包含姿態(tài)、速度和位置；f——INS狀態(tài)遞推方程；w——離散過程噪聲。隨機(jī)噪聲｛w｝是獨(dú)立的零均值高斯白噪聲，w協(xié)方差矩陣記為Q。

3.2 量測方程

在本文研究中，將計程儀測量的速度信息和重力儀測量的重力信息作為觀測量。需要注意的是，計程儀測量的是b系下的速度，在使用的過程中需要經(jīng)過坐標(biāo)轉(zhuǎn)換。觀測矩陣構(gòu)建為

量測方程為

式中：z——k時刻重力異常量測量；h（x）——狀態(tài)和測量之間的函數(shù)；v——量測誤差。

3.3 UKF濾波算法

UKF是對非線性函數(shù)的概率密度分布進(jìn)行近似，用一系列確定樣本來逼近狀態(tài)的后驗概率密度，摒棄了對非線性函數(shù)進(jìn)行線性化的傳統(tǒng)方法。

(3)UAV+RFID的三維路徑規(guī)劃問題的首要任務(wù)為尋找從起始點(diǎn)到終點(diǎn)的最優(yōu)路徑，設(shè)置了路徑最小適應(yīng)度函數(shù)來求起始點(diǎn)和終點(diǎn)之間的最優(yōu)路徑。

計算sigma點(diǎn)

其中，下標(biāo)“：，i”表示矩陣的第i列。每個sigma點(diǎn)都可以通過系統(tǒng)模型傳播

狀態(tài)估計及其誤差協(xié)方差為

UKF的觀測更新過程通過以下公式生成新sigma點(diǎn)

Sigma點(diǎn)和平均觀測信息可通過以下公式求解

觀測方程的新協(xié)方差為

最后，UKF Kalman增益、狀態(tài)向量更新和誤差協(xié)方差更新計算方式為

4 仿真結(jié)果與討論

4.1 試驗環(huán)境

為了評估算法的可行性，本文采用了一組約6 h船載實測數(shù)據(jù)進(jìn)行驗證。試驗船搭載的IMU和DVL設(shè)備主要性能指標(biāo)見表1和表2。試驗船同時搭載了一個單天線的GPS接收機(jī)，以1 Hz頻率輸出速度和位置信息，并作為本次試驗的速度和位置參考基準(zhǔn)。

表1 IMU設(shè)備性能指標(biāo)Tab.1 IMU performance indexes

表2 DVL設(shè)備性能指標(biāo)Tab.2 DVL performance indexes

在本文的半物理仿真實驗中，使用最高階的EIGEN－6C4球諧模型計算重力數(shù)據(jù)，并計算重力匹配圖的分辨率，如圖4～6所示。

圖4 匹配區(qū)重力異常三維圖Fig.4 Gravity anomaly three-dimensional map in matching area

圖5 匹配區(qū)重力異常二維圖Fig.5 Gravity anomaly two-dimensional map in matching area

圖6 重力異常變化曲線Fig.6 Variation curve of gravity anomaly on the track

4.2 結(jié)果與分析

將試驗船處于系泊狀態(tài)下前500 s的IMU數(shù)據(jù)采用優(yōu)化對準(zhǔn)方法進(jìn)行慣導(dǎo)初始對準(zhǔn)，并將初始對準(zhǔn)的結(jié)果用于純慣性推算，純慣性推算的位置誤差如圖7所示。

圖7 純慣性計算位置誤差Fig.7 Pure inertial calculation position errors

厄特沃思估計精度是影響重力儀測量精度的關(guān)鍵因素，尤其是載體處于水下長航時航行狀態(tài)，重力儀無法由衛(wèi)星導(dǎo)航系統(tǒng)提供航行器精確的導(dǎo)航參數(shù)參與厄特沃思計算，其測量精度將會受到較大影響。重力匹配導(dǎo)航傳統(tǒng)算法并未考慮厄特沃思效應(yīng)對重力輔助導(dǎo)航系統(tǒng)性能的影響，都是建立在重力信息準(zhǔn)確測量基礎(chǔ)之上，未充分考慮重力儀輔助導(dǎo)航水下應(yīng)用場景的特殊性。鑒于此，本文考慮了純慣性推算（INS）、INS／DVL組合（INS／DVL）、不考慮厄特沃思校正的INS／DVL／重力儀組合（INS／DVL／Gravimeter）和考慮厄特沃思校正的INS／DVL／重力儀組合的四種導(dǎo)航模式。需要說明的是，不考慮厄特沃思校正的INS／DVL／重力儀組合模式，重力儀測量精度設(shè)置為1 mGal，考慮厄特沃思校正的INS／DVL／重力儀組合模式，重力儀測量精度為1 mGal的基礎(chǔ)上疊加厄特沃思估計誤差的影響。純慣性推算結(jié)果如圖7所示，其余三種導(dǎo)航模式位置誤差曲線如圖8～圖10所示，四種導(dǎo)航模式最大誤差統(tǒng)計如表3所示。

圖8 緯度誤差Fig.8 Latitude errors

圖9 經(jīng)度誤差Fig.9 Longitude errors

圖10 水平徑向位置誤差Fig.10 Horizontal radial position errors

分析表3可知，6小時純慣性推算緯度方向最大誤差為6.28 n mile，經(jīng)度方向最大誤差為2.75 n mile，水平徑向最大誤差為6.39 n mile，可見在無任何外界輔助信息的情況下，慣性導(dǎo)航系統(tǒng)位置誤差隨時間快速積累。當(dāng)采用DVL輔助慣性導(dǎo)航時，相比較于純慣性導(dǎo)航，其定位性能得到了較大改善，DVL的輔助有效抑制了純慣性推算位置誤差快速發(fā)散問題。當(dāng)采用DVL和重力儀共同輔助，不考慮厄特沃思估計誤差時，緯度方向、經(jīng)度方向和水平徑向最大誤差分別為0.26 n mile，0.24 n mile和0.35 n mile，在僅采用DVL作為輔助信息源相比，在三個方向上定位性能分別提升了25.71%，25% 和25.53%?？紤]厄特沃思估計誤差時，緯度方向、經(jīng)度方向和水平徑向最大誤差分別為0.30 n mile，0.25 n mile和0.39 n mile，與僅采用DVL作為輔助導(dǎo)航模式相比，在三個方向上定位性能分別提升了14.29%，21.88%和17.02%。因此，重力儀的參與，即使考慮厄特沃思效應(yīng)，定位性能也得到了提升。

表3 不同導(dǎo)航模式最大誤差統(tǒng)計Tab.3 Maximum error statistics of different navigation modes

利用衛(wèi)星導(dǎo)航系統(tǒng)輸出的速度和位置信息估計厄特沃思校正值，并作為參考基準(zhǔn)。分別計算上述四種導(dǎo)航模式下的厄特沃思校正值，及其與參考基準(zhǔn)之間的差異，如圖11和圖12所示，統(tǒng)計厄特沃思估計誤差最小值、最大值、均值和標(biāo)準(zhǔn)差結(jié)果如表4所示。

圖11 不同導(dǎo)航模式計算的厄特沃思校正值Fig.11 E?tv?s effect in different navigation modes

圖12 不同導(dǎo)航模式計算的厄特沃思誤差Fig.12 E?tv?s errors in different navigation modes

根據(jù)表4可知，將純慣性推算得到的導(dǎo)航信息作為厄特沃思校正計算參數(shù)時，厄特沃思估計誤差最小值為－23.73 mGal，最大值為21.28 mGal，均值為－1.84 mGal，標(biāo)準(zhǔn)差為11.19 mGal。當(dāng)引入DVL輔助信息源時，厄特沃思估計誤差最小值為－2.63 mGal，最大值為4.33 mGal，均值為0.20 mGal，標(biāo)準(zhǔn)差為0.75 mGal，厄特沃思校正值得到了較好地估計。

表4 不同導(dǎo)航模式計算的厄特沃思誤差統(tǒng)計Tab.4 E?tv?s error statistics of different navigation modes

5 結(jié)束語

針對水下衛(wèi)星導(dǎo)航系統(tǒng)拒止環(huán)境，重力儀的厄特沃思校正難以精確估計問題，本文構(gòu)建了水下INS／計程儀／重力儀組合導(dǎo)航框架，采用UKF非線性濾波算法，分析了不同導(dǎo)航模式下厄特沃思估計誤差特性，對比分析了有無厄特沃思估計誤差時重力輔助導(dǎo)航性能差異。半物理仿真試驗表明，引入DVL測量信息時，厄特沃思估計誤差均值為0.20 mGal，標(biāo)準(zhǔn)差為0.75 mGal，較好地估計了厄特沃思效應(yīng)。同時對比仿真分析了四種不同導(dǎo)航模式的定位性能，結(jié)果表明采用DVL的輔助導(dǎo)航方式可有效抑制慣導(dǎo)誤差積累，INS／DVL／Gravimeter三者信息融合模式定位性能進(jìn)一步提高，即使在考慮厄特沃思估計誤差的情況下，與僅采用DVL作為輔助信息源相比，水平徑向誤差定位精度依然提升了17.02%。