（机械电子工程专业论文）机器人的运动规划研究与基于opengl的仿真.pdf

摘要 摘要 过去4 0 年以来，机器人作为人类的伟大发明广泛应用于各个行业。随着柔性自动化系统的 发展，冗余揲作受到了研究学者和企业越来越多的重视。到目前为止，针对冗余机器人已经有 学者作了大量的研究工作。其中人部分是理论研究，主要集中丁二运动学的规划与优化，尤其是 避障的情况f 的研究。研究表明运动学规划在冗余机器人研究领域起到了非常重要的作用方向。 本文对冗余机器人的运动学路径规划进行了研究。研究了3 自由度平面机器人的包括位置 运动学和逆运动学在内的运动学特性，建立了纂于人工势场法的模型来求解机器人的逆运动学， 获得了精确解。本文引入了伪最小平移的概念，并墓于此对路径规划算法进行了研究。 为了便丁进行研究，有必要开发冗余机器人的仿真器。因此在本研究过稃中，开发了个 基to p e n g l 的仿真器。井将仿真结果与当前理论研究的结果进行了详细比较。证明了仿真器 和理论研究的有效性。 关键词：冗余度机器人；逆运动学；路径规划；伪最小平移距离；人工势场法 东南大学硕i 学位论文 a b s t r a c t i nr e c e n t4 0y e a r s r o b o ta st h eh u m a n sb i gi n v e n t i o nh a ss p e e di n t oa l lk i n d so f o c c u p a t i o n h l o n gw i t ht h ei m p r o v e m e n to ft h ef l e x i b l ea n da u t o n o m o u ss y s t e m ，t h e r e d u n d a n to p e r a ti o nw a sr e g a r d e dm o v ea n dm o r ei m p o r t a n tb yt h es c h o l a r sa n d t h e m a n u f a c t u r e s u pt on o w t h e r eh a v eb e e nn u m e r o u ss t u d i e sa b o u tr e d u n d a n tr o b o t s m o s t o ft h e mw e r et h e o r yr e s e a r c h e s ，m a i n l yo nk i n e m a t i c a lp l a n n i n ga n do p t i m i z a t i o n ， e s p e c i a l l yi nt h ec a s eo fo b s t a c l ea v o i d a n c e i ts h o w st h a tt h ek i n e m a t i c a lp l a n n i n g p l a yav e r yi m p o r t a n tr o l ei nt h er e s e a r c hf i e l do fr e d u n d a n tr o b o t s i nt h i st h e s i s ，s o m ei s s u e so np a t hp l a n n i n go fk i n e m a t i c a lr e d u n d a n tr o b o t sa r e s t u d i e d a c c o r d i n gt oap l a n a r3 d o fr o b o t ，k i n e m a t i c sc h a r a c t e r i s t i ci n c l u d i n gb o t ht h e p o s i t i v ek i n e m a t i c sa n dt h ei n v e r s ek i n e m a t i c so ft h er o b o ta r ei n v e s t i g a t e d t oo b t a i n t h ei n v e r s ek i n e m a t i c so ft h er o b o t ，am o d e lo ft h ea r t i f i c i a lp o t e n t i a lf i e l d ( a p f ) i sb u i i t t h em a t h e m a t i c a ls o l u t i o ni so b t a i n e d t h ec o n c e p to ft h ev i r t u a lo b s t a c l e d i s p l a c ei si n t r o d u c e di ut h et h e s i s a n dap a t hp l a n n i n ga l g o r i t h mb a s e do nt h ev i r t u a l o b s t a c l ed i s p l a c ei si n v e s t i g a t e d f o rt h ec o n v e n i e n c eo fr e s e a r c h ，i ti sn e c e s s a r yt od e v e l o pa ne m u l a t o rs y s t e mf o r r e d u n d a n tr o b o t s a n0 p e n g lb a s e ds i m u l a t o ri sd e v e l o p e di nt h et h e s i s t h es i m u l a t i o n r e s u l t o ft h et h e o r yr e s e a r c h e si sp r e s e n t e dw i t had e t a i l e dc o m p a r i n ga n a l y s i s a n d t h es i m u l a t i o ns h o w sc e r t i f yt h ee f f i c i e n c yo fb o t ht h et h e o r yr e s e a r c ha n d t h e s i m u 】a t o r k e y w o r dlr e d u n d a n c yr o b o t ；i n v e r s ek i n e m a t i c s ；p a t hp l a n n i n g ；v i r t u a lo b s t a c l e ： a r t i f i c i a lp o t e n t i a lf i e l d ( a p f ) 东南大学学位论文独创性声明 本人声明所呈交的学位论文是我个人在导师指导下进行的研究工作及取得的研 究成果。尽我所知，除了文中特别加以标注和致谢的地方外，论文中不包含其他人 已经发表或撰写过的研究成果，也不包含为获得东南大学或其它教育机构的学位或 证书而使用过的材料。与我一同工作的同志对本研究所做的任何贡献均已在论文中 作了明确的说明并表示了谢意。 研究生签名：骚圣匕日期：必( ：璺：星) j f 东南大学学位论文使用授权声明 东南大学、中国科学技术信息研究所、国家图书馆有权保留本人所送交学位论 文的复印件和电子文档，可以采用影印、缩印或其他复制手段保存论文。本人电子 文档的内容和纸质论文的内容相一致。除在保密期内的保密论文外，允许论文被查 阅和借阅，可以公布( 包括刊登) 论文的全部或部分内容。论文的公布( 包括刊登) 授权东南大学研究生院办理。 期： 第章绪论 第一章绪论 机器人作为人类2 0 世纪最伟大的发明之一，在短短的4 0 年内发生了日新月异的变化。近几年 机器人己成为高技术领域内最具有代表性的战略目标。机器人技术的出现和发展，不但使传统的 j ：业生产面貌发生了根本性变化，而且将对人类社会产生深远影响。随着辛卜会生产技术的飞速发 展，机器人麻_ h j 领域不断扩展。从自动化生产线剑海洋资源的探索，乃至太空作业等领域，机器 人可谓是无处不在。 1 1机器人的发展现状 机器人技术是一个讣常广阔的研究领域，它涉及到一些学科间的研究和发展。这些学科包括 运动学、动力学、规划系统、控制、传感技术、计算机科学和人工智能。机器人技术代表了机电 一体化的展高成就。目前，机器人技术已应用剑国防、工业、科技等j “泛领域，并向智能化方向 发展。机器人技术的出现和发展，不但使传统的f 业生产和科学技术研究发生了根本性的变化， 而且将对人类的丰十会生活产生意义深远的影响。世界各国都菲常重视机器人技术的发展和机器人 在生产生活中的应用。 1 1 1 机器人的发展现状 按智能程度的商低可把机器人技术发展分为二个历史时期：一、“示教再现”式机器人先由 人手把手地教会它应该经历的所有点，并把各点的位置、速度、加速度都记录在存储器中，然后 在机器人自我运行时，由这些数据计算出伺服系统电机的控制信号，进而控制机器人关节的运动， 最终完成指定的任务；二，具备一定感觉能力的第一二代机器人，它具有了对外界一些信息反馈的 能力，例如，有了一定的听觉、触觉、视觉感知的能力，比起示教再现型控制起来更加复杂，当 然对计算的快速性要求越来越高( 计算机的运算速度的提高) ：三、智能型机器人，它的视觉系统 由摄像机、测距系统、图像处理器芎组成，另外它的听觉、触觉能力比第二代有了很大提高，并 具有了判断决策能力，在很多方面具有了人类的特点。目前，机器人正朝着智能化方向发展，不 仅使朋了各种先进的、高精度的传感器及设备，如摄像机系统、卢纳系统、图像处理器、光纤陀 螺定伊系统及罗盘定位系统等，而且还运_ h j 了各种控制策略和控制算法，如经典的p i d 控制、变 结构控制、变结构内模控制、分层递阶控制、模糊控制、神经网络控制、模糊神经网络控制、遗 传算法( g a ) 及进化计算方法等”1 ，除此而外，各种先进的通信技术也竟相施展，如g p s 定位系 统、基于i n t e r n e t 的多智能体协作以及远距遥控技术( 像水f 无缆机器人、太空机器人、自动引 导小车等) 均是这些技术的应用结果”“。总之。目前机器人性能水平较以前已有了较人提高， 应h j 前景也越来越好。 1 1 2 冗余度机器人 在机器人系统中，为使机器人具有高度的自制性和灵活性，就需要有更多的传感器与驱动器， 具有冗余传感器的系统被称为传感冗余系统；而具有冗余驱动器的系统称为机器冗余系统。机器 冗余是为了提高机器人操作臂的灵活性( d e x t e r i t y ) 。灵活性是指机器人在各种不同的环境中 执行不同作业的能力。一个具有高灵活性的机器人应该是具有两个或多个机械臂而不是仅有一 个，其末端执行器应该是具有多个手指而不是仅有开合式的手爪。机械冗余系统义分为运动冗余 和驱动冗余两种形式。通常我们所说的冗余机器人，除非特殊指定，一般是指具有运动学冗余的 i 东南人学硕p 学位论文 机器人。运动学冗余机器人指的是机器人的操f f 臂具有的自由度数多于完成一个任务所需的最少 自由度的机器人操作臂。 随着对机器人系统灵活性和自主性要求的提高，冗余度操作机愈来愈受剑学者和生产厂家的 重视。至今有关机器人冗余度的研究己有许多，其中大部分是理论上的，或是讨论冗余度逆解的 方法”“或是优化过稗中的不同性能指标”“的完善手段。国内冗余度机器人的研究已起步，如哈 尔滨1 = 业大学、北京航空航天大学、中科院沈阳自动化研究所以及华中理丁大学等都做了相关的 理论和实验研究”1 。当前所研究的冗余度机器人多数为空间七自由度冗余机器人和平面二自由 度冗余机器人。而在冗余度机器人研究中研究最多的是运动学优化问题。 1 1 3 冗余度机器人举例 图1 - 1 是隶属于日本国际贸易与工业部( m i t i ) 的机械l ：稃实验室1 9 7 4 年开发的m e l a r m 机器 人系统。该系统包括两个7 自由度机械臂，每个臂都具有拟人结构，其关节由电液伺服驱动器驱 动。 图卜1m e l a r m 机器人系统 图卜2 是7 自由度直接驱动机械臂，具有拟人结构，目的用丁服装f 业缝补自动化作业，由m i t i 机械上程实验室1 9 8 7 年开发。 2 第一章绪论 图1 - 27 自由度直接驱动机械臂 图卜3 为由美国麻省理工学院研制的s a l i b u r y 手，有三个手指，每个手指三个自由度。 幽卜3s a l i b u r y 手 图1 - 4 是由日本明治人学研究的二指系统，具有二个4 自由度手指。 3 东南大学硕 。学位论文 图卜4 日本明治大学二指手 在国内，有多家研究机构从事冗余度机器人的研究一【= 作，南开大学机器人与信息自动化研究 所研制成功了多机器人控制系统，共1 3 个自由度，其中一个臂为p u m a 7 6 0 ，男一个臂为p u i a 5 6 2 ， 且在p u 姒5 6 2 底座上装有可直线返动的导轨，构成了具有冗余自由度的多机器人协调控制系统， 如图卜5 。 图卜5 南开人学多机器人系统 中国科学院沈r i 自动化研究所研制成功多机器人协调控制系统。其中一台机器人安装在具有视觉 的可移动小车上，构成较为复杂的多冗余度机器人系统。 4 第一章绪论 1 2运动规划的研究现状 冗余度机器人的运动学优化可分为局部优化和全局优化。局部优化是一种在线优化方法，具 有计算效率高、便丁实时控制等优点。但该方法只要求机器人提供当前信息，缺乏全局最优性， 还存在计算稳定性差及非保守性等缺陷。全局优化方法则采用积分形式的性能指标，具有能保证 全局最优性、计算稳定性好等优点，但全局优化方法需要多次对整个运动过稃进行计算，计算量大 不便于实时控制。 1 ) 序列规划 目前机器人序列规划方法的研究还处于发展阶段，由于各种作业任务的复杂性，至今还没有 通j j 的序列规划算法。在使_ l i j 机器人进行装配，插7 l 、点焊的麻用中，较为普遍的方法是把序列 规划问题转化为图论中的“旅行商问题”( t s p ) 来解决。文献“讨论了机器人从起始点开始， 经过一系列作业点，并在各作业点停留，完成所有作业后，最终同到起始点的时间最短规划算法。 文献”“中，作者首先研究了作业顺序无约束的序列规划问题，用机器人在两点之间的运动时间代 替两点之间的距离，把规划问题转化为典犁的旅行商问题来解决。对有作业顺序约束的序列规划 问题，不能直接应用t s p 算法，作者提出了两种修改办法，一是把不能相邻的作业点之间的运动时 间设置为无穷大，二是把有固定顺序的几个作业点看成一个作业点。通过上面两种办法，把带顺序 约束的规划问题转化为旅行商问题来解决。文献采用基于模拟退火算法和遗传算法的进化模 拟退火算法求解机器人竹业点序列优化的问题，获得了比单独采_ h j 任一方法更好的结果。旅行商 问题的求解成为求解序列规划问题的关键。目前，旅行商问题的近似最优解求解算法已经得到了 充分的研究，提出了很多可行的算法，包括启发式算法“”1 、模拟退火算法1 、遗传算法“”等。 其中启发式算法可以细分为环路扩充法和环路改进法，或是两种方法的综合，一般利h j 邻接矩阵 按照一定的规则变换来求解，计算草和求解结果都能满足要求。对于点数月很人的情况，可以先把 邻近的点分隔成一丛，在每一丛中寻找一条最短路径，再把所有的丛连成最短的同路。另外，文献 ”“还讨论了如何获取两作业点间运动时间的方法。 2 ) 路径规划 机器人路径规划的目标是在两个作业点之间生成一条无碰撞路径。机器人路径规划算法主要 有4 类：c 2 空间法，人i 势力场法，假设一修正法，预处理一规划算法。 c 2 空间法由s u d u p a 和t l o z a n o p e r e z 等人发展起来的一种无碰撞路径规划算法 ”6 ，其基本思想是把障碍物映射剑机器人的c 2 空间( 形位空间) ，此空间为机器人的c 2 障碍空 间，余卜的是c 2 自由空间，规划的目标就是要在c 2 自由空间生成一条包含起始点和日标点的路径。 c 2 空间法的难点是如何把障碍物映射到c 2 空间形成c 2 障碍空间，由于机器人在形位空间的自由度 通常超过5 个，这使得障碍物剑c 2 空间的映射相当复杂，技巧性强，计算鹫大( 计算复杂度是机器人 自由度个数的指数函数) 。 人l 势力场法机器人路行规划的重要方法”“”1 ，最初由k h a t i b 提出，其基本思想是引 入一个称作势力场的数值函数来描述机器人空间的儿何结构，通过搜索势力场的p 降方向来完成 运动规划。势力场分为两个部分，即日标位姿产生的引力场和障僻物产生的斥力场，引力场使机器 人向目标付姿靠近，斥力场使机器人绕开障碍物。人i + 势力场法的优点是计算颦较小，但利h j 人i ： 势力场进行路释规划的最人问题是：搜索过稃往往在势力场的局部极小点停止。文献“通过引入 虚拟障碍物使搜索过稃从局部极小点逃脱，但引入虚拟障碍物可能产生新的局部极小点，同时增 加了算法的复杂度。 5 东南人学硕j 学位论文 假设一修正法“通过“路径假设，路径检验，中间点生成”的过拌将运动规划问题分解为 若干子问题来求解。假定一修正法首先在起始点和目标点之间按照一定的方式生成一条路径，然后 对该路行进行碰撞检测，若没有碰撞，则路径规划结束，若有碰撞，则在起始点和目标点之间插入 一无碰撞中间点，然后再重复上面的过程。假设一修正法的大键在于中间点生成的方法，它决定了 该算法的求解效率，文献“给出了一种以机器人和各障碍物之间的距离作为启发性信息的中间 点生成方法，加快了求解速度，但这种中间点生成方法也会出现局部极小点问题。假设一修正法的 优点是算法简单，但假设一修正法每次都要对新生成的路径进行碰撞检测，计算鼋较大。 预处理一规划算法”1 ”1 分为预处理和规划两个独立的过程。预处理阶段，首先在机器人形 位空间廊川概率方法搜索一组无碰撞形位点，然后把这些形位点互联成一个无碰撞网络，网络中 的每条边都处丁机器人自由空间：在规划阶段，首先在预处理阶段生成的网络中寻找两个形位点a 和b ，使起始点利目标点分别与a ，b z 间存在一条无碰撞路径，然后在网络中寻找一条最短路径 把a 和b 连接起来，这样就得刽一条从起始点剑目标点的无碰撞路径。预处理一规划算法的优点在 于其规划过稃的算法简单，计算鼙较小，因为大量的数据处理在预处理阶段完成了，同时该算法和 其他方法相比通用性强，这一点已被实验应_ 【 j 所证明。该算法的难点是如何通过预处理过程得到 一个完整可靠的无碰撞网络，文献“应用概率论方法对此做了详细的讨论。 另外，遗传算法“、神经网络算法”“等智能算法已经成为机器人路行规划的研究热点和发 展方向，这些算法在实际中都有初步的应用，取得了较好的规划效果。 3 ) 碰撞检测 路径规划的目标是在起始点和目标点之间生成一条无碰撞路径，这使得碰撞检测成为解决路 径规划问题的芙键，上面提到的4 种路径规划算法都需要进行碰撞检测或需要计算机器人和障碍 物之间的距离。判断两个物体是否有碰撞发生有两种方式，一是定性判断，_ 二是定蟮计算。定性判 断方法最主要有“包罔盒层次法”“，其基本思路是用一个简单的包嗣盒将复杂的儿何形体围 住。当对两个物体进行碰撞检测时，首先检测两者的包围盒是否相交，若不相交，则说明两个物体 未相交，否则再进一步对两个物体做检测。冈为求包嗣盒的交比求物体的交简单很多，所以可以快 速排除很多不相交的物体，从而加速了算法。对丁复杂物体，可以将物体及其子部分的包罔盒组成 层次结构，如二义树。在各种机器人路径规划算法中，往往需要以机器人和障碍物之间的距离及其 关丁机器人形何的梯度作为路行规划的启发性信息，以加快规划速度，冈此在路径规划中不仅需 要判断有无碰撞发生，还要计算机器人与障碍物之间的距离及其关于机器人形位的梯度。目前， 两个刚性物体之间的距离计算有很多算法，其中对凸多面体的计算最为成熟，凸多面体之间的距 离有两类：分离距离和嵌入距离。目前，凸多面体间的距离范数士要有平移距离“、成k 距离 ”、收缩距离1 、伪平移距离”等。平移距离具有直接的儿何意义，但其嵌入距离的计算非 常困难。文献“定义的伪乎移距离和平移距离存在确定的上下界关系，从儿何意义上看，伪平移 距离是平移距离的近似。凸多面体间的伪平移距离的求解问题可以转化为线性规划问题，其计算 复杂性为d ( 所) ，所是两个凸多面体的顶点数之和。同时，伪平移距离关丁形位空间连续可微，可 以为路径规划提供启发性信息。文献”“定义的成长距离和平移距离也存在确定的上下界关系， 其求解过拌也可转化为线性规划问题，计算复杂度为0r ，同时，成长距离关于形位空间也连续 可微，但成长距离的合理初始点的选择比较凼难。在现实环境中，机器人及障僻物一般是凹儿何体， 可以采用分割的方法把凹几何体分削为多个凸几何体的组合。对一般凸儿何体，如球、圆柱等， 可以_ h j 紧包闱它的凸多面体来近似，然后j j 凸多面体间的距离近似原凸几何体之间的距离。文献 给出了一种计算简单规则凸几何体间距离的方法，但对复杂不规则凸几何体间距离的计算还 6 第一章绪论 有待进一步研究。 1 2本论文的主要研究工作 虽然国内的一些学校对冗余度机器人的研究已经起步，并且也取得了一些成绩，但是我校对 此方面的研究还较少。因此我们讨论认为要进行该方面的研究，还是应该从基本的做起，想要做 一个机械手运动规划的仿真器。所以本课题研究的对象是平面三自由度冗余机器人，要求该机械 手能在起始点和目标点之问生成一条无碰撞路径。为了达到上述的目标，本文主要进行的研究工 作如f ： ( 1 ) 对机器人手臂运动学正、逆问题进行了理论研究，并在此基础上推导出正、逆运动学的 计算公式； ( 2 ) 对路径规划理论进行研究，选择了人工势场算法；为了避免碰到障碍物，引入了伪最小 平移距离概念，井通过计算此距离来判断是否碰剑障碍物； ( 3 ) 基 - o p e ng l 接口，对平面三自由度冗余机器人、目标小球、障碍物以及周围环境进行 三维建模，要求建模程序具有开放性、通_ h | 性、模块化的特点。 1 3 本章小结 本章主要介绍了冗余度机器人的现状，同时概述性地介绍了运动规划的方法及方法的优缺 点；介绍了本课题的研究意义，分析了本课题研究的内容、任务等。 7 东南大学硕1 学位论文 第二章数学基础 物体在j = 作空间内的位置和机械手的位置都是以某个确定的坐标系来描述的；而j ：作任务则 是以某个中间坐标系来规定的，而在笛卡尔坐标系来描述r 作任务时必须把上述规定变换为一系 列能够由机械手驱动的关节位置。我们将为机械手的每一连杆建立一个坐标系，并_ i 齐次变换来 描述这些坐标系间的相对位置和姿态。通常把一个连杆与f 一连杆间相对关系的齐次变换叫做a 矩阵1 。 2 1位姿的描述 机器人机构结构复杂，其几何关系也相应比较复杂。即使对于一个已给定位置的机械手，其 取向也各不相同。因此在研究机器人的返动和操作时，不仅要描述其位置笑系，还要描述其取向 关系，即付置和姿态二个方面，机器人最常见的是心位姿向量来表示”“”+ ”1 。如图2 1 表示机器 人的一个夹手。把所描述的坐标系的原点置丁二夹手指尖的中心，此原点由位置矢颦p 表示。夹 手姿态可以用三个方向单位矢阜表示，其规定如下”“：z 向欠晕处于夹手进入物体的方向上，f ： 称之为接近矢龟口；y 向矢茸的方向从一个指尖指向另一个指尖，处于规定夹手方向上，称为方 向矢彗。最后一个欠窜叫做法线矢量n ，它与矢量。和口一起构成一个右手矢鼙集合，并由矢晕 的交乘规定行= o a 。其中矢颦。和口两者都是正交单位矢量，并且互相乖直。通过这三个姿态 参数，物体的姿态就被唯一确定了，加上位置矢量p 就可以明确地描述出物体某一时刻的空间 状态。 厂，。 。螃 j 彩 y o 图2 - i 手抓坐标系 s 第一二章数学华础 2 2 齐次变换 己知一直角坐标系中的某点坐标，那么该点在另一直角坐标系中的坐标可以通过齐次坐标变 换求得。 平移齐次坐标变换 空间某点由位置矢量a ，+ 6 ，+ q 描述。其中f ，k 为x ，y ，z 轴上的单位矢量此点可用 平移齐次交换表示为 t r a n s ( a ，6 ，c ) = lo 0l o o o o o口 0 b 1c 01 t r a n s ( a ，b ，c ) 为平移变换。 旋转齐次坐标变换 对应丁轴x ，y 或z 作转角为0 的旋转变换分别为 g o t ( x ，p ) = r o t ( y ，们= r o t ( z ，占) = lo 0c o s 0 0s i n 0 00 c o s 0 o s i n 0 o 0o s i n 00 c o s 00 o1 0s i n 00 1oo 0c o s 00 ool c o s o s i n 00 0 s i n 0c 0 s 00 0 0ol0 ooo1 ( 2 1 ) ( 2 2 ) ( 2 3 ) ( 2 4 ) 齐次坐标变换的几何意义 当坐标系f 经过一系列的平移、旋转变化之后。到达坐标系_ ，。如果坐标系，是机械手的末 端执行器的位姿，坐标系f 是其参考坐标系，则坐标系，相对丁- 坐标系f 的位置和姿态就相当丁二 机械手的末端执行器的位姿相对于其参考坐标系的位姿，如前所述_ h j 欠鼙聍，0 ，口，p 来表示。 ( 行，0 ，4 ) 分别表示末端执行器的坐标系j 的三个轴o x ，o y ，o z 在参考坐标系i 中的二个方向余 9 东南人学硕i 学位论文 弦，p 3 j 其在参考系f 中的位置欠量。因此，这两个坐标系之间的关系也可用下式描述： 式中以，p ，p ：为坐标系，的原点在坐标系i 中的坐标。 于是，上述变换用齐次坐标表达成 其中： t = x f y 。 z ， 1= 售 n lo la xp l n yo y a y p y n zo ：a z p z 0ool 上式中的r 显然是一他t 齐次矩阵。把r 分割为瞄p ，则子阵r = 转矩阵，表示坐标系间的姿态变换关系，子阵i = 1 为其比例因子。 2 3 机器人坐标系 ( 2 5 ) ( 2 6 ) 荔至o o ：a ； 对应于旋 疗， ，4 yl 对腿1 。厩 ，l ： ：j 机器人关髓连接前后两个连杆，关节分为移动副和转动副，它们都被驱动器控制，f 能度量 关节运动晕的大小。通常从机器人的基础起到手端，逐一分配坐标系，每个坐标系与其对应的关 耵同连在一起，随笑节一起动作。为了确定各连杆之间的相对运动和位姿关系，在每一个连秆上 同接一个坐标系。与基库髓杆0 同接的称为基坐标系，与连杆1 嘲接的称为坐标系 l ，与连杆i 阔接的坐标系称为坐标系 f 。l - 面讨论连杆坐标系规定的方法。 2 3 1 中问连杆f 的坐标系 坐标系 f 的z 轴z 与关节f 共线，指向任意规定。坐标系 f 的x 轴墨与连杆的公乖线重合 指向从关节f 到关| i 了i + 1 ；当a 。= 0 时，取置= 互。z j 。坐标系p 的y 轴与，：按右手法则确 定，即r = z ，墨。坐标系的原点q 取置与z j 的交点作为原点，当z ，和z ，+ ，相交时 ( a ，= 0 ) ，取其交点作为原点；当互和z l + 1 平行时，原点取在使z “= 0 的地方。 i o 只0 如 + + 乃乃乃 q q 吖 + + + 峨 + + + 一0 b = = = 乙 第一章数学皋础 图2 2 连杆坐标系 2 3 2 首段连杆和末段连杆 坐标系 o 称为基坐标系，与机器人基库同接，闶定不动，可作为参考系，用它来描述其他 连杆坐标系的伊置和方位。 基坐标系可以任意规定。但是为了简化起见，我们总是选择z 轴沿关节轴1 的方向- 并且， 当关节变管l 为。时，使 0 与m 重合。这种规定隐含：= 0 ，并且当关节l 是旋转关节时， d o = 0 ；当关节1 是移动副时，吼= 0 。 末段连杆( 连杆n ) 坐标系如 的规定与基坐标系相似。对丁旋转关节n ，选取j 0 使当b = 0 时，z 。与以一。重合，坐标系如 的原点选择使以= 0 ；对丁移动关节n ，如) 的选取使以= 0 ， 且当以= 0 时k 与石，l 重合。 2 3 3 用连卡丁坐标系规定连杆参数 利用连杆坐标系，则连杆参数可以明确地定义为： 口，= i a 互到互“沿z 测餐的距离； 口，= 从z 。到z f + 1 沿一旋转的角度； z = 从x 。到一沿z l 测茸的距离： 包= 从z - 1 到置沿互旋转的角度 通常选择口0 ，囡它代表连轩的长度；而口，d ，和只是可正可负的颦 i l 东南大学硕l 学位论文 上面所述有关连杆坐标系的规定，并不能保证坐标系的唯一性，例如，虽然z t 与关节轴i 一 致，但是z i 的指向有两种选择；并且，当z ，与z 相交时( a ，= 0 ) x 的方向是互和互“决 定的平面法线，j i 的指向也有两种选择；此外，对于移动关节，坐标系的规定也有一定的任意 性。 2 3 4 连杆坐标系建立的步骤 对给定的机器人，它的各个连杆坐标系建立的步骤如下： 1 找出并画山各个关节的轴线。 2 找出并画山相邻两轴线f 和f + 1 的公乖线口，或两轴线的交点。求出公垂线口，与轴线f 的 交点令这交点为坐标系 f 的原点d ，。 3 规定z 轴与关节f 轴重合。 4 规定x ，轴与公乖线口f 重合，若z ，与z i “相交，则规定置是z f 与z l + l 所形成平面的法 线。 5 按右手法则决定z = z i 置。 6 当第一个关节变量为。时，规定 o 与m 重合。对丁二末端坐标系伽 ，原点和x 。的方向 上可任意选取。但是，总是希望所选抒的坐标系伽) 使连杆参数尽可能为o 。 上面介绍了d - t t 规定各连杆坐标系和确定连杆参数的一般方法，在此基础上，就可以导出机 器人运动方程。 2 4机器人的运动学正问题 机器入的正问题是，给定机器人的结构参数及运动关竹的运动参数，确定机器人末端执行器 在参考坐标系( 一般为机库坐标系) 中所处的位置霸i 姿态。正向运动学士要解决机器人运动学方程 的建立及手部位姿的求解问题。 1 2 第一二章数学草础 瓦= a l a 2 a 3 a 4 a 5 a 6 c 1o j l0 1 o l c _ s 2o c ；i | 亨三悸i 号；悸i 瓦为6 r 机器人的末端执行器相对于机库坐标系的位姿矩阵，令 t = 以0 ， 聍yd y 栉：0 ： o0矧 l 胛，= ( ( c l c 2 3 c 4 + s l s 4 ) c 5 一c l s 2 3 s 5 ) c 6 + ( - c l c 2 3 s 4 + s l c 4 ) s 6 ； 胛y = ( ( s l c 2 3 c 4 一c l s 4 ) c 5 一s l s 2 3 s 5 ) c 6 + ( 一s l c 2 3 s 4 一c l c 4 ) s 6 ； l胛，= ( s 2 3 c 4 c 5 一c 2 3 s 5 ) c 6 一s 2 3 s 4 s 6 ； 仁px = ( ( c l c 2 3 c 4 + s l s 4 ) s 5 + c l s 2 3 c 5 ) a 6 + c l s 2 3 a 4 + c l a 2 c 2 ； = ( ( s l c 2 3 c 4 一c l s 4 ) s 5 + s l s 2 3 c 5 ) a 6 + s l s 2 3 a 4 + s l a 2 c 2 ； p ：= ( s 2 3 c 4 s 5 - c 2 3 c 5 ) a 6 一c 2 3 a 4 + a 2 s 2 + a l ； 1 其中，c l = c o s o = ，s l = s i n o , ，c 2 3 = c o s ( 0 2 + 岛) ，s 2 3 = s i n ( 0 2 + 岛) 下同。 ( 2 7 ) ( 2 8 ) 2 5机器人的逆运动学 上面我们说明了正向求解问题。即给出关竹变晕口，求出手部位姿各欠鼙玎，0 ，a 和p ， 这种求解方法只需将天前变量代入运动学方稃中即可得出。但是在机器人控制中，问题往往相反， 1 3 o o k 1 好叫o o o o ，o 4 “ 老嚣：殳脚心 敬渤叱 巧巧拐为 竺媳 巧西以“舛弘订d 如 麓一 出m e k ， 东南人学硕i 学位论文 即在己知手部要剑达的目标位姿的情况h 如何求出关竹变鼍，以驱动各关节的马达，使手部的位 姿得剑满足。 给定6 r 机器人的位置参数j k ，p 。，见和姿态参数( 用末端相对于底_ 唪坐标系的姿态角 ( 矿，占，伊) ) ，据口= f 。 ) 式可求出六个关节变颦b ，岛，b ，以，b ，吼的值，这就是求 运动学逆解的主要内容。 2 6 三自由度平面冗余度机械臂的运动学分析 图2 3 所示为一三自由度平面冗余度机械臂，其连杆参数如表2 1 所示。由于此机械臂共有三 个转动关节，结构特点是三个关节的轴线是互相平行的。固定坐标系 0 和迩杆1 、连杆2 、连杆3 的坐标系 1 、 2 、 3 ，位于在关肖1 、关节2 、关节3 和末端中心。坐标系 3 也就是末端坐标系。 连杆参数中0 为变量，其余参数d ，a ，口均为常蠼。 图2 3 三自由度平面冗余度机械手 表2 - 1 三自由度平面冗余度机械臂 连杆转角 两连杆之间的 连杆长度口 连杆 关节运动范围 序蟹( 变量) 口距离d 扭角口 l bd i = 0q = 厶= 1 5 0 0 1 2 0 。一1 2 0 。 2 吼 d ，= 0 口2 = l 2 = 1 5 0 0 1 2 0 。一1 2 0 3 岛 d 3 = 0口3 = l 3 = 1 4 0 o 一1 2 0 。一1 2 0 。 该平面关节犁机器人的运动方群为： 五= a l a 2 a 3 1 4 ( 2 9 ) 第一二章数学幕础 2 相对丁连杆l 坐标系 1 的齐次变换矩阵；a 3 表示连杆3 的坐标系 3 相对于连杆2 坐标系 2 的 齐次变换矩阵；于是有： 即：a i = a 1 = r o t ( z o ，只) t r a n s ( l l ，0 , 0 ) 4 = r o t ( z i ，岛) t r a n s ( l 2 ，0 ，0 ) a 3 = r o t ( z 2 ，岛) t r a n a ( 厶，0 ，0 ) = 降 = 陪 斗 c o , j 只 0 o 置和姿态。 ( 2 1 0 ) 0 2 ) ；2 = s i n ( o l + 岛) ； 槲o ， o o l 0 硼嵋o o o o l o o l o 0 1 o o o o o o l o o l o 码饵o o b 最o o 鹕警。 鸠幔o o o l o o 丌o o o o i i i o 儿 鸽幔o 。 包岛0 o k 一 幔哦o。 l o o l o 鸺咀o o 叩难 鸭码o o 1lllj + f j a “ 呶 确幽o l i + + q 1，j 以乃儿 m b 位 ，：， 撇 q q 吒o o o 。o 野 神 q q e o b 孑。m 蛳一限h m一 一 c 埘 = 一 。 咖 端 = ， 。， v j 沫 五 铴畅o o 际 r 一 表 ，3 ) t 、 瓦 卜 澡 “ 铂 = 曼 岛 ! 蓍 晰 “ 一 乘 卿 铲 罅 旷 坼 中 = “ _ 1 东南大学顾l 学位论文 于是写出末端仿置阵列：p = p ， p r p ： 0 表示末端姿态的方向行，o ，a 分别为： 阵= 降 毛c 1 2 3 + 屯c 1 2 + ，l q 毛s 1 2 3 + 1 2 s 1 2 + ，i s l o 1 o = o 。 o y 0 2 o 一, s 1 2 3 c 1 2 3 0 o a = 口j a y a ： o 当转角变龟q 、岛、岛一定时，可以算出具体数值。如图4 - 2 所示，设只= 2 0 0 0 2 = 3 5 0 ，以= 1 5 0 ，则可以根据平面关节犁机器人运动学方程式( 2 1 0 ) 求出运动学正解 即末端的位姿矩阵表达式为： 五= 0 3 4 2 0 0 9 3 9 7 o o 一0 9 3 9 7 0 3 4 2 0 o o 03 6 8 8 4 2 5 03 3 9 9 3 4 8 10 o1 而反过来，已知三自由度平面冗余度机械臂的末端位姿，求解转角变量q ，岛、岛。令m 为 i ：作空问维数，x r 为机器人末端位姿向鼙，p 胄”表示机器人关节空间的广义坐标列阵， 则机器人的运动方稃可表示为： x = 厂( 咖 ( 2 1 1 ) 上式对时问求导可得机器人直角坐标空间和关节空间速度之间的关系式为： 孟= ，( 妒) 驴 ( 2 1 2 ) 其中，= 影乞r ”是冗余度机器人的雅克比( j a c o b i a n ) 矩阵。在给定末端速度j 及关霄 角初值之后，通过上式可求得机器人关节速度矿。若册 o ( 3 1 ) 对于任意的j ，y r “及口0 ，规函数儿具有以f 性质：( i ) 以( 力0 ；( i i ) r a y ) = o 当且仅当y = o ：( i i i ) 以( ，) = a 7 ，( y ) ；( i v ) 九( y + j ，) 儿( y ) + 心( y ) a 此外，如果c 关 于坐标原点对称，即c = 一c ，则还有：( v ) 儿( - y ) = y a y ) 。由上述性质表明以( _ y ) 为r ”上的 一个范数。若集合c 不满足对称条件c = - c 。则几可视为r ”上的一种伪范数。由+ 卜式定义的集 合b c ( ，) 规函数儿意义f 半径为，的闭球，简称为半径为r 的，一闭球 e ( ，) = ，f 以( y ) ， 容易证明，眈p ) = r c 对于任何的r 0 成立。 ( 3 2 ) 定义1 “”假设g 和h 为r ”中的有界闭凸多面体，c r ”为满足条什0 i n t c 的紧凸集，规函 数心意义卜c 和h 之间的伪最小平移距离定义如下： 椰端鉴五荔勰 o g 一日 ( 3 3 ) 0 芒g h ( 3 3 ) 式具有如f 的直观儿何解释：若0 ( g 一日) ，则一r a g ，日) 为集合g h 的内接y 。一 闭球半径；若0 仨( g 一日) ，则扎( g ，日) 为与集合h g 相交的儿一闭球的最小半径，如图2 1 所 示。 需要指出的是，除非c 为对称集合，一般有r a g ，日) r a h ，g ) 。此定义表明伪最小平移距 离是在规函数意义卜的，这一点必须明白。 2 0 第三章机器人运动规划1 j 避障l 仁 以g 肋c 7 一 e o j 一 、，厂。 乙 oj ￥赆蹦一 。 彻 图3 1 伪最小平移距离的几何解释 ( 8 ) 0 g 一日( b ) 0 芒g 一日 当集合c 的选择不同时，可得到不同的伪距离函数y 。( g ，h ) 。至此，已经在规函数下定义了 伪距离函数，但是仍然要找剑距离函数与伪距离函数之间的关系。设1 | i l 为r ”上的一个范数， 爿r ，占er ，则称i 陋一纠i 为a 与b 之间的距离，而对于范数的定义义分为三种，如2 一范数 ( e u c l i d 一范数) ，l - 一范数，l - 范数。若c 为r ”中e u c l i d 单位闭球、l 1 或k 单位c j 】球，则r a g ，h ) 分别为e u c l i d 度晕、厶或l 度晕意义下g 和日之间的最小平移距离。以卜的性质是具有一般性 的，它们不依赖丁集合c 的选择。 定理l 以b ( r ) 表示半径为，且球心位丁坐标原点的e u c l i d 闭球。 i a a , ( c ) = m a x ，f b ( r ) c ，a o ( 0 = m i n r l c 曰p ) ，o i ( c ) 和d i ( c ) 分别称为集合c 内、外 半径对丁任意有界闭凸多面体g ，h r ”恒有如f 不等式成立： q ( c ) k ( g ，h ) i - i r ( a ，h ) i 仃o f c ) l r , ( o ，日m ( 3 4 ) 其中r ( g ，h ) 是j e u c l i d 度苗意义下g 和之间的最小平移距离。通过定理l ”表明一般情况下， 可以将最小平移距离转化为求伪最小平移距离，事实上，伪最小平移距离并不是真正的最小平移距 离，而是对最小平移距离的一个上卜界的估计。几何意义如图2 2 所示。 东南人学硕l 学位论文 l 存贰 怂缈。：r 集台c 图3 2 伪最小平移距离的几何意义 若以a o ( c ) “乘集合c ，并记口( c ) 为c 的内、外半径之比，即，则不等式( 3 4 ) 可写成如f 的标准形式： a ( c ) l r a a ，h ) i - 1 0 下函数一，的极小值，则可得到挣。个具 有如卜形式的l p 问题； 东南人学颀l 学位论文 九= m i n ( 一，) _ fm q = 厉= l ， i = 1 j = 1 q ，尼，“0 ；i = l ，m g ，= 1 ，m h ( 3 1 0 ) 解上述的( l p ) 问题，查看了很多的文献发现，虽然单纯形法的计算在理论上具有指数复杂性，但 是在实际问题中仍然是最有效的方法之一。故本论文中仍采用单纯形法来计算伪最小平移距离。 3 3 3 伪平移距离的可微性及导数