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

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

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

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

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

1 SITAN 算法數(shù)學(xué)模型

SITAN 算法綜合了擴(kuò)展卡爾曼濾波技術(shù)和重力場隨機(jī)線性化技術(shù)，具有實時性、可操作性強(qiáng)等特點[2]。系統(tǒng)狀態(tài)方程和量測方程如下。

1.1 系統(tǒng)狀態(tài)方程

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

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

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

即

其中，F(xiàn)為系統(tǒng)狀態(tài)矩陣，W為系統(tǒng)噪聲，對方程進(jìn)行離散化處理后，得到：

式中，|1kk-φ為系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，1kW-為獨立平穩(wěn)白噪聲矩陣，并且有：

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

1.2 系統(tǒng)量測方程

由慣導(dǎo)系統(tǒng)輸出位置，通過先驗重力異?；鶞?zhǔn)圖可以得到點位重力異常egδ，同時，利用海洋重力數(shù)據(jù)采集裝置得到實測重力異常mgδ，實測值與基準(zhǔn)圖讀取值之差設(shè)定為量測值Z。

實測重力異常值為：

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

基準(zhǔn)圖讀取重力異常值為：

式中，δge為讀取重力異常值，為慣性導(dǎo)航系統(tǒng)輸出位置處的讀取重力異常值，γm為讀取重力異常值偏差。

則有：

假設(shè)系統(tǒng)量測方程為：

1.3 地形隨機(jī)線性化技術(shù)

線性化的基本原理，就是在計算區(qū)域內(nèi)，用平面方程逼近重力異?；鶞?zhǔn)圖曲面方程g（x,y），得到平面方程在緯度和經(jīng)度方向斜率，帶入量測方程進(jìn)行計算。擬合公式如下：

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

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

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

2 抗差估計的SITAN 算法

SITAN 算法所得結(jié)果是基于觀測值服從正態(tài)分布來求取待定參數(shù)的，具有無偏性、最優(yōu)性和一致性等優(yōu)點。當(dāng)觀測值含有粗差，將不服從正態(tài)分布，無法得到理想平差結(jié)果，抗差估計的出現(xiàn)解決了這一問題。

設(shè)誤差方程為:

權(quán)矩陣為

由最小二乘估計，平差準(zhǔn)則為：

抗差估計用一個增長慢的函數(shù)ρ（vi）代替，平差準(zhǔn)則為：

或

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

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

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

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

3 試驗驗證與分析

3.1 仿真實驗設(shè)計

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

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

3.2 一般SITAN算法仿真

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

圖3 線路1匹配結(jié)果Fig.3 Matching resultsof path one

圖4 線路2 匹配結(jié)果Fig.4 Matching resultsof path two

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

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

表2 一般SITAN算法匹配導(dǎo)航結(jié)果精度統(tǒng)計Tab.2 Precisionstatisticsof matching navigation results

3.3 抗差算法仿真

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

表3 改進(jìn)匹配導(dǎo)航結(jié)果精度統(tǒng)計Tab.3 Precision statisticsof improvedmatching navigationresults

圖5 線路1改進(jìn)匹配結(jié)果Fig.5 Improved matching results of path one

圖6 線路2改進(jìn)匹配結(jié)果Fig.6 Improved matching results of path two

4 結(jié)論

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