陳國(guó)軍,陳 巍
(南京工程學(xué)院工業(yè)中心,江蘇 南京 211167)
自主導(dǎo)航是移動(dòng)機(jī)器人研究的重點(diǎn)。為了實(shí)現(xiàn)自主導(dǎo)航,機(jī)器人需要理解運(yùn)動(dòng)場(chǎng)景的結(jié)構(gòu)和方向。與此同時(shí),定位和地圖構(gòu)建(simultaneous location and map construction,SLAM)已經(jīng)成為自主導(dǎo)航研究中的重要內(nèi)容。機(jī)器人創(chuàng)建地圖,更新結(jié)構(gòu)和定位自己,實(shí)現(xiàn)導(dǎo)航。基于粒子濾波的SLAM算法,例如FastSLAM算法和GMapping算法[1],利用了粒子濾波非參數(shù)特性,能夠處理多模態(tài)分布,從而估計(jì)更接近正確值的狀態(tài)解。由于粒子濾波器SLAM的結(jié)果不一致、內(nèi)存消耗大,以及馬爾可夫假設(shè)的誤差積累,使得粒子濾波器SLAM的研究遇到很多問(wèn)題[2]。近年來(lái),許多SLAM算法轉(zhuǎn)向使用位姿圖SLAM(oriented fast and rotated brief SLAM,ORBSLAM)算法。位姿圖SLAM算法包括兩個(gè)主要部分,前端用于從數(shù)據(jù)關(guān)聯(lián)中跟蹤機(jī)器人軌跡以創(chuàng)建姿態(tài)圖,后端用于優(yōu)化姿態(tài)圖。但是位姿圖SLAM也存在很多問(wèn)題。不良的位姿圖結(jié)構(gòu)會(huì)破壞優(yōu)化過(guò)程,使得結(jié)果陷入局部極小值[3]。本文針對(duì)非參數(shù)方法趨向于避免陷入局部位姿圖極小值的問(wèn)題,對(duì)基于粒子濾波的方法進(jìn)行了改進(jìn),利用修正梯度細(xì)化(corrective gradient refinement,CGR)算法對(duì)傳統(tǒng)的Rao-Backwellized粒子濾波SLAM(Rao-Backwellized particle filtering SLAM,RBPF SLAM)算法進(jìn)行了擴(kuò)充。使用柵格地圖結(jié)構(gòu)和激光雷達(dá)傳感器作為提出的廣義SLAM算法,并將其擴(kuò)展為位姿圖SLAM前端圖構(gòu)建的基礎(chǔ)。
在RBPF方法中,將狀態(tài)空間劃分為姿態(tài)狀態(tài)和地圖狀態(tài)。粒子濾波(particle filtering,PF)近似機(jī)器人的姿態(tài)置信度分布,而每個(gè)粒子包含表示環(huán)境模型的映射[4]。RBPF SLAM算法是使用PF來(lái)估計(jì)給定里程計(jì)測(cè)量序列U0:t-1和環(huán)境觀測(cè)Z0:t的機(jī)器人軌跡分布p(xt,m|Z0:t,U0:t-1)以及環(huán)境地圖m。采用該軌跡估計(jì)來(lái)估計(jì)地圖和軌跡上的期望分布:
p(xt,m|Z0:t,U0:t-1)=p(m|xt,Z0:t)p(xt|Z0:t,U0:t-1)
(1)
PF工作類(lèi)似于蒙特卡洛定位,除了一個(gè)給定的地圖,每個(gè)粒子包含一個(gè)單獨(dú)的地圖。其思想是將機(jī)器人軌跡估計(jì)與地圖估計(jì)過(guò)程分開(kāi)[5]。該分解允許首先估計(jì)機(jī)器人軌跡,然后計(jì)算給定軌跡地圖:
p(x1:t,m|Z1:t,U1:t-1)=p(m|x1:t,Z1:t)p(x1:t|Z1:t,U1:t-1)
(2)
由于x1:t和z1:t是已知的(以已知姿態(tài)映射),因此可以有效地進(jìn)行后驗(yàn)過(guò)映射p(m|x1:t,Z1:t)的解析計(jì)算。后部p(x1:t|Z1:t,U1:t-1)的估計(jì)與定位問(wèn)題相似,因?yàn)閮H需要估計(jì)機(jī)器人軌跡。這是用PF進(jìn)行增量處理觀察和測(cè)距讀數(shù)[6]。整個(gè)過(guò)程可以概括如下(對(duì)于每個(gè)粒子i)。
②重要度加權(quán):根據(jù)最近的觀察、姿態(tài)估計(jì)和與該粒子相關(guān)聯(lián)的映射,為每個(gè)粒子分配單獨(dú)的重要度權(quán)重ω(i)。
(3)
③重采樣:具有低重要性權(quán)重的粒子ω通常被高重量的樣品所取代。此步驟是必要的,因?yàn)閮H使用有限數(shù)量的粒子來(lái)近似連續(xù)分布。此外,重采樣允許在真實(shí)分布不同于提議的情況下應(yīng)用PF。
(4)
基于掃描過(guò)程所傳遞的數(shù)據(jù),計(jì)算粒子分布的高斯近似Ψ。
(5)
參數(shù)μ(i)和∑(i)分別是平均值和方差,分別通過(guò)每個(gè)粒子i進(jìn)行確定。這些參數(shù)估計(jì)如下:
(6)
(7)
歸一化因子:
(8)
通過(guò)使用式(7)計(jì)算生成有效粒子,然后權(quán)重可以近似為xj,是接近相應(yīng)掃描所指出的最大概率的點(diǎn)樣本):
(9)
重新分布的粒子將保持在狀態(tài)表示中,而另一些則將被去除,這樣可以替換樣本,并且使得最終的根據(jù)狀態(tài)軌跡重新進(jìn)行粒子分布。通過(guò)重新采樣,更新標(biāo)準(zhǔn)決定執(zhí)行重采樣步驟,通過(guò)估計(jì)有效粒子數(shù)neff的方法是估計(jì)粒子方差。
(10)
(11)
所提出的算法遵循連接圖路由算法(contact graph routing,CGR)局部化結(jié)構(gòu),并通過(guò)Rao-Backwellized粒子濾波器將其擴(kuò)展到SLAM應(yīng)用[8]。該方法是將CGR算法合并到RBPF中,以便改進(jìn)軌跡p(x1:t|z1:t,u1:t-1)。通過(guò)四個(gè)CGR算法步驟(預(yù)測(cè)、精煉、接受、更新和重采樣)[9],其算法可以總結(jié)如下。
①預(yù)測(cè)步驟:遵循CGR預(yù)測(cè)步驟,得到初始狀態(tài)建議分布集q0作為結(jié)果。
③驗(yàn)收測(cè)試步驟:在產(chǎn)生最終分布集q的CGR算法,精確驗(yàn)收測(cè)試步驟。
④更新和重采樣步驟:根據(jù)新的RBPF權(quán)重推導(dǎo),通過(guò)廣義細(xì)化步驟的改進(jìn)建議來(lái)更新權(quán)重。粒子集重采樣準(zhǔn)則改變后,使用選擇性重采樣來(lái)防止粒子退化。
(12)
在實(shí)現(xiàn)過(guò)程中,為了降低復(fù)雜度、避免浮點(diǎn)誤差,忽略了歸一化常數(shù),優(yōu)選對(duì)數(shù)似然版本。D是對(duì)應(yīng)距離dk的集合,將非歸一化的觀測(cè)對(duì)數(shù)似然定義為[11]:
(13)
傳統(tǒng)的Beam端點(diǎn)模型的實(shí)現(xiàn)需要對(duì)整個(gè)地圖的似然分?jǐn)?shù)緩存進(jìn)行預(yù)計(jì)算,這主要是為了進(jìn)行定位。然而,這在SLAM情況下是不合適的。這是因?yàn)樵谟?jì)算觀測(cè)似然分?jǐn)?shù)之前沒(méi)有關(guān)于地圖的信息。此外,通過(guò)使用匹配,不能保證每個(gè)觀測(cè)光束zk都能匹配其對(duì)應(yīng)關(guān)系。因此,當(dāng)未找到其對(duì)時(shí),通過(guò)懲罰似然得分,產(chǎn)生了似然得分的最終形式。
(14)
式中:umull為一個(gè)恒定的懲罰距離,因?yàn)闆](méi)有找到對(duì)應(yīng)關(guān)系-空似然得分。
試驗(yàn)測(cè)試結(jié)果如圖1所示。
圖1 試驗(yàn)測(cè)試結(jié)果
本文在機(jī)器人操作系統(tǒng)(robot operating system,ROS)上實(shí)現(xiàn)了該算法,并進(jìn)行試驗(yàn)評(píng)估。先使用2個(gè)基準(zhǔn)數(shù)據(jù)集進(jìn)行仿真測(cè)試,由激光傳感器采集。數(shù)據(jù)集是在具有動(dòng)態(tài)對(duì)象的無(wú)約束戶(hù)外環(huán)境中收集的。本文算法至少需要20個(gè)粒子來(lái)創(chuàng)建這個(gè)圖,第一個(gè)基準(zhǔn)數(shù)據(jù)集(Intel試驗(yàn)室數(shù)據(jù)集)是由一個(gè)差分驅(qū)動(dòng)器配備LMS激光雷達(dá),仿真場(chǎng)景大小近似為28 m×28 m。本文算法需要5個(gè)粒子來(lái)正確地創(chuàng)建一個(gè)具有5厘米柵格分辨率的閉環(huán)映射,如圖1(a)所示。在第二個(gè)數(shù)據(jù)集(Campus數(shù)據(jù)集)上進(jìn)行測(cè)試,配備SICK LMS激光雷達(dá),仿真場(chǎng)景大小近似為250 m×250 m,如圖1(b)所示。試驗(yàn)表明,本文算法利用在實(shí)際的基準(zhǔn)數(shù)據(jù)集中創(chuàng)建地圖,同時(shí)在所需的粒子方面保持效率。
本文使用從安裝有RPLIDAR A1激光傳感器的開(kāi)源ROS移動(dòng)機(jī)器人平臺(tái)收集自己的試驗(yàn)數(shù)據(jù)集,在計(jì)算機(jī)上運(yùn)行 Liunx(Ubuntu 16.04)的ROS系統(tǒng),完成同時(shí)定位與地圖構(gòu)建。為了測(cè)試改進(jìn)RBPF SLAM算法提供的校正姿態(tài)的質(zhì)量,需要確定適當(dāng)?shù)淖钚¢撝捣謹(jǐn)?shù)以接受或拒絕姿態(tài)。由于得分根據(jù)傳感器和地圖上下文而不同,因此需要調(diào)優(yōu)以獲得適當(dāng)?shù)牡貓D,將掃描匹配得分定義為:
(15)
式中:σsm為掃描匹配標(biāo)準(zhǔn)偏差。
本文將長(zhǎng)度為30 m的試驗(yàn)樓走廊設(shè)置為場(chǎng)景,試驗(yàn)結(jié)果如圖2所示。
其中,圖2(a)、圖2(b)和圖2(c)通過(guò)根據(jù)上面的公式改變分?jǐn)?shù)應(yīng)用RBPF SLAM算法得到的結(jié)果,圖2(d)是根據(jù)本文的算法給出的結(jié)果。從結(jié)果來(lái)看,通過(guò)接受性測(cè)試與當(dāng)前地圖最匹配的觀察可能性來(lái)概率地評(píng)估姿態(tài),本文提出的方法能給出更少失真的地圖,使移動(dòng)機(jī)器人定位精度得到了有效的提高,地圖構(gòu)建的準(zhǔn)確性也得到了有效的提升。試驗(yàn)結(jié)果表明,該方法具有良好的系統(tǒng)魯棒性。
圖2 試驗(yàn)結(jié)果
本文在RBRF SLAM算法的基礎(chǔ)上,通過(guò)將RBPF算法與CGR算法相結(jié)合,提出了基于CGR算法的改進(jìn)RBPF SLAM方法。該方法有效地減少了所需粒子的數(shù)量,降低了在濾波預(yù)測(cè)階段機(jī)器人姿態(tài)估計(jì)的不確定性。本文提出的算法是在激光傳感器上實(shí)現(xiàn)的,在移動(dòng)機(jī)器人試驗(yàn)室環(huán)境下的試驗(yàn)和結(jié)果分析。在LIDAR SLAM基準(zhǔn)數(shù)據(jù)集上的測(cè)試表明,通過(guò)試驗(yàn)分析,基于粒子濾波的RBPF SLAM算法能夠?qū)崟r(shí)在線生成高精度柵格地圖,與實(shí)際地圖更加一致。