分享 | 激光雷达SLAM建图定位与导航

2021-11-03 13:16 作者:机器谱 0人读过 | 我要投稿

即时定位与地图构建（Simultaneous Localization and Mapping，SLAM）技术可以很精确地实现环境的地图构建、定位以及多点导航。目前SLAM技术可以分为激光SLAM（Lidar SLAM）和视觉SLAM（Visual SLAM），激光SLAM采用的传感器为激光雷达，而视觉SLAM则采用深度摄像头。激光SLAM技术较为成熟、误差少，且足以满足当前环境的使用。

近年来，随着计算机视觉和人工智能等高新技术的逐渐成熟，其被越来越广泛地应用于人们生活中的每个角落。室内服务机器人将会是未来一个很大的发展方向，由于在室内环境中GPS无法精准定位，所以SLAM技术应运而生。SLAM（同步定位与地图构建），是指运动物体根据传感器的信息，一边计算自身位置、一边构建环境地图的过程，解决机器人等在未知环境下运动时的定位与地图构建问题。

目前，SLAM主要应用于机器人、无人机、无人驾驶、AR、VR等领域。其用途包括传感器自身的定位，以及后续的路径规划、运动性能、场景理解。当机器人有了SLAM的功能，就相当于机器人有了一双眼睛，其可以扫描未知环境从而获取地图，以及实时获取自己的当前位置；可以自行路径规划到达指定地图上的地点。如智能扫地机就可以通过SLAM实现精准的地面避障遍历，又自行回到充电地点。在无人驾驶以及无人机上亦同样适用，其需要实现一个共同的目的：避障、定位、路径规划。为了让机器人更加智能化，可在实现此项技术上再加上手机app的控制和语音识别控制。

SLAM系统一般分为五个模块，包括传感器数据、视觉里程计、后端、建图及回环检测。

传感器数据：主要用于采集实际环境中的各类型原始数据。包括激光扫描数据、视频图像数据、点云数据等。

视觉里程计：主要用于不同时刻间移动目标相对位置的估算。包括特征匹配、直接配准等算法的应用。

后端：主要用于优化视觉里程计带来的累计误差。包括滤波器、图优化等算法应用。

建图：用于三维地图构建。

回环检测：主要用于空间累积误差消除。

其工作流程大致为：传感器读取数据后，视觉里程计估计两个时刻的相对运动（Ego-motion），后端处理视觉里程计估计结果的累积误差，建图则根据前端与后端得到的运动轨迹来建立地图，回环检测考虑了同一场景不同时刻的图像，提供了空间上约束来消除累积误差。

激光SLAM是目前最稳定、最主流的定位导航方法。目前室内主要采用二维激光雷达，室外采用三维激光雷达。今天我们来分享一下2D激光雷达SLAM建图定位与导航。

该项目我们利用键盘控制福来轮底盘小车运动完成slam建图，并能在已建好的地图里进行自主导航。

slam导航可以拆分为三步：

第一步：能用键盘控制底盘的运动；

第二步：基于实际场景，用键盘控制底盘小车进行slam建图；

第三步：基于已建好的地图，模拟实现slam导航。

福来轮底盘小车

一、主要使用的器材

树莓派、雷达、树莓派相关的实验器材（显示屏、键盘鼠标、充电宝/手机充电器）

二、电路连接

根据slam导航的要求，下面将详细介绍各个电子部件与slam导航车的连接过程。

第一步：先将步进电机与主板的扩展板进行连接（提示：侧重看x、y、z的接线位置，装有雷达的一侧代表底盘小车的车头。雷达左侧的步进电机控制小车的X轴方向；雷达的右侧的步进电机控制小车的Y轴方向；雷达对侧的步进电机控制小车的Z轴方向，具体的接线引脚见下图）

第二步：将主控板的扩展板跟树莓派连接起来（见图中蓝色的线），并用锂电池进行供电。

第三步：将鼠标、键盘、显示屏与树莓派进行连接（见下图）。其中显示屏和树莓派用充电宝供电，鼠标、键盘这里用的是无线的。

第四步：将雷达与树莓派进行连接（建议把雷达连接在树莓派的usb3.0上，见下图）。

经过上边几步的电路连接后，就完成了相应的电路连接（见下图）。

一、软件环境介绍

在使用树莓派之前，我们需要给树莓派的SD卡里烧录系统镜像。而本次实验中的SD卡已经烧录好系统镜像，相关配置如下：

操作系统：Ubuntu18.04系统，基于Debian GNU/Linux，支持x86、amd64（即x64）、ARM和ppc架构。

仿真系统：基于开源机器人操作系统ROS melodic和开源软件平台Arduino开发，上位机采用ROS melodic，基于Rviz完成全向移动底盘小车slam导航运动规划，采用gazebo完成全向移动底盘小车物理运动仿真；下位机采用Arduino实现对全向移动底盘小车运动的控制。

二、程序编写

（一）键盘控制底盘小车运动

（1）实现思路

按下键盘上指定的键，实现底盘小车前进、后退、转向、平移；还可以灵活的设置底盘小车的角速度、线速度来调整底盘小车的运动。下面是规划的控制底盘小车的键盘命令（见下图），可以先熟悉一下，稍后在控制底盘小车时会用到。

（2）编写程序及调试

① 将以下Arduino程序下载到主机中【详细的程序源码可加QQ交流群：221964861进行获取】

② 启动雷达、键盘、rosserial程序包。打开终端,输入roslaunch robot_navigation_control robot.launch 命令（见下图），等待程序的运行启动界面。成功启动后，可以在终端看到底盘小车的当前speed（线速度）为0.04；turn（角速度）为0.08。

启动后的界面（见下图）:

③ 尝试按下键盘命令，控制底盘小车的运动。(注意：1、请先保证终端是激活状态；2、按键盘时，稍微有点间隔，这样给底盘小车转动的时间；3、一定要打开扩展版上的电源开关，否则会发现小车位置在rviz中位置变化，但车不运动）

（二）底盘小车同步定位与建图-SLAM

（1）实现思路

利用Gmapping算法对底盘小车所在的未知环境进行建立地图、同步定位、最后保存此地图。此地图可供底盘小车导航使用。

Gmapping算法是目前基于激光雷达和里程计方案里面比较可靠和成熟的一个算法，它基于粒子滤波，采用RBPF的方法效果稳定。本次底盘slam建图采用的是 gmapping_slam包。下面介绍Gmapping SLAM计算图。Gmapping的作用是根据激光雷达和里程计（Odometry）的信息，对环境地图进行构建，并且对自身状态进行估计。因此它得输入应当包括激光雷达和里程计的数据，而输出应当有自身位置和地图。下面我们从计算图（消息的流向）的角度来看看gmapping算法的实际运行中的结构：

位于中心的是我们运行的slam_gmapping节点，这个节点负责整个gmapping SLAM的工作。它的输入需要有两个：

输入

/tf 以及 /tf_static：坐标变换，类型为第一代的 tf/tfMessage 或第二代的 tf2_msgs/TFMessage 其中一定得提供的有两个tf，一个是base_frame 与 laser_frame 之间的tf，即机器人底盘和激光雷达之间的变换；一个是base_frame 与 odom_frame之间的tf，即底盘和里程计原点之间的坐标变换。odom_frame可以理解为里程计原点所在的坐标系。

/scan :激光雷达数据，类型为sensor_msgs/LaserScan

/scan很好理解，Gmapping SLAM所必须的激光雷达数据，而/tf 是一个比较容易忽视的细节。尽管/tf 这个Topic听起来很简单，但它维护了整个ROS三维世界里的转换关系，而 slam_gmapping要从中读取的数据是base_frame 与 laser_frame 之间的tf，只有这样才能够把周围障碍物变换到机器人坐标系下，更重要的是base_frame 与 odom_frame之间的tf，这个tf 反映了里程计（电机的光电码盘、视觉里程计、IMU）的监测数据，也就是机器人里程计测得走了多少距离，它会把这段变换发布到odom_frame 和 laser_frame 92之间。因此slam_gmapping 会从/tf 中获得机器人里程计的数据。

输出

/tf：主要是输出 map_frame 和 odom_frame 之间的变换

/slam_gmapping/entropy：std_msgs/Float64 类型，反映了机器人位姿估计的分散程度

/map：slam_gmapping 建立的地图

/map_metadata：地图的相关信息

输出的/tf 里又一个很重要的信息，就是map_frame和odom_frame之间的变换，这其实就是对机器人的定位。通过连通map_frame和odom_frame，这样map_frame与 base_frame 甚至与 laser_frame 都连通了。这样便实现了机器人在地图上的定位。

（2）编写程序及调试

① 先搭建一个实际场景，便于建立地图。【注意：为了方便识别，搭建的实际测试地图最好是不规则的区域。如下图测试的场地，在长方形区域的一边是不规则的，这样便于识别建立的仿真地图】

② 按照上述步骤，下载控制底盘小车的Arduino程序；启动雷达、键盘、rosserial；打开终端并输入命令：roslaunch robot_navigation_control robot_car.launch

③ 启动构建地图服务。按（ctrl+shift+T）打开第二个终端并输入：roslaunch four_macnum_slam four_macnum_slam.launch（见下图）。

界面启动后，在rviz中选择“map”，可以在可视化界面看到有地图出现。下面根据实际环境进行建图：按下键盘相关指令（键盘指令请参考上述表格）来控制底盘小车在场地内运行，直至在Rviz内可以看到构建地图的轮廓，如下图所示（注意：地图环境会根据实际场景建立）

为了后续在已知环境中运行底盘小车，需把此次建立的地图进行保存。需要先进入保存地图的文件夹，然后给地图命名，保存即可。例如我们把此次建立的地图命名为your_map_name。【详细的操作步骤可加QQ交流群：221964861进行获取】

（三）底盘小车导航—Navigation

（1）实现思路

在上述搭建好的实际场景中，实现底盘小车的导航效果。

具体思路：加载上节建好的地图，设置底盘小车的初始位置和方向，设定目标位置及方向，底盘小车就可以实现导航效果。

Navigation与底盘关系：ROS的二维导航功能包，简单来说，就是根据输入的里程计等传感器的信息流和机器人的全局位置，通过导航算法，计算得出安全可靠的机器人速度控制指令。但是如何在特定的机器人上实现导航功能包的功能，却是一件较为复杂的工程，作为导航功能包使用的必要先决条件，机器人必须运行ROS，发布tf变换树，并发布使用ROS消息类型的传感器数据。同时为了让机器人更好的完成导航任务，开发者还要根据机器人的外形尺寸和性能，配置导航功能包的一些参数。

在ROS中进行导航需要使用到的三个包是：

① move_base：根据参照的消息进行路径规划，使移动机器人到达指定的位置；

② gmapping：根据激光数据（或者深度数据模拟的激光数据）建立地图；

③ amcl：根据已经有的地图进行定位。