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维的流形，即M=SO(3)，因为流形是关于n维向量空间局部同态(homeomorphic)的，我们建立一个双射映射从M局部邻域到其切空间通过两个封闭操作符

M=SO(3): R\, \boxplus\, r=RExp(r); \! R_1\boxminus R_2 = Log(R_2^TR_1)

上式的Exp()表示指数映射， Log()表示对数映射。 对于混合流形的计算，有

对于上述定义，容易证明：

2) Continuous model:

假设一个IMU以一个已知的外部参数^IT_{L}=\left (^IR_{L} ,^Ip_{L} \right )刚性地附着在激光雷达上，IMU坐标系记作I系，以它为基体base坐标系则有以下的运动学模型：

其中G表示全局坐标系，即第一帧的IMU坐标系。g为未知的重力加速度向量表示在G坐标系中，a和\omega为IMU的测量值，n为IMU测量值的高斯白噪声，b为IMU的bias我们建模为高斯随机游走，代表三维向量a的反对称阵。

3) Discrete model:

基于上述的操作符，我们能够将等式(1）以0阶保持器进行离散化以IMU的采样频率，

x_{i+1}=x_i \, \boxplus(\Delta t f(x_i,u_i,w_i)) ....(2)

上式i表示第i帧IMU数据，x为状态量，u为输入控制，w为噪声。

4) Preprocessing of LiDAR measurements: 雷达数据预处理

激光雷达测量是其局部base坐标系中的点坐标。由于原始固态激光雷达点以非常高的速率采样(例如，200kHz)，因此一旦接收到，通常不可能处理每个新点。一个更实际的方法是在一定的时间内积累这些点，然后一次处理它们。在FAST-LIO中，最小积累间隔设置为20ms，实现高达50Hz的全状态估计（即里程计输出）和地图更新，如图2(a).所示这种累积的点集合称为扫描scan，处理时间记为t_k(见图2(b))。

从原始点中，我们提取局部平滑度高的平面点和局部平滑度低的边缘点，如[10]Loam livox处理一样的。假设特征点的数量为m，每个特征点在\rho _j\in(t_{k-1},t_k]时刻采样，记为，其中是时刻的LiDAR局部坐标系。

在一帧激光雷达scan过程中，还有多个IMU的测量值，每个测量的时间都在代表在(2)式中的状态。请注意，最后一帧激光雷达特征点feature point是扫描的帧尾，即，而IMU测量值不一定与该扫描帧的开始或结尾部对齐。

C. State Estimation

为了估计状态公式(2)中的状态，我们使用了一个迭代扩展卡尔曼滤波器IEKF。此外，我们在状态估计的切空间中描述估计协方差。假设在处最后一次激光雷达扫描scan的最优状态估计为，协方差矩阵为。

协方差矩阵表示下面定义的随机误差状态向量的协方差:

其中\delta\theta=Log(^G\bar{R}_I^T^GR_I)是姿态误差,剩下的误差均为标准相加误差，估计的误差为如

直观地说，姿态误差描述了真实姿态和估计姿态之间的（小的）偏差。这种误差的定义的主要优点是，它允许我们用3X3的协方差矩阵来描述姿态不确定。因为姿态具有三个自由度，这是一个极简表示法。

1) Forward Propagation: 前向传播

图2 前向传播和后向传播

在接收到IMU输入后，就将执行正向传播，更确切的说，状态的前向传播是将(2)式的噪声设置为0，认为没有噪声

上面的是IMU的采样时间，

为了传播协方差，我们使用下面得到的误差状态动态模型：波浪线上标表示实际值和估计值的误差：

其中，，且A(u)即为以下：

若定义噪声w的协方差为Q,则有迭代的传播协方差为

前向传播持续进行，直到在时刻新扫描到达的结束时间，其中传播状态和协方差分别表示为，。其中表示真值状态与状态传播之间误差的协方差。

状态转移方程推导如下：

回顾

记作

那么误差状态等式(5)

就重写为：

那么根据偏导的链式法则，就有

2) Backward Propagation and Motion Compensation:反向传播及运动补偿

当接收到的点积累的时间间隔到时刻时(即在时间才触发更新，但是特征点是对应时刻的，不是在时刻的)，应将新帧的特征点的与传播状态和协方差融合，产生最优状态更新。然而，虽然新的帧点云是在时间时接收到的，但特征点是在它们各自的采样时间≤时测量的(见上图)，这也会导致base基体坐标系的不匹配。

为了补偿时间和时间之间的相对运动（即运动畸变），我们将等式(2）的推导反向传播为, 从0位姿开始和从重置状态(如速度和偏置) 。反向传播是以特征点的频率进行的，这通常远高于IMU速率(如果使用固态雷达则频率，则频率远高于IMU)。对于在两个IMU测量之间采样的所有特征点，我们使用左IMU测量作为反向传播的输入。此外，注意到公式(3)中的的最后三个块元素（对应的是陀螺仪偏置、加速度计偏置和外参）为零，反向传播可以简化为：

这里的, s.f.表示从此开始的简写。

反向传播会在时间和帧尾时间之间个相对的姿态：

该相对姿态使我们能够将局部测量投影到扫描帧结束时的测量，如下（见图2）：

^{{L_k}p_{f_j}=}IT^{-1}_L {I_k}\check{T}{I_j}^{L_k}p{f_j} .......(10)

其中为已知的外参。接着我们将投影点测量值用来构建残差，在下一节展开。

3) Residual computation:残差计算

利用式（10）中的运动补偿，我们可以查看所有同时采样时间为的特征点{}的扫描，并使用它来构造残差。假设迭代卡尔曼滤波器的当前迭代为，对应的状态估计为。当时，，从(4)中传播得到的预测状态。然后，特征点{}可以转换为到全局坐标系，如下所示：

^{G}\hat{p}\kappa_{f_j}=^{G} \hat{T}{I_k}{}^IT_L{L_k}p{f_j}

对于每个激光雷达特征点，假设在地图中由其附近的特征点定义的最近的平面或边缘为该点真正属于的位置。即，残差定义为特征点估计的全局坐标系坐标与地图中最近的平面（或边）之间的距离。表示对应平面（或边）的法向量（或边方向），法向量位于点上，则残差计算为：

其中，为平面特征向量，为边特征。

对的计算和对地图中附近点的搜索，从而定义了相应的平面或边，是通过在附近的地图[10]中建立一个点的kd树来实现的。此外，我们只考虑范数低于一定阈值的残差（例如，0.5m）的残差。(用于固定的离群值剔除)超过这个阈值的残差要么是异常值，要么是新观察到的点。

4) Iterated state update:

为了将上式(12)中计算的残差与IMU数据传播的状态预测和协方差结合起来，我们需要将残差与真实状态和测量噪声联系起来的测量模型线性化。在测量点时，测量噪声来自于激光雷达测距噪声和光束定向噪声。从测量点中去除这个噪声，可以得到真正的点位置:

这个真值点，在通过式（10）投影到Lk坐标系，然后投影到具有真实状态即姿态）的全局坐标系后，应该恰好位于地图中的平面（或边缘）上。也就是说，将式（13）插入式（10），然后插入（11），再插入（12）应该为零。展开即为：

用它在处的一阶近似来近似上述方程，得到:

上式或等价于
为相对于的雅可比矩阵，并且来自于原始的激光数据测量噪声。

请注意，通过第Section. III-C节中的前向传播得到的的先验分布用于：

其中，是相对于的在0处的偏微分：

上式的A(.)定义在式(6),对于第一次迭代(即扩展卡尔曼滤波器的情况），，然后是。

结合式(15)中的先验分布与式(14)中的后验分布，得到最大后验估计(MAP）：

上式的二范数可以表示为：,用式(15)带入式(17)， 优化得到的二次代价得到标准迭代卡尔曼滤波器[21]，可以在下面计算(为了简化符号，m为观测特征的数量
， ，

卡尔曼滤波器卡尔曼系数和更新后的后验估计为：

然后使用更新后的估计值来计算Section. III-C.3 部分中的残差,并重复这个过程，直到收敛（即）收敛后，最优状态估计和协方差 为：

式（18）中常用的卡尔曼增益形式的有一个问题是，它需要对测量维数中的矩阵求逆。在实际应用中，激光雷达特征点的数量非常大，对这种大小的矩阵的求你是禁止的。因此，现有的作品[21,26]只使用了少量的度量值。在本文中，我们证明了这种限制是可以避免的。灵感源于式(17），其中代价函数超过了状态维数，因此解应该根据状态维数计算出复杂性。事实上，如果直接求解(17），我们可以在（18）中得到相同的解，但有一个新的卡尔曼增益形式，如下所示：

基于矩阵逆引理[27]，我们在附录B中证明了这两种形式的卡尔曼增益确实是等价的。由于激光雷达测量是独立的，协方差矩阵R是方块对角矩阵，因此新的公式只需要在状态维数上对两个矩阵求逆，而不是测量维数。新的公式大大节省了计算量，因为状态维数通常远低于LIO中的测量值（例如，在10Hz扫描速率的扫描中有超过1000个有效特征点，而状态维数仅为18个）。

等效卡尔曼增益公式证明：

基于矩阵逆引理

将上式代入式(20),即可得

又

带入上式，可得

5) The algorithm:

算法1流程：

输入： 上一次优化的状态估计和协方差、

IMU输入数据()，雷达的当前帧特征点

第一步： 通过式(4)获得当前帧的先验估计值和式(8)获得协方差

第二步： 后向传播通过式(9)和式(10)获取在第k帧坐标系的特征点

第三步： 开始迭代，以先验估计值开始迭代，计算对应的雅可比J和状态协方差P,进而获得新的观测结果和H矩阵，

有了上面这些就可以计算卡尔曼增益，也就能够获得迭代后的后验估计值，当连续两次迭代的优化后验值时，认为迭代收敛，选择作为当前帧的最优估计。

第四步：将最优估计作为当前帧的后验位姿估计和协方差，用来给下一帧作为初始值。