基站式六自由度对接位姿检测方法与流程

本发明涉及一种基站式六自由度对接位姿检测方法，属于对接装校领域，尤其是涉及一种基于机器视觉的基站式六自由度对接位姿检测方法。

背景技术：

机器视觉技术是近20年发展的新兴的测量技术，是当下热门的研究领域，可以实现非接触的高精度的测量，普遍应用于-需要准确三维信息的领域，在工业机器人、移动机器人和人形机器人中都得到广泛的应用。位姿参数是判断物体在空间中处于何种状态的一个重要参数，包括位置参数和姿态参数，测量对接刚体和标准安装位的位姿偏差可以对对接过程进行量化的指导。本专利描述的对接刚体装校在托板上通过丝杠导轨提升，对接刚体在箱体内沿丝杠导轨提升的过程中由于倾翻力矩使得对接刚体相对于丝杠导轨的位姿会存在微小的偏差，对接刚体与箱体的相对位置并不是一直不变的，所以需要一组机器视觉得到对接刚体的实时的六自由度位姿，同时，考虑到地平面与标准安装位之间不是绝对平行，所以需要另一组机器视觉得到标准安装位的实时的六自由度位姿，再把对接刚体和标准安装位之间的位姿转换到同一坐标系下表示，一次得到对接刚体与标准安装位之间的位姿偏差。因此，本发明提供一种基站式六自由度对接位姿检测方法，以解决上述问题。

技术实现要素：

本发明的目的是提供一种能够保证对接精确度和可靠性的基站式六自由度对接位姿检测方法。

为了达到上述目的，本发明提供的该基站式六自由度对接位姿检测方法，提供的两组机器视觉系统的坐标基准是动态基准，再将两组的数据联立求解得到刚体与标准安装位的位置偏差系统，包括位置偏差参数X、Y、Z、α、β和γ。具体地说，该基站式六自由度对接位姿检测方法包括如下步骤：

S1、将相机分别安装在支撑平台上，对相机进行标定，得到相机的内参数、外参数以及组内相机相对位姿参数。

S2、将G组的色标块平整地粘贴在标准安装位旁，D组的色标块平整地粘贴在对接刚体上，标定G组色标块其中一顶点与标准安装位中心M的几何关系，标定D组色标块其中一顶点与对接刚体中心N的几何关系。

S3、安装相机支撑平台，使用水平仪保持水平，一组相机形成D组相机以拍摄D组色标块的图像，另一组相机形成G组相机以拍摄G组色标块的图像。

S4、标定G组相机与D组相机的坐标系的转换关系。

S5.、分别对G组相机和D组得到的图像进行预处理和角点提取，得到G组色标块的图像和D组色标块的顶点的图像坐标。

S6.、将G组色标块的顶点的图像和D组色标块的顶点的图像坐标，通过双目机器视觉算法解算为相机坐标系下的坐标。

S7、令D组相机坐标系为该检测系统的世界坐标系，即定位该检测系统的基站坐标系。G组相机得到G组色标块的图像的各个顶点的坐标，其中一顶点坐标与标准安装位中心M已标定，即可得到标准安装位中心M在G组相机坐标系下的位置参数x1、y1、z1；同理可得到，对接刚体的几何中心N在D组相机坐标系下的位置参数x2、y2、z2；将G组下的x1、y1、z1转换到基站坐标系表示，令为x1′、y1′、z1′，则标准安装位与对接刚体的位置参数偏差为x2－x1′、z2－z1′。

S8、将得到的G组色标块的图像的四个顶点坐标装换到用基站坐标系表示，四个顶点可拟合出表征标准安装位平面的平面方程，从而得到标准安装位平面的法向量，同理，D组相机得到对接刚体平面的法向量，两个法向量在YZ平面、XZ平面、XY平面投影的夹角为姿态参数偏差α、β、γ。

进一步地，在S4中，所得到的G组相机的坐标系与D组相机的坐标系的转换关系包括平移矩阵和旋转矩阵。

进一步地，在S5中，对图像预处理包括边缘检测，检测出若干矩形，对最内侧的矩形进行重绘，然后在进行角点提取，得到顶点的图像坐标。

进一步地，在S6中，将G组相机的两个相机分别得到图像坐标按顺序匹配带入双目机器视觉的解算公式，得到G组相机坐标系下的坐标；D组相机的数据一样的方法处理。

进一步地，在S8中，在YZ平面的投影夹角为应调整的绕Z轴的旋转角α，在XZ平面的投影夹角为应调整的绕X轴的旋转角β，在XY平面的投影夹角为应调整的绕Y轴的旋转角γ。

本发明还提供一种基站式六自由度对接位姿检测方法，其包括步骤：

步骤1，通过一组G组相机拍摄粘贴在一个标准安装位旁的一个G组色标块，以获得该G组色标块的图像；

步骤2，通过一组D组相机拍摄粘贴在一个对接刚体旁的一个D组色标块，以获得该D组色标块的图像；

步骤3，标定该G组相机和该D组相机的转换关系；

步骤4，分别对该G组色标块的图像和该D组色标块的图像进行预处理和角点提取，以得到该G组色标块的图像和该D组色标块的图像的坐标；

步骤5，将该G组色标块的图像和该D组色标块的图像的顶点的图像坐标，通过双目机器视觉算法解算为一个相机坐标系下的坐标；

步骤6，令该D组相机的坐标系为检测系统的基站坐标系；

步骤7，将通过该G组相机得到的该G组色标块的各个顶点的坐标转换到用该基站坐标系表示，该G组色标块的四个顶点可拟合出表征该标准安装位平面的平面方程，从而得到该标准安装位的法向量，将通过该D组相机得到的该D组色标块的各个顶点的坐标转换到用该基站坐标系表示，该D组色标块的四个顶点可拟合出表征该对接刚体平面的平面方程，从而得到该对接刚体的法向量，进而检测到该对接刚体的位姿偏差。

作为对本发明的该基站式六自由度对接位姿检测方法的进一步优选的实施例，该步骤2在该步骤1之前，从而先通过该D组相机拍摄粘贴在该对接刚体旁的该D组色标块的图像，然后再通过该G组相机拍摄粘贴在该标准安装位旁的该G组色标块的图像。

作为对本发明的该基站式六自由度对接位姿检测方法的进一步优选的实施例，在该步骤1之前还包括：

将该D组相机和该G组相机分别安装在一个支撑平台上；

分别对该D组相机和该G组相机的内参数、外参数以及D组和G组的组内的每个相机的相对位姿参数进行标定；以及

标定该G组色标块的一个顶点与该标准安装位的中心的几何关系和标定该D组色标块的一个顶点与该对接刚体的中心的几何关系。

作为对本发明的该基站式六自由度对接位姿检测方法的进一步优选的实施例，在该步骤3中，该G组相机和该D组相机的转换关系包括平移矩阵和旋转矩阵。

作为对本发明的该基站式六自由度对接位姿检测方法的进一步优选的实施例，在该步骤4中，对该G组色标块的图像和该D组色标块的图像的预处理包括边缘检测，在检测出若干矩形后，对最内侧的矩形进行重绘，然后再进行角点提取，从而得到顶点的图像坐标。

作为对本发明的该基站式六自由度对接位姿检测方法的进一步优选的实施例，在该步骤5中，将G组相机得到的该G组色标块的图像和该D组相机得到的该D组色标块的图像按顺序匹配分别带入双目视觉的解算公式，得到该G组相机的坐标系和该D组相机的坐标系。

本发明的该基站式六自由度对接位姿检测方法的有益效果是：通过图像采集、图像预处理、特征提取、角点提取、空间坐标求解以及空间位姿结算来量化对接刚体和标准安装位之间的位姿偏差系数，在于可以实现非接触高精度的位姿偏差系数测量，本发明的该基站式六自由度对接位姿检测方法提供的这种检测方式应用范围光、例如可以广泛地应用于民用领域和航空航天等领域。

附图说明

为了获得本发明的上述和其他优点和特点，以下将参照附图中所示的本发明的具体实施例对以上概述的本发明进行更具体的说明。应理解的是，这些附图仅示出了本发明的典型实施例，因此不应被视为对本发明的范围的限制，通过使用附图，将对本发明进行更具体和更详细的说明和阐述。在附图中：

图1是基站式六自由度系统的示意图。

图2是该基站式六自由度对接位姿检测方法中所使用的色标块的示意图。

图3是该基站式六自由度对接位姿检测方法中所使用的双目机器视觉法的示意图。

具体实施方式

以下描述用于揭露本发明以使本领域技术人员能够实现本发明。以下描述中的优选实施例只作为举例，本领域技术人员可以想到其他显而易见的变型。在以下描述中界定的本发明的基本原理可以应用于其他实施方案、变形方案、改进方案、等同方案以及没有背离本发明的精神和范围的其他技术方案。

如图1至图3所示，根据本发明的精神提供了一种基站式六自由度对接位姿检测方法，其包括如下步骤：

S1、将相机1分别安装在支撑平台2上，对该相机1进行标定，得到该相机1的内参数、外参数以及组内该相机1相对位姿参数。

S2、将G组色标块3平整地粘贴在标准安装位4旁，D组色标块5平整地粘贴在对接刚体6上，标定该G组色标块3其中一顶点与该标准安装位4的中心M的几何关系，标定该D组色标块5其中一顶点与该对接刚体6的中心N的几何关系。

S3、安装相机支撑平台，使用水平仪保持水平，一组该相机1拍摄该D组色标块5，另一组该相机1拍摄该G组色标块3。

S4、标定G组相机1与D组相机1坐标系的转换关系。

S5、分别对G组相机和D组相机得到的图像进行预处理和角点提取，得到该G组色标块3和该D组色标块5的顶点的图像坐标。

S6、将该G组色标块3和该D组色标块5的顶点的图像坐标，通过双目机器视觉算法解算为相机坐标系下的坐标。

S7、令D组相机坐标系为该检测系统的世界坐标系，即定位该检测系统的基站坐标系。G组相机得到G组色标块的图像各个顶点的坐标，其中一顶点坐标与标准安装位中心M已标定，即可得到该标准安装位4的中心M在G组相机坐标系下的位置参数x1、y1、z1；同理可得到，该对接刚体6的几何中心N在D组相机坐标系下的位置参数x2、y2、z2；将G组下的x1、y1、z1转换到基站坐标系表示，令为x1′、y1′、z1′,则该标准安装位4与该对接刚体6的位置参数偏差为x2－x1′、z2－z1′。

S8、将得到的G组色标块的图像的四个顶点坐标装换到用基站坐标系表示，四个顶点可拟合出表征标准安装位平面的平面方程，从而得到该标准安装位4的平面的法向量，同理，D组相机得到该对接刚体6的平面的法向量，两个法向量在YZ平面、XZ平面、XY平面投影的夹角为姿态参数偏差α、β、γ。

在步骤4中，所得到的G组相机的坐标系与D组相机的坐标系的转换关系包括平移矩阵和旋转矩阵。

在步骤5中，对图像预处理包括边缘检测，检测出若干矩形，对最内侧的矩形进行重绘，然后在进行角点提取，得到顶点的图像坐标。

在步骤6中，将G组两个相机分别得到图像坐标按顺序匹配带入双目机器视觉的解算公式，得到G组相机坐标系下的坐标；D组相机的数据一样的方法处理。

在步骤8中，在YZ平面的投影夹角为应调整的绕Z轴的旋转角α，在XZ平面的投影夹角为应调整的绕X轴的旋转角β，在XY平面的投影夹角为应调整的绕Y轴的旋转角γ。

双目机器视觉法测量空间位置参数是通过标定两摄像机之间相对位姿，根据视差法计算出特征点在摄像机坐标系中的坐标，然后对特征目标点进行三维重建，从而得到特征点的空间位置参数。

假设P为目标坐标系中任意一点，P1、P2分别为点P在两成像面上的投影点。点P1的像素坐标为(u₁，v₁)，点P2的像素坐标为(u₂，v₂)，摄像机坐标系OX_1CY_2CZ_3C与OX_2CY_2CZ_2C之间的位姿关系为

其中，Ra、Ta为已知量，由摄像机相互位姿标定得到；将式(1)转换为代数式可得

图像坐标系到像素坐标系的转换有

其中，M_in称为摄像机内参数，参数值在摄像机标定过程中确定。

联立公式(2)和(3)可得

将式(5)带入式(4)可得

根据公式(6)可求得点P的深度信息Z_2C，带入公式(5)可求得点P在摄像机坐标系OX_2CY_2CZ_2C中三维坐标参数；如果P为特征点，且P与标准安装位中心或对接刚体中心位置偏差已标定，转换到世界坐标系下表示，则可得到标准安装位中心、对接刚体相对于摄像机坐标系的空间位置偏差参数(x、y、z)。

在得到世界坐标系中不共线的3个以上点的空间位置坐标时，即可采用最小二乘法拟合出一个平面。

通过表征标准安装位的平面表达式和对接刚体的平面表达式即可得到两个平面的法向量，两个法向量转换到世界坐标系下表示。

两个法向量在YZ平面的投影夹角为应调整的绕Z轴的旋转角α，在XZ平面的投影夹角为应调整的绕X轴的旋转角β，在XY平面的投影夹角为应调整的绕Y轴的旋转角γ。

本发明的该基站式六自由度对接位姿检测方法包括如下步骤：

步骤1，通过一组G组相机拍摄粘贴在一个标准安装位旁的一个G组色标块，以获得该G组色标块的图像；

步骤2，通过一组D组相机拍摄粘贴在一个对接刚体旁的一个D组色标块，以获得该D组色标块的图像；

步骤3，标定该G组相机和该D组相机的转换关系；

步骤4，分别对该G组色标块的图像和该D组色标块的图像进行预处理和角点提取，以得到该G组色标块的图像和该D组色标块的图像的坐标；

步骤5，将该G组色标块的图像和该D组色标块的图像的顶点的图像坐标，通过双目机器视觉算法解算为一个相机坐标系下的坐标；

步骤6，令该D组相机的坐标系为检测系统的基站坐标系；

步骤7，将通过该G组相机得到的该G组色标块的各个顶点的坐标转换到用该基站坐标系表示，该G组色标块的四个顶点可拟合出表征该标准安装位平面的平面方程，从而得到该标准安装位的法向量，将通过该D组相机得到的该D组色标块的各个顶点的坐标转换到用该基站坐标系表示，该D组色标块的四个顶点可拟合出表征该对接刚体平面的平面方程，从而得到该对接刚体的法向量，进而检测到该对接刚体的位姿偏差。

优选地，该步骤2在该步骤1之前，从而先通过该D组相机拍摄粘贴在该对接刚体旁的该D组色标块的图像，然后再通过该G组相机拍摄粘贴在该标准安装位旁的该G组色标块的图像。

优选地，在该步骤1之前还包括：

将该D组相机和该G组相机分别安装在一个支撑平台上；

分别对该D组相机和该G组相机的内参数、外参数以及D组和G组的组内的每个相机的相对位姿参数进行标定；以及

标定该G组色标块的一个顶点与该标准安装位的中心的几何关系和标定该D组色标块的一个顶点与该对接刚体的中心的几何关系。

优选地，在该步骤3中，该G组相机和该D组相机的转换关系包括平移矩阵和旋转矩阵。

优选地，在该步骤4中，对该G组色标块的图像和该D组色标块的图像的预处理包括边缘检测，在检测出若干矩形后，对最内侧的矩形进行重绘，然后再进行角点提取，从而得到顶点的图像坐标。

优选地，在该步骤5中，将G组相机得到的该G组色标块的图像和该D组相机得到的该D组色标块的图像按顺序匹配分别带入双目视觉的解算公式，得到该G组相机的坐标系和该D组相机的坐标系。

以上对本发明的一个实施例进行了详细说明，但该内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明申请范围所作的均等变化与改进等，均应仍归属于本发明的专利涵盖范围之内。