王消為,賀利樂,趙 濤

(西安建筑科技大學(xué)機(jī)電工程學(xué)院,西安 710055)

同步定位與地圖構(gòu)建SLAM(Simultaneous Localization and Mapping)是移動機(jī)器人研究領(lǐng)域的熱點和難點,是移動機(jī)器人在未知環(huán)境中實現(xiàn)自主導(dǎo)航的前提。SLAM是指移動機(jī)器人在未知環(huán)境移動過程中通過自身攜帶的傳感器獲得環(huán)境信息增量式建立環(huán)境地圖,同時利用已建立的地圖來更新自身的位置與位姿。同步定位與地圖構(gòu)建過程中位置和地圖的估算形成了一個相輔相成、不斷迭代的過程,其本質(zhì)是一個狀態(tài)估計問題,主要通過基于濾波器或基于平滑的方法進(jìn)行求解。文獻(xiàn)[1-4]中采用Doucet等學(xué)者提出基于Rao-Blackwellized粒子濾波器RBPF(Rao-Black wellized Particle Filters)的SLAM方法,此算法能夠較好的近似移動機(jī)器人位姿和環(huán)境地圖的聯(lián)合概率密度,但其存在計算復(fù)雜度較高、占用較大內(nèi)存實時性較差等問題,因此之后很多學(xué)者針對該算法存在的問題進(jìn)行了多種不同形式的改進(jìn);文獻(xiàn)[5-6]中對激光雷達(dá)與單目視覺相融合的SLAM進(jìn)行了研究,將移動機(jī)器人所處環(huán)境中的某些相關(guān)特征進(jìn)行聯(lián)合形成有意義的環(huán)境地標(biāo),雖然有效減少數(shù)據(jù)的不確定性,但其計算量大實時性差;文獻(xiàn)[7]中提出了一種基于退火參數(shù)優(yōu)化混合提議分布的RBPF算法,該算法不僅能夠減少所需粒子數(shù)而且保持了粒子多樣性,但實驗驗證其系統(tǒng)魯棒性較低;文獻(xiàn)[8]中采用了在計算提議分布時將里程計信息與激光采集的距離信息進(jìn)行融合,驗證了其可以有效減少了所需粒子數(shù)并降低了預(yù)測階段機(jī)器人位姿的不確定性,但由于激光數(shù)據(jù)比較單一其實驗效果仍然不佳,因此本文提出在計算提議分布時將移動機(jī)器人所有的觀測數(shù)據(jù)與里程計信息融合。

針對上述在對未知環(huán)境中移動機(jī)器人研究中利用單一傳感器,自主導(dǎo)航時出現(xiàn)不能及時定位、構(gòu)建地圖不精確等問題,本文提出在計算提議分布時將移動機(jī)器人所有的觀測數(shù)據(jù)與里程計信息融合,并重點進(jìn)行基于激光雷達(dá)與雙目視覺信息融合的移動機(jī)器人同步定位與地圖構(gòu)建研究。

1 移動機(jī)器人的同步定位與地圖構(gòu)建

1.1 SLAM問題描述

SLAM問題指的是僅依賴觀測信息zt和控制信息ut的情況下確定機(jī)器人的位姿并構(gòu)建出地圖,即在未知環(huán)境中移動遞增式構(gòu)建環(huán)境地圖,同時利用構(gòu)建的環(huán)境地圖對自身的位姿進(jìn)行估計。SLAM問題示意圖如圖1所示。

圖1 SLAM問題示意圖

圖1中實心圓表示移動機(jī)器人真實的信息情況,空心圓表示估計信息情況,箭頭表示估計信息情況;xt表示t時刻的實際位姿向量,則x1:t表示機(jī)器人的軌跡;zt,k表示t時刻對第k個路標(biāo)的觀測,則z1:t表示觀測信息;ut表示移動機(jī)器人的控制信息,使其從xt-1到達(dá)位姿xt,則u1:t表示控制的歷史信息;mk表示第k個路標(biāo)的位置狀態(tài)向量,則m表示地圖。

因為移動機(jī)器人SLAM問題具有條件獨立性,且自身定位過程滿足一階隱馬爾科夫特性,所以當(dāng)移動機(jī)器人的軌跡完全確定時,對各個路標(biāo)的觀測是相互獨立的。則通過Rao-Blackwellise方法將SLAM表示為[1]:

(1)

Rao-Blackwellized SIR濾波器地圖構(gòu)建過程中增量式的處理獲得的數(shù)據(jù)作為提議分布q,實現(xiàn)SLAM的步驟如下:

(2)

④重采樣:當(dāng)有效粒子數(shù)小于預(yù)先設(shè)置的閾值Neff時,根據(jù)粒子權(quán)值的大小,用權(quán)值大的粒子代替權(quán)值小的粒子,進(jìn)行重新采樣。重采樣后,所有粒子具有相同的權(quán)重。

而Doucet等[1-2]人提出限制提議分布q獲得一個遞歸公式來計算重要性權(quán)重

q(x1:t|z1:t,u1:t-1)=q(xt|x1:t-1,z1:t,u1:t-1)·
q(x1:t-1|z1:t-1,u1:t-2)

(3)

則相應(yīng)的計算權(quán)重公式為:

(4)

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

1.2.1 提議分布改進(jìn)

由于里程計運動模型噪聲比較大,而一般改進(jìn)RBPF-SLAM一般將里程計運動模型作為提議分布,因此會使得各粒子間的權(quán)值差別很大,導(dǎo)致粒子出現(xiàn)乏匱現(xiàn)象。因此,提出基于改進(jìn)提議分布(即在計算提議分布時將所有觀測數(shù)據(jù)和里程計信息融合)的改進(jìn)RBPF算法。即將觀測傳感器(激光雷達(dá)與雙目視覺當(dāng)前觀測信息)提取出來的融合信息zt融入到粒子提議分布中,以有效降低粒子匱乏問題的發(fā)生。采樣提議分布如下:

(5)

則計算重要性權(quán)重公式為:

(6)

1.2.2 基于改進(jìn)的RBPF-SLAM算法步驟

③根據(jù)式(5)求取提議分布,在提議分布中進(jìn)行粒子采樣;

④根據(jù)式(6)進(jìn)行計算并更新粒子權(quán)重,計算有效粒子數(shù)Neff判斷是否達(dá)到閾值,如未達(dá)到則進(jìn)行重新采樣;

2 特征處理及數(shù)據(jù)融合

考慮到移動機(jī)器人在運動過程中容易出現(xiàn)輪子打滑等問題導(dǎo)致里程計數(shù)據(jù)與實際運動距離不一致,二維激光雷達(dá)只能獲得在一定高度的某一平面上信息;視覺易受環(huán)境背景變化等條件影響。故提出應(yīng)用上述1.2節(jié)中改進(jìn)RBPF-SLAM算法進(jìn)行采用雙目視覺與激光雷達(dá)結(jié)合里程計數(shù)據(jù)信息融合,進(jìn)而實現(xiàn)移動機(jī)器人的自主導(dǎo)航。

圖2 特征點提取

2.1 基于ORB的特征提取與匹配

Ethan Rublee等人提出的ORB(Oriented Fast and rotated Brief)算法[9]是基于FAST特征檢測和BRIEF特征描述算子的新算子,此算法用灰度質(zhì)心法給FAST角點檢測基礎(chǔ)上加上方向,用旋轉(zhuǎn)矩陣和機(jī)器學(xué)習(xí)的方式給BRIEF加上方向特性,并通過建立圖像金字塔的方式加上尺度特性。算法的特征提取速度相對于SURF、SIFT等算法有非常大的提高,其主要步驟分為特征點檢測及特征點描述。圖像2為移動機(jī)器人所在實驗室環(huán)境下雙目視覺傳感器獲得的圖像中的ORB特征點提取效果,匹配效果如圖3所示。

圖3 特征點提取匹配

將上述獲得的特征級匹配點作為標(biāo)記路標(biāo),隨機(jī)選取標(biāo)記路標(biāo)點作為代表如圖2所示A、B、C、D,建立起包含標(biāo)記路標(biāo)點坐標(biāo)信息的路標(biāo)數(shù)據(jù)庫,從圖像中提取出的三維坐標(biāo)轉(zhuǎn)化至水平面上,并構(gòu)建出水平面上2D路標(biāo)地圖,選取的4個路標(biāo)點的坐標(biāo)如表1所示。

表1 選取的標(biāo)記路標(biāo)點(圖2中)三維坐標(biāo)

2.2 數(shù)據(jù)融合及步驟

基于多傳感器融合及組合導(dǎo)航的方法已經(jīng)在很多方面得到驗證及廣泛的應(yīng)用[10],來自不同傳感器的數(shù)據(jù)信息不僅可以提供冗余信息,而且能提高環(huán)境特征定位的精確性和特征識別的可靠性。

2.2.1 數(shù)據(jù)融合

移動機(jī)器人不同傳感器所感知獲得的環(huán)境特征要經(jīng)過融合之后才能加入到全局地圖中[11],因此要對雙目視覺系統(tǒng)獲得的數(shù)據(jù)信息ZV狀態(tài)向量與激光測距儀獲得的數(shù)據(jù)信息ZL狀態(tài)向量進(jìn)行處理,首先對這些數(shù)據(jù)信息進(jìn)行對應(yīng)于環(huán)境中同一特征匹配對(即數(shù)據(jù)關(guān)聯(lián)),然后進(jìn)行特征層次的匹配融合。其過程簡述如下:

從以上可以獲得其協(xié)方差矩陣:

2.2.1 數(shù)據(jù)融合步驟

①對激光測距數(shù)據(jù)進(jìn)行分析,得到環(huán)境中障礙物的距離信息;

②用ORB算法從雙目視覺傳感器中獲得的兩幅不同視角的圖像中尋找特征點,匹配并得到旋轉(zhuǎn)角度,將得到的匹配角度與里程計角度融合,使其更加接近實際角度;

③結(jié)合激光測距與雙目視覺傳感器,分別融合距離信息與旋轉(zhuǎn)角度信息,定位移動機(jī)器人的初始位置;

④根據(jù)激光雷達(dá)與雙目視覺傳感器獲得的路標(biāo)信息,構(gòu)建特征地圖;

⑤最后采用上述改進(jìn)的RBPF算法進(jìn)行移動機(jī)器人的自主導(dǎo)航。

圖4 履帶式移動機(jī)器人

3 實驗及分析

3.1 實驗平臺及環(huán)境

本文實驗在一臺履帶式移動機(jī)器人(如圖4)進(jìn)行,移動機(jī)器人帶有HOKUYO二維激光測距儀URG-04LX、雙目攝像機(jī)系統(tǒng)Bumblebee2、里程計及一個串口連接的控制計算機(jī)等附件,控制計算機(jī)為英特爾奔騰雙核、主頻2.93 GHz、內(nèi)存4 G。控制計算機(jī)上的操作系統(tǒng)為Ubuntu14.04 LTS版本,且其系統(tǒng)上安裝了Indigo版本的ROS,通訊采用54 M帶寬的802.11 g無線通訊協(xié)議,通過與另外一臺安裝有Ubuntu14.04操作系統(tǒng)及Indigo版本的ROS的筆記本電腦(PC)建立通信,進(jìn)行遠(yuǎn)程連接并控制機(jī)器人的運動,并用Rviz顯示出所構(gòu)建的地圖。

實驗環(huán)境為學(xué)校附體樓研究生實驗室及外部走廊作為實驗環(huán)境。

3.2 ROS及導(dǎo)航實現(xiàn)流程

ROS是一種開源的機(jī)器人操作系統(tǒng)或者說是次級操作系統(tǒng)[13],它能提供類似操作系統(tǒng)所提供的功能,如硬件抽象描述、底層驅(qū)動程序管理、共用功能的執(zhí)行、程序間的消息傳遞等,它也提供一些工具程序和庫用于獲取、建立、編寫和運行多機(jī)整合的程序,其具有點對點設(shè)計、不依賴編程語言、開源等優(yōu)點。本文實驗研究時正是充分利用了ROS的這些優(yōu)點及其分布式的處理框架。移動機(jī)器人導(dǎo)航實現(xiàn)的流程如下:

①使遠(yuǎn)程控制計算機(jī)和移動機(jī)器人控制計算機(jī)連接到同一個WIFI網(wǎng)絡(luò)下;

②在遠(yuǎn)程控制計算機(jī)上建立節(jié)點管理器、SLAM、路徑規(guī)劃等節(jié)點;

③在移動機(jī)器人控制計算機(jī)上創(chuàng)建各個傳感器采集的數(shù)據(jù)信息及基礎(chǔ)控制器等節(jié)點;

④把以上創(chuàng)建的所有節(jié)點在節(jié)點管理器中進(jìn)行注冊并由節(jié)點管理器統(tǒng)一管理,即實現(xiàn)了在同一網(wǎng)絡(luò)中通過端對端的拓?fù)浣Y(jié)構(gòu)進(jìn)行不同計算機(jī)上節(jié)點間的有效通信;

⑤最后可利用遠(yuǎn)程計算機(jī)上的3D可視化工具RVIZ實時顯示移動機(jī)器人自主導(dǎo)航的狀態(tài)信息。

3.3 實驗結(jié)果及分析

分別使用上述改進(jìn)的RBPF算法與文獻(xiàn)[7]提出的一般改進(jìn)RBPF算法在實驗室內(nèi)及實驗室外部走廊環(huán)境下進(jìn)行移動機(jī)器人的自主導(dǎo)航實驗,且每個環(huán)境下分別重復(fù)進(jìn)行10次實驗,實驗數(shù)據(jù)如表2所示。移動機(jī)器人的移動速度設(shè)置為0.5 m/s,通過遠(yuǎn)程計算機(jī)(PC)機(jī)上的ROS中可視化工具RVIZ可以實時顯示地圖創(chuàng)建的過程如圖5所示。

表2 創(chuàng)建一致性地圖時參數(shù)表

圖5 RVIZ顯示的實時地圖

圖6為在實驗室內(nèi)環(huán)境下采用多傳感器融合的一般改進(jìn)RBPF算法48個粒子構(gòu)建的2D柵格環(huán)境地圖,由于其算法僅使用里程計作為提議分布,因此隨著時間的推移里程計累計誤差越來越大,所構(gòu)建出來的地圖出現(xiàn)了不一致現(xiàn)象。圖7為在實驗室內(nèi)使用上述多傳感器融合改進(jìn)的RBPF算法15個粒子構(gòu)建出精確的2D柵格環(huán)境地圖。圖8、圖9為在實驗室外部走廊環(huán)境下采用多傳感器融合的一般改進(jìn)RBPF算法與上述多傳感器融合改進(jìn)的RBPF算法構(gòu)建出的2D柵格環(huán)境地圖。

圖8 一般改進(jìn)算法構(gòu)建的地圖

圖6 一般改進(jìn)算法構(gòu)建的地圖

圖7 本文改進(jìn)算法構(gòu)建的地圖

圖9 本文改進(jìn)算法構(gòu)建的地圖

從表2明顯可以看出一般改進(jìn)RBPF算法與本文改進(jìn)RBPF算法創(chuàng)建程度一致性地圖時,本文改進(jìn)RBPF算法需要的粒子數(shù)目遠(yuǎn)小于一般改進(jìn)RBPF算法,而且其構(gòu)建地圖的過程中所用的時間也得到了大大的縮短,有效提高了算法的運算效率。從上面一般改進(jìn)RBPF算法與本文改進(jìn)RBPF算法所構(gòu)建的地圖中可以看出一般改進(jìn)算法隨著時間的推移,構(gòu)建出的地圖會出現(xiàn)重復(fù)定位不一致的現(xiàn)象,但采用本文改進(jìn)RBPF算法時圖中可以看出構(gòu)建的地圖相對穩(wěn)定、精確。

在移動機(jī)器人導(dǎo)航實時性可以容忍的情況下,一般改進(jìn)算法與改進(jìn)算法分別選取6,8,12,18,26,32,38,48,52,68;10組粒子數(shù)在實驗室內(nèi)環(huán)境下進(jìn)行實驗,每個條件下進(jìn)行α=20次,創(chuàng)建地圖成功的次數(shù)為β得出相應(yīng)粒子數(shù)下地圖構(gòu)建的成功率s,則計算公式為:s=(β/α)×100%,構(gòu)建地圖粒子數(shù)與成功率的關(guān)系如圖10所示。

圖10 構(gòu)建地圖成功率與粒子數(shù)的關(guān)系曲線

從圖10中對比本文改進(jìn)算法與一般改進(jìn)算法構(gòu)建地圖粒子數(shù)與成功率的關(guān)系可以觀察到,本文改進(jìn)算法構(gòu)建出一個高精度的柵格地圖僅需12個粒子數(shù),而一般改進(jìn)算法需要的粒子數(shù)則多達(dá)32個;當(dāng)移動機(jī)器人在同一個環(huán)境下,使用同樣多的粒子數(shù)創(chuàng)建環(huán)境地圖時,本文改進(jìn)算法的成功率明顯高于一般改進(jìn)算法,有效提高了移動機(jī)器人SLAM的魯棒性。

為了進(jìn)一步地分析基于改進(jìn)RBPF算法組合導(dǎo)航的優(yōu)勢,通過對改進(jìn)算法與一般改進(jìn)算法在MATLAB中仿真位姿狀態(tài)估計精確性分析得到的對比曲線如圖11所示,明顯可以觀察到本文改進(jìn)算法的曲線趨勢相對平滑,即位姿估計的均方根誤差小于一般改進(jìn)算法。因此,本文改進(jìn)算法的狀態(tài)估計精度優(yōu)于一般改進(jìn)算法。這正是因為文章在提議分布中加入了激光雷達(dá)與雙目視覺的融合信息,使得粒子的分布更加的接近其真實狀態(tài)。

圖11 狀態(tài)估計誤差曲線

4 結(jié)論

①通過實驗驗證表明采用文章提出的改進(jìn)RBPF算法,在計算提議分布時將移動機(jī)器人的所有觀測數(shù)據(jù)與里程計信息融合,有效減少了所需粒子的數(shù)量,且降低了粒子濾波器預(yù)測階段移動機(jī)器人的不確定性;

②通過本文提出一種雙目視覺與激光雷達(dá)信息融合的組合導(dǎo)航方法,建立包含世界坐標(biāo)系的路標(biāo)數(shù)據(jù)庫,并獲得全局環(huán)境下的特征地圖進(jìn)而實現(xiàn)自主導(dǎo)航。驗證了多傳感器融合提供的冗余信息能夠構(gòu)建可靠性更高、更精確的2D柵格環(huán)境地圖,能夠有效提高移動機(jī)器人SLAM的魯棒性。

[1] Montemerlo M,Thrun S,Koller D,et al. FastSLAM 2.0:An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping That Provably Converges[C]//Menlo Park CA,USA,AAAI Press 2003,1151-1156.

[2] Doucet A,Freitas N,GORDAN. Sequential Monte Carlo Methods in Practice[J]. New York:Springer Verlag,2001:496-497.

[3] 周紹磊,吳修振,劉剛,等. 一種單目視覺ORB-SLAM/INS組合導(dǎo)航方法[J]. 中國慣性技術(shù)學(xué)報,2016,24(5):633-637.

[4] 朱磊,樊繼壯,趙杰,等. 改進(jìn)粒子濾波器的移動機(jī)器人同步定位與地圖構(gòu)建方法[J]. 重慶大學(xué)學(xué)報,2014,37(4):39-45.

[5] Castellanos J A,Neira J,Tardos J D. Multisensor Fusion for Simultaneous Localization and Mapping[J]. IEEE Trans Robot Automat,2001,17(6):908-914.

[6] 莊嚴(yán),王偉,王珂. 移動機(jī)器人基于激光測距和單目視覺的室內(nèi)同步定位與地圖構(gòu)建[J]. 自動化學(xué)報,2005,31(6):925-933.

[7] 羅元,蘇琴,張毅,等. 基于優(yōu)化RBPF的同步定位與地圖構(gòu)建[J]. 華中科技大學(xué)學(xué)報,2016,44(5):30-34.

[8] 羅元,傅有力,程鐵鳳. 基于改進(jìn)Rao-Blackwellized粒子濾波器的同步定位與地圖構(gòu)建[J]. 控制理論與應(yīng)用,2015,32(2):267-272.

[9] Rublee E,Rabaud V,Konolige K,et al. ORB:An Efficient Alternative to SIFT or SURF[C]//IEEE International Conference on Computer Vision(ICCV). Barcelona,Spain,2011:2564-2571.

[10] 張毅,杜凡宇,羅元,等. 一種融合激光和深度視覺傳感器的SLAM地圖創(chuàng)建方法[J]. 計算機(jī)應(yīng)用研究,2016,33(10):2970-2972.

[11] 陳曉飛,凌有鑄,陳孟元. WSNs環(huán)境下基于高斯混合容積卡爾曼濾波的移動機(jī)器人定位算法[J]. 傳感技術(shù)學(xué)報,2017,30(1):133-138.

[12] 許曈,凌有鑄,陳孟元,等. 基于姿態(tài)測量模塊和閉環(huán)檢測算法的仿生SLAM研究[J]. 傳感技術(shù)學(xué)報,2017,30(6):916-921.

[13] 張錦濤,張瑞雷,譯. ROS機(jī)器人程序設(shè)計[M]. 北京:機(jī)械工業(yè)出版社,2016.