二层台钻杆自动排放装置运动控制建模与仿真

二层台钻杆自动排放装置运动控制建模与仿真
黄 川， 姜菁杰 ， 兰 凯
） （ 山东 青岛 ２ 中国石油大学 机电工程学院 ， ６ ６ ５ ８ ０
／ 摘要 ： 建立了二层台钻杆自动 排 放 装 置 的 逆 运 动 学 、 逆 动 力 学 模 型， 并利用 Ｍ ａ ｔ ｌ ａ ｂ Ｓ ｉ ｍ ｕ ｌ ｉ ｎ ｋ进行 了运动 控制 仿真 。 结 果表明 ： 以前馈 力 矩 补 偿 结 合 Ｐ Ｄ 校正的复合控制方法能使装置的关节运动 快速收敛 于 期望 ， 静 态 误差较小 ， 适 用于二层台钻杆自动排放装置的运动 控制 。 关键词 ： 二层台 ； 钻杆 ； 排放 ； 运动 控制 中图分类号 ： Ｅ ９ ２ ８ 文献标识码 ： Ｔ Ａ
］ ［ ２ １ －
１ 排放方案及原理
图 １ 为设计的二层台钻杆自动排放装置的结构 方案及其工作原 理 。 装 置 结 构 主 要 包 括 旋 转 臂 、 主 臂、 副臂 、 伸缩臂 ４ 段 手 臂 和 主 爪 、 扶正爪２个手爪 以及液压 驱 动 器 等 部 分 。 旋 转 臂 悬 挂 于 二 层 台 下 回 转； 方， 能以舌台为基 座 ３ 主 臂 与 旋 转 臂、 副臂 ６ ０ ° ； 伸 与主臂通过转动关 节 连 接 ， 最大旋转角度为３ ０ ° 缩臂与 副 臂 通 过 移 动 关 节 连 接 ， 最 大 伸 出 长 度 １． ４ ｍ。 排 放 装 置 共 有 ３ 个 转 动 关 节 和 １ 个 移 动 关 节 ， 每个关节都由液 压 驱 动 器 驱 动 。 取 钻 杆 作 业 时 ， 主 爪首先在合适位置 抓 紧 钻 杆 ， 扶正爪稍后靠拢限制 钻杆的水平偏移量 ， 最后待移动关节动作提升钻杆
等： 二层台钻杆自动排放装置运动控制建模与仿真 ２ 卷 第 ７ 期 黄 川 ， 第４
·５·
至一定高度后 ， 手臂 ４ 个关节联动 ， 使主爪合成如图 １ 所示的运动轨迹 Ａ Ｂ Ｃ Ｄ。 装置其他工况的作业过 程以此类推 。 其中 １、 １、 ２、 ３、 ４ 为简化的手臂关节 ， 在图 ２ 中 ， ２、 ３ 为转动关节 ， ４ 为移动关节 ； ①、 ②、 ③、 ④ 为简化 的连 杆 ， 依 次 代 表 旋 转 臂、 主 臂、 副 臂 和 伸 缩 臂； 基 座、 连杆 ①～④ 分别 与 坐 标 系 ０、 １、 ２、 ３、 ４ 固 连。 由 Ｄ－Ｈ 描述方 法 及 装 置 的 结 构 特 点 容 易 得 到 其 逆 运 动学模型 。 ２． ２ 逆动力学建模
０ １ ３年 第４ ２卷 ２ 第７期 第４页 （ ） 文章编号 ： ０ ０ １ ４ ８ ２ ２ ０ １ ３ ０ ７ ０ ０ ４ ４ １ ３ ０ ０ － － －
石 油 矿 场 机 械 Ｉ Ｌ Ｆ Ｉ Ｅ Ｌ Ｄ Ｅ Ｕ Ｉ ＰＭＥ Ｎ Ｔ Ｏ Ｑ
（ ） ： ２ ０ １ ３， ４ ２ ７ ４～７

２ 建模方法
２． １ 逆运动学建模 ） 给定主爪在基 座 （ 即以 Ｏ ０ 为原点的坐标系０ ， 下的坐标值 （ 和俯仰角 ω 绕ｙ 求４个 ｘ， ｚ） ｙ， ４ 轴） ｙ（ Ｔ ［ ］ 关节的位置θ＝ θ 是运动学逆向问 ｄ θ θ １ ２ ３ ４ 题 。 采用 Ｄ－Ｈ 描述方法建立排放装置连杆坐标系 ， 如图 ２ 所示 。
（ ｏ ｌ ｌ ｅ ｅ ｅ ｃ ｈ ａ ｎ ｉ ｃ ａ ｌ ｎ ｄ ｌ ｅ ｃ ｔ ｒ ｉ ｃ ａ ｌ ｎ ｉ ｎ ｅ ｅ ｒ ｉ ｎ ｈ ｉ ｎ ａ ｎ ｉ ｖ ｅ ｒ ｓ ｉ ｔ ｅ ｔ ｒ ｏ ｌ ｅ ｕ ｍ， ｉ ｎ ｄ ａ ｏ ６ ６ ５ ８ ０， ｈ ｉ ｎ ａ） Ｃ ｏ Ｍ ａ Ｅ Ｅ Ｃ Ｕ ｏ Ｐ Ｑ ２ Ｃ ｇ ｇ ｇ， ｙ ｇ ｆ ｆ
图 ３ 二层台排放装置连杆受力分析
ｉ －１杆 作 用 到 杆 的 力 向 Ｏ ｆ ｉ －１ 点 简 化 为 ｆ ｉ（ ｉ＝
Ｔ ［ ］ 以此类 推） 和力矩ｎ ｉ＋１ 杆 作 用 ｆ ｆ ｆ ｘ ｚ） ｉ； ｙ
到ｉ 杆的作用力 按 照 上 述 表 示 方 法 向 Ｏｉ 点 简 化 为 重 心 为 Ｃ， －ｆ ｎ ｉ 杆受重 力 ｍｉ Ｏ ｇ， ｉ ＋１ 和力矩 － ｉ ＋１ 。 ｉ 到重 心 Ｃ 的 矢 径ｒ Ｃ， Ｏ ｃ ｉ ＝Ｏ ｉ ｉ －１ 到 Ｏ ｉ 的 矢 径ｐ ｉ＝ 并视排放装置为多 刚 体 系 统 。 Ｏ Ｏ ｉ －１ ｉ。 不考虑摩擦 ， 采用牛顿 －欧拉迭代算法建立其逆动力学模型为 （ ） １ 式中 ： Ｄ（ ｈ（ θ） ∈Ｒ 为 表 示 惯 性 的 矩 阵 ； θ， θ） ∈Ｒ 为 表示离心力和哥氏力的向量 ； Ｇ（ θ） ∈Ｒ 为表示重力 的向量 。
［］ 调用 Ｍ ａ ｔ ｌ ａ ｂ Ｒ ｏ ｂ ｏ ｔ ｉ ｃ Ｔ ｏ ｏ ｌ ｂ ｏ ｘ４ 正 动 力 学 库 函 数 所
线计算 ， 但随着控制部件的进步和发展 ， 其优势已逐 渐凸显 。 设定手爪负载为１ 从如图 １ 所示的点Ａ＝ ５ｋ Ｎ， （ ） 以正弦加速度规律跟踪一 １． ８ ５ ０， １． ５ ５ ０， －５． ８ ０ ０ ） ， 历 段直线轨迹运 动 至 点Ｂ＝ （ ０． ３ ５ ０， １． ５ ５ ０， ５． ８ ０ ０ 。 建立的 仿 真 框 图 如 图 ４ 所 示 ， 时８ｓ 图 中 ｆ（ ｕ） ＝ （ ／ ／ ／ （ ） ， １． ８ ５－１． ５（ ｕ ８） －ｓ ｉ ｎ（ ２ ｕ ８） ２ Ｋｐ ＝ｄ ｉ ａ π π） ｇ ［ ， ， 初始 ２ ５ ２ ５ １ ５ ３ ５］ Ｋｄ ＝ｄ ｉ ａ ８ ８ ６ １ ０］ ｇ［
Ｍ Ｂ ｏ ｔ ｉ ｏ ｎ Ｃ ｏ ｎ ｔ ｒ ｏ ｌ Ｍ ｏ ｄ ｅ ｌ ｉ ｎ ａ ｎ ｄ Ｓ ｉ ｍ ｕ ｌ ａ ｔ ｉ ｏ ｎ ｏ ｆ Ｍ ｏ ｎ ｋ ｅ ｏ ａ ｒ ｄ Ｐ ｉ ｅ Ｒ ａ ｃ ｋ ｉ ｎ Ｄ ｅ ｖ ｉ ｃ ｅ － ｇ ｙ ｐ ｇ
， ， ｉ ＨＵＡＮＧ Ｃ ｈ ｕ ａ ｎ Ｊ Ｉ ＡＮＧ Ｊ ｉ ｎ ｅ Ｌ ＡＮ Ｋ ａ ｉ － ｊ
Ｔ 给定 ４ 个关节的位置θ（ ｄ θ＝ ［ θ θ θ １ ２ ３ ４］
、 以此类 推 ） 角 速 度 求关节的驱动 θ 和 角 加 速 度¨ θ， 力矩τ 是动力学逆向问题 。 对排放装置连杆进行受 力分析 ， 如图 ３ 所示 。
１— 舌台 ； ２— 二层台 ； ３— 指梁 ； ４— 钻杆立根 ； ５— 旋转臂 ； ６— 扶正爪 ； ７— 主臂 ； ８— 副臂 ； ９— 伸缩臂 ； １ ０— 取钻杆轨迹 ； １ １— 主爪 图 １ 二层台钻杆自动排放装置取钻杆作业原理
块， 它通过运动学反 解 模 型 将 输 入 的 轨 迹 期 望 映 射
·６·
０ １ ３年７月 石 油 矿 场 机 械 ２
至４个 关 节， 将 关 节 角 位 置 期 望 输 出； ｉ ｎ ｖ＿ ｄ ｎ ａ ｍ ｉ ｃ ｙ 是动力学逆解模块 ， 它读入角度 、 角速度和角加速度 值， 通过逆动力学模型反解出关节力矩 ； Ｒ ｏ ｂ ｏ ｔ是排 放装置动力学行为 模 拟 模 块 ， 它是通过输入排放装 质量特性和关节摩擦等参数后 ， 直接 置的结构尺寸 、
， ： Ａ ｂ ｉ ｅ ｒ ａ ｃ ｋ ｉ ｎ ｄ ｅ ｖ ｉ ｃ ｅ ｉ ｎ ｂ ｓ ｔ ｒ ａ ｃ ｔ Ｉ ｎ ｏ ｒ ｄ ｅ ｒ ｔ ｏ ｄ ｅ ｖ ｅ ｌ ｏ ｔ ｈ ｅ ｍ ｏ ｔ ｉ ｏ ｎ ｃ ｏ ｎ ｔ ｒ ｏ ｌ ｓ ｓ ｔ ｅ ｍ ｏ ｆ ｍ ｏ ｎ ｋ ｅ ｏ ａ ｒ ｄ － － ｐ ｐ ｇ ｐ ｙ ｙ ｖ ｅ ｒ ｓ ｅ ｋ ｉ ｎ ｅ ｍ ａ ｔ ｉ ｃ ｓ ａ ｎ ｄ ｉ ｎ ｖ ｅ ｒ ｓ ｅ ｄ ｎ ａ ｍ ｉ ｃ ｍ ｏ ｄ ｅ ｌ ｏ ｆ ｔ ｈ ｅ ｄ ｅ ｖ ｉ ｃ ｅ ａ ｒ ｅ ｅ ｓ ｔ ａ ｂ ｌ ｉ ｓ ｈ ｅ ｄ ． Ｔ ｈ ｅ ｍ ｏ ｔ ｉ ｏ ｎ ｃ ｏ ｎ ｔ ｒ ｏ ｌ ｓ ｉ ｍ－ ｙ ／ ｕ ｌ ａ ｔ ｉ ｏ ｎ ｈ ａ ｓ ｂ ｅ ｅ ｎ ｃ ｏ ｍ ｌ ｅ ｔ ｅ ｄ ｂ ｕ ｓ ｉ ｎ Ｍ ａ ｔ ｌ ａ ｂ Ｓ ｉ ｍ ｕ ｌ ｉ ｎ ｋ． Ｔ ｈ ｅ ｓ ｉ ｍ ｕ ｌ ａ ｔ ｉ ｏ ｎ ｒ ｅ ｓ ｕ ｌ ｔ ｓ ｓ ｈ ｏ ｗ ｔ ｈ ａ ｔ ｔ ｈ ｅ ｃ ｏ ｍ－ ｐ ｙ ｇ ｏ ｕ ｎ ｄ ｃ ｏ ｎ ｔ ｒ ｏ ｌ ｍ ｅ ｔ ｈ ｏ ｄ ｗ ｈ ｉ ｃ ｈ ｃ ｏ ｍ ｂ ｉ ｎ ｅ ｄ ｉ ｎ ｖ ｅ ｒ ｓ ｅ ｄ ｎ ａ ｍ ｉ ｃ ｆ ｅ ｅ ｄ ｆ ｏ ｒ ｗ ａ ｒ ｄ ｗ ｉ ｔ ｈ Ｐ Ｄ ｆ ｅ ｅ ｄ ｂ ａ ｃ ｋ ｃ ａ ｎ ｍ ａ ｋ ｅ ｐ ｙ ， ’ ｏ ｉ ｎ ｔ ｓ ｈ ａ ｖ ｅ ｓ ｍ ａ ｌ ｌ ｅ ｒ ｔ ｒ ａ ｃ ｋ ｏ ｉ ｎ ｔ ｓ ｃ ｏ ｎ ｖ ｅ ｒ ｅ ｔ ｏ ｅ ｘ ｅ ｃ ｔ ａ ｔ ｉ ｏ ｎａ ｎ ｄ ｔ ｈ ｅ ｔ ｈ ｅ ａ ｃ ｔ ｕ ａ ｌ ｍ ｏ ｔ ｉ ｏ ｎ ｏ ｆ ｔ ｈ ｅ ｄ ｅ ｖ ｉ ｃ ｅｓ － ｊ ｊ ｇ ｐ ｉ ｎ ｅ ｒ ｒ ｏ ｒ ． Ｔ ｈ ｉ ｓ ｃ ｏ ｎ ｔ ｒ ｏ ｌ ｍ ｅ ｔ ｈ ｏ ｄ ａ ｌ ｉ ｅ ｓ ｔ ｏ ｔ ｈ ｅ ｍ ｏ ｔ ｉ ｏ ｎ ｃ ｏ ｎ ｔ ｒ ｏ ｌ ｏ ｆ ｔ ｈ ｅ ｄ ｅ ｖ ｉ ｃ ｅ ． ｇ ｐ ｐ ： ； ； ； ｂ ｗ ｏ ｒ ｄ ｓ ｍ ｏ ｎ ｋ ｅ Ｋ ｅ ｏ ａ ｒ ｄ ｉ ｅ ｒ ａ ｃ ｋ ｉ ｎ ｍ ｏ ｔ ｉ ｏ ｎ ｃ ｏ ｎ ｔ ｒ ｏ ｌ － ｙ ｐ ｐ ｇ ｙ 钻台 二层台自动排 放 装 置 主 要 在 井 口 与 指 梁 （ 立根盒 ） 区 域 之 间 自 动 往 返 移 送 和 存 取 钻 杆 立 根。 此外 ， 它还 可 以 与 铁 钻 工 、 液压吊卡等装置配合工 作， 完成单根 接 立 根 作 业
。 由 于 受 到 指 梁、 舌台
源自文库
和井架的限制 ， 二层台排放装置的运动空间狭小 ， 安 全风险大 。 为此 ， 必须使其运动轨迹精确 ， 否则将会 ， 引起钻杆与指梁 的 擦 碰 ， 造 成 事 故。 根 据 文 献 ［ ３］ ｋ ｅ ｒ ＭＨ 公 司 已 开 发 出 机 器 人 运 动 控 制 器 Ａ ， （ 该控 制 器 可 以 使 排 放 装 置 安 全 、 高效地带 ＲＭＣ） 动管具沿着预定路径移动 。 为了寻找适用于排放装 置的控制方法 ， 本文以设计方案为分析对象 ， 建立了 ／ 其逆运动学 、 逆 动 力 学 模 型， 并在 Ｍ ａ ｔ ｌ ａ ｂ Ｓ ｉ ｍ ｕ ｌ ｉ ｎ ｋ 。 环境下进行了运动控制的建模与仿真
２ ０ １ ３ １ １ ０ １ ① 收稿日期 ： － － ， ： 黄 川（ 男， 广西钦州人 ， 硕 士 研 究 生， 主要从事钻机自动化设备的设计工作， １ ９ ８ ８ Ｅ－ｍ ａ ｉ ｌ ｈ ｕ ａ ｎ ｃ ｈ ｕ ａ ｎ ｕ ｃ 作者简介 ： ＠ －） ｇ ｐ １ ６ ３． ｃ ｏ ｍ。
¨ ｈ（ ＋Ｇ（ τ＝Ｄ（ θ） θ＋ θ， θ） θ）
３ Ｓ ｉ ｍ ｕ ｌ ｉ ｎ ｋ 仿真
３． １ 主要模块的建立 排放装置的控制仿真框图 （ 省略结果获取部分 ） 如图 ４ 所 示 。 其 中 ： ｉ ｎ ｖ＿ ｋ ｉ ｎ ｅ ｍ ａ ｔ ｉ ｃ是 轨 迹 期 望 模
图 ２ 二层台排放装置连杆坐标系