Chapter6：机器人SLAM与自主导航

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

1.机器人必备条件

1. 硬件要求

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

2. 深度信息

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

3. 里程计信息

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

4. 仿真环境

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

2.ROS SLAM功能包应用方法

1. $\mathrm{gmapping}$功能包概述

   • 基于激光雷达；
   • $\mathrm{Rao}-\mathrm{Blackwellized}$粒子滤波算法；
   • 二维栅格地图；
   • 需要机器人提供里程计信息；
   • $\mathrm{OpenSlam}$开源算法；
   • 输出地图话题：$nav\_msgs/OccupancyGrid$；

2. $\mathrm{gmapping}$功能包总体框架

   

3. 安装$\mathrm{gmapping}$功能包

   # 安装gmapping
   sudo apt-get install ros-melodic-gmapping
   

   

4. 栅格地图取值原理

   

   • 致命障碍：栅格值为$254$，障碍物与机器人中心重合，此时机器人必然与障碍物发生碰撞；
   • 内切障碍：栅格值为$253$，障碍物处于机器人轮廓的内切圆内，此时机器人也必然与障碍物发生碰撞；
   • 外切障碍：栅格值为$252～128$，障碍物处于机器人轮廓的外切圆内，此时机器人与障碍物临界接触，不一定发生碰撞；
   • 非自由空间：栅格值为$128～0$，障碍物附近区域，一旦机器人进入该区域，将有较大概率发生碰撞，属于危险警戒区，机器人应该尽量避免进入；
   • 自由区域：栅格值为$0$，此处没有障碍物，机器人可以自由通过；
   • 未知区域：栅格值为$255$，此处还没有探知是否有障碍物，机器人可以前往继续建图；

5. 配置$\mathrm{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>
   

6. 运行激光雷达建图例程

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

   建图效果：

   

   地图的$.pgm$文和$.yaml$信息：

   

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

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

   

8. 实际机器人建图实例

   # 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中的导航框架

1. $\mathrm{ROS}$的导航框架

   

2. $move\_base$

   

   • 全局路径规划$(\mathrm{global}\mathrm{planner})$：
     • 全局最优路径规划；
     • $\mathrm{Dijkstra}$或$A^{\ast }$算法；
   • 本地实时规划$(\mathrm{local}\mathrm{planner})$：
     • 规划机器人每个周期内的线速度、角速度，使之尽量符合全局最优路径；
     • 实时避障；
     • $\mathrm{Trajectory}\mathrm{Rollout}$和$\mathrm{Dynamic}\mathrm{Window}\mathrm{Approaches}$算法；
     • 搜索躲避和行进的多条路径，综合各评价标准选取最优路径；

3. $move\_base$功能包的话题和服务

   

4. 配置$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>
   

5. $\mathrm{amcl}$

   • 蒙特卡罗定位方法；
   • 二维环境定位；
   • 针对已有地图使用粒子滤波器跟踪一个机器人的姿态；

6. $\mathrm{amcl}$功能包的话题和服务

   

7. $\mathrm{amcl}$定位

   

   • 里程计定位：只通过里程计的数据来处理$/base$和$/odom$间的$\mathrm{TF}$变换；
   • $\mathrm{amcl}$定位：估算机器人在地图坐标系$/map$下的位姿信息，提供$/base、/odom、/map$间的$\mathrm{TF}$变换；

8. 配置$\mathrm{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. 导航仿真

   # 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"发布机器人目标点；

   

2. 导航$\mathrm{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