丁繼成，杜翔宇，楊崇昭，趙 巖

（哈爾濱工程大學(xué) 智能科學(xué)與工程學(xué)院，哈爾濱 150001）

重力場信息具有良好的時空分布特征，利用重力場信息與重力場數(shù)據(jù)庫進(jìn)行匹配來獲得導(dǎo)航定位信息具有自主性和隱蔽性，且定位誤差不隨時間而累積，經(jīng)常用于輔助慣性導(dǎo)航系統(tǒng)（Inertial Navigation System,INS），來實(shí)現(xiàn)長時間航行并保持高精度導(dǎo)航定位。使用重力導(dǎo)航系統(tǒng)輔助慣導(dǎo)系統(tǒng)進(jìn)行誤差校正，既能獲得高精度導(dǎo)航定位結(jié)果，同時也不會破壞導(dǎo)航系統(tǒng)的自主性[1,2]。

匹配算法是重力匹配導(dǎo)航系統(tǒng)的核心部分，經(jīng)典的算法有地形輪廓匹配（Terrain Contour Matching,TERCOM）算法、最近等值線迭代（Iterative Closest Contour Point,ICCP）算法、桑地亞慣性地形輔助導(dǎo)航（Sandia Inertia Terrain-aided Navigation,SITAN）算法[3,4]。其中TERCOM 算法對于航向誤差較為敏感，SITAN 算法對初始位置的精度要求較高。而ICCP 算法最初來源于點(diǎn)云配準(zhǔn)的迭代最近點(diǎn)（Iterative Closest Point,ICP）算法，具有精度高，易于實(shí)現(xiàn)等特點(diǎn)。其主要思想是通過迭代最近等值線點(diǎn)來實(shí)現(xiàn)測量與基準(zhǔn)圖之間的匹配。該算法先利用慣性導(dǎo)航系統(tǒng)的指示位置，尋找重力圖中相應(yīng)的搜索域，并搜索其最近等值線點(diǎn)，然后以歐氏距離平方和最小的原則不斷進(jìn)行剛性變換，通過重復(fù)變換逐步靠近真實(shí)航跡，從而對慣導(dǎo)系統(tǒng)的誤差進(jìn)行修正[5]。

ICCP 算法成立是建立在兩個假設(shè)基礎(chǔ)上的：(1)INS 提供的指示航跡接近真實(shí)軌跡，且累積誤差不大；(2) 重力傳感器無測量誤差。假設(shè)(1)可以通過不斷使用ICCP 算法提供充足的外部信息來限制INS 誤差的積累，而假設(shè)(2)在實(shí)際應(yīng)用中無法實(shí)現(xiàn)，不論是重力基準(zhǔn)圖的預(yù)測量還是航行中重力傳感器的實(shí)際測量，測量誤差都不可避免且不可能完全一致，因此會造成最近等值線點(diǎn)不一定存在或者不能確定的情況，導(dǎo)致匹配算法失配或誤配[6]。針對于實(shí)際測量中重力傳感器不可避免存在噪聲的問題，國內(nèi)學(xué)者對此進(jìn)行了深入的研究。從噪聲統(tǒng)計(jì)特性方面考慮，肖晶等提出了一種基于概率數(shù)據(jù)關(guān)聯(lián)的地磁ICCP 算法，對某一位置的地磁場進(jìn)行多次測量，并根據(jù)概率數(shù)據(jù)關(guān)聯(lián)的思想，融合測量值對應(yīng)的結(jié)果，提升了匹配算法的定位精度和魯棒性[7]；陳卓等提出了一種基于可信點(diǎn)集和軌跡的搜索方法來拒絕標(biāo)量匹配的不可靠迭代點(diǎn)，提高了匹配的準(zhǔn)確性[8]；許寧徽等提出一種基于多屬性決策的評價(jià)指標(biāo)，通過熵權(quán)法將收斂度指標(biāo)與軌跡相關(guān)性指標(biāo)融合得到綜合評價(jià)指標(biāo)，在噪聲干擾的情況下也能很好地收斂[9]。從濾波角度考慮，喬楠等提出了一種基于多尺度特征值經(jīng)驗(yàn)?zāi)B(tài)分解的地磁濾波方法，通過濾除干擾信號和重構(gòu)信號來抑制測量噪聲[10]。以上文獻(xiàn)對抑制ICCP 算法測量噪聲影響方面進(jìn)行了不同程度的改進(jìn)，但均未考慮測量誤差對ICCP 算法匹配殘差的影響。

針對重力傳感器存在測量誤差導(dǎo)致傳統(tǒng)ICCP 算法誤配或失配的問題，本文首先對ICCP 算法的匹配殘差進(jìn)行分析，仿真驗(yàn)證匹配殘差在高斯噪聲影響下不服從高斯分布，進(jìn)而利用解決稀疏性問題的非凸lp范數(shù)優(yōu)化模型，研究構(gòu)造增廣拉格朗日函數(shù)，基于lp范數(shù)對ICCP 算法進(jìn)行改進(jìn)，使用了加權(quán)的混合正則項(xiàng)，以更泛化地表示匹配殘差，對最近等值線點(diǎn)進(jìn)行重調(diào)。最后，構(gòu)建聯(lián)合匹配算法，使用混合稀疏ICCP（Hybrid Sparse ICCP,HS-ICCP）算法進(jìn)行粗匹配，提供有效的匹配區(qū)域，將匹配后的位置作為新的INS 指示位置提供給經(jīng)典ICCP 算法進(jìn)行精匹配，有效地減少了重力傳感器測量誤差對匹配結(jié)果的影響。

1 ICCP 算法分析

1.1 ICCP 算法原理

ICCP 算法是ICP 算法以等值線為匹配單元的特例，它不需要對環(huán)境進(jìn)行事先估計(jì)分析，只需要不斷重復(fù)迭代計(jì)算，直至達(dá)到迭代的終止條件，將匹配結(jié)果作為對真實(shí)航跡的估計(jì)，從而修正INS 誤差[11]。

ICCP 算法原理圖如圖1 所示，當(dāng)航行器進(jìn)入匹配區(qū)域后，利用重力傳感器測量并記錄航行軌跡點(diǎn)的重力異常值。根據(jù)INS 系統(tǒng)提供的位置信息，提取對應(yīng)區(qū)域的重力異常數(shù)據(jù)庫，再根據(jù)實(shí)測重力異常值gi(i=1,2...N)，在重力異常數(shù)據(jù)庫中提取INS 指示航跡 點(diǎn)xi(i=1,2...N)附近的重力異常等值線ci(i=1,2...N)，并在等值線ci上搜索到與點(diǎn)xi距離最近的點(diǎn)yi(i=1,2...N)，然后以INS 指示點(diǎn)集和最近等值線點(diǎn)集間的歐式距離最小為目標(biāo)函數(shù)，求解對應(yīng)的旋轉(zhuǎn)矩陣和平移矢量，并對INS 指示航跡點(diǎn)集應(yīng)用求解的剛性變換，不斷重復(fù)迭代這一過程，直至收斂[12]。這實(shí)際上是一種求解最優(yōu)化問題的過程，最優(yōu)化目標(biāo)函數(shù)為：

圖1 重力ICCP 算法原理圖Fig.1 ICCP algorithm implementation

其中，X={xi,i=1,2…N}為INS 指示軌跡點(diǎn)集，Y={yi,i=1,2…N}為最近等值線點(diǎn)集為向量l2范數(shù)的平方，R為旋轉(zhuǎn)矩陣，t為平移矢量。

在求解剛性變換的過程中，考慮到旋轉(zhuǎn)和平移變換的不可交換性，需明確對X先做旋轉(zhuǎn)變換R，再做平移變換t，即TX=RX+t。則剛性變換求解數(shù)學(xué)模型為：

可利用四元數(shù)法對旋轉(zhuǎn)矩陣R和平移矢量t進(jìn)行求解。首先分別以為原點(diǎn)建立新的坐標(biāo)系，對INS 指示軌跡點(diǎn)xi和最近等值線點(diǎn)yi進(jìn)行轉(zhuǎn)換：

在求解旋轉(zhuǎn)矩陣R時，可先令平移矢量t′=02×1，將式(6)平方展開可得：

利用四元數(shù)法即可求解旋轉(zhuǎn)矩陣R和平移矢量t，并對INS 指示航跡點(diǎn)集應(yīng)用求解的剛性變換，進(jìn)行迭代求解。當(dāng)?shù)螖?shù)達(dá)到預(yù)設(shè)上限或旋轉(zhuǎn)矩陣和平移矢量增量的絕對值都小于預(yù)設(shè)閾值時，停止迭代?；谏鲜鲈砜梢钥闯?，ICCP 算法遵循“初始對準(zhǔn)—尋找對應(yīng)關(guān)系（最近點(diǎn)）—求解使得目標(biāo)函數(shù)最小的變換—應(yīng)用變換”的循環(huán)過程[13]，在等值線上尋找全局最優(yōu)解，該全局最優(yōu)解就是最終的匹配結(jié)果。

1.2 ICCP 算法的局限性

由ICCP 算法原理可知，該算法是基于兩個前提假設(shè)：（1）載體真實(shí)位置距離INS 指示位置不遠(yuǎn)；（2）重力傳感器沒有測量誤差。該前提假設(shè)只是理想狀態(tài)，實(shí)際中，前者可以通過外部校正信息進(jìn)行修正，但后者在實(shí)際測量中不可實(shí)現(xiàn)，重力基準(zhǔn)圖的構(gòu)建以及航行過程中的重力傳感器的實(shí)際測量不可避免地存在誤差，所以必須考慮測量誤差對算法的影響。圖2 為在不同噪聲情況下的匹配軌跡，分別是：方案1：無測量噪聲；方案2：加入方差為3 mGal 的高斯白噪聲；方案3：加入方差為5 mGal 的高斯白噪聲。圖3 為三種方案對應(yīng)的緯向、經(jīng)向匹配誤差。

圖2 不同測量噪聲情況下的匹配軌跡Fig.2 Simulation of matching trajectories

圖3 不同測量噪聲情況下的匹配誤差Fig.3 Simulation of matching error

由圖2 和圖3 可以看出，ICCP 算法在無測量誤差條件下對航行誤差進(jìn)行校正，可以得出比較高的匹配精度，且較為穩(wěn)定，但其抗噪聲能力較差；當(dāng)噪聲較大時，容易發(fā)生失配或誤配的情況。故通常先使用其他匹配算法作為粗匹配，再使用ICCP 算法進(jìn)行精匹配。

1.3 ICCP 誤差分析

針對重力基準(zhǔn)圖的構(gòu)建和實(shí)時重力異常的測量產(chǎn)生的誤差對ICCP 算法的匹配精度會產(chǎn)生影響的問題[14]，對ICCP 算法的匹配殘差zi的概率分布進(jìn)行仿真分析。如圖4 所示，重力傳感器無測量誤差時，匹配殘差zi概率分布如圖4(a)所示；給重力異常測量值增加方差為5 mGal 的高斯白噪聲，匹配殘差zi概率分布如圖4(b)所示。

圖4 匹配殘差 zi概率分布圖Fig.4 Matching residual ziprobability distribution

由圖4(a)可以看出，在無重力測量誤差條件下，ICCP 算法匹配殘差數(shù)據(jù)直方圖與高斯概率密度擬合較好，此時匹配殘差zi服從高斯分布；由圖4(b)可以看出，在重力異常測量誤差方差為5 mGal 時，ICCP 算法匹配殘差數(shù)據(jù)直方圖不符合高斯概率密度分布鐘型、對稱、均勻變動的特點(diǎn)，不能較好地?cái)M合，此時匹配殘差zi不服從高斯分布。

此外，由于慣導(dǎo)系統(tǒng)本身存在器件誤差，慣導(dǎo)誤差會隨著航行時間的增加而累積，如果不對其進(jìn)行抑制，ICCP 算法可能會因?yàn)槌跏颊`差較大，而無法找到恰當(dāng)?shù)淖罱戎稻€點(diǎn)，出現(xiàn)誤匹配或匹配失敗的情況。

2 基于 lp范數(shù)的HS-ICCP 算法研究

由于ICCP 算法的實(shí)質(zhì)是一個數(shù)學(xué)優(yōu)化問題，其目標(biāo)是通過最小化INS 指示軌跡點(diǎn)集和最近等值線點(diǎn)集之間的歐氏距離，從而實(shí)現(xiàn)高精度匹配。該算法可以看作是一個基于l2范數(shù)的最小二乘優(yōu)化問題。最小二乘法是凸優(yōu)化的一種特殊形式，但其要求匹配殘差服從高斯分布，但在實(shí)際情況下，重力異常測量值存在較大誤差，導(dǎo)致匹配殘差不服從高斯分布。因此，考慮重力異常測量誤差會導(dǎo)致匹配點(diǎn)集間的匹配殘差不服從高斯分布的問題，本文應(yīng)用非凸優(yōu)化模型來解決這個問題，提出了HS-ICCP 算法，以便更好地處理匹配殘差非高斯分布的情況，提高匹配的準(zhǔn)確性。HS-ICCP 算法方案處理流程設(shè)計(jì)如圖5 所示。

圖5 HS-ICCP 算法流程圖Fig.5 Flow chart of HS-ICCP algorithm

2.1 lp范數(shù)

稀疏性問題[15]在變量選擇、圖像處理與計(jì)算機(jī)視覺等領(lǐng)域中一直受到廣泛關(guān)注，壓縮感知理論為解決稀疏性問題提供了理論支撐，許多稀疏性或壓縮感知問題可以轉(zhuǎn)化為非凸優(yōu)化問題，如式(9)所示。

lp范數(shù)最優(yōu)化問題是一個非凸、非光滑、非連續(xù)的優(yōu)化問題，可以將lp范數(shù)近似逼近于l0范數(shù)：

圖6 不同p 取值的范數(shù)圖Fig.6 Simulation of different norms

傳統(tǒng)ICCP 算法采用l2范數(shù)來最小化INS 指示點(diǎn)集和等值線點(diǎn)集之間的歐氏距離，如圖6 所示，在l2范數(shù)的影響下，較大的噪聲會由于距離平方的影響而對總誤差貢獻(xiàn)較大，導(dǎo)致匹配過程可能會被這些異常值所左右；lp范數(shù)函數(shù)圖像在原點(diǎn)附近急劇變尖，意味著在優(yōu)化過程中，較小的偏差對總范數(shù)的貢獻(xiàn)減少，而較大的偏差導(dǎo)致貢獻(xiàn)加大，有利于剔除掉噪聲點(diǎn)、野值點(diǎn)，實(shí)現(xiàn)抗差目的。

2.2 混合稀疏ICCP 函數(shù)

理論上重力ICCP 匹配算法是建立在重力傳感器無測量誤差的基礎(chǔ)上，以歐式距離作為最優(yōu)目標(biāo)函數(shù)進(jìn)行求解。但實(shí)際測量中，重力基準(zhǔn)圖的構(gòu)建和重力傳感器不可避免地存在誤差，且匹配殘差不服從高斯分布，故使用增廣拉格朗日函數(shù)建立非凸優(yōu)化模型，用稀疏誘導(dǎo)lp范數(shù)代替l2范數(shù)[17]，在保證lp范數(shù)的物理意義，同時不影響運(yùn)算結(jié)果的情況下為簡化運(yùn)算，對lp范數(shù)進(jìn)行放大，將lp范數(shù)內(nèi)部的p次冪運(yùn)算放大為歐氏距離的2 次冪運(yùn)算，即令定義稀疏ICCP 如下：

其中，xi為INS 指示軌跡點(diǎn)，yi為最近等值線點(diǎn)，R為旋轉(zhuǎn)矩陣，t為平移矢量。求解式(12)可將其分解為查找等值線點(diǎn)和求解剛性變換兩個步驟：

對于此公式仍可以使用經(jīng)典ICCP 算法的最近等值線點(diǎn)搜索方法進(jìn)行求解。

對于式(14)的求解，則采用增廣拉格朗日算法。引入中間變量Z={zi?R2,i=1…n}，其中，匹配殘差zi=Rxi+t-yi，并將其轉(zhuǎn)換成如下形式：

引入增廣拉格朗日函數(shù)，將式(16)轉(zhuǎn)化為無約束優(yōu)化模型：

式(13)和式(14)定義了一個優(yōu)化問題，目的是將匹配殘差zi分為非異常值和一小部分異常值其中非異常值來自于正確的對應(yīng)點(diǎn)，異常值來自于錯誤或者噪聲造成的對應(yīng)點(diǎn)，故可以通過分類目標(biāo)找到一個稀疏向量z來實(shí)現(xiàn)分類目標(biāo)的目的。

由于式(16)受到單一的p階次正則項(xiàng)的限制，泛化性較差。所以在式(16)的基礎(chǔ)上，使用加權(quán)的混合正則項(xiàng)，可以更泛化地表示匹配殘差zi，定義混合稀疏ICCP（HS-ICCP）函數(shù)如下：

其中,θi? [0,1]為混合正則項(xiàng)的平衡權(quán)重。

通過增廣拉格朗日方法，將式(18)轉(zhuǎn)化為如下形式：

式(19)中含有四個未知變量，可通過交替方向乘子法（Alternating Direction Method of Multipliers,ADMM）將問題分解為三個步驟進(jìn)行求解：

Step1：求解匹配殘差z。

Step2：利用Step1 得到的匹配殘差z，求解旋轉(zhuǎn)矩陣和平移矢量。

Step3：更新懲罰系數(shù)μ與拉格朗日乘數(shù)λi。

該步驟是對懲罰系數(shù)μ進(jìn)行更新。其中，t指的是第t次迭代，目的是為了增大懲罰系數(shù)μ，使Step2 中的(zi+yi-λi/μ)變化盡可能小，防止懲罰系數(shù)過小導(dǎo)致最佳位置錯過，故取μ=0.5，a=1.1。

2.3 平衡權(quán)重的確定

為了高效地求解式(20)，先驗(yàn)地認(rèn)為INS 指示點(diǎn)集經(jīng)匹配算法計(jì)算后，會收斂于正確的匹配位置。對于歐氏距離較大的最近等值線點(diǎn)，增加平衡權(quán)重θi會增加其被分類為噪聲點(diǎn)的概率，從而更好地利用p階正則項(xiàng)的稀疏懲罰能力。而對于歐氏距離較小的最近等值線點(diǎn)，降低平衡權(quán)重θi會增加其被分類為正確點(diǎn)的可能性，可以更好地利用l2范數(shù)正則項(xiàng)的最小二乘逼近能力。

Sigmoid 是二元分類過程中經(jīng)常使用的函數(shù)，使用Sigmoid 函數(shù)[18]構(gòu)建平衡權(quán)重θi和每一個INS 采樣點(diǎn)與對應(yīng)最近等值線點(diǎn)間的歐式距離di的關(guān)聯(lián)函數(shù)：

其中，Median 為INS 采樣點(diǎn)集與其對應(yīng)最近等值線點(diǎn)集對之間歐氏距離的中位數(shù)。

為了弱化離群點(diǎn)，充分進(jìn)行懲罰，HS-ICCP 的稀疏性應(yīng)隨著迭代次數(shù)的增加而增強(qiáng)，即在匹配過程中，Median 只在外循環(huán)搜索最近等值線點(diǎn)結(jié)束后計(jì)算作為初始值，并在每次外循環(huán)迭代過程中逐漸減小為上次迭代的ν倍。隨著Median 的減小，平衡權(quán)重θi將逐漸趨近于1。

圖7 為不同Median 對應(yīng)的Sigmoid 函數(shù)圖像，由圖可直觀看出Median 不斷減小為上次迭代的ν倍的變化趨勢，但變化幅度不應(yīng)過大，若變化幅度過大則會出現(xiàn)過渡較快的情況，就失去了平衡權(quán)重的意義，故本文將v 取0.9，可以達(dá)到均勻過渡的效果，能更好地對等值線點(diǎn)進(jìn)行分類。

圖7 不同Median 對應(yīng)的Sigmoid 函數(shù)Fig.7 Sigmoid function with different median

2.4 標(biāo)量化誤差函數(shù)

由于式(20)中的匹配殘差zi為向量形式，不易進(jìn)行求解，故我們使用MM[19（]Majorization-Minimization）算法處理該優(yōu)化問題。根據(jù)式(20)建立其誤差方程：

這里z=(zx,zy)，h=(hx,hy)均為矢量，將z=(zx,zy)用h=(hx,hy)線性表示為z=ah。可以將求解矢量函數(shù)(20)中z的最優(yōu)解簡化為求解標(biāo)量函數(shù)(26)中a的最優(yōu)解：

對標(biāo)量函數(shù)求標(biāo)量參數(shù)a的偏導(dǎo)數(shù)可得：

對g(ah) 求二階偏導(dǎo)數(shù)，可得：

由于g′(ah) ＞ 0，可知g(ah) 為凸函數(shù)，存在全局最小極值。只有當(dāng)大于g(ah) 的極小值時，ah才有解，否則令a=0。

在此基礎(chǔ)上，HS-ICCP 算法可以通過ADMM 雙循環(huán)的優(yōu)化架構(gòu)，使用外循環(huán)尋找最近等值線點(diǎn)，計(jì)算其對應(yīng)的歐氏距離，并動態(tài)調(diào)整平衡權(quán)重，利用內(nèi)循環(huán)不斷求解其剛性變換矩陣T=(R,t)。最終弱化離群點(diǎn)以及噪聲點(diǎn)的影響，并實(shí)現(xiàn)匹配。

3 重力聯(lián)合抗差匹配算法流程

通過上述算法分析可知，HS-ICCP 算法使用lp范數(shù)作為距離度量函數(shù)，通過ADMM雙循環(huán)優(yōu)化架構(gòu)，對離群點(diǎn)、噪聲點(diǎn)進(jìn)行懲罰，起到了削弱重力傳感器測量誤差影響的作用，但匹配精度會有所下降。而在理想條件下，經(jīng)典ICCP 算法具有穩(wěn)定性好、精度高的特點(diǎn)。基于以上原因，本文提出一種重力聯(lián)合抗差匹配算法，考慮重力傳感器測量誤差，先通過HS-ICCP算法粗略匹配，提供有效的匹配區(qū)域，將匹配后的大致位置作為新的INS 指示航跡輸入經(jīng)典ICCP 算法中進(jìn)行精確匹配。

重力聯(lián)合抗差匹配的具體流程如圖8 所示。

圖8 重力聯(lián)合抗差匹配流程圖Fig.8 Flow chart of gravity combined robust matching

綜上，重力聯(lián)合抗差匹配導(dǎo)航算法的具體步驟設(shè)計(jì)如下：

（1）將INS 指示軌跡的累計(jì)誤差作為閾值，選取重力匹配區(qū)域，建立重力異?；鶞?zhǔn)圖，以保證該區(qū)域能涵蓋匹配航跡；

（2）基于INS 指示航跡，使用HS-ICCP 算法進(jìn)行粗匹配，對含有噪聲的重力異常數(shù)據(jù)進(jìn)行處理；

（3）將粗匹配的結(jié)果作為精匹配的初始INS 指示航跡，使用經(jīng)典ICCP 算法進(jìn)行精匹配；

（4）將精匹配的結(jié)果作為新的初始INS 指示航跡重復(fù)進(jìn)行精匹配，直至達(dá)到收斂條件；

（5）將收斂后的匹配結(jié)果輸出，作為校正航跡。

4 實(shí)驗(yàn)驗(yàn)證與分析

4.1 仿真實(shí)驗(yàn)設(shè)計(jì)

為了驗(yàn)證匹配算法的有效性，選取黃海內(nèi)一片區(qū)域進(jìn)行測試，通過仿真得到載體的航行路線。表1 給出了仿真線路的起始點(diǎn)坐標(biāo)、初始誤差、慣導(dǎo)系統(tǒng)陀螺儀、加速度計(jì)誤差、航向角、航行速度等參數(shù)，重力異常基準(zhǔn)圖選用ICGEM 網(wǎng)站提供的SCG-UGM-2模型[21]，分辨率為1′ × 1′，階數(shù)為2190 階，精度為1～5mGal。

表1 仿真航跡及其參數(shù)Tab.1 Simulated path and their parameters

4.2 仿真實(shí)驗(yàn)

采用表1 中的參數(shù)，參考文獻(xiàn)[14][16-18]并經(jīng)測試驗(yàn)證p取值0.4～0.6 效果較好，權(quán)衡算法性能和魯棒性，本文取p=0.5，ν=0.9，INS 指示航跡采樣間隔為10 min，匹配序列長度為10，使用ICCP 算法和聯(lián)合匹配算法對試驗(yàn)線路進(jìn)行測試。其中線路所在區(qū)域重力基準(zhǔn)圖的緯度范圍為31.5°N～32.5°N，經(jīng)度范圍為121.3°E～122.3°E，重力異常變化范圍為-23.37 mGal～14.71 mGal，基準(zhǔn)圖分辨率為1′ × 1′。

實(shí)驗(yàn)一：在INS 運(yùn)行正常，重力傳感器實(shí)時測量無誤差的情況下，兩種算法得到的匹配軌跡如圖9 所示。其匹配結(jié)果以及慣導(dǎo)航跡的緯向、經(jīng)向誤差如圖10所示。

圖9 實(shí)驗(yàn)一匹配軌跡Fig.9 Matching trajectory of experiment one

圖10 實(shí)驗(yàn)一匹配誤差Fig.10 Matching error of experiment one

表2 為重力傳感器無測量誤差時，INS 以及兩種算法的誤差參數(shù)對比，其中位置誤差表示匹配位置與真實(shí)位置之間的直線距離誤差。從表2 中的數(shù)據(jù)以及圖9 中等值線分布可以看出，在前10 個采樣點(diǎn)進(jìn)行匹配時，重力異常變化范圍在 -10～0 mGal，且變化較為緩慢，致使兩種算法匹配精度相當(dāng)；在后10 個采樣點(diǎn)進(jìn)行匹配時，重力異常變化范圍在 -20～5 mGal，且相鄰點(diǎn)間變化明顯，更有利于重調(diào)野值點(diǎn)進(jìn)行匹配，故聯(lián)合匹配算法匹配精度略好于ICCP 算法。從各項(xiàng)數(shù)值分析來看，聯(lián)合抗差匹配算法在無重力測量誤差的情況下可以正常使用，且導(dǎo)航精度要高于ICCP 算法。

表2 實(shí)驗(yàn)一匹配誤差參數(shù)對比（單位：n mile）Tab.2 Comparison of matching error parameters in experiment one (Unit: n mile)

為了更好地與重力傳感器無測量誤差時的仿真結(jié)果進(jìn)行對比，給重力傳感器持續(xù)加入不同均值的噪聲進(jìn)行仿真實(shí)驗(yàn)。

實(shí)驗(yàn)二：噪聲方差為3 mGal 時，兩種算法得到的匹配軌跡如圖11 所示。其匹配結(jié)果以及慣導(dǎo)航跡的緯向、經(jīng)向誤差如圖12 所示。

圖11 實(shí)驗(yàn)二匹配軌跡Fig.11 Matching trajectory of experiment two

圖12 實(shí)驗(yàn)二匹配誤差Fig.12 Matching error of experiment two

表3 為重力傳感器噪聲方差為3 mGal，INS 以及兩種算法的誤差參數(shù)對比。結(jié)合圖表信息分析，聯(lián)合匹配算法相較ICCP 算法更為穩(wěn)定，緯向誤差、經(jīng)向誤差和位置誤差最大值均控制在1 n mile 以內(nèi)，誤差的平均值最大為0.562 n mile。采用聯(lián)合匹配算法計(jì)算得到的匹配導(dǎo)航結(jié)果精度提升較ICCP 算法提升60%以上。

表3 實(shí)驗(yàn)二匹配誤差參數(shù)對比（單位：n mile）Tab.3 Comparison of matching error parameters in experiment two (Unit: n mile)

實(shí)驗(yàn)三：噪聲方差為5 mGal 時，兩種算法得到的匹配軌跡如圖13 所示。其匹配結(jié)果以及慣導(dǎo)航跡的緯向、經(jīng)向誤差如圖14 所示。

圖13 實(shí)驗(yàn)三匹配軌跡Fig.13 Matching trajectory of experiment three

圖14 實(shí)驗(yàn)三匹配誤差Fig.14 Matching error of experiment three

表4 為重力傳感器噪聲方差為5 mGal 時，INS 以及兩種算法的誤差參數(shù)對比。此時重力傳感器的測量誤差較大，從軌跡圖上可以看出，ICCP 算法已經(jīng)產(chǎn)生較大幅度的偏離，而聯(lián)合匹配算法仍可以較好地定位到真實(shí)航跡，匹配誤差平均值和RMS 都控制在1 n mile 以內(nèi)，且誤差的平均值最大為0.571 n mile。采用聯(lián)合匹配算法計(jì)算得到的匹配導(dǎo)航結(jié)果精度較ICCP 算法提升75%以上。

對比不同測量誤差下，各算法的位置RMS 值，如圖15 所示：

圖15 各算法位置RMS 直方圖Fig.15 Radial RMS histogram of each algorithm

從圖15 可以看出，隨著測量噪聲的不斷增加，ICCP 算法的位置RMS 值也隨之增加，而聯(lián)合匹配算法的位置RMS 值維持在穩(wěn)定范圍內(nèi)，且波動不大。由此可驗(yàn)證本文提出的重力聯(lián)合抗差匹配算法能夠有效抑制重力傳感器測量誤差對匹配結(jié)果的影響，降低匹配誤差。

5 結(jié)論

針對重力測量誤差導(dǎo)致匹配殘差不服從高斯分布，使得經(jīng)典ICCP 算法匹配精度下降甚至失效的問題，提出了一種基于混合稀疏ICCP 的重力聯(lián)合抗差匹配導(dǎo)航方法。利用lp范數(shù)代替l2范數(shù)計(jì)算匹配殘差重調(diào)野值點(diǎn)，組成以基于lp范數(shù)的混合稀疏ICCP 算法為粗匹配、ICCP 算法為精匹配的重力聯(lián)合抗差匹配算法。實(shí)驗(yàn)結(jié)果表明，在無重力異常測量誤差的情況下，重力聯(lián)合抗差匹配算法的匹配精度略優(yōu)于傳統(tǒng)ICCP；在存在重力異常測量誤差的情況下，重力聯(lián)合抗差匹配算法對匹配精度的改善效果明顯，其誤差最大值穩(wěn)定在1 n mile 以內(nèi)，誤差均值小于0.572 n mile，誤差RMS 小于0.628 n mile，導(dǎo)航精度較傳統(tǒng)ICCP 算法提升60%以上，可以達(dá)到抗重力異常測量誤差的效果，提高了匹配算法的魯棒性。