Autonomous and Mobile Robotics Prof. Giuseppe Oriolo. Humanoid Robots 2: Dynamic Modeling

Transcription

1 Autonomous and Mobile Robotics rof. Giuseppe Oriolo Humanoid Robots 2: Dynamic Modeling

2 modeling multi-body free floating complete model m j I j R j ω j f c j O z y x p ZM conceptual models for walking/balancing for running Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 2

3 like a manipulator? can we consider this as a part (leg) of a legged robot? NO: this manipulator cannot fall because its base is clamped to the ground this is a (one)-legged robot monopod Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 3

4 contacts the difference lies in the contact forces one may look at these various contact situations as different fixed-base (not clamped!) robots, each with a specific kinematic and dynamic model or consider a single system with a floating base and limbs that may establish contacts Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 4

5 free-floating system the general model is that of a free-floating multi-body f ext f ext,l f ext,r Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 5

6 configuration f ext vs formally similar B(q) q + C(q, q) q + g(q) = + J T (q)f ext Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 6

7 Lagrangian dynamics 2 dynamic equations (general form) B(q) q + C(q, q) 4 q + g(q) 5 = + J T (q)f ext but here we have a special structure B(q) ẍ g5a + n(q, q) = i J T i (q)f i where g is the (Cartesian) gravity acceleration vector and Ji is the Jacobian matrix associated to the i-th contact force fi Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 7

8 Lagrangian dynamics B(q) ẍ g5a + n(q, q) = i J T i (q)f i mass/ inertia accelerations forces/torques centrifugal/coriolis forces joint torques contact forces joint torques only affect joint coordinates! to move x0 (i.e., the position of the reference body) the contact forces are necessary Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 8

9 Lagrangian dynamics in the absence of external forces, the system becomes 4 5 underactuated, i.e., it has less inputs than dof s concentrate on the 2nd and 3rd set of equations, that are peculiar 2 31to legged robots 2 4 5A 2nd set can be written as M( c + g) = i 4 f i Newton equation (variation of linear momentum) where c : CoM position and M : total system mass hence: we need contact forces to move the CoM in a direction different from that of gravity! Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 9

10 4 5A 4 5 Lagrangian dynamics 3rd set can be written as L = i i (p i c) f i Euler equation (variation of angular momentum) pi : position of the contact point of force fi!k : angular velocity of the k-th body L : angular momentum of the robot wrt its CoM n(x k c) m k ẋ k + I k! k o L = k Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 10

11 recall: moment of a force 2 the moment of a force (or torque) is a measure of its tendency to cause a body 5A to rotate about 4 5a specific point or axis. In order for a moment to develop, the force must act upon the body in such a manner that the body would begin to twist. This occurs every time a force is applied so that it does not pass through the centroid of the body L = i A (p i c) f i 2 3 variation of angular momentum = sum of torques i c pi - c pi fi 1 A (p i c) f i torque generated by the contact force fi around the CoM L = k (x k c) m k ẋ k + I k! k angular momentum around the CoM Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 11

12 2 Lagrangian dynamics: flight phase 31 Flight phase: A 1 M( c + g) = i A 4 5 i f i no contacts with environment 4 no external forces other than gravity c = g constant horizontal speed while falling 5 L = i (p i c) f i L = 0 but the robot is still capable of generating rotations due to the nonholonomy of the angular momentum conservation Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 12

13 Lagrangian dynamics: flat ground some manipulations: c cross-product (Newton Eq) + Euler equation, everything divided by z- component of Newton equation leads to M( c z + g z )= fi z i Mc ( c + g)+ L i M( c z + g z = p i f i ) i f i z flat ground hypothesis (not necessarily horizontal) and we may have p z i =0 g x,y 6=0 g ground z pi x Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 13

14 6 Lagrangian dynamics: flat ground flat ground p z i =0 c y c x or in compact form first two components (x and y) c z c z + g z ( cy + g y )+ c z c z + g z ( cx + g x ) L x M( c z + g z ) = L y M( c z + g z ) = i f i zpy i i f z i i f z i px i i f z i c x,y c z c z + g z ( cx,y + g x,y )+ S L x,y M( c z + g z ) = i f z i px,y i i f z i with S = apple Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 14

15 Lagrangian dynamics: flat ground apple consequences i f z i px,y i i f z i weighted sum = z x,y apple f z 1 p1 contact points f z 4 p4 z is the Center of ressure (Co) point of application of the Ground Reaction Force vector (GRF) z p3 f z 3 p2 f z 2 normal contact forces z x = z y = i f z i px i i f z i i f z i py i i f z i note: GRF can also have a horizontal component when the robot walks Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 15

16 Lagrangian dynamics: flat ground apple consequences Center of ressure z f z 4 f z 3 normal contact forces f z 1 p1 p4 z p3 f z 2 moreover f z i 0 normal force can only be positive (unilateral force) contact points therefore the Co must belong to the convex hull of the contact points, i.e. the Support olygon z x,y 2 conv {p x,y i } # Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 16 p2

17 more on the Co the Center of ressure (Co) z is usually defined as the point on the ground where the resultant of the ground reaction force acts. we have 2 types of interaction forces at the foot/ground interface: normal forces fi z and tangential forces apple fi x,y apple the Co may be defined as the point z where the resultant of the apple normal forces fi z acts f z = i f z i the resultant of the tangential forces may be represented at z by a force f x,y and a moment Mt f x,y = i f x,y i M t = i r i f x,y i where ri is the vector from z to the point of application of fi x,y Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 17

18 more on the Co fi z x,y p i f z z the sum of the normal and tangential components gives the resulting GRF apple ri fi x,y z f x,y Mt resulting GRF apple normal apple forces apple f z = fi z i i = z x,y i f i z i f z i px,y tangential forces f x,y = i M t = i f x,y i Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 18 r i f x,y i z f z Mt f x,y

19 6 Lagrangian dynamics: multi-body system (flat ground) c x,y c z c z + g z ( cx,y + g x,y )+ S L x,y M( c z + g z ) = rewritten as i f z i px,y i i f z i c z c z + g z ( cx,y + g x,y )=(c x,y z x,y )+ S L x,y M( c z + g z ) we can analyze the effect of the various terms on the CoM horizontal acceleration (horizontal = in the x-y plane) Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 19

20 Lagrangian dynamics: multi-body system c z c z + g z cx,y = c z c z + g z gx,y +(c x,y z x,y )+ S L x,y M( c z + g z ) c z c z + g z gx,y c x,y x-y plane S L x,y M( c z + g z ) c z c z + g z cx,y z x,y support polygon aside from the effect of gravity (horizontal components) and variations of the angular momentum, the CoM horizontal acceleration is the result of a force pushing the CoM away from the Co c x,y no gravity in x-y no variations of L z x,y Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 20

21 Lagrangian dynamics: multi-body system more on the Co (Co) rearranged i z x,y = i f i zpx,y i i f z i on flat ground " # x,y i z x,y )fi z = (p i z) f i =0 (p x,y i Zero Moment oint ZM horizontal momenta of the contact forces fi w.r.t. the Co z are equal to zero sometimes defined as the zero tipping moment point on flat ground ZM = Co Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 21

22 more on the ZM Equivalent definitions point where the vertical reaction force intersects the ground point on the ground where the total moment generated due to gravity and inertia equals zero point on the floor at which the horizontal components of the momentum generated by the reaction force and the reaction torque are zero there are basically two equivalent ways to look at the ZM: wrt to the reaction force (horizontal component of the moment of ground reaction force is zero) wrt inertial and gravitational forces (net moment is zero) Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 22

23 more on the ZM The equation for rotational dynamic equilibrium is obtained by noting that the sum of the external moments on the robot, computed either at its CoM or at any stationary reference point, is equal to the sum of the rates of change of angular momentum of the individual segments about the same point. The equations for static equilibrium of the foot are obtained by setting the rates of change of angular momentum of the individual segments about the same point in the rotational dynamic equilibrium to zero. Basically, rotational equilibrium of the stance foot in the single support phase is an important criterion in the evaluation of postural and gait stability since foot rotation reflects a loss of balance. Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 23

24 " Lagrangian dynamics: approximations " on horizontal flat ground + CoM at constant height + neglect L x,y g x,y =0 c z = constant c g pi z x ground c z c z + g z ( cx,y + g x,y )=(c x,y z x,y )+ S L x,y M( c z + g z ) or c x,y c z g z cx,y = z x,y c x,y = gz c z (cx,y z x,y ) Linear Inverted endulum (LI) Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 24

25 Linear Inverted endulum interpretation 2 independent equations c x,y = gz c z (cx,y z x,y ) c x = gz c z (cx z x ) c y = gz c z (cy z y ) how the CoM moves in longitudinal direction (sagittal plane) lateral direction 1.5 Longitudinal CoM and ZM 1 c z CoM typical behaviors x (m) CoM reference CoM actual ZM reference t (sec) Lateral CoM and ZM z x c x x y (m) CoM reference CoM actual ZM reference t (sec) Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 25

26 Linear Inverted endulum interpretation c z oint foot the simplest interpretation of the LI is that of a telescoping (so to remain at a constant height) massless leg in contact with the ground at p x (point of contact) we can interpret the (longitudinal z direction) LI equation as a moment balance around p x M c x c z Mg(c x p x )=0 p x c x x i.e. c x = gz c z (cx p x ) in this case the ZM z x coincides with the point of contact p x of the fictitious leg p x = z x Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 26

27 Linear Inverted endulum interpretation oint foot (longitudinal direction) c z z x step step c z LI t t p x c x typical footsteps and CoM p x = z x step2 CoM step1 c x t step biped t may also be seen as a compass biped with only one leg touching the ground at the same time Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 27

28 Linear Inverted endulum interpretation Finite sized foot (with ankle torque) since z x represents the ZM location, there is no difficulty in extending the interpretation of the LI considering both single and double support phases with a finite foot dimension single support double support z x support polygon z x Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 28

29 Linear Inverted endulum interpretation Finite sized foot (with ankle torque y) we can see the single support phase from the stance foot point of view i.e. with the dynamics of the rest of the humanoid represented by an equivalent fictitious leg. A way to keep the CoM balanced is using an equivalent ankle torque (the real joint torques are such that an equivalent ankle torque is applied) finite foot with equivalent massless leg plus ankle torque ankle torque y finite foot p x Co z x moment w.r.t. z x (Co) = 0 Mg(z x p x ) y =0 z x = p x + y Mg Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 29 y Co + y

30 Linear Inverted endulum interpretation Finite sized foot (with ankle torque y) note that it is possible to move the Co through the ankle torque y without stepping moment around ankle point p x single support M c x c z Mg(c x p x )+ y =0 c x = gz c z c x p x y Mg - Co = -z x z x finite foot with equivalent massless leg plus ankle torque ankle torque p x y Co finite foot Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 30

31 Linear Inverted endulum interpretation Finite sized foot (with ankle torque) c y z x... with z x = p x + y Mg stepx stepy stepx c x t c x = gz c z (cx z x ) longitudinal direction Typical footsteps with single and double support: for example, in the first single support (- -) the left foot is swinging; as soon as the right foot touches the ground the double support starts ( ) and the ZM moves from the left to the right foot (longitudinal and lateral motions) SS DS SS DS z y... stepy t SS: single support DS: double support Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 31

32 Linear Inverted endulum interpretation Finite sized foot and reaction mass it is also possible to extend the point-mass to be a rigid body with its rotational inertia so that also a hip movement can be modelled wy J finite foot with equivalent massless leg plus ankle torque and reaction mass y support polygon z x z x Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 32

33 Linear Inverted endulum interpretation Finite sized foot and reaction mass what is the effect of the rotating inertia around the CoM? Since no rotational inertia around the CoM, a different direction of the GRF would create a non-zero moment p x GRF M 6= 0 M 6= 0 J =0 J 6= 0 p x wy GRF A reaction mass type pendulum, by virtue of its non-zero rotational inertia, allows the ground reaction force to deviate from the lean line. This has important implication in gait and balance. Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 33

34 Linear Inverted endulum interpretation Finite sized foot and reaction mass moment around ankle p x M c x c z Mg(c x p x )+ y wy =0 wy J i.e. c x = gz c z c x z x wy Mg GRF with z x = p x + y Mg - CM ankle torque moves the Co while the reaction mass torque changes the GRF direction CM y z x to highlight the presence of a non-zero moment around the CoM a new point named Centroidal Moment ivot (CM) is introduced and defined as the point where a line parallel to the ground reaction force, passing through the CoM, intersects with the external contact surface Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 34

35 Linear Inverted endulum: basic scope c x,y = gz c z (cx,y z x,y ) Although extremely simplified, the LI equation describes in first approximation the time evolution of the CoM trajectory. Moreover it defines a differential relationship between the CoM trajectory and the ZM (or CM) time evolution it is easier to design a controller which makes the actual CoM follow a desired behaviour dynamic balancing will be characterized in terms of the ZM the problem will then be to understand which CoM trajectory, solution of the LI equation, guarantees that dynamic balancing is achieved Oriolo: Autonomous and Mobile Robotics - Humanoid Robots 2 35