基于ROS的移動(dòng)機(jī)器人SLAM研究與實(shí)現(xiàn)

曹家寧　陸馨妍　熊依琪

摘要： 近年來，隨著高新科學(xué)技術(shù)的不斷發(fā)展，移動(dòng)機(jī)器人的研究進(jìn)展越來越迅速。其中移動(dòng)機(jī)器人在未知環(huán)境下，實(shí)現(xiàn)同時(shí)定位與環(huán)境地圖構(gòu)建（SLAM，Simultaneous Localization and Mapping）過程是機(jī)器人研究的重要領(lǐng)域。本文簡(jiǎn)單介紹了ROS操作系統(tǒng)并研究了與 SLAM 問題相關(guān)的幾類系統(tǒng)模型及Gmapping_slam、Hector_slam和Google開源的Cartographer_slam等相對(duì)成熟的slam算法。并對(duì)上述的三種 SLAM 算法在實(shí)際應(yīng)用中的性能進(jìn)行分析。

關(guān)鍵詞：移動(dòng)機(jī)器人;ROS;SLAM

中圖分類號(hào)：TP311 ? ? ? ?文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1009-3044（2019）13-0199-02

引言

近些年，移動(dòng)機(jī)器人的應(yīng)用已滲透到各個(gè)行業(yè)中，如軍事偵察、醫(yī)學(xué)診療、物料搬運(yùn)、社會(huì)服務(wù)、工業(yè)和農(nóng)業(yè)生產(chǎn)等。正是由于如此廣泛的應(yīng)用，決定了移動(dòng)機(jī)器人的工作場(chǎng)景既復(fù)雜又具很多不能確定的因素。因此移動(dòng)機(jī)器人的定位能力、環(huán)境感知能力等對(duì)其完成任務(wù)的成功率、自身定位的準(zhǔn)確率和自身性能的完善起到了至關(guān)重要的作用。移動(dòng)機(jī)器人是否可以利用自身隨帶的各種傳感器采集到的周圍環(huán)境信息來進(jìn)行運(yùn)動(dòng)場(chǎng)景地圖的創(chuàng)建，以及通過預(yù)設(shè)的一些指令完成相關(guān)指令規(guī)劃的任務(wù)，在運(yùn)行的過程中完成自主分析，自主決策，最終順利到達(dá)目的地 ，這一問題已經(jīng)成為研究學(xué)者們學(xué)習(xí)和鉆研的關(guān)鍵。

隨著GPS技術(shù)的廣泛應(yīng)用，當(dāng)移動(dòng)機(jī)器人處于空曠、信號(hào)良好的室外環(huán)境時(shí)，上述問題可以利用高精度的GPS來解決。但在室內(nèi)或一些復(fù)雜環(huán)境中，GPS的信號(hào)很不穩(wěn)定時(shí)，就需要使用其他技術(shù)來解決移動(dòng)機(jī)器人定位及環(huán)境地圖創(chuàng)建問題。因此，研究學(xué)者們提出了SLAM。至今，SLAM已成為移動(dòng)機(jī)器人研究領(lǐng)域的一項(xiàng)重要課題。

1 ROS系統(tǒng)

ROS（Robot Operating System，下文簡(jiǎn)稱“ROS”）是一個(gè)適用于機(jī)器人的開源的元操作系統(tǒng)。它提供了操作系統(tǒng)應(yīng)有的服務(wù)，包括硬件抽象，底層設(shè)備控制，常用函數(shù)的實(shí)現(xiàn)，進(jìn)程間消息傳遞，以及包管理。

1.1 ROS優(yōu)勢(shì)

1.1.1多語(yǔ)言支持及開源環(huán)境

ROS由于開發(fā)者使用的語(yǔ)言不同，因此ROS獨(dú)立了開發(fā)語(yǔ)言，支持C++、python等多種語(yǔ)言。除了ROS提供的功能包之外，ROS還匯合了全球開源社區(qū)開發(fā)者實(shí)現(xiàn)的大量功能包。這些開源功能包與ROS一起創(chuàng)建了強(qiáng)大的開源生態(tài)環(huán)境。

1.1.2 結(jié)構(gòu)設(shè)計(jì)松耦合

程序運(yùn)行時(shí)是由多個(gè)松耦合的進(jìn)程組成，每個(gè)ROS進(jìn)程稱之為節(jié)點(diǎn)（Node）。所有節(jié)點(diǎn)可以運(yùn)行在一個(gè)處理器上，也可以通過ROS分布式特性運(yùn)行在多個(gè)處理器上。在實(shí)際開發(fā)時(shí)，這種結(jié)構(gòu)設(shè)計(jì)可以讓開發(fā)者根據(jù)作品所需功能方便靈活地添加功能模塊。

1.2 ROS缺陷

ROS的功能由各個(gè)節(jié)點(diǎn)組成，節(jié)點(diǎn)間通過消息機(jī)制通信，通訊環(huán)節(jié)消耗了很多計(jì)算資源。尤其是當(dāng)所有節(jié)點(diǎn)運(yùn)行在同一個(gè)處理器時(shí)，ROS仍然執(zhí)行相應(yīng)的消息分發(fā)，節(jié)點(diǎn)間的數(shù)據(jù)通信通過內(nèi)存復(fù)制，使得系統(tǒng)必須選用高性能的處理器和存儲(chǔ)系統(tǒng)以彌補(bǔ)速度的不足。

2 SLAM相關(guān)模型

2.1常用傳感器模型

2.1.1 激光傳感器

激光傳感器通過激光來測(cè)量到被測(cè)物體的距離等參數(shù)。常用的測(cè)距方法是由激光器發(fā)出時(shí)間極短的脈沖激光發(fā)射到被測(cè)目標(biāo)，回波返回，由光電探測(cè)器接收。根據(jù)主波信號(hào)和回波信號(hào)之間的時(shí)間間隔，就可以算出到待測(cè)目標(biāo)的距離。

2.1.2 視覺傳感器

視覺傳感器探測(cè)范圍較廣、獲取信息較激光傳感器豐富，實(shí)際應(yīng)用中常使用多個(gè)視覺傳感器或者與其他傳感器配合使用，通過科學(xué)合理的算法可以得到物體的點(diǎn)云、距離、色彩等諸多信息;還可通過SSD算法，根據(jù)一個(gè)鏡頭的運(yùn)動(dòng)圖像來計(jì)算本地與目標(biāo)的相對(duì)位移。

2.2 柵格地圖

柵格圖像，即光柵圖像，是指在空間和亮度上都已經(jīng)離散化了的圖像。我們通過把一幅柵格圖像考慮為一個(gè)矩陣，矩陣中的任一元素的坐標(biāo)對(duì)應(yīng)于圖像中的一個(gè)點(diǎn)，而元素的值對(duì)應(yīng)于該點(diǎn)的灰度級(jí)，矩陣中的元素叫作像素。

柵格地圖主要就是把地圖離散化為一個(gè)個(gè)的單元（cell），每個(gè)單元占據(jù) r*r 個(gè)像素，所以 r 的大小就代表了地圖的分辨率。當(dāng) cell 為 free 時(shí)，該 cell 的灰度值就設(shè)為 255，也就是白色，當(dāng) cell 為 occupied 時(shí)，該 cell 的灰度值就設(shè)為 0 ，也就是黑色。初始時(shí)我們不知道 cell 具體的狀態(tài)，我們就將其設(shè)為 100 ，也就是灰色。接下來我們?cè)O(shè) mi為第 i 個(gè) cell 是否occupied 的隨機(jī)變量，那么 p（mi=1） 或 p（mi） 就表示第 i 個(gè) cell 為 occupied 的概率，所以 p（mi）=1 就表示第 i 個(gè) cell 為 occupied ，p（mi）=0 就表示第 i 個(gè) cell 為 free 。

2.3 坐標(biāo)位姿模型

與向量間的旋轉(zhuǎn)類似，我們同樣可以描述兩個(gè)坐標(biāo)系之間的旋轉(zhuǎn)關(guān)系，再加上平移，統(tǒng)稱為坐標(biāo)系之間的變換關(guān)系。在機(jī)器人的運(yùn)動(dòng)過程中，常見的做法是設(shè)定一個(gè)慣性坐標(biāo)（或者叫世界坐標(biāo)系），可以認(rèn)為它是固定不動(dòng)的，如圖中的[Xw，Yw，Zw]定義的坐標(biāo)系。同時(shí)，傳感器或機(jī)器人是一個(gè)移動(dòng)坐標(biāo)系，例如[Xc，Yc，Zc]定義的坐標(biāo)系。相機(jī)視野中某個(gè)向量P，它的坐標(biāo)為[Pc]，而從世界坐標(biāo)系下看，它的坐標(biāo)為[Pw]。這時(shí)，就需要先得到該點(diǎn)針對(duì)機(jī)器人坐標(biāo)系坐標(biāo)值，再根據(jù)機(jī)器人位姿轉(zhuǎn)換到世界坐標(biāo)系中，這個(gè)轉(zhuǎn)換關(guān)系由一個(gè)矩陣T來描述。

3 ?2D SLAM 算法介紹與實(shí)現(xiàn)

算法的輸入一般是：機(jī)器人對(duì)周圍環(huán)境的觀測(cè)信息 z （激光雷達(dá)的掃描點(diǎn)、攝像頭的圖像等） 和 機(jī)器人的控制信息 u （運(yùn)動(dòng)過程中的前進(jìn)轉(zhuǎn)向等信息，一般對(duì)其積分后用里程計(jì) odometry 來表示）;算法的輸出是：機(jī)器人的運(yùn)動(dòng)軌跡 x 和 周圍的地圖 m 。

而我們要做的就是在給定 z 和 u 的條件下估計(jì)出 x 和 m：

p（x，m|z，u）

我們用概率形式來表示在存在各種誤差的情況下，使用 z 和 u 來估計(jì) x 和 m 的分布。并且在 2d SLAM 中， x 就是一個(gè) [x，y，theta] 的三維隨機(jī)變量， 如果 m 是柵格地圖， m 就是一個(gè) r*c 維的隨機(jī)變量，其中 r 是地圖的寬度， c 是地圖的長(zhǎng)度，所以我們可以用一個(gè)條件概率來解決我們的問題。

3.1 Gmapping slam

Gmapping是基于濾波SLAM框架和RBpf粒子濾波算法的常用開源SLAM算法，將定位和建圖過程分離，先進(jìn)行定位再進(jìn)行建圖。如圖1。

優(yōu)點(diǎn)：Gmapping在構(gòu)建室內(nèi)小場(chǎng)景地圖時(shí)擁有計(jì)算量小、精度高的特點(diǎn)。里程計(jì)可以提供機(jī)器人的位姿先驗(yàn)，算法有效利用里程計(jì)的信息，從而對(duì)激光雷達(dá)的頻率要求變低。

缺點(diǎn)：Gmapping犧牲空間復(fù)雜度保證時(shí)間復(fù)雜度，因此不適合構(gòu)建大場(chǎng)景地圖。此算法沒有回環(huán)檢測(cè)，在回環(huán)閉合時(shí)可能會(huì)造成地圖錯(cuò)位。

3.2 Hector slam

Hector_Slam利用現(xiàn)代激光雷達(dá)系統(tǒng)的高更新率，并以傳感器的掃描速率提供2D姿態(tài)估計(jì)。如圖2。

優(yōu)點(diǎn)：不依賴?yán)锍逃?jì)信息，空中無人機(jī)及地面小車在不平坦區(qū)域建圖得以普及;利用高斯牛頓數(shù)學(xué)方法處理scan-matching匹配問題，獲得激光點(diǎn)集會(huì)映射到已建地圖的剛體變換。

缺點(diǎn)：激光雷達(dá)的更新率要求較高，所建環(huán)境噪聲較小。所以在建圖過程中，需要移動(dòng)機(jī)器人控制在速度比較低的條件下，建圖效果才會(huì)比較理想;且在里程計(jì)數(shù)據(jù)比較精確的時(shí)候，無法有效利用里程計(jì)信息。

3.3 Cartographer slam

Cartographer_Slam基于激光雷達(dá)構(gòu)建2D環(huán)境地圖，分辨率精確至5cm。激光雷達(dá)返回的數(shù)據(jù)scans會(huì)以最佳的位置插入到submap中，這個(gè)最佳的位置假設(shè)在一定的時(shí)間內(nèi)可以認(rèn)為是很準(zhǔn)確的。其中scan matching必須是和它相對(duì)應(yīng)的submap進(jìn)行匹配，因此它只和最接近的scan點(diǎn)集有關(guān)。這個(gè)系統(tǒng)結(jié)合了局部最優(yōu)和全局最優(yōu)的方法，都對(duì)激光雷達(dá)觀測(cè)到的位置進(jìn)行了優(yōu)化。優(yōu)化后的位置用[（ξx，ξy，ξθ）]表示，包括（x，y）坐標(biāo)的轉(zhuǎn)化和角度的旋轉(zhuǎn)，實(shí)際上就是對(duì)scan點(diǎn)集的進(jìn)一步優(yōu)化。

我們?cè)趯W(xué)校實(shí)驗(yàn)室中用木紙板搭建了一個(gè)簡(jiǎn)單的地圖環(huán)境，采用Jetson TX2硬件開發(fā)平臺(tái)驅(qū)動(dòng)激光雷達(dá)采集周圍環(huán)境信息，并利用ROS分布式在同一局域網(wǎng)下在PC端運(yùn)行Cartographer算法構(gòu)建地圖，通過上位機(jī)與下位機(jī)的stm32f103單片機(jī)進(jìn)行串口通信控制小車底盤的移動(dòng)，從而實(shí)現(xiàn)了小車環(huán)境的構(gòu)建。如圖3。

4 結(jié)束語(yǔ)

大體上講，SLAM發(fā)展趨勢(shì)有兩大類：一方面是朝輕量級(jí)、小型化方向發(fā)展，讓SLAM能夠在小型移動(dòng)終端上良好運(yùn)行，然后考慮以它為底層功能的實(shí)際應(yīng)用。在大部分場(chǎng)合中，我們的研發(fā)目標(biāo)是實(shí)現(xiàn)機(jī)器人、AR/VR設(shè)備的功能，比如說導(dǎo)航、教學(xué)、娛樂，而SLAM是為上層開發(fā)應(yīng)用提供機(jī)器人本身的位姿估計(jì)。在這些應(yīng)用中，我們不希望SLAM占用太多的計(jì)算資源。另一方面則是利用高性能設(shè)備，實(shí)現(xiàn)精密場(chǎng)景理解等功能。我們的目的是準(zhǔn)確地重建場(chǎng)景地圖，而對(duì)于計(jì)算資源和設(shè)備的便攜性沒有多大要求。因此可以利用GPU，在這個(gè)方向和深度學(xué)習(xí)結(jié)合亦是計(jì)算機(jī)視覺的發(fā)展方向。

參考文獻(xiàn)：

[1] 高博.視覺SLAM十四講[M].北京：電子工業(yè)出版社，2016.

[2] Google.Real-Time Loop Closure in 2D LIDAR SLAM .[R] ICRA ，2016

[3] 王富.未知環(huán)境中移動(dòng)機(jī)器人定位與建圖技術(shù)的研究[D].黑龍江：哈爾濱工程大學(xué)，2013.

[4] 梁明杰，閔華清，羅榮華.基于圖優(yōu)化的同時(shí)定位與地圖創(chuàng)建綜述[J].機(jī)器人，2013（6）.

[5] 顧其洋.基于ROS的家庭服務(wù)機(jī)器人SLAM的設(shè)計(jì)與實(shí)現(xiàn)[D].蘭州：西北師范大學(xué)，2016.

[6] 白佳奇.基于ROS的移動(dòng)機(jī)器人改進(jìn)視覺SLAM算法研究[D].天津：天津工業(yè)大學(xué)，2018.

【通聯(lián)編輯：唐一東】