1、第三章 運動學方程 Chapter Kinematic Equations,3.1 引言 3.8 T6的說明 3.2 姿態(tài)描述 3.9 各種A矩陣的說明 3.3 歐拉角 3.10 根據(jù)A矩陣來確定T6 3.4 搖擺、俯仰和偏轉(zhuǎn) 3.11 斯坦福機械手的運動方程 3.5 位置的確定 3.12 肘機械手的運動方程 3.6 圓柱坐標 3.13 小結 3.7 球坐標,3.1 引言 ( Introduction ),本章，我們采用齊次變換來描述在各種坐標系中機械手的位置與方向。首先介紹各種正交坐標系的齊次變換。然后介紹在非正交關節(jié)坐標系中描述機械手末端的齊次變換。注意，對任何數(shù)目關節(jié)的各種機械手均可以這

2、樣進行。 描述一個連桿與下一個連桿之間關系的齊次變換稱A矩陣。A矩陣是描述連桿坐標系之間的相對平移和旋轉(zhuǎn)的齊次變換。 連續(xù)變換的若干A矩陣的積稱為T矩陣，對于一個六連桿（六自由度）機械手有 T6 = A1 A2 A3 A4 A5 A6 （3.1） 六連桿的機械手有六個自由度，其中三個自由度用來確定位置，三個自由度用來確定方向。表示機械手在基坐標中的位置與方向。則變換矩陣有下列元素 nx ox ax px ny oy ay py T6 = nz oz az pz （3.2） 0 0 0 1,如圖3.1所示，機器人的末端執(zhí)行器（手爪）的姿態(tài)（方向）由 n、o、a 三個旋轉(zhuǎn)矢量描述，其坐標位置由平移

3、矢量 p 描述，這就構成了式（3.2）中的變換矩陣 T。 由于 n、o、a 三個旋轉(zhuǎn)矢量是正交矢量，所以有 n = oa,圖3.1 末端執(zhí)行器的描述,3.2 姿態(tài)描述 ( Specification of Orientation ),對式（3.2）中16個元素一一賦值就可確定T6。假定機械手可以到達要求的位置，而單位旋轉(zhuǎn)矢量o和a正交，即 oo 1 （3.3） aa 1 （3.4） oa 0 （3.5） a形成單位向量 a a （3.6） | a | 構成與o和a正交的n n oa （3.7） 在o和a形成的平面上旋轉(zhuǎn)o，使得o與n和a正交 o an （3.8） 單位向量o是 o o （3.9

4、） | o | 根據(jù)第二章給出的一般性的旋轉(zhuǎn)矩陣ot (k ,)，它把機械手末端的姿態(tài)規(guī)定為繞k軸旋轉(zhuǎn)角。,3.歐拉角 ( Euler Angles ),姿態(tài)變更常用繞x,y或z軸的一系列旋轉(zhuǎn)來確定。歐拉角描述方法是：先繞z軸旋轉(zhuǎn)，然后繞新的y(即y/)軸旋轉(zhuǎn)，最后繞更新的z(z/)軸旋轉(zhuǎn)（見圖3.2）歐拉變換Euler(,)可以通過連乘三個旋轉(zhuǎn)矩陣來求得 Euler(,) ot(z,)ot(y,)ot(z,) （3.10） 在一系列旋轉(zhuǎn)中，旋轉(zhuǎn)的次序是重要的。應注意，旋轉(zhuǎn)序列如果按相反的順序進行，則是繞基坐標中的軸旋轉(zhuǎn)：繞z軸旋轉(zhuǎn) ，接著繞y軸旋轉(zhuǎn)，最后再一次繞z軸旋轉(zhuǎn) ，結果如圖3.3所示

5、，它與圖3.2是一致的。,3.4 搖擺、俯仰和偏轉(zhuǎn) ( Roll, Pitch and Yaw ),搖擺、俯仰和偏轉(zhuǎn)為另一種旋轉(zhuǎn)。如圖3.4所示，就像水中航行的一條小船一樣，繞著它前進的方向（z軸）旋轉(zhuǎn) 稱為搖擺，繞著它的橫向中軸（y軸）旋轉(zhuǎn) 稱為俯仰，繞著它甲板的垂直向上的方向（x軸）旋轉(zhuǎn) 稱為偏轉(zhuǎn)。借助于這種旋轉(zhuǎn)來描述機械手的末端執(zhí)行器如圖3.5所示。規(guī)定旋轉(zhuǎn)的次序為 RPY(,)ot(z,)ot(y,)ot(x,) （3.12） 即，繞x軸旋轉(zhuǎn)，接著繞y軸旋轉(zhuǎn)，最后繞z軸旋轉(zhuǎn) ，這個變換如下 cos 0 sin 0 1 0 0 0 0 1 0 0 0 cos sin 0 RPY(,) =

6、 ot(z,) sin 0 cos 0 0 sin cos 0 （3.13） 0 0 0 1 0 0 0 1 cos sin 0 0 cos sinsin sincos 0 sin cos 0 0 0 cos sin 0 RPY(,) = 0 0 1 0 -sin cossin coscos 0 （3.14） 0 0 0 1 0 0 0 1,圖3.4 搖擺、俯仰和偏 轉(zhuǎn)角,圖3.5 機械手的末端執(zhí)行器 的搖擺、俯仰和偏 轉(zhuǎn),RPY(，) = cos cos cos sinsin sin cos cos sincos + sin sin 0 sin cos sin sinsin + cos co

7、s sin sincoscos sin 0 -sin cossin coscos 0 0 0 0 1,3.5 位置的確定 ( Specification of Position ),一旦方向被確定之后，用一個相應的p向量的位移變換可得到機器人末端執(zhí)行器在基坐標中的位置： 1 0 0 px 0 1 0 py T6 = 0 0 1 pz （3.16） 0 0 0 1,旋轉(zhuǎn) 變換 矩陣,3.6 圓柱坐標 ( Cylindrical Coordinates ),如圖3.6所示，在圓柱坐標中確定機械手的位置是沿x軸 平移r，接著繞z軸旋轉(zhuǎn)，最后沿著z軸平移z。 Cyl(z, ,r) = Trans(0,

8、0,z)Rot(z, ) Trans(r,0,0) cos -sin 0 0 1 0 0 r sin cos 0 0 0 1 0 0 Cyl(z, ,r) = Trans(0,0,z) 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 （3.17） 1 0 0 0 cos -sin 0 rcos 0 1 0 0 sin cos 0 rsin Cyl(z, ,r) = 0 0 1 z 0 0 1 0 0 0 0 1 0 0 0 1 （3.18）,注意：圓柱坐標只能繞 z 軸旋轉(zhuǎn),cos -sin 0 rcos sin cos 0 rsin Cyl(z,r) = 0 0 1 z （3

9、.19） 0 0 0 1 如用一個繞z軸旋轉(zhuǎn)-的變換矩陣右乘式（3.19）,結果如下 cos -sin 0 rcos cos(-) -sin(-) 0 0 sin cos 0 rsin sin(-) cos(-) 0 0 Cyl(z,r) = 0 0 1 z 0 0 0 0 （3.20） 0 0 1 1 0 0 0 1 cos -sin 0 rcos cos sin 0 0 sin cos 0 rsin -sin cos 0 0 Cyl(z,r) = 0 0 1 z 0 0 0 0 （3.21） 0 0 0 1 0 0 0 1 1 0 0 r cos 0 1 0 r sin Cyl(z,r)

10、= 0 0 1 z （3.22） 0 0 0 1 上式表明平移矢量未變，旋轉(zhuǎn)矩陣為單位陣，此時末端坐標的姿態(tài)未變，而只是改變了它的空間位置。,3.7 球坐標 ( Spherical Coordinates ),如圖3.7所示，用球坐標來確定位置向量的方法是： 沿著z軸平移，然后繞y軸旋轉(zhuǎn)，最后繞z軸旋轉(zhuǎn)。 Sph(,) = Rot(z,) Rot(y,) Trans(0,0,) （3.23） cos 0 sin 0 1 0 0 0 0 1 0 0 1 1 0 0 Sph(,) = Rot(z,) -sin 0 cos 0 0 0 1 0 0 0 1 0 0 0 1 （3.24）,cos -si

11、n 0 0 cos 0 sin rsin sin cos 0 0 0 1 0 0 Sph(,) = 0 0 1 0 -sin 0 cos rcos （3.25） 0 0 0 1 0 0 0 1 coscos -sin cossin cossin sincos cos sinsin sinsin Sph(,) = -sin 0 cos cos （3.26） 0 0 0 1 同樣，如果不希望改變末端坐標的姿態(tài)，而只是改變其空間位置，我們可以用Rot(y,-)和Rot(z, -)右乘式（3.26） Sph(,) = Rot(z,)Rot(y,)Trans(0,0,) Rot(y,-) Rot(z,-

12、) （3.27） 1 0 0 cossin 0 1 0 sinsin Sph(,) = 0 0 1 cos （3.28） 0 0 0 1,3.7 T6的確定 ( Specification of T6 ),T6可以用旋轉(zhuǎn)和平移的方法來確定。 T6 =平移旋轉(zhuǎn) （3.29） 表3.1 各種平移與旋轉(zhuǎn)的表達式 Translation Eqn Rotation Eqn px, py ,pz ox o y oz ax a y a z Rot(k,) 2.32 Cyl( z, , r ) 3.22 Euler(,) 3.11 Sph(,) 3.26 RPY(,) 3.12 我們已經(jīng)研究過的各種平移與旋轉(zhuǎn)

13、的式子，總結在表3.1中。如果我們使用Cyl和Sph的非旋轉(zhuǎn)的形式，那么矩陣積（3.29）僅僅是一個平移變換。,3.9 各種A矩陣的確定 ( Specification of matrices A ),現(xiàn)在考慮方程(3.1)右邊各A矩陣的確定。串聯(lián)桿型機械手是由一系列通過連桿與其活動關節(jié)連接在一起所組成 。 如圖3.8所示，任何一個連桿都可以用兩個量來描述：一個是公共垂線距離an，另一個是與an垂直的平面上兩個軸的夾角n，習慣上稱an為連桿長度，n稱為連桿的扭轉(zhuǎn)角。,圖3.8 連桿的長度與扭轉(zhuǎn)角,如圖3.9所示，在每個關節(jié)軸上有兩個連桿與之相連，即關節(jié)軸有兩個公垂線與之垂直，每一個連桿一個。兩

14、個相連的連桿的相對位置用dn和n確定， dn是沿著n關節(jié)軸兩個垂線的距離， n是在垂直這個關節(jié)軸的平面上兩個被測垂線之間的夾角， dn和n分別稱作連桿之間的距離及夾角。,為了描述連桿之間的關系，我們對每個連桿賦一個坐標系。 轉(zhuǎn)動關節(jié)：關節(jié)變量為n。連桿n的坐標原點設在關節(jié)n和關節(jié)n+1軸之間的公共垂線與關節(jié)n+1軸的交點上。在關節(jié)軸相交的情況下（無公垂線），這個原點就在兩個關節(jié)軸的相交點上（an0）。如果兩個關節(jié)軸平行（有無數(shù)條公垂線），則原點的選擇要使下一個連桿的關節(jié)距離為0（dn0），連桿n的z軸與n+1關節(jié)軸在一條直線上。x軸與任何存在的公共垂線成一條直線，并且沿著這條垂線從n關節(jié)指向n

15、+1關節(jié)。在相交關節(jié)的情況下，x軸的方向平行或者逆平行zn-1zn的向量叉積，應該注意，這個條件對于沿著關節(jié)n和n+1之間垂線的x軸同樣滿足。當xn-1和xn平行，且有相同的指向時，則對于第n個轉(zhuǎn)動關節(jié)n0。,表3.2 連桿參數(shù),棱形關節(jié)：關節(jié)變量為dn。關節(jié)軸的方向就是關節(jié)的運動方向。與轉(zhuǎn)動關節(jié)不同，軸的運動方向被確定了，但在空間的位置并沒有確定（見圖2.10）。對于棱形關節(jié)，連桿長度an沒有意義，所以被設置為0。棱形關節(jié)坐標的z軸（zn）與下一個連桿的軸在一條直線上，x軸（xn）平行或逆平行棱形關節(jié)軸的方向（zn-1）與zn的叉積。對于棱形關節(jié)，當dn=0時，定義為0位置（即坐標原點）。因

16、此棱形關節(jié)坐標原點與上一個關節(jié)（n-1）坐標原點重合，上一個關節(jié)的z軸（zn-1）與棱形關節(jié)的軸向相同，其關節(jié)長度an-1為上一個關節(jié)的軸線與zn-1的公垂線長度，xn-1軸向為公垂線向下一個關節(jié)延伸的方向。,根據(jù)上述模式用下列旋轉(zhuǎn)和位移我們可以建立相鄰的n-1和n坐標系之間的關系： 繞 zn-1 旋轉(zhuǎn)一個角度n 沿 zn-1 位移一個距離 dn 沿著被旋轉(zhuǎn)的 xn-1 即 xn 位移 an 繞 xn 旋轉(zhuǎn)的扭轉(zhuǎn)角為n 這四個齊次變換的積為A矩陣，即 An= Rot(z,) Trans(0,0,d) Trans(a,0,0) Rot(x,) （3.30） cos -sin 0 0 1 0 0

17、a 1 0 0 0 sin cos 0 0 0 1 0 0 0 cos -sin 0 An = 0 0 1 0 0 0 1 d 0 sin cos 0 （3.31） 0 0 0 1 0 0 0 1 0 0 0 1 cos -sincos sinsin acos sin coscos -cossin asin An = 0 sin cos d （3.32） 0 0 0 1,對于棱形關節(jié)，an = 0，則式（3.32）A矩陣簡化為 cos -sincos sinsin 0 sin coscos -cossin 0 An = 0 sin cos d （3.33） 0 0 0 1 一旦給機械手各連桿坐

18、標系都賦了值，各種固定的連桿參數(shù)可以確定：對于后面是旋轉(zhuǎn)關節(jié)的連桿參數(shù)為d, a和，對于后面是棱形關節(jié)的連桿參數(shù)為和。根據(jù)這些參數(shù)，的正弦和余弦也可以求出。這樣，對于旋轉(zhuǎn)關節(jié)，A矩陣變成了關節(jié)變量的函數(shù)?；蛟诶庑侮P節(jié)的情況下，變成了d的函數(shù)。一旦這些值給出，對于六個Ai變換矩陣的值就可以確定。,3.10 根據(jù)A矩陣來確定T6( Specification of T6 in Terms of the A matrices ),機械手的坐標變換圖如圖3.11所示，機械手的末端（即連桿坐標系6）相對于連 桿坐標系n-1的描述用n-1T6表示，即： n-1T6 = An An+1 A6 （3.34）,

19、機械手的末端相對于基坐標系（用T6表示）用下式給出 T6 = A1 A2 A3 A4 A5 A6 （3.35） 如果機械手用變換矩陣Z與參考坐標系相聯(lián)系，機械手末端執(zhí) 行器用E來描述，末端執(zhí)行器的位置和方向相對參考坐標系用X來 描述，如圖3.11所示有 X = Z T6 E （3.36） 由此可以得到T6的表達式 T6 = Z-1 X E-1 （3.37）,3.11 斯坦福機械手的運動方程(Kinematic Equations for the Stanford Manipulator),斯坦福機械手及其各關節(jié)坐 標的設置如圖3.12所示。 將角的正弦和余弦簡化 sini = Si cosi

20、= Ci sin (i+j) = Sij cos (i+j) = Cij 注：將所有關節(jié)x軸的方向設置 一致，可簡化坐標變換。,圖3.12 斯坦福機械手坐標系,表3.2 斯坦福機械手連桿參數(shù) Link Variable a d cos sin 1 1 -90 0 0 0 -1 2 2 90 0 d2 0 1 3 d3 0 0 d3 1 0 4 4 -90 0 0 0 -1 5 5 90 0 0 0 1 6 6 0 0 0 1 0,斯坦福機械手的A變換如下： C1 0 -S1 0 S1 0 C1 0 A1 = 0 -1 0 0 （3.38） 0 0 0 1 C2 0 S2 0 S2 0 -C2

21、0 A2 = 0 1 0 d2 （3.39） 0 0 0 1 1 0 0 0 0 1 0 0 A3 = 0 0 1 d3 （3.40） 0 0 0 1,C4 0 -S4 0 S4 0 C4 0 A4 = 0 -1 0 0 （3.41） 0 0 0 1 C5 0 S5 0 S5 0 -C5 0 A5 = 0 1 0 0 （3.42） 0 0 0 1 C6 -S6 0 0 S6 C6 0 0 A6 = 0 0 1 0 （3.43） 0 0 0 1,斯坦福機械手A變換的積如下所示，這些是從連桿6開始，然后逐步回到基坐標。 C6 -S6 0 0 S6 C6 0 0 5T6 = 0 0 1 0 （3.4

22、4） 0 0 0 1 C5C6 -C5S6 S5 0 S5C6 -S5S6 -C5 0 4T6 = S6 C6 0 0 （3.45） 0 0 0 1 C4C5C6-S4S6 -C4C5S6-S4C6 C4S5 0 S4C5C6+C4S6 -S4C5S6+C4C6 S4S5 0 3T6 = -S5C6 S5S6 C5 0 （3.46） 0 0 0 1,C4C5C6-S4S6 -C4C5S6-S4C6 C4S5 0 S4C5C6+C4S6 -S4C5S6+C4C6 S4S5 0 2T6 = -S5C6 S5S6 C5 d3 （3.47） 0 0 0 1 C2(C4C5C6 - S4S6) - S2

23、S5C6 -C2(C4C5S6 + S4C6) + S2S5S6 S2(C4C5C6 - S4S6) + C2S5C6 -S2(C4C5 S6+ S4C6) - C2S5S6 1T6 = S4C5C6 + C4C6 -S4C5S6 + C4C6 0 0 C2C4S5 + S2C5 S2d3 S2C4S5 - C2C5 -C2d3 S4S5 d2 （3.48） 0 1,nx ox ax px ny oy ay py T6 = nz oz az pz （3.49） 0 0 0 1 其中 nx = C1 C2 ( C4C5C6 - S4S6 ) - S2S5C6 - S1( S4C5S6 + C4S

24、6 ) ny = S1 C2 ( C4C5C6 - S4S6 ) - S2S5C6 + C1( S4C5S6+C4S6 ) nz = -S2 ( C4C5C6 - S4S6 ) - C2S5C6 ox = C1 -C2 ( C4C5S6 + S4C6 ) + S2S5C6 - S1( -S4C5S6 + C4S6 ) oy = S1 -C2 ( C4C5C6 + S4C6 ) + S2S5S6 + C1( -S4C5S6 + C4S6 ) oz = S2 ( C4C5C6 + S4C6 ) + C2S5S6 ax = C1 ( C2C4S5 + S2C5 ) S1S4C5 ay = S1 (

25、C2C4S5 + S2C5 ) + C1S4S5 az = S2C4S5 + C2C5 px = C1S2d3 S1d2 py = S1S2d3 + C1d2 pz = C2d3,3.12 肘機械手的運動方程( Kineamtic Equations for an Elbow Manipulator ),肘機械手及其各關節(jié)坐標的設置如圖3.13所示。,表3.3 肘機械手的連桿參數(shù) Link Variable a d cos sin 1 1 90 0 0 0 1 2 2 0 a2 0 1 0 3 3 0 a3 0 1 0 4 4 -90 a4 0 0 -1 5 5 90 0 0 0 1 6 6

26、0 0 0 1 0 注：在以下的T矩陣中用變量23 =2 +3和234 =23 +4來進 行簡化。,肘機械手的A變換如下： C1 0 S1 0 S1 0 -C1 0 A1 = 0 1 0 0 （3.50） 0 0 0 1 C2 -S2 0 C2S2 S2 C2 0 S2a2 A2 = 0 1 1 0 （3.51） 0 0 0 1 C3 -S3 0 C3a3 S3 C3 0 S3a3 A3 = 0 0 1 0 （3.52） 0 0 0 1,C4 0 -S4 C4a4 S4 0 C4 S4a4 A4 = 0 -1 0 0 （3.53） 0 0 0 1 C5 0 S5 0 S5 0 -C5 0 A5 = 0 -1 0 0 （3.54） 0 0 0 1 C6 -S6 0 0 S6 C6 0 0 A6 = 0 0 1 0 （3.55） 0 0 0 1,為了得到T6，我們從連桿6開始來算A矩陣的積，逐步往回計算到基坐標。 C6 -S6 0 0 S6 C6 0 0 5T6 = 0 0 1 0 （3.