張金藝，梁濱，唐笛愷，姚維強(qiáng)，鮑深

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

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

張金藝1,2，梁濱1，唐笛愷1，姚維強(qiáng)2，鮑深2

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

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

ICP-SLAM；粗匹配；初始姿態(tài)矩陣；局部搜索；動(dòng)態(tài)閾值；實(shí)時(shí)性；點(diǎn)云；迭代；

中文引用格式：張金藝，梁濱，唐笛愷，等. 粗匹配和局部尺度壓縮搜索下的快速ICP-SLAM[J]. 智能系統(tǒng)學(xué)報(bào)， 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.

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

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

綜上所述，為了有效提高ICP-SLAM的實(shí)時(shí)性，本文提出基于粗匹配和局部尺度壓縮搜索的快速ICP-SLAM。該快速ICP-SLAM在激光雷達(dá)的基礎(chǔ)上增加了MEMS磁力計(jì)，這個(gè)MEMS磁力計(jì)能直接輸出當(dāng)前航向角。并且引入全局地標(biāo)，通過(guò)激光雷達(dá)掃描并測(cè)量全局地標(biāo)，計(jì)算出機(jī)器人當(dāng)前的位置信息。最后由航向角和機(jī)器人當(dāng)前的位置信息共同構(gòu)成完整的初值姿態(tài)矩陣。當(dāng)?shù)玫搅顺踔底藨B(tài)矩陣后，首先通過(guò)該初值姿態(tài)矩陣實(shí)現(xiàn)當(dāng)前幀和全局地圖的粗匹配。其次，在ICP算法的每次迭代中，采用局部尺度壓縮搜索完成匹配點(diǎn)搜索，從而避免了由全局搜索帶來(lái)的巨大計(jì)算開(kāi)銷。同時(shí)，每次迭代完成之后，匹配點(diǎn)的搜索范圍通過(guò)動(dòng)態(tài)閾值被縮小，加快匹配點(diǎn)搜索的速度，這將進(jìn)一步提高ICP-SLAM實(shí)時(shí)性。實(shí)驗(yàn)結(jié)果顯示，本文提出的快速ICP-SLAM相比于傳統(tǒng)ICP-SLAM，迭代次數(shù)減少了92.34%，ICP算法運(yùn)行時(shí)間減少了98.86%。同時(shí)，ICP-SLAM的系統(tǒng)負(fù)載也被控制在穩(wěn)定狀態(tài)，其整體性能得到較大的改善。

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

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

圖1 計(jì)算姿態(tài)矩陣Fig.1 Initial pose matrix

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

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

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

因?yàn)槿值貥?biāo)為全局的靜態(tài)參照物，所以通過(guò)該方法計(jì)算得到的Poset(xt,yt,θt)不存在累計(jì)誤差。

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

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

2 基于動(dòng)態(tài)閾值的局部尺度壓縮搜索

粗匹配完成之后，當(dāng)前幀中的點(diǎn)和其匹配點(diǎn)的距離被大大縮小，所以本文中的匹配點(diǎn)搜索將通過(guò)局部搜索完成。同時(shí)，每次迭代完成之后，匹配點(diǎn)之間的距離都比上一次迭代時(shí)更小，此時(shí)便可以通過(guò)動(dòng)態(tài)閾值達(dá)到尺度壓縮，從而縮小匹配點(diǎn)搜索的范圍。局部尺度壓縮搜索不僅能加快匹配點(diǎn)搜索速度，還能提高匹配點(diǎn)搜索質(zhì)量。同時(shí)在本文中，因?yàn)镾LAM創(chuàng)建的地圖為柵格地圖，所以本小節(jié)將先闡述本文中采用的柵格地圖的坐標(biāo)表征方式，接著再詳細(xì)剖析局部搜索和尺度壓縮。

2.1 柵格地圖的坐標(biāo)表征

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

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

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

當(dāng)有更多的點(diǎn)落入到某一個(gè)正方形單元格時(shí)，該正方形單元格的顏色會(huì)加深。

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

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

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

圖4 擴(kuò)建柵格地圖Fig.4 Grid map expansion

2.3 基于動(dòng)態(tài)閾值的尺度壓縮

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

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

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

匹配點(diǎn)的權(quán)重通過(guò)式(17)計(jì)算：

式中E為動(dòng)態(tài)閾值。匹配點(diǎn)的數(shù)量為

當(dāng)前幀與全局地圖的重合度可以表示為

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

完成一次迭代之后，E通過(guò)下式被縮?。?/p>

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

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

圖6 實(shí)驗(yàn)平臺(tái)Fig.6 Experiment platform

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

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

3.1 快速ICP-SLAM實(shí)時(shí)性改善驗(yàn)證

通過(guò)第1節(jié)和第2節(jié)的分析可知，ICP-SLAM實(shí)時(shí)性受初始姿態(tài)矩陣、局部搜素和參數(shù)Fdec影響。本小節(jié)將采用復(fù)合開(kāi)環(huán)航跡和復(fù)合閉環(huán)航跡，驗(yàn)證粗匹配和局部尺度壓縮搜索對(duì)ICP-SLAM實(shí)時(shí)性的改善。復(fù)合開(kāi)環(huán)航跡和復(fù)合閉環(huán)航跡分別如圖7中的(a)和(b)所示。

圖7 復(fù)合開(kāi)環(huán)航跡和復(fù)合閉環(huán)航跡Fig.7 Mix-open track and mix-close track

粗匹配對(duì)迭代次數(shù)和ICP算法運(yùn)行時(shí)間的改善如表2和表3所示。

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

表2 小車在復(fù)合開(kāi)環(huán)航跡下的實(shí)驗(yàn)結(jié)果

表3 小車在復(fù)合閉環(huán)航跡下的實(shí)驗(yàn)結(jié)果

圖8 Fdec在復(fù)合開(kāi)環(huán)航跡下的實(shí)驗(yàn)結(jié)果Fig.8 Result of Fdec in mix-open track

圖9 Fdec在復(fù)合閉環(huán)航跡下的實(shí)驗(yàn)結(jié)果Fig.9 Result of Fdec in mix-close track

除此之外，采用全局搜索時(shí)，ICP-SLAM的計(jì)算開(kāi)銷會(huì)隨著全局地圖的增大而增大。但當(dāng)采用局部尺度壓縮搜索時(shí)，ICP-SLAM的計(jì)算開(kāi)銷被控制在穩(wěn)定狀態(tài)，如圖10所示。

圖10 ICP-SLAM的計(jì)算開(kāi)銷對(duì)比Fig.10 Comparison of computation in ICP-SLAM

從圖10中可以看出，當(dāng)采用局部尺度壓縮搜索時(shí)，隨著ICP算法迭代次數(shù)的增加，ICP算法的運(yùn)行時(shí)間被保持在一個(gè)穩(wěn)定值，這表明ICP-SLAM的計(jì)算開(kāi)銷被控制在穩(wěn)定狀態(tài)。但采用全局搜索時(shí)，ICP-SLAM的計(jì)算開(kāi)銷卻隨著ICP算法迭代次數(shù)的增加而不斷增加。所以局部尺度壓縮搜索對(duì)提高ICP-SLAM實(shí)時(shí)性有著顯著的效果。對(duì)比圖10和圖11可知，本文提出的ICP-SLAM比文獻(xiàn)[20]的局部SLAM在計(jì)算負(fù)載的穩(wěn)定性上更有優(yōu)勢(shì)。

圖11 文獻(xiàn)[20]中的SLAM波動(dòng)過(guò)程Fig.11 SLAM unstable process in [20]

3.2 快速ICP-SLAM整體性改善驗(yàn)證

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

圖12 快速ICP-SLAM整體性改善驗(yàn)證結(jié)果Fig.12 Overall improvement in fast ICP-SLAM

4 結(jié)束語(yǔ)

針對(duì)傳統(tǒng)ICP-SLAM實(shí)時(shí)性差，本文提出了粗匹配和局部尺度壓縮搜索。在進(jìn)行ICP算法開(kāi)始之前，通過(guò)MEMS磁力計(jì)和全局地標(biāo)計(jì)算出機(jī)器人當(dāng)前位姿矩陣，并基于該位姿矩陣完成當(dāng)前幀和全局地圖的粗匹配，從而減少ICP算法的迭代次數(shù)。同時(shí)在ICP算法每次迭代中，采用局部尺度壓縮搜索替代全局搜索完成匹配點(diǎn)搜索，加快匹配點(diǎn)搜索速度。實(shí)驗(yàn)結(jié)果表明ICP-SLAM實(shí)時(shí)性得到了很大提升，迭代次數(shù)和系統(tǒng)運(yùn)行時(shí)間分別降低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ī)器人定位中的應(yīng)用[J]. 科技經(jīng)濟(jì)導(dǎo)刊, 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]陳興秀, 張金藝, 晏理, 等. 三維復(fù)雜運(yùn)動(dòng)模式航跡推算慣性導(dǎo)航室內(nèi)定位[J]. 應(yīng)用科學(xué)學(xué)報(bào), 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的室內(nèi)定位算法優(yōu)化技術(shù)[J]. 計(jì)算機(jī)工程與應(yīng)用, 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]王益健. 藍(lán)牙室內(nèi)定位關(guān)鍵技術(shù)的研究與實(shí)現(xiàn)[D]. 南京: 東南大學(xué), 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. 網(wǎng)絡(luò)出版日期：2017-07-05.

國(guó)家“863”計(jì)劃基金項(xiàng)目(2013AA03A1121,2013AA03A1122)；上海市教委重點(diǎn)學(xué)科資助項(xiàng)目(J50104).

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

TP11

A

1673-4785(2017)03-0413-09

張金藝，男，1965年生，研究員，主要研究方向?yàn)橥ㄐ蓬怱oC設(shè)計(jì)與室內(nèi)無(wú)線定位技術(shù)。發(fā)表學(xué)術(shù)論文40余篇，近3年授權(quán)與申請(qǐng)專利30項(xiàng)。

梁濱，男，1991年生，碩士研究生，主要研究方向?yàn)榛诩す饫走_(dá)的室內(nèi)SLAM。

唐笛愷，男，1991年生，碩士研究生，主要研究方向?yàn)榛诩す饫走_(dá)的室內(nèi)SLAM。