熊 安 ，卞春江 ，周 海 ，劉 成

（1.中國(guó)科學(xué)院國(guó)家空間科學(xué)中心北京100190；2.中國(guó)科學(xué)院大學(xué)北京100094）

實(shí)現(xiàn)機(jī)器人像人一樣能夠在真實(shí)環(huán)境中行走自如一直是人類的夢(mèng)想，也是機(jī)器人研究領(lǐng)域中最重要的話題。解決該問(wèn)題的主要關(guān)鍵點(diǎn)在于如何實(shí)現(xiàn)機(jī)器人的自主定位和自主路徑規(guī)劃?，F(xiàn)實(shí)生活中往往存在GPS信號(hào)不穩(wěn)定，定位誤差大等缺點(diǎn)，尤其對(duì)于室內(nèi)環(huán)境存在GPS信號(hào)不可用的情況，此時(shí)傳統(tǒng)的定位方法不在適用。同時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）技術(shù)是解決在機(jī)器人在無(wú)GPS等外部定位信息時(shí)進(jìn)行自主定位的最佳手段。該問(wèn)題是指移動(dòng)機(jī)器人在陌生環(huán)境中利用自身傳感器數(shù)據(jù)，實(shí)現(xiàn)自身定位且利用定位信息在定位過(guò)程中逐步構(gòu)建環(huán)境地圖[1]。SLAM技術(shù)是移動(dòng)機(jī)器人自主化，智能化的重要手段，因此近十幾年以來(lái)，該問(wèn)題始終在機(jī)器人研究領(lǐng)占據(jù)著重要地位。

ROS（robot operating system）是一個(gè)開(kāi)源的機(jī)器人操作系統(tǒng)。它提供了很多關(guān)于機(jī)器人的硬件抽象、以及常用功能的實(shí)現(xiàn)，使得機(jī)器人的開(kāi)發(fā)更加便捷迅速，很快在機(jī)器人研究領(lǐng)域成為研究熱潮。ROS中有很多封裝好的代碼，用于實(shí)現(xiàn)一些常用功能，可以使人們?cè)陂_(kāi)發(fā)機(jī)器人中把研究重點(diǎn)放在核心算法的研究和改進(jìn)上，極大地提高機(jī)器人開(kāi)發(fā)效率[2-4]。基于A*的路徑規(guī)劃算法和基于DWA的局部避障算法[5-7]在ROS中被封裝為Navigation的軟件包，該軟件包能夠?qū)崿F(xiàn)在已知地圖和未知地圖的環(huán)境中機(jī)器人的路徑規(guī)劃和自主避障功能。文中基于ROS系統(tǒng)，設(shè)計(jì)實(shí)現(xiàn)了基于ROS的機(jī)器人導(dǎo)航和建圖的軟件系統(tǒng)，并利用Gazebo3-D仿真驗(yàn)證系統(tǒng)的實(shí)用性。

1 地圖構(gòu)建與定位算法概述

SLAM技術(shù)經(jīng)過(guò)幾十年的發(fā)展，已經(jīng)初步形成了基本的理論框架。其按照傳感器分類可主要分為基于測(cè)距的SLAM和基于視覺(jué)的SLAM[8]。基于測(cè)距的SLAM技術(shù)已經(jīng)形成成熟的理論框架[9]。文獻(xiàn)[10]對(duì)基于視覺(jué)的SLAM技術(shù)的發(fā)展做了分析和總結(jié)。

基于圖優(yōu)化的SLAM是近幾年發(fā)展起來(lái)的新方法，主要以視覺(jué)傳感器為主，是一種在線的全SLAM解決方法。它在視覺(jué)前端和優(yōu)化后端中加入回環(huán)檢測(cè)環(huán)節(jié)，使得構(gòu)建地圖時(shí)能能夠識(shí)別之前到達(dá)過(guò)的場(chǎng)景，很好地解決了移動(dòng)中的累計(jì)誤差，但是計(jì)算量較大[11]。其中典型的代表有ORB-SLAM、LSDSLAM、SVO以及RTAB-MAP[12-16]。這些算法大多是基于視覺(jué)的SLAM技術(shù)，并且采用并行化的思想。由于算力的提升，使得以攝像頭為主要傳感器的視覺(jué)定位成為了如今的主流。但是在小型的嵌入式設(shè)備上，以圖優(yōu)化的SLAM的實(shí)用性大大降低。

基于貝葉斯濾波的SLAM方法是傳統(tǒng)的SLAM解決方案。其中最早的SLAM解決方案是以卡爾曼濾波器為主要手段。擴(kuò)展卡爾曼濾波雖然在一定程度上彌補(bǔ)了卡爾曼濾波器的不足，但是對(duì)于難以建模的復(fù)雜場(chǎng)景仍無(wú)能為力，且由于維護(hù)需要維護(hù)每一步計(jì)算的雅克比矩陣，計(jì)算量巨大。粒子濾波是利用樣本的分布來(lái)表示系統(tǒng)的狀態(tài)分布，因此對(duì)于復(fù)雜環(huán)境的建模往往比較容易實(shí)現(xiàn)，且估計(jì)精度和計(jì)算效率較高[17]。文獻(xiàn)[18]提出的基于Rao-Blackwellized粒子濾波的RBPF-SLAM是最近幾年出現(xiàn)的最為實(shí)用的一種SLAM算法。該方法將地圖和機(jī)器人位姿的聯(lián)合后驗(yàn)分布分解為對(duì)于機(jī)器人路徑的后驗(yàn)估計(jì)和已知位姿時(shí)對(duì)于地圖的估計(jì)，在提高定位與建圖精度的同時(shí)也兼顧了算法的效率。該算法在ROS中被制作成名為Gmapping的軟件包。它利用里程計(jì)信息和激光雷達(dá)測(cè)距信息可以在室內(nèi)建立高精度的二維柵格地圖，能夠?qū)崿F(xiàn)機(jī)器人自主定位和環(huán)境地圖構(gòu)建的功能。

2 ROS系統(tǒng)簡(jiǎn)介

ROS設(shè)計(jì)的初衷是提高機(jī)器人的開(kāi)發(fā)效率，使得人們對(duì)于機(jī)器人的開(kāi)發(fā)主要集中在關(guān)鍵問(wèn)題的研究上，能夠快速開(kāi)發(fā)出用于實(shí)驗(yàn)驗(yàn)證的機(jī)器人。它是一種分布式的處理框架。程序在運(yùn)行時(shí)，所有的進(jìn)程以及他們所進(jìn)行處理的數(shù)據(jù)都會(huì)通過(guò)一種點(diǎn)對(duì)點(diǎn)的網(wǎng)絡(luò)表現(xiàn)出來(lái)，這一級(jí)主要包括：節(jié)點(diǎn)（Node）、消息（message）、主題（topic）、服務(wù)（Service）。節(jié)點(diǎn)就是執(zhí)行計(jì)算任務(wù)的進(jìn)程。節(jié)點(diǎn)之間的通信通過(guò)消息傳遞。消息以一種訂閱和發(fā)布的方式進(jìn)行，每一種消息都是一種嚴(yán)格固定的數(shù)據(jù)結(jié)構(gòu)[19]。ROS節(jié)點(diǎn)之間的通信方式模型如圖1所示。

圖1 ROS節(jié)點(diǎn)的通訊方式

圖1中橢圓表示計(jì)算的節(jié)點(diǎn)（node），矩形表示話題。一個(gè)節(jié)點(diǎn)既可以發(fā)布話題，也可以訂閱相應(yīng)的話題來(lái)接受消息。在以上基礎(chǔ)上，需要有一個(gè)進(jìn)程控制所有的節(jié)點(diǎn)有條不紊的執(zhí)行，該進(jìn)程就是ROS Master，其模型如圖2所示。

圖2 ROS進(jìn)程間通信模型

該進(jìn)程控制所有進(jìn)程的話題和消息，節(jié)點(diǎn)之間的通信通過(guò)ROS Master進(jìn)行。節(jié)點(diǎn)與節(jié)點(diǎn)之間的建立是直接的，ROS Master負(fù)責(zé)提供了查詢信息，就像一個(gè)DNS服務(wù)器。節(jié)點(diǎn)訂閱一個(gè)主題將會(huì)要求建立一個(gè)與發(fā)布該主題的節(jié)點(diǎn)進(jìn)行連接，并且會(huì)在同意連接協(xié)議的基礎(chǔ)上建立連接。所有的服務(wù)由ROS控制器[20]ROS Master控制。

3 基于ROS的系統(tǒng)設(shè)計(jì)

ROS中提供給了大量的功能級(jí)軟件包，包括用于移動(dòng)機(jī)器人在室內(nèi)環(huán)境下定位與建圖的Gmapping軟件包、用于導(dǎo)航和避障的Navigation包、處理激光雷達(dá)點(diǎn)云的軟件包等。這些軟件包被封裝成一個(gè)一個(gè)節(jié)點(diǎn)和功能包集以便開(kāi)發(fā)調(diào)用。如第二節(jié)所示，ROS的通信方式為我們利用這些算法提供便利。在實(shí)現(xiàn)機(jī)器人的定位與建圖時(shí)需要利用軟件包Gmapping，它是一套使用機(jī)器人傳感器和里程計(jì)功能的算法。該軟件包基于RBPF算法，提供基于激光雷達(dá)的SLAM功能。節(jié)點(diǎn)名為slam_gampping。該算法能夠輸出機(jī)器人的精確定位和柵格化的二維環(huán)境地圖。ROS的導(dǎo)航功能包集是一個(gè)名叫move_base的軟件包。該軟件包能夠利用里程計(jì)信息的激光測(cè)距信息實(shí)現(xiàn)精準(zhǔn)的定位與導(dǎo)航功能。其系統(tǒng)組成如圖3所示。

圖3 ROS導(dǎo)航功能的配置項(xiàng)

圖3中，move_base_simple/gaol是一個(gè)ROS消息，人為給定的，該消息就是機(jī)器人導(dǎo)航的目標(biāo)地點(diǎn)；amcl是蒙特卡洛定位，其利用粒子濾波算法來(lái)確定機(jī)器人在構(gòu)建的grid map地圖中的位置，在無(wú)地圖時(shí)可以不用配置；sensor transform是用來(lái)實(shí)現(xiàn)機(jī)器人內(nèi)部各傳感器之間復(fù)雜的坐標(biāo)變換，如編碼器到機(jī)器人中心的坐標(biāo)變換，激光雷達(dá)到機(jī)器人中心的坐標(biāo)變換等；odometry為里程計(jì)發(fā)布信息，該信息是用來(lái)獲取機(jī)器人的運(yùn)動(dòng)軌跡的預(yù)測(cè)；sensor傳感器模塊是檢查機(jī)器人運(yùn)動(dòng)中的障礙物的，本文中為激光測(cè)距儀；全局路徑規(guī)劃global_planner是默認(rèn)情況下是基于Dijkstra算法，本文設(shè)置為更高效的A*導(dǎo)航算法；局部規(guī)劃local_palnner是基于動(dòng)態(tài)窗口的DWA導(dǎo)航算法；map_serve節(jié)點(diǎn)是用來(lái)在導(dǎo)航中獲取地圖數(shù)據(jù)的。recovery_behaviors節(jié)點(diǎn)是用來(lái)配置機(jī)器人在導(dǎo)航失敗后的執(zhí)行動(dòng)作。其中規(guī)劃所用的參數(shù)配置分別用全局代價(jià)地圖global_costmap和局部代價(jià)local_cosmap文件來(lái)進(jìn)行設(shè)置

3.1 導(dǎo)航系統(tǒng)的配置

機(jī)器人通過(guò)兩種導(dǎo)航算法在地圖中移動(dòng)，分別是全局導(dǎo)航和局部導(dǎo)航。全局導(dǎo)航用于從起點(diǎn)到終點(diǎn)的路徑規(guī)劃，局部導(dǎo)航用來(lái)躲避臨時(shí)障礙物。這些信息通過(guò)代價(jià)地圖來(lái)處理，他們有著基本的通用參數(shù)[21]，這些參數(shù)會(huì)保存在共享文件中。調(diào)用這兩個(gè)算法需要配置3個(gè)文件。

3.1.1 通用文件配置

該文件主要用于配置基本參數(shù)，這些參數(shù)會(huì)用于局部規(guī)劃器和全局規(guī)劃器，其中配置如下：

其中obstacle_range和raytrace_rang分別表示傳感器探測(cè)的最大距離和在代價(jià)地圖中引入的探測(cè)的障礙物距離信息。參數(shù)footprint用于將機(jī)器人的幾何參數(shù)告知導(dǎo)航功能包，由于在導(dǎo)航時(shí)，導(dǎo)航路徑的規(guī)劃是以質(zhì)點(diǎn)的形式進(jìn)行的，并沒(méi)有考慮機(jī)器人的幾何信息，該參數(shù)可使得機(jī)器人在導(dǎo)航時(shí)與障礙物能夠保持安全距離。inflation_radius為機(jī)器人的半徑。其他兩個(gè)為導(dǎo)航時(shí)的傳感器信息描述，本文用的是激光雷達(dá)。

3.1.2 全局代價(jià)地圖配置

全局代價(jià)地圖配置是用于全局路徑規(guī)劃的相應(yīng)參數(shù)，其配置如下：

其中g(shù)loab_frame和robot_baseframe是用于定義地圖和機(jī)器人之間的坐標(biāo)轉(zhuǎn)換。update_frequency是用來(lái)配置全局規(guī)劃的頻率的，由于在移動(dòng)中會(huì)遇到障礙物等信息，局部規(guī)劃會(huì)改變?cè)新窂?，因此需要?shí)時(shí)更新全局路徑來(lái)實(shí)現(xiàn)動(dòng)態(tài)的避障效果。static_map表示使用的是靜態(tài)地圖數(shù)據(jù)，即地圖是預(yù)先創(chuàng)建的。

3.1.3 局部代價(jià)地圖的配置

局部代價(jià)地圖是在機(jī)器人進(jìn)行局部避障時(shí)需要設(shè)置的參數(shù)。由于ROS中的局部導(dǎo)航是基于動(dòng)態(tài)窗口的DWA算法。因此需要告訴規(guī)劃器機(jī)器人自身性能相關(guān)參數(shù)。其配置如下。

在局部代價(jià)地圖的配置中，以下3個(gè)參gloab_frame、static_map、robot_baseframe、update_frequency與全局代價(jià)地圖的參數(shù)含義相同，參數(shù)publish_frequency表示局部規(guī)劃器發(fā)布信息的頻率，rolling_window參數(shù)表示代價(jià)地圖始終以機(jī)器人為中心。參數(shù)transform_tolerance表示轉(zhuǎn)換的最大延遲，planner_frequency為規(guī)劃算法的循環(huán)頻率[22]，planner_patinete為尋找一條有效路徑的等待時(shí)間，其余參數(shù)為地圖的尺寸和分辨率。max_vel_x之后的參數(shù)表示機(jī)器人的最大速度，最大角速度，最大加速度和角加速度等信息，這些信息用于速度采樣對(duì)于速度空間的限制。

配置完成以上文件之后，就可以編寫(xiě)仿真的launch文件了，launch文件的編寫(xiě)可參考文獻(xiàn)[4]，其中主要是加入導(dǎo)航時(shí)的一些節(jié)點(diǎn)和以上配置文件的路徑和文件名。以上參數(shù)主要根據(jù)本文設(shè)定的機(jī)器人幾何信息指定，每個(gè)參數(shù)都是具體化的數(shù)據(jù)，對(duì)于不同機(jī)器人，其配置可能略有差異。

3.2 SLAM系統(tǒng)的配置

ROS已經(jīng)為我們提供了機(jī)器人建圖和定位的Gmapping算法，開(kāi)發(fā)者只需要根據(jù)自己的機(jī)器人修改相應(yīng)的參數(shù)即可，這些算法為我們開(kāi)發(fā)機(jī)器人提供了極大的便利。將這些軟件包配置起來(lái)需要以下步驟：

1）讓機(jī)器人發(fā)布所有傳感器相對(duì)于自身的位置關(guān)系，以機(jī)器人的中心為坐標(biāo)原點(diǎn)；

2）機(jī)器人提供運(yùn)動(dòng)消息cmd_vel，包括線速度和角速度。由里程計(jì)（odom）發(fā)布；

3）接受激光雷達(dá)的信息來(lái)實(shí)現(xiàn)地圖的構(gòu)建和定位；

機(jī)器人在開(kāi)發(fā)時(shí)，需要統(tǒng)一機(jī)器人本體坐標(biāo)系和各個(gè)傳感器之間的坐標(biāo)系，在坐標(biāo)系比較多時(shí)，人工管理工作量巨大，非常不便，ROS提供了TF軟件包可以方便的實(shí)現(xiàn)這些坐標(biāo)的管理。在本文中主要是激光傳感器相對(duì)于小車中心的位置關(guān)系。機(jī)器人利用里程計(jì)信息估計(jì)自己的軌跡和位姿，為了減少測(cè)距的誤差同時(shí)利用激光觀測(cè)值來(lái)修正定位精度，隨后采樣和更新地圖。這是基于RBPF粒子濾波算法的核心，具體原理見(jiàn)文獻(xiàn)[18]。

4 實(shí)驗(yàn)仿真及結(jié)果

經(jīng)過(guò)上節(jié)的配置之后，就可以編寫(xiě)launch文件進(jìn)行仿真測(cè)試了。本實(shí)驗(yàn)在以下環(huán)境中進(jìn)行，ROS版本ROS-kinetic，3-D環(huán)境模擬Gazebo8.0，操作系統(tǒng)版本：Ubuntu16.04?；谝陨吓渲?，其中SLAM算法仿真的具體操作如下：

1）啟動(dòng)機(jī)器人模型和Gazebo仿真環(huán)境，該模型由URDF文件描述；

2）啟動(dòng)Gmapping節(jié)點(diǎn)；

3）啟動(dòng)鍵盤(pán)控制節(jié)點(diǎn)，控制機(jī)器完成環(huán)境建圖。

機(jī)器人的URDF文件可參考文獻(xiàn)[4]。Gmapping節(jié)點(diǎn)的啟動(dòng)也是以launch文件的形式。其發(fā)布話題/map?？刂茩C(jī)器人在仿真環(huán)境中行走需要啟動(dòng)鍵盤(pán)控制節(jié)點(diǎn)，該節(jié)點(diǎn)在ROS中是名為teleop_twist_keyboard。調(diào)用命令：rosrun teleop_twist_keyboard.py。啟動(dòng)該節(jié)點(diǎn)后，就可以利用鍵盤(pán)控制機(jī)器人在模擬環(huán)境中移動(dòng)，其中i表示前進(jìn)，k表示暫停，l表示右轉(zhuǎn)，j表示左轉(zhuǎn)。建圖后調(diào)用命令rosrun map_server map_saver-f map完成地圖保存，其中map為地圖的名字，可任意指定。

其中三維的真實(shí)環(huán)境如所示圖4所示。

圖4 Gazebo仿真環(huán)境

仿真結(jié)果如圖5所示。

圖5 最終建立的柵格化地圖

導(dǎo)航算法的仿真步驟主要是啟動(dòng)move_base節(jié)點(diǎn)，調(diào)用amcl算法實(shí)現(xiàn)地圖中的自定位，然后在rviz中點(diǎn)擊2-D Nav Goal并制定目標(biāo)，此時(shí)機(jī)器人會(huì)根據(jù)創(chuàng)建的環(huán)境地圖來(lái)規(guī)劃一條可執(zhí)行的路徑。并且根據(jù)激探測(cè)的障礙物信息進(jìn)行局部避障。實(shí)驗(yàn)結(jié)果如圖6、圖7、圖8所示。

圖6 全局路徑規(guī)劃

圖7 避障重新規(guī)劃

圖8 導(dǎo)航完成

5 結(jié)論

文中利用ROS提供的模擬環(huán)境Gazebo以及Gmapping等相關(guān)軟件包，設(shè)計(jì)實(shí)現(xiàn)了用于機(jī)器人定位與建圖的SLAM系統(tǒng)，同時(shí)又利用ROS中提供的導(dǎo)航功能包，設(shè)計(jì)實(shí)現(xiàn)了基于ROS的機(jī)器人導(dǎo)航系統(tǒng)。實(shí)驗(yàn)仿真結(jié)果表明，該仿真設(shè)計(jì)系統(tǒng)能夠在搭載激光雷達(dá)傳感器的輪式機(jī)器人中實(shí)現(xiàn)室內(nèi)環(huán)境的定位、建圖和導(dǎo)航等功能。對(duì)實(shí)際機(jī)器人的應(yīng)用具有一定的指導(dǎo)價(jià)值。但是本文只是在模擬環(huán)境中實(shí)現(xiàn)了該系統(tǒng)的仿真，后續(xù)將在此系統(tǒng)的基礎(chǔ)上修改相關(guān)節(jié)點(diǎn)驅(qū)動(dòng)，將模擬的激光雷達(dá)和機(jī)器人等替換成真實(shí)的機(jī)器人驅(qū)動(dòng)，把本系統(tǒng)設(shè)計(jì)應(yīng)用于真實(shí)機(jī)器人中。