CN111288888B - 面向机械手自动抓捕的大尺寸圆环目标结构光测量方法 - Google Patents

Abstract

本发明涉及面向机械手自动抓捕的大尺寸圆环目标结构光测量方法，通过在机械手末端设置手眼视觉传感器，将激光分别投射到圆环目标前表面上，用相机采集带有激光条的圆环目标图像，在圆环目标图像中识别出候选目标激光条组合；根据相对位姿求解需要，在候选目标激光条上选择并提取出点线结合的目标图像特征；对每组候选激光光条组合，根据目标图像特征采用基于中间变换的位姿求解方法得到机械手工具坐标系和目标坐标系间相对位置和姿态。本发明采用双侧多线结构光实现大尺寸圆环目标的位姿测量，具有可靠性高、对光照环境适应性强、测量速度快的特点，当基线足够长时，可以达到很高的测量精度。

Description

面向机械手自动抓捕的大尺寸圆环目标结构光测量方法

技术领域

本发明属于计算机视觉领域，利用2组特殊设计的结构光测量传感器可以实现机械手对大尺寸圆环目标抓捕过程中机械手工具坐标系与圆环目标坐标系间相对位置和姿态的在线测量。

技术背景

随着空间技术的不断发展，发射到太空的卫星不断增多，大量失效的卫星已成为太空垃圾，对这些卫星的在轨维修或使其脱离运行轨道进入坟墓轨道的需求越来越迫切，利用空间机械手对失效卫星进行维修与离轨操作已成为大势所趋。空间机械手对失效卫星的自动抓捕是实现在轨维修和离轨操作的前提条件，为达到这一目的，空间机械手必须具备对目标的在线定位和定姿的能力。由于空间机械手对空间目标的抓捕操作属于近距离操作，视觉传感器是最合适有时甚至是唯一可用的测量传感器。因此，空间手眼视觉测量是空间机械手自动抓捕中最为关键的技术之一。由于失效卫星没有安装用于视觉测量的合作标志器，只能对空间卫星本身所固有的典型部件进行自动抓捕。纵观空间卫星的各种典型部件，星箭对接环是所有卫星都有的，且在强度、目标特征显著程度、目标模型已知程度等方面均为最合适的抓捕对象。目前，各空间大国对空间卫星自动抓捕方案多采用抓捕星箭对接环的形式。星箭对接环作为卫星与火箭间的连接装置，当星箭分离后，留在卫星上的部分为半径较大的圆环形状，机械手自动抓捕对接环时，要求在整个接近过程中在线测量出对接环相对于机械手末端工具坐标系间的相对位置和姿态。简单地说，此时待测量参数为圆环目标坐标系和机械手工具坐标系间的相对位置和相对姿态角，分别用三个位置分量(t_x,t_y,t_z)和三个角度分量(a_x,a_y,a_z)来表示。由于圆环目标的特殊性，绕圆环平面法向量方向的旋转对手眼相机采集到的图像没有任何影响，因此，只能求出2个旋转角，即可以得到5个位姿分量。假设圆环坐标系的Z轴与圆环平面垂直，即沿圆环法向量方向，则可求的姿态角为(a_x,a_y)。

由于机械手对目标卫星的抓捕是在舱外进行，空间光照条件复杂且变化较快；在机械手接近过程中，待抓捕目标处于不断运动中，要求机械手能够快速反应才能实现对目标的稳定跟踪，也就要求测量速度快；空间处理器受环境和重量、体积、功耗的限制，处理能力较低，只有降低数据处理的复杂度才能提高测量速度；一旦跟踪测量失败，测量系统重新搜索目标时间较长，会导致目标偏离相机视场，以至于无法找到目标，因此，对测量的可靠性要求极高。空间机械手自动抓捕的整个需求即在复杂光照条件下，对大尺寸圆环目标的可靠、快速、高精度的近距离在线位置和姿态测量。

圆环目标不是为视觉测量专门设计的观察目标，属于非合作目标，对非合作目标的相对位姿测量，典型的方法有双目立体视觉和结构光两种。双目立体视觉在圆环边缘能够准确提取且匹配正确的条件下，测量精度要高于结构光；但其工作的前提是两个相机要有公共视场，需要避免相机视场被遮挡；且在光照复杂时，容易受光照影响导致圆环边缘提取困难，鲁棒性差；另外图像处理和特征匹配的工作量大，测量速度较慢。结构光测量通过加装窄带滤光片的方式可以减弱复杂光照的影响，鲁棒性较高；因为只提取直线特征，不需要左右特征匹配，数据处理量小，测量速度较快；且在一定程度上可以避免相机视场遮挡；但由于可用于位姿求解的特征点数量较少，整体测量精度不如双目立体视觉。因此，结构光测量在光照复杂、对测量可靠性和速度要求较高的场合显示出绝对的优势。

面向空间机械手对圆环目标的自动抓捕，美国的RSGS计划采用基于双目立体视觉的位姿测量方案，通过增加一个相机构成了三相机冗余设计，已经进行了地面演示实验，但受光照影响大，鲁棒性相对差些；欧洲航天局(ESA)的e.Deorbit计划中，加拿大的MDA公司提出了一种结构光测量方案，两套结构光测量相机安装在机械手手爪之间，两相机间有公共视场，可以形成多种组合，但要求两手爪间有足够的空间。

发明内容

为解决复杂光照条件下视觉位姿测量的条件适应性、计算速度和可靠性问题，在保证可靠性的基础上提高测量精度，本发明面向空间机械手对大尺度圆环目标自动抓捕的需求，提出一种双侧多线结构光测量方案，充分利用结构光的特点克服复杂光照条件对相机成像的影响，借助多种约束和验证提高测量算法的可靠性，通过非共视场的两组传感器数据融合提高测量精度。

本发明为实现上述目的所采用的技术方案是：面向机械手自动抓捕的大尺寸圆环目标结构光测量方法，通过在机械手末端设置多线结构光测量传感器，实现机械手工具坐标系与目标坐标系间相对位置和姿态的测量，包括以下步骤：

1)在机械手末端安装2组多线结构光测量传感器，每组多线结构光测量传感器包括一个相机和N个激光发射器，其中N≥3；

2)将激光分别投射到圆环目标前表面上，用相机采集带有激光条的圆环目标图像，在圆环目标图像中识别出候选目标激光条组合；根据相对位姿求解需要，在候选目标激光条上选择并提取出点线结合的目标图像特征；

3)对每组候选激光光条组合，根据目标图像特征采用基于中间变换的位姿求解方法得到机械手工具坐标系和目标坐标系间相对位置和姿态。

所述一个相机的光轴和N个激光发射器的各光轴在同一平面上，且激光发射器发出的线激光的发散角大于等于相机在该方向的视场角。

所述多线结构光测量传感器为两组，在机械手末端两侧对称布置；每组多线结构光测量传感器的相机非共视场，且依据圆环的大小向圆环方向倾斜成设定的角度，保证当机械手握住圆环时，圆环上的激光线在相机视场中央。

所述在圆环目标图像中识别出候选目标激光条组合采用基于多重约束的目标识别方法，包括以下步骤：

1)确定搜索窗口，依据激光条与背景的差异，在搜索窗口内进行图像分割，找到所有激光条的直线形连通区域作为初始激光条候选区域；

2)在初始激光条候选区域中排除不满足单个激光条约束的区域得到激光条候选区域；

3)根据圆环目标三维模型以及测量过程中相对位姿的变化范围，建立各激光条在图像上的相互关系约束，利用这些约束在激光条候选区域中找到满足目标模型约束的激光条图像区域组合作为候选激光条组合。

所述点线结合的目标图像特征包括：

1)投影到圆环前表面的激光条在圆环前表面形成的直线段，用直线段两个端点的图像坐标表示；

2)投影到圆环前表面的激光条与圆环内、外圆的两个交点，分别用交点的图像坐标表示。

所述基于中间变换的位姿求解方法即根据目标图像特征，借助中间坐标系求解机械手工具坐标系和目标坐标系间相对位置和姿态，包括以下步骤：

1)利用结构光原理求出每条激光条上2个交点和1条直线段上所有点在机械手工具坐标系下的三维空间坐标；

2)利用每组多线结构光测量传感器上检测出的M_i条激光条在机械手工具坐标系下的三维空间坐标拟合圆环前表面的平面方程，得到圆环在机械手工具坐标系下的法向量；其中，i表示多线结构光测量传感器的标号，取值为1、2；

3)利用圆环法向量建立中间坐标系，该中间坐标系由机械手工具坐标系旋转得到，其Z轴指向与圆环法向量相同；通过中间坐标系将激光条上2个交点投影到圆环平面上，并在圆环平面上求解圆环圆心坐标，再转换到机械手工具坐标系下；

4)根据圆环圆心在机械手工具坐标系下的三维坐标和圆环平面在机械手工具坐标系下的法向量求解机械手工具坐标系与圆环目标坐标系间的相对位置和姿态。

利用冗余信息优化得到机械手工具坐标系和目标坐标系间相对位置和姿态，具体为根据需要选择简单融合和优化两种方式：

简单融合：

圆环前表面的平面方程拟合过程中，在排除不需要的候选激光条组合后，对满足平面约束的激光条组合，用所有激光条的数据来拟合圆环平面，并通过多次迭代拟合的方式去除直线上误差超过阈值的特征点；

在基于中间变换的位姿求解方法的圆心坐标求解过程中，对满足共圆约束的激光条组合中的所有激光条上内外圆交点坐标，去掉误差超过阈值的特征点后，用多组组合求得的圆心坐标的均值作为输出的圆心坐标；

优化：

(1)首先根据简单融合得到圆环平面方程和圆环圆心坐标的初始值；

(2)以圆环平面方程和圆环圆心坐标作为输入，反求出各特征点图像坐标的计算值，并将各点计算值分别与图像坐标的检测值相比较：当各点误差平方和小于给定阈值时，退出迭代过程；否则，重新调整圆环平面方程和圆环圆心坐标，返回步骤(2)，直到满足阈值约束或迭代次数超过给定阈值后，结束迭代，用当前的圆环法向量和圆环圆心坐标计算相对位姿。

本发明具有以下优点及有益效果：

1)采用双侧多线结构光实现大尺寸圆环目标的位姿测量，具有可靠性高、对光照环境适应性强、测量速度快的特点，当基线足够长时，可以达到很高的测量精度。

2)结构光测量通过在相机上加装窄带滤光片的方式，滤除背景干扰，突出激光条的特征，可以克服复杂光照条件对目标特征成像的影响，既提高位姿测量的鲁棒性又降低了目标识别的复杂度提高了测量速度。

3)单侧激光线数目大于等于3，保证了无论圆环半径已知还是未知，单侧结构光均可实现位姿测量，扩大了适用范围；在已知圆环半径的前提下，单侧结构光测量可允许至少1条激光线检测失败，提高测量鲁棒性；由于采用非共视场的对称布置结构，双侧结构光数据融合可覆盖圆环上更大的区域，大幅度提高位姿测量精度。

4)多条激光线增加了信息冗余，提供了更多的约束条件，在目标识别和位姿解算环节通过多种约束与验证提高了测量算法的可靠性。

附图说明

图1为测量系统组成示意图；

图2为测量传感器安装位置示意图；

图3为单侧结构光测量传感器内部结构示意图；

图4为圆环与激光线关系示意图；

图5为机械手工具坐标系和圆环抓捕坐标系间的关系示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

面向机械手自动抓捕的大尺寸圆环目标结构光测量方法，通过安装在机械手末端的特殊设计的手眼视觉传感器，可以在复杂光照条件下实现机械手工具坐标系与抓捕目标坐标系间相对位置和姿态的可靠和高精度测量，包括以下步骤：

1)安装在机械手末端的手眼视觉传感器由两组特殊设计的多线结构光传感器组成，每组多线结构光传感器均可独立实现位姿测量，两组数据融合可提高测量精度。

2)将激光分别投射到圆环目标前表面上，用相机采集带有激光光条的圆环目标图像，通过基于多重约束的目标识别方法在图像中识别出候选激光条组合；根据相对位姿求解需要，在目标激光条上选择并提取出点线结合的目标图像特征。

3)对每组候选激光条组合，根据目标图像特征采用基于中间变换的位姿求解方法计算机械手工具坐标系和目标坐标系间相对位置和姿态；利用冗余信息提高可靠性和测量精度。

因此，面向机械手自动抓捕的大尺寸圆环目标结构光测量方法的实现可以概括为以下三个关键步骤：测量系统设计、基于多重约束的目标识别和基于中间变换和冗余信息融合的位姿求解。下面分别进行详细描述。

1.测量系统的设计

面向机械手自动抓捕的大尺寸圆环目标结构光测量方法采用将2组结构光测量传感器对称安装在机械手末端实现相对位姿测量，测量系统组成包括测量传感器、处理器和测量软件三部分。测量传感器分为2组，分别安装在机械手的两侧。每组测量传感器都由一个面阵相机和N个线激光发射器组成，其中N≥3。每个激光发射器发射出的线激光在空间构成一个光平面，投影到圆环上形成一个激光条，相机采集带有激光条的圆环图像，并送到处理器中。在处理器上运行的测量软件处理相机采集的图像，通过测量算法识别出激光条，并完成相对位姿的求解。

实际工程应用中，测量系统的设计包括相机参数的设计、激光参数的设计、相机与激光间相互位置关系的设计以及处理器的设计，这些参数的设计取决于实际测量任务的需求，其中最关键的是相机与激光间位置关系的设计。

这里提供一种最小激光数目配置N＝3的设计方案，图1给出了该测量系统的组成框图。该方案中，测量传感器安装在机械手的快换装置上，即相对于机械手操作工具来说靠后安装，以便机械手更换不同的操作工具时，均不影响测量传感器。为避免操作工具对相机视场的遮挡，采用激光在内、相机在外的布置方式，激光向相机侧倾斜安装。结构光测量时，必须保证在整个测量过程中，激光条始终能够投影到圆环目标上，且能够在相应的相机中成像。因此，每个相机和激光构成的结构光测量传感器需要配合圆环的弧度进行倾斜安装。图2给出了测量传感器安装位置示意图，测量系统设计时，需要设计图中的相机和激光器中心连线向圆环方向的倾角γ和机械手快换装置轴心与结构光传感器安装面间的距离d0。图3给出了单侧结构光测量传感器内部结构示意图，每个测量传感器由一个相机和3个线激光发射器组成，相机和激光发射器的主轴在同一平面上，激光发射器向相机方向倾斜，且激光发射器形成的光平面可以覆盖整个相机视场，即激光的发散角要大于等于相机在该方向的视场角。这里，需要根据任务要求的测量范围、测量精度来设计图中的d₁，d₂，d₃以及β₁，β₂和β₃。其中，d₁为第一和第二个激光器出光口之间的距离，d₂为第二和第三个激光器出光口之间的距离，d₃为第三个激光器出光口与相机光轴间的距离；β₁，β₂和β₃分别为三个激光器发出的激光光平面向相机方向的倾角。理论上d₁+d₂和d₃越大，测量精度越高，在三个激光间的距离上，一般采用等距离分布。β₃越大，测量的最远距离越小；β₁越大，测量的最近距离越小，β₂一般取β₁和β₃的均值。这些参数是相互制约的，设计时应该首先确定测量范围，根据测量范围设计相机视场角，再进一步确定其它参数，所有参数确定后，可以通过仿真来估算测量精度。

在硬件实现上，为减弱复杂光照条件对激光条成像质量的影响，需要在相机镜头前加装窄带滤光片，只允许与发射的线激光频段相近的光线进入相机镜头。这样可以突出激光条的亮度，降低背景的亮度，从而使目标特征更加突出。2.基于多重约束的目标识别

基于结构光的圆环位姿测量以投影到圆环上的激光条作为目标特征，圆环目标一般主要由其内外圆构成的圆环平面组成，虽然在圆环平面上可能有一些凹槽或凸起，但其在整个圆环上所占比例较小。因此，每条线激光投影到圆环平面上，都会形成一条近似的直线段，由于三个线激光发射器间的相对位置固定，3条直线段间必然会满足一定的约束条件。由于激光条相对于背景更亮，因此在黑白相机中的成像为白色、具有一定长度和宽度的连续区域。目标识别的目的即在手眼视觉相机采集的图像中，利用多种约束条件识别出图像中的目标激光条区域。基于多重约束的目标识别的主要步骤为：

1)根据先验知识确定搜索窗口，在第一次识别时，没有任何先验知识，搜索窗口为整幅图像；一旦测量成功后，就可以根据已知的目标与机械手间相对运动速度，在上一幅图像的目标位置附近确定一个小的跟踪搜索窗口，后续的所有图像处理均在该窗口内进行。根据3条激光条在图像上的相互距离，可以为每个激光条确定一个跟踪搜索窗口，也可以所有激光条共用一个搜索窗口。

2)依据图像上激光条与背景的差异，在搜索窗口内进行图像分割，找到所有可能是激光条的直线形连通区域作为初始激光条候选区域。

3)根据测量系统的实际特点，建立单个激光条的约束，在初始激光条候选区域中排除不满足单个激光条约束的区域得到精选激光条候选区域。这里，可能的约束条件包括：区域的长度、宽度、长宽比、斜率等。

4)根据圆环目标三维模型以及测量过程中相对位姿的变化范围，建立各激光条在图像上的相互关系约束，利用这些约束在精选激光条候选区域中找到满足目标模型约束的激光条图像区域组合作为候选激光条组合。这里，相对位姿包括位置向量(t_x,t_y,t_z)，即圆环抓捕坐标系原点在机械手工具坐标系下的三维坐标，以及由旋转矩阵R分解出的绕三个坐标轴的旋转角(a_x,a_y,a_z)；可能的约束条件包括：直线区域间长度差异、灰度差异、边界点位置差异、距离等。

5)点线结合的目标特征选择与提取

目标识别的结果得到了候选激光条组合，为计算相对位姿，需要提取出目标直线的图像特征。根据圆环目标特点和位姿求解的需要，选择的图像特征如下：

1)投影到圆环前表面的激光条在圆环前表面形成的直线段，用直线段两个端点的图像坐标表示；

2)投影到圆环前表面的激光条与圆环内、外圆的两个交点，分别用交点的图像坐标表示。

由于圆环结构的不同，上述特征中部分特征可能重合，最少需要2个点，最多需要4个点来表示。例如，当圆环前表面为同一平面时，投影到圆环前表面的每个激光条在圆环前表面形成一条直线段，其端点为该直线与圆环内外圆的交点。此时，只需两个交点坐标即可同时表示点特征和直线特征。而当圆环前表面只有中间是平面，靠近内外圆的部分有凹槽或凸起时，就需要4个点来表示，2个点用于表示直线，2个点用于表示激光线与内外圆的交点。当激光前表面所在平面与内圆或外圆相连时，只需3个点即可同时表示线特征和点特征。图4给出了圆环前表面靠近内圆部分(最内的浅色环面)为平面，靠近外圆部分有凸起和凹槽的情况。此时，AkBk为直线段，Ak和Ck分别为激光线与内外圆的交点。k为1～3。每条激光线需要3个点来表示。

在目标特征提取时，为提高测量精度，需要对得到的激光条的中心线进行直线拟合，且拟合可进行多次，以便排除干扰点。当内外圆交点在特征直线上时，需要将检测出的交点再投影到拟合的直线上，以减小误差。

3.基于中间变换和冗余信息融合的位姿求解

4.1坐标系与位姿参数定义

位姿求解即根据激光条的图像特征和模型特征，求解圆环坐标系与机械手工具坐标系间的相对位置和姿态。要求解相对位姿，首先应该定义坐标系和位姿参数。这里，涉及的坐标系包括图像坐标系、机械手工具坐标系和圆环抓捕坐标系，分别定义如下：

1)图像坐标系O_i

图像坐标系O_i的原点定义在图像的左上角，向右为u轴正向，向下为v轴正向。

2)机械手工具坐标系O_t

机械手工具坐标系O_t即手眼相机测量坐标系，其坐标系原点定义在抓捕手爪正中心与待抓捕目标上表面接触的位置。手爪前方为Z轴正向，手爪张开方向为Y轴，握紧待抓捕目标时，X轴正向指向圆环圆心，Y轴满足右手定则。

3)圆环抓捕坐标系O_m

圆环抓捕坐标系O_m为圆环的局部坐标系，该坐标系原点定义在圆环上表面(如圆环平放在地上，与地接触的环面为下表面，另一侧为上表面)位于内外圆中间的圆周上(圆环上表面中心圆)，为机械手工具坐标系原点在圆环上表面平面上的投影与圆环圆心连线与该中心圆的交点，其Z轴与圆环中心坐标系Z轴平行且指向相同，X轴指向圆环圆心，Y轴与圆相切。

机械手工具坐标系和圆环抓捕坐标系间的关系如图5所示，其中Oo为圆环圆心。圆环抓捕坐标系与机械手工具坐标系间的相对位置和姿态关系定义为：机械手工具坐标系先平移，再绕三个坐标轴旋转后与圆环抓捕坐标系重合。用公式表示为：

P_t＝RP_m+T (1)

其中，P_t为空间一点在机械手工具坐标系下的坐标，P_m为该点在圆环抓捕坐标系下的坐标，T为所求的位置向量，包括(t_x,t_y,t_z)，即圆环抓捕坐标系原点在机械手工具坐标系下的三维坐标。R为旋转矩阵，可以分解为绕三个坐标轴的旋转角(a_x,a_y,a_z)，为后续计算方便，设旋转顺序为x,y,z。

3.2相对位姿态求解过程

对于圆环目标来说，只要求出圆环圆心和法向量在机械手工具坐标系下的坐标，就可以推出圆环抓捕坐标系与机械手工具坐标系间的相对位置和姿态，因此，相对位姿求解过程包括以下步骤：

1)求解特征直线和特征点在机械手工具坐标系下的三维坐标

结构光测量系统经过标定后，可以得到相机的内参数(包括畸变系数)、外参数和C矩阵，以及各激光平面的平面方程。设图像上光条成像点的图像坐标经过畸变校正后为(u_ij,v_ij)，该点在机械手工具坐标系下的三维坐标为(x_tij，y_tij，z_tij)，其中，i表示第i条激光线，j表示第j个图像点，则根据相机的针孔成像模型有：

t为比例系数；又，该点一定在第i个光平面上，即满足：

A_ix_tij+B_iy_tij+C_iZ_tij＝D_i (3)

其中，A_i、B_i、C_i、D_i为光平面方程系数。由式2、3联立可得下面的线性方程组，求解该方程组即可得到光条上各点在机械手工具坐标系下的三维空间坐标(x_tij，y_tij，z_tij)：

式中，c_ij为相机C矩阵中第i行第j列的参数值。

根据上式可以求解出图4中9个图像特征点A1～A3、B1～B3、C1～C3在机械手工具坐标系下的三维坐标，并计算出线段A1B1、A2B2和A3B3上各点在机械手工具坐标系下的三维坐标。

2)求解圆环法向量在机械手工具坐标系下的表示

线段A1B1、A2B2和A3B3都在圆环平面上，前面已经求解出了线段上各点在机械手工具坐标系下的三维坐标，因此，可以用线性最小二乘法拟合出圆环平面在机械手工具坐标系下的平面方程。

A_tx_tij+B_ty_tij+C_tz_tij＝D_t (5)

其中，A_t、B_t、C_t、D_t为平面方程系数。据此，可得到该平面在机械手末端工具坐标系下的法向量(l,m,n)：

3)求解圆环圆心坐标在机械手工具坐标系下的表示

定义一个中间坐标系O_t′,该坐标系与机械手工具坐标系原点相同，只经过旋转，使得坐标系的Z坐标轴与圆环平面的法向量重合。其变换关系为：

其中，x_t、y_t、z_t为空间点在机械手工具坐标系下的三维坐标，(x_t′,y_t′,z_t′)为空间点在中间坐标系下的三维坐标。根据上式，可以得到A1～A3、C1～C3共6个特征点在中间坐标系下的坐标：(x′_Ai,y′_Ai,z′_Ai),(x′_Ci,y′_Ci,z′_Ci)，这些坐标中有：z′_A1＝z′_A2＝z′_A3，z′_C1＝z′_C2＝z′_C3。此时，只需要用各点的X，Y坐标求出圆心坐标即可。

当圆半径已知时，在A1～A3中任取两点Ak，Ah，由于这两点在半径为r1的圆上，圆心一定在Ak，Ah连线的垂直平分线上，根据Ak，Ah可以得到圆心坐标的两个解；又Ck，Ch在半径为r2的圆上，圆心一定在Ck、Ch连线的垂直平分线上，根据Ck、Ch又可以得到圆心坐标的两个解，共可得到4个解，根据各解间的距离可以确定正确解。其中，k、h为1～3。

当圆半径未知时，同一个圆上任意一点可以得到一个关于圆心和半径的圆方程，两个圆方程相减可以得到一个线性方程，通过最小二乘法可以求解出圆心在中间坐标系下的X，Y坐标。

由于圆平面上各点的Z坐标相同，可以根据圆上各点计算出圆心在中间坐标系下的Z坐标。最后，利用7式可求出圆心在机械手工具坐标系下的三维坐标。

4)相对位置和姿态计算

由于圆环抓捕坐标系的XY平面与圆环平面相同，前面求出的旋转矩阵R2代表了机械手工具坐标系与圆环抓捕坐标坐标系间绕X、Y轴的旋转，由该旋转矩阵可以分解出3个旋转角，其中，绕Z轴的旋转角是无意义的，仅取绕X、Y轴的旋转角。

为求解机械手末端工具坐标系与圆环抓捕坐标系间的相对位置和相对姿态，需要求出圆心与机械手工具坐标系原点在圆环平面上投影连线与圆环中心圆的交点坐标，并根据圆环圆平面在参考坐标系下的Z坐标，得到圆环抓捕坐标系原点在参考坐标系下的三维坐标，并进一步换算成在机械手工具坐标系下的三维坐标，该坐标值即圆环抓捕坐标系与机械手工具坐标系间的相对位置。机械手工具坐标系与圆环抓捕坐标系间的相对姿态角中，绕X，Y轴的姿态角前面已经求出，需要再求出绕Z轴的姿态角。由于圆环抓捕坐标系的X轴指向圆环圆心，因此绕Z轴的旋转即将向量：(x_o-x_m,y_o-y_m)构成的单位向量(l_x,l_y)变换成向量(1，0)，据此可以求出绕Z轴的旋转矩阵并进而得到旋转角。

冗余信息验证与数据融合：

为求解相对位姿，理论上，当圆环半径已知时，最少需要2条激光线，当圆环半径未知时，至少需要3条激光线。因此，当圆环半径已知时，单侧测量传感器即存在信息冗余，双侧测量传感器提供了更多的信息冗余；当圆环半径未知时，单侧测量传感器没有信息冗余，但双侧测量传感器会增加3条激光线的冗余。这些冗余信息，一方面可以用来验证所找到的激光条组合的正确性，另一方面，可用于提高测量精度。

信息验证体现在2个环节，首先在圆环平面拟合环节，理论上，两条激光线即可拟合出一个平面，如果其余激光线也在这个平面上，则其余激光线上各点到这个平面的距离一定小于设定的阈值，如果大于该阈值，则意味着该候选激光线组合无法满足激光线共面的约束，不是真正的目标激光线组合。第二个环节为圆环圆心计算环节。在依据最小激光线数求出圆心坐标后(如果圆环半径未知同时可求出圆环半径)，其余激光线上的特征点到圆环圆心的距离与圆环半径差的绝对值一定在给定的范围内，如果超出，则意味着该候选激光线组合的特征点无法满足共圆的约束，不是真正的目标激光线组合。因此，通过这两个环节，可以利用冗余信息排除无效的候选激光条组合。

数据融合可以有多种方式。如果对测量速度要求高，可以采用简单的数据融合在保证测量速度的前提下提高测量精度；否则，可以采用复杂优化的方式，以牺牲处理速度换取测量精度。

简单的数据融合表现在圆环平面拟合和圆心坐标计算两个环节。在圆环平面拟合环节，可以在排除无效的候选激光条组合后，对满足平面约束的激光条组合，用所有激光条的数据来拟合圆环平面，并通过多次迭代拟合的方式去除直线上误差超过阈值的特征点。在圆心坐标求解环节，对满足共圆约束的激光条组合中的所有激光条上内外圆交点坐标，去掉误差超过阈值的特征点后，用多组组合求得的圆心坐标的均值作为输出的圆心坐标。

优化的方法首先根据简单融合方法得到圆环平面方程和圆环圆心坐标的初始值，以圆环平面方程和圆环圆心坐标作为输入，反求出各特征点图像坐标的计算值，并与图像坐标的检测值相比较，当各点误差平方和小于给定阈值时，退出迭代过程；否则，重新调整圆环平面方程和圆环圆心坐标，重复前面的过程，直到满足阈值约束或迭代次数超过给定阈值后，结束迭代，用当前的圆环法向量和圆环圆心坐标计算相对位姿。

以上结合附图对本发明的具体实施方式作了说明，但这些说明不能被理解为限制了本发明的范围，本发明的保护范围由随附的权利要求书限定，任何在本发明权利要求基础上的改动都是本发明的保护范围。

Claims (7)

1.面向机械手自动抓捕的大尺寸圆环目标结构光测量方法，其特征在于，通过在机械手末端设置多线结构光测量传感器，实现机械手工具坐标系与目标坐标系间相对位置和姿态的测量，包括以下步骤：

1)在机械手末端安装2组多线结构光测量传感器，每组多线结构光测量传感器包括一个相机和N个激光发射器，其中N≥3；

2)将激光分别投射到圆环目标前表面上，用相机采集带有激光条的圆环目标图像，在圆环目标图像中识别出候选目标激光条组合；根据相对位姿求解需要，在候选目标激光条上选择并提取出点线结合的目标图像特征；

3)对每组候选激光光条组合，根据目标图像特征采用基于中间变换的位姿求解方法得到机械手工具坐标系和目标坐标系间相对位置和姿态。

2.根据权利要求1的面向机械手自动抓捕的大尺寸圆环目标结构光测量方法，其特征在于，所述一个相机的光轴和N个激光发射器的各光轴在同一平面上，且激光发射器发出的线激光的发散角大于等于相机在该方向的视场角。

3.根据权利要求1的面向机械手自动抓捕的大尺寸圆环目标结构光测量方法，其特征在于，所述多线结构光测量传感器为两组，在机械手末端两侧对称布置；每组多线结构光测量传感器的相机非共视场，且依据圆环的大小向圆环方向倾斜成设定的角度，保证当机械手握住圆环时，圆环上的激光线在相机视场中央。

4.根据权利要求1的面向机械手自动抓捕的大尺寸圆环目标结构光测量方法，其特征在于，所述在圆环目标图像中识别出候选目标激光条组合采用基于多重约束的目标识别方法，包括以下步骤：

1)确定搜索窗口，依据激光条与背景的差异，在搜索窗口内进行图像分割，找到所有激光条的直线形连通区域作为初始激光条候选区域；

2)在初始激光条候选区域中排除不满足单个激光条约束的区域得到激光条候选区域；

3)根据圆环目标三维模型以及测量过程中相对位姿的变化范围，建立各激光条在图像上的相互关系约束，利用这些约束在激光条候选区域中找到满足目标模型约束的激光条图像区域组合作为候选激光条组合。

5.根据权利要求1的面向机械手自动抓捕的大尺寸圆环目标结构光测量方法，其特征在于，所述点线结合的目标图像特征包括：

1)投影到圆环前表面的激光条在圆环前表面形成的直线段，用直线段两个端点的图像坐标表示；

2)投影到圆环前表面的激光条与圆环内、外圆的两个交点，分别用交点的图像坐标表示。

6.根据权利要求5的面向机械手自动抓捕的大尺寸圆环目标结构光测量方法，其特征于，所述基于中间变换的位姿求解方法即根据目标图像特征，借助中间坐标系求解机械手工具坐标系和目标坐标系间相对位置和姿态，包括以下步骤：

1)利用结构光原理求出每条激光条上2个交点和1条直线段上所有点在机械手工具坐标系下的三维空间坐标；

2)利用每组多线结构光测量传感器上检测出的M_i条激光条在机械手工具坐标系下的三维空间坐标拟合圆环前表面的平面方程，得到圆环在机械手工具坐标系下的法向量；其中，i表示多线结构光测量传感器的标号，取值为1、2；

3)利用圆环法向量建立中间坐标系，该中间坐标系由机械手工具坐标系旋转得到，其Z轴指向与圆环法向量相同；通过中间坐标系将激光条上2个交点投影到圆环平面上，并在圆环平面上求解圆环圆心坐标，再转换到机械手工具坐标系下；

4)根据圆环圆心在机械手工具坐标系下的三维坐标和圆环平面在机械手工具坐标系下的法向量求解机械手工具坐标系与圆环目标坐标系间的相对位置和姿态。

7.根据权利要求1的面向机械手自动抓捕的大尺寸圆环目标结构光测量方法，其特征在于，利用冗余信息优化得到机械手工具坐标系和目标坐标系间相对位置和姿态，具体为根据需要选择简单融合和优化两种方式：

简单融合：

圆环前表面的平面方程拟合过程中，在排除不需要的候选激光条组合后，对满足平面约束的激光条组合，用所有激光条的数据来拟合圆环平面，并通过多次迭代拟合的方式去除直线上误差超过阈值的特征点；

在基于中间变换的位姿求解方法的圆心坐标求解过程中，对满足共圆约束的激光条组合中的所有激光条上内外圆交点坐标，去掉误差超过阈值的特征点后，用多组组合求得的圆心坐标的均值作为输出的圆心坐标；

优化：

(1)首先根据简单融合得到圆环平面方程和圆环圆心坐标的初始值；

(2)以圆环平面方程和圆环圆心坐标作为输入，反求出各特征点图像坐标的计算值，并将各点计算值分别与图像坐标的检测值相比较：当各点误差平方和小于给定阈值时，退出迭代过程；否则，重新调整圆环平面方程和圆环圆心坐标，返回步骤(2)，直到满足阈值约束或迭代次数超过给定阈值后，结束迭代，用当前的圆环法向量和圆环圆心坐标计算相对位姿。

Images