Costmap代价地图

以下为ROS navigation导航工具包的move_base框架图。其中有两个关于代价地图的模块(红框所框)，全局代价地图global_costmap和局部代价地图local_costmap，这两个代价地图实际上是调用的同一个功能包代码，通过配置不同的参数实例化为两个代价地图，全局代价地图就提供给全局路径规划器使用，局部代价地图就提供给局部路径规划器使用

全局代价地图（Global Costmap）：为全局路径规划（如A*、Dijkstra算法）提供静态环境模型，生成从起点到目标点的最优路径。

• 范围大：覆盖整个已知环境（如地图或SLAM构建的栅格地图）。

• 更新频率低：主要依赖静态地图（如map_server加载的PGM地图），通常仅在收到新地图时更新。

• 包含静态障碍：如墙壁、家具等固定物体，也可能叠加动态障碍（但更新较慢）。

• 分辨率较低：为了平衡计算效率，栅格粒度可能较粗。

 局部代价地图（Local Costmap）：为局部路径规划（如TEB、DWA算法）提供动态环境模型，处理实时避障和局部路径调整。

• 范围小：仅围绕机器人周围（如4x4米），关注即时环境。

• 更新频率高：实时融合传感器数据（如激光雷达、深度相机），快速响应动态障碍（如行人、移动物体）。

• 高分辨率：栅格粒度更细，以精确避障。

• 短期记忆：可能包含临时障碍物（如inflation_layer膨胀区域）。

协同工作流程：

1. 全局规划：导航开始时，全局代价地图基于静态地图生成一条粗略路径（如navfn或Global Planner）。

2. 局部执行：机器人沿全局路径移动时，局部代价地图实时检测动态障碍，并通过局部规划器（如TEB）调整轨迹。

3. 恢复机制：若局部避障失败（如死胡同），全局规划器会重新规划路径。

 

地图文件与地图参数文件 

在我们完成建图的时候会保存一个pgm格式的地图文件，pgm就是便携式灰度图，除了这种格式外，costmap还支持加载jpg,png等常用的图片格式，还可以使用画图工具修改或重画地图。

地图文件是由很多像素组成的，每个像素我们取它的灰度值作为网格值（取值范围是0-255，0代表黑色，255代表白色）

除此之外还会有一个地图参数文件 .yaml。

image:/home/yf/ros_ws/src/map/mymap.pgm     # 地图文件路径
resolution:0.050000                         # 地图分辨率，米/像素
origin:[-13.800000, -13.800000, 0.000000]   # 小车在地图中的起始姿态(x, y, yaw)
negate:0                                    # 是否应该反转 白/黑 空闲/占用 默认置0
occupied_thresh: 0.65                       # 大于此阈值的像素被视为完全占用。取值[0, 1]
free_thresh: 0.196                          # 小于此阈值的像素被认为是完全空闲的。取值[0, 1]
# 可选参数mode: Trinary 默认值为三元解释，即加载进去的地图最终输出一共有三个状态，分别是完全占用、完全空闲和未知

 

 

footprint

可以理解为小车投影到二维地图上的轮廓

 

膨胀

如下图所示，红色为障碍物，蓝色部分就是障碍物按一定的半径膨胀出来的，为了防止小车撞到障碍物上。膨胀的半径至少是小车轮廓的外接圆半径，小车的中心不能与蓝色膨胀部分相交

 

costmap的地图组成

costmap是由三层地图组成的,:Static Layer(静态地图层)，Obstacle Layer(障碍物层)，Inflation Layer(膨胀层)。如下图所示，Master就是最终在rviz中呈现的地图，它是由静态地图层、障碍物层和膨胀层叠加形成。

1.  Static Layer(静态地图层)：接收/map话题信息，加载建好的地图
2. Obstacle Layer(障碍物层)：接收雷达等信息，实时检测环境的障碍物。可以通过PointCloud,PointCloud2和LaserScan进行观测，常用LaserScan即雷达。
3. Inflation Layer(膨胀层)：根据inflation_radius(膨胀半径)参数，膨胀障碍物

前面说过，在move_base框架中有两个costmap，这两个代价地图模块都是根据costmap2DROS接口来进行实例化

 

costmap类的继承图

如下图所示，重点关注Costmap2D和Layer类，Costmap2D提供存储地图，变换坐标等功能；Layer提供每一层的对地图的操作，包括updateCosts和updateBounds两个主要函数。

类CostmapLayer继承了 Costmap2D和Layer类，所以既提供地图服务，又提供对地图操作的方法。

ObstacleLayer和StaticLayer类继承了CostmapLayer类。

inflationLayer就是膨胀层的类，膨胀层不需要存储地图信息，只对地图进行膨胀操作，所以不需要Costmap2D

LayeredCostmap是将类ObstacleLayer、StaticLayer和inflationLayer实例化后的对象进行整合形成masterlayer，然后可以展示在rviz中。主要的更新函数是updateMap

Costmap2DROS类就是对外提供一个简洁的接口

 

下面介绍这些类是如何一起工作来更新地图的：首先Layeredcostmap里有更新函数updateMap，遍历调用了所有层的两个核心函数(updateCosts和updateBounds，一共三个层，所以共调用6个函数，updateBounds函数用于各层自己更新，updateCosts则将更新后的内容反映到masterlayer上) 

具体流程如下：下图1为初始化后的状态，每一层都调用更新函数在自己的层区域内进行更新(静态地图一般不更新)，可以看到图2障碍物更新以后和初始化状态就不一样了，再调用函数updateCosts就把更新后的状态反映到了master上面。图3 4 5分别为静态层、障碍物层、膨胀层调用函数进行更新并反映到masterlayer上的过程。同时在Costmap2DROS这个接口类开启更新map的线程运行map的updateloop函数来循环调用Layeredcostmap类

 

恢复行为

与global_costmap和local_costmap联系比较密切的recovery_behaviors。这个就是在导航时如果遇到堵住的情况就会进行一些恢复行为看看能不能正常恢复，如果恢复行为还是不行的情况下它就终止导航

官方提供的恢复行为一共有三个插件。在navigation官方的功能包里都是有源码的

参数 

以下参数是从navigation/costmap_2d/cfg中提取出来的，这样修改参数更加直观 。我将navigation功能包的核心参数提出，重新建立一个功能包ucar_nav专门用来修改navigation的参数，以下分别为costmap_common_params.yaml、global_costmap_params.yaml和local_costmap_params.yaml文件，最后再在ucar_nav中写一个启动文件即可启动导航，launch文件内容见篇尾，这个launch文件启动了导航的所有参数文件，其中包括文件costmap_common_params.yaml、global_costmap_params.yaml和local_costmap_params.yaml

#Description:
# 代价地图通用参数配置文件,就是全局代价地图和局部代价地图
# 共同都需要配置的参数,各参数意义如下:
# robot_radius: 机器人的半径
# "obstacle_range" 参数确定最大范围传感器读数，这将导致障碍物被放入代价地图中。
# 在这里，我们把它设置在3米，这意味着机器人只会更新其地图包含距离移动基座3米以内的障碍物的信息。
# “raytrace_range”参数确定了用于清除指定范围外的空间。
# 将其设置为3.0米，这意味着机器人将尝试清除3米外的空间，在代价地图中清除3米外的障碍物。#robot_radius: 0.2#如footprint 参数定义了机器人的轮廓（Footprint），用于表示机器人在二维平面上的形状和大小。它是一个多边形的顶点列表，每个顶点由 [x, y] 坐标表示。这些坐标描述了机器人在地图上的物理边界，用于避障和路径规划。
#机器人的基坐标系（base_link）通常位于机器人的中心。x 轴指向机器人的前方，y 轴指向机器人的左侧，z 轴指向机器人的上方。
#[0.171, -0.128]: 表示机器人轮廓的右前角，距离机器人中心前方 0.171 米，右侧 0.128 米。
footprint: [[0.171, -0.128], [0.171, 0.128], [-0.171, 0.128], [-0.171, -0.128]]obstacle_layer: # 这是用于检测和跟踪障碍物的层。它会根据传感器数据更新代价地图，将障碍物添加到地图中，并尝试清除可能的障碍物enabled: true #  一个布尔值，指示是否启用障碍物层combination_method: 1 # 用于控制多个传感器数据如何融合到代价地图中。它的作用是决定当多个传感器检测到同一区域时，如何合并这些传感器的数据。# combination_method 是一个整数参数,0表示当多个传感器检测到同一区域时，选择最大的代价值作为该区域的最终值,# 如果某个传感器曾经检测到高代价值的障碍物，即使后续传感器数据更新为低代价值，该区域的代价值仍然会保持较高（直到被其他机制清除）。# 1表示当多个传感器检测到同一区域时，后接收到的传感器数据会覆盖之前的数据,每次传感器数据更新时，都会直接覆盖之前的代价值，确保最新的传感器数据被优先使用。# 2表示当多个传感器检测到同一区域时，选择最小的代价值作为该区域的最终值。track_unknown_space: true #  一个布尔值，指示是否跟踪未知空间。如果设置为true，则会将未知区域标记为障碍物。obstacle_range: 2.0 # 最大障碍单位为米。物检测范围raytrace_range: 3.0 #  用于清除指定范围外的空间的参数，单位为米observation_sources: laser_scan_sensorlaser_scan_sensor:{sensor_frame: laser_frame,data_type: LaserScan,topic: scan,marking: true,clearing: true,inf_is_valid: true,}inflation_layer: # inflation_layer: 这是用于扩展障碍物的层，以确保机器人周围的区域被正确考虑。它会根据代价地图中的障碍物信息来调整代价值。增加该值可以扩大避障范围，但可能会限制机器人的运动空间。enabled: truecost_scaling_factor: 13.0 # 控制代价值随距离变化的速率。较大的 cost_scaling_factor:代价值衰减得更快，膨胀区域较小。机器人会更接近障碍物，路径规划更灵活，但避障安全性降低。较小的 cost_scaling_factor:代价值衰减得更慢，膨胀区域较大。机器人会远离障碍物，避障安全性提高，但路径规划可能更保守。# inflation_radius: 0.2inflation_radius: 0.15 #  扩展半径，即将障碍物周围多远的区域考虑在内，单位为米。static_layer: # 这是用于处理静态地图的层。它会将静态地图中的信息添加到代价地图中，以确保机器人周围的环境与实际环境保持一致。enabled: true

#Description:
#  全局代价地图参数配置文件,各参数的意义如下：
#  global_frame:在全局代价地图中的全局坐标系;
#  robot_base_frame:机器人的基坐标系;
#0.2quanju  inflation_layer
global_costmap:global_frame: maprobot_base_frame: base_linkupdate_frequency: 0.5 # 代价地图更新的频率，以赫兹为单位。它表示代价地图将以多快的速度更新和重新计算。值越高，地图更新越快，但计算负载也越大.update_frequency不断更新动态障碍物，更新膨胀区域，更新传感器数据publish_frequency: 5.0 #  代价地图发布的频率，以赫兹为单位。它表示代价地图将以多快的速度发布给其他节点。值越高，地图发布越频繁，但会增加网络负载。static_map: true # 一个布尔值，指示是否使用静态地图。如果设置为true，则代价地图将使用静态地图服务（例如map_server）提供的静态地图。如果设置为false，则代价地图将不使用静态地图服务，而是由代价地图自己生成地图。rolling_window: false # 是否使用滚动窗口模式。如果为 true，全局代价地图会以机器人当前位置为中心动态更新；如果为 false，全局代价地图会覆盖整个地图。transform_tolerance: 10 # 确保在导航过程中，坐标系变换（如从 map 到 base_link 的变换）是有效的和及时的。如果变换的时间戳与当前时间的差值超过了 transform_tolerance，该变换会被认为无效，导航系统可能会采取相应的措施（如停止机器人或重新请求变换）。确保使用的变换是最新的和有效的。避免使用过时的变换数据，导致导航错误。track_unknown_space: true #  一个布尔值，指示是否跟踪未知空间。如果设置为true，则代价地图将尝试跟踪地图中未知空间的信息，否则将忽略未知空间。plugins:- {name: static_layer,    type: "costmap_2d::StaticLayer"}- {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}- {name: inflation_layer, type: "costmap_2d::InflationLayer"}

#Description:
#  本地代价地图需要配置的参数，各参数意义如下:
#  global_frame:在本地代价地图中的全局坐标系;
#  robot_base_frame:机器人本体的基坐标系;
#0.1
local_costmap:global_frame: maprobot_base_frame: base_linkupdate_frequency: 8.0 # 本地代价地图的更新频率，以赫兹（Hz）为单位。指定了代价地图更新的频率，即更新地图中的障碍物信息的速率publish_frequency: 5.0 # 本地代价地图的发布频率，以赫兹（Hz）为单位。指定了代价地图发布更新的频率，即向外部节点发布地图信息的速率。static_map: true # 指定是否使用静态地图。如果设置为true，则使用静态地图来构建代价地图,这意味着局部代价地图会包含全局地图中的静态障碍物信息。。如果设置为false，则使用滚动窗口方式构建代价地图,完全依赖传感器数据激光雷达来构建局部环境信息。rolling_window: true # 指定是否使用滚动窗口方式构建代价地图。如果设置为 true，局部代价地图会以机器人当前位置为中心，动态更新地图内容。地图的范围是固定的（由 width 和 height 参数定义），但地图的内容会随着机器人的移动而滚动更新。如果设置为 false，局部代价地图的范围是固定的，且不会随着机器人的移动而滚动更新。机器人可能会移动到地图边界之外，导致地图无法覆盖当前环境。width: 2.0 # 本地代价地图的宽度和高度，以米为单位。指定了代价地图的尺寸，即代价地图覆盖的地理区域的大小。height: 2.0resolution: 0.03 # 本地代价地图的分辨率，以米为单位。指定了代价地图的分辨率，即每个栅格的大小。transform_tolerance: 10 # TF变换的容忍度，以秒为单位。指定了在更新代价地图时允许的TF变换的最大延迟时间，以确保代价地图的正确性plugins:
#    - {name: static_layer,    type: "costmap_2d::StaticLayer"}- {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}- {name: inflation_layer, type: "costmap_2d::InflationLayer"}

 

<launch><!-- Run the map server --><include file="$(find ucar_controller)/launch/base_driver.launch" > </include><include file="$(find ydlidar)/launch/ydlidar.launch" > </include><node name="map_server" pkg="map_server" type="map_server" args="$(find ucar_nav)/maps/yf.yaml" output="screen"><param name="frame_id" value="map" /></node> <include file="$(find ucar_nav)/launch/config/amcl/amcl_omni.launch" > </include><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"><param name="base_global_planner" value="global_planner/GlobalPlanner"/><param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS"/><rosparam file="$(find ucar_nav)/launch/config/move_base/costmap_common_params.yaml" command="load" ns="global_costmap" /><rosparam file="$(find ucar_nav)/launch/config/move_base/costmap_common_params.yaml" command="load" ns="local_costmap" /><rosparam file="$(find ucar_nav)/launch/config/move_base/global_planner_params.yaml" command="load" /><rosparam file="$(find ucar_nav)/launch/config/move_base/tra_local_planner_params.yaml" command="load" /><rosparam file="$(find ucar_nav)/launch/config/move_base/local_costmap_params.yaml" command="load" /><rosparam file="$(find ucar_nav)/launch/config/move_base/global_costmap_params.yaml" command="load" /><rosparam file="$(find ucar_nav)/launch/config/move_base/laser_filters.yaml" command="load" /><remap from="scan" to="/raw_scan" />  <!-- 输入原始激光话题 --><remap from="scan_filtered" to="/scan" />  <!-- 输出过滤后话题 --></node><node pkg="rviz" type="rviz" name="rviz" args="-d $(find ucar_nav)/launch/config/rviz/rviz.rviz" /></launch>