CN109163721B - 姿态测量方法及终端设备 - Google Patents

姿态测量方法及终端设备 Download PDF

Info

Publication number
CN109163721B
CN109163721B CN201811089340.0A CN201811089340A CN109163721B CN 109163721 B CN109163721 B CN 109163721B CN 201811089340 A CN201811089340 A CN 201811089340A CN 109163721 B CN109163721 B CN 109163721B
Authority
CN
China
Prior art keywords
angle
carrier
output
matrix
attitude
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201811089340.0A
Other languages
English (en)
Other versions
CN109163721A (zh
Inventor
王青江
付迪
于翔
李博
闫勇伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
HEBEI MEITAI ELECTRONIC TECHNOLOGY CO LTD
Original Assignee
HEBEI MEITAI ELECTRONIC TECHNOLOGY CO LTD
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by HEBEI MEITAI ELECTRONIC TECHNOLOGY CO LTD filed Critical HEBEI MEITAI ELECTRONIC TECHNOLOGY CO LTD
Priority to CN201811089340.0A priority Critical patent/CN109163721B/zh
Publication of CN109163721A publication Critical patent/CN109163721A/zh
Application granted granted Critical
Publication of CN109163721B publication Critical patent/CN109163721B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

本发明适用于导航测姿技术领域，提供了一种姿态测量方法及终端设备，所述方法包括：依据陀螺、加速度计以及磁强计或者外部航向信息初始化卡尔曼滤波的状态量；构建卡尔曼滤波的状态方程，对载体的姿态信息和传感器误差进行预测；构建量测方程，利用加速度计输出对水平姿态横滚角和俯仰角进行量测更新，并且根据载体的运动状态自适应调整量测方差矩阵；构建航向角的量测方程并且利用磁强计或者外部航向信息对AHRS系统的航向进行量测更新；根据加速度计的输出判断载体的运动状态，判断陀螺零偏误差的反馈策略，利用上述估计的陀螺零偏误差反馈修正陀螺的原始输出，可以解决现有AHRS系统无法为载体姿态解算提供准确可靠信息的问题。

Description

姿态测量方法及终端设备
技术领域
本发明属于导航测姿技术领域，尤其涉及一种姿态测量方法及终端设备。
背景技术
精确制导武器、无人机以及水下机器人发展日新月异，对姿态测量提出了越来越高的要求。惯性导航系统(Inertial Navigation System，INS)利用其陀螺和加速度计的输出利用一定的算法推算载体的位置、速度、姿态信息。然而由于陀螺和加速度计输出存在误差，惯性导航系统推算的位置、速度、姿态信息会随时间积累，长时间导航会导致导航信息的发散。全球导航卫星系统(Global Navigation Satellite System，GNSS)、INS具有良好的互补特性，是常用的组合导航方式。根据组合方式，GNSS/INS组合导航可以分为松组合、紧组合以及深组合。松组合利用GNSS提供的位置和速度信息作为观测量进行组合，因此要求卫星的可见星数大于四颗。紧组合采用的观测量是卫星伪距和伪距率信息，在观测星数小于四颗的情况下仍然具有一定的导航定位能力。GNSS/INS组合导航系统在卫导信号良好时能够提供稳定可靠的位置、速度以及姿态信息，然而在城市峡谷、森林以及水下等信号遮挡严重的场景无法提供稳定可靠地位置、速度、姿态信息，除此之外GNSS信号容易受到电磁干扰等影响从而严重影响导航信息精度。因此，现有一般采用航姿参考系统(Attitude andHeading Reference System，AHRS)进行姿态测量。
AHRS系统由三轴陀螺仪、三轴加速度计以及三轴磁强计组成。AHRS系统包含了姿态和航向解算算法，融合陀螺、加速度计和磁强计的输出获取载体的横滚、俯仰以及航向信息。通常利用陀螺的数据不断地推算载体的姿态信息而加速度计和磁强计的输出用来修正利用陀螺计算的角度信息，从而实现数据的融合。陀螺零偏和加速度计零偏是影响测姿精度的重要因素，为了得到高精度的姿态信息，数据融合算法必须能够准确地估计出陀螺和加速度计的零偏误差。低精度MEMS器件具有重量轻、体积小功耗低等优势，然而受加工工艺的限制，其误差参数容易受到温度、湿度以及气压等外部因素的影响。如陀螺的零偏误差其随温度变化的幅度可达上百deg/h，比例因子随温度变化的幅度可达上千ppm，而加速度计的零偏误差随温度变化幅度可达数十mg，比例因子误差随温度变化幅度可达上千ppm。当载体处于静止状态时，忽略掉加速度计的噪声，加速度计三个轴向的输出为重力在三个轴向的投影，此时利用陀螺和加速度计的输出可以准确地获得载体的姿态信息，然而当载体处于运动状态并且具有加速度时，此时由于加速度的干扰，无法利用加速度计的输出来得到稳定的姿态信息也无法准确估计出陀螺零偏等误差，从而极大的影响姿态测量的精度。而且在AHRS系统中，由于重力的存在水平姿态角包括横滚角和俯仰角的可观测性较好，因此横滚角和俯仰角能够保持较高的精度，而航向角的可观测性最差，通常通过磁强计或者外部信息辅助修正从而保持航向精度，例如通常通过磁强计测量的地磁场信息对陀螺推算的航向信息进行修正，而在动中通领域，会利用天线跟踪卫星的电平信号对航向信息进行修正，然而在修正时，磁场很容易收到外部信息的干扰，从而产生较大的量测误差，如果在滤波中采用了误差较大的航向量测信息，容易导致滤波的不稳定甚至影响横滚角和俯仰角精度。另外，AHRS系统中利用陀螺来预测载体的姿态信息，利用加速度计的输出对水平姿态角进行修正，当载体处于静止或者匀速直线运动时，加速度计能够准确地测量出重力在各个轴向的分量，利用该信息可以对姿态信息进行准确地估计。然而当载体处于较大加速度的动态下时，加速度计受到载体加速度的影响无法为载体姿态解算提供准确可靠的信息。
发明内容
有鉴于此，本发明实施例提供了一种姿态测量方法及终端设备，以解决现有AHRS系统中低成本MEMS陀螺和加速度计误差受外部环境干扰波动较大影响测姿精度、航向角可观测性较差且容易受到外部干扰、无法为载体姿态解算提供准确可靠信息的问题。
本发明实施例的第一方面提供了一种姿态测量方法，包括：
在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量；
利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测；
根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵；
根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量；
若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿。
本发明实施例的第二方面提供了一种姿态测量装置，包括：
姿态初始化单元，用于在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量；
姿态更新单元，用于利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测；
量测方程构建单元，用于根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵；
滤波修正单元，用于根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量；
反馈补偿单元，用于若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿。
本发明实施例的第三方面提供了一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述方法的步骤。
本发明实施例的第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上述方法的步骤。
本发明实施例与现有技术相比存在的有益效果是：本发明实施例针对实际应用中AHRS系统中低成本MEMS陀螺和加速度计误差受外部环境干扰波动较大影响测姿精度提出利用加速度计输出来判断载体的运动状态，利用静态场景的数据准确估计陀螺零偏；针对AHRS系统中航向角可观测性交叉，并且容易受到外部干扰的问题，通过将横滚角、俯仰角和航向角进行独立的滤波来提高AHRS数据融合算法的可靠性和准确性；并提出利用加速度计输出判断载体动态从而调整加表输出权重的方法，从而得到稳定可靠地高精度姿态信息，同时根据加速度计的输出判断载体的运动状态以调整零偏的估计策略从而提高系统量测精度，设计水平姿态和航向独立的滤波保证航向信息和水平姿态信息的独立性，从而有效提高系统的稳定性。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种姿态测量方法的示意流程图；
图2是本发明实施例提供的一种姿态测量装置示意性框图；
图3是本发明实施例提供的一种终端设备的示意性框图。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本发明实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本发明。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本发明的描述。
为了说明本发明所述的技术方案，下面通过具体实施例来进行说明。
参见图1，图1是本发明实施例提供的一种姿态测量方法的示意流程图，在该实施例中，以终端的角度触发为例进行说明，这里，终端可以为智能手机、平板电脑等移动终端。如图1所示，在该实施例中，终端的处理过程可以包括以下步骤：
S101：在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量。
这里，对初始横滚角、俯仰角和航向角，状态量以及噪声矩阵进行初始化。如果应用中可以通过外部辅助初始化则利用外部信息进行横滚角、俯仰角和航向角的初始化。如果无外部辅助则利用加速度计和磁强计的输出初始化横滚角、俯仰角和航向角。在该实施例中利用加速度计的输出初始化横滚角和俯仰角，并且为了提高初始对准的精度，首先使载体处于静止状态，利用静止状态下的输出计算载体的横滚和俯仰角，计算方法如下：
Figure GDA0001872497180000061
其中，φ代表载体的横滚角，θ代表载体的俯仰角，
Figure GDA0001872497180000062
代表静态场景下加速度计x、y、z轴输出的平均值。
在利用磁强计的输出计算航向角之前首先对磁强计进行标定，该实施例中可以利用二维标定方法，旋转AHRS系统使航向遍历0～360°，旋转过程中使横滚角和俯仰角保持在零度附近，并且记录x、y、z各个轴线输出的最大值和最小值Mxmax、Mxmin，Mymax、Mymin，Mzmax、Mzmin，根据磁强计输出的最大值和最小值来求取磁强计各个轴线的改正值，计算得到磁强计x、y、z轴的补偿值为：
Figure GDA0001872497180000063
利用计算得到的磁强计各轴的补偿值biasx、biasy、biasz来补偿磁强计的输出，得到补偿后的磁强计输出，补偿方法如下：
Figure GDA0001872497180000064
式中
Figure GDA0001872497180000065
为磁强计的原始输出，
Figure GDA0001872497180000066
为校正后的磁强计输出。
计磁强计经过校正后的x、y、z轴的均值依次为：
Figure GDA0001872497180000067
通过下式计算得到载体的航向信息：
Figure GDA0001872497180000071
其中，
Figure GDA0001872497180000072
γm为初始对准当地的磁偏角，其值可以结合纬度信息查阅资料得到。
得到载体的姿态角后便可以计算对应的姿态角四元数
Figure GDA0001872497180000073
并且利用得到的四元数初始化状态向量：
Figure GDA0001872497180000074
其中
Figure GDA0001872497180000075
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
Figure GDA0001872497180000076
其中q0+、q1+、q2+、q3+代表更新后的姿态四元数，q0、q1、q2、q3代表更新前的姿态四元数，ωx、ωy、ωz依次代表陀螺x、y、z轴的输出，F矩阵代表卡尔曼滤波的状态转移矩阵。对状态向量和误差矩阵进行预测的方法如下：
Figure GDA0001872497180000081
Figure GDA0001872497180000082
其中，
Figure GDA0001872497180000083
和
Figure GDA0001872497180000084
表示第k-1历元预测后的状态向量和预测前的状态向量，k表示历元数，就是一个计数表示第几个历元(每次导航解算就算一个计算历元)，Pk-1和Pk,k-1表示第k-1历元预测前的卡尔曼滤波方差矩阵和预测后的卡尔曼滤波方差矩阵，Qk-1表示第k-1历元的量测噪声矩阵。
S103：根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵。
这里，构建卡尔曼滤波量测矩阵，该步骤利用加速度计的输出和方向余弦矩阵投影计算的重力矢量之间的差值作为量测新息，量测系数矩阵如下所示：
Figure GDA0001872497180000085
在上式中g代表当地的重力大小，q0、q1、q2、q3代表当前历元计算得到的姿态四元数；
计算量测信息zk：
Figure GDA0001872497180000086
其中，
Figure GDA0001872497180000087
为从n系到b系的方向余弦矩阵，计算方法如下：
Figure GDA0001872497180000088
其中，φ、θ、ψ依次代表载体的横滚、俯仰和姿态角，其值可以利用姿态四元数计算得到，gn＝[00g]T，g为当地重力值。
量测噪声矩阵的设定需要根据载体的动态进行自适应的判断，为了能够准确的反应载体的运动状态，采用求取固定采样时段方差的方法设定量测方差，具体的计算方法如下：
Figure GDA0001872497180000091
Figure GDA0001872497180000092
式中，N为固定的采样数量，
Figure GDA0001872497180000093
为加速度计x、y、z三轴的输出σa 2反应载体的动态大小，用来调整卡尔曼滤波的量测噪声矩阵：
Rk＝R0·σa 2
式中，R0表示基准的加速度计噪声矩阵。
上述步骤目的是构建卡尔曼滤波的量测方程，构建量测方程需要构建量测系数矩阵、需要计算量测信息并且需要确定方程的量测噪声矩阵，他们之间是独立的，目的都是为了建立量测方程。
S104：根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量。
具体地，利用磁强计或者外部信息对航向进行滤波。
首先计算磁强计推算的航向信息，方法如下：
Figure GDA0001872497180000094
其中，
Figure GDA0001872497180000101
θ、φ分别为计算得到的横滚角和俯仰角，ψ代表利用磁强计计算得到的航向信息；γm为初始对准当地的磁偏角，其值可以结合纬度信息查阅资料得到；
Figure GDA0001872497180000102
分别为经过补偿后的磁强计输出的x、y、z轴的均值，
Figure GDA0001872497180000103
Figure GDA0001872497180000104
依次代表经过倾斜补偿后磁强计x、y、z轴的输出。mw为磁强计输出经过倾斜补偿后的输出向量。
当有外部信息时接收并存储外部信息，
ψm＝ψExt
利用外部信息进行修正，其中，ψExt表示外部航向信息。
构建系数矩阵
Figure GDA0001872497180000105
Figure GDA0001872497180000106
Figure GDA0001872497180000107
Figure GDA0001872497180000108
其中x1、x2、x3、x4为状态量中估计得到的四元数，Hψ代表航向信息量测更新的系数矩阵，
Figure GDA0001872497180000109
表示构建H矩阵的中间变量。
计算增益矩阵：
Figure GDA00018724971800001010
其中Kψ代表航向信息量测更新的增益矩阵，σψ为利用磁强计信息计算得到的航向信息的方差信息或者外部航向信息的方差信息，P表示卡尔曼滤波的方差矩阵。
状态更新：
xi＝xi+Ki(ψm-ψ)
其中，i＝1,2,3...7依次代表卡尔曼滤波状态量的元素，Ki表示航向信息量测更新的增益矩阵，ψ表示利用惯导数据预测的载体的航向角。
S105：若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿。
这里，首先根据步骤S103中计算得到的加速度计输出的方差值和加速度的模值来判断载体的运动状态，当σa 2小于一定阈值后则认为载体处于静止状态，此时滤波器能够较好的估计出零偏误差，因此对陀螺零偏进行更新，并且将状态量置零：
Figure GDA0001872497180000111
Figure GDA0001872497180000112
Figure GDA0001872497180000113
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数据融合算法的可靠性和准确性；并提出利用加速度计输出判断载体动态从而调整加表输出权重的方法，从而得到稳定可靠地高精度姿态信息，同时根据加速度计的输出判断载体的运动状态以调整零偏的估计策略从而提高系统量测精度，设计水平姿态和航向独立的滤波保证航向信息和水平姿态信息的独立性，从而有效提高系统的稳定性。
此外，在一个具体示例中，所述在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量包括：
根据表达式
Figure GDA0001872497180000121
确定载体的横滚角φ，根据表达式
Figure GDA0001872497180000122
确定载体的俯仰角θ，其中，
Figure GDA0001872497180000123
表示静态场景下加速度计x、y、z轴输出的平均值；
根据表达式
Figure GDA0001872497180000124
确定载体的航向角ψ，其中，
Figure GDA0001872497180000125
Figure GDA0001872497180000126
表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，
Figure GDA0001872497180000127
表示经过倾斜补偿后磁强计x、y、z轴的输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
根据表达式
Figure GDA0001872497180000128
确定载体的姿态角四元数向量
Figure GDA0001872497180000129
其中，
Figure GDA00018724971800001210
其中q0、q1、q2、q3表示姿态角四元数；
根据表达式
Figure GDA0001872497180000131
初始化卡尔曼滤波状态向量X0。
此外，在一个具体示例中，所述利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测包括：
根据表达式
Figure GDA0001872497180000132
更新姿态角四元数q0、q1、q2、q3，其中，q0+、q1+、q2+、q3+表示更新后的姿态角四元数，ωx、ωy、ωz表示陀螺x、y、z轴的输出；
根据表达式
Figure GDA0001872497180000133
确定卡尔曼滤波的状态转移矩阵F；
根据表达式
Figure GDA0001872497180000134
对状态向量进行预测，根据表达式
Figure GDA0001872497180000135
对误差矩阵进行预测，其中，
Figure GDA0001872497180000136
和
Figure GDA0001872497180000137
表示第k-1历元预测后的状态向量和预测前的状态向量，k表示历元数，Pk-1和Pk,k-1表示第k-1历元预测前的卡尔曼滤波方差矩阵和预测后的卡尔曼滤波方差矩阵，Qk-1表示第k-1历元的量测噪声矩阵。
此外，在一个具体示例中，所述根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵包括：
根据表达式
Figure GDA0001872497180000141
构建卡尔曼滤波的量测系数矩阵Hk，其中，q0、q1、q2、q3表示姿态角四元数，g表示当地的重力大小；
根据表达式
Figure GDA0001872497180000142
计算卡尔曼滤波的量测信息zk，
Figure GDA0001872497180000143
为从n系到b系的方向余弦矩阵，
Figure GDA0001872497180000144
φ、θ、ψ表示载体的横滚角、俯仰角和航向角，gn＝[0 0 g]T；
根据表达式Rk＝R0·σa 2调整卡尔曼滤波的量测噪声矩阵Rk，其中，σa 2表示加速度计输出的方差，
Figure GDA0001872497180000145
N表示固定的采样数量，nk表示加速度计x、y、z三轴的输出σa 2反应载体的动态大小，R0表示基准的加速度计噪声矩阵。
此外，在一个具体示例中，所述根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量包括：
根据表达式
Figure GDA0001872497180000146
确定载体的航向角ψm，其中，
Figure GDA0001872497180000147
θ、φ表示确定的横滚角和俯仰角，
Figure GDA0001872497180000148
表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，
Figure GDA0001872497180000151
表示经过倾斜补偿后磁强计x、y、z轴的输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
当有外部航向信息时，接收并存储外部航向信息，根据表达式ψm＝ψExt进行修正，其中，ψExt表示外部航向信息；
根据表达式
Figure GDA0001872497180000152
和
Figure GDA0001872497180000153
构建系数矩阵，其中，x1、x2、x3、x4表示状态向量中得到的四元数，Hψ表示航向信息量测更新的系数矩阵；
根据表达式
Figure GDA0001872497180000154
计算增益矩阵，其中，Kψ表示航向信息量测更新的增益矩阵，σψ为利用磁强计信息计算得到的航向信息的方差信息或者外部航向信息的方差信息，P表示卡尔曼滤波的方差矩阵；
根据表达式xi＝xi+Ki(ψm-ψ)更新卡尔曼滤波状态向量，其中，i＝1,2,3...7表示卡尔曼滤波状态向量的元素，Ki表示航向信息量测更新的增益矩阵，ψ表示利用惯导数据预测的载体的航向角。
此外，在一个具体示例中，所述根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿包括：
根据表达式
Figure GDA0001872497180000155
对陀螺零偏进行更新，其中，bgx(t)、bgy(t)、bgz(t)表示当前时刻的x、y、z轴陀螺零偏，bgx(t-)、bgy(t-)、bgz(t-)表示先前的x、y、z轴陀螺零偏，x5＝0，x6＝0，x7＝0；
根据表达式
Figure GDA0001872497180000156
进行反馈补偿，其中，ω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还用于：
根据表达式
Figure GDA0001872497180000171
确定载体的横滚角φ，根据表达式
Figure GDA0001872497180000172
确定载体的俯仰角θ，其中，
Figure GDA0001872497180000173
表示静态场景下加速度计x、y、z轴输出的平均值；
根据表达式
Figure GDA0001872497180000174
确定载体的航向角ψ，其中，
Figure GDA0001872497180000175
Figure GDA0001872497180000176
表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，
Figure GDA0001872497180000177
表示经过倾斜补偿后磁强计x、y、z轴的输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
根据表达式
Figure GDA0001872497180000178
确定载体的姿态角四元数向量
Figure GDA0001872497180000179
其中，
Figure GDA00018724971800001710
其中q0、q1、q2、q3表示姿态角四元数；
根据表达式
Figure GDA0001872497180000181
初始化卡尔曼滤波状态向量X0。
进一步的，所述姿态更新单元202还用于：
根据表达式
Figure GDA0001872497180000182
更新姿态角四元数q0、q1、q2、q3，其中，q0+、q1+、q2+、q3+表示更新后的姿态角四元数，ωx、ωy、ωz表示陀螺x、y、z轴的输出；
根据表达式
Figure GDA0001872497180000183
确定卡尔曼滤波的状态转移矩阵F；
根据表达式
Figure GDA0001872497180000184
对状态向量进行预测，根据表达式
Figure GDA0001872497180000185
对误差矩阵进行预测，其中，
Figure GDA0001872497180000186
和
Figure GDA0001872497180000187
表示第k-1历元预测后的状态向量和预测前的状态向量，k表示历元数，Pk-1和Pk,k-1表示第k-1历元预测前的卡尔曼滤波方差矩阵和预测后的卡尔曼滤波方差矩阵，Qk-1表示第k-1历元的量测噪声矩阵。
进一步的，所述量测方程构建单元203还用于：
根据表达式
Figure GDA0001872497180000188
构建卡尔曼滤波的量测系数矩阵Hk，其中，q0、q1、q2、q3表示姿态角四元数，g表示当地的重力大小；
根据表达式
Figure GDA0001872497180000189
计算卡尔曼滤波的量测信息zk，
Figure GDA00018724971800001810
为从n系到b系的方向余弦矩阵，
Figure GDA0001872497180000191
φ、θ、ψ表示载体的横滚角、俯仰角和航向角，gn＝[00g]T；
根据表达式Rk＝R0·σa 2调整卡尔曼滤波的量测噪声矩阵Rk，其中，σa 2表示加速度计输出的方差，
Figure GDA0001872497180000192
N表示固定的采样数量，nk表示加速度计x、y、z三轴的输出σa 2反应载体的动态大小，R0表示基准的加速度计噪声矩阵。
进一步的，所述滤波修正单元204还用于：
根据表达式
Figure GDA0001872497180000193
确定载体的航向角ψm，其中，
Figure GDA0001872497180000194
θ、φ表示确定的横滚角和俯仰角，
Figure GDA0001872497180000195
表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，
Figure GDA0001872497180000196
表示经过倾斜补偿后磁强计x、y、z轴的输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
当有外部航向信息时，接收并存储外部航向信息，根据表达式ψm＝ψExt进行修正，其中，ψExt表示外部航向信息；
根据表达式
Figure GDA0001872497180000197
和
Figure GDA0001872497180000198
构建系数矩阵，其中，x1、x2、x3、x4表示状态向量中得到的四元数，Hψ表示航向信息量测更新的系数矩阵；
根据表达式
Figure GDA0001872497180000201
计算增益矩阵，其中，Kψ表示航向信息量测更新的增益矩阵，σψ为利用磁强计信息计算得到的航向信息的方差信息或者外部航向信息的方差信息，P表示卡尔曼滤波的方差矩阵；
根据表达式xi＝xi+Ki(ψm-ψ)更新卡尔曼滤波状态向量，其中，i＝1,2,3...7表示卡尔曼滤波状态向量的元素，Ki表示航向信息量测更新的增益矩阵，ψ表示利用惯导数据预测的载体的航向角。
进一步的，所述反馈补偿单元205还用于：
根据表达式
Figure GDA0001872497180000202
对陀螺零偏进行更新，其中，bgx(t)、bgy(t)、bgz(t)表示当前时刻的x、y、z轴陀螺零偏，bgx(t-)、bgy(t-)、bgz(t-)表示先前的x、y、z轴陀螺零偏，x5＝0，x6＝0，x7＝0；
根据表达式
Figure GDA0001872497180000203
进行反馈补偿，其中，ω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可以被分割成姿态初始化单元、姿态更新单元、量测方程构建单元、滤波修正单元和反馈补偿单元，各单元具体功能如下：
在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量；
利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测；
根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵；
根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量；
若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿。
进一步的，所述在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量包括：
根据表达式
Figure GDA0001872497180000211
确定载体的横滚角φ，根据表达式
Figure GDA0001872497180000212
确定载体的俯仰角θ，其中，
Figure GDA0001872497180000213
表示静态场景下加速度计x、y、z轴输出的平均值；
根据表达式
Figure GDA0001872497180000221
确定载体的航向角ψ，其中，
Figure GDA0001872497180000222
Figure GDA0001872497180000223
表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，
Figure GDA0001872497180000224
表示经过倾斜补偿后磁强计x、y、z轴的输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
根据表达式
Figure GDA0001872497180000225
确定载体的姿态角四元数向量
Figure GDA0001872497180000226
其中，
Figure GDA0001872497180000227
其中q0、q1、q2、q3表示姿态角四元数；
根据表达式
Figure GDA0001872497180000228
初始化卡尔曼滤波状态向量X0。
进一步的，所述利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测包括：
根据表达式
Figure GDA0001872497180000229
更新姿态角四元数q0、q1、q2、q3，其中，q0+、q1+、q2+、q3+表示更新后的姿态角四元数，ωx、ωy、ωz表示陀螺x、y、z轴的输出；
根据表达式
Figure GDA0001872497180000231
确定卡尔曼滤波的状态转移矩阵F；
根据表达式
Figure GDA0001872497180000232
对状态向量进行预测，根据表达式
Figure GDA0001872497180000233
对误差矩阵进行预测，其中，
Figure GDA0001872497180000234
和
Figure GDA0001872497180000235
表示第k-1历元预测后的状态向量和预测前的状态向量，k表示历元数，Pk-1和Pk,k-1表示第k-1历元预测前的卡尔曼滤波方差矩阵和预测后的卡尔曼滤波方差矩阵，Qk-1表示第k-1历元的量测噪声矩阵。
进一步的，所述根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵包括：
根据表达式
Figure GDA0001872497180000236
构建卡尔曼滤波的量测系数矩阵Hk，其中，q0、q1、q2、q3表示姿态角四元数，g表示当地的重力大小；
根据表达式
Figure GDA0001872497180000237
计算卡尔曼滤波的量测信息zk，
Figure GDA0001872497180000238
为从n系到b系的方向余弦矩阵，
Figure GDA0001872497180000239
φ、θ、ψ表示载体的横滚角、俯仰角和航向角，gn＝[00g]T；
根据表达式Rk＝R0·σa 2调整卡尔曼滤波的量测噪声矩阵Rk，其中，σa 2表示加速度计输出的方差，
Figure GDA0001872497180000241
N表示固定的采样数量，nk表示加速度计x、y、z三轴的输出σa 2反应载体的动态大小，R0表示基准的加速度计噪声矩阵。
进一步的，所述根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量包括：
根据表达式
Figure GDA0001872497180000242
确定载体的航向角ψm，其中，
Figure GDA0001872497180000243
θ、φ表示确定的横滚角和俯仰角，
Figure GDA0001872497180000244
表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，
Figure GDA0001872497180000245
表示经过倾斜补偿后磁强计x、y、z轴的输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
当有外部航向信息时，接收并存储外部航向信息，根据表达式ψm＝ψExt进行修正，其中，ψExt表示外部航向信息；
根据表达式
Figure GDA0001872497180000246
和
Figure GDA0001872497180000247
构建系数矩阵，其中，x1、x2、x3、x4表示状态向量中得到的四元数，Hψ表示航向信息量测更新的系数矩阵；
根据表达式
Figure GDA0001872497180000248
计算增益矩阵，其中，Kψ表示航向信息量测更新的增益矩阵，σψ为利用磁强计信息计算得到的航向信息的方差信息或者外部航向信息的方差信息，P表示卡尔曼滤波的方差矩阵；
根据表达式xi＝xi+Ki(ψm-ψ)更新卡尔曼滤波状态向量，其中，i＝1,2,3...7表示卡尔曼滤波状态向量的元素，Ki表示航向信息量测更新的增益矩阵，ψ表示利用惯导数据预测的载体的航向角。
进一步的，所述根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿包括：
根据表达式
Figure GDA0001872497180000251
对陀螺零偏进行更新，其中，bgx(t)、bgy(t)、bgz(t)表示当前时刻的x、y、z轴陀螺零偏，bgx(t-)、bgy(t-)、bgz(t-)表示先前的x、y、z轴陀螺零偏，x5＝0，x6＝0，x7＝0；
根据表达式
Figure GDA0001872497180000252
进行反馈补偿，其中，ωx、ωy、ωz表示陀螺x、y、z轴的输出。
上述方案针对实际应用中AHRS系统中低成本MEMS陀螺和加速度计误差受外部环境干扰波动较大影响测姿精度提出利用加速度计输出来判断载体的运动状态，利用静态场景的数据准确估计陀螺零偏；针对AHRS系统中航向角可观测性交叉，并且容易受到外部干扰的问题，通过将横滚角、俯仰角和航向角进行独立的滤波来提高AHRS数据融合算法的可靠性和准确性；并提出利用加速度计输出判断载体动态从而调整加表输出权重的方法，从而得到稳定可靠地高精度姿态信息，同时根据加速度计的输出判断载体的运动状态以调整零偏的估计策略从而提高系统量测精度，设计水平姿态和航向独立的滤波保证航向信息和水平姿态信息的独立性，从而有效提高系统的稳定性。
所述终端设备30可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述终端设备可包括，但不仅限于，处理器300、存储器301。本领域技术人员可以理解，图3仅仅是终端设备30的示例，并不构成对终端设备30的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述终端设备还可以包括输入输出设备、网络接入设备、总线等。
所称处理器300可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器301可以是所述终端设备30的内部存储单元，例如终端设备30的硬盘或内存。所述存储器301也可以是所述终端设备30的外部存储设备，例如所述终端设备30上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器301还可以既包括所述终端设备30的内部存储单元也包括外部存储设备。所述存储器301用于存储所述计算机程序以及所述终端设备所需的其他程序和数据。所述存储器301还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(Read-Only Memory，ROM)、随机存取存储器(RandomAccess Memory，RAM)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括是电载波信号和电信信号。
以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。

Claims (8)

1.一种姿态测量方法，其特征在于，包括：
在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量；
利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测；
根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵；
根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量；
若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿；
其中，所述在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量包括：
根据表达式
Figure FDA0002455855850000011
确定载体的横滚角φ，根据表达式
Figure FDA0002455855850000012
确定载体的俯仰角θ，其中，
Figure FDA0002455855850000013
表示静态场景下加速度计x、y、z轴输出的平均值；
根据表达式
Figure FDA0002455855850000021
确定载体的航向角ψ，其中，
Figure FDA0002455855850000022
Figure FDA0002455855850000023
表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，
Figure FDA0002455855850000024
表示经过倾斜补偿后磁强计x、y、z轴的输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
根据表达式
Figure FDA0002455855850000025
确定载体的姿态角四元数向量
Figure FDA0002455855850000026
其中，
Figure FDA0002455855850000027
其中q0、q1、q2、q3表示姿态角四元数；
根据表达式
Figure FDA0002455855850000028
初始化卡尔曼滤波状态向量X0。
2.如权利要求1所述的姿态测量方法，其特征在于，所述利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测包括：
根据表达式
Figure FDA0002455855850000029
更新姿态角四元数q0、q1、q2、q3，其中，q0+、q1+、q2+、q3+表示更新后的姿态角四元数，ωx、ωy、ωz表示陀螺x、y、z轴的输出；
根据表达式
Figure FDA0002455855850000031
确定卡尔曼滤波的状态转移矩阵F；
根据表达式
Figure FDA0002455855850000032
对状态向量进行预测，根据表达式
Figure FDA0002455855850000033
对误差矩阵进行预测，其中，
Figure FDA0002455855850000034
和
Figure FDA0002455855850000035
表示第k-1历元预测后的状态向量和预测前的状态向量，k表示历元数，Pk-1和Pk,k-1表示第k-1历元预测前的卡尔曼滤波方差矩阵和预测后的卡尔曼滤波方差矩阵，Qk-1表示第k-1历元的量测噪声矩阵。
3.如权利要求1所述的姿态测量方法，其特征在于，所述根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵包括：
根据表达式
Figure FDA0002455855850000036
构建卡尔曼滤波的量测系数矩阵Hk，其中，q0、q1、q2、q3表示姿态角四元数，g表示当地的重力大小；
根据表达式
Figure FDA0002455855850000037
计算卡尔曼滤波的量测信息zk，
Figure FDA0002455855850000038
为从n系到b系的方向余弦矩阵，
Figure FDA0002455855850000039
φ、θ、ψ表示载体的横滚角、俯仰角和航向角，gn＝[00g]T；
根据表达式Rk＝R0·σa 2调整卡尔曼滤波的量测噪声矩阵Rk，其中，σa 2表示加速度计输出的方差，
Figure FDA0002455855850000041
N表示固定的采样数量，nk表示加速度计x、y、z三轴的输出σa 2反应载体的动态大小，R0表示基准的加速度计噪声矩阵。
4.如权利要求1所述的姿态测量方法，其特征在于，所述根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量包括：
根据表达式
Figure FDA0002455855850000042
确定载体的航向角ψm，其中，
Figure FDA0002455855850000043
θ、φ表示确定的横滚角和俯仰角，
Figure FDA0002455855850000044
表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，
Figure FDA0002455855850000045
表示经过倾斜补偿后磁强计x、y、z轴的输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
当有外部航向信息时，接收并存储外部航向信息，根据表达式ψm＝ψExt进行修正，其中，ψExt表示外部航向信息；
根据表达式
Figure FDA0002455855850000046
和
Figure FDA0002455855850000047
构建系数矩阵，其中，x1、x2、x3、x4表示状态向量中得到的四元数，Hψ表示航向信息量测更新的系数矩阵；
根据表达式
Figure FDA0002455855850000051
计算增益矩阵，其中，Kψ表示航向信息量测更新的增益矩阵，σψ为利用磁强计信息计算得到的航向信息的方差信息或者外部航向信息的方差信息，P表示卡尔曼滤波的方差矩阵；
根据表达式xi＝xi+Ki(ψm-ψ)更新卡尔曼滤波状态向量，其中，i＝1,2,3...7表示卡尔曼滤波状态向量的元素，Ki表示航向信息量测更新的增益矩阵，ψ表示利用惯导数据预测的载体的航向角。
5.如权利要求1所述的姿态测量方法，其特征在于，所述根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿包括：
根据表达式
Figure FDA0002455855850000052
对陀螺零偏进行更新，其中，bgx(t)、bgy(t)、bgz(t)表示当前时刻的x、y、z轴陀螺零偏，bgx(t-)、bgy(t-)、bgz(t-)表示先前的x、y、z轴陀螺零偏，x5＝0，x6＝0，x7＝0；
根据表达式
Figure FDA0002455855850000053
进行反馈补偿，其中，ωx、ωy、ωz表示陀螺x、y、z轴的输出。
6.一种姿态测量装置，其特征在于，包括：
姿态初始化单元，用于在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量；
姿态更新单元，用于利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测；
量测方程构建单元，用于根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵；
滤波修正单元，用于根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量；
反馈补偿单元，用于若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿；
其中，所述姿态初始化单元还用于：
根据表达式
Figure FDA0002455855850000061
确定载体的横滚角φ，根据表达式
Figure FDA0002455855850000062
确定载体的俯仰角θ，其中，
Figure FDA0002455855850000063
表示静态场景下加速度计x、y、z轴输出的平均值；
根据表达式
Figure FDA0002455855850000064
确定载体的航向角ψ，其中，
Figure FDA0002455855850000065
Figure FDA0002455855850000066
表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，
Figure FDA0002455855850000067
表示经过倾斜补偿后磁强计x、y、z轴的输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
根据表达式
Figure FDA0002455855850000068
确定载体的姿态角四元数向量
Figure FDA0002455855850000069
其中，
Figure FDA0002455855850000071
其中q0、q1、q2、q3表示姿态角四元数；
根据表达式
Figure FDA0002455855850000072
初始化卡尔曼滤波状态向量X0。
7.一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至5任一项所述方法的步骤。
8.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至5任一项所述方法的步骤。
CN201811089340.0A 2018-09-18 2018-09-18 姿态测量方法及终端设备 Active CN109163721B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811089340.0A CN109163721B (zh) 2018-09-18 2018-09-18 姿态测量方法及终端设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811089340.0A CN109163721B (zh) 2018-09-18 2018-09-18 姿态测量方法及终端设备

Publications (2)

Publication Number Publication Date
CN109163721A CN109163721A (zh) 2019-01-08
CN109163721B true CN109163721B (zh) 2020-06-09

Family

ID=64879821

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811089340.0A Active CN109163721B (zh) 2018-09-18 2018-09-18 姿态测量方法及终端设备

Country Status (1)

Country Link
CN (1) CN109163721B (zh)

Families Citing this family (30)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018081795A1 (en) 2016-10-31 2018-05-03 Zipline Medical, Inc. Systems and methods for monitoring physical therapy of the knee and other joints
GB2574074B (en) 2018-07-27 2020-05-20 Mclaren Applied Tech Ltd Time synchronisation
CN110132271B (zh) * 2019-01-02 2022-04-12 中国船舶重工集团公司第七0七研究所 一种自适应卡尔曼滤波姿态估计算法
CN109827596A (zh) * 2019-04-02 2019-05-31 北京理工大学 一种间断运动条件下mems陀螺仪的零偏估计方法
CN110058288B (zh) * 2019-04-28 2021-04-06 北京微克智飞科技有限公司 无人机ins/gnss组合导航系统航向误差修正方法及系统
CN110032208B (zh) * 2019-05-20 2024-01-26 吉林大学 一种用于水下侦查与摄影无人机及其控制方法
CN110095115B (zh) * 2019-06-04 2020-12-25 中国科学院合肥物质科学研究院 一种基于地磁信息更新的载体航姿测量方法
CN110398245B (zh) * 2019-07-09 2021-04-16 武汉大学 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
GB2588236B (en) * 2019-10-18 2024-03-20 Mclaren Applied Ltd Gyroscope bias estimation
CN112859139B (zh) * 2019-11-28 2023-09-05 中移物联网有限公司 一种姿态测量方法、装置及电子设备
CN112859138B (zh) * 2019-11-28 2023-09-05 中移物联网有限公司 一种姿态测量方法、装置及电子设备
CN110926468B (zh) * 2019-12-05 2022-03-01 中国电子科技集团公司第五十四研究所 基于传递对准的动中通天线多平台航姿确定方法
CN110887480B (zh) * 2019-12-11 2020-06-30 中国空气动力研究与发展中心低速空气动力研究所 基于mems传感器的飞行姿态估计方法及系统
CN111895988A (zh) * 2019-12-20 2020-11-06 北京空天技术研究所 无人机导航信息更新方法及装置
CN111426318B (zh) * 2020-04-22 2024-01-26 中北大学 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN111623775B (zh) * 2020-05-15 2022-10-04 天津时空经纬测控技术有限公司 运载体姿态测量系统、方法以及存储介质
CN111551175B (zh) * 2020-05-27 2024-03-15 北京计算机技术及应用研究所 一种航姿参考系统的互补滤波姿态解算方法
CN112066984B (zh) * 2020-09-17 2022-08-30 深圳维特智能科技有限公司 姿态角度解算方法、装置、处理设备和存储介质
CN112405523A (zh) * 2020-10-20 2021-02-26 深圳拓邦股份有限公司 机器人碰撞检测方法、装置及机器人
CN112729280B (zh) * 2020-12-10 2022-09-13 山东省科学院海洋仪器仪表研究所 一种基于多面体式阵列结构的微惯性姿态测量装置及方法
CN112729348B (zh) * 2021-01-10 2023-11-28 河南理工大学 一种用于imu系统的姿态自适应校正方法
CN113155129B (zh) * 2021-04-02 2022-07-01 北京大学 一种基于扩展卡尔曼滤波的云台姿态估计方法
CN113175926B (zh) * 2021-04-21 2022-06-21 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法
CN113375694A (zh) * 2021-05-25 2021-09-10 南京航空航天大学 一种静基座条件下低成本陀螺全部零偏快速估计方法
CN114370885B (zh) * 2021-10-29 2023-10-13 北京自动化控制设备研究所 一种惯性导航系统误差补偿方法及系统
CN114264304B (zh) * 2021-12-23 2023-07-14 湖南航天机电设备与特种材料研究所 复杂动态环境高精度水平姿态测量方法与系统
CN114413934B (zh) * 2022-01-20 2024-01-26 北京经纬恒润科技股份有限公司 一种车辆定位系统校正方法和装置
CN114485641B (zh) * 2022-01-24 2024-03-26 武汉梦芯科技有限公司 一种基于惯导卫导方位融合的姿态解算方法及装置
CN114964230B (zh) * 2022-05-12 2023-11-03 北京自动化控制设备研究所 一种车载组合导航陀螺漂移修正方法
CN117848331A (zh) * 2024-03-06 2024-04-09 河北美泰电子科技有限公司 基于视觉标签地图的定位方法及装置

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN100516775C (zh) * 2006-08-23 2009-07-22 北京航空航天大学 一种捷联惯性导航系统初始姿态确定方法
CN106342284B (zh) * 2008-08-18 2011-11-23 西北工业大学 一种飞行载体姿态确定方法
CN101706281B (zh) * 2009-11-13 2011-08-31 南京航空航天大学 惯性/天文/卫星高精度组合导航系统及其导航方法
CN102589552B (zh) * 2012-01-19 2015-01-07 北京华力创通科技股份有限公司 低成本组合导航系统的数据融合方法和装置
CN102607562B (zh) * 2012-04-12 2014-10-29 南京航空航天大学 基于载体飞行模态判别的微惯性参数自适应姿态确定方法
CN104567871B (zh) * 2015-01-12 2018-07-24 哈尔滨工程大学 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法

Also Published As

Publication number Publication date
CN109163721A (zh) 2019-01-08

Similar Documents

Publication Publication Date Title
CN109163721B (zh) 姿态测量方法及终端设备
US9068843B1 (en) Inertial sensor fusion orientation correction
US9417091B2 (en) System and method for determining and correcting field sensors errors
US8756995B2 (en) Device and method for combining samples from an inertial measurement sensor cluster
CN109030867B (zh) 使用加速度传感器和地磁传感器计算角速度的方法和设备
US20230366680A1 (en) Initialization method, device, medium and electronic equipment of integrated navigation system
JP2009098125A (ja) 動的に較正されるセンサデータと、ナビゲーションシステム内の繰り返し拡張カルマンフィルタとを使用する、ジャイロコンパスの整合用のシステム及び方法
EP2587219B1 (en) Method to improve leveling performance in navigation systems
CN111076722B (zh) 基于自适应的四元数的姿态估计方法及装置
CN106813679B (zh) 运动物体的姿态估计的方法及装置
EP3527948B1 (en) Air data aided inertial measurement unit
KR101211703B1 (ko) 시선벡터를 이용한 자장계 오차 보정방법 및 이를 이용한 통합 항법 시스템
CN113253325B (zh) 惯性卫星序贯紧组合李群滤波方法
JP2019120587A (ja) 測位システム及び測位方法
CN115435817A (zh) Mems惯导安装误差的标定方法、存储介质和控制计算机
CN116817896A (zh) 一种基于扩展卡尔曼滤波的姿态解算方法
CN116147624A (zh) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN110645976A (zh) 一种移动机器人的姿态估计方法及终端设备
CN110375773B (zh) Mems惯导系统姿态初始化方法
Xuan Application of real-time Kalman filter with magnetic calibration for MEMS sensor in attitude estimation
US20180087906A1 (en) Device orientation systems and methods
CN118347496A (zh) 一种逆向导航方法及装置
CN117705097A (zh) 一种棱镜杆装置、地物碎部点测量方法、设备及介质
Yu et al. Research on Navigation Technology of Small UAV Based on MEMS Sensor

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant