王志剛 顧雪峰

（1.91550部隊(duì) 大連 116023）（2.海軍工程大學(xué)兵器工程系 武漢 430033）（3.海軍工程大學(xué)兵器科研部 武漢 430033）

1 引言

目前，對(duì)于水下潛器的被動(dòng)定位是導(dǎo)航、定位領(lǐng)域研究的一個(gè)熱點(diǎn)和難點(diǎn)問題。水下潛器的慣導(dǎo)系統(tǒng)（Inertial Navigation System，INS）誤差隨時(shí)間積累，因此必須要通過其它導(dǎo)航方式實(shí)時(shí)或定期修正INS。出于隱蔽性要求，水下潛器又很難利用衛(wèi)星或無線電信息，此時(shí)利用水下地理特征信息的輔助導(dǎo)航定位就是很好的選擇。常用的輔助導(dǎo)航定位系統(tǒng)有地形、重力輔助導(dǎo)航系統(tǒng)［1～2］等，目前這些輔助導(dǎo)航系統(tǒng)都是以各種匹配算法為核心來獲得潛器的最佳匹配位置（真實(shí)位置），以實(shí)現(xiàn)對(duì)慣導(dǎo)系統(tǒng)的修正。傳統(tǒng)的匹配算法主要分為單點(diǎn)迭代和序列迭代兩類，分別以SITAN［3～4］（Sandia Inertial Terrain－Aided Navigation）和ICCP［5～6］（Interval Closest Contour Point）算法為典型代表。這些匹配算法分別在實(shí)時(shí)性和穩(wěn)定性方面有其優(yōu)點(diǎn)，但是為了保證良好的匹配效果對(duì)潛器的運(yùn)動(dòng)規(guī)律和初始誤差環(huán)境有很多嚴(yán)格的限制［7］，因而也限制了其應(yīng)用范圍。針對(duì)這一問題，本文提出一種直接利用觀測重力進(jìn)行水下被動(dòng)定位的新模式，應(yīng)用該方法的前提就是確定重力異常與潛器位置（大地經(jīng)、緯度坐標(biāo)）的精確解析表達(dá)式，之后與航空領(lǐng)域的目標(biāo)被動(dòng)定位原理相似，由于觀測重力中包含了目標(biāo)的位置坐標(biāo)信息，直接通過非線性濾波算法，就可對(duì)潛器的航行位置進(jìn)行最優(yōu)估計(jì)。

2 二維高斯樣條函數(shù)逼近

直接利用地球重力進(jìn)行定位，在很大程度上取決于觀測重力與目標(biāo)所在位置關(guān)系的確定，也就是地球重力場模型的確定。目前常用的各類全球重力場模型［8］無論在階次和精度方面都已經(jīng)達(dá)到了很高的水平，它本質(zhì)上屬于球坐標(biāo)系下的調(diào)和分析，因而應(yīng)用過程中需要計(jì)算大量的Legendre系數(shù)，使得模型計(jì)算量巨大，而這對(duì)于定位的實(shí)時(shí)性是一大考驗(yàn)，另外現(xiàn)有的大部分全球重力場模型非線性比較嚴(yán)重，也不太利于濾波估計(jì)時(shí)的線性化處理。

近年來，高斯樣條函數(shù)在醫(yī)學(xué)影像、數(shù)字地形構(gòu)建、計(jì)算機(jī)圖形學(xué)及地球物理等領(lǐng)域有較為廣泛的應(yīng)用，文獻(xiàn)［8］討論了一維高斯樣條在協(xié)方差函數(shù)代數(shù)確定中的應(yīng)用，文獻(xiàn)［9］采用高斯函數(shù)作為樣條基函數(shù)對(duì)計(jì)算區(qū)域重力異常進(jìn)行二維整體逼近，文獻(xiàn)［10］討論了高斯樣條函數(shù)逼近局部重力異常的相關(guān)影響因素。采用高斯樣條函數(shù)逼近局部重力異常運(yùn)算簡單且最終解析式是統(tǒng)一的，文章在該樣條逼近函數(shù)的基礎(chǔ)上建立了重力量測方程并結(jié)合具體的目標(biāo)運(yùn)動(dòng)狀態(tài)方程，成功運(yùn)用擴(kuò)展Kalman濾波實(shí)現(xiàn)了對(duì)潛器位置的最優(yōu)估計(jì)。

2.1 二維高斯樣條函數(shù)逼近數(shù)學(xué)原理

即有

不妨設(shè)

則上式可以簡化為矩陣形式：

則根據(jù)插值條件｛L（xi，yj）＝zi，j｜i＝1，…，m；j＝1，…，n｝，有如下線性方程：

易知X、Y 均為非奇異矩陣，即式（5）有唯一解，解方程即得到系數(shù)矩陣C，將系數(shù)矩陣代入式（2）便得到該局部重力異常基準(zhǔn)圖二維高斯樣條函數(shù)逼近解析式。

由式（5）可以看出系數(shù)矩陣C由X、Y與Z唯一確定，而X、Y由矩陣的階數(shù)和ax，ay決定。文獻(xiàn)［9］曾對(duì)求逆計(jì)算誤差與和計(jì)算階數(shù)的關(guān)系進(jìn)行過詳盡的探討。經(jīng)討論可知：系數(shù)矩陣的求解過程中如X、Y矩陣階數(shù)過高且ax、ay過小或階數(shù)過低且ax、ay過大時(shí)，X、Y矩陣的求逆存在較大誤差。而當(dāng)X、Y矩陣階數(shù)小于5或ax、ay值小于5時(shí)，矩陣求逆運(yùn)算誤差較小?？紤]到求逆計(jì)算誤差以及實(shí)際應(yīng)用中用到的格網(wǎng)區(qū)域遠(yuǎn)大于5×5，我們即可采用文獻(xiàn)［10］給出的斐波那契數(shù)列方法對(duì)ax∈［1，s］，ay∈［1，s］區(qū)間內(nèi)進(jìn)行尋優(yōu)。

2.2 二維高斯樣條函數(shù)逼近精度分析

取范圍i為117°E～121°E，j為21°N～25°N，分辨率為2′×2′的衛(wèi)星測高反演重力異常數(shù)據(jù)作為已知重力異常基準(zhǔn)圖。文章在該已知2′×2′重力異常基準(zhǔn)圖的基礎(chǔ)上，取4′×4′格網(wǎng)點(diǎn)處重力異常數(shù)據(jù)作為基礎(chǔ)數(shù)據(jù)點(diǎn)，進(jìn)行二維高斯樣條函數(shù)逼近，獲得該范圍局部重力異?；鶞?zhǔn)圖解析表達(dá)式，并據(jù)此計(jì)算分辨率為2′×2′的重力異常逼近值，然后與已知的2′×2′格網(wǎng)處重力異常值進(jìn)行比對(duì)分析。

最終的仿真結(jié)果如圖1所示，其中圖1（a）為已知2′×2′重力異?；鶞?zhǔn)圖，圖1（b）為由4′×4′重力異?；鶞?zhǔn)圖逼近的2′×2′重力異?；鶞?zhǔn)圖，由此可以看出二維高斯樣條函數(shù)逼近的局部重力異?；鶞?zhǔn)圖解析式能較好地描述這一范圍的重力異?；鶞?zhǔn)圖。

圖1（c）、圖1（d）分別為逼近誤差圖及相應(yīng)的誤差統(tǒng)計(jì)圖，表1為誤差統(tǒng)計(jì)表，由圖1、表1可以得出，試算區(qū)域4′×4′格網(wǎng)點(diǎn)處二維高斯逼近計(jì)算值絕對(duì)誤差均值為0.19mGal且99.5%計(jì)算點(diǎn)絕對(duì)誤差小于1mGal，逼近精度可以滿足導(dǎo)航定位要求。鑒于重力二維高斯樣條逼近函數(shù)的連續(xù)形式、與經(jīng)緯度坐標(biāo)具有明確的解析關(guān)系以及自身的高精度，使得這一逼近函數(shù)十分適合用于重力被動(dòng)定位時(shí)所需的量測方程。

圖1 局部重力異?；鶞?zhǔn)圖二維高斯逼近仿真實(shí)驗(yàn)

表1 計(jì)算值和真值比較（單位：mGal）

3 水下被動(dòng)定位模型的建立

水下被動(dòng)定位較水面與航空領(lǐng)域的被動(dòng)定位難度更大，究其原因就是在強(qiáng)調(diào)隱蔽性的前提下水下可利用的觀測量更少，即便有可用的觀測量其解析形式也十分復(fù)雜，因而當(dāng)今潛器的水下定位、導(dǎo)航多是基于特定的地理信息數(shù)字圖，通過采用各種匹配算法和觀測信息在數(shù)字圖上估算出潛器的位置坐標(biāo)，這類方法的核心就是匹配算法，但是匹配算法在應(yīng)用時(shí)為了保證具有好的定位效果，對(duì)潛器的初始誤差、運(yùn)動(dòng)模式具有諸多限制，且有的匹配算法還不能保證定位的實(shí)時(shí)性（如ICCP算法），能保證實(shí)時(shí)性的又容易出現(xiàn)誤匹配（如Sitan算法）。水面和航空領(lǐng)域的被動(dòng)定位相對(duì)簡單，它只需要建立合適的目標(biāo)運(yùn)動(dòng)狀態(tài)方程再由具體的觀測方程，將真實(shí)觀測量中所包含的我們感興趣的目標(biāo)狀態(tài)（如經(jīng)、緯度坐標(biāo)）通過非線性濾波技術(shù)［11～14］估算出來。

建立潛器狀態(tài)方程如下：

其中

其中F（k）、Γ（k）為k時(shí)刻的狀態(tài)和擾動(dòng)矩陣，φ（k）和λ（k）為k時(shí)刻的潛器經(jīng)緯度坐標(biāo)，Vφ、Vλ為潛器在緯度和經(jīng)度方向上的運(yùn)動(dòng)速度，T為采樣間隔，v（k）為零均值過程白噪聲。在目標(biāo)定位領(lǐng)域這一建模方式包含了目標(biāo)所有的勻速和勻加速運(yùn)動(dòng)情況。

對(duì)于水下潛器定位的觀測值取為重力儀觀測重力

其中γ＝9.780325（1＋0.00530244sin2（φ）－0.00000587sin2（2φ））為 WGS84橢球正常重力公式。

設(shè)

其中

z（φ，λ）為水下定位時(shí)將要采用的量測值，式（7）就是本文最終建立的量測方程。

式（7）與經(jīng)、緯度坐標(biāo)是一種非線性關(guān)系，本文在對(duì)潛器位置誤差進(jìn)行最優(yōu)估計(jì)時(shí)采用的是經(jīng)典的擴(kuò)展卡爾曼濾波（EKF），因而需要對(duì)式（7）進(jìn)行泰勒展開的線性變換。

將式（7）分別對(duì)φ、λ求導(dǎo)可得

其中

對(duì)式（7）進(jìn)行泰勒展開并舍去泰勒展開高階項(xiàng)，并直接給出線性化后的量測方程如下

其中H（k）為線性化后的量測矩陣，W（k）為觀測噪聲。

由狀態(tài)方程（6）及線性化后的量測方程（9）通過非線性濾波算法就可對(duì)目標(biāo)的水下位置進(jìn)行最優(yōu)估計(jì)。

4 仿真分析

以2.2節(jié)給出的區(qū)域范圍，i為117°E～121°E，j為21°N～25°N，分辨率為2′×2′的衛(wèi)星測高重力異常數(shù)據(jù)作為已知重力基準(zhǔn)圖進(jìn)行仿真分析。水下潛器初始位置為（21.5°，121.7°），北向和東向航速分別為 9nmile／h 和 －5nmile／h，加速度都為1nmile／h2，整個(gè)仿真階段取為100個(gè)采樣點(diǎn)，采樣間隔為6min，總航行時(shí)間為10h；重力儀測量數(shù)據(jù)的仿真采用重力異常數(shù)據(jù)加入測量噪聲的方法?，F(xiàn)今海洋重力儀的動(dòng)態(tài)精度已經(jīng)達(dá)到1mGal量級(jí)（如L＆R的S型海洋重力儀），因此重力測量誤差一般取方差為1mGal2的白噪聲，另外，考慮到各種數(shù)據(jù)濾波及誤差綜合影響（如交叉耦合、厄特沃什改正），取一項(xiàng)白噪聲作為濾波估計(jì)的誤差噪聲，這個(gè)綜合值根據(jù)經(jīng)驗(yàn)取為9mGal2。采用50次的蒙特卡羅仿真，結(jié)果如圖2～圖5所示。

表2 定位誤差統(tǒng)計(jì)表（單位：海里）

圖2、圖3為基于重力觀測值的水下被動(dòng)定位示意圖，由兩圖可以看出估計(jì)航跡在大部分的航行階段都能較好地反映真實(shí)航跡，只是由于運(yùn)動(dòng)模式轉(zhuǎn)換在航行轉(zhuǎn)彎時(shí)有一定程度的波動(dòng)，由以上分析可以得出結(jié)論：

圖2 重力被動(dòng)定位示意圖

圖3 定位放大示意圖

圖4 緯度誤差示意圖

圖5 經(jīng)度誤差示意圖

1）以觀測重力作為量測值是可以用來提取潛器位置坐標(biāo)的，由圖2、圖3的被動(dòng)定位示意圖可以看出，估計(jì)航跡在大部分的航行階段都能較好地反映真實(shí)航跡，這說明文章建立的量測方程是有效的，通過非線性濾波算法對(duì)量測值中的目標(biāo)位置信息進(jìn)行最優(yōu)估計(jì)，可實(shí)現(xiàn)潛器的被動(dòng)導(dǎo)航與定位。

2）圖4、圖5、表2給出的是本文重力被動(dòng)定位方法的定位誤差及統(tǒng)計(jì)結(jié)果，在10h的航行階段最大緯度誤差不超過4.02海里，平均緯度誤差只有0.74海里，最大經(jīng)度誤差2.28海里，平均經(jīng)度誤差0.59海里，從以上數(shù)據(jù)及誤差示意圖可以得出結(jié)論，雖然經(jīng)緯度誤差存在一定程度上的波動(dòng)，但總體的定位誤差已經(jīng)不隨時(shí)間積累，這使得該定位模式在水下可長期使用。

5 結(jié)語

被動(dòng)定位在軍事領(lǐng)域具有極高的應(yīng)用價(jià)值，文章將該原理引入潛器的水下被動(dòng)定位，首先基于二維高斯樣條函數(shù)逼近的重力場模型建立所需的量測方程，隨后通過非線性濾波技術(shù)對(duì)水下潛器的位置坐標(biāo)進(jìn)行實(shí)時(shí)估計(jì)。該定位方法使用簡單，無需使用傳統(tǒng)的匹配算法，因而避免了匹配算法對(duì)潛器的初始環(huán)境以及慣導(dǎo)誤差等的種種限制，最終的仿真結(jié)果證明，直接由實(shí)測重力數(shù)據(jù)對(duì)水下潛器進(jìn)行定位是可行的，它的定位誤差不隨時(shí)間積累，另外隨著海洋重力數(shù)據(jù)測量精度的不斷提高，最終的定位精度也必將得到進(jìn)一步的提高。

［1］Rice H，Mendelsohn L，Robert Aarons R，et al.Next generation marine precision navigation system［C］／／IEEE：Position Location and Navigation Symposium，2000：200－205.

［2］Moryl，J，Rice，H，Shinners，S.The Universal Gravity Module for Enhanced Submarine Navigation［C］／／IEEE Position Location and Navigation Symposium（PLANS'98）Record，20－23April 1998.

［3］Hollowell J.Heli／SITAN：a terrain referenced navigation algorithm for helicopters［C］／／IEEE Position，Location，and Navigation Symposium 1990（PLANS'90），Las Vegas，NV，USA，March 20－23，1990，616：625.

［4］許大欣.利用重力異常匹配技術(shù)實(shí)現(xiàn)潛艇導(dǎo)航［J］.地球物理學(xué)報(bào)，2005，48（4）：812－816.

［5］Behzad K.P，Behrooz K.P.Vehicle location on gravity maps［J］.Proceedings of SPIE－The International Society for Optical Engineering，1999：191.

［6］劉繁明，孫楓，成怡.基于ICCP算法及其推廣的重力定位［J］.中國慣性技術(shù)學(xué)報(bào)，2004，12（5）：36－39.

［7］Bishop G.C.Gravitational field maps and navigational errors［J］.IEEE Journal of Oceanic Engineering，2002，27（3）：726－737.

［8］BIAN Shaofeng，JOACHIM M.determining the parameter of a covariance function by analytical rules［J］.ZfV，1999（7）：212－216.

［9］童余德，邊少鋒，蔣東方，等.基于高斯樣條函數(shù)的局部重力異常場解析重構(gòu)［J］.測繪學(xué)報(bào)，2012，41（5）：754－760.

［10］Overholt K J.Efficiency of the Fibonacci search method［J］.BIT，1973，13（1）：92－96.

［11］楊元喜.自適應(yīng)動(dòng)態(tài)導(dǎo)航定位［M］.北京：測繪出版社，2006.

［12］王丹力，張洪鉞.慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)的非線性濾波算法［J］.中國慣性技術(shù)學(xué)報(bào)，1999，7（3）：17－21.

［13］吳俊偉，曾啟明，聶莉娟.慣性導(dǎo)航系統(tǒng)的誤差估計(jì)［J］.中國慣性技術(shù)學(xué)報(bào)，2002，10（6）：1－5.

［14］秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理［M］.西安：西北工業(yè)大學(xué)出版社，1998.