機(jī)械電子學(xué)-動(dòng)力學(xué)仿真-第二節(jié)

1、機(jī)械電子學(xué)機(jī)械電子學(xué)陳陳學(xué)學(xué)超超Email: 2正動(dòng)力學(xué)：正動(dòng)力學(xué)：已知各個(gè)關(guān)節(jié)的作用力或力矩，各個(gè)關(guān)節(jié)的位移、速度，求得關(guān)節(jié)的加速度多剛體系統(tǒng)動(dòng)力學(xué)公式多剛體系統(tǒng)動(dòng)力學(xué)公式逆動(dòng)力學(xué)：逆動(dòng)力學(xué)：已知各個(gè)關(guān)節(jié)的位移、速度、加速度，各個(gè)桿件受到的外力，求各個(gè)關(guān)節(jié)所需要的驅(qū)動(dòng)力或力矩2( )( , )( )TextM q qD q qG qJ f( , , ,)extqfq q f ( , , ,)extf q q q fn牛頓歐拉法牛頓歐拉法n空間矢量法空間矢量法n拉格朗日法拉格朗日法n凱恩（凱恩（KaneKane）法）法n3n牛頓歐拉法牛頓歐拉法n空間矢量法空間矢量法n拉格朗日法拉格朗日法n凱恩

2、（凱恩（KaneKane）法）法n4n牛頓歐拉法牛頓歐拉法n空間矢量法空間矢量法n拉格朗日法拉格朗日法n凱恩（凱恩（KaneKane）法）法n5n牛頓歐拉法牛頓歐拉法l這種方法是最直觀的方法，通過牛頓方程和歐拉方程求解多剛體系統(tǒng)動(dòng)力學(xué)。l由于時(shí)間的限制，只介紹大體的計(jì)算流程，如果有疑問，可以自學(xué)具體的計(jì)算細(xì)節(jié)。6n基本公式基本公式牛頓方程：描述了剛體質(zhì)心的平移運(yùn)動(dòng)歐拉方程：描述了剛體繞質(zhì)心的旋轉(zhuǎn)運(yùn)動(dòng)7ccNIIcFmvcI是當(dāng)坐標(biāo)系原點(diǎn)在質(zhì)心上時(shí)剛體的慣量n向向外迭代外迭代計(jì)算速度和加速度計(jì)算速度和加速度在已知了各個(gè)關(guān)節(jié)的角度、角速度、角加速度后，可以依次向外迭代，計(jì)算出所有剛體質(zhì)心的線加速度

3、，繞質(zhì)心的角速度、角加速度在桿件坐標(biāo)系中的表示8ccNIIcFmvn作用作用在剛體上的力和力矩在剛體上的力和力矩計(jì)算出所有剛體質(zhì)心的線加速度，繞質(zhì)心的角速度、角加速度后，通過牛頓-歐拉公式便可求出作用在剛體上的力和力矩9iicciiiiNIIiicFmvn向內(nèi)迭代向內(nèi)迭代計(jì)算關(guān)節(jié)的力或力矩計(jì)算關(guān)節(jié)的力或力矩在計(jì)算出來所有剛體所受的凈外力和外力矩后，通過向內(nèi)迭代，可以計(jì)算出相鄰剛體間的相互作用力，通過計(jì)算作用力在關(guān)節(jié)軸向的分量，便可求出關(guān)節(jié)力矩或力。10iT iiiineiT iiiife轉(zhuǎn)動(dòng)關(guān)節(jié)移動(dòng)關(guān)節(jié)n牛頓歐拉法牛頓歐拉法n空間矢量法空間矢量法n拉格朗日法拉格朗日法n凱恩（凱恩（KaneKa

4、ne）法）法n11參考文獻(xiàn)：參考文獻(xiàn)：nFeatherstone R.The acceleration vector of a rigid body J. IJRR, 2001, 20: 841nFeatherstone R. A Beginners Guide to 6-D Vectors (Part 1)J. IEEE Robotics & Automation Magazine, 2010, 17(3): 83-94.nFeatherstone R. A Beginners Guide to 6-D Vectors (Part 2)J. IEEE Robotics & A

5、utomation Magazine, 2010, 17(4): 88-99.12n主要分兩個(gè)部分介紹空間向量第一部分：什么是空間向量？它們?nèi)绾喂ぷ?？如何使用它們？第二部分：如何將空間向量的運(yùn)算方式轉(zhuǎn)換成程序，求正逆動(dòng)力學(xué)？13n主要分兩個(gè)部分介紹空間向量第一部分：什么是空間向量？它們?nèi)绾喂ぷ?？如何使用它們？第二部分：如何將空間向量的運(yùn)算方式轉(zhuǎn)換成程序，解正逆動(dòng)力學(xué)？14n既然一個(gè)剛體有6個(gè)自由度，那么為什么不用一個(gè)6維向量來表示它的運(yùn)動(dòng)與作用到它上的力呢？n優(yōu)點(diǎn)：大大減少了代數(shù)運(yùn)算量，只需更少的變量與更少的公式n如果將6維向量想成僅僅是3維向量的簡單疊加，那么就沒有理解其真正含義n6維向量法

6、其實(shí)是一種思考的方法，具有自有的物理含義與數(shù)學(xué)性質(zhì)15n用3維向量解2個(gè)剛體系統(tǒng)的動(dòng)力學(xué)16n用6維向量解2個(gè)剛體系統(tǒng)的動(dòng)力學(xué)17n空間矢量法的兩種變量空間矢量法的兩種變量類型類型一種描述剛體的運(yùn)動(dòng)，表示為一種描述剛體受到的力，表示為變量上面的符號表示此變量為空間矢量，是一個(gè)6維向量，Plcker坐標(biāo)系被用來描述空間矢量186mM6fFPlcker坐標(biāo)系nPlckerPlcker坐標(biāo)系坐標(biāo)系首先需要建立一個(gè)笛卡爾坐標(biāo)系，笛卡爾坐標(biāo)系的位置與方位定義了Plcker坐標(biāo)系，這兩種坐標(biāo)系所描述的向量集合是1:1的映射關(guān)系19n定義定義如下三種單位矢量如下三種單位矢量： 中的元素代表在x，y，z方向上

7、的單位歐幾里德向量，這組單位向量定義了一個(gè)笛卡爾坐標(biāo)系。 中的前三個(gè)元素分別表示繞ox，oy，oz軸的旋轉(zhuǎn)，后三個(gè)元素分別表示沿x，y，z軸的平移。 中的前三個(gè)元素分別表示繞x，y，z軸的力矩，后三個(gè)元素分別表示沿ox，oy，oz方向的力。20Ci, j, koxoyozxyz6Dd ,d ,d ,d ,d ,dMxyzoxoyoz6e ,e ,e ,e ,e ,eFCD設(shè)一個(gè)剛體的運(yùn)動(dòng)在笛卡爾坐標(biāo)系中的表示為那么此運(yùn)動(dòng)在Plcker坐標(biāo)系中表示為：21xyzijkOOxOyOzvvvvijkxOxyOyzOzOxxOyyOzzvvvvdddddd設(shè)一個(gè)剛體受到的作用力在笛卡爾坐標(biāo)系中表示為那

8、么此作用力在Plcker坐標(biāo)系中的表示為：22xyzffffijkOxOyOznnnOnijkOxxOyyOzzxOxyOyzOznnnffffeeeeee如果采用坐標(biāo)向量的方式表示，那么運(yùn)動(dòng)與力的空間向量表示簡化為：或者為：23xyzOxOyOzvvvvOxOyOzxyznnnffffovvonff這種簡單的表示方式非常方便，也比較常用，但是缺點(diǎn)是看上去好像是三維向量的簡單疊加，容易使人誤解為空間向量是三維向量的簡單疊加，但是這種疊加其實(shí)只是書寫形式的疊加，空間向量有其自己的物理含義。nPlckerPlcker坐標(biāo)變換坐標(biāo)變換在進(jìn)行動(dòng)力學(xué)計(jì)算時(shí)，往往需要將空間向量在不同的坐標(biāo)系之間進(jìn)行轉(zhuǎn)換。

9、假設(shè)有兩個(gè)坐標(biāo)系A(chǔ)和B，E是從坐標(biāo)系A(chǔ)到坐標(biāo)系B的旋轉(zhuǎn)矩陣，r是坐標(biāo)系B的原點(diǎn)在坐標(biāo)系A(chǔ)中的位置。24nPlckerPlcker坐標(biāo)變換坐標(biāo)變換設(shè) ， ， ， 為空間向量，分別表示在A和B坐標(biāo)系中的運(yùn)動(dòng)、受力，那么轉(zhuǎn)換規(guī)則如下：其中 是將運(yùn)動(dòng)從坐標(biāo)系A(chǔ)轉(zhuǎn)到坐標(biāo)系B的轉(zhuǎn)換矩陣， 是將力從坐標(biāo)系A(chǔ)轉(zhuǎn)到坐標(biāo)系B的轉(zhuǎn)換矩陣，它們二者之間的關(guān)系為25AmBmAfBfBBAAmXm*BBAAfXfBAX*BAX*()BBTAAXXnPlckerPlcker坐標(biāo)變換坐標(biāo)變換 和 僅取決于坐標(biāo)系B相對于坐標(biāo)系A(chǔ)的相對位置其中：E是坐標(biāo)系B在A中的姿態(tài)矩陣，r是坐標(biāo)系B的原點(diǎn)在A中的位置。另外， 是斜對稱矩陣，

10、26BAE010X0E-r1*()BBTAAE01-rXX0E01000 xzyyzxzyxrrrrrrrrr rBAX*BAXrn空間矢量的微分空間矢量的微分當(dāng)對一個(gè)在移動(dòng)的Plcker坐標(biāo)系中定義的矢量進(jìn)行微分時(shí)，有如下的式子：A既是Plcker坐標(biāo)系的名字，也是定義這個(gè)坐標(biāo)系的坐標(biāo)框架的名字， 是坐標(biāo)框架A的速度在坐標(biāo)系A(chǔ)中的表示27AAv()AAAAAdmdmvmdtdt*()AAAAAdfdfvfdtdtn空間矢量的微分空間矢量的微分上面兩式定義了兩種運(yùn)算符 和 定義如下28*()AAAAAdmdmvmdtdt*()AAAAAdfdfvfdtdt0oovvv *0oTovvvv n空

11、間矢量的微分空間矢量的微分一種比較特殊的情況（也是用得比較多的情況），即運(yùn)動(dòng)與力都固定在某個(gè)坐標(biāo)系上，它們的改變僅僅是因?yàn)樽鴺?biāo)系本身的運(yùn)動(dòng)，那么V是坐標(biāo)系的速度。29mvm *fvf n空間矢量的加速度空間矢量的加速度歐式空間的速度定義為剛體上一個(gè)固定點(diǎn)O的速度，即 ，加速度定義為剛體上固定點(diǎn)O的加速度，即 。 空間矢量法定義的速度為在空間中一個(gè)固定點(diǎn)測量的剛體的速度，加速度為空間速度的變化。30oddavvrrdtdtr r 空間速度的參考點(diǎn)在剛體上不是固定的，而是一些列變化的點(diǎn)，這是線加速度多出一項(xiàng)的原因n空間矢量的加速度空間矢量的加速度與傳統(tǒng)的加速度相比，空間加速度確實(shí)比較難以理解，但是

12、使用起來非常方便。假如兩個(gè)剛體B1和B2由一個(gè)關(guān)節(jié)連接，那么它們的速度有如下關(guān)系式加速度有如下關(guān)系式s為關(guān)節(jié)的轉(zhuǎn)軸方向矢量，q為關(guān)節(jié)角度3121vvsq 21aasqsqn剛體的空間慣量剛體的空間慣量空間慣量將速度 轉(zhuǎn)成動(dòng)量 空間慣量為如果有N個(gè)剛體被固連在一起，那么有32hIv vhTcTmmImm Ic ccc11NtotiiIIn剛體的空間慣量剛體的空間慣量空間慣量是與10個(gè)量相關(guān)，質(zhì)量1個(gè)，質(zhì)心位置3個(gè)，剛體慣量6個(gè)。注意：傳統(tǒng)的慣量與位置無關(guān)，而空間慣量與質(zhì)心位置相關(guān)。33TcTmmImm Ic ccc1n剛體的空間慣量剛體的空間慣量空間慣量遵循的運(yùn)算法則：另外，剛體的機(jī)械能為34*

13、BBAAABIXI X*dIvIIvdt 12Ev Ivn運(yùn)動(dòng)方程運(yùn)動(dòng)方程一個(gè)速度為v，慣性為I的剛體的運(yùn)動(dòng)方程為： f 是作用在剛體上的合外力， a 是由于外力產(chǎn)生的加速度。35*()dfIvIavIvdt n運(yùn)動(dòng)約束運(yùn)動(dòng)約束相對速度：相對加速度：3621vvSq21aaSqSqn空間矢量運(yùn)算法則總結(jié)空間矢量運(yùn)算法則總結(jié)相對速度：如果剛體 與 的速度分別為 與 ，那么 相對于 的速度為合力：如果 和 作用于同樣的剛體上，那么它們等效于一個(gè)合力 作用力與反作用力：如果剛體 施加了力 到剛體 上，那么 施加一個(gè)力 到 上，這是牛頓第三定律的空間矢量形式3721relvvv1B2B1v2v1B2B

14、1f2ftotf12totfffff1B2B2B1Bn空間矢量運(yùn)算法則總結(jié)空間矢量運(yùn)算法則總結(jié)數(shù)量積：如果力 作用于速度為 的剛體上，那么該力所產(chǎn)生的功率為微分：運(yùn)動(dòng)的微分還是運(yùn)動(dòng)，力的微分還是力。如果 和 固定在速度為 的剛體上，那么 ，加速度：空間加速度是空間速度的微分。如果 ，那么合慣量：如果剛體 和 的慣量分別為 和 ，當(dāng)這兩個(gè)剛體固連后組成的剛體的慣量為38f v fvmfvmv m *fvf 21relvvv21relaaa1B2B1I2I12totIIIn空間矢量運(yùn)算法則總結(jié)空間矢量運(yùn)算法則總結(jié)動(dòng)量：速度為 ，慣量為 的剛體的動(dòng)量為運(yùn)動(dòng)方程：作用于剛體上的合力為剛體動(dòng)量的微分39

15、IvIv*()d IvfIavIvdt n主要分兩個(gè)部分介紹空間向量第一部分：什么是空間向量？它們?nèi)绾喂ぷ?？如何使用它們？第二部分：如何將空間向量的運(yùn)算方式轉(zhuǎn)換成程序，解正動(dòng)力學(xué)與逆動(dòng)力學(xué)？40.au/roy/spatial/n空間矢量法的應(yīng)用例子空間矢量法的應(yīng)用例子逆動(dòng)力學(xué)是計(jì)算給定加速度所需作用力的問題，是一個(gè)相對比較容易的問題，因此從這問題出發(fā)，理解空間矢量法在多剛體動(dòng)力學(xué)求解中的應(yīng)用41(mod, , , )IDel q q q n右側(cè)為求解逆動(dòng)力學(xué)的Matlab代碼，采用了牛頓-歐拉遞歸算法n可以看出，程序非常簡明，這是3維向量難以實(shí)現(xiàn)

16、的42n模型數(shù)據(jù)結(jié)構(gòu)模型數(shù)據(jù)結(jié)構(gòu)model.NB：指定桿件的個(gè)數(shù)model.parent：描述桿件之間的連接關(guān)系，指定當(dāng)前的連桿連接到的母連桿的編號model.Xtree：描述相鄰桿件之間的相對位置關(guān)系，與坐標(biāo)系的建立、桿件的初始位置、桿件的幾何形狀相關(guān)model.pitch：表示關(guān)節(jié)的運(yùn)動(dòng)類型，如移動(dòng)、轉(zhuǎn)動(dòng)等model.m：桿件的質(zhì)量model.c：桿件質(zhì)心的位置model.I：桿件繞其質(zhì)心的慣性張量矩陣，是一個(gè)3x3的對稱矩陣43n連接（連接（ model.parent ）連接圖描述了所有桿件間的連接關(guān)系。一個(gè)固定基座的多剛體系統(tǒng)采用如下幾步處理：固定基座編號為0，作為樹形結(jié)構(gòu)的根剩余的剛

17、體從1到N連續(xù)編號，每個(gè)剛體的編號大于其母節(jié)點(diǎn)關(guān)節(jié)從1到N進(jìn)行編號，第i個(gè)關(guān)節(jié)將第i個(gè)剛體連接到其母節(jié)點(diǎn)44n連接（連接（ model.parent ）當(dāng)給剛體和關(guān)節(jié)編號完成后，便可以通過一個(gè)矩陣 描述剛體的連接， 為第i個(gè)剛體的母節(jié)點(diǎn)的編號，這個(gè)矩陣有如下性質(zhì)：注意：無論是編號還是母節(jié)點(diǎn)矩陣都不是唯一的45( ) i0( ) iin幾何關(guān)系（幾何關(guān)系（ model.Xtree ）模型的幾何關(guān)系描述了每個(gè)關(guān)節(jié)在其剛體上的相對位置。為每個(gè)關(guān)節(jié)引入一對坐標(biāo)系，分別固連與被這個(gè)關(guān)節(jié)連接的兩個(gè)剛體，比如 和46( ),i iFiFn幾何關(guān)系幾何關(guān)系 的求取根據(jù)關(guān)節(jié)類型不同而不同，有純旋轉(zhuǎn)關(guān)節(jié)、純平移關(guān)

18、節(jié)、螺旋關(guān)節(jié)47( )JXin幾何關(guān)系幾何關(guān)系（ model.Xtree ） 是從坐標(biāo)系 到坐標(biāo)系 的轉(zhuǎn)換矩陣，其實(shí)是定義了兩個(gè)關(guān)節(jié)間的相對位置，這個(gè)量存儲于 中，即從坐標(biāo)系 到 的轉(zhuǎn)換矩陣為48( )TXi( ) iF( ),i iFmod.el Xtree imod.( )Tel Xtree iXiiF( ) iF( )iiX( )( )( )iiJTXXi Xi( )( ( )0TXixlt r iIrI n關(guān)節(jié)模型（關(guān)節(jié)模型（ model.pitch ）如果 和 是經(jīng)過關(guān)節(jié)j的速度與力，那么關(guān)節(jié)的數(shù)學(xué)模型由兩個(gè)量組成，轉(zhuǎn)換矩陣 與運(yùn)動(dòng)子空間矩陣S，S與關(guān)節(jié)軸的方向相關(guān)能量平衡方程49(

19、 )JiiivvvJivJifJXJiiiTiiJivS qS fiiJiJiqfvn運(yùn)算法則運(yùn)算法則50( )iiiivvs q( )iiiiiiaas qvq*BiiiiiifI avI v( )JiBiJjjifffTiiJis fiBif 是作用在剛體 上的凈力Jiif 是經(jīng)過關(guān)節(jié) 的作用力( )ii是剛體 的子連桿上個(gè)關(guān)節(jié)的作用力+下個(gè)關(guān)節(jié)的反作用力=剛體的凈力n雅克雅克比矩陣比矩陣第一種形式：第二種形式：51bbvJ q( )biii k bvs q( )brootk b 是剛體 到之間的所有剛體集合12J.eeNsss自學(xué)雅克比矩陣的求解程序自學(xué)雅克比矩陣的求解程序bbbaJ q

20、J q自學(xué)加速度中分量自學(xué)加速度中分量 的求解的求解過程過程bJ q52n二二連桿模型連桿模型總共有兩個(gè)剛體，model.NBmodel.NB=2=2固定坐標(biāo)系編號為0，第一個(gè)桿件編號為1，第二個(gè)桿件編號為2，model.parentmodel.parent = = 0 0 11 model.pitchmodel.pitch = = 1 1,1 1,1表示繞x軸旋轉(zhuǎn)53n二二連桿模型連桿模型 model.Xtreei100000010000001000model.1000100000010000001Xtree100000010000001000model.201010010001000000

21、1XtreeLL54n二二連桿模型連桿模型 model.m=1.0 1.0 質(zhì)心坐標(biāo)0model.101 / 2cL0model.202 / 2cL55n二二連桿模型連桿模型 慣量（對稱正定矩陣）10.0010.002model.10.0010.010.0030.0020.0030.01I10.0010.002model.20.0010.010.0030.0020.0030.01I56n二二連桿模型連桿模型利用現(xiàn)有程序?qū)Υ四Ｐ颓蠼庹齽?dòng)力學(xué)與逆動(dòng)力學(xué)Dynamics_test.mn牛頓-歐拉法與空間向量法均屬于動(dòng)態(tài)平衡法，需要求解加速度與力的關(guān)系。拉格朗日法是功能平衡法，只需求解速度，而不必求解

22、內(nèi)力57n拉格朗日算子n拉格朗日方程（廣義坐標(biāo)系）58( , )( , )( )L q qT q qV q動(dòng)能勢能dLLdtqq外力n拉格朗日方程推導(dǎo)演示笛卡爾坐標(biāo)系中的拉格朗日方程推導(dǎo)自學(xué)廣義坐標(biāo)系中的拉格朗日方程推導(dǎo)59在一個(gè)保守力場中有N個(gè)質(zhì)點(diǎn)，第i個(gè)質(zhì)點(diǎn)的坐標(biāo)為 ，這個(gè)系統(tǒng)的動(dòng)能為其中M是這個(gè)系統(tǒng)的自由度個(gè)數(shù)。當(dāng)質(zhì)點(diǎn)只在1維運(yùn)動(dòng)時(shí)，M=N。當(dāng)質(zhì)點(diǎn)在3維運(yùn)動(dòng)時(shí)，M=3N。60ix2112MiinTm x動(dòng)能對速度的微分是質(zhì)點(diǎn)的動(dòng)量，即將動(dòng)量對時(shí)間微分為61 iiTpxiiidpdTmxdtdtx在保守力場中，作用于質(zhì)點(diǎn)的力是勢能對位置的微分，即根據(jù)牛頓定律可得62 iiVFxiiidpF

23、dt ()iiidTVdtxxi 為作用于質(zhì)點(diǎn)的外力根據(jù)動(dòng)能與勢能公式可得根據(jù)以上的結(jié)果可得630iTx 0iVx動(dòng)能只與速度相關(guān)勢能只與位置相關(guān)()()()iiidTVTVdtxx定義 ， L命名為拉格朗日算子，拉格朗日方程可寫為64()iiidLLdtxxLTVn二連桿模型首先求出質(zhì)點(diǎn)的位置65111 111 10 xyrszrc221 121221 12 120 xyrsr szrcrcn二連桿模型再求出質(zhì)點(diǎn)的速度66111 1 111 1 10 xyrczrs221 1 12 121221 1 1212120()()xyrcrczrsr sn二連桿模型設(shè)桿件的慣量為671111000000 xyzIIII2222000000 xyzIIIIn二連桿模型系統(tǒng)動(dòng)能682221111122222221211()2211()()22zzTm y