机器人学基础ppt课件

1、中南大学中南大学蔡自兴，谢蔡自兴，谢 斌斌zxcai, zxcai, 20192019机器人学根底机器人学根底第三章第三章 机器人运动机器人运动学学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

2、 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)。122 3.0 Introduction to Robot KinematicsIn manipulator robotics, there

3、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 rotation

4、s and orientations to achieve this?3 3.0 Introduction 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 t

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

6、器人的运动学问题。 ( )rfExample of Inverse Kinematics6 3.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) connect

7、ed 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.8 机械手是一系列由关节衔接起来的连杆构成的。为机械手的每一连杆建立一

8、个坐标系，并用齐次变换来描画这些坐标系间的相对位置和姿态。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 Manipulator9T矩阵：A矩阵的乘积 。 对于六连杆机械手，有以下T矩阵 ： 一个六连杆机械手可具有六个自在度，每个连杆含有一个自在度，并能在其运动范围内

9、恣意定位与定向。(3.1)6123456TA A A A A A3.1 Representation of Kinematics 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表示。表

10、示。approach vector a：z向矢量向矢量orientation vector o：y向矢量向矢量normal vector n：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个元素中，只需

11、12个元素具有实践含义。 60001xxxxyyyyzzzznoapnoapTnoap(3.2)3.1 Representation of Kinematics Equation of Manipulator123.1.1 Kinetic Pose and Oriented AngleEuler angle to represent motion pose机械手的运动姿态往往由机械手的运动姿态往往由一个绕轴一个绕轴x ，y 和和 z 的旋转的旋转序列来规定。这种转角的序列来规定。这种转角的序列，称为欧拉序列，称为欧拉Euler角。角。欧拉角欧拉角: 用一个绕用一个绕 z 轴轴旋转旋转角，再绕新

12、的角，再绕新的 y 轴轴 y旋转旋转角，最后绕新的角，最后绕新的 z 轴轴z旋转旋转角来描画任角来描画任 何能够的姿态。何能够的姿态。欧拉变换欧拉变换Euler可由连乘三个旋转矩阵来求得，可由连乘三个旋转矩阵来求得，即即 3.33.2图 欧拉角的定义),(),(),(),(zRotyRotzRotEuler3.1 Representation of Kinematics Equation of Manipulator133.1.1 Kinetic Pose and Oriented AngleRoll, Pitch, Yaw to represent motion pose 另一种常用的旋转集

13、合是横滚另一种常用的旋转集合是横滚roll、俯、俯仰仰pitch和偏转和偏转yaw。 3.3图 用横滚、俯仰和偏转表示机械手运动姿态3.1 Representation of Kinematics Equation of Manipulator143.1.1 Kinetic Pose and Oriented Angle 对于旋转次序，规定：式中，RPY表示横滚、俯仰和偏转三旋转的组合变换。也就是说，先绕 x 轴旋转角 ，再绕 y 轴旋转角，最后绕 z 轴旋角 。 (3.4)( , ,)( , )( , )( ,)RPYRot zRot yRot x 3.1 Representation of

14、 Kinematics Equation of Manipulator153.1 Representation of Kinetic Equation of Robot Manipulator 3.1.2 Kinetic Position and Coordinate 运动位置和坐标运动位置和坐标 一旦机械手的运动姿态由某个姿态变换规定一旦机械手的运动姿态由某个姿态变换规定之后，之后，它在基系中的位置就可以由左乘一个对应于矢量它在基系中的位置就可以由左乘一个对应于矢量 p的平移的平移变换来确定参式变换来确定参式2.20：61000100010001xyzppTp某姿态变换(3.6)3.1 Re

15、presentation 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(),(rTrans

16、zRotzTransrzCyl3.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 Equat

17、ion of Manipulator183.1 Representation of Kinetic Equation of Robot Manipulator 3.1.3 T-Matrix and A-Matrix 连杆变换矩阵及其乘积连杆变换矩阵及其乘积 广义连杆广义连杆 相邻坐标系间及其相应连杆可以用齐次变相邻坐标系间及其相应连杆可以用齐次变换矩阵来表示。要求解操作手所需求的变换矩换矩阵来表示。要求解操作手所需求的变换矩阵，每个连杆都要用广义连杆来描画。在求得阵，每个连杆都要用广义连杆来描画。在求得相应的广义变换矩阵之后，可对其加以修正，相应的广义变换矩阵之后，可对其加以修正，以适宜每个详

18、细的连杆。以适宜每个详细的连杆。 3.1 Representation of Kinematics Equation of Manipulator193.1.3 T-Matrix and A-Matrix机器人机械手是由一系列衔接在一同的连杆杆件构成的。需求用两个参数来描画一个连杆，即公共法线间隔 所在平面内两轴的夹角 ；需求另外两个参数来表示相邻两杆的关系，即两连杆的相对位置 和两连杆法线的夹角 ，如图3.5所示。3.5图 转动关节连杆四参数示意图iiaa 和垂直于iidi3.1 Representation of Kinematics Equation of Manipulator203.

19、1.3 T-Matrix and A-Matrixii3.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 about1ia1i213.1.3 T-Matrix and A-Matrix3.1 Representation of Kinematics Equation of Manipulatord

20、i: Link Offset - variable if joint i is prismatic (平动关节) : Joint Angle - variable if joint i is revolute (转动关节)i223.1.3 T-Matrix 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 R

21、epresentation of Kinematics Equation of Manipulator,iiiia drevolute jointprismatic jointiid233.1.3 T-Matrix and A-Matrix3.1 Representation of Kinematics Equation of Manipulatory-vectors: complete right-hand frames243.1.3 T-Matrix and A-Matrix3.1 Representation of Kinematics Equation of Manipulator1.

22、 Normals 2. Origins3. Z-axes4. X-axes253.1.3 T-Matrix and A-Matrix3.1 Representation of Kinematics Equation of Manipulator263.1.3 T-Matrix 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 Mani

23、pulatorExample-RRR Armlinkdi1231i1iaiiidiia= 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 zi283.1.3 T-Matrix and A-Matrix3.1 Representation of Kinematics Equation of ManipulatorExample-RRR Armlinkdi1000120L10230L20

24、31i1iaiiidiia= 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 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= a

25、ngle 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 绕 轴旋转 角，使 轴转到与 同一平面内。 沿 轴平移一间隔 ，把 移到与 同不断线上。 沿 xi 轴平移一间隔 ai-1 ，使连杆 坐标系的原点与 连杆 i 的坐标系原点重合。 (4) 绕 xi 轴旋转 角，使 zi1 转到与 zi 同不断线上。1izi1ixix1izid1ixix1i1i3.1 R

26、epresentation of Kinematics Equation of Manipulator3.1.3 T-Matrix and A-Matrix这种关系可由表示连杆相对位置的四个齐次变换来描画，并叫做 矩阵。此关系式为： (3.12)展开上式可得 ： 3.13 11( ,)(0,0,)(,0,0)( ,)iiiiiARot zTransd Trans aRot x1000011111111iiiiiiiiiiiiiiiiiidcssascccscasscscAiA313.1 Representation of Kinematics Equation of Manipulator32

27、3.1.3 T-Matrix and A-MatrixUsing A-Matrix to represent T-Matrix机械手的末端安装即为连杆6的坐标系，它与连杆 坐标系的关系可由 表示为：可得连杆变换通式为 ：1i61Ti6161AAATiii(3.15)111111111100001iiiiiiiiiiiiiiiiiiicsas cc csd sTs sc scd c (3.16)3.1 Representation of Kinematics Equation of Manipulator333.2 Solving Kinematical Equation of RobotMan

28、ipulator 机械手运动学方程的求解机械手运动学方程的求解大多数机器人程序设计言语运用某个笛卡儿坐标系来指定机械手的末端位置。这一指定可用于求解机械手最后一个连杆的姿态 。不过，在机械手可以被驱动至这个姿态之前，必需知道与这个位置有关的一切关节的位置。求解运动方程时，我们从 开场求解关节位置。使 的符号表达式的各元素等于 T6 的普通方式，并据此确定 。其它五个关节参数不能够从T6 求得，由于所求得的运动方程过于复杂而无法求解它们。我们可以由上节讨论的其它T 矩阵来求解它们。一旦求得 之后，可由 左乘 的普通方式，得： (3.18) 式中，左边为 和 各元的函数。此式可用来求解其它各关节变

29、量，如 等。 3.2 Solving Kinematics Equation6T6T6T1111A6T61611TTA16T23.2 Solving Kinematics Equation 不断地用的逆矩阵左乘式(3.17)，可得以下4个矩阵方程式： (3.19) (3.20) (3.21) (3.22)上列各方程的左式为 和前 个关节变量的函数。可用这些方程来确定各关节的位置。6261112TTAA636111213TTAAA64611121314TTAAAA6561112131415TTAAAAA6T) 1( i34 3.2 Solving Kinematics Equation353.2

30、.1 Solution of the Euler Transformation 欧拉变换解欧拉变换解根本隐式方程的解根本隐式方程的解 令令由式由式(3.4)和和(3.23)得到：得到：TEuler),(3.23)10000001000csscsssccscsssccssccssccsscccpaonpaonpaonzzzzyyyyxxxx(3.24) 3.2 Solving Kinematics Equation3.2 Solving Kinematics Equation363.2.1 Solution of the Euler Transformation令矩阵方程两边各对应元素一一相等，

31、可得到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 Equation373.2.1 Solution of the Euler Transformation 但这些解答是不确定的：1当由余弦函数求角度时，不仅此角度的符号是不确定的，并且所求角度的准确度也与该角度本身相关。2在求解 和 时，再次用到反余弦函数，且除式

32、的分母为 。当 接近于0时，总会产生不准确。3当 或 时， 和 的求解公式无定义。1zca1xcas1zcnssinsin0180 3.2 Solving Kinematics Equation383.2.1 Solution of the Euler Transformation 在求解时，总是采用双变量反正切函数atan2来确定角度。atan2提供二个自变量，即纵坐标和横坐标，见图3.8。当 - ，由atan2反求角度时，同时检查y和x的符号来确定其所在象限。这一函数也能检验什么时候x或y为0，并反求出正确的角度。atan2的准确程度对其整个定义域都是一样的。 3.8图 反正切函数atan

33、2 3.2 Solving Kinematics Equation用双变量反正切函数用双变量反正切函数(two-argument arc tangent function) 确定角度确定角度393.2.1 Solution of the Euler Transformation用显式方程求各角度用显式方程求各角度 要求得方程式的解，采用另一种通常可以导要求得方程式的解，采用另一种通常可以导致显式解答的方法。用未知逆变换依次左乘知致显式解答的方法。用未知逆变换依次左乘知方程，对于欧拉变换有：方程，对于欧拉变换有： 式式(3.37)的左式为知变换的函数，而右式各元的左式为知变换的函数，而右式各元素

34、或者为素或者为0，或者为常数。，或者为常数。 ),(),(),(1zRotyRotTzRot),(),(),(11zRotTzRotyRot(3.37)(3.38) 3.2 Solving Kinematics Equation40 对方程求解，整理之后确定其等价欧拉角： 假设知一个表示恣意旋转的齐次变换，那么就可以确定其等价欧拉角。),(2atan),(2atan180),(2atanyxyxzyxxyocosncnsaasacaa(3.46)3.2.1 Solution of the Euler Transformation 3.2 Solving Kinematics Equation4

35、13.2.2 Solution of RPY Transformations 滚、仰、偏变换解滚、仰、偏变换解 直接从显式方程来求解用滚动、俯仰和偏转表示的变换方程。 推导计算可得RPY变换各角如下：),(2atan),(2atan180),(2atanyxyxyxzxyocosacasnsncnnn(3.52) 3.2 Solving Kinematics Equation423.2.3 Solution of Spherical Coordinate Transformation 球面变换解球面变换解把求解滚、仰和偏变换方程的技术用于球面坐标表示的运动方程。 可推导出球面变换的解为：zyx

36、zyxxypcpspcsrppspcpp)(),(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个关节确定手腕的方位。个关节确定手腕的方位。

37、各连杆坐标系如图各连杆坐标系如图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图

38、 机器人的连杆坐标系( )a 结构图 3.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 小臂支承于大臂臂体

39、的上关节处，其驱动电机可带动小臂做上下俯仰角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图 机器人的连杆坐标系

40、( )a 结构图( )b 坐标图 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.

41、1 Motion Analysis 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相对基坐

42、标系0的位姿。100000010000555554csscT100000010000666665csscT)()()()()()(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.641000611060zzzzyyyyxxxxpaonpaonpa

43、onTTT3.3.1 Motion Analysis of PUMA 560 3.3 Kinematics Equation of PUMA 560543.643.3.1 Motion Analysis of PUMA 560 3.3 Kinematics Equation of PUMA 560.,;,)(,)(,)(),()(),()(;)(),()(),()(23422233122342332211223423322152354235415235423154152354231652364654236546416523646542316546416523646542316523646542

44、3646541652364654231646541652364654231cdsasapcdsdcacaspsdsdcacacpccscsassccssccsassscsscccassccssccsoccscccssscsscccsoscsccsssscssccccocscsscccsnscccsccssssccccsnscccsscsssscccccnzyxzyxzyxzyx553.3.2 Motion Synthesis of PUMA 560 PUMA 560运动综合求解运动综合求解据式3.59，可把PUMA 560的运动方程3.64写为：假设末端连杆的位姿曾经给定，即 为知，那么求关节

45、变量 的值称为运动反解。用未知的连杆逆变换左乘方程(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)()()()()()(100066555444333222111060TTTTTTpaonpaonpaonTzzzzyyyyxx

46、xx(3.65)010123451162233445566()()()()()TTTTTTT0111T1111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoap 3.3 Kinematics Equation of PUMA 560573.3.2 Motion Synthesis of PUMA 5601.求 1010123451162233445566()()()()()TTTTTTT1111111111111611110 00 0001 00001000 10

47、001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoap1112xyys 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

48、(3.70)11112xyys pc ppdcos ;sinxypp22;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

49、)项元素对应相等：3 34 3a cd sk其中2222222232422xyzpppaaddka 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

50、560613.3.2 Motion Synthesis of 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.

51、2 Motion Synthesis of PUMA 560求根据 解的四种能够组合可以得到相应的四种能够值 ，于是可得到 的四种能够解：式中， 取与 相对应的值。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 5604.求 401034531236445566,()()()TTTTT 1 231 23232 31 231