一種適用于地磁梯度匹配導(dǎo)航的ISCCP算法

金子翔，許蘇鵬，張貴賓，梁建，董根旺，范振宇

(1. 中國地質(zhì)大學(xué)(北京) 地球物理與信息技術(shù)學(xué)院，北京 100083；2.中國冶金地質(zhì)總局 地球物理勘查院，河北 保定 071051)

0 引言

自主導(dǎo)航是實(shí)現(xiàn)國防戰(zhàn)略的關(guān)鍵技術(shù)，地磁匹配導(dǎo)航作為慣性導(dǎo)航系統(tǒng)(inertia navigation system,INS)的一種重要輔助手段，利用其不依靠外界信號源，不向外界輻射能量，不受地點(diǎn)、氣候等外界條件限制，隱蔽性好等特點(diǎn)，能完成航行器在水下長航距、長航時(shí)導(dǎo)航，實(shí)現(xiàn)真正的無源導(dǎo)航[1-3]。

由于地磁匹配算法的導(dǎo)航精度與匹配區(qū)域的地磁變化程度密切相關(guān)，所以在一些地磁變化平緩的區(qū)域，輪廓匹配(contour matching，CM)和迭代最近等值線點(diǎn)(iterated closest contour point，ICCP)算法等一維算法的匹配精度較低，甚至?xí)霈F(xiàn)誤匹配的情況[4-5]，為此，研究人員提出不少解決方案，大致可分為兩類。

第一類解決方案為搜索合適的匹配區(qū)域或匹配路徑，Sun等[6]采用免疫粒子群優(yōu)化算法在地磁參考圖中智能地選擇最佳匹配區(qū)域，且在該區(qū)域內(nèi)使用均方偏差和歸一化乘積算法的相關(guān)匹配算法能得到更高的匹配精度。Xu等[7]通過采用啟發(fā)式和隨機(jī)搜索算法的地磁場導(dǎo)航路徑規(guī)劃方法，可以有效地避免非匹配導(dǎo)航區(qū)域，并在水下地磁匹配導(dǎo)航應(yīng)用中實(shí)現(xiàn)有效的路徑規(guī)劃功能。雖然這類解決方案能夠有效地提高匹配精度，但是降低了航行器的機(jī)動性。

隨著三軸磁傳感器的發(fā)展和地磁測量技術(shù)的進(jìn)步[8-9]，出現(xiàn)了第二類解決方案，即利用多維地磁信息進(jìn)行地磁匹配導(dǎo)航。Song等[10]提出了一種利用地磁矢量的地磁匹配導(dǎo)航方法，該方法在匹配區(qū)域地磁熵信息較小或地磁等值線變化不明顯的情況下，仍能大大提高匹配概率和定位精度,且在實(shí)際地磁參考圖的仿真中驗(yàn)證了該算法的有效性。Chen等[11]將可信點(diǎn)集和航跡搜索原理與地磁矢量相關(guān)約束匹配原理相結(jié)合，提出了一種基于矢量匹配算法的地磁匹配導(dǎo)航新方法。通過仿真和實(shí)驗(yàn)分析了匹配和校正方法的性能。王瓊等[12]利用地磁場的矢量性提出一種基于遺傳算法的地磁匹配搜索方法，仿真結(jié)果證明該方法在地磁場平緩區(qū)域定位的有效性,同時(shí)具有較好的定位精度。利用地磁矢量導(dǎo)航雖然能夠有效提供匹配精度，但同時(shí)也容易受到地磁日變的影響。

本文提出一種適用于地磁梯度匹配導(dǎo)航的迭代搜索最近等值線點(diǎn)(iterated search closest contour point，ISCCP)算法，一方面，將地磁3個(gè)正交方向的梯度數(shù)據(jù)應(yīng)用于匹配導(dǎo)航，能有效降低地磁日變給匹配精度帶來的影響，且地磁梯度信息相較于地磁總場可利用的特征更加豐富[13]。另一方面，針對ICCP算法在地磁變化緩慢的區(qū)域匹配精度差的問題，該算法在ICCP誤差分析基礎(chǔ)上，改進(jìn)了ICCP的搜索方式，即利用迭代法多次迭代搜索最近等值線點(diǎn)。經(jīng)理論分析與仿真實(shí)驗(yàn)，ISCCP算法能夠有效提高ICCP算法在地磁變化平緩區(qū)域的匹配精度，且具有較好的實(shí)時(shí)性。

1 ICCP誤差模型

ICCP算法過程如圖1所示，主要為搜索最近等值線點(diǎn)(search closest contour point，SCCP)與迭代最近點(diǎn)(iterative closest point，ICP)兩個(gè)過程。首先在SCCP過程中，利用航行器在導(dǎo)航過程中測量的地磁值在地磁數(shù)據(jù)庫中提取等值線，并在等值線上搜索與慣導(dǎo)指示航跡點(diǎn)最近的點(diǎn)[14-15]，該點(diǎn)即為最近等值線點(diǎn)(closest contour point，CCP)。其次進(jìn)行ICP過程，該過程作用是將慣導(dǎo)的指示航跡點(diǎn)通過剛性變換中的旋轉(zhuǎn)與平移變換盡可能地逼近CCP。最后，判斷本次匹配結(jié)果與上次匹配結(jié)果是否在一個(gè)閾值范圍內(nèi)，若是則判斷為收斂，否則將本次匹配的結(jié)果代入SCCP過程中重新進(jìn)行匹配[16-17]。

圖1 ICCP匹配流程Fig.1 ICCP matching process

由此過程可知，SCCP作為ICCP算法的第1個(gè)步驟，決定整個(gè)算法的精度。為研究SCCP對ICCP算法精度的影響，如圖2，取地磁變化緩慢區(qū)域中一段匹配路徑進(jìn)行分析。

圖2 ICCP誤差分析Fig.2 ICCP error analysis

慣導(dǎo)指示航跡為Mi(i=1,2,3)，航行器實(shí)際航跡為Ni(i=1,2,3)。慣性指示航跡的點(diǎn)位誤差e為

(1)

進(jìn)行SCCP過程時(shí)，根據(jù)實(shí)際航跡點(diǎn)Ni所測量的地磁值在地磁數(shù)據(jù)庫找到地磁等值線，并在這些等值線上尋找一組點(diǎn)集Ti(i=1,2,3)，使得慣導(dǎo)指示航跡Mi到該點(diǎn)集的距離最近，即滿足下列條件

(2)

則圖2中Ti即為所求的CCP。然后利用ICP將慣導(dǎo)指示航跡Mi變換到Ti上，并且重復(fù)SCCP與ICP直至收斂。由于每次SCCP過程中CCP都為Ti，所以最終的匹配結(jié)果即為Ti，陷入局部最優(yōu)解，且匹配的誤差em為

(3)

(4)

將式(1)代入得

em=sinθ×e。

(5)

由該式結(jié)合上述推導(dǎo)可知：

綜上所述，在地磁變化緩慢的區(qū)域，ICCP算法只能有效減少沿等值線法線方向的慣導(dǎo)誤差，但是無法減少沿等值線切線方向的誤差。需要注意的是，以上分析建立在理想模型上，由于實(shí)際匹配區(qū)域中所有的地磁等值線的切線方向并不完全一致，導(dǎo)致搜索到的CCP與上述結(jié)果具有偏差，此時(shí)利用ICP算法中的歐式距離最小原則，將帶誤差的慣導(dǎo)指示點(diǎn)通過變換盡可能地逼近CCP。

2 ISCCP算法原理

根據(jù)前面的分析，ICCP算法的匹配精度與地磁等值線的分布密切相關(guān)，且誤差主要來源于SCCP的過程，所以為搜索到最佳的CCP以提高匹配精度，本文提出ISCCP算法，該算法對ICCP中的SCCP過程進(jìn)行改進(jìn)，如圖3所示。

圖3 ISCCP匹配流程Fig.3 ISCCP matching process

該過程利用地磁3個(gè)正交方向的梯度等值線信息，并采用迭代搜索的方式，進(jìn)行多次SCCP過程迭代實(shí)現(xiàn)CCP逼近實(shí)際航跡點(diǎn)，具體原理如圖4。

由于地磁梯度有3個(gè)正交方向的等值線信息，因此我們在圖2的基礎(chǔ)上增加了等值線Ⅱ，如圖4所示。由上節(jié)推導(dǎo)過程可知第一次SCCP為Ti，與實(shí)際航跡點(diǎn)Ni的誤差為

圖4 ISCCP誤差分析Fig.4 ISCCP error analysis

(6)

第二次SCCP以上次搜索結(jié)果Ti作為本次搜索的起點(diǎn)進(jìn)行迭代，經(jīng)過在等值線Ⅱ中SCCP的過程，得CCP為Yi，則第二次搜索的結(jié)果誤差e2為

(7)

設(shè)等值線Ⅱ與等值線Ⅰ的夾角為α，得到下式

(8)

將式(6)代入，得

e2=e1×cosα，

(9)

以上論證表明，當(dāng)匹配區(qū)域中有相互垂直的等值線時(shí)，利用這些等值線迭代SCCP過程中，能夠快速找到實(shí)際航跡點(diǎn)的位置，并通過ICP算法將慣導(dǎo)指示航跡點(diǎn)逼近實(shí)際航跡點(diǎn)。

由于實(shí)際匹配區(qū)域中，相互垂直的地磁梯度等值線信息較少，為確保地磁匹配導(dǎo)航的精度，利用迭代法對所有的地磁梯度等值線信息進(jìn)行SCCP也能有效減少慣導(dǎo)指示航跡點(diǎn)的誤差。證明如下：

將式(1)、式(6)代入式(7)，得

e2=e×sinθ1×cosα，

(10)

α=90°-θ2，

(11)

即

e2=e×sinθ1×sinθ2。

(12)

e3=e×sinθ1×sinθ2×sinθ3，

(13)

依次類推，直至等值線n，且該等值線的法線方向與上一次SCCP的誤差向量夾角為θn，那么本次SCCP的誤差en即為

en=en-1×sinθn，

(14)

其中：en-1為上一次SCCP的誤差，且0≤θn≤90°。

由此可知，en≤en-1，則每次迭代SCCP都可以有效減少CCP與實(shí)際航跡點(diǎn)之間的誤差。

綜上所述，在進(jìn)行ISCCP算法匹配時(shí)，首先判斷匹配的區(qū)域是否有相互垂直的地磁梯度等值線，若有，只選擇相互垂直的地磁梯度等值線進(jìn)行ISCCP，否則對所有的地磁梯度等值線進(jìn)行ISCCP。

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

為了驗(yàn)證ISCCP算法的有效性，進(jìn)行仿真實(shí)驗(yàn)前，首先利用克里金插值模型對實(shí)測航磁3個(gè)正交方向的梯度數(shù)據(jù)網(wǎng)格化，作為匹配算法所用的背景場，網(wǎng)格間距為100 m×100 m，如圖5所示。之后在背景場中加入2%的高斯白噪聲作為導(dǎo)航過程中測量的地磁梯度數(shù)據(jù)來源；其次，設(shè)計(jì)航行器的實(shí)際航行軌跡獲取姿態(tài)、速度與位置信息，然后利用捷聯(lián)慣導(dǎo)解算的反演算法逆推出慣性元件的導(dǎo)航信息，包括角增量與速度增量，接著再加入各項(xiàng)慣導(dǎo)系統(tǒng)誤差，通過捷聯(lián)慣導(dǎo)解算得到帶有誤差的慣導(dǎo)指示航跡[18]，表1給出了慣導(dǎo)系統(tǒng)誤差參數(shù)；最后，將以上數(shù)據(jù)代入匹配算法中進(jìn)行計(jì)算，其中，ICCP算法利用地磁總場數(shù)據(jù)進(jìn)行導(dǎo)航，ISCCP利用地磁梯度數(shù)據(jù)進(jìn)行導(dǎo)航，匹配參數(shù)如表1所示。為驗(yàn)證之前得出的結(jié)論，分別進(jìn)行直線航跡與曲線航跡的仿真實(shí)驗(yàn)。

(a)地磁總場強(qiáng)度(b)x方向地磁梯度

(c)y方向地磁梯度(d)z方向地磁梯度圖5 地磁總場與地磁梯度Fig.5 Total geomagnetic field and geomagnetic gradient

表1 仿真參數(shù)Table 1 Simulation parameters

3.1 直線航跡仿真

直線航跡仿真如圖6所示，ICCP算法在前一段的航跡匹配結(jié)果較好，總體匹配航跡的航向基本與實(shí)際航跡一致。根據(jù)之前的結(jié)論，這是由于該算法只能減小沿等值線法向方向的誤差，但同時(shí)沿等值線切線方向的誤差隨時(shí)間積累越來越大，導(dǎo)致航程的偏差越來越大。而ISCCP算法通過3個(gè)正交方向豐富的地磁梯度信息能較好約束慣導(dǎo)誤差，且僅需要3個(gè)采樣點(diǎn)進(jìn)行匹配就能取得不錯(cuò)的匹配效果。

圖6 直線航跡匹配結(jié)果Fig.6 Linear track matching results

匹配誤差如圖7所示，直線航跡下ISCCP算法的最大點(diǎn)位誤差為184.550 m，最小點(diǎn)位誤差僅為1.772 m，平均點(diǎn)位誤差為40.385 m，能將誤差控制在半個(gè)網(wǎng)格長度內(nèi)。

圖7 直線航跡匹配誤差Fig.7 Linear track matching error

3.2 曲線航跡仿真

曲線航跡由圖8所示，ICCP算法在前一段的匹配效果較好，總體匹配航跡的航程與實(shí)際航跡大致相同。但之后航跡的航向逐漸偏離實(shí)際航跡，這也是由于算法只能減小沿等值線法向方向的誤差，導(dǎo)致沿等值線切線方向的誤差不能及時(shí)減小，總體誤差越來越大，而ISCCP算法也還能保持較高的精度。

圖8 曲線航跡匹配情況Fig.8 Curve track matching results

如圖9所示，曲線航跡下ISCCP算法的最大點(diǎn)位誤差為125.701 m，最小點(diǎn)位誤差僅為0.645 m，平均點(diǎn)位誤差為19.002 m，誤差也在半個(gè)網(wǎng)格長度內(nèi)。

圖9 曲線航跡匹配誤差Fig.9 Curve track matching error

通過3.1與3.2的仿真實(shí)驗(yàn)表明，ICCP算法采用單一信息量進(jìn)行導(dǎo)航時(shí)，其原理上固有的局限性導(dǎo)致該算法在上述情形時(shí)匹配精度低，ISCCP利用地磁梯度的3個(gè)正交方向的信息進(jìn)行匹配導(dǎo)航的同時(shí)，采用迭代的搜索方式能最大程度上減小慣導(dǎo)誤差。需要注意的是，ISCCP匹配精度也是受地磁梯度等值線分布的影響，如直線航跡的第31～33號點(diǎn)，曲線航跡的第40～42號點(diǎn)匹配誤差最大，直線航跡的第1～3號點(diǎn)，曲線航跡的第52～54號點(diǎn)匹配誤差較小。下面對這4種情況進(jìn)行分析。

3.3 仿真分析

由圖10所示，當(dāng)匹配區(qū)域中的地磁梯度等值線相交，且接近垂直時(shí)，ISCCP算法匹配的效果最好，當(dāng)匹配區(qū)域中的等值線接近平行時(shí)，相當(dāng)于用單一地磁信息進(jìn)行匹配導(dǎo)航，匹配的效果自然也差，這也驗(yàn)證了之前的結(jié)論。因此，利用地磁豐富的3個(gè)正交方向梯度信息使等值線出現(xiàn)接近垂直的機(jī)率更高，能有效提高ISCCP算法的精度，如若需要進(jìn)一步提高算法的精度，可以優(yōu)先選取地磁梯度等值線近似相互垂直的區(qū)域進(jìn)行匹配。

(a)直線航跡第1^3號點(diǎn)(b)直線航跡第52^54號點(diǎn)

(c)直線航跡第31^33號點(diǎn)(d)直線航跡第40^42號點(diǎn)圖10 局部匹配結(jié)果Fig.10 Local matching results

4 結(jié)論

本文首先闡明ICCP算法在地磁變化緩慢的區(qū)域匹配誤差大的原因，即ICCP算法只能減少沿等值線法線方向的誤差，無法有效減少沿等值線切線方向的誤差；其次，本文提出了適用于地磁梯度匹配導(dǎo)航的ISCCP算法，經(jīng)過誤差模型分析，當(dāng)匹配區(qū)域中有相互垂直的地磁梯度等值線時(shí)，利用ISCCP算法能夠快速準(zhǔn)確找到實(shí)際航跡點(diǎn)的位置，當(dāng)匹配區(qū)域沒有相互垂直的地磁梯度等值線時(shí)，利用迭代法對所有的等值線進(jìn)行ISCCP也能有效減少慣導(dǎo)指示航跡點(diǎn)的誤差；最后，經(jīng)仿真實(shí)驗(yàn)表明，ISCCP算法在地磁變化平緩的區(qū)域匹配時(shí)，平均點(diǎn)位誤差能控制在半個(gè)網(wǎng)格長度以內(nèi)，有效提高了ICCP算法在地磁變化平緩的區(qū)域匹配時(shí)的精度。