阿克曼结构移动机器人的gazebo仿真（八）

阿克曼结构移动机器人的gazebo仿真（八）

第八章、实现小车自主导航

0.前言

上一节通过配置小车的里程计，用gmapping算法建了房间的二维栅格地图；这一节通过配置AMCL定位以及move_base实现小车在房间中的自主导航。
在mini小车仿真的章节讲到导航主要分为机器人定位和路径规划两大部分。ROS分别提供了功能包
1.move_base:实现机器人导航中的最优路径规划。
2.amcl:实现二维地图中的机器人定位。
首先安装导航的功能包：

 sudo apt-get install ros-melodic-navigation

1.配置AMCL

在racebot_gazebo中创建amcl.launch文件，与里程计定位不同的是，amcl定位可以估算机器人在地图坐标系/map下的位姿信息，提供/base_footprint、/odom、/map之间的TF变换，因此关于上述话题的名称要与launch文件中对应。代码如下：

2.配置move_base

move_base是ROS中完成路径规划的功能包，主要由全局路径规划器以及本地实时规划器组成。实现机器人的导航需要上面的amcl做定位，以及通过加载move_base功能包配置代价地图与本地规划器实现来实现全局路径以及局部路径的规划。

配置代价地图

通用文件配置costmap_common_param.yaml

footprint: [[0.15, 0.15], [0.15, -0.15], [-0.15, -0.15], [-0.15, 0.15]] 

obstacle_range: 2.5
raytrace_range: 3.0

static_layer:
  enabled: true


obstacle_layer:
  enabled: true
  track_unknown_space: true
  combination_method: 1

  obstacle_range: 2.5
  raytrace_range: 3.0

  observation_sources: scan
  scan: {
    data_type: LaserScan,
    topic: /scan,
    marking: true,
    clearing: true
  }

配置global_costmap_param.yaml

global_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  transform_tolerance: 0.5

  update_frequency: 15.0
  publish_frequency: 10.0

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


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

配置local_costmap_param.yaml

local_costmap:
  # global_frame: odom
  global_frame: map
  robot_base_frame: base_footprint
  transform_tolerance: 0.5

  # update_frequency: 10.0
  # publish_frequency: 10.0 
  update_frequency: 15.0
  publish_frequency: 10.0
  rolling_window: true
  width: 15
  height: 15
  resolution: 0.05

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


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

配置move_base_param.yaml

shutdown_costmaps: false #当move_base在不活动状态时,是否关掉costmap

controller_frequency: 5.0 #向底盘控制移动话题cmd_vel发送命令的频率.
controller_patience: 3.0

planner_frequency: 0.5
planner_patience: 5.0

#机器人必须移动多远（以米计）才能被视为不摆动。
#如果出现摆动则说明全局规划失败，那么将在超时后执行恢复模块
oscillation_timeout: 10.0
oscillation_distance: 0.1

conservative_reset_dist: 0.1

配置base_global_planner_params.yaml

GlobalPlanner:
  allow_unknown: true
  default_tolerance: 0.05
  use_dijkstra: true
  use_quadratic: true
  use_grid_path: false
  outline_map: false
  old_navfn_behavior: false

  visualize_potential: false
  publish_potential: true

  lethal_cost: 253
  neutral_cost: 50
  cost_factor: 3.0

  orientation_mode: 0
  orientation_window_size: 1

配置TEB本地规划器

先安装teb功能包

 sudo apt-get install ros-melodic-teb-local-planner

TebLocalPlannerROS:

  odom_topic: odom
  map_frame: /map
      
  # Trajectoty 这部分主要是用于调整轨迹

  teb_autosize: True #优化期间允许改变轨迹的时域长度
  dt_ref: 0.3 #期望的轨迹时间分辨率
  dt_hysteresis: 0.03 #根据当前时间分辨率自动调整大小的滞后现象，通常约为。建议使用dt ref的10%
  
  #覆盖全局规划器提供的局部子目标的方向;规划局部路径时会覆盖掉全局路径点的方位角,
  #对于车辆的2D规划,可以设置为False,可实现对全局路径的更好跟踪。
  global_plan_overwrite_orientation: True

#指定考虑优化的全局计划子集的最大长度,如果为0或负数：禁用；长度也受本地Costmap大小的限制
  max_global_plan_lookahead_dist: 3.0 

  feasibility_check_no_poses: 1 #检测位姿可到达的时间间隔，default：4
    
  #如果为true，则在目标落后于起点的情况下，可以使用向后运动来初始化基础轨迹
  #(仅在机器人配备了后部传感器的情况下才建议这样做）
  allow_init_with_backwards_motion: False

  global_plan_viapoint_sep: -1

  #参数在TebLocalPlannerROS::pruneGlobalPlan()函数中被使用
  #该参数决定了从机器人当前位置的后面一定距离开始裁剪
  #就是把机器人走过的全局路线给裁剪掉，因为已经过去了没有比较再参与计算后面的局部规划
  global_plan_prune_distance: 1

  exact_arc_length: False
  publish_feedback: False

  # Robot
  max_vel_x: 2.5
  max_vel_x_backwards: 1
  max_vel_theta: 3.5
  acc_lim_x: 2.5
  acc_lim_theta: 3.5

  #仅适用于全向轮
  # max_vel_y (double, default: 0.0)  
  # acc_lim_y (double, default: 0.5)

  # ********************** Carlike robot parameters ********************
  min_turning_radius: 0.45     # 最小转弯半径 注意车辆运动学中心是后轮中点
  wheelbase: 0.26                 # 即前后轮距离

#设置为true时，ROS话题（rostopic） cmd_vel/angular/z 内的数据是舵机角度，
  cmd_angle_instead_rotvel: True 
  # ********************************************************************

  # footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" 多边形勿重复第一个顶点，会自动闭合
  #   type: "line"
  #   # radius: 0.2 # for type "circular"
  #   line_start: [-0.13, 0.0] # for type "line"
  #   line_end: [0.13, 0.0] # for type "line"
    # front_offset: 0.2 # for type "two_circles"
    # front_radius: 0.2 # for type "two_circles"
    # rear_offset: 0.2 # for type "two_circles"
    # rear_radius: 0.2 # for type "two_circles"
    # vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"

  # GoalTolerance
  footprint_model:
    type: "polygon"
    vertices: [[0.15, 0.15], [0.15, -0.15], [-0.15, -0.15], [-0.15, 0.15]]    

  xy_goal_tolerance: 0.1
  yaw_goal_tolerance: 1.0
  #自由目标速度。设为False时，车辆到达终点时的目标速度为0。
  #TEB是时间最优规划器。缺少目标速度约束将导致车辆“全速冲线”
  free_goal_vel: True
  
  # complete_global_plan: True   
  # Obstacles
      
  min_obstacle_dist: 0.2 # 与障碍的最小期望距离,
  include_costmap_obstacles: True #应否考虑到局部costmap的障碍设置为True后才能规避实时探测到的、建图时不存在的障碍物。
  costmap_obstacles_behind_robot_dist: 3.0 #考虑后面n米内的障碍物2.0
  obstacle_poses_affected: 30 #为了保持距离，每个障碍物位置都与轨道上最近的位置相连。
  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: 2 #1
  weight_kinematics_nh: 1000
  weight_kinematics_forward_drive: 0.1 #1
  weight_kinematics_turning_radius: 1 #1
  weight_optimaltime: 1
  weight_obstacle: 10 #50
  weight_dynamic_obstacle: 10 # not in use yet
  alternative_time_cost: False # not in use yet
  selection_alternative_time_cost: False
  # Homotopy Class Planner

  enable_homotopy_class_planning: False
  enable_multithreading: False
  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

  # # Recovery

  # shrink_horizon_backup: True
  # shrink_horizon_min_duration: 10
  # oscillation_recovery: True
  # oscillation_v_eps: 0.1
  # oscillation_omega_eps: 0.1
  # oscillation_recovery_min_duration: 10
  # oscillation_filter_duration: 10

有关TEB调参方法会在下一节介绍。

创建teb_base.launch文件

创建一个launch文件将上述参数文件全部加载：

3.小车在gazebo仿真环境中导航

创建一个一键打开所有节点的launch文件teb_demo.launch：

开始导航之旅：

roslaunch racebot_gazebo teb_demo.launch

点击红色箭头2D nav goal选择一个地图中的点，小车就会往该目标点导航了。

4.实现move_base+slam

是否能够让小车在未知环境中同时实现导航与建图呢？不妨可以一试，配置launch文件加载gmapping节点与move_base节点,文件名为slam_navi.launch：

测试

roslaunch racebot_gazebo racebot.launch 
roslaunch racebot_gazebo slam_navi.launch 

可以看到实现边导航边建图了，测试成功。实现该功能可能还是依托于仿真中里程计的准确性，各位不妨再把amcl放进去试试。

5.小结

阿克曼小车仿真到这里正式告一段落，本次项目的所有代码均开源，因为后续还有一些小收尾工作，比如模型替换，让仿真中的小车与真车更加相像，以及terb调参的一些技巧等。做完收尾工作后会把代码正式开源到我的github。
阿克曼小车仿真开发到导航为止算是一个阶段性的结束，后续的开发工作，我想结合视觉做一些开发，或者试试3D激光雷达的slam以及纯跟踪算法的应用，坑要一个一个爬，请拭目以待。

参考资料

1.古月老师的<>

2.古月学院《如何在Gazebo中实现阿克曼转向车的仿真 • 王泽恩》