Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.
-
date post
19-Dec-2015 -
Category
Documents
-
view
216 -
download
1
Transcript of Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.
![Page 1: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/1.jpg)
Model Predictive Control for Humanoid Balance and Locomotion
Benjamin StephensRobotics Institute
![Page 2: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/2.jpg)
![Page 3: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/3.jpg)
Compliant Balance and Push Recovery
• Full body compliant control
• Robustness to large disturbances
• Perform useful tasks in human environments
![Page 4: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/4.jpg)
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
![Page 5: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/5.jpg)
Outline
• Optimal Control Formulation
• Humanoid Robot Control
• Examples and Problems
![Page 6: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/6.jpg)
Outline
• Optimal Control Formulation
Formulate balance and foot placement control as an optimal control problem
fp
0p1p
2p
0
refX
COMCOP
1
2
![Page 7: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/7.jpg)
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
![Page 8: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/8.jpg)
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
![Page 9: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/9.jpg)
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
![Page 10: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/10.jpg)
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
![Page 11: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/11.jpg)
Optimal Control Constraints
XX dXC t
XX dBUAC ttX
tt XACdBUC XXX
tt XUU dUC
![Page 12: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/12.jpg)
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
![Page 13: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/13.jpg)
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:
![Page 14: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/14.jpg)
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
![Page 15: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/15.jpg)
-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
![Page 16: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/16.jpg)
-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
![Page 17: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/17.jpg)
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
![Page 18: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/18.jpg)
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
![Page 19: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/19.jpg)
Outline
• Optimal Control Formulation
• Humanoid Robot Control
• Examples and Problems
![Page 20: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/20.jpg)
Outline
• Humanoid Robot Control
Use MPC inside feedback loopto generate desired contactforces and joint torques
![Page 21: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/21.jpg)
• 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)
![Page 22: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/22.jpg)
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
![Page 23: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/23.jpg)
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
![Page 24: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/24.jpg)
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
![Page 25: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/25.jpg)
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
![Page 26: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/26.jpg)
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
![Page 27: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/27.jpg)
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.
![Page 28: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/28.jpg)
Outline
• Optimal Control Formulation
• Humanoid Robot Control
• Examples and Problems
![Page 29: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/29.jpg)
![Page 30: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/30.jpg)
![Page 31: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/31.jpg)
![Page 32: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/32.jpg)
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
![Page 33: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/33.jpg)
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
![Page 34: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/34.jpg)
![Page 35: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/35.jpg)
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
![Page 36: Model Predictive Control for Humanoid Balance and Locomotion Benjamin Stephens Robotics Institute.](https://reader030.fdocuments.in/reader030/viewer/2022032703/56649d2c5503460f94a01fdf/html5/thumbnails/36.jpg)
Open Problems
• Learning from experience
• Using human motion capture
• Higher-level planning
• State Estimation and Localization