王光庭，曹 凱，劉 豪

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

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

1 傳感器性能及功能

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

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

圖2 RPLIDAR A2激光雷達(dá)Fig.2 RPLIDAR A2 sensor

表1 傳感器參數(shù)對(duì)比
Tab.1 The comparison of sensor parameters

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

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

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

2 粒子濾波理論

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

Rao-Blackwellized粒子濾波算法實(shí)現(xiàn)的具體步驟如下：

1)狀態(tài)初始化

(1)

2)初采樣

根據(jù)粒子分布規(guī)則采集初始樣本，從當(dāng)前的粒子集中產(chǎn)生新的下一狀態(tài)的粒子集。那么，此時(shí)機(jī)器人運(yùn)動(dòng)模型的概率密度函數(shù)可表示為

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

(2)

3)計(jì)算粒子的權(quán)值

粒子權(quán)值計(jì)算的遞推公式如下

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

(3)

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

4)重采樣

在理論上，所有粒子都存在退化問題，為了避免這種由粒子退化問題導(dǎo)致的權(quán)值“兩極分化”現(xiàn)象，需要進(jìn)行重采樣。設(shè)立一個(gè)衡量是否需要對(duì)粒子進(jìn)行重采樣的變量，并且設(shè)定一個(gè)閾值，當(dāng)變量的值低于給定閾值時(shí)，說明粒子退化嚴(yán)重，需要進(jìn)行重采樣[7-8]。

5)狀態(tài)參數(shù)估計(jì)

以激光雷達(dá)獲取到的數(shù)據(jù)作為依據(jù)，用機(jī)器人當(dāng)前的位姿估計(jì)對(duì)下一步的粒子濾波進(jìn)行采樣，高斯濾波的估計(jì)參數(shù)如下

(4)

(5)

歸一化參數(shù)為

(6)

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

(7)

(8)

式中的參數(shù)q如下

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

(9)

3 激光雷達(dá)和視覺信息融合

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

圖4 移動(dòng)機(jī)器人操作平臺(tái)Fig.4 Mobile robot operating platform

3.1 基于單線激光雷達(dá)的地圖構(gòu)建

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

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

圖6 RPLIDARA2的地圖構(gòu)建Fig.6 Mapping of RPLIDAR A2

3.2 基于Kinect的點(diǎn)云提取

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

Kinect獲取的三維點(diǎn)云信息如圖7所示，從圖中可以看出，三維信息對(duì)機(jī)器人運(yùn)動(dòng)環(huán)境的描述比單線激光雷達(dá)的二維信息要豐富許多。

圖7 Kinect獲取的點(diǎn)云Fig.7 The point cloud obtained by Kinect

3.3 信息融合的過程

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

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

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

p(f|d)·p(f)

(10)

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

本實(shí)驗(yàn)傳感器是從不同坐標(biāo)系統(tǒng)獲取數(shù)據(jù)，以間接的方式采用Bayes估計(jì)進(jìn)行數(shù)據(jù)融合，為保證數(shù)據(jù)信息的一致性，在融合之前，需要對(duì)數(shù)據(jù)一致性進(jìn)行檢驗(yàn)[10]，檢驗(yàn)公式如下

(11)

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

4 實(shí)驗(yàn)

4.1 建圖

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

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

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

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

(a)場景圖

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

4.2 導(dǎo)航

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

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

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

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

5 結(jié)束語

本文針對(duì)家庭服務(wù)型移動(dòng)機(jī)器人模型，通過實(shí)際場景的建圖和導(dǎo)航實(shí)驗(yàn)可以看出，將單線激光雷達(dá)RPLIDAR A2獲取的信息和Kinect信息融合的低成本化地圖更加接近實(shí)際環(huán)境，不但滿足了室內(nèi)移動(dòng)機(jī)器人對(duì)環(huán)境信息的需求，而且比使用單一的傳感器建圖和導(dǎo)航效果更加準(zhǔn)確。