基于PLC的机器人末端伺服焊钳位姿自动监测设备及方法与流程

本发明属于机器人位姿测量技术领域，具体涉及一种基于plc的机器人末端伺服焊钳位姿自动监测设备及方法。

背景技术：

现有的机器人末端伺服焊钳位姿检测方法主要有三种：第一种是激光跟踪仪测量法，其由激光跟踪仪和坐标靶配合测出多点的位置，结合相应算法求出机器人末端伺服焊钳位姿(即位置、姿态)，测量精度高，但价格昂贵，测量准备工作复杂，不能够在线测量；第二种是三坐标测量机测量法，三坐标测量机由于自由度较少，累计误差很小，可以高精度测出某点的位置，测量精度高，测量方便，常和激光跟踪仪配合使用，用于标定校正点的位置坐标，但三坐标测量机为机械式测量，容易磨损，不能在线测量；第三种是机器视觉检测法，其通过在光杆上设置三个相机，从不同角度拍摄机器人末端伺服焊钳的位置姿态，再根据机器视觉原理重塑测量机器人末端伺服焊钳位姿，测量速度快，设备简单，但只能测量较少的点，仅作为实验工具使用，不能投入大规模生产，不能在线检测。

技术实现要素：

本发明的目的是提供一种基于plc的机器人末端伺服焊钳位姿自动监测设备及方法，以实现机器人末端伺服焊钳位姿的高精度在线监测，同时降低成本。

本发明所述的基于plc的机器人末端伺服焊钳位姿自动监测设备，包括布置在机器人末端伺服焊钳上的红外线感光坐标靶、沿机器人末端伺服焊钳的待测路径(由若干个周期的已知待测点连接形成)布置的直线导轨和可在直线导轨上运动的监测装置；所述监测装置包括plc控制模块，可在直线导轨上运动的底座，安装在底座上的伺服驱动电机、激光测距仪和带编码器第一步进电机，与带编码器第一步进电机的输出轴连接的丝杆，与丝杆通过螺纹配合的升降支撑柱，一端固定在底座上、另一端与升降支撑柱配合的导向杆，安装在升降支撑柱顶部的带编码器第二步进电机，位于升降支撑柱顶部且与带编码器第二步进电机的输出轴连接的相机支架，安装在相机支架中部的可调光源，以及对称安装在相机支架上的两组双目红外相机；导向杆用于升降支撑柱升降时的导向，可调光源用于改善两组双目红外相机的拍照环境的光线，双目红外相机可以避免机器人末端伺服焊钳因部分遮挡而出现测不准的情况；所述plc控制模块与伺服驱动电机电连接，控制伺服驱动电机驱动底座在直线导轨上运动，激光测距仪与plc控制模块电连接，将检测的底座在直线导轨上的直线运动位移发送给plc控制模块，plc控制模块与带编码器第一步进电机电连接，控制带编码器第一步进电机驱动丝杆转动，以使升降支撑柱上升或者下降，并从带编码器第一步进电机的编码器上获取丝杆的转动角位移(对应于升降支撑柱相对于丝杆的升降距离)，plc控制模块与带编码器第二步进电机电连接，控制带编码器第二步进电机驱动相机支架转动，并从带编码器第二步进电机的编码器上获取相机支架的转动角度，plc控制模块与两组双目红外相机电连接，控制两组双目红外相机对所述红外线感光坐标靶进行拍照，并从两组双目红外相机上获取拍摄的照片的数字图像信息，plc控制模块对底座在直线导轨上的直线运动位移、丝杆的转动角位移、相机支架的转动角度以及拍摄的照片的数字图像信息进行处理后，得到机器人末端伺服焊钳的位姿。

所述底座的底部开设有与直线导轨配合的滑槽，并设置有与所述伺服驱动电机连接的滑动机构，底座通过滑槽以及滑动机构可在直线导轨上运动，底座在直线导轨上的运动更稳定。

所述滑动机构包括万向导轮和由伺服驱动电机驱动的驱动轮，万向导轮和驱动轮将底座与直线导轨的滑动配合变为了滚动配合，减少了摩擦磨损，降低了伺服驱动电机的用电量。

所述伺服驱动电机为驻车充电式伺服电机，在监测装置未工作时(即未进行监测时)，伺服驱动电机可自行充电。

所述直线导轨的末端连接有环形导轨，所述监测装置可在该环形导轨上运动，监测装置还可在环形导轨上停留、改变运动方向，也可在环形导轨上进行驻车充电。

所述红外线感光坐标靶为4个贴有红外线反射纸的测量球连接构成的测量靶，该测量靶安装在机器人末端伺服焊钳上，通过测量该测量靶的位姿来获得机器人末端伺服焊钳的位姿。

所述红外线感光坐标靶也可以是在机器人末端伺服焊钳上选取的4个贴有红外线反射纸的点的连线构成的坐标系。

采用上述自动监测设备进行机器人末端伺服焊钳位姿自动监测的方法为：

第一步、将直线导轨沿机器人末端伺服焊钳的待测路径布置，并使直线导轨的起始端固定在已知点(x0,y0,z0)，且位姿已知为r；

第二步、plc控制模块根据待测点的坐标，控制伺服驱动电机驱动底座在直线导轨上运动，控制带编码器第一步进电机驱动丝杆转动，升降支撑柱上升或者下降，控制带编码器第二步进电机驱动相机支架转动，使两组双目红外相机到达待测点，并控制两组双目红外相机对所述红外线感光坐标靶进行拍照，同时plc控制模块获取底座在直线导轨上的直线运动位移l、丝杆的转动角位移、相机支架的转动角度φ和拍摄的照片的数字图像信息；

第三步、plc控制模块对丝杆的转动角位移进行转换得到升降支撑柱的升降距离d，对拍摄的照片的数字图像信息进行处理，得到红外线感光坐标靶的4个点相对于相机支架的三维坐标d1、d2、d3、d4；

第四步、plc控制模块根据公式(1)、(2)、(3)、(4)计算得到红外线感光坐标靶的4个点相对于已知点(x0,y0,z0)和已知位姿r的三维空间坐标d10、d20、d30、d40；其中，公式(1)为：

公式(2)为：

公式(3)为：

公式(4)为：

c0表示监测装置的匹配系数；

第五步、plc控制模块对红外线感光坐标靶的4个点相对于已知点(x0,y0,z0)和已知位姿r的三维空间坐标d10、d20、d30、d40进行向量计算，得到机器人末端伺服焊钳的位姿r0＝f(d10,d20,d30,d40)；其中，f(d10,d20,d30,d40)表示由世界坐标进行向量计算得到位姿的映射关系。

采用上述监测方法的测量模式有两种：第一种是快速测量模式，即plc控制模块控制两组双目红外相机对所述红外线感光坐标靶进行两次拍照，对第一次拍摄的照片的数字图像信息进行处理，得到红外线感光坐标靶的其中2个点相对于相机支架的三维坐标d1、d2，对第二次拍摄的照片的数字图像信息进行处理，得到红外线感光坐标靶的另外2个点相对于相机支架的三维坐标d3、d4；第二种是高精度测量模式，即plc控制模块控制两组双目红外相机对所述红外线感光坐标靶进行四次拍照，对第一次拍摄的照片的数字图像信息进行处理，得到红外线感光坐标靶的其中1个点相对于相机支架的三维坐标d1，对第二次拍摄的照片的数字图像信息进行处理，得到红外线感光坐标靶的第2个点相对于相机支架的三维坐标d2，对第三次拍摄的照片的数字图像信息进行处理，得到红外线感光坐标靶的第3个点相对于相机支架的三维坐标d3，对第四次拍摄的照片的数字图像信息进行处理，得到红外线感光坐标靶的第4个点相对于相机支架的三维坐标d4。

本发明采用监测装置配合直线导轨、红外线感光坐标靶对机器人末端伺服焊钳位姿进行在线自动监测，其既满足了监测要求，又不妨碍在线生产，实现了高精度、高效率的在线定周期测量，同时也降低了成本。

附图说明

图1为本发明进行自动监测时与机器人末端伺服焊钳的位置关系示意图。

图2为本发明中的相机支架、可调光源以及两组双目红外相机的结构示意图。

图3为本发明中的监测装置的部分结构示意图。

图4为图3的左视图。

图5为图4的a-a剖面图。

图6为本发明中底座与直线导轨的配合示意图。

图7为本发明中的测量靶的结构示意图。

具体实施方式

下面结合附图对本发明作详细说明。

如图1至图7所示的基于plc的机器人末端伺服焊钳位姿自动监测设备，包括红外线感光坐标靶、沿机器人末端伺服焊钳的待测路径布置的直线导轨1和可在直线导轨1上运动的监测装置。直线导轨1的末端连接有可供监测装置在其上运动的环形导轨14，红外线感光坐标靶为4个贴有红外线反射纸的测量球连接构成的测量靶13，该测量靶13通过连接杆15安装在机器人末端伺服焊钳上，通过测量该测量靶13的位姿，来获得机器人末端伺服焊钳的位姿。监测装置包括plc控制模块(图中未示出)，可在直线导轨1上运动的底座2，安装在底座靠近机器人末端伺服焊钳一侧的伺服驱动电机3，安装在底座靠近直线导轨1起始端一侧的激光测距仪4和安装在底座2中部的带编码器第一步进电机5，与带编码器第一步进电机5的输出轴连接的丝杆6，与丝杆6通过螺纹配合的升降支撑柱7，一端固定在底座2上、另一端与升降支撑柱7配合的两根导向杆8，安装在升降支撑柱7顶部的带编码器第二步进电机9，位于升降支撑柱7顶部且与带编码器第二步进电机9的输出轴连接的相机支架10，安装在相机支架10中部的可调光源11，以及对称安装在相机支架10上的两组双目红外相机12。导向杆8用于升降支撑柱7升降时的导向，可调光源11用于改善两组双目红外相机12的拍照环境的光线，通过人工调节，双目红外相机12可以避免机器人末端伺服焊钳因部分遮挡而出现测不准的情况；伺服驱动电机3为驻车充电式伺服电机(驻车充电技术为公知技术)，底座2的底部开设有与直线导轨1配合的滑槽21，并设置有与伺服驱动电机3连接的滑动机构，该滑动机构包括万向导轮22和由伺服驱动电机3驱动的驱动轮23，底座2通过滑槽21、万向导轮22和驱动轮23可在直线导轨1上运动。plc控制模块与伺服驱动电机3电连接，控制伺服驱动电机3驱动底座2在直线导轨1上运动，激光测距仪4与plc控制模块电连接，将检测的底座2在直线导轨1上的直线运动位移发送给plc控制模块，plc控制模块与带编码器第一步进电机5电连接，控制带编码器第一步进电机5驱动丝杆6转动，以使升降支撑柱7上升或者下降，并从带编码器第一步进电机5的编码器上获取丝杆6的转动角位移(对应于升降支撑柱7相对于丝杆6的升降距离)，plc控制模块与带编码器第二步进电机9电连接，控制带编码器第二步进电机9驱动相机支架10转动，并从带编码器第二步进电机9的编码器上获取相机支架10的转动角度，plc控制模块与两组双目红外相机12电连接，控制两组双目红外相机12对测量靶13进行拍照，并从两组双目红外相机12上获取拍摄的照片的数字图像信息，plc控制模块对底座2在直线导轨1上的直线运动位移、丝杆6的转动角位移、相机支架10的转动角度以及两组双目红外相机12拍摄的照片的数字图像信息进行处理后，得到机器人末端伺服焊钳的位姿。

另外，对于某些机器人末端伺服焊钳而言，由于其上可以找到4点构成坐标系，因此红外线感光坐标靶也可以是在机器人末端伺服焊钳上选取的4点连线构成的坐标系，构成该坐标系的4点都贴有红外线反射纸。

采用上述基于plc的机器人末端伺服焊钳位姿自动监测设备进行机器人末端伺服焊钳位姿自动监测的方法为：

第一步、将直线导轨1沿机器人末端伺服焊钳的待测路径布置，并使直线导轨1的起始端固定在已知点(x0,y0,z0)，且位姿已知为r。

第二步、plc控制模块根据待测点的坐标，控制伺服驱动电机3驱动底座2在直线导轨1上运动，控制带编码器第一步进电机5驱动丝杆6转动，升降支撑柱7上升或者下降，控制带编码器第二步进电机9驱动相机支架10转动，使两组双目红外相机12到达待测点，并控制两组双目红外相机12对测量靶13进行拍照，同时plc控制模块获取底座2在直线导轨1上的直线运动位移l、丝杆6的转动角位移、相机支架10的转动角度φ和两组双目红外相机12拍摄的照片的数字图像信息。

第三步、plc控制模块对丝杆6的转动角位移进行转换得到升降支撑柱7的升降距离d(该转换方法为公知技术)，plc控制模块对两组双目红外相机12拍摄的照片的数字图像信息进行处理(该处理方法为公知技术)，得到测量靶13的4个点相对于相机支架12的三维坐标d1、d2、d3、d4。

第四步、plc控制模块根据公式(1)、(2)、(3)、(4)计算得到测量靶13的4个点相对于已知点(x0,y0,z0)和已知位姿r的三维空间坐标d10、d20、d30、d40；其中，公式(1)为：

公式(2)为：

公式(3)为：

公式(4)为：

c0表示监测装置的匹配系数(通过实验采集数据、统计分析标定而得到，在本申请中为已知参数)。

第五步、plc控制模块对测量靶13的4个点相对于已知点(x0,y0,z0)和已知位姿r的三维空间坐标d10、d20、d30、d40进行向量计算，得到机器人末端伺服焊钳的位姿r0＝f(d10,d20,d30,d40)；其中，f(d10,d20,d30,d40)表示由世界坐标进行向量计算得到位姿的映射关系(为公知技术)。

采用上述监测方法的测量模式有两种：

第一种是快速测量模式：plc控制模块控制两组双目红外相机12对测量靶13进行两次拍照，对第一次拍摄的照片的数字图像信息进行处理，得到测量靶13的其中2个点相对于相机支架10的三维坐标d1、d2，对第二次拍摄的照片的数字图像信息进行处理，得到测量靶13的另外2个点相对于相机支架的三维坐标d3、d4；然后再执行第四步、第五步，计算得到机器人末端伺服焊钳的位姿r0。

第二种是高精度测量模式：plc控制模块控制两组双目红外相机12对测量靶13进行四次拍照，对第一次拍摄的照片的数字图像信息进行处理，得到测量靶13的其中1个点相对于相机支架10的三维坐标d1，对第二次拍摄的照片的数字图像信息进行处理，得到测量靶13的第2个点相对于相机支架10的三维坐标d2，对第三次拍摄的照片的数字图像信息进行处理，得到测量靶13的第3个点相对于相机支架的三维坐标d3，对第四次拍摄的照片的数字图像信息进行处理，得到测量靶13的第4个点相对于相机支架的三维坐标d4；然后再执行第四步、第五步，计算得到机器人末端伺服焊钳的位姿r0。

本领域技术人员可以根据实际情况选择上述两种测量模式中的一种。