IMU里程计

news2024/11/20 14:30:21

前言：

IMU里程计和视觉里程计、激光里程计等一样，都可以独立计算里程计信息。求解IMU里程计的步骤可以简单的分为三个模块：IMU运动积分、IMU预积分、IMU初始化。

求解IMU里程计的流程参考自ORB：SLAM3。

IMU运动积分

IMU的运动积分公式如下：

由公式可以看出：当前时刻的R、V、P都是在上一时刻的R、V、P的基础上推导过来的，且模型中含有向量重力g。

为了使用一段时间的相对量来代替某个时刻的绝对量，且保留重力的影响，于是定义了预积分。

在求完预积分之后，为了消除预积分中重力的影响，于是进行了IMU初始化求解重力信息。

所以，IMU运动积分可以分解为两个部分：IMU预积分和IMU初始化。

IMU预积分

IMU预积分相对于IMU运动积分的特点是:

1、消除了第i时刻对积分的影响。

2、保留重力的影响。

IMU预积分的公式如下：

ORB-SLAM3中对应的代码：

#Preintegrated::IntegrateNewMeasurement函数中

    // 考虑偏置后的加速度、角速度
    Eigen::Vector3f acc, accW;
    acc << acceleration(0) - b.bax, acceleration(1) - b.bay, acceleration(2) - b.baz;
    accW << angVel(0) - b.bwx, angVel(1) - b.bwy, angVel(2) - b.bwz;


    // Update delta position dP and velocity dV (rely on no-updated delta rotation)
    // 根据没有更新的dR来更新dP与dV  eq.(38)  //note:这里更新顺序为 dP dV dR 不能更换顺序
    dP = dP + dV * dt + 0.5f * dR * acc * dt * dt;
    dV = dV + dR * acc * dt;

    IntegratedRotation dRi(angVel, b, dt);
    // 强行归一化使其符合旋转矩阵的格式 当前时刻的R
    dR = NormalizeRotation(dR * dRi.deltaR);