CN114367979B - 一种矿用钻杆自动加接方法 - Google Patents

Abstract

本发明公开了一种矿用钻杆自动加接方法，主要包括一种自动配准算法、执行装置、机械手爪和辅助装置等。执行装置包括工业机器人、安装法兰、激光轮廓仪。辅助装置包括钻杆夹持器和钻杆安装架等。设定机器人的加接位置为初始状态，此时引入理论虚拟钻杆作为位姿基准。利用激光轮廓仪采集待抓取钻杆点云数据，重构三维表面轮廓，提取其几何特征，采用SAC‑IA+ICP三维点云配准算法，将其几何特征与加接位置处理论虚拟钻杆的几何特征进行匹配，获得钻杆当前位置与加接位置之间的坐标变换矩阵，利用此矩阵，基于机器人逆运动学原理求解机器人各关节运动量，控制机器人抓取当前位置处的钻杆并移动到加接位置处，实现钻杆的自动加接。

Description

一种矿用钻杆自动加接方法

技术领域

本发明属于自动化系统技术领域，具体涉及一种对矿用钻杆进行连续抓取安装的方法。

背景技术

随着我国经济社会的快速发展，石油等自然资源的供应需求日益增多，因此高效地开采石油才能可靠保证其正常供给。在石油开采时，矿井的深度长达数千米，而深井钻探用的单根钻杆的长度有限，需将钻杆沿轴向加接在一起以满足钻井深度要求。目前，钻杆的加接主要采用人工方式或者专门的钻杆安装装置辅以人工协助，不能实现钻杆抓取和安装的自动化。钻杆的数量多且重量大，降低了其安装过程的效率且增加了工作强度。另外，人工辅助也会降低石油开采过程的安全系数，增加劳动力成本。因此，钻杆的自动加接在深井钻探中非常关键。

本发明利用配有感知系统的工业机器人以识别钻杆的位姿并对钻杆进行抓取和安装，从而可靠地实现钻杆的自动加接，有效地提高钻杆的安装效率，节约劳动力成本。

发明内容

本发明的目的在于提供了一种矿用钻杆自动加接方法对蓝绿激光散射特性的计算方法，采用配有感知系统的工业机器人实现钻杆的自动化加接，以取代钻杆加接时的人工作业。

为实现上述目的，本发明提供如下技术方案：一种矿用钻杆自动加接方法，其具体步骤如下：

步骤1：首先在加工位置安装固定工业机器人、钻杆安装箱、钻杆安装架、激光轮廓仪；所述钻杆安装架上设置有钻杆夹持器、钻机；所述钻杆安装箱位于钻杆安装架的旁边；

步骤2：利用激光轮廓仪扫描待抓取钻杆，获取钻杆三维表面轮廓数据，以点云数据P＝{P_i|(x_i,y_i,z_i),i∈Z}描述钻杆三维表面轮廓和坐标位姿；其中χi、уi、zi为钻杆三维数据点坐标值；

步骤3：设定工业机器人处于加接位置为初始状态，提取引入的理论虚拟钻杆对应的点云数据Q＝{Q_i|(x_i,y_i,z_i),i∈Z}描述理论虚拟钻杆三维表面轮廓和坐标位姿；

步骤4：提取点云数据几何特征并采用SAC-IA+ICP三维点云配准算法，计算出待抓取钻杆相对于理论虚拟钻杆的坐标变换矩阵，即坐标系∑F相对于坐标系∑M的坐标变换矩阵即公式(1)：

上式中：

n＝(n_x,n_y,n_z)为∑F的X轴对∑M的三个坐标轴的方向余弦；

o＝(o_x,o_y,o_z)为∑F的Y轴对∑M的三个坐标轴的方向余弦；

a＝(a_x,a_y,a_z)为∑F的Z轴对∑M的三个坐标轴的方向余弦；

p＝(p_x,p_y,p_z)为∑F的原点在∑M中的坐标；

为∑F相对于∑M的旋转矩阵；

为∑F的原点在∑M中的坐标；

步骤5：利用公式(1)计算出的基于机器人逆运动学原理求解机器人各关节运动量(θ₁，θ₂，θ₃，θ₄，θ₅，θ₆)，即公式(2)：

公式(2)中，θ₁～θ₆为机器人各关节运动量；

为∑F相对于∑M的齐次坐标变换矩阵；

为∑Y相对于∑G的齐次坐标变换矩阵；

为∑J相对于∑Y的齐次坐标变换矩阵；

为∑Z相对于∑J的齐次坐标变换矩阵；

为∑W₁相对于∑Z的齐次坐标变换矩阵；

为∑W₂相对于∑W₁的齐次坐标变换矩阵；

为∑W₃相对于∑W₂的齐次坐标变换矩阵；

为∑M相对于∑W₃的齐次坐标变换矩阵；

步骤6：利用公式(2)得到机器人各关节运动量，控制机器人完成对钻杆的抓取；

步骤7：对公式(1)求得的矩阵进行求逆操作，得到加接位置处的理论虚拟钻杆相对于已抓取钻杆的位姿变换矩阵，即坐标系∑M相对于坐标系∑F的坐标变换矩阵/>即公式(3)：

上式中：

n＝(n_x,n_y,n_z)为∑M的X轴对∑F的三个坐标轴的方向余弦；

o＝(o_x,o_y,o_z)为∑M的Y轴对∑F的三个坐标轴的方向余弦；

a＝(a_x,a_y,a_z)为∑M的Z轴对∑F的三个坐标轴的方向余弦；

p＝(p_x,p_y,p_z)为∑M的原点在∑F中的坐标；

为∑M相对于∑F的旋转矩阵/>为∑M的原点在∑F中的坐标；

步骤8：利用公式(3)计算出的基于机器人逆运动学原理求解机器人各关节运动量(θ₁′，θ₂′，θ₃′，θ₄′，θ₅′，θ₆′)，即公式(4)：

公式(4)中，θ₁′～θ₆′机器人各关节运动量；

步骤9：利用公式(4)得到机器人各关节运动量，控制机器人将已抓取的钻杆移动到安装位置；

步骤10：钻杆夹持器夹紧钻杆，钻机旋转，实现钻机与钻杆顶端螺纹连接，完成钻杆安装，钻杆夹持器松开，钻机带动钻杆旋转，进行作业；

步骤11：重复步骤2～步骤10，对钻杆进行连续加接，直至满足钻井深度要求。

与现有技术相比，本发明的有益效果如下：

利用激光轮廓仪采集待抓取钻杆点云数据，重构三维表面轮廓，提取其几何特征，采用SAC-IA+ICP三维点云配准算法，将其几何特征与加接位置处理论虚拟钻杆的几何特征进行匹配，获得钻杆当前位置与加接位置之间的坐标变换矩阵，利用此矩阵，基于机器人逆运动学原理求解机器人各关节运动量，控制机器人抓取当前位置处的钻杆并移动到加接位置处，实现钻杆的自动加接。

配备有感知系统的工业机器人可以识别钻杆的位姿以及钻杆的自动抓取、安装，从而实现钻杆的自动加接，能够有效地提高钻杆的安装效率、节约劳动力成本以及降低工作风险。

附图说明

图1为本发明的总体方案示意图；

图2为本发明的机器人坐标系示意图；

图3为本发明的扫描钻杆示意图；

图4为本发明的抓取钻杆示意图；

图5为本发明的安装钻杆示意图；

图6为本发明的钻杆加接过程流程图；

图7为本发明的三维配准算法流程图；

图8为本发明的SAC-IA+ICP配准前后效果图。

图中，1.1-工业机器人，1.2-安装法兰，1.3-激光轮廓仪，2.1-连接基体，2.2-夹持手指，2.3-电磁铁，2.4-液压缸，3.1-钻杆，3.2-钻杆安装箱，3.3-钻杆夹持器，3.4-理论虚拟钻杆，3.5-钻杆安装架，3.6-钻机，机器人基坐标系为∑G，腰关节坐标系为∑Y，肩关节坐标系为∑J，肘关节坐标系为∑Z，腕关节坐标系为∑W₁，腕关节俯仰坐标系为∑W₂，腕关节坐标系为∑W₃，机械手爪坐标系为∑M，待抓取钻杆坐标系为∑F。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1-8本发明提供一种技术方案：一种矿用钻杆自动加接方法，所采用的设备包括执行装置、机械手爪和辅助装置等，执行装置包括工业机器人1.1、安装法兰1.2、激光轮廓仪1.3，工业机器人1.1为六个自由度，所述工业机器人1.1头部设置设置有机械手爪，机械手爪包括连接基体2.1、夹持手指2.2、电磁铁2.3和设置于连接基体上的液压缸2.4等，辅助装置包括钻杆安装箱3.2，钻杆夹持器3.3和钻杆3.1安装架3.5等，设定工业机器人1.1处于图4所示的加接位置为初始状态，此时引入理论虚拟钻杆3.4作为位姿基准。利用激光轮廓仪1.3采集待抓取钻杆3.1点云数据，重构三维表面轮廓，提取钻杆3.1几何特征，采用SAC-IA+ICP三维点云配准算法，将钻杆3.1几何特征与加接位置处理论虚拟钻杆3.4的几何特征进行匹配，获得钻杆3.1当前位置与加接位置之间的坐标变换矩阵，利用此矩阵，基于机器人逆运动学原理求解机器人各关节运动量，控制工业机器人1.1抓取当前位置处的钻杆3.1并移动到加接位置处，实现钻杆3.1的自动加接。

钻杆3.1的自动抓取和安装过程如图6所示，其具体步骤如下：

(1)安装固定工业机器人1.1、钻杆安装箱3.2和钻杆安装架3.5、激光轮廓仪1.3；

(2)如图3所示，利用激光轮廓仪1.3扫描待抓取钻杆3.1，获取钻杆3.1三维表面轮廓数据，以点云数据P＝{P_i|(x_i,y_i,z_i),i∈Z}描述钻杆3.1三维表面轮廓和坐标位姿；

(3)设定机器人处于图4所示加接位置为初始状态，提取引入的理论虚拟钻杆3.4对应的点云数据Q＝{Q_i|(x_i,y_i,z_i),i∈Z}描述理论虚拟钻杆3.4三维表面轮廓和坐标位姿；

(4)提取点云数据几何特征并采用如图7所示的SAC-IA+ICP三维点云配准算法，计算出待抓取钻杆3.1相对于理论虚拟钻杆3.4的坐标变换矩阵如图8所示，即坐标系∑F相对于坐标系∑M的坐标变换矩阵即公式(1)：

公式(1)中：

n＝(n_x,n_y,n_z)为∑F的X轴对∑M的三个坐标轴的方向余弦；

o＝(o_x,o_y,o_z)为∑F的Y轴对∑M的三个坐标轴的方向余弦；

a＝(a_x,a_y,a_z)为∑F的Z轴对∑M的三个坐标轴的方向余弦；

p＝(p_x,p_y,p_z)为∑F的原点在∑M中的坐标；

(5)利用公式(1)计算出的基于机器人逆运动学原理求解机器人各关节运动量(θ₁，θ₂，θ₃，θ₄，θ₅，θ₆)，即公式(2)：

(6)如图4所示，利用公式(2)得到机器人各关节运动量，控制工业机器人1.1完成对钻杆3.1的抓取；

(7)对公式(1)求得的矩阵进行求逆操作，得到加接位置处的理论虚拟钻杆3.4相对于已抓取钻杆3.1的位姿变换矩阵，即坐标系∑M相对于坐标系∑F的坐标变换矩阵即公式(3)：

式中：

n为∑M的X轴对∑F的三个坐标轴的方向余弦；

o为∑M的Y轴对∑F的三个坐标轴的方向余弦；

a为∑M的Z轴对∑F的三个坐标轴的方向余弦；

p为∑M的原点在∑F中的坐标；

(8)利用公式(3)计算出的基于机器人逆运动学原理求解机器人各关节运动量(θ₁′，θ₂′，θ₃′，θ₄′，θ₅′，θ₆′)，即公式(4)：

(9)利用公式(4)得到工业机器人1.1各关节运动量，控制工业机器人1.1将已抓取的钻杆3.1移动到安装位置；

(10)如图5所示，钻杆夹持器3.3夹紧钻杆，钻机3.6旋转，实现钻机3.6与钻杆3.1顶端螺纹连接，完成钻杆3.1安装，钻杆夹持器3.3松开，钻机3.6带动钻杆3.1旋转，进行作业；

(11)重复步骤(2)～(10)，对钻杆3.1进行连续加接，直至满足钻井深度要求。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (1)

1.一种矿用钻杆自动加接方法，其特征在于，其具体步骤如下：

步骤1：首先在加工位置安装固定工业机器人(1.1)、钻杆安装箱(3.2)、钻杆安装架(3.5)、激光轮廓仪(1.3)；所述钻杆安装架(3.5)上设置有钻杆夹持器(3.3)、钻机(3.6)；所述钻杆安装箱(3.2)位于钻杆安装架(3.5)的旁边；

步骤2：利用激光轮廓仪(1.3)扫描待抓取钻杆(3.1)，获取钻杆(3.1)三维表面轮廓数据，以点云数据P＝{P_i|(x_i,y_i,z_i),i∈Z}描述钻杆(3.1)三维表面轮廓和坐标位姿；其中χi、уi、zi为钻杆三维数据点坐标值；

步骤3：设定工业机器人(1.1)处于加接位置为初始状态，提取引入的理论虚拟钻杆(3.4)对应的点云数据Q＝{Q_i|(x_i,y_i,z_i),i∈Z}描述理论虚拟钻杆(3.4)三维表面轮廓和坐标位姿；

步骤4：提取点云数据几何特征并采用SAC-IA+ICP三维点云配准算法，计算出待抓取钻杆(3.1)相对于理论虚拟钻杆(3.4)的坐标变换矩阵，即坐标系∑F相对于坐标系∑M的坐标变换矩阵即公式(1)：

上式中：

n＝(n_x,n_y,n_z)为∑F的X轴对∑M的三个坐标轴的方向余弦；

o＝(o_x,o_y,o_z)为∑F的Y轴对∑M的三个坐标轴的方向余弦；

a＝(a_x,a_y,a_z)为∑F的Z轴对∑M的三个坐标轴的方向余弦；

p＝(p_x,p_y,p_z)为∑F的原点在∑M中的坐标；

为∑F相对于∑M的旋转矩阵；

为∑F的原点在∑M中的坐标；

步骤5：利用公式(1)计算出的基于机器人逆运动学原理求解机器人各关节运动量(θ₁，θ₂，θ₃，θ₄，θ₅，θ₆)，即公式(2)：

公式(2)中，θ₁～θ₆为机器人各关节运动量；

为∑F相对于∑M的齐次坐标变换矩阵；

为∑Y相对于∑G的齐次坐标变换矩阵；

为∑J相对于∑Y的齐次坐标变换矩阵；

为∑Z相对于∑J的齐次坐标变换矩阵；

为∑W₁相对于∑Z的齐次坐标变换矩阵；

为∑W₂相对于∑W₁的齐次坐标变换矩阵；

为∑W₃相对于∑W₂的齐次坐标变换矩阵；

为∑M相对于∑W₃的齐次坐标变换矩阵；

步骤6：利用公式(2)得到机器人各关节运动量，控制机器人完成对钻杆(3.1)的抓取；

步骤7：对公式(1)求得的矩阵进行求逆操作，得到加接位置处的理论虚拟钻杆(3.4)相对于已抓取钻杆(3.1)的位姿变换矩阵，即坐标系∑M相对于坐标系∑F的坐标变换矩阵/>即公式(3)：

上式中：

n＝(n_x,n_y,n_z)为∑M的X轴对∑F的三个坐标轴的方向余弦；

o＝(o_x,o_y,o_z)为∑M的Y轴对∑F的三个坐标轴的方向余弦；

a＝(a_x,a_y,a_z)为∑M的Z轴对∑F的三个坐标轴的方向余弦；

p＝(p_x,p_y,p_z)为∑M的原点在∑F中的坐标；

为∑M相对于∑F的旋转矩阵/>为∑M的原点在∑F中的坐标；

步骤8：利用公式(3)计算出的基于机器人逆运动学原理求解机器人各关节运动量(θ₁′，θ₂′，θ₃′，θ₄′，θ₅′，θ₆′)，即公式(4)：

公式(4)中，θ₁′～θ₆′机器人各关节运动量；

步骤9：利用公式(4)得到机器人各关节运动量，控制机器人将已抓取的钻杆(3.1)移动到安装位置；

步骤10：钻杆夹持器(3.3)夹紧钻杆(3.1)，钻机(3.6)旋转，实现钻机(3.6)与钻杆(3.1)顶端螺纹连接，完成钻杆(3.1)安装，钻杆夹持器(3.3)松开，钻机(3.6)带动钻杆(3.1)旋转，进行作业；

步骤11：重复步骤2～步骤10，对钻杆(3.1)进行连续加接，直至满足钻井深度要求。