基于人机交互的多轴机械臂的运动控制分析

基于人机交互的多轴机械臂的运动控制分析摘要:多轴机械臂是机器人的重要组成部分，在人类的生产生活中具有广泛的应用。当今社会随着科学技术的迅速发展，人们对生活质量的要求也越来越高。在这种趋势之下机器人越来越多的出现在日常的工作、生产、生活、娱乐和教育等各个领域。其中多自由度机械臂做为一种最常见的机器人，具有时变性，耦合性以及非线性等特点。本文主要针对七自由度机械臂做为研究对象。采用D-H方法建立工业机器人各连杆的坐标系,应用齐次变换矩阵描述连杆坐标系之间的相对应的位置和姿态,求得该机器人的运动学数学模型;己知末端执行器位姿,运用代数法进行运动学反解,进而求得运动学逆解并讨论最优逆解。利用MATLAB的机器人工具箱建立了机器人数学模型，通过编程实现了对机器人动力学模型的检验，经过对运动学方程的求解结果和滑块控制图所设定数值的比较得出误差很小，验证了模型的可靠性，可以对研究者关于位置逆解或轨迹规划等方面的研究结果进行直观的验证，实现了正、逆运动学的数值计算和可视化仿真。应用轨迹规划的原理，对机械臂路径进行优化，减小机械臂的损害，力争得到最完美的结果。关键字：多轴机械臂，逆运动学，MATLAB仿真，路径规划，D-H方法IMotionControlofMultiAxisManipulatorBasedonHuman-ComputerInteractionAbstract：Themulti-axismanipulatorisanimportantpartoftherobot,andithasbeenwidelyappliedtoourdailylife.Therapiddevelopmentofscienceandtechnologyraisesthepublicslivingstandard.Therefore,therobotisappearinginpeoplesfieldofwork,production,life,entertainment,education,etc.inarapidspeed.Characterizedwithtimevarying,couplingandnonlinear,etc.,themultiDOFManipulatorisakindoffrequentlyusedrobots.ThispapermainlyfocusesonthesevenDOFrobotarm.Toobtainthemathematicalmodeloftherobotkinematics,thecoordinatesofeachlinkofindustrialrobotareestablishedbyD-Hmethodandthepositionandattitudeofthepolecoordinatesaredescribedbyhomogeneoustransformationmatrixduringtheresearch.Also,toobtaintheinversekinematicsanddiscussinverseoptimalsolution,algebraicmethodofkinematicinversesolutionisusedunderthecircumstanceofknowingtheposeofendeffector.Besides,therobotmathematicalmodelisestablishedbyusingMATLABroboticstoolboxandtherobotdynamicsmodelistestedbyprogramming.Thereliabilityandtheerrorofthemodelareprovedandbythekinematicsequationsolutionandthecomparisonofsupposednumbersofslidercontrolchartaswell.Inthisway,thispapercanprovepreviousresearchresultsoftheinversepositionsolutionandtrajectoryplanning.Italsorealizesthecomputationofforwardandinversekinematicsnumbersandvisualizationofsimulation.Theapplicationoftheprincipleoftrajectoryplanningtotheresearchoptimizesthepathofthemanipulator,reducesthedamageofthemanipulatorandcontributestogetaperfectresult.Keywords:Multiaxismanipulator,Inversekinematics,MATLABSimulation,Pathplanning,D-HMethodII目录1绪论.11.1课题背景介绍.11.2国内外研究现状.11.2.1机械手控制系统的研究.11.2.2空间轨迹规划的研究.11.3本课题研究的工作内容及目的.31.4EF-IRC-I七轴机械臂实验台简介.32七自由度多轴机械臂的模型建立.42.1位置和姿态的表示.42.1.1位置描述.42.1.2方位描述.52.1.3旋转矩阵在MATLAB下相关程序在实验台上的应用.62.1.4刚体运动在MATLAB下相关程序在实验台上的实现.72.1.5位姿描述.72.2齐次坐标和变换矩阵的理论基础及基于MATLAB在实验台上的应用。.82.2.1齐次正变换.82.2.2齐次逆变换.102.3七轴机械臂的D-H坐标建立及描述.112.4机械臂的雅可比矩阵简介.183七自由度多轴机械臂的运动学分析.193.1机械臂的运动学正问题.193.2机械臂的运用学逆问题.204七自由度机械臂动力学分析与仿真.225七自由度多轴机械臂的运动轨迹规划.236总结与展望.26参考文献.27致谢.29III附录.3001绪论1.1课题背景介绍多轴机械手控制系统的设计可谓是一项复杂的工程,它涉及到诸多学科,如:材料科学、运动力学、控制科学、数据通信、计算机科学、传感器技术和人工智能学。因此,在设计多轴机械臂的运动控制系统时，机械的结构设计，D-H方法的运用，正逆解的求解，路径及轨迹规划等就是需要考虑的。本文研究的对机械臂运动控制的研究，在机械臂的路径规划及实际应用中具有十分重要的意义。1.2国内外研究现状1.2.1机械手控制系统的研究多轴机械臂的运动控制中，轴与轴之间的协调无疑是最重要的，为了实现这一目的，就需要建立一套可靠的数据传输和通讯机制，从而实现主控CPU与各个运动控制关节的数据交换的稳定性。目前所采用的控制系统主要有中央式结构和分布式结构。较为传统的运动控制系统一般使用中央式体系结构,该类控制体系随着工业控制系统复杂度的提高,其缺点将逐渐显现出来,一旦CPU出现故障将对生产和控制系统造成严重影响,同时,该类体系结构有着复杂的线路,维护比较困难,因此,该类结构适合于对单个机械手的控制系统开发。分布式控制方法的提出,解决了中央式体系结构所遇到的难题。它将信息处理和动作控制的任务分配给不同控制单元的自身处理器去完成,此时的中央CPU只需要与各个单元的控制器通过总线来连接,既降低了系统控制的复杂性,也降低了控制系统的成本,同时还提高了系统的稳定性。1.2.2空间轨迹规划的研究空间和直角坐标空间的轨迹规划是机械臂规划的两种。规划和计算时较简单的是多轴机械臂在关节空间中轨迹规划,但路径的不唯一性出现在关节动作规划中,关节的角度函数可选择各种类型,有高阶多项式插值法等。如连续弧焊这种对作业路径、1姿态有严格要求时,就必须在空间直角坐标系中进行轨迹规划,然后将轨迹离散成多个点,再逐点返回到关节空间,得到各关节的变化量后再进行相应的控制。对该类问题的研究国内外学者进行了大量的研究,但是由于问题的复杂性和平台的多样性,至今还没有一套通用的优化算法来实现机械手作业轨迹的最优化。早些时期由VolpeR和KhoslaP提出的基于机器人位置、速度、加速度和二阶加速度约束的最优时间下的轨迹规划方法,并通过柔性多面体搜索算法(FPS)进行求解,该方法通过使用高次多项式曲线来连接空间的关键点从而得到可行的的轨迹。该算法属于局部搜索算法,其初始条件选取将直接影响到算法的实际性能。2002年由崔平远和杨国军提出的基于模糊遗传算法的机械手最优时间轨迹规划算法。该算法克服了传统非线性规划算法易陷入局部极小的不足。同年,王建滨等人研究了一种考虑速度和力矩约束的机械手的时间最优轨迹规划算法,引入了B样条曲线拟来拟合无碰离散路径,并使用动态规划的方法进行了优化和求解,但由于采用了数值求解,所以在计算和仿真之间存在折中问题,当问题规模激增时,要获得一定的求解精度的代价也会剧增。2005年由谭冠正等人提出通过增加轨迹规划曲线上的节点数来提升跟踪精度,并把多项式和余弦函数组合寻找最优轨迹规划方法。21.3本课题研究的工作内容及目的本课题通过学习相关知识，运用D-H算法和坐标变换矩阵完成机械臂运动学建模，解决运动学中运动学正问题和运动学逆问题在坐标系中的求解，并对已建立的方程利用MATLAB进行数值求解,编写MATLAB程序，在MATLAB中进行运动学仿真，并进行比较和分析，最后进行运动控制分析，实现机械臂沿特定轨迹的运动。1.4EF-IRC-I七轴机械臂实验台简介本课程所使用的EF-IRC-I七轴机械臂，为7自由度串联关节式机器人，其轴线相互平行或垂直，能够在空间内进行定位，采用国外进口的高精度伺服电机、内置32位的ARM处理器，12位非接触式约对编码器精确采集位置信息，尺寸小、重量轻、精度高；控制软件采用高校最流行的编程语言MATLAB实现，采用可视化设计，控制简单，编程方便，尤其是MATLAB本身所独有强大的计算能力，对机器人控制的算法研究具有无可比拟的优势。它具有高度的能动性和灵活性，具有广阔的开阔空间，是进行运动规划和编程系统设计的理想对象。整个系统包括机器人1台、实验附件和机器人控制软件1套。机器人采用串联式开链结构，即机器人各连杆由旋转关节或移动关节串联连接，如图1.1所示。各关节轴线相互平行或垂直。连杆的一端装在固定的支座上（底座），另一端处于自由状态，可安装各种工具以实现机器人作业。关节的作用是使相互联接的两个连杆产生相对运动。机器臂关节技术参数如下：1)机械手臂垂直最大长度:53.4cm(从基座到顶端)2)机械手臂水平最大长度:48cm3)重复性位置精度：+/-0.5mm4)手指最大张度：3.5cm3图1.12七自由度多轴机械臂的模型建立一般的，机械臂需要在三维空间中进行运动，在笛卡尔坐标系中机器人的末端手臂需要满足3个方向的位置要求和相对于3个坐标轴的角度要求，因而，其运动和姿态的控制需要6个参数，所以，一般的机械臂运动需要6个自由度。但有的机器人需要在复杂环境下工作，作业时就需避开机构自身的奇异点或障碍物，可能需要冗余的自由度（自由度大于7)12。本文研究对象便是七轴的有冗余自由度的机械臂，其必须具有快速响应、低惯性、高精度、有效避开奇异点的性能。2.1位置和姿态的表示2.1.1位置描述描述机械手各部分间关系时，一旦建立了一个坐标系，我们就能够用某个3l位置矢量来确定该空间内任意点的位置。对于直角坐标系A，空间任意一点P的位置可用31的列矢量AP表示。4AP=PxPyPzT（2.1）其中，PxPyPz是P点在坐标系A中的三个坐标分量，称AP为位置矢量。2.1.2方位描述为了研究机械臂的运动与操作，往往不仅要表示空间某个点的位置，而且需要表示物体的方位。物体的方位可由某个固接于此物体的坐标系描述。具体为：在空间某刚体B的上固接一局部坐标系，如B，用坐标系B的三个单位主矢量xB,yB,zB相对于参考坐标系A的方向余弦形成的3X3旋转矩阵BAR来表示刚体B相对于A的方位。如图2.1所示。BAR=AxBAyBAzB=（2.2）32311rr其中AxB，AyB，AzB都是单位矢量并且两两垂直，故其9个元素可形成6个正交等式，即：AxBAxB=AyBAyB=AzBAzB=1AxBAyB=AyBAzB=AzBAxB=0并且满足BAR-1=BAR-T,|BAR|=1。当围绕轴x，y或z作角度为的旋转后的旋转矩阵分别为：R(x,)=R(y,)=cossin0-01cos0sin1incoR(z,)=10cossini-5图2.1方位描述2.1.3旋转矩阵在MATLAB下相关程序在实验台上的应用1.B绕G的坐标轴单次旋转的问题：以绕X轴为例，B和G刚开始重合，然后B绕G的X轴旋转某个角度，计算B到G的旋转矩阵。如果已知点P在B中的坐标，计算点P在G中的坐标。公式：B到G的旋转矩阵*B中点P的坐标（列向量），相关程序指令为rx，rxd。首先，计算B绕G的X轴逆时针旋转30度后，B到G的旋转变换矩阵：在MATLAB命令行，输入rx30=rxd(30)后回车，可得其旋转矩阵。rx30是我们自己取的变量名。其次，如果点P在B中的坐标为050，坐标窗口中，每个轴显示长度为5，即该点在B的y轴的终点，在MATLAB命令行，计算点P在G中的坐标方式为输入PG=rxd(30)*050后回车，即PG是点P在G中的坐标。另外特别注意：逆时针旋转，角度取正值；顺时针旋转，角度取负值。同样的，计算B绕G的Y轴和Z轴逆时针旋转30度后，计算方法相同，用到的程序指令为ry，ryd和为rz，rzd，公式为：（1）：B到G的旋转矩阵*B中点P的坐标（列向量）（2）：在MATLAB的命令行，输入PG=rxd(30)/ryd(30)/rzd(30)*B中点P的坐标（列向量）后回车，即PG是点P在G中的坐标。2.B绕G的坐标轴连续多次旋转的问题。设连续旋转n次有公式：1）先按B绕G的坐标轴旋转的先后顺序，分别计算每次旋转B到G的旋转矩阵：Q1，Q2，.，Qn。2）再计算B到G最终的旋转矩阵Q。Q=Qn*.*Q2*Q1。特别说明：矩阵6乘法，不满足交换律。3）设点P在B中的坐标向量为PB（列向量），则点P在G中的坐标PG为：PG=Q*PB3.旋转逆变换的问题。B绕G的坐标轴旋转，已知点P在B中的坐标，计算点P在G中的坐标，这种B到G的旋转变换，称为旋转正变换。反过来，已知点P在G中的坐标，计算点P在B中的坐标，这种G到B旋转变换，称为旋转逆变换。B绕G的坐标轴旋转的正变换与逆变换的关系为：B到G的旋转矩阵的转置=G到B的旋转矩阵；G到B的旋转矩阵的转置=B到G的旋转矩阵所以已知G到B的旋转矩阵，以及点P在G的坐标，计算点P在B中的坐标，其方法与旋转正变换计算坐标类似：（1）计算B到G的旋转变换矩阵：Q（2）计算G到B的旋转变换矩阵：A。A为Q的逆矩阵。Q的逆矩阵等于Q的转置矩阵。在MATLAB的表示为：A=Q2.1.4刚体运动在MATLAB下相关程序在实验台上的实现刚体旋转和平移的组合，称为刚体的运动。将刚体的运动分解成先旋转再平移，是表示空间位移最简单的方法。刚体的运动B到G的坐标变换公式点P在G中的坐标（列向量）=B到G的旋转矩阵*点P在B中的坐标（列向量）+B的原点平移到G中某点的坐标（列向量）2.1.5位姿描述在讨论了采用位置矢量和旋转矩阵描述物体的位置和方位后，就可以完整描述刚体B在空间的位置和姿态(简称位姿)。描述公式为：B=BARAPB（2.3）坐标系B的原点选在刚体B的特征点上，如质心等。当表示位置时，旋转矩阵BAR=I；表示方位时，式(2-3)中的位置矢量APB=0。72.2齐次坐标和变换矩阵的理论基础及基于MATLAB在实验台上的应用。2.2.1齐次正变换1.刚体的运动由旋转和平移两部分组成，旋转矩阵是33的矩阵，平移是坐标列向量，将这两者综合成一个矩阵，可以完整描述刚体的运动。该矩阵称为齐次变换矩阵，是一个44的矩阵。既包含了旋转矩阵，也包含了平移坐标。B到G的齐次变换公式：点P在G中的齐次坐标（列向量）=B到G的齐次矩阵*点P在B中的齐次坐标（列向量）公式如图2.25010015020025030035040045050055050100150200250300350400450500图2.2其中B到G的齐次矩阵为：r11r12r13Xor21r22r23Yor31r32r33Zo00018其中，旋转矩阵为：r11r12r13r21r22r23r31r32r33平移坐标为：XoYoZo,点P在G中的齐次坐标表示为：XpYpZp1,点P在B中的齐次坐标表示为：xpypzp1,平移齐次坐标表示为：XoYoZo12、任意齐次变换矩阵，总能分解成平移齐次变换矩阵和旋转齐次变换矩阵的乘积。齐次变换矩阵=平移齐次变换矩阵*旋转齐次变换矩阵3、以B绕G的X轴逆时针旋转30度为例，根据旋转矩阵，计算旋转齐次矩阵。相关的程序语句有rxd，r2tr（返回旋转矩阵的齐次矩阵），tr2r（返回齐次矩阵中的旋转子矩阵），trans2tr（根据平移向量，计算齐次矩阵）。步骤为：1，在MATLAB命令行，输入Q=rxd(30)后回车。得到B绕G的X轴逆时针旋转30度的旋转矩阵。2，在MATLAB命令行，输入TR=r2tr(Q)后回车。得到B绕G的X轴逆时针旋转30度的旋转齐次矩阵。可以看到，平移坐标为（000），表示不平移。4、根据齐次矩阵，计算旋转矩阵。显然，截取齐次变换矩阵的前3行和前3列，就可以得到旋转矩阵。在MATLAB命令行，输入Q=tr2r(TR)后回车。TR是上一步得到的齐次矩阵。5、根据平移坐标，计算平移齐次矩阵。相关的程序语句有trans2tr（根据平移向量，计算齐次矩阵），在MATLAB命令行，输入TR=trans2tr(123)后回车。可以看到，旋转部分为单位矩阵，表示不旋转。6、根据齐次矩阵，计算平移坐标。显然，截取齐次变换矩阵的第4列前3个元素，就可以得到平移坐标。在MATLAB命令行，输入D=tr2r(TR)后回车。TR是上一步得到的齐次矩阵。92.2.2齐次逆变换5010015020025030035040045050055050100150200250300350400450500图2.3图2.3为齐次逆变换的公式，在MATLAB中，求矩阵的逆矩阵，可以用函数inv。下面首先以B绕G的X轴逆时针旋转30度，然后B的原点平移到G中的点（3，4，5）为例，先计算B中点P（1，2，3）在G中的坐标（相关程序语句及函数在上节已做解释）。在MATLAB中计算步骤为:（1）计算B到G的平移齐次矩阵：TRD=trans2tr(345)。（2）计算B到G的旋转齐次矩阵：TRR=r2tr(rxd(30)。（3）计算B到G的齐次矩阵：TRBG=TRD*TRR。（4）P在B中的齐次坐标为PBH=1231,计算P在G中的齐次坐标：PGH=TRBG*PBH。（5）计算P在G中的坐标：PG=PGH(1:3)10其次计算G到B的齐次矩阵，并根据前面计算的点P在G中的齐次坐标，利用齐次逆变换公式，计算点P在B中的坐标，看看是否是（1，2，3）。在MATLAB计算步骤为：（1）计算B到G的平移齐次矩阵：TRD=trans2tr(345)。（2）计算B到G的旋转齐次矩阵：TRR=r2tr(rxd(30)。（3）计算B到G的齐次矩阵：TRBG=TRD*TRR。（4）计算G到B的齐次矩阵：TRGB=inv(TRBG)。（5）P在G中的齐次坐标为PGH,计算P在B中的齐次坐标：PBH=TRGB*PGH。（6）计算P在B中的坐标：PB=PBH(1:3)2.3七轴机械臂的D-H坐标建立及描述1、D-H参数在机器人学中，描述两个相邻坐标系之间的几何关系，通常使用D-H参数。这种方法是由JacquesDenavit和Ri-chardS.Hargenberg两人在1955年发明的，因此，称为D-H参数，确定D-H参数的方法，称为D-H方法。在3维空间中，要确定一个3维的坐标系，只需要确定其中两个坐标轴，另一个坐标轴可以使用右手螺旋定则确定。如，只需要确定3维坐标系的Z轴和X轴，Y轴可用右手螺旋定则确定。用右手握住坐标系的Z轴，大拇指指向Z轴的正方向，四指的绕向即是X轴的正方向绕向Y轴的正方向，绕过的角度为90度。因此，要描述两个坐标系之间的相对关系，只需要描述两个坐标系中两对坐标轴之间的相对关系。D-H参数描述了两个坐标系中两个Z轴之间的相对关系和两个X轴之间的相对关系。设机械臂中，两个相邻连杆的几何关系以及指定的坐标系，如图2.4所示。115010015020025030035040045050055050100150200250300350400450图2.4D-H参数名称及含义：（1）ai：连杆长度。描述了两个相邻坐标系i和i-1的两个Z轴之间的距离。（2）i：连杆扭角。描述了两个相邻坐标系i和i-1的两个Z轴之间的夹角。（3）di：关节距离。也称为连杆偏移。描述了两个相邻坐标系i和i-1的两个X轴之间的距离。（4）i：关节角。描述了两个相邻坐标系i和i-1的两个X轴之间的夹角。夹角的取值：坐标系i-1的Z轴的正方向，绕坐标系i的X轴的正方向，按右手螺旋法则旋转，使坐标系i和i-1的两个Z轴正方向一致。绕X轴正方向按右手螺旋法则旋转，角度为正值；绕X轴负方向按右手螺旋法则旋转，角度为负值。可以看出，D-H参数描述了两个相邻坐标系之间的几何位置的相对关系。在机器人学中，通常用D-H参数表来描述机器人的几何结构。2、根据D-H参数，计算齐次变换矩阵在图2-3.1中，查看根据D-H参数，计算齐次变换矩阵的公式。方程左边的符号，表示坐标系i到i-1的齐次变换矩阵；方程右边，第1项表示12沿坐标系i-1的Z轴，平移d的齐次变换矩阵；第2项表示沿坐标系i-1的Z轴，旋转角度的齐次变换矩阵；第3项表示沿坐标系i的X轴，平移a的齐次变换矩阵；第4项表示沿坐标系i的X轴，旋转角度的齐次变换矩阵。3、D-H方法在机器人学中，使用D-H方法为机器人指定各个坐标系，并确定相邻两个坐标系之间的几何位置关系，即D-H参数。D-H方法包含两个内容：（1）指定坐标系（2）确定两个相邻坐标系D-H参数D-H方法具体实现：（1）编号方法：设机械臂有n个关节，则机械臂就有n个连杆和1个末端工具。因此需要为机械臂指定（n+1）个坐标系。1）关节编号：1n机械臂从下往上，关节编号为1n。共有n个关节编号。2）连杆编号：0n-1基座与关节1之间的部分连杆0；关节1与关节2之间的部分为连杆1；关节（n-1）与关节n之间的部分为连杆（n-1）。共有n个连杆编号。3)坐标系编号：0n为连杆0指定坐标系0；为连杆1指定坐标系1；为连杆n-1指定坐标系n-1；为末端工具指定坐标系n。（2）坐标系0的确定方法：坐标系0是整个机械臂的参考坐标系。机械臂的所有连杆以及末端工具的位置与朝向，都是以坐标系0作为参考的。1）先确定坐标系0的Z轴确定连杆坐标系Z轴的基本原则：连杆坐标系i的Z轴，就是关节i+1的运动轴线。Z轴的正方向，可以任意指定。因此，坐标系0的Z轴，就是关节1的旋转轴线。2）再确定坐标系0的原点O13可以在坐标系0的Z轴上任取一点，作为坐标系0的原点。在实际中，则根据机械臂的实际情况，取一个合适的点作为原点。3)再确定坐标系0的X轴和Y轴坐标系0的X轴和Y轴可以任意指定，但必须满足右手螺旋定则。坐标系0的X轴和Y轴可以任意指定，但必须满足右手螺旋定则。用右手握住Z轴，大拇指指向Z轴的正方向，四指的绕向即为X轴的正方向绕向Y轴的正方向，且绕过角度为90度。确定了坐标系0之后，就可以通过下一级坐标系i-1，来确定上一级坐标系i，然后再确定两个相邻坐标系的D-H参数。即，可以通过坐标系0来确定坐标系1；通过坐标系1，确定坐标系2，。（3）由下向上确定其余连杆坐标系1到n-1及其D-H参数：已知坐标系i-1，确定坐标系i。由于前面已经确定了坐标系0，使用下面的方法，就能确定剩余的所有连杆坐标系：坐标系1到坐标系n-1。1）先确定坐标系i的Z轴根据确定连杆坐标系Z轴的基本原则，坐标系i的Z轴，就是关节i+1的旋转轴线。Z轴的正方向可任意指定。2）再确定坐标系i的原点O及X轴可分为3种情况：a)坐标系i和i-1的两个Z轴不共面（即不在一个平面）对于空间两条不共面的直线，有且只有一条直线与它们都垂直且相交，该直线称为这两条直线的法线坐标系i的X轴：坐标系i和i-1两个Z轴的法线。正方向可以任意指定。坐标系i的原点：坐标系i的X轴与Z轴的交点。b)坐标系i和i-1的两个Z轴相交坐标系i的原点：坐标系i和i-1的两个Z轴的交点。坐标系i的X轴：坐标系i和i-1的两个Z轴相交所确定的平面的法线（即平面的垂线），正方向可以任意指定。c)坐标系i和i-1的两个Z轴平行对于空间两条平行的直线，有无数条直线与它们都垂直且相交，这些直线都称为这两条平行直线的法线。14坐标系i的X轴：从坐标系i和i-1的两个Z轴的法线中，选择过坐标系i-1的原点的法线，作为坐标系i的X轴。坐标系i的原点：坐标系i的X轴与Z轴的交点。对于3维坐标系，只要确定了两个坐标轴，根据右手螺旋定则，即可以确定另一个坐标轴。因此，D-H方法一般不需要再标出Y轴。3)根据D-H参数的定义，确定坐标系i和i-1几何相对位置的D-H参数。（对于旋转关节，关节角是变量，其余3个参数是常量；对于平移关节，关节距离d是变量，其余3个参数是常量。）（4）确定末端工具的坐标系n及D-H参数末端工具坐标系n的确定，是非常自由的。但必须遵守DH方法的基本要求：坐标系i的X轴，必须与坐标系i-1的Z轴垂直且相交50100150200250300350400450500550100200300400500600图2.5从图2.5中，观察坐标系n的指定方法。夹子的Z轴表示了夹子接近物体的运动方向；Y轴表示了夹子的开合方向。（5）根据公式，计算相邻坐标系之间的齐次变换矩阵；根据复合齐次变换公式，计算任意连杆和末端工具相对于固定参考坐标系的齐次变换矩阵。如弹出窗口所示。15复合齐次变换公式中的q1，表示关节1的变量取值，其余类似。下面以确定坐标系0的D-H参数为例，确定机械臂的各个坐标系的D-H参数。确定坐标系0：坐标系0是整个机械臂的参考坐标系。机械臂的所有连杆以及末端工具的位置与朝向，都是以坐标系0作为参考的。（1）先确定坐标系0的Z轴确定连杆坐标系Z轴的基本原则：连杆坐标系i的Z轴，就是关节i+1的运动轴线。Z轴的正方向，可以任意指定。因此，坐标系0的Z轴，就是关节1的旋转轴线。由于仿真模型中，关节按右手螺旋法则旋转，关节角增大的旋转轴线为正方向。为了使仿真模型与实际机械臂一致，关节1的旋转轴线正方向为垂直向上。因为，关节1根据该正方向按右手螺旋法则旋转，关节角也增大。（2）再确定坐标系0的原点O坐标系0的Z轴与底座的交点，作为原点。（3)再确定坐标系0的X轴和Y轴坐标系0的X轴，正方向指向基座电源插头方向。Y轴正方向指向基座EFLAG的公司商标方向。确定了坐标系0之后，就可以通过下一级坐标系i-1，来确定上一级坐标系i，然后再确定两个相邻坐标系的D-H参数。即，可以通过坐标系0来确定坐标系1；通过坐标系1，确定坐标系2，3。1650100150200250300350400450500550100200300400500600图2.6如图2.6，按上述方法7轴智能化机器人手臂的D-H参数，就全部得到了。7轴智能化机器人手臂的D-H参数表：L1=alpha,A,theta,D=pi/2,0,-pi/2,120L2=alpha,A,theta,D=pi/2,0,pi,0L3=alpha,A,theta,D=pi/2,0,pi,140.8L4=alpha,A,theta,D=-pi/2,71.8,pi/2,0L5=alpha,A,theta,D=pi/2,71.8,0,0L6=alpha,A,theta,D=pi/2,0,pi/2,0L7=alpha,A,theta,D=0,0,0,129.6需要特别强调的是：7轴智能化机器人手臂的7个关节都是旋转关节，因此，17D-H参数中，alpha,A,D三个参数是常量，theta参数是关节变量，它将随着关节的旋转而变化。通过D-H方法得到的theta值，主要用于在对机械臂进行建模时，使用这个值计算关节变量的偏移值，使得所建的机械臂模型，与实际的机械臂一致。2.4机械臂的雅可比矩阵简介机械臂的Jacobian矩阵表示机械臂的操作空间与关节空间之间速度的线性映射关系，对于一个n轴的机械臂，机械臂末端在基坐标系中的速度是其中x是6xJq&个元素的向量。对于6个关节机械臂Jacobian矩阵是方阵，如果它是可逆的，则可以由机械臂的末端速度求出各个关节的速度。Jacobian矩阵在机械臂的奇异位姿上是不可逆的。在实际应用中，当机械臂的末端位置接近奇异位置时，Jacobian矩阵是病态的，可能导致关节速度不能正确地得到22。上式解决的是正速度问题，即已知q和求末端执行器的速度。对于逆速度解&x&问题，由上式可以得到速度逆解公式为，注意到此时需要求雅可比矩阵的逆，1Jx由线性方程组理论知上式对任意的，都有解的必要条件是雅可比矩阵的秩rank(J)x=6，这意味着机械臂的自由度数n6。这也说明了具有冗余自由度的机械臂，在末端位姿固定的条件下，能使关节在一个较大的关节空间的子空间中运动，有效地避开障碍或奇异位姿，并把关节位移限制在允许范围内，从而具有更大的运动灵活性。雅可比矩阵可以看成是从关节空间到操作空间运动速度的传动比，同时也可用来表示两空间之间力的传递关系。对于冗余自由度机械臂，其雅可比矩阵是长方矩阵，因J满秩且方程个数少于未知数个数，所以有无穷多个解，这时，一般是求其中的最小范数解，或采用加权最小范数解也就是说使最小的解，其中D是对TqD&称正定加权矩阵。此时的解是使机械臂在能量消耗最小的情况下的解。这时，逆速度问题便转为：求满足且使最小。实际上等q&1Jx2TL同于求性能指标L在约束条件下的极值。应用Lagrange乘子法，以上极值1为题的解是，当D=I时，雅可比矩阵是，称为11()TqDJx&1()TJ雅可比矩阵的伪逆。183七自由度多轴机械臂的运动学分析3.1机械臂的运动学正问题对于一个n-轴刚性连接的机械臂，正向运动学的解给出的是最后一个连杆坐标系的位置和姿态。重复利用上式,得到0112()nnTAKq（3.1）机械臂末端位姿在笛卡尔坐标系中有6个自由度，3个平移，3个旋转。所以，一般来说具有6个自由度的机械臂可以使末端实现任意的位姿19。总的机械臂变换0n一般简写为Tn，对6个自由度的机械臂简写为T6。对于任意的机械臂，无论其它有多少个关节，具有什么结构，正向运动学解都是可以得到的。参考第二章中对正运动学的分析，根据式(2.2)及各连杆参数，可以求得此七自由度机械臂各连杆变换矩阵为：1100CSAd2201CsacA3301CSaA443001SCd55401SC6601SCd(3.2）07123456701xxyyzznoapTA其中n，o，a，p均为关节变量i（i=1，2，7）的函数12356423461456412356()*()*()()*xccssccssc1235123ssy192345642356(s)*()(s)*znccs123614412356)()*xoccssc145456123(s)()(ycsscs23623*)*zcc123451451235(s)s(xaccs3c)cys245235()*(s)*czc6131614123465312312121cds()*(d)xpdcscacsac223455123(scs)yss6312346523232*c(s)()ccsc（3.3）3.2机械臂的运用学逆问题在机械臂的路径规划中，用到的是反向运动学的解10()nqKT，它给出了特定的末端位姿对应的机械臂的关节角度。一般来说，反向运动学的解不是唯一的，对具有某种结构的机械臂，封闭解可能不存在。对于6自由度的机器人而言，运动学逆解非常复杂，一般没有封闭解。只有在某些特殊情况下才可能得到封闭解。不过，大多数工业机器人都满足封闭解的两个充分条件之一（Pieper准则）23（1）三个相邻关节轴交于一点（2）三个相邻关节轴相互平行如果机械臂多于6个关节，称关节为冗余的，这时解是欠定的。如果对于机械臂某个特别的位姿，解不存在，称这个位姿为奇异位姿。机械臂的奇异性可能是由于机械臂中某些坐标轴的重合，或位置不能达到引起的。机械臂的奇异位姿分为两类：20(1)边界奇异位姿，当机械臂的关节全部展开或折起时，使得末端处于操作空间的边界或边界附近，雅克比矩阵奇异，机械臂的运动受到物理结构的约束，这时机械臂的奇异位姿称为边界奇异位姿。(2)内部奇异位姿，两个或两个以上的关节轴线重合时，机械臂各个关节的运动相互抵消，不产生操作运动，这时机械臂的奇异位姿称为内部奇异位姿。机械臂运动学逆解的方法可以分为两类：封闭解和数值解、在进行逆解时总是力求得到封闭解。因为封闭解的计算速度快，效率高，便于实时控制。而数值解法不具有这些特点。机械臂运动学的封闭逆解可通过两种途径得到：代数法和几何法。一般而言，非零连杆参数越多，到达某一目标的方式也越多，即运动学逆解的数目也越多。在从多重解中选择解时，应根据具体情况，在避免碰撞的前提下通常按“最短行程”准则来选择。同时还应当兼顾“多移动小关节，少移动大关节”的原则。n个自由度的机械臂的末端位姿由n个关节变量所决定，这n个关节变量统称为n维关节矢量，记为q。所有的关节矢量构成的空间称为关节空间。机械臂末端的位姿用6个变量描述，3个平移(x,y,z)和3个旋转(x,y,z)，记x=(x,y,z,x,y,z)，x是机械臂末端在基坐标空间中的坐标，所有的矢量x构成的空间称为操作空间或作业定向空间。工作空间是操作臂的末端能够到达的空间范围，即末端能够到达的目标点集合。值得指出的是，工作空间应该严格地区分为两类：(1)灵活（工作）空间指机械臂末端能够以任意方位到达的目标点集合。因此，在灵活空间的每个点上，手爪的指向可任意规定。(2)可达（工作）空间指机械臂末端至少在一个方位上能够到达的目标点集合。机械臂各关节驱动器的位置组成的矢量称为驱动矢量s，由这些矢量构成的空间称为驱动空间。针对七自由度机械臂，我们做了一个逆解求椭圆的方案，并且对其进行了仿真，轨迹跟踪，对比。具体程序如下：tc1=transl(x(1),y(1),z(1);tc1(1:3,1:3)=rpy2r(0,0,0);ax=pi/2/N;ay=0;az=0;Q(1,:)=mod(ikine(m_7r,tc1),2*pi);21fork=1:N-1JT=pinv(jacob0(m_7r,Q(k,:);s1=x(k),y(k),z(k),k*ax,k*ay,k*az;s2=x(k+1),y(k+1),z(k+1),(k+1)*ax,(k+1)*ay,(k+1)*az;Q(k+1,:)=JT*(s2-s1)+Q(k,:);end%Q=mod(Q,2*pi);%plot(m_7r,Q),holdonq=floor(abs(Q*2048/pi);tc=fkine(m_7r,Q);xs=tc(1,4,:);ys=tc(2,4,:);zs=tc(3,4,:);plot3(xs(:),ys(:),zs(:),r.),holdoffmax(abs(x-xs(:),max(abs(y-ys(:),max(abs(z-zs(:)4七自由度机械臂动力学分析与仿真七自由度机械臂的动力学分析是研究机械臂平稳运行的基础，所以其仿真研究对与机械臂有重要的影响，动力学分析主要考虑Q，QD，QDD等因素，我们通过一下编程，分析机械臂运行过程中，这些因素的影响。q0=pipipipipipipi;q1=pipi/2000pi/20;tr0=fki