阅读说明：本技术 标定imu与刚体姿态的方法、设备以及可读存储介质 (method and device for calibrating IMU and rigid body posture and readable storage medium ) 是由 吴迪云 许秋子 于 2019-07-25 设计创作，主要内容包括：本申请提供了标定IMU与刚体姿态的方法、设备以及可读存储介质,用于将刚体与IMU的姿态进行融合,以提高刚体位姿的计算精度。方法包括：固定IMU与刚体之间的位置关系；获取IMU对应的原始姿态数据以及刚体对应的原始姿态数据；对IMU对应的原始姿态数据以及刚体对应的原始姿态数据进行预处理,预处理用于剔除无效数据,无效数据为IMU以及刚体未处于运动状态时的姿态数据；根据预处理后的姿态数据计算IMU坐标到刚体坐标的旋转矩阵以及IMU到世界的姿态数据；根据计算数据将IMU的姿态转换为刚体的姿态,并得出刚体的位姿。(The application provides a method, equipment and readable storage medium for calibrating postures of an IMU (inertial measurement Unit) and a rigid body, which are used for fusing the postures of the rigid body and the IMU so as to improve the calculation precision of the posture of the rigid body. The method comprises the following steps: fixing the position relation between the IMU and the rigid body; acquiring original attitude data corresponding to the IMU and original attitude data corresponding to the rigid body; preprocessing original attitude data corresponding to the IMU and original attitude data corresponding to the rigid body, wherein the preprocessing is used for eliminating invalid data, and the invalid data are attitude data when the IMU and the rigid body are not in a motion state; calculating a rotation matrix from an IMU coordinate to a rigid body coordinate and attitude data from the IMU to the world according to the preprocessed attitude data; and converting the posture of the IMU into the posture of the rigid body according to the calculation data, and obtaining the posture of the rigid body.)

标定IMU与刚体姿态的方法、设备以及可读存储介质

技术领域

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

背景技术

在虚拟现实(Virtual Reality，VR)领域以及混合现实(Mixed Reality，MR)领域，都涉及到了光学式动作捕捉的应用，通过识别跟踪物体的运动轨迹，捕捉物体的空间变化，适用于需要人机交互的应用场景。

光学式动作捕捉通过基于光学的刚***姿跟踪技术实现，以OptiTrack公司以及vicon公司的大空间VR设备为例，就是通过多个红外相机视角捕捉空间中反光球的位置，多个反光球固定在刚体上即可通过反光球的位置关系计算出刚体的位姿，进而完成动作捕捉。

其中，动作捕捉的精确度，离不开刚***姿的精确计算，而在实际应用中发现，刚***姿的计算，仍有较大的误差，因此，如何提高刚***姿计算的精确性，则是实现高精度动作捕捉的关键环节。

发明内容

本申请提供了标定IMU与刚体姿态的方法、设备以及可读存储介质，用于将刚体与惯性测量单元(Inertial Measurement Unit，IMU)的姿态进行融合，以提高刚***姿的计算精度。

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

固定IMU与刚体之间的位置关系；

获取IMU对应的原始姿态数据以及刚体对应的原始姿态数据；

对IMU对应的原始姿态数据以及刚体对应的原始姿态数据进行预处理，预处理用于剔除无效数据，无效数据为IMU以及刚体未处于运动状态时的姿态数据；

根据预处理后的姿态数据计算IMU坐标到刚体坐标的旋转矩阵以及IMU到世界的姿态数据；

根据计算数据将IMU的姿态转换为刚体的姿态，并得出刚体的位姿。

结合本申请第一方面，在本申请第一方面第一种可能的实现方式中，根据预处理后的姿态数据计算IMU坐标到刚体坐标的旋转矩阵以及IMU到世界的姿态数据包括：

从预处理后的IMU姿态数据中提取第0帧IMU姿态数据以及第n帧IMU姿态数据，并从预处理后的刚体姿态数据中提取第0帧刚体到世界的姿态数据以及第n帧刚体到世界的姿态数据；

根据关系等式，计算IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵，以及计算第0帧IMU到世界的姿态数据，关系等式为：

其中，用于指示第n帧刚体到世界的姿态数据，用于指示IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵，[R_i]_n用于指示第n帧IMU姿态数据，用于指示第0帧刚体到世界的姿态数据，[R_i]₀用于指示第0帧IMU姿态数据，用于指示第0帧IMU到世界的姿态数据，用于指示第n帧IMU姿态数据相对于第0帧IMU姿态数据的逆矩阵，E用于指示单位矩阵。

结合申请第一方面第一种可能的实现方式，在本申请第一方面第二种可能的实现方式中，根据关系等式，计算IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵，以及计算第0帧IMU到世界的姿态数据包括：

将第0帧IMU姿态数据、第n帧IMU姿态数据、第0帧刚体到世界的姿态数据以及第n帧刚体到世界的姿态数据代入关系等式，并通过最小二乘法，当关系等式的等号两边数据之差的平方和为最小时求得和最小二乘法公式Q包括：

结合本申请第一方面第一种或者第二种可能的实现方式，在本申请第一方面第三种可能的实现方式中，根据计算数据将IMU的姿态转换为刚体的姿态包括：

根据姿态转换公式将IMU的姿态转换为刚体的姿态，姿态转换公式为：

其中，为IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵的逆变换，为经过转换后的刚体的姿态数据。

结合本申请第一方面第一种可能的实现方式，在本申请第一方面第四种可能的实现方式中，第n帧IMU姿态数据以及第n帧刚体到世界的姿态数据，分别在俯仰角、航向角以及偏航角都具有至少90度的变化。

结合本申请第一方面第一种可能的实现方式，在本申请第一方面第五种可能的实现方式中，当IMU对应的原始姿态数据以及刚体对应的原始姿态数据分别用四元数表示时，第n帧IMU姿态数据以及第n帧刚体到世界的姿态数据分别具有8x组的数据量，x为正整数。

结合本申请第一方面，在本申请第一方面第六种可能的实现方式中，对IMU对应的原始姿态数据以及刚体对应的原始姿态数据进行预处理包括：

通过距离计算公式，计算IMU对应的原始姿态数据以及刚体对应的原始姿态数据中处于相邻帧和/或间隔帧的2个帧数据之间的距离，距离计算公式为：

其中，x_m、y_m以及z_m用于表示第m帧IMU或刚体姿态数据的三轴坐标，x_n、y_n以及z_n用于表示第n帧IMU或刚体姿态数据的三轴坐标，d用于表示第m帧IMU或刚体姿态数据与第n帧IMU或刚体姿态数据之间的距离；

当d小于距离阈值时，判定IMU或刚体未处于运动状态，并剔除未处于运动状态下的姿态数据，完成预处理。

第二方面，本申请提供了标定IMU与刚体姿态的设备，设备包括：

固定单元，用于固定IMU与刚体之间的位置关系；

获取单元，用于获取IMU对应的原始姿态数据以及刚体对应的原始姿态数据；

预处理单元，用于对IMU对应的原始姿态数据以及刚体对应的原始姿态数据进行预处理，预处理用于剔除无效数据，无效数据为IMU以及刚体未处于运动状态时的姿态数据；

计算单元，用于根据预处理后的姿态数据计算IMU坐标到刚体坐标的旋转矩阵以及IMU到世界的姿态数据；

转换单元，用于根据计算数据将IMU的姿态转换为刚体的姿态，并得出刚体的位姿。

结合本申请第二方面，在本申请第二方面第一种可能的实现方式中，计算单元具体用于：

从预处理后的IMU姿态数据中提取第0帧IMU姿态数据以及第n帧IMU姿态数据，并从预处理后的刚体姿态数据中提取第0帧刚体到世界的姿态数据以及第n帧刚体到世界的姿态数据；

根据关系等式，计算IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵，以及计算第0帧IMU到世界的姿态数据，关系等式为：

其中，用于指示第n帧刚体到世界的姿态数据，用于指示IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵，[R_i]_n用于指示第n帧IMU姿态数据，用于指示第0帧刚体到世界的姿态数据，[R_i]₀用于指示第0帧IMU姿态数据，用于指示第0帧IMU到世界的姿态数据，用于指示第n帧IMU姿态数据相对于第0帧IMU姿态数据的逆矩阵，E用于指示单位矩阵。

结合申请第二方面第一种可能的实现方式，在本申请第二方面第二种可能的实现方式中，计算单元具体用于：

将第0帧IMU姿态数据、第n帧IMU姿态数据、第0帧刚体到世界的姿态数据以及第n帧刚体到世界的姿态数据代入关系等式，并通过最小二乘法，当关系等式的等号两边数据之差的平方和为最小时求得和最小二乘法公式Q包括：

结合本申请第二方面第一种或者第二种可能的实现方式，在本申请第二方面第三种可能的实现方式中，计算单元具体用于：

根据姿态转换公式将IMU的姿态转换为刚体的姿态，姿态转换公式为：

其中，为IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵的逆变换，为经过转换后的刚体的姿态数据。

结合本申请第二方面第一种可能的实现方式，在本申请第二方面第四种可能的实现方式中，第n帧IMU姿态数据以及第n帧刚体到世界的姿态数据，分别在俯仰角、航向角以及偏航角都具有至少90度的变化。

结合本申请第二方面第一种可能的实现方式，在本申请第二方面第五种可能的实现方式中，当IMU对应的原始姿态数据以及刚体对应的原始姿态数据分别用四元数表示时，第n帧IMU姿态数据以及第n帧刚体到世界的姿态数据分别具有8x组的数据量，x为正整数。

结合本申请第二方面，在本申请第二方面第六种可能的实现方式中，预处理单元具体用于：

通过距离计算公式，计算IMU对应的原始姿态数据以及刚体对应的原始姿态数据中处于相邻帧和/或间隔帧的2个帧数据之间的距离，距离计算公式为：

其中，x_m、y_m以及z_m用于表示第m帧IMU或刚体姿态数据的三轴坐标，x_n、y_n以及z_n用于表示第n帧IMU或刚体姿态数据的三轴坐标，d用于表示第m帧IMU或刚体姿态数据与第n帧IMU或刚体姿态数据之间的距离；

当d小于距离阈值时，判定IMU或刚体未处于运动状态，并剔除未处于运动状态下的姿态数据，完成预处理。

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

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

从以上技术方案可以看出，本申请具有以下优点：

在固定IMU与刚体之间的位置关系后，获取IMU以及刚体分别测得的原始姿态数据，接着，对该两种原始姿态数据进行预处理，剔除数据中的无效数据，完成数据的清洗，提高姿态数据的有效性，再根据预处理后的姿态数据计算IMU坐标到刚体坐标的旋转矩阵以及IMU到世界的姿态数据，从而，根据这两种计算数据，将IMU的姿态转换为刚体的姿态，并得出刚体的位姿，实现IMU的姿态与刚体的姿态进行融合标定，提高刚***姿计算的精确性，促进高精确性的动作捕捉。

附图说明

图1示出了本申请标定IMU与刚体姿态的方法的一种流程示意图；

图2示出了本申请标定IMU与刚体姿态的方法的又一种流程示意图；

图3示出了本申请标定IMU与刚体姿态的方法的又一种流程示意图；

图4示出了本申请标定IMU与刚体姿态的设备的一种结构示意图；

图5示出了本申请标定IMU与刚体姿态的设备的又一种结构示意图。

具体实施方式

本申请提供了标定IMU与刚体姿态的方法、设备以及可读存储介质，用于将刚体与IMU的姿态进行融合，以提高刚***姿的计算精度。

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或模块的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或模块，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或模块。在本申请中出现的对步骤进行的命名或者编号，并不意味着必须按照命名或者编号所指示的时间/逻辑先后顺序执行方法流程中的步骤，已经命名或者编号的流程步骤可以根据要实现的技术目的变更执行次序，只要能达到相同或者相类似的技术效果即可。

本申请中所出现的模块的划分，是一种逻辑上的划分，实际应用中实现时可以有另外的划分方式，例如多个模块可以结合成或集成在另一个系统中，或一些特征可以忽略，或不执行，另外，所显示的或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，模块之间的间接耦合或通信连接可以是电性或其他类似的形式，本申请中均不作限定。并且，作为分离部件说明的模块或子模块可以是也可以不是物理上的分离，可以是也可以不是物理模块，或者可以分布到多个电路模块中，可以根据实际的需要选择其中的部分或全部模块来实现本申请方案的目的。

首先，在介绍本申请之前，首先介绍本申请涉及的摄像装置、IMU、刚体以及标定IMU与刚体姿态的设备。

摄像装置为光学式动作捕捉处理可能涉及的装置，包括一个或者多个的摄像头，例如全方位摄像机或者多台的运动捕捉相机，摄像装置可拍摄自身所处场景的物体。

IMU为测量物体三轴姿态角(或角速率)以及加速度的装置。通常一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，可测得物体的IMU姿态数据，包括物体在三维空间中的角速度和加速度。

刚体为在运动中和受力作用后，形状和大小不变，而且内部各点的相对位置不变的物体。

在摄像装置的摄像任务过程中，摄像装置处于或者暂时处于运动状态，并拍摄所处场景的物体，IMU以及刚体，可配置于摄像装置上，或者与摄像装置相对独立的配置，跟随着摄像装置的运动而运动，分别测得对应的原始姿态数据。当然，在实际应用中，IMU以及刚体，也可与摄像装置相互独立，在摄像装置未处于运动状态或者未参与的情况下，IMU以及刚体也可处于运动状态，并测得对应的原始姿态数据。

设备为主机、服务器或者用户设备(User Equipment，UE)等具有数据处理能力的设备，应用本申请的标定IMU与刚体姿态的方法。

UE为台式电脑、笔记本电脑、电脑一体机、平板电脑或者智能手机等终端设备。

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

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

步骤S101，固定IMU与刚体之间的位置关系；

可以理解，IMU以及刚体为硬件组件，在固定IMU与刚体之间的位置关系后，即可执行本申请的IMU与刚体姿态的方法的后续步骤。

步骤S102，获取IMU对应的原始姿态数据以及刚体对应的原始姿态数据；

可以理解，标定IMU与刚体姿态的设备可与涉及的IMU以及刚体建立连接，进而可实时地接收IMU测得的原始姿态数据以及实时地接收刚体测得的原始姿态数据，达到实时应用本申请标定IMU与刚体姿态的方法的效果。

或者，标定IMU与刚体姿态的设备也可从本次摄像任务或者位姿数据采集任务的相关存储信息中，提取本次IMU测得的原始姿态数据以及刚体测得的原始姿态数据，将本申请标定IMU与刚体姿态的方法的触发时机与本次摄像任务或者位姿数据采集任务的进行相分割开，从而可更为灵活地应用本申请的标定IMU与刚体姿态的方法。

其中，不同的摄像任务或者位姿数据采集任务，可通过不同的任务标识(Identification，ID)进行标识以及区分，若用户通过UE等设备向标定IMU与刚体姿态的设备发起任务请求，请求标定IMU与刚体姿态的设备对目标摄像任务应用本申请的标定IMU与刚体姿态的方法，则标定IMU与刚体姿态的设备可从该任务请求的预设字段中提取目标任务的任务ID，进而可从目标任务的相关存储信息中提取相对应的原始姿态数据以及原始姿态数据，并继续进行本申请标定IMU与刚体姿态的方法的后续处理。

步骤S103，对IMU对应的原始姿态数据以及刚体对应的原始姿态数据进行预处理；

其中，预处理用于剔除两种原始姿态数据中的无效数据，无效数据为IMU以及刚体未处于运动状态时的无效姿态数据。

可以理解，IMU以及刚体未运动，或者运动不明显，此时所获得的原始姿态数据是不变的，可认为是无效的姿态数据，而IMU以及刚体处于运动状态时获得的原始姿态数据才是有效的姿态数据。

因此，在得到IMU以及刚体对应的两种原始姿态数据后，标定IMU与刚体姿态的设备可对这两种原始姿态数据进行数据清洗，去除其中的无效数据，以提高姿态数据的有效性，进而可提高后续刚体姿态计算的精确性。

步骤S104，根据预处理后的姿态数据计算IMU坐标到刚体坐标的旋转矩阵以及IMU到世界的姿态数据；

在对上述两种原始姿态数据完成预处理后，标定IMU与刚体姿态的设备可根据预处理后的姿态数据，进行上述两种数据的计算，即IMU坐标到刚体坐标的旋转矩阵以及IMU到世界的姿态数据。

步骤S105，根据计算数据将IMU的姿态转换为刚体的姿态，并得出刚体的位姿。

在得到IMU坐标到刚体坐标的旋转矩阵以及IMU到世界的姿态数据后，标定IMU与刚体姿态的设备可对IMU的姿态进行坐标格式的转换，将IMU的姿态转换为刚体的姿态，实现两者在坐标格式上的标定，进而将IMU的姿态与刚体的姿态进行融合标定时，可更快速地得到刚体更为精确的位姿。

从上述内容可看出，本申请提供的标定IMU与刚体姿态的方法，在固定IMU与刚体之间的位置关系后，获取IMU以及刚体分别测得的原始姿态数据，接着，对该两种原始姿态数据进行预处理，剔除数据中的无效数据，完成数据的清洗，提高姿态数据的有效性，再根据预处理后的姿态数据计算IMU坐标到刚体坐标的旋转矩阵以及IMU到世界的姿态数据，从而，根据这两种计算数据，将IMU的姿态转换为刚体的姿态，并得出刚体的位姿，实现IMU的姿态与刚体的姿态进行融合标定，提高刚***姿计算的精确性，促进高精确性的动作捕捉。

在一种实施例中，继续参阅图2，图2示出了本申请标定IMU与刚体姿态的方法的又一种流程示意图，在本申请中，步骤S104，还可以包括如下步骤：

步骤S201，从预处理后的IMU对应的原始姿态数据中提取第0帧IMU姿态数据以及第n帧IMU姿态数据，并从预处理后的刚体对应的原始姿态数据中提取第0帧刚体到世界姿态数据以及第n帧刚体到世界的姿态数据；

可以理解，IMU的原始姿态数据中包含了多个帧的姿态数据，其中，第0帧IMU姿态数据即表示的是IMU初始测得的位置；类似的，第0帧刚体到世界的姿态数据即表示的是刚体初始测得的位置。

步骤S202，根据关系等式，计算IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵，以及计算第0帧IMU姿态数据到世界的姿态数据，关系等式为：

其中，用于指示第n帧刚体到世界的姿态数据，用于指示IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵，[R_i]_n用于指示第n帧IMU姿态数据，用于指示第0帧刚体到世界的姿态数据，[R_i]₀用于指示第0帧IMU姿态数据，用于指示第0帧IMU到世界的姿态数据，用于指示第n帧IMU姿态数据相对于第0帧IMU姿态数据的逆矩阵，E用于指示单位矩阵。

在步骤S201提取到相应数据后，标定IMU与刚体姿态的设备即可将数据代入上述的关系等式，计算得到IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵，以及第0帧IMU到世界的姿态数据。

接着，图1对应实施例步骤S105中，还可包括下述步骤：

根据姿态转换公式将IMU的姿态转换为刚体的姿态，姿态转换公式为：

其中，为IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵的逆变换，为经过转换后的刚体的姿态数据。

在计算得到IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵，以及第0帧IMU到世界的姿态数据后，即可将两者代入上述的姿态转换公式，确定IMU姿态数据从IMU坐标转化到刚体坐标时的转换公式。

可以理解，在触发本申请的标定IMU与刚体姿态的方法时，再进行上述姿态转换公式的确定，可保证IMU姿态数据从IMU坐标转化到刚体坐标时的坐标转换关系，全程符合IMU以及刚体的实际运行状态，避免IMU以及刚体在运行状态中出现移位、振动等异常情况导致IMU与刚体之间位置关系出现变化的意外情况，在保证数据高精确性的前提下，还可促使本申请的标定IMU与刚体姿态的方法，克服不同的IMU和/或刚体对应的不同实际情况所导致的实际误差，提高了稳定性以及兼容性。

在又一种实施例中，可以理解，在上述步骤S202中，代入的第n帧IMU姿态数据以及第n帧刚体姿态数据越多组，计算的结果越符合实际，即具有更高的精确性。

对应的，在实际应用中，上述步骤S202中，具体的，还可包括：

将第0帧IMU姿态数据、第n帧IMU姿态数据、第0帧刚体到世界的姿态数据以及第n帧刚体到世界的姿态数据代入上述的关系等式，并通过最小二乘法，当关系等式的等号两边数据之差的平方和为最小时求得和最小二乘法公式Q包括：

可以理解，通过最小二乘法来求解，不仅容易得到最优解，且计算工作量相对较小，可实现较低的计算时长，提高标定IMU与刚体姿态的设备在步骤S202的计算效率。

在又一种实施例中，为进一步提高目标IMU的精确性，在步骤S201中所提取的第n帧IMU姿态数据以及第n帧刚体到世界的姿态数据，分别在俯仰角、航向角以及偏航角都具有至少90度的变化，以避免可能出现的数据噪声以及克服高速采集时帧数据旋转量角度较小的情况导致误差，提高数据的质量。

在又一种实施例中，当IMU的原始姿态数据以及刚体的原始姿态数据分别用四元数表示时，第n帧IMU姿态数据以及第n帧刚体到世界的姿态数据分别具有8x组的数据量，x为正整数。

其中，优选地，x在10至15的范围内，在四元数表示格式的基础上，步骤S202中所要求解的IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵以及第0帧IMU到世界的姿态数据，具有8个待求变量，如此设置下，每个变量可有80至120组中组数的自由测试数据，不仅可求得结果，且还可带来较低的数据处理量，在精确性与数据处理量之间取得较佳的平衡。

在又一种实施例中，继续参阅图3示出的本申请标定IMU与刚体姿态的方法的又一种流程示意图，在上述步骤S103中提及的预处理，具体可包括如下步骤：

步骤S301，通过距离计算公式，计算IMU对应的原始姿态数据以及刚体对应的原始姿态数据中处于相邻帧和/或间隔帧的2个帧数据之间的距离，距离计算公式为：

其中，x_m、y_m以及z_m用于表示第m帧IMU或刚体姿态数据的三轴坐标，x_n、y_n以及z_n用于表示第n帧IMU或刚体姿态数据的三轴坐标，d用于表示第m帧IMU或刚体姿态数据与第n帧IMU或刚体姿态数据之间的距离；

可以理解，无效数据的筛选，可基于相邻帧数据的距离实现，标定IMU与刚体姿态的设备通过上述的距离计算公式，确定相邻帧和/或间隔帧的距离，该距离，即可表示相邻帧数据和/或间隔帧数据之间对应的距离变化。

步骤S302，当d小于距离阈值时，判定IMU或刚体未处于运动状态，并剔除未处于运动状态下的姿态数据，完成预处理，

其中，距离阈值可以为3cm至5cm中的任意值。

当2个帧数据之间的距离小于距离阈值时，意味着2个帧数据中的IMU或者刚体，未运动，或者运动不明显，并将2个帧数据中帧数高的帧数据识别为无标数据，将其删除或者移除，无需将2个帧数据中帧数高的帧数据作为后续刚***姿计算的输入数据，提高姿态数据的有效性以及精确性。

以上是对本申请标定IMU与刚体姿态的方法的介绍，下面开始介绍本申请的标定IMU与刚体姿态的设备。

参阅图4，图4示出了本申请标定IMU与刚体姿态的设备的一种结构示意图，具体的，标定IMU与刚体姿态的设备可包括如下结构：

固定单元401，用于固定IMU与刚体之间的位置关系；

获取单元402，用于获取IMU对应的原始姿态数据以及刚体对应的原始姿态数据；

预处理单元403，用于对IMU对应的原始姿态数据以及刚体对应的原始姿态数据进行预处理，预处理用于剔除无效数据，无效数据为IMU以及刚体未处于运动状态时的姿态数据；

计算单元404，用于根据预处理后的姿态数据计算IMU坐标到刚体坐标的旋转矩阵以及IMU到世界的姿态数据；

转换单元405，用于根据计算数据将IMU的姿态转换为刚体的姿态，并得出刚体的位姿。

在一种实施例中，计算单元404具体用于：

从预处理后的IMU姿态数据中提取第0帧IMU姿态数据以及第n帧IMU姿态数据，并从预处理后的刚体姿态数据中提取第0帧刚体到世界的姿态数据以及第n帧刚体到世界的姿态数据；

根据关系等式，计算IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵，以及计算第0帧IMU到世界的姿态数据，关系等式为：

其中，用于指示第n帧刚体到世界的姿态数据，用于指示IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵，[R_i]_n用于指示第n帧IMU姿态数据，用于指示第0帧刚体到世界的姿态数据，[R_i]₀用于指示第0帧IMU姿态数据，用于指示第0帧IMU到世界的姿态数据，用于指示第n帧IMU姿态数据相对于第0帧IMU姿态数据的逆矩阵，E用于指示单位矩阵。

在又一种实施例中，计算单元404具体用于：

将第0帧IMU姿态数据、第n帧IMU姿态数据、第0帧刚体到世界的姿态数据以及第n帧刚体到世界的姿态数据代入关系等式，并通过最小二乘法，当关系等式的等号两边数据之差的平方和为最小时求得R_ir和[R_i ^w]₀，最小二乘法公式Q包括：

在又一种实施例中，计算单元具体用于：

根据姿态转换公式将IMU的姿态转换为刚体的姿态，姿态转换公式为：

其中，为IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵的逆变换，为经过转换后的刚体的姿态数据。

在又一种实施例中，第n帧IMU姿态数据以及第n帧刚体到世界的姿态数据，分别在俯仰角、航向角以及偏航角都具有至少90度的变化。

在又一种实施例中，当IMU对应的原始姿态数据以及刚体对应的原始姿态数据分别用四元数表示时，第n帧IMU姿态数据以及第n帧刚体到世界的姿态数据分别具有8x组的数据量，x为正整数。

在又一种实施例中，预处理单元403具体用于：

通过距离计算公式，计算IMU对应的原始姿态数据以及刚体对应的原始姿态数据中处于相邻帧和/或间隔帧的2个帧数据之间的距离，距离计算公式为：

其中，x_m、y_m以及z_m用于表示第m帧IMU或刚体姿态数据的三轴坐标，x_n、y_n以及z_n用于表示第n帧IMU或刚体姿态数据的三轴坐标，d用于表示第m帧IMU或刚体姿态数据与第n帧IMU或刚体姿态数据之间的距离；

当d小于距离阈值时，判定IMU或刚体未处于运动状态，并剔除未处于运动状态下的姿态数据，完成预处理。

参阅图5，图5示出了本申请标定IMU与刚体姿态的设备的另一种结构示意图，具体的，本申请标定IMU与刚体姿态的设备包括处理器501，处理器501用于执行存储器502中存储的标定IMU与刚体姿态程序时实现如图1至图3对应任意实施例中标定IMU与刚体姿态的方法的各步骤；或者，处理器501用于执行存储器502中存储的标定IMU与刚体姿态程序时实现如图4对应实施例中各单元的功能。

示例性的，标定IMU与刚体姿态程序，该计算机程序可以被分割成一个或多个模块/单元，一个或者多个模块/单元被存储在存储器502中，并由处理器501执行，以完成本申请。一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述计算机程序在计算机装置中的执行过程。

标定IMU与刚体姿态的设备可包括，但不仅限于处理器501、存储器502。本领域技术人员可以理解，所述示意仅仅是计算机装置的示例，并不构成对标定IMU与刚体姿态的设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如标定IMU与刚体姿态的设备还可以包括输入输出设备、网络接入设备、总线等，处理器501、存储器502、输入输出设备以及网络接入设备等通过总线相连。

处理器501可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，处理器是标定IMU与刚体姿态的设备的控制中心，利用各种接口和线路连接整个标定IMU与刚体姿态的设备的各个部分。

存储器502可用于存储计算机程序和/或模块，处理器501通过运行或执行存储在存储器502内的计算机程序和/或模块，以及调用存储在存储器502内的数据，实现计算机装置的各种功能。存储器502可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、视频数据等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(FlashCard)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

本申请还提供一种可读存储介质，其上存储有标定IMU与刚体姿态程序，程序被处理器执行时实现如图1至图3对应任意实施例中的标定IMU与刚体姿态的方法。

可以理解，集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各方法实施例的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的标定IMU与刚体姿态的设备及其单元的具体工作过程，可以参考图1至图3对应实施例中标定IMU与刚体姿态的方法的说明，具体在此不再赘述。

综上所述，本申请提供的标定IMU与刚体姿态的方法、设备以及可读存储介质，在固定IMU与刚体之间的位置关系后，获取IMU以及刚体分别测得的原始姿态数据，接着，对该两种原始姿态数据进行预处理，剔除数据中的无效数据，完成数据的清洗，提高姿态数据的有效性，再根据预处理后的姿态数据计算IMU坐标到刚体坐标的旋转矩阵以及IMU到世界的姿态数据，从而，根据这两种计算数据，将IMU的姿态转换为刚体的姿态，并得出刚体的位姿，实现IMU的姿态与刚体的姿态进行融合标定，提高刚***姿计算的精确性，促进高精确性的动作捕捉。

在本申请所提供的几个实施例中，应该理解到，所揭露的标定IMU与刚体姿态的设备及其单元，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

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