IMU标定（三）确定误差的标定

一、确定性误差公式

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

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^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^aend{bmatrix}

b^g=begin{bmatrix} b_x^g & b_y^g & b_z^gend{bmatrix}

二、工具标定

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

2.1 代码细节

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

代码语言：javascript复制

  /** @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)，默认前后两帧加速度计三轴加速度分量相等，我们建立如下的残差方程，直接进行迭代优化即可得到加速度计的参数；

代码语言：javascript复制

  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阶龙格-库塔积分来估计旋转矩阵，我们认为前后两帧的加速度分量是相对不变的，我们建立如下的残差方程：

代码语言：javascript复制

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编译成功后，我们执行如下命令：

代码语言：javascript复制

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

加速度计的标定结果:

陀螺仪的标定结果：

温漂标定结果：

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

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

txt

0 人点赞