機(jī)器人雅可比矩陣

1、機(jī)器人雅可比矩陣第1頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四5.1 引言（Introduction） 微分變換在機(jī)器人視覺、動(dòng)力學(xué)和機(jī)器人控制（如力控、剛度控制、阻抗控制、順應(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)系之間的微分變

2、換、雅可比矩陣和逆雅可比矩陣及其應(yīng)用。 2022/9/202第2頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四5.2 微分矩陣（Derivative Matrixes） 給出一個(gè)44的矩陣A （5.1）矩陣A的微分就是對(duì)矩陣A中的每一個(gè)元素對(duì)自變量x的微分，結(jié)果如下 （5.2）2022/9/203第3頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四5.3 微分平移和旋轉(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)的微

3、分變換可表示為 （5.3） 式中是在基坐標(biāo)的x，y，z軸向上分別平移dx，dy，dz；和繞基坐標(biāo)的向量k旋轉(zhuǎn)d角。由此可得到 （5.4） 如果上述微分變換不是針對(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） 2022/9/204第4頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四 我們用符號(hào) 來(lái)表示式（5.4）和式（5.6）中的 并將它稱為微分變換算子（5.6） 這樣式（5.4）和式（5.6）就可寫成如下形式 （5.7） 和 （5.8） 式（5.7）

4、中的微分變換算子 是針對(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 12022/9/205第5頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四當(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一般性旋轉(zhuǎn)變換的變換矩陣是 kxkxvers + cos kykxvers -

5、 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)系2022/9/206第6頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四將上述關(guān)系代入式（5.11）可得 1 - kzd kyd 0 kzd 1 - kxd 0 Rot( k, d) = - kyd kxd 1 0 (5.12) 0 0

6、0 1由式（5.6）可得 (5.13)2022/9/207第7頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四5.4 微分旋轉(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.16）2022/9/208第8頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四在微分變換的情況下，sind，con1，上面三個(gè)式子變?yōu)?（5.17） （5.18） （5.19）由此可得到

7、 （5.20）2022/9/209第9頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四 比較式（5.12）和式（5.20）可知，繞任意向量k旋轉(zhuǎn)d的微分旋轉(zhuǎn)與繞x、y、z軸分別旋轉(zhuǎn) 的結(jié)果相同，即 （5.21） 由此可得到繞坐標(biāo)軸x、y、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)矢量 的微分變換算子 。2022/9/2010第10頁(yè)

8、，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四【例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í)，求出微分變換的結(jié)果dA。解：首先，由式（5.22）求出微分變換算子 由式（5.7）可得 即微分變換結(jié)果如圖5.1所示。xyzzAyA+dAx圖5.1 坐標(biāo)A的微分變換2022/9/2011第11頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四5.5 坐標(biāo)系之間的微分變換 (Transforming Differential Changes between Coordinate Fr

9、ames) 上節(jié)討論了基于基坐標(biāo)或某個(gè)指定坐標(biāo)的微分變換，本節(jié)繼續(xù)討論坐標(biāo)系之間的微分變換，也就是已知微分變換算子 ，如何求出T坐標(biāo)的微分變換算子 。由式（5.7）和（5.8）可知 （5.26） 則為 （5.27） 上式是一個(gè)重要的表達(dá)式，它描述了坐標(biāo)系之間的微分變換關(guān)系。下面我們用微分平移矢量d和微分旋轉(zhuǎn)矢量 來(lái)推導(dǎo) 的表達(dá)式。 已知變換矩陣T為2022/9/2012第12頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四我們用矢量的叉乘來(lái)得到式（5.27）等號(hào)右邊二項(xiàng)的乘積 （5.29）式中d和 分別是微分平移和微分旋轉(zhuǎn)矢量。用 左乘式（5.29）可得 （5.30）上式矩陣元素都具有

10、如下矢量三重積形式根據(jù)矢量三重積的性質(zhì)有 （5.31）2022/9/2013第13頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四同時(shí)，三重積中只要有二個(gè)矢量是相同的，其結(jié)果為零。如 （5.32）根據(jù)上述性質(zhì)，式（5.30）可寫成 （5.33）對(duì)于正交矢量有 （5.34）這樣，式（5.33）可重寫成 （5.35）2022/9/2014第14頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四 上式可進(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)和平

11、移矢量， 和 是對(duì)應(yīng)坐標(biāo)T的微分平移和旋轉(zhuǎn)矢量。2022/9/2015第15頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四式（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é)果。2022/9/2016第16頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四【例5.2】給出與例5.1相同的坐標(biāo)的變換矩陣、微分平移矢量和微分旋轉(zhuǎn)矢量如下： d = 1i + 0j + 0.5k 0i + 0.1j + 0k 試求出坐標(biāo)A上的等效

12、微分變換dA。解：由坐標(biāo)變換矩陣A可得到相應(yīng)的旋轉(zhuǎn)與平移矢量 由此可求出 根據(jù)式（5.40）和式（5.41）得到2022/9/2017第17頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四用上述結(jié)果來(lái)驗(yàn)證坐標(biāo)A上的等效微分變換dA，由式（5.8）有由已求出的 、 和式（5.36）可得到則上述結(jié)果與例5.1相同。2022/9/2018第18頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四5. 6 機(jī)械手的微分變換方程雅可比方程 （The Manipulator Jacobian） 在第三章我們介紹過，機(jī)械手的運(yùn)動(dòng)學(xué)方程由它的末端相對(duì)于基坐標(biāo)的齊次變換矩陣T6表示，即T6 = A1

13、 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表示，如果是旋轉(zhuǎn)關(guān)節(jié)，關(guān)節(jié)變量是i，它是繞前一個(gè)關(guān)節(jié)坐標(biāo)z軸的旋轉(zhuǎn)角度；如果是滑動(dòng)關(guān)節(jié)，關(guān)節(jié)變量是di，它是沿前一個(gè)關(guān)節(jié)坐標(biāo)z軸滑動(dòng)的距離。同樣，當(dāng)我們討論機(jī)械手的微分變換方程時(shí)，首先定義微分關(guān)節(jié)變量為dqi，如果是旋轉(zhuǎn)關(guān)節(jié)，則為di，如果是滑動(dòng)關(guān)節(jié)，則為ddi。2022/9/2019第19頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四 機(jī)械手第i個(gè)關(guān)節(jié)的微分變換引起第6個(gè)連桿末端（即機(jī)械手末端）的微分變換dT6可由下式表示： （5.43） 則 （5.

14、44） 由式（5.27）可得到機(jī)械手末端的微分變換算子 （5.45） 其中 （5.46） 如果關(guān)節(jié)i是旋轉(zhuǎn)關(guān)節(jié)，則di = 0，式（5.40）和式（5.41）變?yōu)?（5.47）（5.48）2022/9/2020第20頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四 當(dāng) ，為單位微分旋轉(zhuǎn)矢量時(shí)，式（5.47）和（5.48）可進(jìn)一步簡(jiǎn)化為 （5.49） （5.50） 如果關(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è)

15、關(guān)節(jié)的微分平移和微分旋轉(zhuǎn)矢量構(gòu)成，該矩陣稱為雅可比矩陣。它的每一列元素為對(duì)應(yīng)關(guān)節(jié)的微分平移和微分旋轉(zhuǎn)矢量。應(yīng)用雅可比矩陣的機(jī)械手微分變換方程雅可比方程如下： （5.53）2022/9/2021第21頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四5.7 雅可比逆矩陣（The Inverse Jacobian） 當(dāng)微分變換是由直角坐標(biāo)空間向關(guān)節(jié)坐標(biāo)空間進(jìn)行時(shí)，由式（5.53）可得到 （5.72） 上式等號(hào)右邊矩陣是雅可比逆矩陣。顯然，用符號(hào)運(yùn)算來(lái)得到雅可比逆陣是很困難的，因?yàn)槲⒎肿儞Q要進(jìn)行大量算術(shù)運(yùn)算，同時(shí)當(dāng)機(jī)械手出現(xiàn)退化時(shí)，其結(jié)果會(huì)出錯(cuò)。 為此，我們采用第四章介紹的根據(jù)T6的值計(jì)算關(guān)節(jié)

16、坐標(biāo)值的方法和步驟來(lái)計(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é)變量的正弦和余弦值已知。2022/9/2022第22頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四 為了計(jì)算dT6,我們首先根據(jù)式（5.37）和式（5.38）對(duì)T6進(jìn)行微分變換得到微分平移矢量 和微分旋轉(zhuǎn)矢量 ，然后根據(jù)式（5.22）求出 ，最后根據(jù)式（5.8）得到dT6。 下面通過對(duì)第四章介紹的斯坦福機(jī)械手逆運(yùn)

17、動(dòng)學(xué)解的微分變換來(lái)說(shuō)明上述方法的具體步驟。 由第四章式（4.15）有S1 pxC1 py = d2 （5.73） 對(duì)式（5.73）求導(dǎo)可直接得到第一個(gè)關(guān)節(jié)變量1的微分 （5.74） 對(duì)于正切函數(shù) （5.75） 其微分公式為 （5.76）2022/9/2023第23頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四由第四章式（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)行微分

18、可直接得到第三個(gè)滑動(dòng)關(guān)節(jié)變量d3的微分 （5.83）2022/9/2024第24頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四 由第四章式（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+ NS

19、4 （5.91）C5 = S2D41 + C2az （5.92）2022/9/2025第25頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四對(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.95）C6 =S4N611 + C4N6112 （5.96）其中N6111 = C1ox + S1oy （5.97）dN6111 = dC1ox + C1dox + dS1oy + S1doy （5.98）N6112 =S1ox + C1o

20、y （5.99）dN6112 =dS1oxS1dox + dC1oy + C1doy （5.100）2022/9/2026第26頁(yè)，共29頁(yè)，2022年，5月20日，2點(diǎn)20分，星期四N611 = C2N6111S2oz （5.101）dN611 = dC2N6111 + C2dN6111dS2ozS2doz （5.102）N612 =S2N6111C2oz （5.103）dN612 =dS2N6111 + S2dN6111dC2ozC2doz （5.104）N61 =C4N611 + S4N6112 （5.105）dN61 =dC4N611 + C4dN611 + dS4N6112S4dN6112 （5.106）對(duì)式（5.95）和式（5.96）進(jìn)行微分得到