第三章-机器人运动学

第三章机器人运动学§3.1机器人运动方程的表示

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

T2=

A1A2同理，若A3表示第三个连杆相对于第二个连杆的位置和姿态，则有

T3=

A1A2A3称这些A矩阵的乘积为T矩阵，其前置上标若为0，则可省略。对于六连杆机械手，有下列T矩阵

T6=

A1A2A3A4A5A6机械手的运动方向原点由矢量p表示。接近矢量a：z轴设在手指接近物体的方向，称为接近矢量

方向矢量o：y轴设在两手指的连线方向，称为方位矢量

法线矢量n：x轴由右手系确定，即n=o

a

，称为法向矢量。手爪坐标系因此，变换T6具有下列元素。

六连杆机械手的T矩阵（T6）可由指定其16个元素的数值来决定。在这16个元素中，只有12个元素具有实际含义。

机械手由一串用转动或平移关节连接的刚体(杆件)组成。每一对关节–杆件构成一个自由度。杆件的编号由手臂的固定基座开始，固定基座可看成杆件0，第一个运动体是杆件1，依次类推，最后一个杆件与工具相连；关节1处于连接杆件1和基座之间，每个杆件至多与另外两个杆件相联，而不构成闭环。杆件i的长度ai，是杆件上两个关节轴线的最短距离；杆件i的扭转角αi

，是两个关节轴线的夹角。

3.1.1Denavt-Hartenberg(D-H)表示法AiAi+1任何杆件i都可以用两个尺度表征，如上图所示，AiAi-1两个杆件的相对位置由两个参数决定：两杆间的距离di：关节轴上两个法线的距离夹角θi：关节轴上两个法线的夹角

为描述相邻杆件间平移和转动的关系。Denavt和Hartenberg(1955)提出了一种为关节链中的每一杆件建立附体坐标系的矩阵方法。D-H方法是为每个关节处的杆件坐标系建立44齐次变换矩阵，表示它与前一杆件坐标系的关系。这样逐次变换，用“手部坐标”表示的末端执行器可被变换并用机座坐标表示。坐标系的建立有两种方式：固联坐标系后置固联坐标系前置3.1.1Denavt-Hartenberg(D-H)表示法

n关节机器人需建立n+1个坐标系，其中参考(机座)坐标系为O0x0y0z0，机械手末端的坐标系为Onxnynzn，第i关节上的坐标系为Oi-1xi-1yi-1zi-1。坐标系Si置于连杆Li的远离基座的关节上，故称固联坐标系后置。确定和建立每个坐标系应根据下面3条规则:

①zi-1轴沿着第i关节的运动轴；②xi轴垂直于zi-1轴和zi轴并指向离开zi-1轴的方向③yi轴按右手坐标系的要求建立。

按照这些规则，第0号坐标系在机座上的位置和方向可任选，只要z0轴沿着第i关节运动轴。第n坐标系可放在手的任何部位、只要xn轴与zn-1轴垂直。固联坐标系后置转动关节连杆四参数示意图9机器人机械手上坐标系的配置取决于机械手连杆连接的类型。有两种连接——转动关节和棱柱联轴节。现在来考虑棱柱联轴节（平动关节）的情况。下图示出其特征参数。棱柱关节连杆四参数示意图3.1.2几何参数定义θi：绕zi-1轴（右手规则）由xi-1轴向xi轴的关节角；di：从第i—1坐标系的原点到zi-1轴和xi轴的交点沿zi-1轴的距离；ai：从zi-1和xi轴的交点到第i

根据上述对杆件参数及坐标系的定义，描述串联机器人相邻坐标系之间的关节关系可归结如下4个参数：坐标系原点沿xi轴的偏移距离(是zi-1轴和zi两轴间的最小距离）i

：绕xi轴（右手规则）由zi-1轴转向zi轴的偏角。

对于转动关节，di、ai、αi是关节参数，θi是关节变量。移动关节的关节参数是θi、ai、αi，di是关节变量。3.1.2几何参数定义

将第i个坐标系表示的点ri在i-1坐标系表示，需建立i坐标系和i—1坐标系的齐次变换矩阵，需经过以下交换：(1)将坐标系Oi-1xi-1yi-1zi-1绕zi-1轴转θi角，使xi-1轴与xi平行并指向同一方向；(2)将坐标系Oi-1xi-1yi-1zi-1沿zi-1轴平移距离di

，使xi-1轴与Oixiyizi的xi轴重合；(3)将坐标系Oi-1xi-1yi-1zi-1沿xi轴平移距离ai

，使两坐标系的原点重合；(4)将坐标系Oi-1xi-1yi-1zi-1绕xi轴转αi角，使两坐标系完全重合。3.1.3建立i坐标系和i-1坐标系的齐次变换矩阵

i坐标系和i—l坐标系的齐次变换矩阵i-1Ai可以根据矩阵的合成规则得到，i-1Ai称为相邻坐标系i和i—1的D-H变换矩阵。即固联坐标系后置（zi位于i+1关节轴上），变换公式对于在第i坐标系中的点ri在第i—1坐标系中表示为:

确定第i坐标系相对于机座坐标系的位置的齐次变换矩阵i-1Ti是各齐次变换矩阵Ai的连乘积，可表示成固联坐标系前置

连杆Li的固联坐标系Si的zi轴置于i关节的旋转（或移动）轴上，即坐标系Si置于连杆Li的靠近基座的关节上，故称固联坐标系前置。zi-1与zi的公垂线为xi-1,zi与zi+1的公垂线为xi轴,

αi-1:为zi-1与zi的交错角；αi:绕xi轴（右手规则）由zi轴转向zi+1轴的偏角；ai-1：从第i-1坐标系原点到xi-1轴和zi的交点沿xi-1轴的偏移距离ai：从第i坐标系原点到xi轴和zi+1的交点沿xi轴的偏移距离θi

:绕zi轴由xi-1轴向xi轴的关节角；di：从xi-1

轴和zi轴的交点到第i坐标系的原点沿zi轴的距离固联坐标系前置（zi位于i关节轴上），变换公式

例建立右图所示机器人相邻坐标系间的转换矩阵解：建立的坐标系如右图，这是二维坐标系(在三维空间中，各坐标系的z轴垂直于纸面)，其相邻坐标系的变换矩阵为式中，c12=cos(1+2)，s12=sin(1+2)容易验证上式的正确性，即：末端位置为[；]T,姿态为1+2

；例建立下图所示PUMA机器人相邻坐标系间的转换矩阵。

第三章机器人运动学PUMA560是属于关节式机器人，6个关节都是转动关节。前3个关节确定手腕参考点的位置，后3个关节确定手腕的方位。PUMA机器人的连杆及关节参数表

第三章机器人运动学连杆iθi

αi-1

ai-1

di1θ1(90°)0°002θ2(0°)-90°0d23θ3(-90°)0°a204θ4(0°)-90°a3d45θ5(0°)90°006θ6(0°)-90°00式中：ci=cosisi=sini[例]确定下图所示机器人的位置和姿态解：用D—H法建立坐标系转换矩阵，首先列出各连轩及关节参数，如下表所示。斯坦福机器人及其坐标系图斯坦福机器人的连杆及关节参数表

机器人运动学正问题是已知机器人各关节、各连杆参数及各关节变量，求机器人手端坐标在基础坐标中的位置和姿态。§

3.2机器人运动学正问题将表中的参数分别代入i坐标系和i—l坐标系的齐次变换矩阵i-1Ai可得如下变换矩阵：由手端坐标逐一向基础坐标变换，其过程如下nx=c1[c2(c4c5c6-s4s6)-s2s5c6]-s1(s4c5c6+c4s6)ny=s1[c2(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+s2c5)+c1s4s5az=-s2c4s5+c2c5px=-c1s2d3-s1d2py=s1s2d3+c1d2pz=c2d3§

3.2机器人运动学正问题

机器人运动学逆问题，是已知满足某工作要求时末端执行器的位置和姿态，以及各连杆的结构参数，求关节变量。对于通用机器人，求解各关节相应位置的工作由机器人系统程序完成。目前，已经能够对一般结构的六自由度串联机器人进行逆运动学求解。但是，要获得显式解，只有满足下列两个充分条件之一：

(1)3个相邻关节轴交于一点；

(2)3个相邻关节轴平行。§3.3机器人运动学逆问题nx=c1[c2(c4c5c6-s4s6)-s2s5c6]-s1(s4c5c6+c4s6)ny=s1[c2(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+s2c5)+c1s4s5az=-s2c4s5+c2c5px=-c1s2d3-s1d2py=s1s2d3+c1d2pz=c2d3

（1）

[例]

已知上图所示机器人位置和姿态，即已知式①矩阵中各元素的值，试确定机器人各关节变量。

解：用左乘式（1）得（2）方程式（2）的左端为（3）将它表示为其中（4）（4）中3行4列元素为常数，利用式（3）对应元素的相等关系可得即为了解此类方程，作如下三角代换式中（5）将（5）代入（4）得即简化为由于0d2/r1，说明角度（-1）在0-范围内：0（-1）求出1

：式中，正负号对应于1的两个可能解。

根据机器人运动连续性及回避障碍的需要，确定一个1

，从而式(2)左边已知。由式(3)的1行4列及2行4列和式(2)对应元素相等，列出由于d30，可求出2

可求出d3

解：用依次左乘式（2）得以下4个方程式：（6）（9）（8）（7）计算式(8)得（10）式中

由式(10)第3行3列为0，可得：即可求出

由式(10)第1行3列和第2行3列，可得：可求出5

由式(9)可得：

类似有：可求出6

利用雅可比矩阵可以建立起机器人手端在基础坐标中的速度与各关节速度间的关系，以及手部与外界接触力与对应各关节力间的关系，因此机器人雅可比矩阵在机器人技术中占有重要地位。3.4.1雅可比矩阵的定义对于一个n自由度机器人，其关节变量向量可写为§3.4机器人的雅可比矩阵设机器人手部在基础坐标中的位置和姿态为P，则其中前3个元素表示位置，后3个元素表示姿态。它们都是n个关节变量的函数，也可写为：为了求手部在基础坐标中的速度，可对上式求导(3.4-1)(3.4-2)(3.4-3)§3.4机器人的雅可比矩阵简写成(3.4-4)式(3.4-3)或式(3.4-4)表示手部在基础坐标中的速度与关节速度间的关系，联结它们的纽带为矩阵J，称其为雅可比矩阵，它的展开式为(3.4-5)式(3.4-5)为机器人运动学正问题，即已知各关节速度求手端速度。3.4.2雅可比矩阵的矢量积求法

手部速度的前3个元素表示手的线速度，后3个元素表示手的角速度，所以将写成分块形式(3.4-6)其中JLi和JAi分别表示第i个关节变量引起的三维线速度系数和三维角速度系数。只要求JLi和JAi(i＝l，2，…，n)，即可确定雅可比矩阵J。3.4.2雅可比矩阵的矢量积求法3.4.2.1JLi的求法(1)第i个关节为移动关节时，qi＝di，，如左图所示

设某时刻仅此关节运动，其余的关节静止不动，由式（3.4-6)可得（3.4-7)

设bi-1为zi-1轴上的单位矢量，利用它可将局部坐标下的平移速度di转换成基础坐标下的速度：（3.4-8)比较式(3.4-7)或式(3.4-8)，并注意到，可得仅平移关节产生的线速度（3.4-9)3.4.2雅可比矩阵的矢量积求法(2)第i个关节为转动关节时，，如下图所示

设某时刻仅此关节运动，其余的关节静止不动，利用bi-1将zi-1轴上的角速度转化到基础坐标中仅旋转关节产生的线速度而左图中的矢量ri-1,e起于oi-1，止于on，所以由ωi产生的线速度为：所以（3.4-10)（3.4-11)把式(3.4-10)代入式(3.4-11)得:因，所以（3.4-12)3.4.2雅可比矩阵的矢量积求法3.4.2.2JAi的求法(1)第i个关节为移动关节时，qi＝di，，由于移动关节的平移不对手部产生角速度，所以此时

JAi=0(2)第i个关节为转动关节时，，由式（3.4-10)得：所以此时（3.4-13)（3.4-14)综合式（3.4-9)、（3.4-12)、（3.4-13)、（3.4-14)得第i个关节为移动关节时第i个关节为转动关节时（3.4-15)（3.4-16)3.4.2雅可比矩阵的矢量积求法3.4.2.3求bi-1和ri-1,e用b表示zi-1轴上的单位向量把它转换在基础坐标系中，即为向量间的关系图如左图所示，用O、Oi-1、On分别表示基础坐标系、i-l号坐标及手部坐标系的原点。用矢量表示在各自坐标系中的原点。把ri-1,e用齐次坐标表示，令所以（3.4-17)（3.4-18)由式(3.4-18)可方便地确定ri-1,e3.4.2雅可比矩阵的矢量积求法3.4.3.1机器人的微分运动微分平移的齐次变换为：通用旋转变换矩阵3.4.3雅可比矩阵的微分变换求法微分变化相对基系进行微分变化相对坐标系T进行其中当微分运动相对于基系进行时为当微分运动相对于坐标系{T}进行时为当对基系进行微分变化时当对坐标系{T}进行微分变化时3.4.3雅可比矩阵的微分变换求法绕矢量

旋转等价于分别绕坐标系的三个轴旋转即代入上式得：3.4.3雅可比矩阵的微分变换求法可把微分平移和旋转变换看成是由微分平移矢量d和微分旋转矢量构成的，它们分别为3.4.3雅可比矩阵的微分变换求法

用列矢量D来包含微分平移矢量d和微分旋转矢量，并称为刚体或坐标系的微分运动矢量或或同理有：3.4.3雅可比矩阵的微分变换求法

要求得机械手的雅可比矩阵，就需要建立不同坐标系间的微分变化的变换关系。当两坐标系等价时，有变换后得由等式左边求出得：由定义有：令上述两式各元素分别相等，可求出：3.4.3雅可比矩阵的微分变换求法分别为微分坐标变换{T}的列矢量。微分运动矢量TD和D的关系如下：3.4.3雅可比矩阵的微分变换求法3.4.3.2雅可比矩阵的微分变换法3.4.3雅可比矩阵的微分变换求法

对于转动关节i，连杆i相对连杆i-1绕坐标系{i}的zi轴作微分运动dθi，其微分运动矢量为：代入下式，得手相应的微分运动矢量为：3.4.3雅可比矩阵的微分变换求法

对于移动关节i，连杆i沿zi轴相对连杆i-1作微分运动ddi，其微分运动矢量为：手相应的微分运动矢量为：3.4.3雅可比矩阵的微分变换求法可得雅可比矩阵J的第i列如下：对于转动关节i对于移动关节i是iTn的四个列矢量。其中3.4.3雅可比矩阵的微分变换求法

上述求雅可比矩阵TJ的方法是构造性的，只要知道各连杆变换Ti，就可自动生成雅可比，而不必求解方程。步骤如下：①计算各连杆变换T1，T2，…,Tn②计算各连杆至末端连杆的变换Tn，n-2Tn=

n-2Tn-1Tn，…,I-1Tn=

TiiTn，

…，0Tn=T11Tn.③计算雅可比矩阵TJ的各列元素，第i列TJi由iTn决定。T1T2T3T4T5T65T64T63T62T61T6TJ6TJ5TJ4TJ3TJ2TJ1TJi由iTn之间的关系§3.4机器人的雅可比矩阵[例3.4-1]

建立右图的雅可比矩阵。解可以简写为机械臂末端的速度§3.4.4雅可比矩阵的逆3.4.4雅可比矩阵的逆

对于在三维空间运动的n关节机器人，其雅可比矩阵的阶数为6n。当n＝6时，J是66方阵，可直接求其逆。当n6时，J不是方阵．此时若用雅可比矩阵的逆就用其伪逆，用J+表示伪逆。（3.4-19)3.4.5雅可比矩阵的应用3.4.5.1分离速度控制由可得（3.4-20)

由式(3.4-20)可见，当已知手端速度向量，可通过左乘雅可比逆矩阵计算出机器人的关节速度向量，所以式(3.4-20)为运动学逆问题的速度关系式，是对机器人进行速度控制的基本关系式。采用计算机控制时，把速度表示位置增量的形式，故将式(3.4-20)写为（3.4