一种三维空间跟踪定位系统及方法与流程

本发明涉及物体在三维空间的跟踪定位技术领域，具体涉及一种三维空间跟踪定位系统及方法。

背景技术：

三维空间的跟踪定位是虚拟现实VR、增强现实AR和其它一些机器人领域的关键技术。常见的定位方案有激光定位、光学定位以及基于计算机视觉的定位，其中光学定位又分为主动式和被动式两种。激光定位和光学定位存在成本高、可检测范围小、难以实现多目标定位等问题。

传统的基于计算机视觉的定位方法，分为标志物法(Marker)，并行跟踪映射法(PTAM)，关键点模板跟踪法(Keypoint Template Tracking)，单应矩阵法(Homography-based)等。这些方法或对于场景有限制要求，或对于设备的计算能力有较高的要求，并且容易受到环境光的影响，不能处理遮挡，精确度和鲁棒性都不够高，因此难以做到商用级别。

因此，如何设计一种能够解决上述问题的三维空间跟踪定位系统或方法，是亟待解决的问题。

技术实现要素：

针对现有技术中的缺陷，本发明提供一种三维空间跟踪定位系统及方法，实现了对三维空间的高效且可靠的跟踪定位，且对定位场景及设备计算能力均无特殊要求，实现了大范围、不限人数的高精度、高鲁棒性的空间定位技术。

为解决上述技术问题，本发明提供以下技术方案：

一方面，本发明提供了一种三维空间跟踪定位系统，所述系统包括：图像获取单元、固定设置在目标定位室内的二维编码显示单元、以及，与所述图像获取单元通信连接的三维空间位置获取单元；

所述图像获取单元用于实时获取所述目标定位室的室内图像，并将所述室内图像发送至所述三维空间位置获取单元；

所述二位编码显示单元用于显示二维编码图像；

所述三维空间位置获取单元用于识别所述室内图像，并在确定所述室内图像中包含所述二维编码图像时，根据该二维编码图像获取所述图像获取单元当前对应在目标定位室内的三维空间位置信息。

进一步的，所述三维空间位置获取单元包括：图像识别子单元和图像位置生成子单元；

所述图像识别子单元用于接收所述图像获取单元发送的室内图像，并在识别所述室内图像中是否包含有完整或部分的所述二维编码图像时，将包含有该二维编码图像的室内图像发送至所述图像位置生成子单元；

所述图像位置生成子单元用于根据接收的室内图像中的所述二维编码图像，获取所述图像获取单元当前对应在目标定位室内的三维空间位置信息。

进一步的，所述系统还包括：惯性测量单元，且所述惯性测量单元与图像获取单元均设置在虚拟现实设备上，且所述惯性测量单元与所述三维空间位置获取单元通信连接；

所述惯性测量单元用于对所述虚拟现实设备的运动轨迹和方向进行检测，并将检测结果发送至所述三维空间位置获取单元。

进一步的，所述三维空间位置获取单元还包括：传感位置生成子单元和三维空间位置发送子单元；

所述传感位置生成子单元用于接收所述惯性测量单元发送的所述检测结果，并根据所述检测结果获取所述虚拟现实设备当前对应在目标定位室内的三维空间位置信息；

所述三维空间位置发送子单元用于将所述图像位置生成子单元和所述传感位置生成子单元分别生成的三维空间位置信息按时间顺序进行汇总，得到所述虚拟现实设备当前对应在目标定位室内的三维空间位置汇总信息，并将所述三维空间位置汇总信息发送至所述虚拟现实设备或外部显示设备进行显示。

进一步的，所述二位编码显示单元为印设有所述二维编码图像的纸板，且该纸板设置在所述目标定位室的天花板上；

所述二维编码图像由N*N个单元格组成，且N为大于或等于5的整数；

其中，不同的所述二维编码图像中相同位置的单元格组的组成形式不同，所述单元格组由X*Y个单元格组成，且4≤X<N，4≤Y<N。

另一方面，本发明还提供了一种三维空间跟踪定位方法，所述方法包括：

实时获取虚拟现实设备的图像获取单元拍摄的当前目标定位室的室内图像；

识别所述室内图像，并在确定室内图像中包含所述二维编码图像时，对所述二维编码图像进行定位和方向检测，其中，所述二维编码图像由N*N个单元格组成，且N为大于或等于5的整数，不同的所述二维编码图像中相同位置的单元格组的组成形式不同；

以及，根据所述二维编码图像的定位和方向检测的结果确定所述图像获取单元当前对应在目标定位室内的三维空间位置信息。

进一步的，所述方法还包括：

根据设置在所述虚拟现实设备的惯性测量单元对所述虚拟现实设备的运动轨迹和方向进行检测，并根据所述惯性测量单元的检测结果获取所述虚拟现实设备当前对应在目标定位室内的三维空间位置信息；

在经识别确定所述室内图像中包含所述二维编码图像时，将所述图像获取单元的测量误差值调至第一阈值以下，并将所述惯性测量单元的测量误差值调至第二阈值以上，且所述第一阈值小于第二阈值；

以及，在经识别确定所述室内图像未包含所述二维编码图像时，将所述图像获取单元的测量误差值调至第二阈值以上，并将所述惯性测量单元的测量误差值调至第第一阈值以下。

进一步的，所述方法还包括：

将分别由所述二维编码图像以及所述惯性测量单元获取的三维空间位置信息按时间顺序进行汇总，得到所述虚拟现实设备当前对应在目标定位室内的三维空间位置汇总信息；

以及，将所述三维空间位置汇总信息发送至所述虚拟现实设备或外部显示设备进行显示。

进一步的，所述在确定室内图像中包含所述二维编码图像时，对所述二维编码图像进行定位和方向检测，包括：

在识别到某一室内图像中包含所述二维编码图像时，获取所述二维编码图像的单元格组，其中，所述单元格组由X*Y个单元格组成，且4≤X<N，4≤Y<N；

根据所述二位编码图案的所述单元格组中的单元格分布状态，确定所述二位编码图案的三维空间位置信息；

以及，根据所述图像获取单元的内置参数和所述二位编码图案的三维空间位置信息，确定所述图像获取单元当前对应在目标定位室内的三维空间位置信息。

进一步的，所述根据惯性测量单元对所述虚拟现实设备的运动轨迹和方向进行检测，包括：

根据惯性测量单元及扩展卡尔曼滤波器对所述虚拟现实设备的运动轨迹和方向进行检测及外部信号矫正。

由上述技术方案可知，本发明所述的一种三维空间跟踪定位系统及方法，系统包括图像获取单元、固定设置在目标定位室内的二维编码显示单元、以及，与图像获取单元通信连接的三维空间位置获取单元；方法包括：实时获取虚拟现实设备的图像获取单元拍摄的当前目标定位室的室内图像；识别室内图像，并在确定室内图像中包含二维编码图像时，对二维编码图像进行定位和方向检测；根据二维编码图像的定位和方向检测的结果确定图像获取单元当前对应在目标定位室内的三维空间位置信息。本发明实现了对三维空间的高效且可靠的跟踪定位，且对定位场景及设备计算能力均无特殊要求，实现了大范围、不限人数的高精度、高鲁棒性的空间定位技术。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例一的一种三维空间跟踪定位系统的第一种具体实施方式的结构示意图；

图2是本发明实施例二的一种三维空间跟踪定位系统的第二种具体实施方式的结构示意图；

图3是本发明实施例三的定位系统中三维空间位置获取单元30的具体实施方式的结构示意图；

图4是本发明实施例四中的一种三维空间跟踪定位方法的第一种具体实施方式的流程示意图；

图5是本发明实施例五中的一种三维空间跟踪定位方法的第二种具体实施方式的流程示意图；

图6是本发明实施例六中的一种三维空间跟踪定位方法的第三种具体实施方式的流程示意图；

图7是本发明实施例七中的定位方法中的步骤200的流程示意图；

图8是本发明应用实例中的一种二维编码图案的举例示意图；

图9是本发明应用实例中的人为将世界坐标系定义在二维编码图案的中心的举例示意图；

图10是本发明应用实例中的摄像头拍摄到的带有二维编码图案的识别示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的实施例一提供了一种三维空间跟踪定位系统的第一种具体实施方式，参见图1，该三维空间跟踪定位系统具体包括如下内容：

图像获取单元10、固定设置在目标定位室内的二维编码显示单元20、以及，与所述图像获取单元10通信连接的三维空间位置获取单元30。

在图像获取单元10中，所述图像获取单元10用于实时获取所述目标定位室的室内图像，并将所述室内图像发送至所述三维空间位置获取单元30；其中，图像获取单元10可以为设置在所述虚拟现实设备上的摄像头，而本发明所述的虚拟现实设备可以为虚拟现实VR、增强现实AR和其它一些机器人设备。

在二维编码显示单元20中，所述二位编码显示单元20用于显示二维编码图像21，所述二位编码显示单元为印设有所述二维编码图像的纸板，且该纸板设置在所述目标定位室的天花板上；所述二维编码图像21由N*N个单元格组成，为根据实际设置需求自定义的特殊二维编码图案，所述单元格的填充颜色为黑或白，也可以为其他形式的填充，且N为大于或等于5的整数，不同的所述二维编码图像21中相同位置的单元格组的组成形式不同，所述单元格组由X*Y个单元格组成，且4≤X<N，4≤Y<N；在本实施例中，二维编码图案可以为“De Bruijn(德布鲁因)”二维编码图，这种编码图案的中任意一个固定大小的单元X*Y(不一定是正方形单元)的图案都是唯一的；因此只要摄像机拍到含有一个单元以上的图案，即能进行识别跟定位。另外，印设有所述二维编码图像的纸板可以根据应用目标定位室的房间面积而改变大小，即相对较大的房间设置包含较大的二维编码图像21的纸张，相对较小的房间设置包含较小的二维编码图像21的纸张。

在三维空间位置获取单元30中，所述三维空间位置获取单元30用于识别所述室内图像，并在确定所述室内图像中包含所述二维编码图像21时，根据该二维编码图像21获取所述图像获取单元10当前对应在目标定位室内的三维空间位置信息，其中，所述识别到所述二维编码图像21为完全识别到所述二维编码图像21，或者由于所述二维编码图像21被部分遮挡而识别到部分所述二维编码图像21。

从上述描述可知，本发明的实施例实现了对三维空间的高效且可靠的跟踪定位，且对定位场景及设备计算能力均无特殊要求，实现了大范围、不限人数的高精度、高鲁棒性的空间定位技术。

本发明的实施例二提供了一种三维空间跟踪定位系统的第二种具体实施方式，参见图2，该三维空间跟踪定位系统还具体包括如下内容：

惯性测量单元40，且所述惯性测量单元40与图像获取单元10均设置在虚拟现实设备上，且所述惯性测量单元40与所述三维空间位置获取单元30通信连接。

在所述惯性测量单元40中，所述惯性测量单元40用于对所述虚拟现实设备的运动轨迹和方向进行检测，并将检测结果发送至所述三维空间位置获取单元30。

从上述描述可知，本发明的实施例能够在无法识别到所述二维编码图像时，仍能够根据惯性测量单元获取虚拟现实设备当前对应在目标定位室内的三维空间位置信息，以及，能够获取图像获取单元拍摄的2帧之间的中间帧的虚拟现实设备的对应在目标定位室内的三维空间位置信息，实现了大范围、不限人数的高精度、高鲁棒性的空间定位技术。

本发明的实施例三提供了上述定位系统中三维空间位置获取单元30的具体实施方式，参见图3，该三维空间位置获取单元30看具体包括如下内容：

图像识别子单元31、图像位置生成子单元32、传感位置生成子单元33和三维空间位置发送子单元34。

在图像识别子单元31中，所述图像识别子单元31用于接收所述图像获取单元发送的室内图像，并在识别所述室内图像中是否包含有完整或部分的所述二维编码图像21时，将包含有该二维编码图像21的室内图像发送至所述图像位置生成子单元32。

在图像位置生成子单元32中，所述图像位置生成子单元32用于根据接收的室内图像中的所述二维编码图像21，获取所述图像获取单元10当前对应在目标定位室内的三维空间位置信息。

在传感位置生成子单元33中，所述传感位置生成子单元33用于接收所述惯性测量单元40发送的所述检测结果，并根据所述检测结果获取所述虚拟现实设备当前对应在目标定位室内的三维空间位置信息。

在三维空间位置发送子单元34中，所述三维空间位置发送子单元34用于将所述图像位置生成子单元32和所述传感位置生成子单元33分别生成的三维空间位置信息按时间顺序进行汇总，得到所述虚拟现实设备当前对应在目标定位室内的三维空间位置汇总信息，并将所述三维空间位置汇总信息发送至所述虚拟现实设备或外部显示设备进行显示。

从上述描述可知，本发明的实施例利用自定义的特殊二维编码图案，基于计算机视觉技术，以惯性测量单元作为辅助，实现了大范围、不限人数的高精度、高鲁棒性的空间定位技术。

本发明的实施例四提供了应用上述定位系统实现的一种三维空间跟踪定位方法的第一种具体实施方式，参见图4，该三维空间跟踪定位方法具体包括如下内容：

步骤100：实时获取虚拟现实设备的图像获取单元拍摄的当前目标定位室的室内图像。

步骤200：识别所述室内图像，并在确定室内图像中包含所述二维编码图像时，对所述二维编码图像进行定位和方向检测，其中，所述二维编码图像由N*N个单元格组成，所述单元格的填充颜色为黑或白，且N为大于或等于5的整数，不同的所述二维编码图像中相同位置的单元格组的组成形式不同。

步骤300：根据所述二维编码图像的定位和方向检测的结果确定所述图像获取单元当前对应在目标定位室内的三维空间位置信息。

从上述描述可知，本发明的实施例实现了对三维空间的高效且可靠的跟踪定位，且对定位场景及设备计算能力均无特殊要求，实现了大范围、不限人数的高精度、高鲁棒性的空间定位技术。

本发明的实施例五提供了上述定位方法的第二种具体实施方式，参见图5，该定位方法还具体包括如下内容：

步骤400：根据设置在所述虚拟现实设备的惯性测量单元对所述虚拟现实设备的运动轨迹和方向进行检测，并根据所述惯性测量单元的检测结果获取所述虚拟现实设备当前对应在目标定位室内的三维空间位置信息。

其中的根据设置在所述虚拟现实设备的惯性测量单元对所述虚拟现实设备的运动轨迹和方向进行检测为根据惯性测量单元及扩展卡尔曼滤波器对所述虚拟现实设备的运动轨迹和方向进行检测及外部信号矫正。

步骤500：在经识别确定所述室内图像中包含所述二维编码图像时，将所述图像获取单元的测量误差值调至第一阈值以下，并将所述惯性测量单元的测量误差值调至第二阈值以上，且所述第一阈值小于第二阈值。

步骤600：在经识别确定所述室内图像未包含所述二维编码图像时，将所述图像获取单元的测量误差值调至第二阈值以上，并将所述惯性测量单元的测量误差值调至第第一阈值以下。

从上述描述可知，本发明的实施例能够在无法识别到所述二维编码图像时，仍能够根据惯性测量单元获取虚拟现实设备当前对应在目标定位室内的三维空间位置信息，以及，能够获取图像获取单元拍摄的2帧之间的中间帧的虚拟现实设备的对应在目标定位室内的三维空间位置信息，实现了大范围、不限人数的高精度、高鲁棒性的空间定位技术。

本发明的实施例六提供了上述定位方法的第三种具体实施方式，参见图6，该定位方法还具体包括如下内容：

步骤700：将分别由所述二维编码图像以及所述惯性测量单元获取的三维空间位置信息按时间顺序进行汇总，得到所述虚拟现实设备当前对应在目标定位室内的三维空间位置汇总信息。

步骤800：将所述三维空间位置汇总信息发送至所述虚拟现实设备或外部显示设备进行显示。

从上述描述可知，本发明的实施例利用自定义的特殊二维编码图案，基于计算机视觉技术，以惯性测量单元作为辅助，实现了大范围、不限人数的高精度、高鲁棒性的空间定位技术。

本发明的实施例七提供了上述定位方法中步骤200的具体实施方式，参见图7，该步骤200具体包括如下内容：

步骤201：识别所述室内图像。

步骤202：在识别到某一室内图像中包含所述二维编码图像时，获取所述二维编码图像的单元格组。

在步骤202中，所述单元格组由M*M个单元格组成，且4≤M<N。

步骤203：根据所述二位编码图案的所述单元格组中的单元格分布状态，确定所述二位编码图案的三维空间位置信息。

步骤204：根据所述图像获取单元的内置参数和所述二位编码图案的三维空间位置信息，确定所述图像获取单元当前对应在目标定位室内的三维空间位置信息。

从上述描述可知，本发明的实施例提供了对所述二维编码图像进行定位和方向检测的具体方式，实现了对三维空间的高效且可靠的跟踪定位。

为进一步的说明本方案，本发明还提供了应用上述三维空间跟踪定位系统实现三维空间跟踪定位方法的应用实例，以所述单元格的填充颜色为黑或白进行说明，该应用实例具体包括如下内容：

(1)基于摄像头的精准定位技术

1、取得摄像头的图像。

2、判断图像中是否有二维编码图案。

3、对二维编码图案进行定位和方向检测。

4、通过相机的内参和二维编码图案的位置和方向信息确定相机的三维空间坐标。

(2)传感器融合技术

单一摄像头难以解决图像被完全遮挡的问题，这时候需要利用IMU来对运动轨迹和方向进行预测。但是IMU长时间工作会产生累计误差，需要外部的信号矫正。因此，利用传感器融合技术结合摄像头和IMU的优点，可以实现精准又稳定的定位方案。

具体实现过程如下：

(1)基于图像匹配的摄像头姿态估计

如图8所示，这是多种二维编码图案中的一种。所使用的二维编码图案不具有旋转对称性，可以通过摄像机所获取的带有二维编码图案的图像来确定二维编码图案的方向；并且，二位编码图案每个子块(最小到4*4)的分布是唯一的，所以对于被遮挡的二维编码图案，仍然可以通过识别子块，确定子块在二维编码图案中的位置，来确定整个二维编码图案的三维空间位置。

如图9所示，我们人为规定世界坐标系的原点在二维编码图案的中心，并且图案的边长为1。在此处采用右手坐标系，所以，根据右手定则，Z轴应该是垂直于平面向里的。图中标出了二维编码图案所在的坐标系以及角点的坐标值。

在规定了图9中世界坐标的基础上，根据相已知的摄像机内参矩阵K和二位编码图案的投影坐标x，二维编码图案的世界坐标X，可以计算出。摄像机相对于当前使用的二维编码图案的外参矩阵[R|T]。

x＝K×[R|T]×X

在计算出了摄像机相对于当前使用的二维编码图案的外参矩阵[R|T]后，对于不同视角看到的二维编码图案，根据上式，可以很快地计算出摄像机的三维坐标。

图10为对摄像机采集到的图像进行二维编码图案识别的中间过程。通过算法确定各个小方格子的中心位置，从而精准定位二维编码图案。在之后的图像中，结合上一帧图像，对二维编码图像进行快速跟踪。

(2)基于扩展卡尔曼滤波的传感器融合

扩展卡尔曼滤波器(EKF)是传感器融合最常用的工具，主要由预测和矫正两部分组成。是卡尔曼滤波器的工作流程包括：

首先用上一状态，通过状态转移矩阵对当前状态进行估计；

用当前状态的估计值，通过观测矩阵对当前测量进行估计；

计算卡尔曼增益，它代表状态或者测量的可信度；

得到当前时刻的实际测量值，通过卡尔曼增益对当前状态进行纠正。

要将不同传感器的数据进行融合，主要有两个问题：

运动模型的建立：

运动模型决定了状态转移矩阵，在预测阶段主要依靠状态转移矩阵来实现前后状态之间的变换。

参数的动态调整：

在实际应用中，不同情况下不同传感器的数据的重要性不一样，需要动态调整一些参数。当摄像头没有被遮挡的时候，我们把摄像头的测量误差设置得很低，把IMU的测量误差调高。此时，主要依靠摄像头的实际测量值。反之，当摄像头被遮挡时，我们增大摄像头的测量误差，减少IMU的测量误差。此时，主要依靠IMU的测量数据，通过EKF公式推导出摄像头的姿态。

另外，不同传感器有不同的帧率，一般来说，IMU的帧率要高于摄像头的帧率，因此在摄像头两帧之间，需要依靠IMU的数据进行预测，以保证整个系统的输出帧率足够高。

从上述描述可知，本发明的应用实例通过在摄像机图像中快速地定位并跟踪二维编码图案，从而可以计算出摄像机的三维空间坐标。并且采用了传感器融合技术，很好的结合了摄像头的精准性和IMU的预测能力，使得定位算法能够很好地处理环境光的影响，并且在被遮挡时也能准确地定位。整套定位方案具有低成本，高效率，集准确性和鲁棒性于一体。

以上实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。