从零开始学习VIO笔记 --- 第二讲：IMU 数据仿真(分析+代码)

一.理论分析

机器人身上携带了相机与imu传感器
世界坐标系/惯性系为$w$ ; imu的坐标系作为body系 $b$; 相机坐标系为 $c$ ; $b$系与$c$系的外参手动设定 $R_{bc}$ 和 $t_{bc}$

$t_{wb}$ 描述 机器人的位置与世界坐标系原点的关系； $R_{wb}$描述机器人坐标系与世界坐标系的旋转关系
需要：

1.1 机器人的位姿

机器人的位姿 $t_{wb}$与 $R_{wb}$

• 定义机器人运动轨迹 $p=f(t)$ ，p点的坐标值是相对于世界坐标系的，即为 $t_{wb}$
• 定义机器人运动的欧拉角方程 $（roll，pitch，yaw）$ 分别对应$x,y,z$轴的旋转量，也是相对于世界坐标系的； （欧拉角方程描述了机器人坐标系与世界坐标系的旋转关系，更为直观，也更容易建立方程，然后我们需要将这个旋转关系转为旋转矩阵去描述）
  世界坐标系下的一个点 $X$ 通过三次旋转（分别绕 $z,y,x$）可以得到在 body坐标系下的坐标值：即
  $X_b=R_{bw}X$,即可以得到 $R_{wb}=R_{bw}^{-1}=R_{bw}^T$
  

1.2 imu的角速度 和线加速度

imu的角速度 $w^b$ 和线加速度 $a^b$ （imu的坐标系作为body系）

• 对欧拉角方程 $\vartheta$ 求一阶导 $\frac{d\vartheta }{dt}$，得到欧拉角速度，然后通过变换（见下图）获得body系下的角速度 $w^b$
  
• 通过对运动轨迹 $p=f(t)$ 对时间 $t$ 求二阶导获得在世界坐标系下机器人的加速度 $a^w$， 由于还有ENU/G(东北天/导航)坐标系下的重力加速度$g^G=[0,0,-9.81{]}^T$的影响，b系下的线加速度 $a^b$ 通过下式计算：
  $$a^b=R_{bw}(a^w-g^G)$$

1.3 对imu数据添加噪声

偏差噪声bias ，随机噪声（高斯白噪声）n （见上一博文中的 imu加速度计和陀螺仪的数学模型）

1.4 问题

假设我们将机器的运动轨迹描述为：在z=1的平面做半径为 r 的圆周运动，方程则为 $(cos(t),sin(t),1)$,那么机器人坐标系的方向变换呢？即机器人坐标系与世界坐标系的旋转关系 $R_{wb}$ 如何定义呢？
也就是如何确定 随时间变换的欧拉角方程 $（roll，pitch，yaw）$呢？
答：已知机器人在固定的平面做圆周运动，我们的body坐标系，那么 roll和 pitch 为0，yaw随时间变化，。。。 （以后再补充吧）

二.代码

2.1 模型建立与产生数据

1.机器人的运动轨迹 — 可以构造函数 $f(t)$, 函数值为 Postion=（x，y，z），函数自变量为时间 t
$t_{wb}$ 就是Postion

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正好旋转一圈，运动一周
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二阶导数

对运动轨迹进行对时间t的一阶求导 或得 机身速度 $v^w$（是带方向的，为矢量）
对运动轨迹进行对时间t的二阶求导 或得 机身加速度 $a^w$（是带方向的，为矢量）

2.定义欧拉角运动方程，对其求导获得欧拉角速度，然后再转到 body系下的角速度 $w^b$

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 的导数
// 欧拉角速度转到body坐标系下的角速度
Eigen::Vector3d imu_gyro = eulerRates2bodyRates(eulerAngles) * eulerAnglesRates;   //  euler rates trans to body gyro

3.$R_{wb}$ 通过欧拉角方程确定

Eigen::Matrix3d Rwb = euler2Rotation(eulerAngles);         // body frame to world frame

4.需要imu的加速度数据 Vector3d imu_acc — 由于还有世界坐标系下的重力加速度的影响，需要将机身加速度 $a^w$ 转为 imu的加速度:
$$a^b=R_{bw}(a^w-g^w)$$

Eigen::Vector3d gn (0,0,-9.81);                                   //  gravity in navigation frame(ENU)   ENU (0,0,-9.81)  NED(0,0,9,81)
Eigen::Vector3d imu_acc = Rwb.transpose() * ( ddp -  gn );  //  Rbw * Rwn * gn = gs

2.2 保存相机位姿

相机位姿(相机在世界坐标系下位姿) $R_{wc}=R_{wb}{R}_{bc}$ , $t_{wc}=t_{wb}+R_{wb}{t}_{bc}$

cam.timestamp = imu.timestamp;
cam.Rwb = imu.Rwb * params.R_bc;    // cam frame in world frame
cam.twb = imu.twb + imu.Rwb * params.t_bc; //  Tcw = Twb * Tbc ,  t = Rwb * tbc + twb
camdata.push_back(cam);

文件行格式：time，四元数，位移；

void save_Pose(std::string filename, std::vector<MotionData> pose)
{
    std::ofstream save_points;
    save_points.open(filename.c_str());

    for (int i = 0; i < pose.size(); ++i) {
        MotionData data = pose[i];
        double time = data.timestamp;
        Eigen::Quaterniond q(data.Rwb);
        Eigen::Vector3d t = data.twb;
        // Eigen::Vector3d gyro = data.imu_gyro;
        // Eigen::Vector3d acc = data.imu_acc;
        

        save_points<<time<<" "
                   <<q.w()<<" "
                   <<q.x()<<" "
                   <<q.y()<<" "
                   <<q.z()<<" "
                   <<t(0)<<" "
                   <<t(1)<<" "
                   <<t(2)<<" "
                //    <
                //    <
                //    <
                //    <
                //    <
                //    <
                   <<std::endl;
    }
}

2.3 imu数据添加噪声

    // noise
    double gyro_bias_sigma = 1.0e-5;
    double acc_bias_sigma = 0.0001;

    double gyro_noise_sigma = 0.015;    // rad/s
    double acc_noise_sigma = 0.019;      //　m/(s^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);

    Eigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_));
    Eigen::Matrix3d gyro_sqrt_cov = param_.gyro_noise_sigma * Eigen::Matrix3d::Identity();
    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_));
    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_;

}

2.4 使用imu的动力学模型（离散积分得到位姿）验证数据以及模型的准确性

运动模型的离散积分——中值法 （见上一博文），中值法相比欧拉法更为准确

imuGen.init_velocity_ = imudata[0].imu_velocity;
imuGen.init_twb_ = imudata.at(0).twb;
imuGen.init_Rwb_ = imudata.at(0).Rwb;

//读取生成的imu数据并用imu动力学模型对数据进行计算，最后保存imu积分以后的轨迹，
//用来验证数据以及模型的有效性。
void IMU::testImu(std::string src, std::string dist)
{
    std::vector<MotionData>imudata;
    LoadPose(src,imudata);

    std::ofstream save_points;
    save_points.open(dist);

    double dt = param_.imu_timestep;
    Eigen::Vector3d Pwb = init_twb_;              // position :    from  imu measurements
    Eigen::Quaterniond Qwb(init_Rwb_);            // quaterniond:  from imu measurements
    Eigen::Vector3d Vw = init_velocity_;          // velocity  :   from imu measurements
    Eigen::Vector3d gw(0,0,-9.81);    // ENU frame
    Eigen::Vector3d temp_a;
    Eigen::Vector3d theta;
    for (int i = 1; i < imudata.size(); ++i) {

        MotionData imupose = imudata[i];

        //四元数更新量 delta_q = [1 , 1/2 * theta_x , 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
        Qwb = Qwb * dq;
        Vw = Vw + acc_w * dt;
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;  // 

        /// 中值积分

        //　按着imu postion, imu quaternion , cam postion, cam quaternion 的格式存储，由于没有cam，所以imu存了两次
        save_points<<imupose.timestamp<<" "
                   <<Qwb.w()<<" "
                   <<Qwb.x()<<" "
                   <<Qwb.y()<<" "
                   <<Qwb.z()<<" "
                   <<Pwb(0)<<" "
                   <<Pwb(1)<<" "
                   <<Pwb(2)<<" "
                   <<Qwb.w()<<" "
                   <<Qwb.x()<<" "
                   <<Qwb.y()<<" "
                   <<Qwb.z()<<" "
                   <<Pwb(0)<<" "
                   <<Pwb(1)<<" "
                   <<Pwb(2)<<" "
                   <<std::endl;
    }
    std::cout<<"test　end"<<std::endl;
}