3.1.4-工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)方程1.-機(jī)器人運(yùn)動(dòng)學(xué)方程

1、第3章 工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué) 3.1 工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué) 3.2 工業(yè)機(jī)器人的動(dòng)力學(xué)工業(yè)機(jī)器人的動(dòng)力學(xué) 3.3 工業(yè)機(jī)器人的運(yùn)動(dòng)軌跡規(guī)則工業(yè)機(jī)器人的運(yùn)動(dòng)軌跡規(guī)則 習(xí)題習(xí)題 3.1 工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué) 3.1.1 工業(yè)機(jī)器人位姿描述工業(yè)機(jī)器人位姿描述 1. 1. 點(diǎn)的位置描述點(diǎn)的位置描述如圖3.1所示,在直角坐標(biāo)系A(chǔ)中,空間任一點(diǎn)P的位置可用(31)的位置矢量AP表示為zyxApppP(3.1) 其中, px、 py、pz是點(diǎn)P的三個(gè)位置坐標(biāo)分量。 圖3.1點(diǎn)的位置描述2. 2. 點(diǎn)的齊次坐標(biāo)點(diǎn)的齊次坐標(biāo)如用四個(gè)數(shù)組成的(41)列陣表示三維空間直角坐標(biāo)系A(chǔ)

2、中點(diǎn)P, 則該列陣稱為三維空間點(diǎn)P的齊次坐標(biāo), 如下: 1zyxpppP(3.2) 齊次坐標(biāo)并不是惟一的, 當(dāng)列陣的每一項(xiàng)分別乘以一個(gè)非零因子時(shí), 即 cbapppzyx1P(3.3) 其中:a=px, b=py, c=pz。該列陣也表示P點(diǎn),齊次坐標(biāo)的表示不是惟一的。 3. 坐標(biāo)軸方向的描述坐標(biāo)軸方向的描述用i、j、k來(lái)表示直角坐標(biāo)系中X、Y、Z坐標(biāo)軸的單位向量, 用齊次坐標(biāo)來(lái)描述X、Y、Z軸的方向, 則有 0100,0010,0001ZYX規(guī)定: 列陣a b c 0T中第四個(gè)元素為零, 且a2+b2+c2=1, 表示某軸(或某矢量)的方向;列陣a b c T中第四個(gè)元素不為零, 則表示空間

3、某點(diǎn)的位置。 例如, 在圖3.2中, 矢量v的方向用(41)列陣表示為 0cbav其中: a=cos, b=cos, c=cos。 矢量v所坐落的點(diǎn)為坐標(biāo)原點(diǎn), 表示為 1000o 當(dāng)=60, =60, =45時(shí), 矢量為 0707. 05 . 05 . 0v圖3.2坐標(biāo)軸方向的描述4. 4. 動(dòng)坐標(biāo)系位姿的描述動(dòng)坐標(biāo)系位姿的描述動(dòng)坐標(biāo)系位姿的描述就是用位姿矩陣對(duì)動(dòng)坐標(biāo)系原點(diǎn)位置和坐標(biāo)系各坐標(biāo)軸方向的描述。該位姿矩陣為(44)的方陣。 如上述直角坐標(biāo)系可描述為: 1000010000100001A(3.4) 5. 剛體位姿的描述剛體位姿的描述機(jī)器人的每一個(gè)連桿均可視為一個(gè)剛體, 若給定了剛體上

4、某一點(diǎn)的位置和該剛體在空中的姿態(tài), 則這個(gè)剛體在空間上是惟一確定的, 可用惟一一個(gè)位姿矩陣進(jìn)行描述。 如圖3.3所示, 設(shè)OXYZ為與剛體Q固連的一個(gè)坐標(biāo)系, 稱為動(dòng)坐標(biāo)系。 剛體Q在固定坐標(biāo)系OXYZ中的位置可用齊次坐標(biāo)形式表示為 1000zyxp圖 3.3 剛體的位置和姿態(tài)描述 令n、o、a分別為X、 Y、 Z坐標(biāo)軸的單位方向矢量, 即 0,0,0zyxzyxzyxaaaooonnnaon剛體的位姿表示為(44)矩陣: 1000000zaonyaonxaonxzzyyyxxxpaonT6. 手部位姿的描述手部位姿的描述機(jī)器人手部的位姿如圖3.4所示, 可用固連于手部的坐標(biāo)系B的位姿來(lái)表示。

5、坐標(biāo)系B由原點(diǎn)位置和三個(gè)單位矢量惟一確定, 即: (1) 原點(diǎn): 取手部中心點(diǎn)為原點(diǎn)OB; (2) 接近矢量: 關(guān)節(jié)軸方向的單位矢量a; (3) 姿態(tài)矢量: 手指連線方向的單位矢量o; (4) 法向矢量: n為法向單位矢量, 同時(shí)垂直于a、o矢量, 即n=oa。 手部位姿矢量為從固定參考坐標(biāo)系OXYZ原點(diǎn)指向手部坐標(biāo)系B原點(diǎn)的矢量p p。手部的位姿可由(44)矩陣表示: 1000zzzzyyyyxxxxpaonpaonpaonpaonT(3.7) 圖 3.4 機(jī)器人手部的位置和姿態(tài)描述 7. 7. 目標(biāo)物位姿的描述目標(biāo)物位姿的描述任何一個(gè)物體在空間的位置和姿態(tài)都可以用齊次矩陣來(lái)表示, 如圖3.

6、5所示。楔塊Q在(a)圖的情況下可用6個(gè)點(diǎn)描述, 矩陣表達(dá)式為)64(104110411111220000001111Q(3.8) 若讓其繞Z軸旋轉(zhuǎn)90,記為Rot(z,90); 再繞Y軸旋轉(zhuǎn)90,即Rot(y,90), 然后再沿X軸方向平移4,即Trans(4, 0, 0), 則楔塊成為(b)圖位姿, 其齊次矩陣表達(dá)式為 )64(141414141111000011116644Q 用符號(hào)表示對(duì)目標(biāo)物的變換方式可以記錄物體移動(dòng)的過(guò)程, 也便于矩陣的運(yùn)算, 所以應(yīng)該熟練掌握。 圖 3.5 目標(biāo)物的位置和姿態(tài)描述 3.1.2 齊次變換及運(yùn)算齊次變換及運(yùn)算 1. 平移的齊次變換平移的齊次變換如圖3.

7、6所示為空間某一點(diǎn)在直角坐標(biāo)系中的平移,由A(x, y, z)平移至A(x, y, z), 即 zzzyyyxxx(3.10) 或?qū)懗?110001000100011zyxzyxzyx(3.11) 圖3.6點(diǎn)的平移變換記為: a=Trans(x, y, z)a 其中,Trans(x, y,z)稱為平移算子,x、y、z分別表示沿X、Y、Z軸的移動(dòng)量。 即: 1000100010001),(Transzyxzyx(3.12) 注: 算子左乘: 表示點(diǎn)的平移是相對(duì)固定坐標(biāo)系進(jìn)行的坐標(biāo)變換。 算子右乘: 表示點(diǎn)的平移是相對(duì)動(dòng)坐標(biāo)系進(jìn)行的坐標(biāo)變換。 該公式亦適用于坐標(biāo)系的平移變換、 物體的平移變換, 如

8、機(jī)器人手部的平移變換。 2. 旋轉(zhuǎn)的齊次變換旋轉(zhuǎn)的齊次變換點(diǎn)在空間直角坐標(biāo)系中的旋轉(zhuǎn)如圖3.7所示。A(x, y, z)繞Z軸旋轉(zhuǎn)角后至A(x, y, z),A與A之間的關(guān)系為 zzyxyyxxcossinsincos(3.13) 圖3.7點(diǎn)的旋轉(zhuǎn)變換推導(dǎo)如下: 因A點(diǎn)是繞Z軸旋轉(zhuǎn)的, 所以把A與A投影到XOY平面內(nèi), 設(shè)OA=r, 則有 sincosryrx(3.14) 同時(shí)有 sincosryrx(3.15) 其中, =+, 即 )sin()cos(ryrx(3.16) 所以 sincoscossinsinsincoscosrryrrx(3.17) 所以 sincossincosxyyyx

9、x(3.18) 由于Z坐標(biāo)不變, 因此有 zzxyyyxxcossinsincos(3.19) 寫成矩陣形式為 11000010000cossin00sincos1zyxzyx(3.20) 記為: a=Rot(z, )a 其中, 繞Z軸旋轉(zhuǎn)算子左乘是相對(duì)于固定坐標(biāo)系, 即 1000010000cossin00sincos),(Rotz(3.21) 同理, 10000cossin00sincos00001),(Rotx(3.22) 10000cos0sin00100sin0cos),(Roty(3.23) 圖3.8所示為點(diǎn)A繞任意過(guò)原點(diǎn)的單位矢量k旋轉(zhuǎn)角的情況。kx、ky、kz分別為k矢量在固定

10、參考坐標(biāo)軸X、Y、Z上的三個(gè)分量,且k2x+k2y+k2z=1?？梢宰C明, 其旋轉(zhuǎn)齊次變換矩陣為10000cos)cos1 (sin)cos1 (sin)cos1 (0sin)cos1 (cos)cos1 (sin)cos1 (0sin)cos1 (sin)cos1 (cos)cos1 (),(Rotzzxzyyzxxyzyyzyxyxzzxyxxkkkkkkkkkkkkkkkkkkkkkkkkk(3.24) 注: 該式為一般旋轉(zhuǎn)齊次變換通式, 概括了繞X、Y、Z軸進(jìn)行旋轉(zhuǎn)變換的情況。反之,當(dāng)給出某個(gè)旋轉(zhuǎn)齊次變換矩陣, 則可求得k及轉(zhuǎn)角。 變換算子公式不僅適用于點(diǎn)的旋轉(zhuǎn), 也適用于矢量、 坐標(biāo)

11、系、 物體的旋轉(zhuǎn)。 左乘是相對(duì)固定坐標(biāo)系的變換; 右乘是相對(duì)動(dòng)坐標(biāo)系的變換。 圖 3.8 點(diǎn)的一般旋轉(zhuǎn)變換 3.1.3 工業(yè)機(jī)器人的連桿參數(shù)和齊次變換矩陣工業(yè)機(jī)器人的連桿參數(shù)和齊次變換矩陣 1. 連桿參數(shù)及連桿坐標(biāo)系的建立連桿參數(shù)及連桿坐標(biāo)系的建立以機(jī)器人手臂的某一連桿為例。如圖3.9所示,連桿n兩端有關(guān)節(jié)n和n+1。描述該連桿可以通過(guò)兩個(gè)幾何參數(shù): 連桿長(zhǎng)度和扭角。由于連桿兩端的關(guān)節(jié)分別有其各自的關(guān)節(jié)軸線,通常情況下這兩條軸線是空間異面直線, 那么這兩條異面直線的公垂線段的長(zhǎng)an即為連桿長(zhǎng)度，這兩條異面直線間的夾角n即為連桿扭角。 圖 3.9 連桿的幾何參數(shù) 如圖3.10所示,相鄰桿件n與n

12、-1的關(guān)系參數(shù)可由連桿轉(zhuǎn)角和連桿距離描述。沿關(guān)節(jié)n軸線兩個(gè)公垂線間的距離dn即為連桿距離; 垂直于關(guān)節(jié)n軸線的平面內(nèi)兩個(gè)公垂線的夾角n即為連桿轉(zhuǎn)角。 圖 3.10 連桿的關(guān)系參數(shù) 這樣, 每個(gè)連桿可以由四個(gè)參數(shù)來(lái)描述,其中兩個(gè)是連桿尺寸, 兩個(gè)表示連桿與相鄰連桿的連接關(guān)系。當(dāng)連桿n旋轉(zhuǎn)時(shí), n隨之改變, 為關(guān)節(jié)變量,其它3個(gè)參數(shù)不變;當(dāng)連桿進(jìn)行平移運(yùn)動(dòng)時(shí),dn隨之改變, 為關(guān)節(jié)變量,其它3個(gè)參數(shù)不變。確定連桿的運(yùn)動(dòng)類型, 同時(shí)根據(jù)關(guān)節(jié)變量即可設(shè)計(jì)關(guān)節(jié)運(yùn)動(dòng)副,從而進(jìn)行整個(gè)機(jī)器人的結(jié)構(gòu)設(shè)計(jì)。已知各個(gè)關(guān)節(jié)變量的值, 便可從基座固定坐標(biāo)系通過(guò)連桿坐標(biāo)系的傳遞, 推導(dǎo)出手部坐標(biāo)系的位姿形態(tài)。 建立連桿坐

13、標(biāo)系的規(guī)則如下: 連桿n坐標(biāo)系的坐標(biāo)原點(diǎn)位于n+1關(guān)節(jié)軸線上,是關(guān)節(jié)n+1的關(guān)節(jié)軸線與n和n+1關(guān)節(jié)軸線公垂線的交點(diǎn)。 Z軸與n+1關(guān)節(jié)軸線重合。 X軸與公垂線重合;從n指向n+1關(guān)節(jié)。 Y軸按右手螺旋法則確定。 連桿參數(shù)與坐標(biāo)系的建立如表3.1所示。 表表3.1 連桿參數(shù)及坐標(biāo)系連桿參數(shù)及坐標(biāo)系 2. 連桿坐標(biāo)系之間的變換矩陣連桿坐標(biāo)系之間的變換矩陣各連桿坐標(biāo)系建立后,n1系與n系間變換關(guān)系可用坐標(biāo)系的平移、旋轉(zhuǎn)來(lái)實(shí)現(xiàn)。從n1到n系的變換步驟如下: (1) 令n1系繞Zn-1軸旋轉(zhuǎn)n角, 使Xn1與Xn平行, 算子為Rot(z,n)。(2) 沿Zn 1軸平移dn, 使Xn 1與Xn重合, 算

14、子為Trans(0,0,dn)。(3) 沿Xn軸平移an, 使兩個(gè)坐標(biāo)系原點(diǎn)重合, 算子為Trans(an,0,0)。(4) 繞Xn軸旋轉(zhuǎn)an角, 使得n1系與n系重合, 算子為Rot(x,n)。 該變換過(guò)程用一個(gè)總的變換矩陣An來(lái)表示連桿n的齊次變換矩陣為: 1000cossin0sinsincoscoscossincossinsincossincos10000cossin00sincos000011000010000100011000100001000011000010000cossin00sincos) 4() 3() 2() 1 (),0,0)Rot(rans(), 0 , 0(Tra

15、ns),(RotnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnndadaxaTdzA 實(shí)際中,多數(shù)機(jī)器人連桿參數(shù)取特殊值,如an=0或dn=0, 可以使計(jì)算簡(jiǎn)單且控制方便。3.1.4 工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)方程工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)方程1. 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程通常把描述一個(gè)連桿坐標(biāo)系與下一個(gè)連桿坐標(biāo)系間相對(duì)關(guān)系的齊次變換矩陣叫Ai變換矩陣, 簡(jiǎn)稱Ai矩陣。如A1矩陣表示第一個(gè)連桿坐標(biāo)系相對(duì)固定坐標(biāo)系的位姿;A2矩陣表示第二個(gè)連桿坐標(biāo)系相對(duì)第一個(gè)連桿坐標(biāo)系的位姿; Ai表示第i個(gè)連桿相對(duì)于第i-1個(gè)連桿的位姿變換矩陣。那么, 第二個(gè)連桿坐標(biāo)系在固定坐標(biāo)系中的位姿可用A1和A

16、2的乘積來(lái)表示,即: T2=A1A2 (3.26) 依此類推, 對(duì)于六連桿機(jī)器人, 有下列矩陣: T6=A1A2A3A4A5A6 (3.27) 該等式稱為機(jī)器人運(yùn)動(dòng)學(xué)方程。方程右邊為從固定參考系到手部坐標(biāo)系的各連桿坐標(biāo)系之間變換矩陣的連乘;方程左邊T6表示這些矩陣的乘積,即機(jī)器人手部坐標(biāo)系相對(duì)于固定參考系的位姿。 分析該矩陣: 前三列表示手部的姿態(tài); 第四列表示手部中心點(diǎn)的位置。 可寫成如下形式: 100010006zzzzyyyyxxxxnnpaonpaonpaonpRT(3.28) 2. 2. 正向運(yùn)動(dòng)學(xué)及實(shí)例正向運(yùn)動(dòng)學(xué)及實(shí)例如圖3.11所示,SCARA裝配機(jī)器人的3個(gè)關(guān)節(jié)軸線是相互平行的

17、, 0、1、2、3分別表示固定坐標(biāo)系、 連桿1的動(dòng)坐標(biāo)系、連桿2的動(dòng)坐標(biāo)系、 連桿3的動(dòng)坐標(biāo)系, 分別坐落在關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3和手部中心。坐標(biāo)系3即為手部坐標(biāo)系。 連桿運(yùn)動(dòng)為旋轉(zhuǎn)運(yùn)動(dòng), 連桿參數(shù)n為變量, 其余參數(shù)均為常量。 該機(jī)器人的參數(shù)如表3.2所示。 圖 3.11 SCARA裝配機(jī)器人的坐標(biāo)系 表表3.2 SCARA裝配機(jī)器人連桿參數(shù)裝配機(jī)器人連桿參數(shù) 該平面關(guān)節(jié)型機(jī)器人的運(yùn)動(dòng)學(xué)方程為 T3=A1A2A3 (3.29) 其中:A A1連桿1的坐標(biāo)系相對(duì)于固定坐標(biāo)系的齊次變換矩陣; A A2連桿2的坐標(biāo)系相對(duì)于連桿1坐標(biāo)系的齊次變換矩陣; A3手部坐標(biāo)系相對(duì)于連桿2坐標(biāo)系的齊次變換矩陣

18、。 ,0,0)Trans(,Rot(,0,0)Trans(,Rot(,0,0)Trans(,Rot(332322121101lzlzlzAAA(3.30) (3.31) (3.32) T3為手部坐標(biāo)系(即手部)的位姿。由于其可寫成(44)的矩陣形式, 即可得向量p、n、o、a, 把1、2、3代入可得。 如圖3.11(b)所示,當(dāng)轉(zhuǎn)角變量分別為1=30, 2=-60, 3=-30時(shí)，則可根據(jù)平面關(guān)節(jié)型機(jī)器人運(yùn)動(dòng)學(xué)方程求解出運(yùn)動(dòng)學(xué)正解,即手部的位姿矩陣表達(dá)式 0000010032.1705 . 0866. 02 .1830866. 05 . 03T(3.33) 3. 3. 反向運(yùn)動(dòng)學(xué)及實(shí)例反向運(yùn)動(dòng)

19、學(xué)及實(shí)例 反向運(yùn)動(dòng)學(xué)解決的問(wèn)題是:已知手部的位姿,求各個(gè)關(guān)節(jié)的變量。在機(jī)器人的控制中,往往已知手部到達(dá)的目標(biāo)位姿,需要求出關(guān)節(jié)變量,以驅(qū)動(dòng)各關(guān)節(jié)的電機(jī),使手部的位姿得到滿足, 這就是運(yùn)動(dòng)學(xué)的反向問(wèn)題,也稱逆運(yùn)動(dòng)學(xué)。 如圖3.12所示,以6自由度斯坦福(STANFORD)機(jī)器人為例, 其連桿坐標(biāo)系如圖3.13 所示, 設(shè)坐標(biāo)系6與坐標(biāo)系5原點(diǎn)重合, 其運(yùn)動(dòng)學(xué)方程為: T6=A1A2A3A4A5A6 (3.34) 圖 3.12 斯坦福(STANFORD)機(jī)器人現(xiàn)在給出T6矩陣及各桿參數(shù)a、d,求關(guān)節(jié)變量16, 其中3=d3。 其中, A1為坐標(biāo)系1，相當(dāng)于固定坐標(biāo)系O的Z0軸旋轉(zhuǎn)1,然后繞自身坐標(biāo)

20、系X1軸做1的旋轉(zhuǎn)變換,1=90, 所以 100000100cos0sin0sin-0cos),)Rot(,Rot(z111111101xA(3.35) 只要列出A-11,在式(3.34)兩邊分別左乘運(yùn)動(dòng)學(xué)方程, 即可得 65432611AAAAATA展開方程兩邊矩陣, 對(duì)應(yīng)項(xiàng)相等, 即可求得1, 同理可順次求得2、3、6等。 圖3.13斯坦福（STANFORD）機(jī)器人的連桿坐標(biāo)系3.2 工業(yè)機(jī)器人的動(dòng)力學(xué)工業(yè)機(jī)器人的動(dòng)力學(xué)3.2.1 工業(yè)機(jī)器人速度分析工業(yè)機(jī)器人速度分析 1. 工業(yè)機(jī)器人速度雅可比矩陣工業(yè)機(jī)器人速度雅可比矩陣數(shù)學(xué)上, 雅可比矩陣(Jacobian Matrix)是一個(gè)多元函數(shù)

21、的偏導(dǎo)矩陣。假設(shè)有六個(gè)函數(shù), 每個(gè)函數(shù)有六個(gè)變量, 即 ),(),(),(654321666543212265432111xxxxxxfyxxxxxxfyxxxxxxfy(3.36) 可寫成 Y=F(X) 將其微分, 得 666226116666222211226612211111ddddddddddddxxfxxfxxfyxxfxxfxxfyxxfxxfxxfy(3.37) 可簡(jiǎn)寫成 xXFYdd式中, (66)矩陣稱為雅可比矩陣。 XF對(duì)于工業(yè)機(jī)器人速度分析和靜力分析中遇到類似的矩陣, 我們稱為機(jī)器人的雅可比矩陣, 簡(jiǎn)稱雅可比。以2自由度平面關(guān)節(jié)機(jī)器人為例,如圖3.14所示,機(jī)器人的手部坐

22、標(biāo)(x,y)相對(duì)于關(guān)節(jié)變量(1,2)有 22112211sinsincoscosllyllx(3.38) 即 ),(),(2121yyxx(3.39) 求微分有 22112211ddddddyyyxxx(3.40) 寫成矩陣為 212121ddddyyxxyx(3.41) 令 1121yyxxJ(3.42) 則式(3.41)可簡(jiǎn)寫為 dX=J d 其中, 21dddddd,Xyx圖3.14 二自由度平面關(guān)節(jié)機(jī)器人由此可求得 1221221112212211clclclslslslJ(3.43) 對(duì)于n自由度機(jī)器人,關(guān)節(jié)變量q=q1q2qnT,當(dāng)關(guān)節(jié)為轉(zhuǎn)動(dòng)關(guān)節(jié)時(shí),qi=i; 當(dāng)關(guān)節(jié)為移動(dòng)關(guān)節(jié)時(shí),

23、qi=di,則dq=dq1dq2dqnT反映關(guān)節(jié)空間的微小運(yùn)動(dòng)。由X=X(q)可知, dX=J(q)dq 其中J(q)是(6n)的偏導(dǎo)數(shù)矩陣, 稱為n自由度機(jī)器人速度雅可比矩陣。 (3.44) 2. 2. 工業(yè)機(jī)器人速度分析工業(yè)機(jī)器人速度分析把式(3.44)兩邊各除以dt, 得 其中: V機(jī)器人末端在操作空間中的廣義速度,V=X; J(q)速度雅可比矩陣; q機(jī)器人關(guān)節(jié)在關(guān)節(jié)空間中的關(guān)節(jié)速度。 tqtddddq)J(X(3.45) 或 V=J(q) q (3.46) 若把J(q)矩陣的第1列與第2列矢量記為J1、J2,則有V=J11+J22,說(shuō)明機(jī)器人速度雅可比的每一列表示其它關(guān)節(jié)不動(dòng)而某一關(guān)

24、節(jié)運(yùn)動(dòng)時(shí)產(chǎn)生的端點(diǎn)速度。 2自由度手部速度為211221221112212211clclclslslslvvyxV(3.47) 若已知關(guān)節(jié)上1與2是時(shí)間的函數(shù),1=f1(t),2=f2(t), 則可求出該機(jī)器人手部在某一時(shí)刻的速度V=f(t), 即手部瞬時(shí)速度。反之,給定機(jī)器人手部速度,可由V=J(q)q解出相應(yīng)的關(guān)節(jié)速度, q=J-1V, 式中J-1為機(jī)器人逆速度雅可比矩陣。 . 逆速度雅可比J-1出現(xiàn)奇異解的情況如下: 工作域邊界上的奇異: 機(jī)器人手臂全部伸開或全部折回時(shí),叫奇異形位。該位置產(chǎn)生的解稱為工作域邊界上的奇異。 工作域內(nèi)部奇異: 機(jī)器人兩個(gè)或多個(gè)關(guān)節(jié)軸線重合引起的奇異。當(dāng)出現(xiàn)奇

25、異形位時(shí),會(huì)產(chǎn)生退化現(xiàn)象, 即在某空間某個(gè)方向(或子域)上, 不管機(jī)器人關(guān)節(jié)速度怎樣選擇, 手部也不可能動(dòng)。 3.2.2 工業(yè)機(jī)器人靜力學(xué)分析工業(yè)機(jī)器人靜力學(xué)分析 1. 1. 操作臂中的靜力操作臂中的靜力如已知外界環(huán)境對(duì)機(jī)器人最末桿的作用力和力矩, 則可以先分析最后一個(gè)連桿對(duì)上一個(gè)連桿的力和力矩， 依次遞推， 直到分析完第一個(gè)連桿對(duì)機(jī)座的力和力矩， 從而計(jì)算出每個(gè)連桿上的受力情況。 操作臂中單個(gè)桿件受力分析如圖3.15所示。 圖 3.15 桿i上的力和力矩 利用靜力平衡條件,桿上所受合力和合力矩為零。 為方便表示手部端點(diǎn)的力和力矩,可寫成一個(gè)6維矢量: 11nnnnnf,F 各關(guān)節(jié)驅(qū)動(dòng)器的驅(qū)動(dòng)

26、力或力矩可寫成一個(gè)n維矢量的形式, 即 n21其中: 關(guān)節(jié)力矩(或關(guān)節(jié)力)矢量; n關(guān)節(jié)的個(gè)數(shù)。 2. 2. 機(jī)器人力雅可比矩陣機(jī)器人力雅可比矩陣假定關(guān)節(jié)無(wú)摩擦, 忽略各桿件的重力, 則有 FJT(3.50) 其中: 廣義關(guān)節(jié)力矩; F F機(jī)器人手部端點(diǎn)力; JT(n6)階機(jī)器人力雅可比矩陣, 簡(jiǎn)稱力雅可比。 式(3.50)可用虛功原理證明。 證明: 如圖3.16所示, 各個(gè)關(guān)節(jié)的虛位移組成機(jī)器人關(guān)節(jié)虛位移矢量qi; 末端操作器的虛位移矢量為X, 由線虛位移d矢量和角虛位移矢量組成。 zyxzyxddddX(3.51) q=q1 q2 qnT (3.52) 圖3.16關(guān)節(jié)及末端操作虛位移設(shè)發(fā)生

27、上述虛位移時(shí), 各關(guān)節(jié)力為i(i=1, 2, ，n), 環(huán)境作用在機(jī)器人手部端點(diǎn)上的力和力矩分別為fn,n+1和nn,n+1, 由上述力和力矩所做的虛功可以由下式求出: W=1q1+2q2+nqnfn,n+1dnn,n+1 (3.53) 或?qū)懗?W=TqFTX (3.54) 據(jù)虛位移原理,機(jī)器人處于平衡狀態(tài)的充分必要條件是對(duì)任意的符合幾何約束的虛位移, 有W=0, 又因dX=Jdq, 代入得 W=TqFTX=TqFTJq=(JTF)Tq (3.55) 式中, q表示幾何上允許位移的關(guān)節(jié)獨(dú)立變量, 對(duì)任意的q, 欲使W=0成立, 必有 =JTF (3.56) 式中,JT與手部端點(diǎn)力和廣義關(guān)節(jié)力矩

28、之間的力傳遞有關(guān),稱為機(jī)器人力雅克比。 機(jī)器人力雅克比正好是速度雅克比的轉(zhuǎn)置。 3. 機(jī)器人靜力計(jì)算的兩類問(wèn)題機(jī)器人靜力計(jì)算的兩類問(wèn)題從操作臂手部端點(diǎn)力F與廣義關(guān)節(jié)力矩之間的關(guān)系式=JTF可知, 操作臂靜力計(jì)算可分為兩類: (1) 已知外界對(duì)手部作用力F,求滿足靜力平衡條件的關(guān)節(jié)驅(qū)動(dòng)力矩(=JTF)。(2) 已知關(guān)節(jié)驅(qū)動(dòng)力矩, 確定機(jī)器人手部對(duì)外界環(huán)境的作用力F或負(fù)荷質(zhì)量(逆解,即求解F=(JT)1)。當(dāng)自由度n6時(shí),力雅可比可能不是方陣,JT沒(méi)有逆解, 一般情況下不一定能得到惟一的解。 3.2.3 工業(yè)機(jī)器人動(dòng)力學(xué)分析工業(yè)機(jī)器人動(dòng)力學(xué)分析1. 1. 動(dòng)力學(xué)分析的兩類問(wèn)題動(dòng)力學(xué)分析的兩類問(wèn)題工

29、業(yè)機(jī)器人動(dòng)力學(xué)分析的兩類問(wèn)題是：(1) 給出已知的軌跡點(diǎn)的關(guān)節(jié)變量、,即機(jī)器人的關(guān)節(jié)位置、速度和加速度,求相應(yīng)的關(guān)節(jié)力矩向量, 用以實(shí)現(xiàn)對(duì)機(jī)器人的動(dòng)態(tài)控制。 (2) 已知關(guān)節(jié)驅(qū)動(dòng)力矩,求機(jī)器人系統(tǒng)的相應(yīng)的各瞬時(shí)的運(yùn)動(dòng),用于模擬機(jī)器人運(yùn)動(dòng)。分析機(jī)器人動(dòng)力學(xué)的方法很多,有拉格朗日方法、牛頓-歐拉方法、高斯方法、凱恩方法等。其中, 拉格朗日方法不僅求解復(fù)雜的系統(tǒng)動(dòng)力學(xué)方程簡(jiǎn)單, 而且容易理解。 . 2. 拉格朗日方程拉格朗日方程 首先, 定義拉格朗日函數(shù)是一個(gè)機(jī)械系統(tǒng)的動(dòng)能EK和勢(shì)能EP之差, 即 L=EKEP (3.57) 由于系統(tǒng)的動(dòng)能EK是廣義關(guān)節(jié)變量qi和qi的函數(shù),系統(tǒng)勢(shì)能EP是qi的函數(shù)

30、, 因此,拉格朗日函數(shù)L也是qi和qi的函數(shù)。 機(jī)器人系統(tǒng)的拉格朗日方程為 .iiiqLqLtFddi=1,2,n(3.58) 其中, Fi是關(guān)節(jié)廣義驅(qū)動(dòng)力(對(duì)于移動(dòng)關(guān)節(jié)為驅(qū)動(dòng)力; 對(duì)于轉(zhuǎn)動(dòng)關(guān)節(jié)為驅(qū)動(dòng)力矩)。 那么,用拉格朗日法建立機(jī)器人動(dòng)力學(xué)方程的步驟如下所述: (1) 選取坐標(biāo)系, 選定獨(dú)立的廣義關(guān)節(jié)變量qi,i=1, 2, ,n; (2) 選定相應(yīng)的廣義力Fi; (3) 求出各構(gòu)件的動(dòng)能和勢(shì)能, 構(gòu)造拉格朗日函數(shù); (4) 代入拉格朗日方程求得機(jī)器人系統(tǒng)的動(dòng)力學(xué)方程。 3. 關(guān)節(jié)空間和操作空間動(dòng)力學(xué)關(guān)節(jié)空間和操作空間動(dòng)力學(xué)關(guān)節(jié)空間即n個(gè)自由度操作臂末端位姿X是由n個(gè)關(guān)節(jié)變量決定的,這n個(gè)

31、關(guān)節(jié)變量叫n維關(guān)節(jié)矢量q,q所構(gòu)成的空間稱為關(guān)節(jié)空間。 操作空間即末端操作器的作業(yè)是在直角坐標(biāo)空間中進(jìn)行的, 位姿X是在直角坐標(biāo)空間中描述的,這個(gè)空間叫操作空間。 關(guān)節(jié)空間動(dòng)力學(xué)方程為 G(q)qH(q,qD(q) (3.59) 其中, 21212121 q,q,q,對(duì)于n個(gè)關(guān)節(jié)的操作臂, D(q)是(nn)的正定對(duì)稱矩陣, 是q的函數(shù)。如圖3.17所示, 二自由度平面關(guān)節(jié)機(jī)器人有222221222221222221222122112pmcplpmcplpmcplplmpm)()()(D(q)(3.60) H(q,q)是(n1)離心力和哥氏力矢量, 二自由度平面關(guān)節(jié)機(jī)器人有 .21221221

32、22122222122splmsplmsplm)qH(q,(3.61) G(q)是(n1)的重力矢量,與操作臂的形位q有關(guān), 二自由度平面關(guān)節(jié)機(jī)器人有 gspmspmslmpm1222122211211)(G(q)(3.62) 圖 3.17 二自由度平面關(guān)節(jié)機(jī)器人 與關(guān)節(jié)空間動(dòng)力學(xué)方程相對(duì)應(yīng),在笛卡爾操作空間中,可用直角坐標(biāo)變量,即末端操作器的位姿矢量來(lái)表示機(jī)器人動(dòng)力學(xué)方程。 操作空間動(dòng)力學(xué)方程如下:(q)G)q(q,UX(q)MFxxx (3.63) 其中: Mx(q)操作空間的慣性矩陣; Ux(q,q)離心力和哥氏力矢量; Gx(q)重力矢量; F廣義操作力矢量。 .兩個(gè)空間之間的關(guān)系可由

33、以下三式求出: q(q)JqJ(q)XqJ(q)XF(q)J T(3.64) (3.65) (3.66) 3.3 運(yùn)動(dòng)軌跡規(guī)劃運(yùn)動(dòng)軌跡規(guī)劃 3.3.1 3.3.1 路徑和軌跡路徑和軌跡機(jī)器人的軌跡指操作臂在運(yùn)動(dòng)過(guò)程中的位移、速度和加速度。 路徑是機(jī)器人位姿的一定序列,而不考慮機(jī)器人位姿參數(shù)隨時(shí)間變化的因素。如圖3.18所示,如果有關(guān)機(jī)器人從A點(diǎn)運(yùn)動(dòng)到B點(diǎn), 再到C點(diǎn), 那么這中間位姿序列就構(gòu)成了一條路徑。而軌跡則與何時(shí)到達(dá)路徑中的每個(gè)部分有關(guān), 強(qiáng)調(diào)的是時(shí)間。因此, 圖中不論機(jī)器人何時(shí)到達(dá)B點(diǎn)和C點(diǎn),其路徑是一樣的,而軌跡則依賴于速度和加速度,如果機(jī)器人抵達(dá)B點(diǎn)和C點(diǎn)的時(shí)間不同, 則相應(yīng)的軌跡

34、也不同。我們的研究不僅要涉及機(jī)器人的運(yùn)動(dòng)路徑, 而且還要關(guān)注其速度和加速度。 圖 3.18 機(jī)器人在路徑上的依次運(yùn)動(dòng) 3.3.2 3.3.2 軌跡規(guī)劃軌跡規(guī)劃軌跡規(guī)劃是指根據(jù)作業(yè)任務(wù)要求確定軌跡參數(shù)并實(shí)時(shí)計(jì)算和生成運(yùn)動(dòng)軌跡。軌跡規(guī)劃的一般問(wèn)題有三個(gè): (1) 對(duì)機(jī)器人的任務(wù)進(jìn)行描述, 即運(yùn)動(dòng)軌跡的描述。(2) 根據(jù)已經(jīng)確定的軌跡參數(shù), 在計(jì)算機(jī)上模擬所要求的軌跡。(3) 對(duì)軌跡進(jìn)行實(shí)際計(jì)算,即在運(yùn)行時(shí)間內(nèi)按一定的速率計(jì)算出位置、速度和加速度,從而生成運(yùn)動(dòng)軌跡。 在規(guī)劃中,不僅要規(guī)定機(jī)器人的起始點(diǎn)和終止點(diǎn), 而且要給出中間點(diǎn)(路徑點(diǎn))的位姿及路徑點(diǎn)之間的時(shí)間分配, 即給出兩個(gè)路徑點(diǎn)之間的運(yùn)動(dòng)時(shí)間

35、。 軌跡規(guī)劃既可在關(guān)節(jié)空間中進(jìn)行, 即將所有的關(guān)節(jié)變量表示為時(shí)間的函數(shù),用其一階、二階導(dǎo)數(shù)描述機(jī)器人的預(yù)期動(dòng)作, 也可在直角坐標(biāo)空間中進(jìn)行,即將手部位姿參數(shù)表示為時(shí)間的函數(shù), 而相應(yīng)的關(guān)節(jié)位置、 速度和加速度由手部信息導(dǎo)出。 以2自由度平面關(guān)節(jié)機(jī)器人為例解釋軌跡規(guī)劃的基本原理。 如圖3.19所示,要求機(jī)器人從A點(diǎn)運(yùn)動(dòng)到B點(diǎn)。 機(jī)器人在A點(diǎn)時(shí)形位角為=20,=30; 達(dá)到B點(diǎn)時(shí)的形位角是=40,=80。兩關(guān)節(jié)運(yùn)動(dòng)的最大速率均為10/s。當(dāng)機(jī)器人的所有關(guān)節(jié)均以最大速度運(yùn)動(dòng)時(shí),下方的連桿將用2s到達(dá), 而上方的連桿還需再運(yùn)動(dòng)3s,可見路徑是不規(guī)則的,手部掠過(guò)的距離點(diǎn)也是不均勻的。 圖 3.19 2自

36、由度機(jī)器人關(guān)節(jié)空間的非歸一化運(yùn)動(dòng) 設(shè)機(jī)器人手臂兩個(gè)關(guān)節(jié)的運(yùn)動(dòng)用有關(guān)公共因子做歸一化處理,使手臂運(yùn)動(dòng)范圍較小的關(guān)節(jié)運(yùn)動(dòng)成比例的減慢,這樣,兩個(gè)關(guān)節(jié)就能夠同步開始和結(jié)束運(yùn)動(dòng), 即兩個(gè)關(guān)節(jié)以不同速度一起連續(xù)運(yùn)動(dòng), 速率分別為4/s和10/s。如圖3.20所示為該機(jī)器人兩關(guān)節(jié)運(yùn)動(dòng)軌跡, 與前面的不同, 其運(yùn)動(dòng)更加均衡, 且實(shí)現(xiàn)了關(guān)節(jié)速率歸一化。 圖 3.20 2自由度機(jī)器人關(guān)節(jié)空間的歸一化運(yùn)動(dòng) 如果希望機(jī)器人的手部可以沿AB這條直線運(yùn)動(dòng), 最簡(jiǎn)單的方法是將該直線等分為幾部分(圖3.21中分成5份), 然后計(jì)算出各個(gè)點(diǎn)所需的形位角和的值, 這一過(guò)程稱為兩點(diǎn)間的插值。 可以看出,這時(shí)路徑是一條直線, 而形

37、位角變化并不均勻。很顯然, 如果路徑點(diǎn)過(guò)少, 將不能保證機(jī)器人在每一小段內(nèi)的嚴(yán)格直線軌跡, 因此,為獲得良好的沿循精度, 應(yīng)對(duì)路徑進(jìn)行更加細(xì)致的分割。由于對(duì)機(jī)器人軌跡的所有運(yùn)動(dòng)段的計(jì)算均基于直角坐標(biāo)系, 因此該法屬直角坐標(biāo)空間的軌跡規(guī)劃。 圖 3.21 2自由度機(jī)器人直角坐標(biāo)空間的運(yùn)動(dòng) 3.3.3 關(guān)節(jié)空間的軌跡規(guī)劃關(guān)節(jié)空間的軌跡規(guī)劃1. 三次多項(xiàng)式軌跡規(guī)劃三次多項(xiàng)式軌跡規(guī)劃假設(shè)機(jī)器人的初始位姿是已知的,通過(guò)求解逆運(yùn)動(dòng)學(xué)方程可以求得機(jī)器人期望的手部位姿對(duì)應(yīng)的形位角。若考慮其中某一關(guān)節(jié)的運(yùn)動(dòng)開始時(shí)刻ti的角度為i, 希望該關(guān)節(jié)在時(shí)刻tf運(yùn)動(dòng)到新的角度f(wàn)。軌跡規(guī)劃的一種方法是使用多項(xiàng)式函數(shù)以使得初

38、始和末端的邊界條件與已知條件相匹配,這些已知條件為i和f及機(jī)器人在運(yùn)動(dòng)開始和結(jié)束時(shí)的速度,這些速度通常為0或其他已知值。這四個(gè)已知信息可用來(lái)求解下列三次多項(xiàng)式方程中的四個(gè)未知量: 332210)(tctctcct(3.67) 這里初始和末端條件是: 0)(0)()()(fiffiitttt(3.68) 對(duì)式(3.67)求一階導(dǎo)數(shù)得到: 232132)(tctcct(3.69) 將初始和末端條件代入式(3.67)和(3.69)得到: 032)(0)()()(232113321100fffiffffiitctcctcttctctcctct通過(guò)聯(lián)立求解這四個(gè)方程, 得到方程中的四個(gè)未知的數(shù)值, 便可

39、算出任意時(shí)刻的關(guān)節(jié)位置, 控制器則據(jù)此驅(qū)動(dòng)關(guān)節(jié)所需的位置。 盡管每一關(guān)節(jié)是用同樣步驟分別進(jìn)行軌跡規(guī)劃的, 但是所有關(guān)節(jié)從始至終都是同步驅(qū)動(dòng)。如果機(jī)器人初始和末端的速率不為零, 則同樣可以通過(guò)給定數(shù)據(jù)得到未知的數(shù)值。 2. 2. 拋物線過(guò)渡的線性運(yùn)動(dòng)軌跡拋物線過(guò)渡的線性運(yùn)動(dòng)軌跡在關(guān)節(jié)空間進(jìn)行軌跡規(guī)劃的另一種方法是讓機(jī)器人關(guān)節(jié)以恒定速度在起點(diǎn)和終點(diǎn)位置之間運(yùn)動(dòng),軌跡方程相當(dāng)于一次多項(xiàng)式,其速度是常數(shù), 加速度為零。這表示在運(yùn)動(dòng)段的起點(diǎn)和終點(diǎn)的加速度必須為無(wú)窮大，才能在邊界點(diǎn)瞬間產(chǎn)生所需的速度。為避免這一現(xiàn)象出現(xiàn),線性運(yùn)動(dòng)段在起點(diǎn)和終點(diǎn)處可以用拋物線來(lái)進(jìn)行過(guò)渡,從而產(chǎn)生連續(xù)位置和速度, 如圖3.22所示。圖 3.22 拋物線過(guò)渡的線性段規(guī)劃方法 假設(shè)ti=0和tf時(shí)刻對(duì)應(yīng)的起點(diǎn)和終點(diǎn)位置為i和f,拋物線與直線部分的過(guò)渡段在時(shí)間tb和tf-tb處是對(duì)稱的, 得到: 2212210)()(21)(cttccttctcct (3.71) 顯然,這時(shí)拋物線運(yùn)動(dòng)段的加速度是一個(gè)常數(shù), 并在公共點(diǎn)A和B(稱這些點(diǎn)為節(jié)點(diǎn))上產(chǎn)生連續(xù)的速度。 將邊界條件代入拋物線段的方程, 得到: 210)(0)0()0(ctcci (3.72) 整理得 2100ccci(3.73) 從而簡(jiǎn)化拋物線段的方程為 2222)()(21)(cttcttcti 顯然,對(duì)于直線