六足机器人的运动分析及路径规划.docx

《六足机器人的运动分析及路径规划.docx》由会员分享，可在线阅读，更多相关《六足机器人的运动分析及路径规划.docx（25页珍藏版）》请在课桌文档上搜索。

1、五邑高校毕业设计说明书毕业设计题目：六足机器人的运动分析及路径规划庞系机电工穆学龙专业机械工程及自动化学号APOBo8340学生姓名诗焕城学生电话一指导老师李昌明副我校完成日期2012年5月20日六足步行机器人机动性强，适应实力高，能代普多种机器人完成工作，其探讨具有全要的科学意义和实际应用价值。本文针对六足步行机器人的机体设计、步态规划、运动学分析、足端轨迹规划中的空间插值方法及避障路径规划算法等理论和技术问题，开展了较为系统的探讨工作。首先,对六足昆虫进行机械建模，确定选用椭圆形身体布局后，进一步对六足步行机器人在三角形步态卜的爬行植定性进行具体地分析；然后，求解机器人步行是运动学的正逆解

2、问题，利用求解结果协助规划机器人的足端轨迹。MAT1.AB的分析仿真发觉，在六次多项式函数的足端轨迹曲线下，步行足具有较好的运动特性：最终,先简洁介绍了人工势场和蚁群算法，再合理地对两种算法进行了芍效地酷合与改进，扬长避短，得到了一种更高效智能的路径轨迹规划算法。MAT1.AB的仿典试验结果证明白该匏法的有效性。关键词：六足步行机器人：步态规划：运动学：轨迹规划：人工势场：蚁科算法AbstractDuetothegreatmobi1.ityandadaptabi1.ityofhexapodwa1.kingrobot,andtheirhighperformancesinvariousroboti

3、ctasks,theresearchonitisofmomentousscientificsignificanceandpractica1.app1.icationva1.ue.Thisthesisaddressesbodydesign,gaitp1.anningandkinematicsana1.ysis,po1.ynomia1.interpo1.ationneth,.MAT1.ABisusedtodothissimu1.ation.Theso1.utionshowsthatswinging1.egpossessestheexce1.1.entkineticcharacteristicund

4、erthesix-orderPO1.yno1.nina1.functioncurve.Fina1.1.y,abriefdescriptionofartit1.cia1.potentia1.fie1.dInethod(PFM)andantco1.onya1.goriIhm(ACO)exposestheimperfectionofthem.Anewa1.gorithmisproposedbycombiningPFMwithACOeffective1.y.Simu1.ationresu1.tstestifytheva1.idityofthisHICIhOdforrobotpathp1.anning.

5、Keywords：Hexapodwa1.kingrobotGaitp1.anningKinematicsTrajeCtoryp1.anningArtificia1.potentia1.fie1.dAntco1.onya1.gorithm书目摘要AbstractII第1章结论I1.1 课题的来源及探讨的目的与遨义I1.2 文献综述1国外仿生多足机器人探讨概况1国内仿生多足机器人探讨概况41.3 本课题探讨的主要内容51.4 本章小结5第2章仿生六足机耕人机构建模52.1 仿生六足机器人机构模型62.2 基于螺旋理论的机构自由度分析62.3 机器人机体结构72.4 本章小结8第3章六足机器人静态步

6、态规划分析93.1 步态的相关概念93.2 六足机器人的步态分析IO3.3 三角形步态1()三角形步态的稔定性分析I1.六足机器人的步长设计12六足机器人若地点的优化133.4 本章小结14第4章六足机器人的运动学分析144.1 D-H变换154.2 步行足坐标系的建立154.3 运动学正解164.4 运动学逆解174.5 基于微分变换法的雅可比矩阵184.6 本章小结19第5章机器人的足端轨迹规划2()5.1 步行足的摇摆轨迹分析205.2 步行足的摇投轨迹生成215.3 足端轨迹仿真分析235.4 本章小结26第6章六足机器人避障路径轨迹规划266.1 人工势场法路径规划26人工势场法原理

7、26受力分析286.2 蚁群算法路径规划29蚁群算法原理29基本蚁群算法的数学模型306.3 势场和蚁群算法结合与改进31启发信息为的构造32期望启发式因子P的改进326.4 算法步.骤336.5 基F势场蚁群算法路径规划的仿真实现336.6 本堂小结35结论35参考文献36致谢39附录A运动分析的相关程序401计算两组支撑三角形最大重强面积402转换矩阵生成程序403足端轨迹的生成和计算程序414求运动逆解问题44附录B路径规划的相关程序44I路径规划的主程序442计算引力、斥力与X轴的角度453计算引力大小464计算斥力大小465计算合力在在八个可行方向上的至量476计算由合力引起的启发信

8、息497地图生成程序518势场蚊群算法程序52第1章绪论1.1 it题的来源及探讨的目的与意义机器人自问世以来,伴随着电子计算机的发展，整合多科学领域里的新成果，己经成为种现代科学技术的典型产物，在工业、农业、消遣、军事等行业中均扮演着举足轻重的加色。当今，随着科学迅猛发展，人类探究探讨范围渐渐扩展到一些人类无法到达或可能危及生命的特别场合，例如外星球表面、核反应堆、战场、消防及营救等。面对这艰难的挑战，寻求一条解决问题的可行途径已是科学技术发展和人类社会进步的燃眉之急的任芬。地形不规则或难以预料是这些环境的共同特点。从而使轮式机器人和履带式机器人的应用受到肯定的限制“，以往的探讨表明轮式机器

9、人的结构相对也较简洁，在相对平坦的地形上行驶时具有运动速度快速、平稳和限制荷洁的优点。但是由于轮式机器人运动时须要连续的地面支撵，在不平坦或松软的地形上行驶时，能耗大大增加，运动性能也极速降低有时甚至完全丢失移动实力。履带式机器人虽然支掠面积大.牵引附着性能优越，在松软或泥泞的场地行驶时的适应性比轮式有较大的改善，但是在不平地面上行驶时机身晃动严峻：，机动性仍旧差强人意。而多足步行机器人可以利用独立的地面支撵，即使是一系列孤立的点也可以成为它的运动轨迹，所以，与轮式、履带式移动机器人相比，多足步行机器人面对困难的非结构环境时适应性强和敏徒性高，可以代替人类完成很多危急的作业，具有广泛的应用前景

10、。近年来，得益于仿生科学的进步，仿生多足步行机器人如雨后春笋般快速地发展起来，现在已经成为当前各国科学家开发探讨的重点课题之一。1.2 文献综述1.2.1 国外仿生多足机器人探讨柢况自从第一次工业革命以后，随着机械学的不断成熟，机器人渐渐地从人类的幻想进入实现生活中。国外仃据可查的记载是1893年Rygg设计的机械马。此后步行机器人历经了一百多年的发展，取得了长足的进步。在20世纪60年头，美国的Shig1.cy和Ba1.dwin都运用的四轮连杆机构设计出比轮式车和履带式车更为敏捷的四足步行机Wa1.kingTruck,如图1-1所示，被视为现代多足步行机器人发展史上的个里程碑I1.但由于受到

11、当时技术水平及限制技术的限制，步行机不仅效率低适应性差，而且依靠于人的手脚对液压伺服系统的限制来操纵推个步行机的运动。从步态规划的角度若，这种步行机只能兑是人操作的机械移动装置一在上世纪70年头末以来，随着电子技术及计算机技术的快速发展，多足步行机器人超越了单纯依靠机构限制模式而实现了基于电子计算机技术的限制。其中以美国的McGhce与Frank研制的四足步行机器人”PhonyPony”最具典里和开创性。该机器人具有较好的步态运动稔定性，但由于其关节是由逻辑电路组成的状态机限制的，因此机器人的行为受到限制，只能呈现固定的运动形式.随后他们又研制出具有自主避解功能的六足步行机器人“OSU”，这种

12、机器人每条腿具有三个自由度，敏捷性高。此外，具有代表性的机渊人还包括1983年美国研制的六足步行机器人ODEX-I”，如图1-2所示，其6条腿沿圆周方向布置.，每条腿上有三个自由度，适于在狭小空间运动，可以上下台阶.1990年卡内基美隆而校的Whinaker等人研制的用丁外星探测的六足步行机器人AMB1.ER”，图1-3,该机器人机构的特点是躯体分成两部分，腿可从中间穿过，从而使得后部的腿可迈到前部，地面适应实力增加。1993年该高校又开发出用于火山考察的八足步行机器人DANTE”,图1-4是其改进型“DANTE-II”,在对斯伯火山的考察中得到r实际应用，传回r很多宝货的数据和图像网.图1-

13、3六足机零人AMB1.ER图M八足机器人DANTEJI”从美国引进机版人技术后，日本很快成为这方面的强国。自20世纪8()年头起先，E1.本东京工业高校的ShigcoHirose教授领导的成验室，胜利地开发出8代的T1.TAN系列四足步行机器人。其中，TITAN-HI”的足由形态记忆合金组成，装有传感器和信号处理系统，可以自动检测与地面接触的状态.姿态传感器和姿态限制系统依据传盛信息作出限制决策，实现在不平地而的自适应静态步行。1994年研制的“T1TAN-V11”作为移动的平台，能够敏捷自由地在坎坷和陡峭的地方步行。图1.5是最新研制的四足机器人TITAN-Vin”，它具有高度的地面自适应实

14、力，“TITAN-VIH”的腿能够成为有力的工作野，用探测地雷和进行排雷操作。图1-5四足机器人*11TAN-V1I1自本世纪以来，仿生学、材料学及计算机信息学等学科的发展和交叉淞合，把多足步行机器人的探讨推向新的高潮。这一时期，具有多功能性和自主性对机器人技术进入到新的发展阶段。图1-6所示的是2002年印度研制了六足行走式机器人“舞王”，其基座为一六角形的底盘，装有6条长腿，由18个轴限制，分别安装在底盘的6个角上,在18台电子发动机的惜动下，每条腿都转动，形似个巨大的蜘蛛。此外.“舞王的基座上还装有用于限制和监视的电脑。据报道称，“舞王”在一台无线台式电脑的遥控下，不仅可以行走、转弯，还

15、可以爬梯子、翻越45厘米高的障碍物。又如美国斯坦福高校于2(X)6年研发的仿壁虎机器人S1.ickyboi，如图1-7。该机器人从吸附原理、运动形式、结构外形上都比较接近真实的壁虎,可以轻松地在堵避上爬行。图六足机涔人“舞王”图1-7仿壁虎机器人uMiCkybm1.2.2 国内仿生多足机器人探讨梃况我国对多足步行机器人的探讨是从20世纪80年头起先的，经过三十多年的努力，在探讨和应用方面已经取得比较好的成果.中国科学院长春光学精密机械探讨所、中国科学院沈阳自动化探讨所、清华高校、北京航空航天高校、上海交通高校等单位和院校都先后开展了对多足步行机器人技术的探讨。其中比较有代表性的有上海交通高校研

16、制的小型六足仿朝气器人，长30mm、宽4()nm,高20mm,脑量6.3g,步行速度达到3mnVs网。该学校还研制一种仿照乳动物的关节式“JTUWM”系列四足步行机器人，它能以对角线步态行走。其足底安装了PVDF测力传感器，上位机利用模糊神经网络系统对力反馈信息进行处理，调整步行参数，提高了步行的稳定性。2000年，他们双开发了一种形态记忆合金(SMA)驱动的微型六足步行机器人，如图1-8所示，后来时SMA驱动的做型六足步行机器人进行改进，创新设计了双三足机器人的身体，专动关节，并首次提出组合偏动SMA驱动器，使机版人的刚性躯体柔性化从而实现了做里双三足步行机器人的全方位运动。此外，还有中国科

17、学院沈阳自动化所胜利研制了水下全方位六足步行机器人1.R-I；清华高校开发了“DTWM”框架式双二:.足步行机器人、五足爬杆机泯人：哈尔滨工程高校研制开发的仿朝气器猿：中国科学院长春光学精密机械探讨所研制了“4+2”多足步行机器人和图1-9所示的MiniQUad可重构多足步行机罂人削等。与国外相比，我国的步行机器人技术的探讨进展缓慢，各方面技术还不成熟，大多数探讨开发工作只停留在试验室中,图&3自然界中的蚂蚁觅食模拟图6-3中，设A点是蚁巢，D点是食物源，EF为一障碍由于障碍物的存在，蛆蚊只能由A经E或F到达D或由D到达A,各点之间的距离如图6.3a所示。假设每个时间单位有30只蚂蚊由A到达D

18、点，有30只蚂蚁由于D到达A点,蚂蚊过后留卜.的信息为1。为了便利起见，商该物质停留时间为I.在初始时刻，由于路径BF、FC、BE.EC上均无信息存在,位于A和D的蚂蚁可以随机选择路径，从统计学的角度只可以认为蚂蚁以相同的概率选择BF、FC.BE、EC,如图6.3b所示。经过个时间单位后，在路径BFC上的信息量是路径BEC上信息的2倍。又经过一段时间，将20只蚂蚁由B、F和C点到达D,如图6.3c所示。随着时间的推移,蚂蚊将会越来越大的概率选择路径BFC,最终将会完全选择路径BFC.从而找到由蚁巢到食物源的最短路径1.1 .2基本蚊群真法的数学模型为了阐述蚂蚁系统的数学模型，苜先引入经典的TS

19、P问题（旅行商问题）。TSP问题是指给定n个城市以及两两城市之间的距离，要求确定一条经过各个城市当且仅当一次的最短路途。其图论描述为：给定图G=（V,A）,其中V为顶点集，A为各个顶点相互连接组成的边集，己知各个顶点之间的连接距离，要求确定一条长度最短的Hami1.ton回路，即遍历全部顶点当且仅当一次的最短回路。引入以下记号来模拟实际蚂蚊的行为：m：蚁群中蚂蚁数量：b,：t时刻位于城市i的蚂蚁个数:d“：两城市i和j之间的距离：t:启发函数，即路径（1.j）的能见度，反映由城市i转移到城市j的启发程度,在TSP问题中一般取%=d,0,这个量在蚂蚁系统的运行中不变更：rij:路径（i，j）上的

20、信息素轨迹强度：ri1.:蚂蚊k在边（i,j）上留卜的单位长度轨迹信息素量：K：蚂蚁k在城市i选择城市j的转移概率。注:以上符号中的i和j都是城市序号,且i,j=1.n.k是蚂蚁序号，且k=1.m.下同。初始时刻，各条路径上的信息素量相等，设小O）=C（C为常数）。蚂蚁k在运动过程中依据各条路径的能见度和路径上的信息素量确定转移方向。蚂蚁系统所运用的状态转移规则称为随机比例规则,它给出fI时刻位于城市i的蚂蚁k选择移动到城市j的概率3）：耳=W宿S琴成jGa1.1.owedk(6-16)0.otherwise式中，为信息启发式因子，表示轨迹的相对重要性，反映了蚂蚁在运动过程中所1.2 .1启发

21、信息人的构造在路径搜寻中.蚂蚊找寻卜.一位置的启发信息由两部分构成。一部分是蚂蚁受到环境中的势场合力，形成使蚂蚁趋向于沿合力方向行走的启发信息，定义该部分启发信息为网（6-20）式中，A为大于零的常数，FS可由式（6-14）求出，0可由式（6求出。明显，在该启发信息作用卜.，蚂蚁电向于选择与势场合力方向夹角9较小的边行走到F-自由位.置。该启发信息有助于避开机器人与障碍物的碰撞，选择接近目标位理的合适路径行走1另一部分启发信息是由蚂蚁距目标位置的即离供应的。该部分启发信息定义如下：(6-21)式中，PJG表示机器人的姿态位置到目标的距离.由式（6-20）和式（6-21）,构造整个算法的启发信息

22、1.(6-22)由上式（a22）可以看出，当势场力的合力为零，即Fp,=0时，引力与斥力平衡，机器人处于势场的局部最小点处，如单独采纳人工势场法。将陷入局部最优陷阱。而结合蚁群算法，依据式（6-22）,这时启发信息为%*,即在旦m=。的状况卜.，%的启发信息将引导机器人跳出该局部最优陷阱，接着全扁最优路径搜寻。1.3 .2期望启发式因子B的改进随着迭代次数的增加，启发信息为中的信息也渐渐地传播到轨迹信息素由中，%在路径搜寻中起的作用渐渐削减。因为环境的不确定性，我们构造的启发信息,往往存在着很多不合理的因素。假如不对这些不合理的信息进行处理，那么这些不合理的信息将会被固化到轨迹信息素F中，导致

23、轨迹信息素的恶化，从而使算法无法搜寻到全局最优解.通过多次的仿真实践，笔者将B为期望启发式因子进行r改进.改进如下：A=（I-/）氏（6-23）式中，K为总共的迭代次数，k表示第k次迭代。由上式可以看出，随着迭代次数的增加，代表启发信息重量性的B值渐渐削减。当B趋向于。时，妈蚁在搜寻过程中几乎不受启发信息灯的影响，从而避开了不合理信息对路径搜寻的恶化。1.4 算法步舞以上具体阐述了算法的具体设计过程，这里对本文所提出的机器人路径规划算法具体步骤做简洁的总结：步1：初始化各参数。包括建立棚格的单位长度：人工势场的基本参数，如障碍物斥力的影响半径，引力系数、斥力系数等：蚁群搜寻的蚂蚁数量、迭代次数

24、、信息素挥发系数等参数：步骤2：采纳栅格法对机器人的运动环境进行离散化.确定各栅格位置坐标，确定机器人起始栅格、目标所在栅格以及障碍物所在的栅格：步，3：计算环境中各自由位置的势场合力大小与方向，各可行路径与合力方向的夹角,自由位置到目标的距离等;步骤4：分别将m只蚂蚁放在起始栅格上，蛆蚁正在式（6-22）定义的启发信息下，按式（仪16）选择一条允许的边到达下个栅格，并将该栅格位置存储在蚂蚁k建立的路径表中，直到当代全部蚂蚊搜寻停止后，找出当前搜寻到的最短可行路径1.h并按式（6-17）对走过的边进行信息素更新。步1.5：推断算法是否满足停止条件，若满足,则计算停止，并输出最短可行路径,最短路

25、径长度等，否则，转到步骤4,接着迭代计算。1.5 基于势场蚁群算法路径规划的仿真实现依据上面的兑法步骤，在MAT1.AB环境中编制了栅格地图下机器人避障和路径规划仿真程序。创建栅格地图时,为了使程序的应用更具广泛性，对于机罂人的环境能更接近真实性,地图可以依据实际环境进行更改,无障碍物处记为“0”，有障碍物处记为“I”，地图的编号规则为：从左到右，从上到下。图&4所示为两个栅格地图创建矩阵。000000000000000000001101III1100I1I11Ii100000000000O10000001001IIIII01IOI01II10I00000I0I0000I0IO0I010I11

26、010111O0101()010100000I01000()1011010100000I0I00OOI000010I001I1I0I)1O0I)1I1I0I000000000000000000010b图64栅格地图如阵调用附录B-7的程序生成障碍物地图如下（每个栅格的边长为I的正方形）图6-5障码物地图障碍物地图6-5a由栅格地图矩阵&4a生成：障碍物地图6-5b由栅格地图矩阵6.4b生成,黑色代表障碍区域，白色代表为可行自由区域。设置初始值。障碍物地图6.5a设起始点S为编号1,目标点E为编号46；障碍物地图6-5b设起始点S为编号100,目标点E为编号45。引力增益系数4=2：斥力增益系数

27、=2；障碍物最大影响距高4=1.05；迭代次数k=50:蚂蚊个数m=45:信息启发式因子u=1.：期里后发式因子B=|：信息去蒸发系数p=0I5:信息素强度Q=50；常数.=2o运行附录B-I程序生.成迭代的收敛图6-6和蚂蚊的爬行路途图6-7b图66迭代收敛图图6-7蛆坟的於行路途从图66可以看出，地图a也许经过40次的迭代后，最短路径收敛于22-8995,最短路径如图&7a所示；地图b也许经过45次的迭代后，最短路径收敛于52-7279,最短路径如图6-7b所示.通过上面两个实例可以看出本文所提出的机器人路径规划算法能胜利地找出最优的爬行路径,具有优越的性能。1.6 本章小结本章是课题探讨

28、的重:点之一，首先通过简洁介绍路径规划的势场法和蚁群党法，引出它们各自的优缺点，然后对两个算法进行了有机的结合和改进，得到一个更育效智能的算法，最终通过仿真验证了此算法的可行性，得出了让人满足的结果。由于机器人学交叉众多学科，对机器人探讨始终是探讨领域的热点和难点之一。本课题以仿生六足机器人为探讨对象，简洁介绍了机器人运动分析的方法，并融合了人工势场法和蚊群算法优点，改进了机器人避障路径规划中的算法。本文取得的主要探讨成果有:（1）叙述了课题的探讨背景和现实意义，介绍了多足仿朝气器人目前的探讨状况，在此基础上提出了本课题所探讨的主要内容。（2）仿照自然界中的六足昆虫对机器人进行建模，通过比较和

29、分析椭圆机体和长方形机体下的机器人的运动性能,得出：无论是在爬行稳定性还是敏槌性方面，椭圆机体都比长方形机体略胜一筹。（3）确定机体结构后，进一步对该机体下的六足机器人爬行稳定性分析，在这过程中，提出r三角步态下的若地点优化选择的方法并得出一个结论：机器人在前进的过程中摇摆脚落地时,假如能使两组支掾三角形在横向重卷2/3,脚的转换敏捷性最高。（4）在机器人能稳定爬行的前提下，用矩阵变换法求解六足机器人的正运动，学报,2010,46(13)29沈大柱.仿然八足机器人轨迹规划及限制D.华东理工高校硕士论文,201130周阑，周欣荣.机器人稳态分析的一种新方法J.东北林业高校学报,2007,35(8

30、)31隹恩璋,陈美宏.6R串联机器人雅可比矩阵求解和速度仿真J.机床与液压，2010,38(9)32金雷泽，杜振军，贾凯基于势场法的移动机器人路径规划仿真探讨J.计算机工程与应用，2007.43(24)33况菲.移动机涔人二维空间路径规划方法探讨U工湖南高校硕士论文，200634段海滨.蚁群算法原理及其应用M.科学出版社，2005:1-33135 MarcoDorigoThomasStUtz1.e.ntCo1.onyOptimization(M.胡晓敏,罗旭瘠等.清华高校出版社,2007:1-11536罗镌林,吴顺祥,基于势场蚊群鸵法的机器人路径规划J.系统工程与电子技术,2010,32(6)

31、37朱庆保.困难环境下的机器人路径规划蚂蚁算:法J.自动化学报,2006,32(4)38樊晓平，罗熊，易晟,张航.困难环境卜.基于蚁群优化算法的机器人路径规划U.限制与决策,2004,19(2)39张建英,赵志萍,刘瞰.基于人工势场法的机器人路径规划1.J.哈尔滨工业高校学报,2006,38(8)40 1.ucaMariaGambardc1.1.a,MarcoDorigo.So1.vingSymmetricandAsymmetricTSPsbyntCo1.oniesC.PrOCeedingSotheIEEEInternationa1.ConferenceonEvoIutionaryComput

32、ation,1996:622-62741 ThomasStutz1.e1Ho1.gerHoos.TheMax-Minantsystemand1.oca1.searchforthetrave1.ingsa1.esmanprob1.emC.ProceedingsofIEEEInterna1.iona1.ConferenceonEvo1.utionaryComputation,1997:309-31442 I；玮.修正蚁群算法及其在不同环境表达下机器人路径规划性能D.天津高校硕士论文,200943率吟吟.基于蚁群算法的移动机器人路径规划方法的探讨8.沈阳理工高校硕士论文,200744何娟,涂中英,牛

33、玉刚.一种遗传蚁群算法的机器人路径规划方法J计算机仿真,2010,27(3)45李猛，王道波,柏婷婷,盛守照.基于蚊群优化算法和人工势场的无人机航迹规划U1.应用科学学报.2012,30(2)46朱庆保，张玉兰.基于精格法的机器人路径规划蚊群算法J.机器人，2005,27(2)致谢本课题是在导师李昌明副教授的悉心指导和亲切关怀下完成的.导师严谨的学风和丝不苟的工作看法对我以后的人生影响深远。避在此向李老师表示诚意的感谢和崇高的敬意。深深地感谢我们尊做的班导杨冬香老师,得利于她静默的关怀和诚意的帮助我才能项当地走过高校四年的求学之路.感谢教过我的全部老师，没有他们孜孜不倦的训诲，我不行以有今日的

34、进步。感谢我的同学和挚友，感谢他们赐予的支持和激励。感谢我敬爱的家人，他们无私的爱，他们为我付出的巨大牺牲和努力，我将用一生赐予回报.祝堰他们，以及那些赐予我关爱的长辈，祝他们华蜜、安康！2012年5月20日附录A运动分析的相关程序1计算两组支撑三角形大叠面积c1.eara1.1.;C1.Ce定义变盘：a为两三角形X轴方向上惜开的距离,即点B到线AC的距离或点B，到线AC的距离:*b为两三角形y轴方向上错开的矩离，即点B到点B，在y轴方向上的距离：h为三角形在X轴方向的高:Wk为直线BA，的斜率,易知直线AB，的斜率为1.k).symsabhkpositive;*X为点e、d、j、i在X轴的坐

35、标俯殂成的矩阵。X=a(a*k+h*k+b)/(2*k)hh;%Y为点e、d、j.i在y轴的坐标值组成的矩阵.Y=k*a(a*k+h*k+b)/2b+a*kb-a*k;告求四边形edji的面积area。area=sir11p1.ify(po1.yarea(X,Y);W求四边形edji的面枳area的最大(ftmax.dydx=diff(area,a);A三so1.ve(dydx,a)Inax=Simp1.ify(subs(area.A,a)2转换矩阵生成程序functiony=DH(site,d,a,a1.pha)cs三cos(sita);ss三sin(sita);ca=cos(a1.pha)

36、;sa=sin(a1.pha);ifabs(ca-0).O1.ca=0;e1.seifabs(ca-1.).01Ca=I;e1.seifabs(ca+1.).01Ca=-Iendifabs(sa-0).01sa=0;e1.seifabs(sa-1.).01sa三1.;e1.seifabs(sa+1.).01sa=-1.;endy=cs,ca*ss,-ss,ca*cs,O,-sa,sa*ss,O,sa*cs,O,ca,O,-sa*d;ca*d;U；3足端轨迹的生成和计算程序c1.osea1.1.;c1.eara1.1.;c1.c;QR多项式拟合函数的系数.symsCoc1.c2c3c4c5c6t

37、1.t2aa2vv2pp1.p2t;F1.=c0+c1.*t1.+c2*t1.A2+c3*t1.A3+c4*t1.A4+c5*t1.A5+c6*t1.A6;F2-c0+c1.*t2+c2*t2A2+c3*t2A3+c4*t2A4+c5*t2A5+c6*t2A6;F3=diff(F2,t2);F4=diff(F3,t2);f1.=p-c;f2=v-c1.;f3=a0-2*c2;f4=p1.-F1.;f5=p2-F2;f6=v2-F3;f7=a2-F4;cc1.c2c3c4c5c6-so1.ve(f1.zf2,f3,f4,f5,f6,t1.,c,f,c1.,c2,c3,c4,c5,c6);C=Si

38、mp1.ify(c0；c1.；c2；c3；c4；c5；c6)QR足端的凯波拟合函数、速度函数和加速度函数.pt=c0+c1.*t+c2*tA2+c3*tA3+c4*tA4+c5*tA5+c6*tA6;vt=c1.+2*c2*t+3*c3*tA2+4*c4*tA3+5*c5*tA4+6*c6*tA5;at=2*c2+6*c3*t+12*c4*tA2+20*c5*tA3+30*c6*tA4;pt-subs(pt,ip,p2,p1.,v,v2,a,a2,t1.zt2z20;-10;-15,20;10;-15,23;0;-1.1./0;0;0,0;0;0,0.3,0.6);vt=subs(vt,p,p

39、2,p1.,v,v2,aza2,t1.,t2,(20;-10;-15,20;10；-15,23;0;-1.1.,0;0;0,0;0;0,0;0;0,0;0;0,0.3,0.6);at-subs(at,ip,p2,p1.,v,v2,a,a2,t1.,t2,20;-10;-15,20;10;-15,23;0f-1.1.,0;0;0,0;0;0,0;0;0,0;0;0,0.3,0.6);digits(5)；pt=vpa(pt)vt-vpa(vt)at=vpa(at)%亘路径曲线的函数图像.t-0:0.01:0.6;pt=c0+c1.*t+c2*t.A2+c3*t.A3+c4*t.A4+c5*t.A5

40、+c6*t.A6;vt=c1.+2*c2*t+3*c3*t.2+4*c4*t.3+5*c5*t.A4+6*c6*t.5;at=2*c2+6*c3*t+12*c4*t.A2+20*c5*t.A3+30*c6*t.A4;pt=subs(pt,pr2zp1.rv,v2ra,a2rt1.zt2,(20;-10;-15,20;10;-15,23;0;-1.1.,0;0;0,0;0;0,0;0;0,0;0;0,0.3,0.6);vt=subs(vt,p,p2zp1.,vzv2,a,a2,t1.,t2/(20;-10;-15,20;1O;-15,23;0;-1.1.,0;0;0/0;0;0,0;0;0/0;

41、0;0,0.3,0.61);at=subs(at,rp2,1.rvzv2raza2rt1.,t2,(20;-10;-15,20;10;-15,23;0;-1.1.z0;0;0,0;0;0,0;0;0,0;0;0,0.3,0.6);figure(name,在x,y,z方向上的位移、速度和加速度图像。*,y,z方向的路径曲战图像.subp1.ot(3/3/1)p1.ot(t,pt(1,:)zx1.abe1.(,时间t(三),),y1.abe1.(,轴位移(Cm),)subp1.ot(3,3,2)P1.Ot(t,pt(2,:),XIabeIr时间t(三),y工abe1.y轴位移(Cm)subp1.o

42、t(3/3/3)p1.ot(t,pt(3,:),x1.abe1.(时间t(三),),y1.abe1.(,z轴位移(cm),)%,y,z方向的速度曲线图像。subp1.ot(3,3,4)p1.ot(t,vt(1/:),x1.abe1.时间t(三),),y1.abe1.(,x轴速度(cms),)subp1.ot(3,3,5)p1.ot(t,vt(2,:),x1.abe1.(时间t(三),),y1.abe1.(,y轴速度(cms),)subp1.ot(3,3,6)p1.ot(t,vt(3/:),x1.abe1.时间t(三),),y1.abe1.(Z轴速度(cms),)O,y,z方向的加速段曲线图像.

43、subp1.ot(3,3,7)p1.ot(tfat(1,:),X1.abe1.(，时间t(三),y1.abe1.(,x轴加速度(cms2),)subp1.ot(3z3z8)p1.ot(trat(2,:),x1.abe1.(,时间t(三),),y1.abe1.(y轴加速度(cns2),)subp1.ot(3/3/9)p1.ot(tfat(3z:),X1.abe1.CIHfSJt(三),y1.abe1.(z轴力口速度(ci11s2),)学路径的空间曲线图像。figure(name,z,路径的空间曲线图像,)1.ot3(pt(1,:),t(2,：),t(3,:),gridon,x1.abe1.(,x(cm),y1.abe1.(,y(cm),z1.abe1.(,Z(cm),)告下面求各关节转角、角速度和角速度的函数图像，t1.=0.005:0.01:0.595;t2=0.01:0.01:0.59;theta(1,:)=atan(pt(2,:).pt(1,:);x-pt(1,:)-cos(theta(1,:)*3.2;*最左列中