IMU标定+数据融合(一次解决所有困惑内容)

---

---

IMU标定+数据融合

0 惯组模型

1）资料

• 在VIO与MSCKF中常用到IMU的状态误差模型，这里做一个整理，参考。

• 陀螺仪的模型；

1 标定

MEMS惯性器件的误差一般分成两类：系统性误差和随机误差。系统性误差本质就是能找到规律的误差，所以可以实时补偿掉，主要包括常值偏移、比例因子、轴安装误差等。但是随机误差一般指噪声，无法找到合适的关系函数去描述噪声，所以很难处理。一般采用时间序列分析法对零点偏移的数据进行误差建模分析，当然也可以用卡尔曼滤波算法减小随机噪声的影响。

下面会从系统误差、随机误差两个部分去分析IMU组件的标定过程。

1.1 系统性误差

IMU的误差来主要来自于三部分，包括噪声(Bias and Noise)、尺度因子(Scale errors)和轴偏差(Axis misalignments)。加速度计和陀螺仪的测量模型可以用下面来表达。
$$\begin{array}{rl}& a^B=T^a{K}^a\left(a^S+b^a+v^a\right)\\ & w^B=T^g{K}^g\left(w^S+b^g+v^g\right)\end{array}$$
其中上标𝑎 表示加速度计，𝑔 表示陀螺仪，𝐵表示正交的参考坐标系，𝑆 表示非正交的选准坐标系。𝑇表示轴偏差的变换矩阵，𝐾 表示尺度因子，𝑏,𝜈分别表示Bias（随机游走）和白噪声。

论文A robust and easy to implement method for IMU calibration without external equipments是分析这类误差的经典文献，在博客中有对其方法的具体介绍，不再赘述。具体方法是利用非线性优化的方式，对采集的数据进行数据处理，最终估计出$T$和$K$以及$b$，三个矩阵。再通过上面的计算方式，便得到IMU组件的“真值”。

对于陀螺仪的误差，零偏的处理，会影响到姿态估计的准确性（影响很大）。

对陀螺仪的各个参数进行了系统性的介绍。

另外，这里对随机游走进行了介绍。

1.2 ALLAN方差校准随机误差

陀螺仪的随机误差主要包括：量化噪声、角度随机游走、零偏不稳定性、角速率随机游走、速率斜坡和正弦分量。对于这些随机误差，利用常规的分析方法，例如计算样本 均值和方差，并不能揭示出潜在的误差源。另一方面，在实 际工作中通过对自相关函数和功率谱密度函数加以分析将随机误差分离出来是很困难的。

要对陀螺信号进行预处理首先需要对其噪声成分进行辨识。Allan方差分析法是目前最常用的陀螺噪声辨识方法之一。由于Allan方差分析是通过调节Allan方差滤波器带宽，对功率谱进行细致分割，能够辨识出多种不同类型的随机过程误差，并定量分离各项误差系数，而且算法上操作简单、便于计算，在陀螺噪声辨识方面优势明显。

如上所述，在Allan方差分析中，共有5个噪声参数：量化噪声、角度随机游走、零偏不稳定性、速度随机游走和速度爬升。下面主要分析Allan方差的计算、图像的绘制以及最终噪声的分析。

这个开源项目提供了实例(matlab代码)，最终给出下面类似的结果，有参考的价值。

量化噪声 X轴：0.169424 Y轴：0.271556 Z轴：0.581170 单位：arcsec
- 角度随机游走 X轴：0.225774 Y轴：0.258557 Z轴：0.662383 单位：deg/h^0.5
- 零偏不稳定性 X轴：0.558054 Y轴：2.620662 Z轴：5.506688 单位：deg/h
- 角速率游走 X轴：0.511945 Y轴：12.864516 Z轴：54.069972 单位：deg/h/h^0.5
速率斜坡 X轴：5.592172 Y轴：4.763508 Z轴：12.371376 单位：deg/h/h

Allan方差实质上就是通过求取整个信息采集过程中相邻时间段的方差形式来对信号在整个时间段内的稳定情况进行衡量的过程。在使用Allan方差分析惯性器件的误差的时候，可以认为惯性器件输出数据的随机部分是由特定的噪声源产生的。在保证测试环境稳定定情况下，可以认为各噪声源是独立的，那么计算出的Allan方差就是各部分误差的平方和。Allan方差的双对数曲线的典型形式以及不同斜率段的曲线代表的噪声成分如下图：

噪声建模如下：
$$\begin{array}{rl}{\sigma }_{total}^2(\tau )& ={\sigma }_Q^2(\tau )+{\sigma }_N^2(\tau )+{\sigma }_B^2(\tau )+{\sigma }_K^2(\tau )+{\sigma }_R^2(\tau )\\ & =\frac{3Q^2}{{\tau }^2}+\frac{N^2}{\tau }+\frac{2B^2}{\pi }\ln 2+\frac{K^2\tau }{3}+\frac{R^2{\tau }^2}{2}\end{array}$$
采用allan方差分析陀螺仪的噪声，常常需要将IMU静止放置很久（2小时或者1000s）。可以对上述模型进行简化：
$$\Omega (t)={\Omega }_{\text{Ideal}}(t)+{\mathrm{Bias}}_N(t)+{\mathrm{Bias}}_B(t)+{\mathrm{Bias}}_K(t)$$

使用长时间静止的陀螺仪数据对陀螺仪噪声参数进行分析，上式中，三个噪声参数N（角度随机游走）（备注：角速度的白噪声，积分出来的角度就是随机游走），K（速率随机游走）和B（偏差不稳定性）（或者叫零偏不稳定性）。

最终参考https://zhuanlan.zhihu.com/p/71202815，获得解释。

维纳过程可以不严格地理解为高斯白噪声的积分，所谓“角度随机游走”是指角速度测量的是“白噪声”，在一阶积分之后得到了“维纳过程”，也就是随机游走。

总结来说：系统误差可以通过传感器的校准消除。随机误差主要包含随机行走误差和零偏不稳定性误差，其中随机行走误差可以理解为传感器的高斯白噪声，零偏不稳定性误差表现为传感器的零偏会随时间逐渐变化（有时候被认为是一个常值）。一般文献中的 IMU 随机误差模型是高斯白噪声和维纳过程的叠加。最后，IMU 的随机误差的参数可以通过艾伦方法测量。在卡尔曼滤波器中，我们需要使用 IMU 的误差模型来计算状态变量的方差随时间的变化。

到底“速率随机游走”和“零偏不稳定性”，哪个是bias？懵逼了。

速率随机游走并不是bias，零偏不稳定性才是bias，也是一个随机游走变量。

下面举一个例子讨论ALLAN方差的分析过程，采集陀螺仪的数据并做ALLAN分析得到下图。

所以这里的MEMS陀螺所包含的噪声成分主要有:角度随机游走、相关噪声、速率随机游走，而其中代表零偏不稳定性的斜率为0的部分曲线很短，可以认为陀螺中零偏不稳定性影响都相对较小，所以前面进行的陀螺校准得到的零偏可以认为是一个常值。

以上就是通过Allan方差分析得到陀螺仪Bias的过程，一般要采集好几个小时的数据。如果仅需要，零偏参数（bias），则在初始放置的50s左右的时间就足够了。因此描述一个简单的模型，认为陀螺的数据误差主要是零偏和白噪声：
$$\begin{array}{l}{\omega }_m={\omega }_t+b+n_r,\dot{b}=n_w\\ \mathrm{E}\left(n_r(t+\tau )n_r(t)\right)={\sigma }_r^2\delta (\tau ),\mathrm{E}\left(n_w(t+\tau )n_r(t)\right)={\sigma }_w^2\delta (\tau )\end{array}$$
这个式子中的$n_r$项就是高斯白噪声，它由陀螺仪的随机行走误差和一部分零偏不稳定性误差组成；b项是维纳过程，由另外的一部分零偏不稳定性组成。 sigma_r可以理解为积分后角速度的误差的方差单位时间的增量， ${\sigma }_w$可以理解为陀螺仪零偏的方差单位时间的增量。

需要注意的是，这里的随机行走误差指的是陀螺仪的数据（角速度数据）中的，它相当于是一个对白噪声积分的量。

另外，下图直观展示了IMU的各个参数的使用规则：

1.4 开源工具介绍

现在已经有成熟的工具包可以使用，imu_utils 可以用来标定随机误差，imu_tk 可以用来标定确定性误差（系统误差），其均是基于LINUX平台下，另外需要安装ROS组件。这里的方法在机器人领域比较常用。

下面是imu_utils的结果，不清楚gyr_w是做什么。(gyr_w就是零偏、随机游走(walk))

%YAML:1.0
---
type: IMU
name: A3
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 2.0855329162218030e-03
      gyr_w: 3.0516102188797762e-05
   x-axis:
      gyr_n: 2.1462995497479255e-03
      gyr_w: 3.5174142237247770e-05
   y-axis:
      gyr_n: 2.1439814406437130e-03
      gyr_w: 3.2984930038857547e-05
   z-axis:
      gyr_n: 1.9663177582737710e-03
      gyr_w: 2.3389234290287965e-05
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.6639085438429400e-02
      acc_w: 6.6969300085595741e-04
   x-axis:
      acc_n: 2.1966919407157028e-02
      acc_w: 5.3138962789797575e-04
   y-axis:
      acc_n: 2.2567628885792314e-02
      acc_w: 6.2998018768851071e-04
   z-axis:
      acc_n: 3.5382708022338866e-02
      acc_w: 8.4770918698138565e-04

下面是imu_tk的结果：Given a raw sensor reading X (e.g., the acceleration ), the calibrated “unbiased” reading X’ is obtained.

最终生成两个标定文件，分别为陀螺仪和加速度计的旋转矩阵（misalignment matrix），刻度因数(scale matrix)和零偏(bias)。完成标定，基本格式如下：

Misalignment matrix:
    [    1     -mis_yz   mis_zy  ]
T = [  mis_xz     1     -mis_zx  ]
    [ -mis_xy   mis_yx     1     ]
Scale matrix:
    [  s_x      0        0  ]
K = [   0      s_y       0  ]
    [   0       0       s_z ]
Bias vector:
    [ b_x ]
B = [ b_y ]
    [ b_z ]
X' = T*K*(X - B)

2 数据融合

1） 单位

刚开始接触IMU传感器常常对单位很困惑。我以小觅相机的IMU输出数据举例子，可以认为其是IMU惯组输出的标准数据。

小觅相机	其他常用（论文中，应该是转换之后的）(处理过的)
Acc: g m/s^2(g表示重力加速度) gyro: deg/s	acc: m/s^2 gyro: rad/s

2） 设计滤波器

用滤波器的话有一点要注意：滤波器是会造成延迟的，上篇文章提到过，延迟对控制系统的影响是很大的，所以千万不能为了追求滤波效果，造成大的延迟，通常不要超过一个周期最好。一阶低通效果就足够了，当然也可以设计IIR、FIR之类的。

随机误差主要是同随常数、一阶马尔可夫过程和白噪声组成的，惯性器件的随机常数漂移在IMU 启动并进入正常工作状态后一般保持不变。因此，随机漂移是信号漂移中的未知原因引起的，特点是其均值为零，在样本数据达到一定的规模的基础上，对惯性器件不产生影响。在实际应用中，一般不会考虑这个，只对系统性的确定性误差进行补偿。

3 比较精彩的一些开源项目

1）开源索引

开源：

• https://github.com/miguelrasteiro/IMU_dataset

工业上的IMU处理

• https://github.com/eruffaldi/imu_comparison_data

这个开源很高级，给出了论文中的图示

• Gait-Tracking-With-x-IMU](https://github.com/xioTechnologies/Gait-Tracking-With-x-IMU)项目，采用互补滤波器

2）开源分析

互补滤波器相关目录：

1. /Users/chenshiming/Downloads/Gait-Tracking-With-x-IMU-master/Gait Tracking With x-IMU
2. /Users/chenshiming/Downloads/IMU_dataset-master
3. /Users/chenshiming/Desktop/工作资料/protel99se海格/封装代码/激光SLAM/标定/IMUCalibration-Gesture-master（Matlab）

3）标定分析

利用开源3对小觅相机进行标定。

最终不标定与标定的角度积分对比：未标定

标定之后的：

标定之后产生更大的漂移，其结果一定是错误的。发现是因为陀螺仪bias估计错误，根本原因是FindFixdata函数出错。

开源项目作者说需要对FindFixdata进行调整。原始的findfixdata函数的做法，就是找到相应（旋转或者静止）的区域。

4） 一些问题

• 为什么无人机的陀螺仪不产生漂移？

  陀螺仪都有漂移，不要妄想一个感应器就能搞定姿态。比如无人机飞控就会利用低通滤波、扩展卡尔曼滤波的算法，把陀螺仪、罗盘、加速度计、重力计、GPS、甚至双目视觉等来计算飞机自身的姿态和位置。

• 为什么测量系统不产生漂移？

  因为经过同样的低通滤波器之后，也不会产生漂移。