惯性器件内参标定

惯性器件内参标定

1. 概述

标定的本质是参数辨识。首先明确哪些参数可辨识，其次弄清怎样辨识。

参数包括陀螺仪和加速度计各自的零偏、标度因数、安装误差。

2. 标定参数分析

（1）零偏

**误差解释:**陀螺仪或加速度计输出中的常值偏移，就是输出比输入多了一个常值误差，即常说的bias。

**误差特性:**由于零偏存在不稳定性,因此零偏并不是固定不变的。

**解决办法:**实际使用中,只能一段时间内近似为常值。

需要注意的是，我们之前通过Allan方差分析，得到了器件的量化噪声、角度随机游走、角速率随机游走、零偏不稳定性噪声、速率斜坡，仔细看，这些都是对零偏质量的分析，也可以直观的理解为零偏的波动和漂移程度，这里面并没有分析零偏本身的大小，而这个才是我们标定里要去估计的那个常值误差。

加速度计的零偏表示为
$$b_a=\left[\begin{array}{lll}{b}_{ax}& b_{ay}& b_{az}\end{array}\right]$$
陀螺仪的零偏表示为
$$b_g=\left[\begin{array}{lll}{b}_{gx}& b_{gy}& b_{gz}\end{array}\right]$$

（2）刻度系数误差

误差解释：器件的输出往往为脉冲值或模数转换得到的值,需要乘以一个刻度系数才能转换成角速度或加速度值,若
该系数不准,便存在刻度系数误差，意思就是假设器件输出的是标准单位角速度(rad/s)，那么输出和输入的比就是1。如果不是，就得需要标定，修正这个比例。

误差特性：不一定是常值,它会随着输入大小的不同而发生变化,这个就是标度因数的非线性。

解决办法:如果非线性程度比较大,则需要在标定之前先拟合该非线性曲线,并补偿成线性再去做标定。

加速度计的标度因数这里表示如下：
$$K_a=\left[\begin{array}{ccc}{K}_{ax}& & \\ & K_{ay}& \\ & & K_{az}\end{array}\right]$$
陀螺仪的标度因数这里表示为：
$$K_g=\left[\begin{array}{ccc}{K}_{gx}& & \\ & K_{gy}& \\ & & K_{gz}\end{array}\right]$$

（3）安装误差

**误差解释:**如图所示,b坐标系是正交的imu坐标系,g坐标系的三个轴是分别对应三个陀螺仪。由于加工工艺原因,陀螺仪的三个轴并不正交,而且和b坐标系的轴不重合,二者之间的偏差即为安装误差。

由于加工工艺原因，陀螺仪的三个轴并不正交，和我们导航中使用的正交轴不重合。我们需要仔细想一想，这个安装误差怎么在陀螺输出中体现出来的，因为我们标定时只能采集到陀螺的输出，而无法直接去测量安装误差。理论上，在陀螺坐标轴和b系重合的情况下，我们沿b系某一个坐标轴旋转，那么其他两个轴是不会有角速度输出的，而有了安装误差以后，便有了输出，比如说Yb有一个角速度，如果没有误差的话，那么其他两个角速度为零，如果有安装误差，两个坐标系不重合，那么二者之间有夹角，就会有角速度的分量。以图中一项误差为例，Sgxy表示的就是y轴的角速度，在x陀螺上由安装误差造成的输出，Sgxz就是z轴的角速度，在x陀螺上的安装误差造成的输出。由此，我们可以把所有的安装误差都成矩阵形式。

**误差特性:**实际系统中,由于硬件结构受温度影响,安装误差也会随温度发生变化。

**解决办法:**在不同温度下做标定,补偿温度变化量。

陀螺仪的安装误差这里表示如下
$$S_g=\left[\begin{array}{ccc}0& S_{gxy}& S_{gxz}\\ S_{gyx}& 0& S_{gyz}\\ S_{gzx}& S_{gzy}& 0\end{array}\right]$$
加速度计的安装误差这里表示为
$$S_a=\left[\begin{array}{ccc}0& S_{axy}& S_{axz}\\ S_{ayx}& 0& S_{ayz}\\ S_{azx}& S_{azy}& 0\end{array}\right]$$

（4）误差模型

通过上面的参数分析，我们已经可以很容易地写出误差模型了。

陀螺仪：
$$W=K_g\left(I+S_g\right)\omega +b_g\approx \left(K_g+S_g\right)\omega +b_g$$
其中W是陀螺输出，ω是各坐标轴真实输入。该公式的展开形式为
$$\left[\begin{array}{l}{W}_x\\ W_y\\ W_z\end{array}\right]=\left[\begin{array}{lll}{K}_{gx}& S_{gxy}& S_{gxz}\\ S_{gyx}& K_{gy}& S_{gyz}\\ S_{gzx}& S_{gzy}& K_{gz}\end{array}\right]\left[\begin{array}{l}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]+\left[\begin{array}{l}{b}_{gx}\\ b_{gy}\\ b_{gz}\end{array}\right]$$
同理，可以得到加速度计的展开形式为：
$$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[\begin{array}{ccc}{K}_{ax}& S_{axy}& S_{axz}\\ S_{ayx}& K_{ay}& S_{ayz}\\ S_{azx}& S_{azy}& K_{az}\end{array}\right]\left[\begin{array}{l}{a}_x\\ a_y\\ a_z\end{array}\right]+\left[\begin{array}{l}{b}_{ax}\\ b_{ay}\\ b_{gz}\end{array}\right]$$

3. 标定方法

标定的本质是参数辨识,参数包括陀螺仪和加速度计各自的零偏、刻度系数误差、安装误差。
辨识方法包括:

1. 解析法或最小二乘
2. 梯度下降迭代优化
3. 滤波(Kalman等)

常见标定方法与上面辨识方法的对应关系为:

1. 分立级标定: 解析法、最小二乘
2. 半系统级标定: 梯度下降迭代优化
3. 系统级标定: Kalman滤波、最小二乘

(1) 分立级标定–加速度计标定

在IMU的误差模型中,陀螺仪和加速度计的误差方程是互相独立的,可分别标定。

以加速度计为例,其误差模型方程为:
$$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[\begin{array}{ccc}{K}_{ax}& S_{axy}& S_{axz}\\ S_{ayx}& K_{ay}& S_{ayz}\\ S_{azx}& S_{azy}& K_{az}\end{array}\right]\left[\begin{array}{l}{a}_x\\ a_y\\ a_z\end{array}\right]+\left[\begin{array}{l}{b}_{ax}\\ b_{ay}\\ b_{gz}\end{array}\right]$$
误差模型方程为包含12个未知参数的方程组,显然方程组没有唯一解。此时,通过改变输入,构建多个不同方程,

组成的方程组便可求解参数。

以上就是分立级标定方法的思路,具体求解方法包括解析法和最小二乘法。

分立级标定的核心:通过旋转IMU,改变其输入构造方程组,并且每个位置对应的加速度输入和角速度输入都必须
是已知的。

构建方程组时,要不仅要方程组数量足够,而且要能够使误差参数可解,即系数矩阵可逆。为了满足这一点,常见的转位方案有六位置、八位置、十二位置等。

在实际使用时,通过判断系数矩阵是否满秩便可判断,理论上,只要转位方案能满足这一条件,就可以使用。

1）解析法

当IMU水平向上放置时，就有了如下已知条件
$$\{\begin{array}{l}{a}_x=0\\ a_y=0\\ a_z=g\end{array}$$
其中g为重力加速度。把它代入到加速度计展开的误差模型，可以得到
$$\{\begin{array}{l}{A}_x=S_{axz}\ast g+b_{ax}\\ A_y=S_{ayz}\ast g+b_{ay}\\ A_z=K_{az}\ast g+b_{az}\end{array}$$
同理，当IMU水平向下放置时，可以得到
$$\{\begin{array}{l}{A}_x=-S_{axz}\ast g+b_{ax}\\ A_y=-S_{ayz}\ast g+b_{ay}\\ A_z=-K_{az}\ast g+b_{az}\end{array}$$
联立这两个方程组，便可解出6个参数。随后，再次改变IMU放置方式，可解其他参数，并且,由此可以看出,转台需要调平。

2）最小二乘法

由于实际标定过程中，在一个位置采集得到的数据会有噪声，并且由于转台控制误差的存在，仅依靠联立两个位置去求解参数的方法精度有限，因此想通过联立更多位置去求解，这就是最小二乘法的目的。

对于加速度计误差模型，我们可以转换一下形式，写成
$$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=x\left[\begin{array}{c}{K}_{ax}\\ K_{ay}\\ K_{az}\\ S_{axy}\\ S_{axz}\\ S_{ayx}\\ S_{ayz}\\ S_{azx}\\ S_{azy}\\ b_{ax}\\ b_{ay}\\ b_{az}\end{array}\right]$$
其中
$$x=\left[\begin{array}{ll}F& I_{3\times 3}\end{array}\right]$$

$$F=\left[\begin{array}{ccccccccc}{a}_x& 0& 0& a_y& a_z& 0& 0& 0& 0\\ 0& a_y& 0& 0& 0& a_x& a_z& 0& 0\\ 0& 0& a_z& 0& 0& 0& 0& a_x& a_y\end{array}\right]$$

为了后面书写方便，这种新的模型，可以概括写为
$$y=x\theta$$
转台在每个位置，都可以得到一个这样的方程
$$y_i=x_i\theta$$
联立所有的位置，可以得到
$$Y=X\theta$$
其中
$$\begin{gathered} Y={\left[\begin{array}{llll}{y}_0^T& y_1^T& \dots & y_n^T\end{array}\right]}^T \\ X={\left[\begin{array}{llll}{x}_0^T& x_1^T& \dots & x_n^T\end{array}\right]}^T \end{gathered}$$
最后，参数拟合问题等效为最小二乘问题，其解为：
$$\theta =(X^{T}X{)}^{-1}{X}^{T}Y$$
这便是待求的标定参数。

(2) 分立级标定–陀螺仪标定

陀螺仪标定与加速度计标定的重要区别:

a. 转台一般角速度不如角度精度高,因此不是直接以角速度作为真值,而是积分得到角度作为真值;

b. 陀螺的输入角速度中,除了转台转动以外,还包括地球自转角速度(地速)。若要消除它的影响,可以把

转台与地理系(东-北-天)对齐,但操作复杂。常见的做法是在构建方程时,让地速自动抵消。

同理，这里是绕z轴旋转得到x，y的结果，绕x或者y旋转得到z的结果。

（2） 半系统标定

1）标定思路

基于转台的标定方法简单、精度高，但是有一个最大的缺点，就是这个方法太贵了。对于一些低精度的IMU，本身精度不高，那么也就不需要使用这么高精度的标定方法，因此，如果能为这种需求去寻找一些不依赖转台的标定方法，那就再好不过了。

再进一步想下去，好是好，可是怎么实现呢。标定的前提是要有真值，因为我们测量到的是IMU的输入，输入和真值之间的差异是由内参误差引起的，有了真值才能有内参的辨识。自然界天然的真值输入是重力加速度，但是，借助转台的时候，才能知道重力在IMU上的投影是多少，而我们此处想找的是不依赖转台的方法，那怎么搞？其实，不防换一种思路，寻找输入和真值之间得差异，一定需要知道重力在IMU每个轴上的投影是多少吗？当没有内参误差的时候，加速度计三个轴的矢量和必然和重力矢量大小相等，反之，则不等，这不也是找到二者的差异了吗，并且和内参误差建立了联系。

当然，这些只是用文字描述的思路，具体到实现上，还得用数学模型表示。

以下的标定思路、流程是来自于论文：A Robust and Easy to Implement Method for IMU Calibration without External Equipments

并且该文章有对应的开源代码：https://github.com/Kyle-ak/imu_tk

由于符号以及内参模型上有一些差异，因此本文与该论文在表达与公式上会有一些差异，但并不矛盾。

2）内参模型

内参模型不是讲过了吗，为什么还要再讲一次？这就又得费一点口舌了，理解起来可能要费点劲，我希望能讲得明白一些。

回顾基于转台的标定方法，我们定义了12项安装误差，他们表示的是加速度计、陀螺仪的各个敏感轴与IMU的坐标轴（即直角坐标系b系）之间的关系，那这里有一个疑问，b系是怎么来的？或者说，为什么把它规定在现在这个方向，而不是别的方向？此处的意思是指，如果我把一个和现在的b系非常接近，只差0.1°的一个直角坐标系规定为新的b系，有问题吗？好像没什么问题。也就是说b系是可以人为规定的。

在基于转台的标定方法里，IMU的b系其实默认被规定成了和转台的坐标系重合，因为这样转台的输入，才真的是IMU的输入，上面的各种基于转台标定的模型和方法才成立。

而当标定方法脱离转台时，这种约束关系就不存在了，而b系又是可以认为规定的，那么就有一种规定方法，可以简化内参模型。

在规定坐标系时，若令IMU坐标系(b系)的 $X_b$ 轴与加速度计的 $X_a$ 轴重合，且 $X_{b}O{Y}_b$ 与 $X_{a}O{Y}_a$ 共面(如下图)

则此时，加速度计的安装误差只剩下三个参数
$$S_a=\left[\begin{array}{ccc}0& 0& 0\\ S_{ayx}& 0& 0\\ S_{azx}& S_{azy}& 0\end{array}\right]$$
另外，当满足这种关系时，b系就已经完全固定了，因此此时陀螺仪的三个轴和b系之间的安装误差仍为 6 个。

3）加速度计标定

按照前述思路，我们需要建立测量的加速度矢量与重力加速度矢量之间的误差，并以此误差为基础，反推出内参模型中的参数。

按照内参定义，加速度计输出与输入的关系为
$$A=K_a\left(I+S_a\right)a+b_a$$
它表达的是，由真实输入，得到测量值的过程，而反过来，由测量值可以得到真实值为
$$a={\left(I+S_a\right)}^{-1}{K}_a^{-1}\left(A-b_a\right)$$
在求解时，逆运算的存在使模型变得复杂，因此使用以下方式进行简化
$$K_a^{\prime }=\left[\begin{array}{ccc}{K}_{ax}^{\prime }& & \\ & K_{ay}^{\prime }& \\ & & K_{az}^{\prime }\end{array}\right]=K_a^{-1}=\left[\begin{array}{ccc}\frac{1}{K_{ax}}& & \\ & \frac{1}{K_{ay}}& \\ & & \frac{1}{K_{az}}\end{array}\right]$$
$${\left(I+S_a\right)}^{-1}\approx I-S_a$$
简化后的形式为
$$a=\left(I-S_a\right)K_a^{\prime }\left(A-b_a\right)$$
当imu静止时，输入只有重力加速度，把重力矢量定义为
$$\mathbf{g}={\left[\begin{array}{lll}0& 0& g_0\end{array}\right]}^T$$
其中 g_0 为当地重力加速度大小。当内参完全准确时，有
$$\Vert \mathbf{g}{\Vert }^2=\Vert \mathbf{a}{\Vert }^2$$
当内参存在误差时，可写出残差函数为
$$f\left({\theta }^{acc}\right)=\Vert \mathbf{g}{\Vert }^2-\Vert \mathbf{a}{\Vert }^2$$
其中，
$${\mathbf{\theta }}^{acc}=\left[S_{ayx},S_{azx},S_{azy},K_{ax},K_{ay},K_{az},b_{gx},b_{gy},b_{gz}\right]$$
表示待估计的内参。

有了模型，接下来就是参数的辨识了，此处采用的是高斯牛顿法进行辨识，这里不会高斯牛顿法的一些基础知识进行展开，各位可以自行搜索一些资料去了解。暂时不懂的，也不影响理解，我们只需要知道，有了残差函数以及残差对代估参数的雅可比之后，便可以进行优化，求解出参数，残差函数，我们已经给出了，雅可比指的是残差函数对${\mathbf{\theta }}^{acc}$ 中的每一项求偏倒，这都是一些基本的技能，也不详细展开了。

最后，需要注意的是，由于待求解参数很多，而只静止在一个位置是无法求解出全部参数的，因为解不唯一，这和基于转台标定中，要用多个方程联立求解参数的本质是一样的，只是此处的方程不一样，辨识方法也不一样。

4）陀螺仪标定

陀螺仪标定在加速度计标定之后进行，因此在这一步，可以认为加速度计是无误差的。另外，在这种方法中，我们并不使用优化的方式标定陀螺仪的零偏，主要原因还是因为零偏造成的影响偏小，标不准。而且零偏的标定，可以使用前述静止的方法去求解，简单易行。因此，此处陀螺仪的待估参数为
$${\mathbf{\theta }}^{\text{gyro }}=\left[S_{gxy},S_{gxz},S_{gyx},S_{gyz},S_{gzx},S_{gzy},K_{gx},K_{gy},K_{gz}\right]$$
在上面图形所以的IMU多位置放置方案中，令 ${\mathbf{u}}_{a,k}$ 代表在第 k 个静止位置时，三个加速度计的输出构成的矢量在IMU坐标系下的表示，即
$${\mathbf{u}}_{a,k}=R_{b_{k}w}\mathbf{g}$$
其中$R_{b_{k}w}$ 表示从世界坐标系(w系，和水平面平行且不随IMU旋转而旋转的坐标系)到第 k 个位置对应的IMU坐标系的转换矩阵。需要注意的是，我们并不需要知道$R_{b_{k}w}$是多少 ，因为 ${\mathbf{u}}_{a,k}$ 是直接测量的。

在第 k+1 个位置时，同样有
$${\mathbf{u}}_{a,k+1}=R_{b_{k+1}w}\mathbf{g}$$
从第 k 个位置，到第 k+1 个位置，可以根据陀螺仪测量计算出两个位置之间的相对旋转 $R_{b_{k+1}{b}_k}$ (这里需要用到惯性导航解算的知识，这个会在后面的文章中介绍，此处仅需要知道它能实现这样计算就行)，根据该旋转可以算出一个第 k+1 位置加速度计输出矢量的推测值
$${\mathbf{u}}_{g,k+1}=R_{b_{k+1}{b}_k}{\mathbf{u}}_{a,k}$$
可见，推测值的误差就体现了陀螺仪的误差，因此可以根据推测值与观测值构建残差函数
$$f\left({\theta }^{\text{gyro }}\right)={\mathbf{u}}_{a,k+1}-{\mathbf{u}}_{g,k+1}$$
有了残差，就可以同样使用高斯牛顿方法去进行求解了

原文链接：多传感器融合定位理论基础（四）：IMU内参模型及标定