论文粗读系列一：FEVO-LOAM：特征提取与垂直优化的激光雷达里程计和建图

作者：Zelin Wang , Limin Yang, Feng Gao , and Liangyu Wang

来源：IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 7, NO. 4, OCTOBER 2022

摘要

同时定位与建图（SLAM）是机器人领域的一个重要研究课题，因为它是机器人自动化的关键技术之一。尽管基于激光雷达的SLAM方法已经取得了可喜的性能，但传统的激光雷达SLAM方法仍然会产生较大的垂直误差。为了解决这个问题，提出了一种特征提取和垂直优化的激光雷达里程计和建图方法。首先，优化特征提取。具体来说，提出了一种更准确的地面分割方法和新的曲率定义，用于提取更具辨别力的特征。此外，提出了一种激光雷达建图方法，它将新的垂直残差和俯仰残差添加到目标函数中。然后使用两步Levenberg-Marquardt方法来求解姿态变换。最后，在公共数据集和真实环境中评估所提出的方法。实验表明，与其他最先进的方法相比， 本文的方法以相似的计算成本实现了更好的精度并减少了垂直误差。

主要贡献

1. 提出了一种新的地面分割方法。考虑了点到激光雷达的距离以及每个点的法线方向，可以更准确地提取地面点，方便后续步骤。

2. 提出了曲率的一种新定义方法。通过分析点邻域的协方差矩阵，对点进行分类并计算曲率，可以提取更具判别性的特征。

3. 在激光雷达建图中，向目标函数添加了两个新的残差：垂直残差和俯仰残差。这些残差将有助于在垂直方向上收敛到最优解。然后提出了一种两步 Levenberg-Marquardt 方法来解决姿势变换。

核心内容

一、算法框架流程

该算法框架和LeGO-LOAM几乎一样，主要分为两大模块，分别是scan registration和odometry and mapping。scan registration模块的主要功能就是点云预处理，对于第k帧输入的点云，首先进行地面点和非地面点分割，用非地面点去提取角点。odometry and mapping模块主要功能是帧间位姿求解以及位姿优化，并用优化后的位姿将点云注册到全局地图。最后，在将优化前后的位姿进行融合输出，作为当前帧的位姿。

二、地面点分割

本文将地面分割分两个部分，分别为：粗分割（图3所示）和精分割。其中，粗分割分为两步。第一步，选取m（m不需要太大，小于等于3即可）条垂直角度小的线束，使用RANSAC算法对这m条光束的点进行平面拟合，得到估计的地平面方程。第二步，利用激光点与估计地平面的关系进行地面分割。具体做法为：计算出每个点到估计出的地平面方程的距离，然后利用公式（1）判断点是否属于地面。通过这两步就完成了粗配准。对于精配准，将粗配准得到的点构建一个KD-tree，然后计算出每个点的法向量，将该法向量与估计的平面法向量进行对比，只有夹角小于阈值的点才被认为是最终的地面点。

三、特征提取

首先，本文提出一个判断点是否是连续点的方法，这里的连续点是指LOAM论文剔除的被遮挡的点，具体的判别公式如下：

当计算的值小于给定阈值时，认为是连续点，这些连续的非地面点将会被用于角点检测，如果被判别为不连续的点，则认为是面点。

下面，就是角点提取，这里提出了一种全新的曲率计算公式。对于任意一点，在这个点相同的光束上找个邻近点，分别构成前向点集 和后向点集，然后使用公式（3）分别计算这两组点集的协方差矩阵和：

在此基础上，定义曲率计算公式为：

其中，和分别为协方差矩阵和的最大特征值对应的特征向量。图五对比了此方法提取到的角点与LOAM提取到的角点，可以看出使用此方法提出的角点更加准确。

四、激光里程计

图6为激光里程计的求解框架，对于角点，残差定义为：

对于面点，残差定义为：

最后，构成优化函数为最小化加权距离平方和，进而使用LM优化算法求解位姿，具体为：

五、激光雷达建图

整体思路，是LOAM的方法一致，为了得到准确的位姿，将scan与维护的局部地图做配准得到准确的pose。但是，在实际运用中发现，LOAM在垂直方向存在较大的误差。这部分垂直误差主要来自于两个方面，第一，角点和面点对垂直方向的位姿变化不敏感，许多提取的线特征和平面特征几乎垂直于水平平面，特别是在规则建筑密集的环境中。这些特征会使垂直平移难以收敛到最优解，因为不同高度点到垂直线或平面的距离相同。第二，俯仰角估计不准确也会导致垂直误差。特别是当前向距离较长时，小俯仰角误差会导致较大的垂直误差。

因此，在建图时分两步优化，第一步先优化俯仰角变换和垂直变换，引入新的残差函数。对于垂直变换，引入残差函数：

对于俯仰角变换，引入残差函数：

此外，我们还将角点和地面点的残差添加到优化函数中。角点残差可以约束水平变换，地面点主要用于估计角度的变换。为了减少计算开销，地面点与第 k-1 帧的点云匹配，而不是局部地图。由此，得到第一步优化的优化函数为：

下面进行第二步优化，与LeGO-LOAM不同的是，我们不将第一步估计的和直接作为输出，因为它们的错误可能导致后续的姿态估计不鲁棒。因此，优化函数为：

实验部分

个人想法与总结

通过对论文的粗读，可以看出整体结构与LeGO-LOAM可以说一模一样，主要的区别在于地面点和特征的提取以及优化位姿时增加了一些残差项。我的个人观点是这篇文章读完可能在学术界来看没有太多的创新，感觉和LOAM系列论文都差不多，但是这篇论文提出的一些想法在实际工程化时，还是有参考借鉴价值的。比如，在实际定位时z轴和纵向的累计误差增长速度很快，导致定位不准，这篇文章提出的一些残差构建想法可以去尝试一下。此外，很多学术界大佬都提到过定位只需要面特征即可，线特征的引入没有必要。但是，我个人的想法是线特征对纵向的约束是非常好的，仅仅依靠面特征是不足的，这个现象在实际工作中已经遇到了，所以在计算量和实时性允许的情况下，在单独引入一些线特征的约束是可以提高定位精度的。

以上仅代表个人的想法，如有错误，欢迎批评指正，一起讨论交流。

关注方式

一、微信公众号

二、b站主页