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

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

Info

Publication number
CN112923923A
CN112923923A CN202110115099.XA CN202110115099A CN112923923A CN 112923923 A CN112923923 A CN 112923923A CN 202110115099 A CN202110115099 A CN 202110115099A CN 112923923 A CN112923923 A CN 112923923A
Authority
CN
China
Prior art keywords
rigid body
imu
posture
time
quaternion
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
CN202110115099.XA
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 CN202110115099.XA priority Critical patent/CN112923923A/zh
Publication of CN112923923A publication Critical patent/CN112923923A/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
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

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 BDA0002920877360000021
其中，
Figure BDA0002920877360000022
代表第n时刻的刚体和IMU姿态的四元数集合，
Figure BDA0002920877360000023
为第n时刻的刚体姿态的四元数，
Figure BDA0002920877360000024
为第n时刻的IMU姿态的四元数；
根据预设的方程组模型和所述
Figure BDA0002920877360000025
求解出旋转关系矩阵的四元数Qir；
根据预设第一公式得到所述
Figure BDA0002920877360000026
和所述
Figure BDA0002920877360000027
之间的夹角，若集合
Figure BDA0002920877360000028
中所有的
Figure BDA0002920877360000029
知
Figure BDA00029208773600000210
之间的夹角均小于一设定阈值，则判断所述Qir求解正确，否则，求解错误；
若所述Qir求解正确，则进行刚体的追踪定位，获取t时刻IMU姿态的四元数Qit和刚体的中心位置Tt；
根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt；
根据预设第三公式求取t时刻刚体标记点的位置，将所述t时刻刚体标记点的位置与实际3D建模中刚体标记点的位置进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用所述t时刻刚体标记点的位置替代所述实际3D建模中刚体标记点的位置，以获得正确的刚体位置信息。
所述根据预设的方程组模型和所述
Figure BDA00029208773600000211
求解出旋转关系矩阵的四元数Qir包括：所述预设的方程组模型为：
Figure BDA00029208773600000212
Figure BDA00029208773600000213
为四元数相乘操作，利用SVD分解求解所述方程组模型得到Qir或旋转关系矩阵Rir，其中，
Figure BDA00029208773600000214
Figure BDA00029208773600000215
所述根据预设第一公式得到所述
Figure BDA00029208773600000216
和所述
Figure BDA00029208773600000217
之间的夹角，若集合
Figure BDA00029208773600000218
中所有的
Figure BDA00029208773600000219
和
Figure BDA00029208773600000220
之间的夹角均小于一设定阈值，则判断所述Qir求解正确，否则，求解错误，包括：
根据预设第一公式：
Figure BDA00029208773600000221
算出所述
Figure BDA00029208773600000222
和所述
Figure BDA00029208773600000223
之间的夹角
Figure BDA0002920877360000031
其中，
Figure BDA0002920877360000032
为初始时刻IMU姿态的四元数的逆，
Figure BDA0002920877360000033
为旋转关系矩阵的四元数的逆，若集合
Figure BDA0002920877360000034
中所有的
Figure BDA0002920877360000035
和
Figure BDA0002920877360000036
之间的夹角dn均小于一设定阈值Td，则判断所述Qir求解正确，否则，求解错误。
所述根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt包括：
根据预设第二公式：
Figure BDA0002920877360000037
得到t时刻刚体的姿态Rrt以及对应的四元数Qrt。
所述根据预设第三公式求取t时刻刚体标记点的位置，将所述t时刻刚体标记点的位置与实际3D建模中刚体标记点的位置进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用所述t时刻刚体标记点的位置替代所述实际3D建模中刚体标记点的位置，以获得正确的刚体位置信息，包括：
根据预设第三公式：P′mt＝Rrt·pm+Tt，求取t时刻刚体标记点的位置p′mt，将所述p′mt与实际3D建模中刚体标记点的位置pmt进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用所述p′mt替代所述pmt，以获得正确的刚体位置信息。
所述根据比较结果判断刚体追踪是否成功包括：
根据所述t时刻刚体标记点的位置P′mt与所述实际3D建模中刚体标记点的位置pmt求得两者之间的距离d＝||pmt-pmt′||，若所述距离d＞Th，Th为预设阈值，则追踪成功，否则追踪失败。
若判断所述Qir求解失败，则重新固定IMU与刚体之间的位置关系，并重新采集所述刚体和IMU的姿态数据，以使所述Qir求解正确。
第二方面，本申请提供了一种IMU与刚体姿态、位置对齐的装置，所述装置包括：
固定单元，用于固定IMU与刚体之间的位置关系，记录刚体初始位置P0＝{p1，p2，...，pm}，其中，pm＝(xm，ym，zm)代表刚体标记点三维坐标数据，m为刚体标记点个数；
采集单元，用于采集所述刚体和IMU的姿态数据：
Figure BDA0002920877360000038
其中，
Figure BDA0002920877360000039
代表第n时刻的刚体和IMU姿态的四元数集合，
Figure BDA0002920877360000041
为第n时刻的刚体姿态的四元数，
Figure BDA0002920877360000042
为第n时刻的IMU姿态的四元数；
计算单元，用于根据预设的方程组模型和所述
Figure BDA0002920877360000043
求解出旋转关系矩阵的四元数Qir；根据预设第一公式得到所述
Figure BDA0002920877360000044
和所述
Figure BDA0002920877360000045
之间的夹角，若集合
Figure BDA0002920877360000046
中所有的
Figure BDA0002920877360000047
知
Figure BDA0002920877360000048
之间的夹角均小于一设定阈值，则判断所述Qir求解正确，否则，求解错误；若所述Qir求解正确，则进行刚体的追踪定位，获取t时刻IMU姿态的四元数Qit和刚体的中心位置Tt；根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt；根据预设第三公式求取t时刻刚体标记点的位置，将所述t时刻刚体标记点的位置与实际3D建模中刚体标记点的位置进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用所述t时刻刚体标记点的位置替代所述实际3D建模中刚体标记点的位置，以获得正确的刚体位置信息。
第三方面，本申请提供了一种IMU与刚体姿态、位置对齐的设备，所述设备包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的IMU与刚体姿态、位置对齐程序，所述IMU与刚体姿态、位置对齐程序被所述处理器执行时实现如上述第一方面所述的方法。
第四方面，本申请提供了一种计算机可读存储介质，其上存储有IMU与刚体姿态、位置对齐程序，所述IMU与刚体姿态、位置对齐程序被处理器执行时实现如如本申请第一方面或者第一方面任一种可能的实现方式所提供的方法。
从以上技术方案可以看出，本申请具有以下有益效果：
在固定IMU与刚体之间的位置关系后，记录刚体初始位置；采集刚体和IMU的姿态数据；根据预设的方程组模型和姿态数据求解出旋转关系矩阵的四元数Qir；根据预设第一公式得到刚体和IMU不同姿态四元数之间的夹角，若所有夹角均小于一设定阈值，则判断Qir求解正确，然后进行刚体的追踪定位，获取t时刻IMU姿态的四元数Qit和刚体的中心位置，由此根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt；最后根据预设第三公式求取t时刻刚体标记点的位置，将该位置与实际3D建模中刚体标记点的位置进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用t时刻刚体标记点的位置替代实际3D建模中刚体标记点的位置，从而获得正确的刚体位置信息。通过上述方法实现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 BDA0002920877360000071
t＝0时IMU姿态的四元数的逆为
Figure BDA0002920877360000072
IMU在t时刻的姿态为Rit，对应IMU姿态的四元数为Qit，则需要寻找对应的旋转关系矩阵Rir，对应旋转关系矩阵的四元数为Qir，以使得
Figure BDA0002920877360000073
用四元数表示为
Figure BDA0002920877360000074
其中，
Figure BDA0002920877360000075
为四元数相乘操作，刚体和IMU的姿态一般用矩阵表示，求得姿态后便可求得对应的四元数和逆。
根据四元数的左乘和右乘方法得到求取Qir的求解模型：
Figure BDA0002920877360000076
设第n时刻时：
Figure BDA0002920877360000077
其中，
Figure BDA0002920877360000078
为第n时刻的IMU姿态的四元数，
Figure BDA0002920877360000079
为第n时刻的刚体姿态的四元数，则得到齐次方程组模型：
Figure BDA0002920877360000081
即
Figure BDA0002920877360000082
可用SVD分解(奇异值分解方法)求解齐次方程组模型，从而得到Qir或Rir。
如若求得正确的旋转关系矩阵Rir或四元数Qir，那么t时刻刚体的姿态Rrt即可通过IMU就可以求得，即为：
Figure BDA0002920877360000083
或
Figure BDA0002920877360000084
由于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 BDA0002920877360000085
Figure BDA0002920877360000086
其中，
Figure BDA0002920877360000087
代表第n时刻的刚体和IMU姿态的四元数集合，
Figure BDA0002920877360000088
为第n时刻的刚体姿态的四元数，
Figure BDA0002920877360000089
为第n时刻的IMU姿态的四元数；
固定好IMU与刚体之间的位置关系之后，就可在光学多相机系统中进行标定数据的采集，包括采集绑定好的的刚体的姿态和IMU的姿态，其中的刚体和IMU姿态分别为8个以上不同的姿态，多个
Figure BDA00029208773600000810
与
Figure BDA00029208773600000811
的不同姿态之间存在比如30°以上角度的旋转关系，令采集得到的刚体和IMU的姿态数据为：
Figure BDA0002920877360000091
表示分别采集了8个以上不同的刚体和IMU姿态数据。
步骤S103，根据预设的方程组模型和
Figure BDA0002920877360000092
求解出旋转关系矩阵的四元数Qir；
通过步骤S102采集了8个以上不同的姿态数据之后，就可将第n时刻的刚体和IMU姿态的四元数集合
Figure BDA0002920877360000093
中第n时刻的刚体姿态的四元数
Figure BDA0002920877360000094
和第n时刻的IMU姿态的四元数
Figure BDA0002920877360000095
带入上述求解模型中，即
Figure BDA0002920877360000096
由此得到相应的齐次方程组模型，即为预设的方程组模型：
Figure BDA0002920877360000097
即
Figure BDA0002920877360000098
利用SVD分解求解齐次方程组模型就可得到Qir或Rir。
步骤S104，根据预设第一公式得到
Figure BDA0002920877360000099
知
Figure BDA00029208773600000910
之间的夹角，若集合
Figure BDA00029208773600000911
中所有的
Figure BDA00029208773600000912
和
Figure BDA00029208773600000913
之间的夹角均小于一设定阈值，则判断Qir求解正确，否则，求解错误；
在根据步骤S103求解出旋转关系矩阵的四元数Qir之后，还需要进一步验证求解值是否正确，以确保后续能成功进行刚体跟踪定位。
预设第一公式：
Figure BDA00029208773600000914
其中，
Figure BDA00029208773600000915
为初始时刻IMU姿态的四元数的逆(此数据在初始时刻绑定刚体和IMU位置关系之后即可获取得到)，
Figure BDA00029208773600000916
为旋转关系矩阵的四元数的逆，由此公式可得到
Figure BDA00029208773600000917
和
Figure BDA00029208773600000918
之间的夹角关系，即夹角
Figure BDA00029208773600000919
若集合
Figure BDA00029208773600000920
中所有的
Figure BDA00029208773600000921
和
Figure BDA00029208773600000922
之间的夹角dn均小于一设定阈值Td，则判断Qir求解正确，即刚体与IMU的姿态对齐成功，否则，求解错误。
步骤S105，若Qir求解正确，则进行刚体的追踪定位，获取t时刻IMU姿态的四元数Qit和刚体的中心位置Tt；
如果Qir的求解值正确，则可在光学多相机系统中进行刚体的追踪定位，假如追踪定位成功，则可获取t时刻IMU姿态的四元数Qit，以及刚体中心位置Tt。
步骤S106，根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt；
在求得Qir的正确值以及t时刻IMU姿态的四元数Qit之后，即可根据预设第二公式：
Figure BDA0002920877360000101
得到t时刻刚体的姿态Rrt以及对应的四元数Qrt。
步骤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 BDA0002920877360000111
其中，
Figure BDA0002920877360000112
代表第n时刻的刚体和IMU姿态的四元数集合，
Figure BDA0002920877360000113
为第n时刻的刚体姿态的四元数，
Figure BDA0002920877360000114
为第n时刻的IMU姿态的四元数；
计算单元203，用于根据预设的方程组模型和
Figure BDA0002920877360000115
求解出旋转关系矩阵的四元数Qir；根据预设第一公式得到
Figure BDA0002920877360000116
和
Figure BDA0002920877360000117
之间的夹角，若集合
Figure BDA0002920877360000118
中所有的
Figure BDA0002920877360000119
和
Figure BDA00029208773600001110
之间的夹角均小于一设定阈值，则判断Qir求解正确，否则，求解错误；若Qir求解正确，则进行刚体的追踪定位，获取t时刻IMU姿态的四元数Qit和刚体的中心位置Tt；根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt；根据预设第三公式求取t时刻刚体标记点的位置，将t时刻刚体标记点的位置与实际3D建模中刚体标记点的位置进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用该t时刻刚体标记点的位置替代实际3D建模中刚体标记点的位置，以获得正确的刚体位置信息。
上述装置涉及的采集、计算等过程参见上述的方法步骤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与刚体之间的位置关系，记录刚体初始位置P0＝{p1，p2，...，pm}，其中，pm＝(xm，ym，zm)代表刚体标记点三维坐标数据，m为刚体标记点个数；
采集所述刚体和IMU的姿态数据：
Figure FDA0002920877350000011
其中，
Figure FDA0002920877350000012
代表第n时刻的刚体和IMU姿态的四元数集合，
Figure FDA0002920877350000013
为第n时刻的刚体姿态的四元数，
Figure FDA0002920877350000014
为第n时刻的IMU姿态的四元数；
根据预设的方程组模型和所述
Figure FDA0002920877350000015
求解出旋转关系矩阵的四元数Qir；
根据预设第一公式得到所述
Figure FDA0002920877350000016
和所述
Figure FDA0002920877350000017
之间的夹角，若集合
Figure FDA0002920877350000018
中所有的
Figure FDA0002920877350000019
知
Figure FDA00029208773500000110
之间的夹角均小于一设定阈值，则判断所述Qir求解正确，否则，求解错误；
若所述Qir求解正确，则进行刚体的追踪定位，获取t时刻IMU姿态的四元数Qit和刚体的中心位置Tt；
根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt；
根据预设第三公式求取t时刻刚体标记点的位置，将所述t时刻刚体标记点的位置与实际3D建模中刚体标记点的位置进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用所述t时刻刚体标记点的位置替代所述实际3D建模中刚体标记点的位置，以获得正确的刚体位置信息。
2.根据权利要求1所述的方法，其特征在于，所述根据预设的方程组模型和所述
Figure FDA00029208773500000111
求解出旋转关系矩阵的四元数Qir包括：所述预设的方程组模型为：
Figure FDA00029208773500000112
Figure FDA00029208773500000113
st：||Qir||＝1，
Figure FDA00029208773500000114
为四元数相乘操作，利用SVD分解求解所述方程组模型得到Qir或旋转关系矩阵Rir，其中，
Figure FDA00029208773500000115
Figure FDA00029208773500000116
3.根据权利要求1所述的方法，其特征在于，所述根据预设第一公式得到所述
Figure FDA00029208773500000117
和所述
Figure FDA00029208773500000118
之间的夹角，若集合
Figure FDA00029208773500000119
中所有的
Figure FDA00029208773500000120
和
Figure FDA00029208773500000121
之间的夹角均小于一设定阈值，则判断所述Qir求解正确，否则，求解错误，包括：
根据预设第一公式：
Figure FDA0002920877350000021
算出所述
Figure FDA0002920877350000022
和所述
Figure FDA0002920877350000023
之间的夹角
Figure FDA0002920877350000024
其中，
Figure FDA0002920877350000025
为初始时刻IMU姿态的四元数的逆，
Figure FDA0002920877350000026
为旋转关系矩阵的四元数的逆，若集合
Figure FDA0002920877350000027
中所有的
Figure FDA0002920877350000028
和
Figure FDA0002920877350000029
之间的夹角dn均小于一设定阈值Td，则判断所述Qir求解正确，否则，求解错误。
4.根据权利要求1所述的方法，其特征在于，所述根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt包括：
根据预设第二公式：
Figure FDA00029208773500000210
得到t时刻刚体的姿态Rrt以及对应的四元数Qrt。
5.根据权利要求1所述的方法，其特征在于，所述根据预设第三公式求取t时刻刚体标记点的位置，将所述t时刻刚体标记点的位置与实际3D建模中刚体标记点的位置进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用所述t时刻刚体标记点的位置替代所述实际3D建模中刚体标记点的位置，以获得正确的刚体位置信息，包括：
根据预设第三公式：p′mt＝Rrt·pm+Tt，求取t时刻刚体标记点的位置p′mt，将所述p′mt与实际3D建模中刚体标记点的位置pmt进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用所述P′mt替代所述pmt，以获得正确的刚体位置信息。
6.根据权利要求5所述的方法，其特征在于，所述根据比较结果判断刚体追踪是否成功包括：
根据所述t时刻刚体标记点的位置p′mt与所述实际3D建模中刚体标记点的位置pmt求得两者之间的距离d＝||pmt-pmt′||，若所述距离d＞Th，Th为预设阈值，则追踪成功，否则追踪失败。
7.根据权利要求1-6任一项所述的方法，其特征在于，若判断所述Qir求解失败，则重新固定IMU与刚体之间的位置关系，并重新采集所述刚体和IMU的姿态数据，以使所述Qir求解正确。
8.一种IMU与刚体姿态、位置对齐的装置，其特征在于，所述装置包括：
固定单元，用于固定IMU与刚体之间的位置关系，记录刚体初始位置P0＝{p1，p2，...，pm}，其中，pm＝(xm，ym，zm)代表刚体标记点三维坐标数据，m为刚体标记点个数；
采集单元，用于采集所述刚体和IMU的姿态数据：
Figure FDA0002920877350000031
其中，
Figure FDA0002920877350000032
代表第n时刻的刚体和IMU姿态的四元数集合，
Figure FDA0002920877350000033
为第n时刻的刚体姿态的四元数，
Figure FDA0002920877350000034
为第n时刻的IMU姿态的四元数；
计算单元，用于根据预设的方程组模型和所述
Figure FDA0002920877350000035
求解出旋转关系矩阵的四元数Qir；根据预设第一公式得到所述
Figure FDA0002920877350000036
和所述
Figure FDA0002920877350000037
之间的夹角，若集合
Figure FDA0002920877350000038
中所有的
Figure FDA0002920877350000039
和
Figure FDA00029208773500000310
之间的夹角均小于一设定阈值，则判断所述Qir求解正确，否则，求解错误；若所述Qir求解正确，则进行刚体的追踪定位，获取t时刻IMU姿态的四元数Qit和刚体的中心位置Tt；根据预设第二公式得到t时刻刚体的姿态Rrt以及对应的四元数Qrt；根据预设第三公式求取t时刻刚体标记点的位置，将所述t时刻刚体标记点的位置与实际3D建模中刚体标记点的位置进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用所述t时刻刚体标记点的位置替代所述实际3D建模中刚体标记点的位置，以获得正确的刚体位置信息。
9.一种IMU与刚体姿态、位置对齐的设备，其特征在于，所述设备包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的IMU与刚体姿态、位置对齐程序，所述IMU与刚体姿态、位置对齐程序被所述处理器执行时实现如权利要求1至7中任一项所述的方法。
10.一种计算机可读存储介质，其特征在于，其上存储有IMU与刚体姿态、位置对齐程序，所述IMU与刚体姿态、位置对齐程序被处理器执行时实现如权利要求1至7中任一项所述的方法。
CN202110115099.XA 2021-01-28 2021-01-28 一种imu与刚体姿态、位置对齐的方法、装置、设备以及可读存储介质 Pending CN112923923A (zh)

Priority Applications (1)

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

Applications Claiming Priority (1)

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

Publications (1)

Publication Number Publication Date
CN112923923A true CN112923923A (zh) 2021-06-08

Family

ID=76167417

Family Applications (1)

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

Country Status (1)

Country Link
CN (1) CN112923923A (zh)

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 中国人民解放军军事科学院国防科技创新研究院 一种增强现实眼镜跟踪相机和人眼之间的位姿估计方法

Similar Documents

Publication Publication Date Title
JP5946924B2 (ja) シーン構造ベース自己姿勢推定
US20200264011A1 (en) Drift calibration method and device for inertial measurement unit, and unmanned aerial vehicle
Jia et al. Probabilistic 3-d motion estimation for rolling shutter video rectification from visual and inertial measurements
US9378559B2 (en) System and method for motion estimation
CN104025151B (zh) 用于创建组合图像的方法和电子装置
WO2020253260A1 (zh) 时间同步处理方法、电子设备及存储介质
CN109840884A (zh) 一种图像拼接方法、装置及电子设备
US10841570B2 (en) Calibration device and method of operating the same
CN112945231A (zh) 一种imu与刚体姿态对齐的方法、装置、设备以及可读存储介质
CN111791235B (zh) 一种机器人多相机视觉惯性点线特征定位方法及装置
CN110530356B (zh) 位姿信息的处理方法、装置、设备及存储介质
US9443349B2 (en) Electronic apparatus and method for incremental pose estimation and photographing thereof
CN113393563B (zh) 关键点自动标注的方法、系统、电子装置和存储介质
JP2023502635A (ja) 較正方法および装置、プロセッサ、電子機器、記憶媒体
CN110567484B (zh) 标定imu与刚体姿态的方法、设备以及可读存储介质
CN112819860A (zh) 视觉惯性系统初始化方法及装置、介质和电子设备
CN113516692A (zh) 一种多传感器融合的slam方法和装置
CN112991441A (zh) 相机定位方法、装置、电子设备及存储介质
CN111275769B (zh) 一种单目视觉参数的校正方法及装置
CN111955005B (zh) 处理360度图像内容的方法和系统
CN112923923A (zh) 一种imu与刚体姿态、位置对齐的方法、装置、设备以及可读存储介质
CN109451216A (zh) 一种拍摄照片的显示处理方法和装置
CN113392909B (zh) 数据处理方法、数据处理装置、终端及可读存储介质
CN110207687B (zh) 物体姿态测量方法、测量装置、电子设备及存储介质
CN110785792A (zh) 3d建模方法、电子设备、存储介质及程序产品

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