一种全局最优的光学跟踪器位姿计算方法与流程

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

背景技术：

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＝r^t，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系统拥有两个基站，对于一个跟踪器，设第一个基站拍摄到了跟踪器上p1个传感器的图像坐标，第二个基站拍摄到了p2个传感器的图像坐标，那么htcvive系统要求当p1≥5或p2≥5时，才能计算出该跟踪器的位姿。当p1≥5且p2≥5时，两个基站可以各自根据公式(5)得到跟踪器的空间位姿，可分别记为r1、t1、r2、t2。此时需要将两个基站得到的位姿数据进行融合以获得精度更高、鲁棒性更强的跟踪器位姿。htcvive使用的位姿融合算法如公式(6)所示：

其中slerp(·)为球面线性插值函数(参见文献[2]https://en.wikipedia.org/wiki/slerp)，α为系数，其计算方法如公式(7)所示：

α＝p1/(p1+p2)(7)

由于公式(6)所示的位姿融合方法只适用于两个位姿数据的融合，因此当基站数量大于2时将无法使用公式(6)进行位姿融合。

本发明提出的位姿计算方法不局限于只有两个基站的情况，它适用于任意数量基站(或者摄像机)的情况。联立公式(1)和公式(5)可以得到跟踪器的第j个传感器点在刚体局部坐标系下的三维坐标到第i个基站成像平面的图像坐标的投影关系，如下所示：

公式(8)等价于公式(9)的形式：

其中为的反对称矩阵，若则有：

设则有将m带入公式(9)可以得到：

设pi＝[pi1，pi2，pi3，pi4]，并令其中

将cij带入公式(11)，可以得到未知数为r和t的三个方程：

由于式(13)描述的是退化的齐次坐标变换，其中只有2个方程是独立的，因此只选取式(13)中的前两个方程用于求解r和t。由于故带入式(13)的前两个方程中得到：

令aij＝[0，-1，vij]^t，bij＝[1，0，-uij]^t，并将式(12)中的和带入式(14)中，得到：

将式(15)的等式两边取转置，得到：

式(16)即为一组对应的三维空间点与二维图像点产生的有效方程组，当存在n组这样的对应点时，式(16)可以改写成标准的线性方程组，如式(17)所示，其中a为2n×12的矩阵，x为12×1的列向量，b为2n×1的列向量。

ax＝b

x＝[r11，r12，r13，t1，r21，r22，r23，t2，r31，r32，r33，t3]^t(17)

由此本发明将求取跟踪器位姿r、t转换为了求解式(17)所示的线性方程组问题。注意到未知数x的维度为12，故当n≥6时，该方程可以通过x＝a⁺b的方式求取解析解，a⁺为a的广义逆。当4≤n≤5时，方程ax＝b欠定，有多个解，但可以通过增加约束条件的迭代方法求解。由于x中包含跟踪器的旋转矩阵r的全部元素，故可通过抽取x中的9个元素得到旋转矩阵r，该过程用公式(18)所示的函数表示：

r＝fr(x)(18)

因为旋转矩阵r是酉矩阵(即单位正交阵)，满足rr^-1＝i和r^-1＝r^t，i为3×3的单位矩阵，故可以得到约束条件：rr^t＝i，即rr^t-i＝0。由此可以将式(17)所示的线性方程组求解问题转化为以下最优化问题：

式(19)所示的最优化问题可以通过迭代法求解，一种常用的方法为levenberg-marquardt算法，其细节可参考文献[3](moréjj.thelevenberg-marquardtalgorithm:implementationandtheory[j].lecturenotesinmathematics,1978,630:105-116)。

下面总结一下本发明求取跟踪器位姿的步骤：

step1.利用跟踪器上某一传感器点在刚体局部坐标系下的三维坐标和其对应的某个基站的二维图像坐标按照公式(8)-(16)的过程得到2个有效方程组。

step2.将每一组传感器三维点和其对应的图像坐标点按照step1的方法得到有效方程组，再将所有方程组按照公式(17)组成形如ax＝b的线性方程组。

step3.根据对应点数量n的不同选择不同的解算方法。当n≥6时使用解析法x＝a⁺b求解，当4≤n≤5时使用式(19)所示的最优化方法求解。

本发明从全局优化的角度出发计算跟踪器的三维位姿，和目前典型方法的代表htcvive系统相比，本发明方法放宽了跟踪器位姿计算条件，同时支持基站数量超过2台时的位姿数据融合，计算结果更加精确鲁棒。图2对比了本发明方法和htcvive方法在处理过程中的区别。

可以看出，htcvive使用的是基于分布式思想的计算方法，需要单独计算跟踪器相对于各个基站的位姿，再将它们进行融合。本发明是基于全局优化思想的计算方法，不考虑跟踪器相对于各个基站的位姿，只将其对应点信息用于构建线性方程组，通过求解线性方程组得到跟踪器的全局最优位姿，不需要数据融合。

举例来说，假设一个跟踪器上被第i个基站拍摄到的传感器点数量记为pi，i＝1，2，…，m(这里将基站数量记为m)，对于htcvive系统m＝2，它必须满足至少一个pi≥5时，才能计算跟踪器位姿。当p1≥5且p2≥5时，它计算了跟踪器相对于两个基站的位姿，需要使用公式(6)进行位姿数据的融合以获得最终结果。对于本发明而言，基站的数目m不受限制，只需满足即可计算跟踪器的位姿，这大大降低了位姿计算的条件。例如当p1＝2，p1＝2，p2＝2时，htcvive系统不能计算位姿，而本发明方法却可以计算位姿。又如当p1＝5，p2＝3时，htcvive系统只能计算跟踪器相对于基站1的位姿，而跟踪器相对于基站2的位姿由于对应点数量不足无法计算，这相当于浪费了基站2的3组对应点信息。本发明方法根据公式(16)和(17)，能全部利用所有对应点信息，因此计算结果将更加精确鲁棒。下1对比了本发明方法与htcvive方法的性能差异。

表1本发明方法与htcvive方法的性能对比

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。