一、论文

• 研究领域：激光雷达惯性测距框架
• 论文：FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter

• IEEE Robotics and Automation Letters, 2021

• 香港大学火星实验室

• 论文链接
• 论文github

---

二、论文概要

2.1 主要思路

2.2 具体实现

2.3 实验设计

三、论文全文

FAST-LIO：一个快速、鲁棒的紧耦合迭代卡尔曼滤波器LiDAR惯性里程计包

• 摘要

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

• 提出了一个新的公式来计算卡尔曼增益。新公式的计算量取决于状态维而不是测量维。
• 所提出的方法及其实现在各种室内和室外环境中进行测试

• INTRODUCTION

同时定位和地图绘制（SLAM）是移动的机器人（如无人机（UAV））的基本前提。视觉（惯性）里程计（VO），例如立体式VO [1]和单目镜VO [2，3]，由于其重量轻且成本低，通常用于移动的机器人。虽然提供丰富的RGB信息，视觉解决方案缺乏直接的深度测量，并且需要大量的计算资源来重建3D环境以进行轨迹规划。此外，它们对光照条件非常敏感。光探测和测距（LiDAR）传感器可以克服所有这些困难，但对于小型移动的机器人来说成本太高（而且体积太大）。

固态LiDAR最近成为LiDAR发展的主要趋势，例如基于微机电系统（MEMS）扫描[4]和旋转棱镜[5]的那些。这些LiDAR具有非常高的成本效益（在类似于全局快门相机的成本范围内），重量轻（可以由小型无人机携带），并且具有高性能（产生主动和直接的远程和高精度的3D测量）。这些特征使得此类LiDAR对于UAV（尤其是工业UAV）是可行的，其需要获取环境的准确3D地图（例如，空中测绘）或可在具有严重照明变化的杂乱环境中操作（例如，灾后搜索和检查）。

尽管潜力巨大，但固态LiDAR给SLAM带来了新的挑战：

1）LiDAR测量中的特征点通常是几何结构（例如，边缘和平面）。当UAV在没有强特征的杂乱环境中操作时，基于LiDAR的解决方案容易退化。当LiDAR具有较小的FoV时，这个问题更加明显。

 2)由于沿着扫描方向的高分辨率，LiDAR扫描通常包含许多特征点（例如，几千）。虽然这些特征点不足以在退化的情况下可靠地确定姿态，但是将如此大量的特征点紧密地融合到IMU测量需要UAV机载计算机负担不起的巨大计算资源。

3)由于LiDAR使用几个激光器/接收器对顺序地采样点，因此扫描中的激光点总是在不同时间采样，导致运动失真，这将显著降低扫描配准[6]。UAV螺旋桨和马达的恒定旋转也将显著的噪声引入IMU测量。

为了使激光雷达导航可行的小型移动的机器人，如无人机，我们提出了FAST-LIO，计算效率高，鲁棒的激光雷达惯性测距包。具体而言，我们的贡献如下：

1)为了科普快速运动，嘈杂或混乱的环境，退化发生，我们采用了一个紧耦合迭代卡尔曼滤波器融合LiDAR特征点与IMU测量。我们提出了一个正式的反向传播过程来补偿运动失真;

2)为了降低大量的LiDAR特征点所造成的计算负荷，我们提出了一个新的公式计算卡尔曼增益，并证明其等价于传统的卡尔曼增益公式。新公式的计算复杂度取决于状态维度而不是测量维度。

3)我们实现我们的配方到一个快速和强大的LiDAR惯性里程计软件包。该系统能够在小型四旋翼机载计算机上运行。

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

其余文件组织如下：在第节中。

第二部分，讨论了相关的研究工作。

第三部分，我们在第节中给予了完整的系统管道和每个关键组件的详细信息。

第四，实验

第五部分是结论

• RELATED WORKS

关于LiDAR SLAM的现有工作是广泛的。在这里，我们将我们的评论限制在最相关的作品上：仅LiDAR测距和映射，松耦合和紧耦合LiDAR惯性融合方法。

 A. LiDAR Odometry and Mapping

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

FOV雷达是指视场角（Field of View）雷达，是一种用于探测目标的雷达系统。视场角是指雷达波束所能够覆盖到的区域大小，通常以角度来衡量。FOV雷达可以通过改变天线的位置和方向来控制其视场角，从而实现对特定区域的探测。

B. Loosely-coupled LiDAR-Inertial Odometry

IMU测量通常用于减轻无特征环境中的LiDAR退化问题。松散耦合的LiDAR-惯性测距（LIO）方法通常分别处理LiDAR和IMU测量结果，并在稍后融合它们的结果。例如，IMU辅助LOAM [8]将从IMU测量积分的姿态作为LiDAR扫描配准的初始估计。Zhen等人[11]使用误差状态EKF融合IMU测量结果和LiDAR测量结果的高斯粒子滤波器输出。Balazadegan等人[12]添加IMU重力模型以估计6-DOF自我运动，以辅助LiDAR扫描配准。Zuo等人[13]使用多状态约束卡尔曼滤波器（MSCKF）将扫描配准结果与IMU和视觉测量融合。松耦合方法的常见过程是通过配准新的扫描来获得姿态测量，然后将姿态测量与IMU测量融合。扫描配准和数据融合之间的分离减少了计算负荷。然而，它忽略了系统的其他状态（例如，速度）和新扫描的姿态。此外，在无特征环境的情况下，扫描配准可能在某些方向上退化，并且在后期阶段导致不可靠的融合。

C. Tightly-coupled LiDAR-Inertial Odometry

与松耦合方法不同，紧耦合LiDAR-惯性测距方法通常将LiDAR的原始特征点（而不是扫描配准结果）与IMU数据融合。紧密耦合LIO有两种主要方法：基于优化和基于过滤器。日内瓦等人[14]使用具有IMU预积分约束[15]和平面约束的图形优化。最近，Ye et al.[17]提出了LIOM包，它使用类似的图形优化，但基于边缘和平面特征。对于基于滤波器的方法，Bry [18]使用高斯粒子滤波器（GPF）来融合IMU和平面2D LiDAR的数据。这种方法也被用于Boston Dynamics Atlas人形机器人。由于粒子滤波器的计算复杂度随着特征点的数量和系统维度的增加而快速增长，因此卡尔曼滤波器及其变体通常更受欢迎，例如扩展卡尔曼滤波器[19]，无迹卡尔曼滤波器[20]和迭代卡尔曼滤波器[21]。

我们的方法属于紧密耦合方法。我们采用类似于[21]的迭代扩展卡尔曼滤波器来减轻线性化误差。卡尔曼滤波器（及其变体）的时间复杂度为O (m^2)，其中m是测量维度[22]，当处理大量LiDAR测量时，这可能导致非常高的计算负载。朴素的下采样将减少测量的数量，但以信息丢失为代价。[21]通过提取和拟合类似于[9]的地平面，减少了测量次数。然而，这不适用于地平面不总是存在的空中应用。

• 紧密耦合方法
• 采用类似于[21]的迭代扩展卡尔曼滤波器来减轻线性化误差
• 卡尔曼滤波器（及其变体）的时间复杂度为O (m^2)，其中m是测量维度

• METHODOLOGY

框架概述

本文将使用表I所示的符号。我们的工作流程概述如图2.

LiDAR输入被馈送到特征提取模块中以获得平面和边缘特征。然后，提取的特征和IMU测量值被馈送到我们的状态估计模块中，以10Hz−50Hz进行状态估计。然后，估计的姿势将特征点配准到全局帧，并将它们与迄今为止构建的特征点图合并。更新后的地图最终用于在下一步骤中配准另外的新点。

1、LiDAR输入被送到特征提取模块中以获得平面和边缘特征;

• lidar input --> 点积累 --> 特征提取 --> planar and edge 

2、提取的特征和IMU测量值被送到状态估计模块中，以10Hz−50Hz进行状态估计;

3、估计的姿势将特征点配准到全局帧，并将它们与迄今为止构建的特征点图合并；

4、更新后的地图最终用于在下一步骤中配准另外的新点。

System Description