机器人正逆运动学 课件

§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,,0ooodd重合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θndnanαn1θ1(900)00-9002θ2(0)d2a203θ3(-900)0a3-9004θ4(0)d409005θ5(0)00-9006θ6(0)000对下图所示简单机器人，根据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（SelectiveComplianceAssemblyRobotArm装配机器人臂）