当前位置:   article > 正文

从零开始手写vio-ch2-imu数据仿真_imu定位轨迹曲线绘制

imu定位轨迹曲线绘制

一、利用欧拉积分和中指=值积分来计算imu数据,并绘制轨迹,与给定的轨迹方程进行比较

利用欧拉积分或者中值积分计算imu数据

核心代码imu.cpp源码如下:

//
// Created by hyj on 18-1-19.
//

#include <random>
#include "imu.h"
#include "utilities.h"

// euler2Rotation:   body frame to interitail frame
Eigen::Matrix3d euler2Rotation( Eigen::Vector3d  eulerAngles)
{
    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);

    //RIb=RbI^−1=RbI^T
    Eigen::Matrix3d RIb;//这里对应的是公式 是从i系到b系的旋转矩阵的转置
    //i系到b系的旋转矩阵的公式见ppt 就是Rbi=R(yaw,pith,roll)
    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;
}
//变换矩阵--将欧拉角速度从I坐标变到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);
//公式见ppt
    Eigen::Matrix3d R;
    R<<  1,   0,    -sp,
            0,   cr,   sr*cp,
            0,   -sr,  cr*cp;

    return R;
}


IMU::IMU(Param p): param_(p)
{
    gyro_bias_ = Eigen::Vector3d::Zero();
    acc_bias_ = Eigen::Vector3d::Zero();
}

//加入噪声
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_;

}

得到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
    // position(t)=[ellipsex∗cos(K∗t)+5,ellipsey∗sin(K∗t)+5,z∗sin(K1∗K∗t)+5]
    //设定质量快运动轨迹方程(以例程中椭圆为例)
    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;
    /**
     * 在东北天坐标系中,yaw,pitch,roll对应的是绕载体坐标系的z,y,x轴。在北东地坐标系中,yaw,pitch,roll对应的是绕载体坐标系的y, x, z轴。在IMU仿真程序中,我们采用东北天坐标系,欧拉角顺规为yaw,pitch,roll。具体的值为

ϑ \vartheta ϑ = ( k r o l l ∗ c o s ( t ) , k p i t c h ∗ s i n ( t ) , K ∗ t ) T (kroll * cos(t) , kpitch * sin(t) , K*t)^T (kroll∗cos(t),kpitch∗sin(t),K∗t)^T
     **/
    //质量块的欧拉角 是body系在不同时刻相对于惯性系所转角度
    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);  //得到从b坐标系到I坐标系的旋转矩阵(这里I坐标系就是W坐标系??)   这里可以参考从零手写vio-ch2笔记    // body frame to world frame
    //eulerRates2bodyRates(eulerAngles)//b坐标系到I坐标系的角速度的变换矩阵 这里可以参考从零手写vio-ch2笔记
    Eigen::Vector3d imu_gyro = eulerRates2bodyRates(eulerAngles) * eulerAnglesRates;   //  euler rates trans to body gyro

    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 );  //   imu_acc是测量值  ddp是imu得到的值   Rbw * Rwn * gn = gs

    data.imu_gyro = imu_gyro;//角速度
    data.imu_acc = imu_acc;//加速度
    data.Rwb = Rwb;//旋转矩阵 从body 坐标系到world(I系?)
    data.twb = position;//位置,也可理解为平移
    data.imu_velocity = dp;//速度 速度就是轨迹方程的导数
    data.timestamp = t;//时间戳
    return data;

}

读取生成的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;
    //读取imu数据
    for (int i = 1; i < imudata.size(); ++i) {

        MotionData imupose = imudata[i];
        /// imu 动力学模型 欧拉积分 加入bias
       /* //delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
        Eigen::Quaterniond dq;
        Eigen::Vector3d dtheta_half =  (imupose.imu_gyro-imupose.imu_gyro_bias) * dt /2.0;
        //这里的dq就是离散时间段的旋转小量
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        dq.normalize();
        
        //  这一部分对应ppt欧拉法公式
        Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc-imupose.imu_acc_bias) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
        Qwb = Qwb * dq;//imu的速度加上旋转
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
        Vw = Vw + acc_w * dt;*/

        /// 中值积分 加入噪声
        Eigen::Quaterniond Qwbk,Qwbk1,dq;//dq是旋转小量
        Eigen::Vector3d midOmega;
        Eigen::Vector3d dtheta_half;
        midOmega=(imudata[i].imu_gyro-imudata[i-1].imu_gyro_bias+imudata[i-1].imu_gyro-imudata[i-1].imu_gyro_bias)/2;
        dtheta_half=midOmega*dt/2.0;//这就是公式的旋转小量的 1/2* w *dt
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        dq.normalize();//将小量四元数单位化
        Qwbk=Qwb;
        Qwbk1=Qwbk*dq;//得到下一时刻的四元数-旋转
        Eigen::Vector3d acc_w =0.5*( Qwbk * (imudata[i-1].imu_acc-imudata[i-1].imu_acc_bias) + gw +Qwbk1 * (imupose.imu_acc-imudata[i-1].imu_acc_bias) + gw );
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
        Vw = Vw + acc_w * dt;
        Qwb = Qwbk1;




        // 按着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;

}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201
  • 202
  • 203
  • 204
  • 205
  • 206
  • 207
  • 208
  • 209
  • 210
  • 211
  • 212
  • 213
  • 214
  • 215
  • 216
  • 217

没有加噪声

欧拉法:在这里插入图片描述
中值法:
在这里插入图片描述

加噪声

欧拉法:
在这里插入图片描述中值法:
在这里插入图片描述

实验总结

在没有加入噪声之前,可以看出欧拉法的误差比较大,中值法基本与轨迹重合。
加入噪声后,有噪声的数据随着时间推移,误差越来越大,直到完全脱离轨迹。

二、Allan方差标定

参考博客:Allan

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/笔触狂放9/article/detail/891623
推荐阅读
相关标签
  

闽ICP备14008679号