论文精读系列文章——Point-LIO: Robust High-Bandwidth Light Detection and Ranging Inertial Odometry

论文精读系列文章

下面是SLAM算法与工程实践系列文章的总链接，本人发表这个系列的文章链接均收录于此

论文精读系列文章链接

---

下面是专栏地址：

论文精读系列文章专栏

---

---

前言

这个系列的文章是分享论文阅读的相关学习记录

---

论文精读系列文章——Point-LIO: Robust High-Bandwidth Light Detection and Ranging Inertial Odometry

注

关于 Point-LIO 的代码注释我已放在github上：Point-LIO-Noted-JIN

我的 Point-LIO 注释版本给给源代码添加了很多中文注释，包括公式说明等，Point-LIO源码直接上手理解有一定难度，有FAST-LIO的代码基础会更容易理解

同时添加了输出线速度与角速度的代码，方便查看Odometry时分析数据

本人注释版地址：https://github.com/yangtseJin/Point-LIO-Noted-JIN

源代码地址：https://github.com/hku-mars/Point-LIO

论文地址：https://onlinelibrary.wiley.com/doi/full/10.1002/aisy.202200459

Advantages of the point-by-point framework in odometry estimation:

• The point-by-point framework in odometry estimation allows for an extremely high-frequency odometry output, significantly increasing the odometry bandwidth and providing accurate localization and mapping for aggressive motions.
• This framework fundamentally removes the artificial in-frame motion distortion that is often present in other odometry methods based on scans or frames.
• By updating the system state at the true sampling time of each LiDAR point measurement, the point-by-point framework eliminates the need to accumulate points into frames, reducing latency and enabling the system to track very fast motions.
• The high-frequency state update of the point-by-point framework reduces the motion distortion caused by continuous LiDAR motion during a frame, resulting in more accurate odometry estimation, especially for aggressive motions where the velocity may change during a scan.
• Overall, the point-by-point framework in odometry estimation provides a robust and high-bandwidth solution for accurate and reliable odometry in the presence of severe vibrations and high angular velocity.

Contributions of the paper “Point-LIO: Robust High-Bandwidth Light Detection and Ranging Inertial Odometry”:

• The paper introduces a point-wise LiDAR-inertial odometry (LIO) framework that updates the state at each LiDAR point measurement, significantly increasing the odometry bandwidth and removing artificial motion distortion.
• The use of a stochastic process-augmented kinematic model allows accurate localization and reliable mapping for aggressive motions, even when the inertial measurement unit (IMU) measurements are saturated.
• The system achieves high-frequency odometry output (4-8 kHz) and reliable mapping under severe vibrations and aggressive motions with high angular velocity (75 rad/s) beyond the IMU measuring ranges.
• The paper presents an exhaustive benchmark comparison, demonstrating that Point-LIO achieves consistently comparable accuracy and efficiency to other counterparts while requiring fewer computation resources.
• Two example applications of Point-LIO are demonstrated: a racing drone and a self-rotating unmanned aerial vehicle, both with aggressive motions.

Abstract

本文提出了点光检测和测距惯性里程计 (LIO)：一种强大的高带宽光检测和测距 (LiDAR) 惯性里程计，能够估计极其剧烈的机器人运动。

Point-LIO 有两个重要的创新点。

第一个是逐点 LIO 框架，用于更新每个 LiDAR 点测量的状态。该框架允许极高频的里程计输出，显着增加里程计带宽，并从根本上消除人为的帧内运动失真。

The first one is a point-by-point LIO framework that updates the state at each LiDAR point measurement.

This framework allows an extremely high-frequency odometry output, significantly increases the odometry bandwidth, and fundamentally removes the artificial in-frame motion distortion.

第二个是随机过程增强运动学模型，它将 IMU 测量结果建模为输出。这种新的建模方法即使在惯性测量单元 (IMU) 测量在运动中间饱和的情况下也能实现精确定位和可靠的建图。

The second one is a stochastic process-augmented kinematic model which models the IMU measurement as an output.

This new modeling method enables accurate localization and reliable mapping for aggressive motions even with inertial measurement unit (IMU) measurements saturated in the middle of the motion.

进行了各种实际实验来评估性能。

总体而言，Point-LIO 能够在剧烈振动和高角速度 （75 rad s^-1 ）超出 IMU 测量范围的剧烈运动下提供准确的高频里程计 (4–8 kHz) 和可靠的建图。

此外，还进行了详尽的基准比较。 Point-LIO 实现了一致的可比精度和时间消耗。

最后，演示了Point-LIO的两个示例应用，一个是竞赛无人机，另一个是自旋转无人机，两者都具有攻击性动作。

1. Introduction

在过去的几十年里，由于直接、密集、主动和准确的深度测量，3D 光探测和测距 (LiDAR) 传感器在自主应用中发挥着越来越重要的作用，例如基于视图的同步定位和地图绘制（ SLAM），[1,2] 机器人探索和检查，[3,4] 和自动驾驶。[5–8] LiDAR 技术的最新发展[9,10] 已经实现了轻量级、成本效益高且易于使用的商业化和大规模生产。高性能 LiDAR 传感器，有可能使一系列现有和新兴应用受益，例如自主导航[11]和物体检测。[12,13]

应用在导航任务中的激光雷达传感器的一个基本要求是为机器人控制提供准确的位置估计，并为及时感知环境提供一致和高速的映射。通过以极高的速率（例如每秒数百万个）测量点，激光雷达传感器可以实现相当高速率的测距和测绘，从而可以跟踪极高速的运动。

然而，现有的方法都是基于类似于基于视觉的方法的帧架构，其中帧中的点以一定的帧速率（例如，10 Hz）定期处理。

然而，实际上，LiDAR 点是在不同时刻顺序采样的；将这些点累积到一个帧中会引入人为运动失真，并对测绘结果和里程计精度产生不利影响（adversely affect）。

低帧速率还会增加里程计中的延迟（latency）并限制可达到的带宽（attainable bandwidth），其中里程计带宽（odometry bandwidth）的定义类似于（in analogy to）动态系统的带宽（dynamic system,），即系统增益降至 0.707 以下的频率。里程计带宽代表了里程计可以令人满意地估计的运动速度

在这项工作中，我们通过两种关键的新技术解决这些问题：逐点状态更新和随机过程增强运动学模型。更具体地说，我们的贡献如下：

1）我们提出了一种逐点激光雷达惯性里程计（LIO）框架，该框架在实际采样时间融合激光雷达点，而不累积到帧中。点累积的消除消除了帧内运动失真，并允许以接近点采样率的高里程计输出和地图更新，这进一步使系统能够跟踪非常快的运动；

2）为了进一步提高系统带宽超出惯性测量单元（IMU）测量范围，我们使用随机过程模型（ stochastic process model）[14]对IMU测量进行建模。然后，我们将该模型增强到系统运动学中，并将 IMU 测量值视为系统输出。即使 IMU 饱和（saturates），随机过程增强运动学模型也可以平滑估计系统状态，包括角速度和线加速度；

3）我们将这两种关键技术集成到一个完全紧密耦合的LIO系统中，称为Point-LIO。该系统使用流形扩展卡尔曼滤波器 (EKF)，通过在各自的采样时间融合每个 LiDAR 点或 IMU 数据来更新系统状态。通过利用系统的稀疏性和线性度，所开发的系统即使在微型飞行器上基于低功耗高级RISC（精简指令集计算机）机器（ARM）的计算机上也能实现实时状态估计；

4）所开发的系统在由具有非常小的视场的新兴（emerging）固态激光雷达收集的各种具有挑战性的现实世界数据中进行了测试。结果显示了 Point-LIO 的运动失真补偿、高里程计输出速率 (4–8 kHz) 和高带宽 (>150 Hz) 的能力。该系统还能够在初始阶段后通过饱和 IMU 测量来估计极端剧烈运动（角速度超过 75 rad s^-1 ）下的状态。

此外，对各种开放 LiDAR 数据集的 12 个序列进行的详尽基准比较表明，Point-LIO 始终能够实现与其他同类产品相当的精度和效率，同时消耗更少的计算资源。最终展示了在实际无人机（UAV）上的实际应用。

其余文章的组织如下。

在第2节中，我们讨论相关的研究工作。

我们在第 3 节中概述了完整的系统流程。

第 4 节介绍了系统公式、基于 EKF 的状态估计器，并总结了算法。

第 5 节介绍了系统的评估，

第 6 节报告了开放数据集的基准比较。

最后，第 7 节介绍了系统在实际无人机中的应用，

第 8 节给出了结论。

2. Related Works

2.1. LiDAR(-inertial) Odometry

最近许多关于 3D LiDAR 里程计和建图的工作都是基于 LiDAR-odometry and mapping (LOAM) 结构，[15] 其中原始 LiDAR 点累积到一个帧（也称为扫描）中以提取特征点（例如，边缘和飞机）。

然后将提取的特征点注册到先前的扫描，以扫描速率（即 10 Hz）产生里程计输出，并且最近的一些扫描累积到一个小子地图中，该子地图最终以较低的速率（即 1 Hz）来细化相对于地图的 LiDAR 位姿。

LOAM中扫描到扫描（scan to scan）和扫描到地图（ scan to map）之间的分离结构在许多后续工作中被采用，例如Lego-LOAM，[16]考虑了扫描到扫描匹配过程中地面产生的约束，以改进里程计精度，LINS，[17] 将 IMU 数据与扫描配准融合，以及参考文献中的其他数据 [18,19]，重点关注计算效率或准确性的提高。

虽然扫描到扫描和扫描到地图之间的分离可以显着减轻里程计所需的计算负载，但里程计中的扫描到扫描配准（scan-to-scan registration）通常会导致快速漂移积累。

此外，扫描到扫描的配准需要连续扫描之间存在大量重叠，这在小 FoV 固态 LiDAR 中可能不可用。 [20]为了解决这些问题，直接扫描到地图（scan-to-scan registration）（或扫描到本地地图， scan to local map）已被广泛采用，例如基于点地图的地图，[11,21]（G-）ICP，[22]NDT，[22]Surfel地图，[23–26] 或体素图。[27]

特别是，参考文献[20]提出了一种并行扫描到地图方法来处理固态激光雷达的小视场问题。参考号[28]将 IMU 测量融合到高效迭代卡尔曼滤波器框架中的扫描到地图配准中。

scan-to-map 框架中的一个关键问题是如何维护地图结构，以便它可以从新扫描中增量添加点，同时允许高效查询。为了解决这个问题，参考。 [29]提出了一种增量k-d树，ikd-Tree，作为映射结构。受益于这种高效的增量映射结构，系统 FAST-LIO2 能够以 10 Hz 的频率（对于旋转 LiDAR）和 100 Hz 的频率（对于固态 LiDAR）实时执行测距和映射，甚至在基于 ARM 的低功耗计算机上也是如此。

扫描到地图配准的一个主要缺点是里程计是按扫描（或帧）的速率估计的，从而限制了帧速率下的里程计输出频率。有限的输出频率将导致里程计的延迟等于扫描持续时间。

此外，由于奈奎斯特-香农采样定理，有限的状态估计速率将为里程计带宽设置不必要的上限。这个问题已在 Lola-SLAM[30] 和 LLOL[31] 中得到部分解决，它们建议将扫描分割为多个子扫描，并在接收到每个子扫描后将每个子扫描注册到地图上，分别实现 160 和 80 Hz 的里程计。

与这些方法（即扫描到扫描，[15–19]扫描到地图，[11,21–26]和子扫描到地图[30,31]）相比，我们的所提出的系统是一个点到地图框架，一旦收到每个单独的点，它就会将其注册到地图上。这种点到地图框架允许在理论上以点采样率和在实践中以 4-8kHz 进行里程计。前所未有的高频状态更新将延迟降低至微秒，同时显着增加了里程计带宽。

2.2. Motion Distortion Compensation

如上所述，现有的激光雷达（惯性）里程计和测绘工作几乎都是基于扫描（即帧），这会遭受帧内激光雷达连续运动导致的帧内运动失真。为了纠正这种失真，通常需要补偿方法。

大多数努力都假设帧内有恒定速度的运动（constant-velocity motion within the
frame）来补偿运动失真，例如在参考文献中。 [15,17–19,32–39]。当扫描持续时间短或运动平缓时，恒速运动假设是有效的。

但对于速度在扫描过程中可能发生变化的非常剧烈的运动（例如，在无人机特技飞行中），等速方法通常会导致里程计出现较大漂移甚至失败

另一种流行的运动补偿方法是基于连续时间轨迹优化（ continuous-time trajectory optimization,），例如基于 B-Splines[26,40–42] 和高斯过程模型的方法。[43–45] 连续时间轨迹允许评估任何时刻，因此可以补偿每个单独点的失真。然而，连续时间轨迹优化非常耗时，并且通常离线实现。[41,42,44,45]虽然有一些在线实现，[26,40,43]里程计速率通常较低（例如，10 Hz）以确保实时优化。此外，它们需要积累足够的点来进行可靠的轨迹参数优化，这会带来相当大的里程计延迟。

连续时间轨迹固有的平滑性也阻止了机器人经历的高度攻击性运动的描述

利用 IMU 测量是运动补偿的另一种有效方法。[29,46] 这些方法使用帧内的 IMU 数据集成 LiDAR 位姿，以消除所包含点的畸变。由于 IMU 测量频率较高（例如 200 Hz），基于 IMU 的运动补偿对于通常的机器人运动甚至在快速滚动的无人机机动（快速滚动的无人机机动）中非常有效[29]（高达 1242° s^-1 ）。然而，该方法仍然受到 IMU 频率的限制，并且还受到 IMU 测量噪声和偏差估计误差的影响。

上面回顾的临时运动补偿最终是由于现有方法中基于帧的里程计框架。在我们的系统中，我们在真实采样时间融合每个单独的 LiDAR 点，而不是将点累积到帧中。帧累积的消除从根本上从一开始就消除了运动失真，因此没有前面提到的缺点。

2.3. Formulation of Inertial Measurements

为了将IMU测量与LiDAR点配准融合，通常使用两种主流方法，即松耦合和紧耦合。

松耦合方法集成 IMU 测量以获得位姿先验估计，并使用该先验估计作为后续扫描配准的初始位姿。[15,38,47–49] 另一方面，紧耦合方法融合 IMU 测量和 LiDAR联合优化中的点。

针对紧密耦合方法，提出了两种实现方式：

• 基于 EKF 的方法[17,29,50,51] 和基于优化的方法。[18,52,53]

  基于 EKF 的方法[17,29,50,51] 将 IMU 测量集成到EKF 的传播过程以获得位姿估计，随后在 EKF 更新步骤中与 LiDAR 测量融合。

• 相比之下，基于优化的方法预积分 IMU 测量以获得相对位姿约束，然后将预积分的相对位姿约束与点配准误差融合。 [18,52,53]

紧耦合方法通常比松耦合方法具有更高的鲁棒性和准确性。

然而，在所有上述紧耦合方法中，IMU 数据被用作运动学模型的输入，因此它可以在 EKF 传播中传播或预积分一帧持续时间。 [54]

如果机器人运动超出 IMU 测量范围，这种 EKF 传播或预积分将会遇到饱和问题。

在其他研究中，[26,44,45] IMU 数据用于提供从连续时间轨迹预测的角速度和线性加速度的测量结果，并在此基础上优化轨迹参数以及 LiDAR 扫描配准因子。

将 IMU 数据视为模型输出的测量值，可以自然地处理由剧烈运动引起的 IMU 饱和问题，尽管此功能受到前面回顾的连续时间模型的限制。

我们的方法与其他研究类似[26,44,45]，将 IMU 数据视为模型输出的测量值，但将机器人运动建模为随机过程，然后通过运动学进行增强。

这种随机过程增强运动学模型允许 IMU 与 EKF 框架中的 LiDAR 点一起更新状态。开发的系统能够处理振动和高速运动等极端剧烈运动中的饱和 IMU 测量。

据我们所知，之前的任何工作都没有证明 LiDAR 惯性系统可以与饱和 IMU 测量一起工作。

3. System Overview

我们的设计理念是真实地认识到：

1）LiDAR 点是在各自的时间顺序采样的，而不是同时采样的帧；

2）IMU 数据是系统的测量值，而不是输入。

一旦收到各自的测量结果（每个 LiDAR 点或 IMU 数据），我们就会将这两个测量结果融合到流形 EKF 框架 [55] 中

我们设计的系统概述如图1所示；

顺序采样的 LiDAR 点和 IMU 数据都用于更新各自时间戳的状态，从而产生极高速率的里程计输出，即实践中的 4-8 kHz。

特别是，对于接收到的每个 LiDAR 点，从地图中搜索相应的平面。如果该点与地图中的点的平面拟合相匹配，则使用流形卡尔曼滤波器计算残差以更新系统状态。优化后的姿态最终将LiDAR点注册到全局框架中并合并到地图中，然后继续进行下一个测量（LiDAR点或IMU数据）。否则，如果该点没有匹配的平面，则通过卡尔曼滤波器预测的位姿直接将其添加到地图中。

为了在接纳新注册点的同时实现快速平面对应搜索，我们使用增量 k-d 树结构，即 ikd-Tree，最初是在 FAST-LIO2 中开发的。[29]

对于每次IMU测量，IMU的每个通道的饱和度检查是单独进行的，具有饱和值的通道将不会用于状态更新。

4. State Estimation

Point-LIO 的状态估计是一个紧耦合的流形卡尔曼滤波器。在这里，我们简要解释了滤波器的基本公式和工作流程，并建议读者参考[55]，以获取流形卡尔曼滤波器的更详细和理论解释。

4.1. Notations

为了便于解释，我们采用如下符号。

此外，我们引入了参考文献中定义的两个封装操作，$⊞$（“boxplus”）及其逆 $\boxminus$（“boxminus”）。 [55]在 $n$ 维流形 KaTeX parse error: Undefined control sequence: \scr at position 1: \̲s̲c̲r̲{M} 上描述系统，并参数化欧几里得空间 ${\mathbb{R}}^n$ 中的状态误差。

而且，这些操作可以更紧凑地描述离散时间上的系统状态空间模型。我们建议读者参考参考文献。 [55]更详细的定义和推导；在本文中，我们只关心流形 $SO(3)$ 和 ${\mathbb{R}}^n$
$$\begin{array}{rl}⊞:& \mathcal{M}\times {\mathbb{R}}^n\to \mathcal{M};\boxminus :\mathcal{M}\times \mathcal{M}\to {\mathbb{R}}^n\\ & \\ \mathrm{SO}(3):& \mathbf{R}⊞\mathbf{r}=\mathbf{R}\cdot \mathrm{Exp}(\mathbf{r});{\mathbf{R}}_1\boxminus {\mathbf{R}}_2=\mathrm{Log}({\mathbf{R}}_2^T\cdot {\mathbf{R}}_1)\\ & \\ {\mathbb{R}}^n:& \mathbf{a}⊞\mathbf{b}=\mathbf{a}+\mathbf{b};\mathbf{a}\boxminus \mathbf{b}=\mathbf{a}-\mathbf{b}\end{array}$$
其中
$$\mathrm{Exp}(\mathbf{r})=\mathbf{I}+\sin (\Vert \mathbf{r}\Vert )\frac{|\mathbf{r}|}{\Vert \mathbf{r}\Vert }+(1-\cos (\Vert \mathbf{r}\Vert ))\frac{|\mathbf{r}{|}^2}{\Vert \mathbf{r}{\Vert }^2}$$
是 $SO(3)$ 上的指数映射，$\mathrm{Log}$ 是其逆映射。

对于复合流形 KaTeX parse error: Undefined control sequence: \scr at position 1: \̲s̲c̲r̲{M}=\mathrm{SO(… ，即两个子流形 KaTeX parse error: Undefined control sequence: \scr at position 1: \̲s̲c̲r̲{M}=\mathrm{SO(… 和 ${\mathbb{R}}^n$ 之间的笛卡尔积，我们有
$$\left[\begin{array}{c}\mathbf{R}\\ \mathbf{a}\end{array}\right]⊞\left[\begin{array}{c}\mathbf{r}\\ \mathbf{b}\end{array}\right]=\left[\begin{array}{c}\mathbf{R}⊞\mathbf{r}\\ \mathbf{a}+\mathbf{b}\end{array}\right],\left[\begin{array}{c}{\mathbf{R}}_1\\ \mathbf{a}\end{array}\right]\boxminus \left[\begin{array}{c}{\mathbf{R}}_2\\ \mathbf{b}\end{array}\right]=\left[\begin{array}{c}{\mathbf{R}}_1\boxminus {\mathbf{R}}_2\\ \mathbf{a}-\mathbf{b}\end{array}\right]\text{\hspace{1em}(1)}$$

4.2. Kinematic Model

我们首先推导系统模型，它由状态转换模型和测量模型组成。

4.2.1. State Transition Model

以 IMU 帧（记为 $I$）为 body 帧，第一个 IMU 坐标系为全局坐标系（记为 $G$），连续运动学模型为
$$\begin{array}{rl}{}^G{\dot{\mathbf{R}}}_I& ={}^G{\mathbf{R}}_I\lfloor {}^I\omega \rfloor ,{}^G{\dot{\mathbf{p}}}_I={}^G{\mathbf{v}}_I,{}^G{\dot{\mathbf{v}}}_I={}^G{\mathbf{R}}_I{}^I\mathbf{a}+{}^G\mathbf{g},{}^G\dot{\mathbf{g}}=0\\ & \\ {\dot{\mathbf{b}}}_g& ={\mathbf{n}}_{{\mathrm{b}}_g},{\dot{\mathbf{b}}}_a={\mathbf{n}}_{{\mathrm{b}}_a},{}^I\dot{\mathbf{\omega }}={\mathbf{w}}_g,{}^I\dot{\mathbf{a}}={\mathbf{w}}_a\end{array}\text{\hspace{1em}(2)}$$
其中

${}^G{\mathbf{R}}_I$、${}^G{\mathbf{p}}_I$ 和 ${}^G{\mathbf{v}}_I$ 代表全局坐标系中IMU的姿态、位置和速度。

G g {}^G\mathbf{g}