基于ICP的移動機器人同時定位與地圖構(gòu)建的研究

趙希宇，沈一鳴

(沈陽工業(yè)大學 機械工程學院， 遼寧 沈陽 110870)

0 引言

同時定位與地圖構(gòu)建(SLAM, Simultaneous Localization and Mapping)技術(shù)是移動機器人領域最熱門的技術(shù)之一[1]。在SLAM中最常用的算法之一是由Besl和McKay開發(fā)的迭代最近點(ICP, Iterative Closest Point)算法，它用于估計兩個點云的相對轉(zhuǎn)換關系[2]。ICP算法是基于非概率的定位方案，它具有原理簡單、實現(xiàn)成本低的特點[3]，但是由于ICP-SLAM算法的實時性很差，在大環(huán)境下的工作穩(wěn)定性也下降明顯，在目前地圖工作環(huán)境越來越復雜的情況下傳統(tǒng)的ICP-SLAM已經(jīng)不再滿足系統(tǒng)對實時性的要求。

針對以上的問題，本文提出一種高效的基于ICP的SLAM算法，通過將地圖按照固定距離分割成若干子地圖，分別在各個子地圖中進行ICP計算，再利用子地圖與全局地圖坐標系的轉(zhuǎn)換計算機器人的位姿。本方法在一定程度上減少了由于實時性降低所產(chǎn)生的系統(tǒng)累計誤差，可以滿足較大范圍SLAM的精度需求。

1 ICP算法

ICP算法用于在同一個坐標系下對兩個點集進行匹配。已知有兩個數(shù)據(jù)點集P和Q，其中pi∈P,qi∈Q，通過迭代使兩個點集之間最近點的距離平方和縮小來求得其變換矩陣。

二維平面中的兩個2D點可用pi=(xi,yi)和qi=(xj,yj)來表示，其幾何距離可表示為：

(1)

其中：qi=Rpi+T+Ni,i=1,2,…，N，R為旋轉(zhuǎn)變換矩陣，T為位移變換矩陣，Ni為第i點的噪聲，N為點的數(shù)目。兩個點集當中所有最近點幾何距離的平方和為：

(2)

再通過迭代使E收斂到設置的閾值。

1.1 基本ICP算法

根據(jù)Rusinkiewicz和Levoy提出的理論[4],ICP算法利用掃描匹配方法通過迭代使兩個掃描數(shù)據(jù)對齊,在每次迭代中，算法選擇最近的點作為對應關系，計算變換的齊次矩陣進行對齊。

1.1.1 狀態(tài)預測

(3)

(4)

其中：θ為Snew與Sref所在坐標系夾角；tx、ty分別為Snew坐標系原點在Sref坐標系的橫、縱軸坐標值。

1.1.2 數(shù)據(jù)關聯(lián)

for i = 1 to N

for k = 1 to N0

if (dmin(i)>d(k)) dmin(i)=d(k)

end for

c(i)=k

end for

1.1.3 位置估計

定義J為兩組關聯(lián)的掃描點的距離誤差平方的平均數(shù)，這一步的目的是通過減小J來估計轉(zhuǎn)換矩陣的參數(shù)tx、ty和θ。該誤差表示為:

(5)

(6)

(7)

接下來檢測不合適的點并丟棄。定義一個布爾函數(shù)pT(i)來檢測第i個點是否匹配，這樣對能夠正確匹配的點增加其權(quán)重，而對不能正確進行匹配的點減小其權(quán)重以降低它對估計過程的影響，即：

(8)

其中:E為經(jīng)過實驗得到的閾值；Jb(i)為誤差距離的平方和：

(9)

則兩個數(shù)據(jù)集中有效對應點的個數(shù)為：

(10)

掃描點重疊的程度用比率PT表示為：

(11)

掃描匹配可被認為是用來確定使匹配標準IT最小化的2D變換的優(yōu)化問題。因此，對于一個變換T，通過累積匹配誤差的和除以有效的對應個數(shù)nT得到平均誤差值，再除以掃描點重疊的程度比率PT對相關性低的匹配進行懲罰。一般的匹配標準IT表示為：

(12)

(13)

(14)

(15)

(16)

(17)

(18)

(19)

(20)

(21)

1.2 在環(huán)境中應用ICP-SLAM

使用ICP算法進行同時定位與地圖構(gòu)建的基本原理是最新的掃描點集Snew與已存儲在地圖M中的掃描點集Sref進行匹配。首先設置移動機器人的初始位姿P(x,y,θ)為(0,0,0)并存儲在地圖M中。移動機器人到下一位姿狀態(tài)Pnew，同時讀取此時激光雷達數(shù)據(jù)Snew。然后使用ICP算法對當前位姿進行計算，再把Snew存入地圖M中完成地圖更新。最后移動機器人到下一個狀態(tài)進行下一次計算直到機器人停止運行。

ICP算法可以有效地完成同時定位與地圖構(gòu)建，但隨著機器人的持續(xù)工作地圖逐漸增大，點的關聯(lián)步驟所需時間也逐漸增加，這是由于數(shù)據(jù)關聯(lián)是將最新掃描點與地圖M中的所有點進行匹配。因此，要解決這個問題需要限制地圖中點的容量。

1.3 改進的ICP-SLAM算法

改進ICP-SLAM算法的原則是將整個環(huán)境地圖分割成若干子地圖，可以以機器人運動的距離和時間為標準來進行劃分。當機器人以均勻速度移動時可采用時間閾值方式進行分割，但這種方式應用在變速移動的機器人上可能導致地圖的分割不均勻，當移動過快時機器人激光雷達與里程計信息采集量大，所需要的迭代計算量很可能不滿足實時性的要求，甚至定位結(jié)果不收斂，而使用里程計得到的機器人移動行程作為閾值來分割地圖可以有效地解決這個問題。本文以行程作為劃分地圖的標準，設置行程的閾值為Sth，當機器人移動的行程Se達到Sth,Se將保存原地圖同時初始化并創(chuàng)建一個新的子地圖。對于每個子地圖定義一個參考系，同時在每個子地圖Msub中使用ICP進行定位，此時Sref變?yōu)镾sub。

應用改進的ICP-SLAM算法的具體步驟如圖1所示。首先初始化機器人的位姿P(x,y,θ)為(0,0,0)并將激光雷達首次采集的數(shù)據(jù)同時存儲到子地圖Msub和全局地圖M中。然后循環(huán)比較移動機器人移動的行程是否達到子地圖行程的閾值Sth，若Se>Sth則保存機器人最后的位姿狀態(tài)并初始化下一個新的子地圖，若沒有達到閾值則繼續(xù)執(zhí)行下面步驟。接下來，機器人移動到Pnew,同時獲得激光數(shù)據(jù)Snew，通過ICP算法計算其當前位姿。最后將Snew存入子地圖Msub和全局地圖M。機器人移動到下一狀態(tài)讀取激光雷達數(shù)據(jù)并開始進行下一次迭代計算直到機器人停止。

1.4 地圖管理

地圖數(shù)據(jù)使用基于點的幾何表示法，地圖的構(gòu)建過程是增量式的。在機器人移動并掃描周圍環(huán)境時，符合要求的新掃描數(shù)據(jù)會被融合進地圖當中。數(shù)據(jù)點匹配后重合的點融合在一起，如圖2所示融合之后的白色點為地圖的新增點。

圖1 改進的ICP-SLAM算法流程

圖2 數(shù)據(jù)融合

2 實驗及仿真

仿真使用常規(guī)走廊的數(shù)據(jù)集；激光雷達使用URG-04LX-UG01，參數(shù)如下：重量為160 g，功率為2.5 W，測量距離為20 mm～5 600 mm，最大掃描角度為240°，距離在60 mm～1 000 mm范圍時精度為±30 mm，距離大于1 000 mm時誤差為3%，角度分辨率為0.352°(每周掃描1 024次)。里程計誤差如表1所示。其中，Δx、Δy和Δφ分別表示機器人x方向相對誤差、y方向相對誤差和角度相對誤差。

表1 里程計誤差

基于MRPT軟件分別使用傳統(tǒng)ICP與改進的ICP算法進行同時定位與建圖，其結(jié)果見圖3。

圖3 傳統(tǒng)ICP和改進ICP算法建立的地圖比較

通過對比發(fā)現(xiàn),圖3(a)中使用傳統(tǒng)ICP算法建立的地圖由于迭代次數(shù)逐漸增多，計算達不到實時性要求，導致機器人定位誤差較大，從而影響地圖中墻體輪廓位置精度。圖3(b)使用改進的ICP算法通過里程閾值控制迭代的計算量有效地控制了計算的復雜度，從圖像效果可以看出地圖中墻體輪廓較圖3(a)更精確,地圖的質(zhì)量更好。

3 結(jié)語

本文針對移動機器人在復雜環(huán)境下使用傳統(tǒng)的ICP-SLAM進行定位與地圖構(gòu)建時存在的實時性不足問題，根據(jù)ICP的基本算法，使用機器人行駛距離作為閾值條件將地圖分割成若干子區(qū)域分別進行ICP計算，使計算量滿足實時性要求。通過MRPT軟件進行改進算法的仿真，仿真結(jié)果表明，改進的ICP算法能有效地解決實時性降低的問題,比傳統(tǒng)的ICP算法有較強的穩(wěn)定性和魯棒性。