機(jī)器人技術(shù)基礎(chǔ)及應(yīng)用 課件 第3章 工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)

第三章:工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)機(jī)器人技術(shù)基礎(chǔ)及應(yīng)用Fundamentalsandapplicationsofrobotics23.1齊次坐標(biāo)及動(dòng)坐標(biāo)系、對(duì)象物位姿的描述3.2齊次變換3.3工業(yè)機(jī)器人連桿坐標(biāo)系及其齊次變換矩陣3.4工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)方程及其求解目錄3點(diǎn)的直角坐標(biāo)描述點(diǎn)的齊次坐標(biāo)描述坐標(biāo)軸方向的齊次坐標(biāo)描述動(dòng)坐標(biāo)系位姿的齊次坐標(biāo)描述對(duì)象物位姿的齊次坐標(biāo)描述3.1齊次坐標(biāo)及動(dòng)坐標(biāo)系、對(duì)象物位姿的描述4點(diǎn)的直角坐標(biāo)描述式中：Px、Py、Pz是點(diǎn)P在坐標(biāo)系{A}中的三個(gè)位置坐標(biāo)分量。3.1.1點(diǎn)的直角坐標(biāo)描述3.1齊次坐標(biāo)及動(dòng)坐標(biāo)系、對(duì)象物位姿的描述5齊次坐標(biāo)的表示不是惟一的，將其各元素同乘一非零因子

后，仍然代表同一點(diǎn)P，即：3.1.2點(diǎn)的齊次坐標(biāo)描述3.1齊次坐標(biāo)及動(dòng)坐標(biāo)系、對(duì)象物位姿的描述a=ω

Px；b=ω

Py；c=ω

Pz6坐標(biāo)軸方向的描述3.1.3坐標(biāo)軸方向的齊次坐標(biāo)描述3.1齊次坐標(biāo)及動(dòng)坐標(biāo)系、對(duì)象物位姿的描述7矢量位置與方向的描述4?

?1列陣[a

b

cw]T中第四個(gè)元素不為零，則表示空間某點(diǎn)的位置；4?

?1列陣[abcw]T中第四個(gè)元素為零，且滿足a2?+?b2?+?c2?=?1，則表示某軸（矢量）的方向。a=cosα，b=cosβ，c=cosγ8用不同方向角表示方向矢量u、v、w

如圖所示，用齊次坐標(biāo)寫(xiě)出矢量u、v、w的方向列陣。

u:

u=[00.7070.707]Tv:

v=[0.70700.707]Tw:

w=[0.50.50.707]T9連桿位置和姿態(tài)的描述剛體的位置和姿態(tài)3.1.4動(dòng)坐標(biāo)系位姿的齊次坐標(biāo)描述3.1齊次坐標(biāo)及動(dòng)坐標(biāo)系、對(duì)象物位姿的描述10連桿位置和姿態(tài)的描述動(dòng)坐標(biāo)系{B}的位姿描述如圖所示，固連于連桿的坐標(biāo)系{B}位于OB點(diǎn)，Xb?=?2，Yb?=?1，Zb?=?0。在XOY平面內(nèi)，坐標(biāo)系{B}相對(duì)固定坐標(biāo)系{O}有一個(gè)30

的偏轉(zhuǎn)，試寫(xiě)出表示連桿位姿的坐標(biāo)系{B}的44矩陣表達(dá)式。11手部位置和姿態(tài)的描述手部位置及姿態(tài)的描述

12手部位置和姿態(tài)的描述抓握物體Q的手部手部抓握物體Q，物體是邊長(zhǎng)為2個(gè)單位的正立方體，寫(xiě)出表達(dá)該手部位姿的矩陣表達(dá)式。133.1.4對(duì)象物位姿的齊次坐標(biāo)描述3.1齊次坐標(biāo)及動(dòng)坐標(biāo)系、對(duì)象物位姿的描述14平移的齊次變換旋轉(zhuǎn)的齊次變換平移加旋轉(zhuǎn)的齊次變換3.2齊次變換15點(diǎn)的平移變換3.2齊次變換3.2.1平移的齊次變換16相對(duì)于固定坐標(biāo)系平移時(shí)，算子左乘；相對(duì)于動(dòng)定坐標(biāo)系平移時(shí)，算子右乘；亦適用于矢量、坐標(biāo)系、剛體的平移變換。17動(dòng)坐標(biāo)系{A}相對(duì)于固定坐標(biāo)系的X0、Y0、Z0軸作(–1，2，2)平移后到{A

}；動(dòng)坐標(biāo)系{A}相對(duì)于自身坐標(biāo)系的X、Y、Z軸分別作(–1，2，2)平移后到{A

}。A的矩陣表達(dá)式如下。寫(xiě)出坐標(biāo)系{A

}、{A

}的矩陣表達(dá)式。坐標(biāo)系的平移變換18動(dòng)坐標(biāo)系{A}的平移變換算子：19點(diǎn)的旋轉(zhuǎn)變換3.2齊次變換3.2.2旋轉(zhuǎn)的齊次變換202122點(diǎn)的一般旋轉(zhuǎn)變換該式為一般旋轉(zhuǎn)齊次變換通式，概括了繞X、Y、Z軸進(jìn)行旋轉(zhuǎn)變換的情況；相對(duì)于固定坐標(biāo)系旋轉(zhuǎn)時(shí)，算子左乘；相對(duì)于動(dòng)定坐標(biāo)系旋轉(zhuǎn)時(shí)，算子右乘；亦適用于坐標(biāo)軸、矢量、坐標(biāo)系、剛體的旋轉(zhuǎn)變換。verθ=1-cosθ23當(dāng)θ為0

到180

時(shí)，式中取正號(hào)；當(dāng)θ很小時(shí)，很難確定轉(zhuǎn)軸；當(dāng)θ接近0

或180

時(shí)，轉(zhuǎn)軸完全不確定。24兩次旋轉(zhuǎn)變換如圖所示，已知坐標(biāo)系中點(diǎn)U的位置矢量U=[7321]T，將此點(diǎn)繞Z軸旋轉(zhuǎn)90

，再繞Y軸旋轉(zhuǎn)90

，求旋轉(zhuǎn)變換后所得的點(diǎn)W。25手臂轉(zhuǎn)動(dòng)和手腕轉(zhuǎn)動(dòng)如圖所示為單臂操作手，并且手腕也具有一個(gè)旋轉(zhuǎn)自由度。已知手部起始位姿矩陣為G1。若手臂繞Z0軸旋轉(zhuǎn)+90

，則手部到達(dá)G2；若手臂不動(dòng)，僅手部繞手腕Z1軸旋轉(zhuǎn)+90

，則手部到達(dá)G3。寫(xiě)出手部坐標(biāo)系{G2}及{G3}的矩陣表達(dá)式。2627用平移算子（或旋轉(zhuǎn)算子）乘上旋轉(zhuǎn)算子（或平移算子）；并不限定平移變換或旋轉(zhuǎn)變換的次數(shù)或先后順序；運(yùn)算規(guī)則同前，即凡相對(duì)于固定坐標(biāo)系變換則算子左乘，相對(duì)于動(dòng)定坐標(biāo)系平移變換則算子右乘；同樣適用于矢量、坐標(biāo)系、剛體的平移變換。3.2齊次變換3.2.3平移加旋轉(zhuǎn)的齊次變換28如圖所示，已知坐標(biāo)系中點(diǎn)U的位置矢量U=[7321]T，將此點(diǎn)繞Z軸旋轉(zhuǎn)90

，再繞Y軸旋轉(zhuǎn)90

，最后再作4i-3j+7k的平移，求變換后所得的點(diǎn)W。復(fù)合坐標(biāo)變換29連桿參數(shù)連桿坐標(biāo)系的建立連桿坐標(biāo)系之間的齊次變換矩陣3.3工業(yè)機(jī)器人連桿坐標(biāo)系及其齊次變換矩陣30尺寸參數(shù)3.3.1連桿參數(shù)3.3工業(yè)機(jī)器人連桿坐標(biāo)系及其齊次變換矩陣31關(guān)系參數(shù)每個(gè)連桿可以由四個(gè)參數(shù)所描述：其中兩個(gè)描述連桿尺寸，另外兩個(gè)描述連桿與相鄰連桿之間的連接關(guān)系；移動(dòng)關(guān)節(jié)：dn為關(guān)節(jié)變量，其他三個(gè)參數(shù)固定不變；旋轉(zhuǎn)關(guān)節(jié)：

n為關(guān)節(jié)變量，其他三個(gè)參數(shù)固定不變。32連桿n坐標(biāo)系（簡(jiǎn)稱n系）的坐標(biāo)原點(diǎn)位于n+1關(guān)節(jié)軸線上，是關(guān)節(jié)n+1軸線與兩關(guān)節(jié)軸線公垂線的交點(diǎn)；n系的Z軸與n+1關(guān)節(jié)軸線重合；X軸與公垂線重合，從關(guān)節(jié)n指向關(guān)節(jié)n+1；Y軸按右手螺旋法則確定。3.3.2連桿坐標(biāo)系的建立3.3工業(yè)機(jī)器人連桿坐標(biāo)系及其齊次變換矩陣33令n-1系繞Zn-1軸旋轉(zhuǎn)

n角，使Xn-1與Xn平行，算子為Rot(z,

n)；沿Zn-1軸平移dn，使Xn-1與Xn重合，算子為T(mén)rans(0,0,dn)；沿Xn軸平移an，使兩坐標(biāo)原點(diǎn)重合，算子為T(mén)rans(an,0,0)；繞Xn軸旋轉(zhuǎn)

n角，使n-1系與n系重合，算子為Rot(x,

n)。3.3.3連桿坐標(biāo)系之間的齊次變換矩陣3.3工業(yè)機(jī)器人連桿坐標(biāo)系及其齊次變換矩陣34在進(jìn)行機(jī)器人設(shè)計(jì)時(shí)，常常使某些連桿參數(shù)取特別值，如使

n=0或90

，或使dn=0，或使an=0，從而簡(jiǎn)化矩陣An的計(jì)算，同時(shí)也可簡(jiǎn)化控制。35機(jī)器人運(yùn)動(dòng)學(xué)方程正向運(yùn)動(dòng)學(xué)方程求解反向運(yùn)動(dòng)學(xué)方程求解運(yùn)動(dòng)學(xué)方程X=X(q)3.4工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)方程及其求解3636A變換矩陣（A矩陣）：描述一個(gè)連桿坐標(biāo)系與下一個(gè)連桿坐標(biāo)系間相對(duì)關(guān)系（相對(duì)位姿）的齊次變換矩陣。3.4工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)方程及其求解3.4.1機(jī)器人運(yùn)動(dòng)學(xué)方程37手部坐標(biāo)系相對(duì)于固定坐標(biāo)系的位姿等于各連桿坐標(biāo)系之間的變換矩陣的連乘，此即機(jī)器人運(yùn)動(dòng)學(xué)方程。38正向運(yùn)動(dòng)學(xué)方程求解：已知關(guān)節(jié)變量

和d，求手部位姿各矢量n、o、a和p。平面關(guān)節(jié)型機(jī)器人正向運(yùn)動(dòng)學(xué)方程求解斯坦福機(jī)器人正向運(yùn)動(dòng)學(xué)方程求解3.4工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)方程及其求解3.4.2正向運(yùn)動(dòng)學(xué)方程求解3939平面關(guān)節(jié)型機(jī)器人正向運(yùn)動(dòng)學(xué)方程求解SCARA機(jī)器人的坐標(biāo)系連桿轉(zhuǎn)角（變量）θ兩連桿間距離d連桿長(zhǎng)度a連桿扭角α連桿1θ1d1=0a1=l1=100α1=0連桿2θ2d2=0a2=l2=100α2=0連桿3θ3d3=0a3=l3=20α3=0SCARA機(jī)器人連桿參數(shù)40平面關(guān)節(jié)型機(jī)器人正向運(yùn)動(dòng)學(xué)方程求解連桿轉(zhuǎn)角（變量）θ兩連桿間距離d連桿長(zhǎng)度a連桿扭角α連桿1θ1d1=0a1=l1=100α1=0連桿2θ2d2=0a2=l2=100α2=0連桿3θ3d3=0a3=l3=20α3=041平面關(guān)節(jié)型機(jī)器人正向運(yùn)動(dòng)學(xué)方程求解c123=cos(θ1+θ2+θ3);s123=sin(θ1+θ2+θ3);c12=cos(θ1+θ2);s12=sin(θ1+θ2);c1=cosθ1;s1=sinθ142斯坦福機(jī)器人正向運(yùn)動(dòng)學(xué)方程求解斯坦福機(jī)器人及坐標(biāo)系(a)斯坦福機(jī)器人(b)坐標(biāo)系43斯坦福機(jī)器人手臀坐標(biāo)系斯坦福機(jī)器人正向運(yùn)動(dòng)學(xué)方程求解44斯坦福機(jī)器人手臀坐標(biāo)系斯坦福機(jī)器人正向運(yùn)動(dòng)學(xué)方程求解45斯坦福機(jī)器人手臀坐標(biāo)系斯坦福機(jī)器人正向運(yùn)動(dòng)學(xué)方程求解46斯坦福機(jī)器人手腕關(guān)節(jié)斯坦福機(jī)器人正向運(yùn)動(dòng)學(xué)方程求解47斯坦福機(jī)器人手腕坐標(biāo)系斯坦福機(jī)器人正向運(yùn)動(dòng)學(xué)方程求解48斯坦福機(jī)器人手腕坐標(biāo)系斯坦福機(jī)器人正向運(yùn)動(dòng)學(xué)方程求解49斯坦福機(jī)器人手腕坐標(biāo)系斯坦福機(jī)器人正向運(yùn)動(dòng)學(xué)方程求解50斯坦福機(jī)器人正向運(yùn)動(dòng)學(xué)方程求解51反向運(yùn)動(dòng)學(xué)方程求解：己知手部要到達(dá)的目標(biāo)位姿n、o、a和p，求關(guān)節(jié)變量

和d，以驅(qū)動(dòng)各關(guān)節(jié)的馬達(dá)，使手部的位姿得到滿足。斯坦福機(jī)器人反向運(yùn)動(dòng)學(xué)方程求解反向運(yùn)動(dòng)學(xué)方程求解的注意事項(xiàng)3.4工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)方程及其求解3.4.3反向運(yùn)動(dòng)學(xué)方程求解52斯坦福機(jī)器人反向運(yùn)動(dòng)學(xué)方程求解已知斯坦福機(jī)器人的運(yùn)動(dòng)學(xué)方程為T(mén)6=A1A2A3A4A5A6，以及T6矩陣與各桿參數(shù)a、

、d，求關(guān)節(jié)變量

1、

2、d3、

4~

6。求

1：53求

1：斯坦福機(jī)器人反向運(yùn)動(dòng)學(xué)方程求解“+”號(hào)對(duì)應(yīng)右肩位姿，“-”號(hào)對(duì)應(yīng)左肩位姿。54求

2：斯坦福機(jī)器人反向運(yùn)動(dòng)學(xué)方程求解55求d3：求θ4：斯坦福機(jī)器人反向運(yùn)動(dòng)學(xué)方程求解展開(kāi)后取左、右兩邊第三行第三列相等：56展開(kāi)后取左、右兩邊第一行第三列相等、第二行第三列相等：求

5：斯坦福機(jī)器人反向運(yùn)動(dòng)學(xué)方程求解57求

6：展開(kāi)后取左、右兩邊第一行第二列相等、第二行第二列相等：斯坦福機(jī)器人反向運(yùn)動(dòng)學(xué)方程求解58斯坦福機(jī)器人反向運(yùn)動(dòng)學(xué)方程求解分離變量法：將一個(gè)未知數(shù)由矩陣方程的右邊移向左邊，使其與其他未知數(shù)分開(kāi)，解出這個(gè)未知數(shù)，再把下一個(gè)未知效移到左邊，重復(fù)進(jìn)行，直至解出所有未知數(shù)。分離變量法的特點(diǎn)：首先利用運(yùn)動(dòng)方程的不同形式，找出矩陣中能夠簡(jiǎn)單表達(dá)某個(gè)未知數(shù)的元素，力求得到未知數(shù)較少的方程，然后求解。59反向運(yùn)動(dòng)學(xué)方程求解的注意事項(xiàng)解可能不存在工作城外逆解不存在60解的多重性逆解的多重性反向運(yùn)動(dòng)學(xué)方程求解的注意事項(xiàng)61解的多重性PUMA560機(jī)器人的四個(gè)逆解反向運(yùn)動(dòng)學(xué)方程求解的注意事項(xiàng)62解的多重性避免碰撞的一個(gè)可能實(shí)現(xiàn)的解反向運(yùn)動(dòng)學(xué)方程求解的注意事項(xiàng)63解析法：適用于簡(jiǎn)單運(yùn)動(dòng)學(xué)方程數(shù)值法：適用于復(fù)雜運(yùn)動(dòng)學(xué)方程運(yùn)算速度運(yùn)算精度求解方法的多樣性反向運(yùn)動(dòng)學(xué)方程求解的注意事項(xiàng)64角度設(shè)定法RPY角法和歐拉角法表示手部姿態(tài)運(yùn)動(dòng)學(xué)方程X=X(q)3.4工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)方程及其求解3.4.4運(yùn)動(dòng)學(xué)方程X=X(q)65角度設(shè)定法式中矩陣前三列分別是手部坐標(biāo)系的單位方向矢量n、o、a，規(guī)定了手部的姿態(tài)；這種方法在作變換運(yùn)算時(shí)十分方便，但利用它作手部姿態(tài)的描述并不方便，也不直觀；而且n=o

a，9個(gè)元素中只有三個(gè)是獨(dú)立的；這就存在如何用3個(gè)參數(shù)簡(jiǎn)便、直觀地描述手部姿態(tài)的問(wèn)題。通過(guò)連桿坐標(biāo)系之間的變換矩陣A確定手部位姿T的方法所建立的機(jī)器人運(yùn)動(dòng)學(xué)方程為：66角度設(shè)定法：采用相對(duì)于參考坐標(biāo)系或相對(duì)于運(yùn)動(dòng)坐標(biāo)系作三次連續(xù)轉(zhuǎn)動(dòng)來(lái)規(guī)定姿態(tài)的方法。于是，機(jī)器人的手部位姿可用一個(gè)6維列矢量來(lái)表示：角度設(shè)定法式中：x、y、z表示手部位置；

x、

y、

z分別為繞X軸、Y軸和Z軸的轉(zhuǎn)角。67RPY角法和歐拉角法表示手部姿態(tài)RPY角法（x-y-z角設(shè)定法）是手部相對(duì)于參考坐標(biāo)系軸作三次連續(xù)轉(zhuǎn)動(dòng)獲得規(guī)定的姿態(tài)；先繞X軸轉(zhuǎn)動(dòng)

x角，稱為偏轉(zhuǎn)(Yaw)；再繞Y軸轉(zhuǎn)動(dòng)

y角，稱為俯仰(Pitch)；最后繞Z軸轉(zhuǎn)動(dòng)

z角，稱為翻轉(zhuǎn)(Roll)，得到相應(yīng)的旋轉(zhuǎn)矩陣為：6868RPY角法和歐拉角法表示手部姿態(tài)歐拉角法（z-y-x角設(shè)定法）是手部相對(duì)于運(yùn)動(dòng)坐標(biāo)系軸作三次連續(xù)轉(zhuǎn)動(dòng)獲得規(guī)定的姿態(tài)；如果轉(zhuǎn)動(dòng)順序?yàn)閦-y-x，則相應(yīng)的旋轉(zhuǎn)矩陣為：同RPY角法得到的結(jié)果完全相同；如果用其他順序進(jìn)行歐拉角三次連續(xù)轉(zhuǎn)動(dòng)，結(jié)果便不相同了。6969RPY角法和歐拉角法表示手部姿態(tài)知道了旋轉(zhuǎn)矩陣后，則可由以上兩式逆解出手部姿態(tài)的設(shè)定角

x、

y、

z。

x、

y、

z分別為：其中q