基于ICP—SVD的多移動(dòng)機(jī)器人拓?fù)涞貓D創(chuàng)建研究

王娜++王海艷++劉紀(jì)新++鄭義

摘要：移動(dòng)機(jī)器人要完成智能任務(wù)，前提是創(chuàng)建未知環(huán)境地圖，本文利用圖像配準(zhǔn)方法，采用ICP算法結(jié)合奇異值分解算法（SVD），建立了地圖融合的數(shù)學(xué)模型，解決了多移動(dòng)機(jī)器人拓?fù)涞貓D融合問題。實(shí)驗(yàn)結(jié)果表明，將ICP-SVD圖像配準(zhǔn)算法應(yīng)用于機(jī)器人拓?fù)涞貓D融合，算法簡(jiǎn)單，消耗時(shí)間短，且有效提高了地圖的匹配效果。

關(guān)鍵詞：多移動(dòng)機(jī)器人；拓?fù)涞貓D；圖像配準(zhǔn)；ICP-SVD

0 引言

多個(gè)移動(dòng)機(jī)器人位于同一空間，協(xié)同工作，可以提高探索環(huán)境的效率；融合多個(gè)機(jī)器人的傳感器數(shù)據(jù)，可以有效提高環(huán)境地圖的精確性。拓?fù)涞貓D重在描述環(huán)境的拓?fù)浣Y(jié)構(gòu)，占用存儲(chǔ)空間少，計(jì)算時(shí)間短，所以，拓?fù)涞貓D的相關(guān)計(jì)算效率較高。

現(xiàn)階段的研究工作中，Dedeoglu解決了融合基于路標(biāo)的地圖，利用兩個(gè)地圖間的某一頂點(diǎn)匹配，估計(jì)一種轉(zhuǎn)變方案，配準(zhǔn)其他的頂點(diǎn)，產(chǎn)生統(tǒng)一的全局地圖，但是，多機(jī)器人協(xié)作未得到一個(gè)完整的總體解決方案。Konolige采用一種決策框架理論方法，在框架范圍內(nèi)的局部地圖中，實(shí)現(xiàn)了機(jī)器人相對(duì)定位，證明基于特征點(diǎn)的融合方法具有更高效率，缺點(diǎn)是特征點(diǎn)的提取過程較復(fù)雜。

本文中采用的ICP（Iterative Closest Point）圖像配準(zhǔn)方法，即反復(fù)迭代尋找最近點(diǎn)，是Besl在1992年提出的。文中的拓?fù)涞貓D融合來自幾何信息，如路徑連通性，節(jié)點(diǎn)類型等。利用結(jié)構(gòu)的圖像匹配和幾何特征的圖像配準(zhǔn)，依據(jù)匹配范圍和誤差的大小，保留最佳匹配方案的假設(shè)，融合兩個(gè)地圖。

1 地圖融合的數(shù)學(xué)模型建立

首先對(duì)拓?fù)涞貓D進(jìn)行平面轉(zhuǎn)換，采用線性變換中的剛體變換：

將平面中的一點(diǎn)用齊次坐標(biāo)表示，向量 表示點(diǎn) ， 代表旋轉(zhuǎn)矩陣， 代表平移矩陣， 分別表示x方向平移，y方向平移和逆時(shí)針旋轉(zhuǎn)角度。

令 是三個(gè)實(shí)數(shù)，則點(diǎn)p關(guān)于 轉(zhuǎn)移關(guān)系為：

式（1）表示對(duì)點(diǎn) 進(jìn)行角度為 的旋轉(zhuǎn)變換，沿坐標(biāo)軸方向在坐標(biāo)平面內(nèi)進(jìn)行 的平移。

文中的拓?fù)涞貓D融合，采用的是兩兩匹配方法。若兩個(gè)局部地圖已知，目的是搜索某一平面轉(zhuǎn)換，此時(shí)對(duì)應(yīng)的匹配范圍應(yīng)為最大。

將這兩個(gè)地圖記為MapA和MapB，EA、EB表示地圖的兩個(gè)矩陣，矩陣中的元素表示路徑端點(diǎn)坐標(biāo)。設(shè)路徑數(shù)分別為n和m，則地圖MapA、MapB間的相似度關(guān)系如下：

式中， 表示兩個(gè)地圖匹配的程度， 表示地圖MapA中的路徑i， 表示地圖MapB中的路徑j(luò)。

2 ICP-SVD算法解決拓?fù)涞貓D融合

將ICP-SVD算法應(yīng)用于拓?fù)涞貓D融合，基本輸入是兩個(gè)地圖的路徑端點(diǎn)集合，搜索各個(gè)路徑端點(diǎn)在固定地圖上的最近點(diǎn)。

固定地圖的路徑端點(diǎn)集合記作M，坐標(biāo)為： ；另一進(jìn)行平面變換的地圖路徑端點(diǎn)集合記作D，坐標(biāo)為： 。當(dāng)計(jì)算到第k次迭代時(shí)， ，計(jì)算M與 間的變換矩陣，更新原變換，到兩組數(shù)據(jù)間的距離最小為止，即經(jīng)過迭代使如下優(yōu)化函數(shù)最小。

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

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

Step2：采用奇異值分解算法（SVD）計(jì)算旋轉(zhuǎn)矩陣和平移矩陣：

設(shè)d為點(diǎn)集D的質(zhì)心，m為點(diǎn)集M的質(zhì)心，將所有點(diǎn)的坐標(biāo)減去質(zhì)心坐標(biāo)。則平移之后的點(diǎn)為 ， 。平移之后的總誤差為：

最小化E等價(jià)于最大化

其中， 。

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

Step3：將R與T應(yīng)用到點(diǎn)集D，求出新點(diǎn)集 ，并用它代替原有D；

Step4：計(jì)算新點(diǎn)集D與M間的目標(biāo)函數(shù) ，若兩次迭代誤差小于給定閾值 （本文實(shí)驗(yàn)中取值為 ），則迭代結(jié)束。

3 實(shí)驗(yàn)仿真結(jié)果及分析

本文使用機(jī)器人Pioneer3在固定的室內(nèi)環(huán)境中進(jìn)行實(shí)驗(yàn)。Pioneer3共配置了8個(gè)聲納傳感器，每10cm停下測(cè)量一次數(shù)據(jù)，其路線能覆蓋整個(gè)環(huán)境，這樣進(jìn)行實(shí)驗(yàn)檢測(cè)到了多個(gè)數(shù)據(jù)樣本。

為了驗(yàn)證文中ICP-SVD配準(zhǔn)算法融合拓?fù)涞貓D的可行性，利用MATLAB實(shí)驗(yàn)仿真。在實(shí)際環(huán)境中，移動(dòng)機(jī)器人運(yùn)動(dòng)方向多樣化，從不同角度探測(cè)環(huán)境，地圖融合時(shí)，旋轉(zhuǎn)角度 需考慮多種取值。

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

圖4所示為相鄰兩次的迭代誤差差值變化曲線，橫坐標(biāo)是迭代次數(shù)，縱坐標(biāo)是相鄰兩次的迭代誤差差值。此實(shí)驗(yàn)中，當(dāng)?shù)?1次，相鄰兩次的迭代誤差值小于給定閾值0.1，此時(shí)迭代結(jié)束。

ICP-SVD配準(zhǔn)算法融合拓?fù)涞貓D的仿真實(shí)驗(yàn)結(jié)果表明，對(duì)于任意環(huán)境中的兩張地圖，配準(zhǔn)融合后得到的地圖效果很好。而且，算法實(shí)現(xiàn)簡(jiǎn)單，消耗時(shí)間短，很大程度提高了多移動(dòng)機(jī)器人系統(tǒng)的工作效率，得到的環(huán)境地圖更精確。

利用我校的機(jī)器人實(shí)驗(yàn)室條件，對(duì)本文中算法地圖融合的準(zhǔn)確性進(jìn)行了驗(yàn)證（實(shí)驗(yàn)中均采用兩個(gè)機(jī)器人工作）。

機(jī)器人實(shí)驗(yàn)室中的實(shí)驗(yàn)結(jié)果表明，本文中的算法用于實(shí)際環(huán)境中拓?fù)涞貓D融合準(zhǔn)確度較高，減少了時(shí)間損耗，滿足實(shí)際要求。

4 結(jié)論

本文研究了多移動(dòng)機(jī)器人的拓?fù)涞貓D融合，在機(jī)器人相對(duì)位置未知的情況下，對(duì)于兩個(gè)拓?fù)涞貓D，采用圖像配準(zhǔn)方法中經(jīng)典的ICP（Iterative Closest Point）算法結(jié)合奇異值分解算法（SVD），進(jìn)行地圖融合。在MATLAB環(huán)境下，針對(duì)不同的環(huán)境進(jìn)行了仿真，在實(shí)驗(yàn)室中的實(shí)際環(huán)境中進(jìn)行驗(yàn)證實(shí)驗(yàn)。結(jié)果表明，利用文中的ICP- SVD圖像配準(zhǔn)算法進(jìn)行拓?fù)涞貓D融合，能夠達(dá)到理想的效果，而且算法較穩(wěn)定，實(shí)現(xiàn)簡(jiǎn)單，消耗時(shí)間較短，很大程度上提高了多移動(dòng)機(jī)器人系統(tǒng)的工作效率。

參考文獻(xiàn)：

[1]趙翊捷，陳衛(wèi)東.基于地圖的移動(dòng)機(jī)器人定位技術(shù)新進(jìn)展[J].上海交通大學(xué)學(xué)報(bào).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ìn)蟻群算法的改航路徑規(guī)劃[J].計(jì)算機(jī)技術(shù)與發(fā)展.2015（2）：52-54.

[4]易學(xué)淵.一種圖形處理用的多格式定點(diǎn)運(yùn)算器[J].計(jì)算機(jī)技術(shù)與發(fā)展.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ī)器人目標(biāo)定位與機(jī)械臂控制[J].計(jì)算機(jī)技術(shù)與發(fā)展[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，F(xiàn)ebruary 1992.

[8]王娜.移動(dòng)機(jī)器人拓?fù)涞貓D創(chuàng)建研究[D]：[碩士學(xué)位論文].山東大學(xué)，2009.

[9]王娜.基于聲納的移動(dòng)機(jī)器人地圖創(chuàng)建改進(jìn)方法[J].科協(xié)論壇.2010，（6）：65-67.

基金項(xiàng)目：山東省高等學(xué)校科技計(jì)劃項(xiàng)目（J14LB61）