您的位置:首页 > 其它

激光SLAM导航系列(三)Costmap(代价地图)(下)

2017-05-06 16:17 295 查看
costmap程序架构

接上一期,通过阅读indigo版本的costmap源码包,简单整理了下costmap的程序架构。



在move_base刚启动时就建立了两个costmap,而这两个costmap都加载了三个Layer插件,它们的初始化过程如上图所示。

StaticLayer主要为处理gmapping或者amcl等产生的静态地图。

ObstacLayer主要处理机器人移动过程中产生的障碍物信息。

InflationLayer主要处理机器人导航地图上的障碍物信息膨胀(让地图上的障碍物比实际障碍物的大小更大一些),尽可能使机器人更安全的移动。

costmap在mapUpdateLoop线程中执行更新地图的操作,每个层的工作流程如下:

(1)StaticLayer工作流程



上图是StaticLayer的工作流程,updateBounds阶段将更新的界限设置为整张地图,updateCosts阶段根据rolling参数(是否采用滚动窗口)设置的值,如果是,那静态地图会随着机器人移动而移动,则首先要获取静态地图坐标系到全局坐标系的转换,再更新静态地图层到master map里。

(2)ObstacleLayer工作流程



上图是ObstacleLayer的工作流程,updateBounds阶段将获取传感器传来的障碍物信息经过处理后放入一个观察队列中,updateCosts阶段则将障碍物的信息更新到master map。

(3)inflationLayer工作流程



上图是inflationLayer的工作流程,updateBounds阶段由于本层没有维护的map,所以维持上一层地图调用的Bounds值(处理区域)。updateCosts阶段用了一个CellData结构存储master map中每个grid点的信息,其中包括这个点的二维索引和这个点附近最近的障碍物的二维索引。改变每个障碍物CELL附近前后左右四个CELL的cost值,更新到master map就完成了障碍物的膨胀。
内容来自用户分享和网络整理,不保证内容的准确性,如有侵权内容,可联系管理员处理 点击这里给我发消息
标签: