1、中南大学蔡自兴，谢 斌zxcai, 2010,机器人学基础第三章 机器人运动学,1,Fundamentals of Robotics,3.0 Introduction to Robot Kinematics,Kinematics treats motion without regard to the forces that cause it. Within the science of kinematics one studies the position, velocity, acceleration, and all higher order derivatives of the posi
2、tion variables (with respect to time or any other variable).,从几何学的观点来处理手指位置P与关节变量L1, L2, 和 的关系称为运动学(Kinematics)。,2,3.0 Introduction to Robot Kinematics,In manipulator robotics, there are two kinematics tasks:Direct (also forward) kinematics Given are joint relations (rotations, translations) for the
3、 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 Introduction to Robot Kinematics,3.0 Introduction to Robot Kinematics,Examp
4、le of Direct Kinematics,Define position of end effector and the joint variable，According to geometry：,The general vector form,4,3.0 Introduction to Robot Kinematics,式中,同样，如果用向量表示上述关系式，其一般可表示为,Example of Inverse Kinematics,5,3.0 Introduction to Robot Kinematics,机器人到达给定的手爪位置 P有两个姿态满足要求，即图中的 也是其解。此时 和
5、变成为另外的值，即逆运动学的解不是惟一的。,将运动学公式 两边微分即可得到机器人手爪的速度和关节速度的关系，再进一步进行微分将得到加速度之间的关系，处理这些关系也是机器人的运动学问题。,Example of Inverse Kinematics,6,3.0 Introduction to Robot Kinematics,7,3.1 Representation of Kinematics Equation of Robot Manipulator,3.1 Representation of Kinematics Equation of Manipulator,Mechanics of a m
6、anipulator can be represented as a kinematics chain of rigid bodies (links) connected by revolute or prismatic joints.One end of the 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 motion
7、s of each link with respect to the previous one.,8,机械手是一系列由关节连接起来的连杆构成的。为机械手的每一连杆建立一个坐标系，并用齐次变换来描述这些坐标系间的相对位置和姿态。A矩阵：一个描述两连杆间坐标系相对关系的齐次变换 ，如；各 A 矩阵的乘积称为 T 矩阵 。例如： A1，A2，A3 T1=A1 T2=A1A2 T3=A1A2A3 ,3.1 Representation of Kinematics Equation of Robot Manipulator,3.1 Representation of Kinematics Equatio
8、n of Manipulator,9,T矩阵：A矩阵的乘积 。 对于六连杆机械手，有下列T矩阵 ： 一个六连杆机械手可具有六个自由度，每个连杆含有一个自由度，并能在其运动范围内任意定位与定向。,3.1 Representation of Kinematics Equation of Manipulator,3.1 Representation of Kinematics Equation of Robot Manipulator,10,3.1 Representation of Kinematics Equation of Robot Manipulator,3.1.1 Kinetic Pos
9、e and Oriented Angle 运动姿态和方向角Motion Direction原点由矢量p表示。approach vector a：z向矢量orientation vector o：y向矢量normal vector n：x向矢量， Forming a right-hand frame： n = o a or a = n o,3.1 Representation of Kinematics Equation of Manipulator,11,3.1.1 Kinetic Pose and Oriented Angle,因此，变换T6具有下列元素（同式2.35）。 六连杆机械手的T
10、矩阵（ T6 ）可由指定其16个元素的数值来决定。在这16个元素中，只有12个元素具有实际含义。,3.1 Representation of Kinematics Equation of Manipulator,12,3.1.1 Kinetic Pose and Oriented Angle,Euler angle to represent motion pose机械手的运动姿态往往由一个绕轴x ，y 和 z 的旋转序列来规定。这种转角的序列，称为欧拉（Euler）角。欧拉角: 用一个绕 z 轴旋转角，再绕新的 y 轴 y旋转角，最后绕新的 z 轴z旋转角来描述任 何可能的姿态。欧拉变换Eul
11、er可由连乘三个旋转矩阵来求得，即 （3.3）,3.1 Representation of Kinematics Equation of Manipulator,13,3.1.1 Kinetic Pose and Oriented Angle,Roll, Pitch, Yaw to represent motion pose 另一种常用的旋转集合是横滚（roll）、俯仰（pitch）和偏转（yaw）。,3.1 Representation of Kinematics Equation of Manipulator,14,3.1.1 Kinetic Pose and Oriented Angle
12、,对于旋转次序，规定：式中，RPY表示横滚、俯仰和偏转三旋转的组合变换。也就是说，先绕 x 轴旋转角 ，再绕 y 轴旋转角，最后绕 z 轴旋角 。,3.1 Representation of Kinematics Equation of Manipulator,15,3.1 Representation of Kinetic Equation of Robot Manipulator,3.1.2 Kinetic Position and Coordinate 运动位置和坐标 一旦机械手的运动姿态由某个姿态变换规定之后，它在基系中的位置就能够由左乘一个对应于矢量 p的平移变换来确定（参式2.20
13、）：,3.1 Representation of Kinematics Equation of Manipulator,16,3.1.2 Kinetic Position and Coordinate,Description in Cylinder Coordinates 用柱面坐标来表示机械手手臂的位置，即表示其平移变换。这对应于沿 x 轴平移 r，再绕 z 轴旋转，最后沿 z 轴平移 z。如图3.4(a)所示。,3.1 Representation of Kinematics Equation of Manipulator,17,3.1.2 Kinetic Position and Coo
14、rdinate,Description in Spherical Coordinates 用球面坐标表示手臂运动位置矢量的方法，对应于沿 z 轴平移 r，再绕 y 轴旋转角，最后绕 z 轴旋转 角，如图3.4(b)所示，即为：,3.1 Representation of Kinematics Equation of Manipulator,18,3.1 Representation of Kinetic Equation of Robot Manipulator,3.1.3 T-Matrix and A-Matrix 连杆变换矩阵及其乘积 广义连杆 相邻坐标系间及其相应连杆可以用齐次变换矩阵来
15、表示。要求解操作手所需要的变换矩阵，每个连杆都要用广义连杆来描述。在求得相应的广义变换矩阵之后，可对其加以修正，以适合每个具体的连杆。,3.1 Representation of Kinematics Equation of Manipulator,19,3.1.3 T-Matrix and A-Matrix,机器人机械手是由一系列连接在一起的连杆（杆件）构成的。需要用两个参数来描述一个连杆，即公共法线距离 所在平面内两轴的夹角 ；需要另外两个参数来表示相邻两杆的关系，即两连杆的相对位置 和两连杆法线的夹角 ，如图3.5所示。,3.1 Representation of Kinematics
16、Equation of Manipulator,20,3.1.3 T-Matrix and A-Matrix,3.1 Representation of Kinematics Equation of Manipulator,ai-1: Link Length - mutual perpendicular unique except for parallel axis : Link Twist - measured in the right-hand sense about,21,3.1.3 T-Matrix and A-Matrix,3.1 Representation of Kinemati
17、cs Equation of Manipulator,di: Link Offset - variable if joint i is prismatic (平动关节) : Joint Angle - variable if joint i is revolute (转动关节),22,3.1.3 T-Matrix and A-Matrix,Denavit-Hartenberg Parameters4 D-H parameters3 fixed link parameters1 joint variablei and ai : describe the Link idi and i : desc
18、ribe the Links connection,3.1 Representation of Kinematics Equation of Manipulator,23,3.1.3 T-Matrix and A-Matrix,3.1 Representation of Kinematics Equation of Manipulator,y-vectors: complete right-hand frames,24,3.1.3 T-Matrix and A-Matrix,3.1 Representation of Kinematics Equation of Manipulator,1.
19、Normals 2. Origins,3. Z-axes4. X-axes,25,3.1.3 T-Matrix and A-Matrix,3.1 Representation of Kinematics Equation of Manipulator,26,3.1.3 T-Matrix and A-Matrix,3.1 Representation of Kinematics Equation of Manipulator,Example-RRR Arm,27,3.1.3 T-Matrix and A-Matrix,3.1 Representation of Kinematics Equati
20、on of Manipulator,Example-RRR Arm,= 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 zi,28,3.1.3 T-Matrix and A-Matrix,3.1 Representation of Kinematics Equation of Manipulator,Example-RRR Arm,= distance from zi to z
21、i+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 zi,29,3.1.3 T-Matrix and A-Matrix,Denavit-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 t
22、o xi about zi,3.1 Representation of Kinematics Equation of Manipulator,30,3.1.3 T-Matrix and A-Matrix,绕 轴旋转 角，使 轴转到与 同一平面内。 沿 轴平移一距离 ，把 移到与 同一直线上。 沿 xi 轴平移一距离 ai-1 ，使连杆 坐标系的原点与 连杆 i 的坐标系原点重合。(4) 绕 xi 轴旋转 角，使 zi1 转到与 zi 同一直线上。,3.1 Representation of Kinematics Equation of Manipulator,3.1.3 T-Matrix an
23、d A-Matrix,这种关系可由表示连杆相对位置的四个齐次变换来描述，并叫做 矩阵。此关系式为： (3.12)展开上式可得 ： （3.13）,31,3.1 Representation of Kinematics Equation of Manipulator,32,3.1.3 T-Matrix and A-Matrix,Using A-Matrix to represent T-Matrix机械手的末端装置即为连杆6的坐标系，它与连杆 坐标系的关系可由 表示为：可得连杆变换通式为 ：,3.1 Representation of Kinematics Equation of Manipula
24、tor,33,3.2 Solving Kinematical Equation of RobotManipulator 机械手运动学方程的求解,大多数机器人程序设计语言使用某个笛卡儿坐标系来指定机械手的末端位置。这一指定可用于求解机械手最后一个连杆的姿态 。不过，在机械手能够被驱动至这个姿态之前，必须知道与这个位置有关的所有关节的位置。求解运动方程时，我们从 开始求解关节位置。使 的符号表达式的各元素等于 T6 的一般形式，并据此确定 。其它五个关节参数不可能从T6 求得，因为所求得的运动方程过于复杂而无法求解它们。我们可以由上节讨论的其它T 矩阵来求解它们。一旦求得 之后，可由 左乘 的一般
25、形式，得： (3.18) 式中，左边为 和 各元的函数。此式可用来求解其它各关节变量，如 等。,3.2 Solving Kinematics Equation,3.2 Solving Kinematics Equation,不断地用的逆矩阵左乘式(3.17)，可得下列4个矩阵方程式： (3.19) (3.20) (3.21) (3.22)上列各方程的左式为 和前 个关节变量的函数。可用这些方程来确定各关节的位置。,34,3.2 Solving Kinematics Equation,35,3.2.1 Solution of the Euler Transformation 欧拉变换解基本隐式方
26、程的解 令由式(3.4)和(3.23)得到：,3.2 Solving Kinematics Equation,3.2 Solving Kinematics Equation,36,3.2.1 Solution of the Euler Transformation,令矩阵方程两边各对应元素一一相等，可得到9个隐式方程如下：,3.2 Solving Kinematics Equation,37,3.2.1 Solution of the Euler Transformation,但这些解答是不确定的：（1）当由余弦函数求角度时，不仅此角度的符号是不确定的，并且所求角度的准确度也与该角度本身相关。
27、（2）在求解 和 时，再次用到反余弦函数，且除式的分母为 。当 接近于0时，总会产生不准确。（3）当 或 时， 和 的求解公式无定义。,3.2 Solving Kinematics Equation,38,3.2.1 Solution of the Euler Transformation,在求解时，总是采用双变量反正切函数atan2来确定角度。atan2提供二个自变量，即纵坐标和横坐标，见图3.8。当 - ，由atan2反求角度时，同时检查y和x的符号来确定其所在象限。这一函数也能检验什么时候x或y为0，并反求出正确的角度。atan2的精确程度对其整个定义域都是一样的。,3.2 Solvin
28、g Kinematics Equation,用双变量反正切函数(two-argument arc tangent function) 确定角度,39,3.2.1 Solution of the Euler Transformation,用显式方程求各角度 要求得方程式的解，采用另一种通常能够导致显式解答的方法。用未知逆变换依次左乘已知方程，对于欧拉变换有： 式(3.37)的左式为已知变换的函数，而右式各元素或者为0，或者为常数。,3.2 Solving Kinematics Equation,40,对方程求解，整理之后确定其等价欧拉角： 如果已知一个表示任意旋转的齐次变换，那么就能够确定其等价
29、欧拉角。,3.2.1 Solution of the Euler Transformation,3.2 Solving Kinematics Equation,41,3.2.2 Solution of RPY Transformations 滚、仰、偏变换解,直接从显式方程来求解用滚动、俯仰和偏转表示的变换方程。 推导计算可得RPY变换各角如下：,3.2 Solving Kinematics Equation,42,3.2.3 Solution of Spherical Coordinate Transformation 球面变换解,把求解滚、仰和偏变换方程的技术用于球面坐标表示的运动方程。
30、可推导出球面变换的解为：,3.2 Solving Kinematics Equation,43,3.3 Kinematic Equation of PUMA 560 PUMA 560 机器人运动方程,3.3.1 Motion Analysis of PUMA 560 PUMA 560 运动分析（表示）PUMA 560是属于关节式机器人，6个关节都是转动关节。前3个关节确定手腕参考点的位置，后3个关节确定手腕的方位。各连杆坐标系如图3.9所示。相应的连杆参数列于表3.1。,3.3 Kinematics Equation of PUMA 560,44,3.3.1 Motion Analysis o
31、f PUMA 560,3.3 Kinematics Equation of PUMA 560,45,3.3.1 Motion Analysis of PUMA 560,PUMA560每个关节均有角度零位与正负方向限位开关，机器人的回转机体实现机器人机体绕z0轴的回转（角1），它由固定底座和回转工作台组成。安装在轴中心的驱动电机经传动装置，可以实现工作台的回转。,3.3 Kinematics Equation of PUMA 560,46,3.3.1 Motion Analysis of PUMA 560,大臂、小臂的平衡由机器人中的平衡装置控制，在机器人的回转工作台上安装有大臂台座，将大臂下端
32、关节支承在台座上，大臂的上端关节用于支承小臂。大臂臂体的下端安有直流伺服电机，可控制大臂上下摆动（角 2 ）。,3.3 Kinematics Equation of PUMA 560,47,3.3.1 Motion Analysis of PUMA 560,小臂支承于大臂臂体的上关节处，其驱动电机可带动小臂做上下俯仰（角3），以及小臂的回转（4）。,3.3 Kinematics Equation of PUMA 560,48,3.3.1 Motion Analysis of PUMA 560,机器人的腕部位于小臂臂体前端，通过伺服电动机传动，可实现腕部摆动（5）和转动（6）。,3.3 Kine
33、matics Equation of PUMA 560,49,3.3.1 Motion Analysis of PUMA 560,3.3 Kinematics Equation of PUMA 560,50,Using A-Matrix to represent T-Matrix机械手的末端装置即为连杆6的坐标系，它与连杆 坐标系的关系可由 表示为：可得连杆变换通式为 ：,3.3.1 Motion Analysis of PUMA 560,3.3 Kinematics Equation of PUMA 560,51,3.3.1 Motion Analysis of PUMA 560,据连杆变换
34、通式式(3.16)和表3.1所示连杆参数，可求得各连杆变换矩阵如下：,3.3 Kinematics Equation of PUMA 560,52,3.3.1 Motion Analysis of PUMA 560,各连杆变换矩阵相乘，得PUMA 560的机械手变换的T 矩阵： 即为关节变量 的函数。 该矩阵描述了末端连杆坐标系6相对基坐标系0的位姿。,3.3 Kinematics Equation of PUMA 560,53,要求解此运动方程，需先计算某些中间结果4T6 ， 3T6 ， 1T3 ， 1T6 等，见式（3.60）至（3.63）。于是，可求得机械手的T 变换矩阵： 其中nx,
35、ny, nz, ox, oy, oz, ax, ay, az, px, py, pz见式（3.64）,3.3.1 Motion Analysis of PUMA 560,3.3 Kinematics Equation of PUMA 560,54,（3.64）,3.3.1 Motion Analysis of PUMA 560,3.3 Kinematics Equation of PUMA 560,55,3.3.2 Motion Synthesis of PUMA 560 PUMA 560运动综合（求解）,据式（3.59），可把PUMA 560的运动方程（3.64）写为：若末端连杆的位姿已经给
36、定，即 为已知，则求关节变量 的值称为运动反解。用未知的连杆逆变换左乘方程(3.65)两边，把关节变量分离出来，从而求得 的解。,3.3 Kinematics Equation of PUMA 560,56,3.3.2 Motion Synthesis of PUMA 560,1.求 用逆变换 左乘式(3.65)两边：,3.3 Kinematics Equation of PUMA 560,57,3.3.2 Motion Synthesis of PUMA 560,1.求,利用三角代换：,其中,两边(2,4)项元素对应相等：,3.3 Kinematics Equation of PUMA 56
37、0,58,3.3.2 Motion Synthesis of PUMA 560,求 式中，正、负号对应于 的两个可能解。,3.3 Kinematics Equation of PUMA 560,59,3.3.2 Motion Synthesis of PUMA 560,2. 求,两边(1,4)项和(3,4)项元素对应相等：,其中,3.3 Kinematics Equation of PUMA 560,60,3.3.2 Motion Synthesis of PUMA 560,求,正、负号对应 的两种可能解。,求,式中，,3.3 Kinematics Equation of PUMA 560,6
38、1,3.3.2 Motion Synthesis of PUMA 560,3.求,两边(1,4)项和(2,4)项元素对应相等：,3.3 Kinematics Equation of PUMA 560,62,3.3.2 Motion Synthesis of PUMA 560,求根据 解的四种可能组合可以得到相应的四种可能值 ，于是可得到 的四种可能解：式中， 取与 相对应的值。,3.3 Kinematics Equation of PUMA 560,63,3.3.2 Motion Synthesis of PUMA 560,4.求,两边(1,3)项和(3,3)项元素对应相等：,3.3 Kine
39、matics Equation of PUMA 560,64,3.3.2 Motion Synthesis of PUMA 560,5.求,两边(1,3)项和(3,3)项元素对应相等：,3.3 Kinematics Equation of PUMA 560,65,3.3.2 Motion Synthesis of PUMA 560,6.求,两边(3,1)项和(1,1)项元素对应相等：,3.3 Kinematics Equation of PUMA 560,66,3.3.2 Motion Synthesis of PUMA 560,For each solution pictured, ther
40、e is another solution in which the last three joints flip“ to an alternate configuration according to the formulas:,Four solutions of the PUMA560,3.3 Kinematics Equation of PUMA 560,67,3.3.2 Motion Synthesis of PUMA 560,PUMA560的运动反解可能存在8种解。但是，由于结构的限制，例如各关节变量不能在全部360范围内运动，有些解不能实现。在机器人存在多种解的情况下，应选取其中最
41、满意的一组解，以满足机器人的工作要求。,Four solutions of the PUMA560,3.3 Kinematics Equation of PUMA 560,68,3.4 Summary 小结,Representation of Kinematics Equation (Analysis)Motion Orientation of ManipulatorsEuler Angle - Motion PoseRoll, Pitch, Yaw - Motion PoseSolving Kinematics Equation (Synthesis)Solution of the Euler TransformationSolution of Roll, Pitch and Yaw Solution of Spherical Coordinate Transformation Motion Analysis and Synthesis of PUMA560,3.4 Summary,
