【自主探索】基于 rrt_exploration 的单个机器人自主探索建图

一、rrt_exploration 介绍

rrt_exploration 是一个 ROS 软件包，用于实现基于 RRT（rapidly exploring random tree） 的多机器人地图探索算法。它还具有基于图像的边界检测功能，可使用图像处理来提取边界点。

rrt_exploration 是一个 ROS 软件包，为移动机器人实现了一种多机器人地图探索算法。它基于快速探索随机树（RRT）算法。该软件包有 5 个不同的 ROS 节点：

1. 全局 RRT 边界点检测器节点（Global RRT frontier point detector node.）
2. 局部 RRT 边界点检测器节点（Local RRT frontier point detector node.）
3. 过滤器节点（Filter node.）
4. 分配器节点（Assigner node.）
5. 基于 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、拟解决的问题

1. 在一个未知环境中，使机器人快速对环境进行建模。
2. 使机器人在一个未知环境中到达目标点。

4、优缺点

（1）优点

• 适合多维空间， 可以使机器人能够快速有效地搜索高维空间，从而实现对目标点的路径规划。
• 原理简单，适用性强，无需预知环境，也可完成对机器人的路径规划。
• 快速提取地图的边界点；
• 双边界点探测器（局部+全局）提高探索效率和灵活性；

（2）缺点

• 路径规划的效果受规划时间、地形复杂程度以及生长步长的影响，而且规划好的路径不一定是最优路径。
• 对于具有狭窄通道和狭窄入口的区域，RRT生长困难，探索效率低；
• 探索过程的后期，RRT生长缓慢。

二、安装环境

该软件包已在 ROS Kinetic 和 ROS Indigo 上进行过测试，在其他发行版（如 Jade）上也能正常运行。安装软件包前需要满足以下要求：

1. 已经安装了 ROS 发行版（indigo 或更高版本，推荐使用 indigo 或 kinetic）。
2. 创建一个工作区。
3. 安装 “gmapping” ROS package：在 Ubuntu 上，如果运行的是 ROS Kinectic，可以在终端键入以下命令：$ sudo apt-get install ros-kinetic-gmapping
4. 安装 ROS navigation stack。使用以下命令即可完成（假设使用 Ubuntu 和 ROS Kinetic）：$ sudo apt-get install ros-kinetic-navigation
5. Python 2.7（未在 Python 3 上测试）。
6. 应安装以下 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 进行编译。

$ git clone https://github.com/hasauino/rrt_exploration.git

编译：

$ catkin_make

2、运行

运行命令：

$ roslaunch rrt_exploration single.launch 

注意要开启 move_base

如果使用一个 launch 文件，则该文件中需要包含3个内容：
① rrt_exploration
② move_base
③ gmapping（其他建图算法也可以）

例如：rrt_slam.launch 文件

<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 连接，导致自主探索功能失败，有两种解决办法：

1. 在launch文件中将 move_base 节点命名为 move_base_node。

2. 在源代码中修改 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

参考：

1. https://github.com/hasauino/rrt_exploration
2. http://wiki.ros.org/rrt_exploration
3. 论文：Autonomous robotic exploration based on multiple rapidly-exploring randomized trees
4. ROS仿真笔记之——基于rrt_exploration的单个机器人自主探索建图
5. http://wiki.ros.org/rrt_exploration/Tutorials/singleRobot
6. 基于rrt_exploration实现自主建图（ros）
7. ROS移动机器人基于RRT（快速探索随机树）算法 rrt_exploration实现真实机器人自主探索建图