基于ICP—SVD的多移動機器人拓撲地圖創建研究

王娜++王海艷++劉紀新++鄭義

摘要：移動機器人要完成智能任務，前提是創建未知環境地圖，本文利用圖像配準方法，采用ICP算法結合奇異值分解算法（SVD），建立了地圖融合的數學模型，解決了多移動機器人拓撲地圖融合問題。實驗結果表明，將ICP-SVD圖像配準算法應用于機器人拓撲地圖融合，算法簡單，消耗時間短，且有效提高了地圖的匹配效果。

關鍵詞：多移動機器人；拓撲地圖；圖像配準；ICP-SVD

0 引言

多個移動機器人位于同一空間，協同工作，可以提高探索環境的效率；融合多個機器人的傳感器數據，可以有效提高環境地圖的精確性。拓撲地圖重在描述環境的拓撲結構，占用存儲空間少，計算時間短，所以，拓撲地圖的相關計算效率較高。

現階段的研究工作中，Dedeoglu解決了融合基于路標的地圖，利用兩個地圖間的某一頂點匹配，估計一種轉變方案，配準其他的頂點，產生統一的全局地圖，但是，多機器人協作未得到一個完整的總體解決方案。Konolige采用一種決策框架理論方法，在框架范圍內的局部地圖中，實現了機器人相對定位，證明基于特征點的融合方法具有更高效率，缺點是特征點的提取過程較復雜。

本文中采用的ICP（Iterative Closest Point）圖像配準方法，即反復迭代尋找最近點，是Besl在1992年提出的。文中的拓撲地圖融合來自幾何信息，如路徑連通性，節點類型等。利用結構的圖像匹配和幾何特征的圖像配準，依據匹配范圍和誤差的大小，保留最佳匹配方案的假設，融合兩個地圖。

1 地圖融合的數學模型建立

首先對拓撲地圖進行平面轉換，采用線性變換中的剛體變換：

將平面中的一點用齊次坐標表示，向量 表示點 ， 代表旋轉矩陣， 代表平移矩陣， 分別表示x方向平移，y方向平移和逆時針旋轉角度。

令 是三個實數，則點p關于 轉移關系為：

式（1）表示對點 進行角度為 的旋轉變換，沿坐標軸方向在坐標平面內進行 的平移。

文中的拓撲地圖融合，采用的是兩兩匹配方法。若兩個局部地圖已知，目的是搜索某一平面轉換，此時對應的匹配范圍應為最大。

將這兩個地圖記為MapA和MapB，EA、EB表示地圖的兩個矩陣，矩陣中的元素表示路徑端點坐標。設路徑數分別為n和m，則地圖MapA、MapB間的相似度關系如下：

式中， 表示兩個地圖匹配的程度， 表示地圖MapA中的路徑i， 表示地圖MapB中的路徑j。

2 ICP-SVD算法解決拓撲地圖融合

將ICP-SVD算法應用于拓撲地圖融合，基本輸入是兩個地圖的路徑端點集合，搜索各個路徑端點在固定地圖上的最近點。

固定地圖的路徑端點集合記作M，坐標為： ；另一進行平面變換的地圖路徑端點集合記作D，坐標為： 。當計算到第k次迭代時， ，計算M與 間的變換矩陣，更新原變換，到兩組數據間的距離最小為止，即經過迭代使如下優化函數最小。

ICP-SVD算法的每一次迭代包括以下四步：

Step1：初始化：求兩組點集的均值， ， ；初始化旋轉矩陣R= ，平移矩陣T= ，則 ；

Step2：采用奇異值分解算法（SVD）計算旋轉矩陣和平移矩陣：

設d為點集D的質心，m為點集M的質心，將所有點的坐標減去質心坐標。則平移之后的點為 ， 。平移之后的總誤差為：

最小化E等價于最大化

其中， 。

對H進行奇異值分解計算， ，令 ，若X的行列式det（X）=1，表明 和 是2個相互正交的矩陣，得到R=X，平移向量 ；

Step3：將R與T應用到點集D，求出新點集 ，并用它代替原有D；

Step4：計算新點集D與M間的目標函數 ，若兩次迭代誤差小于給定閾值 （本文實驗中取值為 ），則迭代結束。

3 實驗仿真結果及分析

本文使用機器人Pioneer3在固定的室內環境中進行實驗。Pioneer3共配置了8個聲納傳感器，每10cm停下測量一次數據，其路線能覆蓋整個環境，這樣進行實驗檢測到了多個數據樣本。

為了驗證文中ICP-SVD配準算法融合拓撲地圖的可行性，利用MATLAB實驗仿真。在實際環境中，移動機器人運動方向多樣化，從不同角度探測環境，地圖融合時，旋轉角度 需考慮多種取值。

圖1和圖2所示為環境中待配準的原始地圖，配準過程中，將地圖MapA固定，視為“模板”，對地圖MapB進行平面變換。圖3所示為兩張地圖的最終配準結果，從圖中可以看出，配準融合后得到的全局地圖準確度大大提高。

圖4所示為相鄰兩次的迭代誤差差值變化曲線，橫坐標是迭代次數，縱坐標是相鄰兩次的迭代誤差差值。此實驗中，當迭代11次，相鄰兩次的迭代誤差值小于給定閾值0.1，此時迭代結束。

ICP-SVD配準算法融合拓撲地圖的仿真實驗結果表明，對于任意環境中的兩張地圖，配準融合后得到的地圖效果很好。而且，算法實現簡單，消耗時間短，很大程度提高了多移動機器人系統的工作效率，得到的環境地圖更精確。

利用我校的機器人實驗室條件，對本文中算法地圖融合的準確性進行了驗證（實驗中均采用兩個機器人工作）。

機器人實驗室中的實驗結果表明，本文中的算法用于實際環境中拓撲地圖融合準確度較高，減少了時間損耗，滿足實際要求。

4 結論

本文研究了多移動機器人的拓撲地圖融合，在機器人相對位置未知的情況下，對于兩個拓撲地圖，采用圖像配準方法中經典的ICP（Iterative Closest Point）算法結合奇異值分解算法（SVD），進行地圖融合。在MATLAB環境下，針對不同的環境進行了仿真，在實驗室中的實際環境中進行驗證實驗。結果表明，利用文中的ICP- SVD圖像配準算法進行拓撲地圖融合，能夠達到理想的效果，而且算法較穩定，實現簡單，消耗時間較短，很大程度上提高了多移動機器人系統的工作效率。

參考文獻：

[1]趙翊捷，陳衛東.基于地圖的移動機器人定位技術新進展[J].上海交通大學學報.Vol.36，No.10.2002，1435-1438.

[2]W.H.Huang，and K.R.Beevers.Topological map merging[C].The 7th Intl Symp on Distributed Autonomous Robotis Systems（DRAS 2004）.

[3]陳世歡.基于改進蟻群算法的改航路徑規劃[J].計算機技術與發展.2015（2）：52-54.

[4]易學淵.一種圖形處理用的多格式定點運算器[J].計算機技術與發展.2014（10）：147-150.

[5]G.Dedeoglu and G.S.Sukhatme.Landmark-based matching algorithm for cooperative mapping by autonomous robots[C].In L.E.Parker，G.W.Bekey，and J.Barhen，editors，Distributed Autonomous Robotic Systems 4，pages 251–260.Springer-Verlag，2000.

[6]晁衍凱，徐昱琳.基于雙目視覺的機器人目標定位與機械臂控制[J].計算機技術與發展[J].2013（7）：6-9.

[7]P.J.Besl and N.D.McKay.A method for registration of 3-D shapes[C].IEEE Transactions on Pattern Analysis and Machine Intelligence，14（2）：239–256，February 1992.

[8]王娜.移動機器人拓撲地圖創建研究[D]：[碩士學位論文].山東大學，2009.

[9]王娜.基于聲納的移動機器人地圖創建改進方法[J].科協論壇.2010，（6）：65-67.

基金項目：山東省高等學校科技計劃項目（J14LB61）