機器人雅可比矩陣

機器人雅可比矩陣第一頁，共二十九頁，編輯于2023年，星期日5.1引言（Introduction）微分變換在機器人視覺、動力學和機器人控制（如力控、剛度控制、阻抗控制、順應控制等）中十分重要。例如當攝像機或其它傳感裝置檢測到機器人末端執(zhí)行器的位置和方向的微小變化時，需要將該微小變化從攝像機或其它傳感裝置坐標轉換到基坐標或參考坐標系。在機器人剛度控制中，需要獲得在控制坐標系中力與位置的微分變換。又如將直角坐標的微分變換轉化為關節(jié)坐標的微分變換，還有在下一章介紹的機器人動力學問題時，也會用到微分變換。本章將介紹微分變換的基本原理和方法，包括微分平移、微分旋轉、坐標系之間的微分變換、雅可比矩陣和逆雅可比矩陣及其應用。2023/6/192第二頁，共二十九頁，編輯于2023年，星期日5.2微分矩陣（DerivativeMatrixes）

給出一個4×4的矩陣A（5.1）矩陣A的微分就是對矩陣A中的每一個元素對自變量x的微分，結果如下

（5.2）2023/6/193第三頁，共二十九頁，編輯于2023年，星期日5.3微分平移和旋轉變換(DifferentialTranslationandRotation)

微分平移和旋轉變換可以是針對基坐標或參考坐標系，也可以是針對某個指定的坐標系進行。例如對于一個變換矩陣T，它對基坐標的微分變換可表示為（5.3）式中是在基坐標的x，y，z軸向上分別平移dx，dy，dz；和繞基坐標的向量k旋轉dθ角。由此可得到（5.4）如果上述微分變換不是針對基坐標而是針對坐標系T，那么微分變換的結果可表示為（5.5）此時，式中是在T坐標的x，y，z軸向上分別平移dx，dy，dz；是繞T坐標的向量k旋轉dθ角。由此可得到（5.6）2023/6/194第四頁，共二十九頁，編輯于2023年，星期日我們用符號來表示式（5.4）和式（5.6）中的并將它稱為微分變換算子（5.6）這樣式（5.4）和式（5.6）就可寫成如下形式（5.7）和（5.8）式（5.7）中的微分變換算子是針對基坐標的，而式（5.8）中的微分變換算子則是針對T坐標的。在第二章我們給出了平移和一般性旋轉變換的齊次變換矩陣表達式，平移變換矩陣是100a010bTrans(a,b,c)=001c（5.9）00012023/6/195第五頁，共二十九頁，編輯于2023年，星期日當平移向量是微分向量d＝dxi+dyj+dzk時，微分平移矩陣為100dx010dyTrans(d)=001dz（5.10）0001一般性旋轉變換的變換矩陣是kxkxversθ+cosθkykxversθ-kzsinθkzkxversθ+kysinθ0kxkyversθ+kzsinθkykyversθ+cosθkzkyversθ-kxsinθ0Rot(k,θ)=kxkzversθ-kysinθkykzversθ+kxsinθkzkzversθ+cosθ0(5.11)0

0

01當進行微分旋轉變換時，旋轉角dθ極小，此時有如下關系2023/6/196第六頁，共二十九頁，編輯于2023年，星期日將上述關系代入式（5.11）可得1-kzdθkydθ0kzdθ1-kxdθ0Rot(k,dθ)=-kydθkxdθ10(5.12)0

0

01由式（5.6）可得

(5.13)2023/6/197第七頁，共二十九頁，編輯于2023年，星期日5.4微分旋轉(DifferentialRotations)式（5.13）給出的微分變換算子是基于微分旋轉角dθ的微分平移和旋轉變換表達式，下面討論繞坐標軸x、y、z旋轉δx、δy、δz的微分變換。第二章給出的繞坐標軸x、y、z旋轉的變換矩陣分別為（5.14）（5.15）（5.16）2023/6/198第八頁，共二十九頁，編輯于2023年，星期日在微分變換的情況下，sinθ→dθ，conθ→1，上面三個式子變?yōu)椋?.17）

（5.18）

（5.19）由此可得到（5.20）2023/6/199第九頁，共二十九頁，編輯于2023年，星期日比較式（5.12）和式（5.20）可知，繞任意向量k旋轉dθ的微分旋轉與繞x、y、z軸分別旋轉的結果相同，即（5.21）由此可得到繞坐標軸x、y、z旋轉δx、δy、δz的微分變換算子為（5.22）

微分變換算子中的元素由微分平移向量d和微分旋轉向量δ的各個分量組成，即（5.23）（5.24）將上述二個向量組合構成一個微分運動矢量D（5.25）這樣，我們就可根據(jù)式（5.25）給出的微分運動矢量D直接得到微分變換算子，或基于T坐標的微分運動矢量的微分變換算子。2023/6/1910第十頁，共二十九頁，編輯于2023年，星期日【例5.1】已知坐標A的變換矩陣為當用微分平移矢量d=1i+0j+0.5k和微分旋轉矢量δ＝0i+0.1j+0k對坐標A進行變換時，求出微分變換的結果dA。解：首先，由式（5.22）求出微分變換算子由式（5.7）可得即微分變換結果如圖5.1所示。xyzzAyA+dAx圖5.1坐標A的微分變換2023/6/1911第十一頁，共二十九頁，編輯于2023年，星期日5.5坐標系之間的微分變換

(TransformingDifferentialChangesbetweenCoordinateFrames)上節(jié)討論了基于基坐標或某個指定坐標的微分變換，本節(jié)繼續(xù)討論坐標系之間的微分變換，也就是已知微分變換算子，如何求出T坐標的微分變換算子。由式（5.7）和（5.8）可知（5.26）則為（5.27）上式是一個重要的表達式，它描述了坐標系之間的微分變換關系。下面我們用微分平移矢量d和微分旋轉矢量來推導的表達式。已知變換矩陣T為2023/6/1912第十二頁，共二十九頁，編輯于2023年，星期日我們用矢量的叉乘來得到式（5.27）等號右邊二項的乘積

（5.29）式中d和分別是微分平移和微分旋轉矢量。用左乘式（5.29）可得

（5.30）上式矩陣元素都具有如下矢量三重積形式根據(jù)矢量三重積的性質有（5.31）2023/6/1913第十三頁，共二十九頁，編輯于2023年，星期日同時，三重積中只要有二個矢量是相同的，其結果為零。如（5.32）根據(jù)上述性質，式（5.30）可寫成（5.33）對于正交矢量有（5.34）這樣，式（5.33）可重寫成

（5.35）2023/6/1914第十四頁，共二十九頁，編輯于2023年，星期日上式可進一步簡化為（5.36）比較式（5.35）和式（5.36）的矩陣元素可得

（5.37）

（5.38）在式（5.37）和式（5.38）中，n、o、a和p是微分坐標變換矩陣T的旋轉和平移矢量，和是對應坐標T的微分平移和旋轉矢量。2023/6/1915第十五頁，共二十九頁，編輯于2023年，星期日式（5.37）和式（5.38）也可用6×6的矩陣形式表示如下（5.39）將上式寫成式（5.36）和式（5.37）的形式如下

（5.40）

（5.41）式（5.40）和式（5.41）是后續(xù)內容中要經(jīng)常用到的重要結果。2023/6/1916第十六頁，共二十九頁，編輯于2023年，星期日【例5.2】給出與例5.1相同的坐標的變換矩陣、微分平移矢量和微分旋轉矢量如下：

d=1i+0j+0.5kδ＝0i+0.1j+0k試求出坐標A上的等效微分變換dA。解：由坐標變換矩陣A可得到相應的旋轉與平移矢量由此可求出根據(jù)式（5.40）和式（5.41）得到2023/6/1917第十七頁，共二十九頁，編輯于2023年，星期日用上述結果來驗證坐標A上的等效微分變換dA，由式（5.8）有由已求出的、和式（5.36）可得到則上述結果與例5.1相同。2023/6/1918第十八頁，共二十九頁，編輯于2023年，星期日5.6機械手的微分變換方程——雅可比方程

（TheManipulatorJacobian）在第三章我們介紹過，機械手的運動學方程由它的末端相對于基坐標的齊次變換矩陣T6表示，即T6=A1A2A3A4A5A6（5.42）其中每一個關節(jié)變換矩陣Ai描述了該關節(jié)坐標相對于前一個關節(jié)坐標的變換關系，關節(jié)變量用qi表示，如果是旋轉關節(jié)，關節(jié)變量是θi，它是繞前一個關節(jié)坐標z軸的旋轉角度；如果是滑動關節(jié)，關節(jié)變量是di，它是沿前一個關節(jié)坐標z軸滑動的距離。同樣，當我們討論機械手的微分變換方程時，首先定義微分關節(jié)變量為dqi，如果是旋轉關節(jié)，則為dθi，如果是滑動關節(jié)，則為ddi。2023/6/1919第十九頁，共二十九頁，編輯于2023年，星期日機械手第i個關節(jié)的微分變換引起第6個連桿末端（即機械手末端）的微分變換dT6可由下式表示：（5.43）則（5.44）由式（5.27）可得到機械手末端的微分變換算子（5.45）其中（5.46）如果關節(jié)i是旋轉關節(jié)，則di

=0，式（5.40）和式（5.41）變?yōu)?/p>

（5.47）（5.48）2023/6/1920第二十頁，共二十九頁，編輯于2023年，星期日當，為單位微分旋轉矢量時，式（5.47）和（5.48）可進一步簡化為（5.49）（5.50）如果關節(jié)i是棱形滑動關節(jié)，則δi＝0，di=0i+0j+1k，式（5.40）和式（5.41）變?yōu)椋?.51）（5.52）機械手末端坐標T6的微分變換是所有6個關節(jié)微分變量的函數(shù)，可用6×6的矩陣表示，矩陣元素由6個關節(jié)的微分平移和微分旋轉矢量構成，該矩陣稱為雅可比矩陣。它的每一列元素為對應關節(jié)的微分平移和微分旋轉矢量。應用雅可比矩陣的機械手微分變換方程——雅可比方程如下：（5.53）2023/6/1921第二十一頁，共二十九頁，編輯于2023年，星期日5.7雅可比逆矩陣（TheInverseJacobian）當微分變換是由直角坐標空間向關節(jié)坐標空間進行時，由式（5.53）可得到

（5.72）

上式等號右邊矩陣是雅可比逆矩陣。顯然，用符號運算來得到雅可比逆陣是很困難的，因為微分變換要進行大量算術運算，同時當機械手出現(xiàn)退化時，其結果會出錯。為此，我們采用第四章介紹的根據(jù)T6的值計算關節(jié)坐標值的方法和步驟來計算微分關節(jié)坐標值。將關節(jié)坐標的微分變換表示為dT6中各元素的函數(shù)，然后求出各關節(jié)的微分變換值。該方法相對比較簡單，而且在機械手出現(xiàn)退化時，將相應關節(jié)的微分變換值設置為零，這就不會影響后續(xù)關節(jié)的計算結果。在后面的討論中，我們假設機械手的符號解存在，而且關節(jié)變量的正弦和余弦值已知。2023/6/1922第二十二頁，共二十九頁，編輯于2023年，星期日為了計算dT6,我們首先根據(jù)式（5.37）和式（5.38）對T6進行微分變換得到微分平移矢量和微分旋轉矢量，然后根據(jù)式（5.22）求出，最后根據(jù)式（5.8）得到dT6。下面通過對第四章介紹的斯坦福機械手逆運動學解的微分變換來說明上述方法的具體步驟。由第四章式（4.15）有－S1px＋C1

py=d2（5.73）對式（5.73）求導可直接得到第一個關節(jié)變量θ1的微分（5.74）對于正切函數(shù)（5.75）其微分公式為（5.76）2023/6/1923第二十三頁，共二十九頁，編輯于2023年，星期日由第四章式（4.24）和式（4.25）有（5.77）（5.78）對式（5.77）和式（5.78）求微分得到（5.79）（5.80）由公式（5.76）可得到第二個關節(jié)變量θ2的微分（5.81）將式（5.77）代入第四章的式（4.31）有（5.82）對式（5.82）進行微分可直接得到第三個滑動關節(jié)變量d3的微分（5.83）2023/6/1924第二十四頁，共二十九頁，編輯于2023年，星期日由第四章式（4.38）和式（4.39）有NS4=－S1ax＋C1ay（5.84）

NC4=C2D41－S2az（5.85）其中D41=C1ax＋S1ay（5.86）對式（5.84）~式（5.86）進行微分得到（5.87）（5.88）（5.89）由式（5.76）可得到第四個關節(jié)變量θ4的微分dθ4。在計算第五個關節(jié)變量微分時，為了簡化計算，我們可將式（5.76）簡化為（5.90）由第四章式（4.42）和式（4.43）有S5=C4NC4+NS4（5.91）C5=S2D41+C2az（5.92）2023/6/1925第二十五頁，共二十九頁，編輯于2023年，星期日對式（5.91）和式（5.92）進行微分得到（5.93）（5.94）由式（5.90）可得到第五個關節(jié)變量θ5的微分dθ5。最后，我們由第四章式（4.49）和式（4.50）有S6=－C5N61－S5N612（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=－dS1ox－S1dox+dC1oy+C1doy（5.100）2023/6/1926第二十六頁，共二十九頁，編輯于2023年，星期日N611=C2N6111－S2oz（5.101）dN611=dC2N6111+C2dN6111－dS2oz－S2doz（5.102）N612=－S2N6111－C2oz（5.103）dN612=－dS2N6111+S2dN6111－dC2oz－C2doz（5.104）N61=－C4N611+S4N6112（5.105）dN61=－dC4N611+C4dN611+dS4N6112－S4dN6112