機(jī)器人學(xué)導(dǎo)論 課件 第3章 機(jī)器人力學(xué)

機(jī)器人技術(shù)基礎(chǔ)

第3章機(jī)器人力學(xué)

3.1機(jī)器人靜力學(xué)

第1-15周，星期二，16:40-18:15，（五）10325.1.1靜力學(xué)5.1.2靜力學(xué)的迭代求解5.1.3力雅可比5.1.4速度和力矢量的坐標(biāo)變換本節(jié)目錄靜力學(xué)的概念32025/6/1已知機(jī)器人末端負(fù)載（靜力學(xué)不考慮桿件質(zhì)量）求解機(jī)器人靜止?fàn)顟B(tài)下，各關(guān)節(jié)的驅(qū)動力或力矩機(jī)器人靜力學(xué)分析方法假定關(guān)節(jié)為鎖定狀態(tài)從末端到基座逐級列出各連桿的靜力平衡方程逐級求解關(guān)節(jié)負(fù)載關(guān)節(jié)負(fù)載中包含運(yùn)動副的結(jié)構(gòu)約束力和關(guān)節(jié)驅(qū)動力關(guān)節(jié)負(fù)載的符號定義fi：連桿i-1施加在連桿i上的力ni：連桿i-1施加在連桿i上的力矩45.1.1靜力學(xué)5.1.2靜力學(xué)的迭代求解5.1.3力雅可比5.1.4速度和力矢量的坐標(biāo)變換本節(jié)目錄靜力的迭代求解52025/6/1力平衡方程單個連桿的靜力平衡方程力矩平衡方程其中：ifi為連桿i-1作用在連桿i上的力在{i}中的描述ifi+1為連桿i+1反作用于連桿i的力（或末端桿件所受外力）在{i}中的描述其中：ini為連桿i-1作用在連桿i上的力矩在{i}中的描述ini+1為連桿i+1反作用于連桿i的力矩（或末端桿件所受外力矩）在{i}中的描述iPi+1×ifi+1為ifi+1附加作用于連桿i的力矩在{i}中的描述靜力的迭代求解62025/6/1將旋轉(zhuǎn)矩陣代入，重寫靜力平衡方程靜力迭代公式上式即為靜力迭代公式，可根據(jù)末端負(fù)載逐次迭代計算各桿件之間的作用力（包含運(yùn)動副的結(jié)構(gòu)約束力和關(guān)節(jié)驅(qū)動力）問題：如何求解關(guān)節(jié)驅(qū)動力/力矩思路：連桿i-1對連桿i的作用力ifi或力矩ini矢量中，沿移動關(guān)節(jié)導(dǎo)路的力分量或繞旋轉(zhuǎn)關(guān)節(jié)軸的力矩分量顯然由驅(qū)動器提供關(guān)節(jié)力/力矩應(yīng)為關(guān)節(jié)負(fù)載與關(guān)節(jié)軸線矢量的點積（沿關(guān)節(jié)軸線分量）移動關(guān)節(jié)驅(qū)動力轉(zhuǎn)動關(guān)節(jié)驅(qū)動力矩靜力的迭代求解72025/6/1建立各連桿坐標(biāo)系，如右圖實例：右圖所示2R平面機(jī)器人，已知末端力矢量3f3，以及當(dāng)前位形的關(guān)節(jié)角，求關(guān)節(jié)力矩

寫出從末端到基座各坐標(biāo)系間的旋轉(zhuǎn)矩陣、坐標(biāo)原點矢量、負(fù)載矢量將上述已知量從末端{(lán)3}到基座{0}，逐次代入下式，計算關(guān)節(jié)轉(zhuǎn)矩靜力的迭代求解82025/6/1

對于關(guān)節(jié)2，已知則靜力的迭代求解92025/6/1

對于關(guān)節(jié)1，已知則靜力的迭代求解102025/6/1

最終，得到：寫成矩陣形式：思考：當(dāng)θ2=0°或180°，且fy=0時會發(fā)生什么現(xiàn)象？此時，無論fx多大，關(guān)節(jié)力矩始終為零！數(shù)學(xué)上，末端負(fù)載到關(guān)節(jié)負(fù)載的映射矩陣奇異！靜力的迭代求解112025/6/1

回顧坐標(biāo)系{3}中表達(dá)的速度雅可比矩陣：JT在末端坐標(biāo)系中，末端負(fù)載到關(guān)節(jié)負(fù)載的映射矩陣是速度雅可比矩陣的轉(zhuǎn)置這不是巧合！125.1.1靜力學(xué)5.1.2靜力學(xué)的迭代求解5.1.3力雅可比5.1.4速度和力矢量的坐標(biāo)變換本節(jié)目錄力雅可比矩陣132025/6/1虛功原理當(dāng)受力物體的位移趨向無窮小時，不同廣義坐標(biāo)系下力做的功相等虛功等于廣義力與廣義微位移的點積功是標(biāo)量，可在不同廣義坐標(biāo)系下描述利用虛功原理可建立不同廣義坐標(biāo)系下的力、位移映射關(guān)系對6自由度機(jī)器人而言，其中：F為笛卡爾空間的廣義力，6×1維δX為笛卡爾空間微位移，6×1維τ為關(guān)節(jié)空間的廣義力，6×1維δΘ為關(guān)節(jié)空間的微位移，6×1維力雅可比矩陣142025/6/1根據(jù)雅可比矩陣的定義可得：虛功原理δΘ是廣義坐標(biāo)的變分，可任意取，上式成立則必然有：上式是在末端坐標(biāo)系中的結(jié)論，如果已知相對于{0}坐標(biāo)系的雅可比矩陣，則用下式：力雅可比矩陣的特性152025/6/1力雅可比矩陣建立了從笛卡爾空間的力F到關(guān)節(jié)空間的力τ之間的映射關(guān)系從末端力到關(guān)節(jié)力的映射直接基于正運(yùn)動學(xué)模型獲得，而無需求逆運(yùn)算，這一特性有利于在控制中實現(xiàn)末端力控制或補(bǔ)償末端負(fù)載力雅可比矩陣同樣存在奇異性，對應(yīng)著機(jī)器人的奇異位形在奇異位形，微小的關(guān)節(jié)力矩將對應(yīng)著極大的末端力，幾何上對應(yīng)著機(jī)構(gòu)死鎖位置（連桿間壓力角為90°）165.1.1靜力學(xué)5.1.2靜力學(xué)的迭代求解5.1.3力雅可比5.1.4速度和力矢量的坐標(biāo)變換本節(jié)目錄速度和力矢量在不同坐標(biāo)系間的變換172025/6/1假定在一個剛體上存在兩個坐標(biāo)系{A}、{B}，已知在{A}中定義的力矢量和{A}的速度矢量問題：如何獲得在坐標(biāo)系{B}中定義的力矢量和{B}的速度矢量由前述定義，可知如下表達(dá)：6×1維速度矢量6×1維力矢量思路：需要考慮由于{A}、{B}坐標(biāo)原點偏移引起的線速度變化和附加力矩可以利用連桿速度和力迭代計算的方法，區(qū)別僅在于沒有關(guān)節(jié)速度和關(guān)節(jié)力速度和力矢量在不同坐標(biāo)系間的變換182025/6/1回顧速度迭代公式：

于是，可得：寫成矩陣形式：速度矢量的變換其中，叉乘算子為：速度和力矢量在不同坐標(biāo)系間的變換192025/6/1把上式寫成緊湊形式：速度矢量的變換反之，如果已知坐標(biāo)系{B}的速度矢量，則：注意速度伴隨變換矩陣與坐標(biāo)變換矩陣的區(qū)別和聯(lián)系速度伴隨變換矩陣速度和力矢量在不同坐標(biāo)系間的變換202025/6/1回顧力迭代公式：類似的，可得同一個剛體上兩個坐標(biāo)系之間的力矢量轉(zhuǎn)換方法力矢量的變換簡寫為：注意力伴隨變換矩陣與坐標(biāo)變換矩陣的區(qū)別和聯(lián)系力伴隨變換矩陣力矢量變換實例212025/6/1

思路:

根據(jù)：得到：根據(jù)下式可得所需參數(shù)：靜力學(xué)結(jié)束機(jī)器人技術(shù)基礎(chǔ)

第3章機(jī)器人力學(xué)

3.2慣量參數(shù)

第1-15周，星期二，16:40-18:15，（五）103剛體質(zhì)量分布242025/6/1對繞任意軸做旋轉(zhuǎn)運(yùn)動的剛體質(zhì)量分布的描述對物體轉(zhuǎn)動慣量的廣義度量慣量張量（慣性張量）剛體相對于坐標(biāo)系{A}

的慣量張量其中：Ixx、Iyy、Izz稱為慣量矩慣量矩也稱為繞某軸的轉(zhuǎn)動慣量Ixy、Iyz、Ixz稱為慣量積注意：慣量張量與坐標(biāo)系選取有關(guān)當(dāng)選取的坐標(biāo)系使慣量積全為零時，坐標(biāo)系主軸稱為主軸，對應(yīng)的慣量矩稱為主慣量矩25例題：

已知：如圖所示坐標(biāo)系中長方體，密度為ρ；

求：長方體的慣量張量。2025/6/1均勻密度的剛體解：首先，計算慣量矩已知體積單元，故：式中，m是剛體的總質(zhì)量。同理可得Iyy和Izz：慣量張量26然后計算Ixy：2025/6/1同理可得:因此，圖示物體的慣量張量為:慣量張量平行移軸定理272025/6/1同一個剛體在兩個坐標(biāo)系中的慣量張量之間的轉(zhuǎn)換關(guān)系平行移軸定理設(shè)已知定義在剛體質(zhì)心{C}處的慣量張量，則平移到任意坐標(biāo)系{A}的慣量張量為：其中：為{C}坐標(biāo)原點在{A}中的位置矢量平行移軸定理的矢量形式：282025/6/1慣量張量的幾個性質(zhì)慣量張量表示剛體質(zhì)量分布的特征隨坐標(biāo)系定義的不同，慣量矩永遠(yuǎn)為正三個慣量矩的和（跡）保持不變慣量積正負(fù)都有可能當(dāng)慣量積Ixy，Iyz和Izx均為0時，慣量張量變成對角型任意坐標(biāo)系中的慣量張量矩陣的特征值即為剛體主慣量矩，對應(yīng)的特征向量即為主軸慣量張量的性質(zhì)機(jī)器人技術(shù)基礎(chǔ)

第3章機(jī)器人力學(xué)

3.3牛頓-歐拉方程

第1-15周，星期二，16:40-18:15，（五）103303.3.1動力學(xué)概念3.3.2牛頓-歐拉平衡方程3.3.3剛體的線加速度3.3.4剛體的角加速度3.3.5牛頓-歐拉方程的遞推迭代本節(jié)目錄動力學(xué)的概念312025/6/1在考慮機(jī)器人連桿慣量的前提下，研究力與運(yùn)動的關(guān)系動力學(xué)的核心問題分析方法基于力平衡的方法——牛頓-歐拉方程基于能量的方法——拉格朗日方程應(yīng)用分類

——逆動力學(xué)，用于控制——正動力學(xué)，用于仿真323.3.1動力學(xué)概念3.3.2牛頓-歐拉平衡方程3.3.3剛體的線加速度3.3.4剛體的角加速度3.3.5牛頓-歐拉方程的遞推迭代本節(jié)目錄牛頓-歐拉力/力矩平衡方程（Newton-EulerEquation）332025/6/1針對每個桿件應(yīng)用力/力矩平衡方程，逐次遞推獲得桿件之間的作用力/力矩桿件之間的作用力/力矩在關(guān)節(jié)軸上的分量，即為關(guān)節(jié)驅(qū)動力/力矩牛頓-歐拉方程的思想力平衡——牛頓方程

F力矩平衡——歐拉方程

牛頓-歐拉力/力矩平衡方程342025/6/1求解動力學(xué)方程的前提是：

利用牛頓-歐拉方程求解動力學(xué)方程的主要工作是：計算各連桿角速度、角加速度、質(zhì)心線加速度計算各連桿之間的力和力矩牛頓-歐拉力/力矩平衡方程353.3.1動力學(xué)概念3.3.2牛頓-歐拉平衡方程3.3.3剛體的線加速度和角加速度3.3.4牛頓-歐拉方程的遞推迭代本節(jié)目錄剛體的線加速度362025/6/1上式直接對時間求導(dǎo)，得：AVBORF≠0BVQ≠0AΩB≠0回顧：兩剛體{A}和{B}之間既有平動，也有轉(zhuǎn)動，且{B}中向量Q相對于{B}有運(yùn)動的情況，其Q在{A}中的速度表達(dá)為：因為：代入上式，得：剛體的線加速度372025/6/1整理，得：線加速度求和科氏加速度歐拉加速度向心加速度當(dāng)BQ是常量時，可簡化為：剛體的角加速度382025/6/1同樣，把上式直接對時間求導(dǎo)，得：運(yùn)動坐標(biāo)系{B}相對于{A}的角速度為AΩB運(yùn)動坐標(biāo)系{C}相對于{B}的角速度為BΩC則{C}相對于{A}的角速度為：因為：代入上式，得：假定存在固定的參考坐標(biāo)系{A}393.3.1動力學(xué)概念3.3.2牛頓-歐拉平衡方程3.3.3剛體的線加速度和角加速度3.3.4牛頓-歐拉方程的遞推迭代本節(jié)目錄牛頓-歐拉方程的向外遞推迭代402025/6/1

可得：

思路：采用與速度外推迭代類似的方法進(jìn)行求解首先回顧前一章機(jī)器人連桿間角速度遞推公式根據(jù)前述剛體間角加速度關(guān)系：上式即為機(jī)器人旋轉(zhuǎn)關(guān)節(jié)相鄰連桿間角加速度迭代公式當(dāng)?shù)趇+1個關(guān)節(jié)是移動關(guān)節(jié)時，上式可簡化為：412025/6/1對于通過旋轉(zhuǎn)關(guān)節(jié)連接的兩連桿，根據(jù)剛體間線加速度關(guān)系公式：轉(zhuǎn)換到i+1坐標(biāo)系，得旋轉(zhuǎn)關(guān)節(jié)相鄰連桿間的線加速度迭代公式：對于i+1關(guān)節(jié)為移動關(guān)節(jié)的情況，根據(jù)：可得：可得：上式即為移動關(guān)節(jié)相鄰連桿間的線加速度迭代公式牛頓-歐拉方程的向外遞推迭代422025/6/1對于連桿i+1，再次應(yīng)用剛體間線加速度關(guān)系公式：可得連桿i+1質(zhì)心C的線加速度迭代公式：至此，我們已經(jīng)得到了牛頓-歐拉方程所需的角速度、角加速度，以及坐標(biāo)系i的線加速度還需要求解質(zhì)心線加速度回顧牛頓-歐拉方程牛頓-歐拉方程的向外遞推迭代牛頓-歐拉方程的向外遞推迭代小結(jié)—i+1為旋轉(zhuǎn)關(guān)節(jié)432025/6/1連桿i+1質(zhì)心C的線加速度迭代公式：相鄰連桿間的角加速度迭代公式：相鄰連桿間的角速度迭代公式相鄰連桿間的線加速度迭代公式：442025/6/1連桿i+1質(zhì)心C的線加速度迭代公式：相鄰連桿間的角加速度遞推公式：相鄰連桿間的線加速度迭代公式相鄰連桿間的角速度迭代公式注意，在不考慮重力時：牛頓-歐拉方程的向外遞推迭代小結(jié)—i+1為移動關(guān)節(jié)452025/6/1根據(jù)每個連桿的角加速度和質(zhì)心線加速度，根據(jù)牛頓-歐拉公式，得：其中：Fi和Ni可理解為慣性力和力矩顯然，每個連桿的慣性力/力矩應(yīng)等于其所受外力之和由右圖可知，每個連桿質(zhì)心處的力平衡方程為：

注意：方程里沒有重力項牛頓-歐拉方程的向內(nèi)遞推迭代462025/6/1每個連桿質(zhì)心處力矩平衡方程為：其中：為連桿i作用在連桿i+1上的力矩的反作用力矩，在{i}坐標(biāo)系中的表達(dá)最后，可得到力矩平衡方程：后兩項為連桿間作用力在質(zhì)心C處耦合的力矩之和，可化簡為：牛頓-歐拉方程的向內(nèi)遞推迭代472025/6/1將前述連桿的力/力矩平衡方程整理如下：把上式寫成由末端到基座的迭代遞推形式，可得連桿i-1作用于連桿i的力/力矩：其中：

iFi和iNi為連桿i質(zhì)心處的慣性力/力矩，可根據(jù)質(zhì)心線角速度和角加速度求得把ifi和ini為向關(guān)節(jié)軸線投影，可得關(guān)節(jié)i的驅(qū)動力/力矩：移動關(guān)節(jié)：旋轉(zhuǎn)關(guān)節(jié)：牛頓-歐拉方程的向內(nèi)遞推迭代482025/6/1連桿i-1作用于連桿i的力/力矩：關(guān)節(jié)i的驅(qū)動力/力矩：移動關(guān)節(jié)：旋轉(zhuǎn)關(guān)節(jié)：連桿i質(zhì)心處的慣性力/力矩：

牛頓-歐拉方程的向內(nèi)遞推迭代-小結(jié)牛頓-歐拉動力學(xué)方程——實例492025/6/1首先確定牛頓-歐拉遞推公式中的連桿參數(shù)和初始值如右圖所示的2R平面機(jī)器人，假設(shè)每個連桿的質(zhì)量都集中在桿件末端，連桿參數(shù)如圖每個連桿坐標(biāo)原點及質(zhì)心的位置矢量：假設(shè)質(zhì)量集中于質(zhì)心，則各連桿質(zhì)心的慣量張量為零矩陣：末端執(zhí)行器不受外力，所以：機(jī)器人基座固定，所以：考慮重力因素，有：X0Y0OX1Y1X2Y2502025/6/1相鄰連桿坐標(biāo)系之間的旋轉(zhuǎn)矩陣如下：用外推迭代求解兩個連桿的速度、加速度以及慣性力/力矩連桿1角速度：連桿1角加速度：X0Y0OX1Y1X2Y2牛頓-歐拉動力學(xué)方程——實例512025/6/1連桿1坐標(biāo)原點線加速度：連桿1質(zhì)心線加速度：牛頓-歐拉動力學(xué)方程——實例522025/6/1連桿1慣性力：連桿1慣性力矩：牛頓-歐拉動力學(xué)方程——實例532025/6/1連桿2角速度：連桿2角加速度：牛頓-歐拉動力學(xué)方程——實例542025/6/1連桿2坐標(biāo)原點線加速度：連桿2質(zhì)心線加速度：牛頓-歐拉動力學(xué)方程——實例552025/6/1連桿2慣性力：連桿2慣性力矩：牛頓-歐拉動力學(xué)方程——實例562025/6/1用內(nèi)推迭代求解連桿間的力/力矩連桿2受連桿1作用力：連桿2末端受環(huán)境作用力牛頓-歐拉動力學(xué)方程——實例572025/6/1用內(nèi)推迭代求解連桿間的力/力矩連桿2受連桿1作用力矩：連桿2關(guān)節(jié)力矩：牛頓-歐拉動力學(xué)方程——實例582025/6/1用內(nèi)推迭代求解連桿間的力/力矩連桿1受連桿0作用力：牛頓-歐拉動力學(xué)方程——實例592025/6/1用內(nèi)推迭代求解連桿間的力/力矩連桿1受連桿0作用力矩：牛頓-歐拉動力學(xué)方程——實例602025/6/1用內(nèi)推迭代求解連桿間的力/力矩連桿1關(guān)節(jié)力矩：牛頓-歐拉動力學(xué)方程——實例機(jī)器人技術(shù)基礎(chǔ)

第3章機(jī)器人力學(xué)

3.4拉格朗日法

第1-15周，星期二，16:40-18:15，（五）103623.4.1一般質(zhì)點系的拉格朗日方程3.4.2兩自由度平面機(jī)械系統(tǒng)的拉格朗日方程3.4.3操作臂的拉格朗日方程本節(jié)目錄拉氏函數(shù)L=K-U自由度廣義坐標(biāo)廣義力廣義速度一般質(zhì)點系的拉格朗日方程以能量觀點來研究機(jī)械系統(tǒng)的真實運(yùn)動規(guī)律；屬于分析力學(xué)范疇；區(qū)別于牛頓矢量力學(xué)；求解步驟規(guī)范、統(tǒng)一（確定廣義坐標(biāo)，列出動能、勢能和廣義力的表達(dá)式，代入上式即可）。有關(guān)拉氏方程的推導(dǎo)步驟，可以選修研究生階段的機(jī)器人動力學(xué)643.4.1一般質(zhì)點系的拉格朗日方程3.4.2兩自由度平面機(jī)械系統(tǒng)的拉格朗日方程3.4.3操作臂的拉格朗日方程本節(jié)目錄兩自由度機(jī)械系統(tǒng)的動力學(xué)分析若機(jī)械臂在垂直平面上，考慮重力若機(jī)械臂在垂直平面上，無重力X0Y0系統(tǒng)動能是各個連桿動能之和式中，第j個連桿的動能Kj可表示為

事實上，操作臂的動能可以寫成一般形式是操作臂慣性矩陣，

n╳n維，二次型，正定的，動能永遠(yuǎn)為正（1）系統(tǒng)的動能兩自由度機(jī)械系統(tǒng)的動力學(xué)分析（2）系統(tǒng)的勢能系統(tǒng)勢能是各個連桿勢能之和

第j個連桿的勢能Pj可表示為式中，

注意：需要為系統(tǒng)定義一個重力勢能參考平面，而不要為每個廣義坐標(biāo)定義一個重力勢能參考平面。兩自由度機(jī)械系統(tǒng)的動力學(xué)分析例：平面2R機(jī)械手的動力學(xué)建模選取2個轉(zhuǎn)角為廣義坐標(biāo)。將兩個桿均看作是位于在各桿末端的集中質(zhì)量（即各桿的質(zhì)心在其末端）。桿1質(zhì)心的位置和速度：桿2質(zhì)心的位置和速度：還有其它廣義坐標(biāo)的選擇嗎？兩自由度機(jī)械系統(tǒng)的動力學(xué)分析例：平面2R機(jī)械手的動力學(xué)建模兩桿的動能項兩桿的勢能項兩自由度機(jī)械系統(tǒng)的動力學(xué)分析例：平面2R機(jī)械手的動力學(xué)建模

兩自由度機(jī)械系統(tǒng)的動力學(xué)分析713.4.1一般質(zhì)點系的拉格朗日方程3.4.2兩自由度平面機(jī)械系統(tǒng)的拉格朗日方程3.4.3操作臂的拉格朗日方程本節(jié)目錄操作臂的拉格朗日動力學(xué)方程（LagrangianDynamics）722025/6/1關(guān)于機(jī)械系統(tǒng)動力學(xué)的拉格朗日函數(shù)其中：

基于拉格朗日函數(shù)的動力學(xué)方程其中：τ

d為僅考慮慣量和重力的關(guān)節(jié)驅(qū)動力矢量732025/6/1機(jī)器人系統(tǒng)動能其中：

構(gòu)件i

質(zhì)心線速度動能構(gòu)件i

角速度動能操作臂的拉格朗日動力學(xué)方程742025/6/1對于構(gòu)件i的線速度和角速度項，根據(jù)速度雅可比矩陣可知：其中：

代入構(gòu)件動能方程，得：Mi(Θ)為構(gòu)件i的廣義質(zhì)量矩陣，是Θ的函數(shù)操作臂的拉格朗日動力學(xué)方程752025/6/1于是，機(jī)器人系統(tǒng)的總動能可表達(dá)為：其中：M(Θ)為系統(tǒng)在關(guān)節(jié)空間中的廣義質(zhì)量矩陣，n×n維，是Θ的函數(shù)我們知道，質(zhì)點的動能表達(dá)式為：可見，機(jī)器人等多剛體機(jī)械系統(tǒng)的動能表達(dá)式，在形式上與質(zhì)點的動能表達(dá)式類似操作臂的拉格朗日動力學(xué)方程762025/6/1機(jī)器人系統(tǒng)勢能

構(gòu)件i重力勢能的構(gòu)造定義一個零勢能參考坐標(biāo)系{ref}勢能增量是重力做功的負(fù)值：其中：0gT=[0,0,-g]T為重力矢量

操作臂的拉格朗日動力學(xué)方程772025/6/1機(jī)器人系統(tǒng)勢能

由于：于是，得：使參考面勢能為零的項urefi其中：

操作臂的拉格朗日動力學(xué)方程782025/6/1完整的機(jī)器人拉格朗日動力學(xué)方程系統(tǒng)動能：系統(tǒng)勢能：

操作臂的拉格朗日動力學(xué)方程792025/6/1例：右圖所示兩自由度及機(jī)器人，關(guān)節(jié)1為旋轉(zhuǎn)關(guān)節(jié)，關(guān)節(jié)2為移動關(guān)節(jié)，l連桿1質(zhì)心與關(guān)節(jié)1軸線距離為l1，連桿2質(zhì)心與關(guān)節(jié)1軸線的距離為變量d2。兩連桿各自的慣性張量為：連桿1動能：連桿2動能：系統(tǒng)總動能：X0Y0X1Y1操作臂的拉格朗日動力學(xué)方程X0Y0X1Y1802025/6/1連桿1勢能：勢能參考點在θ1=－90°位置連桿2勢能：勢能參考點在θ1=－90°、d2=d2max（最大值）位置系統(tǒng)總勢能：根據(jù)拉格朗日方程：得：操作臂的拉格朗日動力學(xué)方程812025/6/1系統(tǒng)總勢能：根據(jù)拉格朗日方程：首先，對θ1項求偏微分：系統(tǒng)總動能：得到τ1的表達(dá)式：慣性力科氏力重力操作臂的拉格朗日動力學(xué)方程822025/6/1系統(tǒng)總勢能：根據(jù)拉格朗日方程：系統(tǒng)總動能：然后，對d2項求偏微分：得到τ2的表達(dá)式：慣性力向心力重力操作臂的拉格朗日動力學(xué)方程832025/6/1把系統(tǒng)動力學(xué)方程