FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package byTightly-Coupled Iterated Kalman Filter

【摘要】

        本文提出了一种计算效率高、鲁棒性强的激光雷达-惯性里程计框架。我们使用紧密耦合的迭代扩展卡尔曼滤波器将激光雷达特征点与IMU数据融合，以允许在快速运动、有噪声或杂乱的环境中进行鲁棒的导航。为了降低存在大量测量值时的计算负荷，我们提出了一个新的计算卡尔曼增益的公式。新公式的计算负载取决于状态维度，而不是测量维度。所提出的方法及其实现已在各种室内外环境中进行了测试。在所有的测试中，我们的方法实时产生可靠的导航结果：在四旋翼计算机上运行，它在扫描中融合超过1200个有效特征点，并在25ms内完成iEKF步骤的所有迭代。我们的代码可以在Github上开放使用。

I. INTRODUCTION

        即时定位和建图(SLAM)是移动机器人的一个基本前提，如无人机(uav)。视觉惯性里程计(VO)，如立体声VO[1]和单目VO[2,3]，由于它们的轻量级和低成本，被广泛用于移动机器人。虽然视觉传感器能提供了丰富的RGB信息，但可视化解决方案缺乏直接的深度测量，需要大量的计算资源来重建三维环境以进行轨迹规划。此外，它们对照明条件非常敏感。光探测和测距(LiDAR)传感器可以克服所有这些困难，但对于小型移动机器人来说，成本太高（而且笨重）。

        固态激光雷达最近成为激光雷达发展的主要趋势，如基于微机电系统(MEMS)扫描[4]和旋转棱镜[5]的激光雷达。这些激光雷达具有非常高的成本效益（成本范围类似于全局快门相机），重量轻（可由小型无人机携带），而且性能高(可产生远程和高精度的主动和直接3D测量)。这些特性使得这种激光雷达适用于无人机，特别是工业无人机，它们需要获得精确的环境三维地图（例如，空中地图），或者可能在具有严重光照变化的杂乱环境中运行（例如，灾后搜索和检查）。

        尽管固态激光雷达具有巨大的潜力，但它给SLAM带来了新的挑战：

        1)激光雷达测量中的特征点通常是环境中的几何结构如线和平面。当无人机在没有很强的结构特征的杂乱环境中运行时，基于激光雷达的解决方案很容易退化。当激光雷达有一个小的视场时FOV，这个问题更加明显。

        2)由于沿扫描方向的高分辨率，激光雷达扫描通常包含许多特征点（例如，几千个特征点）。虽然这些特征点不足以可靠地确定退化时的姿态，但将如此大量的特征点与IMU测量紧密融合需要大量的计算资源，而这是无人机机载计算机负担不起的。

        3)由于激光雷达采样点与少量激光/接收对顺序采样，扫描中的激光点总是在不同的时间采样，导致运动失真(畸变），从而显著降低扫描配准[6]。无人机螺旋桨和电机的恒定旋转也给IMU的测量带来了显著的噪声。

        为了使激光雷达导航适用于无人机等小型移动机器人，我们提出了FAST-LIO，一种计算效率高、鲁棒性的激光雷达惯性里程计软件框架。更具体地说，我们的贡献如下：

        1)为了应对发生退化的快速运动、噪声或杂乱的环境，我们采用紧密耦合迭代卡尔曼滤波器将激光雷达特征点与IMU测量值融合。我们提出了一种整齐的反向传播过程来补偿运动失真；

        2)降低大量激光雷达特征点造成的计算负荷，提出了一个计算卡尔曼增益的新公式，并证明了它与传统卡尔曼增益公式的等价性。新公式的计算复杂度取决于状态维数，而不是测量维数。

        3)我们将我们的公式实现到一个快速和健壮的激光雷达-惯性里程计程序中。该系统能够在小型四旋翼计算机上运行。

        4)我们在各种室内外环境下进行实验，并通过实际的无人机飞行试验（图1），以验证该系统在存在快速运动或强烈振动噪声时的鲁棒性。

【II. RELATED WORKS 】

         现有的激光雷达激光工作非常广泛。在这里，我们将我们的回顾限制在最相关的工作：纯激光雷达的里程计和建图，松散耦合和紧密耦合的激光雷达-惯性融合方法。 

A. LiDAR Odometry and Mapping 

        Besl等人[6]提出了一种扫描配准的迭代最近点（ICP）方法，为激光雷达里程计奠定了基础。ICP对密集的3D扫描表现良好。然而，对于激光雷达测量的稀疏点云，ICP所要求的精确点匹配很少存在。为了解决这个问题，Segal等人[7]提出了一个基于点到平面距离的广义GICP。然后Zhang等人[8]将这种ICP方法与点边距离结合起来，开发了一个激光雷达里程计和建图（LOAM）框架。此后，许多LOAM的变体被开发出来，如LeGO-LOAM [9]和LOAM-Livox [10]。虽然这些方法适用于结构化环境和大型视场的激光雷达，但它们非常容易受到无特征环境或小型视场激光雷达[10]的影响。

B. Loosely-coupled LiDAR-Inertial Odometry 

         IMU测量通常用于减轻激光雷达在无特征环境中的退化问题。松散耦合的激光雷达-惯性里程法（LIO）方法通常分别处理激光雷达和IMU的测量结果，并在稍后融合它们的结果。例如，IMU辅助的LOAM [8]将从IMU测量中积分的姿态作为激光雷达扫描配准的初始估计。Zhen等人[11]利用误差状态EKF融合了IMU测量值和激光雷达测量值的高斯粒子滤波器输出。巴拉扎德根等人的[12]添加了IMU-重力模型来估计6-DOF的自我运动，以帮助激光雷达扫描配准。Zuo等人的[13]使用多状态约束卡尔曼滤波器（MSCKF）将扫描配准结果与IMU和视觉测量结果相融合。

        松散耦合方法的一个常见过程是通过配准一个新的scan来获得姿态测量，然后融合使用IMU测量值的姿态测量。扫描配准和数据融合之间的分离降低了计算负荷。然而，它忽略了系统的其他状态（例如，速度）和新扫描的姿态之间的相关性。此外，在无特征环境下，扫描配准可能会在一定方向上退化，并在后期导致不可靠的融合。

C. Tightly-coupled LiDAR-Inertial Odometry

         与松散耦合的方法不同，紧密耦合的激光雷达-惯性里程法通常是将激光雷达的原始特征点（而不是扫描配准结果）与IMU数据相融合。紧密耦合的LIO主要有两种方法：基于优化的和基于滤波器的。Geneva等人[14]使用IMU预积分约束[15]和平面约束[16]。最近，Ye等人[17]提出了LIOM包，该包使用类似的图优化，但基于边缘和平面特征。对于基于滤波器的方法，Bry [18]使用高斯粒子滤波器（GPF）来融合IMU和平面二维激光雷达的数据。该方法也被用于波士顿动力学图谱的人形机器人。由于粒子滤波器的计算复杂度随着特征点数和系统维数的增加而快速增长，卡尔曼滤波器及其变体通常更可取，如扩展卡尔曼滤波器EKF[19]、无迹卡尔曼滤波器UKF[20]和迭代扩展卡尔曼滤波器IEKF[21]。        

        我们的方法属于紧密耦合的方法。我们采用一种类似于[21 LINS]的迭代扩展卡尔曼滤波器来减轻线性化误差。卡尔曼滤波器（及其变体）的时间复杂度为Om2，其中m是测量维数[22 On computational complexity reduction methods for kalman fifilter extensions]，这可能导致在处理大量激光雷达测量时产生非常高的计算负载。朴素直接的降采样将减少测量的数量，但以信息损失为代价。[21]通过提取和拟合类似于[9]的地平面来减少测量次数。然而，这并不适用于地面平面可能并不总是存在的空中无人机的应用。

【III. METHODOLOGY】

A. Framework Overview  

        本文将使用表一所示的符号。我们的工作流程概述如图2所示。将激光雷达输入输入到特征提取模块，以获得平面和边缘特征。然后将提取的特征和IMU测量值输入我们的状态估计模块，在10Hz−50Hz下进行状态估计。然后，估计的姿态将特征点配准到全局帧中，并将它们与迄今为止构建的特征点地图合并。更新后的地图最终将用于在下一步中配准进一步的新点云。

 B. System Description

操作符   boxplus/   boxminus

        令M记作n维的流形，即，因为流形是关于n维向量空间局部同态(homeomorphic)的，我们建立一个双射映射从M局部邻域到其切空间通过两个封闭操作符