夏 天 任 彧

(杭州電子科技大學(xué)計(jì)算機(jī)學(xué)院 浙江 杭州 310016)

0 引 言

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

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

1 相關(guān)工作

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

m={mi}

(1)

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

對(duì)于柵格地圖，以激光的觀測數(shù)據(jù)和機(jī)器人的位姿估計(jì)每一個(gè)柵格mi在t時(shí)刻的狀態(tài)：

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

(2)

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

(3)

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

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

(4)

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

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

(5)

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

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

2 基于加權(quán)SDF地圖的SLAM方法

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

2.1 加權(quán)SDF地圖

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

圖1 WSDF地圖原理示意

在加權(quán)SDF地圖中，每次更新柵格的SDF值，即柵格距離回歸線的距離，并根據(jù)一個(gè)柵格的四個(gè)頂點(diǎn)所存儲(chǔ)的SDF值的符號(hào)判斷其是否被占據(jù)。具體規(guī)則如下：

若某柵格的四個(gè)頂點(diǎn)的值均為同符號(hào)(同為正值或同為負(fù)值)，則說明該柵格一直位于某回歸線的同側(cè)，即該柵格不存在障礙物。

若某柵格的四個(gè)頂點(diǎn)的值至少存在一個(gè)值的符號(hào)不同，則說明該網(wǎng)格位于某回歸線的異側(cè)，即該柵格存在障礙物。

加權(quán)SDF地圖以距離加權(quán)每次的更新值，假設(shè)某柵格的更新值為d，該柵格當(dāng)前權(quán)值為w。對(duì)于權(quán)重w，計(jì)算規(guī)則如下：

(6)

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

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

(7)

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

2.2 LM方法

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

(8)

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

Δq=H-1g

(9)

(10)

(11)

(12)

當(dāng)柵格不存在障礙物時(shí)，即柵格四個(gè)頂點(diǎn)符號(hào)相同時(shí)，地圖值及其導(dǎo)數(shù)可由雙線性插值法求得：

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

(13)

(14)

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

(15)

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

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

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

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

(16)

當(dāng)q>0時(shí)，此次迭代有效，并減小懲罰因子。

(17)

當(dāng)q≤0時(shí)，此次迭代無效，需要增大懲罰因子。

(18)

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

3 仿真實(shí)驗(yàn)

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

(19)

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

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

通過將本文方法與Hector SLAM在數(shù)據(jù)集上的實(shí)驗(yàn)，對(duì)比兩種方法對(duì)機(jī)器人的位姿估計(jì)與真實(shí)軌跡，驗(yàn)證了本文方法相比于Hector SLAM具有較低的定位誤差。圖3為實(shí)驗(yàn)結(jié)果對(duì)比圖。

圖3 軌跡對(duì)比圖

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

圖4 軌跡局部放大圖1

圖5 軌跡局部放大圖2

為了更好地對(duì)所提方法進(jìn)行驗(yàn)證，本文通過ROS機(jī)器人操作系統(tǒng)，在Gazebo中使用house仿真場景對(duì)所提方法和Hector SLAM進(jìn)行實(shí)驗(yàn)。仿真環(huán)境中使用HLS-LFCD2激光雷達(dá)，360°激光掃描，其主要測量參數(shù)如表1所示。

表1 HLS-LFCD2激光雷達(dá)主要測量參數(shù)

通過對(duì)同一仿真環(huán)境的建圖和定位，對(duì)比分析了兩種方法的實(shí)驗(yàn)結(jié)果。圖6為兩種方法的建圖結(jié)果。

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

可以看出，Hector SLAM在標(biāo)記處出現(xiàn)了建圖誤差。圓圈標(biāo)記處為場景房間中的大門，大門一側(cè)出現(xiàn)了墻壁過長的現(xiàn)象。同時(shí)方框標(biāo)記處為場景某房間門口處，其墻體出現(xiàn)了錯(cuò)位。而所提方法在圓圈標(biāo)記處房間大門墻體平整，沒有出現(xiàn)墻體過長的現(xiàn)象。同時(shí)方框標(biāo)記處墻壁沒有出現(xiàn)錯(cuò)位的情況，構(gòu)建效果較好。通過仿真結(jié)果表明，在仿真場景下改進(jìn)的SLAM方法能更好地構(gòu)建環(huán)境地圖。

同時(shí)本文對(duì)比了兩種方法定位結(jié)果與真實(shí)軌跡的誤差，結(jié)果如圖7所示?？梢钥闯鯤ector SLAM在時(shí)間大約400 s之后出現(xiàn)了較大的定位誤差，由表1中可知其誤差峰值到達(dá)了0.18 m。而所提方法的誤差穩(wěn)定在0.06 m以下，具有較好的定位精度。

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

表2 兩種方法誤差數(shù)據(jù)對(duì)比 單位：m

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

圖8 Hector SLAM軌跡對(duì)比圖

圖9 本文方法軌跡對(duì)比圖

上述仿真實(shí)驗(yàn)結(jié)果以及對(duì)實(shí)驗(yàn)結(jié)果的分析與對(duì)比可以證明在仿真環(huán)境下，相比于傳統(tǒng)Hector SLAM，本文方法有效降低了機(jī)器人的定位估計(jì)誤差，并且具有較好的建圖結(jié)果。

4 結(jié) 語

本文提出了一種基于加權(quán)SDF的激光SLAM方法。該方法使用L-M法對(duì)Hector SLAM中的高斯牛頓法進(jìn)行了改進(jìn)。同時(shí)提出一種加權(quán)SDF地圖，并將其引入到SLAM方法中。經(jīng)仿真實(shí)驗(yàn)驗(yàn)證，與傳統(tǒng)Hector SLAM相比，本文方法有效提高了位姿估計(jì)的精確性，從而能得到更精確的地圖構(gòu)建，具有一定的應(yīng)用價(jià)值。