第03章_機器人運動學

1、第三章第三章 機器人運動學 第一節(jié) 概述第二節(jié) 機器人的運動學基本問題第三節(jié) 機器人的雅可比矩陣 基本概念基本概念 自由度：自由度：物體能夠對坐標系進行獨立運動的物體能夠對坐標系進行獨立運動的數(shù)目稱為自由度數(shù)目稱為自由度(DOF, degree of freedom)。 剛體具有剛體具有6個自由度個自由度三個旋轉自由度三個旋轉自由度 R1, R2, R3三個平移自由度三個平移自由度T1, T2, T3YXZR1R2R3T1T2T3Robotics運動運動機械手的結構機械手的結構兩自由度兩自由度由兩個構件直接接觸而組成的可動的連接稱為運動副由兩個構件直接接觸而組成的可動的連接稱為運動副 機器人可

2、以用一個開環(huán)關節(jié)鏈來建模機器人可以用一個開環(huán)關節(jié)鏈來建模 由數(shù)個驅動器驅動的轉動或移動關節(jié)串聯(lián)而成由數(shù)個驅動器驅動的轉動或移動關節(jié)串聯(lián)而成 一端固定在基座上，另一端是自由的，安裝工具，用以一端固定在基座上，另一端是自由的，安裝工具，用以操縱物體操縱物體inoa 人們感興趣的是操作機末端執(zhí)行人們感興趣的是操作機末端執(zhí)行器相對于固定參考坐標數(shù)的空間器相對于固定參考坐標數(shù)的空間幾何描述，也就是機器人的運動幾何描述，也就是機器人的運動學問題學問題 機器人的運動學即是研究機器人機器人的運動學即是研究機器人手臂末端執(zhí)行器位置和姿態(tài)與關手臂末端執(zhí)行器位置和姿態(tài)與關節(jié)變量空間之間的關系節(jié)變量空間之間的關系Ro

3、botics運動運動機械手的機構和運動學機械手的機構和運動學關節(jié)：n一般說來，兩個桿件間是用低副相聯(lián)的n 只可能有6種低副關節(jié)：旋轉（轉動）、棱柱（移動）、圓柱形、球形、螺旋和平面，其中只有旋轉和棱柱形關節(jié)是串聯(lián)機器人操作機常見的，各種低副形狀如下圖所示：旋轉旋轉棱柱形棱柱形柱形柱形球形球形螺旋形螺旋形平面平面Robotics運動運動回轉關節(jié)回轉關節(jié):1-DOF:1-DOF棱柱關節(jié)棱柱關節(jié):1-DOF:1-DOF十字鉸鏈十字鉸鏈:2-DOF:2-DOF球鉸球鉸:3-DOF:3-DOF關節(jié)變量關節(jié)變量手爪姿態(tài)手爪姿態(tài):P:P點點運動學運動學: :幾何約束幾何約束斯坦福機械手研究的對象 機器人從機

4、構形式上分為兩種，一種是關節(jié)式串聯(lián)機器人，另外一種是并聯(lián)機器人。PUMA560HexapodFanuc manipulatorRobotics運動運動通常串聯(lián)機構正向運動學簡單通常串聯(lián)機構正向運動學簡單, ,逆向運動學復雜逆向運動學復雜并聯(lián)機構正向運動學復雜并聯(lián)機構正向運動學復雜( (多解多解),),逆向運動學簡單逆向運動學簡單并聯(lián)機器人運動學 燕山大學 黃真 并聯(lián)機器人機構學理論及其控制第一節(jié) 概述 常見的機器人運動學問題可歸納如下：1對一給定的機器人，已知桿件幾何參數(shù)和關節(jié)角矢量求機器人末端執(zhí)行器相對于參考坐標系的位置和姿態(tài)。2已知機器人桿件的幾何參數(shù)，給定機器人末端執(zhí)行器相對于參考坐標系

5、的期望位置和姿態(tài) (位姿)，機器人能否使其末端執(zhí)行器達到這個預期的位姿？如能達到，那么機器人有幾種不同形態(tài)可滿足同樣的條件? 運動學正問題關節(jié)角桿件參數(shù)末端執(zhí)行器運動學正問題關節(jié)角桿件參數(shù)Where is my hand?Direct KinematicsHERE!How do I put my hand here?Inverse Kinematics: Choose these angles! 第一個問題常稱為運動學正問題(直接問題); 第二個問題常稱為運動學逆問題(解臂形問題)。這兩個問題是機器人運動學中的基本問題。 第二節(jié) 機器人運動學的基本問題 一、運動學基本問題 圖31所示為2自由度

6、機器人手部的連桿機構。 圖中的連桿機構是兩桿件通過轉動副聯(lián)接的關節(jié)結構，通過確定連桿長度，以及關節(jié)角，可以定義該連桿機構。在分析機器人的末端手爪的運動時，若把作業(yè)看作主要依靠機器人手爪來實現(xiàn)的，則應考慮手爪的位置（圖中點的位置）。一般場合中，手爪姿勢也表示手指位置。從幾何學的觀點來處理這個手指位置與關節(jié)變量的關系稱為運動學（Kinematics）。 我們引入向量分別表示手爪位置和關節(jié)變量， 因此，利用上述兩個向量來描述一下這個2自由度機器人的運動學問題。手爪位置的各分量，按幾何學可表示為：xry 1211212coscos()xLL11212sinsin()yLL(3-1)(3-2)用向量表示

7、這個關系式，其一般可表示為 式中 表示向量函數(shù)。已知機器人的關節(jié)變量 ，求其手爪位置的運動學問題稱為正運動學（direct kinematics）。該公式被稱為運動方程式。如果，給定機器人的手爪位置，求為了到達這個預定的位置，機器人的關節(jié)變量的運動學問題稱為逆運動學（inverse kinematics）。其運動方程式可以通過以下分析得到。 ( )rff(3-3)如圖所示，根據(jù)圖中描述的幾何學關系，可得 2221122sinarctan( )arctan()cosLyxLL22221212()arccos2xyLLL L式中(3-4)(3-5)(3-6)同樣，如果用向量表示上述關系式，其一般可

8、表示為 1( )frr 如圖所示，機器人到達給定的手爪位置 有兩個姿態(tài)滿足要求，即圖中的 也是其解。這時 和 變成為另外的值。即逆運動學的解不是惟一的，可以有多個解。 12(3-7)坐標變換補充知識坐標變換補充知識 平移坐標變換：平移坐標變換：在坐標系在坐標系B中的位置矢量中的位置矢量Bp在在坐標系坐標系A中的表示可由矢量相加獲得。中的表示可由矢量相加獲得。 旋轉坐標變換旋轉坐標變換：坐標系坐標系B與坐標系與坐標系A原點原點相同，則相同，則p點在兩個坐標系中點在兩個坐標系中的描述具有下列關系：的描述具有下列關系：BABApppxAyAzAoAApxByBzBoBBpABApBxAzAoABpA

9、oAxBzByAyBpRpBABApRpRpATABABABRobotics運動運動 分別繞分別繞x,y,z軸的旋轉變換軸的旋轉變換(基本旋轉變換基本旋轉變換)：任何旋轉變換可以由有限個基本旋轉變換合成得到。任何旋轉變換可以由有限個基本旋轉變換合成得到。 復合變換：復合變換：平移和旋轉構成復合變換。平移和旋轉構成復合變換。,00001),(csscxR,00100),(csscyR10000),(cssczRBABABCACABABBCBCBABABAppRppppRpRpppRpxAyAzAoAApxByBzBoBBpABApBxCyCzCC坐標變換補充知識坐標變換補充知識Robotics運

10、動運動 例題例題1：坐標系坐標系B的初始位姿與參考坐標系的初始位姿與參考坐標系A相同，相同，坐標系坐標系B 相對于相對于A的的zA軸旋轉軸旋轉30 ，再沿，再沿A的的xA軸移軸移動動12，沿沿A的的yA軸移動軸移動6。求位置矢量。求位置矢量ApB和旋轉矩和旋轉矩陣陣 。假設。假設p點在坐標系點在坐標系B的描述為的描述為Bp=5 9 0T，求其在坐標系求其在坐標系A的描述。的描述。 解：解：RAB0294.1683.1106120951000866. 05 . 005 . 0866. 00612;1000866. 05 . 005 . 0866. 01000303003030)30,(BABAB

11、ABAABppRppcssczRR坐標變換補充知識坐標變換補充知識Robotics運動運動例題例題2 2：對于對于例題例題1 1利用齊次坐標求解利用齊次坐標求解A Ap p。10294.1683.1110951000010060866. 05 . 01205 . 0866. 0 ,1000010060866. 05 . 01205 . 0866. 010pTppRTBABABAABAB坐標變換補充知識坐標變換補充知識Robotics運動運動 關于坐標變換的順序關于坐標變換的順序純平移變換與變換次序無關純平移變換與變換次序無關旋轉變換與變換次序有關旋轉變換與變換次序有關復合變換與變換次序有關復合

12、變換與變換次序有關ssscccsssccsscccsssccscscsscccssccssccsscxRzRyR000011000000100),(),(),(sssccscsscccsssccccscsscsscccssccssccsscyRzRxR001001000000001),(),(),(坐標變換補充知識坐標變換補充知識Robotics運動運動 聯(lián)體聯(lián)體(相對坐標相對坐標)坐標：坐標：對于坐標系對于坐標系A、B、C，A是參考坐標系，是參考坐標系， B相對于相對于A的坐標以及的坐標以及C相對于相對于B的坐的坐標稱為聯(lián)體坐標。標稱為聯(lián)體坐標。 設設B在在A中的表示為中的表示為T1， C在

13、在B中的表示為中的表示為T2， 剛體在剛體在C中的表示為中的表示為T3，剛體在，剛體在A中的表示為中的表示為T，則，則 T= T1 T2 T3 上式可以理解為：按上式可以理解為：按 T1 T2 T3順序以相對坐標變換，右乘順序以相對坐標變換，右乘 坐標變換補充知識坐標變換補充知識Robotics運動運動定義定義1：如果所有的變換都是相對于固定坐標系中各坐標軸如果所有的變換都是相對于固定坐標系中各坐標軸旋轉或平移，則依次左乘，稱為絕對變換。旋轉或平移，則依次左乘，稱為絕對變換。定義定義2：如果動坐標系相對于自身坐標系的當前坐標軸旋轉如果動坐標系相對于自身坐標系的當前坐標軸旋轉或平移，則齊次變換為

14、依次右乘，稱為相對變換?；蚱揭?，則齊次變換為依次右乘，稱為相對變換。 坐標變換補充知識坐標變換補充知識Robotics運動運動注意：注意：平移矩陣間可以交換，平移矩陣間可以交換， 平移和旋轉矩陣間不可以交換平移和旋轉矩陣間不可以交換 ,100000000001),(Rotcsscx 平移齊次坐標變換平移齊次坐標變換 旋轉齊次坐標變換旋轉齊次坐標變換 復合變換復合變換1000100010001),Trans(cbacbaTranslation transformation,100000001000),(Rotcsscy100001000000),(RotcssczRotation transfo

15、rmation先平移后旋轉先平移后旋轉1010010BAABABBAABpRRpIT先旋轉后平移先旋轉后平移1010100BAABABBAABABpRRpIRT坐標變換補充知識坐標變換補充知識Robotics運動運動二、機器人位置與關節(jié)變量的關系 1表示方法 以手爪位置與關節(jié)變量之間的關系為例，要想正確表示機器人的手爪位置和姿態(tài)，就要首先建立坐標系，如圖35所示，應分別定義固定機器人的基座和手爪的坐標系，這樣才能很好地描述它們之間的位置和姿態(tài)之間的關系。 圖33 基準坐標系和手爪坐標系 基準坐標系,固定在基座上 BE手爪坐標系 ，固定在手爪上 2姿態(tài)的變換矩陣 如圖34所示，給出原點重合的兩坐

16、標系 ()AAAAOX Y()BBBBOX Y則假設點 的位置 向量的分量在兩坐標系中分別表示為 PpAxAAyppp BxBByppp 則從 向 的變換為：ABAT AATxxxBABAABAT AATyyypepeppRppepeApBpATxBAATyeRe 其中：它是從 坐標向坐標進行位置向 量姿態(tài)變換的矩陣，稱為姿態(tài)變換矩陣（或旋轉矩陣）。 B為了加深印象，現(xiàn)在分析如圖35所示坐標系 ，它是將 圍繞 軸沿正方向旋轉角 后構成的坐標系。 222ZYXO 111ZYXOZ圖35 兩個坐標系的旋轉坐標變換因此，在坐標系 上表示的坐標 與在將坐標系 繞 軸沿正方向旋轉角 得到的坐標系 上表示

17、的坐標 之間，存在下列關系式： 111ZYXOp1111ZYXO z222ZYXO p2pp211000cossin0sincos由上面知從 坐標系向坐標系 的坐標變換矩陣為： 111ZYXO 222ZYXO 1000cossin0sincos21R 因為上述變換是把某一坐標系上表示的坐標，表示到另一坐標系中，因此有時也稱它為坐標變換。在該例子中是從 坐標系向坐標系 的坐標變換，由于坐標系 是 圍繞 軸旋轉 角后構成的坐標系，則該坐標變換矩陣也可用 來表示111ZYXO 222ZYXO 222ZYXO 111ZYXO z)(zR1000cossin0sincos)(21RRz 同理，上述例子中

18、，當考慮圍繞著 軸旋轉時（設其旋轉量 為），可得到如下關系式： xppRpx221cossin0sincos0001)(另外，當圍繞著軸 旋轉時（設其旋轉量 為），可表示為如下關系式 yppRpy221cos0sin010sin0cos)(可以驗證 該矩陣為單位矩陣式中*表示 、 、 中的任何一個。所以有下列等式成立 在分析機器人運動時，當只用圍繞一個軸旋轉不能表示時，可以通過圍繞幾個軸同時旋轉的組合方式進行表示。 )(xR)(yR)(zR均滿足 100010001)()(TRRxxyzTRR)()(13齊次變換 前面討論了機器人在進行旋轉運動時的坐標變換，一般來說，機器人的運動不僅是旋轉運動

19、，有時要做平行移動，或以上兩種運動的合成，因此也應考慮平移運動時的坐標變換，即齊次變換?，F(xiàn)在來看下圖的兩個坐標系，坐標系 是將坐標系 單獨地平行移動 后，再進行適當?shù)匦D得到的坐標系。2222ZYXO 1111ZYXO 0p 這時，某一點 其在坐標系 和 上的坐標分別為 、 ，可以認為， 是由 旋轉而進行坐標變換后，即乘以旋轉坐標變換 ，在加上表示平移的向量 而得到的，因此可寫出下列表達式：P1111ZYXO 2222ZYXO 1p2p1p2pR0p021pRpp 因旋轉而進行的坐標變換，與因平移而進行的坐標變換，可以用一個坐標變換矩陣來表示，記為 ，稱這個矩陣 為齊次坐標變換矩陣，或簡稱為坐

20、標變換矩陣，表示為：AA100pRA三、機器人的運動學的一般表示 前面所介紹的是任意兩個坐標系之間的坐標變換，我們知道，機器人一般是有多個關節(jié)組成的，各關節(jié)之間的坐標變換可以通過坐標變換相乘后，結合在一起進行求解。如前所述，可以把機器人的運動模型看作是一系列由關節(jié)連接起來的連桿機構。一般機器人具有個自由度，為了分析其運動，可將上述方法擴展一下。 通常把描述一個連桿與下一個連桿間相對關系的齊次變換稱為 矩陣。一個 矩陣就是一個描述連桿坐標系間相對平移和旋轉的齊次變換。如果用 表示第一個連桿在基系的位置和姿態(tài)， 表示第二個連桿相對第一個連桿的位置和姿態(tài)，那么第二個連桿在基系的位置和姿態(tài)可由下列矩陣

21、的乘積求得 AA01A12A12012AAT 同理，若 表示第三個連桿相對第二個連桿的位置和姿態(tài)，那么第三個連桿在基系的位置和姿態(tài)可由下列矩陣的乘積求得 23A2312013AAAT 于是，對于六連桿的機器人，有下列矩陣 成立 一般，每個連桿有一個自由度，則六連桿組成的機器人具有六個自由度，并能在其運動范圍內任意定位與定向。其中，三個自由度用于規(guī)定位置，另外三個自由度用來規(guī)定姿態(tài)。所以，表示了機器人的位置和姿態(tài)。 5645342312016AAAAAAT 對于具有 個關節(jié)的機器人，若設坐標系 為固定在指尖上的坐標系時，則從坐標系 到基準坐標系 的坐標變換矩陣 可由下式給出： 不僅是從 坐標系到

22、坐標系 的坐標變換，而且同時還可以解釋為在基準坐標系 上看到的表示指尖位置和方向的矩陣。 nnnnnzyxO nnnnzyxO 0000zyxO T1231201 nnnAAAATTnnnnzyxO 0000zyxO 0000zyxO 四、機器人運動問題的示例 1機器人正運動學問題 機器人正運動學問題就是求機器人運動學的正解（forward kinematics），即在給定組成運動副的相鄰連桿的相對位置情況下，確定機器人末端執(zhí)行器的位置和姿態(tài)。通過上述分析可知，運動學正解可用一個反映此相對關系的變換矩陣來表示，這里一般是指開式鏈的機器人結構。 以一個6自由度的機器人為例，如圖所示，在該機器人中

23、，除第3個關節(jié)為平移關節(jié)外，其余均為旋轉關節(jié)。 對于這個機器人，根據(jù)圖中表示的坐標系 為基準坐標系，正運動學問題就是求該機器人末端手指關節(jié)6的位置和姿態(tài)，也就是在基準坐標系上看關節(jié)6，因此找出由 到 的坐標 變換矩陣 即可。也就是表示這個機器人的末端指尖的位置和方向，可以由下式給出： 0000zyxO 6666zyxO 0000zyxO T)()()()()()(656545434323212101AAAdAAAT 其中10000100cos0sin0sin0cos)(01111101lA10000100cos0sin0sin0cos)(12222212lA100010000100001)(3

24、323ddA100000100cos0sin0sin0cos)(4444434A100000100cos0sin0sin0cos)(5555545A100010000cossin00sincos)(26666656lA 上式即為該6自由度機器人的運動學正解。對于不同類型的機器人，其坐標變換矩陣的形式不同，要根據(jù)實際結構求得。 2機器人逆運動學 機器人的逆解問題比較復雜，為了說明問題，下面先以2自由度的機器人為例。 如圖所示，已知機器人末端的坐標值（x,y） ，試利用 表示 yx,2 根據(jù)圖中的幾何關系可知： )cos(cos21211llx)sin(sin21211lly （338） （339

25、） 聯(lián)立求解上述兩方程，可分別求出 的表達式。21,221222122cos2llllyx因此可進一步得到： )2(cos2122212212llllyx將該式代入前面的幾何表達式就可求出的 表達式。 1 從機器人的手爪末端位置姿態(tài)出發(fā)，可以求出機器人對應的各關節(jié)的角度。該例的機器人是屬于平面多關節(jié)機器人，對于一般的機械手來講，其求解過程比較復雜，往往其解不是唯一的。請有興趣的讀者參考相關的文獻書籍。 五 機器人桿件，關節(jié)和它們的參數(shù) 1 桿件與關節(jié)n操作機由一串用轉動或平移（棱柱形）關節(jié)連接的剛體（桿件）組成n每一對關節(jié)桿件構成一個自由度，因此N個自由度的操作機就有N對關節(jié)桿件。n0號桿件（

26、一般不把它當作機器人的一部分）固聯(lián)在機座上，通常在這里建立一個固定參考坐標系，最后一個桿件與工具相連n關節(jié)和桿件均由底座向外順序排列，每個桿件最多和另外兩個桿件相聯(lián)，不構成閉環(huán)。 AiAi+1Ai-1 桿件參數(shù)的定義 、 、 和n l 和 l在 A 軸 線上的交點之間 的距離。iidiAiAi+1iilid1iliAi-1idn l 和和 l之間的夾之間的夾 角，按右手定則角，按右手定則 由由l轉向轉向 l。 由運動學的觀點來看，桿件保持其兩端關節(jié)間的形態(tài)由運動學的觀點來看，桿件保持其兩端關節(jié)間的形態(tài)不變，這種形態(tài)由兩個參數(shù)決定：桿件長度不變，這種形態(tài)由兩個參數(shù)決定：桿件長度 li 和桿件扭和

27、桿件扭轉角轉角 。桿件的相對位置關系，由另外桿件的相對位置關系，由另外兩兩個參數(shù)決定：個參數(shù)決定：桿件的距離桿件的距離 di 和桿件的回轉角和桿件的回轉角 。iiilin li ii v 上述上述4個參數(shù)，就確定了桿件的結構形態(tài)和相鄰桿件相個參數(shù)，就確定了桿件的結構形態(tài)和相鄰桿件相對位置關系。在轉動關節(jié)中，對位置關系。在轉動關節(jié)中，li, i, di是固定值，是固定值，i是變量。是變量。在移動關節(jié)中，在移動關節(jié)中，li, i , i是固定值，是固定值， d i 是變量。是變量。3 機器人關節(jié)坐標系的建立n 對于每個桿件都可以在關節(jié)軸處建立一個正規(guī)的笛卡對于每個桿件都可以在關節(jié)軸處建立一個正規(guī)的

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

29、位，但必須保證但必須保證 zn與與zn-1 垂直。垂直。n 機器人關節(jié)坐標系的建立主要是為了描述機器人各桿件和終端之間的相對運動，對建立運動方程和動力學研究是基礎性的工作。n 為了描述機器人各桿件和終端之間轉動或移動關系，Denavit和Hartenberg于1955年提出了一種為運動鏈中每個桿件建立附體坐標系的矩陣方法（D-H方法） ，建立原則如下： D-H關節(jié)坐標系建立原則u右手坐標系右手坐標系u原點原點Oi：設在：設在li與與Ai+1軸線的交點上軸線的交點上 uZi軸：軸： 與與Ai+1關節(jié)軸重合，指向任意關節(jié)軸重合，指向任意 uXi軸：軸： 與公法線與公法線Li重合，指向沿重合，指向沿

30、Li由由Ai軸線指向軸線指向Ai+1軸線軸線 uYi軸：軸： 按右手定則按右手定則 關節(jié)坐標系的建立方法AiAi+1iilid1iliAi-11iz1ix1iy1ioizixiyion 原點Oi：設在li與Ai+1軸線的交點上 n zi軸：與Ai+1關節(jié)軸重合，指向任意 n xi軸：與公法線li重合，指向沿li由Ai軸線指向Ai+1軸線 n yi軸：按右手定則 沿 xi 軸， zi-1 軸與 xi 軸交點到 0i 的距離 繞 xi 軸，由 zi-1 轉向zi 沿 zi-1 軸，zi-1 軸和 xi 交點至0i 1 坐標系原點的距離 繞 zi-1 軸，由 xi-1轉向 xi兩種特殊情況n 兩軸相

31、交，怎么建立坐標系？ Oi Ai與Ai+1關節(jié)軸線的 交點； zi Ai+1軸線； xi zi和zi-1構成的平面的 法線 ； yi 右手定則；-1iizzAiAi+1zi-1zixiyiOin兩軸平行，怎么建立坐標系(Ai與Ai+1平行)？先建立 Oi-1然后建立Oi+1最后建立 Oi-1iO D 注意：注意： 由于由于Ai和和Ai+1平行，所以公法線平行，所以公法線 任意點在任意點在A點位置；點位置； 按照先前的定義，按照先前的定義，di為為Oi-1點和點和A點之間的距離，點之間的距離，di+1為為B點和點和C點間點間的距離，這樣設定可以的，但我們可以變更一下，將的距離，這樣設定可以的，但

32、我們可以變更一下，將0i點放在點放在C點，點，定義定義Oi在在li+1和和Ai+1軸的交點上，這樣使軸的交點上，這樣使di+1=0使計算簡便，此時使計算簡便，此時di= 七 相鄰關節(jié)坐標系間的齊次變換過程 機器人運動學正解n將xi-1軸繞 zi-1 軸轉 i 角度，將其與xi軸平行；n沿 zi-1軸平移距離 di ，使 xi-1 軸與 xi 軸重合；n沿 xi 軸平移距離 li，使兩坐標系原點及x軸重合；n繞 xi 軸轉 i 角度，兩坐標系完全重合AiAi+1iilid1iliAi-11iz1ix1iy1ioizixiyio111A(,)(,)( , ) ( ,)iiiiiiiiiiR zTr

33、ans zd Trans x l R x 機器人的運動學正解方程001112iiiTAAA D-H變換矩陣變換矩陣iiA1100010000100001id1000010000cossin00sincosiiii100001000010001il10000cossin00sincos00001iiii1000cossin0sincossincoscossincossinsinsincoscosiiiiiiiiiiiiiiiiidaa=例：Stanford機器人運動學方程A1A2A3A4A5A6d1z1x1y1O1d2z2x2y2O2z3y3x3O3y4z4x4O4z5y5x5O534545,0

34、o o odd重重合合d3z6x6y6O6d6z0y0 x0O0 為右手坐標系 原點Oi： Ai與Ai+1關節(jié)軸線的交點 zi軸：與Ai+1關節(jié)軸重合，指向任意 xi軸： Zi和Zi-1構成的面的法線 yi軸：按右手定則 li 沿沿 xi 軸，軸， zi-1 軸與軸與 xi 軸交點到軸交點到Oi 的距離的距離i 繞繞 xi 軸，由軸，由 zi-1 轉向轉向zidi 沿沿 zi-1 軸，軸，zi-1 軸和軸和 xi 交點至交點至Oi 1 坐標坐標 系原點的距離系原點的距離i 繞繞 zi-1 軸，由軸，由 xi-1轉向轉向 xi解：解： 八 工作空間n 工作空間: 末端操作手可以到達的空間位置集合

35、。n 如何獲得工作空間: 利用正運動學模型,改變關節(jié)變量值。n 靈活空間: 末端操作手可以以任何姿態(tài)到達的空間位置集合。n 可達空間: 末端操作手可以至少以一個姿態(tài)到達的空間位置 集合。n 空洞：空洞：在在 zi軸周圍，參考點軸周圍，參考點Pn沿沿z的全長均不能達到的空間。的全長均不能達到的空間。n 空腔：空腔：參考點不能達到的被完全封閉在工作空間之內的空間。參考點不能達到的被完全封閉在工作空間之內的空間。 空洞空洞空腔空腔如何確定可達空間如何確定可達空間? ?首先，首先，令令 3 3變化變化 示例: 平面 3連桿機器人123112123123112123123123123123coscosc

36、ossinsinsin , xlllylllllllll 3種最常見的歐拉角類型步1步2步3類型1繞OZ軸轉角繞當前OU 軸轉角繞當前OW軸轉角類型2繞OZ軸轉角繞當前OV 軸轉角繞當前OW軸轉角類型3繞OX軸轉角繞OY軸轉角繞OZ軸轉角uvwx(u)y (v)z (w)ouvwu?v?W?),(ZR),(R),(wR N0T100000000110000cssccssccsscccssssccccssscccsssccsscscscc類型類型1：表示法通常用于陀螺運動：表示法通常用于陀螺運動 類型類型2：所得的轉動矩陣為右乘所得的轉動矩陣為右乘 10000c0s-010s0c 10000),

37、(),v(),(RcssccsscwRRZR1000pzpyRpxTccsssssccscsscccssccssccsscccccscssccssccssscssscsccsssccccssccssccsscxRyRz000010010010000),(),(),RR（ZYX偏航偏航俯仰俯仰橫滾橫滾: 已知關節(jié)角度或位移，計算末端操作手的對應位姿.: 已知末端操作手的位姿，求解對應的關節(jié)變量. 可能存在多解或無解 通常需多次求解非線性超越方程十 運動學逆問題 解的存在性存在雙解存在雙解! 求解方法0140i000222018040i100040i0140i000222018040i 25001

38、160i0220in 逆運動學的定義n 逆運動學的存在性n 逆運動學的可解性n 逆運動學的多解性（剔除辦法）n 逆運動學解法（數(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( ）

39、（6 q1000zzzzyyyyxxxxpasnpasnpasnT65544332211060T T T T T TT ),(2xyarctg)/(xyarctg(arccos)cos(cos0/ )(cos180, 0dd為負為正均為負為正為負均為正yxyxyxyxxytg,090901801809090),(200000001 例：歐拉角第一種類型，求逆),(ZR),(R),(wR N0T100000000110000cssccssccsscccssssccccssscccsssccsscscscc類型類型1：表示法通常用于陀螺運動：表示法通常用于陀螺運動 步1步2步3類型1繞OZ軸轉角繞

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

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

42、：作三角變換： 式中：式中： 得到：得到： 即有：即有： 由由1, 4和和2, 4元素對應相等，得：元素對應相等，得： 6261121TTA6362132TTA高腕高腕低腕低腕第三節(jié) 機器人的雅可比矩陣 一、雅可比矩陣的定義 前面討論了機器人的指尖位置和方向與各關節(jié)的變化位置之間的關系。在本節(jié)將進一步討論指尖的速度與各關節(jié)的速度（轉動或平移）之間的關系。 考慮機械手的手爪位置 和關節(jié)變量 的關系用正運動學方程表示如下： r 假定這里考慮的是 )(fr 121, mTmRrrrr121, nTmR的一般情況，并設手爪位置包含表示姿態(tài)的變量，以及關節(jié)變量由回轉角和平移組合而成的情況。（355）若用

43、每個分量表示，則變?yōu)?在 的情況下，將變?yōu)槭肿ξ恢玫年P節(jié)變量有無限個解的冗余機器人。而工業(yè)上常用的多關節(jié)機器人手臂，通常用于作業(yè)的所需手爪應有3個位置變量和3個姿態(tài)變量，總計6個變量。而且由于不采用冗余機器人結構，所以 。 ),(21njjfr ), 2 , 1(mj mn6 mn 將式（355）的兩邊對時間微分，可得到下式 Jr （357）其中 nmnmmnTRfffffJ 1111)(（358） 稱 為雅可比矩陣（Jacobian matrix）。若在式（357）的兩邊乘以微小時間 ，則可得到 JdtJddr （359）該式是用雅可比矩陣表示微小位移間關系的關系式。 二、與平移速度相關的雅

44、可比矩陣 現(xiàn)在設基準坐標系為 ，固定于指尖的坐標系為 ，在 上表示的的坐標為 ，則 可以表示如下： 0000zyxO eeeezyxO 0000zyxO eOePeP)(1000qfTPe（360） 這時，指尖的平移速度可以寫成： qJdtdqJdtdqdqdfdtdPvLLe（361） 式中， ，其中 是關節(jié)的數(shù)目。這里的 稱為與平移速度相關的雅可比矩陣。 Tnqqq),(1 nLJ 下面以2自由度機械手為例，如前面圖32所示的2自由度機械手的雅可比矩陣。前面已推導過，該機器人的指尖位置可以表示為 )cos(cos21211llx)sin(sin21211lly 則與這個機器人的平移速度相關的雅可比矩陣，可以下列形式給出： )cos()