5自由度移动机器人的建模与仿真-硕士论文

曲阜师范大学 硕士学位论文 5自由度移动机器人的建模与仿真 姓名：胡松 申请学位级别：硕士 专业：系统分析与集成 指导教师：解学军 20090301 捅斐 机器人在2 1 世纪生产、生活中的作用日益重大。移动机械手是由移动平台和固定在 其上的多自由度机械手构成的。其工作空间和活动范围可以说是不受约束的。但是，由于具 有强耦合性、高冗余度和非完整性的存在，使得移动机器人在运动过程中的控制变的非常困 难。本文在参考前人研究成果的基础上，对5 自由度移动机器人的运动学和动力学方面做了 比较系统的、全面的分析。这在工业实用领域中有很多实际意义。 本文所做工作主要有以下几点： 第一，首先介绍机械手臂的相关数学概念，然后根据给定的D H 参数表建立5 自由度机械 手的模型，并研究了各关节速度。在此基础上，建立手臂运动学模型，同时也建立了平台的数 学模型。 第二，分析了移动机械手的连续路径规划问题。机器人路径规划具有复杂性、多约束等 复杂系统的特点，在现有的研究基础上，利用微分几何学，将运动的插值法扩展到笛卡尔空间， 产生其与关节空间的光滑运动。在给定术端执行器两端的位姿后，此法可实现手臂的平滑运 动。在接下来的计算机仿真中证明了运动的稳定性。 第三，利用牛顿一欧拉法推导了机械手的动力学模型以及平台的动力学模型，并利用力矩 平衡方程建立统一的动力学模型。最后根据所得结论，为移动机器人设计了动力学跟踪控制 器，使其能按期望轨迹运动。随后利用m a t l a b 对控制器的效果做了仿真，证明控制器的可行 性。 关键词：移动机械手，运动学模型，动力学模型，路径规划，m a t l a b 仿真 A b s tr a c t R o b o ti si n c r e a s i n g l yi m p o r t a n ti np r o d u c t i o na n dl i f ei nt h e2 1s tc e n t u r y M o b i l er o b o ti sa e n 9 1 n eb o d yt h a tc o n s i s to fm o b i l ep l a t f o r ma n dam u l t i d e g r e e o f - f r e e d o mm a n i p u l a t o rl o c a t e d o nt h ep l a t f o r m T h er a n g eo fw o r k s p a c ea n da c t i v i t y i su n l i m i t e d B u tb e c a u s eo fs t r o n g c o u p l i n g ，h i g hm d u n d a n c ya n dn o n h o l o n o m y , t h ec o n t r o lo fm o b i l em a n i p u l a t o rd u r i n gi t s m o V m gp r o c e s sb e c o m e sv e r yd i f f i c u l t I nt h i sp a p e r , b a s e do ns o m e e x i s f i n gr e s u I t s a s y s t e m a t l ca n dc o m p r e h e n s i v ea n a l y s i si sm a d ef o r5 - D O Fm o b i l em a n i p u l a t o ri nt e 订n so f k i n e m a t i c sa n dd y n a m i c s T h ec o n t e n t so ft h i sm a s t e rt h e s i sc a i lb ei l l u s t r a t e da sf o i l o w s ： F i r s to n e ，r e l a t e dm a t h e m a t i c a lc o n c e p t so f m e c h a n i c a la r ma r e 血t r o d u c e d T h e na c c o r d i n g t ot h eg i v e nD H p a r a m e t e rt a b l e ，t h em o d e lo f5 一D O Fm a n i p u l a t o ri se s t a b l i s h e d ，a n dt h e s p e e d o fe a c hj o i n ti ss t u d i e d O nt h i sb a s i s ，k i n e m a t i c sm o d e l so ft h ea m la n dp l a t f o 咖h a v eb e e n e s t a b l i s h e d N e X t ，c o n t i n u o u sp a t hp l a n n i n gp r o b l e mo ft h em o b i l em a n i p u l a t o ri ss t u d i e da 1 1 dd e s i 蛆e d R o b o tp a t h p l a n n i n gi sc o m p l e x ，m u l t i c o n s t r a i n t , e t c O nt h eb a s i so f e x i s t i n gr e s e a r c h ， i n t e r p o l a t i o nm e t h o do fm o v e m e n ti se x p a n d e dt oD e s c a r t e ss p a c eb yu s i n gd i f f e r e n t i a lg e o m e t r y , s ow ec a ng e ts m o o t hm o v e m e n tb e t w e e n jo i n ts p a c ea n dD - s p a c e A f t e rp o s i t i o n i n gt w oe n d so f t h ee n d 。e f f e c t o r , w ec a na c h i e v et h es t a b i l i t yo ft h ea r n l i nm o v e m e n t C o m p u t e rs i m u l a t i o n v e r i f i e si t ss t a b i l i t y A tl a S t ，w i t ht h eh e l po fN e w t o n E u l e rm e t h o d ，d y n a m i c m o d e l so fp l a t f o n l l 觚d m a n i p u l a t o ra l ed e d u c e dr e s p e c t i v e l y , a n dt h e nb yu s i n gt h ee q u i l i b r i u me q u a t i o no ft o r q u e t h e t w om o d e l sh a v eb e e nu n i f i e d F i n a l l y , w i t ht h ec o n c l u s i o n sa b o v e ，ad y n a m i c t r a c k i n gc o n t r o l l e r l Sd e s i g n e df o rt h em o b i l e r o b o t F e a s i b i l i t yo fc o n t r o l l e ri sv e r i f i e db yM A T L A Bs i m u l a t i o n K e yW o r d s ：M o b i l em a n i p u l a t o LK i n e m a t i c s m o d e l ，D y n a m i c sm o d e l ，P a t h p l a n n i n g ，M a t l a bs i m u l a t i o n I l l 曲阜师范大学博士硕士学位论文原创性说明 ( 在口划“”) ? 本人郑重声明：此处所提交的博士口硕士d 论文5 自由度移动机器 人的建模与仿真，是本人在导师指导下，在曲阜师范大学攻读博士口硕 士口学位期间独立进行研究工作所取得的成果。论文中除注明部分外不包含 他人已经发表或撰写的研究成果。对本文的研究工作做出重要贡献的个人和 集体，均已在文中已明确的方式注明。本声明的法律结果将完全由本人承担。 储签名：如叙 日期：叫爻p 曲阜师范大学博士硕士学位论文使用授权书 ( 在口划“) 5 自由度移动机器人的建模与仿真系本人在曲阜师范大学攻读博士口硕 士口学位期间，在导师指导下完成的博士口硕士囱岸位论文。本论文的研 究成果归曲阜师范大学所有，本论文的研究内容不得以其他单位的名义发表。 本人完全了解曲阜师范大学关于保存、使用学位论文的规定，同意学校保留 并向有关部门送交论文的复印件和电子版本，允许论文被查阅和借阅。本人 授权曲阜师范大学，可以采用影印或其他复制手段保存论文，可以公开发表 论文的全部或 作者签名： 导师签名： 日期： 日期： 弓，乡矿 弓、；o 个7 岫 脚 妒 妒 1 1 机器人的概述 第一章绪论 机器人的出现是必然的，因为人们总是想方设法让机器来代替人的繁重工作。机器由低 级到高级的发展，最终形成了机器人。自4 0 年代机器人诞生至今，已过了半个多世纪。机器 人最早用于工业生产，如汽车制造业，由于它在使用中所表现出的大幅度提高工效、提高产 品质量和成品率、显著降低生产成本等方面的优势，使得它在其它制造业中也得到广泛的应 用。机器人的研究和应用对提高劳动生产率具有重要意义，也是衡量一个国家制造业水平的 一个重要标志。 人类进入新世纪后，由科技的高速发展带来了产品的快速更新，高性能、高精度的产品 和工作要求生产者或劳动者必须改进其生产设备。在规模、数量、精度、危险性等方面， 传统的人力以及简单机械环境已经远远不能满足生产和生活的要求。而机器人因它对多种 作业和环境显示出巨大的作用引起了各专业领域的重视，成为高新科技的重要分枝之一。 机器人在经历了长期的改进和更新后，其面貌、结构、用途已经有了根本上的变化，正 从第一代的工作机器人逐渐进化到第三代的人工智能机器人。智能机器人有模式识别和作 业规划能力，有丰富的感知能力，自己拥有知识库和专家系统，可在复杂环境中完成各种作 业。如焊接机器人、喷漆机器人、装配机器人等。作为机器人家族后起之秀的移动机器人 ( 简称M R ) ，由于其用途广泛，工作空间灵活，正以惊人的速度在海洋、航天、军事、农业、 服务、娱乐等领域中得以应用。移动机器人系统是由一个机械手固定在一个全方位移动机 器人平台上构成。这样机器人同时具有移动和作业两个功能。这可以使机械手在广泛的空 间中以精准、适合的姿态来执行任务。这显然比传统的工业手臂更有优势。 从移动方式上看，移动机器人可以分为轮式、履带式、腿式( 单腿、双腿及多腿式) 和 水下推进式等多种结构，其中轮式结构易控制、对稳定性问题影响小、单位移动距离消耗能 量小、且可以比腿式结构移动更快，另外轮式结构的机器人中心由车轮与地面形成多边形来 保持，加上适当的车轮配置方式机器人可以稳定而灵活地就地旋转、横向平移，具有了以上 优点，轮式结构的机器人在移动机器人中得到了更加广泛的应用。据1 9 9 1 年统计，在1 9 个 国家的1 1 2 个机构已宣布的2 2 8 种移动机器人中，有1 0 7 种机器人是轮式移动机器人【3 】。实 践表明，轮式移动机器人非常适合在光滑、坚硬的表面工作。随着移动机器人技术的研究重 点从面向特殊环境到面向一般环境的转变，此类机器人正逐渐成为移动机器人技术研究的 主要方向，引起了十分广泛的关注。 1 2 移动机器人的研究意义 随着各国对国家安全等公共事业的高度重视，研究开发在恶劣环境下的危险作业移动 机器人已受到高度重视，并已成为国家关键技术研究对象。如闩本的“R & D O F R O B O T I C S 1 NE X T R E M E E N V I R O N M E N T ” 极限作业机器人研究计划。我国的“8 6 3 ”计划也在这 一领域取得了突出成果。研究出的移动机械手在易燃、易爆物品的装配、搬运或拆卸，以及 消防灭火、反恐等高度危险场合代替人类或辅助人完成高危工作，具有迫切重要的现实意 义。 13 移动机器人的发展现状及前景 早在6 0 年代，国外就有了关于移动机器人的研究，发展到今天，移动机器人涉及到的各 学科门类日盏丰富，也给国内外的研究人员及学者提出了许多新的挑战，更由于其在军事侦 察、排雷排险、防核污染等危险与恶劣环境具有广阔的应用从而得到了普遍的关注 i j 。 美国N A S A 在实施火星探测计划时，发射了两个火星探测器海盗I 和海盗I I ，它们在火 星上采集样品，作各种实验，并能将实验结果发回地球：在海洋开发方面，有水下移动 图1 1 “海盗I ”号火星探测器图1 2 “海盗I I ”号火星探测 机器人，可在水下上千米处作业t 装有视觉传感器和少自由度机械手臂，可收集海底标本、焊 接管道等：在化工和具有放射性环境中，移动机器人可用来检查、修复管道、阀门等；在军 事上，可在危险地带代替人完成侦察和排雷等任务。在，土活上，可在条件不足或人工无法达 到的状态下，人通过手机或掌上电脑等高新微型电子产品远距离摇控移动机器人前往事件 地点完成任务。机器人身上装有橡胶履带，速度高，多种探测器能收集充足信息，机械手抓力 大，非常实用。如救援机器人，消防机器人，攀登机器人和安保机器人等就属于这样的类型如 图l 一3 和图l _ 4 所示。 移动机械手目日还处于研究、跟踪和试验阶段。最主要的任务之一就是寻找更快更好 的系统控制技术。从目前情况看，控制的方式很多，如：自适应控制、变结构控制、鲁棒控 制、位置功混合控制，非线性模糊神经网络控制、路径跟踪控制、解耦控制等。其主要 图1 3 消防机器人图1 - 4 安保机器人 目的在于找到一种最优控制律，使得机器人手端从任意起始位置出发能快速、精确、平稳的 到达期望位置。 对于自由运动机器人来说，控制器可按照运动特性而分成两种。一种是运动学控制 ( K i n e m a t i c s C o n t r 0 1 ) ，它不考虑机器人的动力学特性，只利用轨述误差进行反馈控制。其 特点是控制律简单，但难保良好的动态品质；另一种是动力学控制( D y n a m i c C o n t r 0 1 ) ，设计 非线性控制律，保证良好的动态品质。然而机器人动力学方程足强耦合的非线性微分方程组， 并含有众多三有函数，其计算量太大，实时控制比较困难。随着计算机技术的发展运算能力 大大提高，这就使各种动力学控制方法得以充分发展而日益完善，本论文也将重点研究移动 平台的动力学控制方式。 在非结构环境下，移动载体和其多自由度机械手的协调控制技术一直未恰当解决，移动 机械手的协调与控制机理和耦合机制还有待更深层的研究，这也是移动机器人应用所面临 的困难和挑战所在。 14 本论文主要研究内容 机器人机械结构设计，是根据实际需要进行的。在机器人机构方面，结合机器人在各个 领域及各种场合的应用，研究人员开展了丰富而富有创造性的 _ 作。在研究初期，机械手臂 主要帆事工业作业。工作简单，适应性差，重复运动多，因此对机械手本身的自由度的要求就 比较低。一般4 自由度就能满足作业要求。然而，在科技高速发展的今天，人类所面临的工 作越来越复杂精细，在实际操作中，需要灵巧多变的操作手来适应任何角度，任何位置的安 排。少自由度操作手臂显然不能满足现代产业制造和任务执行的需要所以要增加自由度以 增大手臂的工作范围和效率。 迄今为止，少自由度( 4 ) 机械手臂的研究已经趋于完善，无论在运动学控制还是在动 力学控制方面，都可按照设计者的意途去执行。这主要得利于少自由度机械手的运算简单， 误差易于修正等因素。但是由于它自身的缺陷，其发展潜力已至极限。多自由度( 5 ) 手臂以其自身结构优势已经成为发展的主流。其特点是：工作范围广，精确度高，稳定性 好。操作更加简单。虽然，当自由度大于5 后其运算量成倍增加，但是依靠先进的计算机技 术，与各关节变量相关的数据计算已不是难题。而且，为了突破传统机器人在工作范围上的 限制，可将多自由度机械手安装在一个移动平台上，形成移动机器人。这更加扩展了机器人 的功能性和实用性。当然，越复杂的机体，其设计就会越困难。对于移动机器人，不光要 考虑机械手的定位，还要考虑平台的位姿、运动轨迹等实际问题，其受控的难度显而易见。 因此多自由度机器人的研究应该得到充足重示。这也正是本文选择5 自由度移动机械手为 研究对象的初衷。 本文一开始构造了合理的5 自由度可移动机械手的数学模型，在查阅大量中外文献资 料后，对移动机械手的手、车两部分分别进行了详细的运动学和动力学的研究。第二章主要 是有关运动学方面的问题研究，文中一开始建立了机械手的D H 参数表，按照几何规则完成 了操作手的定位和定向，并根据数据求出机械手J 下、逆运动学解。同时也分析推导了三轮移 动平台在运动过程，各个轮子的线速度、角速度公式。这为第三章中运用牛顿欧拉定理推 导机械手的动力学方程，以及用牛顿定律推导平台受力和力矩的平衡方程打下了基础。在一 定程度上统一了移动机器人的两部分动力学模型。在第四章中，借鉴弗莱纳雪列原理，利用 曲线几何学对机械手工作连续路径进行了规划，从而实现了连续路径的跟踪。在第五章中， 以移动机器人整体模型为基础，通过对平台的受力分析，推出完整的动力学方程：并设计控 制器，控制移动平台的平稳运动。在最后一章中，运用几何法构造移动平台的路径跟踪误差 方程，即速度误差方程和位姿误差方程。接着设计动力学控制器，使平台速度和位姿趋于给 定值，从而实现路径跟踪。 文中控制过程比较频繁，在构建完模型之后，往往需要通过仿真手段来验证模型的正确 性和控制的可行性，因此，文章中对机械手模型，机械手正逆运动学，牛顿欧拉动力学，移动平 台动力学以及移动机械手路径跟踪都做了M a t l a b 仿真实验。当然，仿真过程中遇到了很多 实际难题，例如移动机器人受到各种不确定因素影响，动力学方程不精确己知，手车之间的强 耦合等，都对控制器的参数选择带来了不便。控制器为了保证稳定的性能，采用了鲁棒控制。 在以后的研究中，模糊神经网络的控制方法将会是一个重点选择。 4 2 1 引言 第二章移动机械手运动学 本章内容是研究所有问题的基础，意义重大。机械手的运动学问题主要是研究机器人各 关节变量与其末端执行器位姿的关系。本文为了分析明确，将整个移动机器人模型分成手和 平台两部分。在建立了5 自由度机械手模型的基础上，设立了相应的D H 参数表，分析了机 械手的正、逆运动学问题，并用M a t l a b 中的R o b o t 工具箱对机械手的动作做了精确的仿真。 后几节是有关平台的运动学分析，推导出轮子的速度、角速度公式及平台运动学模型。最后 推导出用轮速和关节变量角速度来表示末端执行器的速度的非完整约束公式。 2 2 运动学知识8 3 2 2 1 刚体位姿的数学描述 ( 1 ) 位置矢量 对于一个刚体，若给定了其上某一点的位置和该刚体在空间的姿态，则这个刚体在空间 完全得到定位。设有一刚体如图2 1 所示，O 为刚体上任意一点，O X Y Z 为固定坐标系，刚体 在0 系中的坐标可用一个列矩阵表示 M 天。= fY 。f ( 2 - 1 ) U 此式表示的即是刚体上的0 点在D 系中的位置。令疗、t 、b 代表D 。系上这坐标轴方向的 单位矢量，于是刚体在固定坐标系0 内的方向可用门、，、b 三个矢量组合起来的3 阶矩阵 C 表示：C = t , f ，b 】。矩阵C 在0 。系的表示，即为0 各坐标轴投影在0 系上的方向余弦。 图2 1 刚傩在空间中的位姿 r ( 2 ) 转动矩阵 设有两个共原点的右手坐标系O X i Y i Z iJ 阳O X j Y j Z I ，后一坐标系可认为是前一系绕定点 0 旋转而成的。若P 点在i 系中的坐标为k ；Y ；乙】T ，在j 系中为k ， 示，以i 系为参考坐标系，根据投影关系，P 点的坐标变换为： M = I ：R J 【，工 其中： Y jz ，】r ，如图2 2 所 ( 2 2 ) C O S ( X ，Y ) C O S ( X ；，z ) l c o s ( y J ，Y ，) c o s ( y ，z 川 ( 2 3 ) c o s ( z J ，Y ，) c o s ( z Z ) j 式中【江J 称为从j 系向i 系变换的转动矩阵，也可称之为方向余弦矩阵。由于l J = 【绰】_ J = 【j 月】r ，故转动矩阵为正交阵。 绕一个坐标轴旋转的转动矩阵是最基本的转动矩阵。 绕x 轴转口角的转动矩阵为歹尺。，乡，：f 三c 三日一：臼1 ： 10s i n 目 c o s 0l 绕Y 轴转口角的转动矩阵为歹尺( J ，9 ) - - 1 010 C O S 0 0s i n 0 I ； Il 卜s i n 0 0c o s j f c o s s i n O 0 绕z 轴转口角的转动矩阵为R ( z ，臼) = s i n 0 c o s O o 1 。 【_ 0 0 1 j 直角坐标系中，一个点可表示为 abc 】下，若用四个数组成列向量U = X y Z W ( 2 4 a ) ( 2 - 4 b ) ( 2 4 c ) 来表示点 a b c 】T ，且令它们之间的关系为a ：三，b ：羔，c = 三，则称【xYzw T 称为三维空间点【a bc T 的齐 W WW 6 一 局”乙嘶叫州 j l 尼 次坐标。 设矢量U 沿矢量P = a i + b j + c k 平移，可看作U 和P 两矢量相叠加，如图2 3 所示。 即有： V = U + P = 上式中H 即为平移齐次变换矩阵。 X 口+ 一 W 6 + 上 W Z C + 一 W l 1 0 0l 0 0 0 0 0 口 0b 1 c Ol X 少 Z W = 日 ( 2 5 ) 图2 3 刚体坐标。齐次线性变化图 同理，若假设R 为旋转矩阵，则 H = 0 尺0 0 O O 01 为齐次旋转矩阵。这样，平移加旋转变换的复合变换矩阵即为 H = a 尺b C 0 O 0l 以后的文章中均称之为齐次坐标变换矩阵。 ( 2 6 ) p ip 1 儿 9 1 I( 2 7 ) 001 J 2 2 2D e n a vit H a r t e n b e r g ( D H ) 建模方法小9 1 本文研究的串联机械手是由开链简单运动链组成。开链( o p e nc h a i n ) 机器人，也称串接杆 7 件( S e r i a lL i n k ) 机器人，是出若干刚性杆件连成，杆件f 白J 的连接物称为关节，关节分类为转动副 ( R ) 和棱柱副( P ) 。在机器人中把各个连杆连接起来时要考虑和解决许多问题。包括关 节的强度、关节的润滑方式、轴承的类型以及轴承的装配方法。然而在研究机器人的运动 学问题时，仅需要考虑两个参数，这两个参数完全确定了所有连杆的连接方式。这就是 D e n a v i t 和H a r t e n b e r g 在1 9 5 5 年提出的D H 参数法。 为了描述串联机械手的运动链结构，首先对各杆件和关节进行编号：基座为杆0 ，从旱向 外依次为杆1 ，杆2 ，；关节i 连接杆i 一1 和杆i ：杆件r l 是术端执行器。定义坐标系毋的 原点为O i ，轴为墨、r i 、Z i 。坐标系固定在第f - 1 个杆上，f = 1 , 2 ，z + l 。前n 个坐标 系建立的规则如下： 第l 步：确定各坐标系的Z ；轴。基本原则是：选取Z ，轴沿关节i + 1 的轴向( 指向可任 选，但通常都将各平行的Z 轴均取为相同的指向) 。这里需要说明的是： ( 1 ) 当关节f + 1 是移动关节( 即O t + 。= 1 ) 时，其轴线指向已知但位置不确定，这时选取 互轴与互+ 。轴相交( 若还有O i + ：= l ，则取互轴和互+ 轴都与互+ ：轴相交；) 。 ( 2 ) 机器人杆n 的远端没有关节n + l ，这里可选取互轴和z 一。轴重合。 第2 步：确定各坐标系的原点。基本原则是：选取原点D j 在过Z 。轴和Z ，轴的公法线 上( 即D 为此公法线与互轴的交点) 。这里要说明的是： ( 1 ) 当互一。轴和z j 轴平行时，经过两轴的公法线不唯一，确定q 的方法是：若互一，轴和 互轴重合，取D f = D j - J o 若互一轴和Z 。轴平行且不重合，过O 。点作Z H 轴和z j 轴的公法线， 取此公法线与z ，轴的交点9 。 ( 2 ) 由于没有Z 。轴，故无法按上述基本原则选取p ，这时确定q 的方法是：若Z 。和 互相交时，取0 0 = q ；若五和z ，不相交时，取D I ，在磊和z 的公法线上。 第3 步：确定各坐标系的x 轴。基本原则是：选取x ；轴沿过互一轴和互轴的公法线， 方向从Z 。指向Z ，这里要说明的是： ( 1 ) 当互一轴和互轴重合时( 这时Q 一。= q ) ，选取X 轴满足在初始位黄时X 轴与X 一 轴重合。 ( 2 ) 当互一。轴和Z 。轴相交且不重合时，选择X ，= ( z xZ ；) ，通常使所有平行的X 轴 均有相同的指向。 ( 3 ) 当，= 0 时，由上所述知，这时p ，= O 或p ，在互，轴和Z 轴的公法线上，选取在初始 位置时X 。轴与X 轴重合。 第4 步：确定各坐标系的Y 轴。原则是使Z = 互x X 。，即构成右手坐标系。 接下来设置阴参数，如图2 4 所： ( 1 ) 杆件长度a 。定义为从互轴和z ，轴的距离，沿X ；轴的指向为正。 ( 2 ) 杆件扭角口定义为从互一轴和互轴的转有，绕X 轴正向转动为正，且规定 口，( 一7 ，7 】。 ( 3 ) 关节距离d 。定义为从X 轴到X 。轴的距离，沿z 一轴的指向为正。 R ( 4 ) 关节转角9 定义为从X 轴到x ，轴的转角，绕互一。轴正向转动为F ，且规定 9 ( 一万，万】。 图2 4 连杆的D H 参数表示图 考虑到从i l 系到i 系的变换，可先令i 一1 系绕互一轴旋转p 角，再沿互一。轴平移t ，然 后沿X 轴移动a 。，最后绕x 。轴旋转口，角，使i 一1 系与i 系重合。用变换矩阵表示，则有： A i = R o t ( Z ，0 i ) T r a n s ( O ，0 ，d i ) T r a n s ( a i ，0 ，O ) R o t ( X ，C t i ) 。c e 一S e i 0 o I F , 0 s 岛c 岛0 0 0 01 0010 80 0 000 l J L o 0 睢 O0 C 口fJ 口 S O t i c o c i 0O 降誊科Q 。 ( 2 - 8 ) 这也是我们要得到的旋转变换矩阵，且仅表示方向上的变化问题。不失一般性，我们可以将 这两个坐标系的原点放置在相同的位置，将这个旋转分解为两个旋转【2 1 ，；n T l 蛩2 5 所示。 9 q 0 4 B C S 彳1 O 0 0 啄q o 圈2 - 5 坐标旋转示意图 在图中，x z ：是一个中间坐标系f ，它是通过坐标系F 绕互轴旋转一个角谚得到的 ( 如图a ) 。然后，中间坐标系F 。绕旋转角度口，使其位形与坐标系f + 。重合( 如图b ) 。设 前面的旋转分别表示为【c j l 和【人。1 。进而，设丑 - - C O S O f ；，以= s i n a 。，则原来的Q 变为： f - c 岛一J 岛c 口fs O i s a f f - c o , - 丑s o , 。s o , 9 兰 O i i 垒lJ 岛 c 岛c 口f c 岛s 口fl 。ls o , 丑c 9 一u , c O I l 0 s 口fc a ij L0 以五J ：；笺- c S 岛O l 司三乃0 一：， ：【C f 】f 队，L 三GA ， 。2 9 ， l 00 1 儿0 - t i 乃j 为了能在操作手运动学中应用这个条件，将矩阵Q j 分解因子：Q = 互X ；，其中X 和互 定义为两个映像，X 是对y 互平面的映像，互是对X 。1 ) ：平面的映像，即： - 1 0 0 陆s 岛0 置= l0 一五小Z f = Ij 岛一c 岛0I ( 2 1 0 ) 1 0 肼乃jl 00 1 j 注意，X 和互都是对称和自逆的矩阵。下面导出从坐标系F 的原点到坐标系F + ，的原点的 位置矢量瓦的表达同式，图2 - 6 中给出了不同坐标系的原点位置和坐标轴之间的关系。 图2 - 6 连杆坐标系的转变示意图 l O a ：暑D ：D ；= 00 + 0D ( 2 1 1 ) II + - + l、， 这旱，明显地有： 丽，铆c 丽k - = 剐 弘- 2 ， 将两个矢量变在同一坐标系下表达，即在F , - 中，这样 甄卜蚍【瓦】+ 。2 匿搿 陋1 3 ) 因此： 一 f _ a ，c o s O , 1 a i i = l 口js i n 9I ( 2 - 1 4 ) l 匆J 为了简单起见，我们引入下面的定义：i Ai 】。与Q 分解相似地，将矢量表示为孑= Q i 万。 这里，万由下式给出： 一h 匆一A 陋。f ( 2 - 1 5 ) L b A J 其中的段和以由前面的式子定义。 对于转动运动副瓦为常数。根据图2 - 6 可知，显然瓦就是在坐标系F 。T f 拘一a , ，即瓦= k 】。 矩阵Q ；也可以看成坐标变换。实际上，设i ；，_ ；和k ；分别为平行于X 。，y 和Z ；轴的单位矢 量，并且方向指向这些轴的正方向，从上图容易得出： 瞳+ - = 雾孑 c 2 一，6 的 k J 】i = 降叫 倍m ， 从而： J i + 1 1 也蚺】：忡- 兄, s i n 9 0 , ( 2 - 1 6 c ) L 从j 这样，t Z + 和k 川是在F 中的矩阵Q 的3 个列矢量。 在坐标系只。中的任意矢量 M = Q f lI v 州 ( 2 1 7 ) 同样地，在坐标系F + ，中的任意矩阵M 可以通过一个相似变换，变换到坐标系F 中，即： 【M 】，= Q 膨】。 饼】， 它们的逆关系直接给出如下： M 。= 饼】iM ，垒Q 7M ； M 】= Q 1 【M 】， O ，】，垒Q 1 M ，Q 由此而得： M ；垒Q 【，】j + l M 】；一AQ f 【M LQ f r 进而，如果我们有i 个坐标系链F ，互，F ，则从F 到F 向内的坐标变换为： V 】= Q Q ：Q 一，M ， 彤 = Q Q 2 2 一【膨】。( 2 2 Q 一。) 7 类似地，向外的坐标变换为 1 ，】；= ( Q Q 2 Q j 一) 7 ，】， M 】。2 ( Q 。Q ：Q ) 7 M 。( Q ，Q ：Q 。) 2 3 机械手正运动学问题分析 5 自由度机械手有5 个关节变量p ，0 2 ，B 。研究正运动学问题，实际上是对于操作手 的末端执行器的一个简单位姿执行操作手的几何学问题研究。首先我们要明白什么是运动 学正问题。在运动学正问题中，假设已知一个给定的5 轴操作手的关节变量，求操作手末端 执行器的位姿。 本例中5 自由度机械手的全部关节都是旋转关节。设直线g i 作为第i 个转动关节的轴， 直线的正方向可以用一个单位矢量e ；任意定义。这样F + 坐标系相对F 坐标系的旋转可以 通过杆件之间的几何学进行定义，也就是用D H 参数口，吐和嘭，加上P ，和它的相应的关节变 量p 。于是，D H 参数和关节变量唯一地定义了操作手的位姿。实际上，坐标系F + 。相对于坐 标系F 的位置和方向的关系分别为： c O , 一2 , s 0 , ；J 9 Q = ls o , 2 , c 0 , 一。c o , I ( 2 - 2 3 a ) | - 0 以五 l l 吒c o s 0 , l a i2 hs i n 0 , l( 2 - 2 3 b ) L _ 匆- J 它们的实际意义是：矩阵Q 和矢量q 分别表示，坐标系f 旋转到与坐标系f + 方向相同， 坐标系F 的原点移到与坐标系F + 相同的位置，坐标系从F 变换到f 一设末端执行器的方 向表述为Q ，工作点P 的位置为位置矢量声。在坐标系F 下时，有： 即 贴 瞻附， ， 捌，凇， 埘 州州 圳撕 埘撕 谢训 ， ， (，LL，L，L，L 2 、二 厶二 厶厶 Q Q ：】： Q 】1 Q 】。 g 】，= 纠( 2 - 2 4 ) 将 Q 】j 简写为Q ，这样式2 - 2 4 可变为： Q ，Q ：Q 、Q 。Q 、= Q( 2 - 2 4 ) 相似地，由几何关系2 1 可得知： 云。十Q 厅：+ Q Q ：厅、+ Q ，Q ：Q 、云。+ Q 。Q ：Q 、Q 云，= 芦 ( 2 - 2 5 ) 如果引入2 2 1 节中定义的齐次变换，式2 - 2 4 可变为： 4 4 ：4 ，4 A ，= A( 2 - 2 5 ) 其中A 表示从末端执行器坐标系到基座的坐标变换，且 、 4 = 雕 这样么包含了末端执行器的位置，显然彳是矢量 1 c 。s P 分成行和列，即 r 而r Q 2 i 髟f 2 晦一玩面J( 2 - 2 6 ) 需要指出的是Q 的第3 行的虿7 是与矽无关的。注意到第i 个关节轴方向的单位矢e I 按D r i 参数表示，在坐标系F 下的分量形式为 。 k 扣阱 后，e 表示3 维数组，它的最后一个分量等于1 ，其他分量为零。这样，我们有： p 石= Q j 面= P ll 一 l 或者： ( 2 2 8 ) 面。= Q ，e ，否；= Q e( 2 - 2 87 ) 这样，如果把e 前面的第一个关系看成k + L ，第二个关系看成k ，由坐标变换式2 1 9 a 和 2 1 9 b 得： 曩= k 。】。和万= k L( 2 2 9 ) 下面，根据上述定义为文中5 自由度机械手建模。图2 7 为5 自由度移动机械手坐标模 型示意图，其中坐标系i 与连杆i 无相对运动。 图2 75 自由度移动机械手模型草图 由2 2 2 小节中的知识，我们可以写下5 自由度机械手的D H 参数表。如下所示： 表2 15 自由度机械手的D H 参数表 将D H 参数代入式2 2 3 ，可得： 卜E Q = Is O , 0 Li ，Q = 葶了爹习，Q = 葶 1 4 杆件编号f 口 口；( m )只Z ( m ) I 1 p i 2 O q d l = 0 4 0 0 2 0 a 2 = 0 2 0 0 0 2 0 3 p i 2 a 3 = 0 1 5 0 岛 0 4 一p i 2 O 只 d 4 - 0 1 0 0 5 p i 2 O 包 0 6 00 0 d 6 = 0 3 0 0 末端执行器 只以。 卵 吖。 则Q = Q ，Q ：Q ，Q Q ，= ( Q ，Q ：Q ，) ( Q ；Q ，) 如果设： 则有： c O , s O ：O ，s O , 一s O , s O ：J 只- c O + C 0 2 5 0 3 0 c 9 c 只c B 尹( 幺+ Q ) 一c p J 幺5 【幺+ 最J + s O s O ：O ， 5 9 f 只c 够尹( 幺+ 纪) + s O , S O ： + 幺J c o , s o , c o , c o , c o , ；( y 2 + 晓) + j 够c 恺+ 幺J - c o , , O , c ( O , + 幺) + s O s e , 一s O , s o ：( 0 2 + 包) - c O , c O , 一s 见s + 以) Q = E 蟊函，1 = 【P l ， 只= c o , c o ：o ：( o , + 只) 一c E s 配J ( 幺+ 只) + J 鼠J 只c 绣 c p c O , s O , c ( 2 ：+ B + c O c O , s ( O + 幺) + s O ，s O s 8 , J 只c 只f 只罗( 幺+ 纪 + j 9 c 幺s ( 幺+ E ) 一c O , s o ：o ，； c p s 鼠5 ( 包+ 包) 一c o , c ( o , + 谚j = j q c 皖c 幺c ( 包+ 包) + s O , s o , s ( o , + 皖) 一c O , s O , c O , = c 只c e s + 幺) + J 只c + B ) 。 = 一c O , s O , c ( e , + 只) + s p c 只 = 一s 9 J 只c + 幺) 一c E c 幺 2 一J 只J + 只) = c E c 只s 谚c ( 皖+ 包) + c 鼠c 只J + 只) + J 鸟J 只J 谚 = J E c 只c 只c ( 包+ 谚) + J E c 幺J + 0 3 一c 9 J 只J 以 吒= c O s O ：( o ：+ 岛) 一f 包c + 只) 可见，Q 就是机械手运动学正解。 2 4 机械手的M A T L A B 运动仿真 s O , c E 0 ( 2 3 0 ) ( 2 3 1 ) ( 2 3 2 ) 利用M a t l a b 的R o b o t i c sT o o l b o x 工具箱中的li n k 和r o b o t 函数来建立上述五自由 度机械手的模型。简单介绍一下几个函数调用格式： L = L I N K ( a l p h aAt h e t aDs i g m a ，C O N V E N T I O N ) l，J 艰鸺。 秒伊一D 的的o f Q lj 硼一崛o 0 O 1臼口D阳妇o P。，。L = Q 嘲睨嵋憾憾硼 C S P、，L 1j 破织 C C 良防织 S S C 鼠p 幺 C j C + + 伊，幺幺 S S S 矿，织良 f C S p 9 C S 织良反 C C C 易织织 C C S 鼠9 f 摹 PL 1J一 甜， “，“，吼吼吼 只见吼吼吼砟咋 5 在上面格式中，参数C O N V E N T I O N 可以取s t a n d a r d 和m o d i f i e d ，其中 s t a n d a r d 代表采用标准的D H 参数，m o d i f i e d 代表采用改进的D H 参数。参数 a l p h a 代表扭转角，参数A 代表杆件长度，参数t h e m 代表关节角，参数D 代 表横距，参数s i g m a 代表关节类型：0 代表旋转关节，非O 代表移动关节。 另外L I N K 还有一些数据域，如下面的命令行： R O B O T ( L I N K ) 这个命令用L I N K 来创建一个机器人对象 L 1 ) = l i n k ( p i 2 00 O 4 0 0 0 】，s t a n d a r d ) ； L 2 = l i n k ( O 0 2 0 000 O 】，s t a n d a r d ) ； L 3 = l i n k ( p i 2 0 15 000 o l ，s t a n d a r d ) ； L 4 2l i n k ( - p i 2 000 10 0 0 】，s t a n d a r d ) ； L 5 2l i n k ( p i 2 000 O ，s t a n d a r d ) ； r = r o b o t ( L 1 ) ，L 2 ) ，L 3 L 4 ) L 5 ) ) r2 n o n a m e ( 5a x i s ，R R R R R ) g r a v = 【0 0 00 0 09 81 】 s t a n d a r dD & H p a r a m e t e r s a J p h a 1 5 7 0 7 9 6 0 0 0 0 0 0 0 1 5 7 0 7 9 6 1 5 7 0 7 9 6 1 5 7 0 7 9 6 A 0 0 0 0 0 0 0 0 2 0 0 0 0 0 0 1 5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 t h e t a 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 D 0 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 R ，P R ( s