机器人运动学正解逆解课件.ppt

§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机器人运动学方程A1A2A3A4A5A6d1z1x1y1O1d2z2x2y2O2z3y3x3O3y4z4x4O4z5y5x5O534545,,0o o odd???重合d3z6x6y6O6d6z0y0x0O0?为右手坐标系?原点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连杆n θn dn an αn 1 θ1 (900) 0 0 -900 2 θ2 (0) d2 a2 0 3 θ3 (-900) 0 a3 -900 4 θ4 (0) d4 0 900 5 θ5 (0) 0 0 -900 6 θ6 (0) 0 0 0 对下图所示简单机器人，根据D-H法，建立必要坐标系及参数表。

例例 3第一步：根据D-H法建立坐标系的规则建立坐标系第二步：将做好的坐标系简化为我们熟悉的线图形式第三步：根据建立好的坐标系，确定各参数，并写入D-H参数表#da1009020030040-90500906000??1?2?3?4?5?6?2a3a4a????????????????????????????????????????????10000),()0, 0 ,(), 0 , 0(111111111111111111111),(111nnnnnnnnnnnnnnnnnnnnnznnndCSSaSCCCSCaSSCSCAxRotaTransdTransRotATn????????????????#da1009020030040-90500906000??1?2?3?4?5?6?2a3a4a??????????????10000010000011111CSSCA第四步：将参数代入第四步：将参数代入 A矩矩阵阵，可得到，可得到??????????????10000010000011111CSSCA??????????????1000010000222222222aSCSaCSCA??????????????1000010000333333333aSCSaCSCA???????????????1000001000444444444aSCSaCSCA??????????????10000010000055555CSSCA??????????????10000100000066666CSSCA第5步 求出总变化矩阵????????????????????????????????????????1000)()()()()()()()(22323423452346234652346523422323423415152341651623465234165162346523412232342341515234165162346523416516234652341654321aSaSaSSSCCCCSCCSaCaCaCSCCSCSSSCCSCCCSSSCSSCCCSaCaCaCCCSSCCSSSCSCCCCCSSSSCCCCAAAAAATHR依次写出从基坐标系到手爪坐标系之间相邻两坐标系的齐次变换矩阵，它们依次连乘的结果就是末端执行器（手爪）在基坐标系中的空间描述，即已知q1,q2,…,qn，求，称为运动学正解；已知，求q1,q2,…,qn，称为运动学反解。

上式称为运动方程101000)()(0n0n1-n221110nOPRpaonTqTqT?????综上：正解正解反解反解§1.5￿机器人的逆运动学解?????????????1000zzzzyyyyxxxxHRpaonpaonpaonT????????????????????????????????????????1000)()()()()()()()(22323423452346234652346523422323423415152341651623465234165162346523412232342341515234165162346523416516234652341654321aSaSaSSSCCCCSCCSaCaCaCSCCSCSSSCCSCCCSSSCSSCCCSaCaCaCCCSSCCSSSCSCCCCCSSSSCCCCAAAAAATHR给定机器人终端位姿，求各关节变量， 称求机器人运动学逆解 让我们通过下面这道例题来了解一下机器人逆运动学求解的一般步骤前面例子最后方程为：求逆运动学方程的解根据第3行第4列元素对应相等可得到依次用左乘上面两个矩阵，得到：11?A????????????????????????????????????????????10000100056565223234234523462346523462346523422323423452346234652346234652341111111111111111CSSCSaSaSaSSSCCCCSSCCCSaCaCaCSCCSCCCSSCCCCPSPCaSaCoSoCnSnpaonSPCPSaCaSoCoSnCnyxyxyxyxzzZzyxyxyxyx?180)arctan(111??????和xypp根据1，4元素和2，4元素，可得到：将上面两个方程两边平方相加，并利用和差化积公式得到322322242342423411332322322)()(cosaaaaaSpaCSpCpCCCSSzyx?????????于是有：?22323423422323423411aSaSaSpaCaCaCSpCpzyx???????已知2331CS???于是可得到：333arctanCS??依次类推，分别在方程 2.19两边左乘A1~A4的逆，可得到??????????????????????????????????????????????????????????????100000001000)()()()(0)()()()(665656556565342342341123423411234234112342341123411111143423423411234234112342341123423411234CSCSSCSSSCCCaSaSpCPSPCSaCaSaCSoCoSoCSnCnSnCSaSaCoSoCnSnCaaCaCpSpSpCCaSaSaCCoSoSoCCnSnSnCCzyxzyxzyxzyxxyxyxyzyxxyxzyxzyxzyxyxzaaSaaSaCa)CSC180)arctan(1123423423423411234??????（和????接下来再一次利用式由于C12=C1C2-S1S2 以及S12=S1C2+C1S2 ，最后得到：yxzyxxyzyxzyxyxzaCaSaSaSaCCaSaCCaSaSaaSpaSaCSpCpaaCaCSpCpaSaSpaaC112341123451152341123453223444234334234112334234113342342332)(arctan)CCS)())(()())((arctan??????????????????????????????（，可以得到再根据对应项元素相等进而可得：22323423422323423411aSaSaSpaCaCaCSpCpzyx???????最后用A5的逆左乘式2.67，再利用2,1元素和2,2元素，得到：zyxzyxoCoSoCSnCnSnCS23411234234112346)()(arctan????????θ1θ2θ3θ4θ5θ6关节变量都是θ§2.10 机器人的运动学编程在实际应用中，对运动学的求解是相当繁琐和耗时的，因此需要用计算机编程来实现。

并且应尽量避免使用矩阵求逆或高斯消去法等相对繁琐的算法正确的算法是：333arctanCS??yxzyxzyxyxzaCaSaSaSaCCaSpaSaCSpCpaaCaCSpCpaSaSpaaC112341123453223444234334234112334234113342342332)(arctan)())(()())((arctan????????????????????????zyxzyxoCoSoCSnCnSnCS23411234234112346)()(arctan????????)arctan(1xypp??§2.11 设计项目利用本书中所介绍的四自由度机器人，结合本章所学的知识进行四自由度机器人的正逆运动学分析SCARA 型机器人的运动学模型的建立，包括机器人运动学方程的表示，以及运动学正解、逆解等，这些是研究机器人控制的重要基础，也是开放式机器人系统轨迹规划的重要基础为了描述SCARA 型机器人各连杆之间的数学关系，采用 D-H法SCARA型机器人操作臂可以看作是一个开式运动链它是由一系列连杆通过转动或移动关节串联而成的为了研究操作臂各连杆之间的位移关系，可在每个连杆上固接一个坐标系，然后描述这些坐标系之间的关系。

SCARA（Selective￿Compliance￿Assembly￿Robot￿Arm装配机器人臂）机器人坐标系的建立1.SCARA机器人坐标系建立原则根据D-H坐标系建立方法，SCARA机器人的每个关节坐标系的建立可参照以下的三原则(1) 轴沿着第n个关节的运动轴;基坐标系的选择为:当第一关节变量为零时，零坐标系与一坐标系重合2) 轴垂直于轴并指向离开 轴的方向3) 轴的方向按右手定则确定2.构件参数的确定根据D-H构件坐标系表示法，构件本身的结构参数、 和相对位置参数、 可由以下的方法确定:(1) 为绕轴(按右手定则)由轴到轴的关节角2) 为沿轴，将轴平移至轴的距离3) 为沿轴从量至轴的距离4) 为绕轴(按右手定则)由轴到轴的偏转角nznxnznynz1?na1?n?ndn?n?nz1?nxnxndnz1?nxnx1?na1?nx1?nznz1?n?1?nx1?nznz3.变换矩阵的建立全部的连杆规定坐标系之后，就可以按照下列的顺序来建立相邻两连杆n-1和n之间的相对关系:(1)绕轴转角2)沿轴移动3)绕轴转角4)沿轴移动这种关系可由表示连杆n对连杆n-1相对位置齐次变换来表征。

即：展开上式得1?nx1?n?1?nx1?nanzn?nzndnnT1?),(),(),(),(11111nntnnrnntnnrnndzTzTaxTxTT????????111111111cossin0sincoscoscossinsinsinsincossincoscos0001nnnnnnnnnnnnnnnnnnnadTd???????????????????????????????????????由于描述第n个连杆相对于第n-1连杆的位姿，对于SCARA教学机器人(四个自由度)，机器人的末端装置即为连杆4的坐标系，它与基座的关系为: nnT1?0012341234TTTTT?如上图坐标系，可写出连杆n相对于n-1变换矩阵:其中：以下相同相应连杆初始位置及参数列于表2.4，表中、 为关节变量nnT1?111101000000100001csscT??????????????221221200000100001cslscT??????????????223310001000010001lTd??????????????444434000000100001csscT??????????????cos,sinnnnncs????n?nd构件10001020010300104000101na?1n??ndn?1cosn??1sinn??1l2l3d1?2?4?各连杆变换矩阵相乘，可得到 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 41 2 41 2 41 2 41 20 0 0 100xxxxyyyyzzzzn o a pn o apTTTT d Tn o a pccc sss css scsccs sss csc sccccl ssl clscc csc sss ccsscs css ssc cccsc??????????????????????????????????21 221130010000lcsl sld???????????????SCARA机器人的逆运动学分析1.求关节变量为了分离变量，对方程的两边同时左乘,得:即：1?? ?0111T??? ???????010123114223344TTTTdT?????11242424242 2111242424242 230000000010001000100010001xxxxyyyyzzzzcsnoapccsscssccllscnoapsccsssccslnoapd???????? ?????? ?????????? ??????? ??????? ?????? ??左右矩阵中的第一行第四个元素(1.4)，第二行第四个元素(2.4)分别相等。

即:由以上两式联立可得:式中：112211122cossincossincossinxyxyppllppl??????????? ??????????211arctanAA??????????????222212221;arctan2xyyxxyllpppAplpp????????2 求关节变量由式(2.87)可得:式中：2?????1211sinarctancosrrl????????????????22;arctanyxyxprppp????3 求关节变量再令左右矩阵中的第三行第四个元素(3.4)相等，可得:4 求关节变量再令左右矩阵中的第一行第一个元素、第二行第一个元素 (1.1，2.1)分别相等，即：由上两式可求得:3d3zdp? ?4?112424112424cossincoscossinsinsincossincoscossinxyxynnnn?????????????????????????114211sincosarctancossinxyxynnnn?????????????????????至此，机器人的所有运动学逆解都已求出在逆解的求解过程中只进行了一次矩阵逆乘，从而使计算过程大为简化，从的表达式中可以看出它有两个解，所以SCARA机器人应该存在两组解。

运动学分析提供了机器人运动规划和轨迹控制的理论基础1?对机器人相关概念的补充退化：当机器人失去一个自由度，并因此不按所期望的状态运动时即称为退化退化发生条件：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+1AiAi+1Ai-1?杆件参数的定义—— 、 、 和?— li 和 li-1 在 Ai 轴线上的交点之间的距离i?idi?AiAi+1i?ilid1?ili?Ai-1id?— li 和 li-1 之间的夹角，按右手定则由li-1 转向 li由运动学的观点来看，杆件保持其两端关节间的形态不变，这种形态由两个参数决定：杆件长度 li和杆件扭转角杆件的相对位置关系，由另外两个参数决定：杆件的距离di和杆件的回转角i?i?ili?? li —— 关节Ai 轴和Ai+1 轴线公法线的长度i??—关节i 轴线与i+1轴线在垂直于li 平面内的夹角?上述4个参数，就确定了杆件的结构形态和相邻杆件相对位置关系在转动关节中，li, αi, di是固定值，θi是变量在移动关节中，li, αi, θi是固定值， d i 是变量§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.1 D-H关节坐标系建立原则? 右手坐标系? 原点Oi：设在li与Ai+1轴线的交点上? Zi轴：与Ai+1关节轴重合，指向任意? Xi轴：与公法线Li重合，指向沿Li由Ai轴线指向Ai+1轴线? Yi轴：按右手定则§9.3.2 关节坐标系的建立方法AiAi+1i?ilid1?ili?Ai-11?iz1?ix1?iy1?ioizixiyio? 原点Oi：设在li与Ai+1轴线的交点上? zi轴：与Ai+1关节轴重合，指向任意? xi轴：与公法线li重合，指向沿li由Ai轴线指向Ai+1轴线? yi轴：按右手定则? 杆件长度杆件长度li—沿 xi轴， zi-1轴与 xi轴交点到 0i的距离? 杆件扭转角杆件扭转角αi— 绕 xi轴，由 zi-1转向zi? 杆件偏移量杆件偏移量 di— 沿 zi-1轴，zi-1轴和 xi交点至∑0i –1坐标系原点的距离? 杆件回转角杆件回转角θi— 绕 zi-1轴，由 xi-1转向 xi??两种特殊情况? 两轴相交，怎么建立坐标系？? Oi— Ai与Ai+1关节轴线的交点；? zi— Ai+1轴线；? xi— zi和zi-1构成的平面的法线；? yi— 右手定则；-1iizz?????AiAi+1zi-1zixiyiOi?两轴平行，怎么建立坐标系 (Ai与Ai+1平行)？?先建立 ∑Oi-1?然后建立∑Oi+1?最后建立 ∑Oi-1iO DAi-1AiAi+1Ai+2li-1oi-1xi-1yi-1zi-1ABDCoi（xi）（yi）zixiyioi+1xi+1yi+1zi+1di+1li+1di注意：? 由于Ai和Ai+1平行，所以公法线任意点在A点位置；? 按照先前的定义， di为Oi-1点和A点之间的距离， di+1为B点和C点间的距离，这样设定可以的，但我们可以变更一下，将0i点放在C点，定义Oi在li+1和Ai+1轴的交点上，这样使 di+1=0使计算简便，此时 di=§9.4 相邻关节坐标系间的齐次变换过程——机器人运动学正解?将xi-1轴绕 zi-1 轴转?i角度，将其与xi轴平行；?沿 zi-1轴平移距离 di，使 xi-1轴与 xi 轴重合；?沿 xi 轴平移距离 li，使两坐标系原点及x轴重合；?绕 xi轴转?i 角度，两坐标系完全重合．AiAi+1i?ilid1?ili?Ai-11?iz1?ix1?iy1?ioizixiyio111A(,)(,)( , ) ( ,)iiiiiiiiiiR zTrans zd Trans x l R x???????机器人的运动学正解方程001112iiiTAAA???? ? ??D-H变换矩阵变换矩阵iiA1?????????????100010000100001id?????????????1000010000cossin00sincosiiii????????????????100001000010001il?????????????10000cossin00sincos00001iiii??????????????????1000cossin0sincossincoscossincossinsinsincoscosiiiiiiiiiiiiiiiiidaa??????????????==。