探索机器人SLAM：从建图到自主导航的完整指南

前言

这篇文章我开始和大家一起探讨机器人SLAM建图与自主导航 ，在前面的内容中，我们介绍了差速轮式机器人的概念及应用，谈到了使用Gazebo平台搭建仿真环境的教程，主要是利用gmapping slam算法，生成一张二维的仿真环境地图 。我们也会在这篇文章中继续介绍并使用这片二维的仿真环境地图，用于我们的演示。

教程

SLAM算法的引入

（1）SLAM：Simultaneous Localization and Mapping，中文是即时定位与地图构建，所谓的SLAM算法准确说是能实现SLAM功能的算法，而不是某一个具体算法。

（2）现在各种机器人研发和商用化非常火 ，所有的自主机器人都绕不开一个问题，即在陌生环境中，需要知道周边是啥样（建图），需要知道我在哪（定位），于是有了SLAM 课题的研究。SLAM在室内机器人，自动驾驶汽车建图，VR/AR穿戴等领域都有广泛的应用。

（3）SLAM算法根据依赖的传感器不同，可以分为激光SLAM和视觉SLAM，前者是激光雷达，后者是能提供深度信息的摄像头，如双目摄像头，红外摄像头等。除此之外，SLAM算法通常还依赖里程计提供距离信息，否则地图很难无缝的拼接起来，很容易跑飞。一个经典的SLAM 流程框架如下，其中回环检测时为了判断机器人有没有来过之前的位置。

整体视觉SLAM的流程图

gmapping算法的基本原理

（1）现在ROS里有一系列SLAM算法包，如：gmapping ，hector（不需要里程计，比较特别），谷歌开源的cartographer（效率高），rtabmap（前面是二维的，这是三维建图）等。

（2）gmapping是基于激光雷达的，需要里程计信息，创建二维格栅地图。其中IMU信息可以没有 。

（3）ros中激光雷达数据消息是 sensor_msgs/LaserScan ，内容如下：

（4）ros中里程计数据消息是 nav_msgs/Odometry 。

（5）gmapping 发布的地图meta数据：

（6）gmapping 发布的地图栅格数据

mbot_navigation

（1）ubuntu20.04 + ros noetic下，安装gmapping和保存地图文件的map_server

sudo apt-get install ros-noetic-gmapping
sudo apt-get install ros-noetic-map-server
// 补充：这是安装hector
sudo apt-get install ros-noetic-hector-slam

（2）创建 mbot_navigation 和相关文件

cd ~/catkin_ws/src
catkin_create_pkg mbot_navigation geometry_msgs move_base_msgs actionlib roscpp rospy

cd mbot_navigation 
mkdir launch maps rviz
touch launch/gmapping.launch

（3）调用gmapping算法，只需要写launch文件就行了，不用编码。
gmapping.launch

<launch>
	// mbot_gazebo 会通过发/scan topic，传出lidar数据
    <arg name="scan_topic" default="scan" />
    // gammping一大堆参数，这里都是从他的demo里扣出来的，不用改。
    // 如果想用的好，可以尝试修改，甚至改一些代码，这就是算法（调参）工程师！
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">
    	// mbot_gazebo 会通过发/odom topic，传出里程计数据
        <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>
	// 保存的rviz配置文件
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mbot_navigation)/rviz/map.rviz"/>
</launch>

（4）连同mbot_gazebo，一起编译运行

cd ~/catkin_ws
catkin_make -DCATKIN_WHITELIST_PACKAGES="mbot_navigation;mbot_gazebo"
source devel/setup.bash
// 打开仿真环境
roslaunch mbot_gazebo mbot_gazebo.launch
//再开一个窗口，打开gmapping
roslaunch mbot_navigation gmapping.launch
// 控制机器人行动，进行建图
roslaunch mbot_gazebo mbot_teletop.launch
// 建图完成后，新开窗口，执行map_server，保存生成的地图
cd ~/catkin_ws/src/mbot_navigation/maps
rosrun map_server map_saver -f gmapping_save

最终保存下来的地图

总结

在github上面的代码示例

 

访问地址：https://github.com/Jieshoudaxue/ros_senior/tree/main/mbot_navigation/config/move_base

代码示例：

#include <ros/ros.h>
#include <list>
#include <geometry_msgs/Pose.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

geometry_msgs::Pose createPose(double px, double py, double pz, double ox, double oy, double oz, double ow) {
    geometry_msgs::Pose pose;
    pose.position.x = px;
    pose.position.y = py;
    pose.position.z = pz;
    pose.orientation.x = ox;
    pose.orientation.y = oy;
    pose.orientation.z = oz;
    pose.orientation.w = ow;
    return pose;  
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "move_test");

  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_client("move_base", true);

  ROS_INFO("Waiting for move_base action server...");  
  move_base_client.waitForServer();
  ROS_INFO("connected to move base server");

  std::vector<geometry_msgs::Pose> target_list;
  target_list.push_back(createPose(6.543, 4.779, 0.000, 0.000, 0.000, 0.645, 0.764));
  target_list.push_back(createPose(5.543, -4.779, 0.000, 0.000, 0.000, 0.645, 0.764));
  target_list.push_back(createPose(-5.543, 4.779, 0.000, 0.000, 0.000, 0.645, 0.764));
  target_list.push_back(createPose(-5.543, -4.779, 0.000, 0.000, 0.000, 0.645, 0.764));

  for (uint8_t i = 0; i < target_list.size(); i ++) {
    ros::Time start_time = ros::Time::now();

    ROS_INFO("going to %u goal, position: (%f, %f)", i, target_list[i].position.x, target_list[i].position.y);

    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();

    goal.target_pose.pose = target_list[i];

    move_base_client.sendGoal(goal);

    move_base_client.waitForResult();

    if (move_base_client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
      ros::Duration running_time =  ros::Time::now() - start_time;
      ROS_INFO("go to %u goal succeeded, running time %f sec", i, running_time.toSec());
    } else {
      ROS_INFO("goal failed");
    }

  }



  return 0;
}