机器人-动力学-轨迹-详解
机器人运动轨迹规划
第2章 工业机器人运动学和动力学
第5讲 讲 机器人运动轨迹规划 机器人运动轨迹规划
第2章 工业机器人运动学和动力学 在规划中,不仅要规定机器人的起始点和终止点, 而且要 给出中间点(路径点)的位姿及路径点之间的时间分配, 即给出 两个路径点之间的运动时间。 轨迹规划既可在关节空间中进行, 即将所有的关节变量表 示为时间的函数,用其一阶、二阶导数描述机器人的预期动作, 也可在直角坐标空间中进行,即将手部位姿参数表示为时间的 函数, 而相应的关节位置、 速度和加速度由手部信息导出。
θ (t ) = c0 + c1t + c2t 2 + c3t 3
(3.67)
第2章 工业机器人运动学和动力学 这里初始和末端条件是:
θ (ti ) = θ i θ (t ) = θ f f & θ (ti ) = 0 θ (t ) = 0 & f
对式(3.67)求一阶导数得到:
第2章 工业机器人运动学和动力学
二自由度机器人关节空间的归一化运动
第2章 工业机器人运动学和动力学 如果希望机器人的手部可以沿AB这条直线运动, 最简单的 方法是将该直线等分为几部分(图3.21中分成5份), 然后计算出各 个点所需的形位角α和β的值, 这一过程称为两点间的插值。 可 以看出,这时路径是一条直线, 而形位角变化并不均匀。很显然, 如果路径点过少, 将不能保证机器人在每一小段内的严格直线轨 迹, 因此,为获得良好的沿循精度, 应对路径进行更加细致的分割。 由于对机器人轨迹的所有运动段的计算均基于直角坐标系, 因此 该法属直角坐标空间的轨迹规划。
机器人运动学和动力学分析及控制
机器人运动学和动力学分析及控制引言随着科技的不断进步，机器人在工业、医疗、军事等领域发挥着越来越重要的作用。
而机器人的运动学和动力学是支撑其运动和控制的重要理论基础。
本文将围绕机器人运动学和动力学的分析及控制展开讨论，探究其原理与应用。
一、机器人运动学分析1. 关节坐标和笛卡尔坐标系机器人运动学主要涉及的两种坐标系为关节坐标系和笛卡尔坐标系。
关节坐标系描述机器人每个关节的转动，而笛卡尔坐标系则描述机器人末端执行器在三维空间中的位置和姿态。
2. 正运动学和逆运动学正运动学问题是指已知机器人每个关节的位置和姿态，求解机器人末端执行器的位置和姿态。
逆运动学问题则是已知机器人末端执行器的位置和姿态，求解机器人每个关节的位置和姿态。
解决机器人正逆运动学问题对于实现精确控制非常重要。
3. DH参数建模DH参数建模是机器人运动学分析中的重要方法。
它基于丹尼尔贝维特-哈特伯格(Denavit-Hartenberg, DH)方法，将机器人的每个关节看作旋转和平移运动的连续组合。
通过矩阵变换，可以得到机器人各个关节之间的位置和姿态关系。
二、机器人动力学分析1. 动力学基本理论机器人动力学研究的是机器人在力、力矩作用下的运动学规律。
通过牛顿-欧拉方法或拉格朗日方程，可以建立机器人的动力学模型。
动力学模型包括质量、惯性、重力、摩擦等因素的综合考虑，能够描述机器人在力学环境中的行为。
2. 关节力和末端力机器人动力学分析中的重要问题之一是求解机器人各个关节的力。
关节力是指作用在机器人各个关节上的力和力矩，它对于机器人的稳定性和安全性具有重要意义。
另一个重要问题是求解末端执行器的力，这关系到机器人在任务执行过程中是否能够对外界环境施加合适的力。
3. 动力学参数辨识为了建立精确的机器人动力学模型，需要准确测量机器人的动力学参数。
动力学参数包括质量、惯性、摩擦等因素。
动力学参数辨识是通过实验方法，对机器人的动力学参数进行测量和估计的过程。
机器人运动学与动力学分析
机器人运动学与动力学分析随着科技的不断进步，机器人在现代社会中发挥着越来越重要的作用。
机器人的运动学与动力学是研究机器人运动和力学的重要分支，对于机器人的设计和控制具有重要意义。
通过运动学与动力学分析，可以深入探讨机器人的运动规律、力学特性以及动作控制等方面的问题。
首先，机器人运动学分析是研究机器人运动规律和姿态变化的学科。
在机器人的运动学分析中，我们可以通过分析机器人的关节角度和运动变换方程来描述机器人末端执行器的位置与姿态。
运动学分析可以帮助我们了解机器人在不同关节角度下的工作空间范围、姿态变化以及机器人末端执行器的运动轨迹等信息。
这些信息对于机器人的路径规划、避障以及动作控制等方面具有重要意义。
其次，机器人的动力学分析是研究机器人运动过程中受到的力学特性和动态响应的学科。
在机器人的动力学分析中，我们可以研究机器人的惯性特性、组成部分的质量分布以及施加给机器人的外部力和力矩等。
动力学分析可以帮助我们了解机器人系统的惯性特性、质量均衡以及机器人在外部力作用下的响应情况。
这对于机器人的平衡控制、力矩分配以及动作协调等方面具有重要意义。
在机器人运动学与动力学分析中，还涉及到机器人的运动控制问题。
运动控制是指通过对机器人的运动学和动力学特性进行分析，设计合适的控制方法来实现机器人的运动目标。
通过运动控制，我们可以使机器人在给定的轨迹下实现精确的位置和姿态控制，从而实现具体的任务需求。
运动控制的核心是设计合适的控制算法和机器人的执行机构，以实现机器人的动作执行和力学特性的优化。
机器人运动学与动力学分析的结果可以应用于多个领域。
在工业领域，机器人的运动学与动力学分析可以应用于自动化生产线和装配过程中的机器人操作控制，提高生产效率和质量。
在医疗领域，机器人的运动学与动力学分析可以应用于手术机器人的运动控制和手术操作，实现更精确和安全的手术过程。
在军事领域，机器人的运动学与动力学分析可以应用于无人作战系统和侦察机器人的运动规划和动作控制，提高军事作战的效率和准确性。
机器人运动学与动力学分析
机器人运动学与动力学分析机器人已经成为现代技术中的重要组成部分，它们能够执行各种任务，从生产制造到医疗护理。
要了解机器人的运动和控制，我们需要分析机器人的运动学和动力学。
一、机器人运动学分析机器人运动学研究机器人在空间中的位置和姿态随时间的变化规律。
通过机器人的构造，可以确定机器人的运动学特征。
在运动学分析中，我们主要关注以下几个方面：1. 机器人的自由度：机器人的自由度是指机器人在物理空间中能够独立移动的自由方向数量。
例如，一个平面上的二自由度机器人可以进行平移和旋转运动。
2. 机器人的位姿：机器人的位姿包括位置和姿态。
位置表示机器人在空间中的位置坐标，姿态表示机器人在空间中的朝向。
3. 运动学链模型：运动学链模型用于描述机器人的运动学结构。
它由连续的刚性骨链和可变的关节连接组成。
通过分析这些链条的长度和角度变化，可以确定机器人的位姿。
4. 正逆运动学问题：正运动学问题是指根据机器人的关节角度计算出机器人的位姿。
逆运动学问题是指根据机器人的位姿计算出机器人的关节角度。
机器人的运动学分析为我们提供了了解机器人的位置和姿态变化规律的基础。
二、机器人动力学分析机器人动力学研究机器人在运动过程中所受到的力和力矩的变化规律。
了解机器人动力学对于控制机器人的运动和保证机器人的稳定性非常重要。
在动力学分析中，我们主要关注以下几个方面：1. 运动学约束：机器人的运动受到多个约束条件限制，如关节限制、位置限制等。
这些约束条件对机器人的运动学和动力学分析都会产生影响。
2. 动力学链模型：动力学链模型用于描述机器人的动力学结构。
它包括机器人的质量、惯性矩阵和外部力矩。
通过分析链条间的力和力矩传递，可以推导出机器人的运动学和动力学方程。
3. 运动学和动力学方程：机器人的运动学和动力学方程描述了机器人在外部力矩作用下的运动规律。
运动学方程描述了机器人的位移和速度关系，动力学方程描述了机器人的加速度和力矩关系。
机器人的动力学分析为我们提供了了解机器人在运动过程中受到的力和力矩变化规律的基础。
机器人的运动学和动力学模型
机器人的运动学和动力学模型机器人的运动学和动力学是研究机器人运动和力学性质的重要内容。
运动学是研究机器人姿态、位移和速度之间关系的学科，动力学则是研究机器人运动过程中力的产生和作用的学科。
机器人的运动学和动力学模型可以帮助我们理解机器人的运动方式和受力情况，进而指导机器人的控制算法设计和路径规划。
一、机器人运动学模型机器人运动学模型是描述机器人运动方式和位置关系的数学表达。
机器人的运动状态可以用关节角度或末端执行器的位姿来表示。
机器人的运动学模型分为正运动学和逆运动学两种。
1. 正运动学模型正运动学模型是通过机器人关节角度或末端执行器的位姿来确定机器人的位置。
对于串联机器人，可以使用连续旋转和平移变换矩阵来描述机械臂的位置关系。
对于并联机器人，由于存在并联关节，正运动学模型比较复杂，通常需要使用迭代方法求解。
正运动学模型的求解可以通过以下几个步骤：(1) 坐标系建立：确定机器人的基坐标系和各个关节的局部坐标系。
(2) 运动方程描述：根据机器人的结构和连杆长度等参数，建立各个关节的运动方程。
(3) 正运动学求解：根据关节的角度输入，通过迭代计算，求解机器人的末端执行器的位姿。
正运动学模型的求解可以用于机器人路径规划和目标定位。
2. 逆运动学模型逆运动学模型是通过机器人末端执行器的位姿来确定机器人的关节角度。
逆运动学问题在机器人的路径规划和目标定位等任务中起着重要作用。
逆运动学求解的难点在于解的存在性和唯一性。
由于机器人的复杂结构，可能存在多个关节角度组合可以满足末端执行器的位姿要求。
解决逆运动学问题的方法有解析法和数值法两种。
解析法通常是通过代数或几何方法，直接求解关节角度，但是解析法只适用于简单的机器人结构和运动方式。
数值法是通过迭代计算的方式，根据当前位置不断改变关节角度，直到满足末端执行器的位姿要求。
数值法可以用于复杂的机器人结构和运动方式，但是求解时间较长。
二、机器人动力学模型机器人动力学模型是描述机器人运动时受到的力和力矩的模型。
机器人轨迹、路径的定义
机器人轨迹、路径的定义一、路径规划路径规划是机器人轨迹生成的核心环节，它根据机器人的目标位置和初始位置，结合各种约束条件（如速度、加速度、运动时间等），规划出一条从起始点到目标点的最优路径。
路径规划通常采用基于图论的方法、基于搜索的方法、基于插值的方法等。
二、速度规划速度规划是机器人轨迹生成的另一个重要环节，它根据机器人的运动状态和目标位置，结合各种约束条件（如最大速度、最大加速度、运动时间等），规划出一条合理的速度曲线，使得机器人能够以最优的速度到达目标位置。
速度规划通常采用基于函数插值的方法、基于搜索的方法等。
三、姿态规划姿态规划是机器人轨迹生成的重要环节之一，它根据机器人的运动状态和目标位置，结合各种约束条件（如姿态稳定性、最小能量消耗等），规划出一条合理的姿态曲线，使得机器人能够以最优的姿态到达目标位置。
姿态规划通常采用基于函数插值的方法、基于优化算法的方法等。
四、动力学模型动力学模型是机器人轨迹生成的基础，它描述了机器人运动过程中的力学特性，包括机器人质心位置、惯性参数、关节阻尼系数等。
通过建立动力学模型，可以实现对机器人运动过程的精确描述，从而为轨迹生成提供依据。
五、传感器信息传感器信息是机器人轨迹生成的另一个重要环节，它包括机器人自身携带的传感器信息（如陀螺仪、加速度计等）和外部传感器信息（如激光雷达、摄像头等）。
通过获取传感器信息，可以实现对机器人周围环境的感知和理解，从而为轨迹生成提供更多的信息和依据。
六、控制策略控制策略是机器人轨迹生成的重要环节之一，它根据机器人的运动状态和目标位置，结合各种约束条件（如控制精度、稳定性等），采用合适的控制算法实现对机器人的控制。
常用的控制算法包括PID控制算法、模糊控制算法、神经网络控制算法等。
七、反馈机制反馈机制是机器人轨迹生成的重要环节之一，它根据机器人的实际运动状态和目标位置的差异，对机器人的运动过程进行调整和修正，以保证机器人能够精确地按照预设的轨迹运动。
机器人-动力学-轨迹
硕士学位论文晶圆传输机器人振动分析与轨迹控制(应用型)VIBRATION ANALYSIS AND TRAJECTORY CONTROL FOR WAFER TRANSFER ROBOTXXXX大学2011年6月国内图书分类号：TP242.2 学校代码：10213 国际图书分类号：681 密级：公开工学硕士学位论文晶圆传输机器人振动分析与轨迹控制(应用型)硕士研究生：XX导师：XX申请学位：工学硕士学科：机械电子工程所在单位：机电工程学院答辩日期：2011年6月授予学位单位：XX大学Classified Index: TP242.2U.D.C: 681Dissertation for the Master Degree in EngineeringVIBRATION ANALYSIS AND TRAJECTORY CONTROL FOR WAFERTRANSFER ROBOT(Applied)Candidate：XXSupervisor：XXAcademic Degree Applied for：Master of Engineering Speciality：Mechatronics Engineering Affiliation：School of Mechatronics Engineering Date of Defence：June, 2011Degree-Conferring-Institution：XXXX大学工学硕士学位论文摘要集成电路(IC)产业是电子信息业的基础，是推动国民经济发展的高新技术之一。
IC制造装备业的快速发展要求晶圆传输机器人高速运动，从而增加了机器人运动过程中对其支撑框架的冲击。
另一方面，IC生产线的特殊性对振动提出了更高的要求。
针对晶圆传输机器人高速运动时产生的对其支撑框架的冲击问题，结合国家科技重大专项——用于光刻机的硅片传输机械手关键技术攻关及样机研制(项目编号为2009ZX02012-002)，本课题深入讨论了影响机器人振动的各种因素，并从机器人结构和轨迹控制两个角度提出了减小振动的方法。
机器人学中的动力学
机器人学中的动力学机器人学是研究制造、设计和运动控制机器人的学科，广泛应用于工业、医疗保健、国防、探险等领域。
机器人学中的动力学是机器人运动学的重要分支，掌握机器人运动学对于设计、控制机器人运动具有重要意义。
动力学的概念机器人学中的动力学是研究机器人运动的力学学科。
它主要关注如何对机器人的运动进行描述和控制。
机器人动力学包括机器人运动学和机器人力学的研究。
机器人运动学研究机器人的位置和位姿，而机器人力学研究机器人的力学特性和力学运动方程。
机器人学中的动力学主要涉及以下几个方面：- 机器人的运动轨迹和速度规划- 机器人的动力学建模和仿真- 机器人的力学特性和控制机器人的运动轨迹和速度规划机器人的运动轨迹和速度规划是机器人动力学的基本问题。
机器人的运动轨迹是机器人在空间中的运动路径，可以用各种运动学和动力学方法进行描述。
机器人的速度规划通常是在已知机器人的运动轨迹的条件下，确定机器人的运动速度以及加速度和减速度的大小和方向。
机器人的运动轨迹和速度规划在机器人控制中占据着重要的地位。
机器人的控制主要目的是使机器人完成特定的任务，如在制造车间中装配零件等。
在完成这些任务时，机器人需要根据任务的要求确定运动轨迹和速度规划，这样才能在短时间内完成高效的操作。
机器人的动力学建模和仿真机器人的动力学建模是机器人学中难点之一。
一个好的机器人动力学模型必须考虑机器人本身的特性和运动机理。
机器人的动力学模型可以用数学公式或者计算机模拟的方法进行描述。
此外，机器人的动力学模型需要考虑机器人的各种运动方式，如旋转、直线运动等。
机器人的仿真是指利用计算机模拟机器人运动状态和行为的过程。
机器人的仿真可以对机器人的运动轨迹、速度规划和控制逻辑进行模拟和测试，从而为机器人的设计和使用提供依据。
机器人仿真是一种低成本、高效率的机器人研究方法。
机器人的力学特性和控制机器人的力学特性和控制主要研究机器人在行动中的力学特性和控制方法。
机器人的力学特性包括机器人的质量、惯性、摩擦和发热等。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
硕士学位论文晶圆传输机器人振动分析与轨迹控制(附录)VIBRATION ANALYSIS AND TRAJECTORY CONTROL FOR WAFER TRANSFER ROBOTXXXX大学2011年6月目录目录第1部分模型验证与评估 (1)1.1初始条件确定 (1)1.2MATLAB算法实现 (2)1.3ADAMS仿真 (7)第2部分振动分析与轨迹控制 (12)2.1振动分析 (12)2.1.1机器人结构对振动影响 (12)2.1.2电机运动对振动的影响 (12)2.2轨迹控制 (17)2.2.1引言 (17)2.2.2无插补运动程序 (18)2.2.3定步长插补运动程序 (21)2.2.4定时插补运动程序 (27)第3部分实验验证 (35)3.1实验系统搭建 (35)3.1.1硬件连接 (35)3.1.2软件程序 (36)3.2实验原始数据 (86)XX 大学工学硕士学位论文(附录)第1部分模型验证与评估1.1初始条件确定本部分将根据动力学推导公式，利用MATLAB 进行动力学计算，然后与ADAMS 仿真进行对比。
首先确定模型中各已知条件。
机器人模型中各已知量见表1-1所示。
表1-1机器人模型已知量杆件i坐标系参数杆件参数d (mm)i L (mm)i l (mm)i m (kg)i 2zz I (kg m )i 00024.0701169.30－7.5600－257.807.7 5.562228.0210-⨯3284.518542.1 1.705921.6610-⨯4-4.318519.8 1.371221.0810-⨯547.82900 1.198521.2810-⨯机器人初始位置，大臂、小臂、末端手均与前一杆件的Z 轴成120°。
即：23παβγ===(1-1)设定三个电机加速度曲线，如图1-1所示。
XX大学工学硕士学位论文(附录)图1-1三个电机加速度曲线根据以上条件，编写MATLAB算法程序，同时利用ADAMS建立虚拟样机，输入同样的加速度曲线。
1.2MATLAB算法实现利用MATLAB计算动力学公式，算法实现过程如下：图1-2MATLAB算法流程图MATLAB程序如下：m0=24070.4e-3;m1=7560.0e-3;m2=5562.2e-3;m3=1705.9e-3;m4=1371.2e-3;m5=1198. 5e-3;L4=185e-3;L5=185e-3;L6=290e-3;lx3=80.1180e-3;lx4=75.4419e-3;lx5=81.3457e-3;lz3=7.65e-3;lz4=42.1449e-3;lz5=19.7 8e-3;d1=69.3e-3;d2=57.8e-3;d3=284.5e-3;d4=-4.344e-3;d5=47.84e-3;g=9.8;S=0.01;I2=8.0592E-03;I3=1.6571E-02;I4=1.0807E-02;I5=1.2786E-02;Pc(3,5)=0;Pc(1,:)=[0,0,lx3,lx4,lx5];Pc(3,:)=[0,0,lz3,lz4,lz5];%质心矩阵JR=4.3019e-005;JT=2.1774e-004;JZ=1.3031E-010;%三个电机转子转动惯量m=[m1,m2,m3,m4,m5];%质量矩阵d=[d1,d2,d3,d4,d5,0];I=[0,I2,I3,I4,I5];%常量矩阵P(3,6)=0;P(3,:)=d;P(1,:)=[0,0,0,L4,L5,L6];%各关节相对于前一关节的位置坐标iR=100;iT=50;%R、T轴减速比R(3,3,6)=0;R(:,:,1)=[1,0,0;0,1,0;0,0,1];R(:,:,6)=[1,0,0;0,1,0;0,0,1];%各关节相对于前一关节的姿态坐标w2(3,5)=0;w3(3,5)=0;%5个关节角速度和角加速度v3(3,5)=0;%5个关节加速度vc3=[0,0,0]';%临时质心加速度XX大学工学硕士学位论文(附录)fc(3,5)=0;nc(3,5)=0;%5个杆件质心力、力矩dt=0.001;t=0:dt:3;N=length(t);qI(3,N,5)=0;%各关节输入转矩q(3,N,5)=0;%各关节输出转矩f(3,N,6)=0;n(3,N,6)=0;%5个关节的力和力矩，第6列为末端手受力f0(3,N)=0;n0(3,N)=0;%框架力和电机力矩Kt=0.92e004;Kr=1.47e004;%谐波减速器的刚度，资料查得%设置的刚度和阻尼K2=9e3;B2=10;%T轴柔性关节刚度和阻尼K3=1e3;B3=10;%R轴柔性关节刚度和阻尼I2t(N)=0;I3t(N)=0;%柔性关节负载转动惯量瞬时值for k=1:N;%R轴电机运动（0.8-1.5，1.5-2.2）%R轴转动-94.5度，电机转动-94.5*50度，行程334mmif t(k)<0.8;qI(3,k,3)=0;elseif t(k)-0.8<0.1&&t(k)>=0.8;qI(3,k,3)=-(t(k)-0.8)*60*pi;elseif(t(k)-0.8)<0.25&&(t(k)-0.8)>=0.1;qI(3,k,3)=-6*pi;elseif(t(k)-0.8)<0.45&&(t(k)-0.8)>=0.25;qI(3,k,3)=(t(k)-0.8)*60*pi-21*pi;elseif(t(k)-0.8)<0.6&&(t(k)-0.8)>=0.45;qI(3,k,3)=6*pi;elseif(t(k)-0.8)<0.7&&(t(k)-0.8)>=0.6;qI(3,k,3)=-(t(k)-0.8)*60*pi+42*pi;%R轴转动94.5度，电机转动94.5*100度，行程-334mmelseif t(k)-1.5<0.1&&t(k)>=1.5;qI(3,k,3)=(t(k)-1.5)*60*pi;elseif(t(k)-1.5)<0.25&&(t(k)-1.5)>=0.1;qI(3,k,3)=6*pi;elseif(t(k)-1.5)<0.45&&(t(k)-1.5)>=0.25;qI(3,k,3)=-((t(k)-1.5)*60*pi-21*pi);elseif(t(k)-1.5)<0.6&&(t(k)-1.5)>=0.45;qI(3,k,3)=-6*pi;elseif(t(k)-1.5)<0.7&&(t(k)-1.5)>=0.6;qI(3,k,3)=(t(k)-1.5)*60*pi-42*pi;XX大学工学硕士学位论文(附录) elseqI(3,k,3)=0;end%T轴电机运动（0-0.8,2.2-3）%T轴转动228度，T轴电机转动228*50度if t(k)<0.1;qI(3,k,2)=t(k)*800*pi/3;elseif t(k)<0.2&&t(k)>=0.1;qI(3,k,2)=(0.2-t(k))*800*pi/3;elseif t(k)<0.6&&t(k)>=0.2;qI(3,k,2)=0;elseif t(k)<0.7&&t(k)>=0.6;qI(3,k,2)=(0.6-t(k))*800*pi/3;elseif t(k)<0.8&&t(k)>=0.7;qI(3,k,2)=(t(k)-0.8)*800*pi/3;elseif t(k)<2.2&&t(k)>=0.8qI(3,k,2)=0;%T轴转动-228度，T轴电机转动-228*50度elseif t(k)-2.2<0.1&&t(k)>=2.2;qI(3,k,2)=-(t(k)-2.2)*800*pi/3;elseif t(k)-2.2<0.2&&t(k)-2.2>=0.1;qI(3,k,2)=-(0.2-(t(k)-2.2))*800*pi/3;elseif t(k)-2.2<0.6&&t(k)-2.2>=0.2;qI(3,k,2)=0;elseif t(k)-2.2<0.7&&t(k)-2.2>=0.6;qI(3,k,2)=-(0.6-(t(k)-2.2))*800*pi/3;elseif t(k)-2.2<0.8&&t(k)-2.2>=0.7;qI(3,k,2)=-((t(k)-2.2)-0.8)*800*pi/3;elseqI(3,k,2)=0;end%Z轴电机运动（0-8，2.2-3）%Z轴转动1728度，上升48mmif t(k)<0.2;qI(3,k,1)=-t(k)*600*pi;elseif t(k)<0.6&&t(k)>=0.2;qI(3,k,1)=(t(k)-0.4)*600*pi;elseif t(k)<0.8&&t(k)>=0.6;qI(3,k,1)=-t(k)*600*pi+480*pi;XX大学工学硕士学位论文(附录)elseif t(k)<2.2&&t(k)>=0.8;qI(3,k,1)=0;%Z轴转动-1728度，下降48mmelseif t(k)-2.2<0.2&&t(k)>=2.2;qI(3,k,1)=(t(k)-2.2)*600*pi;elseif(t(k)-2.2)<0.6&&(t(k)-2.2)>=0.2;qI(3,k,1)=-((t(k)-2.2)-0.4)*600*pi;elseif(t(k)-2.2)<0.8&&(t(k)-2.2)>=0.6;qI(3,k,1)=(t(k)-2.2)*600*pi-480*pi;elseqI(3,k,1)=0;end%三个周的速度和位移处理if k==1;qI(2,k,:)=0;qI(1,k,:)=0;elseqI(2,k,:)=qI(2,k-1,:)+qI(3,k,:)*dt;qI(1,k,:)=qI(1,k-1,:)+qI(2,k,:)*dt;end%柔性处理if k==1;q(:,k,2)=0;q(:,k,3)=0;elseq(3,k,2)=(K2*(qI(1,k-1,2)-q(1,k-1,2))+B2*(qI(2,k-1,2)-q(2,k-1,2)))/I2t(k-1);q(2,k,2)=q(2,k-1,2)+q(3,k,2)*dt;q(1,k,2)=q(1,k-1,2)+q(2,k,2)*dt;q(3,k,3)=(K3*(qI(1,k-1,3)-q(1,k-1,3))+B3*(qI(2,k-1,3)-q(2,k-1,3)))/I3t(k-1);q(2,k,3)=q(2,k-1,3)+q(3,k,3)*dt;q(1,k,3)=q(1,k-1,3)+q(2,k,3)*dt;endI3t(k)=I3+I4+I5+m3*lx3^2+m4*((L5-lx4)^2*sin(7*pi/6+q(1,k,3))^2+...4*L4^2*cos(7*pi/6+q(1,k,3))^2)+m5*(2*L4*cos(7*pi/6+q(1,k,3))+lx5)^2;I2t(k)=I2+I3t(k);q(:,k,4)=-2*(q(:,k,3));q(:,k,5)=(q(:,k,3));%3、4关节与R轴关系q(:,k,1)=qI(:,k,1);%N_E计算XX大学工学硕士学位论文(附录)P(:,1)=[0,0,d(1)+S*q(1,k,1)/(2*pi)]';%第一个坐标系位置R(:,:,2)=[cos(q(1,k,2)),-sin(q(1,k,2)),0;sin(q(1,k,2)),cos(q(1,k,2)),0;0,0,1];R(:,:,3)=[cos(q(1,k,3)+7*pi/6),-sin(q(1,k,3)+7*pi/6),0;sin(q(1,k,3)+7*pi/6),cos(q(1,k,3) +7*pi/6),0;0,0,1];R(:,:,4)=[cos(q(1,k,4)+2*pi/3),-sin(q(1,k,4)+2*pi/3),0;sin(q(1,k,4)+2*pi/3),cos(q(1,k,4) +2*pi/3),0;0,0,1];R(:,:,5)=[cos(q(1,k,5)+pi/3),-sin(q(1,k,5)+pi/3),0;sin(q(1,k,5)+pi/3),cos(q(1,k,5)+pi/3),0; 0,0,1];for j=1:5;if j==1;w2(:,j)=[0,0,0]';w3(:,j)=[0,0,0]';v3(:,j)=[0,0,-g+S*q(3,k,j)/(2*pi)]';elsew2(:,j)=w2(:,j-1)+[0,0,q(2,k,j)]';w3(:,j)=w3(:,j-1)+[0,0,q(3,k,j)]';v3(:,j)=R(:,:,j)'*(v3(:,j-1)+cross(w3(:,j-1),P(:,j))+cross(w2(:,j-1),cross(w2(:,j-1),P(:,j))));endvc3=v3(:,j)+cross(w3(:,j),Pc(:,j))+cross(w2(:,j),cross(w2(:,j),Pc(:,j)));fc(:,j)=m(j)*vc3;nc(:,j)=I(j)*w3(:,j)+cross(w2(:,j),I(j)*w2(:,j));endfor h=5:-1:1;f(:,k,h)=R(:,:,h+1)*f(:,k,h+1)+fc(:,h);n(:,k,h)=R(:,:,h+1)*n(:,k,h+1)+nc(:,h)+cross(Pc(:,h),fc(:,h))+cross(P(:,h+1),R(:,:,h+1)*f (:,k,h+1));end%n(3,k,3)=n(3,k,3)+I3t(k)*q(3,k,3);%柔性处理%n(3,k,2)=n(3,k,2)+I2t(k)*q(3,k,2);%柔性处理f0(:,k)=[f(1,k,1),f(2,k,1),f(3,k,1)-m0*g]';%框架受到的三个方向上力n0(:,k)=[iR*JR*qI(3,k,3)+(n(3,k,3)-2*n(3,k,4)+n(3,k,5))/iR,iT*JT*qI(3,k,2)+n(3,k,2)/i T,JZ*q(3,k,1)-S*f(3,k,1)/(2*pi)]';%R、T、Z三个周的电机力矩end%框架力和三个轴力矩曲线subplot(3,1,1),plot(t,f0(1,:));subplot(3,1,2),plot(t,f0(2,:));subplot(3,1,3),plot(t,f0(3,:));% subplot(3,1,1),plot(t,n0(1,:));subplot(3,1,2),plot(t,n0(2,:));subplot(3,1,3),plot(t,n0(3,:));XX大学工学硕士学位论文(附录)1.3ADAMS仿真利用SolidWorks建立机器人模型，装配好各零件，其中为了仿真简单，去掉不必要的螺栓，并将螺栓质量等效到机器人装配体中。