CN111750821B - 一种位姿参数测量方法、装置、系统和存储介质 - Google Patents

Abstract

本发明实施例公开了一种位姿参数测量方法、装置、系统和存储介质。该方法包括：获取视觉传感器和被测目标的距离信息；根据距离信息确定视觉传感器的工作模式，发送工作模式信息给视觉传感器，其中，工作模式信息用于指示视觉传感器采集合作标识的面阵图像或者线阵图像；获取视觉传感器采集的面阵图像或线阵图像，根据面阵图像或线阵图像计算被测目标的位姿参数。本发明实施例通过切换视觉传感器的工作模式，计算不同工作模式下被测目标的位姿参数，实现被测目标与视觉传感器位姿参数由粗到精的高速、高精度测量。

Description

一种位姿参数测量方法、装置、系统和存储介质

技术领域

本发明实施例涉及基于视觉传感器的目标位姿参数测量技术，尤其涉及一种位姿参数测量方法、装置、系统和存储介质。

背景技术

视觉测量技术作为重要的三维数据获取手段，具有量程大、非接触、速度快、精度高等优点，正在被广泛应用于在线动态测量领域，尤其在空间近距视觉导航领域得到广泛应用。

其中，复杂环境下近距视觉导航中的目标位姿参数高精度测量一直是当前亟待解决的难题，尤其对于空间对接末端更为重要。常规基于特征点的测量方法容易受雨水、浓雾或遮挡等因素影响，限制了其测量可靠性，而且在现场复杂环境下使用近距离视觉导航时，不可避免地受到外界干扰，降低了其测量精度与可靠性。因此研究现场户外复杂条件下目标位姿参数快速测量方法成为亟待解决的难题。

发明内容

本发明实施例提供一种位姿参数测量方法、装置、系统和存储介质，可以在现场复杂环境情况下，尤其是末端对接视觉导航情况下实现被测目标位姿参数的高速、高精度测量。

第一方面，本发明实施例提供了一种位姿参数测量方法，包括：

获取视觉传感器和被测目标的距离信息；

根据所述距离信息确定所述视觉传感器的工作模式，发送工作模式信息给所述视觉传感器，其中，所述工作模式信息用于指示所述视觉传感器采集合作标识的面阵图像或者线阵图像；

获取所述视觉传感器采集的所述面阵图像或线阵图像，根据所述面阵图像或线阵图像计算所述被测目标的位姿参数。

第二方面，本发明实施例还提供了一种位姿参数测量装置，包括：

箱体，用于承载视觉传感器和结构光传感器，所述箱体上设置有供对接装置穿过的通孔，其中，所述对接装置是即将与被测目标对接的装置；

至少两个视觉传感器，分布于所述通孔的外周，用于工作于由处理器确定的工作模式下，采集被测目标表面的合作标识的面阵图像信息或线阵图像信息，其中，所述合作标识包括反光带和位于所述反光带上的编码特征点；

至少两个结构光传感器，分布于所述通孔的外周，用于测量所述被测目标的三维数据；

处理器，与所述视觉传感器和结构光传感器通信连接，用于执行如本发明任意实施例提供的位姿参数测量方法。

第三方面，本发明实施例还提供了一种位姿参数测量系统，包括如本发明任意实施例提供的位姿参数测量装置，还包括：合作标识，包括由反光材料形成的反光带，所述反光带构成的多边形设置于所述被测目标的表面，且所述多边形的每条边的中间位置均设置有编码特征点；

驱动机构，与所述位姿参数测量装置中的箱体固定连接，且与所述位姿参数测量装置中的处理器通信连接，用于接收所述处理器发送的第四坐标和第二位姿参数，根据所述第四坐标或第二位姿参数调整箱体与所述被测目标的相对方位。

第四方面，本发明实施例还提供了一种包含计算机可执行指令的存储介质，所述计算机可执行指令在由计算机处理器执行时用于执行如本发明任意实施例提供的位姿参数测量方法。

本发明实施例通过获取视觉传感器和被测目标的距离信息，并根据该距离信息确定视觉传感器的工作模式，将工作模式信息发送给视觉传感器，根据视觉传感器采集的面阵图像或线阵图像，计算被测目标的位姿参数。本发明实施例通过切换视觉传感器的工作模式，计算不同工作模式下被测目标的位姿参数，实现被测目标与视觉传感器位姿参数由粗到精的高速、高精度测量。

附图说明

图1是本发明实施例一提供的一种位姿参数测量方法的流程图；

图2是本发明实施例二提供的另一种位姿参数测量方法的流程图；

图3是本发明实施例二提供的一种计算被测目标的位姿参数的流程图；

图4是本发明实施例二提供的一种双目立体视觉测量方法的示意图；

图5是本发明实施例二提供的一种位姿参数测量方法中线阵模式的示意图；

图6是本发明实施例二提供的一种位姿参数测量方法中反光带的空间偏差的计算示意图；

图7是本发明实施例二提供的另一种计算被测目标的位姿参数的流程图；

图8是本发明实施例提供的又一种位姿参数测量方法的流程图；

图9是本发明实施例四提供的一种位姿参数测量装置的结构示意图；

图10是本发明实施例四提供的一种位姿参数测量装置的正视图；

图11是本发明实施例五提供的一种合作标识的结构示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。

实施例一

图1是本发明实施例一提供的一种位姿参数测量方法的流程图，本实施例可适用于如为火箭加注燃料等高危场合下、末端高精度视觉导航对接的情况，该方法可以由处理器来执行，该装置可以采用软件/硬件的方式实现。该装置可配置于位姿参数测量装置中。如图1所示，该方法具体包括：

步骤S110、获取视觉传感器和被测目标的距离信息。

被测目标是即将与对接装置对接的装置，例如，在火箭加注燃料过程中，被测目标可以是加注口，对接装置可以是加注管，加注管可以通过与加注口对接，将燃料输送至火箭中。

具体地，结构光传感器测量视觉传感器与被测目标的距离信息，并将该距离信息发送至处理器中，处理器获取该距离信息。例如，处理器获取视觉传感器和加注口之间的距离信息。

可选地，在获取视觉传感器和被测目标的距离信息之前，还包括：获取视觉传感器内部参数和外部参数，并存入配置文件中，其中，内部参数包括各相机内参数，外部参数包括各个相机之间的相对外参数和视觉传感器与结构光传感器之间的外方位参数。

具体地，在测量之前，线下采用相机标定方法分别获取传感器参数信息，参数信息包括各相机内部参数、各相机之间的相对外参数和各相机与结构光传感器之间的外方位参数，并存入配置文件中，在位姿参数测量系统初始化完成后，将参数信息加载入位姿参数测量系统中。一方面能够矫正各个相机的镜头的畸变，实现各个相机之间组成立体视觉传感器进行测量，另一方面能够将各个相机的数据统一到一个坐标系下。

可选地，在获取视觉传感器和被测目标的距离信息之前，技术人员在被测目标上贴附合作标识，并设定合作标识中心为视觉传感器的中心位置，即视觉传感器中心的对接装置与被测目标达到可对接状态。

可选地，在获取视觉传感器和被测目标的距离信息之前，还包括：读取初始化信息，打开温控模块和防护门模块，打开视觉传感器上的相机和光源，使系统进入测量状态。

步骤S120、根据距离信息确定视觉传感器的工作模式，发送工作模式信息给视觉传感器。

其中，工作模式信息用于指示视觉传感器采集合作标识的面阵图像或者线阵图像。工作模式可以包括面阵模式和线阵模式，其中，面阵模式用于粗略调整箱体与被测目标的相对方位，线阵模式用于精确调整箱体与被测目标的相对方位。面阵图像是视觉传感器在面阵模式下采集合作标识的图像，线阵图像是视觉传感器在线阵模式下采集合作标识的图像。

具体地，处理器根据距离信息确定视觉传感器的工作模式，若工作模式为面阵模式，则处理器发送面阵模式信息给视觉传感器，视觉传感器根据该面阵模式信息采集合作标识的面阵图像，若工作模式为线阵模式，则处理器发送线阵模式信息给视觉传感器，视觉传感器根据该线阵模式信息采集合作标识的线阵图像。

可选地，处理器对视觉传感器上的各数据采集模块发出精确的同步触发信号，触发视觉传感器和结构光传感器等采集图像。

步骤S130、获取视觉传感器采集的面阵图像或线阵图像，根据面阵图像或线阵图像计算被测目标的位姿参数。

位姿参数是与被测目标对接过程中的位置参数和姿态参数，用于确定箱体与被测目标之间的相对位置和相对姿态，可以包括坐标系间的旋转矩阵、平移矩阵和/或变换矩阵等，还可以包括偏航角、俯仰角和/或滚动角等。

具体地，获取视觉传感器采集的面阵图像，根据面阵图像计算被测目标局部坐标系到相机坐标系的旋转矩阵和平移矩阵；或者，获取视觉传感器采集的线阵图像，根据线阵图像计算被测目标的偏航角和俯仰角。

本发明实施例通过获取视觉传感器和被测目标的距离信息，根据该距离信息确定视觉传感器的工作模式，并将工作模式信息发送给视觉传感器，获取视觉传感器采集的面阵图像或线阵图像，根据面阵图像或线阵图像计算被测目标的位姿参数。本发明实施例通过切换视觉传感器的工作模式，计算不同工作模式下被测目标的位姿参数，实现被测目标与视觉传感器位姿参数由粗到精的高速、高精度测量。

实施例二

图2是本发明实施例二提供的另一种位姿参数测量方法的流程图，本实施例在上述实施例的基础上进行优化，如图2所示，该方法包括：

步骤S210、获取结构光传感器测量的视觉传感器与被测目标的距离信息。

其中，距离信息基于结构光传感器测量的被测目标的三维数据计算。三维数据用于描述被测目标在三维空间中的位置。

步骤S220、判断距离信息是否大于等于设定距离阈值，若是，执行步骤S230，否则执行步骤S260。

步骤S230、指示视觉传感器工作于面阵模式，发送面阵模式信息给视觉传感器。

其中，面阵模式下视觉传感器的感兴趣区域ROI呈现矩形状。ROI指视觉传感器采集的图像中选择的一个图像区域，用于对空间按编码特征点进行搜索与测量，可以以矩形、圆形、椭圆形或不规则多边形等方式勾勒出需要处理的区域，并进行图像的下一步处理，能够减少处理时间，提高图像处理精度。

步骤S240、获取视觉传感器采集的面阵图像，根据面阵图像计算被测目标的位姿参数。

图3是本发明实施例二提供的一种计算被测目标的位姿参数的流程图，如图3所示，具体步骤如下：

步骤S241、基于面阵图像确定合作标识包括的编码特征点集的第一坐标矩阵。

步骤S242、基于合作标识包括的编码特征点集的空间坐标确定第二坐标矩阵。

在将合作标识设置到被测目标的表面时，可以测量空间坐标系中合作标识上编码特征点和被测目标的空间坐标，并存储到存储器。可选地，基于编码特征点的空间坐标和被测目标的空间坐标可以构建被测目标局部坐标系，以表示编码特征点与被测目标的空间关系。具体地，处理器由存储器中读取合作标识包括的编码特征点集的空间坐标，进而将编码特征点集的空间坐标作为第二坐标矩阵。

步骤S243、根据第一坐标矩阵和第二坐标矩阵确定被测目标局部坐标系到相机坐标系的旋转矩阵和平移矩阵。

具体地，处理器根据编码特征点集的第一坐标矩阵和基于编码特征点集的空间坐标确定的第二坐标矩阵，确定被测目标局部坐标系到相机坐标系的旋转矩阵和平移矩阵，将旋转矩阵和平移矩阵作为被测目标的第一位姿参数。

示例性地，图4是本发明实施例二提供的一种双目立体视觉测量方法的示意图，如图4所示，设两个相机与被测物体在空间形成一个三角形，利用空间点在两相机像平面上成像点坐标求取空间点的三维坐标，设O_c1-X_c1Y_c1Z_c1为相机1的坐标系，有效焦距为c₁，像平面坐标系为O₁-X₁Y₁，O_c2-X_c2Y_c2Z_c2为相机2坐标系，有效焦距为c₂，像平面坐标系为O₂-X₂Y₂，将相机1坐标系作为双目视觉传感器坐标系O_s-X_sY_sZ_s。两相机之间的空间位置关系为：

式中，

表示相机坐标系2到相机坐标系1的旋转矩阵；t＝[t_x t_y t_z]表示相机坐标系2到相机坐标系1的平移矩阵。基于面阵图像确定合作标识包括的编码特征点集的第一坐标矩阵为P_m,{m＝1,2,3,4}，基于合作标识包括的编码特征点集的空间坐标确定第二坐标矩阵为P_i,{i＝1,2,3,4}，由于被测目标局部坐标系到相机坐标系的旋转矩阵R₁和平移矩阵t₁满足P_m＝P_i·R₁+t₁，通过计算第一坐标矩阵P_m,{m＝1,2,3,4}和第二坐标矩阵P_i,{i＝1,2,3,4}之间的变换矩阵，可得被测目标局部坐标系到相机坐标系的位姿参数[R₁,t₁]。

步骤S244、将旋转矩阵和平移矩阵作为被测目标的第一位姿参数。

步骤S245、获取面阵图像中被测目标的第三坐标，根据第三坐标、旋转矩阵和平移矩阵计算被测目标的相机坐标系中的第四坐标。

具体地，处理器获取面阵图像中被测目标的第三坐标，基于相机透视变换模型，根据第三坐标、旋转矩阵和平移矩阵计算被测目标的相机坐标系中的第四坐标。

示例性地，如图4所示，面阵图像中被测目标的第三坐标为(X₁,Y₁)和(X₂,Y₂)，根据相机透视变换模型，被测目标的相机坐标系中的第四坐标为

由该公式可知，通过旋转矩阵R₁、平移矩阵t₁以及第三坐标(X₁,Y₁)和(X₂,Y₂)，可以求解第四坐标(x_s,y_s,z_s)。

步骤S246、将第四坐标发送给驱动机构，以指示驱动机构基于第四坐标调整箱体与被测目标的相对方位。

其中，箱体用于承载视觉传感器和结构光传感器，且箱体上安装有对接装置，对接装置是即将与被测目标对接的装置。

具体地，处理器将第四坐标发送给驱动机构，以指示驱动机构基于第四坐标调整箱体与被测目标的相对方位，直至视觉传感器与被测目标的距离小于设定距离阈值，退出面阵模式。

可选地，结构光传感器实时获取箱体和被测目标的距离信息。通过实时监测箱体和被测目标之间的距离，不仅能够实现位姿参数的精度测量，还能够防止箱体和被测目标发生碰撞。

示例性地，在火箭加注口与加注管对接过程中，处理器指示驱动机构基于第四坐标调整视觉传感器的相对方位，直至视觉传感器和加注口靠近并在小范围内发生相对运动，结构光传感器实时获取视觉传感器和加注口的距离信息，当加注口和视觉传感器的距离小于设定距离阈值时，退出面阵模式，进入线阵模式。步骤S250、基于被测目标的位姿参数，指示驱动机构调整箱体与被测目标的相对方位。

具体地，当视觉传感器工作于面阵模式时，将基于面阵图像计算的被测目标的位姿参数发送给驱动机构，以指示驱动机构基于位姿参数调整箱体与被测目标的相对方位；当全部视觉传感器无法检测到合作标识中的反光带时，基于检测到反光带的视觉传感器的面阵图像中的编码特征点，指示驱动机构调整箱体与被测目标的相对方位，直至全部视觉传感器可以检测到合作标识中的反光带。

步骤S260、判断全部视觉传感器是否都检测到合作标识中的反光带，若是，执行步骤S270，否则返回步骤S240。步骤S270、指示视觉传感器工作于线阵模式，发送线阵模式信息给视觉传感器。

其中，由于线阵模式下视觉传感器是通过一行或几行感光元素进行图像采集的，因此，线阵模式下视觉传感器的感兴趣区域ROI呈现条带状。当被测目标与视觉传感器距离较近时，视觉传感器中相机的视场中可能测不到特征编码点，但是通过视觉传感器可以采集到合作标识中的反光带，保证在特征编码点被遮挡的情况下，仍可通过搜索识别反光带完成测量，实现快速准确地测量位姿参数。

步骤S280、获取视觉传感器采集的线阵图像，根据线阵图像计算被测目标的位姿参数。

图7是本发明实施例二提供的另一种计算被测目标的位姿参数的流程图，如图7所示，具体步骤如下：

步骤S281、计算线阵图像中合作标识的各个反光带的中心坐标与图像中心点坐标的偏差值。

具体地，当全部视觉传感器都检测到合作标识中的反光带时，处理器提取线阵图像中合作标识的各个反光带的中心坐标，计算线阵图像中合作标识的各个反光带的中心坐标与图像中心点坐标的偏差值。

示例性地，图5是本发明实施例二提供的一种位姿参数测量方法中线阵模式的示意图，如图5所示，C₁和C₃工作于横向线阵模式，用于检测被测目标俯仰角度变化，C₂和C₄工作于竖向工作模式，用于检测被测目标偏航角度变化。当全部视觉传感器都检测到合作标识中的反光带时，即只有当被测目标位于中心时，各视觉传感器检测得到反光带坐标位于相面中心处。提取线阵图像中合作标识的各个反光带的中心坐标，计算各个反光带的中心坐标与图像中心点坐标的偏差值d_v1,d_v2,d_v3,d_v4。

步骤S282、根据距离信息、视觉传感器的焦距和偏差值计算反光带的空间偏差。

具体地，根据结构光传感器测量得到的距离信息、视觉传感器的焦距和线阵图像中合作标识的各个反光带的中心坐标与图像中心点坐标的偏差值，计算反光带的空间偏差值。

示例性地，结构光传感器测量得到的距离信息为D₁,D₂，视觉传感器的焦距为c₁,c₂，各个反光带的中心坐标与图像中心点坐标的偏差值为d_v1,d_v2,d_v3,d_v4，根据相似三角形的相似比，计算可得反光带的空间偏差ΔL₁,ΔL₂,ΔL₃,ΔL₄。

例如，以视觉传感器位置为顶点，以反光带的空间偏差为底边构建第一等腰三角形。以视觉传感器位置为顶点，以视觉传感器的像平面中对应反光带的中心坐标与图像中心点坐标的偏差值为底边构建第二等腰三角形。基于第一等腰三角形和第二等腰三角形的相似比例关系确定反光带的空间偏差。图6是本发明实施例二提供的一种位姿参数测量方法中反光带的空间偏差的计算示意图。如图6所示，以ΔL₁为例说明计算空间偏差的方式，以视觉传感器C₁的位置为顶点，以反光带的空间偏差ΔL₁为底边的第一等腰三角形与以视觉传感器C₁的位置为顶点，以反光带的中心坐标偏差值

为底边的第二等腰三角形相似，基于相似三角形的比例关系可以得到如下公式：

其中，D₁是视觉传感器C₁与反光条L₁的距离，c₁是视觉传感器C₁的焦距。

需要说明的是，剩余反光条的空间偏差可以基于对应的参数，按照上述公式计算得到。可选地，结构光传感器实时获取箱体和被测目标的距离信息。通过实时监测箱体和被测目标之间的距离，不仅能够实现位姿参数的精度测量，还能够保证位姿参数在线阵模式下快速得出，保证在近距危险的情况下的测量速率。

步骤S283、根据空间偏差和距离信息计算被测目标的偏航角和俯仰角。

步骤S284、将偏航角和俯仰角作为被测目标的第二位姿参数。

具体地，根据反光带的空间偏差和结构光传感器测量得到的距离信息，处理器计算被测目标的偏航角和俯仰角，将偏航角和俯仰角作为被测目标的第二位姿参数。

示例性地，反光带的空间偏差为ΔL₁,ΔL₂,ΔL₃,ΔL₄，结构光传感器测量得到的距离信息为D₁,D₂，计算可得被测目标的偏航角为α＝tan^-1(0.5·(ΔL₁+ΔL₃)/D₁)，俯仰角为β＝tan^-1(0.5·(ΔL₂+ΔL₄)/D₂)。

步骤S285、将第二位姿参数发送给驱动机构，以指示驱动机构基于第二位姿参数调整箱体与被测目标的相对方位。

其中，箱体用于承载视觉传感器和结构光传感器，且箱体上安装有对接装置，对接装置是即将与被测目标对接的装置。

具体地，处理器将第二位姿参数发送给驱动机构，以指示驱动机构基于第二位姿参数调整箱体与被测目标的相对方位，用于指导后续对接或抓取等操作。

本发明实施例根据结构光传感器测量的视觉传感器与被测目标的距离信息，指示视觉传感器工作于面阵模式或线阵模式，根据视觉传感器采集的面阵图像计算被测目标的第一位姿参数，根据视觉传感器采集的线阵图像计算被测目标的第二位姿参数。本发明实施例通过对附着在被测目标上的合作标识进行识别、提取、分析和计算，结合结构光传感器测量数据得到被测目标相对视觉传感器的位置姿态，实现被测物与视觉传感器位姿参数由粗到精的高速、高精度地实时测量，而且在雨水、浓雾、结冰、高反光或遮挡等场合以及末端高精度视觉导航对接领域均具有重要的应用价值。

实施例三

本发明实施例公开一种基于点线特征结合的目标位姿参数快速测量方法和装置，包括：在被测物局部贴附反光带和编码特征点作为合作标识；设计一种基于四面阵相机的目标位姿参数快速测量装置，其中两面阵相机可组成立体视觉测量系统，且各相机可切换至线阵高速工作模式；首先标定出各个相机内部参数以及各相机之间相对外参数；其次设定相机工作在面阵模式下，采集被测目标表面的合作标识编码特征点图像；提取编码特征点坐标，通过立体视觉原理测量合作特征点空间坐标，完成目标位姿参数初始测量；根据测得位姿参数调整被测物与视觉传感器相对方位，直至被测物和视觉传感器在小范围内发生相对运动；当各相机均能观测到发光带时，将相机转入线阵工作模式，通过提取光带中心坐标，计算各条带中心坐标偏差实现被测物位置及姿态参数的实时高精度测量。本发明通过切换相机工作模式，配合测量被测物表面点、线特征，实现被测物与视觉传感器位姿参数由粗到精的高速、高精度测量，适合在高危场合下、末端高精度视觉导航对接等领域，具有重要的应用价值。

图8是本发明实施例提供的又一种位姿参数测量方法的流程图，如图8所示，该方法包括：

步骤S310、设计一种横竖为反光条带，条带中间为编码特征点的方形合作标识，在被测物局部贴附该合作标识，且在光源照射合作标识时保证成像清晰；

步骤S320、设计一种基于四面阵相机的目标位姿参数快速测量装置，其中两面阵相机可组成立体视觉测量系统，且各相机通过设置感兴趣区域(ROI)由面阵模式切换至线阵高速工作模式；

步骤S330、线下标定出各个相机内部参数以及各相机之间相对外参数；设定相机工作在面阵模式下，采集被测目标表面的合作标识编码特征点图像；

步骤S340、提取编码特征点坐标，通过立体视觉原理测量合作特征点空间坐标，完成目标位姿参数初始测量；

步骤S350、根据测得位姿参数调整被测物与视觉传感器相对方位，直至被测物和视觉传感器在小范围内发生相对运动；

步骤S360、判断各相机是否能观测到发光带，若是，执行步骤S370，否则返回步骤S340。

步骤S370、当各相机均能观测到发光带时，将相机转入线阵工作模式，通过提取光带中心坐标，计算各条带中心坐标偏差实现被测物位置及姿态参数的实时高精度测量。

其中，步骤S310中设计一种横竖为反光条带，条带中间为编码特征点的方形合作目标，分别作为视觉传感器测量的特征条与特征点，在被测物局部贴附该合作标识。其中该合作标识为反光材料制作，在光源照射合作标识时能够保证成像清晰。

步骤S320中设计一种基于四面阵相机的目标位姿参数快速测量装置，四目相机中间为机械操作装置，用于和被测物发生对接等操作。其中两面阵相机可组成立体视觉测量系统，且各相机通过设置感兴趣区域(ROI)由面阵模式切换至线阵高速工作模式。方法如下：

(1)设计一种由四台工业面阵相机组成的测量装置，其中两两相机可组成立体视觉测量系统，用于快速识别和测量编码特征点空间坐标；

(2)相机的感兴趣区域可灵活设定，当在位姿测量初期，可设置较大ROI区域，相机工作在面阵模式，可测量区域较大；也可设置细长的ROI区域，相机工作在线阵模式下，此时相机采集图像为条带图像，但其采集速度成倍提升，保证位姿参数测量实时性。

步骤S330中线下采用基于平面靶标的标定方法标定出各个相机内部参数以及各相机之间相对外参数；设定相机工作在面阵模式下，采集被测目标表面的合作标识编码特征点图像。

步骤S340中分别提取编码特征点图像中编码区域和中心特征点坐标，通过解码识别出各个特征点，通过立体视觉原理测量合作特征点空间坐标，完成目标位姿参数初始测量，保证初始位姿计算的准确性和唯一性。

步骤S350中根据步骤S340中测得位姿参数调整被测物与视觉传感器相对方位，调整被测物或者视觉传感器，直至被测物和视觉传感器在小范围内发生相对运动。

步骤S370中随着被测物和传感器之间位姿参数的调整，方法如下：

(1)当各相机均能观测到发光带时，改变相机ROI区域，将相机转入线阵工作模式；

(2)通过提取光带中心坐标，计算各条带中心坐标偏差实现被测物位置及姿态参数的实时高精度测量。

实施例四

图9是本发明实施例四提供的一种位姿参数测量装置的结构示意图。该装置可由软件和/或硬件实现，可以实现被测目标与视觉传感器位姿参数由粗到精的高速、高精度测量。如图9所示，该装置包括：箱体，至少两个视觉传感器，至少两个结构光传感器，以及处理器。

箱体用于承载视觉传感器和结构光传感器，箱体上设置有供对接装置穿过的通孔，其中，对接装置是即将与被测目标对接的装置。箱体可以采用“口”字形设计，底部固定在执行机构工装上，例如安装支架，箱体具有防水防尘、防火防爆和温度控制等满足现场使用各项技术要求的功能设计。对接装置可以通过箱体上的通孔与被测目标对接，可以实现液体的传送等。

至少两个视觉传感器分布于通孔的外周，用于工作于由处理器确定的工作模式下，采集被测目标表面的合作标识的面阵图像信息或线阵图像信息，其中，合作标识包括反光带和位于反光带上的编码特征点。例如，图10是本发明实施例四提供的一种位姿参数测量装置的正视图，如图10所示，视觉传感器具有四个，每个视觉传感器具有一个高速相机，分布于箱体的四个角上，在横向和纵向两两组成双目立体视觉测量系统。避免由于雨水、浓雾、结冰、高反光或遮挡等情况导致合作标识部分缺失造成测量失败，保证高速相机采集到合作标识的面阵图像信息或线阵图像信息。即在视觉传感器中部分合作标识的面阵图像信息或线阵图像信息缺失时，可以通过双目立体视觉测量系统采集的图像融合互补，仍可通过立体视觉测量原理重建得到被测目标表面的合作标识的面阵图像信息或线阵图像信息，例如三维信息等。

编码特征点是具有编码信息的特征点，用于确定合作标识的坐标，可以设置在反光带的中心，这样设置的好处在于能够使特征点分布均匀，避免因特征点设置集中而影响双目立体视觉测量准确度。

可选地，视觉传感器的前端设置有滤光片，滤光片允许与单色光的波长相同的光通过。例如，在视觉传感器的相机前端设置与单色光对应波长的窄带滤光片，尽可能保证相机采集到的图像只有合作标识区域高亮，从而确保后续图像处理、计算的速度和鲁棒性。

至少两个结构光传感器分布于通孔的外周，用于测量被测目标的三维数据。例如，如图10所示，在视觉传感器之间安装两组结构光传感器，用于实时高精度测量被测目标的三维数据。考虑到测量过程中，视觉传感器到被测目标之间的距离信息是实现安全操作的前提，因此，在视觉传感器之间安装结构光传感器，实时重建得到被测目标的三维数据，并通过与视觉传感器数据进行融合，不仅能够准确地测量出被测目标的位姿参数，而且能够提高测量精度和保证位姿参数测量装置的可靠性与安全性。

可选地，至少两个结构光传感器发射的横向的结构光属于不同的平面。例如，在火箭系统中，由于火箭是竖直的，发射横向的结构光能够保证光到火箭表面后相交于一条线，通过这样的至少两条线实现对火箭表面的三维拟合，得到火箭的三维数据。

处理器与视觉传感器和结构光传感器通信连接，用于执行如本发明任意实施例提供的位姿参数测量方法。

可选地，该位姿参数测量装置，还包括：

单色照明灯，单色照明灯的形状是环形，单色照明灯设置于每个视觉传感器的外围，且以对应的视觉传感器的光轴为对称轴，用于向合作标识投射单色光。例如，如图10所示，单色照明灯可以是发光二极管(light emitting diode,LED)灯，LED灯环形设置于每个相机的外围，且与相机同轴设置。使用单色照明灯作为反光照明光源，不仅能够保证反射回光效果，而且具有透雾性强、射程远和功率可调等功能。

本发明实施例所提供的位姿参数测量装置可执行本发明任意实施例所提供的位姿参数测量方法，具备执行方法相应的功能模块和有益效果。

实施例五

位姿参数测量系统可以通过执行位姿参数测量方法实现被测目标与视觉传感器位姿参数由粗到精的高速、高精度测量。该系统包括：

本发明任意实施例提供的位姿参数测量装置，还包括：合作标识，包括由反光材料形成的反光带，反光带构成的多边形设置于被测目标的表面，且多边形的每条边的中间位置均设置有编码特征点。

其中，反光材料是具有良好光反射效果的材料，用于在能见度较低的情况下视觉传感器可以采集被测目标表明的合作标识图像，例如，可以选用可在-80°到80°的入射角范围内实现90％以上反射效果的逆反光材料作为反光材料，保证在能见度较低的情况下，如雨雪、雾天等天气，仍能保证合作标识图像的成像质量。可以通过定制镂空网板，以喷涂方式将反光材料喷涂在被测目标表面，形成反光带，消除由于贴附反光物脱落导致的安全隐患，其中，镂空网版的加工尺寸可以根据被测目标表明的被测区域大小决定。当视觉传感器侧的激光光源照射反光带后，即可产生强烈明显的逆反光效果。将反光带构成的多边形设置于被测目标的表面，能够使设计的反光带形态具有易识别、提取精度高等优点，能够为视觉传感器提供良好的图像数据，进而能够为精确、稳定、快速地被测目标相对位姿测量提供良好的基础。合作标识的尺寸可以通过线下测得，即为已知量。编码特征点可以设置在由反光带构成的多边形的每条边的中间位置，这样设置使编码特征点分布均匀，避免因编码特征点设置集中而影响双目立体视觉测量准确度。例如，图11是本发明实施例五提供的一种合作标识的结构示意图，如图11所示，反光带以正方形喷涂在被测目标四周，编码特征点设置于被测目标的上、下、左和右反光带中间位置上，当传感器侧的激光光源照射合作标识中的反光带后，可以产生强烈明显的逆反光效果。同时，合作标识可以采用点线结合的设计模式，编码特征点可以采用编码方式设计，且可线下标定好其相对于被测目标之间的相对位置，保证在相机只观测到部分编码特征点时不会引起识别错误，仍然能够正确引导视觉传感器视场覆盖到全部合作标识，提高了位姿参数测量的鲁棒性和准确性。

驱动机构，与位姿参数测量装置中的箱体固定连接，且与位姿参数测量装置中的处理器通信连接，用于接收处理器发送的第四坐标和第二位姿参数，根据第四坐标或第二位姿参数调整箱体与被测目标的相对方位。

本发明实施例所提供的位姿参数测量系统可执行本发明任意实施例所提供的位姿参数测量方法，具备执行方法相应的功能模块和有益效果。

实施例六

本发明实施例六还提供一种包含计算机可执行指令的存储介质，所述计算机可执行指令在由计算机处理器执行时用于执行一种位姿参数测量方法，该方法包括：

获取视觉传感器和被测目标的距离信息；

根据所述距离信息确定所述视觉传感器的工作模式，发送工作模式信息给所述视觉传感器，其中，所述工作模式信息用于指示所述视觉传感器采集合作标识的面阵图像或者线阵图像；

获取所述视觉传感器采集的所述面阵图像或线阵图像，根据所述面阵图像或线阵图像计算所述被测目标的位姿参数。

当然，本发明实施例所提供的一种包含计算机可执行指令的存储介质，其计算机可执行指令不限于如上所述的方法操作，还可以执行本发明任意实施例所提供的位姿参数测量方法中的相关操作。

通过以上关于实施方式的描述，所属领域的技术人员可以清楚地了解到，本发明可借助软件及必需的通用硬件来实现，当然也可以通过硬件实现，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如计算机的软盘、只读存储器(Read-Only Memory,ROM)、随机存取存储器(RandomAccess Memory,RAM)、闪存(FLASH)、硬盘或光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。

值得注意的是，上述位姿参数测量装置的实施例中，所包括的各个单元和模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能单元的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (9)

1.一种位姿参数测量方法，其特征在于，包括：

获取视觉传感器和被测目标的距离信息；

根据所述距离信息确定所述视觉传感器的工作模式，发送工作模式信息给所述视觉传感器，其中，所述工作模式信息用于指示所述视觉传感器采集合作标识的面阵图像或者线阵图像；

获取所述视觉传感器采集的所述面阵图像或线阵图像，根据所述面阵图像或线阵图像计算所述被测目标的位姿参数；

其中，所述根据所述距离信息确定所述视觉传感器的工作模式，包括：

当所述距离信息大于等于设定距离阈值时，指示所述视觉传感器工作于面阵模式，其中，所述面阵模式下所述视觉传感器的感兴趣区域ROI呈现矩形状；

当所述距离小于设定距离阈值，且全部所述视觉传感器都检测到所述合作标识中的反光带时，指示所述视觉传感器工作于线阵模式，其中，所述线阵模式下所述视觉传感器的感兴趣区域ROI呈现条带状；

所述获取所述视觉传感器采集的所述面阵图像或线阵图像，根据所述面阵图像或线阵图像计算所述被测目标的位姿参数，包括：

获取所述视觉传感器采集的所述面阵图像，根据所述面阵图像计算所述被测目标局部坐标系到相机坐标系的旋转矩阵和平移矩阵；或者，获取所述视觉传感器采集的所述线阵图像，根据所述线阵图像计算所述被测目标的偏航角和俯仰角；

其中，将所述旋转矩阵和平移矩阵作为所述被测目标的第一位姿参数，所述第一位姿参数用于指示驱动机构基于所述第一位姿参数调整箱体与所述被测目标的相对方位，直至全部所述视觉传感器都检测到所述合作标识中的反光带；将所述偏航角和俯仰角作为所述被测目标的第二位姿参数，所述第二位姿参数用于指导后续对接或抓取操作。

2.根据权利要求1所述的位姿参数测量方法，其特征在于，根据所述面阵图像计算所述被测目标局部坐标系到相机坐标系的旋转矩阵和平移矩阵，包括：

基于所述面阵图像确定所述合作标识包括的编码特征点集的第一坐标矩阵；

基于所述合作标识包括的编码特征点集的空间坐标确定第二坐标矩阵；

根据所述第一坐标矩阵和第二坐标矩阵确定被测目标局部坐标系到相机坐标系的旋转矩阵和平移矩阵。

3.根据权利要求2所述的位姿参数测量方法，其特征在于，在根据所述第一坐标矩阵和第二坐标矩阵确定被测目标局部坐标系到相机坐标系的旋转矩阵和平移矩阵之后，还包括：

获取所述面阵图像中所述被测目标的第三坐标，根据所述第三坐标、旋转矩阵和平移矩阵计算所述被测目标的所述相机坐标系中的第四坐标；

将所述第四坐标发送给驱动机构，以指示所述驱动机构基于所述第四坐标调整箱体与所述被测目标的相对方位，其中，所述箱体用于承载视觉传感器和结构光传感器，且所述箱体上安装有对接装置，所述对接装置是即将与所述被测目标对接的装置。

4.根据权利要求1所述的位姿参数测量方法，其特征在于，根据所述线阵图像计算所述被测目标的偏航角和俯仰角，包括：

计算所述线阵图像中所述合作标识的各个反光带的中心坐标与图像中心点坐标的偏差值；

根据所述距离信息、所述视觉传感器的焦距和所述偏差值计算所述反光带的空间偏差；

根据所述空间偏差和所述距离信息计算所述被测目标的偏航角和俯仰角。

5.根据权利要求4所述的位姿参数测量方法，其特征在于，在根据所述空间偏差和所述距离信息计算所述被测目标的偏航角和俯仰角之后，还包括：

将所述第二位姿参数发送给驱动机构，以指示所述驱动机构基于所述第二位姿参数调整箱体与所述被测目标的相对方位，其中，所述箱体用于承载视觉传感器和结构光传感器，且所述箱体上安装有对接装置，所述对接装置是即将与所述被测目标对接的装置。

6.一种位姿参数测量装置，其特征在于，包括：

箱体，用于承载视觉传感器和结构光传感器，所述箱体上设置有供对接装置穿过的通孔，其中，所述对接装置是即将与被测目标对接的装置；

至少两个视觉传感器，分布于所述通孔的外周，用于工作于由处理器确定的工作模式下，采集被测目标表面的合作标识的面阵图像信息或线阵图像信息，其中，所述合作标识包括反光带和位于所述反光带上的编码特征点；

至少两个结构光传感器，分布于所述通孔的外周，用于测量所述被测目标的三维数据；

处理器，与所述视觉传感器和结构光传感器通信连接，用于执行如权利要求1-5中任一项所述的位姿参数测量方法。

7.根据权利要求6所述的位姿参数测量装置，其特征在于，还包括：

单色照明灯，所述单色照明灯的形状是环形，所述单色照明灯设置于每个所述视觉传感器的外围，且以对应的视觉传感器的光轴为对称轴，用于向合作标识投射单色光。

8.一种位姿参数测量系统，其特征在于，包括如权利要求6-7中任一项所述的位姿参数测量装置，还包括：

合作标识，包括由反光材料形成的反光带，所述反光带构成的多边形设置于所述被测目标的表面，且所述多边形的每条边的中间位置均设置有编码特征点；

驱动机构，与所述位姿参数测量装置中的箱体固定连接，且与所述位姿参数测量装置中的处理器通信连接，用于接收所述处理器发送的第四坐标和第二位姿参数，根据所述第四坐标或第二位姿参数调整箱体与所述被测目标的相对方位。

9.一种包含计算机可执行指令的存储介质，其特征在于，所述计算机可执行指令在由计算机处理器执行时用于执行如权利要求1-5中任一所述的位姿参数测量方法。

Images