机器人运动学正解逆解PPT学习教案

1、会计学1机器人运动学正解逆解机器人运动学正解逆解学习目标：学习目标：1. 理解理解D-H法原理法原理 2. 学会用学会用D-H法对机器人建模法对机器人建模学习重点：学习重点：1. 给关节指定参考坐标系给关节指定参考坐标系 2. 制定制定D-H参数表参数表 3. 利用参数表计算转移矩阵利用参数表计算转移矩阵第1页/共60页背景简介：背景简介： 1955年，年，Denavit和和Hartenberg(迪纳维特和哈坦伯格迪纳维特和哈坦伯格)提出提出了这一方法，后成为表示机器人以及对机器人建模的标准方法，了这一方法，后成为表示机器人以及对机器人建模的标准方法，应用广泛。应用广泛。 总体思想：总体思想：

2、 首先给每个关节指定坐标系，然后确定从一个关节到下一个首先给每个关节指定坐标系，然后确定从一个关节到下一个关节进行变化的步骤，这体现在两个相邻参考坐标系之间的变关节进行变化的步骤，这体现在两个相邻参考坐标系之间的变化，将所有变化结合起来，就确定了末端关节与基座之间的总化，将所有变化结合起来，就确定了末端关节与基座之间的总变化，从而建立运动学方程，进一步对其求解。变化，从而建立运动学方程，进一步对其求解。第2页/共60页1.1.第一个关节指定为关节第一个关节指定为关节n,n,第二个关节为第二个关节为n+1,n+1,其余其余关节以此类推。关节以此类推。坐标系的确定坐标系的确定2.Z2.Z轴确定规则

3、：轴确定规则：如果关如果关节是旋转的，节是旋转的，Z Z轴位于按轴位于按右手规则旋转的方向右手规则旋转的方向，转角转角 为关节变量。如为关节变量。如果关节是滑动的，果关节是滑动的，Z Z轴为轴为沿直线运动的方向沿直线运动的方向，连，连杆长度杆长度d d为关节变量。为关节变量。关关节节n n处处Z Z轴下标为轴下标为n-1n-1。 第3页/共60页3.X3.X轴确定规则轴确定规则情况情况1 1：两关节：两关节Z Z轴既不平行也不相交轴既不平行也不相交取两取两Z Z轴公垂线方向作为轴公垂线方向作为X X轴方向，命名规则同轴方向，命名规则同Z Z轴。轴。情况情况2 2：两关节：两关节Z Z轴平行轴平

4、行此时，两此时，两Z Z轴之间有无数条公垂线，可挑选与前一关节的公垂线共线的轴之间有无数条公垂线，可挑选与前一关节的公垂线共线的一条公垂线。一条公垂线。情况情况3 3：两关节：两关节Z Z轴相交轴相交取两条取两条Z Z轴的叉积方向作为轴的叉积方向作为X X轴。轴。4.Y4.Y轴确定原则轴确定原则取取X X轴、轴、Z Z轴叉积方向作为轴叉积方向作为Y Y轴方向。（右手）轴方向。（右手）5.5.变量选择原则变量选择原则用用n+1n+1角表示角表示XnXn到到Xn+1Xn+1绕绕ZnZn轴的旋转角轴的旋转角; ;d dn+1n+1表示从表示从XnXn到到Xn+1Xn+1沿沿ZnZn测量测量的距离的距

5、离; ;a an+1n+1表示关节偏移，表示关节偏移，an+1an+1是从是从ZnZn到到Zn+1Zn+1沿沿Xn+1Xn+1测量的距离测量的距离; ;角角表示关节扭转表示关节扭转, n+1, n+1是从是从ZnZn到到Zn+1Zn+1绕绕Xn+1Xn+1旋转的角度。旋转的角度。 通常情况下，通常情况下，只有只有和和d d是关节变量。是关节变量。第4页/共60页斯坦福机器人斯坦福机器人斯坦福机器人开始的两个关节是旋转的，第三个关节是滑动的，最后三个腕关节全是旋转关节第5页/共60页第6页/共60页第7页/共60页A1A2A3A4A5A6d1z1x1y1O1d2z2x2y2O2z3y3x3O3y

6、4z4x4O4z5y5x5O534545,0o o odd重重合合d3z6x6y6O6d6z0y0 x0O0ai沿沿 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第8页/共60页解：解：第9页/共60页第10页/共60页第11页

7、/共60页第12页/共60页123456关节变量都是关节变量都是第13页/共60页PUMA560机器人的连杆及关节编号第14页/共60页为右手坐标系，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

8、坐标坐标 系原点的距离系原点的距离i 绕绕 zi-1 轴，由轴，由 xi-1转向转向 xiA1A2A3A4A5A6O1O0第15页/共60页第16页/共60页第17页/共60页 对下图所示简单机器人，根据对下图所示简单机器人，根据D-H法，建立必要坐标系及法，建立必要坐标系及参数表。参数表。第18页/共60页第一步：根据第一步：根据D-H法建立坐标系的规则建立坐标系法建立坐标系的规则建立坐标系 第19页/共60页第二步：将做好的坐标系简化为我们熟悉的线图形式第二步：将做好的坐标系简化为我们熟悉的线图形式第20页/共60页第三步：根据建立好的坐标系，确定各参数，并第三步：根据建立好的坐标系，确定

9、各参数，并写入写入D-H参数表参数表#da1009020030040-905009060001234562a3a4a第21页/共60页 10000),()0 , 0 ,(), 0 , 0(111111111111111111111),(111nnnnnnnnnnnnnnnnnnnnnznnndCSSaSCCCSCaSSCSCAxRotaTransdTransRotATn #da1009020030040-905009060001234562a3a4a 10000010000011111CSSCA第22页/共60页第四步：将参数代入第四步：将参数代入A矩阵，可得到矩阵，可得到 100000100

10、00011111CSSCA 1000010000222222222aSCSaCSCA 1000010000333333333aSCSaCSCA 1000001000444444444aSCSaCSCA 10000010000055555CSSCA 10000100000066666CSSCA第23页/共60页第第5步步 求出总变化矩阵求出总变化矩阵 1000)()()()()()()()(22323423452346234652346523422323423415152341651623465234165162346523412232342341515234165162346523416516

11、234652341654321aSaSaSSSCCCCSCCSaCaCaCSCCSCSSSCCSCCCSSSCSSCCCSaCaCaCCCSSCCSSSCSCCCCCSSSSCCCCAAAAAATHR第24页/共60页 依次写出从基坐标系到手爪坐标系之间相邻两坐标系的依次写出从基坐标系到手爪坐标系之间相邻两坐标系的齐次齐次变换矩阵变换矩阵，它们依次连乘的结果就是末端执行器（手爪）在基坐，它们依次连乘的结果就是末端执行器（手爪）在基坐标系中的空间描述，即标系中的空间描述，即已知已知q q1 1,q,q2 2, ,q,qn n，求，求 ，称为运动学正解；，称为运动学正解；已知已知 ，求，求q q1

12、 1,q,q2 2, ,q,qn n，称为运动学反解。，称为运动学反解。上式称为上式称为运动方程运动方程。101000)()(0n0n1 -n221110nOPRpaonTqTqT综上：综上：第25页/共60页正解正解反解反解第26页/共60页 1000zzzzyyyyxxxxHRpaonpaonpaonT 1000)()()()()()()()(22323423452346234652346523422323423415152341651623465234165162346523412232342341515234165162346523416516234652341654321aSaSaS

13、SSCCCCSCCSaCaCaCSCCSCSSSCCSCCCSSSCSSCCCSaCaCaCCCSSCCSSSCSCCCCCSSSSCCCCAAAAAATHR 给定机器人终端位姿，求各关节变量，给定机器人终端位姿，求各关节变量，称求机器人运动学逆解称求机器人运动学逆解。让我们通过下面这道例题来了解一下机器人逆运动学求解的一般步让我们通过下面这道例题来了解一下机器人逆运动学求解的一般步骤。前面例子最后方程为：骤。前面例子最后方程为：求逆运动学方程的解求逆运动学方程的解第27页/共60页 根据第根据第3行第行第4列元素对应相等可得到列元素对应相等可得到依次用依次用 左乘上面两个矩阵，得到：左乘上面

14、两个矩阵，得到：11A 10000100056565223234234523462346523462346523422323423452346234652346234652341111111111111111CSSCSaSaSaSSSCCCCSSCCCSaCaCaCSCCSCCCSSCCCCPSPCaSaCoSoCnSnpaonSPCPSaCaSoCoSnCnyxyxyxyxzzZzyxyxyxyx180)arctan(111 和和xypp第28页/共60页根据根据1，4元素和元素和2，4元素，可得到：元素，可得到：将上面两个方程两边平方相加，并利用和差化积公式得到将上面两个方程两边平方相加，

15、并利用和差化积公式得到322322242342423411332322322)()(cosaaaaaSpaCSpCpCCCSSzyx 于是有：于是有： 22323423422323423411aSaSaSpaCaCaCSpCpzyx 第29页/共60页已知已知2331CS 于是可得到：于是可得到：333arctanCS 依次类推，分别在方程依次类推，分别在方程2.19两边左乘两边左乘A1A4的逆，可得到的逆，可得到 100000001000)()()()(0)()()()(6656565565653423423411234234112342341123423411234111111434234

16、23411234234112342341123423411234CSCSSCSSSCCCaSaSpCPSPCSaCaSaCSoCoSoCSnCnSnCSaSaCoSoCnSnCaaCaCpSpSpCCaSaSaCCoSoSoCCnSnSnCCzyxzyxzyxzyxxyxyxyzyxxyxzyxzyxzyxyxzaaSaaSaCa)CSC180)arctan(1123423423423411234 （和和 第30页/共60页接下来再一次利用式接下来再一次利用式由于由于C12=C1C2-S1S2以及以及S12=S1C2+C1S2，最后得到：，最后得到：yxzyxxyzyxzyxyxzaCaSaS

17、aSaCCaSaCCaSaSaaSpaSaCSpCpaaCaCSpCpaSaSpaaC112341123451152341123453223444234334234112334234113342342332)(arctan)CCS)()()()(arctan （，可以得到，可以得到再根据对应项元素相等再根据对应项元素相等进而可得：进而可得：22323423422323423411aSaSaSpaCaCaCSpCpzyx 第31页/共60页最后用最后用A5的逆左乘式的逆左乘式2.67，再利用，再利用2,1元素和元素和2,2元素，得到：元素，得到：zyxzyxoCoSoCSnCnSnCS23411

18、234234112346)()(arctan 123456关节变量都是关节变量都是第32页/共60页 在实际应用中，对运动学的求解是相当繁琐和耗时的，因此在实际应用中，对运动学的求解是相当繁琐和耗时的，因此需要用计算机编程来实现。并且应尽量避免使用矩阵求逆或高斯需要用计算机编程来实现。并且应尽量避免使用矩阵求逆或高斯消去法等相对繁琐的算法。正确的算法是：消去法等相对繁琐的算法。正确的算法是：333arctanCS yxzyxzyxyxzaCaSaSaSaCCaSpaSaCSpCpaaCaCSpCpaSaSpaaC11234112345322344423433423411233423411334

19、2342332)(arctan)()()()(arctan zyxzyxoCoSoCSnCnSnCS23411234234112346)()(arctan )arctan(1xypp 第33页/共60页 利用本书中所介绍的四自由度机器人，结合本章所学的知识利用本书中所介绍的四自由度机器人，结合本章所学的知识进行四自由度机器人的正逆运动学分析。进行四自由度机器人的正逆运动学分析。 SCARASCARA型机器人的运动学模型的建立，包括机器人运动学方型机器人的运动学模型的建立，包括机器人运动学方程的表示，以及运动学正解、逆解等，这些是研究机器人控制的程的表示，以及运动学正解、逆解等，这些是研究机器人

20、控制的重要基础，也是开放式机器人系统轨迹规划的重要基础。为了描重要基础，也是开放式机器人系统轨迹规划的重要基础。为了描述述SCARASCARA型机器人各连杆之间的数学关系，采用型机器人各连杆之间的数学关系，采用D-HD-H法。法。SCARASCARA型型机器人操作臂可以看作是一个开式运动链。它是由一系列连杆通机器人操作臂可以看作是一个开式运动链。它是由一系列连杆通过转动或移动关节串联而成的。为了研究操作臂各连杆之间的位过转动或移动关节串联而成的。为了研究操作臂各连杆之间的位移关系，可在每个连杆上固接一个坐标系，然后描述这些坐标系移关系，可在每个连杆上固接一个坐标系，然后描述这些坐标系之间的关系

21、。之间的关系。第34页/共60页SCARASCARA（Selective Compliance Assembly Robot ArmSelective Compliance Assembly Robot Arm装配机器人臂）机器人坐标系的建立装配机器人臂）机器人坐标系的建立 1.SCARA机器人坐标系建立原则根据机器人坐标系建立原则根据D-H坐标系建立方法，坐标系建立方法，SCARA机器人的每个关节坐标系的建立可参照以下的三原则机器人的每个关节坐标系的建立可参照以下的三原则(1) 轴沿着第轴沿着第n个关节的运动轴个关节的运动轴;基坐标系的选择为基坐标系的选择为:当第一关当第一关节变量为零时，零

22、坐标系与一坐标系重合。节变量为零时，零坐标系与一坐标系重合。(2) 轴垂直于轴垂直于 轴并指向离开轴并指向离开 轴的方向。轴的方向。(3) 轴的方向按右手定则确定。轴的方向按右手定则确定。 2.构件参数的确定根据构件参数的确定根据D-H构件坐标系表示法，构件本身构件坐标系表示法，构件本身的结构参数的结构参数 、 和相对位置参数和相对位置参数 、 可由以下的方法确可由以下的方法确定定:(1) 为绕为绕 轴轴(按右手定则按右手定则)由由 轴到轴到 轴的关节角。轴的关节角。(2) 为沿为沿 轴，将轴，将 轴平移至轴平移至 轴的距离。轴的距离。(3) 为沿为沿 轴从轴从 量至量至 轴的距离。轴的距离。

23、(4) 为绕为绕 轴轴(按右手定则按右手定则)由由 轴到轴到 轴的偏转角。轴的偏转角。nznxnznynz1na1nndnnnz1nxnxndnz1nxnx1na1nx1nznz1n1nx1nznz第35页/共60页 3.变换矩阵的建立全部的连杆规定坐标系之后，就可以按变换矩阵的建立全部的连杆规定坐标系之后，就可以按照下列的顺序来建立相邻两连杆照下列的顺序来建立相邻两连杆n-1和和n之间的相对关系之间的相对关系:(1)绕绕 轴转轴转 角。角。(2)沿沿 轴移动轴移动 。(3)绕绕 轴转轴转 角。角。(4)沿沿 轴移动轴移动 。这种关系可由表示连杆这种关系可由表示连杆n对连杆对连杆n-1相对位置

24、齐次变换相对位置齐次变换 来表来表征。即：征。即：展开上式得展开上式得 1nx1n1nx1nanznnzndnnT1),(),(),(),(11111nntnnrnntnnrnndzTzTaxTxTT111111111cossin0sincoscoscossinsinsinsincossincoscos0001nnnnnnnnnnnnnnnnnnnadTd第36页/共60页由于由于 描述第描述第n个连杆相对于第个连杆相对于第n-1连杆的位姿，对于连杆的位姿，对于SCARA教学机器人教学机器人(四个自由度四个自由度)，机器人的末端装置即为连杆，机器人的末端装置即为连杆4的坐标系，它与基座的关系为

25、的坐标系，它与基座的关系为: nnT10012341234TT T T T第37页/共60页如上图坐标系，可写出连杆如上图坐标系，可写出连杆n相对于相对于n-1变换矩阵变换矩阵 : 其中：其中： 以下相同。以下相同。相应连杆初始位置及参数列于表相应连杆初始位置及参数列于表2.4，表中，表中 、 为关节变量。为关节变量。 nnT1111101000000100001csscT221221200000100001cslscT223310001000010001lTd444434000000100001csscTcos,sinnnnncsnnd构件10001020010300104000101na1

26、nndn1cosn1sinn1l2l3d124第38页/共60页 各连杆变换矩阵相乘，可得到各连杆变换矩阵相乘，可得到SCARA机器人末端执行器的位机器人末端执行器的位姿方程姿方程(正运动学方程正运动学方程)为下为下 式它表示了式它表示了SCARA手臂变换矩阵手臂变换矩阵 ，它，它描述了末端连杆坐标系描述了末端连杆坐标系4相对基坐标系相对基坐标系0的位姿的位姿 。SCARA机器人的正运动学分析机器人的正运动学分析 40T 001234112233441 2 41 2 41 2 41 2 41 2 41 2 41 2 41 2 41 221 22111 2 41 2 41 2 41 2 41 2

27、 41 2 41 2 41 2 41 20 0 0 100 xxxxyyyyzzzzn o apn o apTTTT d Tn o apccc sss css scsccs sss csc sccccl ssl clscc csc sss ccsscs css ssc cccsc21 221130010000l csl sld第39页/共60页 SCARA机器人的逆运动学分析机器人的逆运动学分析 1.求关节变量求关节变量 为了分离变量，对方程的两边同时为了分离变量，对方程的两边同时左乘左乘 ,得得:即：即：1 0111T 010123114223344TTTTdT112 42 42 42 42

28、 21112 42 42 42 42 230 000 0000 1 000100 0 100010001xxxxyyyyzzzzcsnoapccs sc ssccllscnoapscc ss sccslnoapd第40页/共60页 左右矩阵中的第一行第四个元素左右矩阵中的第一行第四个元素(1.4)，第二行第四个元，第二行第四个元素素(2.4)分别相等。即分别相等。即: 由以上两式联立可得由以上两式联立可得: 式中：式中： 112211122cossincossincossinxyxyppllppl211arctanAA222212221;arctan2xyyxxyllpppAplpp第41页/

29、共60页2 求关节变量求关节变量 由式由式(2.87)可得可得: 式中：式中：21211sinarctancosrrl22;arctanyxyxprppp第42页/共60页3 求关节变量求关节变量 再令左右矩阵中的第三行第四个元再令左右矩阵中的第三行第四个元素素(3.4)相等，可得相等，可得: 4 求关节变量求关节变量 再令左右矩阵中的第一行第一个元再令左右矩阵中的第一行第一个元素、第二行第一个元素素、第二行第一个元素(1.1，2.1)分别相等，即：分别相等，即：由上两式可求得由上两式可求得: 3d3zdp 4112424112424cossincoscossinsinsincossincos

30、cossinxyxynnnn114211sincosarctancossinxyxynnnn第43页/共60页 至此，机器人的所有运动学逆解都已求出。在逆解至此，机器人的所有运动学逆解都已求出。在逆解的求解过程中只进行了一次矩阵逆乘，从而使计算过程大的求解过程中只进行了一次矩阵逆乘，从而使计算过程大为简化，从为简化，从 的表达式中可以看出它有两个解，所以的表达式中可以看出它有两个解，所以SCARA机器人应该存在两组解。运动学分析提供了机器机器人应该存在两组解。运动学分析提供了机器人运动规划和轨迹控制的理论基础。人运动规划和轨迹控制的理论基础。1 第44页/共60页对机器人相关概念的补充对机器人

31、相关概念的补充退化：退化：当机器人失去一个自由度，并因此不按所期望的状当机器人失去一个自由度，并因此不按所期望的状态运动时即称为退化。态运动时即称为退化。退化发生条件：退化发生条件：1.机器人达到物理极限，不能进一步运动机器人达到物理极限，不能进一步运动2.两个相似关节共线两个相似关节共线不灵巧区域：不灵巧区域：能对机器人定位不定姿的区域称为不灵巧区能对机器人定位不定姿的区域称为不灵巧区域。域。D-H法的局限性：法的局限性：无法表示关于无法表示关于y轴的运动。轴的运动。第45页/共60页退化状态下的机器人退化状态下的机器人第46页/共60页1 用矩阵表示点，向量，坐标系及变换的方法用矩阵表示点

32、，向量，坐标系及变换的方法2 正逆运动学方程的建立正逆运动学方程的建立3 用用D-H法建立坐标系及变化方程法建立坐标系及变化方程4 正逆运动学方程的求解正逆运动学方程的求解第47页/共60页第48页/共60页旋转旋转棱柱形棱柱形柱形柱形球形球形螺旋形螺旋形平面平面第49页/共60页AiAi+1Ai-1第50页/共60页iidiAiAi+1iilid1iliAi-1idn l 和和 l之间的之间的夹夹 角，按右手定则角，按右手定则 由由l转向转向 l。 由运动学的观点来看，杆件保持其两端关节间的形态由运动学的观点来看，杆件保持其两端关节间的形态不变，这种形态由两个参数决定：杆件长度不变，这种形态

33、由两个参数决定：杆件长度 li 和杆件扭和杆件扭转角转角 。杆件的相对位置关系，由另外杆件的相对位置关系，由另外两两个参数决定：个参数决定：杆件的距离杆件的距离 di 和杆件的回转角和杆件的回转角 。iiilin li ii 第51页/共60页v 上述上述4个参数，就确定了杆件的结构形态和相邻杆件相个参数，就确定了杆件的结构形态和相邻杆件相对位置关系。在转动关节中，对位置关系。在转动关节中，li, i, di是固定值，是固定值，i是变量。是变量。在移动关节中，在移动关节中，li, i , i是固定值，是固定值， d i 是变量。是变量。第52页/共60页n 对于每个杆件都可以在关节轴处建立一个正规的笛对于每个杆件都可以在关节轴处建立一个正规的笛卡儿坐标系（卡儿坐标系（xi, yi, zi），（），（i=1, 2, , n），），n是自由是自由度数，再加上基座坐标系，一共有（度数，再加上基座坐标系，一共有（n+1）个坐标）个坐标系。系。n 基座坐标系基座坐标系 O0定义为定义为0号坐标系（号坐标系（x0, y0, z0）,它也它也是机器人的惯性坐标系，是机器人的惯性坐标系，0号坐标系在基座上的位置号坐标系在基座上的位置