第三章 工業(yè)機器人運動學-2運動學方程

1、整理課件1 第三章 工業(yè)機器人運動學-2整理課件2主要內容主要內容u 數(shù)學基礎數(shù)學基礎齊次坐標變換齊次坐標變換u 機器人運動學方程的建立（正運動學）機器人運動學方程的建立（正運動學）u 機器人逆運動學分析機器人逆運動學分析整理課件3二、運動學方程的建立（運動學正問題）二、運動學方程的建立（運動學正問題）2.1 2.1 引言引言 2.8 T2.8 T6 6的說明的說明 2.22.2 姿態(tài)描述姿態(tài)描述 2.9 2.9 各種各種A A矩陣的說明矩陣的說明 2.32.3 歐拉角歐拉角 2.10 2.10 根據(jù)根據(jù)A A矩陣來確定矩陣來確定T T6 6 2.4 2.4 搖擺、俯仰和偏轉搖擺、俯仰和偏轉

2、2.11 2.11 斯坦福機械手的運動方程斯坦福機械手的運動方程2.5 2.5 位置的確定位置的確定 2.12 2.12 肘機械手的運動方程肘機械手的運動方程2.6 2.6 圓柱坐標圓柱坐標 2.13 2.13 小結小結 2.7 2.7 球坐標球坐標 整理課件42.1 引言引言 ( Introduction ) 本章，我們采用齊次變換來描述在各種坐標系中機械手的位置與方向。首先介紹各種正交坐標系的齊次變換。然后介紹在非正交關節(jié)坐標系中描述機械手末端的齊次變換。注意，對任何數(shù)目關節(jié)的各種機械手均可以這樣進行。 描述一個連桿與下一個連桿之間關系的齊次變換稱A矩陣。A矩陣是描述連桿坐標系之間的相對平

3、移和旋轉的齊次變換。 連續(xù)變換的若干A矩陣的積稱為T矩陣，對于一個六連桿（六自由度）機械手有 T6 = A1 A2 A3 A4 A5 A6 （2.1） 六連桿的機械手有六個自由度，其中三個自由度用來確定位置，三個自由度用來確定方向。表示機械手在基坐標中的位置與方向。則變換矩陣有下列元素 nx ox ax px ny oy ay py T6 = nz oz az pz （2.2） 0 0 0 1整理課件5 如圖2.1所示，機器人的末端執(zhí)行器（手爪）的姿態(tài)（方向）由 n、o、a 三個旋轉矢量描述，其坐標位置由平移矢量 p 描述，這就構成了式（2.2）中的變換矩陣 T。 由于 n、o、a 三個旋轉矢

4、量是正交矢量，所以有n = oa圖2.1 末端執(zhí)行器的描述 整理課件62.2 姿態(tài)描述姿態(tài)描述 ( Specification of Orientation ) 對式（2.2）中16個元素一一賦值就可確定T6。假定機械手可以到達要求的位置，而單位旋轉矢量o和a正交，即oo 1 （2.3） aa 1 （2.4） oa 0 （2.5） a形成單位向量 a a （2.6） | a | 構成與o和a正交的n n oa （2.7） 在o和a形成的平面上旋轉o，使得o與n和a正交 o an （2.8） 單位向量o是 o o （2.9） | o | 根據(jù)數(shù)學基礎給出的一般性的旋轉矩陣ot (k ,)，它把機

5、械手末端的姿態(tài)規(guī)定為繞k軸旋轉角。整理課件72.歐拉角歐拉角 ( Euler Angles ) 姿態(tài)變更常用繞x,y或z軸的一系列旋轉來確定。歐拉角描述方法是：先繞z軸旋轉，然后繞新的y(即y/)軸旋轉，最后繞更新的z(z/)軸旋轉（見圖2.2）歐拉變換Euler(,)可以通過連乘三個旋轉矩陣來求得Euler(,) ot(z,)ot(y,)ot(z,) （2.10） 在一系列旋轉中，旋轉的次序是重要的。應注意，旋轉序列如果按相反的順序進行，則是繞基坐標中的軸旋轉：繞z軸旋轉 ，接著繞y軸旋轉，最后再一次繞z軸旋轉 ，結果如圖2.3所示，它與圖2.2是一致的。整理課件8xxxxyyyzzzzy圖

6、2.2 歐拉角0 xxxxyyyzzzzy圖2.3 基于基坐標的歐拉角0整理課件92.4 搖擺、俯仰和偏轉搖擺、俯仰和偏轉 ( Roll, Pitch and Yaw ) 搖擺、俯仰和偏轉為另一種旋轉。如圖2.4所示，就像水中航行的一條小船一樣，繞著它前進的方向（z軸）旋轉 稱為搖擺，繞著它的橫向中軸（y軸）旋轉 稱為俯仰，繞著它甲板的垂直向上的方向（x軸）旋轉 稱為偏轉。借助于這種旋轉來描述機械手的末端執(zhí)行器如圖3.5所示。規(guī)定旋轉的次序為 RPY(,)ot(z,)ot(y,)ot(x,) （2.12） 即，繞x軸旋轉，接著繞y軸旋轉，最后繞z軸旋轉 ，這個變換如下 cos 0 sin 0

7、1 0 0 0 0 1 0 0 0 cos sin 0 RPY(,) = ot(z,) sin 0 cos 0 0 sin cos 0 （2.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 （2.14） 0 0 0 1 0 0 0 1 整理課件10圖2.4 搖擺、俯仰和偏 轉角圖2.5 機械手的末端執(zhí)行器 的搖擺、俯仰和偏 轉整理課件11RPY(，) =cos cos cos sinsin sin cos cos s

8、incos + sin sin 0sin cos sin sinsin + cos cos sin sincoscos sin 0 -sin cossin coscos 0 0 0 0 1 （2.15）整理課件122.5 位置的確定位置的確定 ( Specification of Position ) 一旦方向被確定之后，用一個相應的p向量的位移變換可得到機器人末端執(zhí)行器在基坐標中的位置： 1 0 0 px 0 1 0 py T6 = 0 0 1 pz （2.16） 0 0 0 1 旋轉變換矩陣整理課件132.6 圓柱坐標圓柱坐標 ( Cylindrical Coordinates ) 如圖2

9、.6所示，在圓柱坐標中確定機械手的位置是沿x軸平移r，接著繞z軸旋轉，最后沿著z軸平移z。Cyl(z, ,r) = Trans(0,0,z)Rot(z, ) Trans(r,0,0) cos -sin 0 0 1 0 0 r sin cos 0 0 0 1 0 0Cyl(z, ,r) = Trans(0,0,z) 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 （2.17） 1 0 0 0 cos -sin 0 rcos 0 1 0 0 sin cos 0 rsinCyl(z, ,r) = 0 0 1 z 0 0 1 0 0 0 0 1 0 0 0 1 （2.18）zaCzyx

10、BAorn圖2.6 圓柱坐標注意：圓柱坐標只能繞 z 軸旋轉整理課件14 cos -sin 0 rcos sin cos 0 rsin Cyl(z,r) = 0 0 1 z （2.19） 0 0 0 1 如用一個繞z軸旋轉-的變換矩陣右乘式（2.19）,結果如下 cos -sin 0 rcos cos(-) -sin(-) 0 0 sin cos 0 rsin sin(-) cos(-) 0 0Cyl(z,r) = 0 0 1 z 0 0 0 0 （2.20） 0 0 1 1 0 0 0 1 cos -sin 0 rcos cos sin 0 0 sin cos 0 rsin -sin cos

11、 0 0Cyl(z,r) = 0 0 1 z 0 0 0 0 （2.21） 0 0 0 1 0 0 0 1 1 0 0 r cos 0 1 0 r sin Cyl(z,r) = 0 0 1 z （2.22） 0 0 0 1 上式表明平移矢量未變，旋轉矩陣為單位陣，此時末端坐標的姿態(tài)未變，而只是改變了它的空間位置。整理課件152.7 球坐標球坐標 ( Spherical Coordinates ) 如圖2.7所示，用球坐標來確定位置向量的方法是：沿著z軸平移，然后繞y軸旋轉，最后繞z軸旋轉。Sph(,) = Rot(z,) Rot(y,) Trans(0,0,) （2.23） cos 0 sin

12、 0 1 0 0 0 0 1 0 0 1 1 0 0Sph(,) = Rot(z,) -sin 0 cos 0 0 0 1 0 0 0 1 0 0 0 1 （2.24）aonzyx圖2.7 球坐標 整理課件16 cos -sin 0 0 cos 0 sin rsin sin cos 0 0 0 1 0 0 Sph(,) = 0 0 1 0 -sin 0 cos rcos （2.25） 0 0 0 1 0 0 0 1 coscos -sin cossin cossin sincos cos sinsin sinsin Sph(,) = -sin 0 cos cos （2.26） 0 0 0 1

13、同樣，如果不希望改變末端坐標的姿態(tài)，而只是改變其空間位置，我們可以用Rot(y,-)和Rot(z, -)右乘式（2.26） Sph(,) = Rot(z,)Rot(y,)Trans(0,0,) Rot(y,-) Rot(z,-) （2.27） 1 0 0 cossin 0 1 0 sinsin Sph(,) = 0 0 1 cos （2.28） 0 0 0 1 整理課件172.7 T6的確定的確定 ( Specification of T6 ) T6可以用旋轉和平移的方法來確定。 T6 =平移旋轉 （2.29） 表2.1 各種平移與旋轉的表達式 Translation Eqn Rotation

14、 Eqn px, py ,pz ox o y oz ax a y a z Rot(k,) 2.32 Cyl( z, , r ) 2.22 Euler(,) 2.11 Sph(,) 2.26 RPY(,) 2.12 我們已經(jīng)研究過的各種平移與旋轉的式子，總結在表2.1中。如果我們使用Cyl和Sph的非旋轉的形式，那么矩陣積（2.29）僅僅是一個平移變換。 整理課件182.9 各種各種A矩陣的確定矩陣的確定 ( Specification of matrices A ) 現(xiàn)在考慮方程(2.1)右邊各A矩陣的確定。串聯(lián)桿型機械手是由一系列通過連桿與其活動關節(jié)連接在一起所組成 。 如圖2.8所示，任何

15、一個連桿都可以用兩個量來描述：一個是公共垂線距離an，另一個是與an垂直的平面上兩個軸的夾角n，習慣上稱an為連桿長度，n稱為連桿的扭轉角。圖2.8 連桿的長度與扭轉角整理課件19 如圖2.9所示，在每個關節(jié)軸上有兩個連桿與之相連，即關節(jié)軸有兩個公垂線與之垂直，每一個連桿一個。兩個相連的連桿的相對位置用dn和n確定， dn是沿著n關節(jié)軸兩個垂線的距離， n是在垂直這個關節(jié)軸的平面上兩個被測垂線之間的夾角， dn和n分別稱作連桿之間的距離及夾角。圖2.9 連桿參數(shù)xn-1整理課件20 為了描述連桿之間的關系，我們對每個連桿賦一個坐標系。 轉動關節(jié)轉動關節(jié)：關節(jié)變量為n。連桿n的坐標原點設在關節(jié)n

16、和關節(jié)n+1軸之間的公共垂線與關節(jié)n+1軸的交點上。在關節(jié)軸相交的情況下（無公垂線），這個原點就在兩個關節(jié)軸的相交點上（an0）。如果兩個關節(jié)軸平行（有無數(shù)條公垂線），則原點的選擇要使下一個連桿的關節(jié)距離為0（dn0），連桿n的z軸與n+1關節(jié)軸在一條直線上。x軸與任何存在的公共垂線成一條直線，并且沿著這條垂線從n關節(jié)指向n+1關節(jié)。在相交關節(jié)的情況下，x軸的方向平行或者逆平行zn-1zn的向量叉積，應該注意，這個條件對于沿著關節(jié)n和n+1之間垂線的x軸同樣滿足。當xn-1和xn平行，且有相同的指向時，則對于第n個轉動關節(jié)n0。 連桿本身的參數(shù)連桿長度an連桿兩個軸的公垂線距離（x方向）連桿扭

17、轉角n連桿兩個軸的夾角（x軸的扭轉角）連桿之間的參數(shù)連桿之間的距離dn相連兩連桿公垂線距離（z方向平移距）連桿之間的夾角n相連兩連桿公垂線的夾角（z軸旋轉角）表2.2 連桿參數(shù)整理課件21棱形關節(jié)棱形關節(jié)：關節(jié)變量為dn。關節(jié)軸的方向就是關節(jié)的運動方向。與轉動關節(jié)不同，軸的運動方向被確定了，但在空間的位置并沒有確定（見圖2.10）。對于棱形關節(jié)，連桿長度an沒有意義，所以被設置為0。棱形關節(jié)坐標的z軸（zn）與下一個連桿的軸在一條直線上，x軸（xn）平行或逆平行棱形關節(jié)軸的方向（zn-1）與zn的叉積。對于棱形關節(jié)，當dn=0時，定義為0位置（即坐標原點）。因此棱形關節(jié)坐標原點與上一個關節(jié)（n

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

19、0 1 0 0 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 （2.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 （2.32） 0 0 0 1整理課件23 對于棱形關節(jié)，an = 0，則式（2.32）A矩陣簡化為 cos -sincos sinsin 0 sin coscos -cossin 0 An = 0 sin cos d （2.33） 0 0

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

21、-1T6 = An An+1 A6 （2.34） 0zA1A2A3A4A5A60EX0T61T62T63T64T65T6圖2.11 機械手的坐標變換圖整理課件25 機械手的末端相對于基坐標系（用T6表示）用下式給出 T6 = A1 A2 A3 A4 A5 A6 （2.35） 如果機械手用變換矩陣Z與參考坐標系相聯(lián)系，機械手末端執(zhí) 行器用E來描述，末端執(zhí)行器的位置和方向相對參考坐標系用X來 描述，如圖2.11所示有 X = Z T6 E （2.36） 由此可以得到T6的表達式 T6 = Z-1 X E-1 （2.37）整理課件262.11 斯坦福機械手的運動方程斯坦福機械手的運動方程(Kinem

22、atic Equations for the Stanford Manipulator) 斯坦福機械手及其各關節(jié)坐標的設置如圖2.12所示。 將角的正弦和余弦簡化 sini = Si cosi = Ci sin (i+j) = Sij cos (i+j) = Cij注：將所有關節(jié)x軸的方向設置 一致，可簡化坐標變換。圖2.12 斯坦福機械手坐標系整理課件27表2.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

23、 1 6 6 0 0 0 1 0整理課件28斯坦福機械手的A變換如下： C1 0 -S1 0 S1 0 C1 0 A1 = 0 -1 0 0 （2.38） 0 0 0 1 C2 0 S2 0 S2 0 -C2 0 A2 = 0 1 0 d2 （2.39） 0 0 0 1 1 0 0 0 0 1 0 0 A3 = 0 0 1 d3 （2.40） 0 0 0 1 整理課件29 C4 0 -S4 0 S4 0 C4 0 A4 = 0 -1 0 0 （2.41） 0 0 0 1 C5 0 S5 0 S5 0 -C5 0 A5 = 0 1 0 0 （2.42） 0 0 0 1 C6 -S6 0 0 S6

24、 C6 0 0 A6 = 0 0 1 0 （2.43） 0 0 0 1整理課件30斯坦福機械手A變換的積如下所示，這些是從連桿6開始，然后逐步回到基坐標。 C6 -S6 0 0 S6 C6 0 0 5T6 = 0 0 1 0 （2.44） 0 0 0 1 C5C6 -C5S6 S5 0 S5C6 -S5S6 -C5 0 4T6 = S6 C6 0 0 （2.45） 0 0 0 1 C4C5C6-S4S6 -C4C5S6-S4C6 C4S5 0 S4C5C6+C4S6 -S4C5S6+C4C6 S4S5 0 3T6 = -S5C6 S5S6 C5 0 （2.46） 0 0 0 1整理課件31 C

25、4C5C6-S4S6 -C4C5S6-S4C6 C4S5 0 S4C5C6+C4S6 -S4C5S6+C4C6 S4S5 0 2T6 = -S5C6 S5S6 C5 d3 （2.47） 0 0 0 1 C2(C4C5C6 - S4S6) - S2S5C6 -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 （2.

26、48） 0 1整理課件32 nx ox ax px ny oy ay py T6 = nz oz az pz （2.49） 0 0 0 1其中 nx = C1 C2 ( C4C5C6 - S4S6 ) - S2S5C6 - S1( S4C5S6 + C4S6 ) 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

27、( C4C5C6 + S4C6 ) + S2S5S6 + C1( -S4C5S6 + C4S6 ) oz = S2 ( C4C5C6 + S4C6 ) + C2S5S6 ax = C1 ( C2C4S5 + S2C5 ) S1S4C5 ay = S1 ( C2C4S5 + S2C5 ) + C1S4S5 az = S2C4S5 + C2C5 px = C1S2d3 S1d2 py = S1S2d3 + C1d2 pz = C2d3整理課件332.12 肘機械手的運動方程肘機械手的運動方程( Kineamtic Equations for an Elbow Manipulator ) 肘機械手及

28、其各關節(jié)坐標的設置如圖2.13所示。z1z0z2z3z4z5、z6xa3a4a2圖2.13 肘機械手的坐標系x0y0整理課件34 表2.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 0 0 0 1 0注：在以下的T矩陣中用變量23 =2 +3和234 =23 +4來進 行簡化。 整理課件35肘機械手的A變換如下： C1 0 S1 0 S1 0 -C1 0 A1 = 0 1 0 0 （2.50） 0 0 0 1 C2 -S2 0 C2S2 S2 C2 0 S2a2 A2 = 0 1 1 0 （2.51） 0 0 0 1 C3 -S3 0 C3a3 S3 C3 0 S3a3 A3 = 0 0 1 0 （2.52） 0 0 0 1整理課件36 C4 0 -S4 C4a4 S4 0 C4 S4a4 A4 = 0 -1 0 0 （2.53） 0 0 0 1 C5 0 S5 0 S5 0 -C5 0 A5 = 0 -1 0 0 （2.54） 0 0 0 1 C6 -S6 0 0 S6 C6 0 0 A6 = 0 0 1 0 （2.55） 0 0 0 1整理課件37 為了得到T6，我們