LIO-SAM-6轴IMU_lio-sam 6axis

参考文章：

LIO_SAM 6轴 - 知乎

LIO_SAM_6AXIS 适配香港城市数据集UrbanNav - 知乎

参考代码：

 GitHub - JokerJohn/LIO_SAM_6AXIS: LIO_SAM for 6-axis IMU and GNSS.

 GitHub - nkymzsy/LIO-SAM-MID360

前言：

LIO-SAM中源代码使用的9轴IMU，在这里我们想要LIO-SAM代码适配6轴IMU，首先我们要了解9轴IMU和6轴IMU区别是什么？6轴IMU能够测量的量有：xyz方向上的线角速度和xyz方向上的角速度，9轴IMU能够测量的量有：xyz方向上的线速度、xyz方向上的角速度和当前时刻IMU的RPY值（欧拉角）。

因此在9轴IMU中，欧拉角可以通过两种方式获取：直接从传感器测量的原始数据计算得到的欧拉角（即RPY值），或者通过对传感器的测量数据进行积分得到的欧拉角（即预积分欧拉角）。

而6轴IMU中，欧拉角只能通过预积分的方式获取。

坐标系：

在九轴IMU中多了一个磁力计，磁力计的坐标系和加速度计和陀螺仪的坐标系一般为IMU的本体坐标系。

LIO-SAM-6轴IMU思路：

了解了6轴IMU和9轴IMU的区别后，我们就可以去查看LIO-SAM中哪里用到了9轴IMU中的RPY数据。

源头：ImageProjection::imuHandler()  和IMUPreintegration::imuHandler() 中调用imuConverter()从这里接受的imu原始数据，并将imu的RPY保存下来给后面的函数使用。

去向：mapOptimization中使用。

通过读取9轴IMU中的RPY的数据流发现，imu的原始RPY在两个环节进行了使用：

• 1）作为 第一帧 雷达的初始姿态角
• 2）用于scan-to-map预估的位姿和IMU的RPY数据进行姿态角融合

LIO-SAM-6轴方法：

基于上述分析，为了尽可能减少对源代码的改动，针对imu的原始RPY在两个地方的使用方法，在原理上我们只需要做出以下两点改动：

• 1）每一帧的IMU的RPY值我们用单位矩阵代替。
• 2）在进行姿态角融合时，将IMU的RPY权重设置为0。

基于以上原理我们不需要改动LIO-SAM的结构，只需要做出以下主要修改即可：

在params.yaml中：

 #1.设置IMU的RPY值为单位矩阵 
 extrinsicRPY: [1, 0, 0,
                0, 1, 0,
                0, 0, 1]
 
 #2.设置IMU的RPY权重为0
 imuRPYWeight: 0.00

在utility.h文件imuConverter函数中：

    #设置imu的RPY值为单位向量，extQRPY是单位矩阵转化的四元数，前面还有一些操作
    if (imuType == “”6轴“”)
    {
        q_final = extQRPY;
        q_final.normalize();
        # 6轴imu本来没有orientation这个数据，这里人为设置。
        imu_out.orientation.x = q_final.x();
        imu_out.orientation.y = q_final.y();
        imu_out.orientation.z = q_final.z();
        imu_out.orientation.w = q_final.w();
    }

在mapOptmization.cpp文件publishOdometry()函数中：

    if (cloudInfo.imuAvailable == true)
    {
        // 如果俯仰角度小于这个值，对roll角和pitch角进行加权平均一下
        if (std::abs(cloudInfo.imuPitchInit) < 1.4)
        {
            double imuWeight = 0.1; #6轴IMu这里权重应该设置成0
            tf::Quaternion imuQuaternion;
            tf::Quaternion transformQuaternion;
            double rollMid, pitchMid, yawMid
            // roll姿态角加权平均
            transformQuaternion.setRPY(roll, 0, 0);
            imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
            tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
            roll = rollMid
            // pitch姿态角加权平均
            transformQuaternion.setRPY(0, pitch, 0);
            imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
            tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
            pitch = pitchMid;
        }
    }