钻杆排放机械手运动学分析及轨迹控制仿真

2014-03-18黄川兰凯袁焕宇陈凯凯

机床与液压 2014年21期

黄川，兰凯，袁焕宇，陈凯凯

(1.中国石油大学机电工程学院，山东青岛266580 ;2.中国化工集团桂林橡胶机械厂，广西桂林541002)

0 前言

传统钻机主要依靠二层台的井架工人手持管夹，通过体力劳动的方式对钻杆立根进行排放操作。这些钻机在日益要求健康、安全、环保的石油石化行业中，需要进行升级和改造，安装钻杆自动排放装置。钻杆自动排放装置是一种能够在井口与管架之间往返移送和存取钻杆立根的自动化设备［1－2］。目前，机械手式的排放装置以其结构紧凑、质量轻、灵活度高的优点，已被国外钻机广泛采用［3］。对此，经过初步探索和研究，设计了一种新型二层台舌台前置式钻杆排放装置(中国专利:201320019851.1)。作者针对该装置的总体方案及工作原理进行了简要介绍，并通过建立其运动学模型和雅克比矩阵，得到末端夹持器与动作关节之间的位置、速度变换关系，在此基础上利用Simulink 进行轨迹控制仿真，获取了相关运动学参数并验证了控制方法的可行性。

1 总体结构与工作原理

如图1所示，排放机械手结构上主要包括旋转臂、大臂、小臂、伸缩臂4 段手臂连杆，末端夹持器、扶正钳两个动力夹钳以及液压驱动器等部分。旋转臂悬挂于二层台下方，能以舌台为基座360°回转;大臂与旋转臂，小臂与大臂通过转动关节连接，最大转角范围35°;伸缩臂与小臂通过移动关节连接，最大伸长1.4 m。综上，排放机械手共有3 个旋转关节和1 个移动关节，每个关节都由液压执行器驱动，总共4 个自由度。

图1 钻杆排放机械手总体结构示意图

钻机二层台距钻台高24.5 ～26.5 m，与井架固连。如图2，钻杆排放机械手取钻杆作业示意图所示，排放机械手在取钻杆作业时，末端夹持器先以水平姿态夹持钻杆。接着，小臂内置伸缩缸单独动作将钻杆提升至适当高度。然后，所有关节按一定规律联动，使钻杆以竖直姿态沿着期望轨迹ABCD 运动出二层台(管架)并送至井口上方。最后，小臂下降，把钻杆交由其他装置处理。在整个运动过程中，扶正钳始终限制钻杆的水平偏移量，一直使AA'，BB'，…，DD'保持竖直。排放机械手存钻杆作业的动作流程与取钻杆相逆。

图2 钻杆排放机械手取钻杆作业示意图

2 建立位置关系

2.1 数学描述

采用D-H 方法［4］建立排放机械手的连杆坐标系如图3所示，D-H 参数见表1。

图3 钻杆排放机械手连杆坐标系

表1 钻杆排放机械手D-H 参数表

图3 中，1、2、3、4 表示手臂关节，其中1、2、3 为转动关节、4 为移动关节;①、②、③、④表示手臂连杆，依次表示旋转臂、主臂、副臂和伸缩臂。坐标系0、1、2、3、4 分别与基座①、②、③、④固连。

2.2 正向位置关系

建立排放机械手连杆坐标系后，由已知量θ =［θ1θ2θ3d4］T确定各连杆之间的变换矩阵i－1Ai(θi/di)(i=1，2，3，4)，将这些矩阵连乘得到排放机械手的运动学方程

A=0A1(θ1)1A2(θ2)2A3(θ3)3A4(d4)

A 即是包含了位置矢量(px，py，pz)和姿态矢量R 的信息的四阶方阵。由于该机械手的姿态矢量R可在其位置矢量已经确定的情况下由绕y4轴的俯仰角ωy唯一表示。由于绕z4轴的ωz与px、py、pz线性相关，绕x4轴的ωx受结构限制而恒为0，固可不建立它们的关系。因此，正向位置关系为

式中:s(i+ j)= sin(θi+ θj)，si= sinθi，c(i + j)=cos(θi+θj)，ci=cosθi。

2.3 逆向位置关系

采用几何法求解:首先根据px和py易求得θ1，然后利用ΔO1O2O'和ΔO2O3O4在空间中的几何关系列方程解出d4和θ2，最后易知θ3。求解过程中出现的重根由排放机械手的结构限制予以排除。得反向位置关系为

式中:ci=cosθi。

3 建立速度关系

由于排放机械手的工作区域受到钻杆、管架和舌台的限制，允许夹持器的运动空间非常小。因此，必须设计具有较高品质的控制系统，这就需要进一步分析其末端夹持器与动作关节之间的速度关系。

3.1 数学描述［5］

排放机械手的雅克比矩阵是它的末端夹持器速度与关节速度之间的线性变换，也是从关节空间到操作空间的传动比，其定义为

直接将式(5)—(8)代入上式求解并简化，得该机械手的雅克比矩阵J，此处把J 视为一个4 阶的方阵。

3.2 速度关系

排放机械手关节空间与操作空间的速度关系为

因此，只要J 可逆，在已知末端夹持器位姿和速度的情况下，利用式(9)即可求出关节速度。J 若不可逆，则说明末端夹持器处于该位姿则关节处于无法达到的位置或速度，这个位姿在轨迹规划时就应当予以避免。

4 轨迹控制仿真

4.1 轨迹控制原理

关节控制量的产生以式(9)为核心:先将反馈的关节实际位置经过正向位置关系式(1)—(4)转换成夹持器在操作空间中的位姿，再与期望的夹持器位姿进行比较，形成的偏差与逆雅克比矩阵相乘，得到关节的控制量。该运动控制方法的轨迹规划和关节控制都是在操作空间内进行的，因此具有比关节空间运动控制方法更加直接的特性［6］。图4 为利用Simulink在该运动控制方法下的运动学仿真框图:图中xyzpry_T轨迹期望模块把设定的位置期望px，py，pz和姿态量期望wy构成的夹持器期望位姿矩阵A*输出;tr_diff 位姿比较模块把由forward 模块反馈来的夹持器实际位姿A 与期望位姿A*进行比较，输出偏差值;Integrator 模拟关节运动模块将输入的关节速度积分得到关节的位置并反馈到Jacobian、Inv_ jacobian 模块求出雅克比逆矩阵J－1。期望为如图2所示的轨迹ABCD，其中AB、BC 直线轨迹各需时8 s，CD 圆弧轨迹需时6 s。仿真加入PID 控制器以提高控制精度。