CN108413960A - 姿态计算方法和相关设备 - Google Patents

Abstract

本发明实施例公开了一种姿态计算方法和相关设备，用于获取到准确的实时终端姿态。本发明实施例的姿态计算方法应用于终端，所述终端设有磁力计、加速度计和陀螺仪，该方法包括：在第一采样周期，获取磁力计采集的磁力计数据和加速度计采集的加速度计数据，以计算得到第一终端姿态和第一姿态变化率；确定陀螺仪的初始姿态和获取陀螺仪采集的陀螺仪数据，以计算得到第二终端姿态和第二姿态变化率；根据所述第一姿态变化率和所述第二姿态变化率，计算姿态可信度；根据所述姿态可信度的调节，融合所述第一终端姿态和所述第二终端姿态，以得到所述终端的第三终端姿态。本发明实施例得到的第三终端姿态准确可用，实现了获取到准确的实时终端姿态。

Description

姿态计算方法和相关设备

技术领域

本发明涉及数据处理领域，尤其涉及一种姿态计算方法、终端、电子设备和计算机程序产品。

背景技术

捷联惯性导航是一种自主式导航方法，该方法将磁力计、加速度计、陀螺仪安装在终端上，可实现无外部全球定位信号(例如全球定位系统(Global Positioning System,GPS)、北斗)，或其它定位技术(例如无线保真(WIreless-Fidelity，Wi-Fi)，射频识别(Radio Frequency Identification，RFID))时，终端通过相关传感器采集的数据进行计算，以实现自主导航。捷联惯性姿态解算方法是捷联式惯性导航系统的关键技术，该方法可以得到终端的姿态和导航参数计算需要的数据。

现有的捷联惯性姿态解算方法，例如为：可以通过加速度计与磁力计计算方向角，与陀螺仪计算方向角进行卡尔曼滤波获得滤波后的终端姿态；或者，通过加速度计与磁力计计算方向角，与陀螺仪计算方向角进行一阶、二阶互补滤波后，得到终端姿态。

现有的捷联惯性姿态解算方法能获得矫正后的终端姿态，最终的终端姿态收敛，但中间时刻的终端姿态可能出现较大误差，如外部磁场干扰，或终端持续单向加速将对中间时刻的终端姿态产生较大误差，从而获取不到准确的实时终端姿态。

发明内容

针对上述技术问题，本发明实施例提供了一种姿态计算方法、终端、电子设备和计算机程序产品，用于获取到准确的实时终端姿态。

本发明实施例的第一方面提供一种姿态计算方法，所述方法应用于终端，所述终端设有磁力计、加速度计和陀螺仪，所述方法包括：

在第一采样周期，获取所述磁力计采集的磁力计数据和所述加速度计采集的加速度计数据；

使用所述磁力计数据和所述加速度计数据，计算得到所述终端的第一终端姿态和第一姿态变化率；

确定所述陀螺仪的初始姿态；

在第二采样周期，获取所述陀螺仪采集的陀螺仪数据；

在所述陀螺仪的初始姿态下，使用所述陀螺仪数据计算得到所述终端的第二终端姿态和第二姿态变化率；

根据所述第一姿态变化率和所述第二姿态变化率，计算姿态可信度；

根据所述姿态可信度的调节，融合所述第一终端姿态和所述第二终端姿态，以得到所述终端的第三终端姿态。

可选地，所述根据所述第一姿态变化率和所述第二姿态变化率，计算姿态可信度，包括：

根据所述第一姿态变化率和所述第二姿态变化率，并结合所述终端当前运动模式下的整定，计算姿态可信度。

可选地，所述使用所述磁力计数据和所述加速度计数据，计算得到所述终端的第一终端姿态和第一姿态变化率，包括：

使用所述磁力计数据和所述加速度计数据，计算得到所述终端的第一终端姿态；

确定所述终端的角度轴尺寸；

根据所述角度轴尺寸单位化角度轴，得到单位角度轴；

计算所述第一采样周期的当前采样周期和上一采样周期间的第一姿态角差值；

根据所述单位角度轴和所述第一姿态角差值，计算得到第一姿态变化量；

根据所述第一姿态变化量和所述第一采样周期的周期时间，计算得到第一姿态变化率，其中所述第一姿态变化率和所述第一姿态变化量正相关，所述第一姿态变化率和所述第一采样周期的周期时间负相关；

所述在所述陀螺仪的初始姿态下，使用所述陀螺仪数据计算得到所述终端的第二终端姿态和第二姿态变化率，包括：

在所述陀螺仪的初始姿态下，使用所述陀螺仪数据计算得到所述终端的第二终端姿态；

计算所述第二采样周期的当前采样周期和上一采样周期间的第二姿态角差值；

根据所述单位角度轴和所述第二姿态角差值，计算得到第二姿态变化量；

根据所述第二姿态变化量和所述第二采样周期的周期时间，计算得到第二姿态变化率，其中所述第二姿态变化率和所述第二姿态变化量正相关，所述第二姿态变化率和所述第二采样周期的周期时间负相关。

可选地，所述根据所述第一姿态变化率和所述第二姿态变化率，并结合所述终端当前运动模式下的整定，计算姿态可信度，包括：

根据所述第一终端姿态和所述第二终端姿态，计算得到终端姿态扰动率；

使用扰动率公式，计算得到扰动率，其中所述扰动率公式为： Δ为所述扰动率，Δω1为所述第一姿态变化率；Δω2为所述第二姿态变化率，Δω3为所述两种解算方法偏差率；

确定所述终端当前运动模式下的修正参数和整定参数；

根据整定公式，计算得到姿态可信度，其中所述整定公式为λ＝1/(Δ×修正参数)×整定参数，λ为姿态可信度；

所述根据所述姿态可信度的调节，融合所述第一终端姿态和所述第二终端姿态，以得到所述终端的第三终端姿态，包括：

使用融合公式，计算得到所述终端的第三终端姿态，其中所述融合公式为第三终端姿态＝(θ1,α1,β1)*λ+(θ2,α2,β2)*(1-λ)，(θ1,α1,β1)为所述第一终端姿态，(θ2,α2,β2)为所述第二终端姿态。

可选地，所述根据所述姿态可信度的调节，融合所述第一终端姿态和所述第二终端姿态，以得到所述终端的第三终端姿态之后，所述方法还包括：

确定所述第三终端姿态为所述陀螺仪的初始姿态。

可选地，所述使用融合公式，计算得到所述终端的第三终端姿态之后，所述方法还包括：

当校正周期的当前周期的Δ大于所述校正周期的上一周期的Δ最大值时，在所述校正周期的当前周期的Δ开始下降时，确定所述第三终端姿态为所述陀螺仪的初始姿态；

当校正周期的当前周期的Δ小于所述校正周期的上一周期的Δ最小值时，在所述校正周期的当前周期的Δ开始上升时，确定所述第三终端姿态为所述陀螺仪的初始姿态。

本发明实施例第二方面提供了一种终端，所述终端设有磁力计、加速度计和陀螺仪，所述终端包括：

获取单元，用于在第一采样周期，获取所述磁力计采集的磁力计数据和所述加速度计采集的加速度计数据；

计算单元，用于使用所述磁力计数据和所述加速度计数据，计算得到所述终端的第一终端姿态和第一姿态变化率；

确定单元，用于确定所述陀螺仪的初始姿态；

所述获取单元，还用于在第二采样周期，获取所述陀螺仪采集的陀螺仪数据；

所述计算单元，还用于在所述陀螺仪的初始姿态下，使用所述陀螺仪数据计算得到所述终端的第二终端姿态和第二姿态变化率；

所述计算单元，还用于根据所述第一姿态变化率和所述第二姿态变化率，计算姿态可信度；

所述计算单元，还用于根据所述姿态可信度的调节，融合所述第一终端姿态和所述第二终端姿态，以得到所述终端的第三终端姿态。

可选地，所述计算单元，还用于根据所述第一姿态变化率和所述第二姿态变化率，并结合所述终端当前运动模式下的整定，计算姿态可信度。

可选地，所述计算单元包括计算模块和确定模块；

所述计算模块，用于使用所述磁力计数据和所述加速度计数据，计算得到所述终端的第一终端姿态；

所述确定模块，用于确定所述终端的角度轴尺寸；

所述计算模块，还用于根据所述角度轴尺寸单位化角度轴，得到单位角度轴；

所述计算模块，还用于计算所述第一采样周期的当前采样周期和上一采样周期间的第一姿态角差值；

所述计算模块，还用于根据所述单位角度轴和所述第一姿态角差值，计算得到第一姿态变化量；

所述计算模块，还用于根据所述第一姿态变化量和所述第一采样周期的周期时间，计算得到第一姿态变化率，其中所述第一姿态变化率和所述第一姿态变化量正相关，所述第一姿态变化率和所述第一采样周期的周期时间负相关；

所述计算模块，还用于在所述陀螺仪的初始姿态下，使用所述陀螺仪数据计算得到所述终端的第二终端姿态；

所述计算模块，还用于计算所述第二采样周期的当前采样周期和上一采样周期间的第二姿态角差值；

所述计算模块，还用于根据所述单位角度轴和所述第二姿态角差值，计算得到第二姿态变化量；

所述计算模块，还用于根据所述第二姿态变化量和所述第二采样周期的周期时间，计算得到第二姿态变化率，其中所述第二姿态变化率和所述第二姿态变化量正相关，所述第二姿态变化率和所述第二采样周期的周期时间负相关。

可选地，所述计算单元包括计算模块和确定模块；

所述计算模块，用于根据所述第一终端姿态和所述第二终端姿态，计算得到终端姿态扰动率；

所述计算模块，还用于使用扰动率公式，计算得到扰动率，其中所述扰动率公式为：Δ为所述扰动率，Δ_ω1为所述第一姿态变化率；Δ_ω2为所述第二姿态变化率，Δ_ω3为所述两种解算方法偏差率；

所述确定模块，用于确定所述终端当前运动模式下的修正参数和整定参数；

所述计算模块，还用于根据整定公式，计算得到姿态可信度，其中所述整定公式为λ＝1/(Δ×修正参数)×整定参数，λ为姿态可信度；

所述计算单元，还用于使用融合公式，计算得到所述终端的第三终端姿态，其中所述融合公式为第三终端姿态＝(θ1,α1,β1)*λ+(θ2,α2,β2)*(1-λ)，(θ1,α1,β1)为所述第一终端姿态，(θ2,α2,β2)为所述第二终端姿态。

可选地，所述确定单元，还用于确定所述第三终端姿态为所述陀螺仪的初始姿态。

可选地，所述确定单元，还用于当校正周期的当前周期的Δ大于所述校正周期的上一周期的Δ最大值时，在所述校正周期的当前周期的Δ开始下降时，确定所述第三终端姿态为所述陀螺仪的初始姿态；当校正周期的当前周期的Δ小于所述校正周期的上一周期的Δ最小值时，在所述校正周期的当前周期的Δ开始上升时，确定所述第三终端姿态为所述陀螺仪的初始姿态。

本发明实施例第三方面提供了一种电子设备，包括至少一个处理器；

以及与所述至少一个处理器通信连接的存储器；

其中，所述存储器存储有可被所述至少一个处理器执行的指令程序，所述指令程序被所述至少一个处理器执行，以使所述至少一个处理器能够执行如上述第一方面所述的方法。

本发明实施例第四方面提供了一种在终端中使用的计算机程序产品，所述计算机程序产品包括如上述第二方面所述的功能模块。

本发明实施例提供的技术方案中，在设有磁力计、加速度计和陀螺仪的终端上，在第一采样周期，获取磁力计采集的磁力计数据和加速度计采集的加速度计数据，使用磁力计数据和加速度计数据，计算得到终端的第一终端姿态和第一姿态变化率。以及，确定陀螺仪的初始姿态。在第二采样周期，获取陀螺仪采集的陀螺仪数据，从而在陀螺仪的初始姿态下，使用陀螺仪数据计算得到终端的第二终端姿态和第二姿态变化率。然后，根据第一姿态变化率和第二姿态变化率，计算姿态可信度，以及根据姿态可信度的调节，融合第一终端姿态和第二终端姿态，以得到终端的第三终端姿态。因此相对于现有技术，本发明实施例通过第一姿态变化率和第二姿态变化率对第一终端姿态和第二终端姿态的可信度进行调节，使得融合第一终端姿态和第二终端姿态得到的第三终端姿态准确可用，实现了获取到准确的实时终端姿态。

附图说明

图1为本发明实施例中手机的自然坐标系的示意图；

图2为本发明实施例中地磁场的矢量示意图；

图3为本发明实施例中加速度计的工作示意图；

图4为本发明实施例中陀螺仪的示意图；

图5为本发明另一实施例中一种终端的硬件结构示意图；

图6为本发明另一实施例中一种姿态计算方法的方法流程图；

图7为本发明另一实施例中一种姿态计算方法的方法流程图；

图8为图7所示实施例的另一流程示意图；

图9为图7所示实施例涉及的磁力计矢量示意图；

图10为图7所示实施例涉及的旋转坐标的坐标系示意图；

图11为图7所示实施例涉及的扰动率变化示意图；

图12为本发明另一实施例中一种终端的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为了对本发明的各个实施例有更准确的理解，下面先对本发明的各个实施例涉及到的一些术语进行介绍。

1、捷联惯性导航。捷联惯性导航是一种自主式导航方法，该方法将磁力计、加速度计、陀螺仪安装在终端上，可实现无外部全球定位信号(例如GPS、北斗)，或其它定位技术(例如Wi-Fi，RFID)时，终端通过相关传感器采集的数据进行计算，以实现自主导航。

2、手机的自然坐标系。本发明各个实施例涉及的磁力计与加速度计可采用自然坐标系。当涉及到的终端为手机时，该自然坐标系为手机的自然坐标系，手机的自然坐标系的具体图示可参阅图1，如图1所示，当一个手机被放在其默认的方向上时，X轴是水平指向右的，Y轴是垂直向上的，Z轴是指向屏幕正面之外的，即屏幕背面是Z轴的负值。当设备运动或者旋转的时候，这些坐标轴是不会改变的，即它们是跟随手机的。

可以理解，在本发明的一些实施例中，图1所示的手机可以使用其它的具体终端替代。

3、磁力计(Magnetic、M-Sensor)。磁力计也叫地磁、磁感器，可用于测试磁场强度和方向，定位设备的方位，磁力计的原理跟指南针原理类似，可以测量出当前设备与东南西北四个方向上的夹角

如图2所示，地磁场是一个矢量，对于一个固定的地点来说，这个矢量可以被分解为两个与当地水平面平行的分量和一个与当地水平面垂直的分量。磁力计容易受到外部磁场干扰，且响应水平方向旋转迟缓，无法实时准确测定终端水平方向，无外部磁场干扰，且终端水平角速度小的条件下，磁力计测向准确。

4、加速度计(accelerometer)。加速度计是测量加速度的仪表。

如图3所示，因重力场恒定，在终端静止的前提下，重力矢量方向准确，可获得终端准确倾角。运动状态下，因外部作用力的影响，加速度计难以确定终端的姿态。

5、陀螺仪。陀螺仪是用高速回转体的动量矩敏感壳体相对惯性空间绕正交于自转轴的一个或二个轴的角运动检测装置。利用其他原理制成的角运动检测装置起同样功能的也称陀螺仪。

如图4所示，陀螺仪优势在于测量终端自身的旋转运动，不受外部磁场干扰，对角度测算不受终端非转向力影响。但陀螺仪自身不能确定终端的方位，需要通过角速度积分获得角度值，长时间存在积分积累误差，需要外部传感器进行矫正。

图5为本发明实施例提供的一种终端的结构示意图。本发明实施例涉及的姿态计算方法可应用于该终端上。

参阅图5，本实施例的终端包括处理器501、存储器502、磁力计503、加速度计504和陀螺仪505，这些部件可通过总线或者其他方式连接，图5中以通过总线连接为例。

存储器502作为一种非易失性计算机可读存储介质，可用于存储非易失性软件程序、非易失性计算机可执行程序以及模块。处理器501通过运行存储在存储器502中的非易失性软件程序、指令以及模块，从而执行终端的各种功能应用以及数据处理，即实现下述方法实施例中的姿态计算方法。

存储器502可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储创建的数据等。此外，存储器502也可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中，存储器502可选包括相对于处理器501远程设置的存储器，这些远程存储器可以通过网络连接至终端。该网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

磁力计503、加速度计504和陀螺仪505可参考上文的术语介绍部分的描述。

其中，磁力计503可采集到磁力计数据，加速度计504可采集到加速度计数据，以及陀螺仪505可采集到陀螺仪数据，这些磁力计数据、加速度计数据和陀螺仪数据可通过总线传送到处理器501上，然后处理器501读取存储器502上的指令，执行本发明各实施例提供的姿态计算方法。

可以理解，本发明实施例的终端包括但不限于手机、机器人，智能小车，无人飞机等。

可以理解，图5所示的终端结构只是示例性说明，图5并不对本发明实施例的终端构成限定，例如，图5只示出了一个处理器、一个存储器等，在有的实施例中，处理器、存储器、磁力计、加速度计和陀螺仪也可以为多个。

图6为本发明实施例提供的一种姿态计算方法的方法流程图。该姿态计算方法可应用于图5所示的终端上，其中，该终端设有磁力计、加速度计和陀螺仪。

参阅图6，本实施例的姿态计算方法包括：

步骤601：在第一采样周期，获取磁力计采集的磁力计数据和加速度计采集的加速度计数据。

其中，磁力计数据为磁力计采集的数据，反映了终端的姿态信息，磁力计数据可以为一矢量；加速度计数据为加速度计采集的数据，反映了终端的姿态信息，加速度计数据可以为一矢量。第一采样周期为终端获取磁力计采集的磁力计数据和加速度计采集的加速度计数据的周期。

终端获取到第一采样周期的一个周期内的磁力计采集的磁力计数据和加速度计采集的加速度计数据，以用于计算终端的终端姿态和姿态变化率。

步骤602：使用磁力计数据和加速度计数据，计算得到终端的第一终端姿态和第一姿态变化率。

其中，第一终端姿态为根据磁力计数据和加速度计数据计算得到的终端的姿态信息，该第一终端姿态具体可以为角度值。第一姿态变化率为第一采样周期的当前周期对应的第一终端姿态和上一周期对应的第一终端姿态的差值。周期对应的第一终端姿态表示通过在该周期采集的磁力计数据和加速度计数据，计算得到的第一终端姿态。

终端获取到磁力计数据和加速度计数据后，即可使用磁力计数据和加速度计数据，计算得到终端的第一终端姿态和第一姿态变化率。

此时，因外部磁场干扰，或响应水平方向旋转迟缓，使得磁力计数据可能不够准确；和/或运动状态下，因外部作用力的影响，使得加速度计数据可能不够准确。计算得到的第一终端姿态不是实际的准确数据。

步骤603：确定陀螺仪的初始姿态。

通过陀螺仪采集的数据，来计算出终端的终端姿态需要初始的姿态角度来配合方行，为此，终端需要确定陀螺仪的初始姿态。

确定了陀螺仪的初始姿态后，结合陀螺仪获取的陀螺仪数据进行计算，可以得到终端的第二终端姿态。

在本发明实施例中，步骤603包括多种具体的实现方式，下文即举出部分示例。

示例一：终端确定第一终端姿态为陀螺仪的初始姿态。

在步骤602确定出终端的第一终端姿态后，即可使用该第一终端姿态为陀螺仪的初始姿态。

示例二：终端确定上一次计算周期计算得到的第三终端姿态为陀螺仪的初始姿态。

本实施例的终端可以周期性执行步骤601-步骤607，这些步骤执行完成一次为一计算周期，在上一计算周期的步骤607中计算得到第三终端姿态后，可以使用该第三终端姿态作为陀螺仪的初始姿态。

示例三：终端确定上一次计算周期计算得到的第二终端姿态为陀螺仪的初始姿态。

在上一计算周期的步骤605中，可以计算得到第二终端姿态，终端可以确定该第二终端姿态为陀螺仪的初始姿态。

步骤604：在第二采样周期，获取陀螺仪采集的陀螺仪数据。

陀螺仪采集的数据为陀螺仪数据，其可为矢量数据。第二采样周期为终端获取陀螺仪采集的陀螺仪数据的周期。

在第二采样周期，终端获取陀螺仪采集的陀螺仪数据，以用于计算得到终端的第二终端姿态和第二姿态变化率。

可以理解，第一采样周期和第二采样周期可以相同，也可以不相同，本实施例对此不作具体限定。

步骤605：在陀螺仪的初始姿态下，使用陀螺仪数据计算得到所终端的第二终端姿态和第二姿态变化率。

其中，第二终端姿态为终端根据陀螺仪的初始姿态，对陀螺仪数据进行计算后得到的终端姿态，该第二终端姿态具体可以为角度值。第二姿态变化率为第二采样周期的当前周期对应的第二终端姿态和上一周期对应的第二终端姿态的差值。周期对应的第二终端姿态表示通过在该周期采集的陀螺仪数据，在陀螺仪的初始姿态下计算得到的第二终端姿态。

终端获取到陀螺仪的初始姿态和陀螺仪数据后，即可在陀螺仪的初始姿态下，使用陀螺仪数据计算得到终端的第二终端姿态和第二姿态变化率。

此时，因可能因长时间计算使得陀螺仪存在积分积累误差，通过陀螺仪数据计算得到的第二终端姿态可能不够准确。

步骤606：根据第一姿态变化率和第二姿态变化率，计算姿态可信度。

姿态可信度用于表示第一终端姿态和第二终端姿态的可信度，可信度高的终端姿态更加接近终端的实际姿态。

因第一姿态变化率反映了通过第一采样周期采集的数据计算得到的第一终端姿态的变化特征；第二姿态变化率反映了通过第二采样周期采集的数据计算得到的第二终端姿态的变化特征，从而可以终端可以根据第一姿态变化率和第二姿态变化率，计算姿态可信度。

步骤607：根据姿态可信度的调节，融合第一终端姿态和第二终端姿态，以得到终端的第三终端姿态。

获取到可信度后，终端即可根据姿态可信度的调节，融合第一终端姿态和第二终端姿态，以得到终端的第三终端姿态。

因根据姿态可信度的调节，融合了第一终端姿态和第二终端姿态，利用磁力计、加速度计和陀螺仪的优点，屏蔽其缺点，使得计算得到的第三终端姿态更加准确。

可以理解，步骤607的具体实现方式有多种，下面举出其中几个示例：

示例一：如图7所示的实施例所示，终端根据第一姿态变化率和第二姿态变化率，计算出姿态可信度λ后，使用该λ确定第一终端姿态和第二终端姿态的比重，以使用具体比重的第一终端姿态和第二终端姿态进行相加得到终端的第三终端姿态。

示例二：终端根据第一姿态变化率和第二姿态变化率，计算出姿态可信度后，根据姿态可信度在预设的对应表中确定第一终端姿态和第二终端姿态的比重，以使用具体比重的第一终端姿态和第二终端姿态进行相加得到终端的第三终端姿态。

示例三：终端根据第一姿态变化率和第二姿态变化率，计算出姿态可信度后，根据姿态可信度在第一终端姿态和第二终端姿态中选择其中之一，具体为选择可信度高的终端姿态为第三终端姿态。

可选地，因通过上述步骤计算得到的第三终端姿态更加准确，为了消除陀螺仪累计的积分误差，在本发明的一些实施例中，步骤607之后，姿态计算方法还包括：确定第三终端姿态为陀螺仪的初始姿态。

在具体的实施例中，可以确定具体的校正时刻或者校正条件，以确定第三终端姿态为陀螺仪的初始姿态。

陀螺仪特点是响应速度快，短时间求出的第二终端姿态的准确性比上述的第一终端姿态更精准，但长时间存在累计误差，所以需要定期重置陀螺仪初始姿态。

综上所述，本发明实施例提供的技术方案中，在设有磁力计、加速度计和陀螺仪的终端上，在第一采样周期，获取磁力计采集的磁力计数据和加速度计采集的加速度计数据，使用磁力计数据和加速度计数据，计算得到终端的第一终端姿态和第一姿态变化率。以及，确定陀螺仪的初始姿态。在第二采样周期，获取陀螺仪采集的陀螺仪数据，从而在陀螺仪的初始姿态下，使用陀螺仪数据计算得到终端的第二终端姿态和第二姿态变化率。然后，根据第一姿态变化率和第二姿态变化率，计算姿态可信度，以及根据姿态可信度的调节，融合第一终端姿态和第二终端姿态，以得到终端的第三终端姿态。因此相对于现有技术，本发明实施例通过第一姿态变化率和第二姿态变化率对第一终端姿态和第二终端姿态的可信度进行调节，使得融合第一终端姿态和第二终端姿态得到的第三终端姿态准确可用，实现了获取到准确的实时终端姿态。

图7为本发明实施例提供的一种姿态计算方法的方法流程图，图7所示的方法可以基于图6所示的方法实现，图7所示的方法可以应用于图5所示的终端上，该终端设有磁力计、加速度计和陀螺仪。本实施例还提供了图8，以对图7所示的方法进行辅助性说明。

参阅图7，本实施例的方法包括：

步骤701：在第一采样周期，获取磁力计采集的磁力计数据和加速度计采集的加速度计数据。

其中，磁力计数据为磁力计采集的数据，反映了终端的姿态信息；加速度计数据为加速度计采集的数据，反映了终端的姿态信息。例如，磁力计数据为磁力计矢量，其中磁力计测量值＝(Xm,Ym,Zm)，磁力计矢量的具体图示可参阅图9。加速度计数据为加速度计矢量，其中加速度计测量值＝(Xa,Ya,Za)。在磁力计数据为磁力计矢量、加速度计数据为加速度计矢量时，磁力计矢量和加速度计矢量可以采用终端的自然坐标系进行表示，例如采用手机自然坐标系。

第一采样周期为终端获取磁力计采集的磁力计数据和加速度计采集的加速度计数据的周期。例如，第一采样周期可以为0.3秒、1秒、3秒等。

终端在第一采样周期，获取磁力计采集的磁力计数据和加速度计采集的加速度计数据后，便可使用磁力计数据和加速度计数据进行计算求解。

可以理解，终端可以同时获取磁力计数据和加速度计数据，也可以分时获取磁力计数据和加速度计数据。

步骤702：使用磁力计数据和加速度计数据，计算得到终端的第一终端姿态和第一姿态变化率。

其中，第一终端姿态为根据磁力计数据和加速度计数据计算得到的终端的姿态信息。第一姿态变化率为第一采样周期的当前周期对应的第一终端姿态和上一周期对应的第一终端姿态的差值。

终端获取到磁力计数据和加速度计数据后，即可以使用磁力计数据和加速度计数据，计算得到终端的第一终端姿态和第一姿态变化率。

在磁力计数据为磁力计矢量、加速度计数据为加速度计矢量时，具体的计算方法如下：

1.1对磁力计矢量进行处理；

磁力计测量值为(Xm,Ym,Zm)；

磁力计模M为

磁力计矢量(单位化)＝(Xm,Ym,Zm)/M＝(Xm1,Ym1,Zm1)；

1.2对加速度计矢量进行处理；

加速度计测量值为(Xa,Ya,Za)；

加速度计模N为

加速度计矢量(单位化)＝(Xa,Ya,Za)/N＝(Xa1,Ya1,Za1)。

1.3水平矢量测算；

水平矢量＝磁力计矢量X加速度计矢量＝(Ym1*Za1–Zm1*Ya1,Zm1*Xa1–Xm1*Za1,Xm1*Ya1–Ym1*Xa1)＝(Xh,Yh,Zh)；

水平矢量模

水平矢量(单位化)＝(Xh,Yh,Zh)/L＝(Xh1,Yh1,Zh1)。

1.4反向计算磁力计矢量；

反向计算磁力计矢量值(Xm2,Ym2,Zm2)＝(Xa1,Ya1,Za1)×(Xh1,Yh1,Zh1)。

1.5方向角测算

从而根据旋转矩阵获得旋转坐标，其中旋转坐标的坐标系如图10所示。

这样，可计算得到第一终端姿态：

values[0]＝atan2(Yh1,Ym2)＝θ

values[1]＝asin(-Ya1)＝α

values[2]＝atan2(-Xa1,Za1)＝β

其中：

values[0]表示:方位角，绕Z轴旋转(azimuth,rotation around the Z axis)。

values[1]:表示俯仰，绕X轴旋转(pitch,rotation around the X axis)。

values[2]:表示横滚，绕Y轴旋转(roll,rotation around the Y axis)。

atan表示:反正切函数。

上述过程实现了使用磁力计数据和加速度计数据，计算得到终端的第一终端姿态。

关于第一姿态变化率的计算如下：

步骤A1：确定终端的角度轴尺寸；

步骤A2：根据角度轴尺寸单位化角度轴，得到单位角度轴；

步骤A3：计算第一采样周期的当前采样周期和上一采样周期间的第一姿态角差值；

步骤A4：根据单位角度轴和第一姿态角差值，计算得到第一姿态变化量；

步骤A5：根据第一姿态变化量和第一采样周期的周期时间，计算得到第一姿态变化率，其中第一姿态变化率和第一姿态变化量正相关，第一姿态变化率和第一采样周期的周期时间负相关。

具体示例如下：

1.6变化率测算；

因变化率扰动与对应旋转轴长度相关，所以需要计算绕轴尺寸，如下所示。

θ轴：终端y轴向长度h；

α轴：终端y轴向长度h；

β轴：终端X轴向长度l。

然后，单位化角度轴(θr，αr,βr)；

上一第一采样周期计算得到的第一终端姿态为(θ1,α1,β1)；

当前第一采样周期计算得到的第一终端姿态为(θ2,α2,β2)；

当前第一采样周期计算得到的第一终端姿态减去上一第一采样周期计算得到的第一终端姿态，得到角度差值(Δθ，Δα，Δβ)。

终端姿态变化量ΔΦ＝

从而，可计算出第一姿态变化率Δω＝ΔΦ/Δt。其中，Δt为采样周期(第一姿态变化率对应第一采样周期，第二姿态变化率对应第二采样周期)。

获得的变化率能均衡终端尺寸带来的差异，均衡计算三个方向角的变化率。从而实现均衡校正三个方向角。

步骤703：确定第一终端姿态为陀螺仪的初始姿态。

在本实施例中，终端计算得到第一终端姿态后，可以确定第一终端姿态为陀螺仪的初始姿态，以实现对陀螺仪的初始化。

陀螺仪测量值为终端三个轴向的旋转的角速度，其独立无法得出终端姿态，所以需要有初始的姿态角度，然后，再通过各方向的角速度变化，以得出最新的终端姿态。

步骤704：在第二采样周期，获取陀螺仪采集的陀螺仪数据。

陀螺仪采集的数据为陀螺仪数据，其可为矢量数据，例如为三个轴向的旋转的角速度。第二采样周期为终端获取陀螺仪采集的陀螺仪数据的周期。

在第二采样周期，终端获取陀螺仪采集的陀螺仪数据，以用于计算得到终端的第二终端姿态和第二姿态变化率。

可以理解，第一采样周期和第二采样周期可以相同，也可以不相同，本实施例对此不作具体限定。

在本发明有的实施例中，还提供了步骤703和步骤704的具体实现方式，具体如下：

第二采样周期也可称之为陀螺仪采样周期。终端进入陀螺仪采样周期，采集陀螺仪数据，然后，判断磁力计加速度解算的第一终端姿态是否初始化，若否，则终端进入下一陀螺仪采样周期，采集陀螺仪数据，否则，终端判断陀螺仪是否初始化。若陀螺仪未初始化，则终端获取第一终端姿态，并通过第一终端姿态进行陀螺仪姿态初始化，然后再执行陀螺仪初始化时的步骤；若陀螺仪初始化，则判断由校正开关是否为真，若为否，则执行步骤705；若为真，则获取上一第二采样周期的第三终端姿态，使用该第三终端姿态进行陀螺仪姿态初始化，然后，执行步骤705。

步骤705：在陀螺仪的初始姿态下，使用陀螺仪数据计算得到终端的第二终端姿态和第二姿态变化率。

其中，第二终端姿态为终端根据陀螺仪的初始姿态，对陀螺仪数据进行计算后得到的终端姿态，该第二终端姿态具体为角度值。第二姿态变化率为第二采样周期的当前周期对应的第二终端姿态和上一周期对应的第二终端姿态的差值。

终端获取到陀螺仪的初始姿态和陀螺仪数据后，即可在陀螺仪的初始姿态下，使用陀螺仪数据计算得到终端的第二终端姿态和第二姿态变化率。

举例来说，可采用四元数集合角速度积分方式，根据陀螺仪数据解算终端姿态，即步骤705的具体实现过程如下：

2.1陀螺仪姿态初始化；

设第一终端姿态为方向角：(θ,α,β)。

求取旋转矩阵，其中f表示浮点数。旋转矩阵如下：

X轴

xM[0]＝1.0f；xM[1]＝0.0f；xM[2]＝0.0f；

xM[3]＝0.0f；xM[4]＝cosα；xM[5]＝sinα；

xM[6]＝0.0f；xM[7]＝-sinα；xM[8]＝cosα；

Y轴

yM[0]＝cosβ；yM[1]＝0.0f；yM[2]＝sinβ；

yM[3]＝0.0f；yM[4]＝1.0f；yM[5]＝0.0f；

yM[6]＝-sinβ；yM[7]＝0.0f；yM[8]＝cosβ；

Z轴

zM[0]＝cosθ；zM[1]＝sinθ；zM[2]＝0.0f；

zM[3]＝-sinθ；zM[4]＝cosθ；zM[5]＝0.0f；

zM[6]＝0.0f；zM[7]＝0.0f；zM[8]＝1.0f；

计算初始第二终端姿态的矩阵M2＝Z轴×X轴×Y轴。

2.2陀螺仪旋转四元数。

陀螺仪采集的陀螺仪数据为陀螺仪角速度：(ωθ，ωα，ωβ)；

角速度模

角速度(单位化)＝(ωθ，ωα，ωβ)/ωm＝(ωθ1，ωα1，ωβ1)；

计算角度变化Φ＝ωm*Δt/2；其中，Δt为本次采集的时间戳减去上次的采集的时间戳。

四元数：(sin(Φ)*ωθ1,sin(Φ)*ωα1,sin(Φ)*ωβ1,cos(Φ))。

2.3陀螺仪四元数解算终端姿态。

其中：

更新第二终端姿态矩阵M2＝R(q)*M2；

计算得到第二终端姿态为：

values[0]＝atan2(Yh1,Ym2)＝θ

values[1]＝asin(-Ya1)＝α

values[2]＝atan2(-Xa1,Za1)＝β

上述过程实现了在陀螺仪的初始姿态下，使用陀螺仪数据计算得到终端的第二终端姿态。

关于第二姿态变化率的计算过程类似于第一姿态变化率的计算，即包括的步骤如下：计算第二采样周期的当前采样周期和上一采样周期间的第二姿态角差值；然后，根据单位角度轴和第二姿态角差值，计算得到第二姿态变化量；以及根据第二姿态变化量和第二采样周期的周期时间，计算得到第二姿态变化率，其中第二姿态变化率和第二姿态变化量正相关，第二姿态变化率和第二采样周期的周期时间负相关。

具体示例如下：

2.4陀螺仪变化率测算

具体方式可参见上述的1.6部分，其中，1.6部分中的第一采样周期此时为第二采样周期，第一终端姿态此时为第二终端姿态，第一姿态变化率此时为第二终端姿态。

步骤706：根据第一姿态变化率和第二姿态变化率，并结合终端当前运动模式下的整定，计算姿态可信度。

其中，运动模式为终端所处的运动模式，例如具体可以为车载模式、步行模式、奔跑模式等，车载模式表示终端位于车等移动设备上，步行模式表示步行的用户携带着终端，奔跑模式表示奔跑的用户携带着终端。

姿态可信度用于表示第一终端姿态和第二终端姿态的可信度，可信度高的终端姿态更加接近终端的实际姿态。

因第一姿态变化率反映了通过第一采样周期采集的数据计算得到的第一终端姿态的变化特征；第二姿态变化率反映了通过第二采样周期采集的数据计算得到的第二终端姿态的变化特征，从而可以终端可以根据第一姿态变化率和第二姿态变化率，计算姿态可信度。并且因终端在不同的运动模式下，对磁力计、加速度计和陀螺仪有不同的影响效果，从而第一终端姿态和第二终端姿态的准确性也不同，为此可以结合第一姿态变化率和第二姿态变化率以及终端当前运动模式，计算出姿态可信度。

可以理解，步骤706为根据第一姿态变化率和第二姿态变化率，计算姿态可信度的步骤的具体实现方式之一。

步骤707：根据姿态可信度的调节，融合第一终端姿态和第二终端姿态，以得到终端的第三终端姿态。

终端获取到姿态可信度后，可以根据姿态可信度确定第一终端姿态和第二终端姿态分别和终端实际姿态的符合程度，从而融合第一终端姿态和第二终端姿态，以得到终端的第三终端姿态。

因根据姿态可信度的调节，融合了第一终端姿态和第二终端姿态，利用磁力计、加速度计和陀螺仪的优点，屏蔽其缺点，使得计算得到的第三终端姿态更加准确。

关于步骤706和步骤707的具体实现方式，举例如下：

步骤706具体包括：

步骤B1：根据第一终端姿态和第二终端姿态，计算得到两种解算方法偏差率Δω3。

具体来说过程如下：

3.1扰动率测算；

其中，通过上述步骤的计算得到：

第一终端姿态的方向角：(θ1,α1,β1)；

第二终端姿态的方向角：(θ2,α2,β2)；

第一姿态变化率：Δω1；

第二姿态变化率：Δω2；

其中，第一姿态变化率和第二姿态变化率按上述的1.6部分求解。

然后，即可根据第一终端姿态和第二终端姿态，计算得到两种解算方法偏差率Δω3，示例如下：

其中，θ轴：终端y轴向长度h；

α轴：终端y轴向长度h；

β轴：终端X轴向长度l；

然后，单位化角度轴(θr，αr,βr)。

计算第一终端姿态最近两个采样周期间的变化量，得到角度差值(Δθ1，Δα1，Δβ1)。

计算第二终端姿态最近两个采样周期间的变化量，得到角度差值(Δθ2，Δα2，Δβ2)。

可以理解，上述对单位化角度轴、第一终端姿态的角度差值和第二终端姿态的角度差值都可以参考上述的1.6部分的内容。在有的实施例中，因在上述步骤中已经计算得到了单位化角度轴、第一终端姿态的角度差值和第二终端姿态的角度差值，从而在计算终端姿态变化率Δω3时，可以直接使用这些已知的数据。

然后，终端可计算第一终端姿态和第二终端姿态的变化偏差，(Δθ3＝Δθ2-Δθ1，Δα3＝Δα2-Δα1，Δβ3＝Δβ2-Δβ1)；从而可以通过如下公式计算姿态变化量ΔΦ：

然后，即可根据上述结果计算出两种解算方法偏差率Δω3＝ΔΦ/Δt，其中，此处的Δt在第一采样周期和第二采样周期相同的实施例中，Δt为第一采样周期或第二采样周期；在第一采样周期和第二采样周期不相同的实施例中，Δt为第一采样周期和第二采样周期的平均值。

这样，两种解算方法偏差率Δω3反应出第一终端姿态和第二终端姿态，测算的角度差值的偏差。

步骤B2：使用扰动率公式，计算得到终端姿态扰动率。

其中，扰动率公式为：

Δ为扰动率，Δω1为第一姿态变化率；Δω2为第二姿态变化率，Δω3为两种解算方法偏差率。

其中，扰动率越小则加速度计和磁力计测算的第一终端姿态准确率越高，反之越小。扰动率越高陀螺仪对第二终端姿态的准确率越高，反之越小，从而使用扰动率可实现互补滤波。

步骤B3：确定终端当前运动模式下的修正参数和整定参数。

终端根据不同的运动模式可以确定出不同修正参数和整定参数，终端确定出当前运动模式后，即可确定出与当前当前运动模式对应的预设的修正参数和整定参数。

其中，对终端当前运动模式的确定，可以通过按校正周期，根据周期内Δ最小值进行切换。

例如，车载模式参数整定：平稳匀速Δ最小值，终端姿态1可信度最高值0.9；行走模式参数整定：正常行走手持，及口袋，Δ最小值，终端姿态1可信度最高值0.7；奔跑模式参数整定：正常奔跑手持，及口袋，Δ最小值，终端姿态1可信度最高值0.5。

步骤B4：根据整定公式，计算得到姿态可信度。

其中，整定公式为λ＝1/(Δ×修正参数)×整定参数。λ为姿态可信度。

相应地，步骤707具体包括：使用融合公式，计算得到终端的第三终端姿态。

其中，融合公式为第三终端姿态＝(θ1,α1,β1)*λ+(θ2,α2,β2)*(1-λ)。(θ1,α1,β1)为第一终端姿态，(θ2,α2,β2)为第二终端姿态。

这样，即可计算出第三终端姿态。通过计算姿态变化率，来修正λ值。可以实现在终端没有较大旋转动作时，磁力计与加速度计获得的姿态有较高的准确性。当旋转动作大时，因为磁力计响应速度慢，由陀螺仪计算获取的姿态准确性高。

可选地，因通过上述步骤计算得到的第三终端姿态更加准确，为了消除陀螺仪累计的积分误差，在本发明的一些实施例中，步骤707之后，姿态计算方法还包括：确定第三终端姿态为陀螺仪的初始姿态。

陀螺仪通过对角速度积分获得终端姿态，积分随着时间的退推移，误差逐步放大，且终端运动越剧烈，陀螺仪积分的误差将越大。需要周期性通过第三终端姿态初始化陀螺仪姿态，从而消除陀螺仪积分误差对后续姿态解算的影响。

本实施例可以对变化率按周期跟踪，进行陀螺仪姿态校正，消除积分误差。

在一具体实现方式中，在上述的使用融合公式，计算得到终端的第三终端姿态的步骤之后，确定第三终端姿态为陀螺仪的初始姿态的步骤，包括：

当校正周期的当前周期的Δ大于校正周期的上一周期的Δ最大值时，在校正周期的当前周期的Δ开始下降时，确定第三终端姿态为陀螺仪的初始姿态；

当校正周期的当前周期的Δ小于校正周期的上一周期的Δ最小值时，在校正周期的当前周期的Δ开始上升时，确定第三终端姿态为陀螺仪的初始姿态。

例如，终端运动的任何模式下：扰动率Δ存在一定的周期性，具体如图11所示。其中，在图11中，波形上沿，随着扰动率的增加，陀螺仪积分误差累积加大，适合最高点时进行陀螺仪姿态修正。波形下沿，随着扰动率的降小，第三终端姿态准确率上升，适合最低点时进行陀螺仪姿态修正。为此，具体的校正过程如下：

1)初始化：1秒为周期记录扰动率，初始化Δ最大值，Δ最小值。

2)更新周期：校正周期为1秒，如Δ最大值，超过上一校正周期最大值，进入最大值跟踪，当当前采样Δ最大值小于上一采样时进行校正，即确定第三终端姿态为陀螺仪的初始姿态，并继续更新本周期Δ最大值作为初始Δ最大值；如Δ最小值，低于上一校正周期最小值，进入最小值跟踪，当当前采样Δ最小值大于上一采样时进行校正，即确定第三终端姿态为陀螺仪的初始姿态，并继续更新本周期Δ最小值作为初始Δ最小值；如本更新周期未满足前述两种情况，将本校正周期Δ最大值，Δ最小值作为初始最大值与最小值，本校正周期不做校正。

在图7所示实施例的方法中，考虑了终端运动模式，实时计算三种传感器(磁力计、加速度计和陀螺仪)运动扰动因素，动态计算测量值的可信度，实时修正姿态，并结合运动扰动状态计算陀螺仪最佳修正时机，定期纠正陀螺仪初始状态，降低陀螺仪积分积累误差。可优化车载模式，走动模式中间姿态，从而优化终端运动对姿态带来的干扰，为控制通过运动加速度积分得到终端运动轨迹误差提供最优保障。

具体来说，图7所示实施例的方法将能产生如下有益效果：

1、本实施例计算得到的第三终端姿态，对终端外部随机动作具有较强稳定性，并能屏蔽瞬间的外部磁场干扰。

2、本实施例计算得到的第三终端姿态，在终端处于步行与奔跑模式长时间运动状态下，能避免陀螺仪因长时间运动模式积分误差从而终端姿态失效。通过周期性校正，保证解算的终端姿态收敛于准确的实际终端姿态。

3、本实施例计算得到的第三终端姿态，在车载模式下，无外部磁场干扰状态下，具备较高精确度，为进行步惯性导航轨迹获取提供保障，轨迹误差3％以内。

4、本实施例计算得到的第三终端姿态，结合高精度陀螺仪，磁力计，加速度计，可满足步行与奔跑模式的惯性导航要求，轨迹误差5％以内。

5、本实施例计算得到的第三终端姿态，可广泛应用于机器人、无人机室内轨迹定位与控制，人与移动终端室内定位导航，具有广泛的社会经济应用价值。

综上所述，本发明实施例提供的技术方案中，在设有磁力计、加速度计和陀螺仪的终端上，在第一采样周期，获取磁力计采集的磁力计数据和加速度计采集的加速度计数据，使用磁力计数据和加速度计数据，计算得到终端的第一终端姿态和第一姿态变化率。以及，确定陀螺仪的初始姿态。在第二采样周期，获取陀螺仪采集的陀螺仪数据，从而在陀螺仪的初始姿态下，使用陀螺仪数据计算得到终端的第二终端姿态和第二姿态变化率。然后，根据第一姿态变化率和第二姿态变化率，计算姿态可信度，以及根据姿态可信度的调节，融合第一终端姿态和第二终端姿态，以得到终端的第三终端姿态。因此相对于现有技术，本发明实施例通过第一姿态变化率和第二姿态变化率对第一终端姿态和第二终端姿态的可信度进行调节，使得融合第一终端姿态和第二终端姿态得到的第三终端姿态准确可用，实现了获取到准确的实时终端姿态。

图12为本发明实施例提供的一种终端的结构示意图，该终端的各功能模块可集成在图5所示的终端上，图12所示的终端可用于执行上述的图6和图7所示的姿态计算方法。图12所示的终端设有磁力计、加速度计和陀螺仪。

参阅图12，本实施例的终端包括：

获取单元1201，用于在第一采样周期，获取磁力计采集的磁力计数据和加速度计采集的加速度计数据；

计算单元1202，用于使用磁力计数据和加速度计数据，计算得到终端的第一终端姿态和第一姿态变化率；

确定单元1203，用于确定陀螺仪的初始姿态；

获取单元1201，还用于在第二采样周期，获取陀螺仪采集的陀螺仪数据；

计算单元1202，还用于在陀螺仪的初始姿态下，使用陀螺仪数据计算得到终端的第二终端姿态和第二姿态变化率；

计算单元1202，还用于根据第一姿态变化率和第二姿态变化率，计算姿态可信度；

计算单元1202，还用于根据姿态可信度的调节，融合第一终端姿态和第二终端姿态，以得到终端的第三终端姿态。

可选地，计算单元1202，还用于根据第一姿态变化率和第二姿态变化率，并结合终端当前运动模式下的整定，计算姿态可信度。

可选地，计算单元1202包括计算模块1204和确定模块1205；

计算模块1204，用于使用磁力计数据和加速度计数据，计算得到终端的第一终端姿态；

确定模块1205，用于确定终端的角度轴尺寸；

计算模块1204，还用于根据角度轴尺寸单位化角度轴，得到单位角度轴；

计算模块1204，还用于计算第一采样周期的当前采样周期和上一采样周期间的第一姿态角差值；

计算模块1204，还用于根据单位角度轴和第一姿态角差值，计算得到第一姿态变化量；

计算模块1204，还用于根据第一姿态变化量和第一采样周期的周期时间，计算得到第一姿态变化率，其中第一姿态变化率和第一姿态变化量正相关，第一姿态变化率和第一采样周期的周期时间负相关；

计算模块1204，还用于在陀螺仪的初始姿态下，使用陀螺仪数据计算得到终端的第二终端姿态；

计算模块1204，还用于计算第二采样周期的当前采样周期和上一采样周期间的第二姿态角差值；

计算模块1204，还用于根据单位角度轴和第二姿态角差值，计算得到第二姿态变化量；

计算模块1204，还用于根据第二姿态变化量和第二采样周期的周期时间，计算得到第二姿态变化率，其中第二姿态变化率和第二姿态变化量正相关，第二姿态变化率和第二采样周期的周期时间负相关。

可选地，计算单元1202包括计算模块1204和确定模块1205；

计算模块1204，用于根据第一终端姿态和第二终端姿态，计算得到两种解算方法偏差率；

计算模块1204，还用于使用扰动率公式，计算得到扰动率，其中扰动率公式为：Δ为扰动率，Δω1为第一姿态变化率；Δω2为第二姿态变化率，Δω3为两种解算方法偏差率；

确定模块1205，用于确定终端当前运动模式下的修正参数和整定参数；

计算模块1204，还用于根据整定公式，计算得到姿态可信度，其中整定公式为λ＝1/(Δ×修正参数)×整定参数，λ为姿态可信度；

计算单元1204，还用于使用融合公式，计算得到终端的第三终端姿态，其中融合公式为第三终端姿态＝(θ1,α1,β1)*λ+(θ2,α2,β2)*(1-λ)，(θ1,α1,β1)为第一终端姿态，(θ2,α2,β2)为第二终端姿态。

可选地，确定单元1203，还用于确定第三终端姿态为陀螺仪的初始姿态。

可选地，确定单元1203，还用于当校正周期的当前周期的Δ大于校正周期的上一周期的Δ最大值时，在校正周期的当前周期的Δ开始下降时，确定第三终端姿态为陀螺仪的初始姿态；当校正周期的当前周期的Δ小于校正周期的上一周期的Δ最小值时，在校正周期的当前周期的Δ开始上升时，确定第三终端姿态为陀螺仪的初始姿态。

综上所述，本发明实施例提供的技术方案中，在设有磁力计、加速度计和陀螺仪的终端上，获取单元1201在第一采样周期，获取磁力计采集的磁力计数据和加速度计采集的加速度计数据，计算单元1202使用磁力计数据和加速度计数据，计算得到终端的第一终端姿态和第一姿态变化率。以及，确定单元1203确定陀螺仪的初始姿态。获取单元1201在第二采样周期，获取陀螺仪采集的陀螺仪数据，从而计算单元1202在陀螺仪的初始姿态下，使用陀螺仪数据计算得到终端的第二终端姿态和第二姿态变化率。然后，计算单元1202根据第一姿态变化率和第二姿态变化率，计算姿态可信度，以及计算单元1202根据姿态可信度的调节，融合第一终端姿态和第二终端姿态，以得到终端的第三终端姿态。因此相对于现有技术，本发明实施例通过第一姿态变化率和第二姿态变化率对第一终端姿态和第二终端姿态的可信度进行调节，使得融合第一终端姿态和第二终端姿态得到的第三终端姿态准确可用，实现了获取到准确的实时终端姿态。

本申请实施例还提供了一种电子设备，该电子设备的硬件结构示意图可参考图5所示的终端的硬件结构示意图。

该电子设备可执行本申请实施例所提供的方法，具备执行图12所示的终端相应的功能模块和有益效果。未在本实施例中详尽描述的技术细节，可参见本申请实施例所提供的终端或方法。

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

通过以上的实施方式的描述，本领域普通技术人员可以清楚地了解到各实施方式可借助软件加通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种姿态计算方法，其特征在于，所述方法应用于终端，所述终端设有磁力计、加速度计和陀螺仪，所述方法包括：

在第一采样周期，获取所述磁力计采集的磁力计数据和所述加速度计采集的加速度计数据；

使用所述磁力计数据和所述加速度计数据，计算得到所述终端的第一终端姿态和第一姿态变化率；

确定所述陀螺仪的初始姿态；

在第二采样周期，获取所述陀螺仪采集的陀螺仪数据；

在所述陀螺仪的初始姿态下，使用所述陀螺仪数据计算得到所述终端的第二终端姿态和第二姿态变化率；

根据所述第一姿态变化率和所述第二姿态变化率，计算姿态可信度；

根据所述姿态可信度的调节，融合所述第一终端姿态和所述第二终端姿态，以得到所述终端的第三终端姿态。

2.根据权利要求1所述的方法，其特征在于，

所述根据所述第一姿态变化率和所述第二姿态变化率，计算姿态可信度，包括：

根据所述第一姿态变化率和所述第二姿态变化率，并结合所述终端当前运动模式下的整定，计算姿态可信度。

3.根据权利要求1所述的方法，其特征在于，

所述使用所述磁力计数据和所述加速度计数据，计算得到所述终端的第一终端姿态和第一姿态变化率，包括：

使用所述磁力计数据和所述加速度计数据，计算得到所述终端的第一终端姿态；

确定所述终端的角度轴尺寸；

根据所述角度轴尺寸单位化角度轴，得到单位角度轴；

计算所述第一采样周期的当前采样周期和上一采样周期间的第一姿态角差值；

根据所述单位角度轴和所述第一姿态角差值，计算得到第一姿态变化量；

根据所述第一姿态变化量和所述第一采样周期的周期时间，计算得到第一姿态变化率，其中所述第一姿态变化率和所述第一姿态变化量正相关，所述第一姿态变化率和所述第一采样周期的周期时间负相关；

所述在所述陀螺仪的初始姿态下，使用所述陀螺仪数据计算得到所述终端的第二终端姿态和第二姿态变化率，包括：

在所述陀螺仪的初始姿态下，使用所述陀螺仪数据计算得到所述终端的第二终端姿态；

计算所述第二采样周期的当前采样周期和上一采样周期间的第二姿态角差值；

根据所述单位角度轴和所述第二姿态角差值，计算得到第二姿态变化量；

根据所述第二姿态变化量和所述第二采样周期的周期时间，计算得到第二姿态变化率，其中所述第二姿态变化率和所述第二姿态变化量正相关，所述第二姿态变化率和所述第二采样周期的周期时间负相关。

4.根据权利要求2所述的方法，其特征在于，

所述根据所述第一姿态变化率和所述第二姿态变化率，并结合所述终端当前运动模式下的整定，计算姿态可信度，包括：

根据所述第一终端姿态和所述第二终端姿态，计算得到终端姿态扰动率；

使用扰动率公式，计算得到扰动率，其中所述扰动率公式为： Δ为所述扰动率，Δω1为所述第一姿态变化率；Δω2为所述第二姿态变化率，Δω3为所述两种解算方法偏差率；

确定所述终端当前运动模式下的修正参数和整定参数；

根据整定公式，计算得到姿态可信度，其中所述整定公式为λ＝1/(Δ×修正参数)×整定参数，λ为姿态可信度；

所述根据所述姿态可信度的调节，融合所述第一终端姿态和所述第二终端姿态，以得到所述终端的第三终端姿态，包括：

使用融合公式，计算得到所述终端的第三终端姿态，其中所述融合公式为第三终端姿态＝(θ1,α1,β1)*λ+(θ2,α2,β2)*(1-λ)，(θ1,α1,β1)为所述第一终端姿态，(θ2,α2,β2)为所述第二终端姿态。

5.根据权利要求1-4任一项所述的方法，其特征在于，

所述根据所述姿态可信度的调节，融合所述第一终端姿态和所述第二终端姿态，以得到所述终端的第三终端姿态之后，所述方法还包括：

确定所述第三终端姿态为所述陀螺仪的初始姿态。

6.根据权利要求4所述的方法，其特征在于，

所述使用融合公式，计算得到所述终端的第三终端姿态之后，所述方法还包括：

当校正周期的当前周期的Δ大于所述校正周期的上一周期的Δ最大值时，在所述校正周期的当前周期的Δ开始下降时，确定所述第三终端姿态为所述陀螺仪的初始姿态；

当校正周期的当前周期的Δ小于所述校正周期的上一周期的Δ最小值时，在所述校正周期的当前周期的Δ开始上升时，确定所述第三终端姿态为所述陀螺仪的初始姿态。

7.一种终端，其特征在于，所述终端设有磁力计、加速度计和陀螺仪，所述终端包括：

获取单元，用于在第一采样周期，获取所述磁力计采集的磁力计数据和所述加速度计采集的加速度计数据；

计算单元，用于使用所述磁力计数据和所述加速度计数据，计算得到所述终端的第一终端姿态和第一姿态变化率；

确定单元，用于确定所述陀螺仪的初始姿态；

所述获取单元，还用于在第二采样周期，获取所述陀螺仪采集的陀螺仪数据；

所述计算单元，还用于在所述陀螺仪的初始姿态下，使用所述陀螺仪数据计算得到所述终端的第二终端姿态和第二姿态变化率；

所述计算单元，还用于根据所述第一姿态变化率和所述第二姿态变化率，计算姿态可信度；

所述计算单元，还用于根据所述姿态可信度的调节，融合所述第一终端姿态和所述第二终端姿态，以得到所述终端的第三终端姿态。

8.根据权利要求7所述的终端，其特征在于，

所述确定单元，还用于确定所述第三终端姿态为所述陀螺仪的初始姿态。

9.一种电子设备，其特征在于，包括至少一个处理器；

以及与所述至少一个处理器通信连接的存储器；

其中，所述存储器存储有可被所述至少一个处理器执行的指令程序，所述指令程序被所述至少一个处理器执行，以使所述至少一个处理器能够执行如权利要求1-6任一项所述的方法。

10.一种在终端中使用的计算机程序产品，其特征在于，所述计算机程序产品包括如权利要求7或8所述的功能模块。