多传感器融合定位六-惯性导航原理及误差分析

前面几章讲了怎样用单个传感器把图建出来然后再做定位。接下来就进入了融合的环节，即怎样把图和定位建的更好。

1. 惯性技术简介

1.1 惯性技术发展历史

1687年，伟大的英国科学家牛顿提出力学三大定律，为惯性导航技术奠定了理论基础。
自20世纪60年代起，出现了挠性陀螺仪和动力谐调陀螺仪，同时平台式惯导系统发展迅速，并大量装备各种飞机、舰船、导弹和航天飞行器。

1.2 惯性器件

1.2.1 机械陀螺(几乎没人用了)

• 定轴性： 当陀螺转子以高速旋转时，在没有任何外力矩作用在陀螺仪上时，陀螺仪的自转轴在惯性空间中的指向保持稳定不变，即指向一个固定的方向；同时反抗任何改变转子轴向的力量。
• 进动性： 当转子高速旋转时，若外力矩作用于外环轴，陀螺仪将绕内环轴转动；若外力矩作用于内环轴，陀螺仪将绕外环轴转动。其转动角速度方向与外力矩作用方向互相垂直。
  

1.2.2 激光陀螺

Sagnac效应由法国物理学家 Sagnac 于 1913 年发现，其原理是干涉仪相对惯性空间静止时，光路 A 和 B 的光程相等，当有角速度时，光程不相等，便会产生干涉。

1.2.3 光纤陀螺

同样基于Sagnac效应，传播介质改成了光纤。（相比激光陀螺，成本低很多）

1.2.4 MEMS陀螺(常用)

科里奥利力（Coriolis force，简称为科氏力），是对旋转体系（比如自传的地球，旋转的圆盘等）中进行直线运动的质点由于惯性相对于旋转体系产生的直线运动的偏移的一种描述。
如左图所示有一个圆盘，圆盘静止的时候，有一个球从里往外去运动的时候，画出来肯定是一条直线。现在假设这个圆盘它旋转了一下，旋转以后这个球画出来的就不是一条直线了，它是一个弧线。也就是说一个东西往前运动，旋转它的载体时，这个运动的东西会受到一个力，往另一个方向走。
右图有一个摆动器械，它在一个微机械的器械内来回摆动，在静态不动的时候，它就沿直线运动。但是当它有旋转的时候，因为科里奥利力的存在，它的摆动就会画弧线而不是走直线了。这时，支撑着这个摆的四个构件，就会检测到一个扭动的力，这个扭动力量的大小，就反应了当前角速度的大小。通过这种方式来测量角速度。
核心就是惯性-------即保持原来的运动状态不变。进动性----不想改变转轴，我要反抗这个旋转。

1.2.5 加速度计

当运载体相对惯性空间做加速度运动时，仪表壳体也随之做相对运动，质量块保持惯性，朝着与加速度方向相反的方向产生位移（拉伸或压缩弹簧）。当位移量达到一定值时，弹簧给出的力使质量块以同一加速度相对惯性空间做加速运动，加速度的大小与方向影响质量块相对位移的方向及拉伸量。（在静止不动时，这个东西自己是不摆的，但在给它一个加速度的时候，由于惯性的作用，会产生一个反向的摆动。通过检测摆动的大小，就知道了加速度的大小。）

2. 惯性器件误差分析

2.1 信号误差组成

1. 量化噪声
   一切量化操作所固有的噪声，是数字传感器必然出现的噪声；
   产生原因：通过AD采集把连续时间信号采集成离散信号的过程中，精度会损失，精度损失的大小和AD转换的步长有关，步长越小，量化噪声越小。(AD：模拟信号，这个比较好理解，连续信号采集成离散信号，比较采集是有频率的)
   

2. 角度随机游走
   宽带角速率白噪声：陀螺输出角速率是含噪声的，该噪声中的白噪声成分；
   产生原因：计算姿态的本质是对角速率做积分，这必然会对噪声也做了积分。白噪声的积分并不是白噪声，而是一个马尔可夫过程，即当前时刻的误差是在上一时刻误差的基础上累加一个随机白噪声得到的。
   角度误差中所含的马尔可夫性质的误差，称为角度随机游走。(测量出的角速率是有一个白噪声的，因为我们要利用角速率解算，积分成角度，这时的白噪声也包含在积分里面了，那不就算的不准了)

3. 角速率随机游走
   与角度随机游走类似，角速率误差中所含的马尔可夫性质的误差，称为角速率随机游走。而这个马尔可夫性质的误差是由宽带角加速率白噪声累积的结果。(角加速率里面也是有一个白噪声的)

4. 零偏不稳定性噪声
   零偏：即常说的bias，一般不是一个固定参数，而是在一定范围内缓慢随机飘移。
   零偏不稳定性：零偏随时间缓慢变化，其变化值无法预估，需要假定一个概率区间描述它有多大的可能性落在这个区间内。时间越长，区间越大。
   (这玩意好像不是很好描述，但是在工程上，我们需要给它一个近似的模型)

5. 速率斜坡
   该误差是趋势性误差，而不是随机误差。
   随机误差，是指你无法用确定性模型去拟合并消除它，最多只能用概率模型去描述它，这样得到的预测结果也是概率性质的。
   趋势性误差，是可以直接拟合消除的，在陀螺里产生这种误差最常见的原因是温度引起零位变化，可以通过温补来消除。
   (一个角速度，在不考虑之前说的那些随机误差的影响下，那误差就是一个稳定的白噪声。现在它随着时间变化，会有一个趋势性，也就是误差也在飘，但是这个误差是趋势性的。理论上讲这个误差不能叫完全的随机误差，因为趋势性很多时候是有些确定性的模型来影响的，比如说温补------随着温度的升高，误差会变大，这个模型是可以拟合的。但是在随机信号分析的时候，温补的误差是可以分析出来的。)

6. 零偏重复性
   多次启动时，零偏不相等，因此会有一个重复性误差。在实际使用中，需要每次上电都重新估计一次。(意思是，不同次启动时，零偏会发生多大的变化。与前面的所有误差是有区别的，前五项一次充电采集很多信号就可以分析出来。而零偏重复性就需要反复上电断电上电断电，统计一下每次上电断电，这个bias发生了多少变化)
   Allan方差分析时，不包含对零偏重复性的分析。(即Allan方差只能分析前五项，不能分析第六项)

2.2 Allan方差分析

随机信号Allan方差的物理意义及应用在本质上来源于它与功率谱之间的关系。
功率谱（功率谱密度函数）：单位频带内的信号功率，它表示信号功率随着频率的变化情况，即信号功率在频域的分布状况。

假设把随机过程 $X_{\alpha }$ 的功率谱表示为：$\mathrm{PSD}\left[X_{\alpha }\right]=h_{\alpha }{f}^{\alpha }$ 其中 $f^{\alpha }$ 是频率， $h_n$ 为相应的系数。(频率和功率之间的关系)

若多个随机过程相互独立，则其满足线性相加性质，即 $X=\sum X_a$ 此时，其功率谱也同样可以线性相加 $\mathrm{PSD}[X]=\sum \mathrm{PSD}\left[X_{\alpha }\right]$(多个功率谱是可以叠加的)

• Allan方差分析方法的基本思路：
  在惯性器件随机误差分析中，以上提到的5种误差相互独立，且 $\alpha$ 值不同，因此若绘制“时间间隔-方差双对数曲线”(时间间隔是频率的倒数，方差是功率谱的积分)，则得到的曲线斜率必不相同。

  根据曲线斜率识别出各项误差，并计算出对应的误差强度。

(公式知道就行了，这时信号分析的东西，不属于融合范畴)
量化噪声 $Q$ 满足下式
$${\log }_{10}{\sigma }_{QN}(\tau )={\log }_{10}(\sqrt{3}Q)-{\log }_{10}\tau$$角度随机游走 $N$ 满足下式
$${\log }_{10}{\sigma }_{ARW}(\tau )={\log }_{10}N-1/2\ast {\log }_{10}\tau$$角速率游走 $K$ 满足下式
$${\log }_{10}{\sigma }_{RRW}(\tau )={\log }_{10}(K/\sqrt{3})+1/2\ast {\log }_{10}\tau$$零偏不稳定性 $B$ 满足下式
$${\log }_{10}{\sigma }_{BI}(\tau )={\log }_{10}(2B/3)$$速率斜坡 $R$ 满足下式
$${\log }_{10}{\sigma }_{RR}(\tau )={\log }_{10}(R/\sqrt{2})+{\log }_{10}\tau$$以上公式中，$\tau$ 为时间间隔。

根据以上公式分析，可知曲线的形状如下：

即各随机噪声对应的斜率分别为 -1, -1/2, 0, 1/2, 1(即上面的$-{\log }_{10}\tau$、$-1/2\ast {\log }_{10}\tau$…)，找到曲线后，方程就可以求出来了(也就是、、、、)

同时，令 $\tau =1$ ，则 ${\log }_{10}(\tau )=0$
其含义是求曲线与 $\tau =1$ 的交点，此时有：

噪声类型	与 $\tau$ 的交点
量化噪声	$\sqrt{3}Q$
角度随机游走	$N$
角速率游走	$K/\sqrt{3}$
零偏不稳定性	$2B/3$
速率斜坡	$R/\sqrt{2}$

此时可容易地求出 、、、、

现在来说说怎么使用这些误差：
开源代码：https://github.com/gaowenliang/imu_utils

• P1：五个误差全用么？
  不全用，这五个误差更多的是，再生产器件时，知道以哪几个误差为主，来改进生产。但假如是用户，我现在的主要目标是做融合，那么再实际过程中五项误差不全是需要关心的了。
  主要关心的有以下两个：

  用法(以陀螺仪为例)：

  1. 角度随机游走，在融合时作为陀螺仪的噪声使用。(有时也以零偏不稳定性当做噪声，因为很多手册里面都不提供角度随机游走，而零偏不稳定性是必须会给的)，对应的是卡尔曼滤波里面的 $Q$
  2. 角速度随机游走，作为陀螺仪微分项中的噪声。(详细内容在介绍融合时介绍)

注意：
1）其他误差项，仅起到了解器件精度水平的作用；
2）实际融合时，Allan分析的结果，只是作为初值使用，需要在此基础上调参。

单个IMU三个陀螺仪的Allan方差曲线

零偏重复性：之前的几个误差都被计算出来了，但是可以看到所有的模型都在估计一个bias。那么为什么需要估计一个bias出来而不能提前估计出来一直用呢？就是因为零偏重复性的问题，它在每一次上电的时候都不一样。假如我的器件，零偏不稳定性是5°/h，那么它的零偏重复性可能达到500°/h，也就是说bias上面有一个每小时500°的值，而这个值是事先测不出来的，这么大的误差，必然是需要在线估计的。(这概念挺重要)

3. 惯性器件内参标定

内参标定是惯性器件里面非常核心的一个环节，虽然大家在实践过程中可能不需要做这个东西，因为买一个IMU模块，大多数情况下都给标好了。但是这方面的技能还是需要掌握的。

3.1 惯性器件内参误差模型

3.1.1 零偏

误差解释： 陀螺仪或加速度计输出中的常值偏移，即常说的 bias。(注意陀螺仪和加速度计都有一个)
误差特性： 由于零偏存在不稳定性，因此零偏并不是固定不变的。
解决办法： 实际使用中，只能一段时间内近似为常值。
(在多数情况下可以认为是一个常值，在前面说误差分析时，说零偏有随机游走、每次上电还有零偏重复性这类的问题。但是不管怎样，在一次通电的很短的时间内，可以将它认为是一个常值来用，这样可以简化模型，不把模型搞得太复杂。)

加速度计的零偏表示为：
$$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]$$

3.1.2 刻度系数误差

误差解释： 器件的输出往往为脉冲值或模数转换得到的值，需要乘以一个刻度系数才能转换成角速度或加速度值，若该系数不准，便存在刻度系数误差。
误差特性： 不一定是常值，它会随着输入大小的不同而发生变化，这个就是标度因数的非线性。
解决办法： 如果非线性程度比较大，则需要在标定之前先拟合该非线性曲线，并补偿成线性再去做标定。
(假设有$x,y,z$三个轴，绕$z$轴旋转 10°/s，在实际测量的时候，会发现$z$输出的角速度不是 10°/s，可能是 9.9°/s，也就是说上面有一个 0.1°/s 的误差。这时在旋转的时候，要在上面乘上一个比例因子，这个比例因子就是$K=\frac{10}{9.9}$，最后补偿过的角速度才是真实的角速度结果。每个轴上都有一个这样比例系数的补偿因子，所以建成的是一个对角矩阵的形式，与 [ x y z ] \left[\begin{array}{lll} x \\ y \\ z \end{array}\right] ​xyz​ ​相乘。)

关于拟合：输入 10°/s 时得到一个角速度输出，20°/s、30°/s 时都能得到一个角速度输出值。但是比例因子不一定是相同的，在 10°/s 时有一个数，在 20°/s 时可能就是另外一个数了。这里有两种方法处理：

1. 10°/s、20°/s、30°/s 均得到一个点，拟合出一个二次曲线。这样是可以的，但是很多时候标定不是单参数的标定-----它与bias和安装误差都是耦合在一起的，也就是说拟合的结果里面有其他误差的耦合系数。
2. 另一种办法是，仍然是拟合，但是拟合的时候只拟合它的趋势项，比如说它二次曲线的弯曲程度是什么样子的。相当于是补偿这个二次曲线后，变成一个一次。这个一次曲线和真实的一次曲线可能有一个偏移量的差异，这个没有关系。只有它变成了一次曲线，相当于是仍然变成了一个常值。也就是说，我们把二次曲线，先补偿一下变成一次曲线，然后我们再去标一次曲线的平移量就可以了。(工程中是比较复杂的，但实际中，经常就当一次曲线搞就行了。绝大多数情况还是一个线性的，如果出现非线性，再按照这个方法来)

加速度计的标度因数，表示如下
K a = [ K a x K a y K a z ] K_a=\left[\begin{array}{lll} K_{a x} & & \\ & K_{a y} & \\ & & K_{a z} \end{array}\right] Ka​= ​Kax​​Kay​​Kaz​​ ​陀螺仪的标度因数，表示为
K g = [ K g x K g y K g z ] K_g=\left[\begin{array}{lll} K_{g x} & & \\ & K_{g y} & \\ & & K_{g z} \end{array}\right] Kg​= ​Kgx​​Kgy​​Kgz​​ ​

3.1.3 安装误差

误差解释： 如下图所示，b坐标系是正交的imu坐标系，g坐标系的三个轴是分别对应三个陀螺仪。由于加工工艺原因，陀螺仪的三个轴并不正交，而且和b坐标系的轴不重合，二者之间的偏差即为安装误差。
误差特性： 实际系统中，由于硬件结构受温度影响，安装误差也会随温度发生变化。
解决办法： 在不同温度下做标定，补偿温度变化量。
(比方说绕$z$轴旋转，理论上其他轴是没有角速度输出的。但是有一个问题，如果$x$与$z$轴不是正交的，比如正交是$90°$，而它俩是$89.9°$，会造成一个现象，即绕$z$旋转的时候，$x$上就有了角速度输出了，因为它上面因为不完全垂直而有分量了。)

因为$z$在$x$、$y$方向上有误差；因为$y$在$x$、$z$方向上有误差；因为$x$在$y$、$z$方向上有误差，总共有六个误差量。
陀螺仪的安装误差，表示如下
S g = [ 0 S g x y S g x z S g y x 0 S g y z S g z x S g z y 0 ] S_g=\left[\begin{array}{ccc} 0 & S_{g x y} & S_{g x z} \\ S_{g y x} & 0 & S_{g y z} \\ S_{g z x} & S_{g z y} & 0 \end{array}\right] Sg​= ​0Sgyx​Sgzx​​Sgxy​0Sgzy​​Sgxz​Sgyz​0​ ​加速度计的安装误差，表示为
S a = [ 0 S a x y S a x z S a y x 0 S a y z S a z x S a z y 0 ] S_a=\left[\begin{array}{ccc} 0 & S_{a x y} & S_{a x z} \\ S_{a y x} & 0 & S_{a y z} \\ S_{a z x} & S_{a z y} & 0 \end{array}\right] Sa​= ​0Sayx​Sazx​​Saxy​0Sazy​​Saxz​Sayz​0​ ​

3.1.4 惯性器件内参误差模型

利用下面公式(以陀螺仪为例)，可以把各项误差综合在一起：
$$W=K_g\left(I+S_g\right)\omega +b_g\approx \left(K_g+S_g\right)\omega +b_g$$$\omega$为角速度输入，即想求得的理想角速度，真实输入；$W$是角速度的输出，也就是直接在器件上面测量出来的结果。可以看到 $K_g$ 和 $S_g$ 乘在一起了，耦合在一起就很难解耦，这个模型标定的时候就很难标定了，所以要做一个化简，就得到了最右边的样子。核心原因在于$K_g=I+\Delta K_g$，而 $\Delta K_g$ 是一个很小的量，$(I+\Delta K_g)(I+S_g)=I+\Delta K_g+S_g+\Delta K_g{S}_g$，因为$S_g$也是个小量，$\Delta K_g{S}_g$ 可以被忽略。

陀螺仪的输出可以展开为:
[ W x W y W z ] = [ K g x S g x y S g x z S g y x K g y S g y z S g z x S g z y K g z ] [ ω x ω y ω z ] + [ b g x b g y b g z ] \left[\begin{array}{l} W_x \\ W_y \\ W_z \end{array}\right]=\left[\begin{array}{lll} K_{g x} & S_{g x y} & S_{g x z} \\ S_{g y x} & K_{g y} & S_{g y z} \\ S_{g z x} & S_{g z y} & K_{g z} \end{array}\right]\left[\begin{array}{c} \omega_x \\ \omega_y \\ \omega_z \end{array}\right]+\left[\begin{array}{l} b_{g x} \\ b_{g y} \\ b_{g z} \end{array}\right] ​Wx​Wy​Wz​​ ​= ​Kgx​Sgyx​Sgzx​​Sgxy​Kgy​Sgzy​​Sgxz​Sgyz​Kgz​​ ​ ​ωx​ωy​ωz​​ ​+ ​bgx​bgy​bgz​​ ​加速度计的输出可以展开为:
[ A x A y A z ] = [ K a x S a x y S a x z S a y x K a y S a y z S a z x S a z y K a z ] [ a x a y a z ] + [ b a x b a y b g z ] \left[\begin{array}{c} A_x \\ A_y \\ A_z \end{array}\right]=\left[\begin{array}{lll} K_{a x} & S_{a x y} & S_{a x z} \\ S_{a y x} & K_{a y} & S_{a y z} \\ S_{a z x} & S_{a z y} & K_{a z} \end{array}\right]\left[\begin{array}{l} a_x \\ a_y \\ a_z \end{array}\right]+\left[\begin{array}{l} b_{a x} \\ b_{a y} \\ b_{g z} \end{array}\right] ​Ax​Ay​Az​​ ​= ​Kax​Sayx​Sazx​​Saxy​Kay​Sazy​​Saxz​Sayz​Kaz​​ ​ ​ax​ay​az​​ ​+ ​bax​bay​bgz​​ ​

3.2 惯性器件内参误差标定

3.2.1 标定方法概述

标定的本质是参数辨识，参数包括陀螺仪和加速度计各自的零偏、刻度系数误差、安装误差。
辨识方法包括：
1）解析法或最小二乘
2）迭代优化方法
3）滤波（Kalman等）
常见标定方法与上面辨识方法的对应关系为：

1. 基于转台的标定: 解析法、最小二乘；
2. 不需要转台的标定: 梯度下降迭代优化；
3. 系统级标定：kalman滤波(该方法只适用于高精度惯导，而MEMS本身就属于精度不高的一类，这里不做讲解)。

3.2.2 基于转台的标定

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

以加速度计为例，其误差模型方程为:
[ A x A y A z ] = [ K a x S a x y S a x z S a y x K a y S a y z S a z x S a z y K a z ] [ a x a y a z ] + [ b a x b a y b a z ] \left[\begin{array}{c} A_x \\ A_y \\ A_z \end{array}\right]=\left[\begin{array}{lll} K_{a x} & S_{a x y} & S_{a x z} \\ S_{a y x} & K_{a y} & S_{a y z} \\ S_{a z x} & S_{a z y} & K_{a z} \end{array}\right]\left[\begin{array}{l} a_x \\ a_y \\ a_z \end{array}\right]+\left[\begin{array}{l} b_{a x} \\ b_{a y} \\ b_{a z} \end{array}\right] ​Ax​Ay​Az​​ ​= ​Kax​Sayx​Sazx​​Saxy​Kay​Sazy​​Saxz​Sayz​Kaz​​ ​ ​ax​ay​az​​ ​+ ​bax​bay​baz​​ ​左边$A_x,A_y,A_z$是测量出来的，肯定是已知的；$a_x,a_y,a_z$是输入，标定时也是已知的。误差模型方程是一个包含12个未知参数的方程组，显然方程组没有唯一解。此时，通过改变输入，获得多个不同方程 (大于12个)，组成的方程组便可求解参数。

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

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

构建方程组时，不仅要方程组数量足够，而且要能够使误差参数可解，即系数矩阵可逆。

为了满足这一点，常见的转位方案有六位置、八位置、十二位置等。

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

3.2.2.1 加速度计标定

3.2.2.1.1 解析法：

当IMU水平向上放置时，得:
$$\{\begin{array}{l}{a}_x=0\\ a_y=0\\ a_z=g\end{array}$$其中， $\mathrm{g}$ 为重力加速度。带入加速度计误差模型，
[ A x A y A z ] = [ K a x S a x y S a x z S a y x K a y S a y z S a z x S a z y K a z ] [ a x a y a z ] + [ b a x b a y b a z ] \left[\begin{array}{c} A_x \\ A_y \\ A_z \end{array}\right]=\left[\begin{array}{lll} K_{a x} & S_{a x y} & S_{a x z} \\ S_{a y x} & K_{a y} & S_{a y z} \\ S_{a z x} & S_{a z y} & K_{a z} \end{array}\right]\left[\begin{array}{l} a_x \\ a_y \\ a_z \end{array}\right]+\left[\begin{array}{l} b_{a x} \\ b_{a y} \\ b_{a z} \end{array}\right] ​Ax​Ay​Az​​ ​= ​Kax​Sayx​Sazx​​Saxy​Kay​Sazy​​Saxz​Sayz​Kaz​​ ​ ​ax​ay​az​​ ​+ ​bax​bay​baz​​ ​可得：
$$\{\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^{\prime }=-S_{axz}\ast g+b_{ax}\\ A_y^{\prime }=-S_{ayz}\ast g+b_{ay}\\ A_z^{\prime }=-K_{az}\ast g+b_{az}\end{array}$$联立这两个方程组，便可解出 6 个参数。随后，再次改变 IMU放置方式，可解其他参数。
并且，由此可以看出，转台需要调平。

3.2.2.1.2 最小二乘法：

加速度计误差模型：
[ A x A y A z ] = [ K a x S a x y S a x z S a y x K a y S a y z S a z x S a z y K a z ] [ a x a y a z ] + [ b a x b a y b a z ] \left[\begin{array}{c} A_x \\ A_y \\ A_z \end{array}\right]=\left[\begin{array}{lll} K_{a x} & S_{a x y} & S_{a x z} \\ S_{a y x} & K_{a y} & S_{a y z} \\ S_{a z x} & S_{a z y} & K_{a z} \end{array}\right]\left[\begin{array}{l} a_x \\ a_y \\ a_z \end{array}\right]+\left[\begin{array}{l} b_{a x} \\ b_{a y} \\ b_{a z} \end{array}\right] ​Ax​Ay​Az​​ ​= ​Kax​Sayx​Sazx​​Saxy​Kay​Sazy​​Saxz​Sayz​Kaz​​ ​ ​ax​ay​az​​ ​+ ​bax​bay​baz​​ ​可得到：
[ A x A y A z ] = x [ K a x K a y K a z S a x y S a x z S a y x S a y z S a z x S a z y ∇ x ∇ y ∇ z ] → y = x θ \left[\begin{array}{c} A_x \\ A_y \\ A_z \end{array}\right]=x\left[\begin{array}{c} K_{a x} \\ K_{a y} \\ K_{a z} \\ S_{a x y} \\ S_{a x z} \\ S_{a y x} \\ S_{a y z} \\ S_{a z x} \\ S_{a z y} \\ \nabla_x \\ \nabla_y \\ \nabla_z \end{array}\right] \quad \rightarrow y=x \theta ​Ax​Ay​Az​​ ​=x ​Kax​Kay​Kaz​Saxy​Saxz​Sayx​Sayz​Sazx​Sazy​∇x​∇y​∇z​​ ​→y=xθ其中：
x = [ F I 3 × 3 ] F = [ 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 ] \begin{aligned} 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] \end{aligned} xF​=[F​I3×3​​]= ​ax​00​0ay​0​00az​​ay​00​az​00​0ax​0​0az​0​00ax​​00ay​​ ​​转台在每个位置都可以得到一个方程：$y_i=x_i\theta$

所有位置对应的方程联立可得：$Y=X\theta$

其中 $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$

经过变形，标定问题变为线性拟合问题，当第$i$次把IMU朝不同方向放置后，便得到一个方程组。

参数拟合问题等效为最小二乘问题，其解为：$\theta ={\left(X^{T}X\right)}^{-1}{X}^{T}Y$(最小二乘通解)，由此便得到标定参数。

3.2.2.2 陀螺仪标定：

3.2.2.2.1 方法思想

转台一般角速度不如角度精度高，因此不是直接以角速度作为真值，而是以积分得到的角度作为真值。

3.2.2.2.2 解析法

求解刻度系数和安装误差：
首先，计算输出与输入的关系(以绕IMU的$z$轴逆时针旋转为例)
[ W x W y W z ] = [ K g x S g x y S g x z S g y x K g y S g y z S g z x S g z y K g z ] [ 0 0 ω ] + [ b g x b g y b g z ] \left[\begin{array}{l} W_x \\ W_y \\ W_z \end{array}\right]=\left[\begin{array}{lll} K_{g x} & S_{g x y} & S_{g x z} \\ S_{g y x} & K_{g y} & S_{g y z} \\ S_{g z x} & S_{g z y} & K_{g z} \end{array}\right]\left[\begin{array}{c} 0 \\ 0 \\ \omega \end{array}\right]+\left[\begin{array}{c} b_{g x} \\ b_{g y} \\ b_{g z} \end{array}\right] ​Wx​Wy​Wz​​ ​= ​Kgx​Sgyx​Sgzx​​Sgxy​Kgy​Sgzy​​Sgxz​Sgyz​Kgz​​ ​ ​00ω​ ​+ ​bgx​bgy​bgz​​ ​展开可得
$$\{\begin{array}{l}{W}_x=S_{gxz}\ast \omega +b_{gx}\\ W_y=S_{gyz}\ast \omega +b_{gy}\\ W_z=K_{gz}\ast \omega +b_{gz}\end{array}$$对等式两侧进行积分
$$\{\begin{array}{l}{\theta }_{W_x}=S_{gxz}\ast {\theta }_{\omega }+{\theta }_{b_{gx}}\\ {\theta }_{W_y}=S_{gyz}\ast {\theta }_{\omega }+{\theta }_{b_{gy}}\\ {\theta }_{W_z}=K_{gz}\ast {\theta }_{\omega }+{\theta }_{b_{gz}}\end{array}$$绕IMU的z轴顺时针旋转时，同样方法可得，
$$\{\begin{array}{l}{\theta }_{W_x}^{\prime }=-S_{gxz}\ast {\theta }_{\omega }+{\theta }_{b_{gx}}\\ {\theta }_{W_y}^{\prime }=-S_{gyz}\ast {\theta }_{\omega }+{\theta }_{b_{gy}}\\ {\theta }_{W_z}^{\prime }=-K_{gz}\ast {\theta }_{\omega }+{\theta }_{b_{gz}}\end{array}$$
正逆时针的两式可以求解出 $S_{gxz}{S}_{gyz}{K}_{qz}$
此处不通过两式求解零偏，因为旋转所用时间偏短，零偏造成的角度输出太小。

求解零偏：
当转台静止时，可以简单认为陀螺仪输出只有零偏，即(单纯放置着静止不动即可)：
[ W x W y W z ] = [ b g x b g y b g z ] \left[\begin{array}{l} W_x \\ W_y \\ W_z \end{array}\right]=\left[\begin{array}{l} b_{g x} \\ b_{g y} \\ b_{g z} \end{array}\right] ​Wx​Wy​Wz​​ ​= ​bgx​bgy​bgz​​ ​此时采集一段时间内的数据，取平均值，即可得到零偏。
需要强调的是:
a. 有时标定需要考虑地球自转角速度的影响，此时模型比较复杂，可自行参考《惯性仪器测试与数据分析》第 10 章。
b. mems 陀螺仪的零偏重复性极差，因此每次上电都要在线估计零偏，因此离线标定时，零偏标与不标区别不大。

3.2.3 不需要转台的标定

使用转台的方法很好理解、精度也很高，但是问题在于转台很贵，有很多时候是没有转台的。这个时候输入是没有了的----没有很明确的三个轴的输入，但是还是有一个值可以用的，就是重力加速度。也就是说三个加速度计测量出的矢量的和的模，它的值必须和重力加速度值相等，这时候的加速度计才被认为是没有误差的，否则认为是有误差的。

3.2.3.1 整体思路

加速度输入(重力加速度)是已知的，已知值与测量值的差异作为残差，通过优化，估计内参。
参考资料：
论文：A Robust and Easy to Implement Method for IMU Calibration without External Equipments

3.2.3.2 内参模型

在定义坐标系时，若令IMU坐标系的 $X_b$ 轴与加速度计的 $X_a$ 轴重合，且 $X_{b}O{Y}_b$ 与 $X_{a}O{Y}_a$ 共面(如下图)(这里说的IMU坐标系是标准的坐标系意思？)，则加速度计的安装误差只剩下三个参数。(按照定义方式不同，有些模型中会表示成上三角矩阵)
新的安装误差矩阵为 S a = [ 0 0 0 S a y x 0 0 S a z x S a z y 0 ] S_a=\left[\begin{array}{ccc}0 & 0 & 0 \\ S_{a y x} & 0 & 0 \\ S_{a z x} & S_{a z y} & 0\end{array}\right] Sa​= ​0Sayx​Sazx​​00Sazy​​000​ ​
此时，加速度计内参的待估参数为
$${\mathbf{\theta }}^{acc}=\left[S_{ayx},S_{azx},S_{azy},K_{ax},K_{ay},K_{az},b_{gx},b_{gy},b_{gz}\right]$$陀螺仪的误差模型保持不变，但此处并没有估计陀螺仪零偏，因此陀螺仪内参的待估参数为(因为坐标系定的是和IMU坐标系一致，不能说再定义一个陀螺仪的)
$${\mathbf{\theta }}^{gyro}=\left[S_{gxy},S_{gxz},S_{gyx},S_{gyz},S_{gzx},S_{gzy},K_{gx},K_{gy},K_{gz}\right]$$

3.2.3.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 ′ = [ K a x ′ K a y ′ K a z ′ ] = K a − 1 = [ 1 K a x 1 K a y 1 K a z ] K_a^{\prime}=\left[\begin{array}{lll} K_{a x}^{\prime} & & \\ & K_{a y}^{\prime} & \\ & & K_{a z}^{\prime} \end{array}\right]=K_a^{-1}=\left[\begin{array}{ccc} \frac{1}{K_{a x}} & & \\ & \frac{1}{K_{a y}} & \\ & & \frac{1}{K_{a z}} \end{array}\right] Ka′​= ​Kax′​​Kay′​​Kaz′​​ ​=Ka−1​= ​Kax​1​​Kay​1​​Kaz​1​​ ​$$\begin{array}{rl}& {\left(I+S_a\right)}^{-1}\approx I-S_a\\ & a=(\begin{array}{ll}I& S_a)\end{array}{K}_a^{\prime }\left(\begin{array}{ll}\Lambda & b_a\end{array}\right)\end{array}$$当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$$根据高斯牛顿的流程，有此残差函数便可以推导雅可比 (推导留作作业)，通过优化求解出内参。

需要注意的是，一个静止位置的测量不能完全求解出参 数，需要按不同的姿态，在多个位置静止(如右图黑色曲 线所包含的时间段)，所有位置的测量放在同一个优化任 务中，才能求解全部参数。

3.2.3.4 优化模型–估计陀螺仪内参

陀螺仪内参估计在加速度计标定完成后进行，因此认为此时加速度计无误差。
令 ${\mathbf{u}}_{a,k}$ 代表在第 $\mathrm{k}$ 个静止位置时，三个加速度计的输出构成的矢量在IMU坐标系下的表示，即
$${\mathbf{u}}_{a,k}=R_{b_{k}w}\mathbf{g}$$其中 $R_{b_{k}w}$ 表示从世界坐标系(w系，和水平面平行且不随IMU旋转而旋转的坐标系)到第 $\mathrm{k}$ 个位置对应的IMU坐标系的转换矩阵。
注意: 并不需要已知 $R_{b_{k}w}$ ，因为 ${\mathbf{u}}_{a,k}$ 是直接测量的。

在第 $k+1$ 个位置时，同样有
$${\mathbf{u}}_{a,k+1}=R_{b_{k+1}w}\mathbf{g}$$从第 $\mathrm{k}$ 个位置，到第 $\mathrm{k}+1$ 个位置，可以根据陀螺仪测量计算出两个位置之间的相对旋转 $R_{b_{k+1}}{b}_k$，根据该旋转可以算出一个第 $\mathrm{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解算知识，本章不作要求。

3.2.3.4 标定方法比较

1. 基于转台的标定精度较高，但标定成本高；
2. 不依赖转台的标定精度差，但成本低、效率高，对一般MEMS的标定需求已经足够。

4. 惯性器件温补

现在来说一说惯性器件的温补， 温补的影响很大，但是很多时候不需要自己做，因为厂商给做好了，买了厂商的东西，输出结果自然是给补偿好的。

温补的本质是系统辨识，既要找出合适的物理模型，又要识别物理模型的参数。(说白了就是不同温度能得到不同的bias，问题就在于，怎样得到这个bias)

理论上讲，对于MEMS，静止情况下，陀螺的输出本身就是bias，这里不做区分。只是说在高精度的情况下，静止情况下的输出还包含别的因素，所以它不等于bias，但是对于低精度来讲无所谓，可以当成bias来看待。

4.1 物理模型辨识

和温度相关的变量为 $T$ 和 $\Delta T$ ，分别代表温度、变温率(表示温度变化的快慢)(注意，不仅和温度点相关，还跟温度变化的速度是相关的)，温补要做的是识别出器件 bias(B)和这两者的关系：
$$B=f(T,\Delta T)$$而$f$的具体表达多数是靠尝试。

常见的模型为
$$\begin{array}{rl}B=& a\ast T^3+b\ast T^2+c\ast T+\\ & e\ast \Delta T^2+f\ast \Delta T+\\ & g\ast T\ast \Delta T+h\end{array}$$实际使用时，可根据情况在此基础上做减法，去掉一些高阶项。(曲线是二次三次四次都有可能)

4.2 参数辨识

在选定的物理模型基础上做最小二乘曲线拟合，与分立级标定时所用最小二乘原理相同。

4.3 其他改进方法

1. 分段拟合
   bias随温度变化曲线多是不规则曲线，无法用一个完整的曲线模型做拟合。
   常见的方法是按照温度把曲线分成多个区间，每个区间单独拟合一个模型。

2. 基于神经网络
   温补最大的问题是物理模型未知，而神经网络不需要已知物理模型，理论上比较合理。
   但是实际使用中，由于很多处理器只是简单的嵌入式板子，运算能力有限，而且多项式方法已经能解决大部分问题，因此这种方法在实际使用中用的不多。(文献有，实际用的不多)

4.4 关于温补的讨论

1. 温补在器件误差补偿中是最重要的，但也是最“没有技术含量”的。
2. 温变的本质是和器件整体温度场相关，而不只是和局部温度点相关，但温度传感器只能测量后者。
3. 永远无法找到完全准确的温补模型，但是却能知道什么是够用的。自动控制领域有一句名言 “All models are wrong, but some are useful“。