随着计算机技术、传感器、人工智能、控制论等技术理论的不断进步与发展，无人驾驶相关领域的技术研究推进到了一个新时期。SLAM作为创建地图和实时导航的重要手段之一, 对其精度和鲁棒性也提出了更高的要求。

激光雷达传感器可以为周围环境提供较为可靠的三维坐标测量，在机器人系统中得到了广泛应用。一个三维激光雷达可以在360°水平视场(FOV)下，以10 Hz左右的频率感知周围环境, 而且对环境光和场景中的光学纹理不敏感。近年来，激光雷达技术也取得了巨大的进步, 体积和质量都大大降低, 这使其成为了SLAM技术的热门传感器。尽管激光雷达传感器具有诸多优点，但是在SLAM上的应用也面临着一些挑战。激光雷达虽然拥有360°的水平视角, 但是垂直视角较为稀疏, 一个典型的16线激光雷达垂直视角为±15°, 垂直角分辨率仅为2°。最终激光雷达获取的数据将极为稀疏, 如何有效地从中获取可靠的结构信息表示并进行配准是一大挑战。真实场景中还伴随着激光雷达退化、运动畸变等复杂情况。

在本文中, 提出一种带关键帧估计和可靠平面表示多线激光雷达(lidar)和惯性陀螺仪(IMU)紧耦合的定位方案来克服以上问题。利用较为稠密的局部地图获取更为可靠的平面表示, 利用新增帧对局部地图的贡献来选择关键帧。本文工作贡献如下：

1) 局部地图匹配规则和平面表示方法, 可靠的平面表示可以为多线激光雷达和惯性陀螺仪紧耦合的联合优化提供更为鲁棒的约束, 提升鲁棒性；

2) 局部地图更新方案, 在更新局部地图的过程中最大程度地保留先验信息, 使得边缘化残差更表示先验；

3) 关键帧筛选机制, 使得局部地图中不再保留着大量的冗余信息, 从而提高了效率；

4) 算法在室内和室外场景下测试都取得了一定的提升。

1 相关工作

SLAM是创建地图和实时导航的重要手段之一, 最近也取得了巨大的进步^[1-8]。由于传感器本身的特点与性质, 使得基于单一传感器的SLAM^{[1-2, 4, 9]}技术适用性和稳定性受限, 基于多传感器融合技术的SLAM技术^{[3, 5-7, 10-11]}, 已经引起了国外学者浓厚的研究兴趣。其大体上可分为基于视觉和基于激光雷达的方法。

1.1 基于视觉的SLAM

ORB-SLAM^[1]是一个经典且开源视觉SLAM算法, 使用ORB特征作为特征描述, 是一个包括里程计、后端优化、回环检测的完整SLAM框架。ORB-SLAM受限于单一的传感器, 对环境光和环境中的光学纹理较为敏感, VI-ORB^[12]是在ORB-SLAM框架上的改进, 融合了视觉传感器和IMU, 实现了IMU和视觉的紧耦合优化, 在精度和稳定性上取得了巨大突破。

VINS^[3]是香港科技大学秦通于2018年发表的开源视觉和IMU紧耦合优化算法, 他使用光流特征作为前端, 利用局部地图和滑动窗口等策略实现了更鲁棒的跟踪效果。

1.2 基于激光雷达的SLAM

LOAM^[4]是一个优秀的激光雷达定位算法, 其从稀疏点云中提取边缘线条和平面等结构信息并进行匹配，以此为约束构建里程计, 通过构建帧到地图的匹配，对里程计的优化结果, 最终取得了令人震惊的成绩。大量的算法已经证明了平面表示在点云地图中的作用，Geneva对平面的表示方法展开了研究, 在LIPS(lidar intertial 3D plane slam)^[7]提出使用CP(closest point)表示法，最终可以获得更好的性能。Ye在VINS和LOAM的基础上提出了LIO(lidar intertial odometry)^[8], 实现了多线激光雷达和IMU的紧耦合优化, 在稳定和精度上获得了巨大的提升, 同时也是第一个开源地实现了多线激光雷达和IMU的紧耦合优化SLAM算法。

受LOAM和LIO启发, 本文设计了一个带关键帧估计和可靠平面表示的多线激光雷达和IMU的紧耦合优化SLAM算法和LIO、LIPS等方案不同的是, 本文不再使用近邻匹配的少量的局部点拟合平面, 利用大量的相邻匹配将可能位于同一物理平面上的点聚集起来, 然后拟合一个统一且可靠的平面, 通过观察待加入局部地图的雷达帧对局部地图中结构信息(平面的数量)的影响显著程度, 进而判断是否为关键帧。

2 带关键帧和可靠平面表示的里程计

为了保证位姿估计的高效率, 很多方法将这个任务划分为2个并行的任务, 里程计和全局地图优化，但是本文采用和LIO类似的方案，构建了一个激光雷达和IMU紧耦合优化的里程计。主要包括以下3个部分：可靠平面表示、局部地图、联合优化。

2.1 里程计结构概述

图 1是带关键帧和可靠平面表示的里程计结构示意图。已知前一帧状态的情况下, 使用S_j表示当前雷达的原始输入, 使用I_{i, j}表示从上一个雷达帧时间戳i到当前雷达帧的时间戳j所经历的状态变化。里程计进行位姿估计经历的步骤如下:

1) 通过i的状态和累计在i帧和j帧间所有的IMU状态变化获取状态估计结果;

2) 通过IMU预计分将IMU测量过程的误差整合至联合优化中的优化变量Δp_ij, Δv_ij, Δq_ij中;

3) 利用i帧和j帧间的位姿变化来补偿原始雷达帧数据在采集过程中所遭遇的运动畸变;

4) 特征提取可以减少数据冗余提高算法效率。使用曲率对将雷达扫描点分类为平面点和边缘点^[4];

5) 将i帧和j帧的匹配关系作为约束, 将状态估计获得的初始位姿作为初值获得一个更优的位姿, 用于将j帧注册至局部地图中;

6) 局部地图主要包括3个部分:优化窗口、局部地图窗口以及可靠平面表示;

7) 在优化窗口和局部地图窗口之间建立匹配关系;

8) 联合预计分结果和匹配关系对位姿进行非线性优化, 优化结果将应用于步骤1), 防止状态估计发生漂移。

2.2 初始位姿计算

在大量的带IMU的紧耦合方案中都使用状态估计的位姿作为联合优化初值以及使用此初值获取与局部地图的匹配关系, 例如基于视觉和IMU紧耦合的VINS以及基于激光雷达和IMU紧耦合的LIO和LIPS。但是在激光雷达获取到的环境模型为3D点云形式, 相较于图像等形式而言拥有的信息更少。为了得到更好的匹配关系, 本文使用对状态估计位姿作为初值进行优化。

此阶段使用的特征包括边缘特征点和平面特征点, 在上一帧i和当前帧j之间进行匹配。设L^k为在k帧上的边缘特征点集合, P^k表示在k帧上的平面特征点集合。

L_mⁱ表示在i帧上的边缘特征点集合中的点m, 在L^j上的匹配点由2个点表示, L₁^j为L^j中和L_mⁱ最近的点, L₂^j表示在L^j中和L_mⁱ最近且和L₁^j异线的点。P_nⁱ表示在i帧上的平面特征点集合中的点n, 在P^j上的匹配点由3个点表示, P₁^j为P^j中和P_nⁱ最近的点, P₂^j和P₃^j为P^j中和P₁^j邻线且和P_n^j最近的2个点。

利用上述匹配规则可以建立特征匹配, 并利用非线性优化对投影误差关于位姿进行优化。边缘特征匹配的投影误差计算公式如下

(1)

平面特征匹配的投影误差计算公式如下

(2)

上述误差计算公式需要位于同一坐标系下，即需要将第i帧坐标系的Xⁱ点投影为第j帧坐标系下X^j。在i帧和j帧之间的位姿变换包括2个部分旋转R_i^j和平移T_i^j。将i帧投影至于j帧坐标系下投影公式如下

(3)

2.3 局部地图

构建局部地图的大多数方法都将局部地图划分为2个窗口, 分别为优化窗口和局部地图窗口。位于优化窗口的帧的位姿在每次优化过程中都将被更新; 位于局部地图窗口的帧的位姿将保持不变, 且将这些帧投影至局部地图坐标系下构造局部地图点云。在构建匹配关系的过程中利用和2.2节类似的方式获得优化窗口帧和局部地图点云的匹配关系。

局部地图点云是一个较为密集的点云，由局部地图窗口中的多帧图像点云投影至局部地图坐标系下构成, 而2.2节中的匹配方式仅能获取局部的不统一的平面表示, 最终影响优化结果。经过统计发生局部地图点云中的点被重复匹配次数超过3次的情况会达到本次局部地图优化所生成约束的二十分之一, 但是由于各组匹配相互独立, 同一个局部地图点可能会存在多个拟合平面(甚至达到10个以上)。本文用平面拟合的方式在局部地图点云中构建平面的可靠表示, 局部地图点云和优化窗口帧的匹配关系由局部地图窗口中管理的可靠平面与优化窗口帧进行匹配。

用M^L_p表示局部地图点云, M_i^L_p表示局部地图点云中的第i个点, P_{Lk, m}^L_p表示优化窗口中的第k帧的第m个特征点在局部地图坐标系L_p下的坐标。P_{Lk, m}^L_p在局部地图点云上的匹配点为M_a^L_p, M_b^L_p, M_c^L_p, 记为{P_{Lk, m}^L_p, (M_a^L_p, M_b^L_p, M_c^L_p)}, 通过匹配点得到的平面表示为P(M_a^L_p, M_b^L_p, M_c^L_p)。首先根据2.2中描述的匹配方法得到{P_{Lk, m}^L_p, (M_a^L_p, M_b^L_p, M_c^L_p)}和{P_{Lk, n}^L_p, (M_a^L_p, M_b^L_p, M_c^L_p)}。可以发现以上2个匹配中存在相同的局部地图点云点, 将2个关联的匹配进行合并得到统一且稳定的平面表达式P(M_a^L_p, M_b^L_p, M_c^L_p, M_d^L_p, M_e^L_p)。平面拟合公式的公式如下

(4)

平面表示则为最小特征向值所对应的右特征向量。(4)式SDV^T表示的SVD分解, 其中D表示特征值矩阵, S和V分别表示左右特征向量。所以最终获得的匹配为{P_{Lk, m}^L_p, }和{P_{Lk, n}^L_p, }。

2.4 局部地图更新

本文设计的局部地图包括3个主要部分：局部地图窗口，局部地图可靠平面表示以及优化窗口、可靠平面表示其实是对局部地图点云的一个抽象描述，它从局部地图点云中拟合而来；优化窗口中的帧和可靠平面进行配对建立匹配关系。

在进行局部地图更新的时候，需要将优化窗口中的第一帧点云添加至于局部地图窗口中，并从局部地图窗口中移除一帧。使用局部地图中可靠平面表示的数量来衡量剩余局部地图窗口中的帧是否能表示局部地图点云。首先尝试移除局部地图窗口的第一帧，如果可靠平面表示减少的数量小于原数量的五分之一，则移除局部地图窗口的第一帧。否则依次删除局部地图窗口的每一帧，找到并移除删除该帧剩余平面表示数量最多的帧。最后将优化窗口中的第一帧点云添加至于局部地图窗口中并更新局部地图点云。

2.5 联合优化

局部地图是消除累计误差的重要手段之一。将处于优化窗口的地图帧和局部地图进行联合优化, 从而达到消除累计误差的目的。本文的局部地图由局部地图窗口、局部地图可靠平面表示以及优化窗口3个部分组成, 这区别于LIO和VINS等方案的局部地图。首先从可靠平面中获取匹配关系, 如果获取失败则从局部地图点云中获取匹配关系。通过这种方案就可以消除和LIO等区别, 从而利用他的紧耦合优化完成位姿优化。

3 实验和分析 3.1 传感器配置和测试平台

测试平台为一个1.6 GHz四核8 GB内存的笔记本电脑上，运行在Linux下的机器人操作系统(ROS)上。测试数据集为公开数据集MVSEC, 包括大量的传感器数据, 激光雷达为16线Velodyne频率为20 Hz, IMU为ADIS16488频率为200 HZ, GPS型号为UBLOX NEO-M8N。

实验中参与对比的算法包括:LIO^[8]、LOAM^[4]、OUR、OUR_NoUnifyFeature。其中OUR表示包括本文提出的所有模块的算法, OUR_NoUnifyFeature表示只有关键帧筛选但是在优化过程中不包括可靠平面表示特征的算法。

实验结果表明本文算法在长期运动估计中获得了更为鲁棒的结果, 且可靠平面特征等策略明显提升了算法的表现。

3.2 定性结果

图 3和图 4分别展示了本文算法和其他优秀算法在公开数据集MVSEC Day1和Night1下的轨迹误差对比结果, 其中图 3a)和图 4a)为轨迹对比, 图 3b)和图 4b)为三轴轨迹误差对比结果, 它表示在xyz轴上随着时间累计产生的误差。

在图 3和图 4中本文方法都获得了最佳的结果, 与真实轨迹之间的误差很小。LOAM的轨迹偏差较大, 其主要原因为其帧间匹配作为约束条件, 在长期的运动估计中容易积累累计误差, 从而导致随着轨迹的增长, 误差也快速增加。相对于LOAM, LIO取得了更佳的结果, 主要原因为其使用了由连续多帧点云组成的局部地图来消除累计误差, 但是从第二幅图中可以发现其依旧容易积累累计误差, 主要原因为其局部地图由连续的多帧点云组成, 忽略了关键帧和普通帧的差异。本文提出的算法使用了包含了关键帧筛选, 局部地图优化, 归一化平面等策略大幅减少了累计误差, 最终取得了最佳的结果。

3.3 剪枝实验

为了证明本文策略的有效性, 进行了剪枝实验, 包括3个对比算法:

1) LIO即原始的LIO算法, 其局部地图由连续的多个点云帧组成局部地图；

2) OUR_NoUnifyFeature在LIO算法的基础上增加了关键帧筛选的策略, 但不包括归一化平面表示机制；

3) OUR是本文提出的算法的完整版本, 包括关键帧筛选, 归一化平面表示等策略。

图 5和图 6分别展示了本文算法和其他优秀算法在公开数据集MVSEC Day1和Night1下的轨迹误差对比结果。

对比LIO和OUR_NoUnifyFeature在图 5和图 6上的表现, 可以发现LIO更容易发送误差累计, 而OUR_NoUnifyFeature的误差累计明显优于LIO, 这说明本文提出的关键帧筛选策略有利于消减累计误差的产生。

同理, 对比OUR_NoUnifyFeature和OUR的表现, 可以得出结论归一化平面表示策略有效缓解了累计误差的产生。

4 结论

本文基于平面拟合算法, 提出一种局部地图改进和关键帧估计的方案, 解决目前激光定位方案中缺乏关键帧估计, 以及局部地图中平面多向性问题, 主要创新包括局部地图匹配规则和平面表示方法, 局部地图更新等。

给出了公开数据集MVSEC上进行的轨迹对比实验和剪枝实验，展示了本文算法相比于目前主流算法在处理公开数据集的误差，证明了本文提出的策略能有效降低算法运行过程中的误差累计。