ROS理论与实践(以移动机器人为例)连载(八) ——机器人SLAM建图

本章着重讲如何在ROS里面使用SLAM功能包，而不会去讲解算法；之后会专门的专栏做SLAM。

代码包：链接: https://pan.baidu.com/s/1MkWs9ppcX2IeFPH16YLFRw 密码: l1ve

---

1. SLAM原理简介

放几张GIF动图感受一下：

这边再放上一个视频链接Autonomous Aerial Navigation in Confined Indoor Environments是关于一个无人机室内建图的。

还有这个Visual SLAM Car Navigation是关于使用iPhone5做视觉SLAM的。

• SLAM（Simultaneout Localization and Mapping）：即使定位与地图构建。即机器人自身位置不确定的情况下，在完全未知的环境中创建地图，同时利用地图进行自主定位和导航；
• 定位，在地图上估测机器人的位置坐标；
• 地图构建，这一过程是根据传感器的结果来构建一张地图或者是修正当前地图，同时将结果提供给定位算法做为先验地图。

其实我感觉第一本书皮好看呀，两本书的PDF都可以私信我，由于版权问题就不公布了！

机器人必须安装激光雷达等测距设备，可以获取环境深度信息。 单线雷达，反馈的是二维的平面信息。多线雷达，反馈的是三维信息。（左边是单线，也就是TurtleBot3配置的雷达；右边是多线雷达）

2. ROS机器人配置与数据结构

①单线雷达

• angle_min：可检测范围的起始角度；
• angle_max：可检测范围的终止角度，与angle_min组成激光雷达的可检测范围；
• angle_increment：相邻数据帧之间的角度步长；
• time_increment：采集到相邻数据帧之间的时间步长，当传感器处于相对运动状态时进行补偿使用；
• scan_time：采集一帧数据所需要的时间；
• range_min：最近可检测深度的阈值；
• range_max：最远可检测深度的阈值；
• ranges：一帧深度数据的存储数组。

像Kinect这种RGB-D相机，同样可以把收集到的三个维度的信息通过 depthimage_to_laserscan 这个功能包把点云深度数据转换成激光数据。

sudo apt-get install ros-melodic-depthimage-to-laserscan

使用功能包长着个样↓

<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" output="screen">
	<remap from="image" to="/kinect/depth/image_raw" />
	<remap from="camera_info" to="/kinect/depth/camera_info" />
	<remap from="scan" to="/scan" />
	<param name="output_frame_id" value="/camera_link" />
node>

②里程计

• pose：机器人当前位置坐标，包括机器人的 XYZ 三轴位置与方向参数，以及用于矫正误差的协方差矩阵；
• twist：机器人当前的运动状态，包括 XYZ 三轴的线速度与角速度，以及用于矫正误差的协方差矩阵。

使用 Building Editor 创建仿真环境，可见ROS理论与实践(以移动机器人为例)连载(五) ——构建机器人仿真平台。

代码包：链接: https://pan.baidu.com/s/1hMKzm2sozhmIFIn6ZIQQMQ 密码: 1bm8

可以运行↓前提是把提供的功能包放在工作空间下并进行编译

$ roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch

3. 常用SLAM功能包应用

①gmapping

• 基于激光雷达
• Rao-Blackwellized 粒子滤波算法
• 二维栅格地图
• 需要机器人提供里程计信息
• OpenSlam开源算法
• 输出地图话题：nav_msgs/OccupancyGrid
  
  论文可参考：http://openslam.org/gmapping.html 。

安装gmapping

$ sudo apt-get install ros-melodic-gmapping

gmapping功能包中的话题和服务：

gmapping功能包中的TF变换：

参考：http://wiki.ros.org/gmapping/。

该如何使用 gmapping 呢？我们来配置一下 gmapping 的节点↓

<launch>
    <arg name="scan_topic" default="scan" />

    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">
        <param name="odom_frame" value="odom"/>
        <param name="map_update_interval" value="5.0"/>
        
        <param name="maxRange" value="5.0"/>
        <param name="maxUrange" value="4.5"/>
        <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.01"/>
        <param name="srt" value="0.02"/>
        <param name="str" value="0.01"/>
        <param name="stt" value="0.02"/>
        <param name="linearUpdate" value="0.5"/>
        <param name="angularUpdate" value="0.436"/>
        <param name="temporalUpdate" value="-1.0"/>
        <param name="resampleThreshold" value="0.5"/>
        <param name="particles" value="80"/>
        <param name="xmin" value="-1.0"/>
        <param name="ymin" value="-1.0"/>
        <param name="xmax" value="1.0"/>
        <param name="ymax" value="1.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"/>
        <remap from="scan" to="$(arg scan_topic)"/>
    node>
launch>

需要注意的是雷达的话题名为scan，里程计的话题名为odom，需要根据实际情况进行更改；第一次使用不用关心具体某个参数的含义，使用默认的一套参数即可。

同样提供的功能包里有demo，可以这样运行：

$ roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch
$ roslaunch mbot_navigation gmapping_demo.launch
$ roslaunch mbot_teleop mbot_teleop.launch

效果图：

分为 浅灰色——空闲的； 深灰色——未探索的； 黑色——占据的。

保存地图：

$ rosrun map_server map_saver -f cloister_gmapping

同样我们可以使用Kinect将三维点云信息转换为二维信息，也提供了编辑好的功能包：

$ roslaunch mbot_gazebo mbot_kinect_nav_gazebo.launch
$ roslaunch mbot_navigation gmapping_demo.launch
$ roslaunch mbot_teleop mbot_teleop.launch

但是由于传感器信息变化较少，对建图会有很大影响。

②hector_slam

• 基于激光雷达，对传感器要求比较高
• 高斯牛顿方法
• 二维栅格地图
• 不需要里程计数据
• 输出地图话题： nav_msgs/OccupancyGrid
• 会对外输出一个里程计信息

hector功能包中的话题和服务：

hector功能包中的TF变换：

安装hector_slam：

$ git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git

在给出的功能包里面已经包含了这个，直接编译就好，不过我编译的时候出现了点问题，主要就是CMakeLists.txt没有声明用c++11编译加上

add_compile_options(-std=c++11)

使用hector的launch文件怎么写？

<launch>

    <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
        
        <param name="pub_map_odom_transform" value="true"/>
        <param name="map_frame" value="map" />
        <param name="base_frame" value="base_footprint" />
        <param name="odom_frame" value="odom" />

        
        <param name="use_tf_scan_transformation" value="true"/>
        <param name="use_tf_pose_start_estimate" value="false"/>

        
        <param name="map_resolution" value="0.05"/>
        <param name="map_size" value="2048"/>
        <param name="map_start_x" value="0.5"/>
        <param name="map_start_y" value="0.5" />
        <param name="laser_z_min_value" value="-1.0" />
        <param name="laser_z_max_value" value="1.0" />
        <param name="map_multi_res_levels" value="2" />

        <param name="map_pub_period" value="2" />
        <param name="laser_min_dist" value="0.4" />
        <param name="laser_max_dist" value="5.5" />
        <param name="output_timing" value="false" />
        <param name="pub_map_scanmatch_transform" value="true" />

        
        <param name="update_factor_free" value="0.4"/>
        <param name="update_factor_occupied" value="0.7" />    
        <param name="map_update_distance_thresh" value="0.2"/>
        <param name="map_update_angle_thresh" value="0.06" />

         
        <param name="advertise_map_service" value="true"/>
        <param name="scan_subscriber_queue_size" value="5"/>
        <param name="scan_topic" value="scan"/>
    node>

launch>

启动hector_slam演示

$ roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch
$ roslaunch mbot_navigation hector_demo.launch
$ roslaunch mbot_teleop mbot_teleop.launch

效果图就不贴了。

③cartographer功能包

• 谷歌开源
• 基于图网络的优化方法
• 二维或三维条件下的定位及建图功能
• 设计目的是在计算资源有限的情况下，实时获取相对较高精度的2D地图
• 主要基于激光雷达

安装cartographer：

$ sudo apt-get install ros-melodic-cartographer-*

视频链接：Cartographer 3D SLAM Demo 。

仍然需要配置安装好的cartographer节点进行使用，launch文件：

<launch>  
  
  <param name="/use_sim_time" value="true" />  

  <node name="cartographer_node" pkg="cartographer_ros"  
      type="cartographer_node" args="
          -configuration_directory $(find mbot_navigation)/config
          -configuration_basename lidar.lua"
      output="screen"> 
    <remap from="scan" to="scan" />  
  node>  

  
  <node pkg="cartographer_ros" type="cartographer_occupancy_grid_node"
        name="cartographer_occupancy_grid_node" 
        args="-resolution 0.05" />

  <node name="rviz" pkg="rviz" type="rviz" required="true"  
        args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />  
launch>

可以看到cartographer_node中使用了一个lidar.lua脚本，是由lua语言编写的。需要在默认改动的主要是各个frame的名字。

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_footprint",
  published_frame = "base_footprint",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_odometry = true,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1

POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65

return options

保存地图：

$ rosservice call /finish_trajectory 0
$ rosservice call /write_state "{filename:'${HOME}/mymap.pbstream'}"
$ rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=${HOME}/mymap -pbstream_filename=${HOME}/mymap.pbstream -resolution=0.05

对于真实机器人，只需要解决odom和激光雷达的数据封装成话题发布出去，就能够使用现有的算法完成SLAM建图。

④rtabmap

一个三维建图的功能包。

安装rtabmap：

$ sudo apt-get install ros-melodic-rtabmap-ros

启动rtabmap演示：

$ roslaunch mbot_gazebo mbot_kinect_nav_gazebo.launch
$ roslaunch mbot_navigation rtabmap_demo.launch
$ roslaunch mbot_teleop mbot_teleop.launch

查看建图结果：

$ rtabmap-databaseViewer ~/.ros/rtabmap.db

---

写完感觉这节有点水哈Emmmm