一種動態(tài)序列重力匹配算法

徐孝萍,蔡體菁, 倪江生

( 東南大學(xué) 儀器科學(xué)與工程學(xué)院,江蘇 南京 210096)

0 引言

海洋中蘊(yùn)藏著豐富的物質(zhì)資源和戰(zhàn)略資源,水下載體導(dǎo)航技術(shù)在海洋的探測和開發(fā)中必不可少,在當(dāng)前海洋強(qiáng)國的發(fā)展趨勢背景下具有相當(dāng)重要的應(yīng)用前景[1]。普遍采用的水下導(dǎo)航系統(tǒng)為慣性導(dǎo)航系統(tǒng)(INS)[2]。慣性導(dǎo)航具有很強(qiáng)的屏蔽性和抗干擾能力等[3],但其最大的缺點(diǎn)是精度隨著導(dǎo)航時(shí)間增長而降低[4],所以,需要采用重力匹配輔助慣性導(dǎo)航。重力匹配導(dǎo)航不依賴外界信息交換,只借助地球物理特性完成潛艇水下長航的導(dǎo)航技術(shù)[5]。

重力匹配算法按照采樣方式分為單點(diǎn)匹配和序列匹配[6]。單點(diǎn)匹配主要為基于擴(kuò)展卡爾曼濾波的桑迪亞慣性地形輔助導(dǎo)航算法(SITAN)[6],序列匹配主要有迭代最近等值線算法(ICCP)和地形輪廓匹配算法(TERCOM)[7]。

本文提出了一種動態(tài)序列重力匹配算法。首先根據(jù)慣導(dǎo)系統(tǒng)提供位置信息,對航跡按距離進(jìn)行多起點(diǎn)采樣構(gòu)建軌跡序列,然后對每個(gè)起點(diǎn)軌跡序列進(jìn)行相關(guān)極值匹配,最后動態(tài)選擇一條最優(yōu)估計(jì)輸出。該算法減小了序列匹配算法中由于構(gòu)造觀測序列起點(diǎn)選取不同而導(dǎo)致的匹配結(jié)果存在差異,提高了重力匹配的精度和穩(wěn)定性。

1 動態(tài)序列重力匹配算法

1.1 動態(tài)序列匹配思想

圖2 動態(tài)序列匹配

圖3 動態(tài)匹配算法流程

圖4 設(shè)置搜索范圍

1.2 多起點(diǎn)構(gòu)造觀測軌跡

計(jì)算出一個(gè)網(wǎng)格的長度記為S,一條軌跡采樣點(diǎn)個(gè)數(shù)為n。具體步驟如下:

表1 二維數(shù)組arrins(b,n)表格

2) 將b個(gè)起點(diǎn)對應(yīng)的b條觀測軌跡,即表1中數(shù)組的每行對應(yīng)的軌跡進(jìn)行相關(guān)極值匹配。

1.3 相關(guān)極值匹配

1.3.1 搜索范圍設(shè)置

在慣性導(dǎo)航系統(tǒng)中,由于陀螺儀和加速度計(jì)的累計(jì)誤差,使得潛航器的慣導(dǎo)在輸出此時(shí)刻位置的經(jīng)緯度時(shí)具有漂移現(xiàn)象。實(shí)際潛航器的位置應(yīng)該在預(yù)定的航行區(qū)域和以慣導(dǎo)輸出位置為中心的一定范圍內(nèi),隨著航行位置移動而移動。

一般認(rèn)為同一條軌跡序列,每個(gè)采樣點(diǎn)之間的距離間隔為d,則需要d≥S。軌跡長度為S′=n×d。根據(jù)起始點(diǎn)的初始經(jīng)緯度設(shè)定搜索區(qū)域范圍(L′×L′)(見圖2)。要確保真實(shí)位置軌跡在重力基準(zhǔn)圖搜索區(qū)域范圍內(nèi),如此可確保匹配的實(shí)時(shí)性和準(zhǔn)確性,隨著水下潛航器的位置改變,重力基準(zhǔn)圖上的搜索范圍也隨著航跡不斷改變。

1.3.2 剛性變換

如果能夠獲得構(gòu)建的慣導(dǎo)軌跡序列S′相對于實(shí)際航跡的平移量(dx,dy)和偏轉(zhuǎn)角β,可得實(shí)際的航行軌跡。因此,利用旋轉(zhuǎn)和平移對INS輸出的航跡點(diǎn)的位置進(jìn)行剛性變換,可得到多條待匹配軌跡。

本文以2個(gè)采樣點(diǎn)為例。設(shè)構(gòu)建的慣導(dǎo)航跡序列第一個(gè)采樣點(diǎn)的位置為A,第二個(gè)采樣點(diǎn)的位置為B,用二維矩陣旋轉(zhuǎn)得到B′點(diǎn),然后平移到點(diǎn)B″,將該點(diǎn)假設(shè)為可能的真實(shí)位置,即待匹配軌跡點(diǎn),具體旋轉(zhuǎn)平移示意圖如圖5所示。

圖5 二維矩陣旋轉(zhuǎn)與平移示意圖

以第k個(gè)起點(diǎn)構(gòu)建的待匹配軌跡為例,具體步驟如下:

1) 為了保證匹配的實(shí)時(shí)性,當(dāng)二維數(shù)組中,每行數(shù)據(jù)達(dá)到m個(gè)開始匹配。

2) 將這m個(gè)數(shù)據(jù)中第2個(gè)點(diǎn)至第m個(gè)點(diǎn)(同上述B點(diǎn))的位置相對第1個(gè)點(diǎn)A,利用二維矩陣進(jìn)行旋轉(zhuǎn)與平移,在重力基準(zhǔn)圖中構(gòu)建待匹配軌跡。剛性變換構(gòu)建待匹配軌跡過程如下:

(1)

(2)

b.以x軸為0°參考線,第i點(diǎn)相對于第1個(gè)點(diǎn)的角度為α,設(shè)需要旋轉(zhuǎn)的角度為β′=β/180×π。

c.旋轉(zhuǎn)后,B′相對于A點(diǎn)經(jīng)緯度方向上的距離:

(3)

(4)

d.設(shè)x軸方向平移dx距離,y軸方向平移dy距離。平移和旋轉(zhuǎn)后,構(gòu)建的待匹配的序列坐標(biāo)B″相對于A點(diǎn)經(jīng)緯度方向上的距離:

(5)

(6)

3) 之后航跡數(shù)據(jù)每增加1個(gè),重復(fù)步驟2),直到每行數(shù)據(jù)數(shù)達(dá)到n個(gè)。使待匹配軌跡的采樣點(diǎn)逐漸增多,重力變化特征更明顯,匹配效果越好。

4) 為了提高實(shí)時(shí)性和匹配精度,快速啟動匹配,使用了滑動窗口機(jī)制維護(hù)數(shù)組。當(dāng)每行數(shù)據(jù)個(gè)數(shù)達(dá)到n個(gè),刪去該行第1個(gè)點(diǎn)的數(shù)據(jù),將其后每個(gè)點(diǎn)向前移動一位,新輸入點(diǎn)的數(shù)據(jù)放在此行的最后,數(shù)組向前滑動,然后重復(fù)步驟2)。

1.3.3 待匹配軌跡點(diǎn)插值

1.3.4 相關(guān)極值匹配

(7)

式中:j=1～a;n為一條軌跡序列的采樣點(diǎn)個(gè)數(shù)。

1.4 動態(tài)選擇最優(yōu)估計(jì)

2 試驗(yàn)結(jié)果

試驗(yàn)數(shù)據(jù)來自某船在某海域?qū)崪y的激光捷聯(lián)慣導(dǎo)系統(tǒng)與重力儀數(shù)據(jù)。船上有GPS接收機(jī),其位置指示作為船的真實(shí)位置,對重力匹配精度進(jìn)行核驗(yàn)。重力異常圖分辨率為1′×1′。軌跡采樣點(diǎn)個(gè)數(shù)n=20。每條軌跡序列相鄰采樣點(diǎn)的間隔d≥1′。50條軌跡每完成一次匹配,通過相關(guān)極值匹配,選擇一條最優(yōu)估計(jì)的軌跡最后一點(diǎn)作為動態(tài)序列匹配算法的輸出。

圖6、7分別為兩段航行區(qū)域的GPS真實(shí)軌跡、慣導(dǎo)輸出軌跡、MSD匹配算法曲線和動態(tài)匹配算法曲線軌跡圖。圖8、9分別為上述兩段航行區(qū)域的GPS真實(shí)軌跡與慣導(dǎo)輸出軌跡、MSD匹配算法曲線和動態(tài)匹配算法誤差曲線圖。

圖6 航行軌跡1與匹配曲線軌跡圖

圖7 航行軌跡2與匹配曲線軌跡圖

圖8 航行軌跡1曲線誤差圖

圖9 航行軌跡2曲線誤差圖

3 結(jié)束語

本文給出的基于相關(guān)極值的動態(tài)序列重力匹配算法具有精度高,穩(wěn)定性好及實(shí)時(shí)性好的優(yōu)點(diǎn),能夠?yàn)樗螺d體長時(shí)間航行提供導(dǎo)航信息。