基于2D激光掃描到子圖匹配的SLAM方法

丁元浩，吳懷宇+，陳 洋

(1.武漢科技大學 冶金自動化與檢測技術教育部工程研究中心，湖北 武漢 430081；2.武漢科技大學 機器人與智能系統研究院，湖北 武漢 430081)

0 引 言

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

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

針對上述問題，本文基于Hector SLAM算法，結合Cartographer中介紹的子圖的概念，提出了一種掃描到子圖匹配的方法。首先用連續的激光掃描幀構建子圖，對齊新的掃描幀到鄰近的子圖以產生約束，通過優化求解約束估計新的子圖，最后利用舊的子圖中的最優位姿來檢測閉環，生成全局一致地圖。本文方法有效減少了機器人SLAM過程中累計誤差，實驗結果驗證了該方法的有效性。

1 系統概述

本文系統框架基于圖優化理論，主要分為3個部分，即前端、后端和閉環檢測，系統框架如圖1所示。

圖1 系統框架

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

2 算法設計

2.1 掃描匹配插入到子圖

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

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

(1)

激光掃描用每對連續匹配的數據通過變換轉換成子圖。該子圖對應真實環境中的一小塊地圖。隨著機器人移動，不斷有后續的激光掃描數據對齊到子地圖中，激光掃描匹配過程如圖2所示。

圖2 激光掃描匹配

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

圖3 子圖規模增加

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

2.2 高斯牛頓優化縮小誤差

子圖的創建在短時間內的誤差累積是足夠小的。然而隨著時間推移，越來越多的子圖被創建后，子圖間的誤差累積則會越來越大。因此需要通過優化這些子圖的位姿進而消除這些累積誤差，這就將問題轉化成一個非線性最小二乘優化問題。優化位姿為

(2)

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

(3)

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

(4)

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

(5)

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

(6)

(7)

以上即高斯牛頓求最優解的推導過程。

2.3 位姿閉環與地圖矯正

較大的空間通過創建許多小的子圖來處理，將上一步的經過高斯牛頓優化的所有掃描和子圖的信息，存儲在閉環檢測中使用。

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

(8)

(9)

(10)

ρ是Huber損失函數,用來降低由于掃描環境原因產生的而異常值所帶來的影響。

當子圖的構建完成時，也就是不會再有新的幀插入到該子圖時，該子圖就會加入到閉環檢測中。閉環檢測會考慮所有的已完成創建的子圖。當一個新的掃描幀加入到地圖中時，如果該掃描幀的估計位姿與地圖中某個子圖的某個掃描幀的位姿比較接近的話，那么通過匹配就會找到該閉環。閉環矯正原理如圖4所示。

圖4 閉環矯正原理

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

圖5 閉環矯正案例

圖5中的圓圈代表機器人當前位置，可以看出當機器人移動到圖5(a)的地圖時，由于誤差的累計，左圖由虛線框選的位置出現彎曲，但隨著機器人運動到圖5(b)的位置時，由于先前訪問過該地點，在該點附近找到了相關子圖區域的掃描幀相匹配，實現了閉環，進而實現了地圖矯正。

3 實驗與分析

為驗證該方法的有效性，本方案在某室內場景下進行，采用搭載Hokuyo激光雷達的JS-R機器人，進行定位與構圖實驗。

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

圖6 JS-R機器人

SLAM程序在上位機運行，上位機搭載的是Ubuntu12.04，主要通過ROS系統運行SLAM程序，并將記錄的SLAM節點數據實時傳給從機，從機通過TCP/IP與上位機通信，并用Matlab處理接收的SLAM數據。

3.1 移動機器人定位軌跡誤差分析

為分析本文方法與現存的掃描匹配方法在定位環節上的差別，實驗選取為10.7 m×6.9 m的方形實驗場景，如圖7所示。

圖7 機器人移動路徑

機器人按照圖7中箭頭所指的路徑行駛，整條路徑長21.8 m，設置機器人移動速度為0.2 m/s，分別使用幀-幀(scan-to-scan)、幀-圖(scan-to-map)以及本文的幀-子圖(scan-to-submap)方法進行實驗執行20次實驗，求SLAM軌跡的平均值，可得圖8中的曲線。

圖8 定位軌跡

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

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

圖9 SLAM算法誤差

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

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

表1 本文方法與幀-幀方法、幀-圖方法RMSE對比

從表1的數據可以看出，本文提出的掃描到子圖匹配方法的誤差在0.4 m以下，且比幀-圖方法在X方向的誤差減少了73.41%，Y方向的誤差減少了84.72%，這是因為在激光掃描匹配前端，本文方法初步將多幀激光構成子圖，并用高斯牛頓優化局部位姿，這一步保證可以過濾掉很多失效匹配的激光幀，且在后端加入了全局閉環優化環節，更進一步縮小了機器人的平移誤差。而相比之下，幀-幀方法由于是激光直接匹配，沒有考慮到誤檢、漏檢的情況，所以平移誤差有2 m-3 m，幀-圖方法由于缺少后端全局優化環節，平移誤差也比較大，達到了1 m-2 m。

通過上述實驗分析，可以驗證該算法在SLAM定位誤差上相比另外兩種方法更小，SLAM定位更精準。

3.2 制圖精度分析

由圖5所示的實驗場景，運用3種SLAM方法建立的地圖如圖10所示。

圖10 SLAM建圖結果

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

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

X=μ·nx

(11)

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

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

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

3.3 匹配算法效率與準確度分析

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

表3 3種匹配方案效率與準確率

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

綜上所述，本文基于scan-to-submap方法的室內環境下的激光SLAM方法縮小了由于機器人移動造成累計誤差，提高了SLAM前端匹配效率，實現了準確的室內定位，在地圖精度上也有明顯提升。

4 結束語

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