双臂液压钻车的运动学分析及优化.pdf

2 0 1 4年 5月 第 4 2卷 第 9期 机床与液压 MACHI NE TO0L HYDRAUL I C S Ma v 2 0l 4 Vo 1 ． 4 2 No ． 9 DO I 1 0 ． 3 9 6 9 ／ j ． i s s n ． 1 0 0 13 8 8 1 ． 2 0 1 4 ． 0 9 ． 0 2 8 双臂液压钻车的运动学分析及优化 张付 祥 ，孙书鹏 - 河北科技 大学机械工程 学院，河北石 家庄 0 5 0 0 1 8 摘要为了实现双臂钻车的自动化打孔，对其进行运动学分析和优化计算。优化计算的目的是要找到液压钻臂各个关 节的合适的角度位置，能够在钻车工作时实现确定的位姿。采用粒子群算法在 M A T L A B 环境下计算并得出最优解，结果证 明粒子群算法能有效解决两臂钻车的运动学计算。为了方便操作和审核，建立了用户界面并生成了． e x e 文件 ，从而为钻车 自动化打孔奠定了技术基础。 关键词双臂钻车；运动学；粒子群算法 ；M A T L A B 中图分类号T H 1 1 3 文献标识码A 文章编号1 0 0 1 3 8 8 1 2 0 1 4 J 91 0 1 4 Ki ne ma t i c s Ana l y s i s a nd Op t i mi z a t i o n o f a Two- a r ul Hy d r a ul i c Dr i U M a n i p u l a t o r ZHANG F u x i a n g． S UN S h u pe n g C o l l e g e o f Me c h a n i c a l E n g i n e e r i n g ， He b e i U n i v e r s i t y o f S c i e n c e a n d T e c h n o l o g y ， S h i j i a z h u a n g H e b e i 0 5 0 0 1 8 ，C h i n a Ab s t r a c t I n o r d e r t o r e a l i z e d ri l l i n g h o l e s a u t o ma t i c a l l y b y a t w o a ml d r i l l i n g ma n i p u l a t o r ，i t s d i r e c t k i n e ma t i c s a n a l y s i s wa s d o n e ．T h e a i m o f o p t i mi z a t i o n wa s t o fi n d t h e fig h t p o s i t i o n a n d o rie n t a t i o n o f t h e a ／ I n b y wh i c h t h e y c o u l d r e a l i z e t h e p o s i t i o n a n d o r i e n t a t i o n o f t h e d ri l l d e ma n d e d o n t h e d ril l i n g w o r k ．A n o p t i mi z a t i o n p r o g r a m o f t h e P S O alg o ri t h m i n t h e MAT L AB e n v i r o n me n t wa s d e v e l o p e d a n d t h e o p t i mal c a l c u l a t i o n wa s d o n e ．T h e r e s u l t p r o v e s t h e v a l i d i t y o f t h e P S O b a s e d a l g o ri t h m f o r t h e i n v e r s e k i n e ma t i c s o f t h e 2 - a t T ll d r i l l i n g ma n i p u l a t o r ．I n o r d e r t o f a c i l i t a t e t h e o p e r a t i o n a n d a u d i t ， t h e u s e r i n t e r f a c e w a s s e t u p a n d ．e x e f i l e s we r e ma d e ，S O a s t o l a y fou n d a t i o n for d ri l l i n g h o l e s a u t o ma t i c a l l y ． Ke y w o r d s 2 - a r f n d ri l l i n g ma n i p u l a t o r ； K i n e m a t i c s ； P a r t i c l e s w a l l r l o p t i m i z a t i o n P S O ； MA T L A B 在发达国家，钻孔作业都早已完成了手工打孔向 全面机械化打孔的过渡⋯。在我国，虽然钻车也在迅 速发展，各式各样的钻车纷纷涌现 ，但是与国外的快 速发展以及高质量、高精度的钻车钻孔相比，还是比 较落后，主要是在质量、精度方面，因此对钻车进一 步的研究显得尤为重要。在此主要对钻车的运动学方 面进行 简要分 析 ，为实现钻 车的 自动化 打孔奠定 技术 基础。图I 所示为某煤矿机械厂 C M J 2 - 2 7全液压钻井 钻车结构简图 。 图 1 C MJ 2 - 2 7全液压钻车结构 1 运动学计算 此部分计算的目的是求解钻车手部期望位姿对应 的关节变量，手部位姿为 6个 自由度，需要 6轴联 l 一 履 带总 成 2 一 底 盘总 成 3 一 钻 臂总 成 4 一推进系统总成 s - _ 钎 具 6 “- - 动 力系 统 7 -- 电气 系统 8 一液压系统组成 动 。这里建立 C M J 2 - 2 7全液压钻车双臂运动学坐标 系和用 户坐标 系 ，如 图 2所示 。 收稿 日期 2 0 1 3 0 3 0 6 基金项目石家庄市科技发展计划项目重大专项 1 1 1 0 8 1 7 1 A一 1 ；河北科技大学杰出青年资助项目 作者简介张付祥 1 9 7 3 一 ，男，博士，副教授，研究方向为机电控制与机器人技术。E ma i l z h a n g f x h e b u s t ． e d u ． e n 。 1 O 2 机床与液压 图2 C M J 2 2 7全液压钻车的坐标系 第 4 2卷 考虑到研究的钻车为双臂操作，且两个机械结构 参数完全一致 。根据建立的坐标系确定钻车的 D H 基本一致，而且两个机械臂 D H参数中的常量部分 参数 ，如表 1所示。 表 1 液压钻车单臂各连杆 D． H参数数 f 钻车机械臂 坐标变换的矩阵 A表达式为 A R o t Z ， O i T r a n s 0 ， 0 ， d T r a n s a ， 0 , 0 R o t X ， A2 一 s O c f c O O l f SO t ． 0 s O s f c O S O ／ C 0 分别代表 s i n 0 和 c o s 0 ，简写后 a2c2 a 2 s2 0 1 A3 A4 A5 慝 0 0以 O 0 0 0 0 ]●●●●●●●●●●J Q， 0 0 4 0 0 0 0 0 1●● ●●●●●●●J ． ． C 口 哦 0 0 P．．．．．．．．．．。。．．．．．．．．．． L ● 用 1●●●， ● ●●● ●J 0 0以 0 O 0 0 1 0 ]● ，●●●●●●●J ．0 0 0 0 0 o 0 0 0 1 O 0 O 1 0 O O ． ． ． ． ． ． ． ． ． ． ． ． 。 。 ． ． ． ． ． ． ． ． ． ． ． ． ． ． ． ． ． ． ． L 第 9期 张付祥 等 双臂液压钻车的运动学分析及优化 1 0 3 A6 1 0 0 1 0 0 0 0 0 0 0 0 1 d 6 0 1 则用户坐标系到机械臂的变换矩阵为 A 其中用 户坐标系到左机械臂的变换式中 0 。 A T r a n s ，Y ， R o t z ，9 0 。 R o t Y ， r 0 1 0 X u ] 9 0 。、 0 0 1 Y 一 1 0 0 0 0 O l 直接看基座坐标系的位置和各坐标轴在用户坐标 系的投影 。考虑用 户坐标 系 ，建 立机械臂统一 的运 动学方程如下 T A A Al A2 A3 A4 A5 A6 0 y py a 0 0 l 0 s l C 2 C 3 s 4 2 C 4 一C 1 S 3 s 4 0 C 2C4一2 C3 4 0 c 1 C 2 e 3 s 4s 2 c 4 S 1 s 3 s 4 a 一s l e 2 c 3 c 4 5一C 2 S 3 C 5 一 2 s 4 S 5 C 1 S 3 e 4 s 5 a s2 e3c4 s5 一 s2 s3 c5 十 c2s 4s5 a 一c l C 2 C 3 C 4 S 5一C 2 S 3 e 5一 2 s 4 s 5 ，一S 1 S 3 c 4 s 5 e 3 c 5 P 一 S 1 C 2 C 3 C 4 s 5 d 6 s 1 C 2 C 3 c 4 a 4 一 S 1 e 2 c 3 s 4 d 5一 S 1 e 2 c 5 s 3 d 6一 S 1 S 2 S 4 S 5 d 6 一 l S 2 S 4 a 4 一 s l s 2 c 4 d 5 5 1 S 2 d 3 s l a 2 e 20 l s l e 1 c 4 s 3 s 5 d 6 一 c l c 4 s 3 a 4 c l s 3 s 4 d 5 一 C l c 3 C 5 d 6 P 5 2 C 3 C 4 5 5 d 6 a 4 5 2 c 3 4 3 2 C 3 5 4 d 5 5 2 5 3 C 5 d 6 C 2 S 4 S 5 d 6a 4 c 2 s 4d 5 c 2 e 4一c 2 d 3a 2 s 2Y u P 一 C 1 C 2 C 3 C 4 s 5 d 6C 1 c 2 e 3 c 4 a 4一e 1 C 2 C 3 s 4 d 5一 e l c 2 5 3 d 6一 。 l S 2 4 5 d 6 一 e I s 2 a 4 s 4 一 c 1 s 2 c 4 d 5 C 1 2 d 3 C 1 a2 c 2al c l s 1 c 4 s 3 5 5 d 6S I C 4 3 a 4一s I s 3 s 4 d 5 5 1 c 3 c 5 d 6 z 此结构型式的运动学逆解很难求取，经努力发现 并无解析解 ，经分析主要原因为 1 结构型式不 利于运动学求 解 ，连杆均 有扭 角 ； 2 有扭 转角 的 连杆长度不为零 ； 3 回转关节 处连杆 间距不 为零 。 其中第 1 条为关键因素，需要根据作业任务和运 动学求解要求重新设计机械臂的结构形式，第 2 、 3 条是次要因素，可简化运动学求解，但只改变 这些解 决不了根本问题 ，最终需要在改变结构型式 的 基础上尽量改善避免，第 2 、 3 条情况 出现。 因此如果想求取运动学逆解必须采取数值方法，这里 采用粒子群算法来求解。 2 粒子群算法 2 ． 1 粒子群简介 粒子群 P S O 算法是近年来发展起来的一种新 的进化算法。P S O算法属于进化算法的一种 ，和遗传 算法相似，它也是从随机解出发，通过迭代寻找最优 解 ，通过适应度来评价解的品质 ，通过追随当前搜索 到的最优值来寻找全局最优解。P S O初始化为一群随 机粒子 随机解 ，然后通过迭 代找到 最优解 。在每 一 次迭代 中，粒 子通过 跟 踪两个 “ 极值 ” 来 更新 自 己第一个就是粒子本身所找到的最优解，这个解叫 做个体极值 p B e s t ；另一个极值是整个种群 目前找到 的最优解，这个极值是全局极值 g B e s t 。另外也可 以 不用整个种群而只是用其中一部分作为粒子的邻居， 那么在所有邻居中的极值就是局部极值。 在找到这两个最优值时，粒子根据如下的公式来 更新 自己的速度和新的位置 [ ] [ ] e 1 r a n d p b e s t [ ] 一 p r e s e n t [ ] e 2 r a n d g b e s t [ ]一 p r e s e n t [ ] 1 p r e s e n t [ ] p e r s e n t [ ] [ ] 2 式 中 [ ] 是 粒子 的速 度 ，W是 惯性 权重 ，p e r s e n t [ ] 是当前粒 子的位置 ；p b e s t [ ] 和 g b e s t [] 如前 定义； r a n d 是介于 0 ，1 之间的随机数 ；c ， 、c 是学 习因子，通常 e ， c 2 。 2 ． 2粒 子编码 为了解决 ，应该正确地设计变量生成的代码用 于 S P O。根据算法的特点，可以直接使用真实的数字 来表示参数。使用 { 0 ，0 ，0 ，0 ，0 ，d }表 示粒子 当前 位 置 ，V { ， ， ， ， ， }表 示粒子当前速度 ，最终得出 的代码是 0 1 ， 0 2 ， 0 3， 0 4 ， 0 5 ， d 6 2 ． 3 粒子群适应值的计算 解决逆运动学方程是要找到一个正确 的关节位置 得到预期的位置与方 向。在粒子群算法的计算过程 中，每个粒子可以得到一个位置和方向，使用直接运 动学计算粒子 的理想位置 与方 向。可 以利用 函数 n O 1 T ／ l 计算两个向量的距 离位置，M A T L A B中计 算公式如下 s i mi l a r _pn o r m p t p u ／ m a x d i s t a n c e ； 参考两个空间向量的余弦定理⋯ 1 0 4 机床与液压 第 4 2卷 口 b ∞ 这里使用 s i m i l a r _ n ，s i m i l a r _ o和 s i m i l a r a表示粒 子实 际位置 、方 向与 预期 的关 系 ，得 MA T L A B表 达 式 如下 s i mi l a r _n1一 d o t n t ， n u ／ s q r t s k i m n t ．l n t s q r t s u m n u ． n u ； s i mi l a r O1一d o t o t ， o u ／ s q r t s L I m o t ．o t s q r t s u m o u ． o u ； s i mi l a ra1一d o t a t ， a u ／ s q r t s k i m a t ． a t s q r t s u m a l 1 ． a u ； 根据以上几个公式 ，得出适应值的表达式如下 fit n e s sc l爿 c s i mi l a r _nc 2水s i mi l a r Oc 3水s i mi l a r _a ．c 4 I； s i mi l ar _p 2 ． 4 粒子群的计算步骤 1 设置算法参数如大小、尺寸、数量、权重 因子 ，空 间位置 和速度空间 ； 2 设计相关 常量参 数 ； 3 初始化粒子群。根据 约束条件 ，在随机的 粒子初始化位置和速度在参数 区间内列 出方程 ； 4 计算粒子群的适应值； 5 初始化 当前 粒子 的位置 ，作 为 p b e s t 和 g b e s t 的值 ； 6 更新粒子的位置和速度 ； 7 再次计算粒子的适应值； 8 更新粒子 的p b e s t 和 g b e s t ； 9 重复步骤 6 一 8 ，直到迭代计算值达 到设定精度 ； 1 O 得到输出参数p b e s t 和 g b e s t 的实验结果。 3用户界面设计与 E X E文件生成 1 首先编辑生成 M文件 ，当然不止一个 M文 件 ，M文件中存在嵌套关系。 2 由于 M文件都是一堆代码，不便于阅读与 操作 ，出了问题也不便于修改、审核。为了使操作人 一 目了然，方便操作，在 已存在 M文件的基础上经 过一 系列 的操作 实 现 向 G U I 界面 的转 换 。生 成 的 用户界面不仅简单明了，而且方便操作，使操作工人 能很快上手，提高了工作效率 ，简单操作如下 在文 本框中输入理想位姿坐标参数及孔参数 ，点击计算 ， 就可 以在结果栏输 出结果 。生成 的用户 界面如图 3所 示 。 3 进行 G U I 转化成 ． e x e 文件步骤 ①安装编 译器 。可有多种选择 ，MA T L A B自带了一个 L C C ，作 者基于 V C 6 ． 0实现的；②设置编译器。在 MA T ． L A B命令行 输入 m b u i l d - s e t u p ，选择安装 的 C编译器 ； ③调用编译器。在命令窗 口输入 m c c - m r o b o t ． m，生 成 r o b o t ． e x e 、m c c E x c l u d e d F i l e s ． 1 o g以及 r e a d m e ． t x t 文 件；④到此生成了r o b o t ． e x e文件。 钻 车 计 算 一 _ l l l |_ ” ～ ⋯ 0 _ l l_ 一 ⋯ w ⋯0 一 l l l z z l 图3 双臂液压钻车运动学计算用户界面 4 E X E文件 的运行 。 要使 r o b o t ． e x e 文件顺利运行 ，首先需要安装一 个名为 M C R I n s t a l l e r ． e x e 插件，安装完成后 ，即可实 现操作，双击 r o b o t ． e x e 文件 ，输入数据，点击计算， 即可在结果栏输 出结果。若输入数据符合要求 ，则直 接输出结果；若输入数据不符合要求 ，则输出 b i g e r - r o r 提醒输入错误 。 4 计算结果与分析 为 了测试计算结果并进行数据分析 ，作者 输入 一 组数据 ，T 0． 7 0 7 1 0 ． 7 0 7 1 0 0 0 ． 7 0 7 1 0 ． 7 0 7 1 0 0 0 91 5 ． 4 4 0 2 3 4 2． 2 5 1 0 0 1 得出结果 0 1 4 ． 3 2 2 ，0 2 一 8 0 ． 4 5 6 9 ，0 3 4 5 ． 7 2 4 5 ， 0 4 8 0 ． 2 3 7 1 ， 0 5 8 5 ． 9 1 7 2 ， d 6 一1 9 0 7 ． 4 。为 了更 直观地展示 ，作者得 出适应值优化过程 如图 4所示 。 0． 8 0． 7 O． 6 托 O ． 3 0 ． 2 0． 1 图4 粒子群优化算法的优化过程 5结论 钻车的普遍应用使作者有了升级钻车的想法 ，当 然钻车 的高精度 自动化打孔是升级 的主要 目标 ，钻车 打孔精度的提高首先必须依赖高精度的运动学分析以 及粒 子群算法分析 ，在此基础上还要加 以简单易操作 的程序控制，从而实现理论误差、操作误差减低到最 下转第 1 2 3页 第 9期 袁利才一种乳化液泵的变量方式探讨 1 2 3 5 4 1 ． 6 9 L ／ m i n的 6 ． 2 ％范 围内波动 。 600 500 ．皇 4 0 0 3 0 0 巴2 0 0 删1 0 0 壤 0 0 ． 0 0 0 ． 1 0 0 ． 2 O 0 ． 3 0 0 ． 4 0 0 ． 5 0 时 间， s 图8 额定流量时，泵的流量曲线 3 ． 2流量 为额 定值 6 0 ％ 时的 流量 曲线 图 9所示为柱塞 1 2 5加载时 ，泵的流量特性 曲线 ，可 以看 出泵 的流量波动 比较大 ，最大流量达到 了 5 3 4 I ／m i n ，超 过 了理论 流量 3 2 5 L ／ m i n的 6 4 ％ ， 最小流量仅为 1 7 5 I ／m i n ，这是由于曲拐之间的固定 相位差所导致 的，这种情况下必须合理选用蓄能器来 对 流量脉动进行调节 。 在系统排液 口增设 4 L的蓄能器，其进油管径为 1 5 n l l T l 、长度为 0 ． 5 13 1 ，气体压力设置为2 1 M P a ，进行 仿真得到泵的流量曲线如图 l 0所示。流量在理论 流量 的 1 ． 3 ％范围内波动，其脉动对泵及系统的冲击很小。 6 0 0 5 0 0 ．1 4 0 0 3 0o 巴 2 0 0 蚓 1 0 0 爆 0 ．1 00 、 ．宝 昌 ● 删 煺 0． 0 0 0 ． 1 0 0． 20 0． 3 0 0 ． 40 0． 5 0 时 间， s 0 1 ． 0 2 ． 0 3 ． 0 4 ． 0 5 ． 0 6 ． 0 时间， s 图9 流量为额定值的 图 1 0 配置蓄能器后流量 6 0 ％时的流量曲线 为6 0 ％时的曲线 3 ． 3 流 量为 额定值 8 0 ％ 时的流 量 曲线 图 1 1 所示为柱塞 1 2 4 5 加载 时 ，泵的流量 特性 曲线 ，可以看出其 波动也很 大 ，最大流量 为 5 5 0 I ／ m i n ，最小流量为 1 6 5 I ／ m i n 。采用与第 3 ． 2节相 同 参数的蓄能器来重新进行仿真 ，其流量曲线如图 1 2 所示。在理论值 4 3 3 L ／ m i n的 1 ． 2 ％范围内波动，泵 的流量及压力脉动很小 。 60 0 50 0 ．量4 0 0 30 0 巴 2 0 0 10 0 爝 O 一 10 0 ．暑 皇 ● 删 媾 50 00 50 O0 50 0 0 50 0 0 5 O 0 5 0 0 ． 0 0 0 ． 1 O 0 ． 2 0 0 ． 3 0 0 ． 4 0 0 ． 5 0 0 ． 0 1 ． 0 2 ． 0 3 ． 0 4 ． 0 5 ． 0 6 ． 0 时间， s 时 间， s 图 1 1 流量为额定值的 图 1 2 配置蓄能器后流量 为8 0 ％时的曲线8 0 ％时的流量曲线 由上面 3 种情况分 析可知 当泵 变量工 作时 ，必 须配置合适 的蓄能器 ，这样就可 以很好地缓和流量及 压力脉动 ，使泵平稳运行 。 4结 论 通过以上分析及讨论可以得出如下结论 1 通过内卸载方式可以很方便地实现泵的变量控制，卸 载冲击小、结构简单 ； 2 通过仿真分析 ，可以得 出变量时最佳的柱塞加载工作方式，这有利于泵的长 期运行 ； 3 变量 时 ，泵 的流量 脉 动很 大 ，对泵 及 系统的运行非常不利，但配置合适的蓄能器后，其流 量及压力 脉动就会 控制 在合理 的范 围内 ； 4 在 井 下单泵流量满足最大流量要求，但又要求小范围变量 的时候可 以使用这 种变量 泵 ； 5 这种变 量泵再 通 过变 频 器 调 速 ，其 流 量 可 以 达 到 泵 额 定 流 量 的 3 3 ％ ～1 0 0 ％ ，变量范 围更 广 。总 的来说 ，这种 变 量 泵大大降低了系统成本及系统控制难度，有很好的推 广应用价值 。 参考文献 [ 1 ]商广辉， 戴陶珍． 乳化液泵站的变频控制系统[ J ] ． 煤矿 机械 ， 2 0 1 0 ， 3 3 1 5 21 5 4 ． [ 2 ]江玲玲． 基于 A ME S i m的液压系统动态特性仿真与优化 研究[ D] ． 绵阳 西南科技大学， 2 0 0 7 ． [ 3 ]李玉涛， 裴守智． 两级变量卧式三柱塞泵的研制[ J ] ． 煤 矿开采 ， 1 9 9 6 ， 9 3 2 6 2 7 ． 上接 第 1 0 4页 小 ，这才是作者所要达 到的终极 目标 ，对钻 车的运动 学分析 以及粒子群优化 ，使钻 车的精 度大大提高 ，增 强 了钻车 的竞争力 。 参考文献 [ 1 ]李同河 ， 路聚堂， 王振江． 液压掘进车岩掘进工程中的应 用[ J ] ． 江西煤炭科技， 2 0 0 7 4 3 33 4 ． [ 2 ]邹海波， 刘锋， 梁伟， 等． 煤矿用液压掘进钻车选型[ J ] ． 煤矿机械， 2 0 1 0， 3 1 8 2 0 4 2 0 6 ． [ 3 ]孙增圻． 机器人系统仿真及应用[ J ] ． 系统仿真学报 ， 1 9 9 5 ， 7 3 2 3 2 9 ． [ 4] D O L L A R H I D E R o b e L， A G A H A r v i n ．S i m u l a t i o n a n d C o n t r o l o f D i s t ri b u t e d R o b o t S e a r c h T e a ms 『 J ] ． Co mp u t e r s a n d E l e c t ri c a l E n g i n e e ri n g ， 2 0 0 3 ， 2 9 5 6 2 56 4 2 ． [ 5]Z H A O Q i n g j i e ， S U N Z e n g q i ． I m a g e b a s e d R o b o t M o t i o n S i m u l a t i o n [ J ] ． O p t i c s C o mm u n i c a t i o n s ， 2 0 0 2 ， 2 0 5 4 2 5 72 6 3 ． [ 6 ]周驰， 高海兵， 高亮， 等． 粒子群优化算法[ J ] ． 计算机应 用研究 ， 2 0 0 3 1 2 71 1 ． [ 7 ]方伟 ， 孙俊， 谢振平， 等． 量子粒子群优化算法的收敛性 分析及控制参数研究[ J ] ． 物理学报， 2 0 1 0 ， 5 9 6 3 6 8 7 3 6 9 3 ． [ 8 ]C O R K E P e t e r I ． A R o b o t i c s T o o l b o x f o r MA T L A B [ J ] ． I E E E R o b o t i c s a n d A u t o m a t i o n Ma g a z i n e ， 1 9 9 6 ， 3 1 2 4 3 2 ． 鲫 ∞ 蛐 ∞鲫 ∞ 如 O m