首页
学习
活动
专区
工具
TVP
发布
社区首页 >专栏 >IMU标定(三)确定误差的标定

IMU标定(三)确定误差的标定

作者头像
猫叔Rex
发布2020-12-16 10:47:43
2.4K2
发布2020-12-16 10:47:43
举报
文章被收录于专栏:科学计算科学计算

一、确定性误差公式

 我们将第一节给出的IMU确定性误差直接拿过来,轴偏角公式如下:

T^g=\begin{bmatrix} 1 & -\gamma_{yz} & \gamma_{zy}\\ \gamma_{xz} & 1 & -\gamma_{zx}\\ -\gamma_{xy} & \gamma_{yx} & 1 \end{bmatrix}
T^g=\begin{bmatrix} 1 & -\gamma_{yz} & \gamma_{zy}\\ \gamma_{xz} & 1 & -\gamma_{zx}\\ -\gamma_{xy} & \gamma_{yx} & 1 \end{bmatrix}
T^a=\begin{bmatrix} 1 & -\alpha_{yz} & \alpha_{zy}\\ 0 & 0 & -\alpha_{zx}\\ 0 & 0 & 1 \end{bmatrix}

 尺度因子:

K^a=\begin{bmatrix} s_x^a & 0 & 0\\ 0 & s_y^a & 0\\ 0 & 0 & s_z^a \end{bmatrix}
K^a=\begin{bmatrix} s_x^a & 0 & 0\\ 0 & s_y^a & 0\\ 0 & 0 & s_z^a \end{bmatrix}
K^g=\begin{bmatrix} s_x^\omega & 0 & 0\\ 0 & s_y^\omega & 0\\ 0 & 0 & s_z^\omega \end{bmatrix}

 零偏:

b^a = \begin{bmatrix} b_x^a & b_y^a & b_z^a\end{bmatrix}
b^g=\begin{bmatrix} b_x^g & b_y^g & b_z^g\end{bmatrix}

二、工具标定

 这里我们使用开源的imu_tk进行标定,下载:https://github.com/Kyle-ak/imu_tk.git。

2.1 代码细节

 轴偏角、尺度因子、零偏变量如下:

  /** @brief Misalignment matrix */
  Eigen::Matrix< _T, 3 , 3> mis_mat_;
  /** @brief Scale matrix */
  Eigen::Matrix< _T, 3 , 3> scale_mat_;
  /** @brief Bias vector */
  Eigen::Matrix< _T, 3 , 1> bias_vec_;
  /** @brief Misalignment * scale matrix */
  Eigen::Matrix< _T, 3 , 3> ms_mat_;

 首先,进行加速度计的标定:设重力加速度g_mag_(9.81),默认前后两帧加速度计三轴加速度分量相等,我们建立如下的残差方程,直接进行迭代优化即可得到加速度计的参数;

  template <typename _T2>
    bool operator() ( const _T2* const params, _T2* residuals ) const
  {
    Eigen::Matrix< _T2, 3 , 1> raw_samp( _T2(sample_(0)), _T2(sample_(1)), T2(sample_(2)));
    /* Assume body frame same as accelerometer frame, 
     * so bottom left params in the misalignment matris are set to zero */
    CalibratedTriad_<_T2> calib_triad( params[0], params[1], params[2], 
                                     _T2(0), _T2(0), _T2(0),
                                     params[3], params[4], params[5], 
                                     params[6], params[7], params[8] );
    
    Eigen::Matrix< _T2, 3 , 1> calib_samp = calib_triad.unbiasNormalize( raw_samp );
    residuals[0] = _T2 ( g_mag_ ) - calib_samp.norm();
    return true;
  }

 其次,我们进行陀螺仪的标定:g_versor_pos0_是加速度计数据,g_versor_pos1_是下一帧加速度计数据,rot_mat是相邻两帧的旋转矩阵,这里我们使用4阶龙格-库塔积分来估计旋转矩阵,我们认为前后两帧的加速度分量是相对不变的,我们建立如下的残差方程:

template <typename _T2>
    bool operator() ( const _T2* const params, _T2* residuals ) const
  {
    CalibratedTriad_<_T2> calib_triad( params[0], params[1], params[2], 
                                      params[3], params[4], params[5], 
                                      params[6], params[7], params[8],
                                      optimize_bias_?params[9]:_T2(0), 
                                      optimize_bias_?params[10]:_T2(0), 
                                      optimize_bias_?params[11]:_T2(0) );

    std::vector< TriadData_<_T2> > calib_gyro_samples;
    calib_gyro_samples.reserve( interval_pos01_.end_idx - interval_pos01_.start_idx + 1 );
    
    for( int i = interval_pos01_.start_idx; i <= interval_pos01_.end_idx; i++ )
      calib_gyro_samples.push_back( TriadData_<_T2>( calib_triad.unbiasNormalize( gyro_samples_[i] ) ) );
    
    Eigen::Matrix< _T2, 3 , 3> rot_mat;
    integrateGyroInterval( calib_gyro_samples, rot_mat, _T2(dt_) );
    
    Eigen::Matrix< _T2, 3 , 1> diff = rot_mat.transpose()*g_versor_pos0_.template cast<_T2>() - g_versor_pos1_.template cast<_T2>();
    
    residuals[0] = diff(0);
    residuals[1] = diff(1);
    residuals[2] = diff(2);
    
    return true;
  }

 最后,对温度进行直线拟合,标定加速度计与陀螺仪的三轴k,b参数。

2.2 具体操作

 我们使用要完成数据的采集。首先将imu通电,采集温度上升过程当中imu的所有数据imu_temperature.txt,我这里使用的是博世bmx_160,半小时温度达到最高;然后我们采集第二段数据,需要充分激励IMU,需要IMU在各个轴方向分别进行0,45及90度的旋转,我这里使用机械臂进行操作,数据imu.txt采集完成,imu_tk编译成功后,我们执行如下命令:

./test_imu_calib data/imu.txt data/imu_temperature.txt

 加速度计的标定结果:

 陀螺仪的标定结果:

 温漂标定结果:

 标定结果分别位于acc_calib,gyro_calib,acc_temper,gyro_temper文件当中,若各个误差项误差范围均不超过-0.05~0.05,则认为该系列imu加工完好,可以正常使用。

 至此,IMU的整个标定已经完成,下一期我们开始单目及双目摄像头的标定。

本文参与 腾讯云自媒体分享计划,分享自微信公众号。
原始发表:2020-12-09,如有侵权请联系 cloudcommunity@tencent.com 删除

本文分享自 傅里叶的猫 微信公众号,前往查看

如有侵权,请联系 cloudcommunity@tencent.com 删除。

本文参与 腾讯云自媒体分享计划  ,欢迎热爱写作的你一起参与!

评论
登录后参与评论
0 条评论
热度
最新
推荐阅读
目录
  • 一、确定性误差公式
  • 二、工具标定
    • 2.1 代码细节
      • 2.2 具体操作
      领券
      问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档