前言

馬上開學(xué)，目前學(xué)校很多實驗室都是人工智能這塊，大部分都是和機器人相關(guān)，然后軟件這塊就是和cv、ros相關(guān)，就打算開始學(xué)習(xí)一下。

本章節(jié)是虛擬機安裝Ubuntu18.04以及安裝ROS的環(huán)境。

學(xué)習(xí)教程：【Autolabor初級教程】ROS機器人入門，博客中一些知識點是來源于趙老師的筆記在線筆記，本博客主要是做歸納總結(jié)，如有侵權(quán)請聯(lián)系刪除。

視頻中的案例都基本敲了遍，這里給出我自己的源代碼文件：

鏈接：https://pan.baidu.com/s/13CAzXk0vAWuBsc4oABC-_g 
提取碼：0hws 

所有博客文件目錄索引：博客目錄索引(持續(xù)更新)

一、導(dǎo)航概述

1.1、導(dǎo)航模塊

關(guān)鍵技術(shù)有如下五點:

1. 全局地圖：實現(xiàn)導(dǎo)航，需要參考一張全局性質(zhì)的地圖，這就涉及到地圖建模技術(shù)（SLAM ）。
2. 自身定位：確定自身的位置，ros中提供了定位功能包（amcl）。
3. 路徑規(guī)劃：從A->B點需要計算全局路線，且在運動過程中需要根據(jù)實時路況進行調(diào)整。（move_base，包含全局路徑和本地實時路徑）
4. 運動控制：需要借助話題"cmd_vel"發(fā)布geometry_msgs/Twist類型的消息來實現(xiàn)運動。
5. 環(huán)境感知：感知周圍環(huán)境，環(huán)境感知也是一重要模塊實現(xiàn)，它為其他模塊提供了支持。其他模塊諸如: SLAM、amcl、move_base 都需要依賴于環(huán)境感知。

slam介紹

SLAM問題可以描述為: 機器人在未知環(huán)境中從一個未知位置開始移動,在移動過程中根據(jù)位置估計和地圖進行自身定位，同時在自身定位的基礎(chǔ)上建造增量式地圖，以繪制出外部環(huán)境的完全地圖。

SLAM 實現(xiàn)的技術(shù)如：gmapping、hector_slam、cartographer、rgbdslam、ORB_SLAM …

• 對于地圖的生成也需要進行保存：在ROS中保存地圖的功能包是map_server。

完成SLAM依賴的物理傳感器（需要獲取周圍環(huán)境深度信息的能力）：激光雷達、攝像頭、RGB-D攝像頭…

總結(jié)：SLAM 雖然是機器人導(dǎo)航的重要技術(shù)之一，但是 二者并不等價，確切的講，SLAM 只是實現(xiàn)地圖構(gòu)建和即時定位。

amcl介紹

amcl(adaptiveMonteCarloLocalization)：自適應(yīng)的蒙特卡洛定位,是用于2D移動機器人的概率定位系統(tǒng)。它實現(xiàn)了自適應(yīng)（或KLD采樣）蒙特卡洛定位方法，該方法使用粒子過濾器根據(jù)已知地圖跟蹤機器人的姿態(tài)。

move_base

該功能包由兩大規(guī)劃器組成：

1、全局路徑規(guī)劃(gloable_planner)：根據(jù)給定的目標(biāo)點和全局地圖實現(xiàn)總體的路徑規(guī)劃，使用 Dijkstra 或 A* 算法進行全局路徑規(guī)劃，計算最優(yōu)路線，作為全局路線、

2、本地時時規(guī)劃(local_planner)：在實際導(dǎo)航過程中，機器人可能無法按照給定的全局最優(yōu)路線運行，比如:機器人在運行中，可能會隨時出現(xiàn)一定的障礙物… 本地規(guī)劃的作用就是使用一定算法(Dynamic Window Approaches) 來實現(xiàn)障礙物的規(guī)避，并選取當(dāng)前最優(yōu)路徑以盡量符合全局最優(yōu)路徑。

全局路徑規(guī)劃與本地路徑規(guī)劃是相對的，全局路徑規(guī)劃側(cè)重于全局、宏觀實現(xiàn)，而本地路徑規(guī)劃側(cè)重與當(dāng)前、微觀實現(xiàn)。

運動控制

導(dǎo)航功能包集假定它可以通過話題"cmd_vel"發(fā)布geometry_msgs/Twist類型的消息，這個消息基于機器人的基座坐標(biāo)系，它傳遞的是運動命令。這意味著必須有一個節(jié)點訂閱"cmd_vel"話題， 將該話題上的速度命令轉(zhuǎn)換為電機命令并發(fā)送。

環(huán)境感知

感知周圍環(huán)境信息，比如: 攝像頭、激光雷達、編碼器…，攝像頭。

例如：激光雷達可以用于感知外界環(huán)境的深度信息，編碼器可以感知電機的轉(zhuǎn)速信息，進而可以獲取速度信息并生成里程計信息。

1.2、導(dǎo)航之坐標(biāo)系

介紹

定位：就是參考某個坐標(biāo)系(比如:以機器人的出發(fā)點為原點創(chuàng)建坐標(biāo)系)在該坐標(biāo)系中標(biāo)注機器人。

定位實現(xiàn)需要依賴于機器人自身，機器人需要逆向推導(dǎo)參考系原點并計算坐標(biāo)系相對關(guān)系，該過程實現(xiàn)常用方式有兩種：

• 通過里程計定位：時時收集機器人的速度信息計算并發(fā)布機器人坐標(biāo)系與父級參考系的相對關(guān)系。
• 通過傳感器定位：通過傳感器收集外界環(huán)境信息通過匹配計算并發(fā)布機器人坐標(biāo)系與父級參考系的相對關(guān)系。

兩者特點

兩者來進行配合最佳，前者根據(jù)行駛的距離來進行定位，后者則是依據(jù)不同的位置來進行定位：

坐標(biāo)系變換

機器人坐標(biāo)系一般使用機器人模型中的根坐標(biāo)系(base_link 或 base_footprint)

• 里程計定位：父級坐標(biāo)系一般稱之為 odom。
• 傳感器定位：父級參考系一般稱之為 map。

當(dāng)二者結(jié)合使用時，map 和 odom 都是機器人模型根坐標(biāo)系的父級，這是不符合坐標(biāo)變換中"單繼承"的原則的，所以，一般會將轉(zhuǎn)換關(guān)系設(shè)置為: map -> doom -> base_link 或 base_footprint。

二、導(dǎo)航實現(xiàn)

準(zhǔn)備工作（安裝導(dǎo)航包和新建工程包）

安裝ROS下面章節(jié)各個導(dǎo)航包：

# 安裝 gmapping 包(用于構(gòu)建地圖)
sudo apt install ros-melodic-gmapping

# 安裝地圖服務(wù)包(用于保存與讀取地圖)
sudo apt install ros-melodic-map-server

# 安裝 navigation 包(用于定位以及路徑規(guī)劃)
sudo apt install ros-melodic-navigation

創(chuàng)建新的工程包：

# 進入到工程下的src目錄
cd /home/workspace/roslearn/src

# 創(chuàng)建名為08nav_demo的包
catkin_create_pkg --rosdistro melodic 08nav_demo gmapping map_server amcl move_base

2.1、SLAM建圖

2.1.1、認(rèn)識gmapping

官網(wǎng)：gmapping

介紹：gmapping 是ROS開源社區(qū)中較為常用且比較成熟的SLAM算法之一，gmapping可以根據(jù)移動機器人里程計數(shù)據(jù)和激光雷達數(shù)據(jù)來繪制二維的柵格地圖。

gmapping包對于硬件的要求：

• 該移動機器人可以發(fā)布里程計消息。
• 機器人需要發(fā)布雷達消息(該消息可以通過水平固定安裝的雷達發(fā)布，或者也可以將深度相機消息轉(zhuǎn)換成雷達消息)

gmapping功能包的核心節(jié)點：slam_gmapping，需要額外關(guān)注的是該節(jié)點訂閱的話題、發(fā)布的話題、服務(wù)以及相關(guān)參數(shù)。

訂閱topic

tf (tf/tfMessage)：用于雷達、底盤與里程計之間的坐標(biāo)變換消息。

scan(sensor_msgs/LaserScan)：SLAM所需的雷達信息。

發(fā)布topic

map_metadata(nav_msgs/MapMetaData)：地圖元數(shù)據(jù)，包括地圖的寬度、高度、分辨率等，該消息會固定更新。

map(nav_msgs/OccupancyGrid)：地圖柵格數(shù)據(jù)，一般會在rviz中以圖形化的方式顯示。

~entropy(std_msgs/Float64)：機器人姿態(tài)分布熵估計(值越大，不確定性越大)。

服務(wù)service

dynamic_map(nav_msgs/GetMap)：用于獲取地圖數(shù)據(jù)。

參數(shù)（常見參數(shù)）

~base_frame(string, default:“base_link”)：機器人基坐標(biāo)系。

~map_frame(string, default:“map”)：地圖坐標(biāo)系。

~odom_frame(string, default:“odom”)：里程計坐標(biāo)系。

~map_update_interval(float, default: 5.0)：地圖更新頻率，根據(jù)指定的值設(shè)計更新間隔。

~maxUrange(float, default: 80.0)：激光探測的最大可用范圍(超出此閾值，被截斷)。

~maxRange(float)：激光探測的最大范圍。

所需的坐標(biāo)變換

雷達坐標(biāo)系→基坐標(biāo)系：一般由 robot_state_publisher 或 static_transform_publisher 發(fā)布。

基坐標(biāo)系→里程計坐標(biāo)系：一般由里程計節(jié)點發(fā)布。

發(fā)布的坐標(biāo)變換

地圖坐標(biāo)系→里程計坐標(biāo)系：地圖到里程計坐標(biāo)系之間的變換。

2.1.2、實操

步驟一：編寫slam的launch文件

nav01_slam.launch：其中包含了gmapping的一個包對于slam的具體實現(xiàn)以及rviz的配置

<launch>
    <param name="use_sim_time" value="true"/>
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <remap from="scan" to="scan"/><!-- 雷達話題 -->
      <param name="base_frame" value="base_footprint"/><!--底盤坐標(biāo)系-->
      <param name="odom_frame" value="odom"/> <!--里程計坐標(biāo)系-->
      <param name="map_update_interval" value="5.0"/>
      <param name="maxUrange" value="16.0"/>
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="1"/>
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.5"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.0"/>
      <param name="delta" value="0.05"/>
      <param name="llsamplerange" value="0.01"/>
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
    </node>

    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />

    <!-- <node pkg="rviz" type="rviz" name="rviz" /> -->
    <!-- 可以保存 rviz 配置并后期直接使用-->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find 08nav_demo)/conf/gmapping.rviz"/>
   
</launch>

• 其中remap表示重命名以前的話題scan為scan。
• 另外兩個就是對應(yīng)的坐標(biāo)系名稱。

步驟二：依次啟動Gazebo、slam的launch文件以及鍵盤控制

# 啟動gazebo(第7章節(jié)部分的，復(fù)用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch

# 啟動slam及rviz
roslaunch 08nav_demo nav01_slam.launch

# 啟動鍵盤控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

rviz打開之后，來添加一個map組件：指定訂閱主題為/map

令小車進行自轉(zhuǎn)來完成建圖工作：

# 向/cmd_vel主題去發(fā)布消息
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.3"

2.2、地圖服務(wù)（map_server）

2.2.1、認(rèn)識map_server

通過使用gmapping的構(gòu)建地圖并在rviz中顯示了地圖，此時地圖數(shù)據(jù)還是保存在內(nèi)存中，此時就有個問題對于構(gòu)建地圖之后若是服務(wù)程序結(jié)束，那么下次就需要重新建立，如何解決呢？此時就涉及到了地圖的持久化存儲。

本質(zhì)：會將地圖數(shù)據(jù)進行序列化持久存儲到磁盤中，后期會進行反序列話讀取地圖數(shù)據(jù)。

此時我們就可以使用ros中的map_server包來進行地圖的持久化存儲！

介紹map_server：map_server功能包中提供了兩個節(jié)點: map_saver 和 map_server，前者用于將柵格地圖保存到磁盤，后者讀取磁盤的柵格地圖并以服務(wù)的方式提供出去。

訂閱的topic

map(nav_msgs/OccupancyGrid)：訂閱此話題用于生成地圖文件。

2.2.2、實操—保存地圖

前提說明：需要在SLAM建圖完成之后，我們才可進行一個地圖保存服務(wù)（啟動2.1中的案例后再去執(zhí)行該地圖保存服務(wù)）。

nav02_map_save.launch：

<launch>
    <arg name="filename" value="$(find 08nav_demo)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

接著我們可以去用鍵盤控制小車移動，等待地圖繪制完成后，我們就可以去執(zhí)行地圖保存服務(wù)了！

# 運行地圖保存服務(wù)
roslaunch 08nav_demo nav02_map_save.launch

• nav.pgm：保存的地圖圖片。
• nav.yaml：保存的地圖配置。

效果如下：

2.2.3、實操—讀取地圖

nav02_map_read.launch·：

<launch>
    <!-- 設(shè)置地圖的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 運行地圖服務(wù)器，并且加載設(shè)置的地圖-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find 08nav_demo)/map/$(arg map)"/>
</launch>

我們可以在首先先啟動地圖服務(wù)讀取節(jié)點，這樣我們再去運行rviz后，選擇map組件即可看到原先繪制的map地圖了：

# 運行地圖讀取服務(wù)
roslaunch 08nav_demo nav02_map_read.launch

2.3、定位（amcl）

2.3.1、認(rèn)識amcl

amcl已經(jīng)被集成到了navigation包。

認(rèn)識：``AMCL`(adaptive Monte Carlo Localization) 是用于2D移動機器人的概率定位系統(tǒng)，它實現(xiàn)了自適應(yīng)（或KLD采樣）蒙特卡洛定位方法，可以根據(jù)已有地圖使用粒子濾波器推算機器人位置。

訂閱topic

scan(sensor_msgs/LaserScan)：激光雷達數(shù)據(jù)。

tf(tf/tfMessage)：坐標(biāo)變換消息。

initialpose(geometry_msgs/PoseWithCovarianceStamped)：用來初始化粒子濾波器的均值和協(xié)方差。

map(nav_msgs/OccupancyGrid)：獲取地圖數(shù)據(jù)。

發(fā)布的Topic

amcl_pose(geometry_msgs/PoseWithCovarianceStamped)：機器人在地圖中的位姿估計。

particlecloud(geometry_msgs/PoseArray)：位姿估計集合，rviz中可以被 PoseArray 訂閱然后圖形化顯示機器人的位姿估計集合。

tf(tf/tfMessage)：發(fā)布從 odom 到 map 的轉(zhuǎn)換。

提供的服務(wù)service

global_localization(std_srvs/Empty)：初始化全局定位的服務(wù)。

request_nomotion_update(std_srvs/Empty)：手動執(zhí)行更新和發(fā)布更新的粒子的服務(wù)。

set_map(nav_msgs/SetMap)：手動設(shè)置新地圖和姿態(tài)的服務(wù)。

調(diào)用的服務(wù)

static_map(nav_msgs/GetMap)：調(diào)用此服務(wù)獲取地圖數(shù)據(jù)。

常見參數(shù)

~odom_model_type(string, default:“diff”)：里程計模型選擇: “diff”,“omni”,“diff-corrected”,“omni-corrected” (diff 差速、omni 全向輪)

~odom_frame_id(string, default:“odom”)：里程計坐標(biāo)系。

~base_frame_id(string, default:“base_link”)：機器人極坐標(biāo)系。

~global_frame_id(string, default:“map”)：地圖坐標(biāo)系。

2.3.2、坐標(biāo)變換介紹

里程計本身也是可以協(xié)助機器人定位的，不過里程計存在累計誤差且一些特殊情況時(車輪打滑)會出現(xiàn)定位錯誤的情況，amcl 則可以通過估算機器人在地圖坐標(biāo)系下的姿態(tài)，再結(jié)合里程計提高定位準(zhǔn)確度。

• 里程計定位:只是通過里程計數(shù)據(jù)實現(xiàn) /odom_frame 與 /base_frame 之間的坐標(biāo)變換。
• amcl定位: 可以提供 /map_frame 、/odom_frame 與 /base_frame 之間的坐標(biāo)變換。

2.3.3、實操

前提說明：需要預(yù)先準(zhǔn)備好全局地圖配置信息。

步驟一：編寫amcl的配置文件

nav03_amcl.launch：

<launch>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        <!-- Publish scans from best pose at a max of 10 Hz -->
        <param name="odom_model_type" value="diff"/><!-- 里程計模式為差分 -->
        <param name="odom_alpha5" value="0.1"/>
        <param name="transform_tolerance" value="0.2" />
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="30"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="5000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="odom_alpha1" value="0.2"/>
        <param name="odom_alpha2" value="0.2"/>
        <!-- translation std dev, m -->
        <param name="odom_alpha3" value="0.8"/>
        <param name="odom_alpha4" value="0.2"/>
        <param name="laser_z_hit" value="0.5"/>
        <param name="laser_z_short" value="0.05"/>
        <param name="laser_z_max" value="0.05"/>
        <param name="laser_z_rand" value="0.5"/>
        <param name="laser_sigma_hit" value="0.2"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_model_type" value="likelihood_field"/>
        <!-- <param name="laser_model_type" value="beam"/> -->
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="update_min_d" value="0.2"/>
        <param name="update_min_a" value="0.5"/>

        <param name="odom_frame_id" value="odom"/><!-- 里程計坐標(biāo)系 -->
        <param name="base_frame_id" value="base_footprint"/><!-- 添加機器人基坐標(biāo)系 -->
        <param name="global_frame_id" value="map"/><!-- 添加地圖坐標(biāo)系 -->

        <param name="resample_interval" value="1"/>
        <param name="transform_tolerance" value="0.1"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
    </node>
</launch>

• 重點關(guān)注四個有中文注釋的內(nèi)容需要配置清楚。

步驟二：配置測試launch文件（集成地圖讀取服務(wù)、rviz以及amcl定位）

nav03_amcl_test.launch·：

<!-- 測試文件 -->
<launch>
    <!-- 加載地圖服務(wù) -->
    <include file="$(find 08nav_demo)/launch/nav02_map_read.launch" />

    <!-- 啟動rviz -->
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find 08nav_demo)/conf/gmapping.rviz"/>

    <!-- amcl文件 -->
    <include file="$(find 08nav_demo)/launch/nav03_amcl.launch" />

</launch>

接著來運行各個服務(wù)節(jié)點：

# 啟動gazebo(第7章節(jié)部分的，復(fù)用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch

# 啟動amcl測試文件（集成map讀取、rviz、amcl）
roslaunch 08nav_demo nav03_amcl_test.launch

# 啟動鍵盤控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

打開rviz后需要添加PoseArray組件，并且訂閱/particlecloud主題：

接著我們控制鍵盤來讓小車進行移動，接著我們來查看效果：

2.4、路徑規(guī)劃（move_base）

2.4.1、認(rèn)識move_base

move_base已經(jīng)被集成到了navigation包。

介紹：move_base 功能包提供了基于動作(action)的路徑規(guī)劃實現(xiàn)，move_base 可以根據(jù)給定的目標(biāo)點，控制機器人底盤運動至目標(biāo)位置，并且在運動過程中會連續(xù)反饋機器人自身的姿態(tài)與目標(biāo)點的狀態(tài)信息。

組成：move_base主要由全局路徑規(guī)劃與本地路徑規(guī)劃組成。

動作訂閱

move_base/goal(move_base_msgs/MoveBaseActionGoal)：move_base 的運動規(guī)劃目標(biāo)。

move_base/cancel(actionlib_msgs/GoalID)：取消目標(biāo)。

動作發(fā)布

move_base/feedback(move_base_msgs/MoveBaseActionFeedback)：連續(xù)反饋的信息，包含機器人底盤坐標(biāo)。

move_base/status(actionlib_msgs/GoalStatusArray)：發(fā)送到move_base的目標(biāo)狀態(tài)信息。

move_base/result(move_base_msgs/MoveBaseActionResult)：操作結(jié)果(此處為空)。

訂閱topic

move_base_simple/goal(geometry_msgs/PoseStamped)：運動規(guī)劃目標(biāo)(與action相比，沒有連續(xù)反饋，無法追蹤機器人執(zhí)行狀態(tài))。

發(fā)布topic

cmd_vel(geometry_msgs/Twist)：輸出到機器人底盤的運動控制消息。

服務(wù)

~make_plan(nav_msgs/GetPlan)：請求該服務(wù)，可以獲取給定目標(biāo)的規(guī)劃路徑，但是并不執(zhí)行該路徑規(guī)劃。

~clear_unknown_space(std_srvs/Empty)：允許用戶直接清除機器人周圍的未知空間。

~clear_costmaps(std_srvs/Empty)：允許清除代價地圖中的障礙物，可能會導(dǎo)致機器人與障礙物碰撞，請慎用。

2.4.2、move_base與代價地圖

ROS中的地圖其實就是一張圖片，這張圖片有寬度、高度、分辨率等元數(shù)據(jù)，在圖片中使用灰度值來表示障礙物存在的概率。

不過SLAM構(gòu)建的地圖在導(dǎo)航中是不可以直接使用的，主要原因如下：

1. SLAM構(gòu)建的地圖是靜態(tài)地圖，而導(dǎo)航過程中，障礙物信息是可變的，可能障礙物被移走了，也可能添加了新的障礙物，導(dǎo)航中需要時時的獲取障礙物信息。
2. 在靠近障礙物邊緣時，雖然此處是空閑區(qū)域，但是機器人在進入該區(qū)域后可能由于其他一些因素，比如：慣性、或者不規(guī)則形體的機器人轉(zhuǎn)彎時可能會與障礙物產(chǎn)生碰撞，安全起見，最好在地圖的障礙物邊緣設(shè)置警戒區(qū)，盡量禁止機器人進入…

靜態(tài)地圖無法直接應(yīng)用于導(dǎo)航，其基礎(chǔ)之上需要添加一些輔助信息的地圖，比如實時獲取的障礙物數(shù)據(jù)，基于靜態(tài)地圖添加的膨脹區(qū)等數(shù)據(jù)。

組成：global_costmap(全局代價地圖) 和 local_costmap(本地代價地圖)

• 前者用于全局路徑規(guī)劃，后者用于本地路徑規(guī)劃。

兩張代價地圖都可以多層疊加,一般有以下層級:

• Static Map Layer：靜態(tài)地圖層，SLAM構(gòu)建的靜態(tài)地圖。
• Obstacle Map Layer：障礙地圖層，傳感器感知的障礙物信息。
• Inflation Layer：膨脹層，在以上兩層地圖上進行膨脹（向外擴張），以避免機器人的外殼會撞上障礙物。
• Other Layers：自定義costmap。

碰撞算法：

上圖中，橫軸是距離機器人中心的距離，縱軸是代價地圖中柵格的灰度值。

• 致命障礙:柵格值為254，此時障礙物與機器人中心重疊，必然發(fā)生碰撞；
• 內(nèi)切障礙:柵格值為253，此時障礙物處于機器人的內(nèi)切圓內(nèi)，必然發(fā)生碰撞；
• 外切障礙:柵格值為[128,252]，此時障礙物處于其機器人的外切圓內(nèi)，處于碰撞臨界，不一定發(fā)生碰撞；
• 非自由空間:柵格值為(0,127]，此時機器人處于障礙物附近，屬于危險警戒區(qū)，進入此區(qū)域，將來可能會發(fā)生碰撞；
• 自由區(qū)域:柵格值為0，此處機器人可以自由通過；
• 未知區(qū)域:柵格值為255，還沒探明是否有障礙物。

膨脹空間的設(shè)置可以參考非自由空間。

2.4.3、實操—目的地導(dǎo)航

步驟一：編寫move_base配置

nav04_path.launch：在該launch文件中包含了多個配置項

<launch>

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find 08nav_demo)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find 08nav_demo)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find 08nav_demo)/param/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find 08nav_demo)/param/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find 08nav_demo)/param/base_local_planner_params.yaml" command="load" />
    </node>

</launch>

步驟二：編寫各類配置文件

base_local_planner_params.yaml：

TrajectoryPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.5 # X 方向最大速度
  min_vel_x: 0.1 # X 方向最小速速

  max_vel_theta:  1.0 # 
  min_vel_theta: -1.0
  min_in_place_vel_theta: 1.0

  acc_lim_x: 1.0 # X 加速限制
  acc_lim_y: 0.0 # Y 加速限制
  acc_lim_theta: 0.6 # 角速度加速限制

# Goal Tolerance Parameters，目標(biāo)公差
  xy_goal_tolerance: 0.10
  yaw_goal_tolerance: 0.05

# Differential-drive robot configuration
# 是否是全向移動機器人
  holonomic_robot: false

# Forward Simulation Parameters，前進模擬參數(shù)
  sim_time: 0.8
  vx_samples: 18
  vtheta_samples: 20
  sim_granularity: 0.05

costmap_common_params.yaml：

#機器人幾何參，如果機器人是圓形，設(shè)置 robot_radius,如果是其他形狀設(shè)置 footprint
robot_radius: 0.12 #圓形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形狀

obstacle_range: 3.0 # 用于障礙物探測，比如: 值為 3.0，意味著檢測到距離小于 3 米的障礙物時，就會引入代價地圖
raytrace_range: 3.5 # 用于清除障礙物，比如：值為 3.5，意味著清除代價地圖中 3.5 米以外的障礙物


#膨脹半徑，擴展在碰撞區(qū)域以外的代價區(qū)域，使得機器人規(guī)劃路徑避開障礙物
inflation_radius: 0.2
#代價比例系數(shù)，越大則代價值越小
cost_scaling_factor: 3.0

#地圖類型
map_type: costmap
#導(dǎo)航包所需要的傳感器
observation_sources: scan
#對傳感器的坐標(biāo)系和數(shù)據(jù)進行配置。這個也會用于代價地圖添加和清除障礙物。例如，你可以用激光雷達傳感器用于在代價地圖添加障礙物，再添加kinect用于導(dǎo)航和清除障礙物。
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

global_costmap_params.yaml：

global_costmap:
  global_frame: map #地圖坐標(biāo)系
  robot_base_frame: base_footprint #機器人坐標(biāo)系
  # 以此實現(xiàn)坐標(biāo)變換

  update_frequency: 1.0 #代價地圖更新頻率
  publish_frequency: 1.0 #代價地圖的發(fā)布頻率
  transform_tolerance: 0.5 #等待坐標(biāo)變換發(fā)布信息的超時時間

  static_map: true # 是否使用一個地圖或者地圖服務(wù)器來初始化全局代價地圖，如果不使用靜態(tài)地圖，這個參數(shù)為false.

local_costmap_params.yaml：

local_costmap:
  global_frame: odom #里程計坐標(biāo)系
  robot_base_frame: base_footprint #機器人坐標(biāo)系

  update_frequency: 10.0 #代價地圖更新頻率
  publish_frequency: 10.0 #代價地圖的發(fā)布頻率
  transform_tolerance: 0.5 #等待坐標(biāo)變換發(fā)布信息的超時時間

  static_map: false  #不需要靜態(tài)地圖，可以提升導(dǎo)航效果
  rolling_window: true #是否使用動態(tài)窗口，默認(rèn)為false，在靜態(tài)的全局地圖中，地圖不會變化
  width: 3 # 局部地圖寬度 單位是 m
  height: 3 # 局部地圖高度 單位是 m
  resolution: 0.05 # 局部地圖分辨率 單位是 m，一般與靜態(tài)地圖分辨率保持一致

步驟三：編寫路徑規(guī)劃測試文件

nav05_path_test.launch：

<launch>
    <!-- 設(shè)置地圖的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 運行地圖服務(wù)器，并且加載設(shè)置的地圖-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find 08nav_demo)/map/$(arg map)"/>
    <!-- slam（重新繪制地圖使用） -->
    <!-- <include file="$(find 08nav_demo)/launch/nav01_slam.launch" /> -->

    <!-- 啟動AMCL節(jié)點 -->
    <include file="$(find 08nav_demo)/launch/nav03_amcl.launch" />

    <!-- 運行move_base節(jié)點 -->
    <include file="$(find 08nav_demo)/launch/nav04_path.launch" />

    <!-- 運行rviz -->
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find 08nav_demo)/conf/gmapping.rviz"/>
</launch>

此時我們就可以來運行節(jié)點了：

# 啟動gazebo(第7章節(jié)部分的，復(fù)用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch

# 運行路徑規(guī)劃測試launch（地圖加載、amcl、movebase、rviz）
roslaunch 08nav_demo nav05_path_test.launch

接著我們使用2D Nav Goal來指定目標(biāo)位置來進行移動：

若是我們在行進的過程中放置障礙物，也能夠?qū)崿F(xiàn)自主避障，到達目的地：

2.4.4、rviz訂閱全局地圖、本地地圖

訂閱全局地圖：我們可以重新在rviz中添加一個map，重命名為map_global，訂閱主題為/move_base/global_costmap/costmap

訂閱本地map地圖：訂閱主題為/move_base/global_costmap/costmap

2.5、實踐—自主實現(xiàn)建圖（slam+move_base）

nav06_auto_slam.launch：

<launch>
    <!-- 啟動SLAM節(jié)點 -->
    <include file="$(find 08nav_demo)/launch/nav01_slam.launch" />

    <!-- 運行move_base節(jié)點 -->
    <include file="$(find 08nav_demo)/launch/nav04_path.launch" />

    <!-- 運行rviz（在slim節(jié)點中已經(jīng)有了） -->
    <!-- <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find 08nav_demo)/conf/gmapping.rviz"/> -->
</launch>

接著運行l(wèi)aunch文件來啟動服務(wù)：

# 啟動gazebo(第7章節(jié)部分的，復(fù)用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch

# 僅僅只包含slam、move_base、rviz節(jié)點
roslaunch 08nav_demo nav06_auto_slam.launch

在rviz中我們打開了全局地圖、本地地圖，初始還沒有完整建圖的效果如下：

以往我們需要使用的之前的鍵盤來操控小車移動，這里的話我們就可以直接使用2D Naw Goal來進行手動定位目的地來完成建圖！

最終的效果如下：

等待建圖完成之后，我們就可以去保存全局地圖了：

# 同樣也是執(zhí)行之前的全局地圖命令保存節(jié)點
roslaunch 08nav_demo nav02_map_save.launch

三、導(dǎo)航消息

上面的各個導(dǎo)航服務(wù)集成都使用到了消息組件，對應(yīng)組件就需要使用到相關(guān)消息的發(fā)布、訂閱。

3.1、導(dǎo)航—地圖

地圖相關(guān)的消息主要有兩個:

1、nav_msgs/MapMetaData：地圖元數(shù)據(jù)，包括地圖的寬度、高度、分辨率等。

# 查看消息體命令
rosmsg info nav_msgs/MapMetaData

# 消息結(jié)構(gòu)如下：
time map_load_time
float32 resolution #地圖分辨率
uint32 width #地圖寬度
uint32 height #地圖高度
geometry_msgs/Pose origin #地圖位姿數(shù)據(jù)
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

2、nav_msgs/OccupancyGrid：地圖柵格數(shù)據(jù)，一般會在rviz中以圖形化的方式顯示。

# 查看消息體命令
rosmsg info nav_msgs/OccupancyGrid

# 消息結(jié)構(gòu)如下：
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
#--- 地圖元數(shù)據(jù)
nav_msgs/MapMetaData info
  time map_load_time
  float32 resolution
  uint32 width
  uint32 height
  geometry_msgs/Pose origin
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
#--- 地圖內(nèi)容數(shù)據(jù)，數(shù)組長度 = width * height
int8[] data

3.2、導(dǎo)航—里程計

里程計相關(guān)消息是：nav_msgs/Odometry

# 查看消息體命令
rosmsg info nav_msgs/Odometry

# 消息結(jié)構(gòu)如下：
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose #里程計位姿
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist #速度
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z    
  # 協(xié)方差矩陣
  float64[36] covariance

3.3、導(dǎo)航—坐標(biāo)變換

坐標(biāo)變換相關(guān)消息是：tf/tfMessage

# 查看消息體命令
rosmsg info tf/tfMessage

# 消息結(jié)構(gòu)如下：
geometry_msgs/TransformStamped[] transforms #包含了多個坐標(biāo)系相對關(guān)系數(shù)據(jù)的數(shù)組
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  string child_frame_id
  geometry_msgs/Transform transform
    geometry_msgs/Vector3 translation
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion rotation
      float64 x
      float64 y
      float64 z
      float64 w

3.4、導(dǎo)航—定位

定位相關(guān)消息是：geometry_msgs/PoseArray

# 查看消息體命令
rosmsg info geometry_msgs/PoseArray

# 消息結(jié)構(gòu)如下：
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Pose[] poses #預(yù)估的點位姿組成的數(shù)組
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

3.4、導(dǎo)航—目標(biāo)點與路徑規(guī)劃

目標(biāo)點相關(guān)消息是：move_base_msgs/MoveBaseActionGoal

# 查看消息體命令
rosmsg info move_base_msgs/MoveBaseActionGoal

# 消息結(jié)構(gòu)如下：
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
actionlib_msgs/GoalID goal_id
  time stamp
  string id
move_base_msgs/MoveBaseGoal goal
  geometry_msgs/PoseStamped target_pose
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    geometry_msgs/Pose pose #目標(biāo)點位姿
      geometry_msgs/Point position
        float64 x
        float64 y
        float64 z
      geometry_msgs/Quaternion orientation
        float64 x
        float64 y
        float64 z
        float64 w

路徑規(guī)劃相關(guān)消息是：nav_msgs/Path

# 查看消息體命令
rosmsg info nav_msgs/Path

# 消息結(jié)構(gòu)如下：
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/PoseStamped[] poses #由一系列點組成的數(shù)組
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w

3.5、導(dǎo)航—激光雷達

激光雷達相關(guān)消息是：sensor_msgs/LaserScan

# 查看消息體命令
rosmsg info sensor_msgs/LaserScan

# 消息結(jié)構(gòu)如下：
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 angle_min #起始掃描角度(rad)
float32 angle_max #終止掃描角度(rad)
float32 angle_increment #測量值之間的角距離(rad)
float32 time_increment #測量間隔時間(s)
float32 scan_time #掃描間隔時間(s)
float32 range_min #最小有效距離值(m)
float32 range_max #最大有效距離值(m)
float32[] ranges #一個周期的掃描數(shù)據(jù)
float32[] intensities #掃描強度數(shù)據(jù)，如果設(shè)備不支持強度數(shù)據(jù)，該數(shù)組為空

3.6、導(dǎo)航—相機

對應(yīng)的一般的圖像數(shù)據(jù)：sensor_msgs/Image

# 查看消息體命令
rosmsg info sensor_msgs/Image

# 消息結(jié)構(gòu)如下：
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height #高度
uint32 width  #寬度
string encoding #編碼格式:RGB、YUV等
uint8 is_bigendian #圖像大小端存儲模式
uint32 step #一行圖像數(shù)據(jù)的字節(jié)數(shù)，作為步進參數(shù)
uint8[] data #圖像數(shù)據(jù)，長度等于 step * height

對應(yīng)壓縮后的圖像數(shù)據(jù)：sensor_msgs/CompressedImage

# 查看消息體命令
rosmsg info sensor_msgs/CompressedImage

# 消息結(jié)構(gòu)如下：
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string format #壓縮編碼格式(jpeg、png、bmp)
uint8[] data #壓縮后的數(shù)據(jù)

點云數(shù)據(jù)(帶有深度信息的圖像數(shù)據(jù))：sensor_msgs/PointCloud2

# 查看消息體命令
rosmsg info sensor_msgs/PointCloud2

# 消息結(jié)構(gòu)如下：
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height #高度
uint32 width  #寬度
sensor_msgs/PointField[] fields #每個點的數(shù)據(jù)類型
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name
  uint32 offset
  uint8 datatype
  uint32 count
bool is_bigendian #圖像大小端存儲模式
uint32 point_step #單點的數(shù)據(jù)字節(jié)步長
uint32 row_step   #一行數(shù)據(jù)的字節(jié)步長
uint8[] data      #存儲點云的數(shù)組，總長度為 row_step * height
bool is_dense     #是否有無效點

3.7、深度圖像轉(zhuǎn)激光數(shù)據(jù)

3.7.1、認(rèn)識depthimage_to_laserscan

ros功能包：depthimage_to_laserscan，該功能包可以將深度圖像信息轉(zhuǎn)換成激光雷達信息。

應(yīng)用場景：在諸多SLAM算法中，一般都需要訂閱激光雷達數(shù)據(jù)用于構(gòu)建地圖，因為激光雷達可以感知周圍環(huán)境的深度信息，而深度相機也具備感知深度信息的功能，且最初激光雷達價格比價比較昂貴，實際上傳感器選型可以使用深度相機來去替代激光雷達。

原理：depthimage_to_laserscan將實現(xiàn)深度圖像與雷達數(shù)據(jù)轉(zhuǎn)換的原理比較簡單，雷達數(shù)據(jù)是二維的、平面的，深度圖像是三維的，是若干二維(水平)數(shù)據(jù)的縱向疊加，如果將三維的數(shù)據(jù)轉(zhuǎn)換成二維數(shù)據(jù)，只需要取深度圖的某一層即可，為了方面理解。

優(yōu)缺點：

• 優(yōu)點：深度相機的成本一般低于激光雷達，可以降低硬件成本；
• 缺點：深度相機較之于激光雷達無論是檢測范圍還是精度都有不小的差距，SLAM效果可能不如激光雷達理想。

訂閱topic

image(sensor_msgs/Image)：輸入圖像信息。

camera_info(sensor_msgs/CameraInfo)：關(guān)聯(lián)圖像的相機信息。通常不需要重新映射，因為camera_info將從與image相同的命名空間中進行訂閱。

發(fā)布topic

scan(sensor_msgs/LaserScan)：發(fā)布轉(zhuǎn)換成的激光雷達類型數(shù)據(jù)。

參數(shù)

該節(jié)點參數(shù)較少，只有如下幾個，一般需要設(shè)置的是: output_frame_id。

~scan_height(int, default: 1 pixel)：設(shè)置用于生成激光雷達信息的象素行數(shù)。

~scan_time(double, default: 1/30.0Hz (0.033s))：兩次掃描的時間間隔。

~range_min(double, default: 0.45m)：返回的最小范圍。結(jié)合range_max使用，只會獲取 range_min 與 range_max 之間的數(shù)據(jù)。

~range_max(double, default: 10.0m)：返回的最大范圍。結(jié)合range_min使用，只會獲取 range_min 與 range_max 之間的數(shù)據(jù)。

~output_frame_id(str, default: camera_depth_frame)：激光信息的ID。

3.7.2、實操

# 安裝depthimage-to-laserscan
sudo apt-get install ros-melodic-depthimage-to-laserscan

nav07_depthimage_to_laserscan.launch：編寫launch文件執(zhí)行，將深度信息轉(zhuǎn)換成雷達信息

<launch>
    <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
        <remap from="image" to="/camera/depth/image_raw" />
        <param name="output_frame_id" value="camera"  />
    </node>
</launch>

接著我們來啟動各個launch文件：

# 啟動gazebo(第7章節(jié)部分的，復(fù)用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch

# 深度圖像轉(zhuǎn)激光數(shù)據(jù)
roslaunch 08nav_demo nav07_depthimage_to_laserscan.launch

# 運行路徑規(guī)劃測試launch（地圖加載、amcl、movebase、rviz）
roslaunch 08nav_demo nav05_path_test.launch

在rviz中添加一個雷達掃描：

效果展示：

文章來源地址http://www.zghlxwxcb.cn/news/detail-410944.html

到了這里，關(guān)于ROS學(xué)習(xí)筆記08、機器人導(dǎo)航仿真（slam、map_server、amcl、move_base與導(dǎo)航消息介紹）的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！