第三章__机器人运动学

1、 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 机器人的机械手看作是一系列由关节连接起来的连杆构成的。为机械手的每一连杆建立一个坐标系，并用齐次变换来描述这些坐标系间的相对位置和姿态。通常把描述一个连杆与下一个连杆间相对关系的齐次变换叫做A矩阵。一个A矩阵就是一个描述连杆坐标系间相对平移和旋转的齐次变换。如果A A1表示第一个连杆对于基系的位置和姿态， A A2表示第二个连杆相对于第一个连杆的位置和姿态，则第二个连杆在基系中的位置和姿态可由下列矩阵的乘积给出 T T2= A A1 A A2同理，若A A3表示第三个连杆相对于第二个连杆的位置和姿态，则有 T T3= A A1 A A

2、2 A A3称这些A A矩阵的乘积为T T矩阵，其前置上标若为0，则可省略。对于六连杆机械手，有下列T T矩阵 T T6= A A1 A A2 A A3 A A4 A A5 A A63.1n,o,ap图 矢量和机械手的运动方向原点由矢量p表示。接近矢量a：z轴设在手指接近物体的方向，称为接近矢量轴设在手指接近物体的方向，称为接近矢量 方向矢量o：y轴设在两手指的连线方向，称为方位矢量轴设在两手指的连线方向，称为方位矢量 法线矢量n：x轴由右手系确定，轴由右手系确定， 即即 n = o a ，称为法向矢量。，称为法向矢量。手爪坐标系手爪坐标系因此，变换T6具有下列元素。 六连杆机械手的T矩阵（

3、T6 ）可由指定其16个元素的数值来决定。在这16个元素中，只有12个元素具有实际含义。 60001xxxxyyyyzzzznoapnoapTnoap 机械手由一串用转动或平移关节连接的刚体(杆件)组成。每一对关节杆件构成一个自由度。杆件的编号由手臂的固定基座开始，固定基座可看成杆件0，第一个运动体是杆件1，依次类推，最后一个杆件与工具相连；关节1处于连接杆件1和基座之间，每个杆件至多与另外两个杆件相联，而不构成闭环。 杆件杆件i i的长度的长度a ai i，是杆件上两个关节轴线的最短距离；杆件杆件i i的扭转角的扭转角i i ，是两个关节轴线的夹角。 3.1.1 3.1.1 Denavt-H

4、artenberg(D-H)表示法AiiiaAi+1任何杆件i都可以用两个尺度表征，如上图所示，AiAi-1idiaia1iaii两个杆件的相对位置由两个参数决定：两杆间的距离两杆间的距离di：关节轴上两个法线的距离 夹角夹角i：关节轴上两个法线的夹角 为描述相邻杆件间平移和转动的关系。 Denavt和Hartenberg (1955)提出了一种为关节链中的每一杆件建立附体坐标系的矩阵方法。D-H方法是为每个关节处的杆件坐标系建立4 4齐次变换矩阵，表示它与前一杆件坐标系的关系。这样逐次变换，用“手部坐标”表示的末端执行器可被变换并用机座坐标表示。 坐标系的建立有两种方式：固联坐标系后置固联坐

5、标系后置固联坐标系前置固联坐标系前置3.1.1 3.1.1 Denavt-Hartenberg(D-H)表示法 n关节机器人需建立n+1个坐标系，其中参考(机座)坐标系为O0 x0y0z0 ，机械手末端的坐标系为Onxnynzn，第i关节上的坐标系为Oi-1x i-1y i-1z i-1。坐标系Si置于连杆Li的远离基座的关节上，故称固联固联坐标系后置坐标系后置。 确定和建立每个坐标系应根据下面3条规则: z i-1轴沿着第i关节的运动轴；x i轴垂直于z i-1轴和z i轴并指向离开z i-1轴的方向y i轴按右手坐标系的要求建立。 按照这些规则，第0号坐标系在机座上的位置和方向可任选，只要

6、z0轴沿着第i关节运动轴。第n坐标系可放在手的任何部位、只要xn轴与zn-1轴垂直。 固联坐标系后置固联坐标系后置转动关节连杆四参数示意图9机器人机械手上坐标系的配置取决于机械手连杆连接的类型。有两种连接转动关节和棱柱联轴节。现在来考虑棱柱联轴节（平动关节）的情况。下图示出其特征参数 。棱柱关节连杆四参数示意图和d,3.1.2 3.1.2 几何参数定义几何参数定义 i i：绕z i-1轴（右手规则）由x i-1轴向x i轴的关节角；d di i：从第i1坐标系的原点到z i-1轴和x i轴的交点沿z i-1轴的距离；a ai i：从z i-1和x i轴的交点到第i 根据上述对杆件参数及坐标系的

7、定义，描述串联机器人相邻坐标系之间的关节关系可归结如下4个参数：坐标系原点沿x i轴的偏移距离(是z i-1轴和zi两轴间的最小距离） i i ：绕x i轴（右手规则）由z i-1轴转向zi轴的偏角。 对于转动关节转动关节，di、ai、i是关节参数，i是关节变量。移动关节移动关节的关节参数是i i、 a ai i、i i， d di i是关节变量。3.1.2 3.1.2 几何参数定义几何参数定义 将第i个坐标系表示的点ri在i-1坐标系表示，需建立i坐标系和i1坐标系的齐次变换矩阵，需经过以下交换：(1)将坐标系Oi-1xi-1yi-1zi-1绕zi-1轴转i角，使xi-1轴与xi平行并指向同

8、一方向；(2)将坐标系Oi-1xi-1yi-1zi-1沿zi-1轴平移距离d dii ，使xi-1轴与Oixiyizi的xi轴重合；(3)将坐标系Oi-1xi-1yi-1zi-1沿xi轴平移距离a ai i ，使两坐标系的原点重合；(4)将坐标系Oi-1xi-1yi-1zi-1绕xi轴转 ii角，使两坐标系完全重合。3.1.3 3.1.3 建立建立i i坐标系和坐标系和i-1 i-1 坐标系的齐次变换矩阵坐标系的齐次变换矩阵 i坐标系和il坐标系的齐次变换矩阵i-1Ai可以根据矩阵的合成规则得到，i-1Ai称为相邻坐标系i和i1的D-H变换矩阵。即1000cossin0sincossincos

9、cossincossinsinsincoscos10000cossin00sincos000011000010000100011000100001000011000010000cossin00sincos1iiiiiiiiiiiiiiiiiiiiiiiiiiiiidaaadA固联坐标系后置固联坐标系后置（zi位于i+1关节轴上），变换公式),()0 , 0 ,(), 0 , 0(),(11iiiiiiiixRotaTransdTranszRotA对于在第i坐标系中的点ri在第i1坐标系中表示为:iiiirrA11ijjiAAAAAAAT1654321010100000iiiiiiPRpaon

10、确定第i坐标系相对于机座坐标系的位置的齐次变换矩阵i-1T Ti是各齐次变换矩阵Ai的连乘积，可表示成固联坐标系前置固联坐标系前置 连杆Li的固联坐标系Si的zi轴置于i关节的旋转（或移动）轴上，即坐标系Si置于连杆Li的靠近基座的关节上，故称固联坐固联坐标系前置标系前置。zi-1与zi的公垂线为xi-1, zi与zi+1的公垂线为xi轴, i-1:i-1:为zi-1与zi的交错角；i i: :绕x i轴（右手规则）由z i轴转向zi+1轴的偏角；ai-1：从第i-1坐标系原点到x i-1轴和z i的交点沿x i-1轴的偏移距离ai：从第i坐标系原点到x i轴和z i+1的交点沿x i轴的偏移

11、距离i i :绕z i轴由x i-1轴向x i轴的关节角；di：从x i-1 轴和z i轴的交点到第i坐标系的原点沿z i轴的距离 1000coscossincossinsinsinsincoscoscossin0sincos),(),(),(),(A11111111111111iiiiiiiiiiiiiiiiiiiiiiiiiiiddazRotdzTransaxTransxRot 固联坐标系前置固联坐标系前置（zi位于i关节轴上），变换公式 例例 建立右图所示机器人相邻坐标系间的转换矩阵解解：建立的坐标系如右图，这是二维坐标系(在三维空间中，各坐标系的z轴垂直于纸面)，其相邻坐标系的变换矩阵

12、为10010001001100001111111111111,11slcsclsclcssclxzTRA100222222222slcsclscA100100222222221111111121slcsclscslcsclscAAT100100)()(111221212111221212112121221212121112121221212121slslcsclclscslsccslccsssccsclsscclcsscsscc式中，c12=cos(1+ 2)， s12=sin(1+ 2)容易验证上式的正确性，即：末端位置为 ； T ,姿态为1+ 2 ；11122clcl12122slsl例例

13、 建立下图所示PUMA机器人相邻坐标系间的转换矩阵 。 第三章第三章 机器人运动学机器人运动学 PUMA 560是属于关节式机器人，6个关节都是转动关节。前3个关节确定手腕参考点的位置，后3个关节确定手腕的方位。PUMA机器人的连杆及关节参数表 第三章第三章 机器人运动学机器人运动学连杆ii i-1 ai-1 di11(90)00022(0)-900d233(-90)0a2044(0)-90a3d455(0)900066(0)-900010000100000332333csascA10000100000011111csscA10000010000222222csdscA 100000csd10

14、0a0scA444344410000001000055555csscA 100000cs010000scA66666式中 ：ci=cosi si=sini 例例 确定下图所示机器人的位置和姿态 解：解：用DH法建立坐标系转换矩阵，首先列出各连轩及关节参数，如下表所示。斯坦福机器人及其坐标系图斯坦福机器人的连杆及关节参数表 机器人运动学正问题是已知机器人各关节、各连杆参数及各关节变量，求机器人手端坐标在基础坐标中的位置和姿态。 3.2 3.2 机器人运动学正问题机器人运动学正问题10000010000011111csscA10000100000222222dcsscA10001000010000

15、133dA10000010000044444csscA10000010000055555csscA10000100000066666csscA1000cossin0sincossincoscossincossinsinsincoscos1iiiiiiiiiiiiiiiiiiidaaA将表中的参数分别代入i坐标系和il坐标系的齐次变换矩阵i-1Ai可得如下变换矩阵：由手端坐标逐一向基础坐标变换，其过程如下1000010000006666665csscAT100000006656565565656564cscsscssscccAAT100000356565546465464654546465464

16、654633654362dcsscsssccscsscccssccssccsscccTAAAAAT10000035656554646546465454646546465464465463dcsscsssccscsscccssccssccsscccTAAAAT1000)()()()(2546465264654325254265264654265264654232525426526465426526465426226543261dcsccsssscccsdcccscsscccscccscscsscccsdscssccssscscccccssssccccTAAAAAAT100061165432160

17、zzzzyyyyxxxxpaonpaonpaonTAAAAAAATnx=c1c2(c4c5c6-s4s6)-s2s5c6-s1(s4c5c6+c4s6)ny=s1c2(c4c5c6-s4s6)-s2s5c6+c1(s4c5c6+c4s6)nz=-s2(c4c5c6-s4s6)-c2s5c6ox=c1-c2(c4c5s6+s4c6)+s2s5s6-s1(-s4c5s6+c4c6)oy=c1-c2(c4c5s6+s4c6)+s2s5s6-s1(-s4c5s6+c4c6)oz=s2(c4c5s6+s4c6)+c2s5s6ax=c1(c2c4s5+s2c5)-s1s4s5ay=s1(c2c4s5+s2

18、c5)+c1s4s5az=-s2c4s5+c2c5px=-c1s2d3-s1d2py=s1s2d3+c1d2pz=c2d3 3.2 3.2 机器人运动学正问题机器人运动学正问题 机器人运动学逆问题，是已知满足某工作要求时末端执行器的位置和姿态，以及各连杆的结构参数，求关节变量。对于通用机器人，求解各关节相应位置的工作由机器人系统程序完成。 目前，已经能够对一般结构的六自由度串联机器人进行逆运动学求解。但是，要获得显式解，只有满足下列两个充分条件之一： (1)3个相邻关节轴交于一点； (2)3个相邻关节轴平行。 3.3 3.3机器人运动学逆问题机器人运动学逆问题1000611654321660z

19、zzzyyyyxxxxpaonpaonpaonTAAAAAAATTnx=c1c2(c4c5c6-s4s6)-s2s5c6-s1(s4c5c6+c4s6)ny=s1c2(c4c5c6-s4s6)-s2s5c6+c1(s4c5c6+c4s6)nz=-s2(c4c5c6-s4s6)-c2s5c6ox=c1-c2(c4c5s6+s4c6)+s2s5s6-s1(-s4c5s6+c4c6)oy=c1-c2(c4c5s6+s4c6)+s2s5s6-s1(-s4c5s6+c4c6)oz=s2(c4c5s6+s4c6)+c2s5s6 ax=c1(c2c4s5+s2c5)-s1s4s5ay=s1(c2c4s5+s

20、2c5)+c1s4s5az=-s2c4s5+c2c5px=-c1s2d3-s1d2py=s1s2d3+c1d2pz=c2d3 （1 1） 例例 已知上图所示机器人位置和姿态，即已知式矩阵中各元素的值，试确定机器人各关节变量。 解：解：用 左乘式（1）得11A6165432611TAAAAATA（2 2）方程式（2）的左端为10001000000100001111611zzzzyyyyxxxxpaonpaonpaoncsscTA1000)()()()()()()()()()()()(131313131212121211111111611pfafofnfpfafofnfpfafofnfTA（3 3

21、）将它表示为其中paoniicisifiifisicifyxzyx,)()()(11131211111000)()()()(2546465264654325254265264654265264654232525426526465426526465426543261dcsccsssscccsdcccscsscccscccscscsscccsdscssccssscscccccssssccccAAAAAT（4 4）（4）中3行4列元素为常数，利用式（3）对应元素的相等关系可得211213)(dicisdpfyx 即为了解此类方程，作如下三角代换sin,cosrprpyx式中xyyxpppprarct

22、an,22（5 5）将（5）代入（4）得rdrd/)sin(/sincoscossin21211即简化为由于 0d2/r1，说明角度（-1）在0-范围内： 0 （-1） 22121)/(1)(sin1)cos(rd22221arctandrd22221arctanarctandrdppxy 求出求出 1 1 ：式中，正负号对应于1的两个可能解。 根据机器人运动连续性及回避障碍的需要，确定一个1 ，从而式(2)左边已知。由式(3)的1行4列及2行4列和式(2)对应元素相等，列出zyxpdcpspcds321132由于d30，可可求出求出 2 2 zyxppspc112arctanzyxpcpsp

23、csd21123)(可可求出求出d d3 3 解：解：用 依次左乘式（2）得以下4个方程式：15141312,AAAA66561112131415656461112131465463611121365436261112ATTAATTAAATTAAAATTAAAAAAAAAAAAAA（6 6）（9 9）（8 8）（7 7）计算式(8)得100000001000)()()()()()()()()()()()(665656556565434343434242424241414141cscsscssscccpfafofnfpfafofnfpfafofnf（1010）paoniiciscisisiccs

24、ificisicsificissisisicccifyxzyxzyxyxzyx,)()()()()()()()(11421124432112421142112441式中 由式(10)第3行3列为0，可得：0)(43af即0)()(11421124yxzyxacascasasaccs可可求出求出180)(arctan442112114zyxyxasasaccacas 由式(10)第1行3列和第2行3列，可得：zyxyxzyxacasacscacassasasacccs21125114211245)()()(acasacsacassasasacccyxyxzyx2112114211245)()()

25、(arctan可可求出求出 5 5 )0()0(55 由式(9) 可得：100001000000100001)()(00)()(00)()(6666535352525151csscofnfofnfofnf 类似有：)()()()()(114211245211251142112456yxzyxzyxyxzyxocoscososoccscocosocssocossososoccccs可可求出求出 6 6 656arctanss 利用雅可比矩阵可以建立起机器人手端在基础坐标中的速度与各关节速度间的关系，以及手部与外界接触力与对应各关节力间的关系，因此机器人雅可比矩阵在机器人技术中占有重要地位。3.4

26、.1 3.4.1 雅可比矩阵的定义雅可比矩阵的定义对于一个n自由度机器人，其关节变量向量可写为 3.4 3.4机器人的雅可比矩阵机器人的雅可比矩阵Tnqqq,.,21Q设机器人手部在基础坐标中的位置和姿态为P P，则654321ppppppzyxTezeyexeeeP其中前3个元素表示位置，后3个元素表示姿态。它们都是n个关节变量的函数，也可写为：Tnqqq,.,21P为了求手部在基础坐标中的速度，可对上式求导tQQdtd.P(3.4-1)(3.4-2)(3.4-3) 3.4 3.4机器人的雅可比矩阵机器人的雅可比矩阵简写成QPJ(3.4-4)式(3.4-3)或式(3.4-4)表示手部在基础坐

27、标中的速度 与关节速度 间的关系，联结它们的纽带为矩阵J，称其为雅可比矩阵，它的展开式为PQnnqpqpqpqp66616111QJ(3.4-5)式(3.4-5)为机器人运动学正问题，即已知各关节速度求手端速度。3.4.2 3.4.2 雅可比矩阵的矢量积求法雅可比矩阵的矢量积求法 手部速度 的前3个元素表示手的线速度，后3个元素表示手的角速度，所以将 写成分块形式PP16eevP(3.4-6)其中J JLi和J JAi分别表示第i个关节变量引起的三维线速度系数三维线速度系数和三维角速三维角速度系数度系数。只要求J JLi和J JAi(il，2，n)，即可确定雅可比矩阵J。AnAALnLLeeJ

28、JJJJJvP2121niiAiniiLinAnAAnLnLLqqqqqqqq1122112211JJJJJJJJ3.4.2 3.4.2 雅可比矩阵的矢量积求法雅可比矩阵的矢量积求法3.4.2.1 J JLi的求法(1)第i个关节为移动关节时，qidi， ，如左图所示iidq 设某时刻仅此关节运动，其余的关节静止不动，由式（3.4-6)可得（3.4-7) 设b bi-1i-1为为z zi-1i-1轴上的单位矢量轴上的单位矢量，利用它可将局部坐标下的平移速度di转换成基础坐标下的速度：iLieq J iied1 b（3.4-8)比较式(3.4-7)或式(3.4-8)，并注意到 ，可得 1dqi

29、1iLibJ仅平移关节产生的线速度（3.4-9)3.4.2 3.4.2 雅可比矩阵的矢量积求法雅可比矩阵的矢量积求法(2)第i个关节为转动关节时， ，如下图所示iiq 设某时刻仅此关节运动，其余的关节静止不动，利用b bi-1将zi-1轴上的角速度转化到基础坐标中iii1b仅旋转关节产生的线速度eiiiLiiLieeiierqqr, 1, 1JJ而左图中的矢量r ri-1,e起于oi-1，止于on，所以由i产生的线速度为：所以（3.4-10)（3.4-11)把式(3.4-10)代入式(3.4-11)得:ieiieiiiiLiq)()(, 11, 11rbrbJiiq因 ，所以 eiiLi, 1

30、1rbJ（3.4-12)3.4.2 3.4.2 雅可比矩阵的矢量积求法雅可比矩阵的矢量积求法3.4.2.2 J JAi的求法(1)第i个关节为移动关节时，qidi， ，由于移动关节的平移不对手部产生角速度，所以此时 JAi=0iidq (2)第i个关节为转动关节时， ，由式（3.4-10)得：iiqiiiAiiq1bJ1iAibJ所以此时（3.4-13)（3.4-14)综合式（3.4-9)、（3.4-12)、（3.4-13)、（3.4-14)得第i个关节为移动关节时第i个关节为转动关节时01iAiLibJJ1, 11ieiiAiLirbbJJ（3.4-15)（3.4-16)3.4.2 3.4.

31、2 雅可比矩阵的矢量积求法雅可比矩阵的矢量积求法3.4.2.3 求b bi-1i-1和r ri-1,e用b b表示zi-1轴上的单位向量100b把它转换在基础坐标系中，即为b)()()(b1122111 iiiqAqAqA向量间的关系图如左图所示，用O、Oi-1、On分别表示基础坐标系、i-l号坐标及手部坐标系的原点。用矢量表示在各自坐标系中的原点。x1, 11000ineiTOOOOrx把r ri-1,e用齐次坐标表示，令1, 1, 1eieirxxxx)()()()()()(11122112211, 11iinneiiqAqAqAqAqAqAr所以（3.4-17)（3.4-18)由式(3.

32、4-18)可方便地确定r ri-1,e3.4.2 3.4.2 雅可比矩阵的矢量积求法雅可比矩阵的矢量积求法1000100010001zyxtrandddT3.4.3.1 3.4.3.1 机器人的微分运动机器人的微分运动微分平移的齐次变换为：1000000)(cversffsfversffsfversffsfversffcversffsfversffsfversffsfversffcversffRotzzxzyzzxxyzyyzxxyxzzxyxx，f通用旋转变换矩阵TfTT)(),(dRotdddTransdzyx，)(),(IfTTdRotdddTransdzyx，3.4.3 3.4.3 雅

33、可比矩阵的微分变换求法雅可比矩阵的微分变换求法TfT)(),(IdRotdddTransdzyx，)(),(dRotdddTransdzyx，fTTT微分变化相对基系进行微分变化相对基系进行微分变化相对坐标系微分变化相对坐标系T进行进行其中cvers11000010101)(dfdfdfdfdfdfdRotxzxzyz，f0lim, 1coslim,sinlim000versd当微分运动相对于基系进行时为当微分运动相对于坐标系T进行时为If)(),(dRotdddTranszyx，If)(),(dRotdddTranszyxT，T当对基系进行微分变化时当对基系进行微分变化时TTd当对坐标系当对

34、坐标系TT进行微分变化时进行微分变化时TdTT3.4.3 3.4.3 雅可比矩阵的微分变换求法雅可比矩阵的微分变换求法1000000100010001000110000101011000100010001)(),(zxyyxzxyzzyxxyxzyzzyxzyxddfdfddfdfddfdfddddfdfdfdfdfdfddddRotdddTransIf，绕矢量 旋转等价于分别绕坐标系的三个轴 旋转fzyx,zyx,zzyyxxdfdfdf,即代入上式得：3.4.3 3.4.3 雅可比矩阵的微分变换求法雅可比矩阵的微分变换求法 1000000zxyyxzxyzddd 1000000zTxTyT

35、yTxTzxTyTzTTddd 可把微分平移和旋转变换看成是由微分平移矢量d d和微分旋转矢量 构成的，kjikdjdidzyxzyxd,它们分别为3.4.3 3.4.3 雅可比矩阵的微分变换求法雅可比矩阵的微分变换求法 用列矢量D D来包含微分平移矢量d d和微分旋转矢量 ，并称为刚体或坐标系的微分运动矢量zyxzyxdddDdDTTT或kdjdidzTyTxTTdzTyTxTzTyTxTTdddDdD或同理有：3.4.3 3.4.3 雅可比矩阵的微分变换求法雅可比矩阵的微分变换求法kjizTyTxTTTddTTTT, 要求得机械手的雅可比矩阵，就需要建立不同坐标系间的微分变化的变换关系变换

36、关系。当两坐标系等价时，有TTTTTT1变换后得0000)(0)(0)(01adapnoodopnandnpoaTTT由等式左边求出 得： T 1000000zTxTyTyTxTzxTyTzTTddd 由定义有：令上述两式各元素分别相等，可求出：3.4.3 3.4.3 雅可比矩阵的微分变换求法雅可比矩阵的微分变换求法aonadapodopndnpzTyTxTzTyTxTddd,)()()(paon,分别为微分坐标变换T的列矢量。微分运动矢量TD D和和D D的关系如下：zyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzTyTxTzTyTxTdddaaaooonnnaaaooo

37、nnnddd000000000)()()()()()()()()(apapapopopopnpnpnp3.4.3 3.4.3 雅可比矩阵的微分变换求法雅可比矩阵的微分变换求法3.4.3.2 3.4.3.2 雅可比矩阵的微分变换法雅可比矩阵的微分变换法3.4.3 3.4.3 雅可比矩阵的微分变换求法雅可比矩阵的微分变换求法zyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzTyTxTzTyTxTdddaaaooonnnaaaooonnnddd000000000)()()()()()()()()(apapapopopopnpnpnp 对于转动关节转动关节i，连杆i相对连杆i-1绕坐

38、标系i的zi轴作微分运动di，其微分运动矢量为：id100,000d代入下式，得手相应的微分运动矢量为：3.4.3 3.4.3 雅可比矩阵的微分变换求法雅可比矩阵的微分变换求法izzzzzzzTyTxTzTyTxTdaonddd)()()(apopnp 对于移动关节移动关节i，连杆i 沿zi轴相对连杆i-1作微分运动ddi，其微分运动矢量为：iidd100,100ddizzzzTyTxTzTyTxTdaondddd000手相应的微分运动矢量为：3.4.3 3.4.3 雅可比矩阵的微分变换求法雅可比矩阵的微分变换求法可得雅可比矩阵J的第i列如下：对于转动关节i对于移动关节izzzLiT)()()

39、(apopnpJzzzLiTaonJpaon,是iTn的四个列矢量。zzzAiTaonJ000AiTJxyyxzxyyxzxyyxzpapapopopnpna)popnp()()(其中3.4.3 3.4.3 雅可比矩阵的微分变换求法雅可比矩阵的微分变换求法 上述求雅可比矩阵TJ的方法是构造性的，只要知道各连杆变换T Ti，就可自动生成雅可比，而不必求解方程。步骤如下：计算各连杆变换T T1，T T2 ，,T Tn计算各连杆至末端连杆的变换T Tn，n-2T Tn= n-2T Tn-1 T Tn ，, I-1T Tn= T Ti iT Tn ， ，0T Tn=T T1 1T Tn.计算雅可比矩阵

40、TJ的各列元素，第i列TJ Ji由iT Tn决定。T T1T T2T T3T T4T T5T T65T T64T T63T T62T T61T T6TJ J6TJ J5TJ J4TJ J3TJ J2TJ J1TJ Ji由iT Tn之间的关系 3.4 3.4 机器人的雅可比矩阵机器人的雅可比矩阵例例3.4-13.4-1 建立右图的雅可比矩阵。0010012211122111221112211, 001clclslslslslclclkjieLrbJ解解00100122122122122, 112clslslclkjieLrbJ100,1001201bJbJAA,11000000122122111

41、22122112121clclclslslslAALLJJJJJ1112212211122122112121clclclslslslAALLJJJJJ可以简写为机械臂末端的速度机械臂末端的速度2121221122112122112211211221221112212211)()(11clclclslslslclclclslslslrJ 3.4.4 3.4.4 雅可比矩阵的逆雅可比矩阵的逆3.4.4 3.4.4 雅可比矩阵雅可比矩阵的逆的逆 对于在三维空间运动的n关节机器人，其雅可比矩阵的阶数为6n。当n6时，J是6 6方阵，可直接求其逆。当n6时， J不是方阵此时若用雅可比矩阵的逆就用其伪逆，用J+表示伪逆。1)(TTJJJJ（3.4-19)3.4