1、山東大學機械工程學院機電工程研究所2010/09/023.6小節機器人的桿件的速度山東大學機械工程學院機電工程研究所2010/09/023.6 機器人的桿件的速度基本思路：基本思路： 已知基座速度和各關節的相對速度，從基座速度開始，一步一步遞推出末端執行器的速度。山東大學機械工程學院機電工程研究所2010/09/023.4.3、機器人的桿件的速度 機器人桿件的速度包括線速度和角速度，下面介紹如何從i桿件的速度遞推計算i+1桿件的線速度和角速度。 如圖所示，設已知i桿件的速度為i和vi，i+1桿件繞Zi+1軸旋轉的角速度為 。 1i山東大學機械工程學院機電工程研究所2010/09/023.4.3

2、、機器人的桿件的速度 則：在i+1坐標系中表示的i+1桿件桿的角速度為：111111iiiiiiiiiZR 在i+1坐標系中表示的i+1坐標系原點的線速度為：)(1111iiiiiiiiiipvRv在i+1中表示的i+1桿的角速度其中 是在i中表示的指向i+1原點的距離。1iip山東大學機械工程學院機電工程研究所2010/09/023.4.3、機器人的桿件的速度例1、一兩桿關節機器人如圖所示，計算以關節速度為函數的手尖處的速度。山東大學機械工程學院機電工程研究所2010/09/023.4.3、機器人的桿件的速度解：1、建立坐標系，如圖： 2、求位姿矩陣：100001000022022112cs

3、lscM100001000011001101csscM100001000010001223lM山東大學機械工程學院機電工程研究所2010/09/023.4.3、機器人的桿件的速度得：212233000)(220)(0022212111121211112333lclsllclslRv21220000011v11100022001000220221111122clsllcsscv1桿在1中表示的速度山東大學機械工程學院機電工程研究所2010/09/023.4.3、機器人的桿件的速度如果在基座坐標系中表示，僅需乘以R03。100012120121223120103csscRRRR0)(121)(12

4、121211212113303303330clclslslvRv則：山東大學機械工程學院機電工程研究所2010/09/023.4.3、機器人的桿件的速度例2、試求例1中兩桿關節機器人的雅克比矩陣。解：由例1知：212233000)(22212111133lclslvJllclsllclslv32122112121211113333311202)(22v則：及山東大學機械工程學院機電工程研究所2010/09/023.4.3、機器人的桿件的速度 雅克比矩陣的行數等于笛卡爾空間自由度，列數等于機器人的關節數。 同理，我們可以求相對基座坐標系的雅克比矩陣。 0)(121)(12121211212113

5、30330clclslslvRv1212112121)(2212210clclclslslslJ所以：10山東大學機械工程學院機電工程研究所2010/09/023.4.3、機器人的桿件的速度 雅克比矩陣的逆為：121121121221)(2121222110clslclclslclsl lJ 當手尖沿X方向以速度1m/s運動時，由雅克比逆矩陣可得：Tv)0 , 1 (021221,21222211slcslcslc 當2=0時，上式分母為零，兩關節速度將趨于無窮大，它對應機器人的奇異位置。01121121121221)(212122211021clslclclslclsllvvJyx山東大學機

6、械工程學院機電工程研究所2010/09/02第第4 4章章 機器人操作動力學機器人操作動力學4.14.1、概述、概述4.24.2、機器人的牛頓、機器人的牛頓- -歐拉動力學方程歐拉動力學方程4.34.3、機器人拉格朗日動力學方程簡介、機器人拉格朗日動力學方程簡介山東大學機械工程學院機電工程研究所2010/09/024.1、概述、概述為什么要研究機器人的動力學問題？為什么要研究機器人的動力學問題？ 1、為了運動桿件，我們必須加速或減速它們，機器人的運動是作用于關節上的力矩與其他力或力矩作用的結果。 2、力或力矩的作用將影響機器人的動態性能。山東大學機械工程學院機電工程研究所2010/09/024

7、.1、概述、概述機器人動力學研究內容：正問題：已知作用在機器人機構上的力和力矩，求機器人機構各關節的位移、速度、加速度，即：F=ma。反問題：已知機器人機構各關節的位移、速度和加速度，求作用在各關節上的驅動力或驅動力矩，即：am=F 。山東大學機械工程學院機電工程研究所2010/09/024.1、概述、概述機器人動力學研究方法：目標：根據機器人機構的結構特點、運動學和動力學原理，提出通用、快捷的建立動力學方程的方法。 數學工具：矢量方法、張量方法、旋量方法及矩陣方法等。 力學原理：動量矩定理、能量守恒定理、牛頓歐拉方程、達朗貝爾原理、虛功原理、拉格朗日方程、哈密爾頓原理、凱恩方程等。山東大學機

8、械工程學院機電工程研究所2010/09/024.1、概述、概述幾項假設：幾項假設： 1、構成機器人的各桿件都是剛體，即不考慮桿件的變形。 2、忽略各種間隙等因數的影響。 3、暫不考慮驅動系統的動力學。15山東大學機械工程學院機電工程研究所2010/09/024.2 機械人的牛頓歐拉方程 機器人動力學的特點： 1、串聯機器人由多個桿件經關節軸串聯構成，屬于多體動力學的研究范疇。 2、各桿件的速度、加速度是關節位置及時間的函數，隨機器人桿件構形的不同而改變。 3、機器人動力學的計算復雜，多采用數值遞推的方法計算。山東大學機械工程學院機電工程研究所2010/09/024.2 機械人的牛頓歐拉方程我們

9、知道： 剛體運動 =質心的平動 + 繞質心的轉動其中： 質心平動：用牛頓方程描述。 繞質心的轉動：用歐拉方程定義。 它們都涉及到質量及其分布，我們先復習一下轉動慣量的計算。山東大學機械工程學院機電工程研究所2010/09/024.2 機械人的牛頓歐拉方程 如圖所示，設剛體的質量為 ，以質心為原點的隨體坐標系 下的慣量矩陣 由六個量組成，表示為：一、一、 慣量矩陣（張量）慣量矩陣（張量）圖圖3.1mC xyzCI2222()()xiiim yzyz dmI2222()()yiiim zxzx dmI2222()()ziiim xyxy dmIxyyxiiiiim x yx y dmIIyzzyi

10、iiiim y zy z dmIIzxxziiiiim z xz x dmII式中：zzyzxzyzyyxyxzxyxxcIIIIIIIIII山東大學機械工程學院機電工程研究所2010/09/024.2 機械人的牛頓歐拉方程0 xyyzxzIII000000 xCyzIIII 慣量矩陣中的元素 稱為慣量矩(Mass moments of inertia)，而具有混合指標的元素稱為慣量積(Mass products of inertia)。 對于給定的物體，慣量積的值與建立的坐標系的位置及方向有關；如果我們選擇的坐標系合適，可使慣量積的值為零。這樣的坐標系軸稱為主軸（Principle axes

11、）,相應的慣量稱為主慣量。事實上，主慣量是慣量矩陣的三個特征值。zzyyxxIII和、山東大學機械工程學院機電工程研究所2010/09/024.2 機械人的牛頓歐拉方程平行軸定理（Parallel-axis theorem）: 已知相對于某一原點位于物體質心坐標系C的慣量張量，坐標系A平行于坐標系C，則相對于A坐標系的慣量張量為：),(),(),(222222cczzCzzAccyyCyyAccxxCxxAyxmIIzxmIIzymIIccxzCxzAccyzCyzAccxyCxyAzmxIIzmyIIymxII其中： 為質心相對于A坐標系的坐標。),(cccczyxP 20山東大學機械工程學

12、院機電工程研究所2010/09/024.2 機械人的牛頓歐拉方程二、牛頓歐拉方程 我們假設機器人的每個桿件都為剛體，為了運動桿件，我們必須加速或減速它們，運動桿件所需要的力或力矩是所需加速度和桿件質量分布的函數；牛頓方程和用于轉動情況的歐拉方程一起，描述了機器人驅動力矩、負載力（力矩）、慣量和加速度之間的關系。山東大學機械工程學院機電工程研究所2010/09/024.2 機械人的牛頓歐拉方程 我們先研究質心的平動，如圖4.1所示，假設剛體的質量為 ，質心在C點，質心處的位置矢量用 表示，則質心處的加速度為 ；設剛體繞質心轉動的角速度用 表示，繞質心的角加速度為 ，根據牛頓方程可得作用在剛體質心

13、C處的力為：mc mFc 圖圖4.1c山東大學機械工程學院機電工程研究所2010/09/024.2 機械人的牛頓歐拉方程 根據三維空間歐拉方程，作用在剛體上的力矩為： CCM = I + I 圖圖4.1 以上兩式合稱為牛頓歐拉方程。式中，M 為作用力對剛體質心的矩， 為繞質心的角速度和角加速度。和山東大學機械工程學院機電工程研究所2010/09/024.2 機械人的牛頓歐拉方程三、加速度計算1、線加速度 如圖所示，設坐標系i與i-1桿固聯，其原點加速度為ai-1,角速度為i-1;Oi+1隨桿件i相對i坐標系旋轉，相對轉速為 。P為i桿上任意一點。i15山東大學機械工程學院機電工程研究所2010

14、/09/024.2 機械人的牛頓歐拉方程 Pi點的相對速度和加速度為： Pi點的絕對加速度為：)(1 -iiiiiipiPPaa)(21 -1 -1 -1 -1 -iiiieiiieiipiPaPvaaerk)(.iiiiiieiiiePPdtdaPv)()()1 -1 -1 -1 -iiiiiiiiipiPPaa （代入并化簡得：即：上述參數都是在基礎坐標系中表示的。26山東大學機械工程學院機電工程研究所2010/09/024.2 機械人的牛頓歐拉方程 i+1坐標系原點的加速度為：)(1 -iiiiiiciccaa設i桿件質心為ci,則其加速度為：2、角加速度 i桿的角加速度為：)(1 -1

15、iiiiiioiaaaaiiii 1- i1-山東大學機械工程學院機電工程研究所2010/09/024.2 機械人的牛頓歐拉方程四、作用力和力矩 計算出每個桿件質心的加速度后，我們可以應用牛頓-歐拉方程來計算作用在每個桿件質心的慣性力和慣性力矩。 根據牛頓-歐拉方程，有：iciiiciiciiiIINvmF28山東大學機械工程學院機電工程研究所2010/09/024.2 機械人的牛頓歐拉方程圖 2 構件受力圖 如圖2所示，將第i個構件Li作為隔離體進行分析，作用在其上的力和力矩有：1,.1iiiimm 作用在i桿件上的外力和外力矩，i-1桿件作用在i桿件上的力和力矩，以及i+1桿件作用在i桿件

16、上的力和力矩。29山東大學機械工程學院機電工程研究所2010/09/023.3.1 機械臂的牛頓歐拉方程其中： Fi+1,i構件Li+1作用在構件Li上的力。Mi+1,i構件Li+1作用在構件Li上的力矩。Fi-1,i構件Li-1作用在構件Li上的力。Mi-1,i構件Li-1作用在構件Li上的力矩。Fi 作用在第i個構件Li上的外力簡化到 質心C處的合力，即外力的主矢。Mi 作用在第i個構件Li上的外力矩簡化到質心C處的合力矩，即外力的主矩。山東大學機械工程學院機電工程研究所2010/09/023.3.1 機械臂的牛頓歐拉方程 上述力和力矩包括了運動副中的約束反力、驅動力、摩擦力等引起的作用力

17、和作用力矩。 作用在第i個構件上的所有力化簡到質心的總的合力為：iiiiiifffF1, 1它們都在基礎坐標系中表示。山東大學機械工程學院機電工程研究所2010/09/023.3.1 機械臂的牛頓歐拉方程相對于質心的總的合力矩Mi為： 最后，為了便于遞推計算，重新安排力和力矩計算公式為： iiiiiiiiiiiimhfmlfmM1,1, 1, 1iiiiiiFfff1, 1iiiiiiiiiiiiiiMmlflFmhlfm1,1, 1）（iiiiiiiiiiiiiiiiiiiiiiiiiiimlFlfmhlfmmhfmlFffmM1,1, 11,1,1, 1）（）（山東大學機械工程學院機電工程

18、研究所2010/09/023.3.1 機械臂的牛頓歐拉方程 i桿件需要的關節力矩為相鄰桿件作用于它的力矩的Z分量，即：iiiizm, 1牛頓-歐拉方程的遞推算法： 由兩部分組成：首先，從1號桿到n號桿，向前遞推計算各桿的速度和加速度。然后，再從n號桿到1號桿，向后遞推計算作用力和力矩，以及關節驅動力矩。 算法過程總結如下：山東大學機械工程學院機電工程研究所2010/09/023.3.1 機械臂的牛頓歐拉方程向前遞推：i: 06iiii 1 - i1 -iii1 -)(1 -1 -1 -1 -iiiiiiciCCvviciiiciiiiiIIvmFMc向后遞推：i: 61iiiiiiFfff1,

19、1iiiiiiiiiiiiiiMlFmlfmhlfm1,1, 1）（iiiizm, 1慣性力慣性力矩條件：基礎桿件和各關節的角速度和角加速度已知山東大學機械工程學院機電工程研究所2010/09/023.3.1 機械臂的牛頓歐拉方程 引力對桿件作用的影響可以通過設置 來實現，這里，G為引力常數。 上面給出了關節型機器人的動力學計算方法，對于移動關節可以推導相應的方程。 對一些相對簡單的問題，用上述方法，也可能得到閉式解析結果。 上述遞推算法是一種通用算法，可以用于任意自由度數的關節型機器人。Gv 1山東大學機械工程學院機電工程研究所2010/09/02牛頓歐拉方程實例圖 3 平面兩自由度機器人機

20、構例1 如圖3所示的平面兩自由度機器人機構。連桿L1質心為C1，質量為m1，驅動力矩為m m1=0 0 m11T，角速度為1=0 0 1T,加速度為1=0 0 1T; 連桿L2質心為C2，質量為m2，驅動力矩為m m2=0 0 m22T，角速度為2=0 0 2T,加速度為2=0 0 2T，山東大學機械工程學院機電工程研究所2010/09/02牛頓歐拉方程實例 選取關節O和關節A處的轉角1和2為系統的廣義坐標，可以寫出連桿L1的牛頓歐拉方程為:0,11,211 1mfffc 0,10,111,21,2111CmflmfhI連桿L2的牛頓歐拉方程為:1,2222mffc 1,21,2222Cmfl

21、I式中:重力驅動力矩Tgm0011fTgm0022fTm1110100 mmTm2220200 mm山東大學機械工程學院機電工程研究所2010/09/02牛頓歐拉方程實例由以上幾式消去桿件間作用力，可解得:2222222()Cmm gmIcl1111 11222122212()()Cmm gmm gmm gmIcclchm考慮質心位置：11111sincos0llc11212211212sinsin()coscos()0LlLlc求導得：1 1111 11cossin0ll c2111112111111(sincos)(cossin)0ll c山東大學機械工程學院機電工程研究所2010/09/

22、02牛頓歐拉方程實例11121212211121212cos()cos()sin()sin()0LlLlc22111212121 1121212222111212121 1121212sin() sin()cos()cos()cos() cos()sin()sin()0LlLlLlLl c另外：0cos)(sin)(1111111lLlLh0cossin11111lll0)cos()sin(2122122lll山東大學機械工程學院機電工程研究所2010/09/02牛頓歐拉方程實例有：0)cos()sin(0000000000021221222221222111llgccmIIImyxzyx m

23、即： )g)sin()cos()(2122122221211yxzcclmIm 對m22可同樣寫出矩陣方程。代入加速度分量，得： )gsin()sin(sin)cos()(cos)cos()cos(cos)sin()(-sin-)(21212121112122121211212121211121221212112221211 lLlLlLlLlmImz山東大學機械工程學院機電工程研究所2010/09/02牛頓歐拉方程實例化簡可得： 222111221 221 1212 212222 221 22221 22221 212222121211(2cos)(cos)sin2sinsin()()sin

24、zzzmm Llm lm Lm lm lm Llm Llm Llm glmmgl III 222222 221 22122 22221 2122212(cos)()sinsin()zzmm lm L lm lm L lm gl II 上式即為各桿件關節的驅動力計算公式，它是一個以角加速度為變量、變系數的非線性動力學方程。山東大學機械工程學院機電工程研究所2010/09/02牛頓歐拉方程實例以上兩式進一步寫成：式中： 21111 1122122211212122221 122221122mDDDDDmDDDD 222111221 221 1212 22coszzDm L lm lm Lm lII

25、21222 221 22coszDm lm LlI12221 22sinDm Ll 11221 222sinDm Ll 122121211sin()()sinDm glmmgl 22122 221 22coszDm lm LlI22222 2zDm lI21121 22sinDm Ll22212sin()Dm gl 系數是位置的函數山東大學機械工程學院機電工程研究所2010/09/02牛頓歐拉方程實例 例2：如圖所示為兩桿平面機器人，為了簡單起見，我們假設每個桿件的質量集中于桿件的尾部，其大小為m1和m2。222111,XlPXlPcc解：每個桿件的質量中心矢量為： 由于點質量假設，每個桿件相

26、對質心的慣性張量為零，即：0, 021ccII山東大學機械工程學院機電工程研究所2010/09/02牛頓歐拉方程實例末端執行器上無作用力，所以：0, 033nf0, 000,-1Ygvo基座靜止，因此：考慮到引力，我們使用：山東大學機械工程學院機電工程研究所2010/09/02牛頓歐拉方程實例應用遞推公式有：向前：1桿件：山東大學機械工程學院機電工程研究所2010/09/02牛頓歐拉方程實例2桿件：山東大學機械工程學院機電工程研究所2010/09/02牛頓歐拉方程實例山東大學機械工程學院機電工程研究所2010/09/02牛頓歐拉方程實例向后遞推：2桿件：1桿件：山東大學機械工程學院機電工程研究

27、所2010/09/02牛頓歐拉方程實例取力矩的Z分量，得到關節力矩：山東大學機械工程學院機電工程研究所2010/09/02稱 為慣量陣， 是離心力、科氏力等相關部分， 為重力部分。特點：多變量、時變、非線性、強耦合。機器人機構動力學方程 通常，機器人的動力學方程常寫為抽象的形式：QGVM)(),()( 其中： 為廣義坐標向量， 為廣義力向量。 )(M)(G),(VQ山東大學機械工程學院機電工程研究所2010/09/024.3 機器人拉格朗日動力學方程簡介機器人拉格朗日動力學方程簡介 拉格朗日方程是基于能量項對系統變量及時間的微分而建立的。對于簡單系統拉格朗日方程法相較于牛頓歐拉方程法更顯復雜，然而隨著系統復雜程度的增加，拉格朗日方程法建立系統運動微分方程變得相對簡單。 山東大學機械工程學院機電工程研究所2010/09/02拉格朗日函數為系統的動能 和位能 之差 即：4.3 機器人拉格朗日動力學方程簡介系統拉格朗日方程為： iiidLLQdtqq1, 2,.in式中： niQLkEpEkpLEE 系統的廣義坐標數作用在第i