LiDAR-Inertial SLAM with Efficiently Extracted Planes-CSDN博客

本文链接：https://blog.csdn.net/hrq2001421/article/details/139440445

摘要

本文提出了一种具有有效提取平面的LiDAR惯性SLAM，其在里程计中耦合显式平面以提高精度，并在映射中耦合显式平面以提高一致性。该方法包括三个部分：一个有效的点→线→面提取算法，LiDAR惯性-平面紧耦合测距法，和一个全球平面辅助映射。具体来说，我们利用激光雷达点云的环形场来加速基于区域生长的平面提取算法。然后在滑动窗口中将IMU预积分因子、LiDAR测距因子和显式平面因子紧密耦合，以获得更精确的初始位姿进行映射。最后，我们在全局地图中保持显式平面，并通过优化优化里程因子和平面观测因子的因子图来增强系统的一致性。实验结果表明，我们的平面提取方法是有效的，所提出的平面辅助激光雷达惯性SLAM显着提高了准确性和一致性相比，其他国家的最先进的算法，只有一个小的时间消耗增加。

一.介绍

随着实时三维重建和自定位的能力，基于激光雷达的同步定位和地图（SLAM）已被广泛应用于许多室内机器人应用，如自主导航，控制和运动规划。然而，由于狭窄的室内空间和重复的结构，传统的基于LiDAR的SLAM遭受显着的累积误差。另一方面，平面普遍存在于室内环境中，如何快速提取并合理应用于SLAM中，近年来受到了广泛的关注。

一些工作在扫描配准中利用局部拟合平面，例如基于表面的方法[1，2]和局部平面特征方法[3，4]，而忽略环境中明确存在的平面。其他作品介绍了显式平面，如BALM [5]和π-LSAM [6]。然而，平面提取算法是计算密集型的，这限制了它们的实时应用。

为了解决这个问题，我们提出了一个有效的激光雷达环的平面提取算法和紧密耦合的显式平面的测距和映射的激光雷达惯性SLAM（LI-SLAM）。本文的主要贡献如下：

提出了一种基于环的点→线→面提取算法，该算法能够有效、鲁棒地提取点云中的显式平面。
我们开发了一个LI-SLAM系统，在里程计和映射方面都具有紧密耦合的显式平面，这提高了系统的准确性和一致性。
所提出的方法在公共数据集（即，VECtor [7]）和自我收集的数据集。结果表明，我们的系统实现了更好的性能与其他国家的最先进的激光雷达惯性SLAM算法相比，在时间成本增加很少。

二.相关工作

A.激光雷达-惯性SLAM

激光雷达-惯性SLAM方法大致分为两类：松耦合方法和紧耦合方法。松耦合方法依赖于先前的预测，使用IMU数据来消除点云的失真，并为点云配准提供初始位姿。例如，像LOAM [3]这样的早期方法和像LeGO-LOAM [8]这样的最新改进都是松散耦合的系统。然而，由于缺乏使用来自两个传感器的原始数据，松耦合系统的精度较低。

相比之下，紧耦合系统联合处理LiDAR和IMU数据，考虑到它们的内部关系，通常可以提供更好的精度和鲁棒性。LIOmapping [9]采用优化框架[10]来最小化IMU和LiDAR测量的残差。LIO-SAM [4]通过引入LiDAR关键帧并使用因子图来联合优化IMU和LiDAR来限制计算复杂度。它是目前最流行的开源LI-SLAM框架之一。FAST-LIO [11]使用误差状态迭代卡尔曼滤波器将IMU和LiDAR测量紧密耦合。FAST-LIO 2 [12]提出了一种增量kd树来更有效地管理地图。

B.基于激光雷达的平面SLAM

在基于LiDAR的SLAM算法中，大多数算法使用由边缘或平面特征组成的点云图，例如LOAM [3]，林斯[13]，LIO-SAM [4]和Lili-OM [14]。当配准新扫描时，扫描中的每个点通过迭代最近点（ICP）[15]或其变体（例如，广义ICP [16]）。最近的工作VoxelMap [17]使用体素图管理局部平面，并同时估计平面参数和置信度。

其他作品试图明确地提取环境中的平面，并将它们作为地标添加到SLAM中。Kaess等人[18]提出了一种基于四元数的平面参数化方法，并通过构建相对平面模型来提高收敛速度。日内瓦等人。[19]利用最近点（CP）对平面进行参数化，其性能优于[18]中介绍的平面参数化方法。Liu等人在BALM [5]中推导了点到平面成本函数的一阶和二阶导数，从而通过Levenberg-Marquardt算法[20]实现了有效的迭代优化。随后，Zhou等人提出了π-LSAM [6]，它参考了视觉SLAM中的Bundle Adjustment（BA）概念，并使用Plane Adjustment（PA）方法联合优化滑动窗口中的关键帧和平面。

然而，这些作品中的大多数使用RANSAC [21]提取平面，这是非常缓慢的。此外，SLAM框架中的复杂平面利用增加了计算量，这对系统的实时性能带来了重大挑战。

三.系统概述

我们的系统概述如图2所示。对IMU预积分得到LiDAR点云畸变和配准的预测姿态。对于输入的LiDAR点云数据，我们提取局部边缘和平面特征（图2中的特征提取）[3]，并将其与特征图（图2中的扫描配准）进行配准，以获得初始姿态（图2中的LiDAR Odom）。然后，我们提取显式平面（图2中的平面提取，第IV节）供以后使用。我们构建了一个因子图，包括IMU预积分因子[22]、LiDAR里程计因子和滑动窗口内的显式平面因子（图2中的滑动窗口里程）。V-C），并解决该问题，以获得更准确的里程（图2中的Refined Odom）。最后，我们使用优化的里程计因子和全局平面因子执行基于关键帧的全局优化，以实现一致的映射（图2中的映射，第二节）。V-D）。

四.高效的平面提取

在本节中，我们将介绍所提出的高效平面提取方法，该方法将点分组为线，然后将线分组为平面，即点→线→平面。在深入研究之前，我们定义了平面π = [n d] ∈ R4和直线l = [n m] ∈ R6，其中n ∈ R3（n ∈ R2 = 1）是平面或直线的单位方向向量，d ∈ R1是从框架原点到平面的有符号距离，m ∈ R3是直线的中点。

A.基于环的直线拟合

基于环的直线拟合的基本思想类似于区域生长。首先，我们通过环形字段对扫描进行分类，并通过增加时间戳对点进行排序。然后，我们在每个环中随机选择一个点作为种子，并在时间上向前和向后逐个遍历这些点以拟合线段。如果距离dpl（等式1)如果从一个点P1到拟合线L = [n，m]小于阈值Δ P1，则我们将该点分组到线中并更新线的参数。否则，我们选择pi作为新的种子来生长一条新的线。

给定一条直线上的一组点，我们将直线的中点更新为所有点的均值，将方向更新为与所有点的协方差矩阵的最大特征值对应的特征向量。方向更新中的特征分解是直线拟合中最耗时的步骤。为了更有效的计算，我们使用专门针对对称3 × 3矩阵的解析解[24]来加速特征值分解。

B.基于直线的平面提取

为了进一步拟合基于拟合线的平面，我们构造如图3（a）所示的线图。我们将所有线段表示为节点，将线段之间的空间相邻关系表示为边。令li，j表示在扫描的第i个环中检测到的第j个线段。线图中有两种类型的边：（1）同一环中的线之间的边，即li，j和li，j+1之间的边;（2）相邻环中有重叠的线之间的边，即li，r1 - 1和li+1，r2 - 1之间的边。与边连接的节点被捐赠为邻居节点。我们应用经典的呼吸优先搜索（BFS）方法来拟合线图中的平面。如图3（B）所示，具体步骤如下：

从第一个环的第一个节点开始，遍历整个线图的节点。
搜索根节点的所有邻居节点，尝试初始化给定两个节点（线）的平面。
初始化平面后，遍历其所有邻居节点。如果直线在平面上，请将其添加到平面并更新平面参数。
在遍历所有节点之后，算法结束。

我们通过对第IV-a节中提出的点的协方差矩阵进行快速特征值分解来更新平面的方向向量。最小特征值对应的特征向量是平面的方向矢量，平面的参数d是从帧原点到平面中点的有符号距离。

最后，我们删除小面积的平面，合并具有相似参数的平面。

V.平面辅助激光雷达-惯性SLAM

在本节中，我们将介绍所提出的局部平面辅助滑动窗口里程计，以提高里程计的准确性，以及全局平面辅助映射，以建立更一致的地图。在下文中，我们将世界坐标系表示为W，将LiDAR坐标系表示为L。

A.平面关联

给定平面在LiDAR坐标系L中，我们可以将其转换为世界帧W，

其中W L R和W L t分别是从LiDAR坐标系到世界坐标系的旋转和平移。

假设我们已经通过等式2将新扫描的平面从LiDAR帧转移到世界帧。设πi = ni di T是要关联的新扫描的平面，并且设πm = nm dm T是映射中的平面。我们设计一个平面距离不敏感的度量如下：

其中θi，m是法向量ni和nm之间的角度，di，m是从平面πi的中心pi到地图平面πm的距离。如果θi，m < θthre和di，m < dthre，我们认为它们是同一个平面。实际上，θthre设为15 μ m，dthre设为0.2 m。大多数作品 [25–27] 使用距离 di，m = |di − dm|测量两个平面的距离。然而，我们发现它对平面的位置很敏感。如图4所示，法向量na和nm之间存在小角度误差，导致距离da，m大于db，m。相反，从 πa 和 πb 的中心点到平面 πm 的距离几乎相同，使其成为平面关联的更合适的选择。

B.扫描配准

对于时间t∈[ti, tj]的扫描点，我们使用IMU积分对ti和tj之间的位姿进行线性插值，从而将扫描的所有LiDAR点对齐到开始时间ti。为了提高计算效率，我们使用LOAM[3]提出的方法提取局部边缘和平面特征进行扫描配准。为了在时刻i注册激光雷达扫描，我们首先选择一些最近的关键帧来构建局部地图，由局部边缘地图和局部平面地图组成。然后将当前帧的边缘特征和平面特征与相应的特征映射进行匹配。我们利用点到的边缘和点到平面的残差[28]，利用Levenberg-Marquardt算法[20]进行迭代求解，并获得该扫描的初始姿态。

C.平面辅助里程计

为了给平面关联映射提供更精确的初始姿态，我们借助显式平面进一步优化滑动窗口中的姿态。由于滑动窗口中的平面在空间上是相邻的，我们只需使用第V-A节中的方法来关联相同的平面。滑动窗口的因子图如图5所示，由IMU预积分因子[22]、LiDAR里程计因子、显式平面观测因子和边缘化先验因子组成。在滑动窗口优化中，我们选择包含LiDAR里程计因子而不是LiDAR局部特征因子。这是准确性和实时性之间的权衡。

给定世界坐标系中的两个平面πi和πm，平面因子定义如下：

运算减表示两个平面之间的误差，其中Bp ∈ R3×2是ni的切平面上的基的集合，φ ∈ R3被定义为：

D.平面辅助地图构建

1)基于散列的地图平面索引：为了提高扫描和全局平面地图之间的平面关联的速度和鲁棒性，我们提出了一种基于3D体素的平面索引方法来检测平面之间是否存在重叠。此外，我们通过哈希函数将3D体素映射到1D变量，以实现快速索引。如图6（a）所示，我们首先对平面中的线进行采样并得到点集P。然后对于任何点pi ∈ P，我们计算其体素坐标，

其中v是整数坐标，s是体素大小。给定一个体素v =（vx，vy，vz），我们可以使用哈希算法[30]获得它的哈希值：

其中nx，ny，nz是三个大素数，N是哈希表的大小。最后，我们维护一个哈希表，其中的每个元素包含一个数组，该数组的平面id与该元素对应的体素相交。

2)全局地图平面关联：给定当前扫描的一个平面πi，首先计算其体素坐标和哈希索引。然后在哈希表中查询所有哈希索引与πi相同的平面id，得到映射中的候选平面。最后，我们使用V-A节中的平面关联方法来确定映射中的哪个平面与平面π相同。如图6(b)所示，平面b与平面A不仅参数相似，而且几何重叠。此外，我们还提出了两个Double Check标准来提高平面关联的准确性: