CN108318027A - 载体的姿态数据的确定方法和装置 - Google Patents
载体的姿态数据的确定方法和装置 Download PDFInfo
- Publication number
- CN108318027A CN108318027A CN201710039666.1A CN201710039666A CN108318027A CN 108318027 A CN108318027 A CN 108318027A CN 201710039666 A CN201710039666 A CN 201710039666A CN 108318027 A CN108318027 A CN 108318027A
- Authority
- CN
- China
- Prior art keywords
- data
- kth
- time
- kth time
- kalman filter
- 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
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
-
- 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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
本发明公开了一种载体的姿态数据的确定方法和装置。其中，该方法包括：获取载体的惯性传感器输出的传感器数据；使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据，其中，扩展卡尔曼滤波器的状态变量和观测变量都是由旋转变换矩阵表示。本发明解决了相关技术中对载体的姿态数据的确定不准确的技术问题。
Description
技术领域
本发明涉及数据处理领域，具体而言，涉及一种载体的姿态数据的确定方法和装置。
背景技术
目前，在对载体的姿态进行测量时，将加速度值和磁场值作为扩展卡尔曼滤波(Extended Kalman Filter，简称为EKF)状态变量X和观测变量Z。例如，X＝(accex,accey,accez,magx,magy,magz)，为6维实数，其中，accex用于表示投影到x轴上的加速度，accey用于表示投影到y轴上的加速度，accez用于表示投影到z轴上的加速度，magx用于表示投影到x轴上的磁场值，magy用于表示投影到y轴上的磁场值，magz用于表示投影到z轴上的磁场值。自由度为计算某一统计量时，取值不受限制的变量个数，载体的姿态信息的自由度为3，但是X的最大秩为6，这将导致状态变量和观测变量的协方差不满秩，为奇异矩阵，导致对载体的姿态测量不准确，进而对载体的姿态数据的确定不准确。因此，将加速度值和磁场值作为扩展卡尔曼滤波不妥。
相关技术中还提供了另一种对载体的姿态进行测量的方法，使用欧拉角(α,β,γ)作为EKF的状态变量和观测变量。通过使用欧拉角(α,β,γ)作为状态变量可以解决协方差不满秩的问题，但是通过欧拉角描述姿态会存在不连续和万向节死锁(Gimbal Lock)的问题，导致对载体的姿态数据的确定不准确。
针对上述对载体的姿态数据的确定不准确的问题，目前尚未提出有效的解决方案。
发明内容
本发明实施例提供了一种载体的姿态数据的确定方法和装置，以至少解决相关技术中对载体的姿态数据的确定不准确的技术问题。
根据本发明实施例的一个方面，提供了一种载体的姿态数据的确定方法。该载体的姿态数据的确定方法包括：获取载体的惯性传感器输出的传感器数据；使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据，其中，扩展卡尔曼滤波器的状态变量和观测变量都是由旋转变换矩阵表示。
根据本发明实施例的另一方面，还提供了一种载体的姿态数据的确定装置。载体的姿态数据的确定装置包括：获取单元，用于获取载体的惯性传感器输出的传感器数据；融合单元，用于使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据，其中，扩展卡尔曼滤波器的状态变量和观测变量都是由旋转变换矩阵表示。
在本发明实施例中，采用载体的姿态数据的确定方法，通过获取载体的惯性传感器输出的传感器数据；使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据，扩展卡尔曼滤波器的状态变量和观测变量都是由旋转变换矩阵表示，由于使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据，避免了使用加速度值和磁场值作为状态变量和观测变量，状态变量和观测变量的协方差矩阵不满秩的问题，以及避免了使用欧拉角作为状态变量和观测变量所导致的不连续和万向节死锁的问题，提高了对载体的姿态数据进行确定的准确性，进而解决了相关技术中对载体的姿态数据的确定不准确的技术问题。
附图说明
此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1是根据本发明实施例的一种载体的姿态数据的确定方法的硬件环境的示意图；
图2是根据本发明实施例的一种载体的姿态数据的确定方法的流程图；
图3是根据本发明实施例的另一种载体的姿态数据的确定方法的流程图；
图4是根据本发明实施例的一种载体的姿态数据测量的示意图；
图5是根据本发明实施例的一种载体的姿态的示意图；
图6是根据本发明实施例的一种载体的姿态数据的确定装置的示意图；以及
图7是根据本发明实施例的一种终端的结构框图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
实施例1
根据本发明实施例，提供了一种载体的姿态数据的确定方法的实施例。
可选地，在本实施例中，上述载体的姿态数据的确定方法可以应用于如图1所示的由服务器102和终端104所构成的硬件环境中。图1是根据本发明实施例的一种载体的姿态数据的确定方法的硬件环境的示意图。如图1所示，服务器102通过网络与终端104进行连接，上述网络包括但不限于：广域网、城域网或局域网，终端104并不限定于PC、手机、平板电脑等。本发明实施例的载体的姿态数据的确定方法可以由服务器102来执行，也可以由终端104来执行，还可以是由服务器102和终端104共同执行。其中，终端104执行本发明实施例的载体的姿态数据的确定方法也可以是由安装在其上的客户端来执行。
图2是根据本发明实施例的一种载体的姿态数据的确定方法的流程图。如图2所示，该载体的姿态数据的确定方法可以包括以下步骤：
步骤S202，获取载体的惯性传感器输出的传感器数据。
在本发明上述步骤S202提供的技术方案中，获取载体的惯性传感器输出的传感器数据。
载体携带惯性传感器，该惯性传感器可以包括陀螺仪、加速度计和磁场计，可以通过微机电系统(Micro Electro Mechanical Systems，简称为MEMS)集成电路实现，其中，陀螺仪用于输出角速度数据ωk，加速度计用于输出加速度ak，磁场计用于输出磁场值bk。载体可以为虚拟现实设备、飞行器、机器人等。惯性传感器输出传感器数据有一定周期，比如，1秒输出传感器数据100次，一下午输出传感器数据3次等。获取载体的惯性传感器第k次输出的第k次传感器数据，k为大于1的自然数，第k次传感器数据包括陀螺仪第k次输出的角速度数据，第k次传感器数据还包括加速度计第k次输出的加速度数据和/或磁场计第k次输出的磁场强度数据。
步骤S204，使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据。
在本发明上述步骤S204提供的技术方案中，使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据，其中，扩展卡尔曼滤波器的状态变量和观测变量都是由旋转变换矩阵表示。
载体在运动过程中，在不同的时刻可能具有不同的姿态，可选地，姿态为载体在运动过程中，在某一时刻的翻滚(Roll)、偏航(Yaw)、俯仰(Pitch)等姿态，可以通过载体坐标系到世界坐标系的旋转矩阵来表示，其中，定义世界坐标系为垂直地面朝上为Z轴，平行地面东向为X轴，平行地面北向为Y轴的坐标系。李群(Lie group)是一种连续变换群具有群结构的实流形或者复流形，包括载体坐标系到世界坐标系的旋转矩阵，并且假设状态变量的后验分布在李群上满足高斯分布。在获取载体的惯性传感器输出的传感器数据之后，使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据，也即，使用在李群上的扩展卡尔曼滤波器(EKF)对惯性传感器的输出信号进行滤波融合。
作为一种可选的示例，对传感器数据进行融合可以理解为：根据载体的多个传感器多次输出的数据进行预估和校正处理。通过预估和校正处理，可以准确地获取载体的姿态数据。
例如，多个传感器多次输出的数据包括：陀螺仪多次输出的角速度数据、加速度计多次输出的加速度、磁场计多次输出的磁场值；在对传感器数据进行融合的过程中，根据载体的上一次姿态数据和陀螺仪本次输出的角速度数据获得本次载体的姿态数据的预估数据，再根据加速度计本次输出的加速度数据和/或磁场计本次输出的磁场强度数据对预估数据进行校正，得到载体的本次姿态数据，从而实现了对角速度数据、加速度数据和/或磁场强度数据进行融合，准确地得到了载体的姿态数据。
扩展卡尔曼滤波器包括状态变量和观测变量，状态变量和观测变量都由旋转变换矩阵表示，旋转变换矩阵属于李群。
在使用李群上的扩展卡尔曼滤波器对传感器数据进行融合的过程中，获取姿态数据的预估数据和传感器数据，通过李群上的扩展卡尔曼滤波器根据传感器数据对姿态数据的预估数据进行校正，从而得到载体的姿态数据。可选地，获取加速度计本次输出的加速度数据和/或磁场计本次输出的磁场强度数据，根据加速度计本次输出的加速度数据和/或磁场计本次输出的磁场强度数据对上述预估数据进行校正，得到载体本次的姿态数据，从而实现了使用扩展卡尔曼滤波器对陀螺仪本次输出的角速度数据、陀螺仪本次输出的角速度数据进行融合得到姿态数据的目的。
在本申请的实施例中，本次载体的姿态数据的预估数据可以是根据上一次载体的姿态数据预估得到的数据，该预估得到的数据可以是为未进行校正的载体的姿态数据，比如，未根据加速度计本次输出的加速度数据和/或磁场计本次输出的磁场强度数据进行校正的载体的姿态数据。可选地，根据上一次载体的姿态数据、陀螺仪本次输出的角速度数据和时间差获得本次载体的姿态数据的预估数据，其中，上一次姿态数据是使用扩展卡尔曼滤波器对惯性传感器输出的上一次传感器数据进行融合得到的姿态数据。
通过使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据，避免了使用加速度值和磁场值作为状态变量和观测变量，当姿态信息的自由度为3时，状态变量和观测变量的协方差矩阵不满秩的问题，以及避免了使用欧拉角作为状态变量和观测变量所导致的不连续和万向节死锁的问题，提高了对载体的姿态数据进行确定的准确性。
该实施例通过获取载体的惯性传感器输出的传感器数据；使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据，提高了对载体的姿态数据进行确定的准确性，进而解决了相关技术中对载体的姿态数据的确定不准确的技术问题。
作为一种可选的实施方式，旋转变换矩阵属于李群；扩展卡尔曼滤波器的状态变量为传感器坐标系到世界坐标系的旋转变换矩阵；扩展卡尔曼滤波器的观测变量包括扩展卡尔曼滤波器的加速度计观测变量和扩展卡尔曼滤波器的磁场计观测变量。
作为一种可选的实施方式，扩展卡尔曼滤波器的加速度计观测变量用于表示传感器坐标系下重力加速度向量到世界坐标系下重力加速度向量的旋转；扩展卡尔曼滤波器的磁场计观测变量用于表示传感器坐标系下地磁指北向量到世界坐标系下地磁指北向量的旋转。
上述实施例的扩展卡尔曼滤波器包括状态变量和观测变量。旋转变换矩阵属于李群，状态变量和观测变量都是由旋转变换矩阵表示，可以将扩展卡尔曼滤波器的状态变量和观测变量通过传感器坐标系到世界坐标系的旋转所得到的旋转变换矩阵进行表示，其中，传感器坐标系也即载体坐标系，世界坐标系是系统的绝对坐标系，在没有建立载体坐标系之前，所有点的坐标都是以世界坐标系的原点来确定各自的位置，该实施例定义垂直地面朝上为Z轴，平行地面东向为X轴，平行地面北向为Y轴的坐标系为世界坐标系，从而使得状态变量和观测变量在李群上连续。
状态变量为传感器坐标系到世界坐标系的旋转变换矩阵，假设状态变量的后验分布在李群上是一个高斯分布，由于李群具有连续光滑性，该方案比现有技术方案使用加速度值和磁场值作为状态变量更加合理，避免了状态变量的协方差矩阵不满秩的情况发生，也比现有技术使用欧拉角作为状态变量更加合理，避免了存在万向节和死锁的问题。
扩展卡尔曼滤波器的观测变量包括扩展卡尔曼滤波器的加速度计观测变量和扩展卡尔曼滤波器的磁场计观测变量。扩展卡尔曼滤波器的加速度计观测变量用于表示加速度计输出的加速度数据，在获取扩展卡尔曼滤波器的加速度计观测变量时，确定重力加速度在传感器坐标系下的重力加速度向量，确定重力加速度在世界坐标系下的重力加速度向量，将传感器坐标下重力加速度向量绕某一固定轴旋转至与世界坐标系下重力加速度向量平行并同向的旋转来表征卡尔曼滤波器的加速度计观测变量，也即，扩展卡尔曼滤波器的加速度计观测变量用于表示传感器坐标系下重力加速度向量到世界坐标系下重力加速度向量的旋转；扩展卡尔曼滤波器的磁场计观测变量用于表示磁场计输出的磁场强度数据，在获取扩展卡尔曼滤波器的磁场计观测变量时，确定传感器坐标系下的地磁指北向量，确定世界坐标系下的地磁指北向量，将传感器坐标系下地磁指北向量绕某一固定轴旋转至与世界坐标系下地磁指北向量平行并同向的旋转来表征卡尔曼滤波器的加速度计观测变量，也即，扩展卡尔曼滤波器的磁场计观测变量用于表示传感器坐标系下地磁指北向量到世界坐标系下地磁指北向量的旋转。
扩展卡尔曼滤波器包括预测模型，该预测模型可以通过状态转移方程表示。通过预测模型可以获取在对传感器数据进行融合的过程中的状态变量，得到姿态数据的预估数据，进而通过姿态数据的预估数据对传感器数据进一步融合，得到载体的姿态数据。该预测模型可以为匀角速度模型，可以根据陀螺仪输出的角速度数据和时间差进行确定。
作为一种可选的实施方式，惯性传感器包括陀螺仪，其中，在扩展卡尔曼滤波器的预测模型为匀角速度模型的情况下，对于陀螺仪第k次输出的角速度数据，预测模型用于：根据扩展卡尔曼滤波器第k-1次的状态变量、陀螺仪第k次输出的角速度数据和时间差获得该扩展卡尔曼滤波器第k次的状态变量，时间差为陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔。
在该实施例中，惯性传感器按照一定的周期输出传感器数据，比如，1秒输出传感器数据100次，一下午输出传感器数据3次等。在扩展卡尔曼滤波器的预测模型为匀角速度模型的情况下，惯性传感器包括陀螺仪，陀螺仪按照一定的周期输出角速度数据，比如，第k-1次输出的角速度数据，第k次输出的角速度数据，其中，k为大于1的自然数。预测模型可以通过第k-1次的状态变量、陀螺仪第k次输出的角速度数据、陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔获得扩展卡尔曼滤波器第k次的状态变量。为了提高测量的准确性，将高斯噪声也引入计算中。该预测模型通过该以下公式进行表示：
在上述公式中，expG()为矩阵的指数运算， 为 为三维实数空间，
∈＝(∈1,∈2,∈3)T
ωk为陀螺仪第k次输出的角速度数据，Xk为第k次的状态变量，Xk-1为第k-1次的状态变量，时间差Δtk＝tk-tk-1，tk和tk-1分别为第k次输出角速度数据的时间与第k-1次输出角速度数据的时间；过程噪声表示的是nk-1是均值为0，方差为Rk-1的高斯噪声，Rk-1是预测模型的第k-1次的预设过程噪声协方差，可选地，预测模型中每次的预设过程噪声协方差都相同。
作为一种可选的实施方式，惯性传感器包括加速度计，扩展卡尔曼滤波器的观测变量包括扩展卡尔曼滤波器的加速度计观测变量，其中，在加速度计观测变量用于表示传感器坐标系下重力加速度向量到世界坐标系下重力加速度向量的旋转的情况下，传感器坐标系下重力加速度向量包括加速度计第k次输出的加速度数据，加速度计观测变量与扩展卡尔曼滤波器第k次的状态变量通过加速度计测量噪声建立关联关系，加速度计测量噪声为均值为0，方差为第一方差值的高斯白噪声。
惯性传感器还可以包括加速度计，加速度计按一定的周期输出加速度数据，比如，第k-1次输出的加速度数据，第k次输出的加速度数据，其中，k为大于1的自然数。在扩展卡尔曼滤波器的加速度计观测变量用于表示传感器坐标系下重力加速度向量到世界坐标系下重力加速度向量的旋转的情况下，对于加速度计第k次输出的加速度数据，加速度计观测变量对应的测量方程包括：
在上述公式中，定义为使向量v1与向量v2平行并同向的旋转矩阵。v1包括ak，ak为加速度计第k次输出的加速度数据，v2包括g，g为世界坐标系下重力加速度向量，为加速度计观测变量，用于表示向量ak到向量g的旋转，换句话说，用于表示使向量ak与向量g平行并同向的旋转，并且加速度计观测变量与扩展卡尔曼滤波器第k次的状态变量Xk通过加速度计测量噪声建立关联关系，该关联关系通过上述加速度计观测变量对应的测量方程表示，其中，加速度计测量噪声表示的是δk是均值为0，方差为第一方差值的高斯噪声，为预设加速度计测量噪声协方差，可选地，在加速度计每次输出加速度数据时，每次为相同的预设加速度计测量噪声协方差。
作为一种可选的实施方式，惯性传感器包括磁场计，扩展卡尔曼滤波器的观测变量包括扩展卡尔曼滤波器的磁场计观测变量，其中，在扩展卡尔曼滤波器的磁场计观测变量用于表示为传感器坐标系下地磁指北向量到世界坐标系下地磁指北向量的旋转的情况下，传感器坐标系下地磁指北向量包括磁场计第k次输出的磁场强度数据，磁场计观测变量与扩展卡尔曼滤波器第k次的状态变量通过磁场计测量噪声建立关联关系，磁场计测量噪声为均值为0，方差为第二方差值的高斯白噪声。
惯性传感器还可以包括磁场计，磁场计按一定的周期输出磁场强度数据，比如，第k-1次输出的磁场强度数据，第k次输出的磁场强度数据，其中，k为大于1的自然数。在扩展卡尔曼滤波器的磁场计观测变量用于表示传感器坐标系下地磁指北向量到世界坐标系下地磁指北向量的旋转的情况下，对于磁场计第k次输出的磁场强度数据，磁场计观测变量对应的测量方程包括：
在上述公式中，v1包括bk，bk为磁场计第k次输出的磁场强度数据，v2包括r，r为世界坐标系下地磁指北向量，为磁场计观测变量，用于表示向量bk到向量r的旋转，换句话说，用于表示使向量bk与向量r平行并同向的旋转，并且磁场计观测变量与扩展卡尔曼滤波器第k次的状态变量Xk通过磁场计测量噪声建立关联关系，该关联关系通过上述磁场计观测变量对应的测量方程表示，其中，磁场计测量噪声表示的是εk是均值为0，方差为第二方差值的高斯噪声，为预设磁场计测量噪声协方差，可选地，在磁场计每次输出磁场强度数据时，每次为相同的预设磁场计测量噪声协方差。
该实施例在扩展卡尔曼滤波器的预测模型为匀角速度模型的情况下，通过第k-1次的状态变量、陀螺仪第k次输出的角速度数据、第k次与第k-1次的时间差、高斯噪声形成的解析公式表示第k次的状态变量，实现了对预测模型的建立；在扩展卡尔曼滤波器的加速度计观测变量用于表示传感器坐标系下重力加速度向量到世界坐标系下重力加速度向量的旋转的情况下，通过第k次的状态变量、世界坐标系下重力加速度向量和第k次加速度计输出加速度数据时的高斯噪声形成的解析公式表示加速度计观测变量，实现了对加速度计观测变量对应的测量方程的建立；在扩展卡尔曼滤波器的磁场计观测变量用于表示传感器坐标系下地磁指北向量到世界坐标系下地磁指北向量的旋转的情况下，通过第k次的状态变量、世界坐标系下地磁指北向量和第k次磁场计输出磁场强度数据时的高斯噪声形成的解析公式表示磁场计观测变量，实现了对磁场计观测变量对应的测量方程的建立，从而便于载体的姿态数据的确定，达到提高姿态数据确定的准确性的效果。
作为一种可选的实施方式，步骤S202，获取载体的惯性传感器输出的传感器数据包括：获取载体的惯性传感器第k次输出的第k次传感器数据，其中，k为大于1的自然数。
惯性传感器在输出传感器数据时具有一定的周期，比如，1秒输出100次传感器数据，一下午输出3次传感器数据等，该传感器数据包括角速度数据、加速度数据和/或磁场强度数据。获取载体的惯性传感器第k次输出的第k次传感器数据，该第k次传感器数据包括陀螺仪第k次输出的角速度数据，还包括加速度计第k次输出的加速度数据和/或磁场计第k次输出的磁场强度数据。
作为一种可选的实施方式，传感器数据包括陀螺仪输出的角速度数据，扩展卡尔曼滤波器的加速度计输出的加速度数据，和/或扩展卡尔曼滤波器的磁场计输出的磁场强度数据，步骤S204，使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据包括：根据第k-1次姿态数据和陀螺仪第k次输出的角速度数据获得第k次姿态数据的预估数据；根据加速度计第k次输出的加速度数据和/或磁场计第k次输出的磁场强度数据对预估数据进行校正，得到第k次姿态数据。
图3是根据本发明实施例的另一种载体的姿态数据的确定方法的流程图。如图3所示，该载体的姿态数据的确定方法包括以下步骤：
步骤S301，根据第k-1次姿态数据和陀螺仪第k次输出的角速度数据获得第k次姿态数据的预估数据。
在本发明上述步骤S301提供的技术方案中，根据第k-1次姿态数据和陀螺仪第k次输出的角速度数据获得第k次姿态数据的预估数据，其中，第k-1次姿态数据是由扩展卡尔曼滤波器对惯性传感器输出的第k-1次传感器数据进行融合得到的载体的姿态数据，载体的惯性传感器第k次输出的第k次传感器数据包括陀螺仪第k次输出的角速度数据。
在获取载体的惯性传感器第k次输出的第k次传感器数据之后，在获取到陀螺仪第k次输出的角速度数据的情况下，获取第k-1次姿态数据，根据第k-1次姿态数据和陀螺仪第k次输出的角速度数据获得第k次姿态数据的预估数据，实现了时间更新的目的，该第k次姿态数据的预估数据是用于对传感器数据进行融合的数据。
步骤S302，根据加速度计第k次输出的加速度数据和/或磁场计第k次输出的磁场强度数据对预估数据进行校正，得到载体的第k次姿态数据。
在本发明上述步骤S302提供的技术方案中，根据加速度计第k次输出的加速度数据和/或磁场计第k次输出的磁场强度数据对预估数据进行校正，得到载体的第k次姿态数据，其中，载体的惯性传感器第k次传感器数据还包括加速度计第k次输出的加速度数据和/或磁场计第k次输出的磁场强度数据。
在根据第k-1次姿态数据和陀螺仪第k次输出的角速度数据获得第k次姿态数据的预估数据之后，在获取到加速度计第k次输出的加速度数据的情况下，根据加速度计第k次输出的加速度数据对预估数据进行校正，得到载体的第k次姿态数据，从而实现了加速度计测量更新的目的。或者在获取到磁场计第k次输出的磁场强度数据的情况下，根据磁场计第k次输出的磁场强度数据对预估数据进行校正，得到载体的第k次姿态数据，从而实现了磁场计测量更新的目的。或者在获取到加速度计第k次输出的加速度数据的情况下，且在获取到磁场计第k次输出的磁场强度数据的情况下，根据加速度计第k次输出的加速度数据对预估数据进行第一次校正，得到第一次校正数据，再根据磁场计第k次输出的磁场强度数据对第一次校正数据进行第二次校正，得到载体的k次姿态数据。
该实施例的传感器数据包括陀螺仪输出的角速度数据，扩展卡尔曼滤波器的加速度计输出的加速度数据，和/或扩展卡尔曼滤波器的磁场计输出的磁场强度数据，通过根据载体的第k-1次姿态数据和陀螺仪第k次输出的角速度数据获得载体的第k次姿态数据的预估数据，根据加速度计第k次输出的加速度数据和/或磁场计第k次输出的磁场强度数据对预估数据进行校正，得到第k次姿态数据，从而提高了对载体的姿态数据进行确定的准确性，进而解决了相关技术中对载体的姿态数据的确定不准确的技术问题。
作为一种优选的实施方式，步骤S302，根据加速度计第k次输出的加速度数据和/或磁场计第k次输出的磁场强度数据对预估数据进行校正，得到第k次姿态数据包括：根据加速度计第k次输出的加速度数据对预估数据进行校正，得到第k次姿态数据。
该实施例仅通过加速度计第k次输出的加速度数据第k次姿态数据对预估数据进行校正，得到第k次姿态数据，实现了使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合时先进行时间更新，再加速度计测量更新的目的，实现了加速度计和陀螺仪的融合，提高了对载体的姿态数据的确定的准确性。
作为一种可选的实施方式，根据载体的第k-1次姿态数据和陀螺仪第k次输出的角速度数据获得载体的第k次姿态数据的预估数据包括：根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据，其中，时间差为陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔；根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
可选地，根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据包括：通过如下公式获得第k次姿态数据的预估数据μk|k-1，
其中，μk-1|k-1为第k-1次姿态数据，ωk为陀螺仪第k次输出的角速度，Δtk＝tk-tk-1为时间差，tk和tk-1分别为第k次输出角速度数据的时间与所述陀螺仪第k-1次输出角速度数据的时间，expG()为矩阵的指数运算， 为 为三维实数空间，
∈＝(∈1,∈2,∈3)T
可选地，根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量包括：通过如下公式获得第一中间变量Pk|k-1，
其中，Pk-1|k-1也为扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差，Rk-1是扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差，可选地，预测模型的每次的预设过程噪声协方差相同。
作为一种可选的实施方式，根据加速度计第k次输出的加速度数据对预估数据进行校正，得到载体的第k次姿态数据包括：根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第k次姿态数据；根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
可选地，根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第k次姿态数据包括：通过如下公式获得第k次姿态数据μk|k，
其中，Pk|k-1为第一中间变量， μk|k-1为第k次姿态数据的预估数据，g为世界坐标系下重力加速度向量，为预设加速度计测量噪声协方差，ak为加速度计第k次输出的加速度数据，logG()为矩阵的对数运算， 为μk|k也为均值。
可选地，根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差包括：通过如下公式获得第k次的状态变量的协方差Pk|k，
Pk|k＝(I-KkHk)Pk|k-1
其中，Pk|k-1为上述第一中间变量，Kk由上述得到，Hk由上述得到。
需要说明的是，本发明实施例可以采用多种方式实现根据加速度计第k次输出的加速度数据对预估数据进行校正，得到第k次姿态数据，上述获得姿态数据的方法仅为本发明的优选实施例，并不代表本发明实施例的获得姿态数据的方法仅包括上述方式。为了提高对载体的姿态数据的确定的准确性，本发明实施例还可以包括其他获得姿态数据的方法，此处不再一一举例说明。
作为一种可选的实施方式，步骤S302，根据加速度计第k次输出的加速度数据和/或磁场计第k次输出的磁场强度数据对预估数据进行校正，得到第k次姿态数据包括：根据磁场计第k次输出的磁场强度数据对预估数据进行校正，得到第k次姿态数据。
该实施例仅通过磁场计第k次输出的磁场强度数据对预估数据进行校正，得到第k次姿态数据，实现了使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合时先进行时间更新，再进行磁场计测量更新的目的，实现了加速度计和磁场仪的融合，提高了对载体的姿态数据的确定的准确性。
作为一种可选的实施方式，根据载体的第k-1次姿态数据和陀螺仪第k次输出的角速度数据获得载体的第k次姿态数据的预估数据包括：根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据，其中，时间差为陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔；根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
可选地，根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据包括：通过如下公式获得第k次姿态数据的预估数据μk|k-1，
其中，μk-1|k-1为第k-1次姿态数据，ωk为陀螺仪第k次输出的角速度，Δtk＝tk-tk-1为时间差，tk和tk-1分别为第k次输出角速度数据的时间与所述陀螺仪第k-1次输出角速度数据的时间，expG()为矩阵的指数运算， 为 为三维实数空间，
∈＝(∈1,∈2,∈3)T
可选地，根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量包括：通过如下公式获得第一中间变量Pk|k-1，
其中，Pk-1|k-1也为扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差，Rk-1是扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差，可选地，预测模型每次的预设过程噪声协方差都相同。
作为一种可选的实施方式，根据磁场计第k次输出的磁场强度数据对预估数据进行校正，得到载体的第k次姿态数据包括：根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得第k次姿态数据；根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
可选地，根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得第k次姿态数据包括：通过如下公式获得第k次姿态数据μk|，
其中，Pk|-1为第一中间变量， μk|k-1为第k次姿态数据的预估数据，r世界坐标系下地磁指北向量，为预设磁场计测量噪声协方差，bk为磁场计第k次输出的磁场强度数据，logG()为矩阵的对数运算， 为μk|k也为均值。
可选地，根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差包括：通过如下公式获得第k次的状态变量的协方差Pk|k，
Pk|k＝(I-KkHk)Pk|k-1
其中，Pk|k-1为上述第一中间变量，Kk由上述得到，Hk由上述得到。
需要说明的是，本发明实施例可以采用多种方式实现根据磁场计第k次输出的磁场强度数据对预估数据进行校正，得到第k次姿态数据，上述获得姿态数据的方法仅为本发明的优选实施例，并不代表本发明实施例的获得姿态数据的方法仅包括上述方式。为了提高对载体的姿态数据的确定的准确性，本发明实施例还可以包括其他获得姿态数据的方法，此处不再一一举例说明。
作为一种可选的实施方式，根据加速度计第k次输出的加速度数据和/或磁场计第k次输出的磁场强度数据对预估数据进行校正，得到第k次姿态数据包括：根据加速度计第k次输出的加速度数据对预估数据进行校正，得到预估数据的第一次校正数据；根据磁场计第k次输出的磁场强度数据对第一次校正数据进行校正，得到第k次姿态数据。
该实施例首先通过加速度计第k次输出的加速度数据对预估数据进行校正，得到第一校正数据，该第一校正数据包括第一次校正后的姿态数据，再通过磁场计输出第k次输出的磁场强度数据对第一校正数据进行校正，得到第k次姿态数据，实现了使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合时先进行时间更新，再加速度测量更新，最后磁场计测量更新的目的，提高了对载体的姿态数据的确定的准确性。
作为一种可选的实施方式，根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据，其中，时间差为陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔；根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
可选地，根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据包括：通过如下公式获得第k次姿态数据的预估数据μk|k-1，
其中，μk-1|k-1为第k-1次姿态数据，ωk为陀螺仪第k次输出的角速度，Δtk＝tk-tk-1为时间差，tk和tk-1分别为第k次输出角速度数据的时间与所述陀螺仪第k-1次输出角速度数据的时间，expG()为矩阵的指数运算， 为 为三维实数空间，
∈＝(∈1,∈2,∈3)T
可选地，根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量包括：通过如下公式获得第一中间变量Pk|k-1，
其中，Pk-1|k-1也为扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差，Rk-1是扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差，可选地，预测模型每次的预设过程噪声协方差都相同。
可选地，根据加速度计第k次输出的加速度数据对预估数据进行校正，得到预估数据的第一次校正数据包括：根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第一次校正后的姿态数据；根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第二中间变量。
可选地，根据加速度计第k次输出的加速度数据对预估数据进行校正，得到预估数据的第一次校正数据包括：通过如下公式获得预估数据的第一次校正数据
其中，Pk|k-1为第一中间变量， μk|k-1为第k次姿态数据的预估数据，g为世界坐标系下重力加速度向量，为预设加速度计测量噪声协方差，ak为加速度计第k次输出的加速度数据，logG()为矩阵的对数运算， 为
可选地，根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第二中间变量包括：通过如下公式获得第二中间变量
其中，Pk|k-1为上述第一中间变量，Kk由上述得到，Hk由上述得到。
可选地，根据第二中间变量、第一次校正后的姿态数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得第k次姿态数据包括：通过如下公式获得第k次姿态数据μk|k，
其中，为第二中间变量， 为第一次校正后的姿态数据，r世界坐标系下地磁指北向量，为预设磁场计测量噪声协方差，bk为磁场计第k次输出的磁场强度数据，logG()为矩阵的对数运算， 为 μk|k也为均值。
可选地，根据第二中间变量、第一次校正后的姿态数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差包括：通过如下公式获得第k次的状态变量的协方差Pk|k，
其中，为上述第二中间变量，Kk由上述得到，Hk由上述得到。
需要说明的是，本发明实施例可以采用多种方式实现根据加速度计第k次输出的加速度数据对预估数据进行校正，得到预估数据的第一次校正数据；根据磁场计第k次输出的磁场强度数据对第一次校正数据进行校正，得到第k次姿态数据，上述获得姿态数据的方法仅为本发明的优选实施例，并不代表本发明实施例的获得姿态数据的方法仅包括上述方式。为了提高对载体的姿态数据的确定的准确性，本发明实施例还可以包括其他获得姿态数据的方法，此处不再一一举例说明。
作为一种可选的实施方式，李群为SO(3)李群，也即，旋转变化矩阵的秩为3。
作为一种可选的实施方式，在使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据之后，还包括：将载体的姿态数据发送给载体的控制器。
可选地，将第k次姿态数据发送给载体的控制器，通过控制器以控制载体的姿态的变化。该载体可以为虚拟现实设备、飞行器、机器人等。比如，在虚拟现实头盔中，融合惯性传感器数据以实现头部姿态的准确跟踪，在多翼飞行器中，融合传感器数据得到飞行器的姿态数据，反馈给控制器从而控制飞行器的平衡，从而准确地估计载体的姿态。
需要说明的是，本发明实施中使用的惯性传感器为MEMS元件，对于使用单独输出加速度、角速度和磁场信息的其他元件，进行类似融合滤波的方法，也属于本发明的内容。
本发明实施例中状态变量和观测变量都属于李群，其他使用IMU(惯性测量单元)，并采用类似状态变量和观测变量的设计，进行融合滤波获取姿态信息的方法属于本发明的内容。
实施例2
下面结合优选的实施例对本发明的技术方案进行说明。
图4是根据本发明实施例的一种载体的姿态数据测量的示意图。该实施例的载体携带惯性传感器，该惯性传感器包括陀螺仪、加速度计和磁场计。可选地，陀螺仪为三轴陀螺仪，输出角速度数据ωk，加速度计为三轴加速度计，输出加速度ak，磁场计为三轴磁场计，输出磁场值bk。
本发明实施例中使用扩展卡尔曼滤波器，将传感器坐标系到世界坐标系的旋转矩阵X(X∈SO(3)，SO(3)是一个李群G)作为扩展卡尔曼滤波器的状态变量，并假设状态变量X的后验概率分布在李群G上满足均值为μ，方差为P的高斯分布，即：
其中，均值为0，方差为P的高斯分布，即
为 为
∈＝(∈1,∈2,∈3)T
expG()为矩阵的指数运算，
logG()为矩阵的对数运算，
在对载体的姿态数据更新的系统模型中，对于k时刻时间tk，陀螺仪输出角速度数据状态转移方程为：
其中，ωk为陀螺仪第k次输出的角速度数据，Xk为第k次的状态变量，Xk-1为第k-1次的状态变量，时间差Δtk＝tk-tk-1，tk和tk-1分别为第k次与第k-1次的时间；过程噪声表示的是nk-1是均值为0，方差为Rk-1的高斯噪声，Rk-1是预测模型的第k-1次的预设过程噪声协方差，可选地，预测模型每次的预设过程噪声协方差相同。
定义在空间中，将向量v1绕某一固定转轴旋转至与向量v2平行并同向的旋转矩阵为
在tk时间，对于观测到加速度计输出本发明实施例将用于表示传感器坐标系下重力加速度向量到世界坐标系下重力加速度向量的旋转，其中，g为重力在世界坐标系下的表示，从而观测变量也属于SO(3)李群，加速度测量方程为：
其中，v1包括ak，v2包括g，ak为加速度计第k次输出的加速度数据，g为世界坐标系下重力加速度向量，为加速度计观测变量，用于表示向量ak到向量g的旋转，换句话说，用于表示使向量ak与向量g平行并同向的旋转。加速度计测量噪声表示的是δk是均值为0，方差为的高斯噪声，为预设加速度计测量噪声协方差。
对于观测得到的磁场计输出本发明实施例将用于表示传感器坐标系下地磁指北向量到世界坐标系下地磁指北向量的旋转，其中，r为世界坐标系下指北方向，类似加速度测量方程，磁场计测量方程为：
v1包括bk，bk为磁场计第k次输出的磁场强度数据，v2包括r，r为世界坐标系下地磁指北向量，为磁场计观测变量，用于表示向量bk到向量r的旋转，换句话说，用于表示使向量bk与向量r平行并同向的旋转。磁场计测量噪声表示的是εk是均值为0，方差为的高斯噪声，为预设磁场计测量噪声协方差。
在对载体的姿态数据测量的融合算法中，在对时间更新(Propagation)时，也即，对载体的状态进行更新时，陀螺仪输出角速度数据，偏差角，欧拉角，k表示第几次。根据第k-1次姿态数据和陀螺仪第k次输出的角速度数据获得第k次姿态数据的预估数据，采用下述公式进行计算：
其中，μk|k-1为第k次姿态数据的预估数据，μk-1|k-1为第k-1次姿态数据，expG()为矩阵的指数运算， ωk为陀螺仪第k次输出的角速度数据，时间差Δtk＝tk-tk-1，tk和tk-1分别为第k次与第k-1次的时间，为 为三维实数空间，
∈＝(∈1,∈2,∈3)T
扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差为Pk-1|k-1，均值为μk-1|k-1，Rk-1是扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差。
根据加速度计第k次输出的加速度数据对预估数据进行校正，得到预估数据的第一次校正数据包括：
其中， 为预设加速度计测量噪声协方差，第一次校正数据包括：第一次校正后的姿态数据ak为加速度计第k次输出的加速度数据，
logG()为矩阵的对数运算，
为李代数的映射。
根据磁场计第k次输出的磁场强度数据对第一次校正数据进行校正，得到第k次姿态数据包括：通过如下公式获得第k次姿态数据μk|k，
其中， 为预设磁场计测量噪声协方差，bk为磁场计第k次输出的磁场强度数据，为第扩展卡尔曼滤波器的预测模型中第k次的状态变量Xk的协方差为Pk|k，均值为μk|k。
在对传感器进行数据融合后，得到的传感器载体姿态的估计值μk|k，即为tk时刻对载体的姿态数据，提高了对载体的姿态数据进行确定的准确性。由于本发明实施例中使用旋转矩阵作为EKF的状态变量和观测变量，旋转矩阵属于SO(3)，自由度为3，因此状态变量和观测变量协方差均满秩。并且，使用旋转矩阵描述姿态，在李群上连续，不存在万向节和死锁问题。
实施例3
本发明实施例的应用环境可以但不限于参照上述实施例中的应用环境，本实施例中对此不再赘述。本发明实施例提供了用于实施上述载体的姿态数据的确定方法的一种可选的具体应用。
本发明实施例公开了一种基于李群的惯性传感器数据融合算法，旨在更加准确地估计惯性传感器载体的姿态。其中惯性传感器包括三轴加速度计、三轴陀螺仪和三轴磁场计，传感器载体的姿态即其在某一时刻的翻滚(Roll)、偏航(Yaw)和俯仰(Pitch)，如图5。其中，图5是根据本发明实施例的一种载体的姿态的示意图。姿态也可以通过载体坐标系到世界坐标系的旋转矩阵R表示，R属于旋转变换SO(3)群，也是一个李群。本发明中使用离散扩展卡尔曼滤波(Discrete Extended Kalman Filter，D-EKF)对传感器信号滤波融合。在D-EKF的设计中，状态变量和观测变量都是李群，并假设状态变量的后验分布在李群上是一个高斯分布。在本发明实施例中的状态时间更新过程和测量更新过程都有清晰的解析公式描述，便于实施。对比实验结果分析发现，本发明比现有使用欧拉角或四元素作为状态变量的数据融合方法，得到的姿态更为准确。
在该实施例中，惯性传感器是计算和测量加速度、倾斜、冲击、振动、旋转和多自由度(DoF)运动的传感器。惯性传感器是解决导航、定向和运动载体控制的重要部件。该实施例的惯性传感器包括加速度计(或加速度传感计)和角速度数据传感器(陀螺)以及它们的单、双、三轴组合IMU(惯性测量单元)，AHRS(包括磁传感器的姿态参考系统)。
该实施例的MEMS加速度计是利用传感质量的惯性力测量的传感器，通常由标准质量块(传感元件)和计算电路组成。
该实施例的IMU主要由三个MEMS加速度传感器及三个陀螺和解算电路组成。
该实施例的陀螺仪是用高速回转体的动量矩敏感壳体相对惯性空间绕正交于自转轴的一个或二个轴的角运动计算装置。
可选地，该实施例的陀螺仪包括：压电陀螺仪，微机械陀螺仪，光纤陀螺仪和激光陀螺仪，它们都是电子式的，并且它们可以和加速度计、磁阻芯片、定位器等做成惯性导航控制系统。
可选地，该实施例的惯性传感器分为两大类：一类是角速率陀螺，另一类是线加速度计。
可选地，角速率陀螺又分为：机械式干式、液浮、半液浮、气浮角速率陀螺；挠性角速率陀螺；MEMS硅、石英角速率陀螺(含半球谐振角速率陀螺等)；光纤角速率陀螺；激光角速率陀螺等。
该实施例的惯性传感器可以改善其系统性能或功能的应用还包括:平台稳定、工业机械运动控制、安全/监控设备和工业车辆导航等，不仅能改善性能，而且能提高可靠性、安全性并降低成本。
需要说明的是，本发明实施例包括但不限于融合加速度计、陀螺仪和磁场计的方案，同样可以支持两个传感器的融合，即融合加速度计和陀螺仪，或者融合磁场计和陀螺仪。采用类似的状态变量和观测变量，以及更新公式的两个传感器融合方法，属于本发明的内容。
本发明实施中包括但不限于使用IMU为MEMS元件，对于使用单独输出加速度、角速度数据和磁场信息的其他元件，进行类似融合滤波的方法，属于本发明的内容。
本发明实施例的状态变量和观测变量都属于李群，其他使用IMU，并采用类似状态变量和观测变量的设计，进行融合滤波获取姿态信息的方法属于本发明的内容。
需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本发明所必须的。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到根据上述实施例的方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，或者网络设备等)执行本发明各个实施例的方法。
实施例4
根据本发明实施例，还提供了一种用于实施上述载体的姿态数据的确定方法的载体的姿态数据的确定装置。图6是根据本发明实施例的一种载体的姿态数据的确定装置的示意图。如图6所示，该载体的姿态数据的确定装置可以包括：获取单元10和融合单元20。
获取单元10，用于获取载体的惯性传感器输出的传感器数据。
融合单元20，用于使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据，其中，扩展卡尔曼滤波器的状态变量和观测变量都是由旋转变换矩阵表示。
可选地，旋转变换矩阵属于李群；扩展卡尔曼滤波器的状态变量为传感器坐标系到世界坐标系的旋转变换矩阵；扩展卡尔曼滤波器的观测变量包括扩展卡尔曼滤波器的加速度计观测变量和扩展卡尔曼滤波器的磁场计观测变量。
可选地，展卡尔曼滤波器的加速度计观测变量用于表示传感器坐标系下重力加速度向量到世界坐标系下重力加速度向量的旋转；扩展卡尔曼滤波器的磁场计观测变量用于表示传感器坐标系下地磁指北向量到世界坐标系下地磁指北向量的旋转。
可选地，惯性传感器包括陀螺仪，其中，在扩展卡尔曼滤波器的预测模型为匀角速度模型的情况下，对于陀螺仪第k次输出的角速度数据，预测模型用于：根据扩展卡尔曼滤波器第k-1次的状态变量、陀螺仪第k次输出的角速度数据和时间差获得该扩展卡尔曼滤波器第k次的状态变量，时间差为陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔。
可选地，在扩展卡尔曼滤波器的预测模型为匀角速度模型的情况下，对于陀螺仪第k次输出的角速度数据，预测模型通过以下公式表示： 其中，expG()为矩阵的指数运算， 为 为三维实数空间，ωk为陀螺仪第k次输出的角速度数据，Xk为第k次的状态变量，Xk-1为第k-1次的状态变量，时间差Δtk＝tk-tk-1，tk和tk-1分别为陀螺仪第k次输出角速度数据的时间与陀螺仪第k-1次输出角速度数据的时间；过程噪声表示的是nk-1是均值为0，方差为Rk-1的高斯噪声，Rk-1是预测模型的第k-1次的预设过程噪声协方差。
可选地，惯性传感器包括加速度计，扩展卡尔曼滤波器的观测变量包括扩展卡尔曼滤波器的加速度计观测变量，其中，在加速度计观测变量用于表示传感器坐标系下重力加速度向量到世界坐标系下重力加速度向量的旋转的情况下，传感器坐标系下重力加速度向量包括加速度计第k次输出的加速度数据，加速度计观测变量与扩展卡尔曼滤波器第k次的状态变量通过加速度计测量噪声建立关联关系，加速度计测量噪声为均值为0，方差为第一方差值的高斯白噪声。
在扩展卡尔曼滤波器的加速度计观测变量用于表示传感器坐标系下重力加速度向量到世界坐标系下重力加速度向量的旋转的情况下，对于加速度计第k次输出的加速度数据，加速度计观测变量对应的测量方程包括： 其中，定义为使向量v1与向量v2平行并同向的旋转，v1包括ak，v2包括g，ak为加速度计第k次输出的加速度数据，g为世界坐标系下重力加速度向量，为加速度计观测变量，加速度计测量噪声表示的是δk是均值为0，方差为的高斯噪声，为预设加速度计测量噪声协方差。
可选地，惯性传感器包括磁场计，扩展卡尔曼滤波器的观测变量包括扩展卡尔曼滤波器的磁场计观测变量，其中，在扩展卡尔曼滤波器的磁场计观测变量用于表示为传感器坐标系下地磁指北向量到世界坐标系下地磁指北向量的旋转的情况下，传感器坐标系下地磁指北向量包括磁场计第k次输出的磁场强度数据，磁场计观测变量与扩展卡尔曼滤波器第k次的状态变量通过磁场计测量噪声建立关联关系，磁场计测量噪声为均值为0，方差为第二方差值的高斯白噪声。
在扩展卡尔曼滤波器的磁场计观测变量用于表示传感器坐标系下地磁指北向量到世界坐标系下地磁指北向量的旋转的情况下，对于磁场计第k次输出的磁场强度数据，磁场计观测变量对应的测量方程包括：其中，v1包括bk，v2包括r，bk为磁场计第k次输出的磁场强度数据，r为世界坐标系下地磁指北向量，为磁场计观测变量，磁场计测量噪声表示的是εk是均值为0，方差为的高斯噪声，为预设磁场计测量噪声协方差。
可选地，获取单元10用于执行以下步骤来实现获取载体的惯性传感器输出的传感器数据：获取载体的惯性传感器第k次输出的第k次传感器数据，其中，k为大于1的自然数，其中，k为大于1的自然数。
可选地，传感器数据包括陀螺仪输出的角速度数据，扩展卡尔曼滤波器的加速度计输出的加速度数据，和/或扩展卡尔曼滤波器的磁场计输出的磁场强度数据。融合单元20包括第一获取模块和校正模块。其中，第一获取模块用于根据载体的第k-1次姿态数据和陀螺仪第k次输出的角速度数据获得第k次姿态数据的预估数据，其中，第k-1次姿态数据是由扩展卡尔曼滤波器对惯性传感器输出的第k-1次传感器数据进行融合得到的姿态数据，第k次传感器数据包括陀螺仪第k次输出的角速度数据；校正模块用于根据加速度计第k次输出的加速度数据和/或磁场计第k次输出的磁场强度数据对预估数据进行校正，得到载体的第k次姿态数据。
可选地，第一获取模块包括：第一获取子模块和第二获取子模块。其中，第一获取子模块，用于根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据，其中，时间差为陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔；第二获取子模块，用于根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
可选地，校正模块包括：第三获取子模块和第四获取子模块。其中，第三获取子模块，用于根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第k次姿态数据；第四获取子模块，用于根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
可选地，校正模块包括：第一校正子模块，用于根据磁场计第k次输出的磁场强度数据对预估数据进行校正，得到第k次姿态数据。
可选地，第一获取模块包括：第五获取子模块和第六获取子模块。其中，第五获取子模块，用于根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据，其中，时间差为陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔；第六获取子模块，用于根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
可选地，校正模块包括：第七获取子模块和第八获取子模块。其中，第七获取子模块，用于根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得第k次姿态数据；第八获取子模块，用于根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
可选地，校正模块包括：第二校正子模块和第三校正子模块。其中，第二校正子模块，用于根据加速度计第k次输出的加速度数据对预估数据进行校正，得到预估数据的第一次校正数据；第三校正子模块，用于根据磁场计第k次输出的磁场强度数据对第一次校正数据进行校正，得到第k次姿态数据。
可选地，第一获取模块包括：第九获取子模块和第十获取子模块。其中，第九获取子模块，用于根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据，其中，时间差为陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔；第十获取子模块，用于根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
可选地，第二校正子模块用于根据加速度计第k次输出的加速度数据对预估数据进行校正：根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第一次校正后的姿态数据；根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第二中间变量；第三校正子模块用于执行以下步骤来实现根据磁场计第k次输出的磁场强度数据对第一次校正数据进行校正：根据第二中间变量、第一次校正后的姿态数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得第k次姿态数据；根据第二中间变量、第一次校正后的姿态数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
可选地，该实施例的载体的姿态数据的确定装置中的李群为SO(3)李群。
可选地，该实施例的载体的姿态数据的确定装置还包括：发送单元，用于在使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据之后，将载体的姿态数据发送给载体的控制器。
需要说明的是，该实施例中的获取单元10可以用于执行本申请实施例1中的步骤S202，该实施例中的融合单元20可以用于执行本申请实施例1中的步骤S204。
该实施例通过获取单元10获取载体的惯性传感器输出的传感器数据，通过融合单元20使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据，提高了对载体的姿态数据进行确定的准确性，其中，扩展卡尔曼滤波器的状态变量和观测变量都是由旋转变换矩阵表示进而解决了相关技术中对载体的姿态数据的确定不准确的技术问题。
此处需要说明的是，上述单元和模块与对应的步骤所实现的示例和应用场景相同，但不限于上述实施例1所公开的内容。需要说明的是，上述模块作为装置的一部分可以运行在如图1所示的硬件环境中，可以通过软件实现，也可以通过硬件实现，其中，硬件环境包括网络环境。
实施例5
根据本发明实施例，还提供了一种用于实施上述载体的姿态数据的确定方法的服务器或终端。
图7是根据本发明实施例的一种终端的结构框图。如图7所示，该终端可以包括：一个或多个(图中仅示出一个)处理器701、存储器703、以及传输装置705，如图7所示，该终端还可以包括输入输出设备707。
其中，存储器703可用于存储软件程序以及模块，如本发明实施例中的载体的姿态数据的确定方法和装置对应的程序指令/模块，处理器701通过运行存储在存储器703内的软件程序以及模块，从而执行各种功能应用以及数据处理，即实现上述的载体的姿态数据的确定方法。存储器703可包括高速随机存储器，还可以包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器703可进一步包括相对于处理器701远程设置的存储器，这些远程存储器可以通过网络连接至终端。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
上述的传输装置705用于经由一个网络接收或者发送数据，还可以用于处理器与存储器之间的数据传输。上述的网络具体实例可包括有线网络及无线网络。在一个实例中，传输装置705包括一个网络适配器(Network Interface Controller，NIC)，其可通过网线与其他网络设备与路由器相连从而可与互联网或局域网进行通讯。在一个实例中，传输装置705为射频(Radio Frequency，RF)模块，其用于通过无线方式与互联网进行通讯。
其中，具体地，存储器703用于存储应用程序。
处理器701可以通过传输装置705调用存储器703存储的应用程序，以执行下述步骤：
获取载体的惯性传感器输出的传感器数据；
使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据，其中，扩展卡尔曼滤波器的状态变量和观测变量都是由旋转变换矩阵表示。
处理器701还用于执行下述步骤：根据载体的第k-1次姿态数据和陀螺仪第k次输出的角速度数据获得载体的第k次姿态数据的预估数据，其中，第k-1次姿态数据是由扩展卡尔曼滤波器对惯性传感器输出的第k-1次传感器数据进行融合得到的载体的姿态数据；根据加速度计第k次输出的加速度数据和/或磁场计第k次输出的磁场强度数据对预估数据进行校正，得到载体的第k次姿态数据。
处理器701还用于执行下述步骤：根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据，其中，时间差为陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔；根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
处理器701还用于执行下述步骤：根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第k次姿态数据；根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
处理器701还用于执行下述步骤：根据磁场计第k次输出的磁场强度数据对预估数据进行校正，得到第k次姿态数据。
处理器701还用于执行下述步骤：根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据，其中，时间差为陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔；根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
处理器701还用于执行下述步骤：根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得第k次姿态数据；根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
处理器701还用于执行下述步骤：根据加速度计第k次输出的加速度数据对预估数据进行校正，得到预估数据的第一次校正数据；根据磁场计第k次输出的磁场强度数据对第一次校正数据进行校正，得到第k次姿态数据。
处理器701还用于执行下述步骤：根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据，其中，时间差为陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔；根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
处理器701还用于执行下述步骤：根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第一次校正后的姿态数据；根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第二中间变量；根据磁场计第k次输出的磁场强度数据对第一次校正数据进行校正，得到第k次姿态数据包括：根据第二中间变量、第一次校正后的姿态数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得第k次姿态数据；根据第二中间变量、第一次校正后的姿态数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
处理器701还用于执行下述步骤：将载体的姿态数据发送给载体的控制器。
采用本发明实施例，提供了一种载体的姿态数据的确定方法。获取载体的惯性传感器输出的传感器数据；使用在李群上的扩展卡尔曼滤波器对传感器数据进行融合，得到载体的姿态数据，扩展卡尔曼滤波器的状态变量和观测变量都是由旋转变换矩阵表示，提高了对载体的姿态数据进行确定的准确性，进而解决了相关技术中对载体的姿态数据的确定不准确的技术问题。
可选地，本实施例中的具体示例可以参考上述实施例1中所描述的示例，本实施例在此不再赘述。
本领域普通技术人员可以理解，图7所示的结构仅为示意，终端可以是智能手机(如Android手机、iOS手机等)、平板电脑、掌上电脑以及移动互联网设备(Mobile InternetDevices，MID)、PAD等终端设备。图7其并不对上述电子装置的结构造成限定。例如，终端还可包括比图7中所示更多或者更少的组件(如网络接口、显示装置等)，或者具有与图7所示不同的配置。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令终端设备相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：闪存盘、只读存储器(Read-Only Memory，ROM)、随机存取器(RandomAccess Memory，RAM)、磁盘或光盘等。
实施例6
本发明的实施例还提供了一种存储介质。可选地，在本实施例中，上述存储介质可以用于执行载体的姿态数据的确定方法的程序代码。
可选地，在本实施例中，上述存储介质可以位于上述实施例所示的网络中的多个网络设备中的至少一个网络设备上。
可选地，在本实施例中，存储介质被设置为存储用于执行以下步骤的程序代码：
获取载体的惯性传感器输出的传感器数据；
使用在李群上的扩展卡尔曼滤波器对所述传感器数据进行融合，得到所述载体的姿态数据，其中，扩展卡尔曼滤波器的状态变量和观测变量都是由旋转变换矩阵表示。
可选地，存储介质还被设置为存储用于执行以下步骤的程序代码：根据载体的第k-1次姿态数据和陀螺仪第k次输出的角速度数据获得载体的第k次姿态数据的预估数据，其中，第k-1次姿态数据是由扩展卡尔曼滤波器对惯性传感器输出的第k-1次传感器数据进行融合得到的载体的姿态数据；根据加速度计第k次输出的加速度数据和/或磁场计第k次输出的磁场强度数据对预估数据进行校正，得到载体的第k次姿态数据。
可选地，存储介质还被设置为存储用于执行以下步骤的程序代码：根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据，其中，时间差为陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔；根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
可选地，存储介质还被设置为存储用于执行以下步骤的程序代码：根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第k次姿态数据；根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
可选地，存储介质还被设置为存储用于执行以下步骤的程序代码：根据磁场计第k次输出的磁场强度数据对预估数据进行校正，得到第k次姿态数据。
可选地，存储介质还被设置为存储用于执行以下步骤的程序代码：根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据，其中，时间差为陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔；根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
可选地，存储介质还被设置为存储用于执行以下步骤的程序代码：根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得第k次姿态数据；根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
可选地，存储介质还被设置为存储用于执行以下步骤的程序代码：根据加速度计第k次输出的加速度数据对预估数据进行校正，得到预估数据的第一次校正数据；根据磁场计第k次输出的磁场强度数据对第一次校正数据进行校正，得到第k次姿态数据。
可选地，存储介质还被设置为存储用于执行以下步骤的程序代码：根据第k-1次姿态数据、陀螺仪第k次输出的角速度数据和时间差获得第k次姿态数据的预估数据，其中，时间差为陀螺仪第k次输出角速度数据与陀螺仪第k-1次输出角速度数据之间的时间间隔；根据陀螺仪第k次输出的角速度数据、时间差、扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
可选地，存储介质还被设置为存储用于执行以下步骤的程序代码：根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第一次校正后的姿态数据；根据第一中间变量、第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第二中间变量；根据磁场计第k次输出的磁场强度数据对第一次校正数据进行校正，得到第k次姿态数据包括：根据第二中间变量、第一次校正后的姿态数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得第k次姿态数据；根据第二中间变量、第一次校正后的姿态数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差
可选地，存储介质还被设置为存储用于执行以下步骤的程序代码：将载体的姿态数据发送给载体的控制器。
可选地，本实施例中的具体示例可以参考上述实施例1中所描述的示例，本实施例在此不再赘述。
可选地，在本实施例中，上述存储介质可以包括但不限于：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
上述实施例中的集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在上述计算机可读取的存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在存储介质中，包括若干指令用以使得一台或多台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。
在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
在本申请所提供的几个实施例中，应该理解到，所揭露的客户端，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
Claims (36)
1.一种载体的姿态数据的确定方法，其特征在于，包括：
获取载体的惯性传感器输出的传感器数据；
使用在李群上的扩展卡尔曼滤波器对所述传感器数据进行融合，得到所述载体的姿态数据，其中，所述扩展卡尔曼滤波器的状态变量和观测变量都是由旋转变换矩阵表示。
2.根据权利要求1所述的方法，其特征在于，所述旋转变换矩阵属于李群；所述扩展卡尔曼滤波器的状态变量为传感器坐标系到世界坐标系的旋转变换矩阵；所述扩展卡尔曼滤波器的观测变量包括所述扩展卡尔曼滤波器的加速度计观测变量和所述扩展卡尔曼滤波器的磁场计观测变量。
3.根据权利要求2所述方法，其特征在于，所述扩展卡尔曼滤波器的加速度计观测变量用于表示所述传感器坐标系下重力加速度向量到所述世界坐标系下重力加速度向量的旋转；所述扩展卡尔曼滤波器的磁场计观测变量用于表示所述传感器坐标系下地磁指北向量到所述世界坐标系下地磁指北向量的旋转。
4.根据权利要求1所述方法，其特征在于，所述惯性传感器包括陀螺仪，其中，在所述扩展卡尔曼滤波器的预测模型为匀角速度模型的情况下，对于所述陀螺仪第k次输出的角速度数据，所述预测模型用于：根据所述扩展卡尔曼滤波器第k-1次的状态变量、所述陀螺仪第k次输出的角速度数据和时间差获得该扩展卡尔曼滤波器第k次的状态变量，所述时间差为所述陀螺仪第k次输出角速度数据与所述陀螺仪第k-1次输出角速度数据之间的时间间隔。
5.根据权利要求1所述方法，其特征在于，所述惯性传感器包括加速度计，所述扩展卡尔曼滤波器的观测变量包括所述扩展卡尔曼滤波器的加速度计观测变量，其中，在所述加速度计观测变量用于表示传感器坐标系下重力加速度向量到世界坐标系下重力加速度向量的旋转的情况下，所述传感器坐标系下重力加速度向量包括所述加速度计第k次输出的加速度数据，所述加速度计观测变量与所述扩展卡尔曼滤波器第k次的状态变量通过加速度计测量噪声建立关联关系，所述加速度计测量噪声为均值为0，方差为第一方差值的高斯白噪声。
6.根据权利要求1所述方法，其特征在于，所述惯性传感器包括磁场计，所述扩展卡尔曼滤波器的观测变量包括所述扩展卡尔曼滤波器的磁场计观测变量，其中，在所述扩展卡尔曼滤波器的磁场计观测变量用于表示为传感器坐标系下地磁指北向量到世界坐标系下地磁指北向量的旋转的情况下，所述传感器坐标系下地磁指北向量包括所述磁场计第k次输出的磁场强度数据，所述磁场计观测变量与所述扩展卡尔曼滤波器第k次的状态变量通过磁场计测量噪声建立关联关系，所述磁场计测量噪声为均值为0，方差为第二方差值的高斯白噪声。
7.根据权利要求1所述的方法，其特征在于，所述获取载体的惯性传感器输出的传感器数据包括：
获取所述载体的惯性传感器第k次输出的第k次传感器数据，其中，k为大于1的自然数。
8.根据权利要求7所述的方法，其特征在于，所述传感器数据包括陀螺仪输出的角速度数据，所述扩展卡尔曼滤波器的加速度计输出的加速度数据，和/或所述扩展卡尔曼滤波器的磁场计输出的磁场强度数据；其中，所述使用在李群上的扩展卡尔曼滤波器对所述传感器数据进行融合，得到所述载体的姿态数据包括：
根据所述载体的第k-1次姿态数据和所述陀螺仪第k次输出的角速度数据获得所述载体的第k次姿态数据的预估数据，其中，所述第k-1次姿态数据是由所述扩展卡尔曼滤波器对所述惯性传感器输出的第k-1次传感器数据进行融合得到的所述载体的姿态数据；根据所述加速度计第k次输出的加速度数据和/或所述磁场计第k次输出的磁场强度数据对所述预估数据进行校正，得到所述载体的第k次姿态数据。
9.根据权利要求8所述的方法，其特征在于，根据所述载体的第k-1次姿态数据和所述陀螺仪第k次输出的角速度数据获得所述载体的第k次姿态数据的预估数据包括：
根据所述第k-1次姿态数据、所述陀螺仪第k次输出的角速度数据和时间差获得所述第k次姿态数据的预估数据，其中，所述时间差为所述陀螺仪第k次输出角速度数据与所述陀螺仪第k-1次输出角速度数据之间的时间间隔；
根据所述陀螺仪第k次输出的角速度数据、所述时间差、所述扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和所述扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
10.根据权利要求9所述的方法，其特征在于，根据所述加速度计第k次输出的加速度数据对所述预估数据进行校正，得到所述载体的第k次姿态数据包括：
根据所述第一中间变量、所述第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得所述第k次姿态数据；
根据所述第一中间变量、所述第k次姿态数据的预估数据、所述世界坐标系下重力加速度向量和所述预设加速度计测量噪声协方差获得所述扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
11.根据权利要求8所述的方法，其特征在于，根据所述加速度计第k次输出的加速度数据和/或所述磁场计第k次输出的磁场强度数据对所述预估数据进行校正，得到所述载体的第k次姿态数据包括：
根据所述磁场计第k次输出的磁场强度数据对所述预估数据进行校正，得到所述第k次姿态数据。
12.根据权利要求11所述的方法，其特征在于，根据所述载体的第k-1次姿态数据和所述陀螺仪第k次输出的角速度数据获得所述载体的第k次姿态数据的预估数据包括：
根据所述第k-1次姿态数据、所述陀螺仪第k次输出的角速度数据和时间差获得所述第k次姿态数据的预估数据，其中，所述时间差为所述陀螺仪第k次输出角速度数据与所述陀螺仪第k-1次输出角速度数据之间的时间间隔；
根据所述陀螺仪第k次输出的角速度数据、所述时间差、所述扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和所述扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
13.根据权利要求12所述的方法，其特征在于，根据所述磁场计第k次输出的磁场强度数据对所述预估数据进行校正，得到所述载体的第k次姿态数据包括：
根据所述第一中间变量、所述第k次姿态数据的预估数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得所述第k次姿态数据；
根据所述第一中间变量、所述第k次姿态数据的预估数据、所述世界坐标系下地磁指北向量和所述预设磁场计测量噪声协方差获得所述扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
14.根据权利要求8所述的方法，其特征在于，根据所述加速度计第k次输出的加速度数据和/或所述磁场计第k次输出的磁场强度数据对所述预估数据进行校正，得到所述载体的第k次姿态数据包括：
根据所述加速度计第k次输出的加速度数据对所述预估数据进行校正，得到所述预估数据的第一次校正数据；
根据所述磁场计第k次输出的磁场强度数据对所述第一次校正数据进行校正，得到所述第k次姿态数据。
15.根据权利要求14所述的方法，其特征在于，根据所述载体的第k-1次姿态数据和所述陀螺仪第k次输出的角速度数据获得所述载体的第k次姿态数据的预估数据包括：
根据所述第k-1次姿态数据、所述陀螺仪第k次输出的角速度数据和时间差获得所述第k次姿态数据的预估数据，其中，所述时间差为所述陀螺仪第k次输出角速度数据与所述陀螺仪第k-1次输出角速度数据之间的时间间隔；
根据所述陀螺仪第k次输出的角速度数据、所述时间差、所述扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和所述扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
16.根据权利要求15所述的方法，其特征在于，
根据所述加速度计第k次输出的加速度数据对所述预估数据进行校正，得到所述预估数据的第一次校正数据包括：根据所述第一中间变量、所述第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第一次校正后的姿态数据；根据所述第一中间变量、所述第k次姿态数据的预估数据、所述世界坐标系下重力加速度向量和所述预设加速度计测量噪声协方差获得第二中间变量；
根据所述磁场计第k次输出的磁场强度数据对所述第一次校正数据进行校正，得到所述第k次姿态数据包括：根据所述第二中间变量、所述第一次校正后的姿态数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得所述第k次姿态数据；根据所述第二中间变量、所述第一次校正后的姿态数据、所述世界坐标系下地磁指北向量和所述预设磁场计测量噪声协方差获得所述扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
17.根据权利要求1至16中任一项所述的方法，其特征在于，所述李群为SO(3)李群。
18.根据权利要求1至16中任一项所述的方法，其特征在于，在使用在李群上的扩展卡尔曼滤波器对所述传感器数据进行融合，得到所述载体的姿态数据之后，还包括：
将所述载体的姿态数据发送给所述载体的控制器。
19.一种载体的姿态数据的确定装置，其特征在于，包括：
获取单元，用于获取载体的惯性传感器输出的传感器数据；
融合单元，用于使用在李群上的扩展卡尔曼滤波器对所述传感器数据进行融合，得到所述载体的姿态数据，其中，所述扩展卡尔曼滤波器的状态变量和观测变量都是由旋转变换矩阵表示。
20.根据权利要求19所述的装置，其特征在于，所述旋转变换矩阵属于李群；所述扩展卡尔曼滤波器的状态变量为传感器坐标系到世界坐标系的旋转变换矩阵；所述扩展卡尔曼滤波器的观测变量包括所述扩展卡尔曼滤波器的加速度计观测变量和所述扩展卡尔曼滤波器的磁场计观测变量。
21.根据权利要求20所述装置，其特征在于，所述扩展卡尔曼滤波器的加速度计观测变量用于表示所述传感器坐标系下重力加速度向量到所述世界坐标系下重力加速度向量的旋转；所述扩展卡尔曼滤波器的磁场计观测变量用于表示所述传感器坐标系下地磁指北向量到所述世界坐标系下地磁指北向量的旋转。
22.根据权利要求19所述装置，其特征在于，所述惯性传感器包括陀螺仪，其中，在所述扩展卡尔曼滤波器的预测模型为匀角速度模型的情况下，对于所述陀螺仪第k次输出的角速度数据，所述预测模型用于：根据所述扩展卡尔曼滤波器第k-1次的状态变量、所述陀螺仪第k次输出的角速度数据和时间差获得该扩展卡尔曼滤波器第k次的状态变量，所述时间差为所述陀螺仪第k次输出角速度数据与所述陀螺仪第k-1次输出角速度数据之间的时间间隔。
23.根据权利要求19所述装置，其特征在于，所述惯性传感器包括加速度计，所述扩展卡尔曼滤波器的观测变量包括所述扩展卡尔曼滤波器的加速度计观测变量，其中，在所述加速度计观测变量用于表示传感器坐标系下重力加速度向量到世界坐标系下重力加速度向量的旋转的情况下，所述传感器坐标系下重力加速度向量包括所述加速度计第k次输出的加速度数据，所述加速度计观测变量与所述扩展卡尔曼滤波器第k次的状态变量通过加速度计测量噪声建立关联关系，所述加速度计测量噪声为均值为0，方差为第一方差值的高斯白噪声。
24.根据权利要求19所述装置，其特征在于，所述惯性传感器包括磁场计，所述扩展卡尔曼滤波器的观测变量包括所述扩展卡尔曼滤波器的磁场计观测变量，其中，在所述扩展卡尔曼滤波器的磁场计观测变量用于表示为传感器坐标系下地磁指北向量到世界坐标系下地磁指北向量的旋转的情况下，所述传感器坐标系下地磁指北向量包括所述磁场计第k次输出的磁场强度数据，所述磁场计观测变量与所述扩展卡尔曼滤波器第k次的状态变量通过磁场计测量噪声建立关联关系，所述磁场计测量噪声为均值为0，方差为第二方差值的高斯白噪声。
25.根据权利要求19所述的装置，其特征在于，所述获取单元用于执行以下步骤来实现获取所述载体的惯性传感器输出的传感器数据：获取所述载体的惯性传感器第k次输出的第k次传感器数据，其中，k为大于1的自然数。
26.根据权利要求25所述的装置，其特征在于，所述传感器数据包括陀螺仪输出的角速度数据，所述扩展卡尔曼滤波器的加速度计输出的加速度数据，和/或所述扩展卡尔曼滤波器的磁场计输出的磁场强度数据；其中，所述融合单元包括：
第一获取模块，用于根据所述载体的第k-1次姿态数据和所述陀螺仪第k次输出的角速度数据获得所述载体的第k次姿态数据的预估数据，其中，所述第k-1次姿态数据是由所述扩展卡尔曼滤波器对所述惯性传感器输出的第k-1次传感器数据进行融合得到的所述载体的姿态数据；
校正模块，用于根据所述加速度计第k次输出的加速度数据和/或所述磁场计第k次输出的磁场强度数据对所述预估数据进行校正，得到所述载体的第k次姿态数据。
27.根据权利要求26所述的装置，其特征在于，所述第一获取模块包括：
第一获取子模块，用于根据所述第k-1次姿态数据、所述陀螺仪第k次输出的角速度数据和时间差获得所述第k次姿态数据的预估数据，其中，所述时间差为所述陀螺仪第k次输出角速度数据与所述陀螺仪第k-1次输出角速度数据之间的时间间隔；
第二获取子模块，用于根据所述陀螺仪第k次输出的角速度数据、所述时间差、所述扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和所述扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
28.根据权利要求27所述的装置，其特征在于，所述校正模块包括：
第三获取子模块，用于根据所述第一中间变量、所述第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得所述第k次姿态数据；
第四获取子模块，用于根据所述第一中间变量、所述第k次姿态数据的预估数据、所述世界坐标系下重力加速度向量和所述预设加速度计测量噪声协方差获得所述扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
29.根据权利要求26所述的装置，其特征在于，所述校正模块包括：第一校正子模块，用于根据所述磁场计第k次输出的磁场强度数据对所述预估数据进行校正，得到所述第k次姿态数据。
30.根据权利要求29所述的装置，其特征在于，所述第一获取模块包括：
第五获取子模块，用于根据所述第k-1次姿态数据、所述陀螺仪第k次输出的角速度数据和时间差获得所述第k次姿态数据的预估数据，其中，所述时间差为所述陀螺仪第k次输出角速度数据与所述陀螺仪第k-1次输出角速度数据之间的时间间隔；
第六获取子模块，用于根据所述陀螺仪第k次输出的角速度数据、所述时间差、所述扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和所述扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
31.根据权利要求30所述的装置，其特征在于，所述校正模块包括：
第七获取子模块，用于根据所述第一中间变量、所述第k次姿态数据的预估数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得所述第k次姿态数据；
第八获取子模块，用于根据所述第一中间变量、所述第k次姿态数据的预估数据、所述世界坐标系下地磁指北向量和所述预设磁场计测量噪声协方差获得所述扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
32.根据权利要求26所述的装置，其特征在于，所述校正模块包括：
第二校正子模块，用于根据所述加速度计第k次输出的加速度数据对所述预估数据进行校正，得到所述预估数据的第一次校正数据；
第三校正子模块，用于根据所述磁场计第k次输出的磁场强度数据对所述第一次校正数据进行校正，得到所述第k次姿态数据。
33.根据权利要求32所述的装置，其特征在于，所述第一获取模块包括：
第九获取子模块，用于根据所述第k-1次姿态数据、所述陀螺仪第k次输出的角速度数据和时间差获得所述第k次姿态数据的预估数据，其中，所述时间差为所述陀螺仪第k次输出角速度数据与所述陀螺仪第k-1次输出角速度数据之间的时间间隔；
第十获取子模块，用于根据所述陀螺仪第k次输出的角速度数据、所述时间差、所述扩展卡尔曼滤波器的预测模型中第k-1次的状态变量Xk-1的协方差、和所述扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量。
34.根据权利要求33所述的装置，其特征在于，
所述第二校正子模块用于执行以下步骤来实现根据所述加速度计第k次输出的加速度数据对所述预估数据进行校正：根据所述第一中间变量、所述第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得第一次校正后的姿态数据；根据所述第一中间变量、所述第k次姿态数据的预估数据、所述世界坐标系下重力加速度向量和所述预设加速度计测量噪声协方差获得第二中间变量；
所述第三校正子模块用于执行以下步骤来实现根据所述磁场计第k次输出的磁场强度数据对所述第一次校正数据进行校正：根据所述第二中间变量、所述第一次校正后的姿态数据、世界坐标系下地磁指北向量和预设磁场计测量噪声协方差获得所述第k次姿态数据；根据所述第二中间变量、所述第一次校正后的姿态数据、所述世界坐标系下地磁指北向量和所述预设磁场计测量噪声协方差获得所述扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差。
35.根据权利要求19至34中任一项所述的装置，其特征在于，所述李群为SO(3)李群。
36.根据权利要求19至34中任一项所述的装置，其特征在于，所述装置还包括：
发送单元，用于在使用在李群上的扩展卡尔曼滤波器对所述传感器数据进行融合，得到所述载体的姿态数据之后，将所述载体的姿态数据发送给所述载体的控制器。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710039666.1A CN108318027B (zh) | 2017-01-18 | 2017-01-18 | 载体的姿态数据的确定方法和装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710039666.1A CN108318027B (zh) | 2017-01-18 | 2017-01-18 | 载体的姿态数据的确定方法和装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108318027A true CN108318027A (zh) | 2018-07-24 |
| CN108318027B CN108318027B (zh) | 2020-09-01 |
Family
ID=62892305
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710039666.1A Active CN108318027B (zh) | 2017-01-18 | 2017-01-18 | 载体的姿态数据的确定方法和装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108318027B (zh) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109109866A (zh) * | 2018-08-24 | 2019-01-01 | 深圳市国脉畅行科技股份有限公司 | 车辆行驶状态监测方法、装置、计算机设备及存储介质 |
| CN109358624A (zh) * | 2018-10-25 | 2019-02-19 | 湖南万为智能机器人技术有限公司 | 用于机器人的耦合定位方法 |
| CN110986940A (zh) * | 2019-11-05 | 2020-04-10 | 中国船舶重工集团公司第七一七研究所 | 一种捷联式半球谐振陀螺惯导 |
| CN111399023A (zh) * | 2020-04-20 | 2020-07-10 | 中国人民解放军国防科技大学 | 基于李群非线性状态误差的惯性基组合导航滤波方法 |
| WO2022033139A1 (zh) * | 2020-08-13 | 2022-02-17 | 华为技术有限公司 | 一种自运动估计方法及相关装置 |
| CN114384923A (zh) * | 2020-10-22 | 2022-04-22 | 福建江夏学院 | 一种x对称结构测量无人机姿态角的方法 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102982556A (zh) * | 2012-11-01 | 2013-03-20 | 江苏科技大学 | 基于流形上粒子滤波算法的视频目标跟踪方法 |
| CN104296745A (zh) * | 2014-09-29 | 2015-01-21 | 杭州电子科技大学 | 一种基于9-dof传感器组的姿态检测数据融合方法 |
| CN105761281A (zh) * | 2016-03-23 | 2016-07-13 | 沈阳大学 | 基于双边结构张量的粒子滤波目标跟踪算法及系统 |
| WO2016156602A1 (fr) * | 2015-04-01 | 2016-10-06 | Safran Electronics & Defense | Procédé de suivi de navigation d'un porteur mobile avec un filtre de kalman étendu |
-
2017
- 2017-01-18 CN CN201710039666.1A patent/CN108318027B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102982556A (zh) * | 2012-11-01 | 2013-03-20 | 江苏科技大学 | 基于流形上粒子滤波算法的视频目标跟踪方法 |
| CN104296745A (zh) * | 2014-09-29 | 2015-01-21 | 杭州电子科技大学 | 一种基于9-dof传感器组的姿态检测数据融合方法 |
| WO2016156602A1 (fr) * | 2015-04-01 | 2016-10-06 | Safran Electronics & Defense | Procédé de suivi de navigation d'un porteur mobile avec un filtre de kalman étendu |
| CN105761281A (zh) * | 2016-03-23 | 2016-07-13 | 沈阳大学 | 基于双边结构张量的粒子滤波目标跟踪算法及系统 |
Non-Patent Citations (1)
| Title |
|---|
| TAREK HAMEL: ""Attitude estimation on SO(3) based on direct inertial measurements"", 《ICRA 2006》 * |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109109866A (zh) * | 2018-08-24 | 2019-01-01 | 深圳市国脉畅行科技股份有限公司 | 车辆行驶状态监测方法、装置、计算机设备及存储介质 |
| CN109358624A (zh) * | 2018-10-25 | 2019-02-19 | 湖南万为智能机器人技术有限公司 | 用于机器人的耦合定位方法 |
| CN109358624B (zh) * | 2018-10-25 | 2021-12-10 | 长沙万为机器人有限公司 | 用于机器人的耦合定位方法 |
| CN110986940A (zh) * | 2019-11-05 | 2020-04-10 | 中国船舶重工集团公司第七一七研究所 | 一种捷联式半球谐振陀螺惯导 |
| CN111399023A (zh) * | 2020-04-20 | 2020-07-10 | 中国人民解放军国防科技大学 | 基于李群非线性状态误差的惯性基组合导航滤波方法 |
| WO2022033139A1 (zh) * | 2020-08-13 | 2022-02-17 | 华为技术有限公司 | 一种自运动估计方法及相关装置 |
| CN114384923A (zh) * | 2020-10-22 | 2022-04-22 | 福建江夏学院 | 一种x对称结构测量无人机姿态角的方法 |
| CN114384923B (zh) * | 2020-10-22 | 2023-05-23 | 福建江夏学院 | 一种x对称结构测量无人机姿态角的方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108318027B (zh) | 2020-09-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108318027A (zh) | 载体的姿态数据的确定方法和装置 | |
| CN103776451B (zh) | 一种基于mems的高精度三维姿态惯性测量系统以及测量方法 | |
| US10551939B2 (en) | Attitude detecting device | |
| CN107543546A (zh) | 一种六轴运动传感器的姿态解算方法及装置 | |
| US20150149111A1 (en) | Device and method for using time rate of change of sensor data to determine device rotation | |
| CN106153073B (zh) | 一种全姿态捷联惯导系统的非线性初始对准方法 | |
| CN106969783A (zh) | 一种基于光纤陀螺惯性导航的单轴旋转快速标定技术 | |
| US20140222369A1 (en) | Simplified method for estimating the orientation of an object, and attitude sensor implementing such a method | |
| CN108731664B (zh) | 机器人状态估计方法、装置、计算机设备和存储介质 | |
| CN108827313A (zh) | 基于扩展卡尔曼滤波器的多模式旋翼飞行器姿态估计方法 | |
| CN109682377A (zh) | 一种基于动态步长梯度下降的姿态估计方法 | |
| CN108369096A (zh) | 对线加速度和角加速度鲁棒的偏航率陀螺仪 | |
| Zhilenkov et al. | Based on MEMS sensors man-machine interface for mechatronic objects control | |
| CN109030867A (zh) | 使用加速度传感器和地磁传感器计算角速度的方法和设备 | |
| CN110352331A (zh) | 手持云台的姿态解算的方法和云台系统 | |
| JP2019078560A (ja) | ジャイロセンサのオフセット補正装置、オフセット補正プログラム、歩行者自律航法装置 | |
| CN108490972A (zh) | 飞行器的飞行控制方法、系统以及电子设备 | |
| Cirillo et al. | A comparison of multisensor attitude estimation algorithms | |
| Tran | Data Fusion with 9 degrees of freedom Inertial Measurement Unit to determine object’s orientation | |
| CN106708088A (zh) | 坐标计算方法及装置、飞行控制方法及系统、无人机 | |
| CN108592902B (zh) | 一种基于多传感器的定位设备及定位方法、系统和机械臂 | |
| CN106970705A (zh) | 动作捕捉方法、装置和电子设备 | |
| JP2013061309A (ja) | カルマンフィルタ、状態推定装置、カルマンフィルタの制御方法、及びカルマンフィルタの制御プログラム | |
| Blachuta et al. | Attitude and heading reference system based on 3D complementary filter | |
| CN105928544A (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 |