10. ROS costmap代價地圖

博客轉自: https://blog.csdn.net/weixin_42005898/article/details/100051838

在機器人進行路徑規划時，我們需要明白規划算法是依靠什么在地圖上來計算出來一條路徑的。依靠的是gmapping掃描構建的一張環境全局地圖，但是僅僅依靠一張原始的全局地圖是不行的。因為這張地圖是靜態的，無法隨時來更新地圖上的障礙物信息。在現實環境中，總會有各種無法預料到的新障礙物出現在當前地圖中，或者舊的障礙物現在已經從環境地圖中被移除掉了，那么我們就需要來隨時更新這張地圖。同時由於默認的地圖是一張黑白灰三色地圖，即只會標出障礙物區域、自由移動區域和未被探索區域。機器人在這樣的地圖中進行路徑規划，會導致規划的路徑不夠安全，因為我們的機器人在移動時需要與障礙物之間保持一定的安全緩沖距離，這樣機器人在當前地圖中移動時就更安全了。

costmap簡單來說就是為了在這張地圖上進行各種加工，方便我們后面進行路徑規划而存在的。那具體該如何實現costmap呢？在ROS中使用costmap_2d這個軟件包來實現的，該軟件包在原始地圖上實現了兩張新的地圖。一個是local_costmap，另外一個就是global_costmap，根據名字大家就可以知道了，兩張costmap一個是為局部路徑規划准備的，一個是為全局路徑規划准備的。無論是local_costmap還是global_costmap，都可以配置多個圖層，包括下面幾種：

1. Static Map Layer：靜態地圖層，基本上不變的地圖層，通常都是SLAM建立完成的靜態地圖。
2. Obstacle Map Layer：障礙地圖層，用於動態的記錄傳感器感知到的障礙物信息。
3. Inflation Layer：膨脹層，在以上兩層地圖上進行膨脹（向外擴張），以避免機器人的撞上障礙物。
4. Other Layers：你還可以通過插件的形式自己實現costmap，目前已有Social Costmap Layer、Range Sensor Layer等開源插件。

通過以下步驟，創建stdr_move_base軟件包

你可能會納悶，為什么講costmap會提到move_base這個軟件包呢？這是因為move_base軟件包是ROS中自動導航的核心軟件包，我們可以查看下面這張圖大家就會對此軟件包有深刻認識了

根據move_base的內部邏輯流程圖得知，在進行路徑規划時costmap是必不可少的。因此我們需要首先創建個stdr_move_base軟件包，然后配置costmap相關的參數，這樣move_base軟件包內的路徑規划器才能找到一條合適的路徑控制機器人移動到達指定的目的地。創建好軟件包后，接下來就可以來編寫launch文件了，命名為stdr_move_base.launch，該launch文件內容如下：

<!--
  FileName: stdr_move_base.launch
  Copyright: 2016-2018 ROS小課堂 www.corvin.cn
  Author: corvin
  Description:
    啟動move_base節點，加載各個配置文件。
  History:
    20180528: initial this file.
    20180530: add arg param to rename all kinds of frames,topics,add load params file.
-->
<launch>
  <arg name="odom_frame_id"   default="/map_static"/>
  <arg name="base_frame_id"   default="/robot0"/>
  <arg name="global_frame_id" default="/map"/>
 
  <arg name="odom_topic"      default="/robot0/odom"/>
  <arg name="cmd_vel_topic"   default="/robot0/cmd_vel"/>
  <arg name="map_topic"       default="/amcl/map"/>
 
  <node pkg="move_base" type="move_base" name="stdr_move_base" output="screen">
    <rosparam file="$(find stdr_move_base)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find stdr_move_base)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find stdr_move_base)/config/local_costmap_params.yaml"  command="load" />
    <rosparam file="$(find stdr_move_base)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find stdr_move_base)/config/dwa_local_planner_params.yaml"    command="load" />
    <rosparam file="$(find stdr_move_base)/config/move_base_params.yaml"            command="load" />
    <rosparam file="$(find stdr_move_base)/config/global_planner_params.yaml"       command="load" />
   
   <param name="global_costmap/global_frame"     value="$(arg global_frame_id)"/>
   <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
   <param name="local_costmap/global_frame"      value="$(arg odom_frame_id)"/>
   <param name="local_costmap/robot_base_frame"  value="$(arg base_frame_id)"/>
   <param name="DWAPlannerROS/global_frame_id"   value="$(arg odom_frame_id)"/>
 
    <!-- move base default publish cmd to /cmd_vel topic,now remap to /robot0/cmd_vel -->
    <remap from="/cmd_vel" to="$(arg cmd_vel_topic)"/>
 
    <!-- move_base default subscribe odom topic,now remap to /robot0/odom -->
    <remap from="/odom" to="$(arg odom_topic)"/>
 
    <!-- move_base default subscribe map topic,now remap to /amcl/map -->
    <remap from="/map" to="$(arg map_topic)"/>
  </node>
</launch>

在啟動move_base節點時，可以看到我們首先加載了costmap_common_params.yaml到global_costmap和local_costmap兩個命名空間中，因為該配置文件是一個通用的代價地圖配置參數，即local_costmap和global_costmap都需要配置的參數。然后下面是local_costmap_params.yaml專門為了局部代價地圖配置的參數，global_costmap_params.yaml專門為全局代價地圖配置的參數。

配置costmap_common_params.yaml

在config目錄下，創建costmap_common_params.yaml文件，配置的參數如下：

#FileName: costmap_common_params.yaml
#Copyright: 2016-2018 ROS小課堂 www.corvin.cn
#Author: corvin
#Description:
# 代價地圖通用參數配置文件,就是全局代價地圖和局部代價地圖
# 共同都需要配置的參數,各參數意義如下:
# robot_radius: 機器人的半徑
#
#History:
# 20180613: initial this file.
 
robot_radius: 0.2
 
obstacle_layer:
  enabled: true
  combination_method: 1
  track_unknown_space: true
  obstacle_range: 2.5
  raytrace_range: 3.0
  observation_sources: laser_scan_sensor
  laser_scan_sensor: {
    sensor_frame: /robot0_laser_0,
    data_type: LaserScan,
    topic: /robot0/laser_0,
    marking: true,
    clearing: true
  }
 
inflation_layer:
  enabled: true
  cost_scaling_factor: 5.0
  inflation_radius: 0.36
 
static_layer:
  enabled: true

下面來依次解釋下各參數的意義，方便大家以后來根據需要來自行修改調試：

• robot_radius：設置機器人的半徑，單位是米。由於在stdr中機器人是圓形的，所以可以直接設置該參數。如果你的機器人不是圓形的那就需要使用footprint這個參數，該參數是一個列表，其中的每一個坐標代表機器人上的一點，設置機器人的中心為[0,0]，根據機器人不同的形狀，找到機器人各凸出的坐標點即可，具體可參考下圖來設置：

• obstacle_layer:配置障礙物圖層
  • enabled:是否啟用該層
  • combination_method:只能設置為0或1，用來更新地圖上的代價值，一般設置為1

  • track_unknown_space:如果設置為false，那么地圖上代價值就只分為致命碰撞和自由區域兩種，如果設置為true，那么就分為致命碰撞，自由區域和未知區域三種。意思是說假如該參數設置為true的話，就意味着地圖上的未知區域也會被認為是可以自由移動的區域，這樣在進行全局路徑規划時，可以把一些未探索的未知區域也來參與到路徑規划，如果你需要這樣的話就將該參數設置為false。不過一般情況未探索的區域不應該當作可以自由移動的區域，因此一般將該參數設置為true

  • obstacle_range:設置機器人檢測障礙物的最大范圍，意思是說超過該范圍的障礙物，並不進行檢測，只有靠近到該范圍內才把該障礙物當作影響路徑規划和移動的障礙物;
  • raytrace_range:在機器人移動過程中，實時清除代價地圖上的障礙物的最大范圍，更新可自由移動的空間數據。假如設置該值為3米，那么就意味着在3米內的障礙物，本來開始時是有的，但是本次檢測卻沒有了，那么就需要在代價地圖上來更新，將舊障礙物的空間標記為可以自由移動的空間。
  • observation_sources:設置導航中所使用的傳感器，這里可以用逗號形式來區分開很多個傳感器，例如激光雷達，碰撞傳感器，超聲波傳感器等，我們這里只設置了激光雷達;
  • laser_scan_sensor:添加的激光雷達傳感器
  • sensor_frame:激光雷達傳感器的坐標系名稱;
  • data_type:激光雷達數據類型;
  • topic:該激光雷達發布的話題名;
  • marking:是否可以使用該傳感器來標記障礙物;
  • clearing:是否可以使用該傳感器來清除障礙物標記為自由空間;
• inflation_layer:膨脹層，用於在障礙物外標記一層危險區域，在路徑規划時需要避開該危險區域
  • enabled:是否啟用該層;　　

  • cost_scaling_factor:膨脹過程中應用到代價值的比例因子，代價地圖中到實際障礙物距離在內切圓半徑到膨脹半徑之間的所有cell可以使用如下公式來計算膨脹代價：

    exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)

    ,公式中costmap_2d::INSCRIBED_INFLATED_OBSTACLE目前指定為254，注意： 由於在公式中cost_scaling_factor被乘了一個負數，所以增大比例因子反而會降低代價

  • inflation_radius:膨脹半徑，膨脹層會把障礙物代價膨脹直到該半徑為止，一般將該值設置為機器人底盤的直徑大小。如果機器人經常撞到障礙物就需要增大該值，若經常無法通過狹窄地方就減小該值。
• Static_layer:靜態地圖層，即SLAM中構建的地圖層
  • enabled:是否啟用該地圖層

通過下圖來認識下為何要設置膨脹層以及意義：

配置global_costmap_params.yaml

全局代價地圖是作為進行全局路徑規划時的參考，我們需要在config目錄中，創建global_costmap_params.yaml文件，該文件是為全局代價地圖配置的參數，具體配置的參數如下：

#FileName: global_costmap_params.yaml
#Copyright: 2016-2018 ROS小課堂 www.corvin.cn
#Author: corvin
#Description:
#  全局代價地圖參數配置文件,各參數的意義如下：
#  global_frame:在全局代價地圖中的全局坐標系;
#  robot_base_frame:機器人的基坐標系;
#
#History:
#  20180613: initial this file.
global_costmap:
  global_frame: /map
  robot_base_frame: /robot0
  update_frequency: 0.5
  static_map: true
  rolling_window: false
  transform_tolerance: 1.0
  plugins:
    - {name: static_layer,    type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

下面我們來詳細解釋下該全局代價地圖配置文件中各參數的意義：

• global_frame:全局代價地圖需要在哪個坐標系下運行;
• robot_base_frame:在全局代價地圖中機器人本體的基坐標系，就是機器人上的根坐標系。通過global_frame和robot_base_frame就可以計算兩個坐標系之間的變換，得知機器人在全局坐標系中的坐標了。
• update_frequency:全局代價地圖更新頻率，一般全局代價地圖更新頻率設置的比較小;
• static_map:配置是否使用map_server提供的地圖來初始化，一般全局地圖都是靜態的，需要設置為true;
• rolling_window:是否在機器人移動過程中需要滾動窗口，始終保持機器人在當前窗口中心位置;
• transform_tolerance:坐標系間的轉換可以忍受的最大延時;
• plugins:在global_costmap中使用下面三個插件來融合三個不同圖層，分別是static_layer、obstacle_layer和inflation_layer，合成一個master_layer來進行全局路徑規划。

配置local_costmap_params.yaml

局部代價地圖配置參數所建立的地圖主要是為局部路徑規划所使用，我們可以在config目錄下，創建local_costmap_params.yaml文件，完整內容如下：

#FileName: local_costmap_params.yaml
#Copyright: 2016-2018 ROS小課堂 www.corvin.cn
#Author: corvin
#Description:
#  本地代價地圖需要配置的參數，各參數意義如下:
#  global_frame:在本地代價地圖中的全局坐標系;
#  robot_base_frame:機器人本體的基坐標系;
#
#History:
#  20180613: initial this file.
 
local_costmap:
  global_frame: /map_static
  robot_base_frame: /robot0
  update_frequency: 5.0
  publish_frequency: 3.0
  static_map: false
  rolling_window: true
  width: 4.0
  height: 4.0
  resolution: 0.05
  transform_tolerance: 0.5
  plugins:
    - {name: static_layer,        type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

下面來詳細解釋下每個參數的意義：

• global_frame:在局部代價地圖中的全局坐標系，一般需要設置為odom_frame，但是由於stdr沒有這個坐標系，我就拿/map_static來代替了;
• robot_base_frame:機器人本體的基坐標系;
• update_frequency:局部代價地圖的更新頻率;
• publish_frequency:局部代價地圖的發布頻率;
• static_map:局部代價地圖一般不設置為靜態地圖，因為需要檢測是否在機器人附近有新增的動態障礙物;
• rolling_window:使用滾動窗口，始終保持機器人在當前局部地圖的中心位置;
• width:滾動窗口的寬度，單位是米;
• height:滾動窗口的高度，單位是米;
• resolution:地圖的分辨率，該分辨率可以從加載的地圖相對應的配置文件中獲取到;

• transform_tolerance:局部代價地圖中的坐標系之間轉換的最大可忍受延時;
• plugins:在局部代價地圖中，不需要靜態地圖層，因為我們使用滾動窗口來不斷的掃描障礙物，所以就需要融合兩層地圖（inflation_layer和obstacle_layer）即可，融合后的地圖用於進行局部路徑規划;

獲取代價地圖上的點的坐標點位置和朝向

 我們在啟動了move_base節點后就可以使用Rviz中的"2D Nav Goal"功能來獲取到地圖上坐標點的坐標和朝向，具體的命令如下：

rostopic echo /move_base_simple/goal