vins中imu融合_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。

因此 alatest_time=  un_acc_0

at=  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::Quaterniondq;6 Eigen::Matrix 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 returndq;13 }

它的逻辑是这样的：

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

q=[cos(θ/2), nxsin(θ/2),nysin(θ/2),nzsin(θ/2)] T

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

则当θ接近于0时，q=[1, nx(θ/2)， ny(θ/2), nz(θ/2)] T

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

un_acc_0