多刚体系统运动学与动力学

(Simplified)[e-mail]@126.com111PO0x0y0z0000000kjiPzyx++=(1)O1x1y1z1111111kjiPzyx++=(2)000000kjizyx++=111111kjizyx++(3)(3)0i=0x011011011ikijii⋅+⋅+⋅zyx(4)=0j011011011jkjjji⋅+⋅+⋅zyx(5)=0z011011011kkkjki⋅+⋅+⋅zyx(6)=⋅⋅⋅⋅⋅⋅⋅⋅⋅=33323123222113121101010101010101010110rrrrrrrrrkkkjkijkjjjiikijiiR(7)PO0x0y0z0O1x1y1z1[]Tzyx0000=P[]Tzyx1111=P1100PRP=(8)ijRij211020RRR=32211030RRRR=nnn121100−⋅⋅⋅=RRRR(9)21beb3eb2eb3eϕθφ2ϕθφ(10)(11)2beb1eb2eb3eαβγ3αβγ(12)(13)3n+1nPOxyz(xyz)TTxxxx),,,(432141xxx=42xxy=43xxz=(14)x4Txxxx),,,(4321Tkxkxkxkx),,,(4321kTxxxx),,,(4321Txxxx),,,(87658541xxxx=,8642xxxx=,8743xxxx=(15)Oxyz[000a]Ta[1000]T[01000]T[0010]Txyz01144PO0x0y0z0[x0y0z0]T,O1x1y1z1[x1y1z1]TO1x1y1z1O1O0x0y0z0[abc]T+++=11111000czbyaxzyx(16)(12)==1100010001000111111111000zyxcbazyxzyxT(17)T=1000100010001cbaT[abc1]TO1x1y1z1O1O0x0y0z0[x1y1z1]T=−110001111zyxzyxT(18)55PO1x1y1z1[x1y1z11]TO1x1y1z1O0x0y0z0PO1x1y1z1(8)==××1110000001111441113310000zyxzyxzyxRR(19)R44=××1000000331044RR(20)O1x1y1z1O0x0y0z0xyz−=10000cossin00sincos00001),(θθθθθxR(21)−=10000cos0sin00100sin0cos),(θθθθθyR(22)−=10000cos0000cossin00sincos),(θθθθθθzR(23)6-6-O1x1y1z1O0x0y0z0O1x1y1z1O0x0y0z0x0y0z0(15)x0y0z0abc(13)==×TRA441000100010001cba×10000003310R=×10003310cbaR(24)A44A44=1211211AAAA(25)A11=01RA12=[abc]TA12=[000]T77(1)q18(26)11sinqS=11cosqC=−=1000000000011111CSSCAh(27)2q1q29,(28)(29)−−=10000000211212112122CCSSCCSCSSSCAh(30)3q1q2q310q1q2q3(31)++−−+−+−=100000021313213132121313213132123232CCCSSSCSSCSCCSCCSSSSCCSSSSCCCAh(32)4q111=100001000010001hAh(33)5q1q212−=100000000012222CSSChAh(34)70nnii138A1A2212AAT=(35)A33213AAAT=(36)Annn-1nnAAAT⋅⋅⋅=21(37)9nncrrrbrrrarrrAAATL213332313222211312111000==(38)n121−iA10(Jacobian)[]Tnqqq⋅⋅⋅=21qp[]Tpppppp654321=q)(qpφ=(39)qJqqp&=∂∂⋅∂∂=tdtdφ(40)∂∂∂∂∂∂∂∂=∂∂=nnqpqpqpqp616111LMOMLqJφTnqqq=⋅⋅⋅⋅L21qJ==AnAALnLLJJJJJJJJJLL212121(41)JLiJAii∂∂∂∂∂∂=iiiLiqpqpqp321J∂∂∂∂∂∂=iiiAiqpqpqp654J(42)JLiv∑=⋅⋅⋅=+++=+++=niiLinnLnLLqqqq1.212211JvvvJJJvLL(43)JAi∑=⋅⋅⋅=+++=+++=niiAinnAnAAqqqq1.212211JωωωJJJωLL(44)JLiJAii(1)iqii(33).1iiiLiiqq−⋅===kJvv(45)1−=iLikJ1−ikzi-11−iz=−−−1001221101iiiRRRkL(46)0===⋅iAiiqJωω(47)AiJ=0(2)iqiiiiiAiiqq.1−⋅===kJωω(48)1−=iAikJri-1,eOi-1xi-1yi-1zi-1Oi-1OnxnynznOniLieiiiq.,1Jrωv=×=−(49)ieiieiieiiiLiqqq.,11,1.1,1.)()(−−−−−×=×=×=rkrkrωJ(50)eiiLi,11−−×=rkJ(51)111)Newton-Euler2)Lagrange3)Hamilton(Appel)n(1)iriiirTr=(52)iijjjjqqdtdrTrr∂∂==∑=1.&(53)..rrr⋅=2dtd(54))(2Ttrdtdrrr&&=∂∂∂∂=⋅∂∂⋅∂∂=∑∑∑∑====ijkikjkTiTiijiTiikkkiiijjjiqqqqtrqqqqtr1.1.1.1.TrrTrTrT(55)iridmdmqqqqtrdkijkikjkTiTiijii∂∂∂∂=∑∑==1.1.21TrrT()∂∂∂∂=∑∑==ijkikjkTiTiijiqqqdmqtr1.1.21TrrT(56)i∫=linkiiidkK()∂∂∂∂=∑∑∫==ijkikjkTilinkiTiijiqqqdmqtr1.1.21TrrT∂∂∂∂=∑∑==ijkikjkTiijiqqqqtr1.1.21TIT(57)Ii==∫∫∫∫∫∫∫∫∫∫∫∫∫∫∫∫∫dmdmzdmydmxdmzdmzdmzydmzxdmydmzydmydmyxdmxdmzxdmyxdmxdmiiiiiiiiiiiiiiiiiiiiilinkiTiii222rrI(58)n∑∑∑∑====∂∂∂∂==niijkikjkTiijiniiqqqqtrKK11.1.121TIT(59)(2)giriiiiiTiimPrTg−=(60)Tzyxggg)0(=g∑=−=niiiiTimV1rTg(61)(3)L=KV∑∑∑===∂∂∂∂=niijkikjkTiijiqqqqtrL11.1.21TIT∑=+niiiTim1rTg(62)-iiiqLqLdtdτ=∂∂−∂∂&(63)(63)∑∑∑∑====∂∂∂∂+∂∂∂∂=∂∂niijjpTiijiniikkkTiipipqqqtrqqqtrqL11.11..2121TITTIT(64)∂∂∂∂=∂∂∂∂=∂∂∂∂pTiikiTkTiipikTiipiqqtrqqtrqqtrTITTITTIT(65)(64)jk∑∑==∂∂∂∂=∂∂niikkkTiipipqqqtrqL11..TIT(66)pi0=∂∂piqT∑∑==∂∂∂∂=∂∂npiikkkTiipipqqqtrqL1..TIT(67)(67)∑∑==∂∂∂∂=∂∂npiikkkTiipipqqqtrqLdtd1...TIT.11.2mnpiikimkpTiimkiqqqqqtr∑∑∑===∂∂∂∂∂+TIT.11.2mnpiikimkkTiimpiqqqqqtr∑∑∑===∂∂∂∂∂+TIT(68).11.221knpiijikjkTiipiipqqqqqtrqL∑∑∑===∂∂∂∂∂=∂∂TITiinpipiTiknpiijikjiTiipkiqmqqqqqtrrTgTIT∑∑∑∑====∂∂+∂∂∂∂∂+.11.221(69)(69)jkiinpipiTiknpiijikjkTiipiipqmqqqqqtrqLrTgTIT∑∑∑∑====∂∂+∂∂∂∂∂=∂∂.11.2(70)(63)(68)(70)(70)jm=∂∂−∂∂ppqLqLdtd&∑∑==∂∂∂∂npiikkpTiikiqqqtr1&&TIT.11.2mnpiikimkpTiimkiqqqqqtr∑∑∑===∂∂∂∂∂+TITiinpipiTiqmrTg∑=∂∂−(71)(71)piij=iτ∑∑==∂∂∂∂nijjkkiTjjkjqqqtr1..TIT.11.2mnijjkjmkiTjjmkjqqqqqtr∑∑∑===∂∂∂∂∂+TITjjnijijTjqmrTg∑=∂∂−(72)(72)∑∑∑===++=jkjmimkijkjkkijiDqqDqD11..1..τ(73)∑=∂∂∂∂=nijiTjjkjijqqtrDTIT(74)∑=∂∂∂∂∂=nijiTjjmkjijkqqqtrDTIT2(75)jjnijijTjiqmDrTg∑=∂∂−=(76)DijijDijkjkiDii121415LagrangeiiiLLdtdτθθ=∂∂−∂∂&LLagrangeVTL−=τθGθθHθθD=++)(),()(&&&(16))(θD55),(θθH&)(θG5),()(θθHθθD&&&)(θGτ(16)551215545545251155)()(claamDDDIlamD−+==+−=55121554554)(claamDDD−+==543553DDD==3455121532552)(claamDDD−+==23455111521551)(claamDDD−+==4551222124224544)))(()(IclaaamlamDD+−++−+=4443DD=))(()(345512342