機(jī)器人學(xué)_機(jī)器人雅可比矩陣

1、 兩空間之間速度的兩空間之間速度的線(xiàn)性映射關(guān)系線(xiàn)性映射關(guān)系雅可比矩陣（簡(jiǎn)稱(chēng)雅可比矩陣（簡(jiǎn)稱(chēng)雅可雅可比比）。它可以看成是從關(guān)節(jié)空間到操作空間運(yùn)動(dòng)速度的）。它可以看成是從關(guān)節(jié)空間到操作空間運(yùn)動(dòng)速度的傳動(dòng)比傳動(dòng)比，同時(shí)也可用來(lái)表示兩空間之間同時(shí)也可用來(lái)表示兩空間之間力的傳遞關(guān)系。力的傳遞關(guān)系。vxvy12),(21),(yx存在存在怎樣怎樣的關(guān)的關(guān)系系 首先來(lái)看一個(gè)兩自由度的首先來(lái)看一個(gè)兩自由度的平面機(jī)械手，如圖平面機(jī)械手，如圖3-17所示。所示。圖圖3-17 兩自由度平面機(jī)械手兩自由度平面機(jī)械手 容易求得容易求得將其微分得將其微分得寫(xiě)成矩陣形式寫(xiě)成矩陣形式1221112211slslyclclx2

2、11221221112212211ddclclclslslsldydx 假設(shè)關(guān)節(jié)速度為假設(shè)關(guān)節(jié)速度為 ，手爪速度為，手爪速度為 。簡(jiǎn)寫(xiě)成簡(jiǎn)寫(xiě)成 ： dx=Jd。式中式中J就稱(chēng)為機(jī)械手的雅可比（就稱(chēng)為機(jī)械手的雅可比（Jacobian）矩陣矩陣，它由函數(shù)，它由函數(shù)x，y的偏微分組成，反映了關(guān)節(jié)微小位移的偏微分組成，反映了關(guān)節(jié)微小位移d與手部（手爪）微小運(yùn)與手部（手爪）微小運(yùn)動(dòng)動(dòng)dx之間的關(guān)系。之間的關(guān)系。對(duì)對(duì)dx=Jd兩邊同除以?xún)蛇呁詃t，得得Jv Jx可以更一般的寫(xiě)成可以更一般的寫(xiě)成 。 因此機(jī)械手的雅可比矩陣定義為它的操作空間速度與關(guān)節(jié)空因此機(jī)械手的雅可比矩陣定義為它的操作空間速度與關(guān)節(jié)空

3、間速度的線(xiàn)性變換。間速度的線(xiàn)性變換。 （或（或v）稱(chēng)為手爪在操作空間中的廣義速度，稱(chēng)為手爪在操作空間中的廣義速度，簡(jiǎn)稱(chēng)操作速度，簡(jiǎn)稱(chēng)操作速度， 為關(guān)節(jié)速度。為關(guān)節(jié)速度。 J若是若是6n的偏導(dǎo)數(shù)矩陣，它的第的偏導(dǎo)數(shù)矩陣，它的第i行第行第j列的元素為列的元素為 ：njiqqxqJjiij,.,2 , 1; 6,.,2 , 1,)()(式中，式中，x代表操作空間，代表操作空間，q代表關(guān)節(jié)空間。代表關(guān)節(jié)空間。 若令若令J1，J2分別為上例中雅可比矩陣的第一列矢量和第二列分別為上例中雅可比矩陣的第一列矢量和第二列矢量，即矢量，即2121JJx可以看出，可以看出，雅可比矩陣的每一列表示其它關(guān)節(jié)不動(dòng)而某一關(guān)

4、節(jié)以雅可比矩陣的每一列表示其它關(guān)節(jié)不動(dòng)而某一關(guān)節(jié)以單位速度運(yùn)動(dòng)產(chǎn)生的端點(diǎn)速度。單位速度運(yùn)動(dòng)產(chǎn)生的端點(diǎn)速度。由由 ，可以看出，可以看出，J陣的值隨手爪位置的陣的值隨手爪位置的不同而不同，即不同而不同，即1和和2的改變會(huì)導(dǎo)致的改變會(huì)導(dǎo)致J的變化。的變化。1221221112212211clclclslslslJ 對(duì)于關(guān)節(jié)空間的某些形位，機(jī)械手的雅可比矩陣的秩減少，對(duì)于關(guān)節(jié)空間的某些形位，機(jī)械手的雅可比矩陣的秩減少，這些形位稱(chēng)為操作臂（機(jī)械手）的這些形位稱(chēng)為操作臂（機(jī)械手）的奇異形位。奇異形位。上例機(jī)械手雅可比上例機(jī)械手雅可比矩陣的行列式為：矩陣的行列式為：det(J)=l1l2s2 當(dāng)當(dāng)2=0或或

5、2=180時(shí)，機(jī)械時(shí)，機(jī)械手的雅可比行列式為手的雅可比行列式為0，矩陣的秩，矩陣的秩為為1，因此處于奇異狀態(tài)。在奇異，因此處于奇異狀態(tài)。在奇異形位時(shí)，形位時(shí)，機(jī)械手在操作空間的自機(jī)械手在操作空間的自由度將減少。由度將減少。 只要知道機(jī)械手的雅可比只要知道機(jī)械手的雅可比J是滿(mǎn)秩的方陣，相應(yīng)的關(guān)節(jié)速度即是滿(mǎn)秩的方陣，相應(yīng)的關(guān)節(jié)速度即可求出，即可求出，即 。 上例平面上例平面2R機(jī)械手的逆雅可比機(jī)械手的逆雅可比于是得到與末端速度于是得到與末端速度 相應(yīng)的關(guān)節(jié)速度：相應(yīng)的關(guān)節(jié)速度：顯然，當(dāng)顯然，當(dāng)2趨于趨于0（或（或180）時(shí)，機(jī)械手接近奇異形位，相應(yīng)）時(shí)，機(jī)械手接近奇異形位，相應(yīng)的關(guān)節(jié)速度將趨于無(wú)窮

6、大。的關(guān)節(jié)速度將趨于無(wú)窮大。122111221112212222111slslclclslclsl lJ4.1.2 微分變換微分變換 為了補(bǔ)償機(jī)器人為了補(bǔ)償機(jī)器人末端執(zhí)行器位姿與目標(biāo)物體之間的誤差末端執(zhí)行器位姿與目標(biāo)物體之間的誤差，以，以及解決及解決兩個(gè)不同坐標(biāo)系之間的微位移關(guān)系兩個(gè)不同坐標(biāo)系之間的微位移關(guān)系問(wèn)題，需要討論機(jī)器人問(wèn)題，需要討論機(jī)器人桿件在作微小運(yùn)動(dòng)時(shí)的位姿變化。桿件在作微小運(yùn)動(dòng)時(shí)的位姿變化。一一.變換的微分變換的微分 假設(shè)一變換的元素是某個(gè)變量的函數(shù)，對(duì)該變換的微分就是假設(shè)一變換的元素是某個(gè)變量的函數(shù)，對(duì)該變換的微分就是該變換矩陣各元素對(duì)該變量的偏導(dǎo)數(shù)所組成的變換矩陣乘以該變?cè)?/p>

7、變換矩陣各元素對(duì)該變量的偏導(dǎo)數(shù)所組成的變換矩陣乘以該變量的微分量的微分。若它的元素是變量若它的元素是變量x的函數(shù)，則變換的函數(shù)，則變換T的微分為的微分為:例如給定變換例如給定變換T為：為：44434241343332312423222114131211ttttttttttttttttTdxxtxtxtxtxtxtxtxtxtxtxtxtxtxtxtxtdT44434241343332312423222114131211二二. 微分運(yùn)動(dòng)微分運(yùn)動(dòng)所以得所以得 設(shè)機(jī)器人某一桿件相對(duì)于基坐標(biāo)系的位姿為設(shè)機(jī)器人某一桿件相對(duì)于基坐標(biāo)系的位姿為T(mén)，經(jīng)過(guò)微運(yùn)動(dòng)經(jīng)過(guò)微運(yùn)動(dòng)后該桿件相對(duì)基坐標(biāo)系的位姿變?yōu)楹笤摋U件相

8、對(duì)基坐標(biāo)系的位姿變?yōu)門(mén)+dT，若這個(gè)微運(yùn)動(dòng)是若這個(gè)微運(yùn)動(dòng)是相對(duì)相對(duì)于基坐標(biāo)系（靜系）進(jìn)行的于基坐標(biāo)系（靜系）進(jìn)行的(左乘左乘)，總可以用微小的平移和旋轉(zhuǎn)總可以用微小的平移和旋轉(zhuǎn)來(lái)表示，即來(lái)表示，即TdkRotdddTransdTTzyx),(),(TIdkRotdddTransdTzyx44),(),(根據(jù)齊次變換的相對(duì)性，若微運(yùn)動(dòng)是根據(jù)齊次變換的相對(duì)性，若微運(yùn)動(dòng)是相對(duì)某個(gè)桿件坐標(biāo)系相對(duì)某個(gè)桿件坐標(biāo)系i（動(dòng)系）動(dòng)系）進(jìn)行的進(jìn)行的(右乘右乘)，則則T+dT可以表示為可以表示為則相對(duì)基系有則相對(duì)基系有dT=0T，相對(duì)相對(duì)i系有系有dT=Ti 。這里這里的下標(biāo)不同是由的下標(biāo)不同是由于微運(yùn)動(dòng)相對(duì)不同坐

9、標(biāo)系進(jìn)行的。于微運(yùn)動(dòng)相對(duì)不同坐標(biāo)系進(jìn)行的。(,)( ,)xyzTdTT Trans d dd Rot k d所以得所以得44),(),(IdkRotdddTransTdTzyx令令 為微分算子為微分算子44),(),(IdkRotdddTranszyx三三.微分平移和微分旋轉(zhuǎn)微分平移和微分旋轉(zhuǎn) 由于微分旋轉(zhuǎn)由于微分旋轉(zhuǎn)0 ，所以所以sind，cos1，Vers0，將將它們代入旋轉(zhuǎn)變換通式它們代入旋轉(zhuǎn)變換通式(p27)中得微分旋轉(zhuǎn)表達(dá)式中得微分旋轉(zhuǎn)表達(dá)式: 微分平移變換與一般平移微分平移變換與一般平移變換一樣，其變換矩陣為變換一樣，其變換矩陣為:于是得微分算子于是得微分算子10001000100

10、01),(dzdydxdzdydxTrans1000010101),(dkdkdkdkdkdkdkRotxyxzyz44),(),(IdkRotdddTranszyx0000000dzdkdkdydkdkdxdkdkxyxzyz四四. 微分旋轉(zhuǎn)的無(wú)序性微分旋轉(zhuǎn)的無(wú)序性 當(dāng)當(dāng)0 時(shí)，有時(shí)，有sind，cos1若令若令x=dx，y=dy，z=dz，則繞三個(gè)坐標(biāo)軸則繞三個(gè)坐標(biāo)軸(p16)的微分旋轉(zhuǎn)矩陣分別為的微分旋轉(zhuǎn)矩陣分別為10000100100001),(xxxxRot10000100010001),(yyyyRot10000100001001),(zzzzRot10000101001),(),

11、(xyxyxyyyRotxxRot100001010001xyxy10000101001),(),(xyxyyxxxRotyyRot100001010001xyxy略去高略去高階無(wú)窮階無(wú)窮小量小量?jī)烧呓Y(jié)果相同，可見(jiàn)這里兩者結(jié)果相同，可見(jiàn)這里。同理可得同理可得 1000010101),(),(),(xyxzyzzzRotyyRotxxRot 若若Rot（x，y，z） 和和Rot（x，y，z） 表示兩表示兩個(gè)不同的微分旋轉(zhuǎn)，則兩次連續(xù)轉(zhuǎn)動(dòng)的結(jié)果為：個(gè)不同的微分旋轉(zhuǎn)，則兩次連續(xù)轉(zhuǎn)動(dòng)的結(jié)果為：任意兩個(gè)微分旋轉(zhuǎn)的結(jié)果為繞每個(gè)軸轉(zhuǎn)動(dòng)的元素的代任意兩個(gè)微分旋轉(zhuǎn)的結(jié)果為繞每個(gè)軸轉(zhuǎn)動(dòng)的元素的代數(shù)和，即微分旋轉(zhuǎn)是

12、可加的。數(shù)和，即微分旋轉(zhuǎn)是可加的。100001) (0) (10) (1) , , (),(xxyyxxzzyyzzzyxRotzyxRotkxd=x， kyd=y ， kzd=z所以有所以有由等效轉(zhuǎn)軸和等效轉(zhuǎn)角與由等效轉(zhuǎn)軸和等效轉(zhuǎn)角與 等效，有等效，有),(),(),(zzRotyyRotxxRot),(dkRot),(),(),(zzRotyyRotxxRot即即1000010101dkdkdkdkdkdkxyxzyz1000010101xyxzyz將它們代入將它們代入得得0000000zxyyxzxyzddd因此因此可以看成由可以看成由 和和 兩個(gè)矢量組成，兩個(gè)矢量組成， 叫叫微分轉(zhuǎn)動(dòng)矢

13、量微分轉(zhuǎn)動(dòng)矢量， 叫叫微分平移矢量微分平移矢量。分別表示為。分別表示為dd 和和 合稱(chēng)為合稱(chēng)為微分運(yùn)動(dòng)矢量微分運(yùn)動(dòng)矢量，可表示為，可表示為dkdjdiddkjizyxzyxTzyxzyxdddD),(解：解：例：已知一個(gè)坐標(biāo)系例：已知一個(gè)坐標(biāo)系A(chǔ) ，相對(duì)固定系的微分平相對(duì)固定系的微分平移矢量移矢量 ，微分旋轉(zhuǎn)矢量，微分旋轉(zhuǎn)矢量 ，求微分變換求微分變換dA。kid5 . 0j1 . 010000010500110100A00005 . 0001 . 0000011 . 000AdA1000001050011010000005 . 0001 . 0000011 . 00000005 . 01 .

14、0000000101 . 000000000zxyyxzxyzddd因?yàn)橐驗(yàn)閷⑺鼈兇肭懊娴姆匠虒⑺鼈兇肭懊娴姆匠?0000zzzzyyyyxxxxipaonpaonpaonT令00000000dzxydyxzdxyz0000000iiiiiiiiiidzxydyxzdxyziiiTT000TTiii0010現(xiàn)在討論現(xiàn)在討論i系和系和j系之間的微分關(guān)系。不失一般性，假定系之間的微分關(guān)系。不失一般性，假定j系就系就是固定系（基系）是固定系（基系）0系。系。整理得到：整理得到：得得zyxdzdydxaaaooonnnapapapaaaopopopooonpnpnpnnnzyxdzdydxzyxz

15、yxzyxzyxzyxzyxzyxzyxzyxiiiiii000000000)()()()()()()()()(其中其中上式簡(jiǎn)寫(xiě)成上式簡(jiǎn)寫(xiě)成)(0ioPS對(duì)于任何三維矢量對(duì)于任何三維矢量 ，其反對(duì)稱(chēng)矩陣，其反對(duì)稱(chēng)矩陣 定義為：定義為：相應(yīng)地，任意兩坐標(biāo)系相應(yīng)地，任意兩坐標(biāo)系A(chǔ)和和B之間廣義速度的坐標(biāo)變換為：之間廣義速度的坐標(biāo)變換為：BBABAOBABABAAVRPRSRV0)(例：例：知坐標(biāo)系知坐標(biāo)系A(chǔ)及相對(duì)于固定系的微分平移及相對(duì)于固定系的微分平移矢量矢量 ，微分旋轉(zhuǎn)矢量，微分旋轉(zhuǎn)矢量 ，求，求A系中等價(jià)的微分平移矢量系中等價(jià)的微分平移矢量dA和微分旋轉(zhuǎn)矢和微分旋轉(zhuǎn)矢量量A。解：解：因?yàn)橐阎?/p>

16、因?yàn)橐阎?，可以根據(jù)前面的公式求得，可以根據(jù)前面的公式求得dA和和A。也可也可根據(jù)與它一樣的另一組表達(dá)式（寫(xiě)法不同）求解，即根據(jù)與它一樣的另一組表達(dá)式（寫(xiě)法不同）求解，即求得求得 ， 機(jī)器人與外界環(huán)境相互作用時(shí)，在接觸的地方要產(chǎn)生力機(jī)器人與外界環(huán)境相互作用時(shí)，在接觸的地方要產(chǎn)生力 和和力矩力矩 ，統(tǒng)稱(chēng)為，統(tǒng)稱(chēng)為末端廣義（操作）力矢量末端廣義（操作）力矢量。記為。記為n個(gè)關(guān)節(jié)的驅(qū)動(dòng)力（或力矩）組成的個(gè)關(guān)節(jié)的驅(qū)動(dòng)力（或力矩）組成的n維矢量維矢量稱(chēng)為關(guān)節(jié)力矢量稱(chēng)為關(guān)節(jié)力矢量y0 x012 , TFf n),(21),(nf存在怎樣的關(guān)系存在怎樣的關(guān)系 利用虛功原理，令各關(guān)節(jié)的虛位移為利用虛功原理，令

17、各關(guān)節(jié)的虛位移為qi ，末端執(zhí)行器相應(yīng)末端執(zhí)行器相應(yīng)的虛位移為的虛位移為D。根據(jù)虛位移原理，各關(guān)節(jié)所作的虛功之和與末端根據(jù)虛位移原理，各關(guān)節(jié)所作的虛功之和與末端執(zhí)行器所作的虛功應(yīng)該相等，即執(zhí)行器所作的虛功應(yīng)該相等，即簡(jiǎn)寫(xiě)為簡(jiǎn)寫(xiě)為:又因?yàn)橛忠驗(yàn)?, 所以得到所以得到 與與 之間的關(guān)系之間的關(guān)系式中式中 稱(chēng)為稱(chēng)為機(jī)械手的力雅可比。它表示在靜態(tài)平衡狀態(tài)下，機(jī)械手的力雅可比。它表示在靜態(tài)平衡狀態(tài)下，操作力向關(guān)節(jié)力映射的線(xiàn)性關(guān)系。操作力向關(guān)節(jié)力映射的線(xiàn)性關(guān)系。 若若J是是關(guān)節(jié)空間關(guān)節(jié)空間向向操作空間操作空間的映射（微分運(yùn)動(dòng)矢量），則的映射（微分運(yùn)動(dòng)矢量），則 把把操作空間的廣義力矢量操作空間的廣義力矢量

18、映射到映射到關(guān)節(jié)空間的關(guān)節(jié)力矢量關(guān)節(jié)空間的關(guān)節(jié)力矢量。關(guān)節(jié)空間關(guān)節(jié)空間操作空間操作空間雅可比雅可比J力雅可比力雅可比JT若已知若已知?jiǎng)t有則有zyxdzdydxaonaonaonapapapaaaopopopooonpnpnpnnnzyxdzdydxzzzyyyxxxzyxzyxzyxzyxzyxzyxiiiiii000000000)()()()()()()()()(T00TBAABBABAOBABABAAVRPRSRV0)(BJTJ 根據(jù)前面導(dǎo)出的兩坐標(biāo)系根據(jù)前面導(dǎo)出的兩坐標(biāo)系A(chǔ)和和B之間廣義速度的坐標(biāo)變換之間廣義速度的坐標(biāo)變換關(guān)系，可以導(dǎo)出關(guān)系，可以導(dǎo)出A和和B之間廣義操作力的坐標(biāo)變換關(guān)系。

19、之間廣義操作力的坐標(biāo)變換關(guān)系。解：解：由前面的推導(dǎo)知由前面的推導(dǎo)知例例 :如圖如圖3-18所示的平面所示的平面2R機(jī)械手，手爪端點(diǎn)與外界接觸，手爪機(jī)械手，手爪端點(diǎn)與外界接觸，手爪作用于外界環(huán)境的力為作用于外界環(huán)境的力為 ，若關(guān)節(jié)無(wú)摩擦，若關(guān)節(jié)無(wú)摩擦力存在，求力力存在，求力 的等效關(guān)節(jié)力矩的等效關(guān)節(jié)力矩 。所以得：所以得：圖圖3-18 關(guān)節(jié)力和操作力關(guān)系關(guān)節(jié)力和操作力關(guān)系y0 x012TyxFFF,0例：如圖所示的機(jī)械手夾扳手?jǐn)Q螺絲，在腕部（例：如圖所示的機(jī)械手夾扳手?jǐn)Q螺絲，在腕部（Os）裝有力裝有力/力矩力矩傳感器，若已測(cè)出傳感器上的力和力矩傳感器，若已測(cè)出傳感器上的力和力矩 ，求這，求這時(shí)作用在螺釘上的力和力矩時(shí)作用在螺釘上的力和力矩 。( ),zyxSOTrrrP解：根據(jù)圖示的相應(yīng)位姿關(guān)系得解：根據(jù)圖示的相應(yīng)位姿關(guān)系得因此可得兩坐標(biāo)系的微分運(yùn)動(dòng)關(guān)系和靜力傳遞關(guān)系為：因此可得兩坐標(biāo)系的微分運(yùn)動(dòng)關(guān)系和靜力傳遞關(guān)系為：STST微分運(yùn)動(dòng)關(guān)系時(shí)：微分運(yùn)動(dòng)關(guān)系時(shí)：靜力傳遞關(guān)系時(shí)：靜力傳遞關(guān)系時(shí)： 對(duì)于任何機(jī)械系統(tǒng)，拉格朗日函數(shù)對(duì)于任何機(jī)械系統(tǒng)，拉格朗日函數(shù)L定義為系統(tǒng)總的動(dòng)能定義為系統(tǒng)總的動(dòng)能K與總的勢(shì)能與總的勢(shì)能P之差，即之差，即L=K-P。這里，這里，L是拉格朗日算子；是拉格朗日算子；k是動(dòng)是動(dòng)能；能；P是勢(shì)能。是勢(shì)能。 qEqEqEdtdpk