CN110230983B - 抗振式光学三维定位方法及装置 - Google Patents

Abstract

本发明涉及一种抗振式光学三维定位方法，包括：多个光学定位相机分别检测并输出各自的当前姿态以及前方标志点在采集图像中的二维图像坐标；上位机接收来自每一个光学定位相机的当前姿态和二维图像坐标；上位机基于各个光学定位相机的各个当前姿态、二维图像坐标和内外参数信息计算标志点的修正三维定位信息；上位机基于修正三维定位信息确定标志点运动轨迹和标志点所在刚体结构的实时空间位姿信息。多个光学定位相机还可以被集成在同一光学成像设备中且各自除了光学镜头、图像传感器以外的相同元件分别被合并为一个元件。本发明还涉及一种抗振式光学三维定位装置。通过本发明，能够对标志点以及标志点所在刚体结构执行有效三维定位。

Description

抗振式光学三维定位方法及装置

技术领域

本发明涉及三维定位领域，尤其涉及一种抗振式光学三维定位方法及装置。

背景技术

虚拟现实为实现人与虚拟环境及系统的交互，必须确定参与者的头部、手、身体等的位置与方向，准确地跟踪测量参与者的动作，将这些动作实时检测出来，以便将这些数据反馈给显示和控制系统，这些动作的跟踪测量工作对虚拟现实系统是必不可少的核心内容。

动作捕捉是在运动物体的关键部位设置标志点跟踪器，由光学定位相机捕捉标志点位置，再经过计算机处理后得到三维空间坐标的数据。动作捕捉系统定标后，光学定位相机连续拍运动物体的动作，并实时识别其中的标志点，输出二维坐标到计算机。计算机根据至少两个图像传感器同时拍摄得到的坐标点信息进行分析和处理，计算出每一瞬间各个标识点的空间位置，进而得到其运动轨迹。为了得到准确的运动轨迹，光学定位相机应有较高的拍摄速率，一般要达到每秒120帧以上。

当前，光学定位相机都是通过支架安装在活动区域四周的上方，光学定位相机朝侧下方对着活动区域固定安装。当外界因素造成支架轻微振动时，光学定位相机会随之振动，使光学定位相机得到的图像出现抖动，导致检测到的标识点位置整体偏移，影响动作捕捉的位置定位精度。因此安装的时候对固定方式要求很高，要求不能受到外界振动干扰，并且需要周期性的校准，加大了维护成本。

发明内容

为了解决上述问题，本发明提供了一种抗振式光学三维定位方法及装置，采用一个以上的光学定位相机对标志点进行图像采集，在光学定位相机内部利用姿态传感器实时获取每一帧图像对应的姿态信息，根据当前帧的姿态相对于原始标定状态时的姿态偏移量，结合标志点与光学定位相机之间的距离，计算得到修正后光学定位相机无振动偏移时的标识点位置信息。使用修正后的标志点位置信息计算得到标识点的三维空间定位信息，进而确定标志点的运动轨迹和标志点所附着的刚体结构的实时空间位置和姿态信息，从而获取准确的三维定位数据。

为此，本发明需要至少具备以下二处重要的发明点：

(1)建立具体的标志点三维坐标信息的实时修正机制，用于准确确定标志点的运动轨迹和标志点所附着的刚体结构的实时空间位置和姿态信息，从而有效解决了光学定位相机因振动导致的定位不准的问题；

(2)基于多相机空间定位方法得到各个标志点到各个相机之间的距离，然后各个相机再基于这些初始的距离并结合各自的姿态偏移值，修正计算对应的二维图像坐标，根据这些修正后的二维坐标再重新计算各个标志点在三维空间中的坐标值，以获得各个标志点的修正后的三维空间坐标值。

根据本发明的一方面，提供了一种抗振式光学三维定位方法，所述方法包括：

多个光学定位相机分别检测并输出各自的当前姿态以及前方标志点在采集图像中的二维图像坐标；

上位机接收来自每一个光学定位相机的当前姿态和二维图像坐标；

上位机基于各个二维图像坐标和各个光学定位相机的各个内外参数信息，计算标志点的三维定位信息以作为原始三维定位信息输出；

所述上位机基于接收到的原始三维定位信息计算获得标志点到多个光学定位相机的实际距离；

所述上位机基于实际距离、光学定位相机输出的采集图像和光学定位相机的当前姿态计算获得所述光学定位相机的修正后的标志点的二维图像坐标以作为修正二维坐标输出；

所述上位机基于各个修正二维坐标和各个光学定位相机的各个内外参数信息，计算标志点的修正后的三维定位信息以作为实际三维定位信息输出；

所述上位机基于实际三维定位信息确定对应的标志点的运动轨迹和标志点所附着的刚体结构的实时空间位姿信息；

其中，所述多个光学定位相机两两相隔预设间隔以从不同角度对前方的标志点进行同步图像采集。

根据本发明的另一方面，还提供了一种抗振式光学三维定位装置，所述装置包括：

多个光学定位相机，用于分别检测并输出各自的当前姿态以及前方标志点在采集图像中的二维图像坐标；

上位机，用于接收来自每一个光学定位相机的当前姿态和二维图像坐标；

其中，上位机基于各个二维图像坐标和各个光学定位相机的各个内外参数信息，计算标志点的三维定位信息以作为原始三维定位信息输出；

其中，所述上位机基于接收到的原始三维定位信息计算获得标志点到多个光学定位相机的实际距离；

其中，所述上位机基于实际距离、光学定位相机输出的采集图像和光学定位相机的当前姿态计算获得所述光学定位相机的修正后的标志点的二维图像坐标以作为修正二维坐标输出；

其中，所述上位机基于各个修正二维坐标和各个光学定位相机的各个内外参数信息，计算标志点的修正后的三维定位信息以作为实际三维定位信息输出；

其中，所述上位机基于实际三维定位信息确定对应的标志点的运动轨迹和标志点所附着的刚体结构的实时空间位姿信息；

其中，所述多个光学定位相机两两相隔预设间隔以从不同角度对前方的标志点进行同步图像采集。

附图说明

以下将结合附图对本发明的实施方案进行描述，其中：

图1为根据本发明实施方案示出的一种抗振动的光学定位方法流程图。

图2为根据本发明实施方案示出的不带补光的光学定位相机的结构框图。

图3为根据本发明实施方案示出的抗振动的光学定位装置的结构框图。

图4为根据本发明实施方案示出的带补光的光学定位相机的结构框图。

图5为根据本发明实施方案示出的多个光学定位相机对标志点同步采集的场景示意图。

图6为根据本发明实施方案示出的单个光学定位相机含多个图像传感器对标志点同步采集的场景示意图。

图7为根据本发明实施方案示出的根据标志点的原始三维定位信息得到标志点到图像传感器的距离的原理示意图。

图8为根据本发明实施方案示出的得到修正后的标志点二维图像坐标的原理示意图。

具体实施方式

下面将参照附图对本发明的抗振式光学三维定位方法及装置的实施方案进行详细说明。

当前，由于成像相机的抖动无法得到高精度的校正，当外界因素导致光学定位相机振动时，将使光学定位相机的姿态信息发生变化，从而在实际的三维动作捕捉操作中带来二维定位不准乃至三维定位不准的技术问题。

为了克服上述不足，本发明搭建了一种抗振式光学三维定位方法及装置，能够有效解决相应的技术问题。

本发明提供了一种抗振的光学定位方法与装置，所述装置包括：有姿态传感器的光学定位相机、通讯模块、上位机。有姿态传感器的光学定位相机包含：姿态传感器、光学镜头、图像传感器、处理器、通讯接口，光学定位相机在包含上述组件的情况下，也可以包含：补光单元。通讯模块连接光学定位相机和上位机，用于两者间的数据传输。上位机包括通讯接口和处理器，通讯接口用于接收光学定位相机的姿态和标志点二维坐标信息，处理器用于计算得到修正的标志点三维坐标信息，确定标志点的运动轨迹和标志点所附着的刚体结构的实时空间位置和姿态信息。利用上述装置，提供一种抗振的光学定位方法：在光学定位相机内部利用姿态传感器实时获取每一帧图像对应的姿态信息，根据当前帧的姿态相对于原始标定状态时的姿态偏移量，结合标志点与光学定位相机之间的距离，计算得到修正后光学定位相机无振动偏移时的标识点位置信息。使用修正后的标志点位置信息计算得到标识点的三维空间定位信息，从而解决了光学定位相机因振动导致的定位不准的问题。

本发明包含四个实施例

实施例一

如图1所示，本发明提供的抗振动不带补光灯的光学相机的光学定位方法，在使用主动发光的标志点时，使用这种方法。包括以下步骤：

S101：动捕相机采集姿态和标志点图像，并给出所述动捕相机的姿态和标志点二维图像坐标；

其中，姿态信息可以直接从姿态传感器中读取获得，包括角速度、加速度和磁数据等，输出的数据格式可以是欧拉角、四元数或旋转矩阵等。标志点的形状在图像中可以是圆形，可以将反射红外光的小球或者发射红外光的小球作为标志点，又如，可以将反射红外光的圆形标签贴在平面物体上作为标志点。可以将作为标志点的小球或标签与输入设备进行固定，从而该标志点的运动轨迹可以体现输入设备的运动轨迹。在图像中提取标志点的二维坐标时，先将图像做二值化处理，再通过圆心提取算法得到标志点的二维坐标值，可以通过最小二乘拟合的方法提取圆心坐标；

S102：动捕相机向上位机发送姿态和标志点二维图像坐标；

其中，数据可以通过网络接口、USB接口或者无线传输接口从动捕相机传输到上位机；

S103：上位机接收动捕相机发送的姿态和标志点二维图像坐标；

S104：上位机根据标志点二维图像坐标，并结合已经标定好的相机内外参数信息，计算确定标志点的原始三维定位信息；

其中，上位机根据多个传感器对应的标志点的坐标数据，结合图像传感器的全部内外参数，计算确定所述定位点的三维坐标。根据多个标志点的二维坐标确定标志点的三维坐标时，需要执行图像传感器内外参数标定、坐标匹配和三维坐标重建的具体过程。在确定图像传感器的内外参数信息时，利用已知精确几何尺寸的标定杆在传感器的视野区域内多次挥舞成像，从对标定杆上标志点的图像中提取二维图像坐标，利用常用的传感器内外参数标定方法，例如张正友标定法，即可获得所述图像传感器的全部内外参数信息。在由二维图像坐标中获得可标志点的二维坐标匹配关系信息时，可以利用计算机视觉中多视图几何成像的常用方法，例如极线约束方法，获得全部二维图像中对应于同一定位点的二维图像坐标的匹配关系。在标志点的三维坐标重建时，可以利用结合图像传感器模型的三维重建方法，使用同一定位点的多个二维图像坐标，进行计算获得该标志点的三维空间坐标；

S105：上位机根据标志点的原始三维定位信息得到标志点到动捕相机的距离，并且根据当前帧对应的姿态偏移信息，得到修正后标志点的二维图像坐标；

在本实施例中，使用两个相机C₁，C₂来确定空间点的三维坐标，具体包括以下步骤：

步骤401：双目平行校正，通过分别对两张图片用单应矩阵变换使得两个不同方向的图像平面重新投影到同一个平面且光轴互相平行，将两个相机的极线变成水平。然后执行步骤402；

因为有些场景下两个相机需要独立固定，很难保证相机的中心点O_l，O_r完全水平，即使是固定在同一个基板上也会因为装配的原因导致中心点不完全水平。因此，在本发明中，使用双目平行校正方法进行校正。将源图像像素坐标系通过内参矩阵转化成相机坐标系，通过旋转矩阵R₁，R₂进行平行极线校正，然后通过畸变系数校正图像的相机坐标，校正后通过内参矩阵将相机坐标系转换成图像像素坐标系，并根据源图像坐标的像素值赋值给新的图像坐标；

步骤402：计算左右成像点的视差d＝x_l-x_r。然后执行步骤403，具体原理如图7所示；

其中，在进行双目平行校正之后，相机坐标系的X轴与基线平行，Z轴与像平面垂直，因此视差d＝x_l-x_r；

步骤403：根据三角形相似原理，计算基线B与深度Z的比值，得到

然后执行步骤404；

其中，在本发明中，已知x₁+x₂＝b₁，x₁₁+x₂₂＝B，左右图像的宽度均为L，由相似三角形原理即ΔPO_lO_r～ΔPpp'得到：

将两式相加有

又因为

所以

步骤404：计算空间点P到所述基线之间的深度，得到

然后执行步骤405；

由步骤403得到的结果

进一步推导得到BZ-Bf＝BZ-Z(x_l-x_r)，因此Z(x_l-x_r)＝Bf，从而得到

步骤405：步骤404得到的空间点P到所述基线之间的深度Z即为空间点到相机的距离，即物距

其中，再利用物距像距焦距之间的关系、上位机倾斜所产生的夹角以及三角形的边角关系得到标志点修正后的二维图像坐标，具体原理如图8所示；

在本实施例中，为了得到校正之后真实的图像坐标点，具体包括以下步骤：

步骤501：根据步骤405得到的物距u以及物距像距焦距之间的关系，得到像距

然后执行步骤502；

其中，在本实施例中，对于物距u，焦距f，像距v，三者满足下列关系：

当u≥f时，可以认为v＝f，这也就是简单的针孔成像情况，这里的物距u，相机的焦距f均为已知，因此得到

步骤502：计算物体在倾斜透镜下所成的像的顶点与水平虚线的夹角α，得到

然后执行步骤503；

根据步骤501得到的像距

以及几何关系，得到像焦距

再根据三角形的边角关系，有

从而得到

步骤503：计算物体在垂直透镜下所成的像的顶点与水平虚线的夹角γ的正切值，得到

然后执行步骤504；

根据三角形内角之间的关系，得到物体在垂直透镜下所成的像的顶点与水平虚线的夹角γ＝α-θ，再根据三角形的边角关系，有

步骤504：计算物体在垂直透镜下的真实像高X，得到

然后执行步骤505；

根据步骤503得到的物体在垂直透镜下所成的像的顶点与水平虚线的夹角γ的正切值以及三角形的边角关系，有物体在垂直透镜下的真实像高

步骤505：计算经过校正之后真实像高的顶点坐标Q’，得到

在本发明中，由于成像点位于第四象限内，所以纵坐标为负值；

S106：上位机根据修正后的标志点二维图像坐标，并结合已经标定好的相机内外参数信息，计算标志点修正后的三维定位信息；操作方式跟S104相近；

S107：上位机确定修正后的三维定位信息对应的标志点的运动轨迹和所附着的刚体结构的实时空间位姿信息。

如果超过两个相机时，则第3个相机和它邻近的第2个相机再按上述步骤两两计算，第4个和第3个相机，依次类推，实现多个相机对标志点进行空间定位的功能。

本发明可以多个光学定位相机对标志点同步采集，所有光学定位相机同步工作，每个光学定位相机内部只有一个图像传感器和一个姿态测量传感器。如图5所示，使用两个光学定位相机502和503分别从不同角度对标志点501进行同步采集。

使用光学定位相机502和503同时对标志点501进行图像拍摄，分别标定左右相机的内部参数，这个过程保证标定点遍历大部分图像，以获得较为准确的内参和外参；内参确定之后，通过配对的图像确定两个相机坐标系之间的转换关系，结合光学定位相机的所述内外参数，计算确定标志点的三维空间坐标。

如图2所示，不带补光的光学定位相机包括红外光学单元、图像传感器、姿态测量单元、主控处理单元和数据通讯单元。

其中，如图3所示，光学定位相机301是通过通讯接口302完成和上位机303之间的信息交互，通讯接口302用于两者间的数据传输，通讯接口的数据通讯可以是通过网口通讯、USB口通讯或者无线通讯。采用网口通讯时，可以包含交换机。

实施例二

本发明提供一种抗振带补光的光学相机的光学定位方法，在使用被动反光的标志点时，使用这种方法。

如图4所示，带补光的光学定位相机包括：红外光学单元、图像传感器、姿态测量单元、主控处理单元、数据通讯单元、补光灯和补光控制模块。其中的补光单元包括：补光灯和补光灯控制模块。补光灯控制模块接收处理器输出的开关信号，驱动补光灯的亮灭，在图像传感器进行曝光采集图像时驱动点亮补光灯。

在图像传感器进行曝光采集图像时，处理器向补光驱动电路发送驱动脉冲信号，补光电路接收到驱动脉冲信号后导通补光灯控制开关，点亮补光灯。补光灯点亮发光，被动反光球接收到补光灯发射的光线，并将光线反射到红外光学单元，红外光学单元将红外图像聚焦到图像传感器，图像传感器再将图像完成光电转换，形成数字图像信号。这样标志点可以采用一些反光特性好的材料，而不需要具有主动发光的装置在上面，可以减小标志点的成本和复杂度。

除了补光功能之外，其余的步骤与实施例1完全相同，这里不再赘述。

实施例三

本发明可以多个光学定位相机对标志点同步采集，所有光学定位相机同步工作，每个光学定位相机内部只有一个图像传感器和一个姿态测量传感器。如图5所示，使用多个光学定位相机502和503分别从不同角度对标志点501进行同步采集。

使用光学定位相机502和503同时对标志点501进行图像拍摄，分别标定左右相机的内部参数，这个过程保证标定点遍历大部分图像，以获得较为准确的内参和外参；内参确定之后，通过配对的图像确定两个相机坐标系之间的转换关系，结合光学定位相机的所述内外参数，计算确定标志点的三维空间坐标。

实施例四

本发明也可以使用一个光学定位相机，这个光学定位相机带一个姿态测量传感器和至少两个图像传感器的对标志点进行同步采集，所有图像传感器同步工作。如图6所示，光学定位相机604上带有光学图像传感器602和光学图像传感器603，利用这两个图像传感器从不同的角度对标志点601进行同步采集。

首先光学定位相机604上的光学镜头将含标志点的光线聚焦到图像传感器602和603，图像传感器分别将光电转换后的数字图像输出到处理器，处理器从数字图像中提取计算出标志点的二维坐标数据，处理器在图像传感器602和603进行图像曝光采集时读取姿态传感器的姿态信息，姿态信息和标志点的二维坐标信息通过通讯接口对外输出。

光学定位相机根据图像传感器602和603采集的定位图像，直接计算确定定位图像中标志点601的二维图像坐标，并且从姿态传感器读取定位图像对应时间点的姿态信息。

其它处理步骤基于实施例三，在此不在进行赘述。

另外，FPGA(Field－Programmable GateArray)，即现场可编程门阵列，它是在PAL、GAL、CPLD等可编程器件的基础上进一步发展的产物。它是作为专用集成电路(ASIC)领域中的一种半定制电路而出现的，既解决了定制电路的不足，又克服了原有可编程器件门电路数有限的缺点。

FPGA设计不是简单的芯片研究，主要是利用FPGA的模式进行其他行业产品的设计。与ASIC不同，FPGA在通信行业的应用比较广泛。通过对全球FPGA产品市场以及相关供应商的分析，结合当前我国的实际情况以及国内领先的FPGA产品可以发现相关技术在未来的发展方向，对我国科技水平的全面提高具有非常重要的推动作用。

FPGA的开发相对于传统PC、单片机的开发有很大不同。FPGA以并行运算为主，以硬件描述语言来实现；相比于PC或单片机(无论是冯诺依曼结构还是哈佛结构)的顺序操作有很大区别，也造成了FPGA开发入门较难。FPGA开发需要从顶层设计、模块分层、逻辑实现、软硬件调试等多方面着手。

与传统模式的芯片设计进行对比，FPGA芯片并非单纯局限于研究以及设计芯片，而是针对较多领域产品都能借助特定芯片模型予以优化设计。从芯片器件的角度讲，FPGA本身构成了半定制电路中的典型集成电路，其中含有数字管理模块、内嵌式单元、输出单元以及输入单元等。在此基础上，关于FPGA芯片有必要全面着眼于综合性的芯片优化设计，通过改进当前的芯片设计来增设全新的芯片功能，据此实现了芯片整体构造的简化与性能提升。

最后应注意到的是，在本发明各个实施例中的各功能设备可以集成在一个处理设备中，也可以是各个设备单独物理存在，也可以两个或两个以上设备集成在一个设备中。

所述功能如果以软件功能设备的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (10)

1.一种抗振式光学三维定位方法，其特征在于，所述方法包括：

多个光学定位相机分别检测并输出各自的当前姿态以及前方标志点在采集图像中的二维图像坐标；

上位机接收来自每一个光学定位相机的当前姿态和二维图像坐标；

上位机基于各个二维图像坐标和各个光学定位相机的各个内外参数信息，计算标志点的三维定位信息以作为原始三维定位信息输出；

所述上位机基于接收到的原始三维定位信息计算获得标志点到多个光学定位相机的实际距离；

所述上位机基于实际距离、光学定位相机输出的采集图像和光学定位相机的当前姿态计算获得所述光学定位相机的修正后的标志点的二维图像坐标以作为修正二维坐标输出；

所述上位机基于各个修正二维坐标和各个光学定位相机的各个内外参数信息，计算标志点的修正后的三维定位信息以作为实际三维定位信息输出；

所述上位机基于实际三维定位信息确定对应的标志点的运动轨迹和标志点所附着的刚体结构的实时空间位姿信息；

其中，所述多个光学定位相机两两相隔预设间隔以从不同角度对前方的标志点进行同步图像采集。

2.如权利要求1所述的抗振式光学三维定位方法，其特征在于：

每一个光学定位相机包括姿态传感器、光学镜头、图像传感器、第一处理器和第一通讯接口，所述姿态传感器用于检测并输出其所在光学定位相机的当前姿态，所述图像传感器用于输出标志点在其采集图像中的二维图像坐标，所述第一通讯接口分别与所述姿态传感器和所述图像传感器连接，用于接收并发送所述当前姿态和所述二维图像坐标；

上位机包括第二处理器和第二通讯接口，所述第二通讯接口和每一个光学定位相机的第一通讯接口之间建立连接，用于接收来自每一个光学定位相机的第一通讯接口的当前姿态和二维图像坐标；

其中，基于各个二维图像坐标和各个光学定位相机的各个内外参数信息，计算标志点的三维定位信息以作为原始三维定位信息输出包括：利用已知几何尺寸的具有标志点的标定杆在光学定位相机的图像传感器的视野区域内多次挥舞以获取图像传感器的多个采集图像，提取每一个采集图像中标定杆的标志点的二维图像坐标，基于图像传感器内外参数标定方法和每一个采集图像中标志点的二维图像坐标计算图像传感器的内外参数信息以作为对应光学定位相机的内外参数信息，所述图像传感器内外参数标定方法包括张正友标定法；

其中，基于各个二维图像坐标和各个光学定位相机的各个内外参数信息，计算标志点的三维定位信息以作为原始三维定位信息输出还包括：基于极线约束方法和各个二维图像坐标计算获得标志点的二维图像坐标匹配关系信息，基于结合图像传感器模型的三维重建方法、二维图像坐标匹配关系信息和各个二维图像坐标计算获得标志点的三维空间坐标以作为标志点的三维定位信息；

其中，所述当前姿态包括角速度、加速度和磁数据，所述当前姿态的数据格式包括欧拉角、四元数和旋转矩阵；

其中，所述光学镜头为红外光学单元，所述标志点的形状包括圆形，所述标志点为反射红外光或者发射红外光的球体或标签。

3.如权利要求2所述的抗振式光学三维定位方法，其特征在于：

所述多个光学定位相机被集成在同一光学成像设备中且各自的姿态传感器、第一处理器和第一通讯接口分别被合并为一个元件。

4.如权利要求3所述的抗振式光学三维定位方法，其特征在于：

每一个光学定位相机还包括补光仪和补光控制模块元，补光仪包括补光灯和补光灯控制模块，补光灯控制模块包括补光电路和补光灯控制开关；

其中，补光灯控制模块接收第一处理器输出的开关信号以驱动补光灯的亮灭，用于在对应的图像传感器进行曝光采集图像时驱动点亮补光灯，在对应的图像传感器停止进行曝光采集图像时驱动熄灭补光灯；

其中，在对应的图像传感器进行曝光采集图像时驱动点亮补光灯包括：在图像传感器进行曝光采集图像时，第一处理器向补光灯控制模块发送驱动脉冲信号，补光灯控制模块的补光电路接收到驱动脉冲信号后导通补光灯控制模块的补光灯控制开关以点亮补光灯，补光灯点亮发光，被动反光的标志点接收到补光灯发射的光线，将光线反射到红外光学单元，红外光学单元将反射到的光线聚焦到图像传感器，图像传感器再将图像完成光电转换，形成采集到的数字图像；

其中，替换地，所述标志点为使用被动反光的标志点。

5.如权利要求4所述的抗振式光学三维定位方法，其特征在于：

所述姿态传感器为三轴陀螺仪、三轴加速度计和三轴电子罗盘，所述第一处理器为FPGA芯片，所述第二处理器为X86 CPU，所述第二通讯接口和每一个光学定位相机的第一通讯接口之间通过以太网通信链路、USB通信链路或无线通信链路建立连接。

6.一种抗振式光学三维定位装置，其特征在于，所述装置包括：

多个光学定位相机，用于分别检测并输出各自的当前姿态以及前方标志点在采集图像中的二维图像坐标；

上位机，用于接收来自每一个光学定位相机的当前姿态和二维图像坐标；

其中，上位机基于各个二维图像坐标和各个光学定位相机的各个内外参数信息，计算标志点的三维定位信息以作为原始三维定位信息输出；

其中，所述上位机基于接收到的原始三维定位信息计算获得标志点到多个光学定位相机的实际距离；

其中，所述上位机基于实际距离、光学定位相机输出的采集图像和光学定位相机的当前姿态计算获得所述光学定位相机的修正后的标志点的二维图像坐标以作为修正二维坐标输出；

其中，所述上位机基于各个修正二维坐标和各个光学定位相机的各个内外参数信息，计算标志点的修正后的三维定位信息以作为实际三维定位信息输出；

其中，所述上位机基于实际三维定位信息确定对应的标志点的运动轨迹和标志点所附着的刚体结构的实时空间位姿信息；

其中，所述多个光学定位相机两两相隔预设间隔以从不同角度对前方的标志点进行同步图像采集。

7.如权利要求6所述的抗振式光学三维定位装置，其特征在于：

每一个光学定位相机包括姿态传感器、光学镜头、图像传感器、第一处理器和第一通讯接口，所述姿态传感器用于检测并输出其所在光学定位相机的当前姿态，所述图像传感器用于输出标志点在其采集图像中的二维图像坐标，所述第一通讯接口分别与所述姿态传感器和所述图像传感器连接，用于接收并发送所述当前姿态和所述二维图像坐标；

上位机包括第二处理器和第二通讯接口，所述第二通讯接口和每一个光学定位相机的第一通讯接口之间建立连接，用于接收来自每一个光学定位相机的第一通讯接口的当前姿态和二维图像坐标；

其中，基于各个二维图像坐标和各个光学定位相机的各个内外参数信息，计算标志点的三维定位信息以作为原始三维定位信息输出包括：利用已知几何尺寸的具有标志点的标定杆在光学定位相机的图像传感器的视野区域内多次挥舞以获取图像传感器的多个采集图像，提取每一个采集图像中标定杆的标志点的二维图像坐标，基于图像传感器内外参数标定方法和每一个采集图像中标志点的二维图像坐标计算图像传感器的内外参数信息以作为对应光学定位相机的内外参数信息，所述图像传感器内外参数标定方法包括张正友标定法；

其中，基于各个二维图像坐标和各个光学定位相机的各个内外参数信息，计算标志点的三维定位信息以作为原始三维定位信息输出还包括：基于极线约束方法和各个二维图像坐标计算获得标志点的二维图像坐标匹配关系信息，基于结合图像传感器模型的三维重建方法、二维图像坐标匹配关系信息和各个二维图像坐标计算获得标志点的三维空间坐标以作为标志点的三维定位信息；

其中，所述当前姿态包括角速度、加速度和磁数据，所述当前姿态的数据格式包括欧拉角、四元数和旋转矩阵；

其中，所述光学镜头为红外光学单元，所述标志点的形状包括圆形，所述标志点为反射红外光或者发射红外光的球体或标签。

8.如权利要求7所述的抗振式光学三维定位装置，其特征在于：

所述多个光学定位相机被集成在同一光学成像设备中且各自的姿态传感器、第一处理器和第一通讯接口分别被合并为一个元件。

9.如权利要求8所述的抗振式光学三维定位装置，其特征在于：

每一个光学定位相机还包括补光仪和补光控制模块元，补光仪包括补光灯和补光灯控制模块，补光灯控制模块包括补光电路和补光灯控制开关；

其中，补光灯控制模块接收第一处理器输出的开关信号以驱动补光灯的亮灭，用于在对应的图像传感器进行曝光采集图像时驱动点亮补光灯，在对应的图像传感器停止进行曝光采集图像时驱动熄灭补光灯；

其中，在对应的图像传感器进行曝光采集图像时驱动点亮补光灯包括：在图像传感器进行曝光采集图像时，第一处理器向补光灯控制模块发送驱动脉冲信号，补光灯控制模块的补光电路接收到驱动脉冲信号后导通补光灯控制模块的补光灯控制开关以点亮补光灯，补光灯点亮发光，被动反光的标志点接收到补光灯发射的光线，将光线反射到红外光学单元，红外光学单元将反射到的光线聚焦到图像传感器，图像传感器再将图像完成光电转换，形成采集到的数字图像；

其中，替换地，所述标志点为使用被动反光的标志点。

10.如权利要求9所述的抗振式光学三维定位装置，其特征在于：

所述姿态传感器为三轴陀螺仪、三轴加速度计和三轴电子罗盘，所述第一处理器为FPGA芯片，所述第二处理器为X86 CPU，所述第二通讯接口和每一个光学定位相机的第一通讯接口之间通过以太网通信链路、USB通信链路或无线通信链路建立连接。

Images