Feedback control of humanoid robots: balancing and walking Dr.-Ing. - PowerPoint PPT Presentation

Bipedal Robot Model Mg Conservation of momentum:      M c Mg f i  i r , l c               I 0 M 0 c 0 Mg 0                  F        ˆ ˆ i T             ˆ ˆ ˆ J ( q ˆ )  0 M ( q ) q C ( q , q ) 0 u p  í r , l f i p p  p            (    c , )      q            x b     T T J ( q ) J ( q )          br   bl     M ( q ) M ( q ) x x 0            x xq b b   T   C ( q , x , q ) g ( x , q ) J ( q ) F 0 F            b b r r l    M ( q ) M ( q )         q q     qx T 0 J ( q )     Folie 26 l

Bipedal Robot Model Mg Conservation of angular momentum:       L c Mg i Conservation of momentum:      M c Mg f i  i r , l c               I 0 M 0 c 0 Mg 0                  F        ˆ ˆ i T             ˆ ˆ ˆ J ( q ˆ )  0 M ( q ) q C ( q , q ) 0 u p  í r , l f i p p  p            (    c , )      q            x b     T T J ( q ) J ( q )          br   bl     M ( q ) M ( q ) x x 0            x xq b b   T   C ( q , x , q ) g ( x , q ) J ( q ) F 0 F            b b r r l    M ( q ) M ( q )         q q     qx T 0 J ( q )     Folie 27 l

Bipedal Robot Model On a flat ground: Mg           L c Mg p ( M c Mg ) p Conservation of angular momentum:       L c Mg      i p f p p Conservation of momentum:      M c Mg f i  i r , l c               I 0 M 0 c 0 Mg 0                  F        ˆ ˆ i T              ˆ ˆ ˆ J ( q ˆ )  0 M ( q ) q C ( q , q ) 0 u p  í r , l f i p p  p            (    c , )      q            x b     T T J ( q ) J ( q )          br   bl     M ( q ) M ( q ) x x 0            x xq b b   T   C ( q , x , q ) g ( x , q ) J ( q ) F 0 F            b b r r l    M ( q ) M ( q )         q q     qx T 0 J ( q )     Folie 28 l

Bipedal Robot Model On a flat ground: Mg           L c Mg p ( M c Mg ) p c               M 0 c 0 Mg 0 I 0                  F        ˆ ˆ i T             ˆ ˆ ˆ J ( q ˆ )  0 M ( q ) q C ( q , q ) 0 u p  í r , l f i p p (  c , )            x b     T T J ( q ) J ( q )          br   bl     M ( q ) M ( q ) x x 0            x xq b b   T   C ( q , x , q ) g ( x , q ) J ( q ) F 0 F            b b r r l    M ( q ) M ( q )         q q     qx T 0 J ( q )     Folie 29 l

Zero Moment Point

Zero Moment Point [Vukobratovic and Stepanenko,1972] ZMP as a ground reference point: Distributed ground reaction force under the supporting foot can be replaced by a single force acting at the ZMP . F   z ( x , y ) x x 1 2  ( x ) y x F   0 ZMP = CoP (Center of Pressure) x x 1 2 p   0  ( x , y ) p in convex hull of the support polygon. Folie 31

Some facts about the ZMP Can ZMP leave the support polygon?  NO Can ZMP location be used as a stability criterion  NO If ZMP reaches the border of the support polygon  foot rotation possible. ZMP is defined on flat contact (no uneven surface). ZMP gives no information about sliding. F  x x 1 2  ( x ) Folie 32

First usage of the ZMP • Motion of the legs is predefined. • Upper body controls the ZMP in the center of the supporting foot  ensure proper foot contact during walking

How to obtain the ZMP? Measurement Dynamics Computation e.g. by Force/Torque Sensor

How to obtain the ZMP? Measurement Dynamics Computation e.g. by Force/Torque Sensor  f s s z p s p       ( p ) ( p p ) f s s s      ( p p ) f p f  y sz z x sx z p x f z     ( p p ) f p f  x sz z y sy z p y f z

How to obtain the ZMP? Measurement Dynamics Computation e.g. by Force/Torque Sensor  f s s z p s p       ( p ) ( p p ) f s s s      ( p p ) f p f  y sz z x sx z p   x p f f p z p     ( p p ) f p f  x sz z y sy z p y f      p f z p p

How to obtain the ZMP?    P Mg f Dynamics Computation      L c Mg          L c Mg p ( P Mg ) p   0 px   0 py c     Mgc p P L  x z x y   p p   f x Mg P p p z     Mgc p P L  y z y x p        y p f Mg P p p z

A simplified walking model based on the ZMP

 p   p p f p Mass concentrated model   p p  f 

 p  c  p p f p Mass concentrated model   p p  f   c M  c  M 0 c    z P p L

Mass concentrated model     Mgc p P L  x z x y p   x Mg P z     Mgc p P L  y z y x p   y Mg P z   P M c    L c M c   c c   z x p c  c  x x p 0   g c z z   c c   z y p c   p  y y   f g c p p z ZMP of a mass concentrated model      p f p p

Mass concentrated model Cart-Table Model [Kajita]   c x c   c c   z x p c  x x   g c   z    c c 0 z z   c c   z y p p c  y y   g c z   c c   z x p c x x g    p c x x

Mass concentrated model Linear Inverted Pendulum Model [Sugihara] Cart-Table Model [Kajita]   c x c c p p     g c c       z x c c p p c x x x x x c g z  c      p c p x x x x

Capture Point (Extrapolated Center of Mass) ((Divergent Component of Motion))

Capture Point Definition of the “Capture Point” (Pratt 2006, Hof 2008): Point to step in order to bring the robot to stand. Can be computed exactly for simple models, e.g. linear inverted pendulum c  , c model: c g          2 x x p c z ZMP p * p Computation of the Capture Point:  x ( 0 )         p const x ( t ) cosh( t ) x ( 0 ) sinh( t ) ( 1 cosh( t )) p     x ( t ) p  x   0 p * x  0

Capture Point Dynamics  x       x Coordinate transformation: ( x , x ) ( x , )        x x      2 x ( x p )       p System structure: Cascaded system  p x capture COM point open loop exp. stable unstable Dual use of the capture point for robotics 1. step planning 2. control

Capture Point in Human Measurements Linear Inverted Pendulum Human x x y y Data from [*] [*] Hof, The extrapolated center of mass concept suggests a simple control of balance in walking , Human Movement Science 27, pp.112-125, 2008.

Centroidal Moment Pivot

Centroidal Moment Pivot • Observation in human data: For normal level-ground walking, the human body‘s angular momentum (and the angular excursions) about the COM remains small through the gait cycle. • The centroidal moment pivot was introduced as a ground reference point to address the effects of angular momentum about the COM in connection with postural balance strategies.

Centroidal Moment Pivot Forces in the LIP model Effect of an additional hip torque    Mg   M g z  x M   M   x x z F F p CMP     g   g z            x x p x x p z z Mz    CMP p F z

Interpretation • The distance between CMP and ZMP corresponds to the angular momentum about the COM. • While the ZMP cannot leave the support polygon (by definition), the CMP can leave it. • The distance between CMP and the support polygon has been proposed as an indicator which balance strategy should dominate (via ZMP or via angular momentum).

Overview Part I: Modeling Part II: Balancing 1. Basics 2. ZMP based balancing (concentrated mass model) 3. Torque based balancing (multi body dynamics) Part III: Walking Control Folie 52

Humanoid Balance “Balance” is a generic term describing the ability to control the body posture in order to prevent falling. Vision Vision Vestibular system IMU Somatosensory system joint sensing force sensors Folie 53

Humanoid Balance Strategies for human push recovery: Small push: Medium push: Ankle strategy Hip strategy Large Push: Step strategy Human Robot force control angular momentum ZMP control control Folie 54

mass concentrated model Strategies for gait stabilization: Effect of an additional hip torque Mg  1. Controlling ZMP (constraints!) 2. Angular momentum control M   x 3. Step adaptation F p  g        x x p z Mz Folie 55

Overview Part I: Modeling Part II: Balancing 1. Basics 2. ZMP based balancing (concentrated mass model) 3. Torque based balancing (multi-body model) Part III: Walking Control Folie 56

Folie 57 fully compliant Motivation for compliant control compliant control completely stiff

ZMP based balancing Mg F position/velocity controlled robot ext x x  ref  q Joint x Robot Feedback Inverse d d Position Dynamics M   Stabilization  Kinematics x q p Control d ref z F q x Forward Kinematics p F , R F p L     ZMP p x M x F  ext Computation z Mg Folie 58

ZMP based balancing Mg F position/velocity controlled robot ext x x  ref  q Joint x Robot Feedback Inverse d d Position Dynamics M   Stabilization  Kinematics x q p Control d ref z F q x Forward Kinematics p F , R F p L     ZMP p x M x F  ext Computation z Mg Effective Stiffness: Control law for stabilization:          x x K ( x x ) K ( p p ) K Mg   d ref X d ref P ref   X K 1     K z P     K K 0 Stability condition [* ]: X P Stability condition [* ]: [Choi, Kim, Oh, and You, Posture/Walking Control for Humanoid Robot Folie 59 Based on Kinematic Resolution of CoM Jacobian With Embedded Motion , TRO, 2007].

ZMP Based balancing Balancing + Vertical Motion Balancing + Vertical Motion + Compliant Orientation

Overview Part I: Modeling Part II: Balancing 1. Basics 2. ZMP based balancing (concentrated mass model) 3. Torque based balancing (multi-body model) Part III: Walking Control Folie 61

Balancing & Posture Control Mg Compliant COM control [Hyon & Cheng, 2006]        F Mg K ( c c ) K ( c c ) F COM P d D d ext F COM c R  SO ( 3 ) T T Trunk orientation Control ext HIP   V ( R , K )      R T D ( )   HIP R d IMU measurements Folie 62

Balancing & Posture Control Mg Compliant COM control [Hyon & Cheng, 2006]        F Mg K ( c c ) K ( c c ) F COM P d D d ext F COM W  Desired wrench: ( F , T ) d COM HIP R  SO ( 3 ) T T Trunk orientation Control ext HIP   V ( R , K )      R T D ( )   HIP R d IMU measurements Folie 63

Balancing & Posture Control Compliant COM control [Hyon & Cheng, 2006]        F Mg K ( c c ) K ( c c ) COM P d D d Mg F ext W  Desired wrench: ( F , T ) d COM HIP W d Trunk orientation Control   V ( R , K )      R T D ( )   HIP R d IMU measurements Folie 64

Folie 65 Grasping and Balancing Force distribution: Similar problems!

Force Distribution in Grasping f i   F   1         Net wrench acting on the object:     W O G F G F G G     1 1 1       se ( 3 ) F    Grasp Map     F C  se ( 3 ) G  T Ad i PiO F C   FC Well studied problem in grasping: Find contact wrenches such that a desired net wrench on the object is achieved. friction cone Folie 66

Force distribution Relation between balancing wrench & contact forces   R  i G   i  ˆ  p R i i T   f HIP   1         W d G G   1    F   COM f      G  F   f   G C T   3 f Constraints: i ,  f 0 • Unilateral contact: (implicit handling of ZMP constraints) i z • Friction cone constraints Folie 67

Force distribution Relation between balancing wrench & contact forces   R  i G   i  ˆ  p R i i T   f HIP   1         W d G G   1    F   COM f      G  F   f   G C T   3 f Constraints: i ,  f 0 • Unilateral contact: (implicit handling of ZMP constraints) i z • Friction cone constraints Formulation as a constraint optimization problem           2    2   2 f arg min F G f T G f f 1 2 3 C 1 COM F C 2 HIP T C 3 C Folie 68

Contact force control via joint torques Multibody robot model: COM as a base coordinate  system structure with decoupled COM dynamics. [Space Robotics], [Wieber 2005, Hyon et al. 2006]     M                c Mg f M 0 c 0 Mg 0 I 0  i                 F        ˆ ˆ i T           ˆ ˆ ˆ  ˆ  0 M ( q ) q C ( q , q ) 0 u J ( q )  í r , l i    T J ( q ˆ ) f i i c Passivity based compliance control (well suited for balancing)   3 f i Folie 69

Torque based balancing f Object Force c Force Robot Force Torque Generation Mapping Dynamics Distribution Control q IMU for orientation control and COM computation Folie 70

Folie 71 Uncertain Foot Contact [Ott, Roa, Humanoids 2011, best paper award]

Experiments on a Perturbation Platform Leg perturbation setup Movable elastic platform Experimental evaluation of the robustness with respect to disturbances (frequency & amplitude) at the foot Folie 72

synchronous disturbance 2mm, up to 8 Hz Out of phase disturbance

Comparisons 1) Impact experiments 2) Whole body interaction 3) Singularities

Comparison 1/3 1) Impact experiments Position Based Control Torque Based Control

Comparison 1/3 1) Impact experiments Position Based Control Observations: – Balancing after impact is comparable – Torque based controller does not control relative foot location Torque Based Control

Comparison 2/3 3) Whole body interaction Position Based Control Torque Based Control

Comparison 2/3 3) Whole body interaction Position Based Control Observations: – Force sensor based controller depends on a reference frame – Torque based controller does not need information about the point of contact Torque Based Control

Comparison 3/3 4) Singular Configurations Position Based Control Torque Based Control

Comparison 3/3 4) Singular Configurations Position Based Control Observations: – Position based controller uses Inverse Kinematics, which requires singularity handling – Torque based controller uses transposed Jacobian mapping, and thus is not affected by singularities Torque Based Control

Balancing: Summary  On flat floor both approaches allow for a compliant behavior  Torque based controller shows independence on precise ground contact (force mapping based on IMU information)  Admittance controller depends on a reference frame

Overview Part I: Modeling Part II: Balancing Part III: Walking Control 1. Walking pattern generation 2. Feedback control Folie 82

Robot control based on conceptual models realtime x x c d Footstep Pattern ZMP-COM Pos. Controlled Robot Generation Generation Stabilizer p F e.g. LQR Preview Control [Kajita, 2003] Model Predictive Control [Wieber, 2006]

Mass concentrated model Linear Inverted Pendulum Model [Sugihara] Cart-Table Model [Kajita]   x c c p p z g           p x x x x p g z  x      p x p

Mass concentrated model Continuous time control model y  p     0 1 0 0 Cart-Table Model [Kajita]   x          x 0 0 1 x 0 u       x x             0 0 0 1 x       x   z       u x   y 1 0 x   g c Discrete time model     2 3 1 T T / 2 T / 6 p        2 x ( k 1 ) 0 1 T x ( k ) T / 2 u ( k )         z     0 0 1 T     p x x g     y ( k ) 1 0 c / g x ( k ) z    p x

LQR Preview Control How to use future information about the reference?

LQR Preview Control Time-discrete system:      n x ( k ) x ( k 1 ) Ax ( k ) Bu ( k )    p y ( k ) Cx ( k ) y ( k ) Assumptions:   0 C   rank   p n Assume (A,B) is stabilizable & (C,A) is detectable, and [* * ]    B A I [* * ] Ensures that the system has no transmission zero at z= 1. y ref ( k ) Reference output: Assume known for N future time steps Cost function:            T T T J e ( k ) Q e ( k ) x ( k ) Q x ( k ) u ( k ) R u ( k ) e ( k ) y ( k ) y ( k ) e x ref  k 0     x ( k ) x ( k ) x ( k 1 )     Q e , R u ( k ) u ( k ) u ( k 1 ) positive definite. • Uses differential control input  integral action. • Uses the output error compared to reference signal.

LQR Preview Control Time-discrete system:    x ( k 1 ) Ax ( k ) Bu ( k )  y ( k ) Cx ( k )   x ( k ) x ( k )   u ( k ) u ( k ) Modified system representation:            e ( k 1 ) I CA e ( k ) CB I               u ( k )   y ( k 1 )    ref           x ( k 1 ) 0 A x ( k ) B 0 [* * ] Ensures that this system is stabilizable.

LQR Preview Control Time-discrete system:    x ( k 1 ) Ax ( k ) Bu ( k )  y ( k ) Cx ( k )   x ( k ) x ( k )   u ( k ) u ( k ) Modified system representation:            e ( k 1 ) I CA e ( k ) CB I               u ( k )   y ( k 1 )    ref           x ( k 1 ) 0 A x ( k ) B 0 [* * ] Ensures that this system is stabilizable. How to handle future reference input?         x ( k ) y ( k 1 ), , y ( k N ) d ref ref   0 1 0 0   System augmentation Dynamics of the new state:  0 0 0     (for next N reference input values) x ( k 1 ) x ( k )   d d   0 1     0 0 0 0        A d

LQR Preview Control Modified system representation:             e ( k 1 ) I CA I 0 e ( k ) CB               x ( k 1 ) 0 A 0 x ( k ) B u ( k )                          x ( k 1 ) 0 0 A x ( k ) 0   d    d   d     z ( k 1 ) z ( k ) Cost function:          T T T J e ( k ) Q e ( k ) x ( k ) Q x ( k ) u ( k ) R u ( k ) e x  k 0   Q 0 0 e         T T J z ( k ) 0 Q 0 z ( k ) u ( k ) R u ( k )   x    k 0   0 0 0 Standard LQR Design for augmented system!

Preview Control   e ( k )         u ( k ) Kz ( k ) K K K x ( k ) Control law:    e d     x ( k ) d   k N        u ( k ) K y ( i ) y ( i ) K x ( k ) K ( i ) y ( k i )  e ref d ref   i 0 i 1

Example Application: Walking Pattern Generation (Kajita 2003) p … ZMP (Zero Moment Point) loosely speaking: Point on the sole where the reduced contact force is acting. x … Position of the CoM Simplified Model: Cart Table Model   x     2 3 1 T T / 2 T / 6         x x      2 x ( k 1 ) 0 1 T x ( k ) T / 2 u ( k )               x 0 0 1 T             u x y ( k ) 1 0 z / g x ( k ) y  p

Example Application: Walking Pattern Generation p x q Walking Joint d CoM d Footstep Pattern Position IK planning Generator Control Preview Control: T = 5 ms N = 400 Q_x = zeros(3,3); Q_e = 1.0; R = 1e-6; x

Example Application: Walking Pattern Generation p x q Walking Joint d CoM d Footstep Pattern Position IK planning Generator Control Preview Control: T = 5 ms N = 400 Q_x = zeros(3,3); Q_e = 1.0; R = 1e-6; x

Properties of Preview Control • Efficient implementation, controller design can be computed offline • Allows to incorporate predictive information • ZMP contstraints are not considered explicitely • Trajectory based approach Extensions – Model predictive control (handle zmp constraints explicitely, optimization over a finite control horizon) – Trajectory generation  feedback control – Dynamic filter

Feedback Stabilization swing foot trajectory position/velocity controlled robot x  ref  p q Walking Joint x Robot Feedback Inverse d d Footstep d Pattern Position Dynamics Stabilization  Kinematics q planning Generator p Control d ref q x Forward Kinematics F , R F p L ZMP Computation Control law for stabilization:        x x K ( x x ) K ( p p ) d ref X d ref P ref     K K 0 Stability condition [* ]: X P Stability condition [* ]: [Choi, Kim, Oh, and You, Posture/Walking Control for Humanoid Robot Based on Kinematic Resolution of CoM Jacobian With Embedded Motion , TRO, 2007].

Dynamic filter Correction of the error due to model simplification Requires computation of the multi-body dynamics  p x p p q Robot ref Preview Inverse d Footstep d Dynamics Control Kinematics planning x ref Preview Inverse Control Kinematics

DLR-Biped ZMP basierte Gangregelung Präsentiert auf der Industriemesse Automatica, Juni 2010

Overview Part I: Modeling Part II: Balancing Part III: Walking Control 1. Walking pattern generation 2. Feedback control