• 【SLAM】基于rrt_explore的移动机器人自主建图

    系列文章目录

    ·【SLAM】基于explore_lite的移动机器人自主建图

    ·【SLAM】基于rrt_explore的移动机器人自主建图

    ·【问题解决】rrt_exploration功能包使用过程中报错处理

    前言

    前面我们尝试了基于explore_lite的自主建图，本篇文章我们对基于rrt的自主建图算法进行探索尝试，并对其原理进行简述，然后进行在ubuntu16.04和ubuntu20.04虚拟机上都进行仿真实验，指出需要进行修改的地方以便于我们仿真通过。

    一、原理

    这个功能包是基于2D的，通常采用图像算法的边缘检测来检测已知区域与未知区域的边界。基于Rapidly-exploring Random Trees（快速搜索随机树RRT）的探索策略。由于RRT基本上是朝向未知区域的（unexplored and unvisited），并且RRT可以扩展到更高维区域。同时采用local tree与global tree来检查边缘点，使得机器人的exploration更加高效。

    一旦检测到边缘后，就会取其中心为目标点，然后让机器人去探索该点。而为了检测边缘点，需要对整张地图进行处理，而这个操作通常是耗时的，为此大量的研究人员花费精力在检测frontier edges的效率上。

    本包中，RRT树只是用于search边缘点，而检测到的边缘点经过滤波就会依次安排给机器人。当机器人接收到point时，就会运动到对应的点。在此期间，机器人上的传感器将会扫描建图。

    而通过多个独立的RRT树来加速边缘点的检测，则是本包的创新点。

    如下图所示，这个功能包主要分为三个模块。

    • 基于RRT的边界检测模块（负责检测边界点，分为全局检测节点和局部检测节点以及基于opencv的边界检测器节点）
    • 滤波模块（存储边界点，并通过mean shift算法来聚类，检测出无效以及旧的边界点）
    • 以及task allocator模块（接收到滤波模块传来的边界点后，分配到机器人）

    而本包还需要与SLAM及path planning模块(move_base)相结合来使用。
     

    关于global detect与local detector节点，当检测到一个边界点时，local detector会重置，并且会基于机器人当前的位置重新生长。这带来两个好处：

    1. 检测边界点更快。因为机器人总是会向着边界点移动，而如果RRT树从机器人当前的位置开始，那么它到达未知区域的概率更高。
    2. 机器人可能会miss掉一些地图上边角的小的边界点，而为了解决这个问题，就通过global detector，global detect会检测机器人附近的边界点用来弥补global detect的不足。

    但随着地图的变大，global detector会越来越慢（或者说，随着树的变大，exploration会变慢），为此需要local detector进行加速。

     frontier_opencv_detector节点是另一个边界检测器，但它不是基于 RRT。该节点使用 OpenCV 工具来检测边界点。它旨在单独运行，并且在多机器人配置中只应运行一个实例（运行此节点的其他实例没有任何区别，也就是说opencv是选装，不装照样能跑功能包）。
    最初，这个节点是为了与基于 RRT 的边界检测器进行比较而实现的。在 RRT 检测器（本地和全局）旁边运行此节点可以提高 frotiner 点检测的速度。
    注意：您可以运行任何类型和任意数量的检测器，所有检测器都将发布在过滤器节点订阅的同一主题上。

    过滤器节点从所有检测器接收检测到的边界点，过滤这些点，并将它们传递给分配器节点以指挥机器人。过滤包括旧点和无效点的剔除，也包括多余点的剔除。

    分配节点接收目标并探索目标，即过滤节点发布的过滤边界点，并据此指挥机器人向过滤后的边界点移动。分配者节点通过 move_base_node 命令机器人。

    二、安装

    进入到你的工作空间的src目录中，运行以下代码：

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

    或进入这个网址，手动下载

    然后下载相关的依赖：

    OpenCV(cv2)

    sudo apt-get install python-opencv

    gmapping

    sudo apt-get install ros- kinetic - gmapping

    navigation

    sudo apt-get install ros- kinetic - navigation

    Sklearn

    sudo apt-get install python-scikits-learn

    Numpy

    sudo apt-get install python-numpy

     如果Numpy和SKlearn下载不了可以使用pip下载

    1. pip install numpy
    2. pip install scikits-learn

    三、使用说明

    1.global_rrt_frontier_detector

    订阅的话题：全局识别器订阅地图数据和clicked_point 数据

    • 地图数据：大家都知道就是slam节点发布的map话题
    • clicked_point 数据是rrt_explore特定的，可以由用户设定要探索的区域，该区域由rviz给出的五个点规定。前四个点是四个定义要探索的方形区域，最后一个点是树的起点。

    发布的话题：全局识别器发布检测到的点和形状

    • 检测到的点：是机器人下一个需要前往的点
    • 形状：是一些线条用来可视化RRT。

    参数：

    • map_topic（字符串，默认：“/robot_1/map”）此参数定义节点将在其上接收地图的主题名称
    • eta ( float , 默认值: 0.5)该参数控制用于边界点检测的RRT的增长率，单位为米。这个参数应该根据地图大小来设置，一个非常大的值会导致树生长得更快，从而更快地检测到边界点，但是一个大的增长率也意味着树会丢失地图中的小角落（大概意思是分辨率）。

    2.local_rrt_frontier_detector

    订阅的话题：局部识别器和全局识别器基本相同，是订阅地图数据和clicked_point 数据。

    • 地图数据大家是slam节点发布的map话题
    • clicked_point 数据是可以由用户设定要探索的区域，该区域由rviz给出的五个点规定。前四个点是四个定义要探索的方形区域，最后一个点是树的起点。

    发布的话题：全局识别器发布检测到的点和形状

    • 检测到的点：是机器人下一个需要前往的点
    • 形状：是一些线条用来可视化RRT。

    参数：

    • map_topic（字符串，默认：“/robot_1/map”）此参数定义节点将在其上接收地图的主题名称
    • eta ( float , 默认值: 0.5)该参数控制用于边界点检测的RRT的增长率，单位为米。这个参数应该根据地图大小来设置，一个非常大的值会导致树生长得更快，从而更快地检测到边界点，但是一个大的增长率也意味着树会丢失地图中的小角落（大概意思是分辨率）。
    • /robot_1/base_link（字符串，默认：“/robot_1/base_link”）连接到机器人的底盘位置。每次树重置时，都会从从该帧获得的当前机器人位置开始。

    3.frontier_opencv_detector

    订阅的话题：opencv识别器只订阅地图数据

    • 地图数据：是slam节点发布的map话题。

    发布的话题：全局识别器发布检测到的点和形状。

    • 检测到的点：是机器人下一个需要前往的点
    • 形状：是一些线条用来可视化RRT。

    参数：

    • map_topic（字符串，默认：“/robot_1/map”）此参数定义节点将在其上接收地图的主题名称

    4.filter

    订阅的话题：过滤器订阅地图数据、robot_x/move_base_node/global_costmap/costmap和目标主题

    • 地图数据就是slam节点发布的map话题
    • robot_x/move_base_node/global_costmap/costmap：是由导航堆栈发布的代价地图，用来删除无效点
    • 目标主题：名称由 goals_topic 参数定义，这是过滤器节点接收检测到的边界点的主题。

    发布的话题：过滤器发布前沿、质心和filters_points

    • 前沿：过滤器节点发布接收到的边界点以在 Rviz 上进行可视化的主题
    • 质心：过滤器节点仅发布过滤的边界点以在 Rviz 上进行可视化的主题
    • filters_points：所有过滤的点都作为点数组发送到该主题，然后发送给分配者节点

    参数：

    • map_topic（字符串，默认：“/robot_1/map”）此参数定义节点将在其上接收地图的主题名称，该地图用于知道哪些点不再是边界点（旧点）
    • costmap_clearing_threshold ( float , 默认值: 70.0)任何占用率大于此阈值的边界点（墙）都将被视为无效。占用值是从代价地图中获得的。
    • info_radius ( float , 默认值: 1.0)用于计算边界点信息增益的信息半径。
    • goals_topic（字符串，默认值：“/detected_points”）定义节点接收检测到的边界点的主题。
    • n_robots（float，默认值：1.0）机器人数量。
    • 速率（浮动，默认值：100.0）
    • 节点循环速率（Hz）。

    下面两个主题都是使用多机器人联合建图时用到，单机器人保持默认值即可

    • 命名空间（字符串，默认值：）命名空间用于为机器人 tf 框架名称和主题名称添加前缀。通常，对于单个机器人案例，它是一个空字符串。但是，在多机器人的情况下，机器人名称（它们的 tf 框架和主题名称）应该以命名空间为前缀（如前所述）。例如，如果您有三个具有以下命名空间的机器人：“/robot_1”、“/robot_2”和“/robot_3”，则必须将此参数设置为“/robot_”
    • namespace_init_count ( float , 默认值: 1.0)机器人名称的起始索引，该索引遵循上述命名空间。仅用于多机器人案例（单机器人案例不使用此参数，可忽略）。在多机器人的情况下，机器人名称有一个命名空间（您可以使用前面提到的参数设置），然后是一个索引。如果您设置的机器人名称不以 1 开头，那么您可以根据您的设置使用此参数。示例：假设您有两个命名空间为“/robot_4”和“/robot_5”的机器人，那么您必须将此参数设置为 4。

    5.Assigner

    订阅的话题：全局识别器订阅地图数据、过滤边界点主题和目标主题。

    • 地图数据：就是slam节点发布的map话题
    • 过滤边界点主题：主题名称由 ~frontiers_topic 参数定义
    • 目标主题：主题名称由 ~goals_topic 参数定义。这是过滤器节点接收检测到的边界点的主题

    发布的话题：无

    参数：

    • 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）机器人数量
    • namespace（字符串，默认值：）命名空间用于为机器人 tf 框架名称和主题名称添加前缀。
    • namespace_init_count ( float , 默认值: 1.0)机器人名称的起始索引。
    • delay_after_assignement ( float , 默认值: 0.5)单位是秒。它定义了每个机器人分配后的延迟量。
    • global_frame ( string , 默认: "/map")用作全局框架的框架名称。在单机器人中，与“map_topic”参数相同。在多机器人情况下，与全局框架对应的是框架名称。

    最终这些参数的设置都放在了simple.launch中

    2.  
    3.  
    4. <launch>
    5. <arg name="eta" value="1.0"/>
    6. <arg name="Geta" value="15.0"/>
    7. <param name="namespace_init_count" value="1"/>
    8.  
    9.  
    10.   <node pkg="rrt_exploration" type="global_rrt_detector" name="global_detector" output="screen">
    11.   <param name="eta" value="$(arg Geta)"/>
    12.   <param name="map_topic" value="/map"/>
    13.   node>
    14.   
    15.   <node pkg="rrt_exploration" type="local_rrt_detector" name="local_detector" output="screen">
    16.   <param name="eta" value="$(arg eta)"/>
    17.   <param name="map_topic" value="/map"/>
    18.   <param name="robot_frame" value="/base_link"/>
    19.   node>
    20.  
    21.   
    22.   <node pkg="rrt_exploration" type="filter.py" name="filter" output="screen">
    23.   <param name="map_topic" value="/map"/>
    24.   <param name="info_radius" value="1"/> 
    25.   <param name="costmap_clearing_threshold" value="70"/> 
    26.   <param name="goals_topic" value="/detected_points"/>
    27.   <param name="namespace" value=""/> 
    28.   <param name="n_robots" value="1"/>
    29.   <param name="rate" value="100"/>
    30.   node>
    31.   
    32.   <node pkg="rrt_exploration" type="assigner.py" name="assigner" output="screen">
    33.   <param name="map_topic" value="/map"/>
    34.   <param name="global_frame" value="/map"/>
    35.   <param name="info_radius" value="1"/> 
    36.   <param name="info_multiplier" value="3.0"/> 
    37.   <param name="hysteresis_radius" value="3.0"/> 
    38.   <param name="hysteresis_gain" value="2.0"/> 
    39.   <param name="frontiers_topic" value="/filtered_points"/> 
    40.   <param name="n_robots" value="1"/>
    41.   <param name="namespace" value=""/>
    42.   <param name="delay_after_assignement" value="0.5"/>
    43.   <param name="rate" value="100"/>
    44.   node>
    45.   
    46. launch>

    四、仿真实验

    如果使用rrt作者提供的例程，成功运行这个功能包很简单，只需要：

    1. roslaunch rrt_exploration_tutorials simple.launch
    2. roslaunch rrt_exploration simple.launch

    但是如果想在我们自己搭建的仿真环境中使用就需要进行比较多的改动：

    这里我把原来加载机器人模型的部分进行了注释，保留了源代码中给出的move_base文件和rviz配置文件，然后接下来对move_base文件进行修改：

    首先，我将这里的激光雷达话题重映射进行了注释，因为我们自己搭建的雷达模型输出话题是/scan。

    然后是这里的重映射，给move_base发送的速度控制指令，我们机器人的速度控制话题是/cmd_vel，不需要重映射，应该是作者使用的机器人速度控制指令是/mobile_base/commands/velocity。

    最后就是文件最后的几个话题、坐标系需要换成我们搭建的机器人的名字。

    另外要注意一点，如果想使用自己的move_base文件，这里需要修改为move_base_node，因为作者在rrt代码中对move_base的命名是这个，否则就需要修改源代码。

    最后的仿真结果如下，开始仿真：

    建图完成：

    另外，如果要在ubuntu20.04上使用这个包，需要对文件进行修改：

    首先是rrt_exploration包中的simple.launch，需要将所有的frame_id前面的"/" 都去掉，而所有的topic前面都需要带着"/"：

    2.  
    3.  
    4. <launch>
    5. <arg name="eta" value="1.0"/>
    6. <arg name="Geta" value="15.0"/>
    7. <param name="namespace_init_count" value="1"/>
    8.  
    9.  
    10.   <node pkg="rrt_exploration" type="global_rrt_detector" name="global_detector" output="screen">
    11.   <param name="eta" value="$(arg Geta)"/>
    12.   <param name="map_topic" value="/map"/>
    13.   node>
    14.   
    15.   <node pkg="rrt_exploration" type="local_rrt_detector" name="local_detector" output="screen">
    16.   <param name="eta" value="$(arg eta)"/>
    17.   <param name="map_topic" value="/map"/>
    18.   <param name="robot_frame" value="base_link"/>
    19.   node>
    20.  
    21.   
    22.   <node pkg="rrt_exploration" type="filter.py" name="filter" output="screen">
    23.   <param name="map_topic" value="/map"/>
    24.   <param name="info_radius" value="1"/> 
    25.   <param name="costmap_clearing_threshold" value="70"/> 
    26.   <param name="goals_topic" value="/detected_points"/>
    27.   <param name="namespace" value=""/> 
    28.   <param name="n_robots" value="1"/>
    29.   <param name="rate" value="100"/>
    30.   node>
    31.   
    32.   <node pkg="rrt_exploration" type="assigner.py" name="assigner" output="screen">
    33.   <param name="map_topic" value="/map"/>
    34.   <param name="global_frame" value="map"/>
    35.   <param name="info_radius" value="1"/> 
    36.   <param name="info_multiplier" value="3.0"/> 
    37.   <param name="hysteresis_radius" value="3.0"/> 
    38.   <param name="hysteresis_gain" value="2.0"/> 
    39.   <param name="frontiers_topic" value="/filtered_points"/> 
    40.   <param name="n_robots" value="1"/>
    41.   <param name="namespace" value=""/>
    42.   <param name="delay_after_assignement" value="0.5"/>
    43.   <param name="rate" value="100"/>
    44.   node>
    45.   
    46. launch>

    然后对于rrt_exploration包中的move_baseSafe.launch也需要进行上述修改：

    2. <launch>
    3.   <master auto="start"/>
    4.  
    5.   <arg name="namespace"/>
    6.   <arg name="odom_frame_id"   default="odom"/>
    7.   <arg name="base_frame_id"   default="base_link"/>
    8.   <arg name="global_frame_id" default="map"/>
    9.   <arg name="laser_frame_id" default="laser_link" />
    10.   <arg name="laser_topic" default="/scan" />
    11.  
    12.  
    13. <param name="use_sim_time" value="true" />
    14.  
    15.     <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" >
    16.       <remap from="/scan" to="$(arg laser_topic)"/>
    17.       <param name="map_frame" value="$(arg global_frame_id)"/>
    18.       <param name="odom_frame" value="$(arg odom_frame_id)"/>
    19.       <param name="base_frame" value="$(arg base_frame_id)"/>
    20.       <param name="map_update_interval" value="2.0"/>
    21.       <param name="maxUrange" value="50.0"/>
    22.       <param name="maxRange" value="50.0"/>
    23.       <param name="sigma" value="0.05"/>
    24.       <param name="kernelSize" value="1"/>
    25.       <param name="lstep" value="0.05"/>
    26.       <param name="astep" value="0.05"/>
    27.       <param name="iterations" value="5"/>
    28.       <param name="lsigma" value="0.075"/>
    29.       <param name="ogain" value="3.0"/>
    30.       <param name="lskip" value="0"/>
    31.       <param name="srr" value="0.01"/>
    32.       <param name="srt" value="0.02"/>
    33.       <param name="str" value="0.01"/>
    34.       <param name="stt" value="0.02"/>
    35.       <param name="linearUpdate" value="0.01"/>
    36.       <param name="angularUpdate" value="0.01"/>
    37.       <param name="temporalUpdate" value="0.1"/>
    38.       <param name="resampleThreshold" value="0.5"/>
    39.       <param name="particles" value="30"/>
    40.       <param name="xmin" value="-5.0"/>
    41.       <param name="ymin" value="-5.0"/>
    42.       <param name="xmax" value="5.0"/>
    43.       <param name="ymax" value="5.0"/>
    44.       <param name="delta" value="0.1"/>
    45.       <param name="llsamplerange" value="0.01"/>
    46.       <param name="llsamplestep" value="0.01"/>
    47.       <param name="lasamplerange" value="0.005"/>
    48.       <param name="lasamplestep" value="0.005"/>
    49.       <param name="minimumScore" value="0.005"/>
    50.     node>
    51.  
    52.  
    53.   <node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen">
    54.     <param name="footprint_padding" value="0.01" />
    55.     <param name="controller_frequency" value="5.0" />
    56.     <param name="controller_patience" value="3.0" />
    57.     <param name="oscillation_timeout" value="30.0" />
    58.     <param name="oscillation_distance" value="0.5" />
    59.     <param name="planner_patience" value="1" />
    60.     <param name="controller_patience" value="1" /> 
    61.     <remap from="/cmd_vel" to="/cmd_vel"/>
    62.     <param name="recovery_behavior_enabled" value="false" />
    63.     <rosparam file="$(find rrt_exploration_tutorials)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    64.     <rosparam file="$(find rrt_exploration_tutorials)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
    65.     <rosparam file="$(find rrt_exploration_tutorials)/param/local_costmap_params.yaml" command="load" />
    66.     <rosparam file="$(find rrt_exploration_tutorials)/param/global_costmap_params.yaml" command="load" />
    67.     <rosparam file="$(find rrt_exploration_tutorials)/param/base_local_planner_params.yaml" command="load" />  
    68.     <param name="global_costmap/global_frame" value="$(arg global_frame_id)"/>
    69.     <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
    70.     <param name="global_costmap/laser_scan_sensor/sensor_frame" value="$(arg laser_frame_id)"/>
    71.     <param name="global_costmap/laser_scan_sensor/topic" value="$(arg laser_topic)"/>    
    72.     <param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/>
    73.     <param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
    74.     <param name="local_costmap/laser_scan_sensor/sensor_frame" value="$(arg laser_frame_id)"/>
    75.     <param name="local_costmap/laser_scan_sensor/topic" value="$(arg laser_topic)"/>
    76.     <param name="local_costmap/obstacle_layer/laser_scan_sensor/topic" value="$(arg laser_topic)"/>
    77.   node>
    78.  
    79. launch>

    在ubuntu20.04中的仿真效果如下：

  • 相关阅读:
    SpringBoot中的yml文件中读取自定义配置信息
    ARM hint instruction-WFI(Wait For Interrupt)指令详解
    SpringCloud 微服务nacos和eureka
    【HTML】HTML网页设计----非遗网页设计
    2022版史上最牛Java学习路线图（视频教程+配套资料）
    大语言模型Ollama
    spring boot3单模块项目工程搭建-上（个人开发模板）
    Java虚拟机底层原理分析
    Jetpack Compose 入门教程之Text
    2024年第十五届蓝桥杯C/C++B组复盘(持续更新)
  • 原文地址：https://blog.csdn.net/qq_52785580/article/details/127320341