1、機械與汽車工程學院2021年11月14日1第五章第五章 微分變換微分變換 chapter differential relationshipschapter differential relationships5.1 5.1 引言引言5.2 5.2 微分矩陣微分矩陣 5.3 5.3 微分平移和旋轉變換微分平移和旋轉變換 5.4 5.4 微分旋轉微分旋轉 5.5 5.5 坐標系之間的微分變換坐標系之間的微分變換 5.6 5.6 機械手的微分變換方程機械手的微分變換方程 雅可比方程雅可比方程 5.7 5.7 雅可比逆矩陣雅可比逆矩陣5.8 5.8 本章小結本章小結機械與汽車工程學院2021年11月

2、14日25.1 引言引言（introduction） 微分變換在機器人視覺、動力學和機器人控制（如力控、剛度控制、阻抗控制、順應控制等）中十分重要。例如當攝像機或其它傳感裝置檢測到機器人末端執行器的位置和方向的微小變化時，需要將該微小變化從攝像機或其它傳感裝置坐標轉換到基坐標或參考坐標系。在機器人剛度控制中，需要獲得在控制坐標系中力與位置的微分變換。又如將直角坐標的微分變換轉化為關節坐標的微分變換，還有在下一章介紹的機器人動力學問題時，也會用到微分變換。本章將介紹微分變換的基本原理和方法，包括微分平移、微分旋轉、坐標系之間的微分變換、雅可比矩陣和逆雅可比矩陣及其應用。 機械與汽車工程學院202

3、1年11月14日35.2 微分矩陣微分矩陣（derivative matrixes） 給出一個44的矩陣a （5.1）矩陣a的微分就是對矩陣a中的每一個元素對自變量x的微分，結果如下 （5.2）44434241343332312423222114131211aaaaaaaaaaaaaaaaadxxaxaxaxaxaxaxaxaxaxaxaxaxaxaxaxada44434241343332312423222114131211機械與汽車工程學院2021年11月14日45.3 微分平移和旋轉變換微分平移和旋轉變換 ( differential translation and rotation )

4、微分平移和旋轉變換可以是針對基坐標或參考坐標系，也可以是針微分平移和旋轉變換可以是針對基坐標或參考坐標系，也可以是針對某個指定的坐標系進行。例如對于一個變換矩陣對某個指定的坐標系進行。例如對于一個變換矩陣t，它對基坐標的微，它對基坐標的微分變換可表示為分變換可表示為 （5.3） 式中是在基坐標的式中是在基坐標的x，y，z軸向上分別平移軸向上分別平移dx，dy，dz；和繞基坐標；和繞基坐標的向量的向量k旋轉旋轉d角。由此可得到角。由此可得到 （5.4） 如果上述微分變換不是針對基坐標而是針對坐標系如果上述微分變換不是針對基坐標而是針對坐標系t，那么微分變換的，那么微分變換的結果可表示為結果可表示

5、為 （5.5） 此時，式中此時，式中 是在是在t坐標的坐標的x，y，z軸向上分別平移軸向上分別平移dx，dy，dz； 是繞是繞t坐標的向量坐標的向量k旋轉旋轉d角。由此可得到角。由此可得到 （5.6） tdkrotdzdydxtransdtt),(),(tidkrotdzdydxtransdt),(),(),(),(dkrotdzdydxtranstdtt),(),(idkrotdzdydxtranstdt),(dzdydxtrans),(dkrot機械與汽車工程學院2021年11月14日5 我們用符號我們用符號 來表示式（來表示式（5.4）和式（）和式（5.6）中的）中的 并將它稱為微分變換

6、算子并將它稱為微分變換算子（5.6） 這樣式（這樣式（5.4）和式（）和式（5.6）就可寫成如下形式）就可寫成如下形式 （5.7） 和和 （5.8） 式（式（5.7）中的微分變換算子）中的微分變換算子 是針對基坐標的，而式（是針對基坐標的，而式（5.8）中的微分變換）中的微分變換算子算子 則是針對則是針對t坐標的。坐標的。 在第二章我們給出了平移和一般性旋轉變換的齊次變換矩陣表達式，在第二章我們給出了平移和一般性旋轉變換的齊次變換矩陣表達式，平移變換矩陣是平移變換矩陣是 1 0 0 a 0 1 0 btrans( a, b, c ) = 0 0 1 c （5.9） 0 0 0 1),(),(i

7、dkrotdzdydxtransidkrotdzdydxtrans),(),(tdtttdtt機械與汽車工程學院2021年11月14日6當平移向量是微分向量ddxi+dyj+dzk時，微分平移矩陣為 1 0 0 dx 0 1 0 dytrans( d ) = 0 0 1 dz （5.10） 0 0 0 1一般性旋轉變換的變換矩陣是 kxkxvers + cos kykxvers - kzsin kzkxvers + kysin 0 kxkyvers + kzsin kykyvers + cos kzkyvers - kxsin 0rot( k,) = kxkzvers - kysin kykz

8、vers + kxsin kzkzvers + cos 0 (5.11) 0 0 0 1當進行微分旋轉變換時，旋轉角d極小，此時有如下關系dsinlim01coslim00lim0vers機械與汽車工程學院2021年11月14日7將上述關系代入式（5.11）可得 1 - kzd kyd 0 kzd 1 - kxd 0 rot( k, d) = - kyd kxd 1 0 (5.12) 0 0 0 1由式（5.6）可得 (5.13)100001000010000110000101011000100010001dkdkdkdkdkdkdddxyxzyzzyx0000000zxyyxzxyzddkd

9、kddkdkddkdk機械與汽車工程學院2021年11月14日85.4 微分旋轉微分旋轉 (differential rotations) 式（5.13）給出的微分變換算子 是基于微分旋轉角d的微分平移和旋轉變換表達式，下面討論繞坐標軸x、y、z旋轉x、y、z的微分變換。 第二章給出的繞坐標軸x、y、z旋轉的變換矩陣分別為 （5.14） （5.15） （5.16）10000cossin00sincos00001),(xrot10000cos0sin00100sin0cos),(yrot1000000000cossin00sincos),(zrot機械與汽車工程學院2021年11月14日9在微分

10、變換的情況下，sind，con1，上面三個式子變為 （5.17） （5.18） （5.19）由此可得到 （5.20）10000100100001),(xxxxrot10000100010001),(yyyyrot10000100001001),(zzzzrot1000010101),(),(),(xyxzyzzyxzrotyrotxrot機械與汽車工程學院2021年11月14日10 比較式（5.12）和式（5.20）可知，繞任意向量k旋轉d的微分旋轉與繞x、y、z軸分別旋轉 的結果相同，即 （5.21） 由此可得到繞坐標軸x、y、z旋轉x、y、z的微分變換算子為 （5.22） 微分變換算子中的

11、元素由微分平移向量d和微分旋轉向量的各個分量組成，即 （5.23） （5.24） 將上述二個向量組合構成一個微分運動矢量d （5.25） 這樣，我們就可根據式（5.25）給出的微分運動矢量d直接得到微分變換算子 ，或基于t坐標的微分運動矢量 的微分變換算子 。zy、xdkxxdkyydkzz0000000zxyyxzxyzdddkdjdiddzyxkjizyxtzyxzyxdddd),(tttddt機械與汽車工程學院2021年11月14日11【例5.1】已知坐標a的變換矩陣為 當用微分平移矢量d = 1i + 0j + 0.5k和微分旋轉矢量 0i + 0.1j + 0k對坐標a 進行變換時，

12、求出微分變換的結果da。解：首先，由式（5.22）求出微分變換算子 由式（5.7）可得 即10000010500110100aada00005 . 01 . 0000000101 . 001000001050011010000005 . 0001 . 0000011 . 000da微分變換結果如圖5.1所示。xyzzaya+dax圖5.1 坐標a的微分變換機械與汽車工程學院2021年11月14日125.5 坐標系之間的微分變換坐標系之間的微分變換 (transforming differential changes between coordinate frames) 上節討論了基于基坐標或某

13、個指定坐標的微分變換，本節繼續討論坐標系之間的微分變換，也就是已知微分變換算子 ，如何求出t坐標的微分變換算子 。由式（5.7）和（5.8）可知 （5.26） 則為 （5.27） 上式是一個重要的表達式，它描述了坐標系之間的微分變換關系。下面我們用微分平移矢量d和微分旋轉矢量 來推導 的表達式。 已知變換矩陣t為tttttt1t1000zzzzyyyyxxxxpaonpaonpaont機械與汽車工程學院2021年11月14日13我們用矢量的叉乘來得到式（5.27）等號右邊二項的乘積 （5.29）式中d和 分別是微分平移和微分旋轉矢量。用 左乘式（5.29）可得 （5.30）上式矩陣元素都具有如

14、下矢量三重積形式根據矢量三重積的性質有 （5.31）0000zzzzyyyyxxxxdpaondpaondpaont00001dpaaaoanadpoaooonodpnanonnntt1tcbaacbcabcba機械與汽車工程學院2021年11月14日14同時，三重積中只要有二個矢量是相同的，其結果為零。如 （5.32）根據上述性質，式（5.30）可寫成 （5.33）對于正交矢量有 （5.34）這樣，式（5.33）可重寫成 （5.35）0caa0000000adapaonaodopaoonndnpnaontaononanao0000000adapnoodopnandnpoat機械與汽車工程學院

15、2021年11月14日15 上式可進一步簡化為 （5.36） 比較式（5.35）和式（5.36）的矩陣元素可得 （5.37） （5.38） 在式（5.37）和式（5.38）中，n、o、a和p是微分坐標變換矩陣t的旋轉和平移矢量， 和 是對應坐標t的微分平移和旋轉矢量。0000000ztxtytytxtztxtytzttdddndnpdxtodopdytadapdztnxtoytaztdtt機械與汽車工程學院2021年11月14日16式（5.37）和式（5.38）也可用66的矩陣形式表示如下 （5.39）將上式寫成式（5.36）和式（5.37）的形式如下 （5.40） （5.41）式（5.40）

16、和式（5.41）是后續內容中要經常用到的重要結果。zyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxztytxtztytxtdddaaaooonnnapapapaaaopopopooonpnpnpnnnddd000000000dpndxtdpodytdpadzt nxt oyt azt機械與汽車工程學院2021年11月14日17【例5.2】給出與例5.1相同的坐標的變換矩陣、微分平移矢量和微分旋轉矢量如下： d = 1i + 0j + 0.5k 0i + 0.1j + 0k 試求出坐標a上的等效微分變換da。解：由坐標變換矩陣a可得到相應的旋轉與平移矢量 由此可求出 根據式（5

17、.40）和式（5.41）得到10000010500110100akjin010kjio100kjia001kjip0510kjip100kjidp5 . 001kjida15 . 00kjia001 . 0機械與汽車工程學院2021年11月14日18用上述結果來驗證坐標a上的等效微分變換da，由式（5.8）有由已求出的 、 和式（5.36）可得到則上述結果與例5.1相同。aadadaa0000101 . 005 . 01 . 0000000a00005 . 01 . 0000000101 . 000000101 . 005 . 01 . 000000010000010500110100da機械

18、與汽車工程學院2021年11月14日195. 6 機械手的微分變換方程機械手的微分變換方程雅可比方程雅可比方程 （the manipulator jacobian） 在第三章我們介紹過，機械手的運動學方程由它的末端相對于基坐標的齊次變換矩陣t6表示，即t6 = a1 a2 a3 a4 a5 a6 （5.42） 其中每一個關節變換矩陣ai描述了該關節坐標相對于前一個關節坐標的變換關系，關節變量用qi表示，如果是旋轉關節，關節變量是i，它是繞前一個關節坐標z軸的旋轉角度；如果是滑動關節，關節變量是di，它是沿前一個關節坐標z軸滑動的距離。同樣，當我們討論機械手的微分變換方程時，首先定義微分關節變量

19、為dqi，如果是旋轉關節，則為di，如果是滑動關節，則為ddi。機械與汽車工程學院2021年11月14日20 機械手第i個關節的微分變換引起第6個連桿末端（即機械手末端）的微分變換dt6可由下式表示： （5.43） 則 （5.44） 由式（5.27）可得到機械手末端的微分變換算子 （5.45） 其中 （5.46） 如果關節i是旋轉關節，則di = 0，式（5.40）和式（5.41）變為 （5.47）（5.48）iiiiiiitdqaaaaaadqtdt611121666ititqt6666111616aaaaaaiiiiiit6161aaattiiipndxitpodyitpadzit nxi

20、t oyit azit機械與汽車工程學院2021年11月14日21 當 ，為單位微分旋轉矢量時，式（5.47）和（5.48）可進一步簡化為 （5.49） （5.50） 如果關節i是棱形滑動關節，則i0，di = 0i + 0j + 1k，式（5.40）和式（5.41）變為 （5.51） （5.52） 機械手末端坐標t6的微分變換是所有6個關節微分變量的函數，可用66的矩陣表示，矩陣元素由6個關節的微分平移和微分旋轉矢量構成，該矩陣稱為雅可比矩陣。它的每一列元素為對應關節的微分平移和微分旋轉矢量。應用雅可比矩陣的機械手微分變換方程雅可比方程如下： （5.53）kjii100kpapajpopoi

21、pnpndxyyxxyyxxyyxit6kajoinzzzit6kajoindzzzit6kjiit0006654321654321654321654321654321654321654321666666666666666666666666666666666666666666dqdqdqdqdqdqdddddddddddddddddddddztztztztztztytytytytytytxtxtxtxtxtxtztztztztztztytytytytytytxtxtxtxtxtxtztytxtztytxt機械與汽車工程學院2021年11月14日225.7 雅可比逆矩陣雅可比逆矩陣（the in

22、verse jacobian） 當微分變換是由直角坐標空間向關節坐標空間進行時，由式（5.53）可得到 （5.72） 上式等號右邊矩陣是雅可比逆矩陣。顯然，用符號運算來得到雅可比逆陣是很困難的，因為微分變換要進行大量算術運算，同時當機械手出現退化時，其結果會出錯。 為此，我們采用第四章介紹的根據t6的值計算關節坐標值的方法和步驟來計算微分關節坐標值。將關節坐標的微分變換表示為dt6中各元素的函數，然后求出各關節的微分變換值。該方法相對比較簡單，而且在機械手出現退化時，將相應關節的微分變換值設置為零，這就不會影響后續關節的計算結果。在后面的討論中，我們假設機械手的符號解存在，而且關節變量的正弦和

23、余弦值已知。ztytxtztytxtztztztztztztytytytytytytxtxtxtxtxtxtztztztztztztytytytytytytxtxtxtxtxtxtddddddddddddddddddddddqdqdqdqdqdq6666666666666666666666666666666666666666661654321654321654321654321654321654321654321機械與汽車工程學院2021年11月14日23 為了計算dt6,我們首先根據式（5.37）和式（5.38）對t6進行微分變換得到微分平移矢量 和微分旋轉矢量 ，然后根據式（5.22）求出

24、 ，最后根據式（5.8）得到dt6。 下面通過對第四章介紹的斯坦福機械手逆運動學解的微分變換來說明上述方法的具體步驟。 由第四章式（4.15）有s1 pxc1 py = d2 （5.73） 對式（5.73）求導可直接得到第一個關節變量1的微分 （5.74） 對于正切函數 （5.75） 其微分公式為 （5.76）dt66t6tyxxypspcdpsdpcd11111cossintannn22)cos()sin()cos(sin)sin(cosnnndnndnd機械與汽車工程學院2021年11月14日24由第四章式（4.24）和式（4.25）有 （5.77） （5.78）對式（5.77）和式（5.

25、78）求微分得到 （5.79） （5.80）由公式（5.76）可得到第二個關節變量2的微分 （5.81）將式（5.77）代入第四章的式（4.31）有 （5.82）對式（5.82）進行微分可直接得到第三個滑動關節變量d3的微分 （5.83）yxpspcdsns11322zpdcnc322yyxxdpspdsdpcpdcnsd11112)(zdpncd)(232222)(ddpsnsdcdzzpcnssd2223zzdpcpdcnsdsnsdsdd2222223)(機械與汽車工程學院2021年11月14日25 由第四章式（4.38）和式（4.39）有ns4 =s1axc1ay （5.84） nc4

26、 = c2d41s2az （5.85）其中 d41 = c1axs1ay （5.86）對式（5.84）式（5.86）進行微分得到 （5.87） （5.88） （5.89）由式（5.76）可得到第四個關節變量4的微分d4。在計算第五個關節變量微分時，為了簡化計算，我們可將式（5.76）簡化為 （5.90） 由第四章式（4.42）和式（4.43）有s5 = c4nc4+ ns4 （5.91）c5 = s2d41 + c2az （5.92）yyxxdasadsdacadcdd111141yyxxdacadcdasadsnsd11114)(zzdasadsddcddcncd224124123)()(c

27、ossin)(sincosddd機械與汽車工程學院2021年11月14日26對式（5.91）和式（5.92）進行微分得到 （5.93） （5.94）由式（5.90）可得到第五個關節變量5的微分d5。 最后，我們由第四章式（4.49）和式（4.50）有s6 =c5n61s5n612 （5.95）c6 =s4n611 + c4n6112 （5.96）其中n6111 = c1ox + s1oy （5.97）dn6111 = dc1ox + c1dox + ds1oy + s1doy （5.98）n6112 =s1ox + c1oy （5.99）dn6112 =ds1oxs1dox + dc1oy + c1doy （5.100）)()(444444445nsdsnsdsncdcncdcdszzdacadcddsddsdc224124124機械與汽車工程學院2021年11月14日27n611 = c2n6111s2oz （5.101）dn611 = dc2n6111 + c2dn6111ds