CN112859138A - 一种姿态测量方法、装置及电子设备 - Google Patents

一种姿态测量方法、装置及电子设备 Download PDF

Info

Publication number
CN112859138A
CN112859138A CN201911189225.5A CN201911189225A CN112859138A CN 112859138 A CN112859138 A CN 112859138A CN 201911189225 A CN201911189225 A CN 201911189225A CN 112859138 A CN112859138 A CN 112859138A
Authority
CN
China
Prior art keywords
time
real
value
carrier
fixed solution
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
CN201911189225.5A
Other languages
English (en)
Other versions
CN112859138B (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.)
China Mobile Communications Group Co Ltd
China Mobile IoT Co Ltd
Original Assignee
China Mobile Communications Group Co Ltd
China Mobile IoT 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 China Mobile Communications Group Co Ltd, China Mobile IoT Co Ltd filed Critical China Mobile Communications Group Co Ltd
Priority to CN201911189225.5A priority Critical patent/CN112859138B/zh
Publication of CN112859138A publication Critical patent/CN112859138A/zh
Application granted granted Critical
Publication of CN112859138B publication Critical patent/CN112859138B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude
    • G01S19/54—Determining attitude using carrier phase measurements; using long or short baseline interferometry
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/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/18—Stabilised platforms, e.g. by gyroscope
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

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

Abstract

本发明实施例提供一种姿态测量方法、装置及电子设备，根据RTK解算的实时固定解以及载体的加速度计的测量值确定航向角初值、俯仰角初值和横滚角初值，并为四元数赋初值；再根据载体的陀螺仪的实时测量值，对四元数初值进行递推得到实时四元数；最后根据所述实时四元数进行矩阵变换，确定载体的实时航向角，所述实时姿态信息中包括所述载体的实时航向角。本发明实施例通过精度较高的RTK解算的固定解和加速度计的测量值确定四元数初值，再进行四元数推算确定载体的实时航向角，既避免了RTK解算时间较长且不能保证成功率的问题，也避免了欧拉角表述姿态信息存在的万向锁问题，提高了姿态测量的准确度。

Description

一种姿态测量方法、装置及电子设备
技术领域
本发明涉及通信技术领域，尤其涉及一种姿态测量方法、装置及电子设备。
背景技术
现有技术中的姿态测量方案分为两种：一是基于惯性导航的AHRS(航姿参考系统，Attitude and heading reference system)中的三轴陀螺仪，加速度计和磁强计，为载体提供航向，横滚和侧翻信息。二是基于高精度卫星导航定位的姿态测量系统，利用三台高精度卫星导航接收机构成2条基线从而计算载体的横滚角、俯仰角和航向角，来得到载体姿态信息。
但是，第一种方案中，确定航向角的磁场计容易受环境影响，而导致姿态信息精度较低甚至无效；第二种方案中，RTK(载波相位差分技术，Real-time kinematic)易受天线接收质量的影响，而导致姿态测量准确度较低。
可见，现有技术的姿态测量存在准确度较低的问题。
发明内容
本发明实施例提供一种姿态测量方法、装置及电子设备，以解决现有技术的姿态测量准确度较低的问题。
第一方面，本发明实施例提供一种姿态测量方法，所述方法包括以下步骤：
获取载波相位差分技术RTK解算的第一固定解，并同时获取载体的加速度计的当前测量值，其中，所述第一固定解为初次获取的固定解；
根据所述第一固定解，确定航向角初值，并将所述第一固定解记录为基准固定解，同时，根据所述当载体的加速度计的当前测量值，确定俯仰角初值和横滚角初值；
根据所述航向角初值、所述俯仰角初值和所述横滚角初值，确定四元数初值；
根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数；
根据所述实时四元数进行矩阵变换，确定所述载体的实时航向角。
第二方面，本发明实施例还提供一种姿态测量装置，包括：
第一获取模块，用于获取载波相位差分技术RTK解算的第一固定解，并同时获取载体的加速度计的当前测量值，其中，所述第一固定解为初次获取的固定解；
第一确定模块，用于根据所述第一固定解，确定航向角初值，并将所述第一固定解记录为基准固定解，同时，根据所述当载体的加速度计的当前测量值，确定俯仰角初值和横滚角初值；
第二确定模块，用于根据所述航向角初值、所述俯仰角初值和所述横滚角初值，确定四元数初值；
递推模块，用于根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数；
姿态确定模块，用于根据所述实时四元数进行矩阵变换，确定所述载体的实时航向角。
第三方面，本发明实施例还提供一种电子设备，包括处理器、存储器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如本发明实施例提供的姿态测量方法的步骤。
第四方面，本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如本发明实施例提供的姿态测量方法的步骤。
本发明实施例提供的姿态测量方法、装置以及电子设备，根据RTK解算的实时固定解以及载体的加速度计的测量值确定航向角初值、俯仰角初值和横滚角初值，并为四元数赋初值；再根据载体的陀螺仪的实时测量值，对四元数初值进行递推得到实时四元数；最后根据所述实时四元数进行矩阵变换，确定载体的实时航向角，所述实时姿态信息中包括所述载体的实时航向角。通过精度较高的RTK解算的固定解和加速度计的测量值确定四元数初值，再进行四元数推算确定载体的实时航向角，既避免了RTK解算时间较长且不能保证成功率的问题，也避免了欧拉角表述姿态信息存在的万向锁问题，提高了姿态信息测量的准确度。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种姿态测量方法的流程图；
图2是本发明实施例提供的另一种姿态测量方法的流程图；
图3是本发明实施例提供的一种确定和更新航向角初值的方法的流程图；
图4是本发明实施例提供的又一种姿态测量方法的流程图；
图5是本发明实施例提供的一种姿态测量装置的结构图之一；
图6是本发明实施例提供的一种姿态测量装置的结构图之二；
图7是本发明实施例提供的一种姿态测量装置的结构图之三；
图8是本发明实施例提供的一种姿态测量装置的结构图之四；
图9是本发明实施例提供的一种姿态测量装置的结构图之五；
图10是本发明实施例提供的一种姿态测量装置的结构图之六；
图11是本发明实施例提供的一种姿态测量装置的结构图之七；
图12是本发明实施例提供的一种电子设备的结构图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本申请的说明书和权利要求书中的术语“包括”以及它的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。此外，说明书以及权利要求中使用“和/或”表示所连接对象的至少其中之一，例如A和/或B，表示包含单独A，单独B，以及A和B都存在三种情况。
在本发明实施例中，“示例性的”或者“例如”等词用于表示作例子、例证或说明。本发明实施例中被描述为“示例性的”或者“例如”的任何实施例或设计方案不应被解释为比其它实施例或设计方案更优选或更具优势。确切而言，使用“示例性的”或者“例如”等词旨在以具体方式呈现相关概念。
请参见图1，图1是本发明实施例提供的一种姿态测量方法，该方法可以应用于载体姿态测量装置，其中，所述载体姿态测量装置可以是云端的计算机、服务器等具备数据处理功能的设备或者数据平台。
如图1所示，所述方法包括以下步骤：
步骤101、获取载波相位差分技术RTK解算的第一固定解，并同时获取载体的加速度计的当前测量值，其中，所述第一固定解为初次获取的固定解。
载体的姿态信息一般包括航向角、俯仰角和横滚角这三个欧拉角，表示载体坐标系与地面坐标系之间的关系，反映载体相对地面的姿态。其中，航向角用
Figure BDA0002293149020000042
表示，其也可以表述为偏航角，俯仰角用θ表示，所述横滚角用
Figure BDA0002293149020000041
表示，其也可以表述为翻滚角、滚转角、翻转角或者侧翻角，在此不作限定。
对于航向角来说，可以根据RTK实时解算得到的固定解求取，RTK可以基于高精度卫星导航系统中基准站和移动站的载波相位观测值，进行求差解算以获得测量点的三维定位结果。其中，RTK解算得到的固定解为：在RTK解算过程中，将基线模糊度确定为整数时，反代方程解算得到的未知参数解，根据固定解获取的定位结果可以达到厘米级精度，根据固定解确定的航向角的精度会大幅提高。对于俯仰角和横滚角来说，根据加速度计的实时测量值获取的载体的俯仰角和横滚角，精度较高。
现有技术中，RTK解算过程中确定整数模糊度的时间较长，且无法实现100％的成功率，短时间内RTK解算得到的解不能保证航向角的精度。而利用载体的陀螺仪的测量值确定载体的航向角较为准确。另外，欧拉角是按照转动顺序定义航向角、俯仰角和横滚角用动态欧拉角的，存在万向锁的问题，而用四元数实现载体旋转时，则不会存在万向锁的问题。
本发明实施例中，根据RTK解算的固定解和载体的加速度计的实时测量值确定四元数初值，再利用陀螺仪的实时测量值实现四元数推算，利用推算得到的四元数求解载体的姿态信息。
具体的，本步骤中，获取RTK解算的第一固定解，并同时获取载体的加速度计的当前测量值，其中，所述第一固定解为初次获取的固定解。需要说明的是，所述同时获取载体的加速度计的当前测量值，并不限定于加速度计获取当前测量值的时刻与RTK解算得到固定解的时刻完全一致，加速度计的测量间隔时间较小，可以保证姿态信息获取的实时性，即使加速度计获取当前测量值的时刻与RTK解算得到固定解的时刻不完全一致，一定范围内的时间差导致的误差可以忽略不计。
步骤102、根据所述第一固定解，确定航向角初值，并将所述第一固定解记录为基准固定解，同时，根据所述当载体的加速度计的当前测量值，确定俯仰角初值和横滚角初值。
本发明实施例中，在进行四元数推算之前，需要为四元数赋初值，即分别为航向角、俯仰角和横滚角赋初值。为了保证初值的准确性，航向角初值可以基于高精度卫星导航系统的实时测量值，通过RTK解算的固定解确定，俯仰角初值和横滚角初值可以直接根据载体的加速度计的实时测量值确定。同时，将所述第一固定解记录为基准固定解，以此为依据，可以判断后续根据固定解确定的航向角是否确实发生变更。
步骤103、根据所述航向角初值、所述俯仰角初值和所述横滚角初值，计算四元数初值。
本步骤中，根据步骤102中确定的航向角初值、俯仰角初值和横滚角初值，可以基于欧拉角与四元数之间的转换关系，为四元数赋初值。具体的，载体在三维空间的旋转可以由一旋转矩阵表示，既可以通过欧拉角求解载体的旋转矩阵，也可以通过四元数求解载体的旋转矩阵，即欧拉角和四元数之间存在基于载体旋转矩阵的矩阵变换关系，根据该矩阵变换关系，代入航向角初值、俯仰角初值和横滚角初值，即可确定四元数初值。为了布局清楚，具体赋初值的过程，将在后续实施例进行具体说明，为避免重复，在此不再赘述。
步骤104、根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数；
本发明实施例中，载体的陀螺仪可以测量载体偏转或者倾斜时的转动角速度，基于陀螺仪的实时测量值可以确定四元数姿态矩阵的微分方程，求解该微分方程即可获得四元数的解，其反映四元数的递推关系，根据该递推关系可以对步骤103中确定的四元数初值进行递推，得到每一时刻的实时四元数。为了布局清楚，具体递推的过程，将在后续实施例进行具体说明，为避免重复，在此不再赘述。
步骤105、根据所述实时四元数进行矩阵变换，确定所述载体的实时航向角。
本步骤中，根据步骤104中递推得到的实时四元数和步骤103中确定的四元数与欧拉角之间的矩阵变换关系，即可根据实时四元数确定载体的实时航向角。需要说明的是，现有技术中，根据加速度计的测量值确定载体的实时俯仰角和载体的实时横滚角的准确度较高，本发明实施例中，主要确定准确度较高的实时航向角。
本发明实施例中，根据RTK解算的实时固定解以及载体的加速度计的测量值确定航向角初值、俯仰角初值和横滚角初值，并为四元数赋初值；再根据载体的陀螺仪的实时测量值，对四元数初值进行递推得到实时四元数；最后根据所述实时四元数进行矩阵变换，确定载体的实时航向角，所述实时姿态信息中包括所述载体的实时航向角。通过精度较高的RTK解算的固定解和加速度计的测量值确定四元数初值，再进行四元数推算确定载体的实时航向角，既避免了RTK解算时间较长且不能保证成功率的问题，也避免了欧拉角表述姿态信息存在的万向锁问题，提高了姿态信息测量的准确度。
请参见图2，图2是本发明实施例提供的一种姿态测量方法，该方法可以应用于载体姿态测量装置，其中，所述载体姿态测量装置可以是云端的计算机、服务器等具备数据处理功能的设备或者数据平台。
如图2所示，所述方法包括以下步骤：
步骤201、获取载波相位差分技术RTK解算的第一固定解，并同时获取载体的加速度计的当前测量值，其中，所述第一固定解为初次获取的固定解；
可选的，步骤201包括：
在第一预设时间内，获取RTK解算的连续固定解；
判断所述连续固定解是否满足第一预设稳定条件；
若所述连续固定解满足第一预设稳定条件，将所述连续固定解的平均值确定为第一固定解；
其中，所述第一预设时间优选为10s，在此不作限定。所述连续固定解可以理解为：在所述第一预设时间内，RTK解算的连续解均为固定解，不存在浮点解或者其他不确定的参数解。
RTK解算的固定解可以确定航向角初值，若载体当前处于剧烈运动或者其他不稳定情况时，RTK解算的固定解同样不稳定，如果载体姿态测量装置仅获取一个固定解就确定航向角初值，后续进行四元数推算的准确度将会受影响。
本实施例中，如图3所示，初次获取RTK解算的固定解时，载体姿态测量装置可以获取第一预设时间内的连续固定解，若所述连续固定解满足第一预设稳定条件，表示连续固定解较为平滑，载体处于稳定状态，可以将所述连续固定解的平均值确定为第一固定解，根据所述第一固定解确定的航向角能更加准确的反映载体的姿态信息。
进一步的，所述第一预设稳定条件，包括：
在第一预设时间内，RTK解算的基线方差满足第一预设条件，以及根据所述连续固定解确定的连续航向角的方差满足第二预设条件。
本实施例中，载体姿态测量装置可以计算在第一预设时间内，RTK解算的基线方差以及所述连续固定解确定的航向角的方差，通过上述基线方差和航向角方差可以判断连续固定解的稳定性。
其中，第一预设条件可以为RTK解算的基线方差小于或者等于第一预设阈值，第一预设阈值优选为0.0004m2，在此不作限定。第二预设条件可以为连续固定解确定的航向角方差小于或者等于第二预设阈值，第二预设阈值优选为0.04度2，在此不作限定。
步骤202、根据所述第一固定解，确定航向角初值，并将所述第一固定解记录为基准固定解，同时，根据所述当载体的加速度计的当前测量值，确定俯仰角初值和横滚角初值。
本步骤中，航向角初值
Figure BDA0002293149020000081
可以直接由RTK解算的固定解确定，所述根据所述当载体的加速度计的当前测量值，确定俯仰角初值和横滚角初值，可以理解为：将加速度计当前输出的重力加速度分量记为：[ax ay az]T，可以直接确定俯仰角的初值θ0：
Figure BDA0002293149020000082
以及，横滚角初值
Figure BDA0002293149020000083
Figure BDA0002293149020000084
本实施例中，在进行四元数推算之前，需要为四元数赋初值，即分别为航向角、俯仰角和横滚角赋初值。为了保证初值的准确性，航向角初值可以基于高精度卫星导航系统的实时测量值，通过RTK解算的固定解确定，俯仰角初值和横滚角初值可以直接根据载体的加速度计的实时测量值确定。同时，将所述第一固定解记录为基准固定解，以此为依据，可以判断后续根据固定解确定的航向角是否确实发生变更。
步骤203、根据所述航向角初值、所述俯仰角初值和所述横滚角初值，确定四元数初值。
载体在三维空间的旋转可以由一旋转矩阵表示，其中，利用欧拉角求解载体的旋转变换得到矩阵CE，CE可以表示为：
Figure BDA0002293149020000085
其中，
Figure BDA0002293149020000086
表示航向角，θ表示俯仰角，
Figure BDA0002293149020000087
表示横滚角。
四元数可以表示为：Q＝q0+q1i+q2j+q3k＝((q1,q2,q3),q0)。其中，q0为四元数的实部，q1、q2、q3为四元数的虚部，i、j、k为四元数的虚数单位，则利用四元数求解载体的旋转变换得到矩阵CQ，CQ可以表示为：
Figure BDA0002293149020000091
根据上述两式，可以确定四元数与欧拉角之间的转换关系，为：
Figure BDA0002293149020000092
Figure BDA0002293149020000093
Figure BDA0002293149020000094
Figure BDA0002293149020000095
本步骤中，根据步骤202中确定的航向角初值
Figure BDA0002293149020000096
俯仰角初值θ0和横滚角初值
Figure BDA0002293149020000097
直接代入上述欧拉角与四元数之间的转换关系，即可确定四元数初值。
步骤204、根据所述载体的陀螺仪的实时测量值，确定实时四元数递推关系。
其中，载体的陀螺仪输出的测量值为ωx、ωy、ωz，分别表示载体发生偏转或者倾斜时角速度的分量，基于陀螺仪输出的测量值可以确定四元数姿态矩阵的微分方程，所述四元数姿态矩阵微分方程为：
Figure BDA0002293149020000098
求解上述微分方程即可获得四元数的解：
Figure BDA0002293149020000101
上式即为四元数的递推关系，其中，t表示陀螺仪获取实时测量值的时刻，Δt表示陀螺仪的测量间隔，Δt优选为1s，在此不作限定。若载体的姿态信息发生变化，陀螺仪的实时测量值也会随之变化，上述四元数的递推关系会随之变化，进而递推得到的实时四元数也会变化，实时四元数可以准确的反映载体当前的姿态信息。
步骤205、根据所述实时四元数递推关系，对所述四元数初值进行递推得到实时四元数。
本步骤中，通过步骤204中确定的四元数的递推关系可以实现对四元数初值的递推，具体的，根据步骤203中确定四元数初值时，获取第一时刻载体的陀螺仪的实时测量值，将第一时刻的陀螺仪的实时测量值和四元数初值代入上述四元数的解的表达式，实现第一次递推，可以确定第二时刻的实时四元数，以此类推，根据t时刻的陀螺仪的实时测量值和实时四元数，可以递推得到t+Δt时刻的实时四元数，根据该实时四元数即可求解t+Δt时刻的载体的姿态信息，进而确定t+Δt时刻载体的实时航向角。
步骤206、根据所述实时四元数进行矩阵变换，确定所述载体的实时姿态信息，所述实时姿态信息包括所述载体的实时航向角。
根据步骤203中确定的欧拉角求解载体旋转变换得到的矩阵CE和四元数求解载体的旋转变换得到的矩阵CQ，可以推导出利用四元数反求欧拉角的表达式：
Figure BDA0002293149020000102
将步骤204中递推得到的t+Δt时刻的实时四元数代入上式，可以确定t+Δt时刻载体的实时姿态信息，其中
Figure BDA0002293149020000103
为载体的实时航向角，根据四元数递推预测载体的姿态信息，既避免了RTK解算时间长且非100％成功率的问题，还避免了万向锁的问题，提高了确定载体姿态信息的准确度
可选的，在步骤201之后，所述方法还包括：
获取RTK解算的第二固定解，并同时获取所述载体的加速度计的当前测量值，其中，所述第二固定解为获取第一固定解之后，继续获取的实时固定解；
根据所述第二固定解，更新所述航向角初值，并将所述第二固定解确定为基准固定解，同时，根据所述载体的加速度计的实时测量值，更新所述俯仰角初值和所述横滚角初值；
根据更新后的航向角初值、更新后的俯仰角初值和更新后的横滚角初值，确定更新后的四元数初值；
所述根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数，包括：
根据所述载体的陀螺仪的实时测量值，对所述更新后的四元数初值进行递推得到实时四元数。
本实施例中，在获取RTK解算的第一固定解之后，卫星导航系统继续获取实时测量值，并通过RTK继续解算固定解。但RTK解算出固定解需要一定时间，在等待RTK解算出固定解的时间里，利用陀螺仪的实时测量值对四元数进行递推，以确定载体的实时航向角，短时间内弥补RTK解算固定解的空缺，但是陀螺仪作为惯性器件存在漂移特性，随着时间的增长，累积的漂移误差达到一定值时，基于其实时测量值递推得到的四元数的准确度将会降低。
基于此，本实施例中，载体姿态测量装置可以利用RTK结算的固定解对航向角初值实时监测，在初次获取第一固定解之后，继续获取RTK解算得到的实时固定解，当再次获取到第二固定解时，以第二固定解更新航向角初值，同时根据载体的加速度计的实时测量值更新俯仰角初值和横滚角初值。在短时间内由陀螺仪的实时测量值确定载体的实时航向角，当RTK再次解算得到固定解时，更新航向角初值，重新为四元数赋初值，重新进行四元数递推，修正因陀螺仪的漂移特性累积的漂移误差，进一步确保递推得到的四元数的准确度，进而进一步保证确定的实时航向角的准确度。
进一步的，所述根据所述第二固定解，更新所述航向角初值，并将所述第二固定解确定为基准固定解，包括：
判断根据所述第二固定解确定的航向角与根据所述基准固定解确定的航向角初值的差值是否满足第三预设条件；
若根据所述第二固定解确定的航向角与根据所述基准固定解确定的航向角初值的差值满足第三预设条件，判断所述载体的实时航向角是否满足第二预设稳定条件，其中，所述实时航向角为根据所述实时四元数进行矩阵变换确定的实时航向角；
若所述载体的实时航向角满足第二预设稳定条件，根据所述第二固定解，更新所述航向角初值，并将所述第二固定解确定为基准固定解。
本实施例中，为了进一步保证更新的初值的准确度，以及减少不必要的计算，在所述第二固定解满足一定条件时，才根据第二固定解更新航向角初值。
具体的，如图3所示，判断根据所述第二固定解确定的航向角与根据所述基准固定解确定的航向角初值的差值是否满足第三预设条件，可以理解为：判断第二固定解与上一次确定的航向角初值之间是否存在显著差异。由于第一固定解为基准固定解，且根据第一固定解确定了航向角初值，第二固定解为RTK解算得到第一固定解之后继续解算得到的实时固定解，则判断当前固定解确定的航向角与上一次确定的航向角初值之间是否存在显著差异，若所述差值满足第三预设条件，则可以认为当前固定解确定的航向角与上一次确定的航向角初值之间存在显著差异，表示载体的航向角的确发生了变化，此时可以考虑更新航向角初值，在更新航向角初值的同时，更新俯仰角初值和横滚角初值，并重新为四元数赋初值。
其中第三预设条件可以为根据所述第二固定解确定的航向角与根据所述第一固定解确定的航向角初值的差值的绝对值大于第三预设阈值，第三预设阈值优选为5°，在此不作限定。
进一步的，所述第二预设稳定条件包括以下至少一项：
在第二预设时间内，所述载体的陀螺仪的实时测量值满足第四预设条件；
在第二预设时间内，RTK解算的基线方差满足第五预设条件，以及所述载体的实时航向角的方差满足第六预设条件；
在第二预设时间内，卫星数量满足第七预设条件，以及信号强度满足第八预设条件。
其中，第二预设时间优选为100s，在此不作限定。
如图3所示，本实施例中，还考虑了当前载体的实时航向角的稳定性。在当前固定解确定的航向角与上一次确定的航向角初值之间存在显著差异时，可以考虑更新航向角初值，但是若当前载体处于不稳定状态，例如载体正处在转向的过程中或者正处在倾斜的过程中，当前的实时航向角是暂时的，后续姿态变化未知，而当根据所述实时四元数进行矩阵变换确定的实时航向角满足第二预设稳定条件时，可以认为载体稳定在某一姿态保持运行，根据此时获取的固定解更新航向角的初值，能够进一步保证递推的准确性，进一步保证确定的实时航向角的准确度。
本实施例中，所述在第二预设时间内，所述载体的陀螺仪的实时测量值满足第四预设条件，可以理解为：由于载体的陀螺仪的测量值为载体偏转或者倾斜时的角速度，因此可以根据陀螺仪的测量值来判断载体当前是否处于稳定状态，若载体稳定在一个姿态保持运行时，载体的陀螺仪的测量值将会为零或者接近于零，可以设定第四预设条件为：在第二预设时间内，陀螺仪的实时测量值均小于或者等于第四预设阈值内，第四预设阈值优选为0.2°/秒，在此不作限定。
所述在第二预设时间内，RTK解算的基线方差满足第五预设条件，以及所述载体的实时航向角的方差满足第六预设条件，可以理解为：计算第二预设时间内，RTK解算的基线方差，若基线方差满足第五预设条件，可以认为当前载体较为稳定，同时统计第二预设时间内的确定的载体的实时航向角，并计算其方差，若所述方差满足第六预设条件，则说明在第二预设时间内，载体的实时航向角较为平滑，可以认为载体当前处于稳定状态。
其中，第五预设条件可以为在第二预设条件内，RTK解算的基线方差小于或者等于第五预设阈值，第五预设阈值优选为0.0001m2，在此不作限定。第六预设条件可以为在第二预设时间内，所述载体的实时航向角的方差小于或者等于第六预设阈值，第六预设阈值优选为0.25度2，在此不作限定。
所述在第二预设时间内，卫星数量满足第七预设条件，以及信号强度满足第八预设条件，可以理解为：获取第二固定解时的卫星数量大于上一次确定航向角初值时的卫星数量或者双系统RTK下的卫星数量大于或者等于第七预设阈值，获取第二固定解时的信号强度大于上一次确定航向角初值时的信号强度或者在第二预设时间内，信号强度的均值大于或者等于第八预设阈值，其中，第七预设阈值优选为双系统RTK下的14颗，在此不作限定，第八预设阈值优选为44dBHz，在此不作限定。
本实施例中，若第二固定解不满足更新条件，可以不更新航向角初值，则继续根据陀螺仪的实时测量值对四元数继续进行递推，之后可以继续获取RTK解算的固定解，仍然以第一固定解确定的基准固定解为依据判断是否更新航向角初值。若第二固定解满足更新条件，更新初值后，将第二固定解记录为基准固定解，之后再次获取RTK解算的固定解时，将以第二固定解确定的基准固定解为依据，判断是否需要更新航向角初值，以此类推，在短时间内由陀螺仪的实时测量值确定载体的实时航向角，以再次获取的RTK解算的固定解修正因陀螺仪的漂移特性累积的漂移误差，进一步确保递推得到的四元数的准确度，进而进一步保证确定的实时航向角的准确度。图3表示了本实施例中具体确定以及更新航向角初值的实现过程。
可选的，所述方法还包括：
根据所述载体的加速度计的实时测量值，确定第一俯仰角和第一横滚角；
将所述第一俯仰角确定为所述载体的实时俯仰角，以及将所述第一横滚角确定为所述载体的实时横滚角。
进一步的，所述方法还包括：
根据所述实时四元数进行矩阵变换，确定第二俯仰角和第二横滚角；
若同一时刻的所述第一俯仰角与所述第二俯仰角的差值满足第九预设条件时，和/或，若同一时刻所述第一横滚角与所述第二横滚角的差值满足第十预设条件时，获取当前RTK解算的实时固定解，根据所述实时固定解，更新所述航向角初值，以及根据所述第一俯仰角与所述第一横滚角分别更新所述俯仰角初值和所述横滚角初值；
根据更新后的航向角初值、更新后的俯仰角初值和更新后的横滚角初值，确定更新后的四元数初值；
所述根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数，包括：
根据所述载体的陀螺仪的实时测量值，对所述更新后的四元数初值进行递推得到实时四元数。
本实施例中，载体的加速度计可以实时获取测量值，根据该实时测量值可以计算第一俯仰角和第二横滚角，由于载体的加速度计的测量值较为准确，因此，可以将第一俯仰角确定为载体的实时俯仰角，以及将第一横滚角确定为载体的实时横滚角，具体计算方式可以参考本实施例步骤202，为避免重复，在此不再赘述。
同时，根据本实施例步骤202至步骤204中递推得到的实时四元数，可以确定第二俯仰角和第二横滚角，通过比较同一时刻的第一俯仰角和第二俯仰角，以及比较同一时刻的第一横滚角和第二横滚角，可以判断当前四元数递推是否已经存在漂移误差，若同一时刻的所述第一俯仰角与所述第二俯仰角的差值满足第九预设条件时，和/或，若同一时刻所述第一横滚角与所述第二横滚角的差值满足第十预设条件时，获取当前RTK解算的实时固定解，根据所述实时固定解，更新所述航向角初值，以及根据所述第一俯仰角与所述第一横滚角分别更新所述俯仰角初值和所述横滚角初值，根据更新后的初值为四元数重新赋初值，重新进行四元数递推，进一步确保递推得到的四元数的准确度，进而进一步保证确定的实时航向角的准确度。
其中，所述第九预设条件可以为同一时刻的第一俯仰角与第二俯仰角的差值的绝对值小于或者等于第九预设阈值，第九预设阈值优选为1°，在此不作限定。第十预设条件可以为同一时刻的第一横滚角与第二横滚角的差值的绝对值小于或者等于第十预设阈值，第十预设阈值可以为1°，在此不作限定。
本实施例中，利用载体的加速度计的实时测量值确定的第二俯仰角和第二横滚角，修正因陀螺仪的漂移特性累积的漂移误差，进一步确保递推得到的四元数的准确度，进而进一步保证确定的实时航向角的准确度。
综上，可以参考图4进一步理解本实施例的完整实现步骤。对四元数进行递推之前，需要保证递推初值的准确度较高，因此，需要获取到RTK解算的固定解之后，才可以进行后续的递推步骤，并同时获取加速度计的当前测量值，再根据当前固定解确定航向角初值，以及根据加速度计的当前测量值确定俯仰角初值和横滚角初值，由航向角初值、俯仰角初值和横滚角初值确定四元数初值。
在四元数递推过程中，载体姿态测量装置继续获取固定解，并判断是否需要更新航向角初值，其中包括两种更新四元数初值的策略，其一是根据继续获取的固定解判断是否需要更新四元数初值，其二是根据加速度计的实时测量值确定的俯仰角和横滚角判断是否需要更新四元数初值。若需要更新四元数初值，则在更新四元数初值之后，根据陀螺仪的实时测量值，以更新后的四元数初值进行四元数递推，确定实时的航向角；若不需要更新四元数初值，则根据陀螺仪的实时测量值，以当前四元数初值继续进行四元数递推，确定实时的航向角。
本实施例在实施例1的基础上增加了多种可选的实施方式，且均可以进一步确保递推得到的四元数的准确度，进而进一步保证确定的实时航向角的准确度。
请参见图5，图5是本发明实施例提供的一种姿态测量装置的结构图。
如图5所示，所述姿态测量装置500包括：
第一获取模块501，用于获取载波相位差分技术RTK解算的第一固定解，并同时获取载体的加速度计的当前测量值，其中，所述第一固定解为初次获取的固定解；
第一初值确定模块502，用于根据所述第一固定解，确定航向角初值，并将所述第一固定解记录为基准固定解，同时，根据所述当载体的加速度计的当前测量值，确定俯仰角初值和横滚角初值；
第二初值确定模块503，用于根据所述航向角初值、所述俯仰角初值和所述横滚角初值，确定四元数初值；
递推模块504，用于根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数；
姿态确定模块505，用于根据所述实时四元数进行矩阵变换，确定所述载体的实时航向角。
可选的，如图6所示，所述递推模块504包括：
第一确定单元5041，用于根据所述载体的陀螺仪的实时测量值，确定实时四元数递推关系；
递推单元5042，用于根据所述实时四元数递推关系，对所述四元数初值进行递推得到实时四元数。
可选的，如图7所示，所述第一获取模块501包括：
获取单元5011，用于在第一预设时间内，获取RTK解算的连续固定解；
第一判断单元5012，用于判断所述连续固定解是否满足第一预设稳定条件；
第二确定单元5013，用于若所述连续固定解满足第一预设稳定条件，将所述连续固定解的平均值确定为第一固定解。
可选的，所述第一预设稳定条件，包括：
在第一预设时间内，RTK解算的基线方差满足第一预设条件，以及根据所述连续固定解确定的连续航向角的方差满足第二预设条件。
可选的，如图8所示，所述装置500还包括：
第二获取模块506，用于获取RTK解算的第二固定解，并同时获取所述载体的加速度计的当前测量值，其中，所述第二固定解为获取第一固定解之后，继续获取的实时固定解；
第一更新模块507，用于根据所述第二固定解，更新所述航向角初值，并将所述第二固定解确定为基准固定解，同时，根据所述载体的加速度计的实时测量值，更新所述俯仰角初值和所述横滚角初值；
第二更新模块508，用于根据更新后的航向角初值、更新后的俯仰角初值和更新后的横滚角初值，确定更新后的四元数初值；
所述递推模块504，还用于根据所述载体的陀螺仪的实时测量值，对所述更新后的四元数初值进行递推得到实时四元数。
可选的，如图9所示，所述第一更新模块507包括：
第二判断单元5071，用于判断根据所述第二固定解确定的航向角与根据所述基准固定解确定的航向角初值的差值是否满足第三预设条件；
第三判断单元5072，用于若根据所述第二固定解确定的航向角与根据所述基准固定解确定的航向角初值的差值满足第三预设条件，判断所述载体的实时航向角是否满足第二预设稳定条件，其中，所述实时航向角为根据所述实时四元数进行矩阵变换确定的实时航向角；
第一更新单元5073，用于若所述载体的实时航向角满足第二预设稳定条件，根据所述第二固定解，更新所述航向角初值，并将所述第二固定解确定为基准固定解。
可选的，所述第二预设稳定条件包括以下至少一项：
在第二预设时间内，所述载体的陀螺仪的实时测量值满足第四预设条件；
在第二预设时间内，RTK解算的基线方差满足第五预设条件，以及所述载体的实时航向角的方差满足第六预设条件；
在第二预设时间内，卫星数量满足第七预设条件，以及信号强度满足第八预设条件。
可选的，如图10所示，所述装置500还包括：
第一确定模块509，用于根据所述载体的加速度计的实时测量值，确定第一俯仰角和第一横滚角；
第二确定模块510，用于将所述第一俯仰角确定为所述载体的实时俯仰角，以及将所述第一横滚角确定为所述载体的实时横滚角。
可选的，如图11所示，所述装置500还包括：
第三确定模块511，用于根据所述实时四元数进行矩阵变换，确定第二俯仰角和第二横滚角；
第三更新模块512，用于若同一时刻的所述第一俯仰角与所述第二俯仰角的差值满足第九预设条件时，和/或，若同一时刻所述第一横滚角与所述第二横滚角的差值满足第十预设条件时，获取当前RTK解算的实时固定解，根据所述实时固定解，更新所述航向角初值，以及根据所述第一俯仰角与所述第一横滚角分别更新所述俯仰角初值和所述横滚角初值；
第四更新模块513，用于根据更新后的航向角初值、更新后的俯仰角初值和更新后的横滚角初值，计算更新后的四元数初值；
所述递推模块504，还用于根据所述载体的陀螺仪的实时测量值，对所述更新后的四元数初值进行递推得到实时四元数。
本发明实施例提供的装置能够实现图1和图2的方法实施例中载体姿态测量装置实现的各个过程，且可以达到相同有益效果，为避免重复，这里不再赘述。
请参见图12，图12是本发明实施例提供的一种电子设备的结构图，如图12所示，电子设备1200包括处理器1201、存储器1202及存储在所述存储器1202上并可在所述处理器上运行的计算机程序。
其中，所述计算机程序被所述处理器1201执行时实现如下步骤：
获取载波相位差分技术RTK解算的第一固定解，并同时获取载体的加速度计的当前测量值，其中，所述第一固定解为初次获取的固定解；
根据所述第一固定解，确定航向角初值，并将所述第一固定解记录为基准固定解，同时，根据所述载体的加速度计的当前测量值，确定俯仰角初值和横滚角初值；
根据所述航向角初值、所述俯仰角初值和所述横滚角初值，确定四元数初值；
根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数；
根据所述实时四元数进行矩阵变换，确定所述载体的实时航向角。
可选的，所述根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数，包括：
根据所述载体的陀螺仪的实时测量值，确定实时四元数递推关系；
根据所述实时四元数递推关系，对所述四元数初值进行递推得到实时四元数。
可选的，所述获取载波相位差分技术RTK解算的第一固定解，包括：
在第一预设时间内，获取RTK解算的连续固定解；
判断所述连续固定解是否满足第一预设稳定条件；
若所述连续固定解满足第一预设稳定条件，将所述连续固定解的平均值确定为第一固定解。
可选的，所述第一预设稳定条件，包括：
在第一预设时间内，RTK解算的基线方差满足第一预设条件，以及根据所述连续固定解确定的连续航向角的方差满足第二预设条件。
可选的，在所述获取载波相位差分技术RTK解算的第一固定解，并同时获取载体的加速度计的当前测量值之后，所述方法还包括：
获取RTK解算的第二固定解，并同时获取所述载体的加速度计的当前测量值，其中，所述第二固定解为获取第一固定解之后，继续获取的实时固定解；
根据所述第二固定解，更新所述航向角初值，并将所述第二固定解确定为基准固定解，同时，根据所述载体的加速度计的实时测量值，更新所述俯仰角初值和所述横滚角初值；
根据更新后的航向角初值、更新后的俯仰角初值和更新后的横滚角初值，确定更新后的四元数初值；
所述根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数，包括：
根据所述载体的陀螺仪的实时测量值，对所述更新后的四元数初值进行递推得到实时四元数。
可选的，所述根据所述第二固定解，更新所述航向角初值，并将所述第二固定解确定为基准固定解，包括：
判断根据所述第二固定解确定的航向角与根据所述基准固定解确定的航向角初值的差值是否满足第三预设条件；
若根据所述第二固定解确定的航向角与根据所述基准固定解确定的航向角初值的差值满足第三预设条件，判断所述载体的实时航向角是否满足第二预设稳定条件，其中，所述实时航向角为根据所述实时四元数进行矩阵变换确定的实时航向角；
若所述载体的实时航向角满足第二预设稳定条件，根据所述第二固定解，更新所述航向角初值，并将所述第二固定解确定为基准固定解。
可选的，所述第二预设稳定条件包括以下至少一项：
在第二预设时间内，所述载体的陀螺仪的实时测量值满足第四预设条件；
在第二预设时间内，RTK解算的基线方差满足第五预设条件，以及所述载体的实时航向角的方差满足第六预设条件；
在第二预设时间内，卫星数量满足第七预设条件，以及信号强度满足第八预设条件。
可选的，所述方法还包括：
根据所述载体的加速度计的实时测量值，确定第一俯仰角和第一横滚角；
将所述第一俯仰角确定为所述载体的实时俯仰角，以及将所述第一横滚角确定为所述载体的实时横滚角。
可选的，所述方法还包括：
根据所述实时四元数进行矩阵变换，确定第二俯仰角和第二横滚角；
若同一时刻的所述第一俯仰角与所述第二俯仰角的差值满足第九预设条件时，和/或，若同一时刻所述第一横滚角与所述第二横滚角的差值满足第十预设条件时，获取当前RTK解算的实时固定解，根据所述实时固定解，更新所述航向角初值，以及根据所述第一俯仰角与所述第一横滚角分别更新所述俯仰角初值和所述横滚角初值；
根据更新后的航向角初值、更新后的俯仰角初值和更新后的横滚角初值，确定更新后的四元数初值；
所述根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数，包括：
根据所述载体的陀螺仪的实时测量值，对所述更新后的四元数初值进行递推得到实时四元数。
本发明实施例提供的电子设备能够实现图1和图2的方法实施例中载体姿态测量装置实现的各个过程，且可以达到相同有益效果，为避免重复，这里不再赘述。
本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现本发明实施例提供一种姿态测量方法的步骤。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。
上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，均属于本发明的保护之内。

Claims (20)

1.一种姿态测量方法，其特征在于，所述方法包括：
获取载波相位差分技术RTK解算的第一固定解，并同时获取载体的加速度计的当前测量值，其中，所述第一固定解为初次获取的固定解；
根据所述第一固定解，确定航向角初值，并将所述第一固定解记录为基准固定解，同时，根据所述载体的加速度计的当前测量值，确定俯仰角初值和横滚角初值；
根据所述航向角初值、所述俯仰角初值和所述横滚角初值，确定四元数初值；
根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数；
根据所述实时四元数进行矩阵变换，确定所述载体的实时航向角。
2.根据权利要求1所述的方法，其特征在于，所述根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数，包括：
根据所述载体的陀螺仪的实时测量值，确定实时四元数递推关系；
根据所述实时四元数递推关系，对所述四元数初值进行递推得到实时四元数。
3.根据权利要求1所述的方法，其特征在于，所述获取载波相位差分技术RTK解算的第一固定解，包括：
在第一预设时间内，获取RTK解算的连续固定解；
判断所述连续固定解是否满足第一预设稳定条件；
若所述连续固定解满足第一预设稳定条件，将所述连续固定解的平均值确定为第一固定解。
4.根据权利要求3所述的方法，其特征在于，所述第一预设稳定条件，包括：
在第一预设时间内，RTK解算的基线方差满足第一预设条件，以及根据所述连续固定解确定的连续航向角的方差满足第二预设条件。
5.根据权利要求1所述的方法，其特征在于，在所述获取载波相位差分技术RTK解算的第一固定解，并同时获取载体的加速度计的当前测量值之后，所述方法还包括：
获取RTK解算的第二固定解，并同时获取所述载体的加速度计的当前测量值，其中，所述第二固定解为获取第一固定解之后，继续获取的实时固定解；
根据所述第二固定解，更新所述航向角初值，并将所述第二固定解确定为基准固定解，同时，根据所述载体的加速度计的实时测量值，更新所述俯仰角初值和所述横滚角初值；
根据更新后的航向角初值、更新后的俯仰角初值和更新后的横滚角初值，确定更新后的四元数初值；
所述根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数，包括：
根据所述载体的陀螺仪的实时测量值，对所述更新后的四元数初值进行递推得到实时四元数。
6.根据权利要求5所述的方法，其特征在于，所述根据所述第二固定解，更新所述航向角初值，并将所述第二固定解确定为基准固定解，包括：
判断根据所述第二固定解确定的航向角与根据所述基准固定解确定的航向角初值的差值是否满足第三预设条件；
若根据所述第二固定解确定的航向角与根据所述基准固定解确定的航向角初值的差值满足第三预设条件，判断所述载体的实时航向角是否满足第二预设稳定条件，其中，所述实时航向角为根据所述实时四元数进行矩阵变换确定的实时航向角；
若所述载体的实时航向角满足第二预设稳定条件，根据所述第二固定解，更新所述航向角初值，并将所述第二固定解确定为基准固定解。
7.根据权利要求6所述的方法，其特征在于，所述第二预设稳定条件包括以下至少一项：
在第二预设时间内，所述载体的陀螺仪的实时测量值满足第四预设条件；
在第二预设时间内，RTK解算的基线方差满足第五预设条件，以及所述载体的实时航向角的方差满足第六预设条件；
在第二预设时间内，卫星数量满足第七预设条件，以及信号强度满足第八预设条件。
8.根据权利要求1所述的方法，其特征在于，所述方法还包括：
根据所述载体的加速度计的实时测量值，确定第一俯仰角和第一横滚角；
将所述第一俯仰角确定为所述载体的实时俯仰角，以及将所述第一横滚角确定为所述载体的实时横滚角。
9.根据权利要求8所述的方法，其特征在于，所述方法还包括：
根据所述实时四元数进行矩阵变换，确定第二俯仰角和第二横滚角；
若同一时刻的所述第一俯仰角与所述第二俯仰角的差值满足第九预设条件时，和/或，若同一时刻所述第一横滚角与所述第二横滚角的差值满足第十预设条件时，获取当前RTK解算的实时固定解，根据所述实时固定解，更新所述航向角初值，以及根据所述第一俯仰角与所述第一横滚角分别更新所述俯仰角初值和所述横滚角初值；
根据更新后的航向角初值、更新后的俯仰角初值和更新后的横滚角初值，确定更新后的四元数初值；
所述根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数，包括：
根据所述载体的陀螺仪的实时测量值，对所述更新后的四元数初值进行递推得到实时四元数。
10.一种姿态测量装置，其特征在于，所述装置包括：
第一获取模块，用于获取载波相位差分技术RTK解算的第一固定解，并同时获取载体的加速度计的当前测量值，其中，所述第一固定解为获取的固定解；
第一初值确定模块，用于根据所述第一固定解，确定航向角初值，并将所述第一固定解记录为基准固定解，同时，根据所述载体的加速度计的当前测量值，确定俯仰角初值和横滚角初值；
第二初值确定模块，用于根据所述航向角初值、所述俯仰角初值和所述横滚角初值，确定四元数初值；
递推模块，用于根据所述载体的陀螺仪的实时测量值，对所述四元数初值进行递推得到实时四元数；
姿态确定模块，用于根据所述实时四元数进行矩阵变换，确定所述载体的实时航向角。
11.根据权利要求10所述的装置，其特征在于，所述递推模块包括：
第一确定单元，用于根据所述载体的陀螺仪的实时测量值，确定实时四元数递推关系；
递推单元，用于根据所述实时四元数递推关系，对所述四元数初值进行递推得到实时四元数。
12.根据权利要求10所述的装置，其特征在于，所述第一获取模块包括：
获取单元，用于在第一预设时间内，获取RTK解算的连续固定解；
第一判断单元，用于判断所述连续固定解是否满足第一预设稳定条件；
第二确定单元，用于若所述连续固定解满足第一预设稳定条件，将所述连续固定解的平均值确定为第一固定解。
13.根据权利要求12所述的装置，其特征在于，所述第一预设稳定条件，包括：
在第一预设时间内，RTK解算的基线方差满足第一预设条件，以及根据所述连续固定解确定的连续航向角的方差满足第二预设条件。
14.根据权利要求10所述的装置，其特征在于，所述装置还包括：
第二获取模块，用于获取RTK解算的第二固定解，并同时获取所述载体的加速度计的当前测量值，其中，所述第二固定解为获取第一固定解之后，继续获取的实时固定解；
第一更新模块，用于根据所述第二固定解，更新所述航向角初值，并将所述第二固定解确定为基准固定解，同时，根据所述载体的加速度计的实时测量值，更新所述俯仰角初值和所述横滚角初值；
第二更新模块，用于根据更新后的航向角初值、更新后的俯仰角初值和更新后的横滚角初值，确定更新后的四元数初值；
所述递推模块，还用于根据所述载体的陀螺仪的实时测量值，对所述更新后的四元数初值进行递推得到实时四元数。
15.根据权利要求14所述的装置，其特征在于，所述第一更新模块包括：
第二判断单元，用于判断根据所述第二固定解确定的航向角与根据所述基准固定解确定的航向角初值的差值是否满足第三预设条件；
第三判断单元，用于若根据所述第二固定解确定的航向角与根据所述基准固定解确定的航向角初值的差值满足第三预设条件，判断所述载体的实时航向角是否满足第二预设稳定条件，其中，所述实时航向角为根据所述实时四元数进行矩阵变换确定的实时航向角；
第一更新单元，用于若所述载体的实时航向角满足第二预设稳定条件，根据所述第二固定解，更新所述航向角初值，并将所述第二固定解确定为基准固定解。
16.根据权利要求15所述的装置，其特征在于，所述第二预设稳定条件包括以下至少一项：
在第二预设时间内，所述载体的陀螺仪的实时测量值满足第四预设条件；
在第二预设时间内，RTK解算的基线方差满足第五预设条件，以及所述载体的实时航向角的方差满足第六预设条件；
在第二预设时间内，卫星数量满足第七预设条件，以及信号强度满足第八预设条件。
17.根据权利要求10所述的装置，其特征在于，所述装置还包括：
第一确定模块，用于根据所述载体的加速度计的实时测量值，确定第一俯仰角和第一横滚角；
第二确定模块，用于将所述第一俯仰角确定为所述载体的实时俯仰角，以及将所述第一横滚角确定为所述载体的实时横滚角。
18.根据权利要求17所述的装置，其特征在于，所述装置还包括：
第三确定模块，用于根据所述实时四元数进行矩阵变换，确定第二俯仰角和第二横滚角；
第三更新模块，用于若同一时刻的所述第一俯仰角与所述第二俯仰角的差值满足第九预设条件时，和/或，若同一时刻所述第一横滚角与所述第二横滚角的差值满足第十预设条件时，获取当前RTK解算的实时固定解，根据所述实时固定解，更新所述航向角初值，以及根据所述第一俯仰角与所述第一横滚角分别更新所述俯仰角初值和所述横滚角初值；
第四更新模块，用于根据更新后的航向角初值、更新后的俯仰角初值和更新后的横滚角初值，确定更新后的四元数初值；
所述递推模块，还用于根据所述载体的陀螺仪的实时测量值，对所述更新后的四元数初值进行递推得到实时四元数。
19.一种电子设备，其特征在于，包括处理器、存储器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如权利要求1至9中任一项所述的方法的步骤。
20.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至9中任一项所述的方法的步骤。
CN201911189225.5A 2019-11-28 2019-11-28 一种姿态测量方法、装置及电子设备 Active CN112859138B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911189225.5A CN112859138B (zh) 2019-11-28 2019-11-28 一种姿态测量方法、装置及电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911189225.5A CN112859138B (zh) 2019-11-28 2019-11-28 一种姿态测量方法、装置及电子设备

Publications (2)

Publication Number Publication Date
CN112859138A true CN112859138A (zh) 2021-05-28
CN112859138B CN112859138B (zh) 2023-09-05

Family

ID=75985914

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911189225.5A Active CN112859138B (zh) 2019-11-28 2019-11-28 一种姿态测量方法、装置及电子设备

Country Status (1)

Country Link
CN (1) CN112859138B (zh)

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101413800A (zh) * 2008-01-18 2009-04-22 南京航空航天大学 导航/稳瞄一体化系统的导航、稳瞄方法
US20100023180A1 (en) * 2008-07-24 2010-01-28 Gm Global Technology Operations, Inc. Adaptive vehicle control system with driving style recognition based on lane-change maneuvers
US20110267225A1 (en) * 2010-04-28 2011-11-03 Liwen Dai System and method for determining the heading angle of a vehicle
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航系统及其导航定位方法
CN102654404A (zh) * 2011-03-02 2012-09-05 浙江中科无线授时与定位研发中心 一种提高航姿参考系统解算精度和系统抗干扰能力的方法
EP2570823A1 (en) * 2011-09-19 2013-03-20 Raytheon Company Method and apparatus for differential global positioning system (DGPS) - based real time attitude determination (RTAD)
CN104697526A (zh) * 2015-03-26 2015-06-10 上海华测导航技术股份有限公司 用于农业机械的捷联惯导系统以及控制方法
CN106291626A (zh) * 2016-07-21 2017-01-04 深圳市华信天线技术有限公司 姿态角测量初始化方法和装置、姿态角测量方法和装置
CN107478223A (zh) * 2016-06-08 2017-12-15 南京理工大学 一种基于四元数和卡尔曼滤波的人体姿态解算方法
CN108802789A (zh) * 2018-06-20 2018-11-13 北京华力创通科技股份有限公司 载体姿态数据测量方法、装置以及电子设备
CN109163721A (zh) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 姿态测量方法及终端设备
CN109490932A (zh) * 2018-12-26 2019-03-19 上海司南卫星导航技术股份有限公司 判断rtk定向结果可靠性的方法、oem板卡、接收机及存储介质
US20190113342A1 (en) * 2017-10-13 2019-04-18 JVC Kenwood Corporation Angular speed derivation device and angular speed derivation method for deriving angular speed based on output value of triaxial gyro sensor
CN109839650A (zh) * 2019-01-04 2019-06-04 哈瓦国际航空技术（深圳）有限公司 一种无人机兼容rtk定位方法、系统、设备和存储介质
CN109916399A (zh) * 2019-04-04 2019-06-21 中国人民解放军火箭军工程大学 一种阴影下的载体姿态估计方法

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101413800A (zh) * 2008-01-18 2009-04-22 南京航空航天大学 导航/稳瞄一体化系统的导航、稳瞄方法
US20100023180A1 (en) * 2008-07-24 2010-01-28 Gm Global Technology Operations, Inc. Adaptive vehicle control system with driving style recognition based on lane-change maneuvers
US20110267225A1 (en) * 2010-04-28 2011-11-03 Liwen Dai System and method for determining the heading angle of a vehicle
CN102654404A (zh) * 2011-03-02 2012-09-05 浙江中科无线授时与定位研发中心 一种提高航姿参考系统解算精度和系统抗干扰能力的方法
EP2570823A1 (en) * 2011-09-19 2013-03-20 Raytheon Company Method and apparatus for differential global positioning system (DGPS) - based real time attitude determination (RTAD)
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航系统及其导航定位方法
CN104697526A (zh) * 2015-03-26 2015-06-10 上海华测导航技术股份有限公司 用于农业机械的捷联惯导系统以及控制方法
CN107478223A (zh) * 2016-06-08 2017-12-15 南京理工大学 一种基于四元数和卡尔曼滤波的人体姿态解算方法
CN106291626A (zh) * 2016-07-21 2017-01-04 深圳市华信天线技术有限公司 姿态角测量初始化方法和装置、姿态角测量方法和装置
US20190113342A1 (en) * 2017-10-13 2019-04-18 JVC Kenwood Corporation Angular speed derivation device and angular speed derivation method for deriving angular speed based on output value of triaxial gyro sensor
CN108802789A (zh) * 2018-06-20 2018-11-13 北京华力创通科技股份有限公司 载体姿态数据测量方法、装置以及电子设备
CN109163721A (zh) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 姿态测量方法及终端设备
CN109490932A (zh) * 2018-12-26 2019-03-19 上海司南卫星导航技术股份有限公司 判断rtk定向结果可靠性的方法、oem板卡、接收机及存储介质
CN109839650A (zh) * 2019-01-04 2019-06-04 哈瓦国际航空技术（深圳）有限公司 一种无人机兼容rtk定位方法、系统、设备和存储介质
CN109916399A (zh) * 2019-04-04 2019-06-21 中国人民解放军火箭军工程大学 一种阴影下的载体姿态估计方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
LI, XM: "The study of UAV flying attitude testing and measuring system in long-distance", 《ICEMI\'2003: PROCEEDINGS OF THE SIXTH INTERNATIONAL CONFERENCE ON ELECTRONIC MEASUREMENT & INSTRUMENTS》 *
庞春雷;赵修斌;余永林;宋立梅;王祝欣;: "加速度计辅助的双天线姿态快速测量方法", 中国惯性技术学报, no. 06 *
张闻宇;王进;张智刚;何杰;胡炼;罗锡文;: "基于自校准变结构Kalman的农机导航BDS失锁续航方法", 农业机械学报, no. 03 *
李振亚: "一种提高RTK航向角初始化可靠性的方法", 《第九届中国卫星导航学术年会论文集——S01 卫星导航应用技术》 *

Also Published As

Publication number Publication date
CN112859138B (zh) 2023-09-05

Similar Documents

Publication Publication Date Title
CN104884902B (zh) 用于三轴磁力计和三轴加速度计的数据融合的方法和装置
US4930085A (en) Method for determining the heading of an aircraft
JP5717875B2 (ja) 位置決定方法
CN114018274B (zh) 车辆定位方法、装置及电子设备
CN106370178B (zh) 移动终端设备的姿态测量方法及装置
CN116067370B (zh) 一种imu姿态解算方法及设备、存储介质
CN111896007A (zh) 一种补偿足地冲击的四足机器人姿态解算方法
JP7025215B2 (ja) 測位システム及び測位方法
CN110395297B (zh) 列车定位方法
CN110440797A (zh) 车辆姿态估计方法及系统
CN110057356A (zh) 一种隧道内车辆定位方法及装置
CN111854793B (zh) 惯性测量单元和全球导航系统之间杆臂的标定方法和装置
CN112859139B (zh) 一种姿态测量方法、装置及电子设备
CN109916417B (zh) 一种地图建立方法、装置、计算机设备及其存储介质
CN113959433B (zh) 一种组合导航方法及装置
CN114894185A (zh) 基于蓝牙aoa和imu融合的载体姿态零速校正系统
CN112859138B (zh) 一种姿态测量方法、装置及电子设备
CN116718153B (zh) 一种基于gnss和ins的形变监测方法及系统
JP6384194B2 (ja) 情報処理装置、情報処理方法及び情報処理プログラム
CN114994352B (zh) 一种高速旋转制导炮弹转速测量方法
CN114111768B (zh) 一种基于偏角异常补偿的轨道线型拟合方法
CN114895340A (zh) 双天线gnss/ins组合导航系统的定位方法和装置
CN115655269A (zh) 一种三轴超声波辅助下的行人自主定位导航方法
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
CN114280656A (zh) 一种gnss系统的测姿方法和系统

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