IMU器件

IMU的全称是惯性测量单元，包括一个三轴的加速度计以及一个三轴的陀螺仪，分别测量出物体的加速度和角速度信息，不受周围环境结构、光照等外界因素影响。通常IMU的输出频率在100-1000hz之间，远高于相机或者激光雷达的输出频率，一方面可以提高整体系统的输出频率，另一方面，可以在视觉或者激光雷达短期失效的时候提供一段时间的位姿推算。

在大多数的LIO或者VIO中，关于IMU的输出的建模方式如下

角速度部分：

其中

为t时刻测量值

为t时刻真实角速度值

为t时刻零偏

为t时刻噪声

加速度部分

除了零偏和噪声外，也有重力加速度影响
at和g是在世界坐标系下的值

通过上面的模型可知，输出的加速度计和陀螺仪的数据受零偏及高斯白噪声的影响，因此紧耦合的LIO或者VIO都会实时估计IMU的零偏，以实现IMU数据的最大利用率。

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

IMU状态传递方程

IMU如何进行位姿估计

IMU可以获得当前时刻的角速度和加速度值，通过该值可以对系统状态（位置，速度，姿态）进行推算

连续时间两个关键帧bk和bk+1 之间的状态传递公式如下：

平移：

• 公式原理
  
• 用积分的形式表示如下
  

速度：

• 公式原理
  
• 用积分的形式表示如下
  

旋转

• 用积分的形式表示如下
  对四元数求导，相当于积分里面的内容
  
  其中
  

在实际用用的时候，计算机接受的的传感器数据都是离散形式的，所以需要对上面进行离散化

每当收到一帧新的imu数据后，系统状态变化为：

将上面的k和k+1，换成了i和i+1，其中i是IMU的第i帧，k是IMU的第k关键帧。k和k+1帧直接有n帧imu数据。i和i+1帧是没有的

位置离散化为：

速度离散化为：

旋转离散化为：

四元数的模长为1，矩阵里的实部为1，后面的虚部时间非常短，近似为0

IMU预积分

IMU预积分的作用

当k时刻的状态发生变化时，则通过imu积分得到的k+1时刻的状态也会发生相应的变化，而在基于滑窗的后端优化或者因子图的优化中，对一些状态量进行调整是必然发生的，此时，如果每次状态发生调整时，都对imu的积分过程重新执行一遍（但是IMU数据并未发生变换），则实时性必然无法得到保证，因此，预积分理论就是为解决这个问题而提出的(避免重复积分)。

IMU预积分的核心思想

其核心思想就是对IMU积分的结果和上一时刻系统的状态无关，这样，当系统状态在优化过程中发生调整的时候，就不需要对下一时刻的系统状态重新积分。

IMU预积分方法

根据上面连续时间IMU积分的公式，等号两边乘上世界坐标系到机体坐标系的变换

其中

上面的三个变量即预积分量，这三个预积分量都和k时刻或k+1时刻状态无关，因此当k时刻状态发生变化时，不需要对IMU数据重新进行积分

仅和k时刻的机体坐标系有关

关于零偏的建模

通常来说，IMU的零偏会随着时间的变化而偏移，因此为了系统的准确性，零偏也是系统的优化变量之一，预积分量虽然和两帧的具体位姿和速度等状态无关，但是和零偏相关，因此当零偏作为优化变量被优化后，预积分量也会发生相应的变化，那么如果重新积分，预积分的作用就没有了。

为了避免零偏的变化导致预积分量重新积分，考虑到通常零偏的变化在短时间(100ms)非常小，因此，我们可以使用一阶泰勒展开来进行近似，具体为：

这里预积分量关于零偏的雅可比矩阵会在预积分计算的时候一并计算，因此，当零偏被优化调整后，只需要根据事先计算号的雅可比矩阵对预积分量进行更新即可。

离散时间的预积分更新

同样，实际系统是离散得IMU数据，目的是得到两个关键帧（视觉或者激光雷达）之间的预积分结果，而获得的IMU数据是离散的，因此，常见的做法就是每收到一帧新的IMU数据更新一次预积分量，同样，这是一个求和而非连续积分的过程

当收到新的IMU数据后，预积分量更新公式如下：

IMU预积分实现

• IMU预积分根据上面的理论推导，便可以手动进行代码的实现。可以参考 orb3 或者VINS-MONO中的代码
• IMU预积分在LIO-SAM中是通过GT-SAM库实现的。

重点部分

    Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,
                                          const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
    {//get IMU residuals   Qi transform coordinate from i to w; Vi volosity at time i in w coordinate
        Eigen::Matrix<double, 15, 1> residuals;
//    O_P = 0,O_R = 3, O_V = 6, O_BA = 9, O_BG = 12
        Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);//0,9
        Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);//0,12

        Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);//3,12

        Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);//6,9
        Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);//6,12

        Eigen::Vector3d dba = Bai - linearized_ba;
        Eigen::Vector3d dbg = Bgi - linearized_bg;
        //cuihuakun p6-(6)  // IMU预积分的结果,消除掉acc bias和gyro bias的影响, 对应IMU model中的\hat{\alpha},\hat{\beta},\hat{\gamma}
        Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
        Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
        Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
        //ppt ch3-p69 // IMU项residual计算,输入参数是状态的估计值, 上面correct_delta_*是预积分值, 二者求'diff'得到residual.
        residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
        residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
        residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
        residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
        residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
        return residuals;
    }

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{   //获取当前时间
    double t = imu_msg->header.stamp.toSec();
    //首帧判断
    if (init_imu)
    {latest_time = t;init_imu = 0;return;}
    //获取dt并传递时间
    double dt = t - latest_time;
    latest_time = t;
    //获取当前时刻的IMU采样数据
    double dx = imu_msg->linear_acceleration.x;
    double dy = imu_msg->linear_acceleration.y;
    double dz = imu_msg->linear_acceleration.z;
    Eigen::Vector3d linear_acceleration{dx, dy, dz};
    double rx = imu_msg->angular_velocity.x;
    double ry = imu_msg->angular_velocity.y;
    double rz = imu_msg->angular_velocity.z;
    Eigen::Vector3d angular_velocity{rx, ry, rz};
    //注意，以下数据都是世界坐标系下的
    Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;
    Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
    tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);
    Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;
    Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
    tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;
    tmp_V = tmp_V + dt * un_acc;
    //信息传递
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31