基于激光雷達與雙目視覺的移動機器人SLAM研究*

王消為,賀利樂,趙 濤

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

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

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

1 移動機器人的同步定位與地圖構建

1.1 SLAM問題描述

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

圖1 SLAM問題示意圖

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

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

(1)

Rao-Blackwellized SIR濾波器地圖構建過程中增量式的處理獲得的數據作為提議分布q,實現SLAM的步驟如下:

(2)

④重采樣:當有效粒子數小于預先設置的閾值Neff時,根據粒子權值的大小,用權值大的粒子代替權值小的粒子,進行重新采樣。重采樣后,所有粒子具有相同的權重。

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

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)

則相應的計算權重公式為:

(4)

1.2 改進的RBPF-SLAM算法

1.2.1 提議分布改進

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

(5)

則計算重要性權重公式為:

(6)

1.2.2 基于改進的RBPF-SLAM算法步驟

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

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

2 特征處理及數據融合

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

圖2 特征點提取

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

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

圖3 特征點提取匹配

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

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

2.2 數據融合及步驟

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

2.2.1 數據融合

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

從以上可以獲得其協方差矩陣:

2.2.1 數據融合步驟

①對激光測距數據進行分析,得到環境中障礙物的距離信息;

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

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

④根據激光雷達與雙目視覺傳感器獲得的路標信息,構建特征地圖;

⑤最后采用上述改進的RBPF算法進行移動機器人的自主導航。

圖4 履帶式移動機器人

3 實驗及分析

3.1 實驗平臺及環境

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

實驗環境為學校附體樓研究生實驗室及外部走廊作為實驗環境。

3.2 ROS及導航實現流程

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

①使遠程控制計算機和移動機器人控制計算機連接到同一個WIFI網絡下;

②在遠程控制計算機上建立節點管理器、SLAM、路徑規劃等節點;

③在移動機器人控制計算機上創建各個傳感器采集的數據信息及基礎控制器等節點;

④把以上創建的所有節點在節點管理器中進行注冊并由節點管理器統一管理,即實現了在同一網絡中通過端對端的拓撲結構進行不同計算機上節點間的有效通信;

⑤最后可利用遠程計算機上的3D可視化工具RVIZ實時顯示移動機器人自主導航的狀態信息。

3.3 實驗結果及分析

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

表2 創建一致性地圖時參數表

圖5 RVIZ顯示的實時地圖

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

圖8 一般改進算法構建的地圖

圖6 一般改進算法構建的地圖

圖7 本文改進算法構建的地圖

圖9 本文改進算法構建的地圖

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

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

圖10 構建地圖成功率與粒子數的關系曲線

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

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

圖11 狀態估計誤差曲線

4 結論

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

②通過本文提出一種雙目視覺與激光雷達信息融合的組合導航方法,建立包含世界坐標系的路標數據庫,并獲得全局環境下的特征地圖進而實現自主導航。驗證了多傳感器融合提供的冗余信息能夠構建可靠性更高、更精確的2D柵格環境地圖,能夠有效提高移動機器人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組合導航方法[J]. 中國慣性技術學報,2016,24(5):633-637.

[4] 朱磊,樊繼壯,趙杰,等. 改進粒子濾波器的移動機器人同步定位與地圖構建方法[J]. 重慶大學學報,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] 莊嚴,王偉,王珂. 移動機器人基于激光測距和單目視覺的室內同步定位與地圖構建[J]. 自動化學報,2005,31(6):925-933.

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

[8] 羅元,傅有力,程鐵鳳. 基于改進Rao-Blackwellized粒子濾波器的同步定位與地圖構建[J]. 控制理論與應用,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地圖創建方法[J]. 計算機應用研究,2016,33(10):2970-2972.

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

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

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