阿克曼結構移動機器人的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檔案中對應。代碼如下：

<launch>
    <arg name="scan_topic" default="scan"/>
    <arg name="initial_pose_x" default="0.0"/>
    <arg name="initial_pose_y" default="0.0"/>
    <arg name="initial_pose_a" default="0.0"/>

    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
        <param name="min_particles"             value="500"/>
        <param name="max_particles"             value="3000"/>
        <param name="kld_err"                   value="0.02"/>
        <param name="update_min_d"              value="0.20"/>
        <param name="update_min_a"              value="0.20"/>
        <param name="resample_interval"         value="1"/>
        <param name="transform_tolerance"       value="0.5"/>
        <param name="recovery_alpha_slow"       value="0.00"/>
        <param name="recovery_alpha_fast"       value="0.00"/>
        <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
        <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
        <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
        <param name="gui_publish_rate"          value="50.0"/>

        <remap from="scan"                      to="$(arg scan_topic)"/>
        <param name="laser_max_range"           value="3.5"/>
        <param name="laser_max_beams"           value="180"/>
        <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_likelihood_max_dist" value="2.0"/>
        <param name="laser_model_type"          value="likelihood_field"/>

        <param name="odom_model_type"           value="diff"/>
        <param name="odom_alpha1"               value="0.1"/>
        <param name="odom_alpha2"               value="0.1"/>
        <param name="odom_alpha3"               value="0.1"/>
        <param name="odom_alpha4"               value="0.1"/>
        <param name="odom_frame_id"             value="odom"/>
        <param name="base_frame_id"             value="base_footprint"/>
    </node>
</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檔案将上述參數檔案全部加載：

<?xml version="1.0"?>
<launch>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
    <rosparam file="$(find racebot_gazebo)/config/teb/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find racebot_gazebo)/config/teb/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find racebot_gazebo)/config/teb/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find racebot_gazebo)/config/teb/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find racebot_gazebo)/config/teb/move_base_params.yaml" command="load" />
    <rosparam file="$(find racebot_gazebo)/config/teb/base_global_planner_params.yaml" command="load" />
    <rosparam file="$(find racebot_gazebo)/config/teb/teb_local_planner_params.yaml" command="load" />
  </node>
  
</launch> 

           

3.小車在gazebo仿真環境中導航

建立一個一鍵打開所有節點的launch檔案teb_demo.launch：

<launch>

    <!-- 啟動仿真環境 -->
    <include file="$(find racebot_gazebo)/launch/racebot.launch"/>

    <!-- 設定地圖的配置檔案 -->
    <arg name="map" default="room_mini.yaml" />

    <!-- 運作地圖伺服器，并且加載設定的地圖-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find racebot_gazebo)/map/$(arg map)"/>

    <!-- 運作move_base節點 -->
    <include file="$(find racebot_gazebo)/launch/teb_base.launch"/>

    <!-- 啟動AMCL節點 -->
    <include file="$(find racebot_gazebo)/launch/amcl.launch" />

    <!-- 運作rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find racebot_gazebo)/rviz/nav.rviz"/>

</launch>

           

開始導航之旅：

roslaunch racebot_gazebo teb_demo.launch
           

點選紅色箭頭2D nav goal選擇一個地圖中的點，小車就會往該目标點導航了。

4.實作move_base+slam

是否能夠讓小車在未知環境中同時實作導航與建圖呢？不妨可以一試，配置launch檔案加載gmapping節點與move_base節點,檔案名為slam_navi.launch：

<?xml version="1.0"?>
<launch>

<!-- 運作move_base節點 -->
<include file="$(find racebot_gazebo)/launch/teb_base.launch"/>


<include file="$(find racebot_gazebo)/launch/slam_gmapping.launch"/>

<!-- 運作rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find racebot_gazebo)/rviz/nav.rviz"/>


</launch>
           

測試

roslaunch racebot_gazebo racebot.launch 
roslaunch racebot_gazebo slam_navi.launch 
           

可以看到實作邊導航邊建圖了，測試成功。實作該功能可能還是依托于仿真中裡程計的準确性，各位不妨再把amcl放進去試試。

5.小結

阿克曼小車仿真到這裡正式告一段落，本次項目的所有代碼均開源，因為後續還有一些小收尾工作，比如模型替換，讓仿真中的小車與真車更加相像，以及terb調參的一些技巧等。做完收尾工作後會把代碼正式開源到我的github。

阿克曼小車仿真開發到導航為止算是一個階段性的結束，後續的開發工作，我想結合視覺做一些開發，或者試試3D雷射雷達的slam以及純跟蹤算法的應用，坑要一個一個爬，請拭目以待。

參考資料

1.古月老師的<<ROS機器人開發實踐>>

2.古月學院《如何在Gazebo中實作阿克曼轉向車的仿真 • 王澤恩》