（机械电子工程专业论文）六自由度机器人控制算法与实验样机研究.pdf

哈尔滨工程大学硕士学何论文 摘要 机器人是一个典型的机电一体化系统，它融合了造型技术、机械、电子、 传感器、计算机软硬件、和人工智能等众多先进技术，是进行研究和实验最 为理想的教学平台。本课题根据实际项目的需要，对六自由度机器人的运动 学求解与轨迹规划，上位机与下位机控制系统中的关键技术进行了深入研究。 论文采用d - h 的下关节设置法建立六自由度机器人的连杆坐标系矩阵， 并对其运动学求解问题进行分析研究。确定了六自由度机器人运动学求解问 题在软件中的实现策略，同时基于软件程序验证了运动学分析的正确性与可 行性。 论文阐述了上位机软件系统设计和实现的关键技术，根据实用性及易用 性原则将软件系统划分为六个模块，对各个模块的功能以及实现技术进行了 详细分析，同时确定各个模块之间的交互接口以及数据实时交互的实现策略。 分析了下位机控制系统的工作原理与软件控制算法，针对六自由度机器 人在控制过程中存在的速度抖动问题，提出力矩补偿控制方案；同时，对六 自由度机器人的p t p 控制运动与c p 控制运动的实现策略进行了分析。 给出了软件系统的运行结果，根据软件系统需求改进三维仿真界面f 通 过六自由度机器人实际样机的研究调试，验证c p 控制方案的可行性，最后 通过示教实验验证上位机控制系统与下位机控制系统的合理性。 关键词：运动学；上位机系统；控制算法 哈尔滨t 程大学硕士学位论文 a b s t r a c t r o b o ti sat y p i c a lm e c h a n t r o n i c ss y s t e ma n dt h em o s ti d e a lt e a c h i n gp l a t f o r m o fe x p e r i m e n ta n dr e s e a r c l l ，i tc o v e r s al o to ff i e m s s u c ha s m o d e l i n g t e c h n o l o g y , m e c h a n i s m ，e l e c t r o n ，s e n s o r , c o m p u t e rs o f t w a r ea n dh a r d w a r e ，a r t i f i c i a l i n t e l l i g e n c e a n do t h e ra d v a n c e dt e c h n o l o g i e s b a s e do nt h er e q u i r e m e n to f p r a c t i c a lp r o j e c t ，t h ek i n e m a t i c ss o l u t i o na n dt r a j e c t o r yp l a n n i n go f6d o fr o b o t a r en o to n l yi n t e n s e l ys t u d i e d ，b u tt h ek e yt e c h n o l o g yo fh o s tc o m p u t e ra n dl o w e r c o m p u t e r i sa n a l y z e dd e e p l y t h ek i n e m a t i c ss o l u t i o na n dt h ee x p r e s s i o nf o rp o s i t i o na n dp o s ea r ef u r t h e r s t u d i e du n d e rl i n k - p o l ec o o r d i n a t es y s t e mw i t ht h eh y p o z y g a lm e t h o d t h e s t r a t e g yf o rs o f t w a r et or e a l i z ek i n e m a t i c ss o l u t i o ni sd e t e r m i n e d , a n dp r a c t i c a l p r o p e r t yo ft h ef o r w a r da n di n v e r s es o l u t i o ni sv e r i f i e db ys o f t w a r ep r o g r a m t h ek e yt e c h n o l o g ya b o u td e s i g na n dr e a l i z a t i o no fh o s tc o m p u t e ri s d i s c u s s e d i no r d e rt om e e tt h en e e do fp r a c t i c a l i t ya n da d a p t a b i l i t y , t h es o f t w a r e s y s t e mi sd i v i d e di n t os i xm o d u l e s t h ef u n c t i o na n di m p l e m e n t a t i o nt e c h n o l o g y o f e a c hm o d u l e 雒w e l l 舔t h er e a l i z a t i o ns t r a t e g yo fi n t e r a c t i v ei n t e r f a c ea n d d a t a r e a l t i m ei n t e r c h a n g ea r ea n a l y z e d b o t ht h ee s s e n t i a la l g o r i t h mo fp l a n n i n gt r a je c t o r ya n dk e ya l g o r i t h m so f l o w e rc o m p u t e ra r es t u d i e dd e e p l y a i m i n ga tt h er i p p l eo fv e l o c i t yd u r i n g6d o f r o b o tc o n t r o l ，t h et o r q u ec o m p e n s a t i o ni sp r o p o s e d m e a n w h i l e ，p t pc o n t r o la n d c pc o n t r o la r ed e s c r i b e dd e t a i l y t h es o f t w a r es y s t e mi se x e c u t e da n db a s e do nt h a tt h et h r e e d i m e n s i o n a l i n t e r f a c ei si m p r o v e d t h ef e a s i b i l i t yo fc pc o n t r o li sv e r i f i e db yp r o t o t y p ed e b u g a tl a s t ，t h er a t i o n a l i t yo fs o f t w a r es y s t e ma n dh a r d w a r es y s t e mi sp r o v e dt h r o u g h t e a c h i n ge x p e r i m e n t k e yw o r d s ：k i n e m a t i c s ；h o s tc o m p u t e r c o n t r o la l g o r i t h m 哈尔滨工程大学 学位论文原创性声明 本人郑重声明：本论文的所有工作，是在导师的指导下，由 作者本人独立完成的。有关观点、方法、数据和文献的引用己在 文中指出，并与参考文献相对应。除文中已注明引用的内容外， 本论文不包含任何其他个人或集体已经公开发表的作品成果。对 本文的研究做出重要贡献的个人和集体，均已在文中以明确方式 标明。本人完全意识到本声明的法律结果由本人承担。 作者( 签字) ：尹止寸 日期： 砷弓胁日 哈尔滨工程大学 学位论文授权使用声明 本人完全了解学校保护知识产权的有关规定，即研究生在校 攻读学位期间论文工作的知识产权属于哈尔滨工程大学。哈尔滨 工程大学有权保留并向国家有关部门或机构送交论文的复印件。 本人允许哈尔滨工程大学将论文的部分或全部内容编入有关数据 库进行检索，可采用影印、缩印或扫描等复制手段保存和汇编本 学位论文，可以公布论文的全部内容。同时本人保证毕业后结合 学位论文研究课题再撰写的论文一律注明作者第一署名单位为哈 尔滨工程大学。涉密学位论文待解密后适用本声明。 本论文( 瓯在授予学位后即可口在授予学位1 2 个月后 作者c 签字c j 正号聊( ：乡 吼叶硼q 日叶朔f o 昌 口 哈尔滨t 挥大学硕十学伊论文 第1 章绪论 1 1 课题研究背景及意义 机器人是机电一体化的代表性产物，随着网络与集成技术的迅速发展， 机器人除了在制造业、农业、医疗、海洋开发、航天工程等方面得到越来越 广泛的应用外，已经开始渗透到人们生活的各个方面，随着工业机器人向 更深更广方向的发展以及机器人智能化水平的提高，其应用范围仍然会继续 扩大f 2 1 ，对机器人技术与相关方面的入才需要将会迅速增加，而教学机器入 作为这种先进技术的实物载体，在教学中的位置也越来越重要。 教学机器人作为机器人教学过程中必不可少的工具，除了能进行实验演 示外，还能让学生自己动手对机器人的各项功能进行研究和开发，增强学生 对机器人的感性认识，揭开机器人的神秘“面纱”，使学生尽快掌握机器人的 相关软硬件知识与机械结构设计。此外，具有开放式控制器的教学机器人还 能满足使用者深层次的科学研究需要。因此，一种专门面向教学研究，旨在 培养机器入应用基础入才的教学机器入成为国内机器人研究的热点之一玛1 。 目前国内教学机器人的研究规模较小，部分产品价格偏高，而且开放性 与实用性较弱，很难实现大规模的投入使用。因此亟需加大教学机器人的开 发研究工作【4 1 。 本课题模仿m o t o m a n u p 6 构型机器人设计六自由度机器人，用于实 现各种操作与动作功能，同时可以实现在无人参与下，自动地完成操作作业 与期望动作。 1 2 国内外与本课题相关领域研究现状 机器人( r o b o t ) 是1 9 2 0 年由捷克作家卡雷尔查培克在剧本中塑造的一 个具有人的外表、特征和功能，愿意为人服务的机器奴仆“r o b o t a ”一词衍 生而来的。可以这样说：机器人是一个在三维空间中具有较多自由度的，并 能实现诸多拟人动作和功能的机器。 回顾机器人的发展历程，机器人的发展主要经历了三个时代的演变晴1 ： 第一代是顺序控制的，不具有感觉装置( 传感元件) 的机器人。它是依靠 1 哈尔滨t 秤大导：5 员十学伊论文 人们输入程序，能重复进行多种操作的系统。由于其不具备传感器的反馈信 息，因此不能在作业过程中从外界获取实时信息来改善自身的运动特性，故 应用范围和精度受到控制。 第二代是具有简单传感器反馈信息的机器人。它有若干传感器，能对自 身的实际位置、方向、速度、力等进行测量，能通过“视觉”、“触觉”等传 感能力对外部环境进行实际探测，从而由这些反馈信息在事先编好的算法和 程序指导下对操作过程进行调整。 第三代机器人是能感知外界环境与对象物，并具有对复杂信息进行准确 处理，对自己行为作出自主决策能力的智能机器人。这类机器人带有多种传 感器，使机器人可以知道其自身的状态、感知外部的环境，根据变换的内部 状态与外部环境自主决定自身的行为。 1 2 1 国外机器人的研究现状 世界上第一台机器人诞生在美国，世界上最早使用机器人的也是美国， 1 9 6 1 年，美国通用汽车公司年d c h r y s l e r 公司购买了第一批商业化生产的机器人 系统，开创了机器人用于汽车工业的先河。目前美国虽在机器人的应用领域、 全国总拥有量、年产量、年产值等落后于日本，但在基础研究( 如人工智能) 、 新技术开发( 直接驱动机器人) 以及某些特种应用领域( 空间站用机器人) 开发 方面仍处于世界领先地位。按拥有量及年装设台数、国内产量来说，美国仍 是仅次于日本的机器人大国。 在机器人研究方面，日本起步虽比美国稍晚，但由于国内劳动力资源严 重不足，以及政府政策的扶持，使机器人的推广与应用得到了迅猛的的发展。 到8 0 年代中期，其机器人的产量及安装台数己大大超过美国，同时正以美国 两倍的速度推广机器人的应用，被誉为“机器人王国”| 6 1 。 西欧是仅次于日美国家的机器人生产基地，也是日美机器人的重要市场。 在西欧众多的国家中，以德国、意大利、法国、英国、瑞典和西班牙等为西 欧主要生产和使用机器人的国家【7 1 。 现阶段国外机器人技术已经达到相对成熟的阶段，在今后几年里的发展 有以下几个趋势： 1 、工业机器人性能不断提高( 高精度、高速度、高可靠性、便于操作和 2 哈尔滨7 - 程歹：学硕十学传论文 维修) 协。 2 、机械结构向模块化、可重构化发展。 3 、工业机器人控制系统向基于p c 机的开放型控制器方向发展，便于标 准化、网络化。 4 、虚拟显示在机器人中的作用己从仿真、预演，发展到用于过程控制， 如使遥控机器人操作者产生置身于远端作业环境中的感觉来操纵机器人旧。 5 、当代遥控机器入系统的发展特点不是追求全自治系统，而是致力于操 作者与机器人的人机交互控制。 1 2 2 国内机器人的研究现状 我国的机器人研究开始于2 0 世纪7 0 年代初，先后经历了上世纪7 0 年代 的萌芽期，8 0 年代的开发期和9 0 年代的适用化期。前十年处于研究单位自 行开展研究工作状态，发展比较缓慢。进入8 0 年代后，我国机器人技术的开 发与研究得到了政府的重视与支持。“七五”期间，国家投入资金，对工业机 器人及其零部件进行攻关，研制出喷涂、点焊、弧焊和搬运机器人等五类机 型。特别是在“七五”、“八五”、“九五”机器人技术国家攻关、“8 6 3 ” 高技术发展计划的重点支持下取得了重大发展。9 0 年代初期，我国的工业机 器人也在实践中迈进了一大步。点焊、弧焊、装配、喷漆、切割等各种用途 的工业机器人相继出炉，形成了一批机器人产业化基地。从“十五”开始， “8 6 3 ”机器人技术主题对机器人技术发展作了重要战略调整，从单纯的研发 机器人技术向机器人技术与自动化工艺装配扩展，将中心任务定位为：研究 和开发面向先进制造的机器人制造单元及系统，自动化装配机器人，促进传 统机器的智能化和机器人产业发展，提高我国自动化技术的整体水平引。 经过3 0 多年的发展，我国己基本掌握了机器人操作机的设计制造技术、 控制系统硬件和软件设计技术、运动学和轨迹规划技术，生产了部分机器人 关键元器件，开发出喷漆、弧焊、点焊、装配、搬运等机器人；其中有1 3 0 多台机器人在二十多家企业的近3 0 条自动喷漆生产线上获得规模使用，弧焊 机器人已应用在汽车制造厂的焊装线上。 虽然我国机器人的研究在一些方面已经达到了世界先进水平，但仍存在 着一些问题。一方面，在产业化上与国际上有着一定的差距。虽然当前工业 哈尔潺丁弹大警硕十学伊论文 机器人市场已经比较成熟，但是进口机器人占了绝大多数；另一方面，我国 机器入研究多是借鉴外国先进技术，进行二次开发的多，自身技术创新较少。 尽管在某些关键技术上有所突破，但还缺乏整体核心技术的突破，具有自主 知识产权的工业机器人则很少。目前，我国机器人技术相当于国外发达国家 2 0 世纪8 0 年代初的水平，特别是在制造工艺与装配方面，不能生产高精密、 高速与高效的关键部件。 我国机器入的发展战略是根据2 1 世纪初国民经济对先进制造及自动化 技术的要求，瞄准国际前沿高技术发展方向创新地研究和开发工业机器人技 术领域的基础技术、产品技术和系统技术。 1 3 国内外教学机器人发展状况 教学机器人是一个典型的机电一体化系统，它融合了造型技术、机械、 电子、传感器、计算机软件、硬件和人工智能等众多先进技术，是进行工程 训练、教学和实验研究的最理想平台驯。实践表明，在通过教学机器人进行 教学过程中，采用多种模式组织教学，对学生获取知识能力、应用知识能力、 思想道德、创新能力的培养是行之有效的，同时也改善了课堂教学效果和学 生的学习方式”3 | 。 国外教学机器人的研究开展较早。早在上世纪六七十年代，日本、美国、 英国等发达国家已经相继在本国大学里开展了对教学机器人的研究，到了八 十年代他们在中小学也进行了简单的机器人教学，在此过程中也推出了各自 的教学机器人基础开发平台| 1 引。 目前，国内已有不少高校和公司研制开发出了较为成型的教学机器人， 例如上海交通大学、哈尔滨工业大学、清华大学、首钢莫托曼机器入有限公 司等，这些教学机器人都各具特点，但仍有不少方面需要进一步开发，特别 是作为教学机器人核心运动控制器的开放性。另外还有许多高校与企业也正 投入到相关产品的研发工作中，相信不久会有更多相应产品的问世。 由威海凯能精密仪器有限公司生产的t s 6 0 1 教学机器人，是专门为大专 院校机器人教学实验而设计的一款高精度5 轴交流伺服机器人，如图1 1 所 示，它是在充分研究国外名牌产品的基础上，针对我国的教学特点而优化设 计的新一代全闭环控制的交流伺服机器人教具。它采用大屏幕彩色液晶显示 4 晗尔溟工程九学碗士学位论文 器作为人机交互界面，教师可以利用可视化的操作环境，通过点击鼠标，就 可以很方便地操控整个机器人系统。这种机器人教具既可以通过工业标准的 串行接口与p l c 或p c 机通讯，共同组成复杂的加工装配控制系统，也可以 自成体系，独立完成各种机器人动作的编程和调试任务。t s - 6 0 1 教学机器人 具有以下优点： 1 、有5 个自由度的关节式机器人，整体重复精度优于05 m m ，抬举力 20 k g 。 2 、通过遥控键盘实现对每个轴的单独控制，x ，y ，z 三维控制和编程调 试。 3 、通过r s 一2 3 2 或r s _ 4 8 5 接口与p l c 或p c 联机，提供更广阔的应用 空间。 4 、各轴运行速度为无极变速，各轴速度可独立设定可选的数字量输入输 出接口。 e = 百一i 图11 成海凯能精密仪器有限公司t s 6 0 1 教学机器人 由哈尔滨工业大学博实精密测控有限公司研制的六自由度串联型教学机 器人，是一种以典型的工业机器人为参考对象，可实现自动搬运、装配、焊 接、喷涂等作业任务。如图12 所示，通过该系列教学机器人可使学生能够 模拟工业现场的实际运行状况，其结构紧凑，工作范围大，具有高度的灵活 性，是进行运动规划和编程系统设计的理想对象。串联型六自由度教学机器 人具有以下特点： 1 、机构采用关节式结构，按工业标准要求设计，速度快、柔性好。 2 、模块化结构，简单、紧凑，完全满足实验的要求。 3 、控制系统采用w i n d o w s 系列操作系统，二次开发方便、快捷，适用 哈尔滨工程大学硕士学位论文 于教学实验。 4 、提供通用机器人语言编程系统，可通过图形示教自动生成机器人语言 等程序。 5 、提供实验教材，内容涵盖机器人运动学、动力学、控制系统的设计、 机器人轨迹规划等。用户从中选择相关内容满足不同层次的教学实验需要。 图12 六自由度串联型敦学机器人 本实验室自主研发的六自由度机器人是以m o t o m a n u p 6 机器人为参 考，充分研究现阶段国内外教学机器人的特点，针对课题要求研制开发出的 开放式教学机器人，如图i3 所示，通过示教再现操作能够实现机器人的基 本作业任务，可通过编辑框、控件或鼠标操作完成机器人的期望控制。自主 研发的六自由度机器人具有以下特点： 1 、基于v i s u a lc + + 6 0 和o p e n g l 技术实现机器人的虚拟主从控制，设 计井实现具有六自由度的关节式机器人，其工作范围大、速度快、精度高。 2 、上位机控制系统采用v i s u a lc + + 60 设计实现，二次开发方便、快捷， 适用于开放式教学实验。 3 、应用计算机图形学知识建立三维仿真模块，提供友好的人机交互界面， 能够为操作者提供更多的场景信息和更逼真的临场感受，极大提高人机交互 性能。 4 、采用分布式控制原则，即系统、子系统和模块分级式的控制结构，其 构造符合系统可移植性和透明性特征。 5 、采用c a n 总线通讯，便于各独立的功能模块通过通信实现信息交换， 满足实时控制要求。 哈尔滨工程大学硕士学位论文 _ 啊 图13 六自由度机器人 14 论文主要研究内容 本课题的主要任务是针对实际项目设计并开发一套六自由度机器人软件 系统，采用面向对象的分析设计方法对软件系统结构进行模块化设计与实现， 并根据实际样机建立运动学方程、进行运动学分析与轨迹规划。 论文主要工作如下： 1 、建立六自由度机器人数学模型，进行运动学分析，并通过上位机软件 系统对运动学分析结果进行验证。 2 、阐述六自由度机器人上位机软件系统的核心技术，确定其设计方案， 在此基础卜对六自由度机器人软件系统进行模块化设计，并给出各个模块的 具体实现策略。 3 、分析并优化硬件控制系统，实现重力矩补偿，完成最优控制。 4 、研究轨迹规划问题与路径控制策略，完成c p 运动与p t p 运动的最优 控制。 5 、介绍六自由度机器人实验系统，给出上位机软件系统运行结果，并提 出三维界面的改进策略；验证通讯时效性与c p 运动控制特性，并通过实际 样机的作业实验，验证了控制的合理性。 哈尔滨t 稃大学硕十学伊论文 暑i i i i i i i i i i i i i i t i i i i i i i i i i i i i 音备音；音宣i i i i i i i i i i i i i i i 第2 章六自由度机器 2 1 机器人运动学分析刁毛 动学研究 机器人可以看成由一系列刚体通过关节( 运动副) 连接而成的一个运动 链。机器人运动学则描述了机器人关节与组成机器人各刚体之间的运动关系， 是设计、规划、控制机器人的基础。它主要包括两方面的问题：运动学正解 分析与运动学逆解分析”5 1 订3 。 2 2 运动学正解研究 运动学正解是给定机器人各关节角度，要求计算机器人末端姿态和位置 的运动学问题。 2 2 1 末端与基座的变换矩阵 本课题所研究的六自由度机器人按d h 法8 以m 建立坐标系，并将4 、5 、 6 关节3 个轴线相交的关节原点选在同一点上。结构简图与坐标系建立如图 2 1 所示，其d h 参数表见表2 1 所示。 图2 1 六自由度机器人连杆坐标系图 8 哈尔滨t 稃大学硕十学伊论文 表2 1d h 参数表 根据下关节设置法的变换矩阵可确定相邻变换矩阵： ：t ： ! 丁2 t 弘【 ： 六自由度机器人末端和基座( 即0 坐标系) 的变换矩阵为： 9 一j 2 0 一c 2 0 一s 4 0 一c 4 0 一s 6 o c 6 o 连杆 c i 一1 嘭_ 伊 d f 关节变量范围 序号f m m ( 。)( 。)r 啪( o ) l 口o2 0= o鼠( 0 ) d j = o 1 7 0 _ 一1 7 0 2 口，2 1 2 0 瞄= 一9 0 只( 9 0 ) d ，= 0 1 3 5 _ 4 5 3口，- 2 2 0 c z ，= o 敏( 0 ) 以：09 0 一6 0 4 a 3 2 8 0 ( k = 9 0 见( o ) d 4 = 2 0 0 1 8 0 一+ 1 8 0 5 口42 0吼= 9 0敏( 0 ) d 。= 01 3 5 1 3 5 6口；= 0口。= 一9 0 眈( o )d 6 = o 1 8 0 1 8 0 1；，_1 呸o 0 吒矗o， 0 o ， 0 1 o 0 o l 0 o o l 0 o 乞。 以o q o 气。 飞。 一 0 0 o 1 0 0 l o o 1 o 0 o o- o o 1 o 飞q o o 飞色o o 吖一。 白o q 墨0 0 。，。，。l = 吃如o o 。，、。l = g o o 1，j x y ： 以b 以，q 吩巳o以乃以o取b 他0 。，。l i l r丁 哈尔滨t 稗大学硕十学伊论文 其中， 力，= ( c 1 宰c 2 木c 3 c l 木s 2 术s 3 ) 木c 4 + s l 木s 4 木c 5 一( cl 车c 2 车s 3 + cl 幸s 2 宰c 3 ) 掌s 5 木c 6 ( c l * c 2 术c 3 c 1 木s 2 木s 3 ) 牵s 4 一s l * c 4 * s 6 行。= ( s 1 宰c 2 卑c 3 一s l 采s 2 宰s 3 ) 宰c 4 - c 1 宰s 4 搴c 5 一( s l 木c 2 术s 3 + s l 木s 2 木c 3 ) 木s 5 木c 6 一 ( s l 宰c 2 宰c 3 - s l 木s 2 木s 3 ) 木s 4 + c 1 木c 4 木s 6 = - ( s 2 宰c 3 + c 2 宰s 3 ) 木“掌c 5 + ( s 2 枣s 3 - c 2 宰c 3 ) 掌s s 术c 6 + f s 2 木c 3 + c 2 术s 3 ) 堆s 4 宰s 6 q 二 【( c l 半c 2 术c 3 一c l 宰s 2 宰s 3 ) 木c 4 + s l 宰s 4 术c 5 一( c l 木c 2 书s 3 + c l 毒s 2 木c 3 ) 木s s 球s 6 ( c 1 木c 2 木c 3 - c l 木s 2 木s 3 ) 半s 4 一s l 木c 4 木c 6 0 。= - ( s 1 木c 2 木c 3 一s l 宰s 2 宰s 3 ) 木c 4 c l 宰s 4 枣c 5 - ( s 1 c 2 木s 3 + s l 母s 2 水c 3 ) 木s 5 ) 术s 6 一 ( s 1 木c 2 术c 3 - s l 木s 2 木s 3 ) 木s 4 + c l 木c 4 木c 6 d ：= ( s 2 ：c 3 + c 2 宰s 3 ) 木c 4 车c 5 一( s 2 术s 3 - c 2 木c 3 ) 幸s s 水s 6 + ( s 2 木c 3 + c 2 木s 3 ) 木s 4 木c 6 q 二 ( c l 术c 2 宰c 3 一c l 术s 2 木s 3 ) 枣c 4 + s l 宰s 4 木s 5 4 - ( 一c l 枣c 2 宰s 3 一c l 幸s 2 木c 3 ) 宰c 5 a 、= - ( s 1 木c 2 水c 3 一s l 木s 2 木s 3 ) 幸c 4 一c l 车s 4 半s 5 + ( 一s l 冰c 2 木s 3 一s l 术s 2 木c 3 ) 木c 5 吐= 一( 一s 2 宰c 3 一c 2 木s 3 ) 木c 4 宰s 5 + ( s 2 木s 3 一c 2 幸c 3 ) 宰c 5 见= a 3 率c l 宰c 2 半c 3 一a 3 木c l 木s 2 木s 3 一d 4 幸c l 掌c 2 木s 3 2 0 0 木c l 水s 2 术c 3 + a 2 木c 1 木c 2 斗。a l 木c l p 。，= a 3 车s l 聿c 2 宰c 3 - a 3 木s l 宰s 2 木s 3 一d 4 车s l 幸c 2 木s 3 一a 2 木s l 木s 2 章c 3 + a 2 丰s l * c 2 + a l * s l p ：= - a 3 木s 2 拳c 3 - a 3 宰c 2 木s 3 + d 4 幸s 2 木s 3 - d 4 木c 2 宰c 3 一a 2 车s 2 玎= f 玎。，”，z i4 为法向矢量，为机械人末端坐标系的o 轴在其坐标 系下的表示；d = iq ，d ，吱l 。为方向矢量，为机械人末端坐标系的d 】，轴 在其坐标系下的表示；口= fg ，q ，呸1 1 为接近矢量，为机械人末端坐标系 的o z 轴在其坐标系下的表示：p = l 见，p ，p ：1 1 为机械人末端在其坐标下 的位置矢量。其中s l = s i n 鼠，c l = c o s 舅。 从建立的坐标系情况可以看出，基座坐标系与第l 关节坐标系重合，第 6 关节坐标系建立在三轴交点上。在实际应用中，由于作业环境与作业任务 1 0 哈尔溟t 稗入学i 员十学伊论文 的要求，机器人的基座位置与安装工具也各不相同，这时所建立的运动学方 程不能够直接应用。在这种情况下，应根据作业环境与作业任务的要求，利 用求得的变换矩阵7 左乘。扩，。扩为基座坐标系 o ) 相对于世界坐标系 d ，) 的位姿矩阵；利用7 右乘7 ，7 为工具坐标系 f 1 相对于末端f 6 1 的位姿矩 阵。针对某一具体要求，可得。与7 为常数矩阵。 因此，在新的作业环境与作业任务条件下，通过”7 = 。了7 即可得到 最终的正解结果。 2 2 2 六自由度机器人位置与姿态描述 1 、位置描述 对于直角坐标系 a ，可以用一个矢量来表示一个点。p ，并且可等价地 被认为是空间的一个位置，或者简单地用一组有序的三个数字来表示忙。矢 量的各个元素用下标x ，y 和z 来标明： 4 e = l - e 只只1 总之，用一个位置矢量可以描述空间中点的位置。因此根据表2 1 可确 定末端关节的初始位置为： a p ：f 3 2 00 3 0 0 7 2 、姿态描述 在描述机器人末端关节时，不仅需要描述末端关节空间中的位置，还需 要描述末端关节所处空间中的姿态。本课题中，采用r p y 角方法来进行六自 由度机器人末端关节的姿态描述。 r p y 角是描述船舶在海中航行时姿态的一种方法。将船的行驶方向取为 z 轴，则绕z 轴的旋转( 口) 称为回转：将绕y 轴的旋转( ) 称为俯仰；而将绕x 轴的旋转( 力称为偏转。 根据六自由度机器人的末端关节坐标系与基座坐标系，如图2 2 所示， 可确定初始姿态值为口= 1 8 0 , = 一9 0 。，y = 0 。，按照母( ”，r y ( ) ，r ( 口) 的顺序 进行旋转，即 r p r y = r ：( 口) 尺，( ) r ，( 力 哈尔滨t 稗：j ：学硕十学传论文 c 伢c f lc a s , s s y - s 。i c y s e c f ls a s f l sy + c a c y - s pc p s y 由于末端和基座( 即0 坐标系) 的变换矩阵已知，所以可以求得 = at a n2 ( 一，7 ：，刀；+ 刀：) ( 2 1 ) 1 ) 当c 0 时，可以用，7 j ，c 除以刀，c 得到口角，d ：c 除以口：c 得到，角，即 口= a t a n 2 ( 一，? ：，? ：+ 咒i ) ( 2 2 ) y = a t a n 2 ( 0 ：c ，a ：c )( 2 3 ) 2 ) 当c p = 0 时，可求得= + 9 0 。，在这种情况下仅能求得口和y 的和 或差。在这种情况下一般取口= 0 ，求得结果如下： 假若= 9 0 。，解得： 假若= - 9 0 。，解得： y o 8 = 9 0 1 口= 0 。 ( 2 - 4 ) 7 = a t a n2 0 1 2 ，r 2 2 ) p = 一9 0 0 口= 0 。 y = a t a n 2 0 1 2 ，吃2 ) l l x 。 图2 2 坐标系变换图 1 2 ( 2 - 5 ) s c 吖 7 y y 吼 c c 脚胁啦 口 优 缎 孵 哈尔滨丁挥大学硕十学伊论文 2 2 3 运动学正解程序实现研究 在运动学正解算法中，计算的顺序是：已知各关节角度一确定各连杆的 变换矩阵一通过变换矩阵求得运动学方程一由运动学方程中的p ，p p ， 确定位置值一通过r p y 角求解运算确定口，夕，y 姿态值。具体实现流程如图 2 3 所示。 图2 3 运动学正解流程图 2 3 运动学逆解研究 运动学逆解是已知末端的位置和姿态，求机器人对应这个位置与姿态的 全部关节角。 哈尔滨t 稃歹：学 t 贡十学伊论文 2 3 1 运动学逆解概述 在进行机器人控制时，通常是规划末端工具以期望姿态到达目标位置。 根据当前的位置与姿态值反解出各关节角度，因此运动学逆解显得更为重要： 同时，由于进行逆解时会存在可解性、解的存在性、多重解问题，从而其求 解难度也要大于运动学正解问题把。 求逆向运动学的解时，应主要考虑运动学的多解性与可解性问题。造成 运动学逆解具有多解的原因是由于反三角函数方程产生的，而对于机器人的 运动只有一组解与实际情况相符合，为此应剔除多余解，从而做出正确判断 命令忙2 | 。剔除多余解的办法： 1 、根据角度极限选择合适的解。 2 、选择一个最接近的解。 3 、根据避障要求选择合适的解。 2 3 2 运动学逆解解算 在己知位姿求解各关节角度时，可由, t ( s i ，幺，a ，岛) = 。7 确定的1 2 个非 线性方程( 6 个是独立方程) 来求解岛，岛，眈。由。7 = 。扩o r ( q ，岛，人，皖) 7 作 变换可得( q ，幺，人0 6 ) = ( 。7 ) 叫。：r ( 7 ) ，当”扩和7 给定时，右边通常是常 矩阵。因此，求解的核心问题是：己知：t 求解舅，岛，皖的问题。 1 、求解鼠，良，幺 由扩= ：r l r 芗作变换，得： j 丁芗= ( 扩) 叫( ：丁) 一( ：丁) ( ：r ) 。1 左边：2 r 7 - , 铲3 t 4 ，丁：i ；r ；只；只；e 2 i o1 其中：；只= a 3 c 3 - a , s 3 + a 2 ；0 = a 3 s 3 + 以c 3 ；e = o 1 4 哈尔滨t 稗大学硕十学伊论文 右边：( ! 丁) 一( 7 ) 一：丁( ) 其中： = 胪0 1 一刁7 l1 只= q c ：p ，+ c 2 p ，一是只一q c 2 只= 一c i s 2 以一s i s 2 p ，一c ：p ：+ 口1 s 2 = 一s j p x + c 叭p 1 ) 求解岛 由于左、右对应元素( 3 ，4 ) 相等，得： 一s j p f + c l p j = 0 则q = a t a n 2 ( p 1 ， 2 ) 求解岛 p ，) 或q = a t a n 2( - p 1 - p ，) 由于左、右对应元素( 1 ，4 ) 和( 2 ，4 ) 相等，得： a 3 c 3 一以s 3 + c 1 2 - - c l c 2 p j + s j c 2 p j 一s 2 p ：一口l c 2 ； a 3 s 3 + d 4 c 3 = 一c 1 5 2 p ，一s i s 2 p ，一e 2 p ：+ a 1 s 2 ； 为了解算容易，在这里设置中间变量u , v , m 。h ，令： “= q p ；+ & p ，一口l ，v = p ：，则： a 3 c 3 一d 4 s 3 = c 2 一5 2 v 一口2 a 3 s 3 + a , c 3 = 一s 2 z , 一c 2 v 将式( 2 6 ) 和( 2 - 7 ) 两端平方后相加消去s ；、c 3 得： 砬= a t a n 2 ( v ，川士a t a n 2 ( “芦i = ，所) 其中m = 2 + 以2 一甜2 一v 2 一呸2 2 a ， ( 2 6 ) ( 2 7 ) 若出现 f 五可 + ( 屯邑一c j c 3 ) q 1 6 相等，则： ( 2 1 2 ) ( 2 1 3 ) 1j 巳以o 1lllj ，仫0亿& o b ” 哈尔滨t 桴大学硕十学位论文 将门，、聍一，7 和o x 、0 v 、d ：代入式( 2 1 2 ) 和( 2 1 3j 经解算得： o o = a t a i o ( 一乞j 5 ，t 1 岛) 当最= 0 时，六自由度机器人处于奇异位形，此时关节轴4 和关节轴 6 成一条直线，机器人末端连杆的运动只有一种。在这种情况下，所有可能 的解都是只和馥的差。 式( 2 8 ) 左、右对应元素( 1 ，1 ) 和( 1 ，2 ) 相等，则： c 4 c 5 c 6 一$ 4 s 6 = ( c i c ：乞一c i s 2 j 3 ) 1 1 ，+ ( s i c ：c 3 一s i s ：j 3 ) 朋1 一( 5 2 c 3 + c 2 s 3 ) 珂： ( 2 - 1 4 ) 一c 4 c 5 s 6 一s 4 c 6 = ( c i c 2 c 3 一c i s 2 巳) q + ( s i c 2 c 3 一s i s 2 s 3 ) d ，一( s ：c 3 + c 2 s 3 ) 呸 ( 2 1 5 ) 将刀，、刀一刀和o x 、0 0 代入式( 2 1 4 ) 和( 2 。1 5 ) 经解算得： 幺+ 皖= a t a n 2 ( 一：，) ，此时，可以令包为当前角度值，根据已确定的只， 求得幺的角度值。 2 4 3 运动学逆解程序实现研究 基于逆解的多解性，在进行机器人控制时，由于系统只能选择一个解， 因此多重解现象会产生一些问题。解的选择标准是变化的，然而比较合理的 选择应当是“最短行程”解。最短行程解就是使得每个关节在其极限范围 内的移动量最小，即对于机器入的当前位置柬说只需要对逆运动学程序输入 一个小位移量即可，在第四章中将作详细介绍。这样，利用算法能够选择关 节空间内的最短行程解。 从上一节逆运动学分析的结果可知，逆解有8 种可能的组合，如图2 4 所示。然而，控制系统只能按照具体的某一种解来执行，这就要求在多种可 行解中根据实际情况做出取舍。 鼠 ， 岛 良 z ， ：，一壬一象一一一壬一壬： 图2 4 逆解树图 1 7 哈尔滨工程大学硕士学位论文 通过加权计算“最短行程”解，使得这种选择侧重于移动小连杆而不是 6 移动大连杆。因此通过引入最小偏差准则以= n n ( t 只( 宓+ 1 ) 一只( 七) 】2 ) 来选 百 取一组最优解，其中鼠( 七+ 1 ) 为目标角度，e ( 七) 为当前角度，毛为权值。在 具体运算中，令1 ，2 ，3 关节的置= 1 ，即毛= 岛= 如= 1 ；令4 ，5 ，6 关节的 毛= 0 2 ，即屯= 致= 恕= 0 2 。在这里分析一个位置姿态值 ：t = f 1 00 0 ；0 10 1 2 0 ；0 01 2 2 0 ；0 0 0 1 1 ，得到的结果如表 2 2 所示，与几何方法分析结果一致。表中幸的角度值为参考值，角度值仅记 录n d , 数点后2 位，3 ( + ) 表示3 关节的计算角度大于允许的关节转角范围。 最终程序选择了第4 组解，将角度值代入正确表达式，正解结果与给定值一 致( 仅有微小的机器误差) 。 表2 2 运动学逆解结果 组 0 1 ( o )0 2 ( o ) 0 3 ( o ) 0 4 ( o ) l9 0 0 03 1 3 81 8 7 5 10 o o 29 0 0 03 1 3 81 8 7 5 11 8 0 0 0 39 0 o o1 4 8 6 25 1 1 11 8 0 0 0 49 0 0 01 4 8 6 25 1 1 1o 0 0 5，9 0 o o9 6 4 21 5 1 4 lo 0 0 69 0 o o9 6 4 21 5 1 4 11 8 0 0 0 7 9 0 0 01 7 8 5 61 5 0 1奉1 8 0 0 0 89 0 o o1 7 8 5 61 5 o lo 0 0 。 组 0 5 ( 6 )0 6 ( 。)e x ( x1 0 4 ) 备注 13 8 8 99 0 o o4 6 2 23 8 8 9 9 0 0 05 2 6 38 2 4 9 9 0 0 04 2 2 4 8 2 4 99 0 o o3 5 7 56 7 8 3- 9 0 o o4 2 8 66 7 。8 3 9 0 0 04 。9 3 7牛1 6 4 6* 9 0 0 01 0 3p = 2 81 6 4 69 0 0 01 0 3p = 2 1 8 哈尔滨一f 丌大学硕+ 学伊论文 再用几何方法分析得到8 种逆解，与计算结果一致。第4 关节翻转后可 再得到4 组逆解。 在运动学逆解算法中，逆解计算的顺序是根据当前姿态值确定以，0 ，a 一 由位置值确定p _ 求解转动角度_ 判断转动角度是否超出极限范围_ 确定 转动角度。在逆解程序中，实现流程如图2 5 所示。 图l 5 运动学逆僻流程图 1 9 2 4 运动学正解与逆解问题的验证 为了验证软件正解程序与逆解程序的正确性，通过输入各关节角度值， 求解末端关节的位置与姿态值”：通过输入束端关节的位置与姿态值，逆解 分析各关节角度值。从而基于角度值与位姿值的判断验证正逆解的正确性”。 1 ) 非奇异位置点 选取最o 的任意角度点，输入各关节角度值鼠= 1 0 。，只= 一8 0 。，只= 5 。， 最：2 0 。，0 6 = l o 。通过正解程序运算求得位置与姿态值如图2 6 所示。 口= 如图 图27 非奇异点逆解位姿