双臂液压钻车的逆运动学求解.pdf

第 8期 2 0 1 5年 8月 机 械 设 计 与 制 造 Ma c h i n e r y De s i g nMa n u f a c t u r e 1 4 7 双臂液压钻车的逆运动学求解 张付祥 ， 史文军 河北科技大学 机械工程学院， 河北 石家庄0 5 0 0 1 8 摘要 在我国统配煤矿和新并建设中， 每年有大量的岩石巷道开拓与掘进工作 ， 掘进装备的 自动化程度 已成为制约 煤矿提高生产能力的主要因素。为了提高液压钻车的自动化水平， 实现双臂液压钻车 自动化打孔， 通过对其进行运动 学分析， 基于 D H法建立了双臂液压钻车运动学方程， 采用分离变量法并用符号替代矩阵元素推导出了运动学方程 逆解的表达式。 最后在 Ma t l a b 环境下， 通过 8 8孔打孔方案对其正解反解的一致性进行验证， 结果表明了该逆解解法的 正 确 性 。 关键词 双臂液压钻车； 自动化打孔； 齐次变换； 运动学分析； 逆运动学 中图分类号 T H1 6 ； T D 4 2 1 文献标识码 A 文章编号 1 0 0 1 3 9 9 7 2 0 1 5 0 8 一 - 0 1 4 7 一 - 0 3 I n v e r s e Ki n e ma t i c s S o l v e r o f a 2 一 Ar m Hy d r a u l i c Dr i l l Ca r r i a g e Z H A N G F u - x i a n g ， S HI We n - j u n 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 ri 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 ， H e b e i S h i j i a z h u a n g 0 5 0 0 1 8 ， C h i n a A b s t r a c t I n C h i n e s e c o a l m i n e s a n d t h e c o n s t r uct io n o f n e w w e l l s ， t h e r e a r e a l o t of r o c k r o a d w a y e x p l o r a t i o n a n d e x c a v at ion w o r k e v e r yy e a r ， a n dt h e d e g r e e o fa u t o m ati o n o fe q u ip m e n t e x c ava t ion h a s b e c o m e am 4 o r f act o r r e s t r i c t i n gthe i m p r o v e me n t o f c o al p r o d u c t ion c a p a c i t y ． I n o r d e r t O i m p r o v e the l e v e l o f a u t o m a t i o n h y d r aul i c d r i l l i n g ，a n d a c h i e v e 2 - a r m h y d r a u l ic d ri l l i n ga u t o m atio n p u n c h ， b yana l y z i n gi t s k i n e m ati c s ， u s i n gD - H D e n a v h - Ha r t e n b e r g m atr i x me t h o d ， k i nem a t i c e q u a t i o n s of 2 - a r m h y d r a u l i c d ri l l i n g w e r e e s t a b l i s h e d ， and t h e n ， i n t h e Ma t l a b e n v i r o n m e n t ，b y ， the s e p a r ati o n of v a r i abl e s and s y mb o l s t o r e p l a c e the mat r i x e l e me n t s ， e x p r e s s ions ofi n v e r s e 肮 m 耐 c e q u at i o n s o l u t i o n w e r e d e r i v e d ． F i n a l l y ， b y口 e n g 8 8 h o l e s ， th e c o r r e c t n e s s ofi n v e r s e s o l u t i o n W O p r ove d ． Ke y W o r d s 2 - - Ar m Hy d r a u l i c Dr i l l Ca r r i a g e ； Au t o ma t e d Dr i l l i n g ； Ho mo g e n e o u s Tr a n s f o r ma t i o n ；Ki n e ma ti c An a l y s i s ； I n ve r s e Ki n e m a tics 1引言 根据煤炭行业发展“ 十二五” 规划， 到 2 0 1 5年， 我国大型煤 矿采掘机械化程度将达到 9 5 ％以上， 中型煤矿自动化程度也有大 幅度提升。 目前， 随着生产力水平的提高以及社会的发展， 人们对 钻车提出了更高的要求， 但国内外市场上全液压掘进钻车的操作 多为人工操作， 普遍存在钻杆定位困难、 操作不便 ， 效率低下等问 题， 使岩石巷道打孔作业的生产效率受到很大制约。随着矿用钻 车市场需求越来越大 ， 各种各样钻车层出不穷， 探索钻车自动化 钻孑 L 技术， 解决钻车打孔过程中存在的科学技术问题并提出有效 的解决方案是我国矿业开发面临的一项紧迫的任务。因此， 对双 臂液压钻车自动化钻孔关键技术的研究具有非常重要的现实意 义。针对石家庄煤矿机械有限责任公司C M J 2 ． ． -2 7液压钻车的自 动化改造的要求 ， 对机械结构进行改进 ， 并对改进后的钻车进行 运动学分析 ， 求解出运动学求解表达式 ， 为实现钻车自动化打孔 奠定技术基础[ 1 】。 2运动学方程建立 求取运动学方程逆解是求解钻车手部 双臂末端钻杆 期望 位姿对应的关节变量， C M J 2 2 7的每个钻臂均为 6 个 自由度， 需 要 6轴联动目 。通过对钻车结构的分析， 建立钻车运动学坐标系， 如图 1 所示 。 传统的D一 -H D e n a v i t - Ha r t e n b e r g m a t r i x 法是在每个连杆上 都固定一个坐标系， 然后用 4 x 4的齐次变换矩阵来描述相邻两连 杆的空间关系13 1 。通过依次变换可最终推导出末端执行器相对于 基坐标系的位姿， 建立钻臂的运动学方程。 式 1 为6自由度机器 人运动学方程 。 p ， l A A 2 A ， A A A 6 0 z p 0 0 O l 来稿 日期 2 0 1 5 一 - 0 1 2 0 基金项目 河北省科技攻关计划项目 1 4 2 1 1 9 0 2 D 作者简介 张付祥， 1 9 7 3 一 ， 男， 河北青县人， 博士研究生， 副教授， 主要研究方向 机电控制与机器人 1 4 8 张付祥等 双臂液压钻车的逆运动学求解 第 8期 式中,- n 、 、 、 o ， 、 现、 、 吼、 p 、 p 各关节变量的函数 ， 反映 机器人末端的位姿与关节变量之问的关系。 图 1双臂液压钻车运动学坐标系 Fi g ． 1 Ki n e ma t i c Co o r di n a t e s o f t h e 2 - Ar m Hy d r a u l i c Dr i l l Ca r r i a g e 根据图 1 建立的坐标系确定钻车的 D H参数 ，如表 1 所 示。为了满足钻车自动化打孔的需要， C M J 2 2 7钻车的改进结构 去除了两个偏心， 即a ． o -- 0 。 表 1钻车双臂的 D H参数 Ta b ． 1 D H P ar a me t e r s o f t h e t wo Ar mS 连 杆薯转 角 ‘ 连 嗍枷 崩 撇q 连 妒 钻车钻臂坐标变换矩阵4的通用表达式为 Ai R o t Z ， T r a n s 0 ， 0 , 4 T r n s a ； ， 0 ， 0 R o t X， O di c0 i - s O , ci s O i s o ti c s O ； c O ； c O t ； - cO ； s a a i 3 0 0 S O l i C O l i d i 0 0 ．0 1 用 s 。 和C 分别代表 s i n 0 i 和c 0 s ， 则可获得A。- A 。具体为 Al A 3 A 5 。1 0 l 0 1 0 _ c 0 0 1 0 O 0 0 0 1 。 3 0 3 0 3 0 C 3 0 0 1 0 以 O O O 1 C 5 0 5 0 5 0 _ c 5 0 0 1 0 0 0 0 1 A2 A4 A 6 C 2 0 s O 2 0 2 0 2 0 0 1 0 0 0 0 0 1 C 4 0 -- 4 0 4 0 4 0 O 一 1 O 0 O 0 0 1 1 0 0 0 0 1 0 0 0 0 1 d 6 0 0 O 1 用户坐标系到钻臂的变换矩阵为A 其中， 用户坐标系到左 钻臂 的变换式 中 0 ， 用户坐标系到右钻臂 的变换式 中 x o 0 。 A T r a n s x， ， R o t z ， 9 0 。 R o t y ， 9 0 。 0 1 0 0 0 1 一1 0 0 z 0 O O 1 通过观察基座坐标系的位置和各坐标轴在用户坐标系的投 影[3 1 ， 并考虑用户坐标系， 建立钻臂统一的运动学正解方程如下 T 6 A A1 A2 A3 A 4 A 5 A 6 3逆运动学求解 n y o y pT 0 0 O 1 2 逆运动学问题是已知末端执行器的位姿，求相应的关节变 量[4 1 。 运动学逆解的解法很多， 其中主要有解析法[5 1 、 几何法I6 1 、 符号 及数值方法[7 1 、 几何一 解析法嘲 等。 对于大多数工业或商业机器人而 言， 一般可采用代数法求得封闭解 ， 代数法的缺点是工作量大， 往 往需要直接观察和经验， 而且几何概念不强， 不能利用特定的机 器人结构根据几何概念来简化计算过程 ； 解析解可以避免这个 缺点， 因此这里采用解析法中的分离变量法求得解析解。 分离变量法主要是使用齐次变换逆矩阵逐次左乘 或右乘 方程的两边， 将未知数由矩阵方程的右边移到左边 左边移到右 边 ， 使其与其他未知数分开， 解出未知数 ， 它的特点是， 首先利用 运动方程的不同形式，找出矩阵中简单表达某个未知数的元素， 力求得到未知数较少的方程， 然后分离出变量逐个求解 。 下面利用分离变量法来求运动学方程逆解的表达式。 首先先求 d 以及 0 和 用A ． ．‘ 左乘式 2 ， 将式 2 变形得 n l Al A A 3 A 』 4 5 A 6 3 将式 3 左、 右展开得 A 1 A 2 A 3 A 4 A 5 A 6 a I 1 q2 3 a 1 4 l 2 a2 3 a 2 4 1 2 a3 3 4 0 O 0 l 其中 ， 0 1 1 c 1 c 2 c l s 3 c 4 _ - c 4 c ． c 1 c 3 s 1 C 3 5 0 2 一 c 1 c 2 c 3 s 3 s 4 _ _ c 4 口 l 3 c l c 3 3 c 4 广 c 1c r 呵 lC 3 c 5 0 I 4 e lC 2 C 3 “ I s 3 c 4 c l5 4 5 厂 c l c 3 l c 3 c 5 d 一 c 1 c 2 c 3 4 c ls 2 c 4 c I s 2 d 3 a 2 1 s lc 一 c 3 c 4 4 c 5 s l c 2 s _c lc 3 s 5 。 2 一 s l c 2 c 3 c 3 4 ls 4 口 【∞ s lc r c 3 c 4 ． 噶 2 s 4 广 s 1 c 2 s l C 3 c 5 口 z 4 s I c 2 c r _c 3 c 4 s 4 5 一 lc 3 c l c 3 c 5 一 s 1 c 2 c 3 c 3 4 s 1 s 2 c 4 c f 5 1s 2 d 3 啦l s 2 c 3 c 4 c 2 s 4 c ， s 5 嘞 2 c3 s 4 c 2 c 4 嘞3 s 3 c 4 c 4 s r 2 s 3 c 5 口 【3 4 s 2 c 3 c 4 c 4 s r 2 s 3 c 5 5 2 c 3 s 4 c 2 c 4 d r - c 2 以 4 No ． 8 A u g ． 2 0 1 5 机械 设 计 与制 造 1 4 9 以及 z u 1 。 ； A 5 l 一 0 0 0 1 j l l 通过观察 4 式各元素之间的关系可得 吼 4 -- a 1 d 1 r 3 6 n 7 Ⅱ 3 4 Ⅱ 3 3 6 5 __ c 3 8 联立式 6 、 式 7 、 式 8 成方程组求解， 并取式 4 与式 5 矩阵所有元素对应相等， 有 d 6 1 ／ 2 ／ 一 2 2 a p 一 2 a z 一 2 o ,fl s 一 2 a y y 一 2 吨 d 5 2 2 2 p 2 2 2 2 一 2 2 , 2 2 。 2 。 , 2 - o ； 4 2 a p a ； o 4 - a ； 2 p 一 2 p 一D 0 5 2 p 一 2 2 d 52 2 2 p 2 。 2 一 2 p y 2 a 2 a x a yy u 2 ％p y a p 一 2 p y a z 一 2 a p a z 一 2 a p a 2 a 。 2 喀p ， 一 2 喀咒 一 喀 一 2 吐一 2 以 2 2 以 2 2 喀 一 2 2 2 2 2 _ 2 2 2 2 2 2 2 2 2 2 ． 2 2 2 2 2 d 3 - ％p - a - a x P 一 2 2 ％ x u a r o r d 5 2 a X u a o dd 3 a p a r o r ct5 2 a a o d s 一 一 十 一 2 a r p y d 5 2 y 以一 2 a y o r d 5 2 a , o , d s 2 a zuao d 2 az 喀 2 d 5 2 ‘ 2 2 2q 2 a o u 脚 9 a r c t a n d 0 5 ＼ z d 6 ／ - a r c co s 盟 ⋯ 然后依次求得求 、 和O s 03 arctan 等 一 tall ⋯ C lOx-- 10z 以及 0 3 0 3 1 8 0 。 1 2 0 4 a r c c o s s 2 s l 0 1c 2 D ， 如2 c 1o z 1 3 05 arctan 等 tan -s⋯ ,a c ，-s ,．a I_ 以及 O 5 - 1 8 0 。 1 4 至此 。 、 0 2 、 O s 、 0 4 、 、 d 6 全部求出。 4计算结果与仿真分析 为验证 匕 述运动学逆解推导的正确性，利用 Ma t l a b ，对 8 8 孔打孔方案的所有孔进行计算和验证，以其中一个孔的计算为 例 双臂钻车钻臂末端的位置为 p ,, 2 1 0 2 ． 5 5 ， p y 2 4 9 9 ． 7 5 ， P 0 ； 杆 件参数为 d 一 3 5 5 3 ， d c - 4 7 9 ． 5 。 给出期望 的钻臂位姿矩阵 l 0． 7 071 0 ． 7 062 -0 ． 03 53 21 02 ． 55 l 7 町 2 ∞ 5 2 4 l 16 。 l 0 0 ．0 4 9 9 0 ．9 9 8 8 0 l 【 o 0 o 1 J 由式 9 、 式 1 0 、 式 1 1 、 式 1 3 、 式 1 4 和式 1 5 计算， 得 0 1 - 2 4 ．0 2 1 4 ， 0 2 - 8 0 ．2 8 2 2 ， 0 3 - - 4 5 ． 4 3 8 9 ， 0 4 - 6 9 ．0 4 5 2， 0 5 - _ 7 9， 8 6 3 7 ， d 6 -- 一 2 4 7 0 ． 9 。将以上各变量计算结果带入式 2 ，所得数据与式 1 6 一致 ， 即正解反解一致。 5结论 针对石家庄煤矿机械有限责任公司 C M J 2 2 7全液压钻井 钻车的自动化改造的要求， 对机械结构进行改进， 用 D H法建立 起钻车运动学方程 ， 求取了运动学方程的逆解， 运动学逆解的求 解过程中仅需一次矩阵逆乘， 避免了大量的矩阵逆乘运算 ， 求解 过程简单， 用符号表示矩阵元素， 避免了繁琐的表达式， 计算过程 简单清晰。从解的表达式中，我们可以看出0 。 和 分别有多个 根， 求解过程中， 给出了双臂液压钻车对应于某一位姿的多个位 形。 但最后根据各关节量的工作区域范围， 可确定唯一解。 该运动 学方程逆解的求取以及仿真分析为实现 C MJ 2 2 7改进型钻车自 动化打孔奠定了技术基础， 同时这种运动学逆解的求取方法也可 以应用于多自由度串联型机器人运动学逆解的求取。 参考文献 [ 1 ] 张付祥 ， 孙书鹏 ．双臂液压钻车的运动学分析及优化 [ J ] ． 机床 与液压 ， 2 0 1 4 9 1 0 1 1 0 4 1 2 3 ． Z h a n g F u - x i a n g ， S u n S h u - p e n g r H y d r a u l i c d ri l l i n g a r m k i n e ma t i c a n a l y s i s a n d o p t i m i z a t i o n[ J j _Ma c h i n e T o o l Hy d r a u l i c s ， 2 0 1 4 9 1 0 1 1 0 4 1 2 3 ． [ 2 ] 孙增圻．机器人系统仿真及应用[ J ] ．系统仿真学报， 1 9 9 5 ， 7 3 2 3 2 9 ． S u n Z e n g - q i ． R o b o t s y s t e m s i m u l a t i o n a n d a p p l i c a t i o n [ J ] ．S y s t e m S i m u l a t i o n ， 1 9 9 5 ， 7 3 2 3 2 9 ． [ 3 ] 苏学满 ， 孙丽丽 ， 杨 明．基 于 m a t l a b的六 自由度机器人运动特性分析 [ J ] ．机械设计与制造， 2 0 1 3 1 7 8 8 0 ． S u X u e ma n ， S u n L i - l i ， Y a n g Mi n g ． C h a p t e r anal y z e s b a s e d o n s i x d e g r e e s o f f r e e d o m o f t h e r o b o t k i n e m a t i c s m a t l a b[ J ] ．M a c h i n e r y D e s i g n a n dMa n u f a c t u r e ． 2 0 1 3 1 7 8 8 0 ． [ 4 ]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 mo t i o n s i m u l a t i o n[ J ] ． O p t i c s C o mmu n i c a t i o n s ， 2 0 0 2 ， 2 0 5 4 2 5 7 - 2 6 3 ． [ 5 ] 王雪松， 许世范， 郝继飞． M O T O M A N机械手逆运动方程新的推导方法 及求解[ J ] _ 中国矿业大学学报， 2 0 0 1 1 7 5 - 7 8 ． Wa n g X u e s o n g ， Xu S h i - f a n ， H a o J i - foi ． MO T O MA N mani p u l a t o r i n v e r s e k i n e ma t i c s e q u a t i o n s a n d s o l v i n g n e w d e fi i , a t i o n me t h o d[ J ] ． C h i n a U n i v e rsi t y o f Mi n i n g T e c h n o l o g y ， 2 0 0 1 1 7 5 - 7 8 ． [ 6 ]P a u l R P ． R o b o t Ma n i p u l a t o r ． Ma t h e ma t i c s ， P r o g r a mm i n g s a n d C o n t r o l [ M] ．C a mb r i d g e MI T P r e s s ， 1 9 8 1 1 8 1 - 1 8 7 ． [ 7 ] F u K s ， G o n z a l e z R C ， L e e C S G ． R o b o t i c s C o n t r o l S e n s e s V i s i o n a n d I n t e l l i g e n c e [ M] ．N e wY o r k M c G r a w - H i l l ， 1 9 8 7 7 8 8 2 ． [ 8 ] D i n e s h Ma n o c h a ， J o h nFC ann y ．E ffic i e n t i n v e r s e k i n e ma t i c s f o r g e n e r a l 6 R m a n i p u l a t o r s [ J ] ． I E E E T T a n s a c t i o n s O n R o b o t i c s a n d A u t o m a t i o n ， 1 9 9 4 ， 1 0 5 6 4 8 6 5 7 ． [ 9 ] 贺懿濯． P U M A 5 6 0 逆运动方程的新解法[ J ] _ 机器人， 1 9 8 9 ， 1 0 3 1 9 一 H e Y i z h u o ．N e w s o l u t i o n P U M A 5 6 0 i n v e rs e k i n e mat i c s e q u a t i o n s [ J ] ． R o b o t ， 1 9 8 9 ， 1 0 3 1 9 2 6 ． [ 1 0 ] 冯乔生， 刘丹非．种新的符号求解机器人 逆运动学的分离变量法[ J ] ． 机器人 ， 1 9 9 7 2 6 4 6 7 7 1 ． F e n g Q i a o - s h e n g ， L i u Dan - foi ．A n e w s y mb o l o f n o n s e p a r a t i o n o f v a r i a b l e s fo r s o l v i n g i n v e r s e k i n e m a t i c s o f [ J ] _R o b o t ， 1 9 9 7 2 64 6 7