（机械电子工程专业论文）工业机器人运动仿真及作业仿真系统.pdfVIP

摘要一个完善的机器人仿真系统可以对机器人运动学、动力学、轨迹规划和控制算法进行仿真计算，可以仿真机器人的各种操作过程，并对仿真的结果进行分析处理，并且可以不依赖现实中的机器人设备，是研究机器人系统及其相关技术的有效手段和得力工具。机器人运动学图形仿真是机器人仿真系统的一个重要组成部分。它将机器人的运动学仿真结果以3 d 图形的方式显示出来，直观的显示机器人的运动情况。研究人员能在计算机屏幕上看到机器人的运动方式，轨迹规划的结果，还可以交互式逾对机器人进行示教仿真。机器人运动学图形仿真技术为机器人仿真系统建立了良好地人机界面。本文对开发机器人运动学仿真系统的主要算法和图形建模方法进行了讨论。在机器人运动学，机器人轨迹规划，空间碰撞检测，计算机图形学等领域里面前人都已经做了很多的工作，基本上已经有了比较成熟的理论和方法，这就使得整个系统在可行性上有了一定的保障。文中主要开展了如下工作：首先，论文对机器人仿真技术的发展概况进行了概述，总结并提出了机器人图形建模的三种方法。其次，本文对三类算法进行了重点研究和分析：运动学逆解通用算法，轨迹插补算法，碰撞检测快速算法。本文阐述了这些算法的思想、设计理论和具体设计过程，并进行了详细的模拟分析。同时，本文还对机器人语言进行了介绍。第三。本文根据所上述算法开发设计了一个完整的机器人运动学仿真系统程序。最后，本文根据课题研究工作和程序测试结果，指出目前系统存在的不足之处和改进方法。关键词：机器人，运动学仿真，图形建模，碰撞检测a b s t r a c t1 1 l ec o m p r e h e n s i v er o b o ts i m u l a t i o ns y s t e mi n c l u d e sk i n e m a f i c ss i m u l a t i o n d y n a m i c ss i m u l a f i o n , t r a j e c t o r yp l a n n i n g a n dc o n t r o la 1 9 0 r i t h m a n di tc a ns i m u l a t ea l lk i n d so fo p e r a t i o na n da n a l y z et h er e s u l t so ft h es i m u l a t i o n i tm a yb er i o td e p e n do nr o b o t si nt h er e a lw o r l d s or o b o ts i m u l a t i o ns y s t e mi sa ne 塌c i e n tm e t h o da n dp o w e r f u lt o o lo f s t u d y i n gr o b o ts y s t e ma n do t h e rr e l e v a n tt a c h n o l o g y t h er o b o tk i n e m a t i c sg r a p h i cs i m u l a t i o ni so n eo f t h em o s ti m p o r t a n tp a r to f 也er o b o ts i m u l a t i o ns y s t e m i tc a na u d i o ，v i s u a l l ys h o wt h er e s u l to ft h es i m u l a t i o na n dt h em o t i o no fr o b o t sb yu s i n g3 dg r a p h t h r o u g ht h ec o m p u t e r ss c r e e n ，r e s e a r c h e r sc a l lw a t c h 也em o t i o no f r o b o t s t h er e s u l to f t h et r a j e c t o r yp l a n n i n ga n dd oi n t e r a c t i v et e a c h i n g - b y - s h o w i n gw o r k mr o b o tk i n e m a t i c sg r a p h i cs i m u l a t i o ns e tu pg o o dm a n m a c h i n ei n t e r f a c ef o rr o b o ts i m u l a t i o ns y s t e m t h ep a p e rd i s c u s s e st h em a i na l g o f i t h mo fr o b o ts i m u l a t i o ns y s t e ma n dt h em e t h o do fb u i l d i n gg r a p h i cm o d e l w ec a nf i n ds o m eg o o dw o r ki nt h ef i e l d so fk i n e m a t i c s ，t r a j e c t o r yp l a n n i n g ，c o i l i s i o nd e t e c t i o na n dc o m p u t e rg r a p h i c ss ot h a tw ec a nu s et h e i rr e s u l t st oc o n s t r u c to u rs y s t e m n em a i nr e s e a r c hw o r ko ft h i sd i s s e r t a t i o ni s 箍f o l l o w ：f i r s t l y , t h ep a o e re x p a t i a t e st h ed e v e l o p i n gs t a t u s 毋r o b o ts i m u l a t i o ns y s t e m s u m m a r i z e st h r e em e t h o d so fb u i l d i n gg r a p h i cm o d e l s e c o n d l y , t h ea r t i c l eg i v e se m p h a s i st ot h es t u d ya n da n a l y s i so ft h r e ek i n d so fa l g o r i t h m s , i e g e n e r a li n v e r s ek i n e m a t i c sa l g o r i t h m ，t r a j e c t o r yi n t e r p o l a t i o na l g o r i t h ma n df a s tc o l l i s i o nd e t e c t i o na l g o r i t h m a n dt h i sp a p e re x p a t i a t e st h ei d e ao ft h r e es 可l e so fa l g o f i t h m s d e s i g n i n gt h e o r y , d e t a i l e dd e s i g n i n gp r o c e s s ，s i m u l t a n e i t ys i m u l a t i n ga n da n a l y z i n gt h ed e s i g n e da l g o r i t h m s a tt h es a m et i m e ，t h ep a p e ri n t r o d u c e sr o b o tl a n g u a g e t h i r d l y , w ed e s i g nt h er o b o ts i m u l a t i o ns y s t e mb yu s i n gt h ea b o v ea l g o r i t h m s l a s t l y , t h ed i s s e r t a t i o nd i s c u s st h ed i s a d v a n t a g eo f o u rs y s t e ma n dg i v es o m en e wm e t h o dt oi m p r o v ei ta c c o r d i n gt ot h er e s e a r c ha n da c t u a lt e s t i n gw o r k k e yw o r d s ：r o b o t , k i n e m a t i c ss i m u l a t i o n ，g r a p h i cm o d e l i n g , c o l l i s i o nc h e c k王王原创性声明本人声明：所呈交的论文是本人在导师指导下进行的研究工作。除了文中特别加以标注和致谢的地方外，论文中不包含其他人已发表或撰写过的研究成果。参与同一工作的其他同志对本研究所做的任何贡献均已在论文中作了明确的说明并表示了谢意。签名：主缨豆e l 期兰：i ：芒本论文使用授权说明本人完全了解上海大学有关保留、使用学位论文的规定，即：学校有权保留论文及送交论文复印件，允许论文被查阅和借阅：学校可以公布论文的全部或部分内容。( 保密的论文在解密后应遵守此规定)彤九工业机器人运动仿真及作业仿真系统第一章绪论第一章绪论1 1 课题研究的背景工业机器人由操作机( 机械本体) 、控制器、伺服驱动系统和检测传感装置构成【l l ，是一种仿人操作、自动控制、可重复编程、能在三维空间完成各种作业的机电一体化自动化生产设备。特别适合于多品种、变批量的柔性生产。它对稳定、提高产品质量，提高生产效率，改善劳动条件和产品的快速更新换代起着十分重要的作用。机器人技术是综合了计算机、控制论、机构学、信息和传感技术、人工智能、仿生学等多学科而形成的高新技术，是当代研究十分活跃，应用日益广泛的领域。机器人应用情况，是一个国家工业自动化水平的重要标志。机器人并不是在简单意义上代替人工的劳动，而是综合了人的特长和机器特长的一种拟人的电子机械装置，既有人对环境状态的快速反应和分析判断能力，又有机器可长时间持续工作、精确度高、抗恶劣环境的能力，从某种意义上说它也是机器的进化过程产物，它是工业以及非产业界的重要生产和服务性设备，也是先进制造技术领域不可缺少的自动化设备。随着工业机器人被日益广泛地应用到实际生产中，机器人的工作环境也日趋复杂化，这样对机器人及其工作环境乃至生产过程的计算机仿真是必不可少的。机器人仿真的任务，就是在不接触实际机器人及其工作环境的情况下，通过计算机图形技术，提供一个和机器人进行交互作用的虚拟环境。机器人仿真技术圆研究始于七十年代末，八十年代得到迅速发展，目前已经广泛应用于机器人运动学、动力学分析与综合、轨迹和路径规划、机器人与工作环境的相互作用、离线编程等各个领域，成为机器人研究中不可缺少的一个重要组成部分。机器人仿真技术涉及机器人动力学仿真、机器人运动学仿真、机器人控制仿真、机器人作业仿真等诸多内容【3 】。而机器人运动学仿真又不同于动力学仿真，由于动力学仿真是忽略了许多次要因素并通过线性化而得到的近似模型，因此仿真结果与真实情况可能有较大差距；但运动学仿真结果完全是真实可靠的，这是由运动学正、逆问题公式的准确与可靠保证的。机器人仿真技术作为机器人研究应用的安全可靠、实用廉价的工具和手段，日益显示其强大的生命力和不可缺的重要性。1 2 机器人仿真的多种途径目前，在机器人仿真的具体实现上，有各种方法。基于a u t o c a d 平台的图形仿真系统 4 1该方法以a u t o c a d 和a u t o l i s p 为基础来建立机器人仿真系统的。其实现机器人模型运动有两条途径，其一是采用a u t o c a d 提供的脚本文件，把所需的a u t o c a d 命令组合在一起，按预定的顺序执行这些命令，并通过运行脚本文件工业机器人运动仿真及作业仿真系统第一章绪论来产生图形显示。另外一种方法是采用a u t o l i s p 语言编写模型驱动程序来实现模型运动。该方法的优点在于构建机器人模型方便，但是系统缺乏交互性，运动学逆解算功能差等缺点。并且由于以a u t o c a d 为平台的仿真系统，只适于运动学仿真，不利于仿真系统的扩展。基于m a t l a b 的运动仿真系统例m a f l a b ( 矩阵实验室) ，8 0 年代由c l e v e m o l e r 构思并开发，是集合命令翻译、科学计算于一身的交互式软件系统。在机器人仿真系统中，有大量的矩阵运算，采用m a t l a b 作为平台，可以充分利用m a t l a b 强大的矩阵运算功能，提高矩阵运算的正确性和效率。并且现在m a u a b 已经有专门的r o b o t 工具包，能更加方便的实现多种结构机器人的运动学仿真。m a r l a b 本身也具有显示三维图形的功能，即三维线条图。虽然三维线条图使运动仿真简洁明了，但是仿真效果不好，缺乏真实感和精确性，而且线条模型不能满足对仿真系统做观察和检测的需要。基于a u t o c a d 和m a n 曲的机器人仿真系统1 6 j应用a u t o c a d a c t i v e x 技术，在j a 中调用m a t l a b 应用程序。利用m a t l a b强大的矩阵计算功能，结合a u t o c a d 的图形功能，可以开发出运算效率高、图形显示质量好的机器人仿真系统。基于v 1 l m l - j a v a 的运动仿真系统【7 】虚拟制造( v i r t u a lm a n u f a c t u r i n g ) 已是制造业中广为接收的新概念，虚拟现实技术是虚拟制造的一个重要支撑技术，是进行产品虚拟设计的途径。随着j a v a 和v r m l 技术的相继出现以及微机性能的不断提高，在微机上进行虚拟现实仿真已经成为可能。这种仿真系统虽然不具备大型系统那样的真实感和浸入感，但它提供了一个低成本，低门槛的应用平台。v r m l 和j a v a 的跨平台性、网络化和强大的可编程能力，对于实现网络化机器人仿真是一种简单、廉价而有效的手段。基于3 d sm a x 的机器人动画仿真系统嘲3 d sm a x 是a u t o d e s k 公司推出的专门进行图像处理和动画制作的软件，使用方便，功能强大。利用3 d sm a x 作为机器人动画仿真系统，具有模型美观逼真，操作简单，易于改变观察视角等诸多优势。但是3 d sm a x 造型缺乏一些精确定位的功能，仿真的实时性和交互性。基于v i s u a lc + + 和o p e n g l 的运动仿真系统1 9 采用v i s u a lc + + 作为编程语言，一方面是因为v i s u a lc + + 完善的基本类库m f c 和应用向导a p p w i z a r d 可以方便地调用o p e n g l ，以及方便的实现o p e n g l和d x f 、s l p 等图形文件的接口。这有利于实现复杂、丰富的机器人及环境模型。另一方面，采用v i s u a lc + + 作为编程语言有利于正、逆运动学以及轨迹、路径规划等算法及示教功能的实现。o p e n g l 是近几年发展起来的一个性能卓越的三维图形标准，它独立于操作系统，以它为基础开发的应用程序可以十分工业机器人运动仿真及作业仿真系统第一章绪论方便地在各种平台间移植。1 3 研究现状1 3 1 国内研究现状目前，在国内已有许多高校及研究所在工业机器人仿真方面做了很多工作【m ”】，开发了一些软件。如上海交通大学机器人研究所的俞文伟和邓建一研制开发的机器人图形仿真软件r o s i d y 。该软件是以国内外普遍使用的a u t o c a d作为其图形支持，通过对a u t o c a d 的二次开发使其能直接为机器人仿真服务，该软件系统采用人机界面，具有菜单式项目选择、交互式人机对话、文件修改和与c 语言、f o r t r a n 语言连接等功能。而清华大学的崔培莲和孙增圻开发研制的微机机器人仿真系统p c r o b s m ，采用模块化结构，具有一定的通用性和可以移植性，也便于用户进行拓展。整个系统有机器人语言、轨迹规划、运动学和动力学、控制系统仿真、图形数据输出等模块组成。此外，华中科技大学的丁汉等，国防科技大学的潘存云等在这方面也进行了研究开发。这些软件在开发中都做了大量的工作，无论算法结构还是图形研究都具有一定特色。但其中大多数的软件只是针对某种机器人开发、单机仿真的，而且其中的一些软件只能在工作站上运行。1 3 2 国外研究现状国外机器人系统仿真软件很多【m ”】，如英国n o t t i n g h a m 大学的g r a s p ,可用于机器人制造和装配单元仿真，具有固体几何模型，运动学模型；德国w a m e e k e 等人研制的i p a ，能对不同机器入的功能和用途进行比较，以帮助使用者选择合适的机器人；美国通用电气公司研制的s d r c 软件系统、m i e h i g a n大学的a d a m s 软件包，均可以用于机器人工作单元设计和离线编程；m a r y l a n d大学研制的d y n a m a n 系统可实现机器人的动力学分析和仿真。其它的一些仿真系统如，t o a d 、e d y l m a 主要用于机器人动力学仿真，d r a m s 、i m p 、l 烈k a g e s 、和d o m e s 用来解决机器人机构的运动学分析，而m i c r o s 、t o c a d 、c a m 和r o b c a d 等系统针对机器人的结构设计与仿真。圈i - 1k u k a 公司机器人仿真软件工业机器人运动仿真及作业仿真系统第一章绪论国外的机器人生产公司也为自己的产品专门开发了仿真系统，如德国的k u k a ( 图1 1 ) ，日本的k a 、融s a 日等。目前，机器人仿真系统正朝着通用化、完整化、交互式计算机图形化、智能化和商业化的方向发展，而代表这一发展趋势的是r o b c a d 机器人仿真系统。r o b c a d 已在实际工业系统中得到了广泛的应用，美国福特、德国大众、意大利菲亚特等多家汽车公司、美国洛克希德宇航局都使用r o b c a d 进行机器人生产线的设计、仿真和离线编程。目前，r o b c a d 已成为世界上流行最广的机器人仿真系统之一。1 3 3r o b c a d 系统简介l l 纠r o b c a d 是美国t e c n o m a t i x 公司八十年代推出的、运行在s g i 图形工作站。u n i x 操作系统下的大型机器人设计、仿真和离线编程系统。其主要功能包括：完整的二维、三维图形设计，图形的逻辑运算，提供线框图、消隐图、实体造型及光照模型。提供多种标准几何建模协议，实现了其它c a d 软件如i g e s 、v d a f s 、s e t 、g e o m o d 和r o b c a d 的兼容，在这些环境下建立的几何模型可互相调用。机器人及设备运动学自动建模，具有六自由度以下机器人通用运动学逆解调器，提供机器人库及部件库，机器人库包括了目前国际上商业化机器人一百多种。机器人工作单元设计、布置方案可行性检查及优化、工作点及轨迹的交互式生成，机器人及设备的运动和示教，机器人及设备作业任务自动生成。工作单元作业任务描述及机器人、设备之间的相互通讯，提供了通用编译型任务描述语言1 i ) l 。工作单元作业任务动态图形仿真，随机中断和继续，实时观察，碰撞检测及报警，并提供仿真结构视频输出。整个工作单元或单元内任一元素均可根据a n s f i s o d i n 标准绘出机械图纸，提供完整的尺寸标注及数据，具有绘图机接口。可实现离线编程和机器人任务的上调、下装，提供了十七种机器人控制器语言的翻译器或编译器。r o b c a d 由六大模块组成：机械设计、建模模块( m e c h a n i c a lm o d e l i n g )工作单元布置模块( w o r k c e l ll a y o u t )任务描述模块( t a s kd e s c r i p t i o n )仿真模块( s i m u l a t i o n )机械制图模块( d r a f t i n 9 1数据管理模块( d a t am a n a g e m e n t )r o b c a d 是交互式菜单驱动环境，它的全局监控菜单管理系统布局合理，画面美观，好学易懂，使用方便，为用户提供了良好的人机交互界面。工业机器人运动仿真及作业仿真系统第一章绪论但其价格昂贵，操作复杂，只能在s g i 图形工作站，u n i x 系统下运行。这些原因使得该软件不利于在国内的中小企业推广应用。1 4 论文主要工作及系统结构评定一个工业机器人仿真系统的好坏，看它是否满足如下的条件：( 1 ) 具有良好的人机交互界面；( 2 ) 仿真时各种参数以及仿真类型选择都是由用户通过人机对话方式进行，通过改变参数，就可以对不同大小、功能的机器人进行仿真：( 3 ) 系统的程序要实现模块化，便于软件移植、修改和扩展。本文的主要工作就是要研究开发一种基本满足以上三个条件，且成本低廉、具有实用价值和自主知识产权的商业化的机器人仿真系统。我们将此系统命名为i r s s ，i n d u s t r i a lr o b o ts i m u l a t i o ns y s t e m 。i r s s 的编程语言为c + + ，编译器为m i c r o s o f tv i s u a lc + + 6 0 ( 包含o p e n g la p i 函数库) ，运行平台定位为w m d o w s9 8 2 0 0 0 x p 。i r s s 的总体结构见图1 2 。系统采用模块化结构，具有一定的通用性和可移植性。整个系统由机器人及环境图形建模，机器人语言，运动学正解逆解，轨迹规划，碰撞检测，示教，系统帮助等模块组成。几个基本模块的功能分述如下：图1 - 2 i r s s 的总体结构机器人及环境图形建模模块：采用o p e n g l 和v c + + 相结合，生成机器人及环境模型。该模块提供了o p e n g l 与a u t o c a d 的d x f 图形文件和p r o e n g i n e e r 的s l p 图形文件的接口，便于系统充分利用现有的机器人模型资源以及建立更为复杂、丰富的机器人环境模型。机器人语言模块：该模块是用户与仿真系统的接口，包括指令编辑和指令解释两个子模块。它充分体现了仿真系统的功能和特色，是一种面向动作的可编程的机器人语言。轨迹规划模块：该模块根据运动指令产生机器人的期望轨迹。在本系统中轨迹规划的实现主要取决于机器人的控制方案和机器人指令。目前系统提供三种主要的轨迹插补算法：j m o v e 根据起点和终点，在关节变量空间采用五次多项式插值，产生关节变量空间期望的轨迹：l m o v e ：根据起点和终点，工业机器人运动仿真及作业仿真系统第一章绪论在笛卡儿空间进行线性插值，转换到关节变量空间；c 1 m o v e c 2 m o v e ：根据三个路径点，在笛卡儿空间进行圆弧插值，然后转换到关节变量空间。运动学模块：正运动学计算采用的是典型的d h 齐次变换的方法；逆运动学计算采用解析法和迭代法相结合的算法，该算法具有较好的通用性。碰撞检测模块：在机器人环境中有可能发生碰撞的情况有几种：机器人与机器人之间，机器人与环境中其他物体之间，机器人夹持器与被夹持物体之间。图形仿真中通过两物体占有公共空间或彼此相互穿越来判定两物体之间是否发生碰撞，其核心是空间几何体的求交算法。本系统采用改进的公共面法来进行空间几何体的求交计算。示教模块：机器人仿真示教模块是模拟实际机器人示教盒设计的，它实现了机器人示教盒的功能。不仅可以做单机器人的示教，还可以做双机器人的示教，从而支持多机器人的仿真系统。多机器人示教仿真存在选择机器人的问题，由于仿真程序无法同时对多个机器人进行示教，所以只能采取轮流示教的方法。系统帮助模块：该模块提供系统操作及机器人指令说明的帮助文件。目前阶段本系统以开链6 r 结构机器人为主要研究对象，如k a w a s a k i 公司的k a w a s a k if s 0 6 l 型机器人和k u k a 的k r 3 型机器人。1 5 本文结构本文的组织结构是按模块的划分来确定的。第二章介绍了机器人运动学正解及逆解理论，以及本系统所采用的算法。第三章阐述了机器人及环境的3 d 图形建模方法。第四章阐述了碰撞检测的基本理论及现有方法。在总结现有方法的基础上提出了一种新的快速碰撞检测算法，实验表明该算法完全能满足实时仿真的要求。第五章首先论述了关节变量空间轨迹规划和笛卡儿空间轨迹规划的基本方法，并研究了三种主要轨迹插补算法。接着对系统人机接口部分，即机器人指令系统进行阐述。第六章阐述了系统实现与应用。第七章主要是对全文工作做了总结，并提出了现阶段工作中存在的问题以及下一步工作的设想。工业机器人运动仿真及作业仿真系统第二章运动学模块第二章运动学模块机器人运动学是专门研究物体运动规律，而在研究中不考虑产生运动的力和力矩，它涉及到运动物体的位置、速度、加速度和位置变量对时间的高阶导数。机器人运动学可分为两类问题，正问题和逆问题。运动学正问题是关于给定机器人各关节角度，要求计算机械手的位置与姿态的问题，其解是唯一的。运动学逆问题是已知机械手的位置与姿态，求机器人对应于这个位置与姿态的全部关节角的问题。运动学逆问题具有多解性。2 1 运动学正问题2 1 1 转动连杆坐标系的建立及连杆的d e n a v i t - h a r t e n b e r g ( d h ) 参数弘”“j连杆i 坐标系设置在关节i + 1 上：五轴沿关节i + 1 运动轴线方向；x i 轴既垂直于z ，轴，又垂直于z 。轴，其正方向由关节i 指向关节i + 1 ：r 轴由右手定则确定，使 置v , z ， 构成右手直角坐标系。关于原点0 。，当z 。，z ，相交时，由交点确定；当z 。，z 在空间交叉时，由它们的公垂线与z f 的交点确定；当z 。，z j 平行时，以互，z 。的公垂线与互的交点作为q ，且使下一连杆的d 。= 0 。机器人基座坐标原点以连杆d 表示，基座坐标系即连杆0 坐标系 d o x o k z o ) ，它是固定不动的，用作其它连杆坐标系的参考系，并使z 0 轴沿关节l 的轴方向。o o 的设置有任意性，为了方便起见，通常使0 0 与0 1 重合：若0 0与0 l 不重合，可用一个固定的齐次变换阵将坐标系 1 ) 和坐标系 0 联系起来。在n 自由度机器入的终端，固以连杆疗的坐标系 d o x 。k z 0 ) ，常记以o n o a 。由于机器人的主要功能之一是适当地操纵工具及夹持物体，故坐标系 疗 的原点d 。，通常由夹手夹持的工具的终点位置确定，或当夹持物体时，由夹手顶端的正中位置( 或物体中心) 确定，统称工具坐标系。d 。一旦确定，则可构成直角坐标系 仉五艺z 。 或o n o a 。其中单位向量西规定了接近物体的方向，称为接近向量；单位矢量子规定了夹手的开台方向，称为姿态向量；单位矢量亓按右手定则确定，亓= 6 云，称为法向量。由于连杆月的终端不再有关节，故终端坐标系 n 的位移矗和转角色都是相对z 。轴出现的。通常d n 0 时，坐标系 n ) 与 一1 ) 是两个平行的坐标系；不计d 。，即a n = 0 时，两个坐标系是重合的。连杆的d - h 参数：盯。：连杆i 的长度，是z j 互公垂线段的长度；口。：连杆f 的扭角，过q 点作z ；一。z 。，z - 绕x ，轴转至互轴的转角；d i ：连杆i 相对连杆i l 的偏移，为沿z 。轴的o 至o ( o 为公垂线与z 。轴的交点) 的距离；b ：连杆，相对连秆f 一1 的转角，称为关节角，在与z 。轴垂直的平面内度量。角度口，只的符号根据右手定则确定，d ，的符号当丽沿z 。轴正向时为正，口总是正的。工业机器人运动仿真及作业仿真系统第二章运动学模块2 1 2 广义变换矩阵的建立当全部连杆的坐标系确定之后，我们就可以按照以下顺序由两个旋转和平移来建立相邻连杆i 一1 与i 之间的相对关系。( 1 ) 绕z 。轴旋转只角，使x i 一。轴转到与x 。同平面内：( 2 ) 沿2 。轴平移一距离4 ，把x 。移到与x 同一直线上；( 3 ) 沿i 轴平移一距离a 。，把连杆f l 的坐标系移到使其原点与连杆盯的坐标系原点重合的地方；( 4 ) 绕k 。轴旋转口，。角，使：。转到与z ，同一直线上。用齐次变换矩阵4 ，即d h 矩阵来描述某局部坐标系相对前一坐标系的位姿：a i = r o t ( z ，8 , ) t r a t l , ( o ，0 ，d i ) t r a n s ( a ，0 ，o ) r o t ( x ，( 7 9 ，)( 2 1 )展开上式可得：a =cc s 【c a 5 ic i 。c c t j0s a00 s i s c t ,c l i e lc 。s a 。a i s ic a ,4o1( 2 2 )g e e ，s ，表示s j 且( 只) ，5 口，示s i n ( a ，) ，c i 表示c o s ) ，c a ，, 示c o s ( a 。) ，下同。则机器人手部位姿方程可表示为：瓦= a l a 2 a 3 a 4 a 5 a 6r jo xm ，o y行=o =0o口，p 。a ，p ，n ：p ：01( 2 - 3 )其中，子为机器人手部接近矢量，石为手部姿态矢量，元为手部法向矢量，面表示手部位景在基准参考坐标系中的坐标。2 1 3k a w a s a l df s 0 6 l 的运动学方程吲k a w a s a k if s 0 6 l 属于关节式机器人，6 个关节都是转动关节。前三个关节确定手腕参考点的位置，后3 个关节确定手腕的方位。和大多数工业机器人一样，后3 个关节轴线交于一点。该点选作手腕的参考点，也选作为连杆坐标系 5 ，和 6 ) 的原点。各连杆坐标系如图所示，相应参数列于表l 一1 。其中a l ：1 0 0 m m ，a ，= 7 5 0 r a m ，口1 = 4 0 m m ，d j = 4 3 0 m m ，d 4 = 7 0 0 m m ，d 6 = 1 0 0 m m 。据式( 2 2 ) 和表1 1 所示连杆参数，可求得各连杆变换矩阵如下：cj0 一a l c5 l0。ia l s0 一l0d 0oo1a 2 =0a 2 。20a 2 s 210o1工业机器人运动仿真及作业仿真系统第二章运动学模块表2 - ik a w a s a k if s 0 6 l 机器入的连杆参数连杆i变量0iia 也1ol ( 9 0 0 )9 0 0a 1d l2o2 ( 一9 0 0 )o oa 2o303 ( o o )一9 0 0a 30404 ( o d )9 0 。od 25e5 ( 0 0 )一9 0 0oo606 ( 吩0 00d 6a 3 =a 5 =c 30s 300 1o0c 50s 500 1oo一毛d 3 c 3q吒屯oo015 5050000la 4 =a 6 =c 40s 40s 40 一c 40010d 40001c 6 - - s 600s 60 600001d 6o001据式( 2 - 3 ) 和上述各变换矩阵可求得机器人的r 变换矩阵：t = a l a 2 a 3 4 彳5 以=n 。0 ，a ，p ，n yoyn yp y ：0 z日：p ：o001玎= ( c 1 c 2 3 c 4 c 5 + 屯s 4 c 5 一c l j 2 3 j 5 ) c 6 + ( 一c l s 4 c 2 3 + 5 l c 4 ) j 6盯v = ( s l s 5 c 2 3 c 4 一c i s 4 c 5 一占l s 2 3 占5 ) c 6 + ( 一毛j 4 c 2 3 一c l c 4 ) j 6 ：= ( 一c 4 s 2 3 c 5 一c 2 3 $ 5 ) c 6 + $ 2 3 s 4 j 6o x = ( 一s i s 5 c 2 3 c 5 一s 1 5 4 c 5 + c l s 2 3 s 5 ) s 6 + ( 一c 】s 4 c 2 3 + c d ) c 6o v = ( - s l s 5 c 2 3 c 4 + c 1 s 4 c 5 + s i s 2 3 s 5 ) s 6 + ( - s 】s 4 c 2 3 一c 1 c 4 ) c 60 ：= ( c 4 9 2 3 c 5 十c 2 3 s 5 ) 矗+ j 2 3 s 4 吒q2 一c t c 2 3 c 4 c 5 一s i s 4 如一cl s 2 3 c 5a ，2 一s i s 5 c 2 3 c 1 + c i s 4 s 5 一s i s 2 3 c 5d ：2c 4 s 2 3 s 5 一c 2 3 c 5p x = ( 一q c 2 3 c 4 c 5 一& j 4 s 5 一c t s 2 3 c 5 ) 以+ ( 一c l s 2 3 d 4 + a 3 c 1 c 2 3 + a 2 c l c 2 十a l c l )pv = ( 一s l s 5 c 2 3 c 4 + c 】s 4 s 5 一s i s 2 3 岛) d 6 + ( 一s i s 2 3 d 4 + a 3 s i c 2 3 + 0 2 s l c 2 + a l sj )p ：= ( ( c 4 s 2 3 j 5 一c 2 3 c 5 ) d 6 + ( 一c 2 3 d 4 一a 3 s 3 + d 】一( 1 2 s 2 )q( 2 4 )_ 丁业机器人运动仿真及作业仿真系统第二章运动学模块z图2 - 1f s 0 6 l 机器人外形图图2 - 2 f s 0 6 l 机器人的连杆坐标系2 2 运动学逆问题2 2 1 可解性的重要结论【2 4 j0影= 露- 犁+ 君( 2 7 )露掣是把 口) 坐标系中一向量掣转换成了原点与 口) 坐标系原点重合，但与基若由机器人位置向量导出臂的位置向量，便可确定臂的控制角鼋。、g ：、吼。 参1 = 荔1 + 三三c 2 ，一：，注意到多，方向与终端搡作装置接近方向西重合这一事实，而厅的方位由掣的第三列给出，那么，可求出向量舀相对基坐标系的方位角妒及极角口为p ：d 乜n ( 旦)口1胁t a l l ( 3 匾)口。( 2 1 0 )( 2 - 1 1 )工业机器人运动仿真及作业仿真系统第二章运动学模块因此芦，的分量为：p 。= 眵。j s i n 0 c o s 口)p 。= l p 。l s i n o s i n 妒p 。= i n c o s 0故臂的位置向量为：帆j s i n 0 c o s o一p 。| l s i n 0 s i n 口9j p 。| c o s( 2 1 2 )( 2 1 3 )到此便转化为求解臂的运动学方程问题。对于确定的机器人结构形式来说，p ，、p y 、p ：、j p 小口和妒均是已知的，于是己知臂位置l p o ，儿j ，便可以求出臂的三个关节变量。2 2 2 2 腕关节变量吼、q ，、q 。的解从腕运动学方程求解可得到启示，若由机器人方位矩阵豆? 导出腕的方位矩阵时，便可确定腕的关节变量9 4 、q ，、q 。由于故有掣= ( 彤) = ( r o ) 7 甜( 2 ，1 4 )式中掣为已知；( 胃) 是臂关节变量的函数。在求得吼、g ：、口，以后也是已知的。故j i ，可以计算出来，到此便转化成求解腕的运动学方程问题了。2 2 2 3 用臂、腕分离法求解机器人运动学逆解的步骤确定终端操作装置接近向量的极角护和方位角舻；利用p ，妒求臂向量p 。的分量；利用p 。的分量计算臂控制变量g 】、q ：、q ，；利用q i 、q 2 、q ，来求解腕控制变量q 。、q 5 、q 。2 2 3 迭代法求逆运动学解 2 6 - 2 9 本文以文献【2 6 】中提出盼方法为基础，在实际应用中对其进行了适当的修正和完善。将6 自由度机器人分解为位置结构和姿态结构两部分。前3 个自由度组成位置结构，后3 个自由度组成姿态结构。本课题所讨论的位置结构为r r r ( 关节型) 型。2 2 3 1 机器人结构分析一般情况下，机器人的前3 个关节主要影响末端工作点的位置，后3 个关节决定工具坐标系的姿态，各关节坐标系之间采用标准的d h 矩阵：2lr_j_j-e-_jr，fpp 尸_，。，l=，j甜掣血ppp-。，。，l工业机器人运动仿真及作业仿真系统第二章运动学模块c 一s l 。c a ls tc 。c a 0s 岱005 。so l 。一q s a ,c a ,o尺i 是4 中表示姿态转换的3 x 3 子矩阵。设末端工作点位姿以。瓦表示，则。瓦= 彳1 4 2 4 3 4 4 6 =a t ? c la ? s lz1n xo jn yo yn zd z002 kn t 。cq jd1n xp la yp ya = p ：o1将机器人分解为位置结构和姿态结构，从( 2 1 6 ) 可以得到位置结构：姿态结构掣 降：l ；儿jl 轨jr 4 r 5 民= ( r i r 2 e 3 ) - 1 0 r 6为书写简便，记；p x = p ：，：p ，= p j ，：p ：= p ：p ，= n ，：p y = p y ，：p ：= 见( 2 1 5 )( 2 1 6 )( 2 1 7 )( 2 - 1 8 1对于普通机器人结构，根据( 2 - 1 7 ) ，( 2 ，1 8 ) 3 莲替迭代可得到整个机器人的逆解。2 2 3 2 位置结构综合由( 2 1 7 ) 式可得铋似- ；i ： yp 协jl p ；j其中关节变量为最，岛，0 3 ，将( 2 一1 5 ) 式代入( 2 1 9 ) 并展开得到c 2 c 3 5 2 如c 口2$ 2 c 3 + c 2 s 3 c g 2屯占d 2oc 2 j 3 c 口3 一s 2 c 3 c a 2 c a 3 + 5 2 s a 2 s a 3一s 2 s 3 c l z 3 + c 2 c 3 c 口2 c 口3 一c 2 s a 2 s 口3c 3 c o 3 8 d 2 + c o 2 s a 3oc 2 s 3 s a 3 + $ 2 c 3 c ( ! z 2 s a 3 + s 2 c 盘3 s c c s 2 s 3 s a 3 一c 2 c 3 c a 2 s a 3 一c 2 口2 c o 一c 3 s 口2 s 0 3 + c 口2 c o y 30工业机器人运动仿真及作业仿真系统苎三兰垄型兰堡塑记c l p + s t p j ，一a i( 一s t p ，+ c 1 p ，) c a l + ( p ：一d 1 ) s a 】 p ，一q p 。) s a t + ( p ：一d i ) c 口111 9 。p j + a 3f l o = p v c 口3 一p ：s a 3k = ( p y c c t 3 + p c a 3 + g ) c a 2 + d 21 7 = 一( p 口3 + p ：c c t 3 + d 3 ) s c t 2，l2 1 9 c 3 一l l o s 3 + d 2，2 = g s 3 + f l o c 3 ) c a 2 + ，71 3 = p x c l + p v s l a 1f 4 = ( 一p 。置+ p ，c 1 ) c a l + ( p ：一d d s c t iz 5 = ( p ，s 1 一p ，c 1 ) s a l + ( p ：一d 1 ) c a l1 8 = ( 1 9 s 3 + k c 3 ) s a 2 + 1 6其中l l ，1 2 ，矗与变量岛有关，f ，。和z ，r - 与o , ：g y e 。c 2 1 2 s 2 = 1 3f 1 5 2 + 1 2 c 2 = z 4( f 9 屯+ ，l o c 3 ) s o ：2 + 1 6 = 1 5由( 2 2 1 ) 和( 2 2 2 ) 可得f ( ，? + ，；) c ：= ，屯+ z ：f -l u l 2 + 勃s ：= ，1 4 1 2 3( 2 - 2 0 )由( 2 2 0 ) 可知：( 2 2 1 )f 2 - 2 2 )f 2 2 3 )f 2 2 4 )即已知。可以完全确定幺又记，l l = 2 【l o ( ，7 c 口2 + 1 6 s a 2 ) 十盘2 jz t 2 = 2 【f 9 ( l c a 吐十，口2 ) 一吒? i oj。= d ；+ 蘑十碍+ 譬+ 磕一b ? + ( p ：一吐) 2 + p ：+ p ；】，】4 = 7 l 南+ ，1 2 s 3 + k1 15 = 1 8 一( p 。一d t ) c a i式( 2 2 1 ) ，( 2 - 2 2 ) ，( 2 - 2 3 ) 两边对应求和得；+ ，孟+ g + 碍+ 露+ 2 ( 1 9 ( 1 t c a 2 + ，6 s a 2 ) - a 2 1 1 0 ) 岛+ 2 ( 1 【o ( ，7 c a 2 + & s o t 2 ) + d 2 ，9 ) c 】1 4r。iiiiiijiiii。ii址cs口d+峨嘎如如十一乒22d1a口叩叩即是吒芦吼q ，一+q 勺吒如qqt 业机器人运动仿真及作业仿真系统一一笙三量堡垫堂楚墨：p ；+ p ；+ ( p 。一d ，) 2 + 日；- 2 a 1 ( c i p ，+ s i p 。)即有一2 a l ( c 1 p ，+ _ p ，) = 亿又由f 2 2 3 ) 式可得( b s l p y c i ) s o 1 = ，l5其中。，与以无关。下面对( 2 2 5 ) 和( 2 2 6 ) 分四种情况考虑( 2 2 5 )( 2 - 2 6 )虞+ 砖= o 时此时，式( 2 2 3 ) 只含有岛变量，求得岛后再利用( 2 2 2 ) 求得只，然后将矾，岛代入( 2 - 2 1 ) 式校验算式是否成立，若成立，则岛，岛及任意得鼠为逆解，否则无解，这种情况即使存在逆