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

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

1 引 言

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

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

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

2 重力測量與數據處理

2.1 運動加速度方程

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

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

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

2.2 厄特沃思效應及誤差分析

厄特沃思效應計算公式為

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

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

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

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

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

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

2.3 重力異常估計

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

其中，

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

3 水下輔助導航的數學公式

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

圖3 水下組合導航框圖Fig.3 Block diagram of underwater aided navigation

3.1 狀態(tài)方程

SINS在導航坐標系下的比力方程為

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

導航坐標系下的位置微分方程為

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

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

3.2 量測方程

在本文研究中，將計程儀測量的速度信息和重力儀測量的重力信息作為觀測量。需要注意的是，計程儀測量的是b系下的速度，在使用的過程中需要經過坐標轉換。觀測矩陣構建為

量測方程為

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

3.3 UKF濾波算法

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

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

計算sigma點

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

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

UKF的觀測更新過程通過以下公式生成新sigma點

Sigma點和平均觀測信息可通過以下公式求解

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

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

4 仿真結果與討論

4.1 試驗環(huán)境

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

表1 IMU設備性能指標Tab.1 IMU performance indexes

表2 DVL設備性能指標Tab.2 DVL performance indexes

在本文的半物理仿真實驗中，使用最高階的EIGEN－6C4球諧模型計算重力數據，并計算重力匹配圖的分辨率，如圖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 結果與分析

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

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

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

圖8 緯度誤差Fig.8 Latitude errors

圖9 經度誤差Fig.9 Longitude errors

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

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

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

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

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

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

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

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

5 結束語

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