CN107509245A - 一种基于htc‑vive的扩展跟踪方法 - Google Patents
一种基于htc‑vive的扩展跟踪方法 Download PDFInfo
- Publication number
- CN107509245A CN107509245A CN201710545643.8A CN201710545643A CN107509245A CN 107509245 A CN107509245 A CN 107509245A CN 201710545643 A CN201710545643 A CN 201710545643A CN 107509245 A CN107509245 A CN 107509245A
- Authority
- CN
- China
- Prior art keywords
- transmitter
- receiver
- signal
- coordinate
- time
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04B—TRANSMISSION
- H04B10/00—Transmission systems employing electromagnetic waves other than radio-waves, e.g. infrared, visible or ultraviolet light, or employing corpuscular radiation, e.g. quantum communication
- H04B10/11—Arrangements specific to free-space transmission, i.e. transmission through air or vacuum
- H04B10/114—Indoor or close-range type systems
- H04B10/116—Visible light communication
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04B—TRANSMISSION
- H04B14/00—Transmission systems not characterised by the medium used for transmission
- H04B14/02—Transmission systems not characterised by the medium used for transmission characterised by the use of pulse modulation
- H04B14/026—Transmission systems not characterised by the medium used for transmission characterised by the use of pulse modulation using pulse time characteristics modulation, e.g. width, position, interval
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W56/00—Synchronisation arrangements
- H04W56/001—Synchronization between nodes
Abstract
本发明公开了一种基于HTC‑VIVE的扩展跟踪方法,是HTC VIVE的扩展方案,其硬件在HTC VIVE原版基础上增加了同步控制器和频闪器;使用同步控制器控制多发射器基站和频闪器,利用频闪器的延时曝光对各个基站进行编码,在接收器端使用解码算法对各个发射器基站信号进行区分;本发明从技术原理上解决了原版HTC VIVE设备多基站信号干扰的问题,可以级联使用任意数量的发射器而不降低跟踪数据刷新率。发射器数量越多,其扫描覆盖范围越大,本发明可以支持更多数量的发射器,因此可以将跟踪区域扩展到更大范围。
Description
技术领域
本发明属于跟踪定位技术领域,具体涉及一种基于HTC-VIVE的扩展跟踪方法,可用于动作捕捉、手术导航、虚拟现实等需要跟踪定位的应用领域。
背景技术
HTC VIVE系统由发射器基站和光敏接收器构成。发射器基站可发出周期性光信号对跟踪区域进行扫描,接收器接收到发射器的扫描信号后,将光信号转换为数字信号,从而得到接收器相对于发射器的图像坐标,当一定数量的接收器被扫描后,可利用计算机视觉算法获得接收器组成的刚体的空间位姿。
HTC VIVE在跟踪时需要发射器先发出一帧同步扫描信号,然后再依次对水平和垂直方向进行扫描。当多个发射器级联使用时,为了避免信号干扰,同一时间段内只能让一个发射器工作,这导致该系统在多发射器级联使用时的刷新率成倍下降。由于跟踪区域越大,需要的发射器越多,因此为了保证足够的跟踪数据刷新率,目前的HTC VIVE系统只使用两个发射器,而其跟踪区域也被限制在5mx5m的空间内。
发明内容
有鉴于此,本发明的目的是提供一种基于HTC VIVE的扩展跟踪方法,其为HTCVIVE基站添加了一个同步控制器和频闪器,通过上述设备实现了多基站信号同步,将同时工作的基站数目由原来的2个增加为数十个(理论上无上限),从而大大提升了跟踪系统的跟踪范围。本发明可用于动作捕捉、手术导航、虚拟现实等需要跟踪定位的应用领域。
一种基于HTC VIVE的扩展跟踪方法,包括:
步骤1、为HTC VIVE系统增加需要数量的发射器,并为系统中每一个发射器配置一个频闪器;其中,m表示发射器的编号;m=1,2,…,L;L表示发射器总数目;
步骤2、设定发射器内置的LED工作模式,即:在连续两个扫描周期的前一个扫描周期中,等时间间隔发出控制LED发光的两个同步脉冲P1,并将该LED发光信号记为P11;在后一个扫描周期中,等时间间隔发出控制LED发光的两个同步脉冲P2,并将该LED发光信号记为P12,其中脉冲P1的宽度大于P2的宽度;
步骤3、对于发射器m,在所述前一个扫描周期中,发出第一个同步脉冲P1后,间隔ΔT=mΔt时间向发射器m配置的频闪器发出控制脉冲,驱动频闪器闪烁一次,其中,控制脉冲宽度等于同步脉冲P2的宽度;其中,时间间隔Δt满足Δt<T/L,其中T为扫描周期;
步骤4、在所述后一个扫描周期内,向所有发射器先后发出X轴扫描脉冲信号和Y轴扫描脉冲信号,控制所有发射器同时开始扫描;其中,一个扫描周期中,前半周期进行X轴扫描,后半周期进行Y轴扫描;
步骤5、在所述前一个扫描周期,接收器接收到本扫描周期内第一个LED光信号P11和频闪器光信号,并根据二者的持续时间长度对两者进行区分;然后计算各个频闪器光信号与第一个LED光信号P11的接收时间差ΔT;最后根据ΔT=mΔt识别出接收器能接收到信号的发射器的编号m;
步骤6、在所述后一个扫描周期,接收器接收到本扫描周期内第一个LED光信号P12以及各个发射器的X轴扫描信号和Y轴扫描信号;分别计算各个X轴扫描信号与第一个LED光信号P12的时间差,得到接收器与各个发射器之间的X轴扫描时间;分别计算各个Y轴扫描信号与第一个LED光信号P12的时间差,得到接收器与各个发射器之间的Y轴扫描时间;
步骤7、根据步骤5得到的接收器能接收到信号的发射器的编号,以及步骤6得到的接收器与各个发射器之间的X轴扫描时间和Y轴扫描时间,确定各个X轴扫描时间和Y轴扫描时间与发射器的对应关系,根据该对应关系最后确定接收器在各个发射器坐标系内的坐标,完成接收器的跟踪。
较佳的,所述步骤7中,确定各个X轴扫描时间和Y轴扫描时间与发射器的对应关系的具体方法如下:
步骤71、按照一个发射器对应一个X轴扫描时间和一个Y轴扫描的方式,则步骤6得到的X轴扫描时间和Y轴扫描时间与能接收到信号的M个发射器的对应方式共有
步骤72、在步骤71确定的每一种对应方式下,计算接收器在对应的发射器坐标系下的坐标;如此得到种坐标组合;
步骤73、针对每一种坐标组合,计算接收器在该坐标下的理论三维位置;
步骤74、根据步骤73得到的接收器的理论三维位置计算接收器理论上在发射器坐标系中的坐标;
步骤75、对每一种坐标组合对应的接收器理论上在发射器坐标系中的坐标按照设定的评估标准进行评估,得出其中一种最符合评估标准的坐标组合,则该组合所指示的X轴扫描时间和Y轴扫描时间与发射器的对应关系即为最终的对应关系。
较佳的,设定的评估标准为判断判别函数f(xij)是否小于设定阈值,如果小于则表示符合评估标准;其中判别函数为:其中xij表示步骤75中接收器理论上在发射器坐标系中的坐标,xij*是xij′的非齐次坐标形式xij′=[xij*,1]T。
较佳的,所述设定阈值为1。
本发明具有如下有益效果:
本发明是HTC VIVE的扩展方案,其硬件在HTC VIVE原版基础上增加了同步控制器和频闪器。该发明使用同步控制器控制多发射器基站和频闪器,利用频闪器的延时曝光对各个基站进行编码,在接收器端使用解码算法对各个发射器基站信号进行区分。本发明从技术原理上解决了原版HTC VIVE设备多基站信号干扰的问题,可以级联使用任意数量的发射器而不降低跟踪数据刷新率。发射器数量越多,其扫描覆盖范围越大,本发明可以支持更多数量的发射器,因此可以将跟踪区域扩展到更大范围。
附图说明
图1为现有的HTC VIVE系统构成图;
图2为HTC VIVE系统中一个发射器工作时的接收器响应波形图;
图3为HTC VIVE系统中基站扫描信号与接收器响应关系图;
图4为本发明基于HTC VIVE系统构成的扩展跟踪系统结构图;
图5为本发明的跟踪方法中发射器信号与接收器响应脉冲图;
图6为本发明的跟踪方法中图像坐标对应关系判别方法流程图。
具体实施方式
下面结合附图并举实施例,对本发明进行详细描述。
由于本发明是基于HTC VIVE系统改进而来,因此首先对HTC VIVE的工作原理进行简要介绍。如图1所示,HTC VIVE的跟踪系统包含2个发射器基站、1个头盔显示器以及2个手柄。头盔显示器和手柄上安装有数十个光敏接收器,当基站的红外光扫描信号被一定数量的接收器收到时,就能计算出头盔显示器和手柄的空间位置,从而实现用户的位姿跟踪。
虽然接收器只要收到一个基站发射的光信号就能实现跟踪,但是为了避免遮挡和扩大跟踪范围,系统使用了两个发射器基站,由控制器分配工作时间,以保证同一时间段内只有一个发射器对跟踪区域进行扫描。对于一个发射器,工作时首先让其内置红外LED灯闪烁一次,照亮整个跟踪区域,接收器收到该信号作为一帧信息的起始,随后发射器使用面激光器沿X方向对跟踪区域进行扫描,接收器可记录下接收到X方向扫描信号与起始信号之间的时间差t1。随后发射器再发射一帧起始信号,待接收器接收后,使用面激光器沿着Y方向扫描,接收器可记录下接收到Y方向扫描信号与该帧起始信号之间的时间差t2,如图2所示。
图2中可以看出,起始信号的脉冲宽度比扫描信号的更宽,据此可以区分起始信号和扫描信号。若面激光器的扫描角速度为ω,则接收器在该发射器中的图像坐标可表示为:
两个发射器基站在工作时可以通过同步线缆进行连接,以保证两者信号不会互相干扰,此时两个发射器一个工作在b模式下,一个工作在a模式下,基站的扫描信号与接收器收到的脉冲信号可用图3描述。
由图3可以看出,b模式基站和a模式基站的同步信号在每个周期都正常发出,且b模式基站同步信号的脉宽在第1个周期较宽而在第2个周期较窄。a模式基站的同步信号脉宽与b模式基站相反,在第1个周期较窄而在第2个周期较宽。b模式基站的扫描信号只在第1个周期工作,而a模式基站的扫描信号只在第2个周期工作。当两个基站同时工作时,接收器可以根据扫描信号之前的两个同步信号的宽窄顺序判断该扫描信号为哪个基站发出。以图3为例,tbx与tby对应的扫描信号之前的同步信号脉宽顺序为先窄后宽,则该扫描信号由b模式基站发出,tax与tay对应的扫描信号之前的同步信号脉宽顺序为先宽后窄,则该扫描信号由a模式基站发出,由此就可以计算出接收器在2个发射器基站中的图像坐标。
HTC VIVE的基站有b、a两种工作模式,因此整个跟踪系统的发射器基站数量上限是2台,再增加发射器基站就无法区分接收器上的信号来源。本发明在不改变HTC VIVE原有硬件的基础上,通过增加同步控制器和频闪器来实现基站数量的扩展,其系统结构如图4所示。
图4以3个发射器基站的情况为例展示了本发明的扩展方案,其中每个发射器都配置一个频闪器,与之配对使用,它们都用线缆与同步控制器相连,受到同步控制器的控制。频闪器的核心是一个受控LED,它的功能与发射器内置的红外LED相同,即可根据信号脉冲控制发光或者暗灭。该扩展系统内的所有发射器基站均工作在a模式下,利用同步控制器发出的同步信号使它们的扫描周期同步。先给发射器编号,第m个发射器用发射器m表示;在工作时,在连续两个扫描周期的前一个扫描周期中,等间隔发出控制LED发光的两个同步脉冲P1,并将该LED发光信号记为P11;在后一个扫描周期中,等间隔发出控制LED发光的两个同步脉冲P2,并将该LED发光信号记为P12,其中脉冲P1的宽度大于P2的宽度。
在第前一个扫描周期,同步控制器发出第一个同步脉冲P1的同时,即控制内置LED发出起始信号后,对于第m个发射器,间隔ΔT=mΔt时间,向与发射器m配对的频闪器发射控制脉冲,驱动频闪器闪烁一次,其中,控制脉冲宽度等于同步脉冲P2的宽度。其中,时间间隔Δt满足Δt<T/L,其中T为扫描周期。
接收器接收到内置LED和频闪器的信号,根据内置LED信号与频闪器发光信号的脉冲宽度对两者进行识别,然后可以获得频闪器与LED起始信号的时间差ΔT识别发射器的ID号,此时发射器的扫描信号与接收器的响应脉冲如图5所示。在后一个扫描周期,同步控制器先后发出X轴扫描脉冲信号和Y轴扫描脉冲信号,控制所有发射器同时开始扫描。其中,一个扫描周期中,前半周期进行X轴扫描,后半周期进行Y轴扫描。
接收器的响应波形会根据其接收到的发射器信号数量变化,其中为最复杂的是接收到所有3个接收器信号的情况,下面将以该条件为例说明如何计算接收器相对于每个发射器的图像坐标。在接收器响应信号的第1个周期内(0~T),可以根据脉宽的不同,较为容易的提取出3个基站的ID编码脉冲,并计算出它们相对于起始信号的时间分别为ΔTa、ΔTb、ΔTc,根据编码规则ΔT=n·Δt,通过计算ΔT/Δt就可以得到3个发射器的ID,由此得知接收器分别收到了编号为“1”、“2”、“3”的3个发射器的信号。在第2个扫描周期内(T~2T),由于接收器与发射器的位置关系的不同,虽然发射器是同时发出扫描激光,但是接收器接收到不同位置的发射器的时间不同,因此,可根据接收到激光信号的先后关系,并基于第二周期的扫描起始信号,得到各个发射器的扫描信号时间,X方向为txa,txb,txc,Y方向为tya,tyb,tyc。
在第一个周期内,确定了接收器能接收到几个发射器的扫描信号,并能够确定发射器编号;在第二个周期内,确定了能接收到信号的发射器的扫描信号时间,下一步需要确定扫描信号时间与发射器编号的对应关系,该对应可根据申请号为201710024939.5的专利申请确定,由此完成了接收器的跟踪。
其中,上述的专利申请记载的方案如下描述:
将发射器等效为摄像机,则发射器发射激光进行扫描过程即等效为摄像机拍摄目标的过程。将与发射器z轴垂直的平面相距设定距离的平面定义为图像平面,则需要确定接收器在各个发射器的图像平面的坐标,假设发射器与接收器扫描时间信号的对应关系如下表所示:
发射器1 | 发射器2 | 发射器3 | |
X方向扫描时间 | txa | txb | txc |
Y方向扫描时间 | tya | tyb | tyc |
X方向图像坐标 | u1=tan(ωtxa) | u2=tan(ωtxb) | u3=tan(ωtxc) |
Y方向图像坐标 | v1=tan(ωtya) | v2=tan(ωtyb) | v3=tan(ωtyc) |
表1初始假设的发射器与接收器扫描时间信号的对应关系
设接收器在世界坐标系下的三维坐标为Xw=[x,y,z]T,其在不同发射器中对应的图像坐标为xi=[ui,vi]T,i=1,2,3...,m代表发射器的编号,则根据投影成像原理,Xw与xi的关系满足以下公式:
其中为坐标Xw和xi的齐次坐标表述形式,Pi为第i台发射器的投影矩阵,可通过初始标定获得。
(2)在使用3个发射器时,每个接收器的X方向坐标集合{ui}对应各发射器共有种排列方式,Y方向坐标集合{vi}同样也有种排列方式,则一个接收器在一个扫描周期内对应的发射器坐标组合总数为:
(3)所有图像坐标组合中只有一种是正确的组合,其它都是错误的。为了从S种组合中选出唯一正确的一组,需要对各个组合方式进行评估。按顺序从S种组合中选出一组,设该组中每个发射器对应的接收器图像坐标为xij(j=1,2,3...,S,表示第j种图像坐标组合,i=1,2,3,表示发射器序号),它对应的齐次图像坐标记为
(4)根据公式(2),可以得到接收器与3个发射器之间的投影方程:
其中P1,P2,P3为3个发射器对应的投影矩阵,可通过初始标定获得。
将代入公式(2)可以得到公式(5),其中λ为未知非零系数(根据齐次坐标定义,当λ≠0时,与等价,表示同一个坐标)。
若设Pi=[pi1,pi2,pi3]T(pi1,pi2,pi3分别为矩阵Pi的三行),则公式(5)可以展开写为以下三个方程:
若和Pi=[pi1,pi2,pi3]T为已知条件,则由公式(6)中的第3个方程可得到λ的值,再将λ代入前两个方程即可得到未知数为的两个独立方程。由此可知,一组三维空间坐标到二维图像坐标的投影方程可提供2个关于的独立方程。
公式(4)中包含3组投影方程,共能提供2×3=6个关于的独立方程,而接收器的空间三维坐标(齐次坐标形式)为未知数,它包含3个未知数[x,y,z]T,(接收器的非齐次空间坐标表述为Xw=[x,y,z]T,),因此可以通过求解超定线性方程组的方式求得在最小二乘意义上的最优解,继而获得接收器在第j组图像坐标组合下的理论三维位置,记为(齐次坐标形式)。
(5)将步骤(4)中获得的接收器理论三维位置代入公式(2)中,可以计算出在第j种图像坐标组合情况下,接收器在3个发射器中对应的理论图像坐标xij′(i=1,2,3),如下所示:
(6)使用判别函数f(xij)对第j种图像坐标组合及三维重建结果进行评估,判别函数的具体形式如公式(8)所示,其中xij*是xij′的非齐次坐标形式(xij′=[xij*,1]T),m为发射器的个数(本例中为3):
若当前图像坐标组合与正确情况相符,那么判别函数的理论取值应该为0,若图像坐标组合错误,则判别函数取值必大于0。实际应用中考虑到传感器噪声和计算误差等因素,可以设置一个阈值thresh=1,当判别函数取值小于thresh时,认为当前尝试的第j组图像坐标组合与实际情况相符,计算结果Xw有效。若判别函数取值大于thresh,则选取新的图像坐标组合,重复(3)-(7)的步骤进行验证,直至找到有效的接收器三维坐标Xw为止。
利用上述判别方法,本发明可以正确筛选出唯一正确的图像坐标组合,从而成功计算出接收器的三维坐标。上述文字描述的计算流程可用图6表示。
通过上述判别方法,可以得到本例中正确的图像坐标(扫描时间)对应关系,如表2所示。
表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)所示:
Ccij=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(DirectLinear Transform)方法线性求解。关于PnP问题的详细讨论可以参考文献[1]([1]Wu Y,HuZ.PnP Problem Revisited[J].Journal of Mathematical Imaging and Vision,2006,24(1):131-141),这里不再赘述。
HTC VIVE系统拥有两个基站,对于一个跟踪器,设第一个基站拍摄到了跟踪器上p1个传感器的图像坐标,第二个基站拍摄到了p2个传感器的图像坐标,那么HTC VIVE系统要求当p1≥5或p2≥5时,才能计算出该跟踪器的位姿。当p1≥5且p2≥5时,两个基站可以各自根据公式(5)得到跟踪器的空间位姿,可分别记为R1、T1、R2、T2。此时需要将两个基站得到的位姿数据进行融合以获得精度更高、鲁棒性更强的跟踪器位姿。HTC VIVE使用的位姿融合算法如公式(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的列向量。
由此本发明将求取跟踪器位姿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=RT,I为3×3的单位矩阵,故可以得到约束条件:RRT=I,即RRT-I=0。由此可以将式(17)所示的线性方程组求解问题转化为以下最优化问题:
式(19)所示的最优化问题可以通过迭代法求解,一种常用的方法为Levenberg-Marquardt算法,其细节可参考文献[3](MoréJ J.The Levenberg-Marquardt algorithm:Implementation and theory[J].Lecture Notes in Mathematics,1978,630:105-116)。
下面总结一下本发明求取跟踪器位姿的步骤:
步骤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时,采用Levenberg-Marquardt解析法对公式(2)进行求解,得到X,实现位姿解算。
本发明从全局优化的角度出发计算跟踪器的三维位姿,和目前典型方法的代表HTC VIVE系统相比,本发明方法放宽了跟踪器位姿计算条件,同时支持基站数量超过2台时的位姿数据融合,计算结果更加精确鲁棒。图2对比了本发明方法和HTC VIVE方法在处理过程中的区别。
可以看出,HTC VIVE使用的是基于分布式思想的计算方法,需要单独计算跟踪器相对于各个基站的位姿,再将它们进行融合。本发明是基于全局优化思想的计算方法,不考虑跟踪器相对于各个基站的位姿,只将其对应点信息用于构建线性方程组,通过求解线性方程组得到跟踪器的全局最优位姿,不需要数据融合。
举例来说,假设一个跟踪器上被第i个基站拍摄到的传感器点数量记为pi,i=1,2,…,M(这里将基站数量记为M),对于HTC VIVE系统M=2,它必须满足至少一个pi≥5时,才能计算跟踪器位姿。当p1≥5且p2≥5时,它计算了跟踪器相对于两个基站的位姿,需要使用公式(6)进行位姿数据的融合以获得最终结果。对于本发明而言,基站的数目M不受限制,只需满足即可计算跟踪器的位姿,这大大降低了位姿计算的条件。例如当p1=2,p1=2,p2=2时,HTC VIVE系统不能计算位姿,而本发明方法却可以计算位姿。又如当p1=5,p2=3时,HTC VIVE系统只能计算跟踪器相对于基站1的位姿,而跟踪器相对于基站2的位姿由于对应点数量不足无法计算,这相当于浪费了基站2的3组对应点信息。本发明方法根据公式(16)和(17),能全部利用所有对应点信息,因此计算结果将更加精确鲁棒。下1对比了本发明方法与HTC VIVE方法的性能差异。
表1本发明方法与HTC VIVE方法的性能对比
综上所述,以上仅为本发明的较佳实施例而已,并非用于限定本发明的保护范围。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (4)
1.一种基于HTC VIVE的扩展跟踪方法,其特征在于,包括:
步骤1、为HTC VIVE系统增加需要数量的发射器,并为系统中每一个发射器配置一个频闪器;其中,m表示发射器的编号;m=1,2,…,L;L表示发射器总数目;
步骤2、设定发射器内置的LED工作模式,即:在连续两个扫描周期的前一个扫描周期中,等时间间隔发出控制LED发光的两个同步脉冲P1,并将该LED发光信号记为P11;在后一个扫描周期中,等时间间隔发出控制LED发光的两个同步脉冲P2,并将该LED发光信号记为P12,其中脉冲P1的宽度大于P2的宽度;
步骤3、对于发射器m,在所述前一个扫描周期中,发出第一个同步脉冲P1后,间隔ΔT=mΔt时间向发射器m配置的频闪器发出控制脉冲,驱动频闪器闪烁一次,其中,控制脉冲宽度等于同步脉冲P2的宽度;其中,时间间隔Δt满足Δt<T/L,其中T为扫描周期;
步骤4、在所述后一个扫描周期内,向所有发射器先后发出X轴扫描脉冲信号和Y轴扫描脉冲信号,控制所有发射器同时开始扫描;其中,一个扫描周期中,前半周期进行X轴扫描,后半周期进行Y轴扫描;
步骤5、在所述前一个扫描周期,接收器接收到本扫描周期内第一个LED光信号P11和频闪器光信号,并根据二者的持续时间长度对两者进行区分;然后计算各个频闪器光信号与第一个LED光信号P11的接收时间差ΔT;最后根据ΔT=mΔt识别出接收器能接收到信号的发射器的编号m;
步骤6、在所述后一个扫描周期,接收器接收到本扫描周期内第一个LED光信号P12以及各个发射器的X轴扫描信号和Y轴扫描信号;分别计算各个X轴扫描信号与第一个LED光信号P12的时间差,得到接收器与各个发射器之间的X轴扫描时间;分别计算各个Y轴扫描信号与第一个LED光信号P12的时间差,得到接收器与各个发射器之间的Y轴扫描时间;
步骤7、根据步骤5得到的接收器能接收到信号的发射器的编号,以及步骤6得到的接收器与各个发射器之间的X轴扫描时间和Y轴扫描时间,确定各个X轴扫描时间和Y轴扫描时间与发射器的对应关系,根据该对应关系最后确定接收器在各个发射器坐标系内的坐标,完成接收器的跟踪。
2.如权利要求1所述的一种基于HTC VIVE的扩展跟踪方法,其特征在于,所述步骤7中,确定各个X轴扫描时间和Y轴扫描时间与发射器的对应关系的具体方法如下:
步骤71、按照一个发射器对应一个X轴扫描时间和一个Y轴扫描的方式,则步骤6得到的X轴扫描时间和Y轴扫描时间与能接收到信号的M个发射器的对应方式共有
步骤72、在步骤71确定的每一种对应方式下,计算接收器在对应的发射器坐标系下的坐标;如此得到种坐标组合;
步骤73、针对每一种坐标组合,计算接收器在该坐标下的理论三维位置;
步骤74、根据步骤73得到的接收器的理论三维位置计算接收器理论上在发射器坐标系中的坐标;
步骤75、对每一种坐标组合对应的接收器理论上在发射器坐标系中的坐标按照设定的评估标准进行评估,得出其中一种最符合评估标准的坐标组合,则该组合所指示的X轴扫描时间和Y轴扫描时间与发射器的对应关系即为最终的对应关系。
3.如权利要求2所述的一种基于HTC VIVE的扩展跟踪方法,其特征在于,设定的评估标准为判断判别函数f(xij)是否小于设定阈值,如果小于则表示符合评估标准;其中判别函数为:其中xij表示步骤75中接收器理论上在发射器坐标系中的坐标,xij*是xij′的非齐次坐标形式xij′=[xij*,1]T。
4.如权利要求3所述的一种基于HTC VIVE的扩展跟踪方法,其特征在于,所述设定阈值为1。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710545643.8A CN107509245B (zh) | 2017-07-06 | 2017-07-06 | 一种基于htc vive的扩展跟踪方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710545643.8A CN107509245B (zh) | 2017-07-06 | 2017-07-06 | 一种基于htc vive的扩展跟踪方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107509245A true CN107509245A (zh) | 2017-12-22 |
CN107509245B CN107509245B (zh) | 2020-04-17 |
Family
ID=60679452
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710545643.8A Active CN107509245B (zh) | 2017-07-06 | 2017-07-06 | 一种基于htc vive的扩展跟踪方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107509245B (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108414978A (zh) * | 2018-02-08 | 2018-08-17 | 北京理工大学 | 一种可扩展的基站阵列、光学跟踪系统及其跟踪方法 |
CN108414979A (zh) * | 2018-02-08 | 2018-08-17 | 北京理工大学 | 一种基于可扩展的基站阵列的光学跟踪方法 |
CN108765498A (zh) * | 2018-05-30 | 2018-11-06 | 百度在线网络技术(北京)有限公司 | 单目视觉跟踪方法、装置及存储介质 |
CN109632263A (zh) * | 2018-12-29 | 2019-04-16 | 北京理工大学 | 一种基于光栅尺的vr头盔的移动延迟测量方法 |
CN109765035B (zh) * | 2018-12-29 | 2020-06-30 | 北京理工大学 | 一种基于渐变编码的vr头盔的移动延迟测量方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20110051608A1 (en) * | 2009-08-25 | 2011-03-03 | Korea Electrotechnology Research Institute | Wireless location determination system and method |
CN102901948A (zh) * | 2012-11-05 | 2013-01-30 | 北京半导体照明科技促进中心 | 室内定位装置及系统 |
CN106249203A (zh) * | 2016-07-07 | 2016-12-21 | 北京国承万通信息科技有限公司 | 参照光源系统、定位发射系统、定位系统及编码实现方法 |
CN106501771A (zh) * | 2016-12-14 | 2017-03-15 | 北京国承万通信息科技有限公司 | 定位光信号发射系统、方法及定位系统 |
CN106908764A (zh) * | 2017-01-13 | 2017-06-30 | 北京理工大学 | 一种多目标光学跟踪方法 |
-
2017
- 2017-07-06 CN CN201710545643.8A patent/CN107509245B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20110051608A1 (en) * | 2009-08-25 | 2011-03-03 | Korea Electrotechnology Research Institute | Wireless location determination system and method |
CN102901948A (zh) * | 2012-11-05 | 2013-01-30 | 北京半导体照明科技促进中心 | 室内定位装置及系统 |
CN106249203A (zh) * | 2016-07-07 | 2016-12-21 | 北京国承万通信息科技有限公司 | 参照光源系统、定位发射系统、定位系统及编码实现方法 |
CN106501771A (zh) * | 2016-12-14 | 2017-03-15 | 北京国承万通信息科技有限公司 | 定位光信号发射系统、方法及定位系统 |
CN106908764A (zh) * | 2017-01-13 | 2017-06-30 | 北京理工大学 | 一种多目标光学跟踪方法 |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108414978A (zh) * | 2018-02-08 | 2018-08-17 | 北京理工大学 | 一种可扩展的基站阵列、光学跟踪系统及其跟踪方法 |
CN108414979A (zh) * | 2018-02-08 | 2018-08-17 | 北京理工大学 | 一种基于可扩展的基站阵列的光学跟踪方法 |
CN108414979B (zh) * | 2018-02-08 | 2020-06-30 | 北京理工大学 | 一种基于可扩展的基站阵列的光学跟踪方法 |
CN108414978B (zh) * | 2018-02-08 | 2020-08-11 | 北京理工大学 | 一种可扩展的基站阵列、光学跟踪系统及其跟踪方法 |
CN108765498A (zh) * | 2018-05-30 | 2018-11-06 | 百度在线网络技术(北京)有限公司 | 单目视觉跟踪方法、装置及存储介质 |
US10984554B2 (en) | 2018-05-30 | 2021-04-20 | Baidu Online Network Technology (Beijing) Co., Ltd. | Monocular vision tracking method, apparatus and non-volatile computer-readable storage medium |
US11704833B2 (en) | 2018-05-30 | 2023-07-18 | Baidu Online Network Technology (Beijing) Co., Ltd. | Monocular vision tracking method, apparatus and non-transitory computer-readable storage medium |
CN109632263A (zh) * | 2018-12-29 | 2019-04-16 | 北京理工大学 | 一种基于光栅尺的vr头盔的移动延迟测量方法 |
CN109632263B (zh) * | 2018-12-29 | 2020-06-30 | 北京理工大学 | 一种基于光栅尺的vr头盔的移动延迟测量方法 |
CN109765035B (zh) * | 2018-12-29 | 2020-06-30 | 北京理工大学 | 一种基于渐变编码的vr头盔的移动延迟测量方法 |
Also Published As
Publication number | Publication date |
---|---|
CN107509245B (zh) | 2020-04-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107509245A (zh) | 一种基于htc‑vive的扩展跟踪方法 | |
CN107945265B (zh) | 基于在线学习深度预测网络的实时稠密单目slam方法与系统 | |
CN102572505B (zh) | 用于校准深度图像传感器的方法和系统 | |
CN103649680B (zh) | 用于3d扫描的传感器定位 | |
US7190826B2 (en) | Measuring the location of objects arranged on a surface, using multi-camera photogrammetry | |
CN108926355A (zh) | 用于站立对象的x射线系统和方法 | |
Burschka et al. | V-GPS (SLAM): Vision-based inertial system for mobile robots | |
GB2390792A (en) | Patient positioning system using images from two viewpoints | |
WO2001078014A1 (en) | Real world/virtual world correlation system using 3d graphics pipeline | |
CN106949845A (zh) | 基于双目立体视觉的二维激光振镜扫描系统及标定方法 | |
CN108955718A (zh) | 一种视觉里程计及其定位方法、机器人以及存储介质 | |
CN106908764B (zh) | 一种多目标光学跟踪方法 | |
CN108898676A (zh) | 一种虚实物体之间碰撞及遮挡检测方法及系统 | |
CN106705849A (zh) | 线结构光传感器标定方法 | |
US10186051B2 (en) | Method and system for calibrating a velocimetry system | |
JP2005017262A (ja) | 3次元測量装置及びこれを用いた3次元空間物体復元方法 | |
JP2003061936A (ja) | 動立体モデル生成装置及び方法 | |
JP2021093117A (ja) | 連帯的な検出と記述システムおよび方法 | |
CN107452036B (zh) | 一种全局最优的光学跟踪器位姿计算方法 | |
CN109363701A (zh) | 一种调整扫描机架干涉的方法、系统和装置 | |
JP7166446B2 (ja) | ロボットの姿勢を推定するシステムおよび方法、ロボット、並びに記憶媒体 | |
WO2022251605A1 (en) | Visual and rf sensor fusion for multi-agent tracking | |
JP2005241323A (ja) | 撮像システム及び校正方法 | |
JP3520050B2 (ja) | 移動物体追跡装置 | |
Xie et al. | Angular Tracking Consistency Guided Fast Feature Association for Visual-Inertial SLAM |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |