蔡體菁，魏琪鷺，王新宇

(1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，江蘇 南京 210096；2. 中國電子科技集團(tuán)公司第二十六研究所，重慶 400060)

0 引言

重力輔助慣性導(dǎo)航系統(tǒng)(GAINS)使用重力異常或重力梯度值作為輔助信息來校正慣性導(dǎo)航系統(tǒng)(INS)隨時(shí)間累積的定位誤差，并補(bǔ)償陀螺儀和加速度的漂移誤差。GAINS具有自主性高，隱蔽性好，定位精度高，抗干擾能力強(qiáng)等優(yōu)點(diǎn)。因此，在海洋和航空航天領(lǐng)域GAINS應(yīng)用較廣[1-2]。

重力匹配算法是GAINS的關(guān)鍵技術(shù)，關(guān)鍵在于結(jié)合重力和INS指示航跡兩方面的信息得到精確的位置結(jié)果。通過比較重力測(cè)量值序列和重力異常圖中序列的相似性，得到最優(yōu)匹配結(jié)果。其中針對(duì)重力數(shù)據(jù)的處理方法主要有相關(guān)極值分析[3-4]、圖像配準(zhǔn)、擴(kuò)展卡爾曼濾波[5-7]、粒子濾波[8-10]、地形輪廓匹配(TERCOM)[11-14]、迭代最近等值點(diǎn)(ICCP)[15-17]和直線段匹配[18]等。

本文提出了一種基于相關(guān)極值分析的兩步式重力匹配算法。匹配算法的核心是基于相關(guān)極值算法的差分目標(biāo)函數(shù)。首先通過均方差相關(guān)算法(MSD)快速粗匹配得到剛性變換的目標(biāo)位置，并通過剛性變換將INS軌跡變換到真實(shí)軌跡附近；然后精確MSD匹配，通過篩選重力等值點(diǎn)和根據(jù)匹配約束條件搜索匹配線段，依據(jù)貪心算法得到最佳匹配軌跡，即為兩步式匹配算法的最終結(jié)果。

1 基于MSD的差分相關(guān)目標(biāo)函數(shù)模型

常用相關(guān)分析匹配算法包括交叉相關(guān)(COR)算法、平均絕對(duì)離差相關(guān)(MAD)算法和均方差相關(guān)(MSD)算法[4,19]。一般MSD算法性能最優(yōu)，因此,重力觀測(cè)序列與重力異常圖提取序列間基于MSD構(gòu)建目標(biāo)函數(shù)J(λk,φk)為

(1)

運(yùn)載體上測(cè)得的重力為地球引力與在慣性系下運(yùn)載體隨地球自轉(zhuǎn)時(shí)產(chǎn)生離心力的合力，受運(yùn)載體速度及其所在緯度的影響很大，即厄特弗斯效應(yīng)。設(shè)vEi和vNi為i時(shí)刻載體的東向和北向速度，φi為載體當(dāng)?shù)鼐暥龋琑為地球半徑，載體在該點(diǎn)的厄特弗斯改正計(jì)算式為

(2)

由于INS誤差隨時(shí)間增加而積累，運(yùn)載體速度和緯度誤差會(huì)隨之增大，重力異常改正結(jié)果的誤差也會(huì)隨之增大，最終產(chǎn)生重力匹配的虛假定位。針對(duì)減小厄特弗斯效應(yīng)的影響，對(duì)式(1)采用差分思想改進(jìn)，以前、后2個(gè)連續(xù)的重力異常測(cè)量值之差為新的觀測(cè)量[19-20]，可在消除厄特弗斯效應(yīng)影響的同時(shí)，消除重力測(cè)量系統(tǒng)誤差的影響，該差分匹配目標(biāo)函數(shù)為

(3)

2 快速M(fèi)SD匹配方法和剛性變換

2.1 快速M(fèi)SD匹配方法

為了縮小搜索范圍，各INS航跡點(diǎn)的重力等值點(diǎn)搜索區(qū)域由INS漂移范圍(一般由瑞利分布確定)確定。為減少計(jì)算量，基于INS漂移范圍，各點(diǎn)均選取相同且合適的固定搜索區(qū)域∑，使真實(shí)航跡點(diǎn)包含在∑中。為簡化搜索過程和適用于網(wǎng)格化的重力異常圖，假設(shè)∑是該INS指示位置所在網(wǎng)格上、下、左、右M個(gè)網(wǎng)格的范圍內(nèi)，共計(jì)S=(2M+1)2個(gè)網(wǎng)格區(qū)域。

快速M(fèi)SD匹配方法以N個(gè)重力異常測(cè)量點(diǎn)為一組，構(gòu)成一個(gè)匹配序列，從第N個(gè)點(diǎn)開始匹配。把匹配序列各時(shí)刻對(duì)應(yīng)的INS指示位置的搜索區(qū)域∑中第i個(gè)網(wǎng)格點(diǎn)提取出一條匹配曲線Bi。依據(jù)式(3)可從S條匹配曲線中得到一個(gè)最優(yōu)的匹配結(jié)果Bk。由于INS在短時(shí)間內(nèi)具有較高的精度，可用Bk近似對(duì)應(yīng)的真實(shí)軌跡，因此,選取Bk的終點(diǎn)作為一個(gè)匹配結(jié)果[21]。

由于一次匹配只比較有限的S個(gè)序列的目標(biāo)函數(shù)，該方法可快速得到一個(gè)粗略的匹配結(jié)果，但該方法受限于INS誤差累積不能過多，否則在一定時(shí)間積累后，INS軌跡和真實(shí)軌跡形狀和方向差異過大，會(huì)使匹配出現(xiàn)很大誤差。因此，從該方法得到的結(jié)果中提取合理的匹配結(jié)果，可作為接下來精確匹配的基準(zhǔn)位置。

2.2 提取合適匹配點(diǎn)

快速M(fèi)SD匹配結(jié)果的單點(diǎn)誤差可以分為兩種情況，一種是匹配結(jié)果誤差小于一定值，如在實(shí)驗(yàn)中設(shè)定誤差小于0.05°，另一種是匹配結(jié)果與真實(shí)結(jié)果相差遠(yuǎn)。考慮到INS在較短時(shí)間內(nèi)相對(duì)漂移很小，相鄰連續(xù)2個(gè)真實(shí)位置的間距和角度與相應(yīng)慣性導(dǎo)航輸出點(diǎn)距離和角度應(yīng)該近似相等或相差很小[4]，引入約束條件：

(4)

(5)

式中:(xt,yt)、(xt+1,yt+1)分別為前、后連續(xù)2個(gè)相鄰時(shí)刻的坐標(biāo)，均在相應(yīng)置信區(qū)間內(nèi)；ε1、ε2為接近于0的誤差；αINS為INS輸出的位移角度；LINS為INS輸出的位移距離。

由于快速M(fèi)SD匹配方法是粗匹配，故將ε1和ε2約束放寬。當(dāng)遇到不滿足約束條件的匹配點(diǎn)時(shí)，將該點(diǎn)記為不滿足點(diǎn)，否則為滿足點(diǎn)，當(dāng)用約束條件判斷下一個(gè)點(diǎn)時(shí)，不使用它前一個(gè)不滿足點(diǎn)，而使用最近的一個(gè)滿足點(diǎn)進(jìn)行判斷。這種方法提取的滿足點(diǎn)是快速M(fèi)SD匹配的合理匹配結(jié)果。

2.3 剛性變換

利用剛性變換將INS指示航跡變換到真實(shí)航跡處，則INS指示航跡形狀與真實(shí)航跡形狀相同，故INS相對(duì)誤差都保留。

(6)

圖1 一般剛性變換

如圖2所示，將快速M(fèi)SD匹配方法提取的合格匹配點(diǎn)擬合得到線段l1，相對(duì)應(yīng)的INS軌跡點(diǎn)也擬合得到線段l2，可計(jì)算出兩條直線的位置和角度關(guān)系，由此確定剛性變化中整體平移(Δx,Δy)和旋轉(zhuǎn)Δθ。然后利用式(6)的剛性變化，將INS軌跡變換到MSD軌跡指示的新待匹配位置，此時(shí)該剛性變換后的INS軌跡距離真實(shí)軌跡較近，成為新的待匹配軌跡，為下面的精確匹配方法縮小了搜索等值點(diǎn)的范圍，減少了搜索時(shí)間。

圖2 基于快速M(fèi)SD方法的航跡剛性變換

3 精確匹配算法

當(dāng)航行時(shí)間較短或初始位置誤差較小時(shí)，搜索區(qū)域較小，可較快地通過等值點(diǎn)搜索到合適的匹配線段。而當(dāng)航行時(shí)間較長或初始位置誤差較大時(shí)，搜索區(qū)域過大，直接通過等值點(diǎn)搜索匹配線段搜索到的等值點(diǎn)過多，匹配耗費(fèi)時(shí)間過長，影響算法效率和實(shí)時(shí)性。因此，將快速M(fèi)SD結(jié)果作剛性變換基準(zhǔn)得到的線段作為新的待匹配航跡，根據(jù)之前時(shí)刻的位置誤差和INS漂移誤差得到新的置信區(qū)間，然后重新搜索等值點(diǎn)和匹配線段。因?yàn)閯傂宰儞Q產(chǎn)生的新航跡與原始INS指示航跡相同，所以使用新航跡作為等值點(diǎn)匹配方法的被匹配航跡并無影響。同時(shí)由于新置信區(qū)間比舊搜索區(qū)域小，搜索到的等值點(diǎn)數(shù)少很多，可避免大量誤匹配等值點(diǎn)和誤匹配等值線段，提高算法效率和實(shí)時(shí)性。

3.1 搜索等值點(diǎn)

3.2 搜索匹配線段

只對(duì)待匹配軌跡的第1個(gè)位置的搜索區(qū)域∑2遍歷搜索，搜索重力等值點(diǎn)作為匹配線段搜索起點(diǎn)，其他位置通過搜索線段的方式搜索。由于第2節(jié)中剛性變換縮小了搜索范圍，新搜索區(qū)域∑2遠(yuǎn)小于第2節(jié)的搜索區(qū)域∑。

搜索線段的方法是，在已有第L層線段起點(diǎn)Pf的情況下，利用待匹配軌跡對(duì)應(yīng)的相鄰點(diǎn)間距離關(guān)系，可得線段的終點(diǎn)Pb，又基于式(4)、(5)，將Pb附近一定范圍內(nèi)的點(diǎn)都視為線段的可能終點(diǎn)。

從多個(gè)可能終點(diǎn)中篩選出匹配線段終點(diǎn)的方法是，先檢查各可能終點(diǎn)是否為重力等值點(diǎn)，然后計(jì)算線段前、后兩點(diǎn)的重力差分誤差平方值，作為該匹配線段的代價(jià)。該差分誤差為式(3)差分誤差平方和的一個(gè)組成部分。因此，通過下式設(shè)定的閾值判斷是否保留該可能終點(diǎn)，舍棄差分誤差過大的終點(diǎn)。

(7)

其中閾值δ2由重力傳感器系統(tǒng)誤差和厄特弗斯效應(yīng)決定

3.3 連接等值線段和搜索最小路徑

連接相鄰層間的匹配線段，并基于貪心算法思想，通過記錄和更新當(dāng)前路徑代價(jià)和，使每個(gè)節(jié)點(diǎn)僅記錄從第一層節(jié)點(diǎn)到該節(jié)點(diǎn)最小路徑中的該節(jié)點(diǎn)前一節(jié)點(diǎn)位置，舍棄其他非最小路徑。

當(dāng)線段連接完成后，在最后一層尋找最小的當(dāng)前路徑代價(jià)和，然后利用該節(jié)點(diǎn)存儲(chǔ)的該路徑前一節(jié)點(diǎn)，逐層尋找直到第一層，最后可得精確匹配結(jié)果。

4 兩步式匹配方法效果

將已有重力圖利用克里金插值方法細(xì)化為0.001°×0.001°的網(wǎng)格。運(yùn)動(dòng)軌跡和INS導(dǎo)航軌跡均由仿真生成。INS參數(shù)設(shè)定為加速度計(jì)逐次啟動(dòng)漂移,即5×10-5g(g=9.780 369 8 m/s2)，激光陀螺隨機(jī)漂移為0.01 (°)/h，初始位置誤差大于0.5°，初始角度誤差為5°。運(yùn)載體航行速度為15 m/s，每120 s選取1個(gè)重力采樣點(diǎn)，采集20個(gè)采樣點(diǎn)進(jìn)行匹配，選擇3條航向角ψ分別為0°、90°、-45°，經(jīng)過重力適配區(qū)的真實(shí)航跡如圖3～5所示。

圖3 航向角為0°的匹配結(jié)果

圖4 航向角為90°的匹配結(jié)果

圖5 航向角為45°的匹配結(jié)果

快速M(fèi)SD匹配算法匹配10個(gè)點(diǎn)耗時(shí)15～20 s，基于快速M(fèi)SD匹配結(jié)果的剛性變換耗時(shí)0.01～0.02 s，且剛性變換結(jié)果的誤差小于0.05°，故在此基礎(chǔ)上，設(shè)定0.1°×0.1°等值點(diǎn)搜索范圍，使用合適的搜索范圍，可在地圖適合匹配的情況下較快地得到精確匹配結(jié)果，匹配結(jié)果如圖6～8和表1所示，可精確到500 m內(nèi)，且比單純快速M(fèi)SD匹配結(jié)果更優(yōu)。而如果地圖不適合匹配，花費(fèi)較多時(shí)間也能得到理想結(jié)果。

圖6 航向角為0°的單點(diǎn)匹配誤差

圖7 航向角為90°的單點(diǎn)匹配誤差

圖8 航向角為-45°的單點(diǎn)匹配誤差

匹配誤差/m航向角為0°航向角為90°航向角為-45°快速M(fèi)SD均值926.820 6813.245 9758.311 1MSD284.249 7313.232 9284.628 2兩步式均值324.778 3372.047 6386.564 7MSD258.479 9158.810 5300.422 6

5 結(jié)論

1) 快速M(fèi)SD法可快速得到匹配結(jié)果，但當(dāng)匹配條件不理想時(shí)，如INS軌跡誤差稍大或?qū)Ш浇嵌日`差存在或航行區(qū)域粗糙度不足，快速M(fèi)SD結(jié)果可能有很大的誤差。

2) 精確匹配算法需選擇一個(gè)合理搜索范圍，搜索區(qū)域小時(shí)搜索時(shí)間短，如果搜索區(qū)域過大，搜索時(shí)間爆發(fā)式增長，還可能造成誤匹配的情況。

3) 使用剛性變換將兩種方法結(jié)合在一起，由于剛性變換法保留原始INS軌道的形狀特征，不影響精確匹配的約束條件，匹配結(jié)果的精度無改變。在這種情況下，較小的搜索區(qū)域會(huì)減少大量誤匹配的等值點(diǎn)和等值線段。兩步式匹配法可在保證匹配精度的情況下用相對(duì)較小的時(shí)間完成大范圍搜索，且能適應(yīng)INS初始位置誤差較大和導(dǎo)航角度誤差存在的情況。

4) 實(shí)際應(yīng)用中，只有航跡在一個(gè)粗糙度足夠的重力適配區(qū)內(nèi)時(shí)，才可以使用基于相關(guān)極值的重力匹配算法，否則有可能產(chǎn)生誤匹配的情況。

5) 兩步式算法也存在局限性，此方法更適用于大初始位置誤差的情況，如果已知初始位置誤差較小，直接使用精確匹配算法速度更快，更具實(shí)時(shí)性。