《多刚体系统运动学与动力学.doc》由会员分享，可在线阅读，更多相关《多刚体系统运动学与动力学.doc（39页珍藏版）》请在三一文库上搜索。
1、计算多体系统运动学与动力学(Simplified)Computational Kinematics and Dynamics of Multi-body Systemsforengineer forengineer 1旋转变换(7)(#)1POoXQyoZop =+ yJo + z。(i)OXiyZi卩二“人十儿齐十©冏(2)兀 4 + yJo + yJi + z 禹(3)4兀= % + yj +Jo = Jo + y J Jo + z此 Jo(5)Z。= x,i, k0 + yjx 矶 + z禹 k()1 *0 J * 0 耐 *0厂31(#)pOoXoyoZo Oxxxyxzx此
2、=ko儿sPl = k Ji Z JP)=尺比R：=R：R： R冷 R：)R；R；2欧拉角坐标和卡尔丹角坐标(10)pOoXoyoZo Oxxxyxzx此=ko儿s(#)pOoXoyoZo Oxxxyxzx此=ko儿sA"=qS"-耳 qo'0AG100Q0 '-SgA"=s.一 s.C.o'00010s»G 001.(#)A” 二 SQ - C,CcS. - SA - qg - Cd3a p y(H).一 s，o G 丿o o i(12)gA* = S.SQ 4- CS-C.SC 十 ES,-GSy-S.Sy + C.C,c,s
3、6sr + s,crS.CpCaC, J(13)3齐次坐标n+1nOxvz(X y z)T(14)（召,X2 , X3，X4 ）7伙比，g ,心3 ,也-）卩kOxyz0 1 0 0 0 r 0 0 1 0 r04平移齐次变换0 0Oaf aX y z1(15)1 00 ofO1XJ1Z1a b c T4POoXoyoZoXi J| Z1J TO1X1V1Z1ox0 yo1o直oyozo(16)(13)(17)X。10 0 a>0=TJl0 10b0石0 0 1c110 0 0 1110 0 aOi0 10b f Ta b c OXyZ0 0 1c0 0 0 1OoXoyoZo5旋转齐次
4、变换(18)山 yiZi 1OtXiyiZi(8)OiMNZiOixiyiziOoXoyoZoJoZo=o-00V1=K|X4 石10 0 01一J.1(19)0001(20)OXoyoZo x y z(24)4 44 4(#)4 44 4R(x30) =R(y.O)=R(z.o)=10000cos。sin。0cos 80-sin。00100COS&sin&000一 sin 0COS。0sin 80cos。000010001(21)(22)一 sin 6cos。0000COS&0(23)(#)4 44 46平移旋转齐次变换(25)4 44 4(#)4 44 4OoXoy
5、oZoXocyoZo(13)(15)XoZoab100ao'-a010broK1b-4x4 =“03x3"（）A3001c0c000I00 0 10 0 01SiyfiOoDMoOiXJiZiJo(#)4 44 4(26)A2=labc&12=0 o OJ r(#)(#)7较的相对运动学(#)(#)7(#)(#)(1)(#)(27)%8100o G - Si-0 s；G(26)(#)S = sin C, = cos®0GSj0(27)0 0-5, 0C, 00 1(30)3(#)3Q 929 001C2 0Ai = 10 c,=0100 stc,厂Sq0cj
6、(28)(29)(31)3(#)3C20So0祸G-sg0Sic©00001A92 q?(#)3S、S rC、+ c、s、 CS -,C y + S、S A* =10q化-c»- S'S： +CjC3CjS'S + SCg05.-S©c©00001(31)(32)<7i111 0 0(33)513513513510f0c.=0s,00o h-S2 00£0 1(34)7多体系统坐标系的建立13513513a28多体系统正向运动学r2 = a.a2Ann耳=力/沁H-lT = A.A2 - An(35)(36)(37)9多体
7、系统逆向运动学斤2斤3 aTn =r22r九2 b心 c000 1n1210多体系统雅可比矩阵(Jacobian)<r = k q?qP= Px Pi Py 久 Ps PeYp = 0(q)(38)(39)dp 60 dq=dt dq dt=Jq(40)JliZun J J J 9 9 2 2 L A J JLI如 J J -一-JZn(4£1两勿Vv= JLXqx+JL2q2+-JlJqn=v.+v2+ +儿=工/ /=JAlq+JA2q2+ +/皿<7” =<>i +化 + + qE乙>1(43)(44)Ju J.Ai(I)(33)(45)0&
8、;t = R：R 01(46)0=5 = /如 G = °(47)Az = KtriA.e。庶丿忆“On(9=M =Ja(j =k iqOiXj.yi.jZiAOj.(48)(49)JU S =叫 X m =(&T q) X m = (Et X G %(50)(51)11多体系统动力学基础1)Newton-Euler2) Lcigrange3) Hamilton(Appel)(1)(52)(53)(54)（19），/2A5- = 4（19），/2A5- = 4（09）"冲- =d(6£)（89）(IS)Jo 讦 Y X8) = S讪b黑严tlQ$ M i-
9、y iri/ 7i»?詈ng小“xt/ !u Iu岬Juip!2 Jinp'A Jtup'x岬?2 Ju/p纠u/p-2fX jiupf2!xjuip'i juip'21 j“呼Jlup''xuip-xjtup'2'x Iuip!X'x I±IQ IQ / /（90以咼曲以）舊'XZ = uip©)JI =丿Z 啊if,仁="卩J =俛如号号">55mp(bo z%上1L=K V(62)1 n r rV工工mL f-l J-l *-ld dL dL _石範一苑
10、(63)(63)(64)dqr 2 冋 m#<)-#知。(67)d dL * j；= EE ai dqp F=±idqp f w兀7汶:(64)(65)(66)(67)4k CLnQk %+工工mi=p A=1 /n=l(68)(69)Dij(69)V工工厶 ip /! !0肌 恥）. n的你+工iP(63)(68)(70)d dL dLep(70)怜禹;驾p寸人n t i +工工 i=p R=1 m=ld2T, dT：®eqn ' gp (70)(71)(72)Pj=i 4-1j mn j Jji加1 (h些l觀 % )qa -工叫gji(71)(72)匚一
11、工Q乞十工工% qk %十0*-14«1 m«lD =Zr 些冃Dqk = Xtrjmd Tj I,J期5如、0 = £ 叫 g戸kDi(73)(74)(75)(76)12多体系统运动学与动力学计算实例（机器人）15Lagranged dLrdidbeL Lagrange L = T -V(16)D（e）e + h（o.0）+ g（3）= tD（e） 55H（e.e） G（e） 5D（e）eH（e.e）(16)»55 =加1(。1 一厶)'+ 5»54 = »45 = 055 + mla2(a 一，5)。5G（e）D54 =
12、D45 = D55 + mia2（5 一 Z5）c5D53 = 65 = D54»52 = °25 = »53 + mla2（al h）c345D51 = D5 = D52 + m1a1（al 一 /5 ）巾345D44 = D45 + 加 2（“2 M） + 加 1（°2 +。2（。1 ，5）。5）+ 4»43 =。44»42 = »43 + 加2“2（。2 一，4）°34 +加1（。*34 +。2（5 一，5）°345）D41 = D42 + 加2“1（“2 - Z4）c234 + 加I（«
13、I«2C234 + "1（6 一 l5）c2345）63 =。也 + m33 + hD32 = o23 = D33 -m3a2lyS3 +m2a2（a2 -/4）c34 +/,（6/34 + “2（“i "5）c345）»31 = »13 = »32 m3a3s23 + m2al（a2 一4）234 + nh（ala2c234 + a（a 一 ？5）°2345）D22 = D2J + m2（d2-l2）2 + I2 + m3（a； -a2l3s3） + m2a + a2（a2 一/4）c34） + ml（a； + ac +D
14、2l = Dl2 = D22 + m2a1 （a2 - l2）c2 + m,axa2c2 - al3s13） + m2（aia2c2 + a】（a2 - /4）c2M） +“ （aa2c2 + aia2c234 + ax （ax 一 /5 ）c5）Dn = DI2 + “（-/ J? + 人 + 加2（a； + ad© -/,）+ m5（a; + axa2c2 -m2（a;+axa2c2+ aA（a2 - /4）c234） + ml（a; + aa2c2 + aa2c2i4 + ai（al - / Jc冲Jh = W| （5 （fl| ?5 ）$2345©l J + a2（
15、"1 5 ”345（©1 + ©2）' + a （a ，5 ”5（fi + ©2 + ©3 + °4）°）/l4 =/H2（6Fi（Oi -/4）523412 +。2（。24”34（°1 + %）?） + “ （巾切診/ + al（al 一，5”2345°/ + ais34+ ©2）° + “2（"1 5 ”345（°1 + &）°）力3 = ftl3（al3c23C + °2厶。3（01 + ©2 广）+ 加2（。1（
16、。2 -）52341" +。2（。2 4 ”34（©1 + ©2 广）+ ml（aa2s234 +。2 $34（°1 + °2）' + a2（al ，5）$345（°1 + ©2）'）/?2 =加2（。1（"? 2 ”2&厂 + ”3（"1。2$2&厂 +。1，3。23&1 23*3）（&1 + &2 + &3）&3 ”2,3。3（&1 + &2）&） + /Zb （|221 （。2 一 4）$234&
17、 - 他（。2 一 4）$34（°1 + °2）（& + &）一（他 一 4 ）$34（°1 + 占2 +°4）（°3 +&）.+叫（叱2$2船 +?1叱234并+。1（® "5）$2345衬二。2 妆（易 +©2）（3a2（a -5 “545（°1 + $ + &3 + °4 + 色）（°3 + °4 + 色）一 “2（。1 一 5）$5（°1 + & + 色）°5）/l| = -m2 （«! （a2 -
18、 12 ）s2+&2）&2 + a（a2 - h）S22） ,n3（ai°2S2（1 + &2）&2 + 归&厲 +叩3。231 +N +玄）（玄 +玄）+叩3%（久 +玄）+4从3（以 +久 +玄）玄 +6/36© + N ）4 ） -m1（aia2s20l02 +axa1s1（O + 02）02 + 务（勺一厶”冲玄（©2 + d + d）+°1（°2 一仃”纠© +4 + d + d）（d + & + d）+。2（“2 - 仃）*234（"1 + 4 + d + 4）（4
19、 + 2）- "1 （ai°2S2l + 2）d + 6。2$2°1（& + 久）+糾勺$234&（久 +©3 + 04）+ 佈。尹234（& + 久 + 4 + 4 ）（4 + 4 + 4）+ 再（的- 5 ）23451（2 + d + d + 4）+。】（"1 - 4 ”2345+ 3 + 4 + 4）（d + d + 2 + 5）+ d2S34（l +。2）3 + &）+。詁34 （A + ©2 + d + 4）（d + d ） + （6 一 4）$345 G + 2 ）3 + Q 4 ） +。2
20、（6 - /5）5M5（! +©2 + ©3 +4 + °5）3 +©4 +5）+ °2（°1 5）S5（1 + + 3 + 4）4 +。2（ 一 4）$5 G + ©2 + "3 + 4 + ©5 ）5）g5 =加|g（"l -/5）。12345g4 = g5 + 加2g（°2 一 ？4）C234 + 加/°2523403 = &4 一加3g*l23g2 = g3 + 加2g（°2 - l2）“2 + （加3 十叫 + /wl）2C12g【=g2 +- h
21、 ）q + （加2 + M + 加1）gg|13多体系统运动运动学举例（机器人，用kfatlab编程）0.80.70.60.50.40.30.20.10-0.1-0.8-0.6-0.4-0.200.20.40.60.8木平方向（m）1715019150.3O.4O.0.80.60.40.20-0.220150#1500.20.40.6-0.4 1111L-0 8-0.6-0.4-0.20行进方向(m)#502151O0.50.4啦Bt220O.500050005000500050- 11223344 E乏)塞(E)叵代网劉2O-21#21#04111111-0.8-0.6-0.4-0.200.20.40.6水平方向(m)21-100'200o0.20.30.40.5时间0.60.70.80.9400300200100022