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

技术领域

本申请涉及测量领域，尤其涉及了IMU与刚体姿态对齐的方法、装置、设备以及可读存储介质。

背景技术

当前光学动作捕捉是通过基于光学的刚体位姿跟踪技术来实现，大部分大空间VR设备是通过多个红外相机视角识别空间中固定在刚体上的多个(一般三个以上反光球组成一个刚体)反光球的位置，计算出刚体的位姿，进而完成动作捕捉。IMU是一种可获取稳定姿态的惯性传感器，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计能检测物体在载体坐标系统独立三轴的加速度信号，而陀螺能检测载体相对于导航坐标系的角速度信号，由此测量出物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。

刚体位姿包括刚体的姿态和位置信息，动作捕捉的精确度，离不开刚体位姿的精确计算，而在当前的实际应用中发现，刚体位姿的计算，仍有较大的误差。特别是在刚体追踪过程中容易存在反光球被遮挡、抖动等问题，导致刚体姿态的计算存在较大的误差和不稳定，并且即使大部分时候刚体的中心位置定位较稳定、较准确的情况下，刚体的姿态仍存在剧烈的抖动。此外，刚体的姿态信息的正确性也影响着刚体光球的定位，进而影响刚体的定位。因此，如何提高刚体位姿计算的精确性，是实现高精度动作捕捉的关键环节。

发明内容

有鉴于此，如果能获取IMU稳定的姿态，再与刚体的姿态和位置进行对齐，便可提高刚体的位姿计算的正确性和稳定性。本发明主要针对提升刚体的姿态和位置计算的正确性提出一种IMU与刚体姿态、位置对齐的方法、装置、设备以及可读存储介质。

第一方面，本申请提供了一种IMU与刚体姿态对齐的方法，该方法包括：

固定IMU与刚体之间的位置关系，记录刚体初始位置P

采集所述刚体和IMU的姿态数据：

根据预设的方程组模型和所述

根据预设第一公式得到所述

若所述Q

根据预设第二公式得到t时刻刚体的姿态R

多个所述

所述根据预设的方程组模型和所述

所述根据预设第一公式得到所述

根据预设第一公式：

所述根据预设第二公式得到t时刻刚体的姿态R

根据预设第二公式：

若判断所述Q

第二方面，本申请提供了一种IMU与刚体姿态对齐的装置，所述装置包括：

固定单元，用于固定IMU与刚体之间的位置关系，记录刚体初始位置P

采集单元，用于采集所述刚体和IMU的姿态数据：

计算单元，用于根据预设的方程组模型和所述

第三方面，本申请提供了一种IMU与刚体姿态对齐的设备，所述设备包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的IMU与刚体姿态对齐程序，所述IMU与刚体姿态对齐程序被所述处理器执行时实现如上述第一方面所述的方法。

第四方面，本申请提供了一种计算机可读存储介质，其上存储有IMU与刚体姿态对齐程序，所述IMU与刚体姿态对齐程序被处理器执行时实现如如本申请第一方面或者第一方面任一种可能的实现方式所提供的方法。

从以上技术方案可以看出，本申请具有以下有益效果：

在固定IMU与刚体之间的位置关系后，记录刚体初始位置；采集刚体和IMU的姿态数据；根据预设的方程组模型和姿态数据求解出旋转关系矩阵的四元数Q

附图说明

图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时刻刚体的姿态为R

根据四元数的左乘和右乘方法得到求取Q

设第n时刻时：

如若求得正确的旋转关系矩阵R

下面，则基于上述的背景说明，开始对本申请的IMU与刚体姿态、位置对齐的方法进行详细介绍。

参阅图1，图1示出了本申请IMU与刚体姿态对齐的方法的一种流程示意图，具体的，该方法可包括如下步骤：

步骤S101，固定IMU与刚体之间的位置关系，记录刚体初始位置P

可以理解，IMU以及刚体为硬件组件，在固定IMU与刚体之间的位置关系后，就可记录刚体的初始位置P

步骤S102，采集刚体和IMU的姿态数据：

固定好IMU与刚体之间的位置关系之后，就可在光学多相机系统中进行标定数据的采集，包括采集绑定好的的刚体的姿态和IMU的姿态，其中的刚体和IMU姿态分别为8个以上不同的姿态，多个

步骤S103，根据预设的方程组模型和

通过步骤S102采集了8个以上不同的姿态数据之后，就可将第n时刻的刚体和IMU姿态的四元数集合

步骤S104，根据预设第一公式得到

在根据步骤S103求解出旋转关系矩阵的四元数Q

预设第一公式：

步骤S105，若Q

如果Q

步骤S106，根据预设第二公式得到t时刻刚体的姿态R

在求得Q

需要说明的是，在求得上述的刚体姿态后，还可以利用该刚体姿态R

步骤S107，根据预设第三公式求取t时刻刚体标记点的位置，将t时刻刚体标记点的位置与实际3D建模中刚体标记点的位置进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用该t时刻刚体标记点的位置替代实际3D建模中刚体标记点的位置，由此获得正确的刚体位置信息。

根据预设第三公式：p′

需要说明的是，根据t时刻刚体标记点的位置p′

另外，若判断Q

以上是对本申请IMU与刚体姿态、位置对齐的方法的介绍，下面开始介绍本申请的IMU与刚体姿态对齐的装置。

参阅图2，图2示出了本申请IMU与刚体姿态对齐的装置的一种结构示意图，具体的，该装置可包括如下结构：

固定单元201，用于固定IMU与刚体之间的位置关系，记录刚体初始位置P

采集单元202，用于采集刚体和IMU的姿态数据：

计算单元203，用于根据预设的方程组模型和

上述装置涉及的采集、计算等过程参见上述的方法步骤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)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。