機(jī)器人學(xué)基礎(chǔ)機(jī)器人運(yùn)動(dòng)學(xué)PPT課件

1、In 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.

2、Task: What are the joint rotations and orientations to achieve this?1 3.0 Introduction to Robot Kinematics3.0 Introduction to Robot Kinematics第1頁(yè)/共67頁(yè)Example of Direct Kinematics Define position of end effector and the joint variable， According to geometry：xry NoImage1211212coscos()xLL11212sinsin()y

3、LLThe general vector form( )rf2 3.0 Introduction to Robot Kinematics第2頁(yè)/共67頁(yè)2221122sinarctan( ) arctan()cosLyxLL2222121 2()arccos2xyLLLL式中同樣，如果用向量表示上述關(guān)系式，其一般可表示為1( )frExample of Inverse Kinematics3 3.0 Introduction to Robot Kinematics第3頁(yè)/共67頁(yè)1機(jī)器人到達(dá)給定的手爪位置 P有兩個(gè)姿態(tài)滿足要求，即圖中的 也是其解。此時(shí) 和 變成為另外的值，即逆運(yùn)動(dòng)學(xué)的解不是惟

4、一的。2 將運(yùn)動(dòng)學(xué)公式 兩邊微分即可得到機(jī)器人手爪的速度和關(guān)節(jié)速度的關(guān)系，再進(jìn)一步進(jìn)行微分將得到加速度之間的關(guān)系，處理這些關(guān)系也是機(jī)器人的運(yùn)動(dòng)學(xué)問題。 ( )rfExample of Inverse Kinematics4 3.0 Introduction to Robot Kinematics第4頁(yè)/共67頁(yè)53.1 Representation of Kinematics Equation of Robot Manipulator3.1 Representation of Kinematics Equation of Manipulator Mechanics of a manipulat

5、or 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 e

6、ach link with respect to the previous one.第5頁(yè)/共67頁(yè)6 機(jī)械手是一系列由關(guān)節(jié)連接起來的連桿構(gòu)成的。為機(jī)械手的每一連桿建立一個(gè)坐標(biāo)系，并用齊次變換來描述這些坐標(biāo)系間的相對(duì)位置和姿態(tài)。A矩陣：一個(gè)描述兩連桿間坐標(biāo)系相對(duì)關(guān)系的齊次變換 ，如；各 A 矩陣的乘積稱為 T 矩陣 。 例如： A1，A2，A3 T1=A1 T2=A1A2 T3=A1A2A3 3.1 Representation of Kinematics Equation of Robot Manipulator3.1 Representation of Kinematics Equatio

7、n of Manipulator第6頁(yè)/共67頁(yè)7T矩陣：A矩陣的乘積 。 對(duì)于六連桿機(jī)械手，有下列T矩陣 ： 一個(gè)六連桿機(jī)械手可具有六個(gè)自由度，每個(gè)連桿含有一個(gè)自由度，并能在其運(yùn)動(dòng)范圍內(nèi)任意定位與定向。(3.1)6123456TA A A A A A3.1 Representation of Kinematics Equation of Manipulator3.1 Representation of Kinematics Equation of Robot Manipulator第7頁(yè)/共67頁(yè)83.1 Representation of Kinematics Equation of Ro

8、bot Manipulator 運(yùn)動(dòng)姿態(tài)和方向角運(yùn)動(dòng)姿態(tài)和方向角 Motion Direction 原點(diǎn)由矢量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.1n,o,ap圖 矢量和3.1 Representation of Kinematics Equation of Manipulator第8頁(yè)/共67頁(yè)9因此，變換T6具有下列元素（同式2.35）。 六連桿機(jī)械手的T 矩陣（ T6 ）可由指定

9、其16個(gè)元素的數(shù)值來決定。在這16個(gè)元素中，只有12個(gè)元素具有實(shí)際含義。 60001xxxxyyyyzzzznoapnoapTnoap(3.2)3.1 Representation of Kinematics Equation of Manipulator第9頁(yè)/共67頁(yè)10 Euler angle to represent motion pose 機(jī)械手的運(yùn)動(dòng)姿態(tài)往往由一個(gè)繞軸x ，y 和 z 的旋轉(zhuǎn)序列來規(guī)定。這種轉(zhuǎn)角的序列，稱為歐拉（Euler）角。 歐拉角: 用一個(gè)繞 z 軸旋轉(zhuǎn)角，再繞新的 y 軸 y旋轉(zhuǎn)角，最后繞新的 z 軸z旋轉(zhuǎn)角來描述任 何可能的姿態(tài)。 歐拉變換Euler可由連

10、乘三個(gè)旋轉(zhuǎn)矩陣來求得，即 （3.3）3.2圖 歐拉角的定義),(),(),(),(zRotyRotzRotEuler3.1 Representation of Kinematics Equation of Manipulator第10頁(yè)/共67頁(yè)11 Roll, Pitch, Yaw to represent motion pose 另一種常用的旋轉(zhuǎn)集合是橫滾（roll）、俯仰（pitch）和偏轉(zhuǎn)（yaw）。 3.3圖 用橫滾、俯仰和偏轉(zhuǎn)表示機(jī)械手運(yùn)動(dòng)姿態(tài)3.1 Representation of Kinematics Equation of Manipulator第11頁(yè)/共67頁(yè)12 對(duì)于

11、旋轉(zhuǎn)次序，規(guī)定：式中，RPY表示橫滾、俯仰和偏轉(zhuǎn)三旋轉(zhuǎn)的組合變換。也就是說，先繞 x 軸旋轉(zhuǎn)角 ，再繞 y 軸旋轉(zhuǎn)角，最后繞 z 軸旋角 。 (3.4)( , ,)( , )( , )( ,)RPYRot zRot yRot x 3.1 Representation of Kinematics Equation of Manipulator第12頁(yè)/共67頁(yè)133.1 Representation of Kinetic Equation of Robot Manipulator 運(yùn)動(dòng)位置和坐標(biāo)運(yùn)動(dòng)位置和坐標(biāo) 一旦機(jī)械手的運(yùn)動(dòng)姿態(tài)由某個(gè)姿態(tài)變換規(guī)定之后，它在基系中的位置就能夠由左乘一個(gè)對(duì)應(yīng)于矢量

12、 p的平移變換來確定（參式2.20）：61000100010001xyzppTp某姿態(tài)變換(3.6)3.1 Representation of Kinematics Equation of Manipulator第13頁(yè)/共67頁(yè)14 Description in Cylinder Coordinates 用柱面坐標(biāo)來表示機(jī)械手手臂的位置，即表示其平移變換。這對(duì)應(yīng)于沿 x 軸平移 r，再繞 z 軸旋轉(zhuǎn)，最后沿 z 軸平移 z。如圖3.4(a)所示。 3.4圖 用柱面坐標(biāo)和球面坐標(biāo)表示位置)0 , 0 ,(),(), 0 , 0(),(rTranszRotzTransrzCyl3.1 Repre

13、sentation of Kinematics Equation of Manipulator第14頁(yè)/共67頁(yè)15 Description in Spherical Coordinates 用球面坐標(biāo)表示手臂運(yùn)動(dòng)位置矢量的方法，對(duì)應(yīng)于沿 z 軸平移 r，再繞 y 軸旋轉(zhuǎn)角，最后繞 z 軸旋轉(zhuǎn) 角，如圖3.4(b)所示，即為：( , , )( , )( ,)(0,0, )SphrRot zRot yTransr (3.9)3.1 Representation of Kinematics Equation of Manipulator第15頁(yè)/共67頁(yè)163.1 Representation o

14、f Kinetic Equation of Robot Manipulator 連桿變換矩陣及其乘積連桿變換矩陣及其乘積 廣義連桿廣義連桿 相鄰坐標(biāo)系間及其相應(yīng)連桿可以用齊次變換矩陣來表示。要求解操作手所需要的變換矩陣，每個(gè)連桿都要用廣義連桿來描述。在求得相應(yīng)的廣義變換矩陣之后，可對(duì)其加以修正，以適合每個(gè)具體的連桿。 3.1 Representation of Kinematics Equation of Manipulator第16頁(yè)/共67頁(yè)17機(jī)器人機(jī)械手是由一系列連接在一起的連桿（桿件）構(gòu)成的。需要用兩個(gè)參數(shù)來描述一個(gè)連桿，即公共法線距離 所在平面內(nèi)兩軸的夾角 ；需要另外兩個(gè)參數(shù)來表示

15、相鄰兩桿的關(guān)系，即兩連桿的相對(duì)位置 和兩連桿法線的夾角 ，如圖3.5所示。3.5圖 轉(zhuǎn)動(dòng)關(guān)節(jié)連桿四參數(shù)示意圖iiaa 和垂直于iidi3.1 Representation of Kinematics Equation of Manipulator第17頁(yè)/共67頁(yè)18ii3.1 Representation of Kinematics Equation of Manipulatorai-1: Link Length - mutual perpendicular unique except for parallel axis : Link Twist - measured in the righ

16、t-hand sense about1ia1i第18頁(yè)/共67頁(yè)193.1 Representation of Kinematics Equation of Manipulatordi: Link Offset - variable if joint i is prismatic (平動(dòng)關(guān)節(jié)) : Joint Angle - variable if joint i is revolute (轉(zhuǎn)動(dòng)關(guān)節(jié))i第19頁(yè)/共67頁(yè)20 Denavit-Hartenberg Parameters4 D-H parameters3 fixed link parameters1 joint variablei

17、 and ai : describe the Link idi and i : describe the Links connection3.1 Representation of Kinematics Equation of Manipulator,iiiia drevolute jointprismatic jointiid第20頁(yè)/共67頁(yè)213.1 Representation of Kinematics Equation of Manipulatory-vectors: complete right-hand frames第21頁(yè)/共67頁(yè)223.1 Representation o

18、f Kinematics Equation of Manipulator1. Normals 2. Origins3. Z-axes4. X-axes第22頁(yè)/共67頁(yè)233.1 Representation of Kinematics Equation of Manipulator第23頁(yè)/共67頁(yè)243.1 Representation of Kinematics Equation of ManipulatorExample-RRR Arm第24頁(yè)/共67頁(yè)253.1 Representation of Kinematics Equation of ManipulatorExample-R

19、RR 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 zi第25頁(yè)/共67頁(yè)263.1 Representation of Kinematics Equation of ManipulatorExample-RRR Armlinkdi1000120L10230L2031i1iaiiidiia= distance from zi to

20、 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第26頁(yè)/共67頁(yè)273.5圖 轉(zhuǎn)動(dòng)關(guān)節(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 x

21、i about zi3.1 Representation of Kinematics Equation of Manipulator第27頁(yè)/共67頁(yè)28(1) 繞 軸旋轉(zhuǎn) 角，使 軸轉(zhuǎn)到與 同一平面內(nèi)。(2) 沿 軸平移一距離 ，把 移到與 同一直線上。(3) 沿 xi 軸平移一距離 ai-1 ，使連桿 坐標(biāo)系的原點(diǎn)與 連桿 i 的坐標(biāo)系原點(diǎn)重合。(4) 繞 xi 軸旋轉(zhuǎn) 角，使 zi1 轉(zhuǎn)到與 zi 同一直線上。1izi1ixix1izid1ixix1i1i3.1 Representation of Kinematics Equation of Manipulator第28頁(yè)/共67頁(yè) T

22、-Matrix and A-Matrix 這種關(guān)系可由表示連桿相對(duì)位置的四個(gè)齊次變換來描述，并叫做 矩陣。此關(guān)系式為： (3.12)展開上式可得 ： （3.13） 11( ,)(0,0,)(,0,0)( ,)iiiiiARot zTransd Trans aRot x1000011111111iiiiiiiiiiiiiiiiiidcssascccscasscscAiA293.1 Representation of Kinematics Equation of Manipulator第29頁(yè)/共67頁(yè)30 Using A-Matrix to represent T-Matrix機(jī)械手的末端裝置即

23、為連桿6的坐標(biāo)系，它與連桿 坐標(biāo)系的關(guān)系可由 表示為：可得連桿變換通式為 ：1i61Ti6161AAATiii(3.15)111111111100001iiiiiiiiiiiiiiiiiiicsas cc csd sTs sc scd c (3.16)3.1 Representation of Kinematics Equation of Manipulator第30頁(yè)/共67頁(yè)313.2 Solving Kinematical Equation of RobotManipulator 機(jī)械手運(yùn)動(dòng)學(xué)方程的求解 大多數(shù)機(jī)器人程序設(shè)計(jì)語(yǔ)言使用某個(gè)笛卡兒坐標(biāo)系來指定機(jī)械手的末端位置。這一指定可用于求

24、解機(jī)械手最后一個(gè)連桿的姿態(tài) 。不過，在機(jī)械手能夠被驅(qū)動(dòng)至這個(gè)姿態(tài)之前，必須知道與這個(gè)位置有關(guān)的所有關(guān)節(jié)的位置。 求解運(yùn)動(dòng)方程時(shí)，我們從 開始求解關(guān)節(jié)位置。使 的符號(hào)表達(dá)式的各元素等于 T6 的一般形式，并據(jù)此確定 。其它五個(gè)關(guān)節(jié)參數(shù)不可能從T6 求得，因?yàn)樗蟮玫倪\(yùn)動(dòng)方程過于復(fù)雜而無(wú)法求解它們。我們可以由上節(jié)討論的其它T 矩陣來求解它們。一旦求得 之后，可由 左乘 的一般形式，得： (3.18) 式中，左邊為 和 各元的函數(shù)。此式可用來求解其它各關(guān)節(jié)變量，如 等。 3.2 Solving Kinematics Equation6T6T6T1111A6T61611TTA16T2第31頁(yè)/共67頁(yè)

25、3.2 Solving Kinematics Equation 不斷地用的逆矩陣左乘式(3.17)，可得下列4個(gè)矩陣方程式： (3.19) (3.20) (3.21) (3.22)上列各方程的左式為 和前 個(gè)關(guān)節(jié)變量的函數(shù)?？捎眠@些方程來確定各關(guān)節(jié)的位置。6261112TTAA636111213TTAAA64611121314TTAAAA6561112131415TTAAAAA6T) 1( i32 3.2 Solving Kinematics Equation第32頁(yè)/共67頁(yè)33 歐拉變換解歐拉變換解 基本隱式方程的解 令由式(3.4)和(3.23)得到：TEuler),(3.23)1000

26、0001000csscsssccscsssccssccssccsscccpaonpaonpaonzzzzyyyyxxxx(3.24) 3.2 Solving Kinematics Equation3.2 Solving Kinematics Equation第33頁(yè)/共67頁(yè)34令矩陣方程兩邊各對(duì)應(yīng)元素一一相等，可得到9個(gè)隱式方程如下：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)1zca1xcas1zcn

27、s 3.2 Solving Kinematics Equation第34頁(yè)/共67頁(yè)35 但這些解答是不確定的：（1）當(dāng)由余弦函數(shù)求角度時(shí)，不僅此角度的符號(hào)是不確定的，并且所求角度的準(zhǔn)確度也與該角度本身相關(guān)。（2）在求解 和 時(shí)，再次用到反余弦函數(shù)，且除式的分母為 。當(dāng) 接近于0時(shí)，總會(huì)產(chǎn)生不準(zhǔn)確。（3）當(dāng) 或 時(shí)， 和 的求解公式無(wú)定義。1zca1xcas1zcnssinsin0180 3.2 Solving Kinematics Equation第35頁(yè)/共67頁(yè)36 在求解時(shí)，總是采用雙變量反正切函數(shù)atan2來確定角度。atan2提供二個(gè)自變量，即縱坐標(biāo)和橫坐標(biāo)，見圖3.8。當(dāng) - ，

28、由atan2反求角度時(shí)，同時(shí)檢查y和x的符號(hào)來確定其所在象限。這一函數(shù)也能檢驗(yàn)什么時(shí)候x或y為0，并反求出正確的角度。atan2的精確程度對(duì)其整個(gè)定義域都是一樣的。 3.8圖 反正切函數(shù)atan2 3.2 Solving Kinematics Equation用雙變量反正切函數(shù)(two-argument arc tangent function) 確定角度第36頁(yè)/共67頁(yè)37 用顯式方程求各角度用顯式方程求各角度 要求得方程式的解，采用另一種通常能夠?qū)е嘛@式解答的方法。用未知逆變換依次左乘已知方程，對(duì)于歐拉變換有： 式(3.37)的左式為已知變換的函數(shù)，而右式各元素或者為0，或者為常數(shù)。 )

29、,(),(),(1zRotyRotTzRot),(),(),(11zRotTzRotyRot(3.37)(3.38) 3.2 Solving Kinematics Equation第37頁(yè)/共67頁(yè)38 對(duì)方程求解，整理之后確定其等價(jià)歐拉角： 如果已知一個(gè)表示任意旋轉(zhuǎn)的齊次變換，那么就能夠確定其等價(jià)歐拉角。),(2atan),(2atan180),(2atanyxyxzyxxyocosncnsaasacaa(3.46)Solution of the Euler Transformation 3.2 Solving Kinematics Equation第38頁(yè)/共67頁(yè)39 滾、仰、偏變換解

30、直接從顯式方程來求解用滾動(dòng)、俯仰和偏轉(zhuǎn)表示的變換方程。 推導(dǎo)計(jì)算可得RPY變換各角如下：),(2atan),(2atan180),(2atanyxyxyxzxyocosacasnsncnnn(3.52) 3.2 Solving Kinematics Equation第39頁(yè)/共67頁(yè)40球面變換解 把求解滾、仰和偏變換方程的技術(shù)用于球面坐標(biāo)表示的運(yùn)動(dòng)方程。 可推導(dǎo)出球面變換的解為：zyxzyxxypcpspcsrppspcpp)(),(2atan180),(2atan(3.58) 3.2 Solving Kinematics Equation第40頁(yè)/共67頁(yè)413.3 Kinematic E

31、quation of PUMA 560 PUMA 560 機(jī)器人運(yùn)動(dòng)方程 PUMA 560 運(yùn)動(dòng)分析（表示） PUMA 560是屬于關(guān)節(jié)式機(jī)器人，6個(gè)關(guān)節(jié)都是轉(zhuǎn)動(dòng)關(guān)節(jié)。前3個(gè)關(guān)節(jié)確定手腕參考點(diǎn)的位置，后3個(gè)關(guān)節(jié)確定手腕的方位。 各連桿坐標(biāo)系如圖3.9所示。相應(yīng)的連桿參數(shù)列于表3.1。 3.3 Kinematics Equation of PUMA 560第41頁(yè)/共67頁(yè)42 3.3 Kinematics Equation of PUMA 560第42頁(yè)/共67頁(yè)43 PUMA560每個(gè)關(guān)節(jié)均有角度零位與正負(fù)方向限位開關(guān)，機(jī)器人的回轉(zhuǎn)機(jī)體實(shí)現(xiàn)機(jī)器人機(jī)體繞z0軸的回轉(zhuǎn)（角1），它由固定底座和回轉(zhuǎn)

32、工作臺(tái)組成。安裝在軸中心的驅(qū)動(dòng)電機(jī)經(jīng)傳動(dòng)裝置，可以實(shí)現(xiàn)工作臺(tái)的回轉(zhuǎn)。3.9 PUMA 560圖 機(jī)器人的連桿坐標(biāo)系( )a 結(jié)構(gòu)圖 3.3 Kinematics Equation of PUMA 560第43頁(yè)/共67頁(yè)44 大臂、小臂的平衡由機(jī)器人中的平衡裝置控制，在機(jī)器人的回轉(zhuǎn)工作臺(tái)上安裝有大臂臺(tái)座，將大臂下端關(guān)節(jié)支承在臺(tái)座上，大臂的上端關(guān)節(jié)用于支承小臂。大臂臂體的下端安有直流伺服電機(jī)，可控制大臂上下擺動(dòng)（角 2 ）。3.9 PUMA 560圖 機(jī)器人的連桿坐標(biāo)系( )a 結(jié)構(gòu)圖 3.3 Kinematics Equation of PUMA 560第44頁(yè)/共67頁(yè)45 小臂支承于大臂臂

33、體的上關(guān)節(jié)處，其驅(qū)動(dòng)電機(jī)可帶動(dòng)小臂做上下俯仰（角3），以及小臂的回轉(zhuǎn)（4）。3.9 PUMA 560圖 機(jī)器人的連桿坐標(biāo)系( )a 結(jié)構(gòu)圖 3.3 Kinematics Equation of PUMA 560第45頁(yè)/共67頁(yè)46 機(jī)器人的腕部位于小臂臂體前端，通過伺服電動(dòng)機(jī)傳動(dòng)，可實(shí)現(xiàn)腕部擺動(dòng)（5）和轉(zhuǎn)動(dòng)（6）。3.9 PUMA 560圖 機(jī)器人的連桿坐標(biāo)系( )a 結(jié)構(gòu)圖 3.3 Kinematics Equation of PUMA 560第46頁(yè)/共67頁(yè)47 3.9 PUMA 560圖 機(jī)器人的連桿坐標(biāo)系( )a 結(jié)構(gòu)圖( )b 坐標(biāo)圖 3.3 Kinematics Equatio

34、n of PUMA 560第47頁(yè)/共67頁(yè)48 Using A-Matrix to represent T-Matrix機(jī)械手的末端裝置即為連桿6的坐標(biāo)系，它與連桿 坐標(biāo)系的關(guān)系可由 表示為：可得連桿變換通式為 ：1i61Ti6161AAATiii(3.15)111111111100001iiiiiiiiiiiiiiiiiiicsas cc csd sTs sc scd c (3.16) 3.3 Kinematics Equation of PUMA 560第48頁(yè)/共67頁(yè)49 據(jù)連桿變換通式式(3.16)和表3.1所示連桿參數(shù)，可求得各連桿變換矩陣如下： 1000010000001111

35、10csscT100000100002222221csdscT100001000003323332csascT100000100044434443csdascT 3.3 Kinematics Equation of PUMA 560第49頁(yè)/共67頁(yè)50 各連桿變換矩陣相乘，得PUMA 560的機(jī)械手變換的T 矩陣： 即為關(guān)節(jié)變量 的函數(shù)。 該矩陣描述了末端連桿坐標(biāo)系6相對(duì)基坐標(biāo)系0的位姿。100000010000555554csscT100000010000666665csscT)()()()()()(66555444333222111060TTTTTTT 621,(3.59) 3.3 Ki

36、nematics Equation of PUMA 560第50頁(yè)/共67頁(yè)51 要求解此運(yùn)動(dòng)方程，需先計(jì)算某些中間結(jié)果4T6 ， 3T6 ， 1T3 ， 1T6 等，見式（3.60）至（3.63）。 于是，可求得機(jī)械手的T 變換矩陣： 其中nx, ny, nz, ox, oy, oz, ax, ay, az, px, py, pz見式（3.64）1000611060zzzzyyyyxxxxpaonpaonpaonTTT 3.3 Kinematics Equation of PUMA 560第51頁(yè)/共67頁(yè)52（3.64） 3.3 Kinematics Equation of PUMA 56

37、0.,;,)(,)(,)(),()(),()(;)(),()(),()(234222331223423322112234233221523542354152354231541523542316523646542365464165236465423165464165236465423165236465423646541652364654231646541652364654231cdsasapcdsdcacaspsdsdcacacpccscsassccssccsassscsscccassccssccsoccscccssscsscccsoscsccsssscssccccocscsscccsnsccc

38、sccssssccccsnscccsscsssscccccnzyxzyxzyxzyx第52頁(yè)/共67頁(yè)53運(yùn)動(dòng)綜合（求解）據(jù)式（3.59），可把PUMA 560的運(yùn)動(dòng)方程（3.64）寫為：若末端連桿的位姿已經(jīng)給定，即 為已知，則求關(guān)節(jié)變量 的值稱為運(yùn)動(dòng)反解。用未知的連桿逆變換左乘方程(3.65)兩邊，把關(guān)節(jié)變量分離出來，從而求得 的解。 )()()()()()(100066555444333222111060TTTTTTpaonpaonpaonTzzzzyyyyxxxx(3.65)pao,n,和621,621, 3.3 Kinematics Equation of PUMA 560第53頁(yè)/共

39、67頁(yè)54 1.求 用逆變換 左乘式(3.65)兩邊：1)()()()()()(100066555444333222111060TTTTTTpaonpaonpaonTzzzzyyyyxxxx(3.65)010123451162233445566()()()()()TTTTTTT0111T1111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoap 3.3 Kinematics Equation of PUMA 560第54頁(yè)/共67頁(yè)55 1.求 101012345

40、1162233445566()()()()()TTTTTTT1111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoap1112xyys pc ppd利用三角代換：cos ;sinxypp其中22;atan2,xyyxpppp(3.67)兩邊(2,4)項(xiàng)元素對(duì)應(yīng)相等：(3.68) 3.3 Kinematics Equation of PUMA 560第55頁(yè)/共67頁(yè)56 求 式中，正、負(fù)號(hào)對(duì)應(yīng)于 的兩個(gè)可能解。 212122221222122sin()/;cos(

41、)1 (/)atan2,1atan2(,)atan2(,)yxxyddddppdppd 1(3.70)11112xyys pc ppdcos ;sinxypp22;atan2,xyyxpppp 3.3 Kinematics Equation of PUMA 560第56頁(yè)/共67頁(yè)57 2. 求 31111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoap(3.67)1113 234 232213 234232 2xyxzzc ps ppa cd sa cppa

42、sd ca s 兩邊(1,4)項(xiàng)和(3,4)項(xiàng)元素對(duì)應(yīng)相等：3 34 3a cd sk其中2222222232422xyzpppaaddka 3.3 Kinematics Equation of PUMA 560第57頁(yè)/共67頁(yè)58 求 1),(2atan),(2atan22423433kdakda3(3.73)正、負(fù)號(hào)對(duì)應(yīng) 的兩種可能解。3 34 3a cd sk1112222122atan2(,)atan2(,)xyyyxxys pc ppdppdppd求 3 式中，2222222232422xyzpppaaddka 3.3 Kinematics Equation of PUMA 560

43、第58頁(yè)/共67頁(yè)59 3.求 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)項(xiàng)和(2,4)項(xiàng)元素對(duì)應(yīng)相等： 3.3 Kinematics Equation of PUMA 560第59頁(yè)/共67頁(yè)60 求根據(jù) 解的四種可能組合可以得到相應(yīng)的四種

44、可能值 ，于是可得到 的四種可能解：式中， 取與 相對(duì)應(yīng)的值。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 560第60頁(yè)/共67頁(yè)61 4.求 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)項(xiàng)和(3,3)項(xiàng)元素對(duì)應(yīng)相等：4111 231 2323atan2(,)xyxyza