自动驾驶融合定位：IMU内参模型及标定

一、概述

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

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

辨识就比较丰富了，如果让各位先不局限于标定任务，想一想你了解的辨识方法有哪些，常见的回答应该有这样几个：

1）解析法或最小二乘

2）滤波（kalman等）

3）梯度下降迭代优化

确实没错，标定里用的就是这些方法。这说明标定其实就是一个普通的参数辨识问题，它和你遇到的其他参数辨识任务比，并没有特殊在哪里。

本篇文章我们先分析标定的误差参数、误差模型，然后介绍两种标定方法，一种是基于转台的方法，使用解析法或最小二乘法进行参数辨识，另一种是不转台的方法，使用迭代优化的方法进行参数辨识。

基于滤波的方法我们并不打算讲，一是因为从参数辨识的精度讲，滤波和优化比，还是有比较大的劣势，另一个是我过去所基础的基于滤波的标定，都是高精度惯性导航里面使用的，这类方法在自动驾驶或机器人里所用的这种精度的IMU上，并不能很好地work。当然，现在也有一些基于kalman的slam系统里在标内参，那就是另外一个事情了，在这里我们不做讨论。

附赠自动驾驶学习资料和量产经验：链接

二、标定参数分析

1.参数项

1) 零偏

这个比较好理解，就是输出比输入多了一个常值误差。

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

加速度计的零偏在这里表示为

陀螺仪的零偏在这里表示为

2) 标度因数误差

也叫刻度因数误差。假设器件输出的是标准单位角速度(rad/s)，那么输出和输入的比就是1。如果不是，就得需要标定，修正这个比例。

3) 安装误差

一图胜千言，上图吧。

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

这样一共有12项安装误差参数。有的时候，可以简化为9项，具体什么情况下简化，以及怎样简化，我们会在本文的后面讲。

2. 误差模型

三、标定方法

1. 基于转台的标定

1.1 标定原理

在说标定之前，我们先看几个简单的小问题。

第一个问题，假如给你下面的方程组，你会怎么解

应该都能想到，用2式减去1式，得到y=1，然后带入1式，得到x=0。

第二个问题，下面的式子里，你可以随意给定a和b的值，并同时会得到c的结果，这个x和y怎么解。

也很简单，首先a=1,b=0,得到x，然后把a=0,b=1得到y。

第三个问题，下面式子，同样是输入a和b，得到c1和c2，怎么解。

其实就是前两个问题的结合，先让a=0，转成问题1，得到x和y，随后让a=1，得到z。

第四个问题，给你下面的式子，可以随意更改ax、ay和az，怎么解。

想必凭各位的冰雪聪明，一定知道我在说什么了。到这里，虽然还没讲标定，但你已经懂标定了。标定就是通过改变输入，来构建方程组，去解这个方程。

1.2 参数辨识方法

其实标定的过程就是在不停构建方程的过程，这里构建方程使用的就是转台（如下图）。

1) 加速度计内参标定

当IMU固定在转台上以后，通过翻滚转台框架，停在不同的位置（所谓不同位置，指的是转台的内外框架处在不同的角度时，IMU的姿态不一样，下图给出的是一个十二位置的标定方案示意图），就可以得到不同的加速度输入（IMU姿态不同，重力在IMU上的投影不同），从而构建方程组，去求解加速度计的内参。

a. 解析法

b. 最小二乘法

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

对于加速度计误差模型，我们可以转换一下形式，写成

其中

这便是待求的标定参数。

c. 讨论

通过以上两种标定方法的介绍，可以看出，这里默认前提是转台是经过调平的，所谓调平，指的是转台水平面与地球水平面之间是平行的。调平的目的是，直接得到重力在IMU上的投影（各位可以很容易地想到，若两个水平面不平行，当IMU水平向上放置时，三个加速度计的输入就不是0,0,g了）。调平的方法也比较简单，使用水平仪做基准，去调整转台即可。当然，不依赖调平的方法也有，而且很普遍，只是方法复杂一些，我们在这里不做介绍，相信从此处讲的一个简单方法入手，再去查此类文献，你会很快弄懂那些相对高级的方法。

2) 陀螺仪内参标定

其实，明白了加速度计的内参标定方法以后，陀螺仪的内参标定方法就很容易想到了。让转台提供真实输入，通过改变真实输入，得到不同的方程组，最后用解析法或者最小二乘法去辨识参数。

这里需要说明的一点是，陀螺仪的输入是角速度，但是转台一般角速度不如角度精度高，因此不是直接以角速度作为真值，而是以积分得到的角度作为真值。

a. 解析法

以绕IMU的z轴逆时针旋转为例，计算得到输出与输入的关系为

展开并忽略二阶小量（指的是，我们通常认为零偏、安装误差、标度因数误差都是小量，任何两个小量相乘，都认为是二阶小量，小到可以直接忽略），可得

b. 最小二乘法

对于最小二乘法，经过加速度计中解析法和最小二乘法之间联系的了解，相信各位已经明白在陀螺仪标定中该怎么用这种方法了，此处就不啰嗦了。

c. 讨论

在这里，我们并没有考虑地球自转角速度（简称地速），它的大小约为15°/h，当我们要标定的陀螺仪的精度小于这个水平很多的时候，那么不考虑也无所谓，反之，就必须考虑了。一个比较简单的方法是，找到转台坐标系与地球坐标系之间的转换关系，这样，当转台处在任意一个位置的时候，地速在IMU上的投影都是已知的，在输入和输出中直接剪掉它的影响即可，这种方法叫转台对准。

各位可以理解一下转台对准和转台调平之间得联系。调平是为了找到IMU和重力之间得联系，对准是为了找到IMU和地速之间得联系，而重力和地速都是地理系下天然存在的输入。因此，可以概括为，调平和对准都是为了找到转台和地理系之间得转换关系。

但是，对准并不像调平那么简单，调平使用一个水平仪即可，而对准往往需要借助全站仪来操作，流程很复杂。实际上，陀螺仪的标定方法中，也有可以通过联立更多位置，把未知的地速投影直接抵消掉的方法，这样就不需要转台对准了。与加速度计标定的讲解思路类似，此处只是借助讲解简单情况下的方法，让大家理解原理和过程，至于这些相对高级的方法，仍然希望大家通过阅读文献去自己扩展。

2. 不依赖转台的标定

1.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

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

1.2 内参模型

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

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

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

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

在规定坐标系时，若令IMU坐标系(b系)的 𝑋𝑏 轴与加速度计的 𝑋𝑎 轴重合，且 𝑋𝑏𝑂𝑌𝑏 与 𝑋𝑎𝑂𝑌𝑎 共面(如下图)

则此时，加速度计的安装误差只剩下三个参数

另外，当满足这种关系时，b系就已经完全固定了，因此此时陀螺仪的三个轴和b系之间的安装误差仍为 6 个。

1.3 标定方法

1）加速度计标定

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

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

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

具体停留几个位置，以及各个位置怎样放置，答案并不唯一，各位可以自己思考一下这里面的问题，看看至少几个位置可以把参数求解出来。下图给出的是一种方案，根据其中加速度计的测量值（这里的测量值并不直接是 𝑚/𝑠2 为单位，而是乘了一个比例系数，可以直接认为最大值对应的就是 𝑔0=9.8𝑚/𝑠2 ，然后简单换算一下）。