【文献解析】Point-LIO: 一种鲁棒高速带宽的LIO

大家好呀，我是一个SLAM方向的在读博士，深知SLAM学习过程一路走来的坎坷，也十分感谢各位大佬的优质文章和源码。随着知识的越来越多，越来越细，我准备整理一个自己的激光SLAM学习笔记专栏，从0带大家快速上手激光SLAM，也方便想入门SLAM的同学和小白学习参考，相信看完会有一定的收获。如有不对的地方欢迎指出，欢迎各位大佬交流讨论，一起进步。博主创建了一个科研互助群Q：951026257，欢迎大家加入讨论。

Point-LIO: Robust High-Bandwidth LiDAR-Inertial Odometry
论文地址：
https://onlinelibrary.wiley.com/doi/full/10.1002/aisy.202200459
代码：https://github.com/hku-mars/Point-LIO

一、 文章概述

1. 问题导向

应用于导航任务的 LiDAR 传感器的基本要求是提供精确的位置估计以便机器人控制，以及一致且高速的测绘以便及时感知环境。通过以极高的速率（例如每秒数百万个）测量点，LiDAR 传感器可以实现相当高速率的里程计和测绘，从而可以跟踪极高速的运动。LiDAR传感器可以实现相当高的速率里程计和测绘，从而跟踪极高速的运动。然而，现有方法都基于类似于基于视觉的方法的帧体系结构，其中以特定的帧速率(例如，10赫兹)周期性地处理帧中的点。

2.目标

设计出一个能够应对剧烈运动的激光里程计

3.摘要

本文介绍了 Point-LIO：一种强大的高带宽 LiDAR-惯性里程计，能够估计极其剧烈的机器人运动。Point-LIO 以增量 k-d 树 (ikd-Tree) 为基础，用于高效地图组织，并以紧密耦合的流形 Kalaman 滤波器进行状态估计，具有两个关键的创新点，可实现高带宽 LiDAR-惯性里程计 (LIO)。第一个是逐点 LIO 框架，其中状态在每次 LiDAR 点测量时都会更新，而不会将它们累积到一帧中。这种逐点更新允许极高频率的里程计输出，显着增加里程计带宽，并从根本上消除了剧烈运动中人为的帧内运动失真。第二个主要创新点是随机过程增强运动学模型，该模型将 IMU 测量值建模为模型的输出，而不是像现有的基于滤波器的里程计或 SLAM 系统那样将其建模为模型的输入。这种新的建模方法即使在 IMU 测量在运动中期饱和的情况下，也能对剧烈运动进行准确定位和可靠建图。对具有小视场的固态激光雷达进行了各种真实的室内和室外实验，以评估其性能。总体而言，Point-LIO 能够在剧烈振动和剧烈运动下以超出 IMU 测量范围的高角速度（75 rad s−1）提供准确的高频里程计（4-8 kHz）和可靠的建图。我们进一步对来自多线旋转激光雷达和固态激光雷达收集的各种开放激光雷达数据集的 12 个序列进行了详尽的基准比较。 Point-LIO 始终保持相当的精度和时间消耗，表明 Point-LIO 具有计算效率高（例如，使用更少的计算资源实现相当的时间消耗）、稳健（例如，在各种环境下进行可靠的姿态估计）、多功能（例如，适用于多线旋转和固态激光雷达、汽车、机器人汽车、无人机和手持平台）。最后，展示了 Point-LIO 的两个示例应用，一个是赛车四旋翼飞行器，另一个是自旋转无人机，两者均受到超出 IMU 测量范围的高角速度（例如，59 rad s-1 和 25 rad s-1）。我们的系统将在 Github https://github.com/hku-mars/Point-LIO.git 上开放，以造福社区。

4.贡献

1) 我们提出了一种逐点 LiDAR 惯性里程计框架，该框架在实际采样时融合 LiDAR 点，而无需累积到帧中。消除点累积可消除帧内运动失真，并允许以接近点采样率的高里程计输出和地图更新，这进一步使系统能够跟踪非常快速的运动。
2) 为了进一步将系统带宽提高到 IMU 测量范围之外，我们使用随机过程模型 [14] 来对 IMU 测量进行建模。然后，我们将此模型扩展到系统运动学中，并将 IMU 测量视为系统输出。即使 IMU 饱和，随机过程增强运动学模型也可以平稳估计系统状态，包括角速度和线性加速度。。
3) 我们将这两项关键技术集成到一个完全紧密耦合的 LiDAR-惯性里程计系统中，称为 Point-LIO。该系统使用流形扩展卡尔曼滤波器，通过融合每个 LiDAR 点或 IMU 数据在其各自的采样时间更新系统状态。通过利用系统的稀疏性和线性，开发的系统即使在微型飞行器上的低功耗 ARM 计算机上也能实现实时状态估计。
4)所开发的系统在各种具有挑战性的真实世界数据中进行了测试，这些数据由具有非常小的视场角的新兴固态激光雷达收集。结果显示了 Point-LIO 在运动失真补偿、高里程计输出率（4-8 kHz）和高带宽（> 150 Hz）方面的能力。该系统还能够在初始阶段之后使用饱和的 IMU 测量来估计极端剧烈运动（角速度超过 75 rad s−1）下的状态。此外，对来自各种开放 LiDAR 数据集的 12 个序列进行的详尽基准比较表明，Point-LIO 实现了与其他同类产品相当的精度和效率，同时消耗更少的计算资源。最后展示了实际无人机上的真实应用。

二、 方法解析

2.1 系统总览

1）LiDAR 点在相应时间按顺序采样，而不是同时采样一帧
2）IMU数据是系统的测量值，而不是输入。
我们将这两个测量值融合在流形扩展卡尔曼滤波器中。
具体来说，对于接收到的每个 LiDAR 点，都会从地图中搜索相应的平面。如果该点与由地图中的点拟合的平面匹配，则计算残差以使用流形卡尔曼滤波器更新系统状态。优化后的位姿最终将LiDAR点注册到全局框架中并合并到地图中，然后进行下一次测量（LiDAR点或IMU数据）。否则，如果该点没有匹配的平面，则直接通过卡尔曼滤波器预测的姿态将其添加到地图中。为了在接纳新注册点的同时实现快速平面对应搜索，我们使用了增量 k-d 树结构 ikd-Tree，该结构最初是在 FAST-LIO2 中开发的对于每次 IMU 测量，将分别对 IMU 的每个通道进行饱和度检查，具有饱和值的通道将不会用于状态更新。

2.2 状态估计

2.2.1 IMU的连续运动学模型