一種融合深度相機與激光雷達的室內(nèi)移動機器人建圖與導航方法

陳文佑，章 偉，胡 陟，史曉帆

（上海工程技術(shù)大學 機械與汽車工程學院，上海201620）

0 引 言

隨著人們對機器人的需求日益增大，室內(nèi)移動機器人技術(shù)逐漸成為研究的熱點。工業(yè)機器人、服務(wù)機器人、特種作業(yè)機器人等各種用于不同環(huán)境中的室內(nèi)移動機器人都需要一種關(guān)鍵的技術(shù)，即同時定位與建圖SLAM（Simultaneous Localization and Mapping）［1］。根據(jù)當前機器人研究領(lǐng)域的共識，實現(xiàn)室內(nèi)移動機器人智能導航與避障，必須在無GPS信號的情況下，確定機器人的位置并且建立起機器人運動環(huán)境的完整地圖。從20世紀80年代起，SLAM技術(shù)一直是移動機器人研究的重點和難點，研究者們一直在探索使用更加精確的傳感器，更加完善的算法，來精準定位機器人的位置，建立豐富完整的環(huán)境地圖，從而使機器人能夠在地圖中智能導航。

目前SLAM方案大致分為使用激光雷達傳感器的激光SLAM和使用視覺傳感器的視覺SLAM，激光SLAM發(fā)展較為成熟，常用的算法有基于粒子濾波框架的Gmapping算法，基于圖優(yōu)化框架的karto算法和cartographer算法等等。視覺SLAM方面，有基于直接法的大范圍單目定位與制圖（Large-Scale Direct Monocular SLAM，LSD-SLAM）方法、使用RGB-D攝像頭的RGBD-SLAM方法、基于點線特征的VSLAM算法PL-SLAM（Points and Line Segments SLAM），以及基于特征點的ORB-SLAM（Oriented FAST and Rotated BRIEF-SLAM）方法等。激光SLAM具有成本高、精度高、技術(shù)門檻低、獲取的信息較少等特點，視覺SLAM具有成本低、精度相對低、技術(shù)門檻高、獲取的信息豐富、應(yīng)用前景廣闊等特點。未來SLAM技術(shù)發(fā)展的一個方向是結(jié)合激光雷達與相機等傳感器的多傳感器信息融合SLAM方案，因此本文提出一種融合單線激光雷達與RGB-D相機的室內(nèi)移動機器人SLAM方案。搭建機器人實驗平臺，并在實驗平臺上進行導航實驗。

1 多傳感器種類及建圖算法

1.1 傳感器類別

本文所使用的激光雷達為思嵐生產(chǎn)的型號為RPLIDAR A1的單線激光雷達，如圖1所示。該雷達測距范圍為0.15m-12m，掃描角度為0°-360°，掃描頻率為5.5 Hz，采用激光三角測距技術(shù)，可進行每秒8 000次以上的測距動作，實現(xiàn)對周圍環(huán)境的360°掃描測距，從而獲得周圍環(huán)境的輪廓圖。使用的深度相機為奧比中光生產(chǎn)的Astra Pro相機，如圖2所示。該相機測量深度范圍0.6m-8m，1 m范圍內(nèi)精度在±（1-3）mm。

圖1 思嵐RPLIDAR A1激光雷達Fig.1 SLAMTEC RPLIDAR A1 lidar

圖2 奧比中光Astra Pro相機Fig.2 ORBBEC Astraa Pro camera

1.2 基于Gmapping算法的二維平面柵格地圖構(gòu)建

平面柵格地圖用于表示二維平面中障礙物存在的概率值，具有儲存空間較小、容易擴展、建圖速度快等優(yōu)點。Gmapping算法是一種基于RBPF（Rao-Blackwellized Particle Filters）粒子濾波框架的開源SLAM算法，可以實時構(gòu)建室內(nèi)柵格地圖，在構(gòu)建小場景地圖時所需的計算量較小且精度較高。Gmapping算法主要在RBPF算法上對提議分布進行改進。改進的提議分布不但考慮運動（里程計）信息還考慮最近的一次觀測（激光）信息，這樣就可以使提議分布更加精確，從而更加接近目標分布。

里程計提供位姿信息的不確定度要比激光大的多，激光的分布相比里程計分布更接近真正的目標分布，因此如果可以把激光的信息融入到提議分布中，提議分布就會更接近目標分布。

RBPF算法建圖聯(lián)合分布，式（1）：

其中：x1：t為位置信息；m為環(huán)境信息；u1：t為控制數(shù)據(jù)；z1：t為觀測數(shù)據(jù)。

為了改進提議分布，Gmapping使用最近的一次觀測，因此提議分布變?yōu)槭剑?）：

粒子的權(quán)重公式變?yōu)槭剑?）：

為了獲得改進的提議分布，第一步從運動模型中采集粒子，第二步對這些粒子加權(quán)以選出最好的粒子，然后用這些權(quán)重大的粒子來模擬出改進后的提議分布。目標分布大多數(shù)情況下只有一個峰值，從峰值附近采樣就可以大大簡化計算量，因此在峰值附近采K個值來模擬出提議分布。通常使用高斯函數(shù)來構(gòu)建提議分布，因此有了K個數(shù)據(jù)后就可以模擬出一個高斯函數(shù)來構(gòu)建提議分布，式（4）和式（5）：

有了模擬好的提議分布，就可以采樣出下一時刻機器人的位姿信息。有了位姿信息就可以局部建圖，從而生成二維柵格地圖。

1.3 深度圖轉(zhuǎn)激光原理

在將深度圖二維化時，使用ROS（Robot Operating System）機器人操作系統(tǒng)中的depthimage＿to＿laserscan功能包，將深度相機所獲得的深度圖像轉(zhuǎn)化為偽激光雷達二維平面圖，原理如圖3所示。深度圖轉(zhuǎn)激光中，對任意給定的一個深度圖像點m（u，v，z），其轉(zhuǎn)換激光的步驟為：

（1）將深度圖像的點m（u，v，z）轉(zhuǎn)換到深度相機坐標系下的坐標點M（x，y，z）；

（2）計算直線AO和CO的夾角AOC，計算公式（6）如下：

（3）將角AOC影射到相應(yīng)的激光數(shù)據(jù)槽中，已知激光的范圍［α，β］，激光束共細分為N份，那么可用數(shù)組laser［N］表示激光數(shù)據(jù)。那么點M投影到數(shù)組laser中的索引值n可如式（7）計算：

Laser［n］的值為M在x軸上投影的點C到相機光心O的距離r，即式（8）：

圖3 深度圖轉(zhuǎn)激光原理Fig.3 The principle of depthimage＿to＿laserscan

2 深度相機與激光雷達融合建圖

目前傳感器融合方法有卡爾曼濾波、貝葉斯估計、加權(quán)平均法、模糊邏輯、神經(jīng)網(wǎng)絡(luò)等［2］。本文采用貝葉斯方法，該方法是一種基于貝葉斯定理的條件或后驗概率的統(tǒng)計數(shù)據(jù)融合算法，通過已知向量Z，估計未知的n維狀態(tài)向量X。在一個狀態(tài)空間中，貝葉斯估計器提供一種計算后驗概率分布的方法，假設(shè)K時刻的概率為x k，已知K組測量Z k＝｛z1，…，z k｝和先驗分布如式（9）：

其中：p（z k｜x k）為基于給定的傳感器測量模型；p（x k｜Z k-1）為給定的轉(zhuǎn)換系統(tǒng)的模型；p（Z k｜Z k-1）是一種規(guī)格化術(shù)語，保證概率密度函數(shù)歸一化。

Kourosh提出使用貝葉斯遞推公式更新多傳感器觀測數(shù)據(jù)柵格圖［3］，遞推公式轉(zhuǎn)換為占據(jù)柵格結(jié)構(gòu)圖，可得融合公式（10）：

多傳感器融合框架如圖4所示。將深度相機和激光雷達集成到機器人中，雷達采集真實環(huán)境中的信息，生成室內(nèi)環(huán)境的二維點云輪廓，電機中安裝的編碼器獲得電機的轉(zhuǎn)動角度，輸出里程計數(shù)據(jù)，與激光數(shù)據(jù)融合，通過Gmapping算法構(gòu)建激光局部地圖。RGB-D相機采集真實環(huán)境中的視覺數(shù)據(jù)，建立三維局部環(huán)境地圖，通過ROS自帶的depthimage＿to＿laserscan功能包，將深度圖投影為二維點云點圖，通過貝葉斯濾波方法融合激光二維點云與深度投影點云，再進行點云拼接，生成局部融合柵格地圖，建圖未完成則返回環(huán)境信息采集步驟，循環(huán)執(zhí)行上述步驟，建圖完成則生成完整的全局融合柵格地圖。

圖4 多傳感器融合框架Fig.4 Multi-sensor fusion mapping framework

3 室內(nèi)移動機器人融合建圖實驗

本文搭建的室內(nèi)移動機器人實驗平臺為履帶式小車，如圖5所示。機器人配備的傳感器為思嵐RPLIDAR A1激光雷達、奧比中光Astra Pro相機和輪式里程計，上位機使用Intel（R）Core（TM）i5-7300U CPU＠2.60GHz，8GB內(nèi)存PC，下位機使用STM32F103RCT6底層驅(qū)動板，機器人移動底盤采用履帶式有刷電機底盤。上位機在Ubuntu16.04版本Linux操作系統(tǒng)下，使用ROS-kinetic版本機器人操作系統(tǒng)，運用Python和C++語言編寫程序，在封閉室內(nèi)條件下進行實驗。

3.1 機器人建圖

深度相機生成的仿3D激光雷達三維點云圖如圖6所示，該功能在ROS的pointcloud＿to＿laserscan功能包中實現(xiàn)。生成仿激光點云數(shù)據(jù)后，運用hector-slam算法，盡量保持深度相機水平放置，平穩(wěn)移動機器人實驗平臺構(gòu)建地圖，啟動depthimage＿to＿laserscan節(jié)點，所建的仿激光數(shù)據(jù)二維地圖如圖7所示。圖6和圖7展示了用深度相機構(gòu)建柵格地圖的效果，從圖7可以看出，深度相機所建的圖輪廓不分明，不清晰，鋸齒狀的線條較多，不能良好地反映出真實環(huán)境的二維地圖。

圖5 履帶式室內(nèi)移動機器人Fig.5 Tracked indoor mobile robot

圖6 深度相機仿激光點云圖Fig.6 The data that depth camera imitates laser point cloud

圖7 深度相機仿激光數(shù)據(jù)建圖Fig.7 Depth camera built map imitating laser data

激光雷達建圖則較好地解決了深度相機遇到的問題，但是它并不能掃描到處于雷達掃描平面之外的障礙物。如圖8所示，本機器人實驗平臺雷達安裝高度為距離地面19cm，障礙物A的高度為27cm，障礙物B的高度為13cm，紅色虛線表示雷達掃描平面，黑色虛線表示深度相機拍攝范圍。很顯然僅僅用激光雷達傳感器的，雷達掃描平面只能掃描到障礙物A，而掃描不到障礙物B，在這種情況下，即使優(yōu)化了路徑規(guī)劃與避障算法，當前方出現(xiàn)障礙物B時，機器人仍然不會避開，而是直接撞向障礙物B。

圖8 機器人實驗平臺室內(nèi)實驗Fig.8 Indoor experiment of robot experimental platform

本文在激光雷達建圖的基礎(chǔ)上，安裝深度視覺傳感器，來對2D雷達掃描不到的障礙物進行識別。為了避免深度相機將地面識別為障礙物，只截取距離地面5 cm以上的三維點云信息。在實驗前將室內(nèi)雜物清除，只留下障礙物A與障礙物B。僅使用激光雷達與里程計，利用Gmapping算法進行建圖之后的室內(nèi)二維地圖如圖9所示，很明顯機器人只識別到了高度高于雷達的障礙物A。融合深度視覺傳感器后，深度相機構(gòu)建障礙物B的三維點云并投影到二維柵格地圖上的效果如圖10所示，可以看出高度低于雷達的障礙物B也被識別了出來。實驗提高了地圖的完整性，為后期機器人精確、安全的導航奠定了基礎(chǔ)。

圖9 激光雷達建圖Fig.9 Mapping of using laser

3.2 機器人導航

在融合了激光雷達和深度相機后，機器人所構(gòu)建的室內(nèi)地圖如圖11所示。圖11中S點為機器人的起點，P1和P2點分別是人為設(shè)定的目標點，機器人先從S點到P1點規(guī)劃出一條最優(yōu)路線，到達P1點后再重新規(guī)劃P1到P2點的路線。路徑規(guī)劃的方法有遺傳算法、A*算法、狼群算法和人工勢場法等。該路徑規(guī)劃算法采用A*算法，A*算法是一種非常有效的尋路算法，它結(jié)合了Dijkstra’s算法路徑最短的優(yōu)點與快速搜索算法規(guī)劃速度快的優(yōu)點，能夠快速找到起點與目標點的最短路徑。機器人在建圖時不可避免地會產(chǎn)生一些噪點，這些噪點往往是來往的路人所產(chǎn)生的，在建圖完成后必須把這些噪點去除，以防止機器人導航時誤將噪點當作障礙物。同時，也要對地圖中不允許機器人進入的區(qū)域進行約束，并設(shè)置邊界膨脹范圍，約束的目的是防止機器人在導航過程中遇到障礙物主動避障時，誤闖入不應(yīng)當進入的區(qū)域，設(shè)置邊界膨脹范圍的目的是防止機器人與障礙物刮蹭，提高機器人運動的安全性。

機器人融合了深度相機后，提高了對障礙物的識別能力，如圖12所示。地圖上S點周圍一個個圓形是激光雷達融合深度相機識別出不同高度的障礙物后，并對障礙物進行邊界膨脹所形成的形狀。現(xiàn)在機器人也可以檢測到高度低于雷達安裝高度的障礙物，識別出更多的障礙物就有了更加完整的地圖，就可以進行更精確的導航與避障，避免了機器人與高度低于雷達的障礙物的碰撞。

圖11 機器人室內(nèi)建圖與導航Fig.11 Robot mapping and navigation indoor

圖12 機器人進行障礙物識別Fig.12 Robot is discriminatingobstacle

4 結(jié)束語

融合激光雷達與深度相機的移動機器人實驗平臺，相比使用單一激光雷達所構(gòu)建的地圖更加完善，對周圍環(huán)境的障礙物識別率更高，導航與避障效果也更好。但也有不足之處：一方面，融合視覺傳感器后，計算量大大增加，雷達建圖使用樹莓派3即可實現(xiàn)，融合了視覺傳感器后則需要在PC上才能正常運行，后期會考慮用ARM進行開發(fā)（Advanced RISC Machine）；另一方面，本實驗雖然提升了機器人檢測障礙物的能力，但是仍然沒有解決機器人經(jīng)過凹凸不平的路面時顛簸過大的問題，因為機器人自身的傳感器檢測不到不平整的路面，就無法在經(jīng)過這種路面時主動減速，從而導致機身顛簸幅度過大。后期將以此為研究目標，力求讓機器人在路過不平的路面時，能夠緩慢平穩(wěn)地通過，最大程度上減小機器人的顛簸。