一种煤矿井下码垛式杆箱钻杆分离系统与方法与流程

1.本发明属于煤矿井下钻孔机器人自动加卸钻杆领域，具体涉及一种煤矿井下码垛式杆箱钻杆分离系统与方法。


背景技术：

2.近年来煤矿领域正在加快推进“机械化换人、自动化减人、智能化无人”发展，《煤矿机器人重点研发目录》要求探水、防突和防冲钻孔机器人具备自动装卸钻杆、自动调整钻姿、智能钻孔规划和定位功能。目前，已有小批量使用的自动加卸钻杆装置采用多级传递、顺序动作的方式，任何一个环节出现故障，都会使自动装卸钻杆程序出现问题，中断全自动流程，在人员干预下排除故障才可继续进行全自动施工。
3.因此将6自由度工业机器人经过防爆设计后引入到自动加杆方案中，利用码垛、拆垛分离出堆放在杆箱内的钻杆，然而当钻杆因施工时，长期使用磨损后，存在码垛位置因此发生变化的问题，由于地层、压风或压水排渣、转速、转矩等影响，钻杆使用中会有不同程度磨损，且整仓钻杆中每一根磨损程度不同，以φ73 mm整体铣削螺旋钻杆为例，出厂时外径φ73
±
0.3 mm，在连续使用了5个月后，磨损最严重的钻杆外径为φ68.76 mm，磨损最轻微的钻杆外径为φ72.56 mm。若按满仓钻杆全部磨损至φ68 mm估算，杆仓为12层，则高度方向累计最大误差将达55.14 mm，已超出了柔性手爪可以补偿高度误差范围。
4.针对钻杆施工中的磨损而导致6自由度机械臂抓取失败的情况，提出了采用眼在手外的3d相机视觉识别方案，通过点云获取、图像预处理、特征匹配等，对杆箱内大量钻杆依次进行识别、定位与抓取，最终将钻杆从多排多列的钻杆箱中分离，从而解决因钻杆磨损抓取失败的问题。但是在煤矿井下应用中，会因煤矿井下光照条件、粉尘、巷道布置、电磁波干扰、煤矿防爆安全等影响，给基于视觉识别技术的防爆机械臂推广带来困难，尤其是利用结构光3d相机进行杆箱内钻杆识别与定位时，因3d相机对煤矿井下特定环境下光照条件敏感，易存在图像目标识别和提取有用信息难度大、识别鲁棒性差等缺点，进而影响抓取钻杆的准确性。单一依靠视觉，无法提高系统的可靠性，因此亟需寻找与视觉识别进行融合的新方法以提高识别鲁棒性。


技术实现要素：

5.针对现有技术中存在的不足，本发明的目的在于，提供一种煤矿井下码垛式杆箱钻杆分离系统与方法，解决煤矿井下码垛式杆箱中钻杆分离时，单一依靠3d视觉识别鲁棒性低的问题。
6.为了解决上述技术问题，本发明采用如下技术方案予以实现：一种煤矿井下码垛式杆箱钻杆分离方法，该方法采用3d视觉与码放算法相融合对码垛式杆箱内钻杆精确识别与定位后进行钻杆抓取分离；包括以下步骤：步骤一、将码垛式杆箱内最下层第一行第一列钻杆作为示教钻杆记作a，记录钻杆a的位姿（x0,z0）以及机械臂和码垛式杆箱内钻杆的位置关系；
步骤二、依据码垛式杆箱内钻杆的码放规则，逐行逐列计算每根钻杆理论推算位姿即钻杆的坐标；钻杆的行坐标、列坐标分别为：姿即钻杆的坐标；钻杆的行坐标、列坐标分别为：姿即钻杆的坐标；钻杆的行坐标、列坐标分别为：（1）上式中，i为行数，i=0对应第一行钻杆，j为列数，j=0对应第一列钻杆，杆箱内钻杆的层高为相邻奇数行和偶数行的钻杆中心竖向距离62mm，同一行的相邻钻杆中心横向距离为77mm，钻杆半径为36.5mm，钻杆a与杆箱侧壁的距离为75mm；步骤三、利用3d相机视觉识别钻杆并得到钻杆的视觉识别位姿；步骤四、将步骤二中的理论推算位姿与步骤三中的视觉识别位姿进行对比，若理论推算位姿和视觉识别位姿的差值超过设定值，则将理论推算位姿作为抓取钻杆位姿，若差值未超过设定值，则将视觉识别位姿作为抓取钻杆位姿；步骤五、将步骤四得到的抓取钻杆位姿信息送至钻杆分离控制单元中的机械臂控制器，机械臂动作，机械臂末端磁吸手爪上的接近开关判断磁吸手爪是否成功抓取钻杆，若已抓取，继续执行，若未抓取，返回执行步骤四，直至抓到钻杆；步骤六、当施工周期超过30天时，对理论推算位姿进行更新，完成钻杆磨损后抓取钻杆位姿的校正更新，然后继续执行步骤四进行钻杆抓取分离。
7.本发明还包括如下技术特征：具体的，所述步骤三包括：首先3d相机获取钻杆点云数据，为了保持点云的形状特征并提高钻杆点云数据处理速度，采用下采样滤波对钻杆点云进行精简预处理，再用iss特征检测算法提取钻杆局部点云数据边缘特征点，从获取的钻杆点云构建表面曲面，并在重建模型中计算表面法线，使用fpfh快速点特征直方图描述各个点的法线的偏差和角度关系，采用sac-ia粗配准算法进行匹配，旋转平移点云得到初始变化矩阵；再进行icp精配准，首先假设一个初始位姿估计，从扫描点集中选设定数量的点，依据最小欧式距离，寻找与这些点距离最近的点集，计算变换矩阵r和t，改变视角后再扫描，重新关联点对重新计算转换矩阵，使对应点的点集间距离最小化，通过迭代计算，直到残差平方和所构成的目标函数值不变，即得到精匹配矩阵，完成精配准，从而通过3d视觉识别得到钻杆的视觉识别位姿。
8.具体的，所述步骤四中的设定值为钻杆半径。
9.具体的，所述步骤六中对理论推算位姿进行更新包括：选择满箱钻杆时更新，抓取最上层钻杆时，记录最上层所有钻杆的z向位姿即高度值，计算所有高度值的中位数，筛选出位于中位数加减1mm范围内的高度值，将选出的高度值平均后，再与z0求差后取均值，用该均值除以钻杆层数得到新的层高，即完成了钻杆磨损后抓取坐标的校正更新。
10.一种煤矿井下码垛式杆箱钻杆分离系统，包括码垛式杆箱、设在码垛式杆箱上方的可升降式的3d相机、位于码垛式杆箱旁的六自由度的机械臂、连在机械臂末端的磁吸手爪以及钻杆分离控制单元；
所述码垛式杆箱包括底部的方形框架、两个水平的钻杆支承架、挡板、可拆卸的挡销及两个侧门；方形框架包括两个端梁和两个侧梁，两个钻杆支承架相互平行且均垂直连接在两个侧梁之间，钻杆支承架上设有等间距排布的弧形槽以码放钻杆，钻杆垂直于钻杆支撑架放置，挡板竖向固定在钻杆支承架两端以围挡钻杆，挡销可拆卸设在两个挡板上端并与钻杆支承架平行以防止整箱钻杆运输时震动引起钻杆掉落；在两个端梁上均安装所述侧门且侧门能竖向定位或侧向打开；所述3d相机的视野范围能覆盖整个码垛式杆箱区域，包括相机本体、连接相机本体的水平的举升梁、垂直连在举升梁下方的举升油缸以及隔爆灯；相机本体位于码垛式杆箱的正上方，隔爆灯设在举升梁上；所述磁吸手爪包括手爪、永磁铁和接近开关；所述钻杆分离控制单元包括控制层、机械臂控制器和视觉模块；其中：控制层用于控制手爪的张开闭合、磁铁吸合、3d相机的升降；机械臂控制器能进行数据运算、i/o交互与数据采集，以控制机械臂动作；视觉模块用于接收3d相机获取杆箱内钻杆点云信息，并进行图像处理，将处理结果作为机械臂运动的决策依据。
11.具体的，所述钻杆支承架能根据钻杆种类、大小和杆箱内钻杆容量要求进行更换，以适用不同钻杆的码放要求。
12.具体的，所述侧门下端与端梁铰接以能向两侧打开，侧门在运输时围挡杆箱内钻杆，钻进时侧门打开以确保抓杆时不对钻杆长度方向形成干扰防止钻杆从杆箱内取出时掉落。
13.具体的，所述举升油缸上还设有保护举升油缸胶管的钢制拖链；分离系统运输状态时，举升油缸降低将3d相机回收至码垛式杆箱内，分离系统工作状态时，举升油缸达到最大行程举升至最高以确保3d相机视野覆盖至码垛式杆箱内最顶层和最底层的每一根极限位置钻杆。
14.具体的，所述磁吸手爪利用永磁铁吸合来吸取和松开钻杆，取钻杆时，当钻杆远离杆箱内最顶层钻杆后，手爪夹紧钻杆防止碰到障碍物掉落，放钻杆时，钻杆靠近杆箱内最顶层钻杆时，手爪松开钻杆，以便永磁铁松开放下钻杆；接近开关用以判断是否吸取并夹紧钻杆，确保整个抓杆或放杆流程顺利进行。
15.本发明与现有技术相比，具有如下技术效果：本发明将3d视觉识别钻杆位姿与码放算法计算的理论推算位姿相融合，将两种方法优势互补，解决因煤矿井下环境复杂导致单一依靠3d视觉识别失败、分离失败的问题，可提高钻杆分离系统的鲁棒性。
16.本发明能避免现有技术中的漏抓钻杆现象发生，采用出厂时示教最下层的第一行第一列钻杆，并将这个钻杆定义为第一根钻杆位置，获取了第一根钻杆与机械臂位姿关系，得到了钻杆支承架的位置；再按照码放原则，逐行逐列依次获得每一根钻杆理论推算坐标值，当利用机器视觉进行每一次抓取时，与理论推算坐标进行对比，若理论坐标和视觉测试坐标的高度方向差小于36.5 mm（钻杆半径36.5 mm，3d相机可获取钻杆上半部分点云信息），则说明视觉识别无漏层、跃层情况发生，正常抓取。若理论坐标和视觉测试坐标的高度方向差大于36.5 mm，说明视觉识别有漏层现象，则按照理论坐标抓取。若从柔性手爪上的接近开关判断，未能抓起钻杆，则说明因为外部光线原因，导致视觉识别误差超差，则按照
理论坐标重新抓取。
17.本发明考虑了钻杆在使用过程中会磨损，从而导致直径减小的情况，因此钻进30天会对理论坐标计算值更新；以多次视觉识别的整个杆仓钻杆数量和高度为依据，结合示教所获首个钻杆的位置，实时动态校准码垛层高数，即记录最上层所有抓取钻杆的高度信息，选出所有高度值的中位数，取中位数
±
1mm以内的数，将这些选出的高度平均后，对码放算法理论推断的坐标进行二次校正。
附图说明
18.图1是本发明分离系统。
19.图2是本发明磁吸手爪示意图。
20.图3是本发明3d相机和码垛式杆箱结构示意图。
21.图4是本发明码垛式杆箱示意图。
22.图5是本发明钻杆分离控制单元示意图。
23.图6是3d相机识别抓取主程序流程。
24.图7是钻杆点云预处理和匹配识别过程。
25.图8是杆箱内钻杆码垛排布示意。
26.图9是机械臂和杆箱坐标示意。
27.附图标记含义：1.码垛式杆箱，2.3d相机，3.机械臂，4.磁吸手爪，5.钻杆分离控制单元，6.钻杆；11.方形框架，12.钻杆支承架，13.挡板，14.挡销，15.侧门，21.相机本体，22.举升梁，23.举升油缸，24.隔爆灯，41.手爪，42.永磁铁，43.接近开关。
具体实施方式
28.本发明提供一种煤矿井下码垛式杆箱钻杆分离系统与方法，钻杆分离系统包括码垛式杆箱、六自由度的机械臂、柔性的磁吸手爪、隔爆的3d相机和钻杆分离控制单元；机械臂及末端磁吸手爪、防爆3d相机之间借助传感器反馈和can总线、工业以太网、i/o线缆相互连接，利用预先设定多机协作和协调策略完成指定任务。钻杆分离控制单元包括控制层、机械臂控制器和视觉模块，具体涉及钻杆分离主程序、3d视觉识别程序、融合3d视觉和码放的钻杆分离程序、传感信号采集与故障诊断程序、机械臂控制程序、plc控制程序。钻杆分离方法是将3d视觉识别所需抓取钻杆位姿与码放理论计算位姿相融合，将两种方法优势互补，解决因煤矿井下环境复杂导致单一依靠3d视觉识别失败、分离失败的问题，可提高钻杆分离系统的鲁棒性。
29.钻杆分离系统具体实施为：将可升降式3d相机与机械臂组合构成机器人手眼系统，通过可升降式3d相机举升实现视野覆盖整个钻杆箱；根据永磁手爪吸附范围的要求，使用可码垛式钻杆分离装置将钻杆按照码放规则排布，利用视觉和码放算法融合对钻杆进行精确的识别与定位，完成杆箱内钻杆的全自动识别及定位抓取任务。
30.3d视觉识别算法具体过程为：为达到要求的视觉识别精度，通过对比视觉识别中的各种方法，优选出以下识别算法流程：对获取的点云进行背景分割、单根钻杆分割、单根钻杆点云去噪和拟合；在保持点云形状特征和确保点云数据处理速度情况下，采用下采样滤波精简点云。提取钻杆局部边缘特征，在从构建的曲线中计算表面法线，然后利用快速特
征直方图描述法线的偏差和角度，对模型点云和钻杆实际点云进行关键点提取及特征描述，最后经钻杆点云的粗、精匹配后获取钻杆位姿。
31.融合3d视觉和码放算法的码垛式杆箱钻杆分离方法具体实施为：通过示教获取第一根抓取钻杆与机械臂位姿关系，再按照码放原则，逐行逐列依次获得每一根钻杆理论推算位姿；抓取时判断理论和视觉识别位姿差值，当差值超过设定值，按照理论坐标抓取。当钻杆磨损后，通过视觉对理论位姿实时动态校准，纠正因钻杆磨损造成的码垛层高的错误，避免强光造成抓取位置错误，提高钻杆识别定位精度。
32.以下给出本发明的具体实施例，需要说明的是本发明并不局限于以下具体实施例，凡在本技术技术方案基础上做的等同变换均落入本发明的保护范围。
33.实施例1：本实施例提供一种煤矿井下码垛式杆箱钻杆分离方法，该方法采用3d视觉与码放算法相融合对码垛式杆箱内钻杆精确识别与定位后进行钻杆抓取分离；如图6至图9所示，包括以下步骤：步骤一、将码垛式杆箱内最下层第一行第一列钻杆作为示教钻杆记作a，记录钻杆a的位姿（x0,z0）以及机械臂和码垛式杆箱内钻杆的位置关系；步骤二、依据码垛式杆箱内钻杆的码放规则，逐行逐列计算每根钻杆理论推算位姿即钻杆的坐标；钻杆的行坐标、列坐标分别为：姿即钻杆的坐标；钻杆的行坐标、列坐标分别为：姿即钻杆的坐标；钻杆的行坐标、列坐标分别为：（1）上式中，i为行数，i=0对应第一行钻杆，j为列数，j=0对应第一列钻杆，杆箱内钻杆的层高为相邻奇数行和偶数行的钻杆中心竖向距离62mm，同一行的相邻钻杆中心横向距离为77mm，钻杆半径为36.5mm，钻杆a与杆箱侧壁的距离为75mm；在其他实施例中，如果钻杆或钻杆支承架尺寸改变，则用相应的层高、相邻钻杆之间的横向距离、钻杆半径和钻杆a与杆箱侧壁的距离等改变后的值替换上述公式1中的对应各值，即能得到相应的理论推算位姿；步骤三、利用3d相机视觉识别钻杆并得到钻杆的视觉识别位姿；具体的，3d视觉识别抓取主程序流程：首先3d相机拍照获得钻杆箱内钻杆点云信息，同时控制单元预先构建钻杆箱和钻杆的三维模型，通过三维匹配算法实现钻杆位姿的测量和识别定位，根据整个杆箱内钻杆抓取原则确定最优抓取点，结合手眼标定结果转换到机器人坐标系，利用tcp/ip协议发送至机械臂端，控制机械臂抓取钻杆。因永磁铁手爪上永磁体固有特性，在
±
2mm吸附距离内吸附力才能满足吸起钻杆的要求，否则无法实现吸附钻杆任务，为达到要求的视觉识别精度，通过对比测试视觉识别流程中的各环节方法，优选出识别算法中适合杆仓内钻杆识别的算法。步骤三包括：首先3d相机获取钻杆点云数据，为了保持点云的形状特征并提高钻杆点云数据处理速度，采用下采样滤波对钻杆点云进行精简预处理，再用iss特征检测算法提取钻杆局部点云数据边缘特征点，从获取的钻杆点云构建表面曲面，并在重建模型中计算表面法线，使用fpfh快速点特征直方图描述各个点的法
线的偏差和角度关系，采用sac-ia粗配准算法进行匹配，旋转平移点云得到初始变化矩阵；再进行icp精配准，首先假设一个初始位姿估计，从扫描点集中选设定数量的点，依据最小欧式距离，寻找与这些点距离最近的点集，计算变换矩阵r和t，改变视角后再扫描，重新关联点对重新计算转换矩阵，使对应点的点集间距离最小化，通过迭代计算，直到残差平方和所构成的目标函数值不变，即得到精匹配矩阵，完成精配准，从而通过3d视觉识别得到钻杆的视觉识别位姿。
34.本实施例将获取的整仓钻杆点云信息进行预处理，分割出单根钻杆，利用点云模型与钻杆三维建模模型的特征描述，识别目标点云中的钻杆，采用均方根误差法评价得到其匹配点对后获得这些匹配点对的最大距离、最小距离以及x、y、z的均方根误差，将这些参数作为配准算法的评价指标，三种配准算法配准精度评价结果见表1所示；从配准稳定性、配准效率以及配准精度三方面进行了对比，最终选用iss+fpfp+sac-ia+icp配准算法，实现钻杆识别及位姿测量。
35.表 1 三种配准算法误差评价对比步骤四、将步骤二中的理论推算位姿与步骤三中的视觉识别位姿进行对比，若理论推算位姿和视觉识别位姿的差值超过设定值，则将理论推算位姿作为抓取钻杆位姿，若差值未超过设定值，则将视觉识别位姿作为抓取钻杆位姿；步骤四中的设定值为钻杆半径；步骤五、将步骤四得到的抓取钻杆位姿信息送至钻杆分离控制单元中的机械臂控制器，机械臂动作，机械臂末端磁吸手爪上的接近开关判断磁吸手爪是否成功抓取钻杆，若已抓取，继续执行，若未抓取，返回执行步骤四，直至抓到钻杆；步骤六、当施工周期超过30天时，对理论推算位姿进行更新，完成钻杆磨损后抓取钻杆位姿的校正更新，然后继续执行步骤四进行钻杆抓取分离；步骤六中对理论推算位姿进行更新包括：选择满箱钻杆时更新，抓取最上层钻杆时，记录最上层所有钻杆的z向位姿即高度值，计算所有高度值的中位数，筛选出位于中位数加减1mm范围内的高度值，将选出的高度值平均后，再与z0求差后取均值，用该均值除以钻杆层数得到新的层高，即完成了钻杆磨损后抓取坐标的校正更新。
36.实施例2：本实施例提供一种煤矿井下码垛式杆箱钻杆分离系统，该分离系统能实现实施例1中的分离方法，如图1至图5所示，包括码垛式杆箱、设在码垛式杆箱上方的可升降式的3d相机、位于码垛式杆箱旁的六自由度的机械臂、连在机械臂末端的磁吸手爪以及钻杆分离控制单元，上述各部件均安装在车体平台上。
37.码垛式杆箱包括底部的方形框架、两个水平的钻杆支承架、挡板、可拆卸的挡销及两个侧门；方形框架包括两个端梁和两个侧梁，两个钻杆支承架相互平行且均垂直连接在两个侧梁之间，钻杆支承架上设有等间距排布的弧形槽以码放钻杆，钻杆垂直于钻杆支撑架放置，挡板竖向固定在钻杆支承架两端以围挡钻杆，挡销可拆卸设在两个挡板上端并与
钻杆支承架平行以防止整箱钻杆运输时震动引起钻杆掉落；在两个端梁上均安装侧门且侧门能竖向定位或侧向打开。
38.本实施例中，钻杆支承架设计有φ73钻杆和φ89钻杆适用的半圆弧接触面，目的是将钻杆箱内钻杆按照支承架设计的行、列间隙码放整齐，目的是理论计算各钻杆位姿；钻杆支承架能根据钻杆种类、大小和杆箱内钻杆容量要求进行更换，以适用不同钻杆的码放要求。侧门下端与端梁铰接以能向两侧打开，侧门在运输时围挡杆箱内钻杆，钻进时侧门打开以确保抓杆时不对钻杆长度方向形成干扰防止钻杆从杆箱内取出时掉落。码垛式杆箱可将长度1m的多种类型钻杆整齐、有序的码放在杆箱内，为后续拆垛抓取奠定基础。具体的，本实施例中，整个杆箱长1.15 m、宽1.1 m和高1.2 m。
39.3d相机的视野范围能覆盖整个码垛式杆箱区域，包括相机本体、连接相机本体的水平的举升梁、垂直连在举升梁下方的举升油缸以及隔爆灯；相机本体位于码垛式杆箱的正上方，隔爆灯设在举升梁上。举升油缸上还设有保护举升油缸胶管的钢制拖链；分离系统运输状态时，举升油缸降低将3d相机回收至码垛式杆箱内，分离系统工作状态时，举升油缸达到最大行程举升至最高以确保3d相机视野覆盖至码垛式杆箱内最顶层和最底层的每一根极限位置钻杆。
40.磁吸手爪包括手爪、永磁铁和接近开关；磁吸手爪利用永磁铁吸合来吸取和松开钻杆，取钻杆时，当钻杆远离杆箱内最顶层钻杆后，手爪夹紧钻杆防止碰到障碍物掉落，放钻杆时，钻杆靠近杆箱内最顶层钻杆时，手爪松开钻杆，以便永磁铁松开放下钻杆；接近开关用以判断是否吸取并夹紧钻杆，确保整个抓杆或放杆流程顺利进行。
41.钻杆分离控制单元包括控制层、机械臂控制器和视觉模块；其中：控制层用于控制手爪的张开闭合、磁铁吸合、3d相机的升降；具体的，控制层中的plc控制器通过读取指令，计算后发送给电磁阀组模块控制电比例阀实现磁铁吸合、手爪张开闭合和3d相机的举升动作，若遇紧急情况利用机械臂外部电路实现紧急停止；机械臂控制器能进行数据运算、i/o交互与数据采集，以控制机械臂动作；视觉模块用于接收3d相机获取杆箱内钻杆点云信息，并进行图像处理，将处理结果作为机械臂运动的决策依据。更具体的，通过隔爆键盘和鼠标对杆箱内第一根钻杆示教操作，通过3d相机获取钻杆箱内钻杆点云信息，并经过视觉模块的图像处理获取抓取钻杆信息，对钻杆和杆箱实现抓取位单根钻杆位置计算，确定抓取起始位置，结合码放规则位置自动规划分离时机械臂抓取路径，抓取过程中，机械臂控制器主要进行数据运算、i/o交互与数据采集；plc控制器通过读取指令，计算后发送给电磁阀组模块控制电比例阀实现磁铁吸合、手爪张开闭合和3d相机的举升动作，若遇紧急情况利用机械臂外部电路实现紧急停止。