煤矿井下钻杆装卸机器人送杆识别系统的精度测试方法与流程

1.本发明属于煤矿井下钻杆装卸机器人递送钻杆系统和递送空间位姿精度测试领域，具体涉及一种基于合作靶标的钻杆装卸机器人钻杆递送系统和一种使用激光投影法测试煤矿井下钻杆装卸机器人递送钻杆空间位姿精度的方法。


背景技术：

2.基于合作目标的单目视觉位姿识别系统因其结构简单、精度高，是国内航天器交会对接测量系统普遍采用的结构形式，同时鉴于单目视觉视野范围大、成本低、鲁棒性高等优点，在煤矿井下钻杆装卸机器人送杆系统中，在利用合作靶标单目位姿测量时，受钻杆装卸机械臂作业空间的限制，存在现场钻机两体布置视线遮挡的问题，从而影响递送钻杆位置的精度，影响钻杆装卸的准确率，也影响机械臂装卸动作顺畅完成；因此采用基于多合作靶标的单目视觉识别技术，来解决远距离、大倾角、钻机两体布置时递送钻杆位置识别，增强系统灵活性，解决视野受限问题。
3.基于视觉的钻杆装卸机器人送杆空间位姿精度是衡量视觉识别系统、6自由度机械臂和柔性手爪整个钻杆装卸系统综合送杆精度的重要指标。目前在工业机器人行业主要使用高精度激光跟踪仪测量，激光跟踪仪操作复杂，对测试人员要求高，影响了钻杆装卸机器人送杆空间位姿精度出厂检验工作。
4.同时，现有的工业机器人空间位姿精度测试方法，需要专用设备多次测量，测试繁琐，效率低、成本高。另外还有采用摄像头跟踪测量的方式，而视觉方式因对环境要求较高，光线等外界因素会影响测量的精度。


技术实现要素：

5.针对现有技术中存在的不足，本发明的目的在于，提供一种煤矿井下钻杆装卸机器人送杆识别系统的精度测试方法，解决煤矿井下钻杆装卸机器人通过视觉递送钻杆到夹持位置，其送杆空间精度测试问题。
6.为了解决上述技术问题，本发明采用如下技术方案予以实现：一种煤矿井下钻杆装卸机器人送杆识别系统的精度测试方法，该方法能测试出煤矿井下钻杆装卸机器人送杆识别系统送杆空间位姿的精度；所述煤矿井下钻杆装卸机器人送杆识别系统包括设在钻杆装卸车体上的杆箱、机械臂、机械臂末端的柔性手爪、单目相机和钻杆辅助定位装置，以及设在钻进车体上的自对中的夹持器以及设在夹持器前方两侧的双合作靶标；所述钻杆辅助定位装置位于机械臂前方，钻杆辅助定位装置能对机械臂抓取的钻杆进行定位以消除钻杆长度方向的误差，然后机械臂能再将定位后的钻杆送至夹持器；该精度测试方法包括以下步骤：步骤一，在钻杆两端安装高精度点状激光发射模组：先将激光发射模组安装至过渡接头内，再将过渡接头螺接在钻杆两端，将固定吊锤的钢丝线绕钻杆中间的沟槽固定；步骤二，安装格子标尺和底部长度标尺：将两个格子标尺分别安装在夹持器前端
和动力头前端，两个格子标尺平行相对且正对齐，格子标尺中心与夹持器、动力头中心同心，两个格子标尺能用于计算递送钻杆径向误差以及俯仰角和偏航角误差；将底部长度标尺水平安装在夹持器和动力头之间的导轨上，两个格子标尺中心连线的中点是钻杆理论递送中心位置，底部长度标尺能测量递送钻杆轴向误差；步骤三，机械臂及柔性手爪将钻杆递送至夹持器和动力头之间以进行夹持交接的空间位置；步骤四，激光发射模组光点投射在两端的格子标尺上，两个格子标尺上分别建立空间直角坐标系，均以格子标尺的中心为坐标原点，水平方向为x轴方向，竖向为z轴方向，两个格子标尺中心连线方向为y轴方向，记录光点在两端格子标尺x、z方向的投影偏移量，记录钻杆在底部长度标尺上吊锤位置的轴向值；利用oxyz坐标系与中心轴关系计算出钻杆径向和轴向误差，俯仰角和偏航角误差，从而精确测试出煤矿井下钻杆装卸机器人送杆空间位姿的精度；所述俯仰角α为：α=arctan(a b)/d；其中，a和b为两个格子标尺上z轴方向的读数，d为两个格子标尺之间的距离；由上述俯仰角α可得a0=（d/2）*tanα，a0为理论钻杆递送位置中心线在z轴方向的读数，则在zoy坐标系中径向误差
△
z=a-a0；所述偏航角β为：β=arctan(e f)/d；其中，e和f为两个格子标尺上x轴方向的读数，d为两个格子标尺之间的距离；由上述偏航角β可得e0=(d/2)*tanβ，e0为理论钻杆递送位置中心线在x轴方向的读数，则在xoy坐标系中径向误差
△
x=e-e0；轴向距离y通过吊锤中心与底部长度标尺在y方向的距离得到，即可得到轴向误差
△
y。
7.本发明还包括如下技术特征：具体的，所述步骤一中，激光发射模组激光发射中心严格与钻杆中心同轴；测试前将投射在两个格子标尺上的激光发射模组光斑圆点直径调节至0.5mm以下，以保证投影到格子靶标上的光斑满足测试精度要求。
8.具体的，所述步骤二中，格子标尺采用不锈钢材料且为圆形，格子标尺上的正方形格子是以格子标尺圆心为基准，采用激光雕刻方式光刻而成，光刻线条粗细为0.2mm，正方形格子边长为1mm；两个格子标尺下端都设有底座，以钻进车体机身导轨为安装基准，将格子标尺螺接在导轨上。
9.具体的，两个格子标尺分别为前格子标尺和后格子标尺，前格子标尺安装在夹持器前端，后格子标尺安装在动力头前。
10.具体的，所述钻杆辅助定位装置包括底座、设在底座上的斜坡、安装在斜坡上的v形支撑架、设在斜坡下端的挡板以及接近开关；钻杆放置在v形支撑架上，斜坡的倾斜设置能使钻杆滑至挡板以对钻杆长度方向进行定位，接近开关能用于检测钻杆是否滑至挡板处。
11.具体的，所述机械臂和柔性手爪用以完成递送钻杆动作；柔性手爪包括手爪、手爪接近开关和永磁铁；利用永磁铁吸合以吸取和松开钻杆，取钻杆时，当钻杆远离钻杆辅助定位装置后，手爪夹紧钻杆防止碰到障碍物掉落，放钻杆时，当机械臂将钻杆递送至夹持交接
的空间位置时，夹持器夹紧钻杆，手爪松开钻杆，永磁铁消磁，松开钻杆；机械臂和柔性手爪离开送杆位置，接近开关判断钻杆是否已脱离柔性手爪，确保整个递送钻杆流程和后续流程顺利进行。
12.具体的，所述夹持器包括夹持器座、卡瓦连接板、卡瓦、夹持器油缸、平动齿条和齿轮；两个卡瓦连接板设在夹持器座两侧，卡瓦装在卡瓦连接板上，卡瓦连接板两侧分别设置所述夹持器油缸和平动齿条且平动齿条位于卡瓦下方，两个卡瓦连接板所连平动齿条相互平行，在两个平动齿条之间设置所述齿轮且齿轮与两个平动齿条均啮合；当一侧的夹持器油缸缸杆运动，能通过卡瓦连接板带动平动齿条产生平动，平动齿条与齿轮啮合能将平动转换为旋转，齿轮再与另一端平动齿条啮合带动和另一端卡瓦连接板，进而使连接在卡瓦连接板上的两个卡瓦同步夹紧和松开，实现自对中功能。
13.具体的，所述双合作靶标为两个白底黑格的棋盘格靶标，包括大棋盘格靶标和小棋盘格靶标，大棋盘格靶标为9行12列棋盘格，小棋盘格靶标为7行6列棋盘格，且表面不易发生镜面反射。
14.本发明与现有技术相比，具有如下技术效果：（1）本发明精度测试方法能通过激光投影法计算出递送钻杆径向、轴向误差，俯仰和偏航角度误差，从而在煤矿井下钻杆装卸机器人在通过视觉递送钻杆时，精确测试出送杆空间位姿的精度。
15.（2）本发明送杆识别系统送钻杆时，能将从杆箱中抓取分离的钻杆经过钻杆辅助定位装置进行整理定位，消除因3d视觉抓取引入的误差，再利用单目相机与双合作靶标配合识别的递送位置位姿信息，将位姿信息转换到机械臂，机械臂驱动各关节运动，将钻杆递送至识别的位置，使用机械臂末端具有柔性补偿功能的手爪，与自对中夹持器协同完成钻杆递送和夹紧交接动作。
附图说明
16.图1是本发明送杆识别系统示意图；图2是本发明钻杆辅助定位装置结构示意图；图3是本发明双合作靶标示意图；图4是本发明钻进系统与钻杆装卸系统布置和视野范围示意图；图5是本发明柔性手爪示意图；图6是本发明夹持器示意图；图7是本发明精度测试方法中的格子标尺位置示意图；图8是本发明激光发射模组连接示意图；图9是本发明激光投影法原理和前后投影点激光投影图；图10是本发明投影计算示意图；（a）俯仰角α和
△
z径向误差计算；（b）偏航角β和
△
x径向误差计算；（c）轴向投影方向示意。
17.附图标记含义：1.钻杆装卸系统，2.钻进系统，3.杆箱，4.机械臂，5.柔性手爪，6.单目相机，7.钻杆辅助定位装置，8.夹持器，9.双合作靶标，10.钻杆，11.动力头，12.格子标尺，13.激光发射模组，14.过渡接头，15.吊锤；51.手爪，52.手爪接近开关，53.永磁铁，71.底座，72.斜坡，73.v形支撑架，74.挡板，75.接近开关，81.夹持器座，82.卡瓦连接板，83.夹
持器油缸，84.平动齿条，85.齿轮。
具体实施方式
18.本发明提供一种煤矿井下钻杆装卸机器人送杆识别系统及精度测试方法，该钻杆装卸机器人送杆识别系统包括设在钻杆装卸车体上的杆箱、机械臂、机械臂末端的柔性手爪、单目相机和钻杆辅助定位装置，以及设在钻进车体上的自对中的夹持器以及设在夹持器前方两侧的双合作靶标；钻杆辅助定位装置位于机械臂前方，钻杆辅助定位装置能对机械臂抓取的钻杆进行定位以消除钻杆长度方向的误差，然后机械臂能再将定位后的钻杆送至夹持器；送钻杆时将已从杆仓中抓取分离的钻杆经过钻杆辅助定位装置进行整理定位，消除因3d视觉抓取引入的误差，再利用单目相机与双合作靶标配合识别的递送位置位姿信息，将位姿信息转换到机械臂，机械臂驱动各关节运动，将钻杆递送至识别的位置，使用机械臂末端具有柔性补偿功能的手爪，与自对中夹持器协同完成钻杆递送和夹紧交接动作。
19.该空间位姿精度测试方法是对钻杆装卸机器人送杆系统递送完成后，钻杆空间位姿精度的测试，以评价递送完成情况，在精度测试过程中包括：在需要自动装卸的钻杆两端安装高精度点状激光发射模组，在递送钻杆位置并与之交接的自对中夹持器前端安设与之同心的格子标尺，同时在递送钻杆位置并与之交接另一端托板位置也安设与动力头同心的格子标尺，确保夹持器、动力头与两个格子标尺同心，且两个格子标尺径向和轴向中心为理论递送中心位置；当钻杆装卸机器人机械臂将钻杆递送至需要夹持交接的空间位置时，钻杆两端固定的高精度点状激光发射模组将光点投射在两端的格子标尺上，记录激光点在两端格子标尺投影的偏移量，根据格子标尺上偏移方向及偏移量向各轴投影计算出钻杆径向、轴向误差，俯仰和偏航角度误差，从而在煤矿井下钻杆装卸机器人在通过视觉递送钻杆时，精确测试出送杆空间位姿的精度。
20.以下给出本发明的具体实施例，需要说明的是本发明并不局限于以下具体实施例，凡在本技术技术方案基础上做的等同变换均落入本发明的保护范围。
21.实施例 1：本实施例提供一种煤矿井下钻杆装卸机器人送杆识别系统，如图1至图6所示，该系统包括设在钻杆装卸车体上的杆箱、机械臂、机械臂末端的柔性手爪、单目相机和钻杆辅助定位装置，以及设在钻进车体上的自对中的夹持器以及设在夹持器前方两侧的双合作靶标；钻杆辅助定位装置位于机械臂前方，钻杆辅助定位装置能对机械臂抓取的钻杆进行定位以消除钻杆长度方向的误差，然后机械臂能再将定位后的钻杆送至夹持器。
22.钻杆辅助定位装置包括底座、设在底座上的斜坡、安装在斜坡上的v形支撑架、设在斜坡下端的挡板以及接近开关；钻杆放置在v形支撑架上，斜坡的倾斜设置能使钻杆滑至挡板以对钻杆长度方向进行定位，接近开关能用于检测钻杆是否滑至挡板处。钻杆辅助定位装置目的是对钻杆进行二次定位，以消除因3d视觉抓取，给后续递送钻杆过程引入误差干扰，也为基于合作靶标的单目识别奠定基础，尤其钻杆辅助定位可大大提高钻杆送至夹持器位置在轴线方向满足精度，在钻杆拧卸丝扣时更加可靠拧卸。
23.单目相机因钻进系统倾角调节范围大，在-90
°
~ 90
°
之间调节，机身长度为2.5 m，要求单目相机视场角能覆盖
±
90
°
范围，靶标布置时不受机械臂和辅助定位装置遮挡；钻杆装卸系统中的单目相机水平视场角57
°
和垂直视场角108
°
，紫色透明状为单目相机视窗，钻
进系统与钻杆装卸系统车体距离在1.5 m以内，确保机械臂在臂展2.1 m的工作空间内递送钻杆。
24.机械臂和柔性手爪用以完成递送钻杆动作；柔性手爪包括手爪、手爪接近开关和永磁铁；利用永磁铁吸合以吸取和松开钻杆，取钻杆时，当钻杆远离钻杆辅助定位装置后，手爪夹紧钻杆防止碰到障碍物掉落，放钻杆时，当机械臂将钻杆递送至夹持交接的空间位置时，夹持器夹紧钻杆，手爪松开钻杆，永磁铁消磁，松开钻杆；机械臂和柔性手爪离开送杆位置，接近开关判断钻杆是否已脱离柔性手爪，确保整个递送钻杆流程和后续流程顺利进行。
25.夹持器包括夹持器座、卡瓦连接板、卡瓦、夹持器油缸、平动齿条和齿轮；两个卡瓦连接板设在夹持器座两侧，卡瓦装在卡瓦连接板上，卡瓦连接板两侧分别设置夹持器油缸和平动齿条且平动齿条位于卡瓦下方，两个卡瓦连接板所连平动齿条相互平行，在两个平动齿条之间设置齿轮且齿轮与两个平动齿条均啮合；当一侧的夹持器油缸缸杆运动，能通过卡瓦连接板带动平动齿条产生平动，平动齿条与齿轮啮合能将平动转换为旋转，齿轮再与另一端平动齿条啮合带动和另一端卡瓦连接板，进而使连接在卡瓦连接板上的两个卡瓦同步夹紧和松开，实现自对中功能。在齿轮上下两端分别设有挡圈和限位环用以限制齿轮轴向位置，齿轮内设有自润滑轴承。
26.双合作靶标为两个白底黑格的棋盘格靶标，包括大棋盘格靶标和小棋盘格靶标，大棋盘格靶标为9行12列棋盘格，小棋盘格靶标为7行6列棋盘格，且表面不易发生镜面反射。具体的，双合作靶标是采用两个不同合作靶标配合识别的方式，考虑钻进系统调角范围大，为了解决遮挡和视野覆盖问题，如图3所示，在夹持器上下各布置一个棋盘格靶标（白底黑格），也即采用主大、次小双靶标方案，一种是位于上方的7行6列棋盘格，另一种是下方的9行12列棋盘格，采用符合煤安要求，且表面平整度和粗糙度较高玻璃材质制作，由黑白两色构成，背景为白色，黑色图形和白色背景之间的边缘梯度幅度较强，更易于识别，且表面不易发生镜面反射，更适合煤矿井下环境，精度更高、测试效果更好。
27.实施例2：本实施例提供一种煤矿井下钻杆装卸机器人送杆识别系统的精度测试方法，如图7至图10所示，该方法能测试出煤矿井下钻杆装卸机器人送杆识别系统送杆空间位姿的精度；包括以下步骤：步骤一，在钻杆两端安装高精度点状激光发射模组：先将激光发射模组安装至过渡接头内，再将过渡接头螺接在钻杆两端，将固定吊锤的钢丝线绕钻杆中间的沟槽固定；具体的，步骤一中，激光发射模组激光发射中心严格与钻杆中心同轴；测试前将投射在两个格子标尺上的激光发射模组光斑圆点直径调节至0.5mm以下，以保证投影到格子靶标上的光斑满足测试精度要求。具体的，高精度激光发射模组固定在过渡接头孔上，过渡接头螺接在钻杆的公、母头两端；过渡接头和激光发射模组为紧配合连接，且过渡接头的内孔和螺纹孔保证6级的同轴度要求，以使激光发射模组激光发射中心严格与钻杆中心同轴，螺接式结构可保证发射的激光束从钻杆公头和母头中心发射出，且平行于钻杆轴线，与钻杆轴线保持一定同轴度要求。钻杆上有一个1mm的沟槽，用于定位钻杆中心吊锤钢丝线，辅助测试轴向和径向误差值。
28.步骤二，安装格子标尺和底部长度标尺：将两个格子标尺分别安装在夹持器前端
和动力头前端，两个格子标尺平行相对且正对齐，格子标尺中心与夹持器、动力头中心同心，两个格子标尺能用于计算递送钻杆径向误差以及俯仰角和偏航角误差；将底部长度标尺水平安装在夹持器和动力头之间的导轨上，两个格子标尺中心连线的中点是钻杆理论递送中心位置，底部长度标尺能测量递送钻杆轴向误差；具体的，步骤二中，格子标尺采用不锈钢材料且为圆形，格子标尺上的正方形格子是以格子标尺圆心为基准，采用激光雕刻方式光刻而成，光刻线条粗细为0.2mm，正方形格子边长为1mm；两个格子标尺下端都设有底座，以钻进车体机身导轨为安装基准，将格子标尺螺接在导轨上。两个格子标尺分别为前格子标尺和后格子标尺，前格子标尺安装在夹持器前端，后格子标尺安装在动力头前。具体的，在前格子标尺上建立的空间直角坐标系为o1x1y1z1，在后格子标尺上建立的空间直角坐标系为o2x2y2z2。
29.步骤三，机械臂及柔性手爪将钻杆递送至夹持器和动力头之间以进行夹持交接的空间位置；当首次使用时，需要对递送位置（夹持器中心）进行示教，目的是得到夹持器夹紧位置与合作靶标的坐标转换关系，从而在后续抓取中，通过单目相机对合作靶标的识别，获取递送位置。
30.步骤四，激光发射模组光点投射在两端的格子标尺上，根据激光投影法原理，两个格子标尺上分别建立空间直角坐标系，均以格子标尺的中心为坐标原点，水平方向为x轴方向，竖向为z轴方向，两个格子标尺中心连线方向为y轴方向（具体的，前格子标尺圆心至后格子标尺圆心方向为y轴方向），记录光点在两端格子标尺x、z方向的投影偏移量，记录钻杆在底部长度标尺上吊锤位置的轴向值；利用xoz坐标与中心轴关系计算出钻杆径向和轴向误差，俯仰角和偏航角误差，从而精确测试出煤矿井下钻杆装卸机器人送杆空间位姿的精度；俯仰角α为：α=arctan(a b)/d；其中，a和b为两个格子标尺上z轴方向的读数，d为两个格子标尺之间的距离；由上述俯仰角α可得a0=（d/2）*tanα，a0为理论钻杆递送位置中心线在z轴方向的读数，则在zoy坐标系中径向误差
△
z=a-a0；图10中，l0是理论钻杆递送位置中心线，经过递送中心，而l1是实际有误差时递送位置钻杆中心线，l2是把l1平移后求α的辅助线；偏航角β为：β=arctan(e f)/d；其中，e和f为两个格子标尺上x轴方向的读数，d为两个格子标尺之间的距离；由上述偏航角β可得e0=(d/2)*tanβ，e0为理论钻杆递送位置中心线在x轴方向的读数，则在xoy坐标系中径向误差
△
x=e-e0；图10中，l0是理论钻杆递送位置中心线，经过递送中心，而l3是实际有误差时递送位置钻杆中心线，l4是把l3平移后求α的辅助线。
31.轴向距离y通过吊锤中心与底部长度标尺在y方向的距离得到，即可得到轴向误差
△
y。
32.通过该方法，基于单目相机识别送杆位置精度，测试了从-90
°
~90
°
，每隔10
°
测试一组数据，共19个位置误差，误差如表1所示：表1 19组递送钻杆位置精度
由上表试验数据可见，当钻臂倾角在
±
60
°
范围内，满足要求精度，当倾角逐渐增大至
±
90
°
时，无论径向、轴向和倾角误差都在增加。而在钻进中，经常施工倾角范围为-45
°
~ 60
°
，为了不影响
±
60
°
范围内识别精度和整体识别效果，采用误差补偿方法对识别误差进行补偿。