目录
- 0 专栏介绍
- 1 代价地图介绍
- 1.1 基本概念
- 1.2 代价定义
- 2 代价地图配置
- 2.1 通用配置
- 2.2 障碍层配置
- 2.3 静态层配置
- 2.4 膨胀层配置
- 3 代价地图插件案例
- 3.1 构造地图插件类
- 3.2 注册并导出插件
- 3.3 编译与使用插件
0 专栏介绍
本专栏旨在通过对ROS2的系统学习,掌握ROS2底层基本分布式原理,并具有机器人建模和应用ROS2进行实际项目的开发和调试的工程能力。
🚀详情:《ROS2从入门到精通》
1 代价地图介绍
1.1 基本概念
机器人导航必须依赖于地图,而SLAM构建的地图为静态地图,在导航中一般不可以直接使用,因为导航过程中障碍信息是可变的,因此地图信息需要时时更新。
代价地图就是ROS定义的用于动态导航的地图数据结构,其在静态地图基础上添加了一些辅助信息,主要分为以下图层:
- 静态地图层(Static Map Layer):通常是由SLAM建立的静态地图
- 障碍地图层(Obstacle Map Layer):用于动态记录传感器感知到的障碍信息
- 膨胀层(Inflation Layer):在以上两层地图基础上进行障碍膨胀,主要目的是防止机器人靠近障碍物边缘时,因惯性、不规则形体等原因与障碍产生碰撞,因此需要让机器人充分远离障碍物
- 其他图层(Other Layers):可以通过插件形式自定义代价地图层
1.2 代价定义
关于代价地图代价的定义,摘录官方说明:如图所示,横轴是距离机器人中心的距离,纵轴是代价地图中栅格的灰度值,灰度越高代价越大
- 致命障碍:栅格值
254
,此时障碍物与机器人中心重叠,必然发生碰撞 - 内切障碍:栅格值
253
,此时障碍物处于机器人的内切圆内,必然发生碰撞 - 外切障碍:栅格值
[128,252]
,此时障碍物处于其机器人的外切圆内,处于碰撞临界,不一定发生碰撞 - 非自由空间:栅格值
(0,127]
,此时机器人处于障碍物附近,属于危险警戒区,进入此区域,将来可能会发生碰撞 - 自由区域:栅格值
0
,此处机器人可以自由通过 - 未知区域:栅格值
255
,未探明是否有障碍物
看一个实例:其中紫色区域为致命障碍;浅蓝色区域为内切障碍;红色区域为外切障碍与非自由障碍空间,颜色越深代价越高;蓝色为自由空间。
2 代价地图配置
2.1 通用配置
通用配置如下所示:
ros__parameters:update_frequency: 1.0publish_frequency: 1.0global_frame: maprobot_base_frame: base_linkuse_sim_time: Truerobot_radius: 0.22resolution: 0.05track_unknown_space: trueplugins: ["static_layer", "obstacle_layer", "inflation_layer"]
global_frame
:在全局/局部代价地图中的全局坐标系,一般全局设置为地图坐标系map
,局部设置为里程计坐标系odom
robot_base_frame
:机器人基坐标系通过global_frame
和robot_base_frame
就可以计算两个坐标系间的变换,得知机器人在代价地图位置坐标update_frequency
:代价地图的更新频率publish_frequency
:代价地图的发布频率robot_radius
:机器人等效半径,单位是米
2.2 障碍层配置
obstacle_layer:plugin: "nav2_costmap_2d::ObstacleLayer"observation_sources: scanscan:topic: /scanmax_obstacle_height: 2.0clearing: Truemarking: Truedata_type: "LaserScan"raytrace_max_range: 3.0raytrace_min_range: 0.0obstacle_max_range: 2.5obstacle_min_range: 0.0
observation_sources
:设置导航中所使用的传感器,例如激光雷达、碰撞传感器、超声波传感器等。每个传感器需要进行配置:sensor_frame
:传感器坐标系名称data_type
:传感器数据类型topic
:传感器发布的话题名marking
:是否可使用该传感器来标记障碍物clearing
:是否可使用该传感器来清除障碍物标记
obstacle_range
:设置机器人检测障碍物的最大范围,只有进入该范围内才把该障碍物当作影响路径规划和移动的障碍物
2.3 静态层配置
static_layer:plugin: "nav2_costmap_2d::StaticLayer"map_subscribe_transient_local: True
2.4 膨胀层配置
inflation_layer:plugin: "nav2_costmap_2d::InflationLayer"cost_scaling_factor: 3.0inflation_radius: 0.55
-
inflation_radius
:膨胀半径,膨胀层会把障碍物代价膨胀直到该半径为止,一般将该值设置为机器人底盘的直径大小。如果机器人经常撞到障碍物就需要增大该值,若经常无法通过狭窄地方就减小该值,代价膨胀公式:exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (INSCRIBED_INFLATED_OBSTACLE - 1)
其中,
distance_from_obstacle - inscribed_radius
是机器人到实际障碍物与内切圆半径之差(且该差值小于膨胀半径),INSCRIBED_INFLATED_OBSTACLE
设定为254 -
cost_scaling_factor
:膨胀过程代价比例系数,增大比例因子会降低代价
3 代价地图插件案例
然而,这些默认的图层可能并不能满足实际需要。举例而言,社交地图层(Social Costmap Layer)用于机器人在导航过程中考虑与人类之间的社交交互,这个层级允许机器人更加智能地导航,并遵循一些社交规则,以更好地与人类共享空间,避免产生不适或危险的行为;禁区地图层(Prohibition Costmap Layer)用于标记和表示机器人不能进入的区域。这个层级的作用是在机器人的导航过程中限制其进入特定的区域(例如高压区、悬崖边缘、深水区等),从而确保机器人在导航时遵守特定的规则和限制,防止可能的事故和损坏。
3.1 构造地图插件类
所有全局规划插件的基类是nav2_costmap_2d::Layer
,该基类提供了7个纯虚方法来实现控制器插件,一个合法的控制插件必须覆盖这7个基本方法:
onInitialize()
非必须覆盖:在插件初始化结束时调用。通常会在方法里声明ROS参数。任何需要初始化的操作都应该在该方法里完成。updateBounds()
必须覆盖:更新costmap层边界updateCosts()
必须覆盖:每次需要重新计算costmap时都会调用方法。它仅在其边界窗口内更新costmap层matchSize()
非必须覆盖:在每次更改地图大小时调用更新地图尺寸onFootprintChanged()
非必须覆盖:在每次更新机器人footprint位置时调用reset()
必须覆盖:重置costmap层isClearable()
必须覆盖:是否需要在该costmap层执行清除操作
按照上述标准,本文案例中ESDF地图插件的基本成员函数和变量如下所示
class DistanceLayer : public Layer
{
public:DistanceLayer() = default;virtual ~DistanceLayer() = default;void onInitialize() override;void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x,double* max_y) override;void updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;void reset() override;bool isClearable() override;
};
3.2 注册并导出插件
在创建了自定义地图插件的前提下,需要导出该控制器插件以便地图服务器可以在运行时正确地加载。在ROS2中,插件的导出和加载由pluginlib
处理。
-
源文件配置导出宏
#include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::DistanceLayer, nav2_costmap_2d::Layer)
-
配置插件描述文件
xxx_costmap_plugin.xml
,例如本案例为distance_layer_costmap_plugin.xml
文件。此XML文件包含以下信息:library path
:插件库名称及其位置;class name
:地图算法类的名称;class type
:地图算法类的类型;base class
:地图层基类的名称,统一为nav2_costmap_2d::Layer
description
:插件的描述。
实例如下
<library path="esdf_plugin"><class name="nav2_costmap_2d/DistanceLayer" type="nav2_costmap_2d::DistanceLayer" base_class_type="nav2_costmap_2d::Layer"><description>This is a nav2 distance layer plugin.</description></class> </library>
-
配置
CMakeLists.txt
文件
使用cmake
函数pluginlib_export_plugin_description_file()
来导出插件。这个函数会将插件描述文件安装到install/share
目录中,并设置ament
索引以使其可被发现,实例如下pluginlib_export_plugin_description_file(nav2_costmap_2d distance_layer_costmap_plugin.xml)
-
配置
package.xml
描述文件,实例如下:<export><build_type>ament_cmake</build_type><nav2_core plugin="${prefix}/distance_layer_costmap_plugin.xml" /> </export>
3.3 编译与使用插件
编译该插件软件包,接着通过配置文件使用插件。
参数的传递链如下:首先在simulation.launch.py
中引用配置文件navigation.yaml
declare_params_file_cmd = DeclareLaunchArgument('params_file',default_value=os.path.join(simulation_dir, 'config', 'navigation.yaml'),description='Full path to the ROS2 parameters file to use for all launched nodes')
接着在navigation.yaml
中修改插件配置,默认如下,是用的是静态层、障碍层和膨胀层插件:
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]obstacle_layer:plugin: "nav2_costmap_2d::ObstacleLayer"...static_layer:plugin: "nav2_costmap_2d::StaticLayer"...inflation_layer:plugin: "nav2_costmap_2d::InflationLayer"...
将上述替换为自己的插件,本案例为:
plugins: ["static_layer", "obstacle_layer", "inflation_layer", "distance_layer"]distance_layer:plugin: "nav2_costmap_2d/DistanceLayer"...obstacle_layer:plugin: "nav2_costmap_2d::ObstacleLayer"...static_layer:plugin: "nav2_costmap_2d::StaticLayer"...inflation_layer:plugin: "nav2_costmap_2d::InflationLayer"...
接着运行即可看到距离层地图被发布
完整代码通过下方博主名片联系获取
🔥 更多精彩专栏:
- 《ROS从入门到精通》
- 《Pytorch深度学习实战》
- 《机器学习强基计划》
- 《运动规划实战精讲》
- …