十七.激光和惯导LIO-SLAM框架学习之IMU和IMU预积分

专栏系列文章如下：

一：Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono)，基于视觉-激光-惯导里程计的SLAM框架，环境搭建和跑通过程_goldqiu的博客-CSDN博客

二.激光SLAM框架学习之A-LOAM框架---介绍及其演示_goldqiu的博客-CSDN博客

三.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---1.项目文件介绍（除主要源码部分）_goldqiu的博客-CSDN博客

四.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---2.scanRegistration.cpp--前端雷达处理和特征提取_goldqiu的博客-CSDN博客

五.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---3.laserOdometry.cpp--前端雷达里程计和位姿粗估计_goldqiu的博客-CSDN博客

六.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---4.laserMapping.cpp--后端建图和帧位姿精估计（优化）_goldqiu的博客-CSDN博客

七.激光SLAM框架学习之A-LOAM框架---速腾Robosense-16线雷达室内建图_goldqiu的博客-CSDN博客

八.激光SLAM框架学习之LeGO-LOAM框架---框架介绍和运行演示_goldqiu的博客-CSDN博客

九.激光SLAM框架学习之LeGO-LOAM框架---速腾Robosense-16线雷达室外建图和其他框架对比、录包和保存数据_goldqiu的博客-CSDN博客

十.激光SLAM框架学习之LeGO-LOAM框架---算法原理和改进、项目工程代码_goldqiu的博客-CSDN博客

十一.激光惯导LIO-SLAM框架学习之LIO-SAM框架---框架介绍和运行演示

十二.激光SLAM框架学习之livox-loam框架安装和跑数据集_goldqiu的博客-CSDN博客_livox 数据集

十三.激光SLAM框架学习之livox-Mid-70雷达使用和实时室外跑框架_goldqiu的博客-CSDN博客

十四.激光和惯导LIO-SLAM框架学习之惯导内参标定_goldqiu的博客-CSDN博客

十五.激光和惯导LIO-SLAM框架学习之惯导与雷达外参标定（1）_goldqiu的博客-CSDN博客

十六.激光和惯导LIO-SLAM框架学习之配置自用传感器实时室外跑LIO-SAM框架_goldqiu的博客-CSDN博客

IMU

介绍及选型

IMU的全称是惯性测量单元，包括⼀个三轴的加速度计以及一个三轴的陀螺仪，分别测量出物体的加速度和角速度信息，不受周围环境结构，光照等外界因素影响；IMU可以获得当前时刻的角速度和加速度值，通过该值可以对系统状态（位置，速度，姿态）进行推算。

通常IMU的输出频率在100-1000hz之间，远高于相机或者激光雷达的输出频率，一方面可以提高整体系统的输出频率，另一方面，可以在视觉或者激光短期失效的时候提供一段时间的位姿推算。在大多数的LIO（雷达惯性里程计）或者VIO（视觉惯性里程计）中，关于IMU输出的建模方式为

输出的加速度计和陀螺仪的数据受零偏（ba和bw）以及高斯白噪声（na和nw）的影响，紧耦合的LIO或者VIO都会实时估计IMU的零偏，以实现IMU数据的最大利用率，达到融合效果最大化。

这里的加速度要减去重力加速度或者重力加速度的分量才是实际的加速度，而且传感器测得的是世界坐标系下的加速度，然后左乘一个将世界坐标系转换到机体坐标系的旋转矩阵，得到的才是物体的加速度。

然而，实际的IMU数据并没有这么理想，除了零偏和高斯白噪声，还有可能受到刻度误差，尺度因子，轴向偏差等影响，如果把这些因素都通过建模的方式考虑进来，就显得过于复杂；因此，通常的做法是在IMU选型的时候就最大化避免上述误差的影响。也就是说，在选择IMU型号时除了关注价格（价格很多时候是非常重要甚至是决定性的因素），还需要关心其出厂标定情况，是否做过温度补偿等。

IMU标定

无论是VIO还是LIO，IMU和其他传感器的标定结果往往对最终的里程记和建图性能有着显著的影响。同样，在多传感器融合算法中，传感器之间的标定结果的精度对多传感器融合的效果也有着非常大的影响。在lidar-imu融合的算法中，lidar和IMU之间的外参标定非常重要，在一些数据集上往往有着经过良好标定的结果，然而，绝大多数情况下，传感器之间的外参需要我们自己去标定。

LIO-SAM中，作者推荐了一个Lidar-IMU标定的功能包来实现lidar和IMU之间的外参标定。

https://github.com/chennuo0125-HIT/lidar_imu_calibgithub.com/chennuo0125-HIT/lidar_imu_calib

通常来讲，lidar和IMU之间的旋转外参相比于平移外参对里程记的性能有着更显著的影响。因此，条件有限的情况下，可以只标定lidar和IMU之间的旋转外参，平移外参则可以通过手工测量等方式获取。

该旋转标定算法本质上是⼀个手眼标定算法，即先计算出两帧lidar之间的旋转（通过ICP、NDT等点云配准算法），然后计算出两帧IMU之间的旋转（IMU陀螺仪积分），然后通过手眼标定的方式求解出外参，和VINS-Mono中相机IMU旋转外参初始化的过程⼀致，具体方式为：

使用四元数来表示旋转，四元数的乘法可以通过一个公式转成矩阵的乘法。
收集若干组IMU和lidar的相对旋转和平移，联立求解，相当于已知一个4n×4大小的矩阵，求出⼀个4×1向量的最优解，通常n>4；这是一个基本的超定方程求解问题，通常使用SVD方法求解，即将A矩阵进行SVD分解。求解出来的x转换成四元数即为所求的旋转。

前面讲过IMU内参的标定和外参的标定，外参用的是瑞士苏黎世理工大学-自动驾驶实验室开源的lidar_align工程进行标定。

链接如下：

goldqiu：十五.激光和惯导LIO-SLAM框架学习之惯导与雷达外参标定（1）

IMU预积分

为什么需要预积分

当k时刻的状态发生变化时，通过imu积分得到的k+1时刻的状态也会发生相应的变化，而在基于滑窗的后端优化或者因子图的优化中，对一些状态量进行调整是必然发生的。如果每次状态发生调整时imu的积分过程都会重新执行一遍，则实时性无法得到保证。因此，预积分理论就是为解决这个问题而提出的，其核心思想就是IMU积分的结果和上⼀时刻系统的状态无关。这样，当系统状态在优化过程中发生调整的时候，就不需要对下⼀时刻的系统状态重新积分。

关于零偏的建模

通常来说，IMU的零偏会随着时间的变化而偏移，因此为了系统的准确性，零偏也是系统的优化变量之一。预积分量虽然和两帧的具体位姿和速度等状态量无关，但是和零偏相关，因此，当零偏作为优化变量被优化后，预积分量也会发生相应的变化，那么此时我们是否需要重新积分呢？答案是不用的。为了避免零偏的变化导致预积分量重新积分，通常认为零偏的变化在短时间内（100ms）非常小，因此，我们可以使用一阶泰勒展开来进行近似。

预积分量对于零偏的雅克比矩阵会在预积分计算的时候一并用于计算，因此，当零偏被优化调整之后，会计算预积分量对于零偏的雅克比矩阵，然后进行预积分时更新过去即可（即更换预积分量对于零偏的雅克比矩阵）。

离散时间的预积分更新

实际系统是离散的IMU数据，我们的目的是得到两个关键帧（视觉 or lidar）之间的预积分结果，而IMU的频率是远大于雷达和相机的。常见的做法就是每收到⼀帧新的IMU数据就更新⼀次预积分量，这是⼀个求和而非连续积分的过程，而到了相机或者雷达的数据帧后，就用当前更新的预积分量去融合。

预积分在优化问题中的建模

预积分量约束相邻两帧的状态量（位置、速度、姿态），同时考虑到IMU零偏的性质（即短时间内变换速率比较缓慢，可以认为两帧之间的零偏不变）也就是还可以约束两帧的零偏变化。在slam问题中，IMU预积分约束通常会和其他传感器的约束一起构建一个优化问题。

视觉VIO中预积分和视觉的优化建模

这里黄色代表IMU预积分，只对相邻帧发生约束，五角星代表视觉地图点，同一个地图点可以通过视觉重投影误差对多个关键帧的位姿形成约束。

LIO-SAM中预积分和lidar位姿的优化建模

这里蓝色圆圈代表关键帧位姿，蓝色矩形代表关键帧速度和零偏，橙色矩形代表IMU预积分约束，它可以约束相邻帧的位姿、速度和零偏，绿色矩形代表lidar里程记的帧间约束，其约束相邻两帧的位置和姿态。这里包括了lidar里程计因子和预积分因子，是预积分节点因子图的优化模型。