Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（一）

1. LiDAR inertial odometry and mapping简介

在利用3D LiDAR进行SLAM计算时，有时会遇到一些问题，例如两帧之间的运动较为剧烈(大转角)可能会导致点云配准算法失效，在某些场景如长廊场景下有效测量点云较少使运动估计精度下降等，这些现象影响了SLAM算法的鲁棒性。而IMU(Inertial Measurement Unit)作为一种惯性测量单元，可以精确地测量载体的三轴加速度以及三轴角速度，在上述的场景中(大转角，长廊环境)可以为LiDAR提供额外的先验信息，从而增强LiDAR算法的鲁棒性，因此LiDAR-IMU fusioin成为一种近年来备受关注的SLAM方法。事实上，camera-IMU融合已经有一些开源的SLAM算法，例如VINS Mono。在ICRA 2019 上，终于迎来了第一个开源的LiDAR-IMU SLAM算法LIOM(Lidar-Inertial Odometry and mapping)，由香港科技大学的刘明老师及学生共同完成，其主要贡献如下：

• 提出了一种紧耦合的LiDAR-IMU里程计算法，可实现实时、高精度以及高更新率(High update rate)的里程计计算
• 利用LiDAR-IMU里程计的先验信息，基于旋转约束的优化方法进一步优化位姿和点云地图，以生成全局一致、鲁邦的地图和位姿估计
• 进行了大量的室内外实验测试，实验结果优于LiDAR-only或LiDAR-IMU松耦合方法
• 第一个开源的LiDAR-IMU紧耦合SLAM算法(LIO-Mapping)

2. Tightly coupled LiDAR inertial odometry

2.1 LiDAR-IMU odometry overview

整个LiDAR-IMU里程计的算法流程如下图所示：

为了更好的理解LIO算法，我们一起看下面这幅时序图：

假设当前的时间戳是$i$，则由于一般IMU设备的频率(100-1000Hz)会远远高于LiDAR点云数据的更新频率(10Hz)，因此在下一帧点云即时间戳$j$到来之前，会有大量的IMU数据${\tau }_{i,j}$读入，如上图中紫色线条所示。则可以根据这段时间内IMU的数据对IMU的状态进行估计，即流程图中的state-prediction部分，同时利用这段时间间隔内的IMU测量量进行预积分操作，即对应流程图中的Pre-integration。当到达时间戳$j$也即新的激光雷达数据帧过来后，由于激光雷达为连续测量，即在时间戳$i$到时间戳$j$过程中，激光雷达的测量是在不断运动且位姿不断变化的过程中测量得到的，因此会产生一定的运动畸变(motion distortion)。此时，在获得时间戳$j$对应的激光点云数据$S_j$后，应首先根据IMU的state-prediction消除运动畸变即流程图中的De-skewing部分从而得到未畸变的点云${\overline{S}}_j$。
在得到畸变矫正后的点云${\overline{S}}_j$后，为减少计算量，采用了LOAM中的方法从点云提取了两种特征$F_{L_j}$，分别是角点以及平面特征点，即对应流程图中的Feature Extraction部分。然后就是根据局部地图$M_{L_{o,i}}^{L_p}$，确定特征与局部地图的对应关系，也即流程图中的Find relative lidar measurements部分。所谓的局部地图，也就是将上图所示的从时间戳$o$到时间戳$i$的特征点都投影到一个坐标系下形成一个点云局部地图，在本文中，每次都是投影到中间的时间戳即$p$对应的坐标系下，即对应流程图中的Local map management。最后在确定了测量约束和IMU的预积分约束后，建立非线性最小二乘的目标函数并进行联合优化求解，即对应流程图中的Joint non-linear optimization部分。接下来，我们对LIO中的每一个子部分进行详细解析。

2.2 IMU and pre-integration

一般来讲，一个三轴正交的IMU设备可以测量三轴的加速度以及角速度，由牛顿运动定律，若初始状态(位置，姿态，速度)已知，则可以根据加速度以及角速度的测量跟踪一个物体的状态(位置，姿态，速度)，因此广泛的应用于惯性导航(Inertial Navigation System)中。在涉及到IMU的导航系统中，通常IMU的状态可以建模为如下向量：
$$X_{B_i}^W=[{p_{B_i}^W}^T,{v_{B_i}^W}^T,q^{}$$