[发明专利]一种煤矿井下码垛式杆箱钻杆分离系统与方法

权利要求书

1.一种煤矿井下码垛式杆箱钻杆分离方法，其特征在于，该方法采用3D视觉与码放算法相融合对码垛式杆箱内钻杆精确识别与定位后进行钻杆抓取分离；包括以下步骤：

步骤一、将码垛式杆箱内最下层第一行第一列钻杆作为示教钻杆记作A，记录钻杆A的位姿（x₀,z₀）以及机械臂和码垛式杆箱内钻杆的位置关系；

步骤二、依据码垛式杆箱内钻杆的码放规则，逐行逐列计算每根钻杆理论推算位姿即钻杆的坐标；钻杆的行坐标、列坐标分别为：

（1）

上式中，i为行数，i=0对应第一行钻杆，j为列数，j=0对应第一列钻杆，杆箱内钻杆的层高为相邻奇数行和偶数行的钻杆中心竖向距离62mm，同一行的相邻钻杆中心横向距离为77mm，钻杆半径为36.5mm，钻杆A与杆箱侧壁的距离为75mm；

步骤三、利用3D相机视觉识别钻杆并得到钻杆的视觉识别位姿；

步骤四、将步骤二中的理论推算位姿与步骤三中的视觉识别位姿进行对比，若理论推算位姿和视觉识别位姿的差值超过设定值，则将理论推算位姿作为抓取钻杆位姿，若差值未超过设定值，则将视觉识别位姿作为抓取钻杆位姿；

步骤五、将步骤四得到的抓取钻杆位姿信息送至钻杆分离控制单元中的机械臂控制器，机械臂动作，机械臂末端磁吸手爪上的接近开关判断磁吸手爪是否成功抓取钻杆，若已抓取，继续执行，若未抓取，返回执行步骤四，直至抓到钻杆；

步骤六、当施工周期超过30天时，对理论推算位姿进行更新，完成钻杆磨损后抓取钻杆位姿的校正更新，然后继续执行步骤四进行钻杆抓取分离。

2.如权利要求1所述的煤矿井下码垛式杆箱钻杆分离方法，其特征在于，所述步骤三包括：首先3D相机获取钻杆点云数据，为了保持点云的形状特征并提高钻杆点云数据处理速度，采用下采样滤波对钻杆点云进行精简预处理，再用ISS特征检测算法提取钻杆局部点云数据边缘特征点，从获取的钻杆点云构建表面曲面，并在重建模型中计算表面法线，使用FPFH快速点特征直方图描述各个点的法线的偏差和角度关系，采用SAC-IA粗配准算法进行匹配，旋转平移点云得到初始变化矩阵；

再进行ICP精配准，首先假设一个初始位姿估计，从扫描点集中选设定数量的点，依据最小欧式距离，寻找与这些点距离最近的点集，计算变换矩阵R和T，改变视角后再扫描，重新关联点对重新计算转换矩阵，使对应点的点集间距离最小化，通过迭代计算，直到残差平方和所构成的目标函数值不变，即得到精匹配矩阵，完成精配准，从而通过3D视觉识别得到钻杆的视觉识别位姿。

3.如权利要求1所述的煤矿井下码垛式杆箱钻杆分离方法，其特征在于，所述步骤四中的设定值为钻杆半径。

4.如权利要求1所述的煤矿井下码垛式杆箱钻杆分离方法，其特征在于，所述步骤六中对理论推算位姿进行更新包括：选择满箱钻杆时更新，抓取最上层钻杆时，记录最上层所有钻杆的Z向位姿即高度值，计算所有高度值的中位数，筛选出位于中位数加减1mm范围内的高度值，将选出的高度值平均后，再与z₀求差后取均值，用该均值除以钻杆层数得到新的层高，即完成了钻杆磨损后抓取坐标的校正更新。