从零开始学习VIO 笔记---第⼆讲：IMU 数据仿真（分析+代码）
从零开始学习VIO 笔记 --- 第⼆讲：IMU 数据仿真(分析+代码）
⼀.理论分析
世界坐标系/惯性系为 ; imu的坐标系作为body系 ; 相机坐标系为 ; 系与系的外参⼿动设定 和 描述 机器⼈的位置与世界坐标系原点的关系； 描述机器⼈坐标系与世界坐标系的旋转关系 需要：
1.1 机器⼈的位姿机器⼈的位姿 与 定义机器⼈运动轨迹 ，p点的坐标值是相对于世界坐标系的，即为 定义机器⼈运动的欧拉⾓⽅程 分别对应轴的旋转量，也是相对于世界坐标系的； （欧拉⾓⽅程描述了机器⼈坐标系与世界坐标系的旋转关系，更为直观，也更容易建⽴⽅程，然后我们需要将这个旋转关系转为旋转矩阵去描述）世界坐标系下的⼀个点 通过三次旋转（分别绕 ）可以得到在 body坐标系下的坐标值：即 ，即可以得到 1.2 imu 的⾓速度 和线加速度imu的⾓速度 和线加速度 （imu的坐标系作为body系）
w b c b c R bc t bc
t wb R wb t wb R wb
p =f (t )t wb
（roll ，pitch ，yaw ）x ,y ,z X z ,y ,x X =b R X bw R =wb R =bw −1R bw T
w b a b
对欧拉⾓⽅程 求⼀阶导
，得到欧拉⾓速度，然后通过变换（见下图）获得body系下的⾓速度 通过对运动轨迹 对时间 求⼆阶导获得在世界坐标系下机器⼈的加速度 ， 由于还有ENU/G(东北天/导航)坐标系下的重⼒加速度的影响，b系下的线加速度 通过下式计算：
1.3 对imu 数据添加噪声偏差噪声bias ，随机噪声（⾼斯⽩噪声）n （见上⼀博⽂中的 imu加速度计和陀螺仪的数学模型
）
1.4 问题
假设我们将机器的运动轨迹描述为：在z=1的平⾯做半径为 r 的圆周运动，⽅程则为 ，那么机器⼈坐标系的⽅向变换呢？即机器⼈坐标系与世界坐标系的旋转关系 如何定义呢？
也就是如何确定 随时间变换的欧拉⾓⽅程 呢？
答：已知机器⼈在固定的平⾯做圆周运动，我们的body坐标系，那么 roll和 pitch 为0，yaw随时间变化，。。。 （以后再补充吧）⼆.代码
2.1 模型建⽴与产⽣数据
1.机器⼈的运动轨迹 — 可以构造函数 , 函数值为 Postion=（x，y，z），函数⾃变量为时间 t
就是Postion
ϑd t d ϑ
w b p =f (t )t a w g =G [0,0,−9.81]T a b a =b R (a −bw w g )
G (cos (t ),sin (t ),1)R wb （roll ，pitch ，yaw ）f (t )t wb
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的⼀阶求导 或得 机⾝速度 （是带⽅向的，为⽮量）
对运动轨迹进⾏对时间t的⼆阶求导 或得 机⾝加速度 （是带⽅向的，为⽮量）2.定义欧拉⾓运动⽅程，对其求导获得欧拉⾓速度，然后再转到 body系下的⾓速度 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. 通过欧拉⾓⽅程确定
Eigen ::Matrix3d Rwb = euler2Rotation (eulerAngles ); // body frame to world frame 4.需要imu的加速度数据 Vector3d imu_acc — 由于还有世界坐标系下的重⼒加速度的影响，需要将机⾝加速度 转为 imu的加速度:
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 保存相机位姿
相机位姿(相机在世界坐标系下位姿) , 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，四元数，位移；
v w a w w b
R wb a w a =b R (a −bw w g )
w R =wc R R wb bc t =wc t +wb R t wb bc
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)<<" "
/
/ <<gyro(0)<<" "
// <<gyro(1)<<" "
// <<gyro(2)<<" "
// <<acc(0)<<" "
// <<acc(1)<<" "
// <<acc(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的动⼒学模型（离散积分得到位姿）验证数据以及模型的准确性运动模型的离散积分——中值法 （见上⼀博⽂），中值法相⽐欧拉法更为准确
