基于迭代最近點算法的地形匹配算法可靠性分析

1、第22卷,第1期2005年1月深圳大學學報理工版JOURNALOFSHENZHENUNIVERSITYSCIENCEANDENGINEERINGVol 22,No 1Jan 2005文章編號:1000 2618(2005)01 0022 05基于迭代最近點算法的地形匹配算法可靠性分析劉承香,阮雙琛,劉繁明,張 敏2(1.深圳大學工程技術(shù)學院,深圳518060;2.哈爾濱工程大學自動化學院,哈爾濱150001)摘 要:根據(jù)迭代最近點算法的原理,從幾何直觀的角度研究了地形輔助導航系統(tǒng)匹配的可靠性,推導了旋轉(zhuǎn)和平移的可靠性公式,并進行了數(shù)字仿真,結(jié)果表明,本文推導的可靠性結(jié)論是正確的.關(guān)鍵詞:迭代最

2、近點算法;地形匹配;可靠性;旋轉(zhuǎn)和平移可靠性中圖分類號:U666 1 文獻標識碼:A性的直觀推導.本文結(jié)合文獻79的思想,對引 言21世紀,探索和開發(fā)海洋已成為人類向海洋索取物質(zhì)資源和生存空間的主要生產(chǎn)活動,水下潛器將成為人類在這一生產(chǎn)活動中的重要工具和得力助手.由于水下潛器的活動范圍廣、體積小、質(zhì)量輕、噪聲低,且隱蔽性好,因此在軍事上有著非常重要的作用1.目前潛器的導航手段在水下主要采用慣性導航系統(tǒng)或聲學手段,可以預(yù)期能夠利用海底的地形來進行輔助導航.海底地形輔助導航的基本原理是利用潛器實際測量的水深值與已存在的數(shù)字地圖進行匹配,且對潛器進行絕對定位的方法.海底地形匹配導航是水下潛器導航技術(shù)

3、的一個非常新的發(fā)展方向,國內(nèi)尚屬空白,國外在20世紀90年代開始研究.美國和法國合作研究的UUV非傳統(tǒng)導航(NTN)項目1997年啟動,目前已取得初步成果13.他們在重力匹配中采用了迭代最近點(iterativeclosestpoint,ICP)算法1,46,且對ICP算法在非傳統(tǒng)導航系統(tǒng)中的應(yīng)用進行了可行性論證,文獻中也進行了大量的仿真論證.本文也采用ICP算法作為地形匹配算法的數(shù)據(jù)解算部分,更詳細的介紹請參考文獻1.國外一些學者展開了基于ICP算法的圖像對準領(lǐng)域的可靠性研究,其中D Simon在文獻7中給出了ICP算法進行三維變換時平移和旋轉(zhuǎn)的可靠性的推導,文獻8,9給出了三維變換中旋轉(zhuǎn)可

4、靠ICP算法在地形匹配應(yīng)用中進行二維變換時的旋轉(zhuǎn)和平移可靠性進行了幾何直觀推導.1 ICP算法基礎(chǔ)1 1 ICP算法ICP算法不需事前確定對應(yīng)估計,只是不斷重復(fù)(初始)運動變換 確定最近點 求運動變換的過程,逐步改進運動的估計.算法的主要思路是:將測得的沿著航跡的水深值連接起來構(gòu)成曲線,與已存在的水深等值線圖進行匹配.ICP算法的基本描述如下1,46:在潛器航跡上測得的N個水深數(shù)據(jù)的三維坐標Pi=( , ,h)i的集合作為迭代的初值Pi0,并從已知數(shù)字地圖中抽取等值線集C,設(shè)定最小距離TM和最大迭代次數(shù)Tm;!對每一個數(shù)據(jù)點Pi在其等值線上尋找最近點,記為Yi;尋找變換T,使集合Y=Yi與集合

5、Pi之間量測距離最小35,10,11,NYi-T(Pi)2;dk(T)=M(Y,TP)=(1)#Ni=1將集合Pk-1變換到集合TPik-1,即Pik=TPik-1;(2)%當dk-dk+1> 時,返回!繼續(xù)迭代;若dk-dk+1 ,則進入步驟&收稿日期:2004 08 17: ,(),.E szu第1期劉承香,阮雙琛,等:基于迭代最近點算法的地形匹配算法可靠性分析 23&如果系統(tǒng)滿足最終的收斂條件dk+1<TM或迭代次數(shù)m Tm,則計算結(jié)束;否則匹配失敗,不能作為校正依據(jù).對于數(shù)據(jù)點集的任何給定的初始旋轉(zhuǎn)和平移,ICP算法都能使其收斂到局部最優(yōu)匹配,為尋找全局最優(yōu)

6、,必須找到所有局部最優(yōu)的最小值.1 2 目標函數(shù)的表示假定Pi和C分別為測量點集和等值線集.首先旋轉(zhuǎn)測量點集Pi中的一個測量點,設(shè)變換為T.然后,對T(Pi)尋找最近點Yi.在Pi中所有N個點之間的暫時相關(guān)性確定后,變換T便不斷地迭代更新,以減少第k次迭代的目標函數(shù)dk(T),表示為dk(T)=NNd!(!)=K!(!-!0)2+e!.(5)設(shè)測量數(shù)據(jù)點集Pi的質(zhì)心為原點O,圖1表示了第i個對應(yīng)點之間由于旋轉(zhuǎn)而引起的誤差距離.(為點Pi處的法向假定Pi是等值線上的任意點;ni量;Si為點Pi處的切線.P)i是通過繞原點O旋轉(zhuǎn)一個小的擾動角!獲得的(注:由于是進行二維變換,所以旋轉(zhuǎn)軸垂直于平面,

7、即垂直于圖1所示的平面,().PiP)i和(的夾角為這里表示為ai,PiP)i和Sini間的夾角表示為#i.Yi為P)i到等值線上的最近點,假定旋轉(zhuǎn)角度!很小,則Pi附近的等值線認為比較平滑,則Yi可近似在切線Si上.從擾動點P)i到模型上的最近點Yi間的距離表示為mi,它影響在點Pi處的目標函數(shù).從O到Pi間的向量表示為p(i.i=1#Yi-T(Pi).(3)其中,T表示測量點集和最近等值線點集之間的變換,包括平移和旋轉(zhuǎn).以R表示旋轉(zhuǎn),Y和P表示相應(yīng)的平移量和旋轉(zhuǎn)中心.對于理想的精確對準,我們可以寫為Yi=T(Pi)=R(Pi-P)+Y.如果在旋轉(zhuǎn)和平移參數(shù)中有誤差,上式可寫作Yi=(R+R

8、)(Pi-P-P)+Y+Y.將上面的兩個方程聯(lián)立求解,取其一階近似解R(Pi-P)-RP+Y=0.(4)圖1 當P)i是由Pi繞點O旋轉(zhuǎn)!角得到時,P)i到等值線上的最近點Yi的距離miFig 1 ThedistancemifrompointP)itotheclosestpointontheiso contorYi,whenP)iistherotatedpointofPiaroundpointO式(4)意味著方位誤差R和平移誤差Y相關(guān),這使得分析更為復(fù)雜.本文將分別考慮方位誤差和平移誤差.2 基于ICP算法的地形匹配算法可靠性分析2 1 旋轉(zhuǎn)的可靠性如式(3)所示,定義匹配算法的目標函數(shù)為對應(yīng)

9、點之間的均方距離,它可以在最小點附近近似展開成二階多項式函數(shù).因為此節(jié)主要討論對準的旋轉(zhuǎn)變量,目標函數(shù)可以建模為兩個獨立變量的函數(shù).假定當對準參數(shù)!等于!0時目標函數(shù)有一個最小值e!.參數(shù)!表示航跡的旋轉(zhuǎn)角,從而最終迭代!當旋轉(zhuǎn)角!很小時,距離mi可表示為mi=其中,均值,d(!)=NP)iYi=PiP)isin#iip(icosPiP)icosi(6)+表示L2范數(shù).這里定義目標函數(shù)為mi的NNi=1#2mi=Ni=1#(i)p(icos!因為可靠性K!為式(5)中簡化二階目標函數(shù)的二階系數(shù),于是它可表示為K!=N#(i=1N2i).p(icos(7)24深圳大學學報理工版(和用以描述數(shù)字地

10、圖形可靠性K!可用旋轉(zhuǎn)軸a第22卷狀復(fù)雜度的可靠性矩陣來表示.圖1中,當旋轉(zhuǎn)角!很小時,弦PiP)i可用來近似表示Pi旋轉(zhuǎn)形成的圓周的切線.切線方向可以通過旋轉(zhuǎn)軸和位置向量的叉乘獲得,即a,pi.從而cosni+PiP)i=ni+(a,pi)i=ni+a,p(ip(iN(的夾角為一個向量t(獲得的.PiP)i和ni,PiP)i和iSi之間的夾角為#i.Yi為P)i到等值線上的最近點,假定平移量t很小,則Pi附近的等值線認為比較平滑,那么Yi可近似在切線Si上.從擾動點P)i到模型上的最近點Yi之間的距離表示為mi,它影響在點Pi處的目標函數(shù).=1p(ii+(a,p(i).(8)其中.表示單位向

11、量.將式(8)代入式(7)得K!=N=N進行同等變換K!=N=N=aTNi=1N#(i=12i)p(icos#a+(p(i,ni)2.i=1#Na+(p(i,ni)+ (,ni)+a pi圖2 當P)i是由Pi平移向量t(得到時,i=1#T(Ta(pi,ni)(p(i,ni)a(,ni)(p(i,ni)TapiN(9)NP)i到等值線上的最近點Yi的距離miFig 2 ThedistancemifrompointP)itotheclosestpointontheiso contourYi,whenP)iisthetranslatedpointofPibyt(i=1#N/aTMRa.其中旋轉(zhuǎn)可靠

12、性矩陣MR定義為MR=Ni=1#(ni)(p(i,ni).p(i,T(10)當平移向量t(很小時,距離mi可表示為mi=P)iYi=PiP)isin#i旋轉(zhuǎn)可靠性矩陣MR確定后,可計算旋轉(zhuǎn)可靠性K!.式(7)、式(9)和式(10)表明旋轉(zhuǎn)可靠性K!與角度i有關(guān),夾角越大,可靠性越小.其直觀的解釋是:i角越大,旋轉(zhuǎn)角!旋轉(zhuǎn)得到的最近距離mi在法向的投影越小;在有噪聲的情況下,mi越小受噪聲影響越大,可靠性越差.2 2 平移的可靠性單獨推導平移可靠性,忽略旋轉(zhuǎn)角誤差.假定當對準參數(shù)t等于t0時目標函數(shù)有一個最小值et.參數(shù)t可表示航跡的平移量,從而最終迭代的目標函數(shù)在全局最小點t0處的泰勒展開可以

13、近似為dt(t)=Kt(t-t0)2+et.(11)PiP)icosii.t(cos(12)定義目標函數(shù)為m2i的均值,dt(t)=Ni=1#N2mi=Ni=1#cos2N2it.2(13)同理Kt可表示為Kt=N其中,i=1#cosNi.(14)(15)cosni+PiP)i=ni+t.i=將式(15)代入式(14)得Kt=Ncos=i#Ni=12N設(shè)測量數(shù)據(jù)點集Pi的質(zhì)心為原點O,圖2表示了第i個對應(yīng)點之間由于平移引起的誤差距離.(為點Pi如圖2所示,Pi是等值線上的任意點,且niiii=1#(ni+t)N2.(16)從式(16)可見,平移的可靠性與平移矢量和等值線上各點的法線之間的夾角有

14、關(guān),夾角越大,Kt越小,可靠性越差.可直觀解釋為平移向量與法向第1期劉承香,阮雙琛,等:基于迭代最近點算法的地形匹配算法可靠性分析 25夾角越大,則平移向量t(在法向的投影越小,而mi平行于法向,所以噪聲的影響也就越大,可靠性越差.結(jié) 語本文對基于ICP算法的地形匹配算法的可靠性3 仿真及其分析本文舉例驗證基于ICP算法的地形匹配算法的可靠性.考慮一載體以圖3中所示的航跡航行(航跡是任意設(shè)定的),測量了5個點地形水深值分別為900、880、860、840和820(單位:m).圖3中示出了載體的實際航跡(粗虛線表示)和水深測量點及與5個水深值對應(yīng)的等值線,預(yù)設(shè)的測量航跡由實際航跡加上100航向誤

15、差及某些平移矢量形成.從中可見,當對預(yù)設(shè)航跡使用ICP算法時,幾次迭代后就逐漸收斂到實際航跡,所有的迭代路徑都在圖中畫出.值得注意的是,最近等值線點的彎曲形狀同實際航跡和測量航跡不相似,然而進行剛性變換后的航跡跟實際航跡形狀是一致的,更明顯的是,在變換收斂到最優(yōu)匹配的過程中,最近等值線點構(gòu)成的形狀與實際航跡會越來越相似.這說明對于這塊地圖載體所走的路徑利用ICP算法是可收斂的.且可從圖3看出,雖然測量航跡最上面的點偏離真實航跡所對應(yīng)點相對較遠,但因真實航跡最上面的點所在等值線的曲率最大,其經(jīng)過幾次迭代就可很好地找到真實點,從圖中還可看出曲率越大的點收斂越快.即驗證了前述關(guān)于可靠性的推導.進行了

16、研究,推導了對準匹配中的兩個基本參數(shù)(旋轉(zhuǎn)角和平移量)的可靠性,得出可靠性的基本公式.結(jié)果表明:地形輔助導航系統(tǒng)對準匹配的可靠性和等值線的曲率、航跡與等值線法向的夾角有很大關(guān)系.與實際中地形匹配的常識吻合,仿真結(jié)果證明了本結(jié)論的正確性.參考文獻:1劉承香.水下潛器的地形匹配輔助定位技術(shù)研究D.哈爾濱:哈爾濱工程大學,2003.2BehzadKamgar Parsi,BehroozKamgar Parsi.基于重力圖的載體定位A.SPIE會議 無人載體技術(shù)C.佛羅里達:SPIE,1999.182 191(英文版).3GarnerCB.重力場圖和導航誤差J.IEEE海洋工程學報,2002,27(3

17、):726 737(英文版).4PaulJBesl,NeilDMcKay.三維物體對準方法J.IEEE模式分析和機器智能會刊,1992,14(2):239 256(英文版).5ZZhang.對自由曲線和曲面進行迭代點匹配方法研究J.計算機視覺國際期刊,1994,13(2):119 152(英文版).6劉承香,劉繁明,劉 柱,等.快速ICCP算法實現(xiàn)地形匹配技術(shù)研究J.船舶工程,2003,25(3):54 56.7DSimon.快速精確的圖像對準方法D.卡耐基梅隆大學,1996(英文版).8Byung UkLee,Chul MinKim,Rae HongPark.最近點迭代算法的方位可靠性矩陣J

18、.IEEE模式分析和機器智能會刊,2000,22(10):1205 1208(英文版).9Byung UkLee,Chul MinKim,Rae HongPark.ICP算法中旋轉(zhuǎn)角度的誤差靈敏性A.SPIE三維圖像捕獲和應(yīng)用II會議C.佛羅里達:SPIE,1999.146 156(英文版).10BertholdKPHorn.采用單位四元數(shù)進行絕對定位的閉環(huán)解J.美國光學協(xié)會,1987,4(4):629 642(英文版).11BehzadKamgar Parsi,BehroozKamgar Parsi.地球物理圖的對準算法R.美國:美國政府工作報告.974 980(英圖3 地形匹配可靠性實例F

19、ig 3 Anexampleofthereliabilityofterrainmatching文版).26Abstract:1000 2618(2005)01 0026 EA深圳大學學報理工版第22卷AnalysisonthereliabilityofterrainmatchingalgorithmbasedonICPLIUCheng xiang1,RUANShuang chen1,LIUFan ming2,andZHANGming11)CollegeofEngineeringandTechnologyShenzhenUniversityShenzhen,518060P.R.China2)Co

20、llegeofAutomationHarbinEngineeringUniversityHarbin150001P.R.ChinaAbstract:Seabottomterrainmappingassistantnavigationisanewmethodinnavigationtechnologyofunderwatervehicles.Inthispaper,thereliabilityofthealgorithmofterrainmatchingbasedonICPalgorithmwasanalyzed.Thematchingreliabilityofterrainassistantn

21、avigationsystemwasdeterminedbytheintuitivegeometricalreasoning.Theformulasofreliabilityforro tationandtranslationwerededuced.Varioussimulationsresultshaveshowntheeffectivenessofthisnewmethod.Keywords:ICPalgorithm;terrainmatching;reliability;rotateandtransitreliabilityReferences:1LIUCheng xiang.AStud

22、yonTerrainMatchingAssistantPositioningTechnologyforUnderwaterVehicleD.Harbin:HarbinEngineeringUniversity,2003(inChinese).2BehzadKamgar Parsi,BehroozKamgar Parsi.VehiclelocalizationongravitymapsA.SPIEConferenceonUnmannedGroundVehicleTechnologyC.Florida:SPIE,1999.182 191.3GarnerCB.Gravitationalfieldma

23、psandnavigationalerrorsJ.IEEEJournalofOceanicEngineering,2002,27(3):726 737.4PaulJBesl,NeilDMcKay.Amethodforregistrationof3 DshapesJ.IEEETransactionsonPatternAnalysisandMachineIntelligence,1992,14(2):239 256.5ZZhang.IterativepointsmatchingforregistrationoffreeformcurvesandsurfacesJ.InternationalJournalofComputer,1994,13(2):119 152.6LIUCheng xiang,LIUFan ming,LIUZhu,etal.Studyontechniquesfors