CN106370178B - 移动终端设备的姿态测量方法及装置 - Google Patents

Abstract

本申请实施例公开了移动终端设备的姿态测量方法及装置，所述移动终端设备中配置有角速度传感器、重力传感器以及磁力传感器，其中，所述方法包括：确定在同一测量时刻各个传感器的测量值；基于所述角速度传感器的测量值计算得到第一四元数；利用所述重力传感器的测量值，对所述第一四元数中的水平姿态角误差进行修正，得到第二四元数；利用所述磁力传感器的测量值，对所述第二四元数中的垂直姿态角误差进行修正，得到第三四元数；将所述第三四元数转换为旋转矩阵，并利用所述旋转矩阵计算出该测量时刻的姿态角。通过本申请实施例，能够使得计算出的姿态角更为准确。

Description

移动终端设备的姿态测量方法及装置

技术领域

本申请涉及移动终端设备姿态计算技术领域，特别是涉及移动终端设备的姿态测量方法及装置。

背景技术

基于移动终端设备的姿态测量主要用于移动定位应用中，例如，可以用于PDR(Pedestrian Dead Reckoning，步行者航位推算)功能等，也即对步行者行走的步数、步长、方向等进行测量和统计，推算出步行者行走轨迹，和位置等信息。关于移动终端设备的姿态，主要是指在步行者行进的过程中，移动终端设备被放置的姿态，例如，平行于水平面，或者与水平面具有某角度，等等。在步行者行进过程中，移动终端设备的姿态是在不断变化的，为了确定出步行者的行进方向，对移动终端设备进行姿态测量成为关键。

目前，主流的移动终端设备一般都配置了陀螺仪、加速度计、地磁计三类3轴传感器，通过操作系统接口(Android/IOS等接口)，可以获得在移动终端坐标系中的与运动/姿态相关的测量值：

陀螺仪—3轴旋转角速率(单位rad/s)

加速度计—3轴加速度(或重力，单位m/s2)

地磁计—3轴环境磁场强度(单位uT)

因此，在现有技术中，可以通过传感器测量值来确定移动终端坐标系相对于导航坐标系的相对位置关系，这个相对位置关系包括三个角：

俯仰角α，移动终端设备y轴与水平面的夹角；

横滚角γ，移动终端设备x轴与水平面的夹角；

航向角

，移动终端设备y轴在投影到水平面后，与地理北向的夹角。

以上三个角度信息就可以代表移动终端设备的姿态信息，因此也被称为姿态角，其中，俯仰角与横滚角又可以称为水平方向姿态角，而航向角可以被称为垂直方向姿态角。在现有技术中，一般是通过将各个传感器的测量值进行叠加，然后通过欧拉角/旋转矩阵的方式确定出以上三个角度信息。

但是，在实际应用中，由于移动终端设备成本等方面的限制，其配置的传感器质量有限，得到的测量值往往存在一定的偏差；另外，地磁计等传感器比较容易受到环境中的电磁干扰，例如，手机信号的干扰、室内电风扇等设备的干扰等等，这种干扰一般是无法控制和避免的，因此，也更加影响了传感器测量值的准确度，最终导致现有技术中的姿态测量的结果也往往存在不够准确的情况。

因此，如何更准确地进行移动终端设备的姿态测量，成为需要本领域技术人员解决的技术问题。

发明内容

本申请提供了移动终端设备的姿态测量方法及装置，能够使得计算出的姿态角更为准确。

本申请提供了如下方案：

一种移动终端设备的姿态测量方法，所述移动终端设备中配置有角速度传感器、重力传感器以及磁力传感器，所述方法包括：

确定在同一测量时刻各个传感器的测量值；

基于所述角速度传感器的测量值计算得到第一四元数；

利用所述重力传感器的测量值，对所述第一四元数中的水平姿态角误差进行修正，得到第二四元数；

利用所述磁力传感器的测量值，对所述第二四元数中的垂直姿态角误差进行修正，得到第三四元数；

将所述第三四元数转换为旋转矩阵，并利用所述旋转矩阵计算出该测量时刻的姿态角。

一种移动终端设备的姿态测量装置，所述移动终端设备中配置有角速度传感器、重力传感器以及磁力传感器，所述装置包括：

测量值确定单元，用于确定在同一测量时刻各个传感器的测量值；

第一四元数计算单元，用于基于所述角速度传感器的测量值计算得到第一四元数；

第一修正单元，用于利用所述重力传感器的测量值，对所述第一四元数中的水平姿态角误差进行修正，得到第二四元数；

第二修正单元，用于利用所述磁力传感器的测量值，对所述第二四元数中的垂直姿态角误差进行修正，得到第三四元数；

转换单元，用于将所述第三四元数转换为旋转矩阵，并利用所述旋转矩阵计算出该测量时刻的姿态角。

根据本申请提供的具体实施例，本申请公开了以下技术效果：

通过本申请实施例，并不是直接用旋转矩阵进行姿态角的计算，而是采用四元数的方式来描述姿态角，从而可以避免出现“万向节死锁”问题的发生。另外，采用四元数的方式进行姿态角描述时，还可以对四元数直接进行线性加权来进行修正，具体进行修正时，可以先用重力修正四元数，并用修正后的四元数修正磁场水平分量，然后用修正水平分量的磁场和重力来确定四元数，这样可以使磁场干扰不会对俯仰角和横滚角产生负面的修正作用，从而使得计算出的姿态角更为准确。

当然，实施本申请的任一产品并不一定需要同时达到以上所述的所有优点。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本申请实施例提供的方法的流程图；

图2是本申请实施例提供的装置的示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本申请保护的范围。

首先需要说明的是，现有技术中在进行移动终端设备的姿态测量时采用的方法是：在得到各种传感器的测量值后，直接采用欧拉角/旋转矩阵来计算移动终端设备的各个姿态角，但是加速度数据和地磁数据的噪声大，动态性能比较差，无法跟踪人的机动运动。在获得姿态初始值的基础上，通过积分陀螺仪数据也可以递推姿态，陀螺仪具有动态性能好的特点，但由于积分操作导致递推的姿态存在累积误差。一般采用一种互补滤波的算法，即将积分后的陀螺仪数据经过一个高通滤波器，加速度计、地磁计数据经过一个低通滤波器，两个经过滤波的数据进行合成后得到最终的输出。

但是这种实现方式存在“万向节死锁”(Gimbal lock)问题，该问题是指：当移动终端设备平面接近垂直的时候，航向角

会出现奇异(数值上无法描述)，因此难以采用互补滤波的方法进行高通、低通滤波，由于存在奇异，难以进行线性的加权修正、插值等处理；此外，当移动终端设备接近于垂直放置时，系统失效。

本申请发明人在实现本申请的过程中发现，之所以存在上述万向节死锁问题，是因为：欧拉角表示的三维姿态系统所遭遇的万向节死锁问题，其根源是同一个姿态对应多个不同的姿态坐标。比如[θ₁ γ₁

]＝[90,30,-40]度，和[θ₂ γ₂

]＝[90,70,0]度这两种情况下，

的结果是相同的：

实际上欧拉角坐标[θ₁ γ₁

]＝[90,angle_y1,angle_z1]与[θ₂ γ₂

]＝[90,angle_y2,0]是相同的，当满足angle_y1-angle_z1＝angle_y2。

这意味着当这种情况下，任何绕z轴的旋转角度angle_z2都可以通过绕angle_y2角度来实现(不用绕z轴旋转了)，本质上是减少了一个自由度。

但是，采用四元数的表达方式可以消除这个问题：

由于欧拉角([θ γ

])这种对应同一姿态存在多种姿态坐标的特性，由于不具备连续性，不利于在融合过程中进行线性的插值融合处理，而四元数的表达是连续的，因此，在本申请实施例中所有的姿态都采用四元数的形式进行运算和融合(误差校正)。

另外，本申请发明人在实现本申请的过程中还发现，重力只能修正俯仰角和横滚角，而地磁可以修正航向角，但对俯仰角和横滚角的修正往往是容易受干扰的(负面作用的修正)。但是，现有技术中对重力、地磁等测量值不加区分的加以利用，最终导致姿态测量结果不准确。

因此，本申请实施例中，除了采用四元数的方法描述姿态，还分离了两个修正步骤：第一步，用重力修正姿态中的俯仰角和横滚角；第二步，用修正后的俯仰角和横滚角扣除地磁计数据中的水平分量误差，然后，用扣除误差分量的地磁数据修正姿态中的航向角。

下面对具体的实现方式进行详细介绍。

参见图1，本申请实施例首先提供了一种移动终端设备的姿态测量方法，其中，该移动终端设备中配置有角速度传感器、重力传感器以及磁力传感器，具体的，该方法具体可以包括以下步骤：

S101：确定在同一测量时刻各个传感器的测量值；

具体实现时，各个传感器可以按照一定的周期进行测量，在每个测量周期，可以使用同一测量时刻的传感器测量值进行姿态角的计算。

其中，关于角速度传感器，一般存在零偏误差的现象，导致测量值可能不够准确，因此，在本申请实施例中，还可以预先确定出角速度传感器的零偏误差，在进入后续具体的计算过程之前，还可以首先利用该零偏误差对角速度传感器的测量值进行修正，这样可以进一步提高姿态计算结果的准确性。

具体在确定角速度传感器的零偏误差时可以有多种方式，例如，在其中一种实现方式下，可以按照预置时间长度的滑动缓存窗口保存所述角速度传感器的测量值，分别计算在窗口内测量值的平均值、峰峰值以及方差，并保存峰峰值最小值以及方差最小值。当所述角速度传感器获取到新的测量值时，更新计算所述平均值、峰峰值以及方差，如果新计算得到的峰峰值小于所述保存的峰峰值最小值，新计算得到的方差小于所述保存的方差最小值，则将所述新计算得到的平均值确定为所述角速度传感器的零偏误差，并刷新所述保存的峰峰值最小值以及方差最小值。

S102：基于所述角速度传感器的测量值计算得到第一四元数；

具体在进行计算时，首先可以基于角速度传感器测量值计算出第一四元数，具体可以通过以下公式来实现：

其中，[ω_x ω_y ω_z]为所述角速度传感器在所述测量时刻的测量值；

Δt为测量周期；

是四元数相乘。

S103：利用所述重力传感器的测量值，对所述第一四元数中的水平姿态角误差进行修正，得到第二四元数；

具体的修正方式可以有多种，例如，在其中一种实现方式下，可以根据重力传感器的测量值以及第一四元数，计算重力误差的梯度方向，然后根据梯度方向以及预置的增益对第一四元数中的水平姿态角误差进行修正，得到第二四元数。

其中，重力误差的梯度方向可以表示为：

其中，▽f＝[▽f₀ ▽f₁ ▽f₂ ▽f₃]^T

▽f₀＝-4Δa_xq₂+4Δa_yq₁

▽f₁＝4Δa_xq₃+4Δa_yq₀-4Δa_zq₁

▽f₂＝-4Δa_xq₀+4Δa_yq₃-4Δa_zq₂

▽f₃＝4Δa_xq₁+4Δa_yq₂

q₀,q₁,q₂,q₃为第一四元数中的各个分量。

这部分计算是利用重力加速度来修正陀螺仪积分的四元数

(这个是第二四元数)，对水平姿态角具有修正作用，对航向角不能保证朝正确的方向修正，因此，在后续的步骤中还可以利用地磁修正模块做进一步的修正。

S104：利用所述磁力传感器的测量值，对所述第二四元数中的垂直姿态角误差进行修正，得到第三四元数；

该步骤的具体实现方式也可以有多种，例如，在其中一种实现方式下，首先可以利用第二四元数对磁力传感器测量值中的水平分量进行修正，然后利用重力传感器的测量值、理论重力值、磁力传感器测量值以及修正后的磁力传感器测量值，计算出第四四元数，最后再利用第四四元数对第二四元数进行修正，得到第三四元数。

其中，由于步骤S103中计算得到的四元数

的水平方向姿态角误差已经被修正，而磁传感器测量的数据不能提供可靠的水平姿态角信息，所以首先用此时的四元数

消除磁数据中水平分量的误差，保留垂直分量。

具体在利用第二四元数对磁力传感器测量值中的水平分量进行修正时，可以通过以下方式将磁力传感器测量值投影到水平面：

其中，

q₀,q₁,q₂,q₃为所述第二四元数中的各个分量；

为磁力传感器测量值；

修正后的磁力传感器测量值为：

具体在利用重力传感器的测量值、理论重力值、磁力传感器测量值以及修正后的磁力传感器测量值，计算出第四四元数时，可以确定出重力传感器的测量值、理论重力值、磁力传感器测量值以及修正后的磁力传感器测量值的单位化向量：

然后，变换到对角线和平面法线的单位向量：

计算向量

到

的旋转四元数

将

旋转

到

计算第二次旋转

计算出第四四元数

之后就可以利用该第四四元数对第二四元数进行修正，得到第三四元数。具体实现时，可以利用第四四元数对第二四元数进行加权修正，得到第三四元数：

其中，

为所述第二四元数；

λ∈[0,1]为权重。

其中，权重可以λ通过以下方式确定：

首先，根据磁力传感器测量值的矢量长度与当前地理位置的理论地磁强度之间的偏差，确定第一权重分量；根据磁力传感器测量值的矢量与垂直方向的夹角相对于当前地理位置理论值的偏差，确定第二权重分量；根据预置时间段内角速度传感器测量得到的平均角速度，确定第三权重分量；然后，根据所述第一权重分量、第二权重分量、第三权重分量，确定所述权重。

下面分别对各个权重分量进行介绍。

1、关于第一权重分量：地磁矢量的长度

与当地理论地磁强度偏差需在合理范围内(如杭州当地地磁强度为45uT)，设置一阈值，若超出阈值则权重置为0，否则权重线性增大最大不超过1，本权重记为λ₁。

关于线性增大部分，可以设置两个强度阈值(单位uT)thres_1和thres_2，在偏离中心值(45uT)thres_1范围内取λ₁＝1；在偏离中心值介于thres_1和thres_2范围，

当偏离中心值大于thres_2时，λ₁＝0。

2、关于第二权重分量：地磁矢量与铅垂方向的夹角需与当地理论值偏差在合理范围内(如杭州应为-45°)，该夹角可以由测量的重力和地磁数据计算，计算方法可以为：

其中

为测量得重力，

为测量的磁场强度。

具体在确定第二权重分量的值时，可以设置阈值，若超出阈值则权重置为0，否则权重线性增大最大不超过1，记为λ₂。

关于线性增大部分，可以设置两个角度阈值(单位°)thres_3和thres_4，在偏离中心值(-45°)thres_3范围内取λ₂＝1；在偏离中心值介于thres_3和thres_4范围时，可以为：

当偏离中心值大于thres_4，λ₂＝0。

3、关于第三权重分量：权重与旋转速度相关，由于地磁传感器动态性能差，当旋转速度加大，对磁的权重相应减小。计算一段时间内角速度传感器的平均角速度ωm，并将第三权重分量设为λ₃。

设置角速度阈值(单位°/s)thres_5和thres_6，当|ωm|小于阈值thres_5则λ₃＝1，当|ωm|大于阈值thres_6则λ₃＝0，当|ωm|介于thres_5和thres_6之间时线性递减，在一种实现方式下，公式可以为：

最后可以计算出地磁动态权重λ＝λ₁·λ₂·λ₃。

S105：将所述第三四元数转换为旋转矩阵，并利用所述旋转矩阵计算出该测量时刻的姿态角。

在得到第三四元数之后，就可以将其转换为旋转矩阵，并利用该旋转矩阵计算出姿态角。具体的，四元数到旋转矩阵的转换关系可以为：

旋转矩阵与各个姿态角之间的转换关系可以为：

其中的θ，γ，

为姿态角。

需要说明的是，关于四元数到旋转矩阵的转换关系，以及旋转矩阵与各个姿态角的转换关系可以参见已有技术中的具体实现，这里不再详细展开介绍。

另外需要说明的是，本申请实施例中，各步骤的执行主体可以是移动终端设备中的应用等，确定出移动终端设备姿态角后，可以用于进行步行者行进方向的确定等。

总之，通过本申请实施例，并不是直接用旋转矩阵进行姿态角的计算，而是采用四元数的方式来描述姿态角，从而可以避免出现“万向节死锁”问题的发生。另外，采用四元数的方式进行姿态角描述时，还可以对四元数直接进行线性加权来进行修正，具体进行修正时，可以先用重力修正四元数，并用修正后的四元数修正磁场水平分量，然后用修正水平分量的磁场和重力来确定四元数，这样可以使磁场干扰不会对俯仰角和横滚角产生负面的修正作用，从而使得计算出的姿态角更为准确。

与本申请实施例提供的移动终端设备的姿态测量方法相对应，本申请实施例还提供了一种移动终端设备的姿态测量装置，所述移动终端设备中配置有角速度传感器、重力传感器以及磁力传感器，参见图2，所述装置可以包括：

测量值确定单元201，用于确定在同一测量时刻各个传感器的测量值；

第一四元数计算单元202，用于基于所述角速度传感器的测量值计算得到第一四元数；

第一修正单元203，用于利用所述重力传感器的测量值，对所述第一四元数中的水平姿态角误差进行修正，得到第二四元数；

第二修正单元204，用于利用所述磁力传感器的测量值，对所述第二四元数中的垂直姿态角误差进行修正，得到第三四元数；

转换单元205，用于将所述第三四元数转换为旋转矩阵，并利用所述旋转矩阵计算出该测量时刻的姿态角。

具体实现时，所述第一修正单元203具体可以包括：

梯度方向计算子单元，用于根据所述重力传感器的测量值以及所述第一四元数，计算重力误差的梯度方向；

水平误差修正子单元，用于根据所述梯度方向以及预置的增益对所述第一四元数中的水平姿态角误差进行修正，得到第二四元数。

所述第二修正单元204具体可以包括：

水平分量修正子单元，用于利用所述第二四元数对磁力传感器测量值中的水平分量进行修正；

四元数计算子单元，用于利用重力传感器的测量值、理论重力值、磁力传感器测量值以及修正后的磁力传感器测量值，计算出第四四元数；

四元数修正子单元，用于利用所述第四四元数对所述第二四元数进行修正，得到所述第三四元数。

另外，该装置还可以包括：

零偏误差确定单元，用于确定所述角速度传感器的零偏误差；

零偏修正单元，用于在基于所述角速度传感器的测量值计算得到第一四元数之前，利用所述零偏误差对所述角速度传感器的测量值进行修正。

其中，所述零偏误差确定单元具体可以包括：

统计值计算子单元，用于按照预置时间长度的滑动缓存窗口保存所述角速度传感器的测量值，分别计算在窗口内测量值的统计值，所述统计值包括平均值、峰峰值以及方差；

保存子单元，用于保存峰峰值最小值以及方差最小值；

统计值更新子单元，用于当所述角速度传感器获取到新的测量值时，更新计算所述统计值；

零偏误差确定子单元，用于如果新计算得到的峰峰值小于所述保存的峰峰值最小值，新计算得到的方差小于所述保存的方差最小值，则将所述新计算得到的平均值确定为所述角速度传感器的零偏误差，并刷新所述保存的峰峰值最小值以及方差最小值。

其中，关于各个步骤中涉及到的具体实现方式及相关公式，可以参见方法实施例中的介绍，这里不再赘述。

总之，通过本申请实施例提供的上述装置，并不是直接用旋转矩阵进行姿态角的计算，而是采用四元数的方式来描述姿态角，从而可以避免出现“万向节死锁”问题的发生。另外，采用四元数的方式进行姿态角描述时，还可以对四元数直接进行线性加权来进行修正，具体进行修正时，可以先用重力修正四元数，并用修正后的四元数修正磁场水平分量，然后用修正水平分量的磁场和重力来确定四元数，这样可以使磁场干扰不会对俯仰角和横滚角产生负面的修正作用，从而使得计算出的姿态角更为准确。

通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到本申请可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例或者实施例的某些部分所述的方法。

本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统或系统实施例而言，由于其基本相似于方法实施例，所以描述得比较简单，相关之处参见方法实施例的部分说明即可。以上所描述的系统及系统实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

以上对本申请所提供的移动终端设备的姿态测量方法及装置，进行了详细介绍，本文中应用了具体个例对本申请的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本申请的方法及其核心思想；同时，对于本领域的一般技术人员，依据本申请的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本申请的限制。

Claims (10)

1.一种移动终端设备的姿态测量方法，其特征在于，所述移动终端设备中配置有角速度传感器、重力传感器以及磁力传感器，所述方法包括：

确定在同一测量时刻各个传感器的测量值；

基于所述角速度传感器的测量值和测量周期计算得到第一四元数；

利用所述重力传感器的测量值，对所述第一四元数中的水平姿态角误差进行修正，得到第二四元数；

利用所述磁力传感器的测量值，对所述第二四元数中的垂直姿态角误差进行修正，得到第三四元数；其中，所述磁力传感器的测量值中的水平分量依据所述第二四元数修正；

将所述第三四元数转换为旋转矩阵，并利用所述旋转矩阵计算出该测量时刻的姿态角。

2.根据权利要求1所述的方法，其特征在于，所述利用所述重力传感器的测量值，对所述第一四元数中的水平姿态角误差进行修正，得到第二四元数，包括：

根据所述重力传感器的测量值以及所述第一四元数，计算重力误差的梯度方向；

根据所述梯度方向以及预置的增益对所述第一四元数中的水平姿态角误差进行修正，得到第二四元数。

3.根据权利要求1所述的方法，其特征在于，所述利用所述磁力传感器的测量值，对所述第二四元数中的垂直姿态角误差进行修正，得到第三四元数，包括：

利用所述第二四元数对磁力传感器测量值中的水平分量进行修正；

利用重力传感器的测量值、理论重力值、磁力传感器测量值以及修正后的磁力传感器测量值，计算出第四四元数；

利用所述第四四元数对所述第二四元数进行修正，得到所述第三四元数。

4.根据权利要求1至3任一项所述的方法，其特征在于，还包括：

确定所述角速度传感器的零偏误差；

在基于所述角速度传感器的测量值计算得到第一四元数之前，还包括：

利用所述零偏误差对所述角速度传感器的测量值进行修正。

5.根据权利要求4所述的方法，其特征在于，所述确定所述角速度传感器的零偏误差，包括：

按照预置时间长度的滑动缓存窗口保存所述角速度传感器的测量值，分别计算在窗口内测量值的平均值、峰峰值以及方差；

保存峰峰值最小值以及方差最小值；

当所述角速度传感器获取到新的测量值时，更新计算所述平均值、峰峰值以及方差；

如果新计算得到的峰峰值小于所述保存的峰峰值最小值，新计算得到的方差小于所述保存的方差最小值，则将所述新计算得到的平均值确定为所述角速度传感器的零偏误差，并刷新所述保存的峰峰值最小值以及方差最小值。

6.一种移动终端设备的姿态测量装置，其特征在于，所述移动终端设备中配置有角速度传感器、重力传感器以及磁力传感器，所述装置包括：

测量值确定单元，用于确定在同一测量时刻各个传感器的测量值；

第一四元数计算单元，用于基于所述角速度传感器的测量值计算得到第一四元数；

第一修正单元，用于利用所述重力传感器的测量值，对所述第一四元数中的水平姿态角误差进行修正，得到第二四元数；

第二修正单元，用于利用所述磁力传感器的测量值，对所述第二四元数中的垂直姿态角误差进行修正，得到第三四元数；其中，所述磁力传感器的测量值中的水平分量依据所述第二四元数修正；

转换单元，用于将所述第三四元数转换为旋转矩阵，并利用所述旋转矩阵计算出该测量时刻的姿态角。

7.根据权利要求6所述的装置，其特征在于，所述第一修正单元包括：

梯度方向计算子单元，用于根据所述重力传感器的测量值以及所述第一四元数，计算重力误差的梯度方向；

水平误差修正子单元，用于根据所述梯度方向以及预置的增益对所述第一四元数中的水平姿态角误差进行修正，得到第二四元数。

8.根据权利要求6所述的装置，其特征在于，所述第二修正单元包括：

水平分量修正子单元，用于利用所述第二四元数对磁力传感器测量值中的水平分量进行修正；

四元数计算子单元，用于利用重力传感器的测量值、理论重力值、磁力传感器测量值以及修正后的磁力传感器测量值，计算出第四四元数；

四元数修正子单元，用于利用所述第四四元数对所述第二四元数进行修正，得到所述第三四元数。

9.根据权利要求6至8任一项所述的装置，其特征在于，还包括：

零偏误差确定单元，用于确定所述角速度传感器的零偏误差；

零偏修正单元，用于在基于所述角速度传感器的测量值计算得到第一四元数之前，利用所述零偏误差对所述角速度传感器的测量值进行修正。

10.根据权利要求9所述的装置，其特征在于，所述零偏误差确定单元包括：

统计值计算子单元，用于按照预置时间长度的滑动缓存窗口保存所述角速度传感器的测量值，分别计算在窗口内测量值的统计值，所述统计值包括平均值、峰峰值以及方差；

保存子单元，用于保存峰峰值最小值以及方差最小值；

统计值更新子单元，用于当所述角速度传感器获取到新的测量值时，更新计算所述统计值；

零偏误差确定子单元，用于如果新计算得到的峰峰值小于所述保存的峰峰值最小值，新计算得到的方差小于所述保存的方差最小值，则将所述新计算得到的平均值确定为所述角速度传感器的零偏误差，并刷新所述保存的峰峰值最小值以及方差最小值。

Images