IMU在slam系统中的应用(二)

不会算法的阿召

已于 2022-05-24 12:18:26 修改

阅读量1.1k

点赞数 5

文章标签：自动驾驶 c++ 计算机视觉

于 2022-03-29 17:55:51 首次发布

本文链接：https://blog.csdn.net/qq_41916893/article/details/123825649

版权

在IMU在slam系统中的应用(一)中给出了IMU模型的连续形式，这一部分将给出IMU运动模型和预计分模型推导以及具体代码实现。

（1）离散形式的IMU运动模型

欧拉法：k+1时刻的状态由k时刻的测量值计算

假设k时刻的加速度测量值为 $\hat{a}_{b_k}$ ，角速度测量值为 $\hat{\omega}_{b_k}$ ，那么（不考虑噪声）实际真值为：

$a_{b_k}=q^w_{b_k}(\hat{a}_{b_k}-b^a_k)-g^a_k$

$w_{b_k}=\hat{\omega}_{b_k}-b^g_k$

那么考虑k+1时刻的状态为：

$P^w_{b_{k+1}}=P^w_{b_k}+V^w_{b_k}\Delta t + \dfrac{1}{2} {a}_{b_k}\Delta t^2$

$V^w_{b_{k+1}}=V^w_{b_k}+ {a}_{b_k}\Delta t$

$q^w_{b_{k+1}}=q^w_{b_{k}}\bigotimes\left[\begin{array}{c} 0 \\ \frac{1}{2}{ \mathbf{\omega}}_{b_k}\Delta t \end{array}\right]$

中值积分法：k+1时刻的位姿用k时刻和k+1时刻测量值的平均值计算

假设k时刻的加速度测量值为 $\hat{a}_{b_k}$ ，角速度测量值为 $\hat{\omega}_{b_k}$ ，k+1时刻加速度测量值为 $\hat{a}_{b_{k+1}}$ ，角速度测量值为 $\hat{\omega}_{b_{k+1}}$ ，偏置不变，那么（不考虑噪声）实际平均值为：

$\bar{a}_{b_k}=\dfrac{1}{2}[q^w_{b_k}(\hat{a}_{b_k}-b^a_k)-g^a_k + q^w_{b_{k+1}}(\hat{a}_{b_{k+1}}-b^a_{k+1})-g^a_{k+1}]$

$\bar{w}_{b_k}=\dfrac{1}{2}(\hat{\omega}_{b_k}-b^g_k+\hat{\omega}_{b_{k+1}}-b^g_{k+1})=\dfrac{1}{2}(\hat{\omega}_{b_k}+\hat{\omega}_{b_{k+1}})-b^g_k$

那么考虑k+1时刻的状态为

$P^w_{b_{k+1}}=P^w_{b_k}+V^w_{b_k}\Delta t + \dfrac{1}{2} {\bar{a}}_{b_k}\Delta t^2$

$V^w_{b_{k+1}}=V^w_{b_k}+ {\bar{a}}_{b_k}\Delta t$

$q^w_{b_{k+1}}=q^w_{b_{k}}\bigotimes\left[\begin{array}{c} 0 \\ \frac{1}{2}{ {\bar{\omega}}}_{b_k}\Delta t \end{array}\right]$

（二）离散形式下的IMU预积分模型

每输入一帧图片或者点云，要得到其位姿，如果从初始时刻开始用离散积分，是十分麻烦的。跟连续形式的IMU预积分一样，我们只需要去积分相邻时刻输入的IMU数据即可。

假设在相邻两帧图像在 $t_k$ 和 $t_{k+1}$ 测得，在 $[t_k,t_{k+1}]$ 中离散测量一系列加速度和角加速度，给出其中第 $i$ 时刻和第 $i+1$ 时刻内离散形式的预积分量：

欧拉法：与离散形式类似，为简单起见，这里没有转换到世界坐标系。

$\hat{ {\alpha}}_{i+1}^{b_{k}}=\hat{ {\alpha}}_{i}^{b_{k}}+\hat{ {\beta}}_{i}^{b_{k}} \delta t+\frac{1}{2} { }\left(\hat{\gamma}_{i}^{b_{k}}\right)\left(\hat{ {a}}_{i}- {b}_{a_{i}}\right) \delta t^{2}$

$\hat{ {\beta}}_{i+1}^{b_{k}}=\hat{ {\beta}}_{i}^{b_{k}}+ { }\left(\hat{\gamma}_{i}^{b_{k}}\right)\left(\hat{ {a}}_{i}- {b}_{a_{i}}\right) \delta t$

$\hat{\gamma}_{i+1}^{b_{k}}=\hat{\gamma}_{i}^{b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2}\left(\hat{ {\omega}}_{i}- {b}_{w_{i}}\right) \delta t \end{array}\right]$

中值积分法：同样依然在世界坐标系下

$\hat{ {\alpha}}_{i+1}^{b_{k}}=\hat{ {\alpha}}_{i}^{b_{k}}+\hat{ {\beta}}_{i}^{b_{k}} \delta t+\frac{1}{2} \bar{\hat{{a}}} \delta t^{2}$

$\hat{ {\beta}}_{i+1}^{b_{k}}=\hat{ {\beta}}_{i}^{b_{k}}+ \bar{\hat{{a}}} \delta t$

$\hat{\gamma}_{i+1}^{b_{k}}=\hat{\gamma}_{i}^{b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2}\bar{\hat{w}} \delta t \end{array}\right]$

其中：

$\bar{\hat{a}}_{b_k}=\dfrac{1}{2}[\hat{\gamma}^{b_k}_{i}(\hat{a}_{i}-b_i) + \hat{\gamma}^{b_k}_{i+1}(\hat{a}_{{i+1}}-b_{i+1})]$

$\bar{\hat{w}}_{b_k}=\dfrac{1}{2}(\hat{\omega}_{i}+\hat{\omega}_{{i+1}})-b_{w_i}$

（三）VINS-Mono预积分代码注释

预积分部分主要位于vins_estimator\src\vins_estimator.cpp中，接受到的一系列IMU数据是在两帧之间。

既然是预积分，那么函数体的参数当然是两次测量的时间间隔dt，以及加速度linear_acceleration和角速度angular_velocity。

void Estimator::processIMU(double dt, 
                           const Vector3d &linear_acceleration, 
                           const Vector3d &angular_velocity){}

如果是系统的第一个imu数据，是无法进行中值积分的，所以将imu数据保存为初始acc_0和gyr_0，方便后续中值积分使用。

if (!first_imu)
{
    first_imu = true;
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

如果当前图像帧预积分对象为空，则为其创建一个预积分对象（预计分初始化）。

if (!pre_integrations[frame_count])
    {
        pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    }

如果当前帧frame_count=0，那么是第一帧图像，是不用预积分的，因为预积分是两帧图像之间的约束，当frame_count!=0，用imu数据更新这帧图像的预积分。

pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);

3.1预积分初始化

主要看预积分对象的构造函数。将当前帧第一时刻的imu加速度和角加速度保存为acc_0，gyr_0，linearized_acc，linearized_gyr；将偏置保存为linearized_ba和linearized_bg。

初始化预积分误差传递方程jacobian为单位阵，协方差矩阵为零矩阵。

初始化imu当前时刻状态PVQ为“零状态”，初始化时间sum_dt。

初始化误差传递方程矩阵nt。

//构造函数
//传入当前帧第一时刻的imu加速度和角加速度,以及偏置
IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                    const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)
        : acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},
          linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg},
            jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, covariance{Eigen::Matrix<double, 15, 15>::Zero()},
          sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()}

{
    //构造原论文中式9矩阵nt，用来做误差状态传递
    //原文中是[na,nw,nba,nbw]，na和nw应该包括上一时刻和现在的imu噪声数据
    noise = Eigen::Matrix<double, 18, 18>::Zero();
    //上一时刻imu噪声
    noise.block<3, 3>(0, 0) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(3, 3) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
    //当前时刻imu噪声
    noise.block<3, 3>(6, 6) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(9, 9) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
    //随机游走噪声，即偏置的噪声
    noise.block<3, 3>(12, 12) =  (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(15, 15) =  (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();
}

3.2预积分计算

processIMU通过调用push_back()，传入当前时刻imu测量值。

void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
    {
        dt_buf.push_back(dt);
        acc_buf.push_back(acc);
        gyr_buf.push_back(gyr);
        //计算预积分
        propagate(dt, acc, gyr);
    }

调用预积分递推函数propagate()，使用中值积分计算预积分。

利用输入的imu数据中值积分得到当前预积分PVQ，result_delta_p，result_delta_v，result_delta_q，以及将偏置传递下去（保持不变），加速度偏置result_linearized_ba和角速度偏置result_linearized_bg。

将预积分的结果保存为delta_p，delta_v，delta_q，result_linearized_ba，result_linearized_bg。

并且更新acc_0和gyr_0，为下一次预积分计算作准备。

void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{
    //两次测量时间间隔
    dt = _dt;
    //当前时刻加速度
    acc_1 = _acc_1;
    //当前时刻角速度
    gyr_1 = _gyr_1;
    //预积分状态结果
    ector3d result_delta_p;
    Quaterniond result_delta_q;
    Vector3d result_delta_v;
    Vector3d result_linearized_ba;
    Vector3d result_linearized_bg;

    //中值积分
    midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
                        linearized_ba, linearized_bg,
                        result_delta_p, result_delta_q, result_delta_v,
                        result_linearized_ba, result_linearized_bg, 1);

    //更新状态，将递推得到的当前积分量，作为上一时刻结果，等待后续imu数据进入
    //注意这里的acc_0和gyr_0与processIMU()中不同
    delta_p = result_delta_p;
    delta_q = result_delta_q;
    delta_v = result_delta_v;
    linearized_ba = result_linearized_ba;
    linearized_bg = result_linearized_bg;
    delta_q.normalize();
    sum_dt += dt;
    acc_0 = acc_1;
    gyr_0 = gyr_1;  
}

这里调用midPointInteration()预积分，是代码的核心部分，也是IMU中值离散积分模型。

void midPointIntegration(double _dt, 
                         const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                         const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
                         const Eigen::Vector3d &delta_p, const Eigen::Quaterniond                 
                         &delta_q, const Eigen::Vector3d &delta_v,
                         const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
                         Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
                         Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
{
    //ROS_INFO("midpoint integration");
    Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
    Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
    result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
    Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
    Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
    result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
    result_delta_v = delta_v + un_acc * _dt;
    result_linearized_ba = linearized_ba;
    result_linearized_bg = linearized_bg;         

    if(update_jacobian)
    {
       //误差传递方程部分     
    }

}

不会算法的阿召

关注

5
点赞
踩
19

收藏

觉得还不错? 一键收藏
1
评论
IMU在slam系统中的应用(二)

在IMU在slam系统中的应用(一)中给出了IMU模型的连续形式，这一部分将给出IMU运动模型和预计分模型推导以及具体代买实现。（1）离散形式的IMU运动模型欧拉法：k+1时刻的状态由k时刻的测量值计算假设k时刻的加速度测量值为，角速度测量值为，那么（不考虑噪声）实际真值为：那么考虑k+1时刻的状态为：中值积分法：k+1时刻的位姿用k时刻和k+1时刻测量值的平均值计算假设k时刻的加速度测量值为，角速...
复制链接

扫一扫