机器人位姿方程.ppt

第2章 机器人位姿方程,机器人由连杆、关节和末端执行器组成的空间连杆开式链机构 。 把坐标系固连在机器人的每一个连杆关节上，用坐标变换来描述这些坐标系之间的相对位置和方向，进行机器人的位姿分析。,1,2,n,首先给每个关节指定一个坐标系，然后从一个关节到下一个关节（一个坐标到下一个坐标）进行变换。 如果从基座到第一个关节，再从第一个关节到第二个关节直至到最后一个关节的所有变换结合起来，就得到了机器人的总变换矩阵。 -机器人位姿方程 -机器人运动方程,2.1 杆件坐标系建立 一、坐标系、连杆、关节编号 按从机座到末端执行器的顺序，由低到高依次为各关节和各连杆编号。 机座编号为杆件0，与机座相连连杆编号为杆件1，。 机座与连杆1的关节编号为关节1，连杆1与连杆2的连接关节编号为2，依此类推。,与基座(连杆o)固接的称为基坐标系，与连杆1固接的称为坐标系1，与连杆i固接的坐标系称为坐标系i。,二、坐标系方位的确定 两种方法确定坐标系的方位 1一般方法 对坐标系的各坐标轴的分配并无特殊规定，后一坐标系(序号大的坐标系)向前一坐标系的坐标变换按照坐标变换进行。 2D-H方法 1956年由Denauit和Hartenbery提出，定义了坐标系和4个描述连杆几何参数和和相对位置参数。,1.连杆的几何参数 连杆i： 连杆长度ai：两关节转轴i和i+1之间的最短距离，即两轴线之间公垂线的长度； 连杆扭角i ：两个关节轴线i和i+1的夹角； 注意： 当两轴线相交于一点时，ai0。 机座及末端连杆只有一个关节，规定长度为0。 一端为旋转关节，另一端为移动关节的连杆，长度规定为0。,2.关节变量及偏置量 描述相邻连杆的相对位置 关节转角i：垂直于关节轴线的平面内，两个共垂线之间的夹角 连杆偏置di：沿关节i轴线方向，两共垂线之间的距离。,关节变量 旋转关节：关节转角i。 移动关节：连杆偏置di是关节变量，i =0。,D-H参数,为了便于矩阵计算，将每个连杆和关节的参数值填入D-H参数表。,3.D-H坐标系建立 为了确定各连杆之间的相对运动和位姿关系，在每一连杆上固接一个坐标系。 连杆i的坐标系建立： Z轴：与i+1关节轴线重合。 X轴：沿连杆i两关节轴线的公垂线，由i指向i+1关节。 坐标原点：在i十1关节轴线上,是X轴与Z轴的交点。 Y轴：按右手螺旋法则确定。,Oi,轴 i-1,轴 i,轴 i+1,杆 i,杆 i1,Oi,Oi-1,D-H参数小结： 连杆的尺寸参数 连杆长度ai：Zi和Zi-1沿Xi的距离，总为正(可以为零）； 连杆扭角i ：绕Xi ，Zi-1转至Zi的转角，符号根据右手定则确定；（代数量） 相邻连杆的关系参数 连杆偏置di ： Xi-1沿Zi-1至Xi的距离，沿Zi-1正向时为正； 关节转角i ：Xi-1绕Zi-1转至Xi的转角，符号根据右手定则确定。 D-H参数确定了连杆i相对于连杆i-1的位姿,说明： (1)基系：基座坐标系0系是固定不动的。Z0轴取关节1的轴线，O0的设置任意，也可与O1重合。 (2)末杆（工具）n：由于末杆只有一个关节，约定n坐标系与n-1坐标系平行。 手腕坐标系：参照前述 （3）当关节i轴线和关节i+1轴线相交时： 坐标原点Oi取关节i轴线和关节i+1轴线交点，x轴的方向为zn-1与zn构成的面的法线。 （4）当关节i轴线和关节i+1轴线平行时： 坐标原点可取为关节i+1轴线与关节i+2轴线的公垂线与关节i+1轴线的交点。,A矩阵：描述相邻两连杆齐次变换矩阵。A矩阵将上（小）编号连杆坐标系向下（大）编号连杆坐标系变换。 A1描述第一个连杆对于机身的位姿，A2描述第二个连杆坐标系相对于第一个连杆坐标系的位姿。 A1 A2描述第二个连杆坐标系相对于机身的位姿。 T矩阵：连续变换的若干A矩阵的积称为T矩阵。 对于一个六连杆（六自由度）机械手，末端执行器相对于机身坐标系的齐次变换矩阵为,2.2 连杆坐标系的变换矩阵,连杆坐标系的D-H变换 i-1坐标系经过下面四次有序的变换可得到i坐标系： （1）绕Zi-1轴转i ；Rot(Zi-1,i) （2）沿Zi-1轴移动di ；Trans(Zi-1,di) （3）沿Xi轴移动ai ；Trans(Xi,ai) （4）绕Xi轴转i ；Rot(Xi,i),i,以上变换都是相对于动坐标系的，变换矩阵：,解：1) D-H坐标系建立 关节轴线用Ai表示,例1 图示为Stanford机器人，求Ai(i = 1,2,3,4,5,6)及T6的表达式。,2) 确定各连杆的D-H参数和关节变量 3) 求两杆之间的位姿矩阵Ai,例2：已知四轴平面关节SCARA机器 人如图所示，试计算： （1）机器人的运动学方程； （2）当关节变量取 qi=30，-60，120，90T 时，机器人手部的位置和姿态。,SCARA机器人主要用于装配、焊接、密封、搬运和拿放等场合，具有高刚性、高精度、高速度、安装空间小优点。,解： 1.运动学方程 （1）建立D-H坐标系 机座坐标系0 杆件坐标系i 手部坐标系h,1,0,2,3,（2）确定D-H参数表,0,1,2,3,（3）求两杆之间的位姿矩阵 Ai矩阵,2.机器人运动学方程,当qi=30，-60，120，90T， 机器人手部的位置和姿态：,例3 设立图示PUMA560机器人的六杆操作机的坐标系，标定D-H参数，并写出Ai矩阵。,o2,o5,o6,o1,o3,o4,PUMA 560机器人结构简图,前置坐标系,a3,d2,a2,d4,机器人的运动方程，又称位姿方程，以各杆之间的关节变量为变量的方程式。 运动学正问题：已知机器人各杆件的几何参数和关节变量，求末端执行器相对于笛卡儿坐标系的位置和姿态。 运动学逆问题：已知机器人各杆件的几何参数，给定末端执行器相对于笛卡儿坐标系的位置和姿态，确定关节变量的大小。,2.3 机器人运动学方程正、逆解,定义： 关节空间：关节变量的集合。 用广义坐标qi表示第i号连杆的关节变量,笛卡儿空间： 笛卡儿坐标系的构件位姿的集合。 正解：已知关节空间，求笛卡儿空间。 逆解：已知笛卡儿空间，求关节空间。,运动学方程正解 任何机器人看做是一系列由关节连接起来的连杆构成。 A1表示第一个连杆对于基系的位姿，A2表示第二个连杆相对于第一个连杆的位姿，那么第二个连杆在基系中的位姿可由下列矩阵的乘积给出： T2=A1A2 同理，若A3表示第三个连杆相对于第二个连杆的位姿，则有： T3=A1A2A3 六连杆机械手的位姿矩阵： T6=A1A2A3A4A5A6 运动学方程正解：已知关节空间，求末端执行器位姿。 大多数机器人程序设计语言，是用某个笛卡儿坐标系来指定机械手末端位置，即姿态T6。机械手能够被驱动至这个姿态，所有关节变量的大小？,运动学方程逆解,得到12个方程式，一般不联立求解。 求解方法：采用一系列变换矩阵的逆A-1左乘，然后找出右端为常数的元素，并令这些元素与左端元素相等，得出一个可解的三角函数方程式，求出关节变量i或 di。,已知,关节变量 未知,T6 = A1 A2 A3 A4 A5 A6 分别用Ai（i1，2，5）的逆左乘，有 A1-1 T6 = 1T6 ( 1T6 = A2 A3 A4 A5 A6 ) A2-1 A1-1 T6 = 2T6 ( 2T6 = A3 A4 A5 A6 ) A3-1A2-1 A1-1 T6 = 3T6 ( 3T6 = A4 A5 A6 ) A4-1 A3-1A2-1 A1-1 T6 = 4T6 ( 4T6 = A5 A6 ) A5-1 A4-1 A3-1A2-1 A1-1 T6 = 5T6 ( 5T6 = A6 ) 找出上述五个矩阵方程右端为常数的元素，并令这些元素与左端对应元素相等，得到若干个可解的代数方程，便可求出关节变量i或 di。,运动学正逆解求解实例 例4 试求PUMA560机器人的六杆操作机的正逆解,说明：,2. 令PUMA560几何参数d2=a3= 0就是MOTOMAN商用机器人的几何模型；（a2,d4),1、 PUMA560机器人的六杆操作机是几何参数最多的商用机器人；（a2,a3,d2,d4),3. PUMA560操作机位姿方程的正逆解的步骤和技巧具有普遍指导作用。,求笛卡儿空间：,一、位姿方程的正解 已知关节空间：,运动学正解矩阵为：,nx = c1c23(c4c5c6s4s6) s23s5c6+s1(s4c5c6+c4s6) ny = s1c23(c4c5c6s4s6) s23s5c6c1(s4c5c6+c4s6) nz = s23(c4c5c6s4s6) c23s5c6 ox = c1c23(c4c5c6s4s6) + s23s5c6+s1(c4c6 s4c5s6) oy= s1c23(c4c5s6s4c6) + s23s5s6c1(c4c6 s4c5s6) oz = s23(c4c5c6s4s6) + c23s5c6 ax = c1(c23c4s5+s23c5) s1s4c5 ay = s1(c23c4s5+s23c5) + c1s4s5 az =s23c4s5 c23c5 px = c1a2c2 + a2c23 d4 s23 d3s1 py = s1a2c2 + a2c23 d4 s23 + d3c1 pz = a3s23 a2s2 d4c23,二、位姿方程的逆解,已知机械手末端杆的位姿：,求关节空间：,nx = c1c23(c4c5c6s4s6) s23s5c6+s1(s4c5c6+c4s6) nx = s1c23(c4c5c6s4s6) s23s5c6c1(s4c5c6+c4s6) nz = s23(c4c5c6s4s6) c23s5c6 ox = c1c23(c4c5c6s4s6) + s23s5c6+s1(c4c6 s4c5s6) oy = s1c23(c4c5s6s4c6) + s23s5s6c1(c4c6 s4c5s6) oz = s23(c4c5c6s4s6) + c23s5c6 ax = c1(c23c4s5+s23c5) s1s4c5 ay = s1(c23c4s5+s23c5) + c1s4s5 az = s23c4s5 c23c5 px = c1a2c2 + a2c23 d4 s23 d3s1 py = s1a2c2 + a2c23 d4 s23 + d3c1 pz = a3s23 a2s2 d4c23,已知,求解1 2 3 4 5 6 。,a2、a3、d2、d4已知,6号坐标系原点O6的位置坐标，只与位置机构有关，只与1 2 3有关，可首先独立解出1 2 3 。,6号坐标系姿态，与1 2 3 4 5 6 都有关，必须在1 2 3独立解出后方可解出4 5 6 。,T6 = A1 A2 A3 A4 A5 A6 分别用Ai（i1，2，5）的逆左乘，有 A1-1 T6 = 1T6 ( 1T6 = A2 A3 A4 A5 A6 ) A2-1 A1-1 T6 = 2T6 ( 2T6 = A3 A4 A5 A6 ) A3-1A2-1 A1-1 T6 = 3T6 ( 3T6 = A4 A5 A6 ) A4-1 A3-1A2-1 A1-1 T6 = 4T6 ( 4T6 = A5 A6 ) A5-1 A4-1 A3-1A2-1 A1-1 T6 = 5T6 ( 5T6 = A6 ),技巧1：找出上述五个矩阵方程右端为常数的元素，并令左右端对应项相等，得到可解的代数方程，便可求出关节变量i或 di。,1求1,(a),只有变量1,nx = c23c4c5c6 s4s6 s23s5c6 ny = s4c5c6 c4s6 nz = s23c4c5c6s4s6 c23s5c6 ox = c23c4c5c6+s4s6 + s23s5c6 oy = s4c5c6 c4s6 oz = s23c4c5c6+s6s6 + c23s5s6 ax = c23c5s5 s23c5 ay = s4s5 az = s23c4s5 c23c5 px = a2c2 + a3c23 d4s23 py= d3 pz = a3c23 a2s2 d4s23,矩阵右边相乘结果,常数,nx = c23c4c5c6 s4s6 s23s5c6 ny = s4c5c6 c4s6 nz = s23c4c5c6s4s6 c23s5c6 ox = c23c4c5c6+s4s6 + s23s5c6 oy = s4c5c6 c4s6 oz = s23c4c5c6+s6s6 + c23s5s6 ax = c23c5s5 s23c5 ay = s4s5 az = s23c4s5 c23c5 px = a2c2 + a3c23 d4s23 py= d3 pz = a3c23 a2s2 d4s23,s1px+c1py = d3 (b),代入(b)式： c1s s1c = d3 / 得： sin( ) = d3/,则：,令：px = cos ， py = sin,技巧2：作三角代换,nx = c23c4c5c6 s4s6 s23s5c6 ny = s4c5c6 c4s6 nz = s23c4c5c6s4s6 c23s5c6 ox = c23c4c5c6+s4s6 + s23s5c6 oy = s4c5c6 c4s6 oz = s23c4c5c6+s6s6 + c23s5s6 ax = c23c5s5 s23c5 ay = s4s5 az = s23c4s5 c23c5 px = a2c2 + a3c23 d4s23 py= d3 pz = a3c23 a2s2 d4s23,2利用式（a）还可求3,s1px+c1py = d3 (b) c1px + s1py = a3c23 d4s23 + a2c2 pz = a3s23 + d4c23 + a2s2,将上两式左右平方相加，再与式（b）左右平方相加得：,a3c3 d4s3 = K (d),经三角代换后可得：,3求2,=,c1c23px + s1c23py s23pz a2c3 = a3 c1c23px + s1c23py c23pz + a2s3 = d4,将上两式联立求解s23，c23，得,c1c23px + s1c23py s23pz a2c3 = a3 c1c23px + s1c23py c23pz + a2s3 = d4,A = (c1px + s1py)2,4求4,=,axc1c23 + ays1c23 azs23 = c4s5 axs1 + ayc1 = s4s5,axc1c23 + ays1c23 azs23 = c4s5 axs1 + ayc1 = s4s5,只要s50，便可求出4,当s50时，机械手处于奇异形位。 此时，关节轴4和6重合，只能解出4与6的和或差。 奇异形位可以由上式中atan2的两个变量是否都接近零来判别。若都接近零，则为奇异形位，否则，不是奇异形位。 在奇异形位时，可任意选取4的值，再计算相应的6,5求5,ax(c1c23c4 + s1s4) + ay (s1c23c4 c1s4 ) az (s23c4) = s5 ax(c1c23) + ay (s1s23) + az (c23) = c5,6求6,s6 = nx(c1c23s4 s1c4) ny(s1c23s4 + c1c4) + nzs23s4 c6 = nx(c1c23c4 + s1s4)c5 c1s23s5+ny(s1c23c4 c1s4)c4 s1s23s5nz(s23c4c5 + c23s5),问题： c1s s1c = d3 / 得： sin( ) = d3/,可解出1角,或,为什么用双变量反正切函数atan2(x,y)？,采用反正、余弦函数计算 ，会带来如下问题： 1）由于绝对值相同的正负角度的余弦相等，如coscos（-），因此不能确定反余弦的结果是在那个象限； 反正切函数tan1（x / y）所在的象限空间由自变量的分子和分母的符号确定，如果采用正切表达式，就不难确定角所在的象限。,atan2(x,y) 可在- ,)内唯一确定出角度。,2）当sin接近于0时，求出的角度和是不精确的。 反正切函数比反余旋函数在接近0时的精度高： 计算方法：已知x和函数y=f(x),求y时，计算误差：,接近，值域(,1)， 值域过大，精度不稳定,接近1，值域(0,1，值域很小，精度稳定,当接近0时,x接近1,当接近0时,x接近0,注意： 在求解关节变量过程中如出现反正切函数的分子和分母太小，则计算结果误差会很大，应重新选择矩阵元素建立新的方程组再进行计算，直到获得满意的结果为止。 同样，如果计算结果超出了机械手关节的运动范围，也要重新计算，直到符合机械手关节的运动范围。,由于机械手各关节变量的相互耦合，后面计算的关节变量与前面的关节变量有关。当前面关节变量的计算结果发生变化时，后面关节变量计算的结果也会发生变化，所以运动方程的逆解不是唯一的。 应该根据机械手的组合形态和各关节的运动范围，经过多次反复计算，从中选择一组合理解。 由此可见，求解机械手的逆运动方程是一个十分复杂的过程。,对着Z1正向看，O0、O4、O2为逆时针排列，1就是取了正号； O0、O4、O2为顺时针排列，1就是取了负号。,对着Z3正向看，O1、O4、O3为逆时针排列，3就是取了正号； O2、O4、O3为顺时针排列，3就是取了负号。,3取正负号的原则： 保证3的绝对值较小。,1取正负号的原则： 保证1的绝对值较小。,多值问题,a3,d2,a2,d4,对着Z1正向看，O0、O4、O2为逆时针排列，1就是取了正号； O0、O4、O2为顺时针排列，1就是取了负号。,a3,d2,a2,d4,对着Z3正向看