ROS中的导航框架

move_base 导航框架

全局路径规划(global planner)

• 全局最优路径规划 · Dijkstra或A*算法

本地实时规划（local planner）

• 规划机器人每个周期内的线速度、角速度，使之尽量符合全局最优路径。 · 实时避障 · Trajectory Rollout和Dynamic Window Approaches算法 · 搜索躲避和行进的多条路径，综合评价标准选取最优路径

move_base功能包中的话题和服务

move_base配置

一.costmap代价地图

costmap为路径规划而存在,在ROS上是通过costmap_2d软件包实现,在原始地图上实现了两张新的地图.

1.local_costmap:局部路径规划准备的

2.global_costmap:全局路径规划准备

这两者可以配置多个图层:

• Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息
• Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的撞上障碍物。
• Static Map Layer：静态地图层，基本上不变的地图层，通常都是SLAM建立完成的静态地图。
• Other Layers：你还可以通过插件的形式自己实现costmap，目前已有Social Costmap Layer、Range Sensor Layer等开源插件。(这个不知道怎么用)

配置costmap相关的参数

<launch>
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <rosparam file="$(find dashgo_nav)/config/odom/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find dashgo_nav)/config/odom/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find dashgo_nav)/config/odom/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find dashgo_nav)/config/odom/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find dashgo_nav)/config/odom/base_global_planner_param.yaml" command="load" />

    <rosparam file="$(find dashgo_nav)/config/odom/teb_local_planner_params.yaml" command="load" />

     <param name="base_global_planner" value="global_planner/GlobalPlanner"/> 
     <param name="planner_frequency" value="1.0" />
     <param name="planner_patience" value="5.0" />

     <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
     <param name="controller_frequency" value="5.0" />
     <param name="controller_patience" value="15.0" />

  </node>
  
</launch>

启动move_base节点时,首先加载了costmap_common_params.yaml到global_costmap和local_costmap两个命名空间中(ns="xxx")表示命名空间,costmap_common_params.yaml是通用的代价地图配置参数,local_costmap和global_costmap都需要配置参数.

因此:

local_costmap_params.yaml:局部代价地图配置的参数

global_costmap_params.yaml:全局代价地图配置的参数

配置costmap_common_params.yaml

在config目录下，创建costmap_common_params.yaml文件，配置的参数如下：

footprint: [[0.20, -0.26], [0.20, 0.26],[-0.20, 0.26], [-0.20, -0.26]] 
#robot_radius: 0.16

obstacle_layer:
 enabled: true
 max_obstacle_height: 0.6
 min_obstacle_height: 0.0
 obstacle_range: 2.0     #导致障碍物被放入代价地图中,机器人只会更新其地图包含距离移动基座2.5米以内的障碍物的信息
 raytrace_range: 5.0     #清楚指定范围外的空间
 inflation_radius: 0.32   #代价地图的膨胀半径
 combination_method: 1
 observation_sources: laser_scan_sensor
 track_unknown_space: true
 laser_scan_sensor: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}

track_unknown_space: true

inflation_layer:
  enabled:              true
  cost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     0.32 # max. distance from an obstacle at which costs are incurred for planning paths.

static_layer:
  enabled:              true
  map_topic:            "/map"

参数解释:

• footprint :机器人底盘是方形,四个角的参数
• robot_radius:机器人底盘是圆形的,设置机器人底盘半径,单位:米

obstacle_layer：配置障碍物图层

enabled:是否启用该层

• combination_method：只能设置为0或1，用来更新地图上的代价值，一般设置为1;
• track_unknown_space:如果设置为false，那么地图上代价值就只分为致命碰撞和自由区域两种，如果设置为true，那么就分为致命碰撞，自由区域和未知区域三种。意思是说假如该参数设置为false的话，就意味着地图上的未知区域也会被认为是可以自由移动的区域，这样在进行全局路径规划时，可以把一些未探索的未知区域也来参与到路径规划，如果你需要这样的话就将该参数设置为false。不过一般情况未探索的区域不应该当作可以自由移动的区域，因此一般将该参数设置为true;
• obstacle_range:设置机器人检测障碍物的最大范围，意思是说超过该范围的障碍物，并不进行检测，只有靠近到该范围内才把该障碍物当作影响路径规划和移动的障碍物;
• raytrace_range:在机器人移动过程中，实时清除代价地图上的障碍物的最大范围，更新可自由移动的空间数据。假如设置该值为3米，那么就意味着在3米内的障碍物，本来开始时是有的，但是本次检测却没有了，那么就需要在代价地图上来更新，将旧障碍物的空间标记为可以自由移动的空间
• observation_sources:设置导航中所使用的传感器，这里可以用逗号形式来区分开很多个传感器，例如激光雷达，碰撞传感器，超声波传感器等，我这里只设置了激光雷达; data_type:激光雷达数据类型; topic:该激光雷达发布的话题名; marking:是否可以使用该传感器来标记障碍物; clearing:是否可以使用该传感器来清除障碍物标记为自由空间;
• max_obstacle_height:以米为单位插入costmap的任何障碍物的最大高度。此参数应设置为略高于机器人的高度

• min_obstacle_height:传感器读数的最小高度（以米为单位）视为有效。通常设置为地面高度。

 inflation_layer:膨胀层，用于在障碍物外标记一层危险区域，在路径规划时需要避开该危险区域

enabled:是否启用该层;

• cost_scaling_factor:膨胀过程中应用到代价值的比例因子，代价地图中到实际障碍物距离在内切圆半径到膨胀半径之间的所有cell可以使用如下公式来计算膨胀代价： exp(-1.0 * cost_scaling_factor * (distance_from_obstacle – inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE – 1) 公式中costmap_2d::INSCRIBED_INFLATED_OBSTACLE目前指定为254，注意： 由于在公式中cost_scaling_factor被乘了一个负数，所以增大比例因子反而会降低代价。
• inflation_radius:膨胀半径，膨胀层会把障碍物代价膨胀直到该半径为止，一般将该值设置为机器人底盘的直径大小。

· Static_layer:静态地图层，即SLAM中构建的地图层

enabled:是否启用该地图层;

二 .配置global_costmap_params.yaml

全局代价地图是作为进行全局路径规划时的参考，我们需要在config目录中，创建global_costmap_params.yaml文件，该文件是为全局代价地图配置的参数.

global_costmap:
   global_frame: /map
   robot_base_frame: /base_footprint
   update_frequency: 1.0    #代价地图更新的频率
   publish_frequency: 0     
   static_map: true   #true 表示有map_server提供的地图服务来进行代价地图初始化
   rolling_window: false  
   resolution: 0.05
   transform_tolerance: 1.0
   map_type: costmap

GlobalPlanner:        
   allow_unknown: false 

•  global_frame:全局代价地图需要在哪个坐标系下运行;
• robot_base_frame:在全局代价地图中机器人本体的基坐标系，就是机器人上的根坐标系。通过global_frame和robot_base_frame就可以计算两个坐标系之间的变换，得知机器人在全局坐标系中的坐标了
• update_frequency:全局代价地图更新频率，一般全局代价地图更新频率设置的比较小;
• static_map:配置是否使用map_server提供的地图来初始化，一般全局地图都是静态的，需要设置为true;
• rolling_window:是否在机器人移动过程中需要滚动窗口，始终保持机器人在当前窗口中心位置;
•  transform_tolerance:坐标系间的转换可以忍受的最大延时;
• transform_tolerance:坐标系间的转换可以忍受的最大延时;

三.配置local_costmap_params.yaml

局部代价地图配置参数所建立的地图主要是为局部路径规划所使用，我们可以在config目录下，创建local_costmap_params.yaml文件

local_costmap:
   global_frame: /odom   #本地代价地图应该运行的坐标系
   #global_frame: /odom_combined
   robot_base_frame: /base_footprint
   update_frequency: 3.0
   publish_frequency: 1.0  #参数确定代价地图发布可视化信息的速率
   static_map: false
   rolling_window: true   #当机器人移动时,保持机器人在本地代价地图点的中心
   width: 3.0
   height: 3.0
   resolution: 0.05
   transform_tolerance: 1.0
   map_type: costmap

   plugins:
   - {name: static_layer,        type: "costmap_2d::StaticLayer"}
   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
   - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}

参数解释:

• global_frame:在局部代价地图中的全局坐标系，一般需要设置为odom_frame；
•  robot_base_frame:机器人本体的基坐标系;
• update_frequency:局部代价地图的更新频率;
• publish_frequency:局部代价地图的发布频率;
• static_map:局部代价地图一般不设置为静态地图，因为需要检测是否在机器人附近有新增的动态障碍物;
• rolling_window:使用滚动窗口，始终保持机器人在当前局部地图的中心位置;
• width:滚动窗口的宽度，单位是米;
• height:滚动窗口的高度，单位是米;
•  resolution:地图的分辨率，该分辨率可以从加载的地图相对应的配置文件中获取到;
• transform_tolerance:局部代价地图中的坐标系之间转换的最大可忍受延时;

四.teb_local_planner_params.yaml参数的配置

teb_local_planner是局部规划器的plugin,是一个关于图优化的局部路径规划,就是把路径问题构建成一个优化问题.

TebLocalPlannerROS:

 odom_topic: odom
 #odom_topic: /robot_pose_ekf/odom_combined
 map_frame: /odom
 #map_frame: /odom_combined

 # Trajectory

 teb_autosize: True
 dt_ref: 0.45
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: True
 max_global_plan_lookahead_dist: 3.0
 feasibility_check_no_poses: 5

 # Robot
#机器人速度限制
 max_vel_x: 0.3
 max_vel_x_backwards: 0.15
 max_vel_theta: 0.4
 #定义机器人加速度限制
 acc_lim_x: 0.2
 acc_lim_theta: 0.2
 min_turning_radius: 0
 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   vertices: [[0.20, -0.26], [0.20, 0.26],[-0.20, 0.26], [-0.20, -0.26]]

 # GoalTolerance

 xy_goal_tolerance: 0.15  #XY地图平面容忍的最大误差
 yaw_goal_tolerance: 0.2   #航向角容忍的最大误差
 free_goal_vel: False

 # Obstacles

 min_obstacle_dist: 0.32
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0
 obstacle_poses_affected: 7
 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5

 # Optimization

 no_inner_iterations: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 weight_max_vel_x: 2
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet
 alternative_time_cost: False # not in use yet

 # Homotopy Class Planner

 enable_homotopy_class_planning: False
 enable_multithreading: True
 simple_exploration: False
 max_number_classes: 4
 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_keypoint_offset: 0.1
 obstacle_heading_threshold: 0.45
 visualize_hc_graph: False

teb_local_planner参数解释:

•  dt_ref:期望的轨迹时间分辨率
• dt_hysteresis: 根据当前时间分辨率自动调整大小的滞后现象，通常约为。建议使用dt ref的10%。
• global_plan_overwrite_orientation: True #覆盖由全局规划器提供的局部子目标的方向
• max_global_plan_lookahead_dist: 3.0 #指定考虑优化的全局计划子集的最大长度
• feasibility_check_no_poses: 5 #每个采样间隔的姿态可行性分析数，default：4
• min_turning_radius: 机器人最小转弯半径
• xy_goal_tolerance:目标位置的允许距离误差
• yaw_goal_tolerance:目标位置的允许角度误差
• free_goal_vel:去除目标速度的约束
• min_obstacle_dist:与障碍的最小期望距离,注意，teb_local_planner本身不考虑膨胀半径
• include_costmap_obstacles:应否考虑到局部costmap的障碍
• costmap_obstacles_behind_robot_dist:考虑后面n米内的障碍物
• obstacle_poses_affected:为了保持距离，每个障碍物位置都与轨道上最近的位置相连
• costmap_converter_plugin:定义插件名称，用于将costmap的单元格转换成点/线/多边形。若设置为空字符，则视为禁用转换，将所有点视为点障碍
• costmap_converter_spin_thread:如果为true，则costmap转换器将以不同的线程调用其回调队列, default:true
• costmap_converter_rate:定义costmap_converter插件处理当前costmap的频率（该值不高于costmap更新率)
• no_inner_iterations:在每个内循环迭代中调用的实际求解器迭代次数
• no_outer_iterations:在每个外循环迭代中调用的实际求解器迭代次数
• penalty_epsilon:为硬约束近似的惩罚函数添加一个小的安全范围
• weight_max_vel_x:满足最大允许平移速度的优化权重
• weight_max_vel_theta:满足最大允许旋转速度的优化权重
• weight_acc_lim_x:满足最大允许平移加速度的优化权重
• weight_acc_lim_theta:满足最大允许角加速度的优化权重。
• weight_kinematics_nh:运动学的优化权重
• weight_kinematics_forward_drive:强制机器人仅选择正向（正的平移速度）的优化权重。
• weight_kinematics_turning_radius:采用最小转向半径的优化权重
• weight_optimaltime:根据转换/执行时间对轨迹进行收缩的优化权重
• weight_obstacle:保持与障碍物的最小距离的优化权重
• weight_dynamic_obstacle:not in use yet
• alternative_time_cost:not in use yet
• enable_homotopy_class_planning:激活并行规划（因为一次优化多个轨迹，占用更多的CPU资源
• enable_multithreading:激活多个线程，以便在不同的线程中规划每个轨迹
• simple_exploration: False
• max_number_classes:考虑到的不同轨迹的最大数量
• roadmap_graph_no_samples:指定为创建路线图而生成的样本数
• roadmap_graph_area_width:指定该区域的宽度
• h_signature_prescaler:0.2 < value <= 1）缩放用于区分同伦类的内部参数（H-signature）。
• h_signature_threshold:如果实部和复部的差都低于规定的阈值，则假定两个h签名相等。
• obstacle_keypoint_offset:
• obstacle_heading_threshold:指定障碍标头和目标标头之间的标量积的值，以便将(障碍)考虑到勘探
• visualize_hc_graph:可视化创建的图形，用于探索不同的轨迹（在rviz中检查标记消息）