激光SLAM导航系列(三)Costmap(代价地图)（上）

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

上图中，红色部分代表costmap中的障碍物，蓝色部分表示通过机器人内切圆半径膨胀出的障碍，红色多边形是footprint(机器人轮廓的垂直投影)。为了避免碰撞，footprint不应该和红色部分有交叉，机器人中心不应该与蓝色部分有交叉。
ROS的代价地图（costmap）采用网格（grid）形式，每个网格的值（cell cost）从0~255。分成三种状态：被占用（有障碍）、自由区域（无障碍）、未知区域；
具体状态和值对应有下图：



上图可分为五部分，其中红色多边形区域为机器人的轮廓：
（1） Lethal（致命的）:机器人的中心与该网格的中心重合，此时机器人必然与障碍物冲突。
（2） Inscribed（内切）：网格的外切圆与机器人的轮廓内切，此时机器人也必然与障碍物冲突。
（3） Possibly circumscribed（外切）：网格的外切圆与机器人的轮廓外切，此时机器人相当于靠在障碍物附近，所以不一定冲突。
（4） Freespace（自由空间）：没有障碍物的空间。
（5） Unknown（未知）：未知的空间。
ROS中costmap_2d这个包提供了一个可以配置的结构维护costmap,其中Costmap通过costmap_2d::Costmap2DROS对象利用传感器数据和静态地图中的信息来存储和更新现实世界中障碍物的信息。costmap_2d::Costmap2DROS为用户提供了纯粹的2维索引，这样可以只通过columns查询障碍物。举个例子来说，一个桌子和一双鞋子在xy平面的相同位置，有不同的Z坐标，在costm_2d::Costmap2DROS目标对应的的costmap中，具有相同的cost值。这旨在帮助规划平面空间。
Costmap由多层组成，例如在costmap_2d包中，StaticLayer（静态地图层）是第一层，ObstacleLayer（障碍物层）是第二层，InflationLayer（膨胀层）是第三层。这三层组合成了master map（最终的costmap），供给路线规划模块使用。
Costmap主接口是costmap_2d::Costmap2DROS，它维持了Costmap在ROS中大多数的相关的功能。它用所包含的costmap_2d::LayeredCostmap类来跟踪每一个层。每层使用pluginlib（ROS插件机制）来实例化并添加到LayeredCostmap类的对象中。各个层可以被独立的编译，且允许使用C++接口对costmap做出任意的改变。
Costmap ROS接口
ROS对costmap进行了复杂的封装，提供给用户的主要接口是Costmap2DROS,而真正的地图信息是储存在各个Layer中。下图可以简要说明Costmap的各种接口的关系：



Costmap的ObstacleLayer和StaticLayer都继承于CostmapLayer和Costmap2D,因为它们都有自己的地图，Costmap2D为它们提供存储地图的父类，CostmapLayer为它们提供一些对地图的操作方法。而inflationLayer因为没有维护真正的地图所以只和CostmapLayer一起继承于Layer，Layer提供了操作master map的途径。
LayerdCostmap为Costmap2DROS（用户接口）提供了加载地图层的插件机制，每个插件（即地图层）都是Layer类型的。
Costmap初始化流程
在navigation的主节点move_base中，建立了两个costmap。其中planner_costmap_ros_是用于全局导航的地图，controller_costmap_ros_是用于局部导航用的地图。下图为costmap的初始化流程。



（1）Costmap初始化首先获得全局坐标系和机器人坐标系的转换
（2）加载各个Layer，例如StaticLayer，ObstacleLayer，InflationLayer。
（3）设置机器人的轮廓
（4）实例化了一个Costmap2DPublisher来发布可视化数据。
（5）通过一个movementCB函数不断检测机器人是否在运动
（6）开启动态参数配置服务，服务启动了更新map的线程。
Costmap更新
Costmap的更新在mapUpdateLoop线程中实现，此线程分为两个阶段：
（阶段一）UpdateBounds：这个阶段会更新每个Layer的更新区域，这样在每个运行周期内减少了数据拷贝的操作时间。StaticLayer的Static map只在第一次做更新，Bounds 范围是整张Map的大小，而且在UpdateBounds过程中没有对Static Map层的数据做过任何的更新。ObstacleLayer在这个阶段主要的操作是更新Obstacles Map层的数据，然后更新Bounds。InflationLayer则保持上一次的Bounds。
（阶段二）UpdateCosts：这个阶段将各层数据逐一拷贝到Master Map，可以通过下图观察Master Map的生成流程。（图来源于David Lu的《Layered Costmaps for Context-Sensitive Navigation》）



在（a）中，初始有三个Layer和Master costmap,Static Layer和Obstacles Layer维护它们自己的栅格地图，而inflation Layer并没有。为了更新costmap,算法首先在各层上调用自己的UpdateBounds方法（b）。为了决定新的bounds,Obstacles Layer利用新的传感器数据更新它的costmap。然后每个层轮流用UpdateCosts方法更新Master costmap的某个区域,从Static Layer开始（c），然后是Obstacles Layer(d)，最后是inflation Layer(e)。
下一篇系列文章中继续研究costmap在ROS中的程序架构设计。