文章目录
- 摘要
- 一、介绍
- [二、 相关工作](#二、 相关工作)
- [三、 Light-LOAM](#三、 Light-LOAM)
-
- [A. 特征提取与选择](#A. 特征提取与选择)
- [B. 基于图的两阶段特征匹配](#B. 基于图的两阶段特征匹配)
- [C. 一致性引导的激光雷达里程计](#C. 一致性引导的激光雷达里程计)
- [D. 轻量化LiDAR建图](#D. 轻量化LiDAR建图)
- 四、实验
- 五、总结
摘要
代码：github
原文：原文
将SLAM应用于机器人应用中，可靠性和效率是两个最受重视的特性。本文考虑在计算能力有限的平台上实现可靠的基于激光雷达的SLAM功能。首先与大多数选择点云配准的显著特征的方法相反，我们提出了一种非显著特征选择策略，以提高可靠性和鲁棒性。然后使用两阶段对应选择方法来配准点云，其中包括基于KD树的粗匹配，然后是一种基于图的匹配方法，它使用几何一致性来排除不正确的对应关系。此外提出了一种里程计方法，其中权重优化是由前述的几何一致性图的投票结果引导的。通过这种方式，激光雷达里程计的优化迅速收敛，评估出一个相当准确的变换结果，从而使后端模块能够高效完成地图任务。最后，我们在KITTI测距数据集和实际环境中评估了我们提出的框架。实验表明，与主流的基于激光雷达的SLAM解决方案相比，我们的SLAM系统在精度方面达到了相对水平或更高水平，同时在计算效率上取得了更好的平衡。
图1.不同数据关联方法在两个连续扫描之间的特征点对齐。(a) 某种情景的点云扫描。(b) K最近邻方法。© 基于图形的两阶段匹配方法。
一、介绍
同时定位与建图（SLAM）如今被认为是许多移动机器人在GNSS信号受限环境中实现自主导航的必不可少的模块。为了适应不同的任务和平台，已经提出了基于不同传感器、处理方法或功能结构的各种SLAM框架。根据传感器类型，基于视觉的SLAM和基于LiDAR的SLAM是最广泛研究的两种方法。基于视觉的方法在许多稳定环境中提供了最经济和实用的定位解决方案。另一方面，基于LiDAR的方法通常在结构丰富的环境中更为优选，并且被认为比视觉传感器在光照条件变化下更具鲁棒性。LiDAR相关的几个重要工作包括LiDAR里程计与建图（LOAM）[21]、轻量化和地面优化的LiDAR里程计与建图（LeGO-LOAM）[12]，以及Cartographer [5]。
不论如何分类，大多数SLAM工作都集中在两个方面的改进：1）定位性能，如精度和可靠性；2）运行时效率，如计算负担和存储需求。通常，在资源有限的机器人平台上，这两个方面不能同时得到优化。因此，通常需要在性能和效率之间进行权衡，以便为实际的机器人应用配置合适的SLAM系统功能。
在本文中，我们开发了一个轻量化的LiDAR-based SLAM系统，其性能与最先进的方法相当，但具有平衡的计算需求，可以适应资源受限的平台。贡献主要体现在两个方面。首先，我们开发了一种创新的SLAM前端，包括一个不显眼的特征选择策略和一个基于图的特征匹配功能，以实现更好的点云配准。图1是一个视觉化表示，展示了前端点云对齐任务中的定性比较。其次，利用前端可靠的配准结果，我们开发了一个轻量化的后端，可以在计算受限的平台上更高效地执行。实验验证使用了公共数据集和自采集数据。我们的Light-LOAM实现将免费提供，网址：https://github.com/BrenYi/Light-LOAM。
二、 相关工作
大致而言，基于LiDAR的SLAM功能可以分为两个子功能：前端里程计和后端地图优化。在前端部分，主要目的是通过扫描对扫描或扫描对地图的配准，获取连续帧之间的增量变换。在后端部分，使用集成更多状态约束的估计器/优化器来提高前端估计的精度和平滑性。在大多数SLAM框架中，后端任务消耗的计算资源通常多于前端，且更新速度较慢。随着多线程技术的应用，较慢的线程决定了整个SLAM功能的工作频率。在机器人控制循环中，较慢的地图更新速率可能会影响机器人在未知环境中的反馈反应。因此，我们考虑通过提高性能来实现一个更平衡的SLAM功能划分，以适应实际的机器人应用。更具体地，我们将更多计算资源投入到前端，以提高点云配准性能，并通过更精确的前端配准，减少后端部分的计算开销。
在前端部分，主要有两种方法来实现扫描之间或扫描与地图之间的点云配准。第一种方法是通过点对点对应关系直接获得相对变换。迭代最近点（ICP）及其变种，如GICP [11]，是最广泛使用的点云配准方法。ICP方法广泛应用于早期阶段的2D LiDAR-based SLAM框架中，尤其是在点云的稀疏程度远大于3D LiDAR时。此外，法线分布变换（NDT）被认为是另一种直接的点云配准方法。尽管NDT方法使用所有点，但它通过法线分布来表示点云，然后以概率的方式计算相对变换。然而，直接方法使用所有点来优化相对变换，因此它的计算需求可能成为处理实时3D LiDAR扫描序列的主要问题。第二种方法是从点云中提取显著特征，然后仅使用特征点进行配准。在里程碑式的工作LOAM中，点是根据曲率进行选择的，并被指定为平面点和边缘点，然后基于最近邻方法进行配准。类似的几何特征也被用于LeGO-LOAM [12]和F-LOAM [15]。除了几何计算特征外，还有一种趋势是实现基于学习的方法，通过深度神经网络获取特征，因为其在描述符中能够表示非线性。然而，基于学习的方法常常被批评为依赖于数据，并且可能不适用于未知环境。尽管上述方法在点云扫描的表示方式上有所不同，但大多数方法使用基于KD树的技术来进行高效的对应关系索引。虽然KD树[10]在SLAM系统中广泛用于建立初始对应关系[12]，[13]，[15]，[16]，[21]，但在噪声或遮挡环境中，它可能会引入错误的关联。
近年来，研究者在利用几何一致性和图论原理来解决点云数据关联问题方面取得了显著进展。Bailey等人的开创性工作[1]介绍了几何一致性在解决2D LiDAR地图构建问题中的应用。该方法通过构建图形，并通过识别最大公共子图来选择正确的对应关系集。类似地，Lajoie等人[7]利用成对一致的测量来减轻分布式SLAM系统中的虚假回环闭合。这种方法也涉及最大化一致子图，并在多机器人地图合并领域得到应用，如文献[9]中所记载。Yang等人[17]提出了TEASER图论框架，它结合了截断最小二乘优化方法和最大团内点选择技术，有效消除了许多虚假的对应关系。其在点云配准和扫描匹配任务中的有效性得到了验证。在研究[18]和[19]中，研究人员结合几何一致性图和多种投票策略来对对应关系进行排名，并选择可靠的内点。
另一种图论框架的发展是Clipper[8]，它构建了一个加权图，并将内点关联问题表述为优化问题，最终求解出一致对应关系的最密子图。尽管这些基于图的关联问题解决方案在遵守几何一致性约束方面表现出有效性，但它们在优化效率上仍然面临挑战。特别是在面对大规模点云配准和严格的时间约束时，它们的性能未能达到预期水平，例如在基于LiDAR的SLAM框架中。
在本文中，我们提出了一种基于LiDAR的SLAM框架，通过图匹配方法实现了前端和后端功能的更平衡划分。我们将在第III节描述我们的方法流程，并在第IV节对我们提出的方法进行广泛验证。最后，第V节总结了本文内容。
三、 Light-LOAM
我们在图2中展示了Light-LOAM SLAM系统的流程图，该系统由三个核心阶段组成：预处理、两阶段特征匹配和姿态估计。
在预处理阶段，我们首先从每个点云扫描中过滤掉不连通的点。为了选择具有微小局部几何属性的稳定角点和平面特征，我们采用了一种不显眼的选择方法，并过滤掉了最显著的角点和平面特征。这是与其他方法[12]，[15]，[21]的主要区别之一。接下来进行两阶段特征匹配过程。在第一阶段，采用基于KD树的方法[21]来为选定的特征建立初始对应关系。然后，我们引入基于图的相容性投票机制来评估这些对应关系，有效地过滤掉不可靠的关联。进入前端里程计模块后，可靠点对的一致性得分被用来优化变换，从而得到初步的、相对精确的姿态估计。最后，在这些初步可靠估计的支持下，映射模块以更高效的方式优化出更精确的姿态。
A. 特征提取与选择
a) 不连通点去除：由于3D LiDAR传感器产生的数据量庞大，特征提取和基于特征的配准是高效变换评估的广泛采用方法。然而，在提取特征候选之前，去除不连通的物体是至关重要的。不连通的点通常代表离群点或被遮挡物体的部分，它们的包含可能会显著降低后续特征关联和姿态估计的质量。因此，与先前的工作[2]一致，我们采用以下标准来排除这些不连通的点：
∣ ∥ p i + 1 k − p i k ∥ 2 − ∥ p i − 1 k − p i k ∥ 2 ∣ > σ disjoint . (1) \left| \left\| p_{i+1}^k - p_i^k \right\|2 - \left\| p{i-1}^k - p_i^k \right\|2 \right| > \sigma{\text{disjoint}}. \tag{1} pi+1k−pik 2− pi−1k−pik 2 >σdisjoint.(1)
其中 p i k p_i^k pik 表示位于第 k 个激光束通道中的点 i i i， σ disjoint \sigma_{\text{disjoint}} σdisjoint 作为判断阈值。如果一个点到其两侧邻居的欧几里得距离的绝对差超过 σ disjoint \sigma_{\text{disjoint}} σdisjoint，则该点被分类为不连续的；否则，它被认为是连续的候选点。
b) 不显著特征提取和选择：在消除不连续点之后，我们从每个激光束通道中提取特征点。使用平滑度度量（2）来描述点的局部几何属性。
r ( p i k ) = 1 ∣ S ∣ ∥ p i k ∥ ∥ ∑ j ∈ S , j ≠ i ( p i k − p j k ) ∥ . (2) r\left(p_i^k\right) = \frac{1}{|\mathcal{S}| \left\| p_i^k \right\|} \left\| \sum_{j \in \mathcal{S}, j \neq i} \left( p_i^k - p_j^k \right) \right\|. \tag{2} r(pik)=∣S∣ pik 1 j∈S,j=i∑(pik−pjk) .(2)
其中 S S S 表示被评估的点集，包括候选点及其左右两侧的相邻对象。符号 ∣ S ∣ |S| ∣S∣ 表示该集合的基数。如果候选点的平滑度 r ( p k ) r(p_k) r(pk) 超过阈值 r t r_t rt，则被指定为角点特征；否则，它被识别为平面特征。按照[21]中的方法，我们的分类过程涉及评估候选点两侧的5个点，实际实施中使用默认阈值 r t = 0.1 r_t = 0.1 rt=0.1 进行特征提取。
在传统的基于激光雷达的SLAM系统，如LOAM[21]、FLOAM[15]、LEGO-LOAM[12]中，感知空间通常被划分为几个子区域。从每个子区域中选择具有最高或最低平滑度属性的特征候选点进行后续的特征匹配。然而，我们的Light-LOAM SLAM系统引入了一种创新的不显著特征选择策略。如前所述，特征选择通常由区分性原则指导。但是，这些区分性特征真的足够稳健，并且能够作为高质量的优化样本吗？值得注意的是，一些异常值或被遮挡的点可能表现出高度区分性的几何属性。因此，我们认为，与最显著的特征相比，平滑度属性较弱的候选点可能更有价值且更稳健，适合用于数据关联。
我们的优先级是在每个子区域内选择较弱的角点和平面特征作为优化候选点。在启动特征选择过程之前，首先根据每个子区域内的平滑度值对点进行降序排序：
F k = { p i k , ... , p i + j k ∣ r ( p i k ) > ⋯ > r ( p i + j k ) } . (3) \mathcal{F}^k = \left\{ p_i^k, \ldots, p_{i+j}^k \left| r\left(p_i^k\right) \right. \right. > \cdots > r\left(p_{i+j}^k\right)\}. \tag{3} Fk={pik,...,pi+jk r(pik)>⋯>r(pi+jk)}.(3)
我们在有序集合 F k \mathcal{F}^k Fk 中选择第一个 k 个点之后的 m m m 个最尖锐的点，并将它们指定为边缘特征集 F e k \mathcal{F}_e^k Fek。同样，我们选择最后 l l l 个点之前的 n n n 个最平坦的候选点，并将它们包含在平面集 F s k \mathcal{F}_s^k Fsk 中。
F e k = { p i + k + 1 k , ... , p i + k + m k ∣ p x k ∈ F k , r ( p x k ) > r t } . (4) \mathcal{F}e^k = \left\{ p{i+k+1}^k, \ldots, p_{i+k+m}^k \mid p_x^k \in \mathcal{F}^k, r\left(p_x^k\right) > r_t \right\}. \tag{4} Fek={pi+k+1k,...,pi+k+mk∣pxk∈Fk,r(pxk)>rt}.(4)
F s k = { p i + j − l − n k , ... , p i + j − l − 1 k ∣ p x k ∈ F k , r ( p x k ) < r t } . (5) \begin{align*} \mathcal{F}s^k &= \left\{ p{i+j-l-n}^k, \ldots, p_{i+j-l-1}^k \mid p_x^k \in \mathcal{F}^k, r\left(p_x^k\right) < r_t \right\}. \end{align*} \tag{5} Fsk={pi+j−l−nk,...,pi+j−l−1k∣pxk∈Fk,r(pxk)<rt}.(5)
在我们的实现中，我们将每个激光束通道水平划分为6个子区域。在 F k \mathcal{F}^k Fk 中过度的特征排除限制了优化样本，影响了SLAM中的姿态估计精度。通过多次实验，设置 k = 1 k=1 k=1 和 l = 2 l=2 l=2 被证明是合适的。类似于[21]，分别表示为 m 和 n 的选定边缘和平面特征的数量被设置为 2 和 4。
这种不显著特征选择方法，结合不连续点移除的预处理步骤，有效地过滤掉更多的异常值，确保了后续姿态估计的候选集更加可靠。
B. 基于图的两阶段特征匹配
从点云的上一扫描与现有构建地图中识别对应特征是后续扫描对扫描和扫描对地图配准的基本前提。由于其高效性和有效性，KD树[10]是建立对应关系的广泛使用方法，许多研究[12]，[15]，[21]也采用了该方法。尽管KD树被广泛应用，但它容易受到环境遮挡、离群点和点云噪声的影响，从而导致不准确的姿态估计。例如，如图4所示，可能出现当前扫描中的多个候选特征与上一扫描或地图中的同一位置匹配为最近的点，从而导致错误的多对一对应情况。为了减轻这些问题并减少虚假对应，我们引入了一种新型的基于图的两阶段对应选择方法。
a) 通过KD树确定初始对应关系：我们首先使用KD树来寻找特征候选的对应关系，假设上一扫描或地图中的最近点是每个特征的真实对应点。通过公式（6），我们建立了初步的点对集合。
W ≜ { ( p ˉ i k , p ˉ i ′ t ) ∣ p ˉ i k ∈ F e k ∪ F s k , p ˉ i ′ t ∈ F e ′ ∪ F s ′ } . (6) \mathcal{W} \triangleq \left\{ \left( \bar{p}{i}^{k}, \bar{p}{i^{\prime}}^{t} \right) \mid \bar{p}{i}^{k} \in \mathcal{F}{e}^{k} \cup \mathcal{F}{s}^{k}, \bar{p}{i^{\prime}}^{t} \in \mathcal{F}{e^{\prime}} \cup \mathcal{F}{s^{\prime}} \right\}. \tag{6} W≜{(pˉik,pˉi′t)∣pˉik∈Fek∪Fsk,pˉi′t∈Fe′∪Fs′}.(6)
其中 p ˉ i k \bar{p}{i}^{k} pˉik 表示当前扫描的边缘集 F e k \mathcal{F}{e}^{k} Fek 或平面组 F s k \mathcal{F}{s}^{k} Fsk 中的源特征，目标特征点 p ˉ i ′ t \bar{p}{i^{\prime}}^{t} pˉi′t 对应于它们从目标特征集 F e ′ \mathcal{F}{e^{\prime}} Fe′ 和 F s ′ \mathcal{F}{s^{\prime}} Fs′ 中最近的对应点。在前端，这些目标特征集 F e ′ \mathcal{F}{e^{\prime}} Fe′ 和 F s ′ \mathcal{F}{s^{\prime}} Fs′ 包括最后一次扫描的特征集 F e k − 1 \mathcal{F}{e}^{k-1} Fek−1 和 F s k − 1 \mathcal{F}{s}^{k-1} Fsk−1，而在映射阶段，它们代表特征地图 F e W \mathcal{F}{e}^{W} FeW 和 F s W \mathcal{F}{s}^{W} FsW。
b) 通过一致图的可靠关联：在第二阶段，引入了一种基于图的对应验证算法。从 KD-tree 的初始假定关联 W \mathcal{W} W 开始，基于几何一致性原则构建兼容性图。
在深入几何一致性的概念之前，我们假设存在两个正确的关联 ( p ˉ i k , p ˉ i ′ t ) \left( \bar{p}{i}^{k}, \bar{p}{i^{\prime}}^{t} \right) (pˉik,pˉi′t) 和 ( p ˉ j k , p ˉ j ′ t ) \left( \bar{p}{j}^{k}, \bar{p}{j^{\prime}}^{t} \right) (pˉjk,pˉj′t)，它们共享相同的变换参数集 ( R , T ) (R, T) (R,T)。这两对关联可以表述为：
p ˉ i ′ t = R p ˉ i k + T . (7) \bar{p}_{i^{\prime}}^t = R\bar{p}_i^k + T. \tag{7} pˉi′t=Rpˉik+T.(7)
p ˉ j ′ t = R p ˉ j k + T . (8) \bar{p}_{j^{\prime}}^t = R\bar{p}_j^k + T. \tag{8} pˉj′t=Rpˉjk+T.(8)
从理论上讲，两个目标点之间的欧几里得距离在不同帧之间保持不变，如 (9) 所表达的，体现了我们所说的几何一致性。
∥ p ˉ i ′ t − p ˉ j ′ t ∥ 2 = ∥ p ˉ i k − p ˉ j k ∥ 2 . (9) \left\|\bar{p}{i^{\prime}}^{t}-\bar{p}{j^{\prime}}^{t}\right\|{2}=\left\|\bar{p}{i}^{k}-\bar{p}{j}^{k}\right\|{2}. \tag{9} pˉi′t−pˉj′t 2= pˉik−pˉjk 2.(9)
我们可以利用这个约束在图空间而不是欧几里得空间中评估对应关系的兼容性。为了便于说明，假设从KD树生成了四个假设的关联情况，如图3(a)所示。我们可以构建一个兼容性图，其中每个顶点，表示为 V i ( p t , p t ′ ) V_i(p_t, p_{t'}) Vi(pt,pt′)，代表第i个关联关系。图中的边表示两个关联 V i V_i Vi 和 V j V_j Vj 是兼容的或几何上一致的。在图3©显示的图中，存在四个关联： V 1 V_1 V1、 V 2 V_2 V2、 V 3 V_3 V3 和 V 4 V_4 V4。值得注意的是， V 1 V_1 V1、 V 2 V_2 V2 和 V 3 V_3 V3 是相互几何一致的。
随后，如图3所示，使用亲和力矩阵 M M M 构建兼容性图。矩阵中的每个条目 M ( i , j ) M(i, j) M(i,j) 表示对应关系对 ( V i , V j ) (V_i, V_j) (Vi,Vj) 的几何一致性得分，并定量计算为：
S c ( v i , v j ) ≜ exp ( − d ( v i , v j ) 2 σ 2 ) . (10) S_{c}\left(v_{i}, v_{j}\right) \triangleq \exp\left(-\frac{d\left(v_{i}, v_{j}\right)^{2}}{\sigma^{2}}\right). \tag{10} Sc(vi,vj)≜exp(−σ2d(vi,vj)2).(10)
这里，项 d ( v i , v j ) d(v_{i}, v_{j}) d(vi,vj) 定义为：
d ( v i , v j ) ≜ ∥ p ˉ i ′ t − p ˉ j ′ t ∥ 2 − ∥ p ˉ i k − p ˉ j k ∥ 2 . (11) d(v_{i}, v_{j}) \triangleq \left\|\bar{p}{i^{\prime}}^{t} - \bar{p}{j^{\prime}}^{t}\right\|{2} - \left\|\bar{p}{i}^{k} - \bar{p}{j}^{k}\right\|{2}. \tag{11} d(vi,vj)≜ pˉi′t−pˉj′t 2− pˉik−pˉjk 2.(11)
其中 σ \sigma σ 作为距离调整参数。值得注意的是， S c S_c Sc 的范围从 0 到 1，对于完美的几何一致性达到 1。对角线项 M ( i , i ) M(i, i) M(i,i) 始终等于 1。较低的得分表示不一致性程度更高。
基于兼容性图，我们采用投票规则来评估每个对应关系的质量。投票机制，如公式 (12) 所定义的，便于计算每个关联的投票得分 o i o_i oi，提供一种评估其一致性的方法。
o i = o ( v i ) ≜ ∑ j = 0 , j ≠ i ∣ W ∣ ⌊ S c ( v i , v j ) η ⌋ . (12) o_{i} = o(v_{i}) \triangleq \sum_{j=0, j \neq i}^{|\mathcal{W}|} \left\lfloor \frac{S_{c}(v_{i}, v_{j})}{\eta} \right\rfloor. \tag{12} oi=o(vi)≜j=0,j=i∑∣W∣⌊ηSc(vi,vj)⌋.(12)
其中 n n n 作为投票阈值，决定了在投票过程中两个关联的兼容性。此外， W W W 表示对应集合 W W W 的基数。在这个方案中，如果关联 V i V_i Vi 与关联 V j V_j Vj 的一致性得分 S c ( V i , V j ) S_c(V_i, V_j) Sc(Vi,Vj) 达到或超过阈值 n n n，则关联 V i V_i Vi 获得一票。投票过程中的最终一致性水平由所有一致的投票者决定。
在我们完成投票流程后，我们可以将投票结果的序列以降序表示如下：
O = [ o 1 , o 2 > o 3 > ... > o i , i ∈ [ 1 , W ] ] (13) O = [o_1, o_2 > o_3 > \ldots > o_i, i \in [1, W]] \tag{13} O=[o1,o2>o3>...>oi,i∈[1,W]](13)
如果一个对应候选 i i i 收到的投票得分 o i o_i oi 低于总关联候选数的 % \% %，则被认为是不可靠的关联，并随后被过滤掉。在移除这些异常关联后，我们获得了最终的可靠关联集 O ′ O' O′ 以及它们的对应的得分 W ′ \mathcal{W}^{\prime} W′：
O ′ = { o i ∣ o i ∈ O , o i > x ∣ O ∣ } . (14) \mathcal{O}^{\prime} = \left\{ o_i \mid o_i \in \mathcal{O}, o_i > \frac{x}{|\mathcal{O}|} \right\}. \tag{14} O′={oi∣oi∈O,oi>∣O∣x}.(14)
W ′ = { v i ∣ v i ∈ W , o ( v i ) ∈ O ′ } . (15) \mathcal{W}^{\prime} = \left\{ v_i \mid v_i \in \mathcal{W}, o(v_i) \in \mathcal{O}^{\prime} \right\}. \tag{15} W′={vi∣vi∈W,o(vi)∈O′}.(15)
在分析我们的基于图的匹配算法的计算复杂性时，其中包含 N 个对应关系，兼容性图的构建和使用快速排序的对应关系排名的时间复杂度分别为 O ( N 2 ) O(N^2) O(N2) 和 O ( N log N ) O(N \log N) O(NlogN)。这导致我们的基于图的投票算法总的时间复杂度为 O ( N 2 + N log N ) = O ( N 2 ) O(N^2 + N \log N) = O(N^2) O(N2+NlogN)=O(N2)。为了保持实时性能，我们将感知空间划分为 n 个子区域。每个子区域内的对应关系形成子图，处理里程计和映射阶段的任务。在里程计阶段，每个子区域处理大约 200 个对应关系，平均总处理时间为大约 3 m s 3~ms 3 ms。在映射阶段，每个子区域处理大约 350 个对应关系，平均总处理时间为约 7 m s 7~ms 7 ms。这种对应关系的子区域划分确保了实时性能和准确的结果。
图 4. 由 KD 树生成的初始特征对应关系的演示。在每个对应关系中，红点是当前扫描中的源特征，蓝点是来自上一个扫描或地图的对应目标对象。椭圆表示错误的数据关联。 (a) 前端初始对应关系示意图。 (b) 后端初始对应关系示意图。
C. 一致性引导的激光雷达里程计
在激光雷达SLAM系统中，里程计对于通过扫描到扫描的点云匹配来优化初始姿态至关重要。里程计模块通常提供高频但精度较低的姿态估计，作为映射模块的初始输入。里程计模块估计的更准确初始变换加速了最终机器人姿态估计的收敛，从而减少了映射后端的计算成本。鉴于此，我们提出了一种新颖的激光雷达里程计机制，其中姿态优化由兼容性图的投票结果引导。
在我们的里程计模块中，我们旨在优化到第 (k-1) 帧的变换，并更新第 k 帧中点云的全局姿态 T W ∈ S E ( 3 ) T_W \in SE(3) TW∈SE(3)。在优化之前，我们通过假设匀速运动来校正点云中的运动失真。类似于LOAM[21]，我们定义了两种类型的残差项。第一项是点到线的距离残差，由下式给出：
f ( v i ) = ( P u k − P u k − 1 ) × ( P v k − P y k − 1 ) P u k − 1 − P s k − 1 (16) f(v_i) = \frac{(P_u^k - P_u^{k-1}) \times (P_v^k - P_y^{k-1})}{P_u^{k-1} - P_s^{k-1}} \tag{16} f(vi)=Puk−1−Psk−1(Puk−Puk−1)×(Pvk−Pyk−1)(16)
在公式(16)中， p ~ ( i , k ) L = T k k − 1 p ˉ ( i , k ) L \tilde{p}{(i, k)}^{L} = T{k}^{k-1} \bar{p}{(i, k)}^{L} p~(i,k)L=Tkk−1pˉ(i,k)L，其中 p ˉ ( i , k ) L ∈ F e k \bar{p}{(i, k)}^{L} \in \mathcal{F}{e}^{k} pˉ(i,k)L∈Fek 表示一个角点特征。 p ˉ ( i ′ , k − 1 ) L ∈ F e k − 1 \bar{p}{(i^{\prime}, k-1)}^{L} \in \mathcal{F}{e}^{k-1} pˉ(i′,k−1)L∈Fek−1 表示与 p ~ ( i , k ) L \tilde{p}{(i, k)}^{L} p~(i,k)L 同一激光束通道中最近的物体。点对 ( p ˉ ( i , k ) L , p ˉ ( i ′ , k − 1 ) L ) ∈ W ′ \left(\bar{p}{(i, k)}^{L}, \bar{p}{(i^{\prime}, k-1)}^{L}\right) \in \mathcal{W}^{\prime} (pˉ(i,k)L,pˉ(i′,k−1)L)∈W′ 表示通过我们基于图的两阶段特征匹配识别出的可靠对应关系。此外， p ˉ ( j ′ , k − 1 ) L ∈ F e k − 1 \bar{p}{(j^{\prime}, k-1)}^{L} \in \mathcal{F}{e}^{k-1} pˉ(j′,k−1)L∈Fek−1 是来自不同激光通道的 p ~ ( i , k ) L \tilde{p}{(i, k)}^{L} p~(i,k)L 的另一个最近邻。 p ˉ ( i ′ , k − 1 ) L \bar{p}{(i^{\prime}, k-1)}^{L} pˉ(i′,k−1)L 和 p ˉ ( j ′ , k − 1 ) L \bar{p}_{(j^{\prime}, k-1)}^{L} pˉ(j′,k−1)L 共同构成一条线几何残差项。
对于平面残差，我们有：
f s L ( v i ) = n s ⋅ ( p ~ ( i , k ) L − p ˉ ( i ′ , k − 1 ) L ) . (17) f_s^L(v_i) = n_s \cdot \left(\tilde{p}{(i,k)}^L - \bar{p}{(i^{\prime},k-1)}^L\right). \tag{17} fsL(vi)=ns⋅(p~(i,k)L−pˉ(i′,k−1)L).(17)
n s = ( p ‾ ( i ′ , k − 1 ) L − p ‾ ( j ′ , k − 1 ) L ) × ( p ‾ ( i ′ , k − 1 ) L − p ‾ ( l ′ , k − 1 ) L ) ∣ ( p ‾ ( i ′ , k − 1 ) L − p ‾ ( j ′ , k − 1 ) L ) × ( p ‾ ( i ′ , k − 1 ) L − p ‾ ( l ′ , k − 1 ) L ) ∣ (18) \mathbf{n}\mathbf{s} = \frac{\left( \overline{\mathbf{p}}^{L}{(i', k-1)} - \overline{\mathbf{p}}^{L}{(j', k-1)} \right) \times \left( \overline{\mathbf{p}}^{L}{(i', k-1)} - \overline{\mathbf{p}}^{L}{(l', k-1)} \right)}{\left| \left( \overline{\mathbf{p}}^{L}{(i', k-1)} - \overline{\mathbf{p}}^{L}{(j', k-1)} \right) \times \left( \overline{\mathbf{p}}^{L}{(i', k-1)} - \overline{\mathbf{p}}^{L}_{(l', k-1)} \right) \right|} \tag{18} ns= (p(i′,k−1)L−p(j′,k−1)L)×(p(i′,k−1)L−p(l′,k−1)L) (p(i′,k−1)L−p(j′,k−1)L)×(p(i′,k−1)L−p(l′,k−1)L)(18)
在(17)中， p ~ ( i , k ) L = T k k − 1 p i , k L \tilde{\mathbf{p}}^{L}{(i, k)} = \mathbf{T}^{k-1}{k} \mathbf{p}^{L}{i, k} p~(i,k)L=Tkk−1pi,kL，其中 p i , k L ∈ F s k \mathbf{p}^{L}{i, k} \in \mathcal{F}^{k}{s} pi,kL∈Fsk是一个平面特征。 p ‾ ( i ′ , k − 1 ) L \overline{\mathbf{p}}^{L}{(i', k-1)} p(i′,k−1)L和 p ‾ ( j ′ , k − 1 ) L \overline{\mathbf{p}}^{L}{(j', k-1)} p(j′,k−1)L分别是来自同一激光束通道的最接近投影特征 p ~ ( i , k ) L \tilde{\mathbf{p}}^{L}{(i, k)} p~(i,k)L的第一和第二接近物。此外， p ‾ ( l ′ , k − 1 ) L \overline{\mathbf{p}}^{L}{(l', k-1)} p(l′,k−1)L是来自不同通道的最近点。这三个点来自第 ( k − 1 ) (k-1) (k−1)次扫描，共同构成一个平面，以建立点到平面的残差。显然， ( p ‾ ( i ′ , k − 1 ) L , p ‾ ( j ′ , k − 1 ) L , p ‾ ( l ′ , k − 1 ) L ) ∈ W ′ (\overline{\mathbf{p}}^{L}{(i', k-1)}, \overline{\mathbf{p}}^{L}{(j', k-1)}, \overline{\mathbf{p}}^{L}{(l', k-1)}) \in \mathcal{W}' (p(i′,k−1)L,p(j′,k−1)L,p(l′,k−1)L)∈W′也是通过两阶段特征匹配过程获得的有效关联。
在章节II.B中，我们介绍了兼容性图，以过滤掉不可靠的关联，从而减轻可能会降低优化效果的问题。每个关联都与一个一致性投票分数相关，以指示其可靠性水平。我们据此理解了每个关联的潜在积极贡献。利用两阶段特征匹配过程的投票结果，我们为这些更可靠的关联分配更高的权重。设计这些自定义权重的过程可以表述如下：
W i = { α ⋅ o i − o min o max − o min , i ∈ [ 0 , λ ∣ O ′ ∣ ] 1 , i ∈ ( λ ∣ O ′ ∣ , ∣ O ′ ∣ ] . (19) W_{i} = \begin{cases} \alpha \cdot \frac{o_{i} - o_{\min}}{o_{\max} - o_{\min}}, & i \in [0, \lambda |\mathcal{O}'|] \\ 1, & i \in (\lambda |\mathcal{O}'|, |\mathcal{O}'|] \end{cases}. \tag{19} Wi={α⋅omax−ominoi−omin,1,i∈[0,λ∣O′∣]i∈(λ∣O′∣,∣O′∣].(19)
在排序集合 O ′ \mathcal{O}' O′的前 λ % \lambda \% λ%关联中，将接收自定义优化权重。 o min o_{\min} omin和 o max o_{\max} omax分别表示集合 O ′ \mathcal{O}' O′中的最小和最大分数， α \alpha α是比例因子。此外，其余关联的权重保持不变。最后，通过最小化加权残差项的总和来估计位姿：
min T k k − 1 ∑ W i f e L ( v i ) + ∑ W j f s L ( v j ) 。 (20) \min_{\mathbf{T}^{k-1}{k}} \sum W{i} f^{L}{e}(v{i}) + \sum W_{j} f^{L}{s}(v{j})。 \tag{20} Tkk−1min∑WifeL(vi)+∑WjfsL(vj)。(20)
借助代价函数(20)，我们可以使用 L e v e n b e r g − M a r q u a r d t Levenberg-Marquardt Levenberg−Marquardt方法确定位姿 T k k − 1 \mathbf{T}^{k-1}_{k} Tkk−1，如下所示：
T k k − 1 ← T k k − 1 − ( J T J + λ diag ( J T J ) ) − 1 J T f 。 (21) \mathbf{T}^{k-1}{k} \leftarrow \mathbf{T}^{k-1}{k} - \left(\mathbf{J}^{T} \mathbf{J} + \lambda \operatorname{diag}(\mathbf{J}^{T} \mathbf{J})\right)^{-1} \mathbf{J}^{T} \mathbf{f}。 \tag{21} Tkk−1←Tkk−1−(JTJ+λdiag(JTJ))−1JTf。(21)
在优化过程中， λ \lambda λ是拉格朗日乘子， J \mathbf{J} J是雅可比矩阵， f \mathbf{f} f是残差向量项，公式(21)通过迭代最小化代价函数(20)来获得变换 T k k − 1 \mathbf{T}^{k-1}_{k} Tkk−1。与这些高可靠性关联相关的残差项，随着权重增加，在优化过程中起到了主要作用。因此，梯度下降方向和参数更新主要由这些高质量关联引导，从而在位姿估计中实现更准确且更接近真实值的收敛。
D. 轻量化LiDAR建图
建图模块，通常是后端，处理精确的全局位姿估计，但频率较低。然而，我们现在提出一个简化的建图模块，以在精度和效率之间取得平衡。在映射模块的两阶段特征匹配中，KD树识别出邻域集合C，该集合由映射中每个特征p的五个最近点云组成。在映射模块的两阶段特征匹配中，KD树识别出邻域集合C，该集合由映射中每个特征p的五个最近物体组成。在基于图的第二阶段，我们指定C的质心 p ˉ i ′ , m W \bar{p}{i^{\prime}, m}^{W} pˉi′,mW作为特征 p ˉ i k \bar{p}{i}^{k} pˉik的临时对应物，建立特征对应关系 ( p ˉ i k , p ˉ i ′ , m W ) \left(\bar{p}{i}^{k},\bar{p}{i^{\prime}, m}^{W}\right) (pˉik,pˉi′,mW)。这些关系被用来构建兼容性图并执行投票。虽然兼容性图用于在映射过程中移除不可靠的对应关系，但投票结果并未用于衡量每个关联的重要性。
在映射模块的优化过程中，构建了两种类型的残差项：点到线残差项和点到面残差项。残差项 f e M ( v i ) f_{e}^{M}\left(v_{i}\right) feM(vi)和 f s M ( v j ) f_{s}^{M}\left(v_{j}\right) fsM(vj)分别使用与(16)和(17)相同的方程制定，这与基于LOAM的解决方案[21]一致。代价函数定义为：
min T k M ∑ f e M ( v i ) + ∑ f s M ( v j ) . (22) \min_{T_k^M}\sum f_e^M\left(v_i\right)+\sum f_s^M\left(v_j\right).\tag{22} TkMmin∑feM(vi)+∑fsM(vj).(22)
在这个优化过程中，我们使用Levenberg-Marquardt方法[4]估计全局姿态 T k M T_{k}^{M} TkM，而没有对残差项进行任何优先处理。
得益于不可靠关联的移除以及两阶段特征匹配的指导，里程计姿态迅速收敛。这为映射模块提供了更准确的初始变换估计，从而实现更快、更精确的全局姿态计算。相应的结果在第IV-B节中展示。
四、实验
如表I所示Light-LOAM系统在平均性能方面始终胜过其他系统，即使在具有挑战性的情景下，如KITTI数据集的序列01和02，Light-LOAM也保持其强大性能。请注意，我们的(a)指的是特征匹配模块。（注：原文中的"radians"可能是一个不完整的句子或术语，因为没有提供完整的上下文，所以无法提供一个准确的翻译。如果"radians"是单独的句子或术语，那么它的意思是"弧度"。）
表II中的结果强调了使用基于图的特征匹配方法时里程计估计的位姿精度的提高。
定位和地图性能结果如图7所示，显示出我们的地图结果具有高度准确性，并能够完成闭环检测。
五、总结
本文介绍了一种轻量级的LiDAR SLAM系统，名为Light-LOAM，它采用了基于图形匹配的技术，用于高效和准确的位姿估计。与传统的LOAM方法不同，我们提出了一种不引人注目的特征提取策略，以获取稳定的特征。基于图形的两阶段特征匹配方法评估了关联的一致性，过滤了不可靠的对应关系。一致性引导的里程计模块提供可靠的初始位姿估计，而轻量级的地图模块完成了定位和地图任务。在KITTI里程数据集和实际环境中的实验表明，Light-LOAM在准确性和效率方面优于最先进的解决方案，一致性图有效地过滤了异常的关联数据，实现了有限数量的特征样本的高质量位姿优化