1第三章机器人力学分析及动力学模型

1„3.1„3.2„1.——2.3.1——2——2„¾¾¾3„fn,n+1Nn,n+1„/⎥⎦⎤⎢⎣⎡=++1,1,nnnnNfF[]Tn21ττττfn,n+1Nn,n+14„F„„FτFJTττ5„——iqδePδeδφeTnneTnnnnNPfqqWδφδδτδτδ1,1,11++−−++PFqWTTδδτδ−=6qδPFqWTTδδτδ−=0WδPδqδqδ0WδFJTτ()qFJqJFqWTTTTδτδδτδ−=−=7F[]TyxFFF,=⎥⎦⎤⎢⎣⎡+−−−=1221221112212211CLCLCLSLSLSLJ⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡−+−−=⎥⎦⎤⎢⎣⎡yxTFFCLSLCLCLSLSLFJ122122122111221121ττLEC.7-088„C{C},{C}33IXXIYYIZZXYZIXYIXZIYZ.{A}{C}Z⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=ZZYZXZYZYYXYXZXYXXCIIIIIIIIII∫∫∫∫∫∫∫∫∫===VYZVXZVXYdvyzIdvxzIdvxyIρρρ()()()∫∫∫∫∫∫∫∫∫+=+=+=VZZVYYVXXdvxyIdvzxIdvzyIρρρ222222()ccXYCXYAccZZCZZAymxIIyxmII+=++=22×9„C{C},{C}33IXXIYYIZZXYZIXYIXZIYZ.{A}{C}Z⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=ZZYZXZYZYYXYXZXYXXCIIIIIIIIII∫∫∫∫∫∫∫∫∫===VYZVXZVXYdvyzIdvxzIdvxyIρρρ()()()∫∫∫∫∫∫∫∫∫+=+=+=VZZVYYVXXdvxyIdvzxIdvzyIρρρ222222()ccXYCXYAccZZCZZAymxIIyxmII+=++=22CZcYcXczxydmr×CZcYcXcxcyc{C}{A}ZA×10„1Lagrangian2Newton-Euler11„TUnqq,,1()UTqqLii−=,iiiQqLqLdtd=∂∂−∂∂iQiqiFirδ∑∑==⋅=⋅njjjkiiiqQrF11δδiF12„1234[]Tnθθϑ1[]Tnτττ113„LiiiiTiciTciiiIvvmTωω2121+=iIim∑==niiTT114„i{j-1}[]Tnqqq1qJqJqJqJqJqJviAiiAiiAiiLiiLiiLci)()(1)(1)()(1)(1=++==++=ω[][]0,,0,,,0,,0,,,)()(1)()()(1)(iAiiAiAiLiiLiLJJJJJJ==⎩⎨⎧×=−−−cijjjiLjrbbJ,111)(⎩⎨⎧=−1)(0jiAjbJij≤1−jb1−jzcijr,1−{0}CiVciωiθ1θ2θ315„HHHij()∑==+=niTiAiTiATiLTiLTiqHqqJIJqqJJqmT1)()()()(2121()∑=+=niiAiTiAiLTiLiJIJJJmH1)()()()(Hij∑∑===ninjjiijqqHT112116„i{0}0„Fext∑==niciTirgmU1,0cir,0()qFJPFqWTextTTextTδτδδτδ+=+=qQWTδδ=extTFJQ+=τ{0}Cimig0,ci17„i{0}0„Fext∑==niciTirgmU1,0cir,0()qFJPFqWTextTTextTδτδδτδ+=+=qQWTδδ=extTFJQ+=τ18„iiiQqLqLdtd=∂∂−∂∂∑∑∑===+=⎟⎟⎠⎞⎜⎜⎝⎛=⎟⎟⎠⎞⎜⎜⎝⎛∂∂njnjjijjijnjjijiqdtdHqHqHdtdqTdtd111∑∑==∂∂=∂∂=nknkkkijkkijijqqHdtdqqHdtdH11∑∑∑∑====∂∂=⎟⎟⎠⎞⎜⎜⎝⎛∂∂=∂∂njnkkjiijnjnkkjjkiiqqqHqqHqqT11112121∑∑===∂∂=∂∂=njnjjLiTjicjTjiiJgmqrgmqUG11)(,019„1iijjijkii23niQGqqhqHnjnjnkiikjijkjij,,1111==++∑∑∑===ijkkijijkqHqHh∂∂−∂∂=21HijiiHijkhiGijjh20„H,civiωiciIic⎥⎦⎤⎢⎣⎡−=1111111clslvcccθθ{}{}⎥⎦⎤⎢⎣⎡++−+−=221212212121212112121θθθθclclclslslslvcccccqclslvccc⎥⎦⎤⎢⎣⎡−=0101111qclclclslslslvccccc⎥⎦⎤⎢⎣⎡+−−−=12121121212212212)2(LJ)1(LJ21[]q0111θω[]q11212=+=θθω)1(AJ)2(AJ()⎥⎦⎤⎢⎣⎡++++++++++=2222222221222222122212221212112222IlmIlmcllmIlmcllmIcllllmIlmHcccccccc⎩⎨⎧=+==−=+−==0022,2,0221212222212211212121112212122111hhhsllmhsllmhhsllmhhccc[][]⎩⎨⎧+=+=)2(22)1(222)2(22)1(111LLTLLTJmJmgGJmJmgG22⎩⎨⎧+++=+−−+22111222221212221211112GhHHGhhHHθθθτθθθθθτ[](){}⎪⎪⎪⎪⎩⎪⎪⎪⎪⎨⎧=++==+−=−=++=+=+++++=12112122122222112211121212111212222222121222222222122212121111gclmGclclgmgclmGsllmhhhhIlmcllmHIlmHIcllllmIlmHcccccccccc