机器人导航(仿真)-02

二、导航实现

请先安装相关的ROS功能包:

安装 gmapping 包(用于构建地图):sudo apt install ros-<ROS版本>-gmapping
安装地图服务包(用于保存与读取地图):sudo apt install ros-<ROS版本>-map-server
安装 navigation 包(用于定位以及路径规划):sudo apt install ros-<ROS版本>-navigation

新建功能包，并导入依赖: gmapping map_server amcl move_base

1、SLAM建图

1.1 gmapping简介

gmapping 是ROS开源社区中较为常用且比较成熟的SLAM算法之一，gmapping可以根据移动机器人里程计数据和激光雷达数据来绘制二维的栅格地图，对应的，gmapping对硬件也有一定的要求:

该移动机器人可以发布里程计消息
机器人需要发布雷达消息(该消息可以通过水平固定安装的雷达发布，或者也可以将深度相机消息转换成雷达消息)

gmapping 安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-gmapping

1.2 gmapping节点说明

gmapping 功能包中的核心节点是:slam_gmapping。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。

（1）订阅的Topic

tf (tf/tfMessage)

用于雷达、底盘与里程计之间的坐标变换消息。

scan(sensor_msgs/LaserScan)

SLAM所需的雷达信息。

（2）发布的Topic

map_metadata(nav_msgs/MapMetaData)

地图元数据，包括地图的宽度、高度、分辨率等，该消息会固定更新。

map(nav_msgs/OccupancyGrid)

地图栅格数据，一般会在rviz中以图形化的方式显示。

~entropy(std_msgs/Float64)

机器人姿态分布熵估计(值越大，不确定性越大)。

（3）服务

dynamic_map(nav_msgs/GetMap)

用于获取地图数据。

（4）参数

~base_frame(string, default:"base_link")

机器人基坐标系。

~map_frame(string, default:"map")

地图坐标系。

~odom_frame(string, default:"odom")

里程计坐标系。

~map_update_interval(float, default: 5.0)

地图更新频率，根据指定的值设计更新间隔。

~maxUrange(float, default: 80.0)

激光探测的最大可用范围(超出此阈值，被截断)。

~maxRange(float)

激光探测的最大范围。

.... 参数较多，上述是几个较为常用的参数，其他参数介绍可参考官网。

（5）所需的坐标变换

雷达坐标系→基坐标系

一般由 robot_state_publisher 或 static_transform_publisher 发布。

基坐标系→里程计坐标系

一般由里程计节点发布。

（6）发布的坐标变换

地图坐标系→里程计坐标系

地图到里程计坐标系之间的变换。

1.3 gmapping使用

新建launch文件：nav01_slam.launch

注意在新建工作空间时需要添加依赖：amcl gmapping map_server move_base

代码：

<launch>
<!--仿真环境下，将该参数设置位true-->
<param name="use_sim_time" value="true"/>
    <!--gmapping节点-->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
        <!--设置雷达话题-->
        <remap from="scan" to="scan"/>
        <!--关键参数-->
        <param name="base_frame" value="base_footprint"/><!--底盘坐标系-->
        <param name="odom_frame" value="odom"/> <!--里程计坐标系-->

        <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>

    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />

    <node pkg="rviz" type="rviz" name="rviz" />
    <!-- 可以保存 rviz 配置并后期直接使用-->
    <!--
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find my_nav_sum)/rviz/gmapping.rviz"/>
    -->
</launch>

1.4 执行

先启动gazebo仿真环境（略）
再启动地图绘制的 launch 文件
启动键盘键盘控制节点，用于控制机器人运动建图：rosrun teleop_twist_keyboard teleop_twist_keyboard.py
在 rviz 中添加组件，显示栅格地图

2、地图服务

2.1 map_server简介

map_server功能包中提供了两个节点: map_saver 和 map_server，前者用于将栅格地图保存到磁盘，后者读取磁盘的栅格地图并以服务的方式提供出去。

map_server安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-map-server

2.2 地图保存节点(map_saver)

（1）map_saver节点说明

订阅的topic:

map(nav_msgs/OccupancyGrid)

订阅此话题用于生成地图文件

（2）地图保存launch文件：nav02_map_save.launch

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

SLAM建图完毕后，执行该launch文件即可。

2.3 地图服务(map_server)

（1）map_server节点说明

发布的话题

map_metadata（nav_msgs / MapMetaData）

发布地图元数据。

map（nav_msgs / OccupancyGrid）

地图数据。

服务

static_map（nav_msgs / GetMap）

通过此服务获取地图。

参数

〜frame_id（字符串，默认值：“map”）

地图坐标系。

（2）地图读取

通过 map_server 的 map_server 节点可以读取栅格地图数据，编写 launch 文件如下:

nav03_map_server.launch

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

运行该launch文件

如果需要显示launch文件的内容，需要打开rviz，打开终端输入rviz，并添加map属性

3、定位

3.1 amcl简介

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

3.2 amcl使用

首先编写amcl定位的launch文件：nav04_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_footprint"/><!-- 添加机器人基坐标系 -->
    <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>

再编写集成的launch文件：test_amcl.launch

<!--测试文件-->
<launch>
    <!--启动rviz-->
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find nav_demo)/config/nav.rviz"/>

    <!--加载地图服务-->
    <include file="$(find nav_demo)/launch/nav03_map_server.launch" />

    <!--amcl文件-->
    <include file="$(find nav_demo)/launch/nav04_amcl.launch" />
</launch>

备注：有些launch文件之前的内容有写

3.3 执行

（1）先启动gazebo仿真软件

（2）启动键盘控制节点：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

（3）启动launch文件：test_amcl.launch

（4）在启动的 rviz 中，添加RobotModel、Map组件，分别显示机器人模型与地图，添加 posearray 插件，设置topic为particlecloud来显示 amcl 预估的当前机器人的位姿，箭头越是密集，说明当前机器人处于此位置的概率越高

4、路径规划

4.1 move_base简介

move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。

move_base已经被集成到了navigation包，navigation安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-navigation

4.2 move_base与代价地图

（1）概念

机器人导航(尤其是路径规划模块)是依赖于地图的，地图在SLAM时已经有所介绍了，ROS中的地图其实就是一张图片，这张图片有宽度、高度、分辨率等元数据，在图片中使用灰度值来表示障碍物存在的概率。不过SLAM构建的地图在导航中是不可以直接使用的，因为：

SLAM构建的地图是静态地图，而导航过程中，障碍物信息是可变的，可能障碍物被移走了，也可能添加了新的障碍物，导航中需要时时的获取障碍物信息；
在靠近障碍物边缘时，虽然此处是空闲区域，但是机器人在进入该区域后可能由于其他一些因素，比如：惯性、或者不规则形体的机器人转弯时可能会与障碍物产生碰撞，安全起见，最好在地图的障碍物边缘设置警戒区，尽量禁止机器人进入...

所以，静态地图无法直接应用于导航，其基础之上需要添加一些辅助信息的地图，比如时时获取的障碍物数据，基于静态地图添加的膨胀区等数据。

（2）组成

代价地图有两张:global_costmap(全局代价地图) 和 local_costmap(本地代价地图)，前者用于全局路径规划，后者用于本地路径规划。

两张代价地图都可以多层叠加,一般有以下层级:

Static Map Layer：静态地图层，SLAM构建的静态地图。
Obstacle Map Layer：障碍地图层，传感器感知的障碍物信息。
Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。
Other Layers：自定义costmap。

多个layer可以按需自由搭配

（3）碰撞算法

上图中，横轴是距离机器人中心的距离，纵轴是代价地图中栅格的灰度值。

致命障碍:栅格值为254，此时障碍物与机器人中心重叠，必然发生碰撞；
内切障碍:栅格值为253，此时障碍物处于机器人的内切圆内，必然发生碰撞；
外切障碍:栅格值为[128,252]，此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞；
非自由空间:栅格值为(0,127]，此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞；
自由区域:栅格值为0，此处机器人可以自由通过；
未知区域:栅格值为255，还没探明是否有障碍物。

膨胀空间的设置可以参考非自由空间。

4.3 move_base使用

路径规划算法在move_base功能包的move_base节点中已经封装完毕了，但是还不可以直接调用，因为算法虽然已经封装了，但是该功能包面向的是各种类型支持ROS的机器人，不同类型机器人可能大小尺寸不同，传感器不同，速度不同，应用场景不同....最后可能会导致不同的路径规划结果，那么在调用路径规划节点之前，我们还需要配置机器人参数。具体实现如下:

先编写launch文件模板
编写配置文件
集成导航相关的launch文件
测试

（1）launch文件

新建launch文件：nav05_path.launch

<launch>

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

</launch>

其中：

启动了 move_base 功能包下的 move_base 节点，respawn 为 false，意味着该节点关闭后，不会被重启；clear_params 为 true，意味着每次启动该节点都要清空私有参数然后重新载入；通过 rosparam 会载入若干 yaml 文件用于配置参数。

（2）配置文件

# base_local_planner_params.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

# cost_map_common_params.yaml

该文件是move_base 在全局路径规划与本地路径规划时调用的通用参数，包括:机器人的尺寸、距离障碍物的安全距离、传感器信息等。配置参考如下：

#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
robot_radius: 0.12 #圆形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状

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}

# globe_costmap_params.yaml

该文件用于全局代价地图参数设置：

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

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

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

# local_costmap_params.yaml

该文件用于局部代价地图参数设置：

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

  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，一般与静态地图分辨率保持一致

（3）集成的launch文件

nav06_test.launch

<!--集成导航相关的文件-->
<launch>
    <!--地图服务-->
    <include file="$(find nav_demo)/launch/nav03_map_server.launch" />

    <!--amcl-->
    <include file="$(find nav_demo)/launch/nav04_amcl.launch" />

    <!--move_base-->
    <include file="$(find nav_demo)/launch/nav05_path.launch" />

    <!--rviz-->
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find nav_demo)/config/nav.rviz"/>
</launch>

（4）测试

需要先启动gazebo，然后启动上面的集成launch文件

5、导航与SLAM建图

在上面的过程中，是通过键盘控制机器人移动实现建图的，而后续又介绍了机器人的自主移动实现，那么可不可以将二者结合，实现机器人自主移动的SLAM建图呢？

上述需求是可行的。虽然可能会有疑问，导航时需要地图信息，之前导航实现时，是通过 map_server 包的 map_server 节点来发布地图信息的，如果不先通过SLAM建图，那么如何发布地图信息呢？SLAM建图过程中本身就会时时发布地图信息，所以无需再使用map_server，SLAM已经发布了话题为 /map 的地图消息了，且导航需要定位模块，SLAM本身也是可以实现定位的。

该过程实现比较简单，步骤如下:

编写launch文件，集成SLAM与move_base相关节点；
执行launch文件并测试。

（1）编写launch文件

nav07_slam_auto.launch

<launch>
    <!--启动slam节点-->
    <include file="$(find nav_demo)/launch/nav01_slam.launch" />

    <!--导航中的move_base-->
    <include file="$(find nav_demo)/launch/nav05_path.launch" />
</launch>

因为slam文件中已经启动了rviz，所以不用再次添加rviz

（2）测试

首先执行gazebo，然后启动上面的launch文件

参考：

7.2 导航实现 · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程www.autolabor.com.cn/book/ROSTutorials/di-7-zhang-j

机器人导航(仿真)-02

门前小恐龙

二、导航实现

1、SLAM建图

2、地图服务

3、定位

4、路径规划

5、导航与SLAM建图

为你推荐

第四天：在ubuntu16.04中安装pycharm（同时配置好ros环境），重新修改usb摄像头串口映射，通过命令控制机械臂

从零开始搭二维激光SLAM --- 基于Hector的栅格地图的构建

精选机械臂套装与机械臂结合案例介绍！

ROS部署到机器人小车并实现底层电机控制过程中遇到的问题

ROS 2设计文章系列之十七——ROS 2中实时系统实现的提议

Webots小车与Gampping建图仿真与ORB3算法实践

关于作者

门前小恐龙

8

0

0

1

机器人导航(仿真)-01

Ubuntu16.04安装VS Code

URDF集成Gazebo

相关推荐

cartographer环境最新配置（2021.11.11）

听说你只会用Gazebo仿真？

Powering the world!

URDF优化：xacro

精选aubo i5 + realsense D435i识别抓取实践三

stm32与ros进行串口通信

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

机器人导航(仿真)-02

门前小恐龙

二、导航实现

1、SLAM建图

2、地图服务

3、定位

4、路径规划

5、导航与SLAM建图

为你推荐

第四天：在ubuntu16.04中安装pycharm（同时配置好ros环境），重新修改usb摄像头串口映射，通过命令控制机械臂

从零开始搭二维激光SLAM --- 基于Hector的栅格地图的构建

精选机械臂套装与机械臂结合案例介绍！

ROS部署到机器人小车并实现底层电机控制过程中遇到的问题

ROS 2设计文章系列之十七——ROS 2中实时系统实现的提议

Webots小车与Gampping建图仿真与ORB3算法实践

评论（0）

关于作者

门前小恐龙

8

0

0

1

机器人导航(仿真)-01

Ubuntu16.04安装VS Code

URDF集成Gazebo

相关推荐

cartographer环境最新配置（2021.11.11）

听说你只会用Gazebo仿真？

Powering the world!

URDF优化：xacro

精选aubo i5 + realsense D435i识别抓取实践三

stm32与ros进行串口通信

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板