University of Southern California Dissertations and Theses

Estimation-based control for humanoid robots

(USC Thesis Other)

Estimation-based control for humanoid robots

PDF

Download a page range

Download transcript

Copy asset link

Request this asset

Transcript (if available)

Content Estimation-Based Control for Humanoid Robots
by
Nicholas Rotella
A Dissertation Presented to the
FACULTY OF THE GRADUATE SCHOOL
UNIVERSITY OF SOUTHERN CALIFORNIA
In Partial Fulllment of the
Requirements of the Degree
DOCTOR OF PHILOSOPHY
(Computer Science)
May 2018
Copyright 2017 Nicholas Rotella
Table of Contents
Abstract x
Acknowledgments xiii
I Introduction 1
I.1 The Current State of State Estimation . . . . . . . . . . . . . . . . . . . . . 3
I.2 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
II Background 8
II.1 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
II.1.1 The Athena Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
II.2 Robot Dynamics and Control . . . . . . . . . . . . . . . . . . . . . . . . . . 12
II.3 Quadratic Program-Based Inverse Dynamics . . . . . . . . . . . . . . . . . . 14
II.4 Model Predictive Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
II.4.1 The Linear Inverted Pendulum Model . . . . . . . . . . . . . . . . . 17
II.5 MPC-Based Walking Controller . . . . . . . . . . . . . . . . . . . . . . . . . 19
II.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
IIIBase State Estimation 24
III.1 Prediction and Measurement Models . . . . . . . . . . . . . . . . . . . . . . 26
III.1.1 Discrete, Nonlinear Model . . . . . . . . . . . . . . . . . . . . . . . . 30
III.1.2 Continuous, Linear Model . . . . . . . . . . . . . . . . . . . . . . . . 31
III.1.3 Discrete, Linear Model . . . . . . . . . . . . . . . . . . . . . . . . . . 32
III.2 Observability Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
III.3 Experiments and Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
III.4 Inverse Dynamics Experiments . . . . . . . . . . . . . . . . . . . . . . . . . 41
III.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
IVCOM and Momentum Estimation 46
IV.1 Models of Robot Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
IV.2 Estimators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
IV.2.1 Momentum Estimator . . . . . . . . . . . . . . . . . . . . . . . . . . 50
ii
IV.2.2 Oset Estimator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
IV.2.3 COP-Based Oset Estimator . . . . . . . . . . . . . . . . . . . . . . 52
IV.2.4 External Wrench Estimator . . . . . . . . . . . . . . . . . . . . . . . 53
IV.3 Observability Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
IV.3.1 Momentum Estimator . . . . . . . . . . . . . . . . . . . . . . . . . . 54
IV.3.2 Oset Estimator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
IV.3.3 COP-Based Oset Estimator . . . . . . . . . . . . . . . . . . . . . . 56
IV.3.4 External Wrench Estimator . . . . . . . . . . . . . . . . . . . . . . . 56
IV.3.5 Oset and External Wrench Estimator . . . . . . . . . . . . . . . . . 57
IV.4 Experiments and Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
IV.4.1 Momentum Estimator . . . . . . . . . . . . . . . . . . . . . . . . . . 59
IV.4.2 Oset Estimator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
IV.4.3 COP-Based Oset Estimator . . . . . . . . . . . . . . . . . . . . . . 63
IV.4.4 External Wrench Estimator . . . . . . . . . . . . . . . . . . . . . . . 63
IV.5 Closed-Loop Control Using Momentum Estimators . . . . . . . . . . . . . . 64
IV.5.1 Oset Estimators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
IV.5.2 External Wrench Estimator . . . . . . . . . . . . . . . . . . . . . . . 65
IV.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
V Joint State Estimation 76
V.1 Sensor Framework . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
V.1.1 Joint Velocity Computation from Gyroscopes . . . . . . . . . . . . . 80
V.1.2 Joint Acceleration Computation from Accelerometers . . . . . . . . 82
V.2 IMU Pose Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
V.2.1 Orientation Calibration . . . . . . . . . . . . . . . . . . . . . . . . . 84
V.2.2 Position Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
V.3 Joint State Estimators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
V.3.1 Joint Position and Gyroscope Bias Filter . . . . . . . . . . . . . . . 86
V.3.2 Acceleration-Based Joint Velocity Filter . . . . . . . . . . . . . . . . 87
V.4 Experiments and Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
V.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
VIContact State Estimation 96
VI.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96
VI.2 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98
VI.2.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98
VI.2.2 Sensing for Clustering . . . . . . . . . . . . . . . . . . . . . . . . . . 99
VI.3 Clustering Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
VI.4 Base State Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102
VI.5 Experiments and Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
VI.5.1 Contact Clustering Results . . . . . . . . . . . . . . . . . . . . . . . 104
iii
VI.5.2 Base State Estimation Threshold . . . . . . . . . . . . . . . . . . . . 106
VI.5.3 Clustering-Based State Estimation . . . . . . . . . . . . . . . . . . . 106
VI.5.4 Clustering Training Data . . . . . . . . . . . . . . . . . . . . . . . . 106
VI.5.5 IMUs for Clustering Versus Estimation . . . . . . . . . . . . . . . . . 109
VI.6 Clustering-Based Contact Estimation for Control . . . . . . . . . . . . . . . 111
VI.6.1 Closed-Loop Control using Base State Estimation . . . . . . . . . . 111
VI.6.2 Contact Probability-Based Inverse Dynamics Constraints . . . . . . 112
VI.7 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113
VIIConclusions 115
A Kalman Filtering 118
A.0.1 Handling of Rotational Quantities . . . . . . . . . . . . . . . . . . . 120
B Base State EKF Linearization 122
B.1 Quaternion Review and Conventions . . . . . . . . . . . . . . . . . . . . . . 122
B.2 Linearization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
B.2.1 Process Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
B.2.2 Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128
B.2.3 Measurement Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 132
C Observability 136
C.1 Linear Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136
C.2 Nonlinear Observability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137
D Inverse Dynamics Details 139
Bibliography 145
iv
List of Figures
I.1 Estimation involves the fusion of robot models and sensing; in our work, we
rely entirely on proprioceptive or internal sensing (right) such as joint angle
sensors, torque sensors and IMUs rather than exteroceptive or external
sensing (left) such as Vicon, GPS and terrain maps in order to develop
general-purpose methods which work in any environment. . . . . . . . . . 5
II.1 The hydraulic, torque-controlled Sarcos humanoids Hermes and Athena. . 9
II.2 Torque tracking comparison showing superior performance when running
torque control onboard at 5 kHz (right) versus oboard at 1 kHz (left) for
one joint. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
II.3 Illustration of the various quadratic program-based inverse dynamics cost
and constraint terms outlined in this section. . . . . . . . . . . . . . . . . 15
II.4 Illustration of simplication of dynamics from the full humanoid to the
LIPM. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
II.5 Simple walking state machine. . . . . . . . . . . . . . . . . . . . . . . . . . 20
II.6 Example user input mapping for an Xbox controller with multiple modes. 21
III.1 Base state estimation notation for a bipedal robot. . . . . . . . . . . . . . 27
III.2 Base state estimation augmented with the relative foot orientation mea-
surement s
z
i
corresponding to foot i for a bipedal robot. . . . . . . . . . . 28
III.3 Base state estimation notation on Hermes robot in SL simulation environ-
ment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
III.4 Position, velocity and orientation (Euler angle) estimates for the 120 sec-
ond walking task. The portions with a white background indicate the single
support phase while those with a grey background indicate the double sup-
port phase. Overall, the
at foot lter introduced in this work performs
better than the lter introduced in (Bloesch et al., 2012) as conrmed by
the estimation errors listed in Table III.5. . . . . . . . . . . . . . . . . . . 38
III.5 RMS estimation error bar plots for the Point Foot and Flat Foot lters for
the simulated walking task corresponding to those listed in Table III.5. . . 39
v
III.6 The Sarcos humanoid during a single legged squatting experiment with
perturbations. Inset are screen captures of the SL kinematics visualization
updated using the base pose estimator presented in this chapter. . . . . . 41
III.7 QP-Based Inverse Dynamics control using the estimated base pose for a
single-legged balancing task in which the COM is regulated to a desired
position over the stance foot. We disturb the robot with pushes of vary-
ing strength and duration as shown in the bottom plot and highlighted
elsewhere in grey. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
III.8 QP-Based Inverse Dynamics control using the estimated base pose for a
single-legged balancing task in which the desired COM moves sinusoidally
in the vertical direction while the swing foot is set to maintain a position.
Again, we disturb the robot during the portions of the plot having a grey
background. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
IV.1 Illustration of the momentum dynamics. . . . . . . . . . . . . . . . . . . . 49
IV.2 Estimation of COM (top row), linear momentum (middle row) and angular
momentum (bottom row); columns denote x,y and z. RMS errors for the
relevant lters are shown on each plot. . . . . . . . . . . . . . . . . . . . . 58
IV.3 Estimation of linear momentum for increasing frequencies (Top: LIPM
Filter. Bottom: Momentum Estimator) . . . . . . . . . . . . . . . . . . . . 58
IV.4 Zoomed-in views of angular momentum estimates versus low-pass ltered
kinematics measurements. . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
IV.5 Estimation of a 5cm COM oset while stationary (top) and estimation of a
conguration-dependent COM oset during the 15s walking task (bottom). 60
IV.6 Time-varying COM osets for dierentn (representing dierent degrees of
modeling error) in x-direction (top) and y-direction (bottom. . . . . . . . 60
IV.7 Estimation of COM (top row), linear momentum (middle row) and angular
momentum (bottom row) for dierent n (representing dierent degrees
of modeling error). Red denotes the largest modeling error while yellow
denotes the least. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
IV.8 COM estimation for n = 5 in x-direction (top) and y-direction (bottom). . 61
IV.9 Linear momentum estimation forn = 5 inx-direction (top) andy-direction
(bottom). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
IV.10 External wrench estimation for a constant external force in globaly-direction
while stationary (top row), while walking (middle row) and for an impul-
sive force (bottom row) while walking (with process noise values of 0:1 and
1:0) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
IV.11 External wrench estimation for an increasing simulated load at the robot
base in dierent directions. The estimated wrench is actively being com-
pensated in the inverse dynamics controller. . . . . . . . . . . . . . . . . . 67
vi
IV.12 External wrench estimation for an increasing simulated load at the robot
base in dierent directions. Load compensation prevents the robot from
deviating from its desired COM position. . . . . . . . . . . . . . . . . . . . 67
IV.13 Top: External wrench estimation for an increasing simulated load at the
robot base in thez direction during walking (approximately one-fth the
robot's weight). Bottom: COM height tracking, showing the robot going
unstable without the EWE. . . . . . . . . . . . . . . . . . . . . . . . . . . 68
IV.14 Loading of the robot during a single-leg squatting task in ve pound (20N)
increments, both with and without external wrench estimation/compensation. 69
IV.15 Loading of the robot during a single-leg squatting task in ve pound (20N)
increments while estimating the external force (top) and using it for feed-
forward compensation to prevent the COM from moving downwards (bot-
tom). Note that the time at which each weight was added is not known
precisely, thus the increasing load magnitudes are shown as horizontal lines
in the top plot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
IV.16 External wrench estimation for an increasing simulated load at the robot
base in dierent directions. Load compensation prevents the robot from
deviating from its desired COM position. . . . . . . . . . . . . . . . . . . . 72
IV.17 Perturbing the robot in thex-direction while measuring the external force
(top), resulting in poor COM tracking (bottom). . . . . . . . . . . . . . . 73
IV.18 Perturbing the robot in thex-direction while both measuring and estimat-
ing the external force (top) and using it for feedforward compensation to
prevent the COM from moving (bottom). . . . . . . . . . . . . . . . . . . 74
V.1 Illustration of the noisy velocity signal (in blue) computed by dierentiat-
ing a raw joint potentiometer signal and the corresponding delayed signal
(in green) produced by low-pass ltering. . . . . . . . . . . . . . . . . . . . 77
V.2 Inertial Measurement Units (IMUs) attached to the thigh, shank and foot
of a Sarcos hydraulic humanoid. . . . . . . . . . . . . . . . . . . . . . . . . 78
V.3 Comparison between ltered potentiometer-based hip velocities and con-
strained versus unconstrained hip velocities computed from link gyroscopes. 89
V.4 Comparison between ltered potentiometer-based ankle velocities and rotation-
corrected versus uncorrected ankle velocities computed from link gyroscopes. 89
V.5 Filtered potentiometer-based hip joint accelerations versus those computed
from inertial sensors with both manually and automatically-generated IMU
position information. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
V.6 Raw potentiometer-based hip joint acceleration (blue) versus that com-
puted from inertial sensors (red). . . . . . . . . . . . . . . . . . . . . . . . 90
V.7 Simulated gyroscope bias estimation using the joint state lter of Sec. V.3.1. 91
vii
V.8 Hip joint velocity computed from ltered joint sensors, directly from gy-
roscope velocities, ltered using the estimator of Sec. V.3.2 without and
with desired accelerations. . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
V.9 Knee sine tracking for the gains P = 1000 and D = 12, switched from
potentiometer-based velocities to IMU-based velocities at t = 10s. . . . . . 93
V.10 Knee sine tracking for the gains P = 1500 and D = 12 for the IMU-based
velocities only. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
V.11 Knee sine tracking for P = 250 and D = 26, switched to IMU-based
velocities at t = 10s. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
V.12 Knee tracking a 3Hz sine, switched from potentiometer-based velocities
with P = 800 and D = 12 to IMU-based velocities with P = 1500 at
t = 10s. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
VI.1 The top portion shows the six-dimensional contact probability resulting
from a rough terrain walking task (top) along with the measured IMU
linear acceleration and angular velocity (middle) and measured contact
force and torque (bottom). The portions in gray denote contact according
to the probability estimator (all P
i
> 0:5). The lower portion of the plot
shows a zoomed view of one contact cycle with several distinctive contact
events highlighted for discussion in Sec. VI.5.1. . . . . . . . . . . . . . . . 105
VI.2 Root Mean Squared Error (RMSE) for estimation of the unobservable base
position (top) and yaw (bottom) for dierent normal force thresholds. . . 107
VI.3 Root Mean Squared Error (RMSE) for estimation of the unobservable base
position (top) and yaw (bottom) for the contact probability-based base
state estimator and the xed normal force threshold base state estimator. 108
VI.4 Root Mean Squared Error (RMSE) for estimation of the unobservable base
position (top) and yaw (bottom) for dierent training datasets. . . . . . . 109
VI.5 Root Mean Squared Error (RMSE) for estimation of the unobservable base
position (top) and yaw (bottom) for walking in place on a patch of rough
terrain with a varying gait using clustering trained on three dierent gait
types as well as for the xed-threshold base state estimator (BSE). . . . . 110
VI.6 Root Mean Squared Error (RMSE) for estimation of the unobservable
base position (top) and yaw (bottom) with and without using IMU data
for online contact estimation. . . . . . . . . . . . . . . . . . . . . . . . . . 111
A.1 Illustration of the mapping between the manifold of rotations SO(3) and
its tangent space so(3). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
viii
List of Tables
III.1 Rank deciency for a single point foot contact. . . . . . . . . . . . . . . . . 34
III.2 Rank deciency for two point foot contacts. . . . . . . . . . . . . . . . . . . 35
III.3 Rank deciency for an arbitrary number of
at foot contacts. . . . . . . . . 35
III.4 Simulated noise parameters. . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
III.5 RMS and maximum (absolute) error values for the 120 second walking task
for point and
at foot lters. . . . . . . . . . . . . . . . . . . . . . . . . . . 40
IV.1 Simulated sensor noise standard deviations. Corresponding values for 1kHz
sampling rate are shown. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
IV.2 Noise standard deviations for momentum-based estimators. N/A indicates
that a parameter is not used. . . . . . . . . . . . . . . . . . . . . . . . . . . 60
VI.1 Simulated sensor noise standard deviations. Corresponding values for 1kHz
sampling rate are shown. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
ix
Abstract
As sensor, actuator and processor technology continues to improve, humanoid robots have
become more common in both academic and industrial environments as general-purpose
platforms capable of performing a wide variety of automated tasks. These robots have
the potential to operate in complex environments built for humans; their form factor al-
lows them to navigate buildings, utilize existing tools and perform cooperative tasks with
humans, making them ideal for disaster recovery and household labor among many other
applications. However, the challenge of operating autonomously in unknown environments
involves obtaining accurate estimates of the robot's state by fusing information from on-
board sensors and using these estimates for control in ways which allow robustness to
uncertainty and disturbances. In this work, we propose methods for estimating important
states of humanoid robots and evaluate the role of sensory information and state estimation
in executing behaviors on a torque-controlled humanoid. First, we discuss estimation of the

oating base pose of the robot in the world frame using only onboard sensing and kinemat-
ics models. This estimator takes into account contact switching and is shown to exhibit
desirable observability characteristics through a nonlinear observability analysis. While
the base pose and its velocity are crucial in computing joint torques via inverse dynamics,
the conguration-dependent center of mass (COM) and its dynamics (the momentum of
the system) is more-often the quantity of interest for planning and control. However, robot
dynamic models are never perfect; we thus introduce estimators using the momentum dy-
namics of the robot with measured contact wrenches to estimate conguration-dependent
center of mass and momentum osets as well as external wrenches applied about the COM.
We propose to evaluate the utility of this estimator for online replanning in a momentum
model predictive control (MPC) framework, thereby indirectly using measured contact
wrenches for feedback. Finally, we introduce methods for computing joint velocities and
x
accelerations from link-mounted IMUs and knowledge of kinematics, avoiding numerical
dierentiation of noisy joint angle sensors. This information is fused in several estimators
and used to increase the maximum stable joint feedback gains; we propose to utilize inertial
sensor-based estimates of joint derivatives in our whole-body control framework to achieve
better Cartesian damping control and thus improve tracking and robustness in the system.
These estimation methods are discussed and evaluated in the context of optimization-based
inverse dynamics control for locomotion.
xi
Acknowledgments
I've been extremely privileged to work in a research lab which is truly dedicated to and
passionate about robotics. Stefan Schaal and Ludo Righetti have never forced me to
work on a particular project, told me to write a specic paper or discouraged me from
trying a new idea. While the possibility of conducting any research I could dream up was
intimidating at rst, the
exibility and transparency of the research process itself made for
an organic development of ideas, the likes of which I may never experience again in any
other lab. While advice and guidance was always available (in particular from Ludo, who
sacriced many a relaxing evening at home to make video chatting work across a nine hour
time dierence), I'm very thankful for the autonomy I was aorded in developing the work
which constitutes this thesis. From day one, I felt like a trusted and valued member of the
lab rather than a student or employee; I greatly appreciate this egalitarian approach to
advising and want to thank Stefan and Ludo for their eorts and patience in helping me
become (I hope) an integral part of both the CLMC and AMD labs. I was able to travel
all over the world not only to learn more about humanoids but to advertise my work, to
expand my professional network and, in general, to grow as a person. My travels these past
ve years have led to great friendships, expanded my perspective on the world and greatly
enriched my life in general. I cannot emphasize how much these opportunities mean to me,
or thank Stefan and Ludo enough for allowing (and, what's more, encouraging) me to take
them.
My many labmates throughout my time in grad school have also been incredibly sup-
portive in helping me reach this point. In particular, I'm indebted to the core group of
people that shared with me the opportunity (often, the struggle) of working with legged
robots. Thank you to Ludo and Alex Herzog, who worked tirelessly to develop much of the
codebase we now use on an everyday basis - Alex has been a great mentor in both robotics
xii
and software development. John Rebula is an academic in the truest sense of the word,
and never fails to inspire new ideas by approaching problems in what can only be described
as the John-est of ways. I owe a huge thank-you to Sean Mason who is a fantastic engineer
with a true passion for robotics, even in the face of the most discouraging hardware and
software failures; he is an amazing colleague and now a lifelong friend. Felix Grimminger,
without whom every one of our robots would be broken by now, is the best mechatron-
ics engineer I've met. Thank you also to Brahayam Ponton, Maximilien Naveau, Julian
Viereck, Bilal Hammoud, Michael Bloesch of ETH Zurich and the rest of our collaborators
for helping me reach this point.
The degree of collaboration between dierent research groups and labs, both in Los
Angeles and Tuebingen, is unique and wonderful; I felt incredibly lucky to be surrounded
by hard-working people who love what they do and are always willing to help each other
with experiments or simply chat about new ideas. My non-research conversations, of which
there were probably far too many, were similarly enlightening - it was a privilege to work
with such a diverse group of people who greatly expanded my world-view. I'm very grateful
to have shared my journey with Franzi Meier, Vince Enachescu, Bharath Sankaran, Yevgen
Chebotar, Giovanni Sutanto, Karol Hausman and Harry Su during my time at USC.
Finally, a big thank you to the unsung heroes whose names never appear in my papers
or collaborations - my family and friends. First of all, thank you to my wonderful wife
Dr. Lena Hoober-Burkhardt for her love, support, and patience in me and my work. You
helped me achieve something much more important than a degree - a home, a renewed
sense of self, and a life I love. Thank you to my big Italian family back in New York -
Mom, Dad, Vinny, Karina, grandparents, aunts, uncles, cousins, cats, dogs and all the rest
- for teaching me to work hard and never settle for less. Thank you to my wonderful friends
in Los Angeles for all your support and distractions from grad school - Sean, Karol, Ola,
Becky, Devang, Rahne, Harsh, Shruti, Carrie, Mark, Jimmy and many more - I hope we
can continue adventuring together and inspiring one another for years to come.
And thank you, dear reader, for taking the time to read this thesis. I hope it serves
you well.
xiii
Chapter I
Introduction
Over the course of the past decade, legged robots have become popular research platforms
due to the availability of powerful, compact and low-cost sensors, actuators and processors
(Semini, 2010), (Hutter et al., 2012b), (Englsberger et al., 2014). Humanoid robots in
particular oer the ability to operate in complex and unstructured environments built for
humans, making them ideal for a variety of tasks such as disaster recovery and household
assistance. This versatility, however, comes at a cost - legged platforms in general must
deal with underactuation, discontinuous dynamics and an inherent lack of stability. These
are relatively new problems in robotics; wheeled mobile robots have traditionally been used
for research on planning in two-dimensional worlds assuming deterministic dynamics, while
xed-base manipulators were normally used to investigate problems of joint space planning
with kinematic redundancy and robust endpoint control in well-known environments.
In order to locomote autonomously in unknown environments, humanoids have largely
been controlled using model-based approaches rather than the sti joint controllers of-
ten used in manipulator robotics; this aords the system compliance which contributes
to robustness in dealing with unplanned disturbances and contacts (Stephens and Atke-
son, 2010). Purely model-based approaches, however, necessitate accurate models. Our
robot dynamic (and even kinematic) models are often incorrect - and to uncertain degrees.
Parameter identication methods can improve the quality of our models, but only within
the form we assume they take (Mistry et al., 2009). This lack of accurate models has led
1
to the use of a combination of feedforward and feedback control to ensure stability while
remaining robust to disturbances.
Such a framework, however, requires knowledge of the state of the robot. For legged
robots in particular, the quantities we wish to control usually cannot be measured directly
by any one sensor - these include kinematic quantities like endeector poses and well as
dynamic quantities such as the momenta of the rigid-body system (Herzog et al., 2014a).
However, we can characterize the noise properties of individual sensors and build models
which relate these sensor measurements to the robot's state (Woodman, 2007). Estimation
of the state of the robot thus entails fusion of dierent sensing modalities with knowledge
of how the state and measurements evolve according to our imperfect dynamics and sensor
models.
The estimation of high-level quantities for humanoids additionally provides insight into
what kinds of sensors we should incorporate into legged robots. As sensors have increasingly
become cheaper and smaller due to advancements in design and manufacturing, we have
the ability to augment robots with large amounts of redundant sensing. This can range
from surface-mounted inertial sensors to a full tactile skin covering the links of the robot
(Tsagarakis et al., 2007). Not only does such a design provide backup functionality in
case a sensor fails during operation (for example using Kalman Filter validation gates
(Alag et al., 2001)), but it also enables the fusion of many dierent modes of information
having dierent noise characteristics and operating ranges. By analyzing sensor models
and observability, we can make informed design decisions regarding the number of sensors
and their optimal placement necessary to estimate the quantities we desire to control.
Beyond the estimation problem itself, it is dicult to understand how our estimators
and controllers interact with one another in a real system. According to classical controls,
for a linear system the design of an observer and the controller which uses its output for
state feedback can be decoupled; this is known as the separation principle (Friedland, 1995).
Humanoid robots, however, have highly nonlinear dynamics which invalidate this approach
to estimation and control design, meaning that the performance of these components is
coupled. On the other hand, we actually neglect to use much of the sensory information
2
available to us on humanoid platforms because it is unclear how to incorporate this data
in the typical control framework employed on legged robots. Perhaps the most egregious
example is the lack of use of contact wrench sensing on humanoids; despite the fact that
most successful walking controllers plan stable motions by optimizing for desired contact
wrenches, very few use these measurements for anything more than threshold-based contact
detection (Kuindersma et al., 2016). Although it is dicult to use sensor signals directly for
feedback control, we can implicitly incorporate this information in our controllers by using
it to estimate states which are fed back or are used for online model predictive control.
In addition, we can explore methods for utilizing sensor traces recorded from previous
successful task executions to adapt planned motions to disturbances (Pastor et al., 2011).
I.1 The Current State of State Estimation
The results of the DARPA Robotics Challenge (DRC) 2014 trials and 2015 nals gave
us unique insight into estimation strategies which were proven to work well in closed-
loop control on full systems. Team IHMC employed a simple complementary lter for
fusion of IMU sensory data and leg kinematics; however, this estimator included only the
base position and velocity since the military grade Atlas gyroscope was assumed to be
extremely low-drift. This method has the advantage of being computationally cheap but
neglects IMU biases (which can be signicant for cheaper IMUs) and potential foot slip,
and fails to use knowledge of sensor noise to compute optimal state corrections as in a
Kalman Filter (Johnson et al., 2015). Team CMU similarly fused inertial information with
leg kinematics using precomputed steady-state Kalman Filters to estimate base position
and velocity, also using the onboard IMU orientation directly (Feng et al., 2015). This
approach thus fails to address incorporation of the rotational state of the base into the
estimator (which is nontrivial and renders sensor biases observable); further, it does not
permit foot slippage. The addition of a simplied model-based external force estimator
(Xinjilefu et al., 2015) similar to (Stephens, 2011) helped compensate for disturbances and
modeling errors; the COM and momentum estimator proposed here in Chapter IV can be
3
seen as a generalization of this LIPM-based method. MIT developed an EKF similar to that
proposed in Chapter III, however it does not include foot orientation measurements (again
because the gyroscope was assumed to be very accurate) and implements the foot position
measurement as a velocity measurement instead (Kuindersma et al., 2016). Additionally, it
neglects to use multiple contacts when more than one is available. However, this approach
has been extended to use the LIDAR unit mounted on Atlas' head which renders position
and yaw observable (Fallon et al., 2014). Overall, we conclude the following:
Although estimation research over the past few years has focused increasingly on using
the full robot dynamics often in an optimization-based framework (Xinjilefu et al.,
2014b), (Xinjilefu et al., 2014a), (Lowrey et al., 2014), (Kolev and Todorov, 2015), the
estimators used in the DRC were based on simplied dynamics and kinematic models.
This highlights a disconnect between current research and approaches which are
working reliably on real robots. We attempt to nd a middle ground by using dynamic
models primarily for state prediction, by simultaneously estimating modeling errors
and by making better use of sensory information in estimation and control.
While base state estimation is essential for whole-body control as evidenced by each
team's eorts, estimation of the COM and momentum of the system are arguably
more important as these dynamic quantities determine the stability of the robot
and are often modeled incorrectly. Additionally, there are a number of works on
momentum planning and control (Herzog et al., 2014b), (Herzog et al., 2015) which
have seen limited use on real robots due to poor dynamic models. In the DRC, only
CMU developed a COM estimator; it was seen to be highly benecial in control.
Sensory information is underutilized in control, missing both from direct sensor feed-
back as well as indirect feedback through estimation; in future work, we hope to
introduce more coupling between estimation and control. Further, with the availabil-
ity of low-cost MEMS sensors we can augment robots with large amounts of sensing;
we believe that redundant measurements can help improve estimation and should be
utilized.
4
Figure I.1: Estimation involves the fusion of robot models and sensing; in our work, we rely
entirely on proprioceptive or internal sensing (right) such as joint angle sensors, torque sen-
sors and IMUs rather than exteroceptive or external sensing (left) such as Vicon, GPS and
terrain maps in order to develop general-purpose methods which work in any environment.
I.2 Thesis Outline
In this thesis, we introduce methods for the estimation of various aspects of a humanoid
robot's state and evaluate their performance and interaction with state-of-the-art methods
for bipedal planning and whole-body control. The remainder of this document is organized
as follows:
Chapter II introduces the torque-controlled humanoid robots used in this work and
details the low-level control setup and pipeline. Inverse dynamics control for both
5
xed and
oating-based systems is brie
y introduced and used to develop an optimization-
based controller which is used in subsequent experiments. Simplied models of a hu-
manoid's dynamics are presented and used to summarize a class of model predictive
control techniques which are popular for planning humanoid robot locomotion.
Chapter III presents an estimator for the
oating base pose of a humanoid robot
which fuses information from inertial sensors and kinematics, taking into account
contact switching. An observability analysis is used to investigate the theoretical
properties of this estimator and its performance is evaluated both in a noisy simula-
tion environment and on the real robot in the context of inverse dynamics balance
control (Rotella et al., 2014).
Chapter IV discusses the role of the center of mass and linear and angular momentum
in planning and controlling stable motions on a humanoid robot and highlights the
diculties in doing so on real platforms. Multiple estimators based on the momentum
dynamics are presented to address these issues; their performance is evaluated in
simulation using perturbed dynamic models and their incorporation into popular
control methods is proposed (Rotella et al., 2016).
Chapter V addresses the issue of obtaining ltered and low-delay joint angle deriva-
tives for a
oating base robot using link-mounted inertial sensors and knowledge of
the robot's kinematic structure. The resulting estimators motivate the use of re-
dundant sensing in humanoid robots and oer insight on the number and placement
of sensors necessary to obtain estimates which have been shown experimentally to
increase control bandwidth on the real system (Rotella et al., 2016).
Chapter VI investigates methods for estimating the contact state of a legged robot
using proprioceptive sensing and a learning-based method. Clustering is applied to
endeector contact wrench and inertial sensor data collected during walking tasks,
resulting in a method for computing the probability of endeector contact directly
from sensor data with minimal computation. This is seen to improve estimation
6
performance and has direct applications in inverse dynamics control as well (Rotella
et al., 2017).
Chapter VII concludes this thesis with a summary of the presented methods and an
outlook on the future of sensing and control for legged robots.
In addition, several appendices are included in order to present background informa-
tion on key topics relevant to this work. We additionally maintain a large repository
of relevant information at http://www-clmc.usc.edu/ nrotella/MyNotes.pdf which
may prove useful.
7
Chapter II
Background
In this chapter, we begin by describing the Sarcos humanoids used for testing in the re-
mainder of this work and detail the entire control pipeline from low-level hydraulic valve
control to task implementation. We then introduce notation for describing robot dynamics
in the context of methods for inverse dynamics-based robot control. Basic theory for con-
trolling xed-base manipulators is presented in order to describe the transition to
oating
base robots and highlight challenges in controlling legged robots. Finally, we conclude
the chapter by introducing an optimization-based inverse dynamics controller based on a
number of state-of-the-art approaches which will be used extensively for robot testing.
II.1 Hardware
All experiments in this thesis were performed on the lower portion of a Sarcos humanoid
robot known as Hermes (Figure II.1, left) and in the corresponding SL simulation and
control environment (Schaal, 2007). The lower body platform has 17 total degrees of
freedom (DoFs) - seven in each leg and three in the torso. Each joint has potentiometers
which measure joint angles and strain gauge-based load cells which measure torque; joints
are actuated by hydraulic pistons acting through linkages which convert linear to rotational
motion. Custom motor controller cards, located on the back of the torso, control each
actuator by sending a command corresponding to a hydraulic valve position. At the lowest
level, a proportional-integral-derivative (PID) control loop on applied current servos the
8
Figure II.1: The hydraulic, torque-controlled Sarcos humanoids Hermes and Athena.
valve to the desired position, achieving a certain
ow rate. The Sarcos rmware allows for
position, torque or direct valve control; in the past, we have implemented torque control
by computing a valve command according to
u = PID(F;F
des
) +k
vc
_ x
piston
+b
where F is the measured piston force, F
des
is the desired piston force (computed from the
desired torque and the linkage geometry), k
vc
is a velocity compensation gain multiplying
the piston linear velocity _ x
piston
(computed from the joint velocity again using the linkage
geometry) andb is a bias term used to compensate for a constant mechanical valve position
oset. This control is inspired by the model-based torque control of (Boaventura et al.,
9
2012), however we cannot measure piston chamber pressures so we only perform an approx-
imate compensation of the natural velocity feedback present in hydraulic actuators. Valve
commands are computed as above oboard at 1kHz and sent to the motor controller cards
to be translated into current and forwarded to the valve's internal controller. Alternatively,
we have obtained the rmware source code and have successfully implemented the same
valve control law directly on the motor controller cards; this allows us to feed back torque
information at the motor controller frequency of 5kHz. These onboard torque controllers
have been tuned to achieve better torque tracking performance in terms of tracking error
and step response time (see Figure II.2). Additionally, we no longer perform velocity com-
pensation because tracking is already sucient and we prefer to keep some damping in the
system as it contributes to stability.
0 1 2 3 4 5
15
10
5
0
5
10
15
0 1 2 3 4 5
Time (s)
R_HFE Load (Nm)
Time (s)
Desired
Actual
1000Hz 5000Hz
Figure II.2: Torque tracking comparison showing superior performance when running
torque control onboard at 5 kHz (right) versus oboard at 1 kHz (left) for one joint.
10
Task-level controllers which compute desired torque commands are implemented o-
board in SL on a Linux machine patched with the real-time Xenomai kernel (Gerum,
2004). Communication between this machine and the motor controller cards is achieved
via UDP using the RTNet real-time ethernet Xenomai modules. We use Microstrain IMUs
(models 3DM-GX3-25 and 3DM-GX3-45) with the open-source usb4rt real-time driver

to
guarantee streaming of pre-processed angular velocity and linear acceleration at 1kHz.
II.1.1 The Athena Robot
We have a second Sarcos humanoid robot known as Athena (Figure II.1, right) located at
our partner lab
y
which is controlled in an identical manner. The primary dierences in
this platform are
The upper body is fully integrated with the lower body, adding many DoFs which can
be used for creating additional contacts (e.g. holding a handrail while climbing stairs)
and generating angular momentum for balancing. This will cause us to reconsider
the simplied models we use for planning.
The lower portion of each leg has been replaced with an Ottobock prosthesis. This
makes the legs extremely light and easy to swing. However, it presents challenges for
static locomotion approaches because the kinematic model is uncertain due to the
unknown prosthesis de
ection.
While the experiments and results of this thesis focus on the Hermes robot, the es-
timation approaches presented here can be applied to the Athena robot with minimal
modication. From an estimation perspective, the main dierence between the two robots
is Athena's lack of ankle sensing; the ankle state is thus unobservable, making kinematics-
based estimation impossible without assumptions. We propose to attach IMUs to the foot
and shank in order to integrate the relative angular velocity into an estimate of the ankle
angle when in contact; when not in contact, the prosthesis is undeformed and thus the

http://git.kiszka.org/?p=usb4rt.git;a=summary
y
https://am.is.tuebingen.mpg.de/
11
angle is known, allowing to reset its state in a Kalman lter. We leave this ankle angle
estimator to future work.
II.2 Robot Dynamics and Control
While the previous section detailed the low-level control used on our humanoids, here we
discuss the computation of joint torques to achieve desired motions using the robot's dy-
namic model; we begin with a brief summary of inverse dynamics control. While humanoids
have only recently become popular research platforms, torque-controlled robots have ex-
isted for decades in the form of xed-base manipulators. In contrast to the high-precision,
position-controlled manipulators long used in industrial settings, torque-controlled manip-
ulators have traditionally been used for research on controlling interaction between an
endeector with the environment (Nakanishi et al., 2008). The dynamics of such systems
take the form
M(q) q +h(q; _ q) = +J
T
(II.1)
where q2 R
n
is the vector of joint accelerations, 2 R
6m
is the vector of endeector
contact wrenches and 2 R
n
is the vector of joint torques; n denotes the number of
DoFs and m denotes the number of endeector contact points. The matrix M(q)2R
nn
is the conguration-dependent mass inertia matrix of the robot, and h(q; _ q)2 R
n
is the
conguration-dependent vector of nonlinear terms (Coriolis, centripetal and gravity forces)
acting on the robot. J2R
6mn
denotes the endeector Jacobian which (by the principle
of virtual work) relates contact wrenches applied at the endeector to joint torques through
its transpose.
The nal term in Eq (II.1) vanishes for a manipulator moving through free space. In this
case, the feedforward (model-computed) joint torques
ff
required to achieve a particular
vector of joint accelerations q
des
can be computed directly as

ff
=M(q) q +h(q; _ q)
12
this is called inverse dynamics because it maps from desired motion to controls by inverting
the dierential equations which describe the dynamics. Assuming accurate measurements
of q and _ q at each timestep and an accurate dynamic model (no joint friction or other
unmodeled dynamic eects), motion in joint space can be tracked extremely well. Of
course, joint sensors are noisy and models are never perfect, so joint proportional-derivative
(PD) control is added to the feedforward torques as
=
ff
+
fb
=M(q) q +h(q; _ q) +K
p
(q
des
q) +K
d
( _ q
des
_ q)
whereq
des
and _ q
des
are desired joint position and velocity. It can be shown that inverse dy-
namics with PD control is sucient to stabilize a xed-based manipulator from a Lyapunov
stability perspective (Siciliano et al., 2009). The feedforward portion of the above inverse
dynamics control is more generally known in the eld of nonlinear control as feedback lin-
earization. The purpose of this control method is to \linearize" the system by canceling
its nonlinear dynamics using knowledge of the current state and dynamic model, allowing
one to impose any desired dynamics (for example, PD control as above).
For controlling the position and orientation of the manipulator endeector, a trajectory
is usually designed in task (Cartesian) space and mapped to a desired joint space trajectory
through inverse kinematics. In this case, there are six task DoFs; many manipulators
are designed as overactuated (for example, with seven DoFs as in a human arm) which
introduces redundancy into the system and allows for achieving the same endeector motion
while optimizing for secondary tasks, for example posture control or obstacle avoidance
(Nakamura et al., 1987). In contrast to xed-base robots,
oating base robots such as
quadrupeds and humanoids have an additional six DoFs corresponding to the pose of the
base (root) link; these robots are thus underactuated. The dynamics become
M(q) q +h(q; _ q) =S
T
+J
T
(II.2)
where the matrix S2 R
n+6n
is a selector matrix which encodes the underactuation of
the system. Joint torques corresponding to a desired joint plus
oating base acceleration
13
vector q2 R
n+6
thus cannot be solved directly from Eq (II:2); we require an additional
six constraints to determine . These come from contact constraints in the form
J
c
q +
_
J
c
_ q = 0 (II.3)
Here, J
c
2 R
mcn+6
denotes the portion of the Jacobian corresponding to the m
c
endef-
fectors assumed to be in rigid contact with the ground. A single planar foot contact adds
six constraints; when both feet are in planar contact with the ground, the robot becomes
overactuated which allows for redundancy in inverse dynamics. The dynamics of a
oat-
ing base robot thus vary over time as contacts are made and broken, resulting in discrete
changes in the number of constrained degrees of freedom over time. We deal with task
objectives and contact constraints as well as a number of other physical constraints on
the system through the use of an optimization-based solver, developed according to the
approaches used successfully in the DRC.
II.3 Quadratic Program-Based Inverse Dynamics
Optimization-based approaches have become popular in recent years due to the availability
of ecient solvers which enable high frequency torque control (Stephens and Atkeson,
2010), (Hutter et al., 2012a), (Righetti et al., 2013). Some formulations allow a great deal
of
exibility in task specication at the cost of added complexity (Herzog et al., 2014a),
but here we use methods similar those which have been tested extensively on multiple
real systems (Feng et al., 2015), (Johnson et al., 2015), (Fallon et al., 2015), (DeDonato
et al., 2015) to focus on evaluating the estimators developed in this thesis with a widely-
used whole-body control methodology. Again, the
oating-base dynamics of the robot are
formulated as
M(q) q +h(q; _ q) =S
T
+J
T
c

where M 2 R
n+6n+6
is the mass-inertia matrix, h2 R
n+6
is the vector of nonlinear
terms (Coriolis, centrifugal, gravitational and joint friction forces) and J
c
2R
6mn
is the
14
endeector contact Jacobian. As shown in (Herzog et al., 2014a), the above dynamics can
be decomposed into the actuated and unactuated (
oating base) dynamics, respectively:
M
u
(q) q +h
u
(q; _ q) = +J
T
c;u

M
l
(q) q +h
l
(q; _ q) =J
T
c;l

Figure II.3: Illustration of the various quadratic program-based inverse dynamics cost and
constraint terms outlined in this section.
Due to the linear relationship between q, and , we need only optimize over two of
these variables; we thus eliminate joint torques and constrain the optimization using the
unactuated portion of the dynamics. The cost to be minimized is
min
q;
jj x x
des
jj
2
Wx
+jjP
null
q q
des
jj
2
Wq
+jj
des
jj
2
W

+jj
des
jj
2
W
15
subject to the equality constraints
M
l
(q) q +h
l
(q; _ q) =J
T
c;l

J
c
q +
_
J
c
_ q = 0
as well as inequality constraints on foot COPs, friction forces, resultant normal torques,
joint torques and joint accelerations. Joint torques are written in terms of q and as
=M
u
(q) q +h
u
(q; _ q)J
T
c;u

In the above cost, W
x
;W
q
;W

;W

denote weighting matrices, P
null
2R
n+6n+6
projects
into the nullspace of the Cartesian task constraints, and x2 R
6m
denotes the stacked
Cartesian poses of the endeectors. The relationship between joint and Cartesian acceler-
ations is given by
x =J
t
q +
_
J
t
_ q
where J
t
2R
6mtn+6
is the task Jacobian for the m
t
unconstrained endeectors and J
c
2
R
6mcn+6
is the contact Jacobian for them
c
constrained endeectors. Desired endeector
accelerations x
des
are computed from a planned Cartesian reference motion using a PD
control law as
x
des
= x
ref
+P
x
(x
ref
x) +D
x
( _ x
ref
_ x) (II.4)
Similarly, desired joint accelerations q
des
are specied as
q
des
=P
q
(q
ref
q)D
q
_ q
whereq
ref
is a default joint conguration, achieving posture control in the nullspace of the
task through the use of P
null
. The inverse dynamics problem is illustrated in Figure II.3;
for more details on this solver, see Appendix D.
16
II.4 Model Predictive Control
Optimization-based inverse dynamics allows for high-frequency control of con
icting tasks
in a manner which guarantees satisfaction of instantaneous stability constraints. However,
to achieve high-level behaviors such as walking and multi-contact interaction we must plan
trajectories over a horizon. Further, planning should be done in a manner which considers
the key aspects of the robot dynamics and respects physical limitations (for example, the
robot cannot walk a meter in a single stride because of kinematic limits) else the inverse
dynamics solver cannot achieve the behavior. This is known as model predictive control
(MPC) and constitutes the highest layer of our control framework.
While approaches for MPC using the full dynamics of
oating base robots exist and
produce impressive results in simulation (Tassa et al., 2012), these are computationally
expensive and suer from a lack of accurate models on real platforms. Instead, humanoids
are often controlled using plans generated based on the dynamics of a simplied model
which captures the most important aspects of the task (Kajita et al., 2001). Such methods
are generally simple enough to allow ecient online replanning (at frequencies anywhere
from 10Hz to 1kHz) which makes them robust to disturbances and model discrepancies.
Since the inverse dynamics control handles instantaneous stability, the MPC can be run
at a slower rate according to the dynamics used. We discuss the most popular simplied
model for humanoid control below.
II.4.1 The Linear Inverted Pendulum Model
For balancing and walking, the center of pressure (COP) - informally, the point on the
contact surface about which planar moments sum to zero - provides a stability metric used
in planning and control (Goswami, 1999). If the overall COP (assuming coplanar contacts)
leaves the support polygon (the convex hull of all contact points) then an unbalanced
17
Figure II.4: Illustration of simplication of dynamics from the full humanoid to the LIPM.
moment about the COM exists which will cause the robot to topple. The linear and
angular momentum summed about the COP are
_
l =F
res
+mg (II.5)
_
k = (pr
COM
)F
res
+
2
6
6
4
0
0

n
3
7
7
5
(II.6)
where l is the linear momentum, k is the angular momentum about the COM denoted
r
COM
, and F
res
and
n
are the resultant force and normal moment (torque) at the COP
denoted p. We can solve for the COP as
p
x
=r
COM;x

1
_
l
z
mg

r
COM;z
_
l
x
+
_
k
y

(II.7)
p
y
=r
COM;y

1
_
l
z
mg

r
COM;z
_
l
y

_
k
y

(II.8)
18
The COP is thus the sum of the COM and a dynamic term computed from the rate of
change of the system momenta; at rest, the COM and the COP are aligned. This means
that static walking can be planned simply by planning slow motions of the COM. However,
for faster walking trajectories the momentum of the system must be accounted for to ensure
stability. Neglecting angular momentum, limiting the COM to a xed height z
c
(or more
generally, motion on a specied plane) and using the fact that l =m _ r
COM
we obtain
p
x
=r
COM;x
+
z
c
g
r
COM;x
(II.9)
p
y
=r
COM;y
+
z
c
g
r
COM;y
(II.10)
These are the dynamics of the linear inverted pendulum model (LIPM), a popular simplied
model which has been used extensively for humanoid control (as shown in Figure II.4). This
model relates the COP linearly to the COM and its derivatives, allowing us to use linear
model predictive control techniques to compute a COM trajectory which achieves a desired
COP motion - usually set to move linearly between the feet during walking.
Alternatively, recent work has shown the utility of planning and controlling the full mo-
mentum of Eq (II.5) in order to stabilize humanoid robots and generate dynamic motions
(Stephens and Atkeson, 2010),(Lee and Goswami, 2012),(Wensing and Orin, 2013),(Her-
zog et al., 2014b),(Herzog et al., 2015). The advantage of such methods is their use of the
dynamically-consistent momentum dynamics which constitute a reduced rather than sim-
plied model. We discuss the use of this model for estimation and propose the combination
of momentum estimation and control in Chapter IV.
II.5 MPC-Based Walking Controller
The theory and resulting methods detailed in this chapter form the building blocks for
a complete, whole-body walking controller similar to those used extensively in the DRC.
We implement such a controller using a state machine approach, allowing new gait phases
19
to be easily introduced and transitioned to based on either a xed sequence or, ideally, a
higher-level gait controller.
Initialize
/Squat
Landing
Double
Support
Initial
Double
Support
Single
Support
Initial
Single
Support
Liftoff
Final
Single
Support
Final
Double
Support
Wait
Figure II.5: Simple walking state machine.
The advantage of developing a state-based walking controller is that it naturally oers
the
exibility required to translate user input from a device into gait parameter changes
and state transitions. We store gait information in a YAML-based class which can be both
read and modied from within both the walking states and the higher-level state scheduler,
allowing user input data to be passed down to the classes which actually implement gait
phases as well as back up to the state machine controller.
Using this structure, we have implemented a generic walking controller which takes
input from a joystick (or general gaming input device) using a Robot Operating System
(ROS) interface Quigley et al. (2009) to allow starting and stopping the gait, changing step
height, modulating walking step width and length as well as changing the walking forward
direction. In addition, we use the remaining inputs to apply simulated forces of adjustable
magnitude and direction at the robot base; this permits easily testing the robustness of our
walking controller. Using the ROS tool rosbag allows us to record joystick input during a
task and play it back to create repeatable desired walking gaits or disturbance forces from
20
user commands. This signicantly simplies the testing of a new controller or controller
module.
In an eort to make the state machine and user input structure of our controller as
general as possible, we have additionally implemented the ability to switch user input
modes. This allows having, for example, dierent input layouts for walking and upper
body manipulation. The controller designer can then check the input mode state from
within the state machine and translate user input commands accordingly. This has been
used to implement single and dual arm manipulation modes for the simulated, full-body
robot.
Fingers Close Fingers Open
Select Right
Arm
Select Left
Arm
Start Gait
Stop Gait
Hand In/Out
Hand Yaw
Control
Hand Left/Right
Hand Up/Down
Control
Hand Pitch/Roll
Control
Step Time++
Step Time--
Step Height++
Step Height--
Start Gait
Stop Gait
Base Yaw/Pitch
Control
Step Length/Width
Control
Simulated Push
Direction
Push Force++
Push Force--
WALKING MODE MANIPULATION MODE
Figure II.6: Example user input mapping for an Xbox controller with multiple modes.
II.6 Summary
Now that the control pipeline has been introduced, the remainder of this thesis focuses on
the challenges of estimation for use in humanoid control. Not only is controlling legged
robots dicult because of discontinuous dynamics, but also because the additional six
degrees of freedom in the generalized joint angle vector q 2 R
n+6
cannot be measured
directly except through the use of one or more exteroceptive sensors (those which are
external to the robot, for example GPS or visual marker-based tracking systems). Since
the proposed utility of such platforms is their potential for operation in unstructured
environments, exteroceptive sensors are unt for use; this forces the use of proprioceptive
or internal sensing - for example inertial, joint angle and contact wrench sensors - while
21
dealing with intermittent contact switching. Base pose estimation in the context of these
considerations is the focus of Chapter III.
The simplied and reduced model-based approaches of Section II.4 are limited on real
systems due to the lack of accurate estimates of the COM and momentum. Joint veloci-
ties, computed on our robots from noisy potentiometer signals, result in extremely noisy
momentum which limits how much we can damp our controllers. Inaccurate link masses
and link inertias contribute to conguration-dependent osets in the COM and momenta,
while unmodeled wrenches applied about the COM can likewise destabilize control. Even
traditional approaches using simplied models (such as the LIPM) for planning rely on ac-
curate center of mass estimates to achieve good tracking. We therefore focus on momentum
estimators which deal with these challenges in Chapter IV.
In general, achieving good damping on real systems is dicult due to noise on joint
derivatives which gets propagated to task-level quantities in control. Since we compute
joint velocities and accelerations by numerically dierentiating noisy position signals, we
are limited as to how high we can increase our joint and Cartesian damping gains before
the controller goes unstable. By using dynamics-based process models and measurements
derived from link-mounted inertial sensors we can lter the joint state and thus use higher
gains; this is the focus of Chapter V.
Knowledge of contact is crucial for determining endeector constraints in both base
state estimation and whole-body control, however the contact state is almost always deter-
mined by thresholding the measured normal force at the endeector. This is an extremely
simplied view of contact, which is truly a six DoF quantity; it is assumed that a high-
enough force threshold ensures the endeector will not slip or rotate. Instead, in Chapter
VI we propose a clustering-based method which uses endeector force/torque sensing and
additional IMUs to detect slip and rotation automatically and modulate the probability of
contact in six DoFs, and evaluate this contact estimator for base state estimation.
Before proceeding to discuss each of these estimation-based approaches to the control
problems we encounter on real systems when using the solver of Section II.3, we summarize
the humanoid control framework presented in this chapter.
22
We work with Sarcos hydraulic humanoids which are torque-controlled at 5kHz on
the motor controller cards, receiving desired torque commands at 1kHz from the SL
simulation and control framework over real-time ethernet from a Xenomai-patched
Linux machine.
Humanoids are typically controlled via inverse dynamics, a model-based control
method which is used to cancel nonlinearities and impose desired behavior in joint
and/or Cartesian space. By using a combination of feedforward and feedback control,
the robot remains compliant and thus robust to disturbances.
We have developed a quadratic program-based inverse dynamics solver based on
control methods used by the DRC teams on the torque-controlled Atlas humanoid
platform. This method of control has more-or-less become common practice and
allows us to test estimation-based planning and control using a benchmarked solver.
The Linear Inverted Pendulum Model (LIPM) was introduced as a simplied form
of the full robot dynamics for use in Model Predictive Control (MPC) over a time
horizon. This control methodology is used to plan walking trajectories, which are
modulated from user input in the context of a general-purpose walking state machine.
The resulting walking controller is used extensively in this thesis for testing control
and estimation methods in locomotion experiments.
23
Chapter III
Base State Estimation
One of the earliest attempts (Roston and Krotkov, 1991) at base pose estimation for a
legged robot was performed on the CMU Ambler, a hexapod robot having only joint en-
coders. The positions of the feet were computed both from motor commands and encoder
measurements. Each foot contributed a measurement and the transformation which min-
imized the error between the two estimates was computed. Nearly fteen years later,
(Gassmann et al., 2005) relied on the same dead-reckoning method, extending earlier work
by fusing odometry with orientation and position measurements from new inertial sensors
such as IMU and GPS units. However, this approach was inherently unable to handle non
statically-stable gaits. Around the same time, (Lin et al., 2006) extended previous work
on pose estimation using a strain-based model with MEMS inertial sensors to measure
motion of the body. The gait was split into multiple phases, each of which corresponded to
a simple Kalman lter. Models for each phase were switched using sensory cues, allowing
non statically-stable gaits. However, it was shown that the lter provides accurate pose
estimates only when in tripod support.
For quadrupeds, (Chitta et al., 2007) employed a particle lter which used an odometry-
based prediction model for the COM during quadruple support and an update model based
on IMU data, joint encoder readings and knowledge of the terrain relief for state estima-
tion with the quadruped LittleDog. While this method permitted global localization, it
assumed knowledge of the terrain and a statically-stable gait. A state estimator was in-
troduced (Cobano et al., 2008) for the quadruped SILO4 which fused changes in position
24
(computed using dead-reckoning) with magnetometer and DGPS measurements in an Ex-
tended Kalman Filter (EKF). While they tracked global position and heading without gait
assumptions, the full 6 DoF pose was not estimated. (Reinstein and Homann, 2011) intro-
duced an EKF for a quadruped which combined kinematic predictions from IMU data with
a \data-driven" velocity measurement. By assuming an uninterrupted gait, the velocity
was computed using a learned model. However, the gait assumption and model limited
the approach's utility on other platforms. Recent work (Chilian et al., 2011) on the DLR
Crawler hexapod platform introduced an information lter suitable for combining multiple
types of measurements. The process model integrated IMU data to track the pose of the
hexapod while visual and leg odometry were used for updates. Absolute measurements of
roll and pitch angles were obtained from the accelerometer and used as well. While they
made no gait assumptions, the leg odometry measurements were valid only during periods
of three or more contacts.
Base pose state estimation for bipedal legged robots has only recently gained attention
as more humanoid platforms have become available. (Park et al., 2009) introduced a
Kalman Filter in the context of a Zero Moment Point (ZMP) balance controller using the
Linear Inverted Pendulum Model (LIPM) to approximate the dynamics of the humanoid
robot. The position, velocity, and acceleration of the COM of the LIPM were estimated
using the pendulum dynamics and the measured ZMP location was used for the update
step. However, the approach was only applicable in the context of ZMP balancing and
walking. In a similar context, an Unscented Kalman Filter which provided estimates of
the joint angles and velocities for predictive ZMP control was proposed (Wang et al.,
2013). The lter treated the biped in single support as a xed-base manipulator with
corresponding dynamics. Of course, this assumption was violated if the robot lost contact
or slipped. Additionally, the absolute orientation could not be observed. This lter was
also computationally-demanding as it used the full manipulator dynamics for prediction.
The DRC led to the development of a number of base state estimators for humanoids,
however all fused inertial sensing with kinematics as done in the work presented here. Only
one approach (Fallon et al., 2014) was truly novel in that it incorporated LIDAR-based
25
measurements for absolute position and yaw sensing (see Chapter I for a more complete
analysis of estimation in the DRC).
More recently, estimation methods using optimization have been proposed (Xinjilefu
et al., 2014a) using the full robot dynamics and measurements from many sensors. Both
states and controls are estimated in a quadratic program and sensors can be easily inte-
grated since the dynamics are linear in terms of the state. Additionally, constraints can be
enforced whereas this is not straightforward to do in a KF. However, this approach relies
heavily on the full dynamics (namely link inertias) which we usually do not have good esti-
mates of. Further, osets in the COM and momentum are not considered. Another recent
work (Faraji et al., 2015) uses optimization methods to estimate base position, velocity
and yaw angle (assuming roll and pitch can be used directly from the internal IMU esti-
mator) in a multi-stage lter. This method can easily handle contact switching, however
it assumes no foot movement or slippage.
In this chapter, we propose a base pose estimator, analyze its theoretical characteris-
tics and evaluate its performance in a simulation environment with realistic added noise.
Finally, we evaluate the performance of this estimator in combination with the inverse
dynamics solver proposed in Section II.3 for dierent whole-body control tasks on the real
robot.
III.1 Prediction and Measurement Models
In order to introduce required terminology and notation, we begin by introducing the
models used in the presented estimator. Note that these models are nonlinear, leading us to
employ an Extended Kalman Filter (EKF); we chose this formulation over alternatives such
as the Particle Filter or Unscented Kalman Filter for its simplicity and low computational
cost. However, the presented framework and observability analysis hold regardless of the
method employed. For a review of Kalman Filtering, see Appendix A.
In (Bloesch et al., 2012) a continuous-time, nonlinear prediction model describing the
time evolution of the state of the quadruped was developed based on rigid body kinematics
26
Figure III.1: Base state estimation notation for a bipedal robot.
and a simple model of an IMU consisting of a three-axis accelerometer and a three-axis
gyroscope. This prediction model is shown below. Note that this formulation can accom-
modate an arbitrary number of feet, denoted N.
_ r =v (III.1)
_ v =a =C
T
(
~
fb
f
w
f
) +g (III.2)
_ q =
1
2
2
4
~ !b
!
w
!
0
3
5

q (III.3)
_ p
i
=C
T
w
p;i
8i2f1;:::;Ng (III.4)
_
b
f
=w
bf
(III.5)
_
b
!
=w
b!
(III.6)
The state of the lter is x = [r;v;q;p
i
;b
f
;b
w
] wherer is the position of the IMU (assumed
to be located at the base),v is the base velocity,q is the quaternion representing a rotation
from the world to body frame, p
i
is the world position of the i
th
foot and b
f
and b
w
are the accelerometer and gyroscope biases, respectively. Unless noted, C(q) or simply C
27
represents the rotation matrix corresponding to q. The raw accelerometer and gyroscope
data are
~
f and ~ ! and are modeled as being subject to additive thermal noise processes w
f
and w
!
as well as random-walk biases parameterized by the noise processes w
bf
and w
b!
(Woodman, 2007). Finally, w
p;i
denotes the noise process representing the uncertainty of
the i
th
foothold position.
Figure III.2: Base state estimation augmented with the relative foot orientation measure-
ment s
z
i
corresponding to foot i for a bipedal robot.
To extend the established lter to a humanoid platform, we now augment the state
vector with the orientations of the feet. Including only one foot for brevity, the new state
is dened as x = [r;v;q;p;b
f
;b
w
;z] where z is the quaternion representing the rotation
from the world to foot frame. Analogous to the assumption made in (Bloesch et al., 2012),
we assume that the orientation of the foot remains constant while in contact; to permit a
small amount of rotational slippage, the prediction equation is dened to be
_ z =
1
2
2
4
w
z
0
3
5

z (III.7)
28
where w
z
is the process noise having covariance matrix Q
z
. The foot orientation noise is
applied as an angular velocity in order to remain consistent with the original model.
The original lter update step was performed using one measurement equation for each
foot which represented the position of that foot relative to the base as measured in the
base frame. This measurement is a function only of the measured joint angles and the
kinematic model of the leg; it is written as
s
p
=C(pr) +n
p
(III.8)
The noise vectorn
p
represents the combination of noise in the encoders and uncertainty in
the kinematic model. Its covariance is the main tuning parameter of the lter.
Following the original lter formulation, we introduce an additional measurement which
is again a function of only the measured joint angles and the kinematics model. The
orientation of the foot in the base frame represented by the quaternion
s
z
= exp (n
q
)
q
z
1
(III.9)
describes the rotational constraint imposed by a
at foot contact. The noise term n
q
is
applied using the exponential map; see Appendix (A.12) for details. Again, the noise term
depends on the forward kinematics uncertainty and constitutes a tuning parameter.
When a foot loses contact, its measurement equations are temporarily dropped from
the lter. Additionally, its pose is removed from the state; this can be achieved more
simply by setting the variances of w
p
and w
z
corresponding to the foot to large values.
This causes the pose uncertainty to grow rapidly. When contact is restored, the pose and
measurements are included again; this triggers a reset of the foot pose to its new value.
This allows for contact switching without the need for separate models. During an aerial
phase, the lter reduces to integration of the prediction model.
Since the above system is continuous and nonlinear, it must be discretized and linearized
for implementation purposes. Following the EKF framework outlined above, this requires
two systems of equations: a discrete, nonlinear system for prediction of the state and
29
measurement and a discrete, linear system for propagation of the state covariance through
the prediction model and for computation of the gain in the update step. Additionally, we
will derive in an intermediate step a continuous, linear system. We begin with a discussion
of the discrete, nonlinear system used for prediction.
III.1.1 Discrete, Nonlinear Model
The rst step in the EKF is the propagation of the expected value of the state using the
discretized nonlinear model. Assuming a zero-order hold on the IMU data over a small
timestep t, we discretize the original system using a rst-order integration scheme as
^ r

k+1
= ^ r
+
k
+ t^ v
+
k
+
t
2
2
(
^
C
+T
k
^
f
k
+g) (III.10)
^ v

k+1
= ^ v
+
k
+ t(
^
C
+T
k
^
f
k
+g) (III.11)
^ q

k+1
= exp (t^ !
k
)
^ q
+
k
(III.12)
^ p

k+1
= ^ p
+
i;k
(III.13)
^
b

f;k+1
=
^
b
+
f;k
(III.14)
^
b

!;k+1
=
^
b
+
!;k
(III.15)
^ z

k+1
= ^ z
+
i;k
(III.16)
where
^
f
k
=
~
f
^
b
+
f;k
and ^ !
k
= ~ !
^
b
+
!;k
denote the expected values of the measured
acceleration and angular velocity, respectively. Note that the quaternion representing the
base pose is updated using the exponential map formed from the innitesimal rotation t^ !
which is measured in the base frame directly. Also note that a second-order discretization is
used for the position in order to incorporate the IMU acceleration at the current timestep.
The measurement model is discretized simply as
^ s
p;k
=
^
C

k
(^ p

k
^ r

k
) (III.17)
^ s
z;k
= ^ q

k

^ z

k

1
(III.18)
to produce the expected measurement of the nonlinear system.
30
III.1.2 Continuous, Linear Model
Recall that discretized, linearized dynamics are required in order to propagate the state
estimate covariance and perform the update step. It is our preference to linearize and
then discretize; this is the approach found in many texts. Linearization is performed by
expanding each state around its current estimate using a rst-order approximation. For
the full derivation of the linearized system presented in this section, see Appendix B. This
approach results in the linearized model
_
r =v (III.19)
_
v =C
T
f

C
T
b
f
C
T
w
f
(III.20)
_
=!

b
!
w
!
(III.21)
_
p =C
T
w
p
(III.22)
_
b
f
=w
bf
(III.23)
_
b
!
=w
b!
(III.24)
_
=w
z
(III.25)
where the measured IMU quantities are bias-compensated and wherev

denotes the skew-
symmetric matrix corresponding to the vectorv. The error state and process noise vectors
are dened to be x = [r;v;;p;b
f
;b
!
;]
T
and w = [w
f
;w
!
;w
p
;w
bf
;w
b!
;w
z
]
T
,
respectively. The linearized system can then be written in state-space form as
_
x =F
c
x+
L
c
w where
F
c
=
0
B
B
B
B
B
B
B
B
B
B
B
B
B
B
@
0 I 0 0 0 0 0
0 0 C
T
f

0 C
T
0 0
0 0 !

0 0 I 0
0 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 0 0
1
C
C
C
C
C
C
C
C
C
C
C
C
C
C
A
31
and L
c
=diagfC
T
;I;C
T
;I;I;Ig are the prediction and noise Jacobians, respectively.
It is assumed for simplicity that the covariance matrix of each process noise vector is
diagonal with equal entries. The continuous process noise covariance matrix is then Q
c
=
diagfQ
f
;Q
!
;Q
p
;Q
b
f
;Q
b!
;Q
z
g. The measurement model dened by (III.8) and (III.9) is
linearized as
s
p
=Cr + (C(pr))
x
+Cp +n
p
s
z
=C[q
z
1
] +n
z
whereC[m] is used to denote the rotation matrix corresponding to the quaternionm. This
model can be written in the form y =H
c
x +v where v = [n
p
;n
z
]
T
is the measurement
noise vector and
H
c
=
0
@
C 0 (C(pr))
x
C 0 0 0
0 0 I 0 0 0C[q
z
1
]
1
A
is the measurement Jacobian. It is assumed that measurements are uncorrelated and hence
the measurement noise covariance matrix is dened as R
c
= diagfR
p
;R
z
g where R
p
and
R
z
are again each diagonal with equal entries.
III.1.3 Discrete, Linear Model
Assuming a zero-order hold on inputs over the interval t = t
k+1
t
k
, the discretized
prediction Jacobian is given by
F
k
=e
Fct
and the discretized state covariance matrix is given by
Q
k1
=
Z
t
k
t
k1
e
Fc(t
k
)
L
c
Q
c
L
T
c
e
F
T
c
(t
k
)
d
In practice, these expressions are often truncated at rst order for simplicity to yield
F
k
I +F
c
t andQ
k
F
k
L
c
Q
c
L
T
c
F
T
k
t. In contrast to (Bloesch et al., 2012), the system
32
is discretized using a zero-order hold on the noise terms as above in order to simplify the
implementation.
Following the above procedure, we nd the discretized prediction and measurement
Jacobians to be
F
k
=
0
B
B
B
B
B
B
B
B
B
B
B
B
B
B
@
I It 0 0 0 0 0
0 I C
T
k
f

k
t 0 C
T
k
t 0 0
0 0 I!

k
t 0 0 It 0
0 0 0 I 0 0 0
0 0 0 0 I 0 0
0 0 0 0 0 I 0
0 0 0 0 0 0 I
1
C
C
C
C
C
C
C
C
C
C
C
C
C
C
A
;
H
k
=
0
@
C
k
0 (C
k
(p
k
r
k
))

C
k
0 0 0
0 0 I 0 0 0C[q
k

z
1
k
]
1
A
where all quantities are computed using the a priori state vector ^ x

k
. Finally, the continuous
measurement covariance matrix is discretized as R
k1

Rc
t
.
III.2 Observability Analysis
As in (Bloesch et al., 2012), a nonlinear observability analysis of the lter was performed.
This section discusses the resulting observability characteristics and analyzes the theoretical
advantages of the addition of the foot rotation constraints.
The unobservable subspace informally describes all directions along which state dis-
turbances cannot be observed in the outputs. This corresponds to the nullspace of the
observability matrix. For a nonlinear system, determining this matrix involves computing
the gradient of successive Lie derivatives of the measurement model with respect to the
process model as in (Hermann and Krener, 1977); see Appendix C for more details. Since
this space depends on the robot's motion, the presented analysis reveals all possible sin-
gularities and corresponding rank losses (RL). Note that since absolute position and yaw
33
are inherently unobservable in this system, rank loss here represents an increase in the di-
mension of the unobservable subspace beyond this nominal case. While the full derivation
omitted for brevity, the pertinent results are summarized in tables III.1, III.2, and III.3.
Table III.1 below describes the rank deciency for the case in which a single point foot
is in contact. The top row (w = 0) describes the case in which there is no rotational motion.
Depending on the acceleration, the rank loss is either 3 or 5. The second row (w? Cg)
states that rotational motion around an axis which is perpendicular to the gravity axis
leads to a rank loss of 1. The third row (wk Cg) describes the case in which there is
rotation only around the gravity axis; in general, this corresponds to a rank loss of 1. If
the axis of rotation additionally intersects the point of contact, rank loss increases to 2.
Further, if the IMU is directly above the point of contact then rank loss increases to 3.
Finally, the last row summarizes the nominal case.
Table III.1: Rank deciency for a single point foot contact.
Rotation Acceleration/Velocity Foothold RL
w = 0
a =1=2g 5
a6=1=2g 3
w?Cg 1
wkCg
^
a =

C
T
w

v
v =

C
T
w

(rp)
(rp)kg 3
(rp)6kg 2
_
a6=

C
T
w

v
v6=

C
T
w

(rp)
1
^
w6?Cg
w6kCg
0
Table III.2 below details the singular cases for two point foot contacts. These are similar
to the single point foot contact cases but with reduced rank losses.
34
Table III.2: Rank deciency for two point foot contacts.
Rotation Footholds RL
w = 0
2a +gk p 3
2a +g6k p 2
w?Cg 1
wkCg
gk p 1
g6k p 0
^
w6?Cg
w6kCg
0
In comparison to the point foot cases, the
at foot case is signicantly simpler as shown
in Table III.3. These results are valid for any number of
at foot contacts since a single

at foot contact fully constrains the pose of the base.
Table III.3: Rank deciency for an arbitrary number of
at foot contacts.
Rotation RL
w = 0 2
w?Cg 1
w6?Cg 0
The rank loss of the new lter depends only on the base angular velocity. If the
angular velocity is zero then the rank loss is 2; if the axis of rotation is perpendicular to
the gravity axis then the rank loss is 1. For all other cases, only absolute position and yaw
are unobservable; inclusion of foot orientations renders the IMU sensor biases observable.
It's clear that the additional information resulting from the the rotational constraint of
the foot signicantly reduces rank loss, as expected. In summary, the maximum rank loss
is reduced from 5 to 2 when there is no rotational motion. Additionally, rotation purely
around the gravity axis no longer induces rank loss. Finally, since the results of Table
III.3 hold for any number of
at foot contacts, both single and double support walking
35
phases have desirable observability characteristics. The experimental results of section V.4
demonstrate the practical eects of the singular cases.
III.3 Experiments and Results
Figure III.3: Base state estimation notation on Hermes robot in SL simulation environment.
The goal of this work is to implement the proposed EKF on a Sarcos humanoid equipped
with a Microstrain 3DM-GX3-25 IMU. However, it was desired that its performance rst be
veried in simulation. For this purpose, both lters were implemented in the SL simulation
environment (Schaal, 2007).
In order to provide an accurate assessment of these lters, realistic levels of noise were
added in the simulator by drawing samples from an i.i.d. Gaussian white noise process.
Thermal noise was added to the simulated IMU sensor data along with an integrated
random walk bias. Similarly, measurement noise was added to the measurements at each
timestep. The standard deviations of the noise processes are given in Table III.4; all but the
measurement densities are given in continuous time and are converted to discrete variances
by squaring them and dividing by the timestep.
36
Table III.4: Simulated noise parameters.
w
f
0:00078m=s
2
=
p
Hz w
p
0:001m=
p
Hz
w
!
0:000523rad=s=
p
Hz w
z
0:01rad=
p
Hz
w
bf
0:0001m=s
3
=
p
Hz n
p
0.01 m
w
b!
0:000618rad=s
2
=
p
Hz n
z
0.01 rad
The simulated IMU noise parameters were derived directly from the 3DM-GX3-25
datasheet and the measurement noise parameters were based on the observed uncertainty
in the encoders and kinematic model of the actual robot. Experiments were conducted at
an update rate of 1kHz as this is the fastest possible IMU streaming rate.
The measurement noise parameters were empirically tuned from their simulated values;
all other parameters were set to their simulated values. Initialization of the base orienta-
tion was performed using stationary accelerometer measurements, initial foot poses were
computed from kinematics and all other states were initialized to zero.
The plots in Figure III.4 show the results obtained on the simulated walking dataset of
length 120 seconds. Table III.5 below lists the RMS and maximum errors for all plotted
quantities with the last three rows corresponding to the Euler angles roll, pitch and yaw.
Based on the above errors, the two lters perform equally well for many of the plotted
quantities. However, investigation of the velocity estimation reveals that the point foot
lter periodically diverges more drastically throughout the task, causing the
at foot po-
sition estimates to be noticeably more accurate as demonstrated by the plots and error
values. Indeed, the maximum absolute errors in v
x
, v
y
and v
z
for the point foot lter are
reduced for the
at foot lter as compared to the point foot lter as shown in Table III.5.
While small, these repeated periods of divergence can lead to considerably more integrated
error over the course of a lengthy task.
The divergence of the point foot lter inv
y
corresponds primarily to the double support
portions of the task. There is relatively little base rotation during these intervals since the
robot is shifting its center of mass in preparation for the next step; proximity to the ! = 0
37
0 20 40 60 80 100 120
0.2
0
0.2
r
x
(m)
0 20 40 60 80 100 120
0
0.2
0.4
r
y
(m)
Point Foot
Flat Foot
Actual
0 20 40 60 80 100 120
0.1
0
0.1
r
z
(m)
Time (s)
Point Foot
Flat Foot
;
(a) Position
0 20 40 60 80 100 120
−0.2
0
0.2
v
x
(m/s)
0 20 40 60 80 100 120
−0.2
0
0.2
v
y
(m/s)
0 20 40 60 80 100 120
−0.2
0
0.2
v
z
(m/s)
Time (s)
;
(b) Velocity
0 20 40 60 80 100 120
−0.5
0
0.5
roll (rad)
0 20 40 60 80 100 120
−0.5
0
0.5
pitch (rad)
0 20 40 60 80 100 120
−0.5
0
0.5
yaw (rad)
Time (s) ;
(c) Euler Angles
Figure III.4: Position, velocity and orientation (Euler angle) estimates for the 120
second walking task. The portions with a white background indicate the single
support phase while those with a grey background indicate the double support
phase. Overall, the
at foot lter introduced in this work performs better than
the lter introduced in (Bloesch et al., 2012) as conrmed by the estimation errors
listed in Table III.5.
38
x
0
0.005
0.01
0.015
0.02
Position RMS Error (m)
y z
0
0.005
0.01
0.015
0.02
Velocity RMS Error (m/s)
x y z
0
0.02
0.04
0.06
0.08
0.1
0.12
Euler Angle RMS Error (rad)
x y z
Point Foot
Flat Foot
Figure III.5: RMS estimation error bar plots for the Point Foot and Flat Foot lters for
the simulated walking task corresponding to those listed in Table III.5.
39
Table III.5: RMS and maximum (absolute) error values for the 120 second walking task
for point and
at foot lters.
RMS Error Max Error
Point Flat Point Flat
r
x
(m) 0:0088 0:0042 0:0172 0:0086
r
y
(m) 0:0046 0:0017 0:0123 0:0047
r
z
(m) 0:0020 0:0019 0:0051 0:0050
v
x
(m=s) 0:0079 0:0082 0:0417 0:0393
v
y
(m=s) 0:0058 0:0053 0:0282 0:0276
v
z
(m=s) 0:0067 0:0066 0:0357 0:0321
(rad) 0:0011 0:0011 0:0037 0:0038
(rad) 0:0010 0:0013 0:0034 0:0046

(rad) 0:0379 0:0055 0:1032 0:0110
singular case may account for the performance dierence between the lters since the
at
foot lter has a reduced maximum rank loss in this situation. Both lters diverge in v
x
during the single support phase, suggesting that one or more singular cases are reached here
(for example, the angular velocity becomes nearly orthogonal to the gravity axis when the
leg is swung forward). However, it's clear from the plots that the point foot lter provides
poorer state estimates during this interval; this is to be expected since the rank loss cases
for the
at foot lter are fewer and less drastic during single support. Additionally, the
large dierence in error values between lters for the yaw angle is explained by the fact
that constraining the rotational state renders the gyroscope bias fully observable for the

at foot lter. Finally, note that the setup simulated in this paper employs high-quality
sensors and a fast update rate; the dierence between the two formulations is expected to
be even more pronounced on a robot which has a lower control frequency, a less-accurate
kinematic model or low-grade sensors. The eect of hardware on performance remains to
be tested in future work by adjusting simulated noise appropriately.
40
III.4 Inverse Dynamics Experiments
While we have analyzed performance of the base state estimator introduced in this chapter
in a simulation environment with realistic levels of noise in Section V.4, we validate the
approach on a real humanoid platform in the context of inverse dynamics control. Using
the base state estimator of Section III.1 in combination with the controller introduced in
Section II.3, we have conducted a number of experiments on our Sarcos humanoid, Hermes
(see II.1 for more information on the hardware and low-level control). Here, we present
two tasks which highlight the use of Cartesian feedback control in global frame and contact
switching.
Figure III.6: The Sarcos humanoid during a single legged squatting experiment with per-
turbations. Inset are screen captures of the SL kinematics visualization updated using the
base pose estimator presented in this chapter.
The rst task is single support balancing, implemented as COM and swing foot regu-
lation (a desired setpoint for each unconstrained endeector is specied with zero desired
41
velocity and acceleration). The transition to single support is achieved using motions cre-
ated using minimum jerk trajectories (fth-order polynomials with desired initial and nal
position and zero velocity and acceleration endpoint constraints). The robot rst squats
3cm and then shifts the COM above the stance foot while unloading the swing leg; de-
sired vertical forces are specied in the solver to achieve a linear weight shift. A simple
threshold-based contact estimator (with debounce safeguards) is used to trigger the re-
lease of the swing foot contact; the estimated state is used to determine the constraints
in both the estimator and controller. The results of this experiment are summarized in
Figure III.7. COM tracking is good in the x and y directions, however there is an oset
in the z direction due to a discrepancy between the mass model and the true robot mass.
The shaded portions of the plot indicate pushes applied primarily in the y direction to the
robot's base or swing foot using an ATI wrench sensor mounted on a rod; the magnitude
(norm) of the push force is shown in the nal plot. The foot y position is also plotted to
show this endeector's response to pushes around the 35s mark. The COM control is quite
sti, quickly converging back to within several millimeters of the desired y position even
after sustained pushes of upwards of 50N; an integral controller could compensate for such
a prolonged disturbance, or more appropriately we could estimate the external wrench
using the methods of Chapter IV and add it into the dynamic model when performing
inverse dynamics. The swing foot position control is less sti which is desirable during the
swing phase of walking in order to deal with unknown terrain, however the control could
be tuned more aggressively if desired.
The second experiment, shown in Figure III.8, is identical to the rst for the beginning
portion (squatting and shifting COM motions) but the COM is then commanded to move
up and down repeatedly in single support while maintaining the swing foot at a specic
height. Since the COM and swing foot tracking are traded o with weights in the cost
function (see Section II.3), a higher weight on the COM tracking means that the foot moves
a small amount (about 1cm in each direction) in order to make the desired COM motion
feasible. Since the conguration of the leg aects the position of the COM, these tasks
are coupled and thus the swing leg tracking is compromised in order to improve the COM
42
0
0.05
0.1
COM x (m) −0.05
−0.04
−0.03
COM y (m) −0.19
−0.18
−0.17
−0.16
COM z (m) −0.12
−0.1
−0.08
−0.06
Foot y (m)
10 20 30 40 50 60 70 80 90
20
40
60
Time (sec)
Push Force (N)
Figure III.7: QP-Based Inverse Dynamics control using the estimated base pose for a
single-legged balancing task in which the COM is regulated to a desired position over the
stance foot. We disturb the robot with pushes of varying strength and duration as shown
in the bottom plot and highlighted elsewhere in grey.
tracking. Again, the COM height as well as the swing foot height are oset due to mass
modeling errors, however this could be remedied by adjusting the mass model to correct
the gravity compensation portion of the inverse dynamics control. We again disturb the
robot at the base and foot during the task, however the motion continues to track well and
remains stable.
In both experiments, the release of the left foot contact is seen to be smooth and does
not destabilize the estimator or controller. We are currently working to use this controller
for walking using an online MPC solver such as (b. Wieber, 2006) and eventually (Herdt
et al., 2010) which additionally plans reactive footsteps; both of these MPC solvers have
been implemented in simulation and enable robust walking control.
43
0
0.05
0.1
COM x (m)
−0.04
−0.035
−0.03
COM y (m)
−0.24
−0.22
−0.2
−0.18
−0.16
COM z (m)
−0.78
−0.76
−0.74
−0.72
−0.7
Foot z (m)
5 10 15 20 25 30 35
0
50
100
150
Time (sec)
Push Force (N)
Figure III.8: QP-Based Inverse Dynamics control using the estimated base pose for a
single-legged balancing task in which the desired COM moves sinusoidally in the vertical
direction while the swing foot is set to maintain a position. Again, we disturb the robot
during the portions of the plot having a grey background.
III.5 Summary
In this chapter, we have:
Developed an EKF based on (Bloesch et al., 2012) which incorporates endeector
rotational constraints for humanoid base pose estimation.
Described the implementation details related to linearization and discretization of
the lter prediction and measurement models (more details on which can be found
in the corresponding Appendices).
44
Performed a nonlinear observability analysis on the estimator, concluding that the in-
clusion of
at foot constraints simplies and reduces the number of motion-dependent
cases in which the observability matrix loses rank.
Conducted experiments in simulation on ZMP-based walking data which demonstrate
the superiority of the
at foot-constrained approach presented in this work in terms
of estimation error.
Begun to characterize the performance of our base pose estimator in the whole-body
inverse dynamics control framework of Section II.3 on the real robot for single legged
balancing tasks with disturbances.
45
Chapter IV
COM and Momentum Estimation
In the past, researchers (Kwon and Oh, 2007) have recognized the challenge of performing
ZMP preview control using inaccurate COM information. Their solution was a Kalman
Filter (KF) which fused a LIPM-based process model and the expected motion of the
COM, computed from the previewed ZMP and resolved into joint angles. This relied
heavily on the expected ZMP, making it suitable only for open-loop walking on simple
terrain. Endeector contact wrench sensors were used but the simplied model limited
their utility. Rotational motion (angular momentum) was ignored.
Simplied models were again used for estimation of COM position/velocity, center of
pressure (COP), COM osets and external forces (Stephens, 2011). This work introduced
lters based on the LIPM, analyzed their observability and demonstrated their perfor-
mance. However, the use of a LIPM-based process model compromises the lter optimality
which can lead to delays and even destabilize control. In addition, the observability of a
simplied model is not the same as that of the original system. Third, use of the COP
limits these estimators to planar terrain. Finally, this work does not consider estimation
of angular momentum.
Another work (Xinjilefu and Atkeson, 2012) investigated whether simplied models are
sucient for estimation. They fused LIPM dynamics with COM and COP measurements
in one estimator and a planar, ve-link model with joint angle measurements and inertial
measurements from an IMU in another. The estimators were evaluated on simulated data
with added biases. However, biases were not modeled explicitly; instead, noise parameters
46
were tuned to account for them. No observability analysis was performed, making the
lters dicult to analyze; they are dicult to compare since they serve dierent purposes.
In contrast to the above approaches, (Hashlamon and Erbatur, 2013) presented a COM
and COP estimator which does not require contact wrench sensors. Contact wrenches and
COPs were determined from IMU acceleration by solving an optimization problem using
contact constraints. However, they assumed the IMU measures COM acceleration and that
the angular momentum is constant. They then presented a LIPM-based KF which uses the
computed COP as an input to the LIPM dynamics. The resulting lter, despite providing
COP estimates without using contact wrench sensors, demonstrates poor performance
likely due to its strong assumptions.
Another recent work (Xinjilefu et al., 2015) focuses exclusively on COM estimation in
a manner similar to that in (Stephens, 2011) but with applications for inverse dynamics
control on the Atlas humanoid. A COM oset is estimated by a LIPM-based lter and
used as a virtual force to compensate for disturbances in inverse dynamics control as well
as to compute the capture point of the robot which helps predict falls. This work stands
out as the only estimation approach from the DRC which explicitly considers estimation
of the COM for use in control.
Finally, (Carpentier et al., 2016) takes a dierent approach to COM estimation by
fusing kinematics and measured contact wrenches in the frequency domain using a com-
plementary lter according to the spectral characteristics of each source of information.
An observability analysis is presented and lter is successfully applied on human running
data (using a Vicon system and an approximate mass model); application on a humanoid
instead would be straightforward. This approach however does not consider momentum or
external wrench estimation.
This chapter introduces several dierent estimators which address the shortcomings of
previous approaches by using dynamics consistent with the full robot model to estimate the
COM, linear and angular momentum as well as COM and momentum osets and external
wrenches.
47
IV.1 Models of Robot Dynamics
In recent work, we have implemented a momentum controller derived as a state feedback
controller using Linear-Quadratic Regulator (LQR) design and realized using a hierarchical
QP-based inverse dynamics solver (Herzog et al., 2014b). Similarly, we can modify the
inverse dynamics solver presented in III to control momentum rather than the COM and
base pose. We thus propose a momentum estimator to use with this class of controllers
based on the same dynamics as given below. Aside from new notation, these equations are
identical to Eq (II.5) but summed about the point at which the contact wrench sensor is
located rather than about the COP.
_ c =
1
m
l (IV.1)
_
l =
M
X
i=1
(F
i
+w
F
i
) +mg (IV.2)
_
k =
M
X
i=1
(p
i
c) (F
i
+w
F
i
) +
M
X
i=1
(
i
+w

i
) (IV.3)
Here c, l and k are the COM, linear and angular momentum respectively; p
i
, F
i
and
i
are the i
th
contact point position, force and torque respectively. The vectors w
F
i
and w

i
represent additive white Gaussian sensor noise processes with standard deviations q
F
and
q

. These dynamics, illustrated in Figure IV.1, are preferable to the full dynamics for
prediction because they avoid using inaccurate link inertia models and instead use only the
total mass m, which can be measured.
The estimators in (Stephens, 2011) are derived from the LIPM dynamics
x =
g
z
c
(xx
COP
) (IV.4)
where x denotes the COM position, z
c
is the (constant) height of the COM and x
COP
denotes the COP position. The dynamics of the y direction are exactly the same since
the LIPM is decoupled. This equation can be derived from (IV.2) and (IV.3) by replacing
48
Figure IV.1: Illustration of the momentum dynamics.
p
i
with the COP, neglecting rotation and constraining motion to a plane as was done in
Chapter II to obtain Eq (VI.2).
For use with less-dynamic controllers, an estimator based on (IV.4) may prove sucient.
Most previous studies hence used the LIPM to avoid using the full dynamics. However, for
linear systems it can be shown (Friedland, 1995) that an inaccurate process model can lead
to unstable estimation error dynamics; stability proofs assume exact plant cancellation.
Further, an inaccurate model can destabilize both the controller and observer. This is
because the separation principle - which says that the controller and observer can be
designed independently - no longer holds. Of course, since the true dynamics are nonlinear,
these theorems can only be applied qualitatively to inform design. This suggests avoiding
simplied models for estimation when possible.
IV.2 Estimators
In this section, we introduce four estimators based on the momentum dynamics, each serv-
ing a dierent purpose. We choose to implement these using Extended Kalman Filters,
49
however the observability results of Section IV.3 hold for any implementation. Each esti-
mator's nonlinear process dynamics are used for prediction and linearized for the update
step as follows.
_ x =f(x;w) ! _ x =Ax +Lw
y =h(x;w) ! y =Cx +v
where x, w and v are the state, process noise, and measurement noise vectors and A =
@f
@x
, L =
@f
@w
and C =
@h
@x
are the relevant Jacobians. While the controls F and are
used in prediction and in forming Jacobians, they do not explicitly appear in the EKF
formulation. We specify estimators by their nonlinear process and measurement models
and the Jacobians A, L and C resulting from linearization.
IV.2.1 Momentum Estimator
On our humanoid, joint velocities are computed by dierentiating noisy potentiometer
signals; momentum is computed from kinematics using these noisy velocities and base
information from a base-state estimator developed in previous work (Rotella et al., 2014)

.
We currently lter momentum using second-order Butterworth low-pass lters but this
introduces delays. By integrating low-noise measured contact wrenches and using the noisy
kinematics computations as measurements, we can lter momentum without inducing as
large a delay.
We choose the state x = [c;l;k] so the process model is (IV.1)-(IV.3) and the mea-
surement model is y = [c;k]; we measure the COM and angular momentum computed
from base information, kinematics and inertias. These measurements have noise standard

Note that since the COM and momentum are functions of the
oating base pose and its velocity, we
could choose to combine the base state estimator with the COM and momentum estimator proposed here.
However, we wish to decouple estimation of the base pose (which is a kinematic quantity) from estimation
of the momenta (which are functions of the dynamic model).
50
deviationsr
c
andr
k
, respectively. Although inaccurate inertias are used to compute k, we
demonstrate robustness to model errors in Section IV.4. The dynamics are
A =
2
6
6
4
0
1
m
I 0
0 0 0
P

F

i
0 0
3
7
7
5
; L
i
=
2
6
6
4
0 0
I 0
( p
i
c)

I
3
7
7
5
C =
2
4
I 0 0
0 0 I
3
5
wherea

denotes the cross product matrix formed from vector a. The noise JacobianL is
formed by concatenating L
i
horizontally for all i = 1:::M contacts.

F
i
, p
i
and c are the
measured force and the foot and COM positions, respectively, treated as constants over a
single timestep.
IV.2.2 Oset Estimator
The kinematic model of a robot is often imprecise. Here, we assume the total mass m is
known but that incorrect link masses and COM locations contribute to a conguration-
dependent COM oset. These errors also create osets in momentum; we thus extend the
state to x = [c;l;k; c; l] where c and l are the COM and linear momentum oset
vectors, respectively. In theory, there is also an oset k but this is unobservable (see
Section IV.3) so we choose not to estimate it. Since the oset dynamics are unknown, we
assume random walks so the prediction dynamics are (IV.1)-(IV.3) plus _ c = w
c
and

_
l = w
l
where w
c
and w
l
are additive Gaussian white noise processes with standard
deviations q
c
and q
l
. The measurement model is
y =
2
6
6
6
6
6
6
6
6
4
c +
2
6
6
4
c
x
c
y
0
3
7
7
5
l + l
k
3
7
7
7
7
7
7
7
7
5
51
where the linear momentum measurement noise has standard deviation r
l
. This equation
indicates that the kinematics-based COM and linear momentum measurements contain
osets. It is shown in Section IV.3 that c
z
is unobservable and is thus ignored. The
linearized dynamics are given by
A =
2
6
6
6
6
6
6
6
6
4
0
1
m
I 0 0 0
0 0 0 0 0
P

F

i
0 0 0 0
0 0 0 0 0
0 0 0 0 0
3
7
7
7
7
7
7
7
7
5
;L
i
=
2
6
6
6
6
6
6
6
6
4
0 0
I 0
( p
i
c)

I
0 0
0 0
3
7
7
7
7
7
7
7
7
5
;C =
2
6
6
4
I 0 0 I
2
0
0 I 0 0 I
0 0 I 0 0
3
7
7
5
where I
2
= diag(1; 1; 0) since we ignore c
z
. Because the dynamics of c
z
aect the
estimate of l
z
, it is best to ignore this oset and accept that it cannot be observed; this
is why we write the measurement this way. Note that we do not explicitly consider link
inertia errors, however these would contribute only to the unobservable oset k.
y
IV.2.3 COP-Based Oset Estimator
In the above estimator, c
x
and c
y
are observable due to the dynamics of the angular mo-
mentum measurement (see Section IV.3 for details). However, this measurement is subject
to an oset k. While ignored above, this leads to degraded performance for signicant
modeling errors. We add the COP measurement to give us force-based information about
the COM which is accurate despite modeling errors. This comes at the disadvantage of
assuming coplanar contacts.
We use the same state and add the COP measurement (with noise standard deviation
r
COP
). We keep the measurement of k and increase r
k
relative to r
COP
in order to lter
y
Unobservable states can drift arbitrarily; when coupled to other states through their dynamics, this drift
aects their estimation. Because of this, adding an unobservable oset k would corrupt the information
about c which is provided by the angular momentum measurement. We are better o incorrectly assuming
that k = 0 and accepting that our estimates of c may be inaccurate depending on the severity of the
modeling errors. The lter of Section IV.2.3 addresses this.
52
angular momentum while relying primarily on the COP to render c
x
and c
y
observable.
The measurement model is augmented with the measurement
y
COP;x
=c
x

1
P
F
i;z

c
z
X
F
i;x
+
_
k
y

y
COP;y
=c
y

1
P
F
i;z

c
z
X
F
i;y

_
k
x

where
_
k =
P
(p
i
c)F
i
+
P

i
. The measurement Jacobian is
C =
2
6
6
6
6
6
4
I 0 0 I
2
0
0 I 0 0 I
0 0 I 0 0
H 0 0 0 0
3
7
7
7
7
7
5
; H =
2
4
2 0
2
P

F
ix
P

F
iz
0 2
2
P

F
ix
P

F
iz
3
5
where H is the Jacobian relating the COM to the COP.
IV.2.4 External Wrench Estimator
It is often the case that contact force/torque (F/T) sensors drift or disturbances are applied
to the robot. We begin with the Momentum Estimator state and introduce an external
wrench acting at the COM which encapsulates these errors. The state becomes x =
[c;l;k; F; ] where F; are the external force and COM torque (torque computed
about the COM). Again, we assume random walks for each so the dynamics are
_ c =
1
m
l
_
l =
M
X
i=1
(F
i
+w
F
i
) +mg + F
_
k =
M
X
i=1
(p
i
c) (F
i
+w
F
i
) +
M
X
i=1
(
i
+w

i
) +

_
F =w
F
_ =w

53
where w
F
and w

have standard deviations q
F
and q

. The measurement model is
y = [c;k] as in the original Momentum Estimator. The linearized dynamics are
A =
2
6
6
6
6
6
6
6
6
4
0
1
m
I 0 0 0
0 0 0 I 0
P
F

i
0 0 0 I
0 0 0 0 0
0 0 0 0 0
3
7
7
7
7
7
7
7
7
5
;L
i
=
2
6
6
6
6
6
6
6
6
4
0 0
I 0
( p
i
c)

I
0 0
0 0
3
7
7
7
7
7
7
7
7
5
;C =
2
4
I 0 0 0 0
0 0 I 0 0
3
5
IV.3 Observability Analysis
This section investigates the observability of each of the estimators proposed in the previous
section. Since the nonlinearity of the process models is introduced through the product of
the state and input (contact wrench) noise, and since observability is analyzed by assuming
the noise is zero, the proposed estimators can be analyzed using observability analysis for
linear systems. However, the nonlinear observability analysis process produces the same
results. Details on analyzing observability for both linear and nonlinear systems can be
found in Appendix C.
IV.3.1 Momentum Estimator
The observability matrix is
O =
2
6
6
6
6
6
6
6
6
4
I 0 0
0 0 I
0
1
m
I 0
P
F

i
0 0
0
1
m
P
F

i
0
3
7
7
7
7
7
7
7
7
5
This matrix has full rank so the state is observable. We could add a kinematics-based
linear momentum measurement but it would be redundant since it is computed from the
54
same sensors as the COM. Since linear momentum is a function of noisy velocities, it is
subject to considerable noise; it is better to leave it out and let dierentiation occur in the
lter.
IV.3.2 Oset Estimator
The observability matrix is given below; we include the full c in the state to prove that
it cannot be observed.
O =
2
6
6
6
6
6
6
6
6
6
6
6
4
I 0 0 I

0
0 I 0 0 I
0 0 I 0 0
0
1
m
I 0 0 0
P
F

i
0 0 0 0
0
1
m
P
F

i
0 0 0
3
7
7
7
7
7
7
7
7
7
7
7
5
This matrix is rank decient; the unobservable subspace is a linear combination of the
COM and COM oset. This occurs because the skew-symmetric matrix
P
F

i
has at most
rank two. In theory, this means we could observe any two directions of the COM oset,
eectively replacing I

in O. However, when the robot is stationary on
at ground,
X
F

i
=
2
6
6
4
0 mg 0
mg 0 0
0 0 0
3
7
7
5
forcing the z direction to be unobservable. Either the x or y direction can be made
unobservable in its place only when
P
F
i;x
or
P
F
i;y
is nonzero. This occurs only when an
force acts on the robot in these directions at the COM (when the robot accelerates or is
on a slope so gravity aects x and/or y). For this reason, we set I

=I
2
= diag(1; 1; 0) so
that c
x
and c
y
are observable. Note that
P
F
i
= 0 for a robot in
ight phase; in this
case, c is completely unobservable.
55
IV.3.3 COP-Based Oset Estimator
The observability matrix is
O =
2
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
I 0 0 I
2
0
0 I 0 0 I
0 0 I 0 0
H 0 0 0 0
0
1
m
I 0 0 0
P
F

i
0 0 0 0
0
1
m
P
F

i
0 0 0
0
1
m
H 0 0 0
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
Observability is the same as for the Oset Estimator; however, the COP provides informa-
tion about the COM derived from F/T sensors rather than from kinematics. This improves
oset estimation in the case when the unmodeled angular momentum oset is large. This
makes the important point that adding redundant measurements can aect performance
even though observability is unchanged.
IV.3.4 External Wrench Estimator
The observability matrix is
O =
2
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
I 0 0 0 0
0 0 I 0 0
0
1
m
I 0 0 0
P
F

i
0 0 0 I
0 0 0
1
m
I 0
0
1
m
P
F

i
0 0 0
0 0 0
1
m
P
F

i
0
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
56
This matrix has full rank - since the external force and torque appear in the momentum
dynamics, and since momentum is observable through the COM and angular momentum
measurements, the external wrench is observable.
IV.3.5 Oset and External Wrench Estimator
If we extend the Oset Estimator state so that x = [c;l;k; c; l; F; ] then we obtain
the matrix
O =
2
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
I 0 0 I

0 0 0
0 I 0 0 I 0 0
0 0 I 0 0 0 0
0
1
m
I 0 0 0 0 0
0 0 0 0 0 I 0
P
F

i
0 0 0 0 0 I
0 0 0 0 0
1
m
I 0
0
1
m
P
F

i
0 0 0 0 0
0 0 0 0 0
1
m
P
F

i
0
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
This matrix is rank decient; its nullspace is a linear combination of c, c and . This
implies that we can never observe both the osets and an external wrench simultaneously.
However, if the external force is applied at the COM then = 0 and the state (except
c
z
) becomes observable.
IV.4 Experiments and Results
All results presented in this work were obtained in the SL simulation environment (Schaal,
2007). White Gaussian noise was generated based on data from Hermes, discretized at
the ltering frequency and added to simulated sensor outputs. Noise having standard
deviationq

was added to joint angles (and propagated to joint velocities through numerical
dierentiation) as well as to endeector F/T sensor signals. Table VI.1 below lists the values
of the standard deviations of the simulated noise processes.
57
Figure IV.2: Estimation of COM (top row), linear momentum (middle row) and angular
momentum (bottom row); columns denote x,y and z. RMS errors for the relevant lters
are shown on each plot.
Figure IV.3: Estimation of linear mo-
mentum for increasing frequencies (Top:
LIPM Filter. Bottom: Momentum Esti-
mator)
Figure IV.4: Zoomed-in views of angu-
lar momentum estimates versus low-pass
ltered kinematics measurements.
58
Table IV.1: Simulated sensor noise standard deviations. Corresponding values for 1kHz
sampling rate are shown.
Continuous Discrete (1kHz)
q

0:00000316rad=
p
Hz 0:0001rad
q
F
0:06325N=
p
Hz 2N
q

0:00316Nm=
p
Hz 0:1Nm
In this and subsequent sections, we refer to the four lters based on the momentum
dynamics as the ME (Momentum Estimator), OE (Oset Estimator), COE (COP-Based
Oset Estimator) and EWE (External Wrench Estimator). Process noise parameters were
set using the values in Table VI.1. All other noise parameters were tuned for each lter
and are summarized in Table IV.2. Note that measurement noise standard deviations are
specied in discrete time. All lters based on the LIPM are referred to as LIPMF in this
section and denote the corresponding lter and noise parameters introduced in (Stephens,
2011).
Estimation was performed during a 15s ZMP preview control-based walking task (Ka-
jita et al., 2003) having single and double support phases lasting 0:5s each and a forward
motion of 5cm per step for 10 steps. Since this is a relatively-dynamic gait, the contacts
created are often not completely
at and subject to impulsive contact wrenches; this was
desired in order to test the estimators with realistic contact switching. Ideal base state
estimation was assumed for simplicity. Estimation and data recording were performed at
1kHz unless noted.
IV.4.1 Momentum Estimator
Both the LIPMF and ME accurately estimate the COM well as shown in Figure IV.2 (note
that the LIPMF does not estimateCOM
z
,LM
z
andAM). This is expected since the COM
59
Table IV.2: Noise standard deviations for momentum-based estimators. N/A indicates
that a parameter is not used.
Name (Units) ME OE COE EWE
q
c
(m=
p
Hz) 1.0 1.0 1.0 N/A
q
l
(kgm=s=
p
Hz) 1.0 1.0 1.0 N/A
q
F
(N=
p
Hz) N/A N/A N/A 1.0
q

(Nm=
p
Hz) N/A N/A N/A 0.1
r
c
(m) 0.0001 0.001 0.001 0.00001
r
l
(kgm=s) N/A 1.0 1.0 N/A
r
k
(kgm
2
=s) 0.1 1.0 10.0 0.01
r
COP
(m) N/A N/A 1.0 N/A
Figure IV.5: Estimation of a 5cm COM
oset while stationary (top) and estima-
tion of a conguration-dependent COM
oset during the 15s walking task (bot-
tom).
Figure IV.6: Time-varying COM osets
for dierent n (representing dierent de-
grees of modeling error) in x-direction
(top) and y-direction (bottom.
60
Figure IV.7: Estimation of COM (top row), linear momentum (middle row) and angular
momentum (bottom row) for dierentn (representing dierent degrees of modeling error).
Red denotes the largest modeling error while yellow denotes the least.
Figure IV.8: COM estimation for n = 5
inx-direction (top) andy-direction (bot-
tom).
Figure IV.9: Linear momentum estima-
tion for n = 5 in x-direction (top) and
y-direction (bottom).
61
is measured directly and r
c
was chosen small in both. However, linear momentum estima-
tion is signicantly better using the ME since the dynamics of this state are inaccurate in
the LIPMF; the given RMS errors demonstrate this.
The dierence in performance between the two lters is more pronounced when they
are updated at slower rates. Figure IV.3 shows estimation of l
y
by each estimator for
update rates of 50Hz, 125Hz, 250Hz, 500Hz and 1000Hz (lower frequencies are plotted
in red, higher ones in yellow and ground truth in black). The degradation of performance
with decreases in update rate is much more severe for the LIPMF.
The ME was motivated by the desire to avoid low-pass ltering computed momentum.
Figure IV.4 shows the kinematics-based angular momentum ltered using dierent cutos
and compared to the estimated angular momentum. While the estimate lags by several
milliseconds, the delays induced by the Butterworth lters are as large as 20ms. For
dynamic motions, ltering can even change the structure of the signal.
IV.4.2 Oset Estimator
First, a constant oset of c
x
= 5cm was added directly to the COM measurement, as
was done in (Stephens, 2011) (assuming l and k remain zero). Both lters converge
to the true COM, though the OE provides more accurate estimates as shown in the top
plot of Figure IV.5. This is likely due to the fact that a physically-consistent simulator
and realistic sensor noise are used here, unlike in (Stephens, 2011). Next, conguration-
dependent osets in the COM and momentum were created using a set of perturbed link
parameters. The LIPMF manages to estimate the COM but with delays up to 100ms as
shown in the bottom plot of Figure IV.5. This is the result of using simplied dynamics
and was not improved with tuning.
The fact that the OE can track the COM and linear momentum osets despite the
unmodeled angular momentum oset suggests robustness to unmodeled errors. In order to
analyze this we estimate osets for ve dierent sets of link mass parameters. Each was
generated by adding to every mass-weighted link COM position m
i
x
COM
i
an oset drawn
from a zero-mean Gaussian having standard deviation n(m
i
x
COM
i
) with n = 1; 2; 3; 4; 5.
62
Figure IV.6 shows the generated conguration-dependent COM osets throughout the
walking task, with red denoting the most perturbed parameters (n = 5). The COM oset is
as large as 4cm inx and nearly twice that iny forn = 5. Each perturbed model also results
in osets in c
z
and momentum but these are not shown to save space. Figure IV.7 shows
the performance of the OE for the dierent models; black denotes ground truth. Note that
angular momentum estimation is poor yet COM and linear momentum estimation remain
relatively accurate even for larger values of n, demonstrating OE robustness.
IV.4.3 COP-Based Oset Estimator
It is clear that performance of the OE is degraded for signicant modeling errors due to
the unmodeled angular momentum oset. By including the force-based COP measurement
and tuning r
COP
against r
k
, we achieve accurate COM and linear momentum estimation
for n = 5 as shown in Figures IV.8 and IV.9. Also shown are the corresponding LIPMF
estimates; the LIPMF estimates the COM fairly well but with signicant delay while linear
momentum estimation is poor due to the unmodeled oset which is signicant for n = 5.
IV.4.4 External Wrench Estimator
We rst estimate a constant external force equal to half the robot's weight as in (Stephens,
2011). This is applied at the left hip of the robot in the y-direction, creating associated
torques in x and z. The top row of Figure IV.10 shows that the EWE quickly converges
to the true external force and torque. The LIPMF converges to the wrong value, likely
because unlike in (Stephens, 2011), the force is physically applied and there is realistic
sensor noise. This could not be improved with tuning.
The second row of Figure IV.10 shows estimation of a 10N force applied in the same
manner during the walking task. Despite the fact that the robot is subject to impulsive
forces throughout, the EWE provides accurate estimates of both external force and the
small, conguration-dependent COM torques. The LIPMF has diculty estimating the
value of the force due to impulses resulting from contact switching.
63
Figure IV.10: External wrench estimation for a constant external force in globaly-direction
while stationary (top row), while walking (middle row) and for an impulsive force (bottom
row) while walking (with process noise values of 0:1 and 1:0)
Finally, the bottom row of IV.10 shows estimation of a 50N disturbance applied at time
10s for 0:5s during the walking task. The EWE overshoots the force value but performs
much better than the LIPMF overall, exhibiting a fast rise time. As shown in blue, EWE
estimation during the transient disturbance is improved by increasing the external torque
process noise. However, estimates become much noisier.
IV.5 Closed-Loop Control Using Momentum Estimators
Ultimately, the purpose of the estimators introduced in this chapter is to improve whole-
body control in the presence of modeling errors or applied external wrenches. We now
detail how to use these estimated quantities for control.
64
IV.5.1 Oset Estimators
The COM and linear momentum osets resulting from either the OE or the COE can
be used in a number of ways. The most straightforward approach is to use the resulting
COM and linear momentum estimates to adjust the desired COM position and velocity,
respectively, in the inverse dynamics controller introduced in Section II.3. Alternatively,
since the eect of a pure COM oset is a torque about the model COM (since the force
due to gravity acts at a dierent distance from the model COM), this oset can be treated
as pure moment in an additional wrench

ext
=
2
4
0
mgc
3
5
to be compensated in inverse dynamics as
M
l
(q) q +h
l
(q; _ q) =J
T
c;l
+J
T
ext

ext
(IV.5)
where J
ext
=
h
I J
c;l
i
. This is essentially the approach taken by Xinjilefu et al. (2015),
however the COM oset was estimated from a LIPM-based estimator (similar to the LIPMF
of this chapter); using the LIPM dynamics, a COM oset can be written as a force in
m!
2
c where ! =
p
g=z
c
and z
c
again is the constant COM height. This creates a
moment about the COP equivalent to
ext
above if z
c
is the true COM height.
IV.5.2 External Wrench Estimator
In the previous section, we informally proved that a COM oset has the same eect on
the robot dynamics as an external moment at the COM; this is in agreement with the
observability analyses of Section IV.3, which showed that it is impossible to distinguish
between a COM oset and an external wrench. We thus treat the combined eect of
potential modeling error and forces applied at arbitrary locations on the robot as a single
external wrench
ext
at the COM, which we seek to estimate and compensate in inverse
dynamics as in (IV.5).
65
Simulation Experiments
In order to evaluate the proposed method of compensating for an external wrench, we rst
apply simulated forces to the robot while updating the EWE as in Section IV.4.4, however
neglecting simulated sensor noise for simplicity. A force is applied at the robot base in
each Cartesian direction (in base frame), with increasing amplitude over a short time; this
force results in an external wrench about the COM which is estimated and compensated
using (IV.5). Figure IV.11 shows the applied and estimated wrench, which is estimated
accurately and with a fast response time. This allows the robot to compensate for the
increasing load in all directions. The same load was applied to the robot without the use
of external wrench compensation in inverse dynamics; the resulting COM tracking both
with and without load compensation is shown in Figure IV.12.
There is a lag in the estimation which causes the COM to move slightly even when using
the EWE, however the deviation is much smaller than when external wrench compensation
is not used. Note that the amount of deviation of the COM from its desired state is a
function of the chosen Cartesian feedback gains; by using an external wrench estimator
such as the one proposed in this work, we can theoretically lower the COM control gains
while achieving the same robustness to perturbations. Modulating the feedforward torque
through estimation is desirable over sti control because high gains can lead to instability.
The eect of a load is much more pronounced at higher load amplitudes - consider an
experiment in which an increasing external force in the direction opposite gravity simulates
adding 100N of additional weight (which is roughly one-fth the total weight of the robot)
in 20N increments (for example, handing the robot packages to carry). This is done while
the robot is walking in place; without compensating for the wrench estimated by the EWE,
the robot quickly goes unstable as shown in Figure IV.13.
Real Robot Experiments
When controlling the real robot, one must deal with a variety of issues - both modeling
errors which create COM and momentum osets as well as external wrenches caused by
66
Actual
Estimated
0
20
0
20
0
20
10
5
0
0
5
0.0
0.5
F
x
(N) F
y
(N) F
z
(N)
x
(Nm) y
(Nm)
z
(Nm)
4
6 8 10 12 14 16
Time (s)
Figure IV.11: External wrench estimation for an increasing simulated load at the robot
base in dierent directions. The estimated wrench is actively being compensated in the
inverse dynamics controller.
Desired
Using EWE
No EWE
6 8 10 12 14 4 16
c
x
(m ) y
c (m )
-0.007
-0.003
0.003
0.008
-0.058
-0.052
-0.048
-0.357
-0.352
-0.347
-0.342
c
z
(m )
Time (s)
Figure IV.12: External wrench estimation for an increasing simulated load at the robot
base in dierent directions. Load compensation prevents the robot from deviating from its
desired COM position.
67
− 100
− 80
− 60
− 40
− 20
0
F
z
(N)
Actual
Estimated
− 0.44
− 0.42
− 0.40
− 0.38
− 0.36
c
z
(m )
Desired
Using EWE
No EWE
4 5 6 7 8 9 10
Time (s)
Figure IV.13: Top: External wrench estimation for an increasing simulated load at the
robot base in thez direction during walking (approximately one-fth the robot's weight).
Bottom: COM height tracking, showing the robot going unstable without the EWE.
68
Figure IV.14: Loading of the robot during a single-leg squatting task in ve pound (20N)
increments, both with and without external wrench estimation/compensation.
the hydraulic hoses and disturbances. We thus seek to evaluate the EWE for use in
compensating these modeling errors on the real robot.
For the following experiments, the same estimator noise parameters are used as in
simulation (these were originally based on noise statistics from the actual sensors). We
perform a single-leg balancing task in which the robot squats, shifts its COM over one foot
and lifts its leg after switching contacts based on a measured normal force threshold. The
EWE is updated throughout the experiment (across the contact change, demonstrating
proper contact handling); the resulting estimated external torque is smoothly ramped
to its full value over several seconds starting at the beginning of the task. In order to
focus on disturbances created in particular directions, a selection matrix is specied which
premultiplies the external wrench sent to the inverse dynamics solver.
69
First, we performed a similar experiment to that shown in Figure IV.13 in which the
robot was incrementally loaded with 5 pound weights (weighing roughly 20N each) up to
approximately 100N during a single-leg squatting task as shown in the video snapshot of
Figure IV.14. As shown in the top portion of Figure IV.15, the resulting external force
in the direction oposite gravity is estimated well, despite the contact switch and ongoing
squatting task. The estimated external force in the negative z-direction was compensated
in the inverse dynamics solver, preventing the COM from moving downwards more than
several millimeters despite the signicant additional load.
In contrast, Figure IV.16 shows the COM height during the same loading experiment
without use of the external wrench estimator for compensation. The COM moves down
signicantly more and eventually the robot goes unstable after being loaded to 100N, unable
to continue the squatting task in the presence of the external force.
Second, we performed an experiment in which the robot was perturbed impulsively
with a push rod on which a force/torque sensor was attached. The top plot of Figure IV.17
shows the measured disturbance force while the bottom plot shows the COM x-direction
tracking without using external wrench compensation. The COM is seen to move on the
order of a centimeter in the same pattern as the push force. In an experiment with similar
disturbances, external wrench estimation was used to accurately estimate the push force
(up to an oset perhaps due to the hydraulic hose forces) as shown in the top of Figure
IV.18. The resulting COM tracking improves as a result, as shown in the bottom portion
of the gure. Again, note that these results are dependent on the chosen COM control
gains; a more dramatic dierence in tracking would be seen were the COM control gains
lowered.
IV.6 Summary
Overall, the momentum-based lters perform better than the corresponding LIPM-based
lters of (Stephens, 2011) with only the disadvantage of being slightly more complex. The
estimator to use depends on the application, though we expect both osets and external
70
− 100
− 80
− 60
− 40
− 20
0
F
z
(N)
Estimated
Actual
15 20 25 30 35 40 45 50
− 0.187
− 0.186
− 0.185
− 0.184
− 0.183
− 0.182
− 0.181
c
z
(m)
Desired
Using EWE
Time (s)
Figure IV.15: Loading of the robot during a single-leg squatting task in ve pound (20N)
increments while estimating the external force (top) and using it for feedforward compen-
sation to prevent the COM from moving downwards (bottom). Note that the time at
which each weight was added is not known precisely, thus the increasing load magnitudes
are shown as horizontal lines in the top plot.
71
15 20 25 30 35 40 45 50
150
125
100
75
50
25
0
F
z
(N)
Estimated
Actual
15 20 25 30 35 40 45 50
0.188
0.187
0.186
0.185
0.184
0.183
0.182
c
z
(m)
Desired
No EWE
Time (s)
Figure IV.16: External wrench estimation for an increasing simulated load at the robot
base in dierent directions. Load compensation prevents the robot from deviating from its
desired COM position.
wrenches to exist on the real robot. Based on the analysis of Section IV.3.5 we expect a
combined estimator may work well with proper tuning if the external COM torque is small.
Alternatively, we can simplify the problem and treat a COM oset is as an external force
as described in Section IV.5, motivating the use of only the external wrench estimator; this
was done in order to test the EWE for closed-loop control on the real humanoid.
The results of the experiments performed in the previous section are summarized below.
The ME and LIPMF both estimate the COM well but the ME performs better at
estimating linear momentum.
LIPMF performance degrades much more rapidly than ME performance for decreas-
ing update rates.
The ME lters the COM and momentum with much less delay than a typical low-pass
lter.
72
0
10
20
30
40
50
F
x
(N)
Actual
10.0 12.5 15.0 17.5 20.0 22.5 25.0 27.5 30.0
Time (s)
0.012
0.010
0.008
0.006
0.004
0.002
0.000
c
x
(m)
Desired
No EWE
Figure IV.17: Perturbing the robot in the x-direction while measuring the external force
(top), resulting in poor COM tracking (bottom).
73
− 20
− 10
0
10
20
30
40
F
x
(N)
Estimated
Actual
10.0 12.5 15.0 17.5 20.0 22.5 25.0 27.5 30.0
Time (s)
− 0.014
− 0.012
− 0.010
− 0.008
− 0.006
− 0.004
c
x
(m)
Desired
Using EWE
Figure IV.18: Perturbing the robot in thex-direction while both measuring and estimating
the external force (top) and using it for feedforward compensation to prevent the COM
from moving (bottom).
74
Both the OE and LIPMF estimate a constant COM oset well but the OE performs
much better for conguration-dependent osets since it estimates l.
The COE estimates osets more accurately than the OE in cases of large modeling
errors.
The EWE accurately estimates constant, time-varying and impulsive external wrenches
even during walking.
The EWE can be used in closed-loop, inverse dynamics control in order to eectively
compensate for potentially-destabilizing external loads, as shown in both simulated
and real-robot experiments.
75
Chapter V
Joint State Estimation
Feedback control for robots relies on accurate estimates of the joint state. Traditionally,
the position of each joint is measured using an angular sensor from which joint velocity
and acceleration are computed via numerical dierentiation. This produces quantities
subject to considerable noise, requiring low-pass ltering for use in control. However,
ltering introduces a potentially-destabilizing time delay, preventing the use of feedback
gains high enough to achieve satisfactory stiness and damping; see Figure V.1. Rather
than dierentiating joint positions, we develop methods for computing joint velocity and
acceleration from sensors which measure quantities of the same order. These estimates -
derived from gyroscopes and accelerometers - can be used for control directly or through
fusion with position sensing in an optimal ltering framework.
While compact and aordable inertial sensors are fairly new, various types of accelerom-
eters have been used in sensor fusion for decades. One of the rst works to compute rigid
body angular velocity from accelerometers was (Schuler et al., 1967). (Padgaokkar et al.,
1975) introduced methods for computing angular acceleration, with (Liu, 1976) demon-
strating that nine axes are needed for stable solutions. (Zappa et al., 2001) proved that 12
accelerometer axes are actually required to avoid angular velocity singularities.
Human joint angle estimation has been researched extensively in the biomechanics
community. (Williamson and Andrews, 2001) attached IMUs to a subject and integrated
gyroscope signals to obtain knee joint angles. (El-Gohary, 2013) developed a human arm
model used to derive relations between limb IMU measurements and joint state for use in
76
4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 −4
−3
−2
−1
0
1
2
3
4
Time (s)
Velocity (rad/s)
Figure V.1: Illustration of the noisy velocity signal (in blue) computed by dierentiating
a raw joint potentiometer signal and the corresponding delayed signal (in green) produced
by low-pass ltering.
a Kalman Filter. (Seel et al., 2014) determined joint axes and locations from limb IMUs
using knowledge of human kinematic constraints.
Research on robot state estimation using multiple IMUs, however, has been limited.
For base state estimation, (Lin et al., 2006) developed a 12-axis accelerometer suite for
their robot, later adding a three-axis gyroscope to avoid singularities. They also proved
a three-axis gyroscope plus six accelerometer axes are sucient to compute angular ac-
celeration if distributed among three distinct locations (Lin et al., 2012). For joint angle
estimation, (Cheng and Oelmann, 2010) surveyed methods for robots lacking angular sen-
sors. (Santaera et al., 2015) sensorized manipulator links and used integrated orientation
and kinematics to determine joint angles. (Vihonen et al., 2013) developed an estimator
for robots lacking sensors which relied on tilt from accelerometers compensated for inertial
eects using gyroscope-based joint velocities, also compensating biases through comple-
mentary ltering. In (Vihonen et al., 2013), numerical joint accelerations were replaced
77
Figure V.2: Inertial Measurement Units (IMUs) attached to the thigh, shank and foot of
a Sarcos hydraulic humanoid.
with estimates from accelerometers and then used to estimate velocities in a complemen-
tary lter (Vihonen et al., 2014). (Honkakorpi et al., 2013) demonstrated that feedback
using IMU-based estimates yields similar performance to using encoder measurements. Re-
cently, (Xinjilefu et al., 2016) fused predicted joint accelerations, angular velocities from
link-mounted gyroscopes and joint states measured by encoders. An orientation calibra-
tion routine similar to that presented here is detailed, however joint accelerations are not
considered (neither is a joint position calibration routine or gyroscope bias estimation). It
was shown that using estimated joint velocities, task-level gains (on the COM) could be
increased; this is in agreement with the results presented in this chapter, in which we show
that joint feedback damping is considerably improved.
Unlike most previous work (with the exception of (Xinjilefu et al., 2016) which was
developed at the same time), the theory presented here applies to
oating base robots with
three degree of freedom joints, does not rely on global link orientations, introduces lters for
gyroscope bias compensation and considers the use of accelerometers and their role in joint
state estimation. Additionally, we present results on feedback control of individual joints
78
which demonstrate the ability to increase both stiness and damping when using gyroscope-
based joint velocities. We begin by considering the theory necessary for computing joint
velocities and accelerations from IMU sensor measurements.
V.1 Sensor Framework
Assume we have a multibody system composed of N links L
1
;:::;L
N
in series, the rst
of which is a
oating base. These are connected by N joints J
0
;:::;J
N1
where J
0
is
the
oating base orientation in a minimal set of coordinates. We x an IMU containing
a three-axis gyroscope and a three-axis accelerometer to each link at a known position in
the link frame and with the same orientation as the link frame (IMU pose calibration is
detailed in Sec. V.2.1 and V.2.2).
Let !
W
L
1
;!
W
L
2
; ;!
W
L
N
denote the angular velocities of each link in the world (global)
frame; the gyroscope on L
i
thus measures
!
L
i
=R
L
i
W
(!
W
L
i
+b
!;L
i
+w
!;L
i
) (V.1)
whereR
L
i
W
rotates a quantity in world frame into the local frame of L
i
andb
!;L
i
andw
!;L
i
denote time-varying bias and thermal noise vectors, respectively. Similarly,a
W
L
1
;a
W
L
2
; ;a
W
L
N
denote the true accelerations of the IMU locations on each link in world frame so that the
accelerometer on L
i
measures
a
L
i
=R
L
i
W
(a
W
L
i
+b
a;L
i
+w
a;L
i
+g) (V.2)
whereg = [0; 0;9:81] is the gravity vector andb
a;L
i
andw
a;L
i
again denote bias and noise
vectors, respectively. For the time being, we will not consider the eects of noise sources;
these will be addressed in Sec. V.3.1.
79
V.1.1 Joint Velocity Computation from Gyroscopes
Assuming in the most general case that every joint has three Degrees of Freedom (DoFs),
the vector !
i
i1;i
measuring the angular velocity of L
i
relative to that of L
i1
in the L
i
frame corresponds to the velocity
_

i1
2 R
3
of joint J
i1
. From this point on, we will
drop the use of L and J in subscripts for the sake of brevity. For the
oating base (L
1
)
gyroscope we thus have
!
1
=R
1
W
!
W
1
=
_

0
where
_

0
is the base angular velocity in the base link frame. For L
2
we have
!
2
=R
2
W
!
W
2
=R
2
W
(!
W
1
+!
W
1;2
)
where we have used the velocity composition rule. This simplies further to
!
2
=R
2
1
R
1
W
!
W
1
+R
2
W
!
W
1;2
=R
2
1
_

0
+
_

1
whereR
2
1
represents a rotation from frameL
1
toL
2
. Solving for the velocity ofJ
1
, we have
_

1
= !
2
R
2
1
_

0
Similarly, for L
3
we have (again using velocity and rotation composition rules)
!
3
=R
3
W
!
W
3
=R
3
W
(!
W
1
+!
W
1;2
+!
W
2;3
)
=R
3
2
R
2
1
R
1
W
!
W
1
+R
3
2
R
2
W
!
W
1;2
+R
3
W
!
W
2;3
=R
3
2
R
2
1
_

0
+R
3
2
_

1
+
_

2
80
and solving for the velocity of J
2
yields
_

2
= !
3
R
3
2
R
2
1
_

0
R
3
2
_

1
Continuing in this manner, we form the linear system
2
6
6
6
6
6
6
6
6
6
4
I 0 0 0
R
2
1
I 0
.
.
.
.
.
.
R
3
1
R
3
2
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. I 0
R
N
1
R
N
2
R
N
N1
I
3
7
7
7
7
7
7
7
7
7
5
2
6
6
6
6
6
6
6
6
6
4
_

0
_

1
.
.
.
_

N2
_

N1
3
7
7
7
7
7
7
7
7
7
5
=
2
6
6
6
6
6
6
6
6
6
4
!
1
!
2
.
.
.
!
N1
!
N
3
7
7
7
7
7
7
7
7
7
5
(V.3)
which we can write more compactly as T
_
= ! where
_
2 R
3N
and !2 R
3N
are the
full vectors of joint velocities and link gyroscope measurements, respectively. Note that
T is a function of and is composed of relative rotations between links; unlike in other
methods, there is no need for global link poses. Additionally, since T is lower-triangular
this system always has a unique solution which can be determined eciently using forward
substitution.
Computing Constrained Velocities
The above assumes that every joint has three DoFs; however, it is often the case in for a
robot that certain joints have fewer (for example, the knee of a humanoid). In this case,
more accurate solutions may result from properly constraining velocities using knowledge
of the kinematic structure.
For the L
i
gyroscope we again have (neglecting noise)
!
i
=R
i
W
!
W
i
=R
i
W
J
W
i
_ q =J
1
i
_ q
81
where _ q = [!
base
; _ q
joints
] is the vector of generalized joint velocities and J
1
i
is the Jacobian
relating the angular velocity of L
i
to generalized joint velocities in the L
i
frame. Using
this relation for each link, we form the system
2
6
6
6
6
6
6
4
J
1
1
J
2
2
.
.
.
J
N
N
3
7
7
7
7
7
7
5
_ q =
2
6
6
6
6
6
6
4
!
1
!
2
.
.
.
!
N
3
7
7
7
7
7
7
5
(V.4)
where each Jacobian above is computed using Jacobians relative to the base asJ
i
i
=R
i
1
J
1
i
.
We can write this system asT
J
_ q = w and solve a least-squares problem for joint velocities.
While forming the matrix T
J
requires knowledge of the kinematic structure, this method
results in velocities which are properly constrained and expressed in the correct frames.
We will demonstrate the eect of properly constraining velocities in Sec. V.4.
V.1.2 Joint Acceleration Computation from Accelerometers
We now use the setup detailed

in Sec. V.1 to determine the acceleration

i1
of J
i1
.
The IMU accelerometer on the preceding link measures
a
i1
=R
i1
W
(a
W
i1
+g)
and the accelerometer on the link immediately following J
i1
measures
a
i
=R
i
W
(a
W
i
+g) =R
i
W
(a
W
i1
+a
W
i1;i
+g)
where a
W
i1;i
is the acceleration of the L
i
IMU with respect to the L
i1
IMU and is given
by
a
W
i1;i
= (!
W
i1;i
(!
W
i1;i
r
W
i1;i
)) + (
W
i1;i
r
W
i1;i
)

Note that we could again constrain the acceleration using the Jacobian since =J

+
_
J but we wish
to avoid using the Jacobian derivative, which must be computed numerically and is thus typically very
noisy.
82
wherer
W
i1;i
,!
W
i1;i
and
W
i1;i
are the position, angular velocity and angular acceleration of
theL
i
IMU with respect to the L
i1
IMU in world frame. Using the notation ab =a

b
where a

2R
33
, it follows that
a
i
=R
i
i1

R
i1
W
(a
W
i1
+g)

+R
i
W
a
W
i1;i
=R
i
i1
a
i1
+R
i
W

((!
W
i1;i
)

)
2
r
W
i1;i
+ (
W
i1;i
)

r
W
i1;i

=R
i
i1
a
i1
+ ((!
i
i1;i
)

)
2
r
i
i1;i
(r
i
i1;i
)

i
i1;i
=R
i
i1
a
i1
+ ((
_

i1
)

)
2
r
i
i1;i
(r
i
i1;i
)

i1
where we have used the denition of a
i1
, properties of the cross product and the denitions
_

i1
=!
i
i1;i
and

i1
=
i
i1;i
. We rearrange the above equation to get
(r
i
i1;i
)

i1
=R
i
i1
a
i1
a
i
+ ((
_

i1
)

)
2
8i = 2;:::;N
Note that gravity does not appear in the above, making gravity compensation unnec-
essary. Also, the joint velocities
_

i
are determined from the link gyroscopes as in Sec.
V.1.1. However, we cannot solve this equation for

i1
because the skew-symmetric matrix
(r
i
i1;i
)

2R
33
has rank two.
y
We thus add a second IMU to L
i
and solve
2
4
(r
i
i1;i
)

(~ r
i
i1;i
)

3
5

i1
=
2
4
R
i
i1
a
i1
a
i
+ ((
_

i1
)

)
2
R
i
i1
a
i1

~
a
i
+ ((
_

i1
)

)
2
3
5
(V.5)
where ~ r
i
i1;i
and
~
a
i
denote the relative position and the acceleration of the additional IMU.
The matrix multiplying

i1
now has rank three aside from singular cases (for example
when ~ r
i
i1;i
= r
i
i1;i
, implying that the IMU positions should be as distinct as possible).
Note that rather than use two three-axis accelerometers, we could have equivalently used
three two-axis accelerometers to obtain a full-rank matrix; these results are in agreement
with Lin et al. (2012).
y
Real skew-symmetric matrices have purely imaginary eigenvalues, which must come in conjugate pairs;
thus, the rank must be even.
83
V.2 IMU Pose Calibration
To obtain accurate estimates of joint velocities and accelerations, the IMUs must be xed
with known poses. As is often the case for humanoids, the base IMU is rigidly xed with a
known pose; we compute orientation and position osets for each link relative to this IMU
using the following principle. When rotated in the air with locked joints, the entire robot
becomes a single rigid body subject to the same angular velocity and angular acceleration.
This is the basis for the following oine calibration methods which recover the full pose
(orientation and position) of each IMU with respect to its local link frame.
V.2.1 Orientation Calibration
When every link has the same angular velocity, we can equate the velocities of L
i
and that
of the base to obtain
^
R
i
!
i
=R
i
1
!
1
where
^
R
i
is the desired rotational correction for the IMU on L
i
. Transposing both sides
and stacking M consecutive measurements, we obtain
2
6
6
6
6
6
6
4
f !
T
i
g
1
f !
T
i
g
2
.
.
.
f !
T
i
g
M
3
7
7
7
7
7
7
5
^
R =
2
6
6
6
6
6
6
4
f !
T
1
(R
i
1
)
T
g
1
f !
T
1
(R
i
1
)
T
g
2
.
.
.
f !
T
1
(R
i
1
)
T
g
M
3
7
7
7
7
7
7
5
wherefvg
m
denotes the m
th
observation of quantity v. We seek
^
R
i
as the solution to the
problem
^
R = arg min
X
jjAXBjj
2
F
subject to X
T
X = I wherejjAjj
F
denotes the Frobenius matrix norm. This is called the
orthogonal Procrustes problem and is solved by computing the SVD ofA
T
B =UV
T
and
setting
^
R = UV
T
. In order to ensure that the solution is a proper rotation matrix we
84
also require det(
^
R) = +1. This is known as the Kabsch algorithm Kabsch (1976) and is
achieved by instead setting
^
R =U
^
V
T
where
^
= diag(1; 1; sign(det(UV
T
))).
V.2.2 Position Calibration
Assuming every link has the same world frame angular acceleration
W
, the linear accel-
eration of the IMU on L
i
relative to that of the base link IMU is
a
W
i
a
W
1
=!
W
!
W
r
W
1;i
+
W
r
W
1;i
However, we know that a
i
=R
i
W
(a
W
i
+g) and thus a
W
i
=R
W
i
a
i
g so
a
W
i
a
W
1
=R
W
i
a
i
gR
W
1
a
1
+g =R
W
1
(R
1
i
a
i
a
1
)
Note that the gravity vector again cancels, making compensation unnecessary. Multiplying
both sides by R
1
W
, we have
R
1
i
a
i
a
1
=!
1
!
1
r
1
1;i
+
1
r
1
1;i
which we can rewrite in the form

( !

)
2
+

r
1
1;i
=

R
1
i
a
i
a
1

where ! and denote the angular velocity and acceleration measured by the base link
IMU. Again, stacking M consecutive measurements results in the linear system
2
6
6
6
6
6
6
4
f( !

)
2
+

g
1
f( !

)
2
+

g
2
.
.
.
f( !

)
2
+

g
M
3
7
7
7
7
7
7
5
r
1
1;i
=
2
6
6
6
6
6
6
4
fR
1
i
a
i
a
1
g
1
fR
1
i
a
i
a
1
g
2
.
.
.
fR
1
i
a
i
a
1
g
M
3
7
7
7
7
7
7
5
85
Note that we compute numerically from base gyroscope measurements. Since this is
an oine calibration routine, we apply a zero-delay lter to the measurements. Since the
matrix multiplying IMU position is the same for every link, we compute its SVD once and
nd the least-squares solution for each IMU. After solving for the position r
1
1;i
of the i
th
IMU in the base frame, we compute its local link position from kinematics.
V.3 Joint State Estimators
In this section, we introduce two Kalman Filters for the joint state which fuse joint velocities
and accelerations computed from inertial sensors with joint position sensor measurements.
This is advantageous over directly using the computed joint state because it ensures con-
sistency between the joint position and its derivatives. In theory, performing ltering using
an accurate process model also creates less delay than simply low-pass ltering computed
quantities.
V.3.1 Joint Position and Gyroscope Bias Filter
Assume now that each gyroscope is aicted by a time-varying bias and thermal noise as
in the model given by Eq. (V.1). From Eq. (V.4) we thus have
T
J
()
_
= !bw
where b;w2R
3N
are the bias and noise vectors for all gyros, respectively. Combining the
joint positions and gyroscope biases into the state vector x = [
T
;b
T
]
T
2 6N, we can write
the state dynamics as
_ x =f(x; !;w;w
b
)
or more specically as
_
=T
J
()
1
b +T
J
()
1
( !w)
_
b =w
b
86
where we have modeled the bias dynamics as Brownian motion (the integral of white noise
process w
b
). Since we have angular position sensors, we can write a measurement of the
state as
y =
h
I 0
i
2
4

b
3
5
+v
wherev2R
3N
is the measurement noise aicting the joint position sensor measurements.
Since the process model is nonlinear, we choose an EKF for implementation.
V.3.2 Acceleration-Based Joint Velocity Filter
In order to lter joint velocities, we need joint accelerations

for use in the process model.
Dening the state vector x = [
T
;
_

T
]
T
= [x
T
1
;x
T
2
]
T
2 6N we have the trivial dynamics
_ x
1
=x
2
_ x
2
=

The joint acceleration vector

can be specied in any the following ways, depending on
sensor availability:

=
8
>
>
>
>
<
>
>
>
>
:
M()
1
[ +J()
T
c

Fn(;
_
)]
f(;
_
; a)

des
The rst method uses measured joint torques and endeector wrenches

F along with the
robot's dynamic model to compute the current joint accelerations. The second method
solves for accelerations using link accelerometers with Eq. (V.5). Alternatively, we can
simply use the desired or predicted joint accelerations

des
from the current controller.
Note that use of the rst two methods results in a nonlinear process model; in this work,
we consider only using desired accelerations for simplicity.
87
We measure the joint positions from angular sensors and the joint velocities from the
link IMUs as in Sec. V.1.1, leading to the measurement model y =x +v where v2R
6N
is again the measurement noise vector.
V.4 Experiments and Results
The platform used for experiments is the lower body of a Sarcos hydraulic humanoid robot
having a total of 14 DoFs (seven per leg). An IMU containing a three-axis gyroscope
and three-axis accelerometer was mounted to each link - one on the base and one on the
link immediately following the hip, knee and ankle joints as in Figure V.2. We chose
Microstrain 3DM-GX3-25 IMUs for their USB interface, low-noise sensors and maximum
streaming rate of 1 kHz. Initial gyroscope biases were removed at start-up. The IMUs
were xed using an adhesive; while an eort was made to align the IMU axes with the link
frames, using the calibration methods of Sec. V.2.1 and V.2.2 allowed for imprecise sensor
placement.
We rst evaluated the joint velocity computation detailed in Sec. V.1.1 during a sine
tracking task for every joint in the right leg. Joint velocities were computed from measured
gyroscope data in three ways: A) using Eq. (V.3) to compute orientation-corrected but
unconstrained velocities, B) using Eq. (V.4) to compute constrained but non-calibrated
velocities and C) again using Eq. (V.4) to compute velocities both orientation-corrected
and constrained to the kinematics. These were compared against velocities computed from
potentiometer measurements and ltered using a second-order Butterworth lter with a
cuto of 25Hz. Fig. V.3 shows the eect of constraining computed joint velocities; the con-
strained hip velocities more closely match the ltered potentiometer-based velocities. Fig.
V.4 shows the eect of performing the oine rotation calibration routine on constrained
ankle velocities; the benet of calibration is apparent.
We next evaluate the IMU position calibration and accelerometer-based joint accelera-
tion computations detailed in Sec. V.1.2. Fig. V.5 compares the potentiometer-based hip
accelerations (heavily ltered at a cuto of 5Hz) with those computed from IMUs using
88
Figure V.3: Comparison between ltered
potentiometer-based hip velocities and
constrained versus unconstrained hip ve-
locities computed from link gyroscopes.
Figure V.4: Comparison between l-
tered potentiometer-based ankle veloc-
ities and rotation-corrected versus un-
corrected ankle velocities computed from
link gyroscopes.
Eq. (V.5) during a sine task for the hip. We compare the results of computing accelera-
tions using manually-measured IMU positions and automatically-generated positions from
the calibration routine of Sec. V.2.2. Both signals estimate the acceleration well and with
much less noise than unltered, numerically-computed joint accelerations (shown for refer-
ence in Fig. V.6). The ltered joint-based accelerations are clearly heavily delayed; this is
most evident in the z direction. Because we only have one IMU per link, we compute hip
joint accelerations using the base, thigh and shank IMUs with the knee locked. This is not
ideal since these IMUs are not truly on the same link; we expect to compute accelerations
which are more accurate and less noisy by adding a second IMU to the proper link.
It should be noted that obtaining a good position calibration requires sucient angular
motion of the robot, which can be dicult depending on the setup. Additionally, the
position calibration and angular acceleration computations rely on accurate kinematics and
a good IMU orientation calibration, else the gravity terms will not cancel; these appear in
89
Figure V.5: Filtered potentiometer-based hip joint accelerations versus those computed
from inertial sensors with both manually and automatically-generated IMU position infor-
mation.
Eq. (V.5) to create conguration-dependent acceleration biases. Unmodeled accelerometer
biases will have the same eect and should be compensated through calibration, however
were neglected here.
Figure V.6: Raw potentiometer-based hip joint acceleration (blue) versus that computed
from inertial sensors (red).
In order to test the gyroscope bias estimator presented in Sec. V.3.1, we performed
experiments in the SL simulation environment by simulating gyroscopes subject to the
90
noise sources in Eq. (V.1). This was done because the IMUs on our robot have a low bias
instability; however, this is not the case for inexpensive IMUs which are cost-eective to
use in sensorizing every link. We simulate aggressive gyroscope biases which are initially
nonzero and drift orders of magnitude faster than those in our IMUs. Fig. V.7 shows the
results for one of the simulated IMUs. Despite being run during a full-body sine tracking
task with simulated joint sensor noise, the lter manages to track the true biases.
Figure V.7: Simulated gyroscope bias estimation using the joint state lter of Sec. V.3.1.
We also test the joint velocity estimator of Sec. V.3.2 which was implemented for
all 14 joint DoFs. Fig. V.8 compares the joint velocity of one hip DoF for the ltered
potentiometer-based velocity, the constrained IMU-based velocity from Eq. (V.4), and
velocities from the estimator of Sec. V.3.2 both without and with desired joint accelerations
as process model inputs. Both ltered velocities are smoother than the IMU-based velocity,
however the estimate from the lter which uses desired acceleration in its process model
(in black) has tens of milliseconds less delay than the estimate from the lter having a
naive process model (in green below). The desired acceleration-based estimator provides a
ltered signal with only a slight delay compared to the IMU-computed velocity. Given the
91
apparent dierence between the joint accelerations shown in Fig. V.5 and the sinusoids
used to generate them, we expect velocity lter performance to improve considerably by
using either sensor-based joint accelerations or accelerations computed using the dynamic
model. We leave this investigation to future work.
Figure V.8: Hip joint velocity computed from ltered joint sensors, directly from gyroscope
velocities, ltered using the estimator of Sec. V.3.2 without and with desired accelerations.
Finally, we perform sine tracking tasks for a single joint in isolation (here the right knee)
in order to determine whether using the IMU-based joint velocities allows us to increase
controller gains and enable better tracking. A joint PD controller was implemented for the
knee using potentiometer-based joint position and velocity (ltered at a 25Hz cuto) with
the ability to switch to using the raw joint position and gyroscope-based velocity.
For a 0:5Hz sine wave of amplitude 0:25rad, we were able to increase the position gain
to 1000 before the controller using the potentiometer-based velocity went unstable while we
could increase the gain to 1600 before the controller using the velocity computed from Eq.
(V.4) showed signs of instability. Fig. V.9 shows the tracking using a position gain of 1000
and a velocity gain of 12 for both potentiometer and gyroscope-based joint velocities. RMS
tracking error decreases from 0:0103rad to 0:0099rad in position and from 0:3786rad=s to
0:0902rad=s in velocity by switching to gyroscope-based velocities. Fig. V.10 shows the
92
tracking using a position gain of 1500 and a velocity gain of 12 for the gyroscope-based
velocities, demonstrating stable position tracking with an RMS error of 0:0077rad.
Figure V.9: Knee sine tracking for the
gains P = 1000 and D = 12, switched
from potentiometer-based velocities to
IMU-based velocities at t = 10s.
Figure V.10: Knee sine tracking for the
gainsP = 1500 andD = 12 for the IMU-
based velocities only.
We were also able to independently increase the knee velocity gain from a maximum
stable value of 26 using the potentiometer-based velocities to 30 using the gyroscope-based
velocities. Fig. V.11 shows the tracking forD = 26 using each of these velocities. Position
tracking is poor using both since the position gain was held at 250, however it is clear from
the gure that damping is improved after the switch.
We nally demonstrate that the above results hold for faster sine tracking by per-
forming a 3Hz tracking task using position gains close to the stable limit for each of the
potentiometer and gyroscope-based knee velocities. It is evident from Fig. V.12 that both
position and velocity tracking are improved after switching to the IMU-based velocities
due to the ability to use higher gains.
93
Figure V.11: Knee sine tracking for P =
250 and D = 26, switched to IMU-based
velocities at t = 10s.
Figure V.12: Knee tracking a 3Hz sine,
switched from potentiometer-based ve-
locities with P = 800 and D = 12 to
IMU-based velocities with P = 1500 at
t = 10s.
V.5 Summary
In this chapter, we have:
Presented methods for computing joint velocities and accelerations directly from
inertial sensor measurements.
Introduced oine calibration procedures which allow for recovery of the pose of the
IMUs attached to each link of the robot.
Proposed lters which fuse measured joint positions with IMU sensor data, allowing
for estimation of gyroscopes biases and improvements in the quality of the joint
position and velocity estimation.
Conducted robot experiments using velocity and acceleration estimates, allowing to
signicantly increase the stiness and damping of our joint feedback controllers
94
Now that we have characterized the performance of joint estimation-based feedback
control, future work will entail use of these estimators in whole-body control approaches
such as the one developed in Section II.3 and in an LQR control framework (Mason et al.,
2016) in order to achieve higher damping of both joint and Cartesian quantities (such as
momentum, potentially in combination with the approach of Chapter IV). Additionally, we
would like to address the hardware issue of potentiometer oset calibration by extending
previous work on the topic (Yamane, 2011) to utilize the link IMU sensors.
95
Chapter VI
Contact State Estimation
VI.1 Introduction
Control and estimation approaches for legged robots rely on assumptions about the contact
state of the feet. Floating-base inverse dynamics resolves underactuation by projecting
the dynamics into the contact constraints, forcing the endeector acceleration to be zero
Mistry et al. (2010). Locomotion on rough terrain focuses on stabilization through footstep
adaptation but ignores the diculties presented by contact constraint violations Barasuol
et al. (2013). Similarly, legged odometry for base state estimation assumes the pose of an
endeector in contact is constant Bloesch et al. (2012). Methods have been introduced
to robustify kinematics-based base state estimation, including computing a contact point
with minimal instantaneous velocity Masuya and Sugihara (2013) and outlier detection to
discard measurements during slip Bloesch et al. (2013), however few consider relaxing the
contact assumptions by estimating contact quality in parallel.
Contact estimation is a broad topic which has been investigated in various contexts.
Petrovskaya et al.Petrovskaya et al. (2007) were among the rst to consider multi-contact
force control scenarios in which a manipulator interacts with the environment at points
other than its endeector. Del Prete et al. investigated the eect of contact point esti-
mation error on force control for a humanoid with a capacitive skin Prete et al. (2012).
Similar work was done by Manuelli et al. Manuelli and Tedrake (2016) to estimate contact
points without a tactile skin by fusing proprioceptive sensing with the dynamic model.
96
While estimation of contact points has been thoroughly studied, the problem of de-
termining the quality of contacts is less well-dened. One aspect of contact quality is the
determination of contact constraint directions for a given task. Ortenzi et al. Ortenzi
et al. (2016) computed endeector constraints for a manipulator in contact with a surface
using only kinematics, and Nozawa et al. Nozawa et al. (2017) recently presented a similar
method for estimating environment constraints in humanoid control tasks.
For humanoids, the quality of a contact is largely determined by friction. Hoep
inger
et al. Hoep
inger et al. (2013) investigated foothold quality using unsupervised learning.
Terrain elevation map samples were clustered to nd a set of primitives which were evalu-
ated for foothold robustness by computing the friction coecient through exploratory force
control. This allowed for prediction of contact quality from visual features for planning.
While Focchi et al. Focchi et al. (2017) employed oine friction estimation through for a
quadruped walking on steep slopes, Ridgewell et al. Ridgewell (2017) introduced methods
for online friction estimation and control adaptation. While the friction coecient deter-
mines linear slip, the center of pressure (CoP) boundaries determine rotational slip/roll.
Most controllers assume that the support polygon is the same shape as the foot, however
this is invalid on rough terrain where line and even point contacts are encountered. Wiede-
bach et al. Wiedebach et al. (2016) presented one of the only approaches for online CoP
boundary estimation during terrain exploration.
In contrast to approaches which indirectly compute contact quality by computing fric-
tion and CoP bounds, we wish to avoid contact models by directly estimating the prob-
ability of an endeector being constrained in each of its six DoFs independently. In this
direction, Hwangbo et al. Hwangbo et al. (2016) developed a Hidden Markov Model which
uses kinematic and dynamic models to predict contact transitions without force sensing.
This one-dimensional approach requires little sensing, however it does not estimate contact
quality of the contact nor does it evaluate the classier in an estimator.
Camurri et al. Camurri et al. (2017) recently developed a method for contact probabil-
ity estimation using logistic regression. This one-dimensional classier learns the normal
force threshold at which the contact state transitions, ignoring lateral forces under the
97
assumption that sucient friction exists to prevent slip. The resulting probability per en-
deector is used to weight the corresponding measurement in a base state estimator, and
a heuristic for modulating the measurement variance to lter out the eect of impacts is
introduced. Although this estimator performs better than one using a xed threshold, the
classier requires signicant eort to train - ground truth is obtained manually as the con-
tact sequence which minimizes estimation error. Further, all results shown are for walking
on
at ground where slipping does not occur. Finally, only one dimension (normal force)
is considered.
In contrast, we develop a contact estimator which:
is completely unsupervised and model-free
uses only common, proprioceptive sensors (endeector force/torque and IMU)
estimates the probability of contact in all six endeector DoFs independently
We test this contact estimator for use in base state estimation by modulating the
measurement uncertainty associated with each endeector DoF using the corresponding
estimated probability of contact. The following section details the motivation and setup
for this approach.
VI.2 Background
VI.2.1 Motivation
A dicult question arises when designing an estimator for contact state: what does it
mean for an endeector to be in contact? Most approaches treat the endeector as xed
to contact surface if the normal force exceeds a chosen threshold, however contact truly
occurs when the assumed endeector constraints are satised - these statements are not
always the same. The six DoF endeector constraints are equivalent to enforcing that the
feet cannot slip or rotate. An endeector will not slip if the static friction constraint
q
F
2
x
+F
2
y

x;y
F
z
(VI.1)
98
is satised, whereF is the contact force and
x;y
is the translational coecient of friction.
Likewise, the endeector will not rotate if the CoP and rotational friction constraints
2
4

y
=F
z

x
=F
z
3
5

2
4
CoP
x
CoP
y
3
5
(VI.2)
j
z
j
z
F
z
(VI.3)
are satised, where is the contact torque,
z
is the rotational coecient of friction and
CoP
x
, CoP
y
denote the contact support polygon bounds which are functions of contact
surface geometry.
Since a suciently-high normal force F
z
would guarantee that inequalities (VI.1-VI.3)
are satised regardless of the other contact wrench dimensions, most estimation approaches
simply threshold F
z
Bloesch et al. (2012), Fallon et al. (2014), Faraji et al. (2015). How-
ever, this is restrictive especially on rough terrain where low friction and dicult surface
geometry make slip and rotation likely even at high normal force values. It also results in a
one-dimensional contact state estimate as in Hwangbo et al. (2016), Camurri et al. (2017),
whereas the contact constraint is truly six-dimensional.
VI.2.2 Sensing for Clustering
As discussed in the previous section, contact constraints are invalid when an endeector
slips and/or rotates, which is caused by a violation of friction and/or CoP constraints;
these constraints depend on the contact wrench and surface properties (friction coecients
and geometry). Rather than estimate these properties, we seek to cluster measured contact
wrench data to directly learn constraint probabilities.
99
All experiments in this work are performed in the SL simulation environment Schaal
(2007); we add simulated random-walk biases b
F
and b

, along with simulated Gaussian
noise processes w
F
and w

, to the true force F and torque measurements:
F =

F +b
F
+w
F
(VI.4)
= +b

+w

(VI.5)
As low-cost IMUs become available, humanoids are being augmented with additional sens-
ing to improve estimation Rotella et al. (2016), Xinjilefu et al. (2016); in order to give
structure to the clustering problem, we add a simulated IMU to each endeector. We
model the sensor outputs subject to simulated random-walk biases and thermal noise pro-
cesses Woodman (2007) as
a
IMU
=R
IMU
W
(a
W
+g) +b
a
+w
a
(VI.6)
!
IMU
=R
IMU
W
!
W
+b
!
+w
!
(VI.7)
where a2 R
3
and !2 R
3
are the linear acceleration and angular velocity, respectively.
R
IMU
W
2 SO(3) is the rotation from world to IMU frame and g is gravity. Sensors are
assumed to be aligned with the endeector frame, however their positions relative to this
frame origin are not required.
VI.3 Clustering Setup
Because we seek a continuous measure of contact quality rather than a classier, we em-
ploy Fuzzy C-means (FCM) clustering which results in the soft partitioning of a dataset
by allowing each data point to belong to more than one cluster Dunn (1974). This is
accomplished by minimizing the cost
Np
X
i=1
Nc
X
j=1
w
m
i;j
jjx
i
c
j
jj
2
; m> 1 (VI.8)
100
Table VI.1: Simulated sensor noise standard deviations. Corresponding values for 1kHz
sampling rate are shown.
Continuous Discrete (1kHz)

0:00000316rad=
p
Hz 0:0001rad

F
0:06325N=
p
Hz 2N

b
F
0:0001N=s=
p
Hz 0:00316N=s

0:00316Nm=
p
Hz 0:1Nm

b
0:0001Nm=s=
p
Hz 0:00316Nm=s

a
0:00078m=s
2
=
p
Hz 0:02467m=s
2

ba
0:0001m=s
3
=
p
Hz 0:00316m=s
3

!
0:000523rad=s=
p
Hz 0:01653rad=s

b!
0:000618rad=s
2
=
p
Hz 0:01954rad=s
2
where N
p
is the number of data points x
i
, N
c
is the chosen number of clusters, w
i;j
is
the membership weight of point i belonging to cluster j andm is a constant which can be
used to tune the amount of cluster overlap.
This cost is minimized in a manner similar to k-means clustering; rst, initial member-
ship weights are randomly assigned. Then, cluster means are computed as
c
j
=
P
Np
i=1
w
m
i;j
x
i
P
Np
i=1
w
m
i;j
(VI.9)
after which new membership weights are computed with
w
i;j
=
1
P
Nc
k=1

jjx
i
c
j
jj
jjx
i
c
k
jj
2
m1
(VI.10)
Eq. (VI.9-VI.10) are iterated until the membership weights converge. Since
P
Nc
j=1
w
i;j
=
1, we treat w
i;j
as the probability of point i belonging to cluster j.
101
We use an FCM implementation from the Python library Scikit-learn Pedregosa et al.
(2011) with N
C
= 2 clusters (corresponding to contact and no contact states) and default
stopping parameters. The \fuzziness" constant is set tom = 1:2 which is the default value
in most libraries. Increasing this factor can amplify the eect of slip on contact probability,
however it also reduces the probability of contact when no slip occurs.
Each data pointx
k
2R
7T
is a time series of the pastT = 20 samples (at our control rate,
0:020s). We include a short time-history to improve estimation response time; optimization
of this time window is left to future work.
Clustering is performed independently for the six DoFfx;y;z;;;
g of each endef-
fector, using the full contact wrench and the corresponding IMU dimension from
fa
IMU
x
;a
IMU
y
;a
IMU
z
;!
IMU
x
;!
IMU
y
;!
IMU
z
g
The constraint in the local endeector frame y direction uses, for example, data points of
the form
x
k
=ffF
x
kT
; ;F
x
k
g;fF
y
kT
; ;F
y
k
g;fF
z
kT
; ;F
z
k
g; (VI.11)
f
x
kT
; ;
x
k
g;f
y
kT
; ;
y
k
g;f
z
kT
; ;
z
k
g;
fa
IMU
y
kT
; ;a
IMU
y
k
gg
Data from sensors with noise added as in Sec. VI.2.2 is collected and used unltered for
clustering. Preprocessing entails dimension-wise normalization of all x
k
(to ensure that
the scale of dimensions such as F
z
do not dominate) followed by taking the absolute value
(since slip is bi-directional).
VI.4 Base State Estimation
In order to evaluate the utility of the proposed contact estimator, we incorporate it into
a base state estimation framework which relies on stationary contact assumptions. In
previous work Rotella et al. (2014) we have implemented a kinematics-based estimator
102
which fuses IMU data and relative base pose measurements to estimate the
oating base
state of a humanoid (see Chapter III). The estimator measurements take the form
s
p;i
=R(q)(p
i
r) +n
p
(VI.12)
s
z;i
= exp(n
z
)
q
z
1
i
(VI.13)
where R(q) denotes the rotation matrix corresponding to the estimated base quaternion
q, p
i
and z
i
are the estimated foot i position and quaternion respectively, and n
p
and
n
z
are position and orientation measurement noise vectors (see Rotella et al. (2014) for
more details). In most approaches for legged robots, the variances of n
p
and n
z
are set
to constant, tuned values and the measurements are dropped from the lter when the
endeector loses contact, determined based on a xed normal force threshold.
In contrast, in this work we set the contact state (which determines active measure-
ments) and measurement noise variance using the output of the probability estimator.
When the probability of contact vector P
contact
2R
6
exceeds P
i
= 0:5 in every dimension
i, we consider the endeector in contact and use the corresponding measurements. Further,
we set the measurement noise covariance matrix as
=E[nn
T
] =r
2
I +(I diag(P
contact
)) (VI.14)
wherer is the nominal measurement noise standard deviation (sometimes tuned separately
for position and orientation) and is a scaling factor for the probability-dependent term
(we choose = 1 for simplicity). The covariance thus converges to its constant value as
in Rotella et al. (2014) when P
contact
! 1. This is conceptually similar to the approach of
Camurri et al. (2017) but requires less tuning and considers all six contact dimensions.
Since clustering is performed in the endeector frame, the covariance must be trans-
formed into the base frame where the base state estimator measurement is expressed. This
is accomplished with
^
=RR
T
; R = blockdiag(R
Base
Endeff
;R
Base
Endeff
)2R
66
103
where R
Base
Endeff
2R
33
is the rotation from endeector to base frame (a function of kine-
matics and joint angles only).
VI.5 Experiments and Results
We perform a number of experiments to evaluate the performance of the proposed estimator
and analyze its properties. All experiments are performed in the SL simulator Schaal (2007)
during a 60 second rough terrain walking task with simulated joint angle, IMU and contact
wrench sensor noise as in Table VI.1; noisy data is used for clustering, contact estimation
and base state estimation. Control is computed using non-noisy sensor data and ideal base
state estimation, however we investigate using the proposed contact estimator for closed-
loop control in Sec. VI.6. Walking velocity commands were recorded from user input
and played back, producing repeatable trajectories across experiments. The rough terrain
consists of raised patches with a friction coecient of 0:4 (half the normal friction in SL).
To account for the eect of noise and slight contact dierences, Root-Mean-Squared Error
(RMSE) for experiments in this section was computed by averaging performance across
ten trials.
VI.5.1 Contact Clustering Results
Sensor data was recorded from the rough terrain walking task and clustered oine as
detailed in Sec. VI.3 (clustering takes on the order of a few seconds). The cluster means
were then used to compute contact probability during a similar walking task; the results
are shown in Fig. VI.1.
We focus on one contact cycle in the lower portion of Fig. VI.1 to investigate the
clustering results more closely. Slip rst occurs in the y direction at (1), causing the
corresponding contact probability to lag behind the other dimensions. Rotational slip in
is also present during loading, however on a smaller scale. Slip then occurs inx because the
foot is not suciently loaded while the robot tries to create force inx to decelerate the
center of mass; onceF
z
increases, there is sucient friction to stop slipping and a negative
104
0.0
0.5
1.0
x
y
z
50
0
50
26 27 28 29 30 31
0
500
Contact Probability IMU Accel, Angvel
(m/s
2
, rad/s)
Force, T orque
(N, Nm)
Time (s)
0.0
0.5
1.0
x
y
z
50
0
50
30.0 30.1 30.2 30.3 30.4 30.5 30.6
0
500
Contact Probability IMU Accel, Angvel
(m/s
2
, rad/s)
Force, T orque
(N, Nm)
Time (s)
1 2 3 4
Figure VI.1: The top portion shows the six-dimensional contact probability resulting from
a rough terrain walking task (top) along with the measured IMU linear acceleration and
angular velocity (middle) and measured contact force and torque (bottom). The portions
in gray denote contact according to the probability estimator (all P
i
> 0:5). The lower
portion of the plot shows a zoomed view of one contact cycle with several distinctive contact
events highlighted for discussion in Sec. VI.5.1.
105
F
x
is sustained from (2) on. A drop in F
z
during single support at (3) again causes slip,
leading to a decrease in contact probability in all dimensions. Finally, slip inx again occurs
at (4) as the foot is being unloaded. These are only a few highlights of the complex contact
interaction shown, however they aid in understanding where/why slip can occur.
VI.5.2 Base State Estimation Threshold
Since we evaluate the proposed contact probability estimator against a typical humanoid
base state estimator with a xed normal force threshold for contact Rotella et al. (2014),
we rst perform experiments to optimize the chosen threshold. Performance is evaluated
by computing the RMSE for the base position and yaw angle as these four states are
always unobservable without adding exteroceptive sensing. The normal force thresholds
f10N; 40N; 100N; 200N; 400Ng were tested, with performance averaged across ten trials
each; the results are shown in Fig. VI.2. The RMSE mostly decreased for increasing
thresholds, with 200N resulting in the best performance. A threshold of 400N removes
the double support period from estimation entirely, resulting in more error. We use a xed
threshold of 200N for the baseline estimator in experiments in the remainder of this work.
VI.5.3 Clustering-Based State Estimation
We evaluate the base state estimator detailed in our previous work using both a xed
normal force threshold (as is commonly done) and using the proposed clustering-based
contact probability estimator for the same rough terrain walking task. The base state
estimators are identical other than the measurement noise covariance matrix modulation
of Eq. (VI.14). As shown in Fig. VI.3, using the contact clustering for base state estimation
considerably reduces the RMSE.
VI.5.4 Clustering Training Data
In order to test how well the clustering-based estimator generalizes to dierent types of
terrain, we performed clustering using data from two dierent tasks: one which walks over
rough terrain (as in all other experiments) and one which walks in place on
at ground.
106
0.00
0.05
0.10
0.15
Position RMSE (m)
0.15
0.16
0.12
0.10
0.12
0.15
0.17
0.14
0.07
0.10
0.12
0.10
0.08 0.08
0.05
x
y
z
F
z
= 10N F
z
= 40N F
z
= 100N F
z
= 200N F
z
= 400N
Normal Force Threshold
0
10
20
Yaw RMSE (deg)
24.02
27.34
23.18
10.28
17.74
Figure VI.2: Root Mean Squared Error (RMSE) for estimation of the unobservable base
position (top) and yaw (bottom) for dierent normal force thresholds.
We then tested both contact estimators with separate base state estimators on the same
rough terrain walking task; the resulting estimation errors are shown in Fig. VI.4.
Surprisingly, the estimator trained on
at ground walking data performs roughly equally-
well, despite having been trained on a much dierent dataset than was used for testing.
This is a desirable characteristic because obtaining data from rough terrain walking on a
real robot is dicult, especially without accurate state estimation already in place.
We also wish to test how well the clustering-based estimator generalizes to dierent
gaits. We perform clustering using data from
at ground walking in varying directions
using three dierent gaits. The default gait used for walking in this work has a single
support period of 0:5s and a double support period of 0:05s; we denote this the fast gait.
We also perform clustering on slow gait data (single support period of 1:0s, double support
period of 0:5s). Finally, we cluster using data from a mixed gait which varies throughout
the task between fast and slow. We then test the clustering-based estimators for these gaits
107
0.00
0.05
0.10
Position RMSE (m)
0.041
0.122
0.053
0.104
0.034
0.075
x
y
z
Contact Clustering Fixed-Threshold
T
0
5
10
15
Yaw RMSE (deg)
6.657
15.729
Figure VI.3: Root Mean Squared Error (RMSE) for estimation of the unobservable base
position (top) and yaw (bottom) for the contact probability-based base state estimator and
the xed normal force threshold base state estimator.
for a mixed gait walk-in-place task on a patch of rough terrain (varying the gait during
a normal walking task over rough terrain is too unstable). The results are shown in Fig.
VI.5.
The main conclusion which can be drawn from this study is that the best performance is
obtained using the clustering trained on the mixed gait, as expected. However, the slow gait
clustering generalizes much better than the fast gait clustering. The xed-threshold base
state estimator (denoted BSE) also performs quite well for this task, however because this
was a walk-in-place there was mainly foot rotation and minimal slip; as seen from other
tests, the clustering-based base state estimator performs much better when slip occurs.
Further investigation into the eect of training data gait is left to future work.
108
0.00
0.02
0.04
Position RMSE (m)
0.033
0.030
0.046
0.035
0.051 0.052
x
y
z
Flat Rough
0
2
4
Yaw RMSE (deg)
3.716
5.066
Figure VI.4: Root Mean Squared Error (RMSE) for estimation of the unobservable base
position (top) and yaw (bottom) for dierent training datasets.
VI.5.5 IMUs for Clustering Versus Estimation
As motivated in Sec. VI.2.2, the use of endeector IMU data in addition to contact wrench
data essentially supervises the clustering problem, since the accelerometer and gyroscope
capture linear and rotational slip. We expect this sensor data to embed structure in the
resulting clusters, meaning that IMUs should not be required when running the contact
estimator afterwards. To test this, we cluster using data points as in Eq. (VI.11) but
perform clustering-based state estimation with both a) the full data points including IMUs
and b) without IMUs (dropping the last portion of Eq. (VI.11)). The resulting estimation
errors are shown in Fig. VI.6.
Although the RMSE is slightly lower in all dimensions when using the IMU data,
performance is not considerably changed when it is removed. This is a very useful property
because it means that the endeector IMUs can be removed after initially collecting data for
clustering. While some robots are designed with endeector IMUs, most are not; using this
109
0.000
0.005
0.010
0.015
0.020
Position RMSE (m)
0.005
0.020
0.006
0.019
0.012
0.016
0.010
0.012
0.015
0.014
0.014
0.015
x
y
z
Slow Gait Fast Gait Mixed Gait BSE
0
2
4
6
8
Yaw RMSE (deg)
2.259
8.932
3.563
8.091
0.015
Figure VI.5: Root Mean Squared Error (RMSE) for estimation of the unobservable base
position (top) and yaw (bottom) for walking in place on a patch of rough terrain with a
varying gait using clustering trained on three dierent gait types as well as for the xed-
threshold base state estimator (BSE).
clustering method would involve temporarily attaching IMUs as in Rotella et al. (2016),
Xinjilefu et al. (2016). This is reasonable for training, however attaching these sensors
permanently involves designing rigid mounts, routing cables and protecting them from
collisions with the environment. The ability to remove IMUs after training the estimator
is highly advantageous when working with real hardware.
110
0.00
0.01
0.02
0.03
0.04
Position RMSE (m)
0.033
0.028
0.044
0.039
0.035 0.035
x
y
z
No IMU IMU
0
2
4
Yaw RMSE (deg)
5.436
4.561
Figure VI.6: Root Mean Squared Error (RMSE) for estimation of the unobservable base
position (top) and yaw (bottom) with and without using IMU data for online contact
estimation.
VI.6 Clustering-Based Contact Estimation for Control
VI.6.1 Closed-Loop Control using Base State Estimation
While the primary focus of this work has been on the development and evaluation of a
contact probability estimator for use in base state estimation, the proposed method has
applications in humanoid control as well. The most direct application is the use of an
improved base state estimator in a walking controller such as the one used to generate
data in this work; see Chapter II for details.
The estimated base pose is crucial in such a control pipeline for computing both dynamic
model parameters and feedback control for endeector tracking, as discussed in III. The
use of our contact probability estimator in this context improves control considerably,
111
allowing the robot to walk on the rough terrain for a longer time before falling due to an
accumulation of base state estimation error (see the video accompanying the paper).
VI.6.2 Contact Probability-Based Inverse Dynamics Constraints
The quadratic program-based inverse dynamics solver used for all experiments in this
chapter, as detailed in II.3, is written as the optimization problem
min
q;
jj x x
des
jj
2
Wx
+jjP
null
q q
des
jj
2
Wq
+jj
des
jj
2
W

+jj
des
jj
2
W
subject to the equality constraints
M
l
(q) q +h
l
(q; _ q) =J
T
c;l

J
c
q +
_
J
c
_ q = 0
As in base state estimation, the contact state and corresponding constraint dimension
change instantaneously based on the measured normal force and a chosen threshold. This
is problematic because it results in torque discontinuities in the solution to the above
optimization which can destabilize the controller. We thus seek a method of smoothly
varying the contact constraints during walking.
Recently, Feng (2016) brie
y introduced the concept of structural change smoothing
in order to avoid discontinuities when transitioning between contact states. This was
accomplished by modifying the above optimization problem as
min
q;;
jj x x
des
jj
2
Wx
+jjP
null
q q
des
jj
2
Wq
+jj
des
jj
2
W

+jj
des
jj
2
W
+jjjj
2
W
c;
+jjJ
c
q+
_
J
c
_ qjj
2
W
c;Jc
(VI.15)
where the nal two terms represent a cost on creating contact wrenches and a cost on
violating contact constraints, respectively. This optimization problem is subject to the
modied equality constraints
M(q) q +h(q; _ q) =S
T
+J
T

112
Note that the contact equality constraints have been moved to the cost, while the dynamics
equality constraint remains however with the important distinction that the full endeector
Jacobian J is used rather than the contact Jacobian.
This eectively means that all endeectors are considered in contact at all times, while
the new cost terms actually determine the contact constraints. When an endeector loses
contact, the cost weight W
c;Jc
is ramped down from a large value to zero (allowing the
endeector to accelerate), while W
c;
is simultaneously ramped up from zero to a large
value (allowing a wrench to be created). The opposite is done when a contact is gained.
If we change the new cost weights instantaneously, we obtain the same behavior as with
hard constraints; instead, we can smoothly ramp the soft constraints on and o using, for
example, a low pass lter on the planned contact sequence.
VI.7 Summary
In this chapter, we have:
Discussed the notion of contact probability as it relates to contact constraints for
legged robots.
Introduced a contact probability estimator based on the Fuzzy C-means soft cluster-
ing algorithm using endeector contact wrench and IMU data.
Developed a principled method for incorporating contact probabilities into the base
state estimator of Chapter III by modulating the measurement noise covariance.
Evaluated the estimator in simulation using simulated endeector proprioceptive
sensing with realistic added noise, resulting in lower base state estimation RMSE
for unobservable states than using a xed normal force threshold for contact detec-
tion.
Demonstrated that the proposed estimator:
113
{ Can be run without endeector IMUs after training, making it convenient to
run online using only contact wrench data.
{ Performs equally-well using training data from either
at ground or rough terrain
walking, allowing simple training and generalization to dicult terrain.
{ Generalizes to dierent gait timings as long using a combination of fast and slow
gait training data.
{ Can be used in an improved base state estimator for closed-loop inverse dynamics
control, allowing the robot to remain stable during rough terrain walking for a
longer period of time.
Future work will include further analysis of the properties of this contact estimator
as well as a more low-level control application in which endeector constraints in inverse
dynamics are smoothly varied according to the contact probability.
114
Chapter VII
Conclusions
In this work, we have presented solutions to a number of estimation problems which are
crucial in attaining robust, general-purpose humanoid robot control. The estimation of the
base pose of a
oating base robot (Chapter III) is a necessary starting point for investigating
such challenges; this quantity cannot be observed directly through proprioceptive sensing,
yet it is required to compute the dynamics on which all whole-body controllers are based.
Closely-tied to the base pose rate of change is the momentum of the system, which is
used extensively to compute stability criteria based on both simplied and exact models of
the robot dynamics. Obtaining low-noise estimates of linear and angular momentum while
compensating for the eects of modeling error and externally-applied wrenches is essential
for balance and stepping stabilization using model predictive control (Chapter IV).
In a parallel line of research, we have introduced methods for utilizing inexpensive
inertial sensors integrated into the links of a humanoid in order to compute joint state
derivatives subject to unknown pose osets which can be compensated through calibration
(Chapter V). Joint velocity signals with less noise and delay than ltered, dierentiated
angle sensors are crucial in obtaining well-damped joint and Cartesian feedback control,
resulting in better tracking and disturbance rejection.
Finally, we have investigated contact state estimation, which presents serious challenges
to model-based control and estimation methods; both require specifying contact constraint
information to render them solvable. Rather than tune heuristics based on closed-loop
performance using these constraints, we take a learning-based approach by performing
115
clustering on proprioceptive sensor data from endeector contact wrench and inertial sen-
sors (Chapter VI). The resulting contact probability estimates have direct application in
base state estimation and inverse dynamics control, providing a continuous measure of
contact which helps prevent destabilizing discontinuities.
In the process of developing new estimators for humanoid robots, we have developed
a considerable library of contemporary methods for planning and control as well (Chapter
II). The optimization-based inverse dynamics solver used in this work is a combination of
successful methods used in the DARPA Robotics Challenge, and was implemented specif-
ically in order to test possible control extensions (for example, smooth constraint switch-
ing, endeector impedance control and external wrench compensation). Our joystick-based
walking controller combines this solver with a simplied model-based planner in a state
machine framework, making it simple to test new estimation and controller modules in a
user-controlled, closed-loop walking task. These modules can easily be swapped out and
tested against the baseline walking controller and will thus prove useful to others working
on the same platform.
Outlook
The work presented in this thesis re
ects and builds upon current, state-of-the-art methods
for both estimation and control for humanoid robots; while such methods, along with
increasing computational power, have advanced the eld dramatically over the past few
years, much work remains before humanoid robots will be viable options for their target
applications. With regards to the work presented in this thesis in particular, we hope to
see our estimation methods used in whole-body control for walking in order to overcome
the many modeling and sensing challenges which each approach addresses. In addition,
we rmly believe that sensing will play an increasingly crucial role in legged locomotion.
As sensors become less expensive, we should build as many as possible into our robot
designs; this would allow the generation of large amounts of data which can be fed to
learning algorithms. Advances in machine learning need not (and may never be able to)
116
replace model-based methods entirely (nor should they, since rigid body dynamics serves
as a great prior), but they should be used more often to augment traditional approaches
- the contact estimator introduced in this work is a simple example of this. Generating
large amounts of data safely for learning approaches which augment or replace modules of
a walking controller is a necessary rst step towards potentially \solving" locomotion using
deep learning alone. In any case, those of us who work on legged locomotion have plenty
of data to work with and little idea how to utilize it - we believe this is a crucial problem
which will greatly advance the eld when addressed properly.
117
Appendix A
Kalman Filtering
The Kalman Filter provides an estimate of the state vector x along with a corresponding
covariance matrix P which species the uncertainty of the estimate. The lter involves 1)
propagating the estimates through the system in the prediction step to produce a priori
estimates ^ x

k
and P

k
and 2) updating the estimates using a measurement in the update
step to the a posteriori estimates ^ x
+
k
and P
+
k
.
However, the standard Kalman Filter is applicable only for state estimation in linear
systems. Suppose we have the continuous-time, nonlinear system
_ x =f(x;w) (A.1)
y =h(x;v) (A.2)
where f() is the prediction model;h() is the measurement model and w and v are noise
terms. One may simply linearize these models around the current estimate and apply the
Kalman Filter equations. This is known as the Extended Kalman Filter. While optimal-
ity and convergence are no longer guaranteed, this is a common approach for nonlinear
estimation.
118
The EKF algorithm works as follows. First, (A.1) and (A.2) are discretized and the
a priori state and measurement are computed using the resulting discretized models as
follows.
^ x

k
=f(^ x
+
k1
) (A.3)
^ y
k
=h(^ x

k
) (A.4)
Next, the process and measurement models (A.1) and (A.2) are linearized around the a
priori state estimate using a rst-order Taylor series expansion to produce the process and
measurement Jacobians which are discretized to form F
k
and H
k
. The prediction step is
completed by computing the a priori covariance using the linearized prediction model.
F
k
= exp

@f
@x
j
^ x

k

(A.5)
H
k
=
@h
@x
j
^ x

k
(A.6)
P

k
=F
k
P
+
k1
F
T
k
+Q
k
(A.7)
whereQ
k
is the discrete process noise covariance and exp(
_
). Finally, when a measurement
y
k
becomes available the state and covariance matrix are updated using the linearized
model as follows.
S
k
=H
k
P

k
H
T
k
+R
k
(A.8)
K
k
=P

k
H
T
k
S
1
k
(A.9)
^ x
+
k
= ^ x

k
+K
k
(y
k
^ y
k
) (A.10)
P
+
k
= (IK
k
H
k
)P

k
(A.11)
where R
k
is the discrete measurement noise covariance. The lter is initialized with an
estimate ^ x
+
0
of the state and corresponding covariance matrix P
+
0
which represents the
uncertainty in the chosen initial state.
119
A.0.1 Handling of Rotational Quantities
The unit quaternion was chosen to represent the base orientation in the original lter
due to its theoretical and computational advantages. However, since the quaternion is a
non-minimal representation of SO(3), special care must be taken in handling rotational
quantities in the EKF.
Figure A.1: Illustration of the mapping between the manifold of rotations SO(3) and its
tangent space so(3).
First, the exponential map
exp (!) =
0
@
sin (
jj!jj
2
)
!
jj!jj
cos (
jj!jj
2
)
1
A
(A.12)
is used to relate a quaternion at times k and k + 1 given an incremental rotation of
magnitudejj!jj about the unit vector !=jj!jj. That is,
q
k+1
= exp (!)
q
k
(A.13)
where
denotes quaternion multiplication. Roughly speaking, the exponential map is used
for \addition" of rotational quantities. Note that the rst entry of (A.12) is the vector
portion of the quaternion while the second entry is the scalar portion. Also note that there
are two self-consistent conventions for quaternion algebra; to avoid such issues, we employ
the quaternion conventions of (Trawny and Roumeliotis, 2005).
120
In the EKF state vector, a quaternion is represented by its corresponding three-dimensional
error rotation vector 2 so(3). The covariance of the orientation represented by the
quaternion is thus dened with respect to this minimal representation.
During the update step, the innovations vector e corresponding to a quaternion-valued
measurement is computed as the three dimensional rotation vector extracted from the
dierence between the actual measurement quaternion s and the expected measurement
quaternion z. That is,
e = log (s
z
1
) (A.14)
where log () denotes the logarithm mapping an element of SO(3) to its corresponding
element of so(3) (the inverse of the exponential map). The above operation extends the
notion of subtraction to rotational quantities.
Finally, using the innovations vector as computed above, the state correction vector x
is computed during the update step as x =Ke where K is the Kalman gain. While all
non-rotational states are updated simply as ^ x
+
k
= ^ x

k
+ x, a quaternion state is updated
using (A.12) as follows.
^ q
+
k
= exp ()
^ q

k
(A.15)
For more information on quaternion convention and use, see Appendix B.
121
Appendix B
Base State EKF Linearization
B.1 Quaternion Review and Conventions
It is important to note that there are two dierent conventions for using quaternions, each
of which is self-consistent; unfortunately, the quaternion algebra used in these conventions
is often mixed up in the literature, resulting in inconsistent implementations (Shuster,
1993). We use that proposed as a standard by JPL (Breckenridge, 1979) which was also
used in (Trawny and Roumeliotis, 2005) on which this work and that in (Bloesch et al.,
2012) was based. A review of quaternions using this convention is given in this section.
Brie
y, quaternions are one of several choices for representing SO(3), the Lie group of
rotations. The advantage of quaternions over other parameterizations is their numerical
properties, eciency and lack of singularities. We denote a quaternion by
q =
0
B
B
B
B
B
@
q
x
q
y
q
z
q
w
1
C
C
C
C
C
A
where q
w
and q
v
= [q
x
;q
y
;q
z
]
T
are the scalar and vector components, respectively. Note
that the ordering of these components does not depend on any conventions and is purely
a matter of preference. We choose this ordering here to match the literature but actually
implement quaternions in the reverse order to match the SL simulation environment.
122
All rotations are active, meaning that they act to rotate vectors; the quaternion rep-
resenting the base orientation is written as q = q
B
W
which species a rotation from the
world frame W to the base frame B. This quaternion corresponds to the rotation matrix
C =C[q] which rotates vectors dened in the world frame into the base frame.
Successive rotations about local axes are composed via left-multiplication, ie R
C
A
=
R
C
B
R
B
A
represents a rotation from frame A to frame B (given in terms of the frame A
basis) followed by a rotation from frame B to frame C (given in terms of the frame B
basis). Analogously, we have q
C
A
= q
C
B

q
B
A
for quaternions where
denotes quaternion
multiplication.
A vector in frame A is rotated into frame C as v
C
= R
C
A
v
A
, with the reverse trans-
formation given by v
A
= (R
C
A
)
1
v
C
where (R
C
A
)
1
=R
A
C
= (R
C
A
)
T
since this is a rotation
matrix (orthogonal matrix with det = +1). Vector rotations using quaternions are achieved
through conjugation as
v
C
=q
C
A

0
@
v
A
0
1
A

(q
C
A
)
1
where [v
A
; 0]
T
is called a pure quaternion and (q
C
A
)
1
= q
A
C
= [q
x
;q
y
;q
z
;q
w
] is the
inverse or conjugate quaternion satisfying q
C
A

(q
C
A
)
1
= (q
C
A
)
1

q
C
A
= q
I
where q
I
=
[0; 0; 0; 1] is the identity quaternion.
Quaternion multiplication is dened by
q
p =
0
B
B
B
B
B
@
q
w
p
x
+q
z
p
y
q
y
p
z
+q
x
p
w
q
z
p
x
+q
w
p
y
+q
x
p
z
+qyp
w
q
y
p
x
q
x
p
y
+q
w
p
z
+q
z
p
w
q
x
p
x
q
y
p
y
q
z
p
z
+q
w
p
w
1
C
C
C
C
C
A
123
This can be written more concisely as the matrix vector multiplication
q
p =L(q)p =
0
@
q
w
Iq

v
q
v
q
T
v
q
w
1
A
0
@
p
v
p
w
1
A
=R(p)q =
0
@
p
w
I +p

v
p
v
p
T
v
p
w
1
A
0
@
q
v
q
w
1
A
where q
v
= [q
x
;q
y
;q
z
]
T
is the vector part of q and
q

v
=
0
B
B
@
0 q
z
q
y
q
z
0 q
x
q
y
q
x
0
1
C
C
A
is the skew-symmetric matrix corresponding to the vector q
v
.
The rotation matrix corresponding the quaternion q is given by
C[q] = (2q
2
w
1)I 2q
w
q

v
+ 2q
v
q
T
v
=
0
B
B
@
2q
2
x
+ 2q
2
w
1 2(q
x
q
y
+q
z
q
w
) 2(q
x
q
z
q
y
q
w
)
2(q
x
q
y
q
z
q
w
) 2q
2
y
+ 2q
2
w
1 2(q
y
q
z
+q
x
q
w
)
2(q
x
q
z
+q
y
q
w
) 2(q
y
q
z
q
x
q
w
) 2q
2
z
+ 2q
2
w
1
1
C
C
A
where the rst equation can be seen as equivalent to Rodrigues' identity using the
quaternion exponential map
exp (!) =
0
@
sin (
jj!jj
2
)
!
jj!jj
cos (
jj!jj
2
)
1
A
124
which represents a rotation ofjj!jj about an axis !=jj!jj as a quaternion. Letting be
an innitesimal rotation, we see that
q = exp ()
0
@
1
2

1
1
A
is the rst-order approximation of an incremental quaternion. It follows from the denition
of C[q] that we have the rst-order approximation
C[q]I

This can also be seen as the rst-order approximation of the exponential map for rotation
matrices
exp (

) =
1
X
i=0
(

)
i
i!
I

It follows that the rst-order expansion of q about a nominal quaternion q can be written
in matrix form as
C[q
q] =C[q]C[ q] = (I

)

C
where

C =C[ q]. This approximation will be used in the derivations of the linearized lter
dynamics in the next section.
The derivative of a quaternion is related to the angular velocity ! by the equation
_ q =
1
2
0
@
!
0
1
A

q
and the rst-order approximation of
_
q is given by
_
q
0
@
1
2

_

0
1
A
125
B.2 Linearization
Linearization of both the process and measurement models is performed analytically by
expanding the lter states about their expected values using rst-order Taylor series ap-
proximations. Products of small deviations are considered to be negligible, ultimately
resulting in linear equations in terms of state deviations.
B.2.1 Process Model
Position
The original process model for the time-evolution of the position is
_ r =v
Letting r r +r and v v +v leads to
d
dt
(r +r) = _ r = v +v
From which it follows that
_
r +
_
r = v +v
However, we know that the expected value of _ r is
_
r = v, so we nally have
_
r =v
Velocity
The original process model for the time-evolution of the velocity is
_ v =C
T
(
~
fb
f
w
f
) +g
126
Let C (I

)

C, v v +v, and b
f

b
f
+b
f
so that
d
dt
( v +v) = _ v =

C
T
(I +

)(
~
f (

b
f
+b
f
)w
f
) +g
Simplifying yields
_
v +
_
v =

C
T
(I +

)(

f +f) +g
where we have dened

f =
~
f

b
f
and f =b
f
w
f
to be the \large-signal" and
\small-signal" accelerations as in (Sola, 2014). Expanding the right hand side yields
_
v +
_
v =

C
T

f +

C
T
f +

C
T

f +

C
T

f +g
Recognizing that
_
v =

C
T

f +g is the expected value of the velocity process model equation,
we are left with
_
v =

C
T
f +

C
T

f +

C
T

f
The last term on the right hand side is the (cross) product of two small vectors and can
thus be neglected. This leaves
_
v =

C
T
f +

C
T

f
Using the fact that a

b =b

a and expanding the expression for f nally results in
_
v =

C
T

f

C
T
b
f

C
T
w
f
127
B.2.2 Quaternion
The original process model for the time-evolution of the quaternion is
_ q =
1
2
0
@
~ !b
!
w
!
0
1
A

q
We dene a deviation q from the expected value q of the pose q by q = q
q and let
b
!

b
!
+b
!
so that
_ q =
d
dt
(q
q)
1
2
0
@
~ ! (

b
!
+b
!
)w
!
0
1
A

q =
_
q
q +q

_
q
Simplifying and using the fact that
_
q =
1
2
0
@
~ !

b
!
0
1
A

q yields
1
2
0
@
~ ! (

b
!
+b
!
)w
!
0
1
A

q =
_
q
q +q

0
@
1
2
0
@
~ !

b
!
0
1
A

q
1
A
Multiplying on the right of both sides by q
1
and recognizing that q =q
q
1
yields
1
2
0
@
~ ! (

b
!
+b
!
)w
!
0
1
A

q =
_
q +q

1
2
0
@
~ !

b
!
0
1
A
Solving for
_
q yields
_
q =
1
2
0
@
~ ! (

b
!
+b
!
)w
!
0
1
A

qq

1
2
0
@
~ !

b
!
0
1
A
=
1
2
0
@
~ !

b
!
0
1
A

qq

1
2
0
@
~ !

b
!
0
1
A
+
1
2
0
@
b
!
w
!
0
1
A

q
128
where the second step results from the fact that we can split a pure quaternion into the
sum of multiple pure quaternions (since they are just vectors) and distribute them over
quaternion multiplication. For the quaternion q corresponding to the small rotation ,
we may write q
0
@
1
2

1
1
A
and thus
_
q
0
@
1
2
_

0
1
A
so that
0
@
1
2
_

0
1
A
=
1
2
0
@
~ !

b
!
0
1
A

0
@
1
2

1
1
A

0
@
1
2

1
1
A

1
2
0
@
~ !

b
!
0
1
A
+
1
2
0
@
b
!
w
!
0
1
A

0
@
1
2

1
1
A
Using the fact that quaternion products can be written as matrix-vector products as shown
in the previous section and letting ^ ! = ~ !

b
!
, we can write the above as
0
@
1
2
_

0
1
A
=
1
2
2
4
0
@
^ !

^ !
^ !
T
0
1
A

0
@
^ !

^ !
^ !
T
0
1
A
3
5
0
@
1
2

1
1
A
+
1
2
0
@
(b
!
w
!
)

(b
!
w
!
)
(b
!
w
!
)
T
0
1
A
0
@
1
2

1
1
A
=
1
2
0
@
2^ !

0
0
T
0
1
A
0
@
1
2

1
1
A
+
1
2
0
@
b
!
w
!
0
1
A
where the second step results from simplifying and eliminating products of small quan-
tities (products of deviations and products of noise with deviations) in the second term.
Multiplying out this expression yields the equations
1
2
_
=
1
2
!

1
2
(b
!
+w
!
)
0 = 0
where the rst equation results in the linearized orientation dynamics
_
=!

b
!
w
!
and the second equation ensures consistency.
129
Foot Position
The original process model for the time-evolution of the position of the i
th
foot is
_ p =C
T
w
p
Again, letC = (I

)

C so that we haveC
T

C
T
(I +

). Also letp p +p so that
d
dt
( p +p) = _ p =

C
T
(I +

)w
p
Simplifying the above yields
_
p +
_
p =

C
T
w
p
+

C
T

w
p
The second term is the cross product of a state deviation and a (small) noise vector and
thus is neglected. Further, we know that
_
p =E[ _ p] =E[C
T
w
p
] =E[w
p
] = 0
since w
p
is zero-mean and since the rotation matrix C does not alter the statistics of w
p
.
It directly follows that
_
p =

C
T
w
p
Accelerometer/Gyroscope Bias
The original process model for the time-evolution of the accelerometer bias is
_
b
f
=w
b
f
Letting b
f

b
f
+b
f
yields
d
dt
(

b
f
+b
f
) =
_
b
f
=w
b
f
130
It follows that
_

b
f
+
_
b
f
=w
b
f
However, we know that
_

b
f
=E[
_
b
f
] =E[w
b
f
] = 0 since w
b
f
is zero mean and thus
_
b
f
=w
b
f
Likewise, for the gyroscope bias we have
_
b
!
=w
b!
Foot Quaternion
The continuous process model for the time-evolution of the foot quaternion is
_ z =
1
2
0
@
w
z
0
1
A

z
Let zz
z so that _ z = _ z
z +z

_
z. Then we have
_ z
z +z

_
z =
1
2
0
@
w
z
0
1
A

z
However,
_
z = 0 is the expected value of _ z since E[w
z
] = 0. We are thus left with
_ z
z =
1
2
0
@
w
z
0
1
A

z
Multiplying both sides on the left by z
1
and using the fact that z =z
z
1
, we have
_ z =
1
2
0
@
w
z
0
1
A

z
131
Again, writing the quaternion product as a matrix-vector product leads to
0
@
1
2

_

0
1
A
=
1
2
0
@
w

z
w
z
w
T
z
0
1
A
0
@

0
1
A
Multiplying out the above yields
0
@
1
2

_

0
1
A
=
0
@

1
4
w

z
+
1
2
w
z
1
4
w
T
z

1
A
After eliminating products of small quantities, the rst equation yields

_
=w
z
as desired, and the second yields 0 = 0 ensuring consistency.
B.2.3 Measurement Model
Relative Foot Position
The relative foot position measurement is given by (for a single foot)
s
p
=C(pr) +n
p
Letting s
p
s
p
+s
p
, C (I

)

C, p p +p and r r +r we have
s
p
+s
p
=s
p
= (I

)

C(( p +p) ( r +r)) +n
p
Expanding the above yields
s
p
+s
p
=

C( p r) +

C(pr)

C( p r)

C(pr)

+n
p
132
Recognizing that s
p
=

C( p r) +n
p
this simplies to
s
p
=

C(pr)

C( p r)

C(pr)

The last term above is the cross product of state deviations and is thus neglected. It follows
that, after using the fact that a

b =b

a, we have
s
p
=

Cr +

Cp +

C( p r)

Relative Foot Quaternion
The relative foot quaternion measurement is given by (for a single foot)
s
z
=n
z

q
z
1
Letting s
z
s
z

s
z
, qq
q and zz
z we have
s
z

s
z
=s
z
=n
z

(q
q)
(z
z)
1
After expanding the right hand side and regrouping, we have
s
z

s
z
=n
z

q
( q
z
1
)
z
1
Substituting s
z
= q
z
1
in the right hand side and multiplying on the right of both sides
by s
1
z
yields
s
z
=n
z

q

s
z

z
1

s
1
z

In (Trawny and Roumeliotis, 2005) was shown that a triple product of quaternions can be
written as

q
p
q
1

=
0
@
C[q]p
v
p
s
1
A
133
Sincez
1
is the quaternion corresponding to the small rotation, we have the approx-
imation
z
1

0
@

1
2

1
1
A
and thus

s
z

z
1

s
1
z

0
@

1
2
C[ s
z
]
1
1
A
Assuming that the rotations corresponding to s
z
and n
z
are small and rewriting the
quaternion products as matrix-vector products yields
0
@
1
2
s
z
1
1
A
=
0
@
1
2
n
z
1
1
A

2
4
0
@
I
1
2

1
2

1
2

T
1
1
A
0
@

1
2
C[ s
z
]
1
1
A
3
5
=
0
@
1
2
n
z
1
1
A

0
@

1
2
C[ s
z
] +
1
4

C[ s
z
] +
1
2

1
4

T
C[ s
z
] + 1
1
A
=
0
@
I
1
2
n

z
1
2
n
z

1
2
n
T
z
1
1
A
0
@

1
2
C[ s
z
] +
1
4

C[ s
z
] +
1
2

1
4

T
C[ s
z
] + 1
1
A
where s
z
is the vector corresponding to the measurement quaternion deviation, n
z
is the
measurement noise vector and is the vector corresponding to the deviation in the base
134
pose. The expression on the right can be simplied by eliminating terms involving products
of small quantities. This yields
0
@
1
2
s
z
1
1
A
=
0
@
I
1
2
n

z
1
2
n
z

1
2
n
T
z
1
1
A
0
@

1
2
C[ s
z
] +
1
2

1
1
A
=
0
B
B
@

1
2
C[ s
z
] +
1
2
+
1
4
n

z
C[ s
z
]
1
4
n

z
+
1
2
n
z
1
4
n
T
z
C[ s
z
]
1
4
n
T
z
+ 1
1
C
C
A
After again eliminating terms involving products of small quantities and simplifying (using
the fact that C
T
[q] =C[q
1
]), the rst equation yields the linearized measurement
s
z
=C[ q
z
1
] + +n
z
The second equation becomes 1 = 1, ensuring consistency.
135
Appendix C
Observability
A linear system is said to be observable if there exists a nite t
f
> t
0
such that for any
initial state x(t
0
), knowledge of the input u(t) and the measured output y(t) over the
interval [t
0
;t
f
] is sucient to determine x(t) on the interval.
C.1 Linear Systems
A linear (in general, time-varying) system is observable on the interval [t
0
;t
f
] if and only
if the observability Grammian
W
o
(t
0
;t
f
) =
Z
t
f
t
0

T
(;t
0
)C
T
()C()(;t
0
)d
is nonsingular. However, for a linear, time-invariant (LTI) system it can be shown that the
condition for observability is
rank O(A;C) = rank
2
6
6
6
6
6
6
6
6
4
C
CA
CA
2

C
n1
A
3
7
7
7
7
7
7
7
7
5
=n
136
where O(A;C) is called the observability matrix. If this condition does not hold, then the
unobservable subspace is given by the nullspace of O(A;C).
C.2 Nonlinear Observability
For nonlinear systems, observability isn't as well-dened. Essentially, we want to know
how much the outputy =h(x) changes with a change in the state _ x =f(x). Equivalently,
we want to know the derivative of vector eld h(x) along the
ow of vector eld f(x). We
can write:
y =h(x)
_ y =
dh(x)
dx
dx
dt
=L
1
(x)
y =
dL
1
(x)
dx
dx
dt
=L
2
(x)
and so on whereL
i
is called the i
th
Lie derivative of h(x). We can then write:
@
@t
y =
@h
@x
@x
@t
=rh
@x
@t
@
@t
_ y =
@L
1
@x
@x
@t
=rL
1
@x
@t
@
@t
y =
@L
2
@x
@x
@t
=rL
2
@x
@t
and so on. Now, "multiply" both sides by @t to get
0
B
B
B
B
B
B
@
@y
@ _ y
@ y
.
.
.
1
C
C
C
C
C
C
A
=
0
B
B
B
B
B
B
@
rh
rL
1
rL
2
.
.
.
1
C
C
C
C
C
C
A
@x
137
where we dene the nonlinear observability matrix to be
O =
0
B
B
B
B
B
B
@
rh
rL
1
rL
2
.
.
.
1
C
C
C
C
C
C
A
SoO relates a perturbation in the state to changes in the output and its derivatives.
We investigate the observability of an estimator having nonlinear prediction and/or
measurement models by forming the nonlinear observability matrix introduced in (Her-
mann and Krener, 1977). As in the linear case, the state is observable if O has full rank;
unobservable state combinations are parameterized by the nullspace of O. Given a system
with nonlinear process model _ x =f(x) and measurement model y =h(x),
O =
2
6
6
6
6
6
6
4
rh(x)
r(rhf(x))
r(r(rhf(x))f(x))
.
.
.
3
7
7
7
7
7
7
5
wherer denotes the Jacobian with respect tox. In the linear case,rh =C andf(x) =Ax
sor(rhf(x)) =r(CAx) = CA,r(r(rhf(x))f(x)) = CA
2
and so on. Unlike
in the linear case, there is no condition limiting the size of O; however, only a nite
number of derivatives usually need be taken before successive rows become zero (and thus
no longer aect the rank). Essentially,O illustrates that states are observable because they
are measured directly or because they appear in the dynamics (of some order) of a state
which is measured directly. Note that sinceO is state-dependent in general, this procedure
investigates local observability.
138
Appendix D
Inverse Dynamics Details
For the purposes of implementation, we must write the inverse dynamics quadratic program
of Section II.3 in the form
min
x
x
T
Hx + 2g
T
x
subject to constraints
A
eq
x +b
eq
= 0
A
ineq
x +b
ineq
0
Expanding the cost function, we have
( x x
des
)
T
W
x
( x x
des
) + (T
null
q q
des
)
T
W
q
(T
null
q q
des
) +
+ (
des
)
T
W

(
des
) + (
des
)
T
W

(
des
)
=

x
T
W
x
x 2 x
T
des
W
x
x + x
T
des
W
x
x
des

+

q
T
T
T
null
W
q
T
null
q 2 q
T
des
W
x
T
null
q + q
T
des
W
q
q
des

+
+
T
W

T
2
T
des
W

+
des
W

des
+
+
T
W

T
2
T
des
W

+
des
W

des
139
However, we must use the fact that x =J
u
q +
_
J
u
_ q; focusing for now on just the rst term
above,
x
T
W
x
x 2 x
T
des
W
x
x + x
T
des
W
x
x
des
= (J
u
q +
_
J
u
_ q)
T
W
x
(J
u
q +
_
J
u
_ q) 2 x
T
des
W
x
(J
u
q +
_
J
u
_ q) + x
T
des
W
x
x
des
= q
T
J
T
u
W
x
J
u
q + 2 _ q
T
_
J
T
u
W
x
J
u
q + _ q
T
_
J
T
u
W
x
_
J
u
_ q 2 x
T
des
W
x
J
u
q 2 x
T
des
W
x
_
J
u
_ q + x
T
des
W
x
x
des
Any terms which do not depend on q, or drop out of the cost, so we nally have
h
q
T
J
T
u
W
x
J
u
q + 2 _ q
T
_
J
T
u
W
x
J
u
q 2 x
T
des
W
x
J
u
q
i
+

q
T
T
T
null
W
q
T
null
q 2 q
T
des
W
x
T
null
q

+
+

T
W

T
2
T
des
W

+

T
W

T
2
T
des
W

which, after rearranging into the correct form and simplifying usingT
T
null
=T
null
since this
is an orthogonal projection, becomes
2
6
6
4
q

3
7
7
5
T2
6
6
4
J
T
u
W
x
J
u
+T
null
W
q
T
null
0 0
0 W

0
0 0 W

3
7
7
5
2
6
6
4
q

3
7
7
5
+2
2
6
6
4
J
T
u
W
x
(
_
J
u
_ q x
des
)T
null
W
q
q
des
W

des
W

des
3
7
7
5
T2
6
6
4
q

3
7
7
5
Equality Constraints
There are two sets of equality constraints which must be enforced: dynamics constraints
and contact constraints. Recall that the dynamics of the
oating base robot are
M(q) q +h(q; _ q) =S
T
+J
T
c

140
We can write this equation linearly in terms of the optimization variables as
h
M(q) J
T
c
S
T
i
2
6
6
4
q

3
7
7
5
+h(q; _ q) = 0
Similarly, the contact constraint is simply
J
c
q +
_
J
c
_ q = 0
which can be written in the form
h
J
c
0 0
i
2
6
6
4
q

3
7
7
5
+ (
_
J
c
_ q) = 0
Inequality Constraints
Torque Limits
Imposing limits on allowable joint torques is accomplished with the constraints

max

min
!
min
and in matrix form
2
4
0 0 I
0 0 I
3
5
2
6
6
4
q

3
7
7
5
+
2
4

max

min
3
5
0
141
Acceleration Limits
Unlike actuator limits which can be determined from datasheets, acceleration limits must
be chosen or tuned. Since the purpose of these limits is mainly to prevent hitting joint
stops, we limit the acceleration to the value that would drive the joint into an endstop in
t seconds, which can be the actual timestep of the controller or set to something larger
(to be more conservative).
q + _ qt +
1
2
qt
2
q
max
q + _ qt +
1
2
qt
2
q
min
!q _ qt
1
2
qt
2
q
min
which in matrix form is
2
4
1
2
t
2
I 0 0

1
2
t
2
I 0 0
3
5
2
6
6
4
q

3
7
7
5
+
2
4
q + _ qtq
max
q _ qt +q
min
3
5
0
Center of Pressure Limits
The center of pressure p2R
2
is given by
p
x
=

M;y

F;z
p
y
=

M;x

F;z
142
Eliminating Joint Torques
It has been noted that the dynamics of a
oating base robotic manipulator can be decom-
posed into two parts as
M
u
(q) q +h
u
(q; _ q) = +J
T
c;u

M
l
(q) q +h
l
(q; _ q) =J
T
c;l

where the rst set ofn equations describes the motion of the joints while the second set of
6 equations describes the motion of the unactuated
oating base of the robot. This latter
set of equations is also known as the momentum or Newton-Euler dynamics of the system;
it relates momentum rate of change of the mechanical system to the external forces applied
to the robot at its endeectors. Because these equations are linear in the variables q,
and we can simply use the second set of equations to constrain the optimization to the
robot dynamics and then compute the joint torques afterward as
=M
u
(q) q +h
u
(q; _ q)J
T
c;u

The optimization problem thus becomes
min
[ q;]
jj x x
des
jj
2
Wx
+jjT
null
q q
des
jj
2
Wq
+jj
des
jj
2
W

+jj(M
u
(q) q+h
u
(q; _ q)J
T
c;u
)
des
jj
2
W
subject to the constraints
M
l
(q) q +h
l
(q; _ q) =J
T
c;l

J
c
q +
_
J
c
_ q = 0
Because the last term in the above cost now depends on both q and after eliminating
, the hessian and gradient will change accordingly. Expanding just this term (ignoring
143
dependencies on q; _ q and immediately dropping any terms which do not depend on the
optimization variables) yields
(M
u
q +h
u
J
T
c;u

des
)
T
W

(M
u
q +h
u
J
T
c;u

des
)
= q
T
M
T
u
W

M
u
q + q
T
M
T
u
W

h
u
q
T
M
T
u
W

J
T
c;u
q
T
M
T
u
W

des
+
+h
T
u
W

M
u
qh
T
u
W

J
T
c;u
+

T
J
c;u
W

M
u
q
T
J
c;u
W

h
u
+
T
J
c;u
W

J
T
c;u
+
T
J
c;u
W

des
+

T
des
W

M
u
q +
T
des
W

J
T
c;u

which can be put into matrix form as
2
4
q

3
5
T
2
4
J
T
u
W
x
J
u
+T
null
W
q
T
null
+M
T
u
W

M
u
M
T
u
W

J
T
c;u
J
c;u
W

M
u
W

+J
c;u
W

J
T
c;u
3
5
2
4
q

3
5
+ 2
2
4
J
T
u
W
x
(
_
J
u
_ q x
des
)T
null
W
q
q
des
+M
T
u
W

(h
u

des
)
W

des
+J
c;u
W

(
des
h
u
)
3
5
T
2
4
q

3
5
144
Bibliography
Satnam Alag, Alice M Agogino, and Mahesh Morjaria. A methodology for intelligent
sensor measurement, validation, fusion, and fault detection for equipment monitoring
and diagnostics. AI EDAM, 15(04):307{320, 2001.
P. b. Wieber. Trajectory free linear model predictive control for stable walking in the
presence of strong perturbations. In 2006 6th IEEE-RAS International Conference on
Humanoid Robots, pages 137{142, Dec 2006. doi: 10.1109/ICHR.2006.321375.
V. Barasuol, J. Buchli, C. Semini, M. Frigerio, E. R. De Pieri, and D. G. Caldwell. A
reactive controller framework for quadrupedal locomotion on challenging terrain. In
2013 IEEE International Conference on Robotics and Automation, pages 2554{2561,
May 2013. doi: 10.1109/ICRA.2013.6630926.
M. Bloesch, C. Gehring, P. Fankhauser, M. Hutter, M. A. Hoep
inger, and R. Siegwart.
State estimation for legged robots on unstable and slippery terrain. In 2013 IEEE/RSJ
International Conference on Intelligent Robots and Systems, pages 6058{6064, Nov 2013.
doi: 10.1109/IROS.2013.6697236.
Michael Bloesch, Marco Hutter, Mark Hoep
inger, Stefan Leutenegger, Christian Gehring,
C. David Remy, and Roland Siegwart. State estimation for legged robots - consistent
fusion of leg kinematics and IMU. In Proceedings of Robotics: Science and Systems,
Sydney, Australia, July 2012.
T. Boaventura, C. Semini, J. Buchli, M. Frigerio, M. Focchi, and D. G. Caldwell. Dynamic
torque control of a hydraulic quadruped robot. In Robotics and Automation (ICRA),
145
2012 IEEE International Conference on, pages 1889{1894, May 2012. doi: 10.1109/
ICRA.2012.6224628.
W. G. Breckenridge. Quaternions proposed standard conventions. Technical report, NASA
Jet Propulsion Laboratory, October 1979.
M. Camurri, M. Fallon, S. Bazeille, A. Radulescu, V. Barasuol, D. G. Caldwell, and
C. Semini. Probabilistic contact estimation and impact detection for state estimation of
quadruped robots. IEEE Robotics and Automation Letters, 2(2):1023{1030, April 2017.
ISSN 2377-3766. doi: 10.1109/LRA.2017.2652491.
J. Carpentier, M. Benallegue, N. Mansard, and J. P. Laumond. Center-of-mass estimation
for a polyarticulated system in contact - a spectral approach. IEEE Transactions on
Robotics, 32(4):810{822, Aug 2016. ISSN 1552-3098. doi: 10.1109/TRO.2016.2572680.
P. Cheng and B. Oelmann. Joint-angle measurement using accelerometers and gyroscopesa
survey. Instrumentation and Measurement, IEEE Transactions on, 59(2), Oct 2010. ISSN
0018-9456. doi: 10.1109/TIM.2009.2024367.
Stefano Chiaverini, Bruno Siciliano, and Luigi Villani. A survey of robot interaction control
schemes with experimental comparison. IEEE/ASME Transactions on mechatronics, 4
(3):273{285, 1999.
A. Chilian, H. Hirschmuller, and M. Gorner. Multisensor data fusion for robust pose
estimation of a six-legged walking robot. In Intelligent Robots and Systems (IROS),
2011 IEEE/RSJ International Conference on, pages 2497{2504, Sept 2011. doi: 10.
1109/IROS.2011.6094484.
S. Chitta, P. Vernaza, R. Geykhman, and D.D. Lee. Proprioceptive localization for a
quadrupedal robot on known terrain. In Robotics and Automation, 2007 IEEE Interna-
tional Conference on, pages 4582{4587, April 2007. doi: 10.1109/ROBOT.2007.364185.
146
J. A. Cobano, J. Estremera, and P. Gonzalez de Santos. Location of legged robots in
outdoor environments. Robot. Auton. Syst., 56(9):751{761, September 2008. ISSN 0921-
8890. doi: 10.1016/j.robot.2007.12.003. URL http://dx.doi.org/10.1016/j.robot.
2007.12.003.
Mathew DeDonato, Velin Dimitrov, Ruixiang Du, Ryan Giovacchini, Kevin Knoedler,
Xianchao Long, Felipe Polido, Michael A. Gennert, Takn Padr, Siyuan Feng, Hirotaka
Moriguchi, Eric Whitman, X. Xinjilefu, and Christopher G. Atkeson. Human-in-the-loop
control of a humanoid robot for disaster response: A report from the darpa robotics
challenge trials. Journal of Field Robotics, 32(2):275{292, 2015. ISSN 1556-4967. doi:
10.1002/rob.21567. URL http://dx.doi.org/10.1002/rob.21567.
J. C. Dunn. Well-separated clusters and optimal fuzzy partitions. Journal of Cybernetics,
4(1):95{104, 1974. doi: 10.1080/01969727408546059. URL http://dx.doi.org/10.
1080/01969727408546059.
M.A. El-Gohary. Joint Angle Tracking with Inertial Sensors. PhD thesis, Portland State
University, 2013.
J. Englsberger, A. Werner, C. Ott, B. Henze, M. A. Roa, G. Garofalo, R. Burger, A. Beyer,
O. Eiberger, K. Schmid, and A. Albu-Scher. Overview of the torque-controlled hu-
manoid robot toro. In 2014 IEEE-RAS International Conference on Humanoid Robots,
pages 916{923, Nov 2014. doi: 10.1109/HUMANOIDS.2014.7041473.
M. F. Fallon, M. Antone, N. Roy, and S. Teller. Drift-free humanoid state estimation fusing
kinematic, inertial and lidar sensing. In 2014 IEEE-RAS International Conference on
Humanoid Robots, pages 112{119, Nov 2014. doi: 10.1109/HUMANOIDS.2014.7041346.
Maurice Fallon, Scott Kuindersma, Sisir Karumanchi, Matthew Antone, Toby Schneider,
Hongkai Dai, Claudia Prez D'Arpino, Robin Deits, Matt DiCicco, Dehann Fourie, Twan
Koolen, Pat Marion, Michael Posa, Andrs Valenzuela, Kuan-Ting Yu, Julie Shah, Karl
Iagnemma, Russ Tedrake, and Seth Teller. An architecture for online aordance-based
perception and whole-body planning. Journal of Field Robotics, 32(2):229{254, 2015.
147
ISSN 1556-4967. doi: 10.1002/rob.21546. URL http://dx.doi.org/10.1002/rob.
21546.
S. Faraji, L. Colasanto, and A. J. Ijspeert. Practical considerations in using inverse dynam-
ics on a humanoid robot: Torque tracking, sensor fusion and cartesian control laws. In
Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on,
pages 1619{1626, Sept 2015. doi: 10.1109/IROS.2015.7353584.
S. Feng. Online Hierarchical Optimization for Humanoid Control. PhD thesis, Carnegie
Mellon University, 2016.
Siyuan Feng, Eric Whitman, X. Xinjilefu, and Christopher G. Atkeson. Optimization-
based full body control for the darpa robotics challenge. Journal of Field Robotics, 32
(2):293{312, 2015. ISSN 1556-4967. doi: 10.1002/rob.21559. URL http://dx.doi.org/
10.1002/rob.21559.
Michele Focchi, Andrea del Prete, Ioannis Havoutis, Roy Featherstone, Darwin G. Caldwell,
and Claudio Semini. High-slope terrain locomotion for torque-controlled quadruped
robots. Autonomous Robots, 41(1):259{272, Jan 2017. ISSN 1573-7527. doi: 10.1007/
s10514-016-9573-1. URL https://doi.org/10.1007/s10514-016-9573-1.
Bernard Friedland. Advanced Control System Design. Prentice-Hall, Inc., Upper Saddle
River, NJ, USA, 1995. ISBN 0130140104.
B. Gassmann, F. Zacharias, J.M. Zollner, and R. Dillmann. Localization of walking robots.
In Robotics and Automation, 2005. ICRA 2005. Proceedings of the 2005 IEEE Interna-
tional Conference on, pages 1471{1476, April 2005. doi: 10.1109/ROBOT.2005.1570322.
Philippe Gerum. Xenomai-implementing a rtos emulation framework on gnu/linux. Tech-
nical report, Xenomai Project, 2004.
Ambarish Goswami. Postural stability of biped robots and the foot-rotation indica-
tor (fri) point. The International Journal of Robotics Research, 18(6):523{533, 1999.
148
doi: 10.1177/02783649922066376. URL http://ijr.sagepub.com/content/18/6/523.
abstract.
I. Hashlamon and K. Erbatur. Center of mass states and disturbance estimation for a
walking biped. In Mechatronics (ICM), 2013 IEEE International Conference on, pages
248{253, Feb 2013. doi: 10.1109/ICMECH.2013.6518544.
A. Herdt, N. Perrin, and P. B. Wieber. Walking without thinking about it. In Intelligent
Robots and Systems (IROS), 2010 IEEE/RSJ International Conference on, pages 190{
195, Oct 2010. doi: 10.1109/IROS.2010.5654429.
R. Hermann and Arthur J. Krener. Nonlinear controllability and observability. Automatic
Control, IEEE Transactions on, 22(5):728{740, Oct 1977. ISSN 0018-9286. doi: 10.
1109/TAC.1977.1101601.
A. Herzog, L. Righetti, F. Grimminger, P. Pastor, and S. Schaal. Balancing experiments
on a torque-controlled humanoid with hierarchical inverse dynamics. In 2014 IEEE/RSJ
International Conference on Intelligent Robots and Systems (IROS 2014), pages 981{988,
September 2014a. doi: 10.1109/IROS.2014.6942678.
A. Herzog, N. Rotella, S. Schaal, and L. Righetti. Trajectory generation for multi-contact
momentum control. In Humanoid Robots (Humanoids), 2015 IEEE-RAS 15th Inter-
national Conference on, pages 874{880, Nov 2015. doi: 10.1109/HUMANOIDS.2015.
7363464.
Alexander Herzog, Nicholas Rotella, Sean Mason, Felix Grimminger, Stefan Schaal, and
Ludovic Righetti. Momentum control with hierarchical inverse dynamics on a torque-
controlled humanoid. CoRR, abs/1410.7284, 2014b.
M. A. Hoep
inger, M. Hutter, C. Gehring, M. Bloesch, and R. Siegwart. Unsuper-
vised identication and prediction of foothold robustness. In 2013 IEEE Interna-
tional Conference on Robotics and Automation, pages 3293{3298, May 2013. doi:
10.1109/ICRA.2013.6631036.
149
Neville Hogan. Impedance control: An approach to manipulation: Part itheory. Journal
of Dynamic Systems, Measurement, and Control, 107, 1985. ISSN 0022-0434. doi:
10.1115/1.3140702. URL http://dx.doi.org/10.1115/1.3140702.
J. Honkakorpi, J. Vihonen, and J. Mattila. Mems-based state feedback control of multi-
body hydraulic manipulator. In Intelligent Robots and Systems (IROS), 2013 IEEE/RSJ
International Conference on, pages 4419{4425, Nov 2013. doi: 10.1109/IROS.2013.
6696991.
M. Hutter, M. Hoep
inger, C. Gehring, M. Bloesch, C. Remy, and R. Siegwart. Hybrid
operational space control for compliant legged systems. In Proc. of the 8th Robotics:
Science and Systems Conference (RSS), 2012, 2012a. doi: 10.1109/IROS.2010.5648837.
Marco Hutter, Christian Gehring, Michael Bloesch, Mark A Hoep
inger, C David Remy,
and Roland Siegwart. Starleth: A compliant quadrupedal robot for fast, ecient, and
versatile locomotion. In 15th International Conference on Climbing and Walking Robot-
CLAWAR 2012, 2012b.
J. Hwangbo, C. D. Bellicoso, P. Fankhauser, and M. Huttery. Probabilistic foot contact
estimation by fusing information from dynamics and dierential/forward kinematics. In
2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
pages 3872{3878, Oct 2016. doi: 10.1109/IROS.2016.7759570.
Matthew Johnson, Brandon Shrewsbury, Sylvain Bertrand, Tingfan Wu, Daniel Duran,
Marshall Floyd, Peter Abeles, Douglas Stephen, Nathan Mertins, Alex Lesman, John
Car, William Rifenburgh, Pushyami Kaveti, Wessel Straatman, Jesper Smith, Maarten
Grioen, Brooke Layton, Tomas de Boer, Twan Koolen, Peter Neuhaus, and Jerry
Pratt. Team ihmc's lessons learned from the darpa robotics challenge trials. Journal
of Field Robotics, 32(2):192{208, 2015. ISSN 1556-4967. doi: 10.1002/rob.21571. URL
http://dx.doi.org/10.1002/rob.21571.
150
W. Kabsch. A solution for the best rotation to relate two sets of vectors. Acta Crystal-
lographica Section A, 32(5):922{923, Sep 1976. doi: 10.1107/S0567739476001873. URL
http://dx.doi.org/10.1107/S0567739476001873.
S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi, and H. Hirukawa.
Biped walking pattern generation by using preview control of zero-moment point. In
Robotics and Automation, 2003. Proceedings. ICRA '03. IEEE International Conference
on, volume 2, pages 1620{1626 vol.2, Sept 2003. doi: 10.1109/ROBOT.2003.1241826.
Shuuji Kajita, Fumio Kanehiro, Kenji Kaneko, Kazuhito Yokoi, and Hirohisa Hirukawa.
The 3d linear inverted pendulum mode: A simple modeling for a biped walking pat-
tern generation. In Intelligent Robots and Systems, 2001. Proceedings. 2001 IEEE/RSJ
International Conference on, volume 1, pages 239{246. IEEE, 2001.
Svetoslav Kolev and Emanuel Todorov. Physically consistent state estimation and system
identication for contacts. In Humanoid Robots (Humanoids), 2015 IEEE-RAS 15th
International Conference on, pages 1036{1043. IEEE, 2015.
Scott Kuindersma, Robin Deits, Maurice Fallon, Andr es Valenzuela, Hongkai Dai, Frank
Permenter, Twan Koolen, Pat Marion, and Russ Tedrake. Optimization-based locomo-
tion planning, estimation, and control design for the atlas humanoid robot. Autonomous
Robots, 40(3):429{455, 2016. ISSN 1573-7527. doi: 10.1007/s10514-015-9479-3. URL
http://dx.doi.org/10.1007/s10514-015-9479-3.
SangJoo Kwon and Yonghwan Oh. Estimation of the center of mass of humanoid robot.
In Control, Automation and Systems, 2007. ICCAS '07. International Conference on,
pages 2705{2709, Oct 2007. doi: 10.1109/ICCAS.2007.4406826.
Sung-Hee Lee and Ambarish Goswami. A momentum-based balance controller for hu-
manoid robots on non-level and non-stationary ground. Autonomous Robots, 33(4):
399{414, 2012. ISSN 0929-5593. doi: 10.1007/s10514-012-9294-z.
151
P.-C. Lin, H. Komsuoglu, and D.E. Koditschek. Sensor data fusion for body state estima-
tion in a hexapod robot with dynamical gaits. Robotics, IEEE Transactions on, 22(5):
932{943, Oct 2006. ISSN 1552-3098. doi: 10.1109/TRO.2006.878954.
Pei-Chun Lin, Jau-Ching Lu, Chia-Hung Tsai, and Chi-Wei Ho. Design and implementa-
tion of a nine-axis inertial measurement unit. Mechatronics, IEEE/ASME Transactions
on, 17(4):657{668, Aug 2012. ISSN 1083-4435. doi: 10.1109/TMECH.2011.2111378.
Y. King Liu. Discussion: measurement of angular acceleration of a rigid body using linear
accelerometers. ASME Journal of Applied Mechanics, 43(2):377{378, 1976. doi: 10.
1115/1.3423861. URL http://dx.doi.org/10.1115/1.3423861.
Kendall Lowrey, Svetoslav Kolev, Yuval Tassa, Tom Erez, and Emanuel Todorov.
Physically-consistent sensor fusion in contact-rich behaviors. In 2014 IEEE/RSJ Inter-
national Conference on Intelligent Robots and Systems, pages 1656{1662. IEEE, 2014.
L. Manuelli and R. Tedrake. Localizing external contact using proprioceptive sensors: The
contact particle lter. In 2016 IEEE/RSJ International Conference on Intelligent Robots
and Systems (IROS), pages 5062{5069, Oct 2016. doi: 10.1109/IROS.2016.7759743.
S. Mason, N. Rotella, S. Schaal, and L. Righetti. Balancing and walking using full dynamics
lqr control with constraints. In Humanoid Robots (Humanoids), 2016 IEEE-RAS 16th
International Conference on (ACCEPTED), 2016.
K. Masuya and T. Sugihara. Dead reckoning of biped robots with estimated contact points
based on the minimum velocity criterion. In 2013 IEEE/RSJ International Conference
on Intelligent Robots and Systems, pages 3637{3642, Nov 2013. doi: 10.1109/IROS.2013.
6696875.
M. Mistry, S. Schaal, and K. Yamane. Inertial parameter estimation of
oating base
humanoid systems using partial force sensing. In 2009 9th IEEE-RAS International
Conference on Humanoid Robots, pages 492{497, Dec 2009. doi: 10.1109/ICHR.2009.
5379531.
152
M. Mistry, J. Buchli, and S. Schaal. Inverse dynamics control of
oating base systems
using orthogonal decomposition. In 2010 IEEE International Conference on Robotics
and Automation, pages 3406{3412, May 2010. doi: 10.1109/ROBOT.2010.5509646.
Yoshihiko Nakamura, Hideo Hanafusa, and Tsuneo Yoshikawa. Task-priority based redun-
dancy control of robot manipulators. The International Journal of Robotics Research,
6(2):3{15, 1987. doi: 10.1177/027836498700600201. URL http://ijr.sagepub.com/
content/6/2/3.abstract.
Jun Nakanishi, Rick Cory, Michael Mistry, Jan Peters, and Stefan Schaal. Operational
space control: A theoretical and empirical comparison. The International Journal of
Robotics Research, 27(6):737{757, 2008. doi: 10.1177/0278364908091463. URL http:
//ijr.sagepub.com/content/27/6/737.abstract.
S. Nozawa, S. Noda, M. Murooka, K. Okada, and M. Inaba. Online estimation of object-
environment constraints for planning of humanoid motion on a movable object. In 2017
IEEE International Conference on Robotics and Automation (ICRA), pages 1291{1298,
May 2017. doi: 10.1109/ICRA.2017.7989152.
V. Ortenzi, H. C. Lin, M. Azad, R. Stolkin, J. A. Kuo, and M. Mistry. Kinematics-
based estimation of contact constraints using only proprioception. In 2016 IEEE-RAS
16th International Conference on Humanoid Robots (Humanoids), pages 1304{1311, Nov
2016. doi: 10.1109/HUMANOIDS.2016.7803438.
A.J. Padgaokkar, K. W. Krieger, and A. I. King. Measurement of angular acceleration of
a rigid body using linear accelerometers. ASME Journal of Applied Mechanics, 42(3):
552{556, 1975. doi: 10.1115/1.3423640. URLhttp://dx.doi.org/10.1115/1.3423640.
Sangbum Park, Youngjoon Han, and Hernsoo Hahn. Balance control of a biped robot
using camera image of reference object. International Journal of Control, Automation
and Systems, 7(1):75{84, 2009. ISSN 1598-6446. doi: 10.1007/s12555-009-0110-2. URL
http://dx.doi.org/10.1007/s12555-009-0110-2.
153
P. Pastor, L. Righetti, M. Kalakrishnan, and S. Schaal. Online movement adaptation based
on previous sensor experiences. In IEEE International Conference on Intelligent Robots
and Systems (IROS), pages 365{371, 2011.
F. Pedregosa, G. Varoquaux, A. Gramfort, V. Michel, B. Thirion, O. Grisel, M. Blon-
del, P. Prettenhofer, R. Weiss, V. Dubourg, J. Vanderplas, A. Passos, D. Cournapeau,
M. Brucher, M. Perrot, and E. Duchesnay. Scikit-learn: Machine learning in Python.
Journal of Machine Learning Research, 12:2825{2830, 2011.
A. Petrovskaya, J. Park, and O. Khatib. Probabilistic estimation of whole body contacts
for multi-contact robot control. In Proceedings 2007 IEEE International Conference
on Robotics and Automation, pages 568{573, April 2007. doi: 10.1109/ROBOT.2007.
363047.
A. Del Prete, F. Nori, G. Metta, and L. Natale. Control of contact forces: The role of
tactile feedback for contact localization. In 2012 IEEE/RSJ International Conference
on Intelligent Robots and Systems, pages 4048{4053, Oct 2012. doi: 10.1109/IROS.2012.
6385803.
Morgan Quigley, Ken Conley, Brian P. Gerkey, Josh Faust, Tully Foote, Jeremy Leibs, Rob
Wheeler, and Andrew Y. Ng. Ros: an open-source robot operating system. In ICRA
Workshop on Open Source Software, 2009.
M. Reinstein and M. Homann. Dead reckoning in a dynamic quadruped robot: Inertial
navigation system aided by a legged odometer. In Robotics and Automation (ICRA),
2011 IEEE International Conference on, pages 617{624, May 2011. doi: 10.1109/ICRA.
2011.5979609.
C. Ridgewell. Humanoid Robot Friction Estimation in Multi-Contact Scenarios. PhD
thesis, Virginia Polytechnic Institute and State University, 2017.
Ludovic Righetti, Jonas Buchli, Michael Mistry, Mrinal Kalakrishnan, and Stefan Schaal.
Optimal distribution of contact forces with inverse-dynamics control. The International
154
Journal of Robotics Research, 32(3):280{298, March 2013. ISSN 0278-3649, 1741-3176.
doi: 10.1177/0278364912469821. URL http://ijr.sagepub.com/content/32/3/280.
G. P. Roston and Eric Krotkov. Dead reckoning navigation for walking robots. Technical
Report CMU-RI-TR-91-27, Robotics Institute, Pittsburgh, PA, November 1991.
N. Rotella, M. Bloesch, L. Righetti, and S. Schaal. State estimation for a humanoid
robot. In Intelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ International
Conference on, pages 952{958, Sept 2014. doi: 10.1109/IROS.2014.6942674.
N. Rotella, S. Mason, S. Schaal, and L. Righetti. Inertial sensor-based humanoid joint
state estimation. In 2016 IEEE International Conference on Robotics and Automation
(ICRA), pages 1825{1831, May 2016. doi: 10.1109/ICRA.2016.7487328.
N. Rotella, S. Schaal, and L. Righetti. Unsupervised contact learning for humanoid es-
timation and control. In Robotics and Automation (ICRA), 2018 IEEE International
Conference on (SUBMITTED), 2017.
G. Santaera, E. Luberto, A. Serio, M. Gabiccini, and A. Bicchi. Low-cost, fast and accurate
reconstruction of robotic and human postures via imu measurements. In Robotics and
Automation (ICRA), 2015 IEEE International Conference on, pages 2728{2735, May
2015. doi: 10.1109/ICRA.2015.7139569.
Stefan Schaal. The sl simulation and real-time control software package. Technical report,
University of Southern California, Dept. of Comp. Sci., June 2007.
Alfred R. Schuler, Anthony Grammatikos, and Kenneth A. Fegley. Measuring rotational
motion with linear accelerometers. Aerospace and Electronic Systems, IEEE Transac-
tions on, AES-3(3):465{472, May 1967. ISSN 0018-9251. doi: 10.1109/TAES.1967.
5408811.
Thomas Seel, Jrg Raisch, and Thomas Schauer. Imu-based joint angle measurement for
gait analysis. Sensors, 14(4):6891, 2014. ISSN 1424-8220. doi: 10.3390/s140406891.
URL http://www.mdpi.com/1424-8220/14/4/6891.
155
Claudio Semini. Hyqdesign and development of a hydraulically actuated quadruped robot.
Doctor of Philosophy (Ph. D.), University of Genoa, Italy, 2010.
M. D. Shuster. Survey of attitude representations. Journal of the Astronautical Sciences,
41:439{517, October 1993.
Bruno Siciliano, Lorenzo Sciavicco, and Luigi Villani. Robotics : modelling, planning and
control. Advanced Textbooks in Control and Signal Processing. Springer, London, 2009.
ISBN 1-8462-8641-7. URL http://opac.inria.fr/record=b1129198. 013-81159.
Joan Sola. Quaternion kinematics for the error-state KF. Technical report, Universitat
Politcnica de Catalunya, February 2014.
B.J. Stephens. State estimation for force-controlled humanoid balance using simple models
in the presence of modeling error. In Robotics and Automation (ICRA), 2011 IEEE
International Conference on, pages 3994{3999, May 2011. doi: 10.1109/ICRA.2011.
5980358.
B.J. Stephens and C.G. Atkeson. Dynamic balance force control for compliant humanoid
robots. In Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Con-
ference on, Oct 2010. doi: 10.1109/IROS.2010.5648837.
Y. Tassa, T. Erez, and E. Todorov. Synthesis and stabilization of complex behaviors
through online trajectory optimization. In 2012 IEEE/RSJ International Conference on
Intelligent Robots and Systems, pages 4906{4913, Oct 2012. doi: 10.1109/IROS.2012.
6386025.
Nikolas Trawny and Stergios I. Roumeliotis. Indirect Kalman lter for 3D attitude estima-
tion. Technical Report 2005-002, University of Minnesota, Dept. of Comp. Sci. & Eng.,
March 2005.
Nikolaos G Tsagarakis, Giorgio Metta, Giulio Sandini, David Vernon, Ricardo Beira,
Francesco Becchi, Ludovic Righetti, Jose Santos-Victor, Auke Jan Ijspeert, Maria Chiara
156
Carrozza, et al. icub: the design and realization of an open humanoid platform for cog-
nitive and neuroscience research. Advanced Robotics, 21(10):1151{1175, 2007.
J. Vihonen, J. Honkakorpi, J. Mattila, and A. Visa. Geometry-aided mems motion state
estimation for multi-body manipulators. In Advanced Intelligent Mechatronics (AIM),
2013 IEEE/ASME International Conference on, pages 341{347, July 2013. doi: 10.
1109/AIM.2013.6584115.
J. Vihonen, J. Honkakorpi, J. Koivumaki, J. Mattila, and A. Visa. Geometry-aided low-
noise angular velocity sensing of rigid-body manipulator using mems rate gyros and
linear accelerometers. In Advanced Intelligent Mechatronics (AIM), 2014 IEEE/ASME
International Conference on, pages 570{575, July 2014. doi: 10.1109/AIM.2014.6878139.
Liyang Wang, Zhi Liu, C.L.P. Chen, Yun Zhang, Sukhan Lee, and Xin Chen. A ukf-based
predictable svr learning controller for biped walking. Systems, Man, and Cybernetics:
Systems, IEEE Transactions on, 43(6):1440{1450, Nov 2013. ISSN 2168-2216. doi:
10.1109/TSMC.2013.2242887.
Patrick M Wensing and David Orin. Generation of dynamic humanoid behaviors through
task-space control with conic optimization. In Robotics and Automation (ICRA), 2013
IEEE International Conference on, pages 3103{3109. IEEE, 2013.
G. Wiedebach, S. Bertrand, T. Wu, L. Fiorio, S. McCrory, R. Grin, F. Nori, and J. Pratt.
Walking on partial footholds including line contacts with the humanoid robot atlas.
In 2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids),
pages 1312{1319, Nov 2016. doi: 10.1109/HUMANOIDS.2016.7803439.
R. Williamson and B.J. Andrews. Detecting absolute human knee angle and angular
velocity using accelerometers and rate gyroscopes. Medical and Biological Engineering
and Computing, 39(3):294{302, 2001. ISSN 0140-0118. doi: 10.1007/BF02345283. URL
http://dx.doi.org/10.1007/BF02345283.
157
Oliver J. Woodman. An introduction to inertial navigation. Technical Report UCAM-
CL-TR-696, University of Cambridge, Computer Laboratory, August 2007. URL http:
//www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.pdf.
Xinjilefu and C.G. Atkeson. State estimation of a walking humanoid robot. In Intelligent
Robots and Systems (IROS), 2012 IEEE/RSJ International Conference on, pages 3693{
3699, Oct 2012. doi: 10.1109/IROS.2012.6386070.
X. Xinjilefu, S. Feng, and C. G. Atkeson. Dynamic state estimation using quadratic pro-
gramming. In 2014 IEEE/RSJ International Conference on Intelligent Robots and Sys-
tems, pages 989{994, Sept 2014a. doi: 10.1109/IROS.2014.6942679.
X. Xinjilefu, S. Feng, W. Huang, and C. G. Atkeson. Decoupled state estimation for hu-
manoids using full-body dynamics. In 2014 IEEE International Conference on Robotics
and Automation (ICRA), pages 195{201, May 2014b. doi: 10.1109/ICRA.2014.6906609.
X Xinjilefu, Siyuan Feng, and Christopher G Atkeson. Center of mass estimator for hu-
manoids and its application in modelling error compensation, fall detection and preven-
tion. In Humanoid Robots (Humanoids), 2015 IEEE-RAS 15th International Conference
on, pages 67{73. IEEE, 2015.
X. Xinjilefu, S. Feng, and C. G. Atkeson. A distributed mems gyro network for joint
velocity estimation. In 2016 IEEE International Conference on Robotics and Automation
(ICRA), pages 1879{1884, May 2016. doi: 10.1109/ICRA.2016.7487334.
K. Yamane. Practical kinematic and dynamic calibration methods for force-controlled hu-
manoid robots. In Humanoid Robots (Humanoids), 2011 11th IEEE-RAS International
Conference on, pages 269{275, Oct 2011. doi: 10.1109/Humanoids.2011.6100803.
B. Zappa, G. Legnani, and A. J. van den Bogert. On the number and placement of
accelerometers for angular velocity and acceleration determination. Journal of Dynamic
Systems, Measurement, and Control, 123:552{554, 2001. doi: 10.1115/1.1386649.
158

Abstract (if available)

Abstract As sensor, actuator and processor technology continues to improve, humanoid robots have become more common in both academic and industrial environments as general-purpose platforms capable of performing a wide variety of automated tasks. These robots have the potential to operate in complex environments built for humans

Linked assets

University of Southern California Dissertations and Theses

Conceptually similar

PDF

Optimization-based whole-body control and reactive planning for a torque controlled humanoid robot

PDF

The representation, learning, and control of dexterous motor skills in humans and humanoid robots

PDF

Machine learning of motor skills for robotics

PDF

Rethinking perception-action loops via interactive perception and learned representations

PDF

Design of adaptive automated robotic task presentation system for stroke rehabilitation

PDF

The task matrix: a robot-independent framework for programming humanoids

PDF

Learning objective functions for autonomous motion generation

PDF

A robotic system for benthic sampling along a transect

PDF

Mobility-based topology control of robot networks

PDF

From active to interactive 3D object recognition

PDF

Leveraging cross-task transfer in sequential decision problems

PDF

Autonomous mobile robot navigation in urban environment

PDF

Design and control of a two-mode monopod

PDF

Landmark detection for faces in the wild

PDF

Data-driven robotic sampling for marine ecosystem monitoring

PDF

Mobile robot obstacle avoidance using a computational model of the locust brain

PDF

Identification, control and visually-guided behavior for a model helicopter

PDF

Situated proxemics and multimodal communication: space, speech, and gesture in human-robot interaction

PDF

Foundation models for embodied AI

PDF

Information theoretical action selection

Asset Metadata

Creator Rotella, Nicholas Peter (author)

Core Title Estimation-based control for humanoid robots

School Viterbi School of Engineering

Degree Doctor of Philosophy

Degree Program Computer Science

Publication Date 05/07/2018

Defense Date 12/12/2017

Publisher University of Southern California (original), University of Southern California. Libraries (digital)

Tag controls,dynamics,estimation,humanoid,machine learning,OAI-PMH Harvest,robot

Format application/pdf (imt)

Language English

Contributor Electronically uploaded by the author (provenance)

Advisor Righetti, Ludovic (committee chair), Schaal, Stefan (committee chair), Finley, James (committee member), Itti, Laurent (committee member)

Creator Email nicholas.rotella@gmail.com,nrotella@usc.edu

Permanent Link (DOI) https://doi.org/10.25549/usctheses-c40-502703

Unique identifier UC11267475

Identifier etd-RotellaNic-6317.pdf (filename),usctheses-c40-502703 (legacy record id)

Legacy Identifier etd-RotellaNic-6317.pdf

Dmrecord 502703

Document Type Dissertation

Format application/pdf (imt)

Rights Rotella, Nicholas Peter

Type texts

Source University of Southern California (contributing entity), University of Southern California Dissertations and Theses (collection)

Access Conditions The author retains rights to his/her dissertation, thesis or other graduate work according to U.S. copyright law. Electronic access is being provided by the USC Libraries in agreement with the a...

Repository Name University of Southern California Digital Library

Repository Location USC Digital Library, University of Southern California, University Park Campus MC 2810, 3434 South Grand Avenue, 2nd Floor, Los Angeles, California 90089-2810, USA