姿态测量方法及终端设备与流程

本发明属于导航测姿技术领域，尤其涉及一种姿态测量方法及终端设备。

背景技术：

精确制导武器、无人机以及水下机器人发展日新月异，对姿态测量提出了越来越高的要求。惯性导航系统(inertialnavigationsystem，ins)利用其陀螺和加速度计的输出利用一定的算法推算载体的位置、速度、姿态信息。然而由于陀螺和加速度计输出存在误差，惯性导航系统推算的位置、速度、姿态信息会随时间积累，长时间导航会导致导航信息的发散。全球导航卫星系统(globalnavigationsatellitesystem，gnss)、ins具有良好的互补特性，是常用的组合导航方式。根据组合方式，gnss/ins组合导航可以分为松组合、紧组合以及深组合。松组合利用gnss提供的位置和速度信息作为观测量进行组合，因此要求卫星的可见星数大于四颗。紧组合采用的观测量是卫星伪距和伪距率信息，在观测星数小于四颗的情况下仍然具有一定的导航定位能力。gnss/ins组合导航系统在卫导信号良好时能够提供稳定可靠的位置、速度以及姿态信息，然而在城市峡谷、森林以及水下等信号遮挡严重的场景无法提供稳定可靠地位置、速度、姿态信息，除此之外gnss信号容易受到电磁干扰等影响从而严重影响导航信息精度。因此，现有一般采用航姿参考系统(attitudeandheadingreferencesystem，ahrs)进行姿态测量。

ahrs系统由三轴陀螺仪、三轴加速度计以及三轴磁强计组成。ahrs系统包含了姿态和航向解算算法，融合陀螺、加速度计和磁强计的输出获取载体的横滚、俯仰以及航向信息。通常利用陀螺的数据不断地推算载体的姿态信息而加速度计和磁强计的输出用来修正利用陀螺计算的角度信息，从而实现数据的融合。陀螺零偏和加速度计零偏是影响测姿精度的重要因素，为了得到高精度的姿态信息，数据融合算法必须能够准确地估计出陀螺和加速度计的零偏误差。低精度mems器件具有重量轻、体积小功耗低等优势，然而受加工工艺的限制，其误差参数容易受到温度、湿度以及气压等外部因素的影响。如陀螺的零偏误差其随温度变化的幅度可达上百deg/h，比例因子随温度变化的幅度可达上千ppm，而加速度计的零偏误差随温度变化幅度可达数十mg，比例因子误差随温度变化幅度可达上千ppm。当载体处于静止状态时，忽略掉加速度计的噪声，加速度计三个轴向的输出为重力在三个轴向的投影，此时利用陀螺和加速度计的输出可以准确地获得载体的姿态信息，然而当载体处于运动状态并且具有加速度时，此时由于加速度的干扰，无法利用加速度计的输出来得到稳定的姿态信息也无法准确估计出陀螺零偏等误差，从而极大的影响姿态测量的精度。而且在ahrs系统中，由于重力的存在水平姿态角包括横滚角和俯仰角的可观测性较好，因此横滚角和俯仰角能够保持较高的精度，而航向角的可观测性最差，通常通过磁强计或者外部信息辅助修正从而保持航向精度，例如通常通过磁强计测量的地磁场信息对陀螺推算的航向信息进行修正，而在动中通领域，会利用天线跟踪卫星的电平信号对航向信息进行修正，然而在修正时，磁场很容易收到外部信息的干扰，从而产生较大的量测误差，如果在滤波中采用了误差较大的航向量测信息，容易导致滤波的不稳定甚至影响横滚角和俯仰角精度。另外，ahrs系统中利用陀螺来预测载体的姿态信息，利用加速度计的输出对水平姿态角进行修正，当载体处于静止或者匀速直线运动时，加速度计能够准确地测量出重力在各个轴向的分量，利用该信息可以对姿态信息进行准确地估计。然而当载体处于较大加速度的动态下时，加速度计受到载体加速度的影响无法为载体姿态解算提供准确可靠的信息。

技术实现要素：

有鉴于此，本发明实施例提供了一种姿态测量方法及终端设备，以解决现有ahrs系统中低成本mems陀螺和加速度计误差受外部环境干扰波动较大影响测姿精度、航向角可观测性较差且容易受到外部干扰、无法为载体姿态解算提供准确可靠信息的问题。

本发明实施例的第一方面提供了一种姿态测量方法，包括：

在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量；

利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测；

根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵；

根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量；

若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿。

本发明实施例的第二方面提供了一种姿态测量装置，包括：

姿态初始化单元，用于在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量；

姿态更新单元，用于利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测；

量测方程构建单元，用于根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵；

滤波修正单元，用于根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量；

反馈补偿单元，用于若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿。

本发明实施例的第三方面提供了一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述方法的步骤。

本发明实施例的第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上述方法的步骤。

本发明实施例与现有技术相比存在的有益效果是：本发明实施例针对实际应用中ahrs系统中低成本mems陀螺和加速度计误差受外部环境干扰波动较大影响测姿精度提出利用加速度计输出来判断载体的运动状态，利用静态场景的数据准确估计陀螺零偏；针对ahrs系统中航向角可观测性交叉，并且容易受到外部干扰的问题，通过将横滚角、俯仰角和航向角进行独立的滤波来提高ahrs数据融合算法的可靠性和准确性；并提出利用加速度计输出判断载体动态从而调整加表输出权重的方法，从而得到稳定可靠地高精度姿态信息，同时根据加速度计的输出判断载体的运动状态以调整零偏的估计策略从而提高系统量测精度，设计水平姿态和航向独立的滤波保证航向信息和水平姿态信息的独立性，从而有效提高系统的稳定性。

附图说明

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

图1是本发明实施例提供的一种姿态测量方法的示意流程图；

图2是本发明实施例提供的一种姿态测量装置示意性框图；

图3是本发明实施例提供的一种终端设备的示意性框图。

具体实施方式

以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本发明实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本发明。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本发明的描述。

为了说明本发明所述的技术方案，下面通过具体实施例来进行说明。

参见图1，图1是本发明实施例提供的一种姿态测量方法的示意流程图，在该实施例中，以终端的角度触发为例进行说明，这里，终端可以为智能手机、平板电脑等移动终端。如图1所示，在该实施例中，终端的处理过程可以包括以下步骤：

s101：在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量。

这里，对初始横滚角、俯仰角和航向角，状态量以及噪声矩阵进行初始化。如果应用中可以通过外部辅助初始化则利用外部信息进行横滚角、俯仰角和航向角的初始化。如果无外部辅助则利用加速度计和磁强计的输出初始化横滚角、俯仰角和航向角。在该实施例中利用加速度计的输出初始化横滚角和俯仰角，并且为了提高初始对准的精度，首先使载体处于静止状态，利用静止状态下的输出计算载体的横滚和俯仰角，计算方法如下：

其中，φ代表载体的横滚角，θ代表载体的俯仰角，代表静态场景下加速度计x、y、z轴输出的平均值。

在利用磁强计的输出计算航向角之前首先对磁强计进行标定，该实施例中可以利用二维标定方法，旋转ahrs系统使航向遍历0～360°，旋转过程中使横滚角和俯仰角保持在零度附近，并且记录x、y、z各个轴线输出的最大值和最小值mxmax、mxmin，mymax、mymin，mzmax、mzmin，根据磁强计输出的最大值和最小值来求取磁强计各个轴线的改正值，计算得到磁强计x、y、z轴的补偿值为：

利用计算得到的磁强计各轴的补偿值biasx、biasy、biasz来补偿磁强计的输出，得到补偿后的磁强计输出，补偿方法如下：

式中为磁强计的原始输出，为校正后的磁强计输出。

计磁强计经过校正后的x、y、z轴的均值依次为：通过下式计算得到载体的航向信息：

其中，γm为初始对准当地的磁偏角，其值可以结合纬度信息查阅资料得到。

得到载体的姿态角后便可以计算对应的姿态角四元数并且利用得到的四元数初始化状态向量：其中q0、q1、q2、q3表示姿态角四元数。

s102：利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测。

具体地，利用陀螺数据更新载体的姿态四元数，并且构建状态转移矩阵对误差和状态向量进行预测。

利用陀螺原始数据更新姿态四元数的方法如下：

q0+＝q0-q1ωxdt-q2ωydt-q3ωzdt

q1+＝q1+q0ωxdt-q3ωydt-q2ωzdt

q2+＝q2+q3ωxdt+q0ωydt-q1ωzdt

q3+＝q3-q2ωxdt+q1ωydt-q0ωzdt

其中q0+、q1+、q2+、q3+代表更新后的姿态四元数，q0、q1、q2、q3代表更新前的姿态四元数，ωx、ωy、ωz依次代表陀螺x、y、z轴的输出，f矩阵代表卡尔曼滤波的状态转移矩阵。对状态向量和误差矩阵进行预测的方法如下：

其中，和表示第k-1历元预测后的状态向量和预测前的状态向量，k表示历元数，就是一个计数表示第几个历元(每次导航解算就算一个计算历元)，pk-1和pk,k-1表示第k-1历元预测前的卡尔曼滤波方差矩阵和预测后的卡尔曼滤波方差矩阵，qk-1表示第k-1历元的量测噪声矩阵。

s103：根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵。

这里，构建卡尔曼滤波量测矩阵，该步骤利用加速度计的输出和方向余弦矩阵投影计算的重力矢量之间的差值作为量测新息，量测系数矩阵如下所示：

在上式中g代表当地的重力大小，q0、q1、q2、q3代表当前历元计算得到的姿态四元数；

计算量测信息zk：其中，为从n系到b系的方向余弦矩阵，计算方法如下：

其中，φ、θ、ψ依次代表载体的横滚、俯仰和姿态角，其值可以利用姿态四元数计算得到，gⁿ＝[00g]^t，g为当地重力值。

量测噪声矩阵的设定需要根据载体的动态进行自适应的判断，为了能够准确的反应载体的运动状态，采用求取固定采样时段方差的方法设定量测方差，具体的计算方法如下：

式中，n为固定的采样数量，为加速度计x、y、z三轴的输出σa²反应载体的动态大小，用来调整卡尔曼滤波的量测噪声矩阵：

rk＝r0·σa²

式中，r0表示基准的加速度计噪声矩阵。

上述步骤目的是构建卡尔曼滤波的量测方程，构建量测方程需要构建量测系数矩阵、需要计算量测信息并且需要确定方程的量测噪声矩阵，他们之间是独立的，目的都是为了建立量测方程。

s104：根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量。

具体地，利用磁强计或者外部信息对航向进行滤波。

首先计算磁强计推算的航向信息，方法如下：

其中，

θ、φ分别为计算得到的横滚角和俯仰角，ψ代表利用磁强计计算得到的航向信息；γm为初始对准当地的磁偏角，其值可以结合纬度信息查阅资料得到；分别为经过补偿后的磁强计输出的x、y、z轴的均值，依次代表经过倾斜补偿后磁强计x、y、z轴的输出。m^w为磁强计输出经过倾斜补偿后的输出向量。

当有外部信息时接收并存储外部信息，

ψm＝ψext

利用外部信息进行修正，其中，ψext表示外部航向信息。

构建系数矩阵

其中x1、x2、x3、x4为状态量中估计得到的四元数，hψ代表航向信息量测更新的系数矩阵，表示构建h矩阵的中间变量。

计算增益矩阵：

其中kψ代表航向信息量测更新的增益矩阵，σψ为利用磁强计信息计算得到的航向信息的方差信息或者外部航向信息的方差信息，p表示卡尔曼滤波的方差矩阵。

状态更新：

xi＝xi+ki(ψm-ψ)

其中，i＝1,2,3...7依次代表卡尔曼滤波状态量的元素，ki表示航向信息量测更新的增益矩阵，ψ表示利用惯导数据预测的载体的航向角。

s105：若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿。

这里，首先根据步骤s103中计算得到的加速度计输出的方差值和加速度的模值来判断载体的运动状态，当σa²小于一定阈值后则认为载体处于静止状态，此时滤波器能够较好的估计出零偏误差，因此对陀螺零偏进行更新，并且将状态量置零：

x5＝0

x6＝0

x7＝0

上式中bgx(t)、bgy(t)、bgz(t)依次代表当前时刻的x、y、z轴陀螺零偏，bgx(t-)、bgy(t-)、bgz(t-)依次代表先前的x、y、z轴陀螺零偏，状态向量一共包括7个元素，即是一个七维列向量，x5、x6、x7依次代表第5、6、7个元素。

此时在下一个历元计算中利用零偏信息进行反馈补偿：

ωx＝ωx-bgx(t)

ωy＝ωy-bgy(t)

ωz＝ωz-bgz(t)

如果固定数量历元的加速度计均值和方差不满足条件即载体处于运动状态那么在该历元保持陀螺的零偏值不变，也不对状态量置零，在计算中利用固定不变的陀螺零偏对原始输出进行补偿。

从以上描述可知，本发明实施例姿态测量方法，针对实际应用中ahrs系统中低成本mems陀螺和加速度计误差受外部环境干扰波动较大影响测姿精度提出利用加速度计输出来判断载体的运动状态，利用静态场景的数据准确估计陀螺零偏；针对ahrs系统中航向角可观测性交叉，并且容易受到外部干扰的问题，通过将横滚角、俯仰角和航向角进行独立的滤波来提高ahrs数据融合算法的可靠性和准确性；并提出利用加速度计输出判断载体动态从而调整加表输出权重的方法，从而得到稳定可靠地高精度姿态信息，同时根据加速度计的输出判断载体的运动状态以调整零偏的估计策略从而提高系统量测精度，设计水平姿态和航向独立的滤波保证航向信息和水平姿态信息的独立性，从而有效提高系统的稳定性。

此外，在一个具体示例中，所述在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量包括：

根据表达式确定载体的横滚角φ，根据表达式确定载体的俯仰角θ，其中，表示静态场景下加速度计x、y、z轴输出的平均值；

根据表达式确定载体的航向角ψ，其中，表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，表示经过倾斜补偿后磁强计x、y、z轴的输出，m^w表示磁强计输出经过倾斜补偿后的输出向量；

根据表达式确定载体的姿态角四元数向量其中，其中q0、q1、q2、q3表示姿态角四元数；

根据表达式初始化卡尔曼滤波状态向量x0。

此外，在一个具体示例中，所述利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测包括：

根据表达式更新姿态角四元数q0、q1、q2、q3，其中，q0+、q1+、q2+、q3+表示更新后的姿态角四元数，ωx、ωy、ωz表示陀螺x、y、z轴的输出；

根据表达式确定卡尔曼滤波的状态转移矩阵f；

根据表达式对状态向量进行预测，根据表达式对误差矩阵进行预测，其中，和表示第k-1历元预测后的状态向量和预测前的状态向量，k表示历元数，pk-1和pk,k-1表示第k-1历元预测前的卡尔曼滤波方差矩阵和预测后的卡尔曼滤波方差矩阵，qk-1表示第k-1历元的量测噪声矩阵。

此外，在一个具体示例中，所述根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵包括：

根据表达式构建卡尔曼滤波的量测系数矩阵hk，其中，q0、q1、q2、q3表示姿态角四元数，g表示当地的重力大小；

根据表达式计算卡尔曼滤波的量测信息zk，为从n系到b系的方向余弦矩阵，φ、θ、ψ表示载体的横滚角、俯仰角和航向角，gⁿ＝[00g]^t；

根据表达式rk＝r0·σa²调整卡尔曼滤波的量测噪声矩阵rk，其中，σa²表示加速度计输出的方差，n表示固定的采样数量，nk表示加速度计x、y、z三轴的输出σa²反应载体的动态大小，r0表示基准的加速度计噪声矩阵。

此外，在一个具体示例中，所述根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量包括：

根据表达式确定载体的航向角ψm，其中，θ、φ表示确定的横滚角和俯仰角，表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，表示经过倾斜补偿后磁强计x、y、z轴的输出，m^w表示磁强计输出经过倾斜补偿后的输出向量；

当有外部航向信息时，接收并存储外部航向信息，根据表达式ψm＝ψext进行修正，其中，ψext表示外部航向信息；

根据表达式和构建系数矩阵，其中，x1、x2、x3、x4表示状态向量中得到的四元数，hψ表示航向信息量测更新的系数矩阵；

根据表达式计算增益矩阵，其中，kψ表示航向信息量测更新的增益矩阵，σψ为利用磁强计信息计算得到的航向信息的方差信息或者外部航向信息的方差信息，p表示卡尔曼滤波的方差矩阵；

根据表达式xi＝xi+ki(ψm-ψ)更新卡尔曼滤波状态向量，其中，i＝1,2,3...7表示卡尔曼滤波状态向量的元素，ki表示航向信息量测更新的增益矩阵，ψ表示利用惯导数据预测的载体的航向角。

此外，在一个具体示例中，所述根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿包括：

根据表达式对陀螺零偏进行更新，其中，bgx(t)、bgy(t)、bgz(t)表示当前时刻的x、y、z轴陀螺零偏，bgx(t-)、bgy(t-)、bgz(t-)表示先前的x、y、z轴陀螺零偏，x5＝0，x6＝0，x7＝0；

根据表达式进行反馈补偿，其中，ωx、ωy、ωz表示陀螺x、y、z轴的输出。

应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

对应于上文实施例所述的姿态测量方法，图2示出了本发明实施例提供的一种姿态测量装置的示意性框图。本实施例的姿态测量装置200包括的各单元用于执行图1对应的实施例中的各步骤，具体请参阅图1及图1对应的实施例中的相关描述，此处不赘述。本实施例的姿态测量装置200包括姿态初始化单元201、姿态更新单元202、量测方程构建单元203、滤波修正单元204和反馈补偿单元205。

其中，姿态初始化单元201，用于在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量。姿态更新单元202，用于利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测。量测方程构建单元203，用于根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵。滤波修正单元204，用于根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量。反馈补偿单元205，用于若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿。

从以上描述可知，本发明实施例姿态测量装置，针对实际应用中ahrs系统中低成本mems陀螺和加速度计误差受外部环境干扰波动较大影响测姿精度提出利用加速度计输出来判断载体的运动状态，利用静态场景的数据准确估计陀螺零偏；针对ahrs系统中航向角可观测性交叉，并且容易受到外部干扰的问题，通过将横滚角、俯仰角和航向角进行独立的滤波来提高ahrs数据融合算法的可靠性和准确性；并提出利用加速度计输出判断载体动态从而调整加表输出权重的方法，从而得到稳定可靠地高精度姿态信息，同时根据加速度计的输出判断载体的运动状态以调整零偏的估计策略从而提高系统量测精度，设计水平姿态和航向独立的滤波保证航向信息和水平姿态信息的独立性，从而有效提高系统的稳定性。

进一步的，所述姿态初始化单元201还用于：

根据表达式确定载体的横滚角φ，根据表达式确定载体的俯仰角θ，其中，表示静态场景下加速度计x、y、z轴输出的平均值；

根据表达式确定载体的航向角ψ，其中，表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，表示经过倾斜补偿后磁强计x、y、z轴的输出，m^w表示磁强计输出经过倾斜补偿后的输出向量；

根据表达式确定载体的姿态角四元数向量其中，其中q0、q1、q2、q3表示姿态角四元数；

根据表达式初始化卡尔曼滤波状态向量x0。

进一步的，所述姿态更新单元202还用于：

根据表达式更新姿态角四元数q0、q1、q2、q3，其中，q0+、q1+、q2+、q3+表示更新后的姿态角四元数，ωx、ωy、ωz表示陀螺x、y、z轴的输出；

根据表达式确定卡尔曼滤波的状态转移矩阵f；

根据表达式对状态向量进行预测，根据表达式对误差矩阵进行预测，其中，和表示第k-1历元预测后的状态向量和预测前的状态向量，k表示历元数，pk-1和pk,k-1表示第k-1历元预测前的卡尔曼滤波方差矩阵和预测后的卡尔曼滤波方差矩阵，qk-1表示第k-1历元的量测噪声矩阵。

进一步的，所述量测方程构建单元203还用于：

根据表达式构建卡尔曼滤波的量测系数矩阵hk，其中，q0、q1、q2、q3表示姿态角四元数，g表示当地的重力大小；

根据表达式计算卡尔曼滤波的量测信息zk，为从n系到b系的方向余弦矩阵，φ、θ、ψ表示载体的横滚角、俯仰角和航向角，gⁿ＝[00g]^t；

根据表达式rk＝r0·σa²调整卡尔曼滤波的量测噪声矩阵rk，其中，σa²表示加速度计输出的方差，n表示固定的采样数量，nk表示加速度计x、y、z三轴的输出σa²反应载体的动态大小，r0表示基准的加速度计噪声矩阵。

进一步的，所述滤波修正单元204还用于：

根据表达式确定载体的航向角ψm，其中，θ、φ表示确定的横滚角和俯仰角，表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，表示经过倾斜补偿后磁强计x、y、z轴的输出，m^w表示磁强计输出经过倾斜补偿后的输出向量；

当有外部航向信息时，接收并存储外部航向信息，根据表达式ψm＝ψext进行修正，其中，ψext表示外部航向信息；

根据表达式和构建系数矩阵，其中，x1、x2、x3、x4表示状态向量中得到的四元数，hψ表示航向信息量测更新的系数矩阵；

根据表达式计算增益矩阵，其中，kψ表示航向信息量测更新的增益矩阵，σψ为利用磁强计信息计算得到的航向信息的方差信息或者外部航向信息的方差信息，p表示卡尔曼滤波的方差矩阵；

根据表达式xi＝xi+ki(ψm-ψ)更新卡尔曼滤波状态向量，其中，i＝1,2,3...7表示卡尔曼滤波状态向量的元素，ki表示航向信息量测更新的增益矩阵，ψ表示利用惯导数据预测的载体的航向角。

进一步的，所述反馈补偿单元205还用于：

根据表达式对陀螺零偏进行更新，其中，bgx(t)、bgy(t)、bgz(t)表示当前时刻的x、y、z轴陀螺零偏，bgx(t-)、bgy(t-)、bgz(t-)表示先前的x、y、z轴陀螺零偏，x5＝0，x6＝0，x7＝0；

根据表达式进行反馈补偿，其中，ωx、ωy、ωz表示陀螺x、y、z轴的输出。

参见图3，图3是本发明一个实施例提供的一种终端设备的示意框图。如图3所示，该实施例的终端设备30包括：处理器300、存储器301以及存储在所述存储器301中并可在所述处理器300上运行的计算机程序302，例如姿态测量程序。所述处理器300执行所述计算机程序302时实现上述各个姿态测量方法实施例中的步骤，例如图1所示的步骤101至105。或者，所述处理器300执行所述计算机程序302时实现上述各装置实施例中各单元的功能，例如图2所示单元201至205的功能。

所述计算机程序302可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器301中，并由所述处理器300执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序302在所述终端设备30中的执行过程。例如，所述计算机程序302可以被分割成姿态初始化单元、姿态更新单元、量测方程构建单元、滤波修正单元和反馈补偿单元，各单元具体功能如下：

在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量；

利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测；

根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵；

根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量；

若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿。

进一步的，所述在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量包括：

根据表达式确定载体的横滚角φ，根据表达式确定载体的俯仰角θ，其中，表示静态场景下加速度计x、y、z轴输出的平均值；

根据表达式确定载体的航向角ψ，其中，表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，表示经过倾斜补偿后磁强计x、y、z轴的输出，m^w表示磁强计输出经过倾斜补偿后的输出向量；

根据表达式确定载体的姿态角四元数向量其中，其中q0、q1、q2、q3表示姿态角四元数；

根据表达式初始化卡尔曼滤波状态向量x0。

进一步的，所述利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测包括：

根据表达式更新姿态角四元数q0、q1、q2、q3，其中，q0+、q1+、q2+、q3+表示更新后的姿态角四元数，ωx、ωy、ωz表示陀螺x、y、z轴的输出；

根据表达式确定卡尔曼滤波的状态转移矩阵f；

根据表达式对状态向量进行预测，根据表达式对误差矩阵进行预测，其中，和表示第k-1历元预测后的状态向量和预测前的状态向量，k表示历元数，pk-1和pk,k-1表示第k-1历元预测前的卡尔曼滤波方差矩阵和预测后的卡尔曼滤波方差矩阵，qk-1表示第k-1历元的量测噪声矩阵。

进一步的，所述根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵包括：

根据表达式构建卡尔曼滤波的量测系数矩阵hk，其中，q0、q1、q2、q3表示姿态角四元数，g表示当地的重力大小；

根据表达式计算卡尔曼滤波的量测信息zk，为从n系到b系的方向余弦矩阵，φ、θ、ψ表示载体的横滚角、俯仰角和航向角，gⁿ＝[00g]^t；

根据表达式rk＝r0·σa²调整卡尔曼滤波的量测噪声矩阵rk，其中，σa²表示加速度计输出的方差，n表示固定的采样数量，nk表示加速度计x、y、z三轴的输出σa²反应载体的动态大小，r0表示基准的加速度计噪声矩阵。

进一步的，所述根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量包括：

根据表达式确定载体的航向角ψm，其中，θ、φ表示确定的横滚角和俯仰角，表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，表示经过倾斜补偿后磁强计x、y、z轴的输出，m^w表示磁强计输出经过倾斜补偿后的输出向量；

当有外部航向信息时，接收并存储外部航向信息，根据表达式ψm＝ψext进行修正，其中，ψext表示外部航向信息；

根据表达式和构建系数矩阵，其中，x1、x2、x3、x4表示状态向量中得到的四元数，hψ表示航向信息量测更新的系数矩阵；

根据表达式计算增益矩阵，其中，kψ表示航向信息量测更新的增益矩阵，σψ为利用磁强计信息计算得到的航向信息的方差信息或者外部航向信息的方差信息，p表示卡尔曼滤波的方差矩阵；

根据表达式xi＝xi+ki(ψm-ψ)更新卡尔曼滤波状态向量，其中，i＝1,2,3...7表示卡尔曼滤波状态向量的元素，ki表示航向信息量测更新的增益矩阵，ψ表示利用惯导数据预测的载体的航向角。

进一步的，所述根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿包括：

根据表达式对陀螺零偏进行更新，其中，bgx(t)、bgy(t)、bgz(t)表示当前时刻的x、y、z轴陀螺零偏，bgx(t-)、bgy(t-)、bgz(t-)表示先前的x、y、z轴陀螺零偏，x5＝0，x6＝0，x7＝0；

根据表达式进行反馈补偿，其中，ωx、ωy、ωz表示陀螺x、y、z轴的输出。

上述方案针对实际应用中ahrs系统中低成本mems陀螺和加速度计误差受外部环境干扰波动较大影响测姿精度提出利用加速度计输出来判断载体的运动状态，利用静态场景的数据准确估计陀螺零偏；针对ahrs系统中航向角可观测性交叉，并且容易受到外部干扰的问题，通过将横滚角、俯仰角和航向角进行独立的滤波来提高ahrs数据融合算法的可靠性和准确性；并提出利用加速度计输出判断载体动态从而调整加表输出权重的方法，从而得到稳定可靠地高精度姿态信息，同时根据加速度计的输出判断载体的运动状态以调整零偏的估计策略从而提高系统量测精度，设计水平姿态和航向独立的滤波保证航向信息和水平姿态信息的独立性，从而有效提高系统的稳定性。

所述终端设备30可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述终端设备可包括，但不仅限于，处理器300、存储器301。本领域技术人员可以理解，图3仅仅是终端设备30的示例，并不构成对终端设备30的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述终端设备还可以包括输入输出设备、网络接入设备、总线等。

所称处理器300可以是中央处理单元(centralprocessingunit，cpu)，还可以是其他通用处理器、数字信号处理器(digitalsignalprocessor，dsp)、专用集成电路(applicationspecificintegratedcircuit，asic)、现成可编程门阵列(field-programmablegatearray，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述存储器301可以是所述终端设备30的内部存储单元，例如终端设备30的硬盘或内存。所述存储器301也可以是所述终端设备30的外部存储设备，例如所述终端设备30上配备的插接式硬盘，智能存储卡(smartmediacard，smc)，安全数字(securedigital，sd)卡，闪存卡(flashcard)等。进一步地，所述存储器301还可以既包括所述终端设备30的内部存储单元也包括外部存储设备。所述存储器301用于存储所述计算机程序以及所述终端设备所需的其他程序和数据。所述存储器301还可以用于暂时地存储已经输出或者将要输出的数据。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

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

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

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(read-onlymemory，rom)、随机存取存储器(randomaccessmemory，ram)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括是电载波信号和电信信号。

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