基于多模态路标跟踪的紧耦合激光—视觉—惯导里程计（ICRA2021）

3D视觉融合里程计：高效多传感器融合方法
研究背景
随着自动驾驶和机器人技术的快速发展，多传感器融合在提升定位和建模精度方面发挥重要作用。现有方法在激光雷达与视觉信息融合方面存在不足，尤其是在动态环境或极端条件下表现不佳。
方法概述
作者提出了一种高效多传感器融合算法，结合激光雷达、视觉信息和IMU，构建联合优化的因子图。该框架在复杂场景下表现出色，尤其在激光雷达信号退化时，系统切换到视觉信息进行跟踪，确保了鲁棒性。
关键技术
因子图构建：整合视觉、激光雷达和IMU，形成紧凑的优化模型。
特征提取与跟踪：使用FAST角检测器和KLT特征跟踪器提取视觉特征，结合激光雷达深度信息优化路标。
激光雷达深度处理：同步激光雷达与相机时间戳，减少误差并提升优化效率。
实验结果
系统在Newer College Dataset和DARPA SubT Challenge（Urban）数据集上表现优异，尤其在动态条件下，视觉信息弥补了激光雷达信号不足，保持了鲁棒性。计算效率也得以保证，输出频率达15Hz。
系统优势
高效鲁棒性：在极端条件下表现优于现有方法。
计算效率：在相机帧率下输出结果，适合实时应用。
应用潜力：适用于移动平台导航、建图及复杂环境下的定位。
结论
该研究提出了一种高效、鲁棒的多传感器融合方法，为3D视觉里程计领域提供了新的解决方案，具有广泛的应用前景。

Integrated Multi-Sensor Feature Matching in the Context of Tightly Coupled Lidar-Visual-Inertial Odometry System

该论文提出了一种统一的多模态地标跟踪方法，该方法通过整合激光雷达、视觉传感器和惯性测量装置的协同作用，在实时跟踪中表现出高精度和高鲁棒性。通过一系列实验验证，该方法在各种具有挑战性的场景中验证了其有效性。

单位：Oxford University、KTH Royal Institute of Technology, Sweden、Scania AB,Sweden

针对问题：

面向复杂场景的多传感器融合里程计

提出方法：

该系统构建了一个融合视觉、激光雷达和惯性信息的优化因子图。将视觉提取的点特征、激光雷达提取的线、面特征，以及通过IMU预积分构建的残差信息整合到同一个因子图中进行优化。

达到效果：

针对激光雷达与视觉退化场景的平滑过渡实现，通过构建和追踪轻量级基元信息，系统具备了在单个CPU上实现实时处理能力。在Newer College Dataset和DARPA SubT Challenge（Urban）数据集上，系统进行了性能评估。在典型场景下，其性能达到当前先进水平，而应对极端情况时展现出更高的稳定性。

Abstract

本研究开发了一种高效且可靠的多传感器融合里程计，专为移动平台设计。该系统构建了一个联合视觉、激光雷达和惯性信息进行优化的因子图。该系统采用固定滞后平滑技术，在全帧率下实现了实时运行。为了实现这一紧耦合系统，我们提出了一种创新的三维线和平面基元提取方法，该方法从激光雷达点云中提取特征。与传统帧对帧跟踪方法不同，该方法通过将基元视为路标，并在多次扫描中进行跟踪，显著提升了性能。通过被动同步激光雷达和相机帧，我们实现了激光雷达特征与视觉特征及IMU的真正整合。为了简化建模，我们采用轻量级建模策略，使系统能够在单个CPU上实现实时运行。在测试阶段，我们使用了多种场景，包括足型机器人进行地下探索和手持设备进行户外扫描，累计测试时长为96分钟，行驶距离达2.4公里。在这一系列测试中，仅依赖一个外感觉传感器无法完成任务，原因在于其几何限制或光照条件下的视觉不足。在这些复杂条件下，我们的因子图能够自然地整合每个传感器的最佳信息，无需硬性切换。

Motivation

在融合IMU、激光雷达和相机关联的背景下，两个主要挑战是：第一，在移动平台计算资源有限的情况下，实现实时性能；第二，三种不同频率和采集方法运行的信号的适当同步。此前的研究多采用松耦合的方式，或运行两个独立的系统（一个处理激光雷达和惯性，另一个处理视觉惯性里程计），以解决上述问题。

我们的动机是通过以下方式来解决这些问题。首先，我们提取和追踪少量关键元素；其次，我们构建一个连贯的因子图，通过IMU预积分将动态点云实时转换为相邻相机帧的时间戳。前者无需对整个点云进行匹配（如ICP）或追踪数百个特征点（如LOAM）。后者从而实现各传感器间的实时平滑融合。

Contribution

一种创新的因素图表示方法，将视觉、激光雷达和IMU的测量数据紧密集成到一个统一的优化框架中。

一种有效的方法用于提取激光雷达的特征，随后将其优化为路标表示。激光雷达和视觉特征均采用统一的参数流形表示，因为路标都被视为n维空间中的点、线和平面。这种紧凑的参数化方法使得我们能够以恒定帧率处理所有激光扫描数据。

在多个典型场景中进行了系统性的实验验证，相较于众多典型方案，这些方法在个别传感器失效情况下展现出卓越的抗干扰能力。

在此研究工作的基础上，我们基于VILENS框架，开发出了一种新型的系统架构。相较于VILENS，本系统增加了激光雷达特征跟踪功能，并通过激光雷达辅助实现视觉跟踪。相机与激光雷达的融合使其能够在运动剧烈的便携式设备上正常运行，并且在仅具备激光雷达或视觉特征的退化场景下，系统仍能稳定运行。

问题描述

我们旨在通过配置有视觉传感器、多光子传感器、激光雷达系统和IMU的移动系统，估计其位置、方向和线速度。在实验环境中，该系统被应用在足型机器人或手持设备上。

涉及的参考系如图所示，包含机器人固定基座坐标系B、左端相机坐标系C、惯性测量单元坐标系I和激光雷达固定坐标系L。系统估计基座坐标系B相对于固定世界坐标系W的位置。

状态定义

ti时刻的状态向量定义如下：

其中，Ri代表方向信息，pi代表位置信息，vi代表线速度信息。最后两个传感器的偏置信息分别为IMU陀螺仪和加速度计。

其中Xk、Mk、Pk、Lk是在固定滑窗内跟踪的所有状态和路标。

MAP建模

通过最小二乘法进行求解Zk观测下Xk的最大后验概率：

其中，Iij代表ti和tj之间IMU测量的数值，而Kk则表示tk时刻之前所有关键帧的索引。在上式中，残差被定义为：包括IMU数据、激光雷达平面与线特征、视觉路标以及状态先验的残差。

因子图模型

如图所示，本系统构建的因子图由以下部分构成：IMU因子、视觉-激光雷达路标因子、平面因子和直线路标因子。

IMU预积分因子

IMU预积分残差的形式为：

通过Lidar恢复深度的单目路标因子

为了实现多模态传感器的融合，我们采用了单目视觉特征跟踪的方法，并通过激光雷达的重叠视场来补充特征的深度信息。

由于激光雷达和摄像头的分辨率存在差异，在无法将激光雷达测得的深度信息与视觉特征进行对应的情况下，或者受到动态障碍物或噪声的影响，导致前后帧之间的深度变化量超过0.5米的不稳定状态时，从而实现深度信息的立体匹配。

双目路标因子

路标m`在状态xi处的残差是：

平面路标因子

我们使用单位法线∈和其与原点距离的标量d参数化一个平面p：

为了将齐次变换T作用于平面p的所有点，我们定义两个平面pi和pj之间的误差算子为：

其中Bp∈是的切线空间，定义如下:

当一个平面在ti时刻被观测到时，对应的残差对应地转化为与估计平面之间的差异。

直线路标因子

两条线li,lj之间的误差算子定义为：

测量线和其预测之间的残差定义如下：

Implementation

系统结构如图所示。该系统采用四个并行处理单元对传感器数据进行采集与优化处理。系统以相机关键帧频率输出因子图的状态估计结果，同时以IMU采样频率输出IMU前向传播的状态估计，分别应用于导航与制图以及控制功能。

在实验设置中，因子图基于iSAM2的求解过程，并采用5至10秒的滞后时间。视觉和激光雷达因素均采用DCS鲁棒核函数的处理方式，以降低外点的影响。

视觉特征跟踪

我们采用FAST角检测器提取特征点，并利用KLT特征跟踪器追踪特征点位置，通过RANSAC算法去除离群点。基于并行处理机制和逐步优化策略，系统每隔一帧选取为关键帧，实现了15赫兹的帧率输出。

LiDAR特征提取及跟踪

我们的算法具有核心优势，通过从激光雷达点云中提取特征元素，并与相机帧同步表示，从而实现对所有传感器的一体化优化。处理流程包含：去畸变处理、同步处理、过滤处理、基元提取与追踪，以及因子生成。

运动畸变补偿及同步

本研究通过对比图展示了我们传感器在不同输出频率下的性能。在IMU和相机捕捉到数据的瞬间，激光雷达持续进行数据采集。当激光雷达完成一个完整旋转后，其累积的激光回波被转换为点云数据，随后立即启动新的扫描过程。

基于激光回波是在移动过程中的捕获特性，需要利用运动先验对点云进行去畸变处理，并将其与唯一的时间戳进行关联--通常这个时间戳对应扫描的初始时刻。这种方法导致照相机和激光雷达的测量具有不同的时间戳。

我们选择将去畸变的激光雷达观测结果定为与相机时间戳最接近的值。在一般情况下，激光去畸变操作基于L2时刻作为参考点，然而这会导致激光帧与图像捕获时刻存在差异。因此，我们选择与C3时间最邻近的点云时间作为基准进行去畸变。考虑到来自IMU模块的状态前向传播，运动先验部分采用与点云中每个点相关联的时间戳进行线性插值。为简便起见，我们避免采用高斯过程插值，并且不使用时间偏移来扩展状态空间。由于点云现在与C3时间相关联，激光雷达路标被关联到与C3时间相同的节点，而非生成新的节点。

这个细节处理不仅确保了在图的优化中加入一致数量的新节点和因子，同时确保了优化是在IMU、照相机和激光雷达输入之间联合进行的。这固定地输出了相机帧率或激光雷达帧率，而不是两者的混合。

特征滤波

完成点云去畸变后，我们对点云进行分割，将其划分为若干群组。具有少于5个点的聚类将被标记为离群值并予以丢弃，因为这些点很可能属于噪声。

随后，对经过滤波处理的点云中的每个点进行局部曲率计算。其中，曲率最小和最大的点分别被归类为平面候选点CP和直线候选点CL。通过分割和基于曲率的过滤方法，点云中的点的数量通常减少至90%以下，从而显著降低了平面和平直线处理所需的时间。

平面和直线特征提取及跟踪

当两个线l_i和l_j的方向以及它们的中心距离均小于阈值时，将被判定为关联成功。

在我们的场景中，α p = α l = 0.35 rad, β p = β l = 0.5 m。

当一个特征被识别时，其离群值将从候选集合中被排除，随后，这一过程将被应用于剩余的路标。在追踪结束后，我们将注意力转向剩余的候选云，以检测新的路标。点云首先通过欧氏聚类法将线划分为不同的区域，而对于平面，则采用基于法线的区域增长方法。随后，在每个聚类中，我们将采用与路标跟踪相同的策略来识别新的路标。只有当点云特征被追踪达到最低连续扫描次数时，才会被纳入优化过程。

零速度状态因子

为了抑制平台静止状态下的漂移现象，我们在相机、激光雷达与惯性测量单元（IMU）所组成的两种模式的报告中，当报告处于无运动状态时，为因子图施加零速度约束。

EXPERIMENTAL RESULTS

我们的系统在Newer College Dataset和DARPA SubT Challenge（Urban）数据集上进行了系统性评估：

Results

下表展示了算法在10米距离内的平均相对位姿误差（RPE）。

VILENS-LI代表仅配置有IMU和激光雷达的VILENS系统；VILENS-LVI代表配置有IMU、视觉（包含基于激光雷达深度的信息）以及激光雷达特征的VILENS系统。

需要注意的是，该数据集缺乏闭环结构，与LOAM和LeGO-LOAM方法相比，我们的系统未进行建图处理。

对于SubT数据集，VILENS-LVI相较于LOAM在平移和旋转任务上分别实现了38%和21%的性能提升，而VILENS-LI则在两个任务上均超越了LOAM的21%。研究图1展示了ST-A数据集下的估计轨迹与真实轨迹的对比。VILENS-LVI能够在无需建图系统或闭环条件的情况下，达到极低的漂移率。

对于动态因素最小的NC数据集NC-1，VILENS-LVI在性能上与LeGO-LOAM不相上下。值得注意的是，对于动态因素较高的数据集（最高达3.37rad/s），即NC-2和NC-3，VILENS方法的表现显著优于LeGO-LOAM。其关键优势在于激光雷达云与相机时间戳的不失真，视觉特征深度来源于激光雷达，同时尽可能降低计算开销。从实验结果来看，表现最为突出的算法是VILENS-LVI，这得益于视觉与激光雷达特征的紧密耦合，使得我们成功避免了仅依赖激光雷达-惯性方法所存在的局限性。

Multi-Sensor Fusion

虽然大部分数据集为激光雷达和视觉特征跟踪提供了有利条件，但在某些特定场景下，紧耦合的系统展现出更强大的性能。例如，下图展示了NC-2的一个典型案例，当从明亮的阳光环境转变为阴暗环境时，相机的自动曝光功能需要3秒钟才能完成调整。在这段时间内，视觉特征的数量从约30个锐减至不到5个（主要集中于图像的一个角落）。这种变化会对状态估计的稳定性造成显著影响。通过与激光雷达的深度集成，我们能够有效利用有限的视觉特征和激光雷达特征，从而无需牺牲性能。这与采用独立视觉-惯性和激光雷达-惯性子系统的传统方法形成鲜明对比，后者需要明确处理退化情况。

在激光雷达标定基准无法充分约束计算结果（接近退化状态）的情况下，视觉特征的紧密关联确保了优化过程能够充分结合激光雷达的约束信息，从而有效规避退化问题。

Analysis

在优化过程中，采用轻量级基元的主要优势是显著提升了效率。该数据集的平均计算时间显示为：视觉特征跟踪耗时10ms，点云特征跟踪耗时50ms，而消费级笔记本电脑的优化耗时为20ms。因此，该系统在仅依赖激光雷达与惯性测量时，输出频率为10HZ（激光雷达帧率）；当融合视觉、激光雷达和惯性测量数据时，输出频率提升至15HZ（相机关键帧率）。

CONCLUSION

我们开发了一种紧耦合的相机、激光雷达和IMU测量的因子图模型。这种整合能够巧妙地应对传感器退化问题——通过在仅依赖激光雷达的特征跟踪和视觉跟踪（具有激光雷达深度）之间进行整合，这取决于每种模式在特定环境下的约束条件。我们已证明在常规条件下，该方法与最先进的激光雷达-惯性里程计性能相当，而在极端条件下，如剧烈运动或 sudden 光线变化，其性能表现更为出色。此外，我们还提出了一种在同一因子图中联合优化激光雷达和视觉特征的新方法，这使得在长走廊或黑暗等极端环境中仍能实现可靠的估计。

备注：

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

3D视觉精品课程推荐：

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路径！单模态与多元模式/数据与代码
3.彻底搞透视觉三维重建：原理剖析、代码解析与优化提升
4.首个面向工业级实战的点云处理课程体系
5.激光-视觉-IMU-GPS融合SLAM算法的梳理与代码解析
6.全面掌握视觉-惯性SLAM：基于VINS-Fusion正式发布课程
7.全面解析基于LOAM框架的3D激光SLAM技术，从源码解析到算法优化
8.全面解析室内与室外激光SLAM的关键算法原理、代码实现及实战应用（包括cartographer、LOAM和LIO-SAM）

9.从零开始构建一套结构光3D重建系统[理论、源码及实践]

10.单目深度估计方法：算法梳理与代码实现

重磅！3DCVer-学术论文写作投稿交流群已成立

通过扫码与小助手微信联系，可进行申请加入3D视觉工坊的学术论文写作与投稿的微信交流群，该群专设用于交流顶会、顶刊、SCI、EI等领域的写作与投稿事宜。

同时，我们诚挚邀请您加入我们的细分方向交流群。目前，我们已形成以下特色群组：三维视觉、计算机视觉与深度学习、SLAM技术与应用、三维重建技术、点云处理技术、自动驾驶与多传感器融合、CV入门学习、三维测量技术、VR/AR技术应用、医疗影像分析、缺陷检测技术、行人重识别、目标跟踪技术、视觉产品落地方案、视觉竞赛交流、车牌识别技术、硬件选型指导、学术交流群、求职交流群、ORB-SLAM系列源码分享、深度估计技术等。

研究方向 + 所属机构 + 尼称

▲长按加微信群或投稿

▲长按关注公众号

掌握3D视觉技术的系统化学习平台：本知识星球专注于丰富的视频课程资源，涵盖核心课程（包括三维重建系列、三维点云系列等）以及全面的学习体系。具体包括知识点梳理、从入门到进阶的学习路径、前沿动态与互动交流等五大模块。同时，星球为各类大厂算法工程师提供技术指导，并联合知名企业发布算法开发岗位及项目对接信息，致力于打造集技术与职业发展于一体的平台，吸引近4000位学习者共同成长。

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

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

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