《機械原理教案》ppt課件

1、13 工業(yè)機器人機構(gòu)學工業(yè)機器人機構(gòu)學13.1概述概述13.2工業(yè)機器人的組成工業(yè)機器人的組成13.3工業(yè)機器人的分類與性能工業(yè)機器人的分類與性能13.4工業(yè)機器人的運動學根底工業(yè)機器人的運動學根底 13.5工業(yè)機器人的正向運動學工業(yè)機器人的正向運動學 13.6工業(yè)機器人的逆向運動學工業(yè)機器人的逆向運動學 提提 要要研討了坐標變換與空間物研討了坐標變換與空間物體的位姿與位移的齊次坐標表體的位姿與位移的齊次坐標表達；研討了知各個關(guān)節(jié)的相對達；研討了知各個關(guān)節(jié)的相對運動時，如何確定工業(yè)機器人運動時，如何確定工業(yè)機器人末端操作器的位姿；研討了知末端操作器的位姿；研討了知目的對象的位姿時，如何確定目的

2、對象的位姿時，如何確定工業(yè)機器人各個關(guān)節(jié)的相對運工業(yè)機器人各個關(guān)節(jié)的相對運動量。動量。 引見了工業(yè)機器人的組成原理、分類與任務(wù)性能特點。引見了工業(yè)機器人的組成原理、分類與任務(wù)性能特點。Chapter 13 Kinematics of Industrial Robots13 工業(yè)機器人機構(gòu)學工業(yè)機器人機構(gòu)學13.1概述概述工業(yè)機器人是用來搬運資料、零件與工具，進展焊接與噴涂工業(yè)機器人是用來搬運資料、零件與工具，進展焊接與噴涂的可再編程的多功能機械手，經(jīng)過調(diào)用不同的程序來完成預(yù)設(shè)的的可再編程的多功能機械手，經(jīng)過調(diào)用不同的程序來完成預(yù)設(shè)的多種任務(wù)義務(wù)。多種任務(wù)義務(wù)。 圖圖131工業(yè)機器人工業(yè)機器人(

3、a) (b)13.2工業(yè)機器人的組成工業(yè)機器人的組成工業(yè)機器人由三大部分六個子系統(tǒng)組成。三大部分是機械工業(yè)機器人由三大部分六個子系統(tǒng)組成。三大部分是機械部分、傳感部分和控制部分。六個系統(tǒng)是驅(qū)動系統(tǒng)、機構(gòu)與構(gòu)部分、傳感部分和控制部分。六個系統(tǒng)是驅(qū)動系統(tǒng)、機構(gòu)與構(gòu)造系統(tǒng)、覺得系統(tǒng)、機器人與環(huán)境交互系統(tǒng)、人機交互系統(tǒng)和造系統(tǒng)、覺得系統(tǒng)、機器人與環(huán)境交互系統(tǒng)、人機交互系統(tǒng)和控制系統(tǒng)?？刂葡到y(tǒng)。 圖圖132汽車消費線上的工業(yè)機器人汽車消費線上的工業(yè)機器人1. 機器人的機構(gòu)與構(gòu)造系統(tǒng)機器人的機構(gòu)與構(gòu)造系統(tǒng) 工業(yè)機器人的機械部分由三部分組成，即機身、手臂和末工業(yè)機器人的機械部分由三部分組成，即機身、手臂和末

4、端操作器。機身可以是固定的，也可以是挪動的。手臂進一步端操作器。機身可以是固定的，也可以是挪動的。手臂進一步劃分為上臂和下臂，上臂與機身構(gòu)成肩關(guān)節(jié)，上臂與下臂構(gòu)成劃分為上臂和下臂，上臂與機身構(gòu)成肩關(guān)節(jié)，上臂與下臂構(gòu)成肘關(guān)節(jié)，下臂與末端操作器構(gòu)成碗關(guān)節(jié)，如圖肘關(guān)節(jié)，下臂與末端操作器構(gòu)成碗關(guān)節(jié)，如圖13-3所示。所示。 圖圖133 機器人的機構(gòu)與構(gòu)造機器人的機構(gòu)與構(gòu)造(a)(b)圖圖13F01 噴涂機器人噴涂機器人(1) 噴涂機器的一種類型噴涂機器的一種類型2. 機器人手部的機構(gòu)與構(gòu)造系統(tǒng)機器人手部的機構(gòu)與構(gòu)造系統(tǒng)圖圖13-4 單自在度操作器單自在度操作器圖圖13-5 多自在度操作器多自在度操作器

5、(2) 具有多個自在具有多個自在 度的末端操作器度的末端操作器(1) 具有一個相對自具有一個相對自 由度的末端操作器由度的末端操作器13.3工業(yè)機器人的分類與性能工業(yè)機器人的分類與性能直角坐標型操作機如圖直角坐標型操作機如圖136所示，它有三個挪動關(guān)節(jié)所示，它有三個挪動關(guān)節(jié)(PPP)，可使末端操作器作三個方向的獨立位移。，可使末端操作器作三個方向的獨立位移。1. 直角坐標型直角坐標型yxzPxPzPy末端操作器123PPP圖圖136直角坐標型操作機直角坐標型操作機該種型式的工業(yè)機器人該種型式的工業(yè)機器人，定位精度較高，空間軌跡，定位精度較高，空間軌跡規(guī)劃與求解相對較容易，計規(guī)劃與求解相對較容易

6、，計算機控制相對較簡單。它的算機控制相對較簡單。它的缺乏是空間尺寸較大，運動缺乏是空間尺寸較大，運動的靈敏性相對較差，運動的的靈敏性相對較差，運動的速度相對較低。速度相對較低。2. 圓柱坐標型圓柱坐標型圓柱坐標型操作機如圖圓柱坐標型操作機如圖137所示，它有兩個挪動關(guān)節(jié)和一所示，它有兩個挪動關(guān)節(jié)和一個轉(zhuǎn)動關(guān)節(jié)個轉(zhuǎn)動關(guān)節(jié)(PPR)，末端操作器，末端操作器的安裝軸線之位姿由的安裝軸線之位姿由(z，r，)坐標予以表示。該種型式的工坐標予以表示。該種型式的工業(yè)機器人，空間尺寸較小，任業(yè)機器人，空間尺寸較小，任務(wù)范圍較大，末端操作器可獲務(wù)范圍較大，末端操作器可獲得較高的運動速度。它的缺陷得較高的運動速度

7、。它的缺陷是末端操作器離是末端操作器離z軸愈遠，其切軸愈遠，其切向線位移的分辨精度就愈低。向線位移的分辨精度就愈低。rZ圖圖137圓柱坐標型操作機圓柱坐標型操作機3. 球坐標型球坐標型球坐標型操作機如圖球坐標型操作機如圖138所示，它有兩個轉(zhuǎn)所示，它有兩個轉(zhuǎn)動關(guān)節(jié)和一個挪動關(guān)節(jié)動關(guān)節(jié)和一個挪動關(guān)節(jié)(RRP)，末端操作器的安，末端操作器的安裝軸線之位姿由裝軸線之位姿由(， r)坐標予以表示。該種型式坐標予以表示。該種型式的工業(yè)機器人，空間尺寸的工業(yè)機器人，空間尺寸較小，任務(wù)范圍較大。較小，任務(wù)范圍較大。zrxyRRP圖圖138球坐標型操作機球坐標型操作機4. 關(guān)節(jié)型關(guān)節(jié)型腕關(guān)節(jié)的轉(zhuǎn)動腕關(guān)節(jié)的轉(zhuǎn)動

8、Z3屬于末端操作器的自在屬于末端操作器的自在度。該種構(gòu)造的工業(yè)機度。該種構(gòu)造的工業(yè)機器人，空間尺寸相對較器人，空間尺寸相對較小，任務(wù)范圍相對較大小，任務(wù)范圍相對較大，還可以繞過機座周圍，還可以繞過機座周圍的妨礙物，是目前運用的妨礙物，是目前運用較多的一種機型。較多的一種機型?？刂瓶刂葡到y(tǒng)系統(tǒng)Z1Y0諧波減速器諧波減速器光電編碼盤光電編碼盤末端執(zhí)行器末端執(zhí)行器X1伺服伺服電機電機Y0Z1Z2Z3上臂上臂下臂下臂RRRY1X0Z0圖圖139關(guān)節(jié)型操作機關(guān)節(jié)型操作機關(guān)節(jié)型操作機如圖關(guān)節(jié)型操作機如圖139所示，它有三個轉(zhuǎn)動關(guān)節(jié)所示，它有三個轉(zhuǎn)動關(guān)節(jié)(RRR)，即機身上部相對于下部的轉(zhuǎn)動即機身上部相對于

9、下部的轉(zhuǎn)動Y0，肩關(guān)節(jié)的轉(zhuǎn)動，肩關(guān)節(jié)的轉(zhuǎn)動Z1和肘關(guān)和肘關(guān)節(jié)的轉(zhuǎn)動節(jié)的轉(zhuǎn)動Z2。13.4工業(yè)機器人的運動學根底工業(yè)機器人的運動學根底工業(yè)機器人是由假設(shè)干工業(yè)機器人是由假設(shè)干個關(guān)節(jié)所聯(lián)絡(luò)起來的一種開個關(guān)節(jié)所聯(lián)絡(luò)起來的一種開鏈，其一端固結(jié)在機座上，鏈，其一端固結(jié)在機座上，另一端安裝有末端操作器。另一端安裝有末端操作器。確定工業(yè)機器人末端操作器確定工業(yè)機器人末端操作器安裝軸線的方位，確定末端安裝軸線的方位，確定末端操作器的位姿與位移，確定操作器的位姿與位移，確定工業(yè)機器人的操作對象，即工業(yè)機器人的操作對象，即目的物體的位姿與位移，構(gòu)目的物體的位姿與位移，構(gòu)成了工業(yè)機器人運動學根底成了工業(yè)機器人運動學

10、根底應(yīng)該研討的一部分任務(wù)。應(yīng)該研討的一部分任務(wù)。圖圖133 機器人的機構(gòu)與構(gòu)造機器人的機構(gòu)與構(gòu)造末端操作器末端操作器(a)13.4.1目的物體的空間轉(zhuǎn)動矩陣目的物體的空間轉(zhuǎn)動矩陣一個經(jīng)過坐標原點的矢量一個經(jīng)過坐標原點的矢量V1繞經(jīng)過坐標原點的單位矢量繞經(jīng)過坐標原點的單位矢量u轉(zhuǎn)動轉(zhuǎn)動角到達角到達V2，要求確定，要求確定V2的位姿。為了確定矢量的位姿。為了確定矢量V1繞經(jīng)繞經(jīng)過坐標原點的單位矢量過坐標原點的單位矢量u轉(zhuǎn)動轉(zhuǎn)動角到達角到達V2的位姿，將它作如下的位姿，將它作如下轉(zhuǎn)動。轉(zhuǎn)動。圖圖1310目的物體的空間轉(zhuǎn)動目的物體的空間轉(zhuǎn)動OV1uzyxuxuz-uyV2+1.平面內(nèi)單位矢量繞坐標軸的

11、轉(zhuǎn)動矩陣平面內(nèi)單位矢量繞坐標軸的轉(zhuǎn)動矩陣cossinsincosyxyyxxUUUUUUyxyxUUUUcossinsincosUUxyUxUyUyUxUxUy11000cossin0sincos1yxyxUUUU圖圖13.4F01平面內(nèi)單平面內(nèi)單位矢量繞坐標軸的轉(zhuǎn)動位矢量繞坐標軸的轉(zhuǎn)動2.空間內(nèi)單位矢量繞坐標軸的轉(zhuǎn)動矩陣空間內(nèi)單位矢量繞坐標軸的轉(zhuǎn)動矩陣) 113(cossin0sincos0001,Rx)213(cos0sin010sin0cos,yR)313(1000cossin0sincos,zROuzyxuxuzuy圖圖1310(a)空間內(nèi)單位空間內(nèi)單位矢量繞坐標軸線的轉(zhuǎn)動矢量繞坐標軸

12、線的轉(zhuǎn)動矢量矢量V1繞經(jīng)過坐標原點的單位矢量繞經(jīng)過坐標原點的單位矢量u轉(zhuǎn)動轉(zhuǎn)動角的矩陣角的矩陣)513(22233,CVuSuVuuSuVuuSuVuuCVuSuVuuSuVuuSuVuuCVuRzxzyyzxxzyyzyxyzxzyxxu)613(111222222zyxzxzyyzxxzyyzyxyzxzyxxzyxVVVCVuSuVuuSuVuuSuVuuCVuSuVuuSuVuuSuVuuCVuVVV)413(1,133,2VRRRRRVRVyxzxyu矢量矢量V1繞經(jīng)過坐標原點的單位矢量繞經(jīng)過坐標原點的單位矢量u轉(zhuǎn)動轉(zhuǎn)動角的矩陣角的矩陣)713(100000044,zzzyyyxxx

13、uQMNQMNQMNR當式當式(137)中的每一個元素為知時，利用式中的每一個元素為知時，利用式(135)中的中的元素與式元素與式(137)中的前中的前3行行3列元素對應(yīng)相等，即可求出矢量列元素對應(yīng)相等，即可求出矢量V1繞矢量繞矢量u轉(zhuǎn)動的轉(zhuǎn)轉(zhuǎn)動的轉(zhuǎn)角和矢量角和矢量u的姿態(tài)。的姿態(tài)。矢量矢量V1繞矢量繞矢量u轉(zhuǎn)動的轉(zhuǎn)動的角和矢量角和矢量u的姿態(tài)為的姿態(tài)為)813()()()(21sin222xyzxyzMNNQQM)913(1)()()(tan222zyxxyzxyzQMNMNNQQM)1013(sin2sin2sin2xyzzxyyzxMNuNQuQMu例例13-1圖圖1311為單臂操作機械

14、手，手臂相對于機身擁有一個為單臂操作機械手，手臂相對于機身擁有一個轉(zhuǎn)動自在度，手腕相對于手臂擁有一個轉(zhuǎn)動自在度。知手腕轉(zhuǎn)動自在度，手腕相對于手臂擁有一個轉(zhuǎn)動自在度。知手腕上的坐標系上的坐標系oxyz相對于機身坐標系相對于機身坐標系OXYZ的位姿矩陣的位姿矩陣SW為為1000210060012010SWXYyxzOOz1y1x2z2y2O2O1Z圖圖1311單臂操作機械手單臂操作機械手機身機身手臂手臂手腕手腕x1(2) 假設(shè)手臂相對于機身不動，假設(shè)手臂相對于機身不動，手腕上的坐標系手腕上的坐標系Oxyz相對相對于手臂上的于手臂上的z軸轉(zhuǎn)動軸轉(zhuǎn)動90，那么坐標系那么坐標系oxyz轉(zhuǎn)到坐標轉(zhuǎn)到坐標系

15、系O2x2y2z2。試寫出以上。試寫出以上兩種轉(zhuǎn)動的矩陣兩種轉(zhuǎn)動的矩陣SW1、SW2。SW中前三行前三列的元素表示手腕坐標系的姿態(tài)，中前三行前三列的元素表示手腕坐標系的姿態(tài)，2,6,2T表示手腕坐標系原點的位置。表示手腕坐標系原點的位置。假設(shè)手臂相對于機身坐標系假設(shè)手臂相對于機身坐標系OXYZ的的Z軸轉(zhuǎn)動軸轉(zhuǎn)動90，那么，那么坐標系坐標系oxyz轉(zhuǎn)到坐標系轉(zhuǎn)到坐標系o1x1y1z1。XYyxzOOz1y1x2z2y2O2O1Z圖圖1311單臂操作機械手單臂操作機械手機身機身手臂手臂手腕手腕x1坐標系坐標系O1x1y1z1在固定坐標系在固定坐標系OXYZ的位姿矩陣的位姿矩陣SW1為為100021

16、002010600110002100600120101000010000010010,901SWRSWZO2x2y2z2坐標系在固定坐標系坐標系在固定坐標系OXYZ的位姿矩陣的位姿矩陣SW2為為 100021006010200110000100000100101000210060012010,902zRSWSW13.4.2坐標系之間的空間變換矩陣坐標系之間的空間變換矩陣 XYZxyzOOvvzvxvyVxVyVzP圖圖1312點在坐點在坐標系之間的變換標系之間的變換設(shè)單位矢量設(shè)單位矢量v在坐標系在坐標系Oxyz中中的投影分別為的投影分別為vx、vy和和vz；矢量；矢量P在在坐標系坐標系OXYZ

17、中的投影分別為中的投影分別為PX，PY和和PZ；x軸在坐標系軸在坐標系OXYZ中中X、Y和和Z上的投影分別為上的投影分別為txX、txY和和txZ；y軸在坐標系軸在坐標系OXYZ中中X、Y和和Z上的投影分別為上的投影分別為tyX、tyY和和tyZ；z軸在坐標系軸在坐標系OXYZ中中X、Y和和Z上的上的投影分別為投影分別為tzX、tzY和和tzZ。為此，連桿坐標系為此，連桿坐標系O?xyz相對于相對于固定坐標系固定坐標系OXYZ的位姿為的位姿為 )1113(ZYXzyxzZyZxZzYyYxYzXyXxXZYXPPPvvvtttttttttVVVXYZxyzOOvvzvxvyVxVyVzP圖圖1

18、312點在坐點在坐標系之間的變換標系之間的變換txX、txY和和txZ的表達式分別為的表達式分別為txXcos(x，X)，txYcos(x，Y)，txZcos(x，Z)，其他的關(guān)系式類推。，其他的關(guān)系式類推。為了計算機求解方便，將上為了計算機求解方便，將上式改寫為齊次坐標方式式改寫為齊次坐標方式)1213(1)44(110001zyxzyxXzZyZxZXzYyYxYXzXyXxXZYXvvvTvvvPtttPtttPtttVVVV13.4.3 目的物體的齊次坐標表示目的物體的齊次坐標表示在如圖在如圖1313a所示的坐標系所示的坐標系OXYZ中放置一個楔塊，在中放置一個楔塊，在楔塊上設(shè)置坐標系

19、楔塊上設(shè)置坐標系oxyz，其上的特征點為，其上的特征點為A1，A2，A3，A4，A5和和A6。這些特征點在本身坐標系。這些特征點在本身坐標系oxyz中的坐標分別為中的坐標分別為A11,0,0，A21,0,0，A31,0,2，A41,0,2，A51,4,0，A61,4,0。XZyzxYA4A3A2A5A6XZzyxYA6A5A2A1A3A4圖圖1313目的物體的齊次坐標變換目的物體的齊次坐標變換(a)(b)OoA1用齊次坐標用齊次坐標Wxyz(46)表示這些點在本身坐標系表示這些點在本身坐標系oxyz中的位置為中的位置為)1313(111111002200440000111111)64(xyzW

20、XZyzxYA4A3A2A5A6XZzyxYA6A5A2A1A3A4圖圖1313目的物體的齊次坐標變換目的物體的齊次坐標變換(a)(b)OoA1假設(shè)讓楔塊繞假設(shè)讓楔塊繞Z軸轉(zhuǎn)過軸轉(zhuǎn)過90，再繞，再繞Y軸轉(zhuǎn)過軸轉(zhuǎn)過90，最后沿，最后沿X軸軸方向平移方向平移4，那么楔塊到達圖，那么楔塊到達圖12-13b所示的位置。以上的變換所示的位置。以上的變換TxyzXYZ 為為1000001000014100 1 , 0 , 0 , 4,90,90TZYRRTrXYZxyzTXZyzxYA4A3A2A5A6XZzyxYA6A5A2A1A3A4圖圖1313目的物體的齊次坐標變換目的物體的齊次坐標變換(a)(b)

21、OoA1此時，楔塊上的特征點在此時，楔塊上的特征點在OXYZ坐標系中的齊次坐標坐標系中的齊次坐標WXYZ(46)為為 )64(XYZW)1413(1111110022004400001111111000001000014100111111440000111111446644XZyzxYA4A3A2A5A6XZzyxYA6A5A2A1A3A4圖圖1313目的物體的齊次坐標變換目的物體的齊次坐標變換(a)(b)OoA1由圖由圖1313b也可以得到坐標系也可以得到坐標系OXYZ在坐標系在坐標系oxyz中的齊次坐標中的齊次坐標 。知知X軸的方位為軸的方位為0,0,1,0T， Y軸的方位為軸的方位為1,

22、0,0,0T， Z軸的軸的方位為方位為0,1,0,0T，坐標系，坐標系OXYZ的原點的原點O在坐標系在坐標系oxyz中的位置中的位置為為0,0,4,1 T。XZyzxYA4A3A2A5A6XZzyxYA6A5A2A1A3A4圖圖1313目的物體的齊次坐標變換目的物體的齊次坐標變換(a)(b)OoA1為此，坐標系為此，坐標系OXYZ在坐標系在坐標系oxyz中的位姿矩陣中的位姿矩陣TXYZxyz為為1000400101000010 xyzXYZTXZyzxYA4A3A2A5A6XZzyxYA6A5A2A1A3A4圖圖1313目的物體的齊次坐標變換目的物體的齊次坐標變換(a)(b)OoA1可以證明，

23、可以證明，TXY Zxyz與與TxyzXYZ的乘積為單位矩陣，的乘積為單位矩陣，即即TXY ZxyzT xyzXYZ。假設(shè)。假設(shè)TxyzXYZ的普的普通方式為通方式為)1513(1000ZZZZYYYYXXXXPQMNPQMNPQMNXYZxyzT那么那么TxyzXYZ的逆變換矩陣的逆變換矩陣TXYZ xyz為為 1XYZxyzTxyzXYZT)1613(1000QPQQQMPMMMNPNNNZYXZYXZYX13.4.4剛體的空間位移矩陣剛體的空間位移矩陣設(shè) 知設(shè) 知 p 1 = p 1 X p 1 Yp1ZT，q1=q1X q1Yq1ZT，那么，那么q=qX qYqZT 的矢量表達式與矩陣

24、表達的矢量表達式與矩陣表達式分別為式分別為在如圖在如圖1314所示的坐標系所示的坐標系OXYZ中有一個連桿，連桿的中有一個連桿，連桿的初始位置用初始位置用p1q1表示，終止位置用表示，終止位置用pq表示，表示，p1點的位置矢量用點的位置矢量用R表示，連桿上的表示，連桿上的p1點沿一單位矢量點沿一單位矢量u位移位移s，同時連桿繞矢量，同時連桿繞矢量u轉(zhuǎn)動轉(zhuǎn)動角，如今確定角，如今確定q點相對于點相對于q1點的位置。點的位置。p1ZYXp=p1+suq1sqO圖圖1314 剛體的空間位移剛體的空間位移Ruq式式(1319)中的中的 同式同式(135)。)1913(11000)(11111,133,z

25、yxuuzyxqqqpRsupRqqq式式(1319)右端左側(cè)的矩陣稱為剛右端左側(cè)的矩陣稱為剛體的有限螺旋位移矩陣。體的有限螺旋位移矩陣。p1ZYXp=p1+suq1sqO圖圖1314 剛體的空間位移剛體的空間位移Ruq33,uR13.4.5歐拉角表示的變換矩陣歐拉角表示的變換矩陣 在圖在圖1315a所示的固定坐標系所示的固定坐標系OXYZ中放置一個矢量中放置一個矢量U，其初始位置為其初始位置為U1，坐標系，坐標系OXY Z是由是由OXYZ繞繞Z軸轉(zhuǎn)軸轉(zhuǎn)角度角度而得到的位置，此時，矢量而得到的位置，此時，矢量U1轉(zhuǎn)到轉(zhuǎn)到U2的位置；坐標系的位置；坐標系OXYZ是由是由OXYZ繞繞X軸轉(zhuǎn)軸轉(zhuǎn)角度

26、而得到的位置，此時，角度而得到的位置，此時，矢量矢量U2轉(zhuǎn)到轉(zhuǎn)到U3的位置；矢量的位置；矢量U3再繞再繞Z轉(zhuǎn)動轉(zhuǎn)動角而到達角而到達U4的位的位置。置。XYXYXZZYZO123U1U2U3U44XZYO1U1U2U3U4423(a)(b)圖圖1315歐拉角表示的變換歐拉角表示的變換在以上的相對轉(zhuǎn)動中，每次都是相對于動坐標系進展的，在以上的相對轉(zhuǎn)動中，每次都是相對于動坐標系進展的，而不是相對于固定坐標系進展的。而不是相對于固定坐標系進展的。、和和 稱為歐拉角。稱為歐拉角。 假設(shè)讓一切的轉(zhuǎn)動都是相對于固定坐標系假設(shè)讓一切的轉(zhuǎn)動都是相對于固定坐標系OXYZ進展的，如進展的，如圖圖13-15b所示，且

27、轉(zhuǎn)動順序為，先繞所示，且轉(zhuǎn)動順序為，先繞Z軸轉(zhuǎn)軸轉(zhuǎn)角度，再繞角度，再繞X軸轉(zhuǎn)軸轉(zhuǎn)角度，最后繞角度，最后繞Z軸轉(zhuǎn)軸轉(zhuǎn)角度。轉(zhuǎn)動變換矩陣為角度。轉(zhuǎn)動變換矩陣為 XYXYXZZYZO123U1U2U3U44XZYO1U1U2U3U4423(a)(b)圖圖1315歐拉角表示的變換歐拉角表示的變換,ZXZRRRE以上兩種變換的展開式均為以上兩種變換的展開式均為10000coscossinsinsin0sincoscoscoscossinsinsincoscoscossin0sinsincoscossinsincossincossincoscos,RXYXYXZZYZO123U1U2U3U44XZYO1U

28、1U2U3U4423(a)(b)圖圖1315歐拉角表示的變換歐拉角表示的變換13.4.6轉(zhuǎn)動關(guān)節(jié)之間的位移矩陣轉(zhuǎn)動關(guān)節(jié)之間的位移矩陣 連桿連桿n右端的坐標系右端的坐標系OnXnYnZn在左端的坐標系在左端的坐標系On-1Xn-1Yn-1Zn-1中的齊次變換矩陣中的齊次變換矩陣Tn為為10000cossin00sincos00001100010000100011000010000cossin00sincosnnnn1n1n1n1n1nXXXXZnZZZZndaTOnZnaXn連桿連桿nZ1XnYn關(guān)節(jié)關(guān)節(jié)n-1關(guān)節(jié)關(guān)節(jié)n關(guān)節(jié)關(guān)節(jié)n1ZZ-1Xn圖圖1316轉(zhuǎn)動關(guān)節(jié)轉(zhuǎn)動關(guān)節(jié)連桿連桿n-1連桿連桿n

29、1Xn-1Zn-1OZ(n1)OZ(n-2)Yn-2Zn-1Zn-2Yn-1化簡后得轉(zhuǎn)動關(guān)節(jié)之間的位移矩陣為化簡后得轉(zhuǎn)動關(guān)節(jié)之間的位移矩陣為)2113(1000cossin0sinsincoscoscossincossinsincossincos1nnn1nnn1nn1n1n1nnn1nn1n1nZXXZXXZXZZZXXZXZZndaaTOnZnaXn連桿連桿nZ1XnYn關(guān)節(jié)關(guān)節(jié)n-1關(guān)節(jié)關(guān)節(jié)n關(guān)節(jié)關(guān)節(jié)n1ZZ-1Xn圖圖1316轉(zhuǎn)動關(guān)節(jié)轉(zhuǎn)動關(guān)節(jié)連桿連桿n-1連桿連桿n1Xn-1Zn-1OZ(n1)OZ(n-2)Yn-2Zn-1Zn-2Yn-113.5工業(yè)機器人的正向運動學工業(yè)機器人的正向

30、運動學工業(yè)機器人的正向運動工業(yè)機器人的正向運動學是指知各關(guān)節(jié)的類型、相學是指知各關(guān)節(jié)的類型、相鄰關(guān)節(jié)之間的尺寸和相鄰關(guān)鄰關(guān)節(jié)之間的尺寸和相鄰關(guān)節(jié)相對運動量的大小時，如節(jié)相對運動量的大小時，如何確定工業(yè)機器人末端操作何確定工業(yè)機器人末端操作器在固定坐標系中的位姿。器在固定坐標系中的位姿。(a)圖圖131工業(yè)機器人工業(yè)機器人設(shè)工業(yè)機器人中的一個連桿一端關(guān)節(jié)上的坐標系相對于另設(shè)工業(yè)機器人中的一個連桿一端關(guān)節(jié)上的坐標系相對于另一端關(guān)節(jié)上的坐標系的位姿由齊次變換矩陣一端關(guān)節(jié)上的坐標系的位姿由齊次變換矩陣Ti表示，設(shè)表示，設(shè)T1表示表示第一個連桿一端動關(guān)節(jié)上的坐標系相對于另一端固定關(guān)節(jié)上的第一個連桿一端動

31、關(guān)節(jié)上的坐標系相對于另一端固定關(guān)節(jié)上的坐標系的位姿；設(shè)第二個連桿的一端與第一個連桿構(gòu)成動關(guān)節(jié)，坐標系的位姿；設(shè)第二個連桿的一端與第一個連桿構(gòu)成動關(guān)節(jié)，另一端與下一個連桿構(gòu)成動關(guān)節(jié)，齊次變換矩陣用另一端與下一個連桿構(gòu)成動關(guān)節(jié)，齊次變換矩陣用T2表示，那表示，那么第二個連桿相對于固定關(guān)節(jié)上的坐標系的位姿么第二個連桿相對于固定關(guān)節(jié)上的坐標系的位姿W2為為W2T1 T2。依次類推，假設(shè)有六個連桿，那么第六個連桿相對于固定。依次類推，假設(shè)有六個連桿，那么第六個連桿相對于固定關(guān)節(jié)上的坐標系的位姿關(guān)節(jié)上的坐標系的位姿W6為為W6T1 T2 T3 T4 T5 T6 (1222) W6T1 T2 T3 T4 T

32、5 T6 (1322) W6的表現(xiàn)方式可以用以下的的表現(xiàn)方式可以用以下的 (44)矩陣予以表示矩陣予以表示 )2313(1000)44(6zzzzyyyyxxxxPQMNPQMNPQMNW式式(1323)右端的前三列前三行表示末端操作器的姿態(tài)，右端的前三列前三行表示末端操作器的姿態(tài)，第四列前三行表示末端操作器的位置。第四列前三行表示末端操作器的位置。13.5.1平面關(guān)節(jié)型機器人的正向運動方程平面關(guān)節(jié)型機器人的正向運動方程圖圖1317a所示為由一個肩關(guān)節(jié)、一個肘關(guān)節(jié)和一個腕關(guān)所示為由一個肩關(guān)節(jié)、一個肘關(guān)節(jié)和一個腕關(guān)節(jié)組成的平面關(guān)節(jié)型的機器人簡圖，它的三個關(guān)節(jié)的軸線節(jié)組成的平面關(guān)節(jié)型的機器人簡圖，

33、它的三個關(guān)節(jié)的軸線Z0、Z1、Z2是平行的，它的構(gòu)造參數(shù)如表是平行的，它的構(gòu)造參數(shù)如表13-1所示。所示。表表131平面關(guān)節(jié)型的機器人的構(gòu)造參數(shù)平面關(guān)節(jié)型的機器人的構(gòu)造參數(shù) 連桿序連桿序號號n關(guān)于關(guān)于Zn軸軸的轉(zhuǎn)角的轉(zhuǎn)角兩連桿之間兩連桿之間的間隔的間隔dZn-1連桿的長度連桿的長度aXn連桿的連桿的扭角扭角Xn1Z1=1逆時逆時針為正針為正0aX1=a1=(100mm)X1=02Z2=2逆時逆時針為正針為正0aX2=a2=(100mm)X2=03Z3=3逆時逆時針為正針為正0aX3=a3=(100mm)X3=0圖圖1317a所示的平面關(guān)節(jié)型機器人的運動分析簡圖所示的平面關(guān)節(jié)型機器人的運動分析簡

34、圖如圖如圖1317b所示。該平面關(guān)節(jié)型機器人的運動學方程為所示。該平面關(guān)節(jié)型機器人的運動學方程為W3=T1 T2 T3 。圖圖1317 平面關(guān)節(jié)型機器人平面關(guān)節(jié)型機器人X0Y0O0O1Y1X1Y2X2X3Y3O2O3321(b)O0Y0Y1Y2Y3X0X1X2X3O1O2O3123(c)(a)Z0Z1Z2Z3O0O1O2O3Y0Y1Y2Y3X0X1X2X3123aX1aX2aX33210W3T1 T2 T3中每一項的矩陣表達式為中每一項的矩陣表達式為)2413(10000100sin0cossincos0sincos1000010000100011000010000cossin00sincos

35、33333333333333aaaT)2513(10000100sin0cossincos0sincos1000010000100011000010000cossin00sincos22222222222222aaaT)2613(10000100sin0cossincos0sincos1000010000100011000010000cossin00sincos11111111111111aaaTW3T1 T2 T3矩陣表達式為矩陣表達式為)2713(10000100sin)sin()sin(0)cos()sin(cos)cos()cos(0)sin()cos(1121232133213211

36、121232133213213aaaaaaW圖圖1317 平面關(guān)節(jié)型機器人平面關(guān)節(jié)型機器人X0Y0O0O1Y1X1Y2X2X3Y3O2O3321(b)O0Y0Y1Y2Y3X0X1X2X3O1O2O3123(c)(a)Z0Z1Z2Z3O0O1O2O3Y0Y1Y2Y3X0X1X2X3123aX1aX2aX3321假設(shè)轉(zhuǎn)角假設(shè)轉(zhuǎn)角1=30 ，2=60和和3=30 ，如圖如圖1317c所示，那所示，那么該平面關(guān)節(jié)型機器人么該平面關(guān)節(jié)型機器人的手部坐標系的手部坐標系O3X3Y3Z3在固定坐標在固定坐標系系O0X0Y0Z0中的位姿中的位姿W3為為1000010032.1705 . 0866. 02 .18

37、30866. 05 . 03W圖圖1317 平面關(guān)節(jié)型機器人平面關(guān)節(jié)型機器人X0Y0O0O1Y1X1Y2X2X3Y3O2O3321(b)O0Y0Y1Y2Y3X0X1X2X3O1O2O3123(c)(a)計計 算算 演演 示示Z0Z1Z2Z3O0O1O2O3Y0Y1Y2Y3X0X1X2X3123aX1aX2aX3321013.5.2斯坦福機器人的正向運動方程斯坦福機器人的正向運動方程 X1X0Y10圖圖1318 斯坦福機器人斯坦福機器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H1. 坐標系坐標系X1Y1Z1相對于相對于固定坐標系固定坐標系X

38、0Y0Z0的位的位姿姿圖圖1318所示為斯坦福機器人的構(gòu)造簡圖，針對圖示的坐所示為斯坦福機器人的構(gòu)造簡圖，針對圖示的坐標系，其參數(shù)關(guān)系如表標系，其參數(shù)關(guān)系如表122所示。所示。首先，將坐標系首先，將坐標系X0Y0Z0繞繞X0軸轉(zhuǎn)動軸轉(zhuǎn)動X190得坐得坐標系標系X1Y1Z1，然后，繞，然后，繞Z0軸軸轉(zhuǎn)動轉(zhuǎn)動Z0。該變換矩陣為。該變換矩陣為下面求末端操作器的位姿下面求末端操作器的位姿表表132斯坦福機器人的構(gòu)造參數(shù)斯坦福機器人的構(gòu)造參數(shù)桿件編號桿件編號關(guān)節(jié)關(guān)于關(guān)節(jié)關(guān)于Zi軸的轉(zhuǎn)角軸的轉(zhuǎn)角關(guān)節(jié)關(guān)于關(guān)節(jié)關(guān)于Xi軸的改動角軸的改動角Xn桿件長度桿件長度aXn1Z01(90)9002Z12(90)90d

39、2=100mm3Z2300d3=300mm4Z34(90)9005Z45(90)9006Z56(90)0H=50mm1) 坐標系坐標系X1Y1Z1相對于固定坐標系相對于固定坐標系X0Y0Z0的位姿的位姿10000)90cos()90sin(00)90sin()90cos(000011000010000cossin00sincos11110, 10, 01XXZZRRTX1X0Y10圖圖1318 斯坦福機器人斯坦福機器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H)2913(100000100cos0sin0sin0cos11112)坐標系坐

40、標系X2Y2Z2相對于相對于X1Y1Z1的位姿的位姿 1000090cos90sin0090sin90cos000011000100001000011000010000cossin00sincos222222dTX1X0Y10圖圖1318 斯坦福機器人斯坦福機器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H)3013(10000100cos0sin0sin0cos22222d3)坐標系坐標系X3Y3Z3相對于相對于X2Y2Z2的位姿的位姿 )3113(10001000010000133dTX1X0Y10圖圖1318 斯坦福機器人斯坦福機器人

41、Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H4)手腕坐標系相對于手腕坐標系相對于X3Y3Z3的位姿的位姿 (1) 坐標系坐標系X4Y4Z4相對于相對于X3Y3Z3的變換矩陣的變換矩陣 )3213(100000100cos0sin0sin0cos10000)90cos()90sin(00)90sin()90cos(000011000010000cossin00sincos444444443, 43, 34XXZXRRTX1X0Y10圖圖1318 斯坦福機器人斯坦福機器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z

42、5X5Z6X6Y5Y4H(2) 坐標系坐標系X5Y5Z5相對于相對于X4Y4Z4的變換矩陣的變換矩陣)3313(100000100cos0sin0sin0cos1000090cos90sin0090sin90cos000011000010000cossin00sincos555555554, 54, 45XXZZRRTX1X0Y10圖圖1318 斯坦福機器人斯坦福機器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H(3) 坐標系坐標系X6Y6Z6相對于相對于X5Y5Z5的變換矩的變換矩陣陣 )3413(100010000cossin00sin

43、cos1000100001000011000010000cossin00sincos 1 , 0 , 066666666T5, 56HHHTrRTZZX1X0Y10圖圖1318 斯坦福機器人斯坦福機器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H一旦知道了一旦知道了T1T6，那么恣意桿件之間的變換矩陣可以運，那么恣意桿件之間的變換矩陣可以運用以上公式求解出來。用以上公式求解出來。下面依次給出下面依次給出5T6、非相鄰桿件之間的變換矩陣、非相鄰桿件之間的變換矩陣4T6、3T6、2T6和和1T6的矩陣乘積方式，即的矩陣乘積方式，即 5T6 =T

44、6 ，4T6 = T5T6 ，3T6T4T5T6，2T6T3T4T5T6和和1T6T2T3T4T5T6的表達式。首先，給出桿件的表達式。首先，給出桿件6相對于相對于4的位姿矩陣為的位姿矩陣為)3513(100000cossincoscossinsincossinsinsinsincoscoscos100010000cossin00sincos100000100cos0sin0sin0cos66556565556565666655556564HHHTTT其次，給出桿件其次，給出桿件6相對于相對于3的位姿矩陣的位姿矩陣3T6為為)3613(1000cossinsinsincos100000coss

45、incoscossinsincossinsinsinsincoscoscos100000100cos0sin0sin0cos556565545464654646545454646546465466556565556565444465463HCSSCSHCSCCSCSSCCCSHSCCSSCCSSCCCHHTTTT再次，給出桿件再次，給出桿件6相對于相對于2的位姿矩陣的位姿矩陣2T6 為為 )3713(1000cossinsinsincos1000cossinsinsincos100010000100001355656554546465464654545464654646545565655454

46、6465464654545464654646543654362dHCSSCSHCSCCSCSSCCCSHSCCSSCCSSCCCHCSSCSHCSCCSCSSCCCSHSCCSSCCSSCCCdTTTTT最后，給出桿件最后，給出桿件6相對于相對于1的位姿矩陣的位姿矩陣1T6為為 1000cossinsinsincos10000100cos0sin0sin0cos35565655454646546465454546465464654222226543261dHCSSCSHSSCCSCSSCCCSHSCCSSCCSSCCCdTTTTTT00)()()()(646546465465264654265

47、2646542652646542652646542CCSCSSCCCSSSCCSSCCSCSCSSCCCSSSSCSSCCCCSSSSCCCC)3813(10)()(254543525425254235254252542dSHSSSdHCCSCHSCCSCSdHCSSCHCCSSCC于是，手部坐標系于是，手部坐標系X6Y6Z6相對于固定坐標系相對于固定坐標系X0Y0Z0的變的變換矩陣換矩陣0T6為為0T6T1T2T3T4T5T6(1339) 假設(shè)給定各個關(guān)節(jié)關(guān)于假設(shè)給定各個關(guān)節(jié)關(guān)于Zi軸的轉(zhuǎn)角軸的轉(zhuǎn)角i，d2、d3和和H的大小，的大小，如表如表132所示，設(shè)所示，設(shè)0T6的矩陣元素如下的矩陣元

48、素如下）（4013100065432160TTTTTTPQMNPQMNPQMNTZZZZYYYYXXXX式式(1337)中各個元素的表達式分別為中各個元素的表達式分別為NXC1C2(C4C5C6S4S6)C6S2S5 (C5C6 S4C4S6) S1NYC2(C4C5C6S4S6)C6S2S5 S1 C1 (C5C6 S4C4S6) NZ(C4C5C6S4S6) S2C2 C6S5MXC1C2(C4C5S6C6S4)S2S5S6 (C5 S4S6C4C6) S1MYS1C2(C4C5S6C6S4)S2S5S6 C1 (C5 S4S6C4C6) MZ(C4C5S6C6S4) S2C2S5 S6Q

49、XC1 (C2C4S5C5S2)S1S4 S5QY(C2C4S5C5S2) S1C1S4 S5QZC4S2 S5C2C5PXC1 d3S2d2S1H(C1C2C4S5C1C5S2S1S4S5)PYS1 d3S2C1d2H(S1C2C4S5C5S1S2C1S4S5)PZC2d3H(C4S2S5C2C5)以上諸式中，以上諸式中，Cicosi，Sisini，i16。那么。那么0T6的運算結(jié)果為的運算結(jié)果為 1000000130001015010060TX1X0Y10圖圖1218 斯坦福機器人斯坦福機器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H

50、13.6工業(yè)機器人的逆向運動學工業(yè)機器人的逆向運動學 工業(yè)機器人的逆向運動學是工業(yè)機器人的逆向運動學是指知被作對象的初始位姿與終止指知被作對象的初始位姿與終止位姿時，如何確定工業(yè)機器人各位姿時，如何確定工業(yè)機器人各關(guān)節(jié)的相對運動量的大小以及末關(guān)節(jié)的相對運動量的大小以及末端操作器的相對位姿。根據(jù)被作端操作器的相對位姿。根據(jù)被作對象的初始位姿與終止位姿，確對象的初始位姿與終止位姿，確定工業(yè)機器人各關(guān)節(jié)的相對運動定工業(yè)機器人各關(guān)節(jié)的相對運動量的大小是對工業(yè)機器人進展運量的大小是對工業(yè)機器人進展運動控制的根底。動控制的根底。 X1X0Y10圖圖1318 斯坦福機器人斯坦福機器人Z0Y0O0X2Y6Y2

51、Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H下面以圖下面以圖1318所示的斯坦福機器人為例，闡明工所示的斯坦福機器人為例，闡明工業(yè)機器人的逆向運動學的求解方法。業(yè)機器人的逆向運動學的求解方法。 斯坦福機器人手部坐標系斯坦福機器人手部坐標系X6Y6Z6相對于固定坐標系相對于固定坐標系X0Y0Z0的變換矩陣的變換矩陣0T6如式如式(12-39)所示，即所示，即0T6T1T2T3T4T5T6，0T6的矩的矩陣方式如式陣方式如式(13-40) 所示。設(shè)所示。設(shè)給定了一切的構(gòu)造參數(shù)并知手給定了一切的構(gòu)造參數(shù)并知手部坐標系部坐標系X6Y6Z6相對于固定相對于固定坐標系坐標系X

52、0Y0Z0的位姿的位姿(式式1340)，令，令H。下面求各個。下面求各個關(guān)節(jié)的相對運動量的大小。關(guān)節(jié)的相對運動量的大小。X1X0Y10圖圖1318 斯坦福機器人斯坦福機器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H(1) 求坐標系求坐標系X1Y1Z1相對于相對于X0Y0Z0的轉(zhuǎn)角的轉(zhuǎn)角1用用T11左乘式左乘式(1340)，得，得T11 0T6T11T1T2T3T4T5T6T2T3T4T5T6，)4113(100000cossin010000sincos111111TX1X0Y10圖圖1318 斯坦福機器人斯坦福機器人Z0Y0O0X2Y6Y

53、2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H其中其中T11由式由式(1329)以及式以及式(1315) 與與(1316)的變換關(guān)系的變換關(guān)系得到，于是得到，于是T11和和T11 0T6分別為分別為求坐標系求坐標系X1Y1Z1相對于相對于X0Y0Z0的轉(zhuǎn)角的轉(zhuǎn)角1)4213(1000100000cossin010000sincos6543211116011TTTTTPQMNPQMNPQMNTTZZZZYYYYXXXX式式(1342)右端右端T2T3T4T5T6的展開矩陣如式的展開矩陣如式(1338)所所示，只需令式示，只需令式(1338)中的中的H0即可。即可。下

54、面展開式下面展開式(1342)中間兩個矩陣的乘積，得中間兩個矩陣的乘積，得T11 0T6為為1000cossincossincossincossinsincossincossincossincos11111111111111116011YXYXYXYXZZZZYXYXYXYXPPQQMMNNPQMNPPQQMMNNTT00)()()()(6465464654652646542652646542652646542652646542CCSCSSCCCSSSCCCSCCSCSCSSCCCSSSSCSSCCCCSSSSCCCC)4313(1025432525423252542dSSdCCCSCSdSC

55、SSCC令式令式(1343)兩個矩陣的第三行第四列的對應(yīng)元素相等，兩個矩陣的第三行第四列的對應(yīng)元素相等，得含有得含有1的三角方程以及的三角方程以及1的解分別為的解分別為 PXsin1PYcos1d2 (1344) )4513()(arctan2)(arctan22222221YYYXXPdPdPdPPx(2) 求坐標系求坐標系X2Y2Z2相對于相對于X1Y1Z1的轉(zhuǎn)角的轉(zhuǎn)角2 PXcos1PYsin1d3sin2 (1346)PZd3cos2 (1347) 式式(1346) 除以式除以式(1347)得得2為為)4813(sincosarctan1122ZYYPPP令式令式(1343) 兩個矩陣

56、的第一行第四列的對應(yīng)元素相等，兩個矩陣的第一行第四列的對應(yīng)元素相等，第二行第四列的對應(yīng)元素相等，得含有第二行第四列的對應(yīng)元素相等，得含有2的三角方程為的三角方程為(3) 求坐標系求坐標系X3Y3Z3相對于相對于X2Y2Z2的位移的位移d3 將式將式(1346)兩端乘以兩端乘以sin2 ，式，式(1347)兩端乘以兩端乘以cos2 ，然后相加得然后相加得d3為為X1X0Y10圖圖1318 斯坦福機器人斯坦福機器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H)4913(cossin)sincos(22113zyxPPPd(4) 求坐標系求坐標系

57、X4Y4Z4相對于相對于X3Y3Z3的轉(zhuǎn)角的轉(zhuǎn)角4由于由于3T6T4T5T6，T4 13T6T4 1T4T5T6T5T64T6T4 1 T3 1 T2 1 T1 10T6。)5013(100000cossin010000sincos444414TX1X0Y10圖圖1318 斯坦福機器人斯坦福機器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H所以，由式所以，由式(1332)、 (1331)、(1330)以及式以及式(1315) 與與(1316)的變換關(guān)系，首先求出的變換關(guān)系，首先求出T4 、T3和和T2的逆矩陣的逆矩陣T41 、T31和和 T

58、21分別為分別為 (4) 求坐標系求坐標系X4Y4Z4相對于相對于X3Y3Z3的轉(zhuǎn)角的轉(zhuǎn)角4)5113(10001000010000113dT)5213(100000cossin10000sincos2222212dTX1X0Y10圖圖1318 斯坦福機器人斯坦福機器人Z0Y0O0X2Y6Y2Z2Z1Y3d3d24321X3Z365X4Z4Z5X5Z6X6Y5Y4H為此，為此，4T6T4 1 T3 1 T2 1 T1 10T6的表達的表達式為式為 100010000100001100000cossin010000sincos34444601112131464dTTTTTT1000100000c

59、ossin010000sincos100000cossin10000sincos111122222ZZZZYYYYXXXXPQMNPQMNPQMNd將將4T6T4 1 T3 1 T2 1 T1 10T6展開展開00)()()()()()()()()()(1142411241142241124211221121142411241142241124CMSMCSMSSMCMCSCNSNCSNSSNCNCSCMSMCMSCNSNCNSCMSMSSMCSMCMCCCNSNSSNCSNCNCCYXZYXYXYXZYXZYXYXZYXYXYX10)()()()()()()()()()(21142411241142411243211221122114241124114241124dCPSPCSPSSPCPCSCQSQCSQSSQCQCSdSPSPCPSCQSQCQSdCPSPSSPCSPCPCCCQSQSSQCSQCQCCYXZYXYXZYXZYXZYXYXZYXYXZYX4T6T4 1 T3 1 T2 1 T1 10T6(13-53)令式令式(1353) 與式與式(1335)的第三行第三列的矩陣元素的第三行第三列的矩陣元素對應(yīng)相等，得關(guān)于轉(zhuǎn)角對應(yīng)相等，得關(guān)