工业机器人课件第三章机器人运动学

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

2、姿态，则有 T T3= A A1 A A2 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

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

4、3.1.1 3.1.1 Denavt-Hartenberg(D-H)表示法Ai iiiaAi+1i+1任何杆件i都可以用两个尺度表征，如上图所示，Ai iAi-1i-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轴按右手坐标系的要求建立。

6、 按照这些规则，第0号坐标系在机座上的位置和方向可任选，只要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

7、 i-1和x i轴的交点到第i 根据上述对杆件参数及坐标系的定义，描述串联机器人相邻坐标系之间的关节关系可归结如下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-

8、1zi-1绕zi-1轴转i角，使xi-1轴与xi平行并指向同一方向；(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-

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

10、T1654321010100000iiiiiiPRpaon 确定第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：

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

12、维空间中，各坐标系的z轴垂直于纸面)，其相邻坐标系的变换矩阵为10010001001100001111111111111,11slcsclsclcssclxzTRA100222222222slcsclscA100100222222221111111121slcsclscslcsclscAAT100100)()(111221212111221212112121221212121112121221212121slslcsclclscslsccslccsssccsclsscclcsscsscc式中，c12=cos(1+ 2)， s12=sin(1+ 2)容易验证上式的正确性，即：末端位置为 ； T

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

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

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

16、000356565546465464654546465464654633654362dcsscsssccscsscccssccssccsscccTAAAAAT10000035656554646546465454646546465464465463dcsscsssccscsscccssccssccsscccTAAAAT1000)()()()(2546465264654325254265264654265264654232525426526465426526465426226543261dcsccsssscccsdcccscsscccscccscscsscccsdscssccssscscccccs

17、sssccccTAAAAAAT100061165432160zzzzyyyyxxxxpaonpaonpaonTAAAAAAATnx=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(c2c4

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

19、逆问题机器人运动学逆问题1000611654321660zzzzyyyyxxxxpaonpaonpaonTAAAAAAATTnx=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(c2c4

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

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

22、换sin,cosrprpyx式中xyyxpppprarctan,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 zyxppspc112arctanzyxpcpspcsd21123)(可可求出求出d d3 3 解：解：用 依次左乘式（2）得以下4个方程式：15141312,AAAA66561112131415656461112131465463611121365436261112ATTAATTAAATT