基于ROS的機器人定位與導航系統(tǒng)的仿真設計

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

（1.中國科學院國家空間科學中心北京100190；2.中國科學院大學北京100094）

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

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

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

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

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

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

2 ROS系統(tǒng)簡介

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

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

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

圖2 ROS進程間通信模型

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

3 基于ROS的系統(tǒng)設計

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

圖3 ROS導航功能的配置項

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

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

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

3.1.1 通用文件配置

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

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

3.1.2 全局代價地圖配置

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

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

3.1.3 局部代價地圖的配置

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

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

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

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

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

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

2）機器人提供運動消息cmd_vel，包括線速度和角速度。由里程計（odom）發(fā)布；

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

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

4 實驗仿真及結(jié)果

經(jīng)過上節(jié)的配置之后，就可以編寫launch文件進行仿真測試了。本實驗在以下環(huán)境中進行，ROS版本ROS-kinetic，3-D環(huán)境模擬Gazebo8.0，操作系統(tǒng)版本：Ubuntu16.04。基于以上配置，其中SLAM算法仿真的具體操作如下：

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

2）啟動Gmapping節(jié)點；

3）啟動鍵盤控制節(jié)點，控制機器完成環(huán)境建圖。

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

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

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

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

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

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

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

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

圖8 導航完成

5 結(jié)論

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