基于ROS的导航框架
0. 前言
这篇博客给各位介绍一下在ROS环境下常用的局部/全局的导航框架。在机器人运动控制当中。路径规划作为感知定位的下一个部分，机器人需要有一个比较合适的路径规划功能才能使机器人安全的运动到目标附近。
1. move_base
上面这个图很好的展示了move_base的整个框架。同时整张图的结构都非常清晰，首先左上角的amcl（自适应蒙特卡罗定位）模块是ROS的导航定位模块，amcl通过订阅scan、map和tf信息，发布出机器人的pose，以供move_base使用；左下角odom，即机器人里程计信息；右上角，map，是我们使用的先验地图信息，一般通过slam建图后保存；右下角，sensor，为传感器topic。这部分数据在costmap包中发挥了作用，costmap为代价地图，目前主要的有inflation_layer、obstacle_layer、static_layer、voxel_layer四个plugins。分别为膨胀层、障碍物层、静态层和体素层。一般我们的全局路径需要静态层和膨胀层，因为全局规划应该只考虑到地图信息，所以一般都是静态的，而局部路径规划则需要考虑到实时的障碍物信息，所以需要障碍物层和膨胀层，至于为什么不把静态层放到局部路径规划里，这是因为我们的定位是会存在误差的，比如轮子打滑或其他情况，这个时候amcl会起到纠正作用。
作为move_base的核心功能，global planner和local planner控制着整个机器人的路径规划，具体操作是将global planner作为local planner的一个初始参考或者优化方向。目前ROS中可以使用的global planner主要包括：A*和Dijkstra。local planner主要有：dwa、trajectory、teb和eband等。
2. move_base_flex
该存储库包含Move Base Flex（MBF），它是move_base的向后兼容替代品。 MBF可以将现有插件用于move_base，并提供相同ROS接口的增强版本。 其使用和move_base相同的plugin接口的同时拓展了接口virtual函数，保留了move_base相关的topic和servers。 外部执行逻辑可以使用MBF及其操作来执行智能灵活的导航策略。 同时提供各独立功能的action和状态检测相关的servers，可使用SMACH或者BehaviourTrees灵活组合调用各功能。这里贴一个讲的不错的博客链接。同时move_base_flex还具有以下有点
-
完全向后兼容当前ROS导航。
-
提供子模块计划、控制和恢复的操作，以及查询成本图的服务。此界面允许外部主管（如SMACH或行为树）运行高度灵活和复杂的导航策略。
-
可以加载多个计划器和控制器；执行人员可以选择适当的一个，甚至可以并行运行多个。
-
所有操作的综合结果和反馈信息，包括加载插件的错误代码和消息。对于仍然依赖独特导航界面的用户，我们扩展了move_base action，提供了详细的结果和反馈信息（尽管我们仍然提供当前的信息）。
-
抽象导航框架和具体实现之间的分离，允许更快地开发新的应用程序，例如3D导航。
这一个模块也是18年由Sebastian Pütz, Jorge Santos, and Joachim Hertzberg在《Move Base Flex: A Highly Flexible Navigation Framework for Mobile Robots》提出，该项目具有很大的潜力，相比于move_base而言其不需要将软件实现绑定到二维成本映射。在我们的框架中，MoveBase 只是导航系统的一个特殊实现: 它的执行类实现了抽象的导航，将系统绑定到 costmaps。因此，该系统可以很容易地用于其他方法，例如网格导航或三维占用网格地图。
3. GeRoNa
GeRoNa (Generic Robot Navigation)是一个模块化的机器人导航框架，包含路径规划、路径跟随(带避障)、以及各个模块之间交互的管理。它的设计理念是可以针对新的任务和机器人方便的扩进行展。
以下是该导航框架的逻辑框图：主要由三个部分组成，路径控制、路径规划、路径跟随。建图、定位以及障碍物检测是不包含在内的，需要依赖其他程序提供地图服务(mapserver)和位姿(pose)信息(主要是依赖tf_tree)。三个部分之间通过ROS中的Action机制传输数据。
我们可以看一下GERONA中path_follower包的代码结构：
path_follower模块又分为几个子模块：collision_avoidance、controller、factory、local_planner、supervisor，每一个子模块实际都是定义好的一个类。所有程序的入口（main函数）在follower_node.cpp文件中；path_follower_server.cpp中定义了PathFollowerServer类，实现了一个Action server使用actionlib库与path_control节点通信（msgs名字为"follow_path"），所以该类中定义了action的回调函数以及一个PathFollowerServer::spin()函数，其中执行所有的follow任务；pathfollower.cpp中定义了PathFollower类，所有的子模块在这里被实例化（构建一个对象）并被使用，PathFollower类与PathFollowerServer类一起在main()*函数中被实例化，它的构造函数被调用，PathFollower::update()函数是程序的入口，在“boost::variant<FollowPathFeedback, FollowPathResult> PathFollowerServer::update()”函数中以50Hz的频率执行，返回FollowPath的状态或结果。
PathFollower::update()函数是所有算法执行的地方，期望运行频率50Hz，但实际受限于算法运行速度，达不到50Hz。
boost::variant<FollowPathFeedback, FollowPathResult> PathFollower::update()
{
ROS_ASSERT(current_config_);
FollowPathFeedback feedback;
FollowPathResult result;
if(!is_running_) {
start();
}
if (path_->empty()) {
ROS_ERROR("tried to follow an empty path!");
stop(FollowPathResult::RESULT_STATUS_INTERNAL_ERROR);
return result;
}
//! 查找tf_tree获取机器人的pose(查找tf-tree world_frame->robot_frame)
if (!pose_tracker_->updateRobotPose()) {
ROS_ERROR("do not known own pose");
stop(FollowPathResult::RESULT_STATUS_SLAM_FAIL);
} else {
course_predictor_->update();
current_config_->controller_->setCurrentPose(pose_tracker_->getRobotPose());
}
// Ask supervisor whether path following can continue
Supervisor::State state(pose_tracker_->getRobotPose(),
path_,
obstacle_cloud_,
feedback);
Supervisor::Result s_res = supervisors_->supervise(state);
if(!s_res.can_continue) {
ROS_WARN("My supervisor told me to stop.");
stop(s_res.status);
return result;
}
if(current_config_->local_planner_->isNull()) {
// 不使用局部路径规划
//! 接收上层规划的全局路径后，输出运动控制指令
is_running_ = execute(feedback, result);
} else {
//End Constraints and Scorers Construction
publishPathMarker(); // 发布topic "visualization_marker",ns为 "global robot path"
if(obstacle_cloud_ != nullptr){
current_config_->local_planner_->setObstacleCloud(obstacle_cloud_);
}
if(elevation_map_ != nullptr){
current_config_->local_planner_->setElevationMap(elevation_map_);
}
// 获取LocalPlanner的参数
const LocalPlannerParameters& opt_l = *LocalPlannerParameters::getInstance();
if(opt_l.use_velocity()){
//current_config_->local_planner_->setVelocity(pose_tracker_->getVelocity().linear);
current_config_->local_planner_->setVelocity(pose_tracker_->getVelocity());
}
bool path_search_failure = false;
try {
// 进行局部规划
Path::Ptr local_path = current_config_->local_planner_->updateLocalPath();
// ROS_INFO("updateLocalPath...");
path_search_failure = local_path && local_path->empty();
if(local_path && !path_search_failure) {
// 若局部路径规划成功
path_msgs::PathSequence path;
path.header.stamp = ros::Time::now();
path.header.frame_id = getFixedFrameId();
for(int i = 0, sub = local_path->subPathCount(); i < sub; ++i) {
const SubPath& p = local_path->getSubPath(i);
path_msgs::DirectionalPath sub_path;
sub_path.forward = p.forward;
sub_path.header = path.header;
for(const Waypoint& wp : p.wps) {
geometry_msgs::PoseStamped pose;
pose.pose.position.x = wp.x;
pose.pose.position.y = wp.y;
pose.pose.orientation = tf::createQuaternionMsgFromYaw(wp.orientation);
sub_path.poses.push_back(pose);
}
path.paths.push_back(sub_path);
}
local_path_pub_.publish(path); // 发布局部路径，topic为"local_path"
}
//! 接收上层规划的路径(全局或局部)以后，输出运动控制指令
is_running_ = execute(feedback, result);
// ROS_INFO("PathFollower::execute...");
} catch(const std::runtime_error& e) {
ROS_ERROR_STREAM("Cannot find local_path: " << e.what());
path_search_failure = true;
}
if(path_search_failure) {
// 若搜索失败
ROS_ERROR_STREAM_THROTTLE(1, "no local path found.");
feedback.status = path_msgs::FollowPathFeedback::MOTION_STATUS_NO_LOCAL_PATH;
current_config_->controller_->stopMotion();
// publish an empty path
path_msgs::PathSequence path;
path.header.stamp = ros::Time::now();
path.header.frame_id = getFixedFrameId();
local_path_pub_.publish(path);
return feedback;
} else {
const std::vector<SubPath>& all_local_paths = current_config_->local_planner_->getAllLocalPaths();
if(!all_local_paths.empty()) {
nav_msgs::Path wpath;
wpath.header.stamp = ros::Time::now();
wpath.header.frame_id = current_config_->controller_->getFixedFrame();
for(const SubPath& path : all_local_paths) {
for(const Waypoint& wp : path.wps) {
geometry_msgs::PoseStamped pose;
pose.pose.position.x = wp.x;
pose.pose.position.y = wp.y;
pose.pose.orientation = tf::createQuaternionMsgFromYaw(wp.orientation);
wpath.poses.push_back(pose);
}
}
whole_local_path_pub_.publish(wpath); // 发布完整的局部路径，topic为"whole_local_path"
}
is_running_ = execute(feedback, result);
}
}
if(is_running_) {
return feedback;
} else {
return result;
}
}
4. 其他一些比较好的开源算法
-
3d_navigation:
https://github.com/ros-planning/3d_navigation -
深度学习的无人机和地面车辆的自主视觉导航组件:
https://github.com/NVIDIA-AI-IOT/redtail
https://github.com/NVIDIA-AI-IOT/Electron -
探索算法:
https://github.com/hasauino/rrt_exploration
https://github.com/laurimi/ase_exploration -
检测人的导航算法:
https://github.com/marinaKollmitz
https://github.com/marinaKollmitz/human_aware_navigation
…详情请参照古月居
- 点赞
- 收藏
- 关注作者
评论（0）