基于PE-ANGO 的MIMU 现场标定方法

乔美英，姚文豪，高柯飞，杜 衡，赵开东

（1.河南理工大学 电气工程与自动化学院，焦作 454003；2.河南省智能装备直驱技术与控制国际联合实验室，焦作 454003）

近年来随着微机电系统（Micro Electro Mechanical System,MEMS）的快速发展，微惯性测量单元（Miniature Inertial Measurement Unit,MIMU）因低成本、小尺寸、功耗低和可靠性高等优点被广泛应用在运动检测及生物医学等领域[1-3]，然而此类传感器通常受到各种误差的影响[4]，需要进行不定期的标定。传统的标定方法是使用高精度设备提供的可靠基准信息来完成[5]，不满足低成本传感器的使用需求，且标定步骤通常较为繁琐，不利于非专业人员操作。因此，需要研究一种标定精度高、标定步骤简单的传感器现场快速标定方法。

国内外已有学者对现场标定方法做了大量研究。针对加速度计现场标定，其依据的事实是：理想情况下，加速度计测量的比力矢量等于当地重力加速度矢量。如Shokri-Ghaleh 等人[6]提出使用不等极限布谷鸟优化算法获取加速度计误差参数，该方法不需要外部设备辅助。Qureshi 等人[7]从误差模型中推导出代价函数方程，使用牛顿迭代法估算加速度计误差参数。该方法迭代速度快，然而对初值有着严格的要求且容易陷入局部最优。Hassan 等人[8]采用广义非线性最小二乘法估计加速度计误差参数，提高了收敛速度，但数据采集时间较长。乔美英等人[9]通过改进布谷鸟优化算法，提高了加速度计标定的精度和速度，然而其数据采集依赖双轴转台。低成本陀螺仪因无法检测地球自转速度[10]，使得其标定基准很难确定。部分标定方法仍需要借助匀角速度罗盘[11]等类似设备为陀螺仪提供基准，从而建立与加速度计标定类似的目标函数，并使用优化算法求得误差参数。此外，使用优化算法虽然无需求解目标函数的导数信息，在标定精度上表现较好，但其较为依赖初始值的质量，质量较差的初始解可能会影响算法的寻优性能，甚至使算法陷入局部最优。

本文在上述研究的基础上，提出一种无需使用任何辅助设备的MIMU 现场标定方法。相比于传统方法，本文综合考虑了常见误差（比例因子误差、非正交误差和零偏）以及传感器之间的非对准误差。具体实现过程为：首先对加速度计和陀螺仪建立误差模型，模型包含了常见误差以及非对准误差。然后推导了用于加速度计和陀螺仪现场标定的目标函数，并采用基于先验知识增强的自适应北苍鹰优化（Prior Knowledge Enhancement Adaptive North Goshawk Optimization,PE-ANGO）算法进行参数寻优。为了进一步方便MIMU 现场标定，本文引入了手持MIMU采集数据的方法，重新设计了静态检测器的检测阈值、检测周期和初始静止时间，通过手持MIMU 采集36次静态位置信息，即可实现加速度计和陀螺仪的现场标定。

1 MIMU 误差模型

1.1 加速度计误差模型

静止状态下，加速度计无论处于何种姿态，其测量矢量应与当地重力矢量完全一致。基于这一事实，加速度计现场标定可以在不借助任何设备辅助的情况下完成。加速度计标定通常先建立加速度计的测量模型，接着利用当地重力矢量推导出标定加速度计参数的目标约束函数，从而将加速度计标定问题转化为多参数寻优问题。

加速度计测量时通常受到多种误差源影响，其中最重要的是比例因子误差、轴间非正交误差以及零偏误差[4]。因此，加速度计测量模型可以建模为：

其中，ma=[maxmaymaz]T为加速度计测量值；ua=[uaxuayuaz]T为加速度计测量的真实值；表示比例因子误差；ba=[baxbaybaz]T表示加速度计零偏误差；非正交误差Ka可以转化为上三角矩阵或下三角矩阵[12]，表达式如式(2)所示。

其中，θyz、θzy、θzx分别表示加速度计三个测量轴的非正交偏差。

一般的标定方法是基于静止时加速度计测量的比力矢量与当地重力加速度一致的事实进行的。基于多位置法标定的思想，通过采集N组测量数据并最小化式(3)所示的目标函数J(a)，从而得到误差参数[6]。

其中，G=[GaxGayGaz]T表示当地重力矢量。

总目标函数中待求解的变量共9 个，将其整理成向量的形式，如式(4)所示。

于是，加速度计的标定问题就转化为求J(a)的最小化问题。

1.2 陀螺仪误差模型

陀螺仪独立标定时的一般误差模型可以表示为：

其中，mg=[mgxmgymgz]T表示陀螺仪测量值；ug=[ugxugyugz]T表示真实的角速度；Kg表示陀螺仪测量坐标系的非正交误差矩阵；Sg表示陀螺仪测量轴的比例因子误差矩阵，是一个三维对角矩阵；bg=[bgxbgybgz]T表示陀螺仪处于静止状态时测量轴的输出偏差，即零偏。

陀螺仪的独立标定无法解决传感器之间的非对准误差，该误差会造成导航定位误差累积，使得最终结果偏离真实轨迹。因此，将非对准误差考虑进陀螺仪误差模型：

其中，R表示陀螺仪测量坐标系与加速度计坐标系之间的非对准误差矩阵，其本质上是一个旋转矩阵。

为了计算方便，将式(6)改写为式(7)形式：

于是，陀螺仪误差模型中待标定项为ER和bg，共12 个标定参数。

陀螺仪的标定分两步进行，首先对陀螺仪的零偏误差进行估计。静止情况下，陀螺仪的真实值应满足式(9)所示的关系。

其中，Nb表示静置期间采集的数据总数。

利用式(10)可以将bg从陀螺仪测量模型中去除，接着处理误差矩阵ER。由于加速度计和陀螺仪都可以测量载体姿态变化，且理想情况下，加速度计和陀螺仪测量的载体姿态变化应完全一致[13]。依据这一事实，可以分析移动一次MIMU 前后加速度计和陀螺仪测量单位重力矢量的关系。假设单位重力矢量在加速度计测量坐标系的初始位置为，与之对应的，单位重力矢量在陀螺仪测量坐标系的初始位置为，需要注意的是是用已标定加速度计测量得到的。通过将MIMU 旋转一定角度，将会得到下一个静止状态的单位重力矢量位置。参考Chao 等人[10]的方法，由加速度计测量得到的下一个静止状态单位重力矢量位置为：

传统描述陀螺仪坐标系旋转的方法是使用顺序欧拉角旋转，比如假设陀螺仪旋转时先绕x轴旋转，再绕y轴旋转，最后绕z轴旋转。这种旋转次序由人为主观确定，在传感器测量运动期间会引入系统误差。为了提高陀螺仪测量结果表征的精度，本文引入同步正交旋转角度矢量（Simultaneous Orthogonal Rotation Angle）方法[14]表示陀螺仪的旋转。

其中，Φ示同步正交旋转角度矢量；Tt表示陀螺仪采样间隔；Ax、Ay、Az分别表示陀螺仪绕x、y、z旋转的角度。

假设Φ的旋转轴v在采样间隔Tt内保持恒定，则其旋转轴v和模值φ可用来表示陀螺仪实际旋转轴和角度，即：

利用Φ的旋转轴v和模值φ，可以建立陀螺仪本征坐标系中与该旋转相关的旋转矩阵：

其中，c和s分别代表cos(φ)和sin(φ)。

由陀螺仪计算旋转之后的单位矢量位置为：

综上所述，可以建立陀螺仪标定的目标函数J(g)为：

其中，Ns表示静态位置总数。

与加速度计标定相同，将陀螺仪待标定参数组成一个向量：

由此，可以将陀螺仪标定问题转化为求目标函数J(g)最小化参数问题。

2 先验知识增强的北苍鹰优化算法

优化问题的求解方法通常可分为确定性方法和随机方法。确定性方法基于目标函数的导数信息进行优化问题的求解，能够精确求解最优线性或凸非线性问题，然而对于复杂问题的求解容易给出局部最优解。基于随机的优化算法在解决多参数优化问题时通常能给出较为精确的结果，但算法结果常受限于初始值质量以及全局搜索和局部搜索之间的平衡。北苍鹰优化算法由Dehghani 等人于2021 年提出，该算法对北方苍鹰狩猎行为进行数学建模，并将狩猎过程分为猎物识别与攻击阶段（全局搜索阶段）和猎物逃逸与追逐阶段（局部搜索阶段）。北苍鹰优化算法相较于其他传统优化算法，如粒子群优化算法（Particle Swarm Optimization,PSO）、遗传算法（Genetic Algorithm,GA）、鲸鱼优化算法（Whale Optimization Algorithm,WOA）等，具有参数少、迭代速度快和精确度高的优点[15]。

本文对传统北苍鹰优化（North Goshawk Optimization,NGO）算法进行改进，提出了基于先验知识增强的自适应北苍鹰优化算法（PE-ANGO）。首先针对初始值问题，提出使用先验知识预测最优解位置，并在最优解附近模拟爆炸现象产生算法初始种群；然后针对算法全局搜索与局部搜索平衡问题，通过改进自适应参数R增强算法局部搜索的能力。

先验知识的获取来源于传感器上次标定的结果或传感器技术手册。通过计算一阶后向差分预测此次标定最优解的位置，并模拟爆炸现象在预测的最优个体位置附近产生初始种群。这种方法可以集中算法注意力去搜索最有潜力的区域，具体做法为：假设此次正在进行第k次标定，将k-1次的最优个体位置与k-2次的最优个体位置进行一阶后向差分，利用所得差分矢量补偿第k-1次标定得到的最优个体位置，从而预测第k次最优个体位置。以此位置为中心，模拟爆炸现象生成第k次标定的初始种群。此做法可以理解为北苍鹰种群除随机选取猎物之外，还会学习猎物移动的趋势，从而提前预测猎物移动方向和位置，极大地提高了下次狩猎的成功几率与速度。

该方法将之前标定获得的最优个体位置作为先验知识，并利用第k-1次与k-2次最优个体位置构造一阶后向差分方程，如式(21)所示。

其中，ΔX表示前两次标定最优个体位置的差分矢量；Xk-1,best、Xk-2,best分别表示第k-1次和第k-2次标定的最优个体位置。

若无k-2次最优个体位置，则令Xk-2,best=Xk-1,best。将ΔX补偿到第k-1次最优个体位置，以此预测第k次最优个体位置，预测公式如式(22)所示。

图1 爆炸效果图Fig.1 The explosion rendering

爆炸现象基于正态分布进行模拟，利用MATLAB得到符合正态分布的一组随机数：

其中，aver代表正态分布的均值，即预测最优个体的某一维度的值。σ表示正态分布的标准差，该值决定了爆炸的范围，该值越大，则爆炸范围越大，粒子落在远处的几率就大，反之就小。X为m×n维符合正态分布的随机数矩阵。

以二维坐标系原点(0,0)为中心，模拟爆炸现象在x、y轴的[ -1 1]内生成200 个二维数据点，观察不同标准差对数据点分布的影响，如图2 所示。

图2 不同标准差对数据点分布的影响Fig.2 Effect of different standard deviation on the distribution of data points

在实际应用中，标准差σ的选取与信任先验知识的程度有关，越信任先验知识，该值选取越小，即尽可能集中在预测的最优个体位置附近生成初始种群，相反则应取值大一点。

此外，为了平衡算法全局搜索与局部搜索的关系，改进了原算法中参数R的自适应取值。NGO 中参数R的大小将影响北苍鹰的捕猎行为。R越大，种群个体的搜索半径就越大，有利于快速搜索问题解空间中较优区域，使算法具有较好的跳出局部最优的能力。然而在算法后期，较大的搜索半径会拖延算法的收敛速度，甚至出现算法发散的情况。因此，参数R的值应在算法前期尽可能取大一些以保证算法全局寻优能力，在算法中后期应取小一些以使算法在较优区域做更精细的搜索。改进的自适应因子R为：

其中，t表示当前迭代次数；T表示最大迭代次数；w表示自适应因子的衰减程度，即衰减因子。

原算法中，参数R以线性函数形式调整，难以平衡算法的全局搜索和局部搜索阶段。本算法结合原算法优秀的全局搜索能力，增大局部搜索阶段在算法迭代过程中的占比，平衡算法全局与局部搜索，提高了算法的收敛速度和寻优精度。

3 基于手持MIMU 数据采集的标定过程

3.1 手持MIMU 数据采集方式

为了使标定步骤简单快速、易于操作，本文引入了手持MIMU 进行数据采集的方法。2014 年，Tedaldi等[16]提出了确定静态检测阈值、静止周期和初始静止时间的方法，使用的静态检测算子c(t)如式(25)所示。

在静态位置检测期间，确定c(t)是否小于检测阈值。如果c(t)小于检测阈值，则认为加速度计处于静止位置；否则，加速度计被认为是移动的。检测阈值应在现场确定，以减少不稳定因素可能导致阈值设置不合适，从而影响数据采集效率。另外，设置静止周期用于采集传感器在准静态位置一段时间内的测量值，并将该时段内测量值的平均值作为该位置的数据点，这种做法可以降低传感器的噪声干扰。静止周期也应通过实验确定。本文得到合适的检测阈值为1.74×10-4g，静止周期为0.4 s。初始静止时间用于确定陀螺仪零偏误差，使用Allan 方差分析法确定了初始静止时间为18 s。

针对准静态位置的选取，给出了以下细节：首先为了防止计算过程中方程出现奇异性，在考虑不理想位置数据点的情况下，测量位置应严格不低于待标定参数的个数。此外，为了方便非专业人员操作，本方法并不限制特定的静态位置次序，但需要注意的是，为了降低陀螺仪测量噪声影响，在旋转MIMU 时应尽量使角度大一点，且旋转速度不宜过快。最后，由于传感器的数学模型忽略了耦合误差，所以静态位置的选择应尽可能对称，以减少这些误差的影响。移动过程除上述之外没有其他要求。

在实验中，为了获得更多样化的测量数据，静态位置应尽可能覆盖多个方向。IMU 需要移动和暂停至少18 次，这是待校准参数数量的两倍。本文在空间中将IMU 视为正方体，并选取了正方体6 个平面和12个边作为静态位置，并重复一次采集步骤，共采集了36 个静态位置数据。这种做法可以减少异常值的影响，同时覆盖尽可能多的方向。

3.2 标定步骤

基于上述MIMU 数据采集方法，可以方便地进行加速度计和陀螺仪数据采集。数据采集基于实验室自主设计的MIMU 核心板，通过MIMU 核心板上的LED闪烁提示用户成功完成一次静态数据的采集。

具体的标定步骤实施如下：

步骤一：手持MIMU 按3.1 所述36 个静态位置移动，每个静态位置数据采集成功后MIMU 核心板会通过LED 闪烁提示用户，此时可以移动到下一个静态位置进行数据采集。将3.1 所述36 个静态位置采集完成之后进行标定工作；

步骤二：进行加速度计的标定工作，基于1.1 节提出的加速度计目标函数，并使用PE-ANGO 进行参数寻优可以快速得到加速度计误差参数；

步骤三：使用第二步得到的加速度计误差参数补偿加速度计输出数据，并按照1.2 节所述方法标定陀螺仪；

步骤四：得到所有误差参数，并补偿传感器输出。

4 实验验证

4.1 基于PE-ANGO 的加速度计标定仿真实验

为了验证所提算法的性能，以加速度计标定为例设计了仿真实验。仿真环境为MATLAB R2021b，实验选取单位球体上1000 个点作为不同位置下的重力矢量，以此模拟当地重力加速度即真实值，并按照说明书提供的参考误差参数（如表1 所示）进行设定。

表1 加速度计参考误差参数（由说明书提供）Tab.1 Reference error parameters of accelerometer(provided by the manual)

假设加速度计实际误差参数与说明书提供的参考误差参数相差不大，实际误差参数的设定如表2 所示。

表2 加速度计实际误差参数Tab.2 Actual error parameters of accelerometer

将表1设定为第k-1次标定误差参数，结合式(21)和式(22)，由于第k-2次误差参数不可获取，因此令Xk-1,Best=Xk-2,Best，此时预测得到=Xk-1,Best，模拟爆炸现象在预测得到的最优个体附近产生算法初始种群，PE-ANGO 算法初始参数设置如表3 所示。

表3 PE-ANGO 初始参数设置Tab.3 Initial parameter setting for PE-ANGO

为了对比NGO 和PE-ANGO 的收敛速度，设定相同的收敛精度，分别使用两种算法进行仿真标定实验，两种算法的迭代次数与目标函数值变化如图3 所示。

图3 NGO 和PE-ANGO 迭代曲线对比Fig.3 Comparison of NGO and PE-ANGO iteration curves

由图3 可知，使用PE-ANGO 标定时，算法初始种群目标函数值极低，极大地增强了算法的收敛速度，这得益于使用先验知识预测最优个体位置并模拟爆炸现象产生初始种群的策略。由于第k次标定的最优个体位置已被预测，且仿真时加速度计处于理想环境，因此，σ值取的较小。在实际应用中，如果传感器长时间处于恶劣环境下，此时应降低对预测值的信任，选择较大的σ值以增强算法在爆炸外围的搜索能力。

为了对比NGO 和PE-ANGO 的鲁棒性及稳定性，设定相同的最大迭代次数，分别使用两种算法进行50次仿真标定实验，两种算法每次仿真标定完成所需的迭代次数如图4 所示。

图4 NGO 和PE-ANGO 标定50 次仿真结果Fig.4 Simulation results of NGO and PE-ANGO calibration for 50 times

在50 次仿真实验中，分别使用NGO 和PE-ANGO标定所需平均迭代次数和标准差（Standard Deviation,SD）如表4 所示。

表4 相同收敛精度下NGO 与PE-ANGO 的50 次仿真统计结果Tab.4 Simulation statistical results of NGO and PE-ANGO with the same convergence accuracy for 50 times

设定迭代次数为400 次的条件下，再进行50 次NGO 和PE-ANGO 的标定仿真实验，对比两种算法的收敛精度，标定仿真实验的结果如表5 所示。均方根误差（Root Mean Square Error,RMSE）说明了PE-ANGO算法可以精确地标定出加速度计误差参数。此外，通过标准差也可以看出PE-ANGO 的鲁棒性优于NGO。与NGO 算法相比，PE-ANGO 算法的标定精度提高了一个数量级。

表5 相同迭代次数下50 次仿真标定实验结果Tab.5 Experimental results of simulation calibration with the same number of iterations for 50 times

4.2 实测实验

为了验证本文方法的有效性，设计了MIMU 实测实验。实验使用基于MPU9250 的自主设计MIMU 核心板进行数据采集，如图5 所示。室内温度24 ℃。

图5 MIMU 核心板Fig.5 MIMU core circuit board

加速度计18 个静态位置数据点的分布情况如图6所示，其包含了正方体的6 个平面和12 个边的数据。

然后将MIMU 按照第3 节所述方法进行标定，静态检测阈值设置为1.74×10-4g，静止周期设置为0.4 s，初始静止时间设置为18 s。利用标定得到的误差参数补偿加速度计数据点，并绘制补偿后数据点的分布情况，如图7 所示。

图7 加速度计校准后数据点分布Fig.7 Data point distribution after accelerometer calibration

为了验证本文方法对陀螺仪标定的有效性，另外采集一组MIMU 测量数据，通过对补偿前后陀螺仪测量的角速度进行积分，估计出横滚角和俯仰角。除此之外，使用文献[5]提出的优化多位置标定方法标定MIMU 并估计的横滚角和俯仰角与本文方法标定MIMU 并估计的横滚角和俯仰角进行对比。图8 展示了三种方法估计的横滚角和俯仰角。

图8 基于三种方法的姿态估计对比Fig.8 Comparison of attitude estimation based on three methods

关于MIMU 运动情况说明如下：

1）在开始的15 s 内，将MIMU 水平静置。此时可以观察到三种方法估计的横滚角和俯仰角都很接近参考值。

2）在15～25 s 内，将MIMU 拿起并旋转，此时姿态误差开始累积；

3）在25～35 s 内，将MIMU 恢复原状并静置，可以观察到采用本文方法补偿后的横滚角和俯仰角更接近初始位置，表明本文方法对降低陀螺仪误差是有效的。表6 给出了三种方法最终累积的姿态偏差。由表6可以看出，采用本文方法对MIMU 标定后，与未校准方法相比，俯仰角累积误差的抑制效果提高了约89%，横滚角累积误差的抑制效果提高了约87%。与文献[5]提出的优化多位置标定方法相比，本文方法对俯仰角和横滚角累积误差的抑制效果分别提高了71%和68%。

表6 三种方法的姿态偏差Tab.6 Three methods of attitude deviation

5 结论

本文提出了一种对MIMU 现场标定的新方法。首先，为了解决传统矢量约束目标函数导致的方向性误差，建立了标量约束目标函数。为了增强智能优化算法初始解的质量，提出了一种基于PE-ANGO 算法用于目标函数的多参数寻优。此外，为了便于非专业人员进行MIMU 现场标定工作，引入了手持MIMU 进行传感器数据采集。为了验证本文方法的性能，分别进行了仿真和实验。

仿真结果表明，利用本文算法可以有效获取较为精确的传感器误差参数，其精度满足低成本IMU 的使用。通过统计数据分析证明了PE-ANGO 相较于NGO不仅有收敛速度和精度的优势，其稳定性和鲁棒性也优于NGO。实际测量实验验证了本文方法的有效性，与传统标定方法对比，本文方法对俯仰角和横滚角累积误差的抑制效果分别提高了71%和68%，表明本文方法可以在不借助复杂设备的前提下标定加速度计误差参数，能够替代传统方法应用于传感器现场标定。

然而，基于智能优化算法的标定技术目前仍处于探索阶段，未来将继续专注传感器误差标定，争取实现惯性传感器标定的在线应用。