應用抗差估計SITAN算法的水下重力匹配導航方法

歐陽明達，孫藝軒，鄺英才，范昊鵬，王 華

（1. 信息工程大學地理空間信息學院，鄭州 450052；2. 西安測繪研究所，西安 710054； 3. 地理信息工程國家重點實驗室，西安 710054；4. 中山大學海洋科學學院，珠海 519082）

利用地球物理場輔助慣性導航系統（INS）能夠實現水下航行器長時間的高精度導航和定位，減弱INS 隨時間推移產生的系統誤差積累[1-5]。衛星測高技術的發展，使人們可以在較短時間獲取高精度、高分辨率的全球海洋重力數據[6,7]，基于重力數據的無源性、穩定性、良好的空間分布等諸多優點，INS/重力場匹配導航逐漸成為水下匹配導航工作的研究熱點。目前大多數研究仍然停留在模擬計算與分析[8]，也有不少基于實際數據的水下導航試驗[9]。重力場匹配導航首先在于適配區的選擇[10,11]，然后通常依據重力場統計特征參數、綜合特征參數等構建適配算法[12,13]。導航精度依賴于慣性導航系統精度、重力數據采集系統量測精度、背景圖定位精度和匹配算法等要素。其中，算法是匹配導航技術的核心，目前的常用算法包括TERCOM 算法[2,5]、粒子群優化算法[14]、粒子濾波算法[15]、三角形約束算法[16]等。

SITAN 算法最早于上世紀70 年代由美國桑迪亞實驗室提出，主要用于飛行器自主巡航，而后學者們進行擴充研究，相繼誕生了不同應用情境的SITAN 算法，如HELI/SITAN 算法[17]、BUAA 慣性地形輔助導航(BITAN II)算法[18]等，該方法綜合了擴展卡爾曼濾波技術和地形隨機線性化技術，具有實時性好、可操作性強等特點。當觀測值不含粗差且觀測誤差服從正態分布，采用SITAN 算法能夠求得最優無偏估值。但當開展作業任務時，難以避免粗差影響，因而其結果也不再是最優無偏[19]。粗差處理方法分為粗差探測和抗差估計。粗差探測是將粗差歸入函數模型，通過統計檢驗剔除后，再處理不含粗差的觀測值；抗差估計是將粗差歸入隨機模型，通過增大含有粗差的觀測值方差，使其對參數估值的影響降低，其關鍵是構造合適的等價權模型[20]。我國學者對此進行了深入研究，周江文通過等價權概念，將抗差M 估計轉化為與最小二乘一致的平差形式，提出IGGI 和IGGII 方案[21]；歐吉坤提出三步抗差估計方法，分三步改變臨界常數，不同步驟中淘汰臨界值強弱不同，改善了抗差估計估值效率[22]；楊元喜提出IGGIII 相關等價權抗差方案，考慮觀測向量相關權矩陣構造抗差估計解[23]；劉經南提出基于等價方差－協方差的相關抗差估計，在此基礎上，楊元喜提出雙因子等價權相關抗差模型，充分考慮了相關觀測量間的相關信息[24]。

本文在SITAN 算法基礎上，利用重力場模型數據開展仿真試驗。首先，給出了SITAN 算法的狀態方程和量測方程，采用九點擬合法對局部重力場異常值進行隨機線性化擬合，得到不同方向上的斜率參數。針對計算中存在的粗差，采用抗差估計方法，設計一種權調節因子，當匹配點觀測重力異常誤差高于指定閾值時，使用權調節因子進行迭代計算，直到重力異常誤差低于指定閾值，取得了良好效果。

1 SITAN 算法數學模型

SITAN 算法綜合了擴展卡爾曼濾波技術和重力場隨機線性化技術，具有實時性、可操作性強等特點[2]。系統狀態方程和量測方程如下。

1.1 系統狀態方程

取水下航行器的三維位置誤差和二維速度誤差作為狀態，記為：

其中，δ x,δ y,δh表示緯度、經度和高度方向上的位置誤差，δ vx,δvy表示航行器速度在緯度、經度方向上的誤差。

為滿足卡爾曼濾波實時計算的要求，設航行器為勻速定深航行。如此，可以簡化方程，并略去高階小量，得到系統的五維運動方程。即：

即

其中，F為系統狀態矩陣，W為系統噪聲，對方程進行離散化處理后，得到：

式中，|1kk-φ為系統的狀態轉移矩陣，1kW-為獨立平穩白噪聲矩陣，并且有：

其中，T為采樣時間間隔。

1.2 系統量測方程

由慣導系統輸出位置，通過先驗重力異常基準圖可以得到點位重力異常egδ，同時，利用海洋重力數據采集裝置得到實測重力異常mgδ，實測值與基準圖讀取值之差設定為量測值Z。

實測重力異常值為：

式中，gδm為實測重力異常，x,y為真實位置向量，gδr為真實重力異常，γr為實際測量噪聲。

基準圖讀取重力異常值為：

式中，δge為讀取重力異常值，為慣性導航系統輸出位置處的讀取重力異常值，γm為讀取重力異常值偏差。

則有：

假設系統量測方程為：

1.3 地形隨機線性化技術

線性化的基本原理，就是在計算區域內，用平面方程逼近重力異常基準圖曲面方程g（x,y），得到平面方程在緯度和經度方向斜率，帶入量測方程進行計算。擬合公式如下：

采用九點擬合法，如圖1 所示，圍繞中心點p5，在距離p5點1.5σx和1.5σy的位置另外選取八個點，用最小二乘法原理，得到平面方程參數。

圖1 九點擬合法Fig.1 Nine-point fitting method

σ x,σy為卡爾曼濾波實時得到的位置誤差均方根值，直接給出線性化結果：

2 抗差估計的SITAN 算法

SITAN 算法所得結果是基于觀測值服從正態分布來求取待定參數的，具有無偏性、最優性和一致性等優點。當觀測值含有粗差，將不服從正態分布，無法得到理想平差結果，抗差估計的出現解決了這一問題。

設誤差方程為:

權矩陣為

由最小二乘估計，平差準則為：

抗差估計用一個增長慢的函數ρ（vi）代替，平差準則為：

或

抗差估計的法方程與經典最小二乘平差的法方程具有相同的形式，改變觀測權為等價權之后，用經典最小二乘平差程序可進行抗差估計平差。這種抗差估計方法稱為——抗差最小二乘法。

抗差最小二乘法的抗差性取決于等價權函數（權因子），不同的等價權函數意味著不同的抗差估計。等價權函數（權因子函數）是殘差的函數，而殘差又是未知參數和觀測值的函數，所以等價權在平差前是未知的，抗差估計必須以迭代的方式完成。

引入抗差估計方法，設定合理閾值判斷粗差，重新構建等價權函數。當獲得匹配點位置時，讀取重力異常值與INS輸出重力異常值理應高度吻合，倘若位置偏移過大，兩者將產生不符，倘若重力異常差異超過設定閾值τ，則判定該點為粗差。選取具有較強抗差能力的Huber 函數，當差異值在閾值范圍內時，權值為1，大于閾值的觀測點權重采用Huber 等價權函數，設S為權調節因子，當水下航行器向前推進時，每采樣一個點，設定S初始為單位矩陣，在粗差點i處，令為i點重力異常差異，Vmean為1,2,3....i航跡點重力異常差異的平均值。采用迭代方法：

圖2 抗差估計方法流程圖Fig.2 Flow chart of robust estimation method

3 試驗驗證與分析

3.1 仿真實驗設計

選定兩片海域作為本文研究對象，通過仿真得到兩條水下航行器行進線路。表1示出了仿真線路的起始點坐標、初始誤差、航向角、航行速度、慣導系統陀螺、加速度計誤差等參數，重力異常基準圖選用丹麥科技大學發布的DTU12模型，模型為1'分辨率，精度為3-8mGal。

表1 仿真航跡及其參數Tab.1 Simulatedpathand their para meters

3.2 一般SITAN算法仿真

采用SITAN算法，得到匹配導航結果及其與真實航跡點的坐標差異。如圖3和圖4所示，背景圖顯示

圖3 線路1匹配結果Fig.3 Matching resultsof path one

圖4 線路2 匹配結果Fig.4 Matching resultsof path two

出兩片海域豐富的重力場變化信息，適用于開展重力匹配導航，圖中綠色線段為INS指示航跡，紅色點為真實航跡采樣點，黃色點線為匹配航跡。INS指示航跡較真實航跡有較大偏差，經過匹配運算，水下航行器位置得到了有效糾偏。線路1在經度方向的最大偏移量為4n mile，緯度方向為3n mile，線路2在經度方向為6n mile，緯度方向3n mile。將線路分為前段（1-40點位）、中段（40-100點位）和后段（100-160點位）三個部分，線路1前段、后段與真實航跡差異較小，偏移量大于2 n mile的誤差大多在中段線路；線路2前段吻合程度較好，大誤差一般在于中段和后段，最大值為經度方向6 n mile，換算成平面距離約為12km。

表2示出了匹配導航結果精度，匹配航跡與真實航跡偏差標準差較大，也說明存在異常誤差影響。究其原因，一是重力場隨機線性化過程的不確定性，其較多依賴于局部海域的重力場變化，導致匹配航跡偏離真實航跡點的距離存在差異；二是由于該算法采用遞推計算過程，根據卡爾曼濾波計算公式，當某點位置出現較大偏離，往往會造成計算下一位置點的狀態矩陣、權矩陣、增益矩陣等產生變化，因而出現較大誤差。

表2 一般SITAN算法匹配導航結果精度統計Tab.2 Precisionstatisticsof matching navigation results

3.3 抗差算法仿真

利用第2節所述抗差估計方法，設定閾值為重力異常殘差的3倍中誤差，得到兩條仿真線路匹配導航結果，見圖5和圖6以及表3。從計算結果可以看出，通過對權矩陣進行調節，極大降低了粗差點對下一位置點的不利影響，匹配結果出現較大幅度改善，航跡點差異最大值維持在1.5 n mile 以內，偏差的平均值最大為0.61 n mile，標準差控制在0.5n mile以內。采用抗差估計得到的匹配導航結果精度水平較一般SITAN方法提升50%。

表3 改進匹配導航結果精度統計Tab.3 Precision statisticsof improvedmatching navigationresults

圖5 線路1改進匹配結果Fig.5 Improved matching results of path one

圖6 線路2改進匹配結果Fig.6 Improved matching results of path two

4 結論

本文給出了水下重力匹配SITAN算法的數學模型和隨機線性化公式，并開展海洋重力場水下匹配導航仿真實驗。結果表明，SITAN 算法能夠有效糾正慣性導航系統誤差積累影響，使匹配航跡與真實航跡吻合，由于重力場隨機線性化結果的不確定性，會導致出現異常誤差，顯著影響匹配結果。引入抗差估計，設定重力異常閾值，采用抗差等價權迭代方法，設置合理的調節因子，能夠有效降低異常誤差造成的狀態矩陣、權矩陣、增益矩陣變化等不利影響，改善匹配點和真實點吻合程度，提高匹配航跡點位置精度。下一步將在此基礎上，開展顧及阻尼效應的SITAN 算法研究，并將引入無跡卡爾曼濾波算法，研究水下重力匹配導航的非線性濾波算法。