对于ROS小车导航算法基本原理和使用方法,可以看笔记http://t.csdn.cn/NUWHt
1 启动小车导航节点:turn_on_wheeltec_robot navigation.launch
<launch>
 <!-- 开启机器人底层相关节点 同时开启导航功能-->
 <include file="$(find turn_on_wheeltec_robot)/launch/turn_on_wheeltec_robot.launch">
    <arg name="navigation"  default="true"/>
 </include>
 <!-- turn on lidar开启雷达  -->
 <include file="$(find turn_on_wheeltec_robot)/launch/wheeltec_lidar.launch" />
 <!-- 设置需要用于导航的地图  -->
 <arg name="map_file" default="$(find turn_on_wheeltec_robot)/map/WHEELTEC.yaml"/>
 <node name="map_server_for_test" pkg="map_server" type="map_server" args="$(arg map_file)">
 </node>
 <!-- 开启用于导航的自适应蒙特卡洛定位amcl-->
 <include file="$(find turn_on_wheeltec_robot)/launch/include/amcl.launch" />
 <!-- MarkerArray功能节点> -->
 <node name='send_mark' pkg="turn_on_wheeltec_robot" type="send_mark.py">
 </node>
</launch>
在launch文件中打开了以下launch文件:
 1 turn_on_wheeltec_robot.launch
该文件启动了小车下位机电机控制节点,小车urdf文件及TF坐标,扩张卡尔曼滤波算法(robot_pose_ekf.launch 用于处理里程计信号),和建图导航算法
2 wheeltec_lidar.launch
该文件启动激光雷达相关功能
3 /map/WHEELTEC.yaml
该文件为地图的yaml文件,在小车建图中创建,这里我们默认打开文件名WHEELTEC,如果要切换导航地图可以在这里修改文件名
4 amcl.launch
该文件启动amcl定位算法,在launch/include 文件夹里的amcl.launch文件:
<launch>
    <!-- <arg name="set_initpose" default="true"> -->
    <!-- 使用map的topic而不是map文件-->
    <arg name="use_map_topic" default="false"/>
    <!-- 不可以使用PointCloud,只能使用LaserScan -->
    <arg name="scan_topic" default="scan"/>
    <arg name="map_topic"  default="/map"/>
    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
      <!-- <rosparam command="load" file="$(find turn_on_wheeltec_robot)/map/WHEELTEC_end.yaml" />-->
      <param name="use_map_topic" value="$(arg use_map_topic)"/>
      <!-- 使用rtabmap分布的Map,连接TF树 -->
      <remap from="/map" to="/$(arg map_topic)"/>
      <!-- Publish scans from best pose at a max of 10 Hz -->
      <param name="odom_model_type" value="omni-corrected"/>
      <!-- <param name="odom_model_type" value="omnid"/> -->
      <!-- <param name="odom_model_type" value="diff"/> -->
      <!-- <param name="odom_model_type" value="diff-corrected"/> -->
      <param name="odom_alpha5" value="0.1"/>
      <param name="gui_publish_rate" value="10.0"/>
      <param name="laser_max_beams" value="60"/>
      <param name="laser_max_range" value="12.0"/>
      <param name="min_particles" value="500"/>
      <param name="max_particles" value="2000"/>
      <param name="kld_err" value="0.05"/>
      <param name="kld_z" value="0.99"/>
      <param name="odom_alpha1" value="0.2"/>
      <param name="odom_alpha2" value="0.2"/>
      <!-- translation std dev, m -->
      <param name="odom_alpha3" value="0.2"/>
      <param name="odom_alpha4" value="0.2"/>
      <param name="laser_z_hit" value="0.5"/>
      <param name="laser_z_short" value="0.05"/>
      <param name="laser_z_max" value="0.05"/>
      <param name="laser_z_rand" value="0.5"/>
      <param name="laser_sigma_hit" value="0.2"/>
      <param name="laser_lambda_short" value="0.1"/>
      <param name="laser_model_type" value="likelihood_field"/>
      <!-- <param name="laser_model_type" value="beam"/> -->
      <param name="laser_likelihood_max_dist" value="2.0"/>
      <param name="update_min_d" value="0.25"/>
      <param name="update_min_a" value="0.2"/>
      <param name="odom_frame_id" value="odom_combined"/>
      <param name="resample_interval" value="1"/>
      <!-- Increase tolerance because the computer can get quite busy -->
      <param name="transform_tolerance" value="1.0"/>
      <param name="recovery_alpha_slow" value="0.0"/>
      <param name="recovery_alpha_fast" value="0.0"/>
      <remap from="scan" to="$(arg scan_topic)"/>
    </node>
</launch>
可以看到这里有大量和算法相关的参数可以进行修改,之后了解算法原理后可以根据实际情况对参数进行调整
代价地图配置文件
 代价地图有三个配置文件costmap_common_params.yaml,global_costmap_params.yaml,和local_costmap_params.yaml。其中costmap_common_params.yaml配置的是代价地图总体参数。后两个分别为全局和局域的代价地图参数。
在启动导航时一般先加载costmap_common_params文件,后加载local_costmap_params和global_costmap_params。对于相同的参数后加载的文件会覆盖先加载的文件。
costmap_common_params.yaml
#代价地图官方介绍:http://wiki.ros.org/costmap_2d
#静态层官方介绍:http://wiki.ros.org/costmap_2d/hydro/staticmap
#障碍层官方介绍:http://wiki.ros.org/costmap_2d/hydro/obstacles
#设置静态层参数
static_layer:
  #是否开启静态层
  enabled: true
  #静态层的订阅的地图话题
  map_topic: map
  #地图话题中数据值为多少,会转换为静态层代价地图中的未知区域
  unknown_cost_value: -1
  #地图话题中数据值为多少,会转换为静态层代价地图中的完全占用区域
  lethal_cost_threshold: 100
  #是否仅把第一次订阅到的地图数据转换为静态层代价地图,无视后续订阅到的地图数据
  first_map_only: false
  #是否订阅话题 “map_topic”+“_updates”
  subscribe_to_updates: false
  #如果设置为false,地图话题中的未知区域在代价地图中会转换为自由区域
  track_unknown_space: true
  #如果设置为true,静态层代价地图只有未知、自由和完全占用三种情况
  #如果设置为false,静态层代价地图可以有不同的占用程度
  trinary_costmap: true
#设置障碍层参数
obstacle_layer:
  #是否开启障碍层
  enabled: true
  #设置障碍层的观测源名称,可以一次设置多个观测源observation_sources: scan, scan2, camera
  observation_sources: scan
  #设置对应观测源参数
  scan:
    #观测源数据话题名称
    topic: scan
    #观测源的TF坐标名称,如果设置为空,会自动从话题数据寻找TF坐标名称
    #以下三种数据格式支持自动寻找TF坐标名称
    #sensor_msgs/LaserScan, sensor_msgs/PointCloud, and sensor_msgs/PointCloud2
    sensor_frame: laser
    #观测源话题的数据格式,可以为LaserScan、PointCloud、PointCloud2
    data_type: LaserScan
    #保留多久时间内的全部话题数据作为障碍层输入,设置为0代表只保留最近的一帧数据,单位:s
    observation_persistence: 0.0
    #读取观测源话题的频率,如果进行设置,频率应该设置的比传感器频率低一些。默认0,代表允许观测源一直不发布话题。单位:Hz
    expected_update_rate: 0.0
    #是否使用该观测源清除自由空间
    clearing: true
    #是否使用该观测源添加障碍物
    marking: true
    #高于多少的障碍物不加入观测范围,单位:m
    max_obstacle_height: 2.0
    #低于多少的障碍物不加入观测范围,单位:m
    min_obstacle_height: 0.0
    #多少范围内障碍物会被加入代价地图,单位:m
    obstacle_range: 2.5
    #多少范围内障碍物会被追踪,单位:m
    raytrace_range: 3.0
  #在观测源基础上再次进行设置的参数
  #高于多少的障碍物不加入观测范围,单位:m
  max_obstacle_height:  2.0
  #多少范围内障碍物会被加入代价地图,单位:m
  obstacle_range: 2.5
  #多少范围内障碍物会被追踪,单位:m
  raytrace_range: 3.0
  #如果设置为true,障碍层代价地图会有未知、自由和完全占用三种情况
  #如果设置为false,障碍层代价地图只有自由和完全占用两种情况
  track_unknown_space: true
  #障碍层如何与其它地图层处理的方法。
  #0:障碍层覆盖其它地图层; 1:障碍物最大化方法,即各层的占用方格会覆盖其它层的自由方格,这是最常用的方法
  #99:不改变其它地图层,应该是使障碍层层无效的方法
  combination_method: 1
  #如果障碍层类型是"costmap_2d::VoxelLayer",可以对以下参数进行设置
  #代价地图的高度
  #origin_z: 0.0
  #障碍层的Z轴方格的高度
  #z_resolution: 0.2
  #障碍层Z轴上有几个方格
  #z_voxels: 10
  #被认为是“已知”的列中允许的未知单元格数
  #unknown_threshold: 15
  #被认为是“自由”的列中允许的标记单元格数
  #mark_threshold: 0
  #是否发布障碍层的投影地图层话题
  #publish_voxel_map: false
  #如果设置为true,机器人将把它所经过的空间标记为自由区域
  #footprint_clearing_enabled: true
在这里有三个图层可以配置:静态层(static_layer),障碍层(obstacle_layer),和膨胀层(inflation_layer 这里没有配置)。这三个图层类似于PS里面的图层,会一起被用于导航
静态层:预先导入的SLAM地图
 膨胀层:障碍物周围的膨胀区域
 障碍层:实时检测到的障碍物
global_costmap_params.yaml
#代价地图官方介绍:http://wiki.ros.org/costmap_2d
#全局代价地图参数命名空间
global_costmap:
  #代价地图的TF参考坐标系
  global_frame: map 
  #机器人的TF坐标名称
  robot_base_frame: base_footprint
  #global_frame和robot_base_frame间的TF坐标停止发布多久后,控制机器人停止,单位:s
  transform_tolerance: 1
  #代价地图刷新频率,单位:Hz
  update_frequency: 1.5
  #代价地图的可视化话题发布频率,单位:Hz
  publish_frequency: 1.0
  #是否直接使用静态地图生成代价地图
  #static_map: true #使用plugins手动配置代价地图时,该参数无效
  #代价地图是否跟随机器人移动,static_map为true时该参数必须为false
  rolling_window: false
  #代价地图宽度,这里会被静态层扩宽,单位:m
  width: 10.0 
  #代价地图高度,这里会被静态层扩宽,单位:m
  height: 10.0 
  #代价地图分辨率(米/单元格)
  resolution: 0.05
  
  #为代价地图设置地图层,这里设置了三层,分别作为静态层、障碍层和膨胀层
  plugins:
    #定义地图层的名称,设置地图层的类型
    - {name: static_layer,      type: "costmap_2d::StaticLayer"}
    #定义地图层的名称,设置地图层的类型。
    #障碍层可以使用VoxelLayer代替ObstacleLayer
    - {name: obstacle_layer,    type: "costmap_2d::VoxelLayer"}
    #定义地图层的名称,设置地图层的类型
    - {name: inflation_layer,   type: "costmap_2d::InflationLayer"}
  #各地图层的参数,会以地图层名称作为命名空间
  #各地图层的参数,会在【costmap_common_params.yaml】内进行设置
这里配置了全局代价地图的参数,包括了代价地图刷新率(update_frequency),地图话题发布频率(publish_frequency),以及该地图中包含哪几个图层,如这里全局代价地图包括了static_layer,obstracle_layer,inflation_layer这三个层。
局部代价地图配置文件和全局代价地图基本一样,这里不再赘述
路径规划器teb_local_planner.launch。teb路径规划器在turn_on_wheeltec_robot.launch文件中被启动
<launch>
  <!-- Arguments参数 -->
  <arg name="car_mode"  default="" />
  <!-- 导航路径规划公共参数 -->
  <rosparam file="$(find turn_on_wheeltec_robot)/params_nav_common/move_base_params.yaml"          command="load" ns="move_base"/>
  <rosparam file="$(find turn_on_wheeltec_robot)/params_nav_common/base_global_planner_param.yaml" command="load" ns="move_base"/>
  <!-- 导航TEB局部路径规划器参数 -->
  <param name="move_base/base_local_planner"    type="string" value="teb_local_planner/TebLocalPlannerROS"/>
  <rosparam file="$(find turn_on_wheeltec_robot)/params_nav_common/teb_local_planner_params.yaml"  command="load" ns="move_base"/>
  <!-- 导航代价地图公共参数 -->
  <rosparam file="$(find turn_on_wheeltec_robot)/params_costmap_common/costmap_common_params.yaml"     command="load" ns="move_base/global_costmap"/>
  <rosparam file="$(find turn_on_wheeltec_robot)/params_costmap_common/costmap_common_params.yaml"     command="load" ns="move_base/local_costmap"/>
  <rosparam file="$(find turn_on_wheeltec_robot)/params_costmap_common/local_costmap_params.yaml"      command="load" ns="move_base"/>
  <rosparam file="$(find turn_on_wheeltec_robot)/params_costmap_common/global_costmap_params.yaml"     command="load" ns="move_base"/>
  <!-- 启动导航节点 -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <!-- 导航代价地图与路径规划器对应车型参数 -->
    <rosparam file="$(find turn_on_wheeltec_robot)/params_costmap_car/param_$(arg car_mode)/teb_local_planner_params.yaml"  command="load"/>
    <rosparam file="$(find turn_on_wheeltec_robot)/params_costmap_car/param_$(arg car_mode)/costmap_car_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find turn_on_wheeltec_robot)/params_costmap_car/param_$(arg car_mode)/costmap_car_params.yaml" command="load" ns="local_costmap" />
  </node>
</launch>
在该launch文件中调用了全局与具有代价地图,并且引入了参数文件teb_local_planner_params.yaml。该文件配置了teb路径规划器的各类相关参数。
teb_local_planner_params.yaml (位置:turn_on_wheeltec_robot/params_nav_common/teb_local_planner_params.yaml)
#TebLocalPlanner官方介绍:http://wiki.ros.org/teb_local_planner
#局部路径规划器DWAPlannerROS命名空间
TebLocalPlannerROS:
  odom_topic: odom #订阅的里程计话题
  map_frame: map #代价地图的TF参考坐标系
  #障碍物参数
  min_obstacle_dist: 0.1 #和障碍物最小距离,直接影响机器人避障效果
  include_costmap_obstacles: True #是否将动态障碍物预测为速度模型,
  costmap_obstacles_behind_robot_dist: 1.5 #限制机器人后方规划时考虑的局部成本地图障碍物
  obstacle_poses_affected: 15  #障碍物姿态受影响0~30
  costmap_converter_plugin: ""
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5
  include_dynamic_obstacles: True 
  dynamic_obstacle_inflation_dist: 0.6
  #目标点误差允许值
  xy_goal_tolerance: 0.2  #机器人到达目标点时附近时的弧度偏差允许量,在该偏差内认为已经到达目标点,单位为:m
  yaw_goal_tolerance: 0.1 #机器人到达目标点时附近时的弧度偏差允许量,在该偏差内认为已经到达目标点单位为:rad
  free_goal_vel: False    #允许机器人以最大速度驶向目的地
  #轨道配置参数
  teb_autosize: True #优化期间允许改变轨迹的时域长度
  dt_ref: 0.45 #局部路径规划的解析度# minimum 0.01
  dt_hysteresis: 0.1 #允许改变的时域解析度的浮动范围, 一般为 dt_ref 的 10% 左右 minimum0.002
  global_plan_overwrite_orientation: False #覆盖全局路径中局部路径点的朝向
  max_global_plan_lookahead_dist: 3.0 #考虑优化的全局计划子集的最大长度
  feasibility_check_no_poses: 5 #检测位姿可到达的时间间隔 minimum 0
  #轨迹优化参数
  no_inner_iterations: 5
  no_outer_iterations: 4
  optimization_activate: True
  optimization_verbose: False
  penalty_epsilon: 0.1
  obstacle_cost_exponent: 4
  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 #必须大于0
  weight_shortest_path: 0
  weight_obstacle: 100
  weight_inflation: 0.2
  weight_dynamic_obstacle: 10 
  weight_dynamic_obstacle_inflation: 0.2
  weight_viapoint: 1
  weight_adapt_factor: 2
  #不同拓扑中的并行规划
  enable_homotopy_class_planning: False
  enable_multithreading: True
  max_number_classes: 4
  selection_cost_hysteresis: 1.0
  selection_prefer_initial_plan: 0.95
  selection_obst_cost_scale: 1.0
  selection_alternative_time_cost: False
  roadmap_graph_no_samples: 15
  roadmap_graph_area_width: 5
  roadmap_graph_area_length_scale: 1.0
  h_signature_prescaler: 0.5
  h_signature_threshold: 0.1
  obstacle_heading_threshold: 0.45
  switching_blocking_period: 0.0
  viapoints_all_candidates: True
  delete_detours_backwards: True
  max_ratio_detours_duration_best_duration: 3.0
  visualize_hc_graph: False
  visualize_with_time_as_z_axis_scale: False
  #恢复行为
  shrink_horizon_backup: True
  shrink_horizon_min_duration: 10
  oscillation_recovery: False
  oscillation_v_eps: 0.1
  oscillation_omega_eps: 0.1
  oscillation_recovery_min_duration: 10
  oscillation_filter_duration: 10
导航系统完整架构:
 


















