关于Fast-LIO2: 快速直接激光雷达惯性里程计的学术研究报告
本次报告旨在向各位研究者详细介绍一项发表于《IEEE Transactions on Robotics》期刊的重要研究成果。该研究由香港大学机械工程系机电及机器人系统实验室的Wei Xu、Yixi Cai、Dongjiao He、Jiarong Lin以及Fu Zhang（通讯作者）共同完成，论文于2022年8月正式刊发（第38卷第4期）。这项工作的核心是提出了一个名为Fast-LIO2的激光雷达-惯性里程计（Lidar-Inertial Odometry， LIO）框架，其在计算效率、鲁棒性和通用性方面均实现了显著突破。
一、 学术背景与研究目标
该研究属于机器人学与自动驾驶领域中的核心方向——同时定位与建图。随着固态激光雷达技术的成熟与普及，其成本、体积和功耗的降低使其在更广泛的机器人应用（如无人机、手持测绘设备）中成为可能。然而，实现高效、精确的激光雷达里程计与建图仍面临多重挑战：首先，激光雷达每秒产生海量三维点云数据，对实时处理的计算效率要求极高；其次，传统方法依赖人工设计的特征提取模块（如边缘点、平面点），其性能易受环境影响（如无结构环境、小视场角），且难以适配不同扫描模式的激光雷达；再次，激光雷达点云在扫描过程中存在运动畸变，需要与惯性测量单元进行有效融合以补偿；最后，构建并实时维护一个用于配准稀疏点云的大规模稠密地图，需要高效的数据结构支持增量更新和快速最近邻搜索。
为应对上述挑战，Fast-LIO2研究团队设定了明确目标：开发一个能够直接处理原始点云、无需特征提取、且能高效维护大规模地图的激光雷达-惯性里程计系统。该系统应具备高速（高达100 Hz）、高精度、强鲁棒性（适应剧烈运动及杂乱环境）和高通用性（兼容多线旋转和固态激光雷达、不同计算平台）等特性。
二、 研究流程与核心技术
Fast-LIO2的研究工作主要围绕两个核心创新展开：增量KD树（Incremental KD-Tree， iKD-Tree）数据结构和直接原始点云配准方法。整个系统流程可划分为紧密耦合的状态估计模块和基于iKD-Tree的建图模块。
状态估计模块继承了作者团队先前工作Fast-LIO的紧耦合迭代卡尔曼滤波器框架，并集成了在线激光雷达-IMU外参标定。其工作流程严谨而高效：系统以扫描结束时激光雷达坐标系为基准，利用IMU数据通过逆向传播精确补偿扫描期间每个激光点的运动畸变。对于当前扫描中的每一个原始点，将其转换到全局坐标系（利用当前状态估计和外参）后，通过iKD-Tree在全局地图中快速搜索其最近邻点，并用这些邻接点拟合一个局部小平面。每个点与该局部平面之间的距离（残差）构成了系统的隐式测量模型。随后，系统采用一种在流形上操作的迭代卡尔曼滤波器，融合来自IMU传播的先验状态信息和来自所有激光点测量的残差信息，优化估计系统的完整状态（包括位置、姿态、速度、IMU偏置、重力矢量以及激光雷达-IMU外参）。该滤波器的一个关键优势在于其卡尔曼增益的计算复杂度与状态维度相关，而非与通常巨大的测量点数量相关，从而显著提升了计算效率。状态优化完成后，当前扫描的点云被精确地注册到全局坐标系。
建图模块的核心是全新的iKD-Tree数据结构。传统静态KD树在每次地图更新后需要完全重建，计算开销随地图规模增长而剧增，无法满足大规模场景下的实时需求。iKD-Tree通过以下机制解决了这一难题：1) 增量更新：支持单点插入（集成树上降采样功能）和基于轴对齐长方体的批量删除操作，无需完全重建树结构。2) 动态再平衡：设计了双重平衡准则（α-平衡准则和α-删除准则），主动监控子树平衡性。当子树因增量操作变得不平衡或无效节点过多时，触发局部重建。3) 并行重建策略：对于大型子树的重建，采用双线程设计。第二线程负责在后台重建子树，同时第一线程通过一个“操作记录器”捕获在此期间发生的地图更新请求，待后台重建完成后，将这些更新重新施加到新树上，从而保证了主线程状态估计的实时性不被长时间的重建过程中断。4) 高效K近邻搜索：利用节点存储的空间范围信息进行剪枝优化，实现对数时间复杂度的快速搜索。系统通过一个移动的立方体区域（地图尺寸）来管理地图，仅保留机器人当前位置附近一定范围内的点云，超出范围的旧点云通过iKD-Tree的盒式删除功能高效移除，从而控制地图规模。
为了全面验证系统性能，研究团队设计并执行了详尽的实验流程，涵盖了多个数据集和平台。首先，他们从五个公开激光雷达数据集中选取了19个序列用于基准测试，这些数据集包括使用固态激光雷达（Livox Horizon）的Lili-OM数据集，以及使用旋转式激光雷达（Velodyne VLP-16， HDL-32E）的LIO-SAM、UTBM、ULHK和NCLT数据集，场景涵盖校园、街道、森林和大型开阔停车场等。其次，他们还进行了多项真实世界实验，使用Livox Avia固态激光雷达在手持平台、小型无人机和大型测绘无人机上采集数据，测试环境包括室内外混合大场景和存在剧烈旋转运动的场景。
三、 主要实验结果与逻辑关联
实验结果的呈现具有清晰的逻辑层次，依次验证了数据结构、系统整体精度和计算效率，最终在实际应用中展示了其卓越性能。
首先，研究团队对iKD-Tree进行了独立的性能评估。他们将其与三种先进的动态数据结构——R*-Tree、Octree和Nanoflann KD-Tree——进行对比，并将它们分别集成到Fast-LIO2框架中，在18个不同规模的序列上运行测试。关键数据表明：iKD-Tree在K近邻搜索（KNN Search）和点插入（含降采样）的综合耗时上表现最佳。尽管Octree在点插入上稍快，但其搜索时间远高于其他数据结构。Nanoflann KD-Tree虽然平均性能相近，但在处理大规模地图时会出现偶发的、极高的插入时间峰值（可达数秒），严重破坏了系统的实时性保证，而iKD-Tree的最大处理时间在所有序列中均被控制在合理范围内（NCLT数据集中最长为214.4毫秒）。这一结果直接支持了选择iKD-Tree作为Fast-LIO2地图支撑结构的决策，因为其实时性和稳定性是后端建图模块可靠运行的基础。
其次，在系统精度基准测试中，Fast-LIO2与当时其他先进的LIO方法（LILI-OM， LIO-SAM， LINS）在19个序列上进行了对比。评价指标包括有真值序列的绝对轨迹误差均方根（RMSE）和回环序列的端到端误差。数据显示：Fast-LIO2（默认使用直接法，地图尺寸1000米）在19个序列中的17个上取得了最高的精度，且在所有实验中均未发生失败。相比之下，其他方法在部分长序列或开阔场景序列中出现了明显的精度下降甚至完全失效。例如，LILI-OM和LINS在部分NCLT序列中因后端优化失败而产生巨大漂移。此外，研究还进行了消融实验，结果显示：1) 增大地图尺寸（如从600米增至2000米）通常能提高精度，因为可以为配准提供更多历史信息，但过大尺寸（如超过2000米）可能因里程计漂移导致误匹配，收益不再显著。2) 将Fast-LIO2的“直接法”替换为“基于特征的方法”（分别采用为固态和旋转雷达优化的特征提取器）后，在绝大多数序列中精度持平或略有下降，这证明了直接利用原始点云的有效性，尤其是在特征不明显的环境（如树林、开阔区域）中。
再者，计算效率的基准数据极具说服力。在Intel i7-8550U的机载计算平台上，Fast-LIO2处理单次扫描（包括里程计和建图）的平均总时间远低于其他方法。数据显示，Fast-LIO2的总处理时间平均比LILI-OM快8倍，比LIO-SAM快10倍，比LINS快6倍。更值得注意的是，即使只与其他方法的“里程计”部分相比，Fast-LIO2在大多数序列中也更快或相当。这充分证明了其高度优化的算法和iKD-Tree数据结构带来的效率优势。更重要的是，研究团队成功将Fast-LIO2移植到基于ARM Cortex-A73的嵌入式平台（Khadas Vim3）上，并实现了10 Hz的实时运行，这在先前工作中未见报道，突显了其低功耗部署的潜力。
最后，真实世界实验验证了Fast-LIO2的实用性和鲁棒性。在一个使用手持设备采集的、长达650米的室内外混合大场景序列中，Fast-LIO2以100 Hz的扫描频率实时构建了高质量地图，端到端误差仅为0.14米。详细的处理时间分析显示，在Intel平台上，其单帧总处理时间平均仅为1.82毫秒，远低于10毫秒的扫描周期；在ARM平台上平均为5.23毫秒，虽有偶发超时，但平均性能仍满足实时要求。相比之下，其前身Fast-LIO由于需要为每帧新建KD树，建图时间随场景扩大而快速增长至超过10毫秒，无法在此大场景下实时运行。这组对比实验清晰地展示了iKD-Tree对于实现大规模场景实时建图的关键作用。此外，在搭载Livox Avia的小型无人机上进行室内激烈飞行测试时，Fast-LIO2在角速度高达1000度/秒的旋转运动下仍能提供可靠的姿态估计，证明了其在极端动态条件下的鲁棒性。
四、 研究结论与价值
本研究的结论是明确且有力的：Fast-LIO2是一个计算效率极高、鲁棒性强、通用性广且精度优于现有方法的激光雷达-惯性里程计框架。其科学价值在于：1) 提出了创新的iKD-Tree数据结构，为机器人领域中海量点云数据的实时动态管理提供了高效的解决方案；2) 验证了在高效数据结构的支持下，摒弃人工特征提取、直接使用原始点云进行配准的可行性及优越性，这简化了系统流程，增强了环境适应性；3) 展示了紧耦合迭代卡尔曼滤波器与先进数据结构相结合，能够实现从前端状态估计到后端地图维护的全流程高速优化。
其应用价值非常广泛：首先，该系统使计算资源受限的平台（如小型无人机、嵌入式设备）也能实现大规模环境的实时、高精度定位与稠密建图。其次，其对不同扫描模式激光雷达（特别是新兴的固态激光雷达）的自然适配性，降低了技术集成和应用门槛。最后，其开源的特性（代码发布于GitHub）极大地惠及了学术界和工业界的研究与开发社区。
五、 研究亮点
本研究的亮点突出体现在以下几个方面：技术创新性：iKD-Tree和直接原始点云配准是两个核心且新颖的贡献，它们从根本上解决了制约激光雷达里程计算法效率和通用性的关键瓶颈。实验全面性：评估工作极其详尽，涵盖了从数据结构对比、多数据集精度与效率基准测试，到多平台真实场景验证的完整链条，结论支撑坚实可靠。性能卓越性：系统在精度、速度和鲁棒性多个维度上均达到了当时领先水平，特别是在计算效率上的大幅提升和ARM平台的成功部署，展示了其实用化潜力。开源贡献：将Fast-LIO2系统及其iKD-Tree数据结构开源，体现了对开源社区的重要贡献，推动了相关领域的技术发展。
六、 其他有价值内容
论文中对iKD-Tree时间复杂度进行了理论分析，证明了其增量操作、重建和搜索的时间复杂度在三维空间下分别为O(log n)、O(n)或O(n log n)（取决于是否并行重建）、以及O(log n)，从理论层面支撑了其高效性。此外，文中详细描述了系统如何处理地图边界移动和点云降采样，这些工程细节对于实际系统实现具有重要参考价值。