手写VIO代码解析(1) -IMU

欧拉角

相比于四元数、旋转矩阵，采用欧拉角表达姿态更加的直观，一个世界坐标若能依次绕着其3个轴旋转然后与Body坐标系3个轴方向上重合，则就可以用3个旋转角来描述body坐标系的旋转。这个角就是欧拉角，可以看到欧拉角与绕轴旋转的顺序有关， 一般常用的欧拉角是ZYX，表示先绕Z轴旋转(yaw),再绕旋转之后的Y轴旋转(pitch)。再绕旋转之后的X轴旋转(roll)，但是欧拉角表示姿态会出现奇异性问题或者死锁，所以一般不用欧拉角进行迭代或者插值，但考虑到欧拉角直观的优点，常常会用欧拉角表示姿态，运算的时候则将它转换为旋转矩阵或四元数。奇异性问题见下面参考资料。

欧拉角与万向锁
万向锁视频

欧拉角与旋转矩阵的转换

若我们知道body坐标的欧拉角，即惯性坐标(world)按照该欧拉角旋转可以与body坐标系重合，我们可以获得该旋转的矩阵R，R×世界坐标系下坐标 = Body系姿态下的坐标。

// 欧拉角转旋转矩阵
Eigen::Matrix3d euler2Rotation( Eigen::Vector3d  eulerAngles)
{
   // Z-Y-X
    double roll = eulerAngles(0);
    double pitch = eulerAngles(1);
    double yaw = eulerAngles(2);

    double cr = cos(roll); double sr = sin(roll);
    double cp = cos(pitch); double sp = sin(pitch);
    double cy = cos(yaw); double sy = sin(yaw);
    // 获取  body -> inertial
    Eigen::Matrix3d RIb;  
    RIb<< cy*cp ,   cy*sp*sr - sy*cr,   sy*sr + cy* cr*sp,
          sy*cp,    cy *cr + sy*sr*sp,  sp*sy*cr - cy*sr,
          -sp,         cp*sr,           cp*cr;
    return RIb;
}

推导：

欧拉角速度与body角速度的转换

若知道物体姿态的欧拉角，可以获得其欧拉角角速度，但要注意的一点是，欧拉角的角速度并不是物体自身的body系下的旋转速度，它们之间需要一个矩阵来转换。

// 欧拉角速度转Body角速度
// 用欧拉角表示姿态时的角速度不是IMU测量的角速度，或者说欧拉角速度不是body自身旋转的角速度
Eigen::Matrix3d eulerRates2bodyRates(Eigen::Vector3d eulerAngles)
{
    double roll = eulerAngles(0);
    double pitch = eulerAngles(1);

    double cr = cos(roll); double sr = sin(roll);
    double cp = cos(pitch); double sp = sin(pitch);
   // 欧拉角速度 -> body角速度
    Eigen::Matrix3d R;
    R<<  1,   0,    -sp,
         0,   cr,   sr*cp,
         0,   -sr,  cr*cp;
    return R;
}

IMU模拟数据的产生

下面代码在gener_alldata.cpp中，生成任意t 的IMU观测数据再添加噪声

//  生成IMU的数据      时间t时的数据保存在imudata
for (float t = params.t_start; t<params.t_end;) {
    // 生成t时刻的IMU数据
    MotionData data = imuGen.MotionModel(t);
    imudata.push_back(data);

    // add imu noise
    MotionData data_noise = data;
    imuGen.addIMUnoise(data_noise);
    imudata_noise.push_back(data_noise);

    t += 1.0/params.imu_frequency;
}

这个函数生成t时刻的IMU数据，这里注意二点1、IMU测量的加速度是world系下加速度减去重力好后再投影到body坐标的加速度。 2、欧拉角速度并不是body系的角速度(陀螺仪测量的角速度)。

// IMU数据产生
MotionData IMU::MotionModel(double t)
{
    MotionData data;
    // param
    float ellipse_x = 15;
    float ellipse_y = 20;
    float z = 1;                // z轴做sin运动
    float K1 = 10;          // z轴的正弦频率是x，y的k1倍
    float K = M_PI/ 10;    // 20 * K = 2pi 　　由于我们采取的是时间是20s, 系数K控制yaw正好旋转一圈，运动一周

    // translation
    // twb:  body frame in world frame
    Eigen::Vector3d position( ellipse_x * cos( K * t) + 5, ellipse_y * sin( K * t) + 5,  z * sin( K1 * K * t ) + 5);
    // 速度信息
    Eigen::Vector3d dp(- K * ellipse_x * sin(K*t),  K * ellipse_y * cos(K*t), z*K1*K * cos(K1 * K * t));                           // position导数　in world frame
    double K2 = K*K;
    // 加速度    这个加速度只是物体在世界坐标系下运动的速度
    Eigen::Vector3d ddp( -K2 * ellipse_x * cos(K*t),  -K2 * ellipse_y * sin(K*t), -z*K1*K1*K2 * sin(K1 * K * t));        // position二阶导数

    // Rotation
    double k_roll = 0.1;
    double k_pitch = 0.2;
    // 设定欧拉角的变化       四元数与旋转矩阵设定运动不直观   
    Eigen::Vector3d eulerAngles(k_roll * cos(t) , k_pitch * sin(t) , K*t );   // roll ~ [-0.2, 0.2], pitch ~ [-0.3, 0.3], yaw ~ [0,2pi]
    Eigen::Vector3d eulerAnglesRates(-k_roll * sin(t) , k_pitch * cos(t) , K);                                        // euler angles 的导数

//    Eigen::Vector3d eulerAngles(0.0,0.0, K*t );   // roll ~ 0, pitch ~ 0, yaw ~ [0,2pi]
//    Eigen::Vector3d eulerAnglesRates(0.,0. , K);      // euler angles 的导数
   // 欧拉角装旋转矩阵
    Eigen::Matrix3d Rwb = euler2Rotation(eulerAngles);         // body frame to world frame
    // IMU角速度    欧拉角速度转换到body系下   
    Eigen::Vector3d imu_gyro = eulerRates2bodyRates(eulerAngles) * eulerAnglesRates;            //  euler rates trans to body gyro
   // 求取加速度 IMU的测量值  
    Eigen::Vector3d gn (0,0,-9.81);                                                                                                                             //  gravity in navigation frame(ENU)   ENU (0,0,-9.81)  NED(0,0,9,81)
    // 将加速度投影到IMU Body坐标系下
    Eigen::Vector3d imu_acc = Rwb.transpose() * ( ddp -  gn );  //  Rbw * Rwn * gn = gs

    data.imu_gyro = imu_gyro;                           // 陀螺仪测量值
    data.imu_acc = imu_acc;                               // 加速度计测量值
    data.Rwb = Rwb;                                                // IMU的姿态 
    data.twb = position;                                         // IMU的位置
    data.imu_velocity = dp;                                  // IMU的速度 
    data.timestamp = t;                                          // IMU时刻
    return data;                                                          

}

添加噪声
这里要注意： 1、真实的IMU数据需要加上噪声和偏置随机游走的影响，代码里偏置初始化为0，后面的偏置是由随机游走引起的。 2、连续时间下的噪声通过离散时间采样后会改变其方差。

void IMU::addIMUnoise(MotionData& data)
{
    std::random_device rd;
    std::default_random_engine generator_(rd());
    std::normal_distribution<double> noise(0.0, 1.0);        // 期望为0     方差为1   
   // 陀螺仪数据加上噪声、偏置
    Eigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_));
    // 陀螺仪的噪声协方差矩阵
    Eigen::Matrix3d gyro_sqrt_cov = param_.gyro_noise_sigma * Eigen::Matrix3d::Identity();
    // 在采样时间间隔为t时，   噪声变为   gyro_sqrt_cov * noise_gyro / sqrt( param_.imu_timestep ) 
    data.imu_gyro = data.imu_gyro + gyro_sqrt_cov * noise_gyro / sqrt( param_.imu_timestep ) + gyro_bias_;

    Eigen::Vector3d noise_acc(noise(generator_),noise(generator_),noise(generator_));
    Eigen::Matrix3d acc_sqrt_cov = param_.acc_noise_sigma * Eigen::Matrix3d::Identity();
    data.imu_acc = data.imu_acc + acc_sqrt_cov * noise_acc / sqrt( param_.imu_timestep ) + acc_bias_;

    // gyro_bias update            偏置随机游走
    Eigen::Vector3d noise_gyro_bias(noise(generator_),noise(generator_),noise(generator_));
    // 随机游走噪声从连续到离散需要× sqrt(param_.imu_timestep )
    gyro_bias_ += param_.gyro_bias_sigma * sqrt(param_.imu_timestep ) * noise_gyro_bias;
    data.imu_gyro_bias = gyro_bias_;

    // acc_bias update        偏置随机游走
    Eigen::Vector3d noise_acc_bias(noise(generator_),noise(generator_),noise(generator_));
    acc_bias_ += param_.acc_bias_sigma * sqrt(param_.imu_timestep ) * noise_acc_bias;
    data.imu_acc_bias = acc_bias_;

}

IMU运动学模型

欧拉积分

 // 陀螺仪角速度对旋转的影响
 //delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
 Eigen::Quaterniond dq;
 Eigen::Vector3d dtheta_half =  imupose.imu_gyro * dt /2.0;
 dq.w() = 1;
 dq.x() = dtheta_half.x();
 dq.y() = dtheta_half.y();
 dq.z() = dtheta_half.z();

 /// imu 动力学模型 欧拉积分
 Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw;                                           // aw = Rwb * ( acc_body - acc_bias ) + gw        偏置默认为0   随机游走的影响没法计算
 Qwb = Qwb * dq;                                // 更新姿态                                                  
 Vw = Vw + acc_w * dt;                      
 Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;

效果如下：

中值积分

主要部分代码如下

Eigen::Vector3d mean_gyro = ( imupose.imu_gyro  + Old_gyro ) / 2.0;
Eigen::Quaterniond dq;
Eigen::Vector3d dtheta_half = mean_gyro * dt / 2.0 ;  
dq.w() = 1;
dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();

Qwb = Qwb * dq;               // 更新当前时候姿态
Eigen::Vector3d acc_w = (Old_Qwb * Old_imu_acc + gw + Qwb * imupose.imu_acc + gw) / 2 ;  
Pwb = Pwb + Vw * dt + acc_w * dt*dt /2; 
Vw = Vw + acc_w * dt ; 

Old_imu_acc = imupose.imu_acc;
Old_gyro = imupose.imu_gyro;  
Old_Qwb = Qwb;

可以看到中值积分的效果是要好很多的。