（机械设计及理论专业论文）6r机器人的位置反解近似通用算法.pdf

北京邮电大学硕士论文摘要 6 r 机器人的位置反解近似通用算法 摘要 机器人是人类2 0 世纪最伟大的发明之一。由于其卓越的性能， 机器人被广泛应用于社会生产活动中，对人类生活产生了巨大的影 响。可以预见，随着机器人技术的进步，机器人将在人类生活中发 挥更加重要的作用。一般6 r 机器人机构的位置反解具有非常重要 的理论与实际意义。本文在建立d h 矩阵空间位移封闭方程的基础 上，参考前人的一些研究成果，结合一定的理论分析找出1 0 个运动 学方程；通过结式消元得到没有增根也无漏根的一元1 6 次方程，并 编制了机器人反解的求解程序。 特殊尺寸机器人的种类比较多，每一个机器人单独编程，求解 比较繁琐。本文尝试利用一般6 r 机器人的位置反解程序解决这个 问题。当利用一般6 r 机器人的反解程序进行一些特殊尺寸机器人 机构求解的时候，会遇到退化和奇异的问题。本文通过把尺寸做些 微小调整，可以利用这个程序进行求解，避免退化奇异的问题。同 时，这些微小调整基本上不影响机器人的输出位置精度，满足实际 要求。在精度允许的范围内将具有特殊尺寸的机器人视为一般的6 r 机器人，运用一般6 r 机器人的反解算法得出了其运动学反解方程， 并给出了实例验证。 关键词：机器人、位置反解、特殊尺寸、精度 北京邮电人学硕十论文 a b s t r a ( 了 a _ p p r o x i n 队t em e t h o do fi n v e r s ek i m e m 喳t i c a n a l y s i so f6 rs e r l 气lr o b o t a b s t r c t r o b o ti so n eo ft h em o s tg l o r i o u si n v e n t i o n so fh u m a nb e i n g si n 2 0c e n “i r y f o ri t ss u p e r l e x c e e n tp e r | 洒m a n c e s m b o ti sw i d e l yu s e d t oi n d u s t r i a lp m d u c t i o n s ，a n db n n g sa b o u tag n a te c o n o m i cb e n e 行t i ti sp r e d i c t e dt h a tw i t ht h ed e v e l o p m e n to fm b o t i ct e c h 肋l o g y m b o t w i l jp l a yam o r e j m p o n a n tr l ；o l ej n t h eh u m a nl j f 色i n v e n 沦l a n e m a t i c a n a l y s i so fg e n e r a l6 rs e r i a lr o b o t i sv e r yi m p o n a n tt h e o r e t i c a ia n d p 瑚c 缸明ls i g n i 6 c a n i nt h i sp a p e bb a s e d o nt h el i t e m t i i nm s 托h 托s u l 协。1 0e q u a t i o 璐a no b t a i n e d 矗u mt l i ed hm a t r i x ，t h e nt h e ya m u s e di nt h es p a t i a ld i s p l a c e m e n ta n a l y s i so f6 rr o b o t a1 6 t ho r d e r i n p u t o u t p u te q u a t i o nw i t ho n eu n k n o w ni s0 b t a i n e d ac o m p u t e r p m g 均mi sm a d ef o rt h ei n v e 玮e 垴n e m a t i ca 曲l y s i so f ag e n e 船l6 r s e r i a lr o b o t t h e ma m m a n ys p e c i a l d i m e n s i o nm b o t sa n de v e r yo n eo f t h e ms h o u l db ea 的l y s j si n d e p e n d e n t l y i n 咖sp a p e rw e 时t ou 卵m e m e t h o do ft h e 霉e n e i 翟l6 rs e a lm b o tt 0d 0t h ei n v e r s e 酗n e m a 伯c a n a l y s i so ft h es p e c i a l - d i m e n s i o nm b o t s i m d e rt oa v o i ds i n 跚l a r i 坶 d u a n n gt h ep m c e s s ，s o m ed i m e n s i 蚰so ft h em b o ta 托a d j 吣t e da n t t l eu n d e rt h ep 代c i s i o np e 珊i t e d r e g a r d i n gt h es p e c i a l - m m e n s i o n m b o t sa sg e 耻髓l6 r 而b o t sw i t h i nt h ep m c i s i o n ，t h ep a p e rw m o b t a i nt h e 姑n e m a 6 c a l i n v e 玮ee q u a 6 0 nw i t h 恤ea p p h t i o n0 f i n v e 璐ea l g o t h mo ft h eg e n e r a l6 r 辩一a lm b d t ，w m c hw 珊a l s ob e t e s t e db ys e v e r a le x a m p l e s k e yw o r d s ：m b o t l n v e 璐e硒n e m a t i ca n a l y s i s s p e c i a lm m e n s i o n p m c i s i o n 北京邮电人学硕+ 论文声明 独创性( 或创新性) 声明 本人声明所呈交的论文是本人在导师指导下进行的研究工作及取得的研究 成果。尽我所知，除了文中特别加以标注和致谢中所罗列的内容以外，论文中不 包含其它人已经发表或撰写过的研究成果，也不包含为获得北京邮电大学或其它 教育机构的学位或证书而使用过的材料。与我一同工作的同志对本研究所做的任 何贡献均已在论文中作了明确的说明并表示了谢意。 申请学位论文与资料若有不实之处， 本人签名：签：白挞 本人承担切相关责任。 日期：垫! ：! ：墨生 关于论文使用授权的说明 学位论文作者完全了解北京邮电大学有关保留和使用学位论文的规定，即： 研究生在校攻读学位期白j 论文工作的知识产权单位属北京邮电大学。学校有权保 留并向国家有关部门或机构送交论文的复印件和磁盘，允许学位论文被查阅和借 阅；学校可以公布学位论文的全部或部分内容，可以允许采用影印、缩印或其它 复制手段保存、汇编学位论文。( 保密的学位论文在解密后遵守此规定) 保密论文注释：本学位论文属于保密在一年解密后适用本授权书。非保密 论文注释：本学位论文不属于保密范围，适用本授权书。 本人签名：蕉：鱼整日期：型 ：墨! 导师签名： 座丛! 丝 日期：! 12 ：! 北京邮电人学硕十论文 绪论 第一章绪论 1 1 机器人概述 “r o b o t ”一词最早出现于1 9 2 0 年在捷克作家凯勒尔- 凯佩克( k a r e l c a p e k ) 的名为“罗莎姆的万能机器人? 的幻想剧中。在该剧中，机器人按照其 主人的命令默默地工作，没有感觉和感情。以呆板的方式从事繁重的劳动。现实 世界晕的机器人并不像文艺作品中描述的那样神通广大、与真人形似到真假难 分的地步。就目前的水平而占，机器人一般不具有人的形念，只是一个通用的自 动化装黄。国际学术界至今也没有作出统一的、公认的、文字严格的定义。不 同的专家往往给以各有侧重的说法，不同的国家也往往沿用各自习惯的解释。 在1 9 6 7 年日本召开的第一届机器人学术会议上，森政弘与合田周平就合 作提出了有代表性的定义：“机器人是一种具有移动性、个体性、智能性、通用 性、半机械半人性、自动性、奴隶性等7 个特征的柔性机器。”从这一定义出 发，森政弘又提出了用自动性、智能性、个体性、半机械半人性、作业性、通用 性、信息性、柔性、有限性、移动性等1 0 个特性来表示机器人的形象。英国 “机器人协会所下的定义是：机器人是一种能完成一连串复杂运动的可编程的 设备。1 9 8 8 年法国的埃斯皮奥将机器人定义为：“机器人学是指设计能根据传 感器信息实现预先规划好的作业系统，并以此系统的使用方法作为研究对象。” 1 9 8 4 年1 2 月国际标准化组织( i s 0 ) 中的工业自动化系统委员会( t c l 8 4 ) 所 属工业用机器人分会( s c 2 ) 做出的定义是：机器人是种自动控制下通过编程 可完成某些操作或移动作业的机器。 中国科学家对机器人的定义是：“机器人是一种自动化的机器。所不同的是 这种机器具备一些与人或生物相似的智能能力，如感知能力、规划能力、动作能 力和协同能力，是一种具有高度灵活性的自动化机器。”由上可见，对于。机器 人”的定义是仁者见仁，智者见智，但被国际普遍接受的是由美国“机器人工业 协会”( r i a ) 的一批科学家于1 9 7 9 年提出的定义：一种可改编程序的多功能操 作机构，用以按照预先编制的能完成多种作业的动作程序运送材料、零件、工具 或专用设备。机器人是现代机械学、自动控制、计算机科学与工程、人工智能、 微电子学、光学、通讯技术、仿生技术、社会学等多种学科和技术发展的结晶。 机器人技术成为一项具有高度综合性的先进技术。一个国家的机器人技术发展 情况，可以说就是衡量一个国家科学发展水平的重要尺度，是个国家综合国 北京邮电人学硕十论文绪论 力的重要体现。 1 2 中国机器人发展概况 在中国，经过2 0 多年的发展，中国的机器人大致经历了3 个阶段：7 0 年 代的萌芽期，8 0 年代的丌发期和9 0 年代的适用化时期。中国机器人技术的丌 发与研究得到了政府的重视与支持。1 9 8 6 年国家开始实施“国家高技术研究发 展计划( 8 6 3 计划) ”。在国家8 6 3 计划的扶持下，中国开始系统地开展了机器人 基础科学、关键技术与机器人元部件、目标产品、先进机器人系统集成技术及 机器人在自动化工程中的应用的研究，取得了一大批科研成果，成功地研制出 了一批特种机器人，尤其是在水下机器人的研究方面，居世界领先水平。从9 0 年代初期起，中国的经济进入实现两个根本转变时期，掀起了新一轮的经济体 制改革和技术进步热潮。中国的机器人也在实践中迈进一大步，先后研制出了 点焊、弧焊、装配、喷漆、切割、搬运、包装码垛等各种用途的机器人，并实 施了一批机器人应用工程，形成了一批机器人产业化基地，为中国机器人产业 的腾飞奠定了基础。虽然目前中国机器人产业已有了一定的基础，但是由于起 步晚，中国的机器人水平与西方发达国家的先进机器人技术相比仍有很大的差 距，机器人技术的应用普及程度也不高。尤其是中国的多传感信息融合技术、 智能装配机器人、机器人化机械等方面的开发、应用才刚起步，与国外先进水 平差距很大。因此，从世界范围内的发展看，中国的机器人仍是一个弱势行业， 需要大力发展。 1 3 机器人在国民经济中的重要作用 在经历了诞生成长成熟后，机器人如今已成为制造业中不可少的 核心装备。从6 0 年代末机器人开始应用工业生产起到现在，世界累计生产、销 售机器人的台数己超出1 0 0 万；机器人的应用也从最初的点焊扩展到现在的弧 焊、装配、喷漆、切割、采矿、搬运、伐木、包装码垛、航天探索等多方面。 机器人技术一时也风靡于整个世界。其中最本质的因素就是：其一，机器人把 人类从繁重、单调而重复、有毒有害的的工作中( 如装配、喷漆、核工业生产等) 解脱出来，改善了人类的工作环境。其二，机器人不仅可以代替人类从事一些 不适合人类直接干的工作，而且可以代替人类干不了、干不好的工作，如精密 装配、火山探险、深海探秘、空间探索等。其三，机器人的运动一旦被确定， 2 北京邮电人学硕士论文 机器人就可以始终按照设定的轨迹运行。因此机器人工作具有准确、重复精度 高等特点，充分地保证了产品质量。其四，在某些行业，机器人的操作速度比 人工快得多，工作效率得到了很大程度的提高。其五，机器人工作具有耐久性。 它可以同同夜夜地运转而不疲惫，延长了有效工作时日j ，提高了生产强度。其 六，尤其重要的是，除了需要维持机器人运转所需要的能源和只常维护的费用 外，机器人生产几乎不需要任何其它生产投入。与人工生产相比较，机器人生 产大大降低了劳动生产的费用，提高了产出比。一言以概之，机器人在国民经 济中具有巨大的社会效益和经济效益，具有广阔的应用前景和强大的生命力。 1 4 机器人反解的历史过程 伴随着机器人的诞生成长成熟，机器人的反解算法出经历着简单 到复杂的过程。【6 ，1 2 ，1 3 】机械手的位置反解是已知机械手的空间位姿，求解各 个运动副的位移量( 包括角位移量) 。这是机械人进行控制的先决条件。早期的 机械手，由于受硬件的限制，在选择结构尺寸时，为了使控制运算简单，一般 都选择特殊的结构尺寸，如轴线相交或平行，轴线长度为零，等等。例如 p u m a 5 6 0 的三个腕关节的轴线相交于一点，这样它的姿态和位黄之间就没有 耦合，其逆解很容易用分离变量的办法来实现。随着机算机技术的发展及对机 器人应用的更高要求，一般结构尺寸的机械手也随之而出。但一般结构尺寸的 机械手，其控制运算复杂，特别是6 r 机械手，其位置反解是最困难的。它与 空问机构学中的7 r 机构位移分析是一类问题，曾被喻为是空间机构运动分析 中的珠穆朗玛峰。例如德国h w m 公司生产的1 e u o t 机械手就是这样的一种 机械手。该机械手没有三轴相交的情况，它的姿态和位置高度耦合。这种结构 的机器人反解之所以困难是因为它是对一个高次非线性方程组进行求解消元。 目前，机械手反解的主要方法有矢量法，矩阵法，复数法等。当然，随着一些 新的数学工具的出现及在机构学中的应用，一些更为简便的求解方法，也会出 现。 1 5 本课题意义及要解决的问题 1 5 1 本课题的意义 本课题将具有特殊尺寸的机器人视为一般6 r 机器人进行求解。在建立d h 矩阵 3 北京邮电大学硕士论文绪论 空问位移封闭方程的基础上，得出一般6 r 机器人的反解算法，用这个程序以p u m a 机器人与喷漆机器人为例进行了数值验证。 1 5 2 本课题要解决的问题 本文要解决的问题是利用一般6 r 机器人的反解算法，求解具有特殊尺寸机 器人的反解问题，同时解决可能出现的退化和奇异等问题。当一般6 r 机器人的 反解程序进行求解的时候，遇到退化和奇异的问题也可以考虑用此方法解决。本 文首先对一般6 r 机器人反解进行了编程，然后把它用于解决具有特殊尺寸的机 器人反解。具体是通过把不同的特殊尺寸做些微小调整，使之转化为一般6 r 机 器人机构，然后再利用这个程序进行求解。在求解过程中，通过调整不同的尺寸， 及采用不同浮点数的位数，可以使计算误差不超过0 0 1 m m 。通过对p u m a 机器 人和喷漆机器人的反解验算，证明这种方法是可行的。 4 北京邮电人学硕十论文 基本概念及理论 2 1 引言 第二章基本概念及理论 由于一般6 r 机器人的位置反解具有非常重要的理论和实际意义。因此一般 6 r 机器人的运动学分析，尤其是位置反解直以来是人们研究的重点，诸多学 者经过多年的潜心研究发现一般6 r 机器人位置总共有1 6 个反解。 对于一般6 r 机器入，其反解的过程是首先建立6 r 机器人d h 矩阵空间位 移封闭方程，在此方程的基础上运用一定的理论分析找出1 0 个运动学方程，通 过结式消元得到一元1 6 次方程，最后求出全部解。【1 7 】 2 2 机器人学的数学基础 2 2 1 机器人的位置与姿态的描述 用固联在机器人末端执行器上的坐标系原点在基础坐标中的位置来代表机 器人末端的位置，用这个坐标系在基础坐标系下的投影( 即方向余弦) 来表示机器 人末端的姿态。基础坐标系通常固联在机器人的基础上。 通常，采用一个矩阵t 来表示机器人末端的位置与姿态。 其中 r = l 吼口ip i n ，o ，4 ，p ， 吃4 ：p z 0001 n - 0 。 o - ( d 。 a - g 。 珂， 口y 口y ) r d ：) r 吒) r 是机器人末端的三个互相垂直的单位矢量，它们描述了机器人末端的姿态。 p 0 。p ，p ：， 是机器人末端的位置矢量。 5 北京邮电人学硕十论文基本概念及理论 2 2 2 坐标变换 变换带来方便。利用齐次坐标，使平移变换可以用矩阵表示，从而使平移变换与 三维空日j 任一点p 在直角坐标系o x y z 中的坐标为( x ，y ，z ) ，对应的齐次坐标 p 点的直角坐标是单值的，但对应的齐次坐标是多值的，即( x l ，x 2 ，x 3 】【4 ) t 与 o x y z 直角坐标系原点的齐次坐标为( o ，o o ，a ) t ，a 为非零实数。齐次坐标( 1 ， 0 ，0 ，o ) 7 表示x 轴上的无穷远点，同理。( 0 1 0 ，0 ) 7 和( 0 ，0 ，1 ，0 ) 7 分别表示y 轴和z 设坐标系o x y z 经过平移变换为坐标系o x ly lz l ，沿x 、y 、z 轴分别移动距 雌纠 其中： ) ，1 毛 l ri 是平移齐次变换矩阵。 10 o1 o o 0 0 1o ol o 0 o o 0口 06 lc 0l o口 0 6 1c o1 6 北京邮电大学硕士论文基本概念及理论 ( 2 ) 旋转齐次变换 旋转变换矩阵的形式如下： 叫台0 】 其中是3 乘3 方向余弦矩阵，第一列代表o x ly 1z j 坐标系x 轴在m y z 中各坐标轴方向的投影，第二、三列分别代表0 x ly 1z l 坐标系y 、z 轴在o x y z 中各坐标轴方向的投影。由上式可以得出分别绕三个坐标轴旋转的齐次变换矩 阵： r o ，p ) 一 r ( ) ，一) - r ( z ，口) i 1 o 0c o s 口 os i l i 口 oo o0 一s i n 日o c o s 日 0 ol c o s 疗os i n p o1o s i n 口oc o s 口 oo0 10o o s 口一s i n p os i n 日s ooo 我们可以用4 乘4 矩阵m 来表示变换矩阵，它是平移变换与旋转变换的综 合，如下式所示： 肼一 m l l坍1 2 m 2 l 历 埘3 i 小豫 m l 肌n m 1 3m 1 4 坍矗肌2 m ，3m w m 町肼“ 一弦 m 1 1 为方向余弦阵，它表示前后两坐标系之间的旋转变换，当m l i 为单位阵 时，表明m 为平移变换。m 1 2 表示后一坐标系的原点在前一坐标系下的位置。 7 0 o o 1 - 。o o l 北京邮电人学硕士论文一般6 r 机器人的数学模璎的建立及运动学分析 第三章一般6 r 机器人的数学模型的建立及运动学分析 3 1 引言 六自由度机器人的位置反解问题一直是机器人学研究领域的热点和难点之 一。一方面，反解算法中得到的输入输出方程具有很大的理论研究价值，可以 在此基础上进行诸多机构学问题的研究：另一方面，从实际应用的角度考虑， 运动学逆解是机器人进行运动规划和轨迹控制豹关键。并且，对于位置反解而 言，仅仅求得问题的解是不够的，不同的问题对所采用算法的计算精度和计算 效率等有着不同的要求。尤其是在处理实际问题时，为了满足系统的各项指标， 反解算法的准确性、实时性以及处理异常情况的能力等等都是必须要考虑的因 素。 3 2 运动学方程的建立 3 2 1 一般6 r 机器人的位姿描述 l l - l 图3 1相邻坐标变换 常用建立机器人坐标系的方法是d h 方法，相邻两坐标系扣l 和i 之间的 关系，可通过图3 1 中的各种参数来描述。相邻坐标之间的变换矩阵为： 7 一砌f ，b ) 胁船( z l ，毋) m 船( 石f 1 ，口i - 1 ) 肋f 俾b 口) 其中： - 融b ，k ；i n ，】 8 ( 3 一1 ) 北京邮电人学硕十论文一般6 r 机器人的数学模酗的建立霹返动堂j ! 堡 慨】一 b ，】i k ，】i k ，】一 c d s 见 一s i n e o 0 1 一警 矧表示绕坐标种的z 轴旋转只角的变换矩 l ooo1 l 1oo0 1 o1ooi 表示沿坐标系f l 的z 轴平移毛的变换矩阵； ：蚓 1o o 4 ，1 o 1o o i 表示沿坐标系f 的x 轴平移口j 的变换矩阵； o0 1 0i 1o o 0 1 o 哪吒一5 伽q o i 表示绕坐标系f 的x 轴旋转口；角的变换矩阵： o s 加4 jc 邯4 o l 00o1 i 则机器人末端相对于基坐标系的位姿可表示为： - ：r ：r ；r ：r ；r ：r h 矗。i 口。k 。p ：b ：i n ：k ：】陂k i 口。k 。】 ( 3 2 阵； 其中：h ，i ：p 】。l 善喜车j 是表示机器人末端位姿的矩 接近矢量，三个单位矢量两两互相垂直，用来表示机器人末端的 足正交条件：辟一一o d _ 4 4 1 ，一口- d 4 - 口一- o 。p - g 机器人末端的位置矢量。k 】，【4 。】，【4 。】为6 r 机器人的结构参数 寸给定时，该三种参数在计算过程中为常量。 9 位姿，并且满 。，p ，p ：r 表示 ，当机器人尺 北京邮电人学硕十论文一股6 r 机器人的数学模删的建立及运动学分析 机器人的位置反解问题就是已知( 3 2 ) 式中左边的机器人末端位姿参数阶】 和右边的机器人结构参数b 。】，k ，】及l ，】，求解方程右边的6 个角位移量 0 。( f - 1 ，2 ，6 ) a 3 2 2 运动学方程的求解 根据一般6 r 机器人的空日j 矢量封闭方程，将( 3 2 ) 式适当移项得到以下 矩阵形式的封闭方程： p ，r k ：】- 1 - ：】- 1 b ：】_ 1 p ：】- 1 k 。p k 。】_ 1 b ，】- 1 p 。】- 1 【k 。r l 。r b 。r - b ，k k ，p 。b 。k k 。p ，b ，i n ，k ，p 。】 ( 3 3 ) 首先，引入矩阵p ，一 七一上oo 压压。” 占占oo 压压”。 00l0 oo01 对( 3 3 ) 式作如下变换，对陋】对角化： ( p 。p ，r p ) ( p 4 口：】- 1 尸x p 1 瞄：】- 1 p ) ( p 。 】p x p 。1 k 。r p x p 一1 函。r p x p 一1 b 。r p ) - ( p 4 b ，】p x p 。k ，h p 1 【口，】p p 。p b ( p 。k ，鲰p 1 p 。b ( 3 4 ) 其中， p 。t p ，】p - t oo 0 0 薯- 1 o o oo10 0oo1 x l 一一- c o s 8 t + 话i n 8 ；p 。瓯rp = 而- 1 o o o o 而oo 0 ol0 0o ol 则( 3 3 ) 式中未知量的三角函数形式就转换成了复指数形式。将( 3 4 ) 式的 左边记为矿，右边记为，于是( 3 4 ) 可写成： 缈l - o 仇- ( u ) 口 v l iv 1 2 v 廿v v 匀v 笠 v 嚣v m v mv 矩v 拈v o0 o1 1 0 f - 1 ，2 3 ，4j - l 2 ，3 4 ( 3 5 ) 缈k - u 儿u n u 2 lu 芷 u 驰u 弛 00 u nu u u 并 u ”u ，4 ol 北京邮电人学硕十论文 一般6 r 机器人的数学模型的建立及运动学分析 上式为4 4 的矩阵，共有1 6 个矩阵元素，但是由于矩阵的最后一行为恒等式， 因此( 3 5 ) 式中，缈) j 和b 分别含有1 2 个可供选择的标量式。于是只取( 3 5 ) 式中f 一】，z 3 和，一1 ，2 ，3 ，4 的方程。 3 3 方程的消元与变量求解 分析( 3 - 5 ) 中的1 2 个标量式，左边含有一，工2 ，而三个未知量，右边含 有- ，屯，矗三个未知量。我们的求解思路是：先消，再消善。和如，然后 消工：和屯，得到关于毛的一元高次方程。 3 3 1 构造新的方程 为了满足上述消元要求，首先应得到不含墨，和。1 的方程。于是在 杪b 中选择不含而。1 的元素；在( u b 中选择不含和，。的元素，而在矩阵方 程( 3 5 ) 式中，右边3 ，4 两列的元素不含毛，左边2 ，3 两行的元素不含工；1 ， 因此得到如下四式： - ( 3 6 ) u 。一 ( 3 7 ) u 格y 蠢 ( 3 8 ) u 。- 0 ( 3 9 ) 为了进一步消元，得到没有增根的一元高次方程，需要最大限度地找出类 似( 3 5 ) 式形式的所有关系式。此过程可借助于计算机来完成，参见文献 7 1 1 。经过反复寻找、计算和判断，最终可以得到3 4 个关系式。从中选取 类似上述( 3 6 ) ( 3 9 ) 式形式的关系式，为表达方便，令 ，li 嬲珥十，* 2 ， k - 戤+ 2 ， ( ，- - ( ，嚣c ，+ c ，盯2 + c ，辩c ，m ， - + k ，+ ， 1 1 北京邮电大学硕士论文一股6 r 机器人的数学模喇的建立及运动学分析 则除了( 3 6 ) ( 3 9 ) 四式之外，还有六式满足消元的要求，分别为： u 口u i + u 1 3 u 2 + u 3 3 u m 一比k 4 + ，+ ，0 ( 3 1 0 ) 2 u ，。【，。+ 【，。u ，i 一彤。+ ( 3 - 1 1 ) 【，并u + 【，2 4 u 弘l 吒矿勰+ i 么 ( 3 一1 2 ) 【，m u 廿+ u 1 u i k ，+ k 儿 2 u 2 u m u l u h 鼻2 ，m k 么 撕* ，。一u 。一2 吒一k ( 3 一1 3 ) ( 3 一1 4 ) ( 3 一1 5 ) 式( 3 1 0 ) ( 3 1 5 ) 中对于各个变量t 来说，似乎次数应升高为4 次，即t “ 次，但通过计算机符号运算可以发现它们的高次项消失，全为零，而低次项保 留，即只有置“次项，因此与前面( 3 6 ) ( 3 9 ) 式的形式相同，可用来消 元。 整理上述l o 个方程，可以写成如下形式： ；妻耋如一k ，i 一塞妻哆“4 屯“ cr - l z ，j 。，c s l e ， 式中：厶。荟口釉工：4 4 秘和是由结构参数和机器人末端位姿参数决定 的常量。 3 3 2 方程的消元过程 上述1 0 个方程中都不含有， 的变量的数目，现在只需要讨论而， 1 ) 消和黾 即通过适当地选择方程，减少了所要求解 屯，屯，_ 和5 个未知量。 首先，从( 3 一1 6 中任取9 个方程，分别乘以颤毛3 ，则变量_ 和的 次数升高，将z 和黾。( f 一2 a 4 ) 的所有组合作为新的变量，那么9 个方程 可以写成下列矩阵形式： 【c j c ， c ，c c jc c ，c 。c ，k j 【一o ( p 1 7 ) 北京邮电人学硕士论文一般6 r 机器人的数学模型的建立及运动学分析 真每t c | i t c i lc 盘coc i ic 6c i bc - c 海c 警， tl i 堪，3 、 x 。( x 2 屯2x k ，2x k ，2 x _ ，3x ，3x 4 屯3x ：如x ，。4 屯， 口一( o0o0 ooo o0 ) 7 ； c 。，c 2 ，c ，c 。和c 。，c ，c 8 ，c ，中的元素由机器人结构参数和末端位置参 数决定；c ，中的元素为含有工。“，工：“和屯的表达式。 令( 3 一1 7 ) 式的系数矩阵行列式等于o ，得到一个方程，记做h 以。 从方程组( 3 1 6 ) 中另取不同的9 个方程，重复上述过程得到另一方程 钾蛐2 。f 叫订和p 口口们的形式如下： 荟荟荟d 一川4 巧。0 “吐2 其中d 忡只与机器人的结构参数和末端位姿参数有关。 2 ) 消厶和工，构造结式 对钾棚j 和q 蚴2 分别取共轭，得q 搬j j 和p g 搬2 2 ； 叼如j j 和口口加2 2 分别乘以毛，得到方程p 口瑚j 和t 口m 4 ，其形式与 f 驴口j 和c 口蛐2 相同； 叼船j ，f 鲫以，郇嬲和叼蛳4 分别乘以x 2 ，得到另外4 个方程f 鲫酊， 叼m 6 ，叼嬲7 和p 口蛐8 ，形式如下： 荟丕磊e 忡，如7 。0 “，6 7 ，8 ) 整理上述8 个式子，分别乘以z 2 ，以工2 ( f - o 1 ，2 3 ) 和工，( ，一叫) 的组合为变量，写成分块矩阵形式： 其中：矩阵中的各非零元素是只与变量而有关的多项式；j 的表达式为： x 。( x ：。x ：k 。x ：、。x ：毛。x ：1 工：x ：、，x ：、1 ) r 3 ) 得到一元高次方程 料 一 1 x q 时 驴 万 g g “ 筇 g g o 口 o 对 g g 盯 g g r ! ! 塞坚生叁堂堕笙奎二墼坚塑矍竺墼竺丝! ! 燮! ! 堡垫鲨丛 式( 3 一1 8 ) 系数矩阵的行列式为8 8 结式，则令行列式等于零可得到关于屯 的l 元1 6 次方程。 旦 ( 3 _ 1 9 ) 罗h ，x 1 - o 箭 3 3 3 变量求解 1 ) 求。 求解一元多项式方程( 3 一1 9 ) ，得到的1 6 个解。 2 ) 求x 2 和b 。 将矗的值回代到( 3 一1 8 ) 式的方程中，则方程只含有x ：和b 两个未知量。 把仁，工：，x ：屯，石刍工；b ，j ；，x ；屯 作为7 个新的未知量，则任取( 3 1 8 ) 式 中的7 个方程，可表达成如下线性方程的形式： c p k j q ( 3 2 0 ) 其中：p 是由各个新变量的系数组成的7 7 矩阵， 工，x ：屯屯z ，；b 工；工；屯) r ， q b 1 孽2 吼吼如吼鼋，r ，吼o - 1 ，2 7 ) 为常量。 对方程组( 3 2 0 ) 进行线性求解，即可同时求解出工2 和屯 3 ) 求丘和屯 将而，工：和南的值代入( 3 一1 6 ) 式中，取出其中的8 个方程，把 k ? 工；，工i - ，膏。工，工? ，h ，石7 屯x ，z 。南 作为8 个新的变量，将方程表达成下 列形式； 忸k x s ( 3 2 1 ) 其中：冠是由各个新变量的系数组成的8 x 8 矩阵， 工k ? 工，工；，z 工，工? - 工? 屯屯工小) r ， s 6 l5 2s ，s 4 墨s 75 8 ) r ，毛( f 一1 ，2 ，8 ) 为常量。 线性求解方程组( 3 2 1 ) ，便可得到_ 和屯的解。 4 ) 求氏。 选取方程组( 3 5 ) 中含有的任意方程，将毛，工：，而，_ 和毛的值代 入，得到一元一次方程，求解该方程即可得到的解。 1 4 北京邮电人学硕十论文一般6 r 机器人的数学模型的建立及运动学分析 整个逆解算法流程见图3 2 。 3 3 4 算法实例 建立运动学方程 0 构造8 x 8 结式d 0 由d e t i d i = 0 ，得到关于屯的一元1 6 次方程 求解关节变量吼 0 构造7 7 的矩阵方程，同时求解关节变量0 2 和0 3 上 构造8 8 的矩阵方程，同时求解关节变量o 和o s 上 由一元一次方程，求解出关节变量0 。 图3 2逆解算法流程图 一般6 r 机器人手臂的结构参数如下： a 1 = 9 0 0 ，眈= - 9 0 0 ，a 3 = 4 5 0 ，a 4 = 9 0 。，a 5 = 3 0 0 ，a 6 = 5 0 0 s l = l 笱咖，s 2 = 3 0 0 | i n m ，s 3 = 1 5 0 i l l m ，s 4 = 4 0 0 m m ，s 5 = 6 0 0 l i i m ，s 6 = 2 0 0 i n l n a 1 = 舭m ，a 2 = 5 0 0 l n l m ，a 3 = 印o l i i m ，a 4 = 加0 n l m ，a 5 = 7 0 0 | m m ，a 6 3 0 0 | i i l m 机器人末端位置反解结果见表3 1 所示。 北京邮电大学硕士论文一般6 r 机器人的数学模犁的建芷及运动学分析 表3 16 r 机器人 7 = 置反解的1 6 组解 吼一2以 以 以民 1 0 9 9 5 4 2 0 0 1一1 5 7 0 4 6 6 9 31 7 4 0 0 6 7 7 3 1 5 8 6 9 0 9 6 2 +1 4 1 8 1 7 3 5 4 51 6 6 6 1 3 5 7 5 l 9 7 3 6 5 6 2 l+ 1 5 5 3 0 8 6 i1 3 4 8 5 4 9 + i7 7 2 2 0 3 9 6 l+ 1 1 2 4 7 1 1 i8 0 7 9 0 1 5 + l 1 2 0 0 4 4 8 7 2_ 3 7 8 5 5 4 4 + 2一1 5 0 8 2 5 9 3 9 5 0 8 8 7 5 + 51 0 3 2 2 1 9 枷 一2 8 6 4 3 2 0 3 + 2 o 1 4 4 2 0 0 6 宰i5 5 6 0 9 8 2 + l3 5 9 7 7 1 7 i& 8 6 6 5 4 l + i5 6 6 1 4 3 7 + i4 3 9 7 2 4 6 i 31 2 8 2 8 6 3 61 0 9 丁7 4 2 3 0 2 1 5 1 6 6 1 0 8 81 2 2 1 6 2 0 8 61 0 8 8 6 1 6 6 1 95 0 0 3 5 8 4 4 1 3 1 2 0 洲7 4 + 23 7 。8 5 5 4 5 5 21 5 0 8 2 5 9 3 2一9 5 。0 8 8 7 5 - 4 0 1 0 3 2 2 1 9 0 1 4一2 8 j 6 4 3 2 0 3 + 4 0 1 4 4 2 0 0 6 15 5 6 0 9 8 2 + i+ 3 5 9 7 7 1 7 + i5 6 6 1 4 3 7 i0 5 6 6 1 4 3 l4 3 9 7 2 4 6 + i 5 1 0 2 0 8 9 9 8 0 81 1 9 8 1 4 3 4 8 98 4 9 3 4 6 2 4 7 41 1 3 3 5 1 3 0 71 1 6 5 1 0 3 9 12 2 3 9 8 3 6 4 1 0 9 9 5 4 2 0 +1 5 7 0 4 6 6 9 11 7 4 0 0 6 7 7 +1 5 8 6 9 ( ) 9 6 1 4 1 8 1 7 3 5 4 5 1 6 6 6 1 3 5 7 + 6 9 7 3 6 5 6 2 3 i 5 5 3 0 8 6 0 1 l 1 3 4 8 5 4 9 7 + i7 7 2 2 0 3 9 6 + i1 1 2 4 7 1 1 7 8 18 0 7 9 0 1 5 7 i - 3 7 9 7 8 4 4 8 + 9一1 3 9 2 6 8 5 5 + - 1 6 1 9 2 9 8 1 76 0 - 3 3 7 5 1 5 - 3 8 2 4 7 7 5 9 + l7 7 2 5 8 7 1 4 7 2 5 5 8 8 2 9 9 l9 7 9 7 3 9 3 3 + i+ 9 7 6 3 9 4 i4 1 1 4 3 3 7 3 l1 7 8 8 9 9 l d 8 + 1 7 8 1 6 5 3 1 2 i 5 9 7 9 4 4 3 +9 2 o l 菇0 4 0 2 +5 0 7 7 2 0 5 5 2 +- 1 1 3 5 4 5 4 5 1 3 9 4 7 2 8 5 7 - 21 0 1 2 3 0 4 0 2 8 “3 6 9 t 母7 + i4 r 4 2 2 = 1 6 8 l 3 7 1 9 7 0 0 7 + l7 6 3 0 9 r 9 5 13 8 嬲7 3 1+ 4 5 2 4 6 3 1 9踟0 0 0 0 01 6 o c l 0 0 0 01 1 0 0 c 0 0 07 0 0 0 0 0 3 0 伽0 0 0 0 02 0 0 ( ) 0 0 0 0 0 - 5 8 9 勰6 8 7 + 85 9 6 2 4 8 l + 69 2 0 3 7 2 8 9 6 1 6 7 8 3 7 4 4 1 1 0 6 1 8 5 1 9 +- 7 9 7 6 8 9 ( 1 3 + 1 0 6 7 2 0 1 9 8 3 5 i9 1 0 5 7 4 4 4 5 + i柏8 0 3 4 4 i”4 7 3 5 1 1 l 7 3 9 5 8 4 2 1 i0 1 0 8 1 0 8 5 i 2 0 j 6 6 5 9 9 8 6 3 +1 5 2 3 5 9 6 9 +1 5 5 1 0 7 7 7 - 69 3 9 2 5 5 3 9 + 73 8 7 0 5 6 2 3 “ 2 9 9 9 2 5 6 4 + 1 1 5 4 3 盯7 8 7 7 9 + i4 3 9 6 8 7 0 2 j9 7 8 6 3 7 7 5 + i5 0 5 0 1 3 7 3 i2 1 9 2 3 4 7 0 + 14 4 2 9 0 2 0 1 + l 1 24 9 3 0 4 9 2 0 9 7- 1 6 9 5 0 4 4 7 51 7 0 4 8 5 9 4 49 6 1 7 7 0 8 9 “5 1 9 4 5 1 8 5 ”- 5 1 6 4 4 2 7 7 3 _ 5 9 7 9 4 4 3 9 1 9 2 小话0 4 0 2 - 5 0 7 7 2 0 5 5 - 1 1 3 5 4 5 4 5 81 3 9 4 7 2 8 5 + 1 0 1 2 3 0 4 0 - 4 1 3 6 4 3 6 9 6 9 7 i4 8 4 2 2 2 6 8 + 1”1 9 7 0 d 7 + l + 7 6 3 0 9 8 9 i2 3 8 0 8 6 5 7 + i5 2 4 6 3 1 2 5 i 2 0 6 6 5 9 9 8 1 5 2 3 s 9 6 9 3 1 5 5 1 册o +9 3 只z 5 5 3 9 4 - 3 8 7 0 5 6 2 3 4 2 99 9 1 2 5 6 4 1 4 5 4 3 0 7 8 7 7 9 + i4 3 9 6 8 7 0 i6 9 7 8 6 3 7 7 + i7 5 0 5 0 1 3 7 i2 1 9 2 3 4 7 + i4 4 29 | d 2 0 l 3 7 9 7 8 4 4 8 5 一1 3 9 2 鹤5 5 - 91 6 1 9 2 9 8 1 一6 0 3 3 7 5 1 5 +3 8 2 4 7 7 5 9 2 1 7 7 2 5 8 7 1 6 + 1 5 9 2 5 5 8 8 2 9 9 i7 9 7 3 9 3 3 i9 7 6 3 9 4 4 2 l4 1 1 4 3 3 7 3 1 1 1 7 黯9 9 0 i 4 7 - 8 1 6 5 3 1 l - 5 8 9 4 8 6 8 7 -5 9 6 2 牾8 4 1 9 2 0 3 7 2 8 9 6 +1 6 7 8 3 7 4 4 +1 1 0 6 1 8 5 1 9 7 9 7 醴9 0 3 0 1 6 7 2 0 1 9 8 3 l6 9 0 5 7 4 4 4 l4 0 踟b 4 4 i2 7 4 7 3 5 1 1 l7 3 9 5 8 4 2 l i1 0 8 1 0 8 5 7 l 其中3 ，5 ，9 和1 2 四组解为实数解。 1 6 北京邮电大学硕十论文 一般6 r 机器人的数学模型的建立及运动学分析 通过正解对这1 6 组解进行验证，即把每一组解代入式3 2 中求币解，结果 都具有相同的末端位姿，说明此求解方法及程序是正确的。 3 4 本文小结 本章对一般6 r 机器人的反解进行了理论分析，通过矩阵变换和结式消元 方法给出了一般6 r 机器人的反解步骤，得到机器人关节参数的1 6 组解。并利 用m a t h c m a t i c a 软件对此机械臂的反解进行了编程，对一个实例进行了计算， 最后用正解检验了计算结果。 1 7 北京邮电人学硕十论文具有特殊尺寸机器人的位置反解算法 第四章具有特殊尺寸机器人的位置反解算法 4 1 引言 特殊尺寸机器人的种类比较多，每一个机器人单独编程，求解比较繁琐。 由于我们编写完成了一般6 r 机器人的位置反解程序，所以我们可以尝试利用 一般6 r 机器人的位置反解程序束解决这个问题。但当采用一般6 r 机器人的 反解程序进