机器人正逆运动学

§1.4机器人正向运动学

工业机器人的正向运动学是指已知各关节的类型、相邻关节之间的尺寸和相邻关节相对运动量的大小时，如何确定工业机器人末端操作器在固定坐标系中的位姿。

主要包括以下内容：

1)相对杆件的坐标系的确定；

2)建立各连杆的模型矩阵A；

3)正运动学算法；D-H表示法学习目标：1.理解D-H法原理

2.学会用D-H法对机器人建模学习重点：1.给关节指定参考坐标系

2.制定D-H参数表

3.利用参数表计算转移矩阵背景简介：

1955年，Denavit和Hartenberg(迪纳维特和哈坦伯格)提出了这一方法，后成为表示机器人以及对机器人建模的标准方法，应用广泛。

总体思想：

首先给每个关节指定坐标系，然后确定从一个关节到下一个关节进行变化的步骤，这体现在两个相邻参考坐标系之间的变化，将所有变化结合起来，就确定了末端关节与基座之间的总变化，从而建立运动学方程，进一步对其求解。1.第一个关节指定为关节n,第二个关节为n+1,其余关节以此类推。坐标系的确定2.Z轴确定规则：如果关节是旋转的，Z轴位于按右手规则旋转的方向，转角为关节变量。如果关节是滑动的，Z轴为沿直线运动的方向，连杆长度d为关节变量。关节n处Z轴下标为n-1。3.X轴确定规则情况1：两关节Z轴既不平行也不相交取两Z轴公垂线方向作为X轴方向，命名规则同Z轴。情况2：两关节Z轴平行此时，两Z轴之间有无数条公垂线，可挑选与前一关节的公垂线共线的一条公垂线。情况3：两关节Z轴相交取两条Z轴的叉积方向作为X轴。4.Y轴确定原则取X轴、Z轴叉积方向作为Y轴方向。（右手）5.变量选择原则用θn+1角表示Xn到Xn+1绕Zn轴的旋转角;dn+1表示从Xn到Xn+1沿Zn测量的距离;an+1表示关节偏移，an+1是从Zn到Zn+1沿Xn+1测量的距离;角α表示关节扭转,αn+1是从Zn到Zn+1绕Xn+1旋转的角度。

通常情况下，只有θ和d是关节变量。斯坦福机器人斯坦福机器人开始的两个关节是旋转的，第三个关节是滑动的，最后三个腕关节全是旋转关节例1：Stanford机器人运动学方程A1A2A3A4A5A6d1z1x1y1O1d2z2x2y2O2z3y3x3O3y4z4x4O4z5y5x5O5d3z6x6y6O6d6z0y0x0O0为右手坐标系原点Oi：Ai与Ai+1关节轴线的交点zi轴：与Ai+1关节轴重合，指向任意xi轴：Zi和Zi-1构成的面的法线yi轴：按右手定则ai—沿xi

轴，zi-1轴与xi

轴交点到Oi的距离αi—绕xi轴，由zi-1转向zidi—沿zi-1轴，zi-1轴和xi交点至Oi–1坐标系原点的距离θi—绕zi-1轴，由xi-1转向xi关节1坐标系0关节2坐标系1关节3坐标系2连杆0连杆1连杆2连杆3连杆4连杆5关节4坐标系3关节5坐标系4关节6坐标系5解：θ1θ2θ3θ4θ5θ6关节变量都是θ例2、PUMA560运动动学学方方程程（（六个个自自由由度度，，全全部部是是旋旋转转关关节节）PUMA560机器器人人的的连连杆杆及及关关节节编编号号为右右手手坐坐标标系系，，Yi轴：：按按右右手手定定则则Zi轴：：与与Ai+1关节节轴轴重重合合，，指指向向任任意意Xi轴：：Zi和Zi-1构成成的的面面的的法法线线，，或连连杆杆i两端端轴轴线线Ai与Ai+1的公公垂垂线线(即：：Zi和Zi-1的公公垂垂线线)原点点Oi：Ai与Ai+1关节节轴轴线线的的交交点点，，或或Zi与Xi的交交点点ai—沿xi轴，，zi-1轴与与xi轴交交点点到到Oi的距距离离αi—绕xi轴，，由由zi-1转向向zidi—沿zi-1轴，，zi-1轴和和xi交点点至至Oi––1坐标标系原原点点的的距距离离θi—绕zi-1轴，，由由xi-1转向向xiA1A2A3A4A5A6O1O0对下下图图所所示示简简单单机机器器人人，，根根据据D-H法，，建建立立必必要要坐坐标标系系及及参参数数表表。。例3第一一步步：：根根据据D-H法建建立立坐坐标标系系的的规规则则建建立立坐坐标标系系第二二步步：：将将做做好好的的坐坐标标系系简简化化为为我我们们熟熟悉悉的的线线图图形形式式第三三步步：：根根据据建建立立好好的的坐坐标标系系，，确确定定各各参参数数，，并并写写入入D-H参数数表表#da1009020030040-90500906000#da1009020030040-90500906000第四四步步：：将将参参数数代代入入A矩阵阵，，可可得得到到第5步求求出出总总变变化化矩矩阵阵依次次写写出出从从基基坐坐标标系系到到手手爪爪坐坐标标系系之之间间相相邻邻两两坐坐标标系系的的齐次次变变换换矩矩阵阵，它它们们依依次次连连乘乘的的结结果果就就是是末末端端执执行行器器（（手手爪爪））在在基基坐坐标标系系中中的的空空间间描描述述，，即即已知q1,q2,…,qn，求，称为运动学正解；已知，求q1,q2,…,qn，称为运动学反解。上式式称称为为运动动方方程程。综上上：：正解解反解解§1.5机器器人人的的逆逆运运动动学学解解给定定机机器器人人终终端端位位姿姿，，求求各各关关节节变变量量，，称求求机机器器人人运运动动学学逆逆解解。让让我我们们通通过过下下面面这这道道例例题题来来了了解解一一下下机机器器人人逆逆运运动动学学求求解解的的一一般般步步骤骤。。前前面面例例子子最最后后方方程程为为：：求逆逆运运动动学学方方程程的的解解根据据第第3行第第4列元元素素对对应应相相等等可可得得到到依次次用用左左乘乘上上面面两两个个矩矩阵阵，，得得到到：：根据据1，4元素素和和2，4元素素，，可可得得到到：：将上上面面两两个个方方程程两两边边平平方方相相加加，，并并利利用用和和差差化化积积公公式式得得到到已知知于是是可可得得到到：：依次次类类推推，，分分别别在在方方程程2.19两边边左左乘乘A1~A4的逆逆，，可可得得到到接下下来来再再一一次次利利用用式式由于于C12=C1C2-S1S2以及及S12=S1C2+C1S2，最最后后得得到到：：最后后用用A5的逆逆左左乘乘式式2.67，再再利利用用2,1元素素和和2,2元素素，，得得到到：：θ1θ2θ3θ4θ5θ6关节变量都是θ§2.10机器器人人的的运运动动学学编编程程在实实际际应应用用中中，，对对运运动动学学的的求求解解是是相相当当繁繁琐琐和和耗耗时时的的，，因因此此需需要要用用计计算算机机编编程程来来实实现现。。并并且且应应尽尽量量避避免免使使用用矩矩阵阵求求逆逆或或高高斯斯消消去去法法等等相相对对繁繁琐琐的的算算法法。。正正确确的的算算法法是是：：§2.11设计项目目利用本书书中所介介绍的四四自由度度机器人人，结合合本章所所学的知知识进行行四自由由度机器器人的正正逆运动动学分析析。SCARA型机器人人的运动动学模型型的建立立，包括括机器人人运动学学方程的的表示，，以及运运动学正正解、逆逆解等，，这些是是研究机机器人控控制的重重要基础础，也是是开放式式机器人人系统轨轨迹规划划的重要要基础。。为了描描述SCARA型机器人人各连杆杆之间的的数学关关系，采采用D-H法。SCARA型机器人人操作臂臂可以看看作是一一个开式式运动链链。它是是由一系系列连杆杆通过转转动或移移动关节节串联而而成的。。为了研研究操作作臂各连连杆之间间的位移移关系，，可在每每个连杆杆上固接接一个坐坐标系，，然后描描述这些些坐标系系之间的的关系。。SCARA（SelectiveComplianceAssemblyRobotArm装配机器器人臂））机器人人坐标系系的建立立

1.SCARA机器人坐标系建立原则根据D-H坐标系建立方法，SCARA机器人的每个关节坐标系的建立可参照以下的三原则(1)轴沿着第n个关节的运动轴;基坐标系的选择为:当第一关节变量为零时，零坐标系与一坐标系重合。(2)轴垂直于轴并指向离开轴的方向。(3)轴的方向按右手定则确定。

2.构件参数的确定根据D-H构件坐标系表示法，构件本身的结构参数、和相对位置参数、可由以下的方法确定:(1)为绕轴(按右手定则)由轴到轴的关节角。(2)为沿轴，将轴平移至轴的距离。(3)为沿轴从量至轴的距离。(4)为绕轴(按右手定则)由轴到轴的偏转角。3.变换矩阵阵的建立立全部的的连杆规规定坐标标系之后后，就可可以按照照下列的的顺序来来建立相相邻两连连杆n-1和n之间的相相对关系系:(1)绕轴轴转转角角。(2)沿轴轴移移动。。(3)绕轴轴转角角。。(4)沿轴轴移动动。。这种关系系可由表表示连杆杆n对连杆n-1相对位置置齐次变变换来来表表征。即即：展开上式式得由于描描述述第n个连杆相相对于第第n-1连杆的位位姿，对对于SCARA教学机器器人(四个自由由度)，机器人人的末端端装置即即为连杆杆4的坐标系系，它与与基座的的关系为为:如上图坐坐标系，，可写出出连杆n相对于n-1变换矩阵阵:其中：以以下相同同。相应连杆杆初始位位置及参参数列于于表2.4，表中、、为为关节节变量。。构件1000102001030010400010各连杆变变换矩阵阵相乘，，可得到到SCARA机器人末末端执行行器的位位姿方程程(正运动学学方程)为下式式它表示示了SCARA手臂变换换矩阵，，它描描述了末末端连杆杆坐标系系{4}相对基坐坐标系{0}的位姿。。SCARA机器人的的正运动动学分析析SCARA机器人的的逆运动动学分析析1.求关节变变量为为了分离离变量，，对方程程的两边边同时左左乘,得:即：左右矩阵阵中的第第一行第第四个元元素(1.4)，第二行行第四个个元素(2.4)分别相等等。即:由以上两两式联立立可得:式中：2求关节变量由式(2.87)可得:

式中：3求关节变量再令左右矩阵中的第三行第四个元素(3.4)相等，可得:

4求关节变量再令左右矩阵中的第一行第一个元素、第二行第一个元素(1.1，2.1)分别相等，即：由上两式可求得:

至此，机器人的所有运动学逆解都已求出。在逆解的求解过程中只进行了一次矩阵逆乘，从而使计算过程大为简化，从的表达式中可以看出它有两个解，所以SCARA机器人应该存在两组解。运动学分析提供了机器人运动规划和轨迹控制的理论基础。对机器人人相关概概念的补补充退化：当机器人人失去一一个自由由度，并并因此不不按所期期望的状状态运动动时即称称为退化化。退化发生生条件：：1.机器人达达到物理理极限，，不能进进一步运运动2.两个相似似关节共共线不灵巧区区域：能对机器器人定位位不定姿姿的区域域称为不不灵巧区区域。D-H法的局限限性：无法表示示关于y轴的运动动。退化状态态下的机机器人总结1用矩阵表表示点，，向量，，坐标系系及变换换的方法法2正逆运运动学学方程程的建建立3用D-H法建立立坐标标系及及变化化方程程4正逆运运动学学方程程的求求解§9.2机器人人杆件件，关关节和和它们们的参参数§9.2.1杆件与与关节节操作机机由一一串用用转动动或平平移（（棱柱柱形））关节节连接接的刚刚体（（杆件件）组组成每一对关节节杆件构成成一个自由度，因此N个自由度的的操作机就就有N对关节—杆件。0号杆件（一一般不把它它当作机器器人的一部部分）固联联在机座上上，通常在在这里建立立一个固定定参考坐标标系，最后后一个杆件件与工具相相连关节和杆件件均由底座座向外顺序序排列，每每个杆件最最多和另外外两个杆件件相联，不不构成闭环环。关节杆件末端操作手手机座两自由度关关节关节：一般说来，，两个杆件件间是用低副相联的只可能有6种低副关节节：旋转（转动）、、棱柱（移动）、、圆柱形、球形、螺旋和平面，其中只有有旋转和棱柱柱形关节是串联联机器人操操作机常见见的，各种种低副形状状如下图所所示：旋转棱柱形柱形球形螺旋形平面§9.2.2杆件参数的的设定条件关节串联每个杆件最多与2个杆件相连连，如Ai与Ai-1和Ai+1相连。第i关节的关节节轴Ai位于2个杆件相连连接处，如如图所示，，i-1关节和i+1关节也各有有一个关节节轴Ai-1和Ai+1。AiAi+1Ai-1杆件参数的的定义——、、和和—li和li-1在Ai轴线上的交点点之间的距离。AiAi+1Ai-1—li和li-1之间的夹角，按右手手定则由li-1转向li。

由运动学的观点来看，杆件保持其两端关节间的形态不变，这种形态由两个参数决定：杆件长度li

和杆件扭转角。杆件的相对位置关系，由另外两个参数决定：杆件的距离di和杆件的回转角。li—关节Ai轴和Ai+1轴线公法线线的长度。。—关节i轴线与i+1轴线在垂直直于li平面内的夹夹角。上述4个参数，就就确定了杆杆件的结构构形态和相相邻杆件相相对位置关关系。在转转动关节中中，li,αi,di是固定值，，θi是变量。在在移动关节节中，li,αi,θi是固定值，，di是变量。§9.3机器人关节节坐标系的的建立对于每个杆杆件都可以以在关节轴轴处建立一一个正规的的笛卡儿坐坐标系（xi,yi,zi），（i=1,2,…,n），n是自由度数数，再加上上基座坐标标系，一共共有（n+1）个坐标系系。基座坐标系系∑O0定义为0号坐标系（（x0,y0,z0）,它也是机器器人的惯性性坐标系，，0号坐标系在在基座上的的位置和方方向可任选选，但z0轴线必须与与关节1的轴线重合合，位置和和方向可任任选；最后一个坐坐标系（n关节），可可以设在手手的任意部部位，但必须保证zn与zn-1垂直。机器人关节节坐标系的的建立主要要是为了描描述机器人人各杆件和和终端之间间的相对运运动，对建建立运动方方程和动力力学研究是是基础性的的工作。为了描述机机器人各杆杆件和终端端之间转动动或移动关关系，Denavit和Hartenberg于1955年提出了一一种为运动动链中每个个杆件建立立附体坐标标系的矩阵阵方法（D-H方法），建立原则则如下：§9.3.1D-H关节坐标系系建立原则则右手坐标系系原点Oi：设在li与Ai+1轴线的交点点上Zi轴：与与Ai+1关节轴重合合，指向任任意Xi轴：与与公法法线Li重合，指向向沿Li由Ai轴线指向Ai+1轴线Yi轴：按按右手手定则§9.3.2关节坐标系系的建立方方法AiAi+1Ai-1原点Oi：设在li与