第34章機(jī)器人運(yùn)動學(xué)解讀

1、1,第3章 機(jī)器人運(yùn)動學(xué),運(yùn)動學(xué)研究物體的位姿、速度和加速度之間的關(guān)系。,本章將介紹雙輪移動機(jī)器人、三輪全向移動機(jī)器人和關(guān)節(jié)式機(jī)械臂的運(yùn)動學(xué)問題。,雙輪移動機(jī)器人運(yùn)動學(xué),,平面輪式移動機(jī)器人,（x, y, q）表示雙輪機(jī)器人位姿，v 表示機(jī)器人前進(jìn)速度，表示機(jī)器人轉(zhuǎn)動速度w，則,,（3-1）,由（3-1）式可得運(yùn)動學(xué)約束條件，,,是所謂的“非完整約束”。物理含義是，機(jī)器人不能沿輪軸線方向橫移。,設(shè)輪距為D，輪半徑為r，兩輪獨(dú)立驅(qū)動

2、時輪子轉(zhuǎn)速wL，wR,則,（3-2）,2,給定期望的機(jī)器人前進(jìn)速度v，轉(zhuǎn)動速度w，則可以確定機(jī)器人的兩輪轉(zhuǎn)速為,,（3-3）,因此，可以非常方便地通過控制電機(jī)的轉(zhuǎn)速來控制機(jī)器人移動和轉(zhuǎn)動速度。,機(jī)器人位置估計(jì),已知初始位姿為（x0, y0, q0），兩輪轉(zhuǎn)角增量為??L和 ??R，則兩輪移動距離分別為?lR = r??R和?lL = r??L，,機(jī)器人移動距離 ?l=(?lR+?lL)/2,方位角

3、變化 ?q =(?lR-?lL)/D。,第n步機(jī)器人位姿可以按下面公式更新：,,若已知機(jī)器人的初始位姿，根據(jù)該遞推公式可以確定任意時刻機(jī)器人位姿，比較簡單，但因積累誤差大，所以長時間不可靠。,3,三輪全向移動機(jī)器人運(yùn)動學(xué),雙輪移動機(jī)器人運(yùn)動中最大的問題是不能橫向移動，在實(shí)際應(yīng)用中靈活性比較差。,全向移動輪是一種新的輪式移動機(jī)構(gòu)，在大輪的邊緣上布置若干小輪，使得機(jī)器人的移動方向不再限定于大輪所在的平

4、面方向。,全向移動輪,三輪全向移動機(jī)構(gòu),xoy是機(jī)器人坐標(biāo)系，機(jī)器人的運(yùn)動速度用vx、vy和w表示，三個全向輪的角速度分別用w1、w2和w3表示，v1、v2和v3分別表示三個全向輪輪心處的線速度。假設(shè)全向輪的半徑為R，距運(yùn)動機(jī)構(gòu)中心的距離為L，則速度間關(guān)系為：,,,（3-5）,4,三個全向輪的角速度與機(jī)器人速度之間關(guān)系：,（3-6）,圖3-4全向移動機(jī)構(gòu)在場地坐標(biāo)系中的位置,場地坐標(biāo)系下的速度Vx、Vy和W 與機(jī)器人坐標(biāo)系下機(jī)器人速度之

5、間的變換關(guān)系如下：,,,?,,?,可以非常方便地通過控制電機(jī)的轉(zhuǎn)速來控制機(jī)器人在場地坐標(biāo)系下的移動和轉(zhuǎn)動速度。,5,平面機(jī)械臂運(yùn)動學(xué),,平面機(jī)械臂,兩連桿平面旋轉(zhuǎn)關(guān)節(jié)機(jī)械臂，其結(jié)構(gòu)由連桿長度L1，L2和關(guān)節(jié)角?1，?2確定。,表示關(guān)節(jié)位置的變量?1，?2稱為關(guān)節(jié)變量。,旋轉(zhuǎn)關(guān)節(jié)變量用關(guān)節(jié)角?表示，而移動關(guān)節(jié)變量用移動距離d表示。,機(jī)械手末端位置與關(guān)節(jié)角之間的關(guān)系為,其中 c1=cos?1，c12=cos(?1+?2)，s1=sin?1，

6、s12=sin(?1+?2)。,采用矢量表示為 r = f(? ),式中f 表示矢量函數(shù)，r =[x,y]T，?=[?1, ?2]T。,從關(guān)節(jié)變量? 求手爪位置 r 稱為正運(yùn)動學(xué)，,反之，從手爪位置r求關(guān)節(jié)變量? 稱為逆運(yùn)動學(xué)。,6,逆運(yùn)動學(xué)公式：,平面機(jī)械臂簡圖,△OAB中 a 可以根據(jù)余弦定理確定,因此，可以得到 ?2 = ?-?,?1+β和β都可計(jì)算，因此?1也是可以計(jì)算的。,,,因此，,逆運(yùn)動學(xué)的解一般不唯一

7、，顯然圖中機(jī)械臂關(guān)于OB軸對稱的位置也是逆運(yùn)動學(xué)問題的一個解。,空間機(jī)械臂連桿描述,機(jī)械臂可以看成一系列剛體通過關(guān)節(jié)連接而成的鏈?zhǔn)竭\(yùn)動機(jī)構(gòu)。一般把這些剛體稱為連桿，通過關(guān)節(jié)將相鄰的連桿連接起來。旋轉(zhuǎn)關(guān)節(jié)和移動關(guān)節(jié)是機(jī)械臂設(shè)計(jì)中經(jīng)常采用的單自由度關(guān)節(jié)。,稱基座為連桿0。第一個可移動連桿為連桿1，機(jī)械臂的最末端連桿為連桿n。為了使機(jī)械臂末端執(zhí)行器可以在3維空間達(dá)到任意的位置和姿態(tài)，機(jī)械臂至少需要6個關(guān)節(jié)，因此，典型的工業(yè)機(jī)械臂一般都具有6個

8、關(guān)節(jié)。,7,,用一條空間直線表示關(guān)節(jié)的轉(zhuǎn)軸（平移軸），連桿i 的運(yùn)動可以用轉(zhuǎn)軸i 和連桿i相對連桿i-1的轉(zhuǎn)動角度?i 來描述。下面給出幾個連桿參數(shù)的定義：,連桿長度，連桿兩端關(guān)節(jié)軸線間公垂線的長度,連桿轉(zhuǎn)角，過關(guān)節(jié)軸i-1做垂直于公垂線的平面，在該平面內(nèi)做過垂足且平行于關(guān)節(jié)軸i 的直線。該直線與關(guān)節(jié)軸i-1的夾角定義為連桿轉(zhuǎn)角。,連桿轉(zhuǎn)角只在兩個關(guān)節(jié)軸為空間異面直線的情況有意義,連桿偏距，關(guān)節(jié)軸i與相鄰關(guān)節(jié)轉(zhuǎn)軸(i-1和i+1) 公垂

9、線間距離稱為連桿偏距,關(guān)節(jié)角，兩相鄰連桿繞公共軸線旋轉(zhuǎn)的角度稱為關(guān)節(jié)角。,連桿描述,8,,,對于一個6關(guān)節(jié)機(jī)器人，需要18個參數(shù)就可以完全描述機(jī)械臂固定的運(yùn)動學(xué)結(jié)構(gòu)參數(shù)。如果機(jī)器人6個關(guān)節(jié)均為轉(zhuǎn)動關(guān)節(jié)，18個固定參數(shù)可以用6組（ai-1, ai-1, di）表示。,機(jī)器人的每個連桿都可以用以上四個參數(shù)描述，其中連桿長度和連桿轉(zhuǎn)角描述連桿本身，連桿偏距和關(guān)節(jié)角描述連桿之間的連接關(guān)系。,對于轉(zhuǎn)動關(guān)節(jié)，qi為關(guān)節(jié)變量，其它三個參數(shù)是常數(shù)；對于

10、移動關(guān)節(jié)，di為關(guān)節(jié)變量，其它三個參數(shù)是常數(shù)。,這種用連桿參數(shù)描述機(jī)構(gòu)運(yùn)動學(xué)關(guān)系的規(guī)則稱為（Devanit-Hartenberg） DH方法，連桿參數(shù)稱為DH參數(shù)。,空間機(jī)械臂坐標(biāo)系選擇,為了獲得機(jī)械臂末端執(zhí)行器在3維空間的位置和姿態(tài)，需要在每個連桿上定義與連桿固連的坐標(biāo)系來描述相鄰連桿之間的位置關(guān)系。,根據(jù)固連坐標(biāo)系所在連桿的編號對固連坐標(biāo)系命名，如在固連在連桿i上的固連坐標(biāo)系稱為坐標(biāo)系{i}。,9,,,,,固連在基座上的坐標(biāo)系稱為坐

11、標(biāo)系{0}。該坐標(biāo)系在機(jī)械臂運(yùn)動過程中保持固定，因此在研究機(jī)械臂運(yùn)動學(xué)問題時一般把坐標(biāo)系{0}選為參考系，用來描述其它連桿坐標(biāo)系的位置。,坐標(biāo)系{i}選擇示意圖,因此，當(dāng)關(guān)節(jié)1為轉(zhuǎn)動關(guān)節(jié)時a0=0，a0=0，d1=0；當(dāng)關(guān)節(jié)1為移動關(guān)節(jié)時a0=0，a0=0，q1 =0。,原則上參考系{0}可以任意設(shè)定，但為了簡化描述，通常設(shè)定Z0軸沿關(guān)節(jié)軸1的方向，并且關(guān)節(jié)1的關(guān)節(jié)變量為0時參考系{0}與坐標(biāo)系{1}重合。,對于中間連桿i，坐標(biāo)系{i}

12、的Zi軸與關(guān)節(jié)軸i重合，坐標(biāo)系{i}的原點(diǎn)位于公垂線ai與關(guān)節(jié)軸i的交點(diǎn)處。Xi沿ai方向由關(guān)節(jié)i指向關(guān)節(jié)i+1，并按照右手系規(guī)則確定Yi，ai按右手定則繞Xi轉(zhuǎn)角定義。,若ai =0，兩Z軸相交，則選Xi垂于Zi和Zi+1 ，坐標(biāo)系{i}的選擇不是唯一的。,10,,,坐標(biāo)系{i}選擇示意圖,連桿坐標(biāo)系中連桿參數(shù)確定,以上4個參數(shù)中，ai表示連桿長度只能取非負(fù)值；而其余3參數(shù)可以為正，也可以為負(fù)。,DH參數(shù)按以下方法確定：,ai =沿X

13、i軸，從Zi移動到Zi+1的距離；,ai =繞Xi軸，從Zi旋轉(zhuǎn)到Zi+1的角度；,di =沿Zi軸，從Xi-1移動到Xi的距離；,qi =繞Zi軸，從Xi-1旋轉(zhuǎn)到Xi的角度；,建立連桿坐標(biāo)系的步驟,2. 找出關(guān)節(jié)軸i和i+1之間的公垂線或兩個軸的交點(diǎn)，以兩個軸的交點(diǎn)或公垂線與關(guān)節(jié)軸i的交點(diǎn)為坐標(biāo)系{i}的原點(diǎn)。,1. 找出各關(guān)節(jié)軸，并標(biāo)出軸的延長線。步驟2-5僅考慮兩個相鄰關(guān)節(jié)軸（i 和 i+1）和坐標(biāo)系{i}。,坐標(biāo)系{i}選擇示

14、意圖,6. 當(dāng)?shù)谝粋€關(guān)節(jié)變量為0時坐標(biāo) 系{1}與坐標(biāo)系{0}重合。,3. 規(guī)定Zi沿關(guān)節(jié)軸i的方向。,4. 規(guī)定Xi沿公垂線指向關(guān)節(jié)軸i+1，若兩個軸相交，規(guī)定Xi垂直于兩軸所在的平面。,5. 按右手定則確定Yi軸。,對于坐標(biāo)系{n}，原點(diǎn)位置可以在關(guān)節(jié)軸上任意選取， Xn的方向也是任意的。但在選擇時應(yīng)盡量使更多的連桿參數(shù)為0。,11,,,,,例3-1 如圖3-9所示的平面三連桿機(jī)械臂，因?yàn)槿齻€關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)，稱為RRR（或3R）機(jī)

15、構(gòu)。請?jiān)谠摍C(jī)構(gòu)上建立連桿坐標(biāo)系并寫出DH參數(shù)。,平面3R機(jī)械臂,連桿坐標(biāo)系布局,定義參考坐標(biāo)系{0}，它固定在基座上，當(dāng)?shù)谝粋€關(guān)節(jié)變量（q1）為0時坐標(biāo)系{1}與坐標(biāo)系{0}重合，因此建立參考坐標(biāo)系{0}如圖所示，Z0軸與關(guān)節(jié)1的軸線重合且垂直于機(jī)械臂所在平面。,由于機(jī)械臂位于一個平面上，因此所有Z軸相互平行，且連桿偏距d和連桿轉(zhuǎn)角a均為0。該機(jī)械臂的DH參數(shù)如表3-1所示。,表3-1 3R機(jī)械臂DH參數(shù),12,13,空間機(jī)械臂運(yùn)動學(xué),

16、本節(jié)將導(dǎo)出相鄰連桿間坐標(biāo)系變換的一般形式，然后將這些獨(dú)立的變換聯(lián)系起來求出連桿n相對連桿0的位置和姿態(tài)。,,相鄰連桿間坐標(biāo)變換公式,,,建立 {P}、{Q}和{R}3個中間坐標(biāo)系，其中{i}和{i-1}是固定在連桿 i 和 i-1 上的固連坐標(biāo)系，如圖3-13所示。,圖3-13中間坐標(biāo)系選擇示意圖,{i-1} ? {R}，對應(yīng)變換Rot(x, ?i-1 )；,1. 繞 Xi-1 軸旋轉(zhuǎn) ?i-1角,2. 沿 XR軸平移 ai-1,{R}

17、 ? {Q}，對應(yīng)變換Trans(ai-1 ,0,0)；,3. 繞 ZQ軸旋轉(zhuǎn) ?i 角,{Q} ? {P}，對應(yīng)變換Rot(z, ?i ),4. 沿 ZP 軸平移 di,{P} ? {i}，對應(yīng)變換Trans(0 ,0, di),因?yàn)樗凶儞Q都是相對于動坐標(biāo)系的，所以坐標(biāo)系{i}和{i-1}之間的變換矩陣為：,= Rot(x,?i-1)Trans(ai-1,0,0)Rot(z,?i)Trans(0,0,di),14,其中各獨(dú)立

18、變換矩陣如下：,Rot(x,?i-1)Trans(ai-1,0,0)Rot(z,?i)Trans(0,0,di),連桿間的通用變換公式：,對于任意的n連桿機(jī)械臂，只要給出各連桿的DH參數(shù)，即可以計(jì)算機(jī)械臂末端在固定坐標(biāo)系{0}下表示的變換矩陣（位置和姿態(tài)）,,,因此，采用DH規(guī)則選擇連桿坐標(biāo)系，并用DH參數(shù)描述連桿，可以非常容易地獲得機(jī)械臂的變換矩陣，關(guān)鍵是首先獲得機(jī)械臂的DH參數(shù)描述。,15,例3-3 利用表3-1的DH參數(shù)計(jì)算個連桿

19、的變換矩陣，并計(jì)算末端連桿現(xiàn)對固定坐標(biāo)系的變換矩陣。,,，,,其中c123 = cos(q1+q2+q3)，s123 = sin(q1+q2+q3)。從最后一式可以看出，坐標(biāo)系{3}的原點(diǎn)坐標(biāo)與（3-10）式的結(jié)果完全相同。,16,PUMA560工業(yè)機(jī)器人運(yùn)動學(xué),右圖所示PUMA560是一個6自由度工業(yè)機(jī)器人，所有關(guān)節(jié)均為轉(zhuǎn)動關(guān)節(jié)。,圖3-16,圖3-15,機(jī)器人的連桿參數(shù)如表3-2所示。,圖3-15和圖3-16給出了所有關(guān)節(jié)角為零位時

20、，連桿坐標(biāo)系的分布情況。與大多數(shù)工業(yè)機(jī)器人一樣，PUMA560關(guān)節(jié)4、5和6的軸線相交于同一點(diǎn)，且交點(diǎn)與坐標(biāo)系{4}、{5}和{6}的坐標(biāo)原點(diǎn)重合。后面將介紹如此設(shè)計(jì)的原因。,17,,,,,表3-2 PUMA560 連桿參數(shù)表,將相應(yīng)的參數(shù)代入（3-16）得各連桿的變換矩陣如下：,,,,將以上變換矩陣連乘即可得到,因?yàn)樵诘?章逆運(yùn)動學(xué)求解需要，這里計(jì)算一些中間結(jié)果：,,,18,最終得到六個連桿坐標(biāo)變換矩陣的乘積：,其中各元素值為：,,上

21、式即為PUMA560的運(yùn)動學(xué)方程，給出了機(jī)器人末端坐標(biāo)系{6}相對于基座固定坐標(biāo)系{0}的位姿。顯然，手工計(jì)算6自由度機(jī)器人的運(yùn)動學(xué)方程還是比較復(fù)雜的，但是，采用計(jì)算機(jī)編程實(shí)現(xiàn)運(yùn)動學(xué)計(jì)算非常容易。只需要輸入機(jī)器人的DH參數(shù)，再利用（3-16）式，六個矩陣連乘即可獲得運(yùn)動學(xué)方程式（3-24）。,坐標(biāo)系的標(biāo)準(zhǔn)命名規(guī)則,,圖3-17 標(biāo)準(zhǔn)坐標(biāo)系,為了分析處理方便，機(jī)器人和工作空間一般采用規(guī)范的命名，并采用“標(biāo)準(zhǔn)”的名字對各種坐標(biāo)系命名。圖3-

22、17表示了5個坐標(biāo)系，并給出了標(biāo)準(zhǔn)命名。,基坐標(biāo)系{B}固連于機(jī)器人的基座上，就是上節(jié)介紹的坐標(biāo)系{0}。在連桿描述時經(jīng)常稱之為連桿0。,基坐標(biāo)系{B},{S}一般固連于機(jī)器人工作臺的一個角上，有時稱之為任務(wù)坐標(biāo)系，機(jī)器人的所有運(yùn)動都是相對于工作臺坐標(biāo)系{S}執(zhí)行的。工作臺坐標(biāo)系{S}通常根據(jù)基坐標(biāo)系{B}來確定，兩個坐標(biāo)系都是固定坐標(biāo)系。,工作臺坐標(biāo)系{S},{W}固連于機(jī)械臂末端連桿，因此也稱為坐標(biāo)系{n}。一般情況下腕部坐標(biāo)系的原

23、點(diǎn)位于機(jī)械臂的手腕上。 {W}也是相對于基坐標(biāo)系{B}定義的。,腕部坐標(biāo)系{W},19,{T}一般固連于機(jī)器人所夾持工具的末端。工具坐標(biāo)系{T}通常根據(jù)腕部坐標(biāo)系{W}定義。,工具坐標(biāo)系{T},目標(biāo)坐標(biāo)系{G},目標(biāo)坐標(biāo)系{G}是機(jī)器人移動工具時對期望工具位置的描述。在機(jī)器人運(yùn)動結(jié)束時，工具坐標(biāo)系{T}與目標(biāo)坐標(biāo)系{G}重合。目標(biāo)坐標(biāo)系{G}通常相對工作臺坐標(biāo)系{S}定義。,機(jī)器人完成期望操作的主要任務(wù)之一是對所夾持的工具進(jìn)行定位。圖3

24、-17中虛線表示了坐標(biāo)系間的描述關(guān)系，可以用變換矩陣表示這種描述關(guān)系：,工具坐標(biāo)系定位,1. 確定變換關(guān)系,2. 根據(jù)機(jī)器人完成期望操作任務(wù),=,的要求得到變換方程,,?,3. 采用逆運(yùn)動學(xué)求解（下章介紹）期望的機(jī)械臂各關(guān)節(jié)變量。,4. 根據(jù)期望的關(guān)節(jié)變量控制機(jī)械臂運(yùn)動，工具坐標(biāo)系達(dá)到期望位姿。,20,第4 章 機(jī)器人逆運(yùn)動學(xué),已知機(jī)器人末端的位置和姿態(tài)（坐標(biāo)變換矩陣）確定各關(guān)節(jié)變量的值是一個非常關(guān)鍵的問題。該問題就是機(jī)器人逆運(yùn)動學(xué)將要

25、解決的問題。,逆運(yùn)動學(xué)問題的可解性,機(jī)械臂逆運(yùn)動學(xué)問題就是已知,的數(shù)值，求各關(guān)節(jié)變量。,從（3-24）中求解關(guān)節(jié)變量q1, q2, ?, q6是一個非線性問題。,解的存在性,逆運(yùn)動學(xué)問題解是否存在完全取決于機(jī)械臂的工作空間。所謂工作空間是指機(jī)械臂末端執(zhí)行器所能達(dá)到的空間位姿的集合。,當(dāng)期望位姿位于機(jī)械臂的工作空間之外時，逆運(yùn)動學(xué)問題無解。,期望機(jī)械臂末端達(dá)到B點(diǎn),對于給定的機(jī)械臂，其工作空間是固定的。而對于少于6個自由度的機(jī)械臂，它在三

26、維空間內(nèi)不能達(dá)到全部位姿。所以通用工業(yè)機(jī)器人一般都設(shè)計(jì)成6個自由度。,21,,多解問題,逆運(yùn)動學(xué)求解的另一個問題是多解問題。,圖4-2 平面機(jī)械臂，有兩個解,如圖4-2 所示的平面機(jī)械臂有兩個解，虛線表示另外一個解。,逆運(yùn)動學(xué)解的個數(shù)取決于機(jī)械臂關(guān)節(jié)的數(shù)量，同時與連桿參數(shù)和關(guān)節(jié)運(yùn)動范圍有關(guān)。PUMA560工業(yè)機(jī)器人一般存在8個解。,逆運(yùn)動學(xué)問題解法,從運(yùn)動學(xué)方程中求解關(guān)節(jié)變量q1, q2, ?, q6是一個非線性方程組求解問題。而非線性

27、方程組求解方法分為封閉（解析）解法和數(shù)值解法兩大類。,數(shù)值解法不適用，一是機(jī)械臂操作需要頻繁求解逆運(yùn)動學(xué)問題，數(shù)值解法計(jì)算量比較大；二是數(shù)值解法不能保證求出全部解。所以逆運(yùn)動學(xué)問題一般只采用封閉（解析）解法。,機(jī)器人逆運(yùn)動學(xué)問題涉及一個復(fù)雜的非線性方程組求解，而從數(shù)學(xué)角度分析一般的非線性方程組經(jīng)常沒有封閉（解析）解。,對于機(jī)械臂逆運(yùn)動學(xué)問題存在解決方案，因?yàn)闄C(jī)械臂是人造機(jī)構(gòu)，只需將其設(shè)計(jì)成存在封閉解的結(jié)構(gòu)即可解決該問題。,22,理論上已

28、經(jīng)證明，對于6自由度機(jī)械臂，存在封閉解的充分條件是有相鄰的三個關(guān)節(jié)軸相交于一點(diǎn)。因此，已經(jīng)設(shè)計(jì)出來的6自由度機(jī)械臂幾乎都有三個相交的關(guān)節(jié)軸，例如PUMA560的4、5、6軸交于一點(diǎn)。,歐拉變換解,式（2-40）給出了采用歐拉角表示的坐標(biāo)變換，其逆問題是給定旋轉(zhuǎn)矩陣Rzyz，確定對應(yīng)的三個歐拉角。,假設(shè)給定的旋轉(zhuǎn)矩陣表示，根據(jù)歐拉變換方程可得9個方程,,可以由最后一個方程求出?，再由倒數(shù)第二個方程求出?最后根據(jù)倒數(shù)第四個方程計(jì)算出?。,這

29、種方法的不足是反正弦和反余弦解的不確定性和解的精度問題。,實(shí)際問題一般用雙變量反正切函數(shù)計(jì)算相應(yīng)的角度。,23,雙變量反正切函數(shù),atan2的精確程度對其整個定義域都是一樣的。高級編程語言，如C和Matlab等，提供標(biāo)準(zhǔn)庫函數(shù)供編程者調(diào)用。,雙變量反正切函數(shù),在三角函數(shù)求解時，通常采用雙變量反正切函數(shù)atan2(y, x)來確定角度。atan2提供二個自變量，即縱坐標(biāo)和橫坐標(biāo)，見圖4.3。,當(dāng) -π≤θ≤π時，由atan2反求角度過程中

30、，同時檢查y和x的符號來確定其所在象限。該函數(shù)也能檢驗(yàn)什么時候x或y為0，并反求出正確的角度。,,根據(jù)（2-39）和（2-40）式可知：,,（4-2）,上式中，矩陣兩邊對應(yīng)（2，3）元素相等得-axs?+ayc?=0?tan?=ay/ax，所以得?的兩個解,?=atan2(ay,ax)，?= ?+?,24,，計(jì)算各關(guān)節(jié)變量q1, q2, ?, q6。,25,對應(yīng)（1，3）和（3，3）元素相等得 sq= axc?+ays?，cq=az 所

31、以有,?=atan2(c?ax+s?ay, az),對應(yīng)（2，1）和（2，2）元素相等得s?= -nxs?+nyc?，c?=-oxs?+oyc?所以有,?=atan2(-nxs?+nyc?, -oxs?+oyc?),歐拉變換解計(jì)算過程并不復(fù)雜，該問題一般存在兩個解。,當(dāng)?=0時，根據(jù)（4-1）知ay=ax=0，此時（4-3）將不能確定?的值。此時對應(yīng)繞Z軸連續(xù)做兩次旋轉(zhuǎn)，不能確定每次轉(zhuǎn)的角度屬于歐拉角奇異情況。,PUMA560逆運(yùn)動學(xué),

32、將研究PUMA560的逆運(yùn)動學(xué)封閉解，一般的6自由度工業(yè)機(jī)器人逆運(yùn)動學(xué)可以參考該方法進(jìn)行求解。,該問題是已知變換矩陣,各連桿坐標(biāo)系變換關(guān)系如下：,,?,與歐拉角求解類似，根據(jù)第3章PUMA560運(yùn)動學(xué)（3-21）和（3-23）式得,,其中（3-22）的最后三個數(shù)如下：,（4-6）,,令（4-6）式兩邊元素（2, 4）相等，得到,-pxs1+pyc1=d3,做三角恒等變換,px=?cos?，py=?sin?,其中,-c?s1+s?c1=d

33、3/r ? sin(?-?1)=d3/r,所以,因此,最后?1的解可以寫為,26,（4-10）,（4-13）,采用類似的方法可以求得其它關(guān)節(jié)角的解可以寫為,?2=?23-?3 （ 4-17）,（4-19）,（4-22）,,（4-24）,由于（4-10）式和（4-13）式的?1和?3各有兩個解，另外機(jī)械臂腕關(guān)節(jié)“翻轉(zhuǎn)”可以得到?4~?6的另一組解,（