基于仿射變換的地磁匹配定位算法

嚴羽靈，唐清善，白 創(chuàng)

(長沙理工大學 物理與電子科學學院，湖南 長沙 410114)

地磁場是屬于地球的天然資源[1]。大部分動物能夠利用地磁場提供的地磁信息進行導航[2-3]。包括鯨魚、海龜、候鳥在內(nèi)的遷徙動物[4]受各種氣候環(huán)境的影響還可測定精確的位置。地磁導航包括地磁場模型[5]建立、地磁場實時測量和地磁導航算法[6]。地磁導航算法可以分為地磁匹配算法[7]和地磁濾波算法。地磁濾波算法雖然實時性強，但是地磁場相對地理位置具有較強的非線性，導致濾波容易發(fā)散。地磁匹配算法[8-9]原理簡單，且匹配精度較高，在地磁導航中應用較多。

目前比較常見的算法是地磁場等值線匹配(Magnetic Contour Matching，MAGCOM)算法和迭代最近等值線匹配(Iterative Closest Contour Point，ICCP)算法[10]。MAGCOM 算法只能修正位移誤差，不能校正角度誤差[11]。而 ICCP 算法[12-13]只能修正角度誤差和位移誤差，不能修正伸縮誤差，因此精度不高。本文提出了一種基于仿射變換的地磁匹配定位算法。仿射變換[14-15]比較常見的有旋轉(zhuǎn)、平移、伸縮變換、反射和剪切。由于慣導系統(tǒng)輸出的軌跡與真實軌跡之間只存在位移、角度和伸縮誤差，所以只考慮前面3種變換。為了實現(xiàn)該算法，本文改進了第12代國際地磁參考場模型，并輸出地磁數(shù)據(jù)庫。再通過MATLAB對參考軌跡、真實軌跡以及該算法進行了仿真。然后通過Mathematica中的迭代和數(shù)值求解函數(shù)解非線性方程組，并將解輸入到MATLAB中進行匹配定位，將各組解的匹配效果進行了比較。

1 仿射變換算法原理

1.1 問題描述

慣導系統(tǒng)存在隨時間累加的定位誤差，導致慣導系統(tǒng)所輸出的軌跡與真實軌跡之間存在位移、角度和伸縮誤差。該情況可以用如圖1所示的軌跡示意圖來描述。

圖1 軌跡示意圖

其中，曲線Rf表示捷聯(lián)慣導系統(tǒng)輸出的參考軌跡；曲線M是經(jīng)過仿射變換算法得到的匹配軌跡，其位于真實軌跡Re附近；作曲線M的平行線M′。從圖1可以看出匹配軌跡與參考軌跡存在角度誤差α。除了角度誤差，還存在初始位置誤差和伸縮誤差，Δx和Δy分別表示初始經(jīng)度、緯度誤差；k1和k2表示經(jīng)度、緯度方向的伸縮誤差。令(a,b)T是參考軌跡上的一點(a表示經(jīng)度，b表示緯度)，參考軌跡的初始點坐標為(a1,b1)T，那么(a,b)T對應的匹配軌跡上的點是(u,w)T。

1.2 仿射變換模型

仿射變換是二維坐標到二維坐標之間的線性變換，能夠保持二維圖形的“平直性”和“平行性”，即直線經(jīng)變換后還是直線，圓弧經(jīng)變換后還是圓弧，且二維圖形之間的相對位置關(guān)系不會變。它的變化包括旋轉(zhuǎn)、平移、伸縮、反射和剪切。本文針對慣導系統(tǒng)匹配定位，根據(jù)具體問題只考慮前面3種。圖2所示為仿射變換-旋轉(zhuǎn)變化圖。

圖2 仿射變換-旋轉(zhuǎn)變化圖

從圖2可知，原始坐標系下的一個點是(a,b)，要對其進行旋轉(zhuǎn)操作。在基于原點的情況下，通過旋轉(zhuǎn)坐標軸就能得到旋轉(zhuǎn)之后的點的坐標，并得到新的坐標系。在這個坐標系中通過立體幾何關(guān)系確定點(a,b)在新坐標系中的坐標，在新坐標系中它的X、Y坐標分別為(acosα+bsinα)和(bcosα-asinα)。然后在這個位置的基礎(chǔ)上加上其在X軸和Y軸的偏移量Δx、Δy。又因點(a,b)只是參考軌跡上的一個點，除了旋轉(zhuǎn)和平移，參考軌跡與匹配軌跡之間還存在伸縮變換關(guān)系。由此可以建立如下所示的仿射變換模型

(1)

1.3 地磁匹配

地磁匹配問題可表示為平面上兩條曲線的相關(guān)性計算，采用平方差(Squared Difference，SD)算法計算曲線之間的相關(guān)性，即計算匹配軌跡上各點對應的地磁場特征值與實際測量得到的地磁場特征值之差的平方和。因為該值越小則相差度越小，所以該值達到最小時對應的軌跡序列就是最優(yōu)的匹配位置。那么SD相關(guān)性指標函數(shù)表示如下

(2)

式中，I(ui,wi)表示點(ui,wi)T所在位置對應的地磁數(shù)據(jù)庫中地磁特征值；Ir(ai,bi)表示載體在參考點(ai,bi)T時，磁強計實時測量得到的地磁特征值，N表示參考軌跡采樣點的總個數(shù)。

由于匹配軌跡位于真實軌跡附近，所以匹配結(jié)果點所在的地磁特征值約等于對應的真實軌跡上的點的地磁特征值[16]，即存在式(3)。

I(ui,wi)≈Ir(ai,bi)

(3)

為了實現(xiàn)匹配，就要求參考軌跡在匹配軌跡附近，即可將I(ui,wi)在參考點(ai,bi)T處泰勒展開，稱I(ui,wi)在點(ai,bi)T的一階泰勒公式，如式(4)所示。

(4)

其中，I(ai,bi)表示在點(ai,bi)T時所對應的地磁數(shù)據(jù)庫中地磁特征值；?I(ai,bi)/?x表示地磁特征值對經(jīng)度方向的梯度在點(ai,bi)T上的取值；?I(ai,bi)/?y表示地磁特征值對緯度方向的梯度在點(ai,bi)T上的取值；O2表示的是泰勒展開后所有大于一階的高階小項。

聯(lián)立式(3)和式(4)，并且忽略高階小項O2,就可得如下式

(5)

將上式簡寫成如下式

Ix,i(ui-ai)+Iy,i(wi-bi)+It,i≈0

(6)

其中，Ix,i=?I(ai,bi)/?x,Iy,i=?I(ai,bi)/?y,It,i=?I(ai,bi)-Ir(ai,bi)。

將式(1)代入式(6)可得

(7)

那么平方差相關(guān)性指標函數(shù)如下

(8)

其中，a′i=ai-a1；b′i=bi-b1。

因此地磁匹配問題可以轉(zhuǎn)化為求取仿射模型參數(shù)Δx、Δy、α、k1、k2的值，以便使得式(8)達到極小值?？梢詫⑵鋵Ψ律淠Ｐ蛥?shù)Δx、Δy、α、k1和k2求一階偏導數(shù)，并使其結(jié)果為零，如下所示。

(9)

把式(8)代入式(9)可得如下計算式。

(10)

Δy)+Iy,iIt,i]=0

(11)

(-a′icosα-b′isinα)]×[Ix,i(k1(a′icosα+b′isinα)-

a′i+Δx)+Iy,i(k2×(-a′isinα+b′icosα)-b′i+Δy)+

It,i]=0

(12)

(13)

(14)

以上5個計算式組成一個以仿射參數(shù)Δx、Δy、α、k1和k2為變量的非線性方程組

(15)

因此問題又轉(zhuǎn)化為解非線性方程組(15)，在本文中通過Mathematica對它進行求解，然后將計算所得的5個誤差變量Δx、Δy、α、k1和k2代入仿射變換模型式(1)即可得到匹配結(jié)果。

2 仿真實驗

2.1 地磁數(shù)據(jù)庫的建立

為了建立匹配區(qū)域的地磁數(shù)據(jù)庫，本文改進了第12代國際地磁參考場模型。給定經(jīng)緯度的精度、起始點和終點的經(jīng)緯度、年份、海拔高度、地點，可以輸出起點和終點之間所有點的地磁特征值。地磁特征值包括磁偏角、磁傾角、總磁場強度以及北向、東向、垂直方向的磁場強度。本文選取總磁場強度作為地磁特征值。

2.2 仿射變換算法驗證

通過MATLAB模擬參考軌跡和真實軌跡，首先對地磁數(shù)據(jù)庫進行網(wǎng)格化處理，并計算出數(shù)據(jù)庫里面各個點的梯度。然后設(shè)定參考軌跡的初始點為(110°,30°)，沿經(jīng)度和緯度方向的初始速度分別為120 m·s-1和160 m·s-1，每隔50 s取點并給速度加幅值為1 m·s-1粉噪聲，再經(jīng)過雙線性插值，將會得到軌跡上各點的經(jīng)緯度、梯度及相應的點在地磁數(shù)據(jù)庫中檢索得到的地磁特征值。最后令真實軌跡的初始點和終點分別為(110°,30°)和(111°,31°)，總速度為200 m·s-1，對其正交分解得到分速度，每隔50 s取點，再使用雙線性插值輸出軌跡序列點和這些點在庫中的地磁特征值，并給地磁特征值加幅值為3 nT的白噪聲作為測量值。

將以上相關(guān)數(shù)據(jù)作為輸入，通過Mathematica解非線性方程組(15)，采用了兩種方法求解：迭代求解和數(shù)值求解。表1是關(guān)于使用這兩種方法計算所得的仿射參數(shù)，序號1是迭代法求得的解，序號2～13是數(shù)值求解的結(jié)果。

表1 仿射參數(shù)

將表1的仿射參數(shù)Δx、Δy、α、k1、k2輸入到MATLAB中，輸出軌跡圖和誤差圖。最終實驗結(jié)果表明，迭代法所求解的匹配效果最好，圖3～圖5分別是它的軌跡圖、經(jīng)度誤差圖和緯度誤差圖。由圖得出，慣導輸出參考軌跡的最終定位點與真實軌跡的終點相差6 328.5 m，參考軌跡與真實軌跡之間的經(jīng)緯度誤差分別為-0.034°和0.052°；匹配軌跡的最終的定位點與真實軌跡的終點相差293.3 m，而且匹配軌跡與真實軌跡之間的經(jīng)緯度誤差分別為0.003°、0.000 4°。由此可知，相對于慣導系統(tǒng)，匹配軌跡的定位精度明顯提高。

圖3 軌跡圖

圖4 經(jīng)度誤差圖

圖5 緯度誤差圖

3 結(jié)束語

本文針對慣導系統(tǒng)的定位誤差隨時間累積的問題，提出了一種基于仿射變換的地磁匹配定位算法。該算法將地磁匹配問題表示為平面上兩條曲線的相關(guān)性計算，采用了SD算法計算曲線之間的相關(guān)性，即計算匹配軌跡上各點對應的地磁場特征值與真實測量得到的地磁場特征值之差的平方和。依據(jù)相關(guān)性準則，該值達到最小時，其對應的軌跡序列就是最優(yōu)的匹配位置。實驗表明，匹配軌跡的最終定位點與真實軌跡終點相比誤差較小，為慣導系統(tǒng)輸出的軌跡誤差的4.63%。因此本文提出的基于仿射變換的地磁匹配定位算法可以有效修正慣導系統(tǒng)的初始位置誤差、初始航向誤差和初始速度誤差，實現(xiàn)精準的匹配定位。