基于RBPF-SLAM算法的研究與實(shí)現(xiàn)①

王 輝,王 品

1(中國(guó)科學(xué)院大學(xué),北京 100049)

2(中國(guó)科學(xué)院 沈陽(yáng)計(jì)算技術(shù)研究所 高檔數(shù)控國(guó)家工程研究中心,沈陽(yáng) 110168)

3(沈陽(yáng)高精數(shù)控智能技術(shù)股份有限公司,沈陽(yáng) 110168)

實(shí)現(xiàn)機(jī)器人自主移動(dòng)的基礎(chǔ)是構(gòu)建環(huán)境地圖和定位機(jī)器人位置.在各種研究文獻(xiàn)中把機(jī)器人環(huán)境地圖構(gòu)建和位置定位問(wèn)題定義成即時(shí)定位與地圖生成(SLAM)[1,2],該問(wèn)題的復(fù)雜性主要在于定位機(jī)器人的位置需要獲得一個(gè)環(huán)境特征地圖,而在構(gòu)建環(huán)境特征地圖時(shí)又估計(jì)機(jī)器人此刻的位置狀態(tài).地圖評(píng)估特征估計(jì)與機(jī)器人位姿估計(jì)之間的高度依賴使解決SLAM 問(wèn)題變得十分困難.早期解決SLAM 問(wèn)題的方法是在卡爾曼濾波器(Kalman Filter,KF)[3]基礎(chǔ)上發(fā)展而來(lái),后來(lái)隨著粒子濾波(Particle Filters,PF)[4]理論的提出,Montemerlo[5]和Paskin[6]等人提出RBPF 算法作為解決SLAM 問(wèn)題的有效方法,該算法主要根據(jù)建立環(huán)境地圖所需的粒子數(shù)量來(lái)衡量算法的復(fù)雜度,.因此使用較少的粒子構(gòu)建較為精確地圖是該算法解決的主要問(wèn)題,此外重采樣步驟可能潛在剔除有效粒子,從而造成粒子耗散[7]問(wèn)題.

在傳統(tǒng)的RBPF-SLAM 算法實(shí)現(xiàn)中需要粒子數(shù)量十分巨大,主要原因在于計(jì)算建議分布時(shí)只把運(yùn)動(dòng)模型數(shù)據(jù)考慮在內(nèi),造成建議分布與目標(biāo)分布差距較大,需要大量的粒子進(jìn)行擬合.因此通過(guò)把運(yùn)動(dòng)模型數(shù)據(jù)和觀測(cè)數(shù)據(jù)兩者相結(jié)合來(lái)優(yōu)化建議分布,使之更加接近目標(biāo)位姿的后驗(yàn)概率分布.基于優(yōu)化的建議分布進(jìn)行粒子采樣,可以顯著減少粒子數(shù)目.在傳統(tǒng)算法實(shí)現(xiàn)中頻繁的重采樣操作會(huì)導(dǎo)致粒子數(shù)量枯竭,通過(guò)引入自適應(yīng)重采樣方法來(lái)解決該問(wèn)題,該方法通過(guò)計(jì)算當(dāng)前有效粒子數(shù)目來(lái)決定是否進(jìn)行重采樣操作,避免過(guò)度的重采樣造成粒子數(shù)量枯竭現(xiàn)象.

為了隨著時(shí)間的推移能夠高效的更新粒子,本文引用了二叉樹來(lái)組織柵格地圖[8]特征,這種樹形結(jié)構(gòu)使得粒子的更新時(shí)間復(fù)雜度從線性級(jí)降低到對(duì)數(shù)級(jí),使粒子之間共享相同的地圖部分,從一定程度上節(jié)省內(nèi)存消耗.

1 基于粒子濾波的RBPF 算法原理

在RBPF 算法中用p(x1:t|z1:t,u1:t,c1:t)表示機(jī)器人路徑后驗(yàn)分布,其中X表示機(jī)器人位姿,Z表示傳感器觀測(cè)數(shù)據(jù),U表示運(yùn)動(dòng)數(shù)據(jù),C是一致性變量.用一定數(shù)量的粒子來(lái)擬合后驗(yàn)分布,每個(gè)粒子表示機(jī)器人在真實(shí)環(huán)境中的一種位姿假設(shè).采樣粒子集Yt表示為:

每個(gè)粒子包含一個(gè)機(jī)器人的位姿估計(jì) (x,y,θ)T,而且包含一個(gè)具有均值μkj和協(xié)方差Σkj的卡爾曼濾波集合,這個(gè)集合表示每個(gè)粒子對(duì)于地圖中出現(xiàn)特征的估計(jì)和機(jī)器人位姿的狀態(tài),用 μkj和 Σkj表示出現(xiàn)在地圖中的每個(gè)特征位置估計(jì)的均值和方差.

RBPF 粒子濾波器可以表示為如下因式分解:該公式用兩個(gè)獨(dú)立的后驗(yàn)概率乘積表示SLAM 問(wèn)題:p(x1:t|z1:t,u1:t-1)表示機(jī)器人路徑軌跡的后驗(yàn)概率,p(m|x1:t,z1:t)表示機(jī)器人的環(huán)境地圖后驗(yàn)概率.這種概率分解在解決SLAM 問(wèn)題時(shí)可以先計(jì)算機(jī)器人的位姿軌跡,然后在已知機(jī)器人位姿情況下的計(jì)算更新當(dāng)前環(huán)境地圖.

算法步驟描述如下:

1)采樣:根據(jù)建議分布從t-1 時(shí)刻粒子集合 采樣獲取t 時(shí)刻粒子的先驗(yàn)分布集合 ,建議分布使用運(yùn)動(dòng)模型.2)計(jì)算權(quán)重:計(jì)算每個(gè)粒子的權(quán)重,權(quán)重w 反映了建議分布與目標(biāo)后驗(yàn)分布的差距.Yt-1Yt- p(xt|xt-1,ut)3)重采樣:根據(jù)每個(gè)粒子的權(quán)重進(jìn)行重采樣,從臨時(shí)粒子集合中抽取更換M 個(gè)粒子,結(jié)果產(chǎn)生的M 個(gè)粒子形成新的最終粒子集 .z1:t xtmitYt-Yt 4)更新地圖:對(duì)于每個(gè)粒子根據(jù)傳感器觀測(cè)數(shù)據(jù) 和機(jī)器人當(dāng)前位姿 更新地圖中的每個(gè)特征 .mi～p(mi|xt,z1:t) (4)

2 RBPF-SLAM 的改進(jìn)算法

在傳統(tǒng)的RBPF 算法實(shí)現(xiàn)中,在進(jìn)行粒子采樣時(shí),粒子濾波器使用機(jī)器人的遠(yuǎn)動(dòng)模型計(jì)算建議分布,使用傳感器觀測(cè)模型計(jì)算采樣粒子的權(quán)重,權(quán)重更新公式為:

如果距離傳感器的精度比里程計(jì)的精度高,采用上述的建議分布會(huì)導(dǎo)致粒子之間的權(quán)重差距比較大,具有較高權(quán)重的粒子分布在觀測(cè)后驗(yàn)高概率區(qū)域.而且在傳統(tǒng)的算法實(shí)現(xiàn)中每一步都要進(jìn)行重采樣操作,頻繁的重采樣會(huì)潛在的提出有效粒子導(dǎo)致粒子耗散,以及降低算法的計(jì)算效率,影響實(shí)時(shí)性.

2.1 改進(jìn)建議分布

解決傳統(tǒng)算法的建議分布的不足,一種普遍的解決方法,尤其是在定位方面,是使用一個(gè)平滑的似然函數(shù),避免粒子在觀測(cè)似然函數(shù)分布之外的區(qū)域權(quán)值較低.但是這種方法會(huì)丟棄一部分距離傳感器采集到的信息,會(huì)導(dǎo)致建立的地圖不準(zhǔn)確.解決該問(wèn)題的一種改進(jìn)方案,在采集下一代粒子時(shí)把最近的觀測(cè)數(shù)據(jù)zt考慮進(jìn)來(lái).通過(guò)把觀測(cè)數(shù)據(jù)zt結(jié)合到建議分布中,從而在采樣時(shí),只采集分布在觀測(cè)概率函數(shù)區(qū)域內(nèi)的粒子.此改進(jìn)的建議分布如下:

使用該建議分布在計(jì)算權(quán)重時(shí),公式為:

在實(shí)際的實(shí)現(xiàn)中無(wú)法通過(guò)公式(6)直接采樣下一代臨時(shí)粒子樣本集.解決辦法是在建議分布的峰值附近區(qū)域內(nèi)的求取一個(gè)近似的正態(tài)分布函數(shù),并用該函數(shù)進(jìn)行采樣.首先,使用t-1 時(shí)刻的粒子位姿,計(jì)算期望粒子位姿:

然后,根據(jù)距離傳感器采集到的數(shù)據(jù)計(jì)算具有最大觀測(cè)概率的機(jī)器人目標(biāo)位姿

然后在目標(biāo)位姿附近選取K個(gè)樣本點(diǎn),依據(jù)以下公式進(jìn)行采樣:

然后使用采集的K個(gè)樣本點(diǎn){x1,x2,···,xk},使用蒙特卡洛計(jì)算正態(tài)分布函數(shù)的參數(shù)N(μit,Σit).

另xj∈{x1,x2,···,xk},則

通過(guò)此方法,我們可以獲得此改進(jìn)建議分布的近似公式,在此公式中融合了機(jī)器人的里程計(jì)數(shù)據(jù)ut-1和最新的觀測(cè)數(shù)據(jù)zt,可以更加有效準(zhǔn)確獲取采樣粒子.

2.2 自適應(yīng)重采樣

在重采樣時(shí),權(quán)值wi小于閾值的粒子會(huì)被從高權(quán)值區(qū)域附近采樣的粒子所替代.一方面重采樣是必須的,因?yàn)橹恍枰邢薜牧Ｗ觼?lái)近似目標(biāo)分布；另一方面重采樣可能剔除有效粒子導(dǎo)致粒子數(shù)量枯竭.在本文中根據(jù)當(dāng)前的有效粒子數(shù)目來(lái)決定是否要執(zhí)行從采樣操作.有效粒子數(shù)目Neff表示粒子集的退化程度,值越小退化越嚴(yán)重,需要進(jìn)行重采樣.Neff如下定義:

其中,M為粒子數(shù)目,是粒子xi的 歸一化化權(quán)重.Neff越大說(shuō)明粒子集的多樣性越好,不需要重采樣,只有Neff＜N/2時(shí)才需要重采樣.

2.3 改進(jìn)算法流程描述

2)根據(jù)掃描匹配算法估計(jì)具有最大觀測(cè)后驗(yàn)概率的機(jī)器人目標(biāo)位姿.

3)計(jì)算建議分布:根據(jù)公式(11)～(13)計(jì)算建議分布π ～N(μit,Σit).

4)采樣:根據(jù)所求的建議分布進(jìn)行粒子采樣.

5)計(jì)算權(quán)重:根據(jù)公式wit∝wit-1·ηit計(jì)算粒子權(quán)重.

6)依據(jù)公式(14)計(jì)算當(dāng)前粒子集的有效數(shù)目,當(dāng)Neff＜N/2時(shí)需要重采樣.

7)根據(jù)傳感器觀測(cè)數(shù)據(jù)更新地圖.

算法流程圖見(jiàn)圖1.

3 算法的高效實(shí)現(xiàn)

在RBPF-SLAM 算法的實(shí)現(xiàn)中每一步地圖更新好像均需要時(shí)間O(MN),其中,M表示粒子數(shù)目,N表示地圖中的特征數(shù)量.每次更新必須處理M個(gè)粒子,M的線性復(fù)雜度是必須的.N的線性復(fù)雜度是重采樣的結(jié)果,當(dāng)粒子在重采樣步驟中不止一次被抽中時(shí),在一般的實(shí)現(xiàn)中會(huì)復(fù)制屬于該粒子的整個(gè)地圖.通過(guò)引入更多選擇性更新的數(shù)據(jù)結(jié)構(gòu)來(lái)表示粒子,可以避免線性復(fù)制的開銷.其基本思想是將地圖組織為平衡二叉樹,如圖2所示.

假設(shè)在t時(shí)刻合并了一個(gè)新的控制ut和新的測(cè)量zt,Yt里 的每個(gè)新粒子與Yt-1里對(duì)應(yīng)的粒子主要存在兩點(diǎn)不同:第一,得到不同的位姿估計(jì)；第二,觀察到的特征的高斯參數(shù)μki和 Σki會(huì)被更新,所有的其它特征的參數(shù)不變.因此,在復(fù)制粒子時(shí),在代表所有特征的樹里,只有一個(gè)路徑被修改,所以可以在對(duì)數(shù)時(shí)間內(nèi)完成更新.對(duì)于樹節(jié)點(diǎn)的內(nèi)存釋放,是通過(guò)給每個(gè)節(jié)點(diǎn)分配一個(gè)指針計(jì)數(shù)器.新創(chuàng)建的節(jié)點(diǎn)計(jì)數(shù)器初始化為1,當(dāng)其它粒子創(chuàng)建指針指向該節(jié)點(diǎn)時(shí),計(jì)數(shù)器增加,指針移除時(shí)計(jì)數(shù)器減少,當(dāng)計(jì)數(shù)器為零時(shí)釋放該節(jié)點(diǎn).實(shí)驗(yàn)表明使用樹形結(jié)構(gòu)組織地圖可以大量節(jié)省內(nèi)存,加快地圖更新速度.

圖1 改進(jìn)算法流程圖

圖2 特征數(shù)M=8 的粒子地圖樹形結(jié)構(gòu)

4 實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)的硬件平臺(tái)配置包括TurtleBot 機(jī)器人、里程計(jì)和激光雷達(dá),TurtleBot 機(jī)器人是一款基于ROS 操作系統(tǒng)的機(jī)器人配備用里程計(jì)和二維激光雷達(dá).軟件平臺(tái)配置采用ROS 的kinetic 版本,運(yùn)行平臺(tái)為Ubuntu 操作系統(tǒng),并采用Python 作為腳本編程語(yǔ)言.ROS 自帶3 維可視化工具RVIZ(如圖3所示) ,可進(jìn)行人機(jī)交互.

在圖4中分別展示了由傳統(tǒng)RBPF-SLAM 算法和改進(jìn)算法創(chuàng)建的室內(nèi)地圖.在圖中深灰色表示未探測(cè)區(qū)域,淺灰色表示已掃描區(qū)域,其中被包圍的深灰色區(qū)域表示掃描到的地圖特征(障礙物).本實(shí)驗(yàn)的地圖大小為10 米 ×10 米.圖4(a)是用傳統(tǒng)算法創(chuàng)建的二維柵格地圖,采用了20 個(gè)采樣粒子；圖4(c)是由改進(jìn)算法創(chuàng)建的地圖,只采用了10 個(gè)采樣粒子,從效果來(lái)看創(chuàng)建的地圖更加清晰精確.

圖3 3 維可視化工具RVIZ

從圖4的對(duì)比實(shí)驗(yàn)效果來(lái)看,相比于傳統(tǒng)的RBPF-SLAM 算法,改進(jìn)的算法使用更少的粒子數(shù)目,更少的計(jì)算資源以及更少的存儲(chǔ)資源可以創(chuàng)建出精確度更高,效果更好的環(huán)境地圖.在改進(jìn)算法中結(jié)合傳感器觀測(cè)數(shù)據(jù)優(yōu)化建議分布,使得建議分布更加接近于機(jī)器人真實(shí)位姿的目標(biāo)分布.粒子數(shù)量是衡量RBPF算法性能的主要指標(biāo),因此改進(jìn)的算法可以顯著降低粒子數(shù)目,節(jié)省計(jì)算資源.

圖4 室內(nèi)地圖效果對(duì)比

而且在改進(jìn)算法的實(shí)現(xiàn)時(shí)采用了樹形數(shù)據(jù)結(jié)構(gòu)來(lái)表示粒子,這樣可以大量節(jié)省內(nèi)存的消耗.在圖5中給出傳統(tǒng)RBPF 算法與該進(jìn)算法消耗內(nèi)存的對(duì)比.縱坐標(biāo)表示粒子數(shù),橫坐標(biāo)表示內(nèi)存消耗.通過(guò)引用樹形結(jié)構(gòu)可以有效減小內(nèi)存使用情況.

圖5 傳統(tǒng)算法與改進(jìn)算法內(nèi)存使用對(duì)比

分析比較改進(jìn)算法的運(yùn)行時(shí)間,在表1和表2中記錄了在不同的粒子數(shù)目下,改進(jìn)算法和傳統(tǒng)算法主要步驟執(zhí)行時(shí)間.能夠看到,建立環(huán)境地圖所需時(shí)間和粒子數(shù)量近似成正比關(guān)系.改進(jìn)算法所需的計(jì)算時(shí)間明顯少于傳統(tǒng)算法所需的計(jì)算時(shí)間,這是由于改進(jìn)算法的采樣準(zhǔn)確度高,從而大大減少了所需的粒子數(shù)目,而且在算法實(shí)現(xiàn)時(shí)通過(guò)引入樹形結(jié)構(gòu)減少了粒子的查找與更新時(shí)間,大大提高了算法的計(jì)算效率.

表1 改進(jìn)算法的平均計(jì)算時(shí)間(ms)

表2 傳統(tǒng)算法的平均計(jì)算時(shí)間(ms)

5 總結(jié)

在本文中通過(guò)對(duì)傳統(tǒng)RBPF-SLAM 算法優(yōu)化改進(jìn),在計(jì)算時(shí)把運(yùn)動(dòng)模型與觀測(cè)信息相結(jié)合優(yōu)化建議分布,可以顯著減少采樣的粒子數(shù)量；引入自適應(yīng)的重采樣策略,避免不必要的從采樣操作,避免粒子耗散,根據(jù)Neff適時(shí)的采樣隨機(jī)粒子維持粒子集的多樣性.通過(guò)引入更多選擇性更新的數(shù)據(jù)結(jié)構(gòu)來(lái)表示粒子,可以避免線性復(fù)制的開銷.其基本思想是將地圖組織為平衡二叉樹,減少了粒子的查找與更新時(shí)間,提高了算法性能.從實(shí)驗(yàn)結(jié)果來(lái)看對(duì)算法的優(yōu)化改進(jìn)從計(jì)算效率,地圖精度以及存儲(chǔ)消耗等方面都要優(yōu)于傳統(tǒng)算法.