劉繁明，姚劍奇，荊 心，程 石

（哈爾濱工程大學(xué)自動化學(xué)院，哈爾濱 150001）

0 引言

隨重力測量精度的不斷提高，出現(xiàn)了依靠重力場特征定位的重力輔助導(dǎo)航方法［1－2］。該方法依靠重力測量的無源性及重力場的時空穩(wěn)定性，保證導(dǎo)航的隱蔽性及可靠性。此外，至今已積累了大量的船舶及航空重力測量數(shù)據(jù)。綜上所述，重力輔助導(dǎo)航是具有現(xiàn)實(shí)意義的，穩(wěn)健的輔助導(dǎo)航方法。

目前對重力輔助導(dǎo)航的研究主要集中在匹配算法［2－6］及重力場導(dǎo)航適配性［7］的研究上，而重力輔助導(dǎo)航以慣性導(dǎo)航為基礎(chǔ)，除匹配算法及重力場特征外，INS誤差特性將影響匹配算法的性能。本文通過對幾種經(jīng)典的相關(guān)及濾波匹配算法的研究及仿真實(shí)驗(yàn)說明:因不同匹配算法的特點(diǎn)不同，INS定位精度變化會改變算法間的優(yōu)劣關(guān)系，導(dǎo)致匹配算法的適用性變化。

1 相關(guān)匹配算法

1.1 TERCOM算法

設(shè)匹配序列長度為L，k時刻量測向量gk由k至k－L+1時刻重力量測組成。在基準(zhǔn)圖一定范圍內(nèi)，按固定步長移動INS航跡并插值得向量gmi，i=1，…，N（N為向量總數(shù)），則由式 （1）可得匹配位置

（）為INS航跡經(jīng)向、緯向平移量。TERCOM可視為INS航跡沿基準(zhǔn)等值線滑動和垂直其滑動以尋找相關(guān)峰過程，其定位誤差如圖1所示。

圖1 TERCOM定位誤差示意圖Fig.1 Diagram of positioning error of TERCOM

圖1中，T1、T2為真實(shí)及匹配航跡，mi、ni為量測點(diǎn)，i=1，…，L，Ci為mi等值線。以C1為基準(zhǔn)，T2相對T1的偏差為:ni相對mi沿C1滑動及垂直C1滑動的偏移Δxi、Δyi。設(shè)mi對應(yīng)制圖誤差、量測噪聲及 INS速度誤差所引起的 E?tv?s效應(yīng)［8］改正誤差 （以下簡稱 E?tv?s誤差）之和為 δMi，則Δxi、Δyi如式 （2）所示:

hxi為Si沿C1滑動時，因Ci與C1不平行所致的重力變化率。hyi為Si垂直C1滑動時的重力變化率。λx和λy為兩方向滑動步長，Nx、Ny為滑動步數(shù)。因INS誤差積累，Δxi、Δyi隨i遞增，ζxi、ζyi為遞增的部分，且ζx0=ζy0=0。若考慮ζxi、ζyi的影響，式 （2）須改為

1.2 ICCP算法

ICCP算法如圖2所示。

圖2 ICCP定位示意圖Fig.2 Diagram of positioning of ICCP

mi為真實(shí)量測點(diǎn)，Ci為mi等值線，ni為INS航跡量測點(diǎn)，ri為Ci上的ni最近點(diǎn)，i=1，…，L+1。n1'至n'L為匹配航跡，L為匹配序列量測點(diǎn)數(shù)。尋找剛體變換［2］，使 { ni}Li=1與 { ri}間距離平方和最小。以下為算法步驟:

i基準(zhǔn)圖上尋找ni最近等值線點(diǎn)ri，i=1，…，L。

2）計算剛性變換，使ni移至n'i并代替原ni。

3）循環(huán)步驟1）、2）至剛性變換很小或循環(huán)次數(shù)超過閾值，得匹配序列ni，i=1，…，L。

待 { ni}匹配完畢，由 nL+1與 ni，i=2，…，L組成新序列，按以上步驟得nL+1匹配位置，以此類推。

ICCP的旋轉(zhuǎn)操作可降低INS航跡形變對定位的影響。但當(dāng)INS精度提高時，航跡形變減小，等值線分布、量測噪聲、E?tv?s誤差對ri定位的影響所導(dǎo)致的航跡旋轉(zhuǎn)角誤差，反而使旋轉(zhuǎn)操作成為ICCP的劣勢。

王虎彪等人提出了最小均方誤差旋轉(zhuǎn)擬合算法［6］，以下簡稱MMSE旋轉(zhuǎn)擬合算法，其逐步旋轉(zhuǎn)參考航跡并仿照TERCOM做平移及相關(guān)運(yùn)算以尋找相關(guān)峰。此外，算法以最優(yōu)匹配航跡始端8鄰域網(wǎng)格點(diǎn)為起始點(diǎn)尋找8條次優(yōu)航跡，與最優(yōu)航跡按相關(guān)值加權(quán)得最終匹配航跡。該算法可降低INS航跡形變對匹配的影響且不依賴局部等值線分布。但對于高精度INS，其航跡旋轉(zhuǎn)操作同樣可能影響匹配精度，且計算量較大。

作為匹配算法的基準(zhǔn)，INS精度的提高必然有利于匹配定位精度的提高，但由以上研究可知，其對不同算法的有利程度不同:完全有利于TERCOM，而對ICCP及MMSE旋轉(zhuǎn)擬合算法則兼有利弊。此外，相關(guān)匹配完全由量測信息定位，當(dāng)INS精度提高時，量測噪聲將成為影響匹配精度的重要原因。

2 濾波匹配算法

濾波匹配可分為大誤差的并行Kalman濾波匹配［5］和小誤差的局部線性化 Kalman濾波匹配算法［4］。

2.1 并行Kalman濾波匹配算法

（1）算法原理

以INS位置為中心確定搜索區(qū)域并設(shè)置規(guī)則分布的Kalman濾波器，如圖3所示。

該算法假設(shè)距載體最近的濾波器filter（*）滿

圖3 并行濾波器配置Fig.3 The configuration of parallel filter

足式 （4）的系統(tǒng)及量測方程，X（*）k、Z（*）k為狀態(tài)量與量測量。

SWRSmin為SWRS最小值，SWRSmin*為SWRSmin濾波器鄰域Ω以外的SWRS最小值。Thre為閾值，這里N為SWRSmin鄰域Ω內(nèi)濾波器連續(xù)為SWRSmin次數(shù)。最終由式 （7）得匹配位置（x*，y*）:

2.2 局部線性化Kalman濾波匹配算法

（1）算法模型

該算法系統(tǒng)方程如式 （8）。狀態(tài)為經(jīng)度、緯度位置誤差及東、北向速度誤差估計:X=［δλk，δφk，δvEk，δvNk］T。

N為濾波周期，F(xiàn)k為只考慮位置與速度信息的INS誤差方程，具體形式見文獻(xiàn) ［9］。Zk如式 （9）所示

（2）定位誤差研究及定位精度提升方案

重力場局部區(qū)域?qū)?yīng)的觀測陣Hk緩變，以下直觀描述定位受Hk緩變的影響:由位置誤差及量測量物理意義可將濾波狀態(tài)更新，可表示為式（10），dk如圖4所示，Kk為濾波增益。

圖4 位置估計更新示意圖Fig.4 Schematic diagram of the position estimate update

等值線Ck緩變對定位的影響無法避免，但可減弱與濾波增益相關(guān)的θk對匹配的影響。對于定位而言，相對重力場局部變化幅度，目前的重力測量尚未達(dá)到很高的精度;由文獻(xiàn) ［10］的分析可知，Hk緩變降低了系統(tǒng)的可觀測性，使殘差中帶有E?tv?s誤差;由圖4知，僅增大 θk幅值不能使其沿Ck分量指向真實(shí)位置O。以上因素使得根據(jù)殘差調(diào)節(jié)濾波增益的方法往往產(chǎn)生較大的匹配誤差，這與文獻(xiàn) ［11］的實(shí)驗(yàn)結(jié)果近似。為便于分析，僅考慮位置誤差系統(tǒng):因無先驗(yàn)信息，取初始方差為等元素對角陣，即P0=diag（σ2，σ2）。短時間載體航行距離短，其近似處于直角坐標(biāo)系，Φk，k－1為單位陣，Hk近似常值［hE，hN］，則有式（11）成立

可見Xk垂向等值線移動，移動幅度逐步遞減。通常設(shè)置小速度誤差方差以免位置誤差大幅震蕩，則速度誤差估計對Xk影響較小，式（12）受其影響較小。由以上分析可知，可定期重置方差陣為元素大小適宜的對角陣，以恢復(fù)濾波增益作用。此方法雖不能使Xk向真實(shí)位置O移動，但可確保減小誤差且不至誤匹配。

實(shí)際中，因?qū)T性器件常值誤差估計能力較弱，式（8）的系統(tǒng)方程存在模型誤差。此外，即使采用上述定位精度提升方案，協(xié)方差分析過程使Xk采用逐步遞減的方式接近等值線，當(dāng)INS誤差增速較快時，其校正誤差的能力反而減弱;當(dāng)INS精度較高時，系統(tǒng)模型誤差降低，Xk偏離等值線的速度降低，此時協(xié)方差分析的抗噪聲能力成為濾波匹配算法的優(yōu)勢。

3 仿真實(shí)驗(yàn)

以下驗(yàn)證不同精度INS下的匹配算法定位情況。因較難獲取實(shí)測重力數(shù)據(jù)及多種精度的INS數(shù)據(jù)，以TOPEX衛(wèi)星的某海域重力數(shù)據(jù)及仿真INS數(shù)據(jù)進(jìn)行匹配實(shí)驗(yàn)。采用的重力基準(zhǔn)圖如圖5（分辨率1'×1'）所示。

圖5 Topex衛(wèi)星重力異常數(shù)據(jù)Fig.5 Gravity anomaly data of TOPEX

設(shè)陀螺、加速度計常值誤差由 εc、Ac表示，則仿真采用3種INS，INS1:εc=0.01（°）/h、Ac=10－5g;INS2:εc=0.005（°）/h、Ac=5 × 10－6g;INS3:εc=0.001（°）/h、Ac=10－6g， 陀螺及加速度計隨機(jī)誤差標(biāo)準(zhǔn)差為0.5倍常值誤差。載體航速8節(jié)，航向北偏東50°。為驗(yàn)證大誤差匹配能力，使3種INS具有接近的大初始誤差:設(shè)INS1～3進(jìn)入導(dǎo)航區(qū)域前，分別航行5小時、13.33小時及75小時，積累位置誤差5.86'、6.13'及5.98'，且3種INS的真實(shí)航跡非常接近。匹配實(shí)驗(yàn)時間35小時。INS航跡、真實(shí)航跡及INS航跡的經(jīng)度、緯度誤差如圖6、圖7所示。

圖6 各INS航跡及真實(shí)航跡Fig.6 The tracks of INS1 to INS3 and real track

圖7 INS1～3的位置誤差Fig.7 The positioning error of INS1 to INS3

實(shí)驗(yàn)中設(shè)重力量測噪聲為0均值，標(biāo)準(zhǔn)差為1mGal的白噪聲。以TERCOM、ICCP、MMSE旋轉(zhuǎn)擬合、并行Kalman濾波、組合濾波匹配進(jìn)行實(shí)驗(yàn)。實(shí)驗(yàn)中各算法設(shè)置如下。

相關(guān)匹配的重力量測周期為6min，匹配序列含20個量測點(diǎn)。TERCOM航跡搜索半徑為10'，經(jīng)度、緯度方向搜索步長為0.1'。ICCP采用先TERCOM而后轉(zhuǎn)入ICCP的組合方式，其迭代次數(shù)閾值為20次，最近點(diǎn)搜索半徑為7'。MMSE旋轉(zhuǎn)擬合算法航跡旋轉(zhuǎn)角范圍為－8°～8°，旋轉(zhuǎn)角增量步長為15'，航跡搜索設(shè)置同TERCOM。

并行Kalman濾波算法濾波周期為10min，濾波器間隔為0.5'，搜索半徑含40個濾波器，Thre=1.7，α=0.17，INS1～3的Ω分別為以SWRSmin為中心的7×7、5×5、3×3濾波器陣列。因局部線性化Kalman濾波適用小位置誤差，將其與并行Kalman濾波組合，構(gòu)成搜索 （并行Kalman）與跟蹤 （局部線性化Kalman）模式，簡稱組合濾波匹配模式。其先進(jìn)入搜索模式，待有定位輸出后轉(zhuǎn)入跟蹤模式，若殘差連續(xù)5個周期超過閾值則返回搜索模式，以此循環(huán)往復(fù)。INS1～3的殘差閾值為10mGal、7mGal、5mGal。跟蹤模式濾波周期為1min，方差重置周期為2.5h。

每種算法各進(jìn)行20次實(shí)驗(yàn)以統(tǒng)計 RMSE。INS1～3的RMSE如圖8～圖10，RMSE統(tǒng)計數(shù)據(jù)見表1～表3（濾波匹配從有穩(wěn)定的定位輸出開始計算統(tǒng)計值;表中增加總誤差以比較各算法優(yōu)劣:總誤差=［經(jīng)度誤差2+緯度誤差2］）。

圖8 INS1各算法RMSEFig.8 RMSE of each matching algorithm for INS1

圖9 INS2各算法RMSEFig.9 RMSE of each matching algorithm for INS2

圖10 INS3各算法RMSEFig.10 RMSE of each matching algorithm of INS3

表1 INS1各算法RMSE統(tǒng)計數(shù)據(jù)Tab.1 Statistical data of RMSE with different matching algorithms for INS1 （'）

表2 INS2各算法RMSE統(tǒng)計數(shù)據(jù)Tab.2 Statistical data of RMSE with different matching algorithms for INS2 （'）

表3 INS3各算法RMSE統(tǒng)計數(shù)據(jù)Tab.3 Statistical data of RMSE with different matching algorithms for INS3 （'）

仿真條件:CPU P43.2GHz，1GB內(nèi)存，Matlab2007仿真環(huán)境。各算法單航程平均運(yùn)行時間為，TERCOM:45.6s，ICCP:660.9s，MMSE 旋轉(zhuǎn)擬合:1148.0s，組合濾波匹配:18.9s。可見組合濾波及TERCOM計算量較小，而MMSE旋轉(zhuǎn)擬合算法的計算量較大。

由實(shí)驗(yàn)數(shù)據(jù)可知，對于濾波匹配，組合濾波匹配優(yōu)于單純的并行Kalman濾波匹配;對于相關(guān)匹配，隨INS精度提高，各算法的表現(xiàn)為，INS1:MMSE旋轉(zhuǎn)擬合及ICCP的適用性較好且MMSE略優(yōu)，TERCOM最差 （存在大誤差情況），以最優(yōu)算法總誤差均值為基準(zhǔn)，TERCOM與MMSE旋轉(zhuǎn)擬合算法的精度相差59.6%;INS2:TERCOM及MMSE旋轉(zhuǎn)擬合算法適用性較好且TERCOM略優(yōu)，以最優(yōu)算法總誤差均值為基準(zhǔn)，二者精度相差9%;INS3:TERCOM最優(yōu)，其與MMSE旋轉(zhuǎn)擬合的精度相差 28.6%?？梢婋S INS精度提升，TERCOM相對MMSE旋轉(zhuǎn)擬合算法更加適用。而INS精度提高時，等值點(diǎn)分布對計算ICCP航跡旋轉(zhuǎn)角的影響較大，其穩(wěn)定性稍差。

INS1的MMSE旋轉(zhuǎn)擬合精度略優(yōu)于組合濾波匹配，二者精度相差7.4%;INS2的組合濾波匹配與TERCOM精度差距微弱，相差1.3%;INS3的組合濾波匹配優(yōu)于 TERCOM，二者精度相差10.1%。

以上數(shù)據(jù)中，相關(guān)匹配與濾波匹配的精度差距并不明顯，但當(dāng)量測噪聲標(biāo)準(zhǔn)差增至2mGal、3mGal時，INS3的濾波匹配基本不受影響，TERCOM誤差明顯增大，表明了濾波匹配在高精度INS下的適用性。而對于INS1，增大量測噪聲時，各算法誤差均增大且MMSE旋轉(zhuǎn)擬合仍最為適用，原因?yàn)?增大量測噪聲使濾波匹配的Xk（如圖4）趨向等值線的步長減小，減弱抑制INS誤差的能力，而INS1的誤差快速增長，所以濾波匹配算法誤差亦增大。因篇幅原因，此處僅說明增大量測噪聲后的匹配結(jié)果，而省略實(shí)驗(yàn)數(shù)據(jù)。

4 結(jié)論

本文指出，因不同匹配算法的特點(diǎn)不同，INS精度的變化導(dǎo)致匹配算法的適用性變化，并得出結(jié)論:相關(guān)匹配抑制INS誤差對定位影響的能力較強(qiáng)，但不具有抗量測噪聲能力;濾波匹配可處理量測噪聲，但其抑制INS誤差的能力相對較弱。因此對于中等精度INS（誤差增速≈1n mile/h），相關(guān)匹配的MMSE旋轉(zhuǎn)擬合算法適用性較好;對于高精度INS（誤差增速≈1n mile/d），濾波匹配的適用性較好。本文所得結(jié)論為搭載不同精度INS載體的重力場匹配算法選擇提供依據(jù)，具有一定現(xiàn)實(shí)意義。

［1］Lowreys J A.Passive navigation using inertial navigation sensors and maps［J］.Naval Eng J，1997，5:245－251.

［2］BEHZAD K P，BEHROOZ K P.Vehicle localization on gravity maps［C］//.Proceedings of SPIE－The International Society for Optical Engineering，1999，3693:182－191.

［3］閆利，崔晨風(fēng)，吳華玲.基于TERCOM算法的重力匹配［J］.武漢大學(xué)學(xué)報 （信息科學(xué)版），2009，34（3）:261－264.

［4］秦政，邊信黔，施小成，等.水下運(yùn)載體重力輔助慣性導(dǎo)航系統(tǒng)仿真平臺［J］.武漢大學(xué)學(xué)報 （信息科學(xué)版），2008，33（7）:755－758.

［5］戴全發(fā)，許大欣，蔡小波，等.重力異常匹配輔助導(dǎo)航解算模型的優(yōu)化［J］.大地測量與地球動力學(xué)，2007，27（4）:31－34.

［6］王虎彪，王勇，方劍，等.“最小均方誤差旋轉(zhuǎn)擬合法”重力輔助導(dǎo)航仿真研究［J］.中國科學(xué):地球科學(xué)，2012，42（7）:1055－1062.

［7］Yan Li，Wu Hua－ling.Research on the selection of suitable matching area in the gravity aided navigation［C］//.Proceedings of the SPIE:the International Society for Optical Engineering，2007，6752，67523U.

［8］黃謨濤，翟國軍，管錚，等.海洋重力場測定及其應(yīng)用［M］.北京:測繪出版社，2005.

［9］黃德明，程祿.慣性導(dǎo)航系統(tǒng)［M］.北京:國防工業(yè)出版社，1986.

［10］Chen Zhe.Local observability and its application to multiple measurement estimation［J］.IEEE Transactions on Industrial E－lectronics，1991，38（6）:491－496.

［11］王偉，李珊珊.重力異常卡爾曼濾波匹配算法的改進(jìn)［A］.第四屆中國衛(wèi)星導(dǎo)航學(xué)術(shù)年會論文集［C］//.2013.