蘇易衡, 張奇志, 周亞麗
(北京信息科技大學(xué) 自動(dòng)化學(xué)院,北京 100192)
建立環(huán)境模型并且確定自身位置是移動(dòng)機(jī)器人在真實(shí)場景中應(yīng)用所需的一項(xiàng)重要功能,實(shí)時(shí)定位與建圖(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ù)與高精度的里程計(jì),在辦公室環(huán)境具有較強(qiáng)的魯棒性;此外基于Gauss-Newton方法的Hector SLAM,使用較少的運(yùn)算資源,因此可用于無人機(jī)等裝置在三維場景中建立環(huán)境模型[3-4],但該方式對激光雷達(dá)的精度與刷新頻率有很高要求,由于高性能的激光雷達(dá)價(jià)格昂貴,在許多場景中不太適用。
Hector SLAM是一種利用現(xiàn)代高精度激光雷達(dá)的數(shù)據(jù),不需要依賴?yán)锍逃?jì)數(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á)的依賴;同時(shí)解決了地圖構(gòu)建重影的問題,使數(shù)據(jù)噪聲較大、刷新頻率低的激光雷達(dá)在不借助里程計(jì)的條件下也能夠完成SLAM任務(wù)。
概率柵格地圖是激光雷達(dá)描述真實(shí)世界的一種方式,經(jīng)證明能對任意環(huán)境進(jìn)行地圖表述[7]。由于激光雷達(dá)數(shù)據(jù)的離散性,無法直接用于柵格地圖的構(gòu)建與掃描匹配,所以要先通過插值獲得連續(xù)的概率柵格地圖。
獲取到柵格地圖后,需要將后續(xù)到達(dá)的雷達(dá)數(shù)據(jù)進(jìn)行掃描匹配,掃描匹配是將激光雷達(dá)掃描的實(shí)時(shí)數(shù)據(jù)與已有地圖對齊的過程,原系統(tǒng)在這一步參考了圖像對齊的方法構(gòu)建誤差函數(shù)[8],使用Gauss-Newton法得到最為匹配的地圖。該方式在雷達(dá)數(shù)據(jù)更新后,在上一時(shí)刻狀態(tài)附近迭代出當(dāng)前時(shí)刻的最優(yōu)匹配,并計(jì)算得到位姿增量。Hector SLAM所建立的Gauss-Newton方程為:
(1-M(Si(ξ)))
(1)
ξ=(px,py,ψ)T
(2)
(3)
(4)
算法最后通過使用3種不同分辨率地圖避免計(jì)算陷入局部極小值,得到最優(yōu)解,并更新柵格地圖數(shù)據(jù)。
針對上述算法分析,結(jié)合激光SLAM的具體應(yīng)用場景,本文提出采用線段特征提取對雷達(dá)數(shù)據(jù)優(yōu)化,使用雙三次插值算法優(yōu)化地圖精度與梯度,最后通過圖像相減與矩陣變換的方式獲取激光雷達(dá)的運(yùn)動(dòng)模式,并優(yōu)化關(guān)鍵幀更新機(jī)制以消除地圖重影的問題。
為了驗(yàn)證方法的改良效果,首先對激光雷達(dá)精度進(jìn)行驗(yà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)一步驗(yàn)證。具體實(shí)驗(yàn)過程如下:在相同實(shí)驗(yà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ù)下式計(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)角等場景因?yàn)閿?shù)據(jù)特征明顯,經(jīng)常作為路標(biāo)儲(chǔ)存并用于位姿圖優(yōu)化[9-10]。本文選擇常見并且突出的直線特征,把一組激光雷達(dá)數(shù)據(jù)作為1幀圖像矩陣來處理,通過概率霍夫變換提取并由最小二乘法擬合更新直線,對雷達(dá)數(shù)據(jù)優(yōu)化來提高地圖構(gòu)建與匹配的精度。事實(shí)證明,對于檢測距離較短的激光雷達(dá),遠(yuǎn)端的直線數(shù)據(jù)得到明顯的平滑。在后續(xù)的旋轉(zhuǎn)判定實(shí)驗(yàn)中,能觀察到直線特征數(shù)據(jù)中的明顯噪點(diǎn)得到了消除。
在直角坐標(biāo)系中可以用斜率和截距來表示1條直線,即
y=kx+b
(8)
式中,(b,k)表示直線參數(shù)空間的一個(gè)點(diǎn),但由于存在k為無窮大的情況,通常將直線轉(zhuǎn)換為極坐標(biāo)系表示,
ρ=xcosθ+ysinθ
(9)
式中,極徑ρ和極角θ為參數(shù)空間。則可得每一對(ρ,θ)與一條通過(x,y)的直線相關(guān)聯(lián),ρ、θ參數(shù)空間即為霍夫空間。如在直角坐標(biāo)系中存在一條直線,則對應(yīng)在霍夫空間中有多個(gè)點(diǎn)重合,根據(jù)設(shè)定的閾值可檢測出直線[11]。由于霍夫直線變換的運(yùn)算效率低且無法測得線段端點(diǎn),不適合用于雷達(dá)數(shù)據(jù)處理,所以本文采用概率霍夫變換來檢測直線。概率霍夫直線變換的3個(gè)步驟[12]:
(1) 把激光雷達(dá)數(shù)據(jù)儲(chǔ)存為圖像矩陣的形式,從圖像中隨機(jī)抽取一個(gè)數(shù)據(jù)點(diǎn),映射到極坐標(biāo)系并得到曲線。
(2) 當(dāng)極坐標(biāo)系里有交點(diǎn)滿足最小投票數(shù),即霍夫閾值,就得到了該點(diǎn)所對應(yīng)的一條直線L。
(3) 獲取圖像在直線L上的點(diǎn),連成線段并記錄該線段的2個(gè)端點(diǎn)。刪除該直線L上所有的數(shù)據(jù)點(diǎn)并返回步驟(1),直到遍歷圖像中所有的數(shù)據(jù)點(diǎn)。
通過對概率霍夫變換相關(guān)參數(shù)的調(diào)整,可以保留或消除地圖中部分障礙物信息,例如靠墻的桌腿,參數(shù)值可根據(jù)實(shí)際情況來設(shè)定。在得到圖像中所有的直線數(shù)據(jù)以后,利用最小二乘法來獲取最佳擬合直線。假設(shè)擬合直線方程為y=kx+b,對于任意樣本(xi,yi),根據(jù)以下公式得到誤差:
e=yi-(kxi+b)
(10)
接下來計(jì)算偏差的平方和S,當(dāng)S最小時(shí),直線的擬合度最高,
(11)
至此完成對激光雷達(dá)數(shù)據(jù)的處理,對真實(shí)環(huán)境中的直線特征數(shù)據(jù)進(jìn)行了優(yōu)化。該方法減小了特征數(shù)據(jù)的噪聲,并使遠(yuǎn)端較為稀疏的雷達(dá)數(shù)據(jù)得到明顯的平滑。
1.2.2 優(yōu)化概率柵格地圖
在柵格地圖中,每個(gè)柵格都由0、1數(shù)值來表示被障礙占據(jù)的概率,值越接近1表示該處有障礙的概率越大。因?yàn)榧す饫走_(dá)獲取的數(shù)據(jù)是離散的,而系統(tǒng)需要構(gòu)建連續(xù)的柵格地圖,并且掃描匹配時(shí)偏導(dǎo)運(yùn)算也需要數(shù)據(jù)具有連續(xù)性。由于低性能的激光雷達(dá)角精度較低,有效數(shù)據(jù)量少,故使用雙三次插值算法來獲取連續(xù)性更高的概率柵格地圖,以減少系統(tǒng)噪聲、提高后續(xù)使用Gauss-Newton法進(jìn)行地圖匹配時(shí)的準(zhǔn)確性[13]。
雙三次插值算法要選取插值點(diǎn)周圍的16個(gè)采樣點(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個(gè)采樣點(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)
是雙三次插值的計(jì)算方法,等號左側(cè)為算法得到的插值數(shù)值,等號右側(cè)為計(jì)算過程,運(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法迭代時(shí)使用的地圖梯度也同時(shí)得到了優(yōu)化。需要對式(14)求X、Y軸方向的偏導(dǎo):
?f/?u≈A′BCT
(17)
?f/?v≈ABC′T
(18)
把u、v代入
(19)
激光雷達(dá)的數(shù)據(jù)優(yōu)化可以有效減少系統(tǒng)噪聲,獲取精度更高的地圖數(shù)據(jù),但是地圖構(gòu)建時(shí)的重影問題并不能得到有效解決。低性能雷達(dá)在建圖過程中經(jīng)常會(huì)發(fā)生重影的問題,是由于雷達(dá)數(shù)據(jù)更新慢、連續(xù)性差導(dǎo)致的幀間偏移過大,數(shù)據(jù)被系統(tǒng)判定為關(guān)鍵幀更新至地圖。為了確定重影發(fā)生的原因,對激光雷達(dá)的運(yùn)動(dòng)模式進(jìn)行分析。激光雷達(dá)在移動(dòng)機(jī)器人上具有3個(gè)自由度,分別為X、Y軸上的平移與繞Z軸的旋轉(zhuǎn),也就可以將其移動(dòng)模式歸結(jié)為平移與旋轉(zhuǎn)兩種。經(jīng)過測試,低性能激光雷達(dá)在平移時(shí)地圖不會(huì)出現(xiàn)重影;但在旋轉(zhuǎn)時(shí)造成的地圖重影問題十分嚴(yán)重,這就需要對旋轉(zhuǎn)運(yùn)動(dòng)進(jìn)行判定,達(dá)到消除重影的目的。本文根據(jù)平移與旋轉(zhuǎn)時(shí),激光雷達(dá)數(shù)據(jù)的變化特性,應(yīng)用圖像處理中常用的方法即圖像相減與矩陣變換,來確定激光雷達(dá)的運(yùn)動(dòng)模式,最后優(yōu)化關(guān)鍵幀的發(fā)布機(jī)制,消除了地圖重影的問題。
在概率霍夫變換的步驟中,已經(jīng)以二值圖像的形式存儲(chǔ)了雷達(dá)數(shù)據(jù)。本文應(yīng)用圖像相減的方法,將當(dāng)前時(shí)刻的雷達(dá)圖像與上1幀數(shù)據(jù)圖像相減,再計(jì)算差值圖像中非零數(shù)據(jù)點(diǎn)的個(gè)數(shù),就可以判斷出激光雷達(dá)是否運(yùn)動(dòng)。因?yàn)榈退⑿骂l率的雷達(dá)在旋轉(zhuǎn)運(yùn)動(dòng)時(shí)會(huì)產(chǎn)生地圖重影,所以當(dāng)檢測出雷達(dá)發(fā)生運(yùn)動(dòng)后,需要對旋轉(zhuǎn)模式進(jìn)一步判定。當(dāng)沒有檢查到位移時(shí),不進(jìn)行旋轉(zhuǎn)判定,以節(jié)省計(jì)算資源。
激光雷達(dá)發(fā)送的數(shù)據(jù)為(ρ,θ)形式的極坐標(biāo)數(shù)據(jù),即角度與距離數(shù)據(jù)。當(dāng)激光雷達(dá)發(fā)生旋轉(zhuǎn)時(shí),周圍障礙物與激光雷達(dá)之間的距離數(shù)據(jù)不會(huì)有較大的抖動(dòng),改變的只是障礙物相對激光雷達(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)動(dòng)模式為旋轉(zhuǎn)時(shí),將旋轉(zhuǎn)前的雷達(dá)數(shù)據(jù)傳遞至掃描匹配即可;同時(shí)可以在系統(tǒng)中增加適當(dāng)?shù)难訒r(shí),避免機(jī)器人旋轉(zhuǎn)抖動(dòng)造成的誤差,增加系統(tǒng)魯棒性。在引入旋轉(zhuǎn)判定機(jī)制前,激光雷達(dá)的旋轉(zhuǎn)會(huì)造成如圖2所示的地圖重影;優(yōu)化判定機(jī)制后,雷達(dá)正常旋轉(zhuǎn)造成的干擾已經(jīng)消除,如圖3所示。實(shí)驗(yàn)驗(yàn)證該方法可以消除激光雷達(dá)旋轉(zhuǎn)造成地圖重影的問題。
圖2 優(yōu)化前旋轉(zhuǎn)雷達(dá)造成重影
圖3 優(yōu)化后消除旋轉(zhuǎn)帶來重影的問題
針對本文提出的優(yōu)化方法,通過移動(dòng)機(jī)器人搭載低性能激光雷達(dá),在大小為7.2 m×5.0 m的實(shí)驗(yàn)環(huán)境進(jìn)行驗(yàn)證。實(shí)驗(yàn)環(huán)境與機(jī)器人平臺(tái)如圖4、5所示。兩次實(shí)驗(yàn)中,機(jī)器人均以0.3 m/s的速度繞場地運(yùn)行,旋轉(zhuǎn)的角速度約為1 rad/s,且所建立的地圖分辨率均為5 cm。
圖4 實(shí)驗(yàn)環(huán)境
圖5 機(jī)器人平臺(tái)
首先機(jī)器人使用未加入旋轉(zhuǎn)判定的算法,在平移運(yùn)動(dòng)時(shí)能夠正常建圖,但在轉(zhuǎn)彎處出現(xiàn)地圖重影的問題。機(jī)器人的定位出現(xiàn)誤差并且不能修復(fù),無法得到正確的地圖,如圖6所示。接下來使用完整的優(yōu)化算法,機(jī)器人在4個(gè)轉(zhuǎn)彎處都能保持正確的建圖,地圖出現(xiàn)重影的問題已經(jīng)解決,見圖7。使用本文算法所構(gòu)建的地圖可以準(zhǔn)確地反映地圖中擋板、垃圾桶、墻面等有效信息,減少了地圖噪聲并且明顯平滑了地圖中的直線特征數(shù)據(jù)。
圖6 機(jī)器人的旋轉(zhuǎn)造成地圖重影
圖7 優(yōu)化后常規(guī)的旋轉(zhuǎn)不再影響建圖
基于本文優(yōu)化后的Hector SLAM算法,使用低成本的激光雷達(dá)也可以得到準(zhǔn)確的地圖數(shù)據(jù)并消除了地圖重影的問題,同時(shí)在構(gòu)建全局地圖時(shí)直線特征得到明顯平滑,有效減少了系統(tǒng)噪聲。由于低性能激光雷達(dá)的數(shù)據(jù)質(zhì)量較低、每幀數(shù)據(jù)間隔較長,為了提高SLAM算法的實(shí)時(shí)性與準(zhǔn)確性,可以在系統(tǒng)中引入里程計(jì)并使用位姿數(shù)據(jù),實(shí)現(xiàn)SLAM的后端優(yōu)化與回環(huán)檢測,達(dá)到優(yōu)化建圖效果的目的。