SLAM技术框架及自动驾驶定位方案

热文推荐：最流行的GitHub机器学习项目

“本文介绍了VSLAM的典型技术框架。将SLAM算法拆解后，用到的技术多是传统的计算机视觉算法，尤其是多视角几何相关知识。与当前大热的深度学习“黑箱模型”不同，SLAM的各个环节基本都是白箱，能够解释得非常清楚。但SLAM算法并不是上述各种算法的简单叠加，而是一个需要相互折中、密切配合的复杂系统工程。”

本文来源：计算机视觉life  作者：sixgod

什么是SLAM?

SLAM（Simultaneous Localization and Mapping），中文译作同时定位与地图创建。

外行乍一听起来非常拗口，为了不在一开始就吓跑读者，我们先不对其进行专业的解释，用一个形象的例子来进行说明。我们知道现在有不少家用的扫地机器人（如下图所示），可以代替人对室内进行自动清扫。早期的扫地机器人并不智能，它只是具有简单的避障功能，在室内随机游走，遇到障碍物就转弯，这样会导致有很多地方会漏掉，扫地效率非常低。

扫地机器人

要想真正实现智能的清扫，扫地机器人至少需要知道以下几件事情：

1、我在哪里？也就是扫地机器人在工作过程中要知道自己在房间的具体位置。对应的术语叫：定位（Localization)。

2、我周围的环境是什么样子？也就是扫地机器人需要知道整个房间的地面结构信息。对应的术语叫：建图(Mapping)。

3、我怎样到达指定地点（充电器）？当扫地机器人电量不足时，如何以最短的路径到达充电器所在位置进行自动充电。对应的术语叫：路径规划（Route Planning）。

有了以上的几个能力，扫地机器人就立马变的智能了，不再像无头苍蝇一样在室内乱跑，而是可以从任意位置出发，一边清扫一边对周围的环境（房屋结构、障碍物）建立地图，同时根据地图定位自己当前在地图中的位置，并实时调整规划路线。随着使用时间的增长，扫地机器人建立的地图会越来越准确，规划的清扫路线越来越高效，变的越来越智能。这也不难理解断点续扫（一次扫不完，回到充电地点重合点，回到原位置继续清扫）的功能是如何实现的了。

扫地机器人断点续扫示意图

看明白了上面的例子，我们给出SLAM的定义。SLAM是指当某种移动设备（如机器人）从一个未知环境里的未知地点出发，在运动过程中通过传感器观测定位自身位置、姿态、运动轨迹，再根据自身位置进行增量式的地图构建，从而达到同时定位和地图构建的目的。定位和建图是两个相辅相成的过程，地图可以提供更好的定位，而定位也可以进一步扩建地图。SLAM非常强调未知环境，是自主移动机器人的核心技术。

SLAM 传感器

很多人注意到，同一个行业的SLAM应用，有人用相对传统的激光雷达作为传感器，有人用最近几年兴起的视觉摄像头作为传感器，当然，还有人将它们结合起来使用，它们的区别在哪里呢？事实上，SLAM的实现方式与难度和传感器的形式与安装方式密切相关。用于SLAM的传感器主要分为激光雷达和视觉两大类。

1

激光雷达

激光雷达是非常传统的SLAM传感器。它可以提供机器人本身与周围环境障碍物间的距离信息。常见的激光雷达有SICK、Velodyne、Rplidar等。

激光雷达

主流的2D激光传感器扫描一个平面内的障碍物，适用于平面运动的机器人（如扫地机等）进行定位，并建立2D的栅格地图。这种地图在机器人导航中很实用，因为多数机器人还不能在空中飞行或走上台阶，仍限于地面。在SLAM研究史上，早期SLAM研究几乎全使用激光传感器进行建图，且多数使用滤波器方法，例如卡尔曼滤波器与粒子滤波器等。

激光雷达的技术方案有如下几个特点：

1、精度很高，也比较稳定。

2、速度快，计算量也不大，容易做成实时SLAM。

3、理论研究成熟。激光雷达用于SLAM的技术方案（EKF-SLAM）因为研究较早，现在已经非常成熟。当然人们也对EKF-SLAM的缺点也有较清楚的认识，例如不易表示回环、线性化误差严重、必须维护路标点的协方差矩阵等。

4、笨重。从上图就可以看出，激光雷达比普通的摄像头在空间体积上大了不少。

5、价格昂贵。激光雷达成本很高，因此激光的研究主要集中于如何降低传感器的成本上，现在激光雷达也有平价的产品了，现在高级的扫地机器人中就已经在使用激光雷达了，但性能配置会低一些。

2

视觉传感器

视觉SLAM是最近几年SLAM研究热点之一。它有如下几个特点：

1、价格便宜。

2、体积小，重量轻。想想手机里动辄几千万像素的摄像头也就占指甲盖那么小的面积。

3、可以获取丰富且直观的信息。有研究表明，视觉信息占了人类获取信息的80%以上，所以通过摄像头可以获取的信息要远远超过通过激光雷达获得的信息。

4、计算量大。往往信息量和计算量是成正比的，视觉SLAM中的特征点检测、匹配等过程非常耗时。不过随着CPU、GPU处理器速度的飞速发展，很多以前被认为无法实时化的视觉算法，得以近乎实时化地运行。

5、需要一定的假设条件。比如视觉算法无法在无纹理区域进行。

视觉SLAM传感器主要分为三大类：单目、双目（或多目）、RGBD。此外还有鱼眼、全景等特殊相机，由于在研究和产品中都属于少数在此不做介绍。

就实现难度而言，这三类方法难易程度从难到易依次为：单目视觉、双目视觉、RGBD。下面来具体介绍。

单目相机

单目相机

单目相机SLAM简称MonoSLAM，即只用一个摄像头实现SLAM。优势是传感器特别的简单、成本特别的低，所以单目SLAM非常受研究者关注。相比别的视觉传感器，单目有个最大的问题，就是没法确切地得到深度。单目相机有如下几个特点：

1、单目SLAM只能估计一个相对深度。由于绝对深度未知，单目SLAM没法得到机器人运动轨迹以及地图的真实大小。直观地说，如果把轨迹和房间同时放大两倍，单目看到的像是一样的。

2、单目相机必须通过运动才能获取深度。它无法仅根据一张图像获得图像中物体离自己的相对距离。为了估计这个相对深度，单目SLAM要靠运动中的三角测量，来求解相机运动并估计像素的空间位置。也就是说，它的轨迹和地图，只有在相机运动之后才能收敛，如果相机不进行运动时，就无法得知像素的位置。

3、相机必须进行旋转和平移。相机运动还不能是纯粹的旋转，这就给单目SLAM的应用带来了一些麻烦，好在日常使用SLAM时，相机都会发生旋转和平移。

4、既可以用于室内，又可以用于室外。

双目相机

双目相机

与单目不同的是，双目立体视觉既可以在运动时估计深度，也可在静止时估计，消除了单目视觉的许多麻烦。不过，双目或多目相机配置与标定均较为复杂，其深度量程也受双目的基线与分辨率限制。此外，通过双目图像计算深度计算量也非常大。

RGBD相机

RGBD相机是一种可以获得彩色图并测量深度的相机。目前常用的RGBD相机包括Kinect一代、Kinect 二代、Xtion、Realsense等。RGBD相机有如下特点：

1、一般通过结构光或Time-of-Flight原理，直接测出物体离摄像头的距离。相对于双目立体视觉，它的速度非常快，可以用于实时应用。

2、它比单目或双目相机能够提供更丰富的信息。

3、现在多数RGBD相机还存在视场角小、分辨率低等诸多问题。主要用于室内SLAM。

SLAM建图

我们知道SLAM可以一边建图一边定位，还可以用建立好的地图进行导航。

有同学问现在手机上的百度/高德/腾讯等地图类App不就可以做到吗，为什么还需要SLAM？这里解释一下：目前地图类App在室外定位、导航方面确实做的很不错，而且衍生出很多基于地理位置的游戏、社交、生活类应用。这是因为地图类App背后使用的是GPS技术，据说美国军用GPS精度可以达到厘米级定位精度，而开放给大众使用的民用GPS也可以达到米级的定位精度。但是，GPS只能在室外使用！而要想解决建筑物内、洞穴、海底等在GPS失效地域的定位、建图、姿态估计、路径规划，目前最有效的就是SLAM技术。下面这个视频描述的是美国宾夕法尼亚大学的Vijay Kumar教授的团队在几年前的工作，展示了搭载SLAM技术的无人机是如何快速的对复杂建筑物内部建图的。

https://v.qq.com/txp/iframe/player.html?vid=p0510vrf9pz&width=500&height=375&auto=0
搭载SLAM技术的无人机室内建图

最早的SLAM雏形是在军事（核潜艇的海底定位）上的应用，主要传感器是军用雷达。SLAM技术发展到如今已经几十年，目前以激光雷达作为主传感器的SLAM技术比较稳定、可靠，仍然是主流的技术方案。但随着最近几年计算机视觉技术的快速发展，SLAM技术越来越多的应用于家用机器人、无人机、AR设备，基于视觉的Visual SLAM（简称VSLAM）逐渐开始崭露头角。

VSLAM技术架构

本文主要介绍目前非常热门的VSLAM的技术框架，未来会有非常好的前景。VSLAM的技术框架如下，主要包括传感器数据预处理、前端、后端、回环检测、建图。

1

传感器数据

传感器数据预处理。这里的传感器包括摄像头、惯性测量单元（Inertial measurement unit，简称IMU）等，涉及传感器选型、标定、多传感器数据同步等技术。

不同类型视觉传感器的对比

2

前端

又称为视觉里程计（visual odometry，简称VO）。主要是研究如何根据相邻帧图像定量估算帧间相机的运动。通过把相邻帧的运动轨迹串起来，就构成了相机载体（如机器人）的运动轨迹，解决了定位的问题。然后根据估算的每个时刻相机的位置，计算出各像素的空间点的位置，就得到了地图。

VSLAM中，前端主要涉及计算机视觉相关的算法。典型做法一般是：首先提取每帧图像特征点，对相邻帧进行特征点粗匹配，然后利用RANSAC（随机抽样一致）算法去除不合理的匹配对，然后得到位置和姿态信息。整个过程涉及到特征提取、特征匹配、对极几何、PnP、刚体运动、李代数等多视图几何知识。

相邻图像特征点匹配

前面说视觉里程计只计算相邻帧的运动，进行局部估计，这会不可避免的出现累积漂移，这是因为每次估计两个图像间的运动时都有一定的误差，经过相邻帧多次传递，前面的误差会逐渐累积，轨迹漂移（drift）的越来越厉害。

轨迹漂移现象

解决轨迹漂移的方法有两个：后端优化、回环检测。

3

后端

主要是对前端的结果进行优化，得到最优的位姿估计。主要有两种方法：

• 一种是基于滤波理论的优化，主要有 EKF, PF, RBPF, UKF等方法，其中EKF（扩展卡尔曼滤波）在早期是主流的方法。它的思路是将状态估计模型线性化，并用高斯分布近似其噪声，然后按照卡尔曼滤波进行预测来更新。但是实际上，这种对噪声的高斯分布大部分情况下是不成立的，此外，线性化过程中丢失了高阶项。

• 另一种就是非线性优化（图优化）。它的基本思想是将优化的变量作为图的节点，误差项作为图的边，在给定初值后，就可以迭代优化更新。由于图优化的稀疏性，可以在保证精度的同时，降低计算量。

后端优化涉及到的数学知识比较多，具有较高的难度。总的来说，从状态估计的角度来讲，SLAM是一个非线性非高斯系统。因此传统的滤波理论已经逐渐被抛弃，而图优化已经成为主流方法。

4

回环检测

主要目的是让机器人能够认识自己曾经去过的地方，从而解决位置随时间漂移的问题。视觉回环检测一般通过判断图像之间的相似性完成，这和我们人类用眼睛来判断两个相同的地点是一样的道理。因为图像信息丰富，因此VSLAM在回环检测中具有很大的优势。

回环检测效果

当回环检测成功后，就会建立现在的图像和过去曾经见过图像的对应关系，后端优化算法可以根据这些信息来重新调整轨迹和地图，从而最大限度地消除累积误差。

5

建立地图

SLAM根据不同的传感器类型和应用需求建立不同的地图。常见的有2D栅格地图、2D拓扑地图、3D点云地图等。

比如前面提到过的扫地机器人，它只需要知道房屋内部的简单二维地图就可以了，不需要知道房屋到底有多高；它只需要知道哪里可以通过，哪里是障碍物，而不需要知道这个障碍物到底是什么，长什么样子；因此目前大部分具有SLAM功能的扫地机器人几乎都是采用廉价的消费级激光雷达方案，很少采用视觉SLAM方案（VSLAM也不够稳定）。

扫地机器人建立的2D地图

2D拓扑地图更强调地图元素之间的连通关系，而对精确的位置要求不高，去掉了大量地图的细节，是一种非常紧凑的地图表达方式。如下所示：

2D拓扑地图

3D点云地图在VSLAM中用的比较多，主要用于真实场景的视觉重建，重建的地图非常直观漂亮。但是点云地图通常规模很大，比如一张VGA分辨率（640 x 480）的点云图像，就会产生30万个空间点，这会占据非常大的存储空间，而且存在很多冗余信息。

3D点云地图

注：本文使用了清华大学高翔博士的部分公开资料。

自动驾驶领域定位方案

上面我们描述了用于定位的主流传感器，可以看到单一传感器在解决定位问题上都有自己的优缺点。在实际应用中，需要结合多个传感器联合解决定位问题。 以下针对几个典型场景，描述一下多传感器融合的情况：

1. 自动驾驶 GPS+IMU+里程计

GPS 给出的全局锚定，可以消除累计误差问题，不过它的更新频率低，并且信号容易被遮挡。 IMU和轮盘里程计更新频率高，不过有累计误差问题， 最容易想到的是收到GPS定位，使用GPS位置信息，误差就是GPS的精度，在下一次收到GPS定位间隔中，使用IMU（角度累加）和里程计（位移累加）进行位姿累加，中间的位姿误差是初值GPS定位误差和中间累加误差的积累。 

改进的方法是使用非线性卡尔曼滤波，在收到GPS位置信息的时候，要结合IMU和里程计的积累预测值和GPS观测值，算出一个误差收敛的更优的位置估算值。

2. 自动驾驶 GPS+ 多线雷达+高精地图匹配

GPS 给出全局锚定，中间使用雷达SLAM 前端里程计做累加，可以配合高精地图的图匹配，做类似后端回环优化的方式，将GPS、激光雷达及已知地图进行融合定位。

3. 自动驾驶多对双目视觉摄像头SLAM方案

这种方案成本低，更加考究的是算法，有很少的自动驾驶公司宣称自己主攻纯视觉方案，现在不是主流。（该部分摘自：中兴开发者社区）

推荐阅读

▼

▎本文转自计算机视觉life＆中兴开发者社区，作者：sixgod，如需转载请联系原作者。

更

多

精

彩

请猛戳右边二维码

公众号ID

IV_Technology