1、Chapt.4 机器人运动学（正解）,张建瓴,机器人运动学,根据关节变量qi（i=1,2,3,）的值，利用运动学方程，可以计算出手爪相对于基座（工作站）的位姿。 通常把关节矢量构成的空间称为关节空间，而把手爪位姿构成的空间称为操作空间。 由关节空间向操作空间的映射称为正向运动学，其逆映射称为反向运动学。,4.1 连杆参数与关节变量,机器人操作臂可以看成是由一系列连杆通过关节顺次相连的开式运动链。关节决定两相邻构件之间的连接关系，称为运动副。,通常操作臂都是由旋转关节和移动关节构成的低副，具有一个自由度，因此6个自由度的操作臂是由6个杆和6个关节组成的。,一、连杆参数,连杆i1是由关节轴线i1和
2、关节轴线i的公法线长度ai-1，以及两关节轴线的夹角i-1所规定。,1、连杆长度ai-1为连杆i1的长度； 2、连杆扭角i-1为连杆i1的扭角。 公法线ai-1由关节i1指向关节i，i-1的指向规定为轴线i1绕公法线转至轴线i时所转过的角度。,连杆参数,当两关节轴线和i平行时，i-1=0，此时i-1的指向不定，可以任意规定。,当两关节轴线相交时，连杆长度为零。,连杆连接时的参数,1、首、末端连杆的描述,相邻两连杆之间有一个共同的关节轴线。所以每一个关节轴线有两条公法线和它垂直，每条公法线相应于一条连杆。,2、中间连杆连接的描述,连杆参数,（1）连杆偏置 两条公法线的距离称为连杆的偏置，记为di
3、，表示连杆i相对于连杆i1的偏置。,（2）连杆转角 两条公法线之间的夹角称为关节角，记为i，表示连杆i相对于连杆i1绕该轴线i的旋转角度。,连杆参数及坐标系,（1）需要用两个参数来描述一个连杆，即公共法线距离ai和垂直于ai所在平面内两轴的夹角i；,（2）需要另外两个参数来表示相邻两杆的关系，即两连杆的相对位置di和两连杆法线的夹角i，如图所示。,连杆参数与坐标系,除第一个和最后一个连杆外，每个连杆两端的轴线各有一条法线，分别为前、后相邻连杆的公共法线。这两法线间的距离即为di。,ai为连杆长度；i为连杆扭角；di为两连杆距离；i为两连杆夹角。,3.2 机器人坐标系设定,一、标准坐标系,1、基
4、坐标系B：与操作臂的基座固接，也称为坐标系0，与连杆o固接； 2、工作(站)坐标系S：与工作位姿相联系； 3、腕坐标系W：固定在机器人操作臂的末端连杆n上，也称为连杆n坐标系N； 4、工具坐标系T：固结在所握工具的端部； 5、目标坐标系G：用来描述机器人移动工具所应到达的位姿。,二、建立连杆坐标系的步骤,1、找出并画出各个关节轴线； 2、找出并画出相邻两轴线i和i+1的公垂线ai（或两轴线的交点）。找出公垂线ai与轴线i的交点，选为坐标系i的原点Oi+1； 3、Zi轴规定为与关节i轴重合； 4、Xi轴规定为与公垂线ai重合。若Zi与Zi+1相交，规定Xi是Zi与Zi+1所张平面的法线； 5、按
5、右手法则决定Yi=ZiXi(建立右手坐标系)； 6、当第一个关节变量为零时，规定0与1重合； 7、末端坐标系n，原点和Xn的方向可以任取，但应使连杆参数尽可能为零。,三、连杆坐标系,1、首端连杆坐标系的建立,2、末端连杆的坐标系,坐标系的原点置于夹手指尖的中心，由矢量p表示。 （1）z轴：处于夹手进人物体的方向上，并称之为接近矢量a；,（2）y轴：从一个指尖指向另一个指尖，处于规定夹手方向上，称为方向矢量o；,末端连杆的坐标系,最后一个矢量叫做法线矢量n，它与矢量o和a一起构成一个右手矢量集合，并由矢量的交乘所规定：n=oa。因此，变换T6具有下列元素：,3、中间连杆的坐标系建立,对于转动关节
6、，i为关节变量。,连杆i的坐标系：原点，z轴，x轴； 连杆i+1的坐标系：原点，z轴，x轴。,中间连杆坐标系建立,连杆i的坐标系：原点位于关节i和i+1的公共法线与关节i+1轴线的交点上。如果两相邻连杆的轴线相交于一点，那么原点就在这一交点上。如果两轴线互相平行，那么就选择原点使对下一连杆（其坐标原点已确定）的距离di+1为零。,连杆i的z轴与关节i+1的轴线在一直线上，而x轴则在连杆i和i+1的公共法线上，其方向从i指向i+1，如图所示。,中间连杆坐标系建立,当两关节轴线相交时，x轴的方向与两矢量的交积zi1zi平行或反向平行，x轴的方向总是沿着公共法线从转轴i指向i+1。,当两轴xi1和z
7、i平行且同向时，第i个转动关节的i为零。,坐标变换过程,连杆坐标系建立之后，按照下列顺序由两个旋转和两个平移来建立相邻两连杆i1与i之间的相对关系。,（1）绕zi1轴旋转i角，使xi1轴转到与xi同一平面内。 （2）沿zi1轴平移一距离di，把xi1移到与xi同一直线上。 （3）沿i轴平移一距离ai1，把连杆i1的坐标系移到使其原点与连杆n的坐标系原点重合的地方。 （4）绕xi1轴旋转i1角，使zi1转到与zi同一直线上。,广义变换矩阵,这种关系可由表示连杆i对连杆i1相对位置的四个齐次变换来描述，并叫做Ai矩阵。此关系式为：,展开上式可得：,移动关节坐标系建立,如图所示，距离di为联轴节（关
8、节）变量，而联轴节轴线的方向即为此联轴节移动的方向。 对于棱柱联轴节来说，其长度ai没有意义，令其为零。,连杆i的坐标系：原点，z轴，x轴； 连杆i+1的坐标系：原点，z轴，x轴。,移动关节坐标系建立,坐标系i：原点与下一个规定的连杆原点重合；,z轴在关节i+1的轴线上； xi轴平行或反向平行于棱柱联轴节方向矢量与zi矢量的交积。 当di=0时，我们定义该联轴节的位置为零。,2019/4/18,华南农业大学工程学院,移动关节坐标系,移动关节时，i1，ai1，di，i四个参数中，扭角i1、杆长ai1、关节转角i为常量；偏置di为变量，称关节位移量。,移动副连接的两杆件(坐标系前量),坐标变换过程
9、,连个坐标系i1与i之间的相对关系：,（1）绕zi1轴旋转i角，使xi1轴转到与xi同一平面内。 （2）沿zi1轴平移一距离di，把xi1移到与xi同一直线上。 （3）沿i轴平移一距离ai1，把连杆i1的坐标系移到使其原点与连杆n的坐标系原点重合的地方。 （4）绕xi1轴旋转i1角，使zi1转到与zi同一直线上。,广义变换矩阵,对于棱柱关节，A矩阵为：,当各连杆的坐标系被规定之后，就能够列出各连杆的常量参数。旋转关节：常量参数为di，ai1和i1；棱柱联轴节：常量参数为i和i。 这样，A矩阵就成为关节变量的函数（旋转关节）或变量d的函数（棱柱联轴节）。,4.3 机器人运动方程的表示,机器人可看
10、作是由关节连接起来的连杆系统。为机器人的每一连杆建立一个坐标系，并用齐次变换来描述这些坐标系间的相对位置和姿态。,把一个连杆与下一个连杆间齐次变换叫做A矩阵。一个A矩阵就是一个描述连杆坐标系间相对平移和旋转的齐次变换。 A1表示第一个连杆对于基系的位置和姿态，A2表示第二个连杆相对于第一个连杆的位置和姿态，那么第二个连杆在基系中的位置和姿态可由下列矩阵的乘积给出,T2=A1A2,机器人运动方程,同理，若A3表示第三个连杆相对于第二个连杆的位置和姿态，则有：T3=A1A2A3 称这些A矩阵的乘积为T矩阵，其前置上标若为0，则可略去不写。于是，对于六连杆机械手，有下列T矩阵 T6=A1A2A3A4
11、A5A6,一个六连杆机械手具有6个自由度，每个连杆含有1个自由度，并能在其运动范围内任意定位与定向。其中，三个自由度用于规定位置，另三个自由度用来规定姿态。T6表示机械手的位置和姿态。,机器人运动方程,机械手的末端装置即为连杆6的坐标系，它与连杆i1坐标系的关系可由i-1T6表示为：,可得连杆变换通式为：,机器人运动方程,T6为机器人末端的位置姿态矩阵：,可得机器人的运动学方程：,1、用旋转序列表示运动姿态,机械手的运动姿态往往由一个绕轴x，y和z的旋转序列来规定。这种转角的序列，称为欧拉（Euler）角。 欧拉角用绕z轴旋转角，再绕新的y轴（y）旋转角，最后绕新的z轴（z）旋转角来描述任何可
12、能的姿态，如图所示。,欧拉角表示运动姿态,上述欧拉变换Euler(,)可由连乘三个旋转矩阵来求得，即：,2、用RPY角表示运动姿态,另一种常用的旋转集合是横滚（roll）、俯仰（pitch）和偏转（yaw）。,横滚对应于绕z轴旋转角，俯仰对应于绕y轴旋转角，而偏转则对应于绕x轴旋转角。,用RPY角表示运动姿态,RPY角旋转次序：,即：先绕z轴旋转角，再绕y轴旋转角，最后绕x轴旋转角。此旋转变换计算如下：,4.4 机器人运动方程的求解,求解方程时，即给定q1，q2，q6等，由方程式直接可得机械手末端的位置px，py，pz。然后需求解各坐标变换角。,一、欧拉变换解,已知任一变换T矩阵的各元，要求，
13、和，即：,等价于：,一、欧拉变换解,令前面矩阵方程两边各对应元素相等，可得12个方程，关于欧拉变换角的方程有9个：,nx=cccss （5.6.8） ny=scc+cs （5.6.9） nz=sc （5.6.10） ox=ccssc （5.6.11） oy=scs+cc （5.6.12） oz=ss （5.6.13） ax=cs （5.6.14） ay=ss （5.6.15） az=c （5.6.16）,用双变量反正切函数确定角度,据式(5.6.16)得：=arcos(az) （5.6.17）,据式(5.6.14)和(5.6.17)有：=arccos(ax/s) （5.6.18） 又根据式（5
14、.6.10）和（5.6.17）有：=arcos(nx/s) （5.6.19）,用双变量反正切函数确定角度(续1),在求解三角函数时，一般采用双变量反正切函数atan2（令atan表示arctan）来确定角度。atan2提供二个自变量，即纵坐标y和横坐标x，见图。,当，由atan2反求角度时，同时检查y和x的符号来确定其所在象限。这一函数也能检验什么时候x或y为0，并反求出正确的角度。atan2的精确程度对其整个定义域都是一样的。,二、用显式方程求各角度,用未知逆变换依次左乘已知方程，对于欧拉变换有：,前一个方程可以写成：,或写成下面的形式：,用显式方程求各角度(续1),由两个矩阵的第二行第三列
15、的元素相等，得f12(a)=0，即:,求解上式得：,用显式方程求各角度(续2),两解相差1800。,除非出现ay和ax同时为0的情况，我们总能得到两个相差1800的解。 当ay和ax均为0时，角度没有定义。这种情况是在机械手臂垂直向上或向下，且和两角又对应于同一旋转时出现的。称为退化（degeneracy）。这时，我们任取=0。,求得值之后，可方便求得其他两个角度：,用显式方程求各角度(续3),三、RPY角变换解（滚、仰、偏变换解）,直接从显式方程来求解用滚动、俯仰和偏转表示的变换方程。变换后的运动方程式：,由矩阵的第1行第2列相等，可得：,RPY角变换解（续1）,由矩阵方程的(3,1)及(1
16、,1)元素相等，可得：,由矩阵方程的(2,3)和(2,2)元素相等，得：,RPY变换各角如为：,2019/4/18,华南农业大学工程学院,4.5 平面3自由度机械臂运动学方程,如图所示的三杆平面操作机，试求 。,解： （1）设坐标系。共设S0，S1，S2，S3，Se=(S4)五个坐标系，如图所示。 图中zi轴均指向纸外。,2019/4/18,华南农业大学工程学院,平面3自由度机械臂,（2）确定结构参数和关节变量。 因操作机为全回转关节，故di是结构参数，i(1,2,3)为关节变量，如表所示。,2019/4/18,华南农业大学工程学院,平面3自由度机械臂,（3）两杆间的位姿矩阵 。根据参数表得：
17、,2019/4/18,华南农业大学工程学院,平面3自由度机械臂,（4）求末端执行器的位姿矩阵。,2019/4/18,华南农业大学工程学院,平面3自由度机械臂,（5）为求末端执行器的位置和姿势。根据公式可得该平面操作机末端执行器的姿势和位置：,4.6 XHK5140换刀机械手的运动学方程,XHK5140型换刀机械手具有4个自由度，关节1和2是转动关节，关节3和4是移动关节。,4.7 PUMA560型 机器人的运动学方程,PUMA560属于关节式机器人，6个关节都是转动关节。前3个关节确定手腕参考点的位置，后3个关节确定手腕的方位。和大多数工业机器人一样，后3个关节轴线交于一点。该点选作为手腕的参
18、考点，也选作为连杆坐标系4，5和6的原点。,关节1的轴线铅垂，关节2和3的轴线水平，且平行，距离为a2。关节l和2的轴线垂直相交，关节3和4的轴线垂直交错，距离为a3。,PUMA560机器人参数,各连杆坐标系如图所示，相应的连杆参数列于表4-1。 其中： a2=431.8mm a3=20.32mm d2=149.09mm d4=433.07mm。,一、坐标系的设定,坐标系如图5-9所示。,二、连杆参数,表5-1 PUMA560机器人的连杆参数,三、运动学方程的建立,1、设定各个连杆的坐标系，列出相应的连杆参数，见表5-1。,2、写出各个连杆变换矩阵,3、写出手臂变换矩阵和运动学方程,要求解此运动方程，需先计算某些中间结果,运动方程求解,机械手的T变换矩阵,变换矩阵0T6描述了末端连杆坐标系6相对基坐标系0的位姿，是机械手运动分析和综合的基础。,作 业,思考题：讨,作业： 1、矢量Ap轴绕ZA轴旋转角，然后绕XA轴旋转角。试给出依次按上述次序完成旋转的旋转矩阵。 2、坐标系B的位置变化如下：初始时，坐标系A与B重合，让坐标系B绕ZB轴旋转角；然后再绕XB轴旋转角。给出把对矢量Bp的描述变为对Ap描述的旋转矩阵。,作 业,3、如图为一台三自由度机械手的机构。轴l与轴2垂直。（1）建立机械手各连杆的坐标系，然后求各变换矩阵0T1，1T2和2T3。 （2）写出其运动学方程。,
