ROS学习笔记12：机器人导航实现（理论）

前言

本人ROS小白，利用寒假时间学习ROS，在此以笔记的方式记录自己每天的学习过程。争取写满20篇(12/20)。
环境：Ubuntu20.04、ROS1：noetic
环境配置：严格按照下方学习链接的教程配置，基本一次成功。
学习链接：【Autolabor初级教程】ROS机器人入门
对应链接文档：ROS机器人入门课程《ROS理论与实践》
笔记绝大部分代码使用Python语言编写。
本期关键词：SLAM，地图服务，定位，路径规划，导航+SLAM

SLAM

SLAM（Simultaneous Localization and Mapping）是一种机器人技术，全称为“同时定位与地图构建”。它允许移动机器人（如无人驾驶车辆、无人机、探索机器人等）在未知环境中进行自主导航，同时完成定位和地图构建两个任务。可以通过这个链接更形象地了解SLAM：什么是SLAM。

gmapping

1. gmapping 是ROS开源社区中较为常用且比较成熟的SLAM算法之一。可以先用roscd gmapping指令测试自己是否安装了gmapping，没有的话再通过以下指令安装：

sudo apt install ros-noetic-gmapping

2. gmapping节点涉及到的话题、服务、参数等信息比较多，我们记住常用的：
   • 话题类：
   • tf (tf/tfMessage)：坐标变换消息
   • scan(sensor_msgs/LaserScan)：雷达信息
   • map(nav_msgs/OccupancyGrid)：地图数据，一般rviz作为订阅方
   • 参数类：
   • map_update_interval(float, default: 5.0)：地图更新频率
   • maxUrange(float, default: 80.0)：激光雷达探测的最大可用范围
   • maxRange(float)：激光探测的最大范围

gmapping节点的编写：

<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" />
launch>

1. gmapping节点涉及到的param（参数）很多，一般不建议自己手搓，在模板的基础上修改即可。
2. 以下就是gmapping节点对应的模板：

<launch>
	<param name="use_sim_time" value="true"/>
	<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
		<remap from="scan" to="base_scan"/>
		<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>

3. 这里：如果值为true，表示使用仿真时间，在gazebo仿真可以设置为true，其他场景下设置为false就行。

地图服务

地图服务与map_server功能包相关，该功能包包含两个节点：map_saver和map_server。

map_saver地图保存

1. map_saver节点订阅一个话题：/map(nav_msgs/OccupancyGrid)。
2. 启用地图保存的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>

3. 在arg标签的value参数里指定地图文件的保存路径。nav是文件名成，保存后包含两个文件：nav.yaml和nav.pgm。
4. .pgm文件本质就是一张图片，可以直接打开，也可以通过ps等软件对该图片文件进行修改，例如通过ps在图片上画黑线来模拟障碍物。
5. .yaml文件保存了地图的元数据信息，内容格式如下：

image: 地图信息保存的路径
resolution: 0.050000
origin: [-5.000000, -16.800000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

• resolution：图片的分辨率(单位: m/像素)。
• origin：地图右下角距离rviz中心点的偏移量，这里在x上偏移了-5m，y上偏移了-16.8m，偏航角度为0。
• negate：是否将白色与黑色颠倒，即白色变为障碍物，黑色变会无障碍地图
• occupied_thresh：占用率大于该阈值被视为障碍物。
• free_thresh：占用率小于该阈值被视为无障碍。
• 占用率求法：设像素点值为x，$p=\frac{255-x}{255}$

map_server地图发布

1. 该节点主要会发布/map地图数据话题。
2. 编写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>

3. 打开rviz即可观察到对应的地图。

定位

所谓定位就是推算机器人自身在全局地图中的位置。这里的定位主要指导航中的机器人定位，SLAM虽然也有自己的定位实现，但是和导航的定位实现是不一样的。导航的定位实现使用的是AMCL，自适应蒙特卡罗定位。

AMCL

1. AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。可以通过以下链接更形象地了解AMCL：[ROS 2D导航原理系列（二）|自适应蒙特卡罗定位AMCL](ROS 2D导航原理系列（二）|自适应蒙特卡罗定位AMCL_哔哩哔哩_bilibili)。
2. AMCL节点涉及到的话题、服务、参数等也比较多，编写launch文件是也不建议自己手搓，可以在官方给的模板的基础上修改。按照以下步骤，即可查看参考模板，因为我的机器人是差速运动的，所以选择了diff。

roscd amcl
cd examples/
gedit amcl_diff.launch

3. AMCL节点会发布particlecloud(geometry_msgs/PoseArray)话题，在rviz中可以用PoseArray插件订阅该话题。

AMCL文件编写

1. 以下是赵虚左老师教程文档里对官方launch模板的修改：（主要修改坐标系关系和里程计模式）

<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
  
  <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"/>
  
  <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_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>

2. 对应launch文件：（这里使用include标签包含刚刚编写好的amcl.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)"/>
    
    <include file="$(find mycar_nav)/launch/amcl.launch" />
    
    <node pkg="rviz" type="rviz" name="rviz"/>
launch>

路径规划

move_base

1. move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。action类似服务通信，其中间过程多出了连续反馈。
2. move_base节点也是包含很多话题、服务、参数信息，这里列举几个常用的：
   • 订阅
   • move_base/goal(move_base_msgs/MoveBaseActionGoal)：move_base 的运动规划目标。
   • move_base/cancel(actionlib_msgs/GoalID)：取消目标。
   • move_base_simple/goal(geometry_msgs/PoseStamped)：运动规划目标，只是单一的目标点，无连续反馈。
   • 发布
   • move_base/feedback (move_base_msgs/MoveBaseActionFeedback)：连续反馈的信息
   • cmd_vel(geometry_msgs/Twist)：运动控制消息

代价地图

1. 代价地图包括全局代价地图和局部代价地图，想深入了解一下该知识点的，可以通过赵虚左老师的教程视频来学习，链接奉上：代价地图概念。
2. 我是这样理解代价地图的：代价地图就是在静态地图（原始地图）的基础上，在障碍物的周围通过图像来表现”代价“，这个代价可以理解为机器人经过这个区域时会付出的代价，颜色越深，要付出的代价越高，所以算法会让机器人导航时尽量避免这些区域。

move_base使用

路径规划算法的基本实现步骤：

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

1. launch文件模块编写：（move_base模板）

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

2. 配置文件编写，其实就是编写上面launch文件里用到的四个yaml文件：

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

• 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

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

• 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 # 圆体旋转角速度（就是车旋转180°，调转方向）

  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

3. 如果导航的过程中，机器人和全局路径规划的偏差比较大时，可以调整base_local_planner_params.yaml文件里的前进模拟参数。
4. 以上配置，全局和本地代价地图的“膨胀半径”和“代价系数”都设置为相同的（在 costmap_common_params.yaml文件里），实际上，我们可以将其设置为不同。方法就是不在通用参数的yaml文件里设置inflation_radius和cost_scaling_factor参数，而是把这两个参数的设置分别放在global_costmap_params.yaml和local_costmap_params.yaml文件里。
5. 第4点的设置，也可以避免出现机器人在本地路径规划时与全局路径规划不符而进入膨胀区域出现假死的情况。

导航与SLAM建图同时进行

实现起来也比较简单，就是通过launch文件启动SLAM节点和move_base节点，中间省去了地图的保存（map_saver）和地图的发布（map_server），导航模块直接订阅SLAM发布的/map话题即可。
launch文件示例如下：

<launch>
    
    <include file="$(find mycar_nav)/launch/slam.launch" />
    
    <include file="$(find mycar_nav)/launch/path.launch" />
    
    <node pkg="rviz" type="rviz" name="rviz" />
launch>

往期内容

1. ROS学习笔记1：基础知识
2. ROS学习笔记2：话题通信
3. ROS学习笔记3：服务通信与参数服务器
4. ROS常用命令记录
5. ROS学习笔记4：通信机制实操
6. ROS学习笔记5：常用API和模块导入
7. ROS学习笔记6：launch文件
8. ROS学习笔记7：重名解决与名称设置
9. ROS学习笔记8：TF坐标变换
10. ROS学习笔记9：TF坐标变换实操
11. ROS学习笔记10：rosbag与rqt
12. ROS学习笔记11：URDF