徐曉蘇，吳劍飛，徐勝保，王立輝，李佩娟

（1.微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2.東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

基于仿射修正技術(shù)的水下地形ICCP匹配算法

徐曉蘇，吳劍飛，徐勝保，王立輝，李佩娟

（1.微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2.東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

ICCP匹配算法是水下組合導(dǎo)航系統(tǒng)中最為重要的匹配算法。針對(duì)傳統(tǒng)ICCP匹配算法存在僅對(duì)水下航行器慣性導(dǎo)航系統(tǒng)指示航跡作旋轉(zhuǎn)和平移的剛性變換的局限性問題，為提高水下航行器地形輔助導(dǎo)航系統(tǒng)中匹配算法的精度，分析了水下航行器慣性導(dǎo)航系統(tǒng)誤差特性，建立了誤差模型，提出了基于仿射修正技術(shù)的水下地形ICCP匹配算法。首先利用ICCP匹配算法對(duì)慣性導(dǎo)航系統(tǒng)指示航跡進(jìn)行剛性變換，再利用最小二乘法求解仿射參數(shù)，進(jìn)而對(duì)ICCP匹配航跡進(jìn)行仿射修正。仿真研究表明，基于仿射修正技術(shù)的ICCP匹配算法能較好地解決傳統(tǒng)ICCP匹配算法剛性變換的局限性，匹配精度優(yōu)于傳統(tǒng)ICCP算法，匹配誤差小于數(shù)字地圖網(wǎng)格間距的50%，同時(shí)仿射修正所耗費(fèi)時(shí)間極少，所增加的時(shí)間僅為傳統(tǒng)ICCP匹配算法匹配時(shí)間的千分之一。

水下航行器；慣性導(dǎo)航；地形匹配；ICCP；仿射變換

水下航行器因其在未來的軍用和民用領(lǐng)域都具有重要的戰(zhàn)略意義而成為了世界上軍事強(qiáng)國重點(diǎn)發(fā)展的領(lǐng)域。水下航行器有高度隱蔽性和保持長時(shí)間精確航行的要求，而實(shí)現(xiàn)這一要求的關(guān)鍵技術(shù)之一是其導(dǎo)航技術(shù)。水下的導(dǎo)航方式有別于陸路導(dǎo)航，GPS、無線電等導(dǎo)航方式因水中信號(hào)迅速衰減而不適用于水下航行器。慣性導(dǎo)航因其具有自主性、隱蔽性等優(yōu)點(diǎn)，成為了水下航行器導(dǎo)航手段的首選。但慣性導(dǎo)航有固有的缺陷，其誤差會(huì)隨著時(shí)間的積累逐漸增大，因而在慣性導(dǎo)航定位系統(tǒng)運(yùn)行一段時(shí)間后必須采用外來信息對(duì)慣性導(dǎo)航系統(tǒng)進(jìn)行修正。隨著導(dǎo)航技術(shù)特別是組合導(dǎo)航技術(shù)的發(fā)展，應(yīng)用地形、重力、地磁等信息來輔助慣性導(dǎo)航[1]為水下航行器導(dǎo)航研究提供了一種新思路。由于地形高程信息測量較為方便，且陸路上的地形匹配技術(shù)已應(yīng)用于實(shí)際，可為水下地形匹配導(dǎo)航提供借鑒，因而應(yīng)用地形高程信息來輔助水下航行器的導(dǎo)航具備高度可行性。

ICCP（Iterative Closest Contour Point）算法是目前水下地形匹配導(dǎo)航技術(shù)最為重要的匹配算法之一，其本質(zhì)為相關(guān)極值匹配算法[2]。但是ICCP算法對(duì)慣導(dǎo)航跡所作的修正僅僅是旋轉(zhuǎn)和平移，沒有對(duì)航跡進(jìn)行縮放，因而不能修正輔助定位過程中慣導(dǎo)指示航跡相對(duì)于實(shí)際航跡發(fā)生的形變所造成的誤差，因此本文引入了仿射修正技術(shù)，以解決 ICCP算法的缺陷，提高匹配精度。

1 ICCP算法

ICCP算法最初由圖像配準(zhǔn)算法 ICP（Iterative Closest Point）發(fā)展而來的[2]。國內(nèi)外許多專家、學(xué)者對(duì)ICCP算法進(jìn)行了系統(tǒng)研究，Behzad K.P.首次將該算法引入重力匹配輔助導(dǎo)航，并對(duì)其實(shí)現(xiàn)形式進(jìn)行了較為詳細(xì)的闡述。Bishop通過大量仿真試驗(yàn)系統(tǒng)地分析了該算法的可行性及誤差影響[3]，劉承香詳細(xì)分析了 ICCP算法在地形匹配輔助導(dǎo)航的適用性，并對(duì)ICCP關(guān)鍵技術(shù)展開了研究[4]。

1.1 ICCP算法描述

ICCP算法匹配[4-7]示意圖如圖1所示。水下航行器沿著實(shí)際航跡序列(N為采樣點(diǎn)個(gè)數(shù))航行過程中，由于慣性導(dǎo)航系統(tǒng)存在誤差，因此主慣導(dǎo)系統(tǒng)會(huì)給出一個(gè)偏離實(shí)際航跡序列的指示航跡序列，與此同時(shí)，測深裝置也記錄測得的水深序列。在測深裝置沒有誤差的情況下，可以確定實(shí)際航跡序列一定位于根據(jù)實(shí)測水深信息序列生成的水深等值線上。通常認(rèn)為，主慣導(dǎo)系統(tǒng)給出的位置信息在一定誤差范圍內(nèi)是可信的，因此可以將主慣導(dǎo)系統(tǒng)的指示航跡序列作為待匹配航跡序列，根據(jù)水深信息序列在數(shù)字地圖中生成等值線并在等值線上的提取最近點(diǎn)序列作為匹配目標(biāo)，迭代地尋找剛性變換T，T包括一個(gè)旋轉(zhuǎn)量R和一個(gè)平移量t，使以下目標(biāo)函數(shù)最?。?/p>

其中，wi代表其中各序列點(diǎn)的權(quán)值，k代表第k次迭代過程，N代表航跡序列點(diǎn)個(gè)數(shù)。

圖1 ICCP算法匹配示意圖Fig.1 Description of ICCP algorithm

利用四元數(shù)法[4]找到剛性變換T后，對(duì)待匹配航跡序列作剛性變換T·Si：

1.2 ICCP算法的缺陷

主慣導(dǎo)系統(tǒng)的定位誤差有如下特性：

根據(jù)加速度計(jì)的偏移、陀螺儀的漂移以及慣性導(dǎo)航系統(tǒng)的輔助定位周期可以得出[8]：

●水下航行器運(yùn)行一段時(shí)間后，主慣導(dǎo)定位系統(tǒng)給出的位置存在一定的誤差，計(jì)為，其中Xδ為緯度誤差，Yδ為經(jīng)度誤差。

●通常地形輔助定位的時(shí)間比較短，因而可得出該段時(shí)間內(nèi)，慣導(dǎo)解算的速度v為：其中，vr為水下航行器的真實(shí)速度，vδ為輔助定位前航行器速度積累誤差，vε為輔助定位過程中速度誤差的變化量。速度誤差主要是由加速度計(jì)零偏和重力加速度在水平姿態(tài)誤差上的投影所造成，由于地形輔助定位時(shí)間內(nèi)引起的速度誤差遠(yuǎn)小于航行器輔助定位之前引起的誤差，故式(3)可以簡化為：

●同理于速度解算的分析，可得慣導(dǎo)解算的航向d：

其中，dr為水下航行器的真實(shí)航向，dδ為航行器在輔助定位前航向積累誤差，dε為輔助定位過程中因誤差引起的航向變化量。同理于速度解算，計(jì)算航向的式(5)也可以簡化成：

由前面的分析可得，在慣導(dǎo)修正的這段時(shí)間，慣導(dǎo)解算航跡和航行器實(shí)際航跡之間的關(guān)系為：

通過上述對(duì)主慣導(dǎo)系統(tǒng)誤差特性分析可得，由于加速度計(jì)的偏移、陀螺儀的漂移，主慣導(dǎo)系統(tǒng)的指示航跡和水下航行器實(shí)際航跡之間的關(guān)系不僅存在旋轉(zhuǎn)和平移，而且也存在縮放，因而不是一種僅包旋轉(zhuǎn)和平移的剛性變換關(guān)系。ICCP算法作了實(shí)際航跡是水下航行器慣性導(dǎo)航系統(tǒng)的測量航跡的平移和旋轉(zhuǎn)的剛性變換的假設(shè)，因而匹配的誤差較大。

針對(duì)這一缺陷，可引入仿射模型[8]來對(duì)ICCP匹配后的航跡進(jìn)行再次修正，以提高匹配精度。通過模擬退火等算法來搜索仿射變換因子的最優(yōu)解以實(shí)現(xiàn)航跡匹配的計(jì)算量較大，難以滿足水下航行器地形匹配的實(shí)時(shí)性要求，因此本文綜合考慮了匹配精度和效率，采用最小二乘法來求解仿射參數(shù)，提高算法實(shí)時(shí)性，再對(duì) ICCP匹配算法的匹配結(jié)果進(jìn)行仿射變換，以解決ICCP匹配算法所存在的缺陷，提高匹配精度。

2 仿射變換算法

2.1 仿射變換數(shù)學(xué)描述

通過對(duì)慣導(dǎo)系統(tǒng)的誤差模型的分析，可以建立一個(gè)簡單仿射變化模型[9-10]，包含平移、旋轉(zhuǎn)、縮放三種變換，分別對(duì)應(yīng)于水下航行器的初始位置誤差、航向角誤差、速率誤差。其數(shù)學(xué)描述可表示為：

其中，POS為變換前的位置信息，POS’為變換后的位置信息，θ為旋轉(zhuǎn)角，對(duì)應(yīng)于航向角誤差；α為縮放量，對(duì)應(yīng)于速率誤差；tx、ty為平移量，對(duì)應(yīng)于初始位置誤差。

圖2 仿射模型示意圖Fig.2 Description of affine model

2.2 仿射變換參數(shù)求解

仿射變換參數(shù)求解具體方法敘述如下：

將求得ESD(tx,ty,α,θ)最小時(shí)候的g記為，對(duì)ESD求導(dǎo)可得：

由于ESD對(duì)g的二階導(dǎo)數(shù)

3 仿射修正ICCP匹配算法

基于仿射修正的ICCP匹配算法具體敘述如下：

第一步：在匹配區(qū)建立的規(guī)則網(wǎng)格數(shù)字地圖上提取等深線。

第二步：由主慣性導(dǎo)航系統(tǒng)的位置信息序列和航行過程中實(shí)際測量的水深信息，得到一個(gè)預(yù)匹配的初始對(duì)準(zhǔn)航跡。

第三步：應(yīng)用ICCP算法，對(duì)初始對(duì)準(zhǔn)航跡進(jìn)行剛性變換，尋找全局最優(yōu)的航跡。

第四步：應(yīng)用仿射變換，對(duì)由ICCP算法得到的最優(yōu)航跡進(jìn)行縮放和旋轉(zhuǎn)變換。

第五步：對(duì)最后得到的航跡進(jìn)行精度評(píng)估，利用成功匹配的結(jié)果修正主慣導(dǎo)系統(tǒng)。

圖3 射修正ICCP算法流程圖Fig.3 Flow chart of ICCP algorithm based on affine correction

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

為了驗(yàn)證本文所述匹配方法在不同特征地形[11]上的有效性，本文在選取了兩塊特征不同的地形，其中地形a水深標(biāo)準(zhǔn)差為5.7029，地形b水深標(biāo)準(zhǔn)差為7.8779，如圖4(a)、圖4(b)所示。在每塊地形上規(guī)劃三條不同的航跡，再利用 ICCP算法和仿射修正 ICCP算法進(jìn)行匹配，比較匹配誤差以驗(yàn)證仿射修正 ICCP算法在不同地形特征，不同航跡上提高匹配精度的有效性，航跡匹配結(jié)果如圖5(a)、圖5(b)所示。

實(shí)驗(yàn)中的主慣導(dǎo)系統(tǒng)的慣性器件參數(shù)和采樣參數(shù)如表1。兩塊地形匹配結(jié)果統(tǒng)計(jì)如表2(a)、表2(b)。

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

圖4 (a) 仿真實(shí)驗(yàn)所用的地形圖(a)Fig.4(a) Topographic map (a) used in the simulation

圖4 (b) 仿真實(shí)驗(yàn)所用的地形圖(b)Fig.4(b) Topographic map (b) used in the simulation

圖5 (a) 水下航行器航跡匹配圖（a）Fig.5(a) Topographic map (a) used in the simulation

圖5 (b) 水下航行器航跡匹配圖（b）Fig.5(b) Topographic map (b) used in the simulation

表2 (a) 地形a匹配結(jié)果Tab.2(a) Matching results of topographic map a

表2 (b) 地形b匹配結(jié)果Tab.2(b) Matching results of topographic map b

從地形匹配的結(jié)果可以看出：在表1所設(shè)置的仿真參數(shù)條件下，在地形特征不同的兩個(gè)區(qū)域，沿三種不同航向的航跡，利用本文所述的仿射修正ICCP匹配方法都能解決ICCP算法僅做旋轉(zhuǎn)和平移的剛性變換的缺陷。相比于ICCP算法的匹配方法，仿射修正ICCP匹配方法在匹配精度上有很大的提高，同時(shí)，由于ICCP匹配航跡已經(jīng)位于真實(shí)航跡附近，仿射修正時(shí)無需進(jìn)行全局最優(yōu)收斂的判斷，因而運(yùn)算量較小，一般只需ICCP所耗時(shí)間的千分之一。

5 結(jié) 論

本文分析了ICCP算法剛性變換的局限性，并針對(duì)ICCP算法的局限性，提出了基于仿射修正技術(shù)水下地形ICCP匹配導(dǎo)航算法，給出了仿射變換的參數(shù)求解方法。通過仿真實(shí)驗(yàn)的驗(yàn)證了算法在不同地形不同航跡上匹配的有效性，結(jié)果表明：基于仿射修正技術(shù)的水下地形ICCP匹配算法可以以極小的時(shí)間代價(jià)來彌補(bǔ)ICCP算法剛性變換的局限性，匹配精度優(yōu)于ICCP算法，匹配誤差小于數(shù)字地圖格網(wǎng)間距的50%。

（References）：

[1]Deng Z,Ge Y,Guan W,et al.Underwater map-matching aided inertial navigation system based on multigeophysical information[J].Frontiers of Electrical and Electronic Engineering in China,2010,5(4):496-500.

[2]童余德，邊少鋒，蔣東方，等.實(shí)時(shí)ICCP算法重力匹配仿真[J].中國慣性技術(shù)學(xué)報(bào)，2011，19(3)：340-343.TONG Yu-de,BIAN Shao-feng,JLANG Dong-fang,XIAO Sheng-hong.Gravity matching simulation of realtime ICCP algorithm[J].Journal of Chinese Inertial Technology,2011,19(3):340-343.

[3]Bishop G C.Gravitational field maps and navigational errors[J].Oceanic Engineering,2002,27(3):726-737.

[4]劉承香.水下潛器的地形匹配輔助定位技術(shù)研究[D].哈爾濱：哈爾濱工程大學(xué)，2003.LIU Cheng-xiang.Research on the terrain matching system of the underwater vehicle[D].Harbin:Harbin Engineering University,2003.

[5]Zhou Yingying,Zhang Yanshun,Guo Lei.Simulation of geomagnetic/inertial integrated navigation system[C]//2011 Third International Conference on Measuring Technology and Mechatronics Automation.2011,vol.3:253-256.

[6]Zhang Y,Kang C,Li H,et al.Geomagnetism-aided navigation based on matching algorithm for underwater vehicles[M]//Mechanical Engineering and Technology.Springer Berlin Heidelberg,2012:59-65.

[7]張濤，徐曉蘇，李佩娟.混沌優(yōu)化水下地形匹配算法研究[J].中國慣性技術(shù)學(xué)報(bào)，2009，17(2)：156-158，164.ZHANG Tao,XU Xiao-su,LI Pei-juan.Underwater terrain matching algorithm based on chaotic optimization[J].Journal of Chinese Inertial Technology,2009,17(2):156-158,164.

[8]羅詩途，任治新.基于仿射模型變換的地磁匹配導(dǎo)航算法[J].中國慣性技術(shù)學(xué)報(bào)，2010，18(4)：462-465.LUO Shi-tu,REN Zhi-xin.Geomagnetic matching algorithms based on affine model[J].Journal of Chinese Inertial Technology,2010,18(4):426-465.

[9]劉穎，吳美平，謝紅衛(wèi).地磁匹配算法研究框架和組合匹配策略[J].國防科技大學(xué)學(xué)報(bào)，2010，32(06)：172-176.LIU Ying,WU Mei-ping,XIE Hong-wei.A framework for magnetic matching study and an integrated matching method[J].Journal of National University of Defense Technology,2010,32(6):172-176.

[10]Jiang Z,Chen Z,Zheng B,et al.A kind of 3D reconstructtion method based on affine transformation[C]//2009 3rd International Conference on Genetic and Evolu- tionary Computing.2009:358-361.

[11]Wang K,Li Y,Rizos C.Research on terrain suitability of iterative closest contour point algorithm for underwater navigation[C]//Proceedings of the 22nd International Technical Meeting of the Satellite Division of The Institute of Navigation.2009:866-870.

ICCP algorithm for underwater terrain matching navigation based on affine correction

XU Xiao-su,WU Jian-fei,XU Sheng-bao,WANG Li-hui,LI Pei-juan
(1.Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology,Ministry of Education,Southeast University,Nanjing 210096,China;2.School of Instrument Science &Engineering,Southeast University,Nanjing 210096,China)

ICCP is the most important matching algorithm used in underwater integrated navigation system.Traditional ICCP algorithm can only do rigid transformation (rotation and translation) for the indicated track of underwater vehicle’s INS.In order to overcome this problem and improve the accuracy of track matching,the error characteristics of INS are analyzed,and the error model of INS is established.Then a new ICCP algorithm based on affine correction is put forward.The track indicated by INS is transformed according to the ICCP algorithm.The affine parameters are solved with the least squares method.The ICCP matching track is modified by affine transformation.The simulations show that the defects of traditional ICCP algorithm can be avoided by affine correction,and the matching result is better than those of traditional ICCP algorithm,in which the matching error is less than 50% of digital map grid spacing.Meanwhile,the additional time consumed in affine correction is just about one thousandth of that in traditional ICCP algorithm.

underwater vehicle;inertial navigation;terrain matching;ICCP;affine transformation

U666.1

A

1005-6734(2014)03-0362-06

10.13695/j.cnki.12-1222/o3.2014.03.016

2014-01-13；

2014-04-15

國家自然科學(xué)基金項(xiàng)目（51175082，61203192）

徐曉蘇（1961—），男，博士生導(dǎo)師，從事測控技術(shù)與導(dǎo)航定位領(lǐng)域的研究。E-mail：xxs@seu.edu.cn