ROS仿真笔记之——基于rrt_exploration的单个机器人自主探索建图

最新推荐文章于 2025-04-27 08:18:14 发布

gwpscut

最新推荐文章于 2025-04-27 08:18:14 发布

阅读量6.2k

点赞数 9

分类专栏： ROS

本文链接：https://blog.csdn.net/gwplovekimi/article/details/119045783

版权

ROS 同时被 2 个专栏收录

147 篇文章

订阅专栏

SLAM

30 篇文章

订阅专栏

之前博文已经介绍过机器人exploration以及多机器人SLAM map merage《ROS学习笔记之——多机器人探索环境》《ROS仿真笔记之——基于gazebo的多机器人探索环境仿真》《ROS仿真笔记之——多移动机器人SLAM地图融合》《ROS仿真笔记之——移动机器人自主探索式地图构建》之前的都是基于功能包（http://wiki.ros.org/multirobot_map_merge/）

原理

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

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

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

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

如下图所示，主要分为三个模块。

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

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

关于global与local detector。如下图所示。当检测到一个边界点时，local detector会reset，并且会基于机器人当前的位置重新生长。这带来两个好处：1、检测边界点更快。因为机器人总是会向着边界点移动，而如果RRT树从机器人当前的位置开始，那么它到达未知区域的概率更高。2、机器人可能会miss掉一些地图上边角的小的边界点，而为了解决这个问题，就通过global detector。但随着地图的变大，global detector会越来越慢（或者说，随着树的变大，exploration会变慢），为此需要local detector

而关于assigner，其基于以下几个要素来分配机器人导航的目标点：

Navigation cost
Information gain

仿真

而本博文基于rrt_exploration包来进行仿真。网上也有大把基于此包的博客。但是本人实现时都无法跑动（可能是版本的原因？老显示tf无法链）为此，更改了原包的机器人模型为turtlebot3 waffle并且调了一些路径规划的参数，终于调出来了，效果如下视频所示～

single kobuki robot slam based on rrt

单机器人自主探索建图

单机器人自主探索建图(1)

换成kobuki模型效果好很多～

运行命令：

roslaunch rrt_exploration_tutorials single_simulated_house.launch
roslaunch rrt_exploration single.launch

注意要保证move base的开启

single_simulated_house.launch

<!--  Launch file for signle robot on Gazebo -->
<launch>
<env name="GAZEBO_RESOURCE_PATH" value="$(find rrt_exploration_tutorials)/launch/includes/meshes"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="use_sim_time" value="true"/>
<arg name="debug" value="false"/>
<arg name="world_name" value="$(find rrt_exploration_tutorials)/launch/includes/worlds/house.world"/>
<!-- <remap from="/robot_1/cmd_vel" to="/robot_1/mobile_base/commands/velocity"/> -->
</include>

<group ns="robot_1">
<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
<arg name="robot_name" value="robot_1"/>
<arg name="init_pose" value="-x 0.0 -y 0.0 -z 0.0"/>
</include>

<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
<arg name="namespace" value="robot_1"/>
</include> 
</group>

<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rrt_exploration_tutorials)/launch/includes/rviz_config/single.rviz">
<remap from="move_base_simple/goal" to="robot_1/move_base_simple/goal"/>
</node>

</launch>

move_baseSafe.launch

<!-- move base -->
<launch>
  <master auto="start"/>

  <arg name="namespace"/>

   <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <!-- <arg name="cmd_vel_topic" default="/cmd_vel" /> -->
  <!-- <arg name="odom_topic" default="odom" />
  <arg name="move_forward_only" default="false"/> -->


<param name="use_sim_time" value="true" />

    <!-- <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" >
      <remap from="scan" to="base_scan"/>
      <param name="map_frame" value="$(arg namespace)/map"/>
      <param name="odom_frame" value="$(arg namespace)/odom"/>
      <param name="base_frame" value="$(arg namespace)/base_link"/>
      <param name="map_update_interval" value="2.0"/>
      <param name="maxUrange" value="50.0"/>
      <param name="maxRange" value="50.0"/>
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="1"/>
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="srr" value="0.01"/>
      <param name="srt" value="0.02"/>
      <param name="str" value="0.01"/>
      <param name="stt" value="0.02"/>
      <param name="linearUpdate" value="0.01"/>
      <param name="angularUpdate" value="0.01"/>
      <param name="temporalUpdate" value="0.1"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-5.0"/>
      <param name="ymin" value="-5.0"/>
      <param name="xmax" value="5.0"/>
      <param name="ymax" value="5.0"/>
      <param name="delta" value="0.1"/>
      <param name="llsamplerange" value="0.01"/>
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
      <param name="minimumScore" value="0.005"/>
    </node> -->


  <!-- <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" /> 
    <remap from="cmd_vel" to="mobile_base/commands/velocity"/>
    <param name="recovery_behavior_enabled" value="false" />
    <rosparam file="$(find rrt_exploration_tutorials)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find rrt_exploration_tutorials)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find rrt_exploration_tutorials)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find rrt_exploration_tutorials)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find rrt_exploration_tutorials)/param/base_local_planner_params.yaml" command="load" />  
    <param name="global_costmap/global_frame" value="$(arg namespace)/map"/>
    <param name="global_costmap/robot_base_frame" value="$(arg namespace)/base_link"/>
    <param name="global_costmap/laser_scan_sensor/sensor_frame" value="/$(arg namespace)/base_laser_link"/>
    <param name="global_costmap/laser_scan_sensor/topic" value="/$(arg namespace)/base_scan"/>    
    <param name="local_costmap/global_frame" value="$(arg namespace)/odom"/>
    <param name="local_costmap/robot_base_frame" value="$(arg namespace)/base_link"/>
    <param name="local_costmap/laser_scan_sensor/sensor_frame" value="$(arg namespace)/base_laser_link"/>
    <param name="local_costmap/laser_scan_sensor/topic" value="/$(arg namespace)/base_scan"/>
    <param name="local_costmap/obstacle_layer/laser_scan_sensor/topic" value="/$(arg namespace)/base_scan"/>
  </node> -->

   <!-- <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
    <param name="base_frame" value="$(arg namespace)/base_link"/>
    <param name="odom_frame" value="$(arg namespace)/odom"/>
    <param name="map_frame"  value="$(arg namespace)/map"/>
    <param name="map_update_interval" value="2.0"/>
    <param name="maxUrange" value="3.0"/>
    <param name="sigma" value="0.05"/>
    <param name="kernelSize" value="1"/>
    <param name="lstep" value="0.05"/>
    <param name="astep" value="0.05"/>
    <param name="iterations" value="5"/>
    <param name="lsigma" value="0.075"/>
    <param name="ogain" value="3.0"/>
    <param name="lskip" value="0"/>
    <param name="minimumScore" value="50"/>
    <param name="srr" value="0.1"/>
    <param name="srt" value="0.2"/>
    <param name="str" value="0.1"/>
    <param name="stt" value="0.2"/>
    <param name="linearUpdate" value="1.0"/>
    <param name="angularUpdate" value="0.2"/>
    <param name="temporalUpdate" value="0.5"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="particles" value="100"/>
    <param name="xmin" value="-10.0"/>
    <param name="ymin" value="-10.0"/>
    <param name="xmax" value="10.0"/>
    <param name="ymax" value="10.0"/>
    <param name="delta" value="0.05"/>
    <param name="llsamplerange" value="0.01"/>
    <param name="llsamplestep" value="0.01"/>
    <param name="lasamplerange" value="0.005"/>
    <param name="lasamplestep" value="0.005"/>
  </node> -->

   <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
    <param name="base_frame" value="$(arg namespace)/base_link"/>
    <param name="odom_frame" value="$(arg namespace)/odom"/>
    <param name="map_frame"  value="$(arg namespace)/map"/>
    <param name="map_update_interval" value="2.0"/>
    <param name="maxUrange" value="50.0"/>
    <param name="maxRange" value="50.0"/>
    <param name="sigma" value="0.05"/>
    <param name="kernelSize" value="1"/>
    <param name="lstep" value="0.05"/>
    <param name="astep" value="0.05"/>
    <param name="iterations" value="5"/>
    <param name="lsigma" value="0.075"/>
    <param name="ogain" value="3.0"/>
    <param name="lskip" value="0"/>
    <!-- <param name="minimumScore" value="50"/> -->
    <param name="srr" value="0.01"/>
    <param name="srt" value="0.02"/>
    <param name="str" value="0.01"/>
    <param name="stt" value="0.02"/>
    <param name="linearUpdate" value="0.01"/>
    <param name="angularUpdate" value="0.01"/>
    <param name="temporalUpdate" value="0.1"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="particles" value="100"/>
    <param name="xmin" value="-10.0"/>
    <param name="ymin" value="-10.0"/>
    <param name="xmax" value="10.0"/>
    <param name="ymax" value="10.0"/>
    <param name="delta" value="0.1"/>
    <param name="llsamplerange" value="0.01"/>
    <param name="llsamplestep" value="0.01"/>
    <param name="lasamplerange" value="0.005"/>
    <param name="lasamplestep" value="0.005"/>
    <param name="minimumScore" value="0.005"/>
  </node>


   <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.05" />
    <param name="planner_patience" value="1" />
    <param name="controller_patience" value="1" /> 
    <!-- <remap from="cmd_vel" to="mobile_base/commands/velocity"/> -->
    <remap from="cmd_vel" to="cmd_vel"/>
    <param name="recovery_behavior_enabled" value="false" />
    <param name="clearing_rotation_allowed" value="true" />
    <param name="min_vel_x" value="-0.3" />
    <rosparam file="$(find rrt_exploration_tutorials)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find rrt_exploration_tutorials)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find rrt_exploration_tutorials)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find rrt_exploration_tutorials)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find rrt_exploration_tutorials)/param/base_local_planner_params.yaml" command="load" />  
    
    <!-- <rosparam file="$(find turtlebot3_navigation)/param/a_costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" /> -->
    <!-- <rosparam file="$(find turtlebot3_navigation)/param/a_costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" /> -->
    <!-- <rosparam file="$(find turtlebot3_navigation)/param/a_local_costmap_params.yaml" command="load" /> -->
    <!-- <rosparam file="$(find turtlebot3_navigation)/param/a_global_costmap_params.yaml" command="load" /> -->

    <!-- move_based对应的参数 -->
    <!-- <rosparam file="$(find turtlebot3_navigation)/param/move_base_params.yaml" command="load" /> -->
    <!-- 局部路径规划对应的参数 -->
    <!-- <rosparam file="$(find rrt_exploration_tutorials)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find rrt_exploration_tutorials)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find rrt_exploration_tutorials)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find rrt_exploration_tutorials)/param/global_costmap_params.yaml" command="load" /> -->
    <!-- <rosparam file="$(find rrt_exploration_tutorials)/param/base_local_planner_params.yaml" command="load" /> -->

    <!-- <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
    <rosparam file="$(find turtlebot3_navigation)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
    <remap from="odom" to="$(arg odom_topic)"/>
    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" /> -->

    <param name="global_costmap/global_frame" value="$(arg namespace)/map"/>
    <param name="global_costmap/robot_base_frame" value="$(arg namespace)/base_link"/>
    <param name="global_costmap/laser_scan_sensor/sensor_frame" value="/$(arg namespace)/base_laser_link"/>
    <param name="global_costmap/laser_scan_sensor/topic" value="/$(arg namespace)/base_scan"/>    
    <param name="local_costmap/global_frame" value="$(arg namespace)/odom"/>
    <param name="local_costmap/robot_base_frame" value="$(arg namespace)/base_link"/>
    <param name="local_costmap/laser_scan_sensor/sensor_frame" value="$(arg namespace)/base_laser_link"/>
    <param name="local_costmap/laser_scan_sensor/topic" value="/$(arg namespace)/base_scan"/>
    <param name="local_costmap/obstacle_layer/laser_scan_sensor/topic" value="/$(arg namespace)/base_scan"/>
  </node> 

   <!-- <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"> -->
    <!-- 启动局部路径规划base_local_planner，选用dwa_local_planner/DWAPlannerROS方法 -->
    <!-- <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" /> -->
    <!-- 启动costmap节点及对应的参数 -->
    <!-- <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/global_costmap_params.yaml" command="load" /> -->
    <!-- move_based对应的参数 -->
    <!-- <rosparam file="$(find turtlebot3_navigation)/param/move_base_params.yaml" command="load" /> -->
    <!-- 局部路径规划对应的参数 -->
    <!-- <rosparam file="$(find turtlebot3_navigation)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
    <remap from="odom" to="$(arg odom_topic)"/>
    <param name="global_costmap/global_frame" value="$(arg namespace)/map"/>
    <param name="global_costmap/robot_base_frame" value="$(arg namespace)/base_link"/>
    <param name="global_costmap/laser_scan_sensor/sensor_frame" value="$(arg namespace)/base_scan"/>
    <param name="global_costmap/laser_scan_sensor/topic" value="/$(arg namespace)/base_scan"/>    
    <param name="local_costmap/global_frame" value="$(arg namespace)/odom"/>
    <param name="local_costmap/robot_base_frame" value="$(arg namespace)/base_link"/>
    <param name="local_costmap/laser_scan_sensor/sensor_frame" value="$(arg namespace)/base_scan"/>
    <param name="local_costmap/laser_scan_sensor/topic" value="/$(arg namespace)/base_scan"/>
    <param name="local_costmap/obstacle_layer/laser_scan_sensor/topic" value="/$(arg namespace)/base_scan"/>
    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
  </node> -->

</launch>

robot.launch.xml

<!--
  Spawns Kobuki inside a Gazebo simulation
  -->
<launch>
  <arg name="robot_name"/>
  <arg name="init_pose"/>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>

  <include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml">
    <arg name="model" value="$(arg model)" />
  </include>

  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
  <node pkg="gazebo_ros" type="spawn_model" name="spawn_$(arg robot_name)" 
        args="$(arg init_pose) -unpause -urdf -param robot_description -model $(arg robot_name)" respawn="false">
  </node>

  <!-- <param name="robot_description"
        command="$(find xacro)/xacro.py '$(find rrt_exploration_tutorials)/launch/includes/urdf/kobuki_standalone.urdf.xacro'"/>
  <node pkg="gazebo_ros" type="spawn_model" name="spawn_$(arg robot_name)" 
        args="$(arg init_pose) -unpause -urdf -param robot_description -model $(arg robot_name)" respawn="false">
  </node> -->
 
   <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
   <param name="publish_frequency" type="double" value="30.0" />
   <param name="use_tf_static" type="bool" value="false" />
   <param name="tf_prefix" type="string" value="$(arg robot_name)"/>
   </node>
</launch>

single.launch

<!-- Launch file for the rrt-detector and the assigner -->


<launch>
<arg name="eta" value="1.0"/>
<arg name="Geta" value="15.0"/>
<param name="namespace_init_count" value="1"/>


  <node pkg="rrt_exploration" type="global_rrt_detector" name="global_detector" output="screen">
  <param name="eta" value="$(arg Geta)"/>
  <param name="map_topic" value="robot_1/map"/>
  </node>
  
  <node pkg="rrt_exploration" type="local_rrt_detector" name="local_detector" output="screen">
  <param name="eta" value="$(arg eta)"/>
  <param name="map_topic" value="robot_1/map"/>
  <param name="robot_frame" value="robot_1/base_link"/>
  </node>
 
  
  <node pkg="rrt_exploration" type="filter.py" name="filter" output="screen">
  <param name="map_topic" value="robot_1/map"/>
  <param name="info_radius" value="1"/> 
  <param name="costmap_clearing_threshold" value="70"/> 
  <param name="goals_topic" value="/detected_points"/>
  <param name="namespace" value="robot_"/> 
  <param name="n_robots" value="1"/>
  <param name="rate" value="100"/>
  </node>
  
  <node pkg="rrt_exploration" type="assigner.py" name="assigner" output="screen">
  <param name="map_topic" value="robot_1/map"/>
  <param name="global_frame" value="robot_1/map"/>
  <param name="info_radius" value="1"/> 
  <param name="info_multiplier" value="3.0"/> 
  <param name="hysteresis_radius" value="3.0"/> 
  <param name="hysteresis_gain" value="2.0"/> 
  <param name="frontiers_topic" value="/filtered_points"/> 
  <param name="n_robots" value="1"/>
  <param name="namespace" value="robot_"/>
  <param name="delay_after_assignement" value="0.5"/>
  <param name="rate" value="100"/>
  </node>
  
</launch>