移动机器人——ros navigation

一、概述

ROS navigation为移动机器人导航相关包的集合,实现定位规划避障等相关功能。

整体工作流程为:

  1. 加载地图

    navigation通过map_server加载现有地图。navigation无建图相关包,需另外实现后保存,默认只支持2维地图,其他包类似grid_map提供2.5维地图的加载。

  2. 发布TF坐标变换

    navigation包内的AMCL提供map->odom的变换,AMCL实际为全局绝对定位,即map->base,但在应用时里程计会提供odom->base的变换,因此AMCL会获取TF信息转换后再发布map->base变换,navigation不包含里程计的实现,需另外独立实现。

  3. 启动move_base加载相关插件

    move_base为导航的核心框架,通过预设接口(plugin)加载地图(costmap_2d)和规划避障(global_planner, local_planner, recovery)相关插件,读取设置参数(机器人外形,规划避障设置等),并发布相关话题和服务器。

总得来说要实现机器人的自主导航除navigation外还需要一张图和里程计,当然navigation内部的算法都可以根据需要进行更换。

在实际使用后发现,move_base因其设计问题,灵活度较低,只能满足室内基本功能的实现,改进起来也不太方便,若需在复杂场景下使用可参考move_base_flex或者更换navigation2。

二、move_base

基础框架

移动机器人——ros navigation_第1张图片

工作流程

  1. 通过topic/Action设置目标点。

  2. 调用global_planner规划全局路径。

  3. 根据全局路径调用local_planner计算优化路径。

  4. 执行优化路径,根据设置参数重复调用2,3步,直至到底终点。

    ps:global_planner按照启动时设置的参数按照频率重复调用,无法中途修改,local_planner移动一段距离后重复调用

  5. 没有有效路径时,调用recovery_behaviors进行恢复,然后重规划,若成功回到步骤4,失败则按顺序执行设定的行为,全部失败则该运动失败。

Action

Action Subscribed Topics

  • move_base/goal (move_base_msgs/MoveBaseActionGoal)

    A goal for move_base to pursue in the world.

  • move_base/cancel (actionlib_msgs/GoalID)

    A request to cancel a specific goal.

Action Published Topics

  • move_base/feedback (move_base_msgs/MoveBaseActionFeedback)

    Feedback contains the current position of the base in the world.

  • move_base/status (actionlib_msgs/GoalStatusArray)

    Provides status information on the goals that are sent to the move_base action.

  • move_base/result (move_base_msgs/MoveBaseActionResult)

    Result is empty for the move_base action.

参数配置

  • base_global_planner (string, default: "navfn/NavfnROS" For 1.1+ series)

    The name of the plugin for the global planner to use with move_base, see pluginlib documentation for more details on plugins. This plugin must adhere to the nav_core::BaseGlobalPlanner interface specified in the nav_core package. (1.0 series default: "NavfnROS")

  • base_local_planner (string, default: "base_local_planner/TrajectoryPlannerROS" For 1.1+ series)

    The name of the plugin for the local planner to use with move_base see pluginlib documentation for more details on plugins. This plugin must adhere to the nav_core::BaseLocalPlanner interface specified in the nav_core package. (1.0 series default: "TrajectoryPlannerROS")

  • recovery_behaviors (list, default: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: rotate_recovery, type: rotate_recovery/RotateRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}] For 1.1+ series)

    A list of recovery behavior plugins to use with move_base, see pluginlib documentation for more details on plugins. These behaviors will be run when move_base fails to find a valid plan in the order that they are specified. After each behavior completes, move_base will attempt to make a plan. If planning is successful, move_base will continue normal operation. Otherwise, the next recovery behavior in the list will be executed. These plugins must adhere to the nav_core::RecoveryBehavior interface specified in the nav_core package.

    • name: conservative_reset, type: ClearCostmapRecovery

    • name: rotate_recovery, type: RotateRecovery

    • name: aggressive_reset, type: ClearCostmapRecovery

      Note: For the default parameters, the aggressive_reset behavior will clear out to a distance of 4 * ~/local_costmap/circumscribed_radius.

  • controller_frequency (double, default: 20.0)

    The rate in Hz at which to run the control loop and send velocity commands to the base. 控制频率会影响小车移动速度和控制效果,teb算法中dt_ref与该参数有关,同时设置频率不一定为能达到的频率,无法达到则会报警告,然后重新调整该参数。

  • planner_patience (double, default: 5.0)
    How long the planner will wait in seconds in an attempt to find a valid plan before space-clearing operations are performed.

  • controller_patience (double, default: 15.0)

    How long the controller will wait in seconds without receiving a valid control before space-clearing operations are performed.

  • conservative_reset_dist (double, default: 3.0)

    The distance away from the robot in meters beyond which obstacles will be cleared from the costmap when attempting to clear space in the map. Note, this parameter is only used when the default recovery behaviors are used for move_base.

  • recovery_behavior_enabled (bool, default: true)

    Whether or not to enable the move_base recovery behaviors to attempt to clear out space.

  • clearing_rotation_allowed (bool, default: true)

    Determines whether or not the robot will attempt an in-place rotation when attempting to clear out space. Note: This parameter is only used when the default recovery behaviors are in use, meaning the user has not set the recovery_behaviors parameter to anything custom.

  • shutdown_costmaps (bool, default: false)

    Determines whether or not to shutdown the costmaps of the node when move_base is in an inactive state

  • oscillation_timeout (double, default: 0.0)

    How long in seconds to allow for oscillation before executing recovery behaviors. A value of 0.0 corresponds to an infinite timeout. New in navigation 1.3.1

  • oscillation_distance (double, default: 0.5)

    How far in meters the robot must move to be considered not to be oscillating. Moving this far resets the timer counting up to the ~oscillation_timeout New in navigation 1.3.1

  • planner_frequency (double, default: 0.0)

    The rate in Hz at which to run the global planning loop. If the frequency is set to 0.0, the global planner will only run when a new goal is received or the local planner reports that its path is blocked. New in navigation 1.6.0

  • max_planning_retries (int32_t, default: -1)

    How many times to allow for planning retries before executing recovery behaviors. A value of -1.0 corresponds to an infinite retries.

基础应用

move_base提供action和topic两种形式的调用

设置action

//load MoveBaseAction,and wait for the action server to come up
MoveBaseClient ac("move_base", true);
while(!ac.waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for the move_base action server to come up");}

定点运动

  move_base_msgs::MoveBaseGoal goal;

  goal.target_pose.header.frame_id = "map";//global goal
  goal.target_pose.header.stamp = ros::Time::now();

  goal.target_pose.pose.position.x = 1;
  goal.target_pose.pose.position.y = 1;
  goal.target_pose.pose.position.z = 0.0;
  goal.target_pose.pose.orientation.x = 0.0;
  goal.target_pose.pose.orientation.y = 0.0;
  goal.target_pose.pose.orientation.z = 0.0;
  goal.target_pose.pose.orientation.w = 1.0;

  ROS_INFO("Sending goal");
  ac.sendGoal(goal);

  ac.waitForResult();

取消运动

ac.cancelAllGoals();

ac.waitForResult();
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
	ROS_INFO("succeeded");
else
	ROS_INFO("fall");

三、move_base_flex

Move Base Flex (MBF), a backwards-compatible replacement for move_base. MBF can use existing plugins for move_base, and provides an enhanced version of the same ROS interface. It exposes action servers for planning, controlling and recovering, providing detailed information of the current state and the plugin’s feedback. An external executive logic can use MBF and its actions to perform smart and flexible navigation strategies.

move_base_flex是对move_base的加强,使用和move_base相同的plugin接口的同时拓展了接口virtual函数,保留了move_base相关的topic和servers,同时提供各独立功能的action和状态检测相关的servers,可使用SMACH或者BehaviourTrees灵活组合调用各功能。
移动机器人——ros navigation_第2张图片

基于状态机实现的move_base原始的任务决策

移动机器人——ros navigation_第3张图片

Action

  • MoveBase

    # Extension of move_base_msgs/MoveBase action, with more detailed result
    # and feedback and the possibility to specify lists of applicable plugins
    
    geometry_msgs/PoseStamped target_pose
    
    # Controller to use; defaults to the first one specified on "controllers" parameter
    string controller
    
    # Planner to use; defaults to the first one specified on "planners" parameter
    string planner
    
    # Recovery behaviors to try on case of failure; defaults to the "recovery_behaviors" parameter value
    string[] recovery_behaviors
    
    ---
    
    # Predefined success codes:
    uint8 SUCCESS        = 0
    
    # Predefined general error codes:
    uint8 FAILURE        = 10
    uint8 CANCELED       = 11
    uint8 COLLISION      = 12
    uint8 OSCILLATION    = 13
    uint8 START_BLOCKED  = 14
    uint8 GOAL_BLOCKED   = 15
    uint8 TF_ERROR       = 16
    uint8 INTERNAL_ERROR = 17
    # 21..49 are reserved for future general error codes
    
    # Planning/controlling failures:
    uint8 PLAN_FAILURE   = 50
    # 51..99 are reserved as planner specific errors
    
    uint8 CTRL_FAILURE   = 100
    # 101..149 are reserved as controller specific errors
    
    uint32 outcome
    string message
    
    # Configuration upon action completion
    float32 dist_to_goal
    float32 angle_to_goal
    geometry_msgs/PoseStamped final_pose
    
    ---
    
    # Outcome of most recent controller cycle. Same values as in MoveBase or ExePath result.
    uint32 outcome
    string message
    
    float32 dist_to_goal
    float32 angle_to_goal
    geometry_msgs/PoseStamped current_pose
    geometry_msgs/TwistStamped last_cmd_vel  # last command calculated by the controller
    
  • ExePath

    # Follow the given path until completion or failure
    
    nav_msgs/Path path
    
    # Controller to use; defaults to the first one specified on "controllers" parameter
    string controller
    
    # use different slots for concurrency
    uint8 concurrency_slot
    
    # define goal tolerance for the action
    bool tolerance_from_action
    float32 dist_tolerance
    float32 angle_tolerance
    ---
    
    # Predefined success codes:
    uint8 SUCCESS         = 0
    # 1..9 are reserved as plugin specific non-error results
    
    # Predefined error codes:
    uint8 FAILURE         = 100  # Unspecified failure, only used for old, non-mfb_core based plugins
    uint8 CANCELED        = 101
    uint8 NO_VALID_CMD    = 102
    uint8 PAT_EXCEEDED    = 103
    uint8 COLLISION       = 104
    uint8 OSCILLATION     = 105
    uint8 ROBOT_STUCK     = 106
    uint8 MISSED_GOAL     = 107
    uint8 MISSED_PATH     = 108
    uint8 BLOCKED_PATH    = 109
    uint8 INVALID_PATH    = 110
    uint8 TF_ERROR        = 111
    uint8 NOT_INITIALIZED = 112
    uint8 INVALID_PLUGIN  = 113
    uint8 INTERNAL_ERROR  = 114
    uint8 OUT_OF_MAP      = 115  # The start and / or the goal are outside the map
    uint8 MAP_ERROR       = 116  # The map is not running properly
    uint8 STOPPED         = 117  # The controller execution has been stopped rigorously.
    
    # 121..149 are reserved as plugin specific errors
    
    uint32 outcome
    string message
    
    geometry_msgs/PoseStamped  final_pose
    float32 dist_to_goal
    float32 angle_to_goal
    
    ---
    
    # Outcome of most recent controller cycle. Same values as in result
    uint32 outcome
    string message
    
    float32 dist_to_goal
    float32 angle_to_goal
    geometry_msgs/PoseStamped  current_pose
    geometry_msgs/TwistStamped last_cmd_vel  # last command calculated by the controller
    
  • GetPath

    # Get a path from start_pose or current position to the target pose
    
    # Use start_pose or current position as the beginning of the path
    bool use_start_pose
    
    # The start pose for the path; optional, used if use_start_pose is true
    geometry_msgs/PoseStamped start_pose
    
    # The pose to achieve with the path
    geometry_msgs/PoseStamped target_pose
    
    # If the goal is obstructed, how many meters the planner can relax the constraint in x and y before failing
    float64 tolerance
    
    # Planner to use; defaults to the first one specified on "planners" parameter
    string planner
    
    # use different slots for concurrency
    uint8 concurrency_slot
    
    ---
    
    # Predefined success codes:
    uint8 SUCCESS         = 0
    # 1..9 are reserved as plugin specific non-error results
    
    # Possible error codes:
    uint8 FAILURE         = 50  # Unspecified failure, only used for old, non-mfb_core based plugins
    uint8 CANCELED        = 51  # The action has been canceled by a action client
    uint8 INVALID_START   = 52  #
    uint8 INVALID_GOAL    = 53
    uint8 NO_PATH_FOUND   = 54
    uint8 PAT_EXCEEDED    = 55
    uint8 EMPTY_PATH      = 56
    uint8 TF_ERROR        = 57
    uint8 NOT_INITIALIZED = 58
    uint8 INVALID_PLUGIN  = 59
    uint8 INTERNAL_ERROR  = 60
    uint8 OUT_OF_MAP      = 61
    uint8 MAP_ERROR       = 62
    uint8 STOPPED         = 63  # The planner execution has been stopped rigorously.
    
    # 71..99 are reserved as plugin specific errors
    
    uint32 outcome
    string message
    
    nav_msgs/Path path
    
    float64 cost
    
    ---
    
  • Recovery

    # Run one of the recovery behavior listed on recovery_behaviors parameter
    
    string behavior
    
    # use different slots for concurrency
    uint8 concurrency_slot
    
    ---
    
    # Predefined success codes:
    uint8 SUCCESS         = 0
    
    # Possible server codes:
    uint8 FAILURE         = 150
    uint8 CANCELED        = 151
    uint8 PAT_EXCEEDED    = 152
    uint8 TF_ERROR        = 153
    uint8 NOT_INITIALIZED = 154
    uint8 INVALID_PLUGIN  = 155
    uint8 INTERNAL_ERROR  = 156
    uint8 STOPPED         = 157  # The recovery behaviour execution has been stopped rigorously.
    uint8 IMPASSABLE      = 158  # Further execution would lead to a collision
    
    # 171..199 are reserved as plugin specific errors
    
    uint32 outcome
    string message
    string used_plugin
    
    ---
    

servers

  • CheckPath

    uint8                      LOCAL_COSTMAP  = 1
    uint8                      GLOBAL_COSTMAP = 2
    
    nav_msgs/Path              path              # the path to be checked after transforming to costmap frame
    float32                    safety_dist       # minimum distance allowed to the closest obstacle (footprint padding)
    float32                    lethal_cost_mult  # cost multiplier for cells marked as lethal obstacle (zero is ignored)
    float32                    inscrib_cost_mult # cost multiplier for cells marked as inscribed obstacle (zero is ignored)
    float32                    unknown_cost_mult # cost multiplier for cells marked as unknown space (zero is ignored)
    uint8                      costmap           # costmap in which to check the pose
    uint8                      return_on         # abort check on finding a pose with this state or worse (zero is ignored)
    uint8                      skip_poses        # skip this number of poses between checks, to speedup processing
    bool                       path_cells_only   # check only cells directly traversed by the path, ignoring robot footprint
                                                 # (if true, safety_dist is ignored)
    ---
    uint8                      FREE      =  0    # robot is completely in traversable space
    uint8                      INSCRIBED =  1    # robot is partially in inscribed space
    uint8                      LETHAL    =  2    # robot is partially in collision
    uint8                      UNKNOWN   =  3    # robot is partially in unknown space
    uint8                      OUTSIDE   =  4    # robot is completely outside the map
    
    uint32                     last_checked      # index of the first pose along the path with return_on state or worse
    uint8                      state             # path worst state (until last_checked): FREE, INFLATED, LETHAL, UNKNOWN...
    uint32                     cost              # cost of all cells traversed by path within footprint padded by safety_dist
                                                 # or just by the path if path_cells_only is true
    
  • CheckPoint

    uint8                      LOCAL_COSTMAP  = 1
    uint8                      GLOBAL_COSTMAP = 2
    
    geometry_msgs/PointStamped point             # the point to be checked after transforming to costmap frame
    uint8                      costmap           # costmap in which to check the point
    ---
    uint8                      FREE      =  0    # point is in traversable space
    uint8                      INSCRIBED =  1    # point is in inscribed space
    uint8                      LETHAL    =  2    # point is in collision
    uint8                      UNKNOWN   =  3    # point is in unknown space
    uint8                      OUTSIDE   =  4    # point is outside the map
    
    uint8                      state             # point state: FREE, INFLATED, LETHAL, UNKNOWN or OUTSIDE
    uint32                     cost              # cost of the cell at point
    
  • CheckPose

    uint8                      LOCAL_COSTMAP  = 1
    uint8                      GLOBAL_COSTMAP = 2
    
    geometry_msgs/PoseStamped  pose              # the pose to be checked after transforming to costmap frame
    float32                    safety_dist       # minimum distance allowed to the closest obstacle
    float32                    lethal_cost_mult  # cost multiplier for cells marked as lethal obstacle (zero is ignored)
    float32                    inscrib_cost_mult # cost multiplier for cells marked as inscribed obstacle (zero is ignored)
    float32                    unknown_cost_mult # cost multiplier for cells marked as unknown space (zero is ignored)
    uint8                      costmap           # costmap in which to check the pose
    bool                       current_pose      # check current robot pose instead (ignores pose field)
    ---
    uint8                      FREE      =  0    # robot is completely in traversable space
    uint8                      INSCRIBED =  1    # robot is partially in inscribed space
    uint8                      LETHAL    =  2    # robot is partially in collision
    uint8                      UNKNOWN   =  3    # robot is partially in unknown space
    uint8                      OUTSIDE   =  4    # robot is completely outside the map
    
    uint8                      state             # pose state: FREE, INFLATED, LETHAL, UNKNOWN or OUTSIDE
    uint32                     cost              # total cost of all cells within footprint padded by safety_dist
    

四、map_server

用于加载和保存二维地图信息。

地图格式

地图包含两部分,地图文件和地图描述yaml

Image format

通过SDL_Image读取,支持多种格式,map_server保存默认生成pgm(P5,灰度图,二进制)格式。

YAML format

image: testmap.pgm
resolution: 0.1
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
  • image : 照片路径,绝对路径或者YAML文件所在的相对路径
  • resolution : 分辨率,米/像素
  • origin :图片最左下角像素在地图中的2d位置 ( x , y , y a w ) (x,y,yaw) (x,y,yaw),yaw为逆时针偏转,当yaw=0时没有偏转,许多系统会忽略yaw。
  • occupied_thresh:大于这个阀值的占用概率的像素被认为occupied
  • free_thresh:小于这个阀值的占用概率的像素被认为是free
  • negate :是否对white/black free/occupied定于的语义进行翻转,对occupied_thresh/free_thresh无影响。
  • mode(Optional):有三种模式可选trinary, scale, raw,默认trinary

p为占用概率,x为像素值(白255,黑0)

  • negate为false时, p = 255 − x 255 p=\frac{255-x}{255} p=255255x

  • negate为false时, p = x 255 p=\frac{x}{255} p=255x

在Trinary模式下:

  • If p > occupied_thresh, output the value 100 to indicate the cell is occupied.
  • If p < free_thresh, output the value 0 to indicate the cell is free.
  • Otherwise, output -1 a.k.a. 255 (as an unsigned char), to indicate that the cell is unknown.

命令行工具

map_server 加载地图

rosrun map_server map_server mymap.yaml
  • Published Topics

    map_metadata (nav_msgs/MapMetaData)

    • Receive the map metadata via this latched topic.

    map (nav_msgs/OccupancyGrid)

    • Receive the map via this latched topic.
  • Services

    static_map (nav_msgs/GetMap)

    • Retrieve the map via this service.
  • Parameters

    ~frame_id (string, default: "map")

    • The frame to set in the header of the published map.

map_saver 保存地图

rosrun map_server map_saver [--occ <threshold_occupied>] [--free <threshold_free>] [-f <mapname>] map:=/your/costmap/topic
  • mapname:地图储存的路径和名字
  • map:地图话题,默认为map

使用

rosrun map_server map_saver -f mymap

五、Planner

global_planner

  • navfn 默认使用,但其A*算法存在BUG

  • global_planner 修正了BUG,为navfn替代版

    使用(安装ros-melodic-global-planner):

    >
    
  • nav2_smac_planner navigation2的新的规划算法

local_planner

  • dwa_local_planner 默认规划器
  • teb_local_planner 测试效果最好,但参数较多,配置复杂

六、recovery

移动机器人——ros navigation_第4张图片
move_base节点可以在机器人认为自己被卡住时选择性地执行恢复行为。默认情况下, move_base 节点将采取以下操作来清除空间:

地图基于特定策略计算概率进行更新,参考《概率机器人》,地图部分只有在获得数据的情况下才会更新,因此某个区域识别到障碍物后,即使物体已经移动,若不重新扫描,该区域依然存在障碍物,同时因策略的不同,在识别到障碍物后的更新速度和效果有可能会与理想状态不一致。

static即现有地图障碍物不会改变,上述全为非现有地图障碍物。

  • 从地图上清除用户指定区域以外的障碍物
  • 原地旋转清除周围障碍物
  • 清除矩形区域之外的所有障碍(在这个区域内机器人可以原地旋转)
  • 再一次原地旋转清除周围障碍物

如果所有这些都失败了,机器人将认为它的目标是不可行的,并通知用户它已经中止。

可以使用recovery_behaviour参数配置这些恢复行为,并使用 recovery_behavior_enabled 参数禁用这些恢复行为。

navigation包中有3个recovery相关:

  • clear_costmap_recovery:实现了清除代价地图的恢复行为

    遍历所有层,然后如果某层在可清理列表里就清理掉它的 costmap 。默认可清理列表里只有 obstacle layer ,也就是实时扫描建立的 costmap 。

  • rotate_recovery:实现了旋转的恢复行为, 360度旋转来尝试清除空间

    转一圈看看有没有路。在 runBehavior 里只需要发指令让小车转一圈,有没有路是 local costmap 在转一圈过程中建立发现的。

  • move_slow_and_clear:实现了缓慢移动的恢复行为

    清理 costmap 然后什么都不管,按照前进速度和转角速度走。从代码里可以看到,根据指定的距离,这是通过先清除全局 costmap 跟局部 costmap 一圈的 obstacle layer 的障碍,然后直接发指令实现的。由于只清除了 obstacle layer ,其实 static layer 的障碍还在,而且这里清不清除跟发指令关系不大,该撞上去的还是会撞的,相当于闭着眼睛往前走。

参考

ros navigation stack源码学习

turtlebot3-github

创客智造-navigation

navigation

robot_localization Map, Odom, and drift.

ROS: global_planner 整体解析

mpc_local_planner

ROS基础教程–CostMap_2D包的一些理解

Basic Navigation Tuning Guide

通用参数配置文件costmap_common_params.yaml 导航配置文件参数详细解析 (二)

ROS : Navigation - recovery behavior 源码分析

你可能感兴趣的:(移动机器人,ROS,自动驾驶,人工智能,机器学习)