1、1中南大學中南大學蔡自興，謝蔡自興，謝 斌斌zxcai, 2010機器人學基礎機器人學基礎第四章第四章 機器人動力學機器人動力學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 剛體動力學剛體動力學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 二連桿機器手（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)：關節i的加速度在關節i上產生的慣性力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)：關節i,j的加速度在關節j,i上產生的慣性力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)系數關節i,j的速度在關節j,i上產生的向心力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)系數：關節j,k的速度引起的在關節i上產生的哥氏力(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)：關節i,j處的重力4.1.2 Two Solutions for Dynamic Equation4.1 Dynamics of a Rigid Body21對上例指定一些數字，以估計此二連桿機械手在靜止和固定重力負載下的 T1 和 T2 的數值。取 d1=d2=1，m1=2，計算m2=1,4和100（分別表示機械手在地面空載地面空載、地面滿載地面滿載和在外空間負在外空間負載載的三種不同情況；在外空間由于失重而允許有較大的負載）三個不同數值下各系數的數值。Lagrangian Fo

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

20、Dynamics注意：有效慣量的變化將對機械手的控制產生顯著影響！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自由度機械手的運動方程式，在這里，由于關節軸制約連桿的運動，所以可以將運動方程式看作是繞固定軸的運動。1自由度機械手解：假設繞關節軸的慣性矩為 I，

24、取垂直紙面的方向為 z 軸，則有 II000000000IIcos00cmgLN4.1 Dynamics of a Rigid Body4.1.2 Two Solutions for Dynamic Equation該式即為1自由度機械手的歐拉運動方程式。coscmgLI 由歐拉運動方程式()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.通過拉格朗日運動方程式求解之前推導的1自由度機械手。221IK sincmgLP 21sin2cLKPImgLILcoscmgLLcoscImgL解：假設為廣義坐標，則有 ddLLtqq由拉格朗日運動方程4.1 Dynamics of a Rigid Body4.1.2 Two Solutions for Dynamic Equation我們研究動力學的重要目的之一是為了對機器人的運動進行有效控制，以實現預期的軌跡運動。常用的方法有牛頓歐拉法、拉格朗日法等。牛頓歐拉動力學法是利用牛頓力學牛頓力學的剛體力學剛體力學知識導出逆動力學的遞推計算公式，再由它歸

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

28、。進一步的問題請參考相關文獻資料。 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 機械手的動力學方程機械手的動力學

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 四連桿機械手4.2 Dynamic Equation of a Manipulator374.2.1 Computing the VelocityAcceleration of point

31、 P:30033331()()pppipjiTdddTqdtdtdtqavrrpkjjkkjpjiiqqqqTqdtdqTrr33131323313pkjjkkjpjiiqqqqTqqTrr33131323313 圖4.4 四連桿機械手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 四連桿機械手4.2 Dynamic Equation of a Manipulator39Square of velocity of any given point:ijikTikkiijjiqqTqqTTracedtdr1122rrvijkkTkiTikiikiqqqTqTTrace11rr(4.16)4.2.1 Computing the Velocity圖4.4 四連桿機械手4.2 Dynamic Equat

33、ion of a Manipulator40Computing the Kinetic Energy 令連桿3上任一質點P的質量為dm，則其動能為：dmvdKp2321dmqqqTrrqTTracejkiTkkTppi31313333)(2131313333)(21jkiTkkTTppiqqqTrdmrqTTrace圖4.4 四連桿機械手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 動力學方程的推導動力學方程的推導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