機器人學(xué)-運動學(xué)部分(2006)

1、齊次坐標(biāo)和齊次變換知識點：齊次坐標(biāo)和齊次變換知識點： n 點和面的齊次坐標(biāo)和齊次變換點和面的齊次坐標(biāo)和齊次變換n 三個基本旋轉(zhuǎn)矩陣三個基本旋轉(zhuǎn)矩陣n 齊次變換的幾何意義齊次變換的幾何意義n 絕對變換：絕對變換：如果所有的變換都是如果所有的變換都是相對于固定坐標(biāo)系相對于固定坐標(biāo)系中各坐中各坐標(biāo)軸旋轉(zhuǎn)或平移，則依次標(biāo)軸旋轉(zhuǎn)或平移，則依次左乘左乘，稱為，稱為絕對變換絕對變換。n 相對變換：相對變換：如果動坐標(biāo)系如果動坐標(biāo)系相對于自身坐標(biāo)系相對于自身坐標(biāo)系的當(dāng)前坐標(biāo)軸的當(dāng)前坐標(biāo)軸旋轉(zhuǎn)或平移，則齊次變換為依次旋轉(zhuǎn)或平移，則齊次變換為依次右乘右乘，稱為，稱為相對變換相對變換。n 繞任意軸旋轉(zhuǎn)，繞任意軸旋轉(zhuǎn)

2、，5 5步順序步順序n 透視變換透視變換機器人運動學(xué)機器人運動學(xué)第三章第三章 機器人運動學(xué)機器人運動學(xué) n 機器人運動學(xué)主要是把機器人機器人運動學(xué)主要是把機器人相對于固定參考相對于固定參考系系的運動作為的運動作為時間的函數(shù)時間的函數(shù)進行分析研究，而不進行分析研究，而不考慮引起這些運動的力和力矩考慮引起這些運動的力和力矩n 也就是要把機器人的也就是要把機器人的空間位移空間位移解析地表示為時解析地表示為時間的函數(shù)，特別是研究機器人間的函數(shù)，特別是研究機器人關(guān)節(jié)變量空間和關(guān)節(jié)變量空間和機器人末端執(zhí)行器位置和姿態(tài)之間機器人末端執(zhí)行器位置和姿態(tài)之間的關(guān)系的關(guān)系n 本章將討論機器人運動學(xué)幾個具有實際意義的

3、本章將討論機器人運動學(xué)幾個具有實際意義的基本問題?；締栴}。 3.1 3.1 機器人運動學(xué)所討論的問題機器人運動學(xué)所討論的問題 3.1.1 3.1.1 研究的對象研究的對象 機器人從機構(gòu)形式上分為兩種，一種是關(guān)節(jié)式機器人從機構(gòu)形式上分為兩種，一種是關(guān)節(jié)式串聯(lián)機器人，另外一種是并聯(lián)機器人，如圖：串聯(lián)機器人，另外一種是并聯(lián)機器人，如圖： PUMA560HexapodFanuc manipulator研究的問題研究的問題: :n 運動學(xué)正問題運動學(xué)正問題-已知桿件幾何參數(shù)和關(guān)節(jié)角矢量，求操已知桿件幾何參數(shù)和關(guān)節(jié)角矢量，求操作機末端執(zhí)行器相對于固定參考作標(biāo)的位置和姿態(tài)（作機末端執(zhí)行器相對于固定參考作標(biāo)

4、的位置和姿態(tài)（齊齊次變換問題次變換問題）。）。n 運動學(xué)逆問題運動學(xué)逆問題-已知操作機桿件的幾何參數(shù)，給定操作已知操作機桿件的幾何參數(shù)，給定操作機末端執(zhí)行器相對于參考坐標(biāo)系的期望位置和姿態(tài)（位機末端執(zhí)行器相對于參考坐標(biāo)系的期望位置和姿態(tài)（位置），操作機能否使其末端執(zhí)行器達到這個預(yù)期的位姿？置），操作機能否使其末端執(zhí)行器達到這個預(yù)期的位姿？如能達到，那么操作機有幾種不同形態(tài)可以滿足同樣的如能達到，那么操作機有幾種不同形態(tài)可以滿足同樣的條件？條件？運 動 學(xué) 正 問 題關(guān) 節(jié) 角桿 件 參 數(shù)末 端 執(zhí) 行 器運 動 學(xué) 正 問 題關(guān) 節(jié) 角桿 件 參 數(shù)運動學(xué)研究的問題運動學(xué)研究的問題Where

5、 is my hand?Direct KinematicsHERE!How do I put my hand here?Inverse Kinematics: Choose these angles!3.2 3.2 機器人桿件，關(guān)節(jié)和它們的參機器人桿件，關(guān)節(jié)和它們的參數(shù)數(shù) 3.2.1 3.2.1 桿件，關(guān)節(jié)桿件，關(guān)節(jié)n操作機由一串用轉(zhuǎn)動或平移（棱操作機由一串用轉(zhuǎn)動或平移（棱柱形）關(guān)節(jié)連接的剛體（桿件）柱形）關(guān)節(jié)連接的剛體（桿件）組成組成n每一對關(guān)節(jié)桿件構(gòu)成一個每一對關(guān)節(jié)桿件構(gòu)成一個自由度自由度，因此因此N N個自由度的操作機就有個自由度的操作機就有N N對對關(guān)節(jié)關(guān)節(jié)- -桿件。桿件。n0 0號

6、桿件（一般不把它當(dāng)作機器號桿件（一般不把它當(dāng)作機器人的一部分）固聯(lián)在機座上，通人的一部分）固聯(lián)在機座上，通常在這里建立一個固定參考坐標(biāo)常在這里建立一個固定參考坐標(biāo)系，最后一個桿件與工具相連系，最后一個桿件與工具相連n關(guān)節(jié)和桿件均由底座向外順序排關(guān)節(jié)和桿件均由底座向外順序排列，每個桿件最多和另外兩個桿列，每個桿件最多和另外兩個桿件相聯(lián)，不構(gòu)成閉環(huán)。件相聯(lián)，不構(gòu)成閉環(huán)。 關(guān)節(jié)：關(guān)節(jié)：n一般說來，兩個桿件間是用一般說來，兩個桿件間是用低副低副相聯(lián)的相聯(lián)的n只可能有只可能有6 6種低副關(guān)節(jié)：種低副關(guān)節(jié)：旋轉(zhuǎn)旋轉(zhuǎn)（轉(zhuǎn)動）、（轉(zhuǎn)動）、棱柱棱柱（移動）、（移動）、圓柱形圓柱形、球形球形、螺旋螺旋和和平面平面

7、，其中只有，其中只有旋轉(zhuǎn)和棱柱形旋轉(zhuǎn)和棱柱形關(guān)關(guān)節(jié)是串聯(lián)機器人操作機常見的，各種低副形狀如下圖所節(jié)是串聯(lián)機器人操作機常見的，各種低副形狀如下圖所示：示：旋轉(zhuǎn)旋轉(zhuǎn)棱柱形棱柱形柱形柱形球形球形螺旋形螺旋形平面平面AiAi+1Ai-1 桿件參數(shù)的定義桿件參數(shù)的定義- - 和和n li 由運動學(xué)的觀點來看，桿件的作用僅在于它能保由運動學(xué)的觀點來看，桿件的作用僅在于它能保持其兩端關(guān)節(jié)間的形態(tài)不變。這種形態(tài)由兩個參持其兩端關(guān)節(jié)間的形態(tài)不變。這種形態(tài)由兩個參數(shù)決定，一是桿件的長度數(shù)決定，一是桿件的長度 li，一個是桿件的扭轉(zhuǎn)，一個是桿件的扭轉(zhuǎn)角角 iAiAi+1iiliili 桿件參數(shù)的定義桿件參數(shù)的定義-

8、 - 和和n L和和L 在在A軸線上軸線上的交點之間的距離的交點之間的距離n L和和L 之間的夾角，之間的夾角，由由L 轉(zhuǎn)向轉(zhuǎn)向L，由右手，由右手定則決定正負，對于定則決定正負，對于旋轉(zhuǎn)關(guān)節(jié)它是個變量旋轉(zhuǎn)關(guān)節(jié)它是個變量 確定桿件相對位置關(guān)系，由另外確定桿件相對位置關(guān)系，由另外2個參數(shù)決定，一個是桿個參數(shù)決定，一個是桿件的距離件的距離 ，一個是桿件的回轉(zhuǎn)角，一個是桿件的回轉(zhuǎn)角 iidiidiAiAi+1iilid1iliAi-1id 移動關(guān)節(jié)桿件參數(shù)的定義移動關(guān)節(jié)桿件參數(shù)的定義n 確定桿件間形態(tài)的確定桿件間形態(tài)的2個參數(shù)個參數(shù)Li與與i與旋轉(zhuǎn)關(guān)節(jié)是一樣的。確與旋轉(zhuǎn)關(guān)節(jié)是一樣的。確定桿件相對位置關(guān)

9、系的定桿件相對位置關(guān)系的2個參數(shù)則相反。這里個參數(shù)則相反。這里i為常數(shù)，為常數(shù)，di為變量。為變量。n 上述上述4個參數(shù)，就確定了桿件的結(jié)構(gòu)形態(tài)和相鄰桿件相對位個參數(shù)，就確定了桿件的結(jié)構(gòu)形態(tài)和相鄰桿件相對位置關(guān)系，在轉(zhuǎn)動關(guān)節(jié)中，置關(guān)系，在轉(zhuǎn)動關(guān)節(jié)中，Li, i, di是固定值，是固定值，i是變量。在是變量。在移動關(guān)節(jié)中，移動關(guān)節(jié)中，Li, i, i是固定值，是固定值， di 是變量。是變量。3.3 機器人關(guān)節(jié)坐標(biāo)系的建立機器人關(guān)節(jié)坐標(biāo)系的建立o oO Oo on n- -1 1nXn 對于每個桿件都可以在關(guān)節(jié)軸處建立一個正規(guī)的笛卡兒坐對于每個桿件都可以在關(guān)節(jié)軸處建立一個正規(guī)的笛卡兒坐標(biāo)系（標(biāo)系

10、（xi, yi, zi），（），（i=1, 2, , n），），n是自由度數(shù)，再加是自由度數(shù)，再加上基座坐標(biāo)系，一共有（上基座坐標(biāo)系，一共有（n+1）個坐標(biāo)系。）個坐標(biāo)系。n 基座坐標(biāo)系基座坐標(biāo)系 定義為定義為0號坐標(biāo)系（號坐標(biāo)系（x0, y0, z0）,它也是機它也是機器人的慣性坐標(biāo)系，器人的慣性坐標(biāo)系，0號坐標(biāo)系在基座上的位置和方向可號坐標(biāo)系在基座上的位置和方向可任選，但任選，但 軸線必須與關(guān)節(jié)軸線必須與關(guān)節(jié)1的軸線重合，位置和方向可的軸線重合，位置和方向可任選；任選；n 最后一個坐標(biāo)系（最后一個坐標(biāo)系（n關(guān)節(jié)），可以設(shè)在手的任意部位，但關(guān)節(jié)），可以設(shè)在手的任意部位，但必須保證必須保證 與

11、與 垂直。垂直。n 機器人關(guān)節(jié)坐標(biāo)系的建立主要是為了描述機器人各桿件和終機器人關(guān)節(jié)坐標(biāo)系的建立主要是為了描述機器人各桿件和終端之間的相對運動，對建立運動方程和動力學(xué)研究是基礎(chǔ)性端之間的相對運動，對建立運動方程和動力學(xué)研究是基礎(chǔ)性的工作。的工作。n 為了描述機器人各桿件和終端之間轉(zhuǎn)動或移動關(guān)系，為了描述機器人各桿件和終端之間轉(zhuǎn)動或移動關(guān)系，Denavit和和Hartenberg于于1955年提出了一種為運動鏈中每個桿件建立年提出了一種為運動鏈中每個桿件建立附體坐標(biāo)系的矩陣方法附體坐標(biāo)系的矩陣方法（D-H方法）方法） ，建立原則如下：，建立原則如下： D-H關(guān)節(jié)坐標(biāo)系建立原則關(guān)節(jié)坐標(biāo)系建立原則u右

12、手坐標(biāo)系右手坐標(biāo)系u原點原點Oi：設(shè)在設(shè)在Li與與Ai+1軸線的交點上軸線的交點上 uZi軸軸： 與與Ai+1關(guān)節(jié)軸重合，指向任意關(guān)節(jié)軸重合，指向任意 uXi軸軸： 與公法線與公法線Li重合，指向沿重合，指向沿Li由由Ai軸線指向軸線指向Ai+1軸線軸線 uYi軸軸： 按右手定則按右手定則 關(guān)節(jié)坐標(biāo)系的建立原則關(guān)節(jié)坐標(biāo)系的建立原則AiAi+1iilid1iliAi-11iz1ix1iy1ioizixiyion 原點原點Oi：設(shè)在：設(shè)在Li與與Ai+1軸線的交點上軸線的交點上 n Zi軸軸：與：與Ai+1關(guān)節(jié)軸關(guān)節(jié)軸重合，指向任意重合，指向任意 n Xi軸軸：與公法線：與公法線Li重合，指向沿重

13、合，指向沿Li由由Ai軸線指向軸線指向Ai+1軸線軸線 n Yi軸軸：按右手定則：按右手定則 沿 xi 軸， zi-1 軸與 xi 軸交點到 0i 的距離 繞 xi 軸，由 zi-1 轉(zhuǎn)向zi 沿 zi-1 軸，zi-1 軸和 xi 交點至0i 1 坐標(biāo)系原點的距離 繞 zi-1 軸，由 xi-1轉(zhuǎn)向 xi 兩種特殊情況兩種特殊情況n 兩軸相交，怎么建立坐兩軸相交，怎么建立坐標(biāo)系？標(biāo)系？ 0iAi與與Ai+1關(guān)節(jié)軸線的交關(guān)節(jié)軸線的交點；點； ZiAi+1軸線；軸線； XiZi和和Zi-1構(gòu)成的平面的構(gòu)成的平面的法線法線 ； Yi右手定則；右手定則； i i- -1 1i i AiA Ai i+

14、 +1 1o oi iz zi i- -1 1z zi ix xi iy yi in兩軸平行，怎么建立坐標(biāo)系兩軸平行，怎么建立坐標(biāo)系(Ai與與Ai+1平行平行)？先建立先建立 0i-1然后建立然后建立0i+1最后建立最后建立 0i i-1i-1O OD D注意：注意： 由于由于Ai和和Ai+1平行，所以公法線平行，所以公法線 任意點在任意點在A點位置；點位置； 按照先前的定義，按照先前的定義，di為為Oi-1點和點和A點之間的距離，點之間的距離，di+1為為B點和點和C點間點間的距離，這樣設(shè)定可以的，但我們可以變更一下，將的距離，這樣設(shè)定可以的，但我們可以變更一下，將0i點放在點放在C點，點，

15、定義定義Oi在在Li+1和和Ai+1軸的交點上，這樣使軸的交點上，這樣使di+1=0使計算簡便，此時使計算簡便，此時di= 相鄰相鄰關(guān)節(jié)坐標(biāo)系間的齊次變換過程關(guān)節(jié)坐標(biāo)系間的齊次變換過程 機器人運動學(xué)正解機器人運動學(xué)正解n將將xi-1軸繞軸繞 zi-1 軸轉(zhuǎn)軸轉(zhuǎn) i 角度，將其與角度，將其與xi軸平軸平行；行；n沿沿 zi-1軸平移距離軸平移距離 di ，使使 xi-1 軸與軸與 xi 軸重合；軸重合；n沿沿 xi 軸平移距離軸平移距離 Li，使兩坐標(biāo)系原點及使兩坐標(biāo)系原點及x軸軸重合；重合；n繞繞 xi 軸轉(zhuǎn)軸轉(zhuǎn) i 角度，角度，兩坐標(biāo)系完全重合兩坐標(biāo)系完全重合AiAi+1iilid1iliA

16、i-11iz1ix1iy1ioizixiyio),(),(),(),(A111iiiiransiiransiiiixRlxTdZTZR 機器人的運動學(xué)正解方程機器人的運動學(xué)正解方程001112iiiTAAA D-H變換矩陣變換矩陣iiA1100010000100001id1000010000cossin00sincosiiii100001000010001il10000cossin00sincos00001iiii1000cossin0sincossincoscossincossinsinsincoscosiiiiiiiiiiiiiiiiidaa=3.4 3.4 例題例題試求立方體中心在機座坐

17、標(biāo)系試求立方體中心在機座坐標(biāo)系00中的位置中的位置該手爪從上方把物體抓起，同時手爪的開合方向與物體的該手爪從上方把物體抓起，同時手爪的開合方向與物體的Y Y軸同向，軸同向，那么，求手爪相對于那么，求手爪相對于00的姿態(tài)是什么？的姿態(tài)是什么？ 在機器人工作臺上加裝一電視攝像機，攝像機可見到固聯(lián)在機器人工作臺上加裝一電視攝像機，攝像機可見到固聯(lián)著著6DOF關(guān)節(jié)機器人的機座坐標(biāo)系原點，它也可以見到被操作關(guān)節(jié)機器人的機座坐標(biāo)系原點，它也可以見到被操作物體（立方體）的中心，如果在物體中心建一局部坐標(biāo)系，則物體（立方體）的中心，如果在物體中心建一局部坐標(biāo)系，則攝像機所見到的這個物體可由齊次變換矩陣攝像機所

18、見到的這個物體可由齊次變換矩陣T1來表示，如果攝來表示，如果攝像機所見到的機座坐標(biāo)系為矩陣像機所見到的機座坐標(biāo)系為矩陣T2表示。表示。1000101-002001-010-001T100091-00100011010T21xyz解解1 1：xyzz機y機z物y物x物oO機O物 T T 21物機機攝物攝求，已知TTT TT 11 -2）（有：物攝攝機物機TTT 100091-00100011010 1000101-002001-0100011000110010001-11010 O物根據(jù)T1畫出O機根據(jù)T2畫出因此物體位于機座坐標(biāo)系的（因此物體位于機座坐標(biāo)系的（11，10，1）T處，它的處，它的

19、X，Y，Z軸分別與機座坐標(biāo)系的軸分別與機座坐標(biāo)系的-Y，X，Z軸平行。軸平行。 解解2 2：xyzz機y機z物y物x物oO機O物手爪機實際要求Tpzazsznzpyaysynypxaxsxnx1000 向重合手爪開合方向與物體ya:Ts001有方向相反方向物體的從上向下抓，指出手爪zab:Ta 100則有Tkjikjiasnc01000100001:1-00001010因此：姿態(tài)矩陣為重合時與物體中心當(dāng)手爪中心100011-001000111010 T物機OsnayzxX機舉例：舉例：StanfordStanford機器人機器人A1A2A3A4A5A6d1z1x1y1O1d2z2x2y2O2z

20、3y3x3O3y4z4x4O4z5y5x5O534545,0o o odd重重合合d3z6x6y6O6d6z0y0 x0O0 為右手坐標(biāo)系為右手坐標(biāo)系 原點原點Oi： Ai與與Ai+1關(guān)節(jié)軸線的交點關(guān)節(jié)軸線的交點 Zi軸：與軸：與Ai+1關(guān)節(jié)軸關(guān)節(jié)軸重合，指向任意重合，指向任意 Xi軸：軸： Zi和和Zi-1構(gòu)構(gòu)成的面的法線成的面的法線 Yi軸：按右手定則軸：按右手定則 Li 沿沿 xi 軸，軸， zi-1 軸與軸與 xi 軸交點到軸交點到 0i 的距離的距離i 繞繞 xi 軸，由軸，由 zi-1 轉(zhuǎn)向轉(zhuǎn)向zidi 沿沿 zi-1 軸，軸，zi-1 軸和軸和 xi 交點至交點至0i 1 坐標(biāo)系

21、原坐標(biāo)系原 點的距離點的距離i 繞繞 zi-1 軸，由軸，由 xi-1轉(zhuǎn)向轉(zhuǎn)向 xi解：解： 工作空間工作空間n 工作空間工作空間: : 末端操作手可以到達的空間位置集合末端操作手可以到達的空間位置集合n 如何獲得工作空間如何獲得工作空間: : 利用正運動學(xué)模型利用正運動學(xué)模型, ,改變關(guān)節(jié)改變關(guān)節(jié)變量值變量值n 靈活空間靈活空間: : 末端操作手可以以任何姿態(tài)到達的空間末端操作手可以以任何姿態(tài)到達的空間位置集合位置集合n 可達空間可達空間: : 末端操作手可以至少以一個姿態(tài)到達的末端操作手可以至少以一個姿態(tài)到達的空間位置集合空間位置集合如何確定可達空間如何確定可達空間? ?首先，首先，令令

22、3 3變變化化 示例示例: : 平面平面 3 3連桿機器人連桿機器人123112123123112123123123123123coscoscossinsinsin , xlllylllllllll 3種最常見的歐拉角類型種最常見的歐拉角類型步步1步步2步步3類型類型1繞繞OZ軸轉(zhuǎn)軸轉(zhuǎn)角角繞當(dāng)前繞當(dāng)前OU 軸轉(zhuǎn)軸轉(zhuǎn)角角繞當(dāng)前繞當(dāng)前OW軸轉(zhuǎn)軸轉(zhuǎn)角角類型類型2繞繞OZ軸轉(zhuǎn)軸轉(zhuǎn)角角繞當(dāng)前繞當(dāng)前OV 軸轉(zhuǎn)軸轉(zhuǎn)角角繞當(dāng)前繞當(dāng)前OW軸轉(zhuǎn)軸轉(zhuǎn)角角類型類型3繞繞OX軸轉(zhuǎn)軸轉(zhuǎn)角角繞繞OY軸轉(zhuǎn)軸轉(zhuǎn)角角繞繞OZ軸轉(zhuǎn)軸轉(zhuǎn)角角uvwx(u)y (v)z (w)ouvwu?v?W?),(ZR),(R),(wR N0T10

23、0000000110000cssccssccsscccssssccccssscccsssccsscscscc類型類型1：表示法通常用于陀螺運動：表示法通常用于陀螺運動 類型類型2：所得的轉(zhuǎn)動矩陣為右乘所得的轉(zhuǎn)動矩陣為右乘 10000c0s-010s0c 10000),(),v(),(RcssccsscwRRZR1000pzpyRpxTccsssssccscsscccssccssccsscccccscssccssccssscssscsccsssccccssccssccsscxRyRz000010010010000),(),(),RR（ZYX偏航偏航俯仰俯仰橫滾橫滾: : 已知關(guān)節(jié)角度或位移，計算

24、已知關(guān)節(jié)角度或位移，計算末端操作手的對應(yīng)位姿末端操作手的對應(yīng)位姿. .: : 已知已知末端操作手的位姿，求末端操作手的位姿，求解對應(yīng)的關(guān)節(jié)變量解對應(yīng)的關(guān)節(jié)變量. .可能存在多解或無解可能存在多解或無解通常需多次求解非線性超越方程通常需多次求解非線性超越方程3.6 3.6 運動學(xué)逆問題運動學(xué)逆問題 解的存在性解的存在性存在雙解存在雙解! 求解方法求解方法0140i000222018040i100040i0140i000222018040i 25001160i0220in 逆運動學(xué)的定義逆運動學(xué)的定義n 逆運動學(xué)的存在性逆運動學(xué)的存在性n 逆運動學(xué)的可解性逆運動學(xué)的可解性n 逆運動學(xué)的多解性（剔除

25、辦法）逆運動學(xué)的多解性（剔除辦法）n 逆運動學(xué)解法（數(shù)值解、解析解）逆運動學(xué)解法（數(shù)值解、解析解）How do I put my hand here?Inverse Kinematics: Choose these angles! Paul 等人提出的方法等人提出的方法65544332211060TTTTTTT TTTTTT T 6554433221601 -10）（1 q65544332601 -101 -21TTTTTTT）（）（2q65601 -101 -21132143154TTTT)T()T()T(）（）（5 qE601 -101 -65TT) T( ）（6 q100060pzazs

26、znzpyaysynypxaxsxnxT),(2xyarctg)/(xyarctg(arccos)cos(cos0/ )(cos180, 0dd為負為正均為負為正為負均為正yxyxyxyxxytg,090901801809090),(200000001 例：歐拉角第一種類型，求逆例：歐拉角第一種類型，求逆),(ZR),(R),(wR N0T100000000110000cssccssccsscccssssccccssscccsssccsscscscc類型類型1：表示法通常用于陀螺運動：表示法通常用于陀螺運動 步步1步步2步步3類型類型1繞繞OZ軸轉(zhuǎn)軸轉(zhuǎn)角角繞當(dāng)前繞當(dāng)前OU 軸轉(zhuǎn)軸轉(zhuǎn)角角繞當(dāng)前繞

27、當(dāng)前OW軸轉(zhuǎn)軸轉(zhuǎn)角角ccssssccscscazsznzaycaxssycsxsnycnxsaysaxcsyssxcnysnxccssccsscazsznzaysynyaxsxnxcssczcssccssccsscazsznzaysynyaxsxnx010000 00001 1000-01,R 1 100000000110000 1或，可得：而另兩個未知數(shù)在右邊在矩陣方程的左邊，未知數(shù)）左右兩邊，可使一個）左乘式（用）（解：解：),(2tan2111),(2tan01 -11 -1nysnxcsyssxctgnysnxcsyssxcsyssxcsnysnxccayaxtgayaxaysaxc）

28、元素分別對應(yīng)相等，）元素和（，使（所在象限。按照前面的定義，確定具體分析辦法靠結(jié)構(gòu)結(jié)束條件、剔除確定象限靠分子，分母的符號來多值解逆運動唯一解正運動總體來講于使用者的直覺用左乘還是右乘，取決解也可以用右乘的方法求）元素對應(yīng)相等，）元素和（，（,),(2)(tan-333211azaycaxstgazaycaxsazcaycaxss 斯坦福機器人運動學(xué)逆問題解斯坦福機器人運動學(xué)逆問題解6533211060AAAAT61T653321AAA式中：式中： yxyxpCpSpfzpfpSpCpf1113121111)()()(由兩端矩陣對應(yīng)元素相等可得：由兩端矩陣對應(yīng)元素相等可得： 作三角變換：作三角

29、變換： 式中：式中： 得到：得到： 即有：即有： 由由1, 4和和2, 4元素對應(yīng)相等，得：元素對應(yīng)相等，得： 6261121TTA6362132TTA高腕高腕低腕低腕2122212222 l lllyxcN000O0nasX0Y0Z0OnNdTNransTdwRdzdydxTT0N0),(),( 變化后）（NransTdwRdzdydxTdTT0NN0),(),( 記為TdwRdzdydxTdTTrans),(),(TEdwRdzdydxTdTrans),(),()T( . ),(),(的左邊在注意，稱為微動率，令TdTEdwRdzdydxTrans在忽略高次項在忽略高次項的情況下：微的情況

30、下：微動齊次變換與動齊次變換與次序無關(guān)次序無關(guān) dz)dR(z, y)dR(y, x)dx,R （假設(shè)：100001000000100000001000100000000001zcdzsdzsdzcdycdysdysdycdxcdxsdxsdxcd.,zdydxddddsin1996. 0cos 5Qo角度) 1 (10000101011000010101100001000010011000010001000110000100100001xdydxdzdydzdxdzyddzxddydxdyxdzddzdyxddydzdzdzdydydxdxdT忽略高次項忽略高次項y)dR(y, 3 x)dx

31、,R 2 z)dR(z, 1 、（、反過來，如果）相同與式（110000101011000010101z)dR(z, x)dx,R y)dR(y, Txdydxdzdydzdxdydxdzdydxyddzdyxdd平移：平移： 1000100010001dzdydxTr旋轉(zhuǎn)旋轉(zhuǎn)R R ，繞任意軸，繞任意軸 旋轉(zhuǎn)旋轉(zhuǎn) 角角: :dw,wddddddddddddddddddddwcos)cos(1rsinr)cos(1rrsinr)cos(1rrsinr)cos(1rrsinr)cos(1rrcos)cos(1rsinr)cos(1rrsinr)cos(1rrcos)cos(1rR2zxzyyzx

32、xzyyzx2yzyxzyx2x,1000010101sin, 1cosdrdrdrdrdrdrdddxyxzyz在微動范圍內(nèi)繞任意軸轉(zhuǎn)動在微動范圍內(nèi)繞任意軸轉(zhuǎn)動 角角, ,可以看作繞可以看作繞x,y,zx,y,z軸的微轉(zhuǎn)軸的微轉(zhuǎn)動的合成。因此：動的合成。因此： ddrzddryddrxdzyx,1000010101,xdydxdzdydzddwR因此：因此：因此微動率因此微動率= = EdwRdzdydxTrans,0000000dzxdyddyxdzddxydzd微動的齊次變換：微動的齊次變換：dT= T 己知變換矩陣己知變換矩陣 1000000131007010T,001 . 0kjid

33、kjidp6 . 003 . 0轉(zhuǎn)動：轉(zhuǎn)動： 平移：平移： 求求d T 解：解： 00006 . 001 . 0001 . 0003 . 000010000100001000011000011 . 0001 . 010000110006 . 010000103 . 000100009 . 01 . 0000001 . 03 . 0000100000013100701000006 . 001 . 0001 . 0003 . 0000TdT10009 . 01 . 0013101 . 03 . 7010dTT反過來：如果我們要求反過來：如果我們要求 在在 中的齊次交換矩陣為中的齊次交換矩陣為 no

34、oo1000000131007010T實際測得的為實際測得的為 10009 . 01 . 0013101 . 03 . 7010那么末端執(zhí)行器坐標(biāo)系要如何運動才能到達期望值？那么末端執(zhí)行器坐標(biāo)系要如何運動才能到達期望值？ 1000011 .0001 .0100001轉(zhuǎn)動：轉(zhuǎn)動： 10006.010000103.0001平移：平移： n前面研究的是動坐標(biāo)系前面研究的是動坐標(biāo)系On在在Oo中的中的b變換為變換為T，相對于基準(zhǔn)坐標(biāo)系作微平移和，相對于基準(zhǔn)坐標(biāo)系作微平移和微轉(zhuǎn)動，來求微動齊次交換。微轉(zhuǎn)動，來求微動齊次交換。n現(xiàn)在我們研究動坐標(biāo)系現(xiàn)在我們研究動坐標(biāo)系 On相對于自身相對于自身坐標(biāo)系做了微

35、位移或微轉(zhuǎn)動，達到繞基坐標(biāo)系做了微位移或微轉(zhuǎn)動，達到繞基準(zhǔn)坐標(biāo)同樣的效果則如何求解。準(zhǔn)坐標(biāo)同樣的效果則如何求解。 dT=T （繞基準(zhǔn)坐標(biāo)系）（繞基準(zhǔn)坐標(biāo)系） =TT （繞動坐標(biāo)系）（繞動坐標(biāo)系）左乘左乘,繞基準(zhǔn)繞基準(zhǔn)右乘右乘, 繞動坐標(biāo)軸繞動坐標(biāo)軸強調(diào)等效強調(diào)等效TTTTTT11TTT1000zzzzyyyyxxxxpasnpasnpasnT 10001PaaaaPsssspnnnnTTzyxTzyxTzyx設(shè)：設(shè)： 有：有： 0000000pdpdaasdandpdpdssadsndpdpdnnadnsdT0000000pdpdandsdpdpdsndadpdpdnsdad繞自身軸的微動率繞

36、自身軸的微動率和繞固定坐標(biāo)系坐標(biāo)軸的微動和繞固定坐標(biāo)系坐標(biāo)軸的微動率率之間的什么關(guān)系之間的什么關(guān)系，舉例說明：舉例說明： 例：一動坐標(biāo)系相對于固定坐標(biāo)系的齊例：一動坐標(biāo)系相對于固定坐標(biāo)系的齊 次交換為次交換為 1000000131007010Tnsap己知相對固定坐標(biāo)系的微己知相對固定坐標(biāo)系的微動平移和轉(zhuǎn)動動平移和轉(zhuǎn)動 kjidkjidp001 . 06 . 003 . 0求：求： 與與 求求dT 求與之等效的繞動坐標(biāo)系的微平移和微轉(zhuǎn)動求與之等效的繞動坐標(biāo)系的微平移和微轉(zhuǎn)動 解：解： =0000000dzxdyddyxdzddxydyd00006 . 001 . 0001 . 0003 . 0

37、0000100001 . 01 . 0001001 . 00010001 . 0kjikjindkjikjisdkjikjiad kjikjikjipdpd6 . 003 . 0037001 . 0kjikji6 . 003 . 0037001 . 0 kjikjikji9 . 003 . 06 . 003 . 03 . 00009 . 003 . 00103 . 09 . 003 . 00019 . 09 . 003 . 0100kjikjipdpdakjikjipdpdskjikjipdpdn0000000pdpdandsdpdpdsndadpdpdnsdadT00000001 . 03

38、. 00009 . 01 . 00000009 . 01 . 0000001 . 03 . 0000100000013100701000006 . 001 . 0001 . 0003 . 0000TdT00009 . 01 . 0000001 . 03 . 000000000001 . 03 . 00009 . 01 . 0001000000131007010TTdT解解 ：解解： 繞自身平移和轉(zhuǎn)動繞自身平移和轉(zhuǎn)動TTTTTTTTkjidkjidp01 . 0003 . 09 . 0其結(jié)果等于繞固定坐標(biāo)系轉(zhuǎn)其結(jié)果等于繞固定坐標(biāo)系轉(zhuǎn)動和旋轉(zhuǎn)動和旋轉(zhuǎn) kjidkjidp001 . 06 . 003

39、 . 0等效等效說明：說明：如果我們發(fā)現(xiàn)末端操作器相對于基準(zhǔn)坐標(biāo)系有了如果我們發(fā)現(xiàn)末端操作器相對于基準(zhǔn)坐標(biāo)系有了微位移（平移或轉(zhuǎn)動）微位移（平移或轉(zhuǎn)動）, 我們可以認為末端操作器相對我們可以認為末端操作器相對于自己的坐標(biāo)系發(fā)生了微位移。只是微動率于自己的坐標(biāo)系發(fā)生了微位移。只是微動率和和不不同而己。其結(jié)果是等效的。同而己。其結(jié)果是等效的。 這些在進行誤差補償和微動時有用這些在進行誤差補償和微動時有用, 如產(chǎn)生誤差如產(chǎn)生誤差 如何補償？可以反向運動末端關(guān)節(jié)來補償如何補償？可以反向運動末端關(guān)節(jié)來補償ddpdT=T （繞基準(zhǔn)坐標(biāo)系）（繞基準(zhǔn)坐標(biāo)系） =TT （繞動坐標(biāo)系）（繞動坐標(biāo)系）左乘左乘,繞基

40、準(zhǔn)繞基準(zhǔn)右乘右乘, 繞動坐標(biāo)軸繞動坐標(biāo)軸強調(diào)等效強調(diào)等效TTTiiTTT1iiTTT11 微動變換：微動變換： 逆運動學(xué)：逆運動學(xué)： n 數(shù)值解法數(shù)值解法n 解析解法解析解法Paul、幾何方法、幾何方法 n1 -n2110AAATni010ii010iiiiiiiAAd111nniiiiiiiinAAAAAATd1111122110nniiiiiiiiiinnAAAAAATdT111111121ji01 - ji0nnjijijijijijijijiiiiinjijijijijijiAAAAAAATdAAd11111211211111112112111jijijijiiijijiiiiiiiA

41、AAAA 1211111112111111112111211jijiiiiiiiiijijijijiiiiijijijijijijiiiiiAAAAAAAAAA 1116561111iiiiiiAATdAAid 11111656111211121iiiiiiiiAATTdAAA 616666666TdTdTTTdTTdTiiii16661666166TdTTdTTdTTdTiiii10000001310070101T10009 . 01 . 0013101 . 03 . 701011TdT 1116561111iiiiAATdAAii 00009 . 01 . 0000001 . 03 . 0

42、0001000301070017100111TdT000000000000001 . 03 . 00009 . 01 . 000dzxdyddyxdzddxydzdkjidpkjidii03 . 09 . 001 . 00iidpd,TTTTTTTTkjidkjidp01 . 0003 . 09 . 0 11111656111211121iiiiiiiiAATTdAAA100030107001010000009 . 01 . 0000001 . 03 . 0000111TTd00009 . 03 . 001 . 0001 . 0003 . 0000kjidpkjidii6 . 003 . 00

43、01 . 0kjidpkjid6 . 003 . 0001 . 0 000009. 001. 000.13 . 00009 . 01 . 0001. 000009 . 01 . 0000001 . 03 . 000010009 . 31 . 0003 . 70016 . 011 . 001111TdTdTii00000.9-39. 001. 01 . 0006. 01 . 001. 003 . 000010009 . 31 . 0003 . 70016 . 011 . 0000009 . 01 . 0000001 . 03 . 000011111TdTTdii0000000dzxdyddyxdzddxydzd0000000dzxdyddyxdzddxydzd666565656565656522212121212121211110101010101010AAAAAdAAAAAAdAAAAAAdA