移动机器人激光SLAM导航（三）：Hector SLAM 篇

参考引用

• Hector_Mapping ROS-Wiki
• 从零开始搭二维激光SLAM
• 机器人工匠阿杰
• wpr_simulation

移动机器人激光SLAM导航（文章链接汇总）

1. 基于滤波器的 SLAM 问题

1.1 什么是 SLAM

• 什么是SLAM

• SLAM 就是为了构建地图用的，这个地图可以保存下来，用于后续的定位及导航避障中，也有一些 SLAM 作为里程计在使用，始终提供估计的位姿，目前主流 SLAM 的结构分为前端里程计，后端优化，回环检测三个大模块

  • 前端里程计：始终累加位姿，作为里程计使用
  • 后端优化：使用图的结构模型，优化整体位姿，减小前端里程计产生的累计误差
  • 回环检测：可以提供一个更强烈的图结构的约束，能够更好的减小累计误差

1.2 SLAM 问题的数学表述

• 运动方程（提供对状态 $x$ 的先验 $x_{k-1}$，正向推理）
  • 其中 $x$ 位姿，$k-1/k$ 时刻，$u$ 控制器输入/运动测量，$w$ 噪声

$$x_k=f(x_{k-1},u_k,w_k)$$

• 观测方程（提供对状态 $x$ 的后验 $x_k$，由果溯因）
  • 其中 $z$ 传感器读数，$y$ 路标，$j$ 路标编号，$v$ 噪声

$$z_{k,j}=h(y_j,x_k,v_{k,j})$$

• 已知量：上一时刻位姿 $x_{k-1}$，控制器输入/运动测量 $u_k$，当前时刻对路标 $j$ 的观测 $z_{k,j}$
• 估计量：当前时刻机器人位姿 $x_k$（定位），路标 $j$ 的位置 $y_j$（建图）

1.3 SLAM 概率模型

• SLAM（Simultaneous Localization and Mapping）：给定传感器数据情况下，同时进行机器人位姿和地图估计

  • 得到一个精确的位姿需要与地图进行匹配
  • 得到一个良好的地图需要有精确的位姿

• SLAM 条件联合概率分布

  • $1:t$ 表示从起始到 t 时刻
  • $z_{1:t}$ 表示传感器观测数据（如 /scan）
  • $u_{1:t}$ 表示里程计测量数据（如 /odom）
  • $m$ 表示地图，$x_{1:t}$ 表示机器人轨迹/位姿估计（定位）

$$p(x_{1:t},m|z_{1:t},u_{1:t-1})$$

2. Hector_Mapping

2.1 简介

• Hector_Mapping 是一种无需里程计数据的 SLAM 方法，利用激光雷达获得二维姿态估计，虽然没有回环检测功能，但对于大多真实场景，它是较准确的，该系统已用于无人地面机器人/车辆、手持测绘设备、四旋翼无人机

• 硬件要求

  • 需要高精度的激光扫描仪（SICK、hokuyo 等），扫描周围环境时，节点会使用 TF 变换，因此无需将雷达固定，并且不需要里程计数据

2.2 话题节点

• 订阅的话题（Topic）
  • scan (sensor_msgs/LaserScan)：订阅 2D 激光雷达扫描数据
  • syscommand (std_msgs/String)：如果字符串等于 “reset”，则地图和机器人姿态将重置为初始状态
• 发布的话题（Topic）
  • map_metadata (nav_msgs/MapMetaData)：发布 Meta 地图数据（存储地图描述信息）
  • map (nav_msgs/OccupancyGrid)：发布占据栅格地图数据
  • slam_out_pose (geometry_msgs/PoseStamped)：原始的机器人位姿（无协方差）
  • poseupdate (geometry_msgs/PoseWithCovarianceStamped)：校正后的机器人位姿（具有不确定性的高斯估计）
• Service
  • dynamic_map (nav_msgs/GetMap)：获取地图数据
  • reset_map (std_srvs/Trigger)：调用这个服务来重置地图，Hector_Mapping 将从头开始一张全新的地图。注意，这不会重新启动机器人的姿势，而是会从上次记录的姿势重新开始
  • pause_mapping (std_srvs/SetBool)：调用此服务来停止/开始处理激光扫描
  • restart_mapping_with_new_pose (hector_mapping/ResetMapping)：调用此服务来重置地图、机器人的姿势并恢复建图（如果暂停）

2.3 TF 变换

• 必要的 TF 变换

  • <scan frame> → base_frame：通常为固定值，激光雷达坐标系 与 基坐标系 之间的变换，一般由 robot_state_publisher 或 static_transform_publisher 发布

• 发布的 TF 变换

  • map → odom：地图坐标系 与 里程计坐标系 之间的变换，估计机器人在地图中的位姿（仅在参数 “pub_map_odom_transform” 为 true 时提供）

• ROS 中常用坐标系

  • map：地图坐标系，也被称为世界坐标系，是静止不动的
  • odom：里程计坐标系，相对于 map 来说一般是静止的，有些情况下会变动（定位节点为了修正机器人的位姿从而改变了 map->odom 间的坐标变换）
  • base_footprint：位于机器人底盘中心在地面的投影，不提供高度信息，代表机器人的 2D 位姿
  • base_link：位于机器人几何中心，与机器人刚性连接，相对于 base_stabilized 坐标系增加了横滚角和俯仰角
  • base_stabilized：坐标系添加了机器人相对于 map/odom 层的高度信息（对于没有横滚/俯仰运动的平台，base_stabilized 等价于 base_link）
  • laser_link：激光雷达的坐标系，相对于base_link来说是静止的，因为雷达装在机器人上
    

2.4 建图测试

本小节使用 移动机器人激光SLAM导航（二）：运动控制与传感器篇 中安装的测试环境 wpr_simulation（hector_mapping 已在此环境中通过脚本安装）

• 在 wpr_ws 工作空间新建功能包 slam_pkg

  $ cd ~/wpr_ws/src
  $ catkin_create_pkg slam_pkg roscpp rospy std_msgs
  $ code .      # 在 VSCode 中编辑
  1
  2
  3

• 在 slam_pkg 中新建 launch 文件夹，并在 launch 文件夹中新建 hector.launch 文件

  <launch>
      <!-- 载入机器人和 SLAM 仿真环境 -->
      <!-- 如果使用实体机器人，则将下行代码替换为启动激光雷达和底盘控制的 launch 文件即可 -->
      <include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch" />
  
      <!-- Hector SLAM 节点 -->
      <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" />
  
      <!-- rviz 显示 -->
      <!-- 其中 args 是运行该 launch 文件并在 rviz 中添加相关选项后保存的配置文件 -->
      <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/slam.rviz" />
  
      <!-- 机器人运动控制节点 -->
      <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering" />
  </launch>
  1
  2
  3
  4
  5
  6
  7
  8
  9
  10
  11
  12
  13
  14
  15

• 编译并启动 hector.launch 建图

  $ cd ~/wpr_ws
  $ catkin_make
  $ source devel/setup.bash
  $ roslaunch slam_pkg hector.launch
  1
  2
  3
  4

2.5 建图参数设置 Parameters

• ~map_update_distance_thresh (double, default: 0.4)

  • 地图更新的移动距离阈值 [单位：米]，越小则更新越快
  • 每次地图更新后，机器人必须移动超过这个阈值，或满足 map_update_angle_thresh 参数描述的位移角度变化，才会再次更新地图

• ~map_update_angle_thresh (double, default: 0.9)

  • 地图更新的旋转角度闯值 [单位：弧度]，越小则更新越快
  • 每次地图更新后，机器人必须转动超过这个阈值，并产生超过 map_update_distance_thresh 阈值的位移才会再次更新地图

• ~map_pub_period (double, default: 2.0)

  • 地图发布的周期 [单位：秒]

<launch>
    <include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch" />
    
    <!-- Hector SLAM 节点 -->
    <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping">
        <param name="map_update_distance_thresh" value="0.1" />
        <param name="map_update_angle_thresh" value="0.1" />
        <param name="map_pub_period" value="0.1" />
    </node>

    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/slam.rviz" />

    <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering" />

</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

其他参数请参考 Hector_Mapping ROS-Wiki

• 调参效果对比 wpb_hector_comparison.launch

<launch>
  <!-- 第一个 Hector_Mapping 建图节点 -->
  <group ns="slam_1">
    <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping_1">

      <param name="map_update_distance_thresh" value="0.5"/>
      <param name="map_update_angle_thresh" value="0.5" />
      <param name="map_pub_period" value="0.2" />
      
      <param name="map_frame" value="slam_1/map" />
      <param name="base_frame" value="slam_1/base_footprint" />
      <param name="odom_frame" value="slam_1/odom" />
    </node>
  </group>

  <!-- 第二个 Hector_Mapping 建图节点 -->
  <group ns="slam_2">
    <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping_2">

      <param name="map_update_distance_thresh" value="0.1"/>
      <param name="map_update_angle_thresh" value="0.1" />
      <param name="map_pub_period" value="0.2" />

      <param name="map_frame" value="slam_2/map" />
      <param name="base_frame" value="slam_2/base_footprint" />
      <param name="odom_frame" value="slam_2/odom" />
    </node>
  </group>

  <!-- **************************** 分割线 **************************** -->

  <!-- 载入 SLAM 的仿真场景 -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
  <arg name="world_name" value="$(find wpr_simulation)/worlds/slam_simple.world"/>
  <arg name="paused" value="false"/>
  <arg name="use_sim_time" value="true"/>
  <arg name="gui" value="true"/>
  <arg name="recording" value="false"/>
  <arg name="debug" value="false"/>
  </include>

  <!-- 载入 1号机器人 -->
  <include file="$(find wpr_simulation)/launch/wpb_slam_template.launch">
      <arg name="robot_namespace" value="slam_1" /> 
      <arg name="local_x" value="0" /> 
      <arg name="local_y" value="-0.3" /> 
      <arg name="local_yaw" value="0" /> 
  </include>

  <!-- 载入 2号机器人 -->
  <include file="$(find wpr_simulation)/launch/wpb_slam_template.launch">
      <arg name="robot_namespace" value="slam_2" /> 
      <arg name="local_x" value="0" /> 
      <arg name="local_y" value="0.3" /> 
      <arg name="local_yaw" value="0" /> 
  </include>

  <!-- 运动控制 -->
  <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering"/>

  <!-- 速度话题分流 -->
  <node pkg = "topic_tools" type = "relay" name = "relay_1" args="/cmd_vel /slam_1/cmd_vel" />
  <node pkg = "topic_tools" type = "relay" name = "relay_2" args="/cmd_vel /slam_2/cmd_vel" />

</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65

3. TF 和 里程计

3.1 TF 系统

• 地面移动机器人在地图中的位姿描述方式（x, y, yaw）
  

• ROS 中通过 TF（TransForm，坐标系变换） 来获取机器人具体的定位/位姿数值

  • TF 主要用于描述两个坐标系之间的空间关系
  • TF 关系由特定的 ROS 节点以消息包的形式发布到 /tf 话题中去，其他节点通过订阅这个 /tf 话题来查询坐标系

• 案例测试

  # 利用 2.4 小节创建的 hector 建图包
  $ cd ~/wpr_ws
  $ source devel/setup.bash
  $ roslaunch slam_pkg hector.launch
  1
  2
  3
  4

• 查看 /tf 话题类型

  $ rostopic type /tf
  tf2_msgs/TFMessage
  1
  2

---

• 查看 /tf 话题数值

  $ rostopic echo /tf
  ...
  ---
  transforms: 
    - 
      header: 
        seq: 0
        stamp: 
          secs: 780
          nsecs: 481000000
        frame_id: "map"
      child_frame_id: "scanmatcher_frame"
      transform: 
        translation: 
          x: 2.27857303619
          y: 1.645611763
          z: 0.0
        rotation: 
          x: 0.0
          y: 0.0
          z: -0.352547165132
          w: 0.935794045908
  ...
  1
  2
  3
  4
  5
  6
  7
  8
  9
  10
  11
  12
  13
  14
  15
  16
  17
  18
  19
  20
  21
  22
  23

• 查看 TF 关系树

  $ rosrun rqt_tf_tree rqt_tf_tree
  1

3.2 里程计

本小节使用 移动机器人激光SLAM导航（二）：运动控制与传感器篇 中安装的测试环境

• hector_mapping 在长直走廊建图
  • 由于缺少参照物特征的变化，导致机器人无法估计自己的位移，建图失败
  • 解决办法：轮子转过的圈数×轮子周长=走过的距离（轮子里程计算法）

  $ cd ~/wpr_ws
  $ source devel/setup.bash
  $ roslaunch wpr_simulation wpb_corridor_hector.launch
  1
  2
  3

• GMapping 在长直走廊建图
  • 由于 GMapping 自带里程计算法，在里程计的帮助下，激光 SLAM 有效克服了建图过程中位移特征缺失的问题

  $ cd ~/wpr_ws
  $ source devel/setup.bash
  $ roslaunch wpr_simulation wpb_corridor_gmapping.launch
  1
  2
  3

• 激光雷达和里程计输出的 TF 坐标变换关系
  • 激光雷达：map --> base_footprint
  • 里程计：odom --> base_footprint
  • GMapping 的核心算法：先使用里程计推算机器人的位移，再使用激光雷达点云配准算法来修正里程计误差（如：轮子打滑） map --> odom --> base_footprint
    

更直观化的里程计演示请查看视频 什么是里程计