激光IMU融合——LIO-Mapping / LIOM / LINS / LIO-SAM算法解析

Posted 2021-10-26 Jichao_Peng

激光IMU融合——LIO-Mapping / LIOM / LINS / LIO-SAM算法解析

• 激光IMU融合——LIO-Mapping / LIOM / LINS / LIO-SAM算法解析
• • 1. LIO-Mapping
  • • 1.1 总体框架
    • 1.2 滑窗管理
    • 1.3 地图注册
    • 1.4 实验结果
  • 2. LIOM
  • • 2.1 总体框架
    • 2.2 误差状态扩展卡尔曼滤波
    • 2.2 实验结果
  • 3 . LINS
  • • 3.1 总体框架
    • 3.2 误差状态扩展卡尔曼滤波
    • 3.3 实验结果
  • 4. LIO-SAM
  • • 4.1 总体框架
    • 4.2 因子图构建
    • 4.3 实验结果

激光IMU融合——LIO-Mapping / LIOM / LINS / LIO-SAM算法解析

在激光SLAM领域，LOAM、Lego-LOAM属于纯激光领域，除此之外还衍生处理视觉激光结合、激光IMU结合，甚至三者结合的算法，视觉激光结合的算法在我之前写的博文视觉激光融合——VLOAM / LIMO算法解析中有简单总结，本文所介绍的LIO-Mapping / LIOM / LINS / LIO-SAM算法就隶属于激光IMU结合的算法，在发展成熟度上面是要优于视觉激光融合的。

因为我是先学习的视觉SLAM算法，从整体框架上理解，LIOM、LINS类似于MSCKF，后端是基于滤波的方法实现两种传感器的紧耦合，而LIO-Mapping类似于VINS-Mono，后端是基于优化的方法实现两种传感器的紧耦合，LIO-SAM后端是基于因子图实现的多种传感器的紧耦合，对MSCKF和VINS-Mono不了解的同学可以如下参考博客：
VINS-Mono关键知识点总结——前端详解
VINS-Mono关键知识点总结——边缘化marginalization理论和代码详解
VINS-Mono关键知识点总结——预积分和后端优化IMU部分

学习MSCKF笔记——前端、图像金字塔光流、Two Point Ransac
学习MSCKF笔记——四元数基础
学习MSCKF笔记——真实状态、标称状态、误差状态
学习MSCKF笔记——后端、状态预测、状态扩增、状态更新
本篇博客，我对以上算法进行简单总结。

1. LIO-Mapping

LIO-Mapping是2019年发表于ICRA一篇论文，原论文为《Tightly Coupled 3D Lidar Inertial Odometry and Mapping》，从工程层面上将，就是LOAM和VINS-Mono的结合，具体介绍如下：

1.1 总体框架

LIO-Mapping总体框架如下：

具体步骤如下：

1. 在激光雷达数据$S_j$到来之前，IMU原始输入${\mathcal{I}}_{i,j}$进行积分（State prediction）和预积分（Pre-intergration），积分的目的是为了获得我们需要估计的状态变量${\mathbf{p}}_{B_i}^W$，${\mathbf{v}}_{B_i}^W$和${\mathbf{q}}_{B_i}^W$，而预积分的目的是为了获得用于联合优化的$\Delta {\mathbf{p}}_{ij}$，$\Delta {\mathbf{v}}_{ij}$和$\Delta {\mathbf{q}}_{ij}$；
2. 当系统收到借光雷达数据$S_j$后，对激光雷达进行去畸变操作（De-skewing）获得去畸变点云$S_j$，然后在此基础上对去畸变点云提取激光雷达特征点${\mathbf{F}}_{L_j}$（Feature extraction）；
3. 在滑动窗口内的激光雷达特征${\mathbf{F}}_{L_{o,i}}$根据优化后的位姿${\mathbf{T}}_{B_{o,i}}^W$和${\mathbf{T}}_B^L$组成局部地图${\mathbf{M}}_{L_{o,i}}^{L_p}$（Local map management），根据预测的位姿实现当前帧激光特征点和局部地图的匹配（FInd relative lidar measurements）；
4. 最后就是j激光雷达观测${\mathbf{m}}_{L_{p+1,j}}$以及预积分结果$\Delta {\mathbf{p}}_{ij}$，$\Delta {\mathbf{v}}_{ij}$和$\Delta {\mathbf{q}}_{ij}$实现联合优化，优化后的结果用于跟新IMU积分获得的状态变量，避免IMU漂移；

由于激光雷达去畸变及特征提取、预积分、边缘化等操作在VINS-Mono的相关博客中都有详细介绍，因此本篇博客只对LIO-Mapping中较为特殊的滑窗管理和地图注册进行总结

1.2 滑窗管理

LIO-Mapping中滑窗管理示意图如下：

其中，
$o$是滑窗中的第一帧Lidar Sweep，
$i$是滑窗中最后一帧Lidar Sweep，
$j$是当前帧的Lidar Sweep，
$p$是滑窗中的Pivot Lidar Sweep，所谓Pivot Lidar Sweep指的就是整个滑窗内帧的位姿都是基于该帧坐标系建立的，随着滑窗移动，Pivot Lidar Sweep也不断变化。

我们将滑窗内的特征点构建为局部地图 M L o , i L p = F L γ L p , γ ∈ { o , … , i } \\mathbf{M}_{L_{o}, i}^{L_{p}} = \\mathbf{F}_{L_{\\gamma}}^{L_{p}}, \\gamma \\in\\{o, \\ldots, i\\} MLo​,iLp​​=FLγ​Lp​​,γ∈{o,…,i}，然后我们通过KNN算法找到原始特征点 F L α , α ∈ { p + 1 , … , j } \\mathbf{F}_{L_{\\alpha}}, \\alpha \\in\\{p+1, \\ldots, j\\} FLα​​,α∈{p+1,…,j}与局部地图的对应关系并构建点到面的残差，优化过程中会优化窗口中所有帧相对Pivot Lidar Sweep的相对位姿以${\mathbf{T}}_{L_p}^W$及包括Pivot Lidar Sweep的位姿${\mathbf{T}}_{L_p}^W$，而其中相对位姿可以表示为：$${\mathbf{T}}_{L_{\alpha }}^{L_p}={\mathbf{T}}_B^L{\mathbf{T}}_{B_p}^W{}^{-1}{\mathbf{T}}_{B_{\alpha }}^W{\mathbf{T}}_B^{L-1}=[\begin{array}{c}{\mathbf{R}}_{L_{\alpha }}^{L_p}\end{array}$$

激光雷达SLAM包括以下几种方案:

LOAM中关于坐标转换与IMU融合

服务机器人---多传感器融合

服务机器人---多传感器融合

从实战层面上剖析Camera+Liadar+Radar+IMU之间的数据标定融合