淦VINS-MONO源码 01--IMU的误差模型和标定

在看完orb-slam后，也投了一些实习，发现纯视觉的方案在很多方面都没得到采用，特别是自动驾驶中定位感知方面，基本上是雷达+IMU+相机+GPS等等的多传感器融合，所以接下来的计划会看看vins-mono和loam的源代码，一个是IMU+视觉，另一个则是多传感器融合。
在单目的ORB中其实存在一个问题，就是尺度不确定性，轨迹和路标可以等比例放大或缩小都是满足约束的，所以引入IMU是非常有必要的，通过IMU能得到加速度和角速度，通过积分就能求解旋转角和位移。但是毕竟是测量元件，本身的误差是一定存在的，所以第一步就是对IMU的误差进行标定，我们就来研究一下IMU的误差模型。

工作原理

加速度计工作原理

测量原理可以用一个简单的质量块 + 弹簧 + 指示计来表示，加速度计测量值 am 为弹簧拉力对应的加速度

f 弹簧拉力， a 物体在惯性系下的加速度， g 重力加速度。
MEMS 加速度计利用电容或者电阻桥来等原理来测量 am

陀螺仪测量原理

陀螺仪主要用来测量物体的旋转角速度，低端 MEMS 陀螺上一般采用振动陀螺原理，通过测量 Coriolis-force 来间接得到角速度。
• 在旋转坐标系中，运动的物体受到科氏力作用
• MEMS 陀螺仪：一个主动运动轴 + 一个敏感轴

音叉振动陀螺如下图，叉子的中间为旋转轴，叉子左右两个质量块，做方向相反的正弦运动，质量块受到的科氏力方向相反。

实际上，两个质量块不可能完全一致，也就是说陀螺仪的测量可能会受到外部加速度的影响，即常称的 G-sensitivity。这个就是为了消除这个影响。

误差来源

IMU的误差来主要来自于三部分，包括噪声(Bias and Noise)、尺度因子(Scale errors)和轴偏差(Axis misalignments)。加速度计和陀螺仪的测量模型可以用下面式子表达：

其中上标a 表示加速度计，g 表示陀螺仪，B 表示正交的参考坐标系，S 表示非正交的选准坐标系。T 表示轴偏差的变换矩阵，K 表示尺度因子，aS,wS 表示真值，b,ν 分别表示Bias和白噪声。

IMU噪声模型

• 加速度计和陀螺仪的误差可以分为：确定性误差，随机误差。
• 确定性误差可以事先标定确定，包括： bias, scale …
• 随机误差通常假设噪声服从高斯分布，包括：高斯白噪声， bias
随机游走…

bias

理论上，当没有外部作用时， IMU 传感器的输出应该为 0。但是，实际数据存在一个偏置 b。加速度计 bias 对位姿估计的影响。

scale

scale 可以看成是实际数值和传感器输出值之间的比值

轴偏差

多轴 IMU 传感器制作的时候，由于制作工艺的问题，会使得 xyz 轴可能不垂直，如下图所示。

IMU校准

IMU在校准过程中，加速度计和陀螺仪是分开校准的，一般是先校准加速度计，然后利用准确的加速度计信息再来校准陀螺仪。整个校准过程的流程如下：

st=>start: 静止放置IMU T秒
op1=>operation: 旋转IMU使其保持不同的姿态
op2=>operation: 在某种姿态下，至少保持静态t秒
cond=>condition: 是否旋转IMU N次
e=>end: 用下面的算法估计误差参数

st->op1->op2->cond
cond(yes)->e
cond(no)->op1

其中，初始化时间 T一般取50s，旋转后保持静态时间t 取1∼4s，旋转次数N 取36∼50 次。一般来说，这个旋转次数越多越好，至少旋转次数要大于要求解的参数的个数。

确定性误差

六面法标定bias和scale factor

六面法标定加速：
六面法是指将加速度计的 3 个轴分别朝上或者朝下水平放置一段时间，采集 6 个面的数据完成标定。
如果各个轴都是正交的，那很容易得到 bias 和 scale：

其中， l 为加速度计某个轴的测量值， g 为当地的重力加速度。
考虑轴间误差的时候，实际加速度和测量值之间的关系为：

水平静止放置 6 面的时候，加速度的理论值为

对应的测量值矩阵 L ：

利用最小二乘就能够把 12 个变量求出来。

六面法标定陀螺仪：
和加速度计六面法不同的是，陀螺仪的真实值由高精度转台提供，这里的 6 面是指各个轴顺时针和逆时针旋转。

温度相关的参数标定

目的：这个标定的主要目的是对传感器估计的 bias 和 scale 进行温度补偿，获取不同温度时 bias 和 scale 的值，绘制成曲线。
两种标定方法：
• soak method: 控制恒温室的温度值，然后读取传感器数值进行标
定。
• ramp method：记录一段时间内线性升温和降温时传感器的数据
来进行标定。

随机误差

高斯白噪声

IMU 数据连续时间上受到一个均值为 0，方差为 σ，各时刻之间相互
独立的高斯过程 n(t)

其中 δ() 表示狄拉克函数。
IMU 传感器获取的数据为离散采样，所以离散化的高斯白噪声 建模如下：

当传感器采集信号时，假设采样时间段内信息为常数：

只考虑高斯白噪声的积分

协方差为

即：

也就是说高斯白噪声的连续时间到离散时间之间差一个 √1∆t, √∆t 是
传感器的采样时间。

Bias 随机游走

通常用维纳过程 (wiener process) 来建模 bias 随时间连续变化的过程，
离散时间下称之为随机游走。

其中 w 是方差为 1 的白噪声。
将上式代入前面传感器采集信号的公式，并提取 bias 积分部分：

由此可得离散化下的 bias 协方差：


即:

bias 随机游走的噪声方差从连续时间到离散之间需要乘以 √∆t

艾伦方差标定

Allan 方差法是 20 世纪 60 年代由美国国家标准局的 David Allan 提出
的，它是一种基于时域的分析方法。
常用的Allan方差工具，主要有以下两种：

https://github.com/gaowenliang/imu_utils
https://github.com/rpng/kalibr_allan

具体的流程如下：

1. 保持传感器绝对静止获取数据。
2. 对数据进行分段，设定时间段的时长，如下图所示。
3. 将传感器数据按照时间段进行平均。
4. 计算方差，绘制艾伦曲线。
   
   得到的艾伦曲线如下图所示：

Allan方差法可用于5种随机误差的标定。
具体可以参考这两篇文章：
VIO标定IMU随机误差：Allan方差法.
陀螺 Allan 方差分析.

数学模型

加速度计数学模型

导航系 G 为东北天：

理论测量值为：

如果考虑高斯白噪声， bias，以及尺度因子，则为：

通常假设尺度因子为单位矩阵。

陀螺仪数学模型

考虑尺度因子，高斯白噪声，以及 bias, 陀螺仪的误差模型如下：

低端传感器，考虑加速度对陀螺仪的影响，即 g-灵敏度：