Simple-LIO-SAM——（八）IMU预积分模块

发表于 2023-03-30 更新于 2024-09-19 分类于 SLAM ， LIOSAM 阅读次数： Changyan：

IMU预积分

IMU预积分模块无论在VIO或者LIO中都是一个非常重要的模块。VIO或者LIO中的I，也就是Inertial指的是惯性单元，基本上在这类SLAM框架里指的就是IMU里程计。 IMU可以输出车辆坐标系下的三轴加速度、三轴角速度、三轴地磁计（九轴IMU）。我们都知道，加速度的积分是速度、速度的积分是距离，角速度的积分是角度，地磁计可以计算与地磁北极的偏角，因此，如果IMU的输出是准确的话，使用IMU原始数据并进行积分就可以得到车辆的位姿。然而，目前无论多么高精度的IMU，都很难在较长时间下保持较高的准确度，而且积分会对误差也进行积分，导致IMU里程计只能用来做短时间内的位姿校正。

LIOSAM是IMU里程计和激光雷达里程计紧耦合的SLAM框架。“紧耦合”三个字在IMU预积分模块得到明显的表达。如果用一句话说明IMU预积分模块的作用，那就是：利用IMU数据，为车辆位姿提供一个较好的位姿初始估计。由于目前的激光雷达大部分是10Hz，部分可以达到20Hz以上，因此，在两帧雷达点云期间，车辆会发生旋转和位移，如果旋转和位移很小，那么点云匹配就能够单独算出一个较准确的里程计（在运动缓慢的情况下，LIOSAM即使去除了IMU预积分也可以工作，只需要不启动ImuPreintegration节点即可）。但是如果旋转和位移较大，点云匹配就很可能失败。IMU一般拥有较高频率（200Hz以上），因此可以为两帧雷达点云的匹配提供较好的初始位姿值。

前面提到单纯利用IMU数据做积分，时间一长累积误差会很大，无法提供良好的初始位姿估计，因此需要融合其他传感器校正IMU积分算法。在LIOSAM中，通过不断从激光里程计获取最新的里程计信息，作为IMU积分算法的观测来达到校正IMU积分算法的目的。

PS：此部分代码很大程度利用了GTSAM，因此，需要提前阅读本项目Simple-LIO-SAM——（七）GTSAM快速入门

IMU预积分模块的整体框架流程图

零偏估计和IMU里程计

这个模块容易让人看晕的地方在于模块里有两个队列缓存IMU原始数据，同时有两个IMU预积分器。之所以需要有两个预积分器的原因在于，GTSAM中对于IMU的预积分，在预测时（predict）需要指明上一时刻的bias，也就是IMU的零偏。通常这个bias会随着时间慢慢改变，因此不能用离线标定的数据直接使用。于是，在LIOSAM中，有一个线程（LaserHandler）会利用激光里程计当作额外的观测量，不断估计最新的bias值。而另一个线程（ImuHandler）则可以一直利用最新的bias值对实时接收到的IMU数据做积分，并发布为IMU里程计。

为了简单起见，我们将用来做零偏估计的队列和预积分器叫做优化队列和优化预积分器；将用来实时对IMU数据积分并发布IMU里程计的队列和预积分器叫做IMU队列和IMU预积分器。

一图胜千言，下面这幅图描述了两个队列是如何被耦合起来输出更为准确的IMU预积分结果。其中imuQueOpt是优化队列，imuQueImu是IMU队列，两者命令与代码中一致。

零偏估计线程——即odometryHandler

这个回调函数占据了预积分模块大部分内容，而且有较多的重置条件和与imuHandler线程同步的操作，比较容易弄混。

这个回调函数会在接收到新的一帧雷达里程计（发自MapOptimization模块），主要操作上图中的imuQueOpt和优化预积分器。每当节点新收到一帧雷达里程计时，就会执行预积分模块流程图中左侧的一系列操作。主要目的就是利用雷达里程计做观测数据，实时更新IMU的偏差bias。同时，更新过的bias会传递到IMU预积分器。

系统初始化

当接收到第一帧雷达里程计时，需要对系统做初始化操作。主要包括因子图、因子图优化器、两个预积分器的初始化。由于初始化操作需要将第一帧雷达里程计的位姿作为先验因子，因此不能在构造函数中初始化。

判断是否应该重置因子图

为了保证拥有足够的实时性（预积分模块需要与IMU频率保持一致），LIOSAM的预积分模块在因子图中里程计因子数量大于100时会重置因子图与优化器。操作主要是从当前的因子图优化器拿出最新结果，并当作先验因子构造新的因子图。

对优化队列中IMU原始数据进行积分

这里要重新回顾上文的线程交互图。零偏估计线程的主要目的就是为IMU里程计线程提供最新的bias估计，因此每当到达新的一帧雷达里程计，意味着有新的观测到达，bias就可以进行更新。于是需要对优化队列中在当前雷达里程计时间之前的IMU原始数据用IMU预积分器进行积分，然后构造IMU因子。将IMU因子和雷达里程计加入因子图优化，得出最新的bias估计。

因子图优化

GTSAM可以使用IMU预积分器构造IMU因子。利用GTSAM可以用因子图优化出速度,IMU bias,位姿的估计。其中IMU bias是我们要重点关注的内容。

传递优化结果

使用因子图优化出最新的IMU bias 之后，实际上还有优化后的位姿，这个位姿和IMU偏差会用来重置另外一个预积分器——IMU预积分器。IMU预积分器是在另一个回调函数中被主要使用，回顾线程交互图的下半部分和模块流程图的右侧流程。同时，设置完IMU预积分器之后，会将imuQueImu中剩余的其他IMU原始数据放入IMU预积分器做积分，这样IMU预积分器就一直保持最新状态，在imuHandler中可以实时积分并发布IMU里程计。

IMU里程计线程——即imuHandler

这个回调函数比起odometryHandler就要简单很多。这个回调函数的主要功能就只有两个：

1、不断地将新收到的IMU数据塞入两个放置IMU原始数据的队列——线程交互图中的imuQueOpt和imuQueImu； 2、使用IMU预积分器对收到的IMU数据做积分，并将结果作为IMU里程计发布。

主要流程如下图：

这个回调函数比较简单，就不再赘述。

代码要点

坐标系转换

IMU预积分模块中代码涉及到多次坐标系转换。主要是要明白IMU积分的原理和LIOSAM中坐标系的转换关系。关于坐标系有几个注意要点：

1. 所有IMU原始数据都会先旋转到与雷达的坐标系朝向一致后再进行处理

在imuHandler中每收到一个IMU数据就先旋转再存放。code

// 将IMU数据旋转到lidar的旋转朝向
sensor_msgs::msg::Imu thisImu = imuConverter(*imu_raw);

// 缓存IMU数据
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);

2. 里程计都是对齐到雷达坐标系

所有在LIOSAM框架中发布出来的里程计，无论是IMU里程计或者是雷达里程计，度量的都是雷达在地图坐标系下的位姿。但是在IMU预积分模块的零偏估计线程中，使用IMU数据做积分，又需要使用雷达里程计作为观测，两者要同时加入因子图中，所以必须统一两者的坐标系。IMU原始数据在上面提到的第一个注意要点已经旋转到对齐雷达的方向，两者之间还差一个平移。因此，在接收到雷达里程计时，会先平移到旋转后的IMU坐标系，最后再平移回来。雷达里程计平移到旋转后的IMU坐标系代码

1
2
3

// 初始化因子图的prior状态
// 将雷达里程计位姿平移到IMU坐标系，只是做了平移
prevPose_ = lidarPose.compose(lidar2Imu);

将IMU里程计对齐到雷达坐标系代码

1
2
3

// 将IMU里程计完全对齐到雷达（剩下一个平移关系）
gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);

时间同步

LIOSAM框架中各个模块都是进行异步通信（ros topic），因此在每个模块里面都有几个队列保存数据，处理的时候再根据时间戳做同步。IMU预积分模块中有多个地方对数据做了时间戳同步。比如在更新完bias之后，对IMU队列做积分时，会先抛弃早于最新的雷达里程计时间之前的数据。code link

// 同样先做IMU数据队列和雷达里程计的时间同步
double lastImuQT = -1;
while (!imuQueImu.empty() && stamp2Sec(imuQueImu.front().header.stamp) < currentLidarOdomTime - sync_t)
{
    lastImuQT = stamp2Sec(imuQueImu.front().header.stamp);
    imuQueImu.pop_front();
}