文章目录
- [一、rrt_exploration 介绍](#一、rrt_exploration 介绍)
- 二、安装环境
- 三、安装与运行
- 四、配置说明
-
- [1、Robots Network](#1、Robots Network)
- [2、Robot's frame names in tf](#2、Robot's frame names in tf)
- [3、Robot's node and topic names](#3、Robot's node and topic names)
- [4、Setting up the navigation stack on the robots](#4、Setting up the navigation stack on the robots)
- [5、A mapping node](#5、A mapping node)
- [6、A map merging node](#6、A map merging node)
- 五、节点介绍
-
- 1、global_rrt_frontier_detector
-
- [1.1. 参数](#1.1. 参数)
- [1.2. 订阅的主题](#1.2. 订阅的主题)
- [1.3. 发布的主题](#1.3. 发布的主题)
- 2、local_rrt_frontier_detector
-
- [2.1. 参数](#2.1. 参数)
- [2.2. 订阅的主题](#2.2. 订阅的主题)
- [2.3. 发布的主题](#2.3. 发布的主题)
- 3、frontier_opencv_detector
-
- [3.1. 参数](#3.1. 参数)
- [3.2. 订阅的主题](#3.2. 订阅的主题)
- [3.3. 发布的主题](#3.3. 发布的主题)
- 4、filter
-
- [4.1. 参数](#4.1. 参数)
- [4.2. 订阅的主题](#4.2. 订阅的主题)
- [4.3. 发布的主题](#4.3. 发布的主题)
- 5、Assigner
-
- [5.1. 参数](#5.1. 参数)
- [5.2. 订阅的主题](#5.2. 订阅的主题)
- [5.3. 发布的主题](#5.3. 发布的主题)
- 六、分析
一、rrt_exploration 介绍
rrt_exploration 是一个 ROS 软件包,用于实现基于 RRT(rapidly exploring random tree) 的多机器人地图探索算法。它还具有基于图像的边界检测功能,可使用图像处理来提取边界点。
rrt_exploration 是一个 ROS 软件包,为移动机器人实现了一种多机器人地图探索算法。它基于快速探索随机树(RRT)算法。该软件包有 5 个不同的 ROS 节点:
- 全局 RRT 边界点检测器节点(Global RRT frontier point detector node.)
- 局部 RRT 边界点检测器节点(Local RRT frontier point detector node.)
- 过滤器节点(Filter node.)
- 分配器节点(Assigner node.)
- 基于 opencv 的边界点检测器节点(opencv-based frontier detector node.)
1、原理
该包的基本原理是,基于2D的,通常采用图像算法的边缘检测来检测已知区域与未知区域的边界。基于Rapidly-exploring Random Trees的探索策略。由于RRT基本上是朝向未知区域的(unexplored and unvisited),并且RRT可以扩展到更高维区域。同时采用 local tree 与 global tree 来检查边缘点,使得机器人的 exploration 更加高效。
一旦检测到边缘后,就会取其中心为目标点,然后让机器人去探索该点。而为了检测边缘点,需要对整张地图进行处理,而这个操作通常是耗时的,为此大量的研究人员focus在检测 frontier edges 的效率上。
本包中,RRT树只是用于search边缘点,而检测到的边缘点经过滤波就会依次安排给机器人。当机器人接收到 point 时,就会运动到对应的点。在此期间,机器人上的传感器将会扫描建图。
而通过多个独立的RRT树来加速边缘点的检测,则是本包的创新点。
2、主要思想
RRT算法是一种树型算法,它由一个起始点 Xinit 作为树的起始节点(根节点),然后从这个起始点进行随机生长,通过随机采样增加叶子节点 Xnew 的方式,生成一个随机扩展树,当随机树中的叶子节点包含了目标点或进入了目标区域,便从随机树中找到一条由从初始点到目标点的路径。
3、拟解决的问题
- 在一个未知环境中,使机器人快速对环境进行建模。
- 使机器人在一个未知环境中到达目标点。
4、优缺点
(1)优点
- 适合多维空间, 可以使机器人能够快速有效地搜索高维空间,从而实现对目标点的路径规划。
- 原理简单,适用性强,无需预知环境,也可完成对机器人的路径规划。
- 快速提取地图的边界点;
- 双边界点探测器(局部+全局)提高探索效率和灵活性;
(2)缺点
- 路径规划的效果受规划时间、地形复杂程度以及生长步长的影响,而且规划好的路径不一定是最优路径。
- 对于具有狭窄通道和狭窄入口的区域,RRT生长困难,探索效率低;
- 探索过程的后期,RRT生长缓慢。
二、安装环境
该软件包已在 ROS Kinetic 和 ROS Indigo 上进行过测试,在其他发行版(如 Jade)上也能正常运行。安装软件包前需要满足以下要求:
- 已经安装了 ROS 发行版(indigo 或更高版本,推荐使用 indigo 或 kinetic)。
- 创建一个工作区。
- 安装 "gmapping" ROS package:在 Ubuntu 上,如果运行的是 ROS Kinectic,可以在终端键入以下命令:
$ sudo apt-get install ros-kinetic-gmapping
- 安装 ROS navigation stack。使用以下命令即可完成(假设使用 Ubuntu 和 ROS Kinetic):
$ sudo apt-get install ros-kinetic-navigation
- Python 2.7(未在 Python 3 上测试)。
- 应安装以下 Python 模块:
OpenCV (cv2):$ sudo apt-get install python-opencv
Numpy:$ sudo apt-get install python-numpy
Sklearn:$ sudo apt-get install python-scikits-learn
三、安装与运行
1、安装
下载软件包并将其放在工作区的 /src 文件夹中。然后使用 catkin_make 进行编译。
bash
$ git clone https://github.com/hasauino/rrt_exploration.git
编译:
bash
$ catkin_make
2、运行
运行命令:
bash
$ roslaunch rrt_exploration single.launch
注意要开启 move_base
如果使用一个 launch 文件,则该文件中需要包含3个内容:
① rrt_exploration
② move_base
③ gmapping(其他建图算法也可以)
例如:rrt_slam.launch 文件
bash
<launch>
<include file="$(find rrt_exploration)/launch/simple.launch"/>
<include file="$(find slam)/launch/gmapping.launch"/>
<node pkg="rviz" type="rviz" name="rviz"/>
<node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen">
<param name="footprint_padding" value="0.01" />
<param name="controller_frequency" value="5.0" />
<param name="controller_patience" value="3.0" />
<param name="oscillation_timeout" value="30.0" />
<param name="oscillation_distance" value="0.5" />
<param name="planner_patience" value="1" />
<param name="controller_patience" value="1" />
<param name="recovery_behavior_enabled" value="false" />
<rosparam file="$(find slam)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find slam)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find slam)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find slam)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find slam)/config/base_local_planner_params.yaml" command="load" />
<param name="global_costmap/global_frame" value="/map"/>
<param name="global_costmap/robot_base_frame" value="/base_link"/>
<param name="global_costmap/laser_scan_sensor/sensor_frame" value="/base_scan"/>
<param name="global_costmap/laser_scan_sensor/topic" value="/scan"/>
<param name="local_costmap/global_frame" value="/map"/>
<param name="local_costmap/robot_base_frame" value="/base_link"/>
<param name="local_costmap/laser_scan_sensor/sensor_frame" value="/base_scan"/>
<param name="local_costmap/laser_scan_sensor/topic" value="/scan"/>
<param name="local_costmap/obstacle_layer/laser_scan_sensor/topic" value="/scan"/>
<remap from="cmd_vel" to="cmd_vel"/>
</node>
</launch>
注:将 slam 改成自己的文件名称
这里还可能出现一个问题:运行之后发现小车并没有运动,查看/cmd_vel也没有数据,出现这个问题的原因就在于目前 move_base 无法和 rrt_exploraiton 连接。也就是 name="move_base_node" 这一段的问题。
rrt_exploration 作者给的文件 filter.py、functions.py 中是根据 name="move_base_node" 来写代码的,但大部分 ros 都是使用 move_base 这个命名,而作者的源代码采用的是 move_base_node,而且没有在 launch 文件中给出接口来更改 move_base 的命名,因此大部分人在使用 rrt_exploration 的时候会发现move_base 无法和 rrt_exploraiton 连接,导致自主探索功能失败,有两种解决办法:
-
在launch文件中将 move_base 节点命名为 move_base_node。
-
在源代码中修改 move_base_node 名。
(1)functions.py:
/move_base_node/NavfnROS/make_plan 改为 /move_base/NavfnROS/make_plan
(2)filter.py:
'/move_base_node/global_costmap/costmap' 改为 '/move_base/global_costmap/costmap'
改好后重新编译运行就可以了。
运行成功的节点图:
四、配置说明
本软件包为单个或多个机器人提供了探索策略。不过,要让它发挥作用,必须使用 navigation stack 设置好机器人。此外,还必须按照以下步骤设置和准备机器人。
注:如果您想快速运行并测试该程序包,可以尝试使用 rrt_exploration_tutorials 程序包,它提供了单个和多个机器人的 Gazebo 仿真,您可以直接使用它来运行该程序包。
1、Robots Network
对于多机器人配置,该软件包不需要特殊的网络配置,只需有一个 ROS 主控器(可以是其中一个机器人)即可工作。因此,在其他机器人上,ROS_MASTER_URI
参数应指向主控器的地址。有关在多台机器上设置 ROS 的更多信息,请点击此链接。
2、Robot's frame names in tf
所有机器人框架都应以名称为前缀。机器人的命名从"/robot_1"、"/robot_2"、"/robot_3 "开始,以此类推。即使是为单个机器人使用软件包,机器人的框架也应该以其名称为前缀(即 /robot_1)。因此,对于 robot_1,tf 树中的帧应如下所示:
3、Robot's node and topic names
机器人上运行的所有节点和主题也必须以其名称为前缀。对于 robot 1,节点名称应如下所示: /robot_1/move_base_node
、/robot_1/slam_gmapping
。
主题名称应为: /robot_1/odom
, /robot_1/map
, /robot_1/base_scan
, ...等等。
4、Setting up the navigation stack on the robots
必须运行 move_base_node
节点,该节点用于在机器人上建立 navigation stack。这个软件包(rrt_exploration)会生成目标探索点,每个机器人都必须能够接收这些点并向其移动。这就是需要 navigation stack 的原因。此外,每个机器人都必须有一个全局和局部成本地图。所有这些都由 move_base_node
生成。
5、A mapping node
每个机器人都应该有一个由 gmapping 软件包生成的本地地图。
6、A map merging node
在多机器人情况下,应该有一个节点能将所有本地地图合并成一个全局地图。您可以使用这个软件包。
五、节点介绍
rrt_exploration 共有 3 种模块:
- 基于RRT的边界检测模块(负责检测边界点)
- 滤波模块(存储边界点,并通过mean shift算法来聚类,检测出无效以及旧的边界点)
- task allocator模块(接收到滤波模块传来的边界点后,分配到机器人)
本包还需要与 SLAM 及 path planning 模块相结合来使用
下图显示了 rrt_exploration 模块结构:
1、global_rrt_frontier_detector
global_rrt_frontier_detector
节点接收一个占用网格,并在其中寻找边界点(即探索目标)。它发布检测到的点,以便过滤节点进行处理。在多机器人配置中,该节点只运行一个实例。
如果需要,运行额外的全局边界探测器实例可以提高边界点的探测速度。
1.1. 参数
~map_topic
(字符串,默认为 "/robot_1/map"): 该参数定义节点接收地图的主题名称。~eta
(浮点数,默认值:0.5): 该参数控制用于检测边界点的 RRT 增长率,单位为米。该参数应根据地图的大小来设置,数值越大,树的生长速度越快,因此检测边界点的速度也越快,但生长速度过大也意味着树会遗漏地图中的小角落。
1.2. 订阅的主题
map
(主题名称由~map_topic
参数定义)(nav_msgs/OccupancyGrid)clicked_point
(geometry_msgs/PointStamped Message):global_rrt_frontier_detector
节点需要定义要探索的区域。本主题是节点接收定义区域的五个点的地方。前四个点定义了要探索的正方形区域(四个点需要能构成封闭图形,顺逆时针发布都行),最后一个点是树的起点。在此主题上发布这五个点后,RRT 将开始检测边界点。这五个点可以通过 Rviz 上的 Publish Point 按钮发布。
1.3. 发布的主题
detected_points
(geometry_msgs/PointStamped Message): 节点发布检测到的边界点的主题。~shapes
(visualization_msgs/Marker Message): 节点在此主题上发布线形,以便使用 Rviz 可视化 RRT。
2、local_rrt_frontier_detector
该节点与 global_rrt_frontier_detector 类似。但是它的工作方式有所不同,因为每次检测到一个边界点时,这里的树都会不断重置。该节点与 global_rrt_frontier_detector 节点同时运行,负责快速检测机器人附近的边界点。
在多机器人配置中,每个机器人运行一个 local_rrt_frontier_detector 实例。因此,对于一个由 3 个机器人组成的团队,将有 4 个节点用于检测边界点;3 个本地检测器和 1 个全局检测器。如果需要,运行额外的本地边界探测器实例可以提高边界点的检测速度。
所有检测器都将在同一主题(/detected_points)上发布检测到的边界点。
2.1. 参数
~robot_frame
(字符串,默认为 "/robot_1/base_link"):与机器人相连的框架。每次树重置时,都会从该帧获取的当前机器人位置开始。~map_topic
(字符串,默认值:"/robot_1/map"): 该参数定义节点接收地图的主题名称。~eta
(浮点数,默认值:0.5): 该参数控制本地 RRT 的增长率。
2.2. 订阅的主题
map
(主题名称由 ~map_topic 参数定义)(nav_msgs/OccupancyGrid)。clicked_point
(geometry_msgs/PointStamped Message): 与 global_rrt_frontier_detector 类似,lobal_rrt_frontier_detector 也会订阅此主题。
2.3. 发布的主题
detected_points
(geometry_msgs/PointStamped Message): 节点发布检测到的边界点的主题。~shapes
(visualization_msgs/Marker Message): 节点在此主题上发布线形,以便使用 Rviz 可视化 RRT。
3、frontier_opencv_detector
该节点是另一个边界检测器,但不是基于 RRT。该节点使用 OpenCV 工具检测边界点。它可以单独运行,在多机器人配置中只需运行一个实例(运行该节点的其他实例不会产生任何影响)。
最初实施该节点是为了与基于 RRT 的边界点检测器进行比较。在运行 RRT 探测器(局部和全局)的同时运行该节点,可以提高边界点的探测速度。
注意:您可以运行任何类型和数量的探测器,所有探测器都将发布在过滤器节点(将在下一节中解释)订阅的同一主题上。另一方面,过滤器将把过滤后的边界点传递给分配器,以便命令机器人探索这些点。
3.1. 参数
~map_topic
(字符串,默认为"/robot_1/map"): 该参数定义了节点接收地图的主题名称。
3.2. 订阅的主题
map
(主题名称由 ~map_topic 参数定义) (nav_msgs/OccupancyGrid)
3.3. 发布的主题
detected_points
(geometry_msgs/PointStamped Message): 节点发布检测到的边界点的主题。shapes
(visualization_msgs/Marker Message): 节点在此主题上发布检测到的点,以便使用 Rviz 可视化。
4、filter
过滤节点从所有探测器接收检测到的边界点,对这些点进行过滤,然后将它们传递给分配节点,由分配节点指挥机器人。过滤包括删除旧点和无效点,以及删除冗余点。
4.1. 参数
~map_topic
(字符串,默认为"/robot_1/map"): 该参数定义了节点将接收地图的主题名称。地图用于了解哪些点不再是边界点(旧点)。~costmap_clearing_threshold
(浮点数,默认值:70.0): 占用率值大于此阈值的任何边界点都将被视为无效点。占用率值从成本地图中获取。~info_radius
(float, 默认值:1.0): 用于计算边界点信息增益的信息半径。~goals_topic
(字符串,默认值:"/detected_points"):定义节点接收边界点检测结果的主题。~n_robots
(浮点数,默认值:1.0): 机器人数量~rate
(float,默认:100):节点循环速率(单位:Hz)。
4.2. 订阅的主题
map
(主题名称由 ~map_topic 参数定义)(nav_msgs/OccupancyGrid)。robot_x/move_base_node/global_costmap/costmap
(nav_msgs/OccupancyGrid):其中 x(在 robot_x 中)指机器人编号。
过滤器节点订阅所有机器人的所有成本计算主题,因此需要成本计算。通常情况下,成本图应由 navigation stack 发布(在机器人上调出 navigation stack 后,每个机器人都会有一个成本图)。例如,如果 n_robots=2
,节点将订阅:robot_1/move_base_node/global_costmap/costmap
和 robot_2/move_base_node/global_costmap/costmap
。代价图用于删除无效点。
注意:与机器人相对应的所有节点的命名空间都应以 robot_x 开头。x 代表机器人编号。
goals topic
(主题名称由 ~goals_topic 参数定义)(geometry_msgs/PointStamped Message): 过滤节点接收检测到的边界点的主题。
4.3. 发布的主题
frontiers
(visualization_msgs/Marker Message): 过滤节点将接收到的边界点发布到 Rviz 上进行可视化的主题。centroids
(visualization_msgs/Marker Message):过滤节点将接收到的边界点发布到 Rviz 上进行可视化的主题: 过滤节点仅将过滤后的边界点发布到 Rviz 上进行可视化的主题。filtered_points
(PointArray): 所有过滤后的点都会以点数组的形式发送到此主题上的分配器节点。
5、Assigner
该节点接收目标探索目标,即过滤器节点发布的经过过滤的边界点,并据此对机器人发出指令。分配器节点通过 move_base_node
命令机器人。这就是在机器人上设置 navigation stack 的原因。
5.1. 参数
~map_topic
(字符串,默认为"/robot_1/map"): 该参数定义了节点接收地图的主题名称。在单机器人情况下,该主题应设置为机器人的地图主题。在多机器人情况下,该主题必须设置为全局合并地图。~info_radius
(float, 默认值: 1.0): 用于计算边界点信息增益的信息半径。~info_multiplier
(float, 默认值:3.0):单位为米。该参数用于重视边界点的信息增益,而不是成本(到边界点的预期旅行距离)。~hysteresis_radius
(float, 默认值:3.0): 单位为米。该参数定义滞后半径。~hysteresis_gain
(float, 默认值:2.0):滞后半径: 单位为米。该参数定义磁滞增益。~frontiers_topic
(字符串,默认值:"/filtered_points"): 分配器节点接收过滤边界点的主题。~n_robots
(浮点数,默认值:1.0): 机器人数量。~delay_after_assignement
(float, 默认值:0.5):单位为秒。它定义了每次机器人分配后的延迟时间。~rate
(float,默认值:100):节点循环速率(单位 Hz)。
5.2. 订阅的主题
map
(主题名称由 ~map_topic 参数定义)(nav_msgs/OccupancyGrid)。Filtered frontier points topic
(主题名称由 ~frontiers_topic 参数定义)(PointArray)。
5.3. 发布的主题
分配节点不会发布任何信息。它使用 actionlib 将指定的点发送到 move_base_node
(指定节点是 move_base_node actionlib 服务器的 actionlib 客户端)。
六、分析
关于 global 与 local detector:
当检测到一个边界点时,local detector 会 reset,并且会基于机器人当前的位置重新生长。这带来两个好处:
1、检测边界点更快。因为机器人总是会向着边界点移动,而如果RRT树从机器人当前的位置开始,那么它到达未知区域的概率更高。
2、机器人可能会miss掉一些地图上边角的小的边界点,而为了解决这个问题,就通过global detector。但随着地图的变大,global detector会越来越慢(或者说,随着树的变大,exploration会变慢),为此需要local detector
关于assigner,其基于以下几个要素来分配机器人导航的目标点:
1、Navigation cost
2、Information gain
参考:
- https://github.com/hasauino/rrt_exploration
- http://wiki.ros.org/rrt_exploration
- 论文:Autonomous robotic exploration based on multiple rapidly-exploring randomized trees
- ROS仿真笔记之------基于rrt_exploration的单个机器人自主探索建图
- http://wiki.ros.org/rrt_exploration/Tutorials/singleRobot
- 基于rrt_exploration实现自主建图(ros)
- ROS移动机器人基于RRT(快速探索随机树)算法 rrt_exploration实现真实机器人自主探索建图