激光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中的程序架构设计。

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

激光SLAM导航系列(二)SLAM与导航系统框架
激光SLAM系统框架在激光SLAM系统中,Gmapping获取扫描的激光雷达信息以及里程计数据可动态的生成2D栅格地图.导航包则利用这个栅格地图,里程计数据和激光雷达数据做出适合的路径规划和定位,最 ...
激光SLAM导航系列(五)局部路径规划
局部路径规划局部路径规划简介机器人在获得目的地信息后,首先经过全局路径规划规划出一条大致可行的路线,然后调用局部路径规划器根据这条路线及costmap的信息规划出机器人在局部时做出具体行动策略,R ...
costmap代价地图
0x00 什么是costmap代价地图在机器人进行路径规划时,我们需要明白规划算法是依靠什么在地图上来计算出来一条路径的.依靠的是gmapping扫描构建的一张环境全局地图,但是仅仅依靠一张原始的全 ...
cesium 学习笔记（三）在地图上放置3D建筑模型
Cesium的学习搁置了两天,今天准备来研究一下在地图上放置3D建筑模型. 为什么要加载3D建筑模型呢?当然是因为项目没有数据啦!对于有条件倾斜摄影无人机建模,或者谷歌地球上直接有模型的,就不用这么苦 ...
plotly系列| 使用plotly在地图上绘制散点图和密度图
目录 Plotly简介在地图上制作散点图和密度图 1 . 在python中使用pip命令安装 2 .导入包 3 .读取文件 4 . 在地图上绘制散点图 5 . 绘制密度图,其属性与绘制散点图相似关 ...
周末ROS学习沙龙第一期——ROS历史、安装、消息话题节点服务等概念、SLAM导航框架及参数、小车上运行SLAM
非博主原创,出于方便学习的目的,将周末ROS学习沙龙www.corvin.cn的课堂讲义PPT整理在这(老师讲得超棒!),无任何盈利目的,若有侵权则删除. Ros小课堂链接:https://space ...
激光slam课程学习笔记－－第11课：3D激光SLAM介绍
前言:这系列笔记是学习曾书格老师的激光slam课程所得,这里分享只是个人理解,有误之处,望大佬们赐教.这节课主要介绍一些3d slam数学知识,以及典型代表loam. 1. 3d激光SLAM 1.1 ...
ROS导航小车0 代价地图（仅作个人记录）
前言首先了解在导航堆中,move_base包与其它包(如amcl.map_server)的关系,如图所示发布导航命令 rostopic pub /move_base_simple/goal geo ...
机器人学习--网友资料系列激光SLAM建图、粒子滤波定位和位姿图优化
一.移动机器人自主导航的前提是在未知环境中先构建地图 (目前市内很多用的2D激光雷达,构建栅格地图,相当于立体空间中的某个水平面高度的切面) 一般用的是2D 激光SLAM算法构建概率栅格占用地图: ...

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

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

最新文章

热门文章