機器人學基礎第3章機器人運動學蔡自興

1、中南大學中南大學蔡自興，謝蔡自興，謝 斌斌zxcai, 2010機器人學基礎機器人學基礎第三章第三章 機器人運動學機器人運動學1Fundamentals 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, velocity, acceleration, and all higher order der

2、ivatives of the position variables (with respect to time or any other variable). 從幾何學幾何學的觀點來處理手指位置手指位置P與關節(jié)變量關節(jié)變量L1, L2, 和 的關系稱為運動學運動學(Kinematics)。122 3.0 Introduction to Robot KinematicsIn manipulator robotics, there are two kinematics tasks:Direct (also forward) kinematics Given are joint relations

3、 (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 Introduction to Robot Kinematics3.0 Introd

4、uction 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()cosLyxLL2222121 2()arccos2xyLLLL式中同樣，

5、如果用向量表示上述關系式，其一般可表示為1( )frExample of Inverse Kinematics5 3.0 Introduction to Robot Kinematics1機器人到達給定的手爪位置 P有兩個姿態(tài)滿足要求，即圖中的 也是其解。此時 和 變成為另外的值，即逆運逆運動學的解不是惟一的動學的解不是惟一的。2 將運動學公式 兩邊微分即可得到機器人手爪的速度和關節(jié)速度的關系，再進一步進行微分將得到加速度之間的關系，處理這些關系也是機器人的運動學問題。 ( )rfExample of Inverse Kinematics6 3.0 Introduction to Robot

6、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 chain is constrained t

7、o 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 機械手是一系列由關節(jié)連接起來的連桿構成的。為機械手的每一連桿建立一個坐標系，并用齊次變換來描述這些坐標系間的相對位置和姿態(tài)。A矩陣：一個描述兩連桿間坐標系相對關系的齊次變換 ，如；各 A 矩陣的乘積稱為 T 矩

8、陣 。例如： 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 Kinematics Equatio

9、n 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 運動姿態(tài)和方向角運動姿態(tài)和方向角Motion Direction原點由矢量p表示。approach vector a：z向矢量orientation vector o：y向矢量normal vector n：x向矢量， Forming a

10、 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個元素的數(shù)值來決定。在這16個元素中，只有12個元素具有實際含義。 60001xxxxyyyyzzzznoapnoapTnoap(3.2)3.1 Representation of Kinematics Equat

11、ion of Manipulator123.1.1 Kinetic Pose and Oriented AngleEuler angle to represent motion pose機械手的運動姿態(tài)往往由一個繞軸x ，y 和 z 的旋轉序列來規(guī)定。這種轉角的序列，稱為歐拉（Euler）角。歐拉角: 用一個繞 z 軸旋轉角，再繞新的 y 軸 y旋轉角，最后繞新的 z 軸z旋轉角來描述任 何可能的姿態(tài)。歐拉變換Euler可由連乘三個旋轉矩陣來求得，即 （3.3）3.2圖 歐拉角的定義),(),(),(),(zRotyRotzRotEuler3.1 Representation of Kinem

12、atics Equation of Manipulator133.1.1 Kinetic Pose and Oriented AngleRoll, Pitch, Yaw to represent motion pose 另一種常用的旋轉集合是橫滾（roll）、俯仰（pitch）和偏轉（yaw）。 3.3圖 用橫滾、俯仰和偏轉表示機械手運動姿態(tài)3.1 Representation of Kinematics Equation of Manipulator143.1.1 Kinetic Pose and Oriented Angle 對于旋轉次序，規(guī)定：式中，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 運動位置和坐標運動位置和坐標 一旦機械手的運動姿態(tài)由某個姿態(tài)變換規(guī)定之后，它在基系中的位置就能夠由左乘左乘一個對應于矢

14、量 p的平移變換來確定（參式2.20）：61000100010001xyzppTp某姿態(tài)變換(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(),(rT

15、ranszRotzTransrzCyl3.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

16、 Equation of Manipulator183.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-

17、Matrix and A-Matrix機器人機械手是由一系列連接在一起的連桿（桿件）構成的。需要用兩個參數(shù)來描述一個連桿，即公共法線距離 所在平面內兩軸的夾角 ；需要另外兩個參數(shù)來表示相鄰兩桿的關系，即兩連桿的相對位置 和兩連桿法線的夾角 ，如圖3.5所示。3.5圖 轉動關節(jié)連桿四參數(shù)示意圖iiaa 和垂直于iidi3.1 Representation of Kinematics Equation of Manipulator203.1.3 T-Matrix and A-Matrixii3.1 Representation of Kinematics Equation of Manipulat

18、orai-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 Manipulatordi: Link Offset - variable if joint i is prismatic (平動關節(jié)) : Joint Angle - variable

19、 if joint i is revolute (轉動關節(jié))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 Representation of Kinematics Equation of Manipulator,iiiia drevolute jointprismati

20、c 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. Normals 2. Origins3. Z-axes4. X-axes253.1.3 T-Matrix and A-Matrix3.1 Representat

21、ion 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 ManipulatorExample-RRR Armlinkdi1231i1iaiiidiia= distance from zi to zi+1 along xi= d

22、istance 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 Armlinkdi1000120L10230L2031i1iaiiidiia= distance from zi to zi+1 along xi= distance from xi-1 to xi along

23、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圖 轉動關節(jié)連桿四參數(shù)示意圖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

24、of Kinematics Equation of Manipulator303.1.3 T-Matrix and A-Matrix(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-M

25、atrix這種關系可由表示連桿相對位置的四個齊次變換來描述，并叫做 矩陣。此關系式為： (3.12)展開上式可得 ： （3.13） 11( ,)(0,0,)(,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機械手的末端裝置

26、即為連桿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 RobotManipulator 機械手運動學方程的求解機械手運動學方程的求解大多數(shù)機器人程序設計語言使用某個笛卡兒坐標系來指定機械手的末端位置。這一指定可用

27、于求解機械手最后一個連桿的姿態(tài) 。不過，在機械手能夠被驅動至這個姿態(tài)之前，必須知道與這個位置有關的所有關節(jié)的位置。求解運動方程時，我們從 開始求解關節(jié)位置。使 的符號表達式的各元素等于 T6 的一般形式，并據(jù)此確定 。其它五個關節(jié)參數(shù)不可能從T6 求得，因為所求得的運動方程過于復雜而無法求解它們。我們可以由上節(jié)討論的其它T 矩陣來求解它們。一旦求得 之后，可由 左乘 的一般形式，得： (3.18) 式中，左邊為 和 各元的函數(shù)。此式可用來求解其它各關節(jié)變量，如 等。 3.2 Solving Kinematics Equation6T6T6T1111A6T61611TTA16T23.2 Solv

28、ing Kinematics Equation 不斷地用的逆矩陣左乘式(3.17)，可得下列4個矩陣方程式： (3.19) (3.20) (3.21) (3.22)上列各方程的左式為 和前 個關節(jié)變量的函數(shù)?？捎眠@些方程來確定各關節(jié)的位置。6261112TTAA636111213TTAAA64611121314TTAAAA6561112131415TTAAAAA6T) 1( i34 3.2 Solving Kinematics Equation353.2.1 Solution of the Euler Transformation 歐拉變換解歐拉變換解基本隱式方程的解 令由式(3.4)和(3.

29、23)得到：TEuler),(3.23)10000001000csscsssccscsssccssccssccsscccpaonpaonpaonzzzzyyyyxxxx(3.24) 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.2

30、6)(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）當由余弦函數(shù)求角度時，不僅此角度的符號是不確定的，并且所求角度的準確度也與該角度本身相關。（2）在求解 和 時，再次用到反余弦函數(shù)，且除式的分母為 。當 接近于0時，總會產生不準確。（3）當 或 時， 和 的求解公式無定義。1zca1xcas1zcnssinsin0180 3.2 Solving Ki

31、nematics Equation383.2.1 Solution of the Euler Transformation 在求解時，總是采用雙變量反正切函數(shù)atan2來確定角度。atan2提供二個自變量，即縱坐標和橫坐標，見圖3.8。當 - ，由atan2反求角度時，同時檢查y和x的符號來確定其所在象限。這一函數(shù)也能檢驗什么時候x或y為0，并反求出正確的角度。atan2的精確程度對其整個定義域都是一樣的。 3.8圖 反正切函數(shù)atan2 3.2 Solving Kinematics Equation用雙變量反正切函數(shù)用雙變量反正切函數(shù)(two-argument arc tangent fun

32、ction) 確定角度確定角度393.2.1 Solution of the Euler Transformation用顯式方程求各角度用顯式方程求各角度 要求得方程式的解，采用另一種通常能夠導致顯式解答的方法。用未知逆變換依次左乘已知方程，對于歐拉變換有： 式(3.37)的左式為已知變換的函數(shù)，而右式各元素或者為0，或者為常數(shù)。 ),(),(),(1zRotyRotTzRot),(),(),(11zRotTzRotyRot(3.37)(3.38) 3.2 Solving Kinematics Equation40 對方程求解，整理之后確定其等價歐拉角： 如果已知一個表示任意旋轉的齊次變換，那

33、么就能夠確定其等價歐拉角。),(2atan),(2atan180),(2atanyxyxzyxxyocosncnsaasacaa(3.46)3.2.1 Solution of the Euler Transformation 3.2 Solving Kinematics Equation413.2.2 Solution of RPY Transformations 滾、仰、偏變換解滾、仰、偏變換解 直接從顯式方程來求解用滾動、俯仰和偏轉表示的變換方程。 推導計算可得RPY變換各角如下：),(2atan),(2atan180),(2atanyxyxyxzxyocosacasnsncnnn(3.5

34、2) 3.2 Solving Kinematics Equation423.2.3 Solution of Spherical Coordinate Transformation 球面變換解球面變換解把求解滾、仰和偏變換方程的技術用于球面坐標表示的運動方程。 可推導出球面變換的解為：zyxzyxxypcpspcsrppspcpp)(),(2atan180),(2atan(3.58) 3.2 Solving Kinematics Equation433.3 Kinematic Equation of PUMA 560 PUMA 560 機器人運動方程機器人運動方程 3.3.1 Motion An

35、alysis of PUMA 560 PUMA 560 運動分析（表示）運動分析（表示）PUMA 560是屬于關節(jié)式機器人，6個關節(jié)都是轉動關節(jié)。前3個關節(jié)確定手腕參考點的位置，后3個關節(jié)確定手腕的方位。各連桿坐標系如圖3.9所示。相應的連桿參數(shù)列于表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每個關節(jié)均有角度零位與正負方向限位開關，

36、機器人的回轉機體實現(xiàn)機器人機體繞z0軸的回轉（角1），它由固定底座和回轉工作臺組成。安裝在軸中心的驅動電機經傳動裝置，可以實現(xiàn)工作臺的回轉。3.9 PUMA 560圖 機器人的連桿坐標系( )a 結構圖 3.3 Kinematics Equation of PUMA 560463.3.1 Motion Analysis of PUMA 560 大臂、小臂的平衡由機器人中的平衡裝置控制，在機器人的回轉工作臺上安裝有大臂臺座，將大臂下端關節(jié)支承在臺座上，大臂的上端關節(jié)用于支承小臂。大臂臂體的下端安有直流伺服電機，可控制大臂上下擺動（角 2 ）。3.9 PUMA 560圖 機器人的連桿坐標系( )a

37、 結構圖 3.3 Kinematics Equation of PUMA 560473.3.1 Motion Analysis of PUMA 560 小臂支承于大臂臂體的上關節(jié)處，其驅動電機可帶動小臂做上下俯仰（角3），以及小臂的回轉（4）。3.9 PUMA 560圖 機器人的連桿坐標系( )a 結構圖 3.3 Kinematics Equation of PUMA 560483.3.1 Motion Analysis of PUMA 560 機器人的腕部位于小臂臂體前端，通過伺服電動機傳動，可實現(xiàn)腕部擺動（5）和轉動（6）。3.9 PUMA 560圖 機器人的連桿坐標系( )a 結構圖 3

38、.3 Kinematics Equation of PUMA 560493.3.1 Motion Analysis of PUMA 560 3.9 PUMA 560圖 機器人的連桿坐標系( )a 結構圖( )b 坐標圖 3.3 Kinematics Equation of PUMA 56050Using A-Matrix to represent T-Matrix機械手的末端裝置即為連桿6的坐標系，它與連桿 坐標系的關系可由 表示為：可得連桿變換通式為 ：1i61Ti6161AAATiii(3.15)111111111100001iiiiiiiiiiiiiiiiiiicsas cc csd s

39、Ts sc scd c (3.16)3.3.1 Motion Analysis of PUMA 560 3.3 Kinematics Equation of PUMA 560513.3.1 Motion Analysis of PUMA 560 據(jù)連桿變換通式式(3.16)和表3.1所示連桿參數(shù)，可求得各連桿變換矩陣如下： 100001000000111110csscT100000100002222221csdscT100001000003323332csascT100000100044434443csdascT 3.3 Kinematics Equation of PUMA 560523.3

40、.1 Motion Analysis of PUMA 560 各連桿變換矩陣相乘，得PUMA 560的機械手變換的T 矩陣： 即為關節(jié)變量 的函數(shù)。 該矩陣描述了末端連桿坐標系6相對基坐標系0的位姿。100000010000555554csscT100000010000666665csscT)()()()()()(66555444333222111060TTTTTTT 621,(3.59) 3.3 Kinematics Equation of PUMA 56053要求解此運動方程，需先計算某些中間結果4T6 ， 3T6 ， 1T3 ， 1T6 等，見式（3.60）至（3.63）。于是，可求得機

41、械手的T 變換矩陣： 其中nx, ny, nz, ox, oy, oz, ax, ay, az, px, py, pz見式（3.64）1000611060zzzzyyyyxxxxpaonpaonpaonTTT3.3.1 Motion Analysis of PUMA 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.,;,)(,)(,)(),()(),()(;)(),()(),()(234222331223423

42、322112234233221523542354152354231541523542316523646542365464165236465423165464165236465423165236465423646541652364654231646541652364654231cdsasapcdsdcacaspsdsdcacacpccscsassccssccsassscsscccassccssccsoccscccssscsscccsoscsccsssscssccccocscsscccsnscccsccssssccccsnscccsscsssscccccnzyxzyxzyxzyx553.3.2 M

43、otion Synthesis of PUMA 560 PUMA 560運動綜合（求解）運動綜合（求解）據(jù)式（3.59），可把PUMA 560的運動方程（3.64）寫為：若末端連桿的位姿已經給定，即 為已知，則求關節(jié)變量 的值稱為運動反解運動反解。用未知的連桿逆變換左乘方程(3.65)兩邊，把關節(jié)變量分離出來，從而求得 的解。 )()()()()()(100066555444333222111060TTTTTTpaonpaonpaonTzzzzyyyyxxxx(3.65)pao,n,和621,621, 3.3 Kinematics Equation of PUMA 560563.3.2 Mot

44、ion Synthesis of PUMA 5601.求 用逆變換 左乘式(3.65)兩邊：1)()()()()()(100066555444333222111060TTTTTTpaonpaonpaonTzzzzyyyyxxxx(3.65)010123451162233445566()()()()()TTTTTTT0111T1111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoap 3.3 Kinematics Equation of PUMA 560573.3.

45、2 Motion Synthesis of PUMA 5601.求 1010123451162233445566()()()()()TTTTTTT1111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoap1112xyys pc ppd利用三角代換：cos ;sinxypp其中22;atan2,xyyxpppp(3.67)兩邊(2,4)項元素對應相等：(3.68) 3.3 Kinematics Equation of PUMA 560583.3.2 Motion S

46、ynthesis of PUMA 560求 式中，正、負號對應于 的兩個可能解。 212122221222122sin()/;cos()1 (/)atan2,1atan2(,)atan2(,)yxxyddddppdppd 1(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 10001xxxxxxxxyyyyyyyyz

47、zzzzzzzcsnoapnoapscnoapnoapTnoapnoap(3.67)1113 234 232213 234232 2xyxzzc ps ppa cd sa cppa sd ca s 兩邊(1,4)項和(3,4)項元素對應相等：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 sk

48、1112222122atan2(,)atan2(,)xyyyxxys pc ppdppdppd求 3 式中，2222222232422xyzpppaaddka 3.3 Kinematics Equation of PUMA 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

49、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求根據(jù) 解的四種可能組合可以得到相應的四種可能值 ，于是可得到 的四種可能解：式中， 取與 相對應的值。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 23232 336112000010001xxxxyyyyzzzzc cs csa cnoapc ss sc