双臂掘进钻车自动化钻孔方法研究.pdf

第 4 2 卷 第 6期 2 0 1 6年 6月 工矿 自 动化 I n d u s t r y a n d Mi n e Au t o ma t i o n Vo 1 ． 4 2 No ． 6 J u n ．20 1 6 文章编 号 1 6 7 1 2 5 1 X 2 0 1 6 0 6 0 0 4 2 0 4 D OI 1 0 ． 1 3 2 7 2 ／ j ． i s s n ． 1 6 7 1 2 5 1 x ． 2 0 1 6 ． 0 6 ． 0 1 1 张付祥 ， 史文军． 双臂掘进钻车 自动化钻孔方法研究[ J ] ． 工矿 自动化 ， 2 0 1 6 ， 4 2 6 4 2 4 5 ． 双臂掘进钻车自动化钻孔方法研究 张付 祥 ， 史文 军 河北科技大学 机械工程学院，河北 石家庄0 5 0 0 1 8 摘 要 在对 双臂掘 进钻 车进 行运 动 学分析 的基 础上 ， 分析 了钻 车 的 作 业能 力 和理 想 位 姿 ， 研 究 了双 臂 掘 进 钻 车的 钻臂之 间及 钻 臂 与巷 道 之 间 的碰 撞 检 测 方 法 ， 提 出 了 一 种 双 臂 掘 进 钻 车 自动 化 钻 孔 方 法 。在 Ma t l a b环境 下 对该 方法进 行 了仿 真 验证 ， 结 果表 明该 方法 能够使 钻臂 到 达钻 孔位 姿 ， 实现 自动 化 钻孔 ， 为进 一 步研 究双臂 掘进 钻 车钻臂 的运 动规 划及 自动 化钻 孔奠 定 了基 础 。 关键 词 双臂掘 进钻 车 ； 作 业能 力分析 ；理 想位姿 ； 碰 撞检 测 ；自动化 钻孔 中图分类 号 T D 4 2 1 文献 标志码 A 网络 出版 时 间 2 0 1 6 0 6 0 1 1 0 2 8 网络 出版地址 h t t p ／ ／ www． c n k i ． n e t ／ k c ms ／ d e t a i l ／ 3 2 ． 1 6 2 7 ． TP． 2 0 1 6 0 6 0 1 ． 1 0 2 8 ． 0 1 1 ． h t ml Re s e a r c h o n a n a u t o ma t i c dr i l l i n g me t ho d f o r d ua l a r m dr i l 1 r i g Z HANG Fu x i a n g， S HI W e n j u n Sc ho ol o f M e c ha n i c a l En gi n e e r i n g，He be i Uni v e r s i t y of Sc i e n c e a n d Te c hn ol o gy， S h i j i a z h u a n g 0 5 0 0 1 8 ，Ch i n a Ab s t r a c t Ba s e d o n k i ne m a t i c s a n a l ys i s o f d ua l a r m d r i l l r i g，o pe r a t i o n ab i l i t y a n d i d e a l po s i t i on o f du a l a r m dr i l l r i g we r e a na l y z e d，c o l l i s i o n de t e c t i o n me t hod s o f dua l a r m dr i l l r i g be t w e e n t he a r m s a nd be t we e n t h e a r m s a nd r oa dwa y we r e s t u di e d， a nd a n a u t o m a t i c d r i l l i ng m e t ho d f or du a l a r i m d r i l l r i g wa s pu t f or wa r d．A s i m u l a t i o n e xp e r i me nt wa s c a r r i e d o ut i n M a t l a b． The e xpe r i m e nt a l r e s ul t s ho ws t ha t t h e me t h od c a n ma ke t he a r m s o f d i r l l r i g a r r i v e t he i d e a l po s i t i o n a nd r e a l i z e a u t o m a t i c d r i l l i ng，whi c h l a y s a f o u n d a t i o n f o r f u r t h e r r e s e a r c h o n t r a j e c t o r y p l a n n i n g o f t h e a r ms o f d u a l a r m d r i l l r i g a n d a u t o ma t i c d r i l l i n g． Ke y wo r ds d ua l a r m dr i l l r i g；op e r a t i on a bi l i t y a na l y s i s；i de a l po s i t i o n；c ol l i s i o n d e t e c t i on；a ut o ma t i c dr i l l i n g 0 引言 中 国煤 矿 瓦斯爆 炸事 故频 发 ， 死亡人 数众 多 ， 瓦 斯事故已成为煤矿安全生产的最大威胁_ 1 ] 。如何实 现煤 矿高 效 、 高产 和安 全 已成 为 煤 矿 进一 步 发 展 的 关键和核心_ 2 ] 。高瓦斯矿井瓦斯防治管理的最大难 题 是 煤 层 瓦斯 的有 效 抽放 ] ， 而 释放 瓦 斯 的有 效 途 径是钻孔 。计算机控制的自动化钻孑 L 和钻爆精度 等问题的有效解决可提高瓦斯防治管理的安全性和 效率 。 本文 针对 C MJ 2 2 7改 进 型 全液 压 钻 井 钻 车 的 钻臂 机械 结 构进 行运 动 学分 析 ， 并 对 钻 车 的作 业 能 力 与理 想位 姿进 行 分 析 ， 提 出钻 车 自动 碰撞 检 测 方 法 ， 对 目前 常用 的拱形 巷 道爆 破方 案进 行 钻孔 规划 ， 为实现钻车 自动化钻孔奠定 了技术基础 。 1 钻 车钻 臂机 械 结构及 运 动学 分析 C MJ 2 2 7改进 型 全 液 压 钻 井 钻 车 在 作 业 过 程 中， 所有动作都由操作人员在驾驶室手工机械操作 ， 效率 低 ， 钻 孔 一致 性较 差 。为 了解决 这些 问题 ， 对 钻 收稿 日期 2 0 1 5 - 1 2 3 l ； 修回 日期 2 0 1 6 - 0 4 1 7 ； 责任编辑 李明。 基金项 目 河北省科 技攻关计 划资助项 目 1 4 2 1 1 9 0 2 D ； 河北省研究 生创新 资助项 目。 作者简介 张付祥 1 9 7 3 一 ， 男 ， 河北青县人 ， 副教授 ， 博 士， 主要研究方 向为机 电装备与机器人 ， E - ma i l z h a n g f x h e b u s t ． e d u ． c n 。 2 0 1 6 年 第 6期 张付 祥等 双臂掘进 钻 车 自动化 钻孔 方 法研 究 4 3 车进行 改 进 ， 使 其 具 备 数 控 功 能 。钻 车 钻 臂 结 构 如 图 1所示 。 l 2 3 1 一钻 杆 ；2 一 滑 架 ；3 一 推 进器 ；4 一摆 角 油 缸 ；5 一俯 仰 油 缸 ； 6 一 回转臂 ；7 一升降油缸；8 一整臂摆动油缸 图 I C MJ 2 2 7 改进 型全液压钻井钻车钻臂结构 钻 车为双 臂操 作 ， 且 2个 机 械臂 的结 构 基 本 一 致 ， 每 个 机 械 臂 有 5个 转 动 自由度 、 1个 移 动 自由 度 。在 2个机 械 臂 的各个 关节 上分 别建 立关 节 坐标 系 。关 节坐标 系建 立 规则 每 个 机 械臂 按 关 节 和 钻 车车体物理连接的先后次序在各个关节上依次建立 关 节 坐 标 系0 1 X1 yl Z l ，0 X2 y2 Z 2 ，0 3 X3 y 3 Z 3 ， 0 X y Z ， 0 x y z ， 0。 x y Z ， 其 中第 6个 自由 度 为钻 杆 的移 动 ， 坐标 系 0 X y Z 为建 立 在 钻 杆 末端 的钻杆 坐标 系 。 采 用 传 统 的 DH D e n a v i t Ha r t e n b e r g ma t r i x 法 对钻 车钻 臂进 行 正 向运 动 学分 析 ， 即 通过 已 知 的机 械臂 关 节坐标 求 取钻 杆坐 标 系的位 姿 。钻臂 的每个 连杆 上 都 固定一 个坐 标 系 ， 用 4 4的齐次 变 换矩阵来描述相邻 2个连杆的空间关系l 5 ] 。通过依 次 变换 可 最终 推 导 出钻 杆 相 对 于 基 坐标 系 的位 姿 ， 建 立 钻臂 的运 动学 方程 。 逆运 动 学 问 题是 已知 钻 杆 坐标 系 的位 姿 ， 求 相 应 的关节 坐标 。运 动学 逆解 解法 主 要有解 析 法 和代 数法 。代 数 法 工 作 量 大 ， 往 往 需 要 直 接 观 察 和 经 验 ] ， 而解 析法 可避 免这 个 问题 ， 因此 采用 解 析法 中 的分 离 变量法 求解 析解 。分离 变量 法首 先利 用运 动 方程 的 不 同形 式 ， 找 出矩 阵 中简 单 表达 某 个 未 知 数 的元 素 ， 力求 得 到未 知数 较少 的方 程 ， 然 后分 离 出变 量 ， 逐个 求解 。 双臂掘进钻车的运动学分析 目的是求解钻车钻 杆期 望 位姿对 应 的 关 节 坐标 ， 手 部位 姿 为 6个 自由 度 ， 需要 6 轴联动。钻孔作业采用计算机控制 ， 使其 能够 根 据作 业点 坐标 通过 逆 运动 学求 解关 节运 动控 制参 数 ， 从 而实 现钻臂 的全方 位数 字控 制作 业 。 2钻 车作 业能 力与 理想 位 姿分析 钻 车 钻 臂 的 空 间 分 析 与 机 器 人 的 空 间 分 析 类 似Ⅲ 。目前大多采用蒙特卡罗法求解机器人工作空 间， 其为利用随机抽样来求解数学问题 的一种数值 方 法 。 实 际工作 中 ， 最 为理 想 的状 态 是 理 想 钻 车位 姿 时钻孔 ， 即末 端 姿态 为钻 臂 垂 直 断 面 。利 用 Ma t l a b 可求得 钻 车距 离 断 面 1 0 1 0 mm 时 钻 孔 效 果 最 佳 ， 然 后 根据钻 臂垂 直 断面 的边界 条 件和最 佳距 离条 件 得 出理想状 态 工作 空 间 ， 如 图 2所 示 ， ， P 为 用 户坐标 系上 钻 孔 的坐标 。 5o0 O 40 0 O l 。 。 。 ＼ 2 0 o o 1 0 0 0 图 2 双臂掘进钻 车理 想状态工作空I 司 由图 2 可 知 ， 理想钻 车 位姿 钻孔 时 ， 内部包 含一 些空 心 区域 ， 即有 部分 孔位 置钻臂 达 不到 。另 外 ， 钻 车 2个钻 臂 的结 构完 全 相 同 ， 工 作 过 程 中有 相互 重 叠 的 区域 ， 在 同 时移 位 过程 中钻 臂 之 间可 能 产 生相 互碰 撞 。因此 ， 钻 车工 作 时要考 虑钻 车位 姿调 整 。 3 钻 车钻臂 碰 撞检测 方 法 在 实 际工 作 中 ， 由于 双臂 掘进 钻 车 的 串联 式 机 械 臂处 于动 态 的 、 不 确定 的非结 构 性环境 中 ， 碰撞 现 象 可能 经 常发生 。如何判 断 和避免 串联 式机 械臂 的 碰 撞 ， 对 于双臂 掘进 钻车 的实 际应 用非 常重 要 。 钻 臂碰 撞 检测 方法 ① 针对 钻 车 的工 作 情况 进 行 碰撞 检测 理论 分 析 ， 利 用机 器人 运动 学理 论 ， 推导 出各个 关 节 中心点 的坐标 表 达式 ， 求 得 任 意 2个 连 杆之 间 的最小距 离 ， 采 用 包 络 半 径 法对 2个 连 杆 之 间 的最小 距 离 与 2个 连 杆 的 包 络 半 径 之 和 进 行 比 较 ， 得到碰撞检测 方程 ； ② 针对单臂作业情况进行 分析 ， 利用 Ma t l a b工具 ， 结合蒙特卡罗法获取单机 械臂 作业 可能 发生 碰撞 的情 况 ； ③ 针 对 双臂 同时工 作 的情 况 进行 分 析 ， 利 用 Ma t l a b工 具 ， 结 合 蒙特 卡 罗法 获取 双臂 同 时作业 可能 发生 碰 撞 的情 况 ； ④ 针 对钻 车 2个机 械臂 钻杆 的 目标位 姿 ， 利 用 Ma t l a b工 具及 钻 车的解 析逆 解表 达式 获取 2个 机械 臂 的各个 4 4 工矿 自动 化 2 0 1 6年 第 4 2卷 关 节变 量 ； ⑤ 单 臂 作 业 时 ， 针 对 蒙 特 卡 罗 法 检 测 出 的单机 械臂 各连 杆 之问可 能发 生 的碰撞 情况 进行 单 臂碰撞 检测 ， 双 臂 同时作 业时 ， 针对 蒙特 卡 罗法检 测 出 的 2个 机械臂 各 连杆之 间可能发 生 的碰撞 情况 进 行双臂 碰撞 检测 。 在 钻 车实 际 工作 中 ， 钻 臂 与 巷道 也 可 能 发生 碰 撞 ， 加剧 钻臂 的磨 损 ， 需 尽量 避免 。钻 臂与 巷道 的碰 撞检测方法 采用两端具有半径 为 r的半球的圆柱 体将 连杆 包络 起来 r- ， 当钻车 钻臂 工作 时 ， 如 果 圆柱 体两端球心位置 即连杆关节坐标原点 不与巷道碰 撞 ， 则整 个 圆柱体 就不 会与 巷道 发生 碰撞 ， 即钻 臂连 杆 与巷 道之 间不 发生 碰撞 。因此 只要检测 关节 坐标 原 点是 否 与巷道 发生 碰撞 即可 。 通 过 分 析 ， 可将 关 节 坐标 原 点 是 否 与巷 道 发 生 碰撞 的问题 转换 为求 关节 坐标 原点 到巷 道 的最小 距 离问题 如果关节 坐标原点到巷道的最小距离大于 包络 连杆 的半 球 半 径 r ， 则 钻 臂 与巷 道 之 间 不 发 生 碰撞 ， 否则 发生 碰撞 。 在钻 车实 际 工作 中 ， 还 要 考 虑 实时 碰撞 检测 问 题 ， 需 给 出一个 安 全范 围 钻 臂 实 际工 作 时 与巷 道 壁的最小距离 。本文采用煤矿作业时最常用 的拱 形巷道 ， 巷道作业面宽度为 w， 高度为 H， 如图 3所 示。钻孔规划碰撞 检测时 一0 ， 实 时碰撞检 测时 ≠ 0。 图 3 巷道作业面结构 4 钻车 自动化钻孔规划方法 已知 钻 杆 坐 标 系0 6 x y Z 、 用 户 坐 标 系 0 X y Z ， 根据 钻孔 工艺 可确 定各 个钻 孔 在用 户 坐 标系上的坐标 户 ， P ， 而对于各孔的姿态确定 ， 理 论上 需要 满 足关 系 f 一 1 0 0 o 一 1 1 l a a 。 口 。一 1 式 中 ， ， 分 别 为 X 在 X ， y ， Z 轴 上 的 投 影 ； O ， O ， O 分 别 为 y 在 X ， y ， Z 轴 上 的 投 影 ； a ， a ， n 分别 为 Z 在 X ， y ， Z 轴 上 的投影 。 针对煤矿常见 的拱形巷道 ， 在作业面上建立用 户坐标 系 ， 如 图 4所 示 。0 在 底部 直 线 的 中点 ， X 在底部 向右 为正 ， y 在 作业 面 的左 右对 称线 上 向上 为正 ， Z 垂 直 于工作 面 向外 为 正 。确定 钻 臂 目标 姿 态前 需要 先找 几个 特征 孔 钻车初 始 位 ， 即钻 杆水 平 的理 想位 姿 时 ， 钻 头正对 工作 面 的 2个孑 L ； 最 上面 的 1个 孔 ， 最 下 面 的 3个 孔 ； 圆 弧 左 右 侧 4 5 。 处 2个 孔 。然 后将 所有 孔分 为 1 6个 区域 ， 其 中 y 轴左 侧 的孔 由左侧钻臂钻孔 ， y 轴右侧的孔由右侧钻臂钻 孔 ， y 轴上 的孔可 根据 情 况选 择 由左 侧或 右 侧钻 臂 钻 孔 。钻 孔方 案 如 图 4所示 。对这 1 6个 区域 分 别 设定钻杆末端的目标位姿 ， 见表 1 。 图 4钻 孔 方 案 5 计算 结 果与分 析 利用 某矿 用 8 8孔 方案 验证 理论 分 析结 果 ， 以其 中 1个 孔为 例进 行说 明。 首 先 根据 特 定 的 钻 孔方 案 获 取 钻 孔数 据 ， 即作 业孔的用户坐标 。取其中 1 个孔 ， 假定作业孔 在用 户 坐标 系 中的位 姿 为 T 一 O． 7 0 7 1 0． 7 07 1 0 0 O． 7 O 7 1 0． 7 07 1 O O T中第 1 列数据为作业孔 X 轴姿态 ， 第 2 列数 据 为作业 孔 y 。轴姿 态 ， 第 3列数 据 为 作业 孔 Z 轴 姿态， 第 4列数据为钻孔的用户坐标系坐标 。 假 定钻 车位 姿矩 阵 为 O O 5 6 0 1 3 O l l O O 1 O 2 0 1 6年 第 6期 张付 祥等 双 臂掘进 钻 车 自动化 钻孔 方 法研 究 4 5 A 0 ．9 9 7 3 0 ． 05 5 0 0． 0 49 5 一 O． O 5 2 3 0． 9 9 7 1 O． O 5 5 O O ．0 5 2 3 0． O 52 3 O ．9 9 7 3 0 0 0 1 0 1 0 1 O 1 将 数据 输入 程序 ， 得 关节 坐标 0 一 一1 5 ． 0 5 7 0 ， 一 一 9 5 ．9 02 0， 0 3一 一 1 3 7 ．6 77 2， 一 79 ．7 8 0 0， 0 5 一 一 9 7 ．2 1 3 1， d6一 一 1 9 8 4 ．1。 通 过 程 序计 算 出作业 孔 的关 节 坐标 后 ， 即可 通 过 控制 系统 控 制关节 运 动 ， 使 钻臂 到达 钻孑 L 位姿 ， 实 现 自动 化钻 孔 。 6 结 语 对 C MJ 2 2 7改进型全液压钻井钻车进行作业 能 力分 析 、 碰撞 检测 和钻 孔规 划 ， 得到 了 自动化 钻孔 方 法 ， 为进 一步 研究 双 臂 掘 进 钻 车 钻 臂 的 轨迹 规 划 奠 定 了 理论 基 础 ， 为 实 现 C MJ 2 2 7改 进 型 钻 车 自 动 化钻 孔奠 定 了技术 基 础 。 参 考 文 献 [ 1 ] 代勇． 煤 矿用 液压 钻车 钻臂 系统 的研究 [ D ] ． 洛 阳 河 南科技大学 ，2 0 1 2 ． [ 2 ] HA N S M，HwAN G Y K，w0 0 N S ，e t a 1 ．S o l i d - l i q u i d h y d r o d y n a mi c s i n a s l i m h o l e d r i l l i n g a n n u l u s [ J ] ． J o u r n a l o f P e t r o l e u m S c i e n c e a n d E n g i n e e r i n g ， 2 0 1 0，7 0 3 ／ 4 3 0 8 - 3 1 9 ． 柏思忠． 瓦斯抽 放管道设备 i a等级 防爆 型式探讨 [ J ] ． 工矿 自动化 ， 2 0 1 5 ， 4 1 5 5 5 5 7 ． 史建． 露天潜 孔钻 机钻孔 自动 定位 研究 [ D ] ． 长沙 中 南大学 ， 2 0 1 4 ． 苏学满 ， 孙丽 丽 ， 杨 明， 等． 基 于 ma t l a b的六 自由度机 器人运 动 特性 分 析 [ J ] ． 机 械 设 计 与 制 造 ， 2 0 1 3 1 78 - 8 0． 贺昱曜． 机器人 P UMA5 6 0逆运 动方程 的新 解法 [ J ] ． 机 器 人 ， 1 9 8 9， 1 0 3 1 9 - 2 6 ． 马小蒙 ， 沈惠平 ， 黄涛 ， 等． 冗余拟人 双臂机器 人的运动 学及工作 空 间分 析 E J ] ． 机 械科 学 与技 术 ， 2 0 1 4 1 1 25 29 ． 陈靖波 ， 赵猛 ， 张珩． 空间机 械臂在 线实 时避 障路 径规 划研究 [ J ] ． 控制工程 ， 2 0 0 7 ， 1 4 4 4 4 5 4 4 7 ． 姚 宁平 ， 张杰 ， 李泉新 ， 等． 煤矿井下定 向钻孔 轨迹设计 与控制技术 E J ] ． 煤炭科学技术 ， 2 0 1 3 ， 4 1 3 7 - 1 1 ． ] ] ] ] ] ] ] 【 l