LiLi-OM: 走向高性能固态激光雷达惯性里程计和建图系统

点击上方“计算机视觉工坊”，选择“星标”

干货第一时间送达

编辑丨当SLAM遇见小王同学

声明: 本文只是个人学习记录，侵权可删。论文版权与著作权等全归原作者所有，小王自觉遵守《中华人民共和国著作权法》与《伯尔尼公约》等国内外相关法律，其他个人或者组织等转载请保留此声明！！

“LiLi-OM提出了一种新的固态和机械激光雷达的紧耦合激光雷达-惯性里程计和建图方案。其前端是，基于特征的轻量级激光雷达里程计为自适应关键帧选择提供快速运动估计。后端是，通过边缘化执行基于关键帧的分层滑动窗口优化方法，用于直接融合IMU和激光雷达测量数据。”

1

介绍

从本质上来说，基于激光雷达的里程计需要计算给定连续点云帧的六自由度运动。这可以通过扫描匹配算法来执行，例如，通过迭代最近点(ICP)方法。代替直接使用下采样的原始点云，提取表示局部几何的特征用于扫描匹配。在以前工作中，提取平面补片以执行具有点到平面度量的ICP。这也得到进一步推广，其中提出了一个平面到平面的度量来提高鲁棒性。

最近，固态激光雷达进入了消费市场。现有的固态激光雷达通常具有不重复和不规则的扫描模式。此外，它们的视场相对较小，以达到均匀和高分辨率。到目前为止，基于固态激光雷达的里程计还没有得到很好的研究。在最近工作中，传统的激光雷达里程计系统可以适用于Livox Mid-404，这是一种圆形视场为38.4的固态激光雷达。这里特征提取和扫描匹配相似。而且它产生了较好的跟踪精度，并由于并行化而提高了运行效率。

本文对固态激光雷达惯性里程计和建图进行了具体研究。作者选择了2020年第一季度新发布的Livox Horizion，而不是Livox 40-Mid。它是为移动感知而设计的，在Livox Horizon的自主车上，FoV为81.7°×25.1°。以10赫兹扫描，它达到了与典型的64线机械激光雷达相似的覆盖范围，密度更加均匀。此外，它比大多数性能相当的现有3D激光雷达便宜得多。

固态激光雷达的扫描模式总是不规则和不重复的，因此常规三维激光雷达的常见特征提取方法是不适用。例如如果沿入射角和偏转角提取特征对于Livox Horizon来说是不可能的，因为它以完全非结构化和不规则的模式扫描。这种模式更适用于覆盖范围更广的均匀视场，这可能会成为下一代固态激光雷达的常见模式。然而，与传统的3D激光雷达相比，固态激光雷达的视场有限，使得在某些情况下执行里程计变得困难，特别是在快速运动或特征不足的情况下。

这里附上视频：

开源代码：

https://github.com/KIT-ISAS/lili-om

总之

考虑到上述最新研究和3D激光雷达的发展，该项工作贡献如下：

• 提出了一种新的紧密耦合的激光雷达-惯性里程计和建图方案

• 提出一种新颖的特征提取方法适用于Livox  Horizon的不规则扫描模式。

• 为了以统一的方式直接融合激光雷达和IMU的测量结果，提出了一种基于关键帧的分层融合方案。

• 所提出的系统一般适用于常规和部署的固态激光雷达。它实时运行和能提供优于最先进系统

• 发布了Livox Horizon和Xsens MTi-670记录的新固态激光雷达惯性数据集。该系统的代码是LiLi-OM下的开源代码。由于移动平台上的低硬件成本和实时性能，它为各种场景中的机器人感知提供了一个经济高效的解决方案。

2

系统概述

上图显示了系统结构。3D激光雷达(如Livox Horizon)以10Hz的频率输出点云，并进行同步六轴IMU以提供更高频率的陀螺仪和加速度计读数。作者希望同时估计激光雷达帧的六自由度自运动和全局一致的地图。来自激光雷达扫描的原始点云首先使用陀螺仪数据进行下采样和旋转去除偏斜。然后，提取代表平面(红色)和边缘(绿色)的特征点.在给定预处理扫描的情况下，基于轻量级扫描匹配的里程计模块以帧到模型的方式运行，以利用点到边缘和点到平面的度量进行快速运动估计。而连续激光雷达帧的相对运动估计进一步用于消除当前激光雷达要素扫描的平移部分。此外，给定估计的相对运动，自适应地选择关键帧以适应感知的场景过渡。与预处理和激光雷达里程计节点并行，后端模块通过基于关键帧的滑动窗口优化，以统一的方式直接融合激光雷达和惯性测量单元测量值。融合窗口通常覆盖几个关键帧。随着窗口的滑动，关键帧状态在当前窗口中被优化。

3

基于特征的固态激光雷达扫描匹配

 1   不规则扫描模式的特征提取

现有的3D固态激光雷达通常具有不重复和不规则的扫描模式。因此，提取几何特征的流行方法是不适用的。

Livox地平线的3D分辨率比具有重复扫描模式的传统激光雷达更均匀。为了从Livox地平线的点云中提取平面和边缘特征，我们提出了一种新的两阶段方法。我们在其时域中展开每帧扫描，其中6 × 7的patch被一个接一个地分配。在每个patch中，我们对点的3D坐标的协方差执行特征分解。如果第二大特征值基本上大于最小特征值(例如，λ1/λ2< 0.3)，则块中的点被提取为表面特征。对于非平面patch，我们在每条扫描线上搜索曲率最大的点，并对这六个点进行特征分解。如果最大特征值显著大于第二大特征值(例如，λ2/λ3< 0.25)，则这些点形成一条线，并且它们被提取为边缘特征。

我们在图4中示出了从Livox地平线点云的一帧中提取特征的例子。注意，所提出的特征提取算法纯粹是在感知点阵列的每个扫描给定时间戳的时域中执行的。此外，我们将每个特征点与其对应的边的方向向量或平面的法向量相关联，以表示局部几何形状。它将被进一步用于对后端融合模块中的激光雷达残差进行加权。

 2   点到边缘距离

如果特征点 表示一个平面，对应的方向向量为 ,并在相应的局部特征图中找到最近的五个边缘特征点,并计算质心 与协方差矩阵。如果分解后得到的最大特征值显著大于第二大特征值，则取出其特征向量作为该边缘特征的方向向量 .然后，通过 和 拟合该直线，最后得到计算 到该直线的距离公式：

 3   点到平面距离

对于第l帧雷达提取到的平面特征，对应的法向向量为 ，并在局部特征地图中寻找最邻近的五个平面特征点，然后RQ分解求解超定方程 求得法向量 ，单位法向量记为 ，则点到平面的距离为：

 4   距离加权函数

为了量化传感器融合期间每个激光雷达残差的贡献，我们根据关联质量提出度量加权函数如下

4

基于关键帧滑动窗口优化的激光雷达-惯性强耦合融合

 1   基于关键帧的融合层次

最初提出了基于关键帧的方案，并广泛应用于视觉里程计，以实现具有实时性能的精确跟踪。然而，实时性能通常无法实现，因为该方案融合了每帧的激光雷达扫描数据。

因此，在后端保持激光雷达-惯性直接融合优化方案的稀疏性非常重要。如图5所示，所提出的融合方案利用关键帧来建立滑动窗口，其中激光雷达和预集成惯性测量单元测量值通过非线性优化以统一的方式进行融合。由于在两个连续关键帧的时间间隔内惯性测量单元的漂移，设置关键帧会严重影响里程计的精度。我们引入两个关键帧选择标准:(1)如果当前帧的特征和局部特征映射之间的重叠比率小于60%或者(2)如果到最后一个关键帧的时间差大于一定数量(例如，两个)的常规帧，则当前帧被选择作为新的关键帧。随着窗口在优化后向前滑动，我们构建了一个局部因子图，其中包含两个最早的关键帧姿态作为约束，以及由惯性测量单元测量初始化的常规帧姿态。调用小尺度因子图优化来获得激光雷达采样频率下的规则帧姿态。

 2   关键帧的滑动窗口优化

我们通过解决一个非线性优化问题来计算形式(1)的关键帧状态，该问题结合了激光雷达特征和预集成惯性测量单元测量，所有这些都在滑动窗口中的关键帧处观察到。我们使用包含三个分量的最大后验概率目标函数，即

为了在不损失大量信息的情况下减轻激光雷达-惯性融合的计算负担，我们利用了基于关键帧的窗口中的边缘化。这里，最老的关键帧及其测量值通过舒尔补码被边缘化。相应地计算新的先验，并将其添加到现有先验因子之上，以将已移除关键帧的估计值传送到下一个窗口。

 3   激光雷达残差

建立以下剩余项

 4   惯性测量单元测量残差

作者使用空间方向的四元数表示来计算IMU剩余项。为了避免每次优化窗口滑动时重复传播IMU状态，原始惯性读数会在两个连续关键帧k和k+ 1之间预先积分。在此基础上，在关键帧处建立包含惯性测量单元约束的残差项，即

贡献了连续关键帧k和k+1之间的时间间隔。我们使用运算符[]2:4来取出四元数向量的最后三个元素。 是IMU测量值，包含从关键帧k到k+1的陀螺仪和加速度计读数。由于空间限制，我们不提供(3)中针对Mahalanobis范数的IMU预积分和相应噪声协方差 的推导。

基于(3)中提出的目标函数，当前滑动窗口中的关键帧状态可以由x*获得这可以通过典型的非线性最小二乘问题求解器来解决。

5

评估

使用ROS来完成该项任务。图5中所示的三个模块被构造为三个单独的节点。(3)中的非线性优化问题使用Ceres求解器求解。使用GTSAM来执行因子图优化，以纠正循环结束时的全局姿态图。系统是以Livox为重点开发的，因此被命名为LiLi-OM。注意它也适用于带有机械旋转机构的传统激光雷达。因此，作者部署了两个版本的系统进行评估:(1)最初的LiliI-OM用于Livox Horizon(有特征提取方法）(2)使用预处理模块为常规激光雷达提取特征。

现有的公共激光雷达惯性数据集都是使用传统的机械旋转激光雷达记录的。为了进行比较，作者使用以下系统:A-LOAM、Lego-LOAM、LIO-Mapping、LINS和LIOM-SAM。

数据集使用UTBM，它提供了两个长城市导航序列。LIO-SAM要求九轴惯性测量单元测量。为此，我们包括UrbanLoco和UrbanNav数据集.

实验表明LiLi-OM对于所有序列都实现了最佳的实时跟踪精度。LIO-SAM要求九轴惯性测量单元读数用于去偏斜和执行前端里程计，因此不适用于UTBM数据集。对于剩余的序列，LIO-SAM仍然显示出比LiLi-OM更差的跟踪精度，尽管它还利用了磁力计的方向测量。这主要源于LiLi-OM的统一融合方案，其中激光雷达和惯性测量直接融合。LIO-Mapping失去了对城市导航卫星数据集的跟踪，并为第一个UTBM序列提供了非常大的漂移。此外，它不能在推荐的配置下实时运行。由于该实现限制了扫描匹配中的迭代次数，以实现实时处理，因此LOAM还会在UTBM数据集上产生较大的跟踪误差。

6

实验

为了进一步测试LiLi-OM在固态激光雷达惯性里程计和现实场景中的建图性能，我们组装了一个由Livox Horizon和Xsens MTi-670 IMU组成的传感器套件。传感器套件的总成本仅为1700欧元，比传统的激光雷达惯性设置便宜得多。

 1   FR-IOSB数据集

如图6-(B)所示，展示了Livox-Xsens套件。为了与高端机械旋转激光雷达进行比较，作者在船上安装了一个Velodyne HDL-64E，并将其与Xsens MTi-G-700惯性测量单元(约150赫兹)同步。机载传感器的详细设置如图6-(C)所示。此外作者在图6-(A)的Fraunhofer IOSB记录了三个序列:(1)FR-IOSB-短记录在具有短轨迹长度的结构化场景中，(2)FR-IOSB-树记录在树和灌木中，以及(3)FR-IOSB-长记录在长轨迹长度中。我们测试了LiLi-OM的两个版本(针对HDL-64E和Horizon)。

为了进行比较，运行了LOAM、Lego-LOAM和Livox-Horizon-LOAM，这是一种适用于Livox Horizon的LOAM版本。请注意，该实现还利用IMU读数来处理激光雷达点云的运动失真。表2显示了这些方法的端到端位置误差。此外，使用64线Velodyne激光雷达和LiLi-OM的两个版本的地图显示了由LOAM给出的重建地图。所提出的融合方案能够使用传统和固态激光雷达获得精确的测绘结果。

 2   KA-Urban 数据集

提出的Livox-Xsens传感器套件进一步部署在背包系统上，在德国卡尔斯鲁厄市记录了五个序列。除了LiLi-OM，我们还停用了其闭环模块，以评估其激光雷达惯性里程计精度，而无需校正全局姿态图。也运行livox-Horizon-LOAM进行比较。

关于在闭环时不进行校正的纯里程计，LiLi-OM比LH-LOAM提供的漂移小得多。当闭环功能被激活时，LiLi-OM显示非常小的漂移。图1从不同角度显示了在序列Schloss-1上运行LiLi-OM的建图结果。在那里，卡尔斯鲁厄市被转化为3D点云，Horizon-Xsens具有非常高的精度。我们还在图8-(B)中显示了在496m×312的区域中对序列Schloss-2运行LiLi-OM的结果。重建的地图是全局一致的，并且与来自卫星图像的基础地图一致。此外，我们还展示了其他的结果，这是在卡尔斯鲁厄东部的街道上长时间骑车时记录的。尽管经历了快速和动态的运动，LiLi-OM使用所提出的低成本传感器套件(如图9所示)提供了精确的里程计结果和一致的地图。

 3   运行时间

对于上述评估中涉及的所有数据集，LiLi-OM可以实时运行。所有评估都在英特尔酷睿i5-7300HQ CPU，8GB RAM配置下完成。预处理节点主要执行特征提取，激光雷达里程计模块提供快速运动估计，用于选择关键帧和消除偏斜。它们被设计得很轻。在后端节点，激光雷达惯性测量通过优化融合在基于关键帧的滑动窗口中。对于不同激光雷达(包括Velodyne HDL-64E)在不同场景下记录的所有序列，所提出的系统显示出实时性能(相对于激光雷达采样频率)。

6

结论

LiLi-OM系统建立了基于关键帧的分层方案，通过滑动窗口优化直接融合激光雷达和(预集成)IMU的测量结果。给定优化的关键帧状态，通过因子图优化获得帧位姿。该系统普遍适用于常规激光雷达和小视场固态激光雷达。对于后一种使用情况，针对Livox Horizon的不规则且独特的扫描模式设计了一种新的特征提取方法，Livox Horizon是一款新发布的自由旋转固态激光雷达，价格远低于传统的3D激光雷达。

作者对传统激光雷达的公共数据集和使用Livox Horizon的实验进行评估。结果表明，该系统具有实时性，比先进的激光雷达/激光雷达惯性里程计系统具有更高的跟踪和建图精度。提出的LiLi-OM系统是一个高性能激光雷达-惯性里程计和使用固态激光雷达绘图的经济有效的解决方案。

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「计算机视觉工坊」公众号后台回复：深度学习，即可下载深度学习算法、3D深度学习、深度学习框架、目标检测、GAN等相关内容近30本pdf书籍。

下载2

在「计算机视觉工坊」公众号后台回复：计算机视觉，即可下载计算机视觉相关17本pdf书籍，包含计算机视觉算法、Python视觉实战、Opencv3.0学习等。

下载3

在「计算机视觉工坊」公众号后台回复：SLAM，即可下载独家SLAM相关视频课程，包含视觉SLAM、激光SLAM精品课程。

重磅！计算机视觉工坊-学习交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有ORB-SLAM系列源码学习、3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、深度估计、学术交流、求职交流等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~