基于改進ICP算法的室內(nèi)環(huán)境三維地圖創(chuàng)建研究

張彥鐸， 袁 博*， 李 迅

(1.武漢工程大學(xué) 計算機科學(xué)與工程學(xué)院， 武漢 430025； 2.武漢工程大學(xué) 智能機器人湖北省重點實驗室， 武漢 430025)

基于改進ICP算法的室內(nèi)環(huán)境三維地圖創(chuàng)建研究

張彥鐸1，2， 袁 博1，2*， 李 迅1，2

(1.武漢工程大學(xué) 計算機科學(xué)與工程學(xué)院， 武漢 430025； 2.武漢工程大學(xué) 智能機器人湖北省重點實驗室， 武漢 430025)

提出一種基于離散選取機制的改進特征點ICP算法，并設(shè)計了基于該算法的三維地圖創(chuàng)建方法.該方法分為3個階段，首先提取并匹配相機運動過程中采集的RGB彩色圖像中的SURF特征點；然后結(jié)合RANSAC算法進行初始配準，優(yōu)化特征點集初始位姿、去除誤匹配，并結(jié)合基于離散選取機制的特征點ICP算法進行精確配準；最后利用g2o圖優(yōu)化算法結(jié)合關(guān)鍵幀實現(xiàn)對相機運動軌跡的優(yōu)化，減少累計誤差，并將相機采集到的點云數(shù)據(jù)根據(jù)相機當前位姿構(gòu)建三維點云地圖.經(jīng)過在5個公開數(shù)據(jù)集環(huán)境下進行實驗對比，證明本方法的可行性和有效性，在相機運動長度為15.989 m的情況下誤差僅為0.059 m，且能夠準確地創(chuàng)建實驗環(huán)境的三維地圖.

離散選取機制； 改進特征點ICP算法； RANSAC； g2o； 關(guān)鍵幀

同時定位與地圖創(chuàng)建(SimultaneousLocalizationAndMapping，SLAM)[1]是移動機器人的一項重要任務(wù).隨著傳感器技術(shù)的發(fā)展和進步，研究者們提出了基于各種傳感器的地圖創(chuàng)建方法，RGB-D相機由于其深度圖像提供的距離信息為創(chuàng)建稠密的三維地圖提供了便利，且三維地圖相對于二維地圖包含了更多有價值的形狀、環(huán)境信息.因此隨著以Kinect為代表的RGB-D相機的問世，基于RGB-D相機的三維地圖創(chuàng)建逐步成為近年來的研究熱點.

為了得到完整的三維地圖信息，關(guān)鍵問題是將相機從不同視角獲得的數(shù)據(jù)通過坐標變換配準到同一坐標系下，求解坐標變換參數(shù)的過程被稱為運動估計.針對這個問題，研究者們提出了各種解決方案，其中最常用的是1992年由Besl和McKay提出的迭代最近點(IterativeClosestPoint，ICP)算法[2].然而該算法雖然具有較為精確的配準效果，但仍存在較多不足：首先，ICP算法是一種點集對點集的配準方法，因此隨著空間點數(shù)量的增加，算法效率嚴重降低.其次，點集間的初始位姿對算法效果起重要作用，不合適的初始位姿將使ICP算法陷入局部最優(yōu)，嚴重影響配準效果.

研究者們針對ICP算法的特點進行了大量的研究，提出了各種改進算法.文獻[3]利用Kinect相機設(shè)計了一種結(jié)合GPU的實時三維重建和交互系統(tǒng)，針對ICP算法計算大規(guī)模數(shù)據(jù)時時間和空間復(fù)雜度高的缺點，利用GPU加速提高了算法效率，但該方法對硬件性能要求高，且由于內(nèi)存的限制無法完成大規(guī)模場景的重建.文獻[4]提出了一種利用Kinect相機進行3D地圖創(chuàng)建的RGBDSLAM系統(tǒng)，該系統(tǒng)利用RANSAC方法結(jié)合關(guān)鍵幀進行運動估計并利用非線性優(yōu)化方法優(yōu)化相機位姿，并使用多個不同場景的數(shù)據(jù)集進行實驗評估了系統(tǒng)的性能.文獻[5]提出了一種基于特征點的改進ICP方法，與傳統(tǒng)ICP算法不同，基于特征點的ICP算法使用彩色圖片中提取的特征點進行配準，相比于傳統(tǒng)ICP算法大大減少了計算量，提高了算法效率，但由于特征點之間誤匹配的存在，導(dǎo)致該方法存在配準精度低，無法收斂等缺點.文獻[6]針對基于特征點的ICP算法精度低，易配準失敗的問題，提出一種以RANSAC算法為策略，求解基于對應(yīng)特征點的ICP算法.該算法隨機選擇特征點進行配準，采用RANSAC算法策略計算ICP算法模型，提高了算法的準確性和魯棒性.但ICP算法模型中隨機選取的特征點可能出現(xiàn)過于緊密的情況，使得配準結(jié)果無法滿足全局的配準要求，從而導(dǎo)致算法陷入局部最優(yōu)造成配準效果不穩(wěn)定.

綜上所述，雖然文獻[6]提出的算法既提高了實時性又兼顧了準確性，但仍存在隨機選取特征點易使算法陷入局部最優(yōu)從而導(dǎo)致配準效果不穩(wěn)定的問題.

1本文工作

在某些復(fù)雜環(huán)境中，提取的特征點較為稠密，以RANSAC算法為策略，求解基于隨機特征點的ICP算法其特征點選取的隨機性，使得選取的特征點往往出現(xiàn)過于緊密的情況，在后續(xù)的求解幀間坐標變換參數(shù)的過程中，導(dǎo)致三維配準的結(jié)果陷入局部最優(yōu)從而影響配準精度.

本文提出的基于離散選取機制的特征點ICP算法，在選取特征點的過程中引入了特征點之間的三維空間歐氏距離作為閾值，既待選取的特征點和任意一個已選取的特征點之間的三維空間歐氏距離必須大于閾值，使選取的特征點離散化.有效地改進了其缺陷，進一步提高了算法的魯棒性.

1.1算法流程和結(jié)構(gòu)

本文首先提取彩色圖像上的SURF特征點；針對傳統(tǒng)ICP算法中配準點集初始位姿和誤匹配影響配準精度的問題，采用RANSAC算法對特征點集進行初始配準，優(yōu)化點集間的初始位姿并去除誤匹配；采用基于離散選取機制的特征點ICP算法進行精確配準，計算精確的幀間配準關(guān)系；最后利用g2o圖優(yōu)化算法結(jié)合關(guān)鍵幀實現(xiàn)對相機軌跡的優(yōu)化，并創(chuàng)建地圖.算法流程如圖1所示.

1.2相機模型和運動模型

(1)

從而進一步得到像素點從圖像坐標系UV投影到相機坐標系XYZ的坐標變換公式為：

(2)

1.2.2運動模型 通常用剛性變換矩陣T描述相機在不同位姿間的運動：

(3)

Pi+1=Ti+1，iPi.

(4)

假設(shè)相機初始位姿為P0，根據(jù)公式4可知，P0與i時刻相機位姿Pi的關(guān)系為：

(5)

1.3特征點提取與匹配

目前最常用的特征點提取方法包括FAST[8]、ORB[9]、SIFT[10]、SURF[11]等.

FAST算法和ORB算法特征點提取速度快，但特征點不具有尺度不變性，因此魯棒性較差.SIFT算法提取的特征點穩(wěn)定，但提取效率低.而SURF算法繼承了SIFT算法的穩(wěn)定性，又改進了其速度慢、特征點提取效率低的問題.因此，鑒于室內(nèi)環(huán)境相對復(fù)雜的特點，本文采用SURF算法進行特征點提取，在一定程度上既保證了算法的魯棒性又兼顧了實時性.實驗中特征點提取方法效果如表1所示，可以看出SURF算法同時兼顧了特征點的提取數(shù)量和算法的運行效率.

特征點匹配階段本文采用雙向相似最近鄰FLANN算法進行匹配，并根據(jù)對應(yīng)特征點之間的歐氏距離判斷其匹配是否成功.假設(shè)U1V1和U2V2分別代表兩幅彩色RGB圖像Irgb1和Irgb2中匹配的對應(yīng)特征點P1和P2在圖像坐標系UV的坐標，有兩點間的歐氏距離計算公式：

(6)

歐氏距離越小代表特征點的差異程度越低.首先計算特征點對之間的最小歐氏距離distmin，接下來計算每個特征點匹配對之間的歐氏距離dist，若dist≤α×distmin則認為該特征點匹配對匹配成功，本文實驗中參數(shù)α設(shè)為4.

1.4運動估計

1.4.1RANSAC算法初始配準 針對基于特征點的ICP算法中特征點集初始位姿和特征點之間的誤匹配影響配準精度的缺點.本文采用RANSAC算法進行初始配準，優(yōu)化初始位姿.RANSAC算法通過估計圖片上對應(yīng)特征點的單應(yīng)性重投影誤差，將匹配的特征點劃分為內(nèi)點和野點，消除了錯誤匹配.本文中將對應(yīng)特征點二維投影誤差閾值μ設(shè)定為3個像素，即特征點和匹配的特征點對應(yīng)的極線坐標誤差超過3個像素則認為是錯誤匹配.

由圖2可以看出經(jīng)RANSAC算法進行初始配準之后，有效的去除了特征點中的誤匹配.

(7)

本文為解決文獻[6]算法陷入局部最優(yōu)導(dǎo)致配準效果不穩(wěn)定的問題，在隨機選取特征點的基礎(chǔ)上引入了特征點之間的歐氏距離β×distmax作為閾值，對特征點進行離散選取.選取規(guī)則如下：

(8)

假設(shè)有兩幅RGB彩色圖片Is和It，算法步驟如下：

第1步：定義離散選取的特征點集Fs、Ft和配準點集Ps、Pt，點集為空.定義選取的特征點對數(shù)量N以及最大迭代次數(shù)count，并計算Is中特征點之間的最大歐氏距離distmax，本文中N設(shè)置為10，count設(shè)置為50；

第2步：隨機在圖像Is中選取一組歐氏距離dist>β×distmax的特征點放入Fs，并將其在圖像It中匹配的對應(yīng)特征點放入Ft；

第3步：隨機在圖像Is中選取一個特征點p，若該點到Fs中任意一點的歐氏距離dist均大于閾值β×distmax，則將該點及其在圖像It中匹配的對應(yīng)特征點分別放入Fs、Ft；

第4步：重復(fù)步驟3，直至選擇出N對特征點放入Fs、Ft，將點集Fs、Ft投影到以相機為原點的三維空間相機坐標系XYZ中，分別為配準點集Ps、Pt；

第5步：結(jié)合公式(7)，根據(jù)Ps、Pt計算剛性變換矩陣T，若公式(7)中的目標函數(shù)小于閾值λ則配準成功，退出精確配準算法.否則返回步驟2，清空點集Fs、Ft和Ps、Pt，并將迭代次數(shù)加1，本文中λ設(shè)置為0.05；

第6步：若算法迭代次數(shù)達到最大迭代次數(shù)，則配準失敗，退出精確配準算法，丟棄當前幀.

1.5優(yōu)化與地圖創(chuàng)建

ICP算法的思想是通過優(yōu)化兩個點集之間對應(yīng)點的歐氏距離計算剛性變換矩陣的最優(yōu)解，因此不可避免的的存在誤差，而在地圖創(chuàng)建過程中的誤差累計會嚴重影響地圖的準確性.因此本文采用g2o圖優(yōu)化算法減少累計誤差，實現(xiàn)對地圖的快速優(yōu)化.

在基于圖優(yōu)化的SLAM系統(tǒng)中，將相機的位姿看作結(jié)點，而不同結(jié)點之間的剛性變換矩陣看作是聯(lián)結(jié)對應(yīng)結(jié)點的邊.圖優(yōu)化過程首先需要利用關(guān)鍵幀構(gòu)造閉環(huán)，具體方法是對不相鄰的關(guān)鍵幀進行配準，即在對應(yīng)關(guān)鍵幀結(jié)點之間添加一條邊，從而形成閉環(huán)，然后使用g2o算法進行優(yōu)化.本文根據(jù)當前幀相對于上一幀關(guān)鍵幀之間的偏移量ΔP作為關(guān)鍵幀的選取標準，若偏移量ΔP大于0.3則將當前幀選取為關(guān)鍵幀.偏移量ΔP由兩幀之間的旋轉(zhuǎn)矩陣R和平移向量t的范數(shù)表示，計算方式如下：

(9)

2實驗設(shè)計與分析

2.1實驗設(shè)計

本文實驗部分在搭載Intel(R) Core i5-5250U 1.60GHz處理器、4GB內(nèi)存和Xubuntu15.04操作系統(tǒng)的計算機上運行.分別在Computer Vision Group[4，13]提供的5個不同數(shù)據(jù)集環(huán)境下進行實驗.數(shù)據(jù)集包含由Kinect傳感器采集的分辨率為640×480的彩色圖像和深度范圍在2至5米的深度圖像序列、相機的內(nèi)參以及由高精度運動捕捉系統(tǒng)獲取的相機真實運動軌跡，且提供了工具計算相機真實位姿和算法估計位姿之間的均方根誤差(RSME)，從而有效的評估算法的準確性.實驗首先從算法的準確性和實時性兩個方面對比本文算法、文獻[6]算法和RGBD SLAM算法；接下來通過對比不同算法構(gòu)建地圖的效果和相機運動軌跡進一步驗證本文算法的優(yōu)越性.實驗過程中具體參數(shù)和數(shù)據(jù)集詳細信息如表2和表3所示.

2.2實驗結(jié)果分析

2.2.1算法效果分析 為評估算法性能，本文在fr1/room、fr1/desk、fr1/desk2、fr1/xyz和fr2/xyz等5個公開數(shù)據(jù)集環(huán)境下進行實驗.其中fr1/room數(shù)據(jù)集環(huán)境最為復(fù)雜，且相機的運動軌跡形成了閉環(huán)，而fr1/xyz和fr2/xyz數(shù)據(jù)集環(huán)境則相對較為簡單，相機運動軌跡僅包含水平和垂直方向的運動.

本節(jié)從準確性和實時性兩個方面對比評估本文算法、文獻[6]算法和RGBD SLAM算法的性能，文獻[4]詳細記錄了RGBD SLAM算法在各數(shù)據(jù)集環(huán)境下的實驗數(shù)據(jù).表4中算法誤差為各算法運動估計計算的相機位姿和數(shù)據(jù)集提供的相機真實位姿之間的RMSE(均方根誤差).

在準確性方面，由表4可以看出，本文算法和文獻[6]算法運動估計計算的相機位姿誤差均小于RGBD SLAM算法，本文算法的誤差相對于RGBD SLAM算法和文獻[6]算法分別平均減少了57.7%和33.1%.尤其在fr1/room、fr1/desk和fr1/desk2三個相對復(fù)雜的數(shù)據(jù)集環(huán)境中本文算法相對于文獻[6]算法計算的誤差平均降低了48%，而在fr1/xyz和fr2/xyz數(shù)據(jù)集中誤差僅僅降低10.7%.這是由于相對復(fù)雜的環(huán)境中提取的特征點較稠密，文獻[6]方法隨機選取的特征點過于緊密導(dǎo)致算法陷入局部最優(yōu)從而使算法誤差增大，而本文方法提出的基于離散選取機制的特征點ICP算法有效的解決了這個問題，因此本文算法在復(fù)雜環(huán)境下依然表現(xiàn)出了較好的準確性.而在fr1/xyz和fr2/xyz等相對簡單的環(huán)境中，算法提取的特征點較稀疏，文獻[6]方法隨機選取的大部分特征點已經(jīng)較為分散，在一定程度上了掩蓋了其缺陷，因此文獻[6]算法的準確性在特征點較稀疏的環(huán)境中和本文算法差距不大，但仍然優(yōu)于RGBD SLAM算法.綜上所述，本文算法在準確性方面明顯優(yōu)于RGBD SLAM算法和文獻[6]算法，提出的基于離散選取機制的ICP算法有效的解決了文獻[6]算法中在特征點較稠密的復(fù)雜環(huán)境中易陷入局部最優(yōu)從而影響算法準確性的問題，通過實驗證明在各種環(huán)境下本文算法都表現(xiàn)出了良好的配準精度.

在實時性方面，由于RGBD SLAM算法在實驗中加入了多線程技術(shù)，其運算速度提高了2～2.5倍[4]，因此RGBD SLAM算法在實時性方面優(yōu)于文獻[6]算法和本文算法.本文算法在fr1/xyz以及fr2/xyz數(shù)據(jù)集環(huán)境下耗時分別達到了文獻[6]算法的130%和150%，而在其他數(shù)據(jù)集環(huán)境下實時性則略優(yōu)于文獻[6]算法.這是由于在特征點較稀疏的環(huán)境中，文獻[6]方法隨機選取的特征點本身較為分散，因此配準過程可以迅速得到收斂，本文算法則需要進行初始配準以及特征點離線選取的工作，影響了算法效率，因此在文獻[6]算法可以迅速收斂的簡單環(huán)境中，本文算法在配準速度上要慢于文獻[6]方法.而在特征點較稠密的復(fù)雜環(huán)境下，文獻[6]算法隨機選取的特征點過于緊密，導(dǎo)致配準結(jié)果無法收斂，雖然該方法在單次迭代過程中耗時小于本文算法，但由于配準過程中需要反復(fù)選取特征點進行迭代計算，整個精確配準過程中算法總耗時達到了本文方法的105%～110%.而本文算法選取出的離散特征點在復(fù)雜環(huán)境下依然可以迅速收斂，無須對特征點進行反復(fù)選取迭代，因此在復(fù)雜的環(huán)境中實時性優(yōu)于文獻[6]算法.兩種算法在特征點稀疏和稠密兩種不同環(huán)境中配準過程耗時具體分布如表5所示.

圖3為本文算法和文獻[6]方法在數(shù)據(jù)集fr1/room和fr2/xyz環(huán)境下實驗結(jié)果和相機真實位姿之間的平移誤差，x軸為每一幀圖片在數(shù)據(jù)集圖片序列中的時間戳，y軸為算法運動估計過程中計算的相機位姿和真實位姿之間的平移誤差，圖中藍線為本文算法誤差，紅線為文獻[6]算法誤差.有圖可以看出，本文算法相對文獻[6]算法誤差更穩(wěn)定，文獻[6]算法在fr1/room數(shù)據(jù)集環(huán)境中誤差波動較大，在某段時間會出現(xiàn)較大的誤差.這是由于在特征點較稠密的環(huán)境中算法我陷入局部最優(yōu)導(dǎo)致配準效果不穩(wěn)定造成的.而在fr2/xyz數(shù)據(jù)集環(huán)境中算法提取到的特征點較稀疏，因此文獻[6]算法誤差波動不明顯.

2.2.2地圖創(chuàng)建分析 相機位姿計算的越精確，那么根據(jù)由相機拍攝的圖片所創(chuàng)建的地圖效果就越精確.本節(jié)將進一步對比本文算法和文獻[6]算法在fr1/room數(shù)據(jù)集環(huán)境下的建圖效果和相機運動估計，論證本文算法的優(yōu)越性.

fr1/room數(shù)據(jù)集是由一臺手持的Kinect相機在封閉的室內(nèi)環(huán)境中錄制的，環(huán)境中包含了很多易導(dǎo)致配準錯誤的相似物體，環(huán)境非常復(fù)雜，且相機的運動軌跡形成了閉環(huán).因此該數(shù)據(jù)集對算法的可靠性有很高的要求.

圖4為本文算法和文獻[6]算法三維地圖的構(gòu)建效果，左邊為本文方法創(chuàng)建的三維地圖，右邊為文獻[6]方法創(chuàng)建的三維地圖.由圖4可以看出，本文算法獲得了一致性較好的地圖，有效的恢復(fù)了實驗場景，且輪廓清晰，建立了與數(shù)據(jù)集環(huán)境基本一致的三維點云地圖；而文獻[6]算法創(chuàng)建的地圖雖然也較完整的恢復(fù)了場景，但在某些細節(jié)出現(xiàn)了明顯的偏移.因此，本文算法相對于文獻[6]算法創(chuàng)建的地圖更有效的恢復(fù)了場景，精確度更高.

圖5為兩種算法在數(shù)據(jù)集fr1/room環(huán)境下運動估計計算的相機軌跡和相機真實軌跡的比較.圖中黑線為相機真實運動軌跡，藍線為算法估計軌跡，紅色部分為兩者之間的誤差.由圖5可見，本文算法運動估計計算的軌跡與真實軌跡更接近，算法結(jié)果更精確.

3結(jié)論

本文提出一種基于離散選取機制的特征點ICP算法進行運動估計的三維地圖創(chuàng)建方法.首先提取SURF特征點，在運動估計過程中采用RANSAC算法進行初始配準去除特征點中的誤匹配，優(yōu)化初始位姿，并提出了基于離散選取機制的特征點ICP算法進行精確配準計算相機運動軌跡，最后利用g2o圖優(yōu)化算法結(jié)合關(guān)鍵幀減少累計誤差，實現(xiàn)對相機運動軌跡的優(yōu)化.通過在5個數(shù)據(jù)集環(huán)境下完成了地圖的創(chuàng)建并評估算法性能.從準確性和實時性兩個方面對比了文本算法、文獻[6]算法和RGBD SLAM算法，驗證了本文算法在準確性方面優(yōu)于文獻[6]算法和RGBD SLAM算法，而在實時性方面，由于本文算法未使用多線程和GPU加速等方法，因此配準效率僅能達到2幀每秒，無法滿足實時建圖的需要.

未來的研究工作將加入詞袋模型(bag of words)進行閉環(huán)檢測.并加入多線程技術(shù)提高算法實時性，使之滿足實時建圖的需要.

[1] SMITH R C， CHEESEMAN P. On the representation and estimation of spatial uncertainty[J]. The International Journal of Robotics Research， 1986， 5(4): 56-68．

[2] BESL P J， MCKAY N D. A method for registration of 3-D shapes[J]//IEEE Transactions on Pattern Analysis and Machine Intelligence， 1992， 14(2): 239-256．

[3] IZADI S， KIM D， HILLIGES O， et al. Kinect fusion: real-time 3D reconstruction and interaction using a moving depth camera[C]//24th Annual ACM Symposium on User Interface Software and Technology， Santa Barbara: Association for Computing Machinery， 2011: 559-568．

[4] ENDRES F， HESS J， ENGELHARD N， et al. An evaluation of the RGB-D SLAM system[C]// IEEE International Conference on Robotics and Automation. 2012: 1691-1696．

[5] MITRA N J， GELFAND N， POTTMANN H， et al. Registration of point cloud data from a geometric optimization perspective[C]// Euro Graphics Symposium on Geometry Processing， 2004: 23-32．

[6] 賈松敏， 王 可， 李秀智，等. 基于RGB-D相機的移動機器人三維SLAM[J]. 華中科技大學(xué)學(xué)報(自然科學(xué)版)， 2014: 103-109．

[7] OUROSHK K ， SANDER OUDE E. Accuracy and resolution of kinect depth data for indoor mapping applications[J]. Sensors， 2012， 12(2): 1437-1454．

[8] ROSTEN E， DRUMMOND T. Machine learning for high-speed corner detection[J]. ECCV， 2006， 3951: 430-443．

[9] ETHAN R， VINCENT R， KURT K， et al. ORB: an efficient alternative to SIFT or SURF[J]. IEEE International Conference on Computer Vision， 2011， 58(11): 2564-2571．

[10] LOWE D， DAVID G. Object recognition from local scale-invariant features[J]. IEEE International Conference on Computer Vision， 2001， 2(3): 1150-1157.

[11] HERBERT B， TINNE T， ANDREAS E. SURF: speeded up robust features[J]. ECCV， 2006， 110(3): 404-417．

Reconstructing 3D map in indoor environment based on an improved ICP algorithm

ZHANG Yanduo1,2， YUAN Bo1,2， LI Xun1,2

(1.School of Computer Science and Engineering， Wuhan Institute of Technology， Wuhan 430025; 2.Hubei Key Laboratory of Intelligent robot, Wuhan Institute of Technology, Wuhan 430025)

In this paper， an improved Iterative Closest Point (ICP) algorithm is proposed based on features with the discrete selection mechanism for motion estimation to reconstruct 3D map in indoor environment. Firstly， SURF features in consecutive RGB images are extracted and matched. Then， Random Sample Consensus (RANSAC) algorithm is used for initial registration to optimize the initial pose of features and remove the outliers. Furthermore， secondary registration is applied to calculate the refined transformation between point-clouds in the different coordinate systems combining with the Iterative Closest Point algorithm which based on features with the discrete selection mechanism. Finally， the trajectory of the moving camera is optimized using General Gragh Optimization (g2o) framework combining with key frames， and 3D map is reconstructed through projecting 3D points cloud observed by the camera into global map according to current camera poses. The performance of our proposed algorithm in five public datasets is tested. The results demonstrate that the algorithm is feasible and effectively with the translational error just of 0.059m and ability to generate the 3D map of environment accurately．

discrete selection mechanism; improved ICP algorithm; RANSAC; g2o; key frames

2016-10-12.

國家863計劃項目(2013AA12A202);國家自然科學(xué)基金項目(41501505).

1000-1190(2017)02-0264-09

P283.49

A

*通訊聯(lián)系人. E-mail: 304033314@qq.com.