1、1.4 机器人正向运动学,工业机器人的正向运动学是指已知各关节的类型、相邻关节之间的尺寸和相邻关节相对运动量的大小时，如何确定工业机器人末端操作器在固定坐标系中的位姿。,主要包括以下内容：1) 相对杆件的坐标系的确定；2) 建立各连杆的模型矩阵A；3) 正运动学算法；,D-H表示法,学习目标：1. 理解D-H法原理2. 学会用D-H法对机器人建模,学习重点：1. 给关节指定参考坐标系2. 制定D-H参数表3. 利用参数表计算转移矩阵,背景简介：1955年，Denavit和Hartenberg(迪纳维特和哈坦伯格)提出了这一方法，后成为表示机器人以及对机器人建模的标准方法，应用广泛。,总体思想：
2、首先给每个关节指定坐标系，然后确定从一个关节到下一个关节进行变化的步骤，这体现在两个相邻参考坐标系之间的变化，将所有变化结合起来，就确定了末端关节与基座之间的总变化，从而建立运动学方程，进一步对其求解。,1.第一个关节指定为关节n,第二个关节为n+1,其余关节以此类推。,坐标系的确定,2.Z轴确定规则：如果关节是旋转的，Z轴位于按右手规则旋转的方向，转角 为关节变量。如果关节是滑动的，Z轴为沿直线运动的方向，连杆长度d为关节变量。关节n处Z轴下标为n-1。,3.X轴确定规则 情况1：两关节Z轴既不平行也不相交 取两Z轴公垂线方向作为X轴方向，命名规则同Z轴。,情况2：两关节Z轴平行 此时，两Z
3、轴之间有无数条公垂线，可挑选与前一关节的公垂线共线的一条公垂线。,情况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机器人运动学方程
4、,A1,A2,A3,A4,A5,A6,z1,x1,y1,O1,z2,x2,y2,O2,z3,y3,x3,O3,y4,z4,x4,O4,z5,y5,x5,O5,z6,x6,y6,O6,z0,y0,x0,O0,为右手坐标系 原点Oi： Ai与Ai+1关节轴线的交点 zi轴：与Ai+1关节轴重合，指向任意 xi轴： Zi和Zi-1构成的面的法线 yi轴：按右手定则,ai沿 xi 轴， zi-1 轴与 xi 轴交点到Oi 的距离 i 绕 xi 轴，由 zi-1 转向zi di 沿 zi-1 轴，zi-1 轴和 xi 交点至Oi 1 坐标系原点的距离 i 绕 zi-1 轴，由 xi-1转向 xi,关节1
5、 坐标系0,关节2 坐标系1,关节3 坐标系2,连杆0,连杆1,连杆2,连杆3,连杆4,连杆5,关节4 坐标系3,关节5 坐标系4,关节6 坐标系5,解：,例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
6、 转向zi di 沿 zi-1 轴，zi-1 轴和 xi 交点至Oi 1 坐标系原点的距离 i 绕 zi-1 轴，由 xi-1转向 xi,A1,A2,A3,A4,A5,A6,O1,O0,对下图所示简单机器人，根据D-H法，建立必要坐标系及参数表。,例 3,第一步：根据D-H法建立坐标系的规则建立坐标系,第二步：将做好的坐标系简化为我们熟悉的线图形式,第三步：根据建立好的坐标系，确定各参数，并写入D-H参数表,第四步：将参数代入A矩阵，可得到,第5步 求出总变化矩阵,依次写出从基坐标系到手爪坐标系之间相邻两坐标系的齐次变换矩阵，它们依次连乘的结果就是末端执行器（手爪）在基坐标系中的空间描述，即,
7、已知q1,q2,qn，求 ，称为运动学正解；,已知 ，求q1,q2,qn，称为运动学反解。,上式称为运动方程。,综上：,正解,反解,1.5 机器人的逆运动学解,给定机器人终端位姿，求各关节变量，称求机器人运动学逆解。让我们通过下面这道例题来了解一下机器人逆运动学求解的一般步骤。前面例子最后方程为：,求逆运动学方程的解,根据第3行第4列元素对应相等可得到,依次用 左乘上面两个矩阵，得到：,根据1，4元素和2，4元素，可得到：,将上面两个方程两边平方相加，并利用和差化积公式得到,已知,于是可得到：,依次类推，分别在方程2.19两边左乘A1A4的逆，可得到,接下来再一次利用式由于C12=C1C2-S
8、1S2以及S12=S1C2+C1S2，最后得到：,最后用A5的逆左乘式2.67，再利用2,1元素和2,2元素，得到：,2.10 机器人的运动学编程,在实际应用中，对运动学的求解是相当繁琐和耗时的，因此需要用计算机编程来实现。并且应尽量避免使用矩阵求逆或高斯消去法等相对繁琐的算法。正确的算法是：,2.11 设计项目,利用本书中所介绍的四自由度机器人，结合本章所学的知识进行四自由度机器人的正逆运动学分析。SCARA型机器人的运动学模型的建立，包括机器人运动学方程的表示，以及运动学正解、逆解等，这些是研究机器人控制的重要基础，也是开放式机器人系统轨迹规划的重要基础。为了描述SCARA型机器人各连杆之
9、间的数学关系，采用D-H法。SCARA型机器人操作臂可以看作是一个开式运动链。它是由一系列连杆通过转动或移动关节串联而成的。为了研究操作臂各连杆之间的位移关系，可在每个连杆上固接一个坐标系，然后描述这些坐标系之间的关系。,SCARA（Selective Compliance Assembly Robot Arm 装配机器人臂）机器人坐标系的建立,1.SCARA机器人坐标系建立原则根据D-H坐标系建立方法，SCARA机器人的每个关节坐标系的建立可参照以下的三原则 (1) 轴沿着第n个关节的运动轴;基坐标系的选择为:当第一关节变量为零时，零坐标系与一坐标系重合。 (2) 轴垂直于 轴并指向离开 轴
10、的方向。 (3) 轴的方向按右手定则确定。2.构件参数的确定根据D-H构件坐标系表示法，构件本身的结构参数 、 和相对位置参数 、 可由以下的方法确定: (1) 为绕 轴(按右手定则)由 轴到 轴的关节角。 (2) 为沿 轴，将 轴平移至 轴的距离。 (3) 为沿 轴从 量至 轴的距离。 (4) 为绕 轴(按右手定则)由 轴到 轴的偏转角。,3.变换矩阵的建立全部的连杆规定坐标系之后，就可以按照下列的顺序来建立相邻两连杆n-1和n之间的相对关系: (1)绕 轴转 角。 (2)沿 轴移动 。 (3)绕 轴转 角。 (4)沿 轴移动 。 这种关系可由表示连杆n对连杆n-1相对位置齐次变换 来表征。
11、即：展开上式得,由于 描述第n个连杆相对于第n-1连杆的位姿，对于SCARA教学机器人(四个自由度)，机器人的末端装置即为连杆4的坐标系，它与基座的关系为:,如上图坐标系，可写出连杆n相对于n-1变换矩阵 :其中： 以下相同。 相应连杆初始位置及参数列于表2.4，表中 、 为关节变量。,各连杆变换矩阵相乘，可得到SCARA机器人末端执行器的位姿方程(正运动学方程)为下 式它表示了SCARA手臂变换矩阵 ，它描述了末端连杆坐标系4相对基坐标系0的位姿 。,SCARA机器人的正运动学分析,SCARA机器人的逆运动学分析,1.求关节变量 为了分离变量，对方程的两边同时左乘 , 得:即：,左右矩阵中的
12、第一行第四个元素(1.4)，第二行第四个元素(2.4)分别相等。即:由以上两式联立可得:式中：,2 求关节变量 由式(2.87)可得:式中：,至此，机器人的所有运动学逆解都已求出。在逆解的求解过程中只进行了一次矩阵逆乘，从而使计算过程大为简化，从 的表达式中可以看出它有两个解，所以SCARA机器人应该存在两组解。运动学分析提供了机器人运动规划和轨迹控制的理论基础。,对机器人相关概念的补充,退化：当机器人失去一个自由度，并因此不按所期望的状态运动时即称为退化。 退化发生条件： 1.机器人达到物理极限，不能进一步运动 2.两个相似关节共线,不灵巧区域：能对机器人定位不定姿的区域称为不灵巧区域。,D
13、-H法的局限性：无法表示关于y轴的运动。,退化状态下的机器人,总 结,1 用矩阵表示点，向量，坐标系及变换的方法 2 正逆运动学方程的建立 3 用D-H法建立坐标系及变化方程 4 正逆运动学方程的求解,9.2 机器人杆件，关节和它们的参数,9.2.1 杆件与关节 操作机由一串用转动或平移（棱柱形）关节连接的刚体（杆件）组成 每一对关节杆件构成一个自由度，因此N个自由度的操作机就有N对关节杆件。 0号杆件（一般不把它当作机器人的一部分）固联在机座上，通常在这里建立一个固定参考坐标系，最后一个杆件与工具相连 关节和杆件均由底座向外顺序排列，每个杆件最多和另外两个杆件相联，不构成闭环。,关节,杆件,
14、末端操作手,机座,两自由度关节,关节： 一般说来，两个杆件间是用低副相联的 只可能有6种低副关节：旋转（转动）、棱柱（移动）、圆柱形、球形、螺旋和平面，其中只有旋转和棱柱形关节是串联机器人操作机常见的，各种低副形状如下图所示：,旋转,棱柱形,柱形,球形,螺旋形,平面,9.2.2 杆件参数的设定,条件 关节串联 每个杆件最多与2个杆件相连，如Ai与Ai-1和 Ai+1相连。第 i 关节的关节轴 Ai 位于2个杆件相连接处，如图所示， i -1关节和 i +1关节也各有一个关节轴 Ai-1 和 Ai+1。,杆件参数的定义 、 、 和, li 和 li-1 在 Ai 轴线上的交点之间的距离。, li
15、 和 li-1 之间的夹 角，按右手定则由li-1 转向 li。,由运动学的观点来看，杆件保持其两端关节间的形态不变，这种形态由两个参数决定：杆件长度 li 和杆件扭转角 。杆件的相对位置关系，由另外两个参数决定：杆件的距离 di 和杆件的回转角 。,li 关节 Ai 轴和 Ai+1 轴线公法线的长度。,关节i 轴线与i+1轴线在垂直于li 平面内的夹角。,上述4个参数，就确定了杆件的结构形态和相邻杆件相对位置关系。在转动关节中，li, i, di是固定值，i是变量。在移动关节中，li, i , i是固定值， d i 是变量。,9.3 机器人关节坐标系的建立,对于每个杆件都可以在关节轴处建立一
16、个正规的笛卡儿坐标系（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
17、年提出了一种为运动链中每个杆件建立附体坐标系的矩阵方法（D-H方法） ，建立原则如下：,9.3.1 D-H关节坐标系建立原则,右手坐标系 原点Oi：设在li与Ai+1轴线的交点上 Zi轴： 与Ai+1关节轴重合，指向任意 Xi轴： 与公法线Li重合，指向沿Li由Ai轴线指向Ai+1轴线 Yi轴： 按右手定则,9.3.2 关节坐标系的建立方法,Ai,Ai+1,Ai-1,原点Oi：设在li与Ai+1轴线的交点上 zi轴：与Ai+1关节轴重合，指向任意 xi轴：与公法线li重合，指向沿li由Ai轴线指向Ai+1轴线 yi轴：按右手定则,杆件长度li 沿 xi 轴， zi-1 轴与 xi 轴交点到 0
18、i 的距离杆件扭转角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 右手定则；,Ai,Ai+1,zi-1,zi,xi,yi,Oi,两轴平行，怎么建立坐标系(Ai与Ai+1平行)？先建立 Oi-1 然后建立Oi+1 最后建立 Oi,注意： 由于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 角度，两坐标系完全重合,机器人的运动学正解方程,D-H变换矩阵,=,=,
