王 輝,王 品

1(中國科學院大學,北京 100049)

2(中國科學院 沈陽計算技術(shù)研究所 高檔數(shù)控國家工程研究中心,沈陽 110168)

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

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

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

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

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

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

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

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

算法步驟描述如下:

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

2 RBPF-SLAM 的改進算法

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

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

2.1 改進建議分布

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

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

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

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

然后在目標位姿附近選取K個樣本點,依據(jù)以下公式進行采樣:

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

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

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

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

在重采樣時,權(quán)值wi小于閾值的粒子會被從高權(quán)值區(qū)域附近采樣的粒子所替代.一方面重采樣是必須的,因為只需要有限的粒子來近似目標分布；另一方面重采樣可能剔除有效粒子導致粒子數(shù)量枯竭.在本文中根據(jù)當前的有效粒子數(shù)目來決定是否要執(zhí)行從采樣操作.有效粒子數(shù)目Neff表示粒子集的退化程度,值越小退化越嚴重,需要進行重采樣.Neff如下定義:

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

2.3 改進算法流程描述

2)根據(jù)掃描匹配算法估計具有最大觀測后驗概率的機器人目標位姿.

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

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

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

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

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

算法流程圖見圖1.

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

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

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

圖1 改進算法流程圖

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

4 實驗結(jié)果

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

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

圖3 3 維可視化工具RVIZ

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

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

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

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

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

表1 改進算法的平均計算時間(ms)

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

5 總結(jié)

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