VINS详解之IMU预积分（一）

看VINS很久了，但一直处于似懂非懂的状态，因此想借着写博客的机会结合论文对代码进行详细的梳理。第一篇就从IMU预积分开始吧。

一 、首先是接收IMU的message

 1 void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
 2 {
 3     ***
 4     imu_buf.push(imu_msg);
 5 
 6     ***
 7     predict(imu_msg);
 8 
 9     ***
10     pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
11 
12 }

主要是做三个操作

1、将imu_msg push到队列当中

2、调用predict函数对当前imu的位置、朝向、速度进行计算

3、发送imu的里程计信息

1、3的操作很直观，下面主要了解一下predict函数

 

二、predict函数解析

1、首先是从messgae读入线性加速度和角速度

    Eigen::Vector3d linear_acceleration{dx, dy, dz};
    Eigen::Vector3d angular_velocity{rx, ry, rz};

这里的线性加速度、加速度是在当前的IMU坐标系下的，包含传感器的漂移和重力加速度

参见论文的公式（1）

其中带有 尖冒子 a和ω是原始的线性加速度和角速度 ， 而不带尖帽子的a和ω是移除了漂移和重力加速度的。他们都是相对于当前的IMU坐标系的。

 

2、位置计算

Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g; 

上面两行代码具有相同的形式，目的是获得在世界坐标系下的，移除了加速度计漂移和重力加速度的，线性加速度。

当前imu message的时间戳是t， 上一个imu message的接收时间是latest_time。

因此 a_{latest_time}  =  un_acc_0

                     a_t  =  un_acc_1

下面三行代码根据平均加速度计算位移和速度

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;

 

3、朝向计算

 //latest_time 与 t之间的平均角速度
 Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
 //根据旋转角度 得到当前朝向
 tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);

上面两行代码就是根据角速度计算旋转角度，继而得到当前IMU在世界坐标系的朝向

Utility::deltaQ函数的作用是将旋转角度转换成四元数

 1 static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta)
 2     {
 3         typedef typename Derived::Scalar Scalar_t;
 4 
 5         Eigen::Quaternion dq;
 6         Eigen::Matrix3, 1> half_theta = theta;
 7         half_theta /= static_cast(2.0);
 8         dq.w() = static_cast(1.0);
 9         dq.x() = half_theta.x();
10         dq.y() = half_theta.y();
11         dq.z() = half_theta.z();
12         return dq;
13     }

它的逻辑是这样的：

假设某个旋转是绕单位向量n=[n_x,n_y,n_z]^T进行了角度为θ的旋转，那么这个旋转的四元数为：

q=[cos(θ/2), n_xsin(θ/2),n_ysin(θ/2),n_zsin(θ/2)] ^T

根据等价无穷小sinx~x,则sin(θ/2)~θ/2, 以及当x->0s时,cosx->1

则当θ接近于0时，q=[1, n_x(θ/2)， n_y(θ/2), n_z(θ/2)] ^T

根据代码的对应关系，作者貌似是认为旋转轴为[1,1,1]^T（这里我也是猜测，希望有了解的朋友可以解释一下）

转载于:https://www.cnblogs.com/wzy-coding/p/10078505.html