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: 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计算方式.