CN110567486B - 校准3d旋转差异的数学模型构建方法、校准方法及其装置 - Google Patents

Abstract

一种校准3D旋转差异的数学模型构建方法、校准方法及其装置，该数学模型构建方法包括：获取一运动检测装置的第一检测数据和一3D测量装置的第二检测数据；预设转换函数模型，通过该转换函数模型可将第一检测数据转换为对应的第一转换数据；建立损失函数，利用非线性最小二乘法对系数q_a和系数q_b进行优化求解；将优化求解出的系数q_a和系数q_b引入至转换函数模型，建立得到校准3D旋转差异的数学模型。所构建的数学模型可以使运动检测装置的坐标方向旋转到与3D测量装置的坐标方向一致，还可以使运动检测装置、3D测量装置的零度基准位置与坐标方向均保持一致，从而为校准运动检测装置和3D测量装置之间的3D旋转差异提供了条件。

Description

校准3D旋转差异的数学模型构建方法、校准方法及其装置

技术领域

本发明涉及3D测量技术领域，具体涉及一种校准3D旋转差异的数学模型构建方法、校准方法及其装置。

背景技术

目标的姿态定位对于目标的识别以及图像的分析起着十分重要的作用，复杂背景中的目标姿态定位在军事、工业监控、交通控制管理等领域有着重要的应用。当前有采用激光测量技术对目标物体进行的三维测量的技术手段，也有利用双目立体视觉来对目标物体的三维信息进行获取的技术手段，还有利用角运动检测技术对目标物体的三维空间信息进行检测的技术手段。在实际应用中，通常借助3D测量仪对目标物体进行三维测量，以获得目标物体的三维表面信息或三维运动信息。

当前技术中，有多种测量刚体(是指形状和大小均不变的物体)的定向(或是朝向、旋转)的方式，例如：利用陀螺仪或3D测量仪来测量刚体的3D旋转状态，即位姿信息。然而，受3D测量仪的自身精度或者环境因素的影响，利用3D测量仪测量出的位姿信息往往可能存在误差，造成测量精度较低。此时，就需要借助一个高精度且校准过的陀螺仪做比对来确定3D测量仪得到的数据是否准确。但是，由于陀螺仪具有自身定义的坐标系，与3D测量仪采用的坐标系往往不一致，比如刚体往左边转90°，3D测量仪测出的刚体是朝着Y轴旋转90°，对于陀螺仪而言可能定义为朝Z轴或是X轴旋转90°，这样两者描述该90°的数值也不一样，因此会造成三维空间内的旋转差异；另外，陀螺仪和3D测量仪测量旋转的参考位置(零度基准位置)不一样时也会造成数据之间的差异。现有技术中缺乏一种简单有效的校准方式使陀螺仪和3D测量仪的坐标系及基准位置等保持一致。

发明内容

本发明主要解决的技术问题是如何对两个定向测量仪器的旋转差异进行校准，以克服三维测量结果不准确的情形。为克服上述技术问题，本申请公开一种校准3D旋转差异的数学模型构建方法、校准方法及其装置。

根据第一方面，一种实施例中提供一种校准3D旋转差异的数学模型构建方法，包括：获取一运动检测装置的第一检测数据和一3D测量装置的第二检测数据；所述运动检测装置设置于待检测物体上，其与所述3D测量装置均用于对所述待检测物体的三维运动状态进行检测；预设转换函数模型，通过所述转换函数模型可将所述第一检测数据转换为对应的第一转换数据，所述转换函数模型通过重定义所述运动检测装置的坐标系转换系数q_a和重定义所述运动检测装置的零度基准位置转换系数q_b对所述第一检测数据进行3D旋转差异转换；根据所述第一检测数据、所述第二检测数据以及第一转换数据建立损失函数，利用非线性最小二乘法对所述系数q_a和所述系数q_b进行优化求解；将优化求解出的所述系数q_a和所述系数q_b引入至所述转换函数模型，建立得到校准3D旋转差异的数学模型，所述校准3D旋转差异的数学模型用于校准所述运动检测装置和所述3D测量装置之间的3D旋转测量差异。

所述预设转换函数模型，通过所述转换函数模型可将所述第一检测数据转换为对应的第一转换数据包括：令转换函数模型表示为q_gyro＝f(q_raw)，其中，q_gyro表示为将所述第一检测数据转换后的第一转换数据，q_raw表示为所述第一检测数据。

所述转换函数模型的预设过程包括：重定义所述运动检测装置的坐标系转换系数

根据所述系数q_a对所述第一检测数据进行坐标系转换，以使所述运动检测装置的坐标方向旋转到与所述3D测量装置的坐标方向一致，得到坐标系转换后的数据：q_raw′＝q_aq_rawq_a ^-1，其中，q_raw表示所述第一检测数据，且

q_a ^-1为q_a的逆；重定义所述运动检测装置的零度基准位置转换系数

根据所述系数q_b对所述坐标系转换后的数据进行零度基准位置转换，以使所述运动检测装置、所述3D测量装置的零度基准位置与坐标方向均保持一致，得到对所述第一检测数据进行3D旋转差异转换后的数据：q_gyro＝q_bq_raw′；建立所述转换函数模型，且表示为q_gyro＝f(q_raw)＝q_bq_aq_rawq_a ^-1，以及将

作为所述第一转换数据。

所述根据所述第一检测数据、所述第二检测数据以及所述第一转换数据建立损失函数，利用非线性最小二乘法对所述系数q_a和所述系数q_b进行优化求解包括：定义损失函数为：

其中，

代表所述第二检测数据，q_raw代表所述第一检测数据，w_gyro、x_gyro、y_gyro、z_gyro代表根据所述转换函数模型和所述第一检测数据得到的所述第一转换数据，w_a、x_a、y_a、z_a及w_b、x_b、y_b、z_b分别代表所述系数q_a和所述系数q_b的数据值；将上述对应数据值代入所述损失函数中，利用非线性最小二乘法使得所述损失函数的值最小时，即可优化求解出所述系数q_a和所述系数q_b的值。

所述将求解出的所述系数q_a和所述系数q_b引入至所述转换函数模型，建立得到校准3D旋转差异的数学模型，包括：将所述优化求解出的所述系数q_a和所述系数q_b的值引入至所述转换函数模型q_gyro＝f(q_raw)，得到校准3D旋转差异的数学模型公式：

q_gyro＝f(q_raw)＝q_b′q_a′q_raw(q_a′)^-1

其中，q′_a表示为所述优化求解出的q_a的值，q′_b表示为所述优化求解出的q_b的值。

根据第二方面，一种实施例中提供一种3D旋转差异的校准方法，包括：获取一运动检测装置的第一检测数据和一3D测量装置的第二检测数据；所述运动检测装置设置于待检测物体上，其与所述3D测量装置均用于对所述待检测物体的三维运动状态进行检测；根据上述第一方面中所述的数学模型构建方法建立得到校准3D旋转差异的数学模型，所述校准3D旋转差异的数学模型用于校准所述运动检测装置和所述3D测量装置之间的3D旋转测量差异；根据所述校准3D旋转差异的数学模型，将所述第一检测数据转换为对应的第一转换数据，以完成所述运动检测装置和所述3D测量装置之间的3D旋转差异的校准，所述第一转换数据用于与所述第二检测数据进行对比，以确定所述第二检测数据的准确性。

所述第一转换数据用于与所述第二检测数据进行对比，以确定所述第二检测数据的准确性，包括：将所述第一转换数据与所述第二检测数据进行比较，根据比较结果判断所述第二检测数据是否可信，若确定所述第二检测数据不可信，则利用所述第一转换数据替代所述第二检测数据。

根据第三方面，一种实施例中提供一种3D旋转差异的校准装置，包括：获取单元，用于获取一运动检测装置的第一检测数据和一3D测量装置的第二检测数据；所述运动检测装置设置于待检测物体上，其与所述3D测量装置均用于对所述待检测物体的三维运动状态进行检测；构建单元，用于根据上述第一方面中所述的数学模型构建方法建立得到校准3D旋转差异的数学模型，所述校准3D旋转差异的数学模型用于校准所述运动检测装置和所述3D测量装置之间的3D旋转测量差异；校准单元，用于根据所述校准3D旋转差异的数学模型，将所述第一检测数据转换为对应的第一转换数据，以完成所述运动检测装置和所述3D测量装置之间的3D旋转差异的校准，所述第一转换数据用于与所述第二检测数据进行对比，以确定所述第二检测数据的准确性。

根据第四方面，一种实施例中提供一种设备，包括：存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的3D旋转差异的校准方法程序，所述3D旋转差异的校准方法程序被所述处理器执行时实现上述第二方面中所述的3D旋转差异的校准方法的步骤。

根据第五方面，一种实施例中提供一种计算机可读存储介质，包括程序，所述程序能够被处理器执行以实现上述第一方面和/或第二方面所述的方法。

本申请的有益效果是：

依据上述实施例的一种校准3D旋转差异的数学模型构建方法、校准方法及其装置，该数学模型构建方法包括：获取一运动检测装置的第一检测数据和一3D测量装置的第二检测数据；该运动检测装置设置于待检测物体上，其与该3D测量装置均用于对待检测物体的三维运动状态进行检测；预设转换函数模型，通过该转换函数模型将第一检测数据转换为对应的第一转换数据，该转换函数模型通过重定义运动检测装置的坐标系转换系数q_a和重定义运动检测装置的零度基准位置转换系数q_b对第一检测数据进行3D旋转差异转换；根据第一检测数据、第二检测数据以及第一转换数据建立损失函数，利用非线性最小二乘法对系数q_a和系数q_b进行优化求解；将优化求解出的系数q_a和系数q_b引入至转换函数模型，建立得到校准3D旋转差异的数学模型，该校准3D旋转差异的数学模型用于校准运动检测装置和所述3D测量装置之间的3D旋转测量差异。第一方面，由于利用运动检测装置的第一检测数据和3D测量装置的第二检测数据来构建数学模型，使得数学模型的建立过程尽可能地贴近于应用环境，有利于保证模型的准确性；第二方面，由于根据第一检测数据、第二检测数据和第一转换数据建立损失函数，且利用非线性最小二乘法对系数q_a和系数q_b进行优化求解，使得计算得到的系数q_a和系数q_b能够分别反映出运动检测装置与3D测量装置之间的坐标系差异和零度基准位置差异，为检测数据的转换提供精准指导；第三方面，所构建的数学模型可以使运动检测装置的坐标方向旋转到与3D测量装置的坐标方向一致，还可以使运动检测装置、3D测量装置的零度基准位置与坐标方向均保持一致，为校准运动检测装置和3D测量装置之间的3D旋转差异提供了条件；第四方面，由于在校准方法中采用了已经得到的数学模型，可以方便地校准运动检测装置和3D测量装置之间的3D旋转测量差异，使得转换后的数据能够和其他定向测量的数据做比较，确保测量数据的准确性，提升了产品开发的效率，并且不需要对数据做物理上的对齐，也为技术人员带来便利。

附图说明

图1为本申请中校准3D旋转差异的数学模型构建方法的流程图；

图2为转换函数模型建立过程的流程图；

图3为根据损失函数对系数q_a和系数q_b进行优化求解的流程图；

图4为本申请中3D旋转差异的校准方法的流程图；

图5为确定第二检测数据准确性的流程图；

图6为本申请中3D旋转差异的校准装置的结构示意图；

图7为本申请中实现校准方法程序的设备的结构示意图。

具体实施方式

下面通过具体实施方式结合附图对本发明作进一步详细说明。其中不同实施方式中类似元件采用了相关联的类似的元件标号。在以下的实施方式中，很多细节描述是为了使得本申请能被更好的理解。然而，本领域技术人员可以毫不费力的认识到，其中部分特征在不同情况下是可以省略的，或者可以由其他元件、材料、方法所替代。在某些情况下，本申请相关的一些操作并没有在说明书中显示或者描述，这是为了避免本申请的核心部分被过多的描述所淹没，而对于本领域技术人员而言，详细描述这些相关操作并不是必要的，他们根据说明书中的描述以及本领域的一般技术知识即可完整了解相关操作。

另外，说明书中所描述的特点、操作或者特征可以以任意适当的方式结合形成各种实施方式。同时，方法描述中的各步骤或者动作也可以按照本领域技术人员所能显而易见的方式进行顺序调换或调整。因此，说明书和附图中的各种顺序只是为了清楚描述某一个实施例，并不意味着是必须的顺序，除非另有说明其中某个顺序是必须遵循的。

本文中为部件所编序号本身，例如“第一”、“第二”等，仅用于区分所描述的对象，不具有任何顺序或技术含义。而本申请所说“连接”、“联接”，如无特别说明，均包括直接和间接连接(联接)。

实施例一、

请参考图1，本申请公开一种校准3D旋转差异的数学模型构建方法,该数学模型构建方法包括步骤S110-S140，下面分别说明。

步骤S110，获取一运动检测装置的第一检测数据和一3D测量装置的第二检测数据。

在本实施例中，运动检测装置设置于待检测物体上，用于对待检测物体的三维运动状态进行检测；并且，3D测量装置也用于对待检测物体的三维运动姿态进行测量。因此，运动检测装置输出的第一检测数据和3D测量装置输出的第二检测数据均为待检测物体的三维运动状态的检测数据。

需要说明的是，运动检测装置可以是角运动检测仪器，比如陀螺仪，陀螺仪是用高速回转体的动量矩敏感壳体相对惯性空间绕正交于自转轴的一个或二个轴的角运动检测装置；陀螺仪可以具体分为压电陀螺仪、微机械陀螺仪、光纤陀螺仪和激光陀螺仪，它们都是电子式的，并且它们可以和加速度计、磁阻芯片、GPS一起形成惯性导航的定向指示系统。

需要说明的是，通常测量目标物体三维运动姿态的3D测量装置采用非接触式(non-contact)的测量手段，例如借助激光、雷达、结构光栅、双目视觉图像等技术对目标物体进行形状、外观或姿态的测量。当前，市场上已有多种多样的3D测量装置，如三维影像测量仪、三维扫描仪、深度相机等，属于现有技术，因此这里不再进行详细说明。

步骤S120，预设转换函数模型，通过该转换函数模型可将第一检测数据转换为对应的第一转换数据，其中，第一转换数据此时是未知的，需要通过建立下述校准3D旋转差异的数学模型才能计算出第一转换数据，预设的转换函数模型较简单，目的是为了将未知的第一转换数据利用该转换函数模型和已知的第一检测数据表示出来，方便后续建立损失函数求解。

在本实施例中，可以令转换函数模型表示为：

q_gyro＝f(q_raw) (1-1)

其中，q_gyro表示为将第一检测数据转换后的第一转换数据，q_raw表示为所述第一检测数据。那么，就可以根据公式(1-1)将第一检测数据转换为对应的第一转换数据。

在本实施例中，可以将q_gyro、q_raw表示为四元数形式，那么转换过程中会涉及到一些四元数的运算，为便于理解四元数的运算过程，这里将对四元数进行一些解释说明。通常四元数可以理解为简单的超复数，有一个实部和三个虚部构成。

可以将单位四元数表示为：

q＝w+xi+yj+zk

或是表示为：

其中，w、x、y、z均为实数，i、j、k作为一种特殊的虚数单位参与运算，对于i、j、k本身的几何意义可以理解为一种旋转，其中i旋转代表X轴与Y轴相交平面中X轴正向向Y轴正向的旋转，j旋转代表Y轴与Z轴相交平面中Y轴正向向Z轴正向的旋转，k旋转代表Z轴与X轴相交平面中Z轴正向向X轴正向的旋转。

那么，单位四元数的逆运算表示为：q^-1＝w-xi-yj-zk，其中，-i、-j、-k分别代表i、j、k旋转的反向旋转。或者表示为：

那么，以单位四元数q₁和q₂为例来表示单位四元数的乘法运算，公式表示为：

在本实施例中，由上述定义的单位四元数可将转换函数模型通过重定义运动检测装置的坐标系转换系数q_a和重定义运动检测装置的零度基准位置转换系数q_b对第一检测数据进行3D旋转差异转换。在一具体实施例中，参考图2，转换函数模型的建立过程包括步骤S121-S123，分别说明如下。

步骤S121，重定义运动检测装置的坐标系转换系数

根据系数q_a对第一检测数据进行坐标系转换，以使运动检测装置的坐标方向旋转到与3D测量装置的坐标方向一致，坐标系转换后的数据可以表示为：

其中，q_raw表示所述第一检测数据，且

q_a ^-1为q_a的逆；并且，这里的系数q_a暂时为未知数。

需要说明的是，运动检测装置和3D测量装置之间的数据差异的原因之一是运动检测装置自身定义的坐标系(X-Y-Z方向)和3D测量装置的坐标系不一致，所以才需要将运动检测装置的坐标方向旋转到与3D测量装置的坐标方向一致。用该步骤的公式(1-2)可以看出，展开后可见w_raw保持不变，使得三个轴的系数被一个由q_a生成的旋转矩阵调整到和3D测量装置的坐标一致。

步骤S122，重定义运动检测装置的零度基准位置转换系数

根据系数q_b对坐标系转换后的数据q′_raw进行零度基准位置转换，以使运动检测装置、3D测量装置的零度基准位置与坐标方向均保持一致，从而得到将第一检测数据进行3D旋转差异转换后的数据：

q_gyro＝q_bq_raw′ (1-3)

其中，零度基准位置转换系数q_b暂时是未知数。

需要说明的是，运动检测装置和3D测量装置之间的数据差异的另一原因是零度基准位置不一致，由于旋转的偏移位置是相对于基准位置测得的，因此基准位置不一样得出的旋转结果也不一样。所以该步骤在公式(1-2)基础上乘以基准位置差异四元数，即乘以系数q_b。

步骤S123，根据上述公式建立转换函数模型，且表示为：

q_gyro＝f(q_raw)＝q_bq_aq_rawq_a ^-1 (1-4)

在本实施例中，将

作为第一转换数据，此时第一转换数据为未知，可以通过系数q_a和系数q_b以及第一检测数据表示出来。

步骤S130，根据第一检测数据、第二检测数据和第一转换数据建立损失函数，利用非线性最小二乘法对系数q_a和系数q_b进行优化求解，得到各自对应的数据值。在一具体实施例中，参见图3，该步骤S130可以包括步骤S131-S132，分别说明如下。

步骤S131，定义损失函数，用公式表示为：

其中，

代表第二检测数据；q_raw代表第一检测数据；w_gyro、x_gyro、y_gyro、z_gyro代表根据转换函数模型也即根据系数q_a和系数q_b以及第一检测数据表示的第一转换数据，w_a、x_a、y_a、z_a及w_b、x_b、y_b、z_b分别代表系数q_a和系数q_b的数据值。

需要说明的是，因为q_gyro是关于q_a、q_b的表达式，那么对公式(1-5)展开之后，就使得公式(1-5)变为关于未知量w_a、x_a、y_a、z_b和未知量w_b、x_b、y_b、z_a的表达式，由此可以对该未知量进行优化求解。

步骤S132，将上述对应数据值w_3D、x_3D、y_3D、z_3D、w_gyro、x_gyro、y_gyro、z_gyro、w_a、x_a、y_a、z_a及w_b、x_b、y_b、z_b代入损失函数公式(1-5)中，利用非线性最小二乘法使得该损失函数的值最小时，即可优化求解出w_a、x_a、y_a、z_b和w_b、x_b、y_b、z_a，即可得到系数q_a和系数q_b的求解值。

需要说明的是，在公式(1-5)中，为使得Loss的值达到最小，通过最后的两个计算项需要确保系数q_a和系数q_b为单位四元数，即

和

分别等于1。理论上，两组对应的q_3D和q_raw数据可以利用Ceres Solver库等计算出系数q_a和系数q_b的具体取值，但是由于噪声和其他不精准原因，数据应越多越好，数据中最好是有三维空间3个自由度的旋转。

需要说明的是，非线性最小二乘法是以误差的平方和最小为准则来估计非线性静态模型参数的一种参数估计方法，常常使用非线性最小二乘法来最优化问题，比如使用Ceres Solver库(开源的C++库)来建模和解决大型复杂的优化问题，尤其是针对具有边界约束的非线性最小二乘问题。关于使用Ceres Solver库来解决上述损失函数的优化问题，可以参考Ceres Solver库的使用流程：(1)定义残差函数f_i，定义一个代价函数的结构体(struct)，在结构体内重载运算符；(2)创建一个Cost_Function，且把f_i作为其参数；(3)增加一个残差块(Problem.AddResidualBlock)；(4)问题求解(Problem.Solve)。由于使用非线性最小二乘法优化求解参数的过程属于现有技术，所以这里不再进行详细说明。

步骤S140，将优化求解出的系数q_a和系数q_b的值引入至步骤S120中预设的转换函数模型q_gyro＝f(q_raw)中，从而得到校准3D旋转差异的数学模型，用公式表示为：

q_gyro＝f(q_raw)＝q_b′q_a′q_raw(q_a′)^-1 (1-6)

其中，q′_a表示为优化求解出的q_a的值，q′_b表示为优化求解出的q_b的值。可以理解，该3D旋转差异的数学模型用于校准运动检测装置和3D测量装置之间的3D旋转测量差异。

实施例二、

请参考图4，在实施例一中公开的校准3D旋转差异的数学模型构建方法的基础上，本申请还公开一种3D旋转差异的校准方法，该校准方法包括步骤S210-S240。

步骤S210，获取一运动检测装置的第一检测数据和一3D测量装置的第二检测数据。

在本实施例中，运动检测装置可以设置于待检测物体上，用于对待检测物体的三维运动状态进行检测；并且，3D测量装置可以对准待检测物体，同样用于对待检测物体的三维运动状态进行检测。那么，第一检测数据可以表示为

第二检测数据可以表示为

步骤S220，根据实施例一中的数学模型构建方法建立得到校准3D旋转差异的数学模型，该校准3D旋转差异的数学模型用于校准运动检测装置和3D测量装置之间的3D旋转测量差异。

需要说明的是，建立得到校准3D旋转差异的数学模型的表达式为：

q_gyro＝f(q_raw)＝q_b′q_a′q_raw(q_a′)^-1

其中，q′_a表示为优化求解出的q_a的值，q′_b表示为优化求解出的q_b的值。关于该数学模型的构建过程可以具体参考实施一中的步骤S110-S140，这里不再进行赘述。

步骤S230，根据校准3D旋转差异的数学模型，将第一检测数据转换为对应的第一转换数据，以完成运动检测装置和3D测量装置之间的3D旋转差异的校准。

在本实施例中，将第一检测数据

代入至校准3D旋转差异的数学模型中，即可计算得到第一转换数据q_gyro，且表示为

第一转换数据便作为经过校准使运动检测装置和3D测量装置之间的坐标方向及零度基准位置保持一致后的目标数据，获得第一转换数据后，即代表完成了运动检测装置和3D测量装置之间的3D旋转差异的校准。

步骤S240，将第一转换数据与第二检测数据进行对比，以确定第二检测数据q_3D的准确性。在一具体实施例中，见图5，步骤S240可以包括步骤S241-S244，分别说明如下。

步骤S241，将第一转换数据q_gyro与第二检测数据q_3D进行比较，得到比较结果。步骤S242，根据比较结果判断第二检测数据q_3D是否可信，若确定第二检测数据q_3D不可信，则进入步骤S244，反之进入步骤S243。

在一具体实施例中，比较结果，例如两者差值可与设定的一阈值范围进行对比，若在阈值范围内则判断第二检测数据q_3D可信，反之则不可信。

步骤S243，确定第二检测数据q_3D可信，则直接输出第二检测数据q_3D。

步骤S244，确定二检测数据q_3D不可信时，用第一转换数据q_gyro替代第二检测数据q_3D以进行输出，如此，即实现了校准运动检测装置和3D测量装置之间的3D旋转测量差异的目的。

实施例三、

请参考图6，本申请公开一种3D旋转差异的校准装置3，该校准装置主要包括获取单元31、构建单元32和校准单元33，下面分别说明。

获取单元31用于获取一运动检测装置的第一检测数据和一3D测量装置的第二检测数据。可见图6，这里的运动检测装置设置于待检测物体上，用于对待检测物体的三维运动状态进行检测；并且，3D测量装置设置于待检测物体的一侧，也用于对待检测物体的三维运动状态进行测量。关于获取单元31的具体功能，可以参考步骤S210中的相关内容，这里不再进行详细说明。

构建单元32与获取单元31连接，用于根据实施例一中公开的数学模型构建方法得到校准3D旋转差异的数学模型。该校准3D旋转差异的数学模型用于校准运动检测装置和3D测量装置之间的3D旋转测量差异。关于构建单元32的具体功能，可以参考步骤S220中的相关内容，这里不再进行详细说明。

校准单元33与构建单元32连接，用于根据已经建立的校准3D旋转差异的数学模型，将第一检测数据转换为对应的第一转换数据，以完成运动检测装置和3D测量装置之间的3D旋转差异的校准。其中，第一转换数据用于与第二检测数据进行对比，以确定第二检测数据的准确性。关于校准单元33的具体功能，可以参考步骤S230-S240中的相关内容，这里不再进行详细说明。

实施例四、

请参考图7，本申请还公开一种设备4，该设备4可以包括存储器41和处理器42，以及包括存储在存储器41上并可在处理器42上运行的3D旋转差异的校准方法程序。此外需要说明的是，这里的3D旋转差异的校准方法程序能够在被处理器42执行时实现实施例二中关于3D旋转差异的校准方法的步骤(即步骤S210—S240)。

需要说明的是，在本实施例中，存储器41上存储的3D旋转差异的校准方法程序可以包括一些功能模块，如实施例三中请求保护的获取单元31、构建单元32和校准单元33。其中，获取单元31用于获取一运动检测装置的第一检测数据和一3D测量装置的第二检测数据；构建单元32与获取单元31连接，用于根据实施例一中公开的数学模型构建方法建立得到校准3D旋转差异的数学模型；校准单元33与构建单元32连接，用于根据已经建立的校准3D旋转差异的数学模型，将第一检测数据转换为对应的第一转换数据，以完成运动检测装置和3D测量装置之间的3D旋转差异的校准。

本领域技术人员可以理解，上述实施方式中各种方法的全部或部分功能可以通过硬件的方式实现，也可以通过计算机程序的方式实现。当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器、随机存储器、磁盘、光盘、硬盘等，通过计算机执行该程序以实现上述功能。例如，将程序存储在设备的存储器中，当通过处理器执行存储器中程序，即可实现上述全部或部分功能。另外，当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序也可以存储在服务器、另一计算机、磁盘、光盘、闪存盘或移动硬盘等存储介质中，通过下载或复制保存到本地设备的存储器中，或对本地设备的系统进行版本更新，当通过处理器执行存储器中的程序时，即可实现上述实施方式中全部或部分功能。

以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。

Claims (8)

1.一种校准3D旋转差异的数学模型构建方法，其特征在于，包括：

获取一运动检测装置的第一检测数据和一3D测量装置的第二检测数据；所述运动检测装置设置于待检测物体上，其与所述3D测量装置均用于对所述待检测物体的三维运动状态进行检测；

预设转换函数模型，通过所述转换函数模型可将所述第一检测数据转换为对应的第一转换数据，所述转换函数模型通过重定义所述运动检测装置的坐标系转换系数q_a和重定义所述运动检测装置的零度基准位置转换系数q_b对所述第一检测数据进行3D旋转差异转换；其中，所述预设转换函数模型，通过所述转换函数模型可将所述第一检测数据转换为对应的第一转换数据包括：

令转换函数模型表示为q_gyro＝f(q_raw)，其中，q_gyro表示为将所述第一检测数据转换后的第一转换数据，q_raw表示为所述第一检测数据；

所述转换函数模型的预设过程包括：

重定义所述运动检测装置的坐标系转换系数

根据所述系数q_a对所述第一检测数据进行坐标系转换，以使所述运动检测装置的坐标方向旋转到与所述3D测量装置的坐标方向一致，得到坐标系转换后的数据：q_raw′q_aq_rawq_a ^-1，其中，q_raw表示所述第一检测数据，且

q_a ^-1为q_a的逆；

重定义所述运动检测装置的零度基准位置转换系数

根据所述系数q_b对所述坐标系转换后的数据进行零度基准位置转换，以使所述运动检测装置、所述3D测量装置的零度基准位置与坐标方向均保持一致，得到对所述第一检测数据进行3D旋转差异转换后的数据：q_gyro＝q_bq_raw′；

建立所述转换函数模型，且表示为q_gyro＝f(q_raw)＝q_bq_aq_rawq_a ^-1，以及将

作为所述第一转换数据；

根据所述第一检测数据、所述第二检测数据以及所述第一转换数据建立损失函数，利用非线性最小二乘法对所述系数q_a和所述系数q_b进行优化求解；

将优化求解出的所述系数q_a和所述系数q_b引入至所述转换函数模型，建立得到校准3D旋转差异的数学模型，所述校准3D旋转差异的数学模型用于校准所述运动检测装置和所述3D测量装置之间的3D旋转测量差异。

2.如权利要求1所述的数学模型构建方法，其特征在于，所述根据所述第一检测数据、所述第二检测数据以及所述第一转换数据建立损失函数，利用非线性最小二乘法对所述系数q_a和所述系数q_b进行优化求解包括：

定义损失函数为：

其中，

代表所述第二检测数据，q_raw代表所述第一检测数据，w_gyro、x_gyro、y_gyro、z_gyro代表根据所述转换函数模型和所述第一检测数据得到的所述第一转换数据，w_a、x_a、y_a、z_a及w_b、x_b、y_b、z_b分别代表所述系数q_a和所述系数q_b的数据值；

将上述对应数据值代入所述损失函数中，利用非线性最小二乘法使得所述损失函数的值最小时，即可优化求解出所述系数q_a和所述系数q_b的值。

3.如权利要求2所述的数学模型构建方法，其特征在于，所述将求解出的所述系数q_a和所述系数q_b引入至所述转换函数模型，建立得到校准3D旋转差异的数学模型，包括：

将所述优化求解出的所述系数q_a和所述系数q_b的值引入至所述转换函数模型q_gyro＝f(q_raw)，得到校准3D旋转差异的数学模型，用公式表示为：

q_gyro＝f(q_raw)＝q_b′q_a′q_raw(q_a′)^-1

其中，q′_a表示为所述优化求解出的q_a的值，q′_b表示为所述优化求解出的q_b的值。

4.一种3D旋转差异的校准方法，其特征在于，包括：

获取一运动检测装置的第一检测数据和一3D测量装置的第二检测数据；所述运动检测装置设置于待检测物体上，其与所述3D测量装置均用于对所述待检测物体的三维运动状态进行检测；

根据权利要求1-3中任一项所述的数学模型构建方法建立得到校准3D旋转差异的数学模型，所述校准3D旋转差异的数学模型用于校准所述运动检测装置和所述3D测量装置之间的3D旋转测量差异；

根据所述校准3D旋转差异的数学模型，将所述第一检测数据转换为对应的第一转换数据，以完成所述运动检测装置和所述3D测量装置之间的3D旋转差异的校准，所述第一转换数据用于与所述第二检测数据进行对比，以确定所述第二检测数据的准确性。

5.如权利要求4所述的校准方法，其特征在于，所述第一转换数据用于与所述第二检测数据进行对比，以确定所述第二检测数据的准确性，包括：

将所述第一转换数据与所述第二检测数据进行比较，根据比较结果判断所述第二检测数据是否可信，若确定所述第二检测数据不可信，则利用所述第一转换数据替代所述第二检测数据。

6.一种3D旋转差异的校准装置，其特征在于，包括：

获取单元，用于获取一运动检测装置的第一检测数据和一3D测量装置的第二检测数据；所述运动检测装置设置于待检测物体上，其与所述3D测量装置均用于对所述待检测物体的三维运动状态进行检测；

构建单元，用于根据权利要求1-3中任一项所述的数学模型构建方法建立得到校准3D旋转差异的数学模型，所述校准3D旋转差异的数学模型用于校准所述运动检测装置和所述3D测量装置之间的3D旋转测量差异；

校准单元，用于根据所述校准3D旋转差异的数学模型，将所述第一检测数据转换为对应的第一转换数据，以完成所述运动检测装置和所述3D测量装置之间的3D旋转差异的校准，所述第一转换数据用于与所述第二检测数据进行对比，以确定所述第二检测数据的准确性。

7.一种3D旋转差异的校准设备，其特证在于，包括：存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的3D旋转差异的校准方法程序，所述3D旋转差异的校准方法程序被所述处理器执行时实现如权利要求4-5中任一项所述的3D旋转差异的校准方法的步骤。

8.一种计算机可读存储介质，其特征在于，包括程序，所述程序能够被处理器执行以实现如权利要求1-5中任一项所述的方法。

Images