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

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

Info

Publication number
CN109163721A
CN109163721A CN201811089340.0A CN201811089340A CN109163721A CN 109163721 A CN109163721 A CN 109163721A CN 201811089340 A CN201811089340 A CN 201811089340A CN 109163721 A CN109163721 A CN 109163721A
Authority
CN
China
Prior art keywords
angle
carrier
output
indicate
course
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.)
Granted
Application number
CN201811089340.0A
Other languages
English (en)
Other versions
CN109163721B (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.)
MT MICROSYSTEMS CO Ltd
Original Assignee
MT MICROSYSTEMS 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 MT MICROSYSTEMS CO Ltd filed Critical MT MICROSYSTEMS 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

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

Abstract

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

Description

姿态测量方法及终端设备
技术领域
本发明属于导航测姿技术领域，尤其涉及一种姿态测量方法及终端设备。
背景技术
精确制导武器、无人机以及水下机器人发展日新月异，对姿态测量提出了 越来越高的要求。惯性导航系统(Inertial Navigation System，INS)利用其陀螺和 加速度计的输出利用一定的算法推算载体的位置、速度、姿态信息。然而由于 陀螺和加速度计输出存在误差，惯性导航系统推算的位置、速度、姿态信息会 随时间积累，长时间导航会导致导航信息的发散。全球导航卫星系统(Global Navigation Satellite System，GNSS)、INS具有良好的互补特性，是常用的组 合导航方式。根据组合方式，GNSS/INS组合导航可以分为松组合、紧组合以 及深组合。松组合利用GNSS提供的位置和速度信息作为观测量进行组合，因 此要求卫星的可见星数大于四颗。紧组合采用的观测量是卫星伪距和伪距率信 息，在观测星数小于四颗的情况下仍然具有一定的导航定位能力。GNSS/INS 组合导航系统在卫导信号良好时能够提供稳定可靠的位置、速度以及姿态信息， 然而在城市峡谷、森林以及水下等信号遮挡严重的场景无法提供稳定可靠地位 置、速度、姿态信息，除此之外GNSS信号容易受到电磁干扰等影响从而严重 影响导航信息精度。因此，现有一般采用航姿参考系统(Attitude and Heading 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：在载体静止状态下，根据静态场景下加速度计的输出确定载体的横 滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据 确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态 角四元数初始化卡尔曼滤波状态向量。
这里，对初始横滚角、俯仰角和航向角，状态量以及噪声矩阵进行初始化。 如果应用中可以通过外部辅助初始化则利用外部信息进行横滚角、俯仰角和航 向角的初始化。如果无外部辅助则利用加速度计和磁强计的输出初始化横滚角、 俯仰角和航向角。在该实施例中利用加速度计的输出初始化横滚角和俯仰角， 并且为了提高初始对准的精度，首先使载体处于静止状态，利用静止状态下的 输出计算载体的横滚和俯仰角，计算方法如下：
其中，φ代表载体的横滚角，θ代表载体的俯仰角，代表静态场景下加速度计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系的方向余弦矩 阵，计算方法如下：
其中，φ、θ、ψ依次代表载体的横滚、俯仰和姿态角，其值可以利用姿态四 元数计算得到，gn＝[0 0 g]T，g为当地重力值。
量测噪声矩阵的设定需要根据载体的动态进行自适应的判断，为了能够准 确的反应载体的运动状态，采用求取固定采样时段方差的方法设定量测方差， 具体的计算方法如下：
式中，N为固定的采样数量，为加速度计x、y、z三轴的输出σa 2反应 载体的动态大小，用来调整卡尔曼滤波的量测噪声矩阵：
Rk＝R0·σa 2
式中，R0表示基准的加速度计噪声矩阵。
上述步骤目的是构建卡尔曼滤波的量测方程，构建量测方程需要构建量测 系数矩阵、需要计算量测信息并且需要确定方程的量测噪声矩阵，他们之间是 独立的，目的都是为了建立量测方程。
S104：根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输 出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角 计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的 系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态 向量。
具体地，利用磁强计或者外部信息对航向进行滤波。
首先计算磁强计推算的航向信息，方法如下：
其中，
θ、φ分别为计算得到的横滚角和俯仰角，ψ代表利用磁强计计算得到的航 向信息；γm为初始对准当地的磁偏角，其值可以结合纬度信息查阅资料得到； 分别为经过补偿后的磁强计输出的x、y、z轴的均值， 依次代表经过倾斜补偿后磁强计x、y、z轴的输出。mw为磁强计输出经过 倾斜补偿后的输出向量。
当有外部信息时接收并存储外部信息，
ψm＝ψExt
利用外部信息进行修正，其中，ψExt表示外部航向信息。
构建系数矩阵
其中x1、x2、x3、x4为状态量中估计得到的四元数，Hψ代表航向信息量测 更新的系数矩阵，表示构建H矩阵的中间变量。
计算增益矩阵：
其中Kψ代表航向信息量测更新的增益矩阵，σψ为利用磁强计信息计算得到 的航向信息的方差信息或者外部航向信息的方差信息，P表示卡尔曼滤波的方 差矩阵。
状态更新：
xi＝xi+Ki(ψm-ψ)
其中，i＝1,2,3...7依次代表卡尔曼滤波状态量的元素，Ki表示航向信息量测 更新的增益矩阵，ψ表示利用惯导数据预测的载体的航向角。
S105：若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止 状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零 偏信息进行反馈补偿。
这里，首先根据步骤S103中计算得到的加速度计输出的方差值和加速度的 模值来判断载体的运动状态，当σa 2小于一定阈值后则认为载体处于静止状态， 此时滤波器能够较好的估计出零偏误差，因此对陀螺零偏进行更新，并且将状 态量置零：
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轴的输出，mw表示磁强计输出经过倾斜补 偿后的输出向量；
根据表达式确定载体的姿态角四元数向量其中，其中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系的 方向余弦矩阵， φ、θ、ψ表 示载体的横滚角、俯仰角和航向角，gn＝[0 0 g]T；
根据表达式Rk＝R0·σa 2调整卡尔曼滤波的量测噪声矩阵Rk，其中，σa 2表示 加速度计输出的方差，N表示固定的采 样数量，nk表示加速度计x、y、z三轴的输出σa 2反应载体的动态大小，R0表 示基准的加速度计噪声矩阵。
此外，在一个具体示例中，所述根据加速度计的输出确定载体的横滚角和 俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的 横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构 建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向 角，更新卡尔曼滤波状态向量包括：
根据表达式确定载体的航向角ψm，其中，θ、φ表示确定的横滚角和俯 仰角，表示磁强计经过校正后的x、y、z轴的均值，γm表示初 始对准当地的磁偏角，表示经过倾斜补偿后磁强计x、y、z轴的 输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
当有外部航向信息时，接收并存储外部航向信息，根据表达式ψ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轴的输出，mw表示磁强计输出经过倾斜补 偿后的输出向量；
根据表达式确定载体的姿态角四元数向量其中，其中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系的 方向余弦矩阵， φ、θ、ψ表 示载体的横滚角、俯仰角和航向角，gn＝[0 0 g]T；
根据表达式Rk＝R0·σa 2调整卡尔曼滤波的量测噪声矩阵Rk，其中，σa 2表示 加速度计输出的方差，N表示固定的采 样数量，nk表示加速度计x、y、z三轴的输出σa 2反应载体的动态大小，R0表 示基准的加速度计噪声矩阵。
进一步的，所述滤波修正单元204还用于：
根据表达式确定载体的航向角ψm，其中，θ、φ表示确定的横滚角和俯 仰角，表示磁强计经过校正后的x、y、z轴的均值，γm表示初 始对准当地的磁偏角，表示经过倾斜补偿后磁强计x、y、z轴的 输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
当有外部航向信息时，接收并存储外部航向信息，根据表达式ψ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轴的输出，mw表示磁强计输出经过倾斜补 偿后的输出向量；
根据表达式确定载体的姿态角四元数向量其中，其中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系的 方向余弦矩阵， φ、θ、ψ表 示载体的横滚角、俯仰角和航向角，gn＝[0 0 g]T；
根据表达式Rk＝R0·σa 2调整卡尔曼滤波的量测噪声矩阵Rk，其中，σa 2表示 加速度计输出的方差，N表示固定的采 样数量，nk表示加速度计x、y、z三轴的输出σa 2反应载体的动态大小，R0表 示基准的加速度计噪声矩阵。
进一步的，所述根据加速度计的输出确定载体的横滚角和俯仰角，根据磁 强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角 和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根 据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼 滤波状态向量包括：
根据表达式确定载体的航向角ψm，其中，θ、φ表示确定的横滚角和俯 仰角，表示磁强计经过校正后的x、y、z轴的均值，γm表示初 始对准当地的磁偏角，表示经过倾斜补偿后磁强计x、y、z轴的 输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
当有外部航向信息时，接收并存储外部航向信息，根据表达式ψ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可以是中央处理单元(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)、随机 存取存储器(Random Access Memory，RAM)、电载波信号、电信信号以及软 件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法 管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根 据立法和专利实践，计算机可读介质不包括是电载波信号和电信信号。
以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照 前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其 依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特 征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发 明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。

Claims (10)

1.一种姿态测量方法，其特征在于，包括：
在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量；
利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测；
根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵；
根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量；
若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿。
2.如权利要求1所述的姿态测量方法，其特征在于，所述在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量包括：
根据表达式确定载体的横滚角φ，根据表达式确定载体的俯仰角θ，其中，表示静态场景下加速度计x、y、z轴输出的平均值；
根据表达式确定载体的航向角ψ，其中， 表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，表示经过倾斜补偿后磁强计x、y、z轴的输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
根据表达式确定载体的姿态角四元数向量其中，其中q0、q1、q2、q3表示姿态角四元数；
根据表达式初始化卡尔曼滤波状态向量X0。
3.如权利要求1所述的姿态测量方法，其特征在于，所述利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测包括：
根据表达式更新姿态角四元数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历元的量测噪声矩阵。
4.如权利要求1所述的姿态测量方法，其特征在于，所述根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵包括：
根据表达式构建卡尔曼滤波的量测系数矩阵Hk，其中，q0、q1、q2、q3表示姿态角四元数，g表示当地的重力大小；
根据表达式计算卡尔曼滤波的量测信息zk，为从n系到b系的方向余弦矩阵，φ、θ、ψ表示载体的横滚角、俯仰角和航向角，gn＝[0 0 g]T；
根据表达式Rk＝R0·σa 2调整卡尔曼滤波的量测噪声矩阵Rk，其中，σa 2表示加速度计输出的方差，N表示固定的采样数量，nk表示加速度计x、y、z三轴的输出σa 2反应载体的动态大小，R0表示基准的加速度计噪声矩阵。
5.如权利要求1所述的姿态测量方法，其特征在于，所述根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量包括：
根据表达式确定载体的航向角ψm，其中，θ、φ表示确定的横滚角和俯仰角，表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，表示经过倾斜补偿后磁强计x、y、z轴的输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
当有外部航向信息时，接收并存储外部航向信息，根据表达式ψm＝ψExt进行修正，其中，ψExt表示外部航向信息；
根据表达式和构建系数矩阵，其中，x1、x2、x3、x4表示状态向量中得到的四元数，Hψ表示航向信息量测更新的系数矩阵；
根据表达式计算增益矩阵，其中，Kψ表示航向信息量测更新的增益矩阵，σψ为利用磁强计信息计算得到的航向信息的方差信息或者外部航向信息的方差信息，P表示卡尔曼滤波的方差矩阵；
根据表达式xi＝xi+Ki(ψm-ψ)更新卡尔曼滤波状态向量，其中，i＝1,2,3...7表示卡尔曼滤波状态向量的元素，Ki表示航向信息量测更新的增益矩阵，ψ表示利用惯导数据预测的载体的航向角。
6.如权利要求1所述的姿态测量方法，其特征在于，所述根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿包括：
根据表达式对陀螺零偏进行更新，其中，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轴的输出。
7.一种姿态测量装置，其特征在于，包括：
姿态初始化单元，用于在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量；
姿态更新单元，用于利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测；
量测方程构建单元，用于根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵；
滤波修正单元，用于根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量；
反馈补偿单元，用于若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿。
8.如权利要求7所述的姿态测量装置，其特征在于，所述姿态初始化单元还用于：
根据表达式确定载体的横滚角φ，根据表达式确定载体的俯仰角θ，其中，表示静态场景下加速度计x、y、z轴输出的平均值；
根据表达式确定载体的航向角ψ，其中， 表示磁强计经过校正后的x、y、z轴的均值，γm表示初始对准当地的磁偏角，表示经过倾斜补偿后磁强计x、y、z轴的输出，mw表示磁强计输出经过倾斜补偿后的输出向量；
根据表达式确定载体的姿态角四元数向量其中，其中q0、q1、q2、q3表示姿态角四元数；
根据表达式初始化卡尔曼滤波状态向量X0。
9.一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至6任一项所述方法的步骤。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至6任一项所述方法的步骤。
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 true CN109163721A (zh) 2019-01-08
CN109163721B 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)

Cited By (29)

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

Citations (6)

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

Patent Citations (6)

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

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
李枭楠 等: "基于载波相位测量的多天线GNSS/INS组合测姿方法", 《第八届中国卫星导航学术年会论文集——S10多源融合导航技术》 *

Cited By (48)

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

Also Published As

Publication number Publication date
CN109163721B (zh) 2020-06-09

Similar Documents

Publication Publication Date Title
CN109163721A (zh) 姿态测量方法及终端设备
US8756995B2 (en) Device and method for combining samples from an inertial measurement sensor cluster
CN109001787A (zh) 一种姿态角解算与定位的方法及其融合传感器
CN105716610B (zh) 一种地磁场模型辅助的载体姿态和航向计算方法和系统
US10337884B2 (en) Method and apparatus for fast magnetometer calibration
CN106500693B (zh) 一种基于自适应扩展卡尔曼滤波的ahrs算法
CN109855617A (zh) 一种车辆定位方法、车辆定位装置及终端设备
CN109916395A (zh) 一种姿态自主冗余组合导航算法
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
CN105606096B (zh) 一种载体运动状态信息辅助的姿态和航向计算方法和系统
CN106990426A (zh) 一种导航方法和导航装置
CN107390247A (zh) 一种导航方法、系统及导航终端
CN108398128A (zh) 一种姿态角的融合解算方法和装置
CN106662443A (zh) 用于垂直轨迹确定的方法和系统
CN103512584A (zh) 导航姿态信息输出方法、装置及捷联航姿参考系统
CN110440830A (zh) 动基座下车载捷联惯导系统自对准方法
CN103557864A (zh) Mems捷联惯导自适应sckf滤波的初始对准方法
CN108627152A (zh) 一种微型无人机基于多传感器数据融合的导航方法
CN108645404A (zh) 一种小型多旋翼无人机姿态解算方法
Tomaszewski et al. Concept of AHRS algorithm designed for platform independent IMU attitude alignment
CN108871319A (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN111141285B (zh) 一种航空重力测量装置
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN108571980A (zh) 一种捷联惯性导航的误差校正方法及装置
CN115856922A (zh) 一种松耦合陆地组合导航方法、装置、计算机设备和介质

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