5 ROS仿真建模（4- Navigation导航仿真)

5.7 Navigation导航预期准备

5.7.1 导航功能实现框图

Tip：参考连接http://wiki.ros.org/navigation/Tutorials/RobotSetup

5.7.2 安装导航功能包

sudo apt-get install ros-melodic-gmapping
sudo apt-get install ros-melodic-map-server
sudo apt-get install ros-melodic-navigation
sudo apt-get install ros-melodic-amcl

5.7.3 导航仿真条件

1、需要准备仿真场景，如果没有可以在此 0积分下载
2、了解地图、AMCL定位、路径规划等实现关系
3、查阅相关文献了解导航中的坐标变换关系（里程计的坐标系/odom和地图坐标系/map的关系）
4、本文只是对导航仿真进行按部就班的使用

5.8 键盘控制机器人并利用SLAM绘制地图

1、新建功能包，添加依赖项
2、Copy SLAM导航文件内容，并修改
3、启动导航仿真
4、键盘控制机器人绘制地图
5、保存地图

5.8.1 新建功能包

1、新建功能包添加依赖项，依赖项包括：gmapping map_server amcl move_base
例如：ubuntu@ubuntu:~/catkin_ws/src$ catkin_create_pkg hello_navigation gmapping map_server amcl move_base

2、新建文件夹：launch、map、config存放一些文件

5.8.2 在launch文件夹编写SLAM建图文件

直接复制下面内容并做修改（params_slam.launch）

<launch>
<param name="hello_slam" value="true"/> <!--param name can change-->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <remap from="scan" to="scan"/>
      <param name="base_frame" value="base_coordinate"/><!--need change-->
      <param name="odom_frame" value="odom"/> <!--need change-->
      <param name="map_update_interval" value="5.0"/>
      <param name="maxUrange" value="16.0"/>
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="1"/>
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.5"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.0"/>
      <param name="delta" value="0.05"/>
      <param name="llsamplerange" value="0.01"/>
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
    </node>
    
</launch>

5.8.3 新编启动仿真环境和小车的launch文件

Tips：1、本文是在前面基础上一步一步操作进行的仿真，主要内容为，gazebo仿真物理环境，rviz查看机器人信息，
2、新建启动的launch文件目的在于启动gazebo物理环境，加载小车，并配置了rviz内容，实际内容和5 ROS仿真建模（2- urdf文件的编写和场景搭建）一致。
3、复制rviz的配置文件到hello_navigation功能包的config下，修改launch文件的rviz配置文件路径

编写launch文件(myAGV.launch)

<launch>

    <!-- 设置参数 其中param的名字必须是robot_description -->
    <param name="robot_description" textfile="$(find simulation_scene)/urdf/myAGV.urdf" />

    <!-- 启动 rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find hello_navigation)/config/myAGV.rviz" />

    <!-- 启动机器人状态和关节状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />

    <!-- 启动 gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" >
        <arg name="world_name" value="$(find simulation_scene)/world/house.world" />
    </include>

    <!-- 在 gazebo 中显示机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="myAGV" args="-urdf -model myAGV -param robot_description"  />

</launch>

5.8.4 启动SLAM仿真

编写总的SLAM启动文件（hello_slam.launch）

<launch>
    
    <!-- 启动仿真环境加载小车 -->
    <include file="$(find hello_navigation)/launch/myAGV.launch" />
    <!-- 启动SLAM -->
    <include file="$(find hello_navigation)/launch/params_slam.launch" />

</launch>

1、编译文件catkin_make
2、启动slamroslaunch hello_navigation hello_slam.launch
3、在rviz加载map并订阅/map话题
4、启动键盘控制节点rosrun teleop_twist_keyboard teleop_twist_keyboard.py
5、控制机器人运动绘制地图

5.8.5 保存SLAM的地图

1、新建保存地图的launch文件（save_map.launch）
2、注意arg的保存地图位置和文件名

<launch>
    <arg name="filename" value="$(find hello_navigation)/map/hello_map" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

3、启动save_map.launch文件保存当前rviz显示到的地图

5.9 AMCL机器人自身定位仿真

5.9.1 AMCL定位基础

AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。

AMCL核心是清楚导航中的坐标变换关系。

roscd amcl找到官方提供的实例文档

5.9.2 新建map_server服务器文件

1、map_server能够获取地图数据为机器人的定位路径规划等提供信息，而且AMCL定位必须有map_server服务！
2、新建launch文件(params_mapserver.launch)提供map_server服务。
3、此时的map_server包括的功能有：找到保存的地图并加载到服务器里。

<launch>

    <!-- 设置地图的配置文件 -->
    <arg name="map" default="hello_map.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find hello_navigation)/map/$(arg map)"/>

</launch>

5.9.3 新建AMCL定位文件

在5.8的基础上，新建launch文件（params_amcl.launch）实现机器人自身定位

<launch>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        <!-- Publish scans from best pose at a max of 10 Hz -->
        <param name="odom_model_type" value="diff"/><!-- 里程计模式为差分 -->
        <param name="odom_alpha5" value="0.1"/>
        <param name="transform_tolerance" value="0.2" />
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="30"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="5000"/>
        <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.8"/>
        <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_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.2"/>
        <param name="update_min_a" value="0.5"/>

        <param name="odom_frame_id" value="odom"/><!-- 里程计坐标系 -->
        <param name="base_frame_id" value="base_coordinate"/><!-- 添加机器人基坐标系 -->
        <param name="global_frame_id" value="map"/><!-- 添加地图坐标系 -->

        <param name="resample_interval" value="1"/>
        <param name="transform_tolerance" value="0.1"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
    </node>

</launch>

Tip：机器人自身定位前，需要知道地图数据，即在 AMCL的launch文件中，一定是要有地图服务器map_server

5.9.4 控制机器人运动并查看定位话题

编写AMCL启动文件（hello_amcl.launch）

<launch>
    
    <!-- 启动仿真环境加载小车 -->
    <include file="$(find hello_navigation)/launch/myAGV.launch" />
    <!-- 启动map_server -->
    <include file="$(find hello_navigation)/launch/params_mapserver.launch" />
    <!-- 启动AMCL -->
    <include file="$(find hello_navigation)/launch/params_amcl.launch" />

</launch>

1、启动AMCLroslaunch hello_navigation hello_amcl.launch
2、在rviz加载PoseArray并订阅/particlecloud话题
3、启动键盘控制节点rosrun teleop_twist_keyboard teleop_twist_keyboard.py
4、控制机器人运动查看定位状态

5.10 路径规划导航

5.10.1 路径规划move_base基础

1、路径规划的含义即导航最前面框图，表示的move_base即是路径规划，在知道地图数据、传感器数据、里程计信息等，做出全局路径规划和局部路径规划。
2、查阅资料了解代价地图和碰撞算法

后续的配置文件yaml参考B站视频【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程

5.10.2 路径规划配置通用代价地图属性

在hello-navigation的config文件夹下，编写costmap_common.yaml配置代价地图。

#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
# robot_radius: 0.18 #圆形
footprint: [[-0.18, -0.18], [-0.18, 0.18], [0.18, 0.18], [0.18, -0.18]] #其他形状的四个角的坐标XY坐标

obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物


#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0

#地图类型
map_type: costmap
#导航包所需要的传感器
observation_sources: scan
#对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

5.10.3 路径规划配置全局代价地图参数

在hello-navigation的config文件夹下，编写global_costmap.yaml

global_costmap:
  global_frame: map #地图坐标系
  robot_base_frame: base_coordinate #机器人坐标系
  # 以此实现坐标变换

  update_frequency: 1.0 #代价地图更新频率
  publish_frequency: 1.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.

5.10.4 路径规划配置局部代价地图参数

在hello-navigation的config文件夹下，编写local_costmap.yaml

local_costmap:
  global_frame: odom #里程计坐标系
  robot_base_frame: base_coordinate #机器人坐标系

  update_frequency: 10.0 #代价地图更新频率
  publish_frequency: 10.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: false  #不需要静态地图，可以提升导航效果
  rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化
  width: 3 # 局部地图宽度 单位是 m
  height: 3 # 局部地图高度 单位是 m
  resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

5.10.5 路径规划配置局部路径规划器属性

在hello-navigation的config文件夹下，编写base_local_planner.yaml

TrajectoryPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.5 # X 方向最大速度
  min_vel_x: 0.1 # X 方向最小速速

  max_vel_theta:  1.0 # 
  min_vel_theta: -1.0
  min_in_place_vel_theta: 1.0

  acc_lim_x: 1.0 # X 加速限制
  acc_lim_y: 0.0 # Y 加速限制
  acc_lim_theta: 0.6 # 角速度加速限制

# Goal Tolerance Parameters，目标公差
  xy_goal_tolerance: 0.10
  yaw_goal_tolerance: 0.05

# Differential-drive robot configuration
# 是否是全向移动机器人
  holonomic_robot: false

# Forward Simulation Parameters，前进模拟参数
  sim_time: 0.8
  vx_samples: 18
  vtheta_samples: 20
  sim_granularity: 0.05

5.10.6 路径规划仿真文件编写

编写路径规划仿真的launch文件（params_movebase.launch）

<launch>

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

1、机器人路径规划导航是在已知地图数据情况下，前往目的地，此时仿真的示例并非导航+建图，而只是单纯的导航
2、故move_base路径规划包括地图数据即map_server和机器人自身定位信息AMCL

5.10.7 启动路径规划导航

编写路径规划导航启动文件（hello_movebase.launch）

<launch>
    
    <!-- 启动仿真环境加载小车 -->
    <include file="$(find hello_navigation)/launch/myAGV.launch" />
    <!-- 启动map_server -->
    <include file="$(find hello_navigation)/launch/params_mapserver.launch" />
    <!-- 启动AMCL -->
    <include file="$(find hello_navigation)/launch/params_amcl.launch" />
    <!-- 启动movebase -->
    <include file="$(find hello_navigation)/launch/params_movebase.launch" />

</launch>

1、启动仿真环境加载小车roslaunch hello_navigation myAGV.launch
2、启动slamroslaunch hello_navigation hello_movebase.launch
3、在rviz加载map重命名为map_global并订阅/move_base/global_costmap/costmap全局地图话题
4、在rviz加载map重命名为map_local并订阅/move_base/local_costmap/costmap局部路径规划话题
5、在rviz加载path订阅/move_base/NavfnROS/plan路径
6、在rviz的操作栏选择2D Nav Goal（注意不是2D Pose Estimate），在地图上打标记实现路径规划运动

5.11 动态SLAM导航建图

5.11.1 动态SLAM导航建图基础

1、路径规划move_base和机器人定位AMCL都是需要地图服务器map_server支持才能运行的
2、路径规划是在默认已知地图的情况下，对目标点进行导航移动，map_server服务提供的是静态的地图信息
3、动态SLAM导航建图，则在不清楚全局静态地图的情况下，一边导航一边绘制地图
4、因此SLAM本身就集成了map_server服务，实时发布动态信息

5.11.2 编写动态导航建图启动文件

1、动态SLAM需要move_base功能和SLAM功能
2、编写动态SLAM的launch文件(hello_dynamicslam.launch)

<launch>
    
    <!-- 启动仿真环境加载小车 -->
    <include file="$(find hello_navigation)/launch/myAGV.launch" />
    <!-- 启动slam -->
    <include file="$(find hello_navigation)/launch/params_slam.launch" />
    <!-- 启动movebase -->
    <include file="$(find hello_navigation)/launch/params_movebase.launch" />

</launch>

5.12 参考资料和0积分资源

5.12.1参考资料

1、B站视频：【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程
2、ROSWIKI：http://wiki.ros.org/navigation/Tutorials/RobotSetup
3、其他博客资料：ros 建图导航参数配置

5.12.2 0积分资源

0积分资源

上一篇： 5 ROS仿真建模（3- rviz+gazebo+控制仿真机器人)