参考文献链接

标题：Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping

作者：Kenji Koide, Masashi Yokozuka, Shuji Oishi, Atsuhiko Banno

来源：ICRA2022

摘要

本文提出一种实时三维建图框架，该框架基于全图匹配代价的最小化和LiDAR-IMU紧耦合。该框架有一个预处理模块和三个估计模块组成，这三个估计模块为：里程计估计、局部建图和全局建图。这三个估计模块全都基于GPU加速的体素化的GICP匹配代价因子和IMU预积分因子的紧耦合。里程计估计模块采用基于关键帧的固定滞后平滑以满足高效率和低漂移的轨迹估计，同时能够使计算成本可控。全局建图模块通过构建一个包含了IMU约束的因子图来最小化全局匹配误差，因此在特征缺失的环境中，依然能够保证鲁棒的优化效果。该框架在Newer College数据集、KAIST urban数据集上的评估结果显示其在具有挑战性的环境中的定位建图的精度和鲁棒性。

主要工作与贡献

本文的LiDAR-IMU紧耦合方法贯穿所有状态估计阶段。是目前为止第一个在全局轨迹规划上同时利用了LiDAR和IMU的约束的工作。与此前的LiDAR-IMU SLAM框架相比有以下几点不同：

1. 不同于此前的LiDAR SLAM从点云中提取平面和边缘特征来进行匹配，本文使用体素化的GICP匹配代价因子（voxelized GICP matching cost factor），可以在GPU上并行计算；
2. 本方法中的紧耦合里程计模块使用固定滞后平滑；
3. 将紧耦合加入了后端。

方法

在所有估计模块中的待估计状态为，
$$
x_t=[T_t,v_t,b_t]^T
$$
系统总览如图：

整个框架只在预处理阶段，对当前点云会进行K近邻的搜索。

框架中其实就是三个估计器，前端估计器就是里程计，后端两个估计器一个是局部建图，一个是全局建图。三个估计器所使用的因子其实是比较统一的，就是LiDAR的体素化GICP匹配因子和IMU的预积分因子。

LiDAR的体素化GICP匹配因子

当前点云和上一帧点云分别为$P_i$和$P_j$，对应的位姿分别为$T_i$和$T_j$，当前点云中的任一点$p_k\in P_i$都被视为一个高斯分布：
$$
p_k=(\mu_k,C_k)
$$

IMU的预积分因子