[发明专利]基站式六自由度对接位姿检测方法

说明书

一种基站式六自由度对接位姿检测方法，其包括如下步骤：分别通过一组G组相机和一组D组相机得到一个G组色标块和一组D组色标块的图像，并标定该F组相机和该D组相机的转换关系，预处理该G组色标块和该D组色标块的图像和得到该G组色标块和该D组色标块的顶点的图像坐标，在后续分别根据该G组色标块的顶点的图像和该D组色标块的顶点的图像得到承载该G组色标块的标准安装位的法向量和承载该D组色标块的对接刚体的法向量，以检测该标准安装位和该对接刚体的位姿偏差。

技术领域

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

背景技术

机器视觉技术是近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平面投影的夹角为姿态参数偏差α、β、γ。