適用于低端激光雷達(dá)的優(yōu)化Hector SLAM算法

蘇易衡, 張奇志， 周亞麗

(北京信息科技大學(xué) 自動化學(xué)院，北京 100192)

0 引 言

建立環(huán)境模型并且確定自身位置是移動機(jī)器人在真實場景中應(yīng)用所需的一項重要功能，實時定位與建圖(Simultaneous Localization and Mapping,SLAM)關(guān)注的正是這一問題。隨著SLAM研究的不斷深入與激光雷達(dá)性能的提升迭代，建圖與定位的精度已經(jīng)基本滿足室內(nèi)、室外環(huán)境中的應(yīng)用。例如基于改進(jìn)的Rao-Blackwellized粒子濾波SLAM方法Gmapping[1-2]，利用激光雷達(dá)數(shù)據(jù)與高精度的里程計，在辦公室環(huán)境具有較強(qiáng)的魯棒性；此外基于Gauss-Newton方法的Hector SLAM，使用較少的運(yùn)算資源，因此可用于無人機(jī)等裝置在三維場景中建立環(huán)境模型[3-4]，但該方式對激光雷達(dá)的精度與刷新頻率有很高要求，由于高性能的激光雷達(dá)價格昂貴，在許多場景中不太適用。

Hector SLAM是一種利用現(xiàn)代高精度激光雷達(dá)的數(shù)據(jù)，不需要依賴?yán)锍逃嫈?shù)據(jù)的SLAM方法。該算法的一般處理過程為[5]：獲取概率柵格地圖、Gauss-Newton法掃描匹配、使用多分辨率地圖避免陷入局部極小值。首先利用高精度激光雷達(dá)的數(shù)據(jù)，使用雙線性插值算法獲取連續(xù)的概率柵格地圖；接下來繼續(xù)獲取雷達(dá)數(shù)據(jù)，使用當(dāng)前幀與已有地圖的數(shù)據(jù)構(gòu)建誤差函數(shù)，并用Gauss-Newton法得到最優(yōu)解與偏移量；最后使用3種不同分辨率的柵格地圖，避免匹配結(jié)果陷入局部極小值。

掃地機(jī)器人、家庭服務(wù)機(jī)器人[6]等應(yīng)用場景中，經(jīng)常由于成本控制或是設(shè)備老化等原因，所使用的激光雷達(dá)性能較低。本文在Hector SLAM研究的基礎(chǔ)上，對前端進(jìn)行了較為全面的優(yōu)化，降低了算法的硬件應(yīng)用門檻，消除了算法對高精度、高刷新頻率激光雷達(dá)的依賴；同時解決了地圖構(gòu)建重影的問題，使數(shù)據(jù)噪聲較大、刷新頻率低的激光雷達(dá)在不借助里程計的條件下也能夠完成SLAM任務(wù)。

1 算法實現(xiàn)

概率柵格地圖是激光雷達(dá)描述真實世界的一種方式，經(jīng)證明能對任意環(huán)境進(jìn)行地圖表述[7]。由于激光雷達(dá)數(shù)據(jù)的離散性，無法直接用于柵格地圖的構(gòu)建與掃描匹配，所以要先通過插值獲得連續(xù)的概率柵格地圖。

獲取到柵格地圖后，需要將后續(xù)到達(dá)的雷達(dá)數(shù)據(jù)進(jìn)行掃描匹配，掃描匹配是將激光雷達(dá)掃描的實時數(shù)據(jù)與已有地圖對齊的過程，原系統(tǒng)在這一步參考了圖像對齊的方法構(gòu)建誤差函數(shù)[8]，使用Gauss-Newton法得到最為匹配的地圖。該方式在雷達(dá)數(shù)據(jù)更新后，在上一時刻狀態(tài)附近迭代出當(dāng)前時刻的最優(yōu)匹配，并計算得到位姿增量。Hector SLAM所建立的Gauss-Newton方程為:

(1-M(Si(ξ)))

(1)

ξ=(px,py,ψ)T

(2)

(3)

(4)

算法最后通過使用3種不同分辨率地圖避免計算陷入局部極小值，得到最優(yōu)解，并更新柵格地圖數(shù)據(jù)。

針對上述算法分析，結(jié)合激光SLAM的具體應(yīng)用場景，本文提出采用線段特征提取對雷達(dá)數(shù)據(jù)優(yōu)化，使用雙三次插值算法優(yōu)化地圖精度與梯度，最后通過圖像相減與矩陣變換的方式獲取激光雷達(dá)的運(yùn)動模式，并優(yōu)化關(guān)鍵幀更新機(jī)制以消除地圖重影的問題。

1.1 激光雷達(dá)精度驗證

為了驗證方法的改良效果，首先對激光雷達(dá)精度進(jìn)行驗證，所用雷達(dá)見圖1。該低性能激光雷達(dá)檢測范圍0.15～5 m，角精度1°，掃描頻率僅為5～6 Hz。

圖1 所用的激光雷達(dá)

本文獲取3 s內(nèi)激光雷達(dá)對同一入射角的12組重復(fù)測量數(shù)據(jù)，得到均值與標(biāo)準(zhǔn)差對激光雷達(dá)的檢測精度進(jìn)一步驗證。具體實驗過程如下:在相同實驗條件下，重復(fù)測量以90°為入射角的目標(biāo)點(diǎn)12次，距測量點(diǎn)正前方的距離分別為1、2和4 m，其中距離標(biāo)準(zhǔn)由高精度激光測距儀獲得。得到A=[a1,a2,…,a12]，B=[b1,b2,…,b12]和C=[c1,c2,…,c12]3組數(shù)據(jù)。根據(jù)下式計算出均值和標(biāo)準(zhǔn)差:

結(jié)果如表1所示。

表1 激光雷達(dá)性能的測試結(jié)果

1.2 數(shù)據(jù)優(yōu)化

由于低性能激光雷達(dá)的噪聲較大，為了獲取精度更高的建圖與定位效果，本文通過2種方法對地圖數(shù)據(jù)進(jìn)行優(yōu)化：利用地圖中廣泛存在直線特征數(shù)據(jù)，進(jìn)行提取與擬合，減少雷達(dá)自身精度帶來的誤差；改變概率柵格地圖的插值算法，提高地圖的精度與連續(xù)性。

1.2.1 激光雷達(dá)直線特征數(shù)據(jù)的優(yōu)化

在激光雷達(dá)檢測的二維平面中，直線、轉(zhuǎn)角等場景因為數(shù)據(jù)特征明顯，經(jīng)常作為路標(biāo)儲存并用于位姿圖優(yōu)化[9-10]。本文選擇常見并且突出的直線特征，把一組激光雷達(dá)數(shù)據(jù)作為1幀圖像矩陣來處理，通過概率霍夫變換提取并由最小二乘法擬合更新直線，對雷達(dá)數(shù)據(jù)優(yōu)化來提高地圖構(gòu)建與匹配的精度。事實證明,對于檢測距離較短的激光雷達(dá)，遠(yuǎn)端的直線數(shù)據(jù)得到明顯的平滑。在后續(xù)的旋轉(zhuǎn)判定實驗中，能觀察到直線特征數(shù)據(jù)中的明顯噪點(diǎn)得到了消除。

在直角坐標(biāo)系中可以用斜率和截距來表示1條直線，即

y=kx+b

(8)

式中,(b,k)表示直線參數(shù)空間的一個點(diǎn)，但由于存在k為無窮大的情況，通常將直線轉(zhuǎn)換為極坐標(biāo)系表示,

ρ=xcosθ+ysinθ

(9)

式中,極徑ρ和極角θ為參數(shù)空間。則可得每一對(ρ,θ)與一條通過(x,y)的直線相關(guān)聯(lián)，ρ、θ參數(shù)空間即為霍夫空間。如在直角坐標(biāo)系中存在一條直線，則對應(yīng)在霍夫空間中有多個點(diǎn)重合，根據(jù)設(shè)定的閾值可檢測出直線[11]。由于霍夫直線變換的運(yùn)算效率低且無法測得線段端點(diǎn)，不適合用于雷達(dá)數(shù)據(jù)處理，所以本文采用概率霍夫變換來檢測直線。概率霍夫直線變換的3個步驟[12]：

(1) 把激光雷達(dá)數(shù)據(jù)儲存為圖像矩陣的形式，從圖像中隨機(jī)抽取一個數(shù)據(jù)點(diǎn)，映射到極坐標(biāo)系并得到曲線。

(2) 當(dāng)極坐標(biāo)系里有交點(diǎn)滿足最小投票數(shù),即霍夫閾值，就得到了該點(diǎn)所對應(yīng)的一條直線L。

(3) 獲取圖像在直線L上的點(diǎn)，連成線段并記錄該線段的2個端點(diǎn)。刪除該直線L上所有的數(shù)據(jù)點(diǎn)并返回步驟(1)，直到遍歷圖像中所有的數(shù)據(jù)點(diǎn)。

通過對概率霍夫變換相關(guān)參數(shù)的調(diào)整，可以保留或消除地圖中部分障礙物信息，例如靠墻的桌腿，參數(shù)值可根據(jù)實際情況來設(shè)定。在得到圖像中所有的直線數(shù)據(jù)以后，利用最小二乘法來獲取最佳擬合直線。假設(shè)擬合直線方程為y=kx+b，對于任意樣本(xi,yi)，根據(jù)以下公式得到誤差：

e=yi-(kxi+b)

(10)

接下來計算偏差的平方和S，當(dāng)S最小時,直線的擬合度最高，

(11)

至此完成對激光雷達(dá)數(shù)據(jù)的處理，對真實環(huán)境中的直線特征數(shù)據(jù)進(jìn)行了優(yōu)化。該方法減小了特征數(shù)據(jù)的噪聲，并使遠(yuǎn)端較為稀疏的雷達(dá)數(shù)據(jù)得到明顯的平滑。

1.2.2 優(yōu)化概率柵格地圖

在柵格地圖中，每個柵格都由0、1數(shù)值來表示被障礙占據(jù)的概率，值越接近1表示該處有障礙的概率越大。因為激光雷達(dá)獲取的數(shù)據(jù)是離散的，而系統(tǒng)需要構(gòu)建連續(xù)的柵格地圖，并且掃描匹配時偏導(dǎo)運(yùn)算也需要數(shù)據(jù)具有連續(xù)性。由于低性能的激光雷達(dá)角精度較低，有效數(shù)據(jù)量少，故使用雙三次插值算法來獲取連續(xù)性更高的概率柵格地圖，以減少系統(tǒng)噪聲、提高后續(xù)使用Gauss-Newton法進(jìn)行地圖匹配時的準(zhǔn)確性[13]。

雙三次插值算法要選取插值點(diǎn)周圍的16個采樣點(diǎn)進(jìn)行加權(quán)[14]，假設(shè)插值點(diǎn)位于(i+u,j+v)，則(i,j)為插值點(diǎn)的整數(shù)部分;u、v分別是插值點(diǎn)在X、Y軸上的小數(shù)部分。算法選取的是以點(diǎn)(i-1,j-1)到(i+2,j+2)為對角線的矩形網(wǎng)格中的16個采樣點(diǎn)數(shù)據(jù)：

(12)

算法首先構(gòu)造用于對臨近值加權(quán)的雙三次插值核函數(shù)S，

S(x)=

(13)

式中,參數(shù)a在本系統(tǒng)中選值為-0.5[15]。而

f(i+u,j+v)=ABCT

(14)

是雙三次插值的計算方法，等號左側(cè)為算法得到的插值數(shù)值，等號右側(cè)為計算過程，運(yùn)算后可以得到插值點(diǎn)的數(shù)值。其中:

A=

(S(1+u)S(u)S(1-u)S(2-u))

(15)

C=

(S(1+v)S(v)S(1-v)S(2-v))

(16)

由于改變了柵格地圖的插值算法，Gauss-Newton法迭代時使用的地圖梯度也同時得到了優(yōu)化。需要對式(14)求X、Y軸方向的偏導(dǎo):

?f/?u≈A′BCT

(17)

?f/?v≈ABC′T

(18)

把u、v代入

(19)

1.3 地圖重影的解決

激光雷達(dá)的數(shù)據(jù)優(yōu)化可以有效減少系統(tǒng)噪聲，獲取精度更高的地圖數(shù)據(jù)，但是地圖構(gòu)建時的重影問題并不能得到有效解決。低性能雷達(dá)在建圖過程中經(jīng)常會發(fā)生重影的問題，是由于雷達(dá)數(shù)據(jù)更新慢、連續(xù)性差導(dǎo)致的幀間偏移過大，數(shù)據(jù)被系統(tǒng)判定為關(guān)鍵幀更新至地圖。為了確定重影發(fā)生的原因，對激光雷達(dá)的運(yùn)動模式進(jìn)行分析。激光雷達(dá)在移動機(jī)器人上具有3個自由度，分別為X、Y軸上的平移與繞Z軸的旋轉(zhuǎn)，也就可以將其移動模式歸結(jié)為平移與旋轉(zhuǎn)兩種。經(jīng)過測試，低性能激光雷達(dá)在平移時地圖不會出現(xiàn)重影；但在旋轉(zhuǎn)時造成的地圖重影問題十分嚴(yán)重，這就需要對旋轉(zhuǎn)運(yùn)動進(jìn)行判定，達(dá)到消除重影的目的。本文根據(jù)平移與旋轉(zhuǎn)時，激光雷達(dá)數(shù)據(jù)的變化特性，應(yīng)用圖像處理中常用的方法即圖像相減與矩陣變換，來確定激光雷達(dá)的運(yùn)動模式，最后優(yōu)化關(guān)鍵幀的發(fā)布機(jī)制，消除了地圖重影的問題。

1.4 運(yùn)動檢測

在概率霍夫變換的步驟中，已經(jīng)以二值圖像的形式存儲了雷達(dá)數(shù)據(jù)。本文應(yīng)用圖像相減的方法，將當(dāng)前時刻的雷達(dá)圖像與上1幀數(shù)據(jù)圖像相減，再計算差值圖像中非零數(shù)據(jù)點(diǎn)的個數(shù)，就可以判斷出激光雷達(dá)是否運(yùn)動。因為低刷新頻率的雷達(dá)在旋轉(zhuǎn)運(yùn)動時會產(chǎn)生地圖重影，所以當(dāng)檢測出雷達(dá)發(fā)生運(yùn)動后，需要對旋轉(zhuǎn)模式進(jìn)一步判定。當(dāng)沒有檢查到位移時，不進(jìn)行旋轉(zhuǎn)判定，以節(jié)省計算資源。

1.5 旋轉(zhuǎn)判定

激光雷達(dá)發(fā)送的數(shù)據(jù)為(ρ,θ)形式的極坐標(biāo)數(shù)據(jù)，即角度與距離數(shù)據(jù)。當(dāng)激光雷達(dá)發(fā)生旋轉(zhuǎn)時，周圍障礙物與激光雷達(dá)之間的距離數(shù)據(jù)不會有較大的抖動，改變的只是障礙物相對激光雷達(dá)的角度。假設(shè)激光雷達(dá)上1幀數(shù)據(jù)為(ρ,θ)，當(dāng)前幀較上1幀繞正方向旋轉(zhuǎn)了α°，則理想情況下新的雷達(dá)數(shù)據(jù)可以表示為(ρ,θ-α) 。根據(jù)激光雷達(dá)數(shù)據(jù)的該特性，便可求出其旋轉(zhuǎn)角度。

當(dāng)系統(tǒng)識別出激光雷達(dá)的運(yùn)動模式為旋轉(zhuǎn)時，將旋轉(zhuǎn)前的雷達(dá)數(shù)據(jù)傳遞至掃描匹配即可；同時可以在系統(tǒng)中增加適當(dāng)?shù)难訒r，避免機(jī)器人旋轉(zhuǎn)抖動造成的誤差，增加系統(tǒng)魯棒性。在引入旋轉(zhuǎn)判定機(jī)制前，激光雷達(dá)的旋轉(zhuǎn)會造成如圖2所示的地圖重影；優(yōu)化判定機(jī)制后，雷達(dá)正常旋轉(zhuǎn)造成的干擾已經(jīng)消除，如圖3所示。實驗驗證該方法可以消除激光雷達(dá)旋轉(zhuǎn)造成地圖重影的問題。

圖2 優(yōu)化前旋轉(zhuǎn)雷達(dá)造成重影

圖3 優(yōu)化后消除旋轉(zhuǎn)帶來重影的問題

2 實驗結(jié)果

針對本文提出的優(yōu)化方法，通過移動機(jī)器人搭載低性能激光雷達(dá)，在大小為7.2 m×5.0 m的實驗環(huán)境進(jìn)行驗證。實驗環(huán)境與機(jī)器人平臺如圖4、5所示。兩次實驗中，機(jī)器人均以0.3 m/s的速度繞場地運(yùn)行，旋轉(zhuǎn)的角速度約為1 rad/s，且所建立的地圖分辨率均為5 cm。

圖4 實驗環(huán)境

圖5 機(jī)器人平臺

首先機(jī)器人使用未加入旋轉(zhuǎn)判定的算法，在平移運(yùn)動時能夠正常建圖，但在轉(zhuǎn)彎處出現(xiàn)地圖重影的問題。機(jī)器人的定位出現(xiàn)誤差并且不能修復(fù)，無法得到正確的地圖，如圖6所示。接下來使用完整的優(yōu)化算法，機(jī)器人在4個轉(zhuǎn)彎處都能保持正確的建圖，地圖出現(xiàn)重影的問題已經(jīng)解決，見圖7。使用本文算法所構(gòu)建的地圖可以準(zhǔn)確地反映地圖中擋板、垃圾桶、墻面等有效信息，減少了地圖噪聲并且明顯平滑了地圖中的直線特征數(shù)據(jù)。

圖6 機(jī)器人的旋轉(zhuǎn)造成地圖重影

圖7 優(yōu)化后常規(guī)的旋轉(zhuǎn)不再影響建圖

3 結(jié) 語

基于本文優(yōu)化后的Hector SLAM算法，使用低成本的激光雷達(dá)也可以得到準(zhǔn)確的地圖數(shù)據(jù)并消除了地圖重影的問題，同時在構(gòu)建全局地圖時直線特征得到明顯平滑，有效減少了系統(tǒng)噪聲。由于低性能激光雷達(dá)的數(shù)據(jù)質(zhì)量較低、每幀數(shù)據(jù)間隔較長，為了提高SLAM算法的實時性與準(zhǔn)確性，可以在系統(tǒng)中引入里程計并使用位姿數(shù)據(jù)，實現(xiàn)SLAM的后端優(yōu)化與回環(huán)檢測，達(dá)到優(yōu)化建圖效果的目的。