機(jī)器人學(xué)基礎(chǔ)_第4章_機(jī)器人動力學(xué)

1、1中南大學(xué)中南大學(xué)蔡自興，謝蔡自興，謝 斌斌zxcai, 2010機(jī)器人學(xué)基礎(chǔ)機(jī)器人學(xué)基礎(chǔ)第四章第四章 機(jī)器人動力學(xué)機(jī)器人動力學(xué)1Ch.4 Manipulator DynamicsFundamentals of Robotics Fundamentals of RoboticsContents Introduction to Dynamics Rigid Body Dynamics Lagrangian Formulation Newton-Euler Formulation Articulated Multi-Body Dynamics2Ch.4 Manipulator Dynamics3C

2、h.4 Manipulator DynamicsIntroductionCh.4 Manipulator DynamicsManipulator Dynamics considers the forces required to cause desired motion. Considering the equations of motion arises from torques applied by the actuators, or from external forces applied to the manipulator.Ch.4 Manipulator DynamicsTwo m

3、ethods for formulating dynamics model：Newton-Euler dynamic formulationNewtons equation along with its rotational analog, Eulers equation, describe how forces, inertias, and accelerations relate for rigid bodies, is a force balance approach to dynamics.Lagrangian dynamic formulationLagrangian formula

4、tion is an energy-based approach to dynamics. Ch.4 Manipulator DynamicsCh.4 Manipulator DynamicsThere are two problems related to the dynamics of a manipulator that we wish to solve. Forward Dynamics: given a torque vector, , calculate the resulting motion of the manipulator, . This is useful for si

5、mulating the manipulator.Inverse Dynamics: given a trajectory point, , find the required vector of joint torques, . This formulation of dynamics is useful for the problem of controlling the manipulator.Ch.4 Manipulator Dynamics,and ,and Contents Introduction to Dynamics Rigid Body Dynamics Lagrangia

6、n Formulation Newton-Euler Formulation Articulated Multi-Body Dynamics6Ch.4 Manipulator Dynamics74.1 Dynamics of a Rigid Body 剛體動力學(xué)剛體動力學(xué)Langrangian Function L is defined as:Dynamic Equation of the system (Langrangian Equation): where qi is the generalized coordinates, represent corresponding velocit

7、y, Fi stand for corresponding torque or force on the ith coordinate. 4.1 Dynamics of a Rigid BodyLPKniqLqLdtdiii, 2 , 1, F(4.1)(4.2)iq Kinetic EnergyPotential Energy4.1.1 Kinetic and Potential Energy of a Rigid Body82211001122KMMxx0011201)(21gxgxxxMMkP2101()2Dcxx01FxFx W圖4.1 一般物體的動能與位能4.1 Dynamics o

8、f a Rigid Body4.1 Dynamics of a Rigid Body9 is a generalized coordinate Kinetic Energy due to (angular) velocity Kinetic Energy due to position (or angle) Dissipation Energy due to (angular) velocity Potential Energy due to position External Force or Torque010,xx11111xWxPxDxKxKdtd4.1.1 Kinetic and P

9、otential Energy of a Rigid Body4.1 Dynamics of a Rigid Body 10 x0 and x1 are both generalized coordinatesFgxxxxx1010111)()(MkcM Fgxxxxx0010100)()(MkcM 1111000000McckkMcckkxxxFxxxF4.1.1 Kinetic and Potential Energy of a Rigid Body4.1 Dynamics of a Rigid BodyWritten in Matrices form:11Kinetic and Pote

10、ntial Energy of a 2-links manipulator Kinetic Energy K1 and Potential Energy P1 of link 1 211 111 11111111,cos2Km vvdPm ghhd 22111111111,cos2Km dPm gd 圖4.2 二連桿機(jī)器手（1）4.1.1 Kinetic and Potential Energy of a Rigid Body4.1 Dynamics of a Rigid Body12Kinetic Energy K2 and Potential Energy P2 of link 2 222

11、222211212211212sinsincoscosvxyxddydd 2222221,2Km vPmgywhere2222222112212212211 22211221211cos22coscosKm dm dm d dPm gdm gd 4.1.1 Kinetic and Potential Energy of a Rigid Body4.1 Dynamics of a Rigid BodyTotal Kinetic and Potential Energy of a 2-links manipulator are13(4.3)21KKK2222121122122212211211()

12、()22cos()mm dm dm d d 21PPP)cos(cos)(21221121gdmgdmm(4.4)4.1.1 Kinetic and Potential Energy of a Rigid Body4.1 Dynamics of a Rigid BodyContents Introduction to Dynamics Rigid Body Dynamics Lagrangian Formulation Newton-Euler Formulation Articulated Multi-Body Dynamics14Ch.4 Manipulator Dynamics15Lag

13、rangian Formulation Lagrangian Function L of a 2-links manipulator: PKL)2(21)(21222121222212121dmdmm)cos(cos)()(cos2122112121212212gdmgdmmddm(4.5)niqLqLdtdiii, 2 , 1, F4.1 Dynamics of a Rigid Body4.1.2 Two Solutions for Dynamic Equation164.1.2 Two Solutions for Dynamic EquationLagrangian Formulation

14、 Dynamic Equations:2112212212121211122221222211122111212221121121DDDDDDDDDDDDDDTT (4.10)111dLLTdt222dLLTdtWritten in Matrices Form:(4.6)(4.7)有效慣量(effective inertial)：關(guān)節(jié)i的加速度在關(guān)節(jié)i上產(chǎn)生的慣性力4.1 Dynamics of a Rigid BodyWritten in Matrices Form:17Lagrangian Formulation Dynamic Equations:21122122121212111222

15、21222211122111212221121121DDDDDDDDDDDDDDTT (4.10)111dLLTdt222dLLTdt(4.6)(4.7)耦合慣量(coupled inertial)：關(guān)節(jié)i,j的加速度在關(guān)節(jié)j,i上產(chǎn)生的慣性力4.1.2 Two Solutions for Dynamic Equation4.1 Dynamics of a Rigid BodyWritten in Matrices Form:18Lagrangian Formulation Dynamic Equations:211221221212121112222122221112211121222112

16、1121DDDDDDDDDDDDDDTT (4.10)111dLLTdt222dLLTdt(4.6)(4.7)向心加速度(acceleration centripetal)系數(shù)關(guān)節(jié)i,j的速度在關(guān)節(jié)j,i上產(chǎn)生的向心力4.1.2 Two Solutions for Dynamic Equation4.1 Dynamics of a Rigid BodyWritten in Matrices Form:19Lagrangian Formulation Dynamic Equations:2112212212121211122221222211122111212221121121DDDDDDDDD

17、DDDDDTT (4.10)111dLLTdt222dLLTdt(4.6)(4.7)哥氏加速度(Coriolis accelaration)系數(shù)：關(guān)節(jié)j,k的速度引起的在關(guān)節(jié)i上產(chǎn)生的哥氏力(Coriolis force)4.1.2 Two Solutions for Dynamic Equation4.1 Dynamics of a Rigid BodyWritten in Matrices Form:20Lagrangian Formulation Dynamic Equations:2112212212121211122221222211122111212221121121DDDDDDD

18、DDDDDDDTT (4.10)111dLLTdt222dLLTdt(4.6)(4.7)重力項(gravity)：關(guān)節(jié)i,j處的重力4.1.2 Two Solutions for Dynamic Equation4.1 Dynamics of a Rigid Body21對上例指定一些數(shù)字，以估計此二連桿機(jī)械手在靜止和固定重力負(fù)載下的 T1 和 T2 的數(shù)值。取 d1=d2=1，m1=2，計算m2=1,4和100（分別表示機(jī)械手在地面空載地面空載、地面滿載地面滿載和在外空間負(fù)在外空間負(fù)載載的三種不同情況；在外空間由于失重而允許有較大的負(fù)載）三個不同數(shù)值下各系數(shù)的數(shù)值。Lagrangian Fo

19、rmulation of Manipulator Dynamics4.1 Dynamics of a Rigid Body22表4.1給出這些系數(shù)值及其與位置 的關(guān)系。 表4.1 222cos11D12D22D1I2I負(fù)載地面空載09018027010-1064242101111164242323地面滿載09018027010-1018102108404444418102102626外空間負(fù)載09018027010-1040220222022001000100100100100100402202220221022102Lagrangian Formulation of Manipulator

20、Dynamics注意：有效慣量的變化將對機(jī)械手的控制產(chǎn)生顯著影響！4.1 Dynamics of a Rigid BodyContents Introduction to Dynamics Rigid Body Dynamics Lagrangian Formulation Newton-Euler Formulation Articulated Multi-Body Dynamics23Ch.4 Manipulator DynamicsFm a4.1 Dynamics of a Rigid Body4.1.2 Two Solutions for Dynamic EquationNewton-

21、Euler Dynamic FormulationNewtons Lawrate of change of the linear momentum is equal to the applied forcedmvFdtLinear Momentummvpviiiim4.1 Dynamics of a Rigid Body4.1.2 Two Solutions for Dynamic EquationNewton-Euler Dynamic FormulationRotational MotionppiiiimAngular Momentum:densityimdvI4.1 Dynamics o

22、f a Rigid Body4.1.2 Two Solutions for Dynamic EquationNewton-Euler Dynamic FormulationRotational MotionppVdvAngular MomentumppVdvInertia TensorNewton-Euler Dynamic FormulationccFvmNIIcc)(where m is the mass of a rigid body, represent inertia tensor, FC is the external force on the center of gravity,

23、 N is the torque on the rigid body, vC represent the translational velocity, while is the angular velocity.（Euler Equation）3 3CIR（Newton Equation）4.1 Dynamics of a Rigid Body4.1.2 Two Solutions for Dynamic Equation例1. 求解下圖所示的1自由度機(jī)械手的運(yùn)動方程式，在這里，由于關(guān)節(jié)軸制約連桿的運(yùn)動，所以可以將運(yùn)動方程式看作是繞固定軸的運(yùn)動。1自由度機(jī)械手解：假設(shè)繞關(guān)節(jié)軸的慣性矩為 I，

24、取垂直紙面的方向為 z 軸，則有 II000000000IIcos00cmgLN4.1 Dynamics of a Rigid Body4.1.2 Two Solutions for Dynamic Equation該式即為1自由度機(jī)械手的歐拉運(yùn)動方程式。coscmgLI 由歐拉運(yùn)動方程式()CCI I N II000000000IIcos00cmgLN4.1 Dynamics of a Rigid Body4.1.2 Two Solutions for Dynamic Equation30Langrangian Function L is defined as:Dynamic Equatio

25、n of the system (Langrangian Equation): where qi is the generalized coordinates, represent corresponding velocity, Fi stand for corresponding torque or force on the ith coordinate. 4.1 Dynamics of a Rigid BodyLPKniqLqLdtdiii, 2 , 1, F(4.1)(4.2)iq Kinetic EnergyPotential Energy4.1.2 Two Solutions for

26、 Dynamic Equation例例2.通過拉格朗日運(yùn)動方程式求解之前推導(dǎo)的1自由度機(jī)械手。221IK sincmgLP 21sin2cLKPImgLILcoscmgLLcoscImgL解：假設(shè)為廣義坐標(biāo)，則有 ddLLtqq由拉格朗日運(yùn)動方程4.1 Dynamics of a Rigid Body4.1.2 Two Solutions for Dynamic Equation我們研究動力學(xué)的重要目的之一是為了對機(jī)器人的運(yùn)動進(jìn)行有效控制，以實現(xiàn)預(yù)期的軌跡運(yùn)動。常用的方法有牛頓歐拉法、拉格朗日法等。牛頓歐拉動力學(xué)法是利用牛頓力學(xué)牛頓力學(xué)的剛體力學(xué)剛體力學(xué)知識導(dǎo)出逆動力學(xué)的遞推計算公式，再由它歸

27、納出機(jī)器人動力學(xué)的數(shù)學(xué)模型機(jī)器人矩陣形式的運(yùn)動學(xué)方程；拉格朗日法是引入拉格朗日方程拉格朗日方程直接獲得機(jī)器人動力學(xué)方程的解析公式，并可得到其遞推計算方法。4.1 Dynamics of a Rigid Body4.1.2 Two Solutions for Dynamic Equation對多自由度的機(jī)械手，拉格朗日法可以直接推導(dǎo)直接推導(dǎo)運(yùn)動方程式，但隨著自由度的增多演算量將大量增加大量增加。與此相反，牛頓歐拉法著眼于每一個連桿的運(yùn)動，即便對于多自由度的機(jī)械手其計算量也不增加計算量也不增加，因此算法易于編程。由于推導(dǎo)出的是一系列公式的組合，要注意慣性矩陣等的選慣性矩陣等的選擇和求解擇和求解問題

28、。進(jìn)一步的問題請參考相關(guān)文獻(xiàn)資料。 4.1 Dynamics of a Rigid Body4.1.2 Two Solutions for Dynamic EquationContents Introduction to Dynamics Rigid Body Dynamics Lagrangian Formulation Newton-Euler Formulation Articulated Multi-Body Dynamics34Ch.4 Manipulator Dynamics4.2 Dynamic Equation of a Manipulator 機(jī)械手的動力學(xué)方程機(jī)械手的動力學(xué)

29、方程Forming dynamic equation of any manipulator described by a series of A-matrices:(1) Computing the Velocity of any given point;(2) Computing total Kinetic Energy;(3) Computing total Potential Energy;(4) Forming Lagrangian Function of the system;(5) Forming Dynamic Equation through Lagrangian Equati

30、on.354.2 Dynamic Equation of a Manipulator364.2.1 Computation of Velocity 速度的計算速度的計算Velocity of point P on link-3:Velocity of any given point on link-i: ppppTTdtddtdrrrv333300)()(rrviijjjiqqTdtd1(4.15)圖4.4 四連桿機(jī)械手4.2 Dynamic Equation of a Manipulator374.2.1 Computing the VelocityAcceleration of point

31、 P:30033331()()pppipjiTdddTqdtdtdtqavrrpkjjkkjpjiiqqqqTqdtdqTrr33131323313pkjjkkjpjiiqqqqTqqTrr33131323313 圖4.4 四連桿機(jī)械手4.2 Dynamic Equation of a Manipulator38Square of velocityThe trace of an square matrix is defined to be the sum of the diagonal elements. )()()()()(000020TpppppTracevvvvv31313333)()(

32、jkTpkkpjjqqTqqTTracerr31333133)( )(jkjkTTpkpjqqqTqTTracerr4.2.1 Computing the Velocity圖4.4 四連桿機(jī)械手4.2 Dynamic Equation of a Manipulator39Square of velocity of any given point:ijikTikkiijjiqqTqqTTracedtdr1122rrvijkkTkiTikiikiqqqTqTTrace11rr(4.16)4.2.1 Computing the Velocity圖4.4 四連桿機(jī)械手4.2 Dynamic Equat

33、ion of a Manipulator40Computing the Kinetic Energy 令連桿3上任一質(zhì)點(diǎn)P的質(zhì)量為dm，則其動能為：dmvdKp2321dmqqqTrrqTTracejkiTkkTppi31313333)(2131313333)(21jkiTkkTTppiqqqTrdmrqTTrace圖4.4 四連桿機(jī)械手4.2 Dynamic Equation of a Manipulator4.2.2 Computation of Kinetic and Potential Energy 動能和位能的計算 414.2.2 Computation of Kinetic and

34、 Potential EnergyKinetic Energy of any particle on link-i with position vector ir :Kinetic Energy of link-3:dmqqqTqTTracedKijkjikkTiTijjii1121rrijkjikkTiTTiijiqqqTdmqTTrace11)(21rr3333333311link3link31()2TTppjkjkjkTTKdKTracedmq qqq rr4.2 Dynamic Equation of a Manipulator42Kinetic Energy of any given

35、 link-i:Total Kinetic Energy of the manipulator:linkiiiKdK ijikkjkiijiqqqTIqTTrace1121(4.17)ninjkiikkTiijiniiqqqTIqTraceTKK111121(4.19)4.2.2 Computation of Kinetic and Potential Energy4.2 Dynamic Equation of a Manipulator43Computing the Potential EnergyPotential Energy of a object (mass m) at h heig

36、ht:so the Potential Energy of any particle on link-i with position vector ir :whererdmTrdmdPiiTTigg0mghP 1 ,zyxTgggglink link link TiTiiiiiiiiPdPT rdmTrdm ggiiiTiiiiiTrTmrmTgg4.2.2 Computation of Kinetic and Potential Energy4.2 Dynamic Equation of a Manipulator44Potential Energy of any particle on l

37、ink-i with position vector ir :Total Potential Energy of the manipulator: rdmTrdmdPiiTTigg0niniiaiiPPPP11)(niiiiTirTm1g (4.21)4.2.2 Computation of Kinetic and Potential Energy4.2 Dynamic Equation of a Manipulator45Lagrangian FunctionPKLt,2121112111niniiiiTiiainiijikkjkTiiiirTmqIqqqTIqTTraceg2 , 1n (

38、4.22)4.2 Dynamic Equation of a Manipulator4.2.3 Forming the Dynamic Equation 動力學(xué)方程的推導(dǎo)動力學(xué)方程的推導(dǎo)464.2.3 Forming the Dynamic EquationDerivative of Lagrangian function nikikkTiipipqqTIqTTraceqL11211112TniiiijappijipTTTraceIqI qqqnp, 2 , 14.2 Dynamic Equation of a Manipulator47According to Eq.(4.18), Ii is a symmetric matrix, lead toTTTTiiiiiiiiijkkjkjTTTTTTTraceITraceITraceIqqqqqqnipapkikpTiiki