一种基于九轴IMU的姿态解算方法、装置及存储介质

摘要

本申请提供一种基于九轴IMU的姿态解算方法、装置及存储介质，姿态解算方法包括利用九轴IMU的陀螺仪、加速度计和磁力计输出的原始数据，标定三个内部模块的内部参数，借助标定内部参数，将原始数据转化为标定数据；对陀螺仪和加速度计标定数据进行数据融合，输出IMU的初步姿态数据；在基于磁修正时刻对应周期和姿态修正时刻对应周期的磁力和姿态数据修正算法中，利用重力矢量和权重值最高的磁场矢量修正陀螺仪漂移，输出IMU的最终姿态数据。通过长短周期的磁力和姿态数据的互相校正来对IMU姿态进行修正，抑制磁场干扰对姿态解算的不良影响，实现IMU姿态的准确、稳定输出，满足在室内复杂磁场环境中对IMU器件的应用需求。

说明书

技术领域

本申请涉及导航定位技术领域，具体而言，涉及一种基于九轴IMU的姿态解算方法、装置及存储介质。

背景技术

随着信息技术和经济建设的发展，交通运输、工业、农业、测绘等领域对导航技术提出了更高的要求，通过IMU姿态解算方法，对九轴IMU（Inertial measurement unit，惯性测量单元）输出的加速度计（Acc）、陀螺仪（Gyro）和磁力计（Mag）的原始数据进行综合分析计算，使算法稳定地向上位机输出IMU器件三自由度旋转量的姿态数据，可以广泛应用于导航定位等领域。

传统的姿态解算方法，是借助于陀螺仪的三轴角速度数据，通过计算可以得到IMU的实时姿态信息。但是，单纯依靠累积运算，长期下来会有一定漂移，导致姿态解算结果有偏差。因此，对于九轴IMU器件来说，需要借助加速度计和磁力计来定期或不定期地进行姿态校正，以期望输出准确的姿态数据。

但是真实的室内场景具有复杂的磁场环境，IMU器件的磁力计容易受其干扰影响，当磁力计输出不准确的数据时，将明显影响传统的姿态解算方法，受干扰严重时，将直接导致校正失败，姿态数据错误。

发明内容

有鉴于此，本申请的目的在于提供一种基于九轴IMU的姿态解算方法、装置及存储介质，能够通过长短周期磁力和姿态数据的互相校正来对IMU姿态进行修正，抑制磁场干扰对姿态解算的不良影响，进而实现了IMU姿态的准确、稳定输出，满足了在室内复杂磁场环境中对IMU器件的应用需求。

第一方面，本申请提供了一种基于九轴IMU的姿态解算方法，所述姿态解算方法包括：

基于获取到的九轴IMU的陀螺仪、加速度计和磁力计的原始数据，分别得到经过标定后的陀螺仪标定数据、加速度计标定数据和磁力计标定数据；

对得到的陀螺仪标定数据和加速度计标定数据进行数据融合，得到初步姿态数据；

确定一个表征磁场时间统计和空间统计的磁场矢量-权重值集合；

将加速度计标定数据和磁力计标定数据转换成用于矢量描述的加速度矢量和磁场矢量；

当到达预设的磁力修正时刻时，根据所述初步姿态数据和当前磁力修正时刻获得的磁场矢量对所述磁场矢量-权重值集合进行修正，得到修正后的磁场矢量-权重值集合；

当到达预设的姿态修正时刻时，根据修正后的磁场矢量-权重值集合中权重值最高的磁场矢量以及当前处于静止状态的加速度计的加速度矢量，对所述初步姿态数据进行修正，得到最终姿态数据，其中，所述姿态修正时刻对应周期是所述磁力修正时刻对应周期的整数倍。

优选地，所述对得到的陀螺仪标定数据和加速度计标定数据进行数据融合，得到初步姿态数据，包括：

根据上一采样时刻已计算得到的初步姿态数据，结合获得的当前采样时刻陀螺仪标定数据，计算得到当前采样时刻的候选初步姿态数据；

若所述九轴IMU处于非静止状态，则基于所述候选初步姿态数据对所述初步姿态数据进行更新，得到更新后的初步姿态数据；

若所述九轴IMU处于静止状态，由当前采样时刻的加速度计标定数据确定在IMU初始坐标系下的当前采样时刻重力矢量；

根据在所述IMU初始坐标系下预先确定出的初始重力矢量，确定所述当前采样时刻重力矢量和所述初始重力矢量之间的误差矢量；

根据所述候选初步姿态数据以及所述误差矢量，采用数据融合算法，计算得到一个融合后初步姿态数据；基于所述融合后初步姿态数据对所述初步姿态数据进行更新，得到更新后的初步姿态数据；其中，所述IMU初始坐标系为九轴IMU的陀螺仪、加速度计和磁力计经过标定后的初始时刻确定出的坐标系。

优选地，通过以下步骤确定所述磁场矢量-权重值集合：

基于每个初始磁场矢量以及该初始磁场矢量对应的权重值，得到多个初始集合元素；

当新增磁场矢量与任意一个初始磁场矢量的差值在预设圆锥范围内时，确定与该新增磁场矢量对应的初始磁场矢量的累计权重值；

当新增磁场矢量与任意一个初始磁场矢量的差值不在预设圆锥范围内时，确定该新增磁场矢量以及该新增磁场矢量对应的权重值为新增集合元素；

基于与该新增磁场矢量对应的初始磁场矢量的累计权重值，或所述新增集合元素，更新所述磁场矢量-权重值集合。

优选地，通过以下步骤确定所述磁场矢量-权重值集合：

当检测到所述磁场矢量-权重值集合中所有集合元素的元素个数大于设定个数阈值时，删除部分目标元素；

其中，将所有集合元素按照对应的权重值从小到大的顺序排序，从最小权重值开始，选取目标数量个集合元素组成部分目标元素，所述目标数量为所有集合元素的元素个数与设定个数阈值之间的数量差值，所述集合元素包括初始集合元素，或所述初始集合元素和所述新增集合元素之和。

优选地，通过以下公式确定所述磁场矢量-权重值集合：

其中，P表示磁场矢量-权重值集合，

优选地，所述当到达预设的磁力修正时刻时，根据所述初步姿态数据和当前磁力修正时刻获得的磁场矢量，对所述磁场矢量-权重值集合进行修正，得到修正后的磁场矢量-权重值集合包括：

在九轴IMU处于静止状态下，确定所述磁场矢量-权重值集合中最大的权重值对应的磁场矢量；

基于当前磁力修正时刻下所述磁场矢量以及所述初步姿态数据中的陀螺仪的姿态四元数，确定IMU坐标系下的磁场矢量；

将所述IMU坐标系下的磁场矢量与所述磁场矢量-权重值集合中最大的权重值对应的磁场矢量进行比较，得到当前磁力修正时刻所述磁力计检测到的磁场方向与稳定磁场方向之间的角度差值；

若所述角度差值在预设角度差范围内时，将当前磁力修正时刻IMU坐标系下的磁场矢量添加到所述磁场矢量-权重值集合中；

若所述角度差值不在预设角度差范围内时，则不将当前磁力修正时刻IMU坐标系下的磁场矢量添加到所述磁场矢量-权重值集合中。

优选地，所述当到达预设的姿态修正时刻时，根据修正后的磁场矢量-权重值集合中权重值最高的磁场矢量以及当前处于静止状态的加速度计的加速度矢量，对所述初步姿态数据进行修正，得到最终姿态数据，包括：

在九轴IMU处于静止状态下，确定所述磁场矢量-权重值集合中最大的权重值对应的磁场矢量；

基于最大的权重值对应的磁场矢量和当前姿态修正时刻由所述加速度计确定出的加速度矢量，更新所述九轴IMU的姿态四元数；

确定更新后的九轴IMU的姿态四元数为最终姿态数据。

第二方面，本申请实施例提供一种基于九轴IMU的姿态解算装置，所述姿态解算装置包括：

内参标定模块，用于基于获取到的九轴IMU的陀螺仪、加速度计和磁力计的原始数据，分别得到经过标定后的陀螺仪标定数据、加速度计标定数据和磁力计标定数据；

数据处理模块，用于对得到的陀螺仪标定数据和加速度计标定数据进行数据融合，得到初步姿态数据；

集合确定模块，用于确定一个表征磁场时间统计和空间统计的磁场矢量-权重值集合；

数据转换模块，用于将加速度计标定数据和磁力计标定数据转换成用于矢量描述的加速度矢量和磁场矢量；

第一修正模块，用于当到达预设的磁力修正时刻时，根据所述初步姿态数据和当前磁力修正时刻获得的磁场矢量对所述磁场矢量-权重值集合进行修正，得到修正后的磁场矢量-权重值集合；

第二修正模块，用于当到达预设的姿态修正时刻时，根据修正后的磁场矢量-权重值集合中权重值最高的磁场矢量以及当前处于静止状态的加速度计的加速度矢量，对所述初步姿态数据进行修正，得到最终姿态数据，其中，所述姿态修正时刻对应周期是所述磁力修正时刻对应周期的整数倍。

第三方面，本申请还提供一种电子设备，包括：处理器、存储器和总线，所述存储器存储有所述处理器可执行的机器可读指令，当电子设备运行时，所述处理器与所述存储器之间通过总线通信，所述机器可读指令被所述处理器执行时执行如上述基于九轴IMU的姿态解算方法的步骤。

第四方面，本申请还提供一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器运行时执行如上述基于九轴IMU的姿态解算方法的步骤。

本申请提供的基于九轴IMU的姿态解算方法、装置及存储介质，其中，所述姿态解算方法包括：先在内参标定算法中，利用九轴IMU的陀螺仪、加速度计和磁力计输出的原始数据，标定三个内部模块的内部参数，借助标定内部参数，将原始数据转化为标定数据；然后针对陀螺仪和加速度计标定数据，完成姿态初步解算，输出IMU的初步姿态数据，最后在基于磁修正时刻对应周期和姿态修正时刻对应周期的磁力和姿态数据修正算法中，利用重力矢量和权重值最高的磁场矢量修正陀螺仪漂移，输出IMU的最终姿态数据。这样一来，能够通过长短周期的磁力和姿态数据的互相校正来对IMU姿态进行修正，抑制磁场干扰对姿态解算的不良影响，进而实现了IMU姿态的准确、稳定输出，满足了在室内复杂磁场环境中对IMU器件的应用需求。

为使本申请的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本申请实施例所提供的一种基于九轴IMU的姿态解算方法的流程图；

图2为本申请实施例所提供的一种陀螺仪和加速度计依次执行内参标定方法的流程图；

图3为本申请实施例所提供的一种基于互补滤波算法的六轴融合计算方法的流程图；

图4为本申请实施例所提供的一种基于九轴IMU的姿态解算方法的组织结构框图；

图5为本申请实施例所提供的一种基于长短周期的磁力和姿态数据修正算法的流程图；

图6为实验中IMU器件靠近强磁场前的IMU器件姿态的可视图结果示意图；

图7为实验中IMU器件靠近强磁场后的IMU器件姿态的可视图结果示意图；

图8为本申请实施例所提供的一种基于九轴IMU的姿态解算装置的结构示意图；

图9为本申请实施例所提供的一种电子设备的结构示意图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的每个其他实施例，都属于本申请保护的范围。

IMU是常用的定位传感器件，通常情况下，IMU器件被固定在某个刚体上，利用IMU器件输出的IMU原始数据，结合姿态解算方法，实时计算输出IMU(或刚体)的最终姿态数据，供导航定位计算使用。这里的姿态仅表示空间中三维旋转量信息，不包含平移量信息，在导航领域中的姿态被命名为：俯仰角、偏航角和翻滚角。

九轴IMU内部包含一个三轴陀螺仪（Gyro）模块，一个三轴加速度计（Acc）模块，一个三轴磁力计（Mag）模块，三个内部模块可以稳定地输出相对各自坐标系（三轴陀螺仪坐标系、三轴加速度计坐标系、三轴磁力计坐标系）的三轴加速度数据Ax, Ay, Az，三轴角速度数据Gx, Gy, Gz，三轴磁场强度数据Mx, My, Mz，记为IMU原始数据。

通常情况下的IMU姿态解算方法，是利用IMU原始数据作为输入量，依次完成IMU内参标定、IMU姿态计算、IMU姿态校正等任务，最后实时、平稳地输出IMU姿态数据。实际情况下，三个坐标系有固定偏差。在内参标定算法中，利用IMU原始数据完成内参标定后，可以利用标定参数将三个坐标系统一为一个坐标系，记为IMU坐标系。IMU坐标系三轴垂直，坐标系方向由重力方向和标定环境中稳定的地磁北向所确定。

目前已有的九轴IMU姿态解算方法，主要为基于Mahony互补滤波、Madgwick算法、扩展的卡尔曼滤波等算法实现的数据融合姿态解算方法。虽然方法不同，但基本思想都是一致的：一方面，通过对陀螺仪原始角速度数据进行累积计算获得IMU初步姿态，另一方面，通过地球重力方向以及地磁场方向，定期或不定期地消除IMU姿态计算中的累积误差，对其完成校正。然而，这些九轴IMU姿态解算方法，仅针对具有稳定磁场的外部环境，虽然也具有一定的抗磁干扰能力，但在面对复杂不稳定的磁场环境时，计算出的IMU姿态数据难以让人满意，结果不具有可靠性，不能实际应用。

举例说明，在室内环境中，将一个磁铁放置于IMU器件附近，IMU器件通电后拿开磁铁，使用以上这些算法解算出的IMU姿态数据会得出明显错误的结果。分析其原因在于，以上这些公开的姿态解算方法，对复杂磁场环节的应用上缺少针对性的设计，没有任何特征能够直接将地磁场从各种干扰中区分出来。即使当前磁场强度与地磁场强度相同，也不代表磁场方向与地磁场方向一致。地磁场方向不能确定，以其为基础的IMU姿态校正算法就会失败，产生错误。

除此之外，已有的九轴IMU姿态解算方法，同样不适用于室内应用场景。首先，地磁场的磁场强度很弱，只有不到60μT。在室内磁场环境中，干扰源附近的磁场强度远强于地磁场强度。例如，笔记本附近，其值可达3000μT，台式机500μT，手机1000μT，无线发射设备4000μT；其次，室内磁场的方向矢量处处不同；室内环境的磁场通常由多个干扰源作用叠加而成，加上磁化体和具有屏蔽效应的物体，使室内磁场异常复杂。因此，已有的九轴IMU姿态解算方法，更多地只能应用于室外导航场景，不适用于室内应用场景。

基于此，本申请实施例提供了一种基于九轴IMU的姿态解算方法，属于导航定位领域，能够适用于复杂磁场环境，且具有一定的抗磁干扰能力，可以解决室内导航的应用问题。本申请实施例定义了磁场矢量-权重值集合，设计了基于长短周期（姿态修正时刻对应周期和磁修正时刻对应周期）的磁力和姿态数据修正算法，可以保证提供可靠、精准的稳定磁力计标定数据，实现了磁力计标定后数据与IMU初步姿态数据的融合和互相校正，抑制了偏航角的漂移，保证了稳定、准确的IMU器件最终姿态数据输出，充分满足了复杂磁场环境（尤其是室内环境）的应用需求。

请参阅图1，图1为本申请实施例所提供的一种基于九轴IMU的姿态解算方法的流程图。

首先，先介绍本申请的技术构思：IMU器件的初步姿态数据包含了对除偏航角外另外两个轴向漂移误差的抑制，但对偏航角轴向漂移误差不具有抑制校正作用。进而，本申请实施例借助于磁力计标定数据，对IMU器件姿态的偏航角轴向偏差进行校正。

但是，在地表环境尤其是室内环境中，普遍存在金属物体，在磁化效应以及屏蔽效应的作用下，室内磁场环境极度复杂，室内磁干扰现象非常严重，而空间中分布不均匀的磁场或者磁场变化，都会引发磁干扰。因此，工作中的磁力计输出的磁力计标定数据，并不是稳定的磁场数据，包含复杂、严重的磁干扰，会很容易导致校正工作的失败。

针对这一问题，我们对磁力计标定数据进行统计意义上的处理：提取磁力计标定数据中稳定的磁场矢量数据，并借助于该稳定的磁场矢量数据，对器件姿态进行有效、精准的校正，输出最终姿态数据。

具体而言：本申请实施例定义了磁场矢量-权重值集合，采用磁力计标定数据有效性检测，设计了长短修正周期，基于长短周期的磁力和姿态数据进行修正，可以保证提供可靠、精准的稳定磁力计标定数据，实现了磁力计标定后数据与IMU初步姿态数据的融合和校正，抑制了偏航角的漂移，保证了稳定、准确的IMU最终姿态数据输出，充分满足了复杂磁场环境（尤其室内环境）的应用需求。

具体地，如图1中所示，本申请实施例提供的姿态解算方法包括：

S110、基于获取到的九轴IMU的陀螺仪、加速度计和磁力计的原始数据，分别得到经过标定后的陀螺仪标定数据、加速度计标定数据和磁力计标定数据。

步骤S110中，先获取九轴IMU的陀螺仪、加速度计和磁力计的原始数据，然后基于获取到的原始数据进行处理，得到经过标定后的标定数据，即陀螺仪标定数据、加速度计标定数据和磁力计标定数据。

这里，先在内参标定算法中，利用九轴IMU的陀螺仪、加速度计和磁力计输出的原始数据，标定三个内部模块的内部参数，借助标定内部参数，将原始数据转化为标定数据。

具体地，IMU中陀螺仪、加速度计和磁力计三个内部模块中的每一个模块都具有如下形式的内参模型：

其中，x

请参阅图2，图2为本申请实施例所提供的一种陀螺仪和加速度计依次执行内参标定方法的流程图，在内参标定阶段，具体过程如下：Acc模块的内参标定，借助转台连续采集Acc模块的若干帧数据，使用“六位置分立式标定方法”标定该模块的内参。Gyro模块的内参标定，借助转台连续采集Acc和Gyro的若干帧数据，使用“分立式标定方法”标定该模块的内参。

除此之外，Mag模块的内参标定，手动挥舞IMU器件，同时连续采集Mag的若干帧数据，使用“手动绕八字方法”标定该模块的内参，在正常运行阶段，采用上述内参标定阶段标定的内参结果，结合上述公式对IMU原始数据进行计算处理，输出IMU标定后数据。

S120、对得到的陀螺仪标定数据和加速度计标定数据进行数据融合，得到初步姿态数据。

步骤S120中，常采用基于Mahony互补滤波、Madgwick算法、扩展的卡尔曼滤波等数据融合算法实现数据融合姿态解算方法。

具体地，针对陀螺仪标定数据和加速度计标定数据，使用数据融合算法进行数据融合，完成姿态初步解算，输出IMU的初步姿态数据，其本质都是使用陀螺仪累积计算得到姿态数据，再借助于重力矢量不定期校正姿态数据偏差。

进而，对陀螺仪标定数据进行累积计算，可以直接得到每一时刻的IMU器件姿态，但受累积误差效应的影响，如此计算得到的IMU器件姿态会随着时间推移产生越来越大的漂移误差。这里融合算法的本质，就是借用静止状态下加速度计的重力矢量，对IMU器件姿态进行部分校正，对除偏航角外另外两个轴向的漂移误差进行抑制，输出IMU器件的初步姿态数据。这里，针对陀螺仪标定数据和加速度计标定数据，使用互补滤波算法等数据融合算法（比如：Mahony互补滤波算法）进行数据融合，完成姿态初步解算，输出IMU初步姿态四元数数据。其中，借助于静止时刻加速度计的重力矢量，完成对除偏航角外的两个轴向漂移误差的抑制。

请参阅图3，图3为本申请实施例所提供的一种基于互补滤波算法的六轴融合计算方法的流程图，如图3中所示：首先，使用Acc标定数据判断IMU是否处于静止状态，当IMU处于静止状态时，把Acc标定数据由当前采样时刻对应的IMU坐标系转换到IMU初始坐标系下，经过参数的转换计算得到IMU初始坐标系下的当前采样时刻重力矢量，计算求取其与预设的初始重力矢量间的误差，得到误差矢量，利用误差矢量和此时Gyro标定后四元数数据，执行互补滤波算法；当IMU处于非静止状态时，直接利用Gyro标定后四元数数据，执行互补滤波算法；最后，互补滤波算法输出IMU初步姿态数据。

S130、确定一个表征磁场时间统计和空间统计的磁场矢量-权重值集合。

这里，时间统计是指：对九轴IMU运动时间内的所有磁场矢量进行分析，判断其是否加入该磁场矢量-权重值集合；空间统计是指：对所有加入集合的磁场矢量在空间方向上进行比较，以不同权重值的形式记录不同方向的磁场矢量所出现的次数。

步骤S130中，定义了磁场矢量-权重值集合，将受外在因素干扰的时间尺度上磁场矢量的分析，转化为维护一个带统计量的集合操作，而最优磁场矢量是对应权重值最大的磁场矢量，是当前测试环境下最稳定的磁场矢量，从统计学的意义上确保了磁场矢量的可靠性和准确性，从而保证了IMU姿态解算数据的全局稳定性和抗磁干扰能力。

S140、将加速度计标定数据和磁力计标定数据转换成用于矢量描述的加速度矢量和磁场矢量。

这里，加速度计标定数据与加速度矢量是对同一个对象的不同描述，同理，磁力计标定数据和磁场矢量也是对同一个对象的不同描述。

具体地，加速度计标定数据是三轴向的具体加速度值数据的集合；从数学矢量的角度理解，可以把上述集合中的数据理解为一个总矢量在三个方向的分矢量，该总矢量就是加速度矢量。

应当理解下文提到的加速度计标定数据等同于加速度矢量，磁力计标定数据等同于磁场矢量。

S150、当到达预设的磁力修正时刻时，根据所述初步姿态数据和当前磁力修正时刻获得的磁场矢量对所述磁场矢量-权重值集合进行修正，得到修正后的磁场矢量-权重值集合。

步骤S150中，磁力修正时刻对应周期即为短修正周期，磁力修正时刻是磁力修正时刻对应周期的某一时刻。这里，定期执行一次修正操作，修正目标为借助IMU器件的初步姿态四元数和当前磁场矢量来更新磁场矢量-权重值集合。

具体地，将IMU坐标系下的磁场矢量与磁场矢量-权重值集合中最大的权重值对应的磁场矢量进行比较，得到当前磁力修正时刻所述磁力计检测到的磁场方向与稳定磁场方向之间的角度差值；若角度差值在预设角度差范围内时，将当前磁力修正时刻IMU坐标系下的磁场矢量添加到所述磁场矢量-权重值集合中；若角度差值不在预设角度差范围内时，则不将当前磁力修正时刻IMU坐标系下添加到磁场矢量从所述磁场矢量-权重值集合中。

S160、当到达预设的姿态修正时刻时，根据修正后的磁场矢量-权重值集合中权重值最高的磁场矢量以及当前处于静止状态的加速度计的加速度矢量，对所述初步姿态数据进行修正，得到最终姿态数据，其中，所述姿态修正时刻对应周期是所述磁力修正时刻对应周期的整数倍。

步骤S160中，姿态修正时刻对应周期即为长修正周期，这里，定期执行一次修正操作，修正目标为借助磁场矢量-权重值集合来更新IMU器件的姿态四元数，进而完成对初步姿态数据的修正。

这里，基于最大的权重值对应的磁场矢量和当前姿态修正时刻由加速度计确定出的重力矢量，更新所述九轴IMU的姿态四元数，确定更新后的九轴IMU的姿态四元数为最终姿态数据。

需要补充的是，磁场矢量-权重值集合记录的是所有历史时刻上，出现概率最大的几个磁场方向，最终在姿态修正时，选用概率最大（权重值最大）的那个磁场矢量，也就是说选用当前环境下长期最稳定的磁场方向和磁场矢量，可以保证对初步姿态数据实现长期稳定的校正效果。

具体地，请参阅图4，图4为本申请实施例所提供的一种基于九轴IMU的姿态解算方法的组织结构框图，如图4中所示，九轴IMU的姿态解算方法的整体流程是：先在内参标定算法中，利用九轴IMU的陀螺仪、加速度计和磁力计输出的原始数据，标定三个内部模块的内部参数，借助标定内部参数，将原始数据转化为标定数据；然后在六轴融合计算算法中，针对陀螺仪标定数据和加速度计标定数据，使用数据融合算法进行数据融合，完成姿态初步解算，输出IMU的初步姿态数据，其本质都是使用陀螺仪累计计算得到姿态数据，再借助于重力矢量不定期校正姿态数据偏差；最后在基于长短周期（姿态修正时刻对应周期和磁修正时刻对应周期）的磁力和姿态数据修正算法中，利用重力矢量和最优磁场矢量（权重值最高的磁场矢量）修正陀螺仪漂移，输出IMU的最终姿态数据。

这样一来，通过本申请实施例长短周期的磁力和姿态数据的互相校正来对IMU姿态进行修正，能够抑制磁场干扰对姿态解算的不良影响，进而实现了IMU姿态的准确、稳定输出，满足了在室内复杂磁场环境中对IMU器件的应用需求。

在本申请实施例中，作为一种优选的实施例，步骤S120包括：

根据上一采样时刻已计算得到的初步姿态数据，结合获得的当前采样时刻陀螺仪标定数据，计算得到当前采样时刻的候选初步姿态数据；若所述九轴IMU处于非静止状态，则基于所述候选初步姿态数据对所述初步姿态数据进行更新，得到更新后的初步姿态数据；若所述九轴IMU处于静止状态，由当前采样时刻的加速度计标定数据确定在IMU初始坐标系下的当前采样时刻重力矢量；根据在所述IMU初始坐标系下预先确定出的初始重力矢量，确定所述当前采样时刻重力矢量和所述初始重力矢量之间的误差矢量；根据所述候选初步姿态数据以及所述误差矢量，采用数据融合算法，计算得到一个融合后初步姿态数据；基于所述融合后初步姿态数据对所述初步姿态数据进行更新，得到更新后的初步姿态数据；其中，所述IMU初始坐标系为九轴IMU的陀螺仪、加速度计和磁力计经过标定后的初始时刻确定出的坐标系。

这里，上一采样时刻已计算得到的初步姿态数据是已知的姿态数据，本申请实施例直接拿来使用。

步骤S120是基于互补滤波算法进行数据融合，从而得到IMU初步姿态数据。具体过程可以参见图3。

在本申请实施例中，作为一种优选的实施例，步骤S130通过以下步骤确定所述磁场矢量-权重值集合：

基于每个初始磁场矢量以及该初始磁场矢量对应的权重值，得到多个初始集合元素；当新增磁场矢量与任意一个初始磁场矢量的差值在预设圆锥范围内时，确定与该新增磁场矢量对应的初始磁场矢量的累计权重值；当新增磁场矢量与任意一个初始磁场矢量的差值不在预设圆锥范围内时，确定该新增磁场矢量以及该新增磁场矢量对应的权重值为新增集合元素；基于与该新增磁场矢量对应的初始磁场矢量的累计权重值，或所述新增集合元素，更新所述磁场矢量-权重值集合。

这里是集合元素的添加操作：当需要将有效磁场矢量

具体地，步骤S130通过以下步骤确定所述磁场矢量-权重值集合：

当检测到所述磁场矢量-权重值集合中所有集合元素的元素个数大于设定个数阈值时，删除部分目标元素；

其中，将所有集合元素按照对应的权重值从小到大的顺序排序，从最小权重值开始，选取目标数量个集合元素组成部分目标元素，所述目标数量为所有集合元素的元素个数与设定个数阈值之间的数量差值，所述集合元素包括初始集合元素，或所述初始集合元素和所述新增集合元素之和。

这里是集合元素的删除操作：当检测到集合P中的集合元素个数超过设定个数阈值N

具体地，步骤S130通过以下公式确定所述磁场矢量-权重值集合：

其中，P表示磁场矢量-权重值集合，

其中，IMU坐标系为九轴IMU的陀螺仪、加速度计和磁力计经过标定后确定出的坐标系，t时刻IMU坐标系为九轴IMU的陀螺仪、加速度计和磁力计经过标定后的t时刻确定出的坐标系。

这里定义了磁场矢量-权重值集合：定义集合P为磁场矢量-权重值集合：

这样一来，本申请实施例定义了磁场矢量-权重值集合，将受外在因素干扰的时间尺度上磁场矢量的分析，转化为维护一个带统计量的集合操作，而最优磁场矢量是对应权重值最大的磁场矢量，从统计学的意义上确保了磁场矢量的可靠性和准确性，从而保证了IMU姿态解算数据的全局稳定性和抗磁干扰能力。

需要补充的是，本申请实施例仅当满足修正条件时，才执行磁场矢量修正操作和初步姿态数据修正操作。进而，在进行修正操作前，需要判断IMU姿态是否满足修正条件，请参阅图5。

具体地，借助加速度计标定数据检测IMU状态，当和重力矢量比较确认为静止状态时，称为满足修正条件。

这里，IMU初始坐标系下有一个静止的初始重力矢量（3乘1的向量），加速度计有三个轴向的加速度值，将其转换到IMU初始坐标系下，得到IMU初始坐标系下的当前采样时刻重力矢量，将当前采样时刻重力矢量和IMU初始坐标系下的初始重力矢量进行比较，当两个重力矢量之间的误差矢量在误差允许范围内时，确定此时的九轴IMU处于静止状态。

如图5所示，在执行磁场矢量修正操作和姿态数据修正操作前，需要先进行磁力计标定数据的有效性检测。设在

在本申请实施例中，作为一种优选的实施例，步骤S150包括：

在九轴IMU处于静止状态下，确定所述磁场矢量-权重值集合中最大的权重值对应的磁场矢量；基于当前磁力修正时刻下所述磁场矢量以及所述初步姿态数据中的陀螺仪的姿态四元数，确定IMU坐标系下的磁场矢量；将所述IMU坐标系下的磁场矢量与所述磁场矢量-权重值集合中最大的权重值对应的磁场矢量进行比较，得到当前磁力修正时刻所述磁力计检测到的磁场方向与稳定磁场方向之间的角度差值；若所述角度差值在预设角度差范围内时，将当前磁力修正时刻IMU坐标系下的磁场矢量添加到所述磁场矢量-权重值集合中；若所述角度差值不在预设角度差范围内时，将不将当前磁力修正时刻IMU坐标系下的磁场矢量添加到所述磁场矢量-权重值集合中。

具体地，如图5所示，该步骤主要是磁场矢量修正操作。当到达预设的磁力修正时刻时，先检测IMU是否满足修正条件，若不满足，则不继续执行，若满足，则继续执行；对当前帧磁力计标定数据

这样一来，通过定期执行磁场矢量修正操作，可以有效地维护磁场矢量-权重值集合P，保证记录的磁场数据从始至终的稳定性。

在本申请实施例中，作为一种优选的实施例，步骤S160包括：

在九轴IMU处于静止状态下，确定所述磁场矢量-权重值集合中最大的权重值对应的磁场矢量；基于最大的权重值对应的磁场矢量和当前姿态修正时刻由所述加速度计确定出的加速度矢量，更新所述九轴IMU的姿态四元数；确定更新后的九轴IMU的姿态四元数为最终姿态数据。

具体地，如图5所示，该步骤主要是姿态数据修正操作。当到达预设的姿态修正时刻时，先检测IMU姿态是否满足修正条件，若不满足，则不继续执行，若满足，则继续；从当前磁场矢量-权重值集合P中选取权重值最大的磁场矢量，即

如上所述，通过定期执行姿态数据修正操作，纠正了受累积误差效应影响的器件姿态在偏航角轴向上产生的偏差，对磁场矢量-权重值集合的利用保证了磁场矢量的稳定、可靠来源，进而保证了借助磁场矢量定期修正器件姿态操作的有效性和精准性，增强了IMU姿态解算数据的全局稳定性和抗磁干扰能力。

进而，经过上述多步修正过程后的所维护的姿态数据

下面以实验中的具体实施例进行说明：

九轴IMU器件通电前，初始位置附近不存在强磁场；九轴IMU器件工作后，把器件拿到一磁铁（强磁场）附近，做短时间运动，随后把九轴IMU器件拿开放回到初始位置。实验结果如下：IMU器件运动前静止时，IMU姿态数据的四元数表示为(0.7281, -0.0042, -0.06854,0.0038)；IMU器件靠近磁场运动后再静止放到原位置处时，IMU姿态数据的四元数表示为(0.7223, -0.0038, -0.6906, -0.0350)，具体地，如图6和图7所示，图6为实验中IMU器件靠近强磁场前的IMU器件姿态的可视图结果示意图，图7为实验中IMU器件靠近强磁场后的IMU器件姿态的可视图结果示意图；根据实验中运动IMU的抗强磁干扰能力测试结果图可知，IMU器件前后姿态变化不大，在可允许的小范围内。实验表明，本申请实施例适用于复杂磁场环境的九轴IMU姿态解算方法，能有效抑制以上类型的磁场干扰影响，稳定地输出九轴IMU的准确位姿。

具体过程如下：分别标定九轴IMU器件内部模块：陀螺仪，加速度计，和磁力计的内参，其中：借助转台，使用“六位置分立式标定方法”，标定加速度计内参模型的内参；利用转台，使用“分立式标定方法”，直接标定陀螺仪内参模型的内参；使用“手动绕八字方法”，标定磁力计内参模型的内参。这里不限定标定方法，使用通用的成熟算法完成标定即可。

具体地，在基于互补滤波算法的六轴融合计算方法中，使用Mahony互补滤波算法，用加速度计修正陀螺仪，抑制偏航角以外的两个轴向的漂移；静置产品，测试偏航角方向的陀螺仪漂移速度，根据陀螺仪漂移速度和产品性能要求，设定磁力和姿态数据修正的长短修正周期；当到达预设的磁力修正时刻时，基于初步姿态数据和当前磁修正时刻获得的磁场矢量（磁力计标定数据的数学矢量描述形式），对磁场矢量-权重值集合进行修正，必要时删除集合中权重值最低的磁场矢量；当到达预设的姿态修正时刻时，基于磁场矢量-权重值集合，选取其中权重值最高的磁场矢量，对初步姿态数据进行偏航角轴向的偏移修正，获得修正后的最终姿态数据，并输出该最终姿态数据。

本申请实施例提供的基于九轴IMU的姿态解算方法，先基于获取到的九轴IMU的陀螺仪、加速度计和磁力计的原始数据，分别得到经过标定后的陀螺仪标定数据、加速度计标定数据和磁力计标定数据；然后对得到的陀螺仪标定数据和加速度计标定数据进行数据融合，得到初步姿态数据；预先确定一个表征磁场时间统计和空间统计的磁场矢量-权重值集合；并且将加速度计标定数据和磁力计标定数据转换成用于矢量描述的加速度矢量和磁场矢量；进而，当到达预设的磁力修正时刻时，根据所述初步姿态数据和当前磁力修正时刻获得的磁场矢量对所述磁场矢量-权重值集合进行修正，得到修正后的磁场矢量-权重值集合；当到达预设的姿态修正时刻时，根据修正后的磁场矢量-权重值集合中权重值最高的磁场矢量以及当前处于静止状态的加速度计的加速度矢量，对所述初步姿态数据进行修正，得到最终姿态数据，其中，所述姿态修正时刻对应周期是所述磁力修正时刻对应周期的整数倍。这样一来，能够通过长短周期的磁力和姿态数据对IMU姿态进行修正，抑制磁场干扰对姿态解算的不良影响，进而实现了IMU姿态的准确、稳定输出，满足了在室内复杂磁场环境中对IMU器件的应用需求。

基于同一发明构思，本申请实施例中还提供了一种与基于九轴IMU的姿态解算方法对应的基于九轴IMU的姿态解算装置，由于本申请实施例中的装置解决问题的原理与本申请实施例上述方法相似，因此装置的实施可以参见方法的实施，重复之处不再赘述。

请参阅图8，图8为本申请实施例所提供的一种基于九轴IMU的姿态解算装置的结构示意图。如图8中所示，所述姿态解算装置800包括：

内参标定模块810，用于基于获取到的九轴IMU的陀螺仪、加速度计和磁力计的原始数据，分别得到经过标定后的陀螺仪标定数据、加速度计标定数据和磁力计标定数据；

数据处理模块820，用于对得到的陀螺仪标定数据和加速度计标定数据进行数据融合，得到初步姿态数据；

集合确定模块830，用于确定一个表征磁场时间统计和空间统计的磁场矢量-权重值集合；

数据转换模块840，用于将加速度计标定数据和磁力计标定数据转换成用于矢量描述的加速度矢量和磁场矢量；

第一修正模块850，用于当到达预设的磁力修正时刻时，根据所述初步姿态数据和当前磁力修正时刻获得的磁场矢量对所述磁场矢量-权重值集合进行修正，得到修正后的磁场矢量-权重值集合；

第二修正模块860，用于当到达预设的姿态修正时刻时，根据修正后的磁场矢量-权重值集合中权重值最高的磁场矢量以及当前处于静止状态的加速度计的加速度矢量，对所述初步姿态数据进行修正，得到最终姿态数据，其中，所述姿态修正时刻对应周期是所述磁力修正时刻对应周期的整数倍。

在本申请实施例中，作为一种优选的实施例，数据处理模块820在用于对得到的陀螺仪标定数据和加速度计标定数据进行数据融合，得到初步姿态数据时，数据处理模块820具体用于：

根据上一采样时刻已计算得到的初步姿态数据，结合获得的当前采样时刻陀螺仪标定数据，计算得到当前采样时刻的候选初步姿态数据；

若所述九轴IMU处于非静止状态，则基于所述候选初步姿态数据对所述初步姿态数据进行更新，得到更新后的初步姿态数据；

若所述九轴IMU处于静止状态，由当前采样时刻的加速度计标定数据确定在IMU初始坐标系下的当前采样时刻重力矢量；

根据在所述IMU初始坐标系下预先确定出的初始重力矢量，确定所述当前采样时刻重力矢量和所述初始重力矢量之间的误差矢量；

根据所述候选初步姿态数据以及所述误差矢量，采用数据融合算法，计算得到一个融合后初步姿态数据；基于所述融合后初步姿态数据对所述初步姿态数据进行更新，得到更新后的初步姿态数据；其中，所述IMU初始坐标系为九轴IMU的陀螺仪、加速度计和磁力计经过标定后的初始时刻确定出的坐标系。

在本申请实施例中，作为一种优选的实施例，集合确定模块830用于通过以下步骤确定所述磁场矢量-权重值集合：

基于每个初始磁场矢量以及该初始磁场矢量对应的权重值，得到多个初始集合元素；

当新增磁场矢量与任意一个初始磁场矢量的差值在预设圆锥范围内时，确定与该新增磁场矢量对应的初始磁场矢量的累计权重值；

当新增磁场矢量与任意一个初始磁场矢量的差值不在预设圆锥范围内时，确定该新增磁场矢量以及该新增磁场矢量对应的权重值为新增集合元素；

基于与该新增磁场矢量对应的初始磁场矢量的累计权重值，或所述新增集合元素，更新所述磁场矢量-权重值集合。

在本申请实施例中，作为一种优选的实施例，集合确定模块830用于通过以下步骤确定所述磁场矢量-权重值集合：

当检测到所述磁场矢量-权重值集合中所有集合元素的元素个数大于设定个数阈值时，删除部分目标元素；

其中，将所有集合元素按照对应的权重值从小到大的顺序排序，从最小权重值开始，选取目标数量个集合元素组成部分目标元素，所述目标数量为所有集合元素的元素个数与设定个数阈值之间的数量差值，所述集合元素包括初始集合元素，或所述初始集合元素和所述新增集合元素之和。

在本申请实施例中，作为一种优选的实施例，集合确定模块830用于通过以下公式确定所述磁场矢量-权重值集合：

其中，P表示磁场矢量-权重值集合，

在本申请实施例中，作为一种优选的实施例，第一修正模块850在用于当到达预设的磁力修正时刻时，根据所述初步姿态数据和当前磁力修正时刻获得的磁场矢量对所述磁场矢量-权重值集合进行修正时，第一修正模块850具体用于：

在九轴IMU处于静止状态下，确定所述磁场矢量-权重值集合中最大的权重值对应的磁场矢量；

基于当前磁力修正时刻下所述磁场矢量以及所述初步姿态数据中的陀螺仪的姿态四元数，确定IMU坐标系下的磁场矢量；

将所述IMU坐标系下的磁场矢量与所述磁场矢量-权重值集合中最大的权重值对应的磁场矢量进行比较，得到当前磁力修正时刻所述磁力计检测到的磁场方向与稳定磁场方向之间的角度差值；

若所述角度差值在预设角度差范围内时，将当前磁力修正时刻IMU坐标系下的磁场矢量添加到所述磁场矢量-权重值集合中；

若所述角度差值不在预设角度差范围内时，则不将当前磁力修正时刻IMU坐标系下的磁场矢量添加到所述磁场矢量-权重值集合中。

在本申请实施例中，作为一种优选的实施例，第二修正模块860在用于当到达预设的姿态修正时刻时，根据修正后的磁场矢量-权重值集合中权重值最高的磁场矢量以及当前处于静止状态的加速度计的加速度矢量，对所述初步姿态数据进行修正，得到最终姿态数据时，第二修正模块860具体用于：

在九轴IMU处于静止状态下，确定所述磁场矢量-权重值集合中最大的权重值对应的磁场矢量；

基于最大的权重值对应的磁场矢量和当前姿态修正时刻由所述加速度计确定出的加速度矢量，更新所述九轴IMU的姿态四元数；

确定更新后的九轴IMU的姿态四元数为最终姿态数据。

本申请实施例提供的基于九轴IMU的姿态解算装置，包括内参标定模块、数据处理模块、集合确定模块、数据转换模块、第一修正模块和第二修正模块，其中，内参标定模块用于基于获取到的九轴IMU的陀螺仪、加速度计和磁力计的原始数据，分别得到经过标定后的陀螺仪标定数据、加速度计标定数据和磁力计标定数据；数据处理模块用于对得到的陀螺仪标定数据和加速度计标定数据进行数据融合，得到初步姿态数据；集合确定模块用于确定一个表征磁场时间统计和空间统计的磁场矢量-权重值集合；数据转换模块用于将加速度计标定数据和磁力计标定数据转换成用于矢量描述的加速度矢量和磁场矢量；第一修正模块用于当到达预设的磁力修正时刻时，根据所述初步姿态数据和当前磁力修正时刻获得的磁场矢量对所述磁场矢量-权重值集合进行修正，得到修正后的磁场矢量-权重值集合；第二修正模块用于当到达预设的姿态修正时刻时，根据修正后的磁场矢量-权重值集合中权重值最高的磁场矢量以及当前处于静止状态的加速度计的加速度矢量，对所述初步姿态数据进行修正，得到最终姿态数据，其中，所述姿态修正时刻对应周期是所述磁力修正时刻对应周期的整数倍。这样一来，能够通过长短周期的磁力和姿态数据对IMU姿态进行修正，抑制磁场干扰对姿态解算的不良影响，进而实现了IMU姿态的准确、稳定输出，满足了在室内复杂磁场环境中对IMU器件的应用需求。

请参阅图9，图9为本申请实施例所提供的一种电子设备的结构示意图。如图9中所示，所述电子设备900包括处理器910、存储器920和总线930。

所述存储器920存储有所述处理器910可执行的机器可读指令，当电子设备900运行时，所述处理器910与所述存储器920之间通过总线930通信，所述机器可读指令被所述处理器910执行时，可以执行如上述图1所示的基于九轴IMU的姿态解算方法，图2所示的陀螺仪和加速度计依次执行内参标定方法，图3所示的基于互补滤波算法的六轴融合计算方法，图4所示的基于九轴IMU的姿态解算方法以及图5所示的基于长短周期的磁力和姿态数据修正算法的步骤，具体实现方式可参见方法实施例，在此不再赘述。

本申请实施例还提供一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器运行时可以执行如上述图1所示的基于九轴IMU的姿态解算方法，图2所示的陀螺仪和加速度计依次执行内参标定方法，图3所示的基于互补滤波算法的六轴融合计算方法，图4所示的基于九轴IMU的姿态解算方法以及图5所示的基于长短周期的磁力和姿态数据修正算法的步骤，具体实现方式可参见方法实施例，在此不再赘述。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个处理器可执行的非易失的计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者网络设备等）执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（Read-OnlyMemory，ROM）、随机存取存储器（Random Access Memory，RAM）、磁碟或者光盘等各种可以存储程序代码的介质。

最后应说明的是：以上所述实施例，仅为本申请的具体实施方式，用以说明本申请的技术方案，而非对其限制，本申请的保护范围并不局限于此，尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本申请实施例技术方案的精神和范围，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以权利要求的保护范围为准。