[发明专利]一种全局最优的光学跟踪器位姿计算方法

说明书

本发明公开了一种全局最优的光学跟踪器位姿计算方法，改进了传统跟踪器的位姿计算方法，使用了基于全局优化思想的数学模型，使用空间点和图像点的对应关系构建线性方程组，不需要计算跟踪器相对于单个基站的位姿，也不需要进行位姿数据融合，可以直接求解跟踪器全局最优位姿；该方法不限制基站数量，充分利用了所有基站对应点的信息(即使此基站的对应点数量不足以独立计算位姿)，大大降低了跟踪器位姿的最小计算条件(将对应点数量阈值由任意一个基站至少5组对应点降低为所有基站一共4组对应点)；此外，当多个接收器与跟踪器建立联系时，可获得全局最优的位姿融合结果，并且结果更加精确、鲁棒性更强。

技术领域

本发明属于跟踪定位技术领域，具体涉及一种全局最优的光学跟踪器位姿计算方法，可用于动作捕捉、手术导航、虚拟现实等需要光学跟踪定位的应用领域。

背景技术

HTC VIVE系统由发射器基站和光敏接收器构成，发射器可发出周期性光信号对跟踪区域进行扫描，接收器接收到发射器的扫描信号后，将光信号转换为数字信号，从而得到接收器相对于发射器的图像坐标，当一定数量的接收器被扫描后，可利用计算机视觉算法获得接收器组成的刚体的空间位姿。

HTC VIVE拥有两个发射器扫描基站(相当于两个摄像机)，在计算位姿时，需要一个跟踪器上至少5个传感器点被某个基站扫描到才能计算跟踪器与该基站之间的位姿关系，跟踪器在使用中由于自身位置角度的变化可能存在部分传感器被遮挡情况，因此要求跟踪器上必须布设数量较多的传感器点，以保证在部分传感器点被遮挡时，仍有至少有5个传感器点能收到基站扫描信号。传感器点越多，跟踪器的体积就越大，不利于跟踪器的小型化。此外，HTC VIVE系统在进行位姿融合时，使用了加权椭圆拟合的方法，该方法要求较为严格，需要知道每个基站与跟踪器之间的相对位姿，而且只适用于两个基站的位姿数据融合，当基站(摄像机)数量更多时，没有位姿数据全局优化的能力。

发明内容

有鉴于此，本发明的目的是提供一种全局最优的光学跟踪器位姿计算方法，通过改进传统跟踪器的位姿计算方法，放宽了跟踪器位姿计算条件，即在不能计算任意单个接收器与跟踪器位姿时，仅凭跟踪器和多个接收器之间的有限信息即可完成跟踪器位姿计算。

本发明的一种全局最优的光学跟踪器位姿计算方法，包括：

步骤1、针对每一个传感器，确定其能接收到信号的发射器，将一个传感器与其能接收到信号的一个发射器作为一个发射接收组合，遍历所有传感器，统计所有所述发射接收组合数目，并记为N；

步骤2、针对任意一个发射接收组合，令其中的传感器序号为j，发射器序号表示为i；则确定第j个传感器在自身刚体坐标系下的三维坐标确定第j个传感器在其能接收到信号的第i个发射器中的二维图像坐标然后建立关于该发射接收组合中互相对应的三维空间点与二维图像点的有效方程组：

其中，p_i1、p_i2、p_i3和p_i4表示传感器刚体坐标系与第i个发射器的图像坐标系之间的投影关系矩阵P_i中的元素；a_ij＝[0，-1，v_ij]^T，b_ij＝[1，0，-u_ij]^T，其中，u_ij和v_ij分别表示二维图像坐标中两个坐标轴方向的坐标；其中，表示传感器刚体坐标系转换到发射器坐标系的旋转矩阵，表示传感器刚体坐标系转换到发射器坐标系的平移矩阵；

步骤3、针对每一个发射接收组合建立一个式(1)所示的方程组，N个发射接收组合即得到N个方程组，由此形成2N维的线性方程组；

步骤4、将步骤3形成的线型方程组改写成如下形式：

AX＝B (2)