粗匹配和局部尺度壓縮搜索下的快速ICP-SLAM

張金藝，梁濱，唐笛愷，姚維強，鮑深

(1．上海大學 通信與信息工程學院，上海 200010；2. 上海大學 微電子研究與開發中心，上海 200010)

粗匹配和局部尺度壓縮搜索下的快速ICP-SLAM

張金藝1,2，梁濱1，唐笛愷1，姚維強2，鮑深2

(1．上海大學 通信與信息工程學院，上海 200010；2. 上海大學 微電子研究與開發中心，上海 200010)

ICP-SLAM在自主機器人和無人駕駛領域得到了極大的關注，但傳統ICP-SLAM缺少當前幀和全局地圖的相對位置關系，因此本文ICP算法必須經過大量的迭代之后才能達到收斂條件，這導致傳統ICP-SLAM實時性很差。并且在每一次的迭代過程中，必須通過全局搜索才能完成匹配點搜索，這進一步降低了傳統ICP-SLAM的實時性。為此，提出了一種快速ICP-SLAM方案。首先，通過MEMS磁力計和全局地標計算出初始位姿矩陣，通過該初始位姿矩陣實現當前幀和全局地圖之間粗匹配，進而減少達到收斂條件的迭代次數。其次，在每次迭代過程中，將采用局部尺度壓縮搜索完成匹配點搜索，從而減小ICP-SLAM的計算開銷，提高ICP-SLAM實時性；同時，每次迭代完成之后，還將通過動態閾值縮小搜索范圍，達到加快匹配點搜索的速度，進而提高ICP-SLAM實時性。實驗結果表明，和傳統ICP-SLAM相比，在理想室內靜止場景下，快速ICP-SLAM的迭代次數最高減小了92.34%，ICP算法運行時間最高降低了98.86%。除此之外，ICP-SLAM的整體負載也被保持在可控范圍內，ICP-SLAM的整體性能得到很大的提升。

ICP-SLAM；粗匹配；初始姿態矩陣；局部搜索；動態閾值；實時性；點云；迭代；

中文引用格式：張金藝，梁濱，唐笛愷，等. 粗匹配和局部尺度壓縮搜索下的快速ICP-SLAM[J]. 智能系統學報， 2017, 12(3): 413-421.

英文引用格式：ZHANG Jinyi, LIANG bin，TANG Dikai，et al. Fast ICP-SLAM with rough alignment and local scale-compressed searching[J]. CAAI transactions on intelligent systems, 2017, 12(3): 413-421.

自主機器人和無人駕駛車成為近幾年來人工智能領域研究的新熱點，該技術在服務業、交通運輸、工業、環境勘探、國防以及生活各個方面都有著廣闊的應用前景。同時德國工業4.0及中國制造2025也把焦點聚集在無人化和智能化，智能產業正在迎來前所未有的發展機遇，將催生龐大的市場。SLAM(同時定位和地圖創建，simultaneous localization and mapping,)是自主機器人和無人駕駛車領域的關鍵技術。當前SLAM的實現方案可以分為基于概率的方案[1-3]和基于非概率的方案[4-5]。ICP(最近鄰點迭代，iterative closest point)-SLAM作為基于非概率的方案之一，由于其具有原理簡單、成本較低等優點而得到了廣泛關注[6-8]。并且相比于傳統的導航技術，如藍牙定位、慣性導航系統等[9-11]，ICP-SLAM不僅能實現定位，還能實現地圖創建。但隨著自主機器人和無人駕駛車的速度越來越快，所處環境變得越來越復雜，這對SLAM實時性提出了更高要求。由于傳統ICP-SLAM實時性差，同時建模精度和魯棒性也不高，顯然不能再滿足這方面的要求。造成傳統ICP-SLAM實時性差的原因：一方面，因為傳統ICP算法不能提供初始姿態矩陣進行粗匹配，從而導致達到收斂條件的迭代次數大量增加；另一方面，由于缺少粗匹配，必須通過全局搜索才能完成匹配點搜索，這也大大增加了計算開銷，降低了傳統ICP-SLAM實時性。因此，為了提高ICP-SLAM的實時性，必須對傳統ICP-SLAM進行優化。

當前，對傳統ICP-SLAM的優化大都是選取ICP算法的某個步驟進行優化。ICP算法最早由Besl和Mckay為解決3-D物體對準問題而提出的[12]。根據Rusinkiewicz和Levoy的理論[13]，ICP算法可以分成下面6個步驟：選擇控制點，匹配點搜索，計算匹配點權重，設定一個匹配點誤差方程，通過最小化匹配點誤差方程求出旋轉矩陣和平移矩陣，同時定位和地圖創建。當前ICP算法的研究主要集中在優化匹配點搜索策略上。主流的匹配點搜索策略可以大致分為下面幾種。基于特征的匹配點搜索方法[14-16]包括如基于直線特征、基于曲率特征和基于斜率特征等,該方法在提取幾何特征時增加了額外的計算開銷，降低了ICP-SLAM實時性。并且幾何特征的提取容易受測量噪聲和移動物體的影響，降低了ICP-SLAM的魯棒性。另外,基于幾何劃分的匹配點搜索也被廣泛引用，如Delaunay劃分[17]和K-D樹劃分[18]。幾何劃分可以提高匹配點搜索的質量和效率。但當相鄰兩幀的重疊區域很小時，該方法則不再適用。除此之外，考慮到激光雷達作為ICP-SLAM的主要傳感器，所以為了能更好地利用激光雷達特殊的數據結構，提出了基于Polar-Cartesian Hybrid Transforms的Polar Point Matching Rule方法[19]。該方法通過在極坐標系下，尋找相近的旋轉角度，實現了快速的匹配點搜索。但該方法并不能提供初始姿態矩陣實現粗匹配，所以仍然采用全局搜索實現匹配點搜索。綜上，可以看出大多數ICP算法都采用了全局搜索。但隨著全局地圖的擴大，全局搜索會導致計算消耗量的累計效應。雖然局部ICP-SLAM[20-21]某種程度上可以解決這個問題，但是局部ICP-SLAM首先把全局地圖劃分成多個局部地圖，然后對每個局部地圖進行局部ICP-SLAM。所以，局部ICP-SLAM的本質上還是采用了全局搜索。并且局部ICP-SLAM還要保存各個局部地圖的相對位置，增加了額外內存開銷。

綜上所述，為了有效提高ICP-SLAM的實時性，本文提出基于粗匹配和局部尺度壓縮搜索的快速ICP-SLAM。該快速ICP-SLAM在激光雷達的基礎上增加了MEMS磁力計，這個MEMS磁力計能直接輸出當前航向角。并且引入全局地標，通過激光雷達掃描并測量全局地標，計算出機器人當前的位置信息。最后由航向角和機器人當前的位置信息共同構成完整的初值姿態矩陣。當得到了初值姿態矩陣后，首先通過該初值姿態矩陣實現當前幀和全局地圖的粗匹配。其次，在ICP算法的每次迭代中，采用局部尺度壓縮搜索完成匹配點搜索，從而避免了由全局搜索帶來的巨大計算開銷。同時，每次迭代完成之后，匹配點的搜索范圍通過動態閾值被縮小，加快匹配點搜索的速度，這將進一步提高ICP-SLAM實時性。實驗結果顯示，本文提出的快速ICP-SLAM相比于傳統ICP-SLAM，迭代次數減少了92.34%，ICP算法運行時間減少了98.86%。同時，ICP-SLAM的系統負載也被控制在穩定狀態，其整體性能得到較大的改善。

1 基于初始位姿矩陣的粗匹配

從上述分析可以看出，粗匹配是實現快速ICP-SLAM的首要步驟。粗匹配的過程為：通過旋轉和平移變換，使得兩個形狀類似但處于不同空間位置的物體大致重合。這是因為，當形狀相同的兩個物體處于二維平面的不同位置時，必然能找到一個平移矩陣和旋轉角使得這兩個物體完全重合。而機器人的初始位姿矩陣就包含了這樣一個平移矩陣和旋轉角。其中，機器人當前的坐標(xt,yt)可以看作平移矩陣，機器人的航向角θt可以看作旋轉角。那么機器人t時刻的姿態矩陣可以表示成一個1×3矩陣Poset(xt,yt,θt)。Poset(xt,yt,θt)可以通過MEMS磁力計和全局地標計算得到。這樣在ICP算法開始前，可以通過矩陣Poset(xt,yt,θt)完成當前幀和全局地圖的粗匹配。全局地標在全局坐標系中的坐標為已知信息，設圖1中的第n個全局地標在全局坐標系中的坐標為PLn-G(xLn-G,yLn-G)。同時設第n個全局地標在機器人局部坐標系的坐標為PLn-L(xLn-L,yLn-L)，則(xt,yt)可以通過式(1)計算得到：

圖1 計算姿態矩陣Fig.1 Initial pose matrix

當機器人移動到圖2中的(xt,yt)位置時，設其姿態矩陣為Poset(xt,yt,θt)。激光雷達獲取當前掃描幀并記為Ft′(圖2中的灰色圓部分)，并測出第n個全局地標在機器人局部坐標系下的坐標PLn-L(xLn-L,yLn-L)。假設激光雷達的角度分辨率為ρ，則Ft′中的點數N可通過式(2)得到

式中φ為激光雷達的可視角(例如360°)。根據(1)式可計算出(xt,yt)，如式(3)所示：

式中T如公式(4)所示：

因為全局地標為全局的靜態參照物，所以通過該方法計算得到的Poset(xt,yt,θt)不存在累計誤差。

同樣在圖2中記全局地圖為Mglobal(圖2中的點線)，并記Ft′和Mglobal粗匹配的結果為Ft(圖2中的虛線)，結合式(5)，則Ft可以通過式(6)得到，其中Ft′和Ft都是一個N×2矩陣，如式(7)和式(8)所示：

圖2 粗匹配示意圖Fig.2 Rough align

2 基于動態閾值的局部尺度壓縮搜索

粗匹配完成之后，當前幀中的點和其匹配點的距離被大大縮小，所以本文中的匹配點搜索將通過局部搜索完成。同時，每次迭代完成之后，匹配點之間的距離都比上一次迭代時更小，此時便可以通過動態閾值達到尺度壓縮，從而縮小匹配點搜索的范圍。局部尺度壓縮搜索不僅能加快匹配點搜索速度，還能提高匹配點搜索質量。同時在本文中，因為SLAM創建的地圖為柵格地圖，所以本小節將先闡述本文中采用的柵格地圖的坐標表征方式，接著再詳細剖析局部搜索和尺度壓縮。

2.1 柵格地圖的坐標表征

SLAM創建柵格地圖時，柵格地圖中的某個單元格只能處于兩種狀態中的一種：被物體占據或沒有被物體占據。當單元格被物體占據時用深色表示，單元格沒有被物體占據時用淺色表示。在柵格地圖中，水平面被分割為一個包含m×n個正方形單元格平面，記正方形單元格邊長為L，并設柵格地圖能表示的范圍為x^min,x^max,y^min,y^max，同時把正方形單元格的幾何中心作為該正方形單元格的坐標。在圖3中，陰影正方形單元格的坐標(xblack,yblack)可以表示為

同時可以把該陰影正方形單元格在柵格地圖中的標號(xi-black,yi-black)表示為

圖3 柵格地圖示意圖Fig.3 Grid map

當有更多的點落入到某一個正方形單元格時，該正方形單元格的顏色會加深。

2.2 基于Point-to-Region的局部搜索

局部搜索只有當完成粗匹配后才有意義。因為經過粗匹配后，當前幀中的某一點的匹配點只能出現在距離該點某一距離的范圍內。所以為了能采用局部搜索，必須在匹配點搜索前，將Ft′通過Poset(xt,yt,θt)和(5)式投影到Mglobal(記為Ft)，然后再在Mglobal和Ft之間通過局部搜索完成匹配點搜索。以圖4中的a點和b點為例，通過式(9)計算出a點和b點所在正方形單元的標號，記為(cxa,cya)和(cxb,cyb)。局部搜索的搜索范圍可以表示為

其中SearchingRange的值是自己設定的,在圖4中SearchingRange的值為1。在確定搜索范圍后，下一步便計算出點a/b與所有落在搜索范圍內的點的歐式距離di。當di滿足：

圖4 擴建柵格地圖Fig.4 Grid map expansion

2.3 基于動態閾值的尺度壓縮

ICP算法的最終目的是為了得到一個平移矩陣和旋轉角。所以當完成匹配點搜索后，下一步便是通過最小化匹配點誤差方程得到平移矩陣和旋轉角。但在得到最終的平移矩陣和旋轉角之前，當ICP算法完成一次迭代之后，當前幀和全局地圖的匹配能更加準確。這意味著，Ft中的點與其匹配點之間的距離縮短。所以ICP算法在下一次的匹配點搜索中，搜索范圍可以縮小。這不但可以減小匹配點搜索的計算量，還能減少誤匹配。完整的算法流程如圖5所示。

圖5 算法總流程圖Fig.5 Algorithm flow chart

本文將選擇匹配點之間的歐式距離作為匹配點誤差方程，匹配點的歐式距離如式(16)：

匹配點的權重通過式(17)計算：

式中E為動態閾值。匹配點的數量為

當前幀與全局地圖的重合度可以表示為

綜上，最終的誤差方程可以表示為

完成一次迭代之后，E通過下式被縮小：

3 實驗結果與分析

為了有效驗證粗匹配和局部尺度壓縮搜索對ICP-SLAM實時性的改善，本文搭建了一輛可定位的小車作為驗證平臺。該小車搭載了一個RobotPeak激光雷達、一個Uranus MEMS磁力計和一塊Raspberry Pi主板，如圖6所示，各傳感器規格見表1。

圖6 實驗平臺Fig.6 Experiment platform

傳感器測量范圍采樣頻率/Hz距離分辨率/cm角度分辨率/(°)FOV(FieldofView)/(°)RobotPeak激光雷達6m20000.21360UranusMEMS磁力計±4800μT100—0.01°—

本小節將從兩方面驗證快速ICP-SLAM。第一方面是快速ICP-SLAM實時性改善驗證。首先，對比未進行粗匹配和進行粗匹配下ICP-SLAM的迭代次數和ICP算法運行時間，驗證粗匹配對ICP-SLAM實時性的改善；其次，記錄Fdec的值從1到0.3變化過程中ICP-SLAM的迭代次數和ICP算法運行時間，觀察其變化趨勢，進而驗證局部尺度壓縮搜索對ICP-SLAM實時性的改善。第二方面是快速ICP-SLAM整體性改善驗證。其不僅可以驗證同時采用粗匹配和局部尺度壓縮搜索時，ICP-SLAM實時性的改善情況，同時還可以驗證ICP-SLAM在建模精度、魯棒性和計算開銷等方面的提升。

3.1 快速ICP-SLAM實時性改善驗證

通過第1節和第2節的分析可知，ICP-SLAM實時性受初始姿態矩陣、局部搜素和參數Fdec影響。本小節將采用復合開環航跡和復合閉環航跡，驗證粗匹配和局部尺度壓縮搜索對ICP-SLAM實時性的改善。復合開環航跡和復合閉環航跡分別如圖7中的(a)和(b)所示。

圖7 復合開環航跡和復合閉環航跡Fig.7 Mix-open track and mix-close track

粗匹配對迭代次數和ICP算法運行時間的改善如表2和表3所示。

從表2和表3中可以看出，在每一組對比實驗中，當進行粗匹配后，迭代次數和ICP算法運行時間都被大大減小。所以粗匹配改善了ICP-SLAM實時性。局部尺度搜索對迭代次數和ICP算法運行時間的提升結果如圖8和圖9所示,從圖中可以看出當Fdec從1漸變到0.3 時，大大減少迭代次數，縮短了ICP算法的運行時間。

表2 小車在復合開環航跡下的實驗結果

表3 小車在復合閉環航跡下的實驗結果

圖8 Fdec在復合開環航跡下的實驗結果Fig.8 Result of Fdec in mix-open track

圖9 Fdec在復合閉環航跡下的實驗結果Fig.9 Result of Fdec in mix-close track

除此之外，采用全局搜索時，ICP-SLAM的計算開銷會隨著全局地圖的增大而增大。但當采用局部尺度壓縮搜索時，ICP-SLAM的計算開銷被控制在穩定狀態，如圖10所示。

圖10 ICP-SLAM的計算開銷對比Fig.10 Comparison of computation in ICP-SLAM

從圖10中可以看出，當采用局部尺度壓縮搜索時，隨著ICP算法迭代次數的增加，ICP算法的運行時間被保持在一個穩定值，這表明ICP-SLAM的計算開銷被控制在穩定狀態。但采用全局搜索時，ICP-SLAM的計算開銷卻隨著ICP算法迭代次數的增加而不斷增加。所以局部尺度壓縮搜索對提高ICP-SLAM實時性有著顯著的效果。對比圖10和圖11可知，本文提出的ICP-SLAM比文獻[20]的局部SLAM在計算負載的穩定性上更有優勢。

圖11 文獻[20]中的SLAM波動過程Fig.11 SLAM unstable process in [20]

3.2 快速ICP-SLAM整體性改善驗證

在該驗證環節中，實驗平臺首先以低速走過一段弧度較小曲線，用來驗證Pt(xt,yt,θt)的作用。然后快速走完一段直線，用來驗證(xt,yt)的作用。接著作一個急速轉彎，此時θt會產生巨大的變化。最后一段小車慢速走過一段短直線和慢速轉彎。圖12為對比結果。在圖12(a)的方法中，ICP算法開始時會采用粗匹配，并且通過局部尺度壓縮搜索來完成匹配點搜索(Fdec=0.5)。圖12(b)中的對照組為傳統的ICP-SLAM算法。完成整個SLAM過程中，圖12(a)的方法總共進行了28 200次迭代，ICP算法運行時間為4 414 ms。圖12(b)中的方法進行了399 109次迭代，ICP算法運行時間為301 152 ms。可見,圖12(a)的方法比圖12(b)的方法減少了92.34%的迭代次數和98.86%的運行時間。此外，在圖12(a)方法的結果中，累積誤差被大大消除了，創建的地圖精度也比圖12(b)的好，并且沒有出現匹配ICP算法失鎖。綜上，圖12(a)的方法不僅提高了ICP-SLAM實時性，而且ICP-SLAM的整體性能得到很大的提升。

圖12 快速ICP-SLAM整體性改善驗證結果Fig.12 Overall improvement in fast ICP-SLAM

4 結束語

針對傳統ICP-SLAM實時性差，本文提出了粗匹配和局部尺度壓縮搜索。在進行ICP算法開始之前，通過MEMS磁力計和全局地標計算出機器人當前位姿矩陣，并基于該位姿矩陣完成當前幀和全局地圖的粗匹配，從而減少ICP算法的迭代次數。同時在ICP算法每次迭代中，采用局部尺度壓縮搜索替代全局搜索完成匹配點搜索，加快匹配點搜索速度。實驗結果表明ICP-SLAM實時性得到了很大提升，迭代次數和系統運行時間分別降低92.34%和98.86%。此外，ICP-SLAM的整體性能得到很大的提升。

[1] LI Hai, CHEN Qijun. Towards a non-probabilistic approach to hybrid geometry-topological SLAM[C]//Proceedings of 2010 8th World Congress of IEEE on Intelligent Control and Automation. Jinan: IEEE, 2010: 1045-1050.

[2]BARRAU A, BONNABEL S. Invariant filtering for Pose EKF-SLAM aided by an IMU[C]//Proceedings of 2015 IEEE Conference on Decision and Control. Osaka: IEEE, 2015: 2133-2138.

[3]季曉玲, 賀青, 遲宗濤. 基于EKF的SLAM算法在機器人定位中的應用[J]. 科技經濟導刊, 2016(13): 17-19.

[4]ZANDARA S, RIDAO P, RIBAS D, et al. Probabilistic surface matching for bathymetry based SLAM[C]//Proceedings of 2013 IEEE International Conference on Robotics and Automation. Karlsruhe: IEEE, 2013: 40-45.

[5]ALBERT P, RIDAO P, RIBAS D, et al. Bathymetry-based SLAM with difference of normals point-cloud subsampling and probabilistic ICP registration[C]//Proceedings of 2013 MTS/IEEE OCEANS-Bergen. Bergen: IEEE, 2013: 1-8.

[6]TREHARD G, ALSAYED Z, POLLARD E, et al. Credibilist simultaneous Localization And Mapping with a LIDAR[C]//Proceedings of 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. Chicago, IL: IEEE, 2014: 2699-2706.

[7]ARTH C, PIRCHHEIM C, VENTURA J, et al. Instant outdoor localization and SLAM initialization from 2.5D maps[J]. IEEE transactions on visualization and computer graphics, 2015, 21(11): 1309-1318.

[8]CHOUDHARY S, INDELMAN V, CHRISTENSEN H I, et al. Information-based reduced landmark SLAM[C]//Proceedings of 2015 IEEE International Conference on Robotics and Automation (ICRA). Seattle, WA: IEEE, 2015: 4620-4627.

[9]陳興秀, 張金藝, 晏理, 等. 三維復雜運動模式航跡推算慣性導航室內定位[J]. 應用科學學報, 2014, 32(4): 349-350. CHEN Xingxiu , ZHANG Jinyi , YAN Li , et al. Inertial indoor navigation with 3D complex motion mode of pedestrian dead reckoning[J].Journal of appliend sciences-electronics and information engineering, 2014, 32(4): 349-350.

[10]張蒼松, 郭軍, 崔嬌, 等. 基于RSSI的室內定位算法優化技術[J]. 計算機工程與應用, 2015, 51(3): 235-238. ZHANG Cangsong, GUO Jun, CUI Jiao, et al. Indoor positioning optimization techniques based on RSSI[J]. Computer engineering and applications, 2015, 51(3)：235-238.

[11]王益健. 藍牙室內定位關鍵技術的研究與實現[D]. 南京: 東南大學, 2015. WANG Yijian.Research and implementation on key technologies of bluetooth indoor positioning[D].Nanjing:Southeast University,2015.

[12]BESL P J, MCKAY N D. Method for registration of 3-D shapes[J]. IEEE transactions on pattern analysis and machine intelligence, 1992, 14(2): 239-256.

[13]RUSINKIEWICZ S, LEVOY M. Efficient variants of the ICP algorithm[C]//Proceedings of the Third International Conference on 3-D Digital Imaging and Modeling. Quebec City, Que: IEEE, 2001: 145.

[14]BLANCO J L, GONZáLEZ-JIMéNEZ J, FERNáNDEZ-MADRIGAL J A. A robust, multi-hypothesis approach to matching occupancy grid maps[J]. Robotica, 2013, 31(5): 687-701.

[15]XU Haixia, ZHOU Wei, ZHU Jiang. 3D visual SLAM with a time-of-flight camera[C]//Proceedings of 2015 IEEE Workshop on Signal Processing Systems (SiPS). Hangzhou: IEEE, 2015: 1-6.

[16]ULAS C, TEMELTAS H. A robust feature extraction method and semantic data association for 6D SLAM[C]//Proceedings of 2015 IEEE World Automation Congress (WAC). Mexico: IEEE, 2012: 1-6.

[17]GONG Zizhen, HUA Xianghong, YI Chongzheng, et al. The research and implementation of ICP based on Delaunay triangulation[J]. Engineering of surveying and mapping, 2010, 19(5): 29-31.

[18]HU Linjia, NOOSHABADI S, AHMADI M. Massively parallel KD-tree construction and nearest neighbor search algorithms[C]//Proceedings of 2015 IEEE International Symposium on Circuits and Systems (ISCAS). Lisbon: IEEE, 2015: 2752-2755.

[19]ZHANG Lei, CHOI S I, PARK S Y. Polar-Cartesian hybrid transforms: a novel 2D range scan registration algorithm[J]. International journal of control automation and systems, 2013, 11(5): 1001-1008.

[20]TIAR R, LAKROUF M, AZOUAOUI O. FAST ICP-SLAM for a bi-steerable mobile robot in large environments[C]//Proceedings of 2015 International Conference on Advanced Robotics (ICAR). Istanbul: IEEE, 2015: 1-6.

[21]TIAR R, OUADAH N, AZOUAOUI O, et al. ICP-SLAM methods implementation on a bi-steerable mobile robot[C]//Proceedings of IEEE 11th International Workshop of Electronics, Control, Measurement, Signals and their Application to Mechatronics (ECMSM). Toulouse: IEEE, 2013: 1-6.

Fast ICP-SLAM with rough alignment and local scale-compressed searching

ZHANG Jinyi1,2, LIANG Bin1，TANG Dikai1，YAO Weiqiang2，BAO Shen2

(1.School of Communication and Information Engineering, Shanghai University, Shanghai 200010, China； 2. Microelectronic Research and Development Center, Shanghai University, Shanghai 200010, China)

ICP-SLAM has

much attention in the field of autonomous robots and unmanned cars. However, two deficiencies in traditional ICP-SLAM usually result in poor real-time performance. The first is the fact that the relative position between the current scan frame and the global map is not previously known. As a result, the ICP algorithm takes a large number of iterations to reach convergence. The second is that the establishment of correspondence is carried out by global searching and this requires an enormous amount of computational time. To overcome these problems, a fast ICP-SLAM is proposed. To decrease the number of iterations a rough alignment, based on an initial pose matrix, is proposed. In detail, the initial pose matrix is computed using a MEMS magnetometer and global landmarks. Then, a rough alignment is applied between the current scan frame and the global map at the beginning of the ICP algorithm with an initial pose matrix. To accelerate the establishment of correspondence, local scale-compressed searching with a dynamic threshold is proposed where match-points are found within a progressively constrictive range.Compared to traditional ICP-SLAM, under ideal stable conditions, the best experimental results show amount of iteration for ICP algorithm to reach convergence reduces 92.34% and ICP algorithm runtime reduces 98.86%. In addition, computational cost is kept at a stable level due to the elimination of accumulated computational consumption. Moreover, great improvement is observed in the quality and robustness of SLAM

ICP-SLAM; rough alignment; initial pose matrix; local searching; dynamic threshold; real-time performance; cloud point; iteration

OI:10.11992/tis.201605029

http://kns.cnki.net/kcms/detail/23.1538.TP.20170705.1656.008.html

2016-05-27. 網絡出版日期：2017-07-05.

國家“863”計劃基金項目(2013AA03A1121,2013AA03A1122)；上海市教委重點學科資助項目(J50104).

梁濱.E-mail: zhangjinyi@staff.shu.edu.cn.

TP11

A

1673-4785(2017)03-0413-09

張金藝，男，1965年生，研究員，主要研究方向為通信類SoC設計與室內無線定位技術。發表學術論文40余篇，近3年授權與申請專利30項。

梁濱，男，1991年生，碩士研究生，主要研究方向為基于激光雷達的室內SLAM。

唐笛愷，男，1991年生，碩士研究生，主要研究方向為基于激光雷達的室內SLAM。