CN110231054A - 姿态估计算法的评价方法、装置及计算机可读存储介质 - Google Patents
姿态估计算法的评价方法、装置及计算机可读存储介质 Download PDFInfo
- Publication number
- CN110231054A CN110231054A CN201910554605.8A CN201910554605A CN110231054A CN 110231054 A CN110231054 A CN 110231054A CN 201910554605 A CN201910554605 A CN 201910554605A CN 110231054 A CN110231054 A CN 110231054A
- Authority
- CN
- China
- Prior art keywords
- real
- attitude
- time
- quaternary number
- checked
- 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
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Abstract
本发明提供了一种姿态估计算法评价方法、装置以及计算机可读存储介质,涉及姿态估计技术领域。其中,所述方法包括将待检测设备固定至旋转台,并记录所述待检测设备根据待评价姿态估计算法获得的静止姿态四元数;将所述旋转台旋转至少一周,并记录所述待检测设备的实时加速度、以及所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数;根据所述静止姿态四元数、所述实时加速度、所述实时姿态四元数和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数;根据所述实时标准姿态四元数和所述实时姿态四元数之间的误差,对所述待评价姿态估计算法进行评价。在本发明实施例所述的评价方法实施难度较低,操作较为简单。
Description
技术领域
本发明涉及姿态估计技术领域,特别是涉及一种姿态估计算法评价方法、装置以及计算机可读存储介质。
背景技术
在很多应用场景中,例如:多旋翼无人飞机的飞行中、卫星的发射中、导弹的导航中以及虚拟现实的技术中,需要利用姿态估计算法实时估算物体姿态。姿态估计算法的优劣程度并不能仅仅依靠主观感受进行评价,需要客观的、可量化的评价。
现有的技术中,可以将待检测设备固定在旋转台以对姿态估计算法进行评价。然而,在上述采用旋转台对姿态估计算法进行评价的评价方法实施的过程中,通常需要将旋转台水平放置,很容易使得上述评价方法的实施难度较高,操作较为复杂。
发明内容
本发明提供一种姿态估计算法评价方法、装置以及计算机可读存储介质,以解决现有的姿态估计算法的评价方法实施难度较高,操作较为复杂的问题。
为了解决上述问题,本发明公开了一种姿态估计算法的评价方法,包括:
将待检测设备固定至旋转台,并记录所述待检测设备根据待评价姿态估计算法获得的静止姿态四元数;
将所述旋转台旋转至少一周,并记录所述待检测设备的实时加速度、以及所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数,其中,所述实时姿态四元数中包括起始姿态四元数;
根据所述静止姿态四元数、所述实时加速度、所述实时姿态四元数和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数;
根据所述实时标准姿态四元数和所述实时姿态四元数之间的误差,对所述待评价姿态估计算法进行评价。
可选地,所述根据所述静止姿态四元数、所述实时加速度、所述实时姿态四元数和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数,包括:
根据所述实时姿态四元数和所述实时加速度,确定所述旋转台相对于第一坐标系的实时姿态四元数;
根据所述旋转台相对于所述第一坐标系的实时姿态四元数、所述实时加速度以及所述静止姿态四元数,计算出所述待检测设备的标准角速度;
根据所述标准角速度和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
可选地,所述根据所述待检测设备的实时姿态四元数,确定所述旋转台相对于第一坐标系的实时姿态四元数,包括:
根据所述实时加速度,确定所述待检测设备的加速度极限位置;
根据所述待检测设备在所述加速度极限位置的实时姿态四元数,确定所述旋转台相对于第一坐标系的实时姿态四元数。
可选地,所述根据所述待检测设备在所述加速度极限位置的实时姿态四元数,确定所述旋转台相对于第一坐标系的实时姿态四元数,包括:
根据所述待检测设备在所述加速度极限位置的实时姿态四元数,确定将所述旋转台在第一坐标系下的实际转轴旋转至指定转轴的旋转轴坐标,其中,所述指定转轴为所述旋转台水平放置时的转轴;
对所述旋转轴坐标进行归一化处理,得到归一化的旋转轴坐标;
计算所述实际转轴和所述指定转轴之间的夹角;
根据所述夹角和所述归一化的旋转轴坐标,得到所述旋转台相对于所述第一坐标系的实时姿态四元数。
可选地,所述根据所述旋转台相对于所述第一坐标系的实时姿态四元数、所述实时加速度以及所述静止姿态四元数,计算出所述待检测设备的标准角速度,包括:
根据所述实时角速度和所述旋转台相对于所述第一坐标系的实时姿态四元数,得出所述实时角速度在所述第一坐标系中的角速度;
根据所述实时角速度在所述第一坐标系中的角速度和所述静止姿态四元数,计算出所述待检测设备的标准角速度。
可选地,所述根据所述标准角速度和所述起始姿态四元数估计值,计算出所述待检测设备的实时标准姿态四元数,包括:
根据所述标准角速度和相邻两次采样的间隔时间,计算出相邻两次采样的旋转角度;
根据所述标准角速度,得到相邻两次采样的归一化旋转轴坐标;
根据所述相邻两次采样的旋转角度和所述两次相邻采样的归一化旋转轴坐标,得到相邻两次采样的姿态变化量;
根据所述相邻两次采样的姿态变化量和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
为了解决上述问题,本发明还公开了一种姿态估计算法的评价装置,包括:
第一记录模块,用于将待检测设备固定至旋转台,并记录所述待检测设备根据待评价姿态估计算法获得的静止姿态四元数;
第二记录模块,用于将所述旋转台旋转至少一周,并记录所述待检测设备的实时加速度、以及所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数,其中,所述实时姿态四元数中包括起始姿态四元数;
计算模块,用于根据所述静止姿态四元数、所述实时加速度、所述实时姿态四元数和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数;
评价模块,用于根据所述实时标准姿态四元数和所述实时姿态四元数之间的误差,对所述待评价姿态估计算法进行评价。
可选地,所述计算模块包括:
确定子模块,用于根据所述实时姿态四元数和所述实时加速度,确定所述旋转台相对于第一坐标系的实时姿态四元数;
第一计算子模块,用于根据所述旋转台相对于所述第一坐标系的实时姿态四元数、所述实时加速度以及所述静止姿态四元数,计算出所述待检测设备的标准角速度;
第二计算子模块,用于根据所述标准角速度和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
可选地,所述确定子模块包括:
第一确定单元,用于根据所述实时加速度,确定所述待检测设备的加速度极限位置;
第二确定单元,用于根据所述待检测设备在所述加速度极限位置的实时姿态四元数,确定所述旋转台相对于第一坐标系的实时姿态四元数。
可选地,所述第二确定单元包括:
第一确定子单元,用于根据所述待检测设备在所述加速度极限位置的实时姿态四元数,确定将所述旋转台在第一坐标系下的实际转轴旋转至指定转轴的旋转轴坐标,其中,所述指定转轴为所述旋转台水平放置时的转轴;
第二确定子单元,用于对所述旋转轴坐标进行归一化处理,得到归一化的旋转轴坐标;
第一计算子单元,用于计算所述实际转轴和所述指定转轴之间的夹角;
第二计算子单元,用于根据所述夹角和所述归一化的旋转轴坐标,得到所述旋转台相对于所述第一坐标系的实时姿态四元数。
可选地,所述第一计算子模块包括:
第一计算单元,用于根据所述实时角速度和所述旋转台相对于所述第一坐标系的实时姿态四元数,得出所述实时角速度在所述第一坐标系中的角速度;
第二计算单元,用于根据所述实时角速度在所述第一坐标系中的角速度和所述静止姿态四元数,计算出所述待检测设备的标准角速度。
可选地,所述第二计算子模块包括:
第三计算子单元,用于根据所述标准角速度和相邻两次采样的间隔时间,计算出相邻两次采样的旋转角度;
第四计算子单元,用于根据所述标准角速度,得到相邻两次采样的归一化旋转轴坐标;
第五计算子单元,用于根据所述相邻两次采样的旋转角度和所述两次相邻采样的归一化旋转轴坐标,得到相邻两次采样的姿态变化量;
第六计算子单元,用于根据所述相邻两次采样的姿态变化量和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
为了解决上述问题,本发明还公开了一种计算机可读存储介,所述计算机可读存储介质存储有一个或多个指令,以执行上述姿态估计算法的评价方法中任一项所述的姿态估计算法的评价方法中的一个或多个步骤
与现有技术相比,本发明包括以下优点:
在本发明实施例中,通过将待检测设备固定至旋转台,并记录待检测设备根据待评价姿态估计算法获得的静止姿态四元数;将所述旋转台旋转至少一周,并记录所述待检测设备的实时加速度、以及所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数,其中,所述实时姿态四元数中包括起始姿态四元数;根据所述静止姿态四元数、所述实时加速度、所述实时姿态四元数和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数;根据所述实时标准姿态四元数和所述实时姿态四元数之间的误差,对所述待评价姿态估计算法进行评价。本发明实施例中,在所述旋转台为任意姿态时皆可计算出所述实时标准姿态四元数,实现对于所述待评价姿态估计算法的评价,这样,就解决了采用旋转台对姿态估计算法进行评价的过程中需要将旋转台水平放置的问题。也就是说,本发明实施例所述的评价方法实施难度较低,操作较为简单。。
附图说明
图1示出了本发明实施例一的一种姿态估计算法的评价方法的步骤流程图;
图2示出了本发明实施例二的一种姿态估计算法的评价方法的步骤流程图;
图3示出了本发明的一种旋转台的位置示意图;
图4示出了本发明实施例三的一种姿态估计算法的评价装置的结构框图;
图5示出了本发明实施例三的一种姿态估计算法的评价装置的结构框图。
具体实施方式
为使本发明的上述目的、特征和优点能够更加明显易懂,下面结合附图和具体实施方式对本发明作进一步详细的说明。
实施例一
参照图1,示出了本发明实施例一的一种姿态估计算法的评价方法的步骤流程图。
本发明实施例的姿态估计算法的评价方法包括以下步骤:
步骤101:将待检测设备固定至旋转台,并记录所述待检测设备根据待评价姿态估计算法获得的静止姿态四元数。
本发明实施例中,所述待检测设备可以为需要固定在所述旋转台上进行姿态检测的设备,所述待检测设备可以包括但不局限于无人飞机、卫星、虚拟现实设备等设备。所述待评价姿态估计算法可以为一种可以实时获取待检测设计的姿态四元数的姿态估计算法,所述待评价姿态估计算法可以包括但不局限于扩展卡尔曼滤波法、互补滤波法或者梯度下降法中的任意一种,本发明实施例对此不做限定。
在实际应用中,姿态四元数通常用于描述目标三维物体的姿态角信息,对目标三维物体的姿态估计为对目标三维物体的姿态角的估计,计算得到目标三维物体的一个姿态四元数估计值即可视为完成一次对目标三维物体的姿态角的估计,也即完成一次姿态估计。
本发明实施例中,可以将待检测设备固定至所述旋转台,所述旋转台的姿态可以不做限制。例如,所述旋转台的姿态可以为水平或者倾斜任意角度。在将所述待检测设备固定至所述旋转台后,可以记录所述待检测设备根据所述待评价姿态估计算法获得的静止姿态四元数。具体地,所述静止姿态四元数为所述待检测设备在静止状态下的姿态四元数。
步骤102:将所述旋转台旋转至少一周,并记录所述待检测设备的实时加速度、以及所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数,其中,所述实时姿态四元数中包括起始姿态四元数。
本发明实施例中,所述实时加速度可以根据IMU(惯性测量单元)获得,IMU是测量物体三轴姿态角(或角速率)以及加速度的装置。一般的,一个IMU包含了三个单轴的加速度计和三个单轴的陀螺,加速度计检测物体在载体坐标系统独立三轴的实时加速度。
在实际应用中,在所述旋转台旋转至少一周的过程中,可以记录所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数,所述实时姿态四元数加速度可以包括起始姿态四元数和每个采集时刻对应的实时姿态四元数。具体地,所述起始姿态四元数可以为所述旋转台开始旋转时,所述待测设备根据所述待评价姿态估计算法获得的姿态四元数。
步骤103:根据所述静止姿态四元数、所述实时加速度、所述实时姿态四元数和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
本发明实施例中,可以根据所述实时姿态四元数和所述实时加速度,计算出所述旋转台相对于所述第一坐标系的实施姿态四元数,具体地,所述第一坐标系可以为独立于旋转台坐标系和待检测设备坐标系之外的坐标系,例如,所述第一坐标系可以为世界坐标系。
在实际应用中,根据所述旋转台相对于所述第一坐标系的实时姿态四元数、所述实时加速度以及所述静止姿态四元数,可以计算出所述待检测设备的标准角速度,根据所述标准角速度和所述起始姿态四元数,即可计算出所述待检测设备的实时标准姿态四元数。具体地,所述实时标准姿态四元数可以较为客观、真实的描述所述待检测设备的实时真实姿态四元数,因此,所述实时标准姿态四元数可以用来评价所述待评价姿态估计算法的精度。
也就是说,本发明实施例中,在所述旋转台为任意姿态时皆可计算出所述实时标准姿态四元数,实现对于所述待评价姿态估计算法的评价,这样,就解决了采用旋转台对姿态估计算法进行评价的过程中需要将旋转台水平放置的问题。这样,就可以使得所述评价方法的实施难度较低,操作较为简单。
步骤104:根据所述实时标准姿态四元数和所述实时姿态四元数之间的误差,对所述待评价姿态估计算法进行评价。
本发明实施例中,可以根据欧拉角结算方法等方法来计算所述实时标准姿态四元数和所述实时姿态四元数之间的误差,以对所述待评价姿态估计算法进行评价。具体地,在所述误差越小的情况下,所述待评价姿态估计算法的精度越高,在所述误差越大的情况下,所述待评价姿态估计算法的精度越低。
在本发明实施例中,通过将待检测设备固定至旋转台,并记录待检测设备根据待评价姿态估计算法获得的静止姿态四元数;将所述旋转台旋转至少一周,并记录所述待检测设备的实时加速度、以及所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数,其中,所述实时姿态四元数中包括起始姿态四元数;根据所述静止姿态四元数、所述实时加速度、所述实时姿态四元数和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数;根据所述实时标准姿态四元数和所述实时姿态四元数之间的误差,对所述待评价姿态估计算法进行评价。本发明实施例中,在所述旋转台为任意姿态时皆可计算出所述实时标准姿态四元数,实现对于所述待评价姿态估计算法的评价,这样,就解决了采用旋转台对姿态估计算法进行评价的过程中需要将旋转台水平放置的问题。也就是说,本发明实施例所述的评价方法实施难度较低,操作较为简单。
实施例二
参照图2,示出了本发明实施例二的一种姿态估计算法的评价方法的步骤流程图。
本发明实施例的姿态估计算法的评价方法包括以下步骤:
步骤201:将待检测设备固定至旋转台,并记录所述待检测设备根据待评价姿态估计算法获得的静止姿态四元数。
在实际应用中,步骤201的具体实施过程参照实施例一中的步骤101即可,在此不做赘述。具体地,所述静止姿态四元数可以表示为Qb。
步骤202:将所述旋转台旋转至少一周,并记录所述待检测设备的实时加速度、以及所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数,其中,所述实时姿态四元数中包括起始姿态四元数。
在实际应用中,步骤202的具体实施过程参照实施例一中的步骤102即可,在此不做赘述。具体地,所述实时加速度可以表示为W,所述实时姿态四元数可以表示为Q,所述起始姿态四元数可以表示为Qs。
步骤203:根据所述实时姿态四元数和所述实时加速度,确定所述旋转台相对于第一坐标系的实时姿态四元数。
本发明实施例中,可以根据所述实时姿态四元数和所述实时加速度,确定所述旋转台相对所述第一坐标系的实时姿态四元数,以用于后续计算所述待检测设备的标准角速度。
在本发明的一种可选实施例中,步骤202可以包括如下子步骤:
步骤S11:根据所述实时加速度,确定所述待检测设备的加速度极限位置。
本发明实施例中,所述旋转台的姿态可以为水平或者倾斜任意角度,本发明实施例仅以所述旋转台倾斜设置为例进行说明。在实际应用中,在所述旋转台倾斜设置的情况下,可以根据所述旋转台旋转一周时实时加速度判断所述待检测设备的加速度极限位置。
参照图3,示出了本发明的一种旋转台的位置示意图,其中,左边的图示意出了所述旋转台的正视图,右边的图示意出了所述旋转台的俯视图。
在实际应用中,在所述旋转台绕轴L旋转至少一周的情况下,在旋转一周内所述待检测设备的加速度极限位置可以标记为A1、A2,其中,A1表示加速度最大的位置,A2表示加速度最小的位置。如图3所示,在所述旋转台倾斜放置的情况下,所述旋转台会存在某一方向的起伏,在该方向上与所述旋转台平面相交的两端点即为所述待检测设备的加速度极限位置A1、A2。
在实际应用中,所述待检测设备在A1、A2处的实时姿态四元数可以表示为Q1、Q2。
步骤S12:根据所述待检测设备在所述加速度极限位置的实时姿态四元数,确定所述旋转台相对于第一坐标系的实时姿态四元数。
在实际应用中,在确定了所述待检测设备的加速度极限位置A1、A2后,可以根据所述待检测设备在A1、A2处的实时姿态四元数Q1、Q2来确定所述旋转台相对于第一坐标系的实时姿态四元数,以用于后续计算所述待检测设备的标准角速度。
在本发明的一种可选实施例中,步骤S12可以包括如下子步骤:
步骤S121:根据所述待检测设备在所述加速度极限位置的实时姿态四元数,确定将所述旋转台在第一坐标系下的实际转轴旋转至指定转轴的旋转轴坐标,其中,所述指定转轴为所述旋转台水平放置时的转轴。
如图3所示,所述待检测设备在A1处的实时姿态四元数Q1绕L轴旋转180°即可得到所述待检测设备在A2处的实时姿态四元数Q2。
Q2=ΔQ*Q1 (公式1)
根据公式(1),可以得到如下公式:
ΔQ=Q1*Q2-1 (公式2)
其中,ΔQ为绕轴L旋转180°的四元数表示。在实际应用中,轴L在第一坐标系下的坐标可以表示为L(x,y,z),L(x,y,z)可以用于表示所述旋转台在所述第一坐标系下的实际转轴,ΔQ可以表示为ΔQ(q0,q1,q2,q3),由于实时姿态四元数Q1、实时姿态四元数Q2已知,因此,根据公式(2),可以得出ΔQ(q0,q1,q2,q3)。
在实际应用中,ΔQ(q0,q1,q2,q3)可以表示如下:
已知θ=180°,sin(θ/2)=1,则可以得出轴L在第一坐标系下的坐标为L(q1,q2,q3)。
在实际应用中,如果所述旋转台水平的情况下,所述旋转台在所述第一坐标系下的指定转轴L’可以表示为L’(0,0,1),根据所述旋转台在所述第一坐标系下的实际转轴L的坐标L(q1,q2,q3)和指定转轴L’的坐标L’(0,0,1),即可得出将实际转轴L旋转至指定转轴L’的旋转轴坐标axis:
axis=L×L’=(q2,-q1,0) (公式4)
步骤S122:对所述旋转轴坐标进行归一化处理,得到归一化的旋转轴坐标。
在实际应用中,为了便于计算,可以对所述旋转轴坐标进行归一化处理,得到归一化的旋转轴坐标:
步骤S123:计算所述实际转轴和所述指定转轴之间的夹角。
在实际应用中,可以根据实际转轴L的坐标L(q1,q2,q3)和指定转轴L’的坐标L’(0,0,1),计算实际转轴L和指定转轴L’之间的夹角β。
步骤S124:根据所述夹角和所述归一化的旋转轴坐标,得到所述旋转台相对于所述第一坐标系的实时姿态四元数。
在实际应用中,可以根据夹角β和所述归一化的旋转轴坐标,得到所述旋转台相对于所述第一坐标系的实时姿态四元数Qx(qx0,qx1,qx2,qx3)。
本发明实施例中,通过所述旋转台相对于所述第一坐标系的实时姿态四元数Qx(qx0,qx1,qx2,qx3),在所述旋转台为任意姿态的情况上,皆可将所述旋转台上的待测设备的姿态四元素转换成所述第一坐标系下的姿态四元素,以用于后续计算出所述待检测设备的标准角速度。
步骤204:根据所述旋转台相对于所述第一坐标系的实时姿态四元数、所述实时加速度以及所述静止姿态四元数,计算出所述待检测设备的标准角速度。
本发明实施例中,根据所述旋转台相对于所述第一坐标系的实时姿态四元数Qx(qx0,qx1,qx2,qx3)和实时加速度W以及静止姿态四元数Qb,可以计算出所述待检测设备的标准角速度,以用于计算所述待检测设备的实时标准姿态四元数。
在本发明的一种可选实施例中,步骤204可以包括以下子步骤:
步骤S21:根据所述实时角速度和所述旋转台相对于所述第一坐标系的实时姿态四元数,得出所述实时角速度在所述第一坐标系中的角速度。
在实际应用中,可以设定所述旋转台的旋转为匀速旋转,且所述匀速旋转的角速度为实时角速度W,则实时角速度W在所述旋转台坐标系中的角速度可以表示为:
其中,实时角速度W为已知值。则实时角速度W在所述第一坐标系中的角速度可以表示为:
其中,Qx和已知。
步骤S22:根据所述实时角速度在所述第一坐标系中的角速度和所述静止姿态四元数,计算出所述待检测设备的标准角速度。
本发明实施例,根据所述实时角速度在所述第一坐标系中的角速度和静止姿态四元数Qb,可以计算出所述待检测设备在待检测设备坐标系中的的标准角速度
本发明实施例中,通过所述旋转台相对于所述第一坐标系的实时姿态四元数Qx(qx0,qx1,qx2,qx3),可以得到所述旋转台坐标系中的实时角速度W在所述第一坐标系中的角速度再通过静止姿态四元数Qb,可以将所述第一坐标系中的角速度在待检测设备坐标系中的的标准角速度以用于后续实时标准姿态四元数的计算。
步骤205:根据所述标准角速度和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
本发明实施例中,可以根据标准角速度和所述起始姿态四元数Qs,计算出所述待检测设备的实施标准姿态四元数,以用于对所述待评价姿态估计算法进行评价。
在本发明的一种可选实施例中,步骤205具体可以包括以下子步骤:
步骤S31:根据所述标准角速度和相邻两次采样的间隔时间,计算出相邻两次采样的旋转角度。
在实际应用中,相邻两次采样的间隔时间可以用ΔT表示,则根据标准角速度和相邻两次采样的间隔时间可以用ΔT,可以计算出相邻两次采样的旋转角度α:
步骤S32:根据所述标准角速度,得到相邻两次采样的归一化旋转轴坐标。
在实际应用中,相邻两次采样的归一化旋转轴坐标可以表示为
步骤S33:根据所述相邻两次采样的旋转角度和所述两次相邻采样的归一化旋转轴坐标,得到相邻两次采样的姿态变化量。
本发明实施例中,根据相邻两次采样的旋转角度α和两次相邻采样的归一化旋转轴坐标可以得到相邻两次采样的姿态变化量ΔQk(q0 k,q1 k,q2 k,q3 k):
步骤S34:根据所述相邻两次采样的姿态变化量和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
本发明实施例中,根据相邻两次采样的姿态变化量ΔQk(q0 k,q1 k,q2 k,q3 k)和所述起始姿态四元数Qs,可以计算出所述待检测设备的实时标准姿态四元数Qk:
Qk=QsоΔQk (公式14)
在实际应用中,可以将所述旋转台旋转N周,所述N为大于等于1的值,以获取实时标准姿态四元数Qk。
可以理解的是,计算第一个采样点的实时标准姿态四元数的起始姿态四元数Qs可以通过步骤202获得,而之后的每一个采样点的实时标准姿态四元数的起始姿态四元数Qs可以为前一个采样点的实时标准姿态四元数Qk。
步骤206:根据所述实时标准姿态四元数和所述实时姿态四元数之间的误差,对所述待评价姿态估计算法进行评价。
在实际应用中,步骤206的具体实施过程参照实施例一中的步骤104即可,在此不做赘述。
在本发明实施例中,通过将待检测设备固定至旋转台,并记录所述待检测设备根据待评价姿态估计算法获得的静止姿态四元数;将所述旋转台旋转至少一周,并记录所述待检测设备的实时加速度、以及所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数,其中,所述实时姿态四元数中包括起始姿态四元数;根据所述实时姿态四元数和所述实时加速度,确定所述旋转台相对于第一坐标系的实时姿态四元数;根据所述旋转台相对于所述第一坐标系的实时姿态四元数、所述实时加速度以及所述静止姿态四元数,计算出所述待检测设备的标准角速度;根据所述旋转台相对于所述第一坐标系的实时姿态四元数、所述实时加速度以及所述静止姿态四元数,计算出所述待检测设备的标准角速度;根据所述标准角速度和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数;根据所述实时标准姿态四元数和所述实时姿态四元数之间的误差,对所述待评价姿态估计算法进行评价。本发明实施例中,在所述旋转台为任意姿态时皆可计算出所述实时标准姿态四元数,实现对于所述待评价姿态估计算法的评价,这样,就解决了采用旋转台对姿态估计算法进行评价的过程中需要将旋转台水平放置的问题。也就是说,本发明实施例所述的评价方法实施难度较低,操作较为简单。
实施例三
参照图4,示出了本发明实施例三的一种姿态估计算法的评价装置的结构框图。
本发明实施例的姿态估计算法的评价装置400具体可以包括:
第一记录模块401,用于将待检测设备固定至旋转台,并记录所述待检测设备根据待评价姿态估计算法获得的静止姿态四元数。
第二记录模块402,用于将所述旋转台旋转至少一周,并记录所述待检测设备的实时加速度、以及所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数,其中,所述实时姿态四元数中包括起始姿态四元数。
计算模块403,用于根据所述静止姿态四元数、所述实时加速度、所述实时姿态四元数和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
评价模块404,用于根据所述实时标准姿态四元数和所述实时姿态四元数之间的误差,对所述待评价姿态估计算法进行评价。
在本发明实施例中,通过将待检测设备固定至旋转台,并记录待检测设备根据待评价姿态估计算法获得的静止姿态四元数;将所述旋转台旋转至少一周,并记录所述待检测设备的实时加速度、以及所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数,其中,所述实时姿态四元数中包括起始姿态四元数;根据所述静止姿态四元数、所述实时加速度、所述实时姿态四元数和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数;根据所述实时标准姿态四元数和所述实时姿态四元数之间的误差,对所述待评价姿态估计算法进行评价。本发明实施例中,在所述旋转台为任意姿态时皆可计算出所述实时标准姿态四元数,实现对于所述待评价姿态估计算法的评价,这样,就解决了采用旋转台对姿态估计算法进行评价的过程中需要将旋转台水平放置的问题。也就是说,本发明实施例所述的评价方法实施难度较低,操作较为简单。
实施例四
参照图5,示出了本发明实施例三的一种姿态估计算法的评价装置的结构框图。
本发明实施例的姿态估计算法的评价装置500具体可以包括:
第一记录模块501,用于将待检测设备固定至旋转台,并记录所述待检测设备根据待评价姿态估计算法获得的静止姿态四元数。
第二记录模块502,用于将所述旋转台旋转至少一周,并记录所述待检测设备的实时加速度、以及所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数,其中,所述实时姿态四元数中包括起始姿态四元数。
计算模块503,用于根据所述静止姿态四元数、所述实时加速度、所述实时姿态四元数和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
其中,计算模块503可以包括:
确定子模块5031,用于根据所述实时姿态四元数和所述实时加速度,确定所述旋转台相对于第一坐标系的实时姿态四元数。
第一计算子模块5032,用于根据所述旋转台相对于所述第一坐标系的实时姿态四元数、所述实时加速度以及所述静止姿态四元数,计算出所述待检测设备的标准角速度。
第二计算子模块5033,用于根据所述标准角速度和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
评价模块504,用于根据所述实时标准姿态四元数和所述实时姿态四元数之间的误差,对所述待评价姿态估计算法进行评价。
可选地,确定子模块5031可以包括:
第一确定单元,用于根据所述实时加速度,确定所述待检测设备的加速度极限位置。
第二确定单元,用于根据所述待检测设备在所述加速度极限位置的实时姿态四元数,确定所述旋转台相对于第一坐标系的实时姿态四元数。
可选地,所述第二确定单元包括:
第一确定子单元,用于根据所述待检测设备在所述加速度极限位置的实时姿态四元数,确定将所述旋转台在第一坐标系下的实际转轴旋转至指定转轴的旋转轴坐标,其中,所述指定转轴为所述旋转台水平放置时的转轴。
第二确定子单元,用于对所述旋转轴坐标进行归一化处理,得到归一化的旋转轴坐标。
第一计算子单元,用于计算所述实际转轴和所述指定转轴之间的夹角。
第二计算子单元,用于根据所述夹角和所述归一化的旋转轴坐标,得到所述旋转台相对于所述第一坐标系的实时姿态四元数。
可选地,第一计算子模块5032可以包括:
第一计算单元,用于根据所述实时角速度和所述旋转台相对于所述第一坐标系的实时姿态四元数,得出所述实时角速度在所述第一坐标系中的角速度。
第二计算单元,用于根据所述实时角速度在所述第一坐标系中的角速度和所述静止姿态四元数,计算出所述待检测设备的标准角速度。
可选地,第二计算子模块5033可以包括:
第三计算子单元,用于根据所述标准角速度和相邻两次采样的间隔时间,计算出相邻两次采样的旋转角度;
第四计算子单元,用于根据所述标准角速度,得到相邻两次采样的归一化旋转轴坐标;
第五计算子单元,用于根据所述相邻两次采样的旋转角度和所述两次相邻采样的归一化旋转轴坐标,得到相邻两次采样的姿态变化量;
第六计算子单元,用于根据所述相邻两次采样的姿态变化量和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
在本发明实施例中,通过将待检测设备固定至旋转台,并记录所述待检测设备根据待评价姿态估计算法获得的静止姿态四元数;将所述旋转台旋转至少一周,并记录所述待检测设备的实时加速度、以及所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数,其中,所述实时姿态四元数中包括起始姿态四元数;根据所述实时姿态四元数和所述实时加速度,确定所述旋转台相对于第一坐标系的实时姿态四元数;根据所述旋转台相对于所述第一坐标系的实时姿态四元数、所述实时加速度以及所述静止姿态四元数,计算出所述待检测设备的标准角速度;根据所述旋转台相对于所述第一坐标系的实时姿态四元数、所述实时加速度以及所述静止姿态四元数,计算出所述待检测设备的标准角速度;根据所述标准角速度和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数;根据所述实时标准姿态四元数和所述实时姿态四元数之间的误差,对所述待评价姿态估计算法进行评价。本发明实施例中,在所述旋转台为任意姿态时皆可计算出所述实时标准姿态四元数,实现对于所述待评价姿态估计算法的评价,这样,就解决了采用旋转台对姿态估计算法进行评价的过程中需要将旋转台水平放置的问题。也就是说,本发明实施例所述的评价方法实施难度较低,操作较为简单。
本发明实施例还公开了一种计算机可读存储介质,所述计算机可读存储介质存储有一个或多个指令,以执行所述姿态估计算法的评价方法中的一个或多个步骤。该计算机可读存储介质具有与上述姿态估计方法的评价方法具有相同的有益效果,此处不再赘述。
对于前述的各方法实施例,为了简单描述,故将其都表述为一系列的动作组合,但是本领域技术人员应该知悉,本发明并不受所描述的动作顺序的限制,因为依据本发明,某些步骤可以采用其他顺序或者同时进行。其次,本领域技术人员也应该知悉,说明书中所描述的实施例均属于优选实施例,所涉及的动作和模块并不一定是本发明所必须的。
本说明书中的各个实施例均采用递进的方式描述,每个实施例重点说明的都是与其他实施例的不同之处,各个实施例之间相同相似的部分互相参见即可。
最后,还需要说明的是,在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
以上对本发明所提供的一种姿态估计算法评价方法、装置以及计算机可读存储介质,进行了详细介绍,本文中应用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想;同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处,综上所述,本说明书内容不应理解为对本发明的限制。
Claims (13)
1.一种姿态估计算法的评价方法,其特征在于,包括:
将待检测设备固定至旋转台,并记录所述待检测设备根据待评价姿态估计算法获得的静止姿态四元数;
将所述旋转台旋转至少一周,并记录所述待检测设备的实时加速度、以及所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数,其中,所述实时姿态四元数中包括起始姿态四元数;
根据所述静止姿态四元数、所述实时加速度、所述实时姿态四元数和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数;
根据所述实时标准姿态四元数和所述实时姿态四元数之间的误差,对所述待评价姿态估计算法进行评价。
2.根据权利要求1所述的评价方法,其特征在于,所述根据所述静止姿态四元数、所述实时加速度、所述实时姿态四元数和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数,包括:
根据所述实时姿态四元数和所述实时加速度,确定所述旋转台相对于第一坐标系的实时姿态四元数;
根据所述旋转台相对于所述第一坐标系的实时姿态四元数、所述实时加速度以及所述静止姿态四元数,计算出所述待检测设备的标准角速度;
根据所述标准角速度和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
3.根据权利要求2所述的评价方法,其特征在于,所述根据所述待检测设备的实时姿态四元数,确定所述旋转台相对于第一坐标系的实时姿态四元数,包括:
根据所述实时加速度,确定所述待检测设备的加速度极限位置;
根据所述待检测设备在所述加速度极限位置的实时姿态四元数,确定所述旋转台相对于第一坐标系的实时姿态四元数。
4.根据权利要求3所述的评价方法,其特征在于,所述根据所述待检测设备在所述加速度极限位置的实时姿态四元数,确定所述旋转台相对于第一坐标系的实时姿态四元数,包括:
根据所述待检测设备在所述加速度极限位置的实时姿态四元数,确定将所述旋转台在第一坐标系下的实际转轴旋转至指定转轴的旋转轴坐标,其中,所述指定转轴为所述旋转台水平放置时的转轴;
对所述旋转轴坐标进行归一化处理,得到归一化的旋转轴坐标;
计算所述实际转轴和所述指定转轴之间的夹角;
根据所述夹角和所述归一化的旋转轴坐标,得到所述旋转台相对于所述第一坐标系的实时姿态四元数。
5.根据权利要求2所述的评价方法,其特征在于,所述根据所述旋转台相对于所述第一坐标系的实时姿态四元数、所述实时加速度以及所述静止姿态四元数,计算出所述待检测设备的标准角速度,包括:
根据所述实时角速度和所述旋转台相对于所述第一坐标系的实时姿态四元数,得出所述实时角速度在所述第一坐标系中的角速度;
根据所述实时角速度在所述第一坐标系中的角速度和所述静止姿态四元数,计算出所述待检测设备的标准角速度。
6.根据权利要求2所述的评价方法,其特征在于,所述根据所述标准角速度和所述起始姿态四元数估计值,计算出所述待检测设备的实时标准姿态四元数,包括:
根据所述标准角速度和相邻两次采样的间隔时间,计算出相邻两次采样的旋转角度;
根据所述标准角速度,得到相邻两次采样的归一化旋转轴坐标;
根据所述相邻两次采样的旋转角度和所述两次相邻采样的归一化旋转轴坐标,得到相邻两次采样的姿态变化量;
根据所述相邻两次采样的姿态变化量和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
7.一种姿态估计算法的评价装置,其特征在于,包括:
第一记录模块,用于将待检测设备固定至旋转台,并记录所述待检测设备根据待评价姿态估计算法获得的静止姿态四元数;
第二记录模块,用于将所述旋转台旋转至少一周,并记录所述待检测设备的实时加速度、以及所述待检测设备根据所述待评价姿态估计算法获得的实时姿态四元数,其中,所述实时姿态四元数中包括起始姿态四元数;
计算模块,用于根据所述静止姿态四元数、所述实时加速度、所述实时姿态四元数和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数;
评价模块,用于根据所述实时标准姿态四元数和所述实时姿态四元数之间的误差,对所述待评价姿态估计算法进行评价。
8.根据权利要求7所述的评价装置,其特征在于,所述计算模块包括:
确定子模块,用于根据所述实时姿态四元数和所述实时加速度,确定所述旋转台相对于第一坐标系的实时姿态四元数;
第一计算子模块,用于根据所述旋转台相对于所述第一坐标系的实时姿态四元数、所述实时加速度以及所述静止姿态四元数,计算出所述待检测设备的标准角速度;
第二计算子模块,用于根据所述标准角速度和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
9.根据权利要求8所述的评价装置,其特征在于,所述确定子模块包括:
第一确定单元,用于根据所述实时加速度,确定所述待检测设备的加速度极限位置;
第二确定单元,用于根据所述待检测设备在所述加速度极限位置的实时姿态四元数,确定所述旋转台相对于第一坐标系的实时姿态四元数。
10.根据权利要求9所述的评价装置,其特征在于,所述第二确定单元包括:
第一确定子单元,用于根据所述待检测设备在所述加速度极限位置的实时姿态四元数,确定将所述旋转台在第一坐标系下的实际转轴旋转至指定转轴的旋转轴坐标,其中,所述指定转轴为所述旋转台水平放置时的转轴;
第二确定子单元,用于对所述旋转轴坐标进行归一化处理,得到归一化的旋转轴坐标;
第一计算子单元,用于计算所述实际转轴和所述指定转轴之间的夹角;
第二计算子单元,用于根据所述夹角和所述归一化的旋转轴坐标,得到所述旋转台相对于所述第一坐标系的实时姿态四元数。
11.根据权利要求8所述的评价装置,其特征在于,所述第一计算子模块包括:
第一计算单元,用于根据所述实时角速度和所述旋转台相对于所述第一坐标系的实时姿态四元数,得出所述实时角速度在所述第一坐标系中的角速度;
第二计算单元,用于根据所述实时角速度在所述第一坐标系中的角速度和所述静止姿态四元数,计算出所述待检测设备的标准角速度。
12.根据权利要求8所述的评价装置,其特征在于,所述第二计算子模块包括:
第三计算子单元,用于根据所述标准角速度和相邻两次采样的间隔时间,计算出相邻两次采样的旋转角度;
第四计算子单元,用于根据所述标准角速度,得到相邻两次采样的归一化旋转轴坐标;
第五计算子单元,用于根据所述相邻两次采样的旋转角度和所述两次相邻采样的归一化旋转轴坐标,得到相邻两次采样的姿态变化量;
第六计算子单元,用于根据所述相邻两次采样的姿态变化量和所述起始姿态四元数,计算出所述待检测设备的实时标准姿态四元数。
13.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质存储有一个或多个指令,以执行如权利要求1~6中任一项所述的姿态估计算法的评价方法中的一个或多个步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910554605.8A CN110231054B (zh) | 2019-06-25 | 2019-06-25 | 姿态估计算法的评价方法、装置及计算机可读存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910554605.8A CN110231054B (zh) | 2019-06-25 | 2019-06-25 | 姿态估计算法的评价方法、装置及计算机可读存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110231054A true CN110231054A (zh) | 2019-09-13 |
CN110231054B CN110231054B (zh) | 2021-08-13 |
Family
ID=67857301
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910554605.8A Active CN110231054B (zh) | 2019-06-25 | 2019-06-25 | 姿态估计算法的评价方法、装置及计算机可读存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110231054B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113124905A (zh) * | 2021-04-27 | 2021-07-16 | 西安电子科技大学 | 一种用于多轴惯性姿态传感器精度评估的自动测量方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102168989A (zh) * | 2010-12-20 | 2011-08-31 | 北京航空航天大学 | 一种pos方位精度和姿态精度的地面测试方法 |
CN102997932A (zh) * | 2011-09-15 | 2013-03-27 | 北京自动化控制设备研究所 | 一种消除高精度惯导系统标定中转台抖动影响的方法 |
US20140315168A1 (en) * | 2013-02-12 | 2014-10-23 | Emotient | Facial expression measurement for assessment, monitoring, and treatment evaluation of affective and neurological disorders |
CN109631875A (zh) * | 2019-01-11 | 2019-04-16 | 京东方科技集团股份有限公司 | 一种对传感器姿态融合测量方法进行优化的方法和系统 |
-
2019
- 2019-06-25 CN CN201910554605.8A patent/CN110231054B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102168989A (zh) * | 2010-12-20 | 2011-08-31 | 北京航空航天大学 | 一种pos方位精度和姿态精度的地面测试方法 |
CN102997932A (zh) * | 2011-09-15 | 2013-03-27 | 北京自动化控制设备研究所 | 一种消除高精度惯导系统标定中转台抖动影响的方法 |
US20140315168A1 (en) * | 2013-02-12 | 2014-10-23 | Emotient | Facial expression measurement for assessment, monitoring, and treatment evaluation of affective and neurological disorders |
CN109631875A (zh) * | 2019-01-11 | 2019-04-16 | 京东方科技集团股份有限公司 | 一种对传感器姿态融合测量方法进行优化的方法和系统 |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113124905A (zh) * | 2021-04-27 | 2021-07-16 | 西安电子科技大学 | 一种用于多轴惯性姿态传感器精度评估的自动测量方法 |
Also Published As
Publication number | Publication date |
---|---|
CN110231054B (zh) | 2021-08-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108592950B (zh) | 一种单目相机和惯性测量单元相对安装角标定方法 | |
US7341221B1 (en) | Attitude determination with magnetometers for gun-launched munitions | |
WO2020253260A1 (zh) | 时间同步处理方法、电子设备及存储介质 | |
CN103900608B (zh) | 一种基于四元数ckf的低精度惯导初始对准方法 | |
CN106052682B (zh) | 一种混合式惯性导航系统及导航方法 | |
CN103697878B (zh) | 一种单陀螺单加速度计旋转调制寻北方法 | |
CN108762324A (zh) | 云台电机角度和角速度估算方法、装置、云台及飞行器 | |
CN103674023A (zh) | 一种基于陀螺精确角度关联的星敏感器动态测姿方法 | |
CN109813308A (zh) | 姿态估计方法、装置及计算机可读存储介质 | |
CN112461269B (zh) | 惯性测量单元标定方法、设备及服务器 | |
CN108132053B (zh) | 一种行人轨迹构建方法、系统及惯性测量装置 | |
CN110440797A (zh) | 车辆姿态估计方法及系统 | |
CN103644910A (zh) | 基于分段rts平滑算法的个人自主导航系统定位方法 | |
CN108917755B (zh) | 一种成像导引头视线角零位误差估计方法及装置 | |
EP3189303B1 (en) | Pedestrian navigation devices and methods | |
US20170074689A1 (en) | Sensor Fusion Method for Determining Orientation of an Object | |
CN109186634B (zh) | 一种mems惯组导航性能测量方法及装置 | |
CN106595669B (zh) | 一种旋转体姿态解算方法 | |
CN110231054A (zh) | 姿态估计算法的评价方法、装置及计算机可读存储介质 | |
US20150276975A1 (en) | Absolute vector gravimeter and methods of measuring an absolute gravity vector | |
Changey et al. | Experimental validation | |
CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
CN108972536A (zh) | 机械臂的动力学参数确定系统、方法及存储介质 | |
CN114964224B (zh) | 一种微惯导系统误差自主抑制方法 | |
CN115931001A (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 |