FAST_LIO论文精读

摘要：利用紧密耦合的迭代卡尔曼滤波方法将激光雷达特征点和IMU数据融合，提高了计算效率和鲁棒性。

介绍：固态激光雷达当前成为激光雷达发展的主要趋势，比如基于微电系统扫描和旋转棱镜式的激光雷达。这些激光雷达具有成本效益高，重量轻的优点。需要解决的地方：

1.当这种激光雷达在没有明显特征的杂乱环境的情况下，很多解决方案就不适用了，特别是激光雷达视场比较小的时候。

2.特征点数量很大的时候，便携的计算机无法负担。

3.扫描总是在不同时间进行，导致运动失真，导致导致配准误差增加。

卡尔曼滤波（Kalman Filter）是一种常见的状态估计方法，它基于线性高斯模型，通过融合测量和预测，可以对一个系统的状态进行估计，并通过不断地更新状态和协方差矩阵来提高估计精度。

在卡尔曼滤波中，系统状态被表示为一个向量，包含了系统中所有的状态变量。卡尔曼滤波的核心思想是利用当前的测量结果，结合先前的状态预测，来计算出最优的系统状态估计。卡尔曼滤波假设系统是线性的，并且系统的状态和测量误差都服从高斯分布。

卡尔曼滤波通过状态转移方程和观测方程来描述系统的动态特性和测量模型。其中，状态转移方程用于描述系统状态在没有测量输入的情况下的变化，而观测方程则用于将系统状态与测量值联系起来。在每个时间步骤，卡尔曼滤波将当前的状态和协方差矩阵作为先验信息，利用观测方程更新系统状态和协方差矩阵，得到最优的后验估计。

卡尔曼滤波的作用是可以用来对系统状态进行估计，特别是对于那些无法直接观测到的系统状态。例如，在自动驾驶领域中，我们需要对车辆的位置、速度、加速度等状态进行估计，以实现精准的控制。卡尔曼滤波可以通过融合来自GPS、IMU、激光雷达等传感器的测量结果，对车辆的状态进行估计，从而实现自动驾驶。

在FAST-LIO中，卡尔曼滤波被用于融合激光雷达和惯性测量单元的数据，实现高精度的位姿估计。通过迭代地利用先验信息和观测数据，卡尔曼滤波可

以不断地更新状态估计和协方差矩阵，从而提高位姿估计的精度和鲁棒性。

因此就有了《FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter》

贡献：

1.为了应对发生退化的快速运动、噪声或杂乱的环境，我们采用紧密耦合迭代卡尔曼滤波器将激光雷达特征点与IMU测量值融合。

2.为了降低计算量，我们提出了一个新的计算卡尔曼增益的公式，并证明了它与传统的卡尔曼增益公式的等价性。新公式的计算复杂度取决于状态维数，而不是测量维数。

3.我们将我们的公式实现到一个快速和鲁棒的激光雷达惯性测程软件包。该系统能够在小型四旋翼计算机上运行。

相关工作：

A：激光测程和建图

Besl等人[6]提出了一种扫描配准的迭代最近点（ICP）方法，为激光雷达测程奠定了基础。ICP对密集的3D扫描表现良好。然而，对于激光雷达测量的稀疏点云，ICP所要求的精确点匹配很少存在。为了解决这个问题，Segal等人[7]提出了一个基

于点到平面距离的广义icp。然后Zhang等人[8]将这种ICP方法与点边距离结合起来，开发了一个激光雷达测程和映射（LOAM）框架。此后，许多负荷的变体被开发出来，如LeGO-LOAM [9]和LOAM-Livox [10]。虽然这些方法适用于结构化环境和大型视场的激光雷达，但它们非常容易受到无特征环境或小型视场激光雷达[10]的影响。

B：松散耦合激光雷达惯性测程

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

C：紧密耦合激光雷达-惯性测程法

与松散耦合的方法不同，紧密耦合的激光雷达-惯性测程法通常是将激光雷达的原始特征点（而不是扫描配准结果）与IMU数据相融合。紧密耦合的LIO主要有两种方法：基于优化的和基于滤波。Geneva等人[14]使用IMU预集成约束[15]和平面约束[16]。最近，Ye等人[17]提出了LIOM包，该包使用类似的图优化，但基于边缘和平面特征。对于基于滤波器的方法，Bry [18]使用高斯粒子滤波器（GPF）来融合IMU和平面二维激光雷达的数据。该方法也被用于波士顿动力学图谱的人形机器人。由于粒子滤波器的计算复杂度随着特征点数和系统维数的增加而快速增长，卡尔曼滤波器及其变体通常更可取，如扩展卡尔曼滤波器[19]、无迹卡尔曼滤波器[20]和迭代卡尔曼滤波器[21]。我们的方法属于紧密耦合的方法。我们采用一种类似于[21]的迭代扩展卡尔曼滤波器来减轻线性化误差。卡尔曼滤波器（及其变体）的时间复杂度为O(m²)，其中m是测量维数[22]，这可能导致在处理大量激光雷达测量时产生非常高的计算负载。朴素的降采样将减少测量的数量，但以信息损失为代价。[21]通过提取和拟合类似于[9]的地平面来减少测量次数。然而，这并不适用于地面平面可能并不总是存在的空中应用。

主要内容：

A：框架概述

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

表1：本文符号表示

图2：系统流程图

B:系统描述

1. 设M是考虑的维数n的流形（例如，M = SO (3)）。由于流形与Rn是局部同态的，我们可以通过两个封装算子建立从M上的局部邻域到切空间Rn的双射映射（田和曰）

这里      

指的是指数映射，Log（·）是它的逆映射。

1. 连续模型

假设一个IMU以一个已知的外参矩阵刚性地附着在激光雷达上，其中外参矩阵： ，以IMU坐标系(I)作为参照系，得到运动学模型：

                 （1）

其中GpI、GRI为IMU在全局框架（即第一个IMU框架，记为G）中的位置和姿态，齿轮是未知的重力向量在全球框架，我和ωm IMU测量，纳和nω白噪声IMU测量，ba和bωIMU偏差建模为随机行走过程与高斯噪声nba和nbω，和符号|a|∧表示偏对称矩阵向量∈R3映射交叉产品操作。

1. 离散模型

   基于上述定义的操作，我们可以在IMU采样周期∆t时在(1)中离散连续模型。所得到的离散模型为  ：

 

式中，i为IMU测量值的索引，函数f、状态x、输入u和噪声w定义如下：

1. 激光点云预处理

车底架由于原始激光雷达点以非常高的速率采样（例如，200 kHz），因此一旦接收到，通常不可能处理每个新点。一个更实际的方法是在一定的时间内积累这些点，然后一次处理它们。在FAST-LIO中，最小积累间隔设置为20 ms，导致高达50 Hz的全状态估计（即测程输出）和地图更新，如图2 (a).所示。这种累积的点集合称为扫描，处理时间记为tk（见图2 (b)）。从原始点中，我们提取局部平滑度高的平面点和局部平滑度低的边缘点，如[10]。假设特征点的数量为m，每个特征点在ρj∈(tk−1，tk]时刻采样，记为Ljpfj，其中Lj是ρj时刻的LiDAR局部帧。在激光雷达扫描期间，会有多次IMU测量，每个样本在时间τi∈[tk−1，tk]采样，各自的状态xi如(2)所示。请注意，最后一个激光雷达特征点是扫描的结束点，即ρm = tk，而IMU测量值不一定与扫描的开始或结束对齐。

C：状态估计

为了估计状态公式(2)中的状态，我们使用了一个迭代的扩展卡尔曼滤波器。此外，我们在状态估计的切空间中描述估计协方差。假设在tk−1处最后一次激光雷达扫描的最优状态估计为¯xk−1，协方差矩阵为¯Pk−1。然后¯Pk−1表示如下定义的随机误差状态向量的协方差：

其中 为姿态误差，其余为标准加性误差（即，一个量x的估计¯x的误差为 ）。直观地说，姿态

误差δθ描述了真实姿态和估计姿态之间的（小的）偏差。这个误差定义的主要优点是，它允许我们用3×3协方差矩阵 来表示态度的不确定性。由于态度有3个自由度（DOF），这是一个最小的表示。

1）前向传播

收到IMU输入后执行正向传播(见图2）。更具体地说，通过将过程噪声wi设置为零，状态传播如下(2)：

其中，∆t=τi+1−τi。为了传播协方差，我们使用下面得到的误差状态动态模型：

将白噪声w的协方差表示为Q，然后按照下式对传播的协方差^Pi进行迭代计算。

传播继续进行，直到在tk处达到一个新的扫描的结束时间，其中传播的状态和协方差表示为^xk，^Pk。然后^Pk表示地面真状态xk与状态传播之间误差的协方差

2）后向传播和运动补偿

当在tk时刻达到点积累时间间隔时，应将特征点的新扫描与传播状态^xk和协方差^Pk融合，产生最优状态更新。然而，虽然新的扫描是在时间tk时，但特征点是在它们各自的采样时间ρj≤tk时测量的(见第节。III-B.4和图2 (b))，导致身体参考坐标系的不匹配。为了补偿时间ρj和时间tk之间的相对运动（即运动失真），我们向后传播(2)为 。从^xk的零姿态和静止状态（例如，速度和偏差）开始。反向传播是在特征点的频率下进行的，这通常远高于IMU速率。对于在两个IMU测量之间采样的所有特征点，我们使用左IMU测量作为反向传播的输入。此外，注意到f（xj、uj、0）（见(3)）的最后三个块元素（对应于陀螺偏置、加速度计偏差和外部）为零，反向传播可以简化为：

其中，ρj−1∈[τi−1，τi)，∆t=ρj−ρj−1，和s.f。意思是“从头开始”。反向传播将在时间ρj和扫描结束时间tk之间产生一个相对姿态：

这个相对的姿态使我们能够投射出局部的测量值 到扫描结束测量

关于翻译就到这里，下面是手写的笔记，主要内容是参考别的博主回答，然后自己补充一些内容，参考的文章链接： http://t.csdn.cn/8VfZL。笔记如下 ,主要是对遇到的一些公式理解和专业词汇的的概念补充，围殴了更加方便阅读理解该文章：

对以下伪代码的理解：

对文中 协方差矩阵的解释：

协方差矩阵中的对角线上的元素表示每个状态量的方差，非对角线上的元素表示不同状态量之间的协方差。方差是一个衡量变量分布分散程度的度量，协方差则是衡量两个变量之间线性相关性的度量。在估计中，协方差矩阵用于衡量估计量的不确定性。具体而言，对角线上的元素表示每个状态量的不确定性大小，即方差。方差越小，表示估计量越精确。非对角线上的元素则表示不同状态量之间的相关性。

如果两个状态量高度相关，则它们的协方差值会很高。因此，通过协方差矩阵，我们可以了解我们的估计值的准确性以及不确定度的大小。在状态更新时，Kalman增益的计算中也考虑了协方差矩阵，以平衡当前观测和之前的估计值对状态估计的影响，以最大程度地减少估计值的不确定性。

对 雅可比矩阵的解释：

在本文中： 雅可比矩阵用于卡尔曼滤波器中的状态更新和协方差更新。在状态更新中，雅可比矩阵用于计算卡尔曼增益，该增益用于融合IMU和LiDAR测量的数据，从而更新车辆的运动状态。在协方差更新中，雅可比矩阵用于计算状态变量和协方差矩阵之间的关系，从而更新协方差矩阵的值，反映估计的不确定性。