CN111486867B - 一种视觉和惯性混合跟踪组件安装参数的标定装置及方法 - Google Patents

Abstract

本发明公开了一种视觉和惯性跟踪组件安装参数的标定装置和方法，该装置包括相机安装支架、目标安装支架、二维转台、数据采集模块、标定计算机，通过将所有二维转台的转动位置进行分组和迭代处理，完成一组安装参数解算后，控制二维转台继续完成下一组转动位置，重复上述数据采集和安装参数解算过程，并将上一组解算的安装参数值作为下一组安装参数的理论值。重复上述过程，直至满足迭代次数阈值或混合跟踪精度的结束条件。该标定装置和方法利用二维转台的分组转动和自动数据采集，实现了非线性约束方程的迭代线性求解，提高了标定效率。

Description

一种视觉和惯性混合跟踪组件安装参数的标定装置及方法

技术领域

本发明属于机器人和VR/AR等领域中的运动跟踪技术，涉及一种视觉和惯性混合跟踪组件安装参数的标定装置及方法。

背景技术

运动跟踪技术用于确定运动目标相对于所在环境的位移和姿态等运动信息，是机器人和VR/AR等领域中的重要研究内容之一。目前主要有电磁场、超声波、计算机视觉和惯性等不同的测量方法，每一种方法都有其优缺点，如电磁跟踪精度易受环境磁场畸变影响；超声波跟踪数据更新率较低，动态性能差；视觉跟踪易受遮挡限制等。混合跟踪方法是将两种以上的跟踪方法进行优势互补，通过信息融合处理得到优于单一跟踪方法的跟踪性能。

视觉和惯性混合跟踪是将视觉跟踪相机通过支架安装在环境的特定位置处，将预先设计好的视觉跟踪标识和惯性测量单元(Inertial Measurement Unit，IMU)通过支架安装在运动目标的特定位置上，在目标运动时，实时获取视觉跟踪相机输出的标识图像数据和IMU输出的角速度和加速度数据，利用混合跟踪算法解算出运动目标相对于所处环境的位姿信息。

视觉和惯性混合跟踪组件包括视觉跟踪标识、相机和IMU，假设其内部参数均经过了预先单独标定，标识、IMU与跟踪相机的安装支架均经过预先设计和加工，但在安装时均存在不同程度的误差，该安装参数误差对混合跟踪系统精度会产生严重影响，尤其对于高精度跟踪应用来说，该安装参数误差的影响不能忽略，需要通过精确标定加以消除。

目前已有的视觉和惯性混合跟踪系统标定装置及方法，仅对跟踪相机内外参数或IMU和相机之间的相对位姿进行单独标定，且认为采用的标识的位置和姿态精确已知，而不考虑标识的实际安装误差。

因此，对于许多尤其是高精度的运动跟踪应用来说，目前的标定装置和方法无法满足使用要求，有必要提出新的标定装置和方法，以获得视觉和惯性混合跟踪组件的精确安装参数，进而提高混合跟踪精度。

发明内容

本发明的目的是为了满足高精度运动跟踪应用的需求，提出一种视觉和惯性混合跟踪组件安装参数的标定装置和方法。

本发明的技术方案是：

一种采用标定装置进行视觉和惯性混合跟踪组件安装参数的标定方法：该标定装置包括相机安装支架(1)、目标安装支架(2)、二维转台(3)、数据采集模块(4)和标定计算机(5)；

相机安装支架(1)用于固定跟踪相机，使其光轴朝向某个合适的方向；

目标安装支架(2)用于将被跟踪目标固定安装在二维转台(3)的基准面上，以保证目标坐标系与转台坐标系之间实现对准；

二维转台(3)固定安装在跟踪环境的特定位置，且预先进行当地地理坐标系进行对准；

在被跟踪目标上的不同位置安装有视觉标识和IMU器件；

数据采集模块(4)用于采集二维转台的方位角和俯仰角数据与视觉和惯性混合跟踪系统的姿态角测量数据；

标定计算机(5)用于控制二维转台(3)的转动姿态、接收数据采集模块(4)的姿态数据并进行视觉和惯性混合跟踪组件安装参数的标定计算；

采用所述的标定装置进行视觉和惯性混合跟踪组件安装参数的标定方法如下：

首先定义6个右手正交坐标系，分别为世界坐标系、转台坐标系、目标坐标系、跟踪标识坐标系、IMU坐标系和相机坐标系；

标定计算机(5)获得以下数据：

1)视觉跟踪数据，即跟踪标识坐标系相对于相机坐标系的姿态参数，用旋转矩阵T_cv表示；

2)惯性跟踪数据，即IMU坐标系相对于世界坐标系的姿态参数，用旋转矩阵Ti_w表示；

3)二维转台(3)输出的理想姿态数据，即目标坐标系相对于世界坐标系的姿态参数，用旋转矩阵T_ow表示，由于转台坐标系与世界坐标系一致，则有

T_ot·T_tw＝T_ow (1)

视觉标识的安装参数用旋转矩阵T_vo表示，IMU的安装参数用旋转矩阵T_io表示，相机的安装参数用相机坐标系与世界坐标系之间的旋转矩阵T_cw表示，则有如下关系：

T_cv·T_vo·T_ow＝T_cw (2)

T_io·T_ow＝T_iw (3)

式(2)为关于安装姿态角参数和测量姿态角数据的非线性约束方程，其两边同乘以T_ow的转置矩阵可得

考虑到上述各组件的安装参数误差是在设计值的基础上产生的，认为满足小角速误差近似条件，则得

式中和/>分别为T_vo和T_cw的初始值，取理论值或设计值，令(x1，x2，x3)为视觉标识的安装姿态角误差，(x4，x5，x6)为相机的安装姿态角误差，则ΔT_vo和ΔT_cw表示为：

令

在所述公式(9)中，符号aij表示T_cv与相乘所得旋转矩阵的第i行第j列元素；在所述公式(10)中，符号bij表示/>与/>旋转矩阵的第i行第j列元素；

将式(5)～(10)代入式(4)，经整理得到，对于1个转台位置有如下式所示的9维方程组：

令安装姿态角误差向量X＝[x1 x2 x3 x4 x5 x6]^T，则对于N个转台位置获得如下式所示的9×N维线性方程组

AX＝B (12)

求解上式得

X＝(A^TA)^-1A^TB (13)

将上式代入式(7)、(8)，然后将式(7)、(8)代入式(5)、(6)，利用旋转矩阵和欧拉角之间的转换关系，即得到要标定的安装姿态角参数。

标定时按以下步骤进行：

步骤一：控制二维转台(3)转动多个位置，使相机获取视觉标识的图像；

步骤二：将所有二维转台(3)的转动位置进行分组，每组包括m个转动位置，在每个位置处，利用数据采集模块(4)采集转台姿态数据、视觉和惯性姿态数据；

步骤三：完成一组的所有转动位置后，二维转台保持静止，标定计算机(5)利用该组所有位置的姿态数据计算IMU、视觉标识和相机的安装参数；

步骤四：完成一组安装参数计算后，控制二维转台(3)继续完成下一组转动位置，重复上述数据采集和安装参数解算过程，并将上一组解算的安装参数值作为下一组安装参数的理论值；

步骤五：重复上述过程，直至满足迭代次数阈值或混合跟踪精度的结束条件。

进一步地，视觉标识与IMU可直接固连后再安装到被跟踪目标上的一个位置处，也可分别固定安装到被跟踪目标上的不同位置处。

进一步地，标定计算机(5)具有二维转台转动位置自动控制功能，可控制二维转台满足姿态角精度要求，并保持静止状态一定时间(如2～5秒)。

进一步地，标定计算机(5)具有数据采集控制功能，在二维转台满足姿态精度并保持静止时，自动采集多组视觉跟踪和惯性跟踪同时解算的姿态数据，并具有姿态数据均值处理功能。

本发明的优点和有益效果为：

该标定装置和方法利用二维转台的分组转动和自动数据采集，实现了非线性约束方程的迭代线性求解，提高了标定效率；通过增加测量数据和安装参数迭代更新，提高了标定精度。

附图说明

图1为视觉和惯性混合跟踪组件安装参数标定装置组成示意图。

图2为视觉和惯性混合跟踪系统相关坐标系转换示意图。

图3为视觉和惯性混合跟踪组件安装参数标定方法流程图。

对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，可以根据以上附图获得其他的相关附图。

具体实施方式

为了使本技术领域的人员更好地理解本实用新型方案，下面结合具体实施例进一步说明本实用新型的技术方案。

一种视觉和惯性混合跟踪组件安装参数的标定装置，

该装置包括相机安装支架1、目标安装支架2、二维转台3、数据采集模块4、标定计算机5。

相机安装支架1用于固定跟踪相机，使其光轴朝向某个合适的方向，使得相机测量范围尽可能大。通过预先设计、加工和计算，可获得相机安装支架1的理论安装参数，其与实际安装参数存在一定的误差。

目标安装支架2用于将被跟踪目标8固定安装在二维转台3的基准面上，以保证目标坐标系与转台坐标系之间实现对准。通过预先精密设计和加工，目标安装支架2的实际安装误差可忽略。

二维转台3固定安装在跟踪环境的特定位置，且预先进行当地地理坐标系进行对准，即使得方位角零位与当地地理坐标系的正北方向一致，俯仰角零位与水平面一致，且指向当地地理坐标系的正东方向。

在被跟踪目标8上的不同位置安装有视觉标识6和IMU 7(即，惯性测量单元(英文：Inertial measurement unit，简称IMU)是测量物体三轴姿态角(或角速率)以及加速度的装置)，通过预先设计、加工和计算，可获得视觉标识和IMU在被跟踪目标上的理论安装参数，其与实际安装参数存在一定的误差。

数据采集模块4用于采集二维转台的方位角和俯仰角数据与视觉和惯性混合跟踪系统的姿态角测量数据。

标定计算机5用于控制二维转台3的转动姿态、接收数据采集模块4的姿态数据并进行视觉和惯性混合跟踪组件安装参数的标定计算，具体计算方法如下：

如图2所示，首先定义6个右手正交坐标系，分别为世界坐标系、转台坐标系、目标坐标系、跟踪标识坐标系、IMU坐标系和相机坐标系。世界坐标系为环境中的全局基准坐标系，例如可取东-北-天当地地理坐标系作为世界坐标系；转台坐标系与转台底座固连，可令其与世界坐标系之间不存在旋转变换，但存在一个固定的位移向量；目标坐标系与被跟踪目标固连，通过目标安装支架使得其与转台坐标系重合；跟踪标识坐标系与视觉标识固连，其与目标坐标系之间存在固定的旋转和位移变换，此即为待标定的第1组安装参数；IMU坐标系与IMU器件固连，其与目标坐标系之间存在固定的旋转和位移变换，此即为待标定的第2组安装参数；相机坐标系与跟踪相机固连，其与世界坐标系之间存在固定的旋转和位移变换，此即为待标定的第3组安装参数；本实施例仅以安装姿态角参数为例，所述方法可扩展为安装姿态和位移参数。

标定计算机(5)可获得以下数据：

1)视觉跟踪数据，即跟踪标识坐标系相对于相机坐标系的姿态参数，用旋转矩阵T_cv表示；

2)惯性跟踪数据，即IMU坐标系相对于世界坐标系的姿态参数，用旋转矩阵T_iw表示；

3)二维转台(3)输出的理想姿态数据，即目标坐标系相对于世界坐标系的姿态参数，用旋转矩阵T_ow表示，由于转台坐标系与世界坐标系一致，则有

T_ot·T_tw＝T_ow (1)

视觉标识的安装参数用旋转矩阵T_vo表示，IMU的安装参数用旋转矩阵T_io表示，相机的安装参数用相机坐标系与世界坐标系之间的旋转矩阵T_cw表示，则有如下关系：

T_cv·T_vo·To_w＝T_cw (2)

T_io·T_ow＝T_iw (3)

式(2)为关于安装姿态角参数和测量姿态角数据的非线性约束方程，其两边同乘以T_ow的转置矩阵可得

考虑到上述各组件的安装参数误差是在设计值的基础上产生的，认为满足小角速误差近似条件，则得

式中和/>分别为T_vo和T_cw的初始值，取理论值或设计值，令(x1，x2，x3)为视觉标识的安装姿态角误差，(x4，x5，x6)为相机的安装姿态角误差，则ΔT_vo和ΔT_cw表示为：

令

在所述公式(9)中，符号aij表示T_cv与相乘所得旋转矩阵的第i行第j列元素；在所述公式(10)中，符号bij表示/>与/>旋转矩阵的第i行第j列元素；

将式(5)～(10)代入式(4)，经整理得到，对于1个转台位置有如下式所示的9维方程组：

令安装姿态角误差向量X＝[x1 x2 x3 x4 x5 x6]^T，则对于N个转台位置获得如下式所示的9×N维线性方程组

AX＝B (12)

求解上式得

X＝(A^TA)^-1A^TB (13)

将上式代入式(7)、(8)，然后将式(7)、(8)代入式(5)、(6)，利用旋转矩阵和欧拉角之间的转换关系，即得到要标定的安装姿态角参数。

如图3所示，具体标定时：

第一步，控制二维转台依次转动m个姿态角位置，使视觉跟踪相机对被跟踪目标的视觉标识成像，m的取值应平衡方程组(12)的解算精度和运算效率，例如可取m为2～5。

第二步，控制二维转台保持静止状态t0秒，t0一般取为1～3，同时采集转台姿态数据、视觉和惯性姿态数据。

第三步，利用采集的数据，构造并求解式(12)方程组，得到安装参数姿态角误差。

第四步，依次利用式(6)、(7)和(4)、(5)，求解并更新混合跟踪组件的安装姿态角参数。

第五步，为了提高标定精度，将上一步求解得到的安装姿态角参数作为更新后的初始值，重复上述标定过程，即可对式(12)进行迭代求解，直至满足迭代次数阈值或混合跟踪精度指标等标定结束条件。

最后，输出上述最终解算的安装姿态角参数值，即为所述标定装置和方法标定出的混合跟踪组件安装参数结果。

Claims (2)

1.一种采用标定装置进行视觉和惯性混合跟踪组件安装参数的标定方法，其特征在于：该标定装置包括相机安装支架(1)、目标安装支架(2)、二维转台(3)、数据采集模块(4)和标定计算机(5)；

相机安装支架(1)用于固定跟踪相机，使其光轴朝向某个合适的方向；

目标安装支架(2)用于将被跟踪目标固定安装在二维转台(3)的基准面上，以保证目标坐标系与转台坐标系之间实现对准；

二维转台(3)固定安装在跟踪环境的特定位置，且预先进行当地地理坐标系进行对准；

在被跟踪目标上的不同位置安装有视觉标识和IMU器件；

数据采集模块(4)用于采集二维转台的方位角和俯仰角数据与视觉和惯性混合跟踪系统的姿态角测量数据；

标定计算机(5)用于控制二维转台(3)的转动姿态、接收数据采集模块(4)的姿态数据并进行视觉和惯性混合跟踪组件安装参数的标定计算；

采用所述的标定装置进行视觉和惯性混合跟踪组件安装参数的标定方法如下：

首先定义6个右手正交坐标系，分别为世界坐标系、转台坐标系、目标坐标系、跟踪标识坐标系、IMU坐标系和相机坐标系；

标定计算机(5)获得以下数据：

1)视觉跟踪数据，即跟踪标识坐标系相对于相机坐标系的姿态参数，用旋转矩阵T_cv表示；

2)惯性跟踪数据，即IMU坐标系相对于世界坐标系的姿态参数，用旋转矩阵T_iw表示；

3)二维转台(3)输出的理想姿态数据，即目标坐标系相对于世界坐标系的姿态参数，用旋转矩阵T_ow表示，由于转台坐标系与世界坐标系一致，则有

T_ot·T_tw＝T_ow (1)

视觉标识的安装参数用旋转矩阵T_vo表示，IMU的安装参数用旋转矩阵T_io表示，相机的安装参数用相机坐标系与世界坐标系之间的旋转矩阵T_cw表示，则有如下关系：

T_cv·T_vo·T_ow＝T_cw (2)

T_io·T_ow＝T_iw (3)

式(2)为关于安装姿态角参数和测量姿态角数据的非线性约束方程，其两边同乘以T_ow的转置矩阵可得

考虑到上述各组件的安装参数误差是在设计值的基础上产生的，认为满足小角速误差近似条件，则得

式中和/>分别为T_vo和T_cw的初始值，取理论值或设计值，令(x1,x2,x3)为视觉标识的安装姿态角误差，(x4,x5,x6)为相机的安装姿态角误差，则ΔT_vo和ΔT_cw表示为：

令

在所述公式(9)中，符号aij表示T_cv与相乘所得旋转矩阵的第i行第j列元素；在所述公式(10)中，符号bij表示/>与/>旋转矩阵的第i行第j列元素；

将式(5)～(10)代入式(4)，经整理得到，对于1个转台位置有如下式所示的9维方程组：

令安装姿态角误差向量X＝[x1 x2 x3 x4 x5 x6]^T，则对于N个转台位置获得如下式所示的9×N维线性方程组

AX＝B (12)

求解上式得

X＝(A^TA)^-1A^TB (13)

将上式代入式(7)、(8)，然后将式(7)、(8)代入式(5)、(6)，利用旋转矩阵和欧拉角之间的转换关系，即得到要标定的安装姿态角参数。

2.根据权利要求1所述的标定方法，其特征在于，标定时按以下步骤进行：

步骤一：控制二维转台(3)转动多个位置，使相机获取视觉标识的图像；

步骤二：将所有二维转台(3)的转动位置进行分组，每组包括m个转动位置，在每个位置处，利用数据采集模块(4)采集转台姿态数据、视觉和惯性姿态数据；

步骤三：完成一组的所有转动位置后，二维转台保持静止，标定计算机(5)利用该组所有位置的姿态数据计算IMU、视觉标识和相机的安装参数；

步骤四：完成一组安装参数计算后，控制二维转台(3)继续完成下一组转动位置，重复上述数据采集和安装参数解算过程，并将上一组解算的安装参数值作为下一组安装参数的理论值；

步骤五：重复上述过程，直至满足迭代次数阈值或混合跟踪精度的结束条件。