【自主探索】CMU Autonomous Exploration系列笔记

最新推荐文章于 2024-03-11 16:25:04 发布

远方~0924

最新推荐文章于 2024-03-11 16:25:04 发布

阅读量5.9k

点赞数 23

文章标签：自动驾驶

本文链接：https://blog.csdn.net/qq_33263835/article/details/121621923

版权

链接:Development Environment

CMU机器人研究所于2021年7月开源全套移动机器人自主导航和探索框架,其主要算法都出自近两年CMU发出的顶会论文.该框架主要分为自主探索环境和探索planner两部分:

1.自主探索开发环境 https://github.com/HongbiaoZ/autonomous_exploration_development_environment

github截图如下:探索环境部分主要包括移动双轮差速机器人模拟器,传感器模拟器,局部路径规划器,可视化工具,路点发布example,路点发布rviz插件,地形分析程序,扩展地形分析程序等.

几个模块介绍如下:

vehicle_simulator	该ros包主要功能是实现了一个双轮差动底盘的模拟,接收cmd_vel速度信息,自己手写的机器人运动学微分方程来对机器人位姿进行推算,从而输出里程计odometry,而gazebo只是用来做模型显示.该包中包含了CMU制作的几个探索gazebo world,分别包含了不同的环境类型.
velodyne_simulator	velodyne激光雷达模拟器,velodyne开源gazebo插件
local_planner	局部路径规划planner,输入机器人里程计和目标路点,输出控制指令cmd_vel.用于局部路径规划导航,其中包括了localPlanner和pathFoller两部分,localplanner作用是规划局部路径,pathFollower作用是根据规划的局部轨迹进行轨迹跟踪,核心思想和pid差不多.
loam_interface	loam的接口,作用是将loam输出的里程计进行坐标系转换,同时将单帧点云装换到全局坐标系map下面输入给localPlanner.
terrain_analysis	地形分析,叠加多帧点云,将机器人周围区域的地面进行可行驶区域分析,不能通行的区域会以点云的形式传输给localPlanner,用于localPlanner避障.
terrain_analysis_ext	扩展地形分析,从rviz结果来看是对更大区域的地形进行了可行驶区域进行分析,具体代码内容还没有看.
waypoint_example	路点发布example程序,将设置好的路点写到data目录下面,运行就能安装顺序发布路点,从而进行逐个路点导航.
waypoint_rviz_plugin	waypoint发布的rviz插件,运行此插件后,rviz就能通过waypoint按钮鼠标点击发布路点,直接进行导航.
visualization_tools	可视化工具,将探索过程中的三个实验指标曲线进行可视化,通过matplotlib绘图显示出来,包括探索体积探索路程每次规划计算时间.

运行一个vehiclesimulator 的launch文件后

roslaunch vehicle_simulator system_garage.launch

rqt_graph显示如上图,几个模块中最核心的是localPlanner模块,实现了自主导航避障的功能.其代码出自cmu zhangji2019 IROS 和2020JFR的论文.

J. Zhang, C. Hu, R. Gupta Chadha, and S. Singh. Falco: Fast Likelihood-based Collision Avoidance with Extension to Human-guided Navigation. Journal of Field Robotics. vol. 37, no. 8, pp. 1300–1313, 2020. [PDF]

2.tare planner 自主探索算法

链接:TARE Planner

论文解析参考博客 TARE: A Hierarchical Framework for Efficiently Exploring Complex 3D Environments

(翻译的的真清楚)

tare planer社区 https://www.csdn.net/c/TARE

代码模块:

grid_world
keypose_graph
lidar_model	雷达模型,应该是用来计算viewpoint的回报的时候用的.
local_coverage_planner
planning_env
pointcloud_manager
rolling_grid
rolling_occupancy_grid
sensor_coverage_planner	SensorCoveragePlanner3D类函数的实现
tare_planner_node	Tare planner 主程序入口
tare_visualizer	发布一些可视化信息给rviz
tsp_solver	实现了tsp求解器的封装,在tsp规划的时候调用.
utils	杂项功能函数
viewpoint
viewpoint_manager
exploration_path	ExplorationPath结构体以及成员函数的实现,用于存储探索路径信息

(这个表可能要慢慢完善了)

部分代码理解

代码运行入口在sensor_coverage_planner_ground.cpp 这个cpp文件实现了 SensorCoveragePlanner3D类的成员函数.

该类构造函数实现了各个成员的初始化,同时在初始化函数

bool SensorCoveragePlanner3D::initialize(ros::NodeHandle& nh, ros::NodeHandle& nh_p)

开启了一个定时器,见代码:

execution_timer_ = nh.createTimer(ros::Duration(1.0), &SensorCoveragePlanner3D::execute, this); //每秒执行一次execute函数

该定时器的回调函数execute才是全部代码的核心循环执行部分:

void SensorCoveragePlanner3D::execute(const ros::TimerEvent&)
{
    static int a=0;
    std::string  counter_str="execute count:"+std::to_string(a++);
    // PrintExplorationStatus(counter_str,true);
  if (!pp_.kAutoStart && !start_exploration_)
  {
    ROS_INFO("Waiting for start signal");
    return;
  }
  Timer overall_processing_timer("overall processing");     //这里的timer是自己写的，用来计时但是没有回调函数,与ros的timer不同
  update_representation_runtime_ = 0;
  local_viewpoint_sampling_runtime_ = 0;
  local_path_finding_runtime_ = 0;
  global_planning_runtime_ = 0;
  trajectory_optimization_runtime_ = 0;
  overall_runtime_ = 0;
  if (!initialized_)
  {
    SendInitialWaypoint();
    start_time_ = ros::Time::now();
    return;
  }
  overall_processing_timer.Start();
  if (keypose_cloud_update_)
  {
    keypose_cloud_update_ = false;
    /*----------------------------------------------更新环境的表示------------------------------------------------------------------------------------*/
    misc_utils_ns::Timer update_representation_timer("update representation");
    update_representation_timer.Start();
    // Update grid world
    UpdateGlobalRepresentation();      //更新全局环境表示
    int viewpoint_candidate_count = UpdateViewPoints();   //更新viewpoints
    if (viewpoint_candidate_count == 0)
    {
      ROS_WARN("Cannot get candidate viewpoints, skipping this round");
      return;
    }
    UpdateKeyposeGraph();         //更新关键位姿图
    int uncovered_point_num = 0;
    int uncovered_frontier_point_num = 0;
    if (!exploration_finished_)
    {
      UpdateViewPointCoverage();     //更新viewpoint的覆盖
      UpdateCoveredAreas(uncovered_point_num, uncovered_frontier_point_num);  //更新覆盖的面积
    }
    else
    {
      pd_.viewpoint_manager_->ResetViewPointCoverage();
    }
    update_representation_timer.Stop(false);
    update_representation_runtime_ += update_representation_timer.GetDuration("ms");
    /*--------------------------------------------更新环境表示结束----------------------------------------------------------------------------*/
    // Global TSP                                                                      ------------------全局tsp规划
    std::vector<int> global_cell_tsp_order;
    exploration_path_ns::ExplorationPath global_path;
    GlobalPlanning(global_cell_tsp_order, global_path);
    // Local TSP                                                                      ---------------------局部TSP规划
    exploration_path_ns::ExplorationPath local_path;
    LocalPlanning(uncovered_point_num, uncovered_frontier_point_num, global_path, local_path);
    near_home_ = GetRobotToHomeDistance() < pp_.kRushHomeDist;
    at_home_ = GetRobotToHomeDistance() < pp_.kAtHomeDistThreshold;
    if (pd_.grid_world_->IsReturningHome() && pd_.local_coverage_planner_->IsLocalCoverageComplete() &&
        (ros::Time::now() - start_time_).toSec() > 5)
    {
      if (!exploration_finished_)
      {
        PrintExplorationStatus("Exploration completed, returning home", false);
      }
      exploration_finished_ = true;
    }
    if (exploration_finished_ && at_home_ && !stopped_)
    {
      PrintExplorationStatus("Return home completed", false);
      stopped_ = true;
    }
    pd_.exploration_path_ = ConcatenateGlobalLocalPath(global_path, local_path);   //连接全局和局部的路径
    PublishExplorationState();
    lookahead_point_update_ = GetLookAheadPoint(pd_.exploration_path_, global_path, pd_.lookahead_point_);
    PublishWaypoint();
    overall_processing_timer.Stop(false);
    overall_runtime_ = overall_processing_timer.GetDuration("ms");
    pd_.visualizer_->GetGlobalSubspaceMarker(pd_.grid_world_, global_cell_tsp_order); //下面是发布可视化信息到rviz进行显示
    Eigen::Vector3d viewpoint_origin = pd_.viewpoint_manager_->GetOrigin();
    pd_.visualizer_->GetLocalPlanningHorizonMarker(viewpoint_origin.x(), viewpoint_origin.y(), pd_.robot_position_.z);
    pd_.visualizer_->PublishMarkers();
    PublishLocalPlanningVisualization(local_path);//局部规划可视化
    PublishGlobalPlanningVisualization(global_path, local_path);//全局规划可视化
    PublishRuntime();
  }
  // return true;
}

先写到这,等下一步更新viewpoint采样和viewpoint计算方式.