1、山东大学机械工程学院机电工程研究所2010/09/02第第3 3章章 机器人运动学机器人运动学3.1 3.1 机器人的位姿描述机器人的位姿描述3.2 3.2 齐次变换及运算齐次变换及运算3.3 3.3 机器人运动学方程机器人运动学方程3.4 3.4 微分运动与雅克比矩阵微分运动与雅克比矩阵山东大学机械工程学院机电工程研究所2010/09/023.4 微分运动与雅克比矩阵微分运动与雅克比矩阵山东大学机械工程学院机电工程研究所2010/09/023.4 微分运动与雅克比矩阵微分运动与雅克比矩阵3.4.1 概 述 机器人的微分运动是研究机器人关节变量的微小变化与机器人手部位姿的微小变化之间的关系。
2、机器人关节变量的微小变化d（即微分）除以时间的微小变化dt，就是机器人关节的速度：v= d/dt。 因此，本小节研究与机器人速度相关的计算，包括：关节速度、杆件速度和手部速度，以及关节的速度与其手部在笛卡尔空间中的速度之间的关系。 山东大学机械工程学院机电工程研究所2010/09/023.4 微分运动与雅克比矩阵微分运动与雅克比矩阵 两类问题： 1、已知机器人各关节的速度时，求机器人手部在笛卡尔空间的速度。 2、已知机器人手部在笛卡尔空间的速度时，求机器人各关节的速度。应用：机器人控制、误差分析、动力学分析等。 山东大学机械工程学院机电工程研究所2010/09/023.4 微分运动与雅克比矩阵
3、微分运动与雅克比矩阵例：一个有两个转动关节的平面机械手，如图；杆长分别为l1和l2，杆2的端点为M，关节变量为1和2，试求M 速度与关节速度的关系。解：建立齐次变换10000100sin0)cos()sin(cos0)sin()cos(11212111212102llM山东大学机械工程学院机电工程研究所2010/09/023.4 微分运动与雅克比矩阵微分运动与雅克比矩阵 点M在杆件坐标系中的齐次坐标（l2,0,0,1），将点M在基座标系中表示，有：10010202lMyxmm即：)sin(sin)cos(cos2121121211llyllxmm2212121211122121212111)(
4、cos)(coscos)(in-)(in-in-lllyslslslxmm求导的：山东大学机械工程学院机电工程研究所2010/09/023.4 微分运动与雅克比矩阵微分运动与雅克比矩阵写成矩阵形式为：212122121121221211)cos()cos(cos)sin()sin(sinllllllyxmm)cos()cos(cos)sin()sin(sinJq,qJx212212112122121121llllll其中：简写为： 称J为雅克比矩阵，它表示末端执行器的速度与关节速度的“广义传动比”。山东大学机械工程学院机电工程研究所2010/09/023.4 微分运动与雅克比矩阵微分运动与雅克
5、比矩阵又有：xq1 J1J称为雅克比逆阵。 从上例可看出，通常雅克比矩阵和雅克比逆阵不是常阵，而以关节变量有关。)sin(sin)cos(cos)sin()cos(J21211212112122121 -llllll其中：山东大学机械工程学院机电工程研究所2010/09/023.4.2 微分变换-微小运动 可以证明：绕任意轴k转动微量角d，可以用绕x、y、z三个坐标轴旋转x、y和z来等价，我们知道： 绕x轴旋转的微分变换矩阵为： 1000010010000110000cossin00sincos00001lim),(sin0 xxxxxRot 1000010001000110000cos0si
6、n00100sin0coslim),(sin0yyyyyRot 绕y轴旋转的微分变换矩阵为：山东大学机械工程学院机电工程研究所2010/09/023.4.2 微分变换绕z轴旋转的微分变换矩阵为： 100001000010011000010000cossin00sincoslim),(sin0zzzzzRot 可以证明：上述三个微分旋转变换矩阵按任意顺序相乘，只要略去高阶微量，其结果均为： 1000010101),(),(),(xyxzyzzyxzRotyRotxRot （1）山东大学机械工程学院机电工程研究所2010/09/023.4.2 微分变换 我们知道：绕任意轴的转动的变换矩阵为：100
7、0000),(cverskkskverskkskverskkskverskkcverskkskverskkskverskkskverskkcverskkkRotzzxzyyzxxyzyyzyxyxzzxyxx 它表示以k k=(kx,ky,kz)为轴转动角度。 当转角为微小量时，sin，cos1，vers=1-cos0，可得：1000010101),(dkdkdkdkdkdkdkRotxyxzyz（2）山东大学机械工程学院机电工程研究所2010/09/023.4.2 微分变换比较（1）与（2）式，可知：只要保证： dkdkdkzzyyx,x 那么绕任意轴k的任何微转动变换，就相当于绕x,y,z
8、轴按任意次序进行的三个微转动变换。另外，微分平移变换为：1000100010001,transzyxzyxdddddd）（山东大学机械工程学院机电工程研究所2010/09/023.4.2 微分变换 可以证明，连续微分平移变换的最终结果与变换的次序无关；同样，连续的微分转动变换与微分平移变换与变换的次序无关。山东大学机械工程学院机电工程研究所2010/09/023.4.3 变换微分-dM 设机器人运动链中某一杆件相对于机座坐标系的位姿为 ，它是一个4x4矩阵，其元素以q为单变量，即M=M(q)；经过微运动后，q变成q+dq，该杆件位姿变为 ，则位姿的微小变化为：MdMM dqdqdMdM nii
9、idqqMdM1若位姿是若干个变量的函数，则：1、从微分运算的角度推导变换微分 注意：矩阵的导数等于其各元素的导数。山东大学机械工程学院机电工程研究所2010/09/023.4.3 变换微分例例1、已知、已知2自由度机器人及其坐标系如图所示。自由度机器人及其坐标系如图所示。若因杆件若因杆件1下关节轴承装配或制造下关节轴承装配或制造不当，使杆件不当，使杆件1沿关节轴线有沿关节轴线有0.05单位的偏差，又由于两杆件的执行单位的偏差，又由于两杆件的执行器运动不准确，旋转执行器使杆件器运动不准确，旋转执行器使杆件1多转一个多转一个0.01rad的偏差角，移动的偏差角，移动执行器使杆件执行器使杆件2移动
10、了一个移动了一个0.1单位单位的偏差距离。若杆件的偏差距离。若杆件1的长度的长度单位，试求当机器人关节变量取单位，试求当机器人关节变量取 单位时，机器人单位时，机器人手部位姿的偏差。手部位姿的偏差。 10,9021 d 51 l山东大学机械工程学院机电工程研究所2010/09/023.4.3 变换微分 解：采用第一坐标系，杆件参数为：1000100001000011000010001000100001000011000c0c2111111111211111111111111111120102ddslcsclscddsslsccsclsscscMMM1000010001111211111211d
11、slcdcsclsdsc i dii lii 1 d115 90 2 d200 0机器人手部的位姿为：两变量山东大学机械工程学院机电工程研究所2010/09/023.4.3 变换微分 M02中有三个变量 ，由：01. 000000000001112111112111102clsdscslcdcsdM1090000000001 . 001. 00005. 00001. 021l， ),(1210202ddMM山东大学机械工程学院机电工程研究所2010/09/023.4.3 变换微分1 . 000000000000000112202csdddM900000000000001 . 0000105.
12、000001000000000001102dddM000005. 000000000000山东大学机械工程学院机电工程研究所2010/09/023.4.3 变换微分机器人手部位姿的偏差为：机器人手部位姿的偏差为：22021102110202dddMdddMdMdM1090000005. 00001 . 001. 00005. 00001. 021l，山东大学机械工程学院机电工程研究所2010/09/023.4.3 变换微分2、从坐标系变换的角度推导变换微分 设i为基础坐标系，j为当前坐标系，两者之间的位姿关系为M0j,经过微分运动后变为Mij + dMij ，即 Mij Mij + dMij
13、。 机器人姿态的上述变化可以分解为微分平移运动和微分旋转运动的组合：),(dkRot),(zyxdddTrans山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分 从位姿Mij,运动到位姿Mij + dMij，可以通过两种方式实现：即绝对微运动或相对微运动，即： jijijiijijiiijjjijijiiijijMMIRotTransMMIRotTransdMRotTransMMRotTransdMM)()()()()()()()()()jjIRotTransIRotTransjjjiii()()()()其中：称为变换微分算子（或矩阵）。MdMM+dM绝运动对相对运动山
14、东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分1）、变换微分算子 变换微分算子的形式为： 它包含微分平移和微分旋转两个变换。 微分平移变换矩阵与一般的平移变换矩阵一样，为：1000100010001), 0 , 0()0 , 0()0 , 0 ,(),(zyxzyxzyxddddTransdTransdTransdddTransIRotTrans()()山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分 综上所述，变换微分算子即为：IzRotyRotxRotdddtranszyxzyx ),(),(),(),( 0000000zxyyxzxyzdd
15、d 与Rot(k,d)矩阵中的元素比较可知：dkdkdkzzyyxx 因此，可看成由和d d 两个矢量组成的，其中， 称为微分旋转矢量，而 称为微分平移矢量。kjizyxkdjdidzyxd山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分例：考虑只存在微转动的情况，假设绕Z轴做微小转动，如图。100001000010011000010000cossin00sincoslim),(sin0zzzzzRot0000000000000010000100001001zzzzI有：山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分 我们将和d d 合称为微分
16、运动矢量，用D表示为：dDDdddzyxzyx或注意：对应坐标系i和j的微分运动矢量是不同的，用Di和Dj来区分。山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分微分运动矢量的物理意义： 微分旋转矢量：它的方向表示旋转轴方向，大小表示旋转角度，三个分量表示等价的三个绕坐标轴旋转的为角度。 微分平移矢量：表示坐标原点的微平移量。 变换微分矩阵和变换微分算子:变换微分矩阵是与微分旋转矢量和微分平移矢量等价的矩阵表示。 它们都是用元素 对微分运动进行表述，但表述的方式不同。 zyxzyxddd,和山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分例2、试
17、用变换微分矩阵来解例1所示的问题。解：当1=90，l2=10时，机器人手部的位姿为：1000001050011500002M山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分 如图所示，机器人手部相对基座坐标系的平移误差和旋转误差就相当于微分平移矢量和微分旋转矢量，即：kjidkji05. 001 . 001. 00000山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分 由此可得：000005. 000000001. 01 . 0001. 0000000000zxyyxzxyzddd最后，机器人手部姿态的微变化为：000005. 00001 . 0
18、01. 00005. 00001. 010000010500110100000005. 000000001. 01 . 0001. 0002002MdM与前面用微分运算的结果相同。注意：这里的微分运动是在形成M02前完成的，因此，为前乘。另外，对微分运动，运动顺序无关。山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分2）两坐标系间变换微分算子i与j的关系 设任意两个坐标系i和j 之间的变换关系为Mij。若相对于坐标系i进行的微分运动用变换微分算子i表示，相对于坐标系j的微分运动用j表示，定义坐标变换的微分为： jijijiijMMdMijiijjMM11ijjijiMM
19、则： i和j具有不同的微分运动矢量。山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分 i和 j的微分运动矢量的关系如何？ 设i的微分运动矢量为： 设：kjiziyixiikdjdidziyixiid0000000izixiyiyixizixiyiziddd10001000panzzzzyyyyxxxxijpaonpaonpaonM山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分0000)()()()()()()()()()()()(iiiiiiiiiiiiiiijdpaaaoanadpoaooonodpnanonnnijiijjMM1可得：由：由
20、混合积性质可化简上式，我们知道：0)()()()(abaacbcabcba山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分化简为：0000)(0)(0)(0iiiiiiiiiiiijdpanodponadpnoa设：比较两式可得：，izjiyjixjaon,)(,)(,)(iizjiiyjiixjdpaddpoddpnd0000000jzjxjyjyjxjzjxjyjzjddd山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分上式也可以简写为：iiTTTjjdRpSRRd0)(其中：zzzyyyxxxaonaonaonR000S(p)xyxzyzp
21、pppppiTziyixizyxzyxzyxiiizjyjxjjRaaaooonnnaoniTiTiiiiiiiiiiiizjyjxjjpSRdRpapopndadodndpadpodpnddd)()()()()()()(d山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分dMij以及i、j的物理意义： dMij是j坐标系相对i坐标系的位态的微变化，它既可以通过对i坐标系做i变换来实现，也可以通过对j坐标系做j变换来实现。 相对i坐标系做i变换（左乘）效果上相当于对j坐标系做j变换（右乘）。 这相当于机器人各关节的微转动，引起机器人手部位态的微变化；或者，机器人各关节的速
22、度，决定机器人手部的速度。山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分例3、试用右乘微分变换矩阵来解例1所示的问题。解：根据题意有：，）（）（）（）（）（）（001.0 , 0 , 00 , 0 , 101.001.0 , 0 , 01 , 0 , 0001.0 , 0 , 00 , 1 , 0020202aonzyx05. 005. 0 , 1 . 0 ,05. 00 , 0 , 1)(05. 005. 0 , 1 . 0 ,05. 01 , 0 , 0)(1 . 005. 0 , 1 . 0 ,05. 00 , 1 , 0)(05. 0 , 1 . 0 ,05.
23、 005. 0 , 0 , 1 . 00 , 1 . 0 ,05. 0)(22200）（）（）（）（）（）（）（）（）（iiziiyiixdpaddpoddpnddpkjidkji05.001.001.000001000001050011500002M山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分 所以：kjidkji05. 005. 01 . 0001. 0022000005. 00001. 005. 00001 . 001. 0002000005. 00001 . 001. 00005. 00001. 0000005. 00001. 005. 00001 . 00
24、1. 0001000001050011010020202MdM相应的2为：则：可见，与前面例子的结果相同。山东大学机械工程学院机电工程研究所2010/09/023.4.3变换微分 得到机器人手部相对于基座坐标系的姿态微变化dM后，除以时间的微变化dt，就可以获得机器人手部相对基座坐标系的、笛卡尔空间的速度矢量：dtdMvn0或用运动微分矢量表示：dvtvt1lim0山东大学机械工程学院机电工程研究所2010/09/023.5 机器人的雅可比矩阵山东大学机械工程学院机电工程研究所2010/09/023.5 雅可比矩阵 这一小节，我们研究机器人手部速度与关节速度之间的关系。 假设我们有六个独立的函
25、数：),(),(),(654321666543212265432111xxxxxxfyxxxxxxfyxxxxxxfy把它们统一写成向量函数方程，有：)( XFY 山东大学机械工程学院机电工程研究所2010/09/023.5 雅可比矩阵 将上述各方程求微分，得：666226116666222211226612211111dxxfdxxfdxxfdydxxfdxxfdxxfdydxxfdxxfdxxfdy或：dXXFdY 我们称上式中的6X6偏微分矩阵为雅克比（JACOBIAN）矩阵，简记为J。山东大学机械工程学院机电工程研究所2010/09/023.5 雅可比矩阵 通常，雅克比矩阵J是x的函数
26、，我们通常写成：dXXJdY)(等号两边除以时间的微分，可得：XXJY)( 这里，雅克比可查看成是将速度Vx映射为速度Vy。雅克比矩阵中各元素的意义： 元素 表示单位变量（dxi=1）对函数值(dyj)的贡献。xjfJiij山东大学机械工程学院机电工程研究所2010/09/023.5 雅可比矩阵 如果机器人手部速度是相对i坐标系的，那么，其相应的雅克比矩也是相对i坐标系的，表示为Ji（X）。在机器人学领域，有：)(00XJv 它表示机器人手部相对基座坐标系的笛卡尔速度与关节速度之间的关系。如果雅克比矩阵的逆存在的话，有：vJ)(1 雅克比矩阵的逆不存在的位置，称为机器人的奇异位置。山东大学机械
27、工程学院机电工程研究所2010/09/023.5 雅可比矩阵 若将手部的变换微分矩阵用微分运动矢量在n坐标系中的分量来表示，则： niiznnynnxnnznnynnxnnzniynixnizniynixnizniynixnizniynixniznynxnznynxnznynxnznynxndqdqddqddddddddddddddd11111111111111 山东大学机械工程学院机电工程研究所2010/09/023.5 雅可比矩阵 若令J Jn为： znnynnxnnznnynnxnnzniynixnizniynixnizniynixnizniynixniznynxnznynxnndddd
28、ddddddddJ 111111111111 称它为机器人的相对n坐标系的雅克比矩阵，相对不同坐标系有不同的雅克比矩阵。山东大学机械工程学院机电工程研究所2010/09/023.5 雅可比矩阵 雅克比矩阵中各元素的物理意义：1）、雅克比矩阵中的第x行反应各关节的单位微分运动对机器人手部在n坐标系中x微分运动分量的影响。2）、雅克比矩阵中的i列反应i号关节的单位微分运动对机器人手部在n坐标系中所有微分运动分量的影响。3）、雅克比矩阵中元素 反应第i关节的单位为运动引起的机器人手部在n坐标系中x微分运动分量。xnid山东大学机械工程学院机电工程研究所2010/09/023.4.2、机器人的雅可比矩
29、阵 若要求相对基座坐标系的雅克比矩阵J0，仅需要通过位姿矩阵M0n将微运动矢量在基座坐标系中表示，相应的雅克比矩阵就是相对基座坐标系的雅克比矩阵。 即：000010000n00nnnnnnnndRRdpRM山东大学机械工程学院机电工程研究所2010/09/023.4.2、机器人的雅可比矩阵 可知：机器人基座坐标系的雅克比矩阵J0 的各列矢量可由下式确定：ninininidRdR0000， 若已知在n坐标系中的雅克比矩阵，在n中表示的速度为：)(vnnnnJvnnnnvRRv000000)(000000nnnJRRv)(00)(000nnnJRRJ在基坐标系0中为：所以：山东大学机械工程学院机电
30、工程研究所2010/09/023.5 雅可比矩阵雅克比矩阵的应用： 已知手部负载，求关节静力距。 根据功守恒原理，力在任务空间所做的功应等于在关节空间做的功，即：TTxF由于：Jx TTFJFJFTTT或J有：山东大学机械工程学院机电工程研究所2010/09/023.6小节机器人的杆件的速度山东大学机械工程学院机电工程研究所2010/09/023.6 机器人的杆件的速度基本思路：基本思路： 已知基座速度和各关节的相对速度，从基座速度开始，一步一步递推出末端执行器的速度。山东大学机械工程学院机电工程研究所2010/09/023.4.3、机器人的杆件的速度 机器人杆件的速度包括线速度和角速度，下面
31、介绍如何从i杆件的速度递推计算i+1杆件的速度。 如图所示，设已知i杆件的速度为i和vi，i+1杆件绕Zi+1轴旋转的角速度为 。 1i山东大学机械工程学院机电工程研究所2010/09/023.4.3、机器人的杆件的速度 则：在i+1坐标系中表示的i+1杆件杆的角速度为：111111iiiiiiiiiZR 在i+1坐标系中表示的i+1坐标系原点的线速度为：)(1111iiiiiiiiiipvRv在i+1中表示的i+1杆的角速度其中 是在i中表示的指向i+1原点的距离。1iip山东大学机械工程学院机电工程研究所2010/09/023.4.3、机器人的杆件的速度例1、一两杆关节机器人如图所示，计算
32、以关节速度为函数的手尖处的速度。山东大学机械工程学院机电工程研究所2010/09/023.4.3、机器人的杆件的速度解：1、建立坐标系，如图： 2、求位姿矩阵：100001000011001101csscM100001000022022112cslscM100001000011001101csscM100001000010001223lM山东大学机械工程学院机电工程研究所2010/09/023.4.3、机器人的杆件的速度得：212233000)(220)(0022212111121211112333lclsllclslRv21220000011v111000220010002202211111
33、22clsllcsscv山东大学机械工程学院机电工程研究所2010/09/023.4.3、机器人的杆件的速度如果在基座坐标系中表示，仅需乘以R03。100012120121223120103csscRRRR0)(121)(12121211212113303303330clclslslvRv则：山东大学机械工程学院机电工程研究所2010/09/023.4.3、机器人的杆件的速度例2、试求例2中两杆关节机器人的雅克比矩阵。解：由例1知：212233000)(22212111133lclslvJllclsllclslv32122112121211113333311202)(22v则：及山东大学机械工
34、程学院机电工程研究所2010/09/023.4.3、机器人的杆件的速度 雅克比矩阵的行数等于笛卡尔空间自由度，列数等于机器人的关节数。 同理，我们可以求相对基座坐标系的雅克比矩阵。 0)(121)(1212121121211330330clclslslvRv1212112121)(2212210clclclslslslJ所以：山东大学机械工程学院机电工程研究所2010/09/023.4.3、机器人的杆件的速度 雅克比矩阵的逆为：121121121221)(2121222110clslclclslclsl lJ 当手尖沿X方向以速度1m/s运动时，由雅克比逆矩阵可得：Tv)0 , 1 (021221,21222211slcslcslc 当2=0时，上式分母为零，两关节速度将趋于无穷大，它对应机器人的奇异位置。01121121121221)(212122211021clslclclslclsllvvJyx
