丁元浩，吳懷宇+，陳 洋

(1.武漢科技大學(xué) 冶金自動(dòng)化與檢測(cè)技術(shù)教育部工程研究中心，湖北 武漢 430081；2.武漢科技大學(xué) 機(jī)器人與智能系統(tǒng)研究院，湖北 武漢 430081)

0 引 言

掃描匹配[1]方法是基于2D激光SLAM[2-4]的關(guān)鍵技術(shù)之一。對(duì)于SLAM問題，近幾年的研究逐漸從RB粒子濾波(Rao-Blackwellized particle filter, RBPF)方法[5-7]轉(zhuǎn)移到圖優(yōu)化(graph optimization)方法[8-10]上。

圖優(yōu)化SLAM問題最先是用ICP (iterative closest point)[11]算法處理，即幀-幀(scan-to-scan)的方法進(jìn)行的匹配，用最近鄰原則搜索兩幀連續(xù)激光之間的相應(yīng)匹配點(diǎn)，并根據(jù)這些點(diǎn)，構(gòu)造代價(jià)函數(shù)并求解。該方法計(jì)算量過大，難以到達(dá)實(shí)時(shí)的構(gòu)圖效果。文獻(xiàn)[12]提出的Correlation Scan Matching方法，將參考的激光掃描點(diǎn)掃描渲染成概率網(wǎng)格，稱為查找表，對(duì)該表進(jìn)行搜索，提高了匹配效率，但容易陷入局部極值。文獻(xiàn)[13]提到的Hector SLAM算法，用激光掃描到地圖(scan-to-map)匹配的方法，該方法用高斯牛頓法進(jìn)行優(yōu)化求解，側(cè)重于解決SLAM過程中的抖動(dòng)問題，能適應(yīng)不平坦的場(chǎng)景，但該方法對(duì)閉環(huán)[14]優(yōu)化方面的研究較少。Cartographer[15]是Google的開源室內(nèi)制圖項(xiàng)目，它提出了子地圖(submap)的概念，并且引入了回環(huán)檢測(cè)機(jī)制，能極大縮小累計(jì)誤差，建圖效果顯著，唯一不足是該方法占用CPU資源過多，在嵌入式設(shè)備上運(yùn)行，可能會(huì)出現(xiàn)無法正常閉環(huán)的情況。

針對(duì)上述問題，本文基于Hector SLAM算法，結(jié)合Cartographer中介紹的子圖的概念，提出了一種掃描到子圖匹配的方法。首先用連續(xù)的激光掃描幀構(gòu)建子圖，對(duì)齊新的掃描幀到鄰近的子圖以產(chǎn)生約束，通過優(yōu)化求解約束估計(jì)新的子圖，最后利用舊的子圖中的最優(yōu)位姿來檢測(cè)閉環(huán)，生成全局一致地圖。本文方法有效減少了機(jī)器人SLAM過程中累計(jì)誤差，實(shí)驗(yàn)結(jié)果驗(yàn)證了該方法的有效性。

1 系統(tǒng)概述

本文系統(tǒng)框架基于圖優(yōu)化理論，主要分為3個(gè)部分，即前端、后端和閉環(huán)檢測(cè)，系統(tǒng)框架如圖1所示。

圖1 系統(tǒng)框架

SLAM前端輸入激光雷達(dá)的數(shù)據(jù)，前端預(yù)處理通過對(duì)激光雷達(dá)初始數(shù)據(jù)進(jìn)行掃描匹配等方式，提供初始機(jī)器人位姿信息。后端是利用圖模型描述子地圖估計(jì)和概率約束，然后運(yùn)用高效的稀疏矩陣優(yōu)化和概率約束來進(jìn)行進(jìn)一步優(yōu)化，實(shí)現(xiàn)全局地圖估計(jì)。閉環(huán)檢測(cè)環(huán)節(jié)提取前端機(jī)器人位姿信息和后端中的子圖信息作為檢測(cè)準(zhǔn)則，對(duì)新添加的位姿和子圖進(jìn)行非線性優(yōu)化，糾正由于累計(jì)誤差造成地圖歪曲，進(jìn)而形成全局一致地圖。

2 算法設(shè)計(jì)

2.1 掃描匹配插入到子圖

掃描匹配是將激光掃描彼此對(duì)齊或者和現(xiàn)有地圖對(duì)齊的過程，本文加入了子圖的概念，用分治法將全局地圖分成若干個(gè)子圖可使得誤差更小。

對(duì)于剛體機(jī)器人，其位姿由ε=(x,y,θ)T組成，其中(x,y)T表示平移量，θ表示偏轉(zhuǎn)量，這些位姿由激光雷達(dá)的觀測(cè)得到，即激光雷達(dá)掃描幀。在2D平面內(nèi)，將激光掃描光束的端點(diǎn)定義為h=(hx,hy)T，激光每掃描一次，將生成一個(gè)點(diǎn)集，可以將掃描點(diǎn)的集合描敘為H={hi}i=1,…,n,hi∈R2，通過坐標(biāo)變換Tε可以將掃描幀轉(zhuǎn)換成子圖數(shù)據(jù)，該變換為

(1)

激光掃描用每對(duì)連續(xù)匹配的數(shù)據(jù)通過變換轉(zhuǎn)換成子圖。該子圖對(duì)應(yīng)真實(shí)環(huán)境中的一小塊地圖。隨著機(jī)器人移動(dòng)，不斷有后續(xù)的激光掃描數(shù)據(jù)對(duì)齊到子地圖中，激光掃描匹配過程如圖2所示。

圖2 激光掃描匹配

圖2選取的兩幀數(shù)據(jù)間隔很近，圖2(a)中先求解出兩幀數(shù)據(jù)的變換關(guān)系，式(1)將當(dāng)前幀匹配到先前幀的位置。隨著機(jī)器人移動(dòng)，子圖規(guī)模會(huì)不斷增加，如圖3所示。

圖3 子圖規(guī)模增加

機(jī)器人先前位姿為ε′，通過掃描獲取周圍環(huán)境信息，具體表現(xiàn)為灰色柵格，隨著機(jī)器人移動(dòng)到當(dāng)前位置(位姿為ε)，類似特征再次被觀測(cè)到，具體表現(xiàn)為黑色柵格。隨著機(jī)器人移動(dòng)，越來越多的觀測(cè)加入地圖之中，子地圖規(guī)模也不斷增加。

2.2 高斯牛頓優(yōu)化縮小誤差

子圖的創(chuàng)建在短時(shí)間內(nèi)的誤差累積是足夠小的。然而隨著時(shí)間推移，越來越多的子圖被創(chuàng)建后，子圖間的誤差累積則會(huì)越來越大。因此需要通過優(yōu)化這些子圖的位姿進(jìn)而消除這些累積誤差，這就將問題轉(zhuǎn)化成一個(gè)非線性最小二乘優(yōu)化問題。優(yōu)化位姿為

(2)

式中：M函數(shù)是雙立方插值平滑函數(shù),該函數(shù)從激光掃描幀中返回子地圖的坐標(biāo)值。先給出初始位姿ε，定義并估計(jì)Δε，并使得觀測(cè)誤差最小，則

(3)

通過一階泰勒展開M(Si(ε+Δε))，可以得到

(4)

通過高斯牛頓(Gauss-Newton)法求解Δε，使其最小化

(5)

式中：H是海森矩陣(Hessian matrix)，為

(6)

(7)

以上即高斯牛頓求最優(yōu)解的推導(dǎo)過程。

2.3 位姿閉環(huán)與地圖矯正

較大的空間通過創(chuàng)建許多小的子圖來處理，將上一步的經(jīng)過高斯牛頓優(yōu)化的所有掃描和子圖的信息，存儲(chǔ)在閉環(huán)檢測(cè)中使用。

同掃描匹配一樣，閉環(huán)優(yōu)化也被表述為一個(gè)非線性最小二乘問題，它允許很容易地添加殘差來考慮額外的數(shù)據(jù)。每隔幾秒鐘，我們就使用非線性優(yōu)化庫Ceres[16]來計(jì)算

(8)

(9)

(10)

ρ是Huber損失函數(shù),用來降低由于掃描環(huán)境原因產(chǎn)生的而異常值所帶來的影響。

當(dāng)子圖的構(gòu)建完成時(shí)，也就是不會(huì)再有新的幀插入到該子圖時(shí)，該子圖就會(huì)加入到閉環(huán)檢測(cè)中。閉環(huán)檢測(cè)會(huì)考慮所有的已完成創(chuàng)建的子圖。當(dāng)一個(gè)新的掃描幀加入到地圖中時(shí)，如果該掃描幀的估計(jì)位姿與地圖中某個(gè)子圖的某個(gè)掃描幀的位姿比較接近的話，那么通過匹配就會(huì)找到該閉環(huán)。閉環(huán)矯正原理如圖4所示。

圖4 閉環(huán)矯正原理

圖4(a)為矯正之間的地圖，其中A為機(jī)器人運(yùn)動(dòng)的起點(diǎn)，經(jīng)過B，由于機(jī)器人的累計(jì)誤差，最終到達(dá)C點(diǎn)，但在C點(diǎn)附近的觀測(cè)特征與A處的特征一致，故可以找到一處閉環(huán)，將A與C視為同一點(diǎn)，地圖得以矯正。具體閉環(huán)矯正案例如圖5所示。

圖5 閉環(huán)矯正案例

圖5中的圓圈代表機(jī)器人當(dāng)前位置，可以看出當(dāng)機(jī)器人移動(dòng)到圖5(a)的地圖時(shí)，由于誤差的累計(jì)，左圖由虛線框選的位置出現(xiàn)彎曲，但隨著機(jī)器人運(yùn)動(dòng)到圖5(b)的位置時(shí)，由于先前訪問過該地點(diǎn)，在該點(diǎn)附近找到了相關(guān)子圖區(qū)域的掃描幀相匹配，實(shí)現(xiàn)了閉環(huán)，進(jìn)而實(shí)現(xiàn)了地圖矯正。

3 實(shí)驗(yàn)與分析

為驗(yàn)證該方法的有效性，本方案在某室內(nèi)場(chǎng)景下進(jìn)行，采用搭載Hokuyo激光雷達(dá)的JS-R機(jī)器人，進(jìn)行定位與構(gòu)圖實(shí)驗(yàn)。

機(jī)體主控芯片為STM32F103，底層控制方式為麥克納姆四輪全向移動(dòng)，機(jī)器人移動(dòng)速度為0.2 m/s,激光雷達(dá)的有效范圍是0 m-10 m，掃描角度為0°-180°，分辨率為0.25。機(jī)器人如圖6所示。

圖6 JS-R機(jī)器人

SLAM程序在上位機(jī)運(yùn)行，上位機(jī)搭載的是Ubuntu12.04，主要通過ROS系統(tǒng)運(yùn)行SLAM程序，并將記錄的SLAM節(jié)點(diǎn)數(shù)據(jù)實(shí)時(shí)傳給從機(jī)，從機(jī)通過TCP/IP與上位機(jī)通信，并用Matlab處理接收的SLAM數(shù)據(jù)。

3.1 移動(dòng)機(jī)器人定位軌跡誤差分析

為分析本文方法與現(xiàn)存的掃描匹配方法在定位環(huán)節(jié)上的差別，實(shí)驗(yàn)選取為10.7 m×6.9 m的方形實(shí)驗(yàn)場(chǎng)景，如圖7所示。

圖7 機(jī)器人移動(dòng)路徑

機(jī)器人按照?qǐng)D7中箭頭所指的路徑行駛，整條路徑長21.8 m，設(shè)置機(jī)器人移動(dòng)速度為0.2 m/s，分別使用幀-幀(scan-to-scan)、幀-圖(scan-to-map)以及本文的幀-子圖(scan-to-submap)方法進(jìn)行實(shí)驗(yàn)執(zhí)行20次實(shí)驗(yàn)，求SLAM軌跡的平均值，可得圖8中的曲線。

圖8 定位軌跡

圖8中，(0,0)點(diǎn)為機(jī)器人的運(yùn)動(dòng)起點(diǎn)，從曲線可以直觀看出，本文提出的方法最貼近真實(shí)路徑，說明本文在引入的Ceres非線性優(yōu)化方法能很好地估計(jì)機(jī)器人位姿，保持與實(shí)際位置不會(huì)有太大偏差。其次是幀-圖匹配方法，由于同樣使用的高斯牛頓優(yōu)化方法，但是缺少全局的非線性優(yōu)化環(huán)節(jié)，定位效果稍遜色于本文方法。幀-幀匹配方法偏離最遠(yuǎn)，這是由于單純的激光匹配過程會(huì)經(jīng)常遇到匹配失效的情況。

為更精確表明本方法與其它方法的差異，可計(jì)算不同時(shí)刻，機(jī)器人在X方向和Y方向上產(chǎn)生的誤差。整條路徑耗時(shí)109 s，3種SLAM方法產(chǎn)生的點(diǎn)均為13 892個(gè)，由于數(shù)據(jù)量過大，須對(duì)這些點(diǎn)進(jìn)行采樣。為保證數(shù)據(jù)不失真，在橫坐標(biāo)T上，取t=0.2s為一步長，最終取得對(duì)應(yīng)的誤差曲線如圖9所示。

圖9 SLAM算法誤差

圖9(a)、圖9(b)分別為移動(dòng)機(jī)器人位置估計(jì)在X和Y方向上的誤差，橫坐標(biāo)t為時(shí)間步，每次實(shí)驗(yàn)都經(jīng)過522個(gè)時(shí)間步。從圖9可以看出，本文方法面對(duì)觀測(cè)噪聲的魯棒性很強(qiáng)，誤差峰值不超過1 m，另兩種方法面對(duì)噪聲，曲線波動(dòng)很大，特別是幀-幀方法，在Y方向的誤差，達(dá)到了5 m左右。

表1給出了各種SLAM方法的均方根誤差(RMSE)。

表1 本文方法與幀-幀方法、幀-圖方法RMSE對(duì)比

從表1的數(shù)據(jù)可以看出，本文提出的掃描到子圖匹配方法的誤差在0.4 m以下，且比幀-圖方法在X方向的誤差減少了73.41%，Y方向的誤差減少了84.72%，這是因?yàn)樵诩す鈷呙杵ヅ淝岸?，本文方法初步將多幀激光?gòu)成子圖，并用高斯牛頓優(yōu)化局部位姿，這一步保證可以過濾掉很多失效匹配的激光幀，且在后端加入了全局閉環(huán)優(yōu)化環(huán)節(jié)，更進(jìn)一步縮小了機(jī)器人的平移誤差。而相比之下，幀-幀方法由于是激光直接匹配，沒有考慮到誤檢、漏檢的情況，所以平移誤差有2 m-3 m，幀-圖方法由于缺少后端全局優(yōu)化環(huán)節(jié)，平移誤差也比較大，達(dá)到了1 m-2 m。

通過上述實(shí)驗(yàn)分析，可以驗(yàn)證該算法在SLAM定位誤差上相比另外兩種方法更小，SLAM定位更精準(zhǔn)。

3.2 制圖精度分析

由圖5所示的實(shí)驗(yàn)場(chǎng)景，運(yùn)用3種SLAM方法建立的地圖如圖10所示。

圖10 SLAM建圖結(jié)果

不難看出，圖10(a)中的地圖出現(xiàn)了重影，圖10(b)中的地圖輪廓有瑕疵，相比之下，圖10(c)所建立的地圖更精細(xì)。從圖10(a)、圖10(b)和圖10(c)中由橢圓和矩形框選的區(qū)域上看，圖10(c)在地圖細(xì)節(jié)上呈現(xiàn)的更清楚,表明本文方法構(gòu)建出的地圖質(zhì)量高于另外兩種方法。

為進(jìn)一步說明地圖的精確度，可以計(jì)算占用柵格地圖的尺度，通過測(cè)量值與環(huán)境真實(shí)值作對(duì)比，求出誤差。其中真實(shí)環(huán)境的尺度為10.7 m×6.9 m，占用柵格地圖在X方向的計(jì)算公式為

X=μ·nx

(11)

式中：μ為柵格分辨率，nx表示X方向的占用柵格數(shù)，Y方向同理。表2給出了各種SLAM方法的地圖尺度誤差數(shù)據(jù)。

表2 建立的地圖尺度誤差分析

從表2的數(shù)據(jù)可以看出，本文方法相比另外兩種方法建圖誤差更小，尺度誤差控制在了0.5 m左右。在X方向上，本文相比幀-圖方法精確度提高42.3%，在Y方向上精確度提高了45.9%，這是高斯牛頓修正誤差生成子圖與全局的優(yōu)化矯正地圖的緣故。另外兩種方案只是單純的通過激光匹配計(jì)算位姿估計(jì)地圖而已，并沒有對(duì)地圖進(jìn)行優(yōu)化操作，所以幀-圖、幀-幀的方法在制圖方面表現(xiàn)較差。本文的掃描到子圖(scan-to-submap)匹配的方法在建圖的精確度上表現(xiàn)更好。

3.3 匹配算法效率與準(zhǔn)確度分析

通過rosbag記錄的SLAM數(shù)據(jù)進(jìn)行激光匹配實(shí)驗(yàn)，該數(shù)據(jù)包含13 515幀激光掃描數(shù)據(jù)。為了比較3種算法的差別，實(shí)驗(yàn)選取前1000幀數(shù)據(jù)進(jìn)行比較，用每10幀激光數(shù)據(jù)進(jìn)行匹配，共匹配100次。通過計(jì)算各個(gè)算法的掃描匹配的時(shí)間與準(zhǔn)確率，可以得到表3中的數(shù)據(jù)。

表3 3種匹配方案效率與準(zhǔn)確率

由于前端生成的子地圖可為后匹配的激光信息作為檢測(cè)標(biāo)準(zhǔn)，這樣既縮短了匹配的時(shí)間，又提升了匹配的準(zhǔn)確率，以致使本文的子地圖能夠提升38.24%掃描匹配的速度，每次匹配耗時(shí)4.2 s，準(zhǔn)確率高達(dá)93.3%。

綜上所述，本文基于scan-to-submap方法的室內(nèi)環(huán)境下的激光SLAM方法縮小了由于機(jī)器人移動(dòng)造成累計(jì)誤差，提高了SLAM前端匹配效率，實(shí)現(xiàn)了準(zhǔn)確的室內(nèi)定位，在地圖精度上也有明顯提升。

4 結(jié)束語

本文結(jié)合Hector SLAM算法中的scan-to-map方法和cartographer提出的submap的概念，提出了一種基于scan-to-submap方法的室內(nèi)環(huán)境下的激光SLAM方法。首先利用激光掃描匹配方法構(gòu)建子圖，通過Gauss-Newton優(yōu)化獲得較為準(zhǔn)確的機(jī)器人位姿，隨著子圖的累加，通過Ceres非線性優(yōu)化庫消除累計(jì)誤差，進(jìn)而實(shí)現(xiàn)全局一致地圖。經(jīng)過實(shí)驗(yàn)驗(yàn)證，所提出的方法能夠準(zhǔn)確實(shí)現(xiàn)室內(nèi)定位與地圖構(gòu)建，具有良好的實(shí)用價(jià)值。