第三章 工業(yè)機(jī)器人運(yùn)動學(xué)-2運(yùn)動學(xué)方程（章節(jié)課程）

1、 第三章 工業(yè)機(jī)器人運(yùn)動學(xué)-21教書育人主要內(nèi)容 數(shù)學(xué)基礎(chǔ)齊次坐標(biāo)變換 機(jī)器人運(yùn)動學(xué)方程的建立（正運(yùn)動學(xué)） 機(jī)器人逆運(yùn)動學(xué)分析2教書育人二、運(yùn)動學(xué)方程的建立（運(yùn)動學(xué)正問題）2.1 引言 2.8 T6的說明 2.2 姿態(tài)描述 2.9 各種A矩陣的說明 2.3 歐拉角 2.10 根據(jù)A矩陣來確定T6 2.4 搖擺、俯仰和偏轉(zhuǎn) 2.11 斯坦福機(jī)械手的運(yùn)動方程2.5 位置的確定 2.12 肘機(jī)械手的運(yùn)動方程2.6 圓柱坐標(biāo) 2.13 小結(jié) 2.7 球坐標(biāo) 3教書育人2.1 引言 ( Introduction ) 本章，我們采用齊次變換來描述在各種坐標(biāo)系中機(jī)械手的位置與方向。首先介紹各種正交坐標(biāo)系的齊

2、次變換。然后介紹在非正交關(guān)節(jié)坐標(biāo)系中描述機(jī)械手末端的齊次變換。注意，對任何數(shù)目關(guān)節(jié)的各種機(jī)械手均可以這樣進(jìn)行。 描述一個連桿與下一個連桿之間關(guān)系的齊次變換稱A矩陣。A矩陣是描述連桿坐標(biāo)系之間的相對平移和旋轉(zhuǎn)的齊次變換。 連續(xù)變換的若干A矩陣的積稱為T矩陣，對于一個六連桿（六自由度）機(jī)械手有 T6 = A1 A2 A3 A4 A5 A6 （2.1） 六連桿的機(jī)械手有六個自由度，其中三個自由度用來確定位置，三個自由度用來確定方向。表示機(jī)械手在基坐標(biāo)中的位置與方向。則變換矩陣有下列元素 nx ox ax px ny oy ay py T6 = nz oz az pz （2.2） 0 0 0 14教書

3、育人 如圖2.1所示，機(jī)器人的末端執(zhí)行器（手爪）的姿態(tài)（方向）由 n、o、a 三個旋轉(zhuǎn)矢量描述，其坐標(biāo)位置由平移矢量 p 描述，這就構(gòu)成了式（2.2）中的變換矩陣 T。 由于 n、o、a 三個旋轉(zhuǎn)矢量是正交矢量，所以有n = oa圖2.1 末端執(zhí)行器的描述 5教書育人2.2 姿態(tài)描述 ( Specification of Orientation ) 對式（2.2）中16個元素一一賦值就可確定T6。假定機(jī)械手可以到達(dá)要求的位置，而單位旋轉(zhuǎn)矢量o和a正交，即oo 1 （2.3） aa 1 （2.4） oa 0 （2.5） a形成單位向量 a a （2.6） | a | 構(gòu)成與o和a正交的n n o

4、a （2.7） 在o和a形成的平面上旋轉(zhuǎn)o，使得o與n和a正交 o an （2.8） 單位向量o是 o o （2.9） | o | 根據(jù)數(shù)學(xué)基礎(chǔ)給出的一般性的旋轉(zhuǎn)矩陣ot (k ,)，它把機(jī)械手末端的姿態(tài)規(guī)定為繞k軸旋轉(zhuǎn)角。6教書育人2.歐拉角 ( Euler Angles ) 姿態(tài)變更常用繞x,y或z軸的一系列旋轉(zhuǎn)來確定。歐拉角描述方法是：先繞z軸旋轉(zhuǎn)，然后繞新的y(即y/)軸旋轉(zhuǎn)，最后繞更新的z(z/)軸旋轉(zhuǎn)（見圖2.2）歐拉變換Euler(,)可以通過連乘三個旋轉(zhuǎn)矩陣來求得Euler(,) ot(z,)ot(y,)ot(z,) （2.10） 在一系列旋轉(zhuǎn)中，旋轉(zhuǎn)的次序是重要的。應(yīng)注意，旋

5、轉(zhuǎn)序列如果按相反的順序進(jìn)行，則是繞基坐標(biāo)中的軸旋轉(zhuǎn)：繞z軸旋轉(zhuǎn) ，接著繞y軸旋轉(zhuǎn)，最后再一次繞z軸旋轉(zhuǎn) ，結(jié)果如圖2.3所示，它與圖2.2是一致的。7教書育人xxxxyyyzzzzy圖2.2 歐拉角0 xxxxyyyzzzzy圖2.3 基于基坐標(biāo)的歐拉角08教書育人2.4 搖擺、俯仰和偏轉(zhuǎn) ( Roll, Pitch and Yaw ) 搖擺、俯仰和偏轉(zhuǎn)為另一種旋轉(zhuǎn)。如圖2.4所示，就像水中航行的一條小船一樣，繞著它前進(jìn)的方向（z軸）旋轉(zhuǎn) 稱為搖擺，繞著它的橫向中軸（y軸）旋轉(zhuǎn) 稱為俯仰，繞著它甲板的垂直向上的方向（x軸）旋轉(zhuǎn) 稱為偏轉(zhuǎn)。借助于這種旋轉(zhuǎn)來描述機(jī)械手的末端執(zhí)行器如圖3.5所示。規(guī)

6、定旋轉(zhuǎn)的次序為 RPY(,)ot(z,)ot(y,)ot(x,) （2.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(,) = 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 9教書育人圖2.

7、4 搖擺、俯仰和偏 轉(zhuǎn)角圖2.5 機(jī)械手的末端執(zhí)行器 的搖擺、俯仰和偏 轉(zhuǎn)10教書育人RPY(，) =cos cos cos sinsin sin cos cos sincos + sin sin 0sin cos sin sinsin + cos cos sin sincoscos sin 0 -sin cossin coscos 0 0 0 0 1 （2.15）11教書育人2.5 位置的確定 ( Specification of Position ) 一旦方向被確定之后，用一個相應(yīng)的p向量的位移變換可得到機(jī)器人末端執(zhí)行器在基坐標(biāo)中的位置： 1 0 0 px 0 1 0 py T6 = 0

8、0 1 pz （2.16） 0 0 0 1 旋轉(zhuǎn)變換矩陣12教書育人2.6 圓柱坐標(biāo) ( Cylindrical Coordinates ) 如圖2.6所示，在圓柱坐標(biāo)中確定機(jī)械手的位置是沿x軸平移r，接著繞z軸旋轉(zhuǎn)，最后沿著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

9、 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）zaCzyxBAorn圖2.6 圓柱坐標(biāo)注意：圓柱坐標(biāo)只能繞 z 軸旋轉(zhuǎn)13教書育人 cos -sin 0 rcos sin cos 0 rsin Cyl(z,r) = 0 0 1 z （2.19） 0 0 0 1 如用一個繞z軸旋轉(zhuǎn)-的變換矩陣右乘式（2.19）,結(jié)果如下 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

10、（2.20） 0 0 1 1 0 0 0 1 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.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 上式表明平移矢量未變，旋轉(zhuǎn)矩陣為單位陣，此時末端坐標(biāo)的姿態(tài)未變，而只是改變了它的空間位置。14教書育人2.7 球坐標(biāo) ( Spherical Coordinates ) 如圖2.7所示，用球坐標(biāo)來確定位置向量的方法是：沿著z軸平

11、移，然后繞y軸旋轉(zhuǎn)，最后繞z軸旋轉(zhuǎn)。Sph(,) = Rot(z,) Rot(y,) Trans(0,0,) （2.23） cos 0 sin 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 球坐標(biāo) 15教書育人 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

12、cossin sincos cos sinsin sinsin Sph(,) = -sin 0 cos cos （2.26） 0 0 0 1 同樣，如果不希望改變末端坐標(biāo)的姿態(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 16教書育人2.7 T6的確定 ( Specification of T6 ) T6可以用旋轉(zhuǎn)

13、和平移的方法來確定。 T6 =平移旋轉(zhuǎn) （2.29） 表2.1 各種平移與旋轉(zhuǎn)的表達(dá)式 Translation Eqn Rotation 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)研究過的各種平移與旋轉(zhuǎn)的式子，總結(jié)在表2.1中。如果我們使用Cyl和Sph的非旋轉(zhuǎn)的形式，那么矩陣積（2.29）僅僅是一個平移變換。 17教書育人2.9 各種A矩陣的確定 ( Specification of matrices A ) 現(xiàn)在考

14、慮方程(2.1)右邊各A矩陣的確定。串聯(lián)桿型機(jī)械手是由一系列通過連桿與其活動關(guān)節(jié)連接在一起所組成 。 如圖2.8所示，任何一個連桿都可以用兩個量來描述：一個是公共垂線距離an，另一個是與an垂直的平面上兩個軸的夾角n，習(xí)慣上稱an為連桿長度，n稱為連桿的扭轉(zhuǎn)角。圖2.8 連桿的長度與扭轉(zhuǎn)角18教書育人 如圖2.9所示，在每個關(guān)節(jié)軸上有兩個連桿與之相連，即關(guān)節(jié)軸有兩個公垂線與之垂直，每一個連桿一個。兩個相連的連桿的相對位置用dn和n確定， dn是沿著n關(guān)節(jié)軸兩個垂線的距離， n是在垂直這個關(guān)節(jié)軸的平面上兩個被測垂線之間的夾角， dn和n分別稱作連桿之間的距離及夾角。圖2.9 連桿參數(shù)xn-119

15、教書育人 為了描述連桿之間的關(guān)系，我們對每個連桿賦一個坐標(biāo)系。 轉(zhuǎn)動關(guān)節(jié)：關(guān)節(jié)變量為n。連桿n的坐標(biāo)原點設(shè)在關(guān)節(jié)n和關(guān)節(jié)n+1軸之間的公共垂線與關(guān)節(jié)n+1軸的交點上。在關(guān)節(jié)軸相交的情況下（無公垂線），這個原點就在兩個關(guān)節(jié)軸的相交點上（an0）。如果兩個關(guān)節(jié)軸平行（有無數(shù)條公垂線），則原點的選擇要使下一個連桿的關(guān)節(jié)距離為0（dn0），連桿n的z軸與n+1關(guān)節(jié)軸在一條直線上。x軸與任何存在的公共垂線成一條直線，并且沿著這條垂線從n關(guān)節(jié)指向n+1關(guān)節(jié)。在相交關(guān)節(jié)的情況下，x軸的方向平行或者逆平行zn-1zn的向量叉積，應(yīng)該注意，這個條件對于沿著關(guān)節(jié)n和n+1之間垂線的x軸同樣滿足。當(dāng)xn-1和xn平

16、行，且有相同的指向時，則對于第n個轉(zhuǎn)動關(guān)節(jié)n0。 連桿本身的參數(shù)連桿長度an連桿兩個軸的公垂線距離（x方向）連桿扭轉(zhuǎn)角n連桿兩個軸的夾角（x軸的扭轉(zhuǎn)角）連桿之間的參數(shù)連桿之間的距離dn相連兩連桿公垂線距離（z方向平移距）連桿之間的夾角n相連兩連桿公垂線的夾角（z軸旋轉(zhuǎn)角）表2.2 連桿參數(shù)20教書育人棱形關(guān)節(jié)：關(guān)節(jié)變量為dn。關(guān)節(jié)軸的方向就是關(guān)節(jié)的運(yùn)動方向。與轉(zhuǎn)動關(guān)節(jié)不同，軸的運(yùn)動方向被確定了，但在空間的位置并沒有確定（見圖2.10）。對于棱形關(guān)節(jié)，連桿長度an沒有意義，所以被設(shè)置為0。棱形關(guān)節(jié)坐標(biāo)的z軸（zn）與下一個連桿的軸在一條直線上，x軸（xn）平行或逆平行棱形關(guān)節(jié)軸的方向（zn-1）

17、與zn的叉積。對于棱形關(guān)節(jié)，當(dāng)dn=0時，定義為0位置（即坐標(biāo)原點）。因此棱形關(guān)節(jié)坐標(biāo)原點與上一個關(guān)節(jié)（n-1）坐標(biāo)原點重合，上一個關(guān)節(jié)的z軸（zn-1）與棱形關(guān)節(jié)的軸向相同，其關(guān)節(jié)長度an-1為上一個關(guān)節(jié)的軸線與zn-1的公垂線長度，xn-1軸向為公垂線向下一個關(guān)節(jié)延伸的方向。圖2.10 棱型關(guān)節(jié)的連桿參數(shù)an-121教書育人 根據(jù)上述模式用下列旋轉(zhuǎn)和位移我們可以建立相鄰的n-1和n坐標(biāo)系之間的關(guā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,)

18、 Trans(0,0,d) Trans(a,0,0) Rot(x,) （2.30） cos -sin 0 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 122教書育人 對于棱形關(guān)節(jié)，an = 0，則式（2.32）A矩陣簡化為 cos -sincos sinsi

19、n 0 sin coscos -cossin 0 An = 0 sin cos d （2.33） 0 0 0 1 一旦給機(jī)械手各連桿坐標(biāo)系都賦了值，各種固定的連桿參數(shù)可以確定：對于后面是旋轉(zhuǎn)關(guān)節(jié)的連桿參數(shù)為d, a和，對于后面是棱形關(guān)節(jié)的連桿參數(shù)為和。根據(jù)這些參數(shù)，的正弦和余弦也可以求出。這樣，對于旋轉(zhuǎn)關(guān)節(jié)，A矩陣變成了關(guān)節(jié)變量的函數(shù)?；蛟诶庑侮P(guān)節(jié)的情況下，變成了d的函數(shù)。一旦這些值給出，對于六個Ai變換矩陣的值就可以確定。23教書育人2.10 根據(jù)A矩陣來確定T6( Specification of T6 in Terms of the A matrices ) 機(jī)械手的坐標(biāo)變換圖如圖2.1

20、1所示，機(jī)械手的末端（即連桿坐標(biāo)系6）相對于連桿坐標(biāo)系n-1的描述用n-1T6表示，即： n-1T6 = An An+1 A6 （2.34） 0zA1A2A3A4A5A60EX0T61T62T63T64T65T6圖2.11 機(jī)械手的坐標(biāo)變換圖24教書育人 機(jī)械手的末端相對于基坐標(biāo)系（用T6表示）用下式給出 T6 = A1 A2 A3 A4 A5 A6 （2.35） 如果機(jī)械手用變換矩陣Z與參考坐標(biāo)系相聯(lián)系，機(jī)械手末端執(zhí) 行器用E來描述，末端執(zhí)行器的位置和方向相對參考坐標(biāo)系用X來 描述，如圖2.11所示有 X = Z T6 E （2.36） 由此可以得到T6的表達(dá)式 T6 = Z-1 X E-1

21、 （2.37）25教書育人2.11 斯坦福機(jī)械手的運(yùn)動方程(Kinematic Equations for the Stanford Manipulator) 斯坦福機(jī)械手及其各關(guān)節(jié)坐標(biāo)的設(shè)置如圖2.12所示。 將角的正弦和余弦簡化 sini = Si cosi = Ci sin (i+j) = Sij cos (i+j) = Cij注：將所有關(guān)節(jié)x軸的方向設(shè)置 一致，可簡化坐標(biāo)變換。圖2.12 斯坦福機(jī)械手坐標(biāo)系26教書育人表2.2 斯坦福機(jī)械手連桿參數(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 d

22、3 1 0 4 4 -90 0 0 0 -1 5 5 90 0 0 0 1 6 6 0 0 0 1 027教書育人斯坦福機(jī)械手的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 28教書育人 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

23、 1 0 0 （2.42） 0 0 0 1 C6 -S6 0 0 S6 C6 0 0 A6 = 0 0 1 0 （2.43） 0 0 0 129教書育人斯坦福機(jī)械手A變換的積如下所示，這些是從連桿6開始，然后逐步回到基坐標(biāo)。 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 = -S5

24、C6 S5S6 C5 0 （2.46） 0 0 0 130教書育人 C4C5C6-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 S

25、2d3 S2C4S5 - C2C5 -C2d3 S4S5 d2 （2.48） 0 131教書育人 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

26、- S1( -S4C5S6 + C4S6 ) oy = S1 -C2 ( 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 = C2d332教書育人2.12 肘機(jī)械手的運(yùn)動方程( Kineamtic Equations for an

27、 Elbow Manipulator ) 肘機(jī)械手及其各關(guān)節(jié)坐標(biāo)的設(shè)置如圖2.13所示。z1z0z2z3z4z5、z6xa3a4a2圖2.13 肘機(jī)械手的坐標(biāo)系x0y033教書育人 表2.3 肘機(jī)械手的連桿參數(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來進(jìn) 行簡化。 34教書育人肘機(jī)械手的A變換如下： C1 0 S1 0 S1 0 -C1

28、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 135教書育人 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 136教書育人 為了得到T6，我們從連桿6開始來算A矩陣的積，逐步往回計算到基坐標(biāo)。 C6 -