一种全局最优的光学跟踪器位姿计算方法技术

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

A global optimal method for computing the position and posture of optical tracker

The present invention discloses optical tracker pose a global optimum calculation method, calculation method of improved traditional pose tracker, using a mathematical model based on global optimization, the corresponding relationship between space points and image points to construct linear equations, does not need to compute the pose tracker with respect to a single base station, also do not need to the position and attitude of data fusion, the tracker can directly solve the global optimal position; this method does not limit the number of base stations, base station can make full use of all the corresponding point information (even if the number of points corresponding to the base station to calculate independent position), greatly reducing the minimum calculation conditions pose tracker (the corresponding point number threshold by any one base station at least 5 sets of corresponding points is reduced to all base stations a total of 4 sets of corresponding points); in addition, when a plurality of receivers to establish contact with the tracker, can The global optimal position and attitude fusion results are obtained, and the results are more accurate and more robust.

【技术实现步骤摘要】
一种全局最优的光学跟踪器位姿计算方法
本专利技术属于跟踪定位
，具体涉及一种全局最优的光学跟踪器位姿计算方法，可用于动作捕捉、手术导航、虚拟现实等需要光学跟踪定位的应用领域。
技术介绍
HTCVIVE系统由发射器基站和光敏接收器构成，发射器可发出周期性光信号对跟踪区域进行扫描，接收器接收到发射器的扫描信号后，将光信号转换为数字信号，从而得到接收器相对于发射器的图像坐标，当一定数量的接收器被扫描后，可利用计算机视觉算法获得接收器组成的刚体的空间位姿。HTCVIVE拥有两个发射器扫描基站(相当于两个摄像机)，在计算位姿时，需要一个跟踪器上至少5个传感器点被某个基站扫描到才能计算跟踪器与该基站之间的位姿关系，跟踪器在使用中由于自身位置角度的变化可能存在部分传感器被遮挡情况，因此要求跟踪器上必须布设数量较多的传感器点，以保证在部分传感器点被遮挡时，仍有至少有5个传感器点能收到基站扫描信号。传感器点越多，跟踪器的体积就越大，不利于跟踪器的小型化。此外，HTCVIVE系统在进行位姿融合时，使用了加权椭圆拟合的方法，该方法要求较为严格，需要知道每个基站与跟踪器之间的相对位姿，而且只适用于两个基站的位姿数据融合，当基站(摄像机)数量更多时，没有位姿数据全局优化的能力。
技术实现思路
有鉴于此，本专利技术的目的是提供一种全局最优的光学跟踪器位姿计算方法，通过改进传统跟踪器的位姿计算方法，放宽了跟踪器位姿计算条件，即在不能计算任意单个接收器与跟踪器位姿时，仅凭跟踪器和多个接收器之间的有限信息即可完成跟踪器位姿计算。本专利技术的一种全局最优的光学跟踪器位姿计算方法，包括：步骤1、针对每一个传感器，确定其能接收到信号的发射器，将一个传感器与其能接收到信号的一个发射器作为一个发射接收组合，遍历所有传感器，统计所有所述发射接收组合数目，并记为N；步骤2、针对任意一个发射接收组合，令其中的传感器序号为j，发射器序号表示为i；则确定第j个传感器在自身刚体坐标系下的三维坐标确定第j个传感器在其能接收到信号的第i个发射器中的二维图像坐标然后建立关于该发射接收组合中互相对应的三维空间点与二维图像点的有效方程组：其中，pi1、pi2、pi3和pi4表示传感器刚体坐标系与第i个发射器的图像坐标系之间的投影关系矩阵Pi中的元素；aij＝[0，-1，vij]T，bij＝[1，0，-uij]T，其中，uij和vij分别表示二维图像坐标中两个坐标轴方向的坐标；其中，表示传感器刚体坐标系转换到发射器坐标系的旋转矩阵，表示传感器刚体坐标系转换到发射器坐标系的平移矩阵；步骤3、针对每一个发射接收组合建立一个式(1)所示的方程组，N个发射接收组合即得到N个方程组，由此形成2N维的线性方程组；步骤4、将步骤3形成的线型方程组改写成如下形式：AX＝B(2)其中A为2N×12的矩阵，X为12×1的列向量，X＝[r11，r12，r13，t1，r21，r22，r23，t2，r31，r32，r33，t3]T；B为2N×1的列向量，步骤5、当4≤N≤5时，对公式(2)进行求解的具体方法为：在X中抽取9个元素得到旋转矩阵R，表示为：R＝fR(X)并使得旋转矩阵R为酉矩阵，满足RR-1＝I和R-1＝RT，I为3×3的单位矩阵；然后将式(2)所示的线性方程组求解问题转化为以下最优化问题：s.t.fR(X)fR(X)T-I＝0即：在满足的约束条件下，使取最小值的X即为最优解，实现位姿解算；当N≥6时，采用解析法对公式(2)进行求解，得到X，实现位姿解算。所述步骤5中，采用Levenberg-Marquardt算法求解所述最优化问题。本专利技术具有如下有益效果：本专利技术改进了传统跟踪器的位姿计算方法，使用了基于全局优化思想的数学模型，使用空间点和图像点的对应关系构建线性方程组，不需要计算跟踪器相对于单个基站的位姿，也不需要进行位姿数据融合，可以直接求解跟踪器全局最优位姿；该方法不限制基站数量，充分利用了所有基站对应点的信息(即使此基站的对应点数量不足以独立计算位姿)，大大降低了跟踪器位姿的最小计算条件(将对应点数量阈值由任意一个基站至少5组对应点降低为所有基站一共4组对应点)；此外，当多个接收器与跟踪器建立联系时，可获得全局最优的位姿融合结果，并且结果更加精确、鲁棒性更强。附图说明图1为现有的HTCVIVE系统构成图；图2为本专利技术方法和HTCVIVE的方法在处理过程中的区别。具体实施方式下面结合附图并举实施例，对本专利技术进行详细描述。如图1所示，HTCVIVE系统包括1个头盔显示器以及2个手柄。头盔显示器和手柄上安装有数十个光敏接收器，当基站的红外光扫描信号被一定数量的接收器收到时，就能计算出头盔显示器和手柄的空间位置，从而实现用户的位姿跟踪。设跟踪器上第j个光敏传感器在世界坐标系下的三维坐标为Xwj＝[xj，yj，zj]T，其在第i个发射器基站中对应的图像坐标为xij＝[uij，vij]T，则根据投影成像原理，Xwj与xij的关系满足以下公式：其中j＝1,2，…J，J为传感器数目；为坐标Xwj和xij的齐次坐标表述形式(本文中如果不特别说明则均以表示A的齐次坐标)，Pi＝Ki[Rci|Tci]为第i台发射器的投影矩阵，Ki为内参数矩阵，Rci为旋转矩阵，Tci为平移矩阵，它们均可通过初始标定获得。Rci和Tci可以描述三维点坐标从世界坐标系到第i个发射器基站坐标系的变换，设该传感器点在第i个发射器基站坐标系下的三维坐标为Xcij，则Xcij与Xwj的关系如公式(2)所示：Xcij＝RciXwj+Tci(2)设该传感器点在跟踪刚体局部坐标系下的三维坐标为Xrj，根据投影成像原理，也可得到类似公式(1)的成像模型，如公式(3)所示：其中Rri、Tri描述了三维点从跟踪刚体局部坐标系到第i个发射器基站坐标系的变换，如公式(4)所示：Xcij＝RriXrj+Tri(4)结合公式(2)和(4)可以得到Xwj与Xrj之间的转换关系，如公式(5)所示：其中R和T即为跟踪器在世界坐标系中的位姿。由于Rci和Tci固定不变，且在初始标定阶段已经得到，故只需在使用中实时计算Rri与Tri即可根据公式(5)获得跟踪器的三维位姿。回到公式(3)，由于Ki为已知标定数据，故只需知道若干组对应的即可求取Rri、Tri。这种内参数矩阵已知的情况下，用n个空间点与它们对应的图像点估计摄像机位姿，即旋转矩阵与平移矩阵的方法，称为PnP(perspective-n-point)问题，它可以分为两类，一类是3≤n≤5的情况，另一类是n≥6的情况。第一类PnP问题的研究焦点为确定问题的实解最多可达多少个，结论包括：P3P问题最多有4个解；当4个控制点共面时，P4P问题有唯一解，而当4个控制点不共面时，P4P问题最多有4个解；P5P问题最多可以有两个解。第二类PnP问题可以用DLT(DirectLinearTransform)方法线性求解。关于PnP问题的详细讨论可以参考文献[1]([1]WuY,HuZ.PnPProblemRevisited[J].JournalofMathematicalImagingandVision,2006,24(1):131-141)，这里不再赘述。HTCVIVE系统拥有两个基站，对于一个跟踪器，本文档来自技高网...

【技术保护点】
一种光学跟踪器位姿计算方法，其特征在于，包括：步骤1、针对每一个传感器，确定其能接收到信号的发射器，将一个传感器与其能接收到信号的一个发射器作为一个发射接收组合，遍历所有传感器，统计所有所述发射接收组合数目，并记为N；步骤2、针对任意一个发射接收组合，令其中的传感器序号为j，发射器序号表示为i；则确定第j个传感器在自身刚体坐标系下的三维坐标

【技术特征摘要】
1.一种光学跟踪器位姿计算方法，其特征在于，包括：步骤1、针对每一个传感器，确定其能接收到信号的发射器，将一个传感器与其能接收到信号的一个发射器作为一个发射接收组合，遍历所有传感器，统计所有所述发射接收组合数目，并记为N；步骤2、针对任意一个发射接收组合，令其中的传感器序号为j，发射器序号表示为i；则确定第j个传感器在自身刚体坐标系下的三维坐标确定第j个传感器在其能接收到信号的第i个发射器中的二维图像坐标然后建立关于该发射接收组合中互相对应的三维空间点与二维图像点的有效方程组：其中，pi1、pi2、pi3和pi4表示传感器刚体坐标系与第i个发射器的图像坐标系之间的投影关系矩阵Pi中的元素；aij＝[0,-1,vij]T，bij＝[1,0,-uij]T，其中，uij和vij分别表示二维图像坐标中两个坐标轴方向的坐标；其中，表示传感器刚体坐标系转换到发射器坐标系的旋转矩阵，表...

【专利技术属性】
技术研发人员：翁冬冬，李冬，胡翔，
申请(专利权)人：北京理工大学，南昌虚拟现实检测技术有限公司，
类型：发明
国别省市：北京,11