摘要——本文提出一种计算高效、鲁棒的激光雷达惯性里程计框架。我们使用紧耦合的迭代扩展卡尔曼滤波器将激光雷达特征点与IMU数据融合，以允许在发生退化的快速运动、噪声或者杂乱环境中进行稳健导航。为了在出现大量观测情况下降低计算负载，我们提出了一个计算卡尔曼增益的新公式。新公式的计算量取决于状态维度，而不是测量维度。所提出的方法及实现在各种室内和室外环境中进行了测试。在所有测试中，我们的方法都能实时产生可靠的导航结果：在四旋翼机载计算机上运行，在一个scan中融合了超过1200个有效的特征点且在25ms内完成IEKF步骤的所有迭代。https://github.com/hku-mars/FAST_LIO
一、介绍
本文贡献如下：
1）为了应对发生退化的快速运动、噪声或杂乱环境，我们采用了紧密耦合的迭代卡尔曼滤波器来将激光雷达特征点与IMU测量融合。我们提出了一种形式化的反向传播过程来补偿运动失真；
2）为了降低由大量激光雷达特征点引起的计算负荷，我们提出了一个新的卡尔曼增益计算公式，并证明了它与传统的卡尔曼增益公式的等价性。新公式的计算复杂度取决于状态维度而不是测量维度。
3）我们将我们的方法应用到一个快速而强大的激光雷达惯性里程计软件包中。该系统能够在小型四旋翼机载计算机上运行。
4）我们在各种室内和室外环境中进行了实验，并进行了实际的无人机飞行测试（图1），以验证系统在存在快速运动或强烈振动噪声时的鲁棒性。
后文组织如下：
II、相关研究工作介绍；
III、我们在第三节中概述了完整的系统管道和每个关键组件的详细信息。
IV、实验。
V、总结。
二、相关工作
A、激光里程计和建图
B、松耦合激光惯导里程计
IMU测量通常用于缓解无特征环境中的激光雷达退化问题。松耦合激光雷达惯性里程计（LIO）方法通常分别处理激光雷达和IMU测量结果，并在稍后融合其结果。例如，IMU辅助LOAM[8]将从IMU测量中集成的姿态作为激光雷达扫描配准的初始估计。Zhen等人[11]使用误差状态EKF融合IMU测量和激光雷达测量的高斯粒子滤波器输出。Balazadegan等人[12]添加了IMU重力模型来估计6自由度的自我运动，以帮助激光雷达扫描配准。Zuo等人[13]使用多状态约束卡尔曼滤波器（MSCKF）将扫描配准结果与IMU和视觉测量融合。松耦合方法的一个常见过程是通过匹配新的扫描来获得姿态测量，然后将姿态测量与IMU测量融合。扫描配准和数据融合之间的分离减少了计算负载。然而，它忽略了系统的其他状态（例如速度）和新帧的姿态之间的相关性。此外，在无特征环境的情况下，扫描配准可能在某些方向上退化，并在后期导致不可靠的融合。
C、紧耦合激光惯导里程计
与松耦合方法不同，紧耦合激光雷达惯性里程计方法通常将激光雷达的原始特征点（而不是扫描配准结果）与IMU数据融合。有两种主要的方法来实现紧密耦合的LIO：基于优化和基于滤波器。Geneva等人[14]使用具有IMU预积分约束[15]和来自激光雷达特征点的平面约束[16]的图优化。最近，Ye等人[17]提出了LIOM包，该包使用类似的图优化，但基于边缘和平面特征。对于基于滤波器的方法，Bry[18]使用高斯粒子滤波器（GPF）来融合IMU和平面2D激光雷达的数据。这种方法也被用于波士顿动力公司的Atlas类人机器人。由于粒子滤波器的计算复杂度随着特征点的数量和系统维数的增加而快速增长，卡尔曼滤波器及其变体通常更受欢迎，如扩展卡尔曼滤波器[19]、无迹卡尔曼滤波器[20]和迭代卡尔曼滤波器[21]。
我们的方法属于紧密耦合的方法。我们采用类似于[21]的迭代扩展卡尔曼滤波器来减轻线性化误差。卡尔曼滤波器（及其变体）具有时间复杂性O m 2，其中m是测量维度[22]，这可能导致在处理大量激光雷达测量时的计算负载非常高。固定下采样将减少测量次数，但代价是信息丢失。[21]通过类似于[9]的提取和拟合地平面来减少测量次数。然而，这并不适用于地平面可能并不总是存在的航空应用。
III、方法论
A、框架概览
本文使用表I的符号。
本文的工作流概览如下：
激光雷达输入被送到特征提取模块中以获得平面和边缘特征。然后，提取的特征和IMU测量值被输入我们的状态估计模块，用于10Hz−50Hz的状态估计。然后，估计的位姿将特征点注册到全局坐标系下，并将这些特征点融合到特征地图。更新的地图在下一步中用于匹配新的特征点。
B、系统描述
1）广义加/广义减操作符：
设M是考虑维数n的流形(e.g., M = SO(3))。因为流型在局部同胚于Rn，我们可以通过两个封装算子广义加和广义减建立从M上的局部邻域到其切线空间Rn的双射映射
2）连续模型：
3）离散模型
4）激光测量的预处理
激光雷达测量是其局部框架中的点坐标。由于原始激光雷达点是以非常高的速率（例如，200kHz）采样的，因此通常不可能在接收到每个新点后对其进行处理。一种更实用的方法是将这些点累积一段时间，然后一次处理所有这些点。在FAST-LIO中，最小累积间隔设置为20ms，导致高达50Hz的全状态估计（即里程计输出）和地图更新，如图所示。第2（a）段。这样一组累积的点被称为扫描，处理它的时间表示为tk（见图第2（b）段）。从原始点中，我们提取具有高局部平滑度[8]的平面点和具有低局部平滑度的边缘点，如[10]所示。假设特征点的数量为m，每个特征点在时间ρj∈（t k−1，t k]）采样，并表示为L j p f j，其中Lj是时间ρj时的激光雷达局部帧。在激光雷达扫描期间，也有多个IMU测量值，每个测量值在时间τi∈[t k−1，t k]处采样，各自的状态如（2）所示xi。
C、状态估计
为了估计方程2中的状态，我们使用迭代扩展卡尔曼滤波。此外，我们在[23，24]中描述了状态估计的正切空间中的估计协方差。
- Tightly-Coupled LiDAR-inertial FAST-LIO Iterated Odometrytightly-coupled lidar-inertial fast-lio iterated fast-lio lidar-inertial tightly-coupled iterated odometry estimation iterated extended filter learned_inertial_model_odometry inertial odometry learned_inertial_model_odometry flow-based odometry approach scanner