第七講機(jī)器人動(dòng)力學(xué)

第四章機(jī)器人動(dòng)力學(xué)4.1動(dòng)力學(xué)模型4.2牛頓——?dú)W拉方程法習(xí)題04二月2023第四章機(jī)器人動(dòng)力學(xué)04二月2023動(dòng)力學(xué)研究的問(wèn)題：機(jī)器人各個(gè)關(guān)節(jié)的運(yùn)動(dòng)與關(guān)節(jié)需要的驅(qū)動(dòng)力（矩）之間的關(guān)系。正問(wèn)題：已知關(guān)節(jié)運(yùn)動(dòng)，求關(guān)節(jié)驅(qū)動(dòng)力（矩）。逆問(wèn)題：已知關(guān)節(jié)驅(qū)動(dòng)力（矩），求關(guān)節(jié)運(yùn)動(dòng)。數(shù)學(xué)模型：關(guān)節(jié)運(yùn)動(dòng)→位移、速度、加速度變化→關(guān)節(jié)驅(qū)動(dòng)力（矩）→驅(qū)動(dòng)力或驅(qū)動(dòng)力矩→τi動(dòng)力學(xué)方程：

，i=1，…，n正問(wèn)題：已知

，求τi。逆問(wèn)題：已知τi

，求。第四章機(jī)器人動(dòng)力學(xué)04二月20234.1動(dòng)力學(xué)模型1、力學(xué)分析2、拉格朗日方程法3、動(dòng)力學(xué)模型04二月20234.1動(dòng)力學(xué)模型1、力學(xué)分析04二月2023（1）靜力學(xué)分析機(jī)器人各個(gè)關(guān)節(jié)處于靜止?fàn)顟B(tài)。當(dāng)負(fù)載為一重物時(shí)：關(guān)節(jié)承受的力和力矩：關(guān)節(jié)需要的驅(qū)動(dòng)力（矩）：mgf3=mgf2=mgf1=mgm2=mgl2l2l1m1=mg(l1+l2)τ1=0τ2=mgl2τ3=mg4.1動(dòng)力學(xué)模型1、力學(xué)分析04二月2023（1）靜力學(xué)分析機(jī)器人各個(gè)關(guān)節(jié)處于靜止?fàn)顟B(tài)。考慮桿件自重時(shí)：關(guān)節(jié)承受的力和力矩：關(guān)節(jié)需要的驅(qū)動(dòng)力（矩）：mgf3=mgf2=mgf1=mgm2=mgl2l2l1m1=mg(l1+l2)τ1=0τ2=mgl2τ3=mgm3gm2gm1g4.1動(dòng)力學(xué)模型1、力學(xué)分析04二月2023（2）動(dòng)力學(xué)分析機(jī)器人各個(gè)關(guān)節(jié)處于運(yùn)動(dòng)狀態(tài)。當(dāng)負(fù)載為一重物時(shí)：關(guān)節(jié)承受的力和力矩：關(guān)節(jié)需要的驅(qū)動(dòng)力（矩）：f3f2f1m2l2l1m1τ1τ2τ3m34.1動(dòng)力學(xué)模型2、拉格朗日方程法04二月2023拉格朗日方程的一般形式為：

式中，——廣義力，它可以是力，也可以是力矩；

——系統(tǒng)選定的廣義坐標(biāo)；

——廣義坐標(biāo)對(duì)時(shí)間的一階導(dǎo)數(shù)，即速度；

——拉格朗日函數(shù)，又稱(chēng)為拉格朗日算子，它被定義為系統(tǒng)的動(dòng)能與勢(shì)能之差L=T-U。4.1動(dòng)力學(xué)模型2、拉格朗日方程法04二月2023

對(duì)給定的機(jī)器人，可以按以下幾個(gè)步驟建立拉格朗日動(dòng)力學(xué)方程：（1）選取完全并獨(dú)立的廣義坐標(biāo)；（2）選定廣義力；（3）求出系統(tǒng)的動(dòng)能T和勢(shì)能U，并用其構(gòu)造拉格朗日函數(shù)L=T-U；（4）將以上結(jié)果代入拉格朗日方程式中，即可求得機(jī)器人的動(dòng)力學(xué)方程。4.1動(dòng)力學(xué)模型2、拉格朗日方程法04二月2023例：已知二關(guān)節(jié)機(jī)器人如圖所示，機(jī)器人的兩個(gè)連桿長(zhǎng)度分別為l1和l2，質(zhì)量分別為m1和m2，且集中在各連桿的端部。若將機(jī)器人直接懸掛在加速度為g的重力場(chǎng)中，試用拉格朗日方程建立該機(jī)器人的動(dòng)力學(xué)方程。解：①選取連桿繞關(guān)節(jié)的轉(zhuǎn)角為變量θ1和θ2

，則系統(tǒng)的廣義坐標(biāo)就可以選為，即②轉(zhuǎn)動(dòng)關(guān)節(jié)對(duì)應(yīng)的是力矩，所以廣義力就選為，即。θ1θ2m1m2xy關(guān)節(jié)1關(guān)節(jié)24.1動(dòng)力學(xué)模型2、拉格朗日方程法04二月2023求出各連桿的動(dòng)能和勢(shì)能：連桿l1的動(dòng)能為：連桿l1的勢(shì)能為：對(duì)連桿l2求動(dòng)能和勢(shì)能時(shí)，要先寫(xiě)出其質(zhì)心在直角坐標(biāo)系中的位置表達(dá)式：然后求微分，則其速度就為：由此可得連桿的速度平方值為：4.1動(dòng)力學(xué)模型2、拉格朗日方程法04二月2023求出各連桿的動(dòng)能和勢(shì)能：從而連桿l2的動(dòng)能為：勢(shì)能為：則可構(gòu)造出拉格朗日函數(shù)為：

4.1動(dòng)力學(xué)模型2、拉格朗日方程法04二月2023求出機(jī)器人動(dòng)力學(xué)方程：先將拉格朗日函數(shù)對(duì)和進(jìn)行微分，即：4.1動(dòng)力學(xué)模型2、拉格朗日方程法04二月2023求出機(jī)器人動(dòng)力學(xué)方程：再將拉格朗日函數(shù)對(duì)和進(jìn)行微分，即：4.1動(dòng)力學(xué)模型2、拉格朗日方程法04二月2023求出機(jī)器人動(dòng)力學(xué)方程：將以上結(jié)果代入方程即可得關(guān)節(jié)上的力矩分別為：

4.1動(dòng)力學(xué)模型3、動(dòng)力學(xué)模型04二月2023將得到的機(jī)器人動(dòng)力學(xué)方程簡(jiǎn)寫(xiě)為如下形式：當(dāng)機(jī)器人有n個(gè)關(guān)節(jié)時(shí)，上式可推廣為普遍形式：

4.1動(dòng)力學(xué)模型3、動(dòng)力學(xué)模型04二月2023將上式進(jìn)一步簡(jiǎn)化為如下所示的矩陣形式：

上式也稱(chēng)為機(jī)器人的動(dòng)力學(xué)模型。式中：是機(jī)器人動(dòng)力學(xué)模型中的慣性力項(xiàng)，表示機(jī)器人操作機(jī)的質(zhì)量矩陣，它是n×n階的對(duì)稱(chēng)矩陣，。

是n×1階矩陣，表示機(jī)器人動(dòng)力學(xué)模型中非線(xiàn)性的耦合力項(xiàng)，包括離心力（自耦力）和哥氏力（互耦力）。也是n×1階矩陣，表示機(jī)器人動(dòng)力學(xué)模型中的重力項(xiàng)。

4.2牛頓——?dú)W拉方程法04二月20231、牛頓——?dú)W拉方程2、遞推計(jì)算公式3、遞推算法應(yīng)用4.2牛頓——?dú)W拉方程法04二月2023牛頓——?dú)W拉方程原理：將機(jī)器人的每個(gè)桿件看成剛體，并確定每個(gè)桿件質(zhì)心的位置和表征其質(zhì)量分布的慣性張量矩陣。當(dāng)確定機(jī)器人坐標(biāo)系后，根據(jù)機(jī)器人關(guān)節(jié)速度和加速度，則可先由機(jī)器人機(jī)座開(kāi)始向手部桿件正向遞推出每個(gè)桿件在自身坐標(biāo)系中的速度和加速度，再用牛頓——?dú)W拉方程得到機(jī)器人每個(gè)桿件上的慣性力和慣性力矩，然后再由機(jī)器人末端關(guān)節(jié)開(kāi)始向第一個(gè)關(guān)節(jié)反向遞推出機(jī)器人每個(gè)關(guān)節(jié)上承受的力和力矩，最終得到機(jī)器人每個(gè)關(guān)節(jié)所需要的驅(qū)動(dòng)力（矩），這樣就確定了機(jī)器人關(guān)節(jié)的驅(qū)動(dòng)力（矩）與關(guān)節(jié)位移、速度和加速度之間的函數(shù)關(guān)系，即建立了機(jī)器人的動(dòng)力學(xué)方程。4.2牛頓——?dú)W拉方程法04二月2023牛頓——?dú)W拉方程遞推過(guò)程：正向遞推：已知機(jī)器人各個(gè)關(guān)節(jié)的速度和加速度→從1～n遞推出機(jī)器人每個(gè)桿件在自身坐標(biāo)系中的速度和加速度→機(jī)器人每個(gè)桿件質(zhì)心上的速度和加速度→再用牛頓——?dú)W拉方程得到機(jī)器人每個(gè)桿件質(zhì)心上的慣性力和慣性力矩。反向遞推：根據(jù)正向遞推的結(jié)果→從n～1遞推出機(jī)器人每個(gè)關(guān)節(jié)上承受的力和力矩→得到機(jī)器人每個(gè)關(guān)節(jié)所需要的驅(qū)動(dòng)力（矩）。4.2牛頓——?dú)W拉方程法04二月20231、牛頓——?dú)W拉方程（1）牛頓方程→慣性力①矢量。②質(zhì)心上的線(xiàn)加速度。4.2牛頓——?dú)W拉方程法04二月20231、牛頓——?dú)W拉方程（2）歐拉方程→慣性力矩①矢量。②質(zhì)心上的慣性張量矩陣。4.2牛頓——?dú)W拉方程法04二月20231、牛頓——?dú)W拉方程（2）歐拉方程→慣性力矩慣性張量矩陣：a.坐標(biāo)系：與桿件坐標(biāo)系同向，位于桿件質(zhì)心上。b.元素名稱(chēng)：Icxx，Icyy，Iczz——慣性矩；Icxy=Icyx,Icyz=Iczy,Iczx=Icxz——慣性積。4.2牛頓——?dú)W拉方程法04二月20231、牛頓——?dú)W拉方程（2）歐拉方程→慣性力矩慣性張量矩陣：理論計(jì)算方法：實(shí)驗(yàn)測(cè)試法：

慣量擺儀器。4.2牛頓——?dú)W拉方程法04二月20232、遞推計(jì)算公式（1）正向遞推：已知機(jī)器人各個(gè)關(guān)節(jié)的速度和加速度：①?gòu)?～n遞推出機(jī)器人每個(gè)桿件在自身坐標(biāo)系中的速度和加速度；②機(jī)器人每個(gè)桿件質(zhì)心上的速度和加速度；③機(jī)器人每個(gè)桿件質(zhì)心上的慣性力和慣性力矩。4.2牛頓——?dú)W拉方程法04二月20232、遞推計(jì)算公式（1）正向遞推：以第一種桿件坐標(biāo)系為例①桿件速度和加速度遞推計(jì)算公式建立相鄰兩個(gè)桿件的坐標(biāo)系：{i-1}、{i}。Oiii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（1）正向遞推：①桿件速度和加速度遞推計(jì)算公式已知：i關(guān)節(jié)速度和加速度

i-1桿件速度和加速度計(jì)算：i桿件速度和加速度04二月2023ii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（1）正向遞推：①桿件速度和加速度遞推計(jì)算公式分析：I、坐標(biāo)系:

相鄰桿件位姿矩陣II、關(guān)節(jié)速度和加速度的矢量化：04二月2023ii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（1）正向遞推：①桿件速度和加速度遞推計(jì)算公式04二月2023ii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（1）正向遞推：①桿件速度和加速度遞推計(jì)算公式04二月2023ii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（1）正向遞推：①桿件速度和加速度遞推計(jì)算公式04二月2023關(guān)節(jié)iii-1Xi-1Zi-1Oi-1XiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（1）正向遞推：②桿件質(zhì)心上的速度和加速度04二月2023iXiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（1）正向遞推：③桿件質(zhì)心上的慣性力和慣性力矩慣性力：慣性力矩：04二月2023iXiZiOi4.2牛頓——?dú)W拉方程法04二月20232、遞推計(jì)算公式（1）正向遞推：以第二種桿件坐標(biāo)系為例①桿件速度和加速度遞推計(jì)算公式建立相鄰兩個(gè)桿件的坐標(biāo)系：{i-1}、{i}。ii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（1）正向遞推：①桿件速度和加速度遞推計(jì)算公式已知：i關(guān)節(jié)速度和加速度

i-1桿件速度和加速度計(jì)算：i桿件速度和加速度04二月2023ii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（1）正向遞推：①桿件速度和加速度遞推計(jì)算公式分析：I、坐標(biāo)系:

相鄰桿件位姿矩陣II、關(guān)節(jié)速度和加速度的矢量化：04二月2023ii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（1）正向遞推：①桿件速度和加速度遞推計(jì)算公式04二月2023ii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（1）正向遞推：①桿件速度和加速度遞推計(jì)算公式04二月2023ii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（1）正向遞推：②桿件質(zhì)心上的速度和加速度04二月2023iXiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（1）正向遞推：③桿件質(zhì)心上的慣性力和慣性力矩慣性力：慣性力矩：04二月2023iXiZiOi4.2牛頓——?dú)W拉方程法04二月20232、遞推計(jì)算公式（2）反向遞推：已知機(jī)器人各個(gè)桿件的慣性力和慣性力矩：①?gòu)膎～1遞推出機(jī)器人每個(gè)關(guān)節(jié)承受的力和力矩；②機(jī)器人每個(gè)關(guān)節(jié)的驅(qū)動(dòng)力或驅(qū)動(dòng)力矩。4.2牛頓——?dú)W拉方程法04二月20232、遞推計(jì)算公式（2）反向遞推：以第二種桿件坐標(biāo)系為例①關(guān)節(jié)承受的力和力矩遞推計(jì)算公式建立相鄰兩個(gè)桿件的坐標(biāo)系：{i-1}、{i}ii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（2）反向遞推：①關(guān)節(jié)承受的力和力矩遞推計(jì)算公式已知：i-1桿件的慣性力和慣性力矩

i關(guān)節(jié)承受的力和力矩計(jì)算：i-1關(guān)節(jié)承受的力和力矩04二月2023ii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（2）反向遞推：①關(guān)節(jié)承受的力和力矩遞推計(jì)算公式分析：I、坐標(biāo)系:

相鄰桿件位姿矩陣II、i-1桿件受力分析：04二月2023ii-1關(guān)節(jié)iXi-1Zi-1Oi-1XiZiOi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（2）反向遞推：①關(guān)節(jié)承受的力和力矩遞推計(jì)算公式以i-1桿件為研究對(duì)象，由達(dá)朗貝爾原理可得：04二月2023i-1關(guān)節(jié)iXi-1Zi-1Oi-1Oi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（2）反向遞推：①關(guān)節(jié)承受的力和力矩遞推計(jì)算公式以i-1桿件為研究對(duì)象，由達(dá)朗貝爾原理可得：04二月2023i-1關(guān)節(jié)iXi-1Zi-1Oi-1Oi4.2牛頓——?dú)W拉方程法2、遞推計(jì)算公式（2）反向遞推：②關(guān)節(jié)驅(qū)動(dòng)力（矩）平移關(guān)節(jié)：回轉(zhuǎn)關(guān)節(jié)：則關(guān)節(jié)驅(qū)動(dòng)力（矩）為：04二月2023i-1關(guān)節(jié)iXi-1Zi-1Oi-14.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用（1）遞推初始條件①正向遞推機(jī)座0的速度和加速度：*考慮桿件自重或手部負(fù)載為重物時(shí)：為描述在機(jī)座坐標(biāo)系{0}中的標(biāo)準(zhǔn)重力加速度。04二月2023gx0z0o04.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用（1）遞推初始條件②反向遞推機(jī)器人手部負(fù)載：04二月20234.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用（2）遞推應(yīng)用條件①已知機(jī)器人的關(guān)節(jié)變量及其速度和加速度；②已知任一桿件i相對(duì)于與自身坐標(biāo)系{i}方向相同的坐標(biāo)系{Ci}所描述的慣性張量及其質(zhì)心在自身坐標(biāo)系{i}中的位置矢量（可用實(shí)驗(yàn)等方法確定）；③已知相鄰桿件的位姿矩陣及必要的初始數(shù)據(jù)。04二月20234.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用（2）遞推應(yīng)用條件第二種坐標(biāo)系下遞推算法正向遞推:04二月20234.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用（2）遞推應(yīng)用條件第二種坐標(biāo)系下遞推算法反向遞推:04二月20234.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用例：已知二自由度機(jī)器人如圖所示，機(jī)器人兩個(gè)桿件的長(zhǎng)度分別為和，且其質(zhì)量和都集中在桿件的端頭。若用第二種方法建立機(jī)器人的坐標(biāo)系，當(dāng)機(jī)器人各個(gè)關(guān)節(jié)的位移、速度和加速度已知時(shí)，試用牛頓——?dú)W拉遞推算法計(jì)算各關(guān)節(jié)的驅(qū)動(dòng)力矩。04二月2023θ1θ2m1m2xy關(guān)節(jié)1關(guān)節(jié)24.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：建立坐標(biāo)系如圖所示。相鄰桿件的位姿矩陣為：04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y24.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(1)正向遞推已知關(guān)節(jié)速度和加速度分別為，由于考慮桿件的重量，所以機(jī)座的運(yùn)動(dòng)參數(shù)（初始條件）設(shè)為：04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(1)正向遞推04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(1)正向遞推04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(1)正向遞推04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(1)正向遞推04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(1)正向遞推04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(1)正向遞推04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(1)正向遞推04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(1)正向遞推04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(1)正向遞推04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(1)正向遞推04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(1)正向遞推04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(1)正向遞推04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(2)反向遞推由于機(jī)器人手部無(wú)負(fù)載，所以初始條件為：04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法3、遞推算法應(yīng)用解：(2)反向遞推關(guān)節(jié)2受的力和力矩：04二月2023θ1θ2m1m2x0y0關(guān)節(jié)1關(guān)節(jié)2x1x2y1y2g4.2牛頓——?dú)W拉方程法