MOTOMAN机械手逆运动方程新的推导方法及求解.pdf

第3 0 卷第1 期 2 0 0 1 年1 月 中国矿业大学学报 J o u r n a lo fC h i n aU n i v e r s i t y0 fM i n i n g ＆T e c h n o l o g y V 0 1 ．3 0N o ．1 J a n ．2 0 0 1 文章编号1 0 0 0 1 9 6 4 2 0 0 1 0 1 0 0 7 3 0 4 M O T O M A N 机械手逆运动方程 新的推导方法及求解 王雪松，许世范，郝继飞 中国矿业大学信息与电气工程学院，江苏徐州2 2 1 0 0 8 摘要；提出了一种新的推导M O T O M A N 型机器人逆运动学方程的方法．进而给出逆运动问题新 的求解方法．此方法不需要对机械手末端位姿进行坐标变换，另外在解的推导过程中避免了大量 的逆矩阵相乘，方法简单，便于- r - , 3 k 控制和仿真．仿真证明了计算结果的正确． 关键词M O T O M A N 机械手；机器人逆运动学；解析解 中图分类号T P2 4 1文献标识码A 机器人运动学逆解在机器人学中占有非常重 要的地位，它直接关系到运动分析、离线编程、轨迹 规划等，是将工作空间内机器人末端件的位置、姿 势转化成关节量的方法．由于机器人逆运动问题本 身的复杂性，要建立通用算法是相当困难的。许多 人为此付出巨大的努力，做了大量的工作．有关机 器人运动学逆解的求解方法很多，其中主要有解析 法叭“、几何法啪、符号及数值方法Ⅲ、几何一解析 法Ⅱ3 等．在工业控制中人们尤其对一些特殊的机器 人，如M O T O M A N 型机器人很感兴趣．为此，本文 主要讨论M O T O M A N 型机器人逆运动方法．此方 法同文献[ 1 ] 的方法相比在推导逆运动问题时，避 免了大量矩阵的逆乘，而且表达式结果简单，便于 工业控制和仿真，通过实例的验证，表明结果正确． 1 建立数学模型 M O T O M A N 型机器人具有6 个自由度．而且 6 个关节均为旋转关节嘲．运动学传统的描述方法 是基于D - H 方法即矩阵来描述运动学问题 的。从而只要给出关节变量，就可根据公式 A l A 2 A 3 A ‘A 5 A 6 一T 6 ， 1 得到机器人末端的位姿 位置和姿态 I 如以如P z l 瓦一l 雌如a yP ，l ， 2 J 啦峨以且J L o 0 01J 其中a 是机械手接近向量；O 是机械手滑动向量；H 是机械手法向向量；p 是机械手位置向量． 在式 1 中，坐标变换矩阵 rc 。一0 41 ] A l 鬟篡I C iY 1 “盔纠1 I 肛一1 s z所一 d t 一 L00o1 j i 一1 ，2 ，⋯，6 ． 矩阵A 的逆阵 卜寸s 阳一斗- 1 A 『1 f _ 。二嚣。。￡11 耋l L0001 j f 一1 ，2 ，⋯，6 ， 其中s ，2s i n S , ；o c o s 0 , l 一12C O S a i l ；肛一l s i n a , 一1 ． 由磷⋯岛⋯Z ，即个参数就可确定杆件 厶。， L ， 之问的相对关系．q 一称作杆厶一。的扭角，马一， 称作杆厶一。的杆长，d ，是关节i 的偏距，只是关节i 的旋转角．对于M O T O M A N 机器人6 个关节全为 旋转关节来说，q 一．，马一。和d ，为常量，B 为变量． 根据表1 ．可得M O T O M A N 机器人坐标变换 A 矩阵如下 裹1M O T O M A N 机器人杆件参数 T a b l e1L i n kp a r a m e t e r so fr o b o tM O T O M A N 差翌生 差堇壅曼蔓曼竺1 10 1 000 17 0 ～ 1 7 0 2 如01 5 0 9 04 5 ～ 1 5 0 3如02 6 007 0 ～ 1 0 0 4巩2 6 0r i O 9 0 1 8 0 ～ 1 8 0 5巩0 09 0 1 3 5 ～4 - 1 3 5 出业Q业 堑Q 士i 望 收藕日期t2 0 0 0 0 6 2 0 t 盒项且t 国家8 6 3 资助项目 9 8 2 3 0 1 一0 1 作者筒介王雪松 1 9 7 4 一 ．女，安徽省洒县人，中国矿业大学博士研究生．工学磺士．从事煤矿机器人方面的研究． 万方数据 7 4 中国矿业大学学报第2 9 卷 A l A 2 一 A 3 一 A 4 式 3 的右边 。卸 旬 0 0勃 A 3 A 4 A 5 A i l A i - 1 T 6 A i l f 3 “岛一J l 矗 s 薛I 岛十c 3 ％ S 4 C S 0 0 5 A 。一1 o I 一5 5 LO 仁匡 一s s 0 O1 一如0 00 。5 6 0 0～1 c 6 0 0O ；1， 一纠 2 逆运动方程的推导方法及求解 逆运动学问题是已知机器人末端执行器的位 姿，求相应的关节变量．为此将式 1 变形为 A a A 4 A 5 一A i l A i l T 6 A i l ， 3 式 3 的左边 c ‘s 5 一s 乖5 s 3 s 3 c I 如十c 3 c 5一如 一5 ‘S 5C 4 00 a 3 “ 破如 口z ] d 3 缸一d ‘臼1 0 。J 4 f 6 i 1 一％I1 1l s 6I1 f 6 Ⅱld 6 I l c l c 2 p J 十c 2 如户，一a l c 2 l “1 2 6 1 2 1 2＆1 2 1 6 Ⅱ2 矾1 2 1 m 丸。2 乱丸“ J ， 5 c 6 I3 5 6 Ⅱ3 一Ⅲ35 6 I3 c 6 I3一d 6 Ⅱ3 5 1 户f c l p ，1 0001j 式中I1 1 c l f 2 * C 2 5 1 如； 11 一C 1 C 2 0 f C Z 1 0 ，} Ⅲl2C l C 2 4 _ c 2 5 1 a ，； I2 一一f i J 2 月z j z 如h ； I2 一c t s a o z 十5 2 l o ，； Ⅲ22C 1 5 0 以 5 2 5 1 a ，； 1325 1 n I f l n ，} I3 25 1 ％一C l O y ； I3 一 1 a 一c 1 4 ， 令式 3 左右两边元素 3 ，4 相等得 一d 6 旬d 一‘l a y s i p ，一c l p ，一0 ， 6 解式 6 得 忙a r c t a n 筹去 ． ㈣ 令式 3 的左、右两边元素 1 ，4 ， 2 ，4 分别相 等并化简得 c 3 a 3 s 3 d ‘ a 2 一c 2 [ c l 户f s i p ，一4 1 一 d 6 c 1 ‰ 5 1 q ] ， 8 一c s d 4 十 3 a 3 一s z [ f l 户 s 1 丸一4 I 一 d 6 c l 如 s l 口， ] ． 9 将式 8 和式 9 两式平方相加得 d i a l 十d ； 2 a 2 d 4 s 3 十2 a 2 a 3 如一 E c l p s i p ，一4 1 一d 6 q a 1 a y ] 2 ． 1 0 令t 1 一[ f l 声 5 1 P ，一a 1 一d 6 f l a ， 1 a y ] 2 ，t 2 生二蓝云乎二生，式 1 。 整理得 a 3 ‘3 d ．如2 t 2 ． 则 岛一弼扣渤n 【考2 专2J Ⅲ， 其中n √棚 n ；；镪 a r c t a n 宝． 令t 3 一d 6 f 1 ％十5 1 ∞ 十 q A 5 1 p ，～口1 则 式 8 和式 9 可分别化简为 一盟半m 卫 警， 则岛一a r c t a n 蔷徽． 1 2 令式 3 左、右两边元素 1 ，3 相等得 5 3 一s 6 ‘1 ‘2 n 。十f 2 如％ c 6 q ‘2 “ c 2 卸q ， 解之得 以一一铀土a r c t a n 暑竺 ， 1 3 √r 2 一s j 其中 √ c l c 2 n c 2 s l ”， 2 c l f 2 0 f 2 s l D ， 2 ； 们．I．．o鲫。胤，印孙o， 5 0 O l 2 o 1 o o o 。1 o o叫o o o 龟0 0 o 0 “0 一“乱0 』．o O＆m．旬如0 0．“0钆m 万方数据 第1 期王雪松等M O T O M A N 机械手逆运动方程新的推导方法厦求解7 5 弗一a r c t a n 焉音豢． 令式 3 两边元素 3 ，3 相等得 “ s s h n C l l l ， c s s l o 一C t O ， ， 1 4 令式 3 的左、右两边元素 3 ，1 ， 3 ，2 分别相等得 4 C 5 一c s s l n 。一q 唧 一如 5 1 ％一C l q ． 1 5 一s ‘如一一 s 1 4 。一c l a s ． 16 式 1 5 和式 1 6 两式平方相加得 一士 E c 6 如月。～C l o t ， 一5 6 卸。。一c a o , J 2 1 ＆d 。一c l q 2 } ； 则 吼一a r c t a “i s 4 ． 1 7 当轧≠0 时， ，一￡墨 垒塾二鱼坐 垒 生坠 鱼生2o5 乩 一半， 则 a r c t a n 丽石嵩看惫- 丽 以 0 ， a r c t a n 丽t s , i n 嵩手％i 历‘6』一“n y J s 6 L 卸以一旬q J 以 ∞’ 1 9 a r c t a nC 6 1 I n x 嵩篝 粤f i 5 1 0 x 丽c t o r 一‘l ”，J 一 ～ ， 【 巩 o ， 其中r a 一√硪 口i ； t 1 一E c 1 户。 如户，一4 1 一d 6 c l a ， 5 l q ， ] 2 ； t 。罐d i d i ”一 2 口。 ’ 1 本文中的模型A 3 A 。A 。一A i l A 7 1 T 。A ；- 1 是基 于D - H 方法的，然而在解的过程中仅需一次矩阵 逆乘，与文献[ 1 ] 的方法比较，避免了大量的矩阵逆 乘运算。求解过程更简单． 2 从解的表达式中，我们可以看出巩，吼，仇分 别有两个根，在求解过程中，给出了M 0 1 ℃M A N 型机器人对应于某一位姿的8 个位形． 3 该方法为机器人的逆运动学求解提供了一 种新思路． 4 本文提出的方法为实现机器人智能轨迹规 划提供了可能． 这种方法适用于目前6 个自由度的工业或商 用机械手的逆运动学求解． 参考文献 E 1 3P a u lRP ，S h i m a n oBE 。M a y e rG ．K i n e m a t i cc o n t r o l e q u a t i o n sf o rs i m p l em a n i p u l a t i o n s [ J ] ．I E E ET r a n s S M C ．1 9 8 1 ．1 1 6 4 4 9 4 5 5 ． [ 2 ] P a u lRP ．R o b o tM a n i p u l a t o r M a t h e m a t i c s P r 0 _ g r a m m i n g sa n dC o n t r o l [ M ] ．C a m b r M g e IM I TP r e s s ， 1 9 8 1 ．1 8 1 1 8 7 ． E 3 3F uKS ．G o n z a h zRC ，L e eCSG ．R o b o t i c sC o n t r o l S e n s i n sV i s i o na n dI n t e l l i g e n c e [ M J ．N e wY o r k M e C r a w H i l l ，1 9 8 7 ．7 8 8 2 ． [ 4 ] D i n e s hM a n o e h a ，J o h nFC * n n y ．E f f i c i e n ti n v e r s e k i n e m a t i c sf o rg e n e r a l6 Rm a n i p u l a t o r s [ J ] ．I E E E 万方数据 7 6中国矿业大学学报第2 9 卷 T r a n s a c t i o n sO i lR o b o t i c sa n dA u t o m a t i o n ，1 9 9 4 ．1 0 5 ，6 4 8 6 5 7 ． [ 5 ] 贺懿灌．P U M A 5 6 0 逆运动方程的新解法[ J ] ．机器 人，1 9 8 9 ，1 0 3 1 92 6 ． [ 6 ] 余达太．马香峰，鄱安民，等．工业机器人应用工程 [ M ] ．北京冶金工业出版社．1 9 9 9 ．2 3 2 4 ． N e wI n f e r e n t i a lM e t h o da n dE f f i c i e n tS o l u t i o n s f o rI n v e r s eK i n e m a t i c sE q u a t i o n so fR o b o tM O T O M A N W A N GX u e s o n g ，X US h i f a n ．H A OJ i f e i C o U e g eo fI n f o r m a t i o na n dE l e c t i c a lE n g i n e e r i n g ，C U M T ，X u Z h o u ．J i a n g u2 2 1 0 0 8 ，C h i n a A b s t r a c t An e wi n { e r e n t i a lm e t h o df o ri n v e r s ek i n e m a t i c se q u a t i o no { M o T 【帅d A Nm a n l D u l a t o r sw a sp r e - s e n t e d ，a n de f f i c i e n ts o l u t i o n so fi n v e r s ek i n e m a t i c sp r o b l e mw e r eg i v e n ．T h em e t h o dn e e d n ’tc h a n g et h ec o o r d i n a t e so ft h em a n i p u l a t o re n de f f e c t o r ，a n da n a l y t i cs o l u t i o n so ft h ep r o b l e mc a nb eo b t a i n e d ．A d d i t i o n a l l Y ．al o to fm u l t i 口I i c a t i o n so fi n v e r s em a t t i e e sa r ea v o i d e di nt h ec o u r s eo fd e d u c t i o n ．T h i sm e t h o di sv e r ys i m p i ea n du s e f u l ．s oi ti ss u i t a b l ef o rc o n t r o la n ds i m u l a t i o n ．S i m u l a t i o nv e r i f i e st h ec o r r e c t n e s so fc o m p u t a t i o n ． K e yw o r d s r o b o tM O T O M A N ；r o b o ti n v e r s ek i n e m a t i c s ；a n a l y t i cs o l u t i o n s 我校七项成果获19 9 9 年度江苏省科技进步奖 序号成果名称 双开关磁阻调逮电动机并联 拖动系统研究 2 3 米直径大型旋流微泡浮选柱 研究 3 水上、陆地三维地震勘探小断 层解释方法研究及应用 序号成果名称 1 S C I E N2 0 0 0 型河闸、水库多 媒体综合自动化系统 2软岩矿区冲击矿压防治研究 3大屯矿区技术改造与综合开 发研究 4江苏省洁净煤技术政策研究 二等奖 完成单位 中国矿业大学、中国矿业大学 北京校区 中国矿业大学、大屯煤电有限 责任公司选煤厂 大屯煤电 集团 有限责任公 司、中国矿业太学、山东煤田 物探测量队、胜利油田地球物 理科技开发公司 三等奖 完成单位 中国矿业大学、徐州理工计算 机工程技术研究所 中国矿业大学、徐州矿务局权 台煤矿 大屯煤电 集团 有限责任公 司，中国矿业大学 北京校区 江苏省煤炭工业管理办公室 中国矿业大学经济管理学院 主要完成人员 陈是、王聪、谢桂林、张超、盂 宪军 欧泽深、沈中钰、高敏、谈建 德、蒋曙光、张启、谢广元 刘雨忠、刘德平、刘天放、钱建 伟，冯学武、孟宪丰、来中应 主要完成人员 徐中立、王惠敏、于洪珍、徐小 群、马小平 缪协兴、孙海、王继承、茅献 彪、徐林 曹祖民、王立杰、孙明珊、丁日 佳、刘雨忠 郎志军、龙如银、张显清、周德 群、廖海平 万方数据