叢佩超,呂昆峰,周加超
(廣西科技大學(xué)機(jī)械與交通工程學(xué)院,廣西柳州 545006))
隨著傳感器技術(shù)的日益提升,室內(nèi)場(chǎng)景下的移動(dòng)式機(jī)器人集成了地圖構(gòu)建、導(dǎo)航定位、路徑規(guī)劃等諸多功能,其應(yīng)用范圍日益擴(kuò)大[1,2]。
在復(fù)雜的室內(nèi)環(huán)境中,移動(dòng)式機(jī)器人實(shí)現(xiàn)高精度導(dǎo)航的前提是構(gòu)建優(yōu)質(zhì)的先驗(yàn)地圖。因此,提高同時(shí)定位和地圖(Simultaneous Localization And Mapping,SLAM)構(gòu)建精度一直是機(jī)器人研究領(lǐng)域中的一個(gè)開(kāi)放性研究熱點(diǎn)[3,4]。2016年,Sanjiv Singh等人[5]提出了一種實(shí)時(shí)和低漂移激光雷達(dá)建圖方法,實(shí)現(xiàn)了機(jī)器人位姿運(yùn)動(dòng)估計(jì)的低漂移和低計(jì)算復(fù)雜度。2018年,Zhongze Liu等人[6]提出了一種實(shí)時(shí)6維度激光建圖定位方法,有效地降低了激光測(cè)深位姿累積誤差,并具有良好的建圖性能及有助于機(jī)器人完成路徑規(guī)劃。2020年,F(xiàn)UYU NIE等人[7]提出了一種改進(jìn)的激光回環(huán)檢測(cè)和修正系統(tǒng)通過(guò)對(duì)存在高掃描匹配誤差的子圖進(jìn)行分割,減少了地圖內(nèi)部的誤差。
提高室內(nèi)復(fù)雜場(chǎng)景下的機(jī)器人導(dǎo)航精度,需要降低實(shí)時(shí)建圖與定位的累積誤差。其方法有多種,文獻(xiàn)[8]采用GPS衛(wèi)星定位導(dǎo)航技術(shù),可在地圖上獲得機(jī)器人的移動(dòng)軌跡并觀察到機(jī)器人所在的位置,但是容易受到數(shù)據(jù)網(wǎng)絡(luò)波動(dòng)影響,甚至搜星困難,導(dǎo)致機(jī)器人導(dǎo)航定位誤差過(guò)大。文獻(xiàn)[9]提出一種無(wú)源RFID(Radio Frequency Identification,射頻識(shí)別)的室內(nèi)移動(dòng)機(jī)器人定位方法,可提高機(jī)器人定位精度,但是該方法需要提前在室內(nèi)布置好射頻標(biāo)簽(Tag)等設(shè)備。文獻(xiàn)[10]基于深度相機(jī)采集的RGB圖像信息,估算機(jī)器人當(dāng)前位姿,但是存在不同幀間的匹配問(wèn)題,且構(gòu)建的地圖有較多的點(diǎn)云重疊,與實(shí)際環(huán)境有較大的偏差,嚴(yán)重降低移動(dòng)機(jī)器人的導(dǎo)航定位精度。
針對(duì)上述問(wèn)題,本文通過(guò)Gazebo仿真平臺(tái),構(gòu)建搭載Delta3A型號(hào)激光雷達(dá)的虛擬樣機(jī),引入Gmapping(基于Rao-Blackwellized粒子濾波器算法)、Hector SLAM(基于Gauss-Newton迭代法)及Cartographer三種主流算法的建圖機(jī)理,優(yōu)化Delta3A激光傳感器的參數(shù)配置,降低三種建圖算法所獲二值優(yōu)質(zhì)地圖與原圖的匹配偏差,并在機(jī)器人操作系統(tǒng)[11]中融合A-star和DWA算法,分別進(jìn)行全局和局部路徑規(guī)劃的導(dǎo)航分析。本文在虛擬樣機(jī)仿真分析的基礎(chǔ)上,基于Spark-T移動(dòng)式機(jī)器人,進(jìn)行了相應(yīng)的優(yōu)質(zhì)地圖構(gòu)建與導(dǎo)航定位問(wèn)題分析,進(jìn)一步驗(yàn)證了所用算法的實(shí)效性。
SLAM是指根據(jù)移動(dòng)式機(jī)器人在一個(gè)未知環(huán)境下的移動(dòng),在已知機(jī)器人輸入控制值U1:t和傳感器觀測(cè)值Z1:t的條件下,估計(jì)概率分布P(m|X1:t,Z1:t,U1:t)來(lái)分別表示機(jī)器人定位和地圖構(gòu)建,即P(xt,m|Z1:t,U1:t,x1)。
HectorSLAM結(jié)合基于魯棒性掃描匹配的2DSLAM系統(tǒng)[12],利用激光雷達(dá)的高更新率和低距離測(cè)量噪聲特點(diǎn),對(duì)機(jī)器人位姿進(jìn)行實(shí)時(shí)估計(jì)。該算法是基于Gauss-Newton迭代法[13],解決非線性最小二乘問(wèn)題,即通過(guò)觀察自變量和因變量數(shù)值,求解非線性目標(biāo)函數(shù)的系數(shù)參數(shù),使得函數(shù)模型與觀測(cè)量相近似,Hector SLAM將似然概率達(dá)到最大似然估計(jì),通過(guò)最小二乘的方式來(lái)表達(dá)E=argmax(xt,m|Z1:t,U1:t,x1)的最大似然問(wèn)題
(1)
式中T=Tx,Ty,Tθ:機(jī)器人的位姿;Si(T):把激光點(diǎn)i用T轉(zhuǎn)換之后的坐標(biāo);M(x):得到坐標(biāo)x的地圖占用概率;[1-M(Si(T))]2:勢(shì)場(chǎng),其中Si表示得分。
通過(guò)Lagrange插值法[14]解得?M(Si(T)),構(gòu)建離散的柵格地圖,計(jì)算高斯-牛頓方程,解得機(jī)器人位姿。Hector SLAM對(duì)初值敏感,求解?M(Si(T))采用插值法,在理論上存在不連續(xù)的因素,難以處理閉環(huán)問(wèn)題。
Gmapping基于Rao-Blackwellized粒子濾波器(Rao-Blackwellized Particle Filter,RBPF),在Fast-SLAM的基礎(chǔ)上進(jìn)行二次優(yōu)化,但仍然存在粒子耗散問(wèn)題[15]。該算法將SLAM問(wèn)題表示為求解機(jī)器人位姿以及基于位姿構(gòu)建地圖兩部分內(nèi)容。其主要思想是:結(jié)合傳感器觀測(cè)量Z1:t=z1,…,zt-1和里程計(jì)信息U1:t=u1,…,ut-1來(lái)求解機(jī)器人位姿X1:t=x1,…,xt和地圖信息mi=m1,m2,…。聯(lián)合概率密度可表示為
p(x1:t,m|z1:t,u1:t-1)=
p(x1:t|z1:t,u1:t-1)*p(m|x1:t,u1:t-1)
(2)
RBPF利用粒子濾波器求解時(shí),保留高權(quán)重的粒子并舍棄低權(quán)重粒子,重采樣時(shí)存在一定的隨機(jī)性,采樣次數(shù)增加時(shí),粒子的多樣性會(huì)耗散掉,即存在粒子耗散問(wèn)題。引入自適應(yīng)閾值Neff表示當(dāng)前的求解值和真實(shí)分布的差異性
(3)
當(dāng)Neff過(guò)大時(shí),則差異性過(guò)小,不進(jìn)行重采樣,當(dāng)Neff過(guò)小時(shí),則差異性過(guò)大,進(jìn)行重采樣,從而起到減少重采樣次數(shù),緩解粒子耗散的作用。
Cartographer是基于圖優(yōu)化的算法框架,滿足大場(chǎng)景建圖。采用的是實(shí)時(shí)掃描與建圖匹配方法,同時(shí)進(jìn)行回環(huán)檢測(cè)[16],通過(guò)分支定界優(yōu)化算法提高回環(huán)檢測(cè)的效率。
Cartographer算法分為局部和全局進(jìn)行實(shí)時(shí)建圖與定位。構(gòu)建局部地圖是通過(guò)將激光點(diǎn)云圖的激光點(diǎn)H={hk}k=1,…,K轉(zhuǎn)換后匹配到子地圖中,在匹配前轉(zhuǎn)換:
(4)
式中ξ=(ξx,ξy,ξθ):激光點(diǎn)云圖中點(diǎn)束的姿態(tài);Tξ:表示{hk}k=1,…,K在子地圖中的位置。
通過(guò)子地圖的累加構(gòu)建全局地圖,在累加過(guò)程中進(jìn)行回環(huán)檢測(cè)優(yōu)化,其方法同樣采用求解最小二乘問(wèn)題
(5)
將子地圖的位姿Ξm和傳感器的位姿Ξs匹配,則其殘差E為
(6)
Cartographer構(gòu)建全局地圖時(shí),對(duì)子地圖進(jìn)行優(yōu)化,即殘差E達(dá)到最小,其過(guò)程如圖1所示。
圖1 Cartographer算法處理流程圖
Cartograher相比Hector SLAM,采用了回環(huán)檢測(cè),對(duì)建立的子地圖進(jìn)行實(shí)時(shí)優(yōu)化和匹配,在機(jī)器人快速旋轉(zhuǎn)和俯仰等位置下,仍有效地降低地圖的偏移問(wèn)題,構(gòu)圖精度高。相比Gmapping,Cartograph算法構(gòu)建更廣闊的地圖時(shí),對(duì)計(jì)算量要求較低,求解最小二乘值和優(yōu)化的速度也較快,建圖效率高。
上一章理論性地分析了三種SLAM算法,為驗(yàn)證其可行性和空間復(fù)雜度,本章通過(guò)搭建載有Delta3A激光雷達(dá)的虛擬機(jī)器人,在Gazebo仿真平臺(tái)中控制可變參數(shù),將三種算法建圖效果可視化,并對(duì)機(jī)器人進(jìn)行全局路徑規(guī)劃和局部路徑規(guī)劃,測(cè)試其導(dǎo)航精度。
3.1.1 激光雷達(dá)參數(shù)配置
通過(guò)launch文件和xacro文件優(yōu)化Delta3A激光雷達(dá)的參數(shù)配置,相關(guān)優(yōu)化過(guò)程如下:
優(yōu)化完成后,Delta3A激光雷達(dá)傳感器的性能得到極大地提升,其最大測(cè)量范圍約為16米,采樣率為8000次/秒,優(yōu)化后的傳感器可有效降低前述三種算法在建圖時(shí)因激光點(diǎn)云信息丟失而造成的誤差。
3.1.2 建圖與分析
在Gazebo仿真平臺(tái)中搭建室內(nèi)環(huán)境,并投影成二值地圖,如圖2所示。
圖2 模擬實(shí)驗(yàn)中使用的室內(nèi)環(huán)境
為評(píng)估三種SLAM算法所獲地圖質(zhì)量,對(duì)二值地圖與其構(gòu)建地圖間的誤差進(jìn)行分析。為此,計(jì)算二值地圖與算法地圖的最佳擬合對(duì)齊(如圖3所示)。具體工作過(guò)程如下:每個(gè)算法獲得的地圖進(jìn)行二值化,該地圖僅包含場(chǎng)景邊界和障礙物,使用Generalized Dual Bootstrap-ICP圖像配準(zhǔn)算法,將二值化地圖與各自的地面對(duì)齊。由于室內(nèi)模型的二值地圖和三種SLAM算法所獲地圖都是對(duì)齊的,然后使用Matlab軟件中的Knnsearch函數(shù)計(jì)算地圖間的誤差,這種誤差測(cè)量提供了一個(gè)標(biāo)準(zhǔn)化的距離測(cè)量值,如表1所示。
圖3 二值地圖(紅色)與算法地圖(黑色)擬合對(duì)齊
表1 二值地圖中各算法的誤差估計(jì)值
根據(jù)圖2和表1,分析三種算法在Gazebo仿真平臺(tái)中的仿真結(jié)果。在相同的環(huán)境中,Cartographer算法生成的二值地圖與原地圖的重疊率最高,且誤差估計(jì)值比另外兩種算法小,具有較優(yōu)的建圖效果。此外,Gmapping算法構(gòu)建的地圖與原地圖重疊率相比Cartographer低,且其誤差估計(jì)值在5.3670,而Hector SLAM的建圖結(jié)果呈現(xiàn)出最高的誤差值,其主要原因是當(dāng)機(jī)器人處于較大的速度時(shí)容易發(fā)生顯著的地圖漂移。同時(shí),Cartographer是一種基于圖優(yōu)化的算法,其在生成一個(gè)子地圖后,會(huì)進(jìn)行一次局部的閉環(huán)檢測(cè),故在室內(nèi)復(fù)雜大場(chǎng)景下所獲地圖的質(zhì)量比Hector SLAM和Gmapping算法更高。
移動(dòng)式機(jī)器人通過(guò)Cartographer算法獲得高精度的室內(nèi)地圖后,可以此為基礎(chǔ)進(jìn)行優(yōu)良的路徑規(guī)劃。移動(dòng)式機(jī)器人自主導(dǎo)航功能取決于全局和局部路徑規(guī)劃算法。兩種規(guī)劃算法主要基于傳感器數(shù)據(jù)、里程計(jì)數(shù)據(jù)、地圖服務(wù)器等信息,具體的導(dǎo)航框架如圖4所示。
圖4 導(dǎo)航框架
3.2.1 全局路徑規(guī)劃
全局路徑規(guī)劃可通過(guò)地圖服務(wù)器提供的全局代價(jià)地圖,并融合A-star算法[17]來(lái)實(shí)現(xiàn)全局導(dǎo)航。A-star算法使用啟發(fā)式函數(shù),在起點(diǎn)到終點(diǎn)的多個(gè)路徑中,選取恰當(dāng)?shù)男旭偮窂?。其啟發(fā)式函數(shù),如下
F(n)=G(n)+H(n)
(6)
式中:G(n)表示從初始節(jié)點(diǎn)到指定節(jié)點(diǎn)n的移動(dòng)代價(jià),H(n)表示從指定節(jié)點(diǎn)n到最終點(diǎn)的估算代價(jià)。
在地圖中給定機(jī)器人導(dǎo)航目標(biāo)點(diǎn)和方向,機(jī)器人會(huì)根據(jù)實(shí)時(shí)環(huán)境規(guī)劃出最佳導(dǎo)航路徑,當(dāng)起始點(diǎn)到目標(biāo)點(diǎn)之間沒(méi)有障礙物時(shí),其運(yùn)動(dòng)軌跡為一條直線,進(jìn)而實(shí)現(xiàn)機(jī)器人的全局路徑規(guī)劃,如圖5所示。
圖5 基于Cartographer地圖的全局路徑規(guī)劃
3.2.2 局部路徑規(guī)劃
采用動(dòng)態(tài)窗口法 (Dynamic Windows Approach,DWA[18])可實(shí)現(xiàn)移動(dòng)式機(jī)器人的局部路徑規(guī)劃。該算法主要是在機(jī)器人控制過(guò)程中進(jìn)行多組速度離散采樣,并在系統(tǒng)內(nèi)部計(jì)算單元中先模擬這些速度下機(jī)器人的前進(jìn)軌跡,并將每組軌跡進(jìn)行評(píng)價(jià),選取最優(yōu)軌跡及速度傳送給機(jī)器人的速度控制器,以驅(qū)動(dòng)機(jī)器人實(shí)現(xiàn)避障。
仿真過(guò)程:當(dāng)機(jī)器人完成全局路徑規(guī)劃后,在直線行駛路徑上擺放障礙物,機(jī)器人會(huì)相應(yīng)地做出局部路徑規(guī)劃,不再按照直線軌跡行駛,而產(chǎn)生新的行進(jìn)路線,實(shí)時(shí)避開(kāi)障礙物,完成機(jī)器人的局部路徑規(guī)劃,仿真結(jié)果如圖6所示。
圖6 基于Cartographer的局部路徑規(guī)劃
此外,本文還在真實(shí)場(chǎng)景中使用實(shí)體機(jī)器人進(jìn)行測(cè)試,以便在沒(méi)有理想模擬工況下研究三種建圖算法。因?yàn)樵谀M環(huán)境中有相對(duì)較優(yōu)的條件,如:激光傳感器無(wú)噪聲累積、測(cè)量精度不受距離限制等。在真實(shí)環(huán)境中,本文使用Spark-T機(jī)器人(如圖7所示)。在復(fù)雜環(huán)境中進(jìn)行三次不同算法的建圖與定位,并在獲得相對(duì)優(yōu)質(zhì)的地圖后,進(jìn)行其全局與局部路徑規(guī)劃的實(shí)驗(yàn)分析。
圖7 Spark-T移動(dòng)機(jī)器人
本文研究的移動(dòng)式機(jī)器人由上位機(jī)和下位機(jī)構(gòu)成,上位機(jī)負(fù)責(zé)遠(yuǎn)程狀態(tài)監(jiān)控、數(shù)據(jù)處理與分析等工作,下位機(jī)配備了連接Delta3A激光雷達(dá)等傳感器的工控機(jī),并在主控PC中預(yù)裝Ubuntu16.04和ROS Kinetic,其系統(tǒng)架構(gòu)如圖8所示。
圖8 軟、硬件系統(tǒng)架構(gòu)圖
為驗(yàn)證三種算法在相同實(shí)驗(yàn)環(huán)境下的建圖效果,使用Spark-T機(jī)器人進(jìn)行了三次不同軌跡和初始位置的建圖測(cè)試,并在RVIZ可視化界面[19]中實(shí)時(shí)反饋、記錄建圖效果?,F(xiàn)實(shí)復(fù)雜環(huán)境如圖9所示。
圖9 現(xiàn)實(shí)環(huán)境
測(cè)試結(jié)果表明:三種SLAM算法都能成功構(gòu)建地圖,如圖10所示。機(jī)器人在真實(shí)室內(nèi)復(fù)雜環(huán)境構(gòu)建地圖的效果比仿真環(huán)境下略差,這是由于:激光雷達(dá)傳感器掃描地圖時(shí)存在機(jī)器人位置和噪聲的估計(jì)誤差。
圖10 真實(shí)環(huán)境下建圖效果
盡管現(xiàn)實(shí)環(huán)境與虛擬環(huán)境之間存在差異,但現(xiàn)實(shí)環(huán)境與仿真環(huán)境獲得的結(jié)果都遵循前面分析算法的特性。Hector SLAM在真實(shí)環(huán)境中構(gòu)建的地圖與模擬環(huán)境地圖具有相似特性,較快速度下均產(chǎn)生地圖漂移,Gmapping建圖效果相比Hector SLAM較優(yōu),雖然未發(fā)生嚴(yán)重的地圖偏移,但是非常依賴?yán)锍逃?jì)信息,其間接地影響了建圖精度,對(duì)復(fù)雜的大場(chǎng)景物體特征細(xì)節(jié)處理不足。Cartographer在測(cè)試結(jié)果中表現(xiàn)最優(yōu),比其余兩種算法更少地受到傳感器噪聲影響,這是因?yàn)镃artographer在室內(nèi)復(fù)雜的大環(huán)境下建圖采用了先創(chuàng)建子地圖,并通過(guò)后端回環(huán)檢測(cè)降低了子地圖的誤差,利用分支界定法提高求解子地圖與全局地圖匹配問(wèn)題的效率,最終獲得構(gòu)建地圖的最優(yōu)解。
除了對(duì)建圖誤差分析外,本文還將每種算法對(duì)計(jì)算機(jī)的負(fù)載進(jìn)行了實(shí)驗(yàn)分析,以此來(lái)驗(yàn)證三種算法的空間復(fù)雜度。實(shí)體樣機(jī)配備了英特爾G4560型號(hào)CPU、8GBDDR4內(nèi)存,實(shí)驗(yàn)結(jié)果如圖11、表2所示。
圖11 三種SLAM算法的CPU占存隨時(shí)間變化
表2 三種SLAM算法CPU占存的平均值標(biāo)準(zhǔn)差σ
通過(guò)對(duì)比可知:在相同實(shí)驗(yàn)環(huán)境下,將CPU占存作為評(píng)價(jià)指標(biāo)時(shí),Gmapping算法對(duì)系統(tǒng)負(fù)載最高,其次是Cartographer算法和Hector SLAM算法,這與前面對(duì)三種算法分析的結(jié)論相符合。從上述實(shí)驗(yàn)結(jié)果可知:無(wú)論從建圖效果還是空間復(fù)雜度來(lái)評(píng)價(jià)三種算法,基于Delta3A激光雷達(dá)傳感器的Cartographer算法表現(xiàn)最佳。
4.3.1 復(fù)雜環(huán)境的全局路徑規(guī)劃
基于Cartographer算法構(gòu)建的先驗(yàn)地圖,進(jìn)行復(fù)雜環(huán)境下移動(dòng)式機(jī)器人的全局路徑規(guī)劃實(shí)驗(yàn)。將先驗(yàn)地圖導(dǎo)入Spark-T機(jī)器人系統(tǒng),在其操作系統(tǒng)中對(duì)障礙物會(huì)產(chǎn)生安全膨脹邊界,該邊界可起到保護(hù)機(jī)器人的作用。當(dāng)機(jī)器人確定前進(jìn)目標(biāo)點(diǎn)后,融合A-star算法在其當(dāng)前位置與目標(biāo)位置間會(huì)由全局路徑規(guī)劃器生成一條最佳路徑,機(jī)器人根據(jù)該路徑進(jìn)行移動(dòng),可完成全局導(dǎo)航任務(wù),如圖12所示。
圖12 基于先驗(yàn)地圖的全局路徑規(guī)劃
4.3.2 復(fù)雜環(huán)境下的局部路徑規(guī)劃
為測(cè)試復(fù)雜環(huán)境地圖下的機(jī)器人局部路徑規(guī)劃能力,即局部避障。當(dāng)機(jī)器人開(kāi)始循跡全局路徑時(shí),在其移動(dòng)路線上擺放動(dòng)態(tài)障礙物。當(dāng)機(jī)器人遇到移動(dòng)障礙物時(shí),局部路徑規(guī)劃器重新規(guī)劃前進(jìn)路徑,實(shí)現(xiàn)機(jī)器人在復(fù)雜環(huán)境下的實(shí)時(shí)避障動(dòng)作,完成局部路徑規(guī)劃,在繞過(guò)障礙物后繼續(xù)向目標(biāo)點(diǎn)運(yùn)動(dòng),如圖13所示。
圖13 基于先驗(yàn)地圖的局部路徑規(guī)劃
通過(guò)上述實(shí)驗(yàn)分析可知:基于Cartographer算法構(gòu)建的先驗(yàn)地圖,采用A-star算法為全局路徑規(guī)劃算法、DWA算法為局部路徑規(guī)劃算法,可實(shí)現(xiàn)移動(dòng)式機(jī)器人在室內(nèi)復(fù)雜環(huán)境下的高精度導(dǎo)航定位。
本文分別通過(guò)計(jì)算機(jī)仿真和實(shí)驗(yàn)測(cè)試,分析了三種代表性的二維SLAM算法。仿真和實(shí)驗(yàn)結(jié)果表明:采用Cartographer算法可有效解決室內(nèi)復(fù)雜場(chǎng)景的實(shí)時(shí)建圖與定位累積誤差問(wèn)題,且該算法具備高時(shí)效、低功耗等優(yōu)勢(shì)?;谠搩?yōu)質(zhì)先驗(yàn)地圖,融合A-star和DWA路徑規(guī)劃算法,可有效提升移動(dòng)式機(jī)器人的導(dǎo)航精度。
本文采用的Delta3A單線激光雷達(dá)具有結(jié)構(gòu)簡(jiǎn)單、功耗低、成本低等優(yōu)點(diǎn),但其局限于平面式掃描;多線激光雷達(dá)可多維度檢測(cè)物體特征,但成本偏高。本文下一步工作將融合慣性測(cè)量單元信息,提高單線激光雷達(dá)在多維度對(duì)物體信息感知的性能,進(jìn)一步改善復(fù)雜場(chǎng)景下室內(nèi)移動(dòng)式機(jī)器人的SLAM系統(tǒng)。