基于激光雷達與視覺信息融合的SLAM方法

王光庭，曹 凱，劉 豪

(山東理工大學 交通與車輛工程學院，山東 淄博 255049)

同時定位和地圖構建(SLAM)技術是機器人在未知的環境中完成跟蹤、偵查、導航等任務的關鍵和基礎[1]。在實際的應用中，機器人在不同的環境中執行不同的任務，對傳感器的選擇也是不盡相同。對于家庭服務型移動機器人選用單一的傳感器會存在檢測地圖信息不全面、檢測精度不夠或是機器人定位誤差太大等諸多問題[2-3]。如果采用多線激光雷達能夠解決此問題，但會大幅度提高成本，市面上，4線或8線激光雷達的價格就將近10萬元，而單線激光雷達只有幾千元。為了降低成本，通過多傳感器的融合技術能夠使各廉價傳感器測量的信息實現互補，能夠極大地提高機器人SLAM系統的魯棒性及自身定位與地圖構建的精度。本文提出一種融合單線激光雷達信息和視覺信息的方法，實現兩種冗余信息的互補，以提高地圖的精度，保證地圖信息的完整性和移動機器人導航的實時性。

1 傳感器性能及功能

Kinect是微軟推出的一款RGB-D視覺傳感器，如圖1所示。 Kinect有三個攝像頭，中間的鏡頭是彩色攝像機，用來采集彩色的信息(RGB)，左右兩邊的攝像頭分別是紅外線發射器和紅外線CMOS攝像機所構成的3D結構光深度感應器，用來采集深度信息(Depth)，也就是環境中物體到攝像頭的距離信息[4]。Kinect的水平視角為57.5°，垂直視角為43.5°，能夠感知的距離為0.8～3.5 m。RPLIDAR A2 是由SLAMTEC公司開發的新一代低成本二維激光雷達，如圖2所示，它可以實現在二維平面的8 m半徑范圍內進行360°激光測距掃描，激光測距采樣頻率為每秒4 000次。兩者參數對比見表1。

圖1 Kinect視覺傳感器Fig.1 Kinect vision sensor

圖2 RPLIDAR A2激光雷達Fig.2 RPLIDAR A2 sensor

表1 傳感器參數對比
Tab.1 The comparison of sensor parameters

參數水平視角/(°)垂直視角/(°)感知范圍/mKinectRplidar57.5360 43.5 00.8～3.50.15～8

Kinect視覺傳感器和RPLIDAR A2激光雷達測量的范圍如圖3所示，從圖中可以看出，單線激光雷達測量的是二維平面信息，而Kinect視覺傳感器測量的是三維的空間信息，很明顯，視覺傳感器所感知的空間信息要比單線激光雷達的豐富，而單線激光雷達所測量的范圍比視覺傳感器的大。本文的研究意義在于保證家庭服務型移動機器人低成本的基礎上，結合視覺傳感器Kinect和單線激光雷達RPLIDAR A2各自的優點，形成更寬闊的測量范圍。

圖3 傳感器的感知范圍Fig.3 Sensing range of the sensor

2 粒子濾波理論

粒子濾波[5-6](Particle Filter)理論思想是基于蒙特卡洛(Monte Carlo)方法，是利用粒子集來表示概率密度，通過從后驗概率(觀測模型)中抽取隨機狀態分布的粒子來表達分布，可以使用在任何形式的狀態空間模型上。Rao-Blackwellized 粒子濾波理論是對傳統粒子濾波的改進，它的核心思想是對機器人的路徑姿態和地圖進行分解。

Rao-Blackwellized粒子濾波算法實現的具體步驟如下：

1)狀態初始化

(1)

2)初采樣

根據粒子分布規則采集初始樣本，從當前的粒子集中產生新的下一狀態的粒子集。那么，此時機器人運動模型的概率密度函數可表示為

q=p(st|ut,st-1)

(2)

3)計算粒子的權值

粒子權值計算的遞推公式如下

ωt-1p(zt/st,zt-1)

(3)

式中：zt-1為t-1時刻的觀測值；zt為t時刻的觀測值；st，st-1分別為t時刻和t-1時刻機器人的位姿。

4)重采樣

在理論上，所有粒子都存在退化問題，為了避免這種由粒子退化問題導致的權值“兩極分化”現象，需要進行重采樣。設立一個衡量是否需要對粒子進行重采樣的變量，并且設定一個閾值，當變量的值低于給定閾值時，說明粒子退化嚴重，需要進行重采樣[7-8]。

5)狀態參數估計

以激光雷達獲取到的數據作為依據，用機器人當前的位姿估計對下一步的粒子濾波進行采樣，高斯濾波的估計參數如下

(4)

(5)

歸一化參數為

(6)

設機器人與路標的距離為r，機器人與路標的夾角為α，機器人自身的位姿為st，機器觀測到的路標為θmt，則觀測方程與其雅克比矩陣如下

(7)

(8)

式中的參數q如下

q=(θmt,y-st,y)2+(θmt,x-st,x)2

(9)

3 激光雷達和視覺信息融合

本文的實驗是在Turtlebot2機器人平臺上完成的，機器人配置有微軟XBox360 Kinect視覺傳感器、RPLIDAR A2激光雷達以及處理這些設備之間的通信和數據的聯想酷睿i7 2.4GHz處理器。PC端搭載的是在Linux(ubuntu14.04)系統下運行的ROS(Robot Operating System)Indigo操作系統[9]。搭建的融合單線激光雷達和視覺傳感器Kinect平臺如圖4所示。

圖4 移動機器人操作平臺Fig.4 Mobile robot operating platform

3.1 基于單線激光雷達的地圖構建

單線激光雷達是通過掃描二維平面得到的數據來進行地圖構建的，當遇到圖5所示的低于激光雷達掃描平面的障礙物，無法進行掃描，導致構建的地圖信息不夠全面。在實際的應用中，這些地圖中缺失的環境信息，會對機器人的自身定位、導航以及路徑規劃等造成嚴重的后果，只用單線激光雷達構建的柵格地圖如圖6所示，該地圖是用ROS系統中的Gmapping包來構建的，該包中的定位算法是由改進粒子濾波Rao-Blackwellized算法來實現的。從地圖中可以看出，沒有低于單線激光雷達掃描平面的障礙物2的信息。

圖5 激光雷達掃描缺陷Fig.5 Defects of lidar scanning

圖6 RPLIDARA2的地圖構建Fig.6 Mapping of RPLIDAR A2

3.2 基于Kinect的點云提取

Kinect視覺傳感器獲取到的是三維的環境信息，然后提取三維點云，設置閾值，取一定高度范圍內的點云，濾除全部外點，保證計算的簡潔性。本文使用的是ROS-Indigo系統中的Pointcloud-to-laserscan包，需要將三維點云投影到二維的平面上，形成二維的離散障礙圖。

Kinect獲取的三維點云信息如圖7所示，從圖中可以看出，三維信息對機器人運動環境的描述比單線激光雷達的二維信息要豐富許多。

圖7 Kinect獲取的點云Fig.7 The point cloud obtained by Kinect

3.3 信息融合的過程

單線激光雷達獲取的激光數據和Kinect獲取的視覺信息數據的融合過程流程如圖8所示。

在地圖的更新階段，采用Bayes估計方法。Bayes估計是基于Bayes定理的條件或后驗概率的統計數據融合算法，它適用于具有可加高斯噪聲的不確定性信息，可以通過已知的向量Z，估計出未知狀態下n維向量X。假定完成既定任務所需的有關的環境信息向量為f，通過傳感器獲得的數據信息用向量d表示，則數據融合的任務就是要通過數據向量d推導來估計環境向量f，假設p(f,d)是隨機向量f和d的聯合概率分布密度函數，則有Bayes公式如下

p(f,d)=p(f|d)·P(d)=

p(f|d)·p(f)

(10)

式中：p(d)、p(f)分別表示d和f的邊緣分布密度函數。

本實驗傳感器是從不同坐標系統獲取數據，以間接的方式采用Bayes估計進行數據融合，為保證數據信息的一致性，在融合之前，需要對數據一致性進行檢驗[10]，檢驗公式如下

(11)

圖8 信息融合流程圖Fig.8 The flow chart of information fusion

4 實驗

4.1 建圖

本實驗中單線激光雷達的離地高度是32 cm，而設置的障礙物的高度是26 cm，障礙物沒有在單線激光雷達的掃描平面內，在使用單一的傳感器RPLIDAR A2進行建圖的過程中，根本無法識別這樣的障礙物。具體的場景如圖9所示。

圖9 RPLIDAR A2與障礙物的位置Fig.9 Position of RPLIDAR A2 and obstacles

利用單一的激光雷達傳感器RPLIDAR A2來做SLAM，建圖的結果如圖10所示，掃描建立的二維柵格地圖信息不夠全面，圖9中障礙物信息沒能在柵格地圖中出現。而將Kinect獲取到的視覺信息加入以后，會發現圖9中障礙物信息掃描出現在建立的柵格地圖中。

圖10中，(a)是假設的場景圖，(b)是使用單一的激光雷達傳感器RPLIDAR A2掃描建立的二維柵格地圖，(c)是融合Kinect視覺信息建立的二維柵格地圖。從圖中可以看出，掃描的范圍基本沒有變化，保持了單線激光雷達掃描范圍大的特點，而且地圖(b)中沒有掃描到的障礙物信息在地圖(c)中清晰地顯示出來。

(a)場景圖

(b)激光信息柵格地圖 (c)融合視覺信息柵格地圖圖10 建圖過程Fig.10 Mapping

4.2 導航

圖11 障礙物場景Fig.11 The scene of obstacle

(a)初始規劃路徑 (b)重新規劃路徑

(c)避開障礙物 (d)導航完成圖12 導航過程Fig.12 Navigation

在已有的地圖中進行導航，使用圖10中的柵格地圖(b)做先驗地圖，此時，先驗地圖中無設定的障礙物信息。利用先驗地圖，實驗場景如圖11所示，進行導航的過程如圖12所示。圖12中，(a)中規劃的路徑是直接穿過設定的障礙物，那是因為Kinect的感知距離比較小，障礙物沒在Kinect的感知范圍之內；(b)中，隨著移動平臺Turtlebot2接近障礙物，障礙物進入Kinect的感知范圍，檢測到障礙物的信息以后，對路徑進行了動態規劃；(c)圖中，移動平臺Turtlebot2成功避開障礙物；(d)圖中，障礙物的信息成功出現在代價地圖里面。

5 結束語

本文針對家庭服務型移動機器人模型，通過實際場景的建圖和導航實驗可以看出，將單線激光雷達RPLIDAR A2獲取的信息和Kinect信息融合的低成本化地圖更加接近實際環境，不但滿足了室內移動機器人對環境信息的需求，而且比使用單一的傳感器建圖和導航效果更加準確。