第4章 機器人動力學

1、第四章第四章 機器人的動力學初步機器人的動力學初步 第一節(jié)第一節(jié) 前前 言言 機器人動力學是研究機器人運動數(shù)學機器人動力學是研究機器人運動數(shù)學方程的建立。其實際動力學模型可以根據(jù)方程的建立。其實際動力學模型可以根據(jù)已知的物理定律已知的物理定律( (例如牛頓或拉格朗日力學例如牛頓或拉格朗日力學定律定律) )求得。求得。 機器人運動方程的求解可分為兩種不同性質的問題：機器人運動方程的求解可分為兩種不同性質的問題： 正動力學問題。即機器人各執(zhí)行器的驅正動力學問題。即機器人各執(zhí)行器的驅動力或力矩為已知，求解機器人關節(jié)變量在動力或力矩為已知，求解機器人關節(jié)變量在關節(jié)變量空間的軌跡或末端執(zhí)行器在笛卡爾關節(jié)

2、變量空間的軌跡或末端執(zhí)行器在笛卡爾空間的軌跡，這稱為機器人動力學方程的正空間的軌跡，這稱為機器人動力學方程的正面求解，簡稱為正動力學問題。面求解，簡稱為正動力學問題。 機器人運動方程的求解可分為兩種不同性質的問題：機器人運動方程的求解可分為兩種不同性質的問題： 逆動力學問題。即機器人在關節(jié)變量空逆動力學問題。即機器人在關節(jié)變量空間的軌跡已確定，或末端執(zhí)行器在笛卡爾空間的軌跡已確定，或末端執(zhí)行器在笛卡爾空間的軌跡已確定（軌跡已被規(guī)劃），求解機間的軌跡已確定（軌跡已被規(guī)劃），求解機器人各執(zhí)行器的驅動力或力矩，這稱為機器器人各執(zhí)行器的驅動力或力矩，這稱為機器人動力學方程的反面求解，簡稱為逆動力學人動

3、力學方程的反面求解，簡稱為逆動力學問題。問題。 第二節(jié)第二節(jié) 機器人的靜力學機器人的靜力學 一、虛功原理一、虛功原理 在介紹機器人靜力學之前，首先要說明一下在介紹機器人靜力學之前，首先要說明一下靜力學中所需要的虛功原理（靜力學中所需要的虛功原理（principle of principle of virtual workvirtual work）。）。 約束力不作功的力學系統(tǒng)實現(xiàn)平衡的必要且約束力不作功的力學系統(tǒng)實現(xiàn)平衡的必要且充分條件是對結構上允許的任意位移（虛位移）充分條件是對結構上允許的任意位移（虛位移）施力所作功之和為零。這里所指的虛位移施力所作功之和為零。這里所指的虛位移（virtu

4、al displacementvirtual displacement）是描述作為對象的系）是描述作為對象的系統(tǒng)力學結構的位移，不同于隨時間一起產(chǎn)生的實統(tǒng)力學結構的位移，不同于隨時間一起產(chǎn)生的實際位移。為此用際位移。為此用“虛虛”一詞來表示。而約束力一詞來表示。而約束力（force of constraintforce of constraint）是使系統(tǒng)動作受到制約）是使系統(tǒng)動作受到制約的力。的力。 下面看一個例子來理解一下實際上如何使下面看一個例子來理解一下實際上如何使用虛功原理。如圖用虛功原理。如圖4 41 1所示，已知作用在杠桿所示，已知作用在杠桿一端的力一端的力F FA A，試用虛功

5、原理求作用于另一端的，試用虛功原理求作用于另一端的力力F FB B。假設杠桿長度。假設杠桿長度L LA A，L LB B已知。已知。 圖圖4 41 1 杠桿及作用在它兩端上的力杠桿及作用在它兩端上的力 按照虛功原理，杠桿兩端受力所作的虛功按照虛功原理，杠桿兩端受力所作的虛功應該是應該是 （4 41 1） 式中式中 ， ，是杠桿兩端的虛位，是杠桿兩端的虛位移。而就虛位移來講，下式成立移。而就虛位移來講，下式成立 （4 42 2） 0BBAAxFxFAALxBBLxAxBx 式中，式中， 是繞杠桿支點的虛位移。把式是繞杠桿支點的虛位移。把式（42）代入式（）代入式（41）消去）消去 、 ，可，可得

6、到下式得到下式 （43） 由于公式（由于公式（43）對任意的都成立，所以）對任意的都成立，所以有下式成立有下式成立 0)(BBAALFLFAxBx0BBAALFLF 因此得到因此得到 （4 44 4） 當力當力F FA A向下取正值時，向下取正值時，F(xiàn) FB B則為負值，由于則為負值，由于F FB B 的正方向定義為向上，所以這時表明的正方向定義為向上，所以這時表明F FB B的方向是的方向是向下的，即此時向下的，即此時F FA A和和F FB B的方向都朝下。的方向都朝下。 ABABFLLF二、機器人靜力學關系式的推導二、機器人靜力學關系式的推導 利用前面的虛功原理來推導機器人的靜力利用前面

7、的虛功原理來推導機器人的靜力學關系式。學關系式。 如圖如圖4 42 2所示的機械手，要產(chǎn)生圖（所示的機械手，要產(chǎn)生圖（a a）所示的虛位移，推導出圖（所示的虛位移，推導出圖（b b）所示各力之間）所示各力之間的關系式。這一推導方法本身也適用于一般的的關系式。這一推導方法本身也適用于一般的情況。情況。圖圖4 42 2 機械手的虛位移和施加的力機械手的虛位移和施加的力 假設假設 ：手爪的虛位移為手爪的虛位移為 關節(jié)的虛位移為關節(jié)的虛位移為 手爪力為手爪力為 關節(jié)驅動力為關節(jié)驅動力為 如果施加在機械手上的力作為手爪力如果施加在機械手上的力作為手爪力的反力（用的反力（用-F-F來表示）時，機械手的虛功

8、來表示）時，機械手的虛功可表示為：可表示為： （4 45 5） 11,mTmRrrr11,nTnR11,mTmRffF11,nTnRrFWTT)( 為此，如果應用虛功原理，則得到為此，如果應用虛功原理，則得到 （4 46 6） 這里，手爪的虛位移這里，手爪的虛位移 和關節(jié)的虛位移和關節(jié)的虛位移 之間的關系，用雅克比矩陣表示為之間的關系，用雅克比矩陣表示為 （4 47 7） 把式（把式（4 47 7）代入式（）代入式（4 46 6），提出公因），提出公因數(shù)數(shù) ，可得到下式，可得到下式 （4 48 8） 0)(rFTTrJr 0)(JFTT 由于這一公式對任意的由于這一公式對任意的 都成立，因此得

9、到都成立，因此得到下式成立下式成立 （49） 進一步整理，把式中第二項移到等式右邊，并進一步整理，把式中第二項移到等式右邊，并取兩邊的轉置，則可得到下面的機械手靜力學關取兩邊的轉置，則可得到下面的機械手靜力學關系式系式 （410） 上式表示了機械手在靜止狀態(tài)為產(chǎn)生手爪力上式表示了機械手在靜止狀態(tài)為產(chǎn)生手爪力 的驅動力的驅動力 。 0JFTTFJTF 為了加深理解，下面分別求解圖為了加深理解，下面分別求解圖43所示的所示的2自自由度機械手在圖示位置時，生成手爪力由度機械手在圖示位置時，生成手爪力 或或 的驅動力的驅動力 或或 。圖示。圖示 為為 ， 時的姿態(tài)。時的姿態(tài)。圖圖43 求生成手爪力或的

10、驅動力求生成手爪力或的驅動力 TxAfF0TyBfF0AB)(01rad)(2/2rad由關節(jié)角給出如下姿態(tài)由關節(jié)角給出如下姿態(tài) 則由式（則由式（410）可以得到驅動力如下）可以得到驅動力如下 從求解的結果看到，在這里驅動力的大小為從求解的結果看到，在這里驅動力的大小為手爪力的大小和手爪力到作用線距離的乘積。手爪力的大小和手爪力到作用線距離的乘積。 0)cos()cos(cos)sin()sin(sin1222122121121221211LLLLLLLLLJxxxATAfLfLfLLLFJ22212000001212yyBTBfLfLLLFJ三、慣性矩的確定三、慣性矩的確定 動力學不僅與驅動

11、力有關，還與繞質心的慣動力學不僅與驅動力有關，還與繞質心的慣性矩有關。下面以一質點的運動為例，了解慣性性矩有關。下面以一質點的運動為例，了解慣性矩的物理意義。矩的物理意義。 如圖如圖44所示，若將力所示，若將力 作用到質量為作用到質量為 的質點時的平移運動，看作是運動方向的標量，的質點時的平移運動，看作是運動方向的標量，則可以表示為：則可以表示為： （411） 式中，式中， 表示加速度。表示加速度。FmFxm x 若把這一運動看作是質量可以忽略的棒長為若把這一運動看作是質量可以忽略的棒長為 的回轉運動，則得到加速度和力的關系式為的回轉運動，則得到加速度和力的關系式為 （412） （413）圖圖

12、44 質點平移運動作為回轉運動的解析質點平移運動作為回轉運動的解析 式中式中 ， 和是繞軸回轉的角加速度和慣性和是繞軸回轉的角加速度和慣性矩。矩。 r rx rNF N 將式（將式（412）、（）、（413）代入式（）代入式（411），得到），得到 （414）如如 ，則式（，則式（414）就改寫為）就改寫為 （415） 上式是質點繞固定軸進行回轉運動時的運動方上式是質點繞固定軸進行回轉運動時的運動方程式。與式（程式。與式（411）比較）比較 相當于平移運動時相當于平移運動時的質量，在旋轉運動中稱為慣性矩。的質量，在旋轉運動中稱為慣性矩。 Nmr 22mrI NI I 對于質量連續(xù)分布的物體，求

13、解其慣性矩，對于質量連續(xù)分布的物體，求解其慣性矩，可以將其分割成假想的微小物體，然后再把每個可以將其分割成假想的微小物體，然后再把每個微小物體的慣性矩加在一起。這時，微小物體的微小物體的慣性矩加在一起。這時，微小物體的質量質量 及其微小體積及其微小體積 的關系，可用密度的關系，可用密度 表示為表示為 （416） 所以，微小物體的慣性矩所以，微小物體的慣性矩 ，依據(jù)式，依據(jù)式 ，可以寫成可以寫成 （417） 因此，整個物體的慣性矩通過積分求得如下：因此，整個物體的慣性矩通過積分求得如下： （418）dmdVdVdmdI2mrI dVrdmrdI22dVrdII2四、運動學、靜力學、動力學的關系四

14、、運動學、靜力學、動力學的關系 如圖如圖45所示，在機器人的手爪接觸環(huán)境所示，在機器人的手爪接觸環(huán)境時，手爪力時，手爪力 的驅動力的驅動力 的關系起重要作用，的關系起重要作用，在靜止狀態(tài)下處理這種關系稱為靜力學在靜止狀態(tài)下處理這種關系稱為靜力學（statics）。）。圖圖45 手爪力的關節(jié)驅動力手爪力的關節(jié)驅動力 F 在考慮控制時，就要考慮在機器人的動作中，在考慮控制時，就要考慮在機器人的動作中，關節(jié)驅動力關節(jié)驅動力 會產(chǎn)生怎樣的關節(jié)位置會產(chǎn)生怎樣的關節(jié)位置 、關節(jié)、關節(jié)速度速度 、關節(jié)加速度、關節(jié)加速度 ，處理這種關系稱為動，處理這種關系稱為動力學（力學（dynamics）。）。 對于動力學

15、來說，除了與連桿長度對于動力學來說，除了與連桿長度 有關之外，有關之外，還與各連桿的質量還與各連桿的質量 ，繞質量中心的慣性矩，繞質量中心的慣性矩 ，連桿的質量中心與關節(jié)軸的距離連桿的質量中心與關節(jié)軸的距離 有關。如圖有關。如圖46所示。所示。圖圖46 與動力學有關的各量與動力學有關的各量 iLimCiICiL 運動學、靜力學和動力學中各變量的關系如運動學、靜力學和動力學中各變量的關系如圖圖47所示。圖中用虛線表示的關系可通過實線所示。圖中用虛線表示的關系可通過實線關系的組合表示，這些也可作為動力學的問題來關系的組合表示，這些也可作為動力學的問題來處理。處理。圖圖47 運動學、靜力學、動力學的

16、關系運動學、靜力學、動力學的關系 第三節(jié)第三節(jié) 機器人動力學方程式機器人動力學方程式 一、機器人的動能與位能一、機器人的動能與位能 1 1動能動能 為了導出多關節(jié)機器人的運動方程式，首先為了導出多關節(jié)機器人的運動方程式，首先要了解機器人的動能和位能。先看圖要了解機器人的動能和位能。先看圖4 48 8所表示所表示的第的第i i個連桿的運動能量。個連桿的運動能量。圖圖4 48 8 第第i i個連桿的旋轉速度和重心的平移速度個連桿的旋轉速度和重心的平移速度 剛體的運動能量，是由該剛體的平移構剛體的運動能量，是由該剛體的平移構成的運動能量，與該剛體的旋轉而構成的成的運動能量，與該剛體的旋轉而構成的運動

17、能量之和表示的。因此，圖運動能量之和表示的。因此，圖4 48 8中表中表示的連桿的運動能量，可以用下式表示：示的連桿的運動能量，可以用下式表示： （4 41919） 式中，式中，K Ki i為連桿的運動能量，為連桿的運動能量，m mi i為質量，為質量，v vcici為在基準坐標系上表示的重心的平移速為在基準坐標系上表示的重心的平移速度向量，度向量，I Ii i為在基準坐標系上表示的連桿的為在基準坐標系上表示的連桿的轉動慣量，轉動慣量， 為在基準坐標系上表示的轉為在基準坐標系上表示的轉動速度向量。動速度向量。iiTiciTciiiIvvmK2121i 因為機器人的全部運動能量因為機器人的全部運

18、動能量 K ，由各連桿的，由各連桿的運動能量的總和表示，所以得到運動能量的總和表示，所以得到 （420） 式中，式中， 為機器人的關節(jié)總數(shù)。其次我們來考為機器人的關節(jié)總數(shù)。其次我們來考慮把作為機器人各關節(jié)速度的函數(shù)。這里慮把作為機器人各關節(jié)速度的函數(shù)。這里 與與 分別表示如下：分別表示如下： （421） （422）niiKK1nciviqJviLci)(qJiAi)( 式中，式中， 是與是與 i第個連桿重心位置的平移速度第個連桿重心位置的平移速度相關的雅可比矩陣，相關的雅可比矩陣， 是與是與i第個連桿轉動速度相第個連桿轉動速度相關的雅可比矩陣。為了區(qū)別于與指尖速度相關的關的雅可比矩陣。為了區(qū)別

19、于與指尖速度相關的雅可比矩陣，在上面標明了注角（雅可比矩陣，在上面標明了注角（i）。）。 （4-23） （4-24） 在式（在式（423）和式（）和式（424）中，包含著）中，包含著0分量，分量，這是因為第這是因為第i個連桿的運動與其以后的關節(jié)運動是個連桿的運動與其以后的關節(jié)運動是無關的。無關的。 )(iLJ)(iAJ00)()(1)(iLiiLiLJJJ00)()(1)(iAiiAiAJJJ 現(xiàn)在將式（現(xiàn)在將式（421）和式（）和式（422）代進式）代進式（419）和式（）和式（420），機器人的運動能量），機器人的運動能量公式可以寫成公式可以寫成 （425）令令 （426）則機器人的運動能

20、量公式（則機器人的運動能量公式（425）寫為）寫為 （427）這里這里H表示的稱為機器人的慣性矩陣。表示的稱為機器人的慣性矩陣。 )(21)()()()(1qJIJqqJJqmKiAiTiATiLTiLTini)()()()()(1iAiTiAiLTiLiniJIJJJmHqHqKT212勢能勢能 機器人的勢置能量和運動能量一樣，也是由機器人的勢置能量和運動能量一樣，也是由各連桿的位置能量的總和給出，因此可用下式表各連桿的位置能量的總和給出，因此可用下式表示：示： （428） 式中，式中， 表示重力加速度，它是一個在基準坐表示重力加速度，它是一個在基準坐標系上表示的三維向量。標系上表示的三維向

21、量。 表示從基準坐標系原表示從基準坐標系原點，到點，到 個連桿的重心位置的位置向量。個連桿的重心位置的位置向量。 iCTniirgmP, 01giCr,0i二、機器人動力學方程的建立舉例二、機器人動力學方程的建立舉例 1牛頓牛頓歐拉方程式歐拉方程式 首先，以單一剛體為例，如圖首先，以單一剛體為例，如圖49所示，其所示，其運動方程式可用下式表示運動方程式可用下式表示 （429） （430） 圖圖49 單一剛體單一剛體 ccFvmNIIcc)( 式（式（429）和式（）和式（430）分別被稱為牛）分別被稱為牛頓運動方程式及歐拉運動方程式。式中，頓運動方程式及歐拉運動方程式。式中， 是剛是剛體的質量

22、；體的質量； 是繞重心是繞重心 的慣性矩陣，的慣性矩陣， 的的各元素表示對應的力矩元素和角加速度元素間的各元素表示對應的力矩元素和角加速度元素間的慣性矩；慣性矩； 是作用于重心的平動力；是作用于重心的平動力； 是慣性矩；是慣性矩； 是重心的平移速度；是重心的平移速度； 是角速度。是角速度。 m33RIcCcIcFNcv 下面求解一下圖下面求解一下圖410所示的所示的1自由度機械手自由度機械手的運動方程式，在這里，由于關節(jié)軸制約連桿的的運動方程式，在這里，由于關節(jié)軸制約連桿的運動，所以可以將式（運動，所以可以將式（430）的運動方程式看）的運動方程式看作是繞固定軸的運動。作是繞固定軸的運動。圖圖

23、410 1自由度機械手自由度機械手 假設繞關節(jié)軸的慣性矩為假設繞關節(jié)軸的慣性矩為 ，取垂直紙面的方，取垂直紙面的方向為軸向為軸 ，則得到，則得到 （431） （432）Iz II000000000IIcos00cmgLN 式中，式中，g是重力常數(shù)；是重力常數(shù)； 是在第是在第3行第行第3列上列上具有繞關節(jié)軸慣性矩的慣性矩陣。把這些公式代具有繞關節(jié)軸慣性矩的慣性矩陣。把這些公式代入式（入式（430），提取只有），提取只有z分量的回轉，則得到分量的回轉，則得到 （433） 該式為該式為1自由度機械手的歐拉運動方程式，其中：自由度機械手的歐拉運動方程式，其中： （434） 對于一般形狀的連桿，在式（對

24、于一般形狀的連桿，在式（431）中，由于）中，由于 除第除第3分量以外其他分量皆不為分量以外其他分量皆不為0，所以，所以 的第的第1、2分量成了改變軸方向的力矩，但在固定分量成了改變軸方向的力矩，但在固定軸的場合，與這個力矩平衡的約束力生成式（軸的場合，與這個力矩平衡的約束力生成式（432）的第）的第1、2分量，不產(chǎn)生運動。分量，不產(chǎn)生運動。 33 RIccoscmgLI ccmLIIII2拉格朗日方程式拉格朗日方程式 拉格朗日運動方程式一般表示為拉格朗日運動方程式一般表示為 （435）式中，式中， 是廣義坐標，是廣義坐標， 是廣義力。是廣義力。 拉格朗日運動方程式也可以表示為拉格朗日運動方程

25、式也可以表示為 （436）這里，這里， 是拉格朗日算子，是拉格朗日算子， 是動能，是動能， 是勢能。是勢能。 qLqLdtdqPKLLKP 現(xiàn)在再以前面推導的現(xiàn)在再以前面推導的1自由度機械手為例，利自由度機械手為例，利用拉格朗日運動方程式來具體求解，假設用拉格朗日運動方程式來具體求解，假設 為廣為廣義坐標，則得到義坐標，則得到 由于由于 所以用所以用 置換式（置換式（435）中的廣義坐標）中的廣義坐標 后，可得到下式后，可得到下式 （437）該式與前面推導的結果完全一致。該式與前面推導的結果完全一致。 221IK sincmgLP sin212cmgLILILcoscmgLLqcoscmgLI

26、 下面推導下面推導2自由度機械手的運動方程式，如圖自由度機械手的運動方程式，如圖411所示。在推導時，把所示。在推導時，把 ， 當作廣義坐當作廣義坐標，標， ， 當作廣義力，求拉格朗日算子，代入式當作廣義力，求拉格朗日算子，代入式（435）的拉格朗日運動方程式即可。）的拉格朗日運動方程式即可。 （4-38） （439）圖圖411 2自由度機械手自由度機械手 121221111112121CCTCIppmK1111sinCgLmP （440） （441） 式中，式中， 是第是第i個連桿質量中心的個連桿質量中心的位置向量。位置向量。 （442） （443） （444） （445） 根據(jù)理論力學的知識，各連桿的動能可用質根據(jù)理論力學的知識，各連桿的動能可用質量中心平移運動的動能和繞質量中心回轉運動的量中心平移運動的動能和繞質量中心回轉運動的動能之和來表示。動能之和來表示。 22122222)(2121CCTCIppmK)sin(sin2121122CLLgmPTCiyCixCippp111cosLpxC111sinLpyC)cos(cos212112cxCLLp)sin(sin212112cyCLLp 由式（由式（442）（）（445），得到式（），得到式（438），（），（440）中的質量中心速度和為）中的質量中心速度和為 （446）