Control of Full Body Humanoid Push Recovery Using Simple Models

Control of Full Body Humanoid Push Recovery Using Simple Models

Benjamin StephensThesis Proposal

Carnegie Mellon, Robotics InstituteNovember 23, 2009

Committee:Chris Atkeson (chair)

Jessica HodginsHartmut Geyer

Jerry Pratt (IHMC)

Benjamin Stephens | Carnegie Mellon University | Control of Full-Body Humanoid Push Recovery Using Simple Models

2


3

Thesis Proposal OverviewSimple models can be used to simplify control of full-body push recovery for complex robots

RxF

x

Lx

y

RyFLzF

RzF

LyF LxF

x y

z

LyRx

Ry

refp

fp

0fp

Strategy decisions and optimization over future

actions

Simple approximate

dynamics model with COM and two

feet

Reactive full-body force

control


4

Motivations•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 balance disabilities


5

Approaches to Humanoid Balance

Controls Complex

Robot

Utilizes Simple

Model(s)

Reactive to

Pushes

Optimizes Over the Future

ZMP Preview Control S. Kajita, et.al., ‘03

Reflexive ControlPratt, ‘98Yin, et. al., ’07Geyer ‘09

Passive Dynamic WalkingMcGeer ’90

Inverse-Dynamics-Based ControlHyon, et. al., ’07Sentis, ‘07

Proposed Work

Exam

ples


6

Expected Contributions•Analytically-derived bounds on balance

stability defining unique recovery strategies

•Optimal control framework for planning step recovery and other behaviors involving balance

•Transfer of dynamic balance behaviors designed for simple models to complex humanoid through force control


7

Outline•Simple Models of Biped Balance

•Push Recovery Strategies

•Optimal Control Framework

•Humanoid Robot Control

•Proposed Work and Timeline

refp

fp

0fp


8






refp

fp

0fp


9






refp

fp

0fp


10






refp

fp

0fp


11






refp

fp

0fp


12






refp

fp

0fp


13

Simple ModelsVery simple dynamic models approximate full body motion

refp

fp

0fp


•The sum of forces on the COM results in an acceleration of the COM

Simple Biped Dynamics

14

gF

RF LF

PF

gF

RF

LF

P~P Center of mass (COM)

PF

RP LP~, LR PP Foot locations

gPi FPmFF


•The COP is the origin point on the ground of the force that is equivalent to the contact forces


15

gF

PF

gF

P

eqF

CP

eqF

~CP Center of pressure (COP)

PF

RP LP

gPi FPmFF


•Ground torques can be used to move the COP or apply moments to the COM


16

gF

PF

gF

PF

PCP

eqF

HMFPP iii

~H Angular momentum

eqF

RP LP

gPi FPmFF


•The base of support defines the limits of the COP and, consequently, the maximumforce on the COM


17

gF

PF

gF

P

eqF

CP

gPi FPmFF PF

eqF

RP LP

HMFPP iii


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


18

gF

PF

PCP

eqF

RP LP

H

FPm

MFMF

IPPIPPII g

L

L

R

R

LR00


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 the

feet•Friction

H

FPm

MFMF

IPPIPPII g

L

L

R

R

LR00

bAF

19


20

3D Linear Biped Model•The Linear Biped Model is a special case

derived by making a few additional assumptions:▫Zero vertical acceleration▫Sum of moments about COM is zero▫Forces/moments are distributed linearly

P

P

L

L

R

R

L

L

R

R

MF

MFMF

00

00

1 LR

0

0 g

P

P

LLRR

FPmMF

IPPPPI

REFERENCE:Stephens, “3D Linear Biped Model for Dynamic Humanoid Balance,” Submitted to ICRA 2010


21

Linear Double Support Region•Using a fixed double support-phase

transition policy, the weights can be defined by linear functions

Ry

Rx

Ly

Lx

y

x

yx

11yx

Ryx

Rotated Coordinate Frame

DyD

L 2

DyD

R 2

Linear Weighting Functions

D2

REFERENCE:Stephens, “Modeling and Control of Periodic Humanoid

Balance using the Linear Biped Model,” Humanoids 2009


22

Using Linear Biped Model•Analytic solution of contact forces and

phase transition allows for explicit modeling of balance control.


23

Push Recovery Strategies For Simple ModelsSimple model dynamics define unique human-like recovery strategies

refp

fp

0fp


Three Basic Strategies•From simple models, we can describe

three basic push recovery strategies that are also observed in humans

1. 2. 3.

24


25

Ankle StrategyAssumptions:

▫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 PPLmgF


26

Ankle Strategy

COM Position

CO

M

Velo

city

minmaxCC PP

LgPPP

Lg

max2

minCC PPPP

Linear constraints on the COP define a linear stability region for which the ankle strategy is stable

REFERENCE:Stephens, “Humanoid Push Recovery,” Humanoids 2007


27

Hip StrategyAssumptions:

▫Zero vertical acceleration▫Treat COM as a flywheel

Constraints:▫Flywheel “angle” has limits

gF

PF

PCP

eqF

RP LP

L

PPLmgF CP

Im,

REFERENCE:•Pratt J, Carff J., Drakunov S., Goswami A., “Capture Point: A Step toward Humanoid Push Recovery” Humanoids, 2006


28

Hip Strategy

COM Position

CO

M

Velo

city

Linear bounds for the hip strategy are defined by assuming bang-bang control of the flywheel to maximum angle

Stephens, “Humanoid Push Recovery,” Humanoids 2007


Stepping• Stepping can move the base of support to

recover from much larger pushes. Simple models can predict step time, step location and the number of steps required to recover balance.

1. 2. 3. 4.

11, xx

CxSxCOM Position

CO

M V

eloc

ity

0

29

22 , xx

33 , xx




recover from much larger pushes. Simple models can predict step time, step location and the number of steps required to recover balance.

1. 2. 3. 4.

11, xx

CxSxCOM Position

CO

M V

eloc

ity

0

30

22 , xx

33 , xx




recover from much larger pushes.

1. 2. 3. 4.

11, xx

CxSxCOM Position

CO

M V

eloc

ity

0

31

22 , xx

33 , xx



32

Stepping• Analytic models can predict step time, step

location and the number of steps required to recover balance.

-0.7 -0.6 -0.5 -0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3-0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

x position

y po

sitio

n

Reaction RegionLocation of COP during capture swing phase

Capture RegionLocation of capture step that results in stable recovery



33

Strategy State Machine•Analytic push recovery strategies can be

incorporated into a finite state machine framework that then generates appropriate responses.

? Ankle Strategy

HipStrategy

SteppingSimple Model Look-up


34

Optimal Control For Simple Model Push RecoveryEfficient optimal control performed on simple models approximates desired behavior of the full system. refp

fp

0fp


Optimal Control of Simple Model• The dynamics of the simple model can be used to

efficiently perform optimal control over an N-step horizon.

ttt

t

t

t

t

t

t

BuAxpTTT

ppp

TTT

ppp

2

32

1

1

1

5.0166.0

10010

5.01t

t

t

t

t Cxppp

gLz

01

t

Nt

t

t

t Ux

z

zz

Z DC

0

1

t

Nt

t

t

Nt

t

t

t Ux

u

uu

x

x

xx

X BABA

0

1

10

2

1

1

N-step LIPM Dynamics

N-step COP Output

LIPM Dynamics

COP Output

35

REFERENCE: •Kajita, S., et. al., "Biped walking pattern generation by using preview control of zero-moment point," ICRA 2003


36

Optimal Control of Simple Model• Given footstep location, optimal

control can solve for the optimal trajectory of the COM

Objective Function

222

1

2

21

ttreftt

reft

N

treft duppcppbzzaJ

021 JUfHUUJ t

Tt

Tt

t

Tt

TtUt UfHUUU

t 21minarg

reftz

tp

REFERENCE: •Wieber, P.-B., "Trajectory Free Linear Model Predictive Control for Stable Walking in the Presence of Strong Perturbations," Humanoid Robots 2006


Optimal Control for Stepping• Footstep location can be added to the

optimization to determine optimal step location and COM trajectory.

reftz

tp

refp

fp

0fp

222

1

2

21

ttreftt

reft

N

treft duppcppbzzaJ

ffreft ppp 02

1pU

reftf

reft ppz 1UU 00

1

11

pU

0

11

0 U

1

00

1 U

021 J

pU

fpU

HpU

Jf

tT

f

tT

f

t

0reftp

37

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


-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

COMZMP

Optimal Step Recovery (Example)

refp

fp

0fp

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

0

0.05

0.1

0.15

posi

tion

xy

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.1

0

0.1

0.2

0.3zm

p


39

Optimization of Swing Trajectory•The optimization can be

augmented to generate natural swing foot trajectories. xF

bp

fp

p

fF

fF

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

-0.15

-0.1

-0.05

0

0.05

0.1

0.15

0.2

x

y

COMZMPSwing Foot

0 5 10 15 20 250

0.05

0.1

0.15

0.2

posi

tion

Swing Foot Trajectory

xy

0 5 10 15 20 250

0.5

1

velo

city

0 5 10 15 20 25-10

-5

0

5

10

forc

e

timesteps


40

Optimization of Torso Lean• Similarly, a third mass corresponding to the

torso can be added. This can be used to model small rotations of the torso and hip strategies.

xF

p

bff

ffff pp

Lgm

Fpm

btt

tttt pp

LgmFpm

tb

tf

b

f

bb

tt

ff

bb

pmmp

mm

pmmp

pmmp

mm

pmmp


41

Angular Momentum Regulation•Large angular momentum about the COM

must be dissipated quickly to regain balance

•There are two simple possibilities for dissipating angular momentum:

HKH Hdes

Asymptotically decrease angular momentum using a

fixed controller

TNTt

Tt

Tt HHH ,,, 1H

Include change of angular momentum in the optimization

REFERENCE:M. Popovic, A. Hofmann, and H. Herr, "Angular momentum regulation during human walking: biomechanics and control,“ ICRA 2004


42

Minimum Variance Control• As opposed to minimizing jerk trajectories, it has

been suggested that a more human-like objective function minimizes the variance at the target.

REFERENCE:•Harris, Wolpert, “Signal-dependent noise determines motor planning” Nature 1998

1

0

11t

i

Ti

iti

itt BuABuAXCov

refp

fp

0fp

N

it uwXCovJ 21


43

Humanoid Robot Control Using Simple ModelsDynamics, strategies and optimal control of simple models can be combined to control full-body push recovery refp

fp

0fp


44

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

RyFLzF

RzF

LyF LxF

x y

z

LyRx

Ry

Variable FixedContact Force Selection

?


45

General Humanoid Robot Control

FJJ

INN

qx

MMMM

T

Tb

2

1

2

1

2221

1211 0

021

qx

JJ b

Dynamics

Contact constraints

Desired COM Motion

des

gdes

L

L

R

R

LR HFPm

MFMF

IPPIPPII

00

Control Objectives

Pose Bias qqKqqK desd

desp

Lx

RyFLzF

RzF

LyF LxF

LyRx

Ry



46

General Humanoid Robot Control

Lx

RyFLzF

RzF

LyF LxF

LyRx

Ry


qqKqqK

HP

qJxJqJxJ

CGCG

FF

qx

I

DDDD

JJJJ

JJIMMJJMM

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

000000

000000

0


WxxbAxbAxx TT

x minarg

dCx

General Solution To Inverse Dynamics

•Fully general solution•Many “weights” to tune•May choose undesirable forces

Weighted least- squares solution

Linear Inequality Constraints:•COP under the feet•Friction



48

Feed-forward Force Inverse Dynamics• Pre-compute contact forces using simple model

and substitute into the dynamics

qJxJFJNFJN

qx

JJIMM

MM

b

T

Tb

21

22

11

21

2221

1211

0

0

bxA

Linear System

•Easier to solve•Less “weights” to tune•More model/task-specific•Pre-computing forces may be difficult



49

Simple Model Policy-Weighted Inverse Dynamics•Automatically generate weights according

to the optimal controller.▫2nd order model of the value function

determines cost function for applying non-optimal controls. tttttt uxfVuxLuxQ ,,,

uuQuuuu optuu

Toptoptu

21Q cost



50

des

gdes

L

L

R

R

LR HFPm

MFMF

IPPIPPII

00

Simple Model Policy-Weighted Inverse Dynamics•Using the simple model, the cost function

can be converted into weights on inverse dynamics.


optuu

uuQuuuu optuu

Toptoptu

21Q cost


Task Control During Balance•Modeled as a virtual external force/torque

on the system

task

taskg

LR MFFPm

FIPPIPP

II0

00

task

taskTtask

Ttask

T

Tb

MF

JJ

FJJ

INN

qx

MMMM

2

1

2

1

2

1

2221

1211 0

Virtual COM Dynamics

Virtual Humanoid Dynamics

51

taskF

REFERENCE:•Pratt J., et.al., “Virtual Actuator Control," IROS 1996


52

Simulation of Full Body Push Recovery


53

Robot Push Recovery Experiments


54

Proposed Workrefp

fp

0fp


55

Proposed Work•Implementation of human-like push

recovery strategies on the Sarcos humanoid robot


56

Proposed Work

refp

fp

0fp

• Simple model dynamics• Simple model inverse dynamics

• Standing balance strategies• Stepping strategies• Strategy switching state machine• Optimal control of stepping• Extensions to model (swing leg dynamics, hip strategy, etc.)• Sequential quadratic programming to determine optimal step time• 2nd order optimization generating local value function approximation• Full-body inverse kinematics tracking of optimal plan• Force feed-forward inverse dynamics for standing balance• Force feed-forward inverse dynamics for stepping• Policy-weighted inverse dynamics• Integral control for robustness

CompletedIn ProgressTo be completed


57

Receding Horizon Control of Simple Model•The full body will not exactly agree with

the simple model , but by re-optimizing over a receding horizon, control can be robust to small errors.


58

2nd Order Optimization of Simple Model•A 2nd order optimization method produces

a local approximation of the value function along the trajectory

Goal

Initial State

Local 2nd order model of value function

Optimal Trajectory

The 2nd order model describes the relative cost of applying an action other than the optimal action

Simple Model Policy-Weighted Inverse Dynamics

tttttt uxfVuxLuxQ ,,,

uuQuuuu optuu

Toptoptu

21Q cost


59

Sequential Quadratic Programming•SQP used to solve non-linear problems:

▫Step Time Optimization Existing optimal control framework is only

linear if a fixed step time is assumed.▫Double Support Constraints Because the step location is variable, the true

double support constraints are nonlinear.

•Analytic models can be used to estimate fixed values or provide good initial guesses.


60

Integral Balance Control•Integral Balance Control, related to 2nd-

order sliding mode control, was previously applied to control of humanoid balance.

•Can this method be used to transfer robust control of simple system to the full body?

REFERENCE:•Stephens, “Integral Control of Humanoid Balance," IROS 2007•Levant, “Sliding order and sliding accuracy in sliding mode control”, Journal of Control, 1993

uxhy , u

uhuxf

xhy

,

uxfx ,

uxfxhy

uhu des ,

1


61

Timeline•November ‘09 – Thesis Proposal

▫6 months – Controller theory/refinement 1 month – Open loop planning 2 months – Receding horizon planning 3 months – Policy-weighted inverse dynamics

▫4 months – Experiments 1 month - Step recovery robot experiments 2 month - Multiple strategy robot

experiments 1 month – Comparison to human experiments

▫2 months – Thesis writing•December ‘10 - Defense


62

Conclusionrefp

fp

0fp


63

Thesis Proposal OverviewSimple models can be used to simplify control of full-body push recovery for complex robots

RxF

x

Lx

y

RyFLzF

RzF

LyF LxF

x y

z

LyRx

Ry

refp

fp

0fp

Strategy decisions and planning over future actions

Simple approximate

dynamics model with COM and two

feet

Reactive full-body force

control


64

Acknowledgements•Committee:

▫Chris Atkeson (Advisor/Chair)▫Jessica Hodgins▫Hartmut Geyer▫Jerry Pratt (IHMC/External)

•Stuart Anderson•People who helped with practice talk

Questions?

Control of Full Body Humanoid Push Recovery Using Simple Models

Documents

Transcript of Control of Full Body Humanoid Push Recovery Using Simple Models