第四章機(jī)器人的運(yùn)動(dòng)學(xué)0905

1、末端執(zhí)細(xì)隠撫需 1麴姿態(tài)3.5.1手爪(末端執(zhí)行器)坐標(biāo)系 與手爪固接的坐標(biāo)系叫手爪坐標(biāo)系，圖中的B。 z軸為手指接近物體的方向，稱接近矢量a(approach)； y軸為兩手指連線方向，稱方位矢量O(orientation)；X軸稱法向矢量n(normal),由右手法則確定n=OXa3.5.2機(jī)器人位置與姿態(tài)矩陣n在基礎(chǔ)坐標(biāo)系0中的方位（姿態(tài)）可用n在0 的三個(gè)方向余弦nx ny nz表示，O和a也同樣。 9個(gè)方向余弦組成機(jī)器人手爪姿態(tài)矩陣R 1Inxo xaxR=Joya vR為方向余弦陣nzo zaz手爪坐標(biāo)系坐標(biāo)原點(diǎn)在o坐標(biāo)系中的位置矢量 P= Px Py Pz T組成機(jī)器人手爪位置矩

2、陣P位置與姿態(tài)矩陣將手爪姿態(tài)矩陣R和手 爪位置矩陣P合一，寫 成齊次坐標(biāo)形式，即 得機(jī)器人位置與姿態(tài) 矩陣T其逆變換為'nx ny 血址幻僉-0 0 0-P*a1 -矩陣T的塊分解及其幾 何意義與前述相同第四章機(jī)器人運(yùn)動(dòng)學(xué)機(jī)器人運(yùn)動(dòng)學(xué)是專門研究物體運(yùn)動(dòng)規(guī)律，而在研究 中不考慮產(chǎn)生運(yùn)動(dòng)的力和力矩，它涉及到運(yùn)動(dòng)物體 的位置、速度、加速度和位置變量對(duì)時(shí)間（或其他變 量）的高階導(dǎo)數(shù)。實(shí)際上，機(jī)器人運(yùn)動(dòng)學(xué)研究有兩類問題：一類是給定機(jī)器人各關(guān)節(jié)角度，.要求計(jì)算機(jī)器人手 爪的位置與姿態(tài)問題，稱為正問題；正問題是簡(jiǎn)單 的，解是唯一的另一類是已知手爪的位置與姿態(tài)求機(jī)器人對(duì)應(yīng)于這個(gè) 位置與姿態(tài)的全部關(guān)節(jié)角

3、，稱為逆問題。逆問題的 解是復(fù)雜的，而且具有多解性，這給問題求解帶來 困難，往往需要一些技巧與經(jīng)驗(yàn)4.1連桿坐標(biāo)系和連桿參數(shù)4.1.1機(jī)器人連桿坐標(biāo)系的建立方法 Denavit和Hartenberg于1955年提出了一種建立 構(gòu)件坐標(biāo)系的方法，即D-H方法1）連桿坐標(biāo)系的編號(hào) 從基礎(chǔ)到手部由小到大為關(guān)節(jié)和連桿編號(hào)連桿編號(hào)：1n（基礎(chǔ)的編號(hào)為0）關(guān)節(jié)編號(hào)：0n對(duì)于編號(hào)為i的連桿，靠近基礎(chǔ)的關(guān)節(jié)編號(hào)為I,靠 近手部的關(guān)節(jié)編號(hào)為i連桿i的坐標(biāo)系編號(hào)為i，記為io i固定在 編號(hào)為i關(guān)節(jié)上，隨連桿i運(yùn)動(dòng)2)連桿坐標(biāo)系的建立(1)先建各坐標(biāo)系Z軸：乙坐標(biāo)軸是沿著i關(guān)節(jié)的運(yùn)動(dòng)軸。(2)再建各坐標(biāo)系X軸:X

4、i軸是沿著Zi軸和Zi軸的公垂線,2)連桿坐標(biāo)系的建立指向乙軸的方向(3) 坐標(biāo)系原點(diǎn)Oi為Xi軸和乙軸的交點(diǎn)(4) Yi軸的方向按右手直角坐標(biāo)系來建立圖1轉(zhuǎn)動(dòng)連桿譽(yù)散及坐標(biāo)眾4.1.2連桿參數(shù)1）連桿長(zhǎng)度ai：是Zi-1和Zi兩軸間的公垂線長(zhǎng)度。即卩1和 i坐標(biāo)系原點(diǎn)沿Xi軸的距離Zi-1和Zi兩軸相交時(shí)，ai=O2）偏置di:卩1和 i坐標(biāo)系原點(diǎn)沿Zi-1軸線方向的距離，沿Zi1軸正向時(shí)為正，反之為負(fù)3）連桿扭角ai :將Z"軸繞X軸旋轉(zhuǎn)到乙軸的夾角，以繞Xi右旋為正4）連桿轉(zhuǎn)角（關(guān)節(jié)角）由:Xi1軸和Xi軸的之間的夾角，以繞Zi-I軸右旋為正即連桿i繞Zil軸旋轉(zhuǎn)的角度，右旋為

5、正 亦即i坐標(biāo)系相對(duì)于i1坐標(biāo)系繞Zi-I軸旋轉(zhuǎn)的角 度4. 2機(jī)器人連桿坐標(biāo)系變換矩陣421相鄰坐標(biāo)系變換矩陣1）符號(hào)A：_，表示相鄰兩連桿坐標(biāo)系間的齊次變換矩陣 上標(biāo)（i1）表示變換的目的（變換后）坐標(biāo)系 下標(biāo)（i ）表示變換前的坐標(biāo)系機(jī)器人運(yùn)動(dòng)學(xué)正問題求解，變換總是向序號(hào)減少的 方向進(jìn)行，所以a；1的上標(biāo)可省略，簡(jiǎn)寫為4只有相鄰兩坐標(biāo)系間的齊次變換，且變換向序號(hào)減 少的方向進(jìn)行，上標(biāo)才可省略2) Ai的一般形式(對(duì)于轉(zhuǎn)動(dòng)連桿坐標(biāo)系)按D-H方法，當(dāng)四個(gè)連桿參數(shù)均不為零時(shí)，門和“ 間包括四個(gè)齊次變換(1爛/角戒陰關(guān)與xi同-平面內(nèi)。(2)擔(dān)亦斛移-噌滋戒躺闕X*同-験匕II坐標(biāo)系i1經(jīng)上面

6、四次有順序的相對(duì)變換可得到坐 標(biāo)系i,故：Ai=Rot(Zi-1,0i)Trans(O,O,d i)Trans(ai,0,0)Rot(Xi,ai)】00 OOS 竝0銅附JO 00 tfI-沁0朋勺00 L訕0 0*誠(chéng)Q 001000L1 0 0此0 10 0i0 0 1 dib 0 0 L:調(diào) * sin&oos嗎sin oosgooB心0血您I-0 0K8附占01 n4.2.2機(jī)器人連桿坐標(biāo)系變換矩陣序列對(duì)于nn-1 n+1個(gè)連桿的機(jī)器人，有n個(gè)連桿坐標(biāo)系，再 力口 1個(gè)基礎(chǔ)坐標(biāo)系，共計(jì)n+1坐標(biāo)系。即：0（基礎(chǔ)坐標(biāo)系）、2、n（手爪坐標(biāo)系）相鄰兩坐標(biāo)系間的齊次變換矩陣共計(jì)n個(gè)，即

7、：A1、A2、Ai、An,其中A1為向基礎(chǔ)坐標(biāo)系0的變換矩陣An為手爪坐標(biāo)系向 n1坐標(biāo)系的變換矩陣機(jī)器人手爪坐標(biāo)系相對(duì)于基礎(chǔ)坐標(biāo)系（參考坐標(biāo)系） 的變換為（設(shè)n=6）A： =A1 -A2-A3-A4-A5-A6 =T即機(jī)器人位置與姿態(tài)矩陣T4. 3典型機(jī)器人運(yùn)動(dòng)學(xué)正問題角軍431轉(zhuǎn)動(dòng)坐標(biāo)臂(RRR)轉(zhuǎn)動(dòng)臂的結(jié)構(gòu)如 圖，該機(jī)械臂有 三個(gè)轉(zhuǎn)動(dòng)連桿， 類似人的腰、肩、 肘建立DH坐標(biāo)系DH參數(shù)表.關(guān)節(jié)變量為圖加帖球坐標(biāo)骨緒物示意sinco50*o遜-cosSjSinA,=0楓cosa；J 001 1 p 01sT01 5j0-一門 01d4 11 0 10 01.0 00 L表3-8轉(zhuǎn)動(dòng)坐標(biāo)雪D

8、H蠢數(shù)A1dj1!ffi00T1J20030«.10_旳0吆訂Ai -04 2盹001 00°0 1 J0A 3 5j5i 00陽(yáng)工血侃+肘,亀=怕£他+0)0T = A3=A Ai Asj嗨 0 ai+ai宓 0 旳切十砌®0 1 000 Jj h （如艱+郵1勺蘇+的®）0Q譏汁兔耳4.3.2斯坦福六自由度機(jī)器人(Schei nman)斯坦福機(jī)器人結(jié)構(gòu)如 圖所示該機(jī)器人由 球面坐標(biāo)臂(RRP)和 歐拉腕組成，其連桿坐標(biāo)系 的設(shè)置如圖，圖 中,乙軸沿關(guān)節(jié)i 的軸，令所有 Xi軸與xO軸平. 行，y軸按右手 法則確定.原點(diǎn)Oo, 01重 合，原

9、點(diǎn)03, 04, 05, 06 重合，此處未考慮終 端操作裝置的 位移d6訕叫站曲 TO呦叫妙誠(chéng) i0 1 -旳0|Ci 010Il,1- r0 IIFC屯 654,亠6 N PagM冬+*0I54EB+CAL01a6 =2A 2 人6 =。仏坤廠応2卻5心丹百一山鞠）+（埼5“%g+嵌0"（勺偽5& +瓜1）+曲尿 一血（門甲6+環(huán)"加撫車陸+耳“0.皿3-Ms1L0 00Kpy11扯=門畑G心心»打）一比3$5心§1 （.»門為+"鞠）*ny h 51匸辭疋疋6 $4砌）頂塚“+心I歸匸曲$ +3）?n£=皎（f

10、iCjCs 一 £ 諷）銳矗創(chuàng) 1%=門切 3心応+s*Q+出詼S訂一*1 （ §4邙&+"） t 孫=吐一臨（7肛曲5血）+肋5浙+門（一為心陀+門為）, 依=S齣（心儒爲(wèi)+胡口）+"汕M歸血二門（加常5 +晦民）亠5】$血,聽大価（"燦斯+ ”込）+g$“5,鳥=k伽G為+亡£3山?松=G出血込, 筠=5曲必+門皿,4.3.3PUMA560六自由度機(jī)器人PUMA560機(jī)器 人結(jié)構(gòu)如圖3- 17所示。該機(jī) 器人有轉(zhuǎn)動(dòng)坐 標(biāo)臂（RRR） 和歐拉腕組成。其連桿坐標(biāo)系 的設(shè)置見圖3- 17關(guān)節(jié)變量為ffi J-I7 PUMA56O

11、 US人結(jié)構(gòu)示童圖兀5)%")ft.6*4卜日"仇連桿D-H參數(shù)見表3-103-10 PUMA560機(jī)墨數(shù)連律、ft1 1 1；02304:人DH卷數(shù)%0 -12%00兀207T_ 201K2000連桿的D-H坐標(biāo)變換矩陣為:!Ci010filoioi'0CiQ) o-1b10!Ia10Ij1.出fi5001I.0(Tc61.I1JC4C5C6 S4S655 c006S5 d%5054C5C6 + C4560IC4O5S61S4<r(455 d6c45 IS4C5S6 十 456 =55 65456 %6J?d6c+d4 0 0 1FlLo0 .0 1 aRl

12、 HC3 (C4C5C6 S4S6) 53S5O6 M »71 H S3 (C4C5C61st>6 ) +C3S5C6 9aNl =54C5C6 + C4S6 9owl H $ ( C4C36 I S36 ) + S3S5COS H 53 (IC4C5C615上6 ) IC3S5S6ONI H S4C5C6+C4C6 9awluc3c455+s3c5 9as=s3c4s5lc3c5F1NS4S5 9PHI HC3c455a6 + s3(c5a6+a4) 90SHS3c4s5d6lc3(c5d6 + d4) 9H5455*3.如2°工242PxZA 1 4 4 2%&#

13、176;y2ay2PyZ人6 =人2人6 =%2°z2a定2Pz2.0001 .心2 =。23（。4。5。6 環(huán)6）5加5。6，= 235 "H235，砒2 = §23（C4C5C6S4S6 ） + C23$5%，=$23。4$5。23。5，"疋=»。5。6+。4$6 92 = 5，0恣=“3（C4C5C6 敞6）+$23$5$6,久2=。23。4$5力6+523（。56+4）+42。2,Oy2 = $23（ C4C5C6一$疋6）一（2355$6,角2=523加5力6“3（。56 +力4）+©切0垃=_3疋5幷+。疋6，卩唸=沁必

14、+ 血運(yùn)動(dòng)學(xué)方程為：nx°工axpxA：=人1人2人3人4人5人6二nzOyOzPyPz0001_兀產(chǎn)門他（嘆勺陽(yáng)-甜離）-陽(yáng)舟為J鎖曲冰§ + 5坯）* 旳二$1【（ C斗礎(chǔ)礎(chǔ)舁$& ） - $若舟巾+勺（旳門心+門顯）1 力二一 $23（H丹礎(chǔ)-弭為）"£第巧卯，% = “-切（“C“6 + $4卬）+也巧和$一阮曲+訶“）色=$1強(qiáng)（"好 +呼6）卡他巧+5（-唧肌+印蟲）% = $23 （勺鬥并 + 叭）+ 236,axC(G3Q4S5+S23C5)吐$4$5 ,dy = S(C23C4S5 + S23C5)+55465 ,dr

15、 = $23“$5 + C23©5 tAr = C 6(C23C4S5 + $23C5)+ 4$23 +2C2 一 $1 (6S4$5 +必2)，Py S 6(C23Q4S5 +*36)+/1S23 +如彳+C1(力654$5 +2)， pH = d§("3C5 $23°4$5)+ 4°23 一若令01=90。, 02=0° , 63=90° , 04=0° , 05=0° , 06=0° ,并將有關(guān)常量代人矩陣T ,則有!-10"493001921.12T產(chǎn)-1000-0001 62

16、3“ 小4S23C4C1S23B23°23C1G3S4sg5疋23$4+°1°4S23S40623/4+02°1°2 力2*1SS234 +22S1Cz+/2G。23力4 2S2自左而右點(diǎn)乘的結(jié)果"2小25i26*2 2$1 0S1C2sls2Cl£ = A. A =厶1ZS2C20戀2一 0001 一clc23C1S23OBI 2S1S1C2351523如$疋2+/26 $230c23。2匕.0001 -1nx3 = C1C23C4C5 一6出3$5，flx3 C1C23C4C5 SiS4S5 + *15235，卩刃=$疋

17、23。405 +C1S4C5 $1$23$5，<2y3 =$疋23。455 + $1$23心5 >nx3= S23C4C5 C23$5,Oz3= C1C23S4 S1C4,Oy3S1C23S4+c1C4 ,= $23°4$5 +C23C5， 力 3 = 426。2£"1， pyz=dzSC2rdC t。瓷2 =旳3$4 94.4機(jī)器人運(yùn)動(dòng)學(xué)逆問題已知機(jī)械手末端工具在基礎(chǔ)坐標(biāo)系中的位置和 姿態(tài)，求機(jī)器人各關(guān)節(jié) 在自身坐標(biāo)系中的位移，稱 機(jī)器人運(yùn)動(dòng)學(xué)逆問題。目前，已經(jīng)能夠?qū)σ话憬Y(jié)構(gòu)的六自由度串聯(lián)機(jī)器人進(jìn) 行逆運(yùn)動(dòng)學(xué)求解。但是，要獲得顯式解，只有滿足 下列兩

18、個(gè)九分條件之一：(1) 3個(gè)相鄰關(guān)節(jié)軸交于一點(diǎn)；(2) 3個(gè)相鄰關(guān)節(jié)軸平行。【例】已知斯坦福機(jī)器人位置和姿態(tài)，即已知T矩陣 中各元素的值，試確定機(jī)器人各關(guān)節(jié)變量。«：用 左乘式T=得A. A2 A4 As A6右端可算岀:方程式（225）左端為cisi00爪工OjcQ咒Px0 0-10ny Oy ay PyAi lT =一 si ci00處 0役 Pz-000 L-000 KVll（«） /ll（o）可寫為:Ai1T =fi2（o）/13（o）L 00十13（Q）0Ju（P） fl2（P） /b（）式中:Zllf+s&/13（0= Si 行+q 務(wù) i = n、o

19、心 p右端可算岀:右端可算岀:弭3di1 -% =1A22A33A44A55A6C2C4S5+S2C5汕丙 C2C50式中3行4列元素為常數(shù)，利用左右端對(duì)應(yīng)元素的相等關(guān)系可得Mp)= d2即 _+ CYPy = d2為了解此類方程，作如下三角代換:式中：=J代+處px rcos lpy= rsin0 arctanPyJpx >故： sin0cos0 - cos0sin0 = dr簡(jiǎn)化成：sin（0 久）=d/r故:COS（ /_&）= ± 丿1厶/（甲-0） = 士 -仏/廠）2進(jìn)而可得：Q必2<p tfi = arctan±2-衛(wèi)4最后得:9i = a

20、rctanPx-arctan± /r2 - dj根據(jù)機(jī)器人運(yùn)動(dòng)連續(xù)性及回避障礙的需要，確定一個(gè)&嚴(yán)由左 右端1行4列及2行4列對(duì)應(yīng)元素相等，列岀：d3-clpx + s1py_C23= Pz由于d3大于0,故可惟一確定0 2込=arctan沁嚴(yán)Al同時(shí)可確定d3為：= S2（C1Z* 十 Si/，） + C2p±類似地有:-1 -1 -1 -1T 6 = A5 AeA4 A3 A2 Al T741( Zl)Zh(o)九1（。）(T7$%(T九2（兀）人2（0）/42S)0%心一昭一 C50加兀）人3（0）/43(a)0$C600-000L-000L可與為:式中:%

21、二 q；C2(ci + sdy) - 切訂+ w( - siz； + ciL)/42(門=一 *C 凡+ SiiJ_C2仃人3(：)二一印C2(CU； + Si® 訂 + q( _ S2； + Cj iy) (z = w ,o,a ,/>)由式（234）第3行3列為0可得：/43（）=0WC2（C1島+ siaj -包色+C4（ -$但工 + 勺幻）二0 解得：.41 - arctan -廠 二罟丄普 _C2（CiQh 十 Sid/-包如042 = arctan_（ 一S妝+ c®）一 C2（C14+ siaj-巳色由第1行3列和第2行3列對(duì)應(yīng)元素相等可得:S5 二

22、C4C2（CiQ 工 + Si。，）_ §2役+ M 一 Sg + C®C5 = S2（C1Q% + Siaj+C2d解得:+ s4"Siflr + ciav.n C4C2（Cg 如上込0F如+s®）+C2 兔類似地由:1 1 1 1 1A5 A4 A3 A2 Al T = A6可得:皿）fM o o' fiM 辦2（0）0 0人3（0）1 0C6S600S6C6000 0 10_000L-000 L由第1行2列和第2行2列對(duì)應(yīng)元素相等可得: 薩飛血血+抽卜処】+W卜龜+測(cè)+85【乂心+訕局心 曠飛b血+輛飛oj+c4卜槍+qoj解得：% = a

23、rctan °C6PU MA-560 機(jī)器人的運(yùn) 動(dòng)學(xué)逆解可 能存在4個(gè)解。 這是因?yàn)樵?求解3和63 的式中出現(xiàn) 正負(fù)號(hào)，故 可能得4個(gè)解。 圖給出了這4 種解的對(duì)應(yīng) 形態(tài)? PUMArS60機(jī)器人的4科運(yùn)動(dòng)學(xué)逆壽4. 5雅可比矩陣已討論了剛體的位姿描述、齊次變換，機(jī)器人各連桿間的位移 關(guān)系，建立了機(jī)器人的運(yùn)動(dòng)學(xué)方程，研究了運(yùn)動(dòng)學(xué)逆解，建立 了操作空間與關(guān)節(jié)空間的映射關(guān)系.本節(jié)將在位移分析的基礎(chǔ) 上，進(jìn)行速度分析，研究基礎(chǔ)空間速度與關(guān)節(jié)空間速度之間的 線性映射關(guān)系雅可比矩陣（簡(jiǎn)稱雅可比）.雅可比矩陣不僅用來表示基礎(chǔ)空間與關(guān)節(jié)空間之間的速度線性 映射關(guān)系，同時(shí)也用來表示兩空間之間力的

24、傳遞關(guān)系45. 1雅可比矩陣的定義 1 ）疋義. 把.機(jī)器人關(guān)節(jié)速度向量定義為（i=1,2,為連桿湘對(duì)i1的角速度或線速度.手爪在基v=N=x $ 叫烏亦tq、坐標(biāo)系中的廣義速度向量為:， 式中，V為線速度，3為角速度分量.從關(guān)節(jié)空間速度向基礎(chǔ) 空間速度V映射的線性關(guān) 系稱為雅可比矩陣，簡(jiǎn)稱Jacobian,記為 J2）數(shù)學(xué)表達(dá)式<><11 心、I呉5，訶Q在數(shù)學(xué)上，機(jī)器人終端手 爪的廣義位置（位姿）矢量P 可寫成：上式對(duì)時(shí)同求導(dǎo)"有:對(duì)隔式（4-3） W 式（4-丘八可知=4. 5. 2雅可比矩陣的物理意義&百s S育畫 瓷器_斗右転亟亠4. 5. 2雅可比矩陣的物理意義對(duì)于有n