IMU标定（二）随机误差的标定

IMU标定（二）随机误差的标定

一、allan方差基本原理

  allan方差是一种时域分析技术，一般用于仪器的噪声研究，是公认的IMU参数分析方法，其主要思路是利用不同相关时间内所表现的不同特征来描述各种噪声源，其噪声模型为：
$${\sigma }^2(\tau )=\sum _{n=-2}^2{C}_n{\tau }^n$$

  对于IMU来说，主要包含五类噪声源，量化噪声Q，角度随机游走N，零偏不稳定性B，速率随机游走K，速率爬坡R，这五类噪声源的具体推导可参考西工大严恭敏老师的《惯性仪器测试与数据分析》。我们这里直接给出结果：

  如果将各噪声源作单独统计，则其各误差源的平方和就是allan方差，这里我们直接给出表达式：
$${\sigma }^2(\tau )=\frac{3Q^2}{{\tau }^2}+\frac{N^2}{\tau }+B^2(\frac{2}{\pi })ln2+\frac{K^2\tau }{3}+\frac{R^2{\tau }^2}{2}=\sum _{n=-2}^2{C}_n{\tau }^n$$

  将五类噪声源分别作曲线，然后叠加在一起，即为下图allan方差的时域表示：

二、工具标定

  这里我们使用开源的imu_utils进行标定，读者可以从github直接git clone。

2.1 代码细节

1. 首先利用两点求平均扩充点数的思路，将数据分成不同的时间间隔

int mode = numData / 2;
unsigned int maxStride = 1;
int shft  =0;
while(mode){
    mode = mode >> 1;
    maxStride = 1 << shft;
    shft++;
}

2. 在每一个间隔下都进行方差的计算，并将方差与真实曲线建立残差，进行优化：

for ( int i = 0; i < num_samples; ++i ) {
    ceres::CostFunction* f = new ceres::AutoDiffCostFunction< AllanSigmaError, 1, 5 > (new AllanSigmaError( sigma2s_tmp[i], m_taus[i] ) );
    problem.AddResidualBlock( f, NULL, param );
}

3. 残差公式：

template< typename T >
bool operator( )( const T* const _paramt, T* residuals ) const {
    T _Q   = T( _paramt[0] );
    T _N   = T( _paramt[1] );
    T _B   = T( _paramt[2] );
    T _K   = T( _paramt[3] );
    T _R   = T( _paramt[4] );
    T _tau = T( tau );

    T _sigma2    = calcSigma2( _Q, _N, _B, _K, _R, _tau );
    T _dsigma2   = T( calcLog10( _sigma2 ) ) - T( calcLog10( sigma2 ) );
    residuals[0] = _dsigma2;
        
    return true;
}

template< typename T >
T calcSigma2( T _Q, T _N, T _B, T _K, T _R, T _tau ) const {
    return  _Q * _Q / ( _tau * _tau )
          + _N * _N / _tau
          + _B * _B
          + _K * _K * _tau
          + _R * _R * _tau * _tau;
}

我们对$C_n$进行初始化，然后对上面的优化函数进行迭代优化，按照第一节给出的公式即可恢复出五类噪声。下面我们具体操作一下。

2.2 具体操作

  imu_utils默认使用ros环境进行编译安装，这里默认读者已经对ros有了基本的了解，首先我们在catkin_ws/src/imu_utils/launch下新建一个自己传感器的lauch文件，修改自己的imu_topic及name(大亮这里以bmx160为例子)：

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/imu"/>
        <param name="imu_name" type="string" value= "bmx160"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "120"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

执行下面命令：

source ~/catkin_ws/devel/setup.bash
roslaunch ~/catkin_ws/src/imu_utils/launch/bmx160.launch

然后我们采集imu静止2小时以上的数据，注意避免IMU周围环境受到震动，否则陀螺仪会有较大误差，采集好数据后，ros下创建bag文件，我们再开一个终端执行以下命令进行播放(我的IMU采集频率是500赫兹):

rosbag play -r 500 imu.bag

这时第一个终端界面会出现下面结果：

生成的bmx160.yaml文件在imu_utils/data下，打开yaml文件我们可以看到陀螺仪与加速度计的三轴噪声以及随机游走，至此标定完成。

在scripts下生成很多matlab脚本文件，可以直接使用matlab作图，我们这里直接给出陀螺仪的allan方差曲线图

微信公众号：Quant_Times