SLAM技术在智能驾驶领域的应用

01SLAM在智能驾驶领域的应用背景

随着人工智能和自动驾驶技术的进步，车辆搭载的辅助驾驶系统可以实现越来越强的功能，如自适应巡航、高级变道辅助及自动泊车等。为了在复杂环境下对这些高阶功能实现支持，需要对车辆自身的位置以及车辆周围的环境进行精确感知。同时，目前自动驾驶系统的研究逐渐从L2/L3向L4级别系统过渡，为了实现对算法的训练与测试，对位置与环境信息的精度也提出了新的要求。

传统的车载导航与感知系统依托于高精度地图与全球卫星定位系统（Global Navigation Satellite System，GNSS）对位置与环境信息进行采集，虽然目前典型的GNSS如北斗等已经可以实现米级以上精度，在车辆导航领域得到广泛应用，然而民用卫星信号的定位精度越来越难以满足高阶驾驶辅助系统（Advanced Driver Assistance System，ADAS）的需求。同时，在市区和室内环境下，卫星信号易受复杂电磁环境干扰，并且遮蔽物较多，这些都会严重影响定位精度，严重时可能造成ADAS系统误判，产生恶性事故。此外，在高精地图的应用上，也存在政策方面的限制与更新不及时的问题。

为了解决上述需求，原本用于解决自主机器人未知环境探索的同步建图与定位（Simultaneous Localization And Mapping，SLAM）技术开始被应用于开发新型车载数据采集系统，以实现对智能驾驶系统的定位、势态感知和导航。SLAM技术主要解决的定位与建图问题完全可以应对车载数据采集系统的场景提取问题与高精地图的更新问题，从而支持对高阶智能驾驶系统的开发、训练与测试，因此成为了最近智能驾驶领域研究的热点问题之一。

02SLAM原理概述

SLAM技术本质上是一种利用概率论对自身传感器获取的信息进行融合优化，以实现对自身位置及周围环境进行高精度估计的技术。因此一个SLAM系统首先必须搭载有能感知外界信息的传感器并且具备相应的优化算法，SLAM系统的整体结构如图1所示。

图1 SLAM系统结构

由图1可见，一个典型的SLAM系统由前端处理、后端优化、定位与建图算法以及回环检测算法四部分组成，接下来分别对这四部分进行简要介绍。

在前端处理环节，主要是通过各种算法将传感器读取的信息转化为与位姿和地图坐标系变化有关的信息，如IMU预积分算法将IMU的连续读数进行定积分，从而得到关于各时刻自主机器人位移与旋转的离散变化量，使得进一步优化的计算量大大降低。而激光雷达扫描得到的点云则可以通过线性插值算法结合IMU数据进行去畸变，并通过对环境中的特征点进行匹配得到移动体的位移与旋转变化量。

在得到了初步的变化量之后，在后端优化环节，可以通过建立残差相对约束（因子图优化）或误差协方差融合（卡尔曼滤波）的方法对前端中基于不同来源得到的变化量进行优化以实现估计误差最小化。

通常而言，基于卡尔曼滤波的后端优化方法具有计算速度较快、实时性较高的优点，但由于优化只涉及两个时刻的信息，因此全局精度欠佳。而基于图优化进行的后端优化则具有更良好的估计精度，然而由于传统的图优化算法是对全局进行优化，因此在计算负担上较高。

在实际使用中会根据系统的具体需求选择相应的算法，并且两种方法都在向改善自身缺陷的方向进行进一步发展，目前来看因子图优化的SLAM技术是未来的发展方向，因此本文主要以图优化SLAM为核心来进行介绍。

传统的建图与定位基本上是通过雷达的点云或双目摄像机建立的深度图像进行建图，并基于优化后的系统位姿进行定位的，不过在一些目前较先进的多传感器紧耦合算法会结合不同传感器的优势进行建图，以实现更好的效果，如LVI-SAM[1]使用激光雷达的点云特征同视觉传感器的特征点进行匹配，以赋予视觉传感器深度信息，实现单目摄像头立体建图。R3LIVE[2]则将视觉传感器的画面建立为体素信息赋予激光雷达的点云地图，以实现三维彩色高精度建图。

最后的回环检测环节则是一种用于校正传感器退化及漂移的手段，通过对已经过的路段再次进行匹配来实现对长距离运行时逐渐发生的漂移误差进行消除。

03几种典型的SLAM算法

1、LOAM

作为激光SLAM的开山之作，LOAM[3]算法设计了经典的激光雷达点云特征匹配算法。这种方法靠特征点与周围相邻点之间的弧度将特征点分为角点与面点，如式（1）所示：

公式（1）

其中，c为特征点弧度，S为当前点云集合，X为各点的位置信息。之后通过Kd-Tree与上一帧的点云进行匹配，并根据匹配后的误差建立如式（2）、（3）所示目标函数：

公式（2）

公式（3）

将系统中所有节点的目标函数相加并通过最小二乘法进行最小化，即可实现对系统位姿的估计。这种方法为之后基于激光的SLAM算法奠定了基础，并被大多数后续研究所采用。

2、LIO-SAM

在传统的基于图优化的SLAM算法中，为了降低系统的计算负担通常使用滑窗优化来减少需要优化的节点。然而，由于回环检测涉及到对过去不相邻的节点进行优化，如果使用传统的滑窗图优化方法会导致优化矩阵的大面积重构，大幅增加系统的计算负担。

图2 回环检测导致的因子图重构

LIO-SAM[4]的贡献在于引入了iSAM2优化算法，将因子图重构为增量贝叶斯树，使得在引入新的回环因子时不必对因子图整体进行重构，仅需要将与回环因子直接相连的节点删除并重构即可。这使得回环检测得以被引入快速因子图优化SLAM系统，大大提升了系统在长时间运行时的稳定性与精度。

3、LVI-SAM

在LIO-SAM的基础上，Shan等人将视觉传感器引入系统，提出了LVI-SAM。由于视觉传感器提供的地图纹理，这套系统相对于过去的LIO-SAM在建图方面存在巨大的提升。

通常环境下系统运行在视觉传感器为基础的算法上，并通过构造超球面的方法将雷达点云与视觉特征点进行匹配，实现对视觉纹理赋予深度信息。但视觉传感器出现较大退化时，则切换到雷达系统进行建图，使得这种方法在精度下限上不亚于过去的LIO-SAM算法。

本算法同样采用iSAM2进行后端优化，使得其可以很方便的引入回环检测机制而不增加额外的计算负担。

图3 深度点与视觉特征匹配

4、R2LIVE

针对图优化和卡尔曼滤波在SLAM领域的固有缺陷和优点，香港大学的团队另辟蹊径将两种方法结合在一起，提出了R2LIVE算法[5]。

这种方法采用计算速度较快的卡尔曼滤波对刷新频率较高的机器人本身位姿进行后端优化，实现系统位姿的实时更新。

同时利用精度较高的因子图优化方法对变化较小的地图特征点位置进行优化，从而实现高精度建图。最终两者结合实现了对移动机器人高精度实时定位的目的。

5、R3LIVE

作为R2LIVE的后续，香港大学的团队提出了R3LIVE。这套方法重点在于实时三维建图，因此舍弃了计算速度较慢的图优化方法，完全采用卡尔曼滤波进行后端优化。其主要贡献在于将视觉纹理转换为体素点，并将其颜色渲染赋予激光系统构建的三维点云地图，最终得到了具有彩色纹理的高精实时三维地图。根据其实验验证，最终建图精度高于LVI-SAM算法。

上文介绍的几种算法各有优劣，如LOAM算法作为ICP点云匹配的经典之作，至今仍然被视为激光雷达SLAM领域最优秀的算法之一，LIO-SAM创造性的将iSAM2融入优化算法以实现对回环检测的快速处理，港大的R-live系列算法则专注于对彩色3D建图的研究，具有极强的制图功能且用途多样，LVI-SAM则在LIO-SAM的基础上引入了另一套视觉系统，在定位精度上得到了提升且具备了彩色建图功能。

在文献[6]中对现有的一些经典算法在同一数据集下的性能进行了评估，评估显示在未经过特殊优化及适配的条件下，传统的LOAM算法反而具有最好的定位精度，这可能是由于传感器时间戳与算法本身的设置不匹配所导致的信息跳变造成的。然而，这同时也显示了传统的LOAM由于其较强的鲁棒性使其仍然具有实用价值。

图4 M2DGR数据集下各SLAM算法定位精度[6]

04SLAM技术在智能驾驶中应用的未来构想

目前已经存在诸多成熟的SLAM方案，涵盖了激光、视觉、惯性导航与地图匹配等各种传感器类型。然而这些SLAM算法主要用于解决未知环境下移动机器人自主探索所提出，虽然其针对的问题同智能驾驶领域所面临的定位与建图问题存在重合，但是仍然需要从智能驾驶的实际需求出发，在现有成熟SLAM算法的基础上进行改进。

例如，在传统SLAM算法中广泛采用回环检测技术用于消除传感器退化与漂移误差，然而在汽车驾驶领域，很少会存在回环行驶的状况，使得回环检测难以被触发。

相对的，通常情况下智能驾驶主要面对的难题在于车流与人流量较大，突然情况较多的闹市环境，这种环境通常具有完备的地图可以进行匹配，因此在自主机器人探索SLAM领域较少使用的地图匹配算法（如GR-SLAM[7]算法）反而可以在智能驾驶SLAM领域大放异彩。

此外，得益于汽车较大的承载力，其所搭载的计算系统通常要强于无人机与自主机器人所搭载的单片机或嵌入式系统，因此具备充足的计算资源以在前端环节融入深度学习算法，改善视觉系统的纹理分辨率或对激光系统的畸变进行补偿以替代目前常用的线性插值方法。