基于激光雷达强度信息的实时SLAM方案

Posted 2023-03-05 Being_young

点云PCL免费知识星球，点云论文速读。

文章：Real-Time Simultaneous Localization and Mapping with LiDAR intensity

作者：Wenqiang Du and Giovanni Beltrame

编辑：点云PCL

来源：arXiv 2023

欢迎各位加入免费知识星球，获取PDF论文，欢迎转发朋友圈。文章仅做学术分享，如有侵权联系删文。未经博主同意请勿擅自转载。

公众号致力于点云处理，SLAM，三维视觉，高精地图等领域相关内容的干货分享，欢迎各位加入，有兴趣的可联系[e-mail]。未经作者允许请勿转载，欢迎各位同学积极分享和交流。

摘要

本文提出了一种新颖的基于LiDAR强度图像的实时定位和地图构建方法，解决了在非结构化环境中几何退化问题，传统基于LiDAR的前端里程计大多依赖于点、线和平面等几何特征，如果环境中缺乏这些特征，则整个里程计系统可能会失效，为避免这个问题，我们从LiDAR生成的点云中提取特征点，并将其与在LiDAR强度图像中识别出的特征相匹配，然后使用提取的特征点执行扫描点云的配准并估计机器人的自我运动，对于后端，我们联合优化相应特征点之间的距离以及地图中识别出的平面的点到平面的距离，此外利用从强度图像中提取的特征来检测来自先前扫描点云的回环闭合候选，并执行位姿图优化。实验表明，该方法可以在实时环境下高精度运行，并且可以很好地应对光照变化、低纹理和非结构化环境。

图1：两次连续扫描点云之间匹配的3D点及其对应的特征点，图(b)中的点是根据(a)中匹配特征的索引从点云中提取的3D点，红色点表示上一帧的匹配点，绿色点表示当前扫描的匹配点，这些点随后用于扫描配准，以估计两个连续帧之间的相对姿态。

主要贡献

提出了一种基于LiDAR强度SLAM方法，直接从强度图像中提取特征点并执行扫描点云配准来估计机器人的自我运动，主要贡献包括：

•  一种新颖的基于LiDAR强度图像的实时SLAM系统，旨在解决几何退化问题； 

• 将视觉SLAM系统的优点与LiDAR SLAM系统相结合，避免模糊或光照变化的影响； 

•  由于特征点较少，并在后端加入地面平面约束和LiDAR Bundle Adjustment（BA），因此具有轻量级的前端；

• 基于强度的回环闭合检测和位姿图优化。

主要内容

本文提出的方法的流程如图2所示，在我们的系统中，LiDAR会在100毫秒内生成一个点云，称为帧或扫描，为了估计机器人的运动，我们需要计算相邻帧之间的相对姿态，在前端使用强度里程计来实现这个过程，此外，使用术语“里程计”来描述当前帧和初始帧之间的相对姿态，前端的里程计通常不准确，因此我们需要使用后端来优化里程计，在后端使用扫描到地图的优化方法和LiDAR BA 来校正漂移，然而，地图优化通常不能完全消除积累的漂移，因此我们增加了回环闭合检测作为另一种降低漂移的手段，在我们的情况下，在LiDAR强度图像上执行回环闭合检测，并使用姿态图优化来更新轨迹并生成机器人的最终轨迹。

图2：所提出方法的系统概述，整个系统由三部分组成，包括强度里程计，地图优化和姿态图优化，强度里程计部分是该方法的核心，它由强度图像生成，特征跟踪和扫描配准组成，地图优化通过共同最小化LiDAR BA残差和点到地图平面残差来纠正漂移，位姿图优化通过添加回环约束来纠正整个轨迹。

A. 强度里程计

假设有来自激光雷达的两个连续点云帧X和Y，直接估计相对位姿的一种方法是将迭代最近点（ICP）算法直接应用于计算旋转矩阵R和T：

然而，这种方法通常需要消耗大量的时间和计算资源 ，为了降低计算成本，我们需要提取用于扫描数据的配准的代表性点，从而减少用于优化的点的数量，为了减少用于优化的点的数量，Zhang等人尝试提取边缘和平面特征，当前帧的边缘特征点可以与地图中的边缘匹配，平面特征也是如此。通过边缘和平面特征，可以联合优化点到线的距离和点到平面的距离，并估计 R 和 T，然而，在一些场景中，例如长廊或洞穴环境中，我们无法提取足够的边缘特征，在这种情况下，我们将失去估计6自由度位姿的能力。

为了解决这个问题，我们直接从强度图像中提取和跟踪特征，图1a显示了从Ouster-64 LiDAR生成的强度图像，图像分辨率为1024×64，即使垂直分辨率较低，我们仍然可以提取足够的特征（红色和绿色圆圈是ORB特征），用于估计运动，根据强度图像上匹配特征点的索引直接从点云中提取3D点Y2 ，每个3D特征点都被分配一个得分S，这个得分是在特征提取期间获得的，同样，我们可以提取以下扫描中对应的点X2，并将其作为最小二乘估计问题进行扫描匹配:

B. 地图优化

LiDAR 强度测距法生成了一个转换矩阵，该矩阵描述了相邻帧之间的相对姿态：

在当前传感器坐标系和地图坐标系之间，我们生成了一个变换矩阵，在这个模块中，我们共同优化扫描到地图的残差和激光雷达BA（Bundle Adjustment）残差来校正姿态漂移。

激光雷达BA：类似于视觉SLAM BA，可以使用激光雷达BA（一种非线性优化问题）来校正漂移，使用这种策略，最后k帧用于残差函数中，如果帧数大于k，则删除最旧的帧，并将最新的帧添加到滑动窗口中，如图3所示。

图3：用于LiDAR BA（顶部）和带有闭环约束的姿态图（底部）的滑动窗口策略的说明

C. 位姿图优化 

在地图优化期间，可以获得更好的当前帧姿态估计，一旦完成，使用优化结果来纠正未来帧的漂移，并实时发布高频率优化的里程计，在后端，基于LiDAR关键帧构建位姿图，首先使用三个标准从整个LiDAR帧中提取关键帧： 

• •当前帧和上一个关键帧之间的距离大于一个阈值。 

• •两个关键帧之间的角度大于一个阈值。 

• 匹配特征点的数量小于一个阈值。 

将关键帧的优化姿态作为姿态图的顶点，将两个关键帧之间的相对姿态作为姿态图的边，还向姿态图添加回环约束，如图3所示，我们将最新的关键帧用作锚点帧，使用训练好的词汇表，可以将当前关键帧的描述子与存储历史描述子的数据库进行比较，如果无法将其与历史描述子匹配，则没有找到此关键帧的回环，如果我们成功匹配了以前的关键帧，则可以将其放入异常值剔除程序中，以测试是否存在错误的回环，如果是正确的，则可以在姿态图中的当前因子节点和回环候选因子节点之间添加此回环约束，最后，我们使用g2o 来解决位姿图优化问题。

实验

为了证明我们算法的可靠性，我们在长走廊的室内环境、多层室内环境、山区和街道环境中进行了实验（见图9b），我们选择这些环境的原因是它们彼此不同，对于纯激光雷达SLAM来说都是具有挑战性的，在室内环境中，有长走廊和狭窄的通道，在山区环境中，我们有陡峭的坡和狭窄的通道，在街道环境中，有许多障碍和许多转弯，这些情况对于纯激光雷达SLAM来说都很困难。

在这个实验中，将我们的方法与另外两个流行的纯激光雷达SLAM系统LeGO-LOAM和A-LOAM进行了比较，我们的基于强度的SLAM系统在不同环境下都表现出竞争力，包括室内、室外和一些极端场景，比如长走廊，大多数其他激光雷达SLAM系统在这样极端的、边缘特征更少的环境中都会失败。我们首先使用Shan等人提供的一个公共数据集进行了测试，该数据集使用Ouster OS1-128 LiDAR收集，我们将LIO-SAM的轨迹视为地面真值，因为它是基于激光雷达、9轴IMU和GPU估算的，比仅使用激光雷达的SLAM精度更高。

图4：多种环境下的轨迹结果。实验结果证明我们的方法能够在各种情况下精确估计位置，LeGO-LOAM算法在平坦环境中表现良好，但在有坡度的环境中表现不佳。

在图4中，我们展示了我们的方法、LeGO-LOAM和A-LOAM在各种地形中的轨迹，而图5则显示了图4相应的绝对位置误差（APE）。

图5：在多个环境中的绝对位姿误差，我们方法的APE在所有场景中都是最小的，其次是A-LOAM。

从图5可以看出，我们的方法的APE显著低于其他方法，除了图5a的情况，我们的结果与A-LOAM相比不具有显著优势，图4和图5的结果证明了我们提出的方法在与A-LOAM和LeGO-LOAM相比具有竞争力的结果，特别是在图4d中，轨迹在最后一段接近地面真值轨迹，而其他方法漂移很大。

我们还使用装备有Ouster Os0-64激光雷达的Spot机器人在室内进行了算法测试，这个实验场景主要包括与图9b相同的长廊，在这个场景中，我们运行不同的算法来测试在实际环境中的定位和地图构建能力。

图6：在一个拥有长走廊的建筑物内的Spot机器人的地图和轨迹。在这个场景中，沿着走廊走回起点，在这个实验中，LeGO-LOAM的漂移相对较大，而A-LOAM的表现接近于我们的方法，但是从这两个图中无法确定哪种方法更好。

图7：这次实验我们不是围绕圆圈回到起点，而是来回穿过所有的走廊，然后返回起点。这种情况是极端的，因为在俯仰方向上没有好的回环来纠正漂移，实验结果显示，LeGO-LOAM的漂移比前一个场景更大，A-LOAM和我们的方法几乎相同。

图6a和图7a都是我们的算法生成的地图，图6b和图7b展示了不同算法在相应环境中的轨迹，从轨迹可以看出，LeGO-LOAM漂移很大，A-LOAM和我们的轨迹几乎相同，由于是室内环境，我们很难使用RTK收集地面真实轨迹，因此我们尝试通过分析地图细节（图8）来评估算法的优点和缺点。

图8：我们的方法和A-LOAM在长走廊中的差异。可以看出，我们的方法生成的地图与起始位置平滑连接，而A-LOAM生成的地图在末尾有明显的断裂

图8a展示了图6a的前视图，图8b也是A-LOAM生成的同一场景的前视图，我们可以看到，我们的方法可以平滑地连接行程结束时的起点，而A-LOAM的地图则是不连续的，在这一点上，我们可以说我们的方法在这种极端环境下更加可靠。此外，我们分析了在使用Os0-64 LiDAR采集的相同数据上，在Intel处理器上运行不同SLAM算法的时间消耗，表I显示，我们基于强度的前端能够在15毫秒内计算里程计，我们的方法足够高效以满足10 Hz激光雷达的实时要求。

总结

在本文中，我们提出了一种新颖的基于强度的纯LiDAR SLAM方法，我们首先提出了一种新颖的轻量级基于强度的里程计方法，该方法直接匹配从强度图像中提取的3D特征点，然后我们提出了一种新颖的地图优化方法，它联合优化了LiDAR BA和点对平面残差，最后，我们提出了一种新颖的基于强度图像的位姿图优化方法，可以基于强度图像优化位姿图，在室内和室外环境中测试了我们的方法，结果证明，与其他流行的纯LiDAR SLAM方法相比，我们的方法可以取得竞争性的结果。在未来，我们将通过使用更先进的特征提取方法和回环闭合检测方法进一步改进我们的方法。

这篇文章的代码实现可以参考 基于成像激光雷达的鲁棒位置识别

文章：Robust Place Recognition using an Imaging Lidar

作者：Tixiao Shan, Brendan Englot, Fabio Duarte, Carlo Ratti, and Daniela Rus

开源代码：https://github.com/TixiaoShan/imaging_lidar_place_recognition

更多详细内容请加入智驾全栈与3D视觉学习星球

智驾全栈与3D视觉学习星球:主要针对智能驾驶全栈相关技术，3D/2D视觉技术学习分享的知识星球，将持续进行干货技术分享，知识点总结，代码解惑，最新paper分享，解疑答惑等等。星球邀请各个领域有持续分享能力的大佬加入我们，对入门者进行技术指导，对提问者知无不答。同时，星球将联合各知名企业发布自动驾驶，机器视觉等相关招聘信息和内推机会，创造一个在学习和就业上能够相互分享，互帮互助的技术人才聚集群。

以上内容如有错误请留言评论，欢迎指正交流。如有侵权，请联系删除

扫描二维码

                   关注我们

让我们一起分享一起学习吧！期待有想法，乐于分享的小伙伴加入知识星球注入爱分享的新鲜活力。分享的主题包含但不限于三维视觉，点云，高精地图，自动驾驶，以及机器人等相关的领域。

分享及合作方式：微信“920177957”（需要按要求备注） 联系邮箱：[e-mail]，欢迎企业来联系公众号展开合作。

点一下“在看”你会更好看耶