第3章 機(jī)器人運(yùn)動(dòng)學(xué)

1、1 第三章第三章 機(jī)器人運(yùn)動(dòng)學(xué)機(jī)器人運(yùn)動(dòng)學(xué) 第一節(jié)第一節(jié) 概述概述 第二節(jié)第二節(jié) 機(jī)器人的運(yùn)動(dòng)學(xué)基本問題機(jī)器人的運(yùn)動(dòng)學(xué)基本問題 第三節(jié)第三節(jié) 機(jī)器人的雅可比矩陣機(jī)器人的雅可比矩陣 2 第一節(jié)第一節(jié) 概述概述 常見的機(jī)器人運(yùn)動(dòng)學(xué)問題可歸納如下：常見的機(jī)器人運(yùn)動(dòng)學(xué)問題可歸納如下： 1 1對(duì)一給定的機(jī)器人，已知桿件幾何參數(shù)和關(guān)對(duì)一給定的機(jī)器人，已知桿件幾何參數(shù)和關(guān) 節(jié)角矢量求機(jī)器人末端執(zhí)行器相對(duì)于參考坐標(biāo)節(jié)角矢量求機(jī)器人末端執(zhí)行器相對(duì)于參考坐標(biāo) 系的位置和姿態(tài)。系的位置和姿態(tài)。 2 2已知機(jī)器人桿件的幾何參數(shù)，給定機(jī)器人末已知機(jī)器人桿件的幾何參數(shù)，給定機(jī)器人末 端執(zhí)行器相對(duì)于參考坐標(biāo)系的期望位置和姿

2、態(tài)端執(zhí)行器相對(duì)于參考坐標(biāo)系的期望位置和姿態(tài) ( (位姿位姿) )，機(jī)器人能否使其末端執(zhí)行器達(dá)到這個(gè)，機(jī)器人能否使其末端執(zhí)行器達(dá)到這個(gè) 預(yù)期的位姿？如能達(dá)到，那么機(jī)器人有幾種不預(yù)期的位姿？如能達(dá)到，那么機(jī)器人有幾種不 同形態(tài)可滿足同樣的條件同形態(tài)可滿足同樣的條件? ? 3 第一個(gè)問題常稱為運(yùn)動(dòng)學(xué)正問題第一個(gè)問題常稱為運(yùn)動(dòng)學(xué)正問題( (直接問題直接問題);); 第二個(gè)問題常稱為運(yùn)動(dòng)學(xué)逆問題第二個(gè)問題常稱為運(yùn)動(dòng)學(xué)逆問題( (解臂形問題解臂形問題) )。 這兩個(gè)問題是機(jī)器人運(yùn)動(dòng)學(xué)中的基本問題。這兩個(gè)問題是機(jī)器人運(yùn)動(dòng)學(xué)中的基本問題。 4 第二節(jié)第二節(jié) 機(jī)器人運(yùn)動(dòng)學(xué)的基本問題機(jī)器人運(yùn)動(dòng)學(xué)的基本問題 一、運(yùn)

3、動(dòng)學(xué)基本問題一、運(yùn)動(dòng)學(xué)基本問題 圖圖3 31 1所示為所示為2 2自由度機(jī)器人手部的連桿機(jī)構(gòu)自由度機(jī)器人手部的連桿機(jī)構(gòu)。 5 圖中的連桿機(jī)構(gòu)是兩桿件通過轉(zhuǎn)動(dòng)副聯(lián)接的關(guān)節(jié)結(jié)構(gòu)，圖中的連桿機(jī)構(gòu)是兩桿件通過轉(zhuǎn)動(dòng)副聯(lián)接的關(guān)節(jié)結(jié)構(gòu)， 通過確定連桿長度，以及關(guān)節(jié)角，可以定義該連桿機(jī)構(gòu)。通過確定連桿長度，以及關(guān)節(jié)角，可以定義該連桿機(jī)構(gòu)。 在分析機(jī)器人的末端手爪的運(yùn)動(dòng)時(shí)，若把作業(yè)看作主要依靠在分析機(jī)器人的末端手爪的運(yùn)動(dòng)時(shí)，若把作業(yè)看作主要依靠 機(jī)器人手爪來實(shí)現(xiàn)的，則應(yīng)考慮手爪的位置（圖中點(diǎn)的位機(jī)器人手爪來實(shí)現(xiàn)的，則應(yīng)考慮手爪的位置（圖中點(diǎn)的位 置）。一般場合中，手爪置）。一般場合中，手爪 姿勢(shì)也表示手指位置。

4、從姿勢(shì)也表示手指位置。從 幾何學(xué)的觀點(diǎn)來處理這個(gè)幾何學(xué)的觀點(diǎn)來處理這個(gè) 手指位置與關(guān)節(jié)變量的關(guān)手指位置與關(guān)節(jié)變量的關(guān) 系稱為運(yùn)動(dòng)學(xué)（系稱為運(yùn)動(dòng)學(xué)（KinematicsKinematics）。）。 6 引入向量分別表示手爪位置和關(guān)節(jié)變量，引入向量分別表示手爪位置和關(guān)節(jié)變量， 因此，利用上述兩個(gè)向量來描述一下因此，利用上述兩個(gè)向量來描述一下 這個(gè)這個(gè)2 2自由度機(jī)器人的運(yùn)動(dòng)學(xué)問題。自由度機(jī)器人的運(yùn)動(dòng)學(xué)問題。 手爪位置的各分量，按幾何學(xué)可表示為：手爪位置的各分量，按幾何學(xué)可表示為： x r y 1 2 11212 coscos()xLL 11212 sinsin()yLL (3-1) (3-2) 7

5、 用向量表示這個(gè)關(guān)系式，其一般可表示為用向量表示這個(gè)關(guān)系式，其一般可表示為 式中式中 表示向量函數(shù)。已知機(jī)器人的關(guān)節(jié)變量表示向量函數(shù)。已知機(jī)器人的關(guān)節(jié)變量 ， 求其手爪位置的運(yùn)動(dòng)學(xué)問題稱為正運(yùn)動(dòng)學(xué)（求其手爪位置的運(yùn)動(dòng)學(xué)問題稱為正運(yùn)動(dòng)學(xué)（direct direct kinematicskinematics）。該公式被稱為運(yùn)動(dòng)方程式。如果，）。該公式被稱為運(yùn)動(dòng)方程式。如果， 給定機(jī)器人的手爪位置，求為了到達(dá)這個(gè)預(yù)定的位給定機(jī)器人的手爪位置，求為了到達(dá)這個(gè)預(yù)定的位 置，機(jī)器人的關(guān)節(jié)變量的運(yùn)動(dòng)學(xué)問題稱為逆運(yùn)動(dòng)學(xué)置，機(jī)器人的關(guān)節(jié)變量的運(yùn)動(dòng)學(xué)問題稱為逆運(yùn)動(dòng)學(xué) （inverse kinematicsinve

6、rse kinematics）。其運(yùn)動(dòng)方程式可以通過）。其運(yùn)動(dòng)方程式可以通過 以下分析得到。以下分析得到。 ( )rf f (3-3) 8 如圖所示，根據(jù)圖中描述的幾何學(xué)關(guān)系，可得如圖所示，根據(jù)圖中描述的幾何學(xué)關(guān)系，可得 2 22 1 122 sin arctan()arctan() cos Ly xLL 2222 12 12 () arccos 2 xyLL L L 式中式中 (3-4) (3-5) (3-6) 9 同樣，如果用向量表示上述關(guān)系式，其一般可表同樣，如果用向量表示上述關(guān)系式，其一般可表 示為示為 1 ( )fr 如圖所示，機(jī)器人到達(dá)給定的手爪位置如圖所示，機(jī)器人到達(dá)給定的手爪位

7、置 r 有兩個(gè)姿態(tài)滿足要求，即圖中的有兩個(gè)姿態(tài)滿足要求，即圖中的 也是也是 其解。其解。 這時(shí)這時(shí) 和和 變成為另外的值。變成為另外的值。 即逆運(yùn)動(dòng)學(xué)的解不是惟一的，即逆運(yùn)動(dòng)學(xué)的解不是惟一的， 可以有多個(gè)解?？梢杂卸鄠€(gè)解。 1 2 (3-7) 10 二、機(jī)器人位置與關(guān)節(jié)變量的關(guān)系二、機(jī)器人位置與關(guān)節(jié)變量的關(guān)系 1 1表示方法表示方法 以手爪位置與關(guān)節(jié)變量之間的關(guān)系為例，以手爪位置與關(guān)節(jié)變量之間的關(guān)系為例， 要想正確表示機(jī)器人的手爪位置和姿態(tài)，就要要想正確表示機(jī)器人的手爪位置和姿態(tài)，就要 首先建立坐標(biāo)系，如圖首先建立坐標(biāo)系，如圖3 33 3所示，應(yīng)分別定義所示，應(yīng)分別定義 固定機(jī)器人的基座和手爪

8、的坐標(biāo)系，這樣才能固定機(jī)器人的基座和手爪的坐標(biāo)系，這樣才能 很好地描述它們之間的位置和姿態(tài)之間的關(guān)系。很好地描述它們之間的位置和姿態(tài)之間的關(guān)系。 11 圖圖3 33 3 基準(zhǔn)坐標(biāo)系和手爪坐標(biāo)系基準(zhǔn)坐標(biāo)系和手爪坐標(biāo)系 基準(zhǔn)坐標(biāo)系基準(zhǔn)坐標(biāo)系,固定在基座上固定在基座上 B E 手爪坐標(biāo)系手爪坐標(biāo)系 ，固定在手爪上，固定在手爪上 12 2姿態(tài)的變換矩陣姿態(tài)的變換矩陣 如圖如圖3 34 4所示，給出原點(diǎn)重合的兩坐標(biāo)系所示，給出原點(diǎn)重合的兩坐標(biāo)系 () AAAA OX Y () BBBB OX Y 則假設(shè)點(diǎn)則假設(shè)點(diǎn) P 的位置的位置p 向量的向量的 分量在兩坐標(biāo)系中分別表示分量在兩坐標(biāo)系中分別表示 為為

9、A x A A y p p p B x B B y p p p 13 則從則從 向向 的變換為：的變換為： A BAT AAT xxx BABA A BAT AAT yyy pepe ppRp pepe A p B p AT x B A AT y e R e 其中：其中： 它是從它是從 坐標(biāo)向坐標(biāo)進(jìn)行位置向坐標(biāo)向坐標(biāo)進(jìn)行位置向 量姿態(tài)變換量姿態(tài)變換 的矩陣，稱為姿態(tài)變換矩陣（或旋轉(zhuǎn)矩陣）。的矩陣，稱為姿態(tài)變換矩陣（或旋轉(zhuǎn)矩陣）。 B 14 為了加深印象，現(xiàn)在分析如圖為了加深印象，現(xiàn)在分析如圖3 35 5所示坐標(biāo)所示坐標(biāo) 系系 ，它是將，它是將 圍繞圍繞 Z Z軸沿正方向旋軸沿正方向旋 轉(zhuǎn)角轉(zhuǎn)角

10、 后構(gòu)成的坐標(biāo)系。后構(gòu)成的坐標(biāo)系。 pp 21 100 0cossin 0sincos 222 ZYXO 111 ZYXO 圖圖35 兩個(gè)坐標(biāo)系的旋轉(zhuǎn)坐標(biāo)變換兩個(gè)坐標(biāo)系的旋轉(zhuǎn)坐標(biāo)變換 因此，在坐標(biāo)系因此，在坐標(biāo)系 上表示的坐標(biāo)上表示的坐標(biāo) 與在將坐標(biāo)系與在將坐標(biāo)系 繞繞Z Z軸沿正方向旋轉(zhuǎn)角軸沿正方向旋轉(zhuǎn)角得到的坐標(biāo)系得到的坐標(biāo)系 上表示的上表示的 坐標(biāo)坐標(biāo) 之間，存在下列關(guān)系式：之間，存在下列關(guān)系式： p 1 111 ZYXO 222 ZYXO p 2 pp 21 100 0cossin 0sincos 15 由上面知從由上面知從 坐標(biāo)系向坐標(biāo)系坐標(biāo)系向坐標(biāo)系 的坐標(biāo)變換矩陣為：的坐標(biāo)變換矩

11、陣為： 111 ZYXO 222 ZYXO 100 0cossin 0sincos 2 1 R 16 因?yàn)樯鲜鲎儞Q是把某一坐標(biāo)系上表示的坐標(biāo)，因?yàn)樯鲜鲎儞Q是把某一坐標(biāo)系上表示的坐標(biāo)， 表示到另一坐標(biāo)系中，因此有時(shí)也稱它為坐標(biāo)變表示到另一坐標(biāo)系中，因此有時(shí)也稱它為坐標(biāo)變 換。在該例子中是從換。在該例子中是從 坐標(biāo)系向坐標(biāo)系坐標(biāo)系向坐標(biāo)系 的坐標(biāo)變換，由于坐標(biāo)系的坐標(biāo)變換，由于坐標(biāo)系 是是 圍繞圍繞Z軸旋轉(zhuǎn)軸旋轉(zhuǎn)角后構(gòu)成的坐標(biāo)系，則該角后構(gòu)成的坐標(biāo)系，則該 坐標(biāo)變換矩陣也可用坐標(biāo)變換矩陣也可用 來表示來表示 111 ZYXO 222 ZYXO 222 ZYXO 111 ZYXO )( z R 10

12、0 0cossin 0sincos )( 2 1 RRz 17 同理，上述例子中，當(dāng)考慮圍繞著同理，上述例子中，當(dāng)考慮圍繞著 x軸旋轉(zhuǎn)時(shí)（設(shè)軸旋轉(zhuǎn)時(shí)（設(shè) 其旋轉(zhuǎn)量其旋轉(zhuǎn)量為），可得到如下關(guān)系式：為），可得到如下關(guān)系式： ppRp x 221 cossin0 sincos0 001 )( 另外，當(dāng)圍繞著軸另外，當(dāng)圍繞著軸y y旋轉(zhuǎn)時(shí)（設(shè)其旋轉(zhuǎn)量旋轉(zhuǎn)時(shí)（設(shè)其旋轉(zhuǎn)量為），可表示為），可表示 為如下關(guān)系式為如下關(guān)系式 ppRp y 221 cos0sin 010 sin0cos )( 18 可以驗(yàn)證可以驗(yàn)證 該矩陣為單位矩陣式中該矩陣為單位矩陣式中* *表示表示 x x、y y 、z z中的任中的任

13、何一個(gè)。所以有下列等式成立何一個(gè)。所以有下列等式成立 在分析機(jī)器人運(yùn)動(dòng)時(shí)，當(dāng)只用圍繞一個(gè)軸旋轉(zhuǎn)在分析機(jī)器人運(yùn)動(dòng)時(shí)，當(dāng)只用圍繞一個(gè)軸旋轉(zhuǎn) 不能表示時(shí)，可以通過圍繞幾個(gè)軸同時(shí)旋轉(zhuǎn)的不能表示時(shí)，可以通過圍繞幾個(gè)軸同時(shí)旋轉(zhuǎn)的 組合方式進(jìn)行表示。組合方式進(jìn)行表示。 )( x R )( y R )( z R 均滿足均滿足 100 010 001 )()( T RR x T RR)()( 1 19 3 3齊次變換齊次變換 前面討論了機(jī)器人在進(jìn)行旋轉(zhuǎn)運(yùn)動(dòng)前面討論了機(jī)器人在進(jìn)行旋轉(zhuǎn)運(yùn)動(dòng) 時(shí)的坐標(biāo)變換，一般來說，機(jī)器人的運(yùn)時(shí)的坐標(biāo)變換，一般來說，機(jī)器人的運(yùn) 動(dòng)不僅是旋轉(zhuǎn)運(yùn)動(dòng)，有時(shí)要做平行移動(dòng)，動(dòng)不僅是旋轉(zhuǎn)運(yùn)動(dòng)，有

14、時(shí)要做平行移動(dòng)， 或以上兩種運(yùn)動(dòng)的合成，因此也應(yīng)考慮或以上兩種運(yùn)動(dòng)的合成，因此也應(yīng)考慮 平移運(yùn)動(dòng)時(shí)的坐標(biāo)變換，即齊次變換。平移運(yùn)動(dòng)時(shí)的坐標(biāo)變換，即齊次變換。 20 現(xiàn)在來看下圖的兩個(gè)坐標(biāo)系，現(xiàn)在來看下圖的兩個(gè)坐標(biāo)系，坐標(biāo)系坐標(biāo)系 是將坐標(biāo)系是將坐標(biāo)系 單獨(dú)地平行移動(dòng)單獨(dú)地平行移動(dòng) 后，再后，再 進(jìn)行適當(dāng)?shù)匦D(zhuǎn)得到的坐標(biāo)系。進(jìn)行適當(dāng)?shù)匦D(zhuǎn)得到的坐標(biāo)系。 2222 ZYXO 1111 ZYXO 0 p 21 這時(shí)，某一點(diǎn)這時(shí)，某一點(diǎn)P其在坐標(biāo)系其在坐標(biāo)系 和和 上的坐標(biāo)分別為上的坐標(biāo)分別為p 1 、p2 ，可以認(rèn)為，可以認(rèn)為 p 1 是由是由p2 旋轉(zhuǎn)而進(jìn)行坐標(biāo)變換后，即乘以旋轉(zhuǎn)坐標(biāo)變換旋轉(zhuǎn)而進(jìn)行

15、坐標(biāo)變換后，即乘以旋轉(zhuǎn)坐標(biāo)變換 R， 在加上表示平移的向量在加上表示平移的向量 p0而得到的，因此可寫出而得到的，因此可寫出 下列表達(dá)式：下列表達(dá)式： 1111 ZYXO 2222 ZYXO 021 pRpp 22 因旋轉(zhuǎn)而進(jìn)行的坐標(biāo)變換，與因平移而進(jìn)因旋轉(zhuǎn)而進(jìn)行的坐標(biāo)變換，與因平移而進(jìn) 行的坐標(biāo)變換，可以用一個(gè)坐標(biāo)變換矩陣來表行的坐標(biāo)變換，可以用一個(gè)坐標(biāo)變換矩陣來表 示，記為示，記為A ，稱這個(gè)矩陣，稱這個(gè)矩陣 A為齊次坐標(biāo)變換矩為齊次坐標(biāo)變換矩 陣，或簡稱為坐標(biāo)變換矩陣，表示為：陣，或簡稱為坐標(biāo)變換矩陣，表示為： 10 0 pR A 23 三、機(jī)器人的運(yùn)動(dòng)學(xué)的一般表示三、機(jī)器人的運(yùn)動(dòng)學(xué)的一

16、般表示 前面所介紹的是任意兩個(gè)坐標(biāo)系之間的坐標(biāo)前面所介紹的是任意兩個(gè)坐標(biāo)系之間的坐標(biāo) 變換，我們知道，機(jī)器人一般是有多個(gè)關(guān)節(jié)組成變換，我們知道，機(jī)器人一般是有多個(gè)關(guān)節(jié)組成 的，各關(guān)節(jié)之間的坐標(biāo)變換可以通過坐標(biāo)變換相的，各關(guān)節(jié)之間的坐標(biāo)變換可以通過坐標(biāo)變換相 乘后，結(jié)合在一起進(jìn)行求解。如前所述，可以把乘后，結(jié)合在一起進(jìn)行求解。如前所述，可以把 機(jī)器人的運(yùn)動(dòng)模型看作是一系列由關(guān)節(jié)連接起來機(jī)器人的運(yùn)動(dòng)模型看作是一系列由關(guān)節(jié)連接起來 的連桿機(jī)構(gòu)。一般機(jī)器人具有個(gè)自由度，為了分的連桿機(jī)構(gòu)。一般機(jī)器人具有個(gè)自由度，為了分 析其運(yùn)動(dòng)，可將上述方法擴(kuò)展一下。析其運(yùn)動(dòng)，可將上述方法擴(kuò)展一下。 24 通常把描述一

17、個(gè)連桿與下一個(gè)連桿間相對(duì)關(guān)系通常把描述一個(gè)連桿與下一個(gè)連桿間相對(duì)關(guān)系 的齊次變換稱為的齊次變換稱為A矩陣。一個(gè)矩陣。一個(gè)A矩陣就是一個(gè)矩陣就是一個(gè) 描述連桿坐標(biāo)系間相對(duì)平移和旋轉(zhuǎn)的齊次變換。描述連桿坐標(biāo)系間相對(duì)平移和旋轉(zhuǎn)的齊次變換。 如果用如果用 表示第一個(gè)連桿在基系的位置和姿表示第一個(gè)連桿在基系的位置和姿 態(tài)，態(tài)， 表示第二個(gè)連桿相對(duì)第一個(gè)連桿的位置表示第二個(gè)連桿相對(duì)第一個(gè)連桿的位置 和姿態(tài)，那么第二個(gè)連桿在基系的位置和姿態(tài)和姿態(tài)，那么第二個(gè)連桿在基系的位置和姿態(tài) 可由下列矩陣的乘積求得可由下列矩陣的乘積求得 0 1 A 1 2 A 1 2 0 12 AAT 25 同理，若同理，若 表示第三

18、個(gè)連桿相對(duì)第二個(gè)連桿表示第三個(gè)連桿相對(duì)第二個(gè)連桿 的位置和姿態(tài)，那么第三個(gè)連桿在基系的位置的位置和姿態(tài)，那么第三個(gè)連桿在基系的位置 和姿態(tài)可由下列矩陣的乘積求得和姿態(tài)可由下列矩陣的乘積求得 2 3 A 2 3 1 2 0 13 AAAT 26 于是，對(duì)于六連桿的機(jī)器人，有下列矩陣于是，對(duì)于六連桿的機(jī)器人，有下列矩陣 成立成立 一般，每個(gè)連桿有一個(gè)自由度，則六連桿組成的一般，每個(gè)連桿有一個(gè)自由度，則六連桿組成的 機(jī)器人具有六個(gè)自由度，并能在其運(yùn)動(dòng)范圍內(nèi)任機(jī)器人具有六個(gè)自由度，并能在其運(yùn)動(dòng)范圍內(nèi)任 意定位與定向。其中，三個(gè)自由度用于規(guī)定位置，意定位與定向。其中，三個(gè)自由度用于規(guī)定位置， 另外三個(gè)自

19、由度用來規(guī)定姿態(tài)。所以，表示了機(jī)另外三個(gè)自由度用來規(guī)定姿態(tài)。所以，表示了機(jī) 器人的位置和姿態(tài)。器人的位置和姿態(tài)。 5 6 4 5 3 4 2 3 1 2 0 16 AAAAAAT 27 對(duì)于具有對(duì)于具有n個(gè)關(guān)節(jié)的機(jī)器人，若設(shè)坐標(biāo)系個(gè)關(guān)節(jié)的機(jī)器人，若設(shè)坐標(biāo)系 為固定在指尖上的坐標(biāo)系時(shí)，則從坐標(biāo)系為固定在指尖上的坐標(biāo)系時(shí)，則從坐標(biāo)系 到基準(zhǔn)坐標(biāo)系到基準(zhǔn)坐標(biāo)系 的坐標(biāo)變換矩陣的坐標(biāo)變換矩陣T可由下可由下 式給出：式給出： T 不僅是從不僅是從 坐標(biāo)系到坐標(biāo)系坐標(biāo)系到坐標(biāo)系 的坐標(biāo)變換，而且同時(shí)還可以解釋為在基準(zhǔn)坐標(biāo)的坐標(biāo)變換，而且同時(shí)還可以解釋為在基準(zhǔn)坐標(biāo) 系系 上看到的表示指尖位置和方向的矩上看到

20、的表示指尖位置和方向的矩 陣。陣。 nnnn zyxO nnnn zyxO 0000 zyxO 12 3 1 2 0 1 n nn AAAAT nnnn zyxO 0000 zyxO 0000 zyxO 28 四、機(jī)器人運(yùn)動(dòng)問題的示例四、機(jī)器人運(yùn)動(dòng)問題的示例 1機(jī)器人正運(yùn)動(dòng)學(xué)問題機(jī)器人正運(yùn)動(dòng)學(xué)問題 機(jī)器人正運(yùn)動(dòng)學(xué)問題就是求機(jī)器人運(yùn)動(dòng)學(xué)機(jī)器人正運(yùn)動(dòng)學(xué)問題就是求機(jī)器人運(yùn)動(dòng)學(xué) 的正解（的正解（forward kinematics），即在給定），即在給定 組成運(yùn)動(dòng)副的相鄰連桿的相對(duì)位置情況下，確組成運(yùn)動(dòng)副的相鄰連桿的相對(duì)位置情況下，確 定機(jī)器人末端執(zhí)行器的位置和姿態(tài)。通過上述定機(jī)器人末端執(zhí)行器的位置和

21、姿態(tài)。通過上述 分析可知，運(yùn)動(dòng)學(xué)正解可用一個(gè)反映此相對(duì)關(guān)分析可知，運(yùn)動(dòng)學(xué)正解可用一個(gè)反映此相對(duì)關(guān) 系的變換矩陣來表示，這里一般是指開式鏈的系的變換矩陣來表示，這里一般是指開式鏈的 機(jī)器人結(jié)構(gòu)。機(jī)器人結(jié)構(gòu)。 29 以一個(gè)以一個(gè)6自由度的機(jī)器人為例，如圖所示，自由度的機(jī)器人為例，如圖所示， 在該機(jī)器人中，除第在該機(jī)器人中，除第3個(gè)關(guān)節(jié)為平移關(guān)節(jié)外，個(gè)關(guān)節(jié)為平移關(guān)節(jié)外， 其余均為旋轉(zhuǎn)關(guān)節(jié)。其余均為旋轉(zhuǎn)關(guān)節(jié)。 30 對(duì)于這個(gè)機(jī)器人，根據(jù)圖中表示的坐標(biāo)系對(duì)于這個(gè)機(jī)器人，根據(jù)圖中表示的坐標(biāo)系 為基準(zhǔn)坐標(biāo)系，正運(yùn)動(dòng)學(xué)問題就是求該機(jī)器人末端為基準(zhǔn)坐標(biāo)系，正運(yùn)動(dòng)學(xué)問題就是求該機(jī)器人末端 手指關(guān)節(jié)手指關(guān)節(jié)6的位置

22、和姿態(tài)，也就是在基準(zhǔn)坐標(biāo)系上看的位置和姿態(tài)，也就是在基準(zhǔn)坐標(biāo)系上看 關(guān)節(jié)關(guān)節(jié)6，因此找出由，因此找出由 到到 的坐的坐 標(biāo)變換矩陣標(biāo)變換矩陣T即可。即可。也就是表示這個(gè)機(jī)器人的末也就是表示這個(gè)機(jī)器人的末 端指尖的位置和方向，可以由下式給出：端指尖的位置和方向，可以由下式給出： 0000 zyxO 6666 zyxO 0000 zyxO )()()()()()( 6 5 65 4 54 3 43 2 32 1 21 0 1 AAAdAAAT 31 其中其中 1000 010 0cos0sin 0sin0cos )( 0 11 11 1 0 1 l A 1000 010 0cos0sin 0sin

23、0cos )( 1 22 22 2 1 2 l A 32 1000 100 0010 0001 )( 3 3 2 3 d dA 1000 0010 0cos0sin 0sin0cos )( 44 44 4 3 4 A 1000 0010 0cos0sin 0sin0cos )( 55 55 5 4 5 A 1000 0010 0cos0sin 0sin0cos )( 44 44 4 3 4 A 33 1000 0010 0cos0sin 0sin0cos )( 55 55 5 4 5 A 1000 100 00cossin 00sincos )( 2 66 66 6 5 6 l A 34 上式

24、即為該上式即為該6自由度機(jī)器人的運(yùn)動(dòng)學(xué)正解。對(duì)于自由度機(jī)器人的運(yùn)動(dòng)學(xué)正解。對(duì)于 不同類型的機(jī)器人，其坐標(biāo)變換矩陣的形式不同，不同類型的機(jī)器人，其坐標(biāo)變換矩陣的形式不同， 要根據(jù)實(shí)際結(jié)構(gòu)求得。要根據(jù)實(shí)際結(jié)構(gòu)求得。 35 2 2機(jī)器人逆運(yùn)動(dòng)學(xué)機(jī)器人逆運(yùn)動(dòng)學(xué) 機(jī)器人的逆解問題比較復(fù)雜，為了說明問題，下面機(jī)器人的逆解問題比較復(fù)雜，為了說明問題，下面 先以先以2自由度的機(jī)器人為例。自由度的機(jī)器人為例。 如圖所示，已知機(jī)器人末端的坐標(biāo)值如圖所示，已知機(jī)器人末端的坐標(biāo)值 （x,y） ，試?yán)?，試?yán)?x,y表示表示 2 2 36 根據(jù)圖中的幾何關(guān)系可知： )cos(cos 21211 llx )sin(si

25、n 21211 lly （338） （339） 37 聯(lián)立求解上述兩方程，可分別求出聯(lián)立求解上述兩方程，可分別求出 的表達(dá)式。的表達(dá)式。 21, 221 2 2 2 1 22 cos2llllyx 因此可進(jìn)一步得到：因此可進(jìn)一步得到： ) 2 (cos 21 2 2 2 1 22 1 2 ll llyx 將該式代入前面的幾何表達(dá)式就可求出的將該式代入前面的幾何表達(dá)式就可求出的 表達(dá)式。表達(dá)式。 1 38 從機(jī)器人的手爪末端位置姿態(tài)出發(fā)，從機(jī)器人的手爪末端位置姿態(tài)出發(fā)， 可以求出機(jī)器人對(duì)應(yīng)的各關(guān)節(jié)的角度。可以求出機(jī)器人對(duì)應(yīng)的各關(guān)節(jié)的角度。 該例的機(jī)器人是屬于平面多關(guān)節(jié)機(jī)器人，該例的機(jī)器人是屬于平

26、面多關(guān)節(jié)機(jī)器人， 對(duì)于一般的機(jī)械手來講，其求解過程比對(duì)于一般的機(jī)械手來講，其求解過程比 較復(fù)雜，往往其解不是唯一的。請(qǐng)有興較復(fù)雜，往往其解不是唯一的。請(qǐng)有興 趣的愛好者參考相關(guān)的文獻(xiàn)書籍。趣的愛好者參考相關(guān)的文獻(xiàn)書籍。 39 第三節(jié)第三節(jié) 機(jī)器人的雅可比矩陣機(jī)器人的雅可比矩陣 一、雅可比矩陣的定義一、雅可比矩陣的定義 前面討論了機(jī)器人的指尖位置和方向與前面討論了機(jī)器人的指尖位置和方向與 各關(guān)節(jié)的變化位置之間的關(guān)系。在本節(jié)各關(guān)節(jié)的變化位置之間的關(guān)系。在本節(jié) 將進(jìn)一步討論指尖的速度與各關(guān)節(jié)的速將進(jìn)一步討論指尖的速度與各關(guān)節(jié)的速 度（轉(zhuǎn)動(dòng)或平移）之間的關(guān)系。度（轉(zhuǎn)動(dòng)或平移）之間的關(guān)系。 考慮機(jī)械手的

27、手爪位置考慮機(jī)械手的手爪位置r和關(guān)節(jié)變量和關(guān)節(jié)變量的的 關(guān)系用正運(yùn)動(dòng)學(xué)方程表示如下：關(guān)系用正運(yùn)動(dòng)學(xué)方程表示如下： 40 假定這里考慮的是假定這里考慮的是 )(fr 1 21 , m T m Rrrrr 1 21 , n T m R 的一般情況，并設(shè)手爪位置包含表示姿態(tài)的變量，的一般情況，并設(shè)手爪位置包含表示姿態(tài)的變量， 以及關(guān)節(jié)變量由回轉(zhuǎn)角和平移組合而成的情況。以及關(guān)節(jié)變量由回轉(zhuǎn)角和平移組合而成的情況。 （355） 41 若用每個(gè)分量表示，則變?yōu)槿粲妹總€(gè)分量表示，則變?yōu)?在在 的情況下，將變?yōu)槭肿ξ恢玫年P(guān)節(jié)變量的情況下，將變?yōu)槭肿ξ恢玫年P(guān)節(jié)變量 有無限個(gè)解的冗余機(jī)器人。而工業(yè)上常用的多有無限個(gè)

28、解的冗余機(jī)器人。而工業(yè)上常用的多 關(guān)節(jié)機(jī)器人手臂，通常用于作業(yè)的所需手爪應(yīng)關(guān)節(jié)機(jī)器人手臂，通常用于作業(yè)的所需手爪應(yīng) 有有3個(gè)位置變量和個(gè)位置變量和3個(gè)姿態(tài)變量，總計(jì)個(gè)姿態(tài)變量，總計(jì)6個(gè)變量。個(gè)變量。 而且由于不采用冗余機(jī)器人結(jié)構(gòu)，所以而且由于不采用冗余機(jī)器人結(jié)構(gòu)，所以 ),( 21njj fr ), 2 , 1(mj mn 6 mn 42 將式（355）的兩邊對(duì)時(shí)間微分，可得 到下式 Jr （357） 其中 nm n mm n T R ff ff f J 1 1 1 1 )( （358） 43 稱稱 為雅可比矩陣（為雅可比矩陣（Jacobian matrix）。若）。若 在式（在式（357）的

29、兩邊乘以微小時(shí)間）的兩邊乘以微小時(shí)間 ，則可，則可 得到得到 J dt Jddr （359） 該式是用雅可比矩陣表示微小位移間關(guān)系的關(guān)系式。該式是用雅可比矩陣表示微小位移間關(guān)系的關(guān)系式。 44 二、與平移速度相關(guān)的雅可比矩陣二、與平移速度相關(guān)的雅可比矩陣 現(xiàn)在設(shè)基準(zhǔn)坐標(biāo)系為現(xiàn)在設(shè)基準(zhǔn)坐標(biāo)系為 ，固定于指尖，固定于指尖 的坐標(biāo)系為的坐標(biāo)系為 ，在，在 上表示的上表示的 的坐標(biāo)為的坐標(biāo)為 ，則，則 可以表示如下：可以表示如下： 0000 zyxO eeee zyxO 0000 zyxO e O e P e P )( 1 0 0 0 qfTP e （360） 45 這時(shí)，指尖的平移速度可以寫成：這時(shí)

30、，指尖的平移速度可以寫成： qJ dt dq J dt dq dq df dt dP v LL e （361） 式中，式中， ，其中，其中 是關(guān)節(jié)的數(shù)目。這是關(guān)節(jié)的數(shù)目。這 里的里的 稱為與平移速度相關(guān)的雅可比矩陣。稱為與平移速度相關(guān)的雅可比矩陣。 T n qqq),( 1 n L J 46 下面以下面以2自由度機(jī)械手為例，如前面圖自由度機(jī)械手為例，如前面圖32所所 示的示的2自由度機(jī)械手的雅可比矩陣。前面已推自由度機(jī)械手的雅可比矩陣。前面已推 導(dǎo)過，該機(jī)器人的指尖位置可以表示為導(dǎo)過，該機(jī)器人的指尖位置可以表示為 )cos(cos 21211 llx )sin(sin 21211 lly 47

31、 則與這個(gè)機(jī)器人的平移速度相關(guān)的雅可比矩陣，則與這個(gè)機(jī)器人的平移速度相關(guān)的雅可比矩陣， 可以下列形式給出：可以下列形式給出： )cos()cos(cos )sin()sin(sin 21221211 21221211 21 21 lll lll yy xx J L （363） 48 現(xiàn)在，我們來討論一下現(xiàn)在，我們來討論一下 的各列向量的幾何學(xué)意的各列向量的幾何學(xué)意 義，即在義，即在 時(shí)，考慮時(shí)，考慮 ， 的幾何學(xué)的幾何學(xué) 意義。根據(jù)式（意義。根據(jù)式（363），）， 是在時(shí)是在時(shí) ，也就，也就 是第是第2關(guān)節(jié)固定時(shí)，僅在第關(guān)節(jié)固定時(shí)，僅在第1關(guān)節(jié)轉(zhuǎn)動(dòng)的情況下，關(guān)節(jié)轉(zhuǎn)動(dòng)的情況下， 指尖平移速度在基

32、準(zhǔn)坐標(biāo)系上表示出的向量。指尖平移速度在基準(zhǔn)坐標(biāo)系上表示出的向量。 同樣，同樣， 是第是第1關(guān)節(jié)固定時(shí)，僅在第關(guān)節(jié)固定時(shí)，僅在第2關(guān)節(jié)轉(zhuǎn)動(dòng)關(guān)節(jié)轉(zhuǎn)動(dòng) 的情況下，指尖平移速度在基準(zhǔn)坐標(biāo)系上表示出的情況下，指尖平移速度在基準(zhǔn)坐標(biāo)系上表示出 的向量。的向量。 L J ),( 21LLL JJJ 1L J 2L J 1L J0 2 2L J 49 因此，當(dāng)用圖表示因此，當(dāng)用圖表示 和和 時(shí)，就變成了時(shí)，就變成了 如圖所示的情況。如圖所示的情況。 11 L J 22 L J 圖圖39 和和 的幾何學(xué)說明的幾何學(xué)說明 11 L J 22 L J 50 三、與旋轉(zhuǎn)速度相關(guān)的雅可比矩陣三、與旋轉(zhuǎn)速度相關(guān)的雅可比矩

33、陣 一般來講，指尖的旋轉(zhuǎn)速度表示方法，有以下一般來講，指尖的旋轉(zhuǎn)速度表示方法，有以下 兩種類型：兩種類型： 1考慮由表示指尖方向的三變量組合（例如為考慮由表示指尖方向的三變量組合（例如為 歐拉角）構(gòu)成向量歐拉角）構(gòu)成向量 ，然后由它對(duì)時(shí)間的微分，然后由它對(duì)時(shí)間的微分 進(jìn)行表示的一種方法。進(jìn)行表示的一種方法。 2以基準(zhǔn)坐標(biāo)系的各坐標(biāo)軸作為旋轉(zhuǎn)軸，以分以基準(zhǔn)坐標(biāo)系的各坐標(biāo)軸作為旋轉(zhuǎn)軸，以分 別圍繞各旋轉(zhuǎn)軸的角速度作為分量構(gòu)成向量別圍繞各旋轉(zhuǎn)軸的角速度作為分量構(gòu)成向量 ， 然后用然后用 進(jìn)行表示的方法。進(jìn)行表示的方法。 51 在第二種表示方法中，可以把在第二種表示方法中，可以把 解釋為在基準(zhǔn)解釋為在

34、基準(zhǔn) 坐標(biāo)系上，圍繞坐標(biāo)系上，圍繞x軸，軸，y軸和軸和 z軸的旋轉(zhuǎn)速度的軸的旋轉(zhuǎn)速度的 合成，因?yàn)槲锢硪饬x明確。這時(shí)，公式合成，因?yàn)槲锢硪饬x明確。這時(shí)，公式 qJ A （364） 其中矩陣其中矩陣JA稱為與旋轉(zhuǎn)速度相關(guān)的雅可比矩陣。稱為與旋轉(zhuǎn)速度相關(guān)的雅可比矩陣。 52 四、雅可比矩陣的計(jì)算方法四、雅可比矩陣的計(jì)算方法 考慮一般情況，如六維向量，它可以指尖的平考慮一般情況，如六維向量，它可以指尖的平 移速度和旋轉(zhuǎn)速度作為其向量的分量，即移速度和旋轉(zhuǎn)速度作為其向量的分量，即 v p （365） 這時(shí)，若采用這時(shí)，若采用 和和 表示機(jī)器人的雅可比表示機(jī)器人的雅可比 矩陣，則表示矩陣，則表示 L J A J q J J qJp A L （366） 53 這里，為了計(jì)算雅可比矩陣中的各分量，需對(duì)進(jìn)這里，為了計(jì)算雅可比矩陣中的各分量，需對(duì)進(jìn) 一步作下列分割一步作下列分割 式中，式中，n為機(jī)器人的關(guān)節(jié)數(shù)，為機(jī)器人的關(guān)節(jié)數(shù)， 和和 分別表示分別表示 和和 的第個(gè)列向量。而的第個(gè)列向量。而 和和 則分別表則分別表 示只有第示只有第i個(gè)關(guān)節(jié)以速度個(gè)關(guān)節(jié)以速度 運(yùn)行，其他的關(guān)節(jié)運(yùn)行，其他的關(guān)節(jié) 都固定時(shí)的指尖平移速度向量和旋轉(zhuǎn)速度向量。都固定時(shí)的指尖平移速度向量和旋轉(zhuǎn)速度向量。 221 21 JJJ JJJ J AA Ln