# 一、理论基础
SLAM可以描述为：机器人在位置环境下从一个未知位置开始移动，移动过程中根据位置估计和地图进行自身定位，同时建造增量式地图，实现机器人的自主定位和导航
可以理解为：一个盲人在一个位置环境中，双手作为他的传感器，不断探索周围的障碍物情况
SLAM生成的地图是机器人自主移动的主要蓝图
这类问题可以总结为：在服务机器人工作空间中，根据机器人自身的定位导航系统找到一条从起始状态到目标状态、可以避开障碍物的最优路径
对于机器人而言，传感器主要有以下几种类型：
激光雷达：研究最多，使用最频繁。优点是精度高、响应快、数据量小；缺点是成本高
摄像头：有单目摄像头和双目摄像头两种。优点是传感、实用性强；缺点是复杂度高、数据量大
RGB-D摄像头：不仅可以获取RGB信息，也可以获得深度信息。优点是成本低、功能强；缺点是视野窄、盲区大、噪声大
# 二、准备工作
ROS中SLAM和自主导航的相关功能包可以通用于各种移动机器人平台，但是为效果起见，有以下几个硬件要求：
- 导航功能包对差分、轮式机器人的效果好，且机器人可直接使用速度指令进行控制
- 导航功能包要求机器人必须安装激光雷达等测距设备
- 导航功能包以正方形和圆形机器人作为模板进行开发
# 2.1传感器信息
# 2.1.1环境深度信息
ROS在sensor_msgs包中定义了专用数据结构LaserScan用于存储激光信息
- angle_min：可检测范围的起始角度
- angle_max：可检测范围的终止角度
- angle_increment：采集到相邻数据帧之间的角度步长
- time_increment：采集到相邻数据帧之间的时间步长
- scan_time：采集一帧数据所需要的时间
- range_min：最近可检测深度的阈值
- range_max：最远可检测深度的阈值
- ranges：一帧深度数据的存储数组
如果机器人没有激光雷达，但是配备了Kinect等RGB-D摄像头，也可以通过红外摄像头获取周围环境的深度信息
其原理是：将大量数据拦腰斩断，只抽取其中一行数据，重新封装为LaserScan消息，虽然损失了大量有效数据，但是刚好可以满足2D SLAM
ROS中提供了相应功能包depthimage_to_laserscan可以用于处理该问题
<!-- 运行depthimage_to_laserscan节点，将点云深度数据转换成激光数据 -->
<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" output="screen">
<remap from="image" to="/camera/depth/image_raw"/>
<remap from="camera_info" to="/camera/depth/camera_info"/>
<remap from="scan" to="/scan"/>
<param name="output_frame_id" value="/canemra_link">
</node>
# 2.1.2里程计信息
里程计根据传感器获取的数据来估计机器人随时间发生的位置变化
在机器人平台中较为常见里程计是编码器
导航功能包要求机器人能够发布里程计nav_msgs/Odometry消息
该消息主要包含机器人在自由空间中的位置和速度估算值
- pose：机器人当前位置坐标，包括机器人的x、y、z三轴与方向参数用于矫正误差的协方差矩阵
- twist：机器人当前的运动状态，包括x、y、z三轴的线速度与角速度，用于矫正误差的协方差矩阵
# 2.2仿真平台
# 2.2.1创建仿真环境
使用Gazebo Building Editor工具绘制一个走廊的仿真环境并加入一些障碍物
创建完成后保存为.world文件
# 2.2.2加载机器人
使用激光雷达作为深度传感器，将机器人模型放到创建好的仿真环境中，启动launch文件
<launch>
<!-- 设置launch文件的参数 -->
<arg name="world_name" value="$(find mrobot_gazebo)/worlds/cloister.world"/>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<!-- 运行gazebo仿真环境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)" />
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- 加载机器人模型描述参数 -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mrobot_gazebo)/urdf/mrobot_with_rplidar.urdf.xacro'" />
<!-- 运行joint_state_publisher节点，发布机器人的关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
<!-- 运行robot_state_publisher节点，发布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" >
<param name="publish_frequency" type="double" value="50.0" />
</node>
<!-- 在gazebo中加载机器人模型-->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model mrobot -param robot_description"/>
</launch>
使用命令：
$ roslaunch mrobot_gazebo mrobot_laser_nav_gazebo.launch
·运行后查看话题列表如下：
# 三、gmapping
ROS开源社区中汇聚了多种SLAM算法，可以直接使用或进行二次开发，其中最为常用和成熟的gmapping功能包
# 3.1gmapping功能包
gmapping功能包集成了Rao-Blackwellized粒子滤波算法，而为开发者隐去了复杂的内部实现
gmapping功能包订阅了机器人的深度信息、IMU信息和里程计信息，同时完成一些必要的参数配置，即可创建并输出基于概率的二维栅格地图
使用如下方式安装：
$ sudo apt-get install ros-melodic-gmapping
# 3.2gmapping节点的配置与运行
SLAM算法已经在gmapping功能包中实现，无需深入理解算法的实现原理，只需要关注如何借助其提供的接口实现相应功能
# 3.2.1gmapping.launch
使用gmapping的第一步就是创建一个运行gmapping节点的乱传文件，主要用于节点的参数配置，文件位于mrobot_navigation/launch/gmapping.launch
<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"/>
<!-- Set maxUrange < actual maximum range of the Laser -->
<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>
gmapping节点订阅的激光雷达话题名为“/scan”，如果与机器人发布的激光雷达话题名不一致，需要使用<remap>进行重映射
# 3.2.2gmapping_demo.launch
创建一个启动gmapping例程的mrobot_navigation/launch/gmappig_demo.launch文件
<launch>
<include file="$(find mrobot_navigation)/launch/gmapping.launch"/>
<!-- 启动rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find mrobot_navigation)/rviz/gmapping.rviz"/>
</launch>
其包含两部分：
- 启动之前创建的gmapping节点
- 启动RViz界面，查看传感器和地图构建的实时信息
# 3.3在Gazebo中使用SLAM
使用命令启动Gazebo仿真环境和gmapping节点
$ roslaunch mrobot_gazebo mrobot_laser_nav_gazebo.launch
$ roslaunch mrobot_navigation gmapping_demo.launch
运行成功后如下所示
可以看到，机器人在RViz在同步显示，周围的红点是激光雷达传感器实时检测到的二维环境深度信息，并根据当前的深度信息建立了部分已知环境的地图
启动键盘让机器人动起来：
$ roslaunch mrobot_teleop mrobot_teleop.launch
在控制机器人探索完完整一周之后可以使用rosrun map_server map_saver命令保存当前地图
地图会保留在终端所在路径下，包含一个map.pgm和一个map.yaml文件
其中map.yaml文件是一个关于地图的配置文件
image: map.pgm
resolution: 0.050000
origin: [-15.400000, -12.200000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
其中包含的是关联地图数据文件、地图分辨率、起始位置、地图数据的阈值等配置
另外使用Kinect也可以实现SLAM建图，使用命令：
$ roslaunch mrobot_gazebo mrobot_kinect_nav_gazebo.launch
$ roslaunch mrobot_navigation gmapping_demo.launch