第3章 运动学方程精选文档.ppt

第3章运动学方程本讲稿第一页，共三十九页3.1 引言引言(Introduction)本章，我们采用齐次变换来描述在各种坐标系中机械手的位置与方向。首先介绍各种正交坐标系的齐次变换。然后介绍在非正交关节坐标系中描述机械手末端的齐次变换。注意，对任何数目关节的各种机械手均可以这样进行。描述一个连杆与下一个连杆之间关系的齐次变换称A矩阵。A矩阵是描述连杆坐标系之间的相对平移和旋转的齐次变换。连续变换的若干A矩阵的积称为T矩阵，对于一个六连杆（六自由度）机械手有T6=A1A2A3A4A5A6（3.1）六连杆的机械手有六个自由度，其中三个自由度用来确定位置，三个自由度用来确定方向。表示机械手在基坐标中的位置与方向。则变换矩阵有下列元素 nxoxaxpxnyoyaypyT6=nzozazpz（3.2）0001本讲稿第二页，共三十九页如图3.1所示，机器人的末端执行器（手爪）的姿态（方向）由n、o、a三个旋转矢量描述，其坐标位置由平移矢量p描述，这就构成了式（3.2）中的变换矩阵T。由于n、o、a三个旋转矢量是正交矢量，所以有n=oa图3.1末端执行器的描述本讲稿第三页，共三十九页3.2 姿态描述姿态描述(SpecificationofOrientation)对式（3.2）中16个元素一一赋值就可确定T6。假定机械手可以到达要求的位置，而单位旋转矢量o和a正交，即oo1（3.3）aa1（3.4）oa0（3.5）a形成单位向量aa（3.6）|a|构成与o和a正交的nnoa（3.7）在o和a形成的平面上旋转o，使得o与n和a正交oan（3.8）单位向量o是oo（3.9）|o|根据第二章给出的一般性的旋转矩阵ot(k,)，它把机械手末端的姿态规定为绕k轴旋转角。本讲稿第四页，共三十九页3.欧拉角欧拉角(EulerAngles)姿态变更常用绕x,y或z轴的一系列旋转来确定。欧拉角描述方法是：先绕z轴旋转，然后绕新的y(即y/)轴旋转，最后绕更新的z(z/)轴旋转（见图3.2）欧拉变换Euler(,)可以通过连乘三个旋转矩阵来求得Euler(,)ot(z,)ot(y,)ot(z,)（3.10）在一系列旋转中，旋转的次序是重要的。应注意，旋转序列如果按相反的顺序进行，则是绕基坐标中的轴旋转：绕z轴旋转，接着绕y轴旋转，最后再一次绕z轴旋转，结果如图3.3所示，它与图3.2是一致的。本讲稿第五页，共三十九页xxxxyyyzzzzy图3.2 欧拉角0 xxxxyyyzzzzy图3.3 基于基坐标的欧拉角0本讲稿第六页，共三十九页3.4 摇摆、俯仰和偏转摇摆、俯仰和偏转(Roll,PitchandYaw)摇摆、俯仰和偏转为另一种旋转。如图3.4所示，就像水中航行的一条小船一样，绕着它前进的方向（z轴）旋转 称为摇摆，绕着它的横向中轴（y轴）旋转 称为俯仰，绕着它甲板的垂直向上的方向（x轴）旋转 称为偏转。借助于这种旋转来描述机械手的末端执行器如图3.5所示。规定旋转的次序为 RPY(,)ot(z,)ot(y,)ot(x,)（3.12）即，绕x轴旋转，接着绕y轴旋转，最后绕z轴旋转，这个变换如下 cos0sin0100001000cossin0RPY(,)=ot(z,)sin0cos00sincos0（3.13）00010001cossin00cossinsinsincos0sincos000cossin0RPY(,)=0010-sincossincoscos0（3.14）00010001本讲稿第七页，共三十九页图3.4摇摆、俯仰和偏转角图3.5机械手的末端执行器的摇摆、俯仰和偏转本讲稿第八页，共三十九页RPY(，)=coscoscossinsinsincoscossincos+sinsin0sincossinsinsin+coscossinsincoscossin0-sincossincoscos00001（3.15）本讲稿第九页，共三十九页3.5 位置的确定位置的确定(SpecificationofPosition)一旦方向被确定之后，用一个相应的p向量的位移变换可得到机器人末端执行器在基坐标中的位置：100px010pyT6=001pz（3.16）0001旋转变换矩阵本讲稿第十页，共三十九页3.6 圆柱坐标圆柱坐标(CylindricalCoordinates)如图3.6所示，在圆柱坐标中确定机械手的位置是沿x轴平移r，接着绕z轴旋转，最后沿着z轴平移z。Cyl(z,r)=Trans(0,0,z)Rot(z,)Trans(r,0,0)cos-sin00100rsincos000100Cyl(z,r)=Trans(0,0,z)0010001000010001（3.17）1000cos-sin0rcos0100sincos0rsinCyl(z,r)=001z001000010001（3.18）zaCzyxBAorn图3.6圆柱坐标注意：圆柱坐标只能绕z轴旋转本讲稿第十一页，共三十九页cos-sin0rcossincos0rsinCyl(z,r)=001z（3.19）0001 如用一个绕z轴旋转-的变换矩阵右乘式（3.19）,结果如下 cos-sin0rcoscos(-)-sin(-)00sincos0rsinsin(-)cos(-)00Cyl(z,r)=001z0000（3.20）00110001cos-sin0rcoscossin00sincos0rsin-sincos00Cyl(z,r)=001z0000（3.21）00010001100rcos010rsinCyl(z,r)=001z（3.22）0001上式表明平移矢量未变，旋转矩阵为单位阵，此时末端坐标的姿态未变，而只是改变了它的空间位置。本讲稿第十二页，共三十九页3.7 球坐标球坐标(SphericalCoordinates)如图3.7所示，用球坐标来确定位置向量的方法是：沿着z轴平移，然后绕y轴旋转，最后绕z轴旋转。Sph(,)=Rot(z,)Rot(y,)Trans(0,0,)（3.23）cos0sin0100001001100Sph(,)=Rot(z,)-sin0cos000100010001（3.24）aonzyx图3.7球坐标本讲稿第十三页，共三十九页cos-sin00cos0sinrsinsincos000100Sph(,)=0010-sin0cosrcos（3.25）00010001coscos-sincossincossinsincoscossinsinsinsinSph(,)=-sin0coscos（3.26）0001同样，如果不希望改变末端坐标的姿态，而只是改变其空间位置，我们可以用Rot(y,-)和Rot(z,-)右乘式（3.26）Sph(,)=Rot(z,)Rot(y,)Trans(0,0,)Rot(y,-)Rot(z,-)（3.27）100cossin010sinsinSph(,)=001cos（3.28）0001本讲稿第十四页，共三十九页3.7 T6的确定的确定(SpecificationofT6)T6可以用旋转和平移的方法来确定。T6=平移旋转（3.29）表3.1各种平移与旋转的表达式Translation EqnRotationEqnpx,py,pzoxoyozaxayazRot(k,)2.32Cyl(z,r)3.22Euler(,)3.11Sph(,)3.26RPY(,)3.12我们已经研究过的各种平移与旋转的式子，总结在表3.1中。如果我们使用Cyl和Sph的非旋转的形式，那么矩阵积（3.29）仅仅是一个平移变换。本讲稿第十五页，共三十九页3.9 各种各种A矩阵的确定矩阵的确定(SpecificationofmatricesA)现在考虑方程(3.1)右边各A矩阵的确定。串联杆型机械手是由一系列通过连杆与其活动关节连接在一起所组成。如图3.8所示，任何一个连杆都可以用两个量来描述：一个是公共垂线距离an，另一个是与an垂直的平面上两个轴的夹角n，习惯上称an为连杆长度，n称为连杆的扭转角。图3.8连杆的长度与扭转角本讲稿第十六页，共三十九页如图3.9所示，在每个关节轴上有两个连杆与之相连，即关节轴有两个公垂线与之垂直，每一个连杆一个。两个相连的连杆的相对位置用dn和n确定，dn是沿着n关节轴两个垂线的距离，n是在垂直这个关节轴的平面上两个被测垂线之间的夹角，dn和n分别称作连杆之间的距离及夹角。图3.9连杆参数xn-1本讲稿第十七页，共三十九页为了描述连杆之间的关系，我们对每个连杆赋一个坐标系。转转动动关关节节：关节变量为n。连杆n的坐标原点设在关节n和关节n+1轴之间的公共垂线与关节n+1轴的交点上。在关节轴相交的情况下（无公垂线），这个原点就在两个关节轴的相交点上（an0）。如果两个关节轴平行（有无数条公垂线），则原点的选择要使下一个连杆的关节距离为0（dn0），连杆n的z轴与n+1关节轴在一条直线上。x轴与任何存在的公共垂线成一条直线，并且沿着这条垂线从n关节指向n+1关节。在相交关节的情况下，x轴的方向平行或者逆平行zn-1zn的向量叉积，应该注意，这个条件对于沿着关节n和n+1之间垂线的x轴同样满足。当xn-1和xn平行，且有相同的指向时，则对于第n个转动关节n0。连杆本身的参数连杆长度an连杆两个轴的公垂线距离（x方向）连杆扭转角n连杆两个轴的夹角（x轴的扭转角）连杆之间的参数连杆之间的距离dn相连两连杆公垂线距离（z方向平移距）连杆之间的夹角n相连两连杆公垂线的夹角（z轴旋转角）表3.2连杆参数本讲稿第十八页，共三十九页棱棱形形关关节节：关节变量为dn。关节轴的方向就是关节的运动方向。与转动关节不同，轴的运动方向被确定了，但在空间的位置并没有确定（见图2.10）。对于棱形关节，连杆长度an没有意义，所以被设置为0。棱形关节坐标的z轴（zn）与下一个连杆的轴在一条直线上，x轴（xn）平行或逆平行棱形关节轴的方向（zn-1）与zn的叉积。对于棱形关节，当dn=0时，定义为0位置（即坐标原点）。因此棱形关节坐标原点与上一个关节（n-1）坐标原点重合，上一个关节的z轴（zn-1）与棱形关节的轴向相同，其关节长度an-1为上一个关节的轴线与zn-1的公垂线长度，xn-1轴向为公垂线向下一个关节延伸的方向。图3.10棱型关节的连杆参数an-1本讲稿第十九页，共三十九页根据上述模式用下列旋转和位移我们可以建立相邻的n-1和n坐标系之间的关系：绕zn-1旋转一个角度n沿zn-1位移一个距离dn沿着被旋转的xn-1即xn位移an绕xn旋转的扭转角为n这四个齐次变换的积为A矩阵，即An=Rot(z,)Trans(0,0,d)Trans(a,0,0)Rot(x,)（3.30）cos-sin00100a1000sincos0001000cos-sin0An=0010001d0sincos0（3.31）000100010001cos-sincossinsinacossincoscos-cossinasinAn=0sincosd（3.32）0001本讲稿第二十页，共三十九页对于棱形关节，an=0，则式（3.32）A矩阵简化为cos-sincossinsin0sincoscos-cossin0An=0sincosd（3.33）0001一旦给机械手各连杆坐标系都赋了值，各种固定的连杆参数可以确定：对于后面是旋转关节的连杆参数为d,a和，对于后面是棱形关节的连杆参数为和。根据这些参数，的正弦和余弦也可以求出。这样，对于旋转关节，A矩阵变成了关节变量的函数。或在棱形关节的情况下，变成了d的函数。一旦这些值给出，对于六个Ai变换矩阵的值就可以确定。本讲稿第二十一页，共三十九页3.10 根据根据A矩阵来确定矩阵来确定T6(SpecificationofT6inTermsoftheAmatrices)机械手的坐标变换图如图3.11所示，机械手的末端（即连杆坐标系6）相对于连杆坐标系n-1的描述用n-1T6表示，即：n-1T6=AnAn+1A6（3.34）0zA1A2A3A4A5A60EX0T61T62T63T64T65T6图3.11机械手的坐标变换图本讲稿第二十二页，共三十九页机械手的末端相对于基坐标系（用T6表示）用下式给出T6=A1A2A3A4A5A6（3.35）如果机械手用变换矩阵Z与参考坐标系相联系，机械手末端执行器用E来描述，末端执行器的位置和方向相对参考坐标系用X来描述，如图3.11所示有X=ZT6E（3.36）由此可以得到T6的表达式T6=Z-1XE-1（3.37）本讲稿第二十三页，共三十九页3.11 斯坦福机械手的运动方程斯坦福机械手的运动方程(KinematicEquationsfortheStanfordManipulator)斯坦福机械手及其各关节坐标的设置如图3.12所示。将角的正弦和余弦简化sini =Sicosi=Cisin(i+j)=Sijcos(i+j)=Cij注：将所有关节x轴的方向设置一致，可简化坐标变换。图3.12斯坦福机械手坐标系本讲稿第二十四页，共三十九页表3.2斯坦福机械手连杆参数LinkVariableadcossin11-90000-122900d2013d300d31044-90000-1559000016600010本讲稿第二十五页，共三十九页斯坦福机械手的A变换如下：C10-S10S10C10A1=0-100（3.38）0001C20S20S20-C20A2=010d2（3.39）000110000100A3=001d3（3.40）0001 本讲稿第二十六页，共三十九页C40-S40S40C40A4=0-100（3.41）0001C50S50S50-C50A5=0100（3.42）0001C6-S600S6C600A6=0010（3.43）0001本讲稿第二十七页，共三十九页斯坦福机械手A变换的积如下所示，这些是从连杆6开始，然后逐步回到基坐标。C6-S600S6C6005T6=0010（3.44）0001C5C6-C5S6S50S5C6-S5S6-C504T6=S6C600（3.45）0001C4C5C6-S4S6-C4C5S6-S4C6C4S50S4C5C6+C4S6-S4C5S6+C4C6S4S503T6=-S5C6S5S6C50（3.46）0001本讲稿第二十八页，共三十九页C4C5C6-S4S6-C4C5S6-S4C6C4S50S4C5C6+C4S6-S4C5S6+C4C6S4S502T6=-S5C6S5S6C5d3（3.47）0001C2(C4C5C6-S4S6)-S2S5C6-C2(C4C5S6+S4C6)+S2S5S6S2(C4C5C6-S4S6)+C2S5C6-S2(C4C5S6+S4C6)-C2S5S61T6=S4C5C6+C4C6-S4C5S6+C4C600C2C4S5+S2C5S2d3S2C4S5-C2C5-C2d3S4S5d2（3.48）01本讲稿第二十九页，共三十九页nxoxaxpxnyoyaypyT6=nzozazpz（3.49）0001其中nx=C1C2(C4C5C6-S4S6)-S2S5C6-S1(S4C5S6+C4S6)ny=S1C2(C4C5C6-S4S6)-S2S5C6+C1(S4C5S6+C4S6)nz=-S2(C4C5C6-S4S6)-C2S5C6ox=C1-C2(C4C5S6+S4C6)+S2S5C6-S1(-S4C5S6+C4S6)oy=S1-C2(C4C5C6+S4C6)+S2S5S6+C1(-S4C5S6+C4S6)oz=S2(C4C5C6+S4C6)+C2S5S6ax=C1(C2C4S5+S2C5)S1S4C5ay=S1(C2C4S5+S2C5)+C1S4S5az=S2C4S5+C2C5px=C1S2d3S1d2py=S1S2d3+C1d2pz=C2d3本讲稿第三十页，共三十九页3.12 肘机械手的运动方程肘机械手的运动方程(KineamticEquationsforanElbowManipulator)肘机械手及其各关节坐标的设置如图3.13所示。z1z0z2z3z4z5、z6xa3a4a2图3.13肘机械手的坐标系x0y0本讲稿第三十一页，共三十九页 表3.3肘机械手的连杆参数LinkVariableadcossin11900001220a2010330a301044-90 a400-15590 00016600010注：在以下的T矩阵中用变量23=2+3和234=23+4来进行简化。本讲稿第三十二页，共三十九页肘机械手的A变换如下：C10S10S10-C10A1=0100（3.50）0001C2-S20C2S2S2C20S2a2A2=0110（3.51）0001C3-S30C3a3S3C30S3a3A3=0010（3.52）0001本讲稿第三十三页，共三十九页C40-S4C4a4S40C4S4a4A4=0-100（3.53）0001C50S50S50-C50A5=0-100（3.54）0001C6-S600S6C600A6=0010（3.55）0001本讲稿第三十四页，共三十九页 为了得到T6，我们从连杆6开始来算A矩阵的积，逐步往回计算到基坐标。C6-S600S6C6005T6=0010（3.55）0001C5C6-C5S6S50S5C6-S5S6-C504T6=S6C600（3.56）0001C4C5C6-S4S6-C4C5S6-S4C6C4S5C4a4S4C5C6+C4S6-S4C5S6+C4C6S4S5S4a43T6=-S5C6S5S6C50（3.57）0001本讲稿第三十五页，共三十九页C34C5C6-S34S5-C34C5S6-S34C6C34S5C34a4+C3a3S34C5C6+C34S6-S34C5S6+C34C6S34S5S34a4+S3a32T6=-S5C6S5S6C50（3.58）0001C234C5C6-S234S6-C234C5C6-S234C6C234S5C234a4+C23a3+C2a2S234C5C6+C234S6-S234C5S6+C234C6S234S5S234a4+S23a3+S2a21T6=-S5C6S5S6C50（3.59）0001本讲稿第三十六页，共三十九页nxoxaxpxnyoyaypyT6=nzozazpz（3.60）0001其中ox=-C1C234C5S6+S234C6+S1S5S6oy=-S1C234C5S6+S234C6-C1S5S6oz=-S234C5C6+C234C6ax=C1C234S5+S1C5ay=S1C234S5-C1C5az=S234S5px=C1C234a4+C23a3+C2a2py=S1C234a4+C23a3+C2a2pz=S234a4+S23a3+S2a2本讲稿第三十七页，共三十九页3.13 小结小结(Summary)本章，主要介绍了用齐次变换来描述在各种坐标系中机械手的位置与方向。首先介绍了各种正交坐标系的齐次变换。然后介绍了非正交关节坐标系描述机械手末端的齐次变换。上述变换关系对任何数目关节的各种机械手均适用。本讲稿第三十八页，共三十九页任课教师：王劲松联系方式：1.电话：138418192992.办公室：综合楼3073.QQ:3694284414.信箱：研究方向：机电控制与人工智能本讲稿第三十九页，共三十九页