Chapter6：机器人SLAM与自主导航

${\rm ROS1}$ 的基础及应用，基于古月的课，各位可以去看，基于 ${\rm hawkbot}$ 机器人进行实际操作。
${\rm ROS}$ 版本： ${\rm ROS1}$ 的 ${\rm Melodic}$ ；实际机器人： ${\rm Hawkbot}$ ；

1.机器人必备条件

硬件要求
- 差分轮式机器人，使用 ${\rm Twist}$ 速度指令控制
  - ${\rm linear}$ ： ${\rm XYZ}$ 方向上的线速度，单位： ${\rm m/s}$ ；
  - ${\rm angular}$ ： ${\rm XYZ}$ 方向上的角速度，单位： ${\rm rad/s}$ ；
- 机器人必须安装激光雷达等测距设备，可以获取环境深度信息；
- 最好使用正方形和圆形的机器人，其他外形的机器人效果可能不佳；

深度信息

rosmsg show sensor_msgs/LaserScan

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

里程计信息
- ${\rm pose}$ ：机器人当前位置坐标，包括机器人的 ${\rm XYZ}$ 三轴位置与方向参数，及用于校正误差的协方差矩阵；
- ${\rm twist}$ ：机器人当前的运动状态，包括 ${\rm XYZ}$ 三轴的线速度与角速度，及用于校正误差的协方差矩阵；

仿真环境

# 创建仿真环境
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch

2.ROS SLAM功能包应用方法

${\rm gmapping}$ 功能包概述
- 基于激光雷达；
- ${\rm Rao-Blackwellized}$ 粒子滤波算法；
- 二维栅格地图；
- 需要机器人提供里程计信息；
- ${\rm OpenSlam}$ 开源算法；
- 输出地图话题： ${\rm nav\_msgs/OccupancyGrid}$ ；
${\rm gmapping}$ 功能包总体框架
安装 ${\rm gmapping}$ 功能包
```
# 安装gmapping
sudo apt-get install ros-melodic-gmapping
```
栅格地图取值原理
- 致命障碍：栅格值为 $254$ ，障碍物与机器人中心重合，此时机器人必然与障碍物发生碰撞；
- 内切障碍：栅格值为 $253$ ，障碍物处于机器人轮廓的内切圆内，此时机器人也必然与障碍物发生碰撞；
- 外切障碍：栅格值为 $252 ～ 128$ ，障碍物处于机器人轮廓的外切圆内，此时机器人与障碍物临界接触，不一定发生碰撞；
- 非自由空间：栅格值为 $128 ～ 0$ ，障碍物附近区域，一旦机器人进入该区域，将有较大概率发生碰撞，属于危险警戒区，机器人应该尽量避免进入；
- 自由区域：栅格值为 $0$ ，此处没有障碍物，机器人可以自由通过；
- 未知区域：栅格值为 $255$ ，此处还没有探知是否有障碍物，机器人可以前往继续建图；

配置 ${\rm gmapping}$ 功能包

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

运行激光雷达建图例程

# 1.启动gmapping演示
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch
roslaunch mbot_navigaztion gmapping_demo.launch
roslaunch mbot_teleop mbot_teleop.launch

# 2.切换到地图保存目录，保存地图
roscd mbot_navigation/maps/

# rosrun map_server map_saver -f 地图名
rosrun map_server map_saver -f simulink_maps

# 注：
# 建图效果好坏和很多因素有关，如硬件、算法本身、可调参数等；
# 硬件条件不好，建图过程中可能发生移位，导致建图失败；

建图效果：

地图的 ${\rm .pgm}$ 文和 ${\rm .yaml}$ 信息：

运行 ${\rm kinect}$ 建图例程

# 1.启动建图演示
roslaunch mbot_gazebo mbot_kinect_nav_gazebo.launch
roslaunch mbot_navigation gmapping_demo.launch
roslaunch mbot_teleop mbot_teleop.launch

# 2.若运行kinect抛出如下错误
cannot launch node of type [depthimage_to_laserscan/depthimage_to_laserscan]:depthimage_to_laserscan

# 解决方案
sudo apt-get install ros-melodic-depthimage-to-laserscan

# 注：
# 使用摄像头建模，效果好坏和硬件配置很大关系；
# 如下图所示，使用虚拟机建图十分卡顿，很容易发生移位；
# 如果笔记本的配置不是很好，建议了解建图过程即可；
# 如果笔记本配置不是很好，建图过程会直接卡死，想退出也难；

没有完全建好的图效果(发生了移位)：

实际机器人建图实例

# Hawkbot机器人的实际建图过程
# 1.时间同步
# 远程登录移动机器人端，并进行时间同步
ssh ...
sudo ntpdate 虚拟机IP

# 2.启动建图节点
roslaunch hawbot bringup.launch(机器人端)
roslaunch hawkbot teleop_key.launch(机器人或虚拟机端)
roslaunch hawkbot gmapping_slam.launch(虚拟机端)

# 3.保存地图
roscd hawkbot/maps/ 
rosrun map_server map_saver -f 地图名称

3.ROS中的导航框架

${\rm ROS}$ 的导航框架
${\rm move\_base}$
- 全局路径规划 $({\rm global\ planner})$ ：
  - 全局最优路径规划；
  - ${\rm Dijkstra}$ 或 $A^*$ 算法；
- 本地实时规划 $({\rm local\ planner})$ ：
  - 规划机器人每个周期内的线速度、角速度，使之尽量符合全局最优路径；
  - 实时避障；
  - ${\rm Trajectory\ Rollout}$ 和 ${\rm Dynamic\ Window\ Approaches}$ 算法；
  - 搜索躲避和行进的多条路径，综合各评价标准选取最优路径；
${\rm move\_base}$ 功能包的话题和服务

配置 ${\rm move\_base}$ 节点

# move_base.launch文件内容
<launch>

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

${\rm amcl}$
- 蒙特卡罗定位方法；
- 二维环境定位；
- 针对已有地图使用粒子滤波器跟踪一个机器人的姿态；
${\rm amcl}$ 功能包的话题和服务
${\rm amcl}$ 定位
- 里程计定位：只通过里程计的数据来处理 ${\rm /base}$ 和 ${\rm /odom}$ 间的 ${\rm TF}$ 变换；
- ${\rm amcl}$ 定位：估算机器人在地图坐标系 ${\rm /map}$ 下的位姿信息，提供 ${\rm /base、/odom、/map}$ 间的 ${\rm TF}$ 变换；

配置 ${\rm amcl}$ 节点

# amcl.launch文件内容
<launch>
    <arg name="use_map_topic" default="false"/>
    <arg name="scan_topic" default="scan"/>

    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
        <param name="use_map_topic" value="$(arg use_map_topic)"/>
        <!-- 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="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="60"/>
        <param name="laser_max_range" value="12.0"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="2000"/>
        <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.2"/>
        <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_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.25"/>
        <param name="update_min_a" value="0.2"/>
        <param name="odom_frame_id" value="odom"/>
        <param name="resample_interval" value="1"/>
        <!-- Increase tolerance because the computer can get quite busy -->
        <param name="transform_tolerance" value="1.0"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
        <remap from="scan" to="$(arg scan_topic)"/>
    </node>
</launch>

4.ROS机器人自主导航

导航仿真

# 1.启动导航仿真包
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch
roslaunch mbot_navigation nav_cloister_demo.launch

# 2.导航步骤
# 2.1 若机器人不在建图坐标系原点，先点击"2D Pose Estimate"，将地图和雷达数据对上；
# 2.2 匹配好机器人初始位置后，点击"2D Nav Goal"发布机器人目标点；

导航 ${\rm SLAM}$ 仿真

# 1.启动相应.launch文件
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch
roslaunch mbot_navigation exploring_slam_demo.launch

# 2.通过点击"2D Nav Goal"发布机器人目标点进行建图；

# 自主探索SLAM仿真
# 启动相应的.launch文件
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch
roslaunch mbot_navigation exploring_slam_demo.launch
rosrun mbot_navigation exploring_slam.py