FAST-LIO论文阅读

论文：FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter

源码链接

各位大佬对论文的解析：
FAST-LIO论文解读与详细公式推导

FAST-LIO是港大MaRS实验室在2021年提出的一个紧耦合迭代扩展卡尔曼滤波高计算效率、高鲁棒性的雷达里程计。影响深远，后续又陆续提出了FAST-LIO2以及Faster-LIO等框架。

下面，我们简单了解一些论文中的各个模块及其处理流程。

符号说明

$t_k$第K帧激光扫描的结束时间
${\tau }_i$LiDAR扫描帧中的第i个IMU数据
${\rho }_j$LiDAR扫描帧中的第j个激光点时间
$I_i,I_j,I_k$ IMU在${\tau }_i$，${\rho }_j$，以及$t_k$三个时刻的载体坐标系
$L_j,L_k$ LiDAR在${\rho }_j$，$t_k$时刻的激光坐标系。
$X，\hat{X}，\bar{X}$：状态X的真值，预测值，更新值（后验，估计值）
$\tilde{X}$：状态X的真值$X$与估计值$\bar{X}$之间的误差(即：$\tilde{X}=X\boxminus \bar{X}$ ，$X=\tilde{X}⊞\bar{X}$ )
${\hat{X}}^{\kappa }$：迭代扩展卡尔曼滤波（IEKF）中的第$\kappa$次迭代的状态量
$X_i,X_j,X_k$：在${\tau }_i$，${\rho }_j$，以及$t_k$三个时刻的状态量
${\check{X}}_j$：在后向传播中，相对于$t_k$时刻状态$X_k$的估计值$X_j$

基础概念（运算符）

作者在文中定义了两个基础的运算符，$⊞$与$\boxminus$。

这里的$M$表示一种$n$维的流形。
$⊞$操作对应于在流形$M$上增加一个小的扰动。
$\boxminus$操作对应于两个流形$M_1$与$M_2$之间的微小差值。
分别对应于指数映射与对数映射。

同时，我们可以推导出下述结论

文中以IMU坐标系作为载体系，推出来的位姿也在载体系中。假设激光雷达与IMU刚性链接，使用一个外参转换关系${}^{I}T{}_L=({}^{I}R{}_L,{}^{I}p{}_L)$进行转换。

IMU连续模型

IMU的动力学模型如下：

这是易于理解的，位置的导数是速度，速度的导数为加速度（增加的坐标转换与重力影响），重力为一个常数，导数为0，旋转的导数为角速度（推导可以参考高翔博士的SLAM十四讲），陀螺仪与加速计零偏的导数为高斯白噪声。

IMU离散模型

假设，IMU的采样频率为$\Delta t$，则离散模型可以写成如下形式：

其中：

LiDAR帧的概念

实际工作过程中 LiDAR是在不断的连续扫描的（这个频率非常高，数十万Hz），但是我们为了能够处理点云数据，人为的划分成了不同的扫描帧，如文中把累积20ms的点云作为一帧数据，扫描频率为50Hz。

即把上图中$t_{k-1}$到$t_k$的时间段（20ms）划分为一帧点云。

但是，这样引起一个问题是，带来了运动畸变。对于这一问题，在后续的章节中通过后向传播来进行纠正。

状态估计

作者使用误差作为要估计的状态，这样做有一系列好处：参考高翔博士-简明ESKF推导

1. 在旋转的处理上，ESKF的状态变量可以采用最小化的参数表达，也就是使用三维变量来表达旋转的增量。而传统KF需要用到四元数（4维）或者更高维的表达（旋转矩阵，9维），要不就得采用带有奇异性的表达方式（欧拉角）。
2. ESKF总是在原点附近，离奇异点较远，并且也不会由于离工作点太远而导致线性化近似不够的问题。
3. ESKF的状态量为小量，其二阶变量相对来说可以忽略。同时大多数雅可比矩阵在小量情况下变得非常简单，甚至可以用单位阵代替。
4. 误差状态的运动学也相比原状态变量要来得更小，因为我们可以把大量更新部分放到原状态变量中。

前向传播(运动方程)

前向传播的执行过程如下，每次接收到一次数据我们就会执行一次。

此外，由于不知道噪声的值，所以设置噪声$w$为0，不断进行前向传播。当然，这样很快就会“飘”。但是，我们还有观测方程（LiDAR）进行修正。

对公式（4）转换为误差的形式，并进行线性化：

$F_{\tilde{X}}$与$F_W$分别为${\tilde{X}}_{i+1}$与$w_i$变量的雅克比矩阵。形式如下：

其中A（.）的表示方式为：

推导方式见论文中的附录，这里就不详细说了，很烦人。

有了运动方程的线性化表达式，我们还需要对应的协方差更新方式，假设噪声$w$的协方差为$Q$，则更新方式为：

直到一帧的扫描终点时刻$t_k$，一个前向传播过程才结束。终点时刻$t_k$的预测状态表示为${\hat{X}}_k$，对应的协方差表示为${\hat{P}}_k$(状态预测值${\hat{X}}_k$与状态真值$X_k$之间的误差的协方差${\hat{X}}_k\boxminus X_k$)。

后向传播(运动畸变校正)

我们在处理过程中会融合在$t_k$时刻的状态${\hat{X}}_k$与协方差${\hat{P}}_k$。但是，正如我们之前所提到的，每个点都有属于他们自己的时间戳，其测量时间并不是我们所规定的$t_k$时刻，即LiDAR点采样（测量）时间${\rho }_j<t_k$，这会引起参考坐标系的不一致。

如图中的下半部分，为了消除这种影响，作者使用下述公式，反向（后向）从$t_k$时刻的位姿，推算出${\rho }_j$时刻的位姿，并把${\rho }_j$时刻的特征点转换到$t_k$时刻。


注意，因为特征点的频率高于IMU频率，所以并不是每个特征点时刻对应一个位姿。每个特征点的转换位姿都由其左侧的IMU时刻确定。

通过上述计算，得到${\rho }_j$时刻到$t_k$时刻的相对位姿为：${}^{I_k}{\check{T}}_{I_j}={(}^{I_k}{\check{R}}_{I_j}{,}^{I_k}{\check{p}}_{I_j})$

基于此，我们可以通过下式，把局部坐标系的点测量值${}^{L_j}{p}_{f_j}$，投影的扫描终点时刻$t_k$，即${}^{L_k}{p}_{f_j}$。
$${}^{L_k}{p}_{f_j}={}^I{T}_L^{-1}{}^{I_k}{\check{T}}_{I_j}{}^I{T}_L{}^{L_j}{p}_{f_j}$$

式中，${}^I{T}_L$为LiDAR与IMU之间的外参，${}^{L_k}{p}_{f_j}$为投影到扫描终点时刻$t_k$的坐标，用于下面的残差计算。

残差计算

经过上节中的运动畸变校正，我们可以把一个扫描帧中的所有特征点视为在同一时刻$t_k$处进行采样，接着，投影到全局坐标系中：

式中，${}^G{\hat{T}}_{I_k}^{\kappa }$为我们想要求的$t_k$时刻到全局坐标系下的位姿变换。

类似于LOAM的思想，转换后的特征点应该落在其对应的特征“线”“面”上，但是由于存在LiDAR测量误差与前向传播的状态推算误差，导致转换后的特征点并不能完全落在特征线/面上。

式中，$G_j$为法向量$u_j^T$（平面特征）或为边缘线特征朝向的反对称阵${\lfloor u_j\rfloor }_{\wedge }$（边缘特征）。即计算点到面或者点到线之间的距离。

作者只考虑模长小于0.5m的残差值。残差值高于阈值的被认为是噪声点或者是新观测的点。

迭代状态更新

如果我们把激光雷达的测量噪声去除，假设测量的点都是真实的坐标。

那么我们使用这个真值代入上述公式中转换的全局坐标系$G$，再使用状态的真值$X_k$（有变换的真值${}^G{T}_{I_k}$），那么残差$z_j^{\kappa }$的值应该为0；

对上式$h_j$进行一阶近似：

式中，${\tilde{X}}_k^{\kappa }=X_k\boxminus {\hat{X}}_k^{\kappa }$ ，$X_k={\hat{X}}_k^{\kappa }⊞{\tilde{X}}_k^{\kappa }$。

存在：

结合（15）（运动方程）及残差（14）（观测方程），我们得到下述形式的目标函数：

式中:${\Vert x\Vert }_M^2=X^{T}MX$

利用迭代卡尔曼滤波，我们对（17）进行求解

得到${\bar{X}}_k$与${\bar{P}}_k$。

上述求解过程中还存在一个问题是求解卡尔曼增益K需要对$HP{H}^T+R$进行求逆。这个维度为$m\ast m$即特征点的数量（观测的数量）。这个维度是很大的，所以求解比较困难。

作者把卡尔曼增益的公式等价转换为下述形式，求逆的维度为状态量的维度$18\ast 18$，大大降低了计算的维度。

等价转换过程的推导也是比较简单的，利用了矩阵的求逆定理。