在资源受限平台上运行FAST-LIVO2

文章：FAST-LIVO2 on Resource-Constrained Platforms: LiDAR-Inertial-Visual Odometry with Efficient Memory and Computation

作者：Bingyang Zhou, Chunran Zheng, Ziming Wang, Fangcheng Zhu, Yixi Caiand Fu Zhang

编辑：点云PCL

欢迎各位加入知识星球，获取PDF论文，欢迎转发朋友圈。文章仅做学术分享，如有侵权联系删文。

公众号致力于点云处理，SLAM，三维视觉，高精地图等领域相关内容的干货分享，欢迎各位加入，有兴趣的可联系dianyunpcl@163.com。文章未申请原创，未经过本人允许请勿转载，有意转载联系微信920177957。

摘要

本文提出了一种针对资源受限平台优化的轻量级激光雷达-惯性-视觉里程计系统。该系统将一个关注退化的自适应视觉帧选择器集成到误差状态迭代卡尔曼滤波器（ESIKF）中，并采用顺序更新方法，显著提高了计算效率，同时保持了类似的鲁棒性。此外结合局部统一的视觉-激光雷达地图和长期视觉地图的内存高效映射结构，在性能和内存使用之间实现了良好的折衷。在x86和ARM平台上的大量实验表明该系统的鲁棒性和效率。在Hilti数据集上，与FAST-LIVO2相比，我们的系统每帧运行时间减少了33%，内存使用降低了47%，RMSE仅增加了3厘米。尽管存在轻微的精度折衷，我们的系统仍具有竞争力，超越了如FAST-LIO2等最先进的LIO方法以及大多数现有的LIVO系统。这些结果验证了该系统在资源受限的边缘计算平台上可扩展部署的能力。

主要贡献

随着机器人应用扩展到轻量化和便携式平台（例如无人机、自动驾驶车辆和物联网设备），在边缘计算平台上部署里程计变得越来越重要。边缘平台，如ARM架构，具有能源效率高和广泛应用于嵌入式系统等优点，使其成为可扩展且具有成本效益的解决方案。然而由于计算能力、内存和能源效率的限制，传统的里程计框架在确保资源高效性能方面面临着重大挑战。尽管如此，边缘计算为实时里程计带来了独特的好处，如通过本地数据处理减少延迟和网络依赖。为了充分利用这些优势，我们改进了LIVO系统的现有状态，即FAST-LIVO2，通过减少其计算和内存开销，同时保持其鲁棒性和性能。与FAST-LIVO2相比，本文工作的主要贡献如下：

• 激光雷达退化感知的自适应视觉更新：我们没有直接使用所有可用的视觉测量数据，而是将一个激光雷达退化感知的自适应视觉帧选择器集成到FAST-LIVO2的ESIKF框架中，显著减少了计算负担，同时保持了类似的里程计精度。
• 内存高效的混合地图结构：我们提出了一种混合地图结构，结合了紧凑的局部视觉-激光雷达地图和长期视觉地图。后者存储稀疏的历史图像观测，以确保里程计的鲁棒性，而前者则保持在显著更小的规模，从而减少内存开销。此外，当激光雷达约束足够时，自适应视觉更新最小化了添加到地图中的视觉特征，进一步降低了内存使用。
• 广泛的实验验证和数据集开源：验证在公共和私人数据集上进行，其中私人数据集包含更具挑战性和激进的场景。这些私人数据集将被开源，惠及社区。实验在x86个人笔记本和成本效益高的ARM设备上进行，展示了优越的效率。

图1. 低功耗ARM平台上的实时实验概述。 (a) 用于测试的ARM平台RK3588，具有八核架构（4× Cortex-A76 + 4× Cortex-A55），最大频率为2.4GHz，价格约为100美元。 (b) 用于测试的夜间街道场景的点云，橙色轨迹表示收集的路径。 (c) 系统的每帧运行时间详细统计数据，数据输入来自激光雷达和相机，频率为10 Hz（每帧100毫秒）。

主要内容

系统概述如图2所示。在观测层面，作为该系统的输入，高频原始激光雷达点云通过扫描重组被分割成不同的扫描。为了优化计算效率，自适应视觉帧选择器根据环境约束动态选择图像，使我们的退化感知里程计能够自适应地分配计算资源。对于状态估计，作者构建了特定的残差，包括激光雷达点到平面的残差和光度误差，并通过带有顺序更新的ESIKF紧密耦合所有传感器测量值。在建图层面，通过对新观测数据的高效集成、滑动操作和视觉特征重排，维持了一个以机器人为中心的紧凑统一局部地图和一个轻量级的长期视觉地图，从而确保了内存效率和鲁棒的性能。

图2. 系统概述。在右下方详细展示体素数据的图中，虚线椭圆圈定了被认为位于平面上的点，且这些点具有不同的尺度。

方法论

之前的工作 FAST-LIVO2建立了多传感器（激光雷达-惯性-视觉）集成的核心框架，包括状态定义、离散过渡模型、激光雷达-视觉地图管理和通过融合相机与激光雷达测量进行状态估计。在此基础上，本工作聚焦于构建一个在资源受限平台上部署的轻量级LIVO系统。因此，本文强调了在实现计算高效的状态估计和节省内存的地图方面的贡献。

1

使用视觉帧选择器的状态估

首先介绍激光雷达退化评估方法，然后介绍自适应视觉帧选择器，使得基于ESIKF的状态估计更加高效，如图3所示。

图 3. 具有自适应视觉帧选择器的顺序更新 ESIKF 说明

激光雷达退化评估：首先采用了一种方法来评估激光雷达数据的退化情况。对于每一次激光雷达扫描，系统会将获得的点云数据进行处理，假设这些点位于某一平面上。然后通过计算这些点与其理想平面之间的差异，评估激光雷达数据是否出现退化。一般来说，激光雷达数据的退化通常表现为环境的平移自由度（例如位置）受限，而旋转自由度可能出现退化，这通常发生在特殊的环境中，如理想的球形结构或类似的场景。为了判断退化是否发生，我们关注的是平移约束的稳定性。如果平移的约束在连续的几帧中未能提供足够的稳定性，就会判断为激光雷达处于退化状态。这种评估机制帮助系统及时发现激光雷达数据的质量问题，从而调整后续的处理策略。

自适应视觉帧选择器：基于激光雷达的退化状态，我们设计了一个自适应的视觉帧选择器，用来优化视觉信息的利用。在激光雷达数据退化的情况下，例如当激光雷达视野受限时，系统会使用所有可用的视觉图像来尽可能地构建约束，避免定位失败。而在正常情况下，系统仅选择稀疏分布的关键帧，以较低的频率更新状态和地图中的视觉相关信息，从而减少计算开销。关键帧是指当传感器的位置或方向发生显著变化时捕获的图像，这些变化超过了预设的阈值。当系统检测到激光雷达退化时，阈值会进行动态调整，以确保选取的图像能够提供足够的定位约束。通过这种方式，系统能够根据当前的环境和传感器状态灵活地调整图像的选择策略，从而在保证精度的同时，降低计算和内存消耗。

2

地图管理

地图结构

整个地图分为统一的局部地图和长期视觉地图。如图2右侧所示，地图由适应性体素组成，并由一个哈希表进行管理。具体来说，这里的哈希值是根体素中心的位置的全局坐标索引，便于快速检索。根体素的固定大小设置为 0.5 × 0.5 × 0.5 米。

统一局部地图 包含在不同尺度下的稠密点和平面特征（即平面中心、法向量及其相关不确定性），其采用三层八叉树结构进行组织。一些被选为视觉点的点附带了图像补丁的三层金字塔，用于视觉状态估计，而其余的点仅保留几何信息，用于激光雷达扫描与地图匹配以及后续的激光雷达状态估计。

长期视觉地图 是一个历史点的集合，包含用于视觉帧与地图匹配的视觉观测。这张地图在空间上相对稀疏，能够以相对较小的内存消耗存储长期的环境观测数据，同时为长期定位提供重要的历史数据。

地图更新与滑动

将新观测数据和估计的平面特征注册到统一局部地图的过程遵循[6, 26]中概述的方法。如图4所示，不同于FAST-LIVO2中仅当传感器的范围到达地图边界时才移动大尺寸局部地图，本方法采用了较小尺寸的统一局部地图，并采用了更频繁的地图滑动策略。

图4. 我们系统和FAST-LIVO2中地图结构与滑动过程的示意图

具体而言，当机器人的位移超过自上次地图滑动以来的预设阈值时，平面特征和超出局部地图边界的点云会被清除，以保持高效性。那些带有视觉观测的点，如果位于小尺寸统一局部地图的外部，将被转移到长期视觉地图。长期视觉地图采用类似的滑动机制，但作用于更大范围的地图，以处理更广泛的环境信息。

实验

实验设置

公共数据集：使用Hilti数据集对该方法进行评估，该数据集包括在建筑工地、办公室和地下室等挑战性环境中捕获的室内和室外序列。这些数据集包括手持和机器人安装配置，配备激光雷达、摄像头和惯性测量单元（IMU），且它们的工作频率不同。

私有数据集：使用配备鱼眼镜头的B/W相机和Livox Mid-360激光雷达来验证系统的鲁棒性，通过STM32微控制器进行同步。在极暗的环境中，使用15W的机载照明设备来增强可见度。

实验设置与配置：实验在两种硬件平台上进行，为简便起见，分别称为x86平台和ARM平台。x86平台是一台配备第13代Intel Core i9-13900HX CPU的个人笔记本电脑。ARM平台约为100美元，具有成本效益，配备RK3588处理器，采用八核架构（4×Cortex-A76 + 4×Cortex-A55），最大频率为2.4GHz。在所有测试中，使用了统一的退化检测阈值0.07。统一局部地图和长期视觉地图的典型边长分别为200米和800米，在所有序列中保持一致，地图滑动阈值分别为机器人运动的20米和100米。此外关键帧选择阈值根据场景规模进行了调整，室内序列为1米-60°，室外序列为2米-60°。在我们的私有数据集中，采用等距投影模型来处理鱼眼相机图像。其他参数，如不同激光雷达模型的噪声设置，继承自FAST-LIVO2，并经过精心调优以确保鲁棒性。

图 5. 我们的平台采用硬件同步进行数据采集。

系统在公共数据集上的性能

公共数据集上的准确性：在这个实验中，首先验证了我们的方法在Hilti’22和Hilti’23数据集的16个序列上的准确性，并将其与最先进的系统进行对比，包括R3LIVE、FAST-LIO2、SDV-LOAM、LVI-SAM以及之前的工作FAST-LIVO系列。努力调整所有方法的参数，以实现最佳性能进行对比。使用Hilti官方评估工具，RMSE（均方根误差）结果如表I所示。从实验结果来看，我们的系统在准确度上排名第二，平均RMSE为6.3厘米，仅次于FAST-LIVO2的3.4厘米，同时超越了最先进的仅基于LiDAR惯性的方法FAST-LIO2。视觉模块的集成证明是有益的，它提升了定位精度。

具体而言，在视觉纹理有限的序列中（例如，长走廊和楼梯），当LiDAR约束稳健时，该系统通过选择性减少视觉使用，超越了FAST-LIVO2。然而在视觉挑战较大的场景（例如，曝光过度的建筑外部或光线较差的大房间）中，系统相比FAST-LIO2略微降低了精度。在Hilti数据集上，SDV-LOAM表现出明显的局限性，主要是由于缺乏IMU的紧密集成，导致其LiDAR里程计子系统发生显著漂移。此外系统中LiDAR与视觉测量的松散耦合，以及视觉里程计的初始化不足，常常导致收敛问题，如局部最优或优化失败。LVI-SAM在九个序列中出现了失败，因为其基于特征的LIO和VIO子系统未能充分利用原始测量数据，使其在几何或纹理特征较为微弱的环境中表现不佳（例如，纹理极简或几何简单的场景）。虽然R3LIVE通常能取得较强的结果，但在涉及强烈旋转和稀疏结构信息的序列中，其性能有所下降（例如，建筑楼梯、圆顶和阁楼到上层画廊）。在这些场景中，地图对齐时的姿态先验不足导致收敛到次优解，最终导致优化失败。

公共数据集上的计算效率：在Hilti数据集的16个序列上评估了算法在x86和ARM平台上的运行时间，结果汇总在表II中。总体而言，我们的系统在计算效率上显著优于FAST-LIVO2。这一改进主要归因于视觉模块中的自适应帧率策略，该策略显著减少了视觉处理的运行时间。此外，由于代码实现中的优化，LiDAR模块的运行时间略有增加。

公共数据集上的内存消耗：作者进一步评估了该方法在公共数据集上的计算效率和内存消耗。由于FAST-LIVO2在所有比较方法中显示出最高的准确性，如表I所示，将重点对比FAST-LIVO2与我们提出的系统，以突出在资源效率方面的改进。在x86平台上测量了验证过的Hilti序列的内存使用情况，结果汇总在表III中。可以观察到，与FAST-LIVO2相比，我们的系统显著减少了内存消耗。由于算法的内存消耗主要归因于地图存储，因此在小规模场景中，内存的减少主要归功于自适应图像帧选择器，该选择器有效地减少了地图中的冗余视觉观测。在大规模场景中，内存减少主要归因于我们设计的地图结构，该结构利用较小尺寸的局部统一地图来高效管理存储。

系统在私有数据集上的性能

我们使用私有数据集对系统的鲁棒性和计算的实时性能进行了定性测试。

复杂私有序列上的实验： 在多个具有挑战性的私有序列上进行了测试，这些序列涵盖了各种SLAM挑战，如矿井隧道中的动态光照条件、黑暗森林中的极低照度、野外公园中的非结构化环境以及HIT涂鸦墙中的严重LiDAR退化，如图6所示。此外，这些序列包括了不同时间拍摄的室内和室外场景，还包括关灯的室内场景（例如HW走廊）。实验结果表明，我们的系统表现出了极强的鲁棒性，能够生成清晰的点云地图，甚至可以生成生动的彩色点云。在所有数据采集设备物理回到起始位置的序列中，我们的系统实现了低于2 cm的回归漂移误差。与此同时，图6中的百分比表示所选图像相对于所有可用图像的比例，反映了系统对LiDAR环境约束的自适应性。例如，在长隧道状的走廊（如矿井隧道和HW走廊的中段）中，由于LiDAR约束较弱，系统选择了更密集的视觉帧；相反，在香港科技大学红色雕塑等LiDAR约束较强的环境中，系统的视觉帧选择则更为稀疏。

图 6. 代表性测试序列的可视化。* 表示检测到 LiDAR 退化的时刻。每个序列名称旁边的百分比表示该序列中所选图像占所有可用图像的比例。

资源受限的ARM平台上的实时定位实验： 如图7所示，我们在夜间的繁忙街道（有行人）以及地下停车场等场景进行了实时定位测试，这些场景包含了显著的室内外光照变化。所有计算均在ARM板载平台上完成。实验结果表明，系统能够实现稳定的实时定位，每帧计算时间仅为37ms。

图 7. 在 RK3588 平台上的实时定位实验。（a）地下停车场和（b）夜间街道。对于每种场景，左侧图像显示轨迹和点云，右侧图像显示特定时刻的第三人称视角。

自适应图像帧选择器的评估

计算效率优化评估： 从图6中的可视化结果可以观察到，在不同的序列中，根据环境中LiDAR退化的情况，系统从所有可用的视觉测量数据中选择了不同比例的图像。我们对这些序列中的LiDAR和视觉模块的计算时间进行了详细分析，并与FAST-LIVO2进行了对比。如图8所示，在所有情况下，视觉模块的计算时间都显著减少，特别是在LiDAR约束较强的序列中，视觉模块的计算开销极低。

图 8. 我们的方法与 FAST-LIVO2 在七个场景中的 LiDAR 和视觉处理时间。LiDAR 处理时间作为基准，视觉处理时间叠加在其上方。

LiDAR退化检测模块的评估： 如图9所示，我们在两个具有极高LiDAR退化程度的序列上详细分析了LiDAR退化检测模块的有效性。实验结果表明，该算法在识别退化方面具有很高的敏感性，例如在t1时刻，Aivia LiDAR正对墙面，以及t3时刻，Mid360 LiDAR位于长直隧道时，均表现出极低的特征值。此外，分析结果证明，该系统能够在不同类型的LiDAR上使用通用的退化检测阈值，进一步验证了LiDAR退化评估模块的有效性。

图 9. LiDAR 退化评估的验证。图表显示了最小归一化奇异值，当该值低于阈值（虚线）时，即检测到退化（红色阴影区域）。上方的可视化图对应于低（t1, t3）和高（t2, t4）奇异值的时刻，红色点表示当前 LiDAR 扫描结果，右上角图像显示相应的第一人称摄像头视图。

地图结构的评估

通过消融实验评估了所提出的长期视觉地图对定位精度和内存使用的影响，实验基于MARS-LVIG数据集中的大规模序列HKIsland03。

精度对比： 我们使用RTK轨迹作为真实值，对比评估了定位精度，并使用evo工具包计算了RMSE误差。在该序列中，长期视觉地图提高了定位精度，使RMSE从0.85m降低至0.39m，如图10中的详细点云可视化结果所示。

图 10. 我们提出的长期视觉地图结构的消融实验结果，以重建的彩色点云形式展示。（a）使用长期视觉地图。（b）未使用长期视觉地图。

内存消耗对比： 如图11所示，虽然长期视觉地图会引入一定的额外内存开销，但与显著提升的定位精度相比，这一内存开销相对较小。因此，这种权衡是合理的，增强了系统的鲁棒性和精度，证明该地图结构适用于大规模应用场景。与此同时，FAST-LIVO2的本地统一地图的边界长度设为2km（与原论文一致）。在场景被完整包含在地图范围内的情况下，FAST-LIVO2的内存使用量会持续增长，而本研究的地图结构能更高效地管理存储资源。

图 11. 在 MaRS-LVIG 数据集的 HKIsland03 序列中，不同算法的内存使用对比。

总结

本研究提出了一种轻量级的LIVO（LiDAR-视觉-惯性里程计）系统，专为资源受限的平台进行优化。通过集成基于退化感知的自适应视觉帧选择器和高效的地图存储结构，系统在保持高精度里程计性能的同时，显著降低了计算和内存消耗。大量实验验证了系统的鲁棒性，并证明其能够在资源受限的边缘设备上高效运行，实现在复杂环境中的实时运行。