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

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

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

4、靠ICP算法在地形匹配應用中進行二維變換時的旋轉和平移可靠性進行了幾何直觀推導.1 ICP算法基礎1 1 ICP算法ICP算法不需事前確定對應估計,只是不斷重復(初始)運動變換 確定最近點 求運動變換的過程,逐步改進運動的估計.算法的主要思路是:將測得的沿著航跡的水深值連接起來構成曲線,與已存在的水深等值線圖進行匹配.ICP算法的基本描述如下1,46:在潛器航跡上測得的N個水深數據的三維坐標Pi=( , ,h)i的集合作為迭代的初值Pi0,并從已知數字地圖中抽取等值線集C,設定最小距離TM和最大迭代次數Tm;!對每一個數據點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> 時,返回!繼續迭代;若dk-dk+1 ,則進入步驟&收稿日期:2004 08 17: ,(),.E szu第1期劉承香,阮雙琛,等:基于迭代最近點算法的地形匹配算法可靠性分析 23&如果系統滿足最終的收斂條件dk+1<TM或迭代次數m Tm,則計算結束;否則匹配失敗,不能作為校正依據.對于數據點集的任何給定的初始旋轉和平移,ICP算法都能使其收斂到局部最優匹配,為尋找全局最優

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

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

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

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

10、圖形可靠性K!可用旋轉軸a第22卷狀復雜度的可靠性矩陣來表示.圖1中,當旋轉角!很小時,弦PiP)i可用來近似表示Pi旋轉形成的圓周的切線.切線方向可以通過旋轉軸和位置向量的叉乘獲得,即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處的目標函數.=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.其中旋轉可靠

12、性矩陣MR定義為MR=Ni=1#(ni)(p(i,ni).p(i,T(10)當平移向量t(很小時,距離mi可表示為mi=P)iYi=PiP)isin#i旋轉可靠性矩陣MR確定后,可計算旋轉可靠性K!.式(7)、式(9)和式(10)表明旋轉可靠性K!與角度i有關,夾角越大,可靠性越小.其直觀的解釋是:i角越大,旋轉角!旋轉得到的最近距離mi在法向的投影越小;在有噪聲的情況下,mi越小受噪聲影響越大,可靠性越差.2 2 平移的可靠性dt(t)=Kt(t-t0)2+et.(11)PiP)icosii.t(cos(12)定義目標函數為m2i的均值,dt(t)=Ni=1#N2mi=Ni=1#cos2N2

13、it.2(13)同理Kt可表示為Kt=N其中,i=1#cosNi.(14)(15)cosni+PiP)i=ni+t.i=將式(15)代入式(14)得Kt=Ncos=i#Ni=12N設測量數據點集Pi的質心為原點O,圖2表示了第i個對應點之間由于平移引起的誤差距離.(為點Pi如圖2所示,Pi是等值線上的任意點,且niiii=1#(ni+t)N2.(16)從式(16)可見,平移的可靠性與平移矢量和等值線上各點的法線之間的夾角有關,夾角越大,Kt越小,可靠性越差.可直觀解釋為平移向量與法向第1期劉承香,阮雙琛,等:基于迭代最近點算法的地形匹配算法可靠性分析 25夾角越大,則平移向量t(在法向的投影越

14、小,而mi平行于法向,所以噪聲的影響也就越大,可靠性越差.結 語本文對基于ICP算法的地形匹配算法的可靠性3 仿真及其分析.進行了研究,推導了對準匹配中的兩個基本參數(旋轉角和平移量)的可靠性,得出可靠性的基本公式.結果表明:地形輔助導航系統對準匹配的可靠性和等值線的曲率、航跡與等值線法向的夾角有很大關系.與實際中地形匹配的常識吻合,仿真結果證明了本結論的正確性.參考文獻:1劉承香.水下潛器的地形匹配輔助定位技術研究D.哈爾濱:哈爾濱工程大學,2003.2BehzadKamgar Parsi,BehroozKamgar Parsi.基于重力圖的載體定位A.SPIE會議 無人載體技術C.佛羅里達

15、:SPIE,1999.182 191(英文版).3GarnerCB.重力場圖和導航誤差J.IEEE海洋工程學報,2002,27(3):726 737(英文版).4PaulJBesl,NeilDMcKay.三維物體對準方法J.IEEE模式分析和機器智能會刊,1992,14(2):239 256(英文版).5ZZhang.對自由曲線和曲面進行迭代點匹配方法研究J.計算機視覺國際期刊,1994,13(2):119 152(英文版).6劉承香,劉繁明,劉 柱,等.快速ICCP算法實現地形匹配技術研究J.船舶工程,2003,25(3):54 56.7DSimon.快速精確的圖像對準方法D.卡耐基梅隆大學

16、,1996(英文版).8Byung UkLee,Chul MinKim,Rae HongPark.最近點迭代算法的方位可靠性矩陣J.IEEE模式分析和機器智能會刊,2000,22(10):1205 1208(英文版).9Byung UkLee,Chul MinKim,Rae HongPark.ICP算法中旋轉角度的誤差靈敏性A.SPIE三維圖像捕獲和應用II會議C.佛羅里達:SPIE,1999.146 156(英文版).10BertholdKPHorn.采用單位四元數進行絕對定位的閉環解J.美國光學協會,1987,4(4):629 642(英文版).11BehzadKamgar Parsi,B

17、ehroozKamgar Parsi.地球物理圖的對準算法R.美國:美國政府工作報告.974 980(英圖3 地形匹配可靠性實例Fig 3 Anexampleofthereliabilityofterrainmatching文版).26Abstract:1000 2618(2005)01 0026 EA深圳大學學報理工版第22卷AnalysisonthereliabilityofterrainmatchingalgorithmbasedonICPLIUCheng xiang1,RUANShuang chen1,LIUFan ming2,andZHANGming11)CollegeofEngin

18、eeringandTechnology2)CollegeofAutomationReferences:1LIUCheng xiang.AStudyonTerrainMatchingAssistantPositioningTechnologyforUnderwaterVehicleD.Harbin:HarbinEngineeringUniversity,2003(inChinese).2BehzadKamgar Parsi,BehroozKamgar Parsi.VehiclelocalizationongravitymapsA.SPIEConferenceonUnmannedGroundVeh

19、icleTechnologyC.Florida:SPIE,1999.182 191.3GarnerCB.GravitationalfieldmapsandnavigationalerrorsJ.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.StudyontechniquesforseabottomterrainmappingsystemusingrapidICCPalgorithmJ.ShipEngineering,2003,25(3):54 56(inChi1中文責編:云 鷹;英文責編:雨 辰2nes