imu_tk标定算法原理

imu_tk代码地址

https://bitbucket.org/alberto_pretto/imu_tk

II. S ENSOR E RROR M ODEL

对于理想的IMU，加速度计三元组的3个轴和陀螺仪三元组的3个轴定义单个共享的正交3D帧。 每个加速度计检测沿一个不同轴的加速度，而每个陀螺仪测量围绕同一轴的角速度。 不幸的是，在实际IMU中，由于组装不准确，两个三元组形成两个不同的（即，未对准的）非正交的帧。 此外，单个传感器并不完美：通常，用于以实际物理量转换传感器的数字输出的比例因子对于相同传感器的不同实例是不同的，而制造商仅提供默认的标称比例因子。 此外，输出信号几乎总是受非零可变偏差的影响。

如上所述，加速度计帧（AF）和陀螺仪帧（GF）通常都是非正交的。 我们可以通过以下方式定义两个相关的正交理想帧（分别为AOF和GOF）：

•AOF的x轴和AF的x轴重合
•AOF的y轴位于AF的x轴和y轴所跨越的平面中。

对于陀螺仪情况，分别用GF和GOF替换AF和AOF缩写词就足够了。 最后，我们定义一个主体框架（BF），它是一个正交框架，表示例如IMU底盘的坐标系。 车身框架通常与AF和GF框架的角度不同，但通常它们之间没有直接关系。 对于小角度，非正交帧（AF或GF）中的测量s S可以在正交体帧中变换为（对于推导的细节，参见[18]）：

其中s B和s S分别表示车身坐标和加速度计（或陀螺仪）坐标中的特定力（加速度）或等效的旋转速度。 这里βij是围绕第j个BF轴的第i个加速度计或陀螺仪轴的旋转，参见图2。

另一方面，两个正交帧BF和AOF（以及等效地，BF和GOF）通过纯旋转相关。

在所提出的校准方法中，我们假设车身框架BF与加速度计正交框架AOF一致：在这种情况下，角度βxz，βxy，βyx变为零，因此在加速度计情况下，等式1变为：

我们改变字母β，参考一般情况，字母α，指加速度计情况，而O和S分别表示AOF和AF中的特定加速度3。

如前所述，陀螺仪和加速度计测量应参考相同的参考系，在我们的例子中是AOF。 然后，使用Eq 1，对于陀螺仪，我们有：

其中ωO和ωS分别表示AOF和GF中的特定角速度。

加速度计和陀螺仪都受到偏差和尺度误差的影响。 引入了两个缩放矩阵

我们还介绍了两个偏向量

完整的传感器误差模型是

对于加速度计

对于陀螺仪，其中νg和νg分别是加速度计测量噪声和陀螺仪测量噪声。

III. B ASIC C ALIBRATION F RAMEWORK

为了校准加速度计三元组，我们需要估计以下未知参数向量：

我们定义以下函数：

在这里，我们可以忽略测量噪声，因为在我们的校准过程中，我们在每个静态间隔中应用信号平均。

与传统的多位置方案一样，我们将IMU移动到一组M个不同的，暂时稳定的旋转中。 我们可以提取M个加速度矢量Sk（在非正交AF中测量），在每个静态间隔内的时间窗口中对加速度计读数求平均。 我们用来估算加速度计参数的成本函数是：

其中|| g || 是可以从特定公共表格中容易地恢复的局部重力矢量的实际大小（例如，知道我们正在执行校准的位置的纬度，经度和高度）。 为了最小化Eq 10，我们采用Levenberg-Marquardt（LM）算法。

为了校准陀螺仪三元组，我们可以假设系统无偏置，在适当的无运动初始阶段简单地平均静态陀螺仪信号。 以下关于艾伦方差的讨论证明了这一点（参见第IV-C节）。 此外，由于我们需要使用加速度计作为已知参考，我们使用上面计算的校准参数θacc，用等式9校正加速度计读数。

我们定义运算符Ψ，它将n个陀螺仪读数的序列ωSi和由校准的加速度计给出的初始重力值ua，k-1（即表示重力方向的单位矢量）作为输入，并返回最终的重力 或者ug，k，使用第k-1和第k个静态区间之间的陀螺仪测量值计算：

Ψ可以是通过积分输入角速度来计算最终方向的任何积分算法。我们需要估计以校准陀螺仪的未知参数矢量是：

在这种情况下，我们可以将成本函数定义为：

其中M是静态区间的数量，ua，k是在时间窗口中对第k个静态区间中的校准加速度计读数进行平均测量的加速度，并且ug，k是使用等式11计算的加速度对数（即，积分 第k-1和第k个静态区间之间的角速度）。 我们获得θ陀螺，用LM最小化Eq 13。

IV. C ALIBRATION P ROCEDURE

如第二节所述。 I，所提出的校准框架需要收集具有原始加速度计和陀螺仪读数的流的数据集，当操作者在不同的静态位置移动IMU时获取该数据集，以便产生一组不同的，暂时稳定的旋转。 我们的校准协议的简单图表如图1所示。 III，为了减轻Eq 10和Eq 13最小化时的噪声影响，我们需要在适当的时间间隔内对信号求平均值。 这对静态间隔的长度施加了下限（图1中的t等待）。没有运动的初始化周期（图1中的T init）也是必不可少的：这将被用来表征陀螺仪的偏差（ 第IV-C节）和静态探测器操作员（第IV-A节）。

A. Static Detector

校准的准确性很大程度上取决于静态和运动间隔之间分类的可靠性：校准加速度计我们使用静态间隔，而对于陀螺仪校准，我们还包括两个连续静态间隔之间的运动间隔。根据我们的经验， 传递基于滤波器的算子，如[8]中使用的准静态检测器，对真实数据集表现不佳：检测到的静态间隔通常包括一小部分运动。 而且，它们需要微调，因为它们取决于三个参数。

我们建议使用基于方差的静态检测器算子，它利用上面介绍的静态区间长度的下限。 我们将探测器基于加速度计信号：给定时间间隔长度为t秒（参见图1），对于每个加速度计样本（tx，a ty，tz），在时间t，我们计算方差幅度，即 方差的大小如下：

其中var t w（a t）是在以t为中心的长度为t w秒的时间间隔内计算一般信号a t的方差的算子。 我们在静态和运动间隔之间进行分类，只需检查ζ（t）的平方是否低于或大于阈值。

作为阈值，我们考虑在所有初始化时段T init上计算的方差幅度ζinit的平方的整数倍。 在所有实验中，我们使用t w = 2秒，而使用Allan方差估计T init（参见第IV-C节）。 值得注意的是，我们的静态探测器不需要任何参数调整：分类中使用的整数乘数由我们的校准算法自动估算（参见第IV-D节）。 图3报告了我们的静态滤波器如何处理实际数据的示例：在这种情况下，估计的整数乘数为6。

B. Runge-Kutta Integration

D. Complete Procedure

为了避免校准参数估计中的不可观察性，必须收集至少九种不同的高度[15]（例如，图1）。 根据我们的经验，需要更多N个不同的高度来获得更好的校准结果，同时保持减少每个静态间隔的持续时间，以保持陀螺仪偏差的时间稳定性的假设。当36≤N≤50且1秒≤t等于≤4秒时，我们在校准精度，偏差稳定性和降噪之间取得了良好的平衡。

初始化时段T init的持续时间由Allan方差分析给出（参见第IV-C节）。 校准协议总结在图1中，而在算法1中，报告校准算法的伪代码。