机器人学基础_第3章_机器人运动学

null机器人学基础 第三章 机器人运动学中南大学 蔡自兴，谢 斌 zxcai, xiebin@mail.csu.edu.cn 2010机器人学基础 第三章 机器人运动学*Ch.3 Kinematics of RobotsFundamentals of Robotics Fundamentals of Robotics3.0 Introduction to Robot Kinematics3.0 Introduction to Robot KinematicsKinematics 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 position variables (with respect to time or any other variable). 从几何学的观点来处理手指位置P与关节变量L1, L2, 和 的关系称为运动学(Kinematics)。* 3.0 Introduction to Robot Kinematics3.0 Introduction to Robot Kinematics3.0 Introduction to Robot KinematicsIn manipulator robotics, there are two kinematics tasks: Direct (also forward) 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.0 Introduction to Robot KinematicsExample of Direct KinematicsExample of Direct KinematicsDefine position of end effector and the joint variable， According to geometry：The general vector form* 3.0 Introduction to Robot KinematicsExample of Inverse KinematicsExample of Inverse Kinematics式中同样，如果用向量 表 关于同志近三年现实表现材料材料类招标技术评分表图表与交易pdf视力表打印pdf用图表说话 pdf 示上述关系式，其一般可表示为* 3.0 Introduction to Robot KinematicsExample of Inverse KinematicsExample of Inverse Kinematics机器人到达给定的手爪位置 P 有两个姿态满足 要求 对教师党员的评价套管和固井爆破片与爆破装置仓库管理基本要求三甲医院都需要复审吗 ，即图中的 也是其解。此时 和 变成为另外的值，即逆运动学的解不是惟一的。 将运动学公式 两边微分即可得到机器人手爪的速度和关节速度的关系，再进一步进行微分将得到加速度之间的关系，处理这些关系也是机器人的运动学问题。 * 3.0 Introduction to Robot KinematicsnullMechanics of a manipulator 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 motions of each link with respect to the previous one.*3.1 Representation of Kinematics Equation of Robot Manipulator3.1 Representation of Kinematics Equation of Manipulatornull* 机械手是一系列由关节连接起来的连杆构成的。为机械手的每一连杆建立一个坐标系，并用齐次变换来描述这些坐标系间的相对位置和姿态。 A矩阵：一个描述两连杆间坐标系相对关系的齐次变换 ，如；各 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 Manipulatornull*T矩阵：A矩阵的乘积 。 对于六连杆机械手，有下列T矩阵 ： 一个六连杆机械手可具有六个自由度，每个连杆含有一个自由度，并能在其运动范围内任意定位与定向。3.1 Representation of Kinematics Equation of Manipulator3.1 Representation of Kinematics Equation of Robot Manipulator3.1 Representation of Kinematics Equation of Robot Manipulator *3.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：x向矢量， Forming a right-hand frame： n = o  a or a = n  o3.1 Representation of Kinematics Equation of Manipulator3.1.1 Kinetic Pose and Oriented Angle*3.1.1 Kinetic Pose and Oriented Angle因此，变换T6具有下列元素（同式2.35）。 六连杆机械手的T 矩阵（ T6 ）可由指定其16个元素的数值来决定。在这16个元素中，只有12个元素具有实际含义。 3.1 Representation of Kinematics Equation of Manipulator3.1.1 Kinetic Pose and Oriented Angle*3.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.1 Representation of Kinematics Equation of Manipulator3.1.1 Kinetic Pose and Oriented Angle*3.1.1 Kinetic Pose and Oriented AngleRoll, Pitch, Yaw to represent motion pose 另一种常用的旋转集合是横滚（roll）、俯仰（pitch）和偏转（yaw）。 3.1 Representation of Kinematics Equation of Manipulator3.1.1 Kinetic Pose and Oriented Angle*3.1.1 Kinetic Pose and Oriented Angle 对于旋转次序，规定： 式中，RPY表示横滚、俯仰和偏转三旋转的组合变换。也就是说，先绕 x 轴旋转角 ψ，再绕 y 轴旋转角θ，最后绕 z 轴旋角ф 。 3.1 Representation of Kinematics Equation of Manipulator3.1 Representation of Kinetic Equation of Robot Manipulator *3.1 Representation of Kinetic Equation of Robot Manipulator 3.1.2 Kinetic Position and Coordinate 运动位置和坐标 一旦机械手的运动姿态由某个姿态变换规定之后， 它在基系中的位置就能够由左乘一个对应于矢量 p的平移 变换来确定（参式2.20）： 3.1 Representation of Kinematics Equation of Manipulator3.1.2 Kinetic Position and Coordinate*3.1.2 Kinetic Position and CoordinateDescription in Cylinder Coordinates 用柱面坐标来表示机械手手臂的位置，即表 示其平移变换。这对应于沿 x 轴平移 r，再绕 z 轴 旋转，最后沿 z 轴平移 z。如图3.4(a)所示。 3.1 Representation of Kinematics Equation of Manipulator3.1.2 Kinetic Position and Coordinate*3.1.2 Kinetic Position and CoordinateDescription in Spherical Coordinates 用球面坐标表示手臂运动位置矢量的方法，对应于沿 z 轴平移 r，再绕 y 轴旋转β角，最后绕 z 轴旋转  角，如图3.4(b)所示，即为：3.1 Representation of Kinematics Equation of Manipulator3.1 Representation of Kinetic Equation of Robot Manipulator *3.1 Representation of Kinetic Equation of Robot Manipulator 3.1.3 T-Matrix and A-Matrix 连杆变换矩阵及其乘积 广义连杆 相邻坐标系间及其相应连杆可以用齐次变换矩阵来表示。要求解操作手所需要的变换矩阵，每个连杆都要用广义连杆来描述。在求得相应的广义变换矩阵之后，可对其加以修正，以适合每个具体的连杆。 3.1 Representation of Kinematics Equation of Manipulator3.1.3 T-Matrix and A-Matrix*3.1.3 T-Matrix and A-Matrix机器人机械手是由一系列连接在一起的连杆（杆件）构成的。需要用两个参数来描述一个连杆，即公共法线距离 所在平面内两轴的夹角 ；需要另外两个参数来表示相邻两杆的关系，即两连杆的相对位置 和两连杆法线的夹角 ，如图3.5所示。3.1 Representation of Kinematics Equation of Manipulator3.1.3 T-Matrix and A-Matrix*3.1.3 T-Matrix and A-Matrix3.1 Representation of Kinematics Equation of Manipulatorai-1: Link Length - mutual perpendicular unique except for parallel axis : Link Twist - measured in the right-hand sense about3.1.3 T-Matrix and A-Matrix*3.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 (转动关节)3.1.3 T-Matrix and A-Matrix3.1.3 T-Matrix and A-MatrixDenavit-Hartenberg Parameters 4 D-H parameters 3 fixed link parameters 1 joint variable αi and ai : describe the Link i di and θi : describe the Link’s connection *3.1 Representation of Kinematics Equation of Manipulator3.1.3 T-Matrix and A-Matrix3.1.3 T-Matrix and A-Matrix*3.1 Representation of Kinematics Equation of Manipulatory-vectors: complete right-hand frames3.1.3 T-Matrix and A-Matrix3.1.3 T-Matrix and A-Matrix*3.1 Representation of Kinematics Equation of Manipulator1. Normals 2. Origins3. Z-axes 4. X-axes3.1.3 T-Matrix and A-Matrix3.1.3 T-Matrix and A-Matrix*3.1 Representation of Kinematics Equation of Manipulator3.1.3 T-Matrix and A-Matrix3.1.3 T-Matrix and A-Matrix*3.1 Representation of Kinematics Equation of ManipulatorExample-RRR Arm3.1.3 T-Matrix and A-Matrix3.1.3 T-Matrix and A-Matrix*3.1 Representation of Kinematics Equation of ManipulatorExample-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 zi3.1.3 T-Matrix and A-Matrix3.1.3 T-Matrix and A-Matrix*3.1 Representation of Kinematics Equation of ManipulatorExample-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 zi3.1.3 T-Matrix and A-Matrix*3.1.3 T-Matrix and A-MatrixDenavit-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 Manipulator3.1.3 T-Matrix and A-Matrix*3.1.3 T-Matrix and A-Matrix 绕 轴旋转 角，使 轴转到与 同一平面内。 沿 轴平移一距离 ，把 移到与 同一直线上。 沿 xi 轴平移一距离 ai-1 ，使连杆 坐标系的原点与 连杆 i 的坐标系原点重合。 (4) 绕 xi 轴旋转 角，使 zi－1 转到与 zi 同一直线上。3.1 Representation of Kinematics Equation of Manipulator3.1.3 T-Matrix and A-Matrix3.1.3 T-Matrix and A-Matrix这种关系可由表示连杆相对位置的四个齐次变换来描述，并叫做 矩阵。此关系式为： (3.12) 展开上式可得 ： （3.13） *3.1 Representation of Kinematics Equation of Manipulator3.1.3 T-Matrix and A-Matrix*3.1.3 T-Matrix and A-MatrixUsing A-Matrix to represent T-Matrix 机械手的末端装置即为连杆6的坐标系，它与连杆 坐标系的关系可由 表示为： 可得连杆变换通式为 ：3.1 Representation of Kinematics Equation of Manipulator3.2 Solving Kinematical Equation of Robot Manipulator 机械手运动学方程的求解*3.2 Solving Kinematical Equation of Robot Manipulator 机械手运动学方程的求解大多数机器人程序 设计 领导形象设计圆作业设计ao工艺污水处理厂设计附属工程施工组织设计清扫机器人结构设计 语言使用某个笛卡儿坐标系来指定机械手的末端位置。这一指定可用于求解机械手最后一个连杆的姿态 。不过，在机械手能够被驱动至这个姿态之前，必须知道与这个位置有关的所有关节的位置。 求解运动方程时，我们从 开始求解关节位置。使 的符号表达式的各元素等于 T6 的一般形式，并据此确定 。其它五个关节参数不可能从T6 求得，因为所求得的运动方程过于复杂而无法求解它们。我们可以由上节讨论的其它T 矩阵来求解它们。一旦求得 之后，可由 左乘 的一般形式，得： (3.18) 式中，左边为 和 各元的 函数 excel方差函数excelsd函数已知函数     2 f x m x mx m      2 1 4 2拉格朗日函数pdf函数公式下载 。此式可用来求解其它各 关节变量，如 等。 3.2 Solving Kinematics Equation3.2 Solving Kinematics Equation3.2 Solving Kinematics Equation 不断地用的逆矩阵左乘式(3.17)，可得 下列4个矩阵方程式： (3.19) (3.20) (3.21) (3.22) 上列各方程的左式为 和前 个关节变 量的函数。可用这些方程来确定各关节的 位置。* 3.2 Solving Kinematics Equation3.2 Solving Kinematics Equation*3.2.1 Solution of the Euler Transformation 欧拉变换解 基本隐式方程的解 令 由式(3.4)和(3.23)得到： 3.2 Solving Kinematics Equation3.2 Solving Kinematics Equation3.2.1 Solution of the Euler Transformation*3.2.1 Solution of the Euler Transformation令矩阵方程两边各对应元素一一相等，可得到9个隐式方 程如下： 3.2 Solving Kinematics Equation3.2.1 Solution of the Euler Transformation*3.2.1 Solution of the Euler Transformation 但这些解答是不确定的： （1）当由余弦函数求角度时，不仅此角度的符号是不确定的，并且所求角度的准确度也与该角度本身相关。 （2）在求解 和 时，再次用到反余弦函数，且除式的分母为 。当 接近于0时，总会产生不准确。 （3）当 或 时， 和 的求解公式无定义。 3.2 Solving Kinematics Equation3.2.1 Solution of the Euler Transformation *3.2.1 Solution of the Euler Transformation 在求解时，总是采用双变量反正切函数atan2来确定角度。atan2提供二个自变量，即纵坐标和横坐标，见图3.8。当 -π≤θ≤ π，由atan2反求角度时，同时检查y和x的符号来确定其所在象限。这一函数也能检验什么时候x或y为0，并反求出正确的角度。atan2的精确程度对其整个定义域都是一样的。 3.2 Solving Kinematics Equation用双变量反正切函数(two-argument arc tangent function) 确定角度3.2.1 Solution of the Euler Transformation*3.2.1 Solution of the Euler Transformation用显式方程求各角度 要求得方程式的解，采用另一种通常能够导致显式解答的方法。用未知逆变换依次左乘已知方程，对于欧拉变换有： 式(3.37)的左式为已知变换的函数，而右式各元素或者为0，或者为常数。 3.2 Solving Kinematics Equationnull* 对方程求解，整理之后确定其等价欧拉角： 如果已知一个表示任意旋转的齐次变换，那么就能够确定其等价欧拉角。3.2.1 Solution of the Euler Transformation 3.2 Solving Kinematics Equation3.2.2 Solution of RPY Transformations 滚、仰、偏变换解 *3.2.2 Solution of RPY Transformations 滚、仰、偏变换解 直接从显式方程来求解用滚动、俯仰和偏转表示的变换方程。 推导计算可得RPY变换各角如下： 3.2 Solving Kinematics Equation3.2.3 Solution of Spherical Coordinate Transformation 球面变换解*3.2.3 Solution of Spherical Coordinate Transformation 球面变换解把求解滚、仰和偏变换方程的技术用于球面坐标表示的运动方程。 可推导出球面变换的解为： 3.2 Solving Kinematics Equation3.3 Kinematic Equation of PUMA 560 PUMA 560 机器人运动方程 *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 5603.3.1 Motion Analysis of PUMA 560 *3.3.1 Motion Analysis of PUMA 560 3.3 Kinematics Equation of PUMA 5603.3.1 Motion Analysis of PUMA 560 *3.3.1 Motion Analysis of PUMA 560 PUMA560每个关节均有角度零位与正负方向限位开关，机器人的回转机体实现机器人机体绕z0轴的回转（角θ1），它由固定底座和回转工作台组成。安装在轴中心的驱动电机经传动装置，可以实现工作台的回转。 3.3 Kinematics Equation of PUMA 5603.3.1 Motion Analysis of PUMA 560 *3.3.1 Motion Analysis of PUMA 560 大臂、小臂的平衡由机器人中的平衡装置控制，在机器人的回转工作台上安装有大臂台座，将大臂下端关节支承在台座上，大臂的上端关节用于支承小臂。大臂臂体的下端安有直流伺服电机，可控制大臂上下摆动（角 θ2 ）。 3.3 Kinematics Equation of PUMA 5603.3.1 Motion Analysis of PUMA 560 *3.3.1 Motion Analysis of PUMA 560 小臂支承于大臂臂体的上关节处，其驱动电机可带动小臂做上下俯仰（角θ3），以及小臂的回转（θ4）。 3.3 Kinematics Equation of PUMA 5603.3.1 Motion Analysis of PUMA 560 *3.3.1 Motion Analysis of PUMA 560 机器人的腕部位于小臂臂体前端，通过伺服电动机传动，可实现腕部摆动（θ5）和转动（θ6）。 3.3 Kinematics Equation of PUMA 5603.3.1 Motion Analysis of PUMA 560 *3.3.1 Motion Analysis of PUMA 560 3.3 Kinematics Equation of PUMA 5603.3.1 Motion Analysis of PUMA 560 *Using A-Matrix to represent T-Matrix 机械手的末端装置即为连杆6的坐标系，它与连杆 坐标系的关系可由 表示为： 可得连杆变换通式为 ：3.3.1 Motion Analysis of PUMA 560 3.3 Kinematics Equation of PUMA 5603.3.1 Motion Analysis of PUMA 560 *3.3.1 Motion Analysis of PUMA 560 据连杆变换通式式(3.16)和表3.1所示连杆参数，可求得各连杆变换矩阵如下： 3.3 Kinematics Equation of PUMA 5603.3.1 Motion Analysis of PUMA 560 *3.3.1 Motion Analysis of PUMA 560 各连杆变换矩阵相乘，得PUMA 560的机械手变换的T 矩阵： 即为关节变量 的函数。 该矩阵描述了末端连杆坐标系{6}相对基坐标系{0}的位姿。 3.3 Kinematics Equation of PUMA 5603.3.1 Motion Analysis of PUMA 560 * 要求解此运动方程，需先计算某些中间结果4T6 ， 3T6 ， 1T3 ， 1T6 等，见式（3.60）至（3.63）。 于是，可求得机械手的T 变换矩阵： 其中nx, 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 5603.3.1 Motion Analysis of PUMA 560 * （3.64）3.3.1 Motion Analysis of PUMA 560 3.3 Kinematics Equation of PUMA 5603.3.2 Motion Synthesis of PUMA 560 PUMA 560运动综合（求解）*3.3.2 Motion Synthesis of PUMA 560 PUMA 560运动综合（求解）据式（3.59），可把PUMA 560的运动方程（3.64）写为： 若末端连杆的位姿已经给定，即 为已知，则求关节变 量 的值称为运动反解。用未知的连杆逆变换左乘方 程(3.65)两边，把关节变量分离出来，从而求得 的 解。 3.3 Kinematics Equation of PUMA 5603.3.2 Motion Synthesis of PUMA 560*3.3.2 Motion Synthesis of PUMA 5601.求 用逆变换 左乘式(3.65)两边： 3.3 Kinematics Equation of PUMA 5603.3.2 Motion Synthesis of PUMA 560*3.3.2 Motion Synthesis of PUMA 5601.求 利用三角代换：其中两边(2,4)项元素对应相等： 3.3 Kinematics Equation of PUMA 5603.3.2 Motion Synthesis of PUMA 560*3.3.2 Motion Synthesis of PUMA 560 求 式中，正、负号对应于 的两个可能解。 3.3 Kinematics Equation of PUMA 5603.3.2 Motion Synthesis of PUMA 560*3.3.2 Motion Synthesis of PUMA 560 2. 求 两边(1,4)项和(3,4)项元素对应相等：其中 3.3 Kinematics Equation of PUMA 5603.3.2 Motion Synthesis of PUMA 560*3.3.2 Motion Synthesis of PUMA 560 求 正、负号对应 的两种可能解。 求 式中， 3.3 Kinematics Equation of PUMA 5603.3.2 Motion Synthesis of PUMA 560*3.3.2 Motion Synthesis of PUMA 5603.求 两边(1,4)项和(2,4)项元素对应相等： 3.3 Kinematics Equation of PUMA 5603.3.2 Motion Synthesis of PUMA 560*3.3.2 Motion Synthesis of PUMA 560求 根据 解的四种可能组合可以得到相应的四种可能值 ， 于是可得到 的四种可能解： 式中， 取与 相对应的值。 3.3 Kinematics Equation of PUMA 5603.3.2 Motion Synthesis of PUMA 560*3.3.2 Motion Synthesis of PUMA 5604.求 两边(1,3)项和(3,3)项元素对应相等： 3.3 Kinematics Equation of PUMA 5603.3.2 Motion Synthesis of PUMA 560*3.3.2 Motion Synthesis of PUMA 5605.求 两边(1,3)项和(3,3)项元素对应相等： 3.3 Kinematics Equation of PUMA 5603.3.2 Motion Synthesis of PUMA 560*3.3.2 Motion Synthesis of PUMA 5606.求 两边(3,1)项和(1,1)项元素对应相等： 3.3 Kinematics Equation of PUMA 5603.3.2 Motion Synthesis of PUMA 560*3.3.2 Motion Synthesis of PUMA 560For each solution pictured, there 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 5603.3.2 Motion Synthesis of PUMA 560*3.3.2 Motion Synthesis of PUMA 560PUMA560的运动反解可能存在8种解。但是，由于结构的限制，例如各关节变量不能在全部360°范围内运动，有些解不能实现。 在机器人存在多种解的情况下，应选取其中最满意的一组解，以满足机器人的工作要求。Four solutions of the PUMA560 3.3 Kinematics Equation of PUMA 560 3.4 Summary 小结* 3.4 Summary 小结Representation of Kinematics Equation (Analysis) Motion Orientation of Manipulators Euler Angle -> Motion Pose Roll, Pitch, Yaw -> Motion Pose Solving Kinematics Equation (Synthesis) Solution of the Euler Transformation Solution of Roll, Pitch and Yaw Solution of Spherical Coordinate Transformation Motion Analysis and Synthesis of PUMA560 3.4 Summary