《機(jī)器人技術(shù)-建模、仿真及應(yīng)用》課件 第二章機(jī)器人運(yùn)動(dòng)學(xué)

ROBOT機(jī)器人技術(shù)——建模、仿真及應(yīng)用機(jī)器人運(yùn)動(dòng)學(xué)第二章目錄數(shù)學(xué)基礎(chǔ)PART.1運(yùn)動(dòng)學(xué)分析PART.2數(shù)學(xué)基礎(chǔ)PART.1位置與位姿齊次變化仿真實(shí)例數(shù)學(xué)基礎(chǔ)位置描述對(duì)于直角坐標(biāo)系{A}，空間任一點(diǎn)的位置可用3×1階的列矢量來表示(也稱位置矢量)：式中：Px、Py、Pz是點(diǎn)P在坐標(biāo)系{A}中的三個(gè)位置坐標(biāo)分量。數(shù)學(xué)基礎(chǔ)位置描述向量可以由3個(gè)起始和終止的坐標(biāo)來表示。P=（

PX

-OX

）

i

+（PY

-OY

）

j

+（

PZ

-OZ

）

k

若O為原點(diǎn)：式中：Px、Py、Pz是向量在坐標(biāo)系{A}中的三個(gè)位置坐標(biāo)分量。向量的3個(gè)分量也可寫成矩陣形式。(PX=cos

，PY=cos

，PZ=cos)（，，J為機(jī)械臂的雅可比矩陣（6×n矩陣））（為關(guān)節(jié)速度對(duì)末端執(zhí)行速度的3*n作用矩陣）雅可比矩陣取一個(gè)自由度為n的機(jī)械臂，其正運(yùn)動(dòng)學(xué)方程如下：（旋轉(zhuǎn)矩陣R和位移矢量P都是關(guān)于變量

的矩陣方程）將末端執(zhí)行器的線速度

和角速度

表示為所有關(guān)節(jié)速度

的函數(shù)：（為關(guān)節(jié)角速度對(duì)末端執(zhí)行速度的3*n作用矩陣）兩個(gè)方程的緊湊形式：數(shù)學(xué)基礎(chǔ)矢量u、v、w的坐標(biāo)方向用齊次坐標(biāo)表示。例位置描述數(shù)學(xué)基礎(chǔ)姿態(tài)描述規(guī)定空間某剛體B的方位，設(shè)一坐標(biāo)系{B}與此剛體固連，物體相對(duì)于參考坐標(biāo)系{A}的姿態(tài)相對(duì)于參考坐標(biāo)系{A}的方向余弦組成的3×3矩陣旋轉(zhuǎn)矩陣：用矢量?jī)蓛芍g的余弦則表示為：數(shù)學(xué)基礎(chǔ)姿態(tài)描述對(duì)應(yīng)于x、y或z軸做旋轉(zhuǎn)角為的旋轉(zhuǎn)變換，其旋轉(zhuǎn)矩陣分別為：旋轉(zhuǎn)矩陣應(yīng)具有以下幾個(gè)特點(diǎn)：1）3個(gè)主矢量?jī)蓛纱怪保?）9個(gè)元素中，只有3個(gè)是獨(dú)立的；3）3個(gè)單位主矢量滿足6個(gè)約束條件，即：4）旋轉(zhuǎn)矩陣為正交矩陣，并且滿足以下條件，即：數(shù)學(xué)基礎(chǔ)齊次變化齊次坐標(biāo)是指在原有三維坐標(biāo)的基礎(chǔ)上，增加一維坐標(biāo)而形成四維坐標(biāo)。如：空間點(diǎn)p的齊次坐標(biāo)為p=(4,6,8,w)4、6、8分別對(duì)應(yīng)p點(diǎn)在空間坐標(biāo)系中的x、y、z軸坐標(biāo)，w為其對(duì)應(yīng)的比例因子。p=（4,6,8,1）和p=(8,12,16,2）表示的是同一個(gè)p點(diǎn)。當(dāng)比例因子w≠0時(shí)p點(diǎn)的齊次坐標(biāo)的形式是不唯一的當(dāng)比例因子w=0時(shí)該齊次坐標(biāo)表示某一向量如：x=（1，0，0，0）表示坐標(biāo)系的x軸單位向量。y=（0，1，0，0）表示坐標(biāo)系的y軸單位向量。z=（0，0，1，0）表示坐標(biāo)系的z軸單位向量。對(duì)應(yīng)于x、y、z軸做轉(zhuǎn)角位

的旋轉(zhuǎn)變換，分別可得：數(shù)學(xué)基礎(chǔ)齊次變化平移齊次坐標(biāo)變換

對(duì)已知矢量

進(jìn)行平移變換所得的矢量

為：

旋轉(zhuǎn)齊次坐標(biāo)變換Rot表示旋轉(zhuǎn)矩陣。齊次變換平移齊次變換動(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á)式如下。寫出坐標(biāo)系{A

}、{A

}的矩陣表達(dá)式。例齊次變換平移齊次變換動(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á)式如下。寫出坐標(biāo)系{A

}、{A

}的矩陣表達(dá)式。例動(dòng)坐標(biāo)系{A}的平移變換算子：齊次變換旋轉(zhuǎn)齊次變換已知坐標(biāo)系中點(diǎn)U的位置矢量U=[7321]T，繞Z軸旋轉(zhuǎn)90°，再繞Y軸旋轉(zhuǎn)90°，求旋轉(zhuǎn)變換后所得的點(diǎn)W。例數(shù)學(xué)基礎(chǔ)齊次變化平移與旋轉(zhuǎn)齊次坐標(biāo)組合變換根據(jù)平移齊次坐標(biāo)變換和旋轉(zhuǎn)齊次坐標(biāo)變換，空間某點(diǎn)由矢量

描述，其中i、j、k分別為x、y、z軸上的單位矢量，然后對(duì)應(yīng)于x、y、z軸做轉(zhuǎn)角為的旋轉(zhuǎn)變換，分別可得：齊次變換復(fù)合變換已知坐標(biāo)系中點(diǎn)U的位置矢量U=[7321]T，將此點(diǎn)繞Z軸旋轉(zhuǎn)90°，再繞Y軸旋轉(zhuǎn)90°，最后再作4i-3j+7k的平移，求變換后所得的點(diǎn)E。例數(shù)學(xué)基礎(chǔ)仿真實(shí)例平移坐標(biāo)變換實(shí)例T0=transl(0,0,0)T1=transl(1,2,1)trplot(T0,'color','r')holdontrplot(T1,'color','g')axis([-33-33-33])tranimate(T0,T1)例代碼：坐標(biāo)系由原點(diǎn)(0,0,0)分別沿x、y、z軸平移1、2、1個(gè)單位。數(shù)學(xué)基礎(chǔ)仿真實(shí)例平移坐標(biāo)變換實(shí)例T0=rotz(0)T0=rotz(0)T1=rotz(pi/4)trplot(T0,'color','r')axis([-11-11-11]);oldontranimate(T0,T1,'color','b')例代碼：坐標(biāo)系在原點(diǎn)位置繞z軸旋轉(zhuǎn)45°。數(shù)學(xué)基礎(chǔ)仿真實(shí)例先平移再旋轉(zhuǎn)實(shí)例T0=transl(0,0,0)T1=transl(1,2,1)trplot(T0,'color',‘r’)；holdon；trplot(T1,'color','g')axis([-33-33-33])tranimate(T0,T1)T2=T1T3=T2*trotz(pi/2)trplot(T2,'color','r')holdontrplot(T3,'color','g')axis([-33-33-33])tranimate(T2,T3)平移旋轉(zhuǎn)例代碼：坐標(biāo)系從原點(diǎn)位置(0,0,0)先分別沿著x、y、z軸平移1、2、1個(gè)單位，再繞z軸逆時(shí)針旋轉(zhuǎn)90°。數(shù)學(xué)基礎(chǔ)仿真實(shí)例先旋轉(zhuǎn)再平移實(shí)例T0=trotz(0)T1=trotz(pi/2)trplot(T0,'color','r’);holdon；trplot(T1,'color','g')tranimate(T0,T1)T2=T1T3=transl(1,2,1)*T2trplot(T2,'color','r’);holdon；trplot(T3,'color','g')axis([-33-33-33])tranimate(T2,T3)平移旋轉(zhuǎn)例代碼：坐標(biāo)系從原點(diǎn)位置(0,0,0)先繞z軸逆時(shí)針旋轉(zhuǎn)90°，再分別沿著x、y、z軸平移1、2、1個(gè)單位。數(shù)學(xué)基礎(chǔ)仿真實(shí)例旋轉(zhuǎn)和平移同時(shí)進(jìn)行實(shí)例T1=transl(0,0,0)T2=transl(1,2,1)T3=trotz(pi/2)T4=T2*T3trplot(T1,'color','r')holdontrplot(T4,'color','g')axis([-33-33-33])tranimate(T4,'color','b')例代碼：坐標(biāo)系從原點(diǎn)位置分別沿著x、y、z軸平移1、2、1個(gè)單位，同時(shí)繞z軸逆時(shí)針旋轉(zhuǎn)90°。運(yùn)動(dòng)學(xué)分析PART.2正運(yùn)動(dòng)學(xué)分析D-H參數(shù)法正運(yùn)動(dòng)學(xué)仿真逆運(yùn)動(dòng)學(xué)分析逆運(yùn)動(dòng)學(xué)仿真運(yùn)動(dòng)學(xué)分析正運(yùn)動(dòng)學(xué)分析機(jī)器人本體，是機(jī)器人賴以完成作業(yè)任務(wù)的執(zhí)行機(jī)構(gòu)。機(jī)械臂多采用關(guān)節(jié)式機(jī)械結(jié)構(gòu)，一般具有6個(gè)自由度，其中3個(gè)用來確定末端執(zhí)行器的位置，另外3個(gè)則用來確定末端執(zhí)行裝置的方向。機(jī)械臂上的末端執(zhí)行裝置可以根據(jù)操作需要換成焊槍、吸盤、扳手等作業(yè)工具。運(yùn)動(dòng)學(xué)分析D-H參數(shù)法連桿n坐標(biāo)系（簡(jiǎn)稱n系）坐標(biāo)原點(diǎn)位于i關(guān)節(jié)軸線上，是關(guān)節(jié)i的關(guān)節(jié)軸線與i-1和i關(guān)節(jié)軸線公垂線的交點(diǎn)；Z軸與i關(guān)節(jié)軸線重合；X軸與公垂線重合，從關(guān)節(jié)i-1指向關(guān)節(jié)i；Y軸按右手螺旋法則確定。運(yùn)動(dòng)學(xué)分析D-H參數(shù)法每個(gè)連桿可以由四個(gè)參數(shù)所描述名稱含義“

”號(hào)性質(zhì)轉(zhuǎn)角以

方向看，

和

之間的夾角右手法則常量長(zhǎng)度沿著

方向，

和

之間的距離與

正向一致常量關(guān)節(jié)角以

方向看，

和

之間的夾角右手法則轉(zhuǎn)動(dòng)關(guān)節(jié)為變量移動(dòng)關(guān)節(jié)為常量距離沿著

方向，

和

之間的距離沿

正向?yàn)檎D(zhuǎn)動(dòng)關(guān)節(jié)為常量移動(dòng)關(guān)節(jié)為變量在D-H法分析中，連桿坐標(biāo)系

相對(duì)于

的變換

稱為連桿變換矩陣，連桿變換矩陣

相當(dāng)于坐標(biāo)系

經(jīng)過以下變換得到：運(yùn)動(dòng)學(xué)分析D-H參數(shù)法1)繞

軸旋轉(zhuǎn)，使得與平行，如圖a)所示；2)沿軸移動(dòng)，使得與在同一直線上，如圖b）11所示；3)繞軸旋轉(zhuǎn)，使得轉(zhuǎn)到與平行，如圖c）所示；4)沿軸移動(dòng)，使得連桿坐標(biāo)系的原點(diǎn)與的原點(diǎn)11重合，如圖d)所示。由此可得旋轉(zhuǎn)變換矩陣為：D-H法矩陣變換過程運(yùn)動(dòng)學(xué)分析D-H參數(shù)法如圖所示的平面三連桿機(jī)構(gòu)，已知手臂長(zhǎng)

、和，關(guān)節(jié)變量、和，試求末端執(zhí)行器位姿矩陣。例i1000200300解：建立機(jī)械臂各桿的坐標(biāo)系，列出D-H參數(shù)。運(yùn)動(dòng)學(xué)分析D-H參數(shù)法如圖所示的平面三連桿機(jī)構(gòu)，已知手臂長(zhǎng)

、和，關(guān)節(jié)變量、和，試求末端執(zhí)行器位姿矩陣。例==運(yùn)動(dòng)學(xué)分析正運(yùn)動(dòng)學(xué)仿真調(diào)用MATLAB機(jī)器人工具箱，使用D-H參數(shù)法設(shè)置三連桿機(jī)械臂桿長(zhǎng)分別為30、50、40；關(guān)節(jié)角、連桿偏距、連桿轉(zhuǎn)角都為0。例a1=30;a2=50;a3=40;L(1)=Link([00a10])L(2)=Link([00a20])L(3)=Link([00a30])robot=SerialLink(L)teach(robot)代碼：運(yùn)動(dòng)學(xué)分析正運(yùn)動(dòng)學(xué)仿真調(diào)用MATLAB機(jī)器人工具箱，使用D-H參數(shù)法設(shè)置三連桿機(jī)械臂連桿1的桿長(zhǎng)為30，連桿轉(zhuǎn)角為90°，關(guān)節(jié)角為0°，連桿偏距為0；連桿2的桿長(zhǎng)為50，連桿轉(zhuǎn)角為0，關(guān)節(jié)角為0°，連桿偏距為20；連桿3的桿長(zhǎng)為40，連桿轉(zhuǎn)角都為0，關(guān)節(jié)角為0°，連桿偏距為0。例L_1=30;L_2=50;L_3=40;L(1)=Link([00L_1pi/2])L(2)=Link([020L_20])L(3)=Link([00L_30])Robot=SerialLink(L);teach(Robot)代碼：運(yùn)動(dòng)學(xué)分析逆運(yùn)動(dòng)學(xué)分析實(shí)質(zhì)：已知BTH求解θ，從而確定與末端位置有關(guān)的所有關(guān)節(jié)的位置——實(shí)際工程問題已知操作機(jī)桿件的幾何參數(shù)，給定操作機(jī)末端執(zhí)行器相對(duì)于參考坐標(biāo)系的期望位置和姿態(tài)（位姿），操作機(jī)能否使其末端執(zhí)行器達(dá)到這個(gè)預(yù)期的位姿？若能達(dá)到，那么操作機(jī)是否存在不同形態(tài)可滿足條件？逆向運(yùn)動(dòng)學(xué)逆運(yùn)動(dòng)學(xué)分析可解性解的存在問題取決于操作末端的工作空間（Workspace）工作空間：操作臂末端執(zhí)行器所能到達(dá)的范圍，取決于機(jī)器人結(jié)構(gòu)、桿件參數(shù)或手部位姿。工作域外逆解不存在具有轉(zhuǎn)動(dòng)和移動(dòng)關(guān)節(jié)的機(jī)器人，在單一串聯(lián)鏈中共有個(gè)6自由度或小于6個(gè)自由度時(shí)是可解的。通解是數(shù)值解，非解析表達(dá)式，是利用數(shù)值迭代原理求解得到，計(jì)算量比求解析解大得多。要使機(jī)器人有解析解，設(shè)計(jì)時(shí)就要使機(jī)器人的結(jié)構(gòu)盡量簡(jiǎn)單，而且盡量滿足連續(xù)三個(gè)旋轉(zhuǎn)關(guān)節(jié)的旋轉(zhuǎn)軸交會(huì)于一點(diǎn)，或連續(xù)三個(gè)關(guān)節(jié)軸互相平行的充分條件。（Pieper準(zhǔn)則）逆向運(yùn)動(dòng)學(xué)多解性對(duì)于給定位置與姿態(tài)，具有多組解。造成運(yùn)動(dòng)學(xué)逆解多解是由于解反三角函數(shù)方程產(chǎn)生的。PUMA560機(jī)器人的四個(gè)逆解避免碰撞的一個(gè)可能實(shí)現(xiàn)的解逆運(yùn)動(dòng)學(xué)分析逆向運(yùn)動(dòng)學(xué)逆運(yùn)動(dòng)學(xué)分析求解方法逆解形式求解方法閉式解close-formsolution用解析函數(shù)式表示解求解速度快代數(shù)法幾何法數(shù)值解numericalsolution利用迭代性質(zhì)求解求解速度慢數(shù)值法逆向運(yùn)動(dòng)學(xué)逆運(yùn)動(dòng)學(xué)分析代數(shù)法根據(jù)正運(yùn)動(dòng)學(xué)分析，設(shè)機(jī)械臂腕關(guān)節(jié)的位置坐標(biāo)為姿態(tài)角

，機(jī)械臂執(zhí)行端坐標(biāo)為

?；贒-H坐標(biāo)系的機(jī)械臂運(yùn)動(dòng)學(xué)方程如下：平面三連桿機(jī)械臂代數(shù)法求逆運(yùn)動(dòng)學(xué)可知：逆向運(yùn)動(dòng)學(xué)逆運(yùn)動(dòng)學(xué)分析由矩陣兩邊對(duì)應(yīng)相等，結(jié)合上式，可得腕部坐標(biāo)

的表達(dá)式為：即：上式有解的條件是等式右邊值的區(qū)間為[-1,1]，如果此約束條件不滿足，則表明目標(biāo)點(diǎn)超出了機(jī)械臂的可達(dá)工作空間，其逆運(yùn)動(dòng)學(xué)方程無解。代數(shù)法逆向運(yùn)動(dòng)學(xué)逆運(yùn)動(dòng)學(xué)分析代數(shù)法假設(shè)目標(biāo)點(diǎn)在機(jī)械臂的工作空間內(nèi)，則：由式

和式

可得：上式的求解應(yīng)用了雙變量反正切公式，用

計(jì)算根據(jù)

和

的符號(hào)來判別求得的角所在的象限。根據(jù)

帶入式，

得：逆向運(yùn)動(dòng)學(xué)逆運(yùn)動(dòng)學(xué)分析代數(shù)法進(jìn)而可求得：結(jié)合求出的

與，可得：

則三個(gè)關(guān)節(jié)角運(yùn)用代數(shù)法全部解出。逆向運(yùn)動(dòng)學(xué)逆運(yùn)動(dòng)學(xué)分析幾何法平面三連桿機(jī)械臂幾何法求逆運(yùn)動(dòng)學(xué)所示，桿長(zhǎng)

，桿長(zhǎng)、坐標(biāo)系1的原點(diǎn)、坐標(biāo)系的原點(diǎn)的連線組成三角形，由余弦定理可得：由，得計(jì)算圖中的

和逆向運(yùn)動(dòng)學(xué)逆運(yùn)動(dòng)學(xué)分析幾何法即：結(jié)合

和，得即坐標(biāo)3能夠達(dá)到相同位置時(shí)，連桿機(jī)構(gòu)的另一種可能情況，此時(shí)則有：逆向運(yùn)動(dòng)學(xué)逆運(yùn)動(dòng)學(xué)仿真封閉式解法：以KUKAKR5