costmap_2d概述

参考：ROS基础教程--CostMap_2D包的一些理解^[1] 

一、基本概念

体素（Voxel）

顾名思义是体积的像素。用来在三维空间中表示一个显示基本点的单位。类似于二维平面下的pixel（像素）。voxel是三维空间中定义一个点的图象信息的单位。在平面中定义一个点要两个坐标X和Y就够了，而在三维世界中还要有一个坐标。光有3维坐标位置还不行，还要有颜色等信息，这就是voxel的含义。

机器人在costmap_2D中的模型：两个同心圆，在图1里可以看到下方有两个淡蓝色同心圆，一个机器人的轮廓外切圆和一个机器人内切圆，机器人在costmap里就能够简化成为这两个圆。根据机器人中心至边界或者障碍物的距离和两个同心圆半径比较来判断是否碰撞。

足迹（footprint）

机器人的轮廓。在ROS中，它由二维数组表示[x0,y0] ; [x1,y1] ; [x2,y2]……不需要重复第一个坐标。该占位面积将用于计算内切圆和外接圆的半径，用于以适合此机器人的方式对障碍物进行膨胀。为了安全起见，我们通常将足迹稍大于机器人的实际轮廓。要确定机器人的占地面积，最直接的方法是参考机器人的图纸。 此外，您可以手动拍摄其基座顶视图。 然后使用CAD软件（如Solidworks）适当缩放图像，并将鼠标移动到基座轮廓上并读取其坐标。 坐标的起点应该是机器人的中心。 或者，您可以将机器人移动到一张大纸上，然后绘制基座的轮廓。 然后选择一些顶点并使用标尺来确定它们的坐标。

代价（cost）

代价或者占用，0-255的取值，表示该机器人位于该网格点(grid cell)的代价，或者机器人的frootprint中心cell走到该网格点的代价（中心到达某个位置的代价与非中心部分到达某个位置付出的代价不同，如撞击造成的损伤程度等）。

栅格（cell）

栅格、单元、网格

二、足迹（footprint）

footprint只能是一个多边形，或一个圆形。不论哪种形态，都用std::vector<geometry_msgs::Point>进行存储，对每个Point，z分量都是0。文中把这些Point称为顶点。对多边形，顶点数目是多边形中点的数目，圆形则固定用16个顶点。

导航过程中会发布footprint话题，方便rviz实时显示footprint轮廓，以及位置。示例：图2中的红色五边形。这话题有两个，全局地图中的，局部地图中的。

发布时变量是geometry_msgs::PolygonStamped。

三、costmap的栅格代价计算

无论是激光雷达还是深度相机跑出的2D或3D SLAM地图，都不能直接用于实际的导航，必须将地图转化为costmap(代价地图)，ROS的costmap通常采用grid(网格)形式。以前一直没有搞明白每个栅格的概率是如何算出来的，原因是之前一直忽略了内存的存储结构，栅格地图一个栅格占1个字节，也就是八位，可以存0-255中数据，也就是每个cell cost（网格的值）从0~255我们只需要三种情况：Occupied被占用（有障碍），Free自由区域（无障碍）， Unknown Space未知区域。。

Costmap是机器人收集传感器信息建立和更新的二维或三维地图，可以从下图简要了解。

注意: 在图2中，红色cell（图中红色蓝色区域都是一系列cell堆叠出来的）代表的是代价地图中的障碍，蓝色cell代表的是通过机器人内切圆半径计算的障碍物膨胀，红色多边形代表的是机器人footprint(机器人轮廓的垂直投影)。 为了使机器人不碰到障碍物，机器人的footprint绝对不允许与红色cell相交，机器人的中心绝对不允许与蓝色cell相交。

空间状态（Occupied, Free, and Unknown）

ROS的代价地图（costmap）采用网格（grid）形式，每个网格的值（cell cost）从0~255。分成三种状态：被占用（有障碍）、自由区域（无障碍）、未知区域；以激光雷达为传感器（或者kinect之类的深度相机的伪激光雷达），根据激光测量的障碍物距离机器人中心的距离，结合机器人的内切和外切半径，搞一个映射，利用bresenham算法可以填充由激光雷达的位置到障碍物之间的栅格概率了。

虽然代价地图中每个cell可用255个不同值中任何一个值，可是下层数据结构仅需要3个值。 具体来说在这种下层结构中，每个cell仅需要3个值来表示cell的3种状态：free，occupied，unknown。 当投影到代价地图时候，每种状态被赋一个特定的代价值，也就是说每个cell的cost值是由这个cell对应的各层中对应的cell的状态进行加权得到的。 如果列有一定量的占用就被赋代价值costmap_2d::LETHAL_OBSTACLE， 如果列有一定量的unknown cells 就被赋代价值costmap_2d::NO_INFORMATION, 剩余其它列赋代价值为costmap_2d::FREE_SPACE

在某时刻和机器人当前前进方向上的网格点的代价计算示意见图2（如果机器人的前进方向改变，则网格点的代价也会发生变化）。

上图下面的红色五边形区域为机器人的轮廓。坐标系内的区域可分为五部分，（cost的计算是以cell为单位进行的，而不是以某个障碍物为单位进行的，也就是一次只计算一个cell的cost值）

• （1） Lethal（致命的）:机器人的中心（center cell）与该网格的中心重合，此时机器人必然与障碍物冲突。
• （2） Inscribed（内切）：网格中心位于机器人的内切轮廓内，此时机器人也必然与障碍物冲突。
• （3） Possibly circumscribed（可能受限）：网格中心位于机器人的外切圆与内切圆轮廓之间，此时机器人相当于靠在障碍物附近，所以不一定冲突，取决于机器人的方位或者说姿态。
• （4） Freespace（自由空间）：网格中心位于机器人外切圆之外，属于没有障碍物的空间。
• （5） Unknown（未知）：未知的空间。

如果按照cell的三种状态划分，个人认为上述前3种状态，都属于“被占用”状态。

这里介绍一下costmap_2d中计算cost的方法。假设，机器人内切半径为0.5m，外切半径为0.7m，当激光返回障碍距离在机器人中心附近，叫致命障碍，机器人一定能碰到障碍物，比如说0m,直接贴着机器人，或会取小于栅格的边长，比如小于0.1m范围内,则这个栅格值就设为254；当返回来的数值在0.1-0.5m之间，就设253；当在0.5-0.7之间，则可以设128，或者在252-128找个比例值（程序中可以控制），属于受限区域，可能发生碰撞，是否碰撞，取决于机器人的姿态；当0.7-膨胀半径之间，设1-127之间的映射值，不会发生碰撞；当大于膨胀距离，则设为0，称为freespace。Unknown -- 意味着给定的单元没有相应的信息。我们看坐标系中较细的红色光滑曲线就是cost曲线，x是距离机器人footprint的圆心距离，而y是cost值，cost随着x的增大而减小距离，当x>=内切圆半径时开始有值；当x=0时，y=254；当x=resolution/2时，cost=253；（图中右上角的较粗的台阶状红线是单元格的边线，或者认为是障碍物（单元格化后）的边线）。costmap_2d中实际计算公式：

四、Costmap的分层与更新

4.1 Costmap中的地图与分层

Costmap_2D提供了一种2D代价地图的实现方案，该方案利用输入传感器数据，构建数据2D或者3D（依赖于是否使用基于voxel的实现）代价地图。 此外，该包也支持利用map_server初始化代价地图，支持滚动窗口的代价地图，支持参数化订阅和配置传感器主题。

Costmap由多层组成，每种功能放置一层中。例如图3所示，静态地图是一层，障碍物是另一层。默认情况下，障碍物层维护的是3D信息，3D障碍物数据可以让层更加灵活的标记和清除障碍物。例如在costmap_2d包中，StaticLayer（静态地图层）是第一层，ObstacleLayer（障碍物层）是第二层，InflationLayer（膨胀层）是第三层。这三层组合成了master map（最终的costmap），供给路线规划模块使用。

如何将各层私有地图“合并”成主地图？一般有两种方法，一是覆盖，二是取最大值。覆盖，用私有地图值设为主地图值，障碍层合并到主地图用的就是这方法。取最大值是取两者的最大值。这里要注意，表示未知的NO_INFORMATION是uint8_t的最大值255，因而不论覆盖，还是最大值，对NO_INFORMATION要特别处理。以覆盖为例，处理NO_INFORMATION分为两点。对私有地图中代价是未知的栅格，不去碰；对主地图中代价是未知的栅格，则私有地图只要不是未知，就覆盖。计算过程参考CostmapLayer::updateWithMax。总之，在值上，NO_INFORMATION有着255是最大，但在表示代价的物理意义上，它其实是最低。