【ROS导航Navigation】三 | 导航实现 | gmapping建图 | map_server地图服务 | amcl定位 | move_base路径规划

致谢：ROS赵虚左老师

一、依赖功能包 | gmapping | map-server | navigation

二、gmapping 建图

1.costmap_common_params.yaml 通用参数

2.global_costmap_params.yaml 全局代价地图

3.local_costmap_params.yaml 局部代价地图

4.base_local_planner_params 局部规划器（速度）

六、导航实现

致谢：ROS赵虚左老师

Introduction · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程

参考赵虚左老师的实战教程

一、依赖功能包 | gmapping | map-server | navigation

安装 gmapping 包(用于构建地图):

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

安装地图服务包map-server(用于保存与读取地图):

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

安装 navigation 包(用于定位以及路径规划):

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

二、gmapping 建图

使用gmapping要求：

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

需要订阅/发布的消息：

2.1订阅的Topic

tf (tf/tfMessage)

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

scan(sensor_msgs/LaserScan)

SLAM所需的雷达信息。

2.2发布的Topic

map_metadata(nav_msgs/MapMetaData)

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

map(nav_msgs/OccupancyGrid)

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

~entropy(std_msgs/Float64)

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

2.3服务

dynamic_map(nav_msgs/GetMap)

用于获取地图数据。

调参用的参数：

2.4参数

gmapping - ROS Wiki

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

激光探测的最大范围。

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

2.5坐标系变换

雷达坐标系→基坐标系

一般由 robot_state_publisher 或 static_transform_publisher 发布。

基坐标系→里程计坐标系

一般由里程计节点发布。

2.6发布的坐标变换

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

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

【gmapping使用】

编写gmapping节点相关launch文件

参考链接：https://github.com/ros-perception/slam_gmapping/blob/melodic-devel/gmapping/launch/slam_gmapping_pr2.launch

主要是这两行：

雷达坐标系→基坐标系

一般由 robot_state_publisher 或 static_transform_publisher 发布。

<launch>
<param name="use_sim_time" value="true"/>
    <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>

三、map_server 地图服务

map_server功能包中提供了两个节点:

map_saver 和 map_server

3.1map_saver 保存地图

订阅的话题

map(nav_msgs/OccupancyGrid)

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

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>

启动launch文件后会保存一张pgm地图和yaml地图参数：

yaml文件：

image: /home/rosmelodic/ws02_nav/src/mycar_nav/map/nav.pgm # 图片的路径
resolution: 0.050000  # 地图刻度尺， 米/像素
origin: [-50.000000, -50.000000, 0.000000]  # 地图位姿信息
negate: 0  # 取反
occupied_thresh: 0.65  # 占用阈值
free_thresh: 0.196  # 空闲阈值

yaml文件参数：

image:被描述的图片资源路径，可以是绝对路径也可以是相对路径。
resolution: 图片分片率(单位: m/像素)。
origin: 地图中左下像素的二维姿势，为（x，y，偏航），偏航为逆时针旋转（偏航= 0表示无旋转）。
occupied_thresh: 占用概率大于此阈值的像素被视为完全占用。
free_thresh: 占用率小于此阈值的像素被视为完全空闲。
negate: 是否应该颠倒白色/黑色自由/占用的语义。

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 那么视为无物。

备注:

图片也可以根据需求编辑。

3.2map_server 读取地图

发布的话题

map_metadata（nav_msgs / MapMetaData）

发布地图元数据。

map（nav_msgs / OccupancyGrid）

地图数据。

服务

static_map（nav_msgs / GetMap）

通过此服务获取地图。

参数

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

地图坐标系。

launch文件：

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

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

rviz显示地图：

四、amcl 定位

AMCL(adaptive Monte Carlo Localization)

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

amcl已经被集成到了navigation包，navigation前面已经安装。

amcl - ROS Wiki

4.1订阅的Topic

scan(sensor_msgs/LaserScan)

激光雷达数据。

tf(tf/tfMessage)

坐标变换消息。

initialpose(geometry_msgs/PoseWithCovarianceStamped)

用来初始化粒子滤波器的均值和协方差。

map(nav_msgs/OccupancyGrid)

获取地图数据。

4.2发布的Topic

amcl_pose(geometry_msgs/PoseWithCovarianceStamped)

机器人在地图中的位姿估计。

particlecloud(geometry_msgs/PoseArray)

位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合。

tf(tf/tfMessage)

发布从 odom 到 map 的转换。

4.3服务

global_localization(std_srvs/Empty)

初始化全局定位的服务。

request_nomotion_update(std_srvs/Empty)

手动执行更新和发布更新的粒子的服务。

set_map(nav_msgs/SetMap)

手动设置新地图和姿态的服务。

4.4调用的服务

static_map(nav_msgs/GetMap)

调用此服务获取地图数据。

4.5参数

~odom_model_type(string, default:"diff")

里程计模型选择: "diff","omni","diff-corrected","omni-corrected" (diff 差速、omni 全向轮)

~odom_frame_id(string, default:"odom")

里程计坐标系。

~base_frame_id(string, default:"base_link")

机器人极坐标系。

~global_frame_id(string, default:"map")

地图坐标系。

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

4.6坐标变换

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

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

【amcl使用】

amcl包已经给出示例：

roscd amcl
ls examples

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>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mycar_nav)/map/$(arg map)"/>
    <!-- 启动AMCL节点 -->
    <include file="$(find mycar_nav)/launch/amcl.launch" />
    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz"/>
</launch>

五、move_base 路径规划

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

move_base已经被集成到了navigation包，navigation安装前面也有介绍

服务-客户（请求响应服务）：

动作（action）：

实时订阅客户端

5.1动作

动作订阅

move_base/goal(move_base_msgs/MoveBaseActionGoal)

move_base 的运动规划目标。

move_base/cancel(actionlib_msgs/GoalID)

取消目标。

动作发布

move_base/feedback(move_base_msgs/MoveBaseActionFeedback)

连续反馈的信息，包含机器人底盘坐标。

move_base/status(actionlib_msgs/GoalStatusArray)

发送到move_base的目标状态信息。

move_base/result(move_base_msgs/MoveBaseActionResult)

操作结果(此处为空)。

5.2订阅的Topic

move_base_simple/goal(geometry_msgs/PoseStamped)

运动规划目标(与action相比，没有连续反馈，无法追踪机器人执行状态)。

5.3发布的Topic

cmd_vel(geometry_msgs/Twist)

输出到机器人底盘的运动控制消息。

5.4服务

~make_plan(nav_msgs/GetPlan)

请求该服务，可以获取给定目标的规划路径，但是并不执行该路径规划。

~clear_unknown_space(std_srvs/Empty)

允许用户直接清除机器人周围的未知空间。

~clear_costmaps(std_srvs/Empty)

允许清除代价地图中的障碍物，可能会导致机器人与障碍物碰撞，请慎用。

5.5参数

参考官网：http://wiki.ros.org/move_base

5.6代价地图costmap

代价地图有两张:

global_costmap(全局代价地图)

local_costmap(本地代价地图)

前者用于全局路径规划，后者用于本地路径规划。

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

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

多个layer可以按需自由搭配。

5.7代价值

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

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

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

5.8【launch文件】

move_base启动的launch文件

<launch>

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

</launch>

1.costmap_common_params.yaml 通用参数

#机器人几何参，如果机器人是圆形，设置 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}

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.

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

4.base_local_planner_params 局部规划器（速度）

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

六、导航实现

集成【地图服务】、【amcl】、【move_base】与【 Rviz】

【launch文件】的launch文件

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

    <!-- 运行move_base节点 -->
    <include file="$(find mycar_nav)/launch/path.launch" />
    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mycar_nav)/rviz/nav.rviz" />

</launch>

以上配置在实操中，可能会出现机器人在本地路径规划时与全局路径规划不符而进入膨胀区域出现假死的情况，如何尽量避免这种情形：

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

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

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

2023.11.15

渝北仙桃数据谷