基于動態時間規整的磁偏角修正地磁匹配導航算法?

保金宏，尚俊娜?，施滸立

(1.杭州電子科技大學通信工程學院，杭州310018；2.中國科學院國家天文臺，北京100012)

導航技術作為是一門古老且不斷發展的涉及多學科交叉的工程技術，隨著時代的發展在軍用和民用領域一直起著巨大的作用。隨著多學科的發展與互相交叉，導航系統向著無源性、實時性、隱蔽性等方面發展[1]。目前應用的導航技術有慣性導航、衛星導航、天文導航、地形匹配導航以及地磁匹配導航等[2]。作為無源導航，慣性導航在較短位移時間內具有較高精度，常常被應用在海洋、平原、水下等環境條件下，但是其導航誤差會隨著時間的延續而增大。應用的最為廣泛的是衛星導航，因為它具有覆蓋范圍廣、精度較高、使用方便等優點，但是在特殊環境下有時會無法正常接收到有效信號。天文導航能夠提供位置信息和姿態信息，但是存在傳輸信號不連續、信號易受氣候條件影響等缺陷[3]。地磁與重力匹配導航方法相似，其中重力匹配導航的重力儀造價較貴[4]，影響推廣使用，尤其難以在民用領域應用。

地磁場是地球的基本物理場，地磁場中任意一點的地磁矢量都不盡相同，并且地磁矢量與地理經緯度之間存在一一對應關系。因此，只要確定該點的地磁場矢量，便可通過與己有地磁圖比對，找到該點的地理位置，實現全球定位[5-7]。地磁導航雖然在空間位置上有一一對應的關系，但是在實際導航定位應用時，單獨的地磁導航定位計算量大，在地磁圖地磁強度差異性不突出的地方又容易造成誤匹配等原因，約束著它的推廣應用。

由于組合導航擁有改善精度、提高可靠性，擴大工作地理空間[8-9]等長處。為此，把地磁導航和慣性導航進行組合，通過加速度計輸出的加速度和模塊解算的航向角得出載體的運動軌跡，通過運動軌跡限制地磁匹配的搜索范圍，利用地磁匹配去修正慣性導航產生的累積誤差。其中文獻[10]提到的地磁相關匹配算法需要在一定范圍內平移慣導的指示航跡得到待匹配模板，并通過一定的相關準則找到與真實航跡上磁場值序列最相關的待匹配模板作為匹配結果。但是實際采樣過程中，相同運動軌跡上的采樣時間不一致，導致兩者的序列長度不一致，因此需要將在線采集到的序列與數據庫中地磁序列進行映射，再通過相關匹配算法解算運動軌跡。

本文在無里程信息輔助的地磁導航中，采用動態時間規整的地磁匹配方法，定位精度能達到5 m以內，克服了無里程信息地磁匹配導航容易出現較大誤匹配的問題。在基于里程信息輔助的地磁匹配中，由于低成本的導航定位模塊精度較差，需通過對磁偏角進行數據庫采樣，用于補償慣性器件解算時的航向角，經過補償后的航向角和模塊輸出的加速度得出運動軌跡，利用運動軌跡縮小地磁匹配的搜索范圍，通過地磁匹配的方法修正慣導的累積誤差，使得低成本的導航定位模塊的定位誤差精度可達到1 m以內。相較于高成本、高精度的慣性器件模塊采用慣導加地磁匹配的方法獲得亞米級定位精度，本文的方法使得低成本的導航模塊具有更廣的應用范圍。

1 無里程信息輔助的地磁導航

在實際的地磁匹配過程中，若不嚴格按照對應地磁數字圖中的位置來采集數據，通常不能直接用于地磁信息序列間的匹配。參照動態時間規整方法DTW(Dynamic Time Warping)[11-12]，即如何衡量兩個長度間隔不一的時間序列，實現相似度匹配的一種方法。通過該方法來解決地磁序列匹配過程中，在不同時間序列長度情況下，采集信息的匹配問題。如圖1所示。

圖1 時間序列之間誤匹配示意圖

從圖1可知，A、B兩點位于不同時間序列同一長度上，但不能直接匹配，A點位置的物理相似點應為另一條時間序列的點A′。因此采用DTW的方法對兩條時間序列之間進行匹配。

假設離散數據庫中待匹配的地磁場強度序列為A＝[a1，a2，…，an]，在線測量得到的地磁場強度序列為B＝[b1，b2，…，bm]，他們的長度分別為 n和 m。

若n＝m，則可以直接采用歐式距離的方法進行匹配。

若n≠m，應采用動態規整的方法，先使這兩個序列對齊，構造n×m的網絡矩陣(c，r)，矩陣元素為ac和 br，兩點間的距離為 d(ac，br)，(即尋找序列 A的每一個點ac和B的每一個點br之間的相似度關系，d(ac，br)的距離越小，則認為相似度越高。)一般采用歐式距離，即 d(ac，br)＝(ac-br)2。 該方法尋找一條通過此網格中若干格點的路徑，路徑通過的格點即為兩個序列進行計算對齊的點。

首先定義一條路徑為規整路徑W，規整路徑Wk＝(c，r)為兩序列A與B的映射，有:

但是，在最佳規整路徑的解算過程中，規整路徑W需要滿足以下條件:

①邊界條件:

離線階段與在線階段采集地磁的速度有所不同，但地磁特征排列的先后順序不會改變。通過邊界條件可以限定所選的規整路徑的起點是由矩陣網格的左下角出發，終點在矩陣網格的右上角結束。

②單調性條件:

若 Wo-1＝(r′，c′)，則路徑下一點 Wo＝ (r，c)需滿足:

式中:c和r分別為矩陣網格點的橫坐標和縱坐標，此單調性條件限制了規整路徑W是隨著時間單調前行。

③連續性條件:

若 Wo-1＝(r′，c′)，則規整路徑下一個點 Wo＝(r，c)需滿足:

此條件限制了規整路徑W在匹配時只能與自己相鄰的點對齊。保證序列A與B的每個坐標都在W中出現。

綜合連續性和單調性約束，每一個格點的路徑就只有三個方向。假設路徑已經通過了格點(r，c)，那么下一個通過的格點只可能是下列三種情況之一:(r＋1，c)，(r，c＋1)，(r＋1，c＋1)。

在動態時間規整算法中，滿足上述三個約束限制的路徑有數條，DTW所求的是其中規整代價最小的路徑:

式中:K為補償系數，對不同長度的規整路徑W做補償。DTW算法的目的是求得將兩序列規整后距離度量最小的規整路徑，因此為求得此路徑，定義累加距離:從(1，1)點開始匹配兩序列A與B，規整路徑每經過一點，之前所經過的每點的計算距離都會累加，至終點(n，m)時，累加距離即為所有匹配點的總距離之和，亦即序列A與B的相似度:

式中:L＝(c，r)為點ac與br的歐氏距離到達該點的最小的鄰近元素的累積距離之和。求得最小累積距離之和，即求得了最小規整代價的規整路徑D，從而完成對離線階段與在線階段地磁數據的動態時間規整，進而求得兩序列的相似性，完成地磁特征相關性匹配的解算。

2 基于磁偏角補償和里程信息輔助的地磁導航

2.1 地磁匹配算法

地磁匹配算法實質是一種數字地圖匹配算法。在文獻[13]中介紹了目前三類地磁匹配的方法:其中一類方法是按照相關性準則強調他們之間的相似程度，如互相關算法(COR)、相關系數算法(CC)、積相關算法(PROD)和歸一化積相關算法(NPROD)；另一類方法強調他們之間的差別程度，如平均絕對差算法(MAD)、均方差算法(MSD)、絕對差算法(AD)、平方差算法(SD)；而第三類方法則是基于Hausdorff距離的相似算法(HD)。在計算最佳匹配點時，第一類算法求極大值，另兩類則取極小值。

文獻[13]對基礎地磁匹配方法進行了對比，從匹配精度和匹配運算量來看Hausdorff距離方法總體性能最佳。故我們采用Hausdorff距離方法對小車測量的磁場強度與解算位置對應的地磁強度做分段相似度比較。

Hausdorff距離原理[14-16]如下:

給定歐式空間中的兩列點集為 Q＝{q1，q2，…}，P＝{p1，p2，…}，Hausdorff距離就是用來衡量這兩列點集間的距離。

在本文中Q代表著小車在線移動測量得到的一組地磁場強度序列，P代表著經過磁偏角補償后的航位推算在進行地磁匹配解算之后的位置對應的一組地磁數字圖上的磁場強度。

2.2 基于磁偏角補償和里程信息輔助的地磁匹配導航實現流程

基于磁偏角補償和里程信息輔助的地磁匹配導航實現流程如圖2所示。

根據實現流程系統框圖，對模塊定位軌跡解算過程進行詳細闡述，具體介紹如下:

步驟1 對待匹配區域建立離散地磁場強度和磁偏角數據庫[17]。

圖2 實現流程系統框圖

步驟2 隨機給定一個初始位置，然后通過小車上裝有的JY901自身慣性模塊和地磁模塊，得到一段地磁數據序列(m1，m2，…，mk)，其中 m＝(mx，my，mz)，以及慣性器件模塊所測得的一組加速度序列(a1，a2，…，ak)和方位角序列(θ1，θ2，…，θk)。

航位推算原理:假設在 tn時刻，已知位置ptn(xtn，ytn)和當前時刻運動速度 vtn，及航向角 θtn，還有該位置磁偏角σθtn，這時通過航位推算求tn＋1時刻位置 ptn＋1(xtn＋1，ytn＋1)，計算公式為:

式中:磁偏角[σθt1，σθt2，…，σθtn，…，σθtk]是通過以模塊初始位置的角度為真實角度對后面采樣的角度求差值，保存在磁偏角數據庫中，同時由于離散采樣的磁偏角信息與實際采樣過程的角度信息不是完全一一對應關系，通過動態時間規整方法，將在線測量的角度與磁偏角數據庫中的角度進行映射，即得出實際測量位置所需要的角度補償值。

步驟3 慣性器件在短時間內具有導航精度高的特征，但是隨著時間的積累，器件本身的定位累積誤差會逐步增大，故需要引入地磁匹配方法去修正誤差。通過測量達到的三軸磁場強度去做歐式距離匹配優化，其方法如下:

式中:(mix，miy，miz)代表以位置 ptn(xtn，ytn)為中心，l為半徑范圍內對應地磁數字圖上面的三軸磁場強度，求出在該范圍內與測量的三軸磁場強度最為接近的點，作為修正點更新位置ptn(xtn，ytn)。

步驟4 在上一個修正點location(Xtn-1，Ytn-1)到location(Xtn，Ytn)在數字地圖上的路徑，(注:(Xtn-1，Ytn-1)≠ptn-1(xtn-1，ytn-1))對應得到一組地磁場強序列Q＝{q1，q2，…qi}，在兩個修正點之間采樣得到一組地磁場強序列 P＝{p1，p2，…，pj}，且 i≠j，通過采用動態時間規整的方法，將Q和P兩組場強序列的長度整合成一樣長，假設長度為 t，且 t≥max(i，j)，通過采用Hausdorff距離的方式判斷兩序列的相似性，若Hausdorff(Q，P)≤λ，則判斷兩序列匹配程度較高，修正點位置正確。λ是由地磁傳感器模塊的測量精度所決定[18-19]，將模塊靜置，測量一段時間磁場強度，得出模塊測量誤差范圍，通過hausdorff距離的方法，解算出λ值。若H值大于λ，則對整合后的序列采用二分法進行判斷，若是前半段的兩個序列的匹配值小于或等于λ值，則對兩個修正點的中心位置進行三軸地磁強度匹配；若是前半段的兩個序列的匹配值大于λ值，則對修正點location(Xtn-1，Ytn-1)重新定位，取磁場強度匹配值僅大于最小值所對應的位置進行修正，則再重復步驟2中的航位推算，直到步驟4的過程，直到對小車運動位置解算完畢。

3 仿真實驗

3.1 無里程信息輔助的地磁匹配導航實驗仿真

在無里程信息輔助的地磁導航中，不具備橫向空間識別的能力[20]，因此在應用環境中重點應用于管廊、單車道等一維路徑的場景中，在線采集地磁序列過程中，保持在線采樣路徑與離散地磁數據庫采樣路徑一致。我們的測試環境選用在中科蘇州地理科學與文化信息園區大廈內的地下車庫里，用小型履帶車搭載JY901模塊，通過JY901自帶的地磁傳感器測量地磁場強度，模塊輸出數據頻率為10 Hz。對待導航區域進行數據采樣，從初始位置開始，每間隔1 m進行一個點磁場強度數據進行采樣，每個采樣點上采集50個數據，通過拉依達準則去除粗大誤差，然后做均值處理，得出該采樣點的三軸磁場強度值。在圖3中，“O”代表采樣點的位置，然后對采樣點之間做一維廣義延拓插值處理[21]，得到離散的地磁數據庫。

圖3 在地下車庫進行地磁數據采樣

在圖4中，圖4(a)代表的是在車庫中采集到的沿Y軸的地磁場強度序列離散數據庫的數據。圖4(b)是在實測過程中得到的沿Y軸的地磁場強度序列值。可以看出在線采樣得到的地磁序列與數據庫中對應的地磁場強度序列具有較高的相似性，采用DTW匹配的方法，對三軸矢量磁場強度分別去做匹配。得出圖5的模塊運動軌跡圖，其中初始坐標為(46.5，27.5)。

圖4 數據庫地磁序列與在線采集地磁序列

在圖6中“?”代表地磁匹配定位的位置，箭頭代表運動方向。在定位解算過程中，對于采樣得到的三軸磁場強度序列，若有兩軸及兩軸以上的磁場強度序列匹配到同一個點，則代表匹配成功，匹配成功率為91.03%。圖6中，左圖中的“-?”曲線代表匹配定位解算的誤差值，從圖6中可看出其最大定位誤差為4.9 m。從右圖可知，曲線代表定位誤差范圍，由圖所示地磁匹配定位誤差分布在4.65 m(97.7%)以內。

圖5 基于DTW算法的室內定位軌跡圖

圖6 無里程信息輔助的DTW算法匹配定位誤差

3.2 基于磁偏角補償和里程信息輔助的地磁匹配導航實驗仿真

本節實驗論證采用磁偏角補償的航位推算加地磁匹配的方法使得低成本JY901模塊的定位精度，可以接近高成本、高精度LINS354模塊采用慣導加地磁匹配組合的定位精度。

在實驗過程中采用的低成本JY901模塊，其解算航向角的過程融合了地磁場信息，相較于同類只包含相同陀螺儀、加速度計模塊，其解算航向角的累積誤差較小，由于JY901模塊在解算過程中融入了地磁場因素，因此在解算航向角的過程中易受到室內建筑物結構環境干擾，所以需要引入磁偏角對其航向角所受到的影響進行補償。

電子磁羅盤通過感知周圍地磁場的分布來計算地磁北極的方位，在室內環境中，建筑物的結構、強電場等因素都會對地磁場造成影響，使電子磁羅盤測量角度產生偏差。同時因為建筑物和室內的布局較為恒定，對地磁場的影響也較為恒定。

本文用搭載HMC5843磁傳感器芯片的電子磁羅盤和JY901自帶地磁傳感器模塊對室內環境下的磁偏角和三軸磁場強度進行采樣。測試采用高精度LINS354模塊與搭載地磁傳感器模塊的JY901，輸出頻率均為50 Hz。測試環境選擇中科蘇州地理科學與文化信息產業園區大廈，如圖7所示。

圖7 室內測試場景

制作磁偏角和地磁場強度數據庫，具體步驟如下:

①選擇長為10 m寬為5 m的開放辦公區為待采樣區域，并將測試區域按網格劃分，確定采樣點位置，如圖8所示。其中“?”為采樣點位置。

圖8 走廊辦公區采樣點分布

②將電子磁羅盤和地磁傳感器放在圖8對應的“?”處，電子磁羅盤和地磁傳感器方向指向坐標系相同，每個點采集50組數據。

③對每個點采集的50組數據通過拉依達準則去除粗大誤差，然后做均值處理，得出羅盤在每個點的測量均值與坐標真實方向的平均誤差值，以及該采樣點的三軸磁場強度數據值。

④用廣義延拓逼近法[21]處理采樣值，使離散采樣點形成一個連續的函數，覆蓋整個采樣區域。

首先在實驗過程中，高成本、高精度慣性器件LINS354解算的航向角具有較高精度，且隨時間累積航向角誤差較小，不受建筑物結構的影響。同時利用高精度的LINS354模塊進行數據采集得到定位軌跡不需要進行磁偏角補償，定位軌跡如圖10所示。

在圖10中，坐標(2.1，2.3)為初始位置，箭頭代表LINS354模塊運動方向，“-?”曲線為真實導航定位軌跡，“-o”曲線為未經處理的航位推算定位軌跡，“-o”曲線為經過地磁匹配修正的導航定位軌跡。

圖9 室內環境地磁強度曲面

圖10 LINS354模塊采用不同方法時的定位軌跡

表1 LINS354采用不同方法時的定位精度

從圖10和表1中可知，在運動距離較短時，LINS354的航向角誤差較小，可以取得較好的軌跡相似性，但是由于加速度計的累積誤差導致在行走過程中與真實定位軌跡的偏差較大，但通過地磁匹配修正累積誤差的方法，仍然可以在94.85%的情況下取得亞米級定位精度。

其次在對JY901模塊定位精度實驗分析中，需要通過動態時間規整的方法，將實際測量得到的航向角與地磁數據庫中數據進行映射，然后對測量角度進行補償，如圖11所示。

圖11 JY901模塊測量角度與補償之后的角度曲線

表2 JY901模塊角度處理前后誤差值比較

在圖11中，“-?”曲線為測量角度，“-＋”曲線為模塊采樣點對應磁偏角，“-o”曲線為經過磁偏角補償后的角度，“-(”曲線為定位軌跡真實角度。由圖可知未經過磁偏角補償的測量角度與真實角度最大偏差可達101.3°，其與真實角度偏差的均值可達63.7°，標準差為29°；經過磁偏角誤差補償之后的測量角度與真實角度的最大偏差為23.5°，其與真實測量角度偏差的均值為5.2°，標準差為9.4°，由此可見，經過磁偏角誤差補償的角度可以獲得良好的精度。不同方法的JY901模塊定位軌跡如圖12所示。

圖12 JY901模塊采用不同方法的定位軌跡

在圖12中，坐標(2.1，2.3)為初始位置，箭頭代表JY901模塊運動方向。“-?”曲線為真實定位軌跡，“-x”曲線為未經磁偏角補償的航位推算定位軌跡，由于JY901模塊的航向角的解算過程融合地磁信息，在室內受到建筑物鋼結構影響較大，導致模塊測量的航向角有較大的偏差，其中受建筑物影響最大偏角可達101.3°，運動軌跡嚴重偏離真實軌跡，其直接采集得到的航向角不宜為地磁匹配的直接參數，需要對航向角偏移誤差進行補償，經過角度補償之后，航向角的最大偏差不超過23.5°。圖中“-＋”曲線為磁偏角補償的航位推算定位軌跡，由于單獨依賴磁偏角的角度補償不足以修正JY901模塊的累積誤差，所以還應同時引入地磁匹配方法，“-o”曲線為基于磁偏角補償的地磁匹配導航定位軌跡。其中λ值由JY901自帶的地磁傳感器模塊經2.2節中步驟四所述方法得到，設為3.563。

圖13 JY901模塊采用不同方法定位精度的比較

在圖13中，對JY901模塊基于磁偏角補償的航位推算定位精度與基于磁偏角補償的地磁匹配進行比較，得到:基于磁偏角補償的航位推算定位精度最大誤差為3.225 m，定位誤差處于2 m以內的精度可達84.72%。基于磁偏角補償的地磁匹配定位最大誤差為1.687 m，定位誤差在1 m以內的精度可達93.75%。從表1和表3中可知，低成本的JY901模塊通過磁偏角補償的航位推算結合地磁匹配的方法可以接近于高精度LINS354模塊采用慣導加地磁匹配方法的定位精度。

表3 JY901模塊采用不同方法定位的精度比較

4 結論

本文探索了地磁匹配導航方法，通過動態時間規整實現了一維路徑的無里程信息輔助的地磁匹配導航，其精度可達4.65 m(97.7%)以內。同時利用高成本、高精度模塊LINS354與經過磁偏角補償航向角的JY901模塊進行比較，在經過磁偏角補償之后的低成本JY901模塊的航位推算定位精度可達2 m(84.72%)以內，接近于高精度LINS354模塊的航位推算定位精度。且通過地磁匹配方法修正磁偏角補償后的航位推算的累積誤差，使得JY901模塊可以得到亞米級的定位精度，降低應用成本。