CN112833788B - 一种枪体定位方法、装置、设备、仿真枪及可读存储介质 - Google Patents

Abstract

本发明涉及定位技术领域，公开了一种枪体定位方法、装置、设备、仿真枪及可读存储介质，可提供一种基于视觉同步定位与建图SLMA算法实现枪体定位的新方法，即先根据由在枪体上绑定的图像传感器实时采集的图像数据和使用视觉同步定位与建图SLMA算法，获取到所述图像传感器在三维世界坐标系中的空间位置和镜头朝向姿态，然后通过几何变换得到绑定枪体在所述三维世界坐标系中的当前枪体位置坐标和当前枪口朝向角度，从而可直接自主式地对绑定枪体进行空间位置和枪口朝向的定位，使得不必再依赖于场地，也不需要在启动时进行复杂的校准，并且理论上不受同时定位枪体的数量限制，可以支持在限定1立方米范围内对10把以上枪体进行同时定位。

Description

一种枪体定位方法、装置、设备、仿真枪及可读存储介质

技术领域

本发明属于定位技术领域，具体地涉及一种枪体定位方法、装置、设备、仿真枪及可读存储介质。

背景技术

目前在应用诸如仿真枪等枪体进行娱乐或训练的过程中，出于射击管理或安全使用等方面的考虑，需要实时获取枪体的空间位置、枪口朝向、开枪击发和弹夹更换等数据，并通过无线或有线等方式将这些数据实时传送给上位机，以便进行有效监控。但是现有枪体定位方案无法满足枪体在自然环境中的随时使用和多枪同时定位需求，例如对于简单游戏型的仿真枪，其一般是使用陀螺仪或外加定位器来进行枪口朝向数据的获取，如果换地方使用，需要先对陀螺仪进行磁场校准，否则得到的最终结果非常容易产生偏差，无法在厘米级内做到枪口朝向数据的准确获取，导致获取的枪口朝向与枪口真实朝向存在较大误差，更无法做到空间位置的准确获取；此外，外加的定位器也会受到场地限制，即尽管有的定位器可以级联，但还是会受到场地限制，并且成本较高，同时枪体能活动的有效范围只能在定位器之间，并且多枪之间容易相互干扰，使得枪体易被其他物体遮挡，无法产生定位结果，存在枪体丢失风险。试验测试表明，目前很难做到在每立方中对2～3把枪体进行精准地毫米级定位，导致定位结果的鲁棒性不够高。

发明内容

为了解决现有枪体定位方案无法满足枪体在自然环境中的随时使用和多枪同时定位需求的问题，本发明目的在于提供一种新型的枪体定位方法、装置、设备、仿真枪及可读存储介质，可以基于视觉同步定位与建图SLMA算法直接自主式地对绑定枪体进行空间位置和枪口朝向的定位，使得不必再依赖于场地，也不需要在启动时进行复杂的校准，并且理论上不受同时定位枪体的数量限制，便于实际应用和推广。

第一方面，本发明提供了一种枪体定位方法，包括：

获取在枪体移动过程中由图像传感器实时采集的图像数据，其中，所述图像传感器与所述枪体绑定，所述图像数据包含有在不同离散时刻采集的当前图像数据和若干份历史图像数据；

根据所述当前图像数据和所述若干份历史图像数据，使用视觉同步定位与建图SLMA算法获取到所述图像传感器的相机光心在三维世界坐标系中的当前位置坐标和所述图像传感器的相机镜头在所述三维世界坐标系中的当前朝向角度；

根据所述当前位置坐标和所述枪体在相机坐标系中的已知枪体位置坐标，通过几何变换得到所述枪体在所述三维世界坐标系中的当前枪体位置坐标，以及根据所述当前朝向角度和所述枪体在所述相机坐标系中的已知枪口朝向角度，通过几何变换得到所述枪体在所述三维世界坐标系中的当前枪口朝向角度，其中，所述相机坐标系为以所述图像传感器的相机光心为原点且以相机光轴为Z轴建立的三维直角坐标系；

将所述当前枪体位置坐标和所述当前枪口朝向角度无线发送至上位机。

基于上述发明内容，可提供一种基于视觉同步定位与建图SLMA算法实现枪体定位的新方法，即先根据由在枪体上绑定的图像传感器实时采集的图像数据和使用视觉同步定位与建图SLMA算法，获取到所述图像传感器在三维世界坐标系中的空间位置和镜头朝向姿态，然后通过几何变换得到绑定枪体在所述三维世界坐标系中的当前枪体位置坐标和当前枪口朝向角度，从而可直接自主式地对绑定枪体进行空间位置和枪口朝向的定位，使得不必再依赖于场地，也不需要在启动时进行复杂的校准，并且理论上不受同时定位枪体的数量限制，可以支持在限定1立方米范围内对10把以上枪体进行同时定位，便于实际应用和推广。

在一个可能的设计中，将所述当前枪体位置坐标和所述当前枪口朝向角度无线发送至上位机，包括：

获取在所述枪体移动过程中由微机械陀螺仪实时采集的且在所述三维世界坐标系中的姿态数据，其中，所述微机械陀螺仪与所述枪体绑定，所述姿态数据包含有在不同离散时刻采集的当前姿态数据/和若干份历史姿态数据；

根据所述姿态数据、所述已知枪体位置坐标和所述微机械陀螺仪在所述相机坐标系中的已知陀螺仪位置坐标，通过几何变换得到所述枪体在所述三维世界坐标系中的姿态数据；

根据所述枪体在所述三维世界坐标系中的姿态数据，对所述当前枪口朝向角度进行矫正，得到新的当前枪口朝向角度；

将所述当前枪体位置坐标和所述新的当前枪口朝向角度无线发送至所述上位机。

基于上述可能设计，可以通过结合由微机械陀螺仪所采集的姿态数据，来对所述当前枪口朝向角度进行矫正，进一步提升实时定位结果的稳定性。

在一个可能的设计中，根据所述枪体在所述三维世界坐标系中的姿态数据，对所述当前枪口朝向角度进行矫正，得到新的当前枪口朝向角度,包括：

根据所述枪体在所述三维世界坐标系中的姿态数据，得到所述枪体在所述三维世界坐标系中的枪口朝向角度观测值；

通过卡尔曼滤波算法对所述枪口朝向角度观测值和所述当前枪口朝向角度进行加权平均处理，得到所述新的当前枪口朝向角度。

在一个可能的设计中，所述图像传感器为单目摄像头或双目摄像头。

在一个可能的设计中，所述图像传感器包括有红外线热成像摄像头。

第二方面，本发明提供了一种枪体定位装置，包括有依次通信连接的图像数据获取单元、位置朝向确定单元、位置朝向换算单元和定位结果传送单元；

所述图像数据获取单元，用于获取在枪体移动过程中由图像传感器实时采集的图像数据，其中，所述图像传感器与所述枪体绑定，所述图像数据包含有在不同离散时刻采集的当前图像数据和若干份历史图像数据；

所述位置朝向确定单元，用于根据所述当前图像数据和所述若干份历史图像数据，使用视觉同步定位与建图SLMA算法获取到所述图像传感器的相机光心在三维世界坐标系中的当前位置坐标和所述图像传感器的相机镜头在所述三维世界坐标系中的当前朝向角度；

所述位置朝向换算单元，用于根据所述当前位置坐标和所述枪体在相机坐标系中的已知枪体位置坐标，通过几何变换得到所述枪体在所述三维世界坐标系中的当前枪体位置坐标，以及根据所述当前朝向角度和所述枪体在所述相机坐标系中的已知枪口朝向角度，通过几何变换得到所述枪体在所述三维世界坐标系中的当前枪口朝向角度，其中，所述相机坐标系为以所述图像传感器的相机光心为原点且以相机光轴为Z轴建立的三维直角坐标系；

所述定位结果传送单元，用于将所述当前枪体位置坐标和所述当前枪口朝向角度无线发送至上位机。

在一个可能的设计中，所述定位结果传送单元包括有依次通信连接的姿态数据获取子单元、姿态数据变换子单元、枪口朝向矫正子单元和无线发送子单元；

所述姿态数据获取子单元，用于获取在所述枪体移动过程中由微机械陀螺仪实时采集的且在所述三维世界坐标系中的姿态数据，其中，所述微机械陀螺仪与所述枪体绑定，所述姿态数据包含有在不同离散时刻采集的当前姿态数据/和若干份历史姿态数据；

所述姿态数据变换子单元，用于根据所述姿态数据、所述已知枪体位置坐标和所述微机械陀螺仪在所述相机坐标系中的已知陀螺仪位置坐标，通过几何变换得到所述枪体在所述三维世界坐标系中的姿态数据；

所述枪口朝向矫正子单元，用于根据所述枪体在所述三维世界坐标系中的姿态数据，对所述当前枪口朝向角度进行矫正，得到新的当前枪口朝向角度；

所述无线发送子单元，用于将所述当前枪体位置坐标和所述新的当前枪口朝向角度无线发送至所述上位机。

在一个可能的设计中，所述枪口朝向矫正子单元包括有通信相连的观测值获取孙单元和加权平均处理孙单元；

所述观测值获取孙单元，用于根据所述枪体在所述三维世界坐标系中的姿态数据，得到所述枪体在所述三维世界坐标系中的枪口朝向角度观测值；

所述加权平均处理孙单元，用于通过卡尔曼滤波算法对所述枪口朝向角度观测值和所述当前枪口朝向角度进行加权平均处理，得到所述新的当前枪口朝向角度。

第三方面，本发明提供了一种枪体定位设备，包括有用于将所述枪体定位设备绑定在枪体上的安装结构，还包括有图像传感器、存储器、处理器和收发器；

所述图像传感器通信连接所述处理器，用于在所述枪体移动过程中实时采集图像数据；

所述存储器通信连接所述处理器，用于存储计算机程序；

所述收发器通信连接所述处理器，用于收发数据；

所述处理器用于读取所述计算机程序，执行如第一方面或第一方面中任意一种可能设计所述的枪体定位方法。

第四方面，本发明提供了一种仿真枪，包括有仿真枪本体和如第三方面所述的枪体定位设备，其中，所述枪体定位设备通过外挂方式的安装结构与所述仿真枪本体绑定。

在一个可能的设计中，还包括有通过无线模块通信连接上位机的开枪击发检测传感器和/或弹夹更换检测传感器，其中，所述无线模块包括有蓝牙模块和/或无线保真WiFi模块。

第五方面，本发明提供了一种可读存储介质，所述可读存储介质上存储有指令，当所述指令在计算机上运行时，执行如上第一方面或第一方面中任意一种可能设计的所述枪体定位方法。

第六方面，本发明提供了一种包含指令的计算机程序产品，当所述指令在计算机上运行时，使所述计算机执行如上第一方面或第一方面中任意一种可能设计的所述枪体定位方法。

附图说明

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

图1是本发明提供的枪体定位方法的流程示意图。

图2是本发明提供的枪体定位装置的结构示意图。

图3是本发明提供的枪体定位设备的结构示意图。

具体实施方式

下面结合附图及具体实施例来对本发明作进一步阐述。在此需要说明的是，对于这些实施例方式的说明虽然是用于帮助理解本发明，但并不构成对本发明的限定。本文公开的特定结构和功能细节仅用于描述本发明的示例实施例。然而，可用很多备选的形式来体现本发明，并且不应当理解为本发明限制在本文阐述的实施例中。

应当理解，尽管本文可能使用术语第一、第二等等来描述各种单元，但是这些单元不应当受到这些术语的限制。这些术语仅用于区分一个单元和另一个单元。例如可以将第一单元称作第二单元,并且类似地可以将第二单元称作第一单元，同时不脱离本发明的示例实施例的范围。

应当理解，对于本文中可能出现的术语“和/或”，其仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，单独存在B，同时存在A和B三种情况；对于本文中可能出现的术语“/和”，其是描述另一种关联对象关系，表示可以存在两种关系，例如，A/和B，可以表示：单独存在A，单独存在A和B两种情况；另外，对于本文中可能出现的字符“/”，一般表示前后关联对象是一种“或”关系。

应当理解，在本文中若将单元称作与另一个单元“连接”、“相连”或“耦合”时，它可以与另一个单元直相连接或耦合，或中间单元可以存在。相対地，在本文中若将单元称作与另一个单元“直接相连”或“直接耦合”时，表示不存在中间单元。另外，应当以类似方式来解释用于描述单元之间的关系的其他单词(例如，“在……之间”对“直接在……之间”,“相邻”对“直接相邻”等等)。

应当理解，本文使用的术语仅用于描述特定实施例，并不意在限制本发明的示例实施例。若本文所使用的，单数形式“一”、“一个”以及“该”意在包括复数形式，除非上下文明确指示相反意思。还应当理解，若术语“包括”、“包括了”、“包含”和/或“包含了”在本文中被使用时,指定所声明的特征、数量、步骤、操作、单元和/或组件的存在性,并且不排除一个或多个其他特征、数量、步骤、操作、单元、组件和/或他们的组合存在性或增加。

应当理解，还应当注意到在一些备选可能设计中，所出现的功能/动作可能与附图出现的顺序不同。例如,取决于所涉及的功能/动作,实际上可以实质上并发地执行,或者有时可以以相反的顺序来执行连续示出的两个图。

应当理解，在下面的描述中提供了特定的细节,以便于对示例实施例的完全理解。然而,本领域普通技术人员应当理解可以在没有这些特定细节的情况下实现示例实施例。例如可以在框图中示出系统，以避免用不必要的细节来使得示例不清楚。在其他实例中，可以不以非必要的细节来示出众所周知的过程、结构和技术，以避免使得示例实施例不清楚。

如图1所示，本实施例第一方面提供的所述枪体定位方法，可以但不限于由内置有视觉同步定位与建图SLMA算法的单片机来执行，其中，所述单片机通信连接有绑定在枪体上的图像传感器，并可以但不限于采用型号为STM32F103系列的单片机芯片或内置有安卓/Linux系统的处理器芯片等及它们的外围电路实现。所述枪体定位方法，可以但不限于包括有如下步骤S101～S104。

S101.获取在枪体移动过程中由图像传感器实时采集的图像数据，其中，所述图像传感器与所述枪体绑定，所述图像数据包含有在不同离散时刻采集的当前图像数据和若干份历史图像数据。

在所述步骤S101中，所述枪体可以但不限于为仿真枪的枪体，例如也可以为真实手枪及步枪等的枪体。所述图像传感器用于在不同离散时刻实时采集(例如每隔1/30秒采集一次)随所述枪体的移动而变化视野的图像数据，并通过有线或无线方式将采集的图像数据传送至所述单片机，以便使所述单片机能够获取到所述图像数据。具体的，所述图像传感器为单目摄像头或双目摄像头。由于所述双目摄像头具有测距功能(即可以利用双目测距原理得到环境物体至相机聚焦中心的距离，得到反映环境物体至相机聚焦中心的当前远近程度)，方便后续在视觉同步定位与建图SLMA算法中进行空间建模，因此所述图像传感器优选为双目摄像头；并为了方便在黑暗环境中也能产生很好的枪体定位效果，所述图像传感器还优选包括有红外线热成像摄像头，使得所述图像数据可包含有红外线热成像数据。此外，所述图像传感器与所述枪体的具体绑定方式可以但不限于为粘贴、卡扣或外挂等安装方式；所述图像传感器在所述枪体上的绑定位置及绑定姿态不限。

S102.根据所述当前图像数据和所述若干份历史图像数据，使用视觉同步定位与建图SLMA算法获取到所述图像传感器的相机光心在三维世界坐标系中的当前位置坐标和所述图像传感器的相机镜头在所述三维世界坐标系中的当前朝向角度。

在所述步骤S102中，所述视觉同步定位与建图SLMA算法也简称为视觉SLMA算法。SLAM是“SimultaneousLocalizationAndMapping”的缩写，可译为同步定位与建图；概率SLAM问题起源于1986年的IEEERoboticsandAutomationConference大会上，研究人员希望能将估计理论方法应用在构图和定位问题中；SLAM最早被应用在机器人领域，其目标是在没有任何先验知识的情况下，根据传感器数据实时构建周围环境地图，同时根据这个地图推测自身的定位。假设机器人携带传感器(相机)在未知环境中运动，为方便起见，可把一段连续时间的运动变成离散时刻t＝1、…、k，而在这些时刻，用x表示机器人的自身位置，则各时刻的位置就记为x1、x2、…、xk，它构成了机器人的轨迹；在地图方面，假设地图由许多个路标组成，而每个时刻，传感器会测量到一部分路标点，得到它们的观测数据；设路标点共有N个，用y1、y2、…、yn表示；由此通过运动测量u和传感器读数z来求解定位问题(估计x)和建图问题(估计y)。

目前只利用相机作为外部感知传感器的SLAM称为视觉SLAM(也用vSLAM表示)，相机具有视觉信息丰富、硬件成本低等优点，经典的vSLAM系统一般包含有前端视觉里程计(即VisualOdometry，仅有视觉输入的姿态估计)、后端优化(即Optimization，后端接受不同时刻视觉里程计测量的相机位姿，以及闭环检测的信息，对它们进行优化，得到全局一致的轨迹和地图)、闭环检测(LoopClosing，指机器人在地图构建过程中,通过视觉等传感器信息检测是否发生了轨迹闭环,即判断自身是否进入历史同一地点)和构图(即Mapping，根据估计的轨迹，建立与任务要求对应的地图)等四个主要部分，由此可将所述当前图像数据和所述若干份历史图像数据作为观测数据，导入常规改动的现有视觉SLMA算法中，使得在携带所述枪体及所述图像传感器从未知环境的未知地点出发后，可通过在枪体移动过程中重复观测到的环境特征定位所述图像传感器自身的空间位置和镜头朝向姿态，即获取到所述图像传感器的相机光心(即相机聚焦中心)在三维世界坐标系(例如大地坐标系或NED坐标系等)中的当前位置坐标和所述图像传感器的相机镜头在所述三维世界坐标系中的当前朝向角度。此外，还可以进一步根据所述图像传感器自身的空间位置构建周围环境的增量式地图，从而达到实时定位和地图构建的目的。

S103.根据所述当前位置坐标和所述枪体在相机坐标系中的已知枪体位置坐标，通过几何变换得到所述枪体在所述三维世界坐标系中的当前枪体位置坐标，以及根据所述当前朝向角度和所述枪体在所述相机坐标系中的已知枪口朝向角度，通过几何变换得到所述枪体在所述三维世界坐标系中的当前枪口朝向角度，其中，所述相机坐标系为以所述图像传感器的相机光心为原点且以相机光轴为Z轴建立的三维直角坐标系。

在所述步骤S103中，所述已知枪体位置坐标和所述已知枪口朝向角度可基于所述图像传感器在所述枪体上的绑定位置和绑定姿态，提前通过常规测量方式来确定，因此可基于常规的几何换算变换得到所述枪体在所述三维世界坐标系中的当前枪体位置坐标和当前枪口朝向角度。

S104.将所述当前枪体位置坐标和所述当前枪口朝向角度无线发送至上位机。

在所述步骤S104中，所述上位机可以但不限于用于通过常规软件对枪体进行有效监控；所述无线发送的具体方式可以但不限于为蓝牙通信方式或WiFi通信方式等，以便满足上位机对枪体方位数据的获取需求。

由此基于上述步骤S101～S104所详细描述的定位技术方案，可提供一种基于视觉同步定位与建图SLMA算法实现枪体定位的新方法，即先根据由在枪体上绑定的图像传感器实时采集的图像数据和使用视觉同步定位与建图SLMA算法，获取到所述图像传感器在三维世界坐标系中的空间位置和镜头朝向姿态，然后通过几何变换得到绑定枪体在所述三维世界坐标系中的当前枪体位置坐标和当前枪口朝向角度，从而可直接自主式地对绑定枪体进行空间位置和枪口朝向的定位，使得不必再依赖于场地，也不需要在启动时进行复杂的校准，并且理论上不受同时定位枪体的数量限制，可以支持在限定1立方米范围内对10把以上枪体进行同时定位，便于实际应用和推广。

本实施例在前述第一方面的技术方案基础上，还具体提出了一种提升实时定位结果稳定性的可能设计一，即将所述当前枪体位置坐标和所述当前枪口朝向角度无线发送至上位机，包括但不限于有如下步骤S401～S404。

S401.获取在所述枪体移动过程中由微机械陀螺仪实时采集的且在所述三维世界坐标系中的姿态数据，其中，所述微机械陀螺仪与所述枪体绑定，所述姿态数据包含有在不同离散时刻采集的当前姿态数据/和若干份历史姿态数据。

在所述步骤S401中，所述微机械陀螺仪用于在不同离散时刻实时采集(例如每隔1/30秒采集一次)随所述枪体的移动而变化姿态的姿态角数据(例如方位角、俯仰角和翻滚角等)，并通过有线或无线方式将采集的姿态数据传送至所述单片机，以便使所述单片机能够获取到所述姿态数据。此外，所述微机械陀螺仪与所述枪体的具体绑定方式可以但不限于为粘贴、卡扣或外挂等安装方式；所述微机械陀螺仪在所述枪体上的绑定位置及绑定姿态不限。

S402.根据所述姿态数据、所述已知枪体位置坐标和所述微机械陀螺仪在所述相机坐标系中的已知陀螺仪位置坐标，通过几何变换得到所述枪体在所述三维世界坐标系中的姿态数据。

在所述步骤S402中，所述已知陀螺仪位置坐标可基于所述微机械陀螺仪和所述图像传感器分别在所述枪体上的绑定位置，提前通过常规测量方式来确定，因此可基于常规的几何换算变换得到所述枪体在所述三维世界坐标系中的姿态数据。

S403.根据所述枪体在所述三维世界坐标系中的姿态数据，对所述当前枪口朝向角度进行矫正，得到新的当前枪口朝向角度。

在所述步骤S403中，优选的，根据所述枪体在所述三维世界坐标系中的姿态数据，对所述当前枪口朝向角度进行矫正，得到新的当前枪口朝向角度,包括但不限于有如下步骤S4031～S4032。

S4031.根据所述枪体在所述三维世界坐标系中的姿态数据，得到所述枪体在所述三维世界坐标系中的枪口朝向角度观测值。

在所述步骤S4031中，由于所述姿态数据包括有方位角、俯仰角和翻滚角等，因此可以直接通过常规几何换算变换得到所述枪口朝向角度观测值。

S4032.通过卡尔曼滤波算法对所述枪口朝向角度观测值和所述当前枪口朝向角度进行加权平均处理，得到所述新的当前枪口朝向角度。

在所述步骤S4032中，所述卡尔曼滤波算法(Kalmanfiltering)是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，因此可以通过对现有卡尔曼滤波算法进行常规改动得到的具体卡尔曼滤波算法，来对所述枪口朝向角度观测值和所述当前枪口朝向角度进行加权平均处理(此处的加权平均处理为常规的测量矫正方式)，得到所述新的当前枪口朝向角度，实现枪口朝向角度矫正目的。

S404.将所述当前枪体位置坐标和所述新的当前枪口朝向角度无线发送至所述上位机。

由此基于上述步骤S401～S404所描述的可能设计一，可以通过结合由微机械陀螺仪所采集的姿态数据，来对所述当前枪口朝向角度进行矫正，进一步提升实时定位结果的稳定性。

如图2所示，本实施例第二方面提供了一种实现第一方面或第一方面中任意一种可能设计所述枪体定位方法的虚拟装置，包括有依次通信连接的图像数据获取单元、位置朝向确定单元、位置朝向换算单元和定位结果传送单元；

所述图像数据获取单元，用于获取在枪体移动过程中由图像传感器实时采集的图像数据，其中，所述图像传感器与所述枪体绑定，所述图像数据包含有在不同离散时刻采集的当前图像数据和若干份历史图像数据；

所述位置朝向确定单元，用于根据所述当前图像数据和所述若干份历史图像数据，使用视觉同步定位与建图SLMA算法获取到所述图像传感器的相机光心在三维世界坐标系中的当前位置坐标和所述图像传感器的相机镜头在所述三维世界坐标系中的当前朝向角度；

所述位置朝向换算单元，用于根据所述当前位置坐标和所述枪体在相机坐标系中的已知枪体位置坐标，通过几何变换得到所述枪体在所述三维世界坐标系中的当前枪体位置坐标，以及根据所述当前朝向角度和所述枪体在所述相机坐标系中的已知枪口朝向角度，通过几何变换得到所述枪体在所述三维世界坐标系中的当前枪口朝向角度，其中，所述相机坐标系为以所述图像传感器的相机光心为原点且以相机光轴为Z轴建立的三维直角坐标系；

所述定位结果传送单元，用于将所述当前枪体位置坐标和所述当前枪口朝向角度无线发送至上位机。

在一个可能的设计中，所述定位结果传送单元包括有依次通信连接的姿态数据获取子单元、姿态数据变换子单元、枪口朝向矫正子单元和无线发送子单元；

所述姿态数据获取子单元，用于获取在所述枪体移动过程中由微机械陀螺仪实时采集的且在所述三维世界坐标系中的姿态数据，其中，所述微机械陀螺仪与所述枪体绑定，所述姿态数据包含有在不同离散时刻采集的当前姿态数据/和若干份历史姿态数据；

所述姿态数据变换子单元，用于根据所述姿态数据、所述已知枪体位置坐标和所述微机械陀螺仪在所述相机坐标系中的已知陀螺仪位置坐标，通过几何变换得到所述枪体在所述三维世界坐标系中的姿态数据；

所述枪口朝向矫正子单元，用于根据所述枪体在所述三维世界坐标系中的姿态数据，对所述当前枪口朝向角度进行矫正，得到新的当前枪口朝向角度；

所述无线发送子单元，用于将所述当前枪体位置坐标和所述新的当前枪口朝向角度无线发送至所述上位机。

在一个可能的设计中，所述枪口朝向矫正子单元包括有通信相连的观测值获取孙单元和加权平均处理孙单元；

所述观测值获取孙单元，用于根据所述枪体在所述三维世界坐标系中的姿态数据，得到所述枪体在所述三维世界坐标系中的枪口朝向角度观测值；

所述加权平均处理孙单元，用于通过卡尔曼滤波算法对所述枪口朝向角度观测值和所述当前枪口朝向角度进行加权平均处理，得到所述新的当前枪口朝向角度。

本实施例第二方面提供的前述装置的工作过程、工作细节和技术效果，可以参见第一方面或第一方面中任意一种可能设计所述的枪体定位方法，于此不再赘述。

如图3所示，本实施例第三方面提供了一种执行第一方面或第一方面中任意一种可能设计所述枪体定位方法的枪体定位设备，包括有用于将所述枪体定位设备绑定在枪体上的安装结构，还包括有图像传感器、存储器、处理器和收发器；所述图像传感器通信连接所述处理器，用于在所述枪体移动过程中实时采集图像数据；所述存储器通信连接所述处理器，用于存储计算机程序；所述收发器通信连接所述处理器，用于收发数据；所述处理器用于读取所述计算机程序，执行如第一方面或第一方面中任意一种可能设计所述的枪体定位方法。

具体举例的，所述存储器可以但不限于包括随机存取存储器(Random-AccessMemory，RAM)、只读存储器(Read-OnlyMemory，ROM)、闪存(FlashMemory)、先进先出存储器(FirstInputFirstOutput，FIFO)和/或先进后出存储器(FirstInputLastOutput，FILO)等等；所述收发器可以但不限于为WiFi(无线保真)无线收发器、蓝牙无线收发器、GPRS(GeneralPacketRadioService，通用分组无线服务技术)无线收发器和/或ZigBee(紫蜂协议，基于IEEE802.15.4标准的低功耗局域网协议)无线收发器等；所述处理器可以不限于采用型号为STM32F103系列的单片机芯片或内置有安卓/Linux系统的处理器芯片等。此外，所述枪体定位设备还可以但不限于包括有电源模块、微机械陀螺仪和其它必要的部件。

本实施例第三方面提供的前述枪体定位设备的工作过程、工作细节和技术效果，可以参见第一方面或第一方面中任意一种可能设计所述的枪体定位方法，于此不再赘述。

本实施例第四方面提供了一种采用第三方面所述枪体定位设备的仿真枪，包括有仿真枪本体和如第三方面所述的枪体定位设备，其中，所述枪体定位设备通过外挂方式的安装结构与所述仿真枪本体绑定。

在一个可能的设计中，所述仿真枪还包括有通过无线模块通信连接上位机的开枪击发检测传感器和/或弹夹更换检测传感器，其中，所述无线模块可以但不限于包括有蓝牙模块和/或无线保真WiFi模块。此外，所述开枪击发检测传感器用于检测所述仿真枪本体是否发生开枪击发动作，具体可采用现有通用的传感器实现，例如安装在开枪扣动部上的位移传感器；所述弹夹更换检测传感器用于检测所述仿真枪本体是否发生弹夹更换动作，具体可采用现有通用的传感器实现，例如安装在弹夹盒中的压力传感器。

本实施例第四方面提供的前述仿真枪的工作过程、工作细节和技术效果，可以参见第一方面或第一方面中任意一种可能设计所述的枪体定位方法，于此不再赘述。

本实施例第五方面提供了一种存储包含第一方面或第一方面中任意一种可能设计所述枪体定位方法的指令的可读存储介质，即所述可读存储介质上存储有指令，当所述指令在计算机上运行时，执行如第一方面或第一方面中任意一种可能设计所述的枪体定位方法。其中，所述可读存储介质是指存储数据的载体，可以但不限于包括软盘、光盘、硬盘、闪存、优盘和/或记忆棒(MemoryStick)等，所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。

本实施例第五方面提供的前述可读存储介质的工作过程、工作细节和技术效果，可以参见第一方面或第一方面中任意一种可能设计所述的枪体定位方法，于此不再赘述。

本实施例第六方面提供了一种包含指令的计算机程序产品，当所述指令在计算机上运行时，使所述计算机执行如第一方面或第一方面中任意一种可能设计所述的枪体定位方法。其中，所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。

以上所描述的实施例仅仅是示意性的，若涉及到作为分离部件说明的单元，其可以是或者也可以不是物理上分开的；若涉及到作为单元显示的部件，其可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。

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

最后应说明的是，本发明不局限于上述可选的实施方式，任何人在本发明的启示下都可得出其他各种形式的产品。上述具体实施方式不应理解成对本发明的保护范围的限制，本发明的保护范围应当以权利要求书中界定的为准，并且说明书可以用于解释权利要求书。

Claims (10)

1.一种枪体定位方法，其特征在于，包括：

获取在枪体移动过程中由图像传感器实时采集的图像数据，其中，所述图像传感器与所述枪体绑定，所述图像数据包含有在不同离散时刻采集的当前图像数据和若干份历史图像数据；

根据所述当前图像数据和所述若干份历史图像数据，使用视觉同步定位与建图SLMA算法获取到所述图像传感器的相机光心在三维世界坐标系中的当前位置坐标和所述图像传感器的相机镜头在所述三维世界坐标系中的当前朝向角度；

根据所述当前位置坐标和所述枪体在相机坐标系中的已知枪体位置坐标，通过几何变换得到所述枪体在所述三维世界坐标系中的当前枪体位置坐标，以及根据所述当前朝向角度和所述枪体在所述相机坐标系中的已知枪口朝向角度，通过几何变换得到所述枪体在所述三维世界坐标系中的当前枪口朝向角度，其中，所述相机坐标系为以所述图像传感器的相机光心为原点且以相机光轴为Z轴建立的三维直角坐标系；

将所述当前枪体位置坐标和所述当前枪口朝向角度无线发送至上位机。

2.如权利要求1所述的枪体定位方法，其特征在于，将所述当前枪体位置坐标和所述当前枪口朝向角度无线发送至上位机，包括：

获取在所述枪体移动过程中由微机械陀螺仪实时采集的且在所述三维世界坐标系中的姿态数据，其中，所述微机械陀螺仪与所述枪体绑定，所述姿态数据包含有在不同离散时刻采集的当前姿态数据和若干份历史姿态数据；

根据所述姿态数据、所述已知枪体位置坐标和所述微机械陀螺仪在所述相机坐标系中的已知陀螺仪位置坐标，通过几何变换得到所述枪体在所述三维世界坐标系中的姿态数据；

根据所述枪体在所述三维世界坐标系中的姿态数据，对所述当前枪口朝向角度进行矫正，得到新的当前枪口朝向角度；

将所述当前枪体位置坐标和所述新的当前枪口朝向角度无线发送至所述上位机。

3.如权利要求2所述的枪体定位方法，其特征在于，根据所述枪体在所述三维世界坐标系中的姿态数据，对所述当前枪口朝向角度进行矫正，得到新的当前枪口朝向角度,包括：

根据所述枪体在所述三维世界坐标系中的姿态数据，得到所述枪体在所述三维世界坐标系中的枪口朝向角度观测值；

通过卡尔曼滤波算法对所述枪口朝向角度观测值和所述当前枪口朝向角度进行加权平均处理，得到所述新的当前枪口朝向角度。

4.如权利要求1所述的枪体定位方法，其特征在于，所述图像传感器为单目摄像头或双目摄像头。

5.如权利要求1所述的枪体定位方法，其特征在于，所述图像传感器包括有红外线热成像摄像头。

6.一种枪体定位装置，其特征在于，包括有依次通信连接的图像数据获取单元、位置朝向确定单元、位置朝向换算单元和定位结果传送单元；

所述图像数据获取单元，用于获取在枪体移动过程中由图像传感器实时采集的图像数据，其中，所述图像传感器与所述枪体绑定，所述图像数据包含有在不同离散时刻采集的当前图像数据和若干份历史图像数据；

所述位置朝向确定单元，用于根据所述当前图像数据和所述若干份历史图像数据，使用视觉同步定位与建图SLMA算法获取到所述图像传感器的相机光心在三维世界坐标系中的当前位置坐标和所述图像传感器的相机镜头在所述三维世界坐标系中的当前朝向角度；

所述位置朝向换算单元，用于根据所述当前位置坐标和所述枪体在相机坐标系中的已知枪体位置坐标，通过几何变换得到所述枪体在所述三维世界坐标系中的当前枪体位置坐标，以及根据所述当前朝向角度和所述枪体在所述相机坐标系中的已知枪口朝向角度，通过几何变换得到所述枪体在所述三维世界坐标系中的当前枪口朝向角度，其中，所述相机坐标系为以所述图像传感器的相机光心为原点且以相机光轴为Z轴建立的三维直角坐标系；

所述定位结果传送单元，用于将所述当前枪体位置坐标和所述当前枪口朝向角度无线发送至上位机。

7.一种枪体定位设备，其特征在于,包括有用于将所述枪体定位设备绑定在枪体上的安装结构，还包括有图像传感器、存储器、处理器和收发器；

所述图像传感器通信连接所述处理器，用于在所述枪体移动过程中实时采集图像数据；

所述存储器通信连接所述处理器，用于存储计算机程序；

所述收发器通信连接所述处理器，用于收发数据；

所述处理器用于读取所述计算机程序，执行如权利要求1～5中任意一项所述的枪体定位方法。

8.一种仿真枪，其特征在于，包括有仿真枪本体和如权利要求7所述的枪体定位设备，其中，所述枪体定位设备通过外挂方式的安装结构与所述仿真枪本体绑定。

9.如权利要求8所述的仿真枪，其特征在于，还包括有通过无线模块通信连接上位机的开枪击发检测传感器和/或弹夹更换检测传感器，其中，所述无线模块包括有蓝牙模块和/或无线保真WiFi模块。

10.一种可读存储介质，其特征在于,所述可读存储介质上存储有指令，当所述指令在计算机上运行时，执行如权利要求1～5中任意一项所述的枪体定位方法。

Images