CN111489399A - 一种视觉跟踪组件安装参数标定装置和方法 - Google Patents

Abstract

本发明公开了一种视觉跟踪组件安装参数标定装置和方法，包括相机安装支架、目标安装支架、二维转台、数据采集模块、标定计算机。控制二维转台转动，在每个转动位置处，利用数据采集模块采集二维转台的姿态数据和视觉跟踪系统输出的姿态数据并将数据发送给标定计算机；标定计算机采用扩展卡尔曼滤波算法将视觉跟踪组件的安装参数标定转换为状态估计，以待标定的视觉跟踪组件安装参数作为状态变量，将二维转台输出的姿态数据进行转换，以得到的姿态矩阵元素作为量测向量，利用视觉跟踪系统姿态解算数据和上一时刻的状态变量构造一阶线性化的量测矩阵。本发明通过将视觉跟踪系统引入标定流程构成闭环，可消除未建模误差，提高标定精度。

Description

一种视觉跟踪组件安装参数标定装置和方法

技术领域

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

背景技术

运动跟踪技术是机器人和VR/AR等领域中的重要研究内容之一，用于确定运动目标相对于所在环境的位移和姿态等运动信息。视觉跟踪系统是指将视觉跟踪相机安装在环境中，将预先设计好的图案安装在运动目标上或采用运动目标自身特征作为跟踪靶标，在目标运动时，利用视觉相机对目标上的靶标进行成像探测，经过图像处理和视觉算法解算，实时跟踪运动目标相对于环境的位姿变化信息。

视觉跟踪组件包括靶标和跟踪相机，一般靶标经过精密设计和加工，其精度可以满足要求，但其在运动目标上的安装与跟踪相机在固定环境中的安装均会不可避免地存在误差。

目前现有的视觉跟踪系统标定装置和方法存在如下问题：

1)仅对跟踪相机内外参数进行单独标定，一般采用三维立体或二维平面精密靶标，且假设靶标的位置和姿态精确可控或已知，而没有考虑靶标的实际安装误差；

2)没有将视觉跟踪系统的解算结果作为标定的反馈信息，以构成在正常工作模式下的标定闭环，没有考虑跟踪系统的过程噪声和测量噪声统计特性，无法考虑和消除其他未知或未建模的误差源。

3)机器人手眼标定一般需要利用末端执行器具有较多旋转自由度以执行特定运动，对于相机安装在固定平台上的应用来说无法满足标定条件。

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

发明内容

本发明的目的在于克服现有技术的不足，提出一种视觉跟踪组件安装参数的标定装置和方法，以满足高精度运动跟踪应用的需求。

本发明是通过以下技术方案实现的：

一种视觉跟踪组件安装参数的标定装置，包括相机安装支架(1)、目标安装支架(2)、二维转台(3)、数据采集模块(4)、标定计算机(5)；

相机安装支架(1)用于固定跟踪相机，使其光轴在固定参考坐标系中朝向合适的方向；

被跟踪目标通过目标安装支架(2)固定在二维转台(3)上，在被跟踪目标上安装有靶标；

数据采集模块(4)用于采集二维转台的姿态数据和视觉跟踪系统的姿态数据；

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

采用上述标定装置进行视觉跟踪组件安装参数标定的方法如下：

控制二维转台(3)转动N个位置，使相机能够对靶标进行成像；

在二维转台(3)的每个转动位置处，利用数据采集模块(4)采集二维转台(3) 的姿态数据和视觉跟踪系统输出的姿态数据，并将采集的数据发送给标定计算机 (5)；

标定计算机(5)采用扩展卡尔曼滤波算法将视觉跟踪组件的安装参数标定转换为状态估计，以待标定的视觉跟踪组件安装参数作为状态变量，将二维转台输出的姿态数据进行转换，以得到的姿态矩阵元素作为量测向量，利用视觉跟踪系统姿态解算数据和上一时刻的状态变量构造一阶线性化的量测矩阵。

在上述技术方案中，被跟踪目标上的靶标为一组或者多组。

在上述技术方案中，控制二维转台满足姿态角精度要求，并保持静止状态一定时间(如2秒)，以保证在每个位置处采集到准确的数据。

在上述技术方案中，定义4个右手正交坐标系，包括运动目标坐标系、靶标坐标系、相机坐标系和世界坐标系，其中靶标在运动目标上的安装参数用运动目标坐标系与靶标坐标系之间的旋转矩阵T_om表示，靶标坐标系与相机坐标系之间的旋转矩阵T_mc通过视觉跟踪系统给出，相机在工作环境中的安装参数用相机坐标系与世界坐标系之间的旋转矩阵T_cw表示，运动目标在工作环境中的运动姿态由运动目标坐标系与世界坐标系之间的旋转矩阵T_ow表示，由二维转台(3)给出，姿态矩阵存在如下关系式：

T_om·T_mc·T_cw＝T_ow (1)

在上述技术方案中，将视觉跟踪组件的安装参数用于欧拉角参数描述，系统的状态向量为

X＝[r_om,p_om,a_om,r_cw,p_cw,a_cw]^T (2)

其中，a_om、p_om和r_om分别表示运动目标坐标系相对于靶标坐标系的方位角、俯仰角和横滚角，a_cw、p_cw和r_cw分别表示相机坐标系相对于世界坐标系的方位角、俯仰角和横滚角；

在二维转台转动期间视觉跟踪组件安装参数均保持不变，并加入过程噪声表示未知或未建模的系统误差，可建立如下系统方程：

X(k+1)＝X(k)+w(k) (3)

式中w(k)为二维转台第k个位置处的零均值和标准差为σ_w的高斯白噪声。

令视觉跟踪系统解算的方位角、俯仰角和横滚角分别为[a_mc,p_mc,r_mc]，则其对应的旋转矩阵为：

由于安装参数与设计理论值之间的误差一般较小，利用小角度近似对式(5)、式(6)进行简化计算，可得

将二维转台输出的方位角、俯仰角和横滚角(横滚角为0)转换得到的姿态矩阵元素组成9维量测向量，则量测方程为如下非线性方程：

对上式求量测向量Y(k)对于状态向量X(k)的一阶导数，整理后得到量测方程为：

Y(k)＝H(k)X(k)+v(k) (10)

式中v(k)为二维转台的第k个位置测量数据中存在的零均值和标准差为σ_v的高斯白噪声，以表示姿态测量中存在的未知或未建模随机误差。

利用视觉跟踪系统姿态解算数据和上一时刻的状态变量，进行一阶线性化得到量测矩阵：

在每个位置的姿态测量向量为：

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

发明的标定装置和方法对视觉跟踪组件靶标与相机的安装参数进行统一标定，通过将视觉跟踪系统解算结果作为反馈信息构成闭环标定流程，可消除实际系统中的未知或未建模误差，从而提高视觉跟踪组件安装参数标定精度，并最终提高运动目标跟踪精度。

附图说明

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

图2为视觉跟踪组件安装参数标定方法流程图。

图3为视觉跟踪系统相关坐标系转换示意图。

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

具体实施方式

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

实施例一

一种视觉跟踪组件安装参数的标定装置：

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

相机安装支架1用于固定跟踪相机，使其光轴在固定参考坐标系中朝向合适的方向；

被跟踪目标上安装有若干组靶标，被跟踪目标通过目标安装支架2固定在二维转台3上，二维转台3固定安装在固定参考坐标系中；

数据采集模块4用于采集二维转台的姿态数据和视觉跟踪系统输出的姿态数据；

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

实施例二

采用上述标定装置对视觉跟踪组件安装参数进行标定的方法如下：

将跟踪相机通过安装支架1固定在所在环境的正常工作位置处，将二维转台3 固定安装在被跟踪目标所在环境的正常工作范围的中心位置处，通过安装支架2将被跟踪目标固定在二维转台3上，使得相机能够在目标一定运动范围内对目标上的靶标进行成像。

控制二维转台3转动N个位置，使相机能够对目标上的每组靶标进行成像，在二维转台3每个转动位置处，利用数据采集模块4采集二维转台3输出的姿态数据和视觉跟踪系统输出的姿态数据，随着二维转台3的逐个位置转动，完成所有转动位置的数据采集和更新，采集的姿态数据输入标定计算机5中。

标定计算机5采用扩展卡尔曼滤波算法将视觉跟踪组件的安装参数标定转换为状态估计，以待标定的视觉跟踪组件安装参数作为状态变量，将二维转台输出的姿态数据进行转换，以得到的姿态矩阵元素作为量测向量，利用视觉跟踪系统姿态解算数据和上一时刻的状态变量构造一阶线性化的量测矩阵。下面具体介绍标定计算机进行视觉跟踪组件安装参数的计算方法：

如图3所示，首先，定义4个右手正交坐标系，包括运动目标坐标系、靶标坐标系、相机坐标系和世界坐标系，分别与运动目标、靶标、相机和工作环境固连，其中靶标在运动目标上的安装参数用运动目标坐标系与靶标坐标系之间的旋转矩阵T_om表示，靶标坐标系与相机坐标系之间的旋转矩阵T_mc通过视觉跟踪系统解算给出，相机在工作环境中的安装参数用相机坐标系与世界坐标系之间的旋转矩阵T_cw表示，运动目标在工作环境中的运动姿态由运动目标坐标系与世界坐标系之间的旋转矩阵T_ow表示，由二维转台直接给出，上述姿态矩阵存在如下关系式：

T_om·T_mc·T_cw＝T_ow (1)

其中T_om和T_cw分别表示待标定的安装参数，T_mc和T_ow分别表示视觉跟踪系统解算的姿态数据和二维转台(3)输出的姿态数据。

视觉跟踪组件的安装参数可用旋转矩阵T_om和T_cw表示，也可转换为其他姿态参数描述，包括欧拉角、旋转向量、四元数或罗德里格参数等，并以选定的姿态参数组成卡尔曼滤波算法的状态向量。本实施例选择欧拉角作为示例，a_om、p_om和r_om分别表示运动目标坐标系相对于靶标坐标系的方位角、俯仰角和横滚角，a_cw、p_cw和r_cw分别表示相机坐标系相对于世界坐标系的方位角、俯仰角和横滚角，则系统的状态向量为

X＝[r_om,p_om,a_om,r_cw,p_cw,a_cw]^T (2)

在二维转台转动期间视觉跟踪组件安装参数均保持不变，并加入过程噪声表示未知或未建模的系统误差，可建立如下系统方程：

X(k+1)＝X(k)+w(k) (3)

式中w(k)为二维转台第k个位置处的零均值和标准差为σ_w的高斯白噪声。

令视觉跟踪系统解算的方位角、俯仰角和横滚角分别为[a_mc,p_mc,r_mc]，则其对应的旋转矩阵为：

由于安装参数与设计理论值之间的误差一般较小，利用小角度近似对式(5)、式(6)进行简化计算，可得

将二维转台输出的方位角、俯仰角和横滚角(横滚角为0)转换得到的姿态矩阵元素组成9维量测向量，则量测方程为如下非线性方程：

对上式求量测向量Y(k)对于状态向量X(k)的一阶导数，整理后得到量测方程为：

Y(k)＝H(k)X(k)+v(k) (10)

式中v(k)为二维转台的第k个位置测量数据中存在的零均值和标准差为σ_v的高斯白噪声，以表示姿态测量中存在的未知或未建模随机误差。

利用视觉跟踪系统姿态解算数据和上一时刻的状态变量，进行一阶线性化得到量测矩阵：

在每个位置的姿态测量向量为：

扩展卡尔曼滤波算法的状态变量初始值取为设计理论值。根据上述公式，即可实现扩展卡尔曼滤波算法，最终完成视觉跟踪组件的安装参数的标定。

Claims (3)

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

相机安装支架(1)用于固定跟踪相机，使其光轴在固定参考坐标系中朝向合适的方向；

被跟踪目标通过目标安装支架(2)固定在二维转台(3)上，在被跟踪目标上安装有靶标；

数据采集模块(4)用于采集二维转台的姿态数据和视觉跟踪系统的姿态数据；

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

2.采用权利要求1所述的标定装置进行视觉跟踪组件安装参数标定的方法，其特征在于：

控制二维转台(3)转动N个位置，使相机能够对靶标进行成像；

在二维转台(3)的每个转动位置处，利用数据采集模块(4)采集二维转台(3)的姿态数据和视觉跟踪系统输出的姿态数据，并将采集的数据发送给标定计算机(5)；

标定计算机(5)采用扩展卡尔曼滤波算法将视觉跟踪组件的安装参数标定转换为状态估计，以待标定的视觉跟踪组件安装参数作为状态变量，将二维转台输出的姿态数据进行转换，以得到的姿态矩阵元素作为量测向量，利用视觉跟踪系统姿态解算数据和上一时刻的状态变量构造一阶线性化的量测矩阵。

3.根据权利要求2所述的视觉跟踪组件安装参数标定的方法，其特征在于：

首先，定义4个右手正交坐标系，包括运动目标坐标系、靶标坐标系、相机坐标系和世界坐标系，其中靶标在运动目标上的安装参数用运动目标坐标系与靶标坐标系之间的旋转矩阵T_om表示，靶标坐标系与相机坐标系之间的旋转矩阵T_mc通过视觉跟踪系统给出，相机在工作环境中的安装参数用相机坐标系与世界坐标系之间的旋转矩阵T_cw表示，运动目标在工作环境中的运动姿态由运动目标坐标系与世界坐标系之间的旋转矩阵T_ow表示，由二维转台(3)给出，姿态矩阵存在如下关系式：

T_om·T_mc·T_cw＝T_ow (1)

将视觉跟踪组件的安装参数用于欧拉角参数描述，系统的状态向量为

X＝[r_om,p_om,a_om,r_cw,p_cw,a_cw]^T (2)

其中，a_om、p_om和r_om分别表示运动目标坐标系相对于靶标坐标系的方位角、俯仰角和横滚角，a_cw、p_cw和r_cw分别表示相机坐标系相对于世界坐标系的方位角、俯仰角和横滚角；

在二维转台转动期间视觉跟踪组件安装参数均保持不变，并加入过程噪声表示未知或未建模的系统误差，可建立如下系统方程：

X(k+1)＝X(k)+w(k) (3)

式中w(k)为二维转台第k个位置处的零均值和标准差为σ_w的高斯白噪声。

令视觉跟踪系统解算的方位角、俯仰角和横滚角分别为[a_mc,p_mc,r_mc]，则其对应的旋转矩阵为：

由于安装参数与设计理论值之间的误差一般较小，利用小角度近似对式(5)、式(6)进行简化计算，可得

将二维转台输出的方位角、俯仰角和横滚角转换得到的姿态矩阵元素组成9维量测向量，则量测方程为如下非线性方程：

对上式求量测向量Y(k)对于状态向量X(k)的一阶导数，整理后得到量测方程为：

Y(k)＝H(k)X(k)+v(k) (10)

式中v(k)为二维转台的第k个位置测量数据中存在的零均值和标准差为σv的高斯白噪声，以表示姿态测量中存在的未知或未建模随机误差。

利用视觉跟踪系统姿态解算数据和上一时刻的状态变量，进行一阶线性化得到量测矩阵：

在每个位置的姿态测量向量为：

Images