多自由度机械臂控制算法设计..

1、摘要机器人是一种能够进行编程并在自动控制下执行某些操作和移动作业任务的机械装置。而机械臂作为机器人最主要的执行机构，是一个十分复杂的多输入多输出非线性系统,它具有时变、强耦合和非线性的动力学特征，因其控制的复杂性引起了相关从业人员的广泛关注。随着时代的进步，像军事制造、工业生产、日常生活及教育娱乐等各个领域对机器臂控制技术应用需求逐渐加大，从而使得设计一套工作空间大，运动灵活的多自由度机器臂尤为重要。机械手臂运行轨迹追踪控制技术有包括：adaptivecontrol（自适应控制）、smvsc（滑模变结构控制）、Robustadaptivecontrol（鲁棒自适应控制）、Fuzzyadapti

2、ve（模糊自适应）等四大类。本文主要运用模糊PID控制设计二自由度机械臂控制算法，该控制方法具有模糊控制灵活和适应性强的优点，也具有经典PID控制精度高的特点。本文围绕二自由度机械臂控制算法设计，首先建立二自由度关节型机械臂的数学模型，即二自由度机械臂输入驱动力矢量和输出转动角度矢量之间的函数关系。然后运用模糊PID控制设计一套机械臂轨迹规划算法，能够根据使用者的作业任务要求，求出二自由度机械臂终端执行器的轨迹。并研究如何对于给定的系统设计出PID控制器，实现控制系统的输出对参考输入跟踪，以及对扰动输入响应具有较小的振幅，且能够衰减到零即无稳态误差。最后给出了基于MATLAB/SIMULINK

3、软件的案例分析，阐释模糊PID控制算法行之有效性。关键词：多自由度，机械臂，PID算法控制，数学模型第I页，注讼罟毕业设计（论文）专用纸AbstractRobotisakindofprogrammingandperformcertainoperationsandmobiletaskmechanisminautomaticcontrol.Androbotarmasthemainexecutivebody,isaverycomplexmultiinputandmultioutputnonlinearsystem,ithasatime-varying,strongcouplingandnonline

4、ardynamiccharacteristics,duetothecomplexityofthecontrolcausedwideattentionofpractitioners.Withtheprogressofthetimes,likemilitarymanufacturing,industrialproduction,dailylifeandentertainment,educationandotherfieldsofarobotarmcontroltechnologyapplicationrequirementsgraduallyincrease,fromthedesignalarge

5、workingspace,theflexiblemovementofthemultidegreeoffreedomrobotarmisparticularlyimportant.Mechanicalarmtrajectorytrackingcontroltechnologyincluding:adaptivecontrol,SMVSC,robustadaptivecontrol,fuzzyadaptiveetc.Inthispaper,theuseoffuzzyPIDcontroldesignfortwodegreeoffreedommanipulatorcontrolalgorithm,th

6、econtrolmethodwithfuzzycontrolofaflexibleandadaptableadvantages,alsohastheclassicPIDcontrolthecharacteristicsofhighprecision.Thispaperfocusesonthedesignofcontrolalgorithmoftwodegreesoffreedommanipulator,amathematicalmodeloftwo-DOFManipulator,namelytwo-DOFManipulatordrivingforceandrotationanglebetwee

7、ntheoutputfunction.ThenusethecontroltodesignamanipulatortrajectoryplanningarithmeticoffuzzyPIDmethod,accordingtotheuser'staskrequirementsandfortwodegreesoffreedommanipulatorendeffectortrajectory.AndstudyhowtodesignasystemforPIDcontrollerisgiven,toachievetheoutputofthecontrolsystemofthereferencei

8、nputtracking,anddisturbanceinputresponseamplitudeissmaller,andcandecaytozeronosteadystateerror.Finally,acaseanalysisbasedonMATLAB/SIMULINKsoftwareispresentedtoillustratetheeffectiveofPIDfuzzycontrolalgorithm.KeyWords:Multidegreeoffreedom,manipulator,control,PIDalgorithm,mathematicalmode目录摘要IAbstract

9、II1 绪论III1.1 本文研究的目的与意义11.2 机械臂控制算法研究现状11.3 本论文的工作总结32 机械臂控制系统概述42.1 二自由度机械臂的动力学控制模型42.2 二自由度机械臂运动学正解52.3 二自由度机械臂运动学中的反解72.3.1 运动学反解的不唯一性82.3.2 在运动学反解中出现的个别情形93 控制算法设计103.1 PID的概述103.2模糊PID控制123.3二自由度运动路径规划133.3.1 直角坐标空间中规划算法133.3.2 直线插补和圆弧插补算法163.4 模糊PID控制算法设计174 算例分析204.1二自由度机械臂PID模糊控制器的建模204.1.1

10、模糊PID控制204.1.2 PID模糊建模214.1.3 PID模糊控制224.2设计模糊控制器规则以及其仿真分析224.2.1 模糊控制器规则22第III页乩毕业设计（论文）专用纸4.2.2 PID参数的模糊整定254.3 仿真结果分析30总结32致谢33参考文献34第IV页，注讼罟毕业设计(论文)专用纸1绪论1.1 本文研究的目的与意义机器人是一种能够进行编程并在自动控制下执行某些操作和移动作业任务的机械装置。机械臂作为机器人最主要的执行机构，对它的研究越来越受到工程技术人员的关注。它涉及的学科有材料科学、控制技术、传感器技术、计算机技术、微电子技术、通讯技术、人工智能、仿生学等等很多学

11、科。一个机械臂系统主要包括机械、硬件和软件、算法这四个部分。到具体设计需要考虑结构设计、控制系统设计、运动学分析、动力学分析、轨迹规划研究、路径规划研究、运动学动力学仿真等部分。对于一套轻便型机械臂的研发，需要把各个部分紧密联系，互相协调设计。随着时代的进步，机器臂技术的应用越来越普及，已逐渐渗透到军事、航天、医疗、日常生活及教育娱乐等各个领域。目前实际应用的绝大多数机器臂都是固定在基座上的，它们只能固定在某一位置上进行操作，因而其应用范围多限于工业生产中的重复性工作。于是实际生产生活中迫切需要一种活动空间大，能适用于各种复杂环境和任务的可移动机器人。由于移动机器人工作空间大、运动灵活等优点，

12、对它们的研究也是越来越多，但是这种机器人很多都是实现移动的，并没有可控制的手臂，所以没有抓取物体的功能。为了让移动机器人能够完成简单的作业，在它上面安装两只轻型服务型机械臂显的尤其必要。1.2 机械臂控制算法研究现状最早的机械臂是1962年美国联合公司制造的名为Unimate的机械臂，该系统的设计参照坦克塔台，其应用于将一些配件传送到生产线，其控制系统是一个大型计算机。在文献8中，LiuXinjun等人对二自由度并联机器人的运动学和动力学进行建模分析，并对二自由度并联机器人的综合性能做了深刻探讨，并提出新的研究方法。KimJY对二自由度五连杆机械臂进行运动学的研究分析9。在对机械臂实现控制时，

13、控制器的设计过程采用无模型的控制思想，例如可以通过神经网络滑模变结构控制(NN-SMVSC)等方法实现，即能够保障控制器的效率，同时，又能避免惯量矩阵或逆矩阵的计算10。在文献11中，Su考虑了在执行器约束的情况下，针对两关节机械臂，研究了机械臂的全局输出反馈整定问题，并结合PID控制与补偿措施方法，通过Lyapunov方法证明了闭环系统的全局渐近稳定性。Liuzzo等I2提出了一种不依赖于模型的控制器，该控制器在对机械臂实现控制时，仅依赖于输入的周期信号值及机械臂的动力学的常数界限。通过对每个关节的输入信号进行傅立叶级数展开，判断控制系统的全局稳定性和局部稳定性。当给定的输入信号的傅立叶级数

14、展开有界时，可以获得全局稳定和局部指数稳定的误差动力学，使跟踪轨迹的误差达到任意精度。同时，自适应PID瓜控制器通过辨识输入信号的傅立叶系数来学习输入信号。Purwar等13超出了Chebyshev自适应神经元控制器，该控制器的设计主要考虑了机械臂执行器的约束，并估计系统负载变化、未知非线性LJ及带干扰的输入力矩等结构化或非结构化的不确定性。该方法设计的控制器避免了速度信号通过位置微分获得而容易掺入噪声的缺点，只依赖于关节位置信息，能够实现神经网络逼近与动态滤波器的融合。文献14主要针对系统在具有结构不确定性和非结构不确定性时，提出一种模糊自适应控制方法，并对机械臂的轨迹跟踪控制进行实验。当机

15、械臂的关节速度不可测时，引入速度观测器，设计了自适应输出反馈控制器。通过研究机械臂的动力学特性，并对不确定性项进行解稱，就能够降低模糊控制器模糊规则的数目，简化控制器。当机械臂的关节速度可测时，通过设计全状态模糊自适应反馈控制器，可保证系统闭环动力学稳定。国内的许多单位自从上世纪80年代开始，也开始重点研究机械臂的控制系统。例如，清华大学、浙江大学、沈阳自动化研究所及上海交通大学等学校或研究所在国家高新技术计划自动化领域智能机器人专题中做了很好的带头作用，且收获颇多15-16，促进了国内对机械臂的研究。在文献17中，王启明和汪劲松对二自由度的并联机器人操作臂进行了运动学和动力学建模，并进行仿真

16、。在文献18中，北京工业大学的刘善增对平面二自由度并联机器人进行了动力学设计研究，但是并没有对并联机器人作控制研第3页，注讼罟毕业设计（论文）专用纸究。陈国栋等19，在对机器人的研究中，采用经过滤波器作用的位置误差信号作为反馈信号，并用参考速度来替代非线性部分输出端的实际速度信号，仿真结果中取得了满意的跟踪效果，该文献的缺点是该方法必须基于精确的动力学模型。目前国内已提出很多抑制抖振的方法20，例如，滑模控制通常与模糊控制、神经网络控制等方法相结合，来消除机械臂控制输入的抖振现象。新的滑模面及控制方式也被不断提出并应用到机械臂轨迹跟踪控制领域。1.3本文主要工作内容本论文第二章主要就是构建二自

17、由度机械臂动力学控制模型，首先详细分析二阶机械臂运动学解，定义各个目标变量，然后寻找输入机械臂驱动力矩矢量和输出机械臂转动矢量的数学约束关系。第三章主要就是介绍二自由度机械臂控制算法设计，详细介绍了PID控制和模糊PID控制的基本理论，比较二者优缺点，以及各自适宜的情况和约束条件。第四章主要就是叙述基于MATLAB/SIMULINK软件的案例分析,在Simulink仿真环境下搭建完整控制系统得到给定轨迹曲线、跟踪轨迹曲线、以及误差轨迹曲线。对仿真结果和误差来源进行分析，得出结论，并阐释模糊PID控制方法的有效性和可行性。第4页2机械臂控制系统概述因为机械臂随着自由度个数的增大，控制系统的复杂程

18、度加大，但是其基本原理类似，所以本文主要以二自由度机械臂为例阐释多自由度的基本原理，2.1 二自由度机械臂的动力学控制模型机械臂的运动方式在运动空间上来分一般而言有两大类，即关节型和直角型。各个关节的的运动直接取决于它的运动坐标，把全部的关节变量设为一个关节矢量，全部的关节矢量组成的集合则为它的关节空间。通过控制所有关节的移动来控制机器人的移动便为关节空间运动模式。机械臂终端对象的具体地点和角度一般在直角坐标空间中表现出来，这种模式就是称为直角坐标运动模式，在这种运动模式中，机械臂用户规定的任务是由机械臂末端对象在直角坐标空间中的移动来实现的。就操作者而言，直角坐标空间更为人们接受，故而在直角

19、坐标空间中进行对机械臂的操作。因此我们就要在关节坐标和直角坐标之间建立一一对应的数学约束关系。换言之，如知道机械臂每一个关节的坐标常数，就需要求解它的终端在直角坐标空间中的具体位置坐标，这个被叫做为运动学正解;反之，如知道它末端在直角坐标空间的坐标就需要求解各个运动关节的坐标参数，这个则被称为运动学反向解。设q二qi,%,和为机械臂在关节坐标空间中的变量，X-比y，Z，0为它的终端对象在直角坐标空间中的具体位置坐标，那么它们两个之间的关系为:F（x，q）二0（2.1）上式是一个隐式方程，若能够从中求解出:x二f（q）（2.2）那么直角坐标变量由关节坐标变量来定量表达，就能够得到机械臂的运动学正

20、解。通常而言，可以从式（2.1）得到它的的惟一正解，得到的正解为:q二g（x）（2.3）二：毕业设计（论文）专用纸然而一般情况下，我们不容易获得它的运动学反解，是因为它的反解通常就是多解，所以在现实情况下，通常运用几何机械臂的运动学解。2.2二自由度机械臂运动学正解已知:关节1连杆长度1，关节的值为qi（如下图所示）；关节2连杆长度12,关节的值为q2（角度如下图所示）求解:记下图中关节连杆末端对象固定点的直角空间坐标：X二兀y第6页机械臂操作端的位置方程为:P=1cosq+1cos(q+q)x11212P=1sinq+1sin(q+q)y11212P.卜J叫;2.4)2.5)上述方程的Jac

21、obian矩阵JG）为-1sin6-1sin+6)112121cos6+1cos6+6丿11212-1sin21 cos2in+6'+6)12机械臂的动力学方程为M（e馆+n6,0）+G）=T2.6)61为转动角度矢量;2其中，T=tTI为总的驱动力矩矢量，0=6121M0）为转动惯量矩阵，N0,0）为Coriolis离心转矩，G0）为重力矢量。M0）、;宀心毕业设计（论文）专用纸第9页N6,0）、G（0）的数学表达式如下:M0)=a+a/2丿cos0322a3a+acos0a+a.2)cos032，2一(asian0)G0+022212.2(asian0>02/2221N0,G

22、0)=其中1a=一m12+13111a=m421+mJt0J0)+mJt0')J0,00cos0+acos(0+0)11512acos(0+051+mJt0)g2.7)2.8)2.9)1m32212+m12,21=mll,212g11+mg1，21a=5a2mgl,m2221=15.19kg,m=11.36kg,2定义状态变量1=1=0.432m,m=10kg,g=IO)2x=0,x=0,x=0,1122|31X=Lxx-k,X=Lxx1212x4x3D(0)=ME(,0)=N(,0)-Gu二t=T1则机械臂模型的状态方程可以写成x1x2x3x4x3-o-xD-1(x)E(x)+D-1

23、(x)u11u2系统输出方程为y(X)=h(X)=h"P_h6)1xPyh(x)2iix1x21cosx+1cosx+x111/(12)1sinx+1smVx+x丿112129.8112.10)2.11)2.12)2.13)2.3二自由度机械臂运动学中的反解已知:记关节1连杆长度为11,关节2连杆长度为12,关节连杆末端的点在平面坐标空间的坐标为：x二兀y求解:记关节值为qi（角度如下图所示），关节值为q2（角度如下图所示）。图2.2机械臂运动学的反解如上图所示，在不考虑奇异点的情况下，得到如下公式:r二x2+y2（2.14）12+12rcosd=ii321112sind=,.

24、9;1一cos2d33sind11sind2.15)(2.16)2.17)d=atan2(sind,cosd)111(2.18)2.19)d=atan2(y,x)(2.20)2d=atan2(y1sinq),(x1cosq)31111(2.21)二：毕业设计(论文)专用纸求解可知运动学的反解计算公式:2.22)q=d-dJ121q-q241atan（x，y）是用来计算xy的反正切值（单位为弧度）的数学函数。从得知的运动学反解上来看，可以获得是非线性系统。2.3.1 运动学反解的不唯一性图2.3机械臂的两个反解它的运动学反解一般多解，会存在有好几组关节变量终端在用户要求的位置点。图2-3很好的反

25、应二自由度机械臂会在工作空间中运动时有两组运动学反解这一情况。综上所述，在实际操作中从反解中的多解问题找到一组最优解是一个重要问题。通常情形下可以用优化算法来处理这种多组反解问题。当设置此算法的优化准则时，可以选取的优化准则有两种情况: 当没有障碍物的时候，就用“最短”原理。由于机械臂的动作是持续存在的而非间断的，可以根据之前机械臂运动反解获得的关节坐标值，能够得到机械臂关节运动最短的解确定为反解，所以使之保持连续。 当有障碍物的时候，沿“最短”轨迹原则会发生碰撞，那么就综合选用机械臂的有障碍路径轨迹，算法等方法来得到反解。2.3.2在运动学反解中出现的个别情形当目标点位置(x,y)在机械臂运

26、动空间范围之外，就用下面的公式判断:12+12(X2+y2)A=_J2_211122.7)如果满足：2.8)就能够判断特定地点在机械臂的运动空间范畴之外，换句话就是说运动学反解无解，式(2.8)£为一无穷小正分数，譬如选取£=10-82.9)有一个特殊情况就是目标位置正好落在机械臂工作空间边界上，在这种情况下(奇异位置)，机械臂的移动情况就变得差了，控制的轴移动也许会发生跳变的情况。这种情况下奇异情况的判别条件为:A<(1£)2.10)图2.4反解中的特殊情况根据以上可得到在特殊情况的反解为:q=atan2(y,x)<iq=0(2.11)2第11页;宀

27、心毕业设计(论文)专用纸3.控制算法设计3.1 PID的概述从ziegler还有Nichols提出PID参数整定概念起，把PID控制器的自动和手动整定的思想理念用在诸多科学技术之上。并且PID控制为目前最常用的方法，被用于很多反馈控制亦或是其不大的变形控制。PID调节器跟它的优化型在工业的控制中最普遍。迄今为止，百分之八十四的依旧是单纯的PID调节器，然而优化型就包括在其中的就在百分之九十以上。PID控制器作为使用最广泛的控制器,为微分、比例、积分并联控制器。PID控制器的数学模型可以用下式表示:u(t)二Kpe(t)+1Je(t)dt+Tde(3.1)Tddti其中：u(t)一控制器的输出e

28、(t)一控制器输入，即误差信号。Kp一控制器的比例系数。Ti一控制器的积分时间。Td一控制器的微分时间。在PID控制器中，其数学模型由微分(D)、比例(P)、积分(I)三部分组成。这三部分分别是：比例部分数学式表示如下：KP-e(3.2)控制器的功能随着偏差的出现而出现，并且让其偏差朝向减小的方向变动。控制功能的程度由比例系数Kp决定。随着比例系数增大，那么过渡过程就会减短，伴随着控制结果的稳态误差也减小；然而Kp愈大，其超调量愈大，产生振荡的可能性加大，致使动态性能变坏的劣势，更有甚者会令其闭环系统不稳定。为了达到过渡时间少和稳态误差小的良好实验结果，比例系数Kp的择取是一定要非常合适。积分

29、部分数学表达式表示如下式所示:ipIe(t)dt(3.3)第14页根据式(3.3)可知，只要存在误差，控制功能一直积累，输出控制量是很难去消除误差当存在误差的时候。可知积分部分的功能是可以消除系统的误差。但因为它具有滞后性，会使得积分控制功能很强将使系统超调不能够往变小的方向变化，其动态机能变弱，甚至会造成闭环系统不稳定的情况。Ti对积分部分有着极大的控制能力，Ti愈大积分功能愈弱。这种情况下对于变小系统超调有着极大的优势，过渡过程很难产生振荡。却会使得消除误差所需时间增长。当i愈小积分功能愈强。这种情况下系统过渡过程中会产生振荡，使得消除误差所需的时间减少。 微分部分数学表达式表示如下:3.

30、4)kpTd如dt微分部分能够有效掌控误差的变动趋向，加大其控制功能能使系统更快地反应，震荡不变大，系统的稳定性增强，其劣势就是降低了系统抵制外界扰动的功能。微分时间Td长短决定微分部分的功能强弱。Td愈大它抑制e(t)变化的能力愈强；否则，Td愈小它反抗e(t)变化的能力愈弱。在数字控制的计算机系统中，计算机PID控制算法程序可以达到计算机数字控制器的掌控。通常而言，这种控制系统就是一种采样数据系统。并且在处理数字信号的问题中，均需要用数值计算来无限逼近。因此，PID控制规律的实现，一定要数值逼近的方法。若只有很短的采样周期，则利用积分被求和取代，差商取代微分，让PID算法使其离散化。表述连

31、续时间PID算法的微分方程，替换成表述离散时间PID算法的差分方程式，便是数字PID位置型算式，即式(3.5)：u(k)=Kpe(k)+TS丈e(i)+Tde(k)+e(k-1)(3.5)i=0TiTs式中:u(k)一k采样周期时的输出e(k)一k采样周期时的误差Ts一采样周期其中3.6)=KpTPTs即为u(k)=Ke(k)+K丈e(i)+Ke(k)+e(k-1)(3.7)PIDi=0其中Kp为比例系数，KI为积分系数，KD为微分系数3.2模糊PID控制在常规的二维模糊控制器中，其输入变量是偏差和输入变量的变化量。所以通常而言，就把这种控制器认为拥有Fuzzy比例和微分两种控制功能，但是缺乏

32、Fuzzy积分控制功能。然而，线性控制理论的积分控制功能是抵消稳定偏差，可是动态运行反应缓慢；比例控制功能动态响应快速；然而高的稳态精度以及快速的动态响应可由比例积分控制功能来获取。所以模糊控制器中增添了PI(PID)控制策略，从而组成Fuzzy-PI(或PID)复合型控制，除了让动静态性均能有可观的提高外，也有有动态响应的速度快、超调小、稳扰动偏差小的优点。确定PID参数是PID控制的重点部分，这种方法就是通过模糊控制来确定PID参数，即利用误差变化率ec和系统误差e。在线修改PID参数并且运用模糊控制规则。实现思想就是找到PID各个参数与误差变化率ec和误差e之间的联系，从实验之中一直检测

33、误差变化率ec和误差e。进行在线修改各个物理参数，从而可以符合在不同e和ec时满足对控制参数的不同用户要求。因此可以让控制对象有良机毕业设计(论文)专用纸好的动性能、静态性能、计算量小，易于用单片机实现的多种优势等。其原理框图如图3.1所示：图3.1模糊PID控制算法流程图3.3 二自由度运动路径规划在得到机械臂的运动学解的情况下，当它向目标轨迹运动时，需要设计一下它的运动路径。这里有直角坐标空间中的和关节空间中的路径规划这两种路径规划算法。3.3.1 直角坐标空间中规划算法关节值是终极控制着机械臂终端的移动，要是可以设计关节空间中的轨迹，那么就能有效阻止发生雅可比矩阵的奇异所造成的速度不受控

34、制的情况，还可以大大减少计算的时间。然而多数情况下，关节坐标空间跟直角坐标空间这两者非线性关系。因此能在关节空间中进行直接规划的就只能够是对路径没有要求的作业，像是连续弧焊的对运动路径要求较高的作业，就一定只能在直角坐标空间中设计。之后便是将设计获得的直角坐标空间中的轨迹序列，利用所对应反解算法换算解得，如下图所示。图3.2路径规划控制流程图有必要的话在用运动学求反解得到的关节变量空间序列来进行关节坐标空间中的路径插补。通常而言，运动控制器在执行运动轴伺服控制时，会进而对关节指定变量作插补，插补速率曲线采用的方式有两种，分别是梯形图形方式、s形图形方式。记在直角坐标空间中，二自由度机械臂终端对

35、象路径空间曲线方程为:x=x(t)<y=y(t)(3.8)z=z(t)上式中，记x,y,z分别表示机械臂在三维直角坐标空间中的位置坐标值，记t为时间。为方便实际当中的控制，通常会选择弧长参数表示的曲线表达式，则知道起点的初始值为s,的曲线函数为：x=x(s)+x(s)A<y=y(s)+y(s)Az=z(s)+z(s)A上式中S表示为弧长参数，由积分公式:3.10)syX(t)2+y(t)2+z(t)2dt得到t=t(s)，然后代入式(2.12)就可以得到式(2.13)所示曲线函数。记At为运动轨迹的更新时间，即每隔At时间就会有一个插值点生成，T为轨迹期望的运动时间，那么所得轨迹插

36、补序列的长度为:第16页乩毕业设计（论文）专用纸一n=At(3.11)当弧长给出的速率移动曲线段，每个插补弧长的增量As=s/n已知，就能够获得轨迹插补序列表达式:x=x(s)+x(kAs)kAVy=y(s)+y(kAs)(k=0,1,.,n)(3.12)kAz=z(s)+z(kAs)kA若在梯形速度曲线模式下（路径段是由减速、等速和加速构成），则使得加速度和减速度大小相等，加速和减速时间相同，如图2-6所示，取ta为加速和减速的时间。便能够得到的路径长度表达式为:s=a(込t)2；(0<i<1j23.13)1T-2ts=v(jAt)+at2;(0<j<一-a)2j丿2

37、a丿Ats=at2+v(一-2t)+a(kAt)2；(0<k<3j2aa2图3.3梯形模式的路径规划时间点不同时，遵循式子（3.13）路径的不同分段，将结果si代入式（3.9）,就能解出轨迹插补位置后序列的坐标值。在解出轨迹插补位置序列的坐标值后，会由于轨迹更新周期At短暂的原因在实际操作可能使用相邻位置序列点的差分近似来进行轨迹插补速度。3.3.2直线插补和圆弧插补算法在平面直角坐标系之中，二自由度机械臂只有X、Y的两维参数值。直线插补和圆弧插补这两种方法技巧在运动控制系统中采用最广泛，一些繁杂曲面可以由直线段和圆弧段近似逼近。本文会给出直线插补和圆弧插补技术在二自由度机械臂平面

38、直角坐标内的一个简单运用。 直线插补算法在平面二维直角坐标中，At为轨迹校正时间周期，T为轨迹理想的的运动时间，（，yo）为起始点，（Xi,”为指定点。在等速度规则之下，每一次插补弧长的进给量为山二a+小2，直线插补序列的长度为n=TXt，对于直线规划，等弧长进给等效于等Ax、Ay的进给，每一次插补X、Y方向的变化量:(3.10)直线位置插补序列为:=x+kAx0,(k=0,1,.,n)Iyk(3.11)=y+kAy0 圆弧插补算法如图2-7所示，需要在平面X-Y内作出一个从（xa，yA）到（xb，yB）、圆心角为的圆弧、r为半径、0以（xo,yo）为圆心。求圆弧插补位置序列。第18页;宀心毕

39、业设计(论文)专用纸记(Xa，乙)为起点，它所对应的圆心角为°0,当插值序列长度为n,每个插值圆心角的增加值为"=n，则圆弧插值序列的方程式为:x=x+rcos(°+kA0)彳k003.12)y=y+rsin(°+kA°)k003.4模糊PID控制算法设计piD控制器发展到现在已有五六十年的时间，由于很好理解，不需要详细的系统模型等条件，并且仍然是使用最广泛。使用积分(I),微分(D)和比例(P)通过线性结合来组成控制量对被控对象进行操控的方法，就称之为PID控制。当计算机开始用在控制部分时，拥有用系统软件的方法达到完成PID控制算法，和计算机

40、的逻辑计算作用，从而让PID控制算法越发灵活适于用户需要，便是数字PID控制。假定值r(t)与输出反馈值C(t)的误差error(t)=沏-yout(t)按积分(I)、比例(P)、微分(D)运算后，经过线性组合组成控制量yout(t)，对控制对象进行操控。如图3.2所示：图3.5PID控制原理记给定值rin(t)与现实中输出值yout(t)组成控制误差公式为：(3.13)error(t)二rin(t)-yout(t)PID的控制规律为：u(t)=k(error(t)+Jterror(t)dt+TDderror(t)3.14)PT0dtl写成传递函数的形式：3.15)G(s)二U(S)=k(1+

41、丄+Ts)E(s)PTsDlPID控制器的校正情况的功能如下： 比例环节：kp按照比例地反映控制系统的误差信号error(t)，控制器的控制功能随误差的出现而出现，从而能够减小误差。 积分环节：用于抵消静差带来的影响，从而能够使得系统的准确度上升。积分时间常数T的长短被积分功能的性能好坏决定，T越大积分功能越弱，倒过来就越强。 微分环节：TD用来表示误差信号的变动，还会在误差信号变化很大时，在系统中添加有用的提前修正信号，用来使系统的响应速率大大变得快速，那么会使调节时间大大减少。下面用典型二阶系统单位阶跃响应的误差曲线进行分析如下：第20页二：毕业设计（论文）专用纸图3.6二阶单位阶跃响应从

42、误差曲线看出： 当误差间的值较大时，即误差的绝对值较大,不管误差的变化方向是怎样，应该让kp取值较大，从而来增加响应速度;但是为了防止由于eC瞬时过大,Td的取值会比较小;为了方便控制超调，使ti取值很小。 当误差间的值为一般时，需要确保系统的响应速率以及超调的操控，kP的值不增大，并且TI值应不减少，恰好的Td值是有必要的。 误差e不大时，为了确保系统的稳定性好，可以变大kp,T的取值，并且Td的取值应和Ed联系起来阻止发生振荡现象,。PID控制器的优点有：非常简单并且容易理解、没有精确的系统模型约束。然而它在控制非线性，时变，藕合及参数和结构不知道的情况下，控制效果不好。若是PID控制器功

43、能过于繁杂的话，也许会出现不管怎样调整参数都不能够做到我们想要的控制程度。另外，为了得到kp、-、Td的值时，若是没有其他一些对应的辅助技术，通常是运用试凑法来做，那么我们必须进行大量的实验，直到拼凑出最优参数，这样不仅造成浪费而且耗费精力和时间。第21页;宀心毕业设计(论文)专用纸第25页4算例分析4.1二自由度机械腎ID模糊控制器的建模4.1.1模糊PID控制模糊PID控制就是PID算法跟模糊控制理论二者相联合的新型控制理论。模拟PID控制系统控制方程式如下式(4.1)所示，让计算机进行PID控制，一定要将模拟PID数字化即PID控制离散化。因为计算机只能够依照离散采样每个时刻的误差值知道

44、控制量，却不会跟模拟控制器会一直不断的输出一些控制量，所以式(3.4)中的微分(D)和积分(I)项便只可以以间接的方法计算准，唯有通过用取微元计算的数学方法来无限靠近。uK(e+/edt+T)+uPTDdt0I0(4.1)如果T为采样周期，用和式来替换积分，用增量替换微分，用离散采样时刻点iT描述连续时间t，就能够得到以下近似变换:tiT,i0,1,2.fe(t)dtT工ej0j0de(t)e-eiidtT4.2)由上可知,得到(4.3):TiTuKe+2e+d(ee)+uiPiTjTii-10Ij0(4.3)式中ui为第i个采样时刻的输出值;J为第i个采样时刻的系统的一个输出偏差，即PID控

45、制器的一个输入值;ei-1为第i-1个采样时刻的系统输出误差;uo为开始进行PID控制时的原始值。复频域中，传递函数G为：cKs2+Ks+K(4.4)(4.5)G(s)=dP1cs替换成绝对式数字PID控制器为：u(k)=Ke(k)+KTe(i)+Ke(k)e(k»PIDTi=0=Ke(k)+KT艺e(i)+K(e(k)-e(k-1)PIDi=0其中KI=KIT，Kd=Kd/T有系统采用步进电机等增长型操作机构，该系统就只要的是控制量的增大量，而非位置的正值。故此对上式进行改写和替换，因此有以下增量等量关式:Au(k)=u(k)-u(k-1)=K(e(k)-e(k-1)+Ke(k)+

46、K(e(k)-2e(k-1)+e(k-2)PIDu(k)=u(k-1)+Au(k)(4.6)增长式的计算方法，仅仅只要计算到现时之前三个时刻的误差即行，这会使大大削减控制算法计算过程中的工作量，最重要是这样的方式可以在一定限度内阻止控制器造成积分饱和的现象。4.1.2PID模糊建模模糊PID控制的系统结构图如下4.1所示。图4.1模糊PID控制系统结构图在通常规模的PID基础上，y就是给定值的偏差变化率ec和偏差e跟被控对象的反馈值，利用模糊推理，在线自整定Kp,Ki,Kd值的大小。进而满足不同e和ec。若其参数有不一样用户规定的话，会让被控制对象拥有很好的静态(statics)性能和动态(d

47、ynamic)性能。模糊控制的基本论域，即e、ec、y的实际变动界限。对于模糊控制理论，输乩毕业设计（论文）专用纸入变量跟输出变量被用模糊概念来理解，e和ec即变量输入，y即变量输出。模糊集合就是语言变量，它的值即为语言变量的值。用户的疑难就是唯一证实确保语言变量值，它就是模糊子集。语言变量的词集由语言变量值来形成，然而，e、ec在基本论域内的一个现实值，被施加了模糊控制。我们需要把输入e,ec转变成语言变量值。并且，该转变过程就叫做模糊化，还要取决于该语言变量一个从属度函数。4.1.3 PID模糊控制由4.1图，来建模模糊PID控制器。因为系统受控时，对照不一样的e,ec，将PID参数整定的

48、规则一起总结归纳: 当间愈大时，表示其误差的绝对值愈大，那么Akp取较大值，来增加响应的迅速性;防止ATd瞬时值太大，ATd取较小的值;对积分给予一定的约束范围，一般取ATI=0来避免出现较大的超调。 当间一般数值时，由于Akp取不大些从而使系统相应超调不大;那么在这样情形下，ATd对造成系统输出结果扰动较大，所以取值很关键，要得当，因而的也要得当。 当间较小时，Akp、取一般大值使系统具有合适的不变性，与此同时，考虑抗干扰性能，适当地选取ATd值避免系统在固定点附近泛起不稳定的情况ed的取值与ATd取值规律是反向变化的，一般而言ATd为中等大小。4.2 设计模糊控制器规则以及其仿真分析4.2

49、.1 模糊控制器规则模糊控制器设计的重点在于它的算法，通常来说有三个组成部门，即界说各个模糊子集，创设模糊控制器的控制规则及择取描写输入变量、输出变量的词集。描写输入变量、输出变量能够由较多的语句来表达，那么能够简易拟定控制规则，相反的是控制规则相应也繁杂和麻烦；要是择取表达的语句不多，就会让描写变量单一，致使控制器的工能变弱。通常而言是几个个词语，但也能够按照现实情形下的要求来择取三四个措辞变量。要尽可能减小稳态误差被控对象，使其稳定是提高模糊控制输出的目的有效方法。因此，采取负偏向大值，负偏向中值，负偏向小值，不变，正偏向小值，正篇向中值，正偏向大值来对应于控制器输入之一的扰动：用英文字头

50、缩写为：GN,MN,SN,CON,SP,MP,GP然后便是构造模糊子集，本质上等于是要模糊子集在函数曲线。构成了一个相应的模糊变量的模糊子集。从概念上来说,在浩繁从属函数图中,来描写人举行控制活动时的模糊理论最合适的是正态型模糊变量。然而工程的现实运作中,机械正态型散布的模糊变量的运算是极其迟钝和非常繁杂的,而三角型散布的模糊变量的运算过程简易、反应敏捷。所以控制系统中很多的控制器大都选择三角型分布,最后就是创建模糊控制器的控制规则。选择输入语言变量为误差变化率ec和误差e,模糊变量值取GN,MN,SN,CON,SP,MP,GP这几个模糊值，对应着负偏向大值，负偏向中值，负偏向小值，不变，正偏

51、向小值，正篇向中值，正偏向大值;选择输出语言变量为巴、贞DA,，模糊变量值取GN,MN,SN,CON,SP,MP,GP这几个模糊值，建立从p、贞DATt，的模糊规则表如下表4-1、表4-2、表4-3。表4-1Ak的模糊规则表pAkpGNMNSNCONSPMPGPGNGPGPMPMPSPCONCONMNGPGPMPSPSPCONSNSNMPMPMPSPCONSNSNCONMPMPSPCONSNMNMNSPSPSPCONSNSNMNMNMPSPCONSNMNMNMNGNGPCONCONMNMNMNGNGN表4-2Aki的模糊规则表第27页，注讼罟毕业设计（论文）专用纸AkpGNMNSNCONSPM

52、PGPGNGNGNMNMNSNCONCONMNGNGNMNSNSNCONCONSNGNMNSNSNCONSPSPCONMNMNSNCONSPMPMPSPMNSNCONSPSPMPGPMPCONCONSPSPMPGPGPGPCONCONSPMPMPGPGP表4-3Ak的模糊规则表DAkpGNMNSNCONSPMPGPGNSPSNGNGNGNMNSPMNSPSNGNMNMNSNCONSNCONSNMNMNSNSNCONCONCONSNSNSNSNSNCONSPCONCONCONCONCONCONCONMPGPSNSPSPSPSPGPGPGPMPMPMPSPSPGP定义e和ec的论域：e,ec二-5

53、,-4,-3,-2,-1,0,1,2,3,4,5为各模糊子集的隶属度的值,按照各参数模糊控制模型和各模糊子集的从属度对应表,应用模糊规则设定PID参数取值表，代入式（4.7）'K=K'+AKppp<K=K'+AKIIIK=K'+AK儿八IDDD（4.7）其中KP、K；、KD是之前默认好的初始PID参数,Akp、Aki、AkD为模糊控制器的这些输出值，控制系统在对模糊逻辑规则的对表和计算、结果的处理，从而达到对PID参数的在线更正。其工作流程如图4-2所示。入口当前采样值Ie(k)=r(k)-y(k)eck)=e(k)-e(k-l)e(k-i)=e(k)1§(k),ec(k)模糊化模糊整定Akp、AkkAkd1计算当前kAki、kd模糊pb控制器输出返M图4.2模糊PID在线校正参数流程4.2.2PID参数的模糊整定按照上文中对二自由度运动学解