机器人学基础-第3章-机器人运动学课件.ppt

1、中南大学中南大学蔡自兴，谢蔡自兴，谢 斌斌zxcai,2010机器人学基础机器人学基础第三章第三章 机器人运动学机器人运动学1Ch.3 Kinematics of RobotsFundamentals of Robotics Fundamentals of Robotics3.0 Introduction to Robot KinematicsKinematics treats motion without regard to the forces that cause it.Within the science of kinematics one studies the position,v

2、elocity,acceleration,and all higher order derivatives of the position variables(with respect to time or any other variable).从几何学几何学的观点来处理手指位置手指位置P与关节变量关节变量L1,L2,和 的关系称为运动学运动学(Kinematics)。122 3.0 Introduction to Robot KinematicsIn manipulator robotics,there are two kinematics tasks:Direct(also forwar

3、d)kinematics Given are joint relations(rotations,translations)for the robot arm.Task:What is the orientation and position of the end effector?Inverse kinematics Given is desired end effector position and orientation.Task:What are the joint rotations and orientations to achieve this?3 3.0 Introductio

4、n to Robot Kinematics3.0 Introduction to Robot KinematicsExample of Direct KinematicsDefine position of end effector and the joint variable，According to geometry：xry 1211212coscos()xLL11212sinsin()yLLThe general vector form()rf4 3.0 Introduction to Robot Kinematics2221122sinarctan()arctan()cosLyxLL2

5、222121 2()arccos2xyLLLL式中同样，如果用向量表示上述关系式，其一般可表示为1()frExample of Inverse Kinematics5 3.0 Introduction to Robot Kinematics1机器人到达给定的手爪位置 P有两个姿态满足要求，即图中的 也是其解。此时 和 变成为另外的值，即逆运逆运动学的解不是惟一的动学的解不是惟一的。2 将运动学公式 两边微分即可得到机器人手爪的速度和关节速度的关系，再进一步进行微分将得到加速度之间的关系，处理这些关系也是机器人的运动学问题。()rfExample of Inverse Kinematics6 3

6、.0 Introduction to Robot Kinematics73.1 Representation of Kinematics Equation of Robot Manipulator3.1 Representation of Kinematics Equation of ManipulatorMechanics of a manipulator can be represented as a kinematics chain of rigid bodies(links)connected by revolute or prismatic joints.One end of the

7、 chain is constrained to a base,while an end effector is mounted to the other end of the chain.The resulting motion is obtained by composition of the elementary motions of each link with respect to the previous one.8 机械手是一系列由关节连接起来的连杆构成的。为机械手的每一连杆建立一个坐标系，并用齐次变换来描述这些坐标系间的相对位置和姿态。A矩阵：一个描述两连杆间坐标系相对关系的齐

8、次变换，如；各 A 矩阵的乘积称为 T 矩阵。例如：A1，A2，A3 T1=A1 T2=A1A2 T3=A1A2A3 3.1 Representation of Kinematics Equation of Robot Manipulator3.1 Representation of Kinematics Equation of Manipulator9T矩阵：A矩阵的乘积。对于六连杆机械手，有下列T矩阵：一个六连杆机械手可具有六个自由度，每个连杆含有一个自由度，并能在其运动范围内任意定位与定向。(3.1)6123456TA A A A A A3.1 Representation of Kin

9、ematics Equation of Manipulator3.1 Representation of Kinematics Equation of Robot Manipulator103.1 Representation of Kinematics Equation of Robot Manipulator 3.1.1 Kinetic Pose and Oriented Angle 运动姿态和方向角运动姿态和方向角Motion Direction原点由矢量p表示。approach vector a：z向矢量orientation vector o：y向矢量normal vector n：

10、x向矢量，Forming a right-hand frame：n=o a or a=n o3.1n,o,ap图 矢量和3.1 Representation of Kinematics Equation of Manipulator113.1.1 Kinetic Pose and Oriented Angle因此，变换T6具有下列元素（同式2.35）。六连杆机械手的T 矩阵（T6）可由指定其16个元素的数值来决定。在这16个元素中，只有12个元素具有实际含义。60001xxxxyyyyzzzznoapnoapTnoap(3.2)3.1 Representation of Kinematics

11、Equation of Manipulator123.1.1 Kinetic Pose and Oriented AngleEuler angle to represent motion pose机械手的运动姿态往往由一个绕轴x，y 和 z 的旋转序列来规定。这种转角的序列，称为欧拉（Euler）角。欧拉角:用一个绕 z 轴旋转角，再绕新的 y 轴 y旋转角，最后绕新的 z 轴z旋转角来描述任 何可能的姿态。欧拉变换Euler可由连乘三个旋转矩阵来求得，即 （3.3）3.2图 欧拉角的定义),(),(),(),(zRotyRotzRotEuler3.1 Representation of Ki

12、nematics Equation of Manipulator133.1.1 Kinetic Pose and Oriented AngleRoll,Pitch,Yaw to represent motion pose 另一种常用的旋转集合是横滚（roll）、俯仰（pitch）和偏转（yaw）。3.3图 用横滚、俯仰和偏转表示机械手运动姿态3.1 Representation of Kinematics Equation of Manipulator143.1.1 Kinetic Pose and Oriented Angle 对于旋转次序，规定：式中，RPY表示横滚、俯仰和偏转三旋转的组合

13、变换。也就是说，先绕 x 轴旋转角，再绕 y 轴旋转角，最后绕 z 轴旋角。(3.4)(,)(,)(,)(,)RPYRot zRot yRot x 3.1 Representation of Kinematics Equation of Manipulator153.1 Representation of Kinetic Equation of Robot Manipulator 3.1.2 Kinetic Position and Coordinate 运动位置和坐标运动位置和坐标 一旦机械手的运动姿态由某个姿态变换规定之后，它在基系中的位置就能够由左乘左乘一个对应于矢量 p的平移变换来确定

14、（参式2.20）：61000100010001xyzppTp某姿态变换(3.6)3.1 Representation of Kinematics Equation of Manipulator163.1.2 Kinetic Position and CoordinateDescription in Cylinder Coordinates 用柱面坐标来表示机械手手臂的位置，即表示其平移变换。这对应于沿 x 轴平移 r，再绕 z 轴旋转，最后沿 z 轴平移 z。如图3.4(a)所示。3.4图 用柱面坐标和球面坐标表示位置)0,0,(),(),0,0(),(rTranszRotzTransrzCy

15、l3.1 Representation of Kinematics Equation of Manipulator173.1.2 Kinetic Position and CoordinateDescription in Spherical Coordinates 用球面坐标表示手臂运动位置矢量的方法，对应于沿 z 轴平移 r，再绕 y 轴旋转角，最后绕 z 轴旋转 角，如图3.4(b)所示，即为：(,)(,)(,)(0,0,)SphrRot zRot yTransr(3.9)3.1 Representation of Kinematics Equation of Manipulator183

16、.1 Representation of Kinetic Equation of Robot Manipulator 3.1.3 T-Matrix and A-Matrix 连杆变换矩阵及其乘积连杆变换矩阵及其乘积 广义连杆广义连杆 相邻坐标系间及其相应连杆可以用齐次变换矩阵来表示。要求解操作手所需要的变换矩阵，每个连杆都要用广义连杆来描述。在求得相应的广义变换矩阵之后，可对其加以修正，以适合每个具体的连杆。3.1 Representation of Kinematics Equation of Manipulator193.1.3 T-Matrix and A-Matrix机器人机械手是由一

17、系列连接在一起的连杆（杆件）构成的。需要用两个参数来描述一个连杆，即公共法线距离 所在平面内两轴的夹角 ；需要另外两个参数来表示相邻两杆的关系，即两连杆的相对位置 和两连杆法线的夹角 ，如图3.5所示。3.5图 转动关节连杆四参数示意图iiaa 和垂直于iidi3.1 Representation of Kinematics Equation of Manipulator203.1.3 T-Matrix and A-Matrixii3.1 Representation of Kinematics Equation of Manipulatorai-1:Link Length-mutual pe

18、rpendicular unique except for parallel axis :Link Twist-measured in the right-hand sense about1ia1i213.1.3 T-Matrix and A-Matrix3.1 Representation of Kinematics Equation of Manipulatordi:Link Offset-variable if joint i is prismatic(平动关节):Joint Angle-variable if joint i is revolute(转动关节)i223.1.3 T-Ma

19、trix and A-MatrixDenavit-Hartenberg Parameters4 D-H parameters3 fixed link parameters1 joint variablei and ai:describe the Link idi and i:describe the Links connection3.1 Representation of Kinematics Equation of Manipulator,iiiia drevolute jointprismatic jointiid233.1.3 T-Matrix and A-Matrix3.1 Repr

20、esentation of Kinematics Equation of Manipulatory-vectors:complete right-hand frames243.1.3 T-Matrix and A-Matrix3.1 Representation of Kinematics Equation of Manipulator1.Normals 2.Origins3.Z-axes4.X-axes253.1.3 T-Matrix and A-Matrix3.1 Representation of Kinematics Equation of Manipulator263.1.3 T-M

21、atrix and A-Matrix3.1 Representation of Kinematics Equation of ManipulatorExample-RRR Arm273.1.3 T-Matrix and A-Matrix3.1 Representation of Kinematics Equation of ManipulatorExample-RRR Armlinkdi1231i1iaiiidiia=distance from zi to zi+1 along xi=distance from xi-1 to xi along zi=angle from zi to zi+1

22、 about xi=angle from xi-1 to xi about zi283.1.3 T-Matrix and A-Matrix3.1 Representation of Kinematics Equation of ManipulatorExample-RRR Armlinkdi1000120L10230L2031i1iaiiidiia=distance from zi to zi+1 along xi=distance from xi-1 to xi along zi=angle from zi to zi+1 about xi=angle from xi-1 to xi abo

23、ut zi293.1.3 T-Matrix and A-Matrix3.5图 转动关节连杆四参数示意图iidiiaDenavit-Hartenberg notation=distance from zi to zi+1 along xi=distance from xi-1 to xi along zi=angle from zi to zi+1 about xi=angle from xi-1 to xi about zi3.1 Representation of Kinematics Equation of Manipulator303.1.3 T-Matrix and A-Matrix(

24、1)绕 轴旋转 角，使 轴转到与 同一平面内。(2)沿 轴平移一距离 ，把 移到与 同一直线上。(3)沿 xi 轴平移一距离 ai-1，使连杆 坐标系的原点与 连杆 i 的坐标系原点重合。(4)绕 xi 轴旋转 角，使 zi1 转到与 zi 同一直线上。1izi1ixix1izid1ixix1i1i3.1 Representation of Kinematics Equation of Manipulator3.1.3 T-Matrix and A-Matrix这种关系可由表示连杆相对位置的四个齐次变换来描述，并叫做 矩阵。此关系式为：(3.12)展开上式可得：（3.13）11(,)(0,0,

25、)(,0,0)(,)iiiiiARot zTransd Trans aRot x1000011111111iiiiiiiiiiiiiiiiiidcssascccscasscscAiA313.1 Representation of Kinematics Equation of Manipulator323.1.3 T-Matrix and A-MatrixUsing A-Matrix to represent T-Matrix机械手的末端装置即为连杆6的坐标系，它与连杆 坐标系的关系可由 表示为：可得连杆变换通式为：1i61Ti6161AAATiii(3.15)111111111100001ii

26、iiiiiiiiiiiiiiiiicsas cc csd sTs sc scd c (3.16)3.1 Representation of Kinematics Equation of Manipulator333.2 Solving Kinematical Equation of RobotManipulator 机械手运动学方程的求解机械手运动学方程的求解大多数机器人程序设计语言使用某个笛卡儿坐标系来指定机械手的末端位置。这一指定可用于求解机械手最后一个连杆的姿态 。不过，在机械手能够被驱动至这个姿态之前，必须知道与这个位置有关的所有关节的位置。求解运动方程时，我们从 开始求解关节位置。使

27、 的符号表达式的各元素等于 T6 的一般形式，并据此确定 。其它五个关节参数不可能从T6 求得，因为所求得的运动方程过于复杂而无法求解它们。我们可以由上节讨论的其它T 矩阵来求解它们。一旦求得 之后，可由 左乘 的一般形式，得：(3.18)式中，左边为 和 各元的函数。此式可用来求解其它各关节变量，如 等。3.2 Solving Kinematics Equation6T6T6T1111A6T61611TTA16T23.2 Solving Kinematics Equation 不断地用的逆矩阵左乘式(3.17)，可得下列4个矩阵方程式：(3.19)(3.20)(3.21)(3.22)上列各方

28、程的左式为 和前 个关节变量的函数。可用这些方程来确定各关节的位置。6261112TTAA636111213TTAAA64611121314TTAAAA6561112131415TTAAAAA6T)1(i34 3.2 Solving Kinematics Equation353.2.1 Solution of the Euler Transformation 欧拉变换解欧拉变换解基本隐式方程的解 令由式(3.4)和(3.23)得到：TEuler),(3.23)10000001000csscsssccscsssccssccssccsscccpaonpaonpaonzzzzyyyyxxxx(3.2

29、4)3.2 Solving Kinematics Equation3.2 Solving Kinematics Equation363.2.1 Solution of the Euler Transformation令矩阵方程两边各对应元素一一相等，可得到9个隐式方程如下：xyzxyzxyznc c cs sns c cc sns coc c ss cos c sc cos sac sas sac (3.25)(3.26)(3.27)(3.28)(3.29)(3.30)(3.31)(3.32)(3.33)1zca1xcas1zcns 3.2 Solving Kinematics Equatio

30、n373.2.1 Solution of the Euler Transformation 但这些解答是不确定的：（1）当由余弦函数求角度时，不仅此角度的符号是不确定的，并且所求角度的准确度也与该角度本身相关。（2）在求解 和 时，再次用到反余弦函数，且除式的分母为 。当 接近于0时，总会产生不准确。（3）当 或 时，和 的求解公式无定义。1zca1xcas1zcnssinsin0180 3.2 Solving Kinematics Equation383.2.1 Solution of the Euler Transformation 在求解时，总是采用双变量反正切函数atan2来确定角度。

31、atan2提供二个自变量，即纵坐标和横坐标，见图3.8。当-，由atan2反求角度时，同时检查y和x的符号来确定其所在象限。这一函数也能检验什么时候x或y为0，并反求出正确的角度。atan2的精确程度对其整个定义域都是一样的。3.8图 反正切函数atan2 3.2 Solving Kinematics Equation用双变量反正切函数用双变量反正切函数(two-argument arc tangent function)确定角度确定角度393.2.1 Solution of the Euler Transformation用显式方程求各角度用显式方程求各角度 要求得方程式的解，采用另一种通常

32、能够导致显式解答的方法。用未知逆变换依次左乘已知方程，对于欧拉变换有：式(3.37)的左式为已知变换的函数，而右式各元素或者为0，或者为常数。),(),(),(1zRotyRotTzRot),(),(),(11zRotTzRotyRot(3.37)(3.38)3.2 Solving Kinematics Equation40 对方程求解，整理之后确定其等价欧拉角：如果已知一个表示任意旋转的齐次变换，那么就能够确定其等价欧拉角。),(2atan),(2atan180),(2atanyxyxzyxxyocosncnsaasacaa(3.46)3.2.1 Solution of the Euler

33、Transformation 3.2 Solving Kinematics Equation413.2.2 Solution of RPY Transformations 滚、仰、偏变换解滚、仰、偏变换解 直接从显式方程来求解用滚动、俯仰和偏转表示的变换方程。推导计算可得RPY变换各角如下：),(2atan),(2atan180),(2atanyxyxyxzxyocosacasnsncnnn(3.52)3.2 Solving Kinematics Equation423.2.3 Solution of Spherical Coordinate Transformation 球面变换解球面变换解

34、把求解滚、仰和偏变换方程的技术用于球面坐标表示的运动方程。可推导出球面变换的解为：zyxzyxxypcpspcsrppspcpp)(),(2atan180),(2atan(3.58)3.2 Solving Kinematics Equation433.3 Kinematic Equation of PUMA 560 PUMA 560 机器人运动方程机器人运动方程 3.3.1 Motion Analysis of PUMA 560 PUMA 560 运动分析（表示）运动分析（表示）PUMA 560是属于关节式机器人，6个关节都是转动关节。前3个关节确定手腕参考点的位置，后3个关节确定手腕的方位。

35、各连杆坐标系如图3.9所示。相应的连杆参数列于表3.1。3.3 Kinematics Equation of PUMA 560443.3.1 Motion Analysis of PUMA 560 3.3 Kinematics Equation of PUMA 560453.3.1 Motion Analysis of PUMA 560 PUMA560每个关节均有角度零位与正负方向限位开关，机器人的回转机体实现机器人机体绕z0轴的回转（角1），它由固定底座和回转工作台组成。安装在轴中心的驱动电机经传动装置，可以实现工作台的回转。3.9 PUMA 560图 机器人的连杆坐标系()a 结构图 3.

36、3 Kinematics Equation of PUMA 560463.3.1 Motion Analysis of PUMA 560 大臂、小臂的平衡由机器人中的平衡装置控制，在机器人的回转工作台上安装有大臂台座，将大臂下端关节支承在台座上，大臂的上端关节用于支承小臂。大臂臂体的下端安有直流伺服电机，可控制大臂上下摆动（角 2）。3.9 PUMA 560图 机器人的连杆坐标系()a 结构图 3.3 Kinematics Equation of PUMA 560473.3.1 Motion Analysis of PUMA 560 小臂支承于大臂臂体的上关节处，其驱动电机可带动小臂做上下俯仰

37、（角3），以及小臂的回转（4）。3.9 PUMA 560图 机器人的连杆坐标系()a 结构图 3.3 Kinematics Equation of PUMA 560483.3.1 Motion Analysis of PUMA 560 机器人的腕部位于小臂臂体前端，通过伺服电动机传动，可实现腕部摆动（5）和转动（6）。3.9 PUMA 560图 机器人的连杆坐标系()a 结构图 3.3 Kinematics Equation of PUMA 560493.3.1 Motion Analysis of PUMA 560 3.9 PUMA 560图 机器人的连杆坐标系()a 结构图()b 坐标图

38、3.3 Kinematics Equation of PUMA 56050Using A-Matrix to represent T-Matrix机械手的末端装置即为连杆6的坐标系，它与连杆 坐标系的关系可由 表示为：可得连杆变换通式为：1i61Ti6161AAATiii(3.15)111111111100001iiiiiiiiiiiiiiiiiiicsas cc csd sTs sc scd c (3.16)3.3.1 Motion Analysis of PUMA 560 3.3 Kinematics Equation of PUMA 560513.3.1 Motion Analysis

39、of PUMA 560 据连杆变换通式式(3.16)和表3.1所示连杆参数，可求得各连杆变换矩阵如下：100001000000111110csscT100000100002222221csdscT100001000003323332csascT100000100044434443csdascT 3.3 Kinematics Equation of PUMA 560523.3.1 Motion Analysis of PUMA 560 各连杆变换矩阵相乘，得PUMA 560的机械手变换的T 矩阵：即为关节变量 的函数。该矩阵描述了末端连杆坐标系6相对基坐标系0的位姿。10000001000055

40、5554csscT100000010000666665csscT)()()()()()(66555444333222111060TTTTTTT 621,(3.59)3.3 Kinematics Equation of PUMA 56053要求解此运动方程，需先计算某些中间结果4T6，3T6，1T3，1T6 等，见式（3.60）至（3.63）。于是，可求得机械手的T 变换矩阵：其中nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz见式（3.64）1000611060zzzzyyyyxxxxpaonpaonpaonTTT3.3.1 Motion Analysis of PUMA

41、 560 3.3 Kinematics Equation of PUMA 56054（3.64）3.3.1 Motion Analysis of PUMA 560 3.3 Kinematics Equation of PUMA 560.,;,)(,)(,)(),()(),()(;)(),()(),()(2342223312234233221122342332215235423541523542315415235423165236465423654641652364654231654641652364654231652364654236465416523646542316465416523646

42、54231cdsasapcdsdcacaspsdsdcacacpccscsassccssccsassscsscccassccssccsoccscccssscsscccsoscsccsssscssccccocscsscccsnscccsccssssccccsnscccsscsssscccccnzyxzyxzyxzyx553.3.2 Motion Synthesis of PUMA 560 PUMA 560运动综合（求解）运动综合（求解）据式（3.59），可把PUMA 560的运动方程（3.64）写为：若末端连杆的位姿已经给定，即 为已知，则求关节变量 的值称为运动反解运动反解。用未知的连杆逆变换

43、左乘方程(3.65)两边，把关节变量分离出来，从而求得 的解。)()()()()()(100066555444333222111060TTTTTTpaonpaonpaonTzzzzyyyyxxxx(3.65)pao,n,和621,621,3.3 Kinematics Equation of PUMA 560563.3.2 Motion Synthesis of PUMA 5601.求 用逆变换 左乘式(3.65)两边：1)()()()()()(100066555444333222111060TTTTTTpaonpaonpaonTzzzzyyyyxxxx(3.65)010123451162233

44、445566()()()()()TTTTTTT0111T1111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoap 3.3 Kinematics Equation of PUMA 560573.3.2 Motion Synthesis of PUMA 5601.求 1010123451162233445566()()()()()TTTTTTT1111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzz

45、zzzzcsnoapnoapscnoapnoapTnoapnoap1112xyys pc ppd利用三角代换：cos;sinxypp其中22;atan2,xyyxpppp(3.67)两边(2,4)项元素对应相等：(3.68)3.3 Kinematics Equation of PUMA 560583.3.2 Motion Synthesis of PUMA 560求 式中，正、负号对应于 的两个可能解。212122221222122sin()/;cos()1(/)atan2,1atan2(,)atan2(,)yxxyddddppdppd 1(3.70)11112xyys pc ppdcos;s

46、inxypp22;atan2,xyyxpppp 3.3 Kinematics Equation of PUMA 560593.3.2 Motion Synthesis of PUMA 5602.求 31111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoap(3.67)1113 234 232213 234232 2xyxzzc ps ppa cd sa cppa sd ca s 两边(1,4)项和(3,4)项元素对应相等：3 34 3a cd sk其中22222

47、22232422xyzpppaaddka 3.3 Kinematics Equation of PUMA 560603.3.2 Motion Synthesis of PUMA 560求 1),(2atan),(2atan22423433kdakda3(3.73)正、负号对应 的两种可能解。3 34 3a cd sk1112222122atan2(,)atan2(,)xyyyxxys pc ppdppdppd求 3 式中，2222222232422xyzpppaaddka 3.3 Kinematics Equation of PUMA 560613.3.2 Motion Synthesis o

48、f PUMA 5603.求 201034531236445566,()()()TTTTT 1 231 23232 31 231 23232 336112000010001xxxxyyyyzzzzc cs csa cnoapc ss sca snoapTnoapscd1 231 23232 331 231 23232 34xyzxyzc c ps c ps pa cac s ps s pc pa sd(3.75)两边(1,4)项和(2,4)项元素对应相等：3.3 Kinematics Equation of PUMA 560623.3.2 Motion Synthesis of PUMA 560

49、求根据 解的四种可能组合可以得到相应的四种可能值 ，于是可得到 的四种可能解：式中，取与 相对应的值。231和2323232(3.78)2332 3112 3423221142 3112 33232211232332 3112 3442 3112 33atan2,zxyzxyzxyzxyzxyzxyaa cpc ps pa sdspc ps pda cpc ps pa cacpc ps paa cpc ps pa sdda cpc ps pa ca(3.77)3.3 Kinematics Equation of PUMA 560633.3.2 Motion Synthesis of PUMA

50、5604.求 401034531236445566,()()()TTTTT 1 231 23232 31 231 23232 336112000010001xxxxyyyyzzzzc cs csa cnoapc ss sca snoapTnoapscd1 231 23234 5114 5xyzxyc c as c as ac ss ac as s(3.75)两边(1,3)项和(3,3)项元素对应相等：4111 231 2323atan2(,)xyxyza sa ca c ca s ca s(3.79)3.3 Kinematics Equation of PUMA 560643.3.2 Moti