一種融合深度相機(jī)與激光雷達(dá)的室內(nèi)移動(dòng)機(jī)器人建圖與導(dǎo)航方法

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

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

0 引 言

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

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

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

1.1 傳感器類別

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

圖1 思嵐RPLIDAR A1激光雷達(dá)Fig.1 SLAMTEC RPLIDAR A1 lidar

圖2 奧比中光Astra Pro相機(jī)Fig.2 ORBBEC Astraa Pro camera

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2 深度相機(jī)與激光雷達(dá)融合建圖

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

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

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

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

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

3 室內(nèi)移動(dòng)機(jī)器人融合建圖實(shí)驗(yàn)

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

3.1 機(jī)器人建圖

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

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

圖6 深度相機(jī)仿激光點(diǎn)云圖Fig.6 The data that depth camera imitates laser point cloud

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

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

圖8 機(jī)器人實(shí)驗(yàn)平臺(tái)室內(nèi)實(shí)驗(yàn)Fig.8 Indoor experiment of robot experimental platform

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

圖9 激光雷達(dá)建圖Fig.9 Mapping of using laser

3.2 機(jī)器人導(dǎo)航

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

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

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

圖12 機(jī)器人進(jìn)行障礙物識(shí)別Fig.12 Robot is discriminatingobstacle

4 結(jié)束語

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