Transcript humanoids2010poster

Push Recovery by Stepping for Humanoid Robots with Force
Controlled Joints
Benjamin J. Stephens, Christopher G. Atkeson
http://www.cs.cmu.edu/~bstephe1
Overview
Full Body Control
This paper presents a step recovery controller
using online optimal control to simultaneously
determine desired center of mass (COM)
acceleration and desired footstep locations that
is applied to a force-controlled humanoid robot.
This force-based controller allows the robot to
be compliant and at the same time recover from
disturbances such as large pushes.
Robot control is achieved by Dynamic Balance
Force Control[2] using COM accelerations
provided by PR-MPC combined with inverse
kinematics using low-gain PD servos.
Push Recovery Model Predictive Control
PR-MPC simultaneously solves for a trajectory
of the COM and footstep locations using
optimal control[1]. The goal state places the
COM between the two feet after the step with
zero velocity.
Example trajectory
output by PR-MPC
0.25
0.2
X (m)
0.15
0.1
0.05
-0.05
-0.1
0.2
0.1
0
Y (m)
-0.1
-0.2
The dynamics of the system are represented by
a Linear Inverted Pendulum model,
T
1
0
0  Pt   0 
g     
 T  Vt    0  Z t 
L  
1  Z t  T 
P ~ COM Position
V ~ COMVelocity
Z ~ Center of Pressure
T ~ MPC Timestep
giving the trajectory over the next N timesteps:
 X t 1   A 
 B

  2

X
AB
 t 2   A 

       Xt   

  N
 N
X  A
A B
 tN   
0  Z t 
 

B
0 0  Z t 




 0 
 

A N 1 B  B  Z t 
0
0
Pt  A P X t  B P Ut
Vt  A V X t  B V Ut
Zt  A Z X t  B Z Ut
Footstep locations
are included in the
optimization
The goal location
places COM in the
center of the two
feet after the
step(s)
PL
PL (t )
PR
Real-time PR-MPC
2
w3
w4 ref
2

U 
Pf  Pf
2
2

2
U  arg min U HU  f U
T
T
U
s.t. DU  d
Feed-forward joint torques are computed by
considering both the full floating body dynamics
and the desired COM accelerations given by
PR-MPC. The system of equations is solved
with constraints to keep the center of pressure
(COP) under each foot.
 M 11 M 12

 M 21 M 22
 J L1 J L 2

 J R1 J R 2
 0
0

0
 0
0
J
I
J
0
0
0
0
0
0
I
PL  P 

  G1 

 xb  
J
 q    G2 
des 




P
0
L



 ff   des 
0
 F   PR 
L
P
des 



I
0   

 FR  

0
PR  P  I 


J
T
L1
T
L2
0
I
T
R1
T
R2
M ~ inertia matrix
G ~ gravitatio nal
 ff ~ torques
P ~ center of mass
PL , PR ~ foot positions
Floating Body Inverse Kinematics
When combined with a quadratic cost function,
the solution simplifies to a quadratic
programming problem:
2
w1 goal
w2
J
P P 
V
2
2
F̂
Floating Body Force Control
0
 Pt 1   1

 g
 Vt 1    T
Z  L
 t 1   0
P (t )
PR-MPC is solved online at a rate of 50Hz using
the current state of the humanoid robot. Between
evaluations, the desired COM acceleration is
used to generate desired full body torques.
COM
COP
0.3
P
Linear inequality
constraints keep the COP
in the base of support
Full-body reference poses are generated using
floating body inverse kinematics[3]. Low-gain
PD controls are added to the feed-forward joint
torques.
   ff  K p q
des

 q  Kd q
[1] H. Diedam, D. Dimitrov, P. Wieber, K. Mombaur, and M. Diehl, "Online Walking Gait Generation with
Adaptive Foot Positioning Through Linear Model Predictive Control,“ International Conference on
Intelligent Robots and Systems, IEEE, 2008, pp. 1121-1126.
[2] Stephens, Benjamin J, and Christopher G Atkeson. “Dynamic Balance Force Control for Compliant
Humanoid Robots.” In International Conference on Intelligent Robots and Systems, 2010.
[3] Mistry, M, J Nakanishi, G Cheng, and S Schaal. “Inverse kinematics with floating base and
constraints for full body humanoid robot control.” International Conference on Humanoid Robots, 2008.