Model Predictive Control for Humanoid Balance and Locomotion

Benjamin StephensRobotics 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

fp

0p1p

2p

0

refX

COMCOP

1

2

Linear Inverted Pendulum Model

Assumptions:– Zero vertical acceleration– No torque about COM

Constraints:– COP within the base

of support

gF

PF

PCP

eqF

RP LP

REFERENCE:Kajita, S.; Tani, K., "Study of dynamic biped locomotion on rugged terrain-derivation and application of the linear inverted pendulum mode," ICRA 1991

CP PPL

mgF

LIPM State Space Dynamics

ttt BUAXX 1

tt

t

t

t

t

t

Z

TZ

V

P

TL

gTL

gT

Z

V

P

0

0

100

1

01

1

1

1

LIPM State Space Trajectories

t

t

t

NN

t

NNt

t

t

Z

Z

Z

BBABA

BAB

B

X

A

A

A

X

X

X

1

22

1

0

00

000

ttt X BUAX

ttt X UBAP PP

ttt X UBAV VV

ttt X UBAZ ZZ

Optimal Control Objective

tTtreft

TreftJ RUUXXQXX

2

1

2

1

tTtreftt

Treftt XXJ RUUXBUAQXBUA

2

1

2

1

tT

refttTT

t XJ QBUXAUQBBRU 2

1

tT

tTtJ UfHUU

2

1

Optimal Control Constraints

XX dXC t

XX dBUAC ttX

tt XACdBUC XXX

tt XUU dUC

Optimal Control of WalkingObjective Function

N

ttreftt

reftt

reft ZdVVcPPbZZaJ

1

2222

2

1

tT

tTtt

t

UfHUUUU 2

1minarg ref

tz

tp

•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

tT

tTtJ UfHUU

2

1

Optimal Control with Foot Placement

0

0

0

1

1

0

S

f

tT

f

t

T

f

tJ

P

Uf

P

UH

P

U

2

1Time of step is encoded in U0 and U1

Diedam, H., et. al., "Online walking gait generation with adaptive foot positioning through Linear Model Predictive control," IROS 2008

ff

f

f

f

freft p

p

p

p

p PSSSSZ 11

00

3

2

1

00

100

010

001

000

000

1

S

reftz

tp 0fp

1fp

3fp

2fp

Next 3 Footsteps:

Optimal Step RecoveryObjective Function

reftz

tp

refp

fp ff

reft ppp 02

1

reftf

reft pp 1SSZ 00

0fp

•Must provide footstep timing•Must decide which foot to step with•Constraints in double support are nonlinear due to variable foot location

0reftV

1. 2. 3.

f

tT

f

t

T

f

tJ

P

Uf

P

UH

P

U

2

1

-0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35

-0.15

-0.1

-0.05

0

0.05

0.1

0.15

0.2

COM

ZMP

Optimal Step Recovery

refp

fp

0fp

a=1e-6 b=0.1 c=0.01 d=1e-6 X0 = [0,0,0.4,-0.1] T=0.05 Tstep=0.4 N=20

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1-0.1

0

0.1

0.2

0.3

posi

tion

x

y

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1-0.2

0

0.2

0.4

0.6

velo

city

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1-0.2

0

0.2

0.4

0.6zm

p

-0.2 -0.1 0 0.1 0.2 0.3

-0.15

-0.1

-0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

-0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25

-0.15

-0.1

-0.05

0

0.05

0.1

0.15

Tdsp = 0.0s Tstep = 0.45s Tdsp = 0.1s Tstep = 0.35s

Initial double support phase

Re-planning after each step (3-step)

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0 20 40 60 80 100 120 140 160 180-0.2

0

0.2

0.4

0.6

0.8

1

1.2

0 0.2 0.4 0.6 0.8 1 1.2 1.4-0.6

-0.4

-0.2

0

0.2

0.4

COM

ZMP

0 20 40 60 80 100 120-0.5

0

0.5

1

1.5

0 20 40 60 80 100 120-0.2

0

0.2

0.4

0.6

Walking

Outline

• Optimal Control Formulation

• Humanoid Robot Control

• Examples and Problems

Outline

• Humanoid Robot Control

Use MPC inside feedback loopto generate desired contactforces and joint torques

• Instantaneous 3D biped dynamics form a linear system in contact forces.

Simple Biped Dynamics

21

gF

PF

PZ

eqF

RP LP

H

FPm

M

F

M

F

IPPIPP

II g

L

L

R

R

LR

00

~P Center of mass (COM)

~, LR PP Foot locations~H Angular momentum~Z Center of pressure (COP)

Simple Biped Inverse Dynamics

• The contact forces can be solved for generally using constrained quadratic programming

WFFbAFbAFF TT

F minarg

dCF

Least squares problem(quadratic programming)

Linear Inequality Constraints•COP under each foot•Friction

H

FPm

M

F

M

F

IPPIPP

II g

L

L

R

R

LR

00bAF

22

Controlling a Complex Robot with a Simple Model

• Full body balance is achieved by controlling the COM using the policyfrom the simple model.

• The inverse dynamics chooses from the set of valid contact forces the forcesthat result in the desired COM motion.

RxF

x

Lx

y

RyF

LzF

RzF

LyFLxF

xy

z

LyRx

Ry

General Humanoid Robot Control

FJ

J

IN

N

q

x

MM

MMT

Tb

2

1

2

1

2221

1211 0

021

q

xJJ b

Dynamics

Contact constraints

Desired COM Motion

des

gdes

L

L

R

R

LR H

FPm

M

F

M

F

IPPIPP

II

00

Control Objectives

Pose Bias qqKqqK desd

desp

Lx

RyF

LzF

RzF

LyFLxF

LyRx

Ry

General Humanoid Robot Control

Lx

RyF

LzF

RzF

LyFLxF

LyRx

Ry

qqKqqK

H

P

qJxJ

qJxJ

CG

CG

F

F

q

x

I

DD

DD

JJ

JJ

JJIMM

JJMM

desd

desp

des

des

b

b

R

L

b

RL

RL

RR

LL

TR

TL

TR

TL

21

21

22

11

22

11

21

21

222221

111211

0000

000

000

000

000

0

Feed-forward Force Inverse Dynamics

• Pre-compute contact forces using simple model and substitute into the dynamics

qJxJ

FJN

FJN

q

x

JJ

IMM

MM

b

T

Tb

21

22

11

21

2221

1211

0

0

Lx

RyF

LzF

RzF

LyFLxF

LyRx

Ry

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

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.1

-0.05

0

0.05

0.1

0.15

Time(s)

Y

COM-Y

COM-Y-D

STEP-Y-DFOOT-Y

FOOT-Y-D

Unperturbed Walking In Place

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.15

-0.1

-0.05

0

0.05

0.1

0.15

Time(s)

Y

COM-Y

COM-Y-D

STEP-Y-DFOOT-Y

FOOT-Y-D

Large Mid-Swing Push While Walking in Place

Extensions

• Different Models– Swing Leg– Torso– Angular Momentum

• Different Objective Functions– Capture Point– Minimum Variance Control

• Step Time Optimization

xF

bp

fp

p

fF

fF

xF

p

Open Problems

• Learning from experience

• Using human motion capture

• Higher-level planning

• State Estimation and Localization