文章目录

• 前言
• 准备条件
• 1. 导航实现01_SLAM建图
• • 1.1 gmapping简介
  • 1.2 gmapping节点说明
  • 1.3 gmapping使用
  • • 1.3.1 编写gmapping节点相关launch文件
    • 1.3.2 执行
• 2. 导航实现02_地图服务
• • 2.1 map_server简介
  • 2.2 map_server使用之地图保存节点(map_saver)
  • • 2.2.1 map_saver节点说明
    • 2.2.2 地图保存launch文件
  • 2.3 map_server使用之地图服务(map_server)
  • • 2.3.1 map_server节点说明
    • 2.3.2 地图读取
    • 2.3.3 地图显示
• 3. 导航实现03_定位
• • 3.1 amcl简介
  • 3.2 amcl节点说明
  • 3.3 坐标变换
  • 3.4 amcl使用
  • • 3.4.1 编写amcl节点相关的launch文件
    • 3.4.2 编写测试launch文件
    • 3.4.3 执行
• 4. 导航实现04_路径规划
• • 4.1 move_base简介
  • 4.2 move_base节点说明
  • 4.3 move_base与代价地图
  • • 4.3.1 概念
    • 4.3.2 组成
    • 4.3.3 碰撞算法
  • 4.4 move_base使用
  • • 4.4.1 launch文件
    • 4.4.2 配置文件
    • • 4.4.2.1 costmap_common_params.yaml
      • 4.4.2.2 global_costmap_params.yaml
      • 4.4.2.3 local_costmap_params.yaml
      • 4.4.2.4 base_local_planner_params
      • 4.4.2.5 参数配置技巧
    • 4.4.3 launch文件集成
    • 4.4.4 测试
• 5. 导航与SLAM建图
• • 5.1 编写launc文件
  • 5.2 测试

前言

📢本系列将依托赵虚左老师的ROS课程，写下自己的一些心得与笔记。
📢课程链接:https://www.bilibili.com/video/BV1Ci4y1L7ZZ
📢讲义链接:http://www.autolabor.com.cn/book/ROSTutorials/index.html
📢 文章可能存在疏漏的地方，恳请大家指出。

准备条件

安装相应功能包:

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

创建功能包,暂时只需用到以下依赖:gmapping map_server amcl move_base

1. 导航实现01_SLAM建图

1.1 gmapping简介

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

• 该移动机器人可以发布里程计消息

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

1.2 gmapping节点说明

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

订阅的Topic

发布的Topic

服务

参数

所需的坐标变换

发布的坐标变换

1.3 gmapping使用

1.3.1 编写gmapping节点相关launch文件

launch文件编写可以参考 github 的演示 launch文件：https://github.com/ros-perception/slam_gmapping/blob/melodic-devel/gmapping/launch/slam_gmapping_pr2.launch
复制并修改如下:
nav01_slam.launch

1.3.2 执行

1.先启动 Gazebo 仿真环境demo01_gazebo.launch

2.然后再启动地图绘制的 launch 文件:
roslaunch 包名 launch文件名
3.启动键盘键盘控制节点，用于控制机器人运动建图
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
4.在 rviz 中添加组件，显示栅格地图

最后，就可以通过键盘控制gazebo中的机器人运动，同时，在rviz中可以显示gmapping发布的栅格地图数据了，下一步，还需要将地图单独保存。

2. 导航实现02_地图服务

2.1 map_server简介

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

2.2 map_server使用之地图保存节点(map_saver)

2.2.1 map_saver节点说明

订阅的topic:

• map(nav_msgs/OccupancyGrid)
  订阅此话题用于生成地图文件。

2.2.2 地图保存launch文件

地图保存的语法比较简单，编写一个launch文件，内容如下:
nav02_mapsave.launch

SLAM建图完毕后，执行该launch文件即可,在指定路径下会生成两个件，xxx.pgm 与 xxx.yaml

xxx.pgm 本质是一张图片，直接使用图片查看程序即可打开。

xxx.yaml 保存的是地图的元数据信息，用于描述图片，内容格式如下:

image: /home/yuan/catkin_ws/src/nav_demo/map/test01.pgm
resolution: 0.050000
origin: [-50.000000, -50.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

PS: 若要更改地图文件名称,yaml中的images部分也得修改

map_server 中障碍物计算规则:

• 地图中的每一个像素取值在 [0,255] 之间，白色为 255，黑色为 0，该值设为 x；
• map_server 会将像素值作为判断是否是障碍物的依据，首先计算比例: p = (255 - x) / 255.0，白色为0，黑色为1(negate为true，则p = x / 255.0)；
• 根据步骤2计算的比例判断是否是障碍物，如果 p > occupied_thresh 那么视为障碍物，如果 p < free_thresh 那么视为无物。

2.3 map_server使用之地图服务(map_server)

2.3.1 map_server节点说明

发布的话题

服务

参数

2.3.2 地图读取

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

其中参数是地图描述文件的资源路径，执行该launch文件，该节点会发布话题:map(nav_msgs/OccupancyGrid)

2.3.3 地图显示

在 rviz 中使用 map 组件可以显示栅格地图：

3. 导航实现03_定位

3.1 amcl简介

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

3.2 amcl节点说明

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

订阅的Topic

发布的Topic

服务

调用的服务

参数

3.3 坐标变换

里程计本身也是可以协助机器人定位的，不过里程计存在累计误差且一些特殊情况时(车轮打滑)会出现定位错误的情况，amcl 则可以通过估算机器人在地图坐标系下的姿态，再结合里程计提高定位准确度。

• 里程计定位:只是通过里程计数据实现 /odom_frame 与 /base_frame 之间的坐标变换。
• amcl定位: 可以提供 /map_frame 、/odom_frame 与 /base_frame 之间的坐标变换。
  

3.4 amcl使用

关于launch文件的实现，在amcl功能包下的example目录已经给出了示例，可以作为参考，具体实现:

roscd amcl
ls examples

该目录下会列出两个文件: amcl_diff.launch 和 amcl_omni.launch 文件，前者适用于差分移动机器人，后者适用于全向移动机器人.

3.4.1 编写amcl节点相关的launch文件

nav04_amcl.launch

3.4.2 编写测试launch文件

test_amcl.launch

3.4.3 执行

1.先启动 Gazebo 仿真环境

2.启动键盘控制节点：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

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

5.通过键盘控制机器人运动，会发现 posearray 也随之而改变。

开始时箭头发散
运行后箭头聚拢,且感知信息得到矫正.

4. 导航实现04_路径规划

4.1 move_base简介

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

4.2 move_base节点说明

动作

动作订阅

动作发布

订阅的Topic

发布的Topic

服务

4.3 move_base与代价地图

4.3.1 概念

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

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

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

4.3.2 组成

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

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

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

4.3.3 碰撞算法

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

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

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

4.4 move_base使用

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

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

4.4.1 launch文件

nav05_path.launch

4.4.2 配置文件

关于配置文件的编写，可以参考一些成熟的机器人的路径规划实现，比如: turtlebot3，github链接：https://github.com/ROBOTIS-GIT/turtlebot3/tree/master/turtlebot3_navigation/param，先下载这些配置文件备用。

在功能包下新建 param 目录，复制下载的文件到此目录: costmap_common_params_burger.yaml、local_costmap_params.yaml、global_costmap_params.yaml、base_local_planner_params.yaml，并将costmap_common_params_burger.yaml 重命名为:costmap_common_params.yaml。

配置文件修改以及解释:

4.4.2.1 costmap_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}

4.4.2.2 global_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.

4.4.2.3 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 # 局部地图宽度 单位是 mheight: 3 # 局部地图高度 单位是 mresolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

4.4.2.4 base_local_planner_params

基本的局部规划器参数配置，这个配置文件设定了机器人的最大和最小速度限制值，也设定了加速度的阈值。

TrajectoryPlannerROS:# Robot Configuration Parametersmax_vel_x: 0.5 # X 方向最大速度min_vel_x: 0.1 # X 方向最小速速max_vel_theta:  1.0 # min_vel_theta: -1.0min_in_place_vel_theta: 1.0acc_lim_x: 1.0 # X 加速限制acc_lim_y: 0.0 # Y 加速限制acc_lim_theta: 0.6 # 角速度加速限制# Goal Tolerance Parameters，目标公差xy_goal_tolerance: 0.10yaw_goal_tolerance: 0.05# Differential-drive robot configuration
# 是否是全向移动机器人holonomic_robot: false# Forward Simulation Parameters，前进模拟参数sim_time: 0.8vx_samples: 18vtheta_samples: 20sim_granularity: 0.05

4.4.2.5 参数配置技巧

全局路径规划与本地路径规划虽然设置的参数是一样的，但是二者路径规划和避障的职能不同，可以采用不同的参数设置策略:

• 全局代价地图可以将膨胀半径和障碍物系数设置的偏大一些；

• 本地代价地图可以将膨胀半径和障碍物系数设置的偏小一些。

这样，在全局路径规划时，规划的路径会尽量远离障碍物，而本地路径规划时，机器人即便偏离全局路径也会和障碍物之间保留更大的自由空间，从而避免了陷入“假死”的情形。

global_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 #等待坐标变换发布信息的超时时间#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物inflation_radius: 0.4#代价比例系数，越大则代价值越小cost_scaling_factor: 10.0static_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 #等待坐标变换发布信息的超时时间#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物inflation_radius: 0.3#代价比例系数，越大则代价值越小cost_scaling_factor: 3.0static_map: false  #不需要静态地图，可以提升导航效果rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化width: 3 # 局部地图宽度 单位是 mheight: 3 # 局部地图高度 单位是 mresolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

初始状态:
更改过后
具体效果见章末导航测试2.

4.4.3 launch文件集成

如果要实现导航，需要集成地图服务、amcl 、move_base 与 Rviz 等，集成示例如下:
nav06_path.launch

4.4.4 测试

1.先启动 Gazebo 仿真环境；
2.启动导航相关的 launch 文件；
3.添加Rviz组件(参考演示结果),可以将配置数据保存，后期直接调用；

PS1: 每次测试后,若要恢复初态,需要重启roscore.

PS2: 若出现rviz与gazebo中地图不重合的问题,可以如下操作

点击绿色的箭头 2D Pose Estimate 调整机器人初始位置.
重启rviz,即可得到以下结果.
结果:

ROS_导航_测试1_未对参数进行修改

ROS_导航测试2

5. 导航与SLAM建图

SLAM建图过程中本身就会时时发布地图信息，所以无需再使用map_server，SLAM已经发布了话题为 /map 的地图消息了，且导航需要定位模块，SLAM本身也是可以实现定位的。

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

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

5.1 编写launc文件

当前launch文件实现，无需调用map_server的相关节点，只需要启动SLAM节点与move_base节点，示例内容如下:
nav07_slam.launch

5.2 测试

1.首先运行gazebo仿真环境；

2.然后执行launch文件；

3.在rviz中通过2D Nav Goal设置目标点，机器人开始自主移动并建图了；

4.最后可以使用 map_server 保存地图。
大致过程如下:

ROS_导航_测试3