毕业设计论文仿生微小机器人模糊PID控制系统设计.doc

中北大学信息商务学院2010届本科毕业设计说明书 仿生微小机器人模糊PID控制系统设计 1 引言 在人类不断地探索自然、认识自然和改造自然的历史中，机械与自动化一直是人们不断探求的技术之一，无论是用于农业的磨盘和播种机、用于出行的马车和飞机还是用于战争的弩机和机枪，无疑都是人类在这一领域智慧结晶的具体展现，都标志着人类科技的持续进步。而“机器人”这一代表着机械与自动化技术完美结合与最高成就的产物，正是人类不断解放生产力、发展生产力的必然产物，是当下人类最为向往的技术之一。机器人”是存在于多种语言和文字中的新造词，它体现了人类长期以来的一种愿望，即创造出一种象人一样的机器甚至是人造人，以便能够代替人去进行各种工作1J。从传说中三千年前诸葛亮设计的“木牛流马”到20世纪20年代捷克作家凯培克在罗莎姆的万能机器人中所创造和描述的“Robot”，都显示出人类对机器人的美好憧憬。根据美国作家阿西莫夫在小说中提出并被机器人研究者、制造商和用户所广泛接受的“机器人三守则”所述，机器人必须不危害人类，也不允许它眼看人将受害而袖手旁观;机器人必须绝对服从于人类，除非这种服从有害于人类;机器人必须保护自身不受伤害，除非为了保护人类或者是人类命令它做出牺牲。从初创时起，机器人就成为世界各国争相研究和发展的热点，发展至今已在工业领域，如焊接、装配等产业发挥着广泛的作用，并逐渐步入航空航天、地下水下、生活娱乐等领域，具有很高的科研和产业价值。因此机器人技术的发展程度对国家的发展具有重要的战略和现实意义。本论文主要进行以下的具体研究工作：1.1机器人学 二战期间美国军方首先研究并投入生产的遥控操作器和数控机床，标志着现代机器人技术2开始走向应用。近年来，机器人技术在军事、航天、建筑、工业等领域都获得了快速显著的发展，在最艰苦、最危险的工作环境下协助或者代替人类进行着大量的工作，如排爆机器人、无人机、水下机器人、火星探测机器人等，同时也大大提高了生产效率，如各种工业机械臂等为人类做出了巨大的贡献。机器人学交叉融合了机械、电子、材料、计算机、自动化、传感技术、控制技术、人工智能、仿生学等各门学科和技术，立足高科技发展的前沿，成为当今科技发展的标杆，是目前发展最活跃的研究领域之一，并且开始逐渐步入人们的日常生活中，各种服务型机器人、娱乐型机器人的产生已经开始改变人们的生活、娱乐方式。目前机器人学不断发展，各种新型机器人层出不穷，其中又以仿人机器人最为引人注目13一，。仿人机器人是模拟人的形态和行为的机器人，其具有类似体理结构的关节连杆结构，通常有头部、身体、腰、手、足等部件组成，并由各种传感器来模拟视觉、触觉等方面的功能，可以模拟人类行走、抓握等行为方式。因为仿人机器人拥有类似人类的外形和物理结构，更容易和人类相处，这类机器人的出现标志着该领域新时代的开始，对其研究已经成为机器人领域世界各国竞相发展的重点。仿人机器人比普通机器人有更强的性能和适应性，但因此也极大地增加了结构复杂度，其关节连杆结构和自由度成倍增加，也加大了设计与制造时的工作量和难度，对机器人研究领域提出了新的挑战。 1.2机器人建模与仿真 目前机器人领域研究开发主要困难在于实物机器人结构复杂、价格昂贵、故障排除困难和损耗较大。同时机器人(尤其是仿人机器人)通常是由多连杆、多自由度组成的复杂结构。在实际开发和测试实物机器人的过程中，需要设计和解析其复杂的机械结构，采集并分析其大量的运动学、动力学参数，设计和验证机浙江大学硕士学位论文第】章绪论器人运动规划数据(如仿人机器人步态规划数据)，通过反复实验以完成机械结构设计的精确度和规划数据设计的准确性等校验。随着机器人机械结构日趋复杂，传统设计过程中采用人工计算需要极大的工作量，这是困难而低效的，也因此带来了技术研究的瓶颈。目前突破传统设计的一个主要方向是采用建模与仿真方法，即通过对机器人硬件及所处环境建模并仿真，以有效提高机器人控制软件和算法的开发效率，提供更优越的软件测试条件，减少不必要的扰动和噪声，并降低硬件损耗以避免不必要的损失，降低机器人研究、开发与实验的难度。机器人建模系统可建立机器人所处环境模型和机器人自身模型。环境模型包括机器人所处现实物理环境各种物体的模拟，通过建立地面、障碍物、目标物等模型支持机器人与虚拟环境的交互。机器人自身模型包括机器人几何外型模型、连杆结构模型、物理结构模型、驱动器模型等。通过此类建模可以实现在虚拟实环境中构建模型并进行相应测试和修改，通过校验验证其真实机械结构设计的精确度，从而减少人工计算量和实物实验成本。机器人仿真系统可在虚拟环境下展现机器人所处的真实世界，在获取机器人模型后通过对其进行仿真，可模拟其机械结构、控制系统和运动规划，并在虚拟的仿真环境下完成机器人的运动学、动力学分析，模拟其运动过程和结果，反馈各种有效的物理参数。通过在机器人系统仿真环境下进行各种设计、开发和测试实验，发现机器人控制系统、机械结构等方面的各种不足并做出及时有效调整减少对机器人实体反复进行的无效物理实验，对机器人研发有着实际指导意义。因此在制造物理实体机器人之前的设计过程中，对机器人连杆、物理结构等进行建模，并在虚拟现实的仿真环境中模拟机器人所处环境、控制系统、机械结构，进行运动学、动力学和控制仿真，可以大幅减少研发成本、有效缩短开发周期、显著提高设计质量，正是目前机器人设计领域的研究热点和发展趋势。机器人仿真系统的另外一个应用是其可用于机器人的教学和培训。机器人价格昂贵，在进行机器人教学和培训中使用实体机器人成本耗费巨大，而成熟的机器人建模与仿真系统为此提供一个方便和灵活的实验工具和手段。1.3问题的提出 目前己有很多研究者致力于机器人建模与仿真并提出其方法，但仍然存在建模复杂耗时、仿真参数获取困难、操作过程繁复等问题。针对上述情况，本文研究基于图形交互的机器人建模与仿真方法并实现其系统，提供易用的交互式用户界面，支持快速建立和校验实物机器人关节连杆结构、物理碰撞检测形状、驱动器等模型;支持实时高效的机器人三维动力学仿真、绘制和动力学参数反馈，支持控制仿真和驱动器调试等功能。本文从以下三点进行研究。本文通过研究仿人机器人建模方法，建立可视化的机器人建模系统，可交互式地完成从机器人几何模型到连杆模型、物理模型的快速建模和校验，从而实现对实物机器人连杆结构和物理碰撞检测形状的模拟。以关节连杆结构为基础，机器人完整仿真模型内容可根据用户需求进一步扩展，增加驱动器模型、传感器模型等建模。同时，设定完毕的机器人仿真模型文件既可单独用于完成机器人运动学仿真过程，也可用于各种机器人仿真系统如基于MSRS的机器人仿真系统和基于物理引擎的机器人仿真系统的输入。本文研究并实现基于 MsRs(MicrosonRobotieSstudio)的机器人仿真系统。系统充分利用MSRS系统集成度高、通用性强等优势，用户通过导入机器人连杆模型SGM文件和运动规划文件可交互地完成实时机器人三维动力学仿真并获取部分动力学参数，改进了MSRS难以获取机器人动力学参数的问题，同时将大量环境对象、机器人模型等封装成服务以支持重用。本文研究并实现基于物理引擎的机器人仿真系统。系统充分利用Physx刀引擎物理仿真和碰撞检测准确度高等优势，真实模拟机器人关节电机和控制器的复杂结构，用户导入机器人完整仿真模型文件ROB并交互进行实时三维物理仿真和控制过程，系统绘制仿真结果并根据用户需求反馈大量物理参数以供分析。系统对机器人研究者在建立真实机器人之前进行的电机选型、机械结构设计和控制器设计等工作具有现实指导意义。本文研究方法和系统可指导实物机器人设计和选型，有效提高机器人研发效率、大幅降低研发时间和成本。1.4本文工作和论文结构 1本文研究的机器人可视化建模与仿真方法，要求快速建立准确的机器人连杆结构模型、物理模型等并实现实时精确的机器人运动学和动力学仿真和碰撞检测过程，同时反馈建立实物机器人所需的可靠评估参数。基于以上要求，本文做了大量针对性研究工作并通过建立以下三个系统完成以上要求。1.基于图形交互的快速机器人建模系统(Gr即hies一 basedRoboticSModelingsystem:o删s)。基于visuale+建立用户界面并采用openGL8图形绘制库实时完成三维绘制。根据导入的机器人几何模型，由系统提供交互式接口帮助用户设定机器人关节连杆结构及其参数，同时利用连杆结构校验功能快速精确地完成机器人连杆和物一理模型建模并导出建立完毕的机器人仿真模型文件以供各种仿真系统使用。 2基于MSRS的机器人仿真系统(MsRS一 basedRoboticssimulationsystem:MRSS)。基于 MierosoftRobotieSStudio机器人仿真平台进行开发。通过导入机器人仿真模型文件，导入机器人运动规划文件或输入运动控制数据，系统支持用户交互地进行实时机器人三维动力学仿真和绘制，同时反馈物理参数以供分析。 3基于物理引擎的机器人仿真系统(PhysiesEngine basedRobotieSSimulationsystem:PERss)。基于visuale+建立用户界面，采用physX物理引擎实时完成三维动力学仿真并采用OpenGL图形绘制库实时完成三维仿真结果绘制。系统的设计灵活，通过设定关节、马达和控制器参数，可模拟完整的机器人马达和控制器，方便用户进行针对性更强的机器人三维动力学仿真，其反馈参数也更丰富。本文内容共分为六章，其结构安排如下: 第一章为本文绪论。主要介绍机器人学的研究现状和发展趋势，本文的研究意义和主要研究内容。 第二章为本文研究背景。主要对机器人建模和机器人仿真研究的技术背景进行详细分析，也为其余各章节作理论张本。 第三章介绍基于图形交互的机器人建模系统GRMS。详细阐述机器人连杆结构模型、物理模型和完整仿真模型的结构、设计含义及其建模实现方法，阐述机浙江大学硕士学位论文.绪论器人模型校验过程，并通过实验对其进行分析。 第四章介绍基于MSRS的机器人仿真系统MRSS。提出基于MSRS的机器人仿真方法并细分析其实现，最后通过实验分析其仿真结果和可行性。 第五章介绍基于物理引擎的机器人仿真系统PERSS。提出基于物理引擎封装层的机器人仿真系统并详细分析其实现方法，提出基于PID控制原理的机器人关节驱动器仿真方法。最后通过实验分析其仿真结果和可行性。2 微小型机器人结构原理 章微小型装配机器人结构原理装配机器人是指在工业生产中，用于装配生产线上，对零部件进行装配的工业机器人，它属于高、精、尖的机电一体化产品，是集机械、微电子、自动控制于一体的高科技产品，具有很强大的功能和很高的经济效益。相对于一般的工业机器人，装配机器人具有精度高、柔顺性好、工作范围小能与其它系统配套使用的特点。而微小型装配机器人，应具有结构紧凑、运动灵活、重复定位精度高等特点。因此，首先要设计具有完成装配能力的机器人结构本体，并设计可以直接驱动机器人关节，而不需要繁琐的减速及传动机构的驱动器。2.1 机器人设计原理机器人原理设计在机械装备、电子设备制造等行业中，用于装配作业的机器人的主要操作是：垂直向上抓起零部件，水平移动它，然后垂直放下插入。要求这些操作进行的既快又平稳。因此，一种能够沿着水平和垂直方向移动，并能对工作平面施加压力的机器人是最适于装配工作的。此外，对于某些装配工作，要求装配机器人或其装配工具具有某种柔顺性，即具有自动对准中心孔的能力。平面型关节机器人可以看作是关节坐标式机器人的特例。它只有平行的肩关节，关节轴线共面。其最显著的特点是在XY平面上的运动具有较大的柔性，而沿Z轴具有很强的刚性，所以它具有选择的柔性，这种机器人在装配作业中获得了很好的应用。本设计为四自由度平面关节型机器人（如图2-1所示）。有两个回转关节，可以保证装配机器人对操作空间内同一平面中不同位置处的零部件操作；一个移动关节，实现机器人在操作空间内竖直方向的移动；利用末端夹持器对零部件进行抓取和装配操作。回转关节和移动关节分别由回转型SMA驱动器和移动型SMA驱动器驱动。这些运动关节保证了末端执行器可以到达操作空间的任一位置进行装配作业。通过对运动关节的控制，实现装配机器人末端执行器的位姿控制。 2.2 SMA驱动器原理SMA驱动器性能的好坏与SMA的相变温度、相变回复力大小以及其机械性能和物理性能有关。SMA的这些性能又受合金成分、使用条件等因素的影响。其中合金成分的影响最大，实验表明，Ni含量改变0.1at%(at%表示元素原子百分含量)，TiNi基的相变温度将改变10°C左右。目前应用最多的是NiTi基和Cu基SMA，前者可恢复应变大，疲劳寿命短；后者有更大的可恢复应变和更短的疲劳寿命。Ni-Ti基SMA比Cu基SMA高成本高5-10倍，且相对较难控制。因此在机械手、温度探测等领域多采用Cu基SMA。2.21回转型SMA驱动器原理 目前SMA的单程形状记忆处理较成熟，因此实用的双程SMA驱动器都采用具有单程形状记忆的SMA在加热时产生驱动力。根据偏置元件的不同又分为偏动式和差动式。差动式SMA驱动器就是指偏置元件也采用相同的单程形状记忆合金丝，工作时，两段合金丝分别处于加热和冷却阶段，利用两个SMA元件在高低温时的回复力之差对外做功。其优点就是输出力大，转换效率高。而偏动式采用普通弹簧等作偏置元件，不存在两个SMA间的协调控制问题，因此控制相对容易，结构简单，成本也较低。弹簧状SMA可获得较大的输出位移，而丝状SMA驱动器则可以输出较大的力。本课题将采用偏动式SMA丝驱动器，原理如图2-2所示。 a) 回转型SMA驱动器b) 移动型SMA驱动器图2-2 SMA驱动器原理示意图Fig.2-2 Principle of bias type SMA Actuator 回转型SMA驱动器原理如图2-2(a)所示。主要包括SMA丝，偏置弹簧和被驱动的回转体。其驱动原理是SMA丝通过电流加热变形，使回转体发生转动，同时使偏置弹簧拉伸；当SMA丝停止加热开始冷却后，随着SMA本身温度的降低，SMA丝在偏置弹簧的作用下，恢复到加热变形前的状态，并驱动回转体向相反的方向转动。通过不断的加热冷却，实现驱动器的回转运动。回转型SMA驱动器主要由三部分组成：SMA丝的热驱动模型、SMA丝的本构关系模型和SMA驱动器的动力学模型。 SMA材料通过电流加热-自然冷却的方式来控制其自身的温度，实现对SMA材料响应速度、输出力和位移的控制。此种方式中，SMA形变的响应速率主要由SMA材料的冷却速率决定。SMA丝通过电流进行加热，散失的热量主要为热辐射和热对流，而热辐射在热量损失中占绝大部分比重。因此，根据电流加热定律、热力学基本定律和热量散失的牛顿方程可得到加热电流和SMA材料温度之间的关系为20cmT (t) =i (t) R +hA( T (t) T0) (2-1) 式中m、c、R和A分别为SMA丝的质量、比热容、电阻值和表面积，i(t)、T(t)和T0分别为SMA丝的加热电流、SMA材料的温度和环境温度，h为SMA丝与周围环境的换热系数，下同。 SMA材料的电阻值是其温度的函数，会随温度的变化而变化，但其变化范围在稳定值的10%以内，因此为简化模型，将其视为常量。同样，SMA丝的表面积也会在工作过程中随着SMA丝的变形而发生变化，但这种变化很小，可以忽略不计。 SMA丝的本构关系模型主要体现为它的非线性迟滞行为。即SMA中马氏体体积分数（）和SMA材料温度（T）间的函数关系。从相变将使自由能降低10用热力学原理可以建立经受热弹性马氏体相变及逆相变材料的本构关系，其导数形式的力学本构关系可以表示为=E +T+ (2-2 式中、T、E、?分别表示SMA材料的应力、应变、温度、马氏体体积份数、弹性模量、热弹性系数和相变系数。下同。，2.2.2 移动型SMA驱动器原理 理移动型SMA驱动器原理如图2-2（b）所示，驱动器在水平面内。为偏动式SMA驱动器，工作时依靠SMA丝受热产生的变形提供驱动力，产生直线方向的位置移动，并使偏置弹簧受迫拉伸，热能通过SMA的变形转化为弹簧的变形能；SMA丝冷却过程中，随着温度的降低，SMA丝产生马氏体逆相变，并在偏置弹簧的作用下产生变形，并逐步恢复到加热变形前的形状，同时使驱动器产生与加热过程中方向相反的移动力和位移。在不断的加热-冷却循环过程中实现SMA驱动器对执行机构的直线驱动。 移动SMA驱动器具有和回转型SMA驱动器相同的热驱动模型和本构关系模型，但回转型SMA驱动器输出为力矩而移动性SMA驱动器为力输出，分别用于驱动装配机器人的回转运动关节臂和直线移动关节臂。2-3为移动型SMA驱动器在开环条件下，分别提供方波电流输入和正弦电流输入时的系统响应曲线。图2-3(a)为移动SMA驱动器在周期40s、占空比0.5、电流幅值为1.2A的脉冲激励下的位移输出，图2-3(b)为移动SMA驱动器在正弦电流激励下的位移输出，两者都很好的吻合了位移随着控制电流的变化曲线。 a)阶越电流时驱动器位移-时间曲线 b) 正弦电流时驱动器位移-时间曲线图 2-3移动型SMA驱动器的电流激励响应Fig.2-3 Response of shift SMA actuator with electric current input2.3机器人运动学分析 器人末端执行器在空析机间运动的轨迹是由机器人工作任务所决定的，轨迹上的各点对应于它们所要求的关节变量值，解出这些关节变量去控制机器人各个执行机构的运动，从而实现所规划轨迹的跟踪，就是运动学的基本任务。 工业机器人都是以关节坐标直接编制控制程序的。机器人的工作是由控制器指挥，而关节在每个置的参数是预先记录好的。当机器人执行工作任务时，控制器给出记录好的位置数据，使机器人按照预定的位置序列运动。物体在工作空间内的位置以及机器人末端执行器的位置都是以某个确定的坐标系来描述的；而工作任务则是以某个中间坐标系来规定的。 利用笛卡尔坐标系来描述工作任务时，必须把上述这些规定变换为一系列能够由手臂驱动的关节位置。确定手臂位置和姿态的各关节位置的解答，即运动方程的求解。根据以上分析，建立如图2-1所示的装配机器人笛卡尔坐标系。利用齐次矩阵变换理论和D-H矩阵参数表（如表2-1所示）建立机器人运动学模型。按Denavit-Hartenberg方法37建立操作臂运动学方程。表中1、2、3和4分别为装配机器人各关节的关节变量。其它参数所代表的意义如图2-1和表2-1中的标注所示。连杆夹角连杆扭角连杆长度L连杆距离d关节变量q 1 1(0) 0 L1 L0 1 2 2(0) 0 L2 0 2 3 0 0 0 -d3 d3 4 4(0) 0 0 -L4 4 表2-1装配机器人连杆参数 2.3.1运动仿真学 虚拟样机建模机械工程中的虚拟样机技术又称为机械系统动态仿真技术，是20世纪80年代随着计算机技术的发展而迅速发展起来的一项新技术，其核心是机械系统的运动学和动力学仿真技术，同时还包括三维CAD建模技术、有限元分析技术、电液控制技术、最优化技术等相关工作。Adams可以对简单机构进行粗略的建模，对于复杂机构则表达不细致，操作不方便。因此考虑采用专业的建模软件（如CATIA、Pro/e、UG等）与Adams相结合的方式实现机器人的建模。此文中，通过Pro/e和Adams接口软件Mech/Pro实现Pro/e和Adams的联合建模与仿真38。装配机器人的实体建模过程如图2-4所示。首先在Pro/e中进行机器人各个运动部件的三维实体建模并完成机器人的机构装配(如图2-1(a)所示)。添加一些简单的坐标系和约束以后通过Mech/pro接口软件将机器人装配模型导入到Adams中，并在Adams中对机器人施加必要的约束和驱动，并进行静力学和运动学分析。用Pro/e进行机器人三维实体建模在Pro/e中完成机构装配形成刚体和简单的约束通过Mech/pro模块将模型导入Adams在Adams中添加复杂的约束和驱动在Adams中实现机器人的运动和仿真结束否得到仿真曲线, b)移动关节变量变化曲线 图26各个关节变量变化曲线 Fig.2-4 Principle chart of union modeling for the robot16各关节的运动角度（位移）变化情况如图2-6所示。其中仿真时间及各关节在运动一定转角所需要的时间需根据实际情况和需要进行设定。在被装配件转移过程中更强调转移的快速性，而在抓取和装配的操作环节则强调高的位置控制精度。2.5本章小结本章介绍了装配机器人的用途及操作特点，并据此设计了具有两个回转关节及一个移动关节的装配机器人及其虚拟样机模型。建立了机器人的笛卡尔坐标系和D-H参数表，完成了机器人的运动学分析，从而为装配机器人运动轨迹规划做好了准备。利用Adams对机器人进行了运动学分析和仿真，进一步验证了模型设计及运动学分析的可行性和有效性。针对机器人运动关节和移动关节分别设计了回转型SMA驱动器和移动性SMA驱动器，为机器人的微小型化和轻量化做出了必要的准备。在机器人运动学分析的基础上，运用三次样条插值法对机器人在装配作业过程中的轨迹进行了规划，并进行了运动学仿真，为机器人轨迹跟踪控制做好了必要的准备。2.4模型的实现及仿真实例 下图为机器人动力学模型。它都采用SIMULINK的S函数来实现。 fsys2=addvar(fsys2,'input','E',-60 60)fsys2=addmf(fsys2,'input',1,'NB','zmf',-60 -40);fsys2=addmf(fsys2,'input',1,'NM','pimf',-60 -40 -39 -20);fsys2=addmf(fsys2,'input',1,'NS','pimf',-40 -21 -19 0);fsys2=addmf(fsys2,'input',1,'ZO','pimf',-10 -1 1 10);fsys2=addmf(fsys2,'input',1,'PS','pimf',0 19 21 40);fsys2=addmf(fsys2,'input',1,'PM','pimf',20 39 41 60);fsys2=addmf(fsys2,'input',1,'PB','smf',40 60);plotmf(fsys2,'input',1)fsys2 = name: 'newsys' type: 'mamdani' andMethod: 'min' orMethod: 'max' defuzzMethod: 'centroid' impMethod: 'min' aggMethod: 'max' input: output: rule: fsys2 = name: 'newsys' type: 'mamdani' andMethod: 'min' orMethod: 'max' defuzzMethod: 'centroid' impMethod: 'min' aggMethod: 'max' input: 1x1 struct output: rule: 2.5本章小结 本章小结本章介绍了装配机器人的用途及操作特点，并据此设计了具有两个回转关节及一个移动关节的装配机器人及其虚拟样机模型。建立了机器人的笛卡尔坐标系和D-H参数表，完成了机器人的运动学分析，从而为装配机器人运动轨迹规划做好了准备。利用Adams对机器人进行了运动学分析和仿真，进一步验证了模型设计及运动学分析的可行性和有效性。针对机器人运动关节和移动关节分别设计了回转型SMA驱动器和移动性SMA驱动器，为机器人的微小型化和轻量化做出了必要的准备。在机器人运动学分析的基础上，运用三次样条插值法对机器人在装配作业过程中的轨迹进行了规划，并进行了运动学仿真，为机器人轨迹跟踪控制做好了必要的准备。3微小型装配机器人动力学分析3.1机器人动力学建模动力学研究物体运动和受力之间的关系。机器人动力学方程是机器人机械系统的运动方程，它表示机器人各关节的关节位置、关节速度、关节加速度和各关节驱动器驱动力矩之间的关系。机器人拉格朗日力学法推导机器人动力学方程共分为五步进行： 1）计算任一连杆上任一点的速度 2）计算各连杆的动能和机器人的总动能 3）计算各连杆的位能和机器人的总位能 4）建立机器人系统的拉格朗日函数 5）对拉格朗日函数求导，以得到动力学方程。拉格朗日函数L被定义为系统的总动能K和总位能P之差，即 L =K-P式中K和P可以用任何的方便坐标系表示 3.1.1动能与位能 动能与位能根据拉格朗日法推导机器人动力学方程的步骤，首先需要计算机器人总的动能和总的位能。为最大程度上反映机器人动能的实际情况，得到机器人关节臂上任一点的速度后，对此点临域内微小质量元的动能进行计算，并对微元质量的动能在整个关节臂上进行积分，从而得到关节臂的动能。 1）高度非线性。动力学方程中共有三个方程，方程中的项是非线性的，方程中包含正、余弦三角函数。 2）高度耦合性。机器人动力学方程有很大数量的耦合项，杆件1和杆件2之间存在很强的耦合关系，相互影响严重。而移动臂与两个转动臂之间则不存在耦合影响。 3）含有不确定参数项。当计算大臂驱动器所驱动对象的转动惯量时，由于各杆件之间的位置关系不确定，因此转动惯量属于时变量。 4）D(q)为对称正定阵，并存在逆阵，且其逆也为对称、正定矩阵。3.2SMA驱动关节动力学模型 以小臂为例建立SMA驱动回转运动关节动力学模型，原理如图3-2所示。小臂的运动是通过固定在大臂上的SMA驱动器驱动的，在SMA的应变和小臂的回转角度之间有一个定量的函数关系。假设加粗线代表SMA丝，将其两端接26通过电流对其进行加热，并采用自然风冷的办法使其冷却。 SMA具有热滞后效应，即其本身温度必须低于或高于某一温度时，才能导致SMA发生相变。相变过程就是马氏体和奥氏体随温度变化下的相互转化过程。马氏体和奥氏体的相互转化过程，在微观上表现为晶体结构的变化，而在晶体结构变化过程中产生应力应变，宏观上表现为SMA丝的长度变化。4. PID控制原理 对于模拟控制系统，常规PID控制1314由模拟PID控制器和被控对象组成。其将偏差比例（P）、积分（I）、和微分（D）通过线性组合构成控制量，并对被控对象进行控制，控制原理框图如图4.1示。图4.1模拟PID控制系统原理框图 PID控制器是一种线性控制器，它根据给定值r(t)与实际输出值c(t)构成控制偏差为：e(t)=r(t)-c(t)其控制规律为：其中， 比例系数; 积分时间常数； 微分时间常数。简单说来，PID控制器各控制环节的作用如下： 1.比例环节：为了即时成比例地反应控制系统的偏差信号e(t)，一旦偏差产生，控制器以最快的速度产生控制作用，以减少偏差。 2.积分环节：主要用于提高系统的无差度，即保证被控制量c(t)在稳态时对给定量r(t)的无静差控制。积分作用的强弱取决于积分时间常数，积分时间常数越大积分作用越弱，反之则越强。 3.微分环节：能反映偏差信号的变化趋势，并能在偏差信号值变得太大之前，在系统中引入一个有效的早期修正信号，从而加快系统的动作速度，减小调节时间。4.1模糊控制基本原理 模糊控制是以模糊数学为基础，以模糊集合表示变量，由模糊逻辑运算推理判决的一种高级控制策略，是现代智能控制理论的一个分支。模糊控制是通过模糊逻辑和近似推理方法，把人的经验形式化、模型化，变成计算机可以接受的控制模型，让计算机代替人来进行有效的控制。 模糊控制的基本原理如图4.2所示，其核心部分为模糊控制器。模糊控制器主要由计算控制变量、模糊量化处理、模糊控制规则、模糊推理（模糊决策）和非模糊化处理等部分组成。模糊控制的过程是这样的：采样设备经中断采样获取被控制量的精确值，然后将此量与给定值比较得到误差量，将误差量作为模糊控制器的输入量，通过模糊量化处理，将误差的精确量变成模糊量，误差的模糊量可用相应的模糊语言表示，至此，得到了误差的模糊语言集合的一个子集，再由和模糊控制规则（模糊关系）根据推理的合成规则进行模糊推理（模糊决策），得到模糊控制量；最后，为了对被控对象施加精确的控制，还需要将模糊量转换为精确量，这个过程称为非模糊化处理。得到了精确的控制量之后，通过执行机构作用于被控对象，从而完成了一次的模糊控制过程，通过再次采样，进行第二次的、第三次的模糊控制过程，最终实现了系统的模糊控制。、4.1.1常用pID的整定方法 PID控制方法最主要的任务是整定控制器PID参数。其实参数整定就是设置和调整控制器的参数，使控制系统的控制效果达到满意的程度。目前为止，国内外对于控制器PID参数整定方法的研究已经有几十年的历史提出了多种优秀的参数整定方法，主要有以下几种:基于被控过程对象参数辨识的整定方法;基于抽取对象输出响应特征参数整定法，如Z一N参数整定法(也称临界比例度法);参数优化方法基于模式识别的专家系统法以及基于控制器自身控制行为的控制器参数在线整定等。1、凑试法 凑试法是指先将调节器的参数根据经验设定在某一数值上，然后在闭环系统中加干扰源，观察过度的曲线形状，如曲线不够理想，则以调节器P、I、D参数对系统过渡过程的影响为依据，按照先比例，后积分，最后微分的顺序，将调节器参数逐个进行反复凑试，直到获得满意的控制效果。具体步骤如下: (l)设置初始条件下的比例度占，运行系统，整定比例度。 (2)引入积分调节，将T:由大到小进行整定。 (3)若需要引入微分调节，则按经验设置T。，同样由小到大进行整定。2、临界比例度法(Z一N法) 本方法是齐格勒(Ziegler)和尼柯尔斯(Nichols)提出的一种P功参数工程整定的方法。这种方法基于闭环响应，适应在闭合的控制系统里，将调节器置于纯比例作用下，从大到小逐渐改变调节器的比例度占，得到等幅振荡的过程，此时的比例度称为临界比例度，用氏表示;用TK表示相邻两个波峰间的时间间隔，称为临界振荡周期，通过计算可求出调节器的整定参数。具体步骤如下: (1)将调节器的积分时间T:设置为最大，微分时间T。设置为零，比例度占适当，然后运行系统。 (2)逐渐减小比例度占，得到等幅振荡，记录临界比例度占和临界振荡周期T的值。 (3)根据氏、TK值，采用表3一1中的经验公式，计算出占，T:，T。的值。 表3一1临界比例度法参数整定 控制器参数控制规律 P 2 PI 2.2 0.85 PID 1.6 0.5 0.125 图4.2模糊控制器的系统方框图4.2模糊PID控制4.2.1 模糊PID控制的基本思想 近年来，汽车动力学稳定性控制系统的研究吸引了国内外大批的专家在控制策略上进行了探讨和研究15，主要控制方法有最优控制、滑模变结构控制和模糊PID控制等。最优控制根据汽车动力学模型，再基于状态空间法进行控制。它按照最优原理来求得控制系统的最优指标，对控制系统数学精度依赖性较大，但由于车辆系统本身是个复杂的动力学系统，很难建立精确的数学模型，这在一定程度上限制了最优控制方法在车辆稳定性控制中的应用。滑模变结构控制以经典的数学控制理论为基础，本身具有较强的抗干扰能力和自适应性，属于一种非线性控制系统，但是它的缺点是在切换线附近切换时，在滑动中会因系统的惯性而产生一个附加的抖动。 模糊PID控制是运用模糊数学的基本理论和方法，把规则的条件、操作用模糊集表示，并把这些模糊控制规则及有关信息（如评价指标、初始PID参数等）作为知识存入计算机知识库中，然后计算机根据控制系统的实际响应情况，运用模糊推理，自动实现对PID参数的最佳调整。由于，PID控制本质上属于线性控制，对于具有很强非线性的对象来说，控制效果具有先天的不足；模糊控制虽然其本质上是非线性的，且具有一定的智能性，但是其对数学模型的依赖性弱。本文综合考虑这些因素，利用模糊PID控制将二者有机的结合起来，这样即可以使PID控制具有模糊控制的智能和非线性特点，同时使模糊控制有了PID控制的确定结构，发挥其二者的长处，使汽车动力学模型得到令人满意的控制效果。4.2.2模糊自适应PID控制的基本原理 自适应模糊PID控制器1619以误差e和误差变化ec作为输入，可以满足不同时刻的e和ec对PID参数自整定的要求。利用模糊控制规则在线对PID参数进行修改，便构成了自适应模糊PID控制器，其结构如图4.3示。 PID参数模糊自整定是找出PID三个参数与e和ec之间的模糊关系，在运行中通过不断检测e和ec，根据模糊控制原理来对三个参数进行在线修改，以满足不同e和ec时对控制参数的不同要求，而使被控对象有良好的动、静态性能。图4.3自适应模糊PID控制器结构图 PID参数模糊自整定是找出PID三个参数与e和ec之间的模糊关系，在运行中通过不断检测e和ec，根据模糊控制原理来对三个参数进行在线修改，以满足不同e和ec时对控制参数的不同要求，而使被控对象有良好的动、静态性能。从系统的稳定性、响应速度、超调量和稳态精度等各方面来考虑Kp、Ki、Kd的作用如下： 1比例系数Kp的作用是加快系统的响应速度，提高系统的调节精度。但是如果Kp过大，系统的响应速度越快