第2章工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)

1、注：1） 2008年春季講課用；2）帶下劃線的黑體字為板書內(nèi)容；3）公式及帶波浪線的部分為必講內(nèi)容第2章工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)2.1引言通過上一章的學(xué)習(xí)我們知道，從機(jī)構(gòu)學(xué)的角度看，工業(yè)機(jī)器人可以認(rèn)為是用一系列關(guān)節(jié)連接起來的連桿所組成的開鏈機(jī)構(gòu) 。工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)研究的是各連桿之間的位移關(guān) 系、速度關(guān)系和加速度關(guān)系。本章僅研究位移關(guān)系，重點(diǎn)是研究手部相對于機(jī)座的位姿和 各連桿之間的相互關(guān)系?！拔蛔恕笔恰拔恢煤妥藨B(tài)”的簡稱。工業(yè)機(jī)器人手部相對于機(jī)座的位姿和工業(yè)機(jī)器人各連桿之間的相互關(guān)系直接相關(guān)。為 了便于數(shù)學(xué)上的分析，一般選定一個(gè)和機(jī)座固聯(lián)的坐標(biāo)系，稱為固定坐標(biāo)系，并為每一個(gè) 連桿（包括手部）選定一個(gè)和

2、之固聯(lián)的坐標(biāo)系，稱為連桿坐標(biāo)系。一般把機(jī)座也視為一個(gè) 連桿，即零號(hào)連桿。這樣，連桿之間的相互關(guān)系可以用連桿坐標(biāo)系之間的相互關(guān)系來描述。 工業(yè)機(jī)器人手部相對機(jī)座的位姿就是固聯(lián)在手部的坐標(biāo)系相對固定坐標(biāo)系的位姿。工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)主要包括正向運(yùn)動(dòng)學(xué)和反向運(yùn)動(dòng)學(xué)兩類問題。正向運(yùn)動(dòng)學(xué)是在已知 各個(gè)關(guān)節(jié)變量的前提下，解決如何建立工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)方程，以及如何求解手部相對固 定坐標(biāo)系位姿的問題。反向運(yùn)動(dòng)學(xué)則是在已知手部要到達(dá)目標(biāo)位姿的前提下，解決如何求 出關(guān)節(jié)變量的問題。反向運(yùn)動(dòng)學(xué)也稱為求運(yùn)動(dòng)學(xué)逆解 。在工業(yè)機(jī)器人控制中，先根據(jù)工作任務(wù)的要求確定手部要到達(dá)的目標(biāo)位姿，然后根據(jù) 反向運(yùn)動(dòng)學(xué)求出關(guān)節(jié)變量，控制器

3、以求出的關(guān)節(jié)變量為目標(biāo)值，對各關(guān)節(jié)的驅(qū)動(dòng)元件發(fā)出 控制命令，驅(qū)動(dòng)關(guān)節(jié)運(yùn)動(dòng)，使手部到達(dá)并呈現(xiàn)目標(biāo)位姿。可見，工業(yè)機(jī)器人反向運(yùn)動(dòng)學(xué)是 工業(yè)機(jī)器人控制的基礎(chǔ)。在后面的介紹中我們會(huì)發(fā)現(xiàn)，正向運(yùn)動(dòng)學(xué)又是反向運(yùn)動(dòng)學(xué)的基礎(chǔ)。工業(yè)機(jī)器人相鄰連桿之間的相對運(yùn)動(dòng)不是旋轉(zhuǎn)運(yùn)動(dòng)，就是平移運(yùn)動(dòng)，這種運(yùn)動(dòng)體現(xiàn)在 連接兩個(gè)連桿的關(guān)節(jié)上。物理上的旋轉(zhuǎn)運(yùn)動(dòng)或平移運(yùn)動(dòng)在數(shù)學(xué)上可以用矩陣代數(shù)來表達(dá)， 這種表達(dá)稱之為坐標(biāo)變換。和旋轉(zhuǎn)運(yùn)動(dòng)對應(yīng)的是旋轉(zhuǎn)變換，和平移運(yùn)動(dòng)對應(yīng)的是平移變換。 坐標(biāo)系之間的運(yùn)動(dòng)關(guān)系可以用矩陣之間的乘法運(yùn)算來表達(dá)。用坐標(biāo)變換來描述坐標(biāo)系（剛 體）之間的運(yùn)動(dòng)關(guān)系是工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)分析的基礎(chǔ)。在工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)分析中

4、要注意下面四個(gè)問題 ：1）工業(yè)機(jī)器人操作臂可以看成是一個(gè)開式運(yùn)動(dòng)鏈，它是由一系列連桿通過轉(zhuǎn)動(dòng)或移 動(dòng)關(guān)節(jié)串聯(lián)起來的。開鏈的一端固定在機(jī)座上，另一端是自由的。自由端安裝著手爪（或 工具，統(tǒng)稱手部或末端執(zhí)行器），用以操作物體，完成各種作業(yè)。關(guān)節(jié)變量的改變導(dǎo)致連 桿的運(yùn)動(dòng)，從而導(dǎo)致手爪位姿的變化。2）在開鏈機(jī)構(gòu)簡圖中，關(guān)節(jié)符號(hào)只表示了運(yùn)動(dòng)關(guān)系。在實(shí)際結(jié)構(gòu)中，關(guān)節(jié)由驅(qū)動(dòng)器 驅(qū)動(dòng)，驅(qū)動(dòng)器一般要通過減速裝置（如用電機(jī)或馬達(dá)驅(qū)動(dòng)）或機(jī)構(gòu)（如用油缸驅(qū)動(dòng)）來驅(qū) 動(dòng)操作臂運(yùn)動(dòng)，實(shí)現(xiàn)要求的關(guān)節(jié)變量。3）為了研究操作臂各連桿之間的位移關(guān)系，可在每個(gè)連桿上固聯(lián)一個(gè)坐標(biāo)系，然后描述這些坐標(biāo)系之間的關(guān)系。Denavit和H

5、artenberg提出一種通用的方法，用一個(gè) 4X4的 齊次變換矩陣描述相鄰兩連桿的空間關(guān)系，從而推導(dǎo)出“手部坐標(biāo)系”相對于“固定坐標(biāo) 系”的齊次變換矩陣，建立操作臂的運(yùn)動(dòng)方程。4）在軌跡規(guī)劃時(shí)，人們最感興趣的是手部相對于固定坐標(biāo)系的位姿。22齊次坐標(biāo)及對象物的描述齊次變換具有較直觀的幾何意義，非常適合描述坐標(biāo)系之間的變換關(guān)系。另外，齊次 變換可以將旋轉(zhuǎn)變換和平移變換用一個(gè)矩陣來表達(dá)，關(guān)系明確，表達(dá)簡潔。所以常用于解 決工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)問題。下面我們先介紹有關(guān)齊次坐標(biāo)和齊次變換的內(nèi)容。點(diǎn)的位置描述如圖2-1所示，在選定的三維空間直角坐標(biāo)系A(chǔ)中，空間任一點(diǎn)P的坐標(biāo)可以用一 個(gè)（3X 1）列陣（或

6、稱三維列向量）Ap表示，即：XyZ 一式中：x，y，z是點(diǎn)P在坐標(biāo)系A(chǔ)中的三個(gè)坐標(biāo)分量；AP的左上標(biāo)A代表選定的參考坐標(biāo)系。齊次坐標(biāo)如果用四個(gè)數(shù)組成的（4X 1）列陣（或稱四維列向量） 表示三維空間直角坐標(biāo)系A(chǔ)中的點(diǎn)P,即：_x1yzj則定義列陣x y z 1T為三維空間點(diǎn)P的齊次坐標(biāo)(2-1)(2-2)AO-P(x，y，z)圖2-1點(diǎn)的位置描述w后,式中：x= a/w，坐標(biāo)軸的描述o(2-3)必須注意，齊次坐標(biāo)的表示不是唯一的。如果將列陣p中的元素同乘一非零系數(shù) 仍然代表同一點(diǎn)P,即：的方向列陣。TTT匸1 j = 0 k= 0h的方向列陣為:h= a b c 0T= cos： cos： c

7、os 0T如圖2-2所示，i、j、k分別是直角坐標(biāo)系中X、丫、Z坐標(biāo)軸的單位矢量，若用齊次 坐標(biāo)來描述X、丫、Z軸，則定義下面三個(gè)（4X1）列陣分別為單 位矢量i、j、k （即X、丫、Z坐標(biāo)軸）001圖2-2中所示矢量v的單位矢量丨（2-4）式中，：、分別是矢量v和坐標(biāo)軸X、丫、Z的夾角，0 Y、Z坐標(biāo)軸的單位方向矢量，每個(gè) 單位方向矢量在固定坐標(biāo)系上的分量為動(dòng)坐標(biāo)系該坐標(biāo) 軸的方向余弦，用齊次坐標(biāo)列陣分別表示為：iz 0 = cos x oz 0T = coSY az 0 = cos r-zZQYo(xo, y,(2-5)OYX圖2-4剛體的位置和姿態(tài)O ,建立和剛體固連的坐標(biāo)系zn= ix

8、 o= ox Oy a= ax aycos：xcosycoszcos Xcos Ycos Z式中：:X、-Y、: Z分別為X、Y、Z坐標(biāo)軸和X坐標(biāo)軸的夾角;iy00T0T(2-6):X、X、因此，Py、Bz分別為X 丫 Z坐標(biāo)軸和丫坐標(biāo)軸的夾角；Y、Z分別為X、丫、Z 坐標(biāo)軸和Z坐標(biāo)軸的夾角。圖2-4中剛體的位姿可用下面的OxOyOz0T 二no a p二nx|nynz(4X 4)矩陣來描述:axayaz0X0y0Z01(2-7)很明顯，對剛體Q位姿的描述就是對固連于剛體 Q的坐標(biāo)系OXYN位姿L的描述 例2-2圖2-5表示固連于剛體的坐標(biāo)系B位于Ob點(diǎn)，Xb= 00，Oa30Xa圖2-5動(dòng)坐

9、標(biāo)系B的描述YaAxZb0.866-0.5000.00010.010.5000.8660.0006.00.0000.0001.0000.0-0001 一T 二0.0 1TB的位姿來表示，如圖2-6所z inXbOYbXB Zb圖2-6手部位置及姿態(tài)的描述軸和Za軸和紙面垂直，坐標(biāo)系B相對固定坐標(biāo)系A(chǔ)有一個(gè)30的偏轉(zhuǎn)，試寫出表示剛體 位姿的坐標(biāo)系B的(4X 4)矩陣表達(dá)式。解：Xb 的方向列陣：n cos30* cos60。cos90 0T 0.866 0.5 0.0 0T Yb 的方向列陣：o cos120 cos30 cos90 0T -0.5 0.866 0.0 0T Zb 的方向列陣：a

10、 cos90 cos90 cos0 0T 0.00.01.0 0T坐標(biāo)系B的位置列陣：p 10.0 6.0所以，坐標(biāo)系B的(4X4)矩陣表達(dá)式為：2)手部位姿的表示工業(yè)機(jī)器人手部的位姿也可以用固連于手部的坐標(biāo)系 示。坐標(biāo)系B可以這樣來確定：取手部的中心 點(diǎn)Ob為原點(diǎn)；關(guān)節(jié)軸為Zb軸，Zb軸的單位方向 矢量a稱為接近矢量，指向朝外；兩個(gè)手指的連 線為Yb軸，指向可任意選定，Yb軸的單位方向 矢量o稱為姿態(tài)矢量；Xb軸和Yb軸及Zb軸垂直， Xb軸的單位方向矢量n稱為法向矢量，且n o X a,指向符合右手法則。- f - - - - - L - 1 lxny2例2-3圖2-7表示手部抓握物體 手

11、部位姿的矩陣式。T 二no a p二OxOyOz0axayaz0PxPyPz1(2-8)Q，物體為邊長2個(gè)單位的正立方體，寫出表達(dá)該解：因?yàn)槲矬wQ的形心和手部坐標(biāo)系OXYZ，的坐標(biāo)原點(diǎn) 為正立方體的后下方左側(cè)頂點(diǎn)，所以手部位置的(4X 1)列陣為:p 1 1 1 1T設(shè)n、o、a為手部坐標(biāo)系三個(gè)坐標(biāo)軸的單位方向矢量，由圖 矢量n的方向角為：= 90:x 180矢量o的方向角為：:y 180-y 90矢量a的方向角為：:z 90z 90于是有：n cos： xo cos： Ya cos： zO重合，固定坐標(biāo)系原點(diǎn)O2-7可知:cosCOS：YCOSZ：x cos x 0T 0 -1 cos Y

12、0T -10cos Z0T 0 0x 90y 90z 18000-10T0T0T手部的位置矢量為固定坐標(biāo)系原點(diǎn)指向手 部坐標(biāo)系B原點(diǎn)的矢量p，手部的方向矢量為 n、o、a。于是，手部的位姿可用(4X 4)矩陣表示 為：根據(jù)式(2-8)可知，表達(dá)該手部位姿的矩陣式為:-0-1-100011T = n o a p=00-11-0001225目標(biāo)物的齊次矩陣表示設(shè)有一楔塊Q如圖2-8所示，坐標(biāo)系OXYZ為固定坐標(biāo)系，坐標(biāo)系OXYZ，為和楔塊 Q固連的動(dòng)坐標(biāo)系。在圖(a)情況下，動(dòng)坐標(biāo)系OXYZ和固定坐標(biāo)系OXYZ重合。楔塊Q 的位置和姿態(tài)可用6個(gè)點(diǎn)的齊次坐標(biāo)來描述，在圖(a)情況下，其矩陣表達(dá)式為：

13、ABCD1-1-11Q00000022E F1 -1440 01 1若讓楔塊ZQ先繞Z軸旋轉(zhuǎn)90。，再繞丫軸旋轉(zhuǎn)90，最后祈X軸方向平移4,則楔塊成為圖(b)之情況 式為：(1,0, 2)/c此時(shí)楔塊用新的6個(gè)點(diǎn)的齊次坐標(biāo)來描述它的位章和姿態(tài)，其矩陣表達(dá)O(O) V-Q E(1,4, 0)A(1,0, 0) 十 X(X )B(-1,0, 0)A:0LY(Y )_1CDE /6 6 4(Q畀,-1 0)穆(6, -0 O) 4 , 41 1 1一 1 1 X 1體的齊次矩表示2-8(b)直接寫出來的，后面講完齊次變換以后將會(huì)知道，這個(gè)矩陣 F(-1,4,/YA(4, 1,0)(a)圖2-8Q,1

14、, 0)(b)這個(gè)矩陣是根據(jù)圖可以由圖2-8(a)對應(yīng)的矩陣計(jì)算得到，見例2-7。11 11(2-9)F y010 AyyF z0 0 1AzzJ 一I0 0 0 1 一心 xx00 ：y,z) a約-1 A(x, y, z)(2-10)圖2-9點(diǎn)的平移變換卄2.3齊次變換及運(yùn)算岡H體的運(yùn)動(dòng)是由轉(zhuǎn)動(dòng)和平移組成的。為了能用同一矩陣表示轉(zhuǎn)動(dòng)和平移，有必要引入 (4X 4)的齊次坐標(biāo)變換矩陣。平移的齊次變換我們首先介紹點(diǎn)在空間直角坐標(biāo)系中的平移。如圖2-9所示，空間某一點(diǎn)A的坐標(biāo)為(x, y, z),當(dāng)它平移至A點(diǎn)后，坐標(biāo)為(x, y: z)，且有：x = x+=x y = y+ 二 y z = z

15、+ z或?qū)懗扇缦戮仃囆问剑阂部梢院唽憺椋篴 = Trans( ：x,Trans( ：x, y, ：z)二100衛(wèi)01000010LX 與Az1 一X, 丫 , Z(2-11)其中，第四列元素 x, ：y, z分別表示沿坐標(biāo)軸齊次坐標(biāo)變換的運(yùn)算規(guī)則：若算子左乘，表示坐標(biāo)變換是相對固定坐標(biāo)系進(jìn)行的；假如相對動(dòng)坐標(biāo)系進(jìn)行坐標(biāo)變換，則算子應(yīng)該右乘。平移齊次變換公式(2-10)同樣適用于坐標(biāo)系、物體等的變換，這時(shí)最右端為一個(gè)(4X n)的矩陣。對于坐標(biāo)系的變換，n=4;對于物體的變換，門=描述物體的頂點(diǎn)個(gè)數(shù)。例2-4圖2-10中有下面三種情況：1)動(dòng)坐標(biāo)系A(chǔ)相對于固定坐標(biāo)系作(-1 : 2, 2)平移后

16、到A ; 2)動(dòng)坐標(biāo)系A(chǔ)相對于自身坐標(biāo)系(即動(dòng)坐標(biāo)系)作(-1 , 2, 2)平移后到A ;3)物體Q相對于固定坐標(biāo)系作的移動(dòng)量。Zo試計(jì)X0丫0-100 系A(chǔ) Y、AX(2,6, 0)平移后到Q。已知:Zo-1007以及物體Q 的矩陣表達(dá)式。(3 6 1) *算子均為(1,3, 0) (3, 6, 1)00-10(1,們1(-1,0, 1)心込0110Tq TO0巳10) 1一1 1 11 -133(0, 6, :z動(dòng)坐標(biāo)系QA的兩個(gè)平移坐標(biāo)變換X0圖2-10坐標(biāo)系及物體的平移變換(3, 6, 0)(1,6, 0)(3, 9, 0)丫0,9, 0)式中，Trans( x, y, z)表示齊次

17、坐標(biāo)變換的平移算子, 且：100-n0102Trans(心x, Ay, Az)=0012001 一A 坐標(biāo)系是動(dòng)坐標(biāo)系A(chǔ)相對于固定坐標(biāo)系作平移變換得來的，變換算子應(yīng)該左乘,因此，A 的矩陣表達(dá)式為：-100-10-10們10-10 0A = Trans( -1, 2, 2) A =0102-1001=-100 3001200-1100-130001 一.0001_1000 1 一從這個(gè)(4 X 4)的矩陣可以看出，O在O0X0Y0Z0坐標(biāo)系中的坐標(biāo)為(0,3, 3)。A 坐標(biāo)系是動(dòng)坐標(biāo)系A(chǔ)相對于自身(動(dòng)坐標(biāo)系)作平移變換得來的，變換算子應(yīng) 該右乘，因此，A 的矩陣表達(dá)式為：-0-10m100-

18、n10-10-11A=A Trans(1,2, 2)=-10010102-100200-11001200-1-10001 一001 _10001 一從這個(gè)(4 X 4)的矩陣可以看出，O在O0X0Y0Z0坐標(biāo)系中的坐標(biāo)為(-1,2,-1)。物體Q的平移坐標(biāo)變換算子為:-100210106Trans(Ax, Ay, Az)=00100001 一變換算子應(yīng)該左乘，因此,Q相對于固定坐標(biāo)系做平移變換,10 0” 0 1 0 Q =Trans(2,6,0) Q =- 0 0 10 0 021-1-111-n600003300011001一111111 一Q 的矩陣表達(dá)式為：3113316 6 6 6

19、9 90 0 110 0.111111 經(jīng)過平移變換后，坐標(biāo)系A(chǔ) 、AJ以及物體Q的實(shí)際情況已圖解在圖2-10中了。我們可以根據(jù)所作的移動(dòng)，從圖中分析出 O在O0X0Y0Z0坐標(biāo)系中的坐標(biāo)。因?yàn)樽鴺?biāo) 系A(chǔ)的原點(diǎn)為(1，1，1)，當(dāng)它沿X0軸反向移動(dòng)1個(gè)單位后變?yōu)?1-1，1，1)，再沿丫。軸 正向移動(dòng)2個(gè)單位后變?yōu)?0，1+2，1)，最后再沿Z0軸正向移動(dòng)2個(gè)單位后就變?yōu)?0, 3, 1+2)，即(0, 3, 3)?？梢?，上面計(jì)算的結(jié)果和此相符。我們可以根據(jù)所作的移動(dòng)，從圖中分析出O在O0X0Y0Z0坐標(biāo)系中的坐標(biāo)。因?yàn)樽鴺?biāo)系A(chǔ)的原點(diǎn)為(1,1, 1),當(dāng)它沿X軸反向(即沿丫。軸正向)移動(dòng)1個(gè)

20、單位后變?yōu)?1, 1+1, 1),再沿丫軸正向(即沿X0軸反向)移動(dòng)2個(gè)單位后變?yōu)?1-2, 2, 1)，最后再沿Z軸正 向(即沿Z0軸反向)移動(dòng)2個(gè)單位后就變?yōu)?-1 , 2, 1-2),即(-1, 2,-1)?？梢?，上面計(jì) 算的結(jié)果和此相符。旋轉(zhuǎn)的齊次變換首先我們介紹點(diǎn)在空間直角坐標(biāo)系中的旋轉(zhuǎn)。如圖2-11所示，空間某一點(diǎn)A，坐標(biāo)為（x，y，z），當(dāng)它繞Z軸旋轉(zhuǎn)二角后至A點(diǎn)，坐標(biāo)為（x，y: z）k = cosT，x sin 日 y * y = sin 日，x +cos。y z z或用矩陣表示為：(2-12)A點(diǎn)和A點(diǎn)的坐標(biāo)關(guān)系為:* y=sin v0sin)cos v00x01A點(diǎn)和A點(diǎn)

21、的齊次坐標(biāo)分別為_1T，因此A點(diǎn)的旋轉(zhuǎn)齊次變換過程為:sin 日y一三x y z 1T和x y zr ysin日cos 600yF z0010zJ 一1 10001 一| cos0x0巧az A(x, y z)O(2-13)y yX圖2-11點(diǎn)的旋轉(zhuǎn)變換A(x，y，z)也可簡寫為:a = Rot（z,式中，Rot（z, R表示齊次坐標(biāo)變換時(shí)繞Z軸的旋轉(zhuǎn)算子,0010算子的內(nèi)容為:Rot(z,巧=CVs日0.0-s-0001001(2-14)(2-15)式中：ccost； S）= sin同理，可寫出繞X軸的旋轉(zhuǎn)算子和繞軸的旋轉(zhuǎn)算子，其內(nèi)容為:_1 00010 c日- s00 s日c日00 001

22、 一-c日0010100-s日0c。0-0001 一Rot(x)二Rot (y,可二(2-16)zAO(2-17)圖2-12所示為點(diǎn)A繞任意過原點(diǎn)的單位矢量k旋轉(zhuǎn)/角的 情況。kx，ky，kz分別為單位矢量k在固定坐標(biāo)系坐標(biāo)軸X、丫、 Z上的三個(gè)分量（方向余弦），且kx2+ky2+kz2= 1?？梢宰C得，繞任意過原點(diǎn)的單位矢量 k轉(zhuǎn)二角的旋轉(zhuǎn)齊次變換公式為:圖2-12 一般旋轉(zhuǎn)變換| kxkxvers J c) kxkyvers 日 + kzs9 kxkzvers日-kys9- 0式中；versr = (1-cosr)Rot(k,v)二kykxvers v - kzskykyversv cnk

23、ykzversv kxs)0kzkxVersr kyskzkyversv - kxsvkzkzversv cv001001(2-18)軸、丫軸、Z軸進(jìn)行的旋轉(zhuǎn)齊次變換是式(2-18)稱為一般旋轉(zhuǎn)齊次變換的通式，繞X其特殊情況，例如：當(dāng)kx= 1, ky= kz= 0時(shí)，即繞X軸旋轉(zhuǎn)，則由式(2-18)可得到式(2-16); 當(dāng)ky= 1, kx= kz= 0時(shí)，即繞丫軸旋轉(zhuǎn)，則由式(2-18)可得到式(2-17); 當(dāng)kz= 1，kx= ky= 0時(shí)，即繞Z軸旋轉(zhuǎn)，則由式(2-18)可得到式(2-15)。 反之，若給出某個(gè)旋轉(zhuǎn)齊次變換矩陣：|nynz則可根據(jù)式(2-18)求出其等效轉(zhuǎn)軸的單位矢

24、量OxOyOz0axayaz001001k及等效轉(zhuǎn)角匕計(jì)算公式為:.(Oz -ay)2 & -nz)2 血 -Ox)2nx +Oy +az _1kxkyOz - ay2 sin -3x 2z2 sin vkzny Ox2 sin 二式中：當(dāng)二取0到180之間的值時(shí)，式中的符號(hào)取+號(hào)。當(dāng)轉(zhuǎn)角幷艮小時(shí)，公式(2-19)很難確定轉(zhuǎn)軸。當(dāng)接近0 或180時(shí)，轉(zhuǎn)軸完全不確定。和平移變換一樣，旋轉(zhuǎn)變換算子公式(2-15)、(2-16) (2-17) 以及一般旋轉(zhuǎn)變換算子公式(2-18)，不僅適用于點(diǎn)的旋轉(zhuǎn)變 換，而且也適用于矢量、坐標(biāo)系、物體等的旋轉(zhuǎn)變換。若相對固定坐標(biāo)系進(jìn)行變換，則算子左乘；若相對動(dòng)坐標(biāo)

25、系進(jìn)行 變換，則算于右乘。例2-5已知坐標(biāo)系中點(diǎn) U的位置矢量u=73 21T，將此點(diǎn)繞Z軸旋轉(zhuǎn)90，再繞丫軸旋轉(zhuǎn)90，如圖2-13所示，求旋轉(zhuǎn)變換后所得的點(diǎn) W。解:001-1.0例2-6如圖2-14所示單臂操 作手，手腕也具有一個(gè)自由度。已知 手部起始位姿矩陣為：1000w = Rot( y, 90 ) Rot(z, 90 ) u010衛(wèi)00-102621若手臂繞Z0軸旋轉(zhuǎn)+90 ,則手部到 達(dá)G2;若手臂不動(dòng)，僅手部繞手腕 Z1軸旋轉(zhuǎn)+90，貝U手部到達(dá) G3。寫 出手部坐標(biāo)系G2及G3的矩陣表 達(dá)式。解：手臂繞Z0軸轉(zhuǎn)動(dòng)是相對固 定坐標(biāo)系作旋轉(zhuǎn)變換， 該左乘，即：01001000000

26、0110衛(wèi)0 721eYXflJ/圖2-14手臂轉(zhuǎn)動(dòng)和手腕轉(zhuǎn)動(dòng)W-0-1000102_一100-6Rot (z, 90 ) G1 =10001006=0102001000-1200-120001 一001 -10001 一所以，算子應(yīng)G 2 =-0102_10_ 100_10021Rot (z, 90 )=10061000_0_ 106001200100012-0001 _0001 _1 10001 _手部繞手腕乙軸旋轉(zhuǎn)是相對動(dòng)坐標(biāo)系作旋轉(zhuǎn)變換，所以，算子應(yīng)該右乘，即:G 3 二 G1講課時(shí)對照圖分析這兩個(gè)矩陣。平移加旋轉(zhuǎn)的齊次變換平移變換算子和旋轉(zhuǎn)變換算子可以組合在一個(gè)（4X 4）的矩陣中。

27、若例2-5中的點(diǎn)W還E要作4i-3j +7k的平移，如圖2-15所示，則只要再左乘上平移變換算子，即可得到最后 點(diǎn)的列陣表達(dá)式，即：e = Trans(4, -3, 7) Rot(y, 90 ) Rot(z, 90 ) u講課時(shí)分析一下其組成特點(diǎn)。例2-7圖2-8所示的楔塊Q,在圖(a)情況下描述它的齊次矩陣為:-1-1-111-nQ(a)=000044002200111111 一試證明楔塊經(jīng)過繞固定坐標(biāo)系 OXYZ的Z軸旋轉(zhuǎn)90 ,再繞丫軸旋轉(zhuǎn)90 ,最后沿X軸方 向平移4后見圖2-8(b)的齊次矩陣表達(dá)式為：-446644Q(b)=1-1-111-1000044111111證明：因?yàn)樾▔K從

28、圖(a)至圖(b)的所有變換都是相對于固定坐標(biāo)系OXYZ進(jìn)行的，所以各坐標(biāo)變換算子應(yīng)該依次左乘，即：Q(b)二=Trans(4, 0,0)3Rolt(y,290)R、ot(z,190)Q(a)-1004001010-1001j-1-111-T01000100100000004400101-100000100022000001 一.0001 一001 一J11111 一0014_1-1-1 11-1_446644 _10000000441-1-111-101000022000000440001_-111111 _I111111 一0 014式中：1 0 0 0即為楔塊平移加旋轉(zhuǎn)的復(fù)合變換矩陣0

29、10 0b 0 0 i一講課時(shí)分析一下其組成特點(diǎn)。證畢。旋量的概念旋量Screw（k, r,：:）表示沿k軸移動(dòng)r，并繞k軸轉(zhuǎn)動(dòng) 角的綜合齊次變換。 旋量Screw（k，r，）和移動(dòng)和轉(zhuǎn)動(dòng)發(fā)生的先后次序無關(guān)，只要它們連續(xù)即可，即：Screw（k, r，申）=Rot（k，）Trans（k，r）= Trans（k，r）Rot（k，）（2-19a）例如，沿Z軸移動(dòng)r，并繞Z軸轉(zhuǎn)動(dòng)角的綜合變換為：-s0010002-s0 0*100 0【c護(hù)-s半001sc000100sc000100sc000010001r001r001r0010001 一衛(wèi)001 _I0001 _10001 一001 一式中：c表

30、示 cos，s表示 sin。2.4工業(yè)機(jī)器人連桿參數(shù)及其齊次變換矩陣工業(yè)機(jī)器人可以認(rèn)為是一系列通過關(guān)節(jié)連接起來的連桿組成的開鏈機(jī)構(gòu)（P5圖1-4）手部相對固定坐標(biāo)系的位姿和各連桿之間的相互關(guān)系直接相關(guān)。因此，在研究手部相對于 機(jī)座的幾何關(guān)系時(shí)，首先要分析兩個(gè)相鄰連桿之間的關(guān)系，這種關(guān)系可以用固連于相鄰連 桿上的坐標(biāo)系之間的關(guān)系來描述。為此，我們首先必須建立連桿坐標(biāo)系。下面介紹一種由 Denavit和Hartenberg提出的通用方法，即 DH法。連桿參數(shù)及連桿坐標(biāo)系的建立工業(yè)機(jī)器人相鄰連桿之間的關(guān)系，和連桿自身的特征和連桿之間的連接方式有關(guān)。因 此，我們首先應(yīng)該清楚如何對連桿的特征和連接方式進(jìn)

31、行描述。一、連桿特征的描述如圖2-16所示，連桿i兩端有關(guān)節(jié)i和i+1。該連桿的特征可以用兩個(gè)參數(shù)來描述：一個(gè)是兩個(gè)關(guān)節(jié)軸線沿公垂線的距離li，稱為連桿長度；另一個(gè)是在垂直于公垂線的平面內(nèi)兩個(gè)軸線的夾角：V,稱為連桿扭角。這兩個(gè) 參數(shù)為表述連桿特征的 尺寸參數(shù)。連桿長度li- - - - .恒為非負(fù)數(shù)，但連桿扭角:,可正、可負(fù)i的正負(fù)是這樣規(guī)定的：公垂線的正向規(guī)定為從 關(guān)節(jié)i指向關(guān)節(jié)i+1，按右手法則從軸線i繞公 垂線轉(zhuǎn)至軸線i+1，逆時(shí)針為正，瞬時(shí)針為負(fù)。 兩軸線平行時(shí)，：,=0;兩軸線相交時(shí)，h = 0， 此時(shí)扭角:i為兩軸線的夾角，正負(fù)和 Xi軸選 向有關(guān)。二、連桿連接方式的描述如圖2-

32、17所示，連桿i和連桿i-1通過關(guān)節(jié) 和它垂直。兩條公垂線的相對位置可用兩個(gè)參數(shù)i相連，因此，關(guān)節(jié)i的軸線有兩條公垂線 di和詁來確定，其中di是沿關(guān)節(jié)i軸線測日i量的兩個(gè)公垂線和i軸線交點(diǎn)的距離，當(dāng)關(guān)節(jié)軸線相交時(shí),di為i軸線上兩交點(diǎn)的距離； 是在關(guān)節(jié)i軸線的垂直平面內(nèi)兩個(gè)公垂線的夾角，當(dāng)公垂線不存在時(shí)，對旋轉(zhuǎn)關(guān)節(jié) 刁仍然 存在。di和可是表達(dá)相鄰連桿 連接關(guān)系的參數(shù)。di和可都可正、可負(fù)（詳見表2-1）。這樣，相鄰兩個(gè)連桿之間的關(guān)系可以由四個(gè)參數(shù)所描述：其中兩個(gè)參數(shù)（li和i）描述連桿i的尺寸；另外兩個(gè)參數(shù)（di和二關(guān)節(jié)描述連桿i和連桿i-1之關(guān)節(jié)勺連接關(guān)系。對于旋轉(zhuǎn) 其它三個(gè)參數(shù)固定不變

33、；對于移動(dòng)關(guān)節(jié)j關(guān)節(jié)即為一個(gè)自由度） 連桿i卅標(biāo)系i簡稱 i+1的軸線重合 關(guān)節(jié)i指向關(guān)2-16） ；定即 YiZiX Xi ；：的交點(diǎn)上。當(dāng)關(guān)節(jié)】丄的軸線和關(guān)節(jié)i+1的軸線相交 .i的軸線和關(guān)節(jié).i+1.的軸線平行時(shí)，原點(diǎn)關(guān)節(jié)，-i是關(guān)節(jié)變量，k 參數(shù)固定不變。卄（對照圖2-17解釋，三、連桿坐標(biāo)系的建立D-H法要求按下面規(guī)貝連桿立連桿 坐標(biāo)系i和j 兩個(gè)關(guān)節(jié)x Zi 坐標(biāo) 坐標(biāo) 時(shí)，Oi取在使di= 0的地方圖2-17畫出了坐標(biāo)系i-1Wi的設(shè)定位姿（為什么叫設(shè)定位姿）圖2-17連桿關(guān)系參數(shù)d及日1）2）3）Z但X軸取向勺的Y i軸影i正負(fù) few的原點(diǎn)0站X 和連桿i+1i軸和連桿i的時(shí)

34、，取Xi=-Zi_i在建立連桿坐標(biāo)系時(shí)，下面四點(diǎn)值得注意:1)2)3)4)連桿坐標(biāo)系的建立不是唯一的。例如，雖然乙軸和關(guān)節(jié)i+1的軸線重合，但Zi軸的指向有兩種選擇；當(dāng) 乙軸和Zi-1軸相交時(shí)，Xi軸的指向也有兩種選擇； 坐標(biāo)系i也可以建立在關(guān)節(jié)i的軸線上，并使 乙軸和關(guān)節(jié)i的軸線重合； 建立不同的連桿坐標(biāo)系，相應(yīng)的連桿參數(shù)將會(huì)不同。應(yīng)使描述連桿i的四個(gè)參數(shù)中 盡可能多地為零；和機(jī)座固連的0系原則上可以任意規(guī)定，但是，為了方便計(jì)算，一般應(yīng)將0系建 立在連桿1的關(guān)節(jié)1的軸線上，并使0系和1系盡量靠近或重合（畫極坐標(biāo)型）?，F(xiàn)將連桿參數(shù)和坐標(biāo)系的建立歸納為表2-1。表2-1連桿參數(shù)及坐標(biāo)系連桿i的參

35、數(shù)名稱含義正負(fù)號(hào)性質(zhì)Q轉(zhuǎn)角Xi-l軸繞Zi-l軸轉(zhuǎn)至和Xi軸平行時(shí)的轉(zhuǎn)角按右手法則確定轉(zhuǎn)動(dòng)關(guān)節(jié)為變量 移動(dòng)關(guān)節(jié)為常量di距離Xi-l軸沿Zi-l方向移動(dòng)至和Xi軸相交時(shí)發(fā)生的位移和Zi-l正向一致為正轉(zhuǎn)動(dòng)關(guān)節(jié)為常量 移動(dòng)關(guān)節(jié)為變量li長度Zi-l軸沿Xi方向移動(dòng)至和 乙軸相交時(shí)移動(dòng)的距離恒為非負(fù)數(shù)常量a扭角:Zi-l軸繞Xi軸轉(zhuǎn)至和 乙軸平行時(shí)的轉(zhuǎn)角按右手法則確定常量連桿i的坐標(biāo)系OiXiYiZi原點(diǎn)Oi坐標(biāo)軸乙坐標(biāo)軸Xi坐標(biāo)軸Yi位于連桿i兩關(guān)節(jié)軸線之公垂線和 關(guān)節(jié)i+1軸線的交點(diǎn)處和關(guān)節(jié)i+1的軸線重合，方向 任意確定沿連桿i兩關(guān)節(jié)軸線的公垂 線，并指向i+1關(guān)節(jié)按右手法則確定連桿坐標(biāo)系之

36、間的變換矩陣建立了各連桿的坐標(biāo)系后,i-1系和i系之間的變換關(guān)系可以用坐標(biāo)系的平移、旋轉(zhuǎn)來 實(shí)現(xiàn)。從i 1系到i系的變換，可先令i 1系繞Z4軸旋轉(zhuǎn)?芯角（旋轉(zhuǎn)后X訂軸和誤丄軸平行）， 再沿 Zi丄軸平移di （平移后Xi丄軸和X軸重合），然后沿Xi_軸平移Ii （平移后0上丄點(diǎn)和0 點(diǎn)重合），最后繞 Xi_軸旋轉(zhuǎn)；丄角（旋轉(zhuǎn)后 Z軸和 Zi_軸重合，Y訂 軸和 Y4-軸也自然重合）， 最后使得i-1系和i系重合。建立了各連桿的坐標(biāo)系后，i-1系和i系之間的變換關(guān)系可以用坐標(biāo)系的平移、旋轉(zhuǎn)來 實(shí)現(xiàn)。假設(shè)i系和i-1系原來是重合的，先令i系繞乙軸旋轉(zhuǎn)二i角（旋轉(zhuǎn)后Xi軸和實(shí)際位 置平行），再沿乙

37、軸平移di （平移后Xi軸和實(shí)際位置重合），然后沿Xi軸平移li （平移后 Oi點(diǎn)和實(shí)際位置重合），最后繞Xi軸旋轉(zhuǎn):i角（旋轉(zhuǎn)后乙軸和實(shí)際位置重合，Yi軸也自然 實(shí)際位置重合），最后i系到達(dá)了實(shí)際的位姿。用一個(gè)變換矩陣Ai來綜合表示上述四次變換時(shí)，應(yīng)注意原來的i系在每次旋轉(zhuǎn)或平移 后發(fā)生了變動(dòng)，后一次變換是相對動(dòng)坐標(biāo)系進(jìn)行的，因此，在運(yùn)算中變換算子應(yīng)該右乘。 于是，連桿i的齊次變換矩陣為：123-cosq-sin 熱001刁00sin qcosq0001000100010001 一000Ai = Rot(z,弓)Trans(O, 0, di) Trans(Ii,0,0) Rot(x, ： i

38、)0【_100lnl10001001000cos%-si nW0di00100si n%cos%01 一001 一001 一3COSRsin qI 00-sin * cos ： iCOS R COS isin _：訂0sin m sin _：和-cosr sin ： icos： i0li COSKli si nddi1(2-20)也可以利用前面介紹的旋量概念用 Screw(z，di， v)Screw(x，li，)來計(jì)算Ai。實(shí)際上，很多工業(yè)機(jī)器人在設(shè)計(jì)時(shí)，常常使某些連桿參數(shù)取特殊值，如使口i=0?；?0。， 也有使li=0或di=0,從而可以簡化變換矩陣 Ai的計(jì)算，同時(shí)也可以簡化控制。工業(yè)機(jī)

39、器人正向運(yùn)動(dòng)學(xué)方程及實(shí)例工業(yè)機(jī)器人正向運(yùn)動(dòng)學(xué)主要解決正向運(yùn)動(dòng)學(xué)方程的建立及手部位姿的求解問題。當(dāng)為機(jī)器人的每個(gè)關(guān)節(jié)建立了連桿坐標(biāo)系之后，就可以獲得相鄰兩個(gè)連桿坐標(biāo)系之間 的變換矩陣Ai (i=1，2，n。n為機(jī)器人的自由度數(shù))，則有下列矩陣T n=A 1 A 2A n(2-21)矩陣Tn就是工業(yè)機(jī)器人手部坐標(biāo)系相對于固定坐標(biāo)系的位姿，我們稱式(2-21)為機(jī)器人正向運(yùn)動(dòng)學(xué)方程，Tn是如下的(4X4)矩陣：nxnynzTn = n o ap廠OxOyOz0axayaz0PxlPyPz1(2-22)圖2-19 SCARA型機(jī)器人的坐標(biāo)系連桿轉(zhuǎn)角0兩連桿之間距離d連桿長度l連桿扭角a連桿1ed1 =

40、 0l1= 1000(1= 0連桿2d2= 0l2= 1000(2= 0連桿3d3= 0l3= 200B= 0該SCARA型工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)方程為：T 3 = A1A 2A 3式中，Ai (i=1 ,2, 3)表示坐標(biāo)系i相對于坐標(biāo)系i-1 的齊次變換矩陣。把表2-2中每一行的參數(shù)代入公式(2-20)中，即可式中，前二列表示手部的姿態(tài)，第四列表示手部的位置事實(shí)上，很多工業(yè)機(jī)器人在設(shè)計(jì)時(shí)，常常使某些連桿參數(shù)取特殊值，使得連桿坐標(biāo)系 比較容易建立，同時(shí)，相鄰兩個(gè)連桿坐標(biāo)系之間的變換矩陣 Ai也比容易獲得，可以不必非 得按照DH法來做。下面通過兩個(gè)實(shí)例來介紹建立工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)方程的方法。1)平面

41、關(guān)節(jié)型工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)方程圖2-19(a)所示為具有一個(gè)肩關(guān)節(jié)、一個(gè)肘關(guān)節(jié)和一個(gè)腕關(guān)節(jié)的3自由度SCARA型工業(yè)機(jī)器人?？紤]到關(guān)節(jié)軸線相互平行，并且連桿都在一個(gè)平面內(nèi)的特點(diǎn)，將固定坐標(biāo)系0 和連桿1、連桿2、連桿3的坐標(biāo)系1、2、3分別建立在關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3和手 部的中心，如圖2-19(a)所示。坐標(biāo)系 就是手部坐標(biāo) 系。連桿參數(shù)中二為變量，d、l、均為常量。建立了連 桿坐標(biāo)系之后，即可列出該工業(yè)機(jī)器人的連桿參數(shù)如表2-2所示。表2-2圖2-19(a)所示3自由度SCARA工業(yè)機(jī)器人的連桿參數(shù)得出齊次變換矩陣A1、A2和A3。因?yàn)樵揝CARA型工業(yè)機(jī)器人的各連桿之間的關(guān)系比較簡單，可以

42、參考圖2-19(b)直接寫出矩陣A1、A2和A3。我們以Ai為例說明其計(jì)算方法。 1系的運(yùn)動(dòng)過程是：先沿Xo移動(dòng)li,再繞Zo轉(zhuǎn)動(dòng)山，因?yàn)檗D(zhuǎn)動(dòng)是相對固定坐標(biāo)系進(jìn)行的, 所以，Rot(z,胡)應(yīng)該左乘Trans(li, 0, 0)。因此，Ai、A2和A3分別為：A1= Rot(z,日 1) Trans(l1, 0, 0)A2= Rot(z, x)Trans(l2, 0, 0)A3= Rot(z,)3)Trans, 0, 0)即：g-s00j00ljcs60|的1A1 =000100sc0hs001000100010-0001 一衛(wèi)001 _10001 _-c日2-sT200：1001C日2-S日20l2c 日 2 1A 2 =S02c日2000100S日2C日20l2s 日 20010001000100001 一p001 _I0001 一-C3-s日300：100la!IC日3-S日30l3C