《机器人工学第二章》ppt课件

1、机器人工学 第二章 机器人运动学 一、 引言 机器人的操作机可用一个开环关节链来建模，此链由数个刚体杆件被驱动器驱动的转动或挪动关节串连而成。开链的一端固接在基座上，另一端是自在的，安装着工具未端执行器，用的支配物体，或完成装配作业。关节的相对运动导致杆体运动，使手定位于所需的方位上。在很多机器人运用中，人们感兴趣的是操作机未端执行器相对于固定参考坐标系的空间描画。 机器人运动学的两个根本问题 第一个常称为运动学的正问题 直接问题第二个常称为运动学逆问题 解臂形问题1、运动学的正问题 对一给定的操作机，知杆件几何参数和关节角矢量 其中n是自在度数，求操作机未端执行器相对参考坐标的位置和姿态。

2、Tn21)(t)q,(t),.q(t),q(q(t) 运动学逆问题 知操作机杆件的几何参数，给定操作机未端执行器相对参考坐标系的期望位置和姿态位态，操作机能否使其未端执行器到达这个预期的位姿？如能到达，那么操作机有几种不同形状可满足同样条件？ 两种关系的简一方框图 杆件的几何参数运动学正问题操作机未端执行器位态关节角 (t)q,(t),.q(t),qn21(t)q,(t),.q(t),qn21关节角 运动学逆问题杆件的几何参数二、坐标系与坐标变换 (一)、转动矩阵 机器人的执行机构属于空间机构，因此可以采用空间坐标变换根本原理及坐标变换的矩阵解析方法建立描画各构件坐标系之间相对位置和姿态的矩阵

3、方程。 1、刚体位置和方向的矩阵表示 设有一刚体如下图，O为刚体上恣意一点，oxyz为固定坐标系，刚体在O系中的坐标可用一个列矩阵表示 0000ZYXRXYZOo在刚体上建立一个坐标系OXYZ 刚体的方向可以由O系坐标轴的方向表示，令 代表x、y、z坐标轴方向的单位矢量btn,XYZOoXYnZtb刚体在固定坐标系内的方向表示 每个单位矢量在O系上的分量为O系各坐标轴投影在O系上的方向余弦，于是刚体在固定坐标系内的方向 可用由三个矢量组合起 来的3阶矩阵C表示。XYZOoXYnZtbc= btn,二、转动矩阵的普通方式 设有两个共原点的右手坐标系和如图2-2，后一坐标系可以为是前一系绕定点O旋

4、转而成的。 o ir jr iZ iXiY jXjY jZ P P点从j系变换到i系的坐标变换 假设空间有一点P，该点在i系内的坐标为: 在j系内的坐标为: 假设以i系为参考坐标，根据投影关系，P点从j系变换到i系的坐标变换关系为： TiiiizyxR TjjjjzyxR)cos()cos()cos(jijjijjijizxzyxyxxxx)cos()cos()cos(jijjijjijizyzyyyxyxy)cos()cos()cos(jijjijjijizzzyzyxzxz普通方式的转动矩阵 这一关系可以用矩阵表示为jjiirRr-(1)j坐标系绕i坐标系的某一轴转动的方向余弦矩阵 )co

5、s()cos()cos()cos()cos()cos()cos()cos()cos(jijijijijijijijijijizzyzxzzyyyxyzxyxxxR当两个坐标系无相对转动时 IRjiIRji1000100010cos90cos90cos90cos0cos90cos90cos90cos0cos000000假设取j系为参考系，那么p点从i系到j系的坐标变换为 式中 : iijjrRr)cos()cos()cos()cos()cos()cos()cos()cos()cos(ijijijijijijijijijijzzyzxzzyyyxyzxyxxxR-(2)转动矩阵为正交阵 由于式：

6、- iijjrRr1比较两式： TjijiijRRR1 TjiijRR得:故转动矩阵为正交阵 三、绕一个坐标旋转的转动矩阵 绕x,y,z坐标轴的旋转矩阵是最根本的转动矩阵，它们是普通转动矩阵的特例，故可直接由普通转动矩阵得到。 先看绕x轴的旋转，如图oXj Yj Zj 系可以为是oXi Yi Zi系绕X轴旋转角而成 。绕X轴旋转轴角的转动矩阵 cossin0sincos0001),(XRRjiij绕Y轴旋转角后的转动矩阵 cos0sin010sin0cos),(YRRjiij绕Z轴旋转角后的转动矩阵 1000cossin00sincos),(ZRRjiij转动矩阵的4个特点 1主对角线上有一个

7、元素为1，其他均为转角d的余弦。2绕哪一根轴转动与元素1所在的行，列号对应。3元素1所在的行列，其它元素为04以元素1所在行为准，至上而下，先出现的正弦为负，后出现的为正。 四、绕二个坐标轴旋转的旋转矩阵 设坐标系OXi Yi Zi 先绕Zi 轴旋转角构成坐标系OXm Ym Zm ,再绕Xm 轴旋转角，构成坐标系OXj Yj Zj 绕二个坐标轴旋转的旋转矩阵 前一个旋转: mmimmiirRrZRr,),(后一个旋转 : jjmjjmmrRrXRr,),(坐标系j向i进展坐标变换的矩阵 jjijjijjmmijmimiirRrRrRRrZRZRr,),(),(),( 此式阐明，运用转动矩阵的依

8、次连乘，可进展坐标系的延续变化 此时的转动矩阵为: cossin0sincoscoscossinsinsincossincoscossin0sincos00011000cossin0sincos),(,jijiRR绕三个坐标轴旋转的旋转矩阵 次序是1绕Z轴转过角；2绕新的Xk轴转过角；3绕新的Yl轴转过角构成j坐标系 cssccssccsscRRRRjllkkiji001000000110000,留意: 矩阵相乘普通是不可交换的， 即A B B A。所以，旋转顺序不同，结果是不同的。右乘的次序阐明延续绕新的坐标轴转动，往左乘的次序那么阐明绕固定参考坐标轴依次转动。 例试求表示绕ox轴转角，然后

9、绕oz轴转角，再绕oy轴转角系的合成旋转矩阵。 解:由于是绕固定的坐标系的坐标轴转动，那么应左乘相应的根本旋转矩阵： sssccsccsscsscccscsssccscsscccssccssccsscRRRRRRRxzyjllkkjji000011000000100, 假设转动的次序改为绕oy轴转角，然后绕oz轴转角，再绕ox轴转角,那么表示这种转动的旋转矩阵为: ccssscssccsscssscccsscscscscccssccssccsscRRRRRRRyzxjllkkjji001001000000001,五、旋转矩阵的几何解释 一单位量e即代表了矢量 的方向，单位矢量在坐标系中的方向可

10、用与其坐标轴x,y,z方向的夹角，即方向角来表示 l矢量 的方向余弦 单位矢量e对三个坐标轴的投影， 即为矢量 的方向余弦: ( cos,cos,cos)或 : 是一个列矩阵。也可写成行矩阵: E= coscoscosT llcoscoscose例有转动坐标系ouvw和参考坐标系oxyz如下图。令: 和 分别为沿oxyz和ouvw系坐标轴的单位矢量. XYZOUPVW假定空间某点P在ouvw坐标系中静止并固定，那么，点P可以用它在ouvw系和oxyz系中的坐标分量分别表示为:Puvw=(Pu,Pv,Pw)T和Pxyz=(Px,Py,Pz)T显然，这是Pxyz 的Puvw表示的是在不同坐标系中的

11、同一点。由矢量分量的定义，我们有:Pxyz=Puiu+ Pvjv+ Pwkw Pxyz=Pxix+ Pyjy+ Pzkz这里Pu，Pv，Pw，及Px, Py和Pz分别表示P点ox,oy,oz轴R ov,v,ow轴的分量，或P在各轴上的投影。) ),(zyxkji),(wvukji 如选择固定在ouvw坐标系上的点p为1，0，0T即Puvw =iu ,那么旋转矩阵的第一列就表示此点在oxyz坐标系中的坐标分量。 类似地，假设取点P(0,1,0) T 和0，0，1T，那么可看出旋转矩阵的第二列和第三列分别表示ouvw坐标系的ov轴和ow轴单位矢量在oxyz坐标中的坐标分量。 旋转矩阵的性质 一:

12、旋转矩阵的每一列向量代表了用参考系坐标轴单位矢量表示的转动坐标轴单位矢量，而每一行矢量代表了用ouvw系转动轴单位向量表示的参考系坐标轴单位矢量。旋转矩阵的性质 二: 由于每一行或列代表一个单位矢量，故其模等于1。这是正交坐标系的一个根本性质。此外，旋转矩阵的行列对右手坐标系为+1，而对左手坐标系为-1。 旋转矩阵的性质 三: 由于每一行均为相互正交矢量之一，故不同的两行的内积点乘为零。同样地，不同的两列的内积也为零。旋转矩阵的性质 四:旋转矩阵的逆阵就是它的转置。 R-1=RT 和 RRT=I3 其中I3是3*3单位矩阵 例将转动坐标系ouvw中的ou,ov和ow坐标轴绕ox轴转角，参考系的

13、坐标在转动坐标系ouvw是如何表示。 解新坐标单位矢量在它们本人的坐标系中 iu=(1,0,0)T j=(0,1,0)T kw=(0,0,1)T XYZXXvwv1w1u原系单位矢量在新系中就是 :ix=1iu+0jv+0kw=(1,0,0)Tjoy=0iu+cosdjv-sindkw =(0,cosd,-sind)T kz=0iu+sindjv+cosdkw =(0,sind,-cosd)T 运用性质1并把这些矢量看作旋转矩阵，Rx,d,矩阵即可建立如下，由于在转动坐标系中表示参考坐标系。 cossin0sincos0001,xR三、齐次坐标变换 1、齐次坐标 在三维空间位置矢量，P=Px,

14、Py,PzT 中引进了第四个坐标分量，使它变成: =wpx,wpy,wpz,wT,这样，我们用齐次坐标表示了位置矢量 ，这样，以符(即)表示把一个三维空间矢量写成齐次坐标的方式。) pp实践坐标和齐次坐标的关系三维空间中的位置矢量: P=Px,Py,PzT，在齐次坐标中用增广矢量wpx,wpy,wpz,wT表示。实践坐标和齐次坐标的关系如下： Px= wpx/w Py= wpy/w Pz= wpz/w 对于三维空间的位置矢量，齐次坐标表达并不是独一的 例如：齐次坐标: P1=w1px,w1py,w1pz,w1T P2=w2px,w2py,w2pz,w2T 表示的是同一位置矢量: P=Px,Py

15、,PzT。 2、齐次变换矩阵 齐次变换矩阵是4*4矩阵，它把一个以齐次坐标表示的位置矢量由一个坐标系映射到另一个坐标系，可以把齐次变换矩阵看成是由四个子矩阵组成。 11311333fPRT 旋转矩阵 位置矢量= 透视变换 比例3、转动的齐次变换矩阵 假设三维空间的位置矢量P表示齐次坐标即 =Px,Py,Pz，1T。那么，利用变换矩阵的概念，对纯转动的动作, 3*3旋转矩阵可扩展成4*4齐次变换矩阵，由此，前面的旋转矩阵用齐次变换矩阵表示。 p根本齐次旋转矩阵 100000000001,csscMx100000001000,csscMy100001000000,csscMz根本齐次平移矩阵 齐次

16、变换矩阵的右上角3*1子矩阵具有使ouvw附体坐标系平移的作用。例如以下齐次变换矩阵M使Ouvw坐标系的原点平移到参考坐标系的dx,dy,dz点而坚持坐标轴的平行。 1000100010001zyxdddM部分和整体比例变化 齐次变换矩阵的主对角线元素构成部分和整体比例变化。前三个对角元素构成部分扩展或比例变化，例如： 111000000000000czbyaxzyxcba第四个对角元素产生整体比例变换 例如 :szyxzyxs1000010000100001这里s0,实践矢量的坐标为: sxpxsypyszpz1ssw齐次变换矩阵把在ouvw坐标系中用齐次坐标表示的矢量映射到oxyz参考坐标

17、中 这就是说，当w=1时 = xyzpuvwpM10001000333231232221131211pasnpAAApAAApAAAMzyx四,齐次变换矩阵的几何解释 当变换矩阵为单位矩阵时，即 1000010000100001M 阐明杆件的附体坐标系与参考坐标系完全重合自左至右的列矢量1 0 0 0T 0 1 0 0T 和(0010)T表示两坐标系的各对应xi (xj)轴，yi (yj)轴和zi (zj)轴。而最右边列矢量0001T表示两个坐标重合的原点Oi (Oj)例：当杆件的附体坐标系xj-yj-zj相对于参考坐标系xi-yi-zi先绕zi轴转=90，再绕yi轴转= 90，使xj轴与yi

18、轴重合，yj轴与zi轴重合，如下图。那么 : xi zjyi xjzi yjYj100001000090cos90sin0090sin90cos0000zM1000090cos090sin0010090sin090cos0000yM由于是绕固定参考转动。所以应左乘相应的齐次变矩阵，即: 实践上从这个矩阵中也可看出xj与yi轴重合，yj与zi轴重合，zj与xi轴重合。 100000100001010010000100000100101000000100100100zyjiMMM如下图，有一矢量P可写成列矩阵 当动坐标系xi-yi-zi向参考坐标系xi-yi-zi作齐次坐标变换时，可写成如下变换矩

19、阵，有： 734zyxpppp1000701030014100M而坐标系xj-yj-zj的原点Oj在j坐标系是用0001T表示，所以j坐标系原点的位置向量在xi-yi-zi坐标系中应为: 173410001000701030014100当 : 同样表示杆件动坐标系与参考坐标系完全重合，齐次变换的第一、二、三列1001T，0101T，和0011T，表示对应轴xj,yj,zj上的单位矢量在i系中的坐标分量。 1111010000100001M时 假设杆件动坐标系各轴上的单位矢量绕Zi轴转90绕yi轴转90再平移4，-3，7变换后：得 它表示变换后，杆件动坐标系xj-yj-zj各轴上单位矢量的顶点和

20、坐标原点，在参考坐标系xi-yi-zi上的新坐标值。 111177873332454411110100001000011000701030014100M齐次变换矩阵的逆 设 :10001000pasnpasnpasnpasnMzzzzyyyyxxxx那么逆矩阵为： 10001paaaapsssspnnnnMTzyxTzyxTzyx式中： zyxzyxTpppnnnpn齐次变换矩阵的规那么 1、两坐标系最初相重合，因此齐次变换矩阵是4*4单位矩阵I4 2、假设动坐标j绕或沿固定参考坐标系i的轴转动或平移，那么用相应的根本齐次旋转或平移矩阵左乘原有的齐次变换矩阵。 3、假设动坐标系j绕或沿它本人的

21、轴转动或平移，那么用相应的根本齐次旋转或平移矩阵右乘原有的齐次变换矩阵 例1两个点aj=(4,3,2)T 和bj=(6,2,4)T均沿ox轴平移+5个单位，oz轴挪动-3个单位。利用适宜的齐次变换矩阵决议两个点的新位置ai和bi。 解: 113912341000310000105001ia1121114261000310000105001ib平移后的两点是ai=(9,3,-1)T和bi=(11,2,1)T 例假设动坐标系先绕ox轴转角，再沿转动后的oyj,轴平移b个单位，求对应的合成齐次变换矩阵M。 解1绕ox轴转角之后 ，oyj轴用参考系i的单位矢量ix,jy,kz,表示应为:jrj=cos

22、jyi+sinkzikzj =-sinjyi+ coskziixj轴与原来不变 比较矩阵 即为矩阵中第二、三列。另外，沿转动后的oyj轴平移b单位，就是 : bjYj=bcosdjYi+bsindkzi 故M矩阵为 :10000cossin00sincos00001,xM1000sincossin0cossincos00001,bbMx解2按前述的规那么，由于Mx矩阵把oy轴转至oyj轴，沿oyj,轴再平移可到达同样的目的，即: 1000sincossin0cossincos0000110000100010000110000cossin00sincos00001,bbbMMMbYx两个杆件坐标

23、系的关系 假设两个坐标系分别固联在机器人的两个杆件上，例如杆件i-1和杆件i；利用M矩，我们可把杆件i上在i坐标系中的定点Pi 用杆件i-1坐标系表示为: Pi-1=i-1Mi Pi 五、杆件、关节和它们的参数 操作机由一串用转动或平移关节衔接的刚体组成,每一对关节一杆件构成一个自在度。因此，N个自在度的操作机就有N对关节-杆件，0号杆件固联在支座上，通常在这里建立一个惯性坐标系。最后一个杆件与工具相连。关节和杆件均由支座向外顺序编号；关节1处于衔接杆件1和支座的点上，每个杆件至多与另外两个杆件相连，不构成闭环。 六种低付关节 旋转转动，棱柱形挪动，圆柱形，球形，螺旋和平面。其中只需旋转和棱柱

24、形关节是机器人操作中常见的。 机器人手部的姿态表示:定义为接近矢量，它沿手掌的法线方向.:手的法向矢量.假设为平指手爪,那么它与机器人的“手指垂直.:手的滑动矢量，它指向手爪张合时手指的运动方向 aonRano可以用一个3*3的矩阵表示：机器人手部的位姿ano接近矢量法向矢量滑动矢量这三个矢量构成右手矢量积即:aon手部的位置可以用从基准参考原点指向手部中心的矢量 来表示，即: pTzyxpppp 手部的位姿可以用4*4矩阵表示 1000zzzzyyyyxxxxpaonpaonpaon确定杆系的D-H法Denavit-Hartenberg表示法 机器人运动学的重点是研讨手部的位姿和运动，而手部

25、位置是与机器人各杆件的尺寸、运动副类型及杆件的相互关系直接相关连的。因此要研讨手部相对于机座的几何关系，必需分析两相邻杆件的相互关系，即要先确定杆件坐标系。 连杆参数的表示 任何一个连杆，两瑞有关节i和 i-1,该连杆可以用两个量来描画，一个是两关节的轴线沿公垂线的间隔ai-1，称作连杆长度，另一个是在垂直于ai-1的平面内两个轴线的夹角 ，称之为连杆扭角。 1n表达相邻杆件相互关系的两个参数 di是沿关节i的轴线两个公垂线的间隔(两连杆的偏置) ，i是垂直于关节i轴线的平面内两个公垂线的夹角 (两连杆之间的关节角) 建立杆件的坐标系 按D-H的方法: i-1系的坐标原点设在关节i-1的轴线和

26、关节i的轴线的公垂线与关节i的轴线相交之处. i-1系的Z轴Zi-1与关节i的轴线重合 X轴Xi-1与上述公垂线重合，且方向从关节i-1指向关节i 。当关节是转动关节时，i成为关节变量，假设关节为挪动关节，di成为关节变量，当i-1系的Xi-1轴与i系的Xi平行且方向一样时，定义n=0. 建立杆件i-1的坐标系详细方法 1)坐标系i-1的Z轴Zi-1与关节i的轴线重合 ； 2) X轴Xi-1为相邻Z轴(Zi与Zi-1)的公垂线 (指向由关节i-1到关节i)； 3Y轴那么按右手系确定； 40号坐标系在机座上位置和方向可任选，只需Zo轴沿着第一关节运动轴； 5最后一个坐标系可放在手的任何部位，只需

27、xn与zn-1轴垂直即可 。 杆件的坐标系1轴i杆件的坐标系2Zn-1Xn-1杆件参数与坐标系的关系1)di为沿 zi-1，轴从xi-1到xi轴的间隔，规定与zn-1轴正方向一致时dn为正;2)i为绕zi-1轴从xi-1轴到xi的转角，以逆时针为正;3)ai为沿Xi轴从Zi-1到Zi的间隔，与Xi正方向一致时为正;4)di为绕Xi轴从Zi-1到Zi的转角，也以逆时针为正。杆件坐标系之间的变换矩阵 在用D-H法建立了各杆件坐标系后，即可方便在确定联络i坐标系和i-1坐标系的齐次变换矩阵。即i与i-1系间的变换关系可用坐标和平移，旋转来实现。 杆件坐标系之间变换的步骤 1、将Xi-1轴绕Zi-1轴

28、转i角，使它同Xi轴对准即Xi-1轴与Xi轴平行并指向同一方向； 2、沿Zi-1轴平移间隔di，使Xi-1轴与Xi轴重合， 3、沿Xi轴挪动间隔hi，使两坐标系。原点及X轴重合。 4、绕Xi轴转i角，使两坐标系完全重合。 相邻坐标n和n-1的D-H变换矩阵 10000100000000001100001000010001100001000000100010000100001),(),(),(),(1nnnnnnnnnnnnnnnnnnnnnnnnnnnnndcsshcsccschssscccsschcscdXMkxMzMdzMM六. 正向运动学 正向运动学主要处理机器人运动方程的建立及手部位姿

29、的求解问题。 机器人机构可以以为是一系列杆件由关节衔接起来，我们把描画杆件之间关系的齐次变换矩阵记为M阵 从固定系到手部的各坐标系之间的变换矩阵 M1描画第一个杆系相对固定系的位姿; M2描画第二个杆系相对于第一个杆系的位姿; 而第二个杆系相对于固定系的位姿可用M1M2表示，令其等于T2，即T2=M1M2; 第三个杆系对固定点有T3=M1M2M3，六杆机器人的运动方程 对六杆机器人，有 这里T6表示了手部的位姿，而方程6543216MMMMMMT 6543216321321211.MMMMMMTMMMTMMTMT表示了从固定系到手部的各坐标系之间的变换矩阵与手部位姿的关系，我们称之为机器人的运

30、动方程 .手部的位姿矩阵 是一个4*4的矩阵，表示了手部的位姿 10006zzzzyyyyxxxxpaonpaonpaonT例：1、斯坦福机器人的运动方程 斯坦福机器人是6自在度RRPRRR型机器人，操作机由机座o及六个活动杆件组成。其中杆件2和3构成挪动关节，其他5个均为方程旋转关节。 1、首先建立各杆件坐标系 留意各系z轴沿转动轴线的方向，x轴沿相邻两z轴的公垂线方向。 O系的位置可任选。先确定机座坐标系Xo-Yo-Zo,将其原点O，平移到O1点处，使与x1-y1-z1，坐标共原点， 1系的原点设在1轴2轴的公垂线与2轴的交点处，2系的原点设在2轴与3轴的交点处。而最后的一个坐标系的原点只

31、需其X6轴与Z5轴垂直即可，故和3，4，5，6，共原点。 杆件关节变量i aidi11 -900022900d233 (d3)00d344 -900055 900066 0002、确定各杆件的构造参数和运动变量3、写出各相邻两杆件坐标系之间的位姿矩阵 1系与0系是旋转关节衔接 1000001000001000001001000001100001000000100000000001100001000000),(),(111111111111111111101cssccssccssccsscXMZMM2系与1系旋转关节衔接，杆长d2 10000010010000010000100001000011

32、00001000000100000000001000010000100001100001000000),(), 0 , 0(),(22222222222222222212dcssccsscdcsscxMdMZMM1000110000022222dcssc3系与2系是挪动关节衔接 ，挪动行程为d3 100010000100001), 0 , 0(313ddMM4系与3系是旋转关节衔接 1000001000001000001001000001100001000000100000000001100001000000),(),(4444444444444444444343cssccssccssccss

33、cXMZMM5系与4系是旋转关节衔接 1000001000001000001001000001100001000000100000000001100001000000),(),(5555555555555555555454cssccssccssccsscXMZMM6系与5系是旋转关节衔接 100001000000),(66666565csscZMM手部位置的解 上述方程中各元素均为和d的函数，当和d给出后，可以计算出工件的位置和方向，这些值即手部位置的解，这个求解过程就是正向求解 6655443322110606554433221616554433262655443636554646565TM

34、MMMMMTMMMMMTMMMMTMMMTMMTMT10006zzzzyyyyxxxxpaonpaonpaonT手部位置的正向求解 上式中： 65264542z65216465416465421y65216465416465421xcsc-)ss-c6c(c-sncsss-)sccc(sc)ss-cc(ccsncssc-)ccss(ss-)ss-cc(cccn251646542z65216465416546421y65216465416546421xscc)cs-cc(cs0ssss)cccc(sc-)ccc-c(scs0sssc)cccc(ss)ccc-c(scc0s2s5c4-c2c5az

35、s1s2c5c1s4s5s1s5c2c4ayc1c5s2s1s4s5-c1c2c4s5ax322121321213dcpzdcssdpydsscdpx6系的原点 这里，我们把6的坐标原点与,3,4,5的坐标原点重合在一同，实践上，也可把6的原点挪动到工件的中心及夹持器的中心，这样就可少乘一个矩阵，而求出手部的位姿，但这时的d60，而是d6=dT。 10001000000), 0 , 0(),(66666565TTdcsscdMZMM七.反向运动学 在机器人控制及轨迹规划中，即在知手部要到达的空间位置的情况下，如何求出关节分量，以驱动各关节的马达，使手部的位置得到满足，这就是反向运动学问，也称间

36、接位置求解问。代数法中的逆变换法 如有一个具有四个自在度的机器人，其末端执行相对于机座坐标系的位置矩阵0M4应为: 0M4=0M11M22M33M4 (a) 将一组逆矩阵0M1-1，1M2 -1 ，2M3 -1延续左乘式两端，可得三个矩阵方程式。以0M1-1左乘(a) 式:0M4=1M22M33M4得: 0M1-1 0M4=1M22M33M4或简写成: 0M1-1 0M4=1M4 (b) 同样可以有: 1M2-1 0M1-1=0M4=2M4 (c) 2 M 3 - 1 1 M 2 - 1 0M10M4=3M4 (d) 以上面b(c)(d)的每一个矩阵方程可得12个方程，在这些关系式中可选择只包

37、含一个或不多于两个待求运动参数的关系式，使在求解运动参数时，不用或少用数学上的消元法，在多数情况下，前面由(b)(d)的逆推过程不一定全部作完，就可利用等式两端矩阵中所包含对应元素的关系式，求得所需的全部待求运动参数。 斯坦福机器人运动学方程的逆解 采用逆变换法求解，将用到变换矩阵的逆。 1000100011paaaapsssspnnnnPasnpasnpasnMTzyxTzyxTzyxZzzzyyyyxxxx 斯坦福机器人的运动方程 : T6=0M11M22M33M44M55M6 如今知手部末端执行口位置T6。及各杆件的构造参数a,h,，求:16 (d1d6)先求1，用0M1-1左乘上式，得

38、：0M1-1A1=1M22M33M44M55M6 100000010000100000100000111111111110cssccsscM 代入上式: 1000010000001000001000001000001000001000100001000011000010000010001000000100006666555544443222221111cssccssccsscddcsscPasnpasnpasncsscZzzzyyyyxxxx展开得： 1000)()()()(100025464654646543252542646542652646542325254264654265264654

39、21111111111111111dssccccsscccsdccsscssscccscscsscccsdscssccsscccccsssscccccpspcasacosocnsnpaonspcpsacasocosncnyxyxyxyxzzzzyxyxyxyx取第三行第四列，有： 为求1引入中间变量r, px=r cos py=r sin211dpcpsyx(a) xyyxpptgppr122 py r px 那么(a)式化为 : -sin1rcos+cos1rsin=d2 Cos1sin-cossin1=d2/r 利用和差公式:sin()=sindsoccossin 化为: sin(-1)=

40、d2/r 这里： 0d2/r1，0-1,又cos2+sin2=1 Cos(-1)= 故有: 22)(1rd2221222111drdtgrdrdtg222211222111dppdtgpptgdrdtgpptgyxxyxy求2，取式的第一行第上列和第二行第四列，有 : c1 px+s1py=sin2d3 (e) -pz=-c2d3 (d) 将上式相除 得： zyxppspctg112zyxppspctg1112 求d3，显然式不便于求d3，由于按上面(e)、(d)两式 或 即分母中有sin2或cos2，当2=90o或2=0o 都分别使d3=显然是不合理 2113spspcdyx23cpdz用1

41、M2-1 左乘得: 1M2-1 1T6=1M22M33M44M55M6展开 展开提取第三行第四列，那么有：1000001000)()()()()()()(356565546465464654546465464654211221122112212211111111211221122112212dcsscsssccscsscccssccssccssccccpspcpscasacascosocosncncnsdcpspsacasocosncnspspcpcsasacacsosococsnncnczyxzyxzyxzyxyxxyxyxyzyxzyxzyxzyx3z2y1x12dpc)psp(cs 求

42、，采用如下运动方程： 第一列： c2c4(nxc1+ny)-c4nzs2+s4(nyc1-nxs1) -s2(nxc1+ny)+c2n2 -s4c2(nxc1+ny)+s4nzs2+(nyc1-nxs1) 0 4655463-143MMTM 第二列： c4c2(oxc1+oys1)- 2=ozs2+(oyc1-oxs1)-s2(oxc1+oys1)+c2o2-s4c2(oxc1+oys1)- 2s2+c4(oyc1-oxs1)0第三列： c4c2(axc1+ays1)-s2ax+s4(ayc1-axs1)-s2(axc1+ays1)+c2az-s4c2(axc1+ays1)-s2az+c4(a

43、yc1-axs1)0 第四列： c4c2(pxc1+pys1)-s2pz+s4pyc1-pxs1+d2 -s2(pxc1+pys1)-pzc2+d3 -s4c2(pxc1+pys1)+s4s2pz+pyc1c4-c4pxs1+c4d2 010000 0 cs0c-ss-cs0ssc-cc6 65656556565 取第二行第二列，有： 0aca-scas-)asa(ccs-y1x14z2y1x124z2y1x12y1x114as-)asa(ccacas-tgo44180及 实践上可从前面的(f)式展开式中取第一行第三列、第二行第三列，有： 1M2-1T6=2M35M6 此时假设50，那么 :5

44、4z2y1x12scas-)asa(cc54x1x1ssacas-z2y1x12x1x14as-)asa(ccacas- 假设50，那么: 4=4+180o 假设5= 0，那么位姿退化 (即自在度退化,4与5的转角互换) 此时关节4与6的轴线重合. 求5，采用上述3M4-13T6=4M55M6的展开式， 取第一行第三列，第二行第三列，有：5y1x14z2y1x124s)aca(-ssas-)asa(ccc522y1x12cac)asa(csz2y1x12y1x1422y1x1241 -5ac)asa(cs)aca(-ssas-)asa(ccc tg 求6采用下述方程： 4M5-1 4T6=5M

45、6 展开第一行第二列及第二行第二列，有： -c5c4c2(c1ox+s1oy)-s2oz+s4-s1ox+c1oy+s5s2(c1ox+s1oy)+c2oz =s6 -s4c2(c1ox+s1oy)-s2oz+c4(-s2oz)+c4(-s1ox+c1oy)=c6100001000000100001)()(00)()(00)()(6666535352525151csscofnfofnfofnf)oco(-sc)o(-scos-)oso(ccs-oc)oso(cssoco-ssos-)oso(cccc-tgtgy1x14z24z2y1x124z2y1x125y1x14z2y1x12451 -66

46、1 -6cs 这里求出的全部关节参数，就是位姿矩 阵的解，由求1，2，6的各 关系式中可看出，只在1，2，d3三 个关系中有px,py,pz。在4、5、6 三个关系中有 等参数 3.关于解的讨论 aon, 对六关节机器人操作机， 当后三个关节轴线Z轴交于一点时，前三个关节决议了机械接口坐标系原点06，在空间的位置。因此前三关节连同其杆件称为位置机构。后三个关节角决议了机械接口坐标系的姿态。那么后三关节连同杆件，称为姿态机构。 求解中能够碰到的另一个问题: 机器人操作机运动学的解，普通不是独一的，存在多解的能够。PUMA机器人操作机到达同一位置有八种能够 其手臂相对于机座可构成左臂和右臂两种位形

47、。每一种有肘向上和肘向下两种位形。这样由前三个关节角构成四种解，后三个关节还有倆组解即腕上翘和腕下垂。 斯比福机器人的树状解 对于各关节中运动变量的多解性，而构成的多组解，可以用如下图的树状的图线来表示这种多解关系，称为树状解。 11223d3d44 4 455 5 566 6 6八.机器人的任务空间 1、根本概念 任务空间是手臂末端执行器的夹持中心能到达的空间范围 . 乖巧任务空间手臂末端执行器能以任何姿态到达的点所构成的空间范围称为乖巧任务空间. (例):3R平面关节操作机手臂，由长度为h1,h2,h3的三个活动构件和机座构成，其中h2+h3h1，取点p为末端执行器的参考点. 以h1+h2

48、+h3为半径的圆c1，和以h1-h2-h3为半径的圆c4及它们之间的环形面积就是机器人手臂在x-y平面内的任务面积及其边境，以h1+h2-h3为半径的圆c2和以h1-h2+h3为半径的圆c3及它们之间的环形面积，就是其乖巧任务面积其边境。 2、确定任务空间的方法。 1、几何计算法确定任务空间 最大作业半径:r=l2+l3+h弧形垂直面积:F=dh任务空间体积:v=(l2+l3+h)2d-(l2+l3)2d 夹持器形心点p的坐标值: x=(l2+l3+h)cos1 y=(l2+l3+h)sin1 z=l1+d 由上式即可用来判别其任务空间 的截面外形. 当1=0时, x=l2+l3+h y=0

49、z=l1+d 由于l2+l3,l1为构造参数，h和d为直线位移的运动参数，且相互垂直，故任务空间的垂直截面为矩形，当d=0,01 360o时，那么构成任务空间的截面为环状。 2用运动学矩阵方程确定任务空间 求6r机器人操作机即PUMA机器人的任务空间 。x1y1 根据国标GB12643-90中的规定: 任务空间是指工业机器人正常运转时，手腕参考点能在空间活动的最大范围。故用03任务为所求任务空间的参考点，将求6R机器人操作机任务空间简化成求3R机器人操作机任务空间的计算。 首先建立D-H坐标系，写出变换矩阵如下： 令1=0，得03点在y-z平面内的坐标表达式为： 1chddssh-cchcsh

50、-scch-csch-sshscshss sh1ooo22013233231223213321312232133213z3y3x31chddssh-cchsh sch csh-01ooo220132332322323323z3y3x3 这一式子是03点在y-z平面内的一组圆族方程。 假设给出关节角的取值范围： -150o30o；-90o20o 下面分两种情况讨论上式： 1、在-150o30o范围内，取3为参数，2为变量，由上式得： y=-(h2+h3c3)s2-(h3s3)c2 z-d1-d0=(h3c3+h2)c2-(h3s3)s2 将上两式平方在相加得： y2+(z-d1-d0)2=(h2

51、+h3c3)2+(h3s3)2=R2 它是在y-z平面内以01点为圆心，R为半径的同心圆族，这里R是3的函数，图中:情况1:3=0o , 3=-90o3=-120o，3=-150o情况2:2=0o , 2=-30o2=-60o， 2=-90o 在-90o20o范围内，取2为参数，3为变量，由上式得： y+h2s2=-(h3c2)s3-(h3s2)c3 z-h2c2-do-d1=(h3c2)c3-(h3s2)s3 得方程： y+h2s22+(z-h2c2-d1-d0)2=h32 它是在y-z平面内， 以(-h2s2，h2c2+d1+d0)点为圆心 ,h3为半径的圆。这里圆心坐标是2的函数。 两式

52、构成了y-z平面内机器人任务区域的边境，如图中粗实线所示。 当1绕2轴在取值范围内转动，那么构成此机器人操作的任务空间。 它是一个有一段开口的环形空间，其极限位置取决于1的转角范围。 九.微分变换 微分变换是研讨机器人运动中各坐标之间的微分关系，这个关系是机器人微运动学和动力学的根本点。在机器人运动时，有时需求对手部位姿作微小的调整，因此要讨论机器人杆件在作微小运动时的位姿变化。为了使手部按给定的方向并以给定的速动运动，需求调整各个关节的运动，因此需求推导关节位移和末杆位置的微分关系，解出各关节的速度，在此根底上可进展加速度和动力学分析。 1、机器人的微运动 设机器人运动链中某一杆件对于固定系

53、的位置为T，经过微运动后该杆对固定系的位姿为T+dT，假设这个微运动是相对于固定系进展的，总可以用微小的平移和旋转来表示，即：左乘 IT-)M(k.d.d.dM(dT-)T)M(k.d.d.dM(dT)T)M(k.d.d.dM(ddTTzyxzyxzyxd相对于某个杆系i进展 根据齐次变换的相对性，假设微运动是相对于某个杆系i进展的，那么T+dT可以表示。 I-)M(k.d.d.dTM(dT-)M(k.d.d.dTM(ddT)M(k.d.d.dTM(ddTTzyxzyxzyx 可见无论对哪个系作微运动，均会出现方括号内的公共部分，我们将此部分表示为 : =M(dx.dy.dz)M(k.do)-

54、I 于是上式变为 dT=0 T 及 dT=Ti 这是的下标不同是由于微运动是相对不同的坐标系进展的。 可见无论对哪个系作微运动，均会出现方括号内的公共部分，我们将此部分表示为 : =M(dx.dy.dz)M(k.do)-I 于是上式变为 dT=0 T 及 dT=Ti 这是的下标不同是由于微运动是相对不同的坐标系进展的。 2、微分平移和微分旋转 微分平移变换与普通平移变换一样，其变换矩阵为: 1000100010001dzdy,dx,zyxdddM微分旋转 微分旋转表达式可由普通旋转变换式求出，我们先求普通旋转变换式。 设K是其坐标系C的Z轴的单位向量，并设 1000000zzzyyyxxxao

55、naonaonc 假设i,j,k为基准系坐标轴的单位向量，那么K可表示为: K=axi+ayj+azk 绕K轴旋转等于绕C系内的Z轴旋转，即: M(k.)=M(z, ) 设基准系用T阵描画，那么总可以找到一个变换矩阵X，使C系与基准系T建立关系，同样这个X阵也是一个坐标系 T=CX 在这个式中，X表示T相对于坐标系C的位置，对X求解得: 因此 X=C-1T -(a) 现K轴即C系内的Z轴，T绕K轴旋转等于X绕C系的Z轴旋转，从T系看去，X绕C系Z轴的旋转应为CMZX，故有: Mk.T=CM(z.)X 代入上式X=C-1T ，得 : Mk.T=CM(z.) C-1T M(k.)=CM(z.)C-

56、1 展开此式得: )(100000010000001000010000001000000),(333231232221121211baaaaaaaaaaaaooonnncsscaonaonaonKMzyxzyxzyxzzzyyyxxx 式中 : 222x11cocnxxaayxxyxyyxyx12 c o os o ns o n-c n n aaazxxzxzzx zx13 c o os o ns o n-cn naaayxxyyyxyyx21 c o os o ns o n-c n naaa222y22cocnyyaazyyzyzzy zy23 c o os o ns o n-cn naaa

57、xzxzzxxzzx31a ac o os n ns o n-c n n azyzyzyyz zy32 c o os o ns o n-cn naaa222z33cocnzzaa 利用正交阵的某些性质可以简化(b)式，令Kx=ax Ky=ay Kz=az，并定义(正矢): Vers=1-cos，那么有 :10000cVersKkzsVersKKsK-Vers KK0sK-VersKKcVers KsKVersKK0 sKVersKKsK-VersKKcVersK),(2zzyyzxxzy2yzyxyzxzyx2xKM此即绕恣意向量K旋转时的变换矩阵的普通表达式。绕坐标轴旋转变换只是其特例。 普

58、通旋转变换矩阵有了以后就可求出微分旋转表达式，留意当0时，sind，cos1，Vers0， 因此上式变成 :100001kzddK-0dK-1dK0 dKdK-1),(yxzyzKM-A* 令:Kxd=x Kyd=y Kzd=z 于是又可改成 0000000zxyyxzxyzddd 因此可以看成由和d两个向量组成，叫微分旋转矢量，d叫微分平移向量，分别表示为 kjizyxkdjdiddzyx 于是变为 :0000d0kzddK-ddK-0dKd dKdK-010000100001000011000d1kzddK-ddK-1dKd dKdK-11000100010001),(),(zyyxzxy

59、zzyyxzxyzzyxzyxdddIKMdddM-(d) 3、微分旋转的无序性 在齐次变换中，矩阵左乘与右乘的意义与效果是不同的，而对微分旋转来说，左乘右乘的次序是无关的。为了阐明这一点，先看坐标轴微分旋转的情况。 当0时，sind，cos1，假设令:x=dx，y=dy，z=dz 那么绕三个坐标轴的微分旋转矩阵分别变成 :10000100100001),(xxxXM10000100010001),(yyyYM10000100001001),(zzzYM左乘右乘的结果 最后结果中忽略了高阶小量xy,两者结果一样，可见这里左乘右乘等效。微分旋转其结果与转动次序无关，这是与有限转动的一个重要区别。

60、 10000101000110000101001),(),(xyxyxyxyxyyxYMXM10000101000110000101001),(),(xyxyxyxyyxxyYMXM 在上述两次微分转动后，再左乘或右乘上一个绕Z轴的微分转动，得 察看上面绕一个轴至三个轴微分旋转的矩阵，发现它们都是反对称矩阵1000010101),(),(),(xyxzyzzyxZMYMXM( c ) 对照前面(a)式与(c)式，两者是等效的，故有以下关系: Kxd=x Kyd=y Kzd=z 于是(2-141)又可改成)(0000000fdddzxyyxzxyz 因此可以看成由和d两个向量组成，叫微分旋转矢量