ros1仿真导航机器人 navigation

仅为学习记录和一些自己的思考，不具有参考意义。

1navigation导航框架

2导航设置过程

（1）启动仿真环境

roslaunch why_simulation why_robocup.launch

（2）启动move_base导航、amcl定位

roslaunch why_simulation nav.launch

<launch>

    <node pkg="move_base" type="move_base" name="move_base">
        <rosparam file="$(find why_simulation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find why_simulation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find why_simulation)/config/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find why_simulation)/config/local_costmap_params.yaml" command="load" />
        <param name="base_global_planner" value="global_planner/GlobalPlanner" /> 
        <param name="base_local_planner" value="wpbh_local_planner/WpbhLocalPlanner" />
    </node>

    <node pkg="map_server" type="map_server" name="map_server" args="$(find why_simulation)/maps/map.yaml"/>

    <node pkg="amcl" type="amcl" name="amcl"/>


</launch>

（3）启动rviz

rviz

（4）设置目标点

（5）将左右指令完善到launch文件中

<launch>

    <include file="$(find why_simulation)/launch/why_robocup.launch"/>

    <node pkg="move_base" type="move_base" name="move_base">
        <rosparam file="$(find why_simulation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find why_simulation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find why_simulation)/config/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find why_simulation)/config/local_costmap_params.yaml" command="load" />
        <param name="base_global_planner" value="global_planner/GlobalPlanner" /> 
        <param name="base_local_planner" value="wpbh_local_planner/WpbhLocalPlanner" />
    </node>

    <node pkg="map_server" type="map_server" name="map_server" args="$(find why_simulation)/maps/map.yaml"/>

    <node pkg="amcl" type="amcl" name="amcl"/>

    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find why_simulation)/rviz/nav.rviz"/>
    
</launch>

3全局路径规划算法

功能包简介

navfn与global_planner功能相同，同时包含了Dijkstra与A*算法。

navfn默认使用Dijkstra算法，算法无问题，但是此包的A*存在问题。

global_planner功能包无bug。

carrot_planner功能包碰到障碍物就停止了，常作为自己书写的规划器的模板。

启动全局规划

    <node pkg="move_base" type="move_base" name="move_base">
        <param name="base_global_planner" value="global_planner/GlobalPlanner" /> 
    </node>

golbal_planner默认使用Dijkstra算法，若要切换为A*，则需要以下修改。

    <node pkg="move_base" type="move_base" name="move_base">

        <param name="base_global_planner" value="global_planner/GlobalPlanner" /> .

        <param name="GlobalPlanner/use_dijkstra" value="false" /> 
        <param name="GlobalPlanner/use_grid_path" value="true" /> 

    </node>

4 amcl

    <node pkg="amcl" type="amcl" name="amcl"/>

开始导航后真实位置的粒子越来越少

5 costmap

代价地图

代价地图的参数设置

costmap_common_params.yaml

robot_radius: 0.25
inflation_radius: 0.5
obstacle_range: 6.0
raytrace_range: 6.0
observation_sources: base_lidar
base_lidar: {
    data_type: LaserScan,
    topic: /scan, 
    marking: true, 
    clearing: true
    }

global_costmap_params.yaml

global_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  static_map: true
  update_frequency: 1.0
  publish_frequency: 1.0
  transform_tolerance: 1.0

recovery_behaviors:
  - name: 'conservative_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'
  - name: 'rotate_recovery'
    type: 'rotate_recovery/RotateRecovery'
  - name: 'aggressive_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'

conservative_reset:
  reset_distance: 2.0
  layer_names: ["obstacle_layer"]

aggressive_reset:
  reset_distance: 0.0
  layer_names: ["obstacle_layer"]

local_costmap_params.yaml

local_costmap:
  global_frame: odom
  robot_base_frame: base_footprint
  static_map: false
  rolling_window: true
  width: 3.0
  height: 3.0
  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 1.0

6 recovery_behaviors

应急机制，在导航进行停滞时，尝试刷新周围障碍物的信息，重新进行全局路径规划。

recovery_behaviors:
  - name: 'conservative_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'
  - name: 'rotate_recovery'
    type: 'rotate_recovery/RotateRecovery'
  - name: 'aggressive_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'

7局部路径规划算法

更改launch文件中的以下代码即可更换算法

        <param name="base_local_planner" value="wpbh_local_planner/WpbhLocalPlanner" />

DWA测试

<launch>

    <include file="$(find why_simulation)/launch/why_robocup.launch"/>

    <node pkg="move_base" type="move_base" name="move_base">
        <rosparam file="$(find why_simulation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find why_simulation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find why_simulation)/config/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find why_simulation)/config/local_costmap_params.yaml" command="load" />
        <param name="base_global_planner" value="global_planner/GlobalPlanner" /> 
        <!-- <param name="GlobalPlanner/use_dijkstra" value="false" />  -->
        <!-- <param name="GlobalPlanner/use_grid_path" value="true" />  -->
        
        <!-- DWA -->
        <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
        <rosparam file="$(find why_simulation)/config/dwa_local_planner_params.yaml" command="load" />

    </node>

    <node pkg="map_server" type="map_server" name="map_server" args="$(find why_simulation)/maps/map.yaml"/>

    <node pkg="amcl" type="amcl" name="amcl"/>

    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find why_simulation)/rviz/nav.rviz"/>

</launch>

导航结果可以看过许多白色的候选路径，绿色为最优路线。

dwa_local_planner_params.yaml

DWAPlannerROS:
  # 速度参数
  max_vel_x: 0.3      # 最大x方向速度
  min_vel_x: -0.05    # 最小x方向速度（设置负数将会允许倒车）
  max_vel_y: 0.0      # 差分驱动机器人的最大y方向速度为 0.0
  min_vel_y: 0.0      # 差分驱动机器人的最小y方向速度为 0.0
  max_vel_trans: 0.3  # 最大平移速度
  min_vel_trans: 0.01 # 最小平移速度（建议不要设置为 0.0 ）
  trans_stopped_vel: 0.1  # 当平移速度小于这个值，就让机器人停止
  acc_lim_trans: 2.5      # 最大平移加速度
  acc_lim_x: 2.5          # x方向的最大加速度上限
  acc_lim_y: 0.0          # y方向的加速度上限（差分驱动机器人应该设置为 0.0 ）
  
  max_vel_theta: 1.0      # 最大旋转速度，略小于基座的功能
  min_vel_theta: -0.01    # 当平移速度可以忽略时的最小角速度
  theta_stopped_vel: 0.1  # 当旋转速度小于这个值，就让机器人停止
  acc_lim_theta: 6.0      # 旋转的加速度上限

  # 目标容差参数
  yaw_goal_tolerance: 0.1         # 目标航向容差
  xy_goal_tolerance: 0.05         # 目标xy容差
  latch_xy_goal_tolerance: false  # 到达目标容差范围后，停止移动，只旋转调整航向

  # 向前模拟参数
  sim_time: 1.7       # 模拟时间，默认值 1.7
  vx_samples: 3       # x方向速度采样数，默认值 3
  vy_samples: 1       # 差分驱动机器人y方向速度采样数，只有一个样本
  vtheta_samples: 20  # 旋转速度采样数，默认值 20

  # 轨迹评分参数
  path_distance_bias: 32.0  # 靠近全局路径的权重，默认值 32.0
  goal_distance_bias: 24.0  # 接近导航目标点的权重，默认值 24.0
  occdist_scale: 0.01       # 控制器避障的权重，默认值 0.01
  forward_point_distance: 0.325 # 从机器人到评分点的位置，默认值 0.325
  stop_time_buffer: 0.2     # 在碰撞前机器人必须停止的时间长度，留出缓冲空间，默认值 0.2
  scaling_speed: 0.25       # 缩放机器人速度的绝对值，默认值 0.25
  max_scaling_factor: 0.2   # 机器人足迹在高速时能缩放的最大系数，默认值 0.2

  # 防振动参数
  oscillation_reset_dist: 1.05 # 重置振动标志前需要行进的距离，默认值 0.05

  # 辅助调试选项
  publish_traj_pc : true      # 是否在 RViz 里发布轨迹
  publish_cost_grid_pc: true  # 是否在 RViz 里发布代价网格
  global_frame_id: odom       # 基础坐标系

  # 差分驱动机器人配置
  holonomic_robot: false # 是否全向移动机器人

在线调参工具

rosrun rqt_reconfigure rqt_reconfigure

TEB测试

<launch>

    <include file="$(find why_simulation)/launch/why_robocup.launch"/>

    <node pkg="move_base" type="move_base" name="move_base">
        <rosparam file="$(find why_simulation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find why_simulation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find why_simulation)/config/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find why_simulation)/config/local_costmap_params.yaml" command="load" />
        <param name="base_global_planner" value="global_planner/GlobalPlanner" /> 
        <!-- <param name="GlobalPlanner/use_dijkstra" value="false" />  -->
        <!-- <param name="GlobalPlanner/use_grid_path" value="true" />  -->
        
        <!-- DWA -->
        <!-- <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" /> -->
        <!-- <rosparam file="$(find why_simulation)/config/dwa_local_planner_params.yaml" command="load" /> -->

        <!-- TEB -->
        <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
        <rosparam file="$(find why_simulation)/config/teb_local_planner_params.yaml" command="load" />


    </node>

    <node pkg="map_server" type="map_server" name="map_server" args="$(find why_simulation)/maps/map.yaml"/>

    <node pkg="amcl" type="amcl" name="amcl"/>

    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find why_simulation)/rviz/nav.rviz"/>

</launch>