工业机器人的运动学

工业机器人湖北工业大学机械工程学院主讲：张铮第2章工业机器人的运动学2.1齐次坐标及对象物的描述2.2齐次变换及运算2.3工业机器人连杆参数及其齐次变换矩2.4工业机器人运动学方程2.1齐次坐标及对象物的描述一、点的位置描述在选定的直角坐标系{A}中,空间任一点P的位置可用3×1的位置矢量p表示,其左上标代表选定的参考坐标系pxp=pypz式中:px、py、pz是点P在坐标系{A}中的三个位置坐标分量,如图二、齐次坐标如用四个数组成的(4×1)列阵PxP=PyPz1表示三维空间直角坐标系{A}中点P,则列阵[PxPyPz1]称为三维空间点P的齐次坐标。必须注意,齐次坐标的表示不是惟一的。我们将其各元素同乘一非零因子w后,仍然代表同一点P,即PxaP=Py=bPzc1w式中:a=wpx;b=wpy;c=wpz。三、坐标轴方向的描述如图所示,i、j、k分别是直角坐标系中X、Y、Z坐标轴的单位向量。若用齐次坐标来描述X、Y、Z轴的方向,则X=[1000]TY=[0100]TZ=[0010]T从上可知,我们规定:(4×1)列阵[abc0]T中第四个元素为零,且a2+b2+c2=1,则表示某轴(某矢量)的方向;(4×1)列阵[abcw]T中第四个元素不为零,则表示空间某点的位置。图中矢量v的方向用(4×1)列阵可表达为v=[abc0]Ta=cosα,b=cosβ,c=cosγ图中矢量v所坐落的点O为坐标原点,可用(4×1)列例2-1用齐次坐标写出图2-3中矢量u、v、w的方向列阵。解矢量u:cosα=0,cosβ=0.7071067,cosγ=0.7071067u=[00.70710670.70710670]T矢量v:cosα=0.7071067,cosβ=0,cosγ=0.7071067v=[0.707106700.70710670]T矢量w:cosα=0.5,cosβ=0.5,cosγ=0.7071067w=[0.50.50.70710670]T四、动坐标系位姿的描述动坐标系位姿的描述就是对动坐标系原点位置的描述以及对动坐标系各坐标轴方向的描述。1.刚体位置和姿态的描述设有一刚体Q,如图2-4所示,O′为刚体上任一点,O′X′Y′Z′为与刚体固连的一个坐标系,称为动坐标系刚体Q在固定坐标系OXYZ中的位置可用齐次坐标形式的一个(4×1)列阵表示为：x0p=y0z01刚体的姿态可由动坐标系的坐标轴方向来表示。令n、o、a分别为X′、Y′、Z′坐标轴的单位方向矢量,每个单位方向矢量在固定坐标系上的分量为动坐标系各坐标轴的方向余弦,用齐次坐标形式的(4×1)列阵分别表示为n=[nxnynz0]T,o=[oxoyoz0]T,a=[axayaz0]T因此,图2-4中刚体的位姿可用下面(4×4)矩阵来描述:nxoxaxx0T=[noap]=nyoyayy0nzozazz00001很明显,对刚体Q位姿的描述就是对固连于刚体Q的坐标系O′X′Y′Z′位姿的描述。例2-2图2-5表示固连于刚体的坐标系{B}位于OB点，xb=10,yb=5,zb=0。ZB轴与画面垂直,坐标系{B}相对固定坐标系{A}有一个30°的偏转,试写出表示刚体位姿的坐标系{B}的(4×4)矩阵表达式。解XB的方向列阵:n=[cos30°cos60°cos90°0]T=[0.8660.5000.0000]TYB的方向列阵:o=[cos120°cos30°cos90°0]T=[-0.5000.8660.0000]TZB的方向列阵:a=[0.0000.0001.0000]T坐标系{B}的位置列阵:p=[10.05.00.01]T所以,坐标系{B}的(4×4)矩阵表达式为0.866-0.5000.00010.0TT=0.5000.8660.0005.00.0000.0001.0000.000012.手部位置和姿态的表示机器人手部的位置和姿态也可以用固连于手部的坐标系{B}的位姿来表示,如图所示。手部的位姿可用(4×4)矩阵表示为：nxoxaxpx[noap]=nyoyaypynzozazpz0001例2-3图表示手部抓握物体Q,物体为边长2个单位的正立方体,写出表达该手部位姿的矩阵式。解因为物体Q形心与手部坐标系O′X′Y′Z′的坐标原点O′相重合,所以手部位置的(4×1)列阵为p=[1111]T手部坐标系X′轴的方向可用单位矢量n来表示:α=90°,β=180°,γ=90°n:nx=cosα=0ny=cosβ=-1nz=cosγ=0同理,手部坐标系Y′轴与Z′轴的方向可分别用单位矢量o和a来表示，根据式(2-8)可知,手部位姿可用矩阵表达为0-101T=[noap]=-100100-1100012.2齐次变换及运算刚体的运动是由转动和平移组成的。为了能用同一矩阵表示转动和平移,有必要引入(4×4)的齐次坐标变换矩阵。一、平移的齐次变换首先,我们介绍点在空间直角坐标系中的平移。如图所示,空间某一点A,坐标为(x,y,z),当它平移至A′点后,坐标为(x′,y′,z′)x′=x+Δxy′=y+Δyz′=z+Δz或写成:x′100Δxxy′=010Δyyz′001Δzz100011也可以简写为A′=Trans(Δx,Δy,Δz)A式中:Trans(Δx,Δy,Δz)表示齐次坐标变换的平移算子。例2-4有下面两种情况(如图所示）动坐标系{A}相对于固定坐标系的X0、Y0、Z0轴作(-1,2,2)平移后到{A′};动坐标系{A}相对于自身坐标系(即动系)的X、Y、Z轴分别作(-1,2,2)平移后到{A″}。已知写出{A′}{A″}矩阵表达式。0-101A=-100100-110001解动坐标系{A}的两个平移坐标变换算子均为100-1Trans(Δx,Δy,Δz)=010200120001{A′}坐标系是动系{A}沿固定坐标系作平移变换得来的,因此算子左乘,{A′}的矩阵表达式为A′=Trans(-1,2,2)A0-100=-100300-130001{A″}坐标系是动系{A}沿自身坐标系作平移变换得来的,因此算子右乘,{A″}的矩阵表达式为A″=ATrans(-1,2,2)0-10-1=-100200-1-10001二、旋转的齐次变换首先我们介绍点在空间直角坐标系中的旋转,如图2-10所示,空间某一点A,坐标为(x,y,z),当它绕Z轴旋转θ角后至A′点,坐标为(x′,y′,z′)。A′点和A点的坐标关系为:x′=cosθx-sinθyy′=sinθx+cosθyz′=z或用矩阵表示为x′cosθ-sinθ0xy′=sinθcosθ0yz′001zx′cosθ-sinθ00xy′=sinθcosθ00yz′0010z100011A′点和A点的齐次坐标分别为[x′y′z′1]T和[xyz1]T,因此A点的旋转齐次变换过程为a′=Rot(z,θ)aRot(z,θ)表示齐次坐标变换时绕Z轴的旋转算子,算子左乘表示相对于固定坐标系进行变换下图所示为点A绕任意过原点的单位矢量k旋转θ角的情况,kx,ky,kz分别为k矢量在固定参考系坐标轴X、Y、Z上的三个分量,且kx2+ky2+kz2=1。可以证得,绕任意过原点的单位矢量k转θ角的旋转齐次变换公式为kxkxversθ+cθkykxversθ-kzsθkzkxversθ+kysθ0Rot(k,θ)=kxkyversθ+kzsθkykyversθ+cθkzkyversθ-kxsθ0kxkzversθ-kysθkykzversθ+kxsθkzkzversθ+cθ00001式中:versθ=1-cosθ;θ值的正负号由右手螺旋法则决定。上式称为一般旋转齐次变换通式,它概括了绕X、Y、Z轴进行旋转齐次变换的各种特殊情况。三、平移加旋转的齐次变换平移变换和旋转变换可以组合在一个齐次变换中,点W若还要作4i-3j+7k的平移,则如图2-14所示,只要左乘上平移变换算子,即可得到最后E点的列阵表达式e=Trans(4,-3,7)Rot(y,90°)Rot(z,90°)u③②①100400107=010-310003001701002000100011001476=100-33=401072100001110014100-301070001其中,为平移加旋转的一般齐次变换矩阵。2.3工业机器人连杆参数及其齐次变换矩阵如图1所示,连杆两端有关节n和n+1。该连杆尺寸可以用两个量来描述:一个是两个关节轴线沿公垂线的距离an,称为连杆长度;另一个是垂于an的平面内两个轴线的夹角αn,称为连杆扭角。这两个参数为连杆的尺寸参数。一、连杆参数及连杆坐标系的建立再考虑连杆n与相邻连杆n-1的关系,若它们通过关节相连,如图2所示,其相对位置可用两个参数dn和θn来确定,其中dn是沿关节n轴线两个公垂线的公距离,θn是垂直于关节n轴线的平面内两个公垂线的夹角。这是表达相邻杆件关系的两个参数。图1图2二、连杆坐标系之间的变换矩阵建立了各连杆坐标系后,n-1系与n系间的变换关系可以用坐标系的平移、旋转来实现从n-1系到n系的变换,可先令n-1系绕Zn-1轴旋转θn角,再沿Zn-1轴平移dn,然后沿Xn轴平移an,最后绕Xn轴旋转αn角,使得n-1系与n系重合。于是连杆n的齐次变换矩阵为：An=Rot(z,θn)Trans(0,0,dn)Trans(an,0,0)Rot(x,αn)①②③④cθn-sθn00100an1000=sαncαn0001000cθn-sθn00010001dn0sαncαn0000100010001cθn-sθncαnsθnsαnancθn=sθncθncαn-cθnsαnansθn0sαncαndn00012.4工业机器人运动学方程一机器人运动学方程通常把描述坐标系与下一个连杆间的相对关系的齐次变换矩阵叫做A变换矩阵或A矩阵。如果A1矩阵表示第一个连杆坐标系相对于固定的坐标系的位置，A2矩阵表示第二个连杆坐标系相对第一连杆坐标系的位置，那么第二个连杆坐标系在固定坐标系中的位置可用A1和A2的乘积来表示：T2=A1A2。若A3矩阵表示第三个连杆坐标系相对于第二个连杆坐标系的位置，则有：T3=A1A2A3。如此类推，对于六连杆机器人，有下列T6矩阵：T6=A1A2A3A4A5A6。计算结果T6是一个(4×4)矩阵,即nxoxaxpxT6=nyoyaypynzozazpz0001式中:前三列表示手部的姿态;第四列表示手部的位置。二、正向运动学及实例连杆转角(变量)θ两连杆间距离d连杆长度a连杆扭角α连杆1θ1d1=0a1=l1=100α1=0连杆2θ2d2=0a2=l2=100α2=0连杆3θ3d3=0a3=l3=20α3=01.平面关节型机器人的运动学方程下图1所示为具有一个肩关节、一个肘关节和一个腕关节的SCARA机器人。机器人连杆的参数如表1所示，机器人坐标系图2所示。图1图2表1该平面关节型机器人的运动学方程为T3=A1A2A3式中:A1表示连杆1的坐标系{1}相对于固定坐标系{0}的齐次变换矩阵;A2表示连杆2的坐标系{2}相对于连杆1的坐标系{1}的齐次变换矩阵;A3表示连杆3的坐标系即手部坐标系{3}相对于连杆2的坐标系{2}的齐次变换矩阵。参考图2,于是有A1=Rot(z0,θ1)Trans(l1,0,0)A2=Rot(z1,θ2)Trans(l2,0,0)cθ1-sθ10l1cθ1cθ2-sθ20l2cθ2=sθ1cθ10l1sθ1=sθ2cθ20l2sθ20010001000010001A3=Rot(z2,θ3)Trans(l3,0,0)cθ3-sθ30l3cθ3=sθ3cθ30l3sθ300100001c123-s1230l3c123+l2c12+l1c1T3=s123c1230l3s123+l2s12+l1s100100001T3是A1、A2、A3连乘的结果,表示手部坐标系{3}(即手部)的位置和姿态。nxoxaxpxT3=nyoyaypynzozazpz0001于是,可写出手部位置(4×1)列阵为pxl3c123+l2