【光电智造】机器人SLAM建图与自主导航：从基础到实践

今日光电 2024-12-18 18:00

年终搞个特价示波器奖励员工？ 【有奖直播】深入学习Microchip时钟和全新单片机方案

今日光电

有人说，20世纪是电的世纪，21世纪是光的世纪；知光解电，再小的个体都可以被赋能。追光逐电，光赢未来...欢迎来到今日光电！

----追光逐电光赢未来----

前言

这篇文章我开始和大家一起探讨机器人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-gmappingsudo apt-get install ros-noetic-map-server// 补充：这是安装hectorsudo apt-get install ros-noetic-hector-slam

（2）创建 mbot_navigation 和相关文件

cd ~/catkin_ws/srccatkin_create_pkg mbot_navigation geometry_msgs move_base_msgs actionlib roscpp rospy
cd mbot_navigation mkdir launch maps rviztouch launch/gmapping.launch

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

gmapping.launch

  // mbot_gazebo 会通过发/scan topic，传出lidar数据    "scan_topic" default="scan" />    // gammping一大堆参数，这里都是从他的demo里扣出来的，不用改。    // 如果想用的好，可以尝试修改，甚至改一些代码，这就是算法（调参）工程师！    "gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">      // mbot_gazebo 会通过发/odom topic，传出里程计数据        "odom_frame" value="odom"/>        "map_update_interval" value="5.0"/>                "maxRange" value="5.0"/>        "maxUrange" value="4.5"/>        "sigma" value="0.05"/>        "kernelSize" value="1"/>        "lstep" value="0.05"/>        "astep" value="0.05"/>        "iterations" value="5"/>        "lsigma" value="0.075"/>        "ogain" value="3.0"/>        "lskip" value="0"/>        "srr" value="0.01"/>        "srt" value="0.02"/>        "str" value="0.01"/>        "stt" value="0.02"/>        "linearUpdate" value="0.5"/>        "angularUpdate" value="0.436"/>        "temporalUpdate" value="-1.0"/>        "resampleThreshold" value="0.5"/>        "particles" value="80"/>        "xmin" value="-1.0"/>        "ymin" value="-1.0"/>        "xmax" value="1.0"/>        "ymax" value="1.0"/>        "delta" value="0.05"/>        "llsamplerange" value="0.01"/>        "llsamplestep" value="0.01"/>        "lasamplerange" value="0.005"/>        "lasamplestep" value="0.005"/>        "scan" to="$(arg scan_topic)"/>      // 保存的rviz配置文件    "rviz" type="rviz" name="rviz" args="-d $(find mbot_navigation)/rviz/map.rviz"/>

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

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

最终保存下来的地图

总结

代码示例：

#include #include #include #include #include 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_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 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;}