[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN109813308A - 姿态估计方法、装置及计算机可读存储介质 - Google Patents

姿态估计方法、装置及计算机可读存储介质 Download PDF

Info

Publication number
CN109813308A
CN109813308A CN201910173038.1A CN201910173038A CN109813308A CN 109813308 A CN109813308 A CN 109813308A CN 201910173038 A CN201910173038 A CN 201910173038A CN 109813308 A CN109813308 A CN 109813308A
Authority
CN
China
Prior art keywords
angular velocity
value
acceleration
attitude
error
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.)
Pending
Application number
CN201910173038.1A
Other languages
English (en)
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.)
BOE Technology Group Co Ltd
Beijing BOE Optoelectronics Technology Co Ltd
Original Assignee
BOE Technology Group Co Ltd
Beijing BOE Optoelectronics Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by BOE Technology Group Co Ltd, Beijing BOE Optoelectronics Technology Co Ltd filed Critical BOE Technology Group Co Ltd
Priority to CN201910173038.1A priority Critical patent/CN109813308A/zh
Publication of CN109813308A publication Critical patent/CN109813308A/zh
Priority to US16/537,241 priority patent/US11120562B2/en
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F1/00—Details not covered by groups G06F3/00 - G06F13/00 and G06F21/00
    • G06F1/16—Constructional details or arrangements
    • G06F1/1613—Constructional details or arrangements for portable computers
    • G06F1/1633—Constructional details or arrangements of portable computers not specific to the type of enclosures covered by groups G06F1/1615 - G06F1/1626
    • G06F1/1684—Constructional details or arrangements related to integrated I/O peripherals not covered by groups G06F1/1635 - G06F1/1675
    • G06F1/1694—Constructional details or arrangements related to integrated I/O peripherals not covered by groups G06F1/1635 - G06F1/1675 the I/O peripheral being a single or a set of motion sensors for pointer control or gesture input obtained by sensing movements of the portable computer
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T19/00—Manipulating 3D models or images for computer graphics
    • G06T19/006—Mixed reality
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/207—Analysis of motion for motion estimation over a hierarchy of resolutions
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V40/00—Recognition of biometric, human-related or animal-related patterns in image or video data
    • G06V40/10—Human or animal bodies, e.g. vehicle occupants or pedestrians; Body parts, e.g. hands
    • G06V40/103—Static body considered as a whole, e.g. static pedestrian or occupant recognition
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B5/00—Measuring for diagnostic purposes; Identification of persons
    • A61B5/103—Measuring devices for testing the shape, pattern, colour, size or movement of the body or parts thereof, for diagnostic purposes
    • A61B5/11—Measuring movement of the entire body or parts thereof, e.g. head or hand tremor or mobility of a limb
    • A61B5/1116—Determining posture transitions
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B5/00—Measuring for diagnostic purposes; Identification of persons
    • A61B5/103—Measuring devices for testing the shape, pattern, colour, size or movement of the body or parts thereof, for diagnostic purposes
    • A61B5/11—Measuring movement of the entire body or parts thereof, e.g. head or hand tremor or mobility of a limb
    • A61B5/1118—Determining activity level
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F3/00—Input arrangements for transferring data to be processed into a form capable of being handled by the computer; Output arrangements for transferring data from processing unit to output unit, e.g. interface arrangements
    • G06F3/01—Input arrangements or combined input and output arrangements for interaction between user and computer
    • G06F3/011—Arrangements for interaction with the human body, e.g. for user immersion in virtual reality
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/60—Analysis of geometric attributes

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computer Hardware Design (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Engineering & Computer Science (AREA)
  • Human Computer Interaction (AREA)
  • Computer Graphics (AREA)
  • Software Systems (AREA)
  • Automation & Control Theory (AREA)
  • Gyroscopes (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供了一种姿态估计方法、装置及计算机可读存储介质，涉及姿态估计领域，用于解决姿态估计的准确度较低的问题。其中，该姿态估计方法包括：计算当前时刻的加速度测量值和加速度估计值之间的误差；根据计算得到的误差，通过比例‑积分‑微分控制算法计算当前时刻的角速度控制量；获取当前时刻的角速度测量值，根据计算得到的角速度控制量对当前时刻的角速度测量值进行校正，计算得到当前时刻的角速度校正值；根据计算得到的角速度校正值和当前时刻的姿态四元数估计值，计算得到下一时刻的姿态四元数估计值。所述姿态估计方法用于姿态估计。

Description

姿态估计方法、装置及计算机可读存储介质
技术领域
本发明涉及姿态估计技术领域，尤其涉及一种姿态估计方法、装置及计算机可读存储介质。
背景技术
很多应用场景中，例如：多旋翼无人机的飞行中，卫星的发射中，导弹的导航中，以及近几年兴起的VR(Virtual Reality，虚拟现实)技术中，均需要对某一目标三维物体的姿态进行估计。在目标三维物体的姿态估计中，通常根据加速度计、磁力计和陀螺仪等传感器所获取的相应测量数据，进行目标三维物体的姿态估计计算。
在现有技术中，常用的姿态估计方法有扩展卡尔曼滤波法、互补滤波法、梯度下降法等，然而上述现有姿态估计方法由于种种原因，在姿态估计时的准确度较低。例如，扩展卡尔曼滤波法和梯度下降法在进行姿态估计时计算量较大，造成姿态估计延迟、姿态估计的频率较低，导致姿态估计的准确度下降；互补滤波法的估计精度较低，导致姿态估计的准确度较低。
发明内容
针对上述现有技术中的问题，本发明的实施例提供一种姿态估计方法、装置及计算机可读存储介质，以解决上述现有技术中姿态估计的准确度较低的问题。
为达到上述目的，本发明的实施例采用如下技术方案：
第一方面，本发明的实施例提供了一种姿态估计方法，包括计算当前时刻的加速度测量值和加速度估计值之间的误差；根据计算得到的误差，通过比例-积分-微分控制算法计算当前时刻的角速度控制量；获取当前时刻的角速度测量值，根据计算得到的角速度控制量对当前时刻的角速度测量值进行校正，计算得到当前时刻的角速度校正值；根据计算得到的角速度校正值和当前时刻的姿态四元数估计值，计算得到下一时刻的姿态四元数估计值。
在上述姿态估计方法中，在对角速度测量值的校正过程中，采用了比例-积分-微分控制计算输出角速度控制量，在比例控制使加速度测量值和加速度估计值之间的误差快速缩小的基础上，积分控制能够消除控制所过程中所产生的加速度测量值和加速度估计值之间的稳态误差，提高了所输出的角速度控制量的准确度；微分控制能够预测控制过程中的加速度测量值和加速度估计值之间的误差的变化趋势，提前抑制超调的形成，使控制过程趋向稳定状态，提高了所输出的角速度控制量的稳定性。因此，根据上述角速度控制量对角速度测量值进行校正后，所得到的角速度校正值的准确度和稳定性较高。从而根据该角速度校正值进行积分运算，所得到的下一时刻的姿态四元数估计值的准确度和稳定性均较高，即提高了姿态估计的准确度和稳定性。
此外，由于通过上述姿态估计方法获取姿态四元数估计值时无需经过大量的计算，因此其进行姿态估计的速度较快，从而提高了姿态估计的频率、减少了姿态估计的延迟，进一步提高了姿态估计的准确度。
基于上述技术方案，可选的，所述计算当前时刻的加速度测量值和加速度估计值之间的误差的步骤，包括：获取当前时刻T的加速度测量值ac(xc,yc,zc)；获取当前时刻T的姿态四元数估计值QT(q0,q1,q2,q3)，根据如下公式(1)计算当前时刻T的加速度估计值ag：
根据所述公式(2)计算当前时刻T的加速度测量值ac和加速度估计值ag之间的误差ET：
可选的，所述根据计算得到的误差，通过比例-积分-微分控制算法计算当前时刻的角速度控制量的步骤，包括：根据如下公式(3)计算当前时刻T的角速度控制量μ：
其中，表示从初始时刻0至当前时刻T的加速度测量值ac和加速度估计值ag之间的误差的积分；ET-1表示上一时刻T-1的加速度测量值ac和加速度估计值ag之间的误差；Kp表示比例控制系数；Ki表示积分控制系数；Kd表示微分控制系数。
可选的，所述计算得到当前时刻T的角速度校正值的步骤，包括：根据如下公式(4)计算得到当前时刻T的角速度校正值ω(ωx,ωy,ωz)，其中，ωc表示当前时刻T的角速度测量值：
ω＝ωc+μ (4)。
可选的，所述计算得到下一时刻的姿态四元数估计值的步骤，包括：根据如下公式(5)计算下一时刻T+1的姿态四元数估计值QT+1(q0,q1,q2,q3)，其中，ΔT表示角速度测量值的采样时间间隔：
第二方面，本发明实施例提供了一种姿态估算装置，所述姿态估算装置包括：误差计算器，配置为计算当前时刻的加速度测量值和加速度估计值之间的误差；与所述误差计算器相连的比例-积分-微分控制器，配置为根据所述误差计算器计算得到的误差，通过比例-积分-微分控制算法计算当前时刻的角速度控制量；与所述比例-积分-微分控制器相连的角速度校正器，配置为获取当前时刻的角速度测量值，并根据所述比例-积分-微分控制器计算得到的角速度控制量对当前时刻的角速度测量值进行校正，计算得到当前时刻的角速度校正值；与所述角速度校正器相连的姿态估计器，配置为根据当前时刻的姿态四元数估计值和所述角速度校正器计算得到的角速度校正值，计算得到下一时刻的姿态四元数估计值。
上述姿态估算装置所能产生的有益效果与第一方面所提供的姿态估计方法的有益效果相同，此处不再赘述。
第三方面，本发明实施例提供了一种计算机可读存储介质，所述计算机可读存储介质存储有一个或多个指令，以执行如第一方面所述的姿态估计方法中的一个或多个步骤。
上述计算机可读存储介质产生的有益效果与第一方面所提供的姿态估计方法的有益效果相同，此处不再赘述。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1为本发明实施例所提供的姿态估计方法的第一种流程图；
图2为本发明实施例所提供的姿态估计方法的第二种流程图；
图3为本发明实施例所提供的姿态估算装置的结构示意图。
附图标记说明：
100-姿态估计装置； 1-误差计算器；
2-比例-积分-微分控制器； 3-加速度校正器；
4-姿态估计器； 200-加速度传感器；
300-角速度传感器。
具体实施方式
正如背景技术所述，在进行目标三维物体的姿态估计时存在准确度较低的问题。本发明的发明人经研究发现，用于目标三维物体的姿态估计的测量传感器主要有陀螺仪、加速度计和磁力计等。其中，陀螺仪能够测量目标三维物体的角速度，并通对所测得的角速度进行积分运算得到目标三维物体的下一时刻的姿态角估计值，从而完成对目标三维物体的姿态估计。在短时间内根据陀螺仪进行姿态估计的结果较为准确，但由于陀螺仪在姿态估计中需要积分运算，因此即使较小的测量偏差随着时间的推移也会累积为较大的累积误差，从而导致较长时间后姿态估计的结果不准确。
加速度计能够实时测量目标三维物体的三轴方向的加速度，根据加速度计计算得到的目标三维物体的当前时刻的姿态角不存在累积误差，但其受目标三维物体的运动状态变化的影响较大，因此根据加速度计计算得到的目标三维物体的下一时刻的姿态角估计值中存在较大噪声，导致姿态估计的结果不准确、不稳定。在实际的姿态估计中，通常需要陀螺仪和加速度计配合使用。
基于上述研究，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行描述。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员所获得的所有其它实施例，均属于本发明保护的范围。
本发明的实施例提供了一种姿态估计方法，如图1所示，包括：
S1.计算当前时刻的加速度测量值和加速度估计值之间的误差；
S2.根据计算得到的误差，通过比例-积分-微分控制算法计算当前时刻的角速度控制量；
S3.获取当前时刻的角速度测量值，根据计算得到的角速度控制量对当前时刻的角速度测量值进行校正，计算得到当前时刻的角速度校正值；
S4.根据计算得到的角速度校正值和当前时刻的姿态四元数估计值，计算得到下一时刻的姿态四元数估计值。
需要说明的是，姿态四元数通常用于描述目标三维物体的姿态角信息，对目标三维物体的姿态估计为对目标三维物体的姿态角的估计，计算得到目标三维物体的一个姿态四元数估计值即可视为完成一次对目标三维物体的姿态角的估计，也即完成一次姿态估计。
在上述姿态估计方法的步骤S1中，“加速度估计值”指的是根据当前时刻的姿态角估计值计算得到的加速度值，其中，该姿态角估计值指的是根据陀螺仪所测得的角速度积分运算后所得到的姿态角估计值。“加速度测量值”指的是实际测量得到的加速度值，其更接近于加速度的真实值，通常可以将加速度测量值近似看作加速度的真实值。因此，当前时刻的加速度测量值与加速度估计值之间的误差能够表征当前时刻的根据陀螺仪得到的姿态角估计值与姿态角真实值之间的误差。
在本发明的上述姿态估计方法中，将当前时刻的加速度测量值与加速度估计值之间的误差作为输入量，通过比例-积分-微分控制计算得到的输出量作为角速度控制量，并根据该角速度控制量来对角速度估计值进行校正，从而实现对角速度积分后所产生的累积误差的校正。由于在对角速度测量值的校正过程中，采用了比例-积分-微分控制计算输出角速度控制量，在比例控制使加速度测量值和加速度估计值之间的误差快速缩小的基础上，积分控制能够消除比例控制所产生的加速度测量值和加速度估计值之间的稳态误差，提高了所输出的角速度控制量的准确度；微分控制能够预测控制过程中的加速度测量值和加速度估计值之间的误差的变化趋势，并防止在在控制过程中形成超调，提高了所输出的角速度控制量的稳定性。因此，根据上述角速度控制量对角速度测量值进行校正后，所得到的角速度校正值的准确度和稳定性较高。从而根据该角速度校正值进行积分运算，所得到的下一时刻的姿态四元数估计值的准确度和稳定性均较高，即提高了姿态估计的准确度和稳定性。
此外，由于通过上述姿态估计方法获取姿态四元数估计值时无需经过大量的计算，因此其进行姿态估计的速度较快，从而提高了姿态估计的频率、减少了姿态估计的延迟，进一步提高了姿态估计的准确度。
基于上述技术方案，在本发明所提供的另一些实施例中，如图2所示，作为一种可实现的方式，在上述姿态估计方法的步骤S1中，计算当前时刻的加速度测量值和加速度估计值之间的误差时可以包括以下步骤：
S11.获取当前时刻T的加速度测量值ac(xc,yc,zc)。
S12.获取当前时刻T的姿态四元数估计值QT(q0,q1,q2,q3)，根据如下公式(1)计算当前时刻T的加速度估计值ag：
S13.根据如下公式(2)计算得到当前时刻T的加速度测量值ac和加速度估计值ag之间的误差ET：
在步骤S11中，加速度测量值ac可以是通过传感器获取得到的。例如，可以通过加速度计测量得到加速度测量值ac，加速度计能够实时地、准确地获取目标三维物体的加速度。
作为一种示例，在步骤S11中，上述“当前时刻T的姿态四元数估计值QT(q0,q1,q2,q3)”可以是在T-1时刻通过本发明所提供的上述姿态估计方法计算得到的姿态四元数的估计值。
需要说明的是，在步骤S12中，上述公式(1)可以根据如下公式(6)、(7)和(8)推导得到：
其中，公式(7)为当前时刻T的姿态四元数估计值QT(q0,q1,q2,q3)所对应的方向余弦矩阵的表达式。方向余弦矩阵为从世界坐标系到目标三维物体坐标系的旋转矩阵，用于描述从世界坐标系向目标三维物体坐标系的旋转情况。当目标三维物体的姿态四元数估计值QT(q0,q1,q2,q3)确定时，该姿态四元数估计值QT(q0,q1,q2,q3)所对应的方向余弦矩阵也是确定的。还需要指出的是，公式(7)仅示出方向余弦矩阵的一种数学表达式，方向余弦矩阵经数学运算后可变换为其他表达形式。
公式(8)为世界坐标系下的归一化后的重力加速度g的表达式。
公式(6)中，当将世界坐标系下的归一化后的重力加速度g，通过当前姿态角估计值所对应的方向余弦矩阵转换目标三维物体坐标系时，即得到加速度估计值ag。
在本发明的一些实施例中，如图2所示，进一步的，作为上述姿态估计方法的步骤S2的一种可实现方式，在根据计算得到的误差，通过比例-积分-微分控制算法计算当前时刻的角速度控制量时，可以包括以下步骤：
根据如下公式(3)计算当前时刻T的角速度控制量μ：
其中，表示从初始时刻0至当前时刻T，加速度测量值和加速度估计值之间的误差的积分，ET-1表示上一时刻T-1的加速度测量值和加速度估计值之间的误差，Kp表示比例控制系数，Ki表示积分控制系数，Kd表示微分控制系数。
需要说明的是，在上述姿态估计方法中，比例控制系数Kp会影响控制过程中的加速度估计值ag向加速度测量值ac的收敛速度。例如，当比例控制系数Kp较大时，加速度估计值ag向加速度测量值ac收敛的速度较快，但在控制过程中容易产生较大的超调，降低控制过程的稳定性；当比例控制系数Kp较小时，加速度估计值ag向加速度测量值ac收敛的速度较慢。
积分控制系数Ki会影响控制过程中消除稳态误差的速度。例如，当积分控制系数Ki较大时，上述稳态误差消除的速度较快，但会导致控制过程产生较大的振荡，降低了控制过程的稳定性；当积分控制系数Ki较小时，上述稳态误差消除的速度较慢。
微分控制系数Kd则会影响控制过程误差ET达到稳定值的时长。例如，当微分控制系数Kd较大时，误差ET达到稳定值的时间较长，也即调节过程达到稳定状态的时间较长；当微分控制系数Kd较小时，微分控制对于超调的抑制作用较弱，起不到提前抑制超调形成的效果。
示例性的，在上述姿态估计方法中，执行比例-积分-微分控制的执行部件可以是陀螺仪、加速度计等传感器。其中，比例控制系数Kp、积分控制系数Ki和微分控制系数Kd可以根据执行比例-积分-微分控制的执行部件的自身情况(例如该执行部件的测量误差)来确定。示例性的，可以通过对进行比例-积分-微分控制的执行部件进行有限次数的实验来确定上述比例控制系数Kp、积分控制系数Ki和微分控制系数Kd。
还需要说明的是，上述公式(3)可以根据如下公式(9)得到：
公式(9)为比例-积分-微分控制算法的连续表达式，其中，μ0表示未做离散处理的角速度控制量，t表示从初始时刻0到当前时刻T所间隔的时间。在实际应用中，由于加速度测量值等的获取具有一定的采样周期，即相邻的两次加速度测量值获取之间存在一定的时间间隔，从而加速度测量值ac和加速度估计值ag之间的误差ET也具有一定的获取周期。因此，在采用比例-积分-微分控制算法计算角速度控制量μ时，需要采用比例-积分-微分控制算法的离散表达式，上述公式(3)即为由上述公式(9)作离散化变形后得到的比例-积分-微分控制算法的离散表达式。
在本发明的一些实施例中，如图2所示，进一步的，作为上述姿态估计方法的步骤S3的一种可实现方式，在计算得到当前时刻T的角速度校正值时，可以包括以下步骤：
根据如下公式(4)计算得到当前时刻T的角速度校正值ω(ωx,ωy,ωz)：
ω＝ωc+μ (4)，
其中，ωc表示当前时刻T的角速度测量值。
从而通过将当前时刻T的角速度控制量与角速度测量值进行叠加，对当前时刻的角速度测量值进行校正，得到当前时刻T的角速度校正值ω。
在本发明的一些实施例中，如图2所示，进一步的，作为上述姿态估计方法的步骤S4的一种可实现方式，计算得到下一时刻的姿态四元数估计值时，可以包括以下步骤：
根据如下公式(5)计算下一时刻T+1的姿态四元数估计值QT+1(q0,q1,q2,q3)：
其中，ΔT表示角速度测量值ωc的采样时间间隔。
需要说明的是，上述公式(5)可以根据如下公式(10)推导得到：
在公式(5)中计算下一时刻T+1的姿态四元数估计值QT+1(q0,q1,q2,q3)时，所用到是已经过比例-积分-微分控制校正过的角速度校正值ω，因此通过公式(5)所得到的下一时刻T+1的姿态四元数估计值QT+1(q0,q1,q2,q3)的准确度和稳定性较高。
需要说明的是，通常根据加速度值所计算得到的为目标三维物体的姿态角信息中的俯仰角和翻滚角，以上实施例中仅以角速度控制量为俯仰角和翻滚角的角速度控制量为例(也即仅以对俯仰角和翻滚角的角速度测量值进行校正为例)进行了说明。
在本发明的一些实施例中，作为上述姿态估计方法的一种可实现方式，也可以对姿态角信息中的偏航角的角速度测量值进行校正。示例性的，在上述步骤S2之前，还可以包括：计算当前时刻的磁场强度测量值和磁场强度估计值之间的误差。在步骤S2中，通过比例-积分-微分控制算法计算当前时刻的角速度控制量所根据的误差还可以包括当前时刻的磁场强度测量值和磁场强度估计值之间的误差。由此得出的角速度控制量中既包括俯仰角和翻滚角的角速度控制量，也包括偏航角的角速度控制量，从而使角速度测量值校正过程的准确度进一步提高，进而也进一步提高了姿态估计的准确度。示例性的，上述磁场强度测量值可以通过磁力计获取。
需要指出的是，在实现过程中，上述姿态估计方法的各步骤可以通过处理器中的硬件的集成逻辑电路或者软件形式的指令完成。结合本发明的实施例所公开的方法的步骤可以直接体现为硬件处理器执行完成，或者用处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质可以位于存储器，处理器读取存储器中的信息，结合其硬件完成上述方法的步骤。
在本发明的一些实施例中，如图3所示，还提供了一种姿态估算装置100，包括：误差计算器1，配置为计算当前时刻的加速度测量值和加速度估计值之间的误差；与误差计算器1相连的比例-积分-微分控制器2，配置为根据误差计算器1计算得到的误差，通过比例-积分-微分控制算法计算当前时刻的角速度控制量；与所述比例-积分-微分控制器2相连的角速度校正器3，配置为获取当前时刻的角速度测量值，并根据比例-积分-微分控制器2计算得到的角速度控制量对当前时刻的角速度测量值进行校正，计算得到当前时刻的角速度校正值；与角速度校正器3相连的姿态估计器4，配置为根据当前时刻的姿态四元数估计值和角速度校正器3计算得到的角速度校正值，计算得到下一时刻的姿态四元数估计值。
该姿态估算装置100具有与上述姿态估计方法相同的有益效果，此处不再赘述。
在一些实施例中，作为一种可能的设计，误差计算器1在计算当前时刻的加速度测量值和加速度估计值之间的误差时，可以从姿态估计器4中获取得到当前时刻的姿态四元数估计值，并根据该当前时刻的姿态四元数估计值计算得到当前时刻的加速度估计值。
在一些实施例中，误差计算器1可以从加速度传感器200中获取加速度测量值，角速度校正器3可以从角速度传感器300中获取角速度测量值。
在本发明的一些实施例中，还提供了一种计算机可读存储介质，该计算机可读存储介质存储有一个或多个指令，以执行上述姿态估计方法中的一个或多个步骤。
该计算机可读存储介质具有与上述姿态估计方法相同的有益效果，此处不再赘述。
以上所述仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (7)

1.一种姿态估计方法，其特征在于，包括：
计算当前时刻的加速度测量值和加速度估计值之间的误差；
根据计算得到的误差，通过比例-积分-微分控制算法计算当前时刻的角速度控制量；
获取当前时刻的角速度测量值，根据计算得到的角速度控制量对当前时刻的角速度测量值进行校正，计算得到当前时刻的角速度校正值；
根据计算得到的角速度校正值和当前时刻的姿态四元数估计值，计算得到下一时刻的姿态四元数估计值。
2.根据权利要求1所述的姿态估计方法，其特征在于，所述计算当前时刻的加速度测量值和加速度估计值之间的误差的步骤，包括：
获取当前时刻T的加速度测量值ac(xc,yc,zc)；
获取当前时刻T的姿态四元数估计值QT(q0,q1,q2,q3)，根据如下公式(1)计算当前时刻T的加速度估计值ag：
根据如下公式(2)计算得到当前时刻T的加速度测量值ac和加速度估计值ag之间的误差ET：
3.根据权利要求2所述的姿态估计方法，其特征在于，所述根据计算得到的误差，通过比例-积分-微分控制算法计算当前时刻的角速度控制量的步骤，包括：根据如下公式(3)计算当前时刻T的角速度控制量μ：
其中，表示从初始时刻0至当前时刻T的加速度测量值ac和加速度估计值ag之间的误差的积分；ET-1表示上一时刻T-1的加速度测量值ac和加速度估计值ag之间的误差；Kp表示比例控制系数；Ki表示积分控制系数；Kd表示微分控制系数。
4.根据权利要求3所述的姿态估计方法，其特征在于，所述计算得到当前时刻T的角速度校正值的步骤，包括：根据如下公式(4)计算得到当前时刻T的角速度校正值ω(ωx,ωy,ωz)：
ω＝ωc+μ (4)；
其中，ωc表示当前时刻T的角速度测量值。
5.根据权利要求4所述的姿态估计方法，其特征在于，所述计算得到下一时刻的姿态四元数估计值的步骤，包括：根据如下公式(5)计算下一时刻T+1的姿态四元数估计值QT+1(q0,q1,q2,q3)：
其中，ΔT表示角速度测量值的采样时间间隔。
6.一种姿态估算装置，其特征在于，所述姿态估算装置包括：
误差计算器，配置为计算当前时刻的加速度测量值和加速度估计值之间的误差；
与所述误差计算器相连的比例-积分-微分控制器，配置为根据所述误差计算器计算得到的误差，通过比例-积分-微分控制算法计算当前时刻的角速度控制量；
与所述比例-积分-微分控制器相连的角速度校正器，配置为获取当前时刻的角速度测量值，并根据所述比例-积分-微分控制器计算得到的角速度控制量对当前时刻的角速度测量值进行校正，计算得到当前时刻的角速度校正值；
与所述角速度校正器相连的姿态估计器，配置为根据当前时刻的姿态四元数估计值和所述角速度校正器计算得到的角速度校正值，计算得到下一时刻的姿态四元数估计值。
7.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有一个或多个指令，以执行如权利要求1～5中任一项所述的姿态估计方法中的一个或多个步骤。
CN201910173038.1A 2019-03-07 2019-03-07 姿态估计方法、装置及计算机可读存储介质 Pending CN109813308A (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201910173038.1A CN109813308A (zh) 2019-03-07 2019-03-07 姿态估计方法、装置及计算机可读存储介质
US16/537,241 US11120562B2 (en) 2019-03-07 2019-08-09 Posture estimation method, posture estimation apparatus and computer readable storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910173038.1A CN109813308A (zh) 2019-03-07 2019-03-07 姿态估计方法、装置及计算机可读存储介质

Publications (1)

Publication Number Publication Date
CN109813308A true CN109813308A (zh) 2019-05-28

Family

ID=66608341

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910173038.1A Pending CN109813308A (zh) 2019-03-07 2019-03-07 姿态估计方法、装置及计算机可读存储介质

Country Status (2)

Country Link
US (1) US11120562B2 (zh)
CN (1) CN109813308A (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110293563A (zh) * 2019-06-28 2019-10-01 炬星科技（深圳）有限公司 估计机器人位姿的方法、设备及存储介质
CN111026139A (zh) * 2019-09-25 2020-04-17 中国人民解放军63850部队 一种基于飞行轨迹的三维模型姿态调整控制方法
CN112462793A (zh) * 2019-09-09 2021-03-09 顺丰科技有限公司 基于传感器的设备控制方法、装置、计算机设备
CN112762918A (zh) * 2020-12-28 2021-05-07 中国科学院长春光学精密机械与物理研究所 集成式数字角速度陀螺仪、预测滤波方法和电子设备
CN112904091A (zh) * 2021-01-13 2021-06-04 成都四威功率电子科技有限公司 一种基于pid的场强辐射自动测试系统控制方法
CN114326767A (zh) * 2021-12-14 2022-04-12 宁波天擎航天科技有限公司 飞行器姿态控制方法、飞行器姿态控制系统以及飞行器

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110896495A (zh) * 2019-11-19 2020-03-20 北京字节跳动网络技术有限公司 用于目标设备的视图调整方法、装置、电子设备和介质
CN112683267B (zh) * 2020-11-30 2022-06-03 电子科技大学 一种附有gnss速度矢量辅助的车载姿态估计方法
CN113229804A (zh) * 2021-05-07 2021-08-10 陕西福音假肢有限责任公司 一种用于关节活动度的磁场数据融合电路及其方法
CN114504385A (zh) * 2022-01-18 2022-05-17 上海术航机器人有限公司 手术机器人末端姿态调整方法、系统、设备和介质
CN114383613B (zh) * 2022-01-18 2023-12-15 广东工业大学 一种基于北斗定向的电力线舞动监测系统和方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102955477A (zh) * 2012-10-26 2013-03-06 南京信息工程大学 一种四旋翼飞行器姿态控制系统及控制方法
CN103611324A (zh) * 2013-11-14 2014-03-05 南京航空航天大学 一种无人直升机飞行控制系统及其控制方法
CN106249745A (zh) * 2016-07-07 2016-12-21 苏州大学 四轴无人机的控制方法
RU2615032C1 (ru) * 2015-10-06 2017-04-03 Акционерное Общество "Конструкторское Бюро "Луч" Бесплатформенная инерциальная курсовертикаль на чувствительных элементах высокой точности
CN108827299A (zh) * 2018-03-29 2018-11-16 南京航空航天大学 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9588598B2 (en) * 2015-06-30 2017-03-07 Ariadne's Thread (Usa), Inc. Efficient orientation estimation system using magnetic, angular rate, and gravity sensors
JP2020020631A (ja) * 2018-07-31 2020-02-06 セイコーエプソン株式会社 姿勢推定方法、姿勢推定装置及び移動体

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102955477A (zh) * 2012-10-26 2013-03-06 南京信息工程大学 一种四旋翼飞行器姿态控制系统及控制方法
CN103611324A (zh) * 2013-11-14 2014-03-05 南京航空航天大学 一种无人直升机飞行控制系统及其控制方法
RU2615032C1 (ru) * 2015-10-06 2017-04-03 Акционерное Общество "Конструкторское Бюро "Луч" Бесплатформенная инерциальная курсовертикаль на чувствительных элементах высокой точности
CN106249745A (zh) * 2016-07-07 2016-12-21 苏州大学 四轴无人机的控制方法
CN108827299A (zh) * 2018-03-29 2018-11-16 南京航空航天大学 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
宫琛: "四轴飞行器的研究与设计", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 *

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110293563A (zh) * 2019-06-28 2019-10-01 炬星科技（深圳）有限公司 估计机器人位姿的方法、设备及存储介质
CN110293563B (zh) * 2019-06-28 2022-07-26 炬星科技（深圳）有限公司 估计机器人位姿的方法、设备及存储介质
CN112462793A (zh) * 2019-09-09 2021-03-09 顺丰科技有限公司 基于传感器的设备控制方法、装置、计算机设备
CN111026139A (zh) * 2019-09-25 2020-04-17 中国人民解放军63850部队 一种基于飞行轨迹的三维模型姿态调整控制方法
CN111026139B (zh) * 2019-09-25 2023-07-18 中国人民解放军63850部队 一种基于飞行轨迹的三维模型姿态调整控制方法
CN112762918A (zh) * 2020-12-28 2021-05-07 中国科学院长春光学精密机械与物理研究所 集成式数字角速度陀螺仪、预测滤波方法和电子设备
CN112762918B (zh) * 2020-12-28 2023-07-14 中国科学院长春光学精密机械与物理研究所 集成式数字角速度陀螺仪、预测滤波方法和电子设备
CN112904091A (zh) * 2021-01-13 2021-06-04 成都四威功率电子科技有限公司 一种基于pid的场强辐射自动测试系统控制方法
CN112904091B (zh) * 2021-01-13 2023-11-21 成都四威功率电子科技有限公司 一种基于pid的场强辐射自动测试系统控制方法
CN114326767A (zh) * 2021-12-14 2022-04-12 宁波天擎航天科技有限公司 飞行器姿态控制方法、飞行器姿态控制系统以及飞行器

Also Published As

Publication number Publication date
US20200286245A1 (en) 2020-09-10
US11120562B2 (en) 2021-09-14

Similar Documents

Publication Publication Date Title
CN109813308A (zh) 姿态估计方法、装置及计算机可读存储介质
CN110887480B (zh) 基于mems传感器的飞行姿态估计方法及系统
CN108827299B (zh) 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法
CN108957495B (zh) Gnss与mimu组合导航方法、装置及计算机设备
US10101174B2 (en) Air data aided inertial measurement unit
CN111551174A (zh) 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
CN107618678B (zh) 卫星姿态角度偏差下的姿控信息联合估计方法
CN106767798B (zh) 一种无人机导航用的位置及速度的实时估计方法及系统
EP3527948B1 (en) Air data aided inertial measurement unit
CN103712598B (zh) 一种小型无人机姿态确定方法
EP3388788B1 (en) Inertially-aided air data computer altitude rate
CN116067370B (zh) 一种imu姿态解算方法及设备、存储介质
WO2022063120A1 (zh) 组合导航系统初始化方法、装置、介质及电子设备
RU2762143C2 (ru) Система определения курса и углового пространственного положения, выполненная с возможностью функционирования в полярной области
CN110440797A (zh) 车辆姿态估计方法及系统
CN111141313A (zh) 一种提高机载局部相对姿态匹配传递对准精度的方法
CN110873563B (zh) 一种云台姿态估计方法及装置
CN111044053B (zh) 单舵轮无人车的导航方法、装置和单舵轮无人车
CN104819717B (zh) 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法
RU2564379C1 (ru) Бесплатформенная инерциальная курсовертикаль
CN113720349A (zh) 一种基于卡尔曼滤波的里程计信息平滑方法
CN110209185A (zh) 一种利用星敏信息姿态重置的航天器姿态机动稳定方法
JP2001264106A (ja) 慣性航法装置、慣性航法装置の初期化方法及び記録媒体
TW201416815A (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
RJ01 Rejection of invention patent application after publication

Application publication date: 20190528

RJ01 Rejection of invention patent application after publication