LIO论文阅读：Tightly Coupled 3D Lidar Inertial Odometry and Mapping
论文地址与代码地址
摘要
Ego-motion 在移动机器人应用中很重要。传感器融合可以补偿单传感器缺陷。这篇文章主要介绍了一种LiDAR-IMU 融合的方法，通过联合优化IMU和LiDAR的测量数据，可以做到在LiDAR退化的情况下也没有明显漂移。
另外，本文提出了一种rotation-constrained refinement 算法（LIO-mapping），用于配准LiDAR位姿和全局地图。
实验结果显示，所提出的方法可以做到以IMU的速率估计传感器的位置，即使在快速移动和缺少特征情况下也能工作。
I. INTRODUCTION
LiDAR优点：在机器人领域广泛应用，频率10Hz，水平的 field of view(FOV)是360 \degree。受光照影响小，高精度和高可靠性。
LiDAR的缺点：竖直方向上分辨率太小，太过于稀疏，因此能提供的特征也很少，让特征跟踪变得棘手。
安装在机器人上的LiDAR由于机器人的运动，会使测量的数据变差。
另外，在一个狭窄的走廊环境中，只能得到部分天花板，墙和底板的点云。特征过少导致跟踪的丢失。
还有一个问题：更新频率太低。
在本文中，提出IMU-LiDAR紧耦合的位姿估计算法来解决上述问题。
使用了 fix-lag smoothing 和 原有位姿边缘化的方法达到实时和一致性的估计，然后再进行 rotation-constrained refinement。
主要工作如下：
- LiDAR-IMU紧耦合算法提供高频的，实时的，准确的位姿估计。
- rotational constrained refinement 方法优化最终的位姿并且产生点云地图，提高一致性和鲁棒性的估计。
- 算法在室内外环境都进行了测试，效果比松耦合算法优异。
- 算法开源，业界第一个LiDAR-IMU紧耦合算法。
本文总体结构：
II 介绍相关工作；
III 介绍标记与注记；
IV 介绍里程计算法；
V 介绍 refinement （配准）算法；
VI 和 VII 介绍了实现与测试结果；
VIII 结论。
II. RELATED WORKS
松耦合算法：将LiDAR和IMU的状态估计分别单独考虑。
[1] 中的雷达里程计算法带有IMU的辅助信息。使用加速度信息时会假设它是 zero velocity 的。它将 IMU 和雷达的数据分离，并且将IMU信息作为整个系统的先验，因此在优化中，它没有最大化 IMU 信息的利用。
[2] 中使用EKF来融合2D 情况下的 LiDAR 和 IMU 的信息，但它不能处理3D或者更复杂的环境。
[3] 讲了一种模块化的IMU融合方式，通过 EKF 进行融合。这种松耦合的方式虽然计算效率很高，但是和紧耦合的相比准确度不够，因为它只把里程计作为一个黑盒，却没有通过IMU来更新它。
[4]采用了紧耦合的方式。
[5] 是 Soloviev 等人提出的。针对2D平面运动估计，他们提出了一种方法：抽取和匹配2D平面的线特征。通过 IMU 的预测方位补偿雷达信息。过程中应用了 卡尔曼滤波器。
[6] Hemann 等人提出了以误差状态的卡尔曼滤波器形式，紧耦合IMU 传播误差和 LiDAR heightmap。这种方式在环境地图已知时，没有GPS信息也能长时间工作，但没有地图无法工作。
[7] 和 [8] 未处理数据和 IMU 的预测数据直接用于计算残差和进行优化。这种方式没有牵涉转化和状态估计，在系统快速运动时，系统会不可用。
受到其他 VI 方式启发，如[10] ,[11] ，我们基于预积分和紧耦合的方式设计了整个系统。使用未处理的IMU数据和LiDAR数据进行状态优化。
整个系统可以在雷达退化或者运动十分快速时也能工作得很好。
据我们所知，我们的系统是少数几个适合复杂3D环境的。
III. NOTATIONS AND PRELIMINARIES
A. Notations
雷达每一线的测量记为 scan \ C。
包含所有 scan 的一次测量记为 sweep \ S。16线雷达在一次sweep中包含16个scan。
变换矩阵\bold{T}_{b}^{a} \in SE(3),表示把点 \bold{x^b} \in \mathbb{R^3} 从坐标系 F_b 变换到坐标系 F_a；
\bold{\bar{T}}_{b}^{a}表示IMU预测的变换；
\bold{R}_b^a \in SO(3) 和 \bold{p}_b^a 分别是\bold{T}_{b}^{a}的旋转和平移部分；
四元素\bold{q}_b^a对应于\bold{R}_b^a；
\bold{\hat{a}}_k和\bold{\hat{\omega}}_k表示在时刻 k 时，IMU的加速度和角速度测量；
在坐标系 F_a下抽取的特征表示为 \bold{F}_a， \bold{F}_a可以通过 \bold{F}_a^b变换到坐标系 {F}_b。
A. IMU Dynamics
1）状态
F_{B_i} 和 F_{L_i} 分别是IMU和雷达的坐标系；
雷达在离散时刻 i 时接收 sweep\ S_i；
我们要进行估计的是IMU在世界坐标系下 F^W 的状态 \bold{X}_{B_i}^W以及雷达和IMU之间的外参 \bold{T}_B^L，具体如下：
\bold{X}_{B_i}^W=[ {\bold{p}_{B_i}^W}^T \ {\bold{v}_{B_i}^W}^T \ {\bold{q}_{a_i}^W}^T \ {\bold{b}_{g_i}^W}^T ]^T, \\ \bold{T}_B^L=[{\bold{p}_{B}^L}^T \ {\bold{q}_{B}^L}^T] \tag{1}
上面的公式中：
{\bold{p}_{B_i}^W} 是当前body frame相对于世界坐标系的位置 ；
{\bold{v}_{B_i}^W} 是当前body frame相对于世界坐标系的速度 ；
{\bold{q}_{B_i}^W} 是当前body frame相对于世界坐标系的姿态 ；
\bold{b}_a是IMU的加速度 偏置；
\bold{b}_g是IMU的角速度 偏置；
2）动态模型
输入：IMU的加速度和角速度；
输出：{\bold{X}_{B_k}^W}，将{\bold{X}_{B_i}^W} 更新到{\bold{X}_{B_j}^W} ；
更新方程如下：
方程内：
\Delta t是两次IMU连续测量的时间间隔；
我们积分的时间区间是两次雷达扫描之间的时间；
\bold{g}^W是世界坐标系下的重力；
简写：
(\cdot)_i \dot{=}(\cdot)^W_{B_i},
\Delta t_{ij}=\Sigma ^{j-1}_{k=i}{\Delta t} ,
\prod_{k=i}^{j-i}是四元素的连乘；
3）预积分
在时刻 i 到时刻 j 之间，body frame 的运动可以通过预积分得到：
z_j^i=\{\Delta \bold{p}_{ij},\Delta \bold{v}_{ij},\Delta \bold{q}_{ij}\}
z^i_j具有协方差\bold{C}^{B_i}_{B_j}
这部分参考补充材料。
IV. TIGHTLY COUPLED LIDAR-IMU ODOMETRY
受到[7],[1],[3]的启发，他们将里程计和建图两部分分开。
本文也分成两个部分：
Sec. IV介绍紧耦合的lidar-IMU里程计，在局部窗口中优化所有的状态；
Sec. V介绍rotation constrained refinement 过程，这是一个全局建图过程。
A. Lidar-IMU Odometry Overview
lidar-IMU里程计系统整体框图：
输入是 lidar raw inputs S_j 和 IMU raw inputs I_{i,j}
步骤：
1）在 S_j 接收到之前，IMU状态通过公式-2进行迭代;
2）同时预积分得到 \Delta \bold{p}_{ij},\Delta \bold{v}_{ij},\Delta \bold{q}_{ij}；
3）接收到 S_j 后，进行校正，然后得到 \bar{S}_j;
4）进行特征抽取，给数据降维，得到最重要的特征点 \bold{F}_{L_j};
5）上一次雷达特征点 \bold{F}_{L_{o,i}} 根据上一次的对应优化状态 \bold{T}^W_{b_{o,i}} 和 \bold{T}^L_B，得到一个 local map \bold{M}^{L_{p}}_{L_o,i{}};
6）根据对 \bold{F}_j时的 lidar pose 的预测，得到 lidar 的性对测量 \bold{m}_{L_{p+1,j}};
7）联合优化，根据雷达得到的两帧之间的相对变换和IMU的预积分进行最大后验估计，估计的结果再去更新1）中 IMU 状态，避免 drift。
B. De-skewing and Feature Extraction
这一部分对应上面总体框图的步骤3，步骤4。
3D雷达在运动时得到的数据会有失真，解决方法如下：
1.使用和LOAM中类似的方法，假设了雷达的匀速运动模型。
2.然后从IMU状态中预测出 \bold{\bar{T}}^{L_j}_{L_{j'}}。
3.对于每个点 \bold{x}(t) \in S_j \subset \mathbb{R}^3,通过 \bold{\bar{T}}^{L_j}_{L_{j'}} 线性插补，进行位姿校正并转换坐标系到 ending pose of the sweep，得到 \bar{S}_j。
4.这一 sweep 中的时间戳 t \in (t_{j'},t_j],t_{j'} 是 sweep 的开始，t_{j} 是 sweep 的结束。
使用与LOAM中相同的提取特征边和特征平面的方法提高计算效率。
\bold{F}_{L_j} \in \bar{S}_j通过曲率进行选择，选择最平的和最陡的。
C. Relative Lidar Measurements
这一部分对应上面总体框图的步骤5，步骤6。
为了和IMU预积分相适应，对于 lidar 数据采用 sweep 之间的相对位姿运动。算法如下面框图所示：
因为单个 sweep 不够稠密，计算准确的特征对应很困难，所以需要先建立一个local map。
local map包含 N_m 个离散的时刻：
\{o,...,p,...,i\}
如下图所示，
o 是第一个lidar sweep；
p 是 pivot lidar sweep；
i 是最后一个 lidar sweep；
local map \bold{M}^{L_{p}}_{L_o,i{}} 采用 \bold{F}^{L_{p}}_{L_{\gamma}} ,\gamma \in \{o,...,i\}的数据，并建立在pivot坐标系上。
\bold{F}^{L_{p}}_{L_{\gamma}} 经过上一时刻优化得到的 lidar poses \bold{T}^{L_{p}}_{L_{\gamma}} （这里简写了，准确来说是\bold{\bar{T}}^{L_{p}}_{L_{\gamma}} ）。
需要被预测的状态是： \{p+1,...,i,j\}，里面：
p+1是 pivot的下一个时间戳；
j 是窗口中当前的 lidar sweep。
有了local map之后，可以找到 \bold{M}^{L_{p}}_{L_o,i{}} 和原始 \bold{F}_{L_{\alpha}} ,\alpha \in \{ p+1,...,j\} 之间的对应关系。
这种对应关系实质就是相对雷达运动，他们两者都是相对于他们的 pivot 位姿的运动，pivot 位姿会随着滑动窗口变化。
在 Sec. IV-B 中抽取的原始特征是坐标系 F_{L{\alpha}} 中最平坦或者是 edge 点。
在实际中，edge对效果提高不明显，所以我们后面只针对平面点。
使用KNN 找到 \bold{x}^{L_p} \in \bold{F}^{L_{p}}_{L_{\alpha}} 的K 个最近点 \pi(\bold{x}^{L_{p}}) \in \bold{M}^{L_{p}}_{L_o,i{}}；
然后将这些点拟合到坐标系 F_{L_{p}} 中的一个平面上去；
平面的系数通过如下方程确定：
\bold{ \omega }^{T}\bold{x'}+d=0, \bold{x'} \in \pi(\bold{x}^{L{p}})
\bold{\omega }是平面的法向量；
d 是距离坐标系 F_{L_{p}} 原点 o 的距离；
对于每个平面特征点 \bold{x} \in \bold{F}_{L_{\alpha}},m=[ \bold{x},\omega ,d] \in \bold{m}_{L{\alpha}} 是它的相对雷达运动。
每个m \in \bold{m}_{L_{\alpha}} , \bold{x}定义在坐标系 F_{L_{\alpha}} 中，d 定义在坐标系 F_{L_{p}} 中。
D. Lidar Sweep Matching
这一部分对应上面总体框图的步骤7。
相对位姿提供的是pivot 和后续lidar poses之间的相对运动。
优化过程会优化窗口中的所有位姿，包括第一个位姿 \bold{T}^W_{L_p}（也就是说pivot所在的坐标系 F_{L_p}不是固定的）。
因此在lidar的 代价函数中，每个位姿都会涉及到两个量： \bold{T}^W_{L_p} 和 \bold{T}^W_{L_{\alpha}},\alpha \in \{p+1,...,j\}。
优化 pivot 的 pose 可以减小预积分误差。
估计的状态都是IMU的，因此需要通过IMU状态引入外参来表示 lidar constraints。
pivot 后面的 lidar poses 相对于 pivot 的相对位姿变换可以这样定义：
有了前面的对应关系，每个lidar 测量m=[ \bold{x},\omega ,d] \in \bold{m}_{L{\alpha}},\alpha \in \{p+1,...,j\} 的残差都可以表示成点到平面的距离。
E. Optimization
这一部分对应总体框图的步骤7。
优化状态的获得通过 fixed-lag 平滑和边缘化。
fixed-lag 平滑在滑动窗口内保持 N_s 个IMU 状态（\bold{X}^W_{B_p} 到 \bold{X}^W_{B_j}），图2所示。
当有新的测量数据进啦，平滑器将加入新的量，将窗口中最旧的量边缘化。
整个要估计的量是：
\bold{X}=[\bold{X}^W_{B_p}, ..., \bold{X}^W_{B_j}, \bold{T}^{L}_{B}] \tag{5}
然后去求下列马氏距离的代价函数的最小值，获的 \bold{X} 的最大后验估计。
上面公式中：
\bold{r}_{\mathcal{P} }(\bold{X}) 是边缘化的先验项；
\bold{r_{\mathcal{L}}}(m,\bold{X}) 是 relative lidar constraints；
\bold{r_{\mathcal{B}}}(z^{\beta}_{\beta +1},\bold{X}) 是 residual of the IMU constraints；
上面的非线性最小二乘的代价函数可以通过 Gauss-Newton 法按照 \bold{H \delta X=-b} 的形式求解。LIO通过Ceres Solver 进行求解。
对于每个 lidar 的相对测量，可以从等式-4中获得 \bold{r_{\mathcal{L}}}(m,\bold{X})；
协方差矩阵 \bold{C}^{m}_{L_{\alpha}} 由 lidar accuracy 决定；
IMU constraint \bold{r_{\mathcal{B}}}(z^{\beta}_{\beta +1},\bold{X}) 通过IMU的预积分获得，和[10]类似；
||\bold{r}_{\mathcal{P} }(\bold{X})||^2=\bold{b}^T_{\mathcal{P} }\bold{H}^+_{\mathcal{P} }\bold{b}_{\mathcal{P} } 可以通过舒尔补获得[文献12]；
V. REFINEMENT WITH ROTATIONAL CONSTRAINTS
将特征点注册到全局地图坐标系 F_W 而不是局部地图，能使lidar poses 具有一致性。refinement 方法使用相对lidar 测量 \bold{m}_L。
对齐最近的lidar 特征点到global map：
上面的公式中：
\bold{T}=\bold{T}^W_L 是小估计的lidar poses；
相对lidar 测量m 中包含的 \bold{x} 定义在坐标系 F_L 中，系数 \omega,d 定义在 F_W 中；
然后用一个相似的Gauss-Newton方法去优化 \bold{C}_{\mathcal{M}};
优化方式：通过残差 \bold{C}_{\mathcal{M}}，\bold{J_p^C}, \bold{J_{\theta}^C} , \theta 是对应于四元素 \bold{q} 的误差状态。
长时间的误差累积会使建图错误更大，LIO采取了和文献[16]类似的方法，以 SE(2)-constraints 优化SE(3)。
这种方法保证了最后的图总是和重力是对齐的。
图3说明了这种方法：
z轴方形的姿态有更高的不确定，而roll和pitch更准确，因此姿态的Jacobian对代价函数进行限制（详细的推导在文献[12]中）：
上面公式中：
\breve{(\cdot)} 是上一次迭代的状态；
\breve \bold{\Omega}_z 是姿态相对于坐标系 F_W 的信息矩阵的近似值；
\epsilon_{x} , \epsilon_{y}通过information ratio 获得；
然后，在优化步骤中通过\bold{J_p^C}, \bold{J_{\theta_z}^C}作为Jacobians，lidar poses 的增量可以表示为 \delta\bold{\theta}_z 和\delta\bold{p}，然后获得状态的更新：
VI. IMPLEMENTATION
介绍关于以下四个方面的内容：
- 传感器配置
- 系统初始化
- 参数
- 室内及室外测试
A. Different Sensor Configurations
lidar和IMU距离很近的时候，算法按照上面介绍的进行就可以；
lidar和IMU距离远的时候，需要将外参加到 等式-6 (总体的最小二乘代价函数)，如下图：
B. Initialization
1.粗略的匹配算法使用LOAM的初始化方式；
2.当IMU和 lidar 的测量数据足够多的时候，可以用这些数据进行初始化；
3.在实验中，采用文献[10]的方式进行线性外参初始化；
4.最后再滑动窗口内进行非线性优化。
VII. TESTS AND ANALYSES
进行了室内和室外的实验，并提供定性和定量的分析。
A. Quantitative Analysis
1）不同环境下的测验：
LIO 是带有窗口优化的方法；
LIO-raw 是没有运动补偿的方法；
LIO-no-ex 是没有在线外参估计的方法；
LIO-mapping 是带有 rotaional constraints的方法，效果最好，速度在高速时效果更好，低速时漂移多一点。
2）随时间漂移
LIO-mapping 的效果比LOAM或LOAM+IMU的效果好。
B. Qualitative Results
进行了 KAIST Urban dataset 的实验，在视频中展示结果。
C. Running Time Analysis
配置：Intel i7-7700K CPU at 4.20GHz,16GB RAM
总结了室内室外雷达的建图运行时间。
VIII. CONCLUSION
1.提出了紧耦合的 lidar-IMU 融合方法；
2.LIO需要初始化，但是也更加鲁棒，更新频率也更高。