激光SLAM总结——Fast LIO / Fast LIO2 / Faster LIO

激光SLAM总结——Fast LIO / Fast LIO2 / Faster LIO

在之前的工作中有接触过LOAM，最近在工作中又接触到Faster LIO相关的工作，于是想着对Fast LIO / Fast LIO2 / Faster LIO这一系列工作进行一个简单的总结，以加深自己对激光SLAM算法的理解，之前总结过的一些和激光SLAM算法相关的博客还有，感兴趣的同学可以一起学习讨论：
激光SLAM总结——VLOAM / LIMO算法解析
激光SLAM总结——LIO-Mapping / LIOM / LINS / LIO-SAM算法解析

1. Fast LIO

FAST LIO是2021年提出的一个激光和IMU紧耦合迭代卡尔曼滤波框架，论文的贡献主要是提供了一个新的卡尔曼增益计算公式，新公式不依赖于测量维度，仅依赖于状态维度，从而达到高效的计算

Fast LIO是基于LINS进行进一步优化，那么LINS有什么问题呢？LINS是紧耦合框架，点云构建的残差被直接写入了观测方程，点云残差构建如下：$$f_i\left({\mathbf{x}}_{b_{k+1}}^{b_k}\right)=\{\begin{array}{cl}\frac{\left|\left({\hat{\mathbf{p}}}_i^{l_k}-{\mathbf{p}}_a^{l_k}\right)\times \left({\hat{\mathbf{p}}}_i^{l_k}-{\mathbf{p}}_b^{I_k}\right)\right|}{\left|{\mathbf{p}}_a^{l_k}-{\mathbf{p}}_b^{l_k}\right|}& \text{ if }{\mathbf{p}}_i^{l_{k+1}}\in {\mathbb{F}}_e\\ \frac{\left|{\left({\hat{\mathbf{p}}}_i^k-{\mathbf{p}}_a^l\right)}^T\left(\left({\mathbf{p}}_a^k-{\mathbf{p}}_b^k\right)\times \left({\mathbf{p}}_a^l-{\mathbf{p}}_c^l\right)\right)\right|}{\left|\left({\mathbf{p}}_a^{l_k}-{\mathbf{p}}_b^{l_k}\right)\times \left({\mathbf{p}}_a^{l_k}-{\mathbf{p}}_c^{l_c}\right)\right|}& \text{ if }{\mathbf{p}}_i^{l_{k+1}}\in {\mathbb{F}}_p\end{array}$$残差的维度和点的数量相关，假设残差的维度为$m$，在计算卡尔曼增益过程中会计算残差相对状态变量的线性化矩阵${\mathbf{H}}_k$，其维度为$m\times n$，其中$n$为状态变量维度，进而计算${\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+\mathbf{V}$时维度将变为$m\times m$，在计算卡尔曼增益的过程中是需要对该矩阵进行求逆，可以预见当用于计算残差的特征点数量特别大时，计算卡尔曼增益也会变得相对耗时，在Fast-LIO就是通过SMW恒等式解决了该问题，使得计算卡尔曼增益不再依赖于观测维度，下面我们进一步看看具体是怎么做的

1.1 总体框架

Fast LIO的总体框架如下图所示：

大致流程是将激光雷达输入到特征提取模块，可获得平面和边缘特征，将提取后的特征和IMU测量值进行状态估计，输出$10HZ$到$50HZ$的状态估计结果，并根据状态估计结果将特征点注册到全局地图。

1.2 Iterated Kalman Filter

下面进行Fast LIO的公式推导：

假设IMU系为Body系，激光系到IMU系为刚体变换${}^I{\mathbf{T}}_L=\left({}^I{\mathbf{R}}_L,{}^I{\mathbf{p}}_L\right)$

IMU的连续运动学方程为：$\begin{array}{rl}{}^G{\dot{\mathbf{p}}}_I& ={}^G{\mathbf{v}}_I,{}^G{\dot{\mathbf{v}}}_I={}^G{\mathbf{R}}_I\left({\mathbf{a}}_m-{\mathbf{b}}_{\mathrm{a}}-{\mathbf{n}}_{\mathrm{a}}\right)+{}^G\mathbf{g},{}^G\dot{\mathrm{g}}=0\\ {}^G{\dot{\mathbf{R}}}_I& ={}^G{\mathbf{R}}_I\mid {\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\omega }\rfloor \Lambda ,{\dot{\mathbf{b}}}_{\mathbf{\omega }}={\mathbf{n}}_{\mathbf{b}\mathbf{\omega }},{\dot{\mathbf{b}}}_{\mathbf{a}}={\mathbf{n}}_{\mathbf{ba}}\end{array}$其中${}^G{\mathbf{p}}_I$，${}^G{\mathbf{R}}_I$是IMU的位置和姿态，${\mathbf{a}}_m$，${\mathbf{\omega }}_m$为IMU的测量，${\mathbf{b}}_{\mathbf{a}}$，${\mathbf{b}}_{\omega }$为随机游走高斯噪声

IMU的离散运动学方程为$${\mathbf{x}}_{i+1}={\mathbf{x}}_i⊞\left(\Delta t\mathbf{f}\left({\mathbf{x}}_i,{\mathbf{u}}_i,{\mathbf{w}}_i\right)\right)$$其中：$$\mathbf{x}\doteq [\begin{array}{l}\end{array}$$