適用于低端激光雷達的優化Hector SLAM算法

蘇易衡, 張奇志， 周亞麗

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

0 引 言

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

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

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

1 算法實現

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

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

(1-M(Si(ξ)))

(1)

ξ=(px,py,ψ)T

(2)

(3)

(4)

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

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

1.1 激光雷達精度驗證

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

圖1 所用的激光雷達

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

結果如表1所示。

表1 激光雷達性能的測試結果

1.2 數據優化

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

1.2.1 激光雷達直線特征數據的優化

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

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

y=kx+b

(8)

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

ρ=xcosθ+ysinθ

(9)

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

(1) 把激光雷達數據儲存為圖像矩陣的形式，從圖像中隨機抽取一個數據點，映射到極坐標系并得到曲線。

(2) 當極坐標系里有交點滿足最小投票數,即霍夫閾值，就得到了該點所對應的一條直線L。

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

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

e=yi-(kxi+b)

(10)

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

(11)

至此完成對激光雷達數據的處理，對真實環境中的直線特征數據進行了優化。該方法減小了特征數據的噪聲，并使遠端較為稀疏的雷達數據得到明顯的平滑。

1.2.2 優化概率柵格地圖

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

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

(12)

算法首先構造用于對臨近值加權的雙三次插值核函數S，

S(x)=

(13)

式中,參數a在本系統中選值為-0.5[15]。而

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

(14)

是雙三次插值的計算方法，等號左側為算法得到的插值數值，等號右側為計算過程，運算后可以得到插值點的數值。其中:

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法迭代時使用的地圖梯度也同時得到了優化。需要對式(14)求X、Y軸方向的偏導:

?f/?u≈A′BCT

(17)

?f/?v≈ABC′T

(18)

把u、v代入

(19)

1.3 地圖重影的解決

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

1.4 運動檢測

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

1.5 旋轉判定

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

當系統識別出激光雷達的運動模式為旋轉時，將旋轉前的雷達數據傳遞至掃描匹配即可；同時可以在系統中增加適當的延時，避免機器人旋轉抖動造成的誤差，增加系統魯棒性。在引入旋轉判定機制前，激光雷達的旋轉會造成如圖2所示的地圖重影；優化判定機制后，雷達正常旋轉造成的干擾已經消除，如圖3所示。實驗驗證該方法可以消除激光雷達旋轉造成地圖重影的問題。

圖2 優化前旋轉雷達造成重影

圖3 優化后消除旋轉帶來重影的問題

2 實驗結果

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

圖4 實驗環境

圖5 機器人平臺

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

圖6 機器人的旋轉造成地圖重影

圖7 優化后常規的旋轉不再影響建圖

3 結 語

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