第5章機(jī)器人控制技術(shù)微分變換ppt課件

1、第五章第五章 微分變換微分變換 ChapterChapter Differential Differential RelationshipsRelationships5.1 5.1 引言引言5.2 5.2 微分矩陣微分矩陣 5.3 5.3 微分平移和旋轉(zhuǎn)變換微分平移和旋轉(zhuǎn)變換 5.4 5.4 微分旋轉(zhuǎn)微分旋轉(zhuǎn) 5.5 5.5 坐標(biāo)系之間的微分變換坐標(biāo)系之間的微分變換 5.6 5.6 機(jī)械手的微分變換方程機(jī)械手的微分變換方程 雅可比如程雅可比如程 5.7 5.7 雅可比逆矩陣雅可比逆矩陣5.8 5.8 本章小結(jié)本章小結(jié)5.1 引言引言Introduction 微分變換在機(jī)器人視覺、動(dòng)力學(xué)和機(jī)器人

2、控制如力控、剛度控制、阻抗控制、順應(yīng)控制等中非常重要。例如當(dāng)攝像機(jī)或其它傳感安裝檢測(cè)到機(jī)器人末端執(zhí)行器的位置和方向的微小變化時(shí)，需求將該微小變化從攝像機(jī)或其它傳感安裝坐標(biāo)轉(zhuǎn)換到基坐標(biāo)或參考坐標(biāo)系。在機(jī)器人剛度控制中，需求獲得在控制坐標(biāo)系中力與位置的微分變換。又如將直角坐標(biāo)的微分變換轉(zhuǎn)化為關(guān)節(jié)坐標(biāo)的微分變換，還有在下一章引見的機(jī)器人動(dòng)力學(xué)問題時(shí)，也會(huì)用到微分變換。本章將引見微分變換的根本原理和方法，包括微分平移、微分旋轉(zhuǎn)、坐標(biāo)系之間的微分變換、雅可比矩陣和逆雅可比矩陣及其運(yùn)用。 5.2 微分矩陣微分矩陣Derivative Matrixes 給出一個(gè)44的矩陣A 5.1矩陣A的微分就是對(duì)矩陣A中

3、的每一個(gè)元素對(duì)自變量x的微分，結(jié)果如下 5.244434241343332312423222114131211aaaaaaaaaaaaaaaaAdxxaxaxaxaxaxaxaxaxaxaxaxaxaxaxaxadA444342413433323124232221141312115.3 微分平移和旋轉(zhuǎn)變換微分平移和旋轉(zhuǎn)變換 ( Differential Translation and Rotation ) 微分平移和旋轉(zhuǎn)變換可以是針對(duì)基坐標(biāo)或參考坐標(biāo)系，也可以是針對(duì)某個(gè)指定的坐標(biāo)系進(jìn)展。例如對(duì)于一個(gè)變換矩陣T，它對(duì)基坐標(biāo)的微分變換可表示為 5.3 式中是在基坐標(biāo)的x，y，z軸向上分別平移dx，

4、dy，dz；和繞基坐標(biāo)的向量k旋轉(zhuǎn)d角。由此可得到 5.4 假設(shè)上述微分變換不是針對(duì)基坐標(biāo)而是針對(duì)坐標(biāo)系T，那么微分變換的結(jié)果可表示為 5.5 此時(shí)，式中 是在T坐標(biāo)的x，y，z軸向上分別平移dx，dy，dz； 是繞T坐標(biāo)的向量k旋轉(zhuǎn)d角。由此可得到 5.6 TdkRotdzdydxTransdTT),(),(TIdkRotdzdydxTransdT),(),(),(),(dkRotdzdydxTransTdTT),(),(IdkRotdzdydxTransTdT),(dzdydxTrans),(dkRot 我們用符號(hào) 來表示式5.4和式5.6中的 并將它稱為微分變換算子5.6 這款式5.4和

5、式5.6就可寫成如下方式 5.7 和 5.8 式5.7中的微分變換算子 是針對(duì)基坐標(biāo)的，而式5.8中的微分變換算子 那么是針對(duì)T坐標(biāo)的。 在第二章我們給出了平移和普通性旋轉(zhuǎn)變換的齊次變換矩陣表達(dá)式，平移變換矩陣是 1 0 0 a 0 1 0 bTrans( a, b, c ) = 0 0 1 c 5.9 0 0 0 1),(),(IdkRotdzdydxTransIdkRotdzdydxTrans),(),(TdTTTdTT當(dāng)平移向量是微分向量ddxi+dyj+dzk時(shí)，微分平移矩陣為 1 0 0 dx 0 1 0 dyTrans( d ) = 0 0 1 dz 5.10 0 0 0 1普通性

6、旋轉(zhuǎn)變換的變換矩陣是 kxkxvers + cos kykxvers - kzsin kzkxvers + kysin 0 kxkyvers + kzsin kykyvers + cos kzkyvers - kxsin 0Rot( k,) = kxkzvers - kysin kykzvers + kxsin kzkzvers + cos 0 (5.11) 0 0 0 1當(dāng)進(jìn)展微分旋轉(zhuǎn)變換時(shí)，旋轉(zhuǎn)角d極小，此時(shí)有如下關(guān)系dsinlim01coslim00lim0vers將上述關(guān)系代入式5.11可得 1 - kzd kyd 0 kzd 1 - kxd 0 Rot( k, d) = - kyd

7、kxd 1 0 (5.12) 0 0 0 1由式5.6可得 (5.13)100001000010000110000101011000100010001dkdkdkdkdkdkdddxyxzyzzyx0000000zxyyxzxyzddkdkddkdkddkdk5.4 微分旋轉(zhuǎn)微分旋轉(zhuǎn) (Differential Rotations) 式5.13給出的微分變換算子 是基于微分旋轉(zhuǎn)角d的微分平移和旋轉(zhuǎn)變換表達(dá)式，下面討論繞坐標(biāo)軸x、y、z旋轉(zhuǎn)x、y、z的微分變換。 第二章給出的繞坐標(biāo)軸x、y、z旋轉(zhuǎn)的變換矩陣分別為 5.14 5.15 5.1610000cossin00sincos00001),(

8、xRot10000cos0sin00100sin0cos),(yRot1000000000cossin00sincos),(zRot在微分變換的情況下，sind，con1，上面三個(gè)式子變?yōu)?5.17 5.18 5.19由此可得到 5.2010000100100001),(xxxxRot10000100010001),(yyyyRot10000000001001),(zzzzRot1000000000),(),(),(xyxzyzzyxzRotyRotxRot 比較式5.12和式5.20可知，繞恣意向量k旋轉(zhuǎn)d的微分旋轉(zhuǎn)與繞x、y、z軸分別旋轉(zhuǎn) 的結(jié)果一樣，即 5.21 由此可得到繞坐標(biāo)軸x、y

9、、z旋轉(zhuǎn)x、y、z的微分變換算子為 5.22 微分變換算子中的元素由微分平移向量d和微分旋轉(zhuǎn)向量的各個(gè)分量組成，即 5.23 5.24 將上述二個(gè)向量組合構(gòu)成一個(gè)微分運(yùn)動(dòng)矢量D 5.25 這樣，我們就可根據(jù)式5.25給出的微分運(yùn)動(dòng)矢量D直接得到微分變換算子 ，或基于T坐標(biāo)的微分運(yùn)動(dòng)矢量 的微分變換算子 。zy、xdkxxdkyydkzz1000000zxyyxzxyzdddkdjdiddzyxkjizyxTzyxzyxdddD),(TTTdDT【例5.1】知坐標(biāo)A的變換矩陣為 當(dāng)用微分平移矢量d = 1i + 0j + 0.5k和微分旋轉(zhuǎn)矢量 0i + 0.1j + 0k對(duì)坐標(biāo)A 進(jìn)展變換時(shí)，

10、求出微分變換的結(jié)果dA。解：首先，由式5.22求出微分變換算子 由式5.7可得 即10000010500110100AAdA00005 . 01 . 0000000101 . 001000001050011010000005 . 0001 . 0000011 . 000dA微分變換結(jié)果如圖5.1所示。xyzzAyA+dAx圖5.1 坐標(biāo)A的微分變換5.5 坐標(biāo)系之間的微分變換坐標(biāo)系之間的微分變換 (Transforming Differential Changes between Coordinate Frames) 上節(jié)討論了基于基坐標(biāo)或某個(gè)指定坐標(biāo)的微分變換，本節(jié)繼續(xù)討論坐標(biāo)系之間的微分變

11、換，也就是知微分變換算子 ，如何求出T坐標(biāo)的微分變換算子 。由式5.7和5.8可知 5.26 那么為 5.27 上式是一個(gè)重要的表達(dá)式，它描畫了坐標(biāo)系之間的微分變換關(guān)系。下面我們用微分平移矢量d和微分旋轉(zhuǎn)矢量 來推導(dǎo) 的表達(dá)式。 知變換矩陣T為TTTTTT1T1000zzzzyyyyxxxxpaonpaonpaonT我們用矢量的叉乘來得到式5.27等號(hào)右邊二項(xiàng)的乘積 5.29式中d和 分別是微分平移和微分旋轉(zhuǎn)矢量。用 左乘式5.29可得 5.30上式矩陣元素都具有如下矢量三重積方式根據(jù)矢量三重積的性質(zhì)有 5.310000zzzzyyyyxxxxdpaondpaondpaonT00001dpaa

12、aoanadpoaooonodpnanonnnTT1Tcbaacbcabcba同時(shí)，三重積中只需有二個(gè)矢量是一樣的，其結(jié)果為零。如 5.32根據(jù)上述性質(zhì)，式5.30可寫成 5.33對(duì)于正交矢量有 5.34這樣，式5.33可重寫成 5.350caa0000000adapaonaodopaoonndnpnaonTaononanao0000000adapnoodopnandnpoaT 上式可進(jìn)一步簡(jiǎn)化為 5.36 比較式5.35和式5.36的矩陣元素可得 5.37 5.38 在式5.37和式5.38中，n、o、a和p是微分坐標(biāo)變換矩陣T的旋轉(zhuǎn)和平移矢量， 和 是對(duì)應(yīng)坐標(biāo)T的微分平移和旋轉(zhuǎn)矢量。000

13、0000zTxTyTyTxTzTxTyTzTTdddndnpdxTodopdyTadapdzTnxToyTazTdTT式5.37和式5.38也可用66的矩陣方式表示如下 5.39將上式寫成式5.36和式5.37的方式如下 5.40 5.41式5.40和式5.41是后續(xù)內(nèi)容中要經(jīng)常用到的重要結(jié)果。zyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzTyTxTzTyTxTdddaaaooonnnapapapaaaopopopooonpnpnpnnnddd000000000dpndxTdpodyTdpadzT nxT oyT azT【例5.2】給出與例5.1一樣的坐標(biāo)的變換矩陣、微分

14、平移矢量和微分旋轉(zhuǎn)矢量如下： d = 1i + 0j + 0.5k 0i + 0.1j + 0k 試求出坐標(biāo)A上的等效微分變換dA。解：由坐標(biāo)變換矩陣A可得到相應(yīng)的旋轉(zhuǎn)與平移矢量 由此可求出 根據(jù)式5.40和式5.41得到10000010500110100Akjin010kjio100kjia001kjip0510kjip100kjidp5 . 001kjidA15 . 00kjiA001 . 0用上述結(jié)果來驗(yàn)證坐標(biāo)A上的等效微分變換dA，由式5.8有由已求出的 、 和式5.36可得到那么上述結(jié)果與例5.1一樣。AAdAdAA0000101 . 005 . 01 . 0000000A00005

15、 . 01 . 0000000101 . 000000101 . 005 . 01 . 000000010000010500110100dA5. 6 機(jī)械手的微分變換方程機(jī)械手的微分變換方程雅可比如程雅可比如程 The Manipulator Jacobian 在第三章我們引見過，機(jī)械手的運(yùn)動(dòng)學(xué)方程由它的末端相對(duì)于基坐標(biāo)的齊次變換矩陣T6表示，即T6 = A1 A2 A3 A4 A5 A6 5.42 其中每一個(gè)關(guān)節(jié)變換矩陣Ai描畫了該關(guān)節(jié)坐標(biāo)相對(duì)于前一個(gè)關(guān)節(jié)坐標(biāo)的變換關(guān)系，關(guān)節(jié)變量用qi表示，假設(shè)是旋轉(zhuǎn)關(guān)節(jié)，關(guān)節(jié)變量是i，它是繞前一個(gè)關(guān)節(jié)坐標(biāo)z軸的旋轉(zhuǎn)角度；假設(shè)是滑動(dòng)關(guān)節(jié)，關(guān)節(jié)變量是di，它是

16、沿前一個(gè)關(guān)節(jié)坐標(biāo)z軸滑動(dòng)的間隔 。同樣，當(dāng)我們討論機(jī)械手的微分變換方程時(shí)，首先定義微分關(guān)節(jié)變量為dqi，假設(shè)是旋轉(zhuǎn)關(guān)節(jié)，那么為di，假設(shè)是滑動(dòng)關(guān)節(jié)，那么為ddi。 機(jī)械手第i個(gè)關(guān)節(jié)的微分變換引起第6個(gè)連桿末端即機(jī)械手末端的微分變換dT6可由下式表示： 5.43 那么 5.44 由式5.27可得到機(jī)械手末端的微分變換算子 5.45 其中 5.46 假設(shè)關(guān)節(jié)i是旋轉(zhuǎn)關(guān)節(jié)，那么di = 0，式5.40和式5.41變?yōu)?5.475.48iiiiiiiTdqAAAAAAdqTdT611121666iTiTqT6666111616AAAAAAiiiiiiT6161AAATTiiipndxiTpodyiTp

17、adziT nxiT oyiT aziT 當(dāng) ，為單位微分旋轉(zhuǎn)矢量時(shí)，式5.47和5.48可進(jìn)一步簡(jiǎn)化為 5.49 5.50 假設(shè)關(guān)節(jié)i是棱形滑動(dòng)關(guān)節(jié)，那么i0，di = 0i + 0j + 1k，式5.40和式5.41變?yōu)?5.51 5.52 機(jī)械手末端坐標(biāo)T6的微分變換是一切6個(gè)關(guān)節(jié)微分變量的函數(shù)，可用66的矩陣表示，矩陣元素由6個(gè)關(guān)節(jié)的微分平移和微分旋轉(zhuǎn)矢量構(gòu)成，該矩陣稱為雅可比矩陣。它的每一列元素為對(duì)應(yīng)關(guān)節(jié)的微分平移和微分旋轉(zhuǎn)矢量。運(yùn)用雅可比矩陣的機(jī)械手微分變換方程雅可比如程如下： 5.53kjii100kpapajpopoipnpndxyyxxyyxxyyxiT6kajoinzzzi

18、T6kajoindzzziT6kjiiT0006654321654321654321654321654321654321654321666666666666666666666666666666666666666666dqdqdqdqdqdqdddddddddddddddddddddzTzTzTzTzTzTyTyTyTyTyTyTxTxTxTxTxTxTzTzTzTzTzTzTyTyTyTyTyTyTxTxTxTxTxTxTzTyTxTzTyTxT5.7 雅可比逆矩陣雅可比逆矩陣The Inverse Jacobian 當(dāng)微分變換是由直角坐標(biāo)空間向關(guān)節(jié)坐標(biāo)空間進(jìn)展時(shí)，由式5.53可得到 5.72

19、 上式等號(hào)右邊矩陣是雅可比逆矩陣。顯然，用符號(hào)運(yùn)算來得到雅可比逆陣是很困難的，由于微分變換要進(jìn)展大量算術(shù)運(yùn)算，同時(shí)當(dāng)機(jī)械手出現(xiàn)退化時(shí)，其結(jié)果會(huì)出錯(cuò)。 為此，我們采用第四章引見的根據(jù)T6的值計(jì)算關(guān)節(jié)坐標(biāo)值的方法和步驟來計(jì)算微分關(guān)節(jié)坐標(biāo)值。將關(guān)節(jié)坐標(biāo)的微分變換表示為dT6中各元素的函數(shù)，然后求出各關(guān)節(jié)的微分變換值。該方法相對(duì)比較簡(jiǎn)單，而且在機(jī)械手出現(xiàn)退化時(shí)，將相應(yīng)關(guān)節(jié)的微分變換值設(shè)置為零，這就不會(huì)影響后續(xù)關(guān)節(jié)的計(jì)算結(jié)果。在后面的討論中，我們假設(shè)機(jī)械手的符號(hào)解存在，而且關(guān)節(jié)變量的正弦和余弦值知。zTyTxTzTyTxTzTzTzTzTzTzTyTyTyTyTyTyTxTxTxTxTxTxTzTzTz

20、TzTzTzTyTyTyTyTyTyTxTxTxTxTxTxTddddddddddddddddddddddqdqdqdqdqdq6666666666666666666666666666666666666666661654321654321654321654321654321654321654321 為了計(jì)算dT6,我們首先根據(jù)式5.37和式5.38對(duì)T6進(jìn)展微分變換得到微分平移矢量 和微分旋轉(zhuǎn)矢量 ，然后根據(jù)式5.22求出 ，最后根據(jù)式5.8得到dT6。 下面經(jīng)過對(duì)第四章引見的斯坦福機(jī)械手逆運(yùn)動(dòng)學(xué)解的微分變換來闡明上述方法的詳細(xì)步驟。 由第四章式4.15有S1 pxC1 py = d2 5.7

21、3 對(duì)式5.73求導(dǎo)可直接得到第一個(gè)關(guān)節(jié)變量1的微分 5.74 對(duì)于正切函數(shù) 5.75 其微分公式為 5.76dT66T6TyxxypSpCdpSdpCd11111cossintanNN22)cos()sin()cos(sin)sin(cosNNNdNNdNd由第四章式4.24和式4.25有 5.77 5.78對(duì)式5.77和式5.78求微分得到 5.79 5.80由公式5.76可得到第二個(gè)關(guān)節(jié)變量2的微分 5.81將式5.77代入第四章的式4.31有 5.82對(duì)式5.82進(jìn)展微分可直接得到第三個(gè)滑動(dòng)關(guān)節(jié)變量d3的微分 5.83yxpSpCdSNS11322zpdCNC322yyxxdpSpdS

22、dpCpdCNSd11112)(zdpNCd)(232222)(ddpSNSdCdzzpCNSSd2223zzdpCpdCNSdSNSdSdd2222223)( 由第四章式4.38和式4.39有NS4 =S1axC1ay 5.84 NC4 = C2D41S2az 5.85其中 D41 = C1axS1ay 5.86對(duì)式5.84式5.86進(jìn)展微分得到 5.87 5.88 5.89由式5.76可得到第四個(gè)關(guān)節(jié)變量4的微分d4。在計(jì)算第五個(gè)關(guān)節(jié)變量微分時(shí)，為了簡(jiǎn)化計(jì)算，我們可將式5.76簡(jiǎn)化為 5.90 由第四章式4.42和式4.43有S5 = C4NC4+ NS4 5.91C5 = S2D41 +

23、 C2az 5.92yyxxdaSadSdaCadCdD111141yyxxdaCadCdaSadSNSd11114)(zzdaSadSdDCDdCNCd224124123)()(cossin)(sincosddd對(duì)式5.91和式5.92進(jìn)展微分得到 5.93 5.94由式5.90可得到第五個(gè)關(guān)節(jié)變量5的微分d5。 最后，我們由第四章式4.49和式4.50有S6 =C5N61S5N612 5.95C6 =S4N611 + C4N6112 5.96其中N6111 = C1ox + S1oy 5.97dN6111 = dC1ox + C1dox + dS1oy + S1doy 5.98N6112 =S1ox + C1oy 5.99dN6112 =dS1oxS1dox + dC1oy + C1doy 5.100)()(444444445NSdSNSdSNCdCNCdCdSzzdaCadCdDSDdSdC224124124N611 = C2N6111S2oz 5.101dN611 = dC2N6111 + C2dN6111dS2ozS2doz 5.102N612 =S2N6111C2oz 5.103dN612