一种多目标光学跟踪方法与流程

本发明属于跟踪定位领域，尤其涉及一种多目标光学跟踪方法。

背景技术：

在当今的跟踪定位领域中，Vicon和HTC VIVE是较为常用的技术。Vicon使用高帧率、高分辨率的摄像机作为信号捕捉设备，使用红外反光球作为被跟踪标记物，若反光球被多个摄像机拍摄到，即可通过计算机视觉方法计算其空间位置。Vicon系统的跟踪精度和刷新率与使用的跟踪摄像机的性能相关，例如其V16摄像机分辨率达到1600万像素而帧率到达120Hz。由于高性能摄像机的制造难度很大且需要后续数据处理设备，这导致Vicon系统的成本高昂，只能在专业领域使用而不能获得广泛普及。

HTC VIVE使用与本发明类似的技术原理，其硬件系统同样由发射器和接收器构成。该方案硬件系统较为简单，成本低廉，已经投入市场。如图3所示，HTC VIVE在跟踪时需要发射器先发出一帧同步扫描信号，然后再依次对水平和垂直方向进行扫描。当多个发射器级联使用时，为了避免信号干扰，同一时间段内只能让一个发射器工作，必须由控制器分配工作时间，以保证同一时间段内只有一个发射器对跟踪区域进行扫描。这导致该系统在多发射器级联使用时的刷新率成倍下降。由于跟踪区域越大，需要的发射器越多，因此为了保证足够的跟踪数据刷新率，目前的HTC VIVE系统只使用两个发射器，而其跟踪区域也被限制在5mx5m的空间内。

技术实现要素：

为解决上述问题，本发明提供一种高精度、低成本的光学跟踪方法，可允许所有发射器同时工作，保证了多发射器级联使用时跟踪数据刷新率不下降，可以通过多发射器级联扩展跟踪区域的大小。

一种多目标光学跟踪方法，包括以下步骤：

步骤1：接收器分别记录各个发射器在水平X方向上发出的扫描信号与起始信号的时间差，以及各个发射器在垂直Y方向上发出的扫描信号与起始信号的时间差；

步骤2：利用步骤1的时间差计算接收器相对于m个发射器的所有图像坐标u_i和v_i，其中u_i为接收器相对于第i个发射器的水平X方向上的图像坐标，v_i为接收器相对于第i个发射器的垂直Y方向上的图像坐标，i＝1,2,3,...,m；

步骤3：将m个水平X方向上的图像坐标u_i构成集合{u_i}，m个水平Y方向上的图像坐标v_i构成集合{v_i}，从集合{u_i}和集合{v_i}中分别任选一个图像坐标u_i和v_i进行排列组合，得到接收器在一个周期内相对于m个发射器所有可能的图像坐标的组合总数S；

步骤4：从S种组合中任意选一组图像坐标，将该组中接收器对应各个发射器的估计图像坐标记为x_j，其中j＝1,2,3,...,S，表示第j种图像坐标组合；

步骤5：根据估计图像坐标x_j，利用超定线性投影方程计算接收器在第j组图像坐标组合下的理论空间三维坐标X_w：

步骤6：将接收器理论空间三维坐标X_w作为已知量代入超定线性投影方程，计算在第j种图像坐标组合情况下，接收器在m个发射器中对应的理论图像坐标x_j'；

步骤7：对第j种图像坐标组合的估计值x_j与理论值x_j'进行欧式距离评估，根据评估结果判断当前尝试的第j组图像坐标组合与实际情况是否相符；

如果相符，则步骤5中获得的接收器理论空间三维坐标X_w正确，实现对接收器的光学跟踪；

如果不相符，则进入步骤8；

步骤8：选取新的图像坐标组合，重复步骤4-7进行验证，直至找到正确的接收器三维空间坐标X_w。

一种多目标光学跟踪方法，步骤1中所述的接收器分别记录各个发射器在水平X方向上发出的扫描信号与起始信号的时间差，以及各个发射器在垂直Y方向上发出的扫描信号与起始信号的时间差，具体步骤为：

步骤11：固定m个发射器，将接收器置于m个发射器的扫描重叠区域内；

步骤12：m个发射器同时发射一帧起始信号；

步骤13：接收器接收起始信号后，各发射器沿水平X方向对跟踪区域发射扫描信号，接收器记录水平X方向的m个扫描信号与起始信号之间的时间差tx_i；

步骤14：m个发射器再同时发射一帧起始信号；

步骤15：接收器接收起始信号后，各发射器沿垂直Y方向对跟踪区域发射扫描信号，接收器记录垂直Y方向的m个扫描信号与起始信号之间的时间差ty_i。

一种多目标光学跟踪方法，步骤2所述的计算接收器相对于m个发射器的所有图像坐标u_i和v_i具体方法为：

利用公式(1)依次计算接收器对应m个发射器的所有图像坐标u_i和v_i：

其中ω为已知的扫描速度。

一种多目标光学跟踪方法，步骤3所述的m个发射器所有可能的图像坐标的组合总数S具体计算方法为：

对于m个发射器，每个接收器在水平X方向上对应各发射器的图像坐标集合{u_i}共有种排列方式，垂直Y方向上对应各发射器的图像坐标集合{v_i}同样也有种排列方式，则一个接收器在一个周期内对应的发射器图像坐标组合总数S为：

一种多目标光学跟踪方法，步骤5所述的理论空间三维坐标X_w计算方法为：

令X_w＝X_w，X_w为接收器齐次坐标形式的空间三维坐标，其中X_w＝[X_w,1]^T，X_w＝[x,y,z]^T，X_w为接收器的非齐次形式的空间三维坐标，上标T表示矩阵转置，求解接收器与m个发射器之间的超定线性投影方程(3)，得到接收器在第j组图像坐标组合下的理论空间三维坐标X_w：

x_j＝PX_w (3)

其中P为通过初始标定获得的各个发射器对应的投影矩阵，x_j为估计图像坐标x_j对应的齐次坐标。

一种多目标光学跟踪方法，步骤6所述的理论图像坐标x_j'计算方法为：

将步骤5中获得的接收器理论空间三维坐标X_w再次代入超定线性投影方程(3)中得到方程(4)，计算在第j种图像坐标组合情况下，接收器在m个发射器中对应的理论图像坐标x_j'：

x_j'＝PX_w (4)

一种多目标光学跟踪方法，步骤7所述的对第j种图像坐标组合的估计值x_j与理论值x_j'进行欧式距离评估具体为：使用判别函数f(x_j)对第j种图像坐标组合的估计值x_j与理论值x_j'进行评估：

其中x_j*是x_j'的非齐次坐标形式；

若判别函数f(x_j)的值小于设定阈值thresh，则当前尝试的第j组图像坐标组合与实际情况相符，完成接收器空间三维坐标的计算；

若判别函数f(x_j)的值大于设定阈值thresh，则当前尝试的第j组图像坐标组合与实际情况不符，并进入步骤8。

有益效果：

本发明采用一种基于光学跟踪系统的多目标光学跟踪方法，其中硬件构成包含发射器、接收器、处理器等；发射器在水平和垂直两个方向对跟踪区域进行扫描，接收器硬件可输出某一时刻其在所有发射器中的图像坐标集合，但此时并不知道坐标与各个发射器的对应关系。因此本发明利用接收器计算接收到扫描信号的时间从而获取接收器在发射器中的图像坐标，并对所有可能的图像坐标对应关系进行检验，从中找出唯一正确的接收器图像坐标与各发射器的对应关系，继而实现了接收器空间坐标的解算；本发明从技术原理上解决了使用多发射器时的数据刷新率下降问题，可以级联使用任意数量的发射器而不降低跟踪数据刷新率。发射器数量越多，其扫描覆盖范围越大，本发明可以支持更多数量的发射器，因此可以将跟踪区域扩展到更大范围；

本发明所有发射器同时发射同步信号和进行X、Y方向扫描，根据所处位置的不同，一个扫描周期内同一个接收器会先后接收到若干个扫描信号；通过对所有可能的图像坐标对应关系进行检验，对没有正确匹配的发射器图像坐标进行配对，可允许所有发射器同时工作，保证了多发射器级联使用时跟踪数据刷新率不下降。

附图说明

图1为本发明的实现方法流程图；

图2为本发明的跟踪系统硬件结构示意图；

图3为HTC VIVE的接收器信号波形图；

图4为本发明的接收器信号波形图。

具体实施方式

下面结合发射器数量m＝3的具体实施方式，对本发明进行详细说明。

如图1所示，为本发明的实现方法流程图；具体的，一种多目标光学跟踪方法，包括以下步骤：

步骤1：接收器分别记录各个发射器在水平X方向上发出的扫描信号与起始信号的时间差，以及各个发射器在垂直Y方向上发出的扫描信号与起始信号的时间差；

步骤2：利用步骤1的时间差计算接收器相对于3个发射器的所有图像坐标u_i和v_i，其中u_i为接收器相对于第i个发射器的水平X方向上的图像坐标，v_i为接收器相对于第i个发射器的垂直Y方向上的图像坐标，i＝1,2,3；

步骤3：将3个水平X方向上的图像坐标u_i构成集合{u_i}，3个水平Y方向上的图像坐标v_i构成集合{v_i}，从集合{u_i}和集合{v_i}中分别任选一个图像坐标u_i和v_i进行组合，得到接收器在一个周期内相对于3个发射器所有可能的图像坐标的组合总数S；

步骤4：所有图像坐标组合中只有一种是正确的组合，其它都是错误的；从S种组合中任意选一组图像坐标，将该组中各个发射器对应的接收器估计图像坐标记为x_j，其中j＝1,2,3,...,S，表示第j种图像坐标组合；由于每个图像坐标组合中包括3个坐标，为了区分各个图像坐标组合中接收器对应每个发射器的坐标，将第j组估计图像坐标x_j记为x_ij，即x_1j、x_2j以及x_3j；

步骤5：根据估计图像坐标x_ij，利用超定线性投影方程计算接收器在第j组图像坐标组合下的理论空间三维坐标X_w；

步骤6：将接收器理论空间三维坐标X_w作为已知量代入超定线性投影方程，计算在第j种图像坐标组合情况下，接收器在3个发射器中对应的理论图像坐标x_ij'；

步骤7：对第j种图像坐标组合的估计值x_ij与理论值x_ij'进行欧式距离评估，根据评估结果判断当前尝试的第j组图像坐标组合与实际情况是否相符；

如果相符，则步骤5中获得的接收器理论空间三维坐标X_w正确，实现对接收器的光学跟踪；

如果不相符，则进入步骤8；

步骤8：选取新的图像坐标组合，重复步骤4-7进行验证，直至找到正确的接收器三维空间坐标X_w。

步骤1中所述的接收器分别记录各个发射器在水平X方向上发出的扫描信号与起始信号的时间差，以及各个发射器在垂直Y方向上发出的扫描信号与起始信号的时间差，具体步骤为：

步骤11：如图2所示，固定3个发射器，将接收器置于3个发射器的扫描重叠区域内；

步骤12：3个发射器首先让其内置红外LED灯同时闪烁一次，照亮整个跟踪区域，接收器收到该信号作为一帧信息的起始信号；

步骤13：接收器接收起始信号后，各发射器使用面激光器沿水平X方向对跟踪区域发射扫描信号，如图4所示，接收器记录水平X方向的3个扫描信号与起始信号之间的时间差tx₁，tx₂，tx₃；由于所有发射器均处于工作状态，故此时无法判断各个时间差对应哪个发射器；

步骤14：3个发射器再让其内置红外LED灯同时闪烁一次，照亮整个跟踪区域，接收器收到该信号作为一帧信息的起始信号；

步骤15：接收器接收起始信号后，各发射器使用面激光器沿垂直Y方向对跟踪区域发射扫描信号，如图4所示，接收器记录垂直Y方向的3个扫描信号与起始信号之间的时间差ty₁，ty₂，ty₃；由于所有发射器均处于工作状态，故此时无法判断各个时间差对应哪个发射器。

步骤2所述的计算接收器对应3个发射器的所有图像坐标u_i和v_i具体方法为：

利用公式(1)依次计算接收器对应3个发射器的所有图像坐标u_i和v_i，则有：

其中ω为已知的面激光器的扫描速度。

步骤3所述的3个发射器所有可能的图像坐标的组合总数S具体方法为：

对于3个发射器，每个接收器在水平X方向上对应各发射器的图像坐标集合{u_i}共有种排列方式，垂直Y方向上对应各发射器的图像坐标集合{v_i}同样也有种排列方式，则一个接收器在一个周期内对应的发射器图像坐标组合总数S为：

步骤5所述的理论空间三维坐标X_w计算方法为：

令X_w＝X_w，根据超定线性投影方程(3)，获得接收器在第j组图像坐标组合下的理论空间三维坐标X_w：

其中P₁，P₂，P₃为通过初始标定获得的3个发射器对应的投影矩阵，x_ij为估计图像坐标x_ij对应的齐次坐标，X_w为未知数，是接收器齐次坐标形式的空间三维坐标，其中X_w包含3个未知数；同时又有X_w＝[X_w,1]^T，X_w＝[x,y,z]^T，X_w为接收器的非齐次形式的空间三维坐标，上标T表示矩阵转置。

由于将估计图像坐标x_ij代入公式(3)可得到公式(8)：

其中λ为未知非零系数(根据齐次坐标定义，当λ≠0时，x_ij与λx_ij等价，表示同一个坐标)。若设P_i＝[p_i1，p_i2，p_i3]^T，(p_i1，p_i2，p_i3，分别为矩阵P_i的三行)，则公式(8)可以展开写为以下三个方程：

若x_ij＝[u_i,v_i,1]^T和P_i＝[p_i1，p_i2，p_i3]^T为已知条件，则由第三个方程可得到λ的值，再将λ带入前两个方程即可得到未知数为X_w＝[x,y,z,1]^T的两个独立方程。由此可知，一组三维空间坐标到二维图像坐标的投影方程可提供2个关于X_w的独立方程。

公式(7)中包含3组投影方程，共能提供2×3＝6个关于X_w的独立方程，而接收器的空间三维坐标X_w(齐次坐标形式)为未知数，它包含3个未知数[x,y,z]^T，(接收器的非齐次空间坐标表述为X_w＝[x,y,z]^T，X_w＝[X_w,1]^T)，因此可以通过求解超定线性方程组的方式求得X_w在最小二乘意义上的最优解，继而获得接收器在第j组图像坐标组合下的理论三维位置，记为X_w。

步骤6所述的理论图像坐标x_ij'计算方法为：

将步骤5中获得的接收器理论空间三维坐标X_w再次带入超定线性投影方程(3)中得到方程(4)，计算在第j种图像坐标组合情况下，接收器在m个发射器中对应的理论图像坐标x_ij'：

步骤7所述的对第j种图像坐标组合的估计值x_ij与理论值x_ij'进行欧式距离评估具体为：使用判别函数f(x_ij)对第j种图像坐标组合的估计值x_ij与理论值x_ij'进行评估：

其中x_ij*是x_ij'的非齐次坐标形式，即x_ij'＝[x_ij*,1]^T；

若当前图像坐标组合与正确情况相符，那么判别函数的理论取值应该为0，若图像坐标组合错误，则判别函数取值必大于0。实际应用中考虑到传感器噪声和计算误差等因素，可以设置一个阈值thresh＝1。

若判别函数f(x_ij)的值小于阈值1，则当前尝试的第j组图像坐标组合与实际情况相符，完成接收器空间三维坐标的计算，实现接收器的光学跟踪；

若判别函数f(x_ij)的值大于阈值1，则当前尝试的第j组图像坐标组合与实际情况不符，并进入步骤8。

当然，本发明还可有其他多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。