考虑变幅机构影响的钻机取杆机构轨迹规划

高志伟 ，李群明～3 ，周 围 ，谢 帅

（1.中南大学 机电工程学院，长沙 410083；2.中南大学 轻合金研究院，长沙 410083；3.中南大学 高性能复杂制造国家重点实验室，长沙 410083）

0 引言

在钻探过程中，由于受到钻杆长度的限制，需要人工加装和拆卸钻杆，这在很大程度上延长了钻孔作业时间，降低了施工效率[1]。因此，实现加装和拆卸钻杆的取杆机构自动化，可以降低工作人员的劳动强度、提高工作效率、保障施工人员安全。目前，王存洋、刘银等人针对水平钻孔的钻机研究了一种能够携带6根钻杆的钻杆库和取杆机械手[1]，沙永柏等人针对垂直钻孔的地面科学钻机设计了钻杆自动传送系统，包含一个五自由度取杆机械手[2]，罗华针对水平和倾斜钻孔的钻机设计了一种六自由度取杆机构[3]，阿特拉斯·科普柯Explorac100反循环钻机配置的取杆机构可以实现将钻杆夹取到垂直或倾斜位置桅杆处，但存在夹取钻杆时需要人工辅助定位和工作空间较小的缺点[4]。随着钻探技术的发展，实现钻进的自动化已成为现代钻机设计的主要标志之一[5]，实现取杆机构的自动化是实现钻机自动化的关键技术之一。为使取杆机构能够高效、稳定的自动加装和拆卸钻杆，需要对其运动轨迹进行规划。胡小平等人采用五次多项式和Neewton相结合的方法对多工位多节点的作业任务进行轨迹规划[6]，韩江等人利用高次多项式与B样条混合插值方法对机器人关节轨迹规划，使机器人运行更加平稳[7]，K.Petrinec等人采用五次多项式和四次多项式结合的方法，确保了机器人速度、加速度的连续性[8]，郝雪弟等人利用五次多项式插值法对运动路径规划，得到各关节的运动轨迹方程[9]，张蕾等人利用七次多项式插值法对运动路径进行规划，得到速度、加速度曲线没有突变[10]，林建雄等人分别利用九次多项式和三角函数对轨迹规划，得到的加加速度连续且有界[11]。

本文主要针对工程钻机钻探过程中工作范围小和加装钻杆自动化的需求，提出一种结合桅杆带有变幅机构的钻机取杆机构，实现自动化加装钻杆的工作和不改变钻机位置就能实现钻探不同位置的需求，增加了钻机的工作范围。在保证取杆机构末端手抓准确到达抓杆点的情况下，对变幅机构各油缸行程区间进行分析，确定变幅机构各油缸行程的适用范围；确定取杆机构各个关节运动顺序，避免取杆机构运动过程中与送杆机构和动力头相碰撞，对各个关节运动进行规划，并得到末端手抓运动轨迹光滑、连续。

1 钻机取杆机构工作原理：

取杆机构7安装在桅杆1一侧（桅杆与送杆机构之间），如图1所示。钻机在工作时，经履带底盘5运动到工作位置，由变幅机构8调节桅杆倾角和位置，使动力头2中心线对准孔口后固定不动。在自动控制模式下，送杆机构6依此将钻杆箱中的钻杆输送到固定位置给予取杆机构7的末端手抓抓取，经取杆机构各关节先后顺序运动将钻杆输送到双夹持器4内夹紧。

图1 钻机主要组成部分布置示意图

图2 变幅机构结构图

变幅机构主要由机架连接板4、连杆5、连杆7、连杆连接板8、桅杆滑移导轨11等主要结构组成，如图2所示。通过连杆油缸6、变幅油缸9、滑移油缸10的伸缩，伸缩连杆油缸6可以对桅杆上下微调和左右微调，伸缩变幅油缸9可以调节桅杆的倾角大小，伸缩滑移油缸10可以对桅杆上下位置进行调节。

钻机取杆机构结构如图3所示，取杆机构通过上底座2和下底座3安装在桅杆上。在摆动油缸1的作用下，取杆机构整体绕摆动油缸1中心旋转；液压马达9通过链轮链条机构带动末端手抓上下移动；旋转摆动油缸8控制左右末端手抓中心线与钻杆或双夹持器平行，方便抓取和夹持器夹持钻杆；旋转摆动油缸7控制左右末端手抓的开口方向；伸缩油缸6调节末端手抓的抓杆点；左右末端手抓夹持钻杆两个不同位置，保证钻杆移动过程中平稳。

根据上述分析，该取杆机构共有五个自由度，三个旋转自由度，两个移动自由度，分别由摆动油缸1、摆动油缸7、摆动油缸8、液压马达9、伸缩油缸6驱动。

图3 钻机取杆机构结构图

2 钻机取杆机构运动学建模

变幅机构各油缸伸长量变化，导致桅杆相对于机架的空间位置变化，进而影响取杆机构上下底座在空间中的位置。为得到取杆机构末端手抓相对于基坐标系的运动学模型。首先，以取杆机构上底座为研究对象，建立上底座坐标系相对于基坐标系的运动学模型；其次，建立取杆机构末端手抓中心坐标系相对于上底座坐标系的运动学模型；最后，得到取杆机构末端手抓相对于基坐标系的运动学模型。

根据D-H法建立坐标系的原则[12～14]，对坐标系的建立做如下规定：以变幅机构连杆6和机架连接板3在桅杆对称中心面上投影的交点作为基坐标系O'0x'0y'0z'0，取杆机构上底座2与摆动油缸1轴线在上底座2上下对称面上投影的交点作为取杆机构上底座坐标系O0x0y0z0，取杆机构末端手抓中心坐标系为O5x5y5z5，取杆机构末端手抓坐标系为OHnoa，连杆i的坐标系为Oixiyizi，连杆i的坐标原点位于关节i轴线与关节i+1轴线的交点处或两轴线公垂线与关节i+1轴线的交点处，连杆i的z轴与关节关节i+1轴线重合，连杆i的x轴在关节i和关节i+1轴线的公垂线上，方向指向关节i+1，连杆i的y轴由右手法则确定。

根据机器人学原理[15]，机械手抓坐标系相对与基坐标系可以用n、o、a、p来表示，如图4所示。其中，p表示手抓坐标系原点相对与基坐标原点位置的矢量；a称为接近矢量，表示z向矢量处于手抓进入物体的方向；0称为方向矢量，表示y向矢量的方向从一个指尖指向另一个指尖；n称为法线矢量，与a、o构成右手矢量集合，且由矢量叉乘规定：n=o×a。

图4 机械手抓坐标系示意图

2.1 取杆机构上底座相对于基坐标系的运动学模型

变幅机构可看成一个多关节机械臂[12]，利用上述规定，建立桅杆和变幅机构的各个关节坐标系，图5所示为桅杆和变幅机构D-H坐标系。其中：O'0A代表连杆油缸长度；O'0O'1、BC代表两连杆长度；四边形O'0ACO'2表示连杆连接板；CD表示变幅油缸的长度；CD'表示变幅油缸初始长度（行程为0），EF表示滑移油缸，d0为滑移油缸的长度。由变幅机构和桅杆的实际尺寸得到其D-H参数表，如表1所示，其中：θi为相邻两连杆的夹角；di为相邻两连杆距离；ai为连杆长度；αi表示连杆扭角，θ'i和θ'2均和连杆油缸长度有关。

图5 桅杆和变幅机构的D-H坐标系

根据坐标系图5中几何关系可得出：

表1 桅杆和变幅机构D-H参数表

在三角形O'0O'1A中，由余弦定理可求得：

三角形CO'2D'中，根据余弦定理求得：

在三角形CO'2D中，根据余弦定理求得：

将式(2)～式(4)代入式(1)中，可以得出关节变量θ与各油缸长度之间的关系。

将表1参数分别代入式(5)中，可得到取杆机构上底座相对于基座标系O'0的运动学模型：

2.2 取杆机构末端手抓中心相对于取杆机构上底座坐标系的运动学模型

利用D-H法建立坐标系的原则，建立取杆机构各个关节的坐标系，如图6所示；根据取杆机构实际尺寸参数，得出取杆机构D-H参数表，如表2所示。

图6 取杆机构D-H坐标系

表2 取杆机构D-H参数表

将表2参数分别代入式(5)中，得到取杆机构末端手抓中心相对于取杆机构上底座坐标系O0的运动学模型为：

式中：ci表示cosθi；si表示sinθi。

2.3 取杆机构相对于基坐标系的运动学模型

由图6中可知，取杆机构末端手抓中心坐标系绕自身y轴旋转90°，即可得到取杆机构末端手抓坐标系。绕当前坐标系y轴旋转θ角度的旋转矩阵可表示为：

将式(6)与式(7)相乘，并右乘矩阵(8)得到取杆机构相对于基坐标系的运动学模型：

式中：[nxnynz]、[oxoyoz]、[axayaz]分别为取杆机构末端手抓相对于基坐标系的姿态向量；[pxpypz]为取杆机构末端手抓相对于基坐标系的位置向量。

式(9)中：

取杆机构的运动学方程为：

3 变幅机构对抓杆点的影响及变幅机构油缸行程区间分析

从式(10)中可以看出，取杆机构末端手抓的姿态向量只与桅杆倾斜角度θ'3有关，即只与变幅油缸伸长长度有关。从式(11)中可以看出，取杆机构末端手抓的位置向量px、py与θ'1和θ'3有关，而pz与θ'1和θ'3无关。

3.1 变幅机构对抓杆点的影响

取杆机构工作时，送杆机构将钻杆输送到固定位置给予取杆机构抓取。抓杆时，取杆机构左右末端手抓中心线与钻杆平行，手抓开口方向指向钻杆，即取杆机构各关节变量θ为：θ1=90°；θ3=-θ'3；θ4=0°。可见，变幅机构对抓杆时姿态向量影响，反映在关节变量θ3上。将θ1、θ3、θ4值代入式(10)和式(11)中得到：

从式(12)得出，抓杆时px、py与变幅机构参数θ'1、θ'3和d0有关，pz为固定值。由于，送杆机构将钻杆送到固定位置，即钻杆在空间的位置一定，对于抓杆点py、pz值一定，为保证能将钻杆精确的送入夹持器中夹紧，因此，对抓杆时px位置有一定要求。

3.2 变幅机构油缸行程区间分析

由式(12)中px、py，得到d2和d5的表达式：

已知：O '0A=300 mm ， O '1O '0=860 ，O'2D'=1260mm；CD'=1000mm；CO'2=410.4mm。利用控制变量法，改变O'0A、O'2D'、CD的值代入式(1)、式(2)、式(3)、式(4)、式(13)中，求出对应的d2和d5的值，判断是否都在各自取值范围内，程序框图如图7所示。

其中：

353+841/sinθ'3>2158用来判断末端手抓上升出送杆机构的距离是否足够。

根据连杆油缸的行程0～190mm、变幅油缸行程0～572mm、滑移油缸0～800mm，得到O'0A的取值范围为：607mm～797mm；CD的取值范围为：997mm～1569mm；d0的取值范围为：1747mm～2547mm。

图7 变幅机构油缸行程区间计算流程图

保证能将钻杆精确的送入夹持器中夹紧的px范围为1032mm～1207mm，py=224mm。经过上述流程图计算可以得到，能够使取杆机构末端手抓正确抓取钻杆的变幅机构各油缸行程参数，如图8所示，由于计算出的数据比较大，图中仅给出能够使末端手抓正常抓取钻杆的各油缸行程极限值。

图8 变幅机构油缸行程参数图

图8中各名称代表的含义是：连杆油缸行程一定时，变幅油缸行程为L0时，对应滑移油缸行程最大值为L1，变幅油缸行程为L01时，对应滑移油缸行程最大值为L11。

从图8中可以看出：连杆油缸伸出长度一定时，变幅油缸伸出越长，滑移油缸行程可用区间越小；连杆油缸伸出越长，对应的滑移油缸和变幅油缸的行程可用区间就越小。

4 取杆机构运动过程分析与轨迹规划

通过对取杆机构末端手抓抓杆点的分析，得到变幅机构各油缸对应行程区间，但取杆机构在输送钻杆的过程中，受到钻杆长度的影响容易出现与动力头和送杆机构发生碰撞的现象，因此，还需对取杆机构的运动轨迹进行规划，以实现取杆机构在自主运动过程中精确加装和拆卸钻杆、不发生碰撞的功能，确定取杆机构运动过程，满足不同变幅机构状态下，钻杆无碰撞运动。

4.1 取杆机构运动过程分析

由图1及表2可以看出，由于送杆机构和动力头的位置以及各关节的运动范围限制了取杆机构的运动空间，且变幅机构影响取杆机构各关节变量值，因此，需要确定取杆机构运动过程。输送钻杆过程运动过程如图9所示，考虑变幅机构的影响，若桅杆处于倾斜状态，图9中钻杆状态2、3、4运动到与桅杆平行状态。其中：A表示旋转关节1，B表示移动关节2，C表示旋转关节3，D表示旋转关节4，E表示移动关节5，图中箭头为关节的运动方向。

图9 钻杆运动过程

根据实际工况，设定输送钻杆的时间为24s。根据图中钻杆的运动过程，将取杆机构的运动分为三个过程：

1）抓取杆件过程：取杆机构在抓取钻杆之后，末端机械手不能通过旋转进行姿态变换，因此，需要将末端机械手提升至送杆机构上方。钻杆状态从0～1，关节运动B1，B1上升到关节2的极限位置，运动时间设定0～4s。抓杆时手抓末端到送杆机构的最大距离为841mm，B1运动距离必须大于841mm。

2）输送杆件过程：将杆件从送杆机构上方，传递到动力头中心位置。钻杆状态从1-2-3。钻杆状态从1～2过程中，关节运动C和D，D运动时间设定4s～12s，C运动时间设定8s～12s；钻杆状态从2～3过程中，关节运动A和B2，A运动时间设定12s～20s，B2运动时间设定为14-18s。

3）夹持杆件过程：将杆件输送到夹持器中夹紧。钻杆状态从3～4，关节运动E，运动时间设定为20s～24s。

以变幅机构参数O'0A=611；CD=1569；d0=1747为例，即桅杆垂直（θ'3=90°）情况下，验证上述运动的正确性。

在该变幅机构参数下，末端机械手起始点设为：

末端机械手终止点设为：

通过运动学方程可以得出起始点和终止点各关节的参数：

起始点各关节参数：θ1=90°，d2=-1931mm，θ3=90°，θ4=0，d5=1343mm；

终止点各关节参数：θ1=90°，d2=-2158mm，θ3=0，θ4=180°，d5=885mm；

按照上述运动过程，得到末端手抓的运动轨迹如图10所示，图中0、1、2、3、4分别与图9中钻杆运动状态相对应。

图10 末端手抓运动轨迹

4.2 取杆机构轨迹规划

根据上一小节取杆机构运动过程的分析，取杆机构主要是将送杆机构送出的杆件输送到夹持机构夹紧，属于两点之间的运动，适合采用点到点（PTP）轨迹规划方法。五次多项式插值算法相比于三次多项式插值法、摆线法、三次B样条曲线，具有计算量适中、曲线平滑、较好的动态特性的优势[16]。因此，本文采用五次多项式插值法对取杆机构的运动进行规划，并利用取杆机构运动学方程，得到取杆机构末端机械手的运动轨迹。

1）五次多项式插值法轨迹规划

假设取杆机构某一关节开始运动时刻ti的角度值为θ1或位移值为di，速度为加速度为在终止时刻tf的角度值为θf或位移值为df，速度为加速度为可得到关节的初始和末端条件：

初始和末端条件可确定一个五次多项式方程：

将初始和末端条件代入上述方程，可解得五次多项式方程中的各项系数为：

将上述运动分析过程中的参数代入式(14)中，解出方程的各项系数，得到各关节的运动函数分别为：

2）仿真结果

图11 末端手抓的五次多项式插值运动轨迹

图12 钻杆空间运动轨迹y-z方向视图和x-y方向视图

从图11中可以看出，采用五次多项式插值法得到的末端机械手的运动轨迹平滑、连续。从图12中可以看出，y-z方向视图中0、1、2、3、4分别对应图9中钻杆的运动状态，钻杆状态从0～2过程中，关节运动B1、C、D使钻杆与动力头中心线平行，从图12中可以看出，运动过程中不会与送杆机构发生碰撞，同样，在变幅机构使桅杆倾斜情况下，关节运动C属于垂直状态下关节运动C的一部分，关节运动D与垂直状态下关节运动D相同，因此，运动过程也不会与送杆机构发生碰撞，钻杆运动状态从2～3过程中，桅杆倾斜和垂直，关节运动A、B2相同，图12中钻杆顶端与动力头之间的最小距离为200mm，满足不会与动力头发生碰撞要求。因此，按照上述运动过程对于不同变幅机构状态下取杆机构输送钻杆均不会与动力头和送杆机构发生碰撞。

5 结语

1）提出一种结合桅杆带有变幅机构的钻机取杆机构。通过D-H法，建立了取杆机构运动学模型，分析了能够使末端机械手正确到达抓杆点的变幅机构各油缸行程区间。仿真结果表明：连杆油缸伸出长度一定时，变幅油缸伸出越长，滑移油缸行程可用区间越小；连杆油缸伸出越长，对应的滑移油缸和变幅油缸的行程可用区间就越小。

2）对取杆机构的运动关节进行了轨迹规划，确定各关节运动的顺序。利用五次多项式插值法得到各关节的运动函数，代入取杆机构末端手抓的运动学方程中，得到末端手抓的运动轨迹。仿真结果表明：五次多项式插值法得到的末端手抓运动轨迹平滑、连续；并且，在钻杆运动过程中没有与送杆机构和动力头发生碰撞，满足不同变幅机构状态下的取杆机构运动。