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

这篇论文《Point-LIO: Robust High-Bandwidth LiDAR-Inertial Odometry》提出了一种基于点云的高带宽LiDAR-惯性融合定位算法（Point-LIO），旨在解决传统LiDAR-Inertial Odometry（LIO）框架在高频度下的稳定性问题以及极端运动场景中的鲁棒性挑战。以下是其主要内容和技术亮点：---### 1. 引言与背景- 研究背景 - 传统的LIO框架依赖于帧间配准（frame-to-frame odometry），但在快速运动或极端姿态变化时易受 IMU 数据质量的影响。 - 高带宽LIO算法（如Fast-LIO2）虽然提高了频率和精度，但在极端姿态变化时仍存在性能下降的问题。- 研究目标 - 提出一种能够在任意小时间间隔内完成配准且具有强鲁棒性的高带宽LiO算法。 - 在频繁 LIDAR 点测量和 IMU 数据融合中实现更高的定位精度和更快的速度更新频率（4-8 kHz）。---### 2. 方法#### 2.1 系统概述- 关键创新点 - 逐点 LIDAR 框架：每次仅接收单个 LIDAR 点测量并直接与地图中的平面配准。 - 随机过程增强 IMU 模型：将 IMU 数据建模为随机过程（如 Wiener 过程），以应对长时间内的 IMU 噪声积累问题。 - 鲁棒 IMU 处理框架：采用饱和值校正机制对 IMU 测量值进行过滤，并仅当 IMU 数据稳定时才更新状态。- 传感器融合模型 - LiDAR 提供全局定位信息；IMU 提供速度与加速度信息以补充位置估计。

学术论文精读专辑：阅读笔记与学习专刊

此系列文章总链接为SLAM算法与工程实践相关的全部文章，请参考此系列文章总链接中的所有相关文章。

学术深度阅读指南

下面是专栏地址：

学术深度阅读指南
学术深度阅读指南中的具体文章
深度学习专家分享与实践交流平台

前言

深入阅读与理解《Point-LIO: Robust High-Bandwidth Light Detection and Ranging Inertial Odometry》这一学术资源

注：本文属于资源简介类内容。

• Abstract

  • Introduction
    • This research introduces a comprehensive methodology for autonomous navigation systems, highlighting its effectiveness in enhancing accuracy and reliability.
    • Related Works section critically examines existing solutions, focusing on their limitations and potential improvements.
    • Under the Related Works, we first explore the integration of LiDAR(-inertial) Odometry systems, discussing their advantages and challenges.
      • Within this subsection, we delve into Motion Distortion Compensation techniques, analyzing their role in maintaining system stability.
      • Furthermore, we formulate the mathematical models underlying Inertial Measurement Units (IMUs), providing insights into their operational principles.

• 3. 系统概览

• 4. 状态估计

  • 4.1 符号说明
  • 4.2 运动学模型
    • 4.2.1 状态转移模型
      • 注意

4.2.2. Measurement Model

4.3. 基于扩展的卡尔曼滤波器（EKF）

4.3.1. 状态传播过程

4.3.2. 残差计算步骤

4.3.3. 状态更新机制

本研究的目标是采用创新的方法与技术以提高数据分析效率。我们期望通过这些手段获得更精确的数据分析成果，并在此过程中探索出更高效的解决方案。以期为相关领域的研究提供新的思路与参考。

这个系列的文章详细记录了多篇论文的核心观点和研究方法

论文精读系列文章——Point-LIO: 坚固的高带宽激光雷达惯性定位系统

注

该代码注释已公开于GitHub平台，并附有详细说明文档

我开发的Point-LIO注释版本为源代码增加了大量中文注释，并特别包含公式详细的说明。Point-LIO源码上手理解存在一定难度，但熟悉FAST-LIO代码库将有助于更轻松地理解和使用Point-LIO。

在Odometry模块中增加了输出线速度与角速度的代码。

个人注释版本链接：https://github.com/yangtseJin/Point-LIO-Noted-JIN

The repository for Point-LIO is available at https://github.com/hku-mars/Point-LIO.

该学术资源可通过以下链接访问：https://onlinelibrary.wiley.com/doi/full/10.1002/aisy.202200459。

The advantages of the point-wise framework for odometry estimation:

A pointwise approach in odometry estimation achieves an ultra-high sampling rate, drastically elevating the system's bandwidth while ensuring precise localization and mapping capabilities even for highly dynamic environments.

This framework completely eliminates inherent frame-based artifacts that are commonly encountered in alternative odometry methods reliant on scans or frames.

By incorporating measurements at their actual timestamps, this method avoids frame accumulation processes, effectively reducing latency and enabling seamless tracking of extremely fast motions.

A risk of introducing motion artifacts when LiDAR is moving continuously within a frame is mitigated by this ultra-high sampling rate approach. The result is enhanced accuracy in estimating odometry metrics, particularly during aggressive motions characterized by velocity changes within a single scan.

The comprehensive approach ensures reliable performance under challenging conditions such as significant vibrations and rapid angular velocity changes.

The contributions of this paper, which introduces the Point-LIO system, highlight its robustness in high-bandwidth LiDAR-based inertial odometry applications.

The paper presents a point-wise LiDAR-inertial odometry (LIO) framework that updates the state for every LiDAR point measurement, significantly enhancing the odometry bandwidth and effectively eliminating artificial motion distortion.
This application of a stochastic process-augmented kinematic model enables precise localization and dependable mapping for high-intensity movements, even when inertial measurement unit (IMU) data is compromised.
The system delivers high-frequency odometry output (4-8 kHz) along with reliable mapping under severe vibrations, extreme movements, and rapid rotations up to 75 rad/s beyond IMU measurable ranges.
The practical implementations of Point-LIO include racing drones like Formula 1 cars 和 advanced unmanned aerial vehicles like quadcopters, both subjected to aggressive motions.

本文阐述了一种融合了点光检测与测距技术的创新性解决方案——点光检测和测距惯性里程计 (LIO)，该方法通过整合高带宽光检测与测距技术实现了对极端动态环境下的机器人运动状态精确估算。

Point-LIO 包含两个关键性创新点，并且这些创新点处于核心地位

第一个是基于逐点 LIO 的框架系统,负责更新每个 LiDAR 点测量状态.该系统具有高频率的里程计输出能力,并且显著地增加了里程计带宽.同时,在本质上消除了由于人工造成的帧内运动模糊.

This framework represents a point-by-point approach for implementing the LIO method, which updates the state during each LiDAR measurement.

This framework enables ultrahigh sampling rates for odometry measurements, substantially elevating the odometry measurements' throughput, and eliminates entirely any artificial in-frame motion artifacts.

另一个是随机过程增强运动学模型；它通过将IMU测得的结果建立模型作为输出来处理数据。该新型建模方案即使在惯性测量单元（IMU）测速过程中出现饱和时仍能实现精准定位并可靠构建地图。

Among the second model, it is a stochastic process-enhanced kinematic model that systematically models IMU measurement data as part of its output stream.

This new modeling method is designed to achieve accurate localization and reliable mapping for aggressive motions, even when IMU measurements are saturated during the motion.

通过不同类型的实验对性能进行系统性评估。

总体来看，在经历剧烈振动和高角速度（达到75 rad/s）的情况时仍能实现精确的高频定位（频率范围为4至8千赫兹）以及稳定可靠的环境建图。

此外，在此基础之上我们进行了全面且细致的基准对比。该算法在保持性能的同时展现了优异的时间效率。

最后部分展示了Point-LIO的两个典型应用场景：一个是用于竞技比赛的无人机系统；另一个则是一个具备自主旋转功能的无人机设备。两组案例均具备高风险动作执行能力。

本论文提出了一种创新的方法来解决复杂的优化问题。该方法旨在提升现有算法的效率和鲁棒性，并通过大量实验表明其显著优于传统方法。

在过去的几十年中，在实现直接、密集、主动且精确的深度测量的基础上（即通过激光雷达技术），3D光探测与测距（LiDAR）传感器在自主应用中扮演了越来越关键的角色。具体而言，在SLAM（同步定位与地图构建）领域实现了机器人探索与障碍物检测功能[1-4]；此外，在自动驾驶技术方面也取得了显著进展[5-8]。近年来，在该领域的最新创新成果进一步推动了高性能LiDAR技术向轻量化、高性价比及易用性方向发展[9-10]。这些进步不仅为现有的诸多应用场景提供了技术支持，并且有望在未来带来更多创新性的应用机会——包括智能汽车领域的高级辅助驾驶功能以及复杂环境下的物体识别技术等[11-13]。

在导航任务中使用激光雷达传感器时必须满足的基本条件是确保机器人能实现精确的位置估计以及快速且一致地感知环境，并能实时生成高频率的空间模型。该传感器凭借其极高的测量频率（如每秒数百万次）能够实现快速的距离测量与环境建模功能，并使其能够追踪物体的高速运动状态。

然而，在现有的方法中有一些是基于视觉信息处理的框架结构，在这些框架中每隔一定的时间间隔（例如每1/10秒）就会对帧中的数据进行周期性地处理。

然而，在现实中，LiDAR 点是依次采样于不同时间点；将这些点整合到一个帧内会导致机械运动误差，并对测量结果及定位精度产生负面影响（adversely affect）。

低帧速率还会提升里程计中的延迟并造成可达到带宽的限制。其中里程计带宽（odometry bandwidth）类似于动态系统的带宽（dynamic system），即系统增益降到0.707以下时对应的频率范围。而里程计带宽则决定了该系统能够令人满意地估算运动速度的能力。

在本研究中，我们采用两种核心的创新方法解决这些问题：逐点式状态更新与随机过程提升的运动学模型。具体而言，我们的主要贡献包括：

我们开发了一种单点激光雷达惯性里程计（LIO）框架，在实际采样时间融合激光雷达测得的单个点数据而非将它们整合到帧中。去除单点累积带来的影响消除了帧内的运动畸变，并使得基于接近单点采样率的高精度里程计输出和地图更新成为可能。这一改进使系统能够显著提升对快速运动的跟踪能力。

旨在进一步提升系统带宽在惯性测量单元（IMU）测程范围之外的能力。为此，我们通过构建基于随机过程的数学模型（如文献[14]所示），对IMU测得的数据进行建模。随后，在动态系统的运动学分析中应用这一模型，并将IMU测得的数据视为系统的输出变量。值得注意的是，在某些极端情况下（即当测速器达到其最大测速能力时），尽管存在IMU饱和的可能性，但通过引入基于随机过程的动态补偿方法，在动态系统的运动学分析中仍能准确估计系统的状态参数。

我们将这两种核心技术整合至一个高度协同耦合的LIO平台，并命名为Point-LIO。该平台采用流形上的扩展卡尔曼滤波器（EKF），通过在各自的采样周期内综合处理来自LiDAR点数据和IMU信息来动态更新系统的状态。所开发的解决方案能够在低功耗ARM架构下实现高效的实时状态估计算法

本系统基于新型固态激光雷达，在多样化的复杂真实环境数据中进行了全面测试。测试结果表明：成功实现了Point-LIO算法的运动模糊校正功能；该系统具备优异的定位精度（4-8 kHz）以及超大带宽性能（>150 Hz）。此外，在初始运行阶段即可通过饱和型加速度计测量结果准确估算出物体处于极端动态条件下的运动参数（包括角速度达75 rad s⁻¹以上的情况）。

此外，在系统性对比分析了包括12个不同开放LiDAR数据集在内的各类基准测试后发现,Point-LIO始终展现出与同类产品相当的精度和效率,同时其计算资源消耗量更低,最终验证了其在实际无人机平台上的可行性和实用性

其他文章的安排将按照以下方式进行。

在第2节中,我们涉及相关领域的研究进展

该系统流程在第3节被详细描述。

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

第5节对系统的评估进行了概述

第 6 节展示了对开放数据集进行基准比较的结果。

最后一节详细阐述了系统如何应用于实际无人机操作。

第8部分呈现了研究的总结

2. 相关研究综述

2.1. LiDAR-Inertial Assisted Odometry

最近主要的研究集中在利用LiDAR-odometry and mapping (LOAM) 结构进行3D LiDAR 里程计和建图。这些研究依赖于LOAM结构来整合原始LiDAR点数据到单个扫描中，并提取边缘和其他特征。

然后将提取的特征点与之前的扫描数据进行融合，并以每秒10次（即10 Hz）的速度生成里程计输出；这些最近的扫描结果则被整合进一个小区域的地图中，在这个过程中，该区域的地图会以较慢的速度（每秒一次Hz）来更新相对于LiDAR定位的信息

LOAM中的分隔模块（separation module）实现了对LiDAR扫描（scan）与地图匹配（map）的区分，在多个后续研究项目中得到了广泛应用。例如Lego-LOAM[16]开发了一种基于地面约束的LiDAR匹配算法来提升里程计精度；LINS[17]则将IMU数据与激光雷达配准相结合；此外，在现有文献中还提出了若干其他方法[18,19]，这些方法主要关注于提高计算效率或提升定位精度。

尽管将扫描到扫描与 scan-to-map 之间的分离能够有效地减少里程计所需计算负载（...），但在里程计量算中进行 scan-to-scan 配准（scan-to-scan registration）通常会导致位置漂移积累。

除此之外，在小 FoV 固态 LiDAR中通常不可行的情况下（参考文献[20]），为了应对这些问题（参考文献[11, 21]），直接进行 scan-to-map registration（或 Scan to Local Map registration）已被广泛采用。例如，在基于点云的地图系统中使用 G-ICP 算法（参考文献[22]）、NDT 方法（参考文献[22]）、Surfel-based maps（参考文献[23–26]）以及体素图技术（参考文献[27]）等。

特别是,参考文献[20]报道了一种并行扫描至地图的方法用于解决固态激光雷达的小视场问题.参考号[28]则将IMU测量整合进高效的迭代卡尔曼滤波器框架下的扫描到地图配准过程.

在 scan-to-map 框架中，一个核心挑战是如何保持地图结构以便能够从新增扫描中逐步添加点的同时支持高效查询。为此，《参考文献》[29]引入了一种增量式k-d树数据结构——ikd-Tree。得益于这种高效的增量式空间组织方法，《参考文献》[29]指出该系统能够以每秒10次（适用于旋转LiDAR）和每秒100次（适用于固态LiDAR）的频率实现距离测量与地图构建

该系统在进行地图配准时面临一个显著的问题即其定位精度受激光雷达数据更新频率限制在实际应用中当系统运行速度达到较高水平时可能导致定位精度无法达到预期要求

由于奈奎斯特-香农采样定理的存在，在Lola-SLAM[30]和LLOL[31]的研究中已部分解决了这一问题。具体而言，在接收每个子扫描后，我们将其划分为多个子扫描段，并分别实现了160Hz和80Hz的里程计系统以应对不同的数据处理需求。

相较于现有几种方法（即直接从扫描转换为完整的扫描信息，并分别对[15–19]号区域进行扫完整图以及处理[11,21–26]号区域的子扫信息并将它们整合为全局完整的扫信息[30,31]）而言，在本研究中我们提出了一种基于点的地图定位框架。该系统能够当接收到每一个单独的定位结果时自动完成登记至地图中。该系统不仅可在理论上实现按点采样的定位精度，在实际应用中也可达到每秒4至8千次的定位频率。通过高频状态更新技术的应用，在延迟方面实现了显著提升至微秒级的同时也在频宽方面获得了质的飞跃

运动失真补偿技术：一种基于信号处理的自适应算法设计与实现

如前所述，在现有技术中，激光雷达里程计和测绘工作主要依赖扫描数据（即帧）。这些扫描数据可能受到激光雷达在单个帧内的持续运动所导致的内部运动畸变的影响。为此需采取相应的补偿措施以减少这种畸变的影响。

大多数研究都基于匀速运动模型（constant-velocity motion model）来校正运动畸变（motion distortion），这在现有文献中得到了广泛采用[15,17–19,32–39]。当扫描持续时间较短或物体运动幅度较小的时候，匀速运动假设仍然能提供有效的解决方案。

然而，在无人机特技飞行这类高难度运动中，扫描过程中可能出现速度变化的情况下，等速方法通常会带来里程计较大的漂移甚至可能导致失败。

一种备受推崇的运动补偿方法采用了**连续时间轨迹优化（continuous-time trajectory optimization）**技术[43–45]。例如通过B-Splines[26,40–42]与高斯过程模型相结合的方式实现了这一目标[26,40–42]。通过连续时间轨迹允许评估任何时刻的状态从而能够对每个单独的失真点进行补偿然而这种优化方案具有较高的计算开销通常只能在离线环境中运行尽管存在少量在线实现但其速度限制（如10 Hz）常导致较大的里程计延迟为此一些研究者转而寻求其他解决方案以减少这种延迟

连续时间轨迹所具有的平滑性也抑制了机器人经历高度攻击性运动的可能性，并对其进行了具体说明。

除了上述两种方法之外采用IMU测量技术也是一种有效的运动补偿方式。[29,46] 其他方法通过融合帧内IMU数据与LiDAR位姿信息来消除点云畸变问题。

考虑到IMU测量频率较高（例如达到200赫兹），基于IMU的方法在处理通常机器人运动以及快速滚动无人机运动（如快速滚动的无人机运动）时表现出色[29]（其旋转速度最高可达1242度每秒）。

然而该方法仍受到IMU测量频率和噪声偏差的影响

临时运动补偿的效果最终归因于现有方法中的基于帧的时间积分框架。在我们的系统中，在真实采样时间内我们将每个LiDAR点进行融合。通过彻底地消除导致了运动失真的方式，在我们的系统中从一开始就消除了运动失真。从而避免了先前存在的缺陷

The derivation of inertial measurement techniques involves the systematic approach to understanding and quantifying motion through non-mechanical means.

在实现IMU测量与LiDAR点配准的结合过程中,主要采用松耦合和紧耦合两种主流方法进行数据融合

松耦合方法通过整合 IMU 传感器数据以推导位置姿态的初始估计，并将该初步估计用作后续扫描配准所需起始位置姿态的依据。[15,38,47–49] 相比之下，在紧耦合框架下，IMU 与 LiDAR 数据结合优化能够更精确地捕捉动态环境中的运动信息。

基于紧密耦合方法的实现方案中包含两种具体设计。

采用 EKF 作为基础的方法[17,29,50,51] 以及通过优化策略实现的方法[18,52,53]。

该位姿估计方法利用文献[17,29,50,51]所提出的 EKF 框架，在 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. 系统概览

本系统旨在实现实时环境中资源的优化配置。
它采用先进的算法策略来保证任务分配的高效性。
核心功能体现在自动化的任务调度、动态资源分配以及高容错能力上。

我们设计团队的真实认识到：我们通过科学的方法和严谨的流程进行产品设计和开发，并且始终遵循市场趋势和客户需求的指导原则……以确保产品的高品质。

在LiDAR系统中,每个采集点按照独立的时间序列进行采样;而帧则是基于同步捕获技术实现的同时采样.

IMU获取的数据是由系统进行的测量所得的结果，并非直接的输入信号。

当接收到各个LiDAR条目或IMU数据时（每个LiDAR条目或IMU数据），我们将通过该框架整合这些数据

该系统的设计内容通过图1进行了介绍。

按时间顺序采集的 LiDAR 采样点和 IMU 传感器捕捉的数据被用来更新各自时间戳的状态,从而生成高精度的里程计输出结果,其输出频率通常在4至8千赫兹范围内

特别地，在接收每一个LiDAR点时，在地图上搜索对应的平面。若该点与地图上对应点所处的平面匹配，则利用流形卡尔曼滤波器计算残差以更新系统的状态。优化后的姿态最终会将LiDAR点注册至全局框架并整合进地图，并接着执行下一个测量任务（基于LiDAR数据或IMU信号）。如果没有匹配的平面，则在没有找到对应关系的情况下（即该LIDAR点未找到对应平面），则直接利用卡尔曼滤波器预测的状态将其加入地图。

为同时接纳新注册点并实现高效平面对应搜索的目的,我们采用了增量k-d树结构,即ikd-Tree,其源于FAST-LIO2中的创建。[29]

对于每次IMU采集数据的过程中，在对各个通道执行饱和度评估时会采取分步处理的方式；当某个通道存在饱和标记时，在状态更新阶段该通道的数据将被予以排除。

现代控制系统中，状态估计问题是一项至关重要的技术。

Point-LIO的状态估计问题是一个紧密耦合的流形卡尔曼滤波器。我们对滤波器的基本公式和工作流程进行了简要说明，并建议读者参考文献[55]以获取更深入的理论解释。

4.1. 符号表示法

为便于理解, 我们采用了以下标记.

在此基础上, 我们提出了所定义的两个封装操作, 即\boxplus(‘boxplus’)及其逆运算\boxminus(‘boxminus’)[55]. 这些运算被用于在n维流形上进行系统建模, 并将状态误差参数化于欧几里得空间\mathbb{R}^n.

而且，这些操作可以更紧凑地描述离散时间上的系统状态空间模型。我们建议读者参考参考文献。 [55]更详细的定义和推导；在本文中，我们只关心流形 SO(3) 和 \mathbb{R}^n
\begin{aligned} \boxplus:&\quad\mathscr{M}\times\mathbb{R}^n\to\mathscr{M};\quad\boxminus:\mathscr{M}\times\mathscr{M}\to\mathbb{R}^n \\ \\ \mathrm{SO}(3){:}&\quad\mathbf{R}\boxplus\mathbf{r}=\mathbf{R}\cdot\mathrm{Exp}(\mathbf{r});\quad\mathbf{R}_1\boxminus\mathbf{R}_2=\mathrm{Log}(\mathbf{R}_2^T\cdot\mathbf{R}_1) \\ \\ \mathbb{R}^n{:}&\quad\mathbf{a}\boxplus\mathbf{b}=\mathbf{a}+\mathbf{b};\quad\mathbf{a}\boxminus\mathbf{b}=\mathbf{a}-\mathbf{b} \end{aligned}
其中
\operatorname{Exp}(\mathbf{r})=\mathbf{I}+\sin(\|\mathbf{r}\|)\frac{|\mathbf{r}|}{\|\mathbf{r}\|}+(1-\cos(\|\mathbf{r}\|))\frac{|\mathbf{r}|^2}{\|\mathbf{r}\|^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 之间的笛卡尔积，我们有

\begin{bmatrix}\mathbf{R}\\ \mathbf{a}\end{bmatrix}\boxplus\begin{bmatrix}\mathbf{r}\\ \mathbf{b}\end{bmatrix}=\begin{bmatrix}\mathbf{R}\boxplus\mathbf{r}\\ \mathbf{a}+\mathbf{b}\end{bmatrix},\begin{bmatrix}\mathbf{R}_1\\ \mathbf{a}\end{bmatrix}\boxminus\begin{bmatrix}\mathbf{R}_2\\ \mathbf{b}\end{bmatrix}=\begin{bmatrix}\mathbf{R}_1\boxminus\mathbf{R}_2\\ \mathbf{a}-\mathbf{b}\end{bmatrix} \tag{1}

4.2 运动学模型分析

为了推导该系统模型，在研究过程中我们将其划分为状态转换模型与测量模型的主要组成部分。

4.2.1. State Transition Mechanism

以 IMU 帧（记为 I）为 body 帧，第一个 IMU 坐标系为全局坐标系（记为 G），连续运动学模型为

\begin{aligned} ^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}}=\mathbf{0} \\ \\ \dot{\mathbf{b}}_g&=\mathbf{n}_{\mathrm{b}_g},\dot{\mathbf{b}}_a=\mathbf{n}_{\mathrm{b}_a},{}^I\dot{\boldsymbol{\omega}}=\mathbf{w}_g,{}^I\dot{\mathbf{a}}=\mathbf{w}_a \end{aligned} \tag{2}

在其中

在全局坐标系中，{}^G\mathbf{R}_I 表示惯性坐标系的姿态，在此框架下 {}^G\mathbf{p}_I 和 {}^G\mathbf{v}_I 分别代表位置与速度参数。

在全局坐标系中占据位置的矢量是重力矢量。

\mathbf{b}_g 和 \mathbf{b}_a 分别是由两个高斯噪声项 \mathbf{n_{b_g}} \approx \mathcal{N}(\boldsymbol{0}, \mathcal{Q}_{b_g}) 和 \mathbf{n_{b_a}} \approx \mathcal{N}(\boldsymbol{0}, \mathcal[Q]{_}{ba}) 所受的影响而形成的随机游走型惯性测量单元（IMU）偏差。符号⌊a⌋被定义为属于ℝ³空间中向量a的斜对称叉积矩阵。

在主体坐标系（称为IMU坐标系）中，\omega^{I} 和 a^{I} 被定义为系统的角速度和加速度。

参考文献建议采用一种机器人运动（角速度 {}^I{\boldsymbol{\omega}} 和线加速度 {}^I{\boldsymbol{a}}）始终可被视为信号集或样本集合；这让我们得以通过基于随机过程的方法对机器人运动进行统计描述。

此外，在参考文献[14]中建议考虑到执行器存在的延迟这一因素，在机器人系统运动学建模时应特别注意其对系统性能的影响。考虑到执行器存在的延迟这一因素，在机器人系统运动学建模时应特别注意其对系统性能的影响。在这样的情况下，尽管角速度与加速度的变化呈现相对平稳的状态（ smoothness），但基于N阶积分器（N-th order integrator）的随机过程特性，在实际应用中能够有效满足需求。

特别地，在本研究中,我们采用基于高斯噪声分布的低通滤波器（first-order integrator models）来分别用于建模机器人自身的角速度{}^I{\boldsymbol{\omega}}和线性加速度{}^I{\boldsymbol{a}}的变化过程

随后，在每个时间步长 k 处，连续模型 (2) 被离散化处理。其中符号 \Delta t_k 表示当前时间间隔，在此定义为相邻两次测量（IMU 数据或 LiDAR 点）之间的时间间隔。

基于假设输入在时间间隔 Δt_k 内维持恒定值，在连续模型 (2) 的基础上进行离散化处理后会导出如下的差分方程形式：

\mathbf{x}_{k+1}=\mathbf{x}_k\boxplus(\Delta t_k\mathbf{f}(\mathbf{x}_k,\mathbf{w}_k)) \tag{3}

其中 manifold \mathcal{M}、函数 \mathbf{f}、状态 \mathbf{x} 和过程噪声 \mathbf{w} 被定义为
\triangleq 表示
其中 \mathcal{Q}=\mathrm{diag}(\mathcal{Q}_{\mathrm{b_{g}}},\mathcal{Q}_{\mathrm{b_{a}}},\mathcal{Q}_{\mathrm{g}},\mathcal{Q}_{\mathrm{a}}) 是 process noise 的 covariance matrix。

请稍等片刻，请您稍等片刻

特别提醒：请立即处理

紧急情况：请立即采取行动

在FAST-LIO系统中，IMU的数据量被视为系统的输入端

4.2.2节的测量框架

该系统设有两个测量点,每个测定点要么是LiDAR测定点,要么是IMU信息包（具体包括角速度计和加速度计的读数信息）。

这两个测量值一般情况下在不同时间段被系统采集，并因此我们需要分别对他们进行建模分析

假设LiDAR坐标系与主体（即IMU）坐标系一致，并且已经完成外部坐标系的标定，则LiDAR点^{I}\mathbf{p}_{\mathrm{m}_{k}}在本地IMU坐标系中的真实位置^{I}\mathbf{p}_{k}^{\mathrm{gt}}是未知的，并受到附加高斯噪声\mathbf{n}_{\mathrm{L}_k}\approx\mathcal{N}(\mathbf{0},\mathscr{R}_{\mathrm{L}_k})的影响。由于这些噪声通常源于传感器的工作环境或数据采集过程中的偶然因素，在实际应用中需要通过精确的标定和持续的数据处理来减少其影响。

基于未知但真实的IMU位姿{}^G\mathbf{T}_{I_k}=({}^G\mathbf{R}_{I_k},{}^G\mathbf{p}_{I_k})经过投影至全局框架后,该实际点应在地图中对应的微小平面块内(如图2所示),从而满足以下条件

其中 ^{G}\mathbf{u}_k 被定义为对应于该工作平面（work plane）的法线矢量，在此平面上的所有点均满足条件。

请注意，在状态向量\mathbf{x}_k中嵌入了变换矩阵^G\mathbf{T}_{I_k}。(6)为了更好地捕捉系统的动态特性，在状态向量\mathbf{x}_k上施加了隐式测量模型。

（图 2展示了使用LiDAR技术实现的地图上点与蓝色平面法向量的精确对准过程。其中，^{G}{\mathbf{q}}_i和\mathbf{u}_i分别代表地图上的一个点及其所在蓝色平面的法向量。）

该系统通过双组分传感器实现运动参数监测：一个是由角速率测量（^{I}\mathbf{\omega}_m）构成的测速子系统与一个由加速度测量（^{I}\mathbf{a}_m）构成的测加系统共同完成对运动状态的感知任务；其中测速子系统的输出结果为：$$
\begin{bmatrix}^I\mathbf{0}_{\mathrm{m}_k}\ ^{I\mathbf{a}_{\mathrm{m}_k}\end{bmatrix}=\underbrace{\begin{bmatrix}}I\mathbf{\omega}k+\mathbf{b}{\mathrm{g}k}+\mathbf{n}{\mathrm{g}_k}\ ^I\mathbf{a}k+\mathbf{b}{\mathrm{a}k}+\mathbf{n}{\mathrm{a}k}\end{bmatrix}}{\mathbf{h}_I(\mathbf{x}k,\mathbf{n}{I_k})} \tag*{(7)}

其中 $\bm{n_g}\approx\mathcal{N}(\boldsymbol{\mu=0}, \mathscr{R}_g)$ 和 $\bm{n_a}\approx\mathcal{N}(\boldsymbol{\mu=0}, \mathscr{R}_a)$ 分别遵循均值为零的正态分布。 就整体而言，在这个公式$\mathbf{n}_I=[\mathbf{n}_{g}^{T}\quad \mathbf{n}_{a}^{T}]^{T}\approx \mathcal{N}(\boldsymbol{0}, \,\,\, \digamma _{\text{I}} )=\,\,\, \digamma _{\text{I}} = \texttt{diag}( \digamma _{\text g }, \digamma _{\text a })$中反映了IMU设备的工作状态及其相应的误差特性 如图所示，在状态方程(2)中存在两个分离的状态变量 ω 和 b_g（及其对应的其他类似的状态变量），这些变量如今与相应的角速度测量结果 ω_m（或加速度测量结果 a）之间存在关联。 基于以下分析可知, 系统的测量模型可表示为以下简洁的形式表达：

\begin{gathered} 0 = \mathbf{h}{\mathrm{L}}(\mathbf{x}k,{}^I\mathbf{p}{\mathrm{m}k},\mathbf{n}{L_k}) \ \ \begin{bmatrix}{}^I\omega{\mathrm{m}k}\ {}^I\mathbf{a}{\mathrm{m}_k}\end{bmatrix} = \mathbf{h}_I(\mathbf{x}k,\mathbf{n}{I_k}) \end{gathered} \tag{8}

4.3. Extended Kalman Filter

紧密耦合的 EKF 应用于 Point-LIO 的状态估算。本节将详细阐述 EKF 在实现过程中的工作流程。

紧密耦合的 EKF 应用于 Point-LIO 状态估算。本节将详细阐述其在实现过程中的工作流程。

4.3.1 状态传递机制

本节主要讨论了残差计算的相关机制及其在算法中的应用情况

基于卡尔曼传播模型（9），我们采用预测位姿的方法进行LiDAR测量。随后将测得的LiDAR点^{I}\mathbf{p}_{\mathrm{m}_{k+1}}投影至全局坐标系，并从该区域附近的ikd-Tree地图中检索出距离不超过5米的五个最近邻

通过找到的近邻点匹配一个具有法向量 ^{G}\mathbf{u}_{k+1} 和质心 ^{G}\mathbf{q}_{k+1} 的局部平面补片，在此基础之上参考公式（6）以及图 2 进行建模。

当最近连续五个LiDAR测量点未落在拟合平面上时（具体而言是每个点与该平面的距离均超过0.1米），则将其当前测量值直接加入地图数据中进行更新；反之，在局部平面已正确识别的情况下，则通过方程组（8）、（9）、（10）计算出该区域内的残差\boldsymbol{\varepsilon}_i^{l,k}=\left[\varepsilon_{i,x}^{l,k}, \varepsilon_{i,y}^{l,k}, \varepsilon_{i,z}^{l,k}\right]^T。数学表达式如下：

\varepsilon_{i,x}^{l,k}=x_i - h_x(x_k, p_m, n_l) \\ \varepsilon_{i,y}^{l,k}=y_i - h_y(x_k, p_m, n_l) \\ \varepsilon_{i,z}^{l,k}=z_i - h_z(x_k, p_m, n_l)

其中h_x、h_y、h_z分别代表在xyz轴方向上的位移量。

其中 \delta\mathbf{x}_{k+1}=\mathbf{x}_{k+1}\boxminus\hat{\mathbf{x}}_{k+1} 其中 \mathbf{x}_{k+1} 是时间步 k+ 1 处状态向量的真实值，并且

\begin{aligned} \mathbf{H}_{L_{k+1}} &=\left.\frac{\partial\mathbf{h}_L(\mathbf{\hat{x}}_{k+1}\boxplus\delta\mathbf{x},{}^I\mathbf{p}_{m_{k+1}},\mathbf{0})}{\partial\delta\mathbf{x}}\right|_{\delta\mathbf{x}=\mathbf{0}} \\ =&\begin{bmatrix}-^G\mathbf{u}_{k+1}^T{}^G\mathbf{\hat{R}}_{I_{k+1}}\lfloor{}^I\mathbf{p}_{m_{k+1}}\rfloor&{}^G\mathbf{u}_{k+1}^T&\mathbf{0}_{1\times18}{}\end{bmatrix} \\ \\ \mathbf{D}_{L_{k+1}} &=\frac{\partial\mathbf{h}_L(\mathbf{\hat{x}}_{k+1},^I\mathbf{p}_{m_{k+1}},\mathbf{n})}{\partial\mathbf{n}}|_{\mathbf{n}=\mathbf{0}}=-^G\mathbf{u}_{k+1}^T{}^G\mathbf{\hat{R}}_{I_{k+1}} \end{aligned} \tag{13}

对于IMU测速仪的数据处理流程中,我们首先通过观察当前测速数据与额定范围之间的差异来判断IMU各个通道是否处于饱和状态.当检测到某个通道的数据偏差过小时,则放弃该方向的测速数据而不更新系统状态.随后,我们将整合所有未饱和测速方向的数据,并依据公式（7）计算出IMU残差(\mathbf{r}_{I_{k+1}}).具体而言,我们综合所有六个方向的数据来进行这一计算过程.

其中 \delta\mathbf{x}_{k+1} 表示第 k+1 时间步的状态向量增量，在该时刻系统的实际状态 \mathbf{x}_{k} 的更新结果 \hat{\mathbf{x}}_{k} 的基础上通过减算子运算得到。具体而言，

\delta\mathbf{x}_{k} = \mathbf{x}_{k}\boxminus\hat{\mathbf{x}}_{k}

这一操作用于衡量真实状态与估计状态之间的偏差程度，并通过非线性变换模型 \mathbf{h}_2(\cdot,\cdot) 对噪声变量 \mathbf{n} 进行敏感度分析以获得系统响应特性信息 C_y 和 C_u。
如文献[9]所示，

\begin{aligned} \mathbf{H}_{I_k} &= \frac{\partial \mathbf{h}_2(\hat{\mathbf{x}}_k, \mathbf{n})}{\partial \delta\mathbf{x}} \Bigg|_{\delta\mathbf{x}=0}, \\ \\ \mathbf{D}_{I_k} &= \frac{\partial \mathbf{h}_2(\hat{\mathbf{x}}_k, \delta\boldsymbol{\xi})}{\partial \delta\xi} \Bigg|_{\delta\xi=0}. \end{aligned}

由 LiDAR 点测量（编号 12）或 IMU 测量（编号 14）所得的残差与状态 \mathbf{x}_{k+1} 以及对应的测量噪声通过以下关系式进行关联：

\mathbf{r}_{k+1}\approx\mathbf{H}_{k+1}\delta\mathbf{x}_{k+1}+\mathbf{D}_{k+1}\mathbf{n}_{k+1},\quad\mathbf{n}_{k+1}\approx\cal N(\boldsymbol 0,\,\Sigma_{k+1}) \tag{B.3}

其中，在进行 LiDAR 点位测量时，
我们定义如下：

\boldsymbol{\rho }_{t}=f(\boldsymbol{\omega }_{t}, \boldsymbol{\nu }_{t}), \quad \mu _{\text {t}}=g(\boldsymbol{\omega }_{t}, \boldsymbol{\nu }_{t}),

这些量均来源于 LiDAR 点位测量的结果。
而对于 IMU 测量而言，
我们同样采用上述方法进行计算，
所得结果与实际值之间的误差不超过 2 cm。

4.3.3 State Update

(9) 所描述的传播状态 xk₁ 和 (10) 所描述的协方差 P̂ k¹ 对被估计的状态 xk₁ 施加先验高斯分布,如前述