基于加權SDF地圖的二維激光SLAM方法

夏 天 任 彧

(杭州電子科技大學計算機學院 浙江 杭州 310016)

0 引 言

在實際應用中，機器人需要在未知的環境中進行有效的路徑規劃并自主探索環境，這使得它必須具備自主創建環境地圖并且實現準確自我定位的能力，這一問題被稱為即時定位和地圖構建(SLAM)。SLAM技術是一項用于解決機器人在未知環境中進行自我定位和環境地圖構建問題的技術。在SLAM中，機器人的自主定位和對環境的地圖構建二者相互依存，精準的地圖構建需要建立在準確的定位的基礎之上，同時定位需要環境地圖的數據作為支撐。隨著近年來人工智能和控制理論的不斷發展，國內外各大學者提出了大量用于解決SLAM問題的方案。

機器人操作系統(ROS)[1]包含了很多可用的2D SLAM算法[2]，如Hector SLAM[3]、Gmapping算法[4]和CoreSLAM[5]等。Gampping是一種基于粒子濾波的SLAM方案，其在長廊等低特征場景中建圖效果好，但其計算量較大，且依賴里程計，無法適用無人機及地面小車不平坦區域；CoreSLAM是一種小型的簡單易懂的SLAM方法，同樣使用粒子濾波方法，其方法較為簡單，建圖誤差通常較大。Hector SLAM算法使用高斯牛頓法求解掃描匹配問題，無須依賴里程計，可適用于無人機和地面不平坦區域，要求高更新頻率的激光掃描儀。由于在Hector SLAM中，占據柵格地圖存在固有的精度限制，且高斯牛頓法存在可能不收斂且局部近似不夠準確的缺陷。因此，本文提出了一種采用改進的符號距離函數(Signed Distance Function，SDF)地圖的SLAM方法。2D-SDF地圖是Fossel等[16]基于傳統占據柵格地圖提出的一種地圖模型，該地圖使用SDF值構建地圖，可以低于柵格的精度構建地圖。本文采用一種加權SDF地圖，將SDF地圖通過距離加權后可以獲得更平滑的結果，容錯率較高。同時使用L-M法求解掃描匹配問題，L-M算法是介于高斯牛頓法與梯度下降法之間的一種非線性優化方法，其采用信賴域的思想，在信賴域內求解下降方向的最優步長。與Hector SLAM使用高斯牛頓法相比，使目標函數陷入局部極小值的機會大大減小。最后通過將本文方法與Hector SLAM算法在ROS下進行仿真實驗對比，結果表明所提方法具有較低的定位誤差和較好的地圖構建效果。

1 相關工作

占據柵格地圖是激光SLAM中常用的一種地圖表示方法，其將環境地圖劃分成等大的有限數量的網格，實際環境中的每個位置所在的網格只有兩種狀態：占據(Occupied，存在障礙物)或者空閑(Free，不存在障礙物)。環境地圖可表示為：

m={mi}

(1)

式中：mi為柵格地圖中的柵格。

對于柵格地圖，以激光的觀測數據和機器人的位姿估計每一個柵格mi在t時刻的狀態：

p(mi|z1 ∶t,x1 ∶t)

(2)

式中：z1 ∶t是激光在1 ∶t時刻的觀測數據；x1 ∶t是機器人在1 ∶t時刻的位姿。而柵格mi在t時刻的狀態由其在1 ∶t-1時刻狀態和第t時刻的觀測數據和位姿決定。對于整個地圖m，假設地圖中的每個柵格單元mi相互獨立，則可用邊緣概率的乘積近似表示整個地圖m的后驗概率：

(3)

柵格地圖的網格中存儲該網格被占據的概率值，并使用二值貝葉斯濾波方法不斷更新計算網格值，以此確定該網格最終的狀態(p=1表示占據，p=0表示空閑)。

SLAM中的定位問題可以由掃描匹配來解決，掃描匹配方法通過最大化重疊已存在地圖和當前掃描點，以求解機器人的當前位姿和上一時刻位姿間的相對平移T和旋轉θ。掃描匹配可以有效計算且高精度地估計機器人位姿。為了求解出機器人兩個時刻間的相對位姿，通常使用上一時刻位姿對當前掃描點進行旋轉變換，然后投影到已存在地圖M，并使用歐氏距離度量掃描點pi到地圖M的匹配程度。該問題是一個最小化問題，可以描述為：

(4)

式中：∏(M,q⊕pi)為變換后的點到地圖的歐氏投影。q⊕pi為掃描點通過位姿的旋轉變換：

q⊕pi=R(θ)·pi+T

(5)

式中：R(θ)為旋轉矩陣；T為平移矩陣。

為了解決上述最小化問題，研究者們提出了許多相關算法。ICP算法是最早被用作掃描匹配進行定位的算法，該算法通過搜索不同位姿下的掃描點的最近點，求出使其重合的最優剛體變換。目前已提出了眾多關于ICP的變種算法[7-8]。大多數基于ICP算法的主要缺點是每次迭代過程中搜索對應點的復雜度較高。Pedrosa等提出了一種基于似然場模型[10-11]的掃描匹配方法。Hector SLAM使用了一種將當前掃描端點與地圖值匹配的方案，這種方法無須搜索對應點。本文在Hector SLAM的基礎上，將加權SDF地圖引入到柵格地圖中，并使用L-M方法改進了Hector SLAM中求解掃描匹配問題的高斯牛頓法，最后通過仿真實驗對比，驗證了本文方法相比Hector SLAM有較好的定位和建圖效果。

2 基于加權SDF地圖的SLAM方法

掃描匹配是激光掃描點和已存在地圖間的匹配，目前的激光雷達具有低測量誤差和較高的掃描頻率，因此掃描匹配的方法對于激光傳感器來說具有比較準確的效果。求解掃描匹配問題首先需要確定地圖模型。傳統的占據柵格地圖的精度取決于柵格的大小，即地圖的分辨率，存在固有的精度問題，當柵格的大小過大時，地圖會呈現出不夠平滑的現象，會影響到機器人定位的精度。因此本文將加權SDF地圖引入柵格地圖之中，該地圖可以以低于柵格大小的精度捕獲環境，同時具有更高的定位精度。

2.1 加權SDF地圖

SDF地圖是基于占據柵格地圖之上的一種改進地圖。SDF地圖中，使用同一(或多個)柵格內的激光掃描點擬合直線來描述檢測到的物體輪廓，物體輪廓以子柵格大小的精度用SDF進行描述，SDF值計算柵格到回歸線的距離，并根據SDF值判斷當前柵格的符號，由于回歸線將穿過柵格，所計算的SDF值的絕對值將小于柵格大小，因此該地圖具有低于柵格大小的精度。如圖1所示，由柵格M中的三個激光點通過戴明回歸擬合的直線f(x)，柵格M的SDF值為其中心到直線f(x)的距離。當機器人位置P到柵格的距離大于其到直線f(x)的距離時，柵格更新為負的SDF值，反之柵格更新為正的SDF值。SDF地圖在使用符號距離函數更新物體輪廓時，使用所有時間的測量平均值。該方法可以減少高斯傳感器噪聲引起的誤差。然而在每次掃描更新時，不僅會更新掃描點所在柵格，還會更新周圍其他柵格，因此在所有柵格更新時取平均值會將噪聲引入地圖。本文引入三維重建中[14-15]中使用加權距離的思想，對SDF地圖值的每次更新進行加權處理，減少噪聲的影響，使得更新值更為平滑和有效。

圖1 WSDF地圖原理示意

在加權SDF地圖中，每次更新柵格的SDF值，即柵格距離回歸線的距離，并根據一個柵格的四個頂點所存儲的SDF值的符號判斷其是否被占據。具體規則如下：

若某柵格的四個頂點的值均為同符號(同為正值或同為負值)，則說明該柵格一直位于某回歸線的同側，即該柵格不存在障礙物。

若某柵格的四個頂點的值至少存在一個值的符號不同，則說明該網格位于某回歸線的異側，即該柵格存在障礙物。

加權SDF地圖以距離加權每次的更新值，假設某柵格的更新值為d，該柵格當前權值為w。對于權重w，計算規則如下：

(6)

式中：dmin與dmax為距離d上限值與下限值；Wmax為權值的最大值。

當前權值的大小決定了是否進行更新，決策規則如下：若w≥r%×wlast_max，則更新；反之則不進行更新。其中wlast_max為最近最大更新的權值。上述規則集成了接近受噪聲影響的最后更新的最大權值的距離值。每一時刻的更新規則如下：

(7)

為了防止W快速飽和(達到上限值Wmax)，通常令wt+1=1。

2.2 LM方法

在Hector SLAM中，使用高斯牛頓法求解掃描匹配問題[3]。假設某時刻下機器人位姿為q=(T,θ)，基于加權SDF地圖的掃描匹配問題具有如下形式[13]：

(8)

式中Si=R(θ)·di+T，表示機器人當前位姿q下掃描端點di的世界坐標。式(8)的意義在于求解使得地圖SDF值最小時機器人相鄰時刻的位姿增量。使用高斯牛頓法可以求得掃描匹配問題的解：

Δq=H-1g

(9)

(10)

(11)

(12)

當柵格不存在障礙物時，即柵格四個頂點符號相同時，地圖值及其導數可由雙線性插值法求得：

M(di)≈|y(m3x+m2(1-x))+(1-y)(m1x+m0(1-x))|

(13)

(14)

當柵格存在障礙物時，此時地圖梯度近似值是掃描端點di及其在回歸線上的正交投影h之間的相應軸的距離，地圖值近似為二者之間的歐氏距離：

(15)

由于高斯牛頓法所計算的H矩陣只有半正定性，所以在使用高斯牛頓法時可能出現H為奇異矩陣的情況，此時會造成增量的穩定性較差，導致算法不收斂。同時當所求步長Δq較大時，也會使得局部近似不夠準確。因此本文采用L-M方法求解式(8)。

L-M算法[9]是一種求解非線性最優化問題的算法，該算法屬于一種信賴域算法。其思想是首先從初始點假設一個可信賴的最大位移s，并在以當前點為中心、以s為半徑的區域內，通過尋找目標函數的一個近似函數(二次的)的最優點來求解得到真正的位移。根據求解結果計算目標函數值，判斷本次位移是否可靠，若可靠則按此規則繼續迭代；反之則應減小信賴域的范圍，放棄本次求解結果，并重新求解。

對于L-M方法，式(8)的解為：

Δq=(H+μI)-1g

(16)

當q>0時，此次迭代有效，并減小懲罰因子。

(17)

當q≤0時，此次迭代無效，需要增大懲罰因子。

(18)

L-M方法中，通過信賴域的方法保證了H+μI矩陣的非奇異性。當μ的值較小時，Δq接近高斯牛頓法的方向，μ的值很大時，Δq接近梯度下降的方向。L-M方法通過μ控制每次迭代的步長，是高斯牛頓法和梯度下降的充分結合，使得每一次迭代下降的步長更加精確，同時大大減小目標函數陷入局部極小值的情況。

3 仿真實驗

為了驗證所提出的方法的可行性，本文首先通過錄制的數據集對L-M方法和高斯牛頓法的實驗結果進行了對比，并對本文提出的方法和傳統Hector SLAM進行對比。最后進行ROS仿真實驗，通過分析算法結果的誤差、生成的軌跡和地圖的質量等方面對比了本文方法和它們之間的差異。本文使用在L-M方法和高斯牛頓法在相同的數據集上進行了實驗，兩種方法都基于占據柵格地圖。通過分析掃描匹配的均方根誤差rmse，可以度量二者掃描匹配的準確度。由于經過雙線性插值后的柵格地圖的網格值可以看作一個連續值的分布，它是該網格是否存在障礙物的概率值，因此匹配的目的即為使得式(4)的值盡可能趨近于0。圖2為不同時刻兩種方法的誤差曲線圖，可以比較兩種方法在每一時刻的誤差，其中誤差計算方式如下：

(19)

(a) 高斯牛頓法(b) L-M方法圖2 兩種方法的誤差箱型圖

圖2顯示了1 500多個時刻兩種方法的掃描匹配誤差的箱型圖，可以看出與高斯牛頓法相比，L-M方法具有較好的穩定性，且有效降低了掃描匹配誤差。這證明了L-M方法的有效性。

通過將本文方法與Hector SLAM在數據集上的實驗，對比兩種方法對機器人的位姿估計與真實軌跡，驗證了本文方法相比于Hector SLAM具有較低的定位誤差。圖3為實驗結果對比圖。

圖3 軌跡對比圖

圖4、圖5為圖3軌跡放大圖，可以看出，本文使用加權SDF地圖的方法與真實軌跡較為接近，而Hector SLAM大約存在5 cm的誤差，這在機器人發生轉向時尤為明顯。

圖4 軌跡局部放大圖1

圖5 軌跡局部放大圖2

為了更好地對所提方法進行驗證，本文通過ROS機器人操作系統，在Gazebo中使用house仿真場景對所提方法和Hector SLAM進行實驗。仿真環境中使用HLS-LFCD2激光雷達，360°激光掃描，其主要測量參數如表1所示。

表1 HLS-LFCD2激光雷達主要測量參數

通過對同一仿真環境的建圖和定位，對比分析了兩種方法的實驗結果。圖6為兩種方法的建圖結果。

圖6 Hector SLAM(左)與所提方法(右)建圖結果

可以看出，Hector SLAM在標記處出現了建圖誤差。圓圈標記處為場景房間中的大門，大門一側出現了墻壁過長的現象。同時方框標記處為場景某房間門口處，其墻體出現了錯位。而所提方法在圓圈標記處房間大門墻體平整，沒有出現墻體過長的現象。同時方框標記處墻壁沒有出現錯位的情況，構建效果較好。通過仿真結果表明，在仿真場景下改進的SLAM方法能更好地構建環境地圖。

同時本文對比了兩種方法定位結果與真實軌跡的誤差，結果如圖7所示。可以看出Hector SLAM在時間大約400 s之后出現了較大的定位誤差，由表1中可知其誤差峰值到達了0.18 m。而所提方法的誤差穩定在0.06 m以下，具有較好的定位精度。

圖7 兩種方法軌跡誤差曲線圖

表2 兩種方法誤差數據對比 單位：m

圖8、圖9為Hector SLAM與本文方法與真實軌跡的對比圖。由圖8可知，正如誤差曲線圖所示，Hector SLAM在400時刻附近進行轉向時產生了較大的誤差，且之后一直無法修正，從而影響到了對環境地圖的構建結果。而圖9中本文方法在進行多次轉向時仍能保持比較準確的定位結果，與真實軌跡基本一致，具有較好的定位精度。

圖8 Hector SLAM軌跡對比圖

圖9 本文方法軌跡對比圖

上述仿真實驗結果以及對實驗結果的分析與對比可以證明在仿真環境下，相比于傳統Hector SLAM，本文方法有效降低了機器人的定位估計誤差，并且具有較好的建圖結果。

4 結 語

本文提出了一種基于加權SDF的激光SLAM方法。該方法使用L-M法對Hector SLAM中的高斯牛頓法進行了改進。同時提出一種加權SDF地圖，并將其引入到SLAM方法中。經仿真實驗驗證，與傳統Hector SLAM相比，本文方法有效提高了位姿估計的精確性，從而能得到更精確的地圖構建，具有一定的應用價值。