基于仿射變換的慣性/地磁匹配迭代定位方法
【專利摘要】本發(fā)明涉及一種基于仿射變換的慣性/地磁匹配迭代定位方法，步驟包括：從慣導(dǎo)系統(tǒng)讀取待匹配位置測量值，從磁強(qiáng)計(jì)獲得地磁場強(qiáng)度信息；根據(jù)慣導(dǎo)系統(tǒng)指示的位置，從地磁數(shù)據(jù)庫中讀取地磁場強(qiáng)度和地磁場強(qiáng)度的梯度；引入并初始化匹配位置偏移量、角度偏移量和伸縮系數(shù)；計(jì)算迭代參數(shù)；計(jì)算匹配位置偏移量的增量、角度偏移量的增量和伸縮系數(shù)的增量；更新匹配位置偏移量、角度偏移量和伸縮系數(shù)；根據(jù)終止迭代條件判斷是否更新迭代變量，進(jìn)行重復(fù)迭代；計(jì)算輸出匹配定位結(jié)果。本發(fā)明能同時(shí)修正慣導(dǎo)系統(tǒng)的初始位置誤差、初始航向誤差和初始速度誤差，提高載體的定位精度；用數(shù)值迭代方法取代遍歷搜索的求解手段，減小了匹配定位時(shí)間，提高了實(shí)時(shí)性。
【專利說明】
基于仿射變換的慣性/地磁匹配迭代定位方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及組合導(dǎo)航定位技術(shù)領(lǐng)域，尤其涉及一種基于仿射變換的慣性/地磁匹 配迭代定位方法。
【背景技術(shù)】
[0002] 地磁場是地球的基本物理場，處在近地空間內(nèi)任意一點(diǎn)都具有磁場強(qiáng)度，因此地 磁場構(gòu)成了一個(gè)天然的導(dǎo)航坐標(biāo)系。以地磁場為基礎(chǔ)的地磁導(dǎo)航具有無源、無輻射、隱蔽性 強(qiáng)、誤差不隨時(shí)間累積等眾多優(yōu)點(diǎn)。然而由于地磁場分布存在平緩區(qū)域、地磁場容易受到外 界干擾磁場影響等原因，單獨(dú)使用地磁場信息導(dǎo)航有可能產(chǎn)生可靠性差等問題。若將地磁 導(dǎo)航技術(shù)和慣導(dǎo)（即慣性導(dǎo)航的簡稱)系統(tǒng)組合，一方面可以根據(jù)地磁導(dǎo)航信息及時(shí)對慣導(dǎo) 系統(tǒng)進(jìn)行修正，消除慣導(dǎo)系統(tǒng)運(yùn)行過程中的累積誤差，另一方面，可以根據(jù)慣導(dǎo)系統(tǒng)的導(dǎo)航 信息將地磁匹配區(qū)域限定在一定范圍內(nèi)，提高地磁導(dǎo)航的可靠性和精度，從而實(shí)現(xiàn)兩種導(dǎo) 航的優(yōu)勢互補(bǔ)。
[0003] 慣性/地磁匹配定位需要根據(jù)慣導(dǎo)系統(tǒng)輸出的參考軌跡形狀，在預(yù)存的地磁圖中 進(jìn)行相關(guān)性匹配，確定參考軌跡在預(yù)存的地磁圖中的匹配位置，從而計(jì)算出運(yùn)動載體的實(shí) 時(shí)位置信息。
[0004] 慣性/地磁匹配方法主要有ICCP匹配方法和輪廓線匹配方法。ICCP匹配方法求取 測量軌跡與最近等值點(diǎn)軌跡之間的最優(yōu)剛性變換，通過該變換校正測量軌跡來實(shí)現(xiàn)匹配的 目的，然而該算法以磁場測量等值線上的最近點(diǎn)作為匹配目標(biāo)，因此算法收斂于磁場測量 等值線上的最近點(diǎn)而不收斂于真實(shí)軌跡，因此ICCP匹配方法的精度無法得到保證。輪廓線 匹配方法要求根據(jù)慣導(dǎo)系統(tǒng)輸出的參考軌跡形狀，遍歷有效范圍內(nèi)所有平行于參考軌跡的 序列，并通過相關(guān)性準(zhǔn)則確定最優(yōu)的匹配結(jié)果。首先，該方法采用遍歷的手段進(jìn)行大范圍搜 索，匹配速度慢，實(shí)時(shí)性差；其次，通常該方法只用來修正慣導(dǎo)系統(tǒng)的位置誤差，匹配精度 差。

【發(fā)明內(nèi)容】

[0005] 為了克服現(xiàn)有技術(shù)下的上述缺陷，本發(fā)明的目的在于提供一種基于仿射變換的慣 性/地磁匹配迭代定位方法，該方法可以同時(shí)修正慣導(dǎo)系統(tǒng)的初始位置誤差、初始航向誤差 和初始速度誤差，提高載體的定位精度，同時(shí)采用迭代算法求解匹配結(jié)果，提高載體定位的 實(shí)時(shí)性。
[0006] 本發(fā)明的技術(shù)方案是： 一種基于仿射變換的慣性/地磁匹配迭代定位方法，包括如下步驟： 步驟1，從慣導(dǎo)系統(tǒng)讀取當(dāng)前時(shí)刻以及前《-1個(gè)時(shí)刻的待匹配點(diǎn)的位置測量值巧和心， 其中巧表示經(jīng)度，灼表示煒度，下標(biāo)if表示不同時(shí)刻，i = 1L 為整數(shù)且;1 >2，?為1表示當(dāng) 前時(shí)刻;通過磁強(qiáng)計(jì)獲得當(dāng)前時(shí)刻以及前好-1個(gè)時(shí)刻的地磁場強(qiáng)度的測量值fv·(即對應(yīng)上 述η個(gè)待匹配點(diǎn)的地磁場強(qiáng)度的測量值 步驟2,根據(jù)慣導(dǎo)系統(tǒng)指示的Π 個(gè)待匹配點(diǎn)的位置，從預(yù)先存儲的地磁數(shù)據(jù)庫中分別讀 取相應(yīng)的地磁場強(qiáng)度的參考值，?、地磁場強(qiáng)度的梯度的參考值Tjt，?和，其中_;__表不 地磁場強(qiáng)度在經(jīng)度方向的梯度在第?點(diǎn)位置上的取值,??表示地磁場強(qiáng)度在煒度方向的梯 度在第?點(diǎn)位置上的取值； 步驟3，引入并初始化匹配位置偏移量、角度偏移量和伸縮系數(shù)Jf::
步驟4,根據(jù)公式(2)、公式(3)和公式(4)計(jì)算迭代參數(shù)和丑，


步驟5，計(jì)算匹配位置偏移量的增量、角度偏移量的增量和伸縮系數(shù)的增量: δΜ = -HyG (5) 步驟6，更新匹配位置偏移量、角度偏移量和伸縮系數(shù)財(cái)： Μ --- M (6) 步驟7，判斷是否滿足終止迭代條件，若滿足則停止迭代并跳到步驟10，否則跳到步驟8; 終止迭代條件為①、②中任意一個(gè)或兩個(gè):①迭代次數(shù)達(dá)到預(yù)設(shè)次數(shù);②匹配位置偏移 量的增量、角度偏移量的增量和伸縮系數(shù)的增量說的2范數(shù)小于設(shè)定值，SP |_| 2<f (7) 其中s為預(yù)先設(shè)定的迭代最小誤差； 步驟8，根據(jù)更新后的Μ計(jì)算參數(shù):iV和茂況： N = g(M) (8) (65 iV == ΛΓ - G (9) 步驟9,根據(jù)公式(10)和公式(11)更新迭代變量好和G，并跳到步驟5，
步驟10,根據(jù)迭代計(jì)算得到的匹配位置偏移量、角度偏移量和伸縮系數(shù)Μ，按公式 (12)計(jì)算輸出匹配結(jié)果，
其中夂^為第塒刻匹配結(jié)果的位置經(jīng)度，Α,?為第塒刻匹配結(jié)果的位置煒度。
[0007] 所述地磁場強(qiáng)度優(yōu)選為地磁場總強(qiáng)度、地磁異常場總強(qiáng)度或者地磁場總強(qiáng)度在地 理坐標(biāo)系下某一方向的分量； 當(dāng)所述地磁場強(qiáng)度為地磁場總強(qiáng)度時(shí)，所述磁強(qiáng)計(jì)采用標(biāo)量磁強(qiáng)計(jì)或者三軸矢量磁強(qiáng) 計(jì)，從所述磁強(qiáng)計(jì)直接獲得所述地磁場總強(qiáng)度，作為所述地磁場強(qiáng)度的測量值？ 當(dāng)所述地磁場強(qiáng)度為地磁異常場總強(qiáng)度時(shí)，所述磁強(qiáng)計(jì)采用標(biāo)量磁強(qiáng)計(jì)或者三軸矢量 磁強(qiáng)計(jì)，從所述磁強(qiáng)計(jì)直接獲得地磁場總強(qiáng)度，并根據(jù)地球磁場模型計(jì)算出地磁異常場總 強(qiáng)度，作為所述地磁場強(qiáng)度的測量值^,, ; 當(dāng)所述地磁場強(qiáng)度為地磁場總強(qiáng)度在地理坐標(biāo)系下某一方向的分量時(shí)，所述磁強(qiáng)計(jì)采 用三軸矢量磁強(qiáng)計(jì)，依據(jù)所述磁強(qiáng)計(jì)的三軸矢量測量值以及載體的姿態(tài)，計(jì)算出地磁場總 強(qiáng)度在地理坐標(biāo)系下該方向的分量，作為所述地磁場強(qiáng)度的測量值
[0008] 上述任意一種所述的基于仿射變換的慣性/地磁匹配迭代定位方法還可以包括二 次迭代步驟，所述二次迭代步驟的內(nèi)容為:第二次重復(fù)執(zhí)行所述步驟1-10，與第一次執(zhí)行所 不同的是，用第一次執(zhí)行所得到的匹配定位結(jié)果和)替代所述步驟1中從慣導(dǎo)系統(tǒng)讀 取的A和:Ji。
[0009] 本發(fā)明的有益效果為： 與輪廓線匹配方法相比，由于本方法引入了匹配位置偏移量、角度偏移量和伸縮系數(shù)， 分別用于反映參考軌跡相對于真實(shí)軌跡的平移變化、旋轉(zhuǎn)變化和伸縮變化，而這三種變化 分別由慣導(dǎo)系統(tǒng)的初始位置誤差、初始航向誤差和初始速度誤差導(dǎo)致，因此，同時(shí)引入匹配 位置偏移量、角度偏移量和伸縮系數(shù)并使它們都參與到匹配定位的最優(yōu)化求解過程中，并 通過一系列特定的算法，使匹配結(jié)果收斂于真實(shí)軌跡，還同時(shí)修正了慣導(dǎo)系統(tǒng)的初始位置 誤差、初始航向誤差和初始速度誤差，相比傳統(tǒng)方法顯著提高了定位精度。
[0010] 本發(fā)明的方法還采用了傳統(tǒng)定位方法所沒有的迭代計(jì)算方式，與傳統(tǒng)的遍歷搜索 方法相比具有明顯優(yōu)勢，該迭代計(jì)算方式與前述同時(shí)引入多個(gè)新的變量以修正慣導(dǎo)系統(tǒng)多 種初始誤差的特點(diǎn)有機(jī)結(jié)合，不可分割，使本發(fā)明的方法具有突出的實(shí)質(zhì)性特點(diǎn)和顯著的 進(jìn)步。例如，如果在傳統(tǒng)的定位方法中引入角度偏移量和伸縮系數(shù)進(jìn)行搜索，由于沒有從根 本上改變遍歷搜索方式，計(jì)算量將會大幅提高，導(dǎo)致無法完成在線計(jì)算，也就無法實(shí)現(xiàn)實(shí)時(shí) 定位。
[0011] 通過二次迭代計(jì)算匹配位置偏移量、角度偏移量和伸縮系數(shù)，由于二次迭代用的 初值是經(jīng)過第一次迭代獲得的已經(jīng)消除了大部分初始位置誤差、大部分初始航向誤差和大 部分初始速度誤差的值，再次通過迭代計(jì)算，其結(jié)果比第一次迭代更接近真實(shí)軌跡，使匹配 定位結(jié)果精度得到進(jìn)一步提高。
[0012] 由于以數(shù)值迭代方法替代了輪廓線匹配方法中的遍歷求解手段，明顯改善了定位 算法的計(jì)算效率，顯著提高了定位過程的實(shí)時(shí)性，即使角度偏移量和伸縮系數(shù)同匹配位置 偏移量一樣都加入搜索，也不會導(dǎo)致計(jì)算量的大幅提高，仍然能夠很好地滿足在線計(jì)算的 要求，為在線計(jì)算即滿足實(shí)時(shí)性要求提供了前提保證，是輪廓線匹配方法所不能比擬的。
【附圖說明】
[0013] 圖1是本發(fā)明的定位方法的流程圖； 圖2是采用本發(fā)明的定位方法的經(jīng)度誤差曲線圖舉例； 圖3是采用本發(fā)明的定位方法的煒度誤差曲線圖舉例。
【具體實(shí)施方式】
[0014] 如圖1所示，本發(fā)明公開了一種基于仿射變換的慣性/地磁匹配迭代定位方法，包 括如下步驟： 步驟1，從慣導(dǎo)系統(tǒng)讀取當(dāng)前時(shí)刻以及前個(gè)時(shí)刻的待匹配點(diǎn)的位置測量值巧和:乃， 其中A表示經(jīng)度，為表示煒度，下標(biāo)壞示不同時(shí)刻，i' = !Li3，》為整數(shù)且??>2，Σ_為1表示當(dāng) 前時(shí)刻;通過磁強(qiáng)計(jì)獲得當(dāng)前時(shí)刻以及前??τ1個(gè)時(shí)刻的地磁場強(qiáng)度的測量值 步驟2,根據(jù)慣導(dǎo)系統(tǒng)指示的《個(gè)待匹配點(diǎn)的位置，從預(yù)先存儲的地磁數(shù)據(jù)庫中分別讀 取相應(yīng)的地磁場強(qiáng)度的參考值Ρ?Ι, 3·、地磁場強(qiáng)度的梯度的參考值：!_和Tj, ?，其中TJt，?表不 地磁場強(qiáng)度在經(jīng)度方向的梯度在第?點(diǎn)位置上的取值，表示地磁場強(qiáng)度在煒度方向的梯 度在第1'點(diǎn)位置上的取值； 步驟3，引入并初始化匹配位置偏移量、角度偏移量和伸縮系數(shù)??::
Τ為轉(zhuǎn)置，Δχ、4^對應(yīng)于匹配位置偏移量，α對應(yīng)于角度偏移量，.?、:?對應(yīng)于伸縮系數(shù)， %和.& 2大于1表示真實(shí)軌跡比參考軌跡長;小于1表示真實(shí)軌跡比參考軌跡短，等于1表示真 實(shí)軌跡和參考軌跡等長。
[0015] 步驟4,根據(jù)公式(2)、公式(3)和公式(4)計(jì)算迭代參數(shù)和H，


步驟5，計(jì)算匹配位置偏移量的增量、角度偏移量的增量和伸縮系數(shù)的增量: δΜ^-Hy.G (5) 步驟6，更新匹配位置偏移量、角度偏移量和伸縮系數(shù)Μ: Μ =Μ + δΜ (6) 步驟7，判斷是否滿足終止迭代條件，若滿足則停止迭代并跳到步驟10，否則跳到步驟8; 終止迭代條件為①、②中任意一個(gè)或兩個(gè):①迭代次數(shù)達(dá)到預(yù)設(shè)次數(shù);②匹配位置偏移 量的增量、角度偏移量的增量和伸縮系數(shù)的增量iiV/的2范數(shù)小于設(shè)定值，ΒΡ 1^12<^ ⑴ 其中e為預(yù)先設(shè)定的迭代最小誤差； 步驟8，根據(jù)更新后的ilf計(jì)算參數(shù)JV和: N = g(M) (8) SN=N~G (9) 步驟9,根據(jù)公式(10)和公式(11)更新迭代變量設(shè)和t?，并跳到步驟5，
步驟10,根據(jù)迭代計(jì)算得到的匹配位置偏移量、角度偏移量和伸縮系數(shù)Μ，按公式 (12)計(jì)算輸出匹配結(jié)果，
其中為第Z時(shí)刻匹配結(jié)果的位置經(jīng)度，為第〗時(shí)刻匹配結(jié)果的位置煒度。
[0016] 所述地磁場強(qiáng)度優(yōu)選為地磁場總強(qiáng)度、地磁異常場總強(qiáng)度或者地磁場總強(qiáng)度在地 理坐標(biāo)系下某一方向的分量； 當(dāng)所述地磁場強(qiáng)度為地磁場總強(qiáng)度時(shí)，所述磁強(qiáng)計(jì)采用標(biāo)量磁強(qiáng)計(jì)或者三軸矢量磁強(qiáng) 計(jì)，從所述磁強(qiáng)計(jì)直接獲得所述地磁場總強(qiáng)度，作為所述地磁場強(qiáng)度的測量值;相應(yīng)地， 預(yù)先存儲的地磁場強(qiáng)度和地磁場強(qiáng)度的梯度應(yīng)為地磁場總強(qiáng)度和地磁場總強(qiáng)度的梯度。
[0017] 當(dāng)所述地磁場強(qiáng)度為地磁異常場總強(qiáng)度時(shí)，所述磁強(qiáng)計(jì)采用標(biāo)量磁強(qiáng)計(jì)或者三軸 矢量磁強(qiáng)計(jì)，從所述磁強(qiáng)計(jì)直接獲得地磁場總強(qiáng)度，并根據(jù)地球磁場模型計(jì)算出地磁異常 場總強(qiáng)度，作為所述地磁場強(qiáng)度的測量值;相應(yīng)地，預(yù)先存儲的地磁場強(qiáng)度和地磁場強(qiáng) 度的梯度應(yīng)為地磁異常場總強(qiáng)度和地磁異常場總強(qiáng)度的梯度。
[0018] 當(dāng)所述地磁場強(qiáng)度為地磁場總強(qiáng)度在地理坐標(biāo)系下某一方向的分量時(shí)，所述磁強(qiáng) 計(jì)采用三軸矢量磁強(qiáng)計(jì)，依據(jù)所述磁強(qiáng)計(jì)的三軸矢量測量值以及載體的姿態(tài)，計(jì)算出地磁 場總強(qiáng)度在地理坐標(biāo)系下該方向的分量，作為所述地磁場強(qiáng)度的測量值相應(yīng)地，預(yù)先 存儲的地磁場強(qiáng)度和地磁場強(qiáng)度的梯度應(yīng)為地磁場總強(qiáng)度在地理坐標(biāo)系下該方向的分量 及該分量的梯度。
[0019] 所述基于仿射變換的慣性/地磁匹配迭代定位方法還可以包括二次迭代步驟，所 述二次迭代步驟的內(nèi)容為:第二次執(zhí)行所述步驟1-10,與第一次執(zhí)行（即一次迭代)所不同 的是，用第一次執(zhí)行所得到的匹配定位結(jié)果和替代所述步驟1中從慣導(dǎo)系統(tǒng)讀取的 A和凡·(如式（13)所示），即將一次迭代得到的修正后的待匹配點(diǎn)位置值作為二次迭代的初 始值。由于二次迭代用的初值是經(jīng)過第一次迭代獲得的已經(jīng)消除了大部分初始位置誤差、 大部分初始航向誤差和大部分初始速度誤差的值，再次通過迭代計(jì)算，其結(jié)果比第一次迭 代更接近真實(shí)軌跡，使匹配定位結(jié)果精度得到進(jìn)一步提高。
本發(fā)明的原理為：引入匹配位置偏移量、角度偏移量和伸縮系數(shù)三個(gè)變量，按照匹配位 置偏移量、角度偏移量和伸縮系數(shù)對慣性導(dǎo)航系統(tǒng)的指示軌跡進(jìn)行平移、旋轉(zhuǎn)和伸縮變換 以構(gòu)建匹配軌跡，將指示軌跡上各點(diǎn)位置值和匹配軌跡上對應(yīng)點(diǎn)的位置值代入由指示軌跡 上各點(diǎn)的地磁場特征量測量值與匹配軌跡上相對應(yīng)點(diǎn)的地磁場特征量參考值共同構(gòu)建的 誤差指標(biāo)函數(shù)（即最優(yōu)化求解的目標(biāo)函數(shù)），其中匹配軌跡上對應(yīng)點(diǎn)的位置值由指示軌跡上 各點(diǎn)位置值與匹配位置偏移量、角度偏移量和伸縮系數(shù)決定，通過迭代計(jì)算得到最優(yōu)的匹 配位置偏移量、角度偏移量和伸縮系數(shù)，即進(jìn)行最優(yōu)化求解，得到使所述誤差指標(biāo)函數(shù)最小 的匹配位置偏移量、角度偏移量和伸縮系數(shù)，就相當(dāng)于得到了最優(yōu)匹配軌跡。
[0021]為了便于數(shù)值計(jì)算，本發(fā)明的所述誤差指標(biāo)函數(shù)是經(jīng)過簡化處理的。例如可以以 指示軌跡上多點(diǎn)序列中每一點(diǎn)的地磁場特征量的測量值與匹配軌跡上對應(yīng)點(diǎn)的地磁場特 征量的參考值之差的平方再各點(diǎn)取和作為所述誤差指標(biāo)函數(shù)。因匹配軌跡相應(yīng)的地磁場特 征量的參考值隨匹配軌跡位置的變化而變化，而匹配軌跡位置又是匹配位置偏移量、角度 偏移量和伸縮系數(shù)的函數(shù)，因此，可通過多項(xiàng)式展開(如泰勒展開)并省略二階以上項(xiàng)、離散 化等類似處理，將目標(biāo)函數(shù)簡化為僅有匹配位置偏移量、角度偏移量和伸縮系數(shù)變量的多 變量表達(dá)式，形成計(jì)算中實(shí)際使用的目標(biāo)函數(shù)，再利用使該目標(biāo)函數(shù)對上述各變量的一階 偏導(dǎo)為零，將地磁匹配問題轉(zhuǎn)化為含上述多個(gè)變量的非線性方程組的求解問題。經(jīng)過上述 變換處理后，迭代計(jì)算過程更為簡單明了。作為本發(fā)明的一種實(shí)施例，經(jīng)過上述變換處理 后，對所述非線性方程組迭代求解可以得到公式(2)-(5)。在利用公式(2)-(5)進(jìn)行本方法 的迭代計(jì)算時(shí)不再需要列目標(biāo)函數(shù)，不需要求取偏微分，只需要進(jìn)行多次的代數(shù)運(yùn)算即可 得到最終匹配結(jié)果。
[0022] 所述迭代計(jì)算以慣導(dǎo)系統(tǒng)指示軌跡上各個(gè)待匹配點(diǎn)對應(yīng)的位置測量值、地磁場特 征量測量值、來自地磁數(shù)據(jù)庫的地磁場特征量參考值為已知參數(shù)，計(jì)算匹配位置偏移量的 增量、角度偏移量的增量和伸縮系數(shù)的增量，用所述增量對應(yīng)更新所述匹配位置偏移量、角 度偏移量和伸縮系數(shù)，將更新后的所述匹配位置偏移量、角度偏移量和伸縮系數(shù)投入到迭 代計(jì)算中的下一循環(huán)，直至滿足當(dāng)次迭代計(jì)算的終止條件，用最后得到的所述匹配位置偏 移量、角度偏移量和伸縮系數(shù)對各個(gè)待匹配點(diǎn)的位置進(jìn)行修正，將該修正后的待匹配點(diǎn)位 置值作為對應(yīng)于當(dāng)次迭代計(jì)算的匹配定位結(jié)果。修正后的待匹配點(diǎn)位置值即為最優(yōu)匹配軌 跡上的點(diǎn)的位置值。所述更新優(yōu)選為線性更新。
[0023] 迭代計(jì)算中所述匹配位置偏移量和角度偏移量的初始值優(yōu)選為0,所述伸縮系數(shù) 的初始值優(yōu)選為1。
[0024] 所述地磁場特征量可以包括地磁場總強(qiáng)度、地磁異常場總強(qiáng)度、水平磁場強(qiáng)度、東 向分量、北向分量、垂直分量、磁偏角及磁傾角中的一種或多種。所述地磁場特征量參考值 至少可以包括地磁場強(qiáng)度參考值和地磁場強(qiáng)度的梯度參考值。所述迭代計(jì)算的終止條件可 以是以下任意一種或兩種條件的組合:條件（1)，迭代次數(shù)達(dá)到預(yù)設(shè)次數(shù);條件(2)，所述匹 配位置偏移量的增量和角度偏移量的增量小于預(yù)先設(shè)定值。
[0025] 依照圖1，以某實(shí)驗(yàn)跑車為例實(shí)施本發(fā)明的一種基于仿射變換的慣性/地磁匹配迭 代方法的過程如下。
[0026] 實(shí)驗(yàn)條件:地磁場選用地磁異常場總強(qiáng)度。選用質(zhì)子磁力儀實(shí)時(shí)測量磁場信息，質(zhì) 子磁力儀的主要性能指標(biāo)如下:分辨率:〇 Olwr，精度:±0.2?Γ。慣導(dǎo)系統(tǒng)的主要性能指標(biāo)如 下：陀螺零偏不穩(wěn)定性:〇.〇〇8&Μ，陀螺隨機(jī)游走加速度計(jì)零偏不穩(wěn)定性:1/J/也 ，加速度計(jì)隨機(jī)游走:2pg / 。
[0027] 將經(jīng)度范圍107.5°到109.5°和煒度范圍39.3°到41.3°區(qū)間的地磁異常場總強(qiáng)度 數(shù)據(jù)存入機(jī)載計(jì)算機(jī)，采用前向差分方法計(jì)算地磁異常場總強(qiáng)度的梯度信息并存入機(jī)載計(jì) 算機(jī);選取待匹配點(diǎn)個(gè)數(shù)為10個(gè)，即10。
[0028] 采用一種基于仿射變換的慣性/地磁匹配迭代定位方法，步驟如下： 步驟1:從慣導(dǎo)系統(tǒng)讀取當(dāng)前時(shí)刻以及前9個(gè)時(shí)刻的待匹配點(diǎn)的位置測量值巧和八·，如 表1所示;根據(jù)質(zhì)子磁力儀的測量值和地球磁場模型，得到當(dāng)前時(shí)刻以及前9個(gè)時(shí)刻的磁場 強(qiáng)度測量信息％6如表2所示。
[0029] 表1慣導(dǎo)系統(tǒng)測量位置坐標(biāo)
表2磁場強(qiáng)度測量信息
步驟2:根據(jù)慣導(dǎo)系統(tǒng)指示的10個(gè)位置，從預(yù)先存儲的地磁數(shù)據(jù)庫中分別讀取相應(yīng)位置 的地磁場強(qiáng)度信息^M_，以及該位置的地磁場強(qiáng)度的梯度信息和％，3?，如表3所示。 [0030]表3地磁數(shù)據(jù)庫中的地磁場強(qiáng)度和梯度信息
步驟3:初始化匹配位置偏移量、角度偏移量和伸縮系數(shù)Μ:
步驟4:根據(jù)公式(2)、公式(3)和公式(4)計(jì)算迭代參數(shù)G、F和//，


步驟5至步驟9:選取?=0.⑶1，預(yù)設(shè)迭代次數(shù)為50次。執(zhí)行公式(5)、公式(6)、公式(8) 至公式（11)，實(shí)施迭代算法。并根據(jù)公式(7)判斷迭代終止條件，可知當(dāng)?shù)螖?shù)為11時(shí)迭 代終止。迭代計(jì)算得到的匹配位置偏移量、角度偏移量和伸縮系數(shù)財(cái)為
步驟10:根據(jù)迭代計(jì)算得到的匹配位置偏移量、角度偏移量和伸縮系數(shù)M，計(jì)算匹配 結(jié)果如表4所示。
[0031] 表4匹配結(jié)果位置坐標(biāo)
上述迭代計(jì)算在計(jì)算機(jī)中耗時(shí)為16.9毫秒。
[0032]為了驗(yàn)證實(shí)驗(yàn)結(jié)果，在實(shí)驗(yàn)跑車上安裝GPS導(dǎo)航定位系統(tǒng)，從而可以得到10個(gè)時(shí)刻 的真實(shí)位置坐標(biāo)，如表5所示。
[0033] 表5跑車真實(shí)位置坐標(biāo)
采用二次迭代來提高匹配定位方法的精度。二次迭代時(shí)，將步驟1中從慣導(dǎo)系統(tǒng)讀取的 待匹配點(diǎn)的位置測量值\和Λ由第一次迭代所得到的匹配定位結(jié)果氣^和)來替代。二 次迭代步驟如下： 步驟1:用第一次的迭代結(jié)果(見表4)替代從慣導(dǎo)系統(tǒng)讀取的10個(gè)時(shí)刻的待匹配點(diǎn)的位 置測量值巧和Λ·，如表6所示;根據(jù)質(zhì)子磁力儀的測量值和地球磁場模型，得到當(dāng)前時(shí)刻以 及前9個(gè)時(shí)刻的磁場強(qiáng)度測量信息，如表2所示。
[0034]表6二次迭代時(shí)的待匹配點(diǎn)位置坐標(biāo)初值
步驟2:根據(jù)慣導(dǎo)系統(tǒng)指示的10個(gè)位置(對應(yīng)表6中的10個(gè)位置），從預(yù)先存儲的地磁數(shù) 據(jù)庫中分別讀取這10個(gè)位置的地磁場強(qiáng)度^^，以及該位置的地磁場強(qiáng)度的梯度和 如表7所示。
[0035]表7地磁數(shù)據(jù)庫中的地磁場強(qiáng)度和梯度信息
步驟3:初始化匹配位置偏移量、角度偏移量和伸縮系數(shù)Μ:
步驟4:根據(jù)公式(2)、公式(3)和公式(4)計(jì)算迭代參數(shù)C?、F和Η，
步驟5至步驟9:選取f=O.OCil，預(yù)設(shè)迭代次數(shù)為50次。執(zhí)行公式（5)、公式(6)、公式(8) 至公式（11)，實(shí)施迭代算法。并根據(jù)公式(7)判斷迭代終止條件，可知當(dāng)?shù)螖?shù)為Π 時(shí)迭 代終止。迭代計(jì)算得到的匹配位置偏移量、角度偏移量和伸縮系數(shù)財(cái)為
步驟10:根據(jù)迭代計(jì)算得到的匹配位置偏移量、角度偏移量和伸縮系數(shù)Μ，計(jì)算匹配 結(jié)果如表8所示。
[0036] 表8匹配結(jié)果位置坐標(biāo)
兩次迭代計(jì)算在計(jì)算機(jī)中總耗時(shí)為19.2毫秒。
[0037]根據(jù)GPS導(dǎo)航定位系統(tǒng)的定位結(jié)果(表5)，可以繪出10個(gè)時(shí)刻的慣導(dǎo)系統(tǒng)測量誤差 曲線、第一次迭代結(jié)果的誤差曲線和第二次迭代結(jié)果的誤差曲線，如圖2和圖3所示，其中圖 2給出了經(jīng)度誤差曲線，圖3給出了煒度誤差曲線。根據(jù)圖2和圖3可以看出，所提出的一種基 于仿射變換的慣性/地磁匹配迭代定位方法具有較高的定位精度，而且重復(fù)迭代后，定位精 度會進(jìn)一步提尚。
【主權(quán)項(xiàng)】
1. 一種基于仿射變換的慣性/地磁匹配迭代定位方法，其特征在于包括如下步驟： 步驟1，從慣導(dǎo)系統(tǒng)讀取當(dāng)前時(shí)刻以及前《-1個(gè)時(shí)刻的待匹配點(diǎn)的位置測量值巧和Χ?， 其中5:表示經(jīng)度，Λ表示煒度，下標(biāo)?表示不同時(shí)刻，z_ = lL ?，為整數(shù)且ra >.2，?_為1表示當(dāng) 前時(shí)刻;通過磁強(qiáng)計(jì)獲得當(dāng)前時(shí)刻以及前故-1個(gè)時(shí)刻的地磁場強(qiáng)度的測量值？V:·; 步驟2,根據(jù)慣導(dǎo)系統(tǒng)指示的《個(gè)待匹配點(diǎn)的位置，從預(yù)先存儲的地磁數(shù)據(jù)庫中分別讀 取相應(yīng)的地磁場強(qiáng)度的參考值匕地磁場強(qiáng)度的梯度的參考值，其中f Μ表示 地磁場強(qiáng)度在經(jīng)度方向的梯度在第?點(diǎn)位置上的取值，表示地磁場強(qiáng)度在煒度方向的梯 度在第?點(diǎn)位置上的取值； 步驟3,引入并初始化匹配位置偏移量、角度偏移量和伸縮系數(shù)Μ: Ay ^:； h 〇 〇: ? Ψ (1) 步驟4,根據(jù)公式(2)、公式(3)和公式(4)計(jì)算迭代參數(shù)G、F和孖，X c |· V ^ C v X ^ t i ? · · ·?· 7 - · 步驟5,計(jì)算匹配位置偏移量的增量、角度偏移量的增量和伸縮系數(shù)的增量 SM = -H 'xG (5) 步驟6，更新匹配位置偏移量、角度偏移量和伸縮系數(shù)Μ: 財(cái):二:虹十(6) 步驟7，判斷是否滿足終止迭代條件，若滿足則停止迭代并跳到步驟10，否則跳到步驟8; 終止迭代條件為①、②中任意一個(gè)或兩個(gè):①迭代次數(shù)達(dá)到預(yù)設(shè)次數(shù);②匹配位置偏移 量的增量、角度偏移量的增量和伸縮系數(shù)的增量:??的2范數(shù)小于設(shè)定值，BP ||c5M|2 <ε (7) 其中￡為預(yù)先設(shè)定的迭代最小誤差； 步驟8，根據(jù)更新后的Μ計(jì)算參數(shù)漢和3iV: N = g(M) (8) SN^N-G (9) 步驟9，根據(jù)公式(10 )和公式(11)更新迭代變量及和G，并跳到步驟5， Η = Η +[(δΜ) - Η{δΝ)](δΜ)τΗ ?[{5Μ)τΗ{δΝ)] (10) G = N (11) 步驟10,根據(jù)迭代計(jì)算得到的匹配位置偏移量、角度偏移量和伸縮系數(shù)Μ，按公式（12) 計(jì)算輸出匹配結(jié)果，其中*扣為第i時(shí)刻匹配結(jié)果的位置經(jīng)度，為第;時(shí)刻匹配結(jié)果的位置煒度。2. 如權(quán)利要求1所述的基于仿射變換的慣性/地磁匹配迭代定位方法，其特征在于所述 地磁場強(qiáng)度為地磁場總強(qiáng)度、地磁異常場總強(qiáng)度或者地磁場總強(qiáng)度在地理坐標(biāo)系下某一方 向的分量； 當(dāng)所述地磁場強(qiáng)度為地磁場總強(qiáng)度時(shí)，所述磁強(qiáng)計(jì)采用標(biāo)量磁強(qiáng)計(jì)或者三軸矢量磁強(qiáng) 計(jì)，從所述磁強(qiáng)計(jì)直接獲得所述地磁場總強(qiáng)度，作為所述地磁場強(qiáng)度的測量值^,:3:; 當(dāng)所述地磁場強(qiáng)度為地磁異常場總強(qiáng)度時(shí)，所述磁強(qiáng)計(jì)采用標(biāo)量磁強(qiáng)計(jì)或者三軸矢量 磁強(qiáng)計(jì)，從所述磁強(qiáng)計(jì)直接獲得地磁場總強(qiáng)度，并根據(jù)地球磁場模型計(jì)算出地磁異常場總 強(qiáng)度，作為所述地磁場強(qiáng)度的測量值_$ 當(dāng)所述地磁場強(qiáng)度為地磁場總強(qiáng)度在地理坐標(biāo)系下某一方向的分量時(shí)，所述磁強(qiáng)計(jì)采 用三軸矢量磁強(qiáng)計(jì)，依據(jù)所述磁強(qiáng)計(jì)的三軸矢量測量值以及載體的姿態(tài)，計(jì)算出地磁場總 強(qiáng)度在地理坐標(biāo)系下該方向的分量，作為所述地磁場強(qiáng)度的測量值Α,?。3. 如權(quán)利要求1或2所述的基于仿射變換的慣性/地磁匹配迭代定位方法，其特征在于 還包括二次迭代步驟，所述二次迭代步驟的內(nèi)容為:第二次重復(fù)執(zhí)行所述步驟1-10，與第一 次執(zhí)行所不同的是，用第一次執(zhí)行所得到的匹配定位結(jié)果^^和#^替代所述步驟1中從慣 導(dǎo)系統(tǒng)讀取的5和Κ。
【文檔編號】G01C21/16GK105865444SQ201610247610
【公開日】2016年8月17日
【申請日】2016年4月20日
【發(fā)明人】解偉男, 奚伯齊, 李清華, 黃黎平, 王常虹
【申請人】哈爾濱工業(yè)大學(xué)