CN112945231A - 一种imu与刚体姿态对齐的方法、装置、设备以及可读存储介质 - Google Patents

一种imu与刚体姿态对齐的方法、装置、设备以及可读存储介质 Download PDF

Info

Publication number
CN112945231A
CN112945231A CN202110117061.6A CN202110117061A CN112945231A CN 112945231 A CN112945231 A CN 112945231A CN 202110117061 A CN202110117061 A CN 202110117061A CN 112945231 A CN112945231 A CN 112945231A
Authority
CN
China
Prior art keywords
rigid body
imu
posture
quaternion
solving
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
CN202110117061.6A
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.)
Shenzhen Realis Multimedia Technology Co Ltd
Original Assignee
Shenzhen Realis Multimedia 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 Shenzhen Realis Multimedia Technology Co Ltd filed Critical Shenzhen Realis Multimedia Technology Co Ltd
Priority to CN202110117061.6A priority Critical patent/CN112945231A/zh
Publication of CN112945231A publication Critical patent/CN112945231A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • 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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本申请提供了一种IMU与刚体姿态对齐的方法，包括：固定IMU与刚体之间的位置关系，记录刚体初始位置；采集刚体和IMU的姿态数据；根据预设的方程组模型求解出旋转关系矩阵的四元数Qir；然后根据预设的多个公式得到t时刻刚体的姿态，从而获得正确的刚***姿信息。通过该方法实现IMU的姿态与刚体的姿态的融合标定以及刚体与IMU的对齐，提高了刚***姿计算的精确性，最终实现稳定和校正刚体的姿态与位置的测量数据。

Description

一种IMU与刚体姿态对齐的方法、装置、设备以及可读存储 介质
技术领域
本申请涉及测量领域，尤其涉及了IMU与刚体姿态对齐的方法、装置、设备以及可读存储介质。
背景技术
当前光学动作捕捉是通过基于光学的刚***姿跟踪技术来实现，大部分大空间VR设备是通过多个红外相机视角识别空间中固定在刚体上的多个(一般三个以上反光球组成一个刚体)反光球的位置，计算出刚体的位姿，进而完成动作捕捉。IMU是一种可获取稳定姿态的惯性传感器，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计能检测物体在载体坐标***独立三轴的加速度信号，而陀螺能检测载体相对于导航坐标系的角速度信号，由此测量出物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。
刚***姿包括刚体的姿态和位置信息，动作捕捉的精确度，离不开刚***姿的精确计算，而在当前的实际应用中发现，刚***姿的计算，仍有较大的误差。特别是在刚体追踪过程中容易存在反光球被遮挡、抖动等问题，导致刚体姿态的计算存在较大的误差和不稳定，并且即使大部分时候刚体的中心位置定位较稳定、较准确的情况下，刚体的姿态仍存在剧烈的抖动。此外，刚体的姿态信息的正确性也影响着刚体光球的定位，进而影响刚体的定位。因此，如何提高刚***姿计算的精确性，是实现高精度动作捕捉的关键环节。
发明内容
有鉴于此，如果能获取IMU稳定的姿态，再与刚体的姿态和位置进行对齐，便可提高刚体的位姿计算的正确性和稳定性。本发明主要针对提升刚体的姿态和位置计算的正确性提出一种IMU与刚体姿态、位置对齐的方法、装置、设备以及可读存储介质。
第一方面，本申请提供了一种IMU与刚体姿态对齐的方法，该方法包括：
固定IMU与刚体之间的位置关系，记录刚体初始位置P0＝{p1，p2，...，pm}，其中，pm＝(xm，ym，zm)代表刚体标记点三维坐标数据，m为刚体标记点个数；
采集所述刚体和IMU的姿态数据：
Figure BDA0002921017260000021
n≥8，其中，
Figure BDA0002921017260000022
代表第n时刻的刚体和IMU姿态的四元数集合，
Figure BDA0002921017260000023
为第n时刻的刚体姿态的四元数，
Figure BDA0002921017260000024
为第n时刻的IMU姿态的四元数；
根据预设的方程组模型和所述
Figure BDA0002921017260000025
求解出旋转关系矩阵的四元数Qir；
根据预设第一公式得到所述
Figure BDA0002921017260000026
和所述
Figure BDA0002921017260000027
之间的夹角，若集合
Figure BDA0002921017260000028
中所有的
Figure BDA0002921017260000029
和
Figure BDA00029210172600000210
之间的夹角均小于一设定阈值，则判断所述Qir求解正确，否则，求解错误；
若所述Qir求解正确，则进行刚体的追踪定位，获取t时刻IMU姿态的四元数Qit和刚体的中心位置Tt；
根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt。
多个所述
Figure BDA00029210172600000211
与
Figure BDA00029210172600000212
的不同姿态之间存在30°以上角度的旋转关系。
所述根据预设的方程组模型和所述
Figure BDA00029210172600000213
求解出旋转关系矩阵的四元数Qir包括：所述预设的方程组模型为：
Figure BDA00029210172600000214
st：||Qir||＝1，
Figure BDA00029210172600000215
为四元数相乘操作，利用SVD分解求解所述方程组模型得到Qir或旋转关系矩阵Rir，其中，
Figure BDA00029210172600000216
Figure BDA00029210172600000217
所述根据预设第一公式得到所述
Figure BDA00029210172600000218
和所述
Figure BDA00029210172600000219
之间的夹角，若集合
Figure BDA00029210172600000220
中所有的
Figure BDA00029210172600000221
和
Figure BDA00029210172600000222
之间的夹角均小于一设定阈值，则判断所述Qir求解正确，否则，求解错误，包括：
根据预设第一公式：
Figure BDA00029210172600000223
算出所述
Figure BDA00029210172600000224
和所述
Figure BDA00029210172600000225
之间的夹角
Figure BDA00029210172600000226
其中，
Figure BDA00029210172600000227
为初始时刻IMU姿态的四元数的逆，
Figure BDA00029210172600000228
为旋转关系矩阵的四元数的逆，若集合
Figure BDA00029210172600000229
中所有的
Figure BDA00029210172600000230
和
Figure BDA00029210172600000231
之间的夹角dn均小于一设定阈值Td，则判断所述Qir求解正确，否则，求解错误。
所述根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt包括：
根据预设第二公式：
Figure BDA0002921017260000031
得到t时刻刚体的姿态Rrt以及对应的四元数Qrt。
若判断所述Qir求解失败，则重新固定IMU与刚体之间的位置关系，并重新采集所述刚体和IMU的姿态数据，以使所述Qir求解正确。
第二方面，本申请提供了一种IMU与刚体姿态对齐的装置，所述装置包括：
固定单元，用于固定IMU与刚体之间的位置关系，记录刚体初始位置P0＝{p1，p2，...，pm}，其中，pm＝(xm，ym，zm)代表刚体标记点三维坐标数据，m为刚体标记点个数；
采集单元，用于采集所述刚体和IMU的姿态数据：
Figure BDA0002921017260000032
n≥8，其中，
Figure BDA0002921017260000033
代表第n时刻的刚体和IMU姿态的四元数集合，
Figure BDA0002921017260000034
为第n时刻的刚体姿态的四元数，
Figure BDA0002921017260000035
为第n时刻的IMU姿态的四元数；
计算单元，用于根据预设的方程组模型和所述
Figure BDA0002921017260000036
求解出旋转关系矩阵的四元数Qir；根据预设第一公式得到所述
Figure BDA0002921017260000037
和所述
Figure BDA0002921017260000038
之间的夹角，若集合
Figure BDA0002921017260000039
中所有的
Figure BDA00029210172600000310
和
Figure BDA00029210172600000311
之间的夹角均小于一设定阈值，则判断所述Qir求解正确，否则，求解错误；若所述Qir求解正确，则进行刚体的追踪定位，获取t时刻IMU姿态的四元数Qit和刚体的中心位置Tt；根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt。
第三方面，本申请提供了一种IMU与刚体姿态对齐的设备，所述设备包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的IMU与刚体姿态对齐程序，所述IMU与刚体姿态对齐程序被所述处理器执行时实现如上述第一方面所述的方法。
第四方面，本申请提供了一种计算机可读存储介质，其上存储有IMU与刚体姿态对齐程序，所述IMU与刚体姿态对齐程序被处理器执行时实现如如本申请第一方面或者第一方面任一种可能的实现方式所提供的方法。
从以上技术方案可以看出，本申请具有以下有益效果：
在固定IMU与刚体之间的位置关系后，记录刚体初始位置；采集刚体和IMU的姿态数据；根据预设的方程组模型和姿态数据求解出旋转关系矩阵的四元数Qir；根据预设第一公式得到刚体和IMU不同姿态四元数之间的夹角，若所有夹角均小于一设定阈值，则判断Qir求解正确，然后进行刚体的追踪定位，获取t时刻IMU姿态的四元数Qit和刚体的中心位置，由此根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt。通过上述方法实现IMU的姿态与刚体的姿态的融合标定以及刚体与IMU的对齐，提高刚***姿计算的精确性，最终实现稳定和校正刚体的姿态与位置的测量数据。
附图说明
图1示出了本申请一种IMU与刚体姿态对齐的方法的流程示意图；
图2示出了本申请一种IMU与刚体姿态对齐的装置的一种结构示意图；
图3示出了本申请一种IMU与刚体姿态对齐的设备的一种结构示意图。
具体实施方式
本申请提供了一种IMU与刚体姿态、位置对齐的方法、装置、设备以及可读存储介质，用于将刚体与IMU的姿态进行融合标定和对齐，以提高刚***姿的计算精度。
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或模块的过程、方法、***、产品或设备不必限于清楚地列出的那些步骤或模块，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或模块。在本申请中出现的对步骤进行的命名或者编号，并不意味着必须按照命名或者编号所指示的时间/逻辑先后顺序执行方法流程中的步骤，已经命名或者编号的流程步骤可以根据要实现的技术目的变更执行次序，只要能达到相同或者相类似的技术效果即可。
本申请中所出现的模块的划分，是一种逻辑上的划分，实际应用中实现时可以有另外的划分方式，例如多个模块可以结合成或集成在另一个***中，或一些特征可以忽略，或不执行，另外，所显示的或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，模块之间的间接耦合或通信连接可以是电性或其他类似的形式，本申请中均不作限定。并且，作为分离部件说明的模块或子模块可以是也可以不是物理上的分离，可以是也可以不是物理模块，或者可以分布到多个电路模块中，可以根据实际的需要选择其中的部分或全部模块来实现本申请方案的目的。
首先，在介绍本申请之前，首先介绍本申请涉及的摄像装置、IMU、刚体以及标定IMU与刚体姿态的设备。
摄像装置为光学式动作捕捉处理可能涉及的装置，包括一个或者多个的摄像头，例如全方位摄像机或者多台的动作捕捉相机，摄像装置可拍摄自身所处场景的物体。
IMU为测量物体三轴姿态角(或角速率)以及加速度的装置。通常一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标***独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，可测得物体的IMU姿态数据，包括物体在三维空间中的角速度和加速度。
刚体为在运动中和受力作用后，形状和大小不变，而且内部各点的相对位置不变的物体。
在摄像装置的摄像任务过程中，摄像装置处于或者暂时处于运动状态，并拍摄所处场景的物体，IMU以及刚体，可配置于摄像装置上，或者与摄像装置相对独立的配置，跟随着摄像装置的运动而运动，分别测得对应的原始姿态数据。当然，在实际应用中，IMU以及刚体，也可与摄像装置相互独立，在摄像装置未处于运动状态或者未参与的情况下，IMU以及刚体也可处于运动状态，并测得对应的原始姿态数据。
设备为主机、服务器或者用户设备(User Equipment，UE)等具有数据处理能力的设备，应用本申请的标定IMU与刚体姿态的方法。
UE为台式电脑、笔记本电脑、电脑一体机、平板电脑或者智能手机等终端设备。
其次，介绍下如何根据现有的方程组模型，通过融合IMU与刚体的姿态数据得到刚体稳定的姿态信息。
要得到刚体的姿态，需要对IMU与刚体姿态进行对齐，此时需要对相机坐标系和IMU坐标系进行对齐，设t时刻刚体的姿态为Rrt，对应刚体的姿态的四元数为Qrt，设t＝0时在摄像装置上绑定刚体时，刚体姿态单位矩阵(也即初始姿态)为Rr0，t＝0时IMU的姿态(也即初始姿态)为Ri0，则对应的逆为
Figure BDA0002921017260000061
t＝0时IMU姿态的四元数的逆为
Figure BDA0002921017260000062
IMU在t时刻的姿态为Rit，对应IMU姿态的四元数为Qit，则需要寻找对应的旋转关系矩阵Rir，对应旋转关系矩阵的四元数为Qir，以使得
Figure BDA0002921017260000063
用四元数表示为
Figure BDA0002921017260000064
其中，
Figure BDA0002921017260000065
为四元数相乘操作，刚体和IMU的姿态一般用矩阵表示，求得姿态后便可求得对应的四元数和逆。
根据四元数的左乘和右乘方法得到求取Qir的求解模型：
Figure BDA0002921017260000066
设第n时刻时：
Figure BDA0002921017260000067
其中，
Figure BDA0002921017260000068
为第n时刻的IMU姿态的四元数，
Figure BDA0002921017260000069
为第n时刻的刚体姿态的四元数，则得到齐次方程组模型：
Figure BDA0002921017260000071
即
Figure BDA0002921017260000072
st：||Qir||＝1，可用SVD分解(奇异值分解方法)求解齐次方程组模型，从而得到Qir或Rir。
如若求得正确的旋转关系矩阵Rir或四元数Qir，那么t时刻刚体的姿态Rrt即可通过IMU就可以求得，即为：
Figure BDA0002921017260000073
或
Figure BDA0002921017260000074
由于IMU的姿态数据比较准确和稳定，因此只要通过合适方式获得准确的Qir或Rir数据，再结合上述方法求得的刚体的姿态也比较准确和稳定。
下面，则基于上述的背景说明，开始对本申请的IMU与刚体姿态、位置对齐的方法进行详细介绍。
参阅图1，图1示出了本申请IMU与刚体姿态对齐的方法的一种流程示意图，具体的，该方法可包括如下步骤：
步骤S101，固定IMU与刚体之间的位置关系，记录刚体初始位置P0＝{p1，p2，...，pm}，其中，pm＝(xm，ym，zm)代表刚体标记点三维坐标数据，m为刚体标记点个数；
可以理解，IMU以及刚体为硬件组件，在固定IMU与刚体之间的位置关系后，就可记录刚体的初始位置P0＝{p1，p2，...，pm}，其中，pm＝(xm，ym，zm)代表刚体标记点(也即反光球)的三维坐标数据，m为刚体标记点个数，接下来便可执行本申请的IMU与刚体姿态对齐的方法的后续步骤。
步骤S102，采集刚体和IMU的姿态数据：
Figure BDA0002921017260000075
n≥8，其中，
Figure BDA0002921017260000076
代表第n时刻的刚体和IMU姿态的四元数集合，
Figure BDA0002921017260000077
为第n时刻的刚体姿态的四元数，
Figure BDA0002921017260000078
为第n时刻的IMU姿态的四元数；
固定好IMU与刚体之间的位置关系之后，就可在光学多相机***中进行标定数据的采集，包括采集绑定好的的刚体的姿态和IMU的姿态，其中的刚体和IMU姿态分别为8个以上不同的姿态，多个
Figure BDA0002921017260000079
与
Figure BDA00029210172600000710
的不同姿态之间存在比如30°以上角度的旋转关系，令采集得到的刚体和IMU的姿态数据为：
Figure BDA0002921017260000081
n≥8，表示分别采集了8个以上不同的刚体和IMU姿态数据。
步骤S103，根据预设的方程组模型和
Figure BDA0002921017260000082
求解出旋转关系矩阵的四元数Qir；
通过步骤S102采集了8个以上不同的姿态数据之后，就可将第n时刻的刚体和IMU姿态的四元数集合
Figure BDA0002921017260000083
中第n时刻的刚体姿态的四元数
Figure BDA0002921017260000084
和第n时刻的IMU姿态的四元数
Figure BDA0002921017260000085
带入上述求解模型中，即
Figure BDA0002921017260000086
由此得到相应的齐次方程组模型，即为预设的方程组模型：
Figure BDA0002921017260000087
即
Figure BDA0002921017260000088
st：||Qir||＝1，利用SVD分解求解齐次方程组模型就可得到Qir或Rir。
步骤S104，根据预设第一公式得到
Figure BDA0002921017260000089
和
Figure BDA00029210172600000810
之间的夹角，若集合
Figure BDA00029210172600000811
中所有的
Figure BDA00029210172600000812
和
Figure BDA00029210172600000813
之间的夹角均小于一设定阈值，则判断Qir求解正确，否则，求解错误；
在根据步骤S103求解出旋转关系矩阵的四元数Qir之后，还需要进一步验证求解值是否正确，以确保后续能成功进行刚体跟踪定位。
预设第一公式：
Figure BDA00029210172600000814
，其中，
Figure BDA00029210172600000815
为初始时刻IMU姿态的四元数的逆(此数据在初始时刻绑定刚体和IMU位置关系之后即可获取得到)，
Figure BDA00029210172600000816
为旋转关系矩阵的四元数的逆，由此公式可得到
Figure BDA00029210172600000817
和
Figure BDA00029210172600000818
之间的夹角关系，即夹角
Figure BDA00029210172600000819
若集合
Figure BDA00029210172600000820
中所有的
Figure BDA00029210172600000821
和
Figure BDA00029210172600000822
之间的夹角dn均小于一设定阈值Td，则判断Qir求解正确，即刚体与IMU的姿态对齐成功，否则，求解错误。
步骤S105，若Qir求解正确，则进行刚体的追踪定位，获取t时刻IMU姿态的四元数Qit和刚体的中心位置Tt；
如果Qir的求解值正确，则可在光学多相机***中进行刚体的追踪定位，假如追踪定位成功，则可获取t时刻IMU姿态的四元数Qit，以及刚体中心位置Tt。
步骤S106，根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt；
在求得Qir的正确值以及t时刻IMU姿态的四元数Qit之后，即可根据预设第二公式：
Figure BDA0002921017260000091
得到t时刻刚体的姿态Rrt以及对应的四元数Qrt。
需要说明的是，在求得上述的刚体姿态后，还可以利用该刚体姿态Rrt对刚体的位置进行校正，详见步骤S107：
步骤S107，根据预设第三公式求取t时刻刚体标记点的位置，将t时刻刚体标记点的位置与实际3D建模中刚体标记点的位置进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用该t时刻刚体标记点的位置替代实际3D建模中刚体标记点的位置，由此获得正确的刚***置信息。
根据预设第三公式：p′mt＝Rrt·pm+Tt，可求取t时刻刚体标记点的位置p′mt，pm为步骤S101中刚体标记点的三维坐标数据，将p′mt与实际3D建模中刚体标记点的位置pmt进行比较，即求距离，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用p′mt替代pmt，从而获得正确的刚***置信息。
需要说明的是，根据t时刻刚体标记点的位置p′mt与实际3D建模中刚体标记点的位置pmt可求得两者之间的距离d＝||pmt-pmt′||，若距离d＞Th，Th为预设阈值，则追踪成功，否则追踪失败。如果追踪成功，则可以用来校正3D建模中求取的刚体标记点位置，实现刚体与IMU的融合，最终实现通过利用IMU的姿态来稳定和校正刚***姿的结果。
另外，若判断Qir求解错误，则返回步骤S101，重新固定IMU与刚体之间的位置关系，并重新采集刚体和IMU的姿态数据，以使Qir求解正确。
以上是对本申请IMU与刚体姿态、位置对齐的方法的介绍，下面开始介绍本申请的IMU与刚体姿态对齐的装置。
参阅图2，图2示出了本申请IMU与刚体姿态对齐的装置的一种结构示意图，具体的，该装置可包括如下结构：
固定单元201，用于固定IMU与刚体之间的位置关系，记录刚体初始位置P0＝{p1，p2，...，pm}，其中，pm＝(xm，ym，zm)代表刚体标记点三维坐标数据，m为刚体标记点个数；
采集单元202，用于采集刚体和IMU的姿态数据：
Figure BDA0002921017260000101
n≥8，其中，
Figure BDA0002921017260000102
代表第n时刻的刚体和IMU姿态的四元数集合，
Figure BDA0002921017260000103
为第n时刻的刚体姿态的四元数，
Figure BDA0002921017260000104
为第n时刻的IMU姿态的四元数；
计算单元203，用于根据预设的方程组模型和
Figure BDA0002921017260000105
求解出旋转关系矩阵的四元数Qir；根据预设第一公式得到
Figure BDA0002921017260000106
和
Figure BDA0002921017260000107
之间的夹角，若集合
Figure BDA0002921017260000108
中所有的
Figure BDA0002921017260000109
和
Figure BDA00029210172600001010
之间的夹角均小于一设定阈值，则判断Qir求解正确，否则，求解错误；若Qir求解正确，则进行刚体的追踪定位，获取t时刻IMU姿态的四元数Qit和刚体的中心位置Tt；根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt。
上述装置涉及的采集、计算等过程参见上述的方法步骤S101-S107，此处不再赘述。
请参阅图3，图3示出了本申请提供的一种IMU与刚体姿态对齐的设备，具体的，该设备包括处理器301、存储器302以及存储在存储器302上并可在处理器301上运行的IMU与刚体姿态对齐程序，IMU与刚体姿态对齐程序被处理器301执行时实现如图1对应的任意实施例中IMU与刚体姿态对齐的方法的各步骤。
示例性的，IMU与刚体姿态对齐程序，该计算机程序可以被分割成一个或多个模块/单元，一个或者多个模块/单元被存储在存储器302中，并由处理器301执行，以完成本申请。一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述计算机程序在计算机装置中的执行过程。
IMU与刚体姿态对齐的设备可包括，但不仅限于处理器301、存储器302。本领域技术人员可以理解，所述示意仅仅是计算机装置的示例，并不构成对IMU与刚体姿态对齐的设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如IMU与刚体姿态对齐的设备还可以包括输入输出设备、网络接入设备、总线等，处理器301、存储器302、输入输出设备以及网络接入设备等通过总线相连。
处理器301可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，处理器是IMU与刚体姿态对齐的设备的控制中心，利用各种接口和线路连接整个IMU与刚体姿态对齐的设备的各个部分。
存储器302可用于存储计算机程序和/或模块，处理器301通过运行或执行存储在存储器302内的计算机程序和/或模块，以及调用存储在存储器302内的数据，实现计算机装置的各种功能。存储器302可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作***、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、视频数据等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(FlashCard)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
本申请还提供一种可读存储介质，其上存储有IMU与刚体姿态对齐程序，该程序被处理器执行时实现如图1对应任意实施例中的IMU与刚体姿态对齐的方法。
可以理解，集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各方法实施例的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。

Claims (10)

1.一种IMU与刚体姿态对齐的方法，其特征在于，所述方法包括：
固定IMU与刚体之间的位置关系，记录刚体初始位置；
采集所述刚体和IMU的姿态数据：
Figure FDA0002921017250000011
n≥8，其中，
Figure FDA0002921017250000012
代表第n时刻的刚体和IMU姿态的四元数集合，
Figure FDA0002921017250000013
为第n时刻的刚体姿态的四元数，
Figure FDA0002921017250000014
为第n时刻的IMU姿态的四元数；
根据预设的方程组模型和所述
Figure FDA0002921017250000015
求解出旋转关系矩阵的四元数Qir；
根据预设第一公式得到所述
Figure FDA0002921017250000016
和所述
Figure FDA0002921017250000017
之间的夹角，若集合
Figure FDA0002921017250000018
中所有的
Figure FDA0002921017250000019
和
Figure FDA00029210172500000110
之间的夹角均小于一设定阈值，则判断所述Qir求解正确，否则，求解错误；
若所述Qir求解正确，则进行刚体的追踪定位，获取t时刻IMU姿态的四元数Qit；
根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt。
2.根据权利要求1所述的方法，其特征在于，所述刚体初始位置为P0＝{p1，p2，...，pm}，其中，pm＝(xm，ym，zm)代表刚体标记点三维坐标数据，m为刚体标记点个数。
3.根据权利要求1所述的方法，其特征在于，多个所述
Figure FDA00029210172500000111
与
Figure FDA00029210172500000112
的不同姿态之间存在30°以上角度的旋转关系。
4.根据权利要求1所述的方法，其特征在于，所述所述根据预设的方程组模型和所述
Figure FDA00029210172500000113
求解出旋转关系矩阵的四元数Qir包括：所述预设的方程组模型为：
Figure FDA00029210172500000114
Figure FDA00029210172500000115
为四元数相乘操作，利用SVD分解求解所述方程组模型得到Qir或旋转关系矩阵Rir，其中，
Figure FDA00029210172500000116
5.根据权利要求1所述的方法，其特征在于，所述根据预设第一公式得到所述
Figure FDA00029210172500000117
和所述
Figure FDA00029210172500000118
之间的夹角，若集合
Figure FDA00029210172500000119
中所有的
Figure FDA00029210172500000120
和
Figure FDA00029210172500000121
之间的夹角均小于一设定阈值，则判断所述Qir求解正确，否则，求解错误，包括：
根据预设第一公式：
Figure FDA00029210172500000122
算出所述
Figure FDA00029210172500000123
和所述
Figure FDA00029210172500000124
之间的夹角
Figure FDA0002921017250000021
其中，
Figure FDA0002921017250000022
为初始时刻IMU姿态的四元数的逆，
Figure FDA0002921017250000023
为旋转关系矩阵的四元数的逆，若集合
Figure FDA0002921017250000024
中所有的
Figure FDA0002921017250000025
和
Figure FDA0002921017250000026
之间的夹角dn均小于一设定阈值Td，则判断所述Qir求解正确，否则，求解错误。
6.根据权利要求1所述的方法，其特征在于，所述根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt包括：
根据预设第二公式：
Figure FDA0002921017250000027
得到t时刻刚体的姿态Rrt以及对应的四元数Qrt。
7.根据权利要求1-6任一项所述的方法，其特征在于，若判断所述Qir求解错误，则重新固定IMU与刚体之间的位置关系，并重新采集所述刚体和IMU的姿态数据，以使所述Qir求解正确。
8.一种IMU与刚体姿态对齐的装置，其特征在于，所述装置包括：
固定单元，用于固定IMU与刚体之间的位置关系，记录刚体初始位置；
采集单元，用于采集所述刚体和IMU的姿态数据：
Figure FDA0002921017250000028
n≥8，其中，
Figure FDA0002921017250000029
代表第n时刻的刚体和IMU姿态的四元数集合，
Figure FDA00029210172500000210
为第n时刻的刚体姿态的四元数，
Figure FDA00029210172500000211
为第n时刻的IMU姿态的四元数；
计算单元，用于根据预设的方程组模型和所述
Figure FDA00029210172500000212
求解出旋转关系矩阵的四元数Qir；根据预设第一公式得到所述
Figure FDA00029210172500000213
和所述
Figure FDA00029210172500000214
之间的夹角，若集合
Figure FDA00029210172500000215
中所有的
Figure FDA00029210172500000216
和
Figure FDA00029210172500000217
之间的夹角均小于一设定阈值，则判断所述Qir求解正确，否则，求解错误；若所述Qir求解正确，则进行刚体的追踪定位，获取t时刻IMU姿态的四元数Qit；根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt。
9.一种IMU与刚体姿态对齐的设备，其特征在于，所述设备包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的IMU与刚体姿态对齐程序，所述IMU与刚体姿态对齐程序被所述处理器执行时实现如权利要求1至7中任一项所述的方法。
10.一种计算机可读存储介质，其特征在于，其上存储有IMU与刚体姿态对齐程序，所述IMU与刚体姿态对齐程序被处理器执行时实现如权利要求1至7中任一项所述的方法。
CN202110117061.6A 2021-01-28 2021-01-28 一种imu与刚体姿态对齐的方法、装置、设备以及可读存储介质 Pending CN112945231A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110117061.6A CN112945231A (zh) 2021-01-28 2021-01-28 一种imu与刚体姿态对齐的方法、装置、设备以及可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110117061.6A CN112945231A (zh) 2021-01-28 2021-01-28 一种imu与刚体姿态对齐的方法、装置、设备以及可读存储介质

Publications (1)

Publication Number Publication Date
CN112945231A true CN112945231A (zh) 2021-06-11

Family

ID=76238520

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110117061.6A Pending CN112945231A (zh) 2021-01-28 2021-01-28 一种imu与刚体姿态对齐的方法、装置、设备以及可读存储介质

Country Status (1)

Country Link
CN (1) CN112945231A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115388911A (zh) * 2022-08-24 2022-11-25 Oppo广东移动通信有限公司 光学动作捕捉***的精度测量方法、装置、电子设备
TWI812369B (zh) * 2021-07-28 2023-08-11 宏達國際電子股份有限公司 控制方法、追蹤系統以及非暫態電腦可讀取媒體

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102162847A (zh) * 2011-01-14 2011-08-24 中国人民解放军理工大学 一种基于奇异值分解的高效航迹相关方法
CN102542604A (zh) * 2010-09-22 2012-07-04 卡西欧计算机株式会社 Ar处理装置和ar处理方法
JP2015227813A (ja) * 2014-05-30 2015-12-17 アニマ株式会社 センサーモジュールの位置の取得方法及び装置、及び、動作計測方法及び装置
CN105549057A (zh) * 2015-12-07 2016-05-04 韩厚增 一种惯性辅助的gps/bds融合大比例尺快速宗地测量装置与方法
US20160273919A1 (en) * 2014-10-09 2016-09-22 Honeywell International Inc. Systems and methods for producing two independent dissimilar attitude solutions, two independent dissimilar inertial solutions or both from one improved navigation device
CN106871859A (zh) * 2017-01-03 2017-06-20 电子科技大学 一种刚体空间定轴转动角度检测方法
CN107941212A (zh) * 2017-11-14 2018-04-20 杭州德泽机器人科技有限公司 一种视觉与惯性联合定位方法
CN108605098A (zh) * 2016-05-20 2018-09-28 深圳市大疆创新科技有限公司 用于卷帘快门校正的***和方法
CN108629793A (zh) * 2018-03-22 2018-10-09 中国科学院自动化研究所 使用在线时间标定的视觉惯性测程法与设备
CN110956666A (zh) * 2019-11-12 2020-04-03 深圳市瑞立视多媒体科技有限公司 运动数据标定方法、装置、终端设备及存储介质
CN110986925A (zh) * 2019-12-02 2020-04-10 北京自动化控制设备研究所 初始姿态最优估计方法
CN111882608A (zh) * 2020-07-14 2020-11-03 中国人民解放军军事科学院国防科技创新研究院 一种增强现实眼镜跟踪相机和人眼之间的位姿估计方法

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102542604A (zh) * 2010-09-22 2012-07-04 卡西欧计算机株式会社 Ar处理装置和ar处理方法
CN102162847A (zh) * 2011-01-14 2011-08-24 中国人民解放军理工大学 一种基于奇异值分解的高效航迹相关方法
JP2015227813A (ja) * 2014-05-30 2015-12-17 アニマ株式会社 センサーモジュールの位置の取得方法及び装置、及び、動作計測方法及び装置
US20160273919A1 (en) * 2014-10-09 2016-09-22 Honeywell International Inc. Systems and methods for producing two independent dissimilar attitude solutions, two independent dissimilar inertial solutions or both from one improved navigation device
CN105549057A (zh) * 2015-12-07 2016-05-04 韩厚增 一种惯性辅助的gps/bds融合大比例尺快速宗地测量装置与方法
CN108605098A (zh) * 2016-05-20 2018-09-28 深圳市大疆创新科技有限公司 用于卷帘快门校正的***和方法
CN106871859A (zh) * 2017-01-03 2017-06-20 电子科技大学 一种刚体空间定轴转动角度检测方法
CN107941212A (zh) * 2017-11-14 2018-04-20 杭州德泽机器人科技有限公司 一种视觉与惯性联合定位方法
CN108629793A (zh) * 2018-03-22 2018-10-09 中国科学院自动化研究所 使用在线时间标定的视觉惯性测程法与设备
CN110956666A (zh) * 2019-11-12 2020-04-03 深圳市瑞立视多媒体科技有限公司 运动数据标定方法、装置、终端设备及存储介质
CN110986925A (zh) * 2019-12-02 2020-04-10 北京自动化控制设备研究所 初始姿态最优估计方法
CN111882608A (zh) * 2020-07-14 2020-11-03 中国人民解放军军事科学院国防科技创新研究院 一种增强现实眼镜跟踪相机和人眼之间的位姿估计方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
TWI812369B (zh) * 2021-07-28 2023-08-11 宏達國際電子股份有限公司 控制方法、追蹤系統以及非暫態電腦可讀取媒體
CN115388911A (zh) * 2022-08-24 2022-11-25 Oppo广东移动通信有限公司 光学动作捕捉***的精度测量方法、装置、电子设备

Similar Documents

Publication Publication Date Title
US20200250429A1 (en) Attitude calibration method and device, and unmanned aerial vehicle
EP3028252B1 (en) Rolling sequential bundle adjustment
US20200264011A1 (en) Drift calibration method and device for inertial measurement unit, and unmanned aerial vehicle
US9378559B2 (en) System and method for motion estimation
CN104025151B (zh) 用于创建组合图像的方法和电子装置
US10841570B2 (en) Calibration device and method of operating the same
CN110567484B (zh) 标定imu与刚体姿态的方法、设备以及可读存储介质
CN110717861B (zh) 图像拼接方法、装置、电子设备和计算机可读存储介质
CN112945231A (zh) 一种imu与刚体姿态对齐的方法、装置、设备以及可读存储介质
WO2013130208A1 (en) Scene structure-based self-pose estimation
CN109840884A (zh) 一种图像拼接方法、装置及电子设备
CN110956666B (zh) 运动数据标定方法、装置、终端设备及存储介质
CN106570907B (zh) 一种相机标定方法及装置
JP2023502635A (ja) 較正方法および装置、プロセッサ、電子機器、記憶媒体
CN113393563B (zh) 关键点自动标注的方法、***、电子装置和存储介质
WO2018053756A1 (zh) 一种图像检测方法及终端
CN113409391A (zh) 视觉定位方法及相关装置、设备和存储介质
US20160163091A1 (en) Electronic apparatus and method for incremental pose estimation and photographing thereof
CN113516692A (zh) 一种多传感器融合的slam方法和装置
JP2023502192A (ja) 視覚的ポジショニング方法および関連装置、機器並びにコンピュータ可読記憶媒体
CN112991441A (zh) 相机定位方法、装置、电子设备及存储介质
CN113407030B (zh) 视觉定位方法及相关装置、设备和存储介质
CN111275769B (zh) 一种单目视觉参数的校正方法及装置
CN110991085B (zh) 一种机器人图像仿真数据的构建方法、介质、终端和装置
EP3718302B1 (en) Method and system for handling 360 degree image content

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