机器人学-第2章 机器人运动学(4)

1、机器人运动学机器人运动学机器人运动学的主要内容1.位置与姿态描述位置与姿态描述2.坐标变换坐标变换3.连杆变换矩阵连杆变换矩阵4.机器人正向运动学机器人正向运动学5.机器人逆向运动学机器人逆向运动学机器人运动学前言p机器人操作涉及到各物体之间的关系和各物体与机械臂之间的关机器人操作涉及到各物体之间的关系和各物体与机械臂之间的关系。这一章将给出描述这些关系必须的表达方法。类似这种表示系。这一章将给出描述这些关系必须的表达方法。类似这种表示方法在计算机图形学中已经解决。在计算机图形学和计算机视觉方法在计算机图形学中已经解决。在计算机图形学和计算机视觉中，物体之间的关系是用齐次坐标变换来描述的。中，

2、物体之间的关系是用齐次坐标变换来描述的。p本课程将采用齐次坐标变换来描述机械手各关节坐标之间、各物本课程将采用齐次坐标变换来描述机械手各关节坐标之间、各物体之间以及各物体与机器人体之间以及各物体与机器人( (机械臂机械臂) )之间的关系。之间的关系。 n运动学研究的问题：机器人运机器人运动学正问题是已知机器人动学正问题是已知机器人各关节、各连杆参数及各各关节、各连杆参数及各关节变量，求机器人手端关节变量，求机器人手端坐标在基础坐标中的位置坐标在基础坐标中的位置和姿态。和姿态。机器人运动学前言机器人运机器人运动学逆问题，是已知满足动学逆问题，是已知满足某工作要求时末端执行器某工作要求时末端执行器

3、的位置和姿态，以及各连的位置和姿态，以及各连杆的结构参数，求关节变杆的结构参数，求关节变量。量。Where is my hand?How to move my hand?机器人运动学前言机器人关机器人关节坐标的微小运动与机器人末节坐标的微小运动与机器人末端的位置和姿态的变化之间的端的位置和姿态的变化之间的变换关系。变换关系。通常采通常采用微分运动原理对机器人的各用微分运动原理对机器人的各个关节的运动进行控制。个关节的运动进行控制。How to solve the magic cube ？ 1. 位置描述1.1笛卡尔坐标系：笛卡尔坐标系：在选定的直角坐标系在选定的直角坐标系A中，中，空间任一点空

4、间任一点P的位置可用的位置可用位置矢量位置矢量 表示：表示：利用利用31矩阵表示：矩阵表示：APxAyzPPPP(,)xyzP P P POXYZkpjpippzyxA图图 1.11.1笛卡尔坐标系笛卡尔坐标系1. 位置描述1.2 三维空间点三维空间点P的齐次坐标：的齐次坐标：加入一个比例因子加入一个比例因子w, 位置向量可以写位置向量可以写为：为：假设假设ijk是直角坐标系中是直角坐标系中XYZ坐标坐标轴的单位向量，则轴的单位向量，则XYZ轴可表示轴可表示为为1xyzPPPPxyzwPawPbwPcww100001000010TTTXYZ1. 位置描述1.3 坐标系的表示：坐标系的表示：I.

5、在固定参考坐标系原点的表示：在固定参考坐标系原点的表示：用三个相互垂直的单位向量来用三个相互垂直的单位向量来表示一个中心位于参考坐标系原点的坐标系，分别为表示一个中心位于参考坐标系原点的坐标系，分别为n,o,a，依次，依次表示法线表示法线(normal)，指向，指向(oritentation)，和接近，和接近(approach)。这样，。这样，坐标系就可以由三个向量以矩阵的形式表示为坐标系就可以由三个向量以矩阵的形式表示为zyxzyxzyxooonnnF1. 位置描述II.坐标系不在固定参考坐标系的原点坐标系不在固定参考坐标系的原点:可以在该坐标系可以在该坐标系的原点与参考坐标系原点之间做一个

6、向量，而这个向量的原点与参考坐标系原点之间做一个向量，而这个向量由上节中提到的参考坐标系的三个坐标向量表示。这样，由上节中提到的参考坐标系的三个坐标向量表示。这样，这个坐标系就可以由三个表示方向的单位向量以及第四这个坐标系就可以由三个表示方向的单位向量以及第四个位置向量来表示。个位置向量来表示。1000zyxzyxzyxzyxpppooonnnF1. 位置描述示例：示例：坐标系位于参考坐标系的坐标系位于参考坐标系的3 3，5 5，7 7的位置。的位置。n n轴与轴与x x轴平行，轴平行，o o轴相对于轴相对于y y轴角度轴角度4545，a a轴相对于轴相对于z z轴角度轴角度45 45 ）F=

7、1 0 0 30 0.707 -0.707 50 0.707 0.707 70 0 0 12. 姿态描述姿态描述：刚体的空间表示。姿态描述：刚体的空间表示。一个刚体在空间有几个自由度？一个刚体在空间有几个自由度？通常的做法是：定义两个坐标系通常的做法是：定义两个坐标系空空间固定坐标系和刚体固定坐标系。间固定坐标系和刚体固定坐标系。常用的姿态描述：常用的姿态描述：旋转矩阵的姿态描述（笛卡尔坐标系旋转矩阵的姿态描述（笛卡尔坐标系下），下），欧拉（欧拉（Euler）角的姿态描述，）角的姿态描述，利用横滚（利用横滚（R：Roll）、俯仰（）、俯仰（P：pitch）、偏转（）、偏转（Y：yaw）角）角（

8、RPY角）的姿态描述等。角）的姿态描述等。OX/uY/vZ/wrqp图图2-1 固定坐标系下六个自由度上的运动分量固定坐标系下六个自由度上的运动分量G2.1 姿态描述 表示与表示与B的坐标轴的坐标轴平行的三个单位矢量在坐标系平行的三个单位矢量在坐标系A中的描述。中的描述。 表示刚体表示刚体B相对于坐标系相对于坐标系A的姿态的姿态。 刚体刚体B相对于坐标系相对于坐标系A的的姿态的旋转矩阵。姿态的旋转矩阵。BARzyxzyxzyxBABABABABAooonnnonRrrrrrrrrrZyxR333231232221131211,BABABAZyx,krjrirzkrjrirykrjrirxBAB

9、ABA3323133222123121112.1 姿态描述n旋转矩阵的性质：旋转矩阵的性质：q单位向量，相互垂直，单位向量，相互垂直，正交。正交。q正交矩阵：正交矩阵：zyxzyxzyxBAooonnnonR01BABABABABABABABABABABABAxzzyyxzzyyxx1,1TBATBABARRR2. 2位姿描述n位置与姿态简称位姿。刚体位置与姿态简称位姿。刚体B在参考坐标系在参考坐标系A中的位中的位姿利用坐标系姿利用坐标系B描述。描述。q齐次变换矩阵形式齐次变换矩阵形式,BABApRB1000zyxzyxzyxzyxBApppooonnnT3.坐标变换3.13.1平移变换平移变

10、换（Translation transformationTranslation transformation）：）：坐标系坐标系BB与与A A的方向向量平行，原点不同。的方向向量平行，原点不同。XA其中其中px , py和和pz是纯平移向量是纯平移向量APB相对相对于参考坐标系于参考坐标系x , y和和z轴的三个分量。轴的三个分量。n矩阵的前三列表示没有旋转运动矩阵的前三列表示没有旋转运动（等同于单位阵），而最后一列表示（等同于单位阵），而最后一列表示平移运动。平移运动。BABAzBzyByxBxzByBxBBAzyxppppppppppppTpTppppT1,1000100010001YAZ

11、AOAYBXBZBOBAPBBP3.坐标变换3.23.2旋转坐标变换旋转坐标变换（Rotation transformationRotation transformation）q假设坐标系（假设坐标系（n ,o ,an ,o ,a）位于参考坐标系（）位于参考坐标系（x ,y ,zx ,y ,z）的原点，坐）的原点，坐标系（标系（n ,o ,an ,o ,a）绕参考坐标系的）绕参考坐标系的x x轴旋转一个角度轴旋转一个角度，再假设旋，再假设旋转坐标系（转坐标系（n ,o ,an ,o ,a）上有一点）上有一点P P相对于参考坐标系的坐标为相对于参考坐标系的坐标为Px,PyPx,Py和和PzPz，

12、相对于运动坐标系的坐标为，相对于运动坐标系的坐标为PnPn, Po, Po和和PaPa。当坐标系。当坐标系绕绕x x轴旋转时，坐标系上的点轴旋转时，坐标系上的点P P也随坐标系一起旋转也随坐标系一起旋转3.坐标变换aonzyxppppppcossin0sincos0001n旋转后，该点坐标旋转后，该点坐标PnPn, Po, Po和和PaPa在旋转坐标系中保持不在旋转坐标系中保持不变，但在参考坐标系中：变，但在参考坐标系中：pRpPPPPPPxRotPPPBABAaonaonzyx110000cossin00sincos000011,1旋转变换矩阵旋转变换矩阵3.坐标变换1000010000co

13、ssin00sincos,10000cos0sin00100sin0cos,10000cossin00sincos00001,zRotyRotxRotn绕绕 x, y, z x, y, z 轴分别旋转轴分别旋转角的相应齐次变换是角的相应齐次变换是: :n假设坐标系（假设坐标系（n ,o ,an ,o ,a）和）和参考坐标系（参考坐标系（x ,y ,zx ,y ,z）的原）的原点不重合。点不重合。n用位置矢量表示用位置矢量表示B B的原的原点相对点相对A A的位置，用旋转的位置，用旋转矩阵表示矩阵表示B B相对与相对与A A的方位。的方位。BABABAppRp3.坐标变换n任何变换都可以分解为按

14、一定顺序的一组平移和旋转变换。任何变换都可以分解为按一定顺序的一组平移和旋转变换。n示例示例：假设坐标系（假设坐标系（n ,o ,an ,o ,a）位于参考坐标系）位于参考坐标系（x ,y ,zx ,y ,z）的原点，坐标系（）的原点，坐标系（n,o,an,o,a）上的点）上的点P P（7 7，3 3，2 2）经历如下变换，求出变换后该点相对于参考坐标系的坐标经历如下变换，求出变换后该点相对于参考坐标系的坐标。(1)(1)绕绕z z轴旋转轴旋转9090度；度；(2)(2)接着绕接着绕y y轴旋转轴旋转9090度；度；(3)(3)接着再平移接着再平移44，-3-3，77。Pxyz=Trans(4

15、,-3,7)Rot(y,90)Rot(z,90)Pnoa3.坐标变换Pxyz=6，4，10，1Tp示例示例例题：nB和和A位姿重合。现在位姿重合。现在将将B绕绕AzA轴转轴转30度，度，再沿再沿A的的xA轴移动轴移动12单位，单位，再沿再沿A的的yA轴移动轴移动6单位。单位。假设点假设点p在在B中位置为中位置为5,9,0T，求点，求点p在在A中位置。中位置。qApB=12, 6, 0,1TqAp=11.1, 13.6, 0,1T1000010000cossin00sincos,zRot3.坐标变换3.33.3逆变换（逆变换（Inverse transformation)Inverse tran

16、sformation)q所谓逆变换就是将被变换的坐标系返回到原来的坐标系。所谓逆变换就是将被变换的坐标系返回到原来的坐标系。q变换矩阵的一般表达形式：变换矩阵的一般表达形式：式中式中 n, o, a n, o, a 是旋转变换列向量，是旋转变换列向量，p p 是平移向量，其逆是是平移向量，其逆是1000zzzzyyyyxxxxpaonpaonpaonT10001apaaaopooonpnnnTzyxzyxzyx3.坐标变换3.3联体坐标变换联体坐标变换q对于坐标系对于坐标系ABC，假设，假设A是参考坐标是参考坐标系（基坐标系），则系（基坐标系），则B相对于相对于A的坐标变换以的坐标变换以及及C

17、相对于相对于B的坐标变换称为联体坐标变换。的坐标变换称为联体坐标变换。q已知已知B 在在A中的表示为中的表示为T1，C在在B中的中的表示为表示为T2，刚体在，刚体在C中的表示为中的表示为T3，则刚体在，则刚体在A中的表示为中的表示为TT1T2T3n 设设C在基在基W下的描述下的描述为为WTC，在，在B下的描述为下的描述为BTC。WTC WTB BTCBTC WT-1C WTB 3.坐标变换n 通用旋转变换：如果旋转通用旋转变换：如果旋转所绕的轴不是坐标轴，而所绕的轴不是坐标轴，而是一根任意轴？设是一根任意轴？设f为单位为单位矢量，矢量，为旋转角。为旋转角。n 设设B在基在基W下的下的描述为描述

18、为WTB，且，且f为为B的的z轴上的单位矢量。轴上的单位矢量。,1000BzyxzyxzyxzyxBWZRotfRotpppooonnnT1,BWBBWCBBBWCWTZRotTfRotTZRotTTfRot3.坐标变换q通用旋转变换通用旋转变换3.坐标变换n思考：如何求解思考：如何求解T在在B下的位置？下的位置？B：基坐标系G：目标系T：工具系4.连杆变换矩阵n机械手是一系列由关节连接起来的连杆构成。机械手是一系列由关节连接起来的连杆构成。n每一个连杆建立一个坐标系，并用齐次变换描述每一个连杆建立一个坐标系，并用齐次变换描述坐标系之间的相对位置和姿态。坐标系之间的相对位置和姿态。nA A矩阵

19、：一个连杆和下一个连杆坐标系间的相对关矩阵：一个连杆和下一个连杆坐标系间的相对关系的齐次变换。系的齐次变换。nn对于六连杆机械手：对于六连杆机械手：T6A1A2A3A4A5A64.连杆变换矩阵4.14.1关节与连杆：关节与连杆：n 在机器人中，通常有两类关在机器人中，通常有两类关节：节：转动关节和移动关节。转动关节和移动关节。n自由度自由度：物体能够相对于坐：物体能够相对于坐标系进行独立运动的数目标系进行独立运动的数目q不同于人类的关节，一般机不同于人类的关节，一般机器人关节为一个自由度的关器人关节为一个自由度的关节，其目的是为了简化力学、节，其目的是为了简化力学、运动学和机器人的控制。运动学

20、和机器人的控制。q转动关节提供了一个转动自转动关节提供了一个转动自由度，移动关节提供一个移由度，移动关节提供一个移动自由度，各关节间是以固动自由度，各关节间是以固定杆件相连接的。定杆件相连接的。4.连杆变换矩阵n关节轴线：关节轴线：对于旋转关节，对于旋转关节，其转动轴的中心线作为关其转动轴的中心线作为关节轴线。对于平移关节，节轴线。对于平移关节，取移动方向的中心线作为取移动方向的中心线作为关节轴线。关节轴线。n连杆参数：连杆参数：q连杆长度连杆长度：两个关节的关：两个关节的关节轴线节轴线Ji与与Ji+1 的公垂线的公垂线距离为连杆长度，记为距离为连杆长度，记为ai。q连杆扭转角连杆扭转角：由：

21、由Ji与公垂与公垂线组成平面线组成平面P，Ji+1 与平与平面面P的夹角为连杆扭转角，的夹角为连杆扭转角，记为记为i 。4.连杆变换矩阵q连杆偏移量连杆偏移量：除第一和：除第一和最后连杆外，中间的连最后连杆外，中间的连杆的两个关节轴线杆的两个关节轴线Ji与与Ji+1 都有一条公垂线都有一条公垂线ai，一个关节的相邻两条公一个关节的相邻两条公垂线垂线ai与与ai-1 的距离为的距离为连杆偏移量，记为连杆偏移量，记为di。q关节角关节角：关节：关节Ji的相邻的相邻两条公垂线两条公垂线ai与与ai-1在以在以Ji为法线的平面上的投为法线的平面上的投影的夹角为关节角，记影的夹角为关节角，记为为i。qa

22、i,i, di, i这组参数称为这组参数称为Denavit-Hartenberg(D-H)参数。参数。4.连杆变换矩阵连杆本身连杆本身的参数的参数连杆长度连杆长度an连杆两个轴的公垂线距离（连杆两个轴的公垂线距离（x方向）方向）连杆扭转角连杆扭转角n连杆两个轴的夹角（连杆两个轴的夹角（x轴的扭转角）轴的扭转角）连杆之间连杆之间的参数的参数连杆之间的距连杆之间的距离离dn相连两连杆公垂线距离（相连两连杆公垂线距离（z方向平移距）方向平移距）连杆之间的夹连杆之间的夹角角n相连两连杆公垂线的夹角（相连两连杆公垂线的夹角（z轴旋转角）轴旋转角）D-H参数参数4.连杆变换矩阵n连杆坐标系：连杆坐标系：q

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

24、定义法定义法nCraig定义法定义法4.连杆变换矩阵nPaul定义法定义法:q中间连杆中间连杆Ci坐标系的建立：坐标系的建立：n原点原点Oi：取关节轴线：取关节轴线Ji与与Ji+1的公垂线在与的公垂线在与Ji+1的的交点为坐标系原点。交点为坐标系原点。nZi轴：取轴：取Ji+1的方向为的方向为Zi轴方向。轴方向。nXi轴：取公垂线指向轴：取公垂线指向Oi的的方向为方向为Xi轴方向。轴方向。nYi轴：根据右手定则由轴：根据右手定则由Xi轴和轴和Zi轴确定轴确定Yi轴的方向轴的方向。4.连杆变换矩阵q第一连杆第一连杆C1C1坐标系的建立：坐标系的建立：n原点原点O1O1：取基坐标系原点为坐标系原点

25、。：取基坐标系原点为坐标系原点。nZ1Z1轴：取轴：取J1J1的方向为的方向为Z1Z1轴方向。轴方向。nX1X1轴：轴：X1X1轴方向任意选取。轴方向任意选取。nY1Y1轴：根据右手定则由轴：根据右手定则由X1X1轴和轴和Z1Z1轴确定轴确定Y1Y1轴的方向。轴的方向。4.连杆变换矩阵q最后连杆最后连杆Cn坐标系的建立：最坐标系的建立：最后一个连杆一般是抓手。后一个连杆一般是抓手。n原点原点OnOn：取抓手末端中心点为：取抓手末端中心点为坐标系原点。坐标系原点。nZnZn轴：取抓手的朝向轴：取抓手的朝向, , 即指向即指向被抓取物体的方向为被抓取物体的方向为ZnZn轴方向。轴方向。nXnXn轴

26、：取抓手一个指尖到另一轴：取抓手一个指尖到另一个指尖的方向为个指尖的方向为XnXn轴方向。轴方向。nYnYn轴：根据右手定则由轴：根据右手定则由XnXn轴和轴和ZnZn轴确定轴确定YnYn轴的方向轴的方向。4.连杆变换矩阵nCraig定义法定义法:对于相邻两个连杆对于相邻两个连杆CiCi和和Ci+1Ci+1，有三个，有三个关节关节Ji-1Ji-1、JiJi和和Ji+1Ji+1。n中间连杆中间连杆CiCi坐标系的建立：坐标系的建立：q原点原点OiOi：取关节轴线：取关节轴线JiJi与与Ji+1Ji+1的的公垂线在与公垂线在与JiJi的交点的交点为坐标系原点。为坐标系原点。qZiZi轴：取轴：取J

27、iJi的方向为的方向为ZiZi轴方向。轴方向。qXiXi轴：取公垂线从轴：取公垂线从OiOi指向指向Ji+1Ji+1的方向为的方向为XiXi轴方轴方向。向。qYiYi轴：根据右手定则由轴：根据右手定则由XiXi轴和轴和ZiZi轴确定轴确定YiYi轴的轴的方向。方向。4.连杆变换矩阵n第一连杆第一连杆C1C1坐标系的建立：坐标系的建立：q原点原点O1O1：取基坐标系原点为坐标系原点。：取基坐标系原点为坐标系原点。qZ1Z1轴：取轴：取J1J1的方向为的方向为Z1Z1轴方向。轴方向。qX1X1轴：轴：X1X1轴方向任意选取。轴方向任意选取。qY1Y1轴：根据右手定则由轴：根据右手定则由X1X1轴和

28、轴和Z1Z1轴确定轴确定Y1Y1轴的方向。轴的方向。n最后连杆最后连杆CnCn坐标系的建立：最后一个连杆一般是抓手。坐标系的建立：最后一个连杆一般是抓手。q原点原点OnOn：取抓手末端中心点为坐标系原点。：取抓手末端中心点为坐标系原点。qZnZn轴：取抓手的朝向轴：取抓手的朝向, , 即指向被抓取物体的方向为即指向被抓取物体的方向为ZnZn轴方向。轴方向。qXnXn轴：取抓手一个指尖到另一个指尖的方向为轴：取抓手一个指尖到另一个指尖的方向为XnXn轴方向。轴方向。qYnYn轴：根据右手定则由轴：根据右手定则由XnXn轴和轴和ZnZn轴确定轴确定YnYn轴的方向。轴的方向。4.连杆变换矩阵n4.

29、连杆变换矩阵nPaul定义法的连杆变换矩阵:qCi-1坐标系经过两次旋转和两次平移可以变换到坐标系经过两次旋转和两次平移可以变换到Ci坐标系。坐标系。n第一次：以第一次：以Zi-1轴为转轴，旋转轴为转轴，旋转i角度，使新的角度，使新的Xi-1轴与轴与Xi轴同向。轴同向。n第二次：沿第二次：沿Zi-1轴平移轴平移di，使新的，使新的Oi-1移动到关节轴线移动到关节轴线Ji与与Ji+1的公垂线在与的公垂线在与Ji的交点。的交点。n第三次：沿新的第三次：沿新的Xi-1轴（轴（Xi轴）平移轴）平移ai，使新的，使新的Oi-1移动移动到到Oi。n第四次：以第四次：以Xi轴为转轴，旋转轴为转轴，旋转i角度

30、，使新的角度，使新的Zi-1轴与轴与Zi轴同向。轴同向。q至此，坐标系至此，坐标系Oi-1Xi-1Yi-1Zi-1与坐标系与坐标系OiXiYiZi已已经完全重合。经完全重合。Paul定义法的连杆变换矩阵n可以用连杆Ci-1到连杆Ci的4个齐次变换来描述。总的变换矩阵(D-H矩阵)为：4.连杆变换矩阵nCraig定义法的连杆变换矩阵：qCi-1Ci-1坐标系经过两次旋转和两次平移可以变换到坐标系经过两次旋转和两次平移可以变换到CiCi坐标坐标系。系。n第一次：沿第一次：沿Xi-1Xi-1轴平移轴平移ai-1,ai-1,将将Oi-1Oi-1移动到移动到Oi-1Oi-1。n第二次：以第二次：以Xi-

31、1Xi-1轴为转轴，旋转轴为转轴，旋转i-1i-1角度，使新的角度，使新的Zi-1(Zi-1)Zi-1(Zi-1)轴与轴与ZiZi轴同向。轴同向。n第三次：沿第三次：沿ZiZi轴平移轴平移didi，使新的，使新的Oi-1Oi-1移动到移动到OiOi。n第四次：以第四次：以ZiZi轴为转轴，旋转轴为转轴，旋转ii角度，使新的角度，使新的Xi-1 Xi-1 (Xi-1)(Xi-1)轴与轴与XiXi轴同向。轴同向。q至此，坐标系至此，坐标系Oi-1Xi-1Yi-1Zi-1Oi-1Xi-1Yi-1Zi-1与坐标系与坐标系OiXiYiZiOiXiYiZi已经已经完全重合。完全重合。Craig定义法的连杆

32、变换矩阵n这种关系可以用连杆Ci-1到连杆Ci的4个齐次变换来描述。总的变换矩阵(D-H矩阵)为：5.机器人正向运动学n有有n n个自由度的工业机器人所有连杆的位置和姿态，个自由度的工业机器人所有连杆的位置和姿态，可以用一组关节变量（可以用一组关节变量（didi或或ii）以及杆件几何常数）以及杆件几何常数来表示。这组变量通常称为来表示。这组变量通常称为关节矢量或关节坐标关节矢量或关节坐标，由，由这些矢量描述的空间称为这些矢量描述的空间称为关节空间关节空间。n一旦确定了机器人各个关节的关节坐标，机器人末端一旦确定了机器人各个关节的关节坐标，机器人末端的位姿也就随之确定。因此由机器人的的位姿也就随

33、之确定。因此由机器人的关节空间到机关节空间到机器人的末端笛卡尔空间之间的映射，是一种单射关系器人的末端笛卡尔空间之间的映射，是一种单射关系。n机器人的正向运动学，描述的就是机器人的机器人的正向运动学，描述的就是机器人的关节空间关节空间到机器人的末端笛卡尔空间之间的映射关系到机器人的末端笛卡尔空间之间的映射关系。5.机器人正向运动学n对于具有对于具有n个自由度的串联结构工业机器人，各个连个自由度的串联结构工业机器人，各个连杆坐标系之间属于联体坐标关系。若各个连杆的杆坐标系之间属于联体坐标关系。若各个连杆的D-H矩阵分别为矩阵分别为Ai，则机器人末端的位置和姿态为：，则机器人末端的位置和姿态为：T

34、=A1A2A3Ann相邻连杆相邻连杆Ci-1和和Ci，两连杆坐标系之间的变换矩阵即，两连杆坐标系之间的变换矩阵即为连杆变换矩阵位姿：为连杆变换矩阵位姿：i-1Ti=Ain机器人的末端相对连杆机器人的末端相对连杆Ci-1的位置和姿态为：由于坐的位置和姿态为：由于坐标系的建立不是唯一的，不同的坐标系下标系的建立不是唯一的，不同的坐标系下D-H矩阵是矩阵是不同的，末端位姿不同的，末端位姿T不同。但对于相同的基坐标系，不同。但对于相同的基坐标系，不同的不同的D-H矩阵下的末端位姿矩阵下的末端位姿T相同。相同。i-1Tn=AiAi+1An5.1PUMA560机器人的正向运动学n PUMA 560是属于关

35、节式机器人，6个关节都是转动关节。前3个关节确定手腕参考点的位置，后3个关节确定手腕的方位。5.1PUMA560机器人的正向运动学p连杆及关节参数表连杆及关节参数表大臂小臂腰关节肩关节肘关节腕关节5.1PUMA560机器人的正向运动学n坐标系的建立：坐标系的建立：q初始位置：大臂处于某一朝初始位置：大臂处于某一朝向时，作为腰关节的初始位向时，作为腰关节的初始位置；大臂处于水平位置时，置；大臂处于水平位置时，作为肩关节的初始位置；小作为肩关节的初始位置；小臂处在下垂位置时，关节轴臂处在下垂位置时，关节轴线线J4与与J1平行，作为肘关节平行，作为肘关节的初始位置；关节轴线的初始位置；关节轴线J6与

36、与J4平行时，作为腕关节的的平行时，作为腕关节的的初始位置，抓手两个指尖的初始位置，抓手两个指尖的连线与大臂平行时，作为腕连线与大臂平行时，作为腕旋转关节的初始位置。旋转关节的初始位置。5.1PUMA560机器人的正向运动学n坐标系的建立：坐标系的建立：q基坐标系基坐标系OX0Y0Z0：原点原点O O0 0选取选取J1J1与与J2J2的交点，的交点，z0z0轴方轴方向选取为沿向选取为沿J1J1轴向上的方向，轴向上的方向，y0y0轴方向选取轴方向选取J2J2轴线的方向，轴线的方向，x0 x0轴根据右手法则确定。轴根据右手法则确定。q坐标系坐标系O1X1Y1Z1：原点原点O O1 1选取选取J1J

37、1与与J2J2的交点，的交点，z1z1轴方轴方向为向为J2J2轴线的方向，轴线的方向，y1y1轴方向轴方向选取与基坐标系选取与基坐标系z0z0轴相反的方轴相反的方向，向，x1x1轴的方向与轴的方向与x0 x0轴方向相轴方向相同。同。5.1PUMA560机器人的正向运动学q坐标系坐标系O2X2Y2Z2：原原点点O O2 2选取大臂与选取大臂与J3J3的交点，的交点，z2z2轴方向为轴方向为J3J3轴线的方向，轴线的方向，x2x2轴的方向选取轴的方向选取J2J2与与J3J3的公的公垂线指向垂线指向O2O2的方向。的方向。q坐标系坐标系O3X3Y3Z3：原原点点O O3 3选取选取J4J4、J5J5

38、与与J6J6的交点，的交点，z3z3轴方向为轴方向为J4J4轴线的方向，轴线的方向，y3y3轴的方向与轴的方向与z2z2轴相反的方轴相反的方向。向。5.1PUMA560机器人的正向运动学q坐标系坐标系O4X4Y4Z4：原点原点O O4 4选选取取J4J4、J5J5与与J6J6的交点，的交点，z4z4轴方向轴方向为为J5J5轴线的方向，轴线的方向，y4y4轴的方向与轴的方向与z3z3轴相同的方向。轴相同的方向。q坐标系坐标系O5X5Y5Z5：原点原点O O5 5选选取取J4J4、J5J5与与J6J6的交点，的交点，z5z5轴方向轴方向为为J6J6轴线的方向，轴线的方向，y5y5轴的方向与轴的方向

39、与z4z4轴相反的方向。轴相反的方向。q坐标系坐标系O6X6Y6Z6：原点原点O O6 6选选取取J4J4、J5J5与与J6J6的交点，的交点，z6z6轴方向轴方向为为J6J6轴线的方向，轴线的方向，x6x6轴的方向选轴的方向选取抓手一个指尖到另一个指尖的取抓手一个指尖到另一个指尖的方向。方向。5.1PUMA560机器人的正向运动学n连杆变换矩阵：连杆变换矩阵：q基坐标系基坐标系OX0Y0Z0与与O1X1Y1Z1：原点重合，连杆长度和连杆偏移量原点重合，连杆长度和连杆偏移量为零。关节角为为零。关节角为1 1，连杆扭角为，连杆扭角为90。.100000100cos0sin0sin0cos1111

40、1A5.1PUMA560机器人的正向运动学q坐标系坐标系O1X1Y1Z1与与 O2X2Y2Z2 ：连杆长度为连杆长度为a2，连杆偏移量为连杆偏移量为d2，关节角为，关节角为2，连杆扭转角为零。连杆扭转角为零。1000100sin0cossincos0sincos2222222222daaAq坐标系坐标系 O2X2Y2Z2 与与O3X3Y3Z3 ：连杆长度为连杆长度为a3，连杆偏移量为连杆偏移量为d3，关节角为，关节角为3，连杆扭转角为连杆扭转角为90 。 。1000010sincos0sincossin0cos3333333333daaA5.1PUMA560机器人的正向运动学q坐标系坐标系O3

41、X3Y3Z3与与 O4X4Y4Z4 ：连杆长度和连连杆长度和连杆偏移量为杆偏移量为0，关节角为，关节角为4，连，连杆扭转角为杆扭转角为90 。 。q坐标系坐标系 O4X4Y4Z4 与与O5X5Y5Z5 ：连杆长度和连连杆长度和连杆偏移量为杆偏移量为0，关节角为，关节角为5，连杆扭转角为连杆扭转角为90 。 。100000100cos0sin0sin0cos44444A100000100cos0sin0sin0cos55555A5.1PUMA560机器人的正向运动学q坐标系坐标系 O5X5Y5Z5 与与O6X6Y6Z6 ：连杆长度和连连杆长度和连杆偏移量为杆偏移量为0，关节角为，关节角为6，连，

42、连杆扭转角为杆扭转角为0 。 。1000010000cossin00sincos66666Aq由六个连杆的由六个连杆的DH矩阵，可矩阵，可以求取机器人末端在基坐标系以求取机器人末端在基坐标系下的位置和姿态：下的位置和姿态：T=A1A2A6q上述即为上述即为PUMA560机机器人人的运动学方程。器人人的运动学方程。作业：斯坦福机械手的运动方程斯坦福机器人的连杆及关节参数表斯坦福机器人的连杆及关节参数表6.机器人逆向运动学n正向运动学：正向运动学：关节空间关节空间末端笛卡儿空间，单射末端笛卡儿空间，单射n逆向运动学：逆向运动学：末端笛卡儿空间末端笛卡儿空间关节空间，复射关节空间，复射n所谓逆运动学方程的