[发明专利]抗振式光学三维定位方法及装置

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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