Transcript Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute Compliant Balance and Push Recovery • Full body compliant control • Robustness to large disturbances •

Model Predictive Control for
Humanoid Balance and Locomotion
Benjamin Stephens
Robotics Institute
Compliant Balance and Push Recovery
• Full body compliant
control
• Robustness to large
disturbances
• Perform useful tasks in
human environments
Motivation
• Improve the performance and usefulness of
complex robots, simplifying controller design by
focusing on simpler models that capture
important features of the desired behavior
• Enabling dynamic robots to interact safely with
people in everyday uncertain environments
• Modeling human balance sensing, planning and
motor control to help people with disabilities
Outline
• Optimal Control Formulation
• Humanoid Robot Control
• Examples and Problems
Outline
• Optimal Control Formulation
Formulate balance and foot
placement control as an
optimal control problem
COM
COP
2
pf
p2
X ref
1
p1
p0
0
Linear Inverted Pendulum Model
FP
Assumptions:
– Zero vertical acceleration
– No torque about COM
mg
FP  P  PC 
L
Fg
Feq
Constraints:
– COP within the base
of support
REFERENCE:
PC PR
Kajita, S.; Tani, K., "Study of dynamic biped locomotion on rugged terrain-derivation and
application of the linear inverted pendulum mode," ICRA 1991
P
PL
LIPM State Space Dynamics
X t 1  AXt  BUt
0  Pt   0 
 Pt 1   1 T

 g
g     
 Vt 1    T 1  T  Vt    0  Z t 
L  
Z  L
0
1  Z t  T 
 t 1   0
LIPM State Space Trajectories
Xt  AX t  BUt
 X t 1   A 
 B

  2

AB
 X t 2   A 


X

t
     
 

  N
 N
X 
A B
 tN   A 
0  Z t 
 

B
0 0  Z t 

 0   
 

A N 1 B  B  Z t 
0
Pt  AP X t  BP Ut
Vt  AV X t  BV Ut
Zt  AZ X t  BZ Ut
0
Optimal Control Objective
1
1 T
T
J  Xt  X ref  QX t  X ref   U t RU t
2
2
1
1 T
T
J  AX t  BU t  X ref  QAX t  BU t  X ref   U t RU t
2
2


1 T
T
T
J  U t R  B QB U t  AX t  X ref  QBUt
2
1 T
J  U t HU t  f T U t
2
Optimal Control Constraints
CX Xt  dX
CX AX t  BUt   d X
CXBUt  d X  CX AX t
CU Ut  dU  X t 
Optimal Control of Walking
Objective Function

 
 

2
2
2
1 N
ref
ref
ref
J   a Zt  Zt  b Pt  Pt  c Vt  Vt  dZt2
2 1

1 T
T
J  U t HU t  f U t
2
1 T

T
U  arg min Ut HUt  f Ut 
Ut
2


t
ztref
pt
•Must provide footstep locations and timings
•Double support is largely ignored
Wieber, P.-B., "Trajectory Free Linear Model Predictive Control for Stable Walking in the Presence of Strong
Perturbations," Humanoid Robots 2006
Optimal Control with Foot Placement
Next 3 Footsteps:
 pf1 


Ztref  S 0 p f 0  S1  p f 2   S 0 p 0f  S1P f
p 
 f3
1  Ut 
J   
2  Pf 
T
 Ut  T  Ut 
H   f  
 Pf   Pf 
1
0
 

1
0


 

0
1
S 0    S1  


0
0
 


 

0
0
 

0

0


0
 
0


1 
0
0

0

1

0
pf 3
pf 2
Time of step is encoded in U0 and U1
pf1
ztref
Diedam, H., et. al., "Online walking gait generation with adaptive foot
positioning through Linear Model Predictive control," IROS 2008
pt
pf 0
Optimal Step Recovery
Objective Function
ref
t
p
Vt
ref
1.
1
 p f 0  p f 
2
3.
2.
pf
0
Ztref  S0 p f 0  S1 ptref
1  Ut 
J   
2  Pf 
T
 Ut  T  Ut 
H   f  
 Pf   Pf 
ztref
pref
pt
pf 0
•Must provide footstep timing
•Must decide which foot to step with
•Constraints in double support are nonlinear due to variable foot location
Optimal Step Recovery
0.3
0.2
0.2
position
pf
0.15
0.1
x
y
0
-0.1
0.1
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
0.6
0.05
velocity
0.4
pref
0
0.2
0
-0.2
-0.05
0.6
0.4
zmp
-0.1
pf 0
-0.15
-0.1
-0.05
0
0.05
COM
ZMP
0.1
0.15
0.2
a=1e-6 b=0.1 c=0.01 d=1e-6
0.25
0.3
0.35
0.2
0
-0.2
X0 = [0,0,0.4,-0.1]
T=0.05 Tstep=0.4 N=20
Initial double support phase
Tdsp = 0.0s Tstep = 0.45s
Tdsp = 0.1s Tstep = 0.35s
0.15
0.3
0.25
0.1
0.2
0.05
0.15
0.1
0
0.05
-0.05
0
-0.05
-0.1
-0.1
-0.15
-0.15
-0.2
-0.1
0
0.1
0.2
0.3
-0.1
-0.05
0
0.05
0.1
0.15
0.2
0.25
Re-planning after each step (3-step)
1.2
0.4
1
0.3
0.2
0.8
0.1
0.6
0
0.4
-0.1
0.2
-0.2
-0.3
0
-0.4
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
-0.2
0
20
40
60
80
100
120
140
160
180
Walking
1.5
1
0.4
0.5
0
0.2
-0.5
0
20
40
60
80
100
120
0
20
40
60
80
100
120
0
0.6
-0.2
0.4
0.2
-0.4
COM
ZMP
-0.6
0
0.2
0.4
0.6
0.8
1
1.2
1.4
0
-0.2
Outline
• Optimal Control Formulation
• Humanoid Robot Control
• Examples and Problems
Outline
• Humanoid Robot Control
Use MPC inside feedback loop
to generate desired contact
forces and joint torques
Simple Biped Dynamics
• Instantaneous 3D biped dynamics form a
FP
linear system in contact forces.
 FR 


  F 
I
0
I
0 M R   mP

g



 P  P   I  P  P   I   F  


L
 R
 L   H 
M 
 L
Z ~ Center of pressure (COP)
H ~ Angular momentum
PR , PL ~ Foot locations
P ~ Center of mass (COM)
Fg
Feq
Z PR
P
21
PL
22
Simple Biped Inverse Dynamics
• The contact forces can be solved for generally
using constrained quadratic programming
 FR 


  F 
I
0
I
0 M R   mP

g



 P  P   I  P  P   I   F  


L
 R
 L   H 
M 
 L

F   arg min  AF  b   AF  b   F TWF
F
T
CF  d

AF  b
Least squares problem
(quadratic programming)
Linear Inequality Constraints
•COP under each foot
•Friction
Controlling a Complex Robot with a
Simple Model
• Full body balance is achieved by
controlling the COM using the policy
from the simple model.
x
y
z
x
y
• The inverse dynamics chooses from
the set of valid contact forces the forces
that result in the desired COM motion.
 Rx
FLy
FRx
FLx
FLz
FRy FRz
 Ry
 Lx
 Ly
General Humanoid Robot Control
 M 11 M 12  xb   N1  0  J1T 
Dynamics








F


M
     
T
 21 M 22  q   N 2   I   J 2 
 xb 
J1 J 2    0
Contact constraints

q
 
Control Objectives
FLy
 FR 


des  F 
I
0
I
0 M R   mP

g

P  P  I P  P  I  F   
des


L
 R
 L   H

M 
 L
  K p qdes  q Kd q des  q 
Desired
COM Motion
FLz
FRy FRz
 Rx
Pose Bias
FLx
 Ry
 Lx
 Ly
General Humanoid Robot Control
  M 11 M 12 0  J LT1  J RT1  

  G1  C1 




T 
T

  G2  C 2 
  M 21 M 22  I  J L 2  J R 2  

  J x  J q 
 J
0 
0
0
J L2
2
L1


 1 b







x
 J1 xb  J 2 q 
0   b  
0
0
  J R1 J R 2

 
 q  

  

des 
P
0 0 0 DL1 DR1 
    

 des 


0 0 0 D


 H 
DR 2 
 FL  
L2



 

 FR  







 K q des  q  K q des  q
0 0 I 0 0


d
 p





 




















FLy
FLx
FLz
FRy FRz
 Rx
 Ry
 Lx
 Ly
Feed-forward Force Inverse Dynamics
• Pre-compute contact forces using simple model and
substitute into the dynamics
 M 11
M
 21
 J1
M 12
M 22
J2
0  xb    N1  J1T F  

  

T

 I  q     N 2  J 2 F 
0      J1 xb  J2 q 
FLy
FLx
FLz
FRy FRz
 Rx
 Ry
 Lx
 Ly
Other Tasks
•
•
•
•
Posture Control
Angular Momentum Regulation
Swing Foot Control
Task Control (e.g. lifting heavy object)
Benjamin Stephens, Christopher Atkeson, "Push Recovery by Stepping for
Humanoid Robots with Force Controlled Joints,"Accepted to 2010 International
Conference on Humanoid Robots, Nashville, TN.
Benjamin Stephens, Christopher Atkeson, "Dynamic Balance Force Control for
Compliant Humanoid Robots,“ 2010 International Conference on Intelligent
Robots and Systems (IROS), Taipei, Taiwan.
Outline
• Optimal Control Formulation
• Humanoid Robot Control
• Examples and Problems
Unperturbed Walking In Place
0.15
0.1
Y
0.05
0
COM-Y
COM-Y-D
STEP-Y-D
FOOT-Y
FOOT-Y-D
-0.05
-0.1
0
0.2
0.4
0.6
0.8
1
Time(s)
1.2
1.4
1.6
1.8
2
Large Mid-Swing Push While Walking in Place
0.15
0.1
Y
0.05
0
-0.05
COM-Y
COM-Y-D
STEP-Y-D
FOOT-Y
FOOT-Y-D
-0.1
-0.15
0
0.2
0.4
0.6
0.8
1
Time(s)
1.2
1.4
1.6
1.8
2
Extensions
 Ff
pb
p
• Different Models
– Swing Leg
– Torso
– Angular Momentum
Fx
Ff
p f
• Different Objective Functions
– Capture Point
– Minimum Variance Control
p
• Step Time Optimization
Fx
Open Problems
• Learning from experience
• Using human motion capture
• Higher-level planning
• State Estimation and Localization