蔡體菁,魏琪鷺,王新宇
(1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院,江蘇 南京 210096;2. 中國電子科技集團(tuán)公司第二十六研究所,重慶 400060)
重力輔助慣性導(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é)果。
常用相關(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)
為了縮小搜索范圍,各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)位置。
快速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é)果。
利用剛性變換將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方法的航跡剛性變換
當(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í)性。
只對(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)決定
連接相鄰層間的匹配線段,并基于貪心算法思想,通過記錄和更新當(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é)果。
將已有重力圖利用克里金插值方法細(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
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í)性。