機(jī)構(gòu)學(xué)與機(jī)器人學(xué)課件

第六章機(jī)器人的動(dòng)力學(xué)第六章機(jī)器人的動(dòng)力學(xué)1分析機(jī)器人操作的動(dòng)態(tài)數(shù)學(xué)模型，主要采用下列兩種理論：（1）動(dòng)力學(xué)基本理論，包括牛頓-歐拉方程。（2）拉格朗日力學(xué)，特別是二階拉格朗日方程。拉格朗日函數(shù)L被定義為系統(tǒng)的動(dòng)能K和位能P之差，即:（4.1）其中，K和P可以用任何方便的坐標(biāo)來(lái)表示。分析機(jī)器人操作的動(dòng)態(tài)數(shù)學(xué)模型，主要采用下列兩種理論：（42系統(tǒng)動(dòng)力學(xué)方程式，即拉格朗日方程如下：（4.2）式中，為表示動(dòng)能和位能的坐標(biāo)，速度，而為相應(yīng)的Fi是力或是力矩，由qi為直線(xiàn)坐標(biāo)或角坐標(biāo)所決定。這些力、力矩和坐標(biāo)稱(chēng)為廣義力、廣義力矩和廣義坐標(biāo)，n為連桿數(shù)目。Fi為作用在第i個(gè)坐標(biāo)上的力或是力矩。系統(tǒng)動(dòng)力學(xué)方程式，即拉格朗日方程如下：（4.2）式中，為表34.1.1剛體的動(dòng)能與位能在理論力學(xué)或物理力學(xué)部分，曾對(duì)如圖4.1所示的一般物體平動(dòng)時(shí)所具有的動(dòng)能和位能進(jìn)行過(guò)計(jì)算，其求法是大家所熟悉的，如下：4.1.1剛體的動(dòng)能與位能4式中，表示物體所具有的動(dòng)能K、位能P，所消耗的能量D和外力所做的功W；M0和M1為支架和運(yùn)動(dòng)物體的質(zhì)量；x0和x1為運(yùn)動(dòng)坐標(biāo)；g為重力加速度；k為彈簧虎克系數(shù)；c為摩擦系數(shù)；F為外施作用力。對(duì)于這一問(wèn)題，存在兩種情況。1、為廣義坐標(biāo)式中，表示物體所具有的動(dòng)能K、位能P，所消耗的能量D和外力所5其中，左式第一項(xiàng)為動(dòng)能隨速度（或角速度）和時(shí)間的變化；第二項(xiàng)為動(dòng)能隨位置（或角度）的變化；第三項(xiàng)為能耗隨速度的變化；第四項(xiàng)為位能隨位置的變化。右式為實(shí)際外加力或力矩。代入相應(yīng)各項(xiàng)的表達(dá)式，并化簡(jiǎn)可得:其中，左式第一項(xiàng)為動(dòng)能隨速度（或角速度）和時(shí)間的變化；第二項(xiàng)6表示為一般形式為:即為所求x0=0時(shí)的動(dòng)力學(xué)方程式。其中，左式三項(xiàng)分別表示物體的加速度、阻力和彈力，而右式兩項(xiàng)分別表示外加作用力和重力。表示為一般形式為:即為所求x0=0時(shí)的動(dòng)力學(xué)方程式。其中，左72、均為廣義坐標(biāo)這時(shí)有下式:或用矩陣形式表示為:2、均為廣義坐標(biāo)這時(shí)有下式:或用矩陣形式表示為:8下面來(lái)考慮二連桿機(jī)械手（見(jiàn)圖4.2）的動(dòng)能和位能。這種運(yùn)動(dòng)機(jī)構(gòu)具有開(kāi)式運(yùn)動(dòng)鏈，與復(fù)擺運(yùn)動(dòng)有許多相似之處。圖中，T1和T2為轉(zhuǎn)矩，m1和m2為連桿1和連桿2的質(zhì)量，且以連桿末端的點(diǎn)質(zhì)量表示；d1和d2分別為兩連桿的長(zhǎng)度，θ1和θ2為廣義坐標(biāo)；g為重力加速度。下面來(lái)考慮二連桿機(jī)械手（見(jiàn)圖4.2）的動(dòng)能和位能。這種9先計(jì)算連桿1的動(dòng)能K1和位能P1。因?yàn)?，所以有:再求連桿2的動(dòng)能K2和位能P2:先計(jì)算連桿1的動(dòng)能K1和位能P1。因?yàn)?，所以有:再求連桿210式中:于是可求得:以及:式中:于是可求得:以及:11這樣，二連桿機(jī)械手系統(tǒng)的總動(dòng)能和總位能分別為:（4.3）（4.4）這樣，二連桿機(jī)械手系統(tǒng)的總動(dòng)能和總位能分別為:（4.3）（12機(jī)構(gòu)學(xué)與機(jī)器人學(xué)課件13將相應(yīng)各導(dǎo)數(shù)和偏導(dǎo)數(shù)代入(4.2)，即可求得力矩T1和T2的動(dòng)力學(xué)方程式：將相應(yīng)各導(dǎo)數(shù)和偏導(dǎo)數(shù)代入(4.2)，即可求得力矩T1和T2的14機(jī)構(gòu)學(xué)與機(jī)器人學(xué)課件15重力項(xiàng)：重力項(xiàng)：16在分析簡(jiǎn)單的二連桿機(jī)械手系統(tǒng)的基礎(chǔ)上，我們分析由一組A變換描述的任何機(jī)械手，求出動(dòng)力學(xué)方程。推導(dǎo)過(guò)程分五步進(jìn)行。（1）計(jì)算任一連桿上任一點(diǎn)的速度；（2）計(jì)算各連桿的動(dòng)能和機(jī)械手的總動(dòng)能；（3）計(jì)算各連桿的位能和機(jī)械手的總位能；（4）建立機(jī)械手系統(tǒng)的拉格朗日函數(shù)；（5）對(duì)拉格朗日函數(shù)求導(dǎo)，以得到動(dòng)力學(xué)方程式。在分析簡(jiǎn)單的二連桿機(jī)械手系統(tǒng)的基礎(chǔ)上，17圖4.4表示一個(gè)四連桿機(jī)械手的結(jié)構(gòu)。我們先從這個(gè)例子出發(fā)，求得此機(jī)械手某個(gè)連桿（例如連桿3）上某一點(diǎn)（如點(diǎn)P）的速度、質(zhì)點(diǎn)和機(jī)械手的動(dòng)能與位能、拉格朗日算子，再求系統(tǒng)的動(dòng)力學(xué)方程式。然后，由特殊到一般，導(dǎo)出任何機(jī)械手的速度、動(dòng)能、位能和動(dòng)力學(xué)方程的一般表達(dá)式。圖4.4表示一個(gè)四連桿機(jī)械手的結(jié)構(gòu)。我們先從這個(gè)例184.2.1速度的計(jì)算圖4.4中連桿3上點(diǎn)P的位置為：式中，為總（基）坐標(biāo)系中的位置矢量；為局部（相對(duì)關(guān)節(jié)O3）坐標(biāo)系中的位置矢量；T3為變換矩陣，包括旋轉(zhuǎn)變換和平移變換。對(duì)于任一連桿i上的一點(diǎn)，其位置為:（4.14）4.2.1速度的計(jì)算式中，為總（基）坐標(biāo)系中的位置矢19點(diǎn)P的速度為:式中，，所以有對(duì)于連桿i上任一點(diǎn)的速度為點(diǎn)P的速度為:式中，，所以有對(duì)于連桿i上任一點(diǎn)的速度為20P點(diǎn)的加速度P點(diǎn)速度的平方P點(diǎn)的加速度P點(diǎn)速度的平方21對(duì)于任一機(jī)械手上一點(diǎn)的速度平方為式中，Trace表示矩陣的跡。對(duì)于n階方程來(lái)說(shuō)，其跡即為它的主對(duì)角線(xiàn)上各元素之和。對(duì)于任一機(jī)械手上一點(diǎn)的速度平方為式中，Trace表示矩陣的跡224.2.2動(dòng)能和位能的計(jì)算令連桿3上任一質(zhì)點(diǎn)p的質(zhì)量為dm，則其動(dòng)能為4.2.2動(dòng)能和位能的計(jì)算23任一機(jī)械手連桿i上位置矢量其動(dòng)能如下式表示：的質(zhì)點(diǎn)，任一機(jī)械手連桿i上位置矢量其動(dòng)能如下式表示：的質(zhì)點(diǎn)，24對(duì)連桿3積分dK3，得連桿3的動(dòng)能為式中，積分稱(chēng)為連桿的偽慣量矩陣，并記為這樣，對(duì)連桿3積分dK3，得連桿3的動(dòng)能為式中，積分稱(chēng)為連桿的偽25任何機(jī)械手上任一連桿i動(dòng)能為（4.17）式中，Ii為偽慣量矩陣，其一般表達(dá)式為任何機(jī)械手上任一連桿i動(dòng)能為（4.17）式中，Ii為偽慣量26根據(jù)理論力學(xué)或物理學(xué)可知，物體的轉(zhuǎn)動(dòng)慣量、矢量積以及一階矩量為如果令根據(jù)理論力學(xué)或物理學(xué)可知，物體的轉(zhuǎn)動(dòng)慣量、矢量積以及27機(jī)構(gòu)學(xué)與機(jī)器人學(xué)課件28于是可把Ii表示為（4.18）于是可把Ii表示為（4.18）29具有n個(gè)連桿的機(jī)械手總的動(dòng)能為（4.19）此外，連桿i的傳動(dòng)裝置動(dòng)能為式中，Iai為傳動(dòng)裝置的等效轉(zhuǎn)動(dòng)慣量，對(duì)于平動(dòng)關(guān)節(jié)，Iai為等效質(zhì)量；為關(guān)節(jié)i的速度。所有關(guān)節(jié)的傳動(dòng)裝置總動(dòng)能為具有n個(gè)連桿的機(jī)械手總的動(dòng)能為（4.19）此外，連桿i的傳30于是得到機(jī)械手系統(tǒng)（包括傳動(dòng)裝置）的總動(dòng)能為（4.20）下面再來(lái)計(jì)算機(jī)械手的位能，一個(gè)在高度h處質(zhì)量為m的物體，其位能為連桿i上位置處的質(zhì)點(diǎn)dm，其位能為于是得到機(jī)械手系統(tǒng)（包括傳動(dòng)裝置）的總動(dòng)能為（4.20）下面31式中，其中，mi為連桿i的質(zhì)量：關(guān)節(jié)坐標(biāo)系的重心位置。為連桿i相對(duì)于其由于傳動(dòng)裝置的重力作用Pai一般是很小的，可以略之不計(jì)，所以，機(jī)械手系統(tǒng)的總位能為（4.21）式中，其中，mi為連桿i的質(zhì)量：關(guān)節(jié)坐標(biāo)系的重心位置。為324.2.3動(dòng)力學(xué)方程的推導(dǎo)據(jù)式（4.1）求拉格朗日函數(shù)（4.22）再據(jù)式（4.2）求動(dòng)力學(xué)方程。先求導(dǎo)數(shù)4.2.3動(dòng)力學(xué)方程的推導(dǎo)（4.22）再據(jù)式（4.2）33據(jù)式（4.18）知，Ii為對(duì)稱(chēng)矩陣，即，所以下式成立當(dāng)p>i時(shí)，后面連桿變量qp對(duì)前面各連桿不產(chǎn)生影響，即

。這樣可得據(jù)式（4.18）知，Ii為對(duì)稱(chēng)矩陣，即，所以下式成立當(dāng)p>34因?yàn)橐驗(yàn)?5所以所以36再求項(xiàng)在上列兩式運(yùn)算中，交換第二項(xiàng)和式的啞元j和k，然后與第一項(xiàng)和式合并，獲得化簡(jiǎn)式。把上述兩式代入（4.2）的右式得再求項(xiàng)在上列兩式運(yùn)算中，交換第二項(xiàng)和式的啞元j和k，然后與37交換上列各和式中的啞元，以i代替p，以j代替i，以m代替j，即可得具有n個(gè)連桿的機(jī)械手系統(tǒng)動(dòng)力學(xué)方程如下交換上列各和式中的啞元，以i代替p，以j代替i，以m代替j，38這些方程式是與求和次序無(wú)關(guān)的。我們把式（4.23）寫(xiě)成下列形式：（4.24）（4.25）(4.26)（4.27）式中取n=6，而且慣量項(xiàng)加速度系數(shù)項(xiàng)重力項(xiàng)這些方程式是與求和次序無(wú)關(guān)的。我們把式（4.23）寫(xiě)成下列形39上述各方程與4.1.2節(jié)的慣量項(xiàng)及重力項(xiàng)一樣。這些項(xiàng)在機(jī)械手控制中特別重要，因?yàn)樗鼈冎苯佑绊憴C(jī)械手系統(tǒng)的穩(wěn)定性和定位精度。只有當(dāng)機(jī)械手高速運(yùn)動(dòng)時(shí)，向心力和哥氏力才是重要的。這時(shí)，它們所產(chǎn)生的誤差不大。傳動(dòng)裝置的慣量Iai往往具有相當(dāng)大的值，而且對(duì)減少有效慣量的結(jié)構(gòu)相關(guān)性和耦合慣量項(xiàng)的相對(duì)重要性有顯著影響。上述各方程與4.1.2節(jié)的慣量項(xiàng)及重力項(xiàng)一樣。這些404.3.1二連桿機(jī)械手動(dòng)力學(xué)方程在前面討論過(guò)二連桿機(jī)械手的動(dòng)力學(xué)方程，下面討論二連桿機(jī)械手慣量項(xiàng)及重力項(xiàng)的計(jì)算。首先，規(guī)定機(jī)械手的坐標(biāo)系，如圖，并計(jì)算A矩陣和T矩陣。表4.2表示各連桿參數(shù)。表4.2二連桿機(jī)械手連桿參數(shù)連桿變量αad1θ10°d102θ20°d204.3.1二連桿機(jī)械手動(dòng)力學(xué)方程表4.2二連桿機(jī)械手連41A矩陣和T矩陣如下：A矩陣和T矩陣如下：42對(duì)于旋轉(zhuǎn)關(guān)節(jié)，可得微分平移矢量和微分旋轉(zhuǎn)矢量如下：對(duì)于平移關(guān)節(jié)，可得各矢量如下：對(duì)于旋轉(zhuǎn)關(guān)節(jié)，可得微分平移矢量和微分旋轉(zhuǎn)矢量如下：對(duì)于平移關(guān)43以為基礎(chǔ)，有下式(p=1，i=1)以為基礎(chǔ)，可得下式(p=2，i=2)以為基礎(chǔ)，可得下式(p=2，i=1)因兩關(guān)節(jié)均屬旋轉(zhuǎn)型，根據(jù)來(lái)計(jì)算d和δ：以為基礎(chǔ)，有下式(p=1，i=1)以為基礎(chǔ)，可得下式(p44前述慣量項(xiàng)簡(jiǎn)化為：對(duì)于這個(gè)簡(jiǎn)單機(jī)械手，其所有慣性力矩為零，即前述慣量項(xiàng)簡(jiǎn)化為：對(duì)于這個(gè)簡(jiǎn)單機(jī)械手，其所有慣性力矩為零，即45機(jī)構(gòu)學(xué)與機(jī)器人學(xué)課件46機(jī)構(gòu)學(xué)與機(jī)器人學(xué)課件47前述重力項(xiàng)簡(jiǎn)化為：對(duì)于旋轉(zhuǎn)關(guān)節(jié)i：前述重力項(xiàng)簡(jiǎn)化為：對(duì)于旋轉(zhuǎn)關(guān)節(jié)i：48所以可得下列各式：noap所以可得下列各式：noap49再求各質(zhì)心矢量再求各質(zhì)心矢量50以上所求得各項(xiàng)，可與4.1.2節(jié)中D1和D2加以比較，以檢驗(yàn)計(jì)算結(jié)果的正確性。以上所求得各項(xiàng)，可與4.1.2節(jié)中D1和D2加以比較，以檢驗(yàn)51第六章機(jī)器人的動(dòng)力學(xué)第六章機(jī)器人的動(dòng)力學(xué)52分析機(jī)器人操作的動(dòng)態(tài)數(shù)學(xué)模型，主要采用下列兩種理論：（1）動(dòng)力學(xué)基本理論，包括牛頓-歐拉方程。（2）拉格朗日力學(xué)，特別是二階拉格朗日方程。拉格朗日函數(shù)L被定義為系統(tǒng)的動(dòng)能K和位能P之差，即:（4.1）其中，K和P可以用任何方便的坐標(biāo)來(lái)表示。分析機(jī)器人操作的動(dòng)態(tài)數(shù)學(xué)模型，主要采用下列兩種理論：（453系統(tǒng)動(dòng)力學(xué)方程式，即拉格朗日方程如下：（4.2）式中，為表示動(dòng)能和位能的坐標(biāo)，速度，而為相應(yīng)的Fi是力或是力矩，由qi為直線(xiàn)坐標(biāo)或角坐標(biāo)所決定。這些力、力矩和坐標(biāo)稱(chēng)為廣義力、廣義力矩和廣義坐標(biāo)，n為連桿數(shù)目。Fi為作用在第i個(gè)坐標(biāo)上的力或是力矩。系統(tǒng)動(dòng)力學(xué)方程式，即拉格朗日方程如下：（4.2）式中，為表544.1.1剛體的動(dòng)能與位能在理論力學(xué)或物理力學(xué)部分，曾對(duì)如圖4.1所示的一般物體平動(dòng)時(shí)所具有的動(dòng)能和位能進(jìn)行過(guò)計(jì)算，其求法是大家所熟悉的，如下：4.1.1剛體的動(dòng)能與位能55式中，表示物體所具有的動(dòng)能K、位能P，所消耗的能量D和外力所做的功W；M0和M1為支架和運(yùn)動(dòng)物體的質(zhì)量；x0和x1為運(yùn)動(dòng)坐標(biāo)；g為重力加速度；k為彈簧虎克系數(shù)；c為摩擦系數(shù)；F為外施作用力。對(duì)于這一問(wèn)題，存在兩種情況。1、為廣義坐標(biāo)式中，表示物體所具有的動(dòng)能K、位能P，所消耗的能量D和外力所56其中，左式第一項(xiàng)為動(dòng)能隨速度（或角速度）和時(shí)間的變化；第二項(xiàng)為動(dòng)能隨位置（或角度）的變化；第三項(xiàng)為能耗隨速度的變化；第四項(xiàng)為位能隨位置的變化。右式為實(shí)際外加力或力矩。代入相應(yīng)各項(xiàng)的表達(dá)式，并化簡(jiǎn)可得:其中，左式第一項(xiàng)為動(dòng)能隨速度（或角速度）和時(shí)間的變化；第二項(xiàng)57表示為一般形式為:即為所求x0=0時(shí)的動(dòng)力學(xué)方程式。其中，左式三項(xiàng)分別表示物體的加速度、阻力和彈力，而右式兩項(xiàng)分別表示外加作用力和重力。表示為一般形式為:即為所求x0=0時(shí)的動(dòng)力學(xué)方程式。其中，左582、均為廣義坐標(biāo)這時(shí)有下式:或用矩陣形式表示為:2、均為廣義坐標(biāo)這時(shí)有下式:或用矩陣形式表示為:59下面來(lái)考慮二連桿機(jī)械手（見(jiàn)圖4.2）的動(dòng)能和位能。這種運(yùn)動(dòng)機(jī)構(gòu)具有開(kāi)式運(yùn)動(dòng)鏈，與復(fù)擺運(yùn)動(dòng)有許多相似之處。圖中，T1和T2為轉(zhuǎn)矩，m1和m2為連桿1和連桿2的質(zhì)量，且以連桿末端的點(diǎn)質(zhì)量表示；d1和d2分別為兩連桿的長(zhǎng)度，θ1和θ2為廣義坐標(biāo)；g為重力加速度。下面來(lái)考慮二連桿機(jī)械手（見(jiàn)圖4.2）的動(dòng)能和位能。這種60先計(jì)算連桿1的動(dòng)能K1和位能P1。因?yàn)?，所以有:再求連桿2的動(dòng)能K2和位能P2:先計(jì)算連桿1的動(dòng)能K1和位能P1。因?yàn)?，所以有:再求連桿261式中:于是可求得:以及:式中:于是可求得:以及:62這樣，二連桿機(jī)械手系統(tǒng)的總動(dòng)能和總位能分別為:（4.3）（4.4）這樣，二連桿機(jī)械手系統(tǒng)的總動(dòng)能和總位能分別為:（4.3）（63機(jī)構(gòu)學(xué)與機(jī)器人學(xué)課件64將相應(yīng)各導(dǎo)數(shù)和偏導(dǎo)數(shù)代入(4.2)，即可求得力矩T1和T2的動(dòng)力學(xué)方程式：將相應(yīng)各導(dǎo)數(shù)和偏導(dǎo)數(shù)代入(4.2)，即可求得力矩T1和T2的65機(jī)構(gòu)學(xué)與機(jī)器人學(xué)課件66重力項(xiàng)：重力項(xiàng)：67在分析簡(jiǎn)單的二連桿機(jī)械手系統(tǒng)的基礎(chǔ)上，我們分析由一組A變換描述的任何機(jī)械手，求出動(dòng)力學(xué)方程。推導(dǎo)過(guò)程分五步進(jìn)行。（1）計(jì)算任一連桿上任一點(diǎn)的速度；（2）計(jì)算各連桿的動(dòng)能和機(jī)械手的總動(dòng)能；（3）計(jì)算各連桿的位能和機(jī)械手的總位能；（4）建立機(jī)械手系統(tǒng)的拉格朗日函數(shù)；（5）對(duì)拉格朗日函數(shù)求導(dǎo)，以得到動(dòng)力學(xué)方程式。在分析簡(jiǎn)單的二連桿機(jī)械手系統(tǒng)的基礎(chǔ)上，68圖4.4表示一個(gè)四連桿機(jī)械手的結(jié)構(gòu)。我們先從這個(gè)例子出發(fā)，求得此機(jī)械手某個(gè)連桿（例如連桿3）上某一點(diǎn)（如點(diǎn)P）的速度、質(zhì)點(diǎn)和機(jī)械手的動(dòng)能與位能、拉格朗日算子，再求系統(tǒng)的動(dòng)力學(xué)方程式。然后，由特殊到一般，導(dǎo)出任何機(jī)械手的速度、動(dòng)能、位能和動(dòng)力學(xué)方程的一般表達(dá)式。圖4.4表示一個(gè)四連桿機(jī)械手的結(jié)構(gòu)。我們先從這個(gè)例694.2.1速度的計(jì)算圖4.4中連桿3上點(diǎn)P的位置為：式中，為總（基）坐標(biāo)系中的位置矢量；為局部（相對(duì)關(guān)節(jié)O3）坐標(biāo)系中的位置矢量；T3為變換矩陣，包括旋轉(zhuǎn)變換和平移變換。對(duì)于任一連桿i上的一點(diǎn)，其位置為:（4.14）4.2.1速度的計(jì)算式中，為總（基）坐標(biāo)系中的位置矢70點(diǎn)P的速度為:式中，，所以有對(duì)于連桿i上任一點(diǎn)的速度為點(diǎn)P的速度為:式中，，所以有對(duì)于連桿i上任一點(diǎn)的速度為71P點(diǎn)的加速度P點(diǎn)速度的平方P點(diǎn)的加速度P點(diǎn)速度的平方72對(duì)于任一機(jī)械手上一點(diǎn)的速度平方為式中，Trace表示矩陣的跡。對(duì)于n階方程來(lái)說(shuō)，其跡即為它的主對(duì)角線(xiàn)上各元素之和。對(duì)于任一機(jī)械手上一點(diǎn)的速度平方為式中，Trace表示矩陣的跡734.2.2動(dòng)能和位能的計(jì)算令連桿3上任一質(zhì)點(diǎn)p的質(zhì)量為dm，則其動(dòng)能為4.2.2動(dòng)能和位能的計(jì)算74任一機(jī)械手連桿i上位置矢量其動(dòng)能如下式表示：的質(zhì)點(diǎn)，任一機(jī)械手連桿i上位置矢量其動(dòng)能如下式表示：的質(zhì)點(diǎn)，75對(duì)連桿3積分dK3，得連桿3的動(dòng)能為式中，積分稱(chēng)為連桿的偽慣量矩陣，并記為這樣，對(duì)連桿3積分dK3，得連桿3的動(dòng)能為式中，積分稱(chēng)為連桿的偽76任何機(jī)械手上任一連桿i動(dòng)能為（4.17）式中，Ii為偽慣量矩陣，其一般表達(dá)式為任何機(jī)械手上任一連桿i動(dòng)能為（4.17）式中，Ii為偽慣量77根據(jù)理論力學(xué)或物理學(xué)可知，物體的轉(zhuǎn)動(dòng)慣量、矢量積以及一階矩量為如果令根據(jù)理論力學(xué)或物理學(xué)可知，物體的轉(zhuǎn)動(dòng)慣量、矢量積以及78機(jī)構(gòu)學(xué)與機(jī)器人學(xué)課件79于是可把Ii表示為（4.18）于是可把Ii表示為（4.18）80具有n個(gè)連桿的機(jī)械手總的動(dòng)能為（4.19）此外，連桿i的傳動(dòng)裝置動(dòng)能為式中，Iai為傳動(dòng)裝置的等效轉(zhuǎn)動(dòng)慣量，對(duì)于平動(dòng)關(guān)節(jié)，Iai為等效質(zhì)量；為關(guān)節(jié)i的速度。所有關(guān)節(jié)的傳動(dòng)裝置總動(dòng)能為具有n個(gè)連桿的機(jī)械手總的動(dòng)能為（4.19）此外，連桿i的傳81于是得到機(jī)械手系統(tǒng)（包括傳動(dòng)裝置）的總動(dòng)能為（4.20）下面再來(lái)計(jì)算機(jī)械手的位能，一個(gè)在高度h處質(zhì)量為m的物體，其位能為連桿i上位置處的質(zhì)點(diǎn)dm，其位能為于是得到機(jī)械手系統(tǒng)（包括傳動(dòng)裝置）的總動(dòng)能為（4.20）下面82式中，其中，mi為連桿i的質(zhì)量：關(guān)節(jié)坐標(biāo)系的重心位置。為連桿i相對(duì)于其由于傳動(dòng)裝置的重力作用Pai一般是很小的，可以略之不計(jì)，所以，機(jī)械手系統(tǒng)的總位能為（4.21）式中，其中，mi為連桿i的質(zhì)量：關(guān)節(jié)坐標(biāo)系的重心位置。為834.2.3動(dòng)力學(xué)方程的推導(dǎo)據(jù)式（4.1）求拉格朗日函數(shù)（4.22）再據(jù)式（4.2）求動(dòng)力學(xué)方程。先求導(dǎo)數(shù)4.2.3動(dòng)力學(xué)方程的推導(dǎo)（4.22）再據(jù)式（4.2）84據(jù)式（4.18）知，Ii為對(duì)稱(chēng)矩陣，即，所以下式成立當(dāng)p>i時(shí)，后面連桿變量qp對(duì)前面各連桿不產(chǎn)生影響，即

。這樣可得據(jù)式（4.18）知，Ii為對(duì)稱(chēng)矩陣，即，所以下式成立當(dāng)p>85因?yàn)橐驗(yàn)?6所以所以87再求項(xiàng)在上列兩式運(yùn)算中，交換第二項(xiàng)和式的啞元j和k，然后與第一項(xiàng)和式合并，獲得化簡(jiǎn)式。把上述兩式代入（4.2）的右式得再求項(xiàng)在上列兩式運(yùn)算中，交換第二項(xiàng)和式的啞元j和k，然后與88交換上列各和式中的啞元，以i代替p，以j代替i，以m代替j，即可得具有n個(gè)連桿的機(jī)械手系統(tǒng)動(dòng)力學(xué)方程如下交換上列各和式中的啞元，以i代替p，以j代替i，以m代替j，89這些方程式是與求和次序無(wú)關(guān)的。我們把式（4.23）寫(xiě)成下列形式：（4.24）（4.25）(4.26)（4.27）式中取n=6，而且慣量項(xiàng)加速度系數(shù)項(xiàng)重力項(xiàng)這些方程式是與求和次序無(wú)關(guān)的。我們把式（4.23）寫(xiě)成下列形90上述各方程與4.1.2節(jié)的慣量項(xiàng)及重力項(xiàng)一樣。這些項(xiàng)在機(jī)械手控制中