FACULTY OF AUTOMATION AND COMPUTER SCIENCE

AUTOMATION DEPARTMENT

In cooperation with



GHENT UNIVERSITY, FACULTY OF ENGINEERING
ELECTRICAL ENERGY, SYSTEMS & AUTOMATION DEPARTMENT








MODEL PREDICTIVE CONTROLLER APPLIED ON
WALKING LOCOMOTION OF A HUMANOID ROBOT



DIPLOMA THESIS









Student: Rbert MARC

Project Mentor: Eng. Abhishek DUTTA, MSc

Project Supervisor: Prof. Eng. Robin DE KEYSER, PhD

Project Supervisor: Prof. Eng. Ioan NACU, PhD


2010

FACULTY OF AUTOMATION AND COMPUTER SCIENCE
AUTOMATION DEPARTMENT

VISED,

DEAN, HEAD OF DEPARTMENT,

Prof. Eng. Sergiu NEDEVSCHI, PhD Prof. Eng. Liviu MICLEA, PhD




Student: Rbert MARC



MODEL PREDICTIVE CONTROLLER APPLIED ON WALKING
LOCOMOTION OF A HUMANOID ROBOT



1. The Problem: The project proposes t o i mplement, appl y and validat e an
MPC controll er on a three link biped robot, wit h point f eet .

2. Project Content: Cover pages, Declarati on of aut horship, Synthesis,
Acknowledgements, List of figures, Li st of t abl es, Abbreviations,
Nomenclat ure, Abst ract , Table of cont ents, Introducti on i nto bi ped
locomotion, Modelli ng of planar point feet bipedal robot, Feedback
cont rol of a bi ped, Model predi cti ve control of nonlinear pl ant ,
Conclusi ons, Ref erences, Appendi x.

3. Place of documentation: Ghent University, Ghent, Belgium

4. Consultant: Eng. Abhishek DUTTA, MSc.

5. Thesis emission date: February, 10, 2009

6. Thesis delivery date: June, 10, 2009








Student signature______________

Projects supervisor signature_______________



i








Declaration of authorship




I Rbert MARC, graduate student of Faculty of Automatics and Computer Science
from Technical University of Cluj-Napoca, declare that the ideas, analysis, design,
development, results and conclusions within this Diploma Thesis represent my own
effort except for those elements which do not and will be highlighted accordingly in
the document.
I also declare that from my knowledge this thesis is in an original form and was never
presented in other places or institutions but those explicitly indicated by me.



















Date: June, 10, 2010 Student: Rbert MARC
Matriculate number: 21020139
Signature: _________________



ii

Synthesis
Of the Diploma Thesis:

MODEL PREDICTIVE CONTROLLER APPLIED ON WALKING
LOCOMOTION OF A HUMANOID ROBOT


Student: Rbert MARC
Project Mentor: Eng. Abhishek DUTTA, MSc.
Project Supervisor: Prof. Eng. Robin DE KEYSER, PhD
Project Supervisor: Prof. Eng. Ioan NACU, PhD

1. Problem definition:
The control problem of bipedal robots can be defined as choosing a proper input such
that the system behaves stable with semi erect torso, and two walking legs. The present
thesis proposes a model predictive controller to regulate the walking motion of a three link
biped model with point feet.

2. Application domain:
Model predictive control with application to robotics.

3. Obtained Results:
The obtained MPC controller drives the outputs quicker to zero then the classical
computed torque feedback linearized controllers.

4. Testing and Verification:
Classical controls (computed torque with feedback linearization) along with the MPC
control were tested. The comparison was made in a biped simulator package. For
simulation purposes MATLAB 7.9.0.529 (R2009b) was used.

5. Personal Contributions:
The swing phase model was implemented in Simulink, along with the model predictive
control.

6. Documentation Sources:
Library of Ghent University (Universiteitsbibliotheek Gent), Internet







Date: June, 10, 2010 Student signature ______________

Projects supervisor signature_______________






iii

Acknowledgements

My deepest gratitude goes to my family for their unflagging love and support
throughout my life, especially during my student years, without them it would be
impossible to graduate. Moreover, I would like to thank to my girlfriend and my sister
for their moral support, love and courage, especially during my stay in Belgium.
Secondly, I would like to show my gratitude Professor Robin De Keyser (from Ghent
University) for offering me the possibility to do this thesis, for his advices and for the
wonderful trips that we made. Many thanks go to my promoter at the Technical
University of Cluj-Napoca, Professor Ioan Nacu, for allowing me to come to study in
Ghent.
I am heartily thankful to my direct supervisor, Abhishek Dutta, whose
encouragement, guidance and support from the initial to the final level enabled me to
develop an understanding of the subject. I am very grateful to my colleague Istvn,
who has been the best friend of mine, during our stay in Belgium. Lots of thanks go to
the other people in the lab, Daniel, Ramona, Ernesto, Yu, Andres, Adrian, Cristina and
Bogdan. Thank you all!
The financial support from the Technical University of Cluj-Napoca, through
the Erasmus Placement Scholarship is greatly appreciated. Without their support, my
ambition to study abroad can hardly be realized.
Lastly, I offer my regards to all of those who supported me in any respect
during the completion of the project.

Rbert Marc



iv

List of figures



Figure 1.1 Timeline: development of biped robots 2
Figure 1.2 Biped robots controlled by ZMP scheme: WL-10RD
(left), ASIMO (right)
3
Figure 1.3 Famous robots: Johnnie, HRP-2L, WL-16R, HUBO 3
Figure 1.4 Underactuated in SSP (a) and over-actuated in DSP (b) 8
Figure 1.5 The human planes of section 8
Figure 1.6 Walking cycle of a biped 9
Figure 2.1 Representation of the three link biped walker with torso 11
Figure 2.2 Key position and force nomenclature. 13
Figure 2.3 Configuration of a link 14
Figure 2.4 The model is indicated in: a) Generalized coordinates b)
Body coordinates
18
Figure 2.5 Hybrid model of walking with point feet 22
Figure 3.1 High level view of the present MIMO system 25
Figure 3.2 The inputs applied in the case of Bernstein Bhat controller 31
Figure 3.3 The inputs applied in the case of the feedback linearized
controller
31
Figure 3.4 Output of the MIMO system during three steps 32
Figure 3.5 Output of the MIMO system during five steps 32
Figure 4.1 MPC principle 35
Figure 4.2 MPC strategy as a block scheme 36
Figure 4.3 Simulink implementation of the swing phase model 40
Figure 4.4 Implementation of the inertia matrix (D) in Simulink 41
Figure 4.5 Sub block for a column in D 41
Figure 4.6 Fcn1 block parameters 41
Figure 4.7 Implementation of the Coriolis matrix (C) in Simulink 42
Figure 4.8 Sub block for a column in C 42
Figure 4.9 Fcn2 block parameters 42
Figure 4.10 First integrator block 43
Figure 4.11 Second integrator block 43
Figure 4.12 Output together with the optimization parameters 44
Figure 4.13 Nonlinear model plant 46
Figure 4.14 Step response generated 47
Figure 4.15 MPC scheme of the Model Predictive Control Toolbox 48
Figure 4.16 Signal properties in MPC toolbox 48
Figure 4.17 Simulated MPC controller inputs 49
Figure 4.18 Simulated MPC controller outputs 49
Figure 4.19 Simulink representation of swing phase dynamics together
with MPC controller
50
Figure 4.20 Initial call-back function associated with model, with
initial parameters
51
Figure 4.21 The two inputs generated by the simulation, after
resampling
51
Figure 4.22 Resulting outputs during one step after the 2D simulation 52
Figure 4.23 Forces calculated on the stance leg 53
Figure 4.24 Graphical representation of the states 53
Figure 4.25 Three link biped representation in the simulator 54


v

List of tables
Table 1 Biped parameters 23
Table 2 Optimizing the virtual constraints for minimal energy
consumption
26




vi

Abbreviations
COM Centre of Mass
COP Centre of Pressure
DC Direct Current
DOF Degree of Freedom
DSP Double Support Phase
EPSAC Extended Predictive Self-Adaptive Control
FES Functional Electrical Stimulation
FNS Functional Neuromuscular Stimulation
FRI Foot Rotation Indicator
GUI Graphical User Interface
MBPC Model Based Predictive Control
MIMO Multiple Input Multiple Output
MPC Model Predictive Control
SISO Single Input Single Output
SPP Single Phase Support
ZMP Zero Moment Point



vii

Nomenclature

Absolute angular coordinates

Absolute orientation or the absolute angle of link i

Body coordinates
Cartesian coordinates
Configuration and state manifolds

Configuration space of the link

External forces acting on the robot at the contact points

Generalized acceleration
Generalized configuration variables

Generalized coordinates for the single-support phase

Generalized velocities

Gravitational constant
Gravitational matrix

Horizontal Cartesian coordinate

Horizontal position of the centre of mass of link i

Identity matrix
Input
Input disturbance
Input matrix

Interval of DSP
Kinetic and potential energies

Mass of the link i

Mass-inertia matrix
Matrix of Coriolis and centrifugal terms
Number of links
Number of steps

One walking step length

Point on a link, with respect to the link coordinate frame

Position just before the impact, respectively after the impact

Position of the centre of mass

Position of the hips

Predicted output data vector
States

Tangent and normal forces

Total mass

Velocity just before the impact, respectively after the impact

Vertical Cartesian coordinate

Vertical position of the centre of mass of link i

Weight matrix

Zero matrix

Zero-mean white noise sequence

viii

Abstract
Biped robots form a subclass of legged or walking robots. The study of mechanical
legged motion has been motivated by its potential use as a means of locomotion in rough
terrain, or in the entertainment business, as well as its potential benefits to prosthesis
development and testing. This thesis concentrates on issues related to the automatic control of
biped robots.
In the legged robotics control literature feedback linearization is mostly used till date,
along with computed torque control, variable structure control, optimal and adaptive control.
The biped robot control locomotion in general addresses the following three problems. Firstly,
the reference trajectory is planned based on the stability analysis of the robot (ZMP/FRI).
Then it is also desirable to obtain a minimum jerk humanistic movement. Moreover the robot
actuators have obvious physical limitations. It is interesting to note that the above pattern of
problem formulation naturally fits into the MPC framework. This work explores the existing
void in this direction. One of the advantages of MPC is also that robust control ideas can be
easily incorporated. A non-linear MIMO dynamical system of a three link biped robot with
rigid, point feet would be considered for simulation purposes.
The thesis is organized in five parts: introduction, modelling, preliminary studies (such
as feedback control), model predictive control and least but not last, the conclusions of this
work.
In Chapter 1 will be an introduction to the biped locomotion and a short history. In
order to fully understand the work, it is necessary to be familiarized with some basic
terminology. Also there is presented an extensive overview of the current literature and
existing control strategies.
In the second part the biped model is introduced, which is a hybrid one, meaning that
it consists of the two parts: the single support phase and the double support phase. The
differential equations presented involve basic Lagrangian mechanics, without them it is
impossible to do the identification.
In Chapter 3 the nonlinear MIMO system is presented, along with the controlled
variables and zero dynamics. But the main consideration is the preliminary study about the
current control strategy, namely computed torque. The intention is to obtain a better
controller, with parameter tuning.
The major work is carried out in Chapter 4, specifically the linearization of the plant in
a certain operating point, the design, implementation and validation of a linear model
predictive controller.
Finally conclusions are presented.



ix

Table of contents

Declaration of authorship ................................................................................................ i
Synthesis ......................................................................................................................... ii
Acknowledgements ........................................................................................................ iii
List of figures ................................................................................................................. iv
List of tables ................................................................................................................... v
Abbreviations ................................................................................................................. vi
Nomenclature ................................................................................................................ vii
Abstract ........................................................................................................................ viii
Table of contents ............................................................................................................ ix
Chapter 1. Introduction into biped locomotion ............................................................ 1
1.1. A brief history ........................................................................................... 1
1.2. A brief overview of the literature .............................................................. 4
1.3. Why study the control of bipedal robots? ................................................. 4
1.4. Control of bipedal robots .......................................................................... 5
1.5. Biped basics .............................................................................................. 7
Chapter 2. Modelling of planar point feet bipedal robot ............................................ 10
2.1. Robot, gait and impact hypotheses ......................................................... 11
2.2. Mathematical background of the model ................................................. 13
2.3. Dynamic model of walking: swing phase ............................................... 17
2.4. Dynamic model of walking: impact model ............................................. 19
2.5. Hybrid model .......................................................................................... 21
2.6. Numerical example ................................................................................. 22
2.7. Stability framework ................................................................................ 22
2.8. Poincar maps and stability .................................................................... 23
Chapter 3. Feedback control of a biped ..................................................................... 25
3.1. MIMO system ......................................................................................... 25
3.2. The controlled variables .......................................................................... 25
3.3. The zero dynamics of walking ................................................................ 27
3.4. Computed torque with finite-time feedback control: .............................
Bernstein-Bhat controller............................................................... 28
3.5. Computed torque with linear feedback control ....................................... 30
3.6. Comparison of the two controllers .......................................................... 30
Chapter 4. Model predictive control of nonlinear plant ............................................. 33
4.1. Introduction ............................................................................................. 33
4.2. MPC principle ......................................................................................... 34
4.3. Predictive Control of MIMO Systems .................................................... 36
4.4. Simulink implementation of the swing phase model .............................. 40
4.5. Linearization ........................................................................................... 44
4.6. MPC controller design ............................................................................ 47
4.7. Simulation of the nonlinear model with MPC controller ........................ 50
4.8. Discussion of the results ......................................................................... 51
Chapter 5. Conclusions .............................................................................................. 55
References ..................................................................................................................... 56
Appendix A: Biped simulator ....................................................................................... 58
Appendix B: Script to obtain variables from Simulink ................................................ 71

1

Chapter 1. Introduction into biped locomotion
The idea of designing and realizing artificial entities is almost as old as humanity.
After some wonderful formal studies by Leonardo Da Vinci (considered as the most diversely
talented person ever to have lived), the first notable artificial machines, called automatons
were built, among others by Jacquard, Jacquet-Droz, Vaucanson (including his famous duck)
mainly in France around the 18th century. This was followed by a florilegium of realizations
from 1850 to World War I. The area then experienced a long eclipse, up to the early 1970s. It
should be noticed that the revival was then driven by scientists, instead of the brilliant
engineers, artists or magicians of the previous centuries.

1.1. A brief history
The pioneering works in the field of legged robots were achieved around 1970 by two
famous researchers: Kato and Vukobratovic. Both works were characterized by the design of
relevant experimental systems. In Japan, the first anthropomorphic robot, WABOT 1, was
demonstrated in 1973 by I. Kato and his team at Waseda University. Using a very simple
control scheme, it was able to realize a few slow steps in static equilibrium. This achievement
was the starting point of a prolific generation of legged robots in Japan [1].
In parallel, Vukobratovic and his team were very involved in the problems generated
by functional rehabilitation. At the Mihailo Puppin Institute, Belgrade, Yugoslavia, they
designed the first active exoskeletons, and several other devices such as the Belgrades hand,
but the most well-known outcome remains their analysis of locomotion stability, which
exhibited around 1972 the concept of Zero Moment Point, widely used since that time [2].
This was the first attempt to formalize the need for dynamical stability of legged robots; the
idea was to use the dynamic wrench in order to extend a classical criterion of static balance
(the centre of mass should project inside the convex hull of contact points).
In the next decade, the breakthroughs came from the United States. Following the
early work of McGhee in the 1960s at University of Southern California, then in the 1970s at
Ohio State University, this resulted in the first computer-controlled walking machine. Raibert
started to study dynamically stable running at Carnegie Mellon University. Then, he launched
the Massachusetts Institute of Technology LegLab, where a sequence of active hopping
robots, with one, two or four legs were designed, with impressive results, among them a
famous flip performed by a two-legged hopping machine.
Simultaneously, R. McGhee and K. Waldron, after the building of some prototypes,
achieved the design of the largest hexapod in the world, called the adaptive suspension
vehicle, a quasi-industrial system able to walk on natural irregular terrain, which was driven
by a human.
A third key period for research in legged robots was the early 1990s. Indeed, the idea
of studying purely passive mechanical systems was pioneered by McGeer [3]. Passive
walking is stable in theory, but in practice, vast work should be done to adjust its parameters
before realizing stable walking. In his seminal paper, McGeer introduces the concept of
natural cyclic behaviour, for a class of very simple systems: a plane compass on an inclined
plane. Stable walking results from the balance between increase of the energy due to the slope
and loss at the impacts. However, what should be emphasized here is that he popularized for
roboticists the analysis of such systems in terms of orbital stability using Poincar maps.
Moreover an important remark must be considered: the robot should walk on a slope, so it has
little actual significance. But several researchers have followed the tracks open by McGeer,

2

with many extensions: adding trunk, feet and knees, semi-passive control, walking/running
underactuated systems like the Rabbit robot [4], etc.
Finally, the end of the millennium was a period of intense technological activities
(timeline in Figure 1.1). Industrial breakthroughs showed to the world that building true
humanoids was now possible. In Japan, the first humanoid robot, P2, was exhibited by Honda
in 1996, followed by several more [5]. Presently, the most impressive technical achievements
are still realized by industrial companies: ASIMO (Honda), QRIO (Sony), HRP (Kawada),
being the major examples today, among others. In parallel, it should be noted that the market
for small humanoid robots, mainly aimed at entertainment, has grown steadily over the last
decade.


Figure 1.1. Timeline: development of biped robots
In the following section there will be a detailed introduction of famous biped robots,
mainly to underline the importance of the ZMP scheme. The ZMP might be one of the most
famous technical terms born in robotics community.
In the left side of 0 is WL-10RD, developed by Takanishi and Kato. This is the first
ZMP-based robot, which successfully realized dynamic biped walking in 1985 [6]. It is a 12-
DOF biped, 1.43 m high and weighing 84.5 kg, and driven by hydraulic actuators. On the
right side of 0 is ASIMO, a 26-DOF humanoid robot developed by Honda Motor Co. in 2000
[7]. This is one of the most famous robots in public culture, and at the same time, its superior
performance of biped locomotion (walking and running) is well acknowledged by specialists.
According to the published papers and patents, ZMP takes an important role in the walking
control of ASIMO.
Figure 1.3 shows also recently developed biped robots controlled by the ZMP scheme.
On the left it is Johnnie, developed by Gienger et al. in 2001 [8]. It is a 1.80 m high 17-DOF
humanoid weighing 40 kg, driven by DC servo motors with harmonic drive gears and ball
screws.
On Figure 1.3 there is HRP-2L, which was developed by Kaneko et al.[9]. It is a 1.41
m high 12-DOF biped weighing 58.2 kg, driven by DC servo motors with harmonic drive
reduction gears. On the figure below there is also the robot with the name WL-16R,
developed by Takanishi et al. [10] as a walking chair that can carry a human weighing up to
94 kg. It is a 1.29 m-high 12-DOF biped weighing 55 kg with Stewart-platform-type legs
driven by electric linear actuators. And also HUBO is present, developed by Oh et al. [11].It
is a 1.25 m-high 41-DOF humanoid robot weighing 55 kg.

3



Figure 1.2. Biped robots controlled by ZMP scheme: WL-10RD (left), ASIMO (right)

Although these robots have different leg mechanism and outlook, they share some
common features:
there are at least six fully actuated joints for each leg,
the joints are position controlled,
the feet are equipped with force sensors, which are used to measure the ZMP.



Figure 1.3. Famous robots: Johnnie, HRP-2L, WL-16R, HUBO
While examining the history above and the present state of the art, it is clear that
roboticists are now facing a challenge. Very nice technological achievements are available,
especially biped robots. However, the ability of these systems to walk truly autonomously on
uneven and various terrains in a robust way, i. e., in daily life, remains to be demonstrated.

4

1.2. A brief overview of the literature
In recent years, there has been a large effort in the development of bipedal robot
prototypes and in the control and analysis of bipedal gaits. The literature may be largely
divided into two categories: the analysis of passive walking - walking where gravity alone
powers the walking motion - and the analysis and control of powered walking - walking that
requires an external power source.
The available literature addresses a wide range of topics, from model formulation,
efficient means of computing the dynamical equations, relations between mechanical legged
locomotion and biological legged locomotion, methods of synthesizing gaits, the mechanical
realization of biped robots, and control.
A quick search of the literature, see for example [22], reveals over a hundred walking
mechanisms built by public research laboratories, universities, and major companies. A
canonical problem in bipedal robots is how to design a controller that generates closed-loop
motions, such as walking or running, that are periodic and stable (i.e., stable limit cycles).
There is a huge deficit in fundamental control design concepts in comparison to the number of
bipedal prototypes.
One can distinguish several control design approaches from the literature. By far, the
most common approach to control is through the tracking of pre-computed reference
trajectories. The trajectories may be determined via analogy, either with biological systems
[12] or with simpler, passive (the system is not actuated), mechanical biped systems; they can
be generated by an oscillator, such as van der Pol's oscillator [13], or computed through
optimization of various cost criteria, such as minimum expended control energy over a
walking cycle [14], [15]. Within the context of tracking, many different control methods have
been explored, including continuous-time methods based on PID controllers [16], computed
torque and sliding mode control or essentially discrete-time methods, based on impulse
control [17]. Other control methods have been investigated which do not rely on pre-
computed reference trajectories for the angular positions; these include controlling energy,
angular momentum, and others.
To date, for the case of a biped robot with a torso, there are a few of the various
control approaches that have produced a closed-loop system with provable stability
properties. Proving stability could be considered as rigorous and time consuming activity, but
a necessity. Since regular walking can be viewed as a periodic solution of the robot model,
the method of Poincar sections is the natural means to study asymptotic stability of a walking
cycle. However, due to the complexity of the associated dynamic models, this approach has
only been applied only to Raibert's one-legged-hopper [18], a biped robot without a torso [19]
and in theory to Grizzles three link biped robot.

1.3. Why study the control of bipedal robots?
The motivation for studying walking robots arises from diverse sociological and
commercial interests, ranging from the desire to replace humans in hazardous occupations
(demining, nuclear power plant inspection, military interventions, etc.) to the restoration of
motion in the disabled (dynamically controlled lower-limb prostheses, and FNS/FES,
rehabilitation robotics, and functional neural stimulation [21]), and the appeal of machines
that operate in anthropomorphic or animal-like ways (well-known biped and quadruped toys).
On the practical side, the study of mechanical legged locomotion has been motivated
by its potential use as a means of locomotion in rough terrain, or environments with
discontinuous supports, such as stairs, or the rungs of a ladder. It must also be acknowledged
that much of the current interest in legged robots stems from the appeal of machines that
operate in anthropomorphic or animal-like ways

5

As two major contributors of the biped literature affirmed "A humanoid robot is a
robot with an overall appearance based on that of the human body" (Hirai et al., 1998,
Hirukawa et al., 2004). This affirmation is considered general, but not the less, very real. The
truth is that humans are always fascinated by replicating themselfs, with one condition: to
have a control on the human like robots.

1.4. Control of bipedal robots
The control problem of bipedal robots can be defined as choosing a proper input
such that the system behaves in a desired fashion. The key issue of controlling the motion of
bipeds still hinges on the specification of a desired motion. There are numerous ways that one
can specify the desired behaviour of a biped, which in itself is an open question. The control
problem can become very simple or extremely complex depending on the specified desired
behaviour and the structure of the system. Typical bipedal machines are designed to perform
tasks that are not confined to simple walking actions. Such tasks may include manoeuvring in
tight spaces, walking or jumping over obstacles, and running. In this work, the main focus is
on tasks that are related to walking. Therefore, it is no concern with actions such as
performing manipulation tasks with the upper limbs.
The control action must assure that the motion of a multi-link kinematic chain, which
can characterize a typical biped, is that of a walker. Although, the characteristics of the
motion of a walker is still open to interpretation, one can translate this requirement to a set of
target/objective functions, where there are vector of parameters that prescribes certain aspects
of the walking action such as progression speed, step length, etc. Note that for the sake of
simplicity the notations will denote in the sequel the vector of generalized coordinates of
the model considered by the referenced author. The simplest way to proceed is to specify the
time profiles of the joint trajectories. Investigators in the field used kinematics of human gait
as desired profiles. One can also simply specify certain aspects of locomotion such as walking
speed, step length, upright torso, etc. Beletskii and Chudinov use the components of the
ground reaction forces in addition to kinematics in order to completely specify the control
torques. Once the objective functions are specified, one has to choose a control scheme in
order to specify the joint moments (control torques) that drive the system toward the desired
actions.
In the literature [26], there is a general study carried out. One can encounter several
approaches to this problem that can be enumerated as follows:
(1) Linear control: The equations of motion are linearized about the vertical stance,
assuming that the posture of the biped does not excessively deviate from this position. For
example, in many works a PD controller was used to track joint trajectories. The linear
controller, however, cannot track time functions. Thus, the authors discretized the desired
joint profiles and let the controller track the trajectory in a point-to-point fashion. Van der
Soest Knoek, Heanen, and Rozendaal studied the influence of delays in the feedback loop
on stance stability, including muscle model.
(2) Computed torque control: This method was to bipedal locomotion models with
various levels of complexities. Chevallereau combined the computed torque with a time-
scaling of the desired trajectories optimally designed, which allows the finite-time
convergence of the systems state towards the desired motion. The finite-time convergence
especially allows one to avoid the tricky problems due to tracking errors induced by impacts.
(3) Variable structure control: This method results in a feedback law that ensures
tracking despite uncertainties in system parameters. In this approach, one chooses the control
vector as

; where

is a trajectory tracking controller with fixed

estimated parameters. The second term is the variable structure part of the control input. The
function defines the sliding surface that represents the desired motion. This is a high gain

6

approach that is advantageous because it ensures convergence in finite time. In locomotion,
the stability of the overall motion relies on the effectiveness of the controller in eliminating
the errors induced by impact during the subsequent step. Please refer to Chang and Hurmuzlu
(1994) and Lum et al. (1999) to see the application of such a controller to a five-element
planar model, which is a more complex system with knees. In contrast to the three link biped
model discussed in this work
(4) Optimal control: Optimal control methods have been used by researchers to
regulate the smooth dynamic phase of bipedal locomotion systems. Two approaches have
been taken to the optimization problem. The first method is based on computing the values of
selected parameters in the objective functions that minimize energy-based cost functions. One
should note that optimal trajectory planning including the dynamics is equivalent to searching
for an optimal open-loop control. The second approach is based on variation of methods to
obtain controllers that minimize cost functions. It is the direct application of classical optimal
control methods to bipedal locomotion; see Channon, Hopkins, & Pham (1996) for the most
advanced work in this topic. Here the authors regulate the motion of the biped over a support
phase with a quadratic cost function. Van der Kooij, Jacobs, Koopman, & Van der Halm
propose a model predictive controller designed from a tangent linearization to regulate gait
descriptors formulated as end-point conditions. The main obstacle towards real
implementation is a too large computation time.
(5) Adaptive control: The adaptive control approach has received very little attention
in biped control. Perhaps it does not have real advantage in controlling bipedal locomotion.
Nevertheless, Yang (1994) has applied adaptive control approach to a three link, planar robot.
Experiments have been led at the MIT Leg Lab (Pratt, 2000) using adaptive control.
(6) Shaping discrete event dynamics: The abrupt nature of impact makes it practically
impossible to directly control its effect on the system state. Even an approximation of an
impulsive Dirac input would demand actuators with too high bandwidth (to say nothing of
induced vibrations in the mechanical structure). An alternate approach can be found in
shaping the system state prior to the impact instant such that a desired outcome is assured.
Such an approach was taken in Hurmuzlu in 1993 and Chang and Hurmuzlu in 1994. In these
studies, a set of objective functions was tailored. Assuming perfect tracking, the authors
derived the expression for the system state immediately before the instant of impact.
Subsequently, the post-impact state was computed for specific values of the parameter vector.
The parameter space was partitioned into regions according to slippage and contact conditions
that result from the foot impact. Then, this partitioning was used to specify controller
parameters such that the resulting gait pattern has only single support phase and the feet
would not slip as a result of the feet impact. Dunn and Howe (1994) developed conditions in
terms of motion and structural parameters such that they minimize/eliminate the velocity
jumps due to ground impact and limb switching. Thus, in their case, the objective of the
shaping was to remove the effect of the impact altogether. Miura and Shimoyama (1984) used
a feed-forward input that modifies the motion at the end of each step from measurements
information. Grizzle et al. (1999, 2001) and Werstervelt et al. (2003) have also used a similar
approach. They shape the state before the impact, so that at the next step the state resides in
the zero dynamics. Doing so they create a periodic gait that corresponds to the zero dynamics
defined from a set of output functions, on which work this thesis is based on. Piiroinen and
Dankowicz (2002) locally stabilize a passive walk with a specific strategy.
(7) Stability and periodic motions: Stability of the overall gait is often overlooked in
locomotion studies. Typically, controllers have been developed, and few gait cycles have been
shown to demonstrate that the biped walks with the given controller. A thorough analysis of
the nonlinear dynamics of a planar, five-element biped reveals a rich set of stable, periodic
motions that do not necessarily conform to the classical period one locomotion. Tracking
errors in the control action may lead to stable gait patterns that are different than the ones that

7

are intended by the objective functions. One way to overcome this difficulty is to partition the
parameter space such that one would choose specific values that lead to a desired gait pattern.
(8) Other specialized control schemes: Several investigators used simplified models
without impact and constructed periodic trajectories by concatenation of orbits obtained from
individually controlled segments of the gait cycle. This approach is quite similar in spirit to
the Kobrinskii (1965) method that is used the existence of trajectories of the impact damper
and the impacting inverted pendulum. Blajer and Schielen (1992) compute a nonlinear feed-
forward torque corresponding to a non-impacting reference walk and use PD motion and PI
force feedback to stabilize around the reference trajectory. Fuzzy logic control was used to
develop a force controller that regulates ground reaction forces in swaying actions of an
experimental biped. A group of investigators changed the parameters in the objective
functions such that the desired motion is adapted to changing terrain conditions. Zheng used
an acceleration compensation method to eliminate external disturbances from the motion of
an experimental eight joint robot. Kuo (in 1999) derives numerically an impact Poincar map
that represents the walking cycle, and proposes a linear state feedback that stabilizes this
cycle. Clearly, this is conceptually completely different from the works described above since
the design is based on a linearization of the Poincar map itself and not of the continuous
dynamics on one step.
This framework encompasses all the control types till date which have been used to
study locomotion in the control and robotics literature.
In conclusion it can be affirmed, as an opinion, that one important development is still
missing in the field of biped design: a concise and sufficiently general theoretical analysis
framework, based on realistic models, which allows the designer to derive stable controllers
taking into account the hybrid dynamics in their entirety.

1.5. Biped basics
Before going further, some basic terminology is introduced; the terminology will
allow a description of the essential elements of a dynamic model of a bipedal robot to be
given.
A biped is an open kinematic chain consisting of two sub chains called legs and, often,
a sub chain called the torso, all connected at a common point called the hip. When the
locomotion mechanism changes it is the transition from an open to a closed kinematic chain.
One or both of the legs may be in contact with the ground. When only one leg is in
contact with the ground, the contacting leg is called the stance leg and the other is called the
swing leg. The end of a leg, whether it has links constituting a foot or not, will sometimes be
referred to as a foot. The statically unstable single support or swing phase is defined to be the
phase of locomotion where only one foot is on the ground. Conversely, the statically stable
double support is the phase where both feet are on the ground; see Figure 1.4, obviously valid
if all of the joints of the robot are actuated and the feet are not slipping.
Walking is then defined as alternating phases of single and double support, with the
requirement that the displacement of the horizontal component of the robots COM is strictly
monotonic. Implicit in this description is the assumption that the feet are not slipping when in
contact with the ground.
The sagittal plane is the longitudinal plane that divides the body into right and left
sections. The frontal plane is the plane parallel to the long axis of the body a perpendicular to
the sagittal plane that separates the body into front and back portions. The transverse plane is
perpendicular to both the sagittal and frontal planes. See 0 for an illustration of these planes of
section (taken from [23]).


8



Figure 1.4. Underactuated in SSP (a) and over-actuated in DSP (b)
A planar biped is a biped with motions taking place only in the sagittal plane, whereas
a three-dimensional walker has motions taking place in both the sagittal and frontal planes. A
statically stable gait is periodic locomotion in which the bipeds COM does not leave the
support polygon, that is, the convex hull formed by all of the contact points with the ground.3
A quasi-statically stable gait is one where the COP of the bipeds stance foot remains strictly
within the interior of the support polygon, and hence does not lie on the boundary. Loosely
speaking, a dynamically stable gait is then a periodic gait where the bipeds COP is on the
boundary of the support polygon for at least part of the cycle and yet the biped does not
overturn.

Figure 1.5. The human planes of section


9


Figure 1.6. Walking cycle of a biped

One can define the walking cycle as a periodic phenomenon, seen also on the Figure
1.6, with some notations:
one walking step ()
the number of steps ()
the interval of DSP ()


10

Chapter 2. Modelling of planar point feet bipedal robot
This chapter introduces dynamic model for walking motions of planar bipedal robots
with point feet. The robot is assumed to consist of rigid links with mass, connected via rigid,
frictionless, revolute joints to form a single open kinematic chain lying in a plane. It is further
assumed that there are two identical subchains called the legs, connected at a common point
called the hip, and, additional subchain that may be identified as a torso. Each leg end is
terminated in a point so that, in particular, either the robot does not have feet, or it is walking
tiptoe.
All motions will be assumed to take place in the sagittal plane and consist of
successive phases of single support and double support in the case of walking. Conditions that
guarantee the leg ends alternate in ground contact - while other link such as the torso remain
free in the air - will be imposed during control design in later. Motions such as crawling,
tumbling, skipping, hopping, dancing will not be studied.
The distinct phases of walking motions naturally lead to mathematical models that are
comprised of distinct parts: the differential equations describing the dynamics during a single
support phase, and a model that describes the dynamics when a leg end impacts the ground.
For the models developed here, the ground - also called a walking or running surface - is
assumed to be smooth and perpendicular to the gravitational field, that is, the ground is
assumed to be flat as opposed to sloped. Impacts with the ground can be compliant or
inelastic. An inelastic model can be rigid (as used in this thesis) or non-rigid. For common
walking surfaces the impact duration or transient phase of the impact model is very short. In
ordinary human gait is less than 20% of a step cycle, studies revealed. The corresponding
differential equations are numerically very stiff and including them can greatly complicate the
simulation and analysis of a walking gait; moreover, determining physically reasonable
parameters for a compliant impact model is itself a very challenging problem. To avoid these
difficulties, throughout this thesis, a rigid (i.e., perfectly inelastic) contact model will be
assumed for the purposes of control design and analysis. The rigid contact model of [25]
effectively collapses the impact phase to an instant in time. The impact forces are
consequently modelled by impulses, and a discontinuity or jump is allowed in the velocity
component of the robots state, with the configuration variables remaining continuous or
constant during the impact. The dynamic models of walking are thus hybrid in nature,
consisting of continuous dynamics and a reinitialization rule at the impact event.
This section introduces the dynamic model of a simple, planar biped robot. The robot
consists of a torso, hips, and two legs of equal length, with no ankles and no knees, see Figure
2.1 just below. It has five degrees of freedom. Two torques are applied between the legs and
the torso, so the system is under actuated. It is assumed that the walking cycle takes place in
the sagittal plane. It is further assumed that the walking cycle consists of successive phases of
single support (meaning only one leg is touching the ground), with the transition from one leg
to another taking place in an infinitesimal length of time. This assumption entails the use of a
rigid model to describe the impact of the swing leg with the ground. The model of the biped
robot thus consists of two parts: the differential equations describing the dynamics of the
robot during the swing phase, and an impulse model of the contact event. Such models are
very common in the field of biped locomotion.

11


Figure 2.1. Representation of the three link biped walker with torso
During the swing phase, the stance leg is modelled as a pivot. Between impacts, it is
assumed that the swing leg does not interact with the ground. The assumptions are more
precisely written down in the next subchapter.
This a logically weak link in walking models without extensible legs of some sort
(either angular or prismatic joints at the "knees"): one is obliged to imagine (postulate) some
means for the swing leg to move without touching the ground until the desired moment of
contact. The idea here is that: if for one reason or another, a person's knee is immobilized,
walking is still possible. The motion consists of flexing the hip and causing the leg to swing
out of the plane of forward motion, and into the frontal plane, normal to the direction of
motion. This allows the swing leg to clear the ground and be posed in front of the stance leg.
Here, it will be further assumed that the swing leg is designed to renter the plane of motion
when the angle of the stance leg attains a given value

.
It is emphasized that all of the above will be illustrated on one of the simplest possible
biped robot models. The robot consists of a torso, hips, and two legs of equal length, with no
ankles and no knees. The two legs are actuated. The reason for this choice of model is
twofold: firstly, asymptotically stable walking has never been proved for such a model, and
thus this simplest problem is still open; secondly, from a pragmatic standpoint, it did not seem
advantageous to obscure the main elements of the control approach with the computational
complexity of a more complete biped model.

2.1. Robot, gait and impact hypotheses
With the terminology described in the previous chapter in mind, complete lists of
hypotheses are now enumerated for the robot model, the desired walking gaits, and the impact
model identical to [21].

2.1.1. Robot with Point Feet Hypotheses
The robot is assumed to be:

12

HR1) comprised of N rigid links connected by ideal revolute joints (i.e., rigid and
frictionless) to form a single open kinematic chain; furthermore, each link has nonzero mass
and its mass is distributed (i.e., each link is not modeled as a point mass);
HR2) planar, with motion constrained to the sagittal plane;
HR3) bipedal, with two symmetric legs connected at a common point called the hip, and both
leg ends are terminated in points;
HR4) independently actuated at each of the ideal revolute joints;
HR5) not actuated at the point of contact between the stance leg and ground and
HR6) the model is expressed in body coordinates

plus one
absolute angular coordinate

.

2.1.2. Gait Hypotheses for Walking
Conditions on the controller will be imposed and shown to ensure that the robot's
consequent motion satisfies the following properties consistent with the notion of a simple
walking gait:
HGW1) there are alternating phases of single support and double support;
HGW2) during the single support phase, the stance leg end acts as an ideal pivot, that is,
throughout the contact, it can be guaranteed that the vertical component of the ground reaction
force is positive and that the ratio of the horizontal component to the vertical component does
not exceed the coefficient of static friction so it does not slip;
HGW3) the double support phase is instantaneous and the associated impact can be modelled
as a rigid contact [24];
HGW4) at impact, the swing leg neither slips nor rebounds, while the former stance leg
releases without interaction with the ground;
HGW5) in steady state, the motion is symmetric with respect to the two legs;
HGW6) in each step, the swing leg starts from strictly behind the stance leg and is placed
strictly in front of the stance leg at impact and
HGW7) walking is from left to right and takes place on a level surface.
In particular, Hypotheses HGW5 and HGW6 impose the swapping of the roles of the
two legs at impact so that walking does not consist of rocking back and forth on the same
support leg. The symmetric nature of the gait is a natural requirement for a simple walking
motion, but is not a necessary condition for applying the methods.

Remark: Hypotheses HR1 and HR2 imply the robot has (N+2)-degrees of freedom (DOF) (N
joint angles plus the Cartesian coordinates of the hip, for example). Hypothesis HGW2
implies that in single support, the robot has N-DOF (the N joint angles, for example).
Hypotheses HR4, HR5 and HGW2 imply that in single support, the robot has one degree of
under actuation, i.e., one less actuator than DOF.

2.1.3. Rigid Impact Model Hypotheses
An impact occurs when the swing leg contacts the ground. The impact is modelled as
a contact between two rigid bodies. There are many rigid impact models in the literature and
all of them can be used to obtain an expression for the generalized velocity just after the

13

impact of the swing leg with the walking surface in terms of the generalized velocity and
position just before the impact. The model from [24] is used here for walking. The model is
essentially identical in the two cases. The one difference is noted in the list of hypotheses:
HI1) an impact results from the contact of the swing leg end with the ground;
HI2) the impact is instantaneous;
HI3) the impact results in no rebound and no slipping of the swing leg;
HI4) in the case of walking, at the moment of impact, the stance leg lifts from the ground
without interaction (after impact the vertical component of the velocity of the swing leg end
must be positive.);
HI5) the externally applied forces during the impact can be represented by impulses;
HI6) the actuators cannot generate impulses and hence can be ignored during impact and
HI7) the impulsive forces may result in an instantaneous change in the robot's velocities, but
there is no instantaneous change in the configuration.

2.1.4. Some Remarks on Notation
While developing the dynamic models of walking, the generalized coordinates for the
stance (or single-support) phase will be denoted by

. In general, a point on the robot

(or its centre of mass) will be denoted by its Cartesian coordinates

with respect
to the inertial frame. Some points and forces of particular interest are identified in Figure 2.2,
namely, the ends of the stance and swing legs, denoted respectively by

and

, t he
position of the hips,

, and the position of the centre of mass,

. For any robot satisfying

HR1HR5, the Cartesian positions of the leg ends, hip, and centre of mass are identified on
Figure 2.2, as well as possible forces acting on the leg ends.

Figure 2.2. Key position and force nomenclature.

2.2. Mathematical background of the model
This section provides a very selective overview of Lagrangian dynamics as used to
compute dynamic models of planar bipedal robots [21]. Much more general treatments are
available in most textbooks dealing with robotic manipulator dynamics and some recent
monographs on model-based animation of human figures.
It is assumed that individual links have nonzero mass, length, and moment of inertia
about their centre of mass. While the nonzero mass and moment of inertia assumptions are not
strictly necessary on every link in order to arrive at a well defined mechanical model, they

14

rule out certain trivial cases that one would otherwise had to carefully delineate. The
connection of links through prismatic joints is also not considered.
The topics discussed include, computing the kinetic and potential energies of a single
link and multiple links in common situations, absolute versus relative angles, generalized
coordinates, the Lagrangian and Lagranges equation (also called the Euler-Lagrange
equation).
This subchapter summarizes how to use the method of Lagrange in order to derive the
equations of motion for robots comprised of N-link, open kinematic chains with N, one-DOF
revolute joints, moving in three dimensions. The purpose of including this material is to
underline, in a more fundamental manner, the invariance of the kinetic energy under
translations and rotations of the inertial frame, which is the source of cyclic variables of the
kinetic energy. The mechanical portions of the planar bipedal robot model of this thesis are
special cases of the models derived here.

2.2.1. Kinetic and potential energy of a single link
The kinetic and potential energy definition in this case is a bit more complicated, in
order to deduce it is introduced some extra notations and definitions.

Figure 2.3. Configuration of a link
As in Figure 2.3, let

denote the Cartesian position of the origin

of the coordinate frame of link i with respect to a fixed coordinate frame, called a world frame
or an inertial frame, and let

be an element in, the circle, denote its orientation with

respect to the inertial frame. The angle

is called the absolute orientation or the absolute

angle of link i since it references the links orientation to the inertial frame. It will be assumed
that all angles are positive in the counter clockwise direction, more precisely, it will be
expected that all angles are measured such that they are increasing in the counter clockwise
direction. An advantage of the latter convention is that the absolute orientation of the link
with respect to the inertial frame

is easily indicated.
The configuration space of the link is

. Referring again to Figure 2.3,

any point in the link frame can be mapped to its Cartesian coordinates

in the inertial
frame using

:

*

+ *

+ (

) *

+ (2.1)
Where:

(

) *
(

) (

)
(

) (

)
+
(2.2)
The velocity of the link in the inertial frame is given by

15

*

+ *

+ *


+ (

) *

(2.3)
Where

is the absolute angular velocity, and it is used the fact that

*


+
(2.4)
That is

*
(

) (

)
(

) (

)
+
*


+ *
(

) (

)
(

) (

)
+
(2.5)
When the link is free, its configuration and velocity can take on any value in

link and one says that the link has 3 DOF. Recall that it is assumed that a link has
nonzero mass, length, and moment of inertia about the centre of mass. The position of the
centre of mass of link i in the world frame can be expressed as:

*

+ *

+ (

) *

+
(2.6)
And its velocity is given by

*

+ *

+ *


+ (

) *

(2.7)
The position and velocity of the centre of mass will now be used to determine the
potential energy and kinetic energy of a link. Potential energy of one link is simply:

(2.8)
Where

denotes the gravitational constant,

is the mass of the link i,

represents vertical position of the centre of mass of link i (assuming that the gravity is
directed downward along the vertical axis of the inertial frame).
Kinetic energy is defined as:

((

(2.9)
The moment of inertia about the centre of mass of link i is denoted by

.

2.2.2. Kinetic and potential energy of N links in the case of pinned open kinematic chains
In the case of N links, one could simply sum up the Eq. (2.8) and Eq. (2.9) for
links to compute the total potential and the total kinetic energy.
Given the set of generalized coordinates,

one should note the

following, with abusing the notation:

*

+ *

+
(2.10)
And the velocities are computed, via the chain rule

16


*

+ (

+)

(2.11)
The first ingredient required to calculate the Lagrangian is the total potential energy of
the robot. Calculation of the potential energy is considerably less complicated than calculation
of the kinetic energy.
Substituting Eq. (2.10) into Eq. (2.8), the total potential energy results:

(2.12)
Where

denotes the total mass of the biped,

is the gravitational constant

represents vertical position of the centre of mass of the structure.

Similarly, if substituting Eq. (2.11) into Eq. (2.9), the total kinetic energy results:

((

(2.13)

denotes the moment of inertia about the centre of mass of link i. Total kinetic energy is:

(2.14)
The total kinetic energy is always a positive definite, quadratic function of the
generalized velocities, and can be written as:

(2.15)
Where

is and positive definite for each

, and it is called the mass-

inertia matrix.

2.2.3. Lagranges equations
So that in mind, one could develop the modelling of biped robots. In order to obtain
the dynamic model the Lagrange method is used.
The Lagrangian for an N-link, rigid body open-chain robot with N, one-DOF revolute
joints is a functional acting on points in the state space, where Q is a
is a simply connected. The generalized coordinates give the robots shape, orientation,
and position in three-dimensional space.
The Lagrangian is the real-valued function given by the total kinetic
energy minus the total potential energy,


(2.16)
From Hamiltons principle, the equations of motion can be calculated directly from the
Lagrangian as:

(2.17)
Where are joint torques and other non-conservative forces affecting the i
th

generalized coordinate.

17

If the kinetic energy is quadratic, then Eq. (2.17) becomes a second-order differential
equation, which is a general equation:


(2.18)
It is denoted

as the gravity vector; and the Coriolis matrix,

namely (

; D the inertia matrix; and least but

not last denote the vector of generalized torques and forces.
The matrix function C not uniquely defined but it is traditional to choose

(2.19)
Where

and

is the entry of the matrix C.

2.3. Dynamic model of walking: swing phase
This and the next section develop a mathematical model for the study of a walking gait
of a biped satisfying Robot Hypotheses HR1HR5, Gait Hypotheses HGW1 HGW7, and
Impact Hypotheses HI1HI7. An inertial reference frame is assumed to be given and oriented
in the standard manner with respect to gravity. From Hypothesis HGW7, the walking surface
is flat, and thus it can be assumed without loss of generality that the ground height is zero
with respect to the inertial frame.
The swing phase model corresponds to a pinned open kinematic chain. Since by
Hypothesis HGW5, the gait is assumed to be symmetric, it does not matter which leg end is
pinned, so assume it is leg-1. The swapping of the roles of leg-1 and leg-2 will be accounted
for in the impact model of the next section.
During the swing phase of the motion, the stance leg is acting as a pivot, and thus
there are only three degrees of freedom. The definition of the angular coordinates and the
disposition of the masses of the legs, hips and torso are indicated in Figure 2.4. In particular,
note that all masses are lumped, and positive angles are computed clockwise with respect to
the indicated vertical lines. Two torques,

and

are applied between the torso and the

stance leg, and the torso and the swing leg, respectively. The dynamic model of the robot
between successive impacts is easily derived using the method of Lagrange, introduced
already. This results in a standard second order system:

(2.20)
Eq. (2.20) is rewritten in state space form:
[

]
] (2.21)
A normalized form is given, and

.

(2.22)
The input torque is denoted by

. A normalized form is given, and

.

18


Figure 2.4. The model is indicated in: a) Generalized coordinates b) Body coordinates
It is clear that not all configurations of the model are compatible with our notion of the
single support phase of walking. For example, with the exception of the end of the stance leg,
all points of the robot should be above the walking surface, and for human-like walking, the
knees should not hyperextend. In addition, there are kinetic constraints, such as, for the leg
end to act like a pivot, the forces on the leg end must lie in the static friction cone, and the
normal component of the reaction force must be positive.
Consider

as absolute orientations of the various links

parameterizes the stance leg,

the swing leg and

the torso as in Figure 2.4.

The matrices

inertia matrix,

Coriolis matrix,

gravity matrix and

which maps the joint torques to generalized forces are given in below in Eq. (2.23), Eq.
(2.24), Eq. (2.25) and Eq. (2.26):

(2.23)

(2.24)

(2.25)

[



]
(2.26)


19

2.4. Dynamic model of walking: impact model
The impact between the swing leg and the ground is modelled as a contact between
two rigid bodies. The standard model from [24] is used. The motion of the robot is only
analysed for the case that the contact of the swing leg with the ground results in no rebound
and no slipping of the swing leg and the stance leg naturally lifting from the ground without
interaction [21]. The conditions for these assumptions to be valid will be indicated.
The contact model requires the full five degrees of freedom of the robot. Let

be the
generalized coordinates used in the single support model and complete these to a set of
generalized coordinates for the unpinned model by letting

be the Cartesian
coordinates of some fixed point on the robot or its centre of mass. Using the generalized
coordinates

the method of Lagrange results in:

(2.27)
The external forces acting on the robot at the contact point(s) is represented by

.
The basic premises in [24] are that:
the impact takes place over an infinitesimally small period of time;
the external forces during the impact can be represented by impulses;
impulsive forces may result in an instantaneous change in the velocities of the
generalized coordinates, but the positions remain continuous;
the torques supplied by the actuators are not impulsional.
With these assumptions, Eq. (2.27) is "integrated" over the "duration" of the impact to
obtain:

(2.28)
Where

, is the result of integrating the contact impulse over the

impact duration,

is the velocity just after the impact and

is the velocity just before the

impact. Since the positions do not change during the impact

, as is affirmed by
Hypothesis HI7.
By definition, the velocity just before impact is determined from the single support
model. During the single support phase,

the Cartesian coordinate added to the robots body,

can be determined from

; denote this by

. Thus:

]
(2.29)
From Hypothesis HI4,

is the reaction force at the end of the swing leg, which is

leg two. Letting

denote the position of the end of the swing leg with respect to the
inertial frame, it follows from the principle of virtual work that

(2.30)
Where,

and

and

are the tangent and

normal forces, respectively, applied at the end of the swing leg. Eq. (2.24) represents (N + 2)
equations and (N + 4) unknowns; the unknowns are

and

. In order to be able to
solve for all of the unknowns, the above equations must be augmented with additional
equations that proscribe what happens at the two contact ends. Since the stance leg is assumed
to detach from the ground without interaction, the external forces acting at the pivot point are
zero. The two additional required equations come from the no slip and rebound condition of
Hypothesis HI3:

20

(2.31)
The impact equations Eq. (2.28) and Eq. (2.31), taken together, become

[

] *

+ *

+
(2.32)
Where,

and the positive definite, symmetric matrix

(2.33)
This has the entries as in [20]:

The solvability of Eq. (2.32) is equivalent to the invertibility of the matrix on the left
hand side. The invertibility of this matrix follows from the fact that D
e
is positive definite and
E has full rank; indeed, the determinant of the left hand side of Eq. (2.28) can be computed to
be

This is non-zero everywhere.
The result of solving Eq. (2.28) and Eq. (2.31) yields an expression for

in term
of

, which should then be used to re-initialize the model Eq. (2.25). The coordinates must
be relabelled because the roles of the legs must be swapped: the former swing leg is now in
contact with the ground and is poised to take on the role of the stance leg. The result of the
impact and the relabeling of the states is then an expression

(2.34)

21

Where

expressed in terms of

, this is the state value just

after impact respectively before impact.
The mapping is then evaluated by the following steps:
Step 1: solve Eq. (2.32) for

, and pick-off

; since

only depends on

and since the

positions do not change during the impact, the result is

expressed as a function of

.
Step 2: transform the coordinates so that

(generalized coordinate, as in Figure 2.4)

corresponds to the stance leg and

to the swing leg; this means swapping the first two

position coordinates, and the first two velocities, respectively. The final result is

(2.35)
In conclusion there are some remarks: (a) Computing in closed form would mean
inverting a matrix; hence this is only done numerically, (b) The no-rebound, no-slip
condition of the impact, ensures that the impact results in the end of the swing leg being at
rest, and hence, after doing the coordinate transformation, the end of the stance leg will be at
rest, (c) For the impact model to be valid, it must be verified a posteriori that no-slipping
was a valid assumption (that is |

| ), and that the stance leg lifts from the ground

without interaction.

2.5. Hybrid model
The overall biped model can now be expressed as a system with impulse effects.
Assume that the system trajectories possess finite left and right limits, and denote them by

{

(2.36)
The switching set is chosen to be {

} which
is considered in our specific case

, where

is the desired
stance leg angle. In simple words, a trajectory of the robot is specified by the mechanical
model until an impact occurs. Impact occurs when the state "attains" the set S , which
represents the walking surface. In order for the swing leg end to be at ground level at the end
of the step, it must be the case that:

At this point, the impact with the surface results in a very rapid change in the velocity
components of the state vector. The impulse model of the impact compresses the impact event
into an instantaneous moment in time, resulting in a discontinuity in the velocities. The
ultimate result of the impact model is a new initial condition from which the mechanical
model evolves until the next impact. In order for the state not to be obliged to take on two
values at the "impact time", the impact event is, roughly speaking, described in terms of the
values of the state "just prior to impact" at time

and "just after impact" at time

. These
values are represented by the left and right limits,

and

respectively.
A step of the robot is a solution of Eq. (2.35) that starts with the robot in double
support, ends in double support with the configurations of the legs swapped, and contains
only one impact event. Walking is a sequence of steps.

22


Figure 2.5. Hybrid model of walking with point feet
The above figures key elements are the continuous dynamics of the single support
phase, written in state space form as

, the switching or impact condition

which detects when the height of the swing leg above the walking surface is zero and the
swing leg is in front of the stance leg, and the re-initialization rule coming from the impact
map, .

2.6. Numerical example
Consider the model Eq. (2.35), with the following values of the parameters:

Parameter Notation Units Value
Torso length l m 0.5
Leg length r m 1.0
Torso mass M
T
kg 10
Hip mass M
H
kg 15
Leg mass m kg 5
Acceleration due
to the gravity
g m/s
2
9.81
Table 1 Biped parameters
The above values corresponding to the mass of the legs, the mass of the hips, the mass
of the torso, the length of the legs and the distance between the centre of mass of the hips and
the centre of mass of the torso.

2.7. Stability framework
The most crucial problem concerning the dynamics of bipedal robots is their stability.
As has been explained in the present chapter, a biped is far from being a simple set of
(controlled) differential equations.
Moreover, the objectives of walking are quite specific. One is therefore led to first
answer the question: what is a stable biped? And, consequently, what mathematical
characterization of this stability can be constructed from the complementarity models? This is
closely related to the fact that bipeds can be considered as hybrid dynamical systems, the
stability of which can be attacked from various angles. The goal of this section is to present in

23

general case some tools which can serve for the stability analysis of models. One should note
that this thesis does not have the aim to discuss this in detail.
Firstly, is intended to spend some time on describing invariant sets for
complementarity systems. The point of view that is put forth is that various existing, or to be
investigated, stability frameworks are better understood when invariant sets are classified.
Secondly, it is reviewed the so-called impact Poincar maps, which have been used
extensively in the applied mathematics and mechanical engineering literature for vibro-impact
systems.
The control torques has to obey certain conditions to ensure sticking of the contacting
feet at all times. This can called constraints, that guarantee foot sticking and stability margins.
These conditions can be written as
(2.37)
The detailed calculations of inequalities (22) and (23) can be found in Gnot,
Brogliato, and Hurmuzlu work. The inequality in Eq. (2.36) is necessary and sufficient
condition to be satisfied by the control input u so that during the whole motion (smooth and
non-smooth) sticking is maintained and detachment is monitored. The inequality in Eq. (2.36)
can be rewritten as and can be used to compute stability margins (Wieber, 2002).
The notion of stability margin has been introduced correctly in Seo and Yoon (1995), who
formulated a set of constraints. The distance from the trajectory to the constraint boundary
is defined as the maximal magnitude of a disturbance that is applied on the biped. Stability
margins help understanding the deference between static gait (centre of gravity located within
its base of support), and dynamic gait (centre of gravity may fall outside the support base)
(Vaughan, 2003). To the best of our knowledge, none of the control laws proposed in the
literature until now was shown to be stable with respect to these fundamental conditions,
mainly due to the fact that the underlying dynamics of the robot does not capture the
unilateral feature of the feet-ground contacts.

2.8. Poincar maps and stability
Generally, the approach to the stability analysis takes into account two facts about
bipedal locomotion: the motion is discontinuous because of the impact of the limbs with the
walking surface [30], and the dynamics is highly nonlinear and non-smooth and linearization
about vertical stance generally should be avoided (Vukobratovic et al., 1990; Hurmuzlu,
1993; Grizzle, Abba & Plestan, 1999). A classical technique to analyse dynamical systems is
that of Poincar maps. On a the three-link bipedal model of Section 2, Grizzle et al. showed
that periodic motions of a simple biped can be represented as closed orbits in the phase space.
Poincar return map obtained from the points of the trajectory that coincide with the instant of
heel strike. A Poincar map for a generalized coordinate (which is typically a joint angle in
bipedal systems) at the instant of heel strike now can be obtained by plotting the values of a
generic coordinates at i
th
versus the values at (i + 1)
th
heel strike. One can choose an event
such as the mere occurrence of heel strike, to define the Poincar section (Hurmuzlu &
Moskowitz, 1987; Kuo, 1999). It can be constructed several mappings depending on the type
of motion. In general, however, the section can be written as follows:

(2.38)
This condition establishes the Poincar section. The discrete map obtained by
following the procedure described above can be written in the following general form:

(2.39)

24

Note that is a reduced dimension state vector, and the subscripts denote the i
th
and (i
1)
th
return values, respectively. Periodic motions of the biped correspond to the 6xed points
of P where

(2.40)
Where

is the k
th
iterating. The stability of

reflects the stability of the

corresponding flow. The fixed point

is said to be stable when the eigenvalues

, of the
linearized map,

(2.41)
Have moduli less than one. This technique employed by numerous researchers has
several advantages. Using this approach the stability of gait conforms to the formal stability
definition accepted in nonlinear mechanics. The eigenvalues of the linearized map provide
quantitative measures of the stability of bipedal gait. Finally, to apply the analysis to
locomotion one only requires the kinematic data that represent all the relevant degrees of
freedom. No specific knowledge of the internal structure of the system is needed. This feature
also makes it possible to extend the analysis to the study of human gait.


25

Chapter 3. Feedback control of a biped
The method of computed torque, also known as inverse dynamics, is ubiquitous in the
field of robotics. It consists of defining a set of outputs, equal in number to the inputs, and
then designing a feedback controller that asymptotically drives the outputs to zero.

3.1. MIMO system
In the following the Multiple Input Multiple Output system will be introduced, outputs
will be defined and later on the controlling factors will be chosen.
Many real world systems are non-linear. And more often they are Multiple Input-
Multiple Output based systems, which one can run up against with daily basis, even though
the person did not realise.
MIMO systems are typically more complex than SISO systems. The interactions of the
dynamics make simple linear predictions of system performance difficult. For MIMO systems
the SISO frequency domain techniques require some adjustment.
SISO techniques exist for transfer functions and state equations. The same is true for
MIMO. However, most of the effort and focus is on state equations for MIMO and transfer
functions for SISO.
However, the focus in MIMO control system theory appears to be on optimal and
robust controls which involve designing a controller that minimizes a cost function. An
example cost function would energy required for control (in our specific case).
Often these optimal and robust techniques require full observability of all states and so
a state estimator is required. The most common state estimator is the Kalman Filter, which is
used also by Matlabs Model Predictive Control Toolbox. The Kalman filter produces
estimates of the true values of measurements and their associated calculated values by
predicting a value, estimating the uncertainty of the predicted value, and computing a
weighted average of the predicted value and the measured value.
The Multi Input Multi Output system of the biped robot model can be seen at Figure
3.1 . Two torques are applied between the legs and the torso and the outputs will be defined as
follows in the next paragraphs.

Figure 3.1. High level view of the present MIMO system

3.2. The controlled variables
At the most basic level, walking consists of two things: posture control, that is,
maintaining the torso in a semi-erect position, and swing leg advancement, that is, causing the
swing leg to come from behind the stance leg, pass it by a certain amount, and prepare for
contact with the ground. This motivates the direct control of the angles

(describing the
torso) and

(describing the swing leg). In a normal walking motion, it is clear that the
horizontal motion of the hips is monotonically strictly increasing. For the three-link walker

strictly increasing at every step of the walking cycle. Thus, for any desired trajectories

and

that express a desired walking pattern for the biped, it is therefore reasonable

26

to assume that the corresponding trajectory for

has the property that

is strictly
monotonic.
The simplest version of posture control is to maintain the angle of the torso at some
constant value, say

, while the simplest version of swing leg advancement is to command

the swing leg to behave as the mirror image of the stance leg, that is,

. Thus the
behavior of walking can be encoded into the dynamics of the robot by defining outputs with
the control objective being to drive the outputs to zero as it can be seen in Eq. (3.1). Driving y
to zero will force

and

to converge to known functions of

(here,

being a constant).

*

+ [

] [

] (3.1)
With the outputs defined as in Eq. (3.1), suppose that the desired inclination angle of
the torso is

and that impact occurs with the walking surface when

, as it
is assumed in the simulation.
The virtual constraints selected in Eq. (3.1) have the advantage of being simple and
intuitive. They do not, however, provide very much design freedom. The only parameter that
may be varied is the torso lean angle, which can be used to vary walking speed to a certain
extent, but there is no possibility of minimizing torque requirements for a given walking
speed, for example. For this reason, [30] considers a set of outputs of the form

*

+ [

]
(3.2)
Where

(3.3)

(3.4)
The rather particular form of

was arrived at by imposing that

) 1, which is the condition needed for the swing leg end to have height zero
at impact. A gradient descent algorithm was used to minimize the cost function, initialized at
values of the parameters for which the new outputs were equivalent to the original outputs
with

; see Table 2. The process of minimizing the integral of squared torque also
fortuitously reduced the peak torque magnitude [30].

Original values

Optimized values

Table 2 Optimizing the virtual constraints for minimal energy consumption

In the following sections it will be introduced the feedback control principle, just to
have a rough idea of it. This section identifies the swing phase zero dynamics for a particular
class of outputs that has proven useful in constructing feedback controllers for bipedal
walkers. The purpose of this is that the preliminary study phase of the biped robot
locomotion, based on the work of Grizzle et al. [20].


27

3.3. The zero dynamics of walking
A geometric task for the robot may be encoded into a set of outputs in such a way that
the zeroing of the outputs is asymptotically equivalent to achieving the task, whether the task
is asymptotic convergence to an equilibrium point, a surface, or a time trajectory. For a
system modelled by ordinary differential equations (in particular, without impact dynamics),
the maximal internal dynamics of the system that is compatible with the output being
identically zero is called the zero dynamics. Hence, the method of computed torque can be
seen as an indirect means of designing a set of zero dynamics for the robot.
Since, in general, the dimension of the zero dynamics is considerably less than the
dimension of the model itself, the task to be achieved by the robot is implicitly encoded into a
lower-dimensional system.
One of the main points of [31] is that this process can be explicitly exploited in the
design of feedback controllers for walking mechanisms, even in the presence of impacts.
This following section identifies the swing phase zero dynamics for a particular class
of outputs that have proven useful in constructing feedback controllers for bipedal walkers
Since no impact dynamics are involved, the work here is simply a specialization of the
general results. The results summarized here will form the basis for defining a zero dynamics
of the complete hybrid model of the planar biped walker, which is the desired. Note that if an
output depends only on the configuration variables, then, due to the second order
nature of the robot model, the derivative of the output along solutions does not depend
directly on the inputs. Hence its relative degree is at least two. Differentiating the output once
again computes the accelerations, resulting in

(3.5)
The matrix

is called the decoupling matrix and depends only on the

configuration variables. A consequence of the general results is that the invertibility of this
matrix at a given point assures the existence and uniqueness of the zero dynamics in a
neighbourhood of that point. With a few extra hypotheses, these properties can be assured on
a given open set.
As in their paper, [31], the swing phase zero dynamics lemma is defined as follows.
Suppose that a smooth function h is selected so that
HH1) is a function of only the configuration coordinates;
HH2) there exist an open set

such that for each point

, the decoupling
matrix

is square and invertible;

HH3) there exist a smooth real valued function such that (

is a diffeomorphism onto its image;
HH4) there exists at least one point in

where vanishes.
Then,
1. {

} is a smooth two dimensional submanifold of

and
2. The feedback control

renders invariant under the

swing dynamics; that is, for every

. is
called the zero dynamics manifold and is called the zero dynamics.
From hypotheses HH1 and HH3, is a valid coordinate transformation on

and thus

(3.6)

28

(3.7)
Is a coordinate transformation on

. In these coordinates, the system takes the form

(3.8)

(3.9)
Where is evaluated at

(3.10)

(

+
(3.11)
Enforcing results in (

), and the zero dynamics becoming

(3.12)
While it is useful to know that the zero dynamics can be expressed as a second order
system, this form of the equations is very difficult to compute directly due to the need to
invert the decoupling matrix. However, this can be avoided. In a neighbourhood of any point
where the decoupling matrix is invertible, there exists a smooth scalar function such that

(3.13)
Is a valid coordinate transformation and

.
In the coordinate of Eq. (3.13), the zero dynamics become

(3.14)
Where there right hand side is evaluated at

(3.15)

(

]
(3.16)

3.4. Computed torque with finite-time feedback control:
Bernstein-Bhat controller
Since no impact dynamics are involved, the work here is simply a specialization of the
general results to a model of the form in Eq. (2.21) and an output that is independent of the
velocity. The results summarized here will form the basis for defining a zero dynamics of the
complete hybrid model of a planar bipedal walker.
The period of a step is finite, so controller should settle the system in finite time. The
control objective is considered as driving the outputs to zero. Since the outputs only depend
on the generalized positions, and the dynamical model is second order the relative degree of
each output component is at least two. Note that an output is of the form:

29

The derivative of the output along solutions of (5.29) does not depend directly on the inputs:

(3.17)



[

] [[

[ ]
]

]
(3.18)

[

] [

[ ]
]

] *

(3.19)

(3.20)
Because

is zero. Hence, the relative degree of the output is at least two.

Differentiating the output once again computes the accelerations, resulting in

] *[

[ ]
] *

+ +
(3.21)

[

] [

[ ]
]

(3.22)

(3.23)
In which was used standard Lie derivative notation, as the final calculation yields:

(3.24)
The matrix

is called the decoupling matrix. This is an important one, mainly

because it decouples the MIMO system with two inputs and two outputs into two SISO
systems, with one input, one output, each. It depends only on the configuration variables. The
consequence of it is that the matrix is always reversible. When the input is:

(

(3.25)
The nonlinear system can be linearized as a double integral system and the controller
will be referred later on as a Bernstein-Bhat controller. The feedback functions are:

[

]
(3.26)

(3.27)
Where

satisfies the following:

v is continuous;
the origin in closed-loop with Eq. (3.15) is globally finite-time stable;
the settling time function (

) depends continuously on the initial condition.

30

It is used, with and . The parameter allows the settling time of
the controller to be adjusted. With this feedback, CH2-CH5 holds. In the impact model, it is
supposed that the friction coefficient . In the course of the simulations, it has been
verified that the impact model is valid, so this point will not be discussed further.
As the feedback control was presented briefly in a few steps, now lets move on to
introduce the input-output linearizing controller.

3.5. Computed torque with linear feedback control
Suppose that the decoupling matrix

is invertible. Let

and

be
positive definite matrices and let be a positive scalar tuning
parameter, as in [21]. Then the feedback is:

)
(3.28)
Eq. (3.16) applied to the swing phase portion of (2.35) results in:

(3.29)
The solutions of Eq. (3.28) converge exponentially to zero. In bipedal walking, the
impact map tends to increase the norm of at each impact. The parameter provides control
over the speed with which and converge to zero during the continuous phase, so
that, over a cycle consisting of an impact event followed by a swing phase, the contraction
taking place in the swing phase dominates the expansion coming from the impact. In this way,
the solution of the closed-loop system may converge to the hybrid zero dynamics, and hence
to an exponentially stable periodic orbit of the hybrid zero dynamics, as is defined more
rigorously in [21].

3.6. Comparison of the two controllers
Below one can find a graphical representation of the inputs and the outputs of the
MIMO system controlled through the two controllers introduced already, proportional and
derivative gains were tuned in order to achieve better performance (smaller settling time and
less overshoot).
As it can be seen (in Figure 3.2 respectively Figure 3.3) two torques are applied on the
system resulting two positions (Figure 3.4), measured in radian (y
1
, y
2
). Y1_BB refers to the
first output generated over three steps by the Bernstein-Bhat controller, similarly Y2_BB.
Y1_FL is denoted the first response of the system due to de feedback linearized controller.
Refer to Y2_FL in the same way.
The aim of this preliminary study was a better understanding of the nonlinear system
and to conclude a few things. Firstly as it can be clearly seen that after a rigorous tuning of the

parameters from Eq. (3.9) the nonlinear system has a better behaviour as the original
Bernstein-Bhat controller. In this specific case as shown in Figure 3.4 the derivative and
proportional gains are

respectively

.
Secondly the simulation of the biped results in a much smoother walking cycle. The
stability was fulfilled in both cases.

31


Figure 3.2. The inputs applied in the case of Bernstein Bhat controller

Figure 3.3. The inputs applied in the case of the feedback linearized controller
There is another representation of the outputs (with the same controllers), but this time
it is illustrated during five walking steps (Figure 3.5). It is shown the settling time positions,
in order to be able to calculate it. In these cases of the classical controls the settling time of
the outputs was around 30% respectively 34% of one step duration.

0 0.5 1 1.5 2 2.5 3 3.5
-60
-50
-40
-30
-20
-10
0

1

(
N
m
)
Control Signals
0 0.5 1 1.5 2 2.5 3 3.5
-50
-40
-30
-20
-10
0
10
20

2

(
N
m
)
time (sec)
0 0.5 1 1.5 2 2.5 3 3.5
-60
-50
-40
-30
-20
-10
0

1

(
N
m
)
Control Signals
0 0.5 1 1.5 2 2.5 3 3.5
-250
-200
-150
-100
-50
0
50

2

(
N
m
)
time (sec)

32


Figure 3.4. Output of the MIMO system during three steps

Figure 3.5. Output of the MIMO system during five steps

0 500 1000 1500 2000 2500 3000
-0.05
-0.04
-0.03
-0.02
-0.01
0
0.01
0.02
0.03


X: 114
Y: -0.001411 Y
1
t
X: 520.2
Y: 7.939e-009
X: 180
Y: 0.0002322
Y1_BB
Y1_PD
0 500 1000 1500 2000 2500 3000
-0.05
-0.04
-0.03
-0.02
-0.01
0
0.01


X: 524.5
Y: -0.0001659
Y
1
t
X: 204.3
Y: -4.595e-005
X: 158.2
Y: -0.0003146
Y2_BB
Y2_PD

33

Chapter 4. Model predictive control of nonlinear plant
Model Predictive Control, or MPC, is an advanced method of process control that has
been in use in the process industries since the 1980s. Model predictive controllers rely on
dynamic models of the process, most often linear empirical models obtained by system
identification.
Despite the fact that most real processes are approximately linear within only a limited
operating window, linear MPC approaches are used in the majority of applications with the
feedback mechanism of the MPC compensating for prediction errors due to structural
mismatch between the model and the process. In model predictive controllers that consist only
of linear models, the superposition principle of linear algebra enables the effect of changes in
multiple independent variables to be added together to predict the response of the dependent
variables. This simplifies the control problem to a series of direct matrix algebra calculations
that are fast and robust.
MPC will be applied to the nonlinear mathematical model, but not before linearization.
The three link biped robot model will be used for simulation purposes, introduced in Chapter
2, in order to maintain his torso at some constant angle.

4.1. Introduction
Model Based Predictive Control (MBPC) is a control methodology developed around
certain common key principles. Two of these principles are:
Explicit on-line use of a process model to forecast the process output at future time
instants;
Calculation of an optimal control action based on the minimization of one or more
cost functions, possibly including constraints on the process variables.

The various algorithms, members of the large MBPC-family, differ mainly in:
The type of model used to represent the process and its disturbances,
The cost function(s) to be minimized, with or without constraints.

The MBPC-strategy is simple to understand and makes good practical sense:
Use the process model to predict the evolution of the process output as a function of
future (intended) control actions and
Minimize (over these control actions) a specified cost index; this cost includes the
errors between desired and predicted process outputs, and possibly also the required
control effort.

Model Predictive Control (MPC) possesses many attributes which makes it a
successful approach to industrial control design:
Simplicity: the basic ideas of MPC do not require complex mathematics and are
intuitive.
Richness: all of the basic MPC components can be tailored to the details of the
problem in hand.
Practicality: it is often the resolution of problems such as satisfying control- or output
constraints, which determines the utility of a controller.
Demonstrability: it works, as shown by many real applications in industry where MPC
is routinely and profitably employed.

34

At present MPC is the most widely used multivariable control algorithm in the process
industry. While MPC is suitable for almost any kind of problem, it displays its main strength
when applied to problems with:
A large number of manipulated and control variables.
Constraints imposed on both the manipulated and controlled variables.
Changing control objective and/or equipment (sensor/actuator) failure.
Time delays.
Industrial developments in Europe (MAC: Richalet 1978) and in the USA (DMC:
Cutler 1980) resulted around 1980 in the first commercial control packages using explicitly a
process model to predict and to control the process variables. Simultaneously (although
independently) some European academic research groups, with a strong history in adaptive
prediction and control, started the development of controllers based on multistep predictors
(EPSAC: De Keyser 1981; EHAC Ydstie 1984; MUSMAR: Mosca 1984; GPC: Clarke 1987)
as stated in [33].
These names are only stated to be indicative and represent a few of the (earlier) MBPC
algorithms. Since the beginning of the 1990s, a real boom in the number of industrial MBPC
applications has been reported (first in the USA and Japan, later also in Europe), in parallel
with a dramatic increase of the academic research activity on MBPC-related topics.
Today, commercial MPC packages typically contain tools for model identification and
analysis, controller design and tuning, as well as controller performance evaluation. The
commercially available packages include:
FLSmidth Automation ECS/Process Expert for Cement and Mineral Applications
Connoisseur control and identification package (Invensys)
INCA (linear, non-linear, Batch) from IPCOS
Pavilion8 (Pavilion Technologies)
ADMC & DMCX1 (both Cutlertech)
DMC Plus (Aspen Technology)
RMP(Honeywell)
3dMPC & Expert Optimizer (both ABB)
DeltaV Predict and PredictPRO (Emerson)
APC Library (Siemens|PCS 7)
MACS (Capstone Technology)
eMPC (eposC) and Control Station's LOOP-PRO
ControlMV, PharmaMV and WaterMV from Perceptive Engineering
MATLAB Model Predictive Control Toolbox
Prime (RandControls)

4.2. MPC principle
Model Predictive Control is a form of control in which the current control action is
obtained by solving on-line, at each sampling instant, a finite horizon open-loop optimal
control problem, using the current state of the plant as the initial state; the optimization yields
an optimal control sequence and the first control in this sequence is applied to the plant.
At time the current plant state is sampled and a cost minimizing control strategy is
computed (via a numerical minimization algorithm) for a relatively short time horizon in the
future:[ ] Specifically, an online or on-the-fly calculation is used to explore state
trajectories that emanate from the current state and find (via the solution of Euler-Lagrange
equations) a cost-minimizing control strategy until time . Only the first step of the
control strategy is implemented, then the plant state is sampled again and the calculations are

35

repeated starting from the now current state, yielding a new control and new predicted state
path. One can see the principle of Model Predictive Control in Figure 4.1.


Figure 4.1. MPC principle
Referring to Figure 4.1, the MBPC principle is characterized by the following strategy,
as in [33]:
At each current moment t (whi ch is k on the above fi gure) , the process output
is predicted over a time horizon. The value is called the prediction horizon. The
prediction is done by means of a model of the process; it is assumed that this model is
available. The forecast depends on the past inputs and outputs, but also on the future
control scenario |(i.e. the control actions that is intended to apply from the
present moment on);
A reference trajectory |, starting at | and evolving towards
the set point, is defined over the prediction horizon, describing how it is wanted to
guide the process output from its current value to its set point; in case the process has
a time-delay (dead-time), it is reasonable to start the reference trajectory after the time-
delay;
The control vector |is calculated in order to minimize a specified cost
function, depending on the predicted control errors | |;
Also, in most methods there is some structuring of the future control law
|and there might also be constraints on the process variables;
The 1
st
element | of the optimal control vector | is actually applied to
the real process. All other elements of the calculated control vector can be forgotten,
because at the next sampling instant all time-sequences are shifted, a new output
measurement is obtained and the whole procedure is repeated;
This leads to a new control input | , which is generally different from the
previously calculated |; this approach is called the receding horizon principle.

In above strategy, some important elements characterizing MBPC can be recognized:
prediction by means of a process model;
specification of a reference trajectory;
structuring of the future (postulated) control law;
definition of a cost function (and constraints);
calculation of the optimizing control scenario.

36



Figure 4.2. MPC strategy as a block scheme
The prediction horizon keeps being shifted forward and for this reason MPC is also
called receding horizon control. Although this approach is not optimal, in practice it has given
very good results. Much academic research has been done to find fast methods of solution of
Euler-Lagrange type equations, to understand the global stability properties of MPCs local
optimization, and in general to improve the MPC method. To some extent the theoreticians
have been trying to catch up with the control engineers when it comes to MPC.

4.3. Predictive Control of MIMO Systems
For simplicity of illustration the predictive control systems can be designed based on a
Single Input Single Output system. These are not the case here, but in order to have a better
understanding, please refer to [34]. The simple SISO case can be readily extended to Multi
Input Multi Output systems without much additional effort, because of the state-space
formulation.

4.3.1. General Formulation of the Model
Assume that the plant has m inputs, q outputs and n1 states. It is also assumed that the
number of outputs is less than or equal to the number of inputs (i.e., q m). If the number of
outputs is greater than the number of inputs, one cannot hope to control each of the measured
outputs independently with zero steady-state errors. In the general formulation of the
predictive control problem, one can also take the plant noise and disturbance into
consideration.

(4.1)

(4.2)
Where is the input disturbance, assumed to be a sequence of integrated white
noise. This means that the input disturbance is related to a zero-mean, white noise
sequence by the difference equation:


(4.3)

37

Note that from Eq. (4.1), the following difference equation is also true:

(4.4)
By defining

and , then subtracting

Eq. (4.4) from Eq. (4.1) leads to

(4.5)
In order to relate the output to the state variable

, it is deduced that

(4.6)
Where .
Choosing a new state variable vector [

:
[

]
[

] [

] [

]
[

]
(4.7)
[

] [

]
(4.8)
Where

is the identity matrix with dimensions, which is the number of

outputs; and

is a zero matrix. In Eq. (4.6),

and

have dimension
, and , respectively.
For notational simplicity, it is denoted Eq. (4.6) by

(4.9)
Where A, B and C are matrices corresponding to the forms given in (1.38). In the
following, the dimensionality of the augmented state-space equation is taken to be
.
There are two points that are worth investigating here. The first is related to the
eigenvalues of the augmented design model. The second point is related to the realization of
the state-space model. Both points will help us understand the model.

4.3.2. Eigenvalues of the augmented model
Note that the characteristic polynomial equation of the augmented model is:
*

(4.10)
Here it is used the property that the determinant of a block lower triangular matrix
equals the product of the determinants of the matrices on the diagonal. Hence, the eigenvalues
of the augmented model are the union of the eigenvalues of the plant model and the
eigenvalue, . This means that there are integrators embedded into the augmented
design model. This is the means it is used to obtain integral action for the MPC systems.





38

4.3.3. Controllability and Observability of the Augmented Model
Because the original plant model is augmented with integrators and the MPC design is
performed on the basis of the augmented state-space model, it is important for control system
design that the augmented model does not become uncontrollable or unobservable,
particularly with respect to the unstable dynamics of the system. Controllability is a pre-
requisite for the predictive control system to achieve the desired closed-loop control
performance and observability is a pre-requisite for a successful design of an observer.
However, the conditions may be relaxed to the requirement of stabilizability and detectability;
if only closed-loop stability is of concern. A system is stabilizable if its uncontrollable modes,
if any, are stable. Its controllable modes may be stable or unstable. A system is detectable, if
its unobservable modes, if any, are stable. Its observable modes may be stable or unstable.
Stable modes here means that the corresponding eigenvalues are strictly inside the unit circle.
Because the augmented model introduced additional integral modes, one needs to
examine under what conditions these additional modes become controllable. The simplest
way for the investigation is based on the assumption of minimal realization of the plant
model. The discussion of minimal realization, controllability and observability can be found
in control textbooks.

4.3.4. Solution of Predictive Control for MI MO Systems
The extension of the predictive control solution is quite straightforward, and one needs
to pay attention to the dimensions of the state, control and output vectors in a MIMO
environment. Define the vectors Y and U as
[

(4.11)
[

(4.12)
Based on the state-space model (A, B, C), the future state variables are calculated
sequentially using the set of future control parameters:

)

(4.13)
With the assumption that

is a zero-mean white noise sequence, the predicted

value of

at future sample is assumed to be zero. The prediction of state

variable and output variable is calculated as the expected values of the respective variables,
hence, the noise effect to the predicted values being zero. For notational simplicity, the
expectation operator is omitted without confusion.
Effectively:

(4.14)
Where

39


[

The incremental optimal control within one optimization window is given by

(4.15)
Where matrix

has dimension

and has dimension

,
and

equals the last q columns of

. The weight matrix

is a block matrix with m

blocks and has its dimension equal to the dimension of T. The set-point signal is

as the q set-point signals to the multi-output system.

Applying the receding horizon control principle, the first m elements in U are taken
to form the incremental optimal control:

(4.16)
Where

and

are, respectively, the identity and zero matrix with dimension .

Further work on predictive control approaches to the particular MIMO system will be
discussed and presented in the following chapters, but not before the discussion of Kalman
filter, which provides observability.

4.3.5. Observability and the Kalman filter
In the design of model predictive controllers, it is assumed that the information

is available at the time

. This assumes that all the state variables are measurable. In

reality, with most applications, not all state variables are measured (or available). Some of
them may be impossible to measure. One approach is to estimate the state variable from
the process measurement. The soft instrument used to estimate unknown state variables
based on process measurement, in a control engineering context, is called an observer. The
concept of an observer has been widely used in the science and engineering fields. In addition,
in a noisy environment, a state observer can also act like a noise filter to reduce the effect of
noise on the measurement. Our focus here is to use an observer in the design of predictive
control.
For a multi-output system, the observer gain vector

can be calculated recursively

using a Kalman filter. Kalman filters are proposed in a stochastic setting. And it is used also
by MATLAB.

40

4.4. Simulink implementation of the swing phase model
This subchapter presents the Simulink blocks created for the biped simulation. The
swing phase dynamics of a three link, planar biped is used as presented in Chapter 2.

4.4.1. Swing phase model
The Eq.(2.20) presented in 2.3 is rewritten in the state space form as
[

]
]
(4.17)
For simplicity it is further modified:
[

]
(4.18)
The model has six states denoted by

. The velocity is denoted by

and the acceleration by

. To obtain the speed and the position from them built in integrators
are used.

Figure 4.3. Simulink implementation of the swing phase model

41

The above block is called nonlinear_swing_model. This block will be an integral part
of the linearized model (4.5) and the control structure (4.6). It is based on Eq. (4.18), which is
the state space representation of the Single Support Phase of the biped model. It has two
inputs (

), representing the two torques applied at the hips, one is applied between one
leg and hip, similarly is the second. The Simulink model has four outputs, represented by

which are the outputs of the swing phase model, and

which are the two

references. These two are used in the MPC block as references. The other subsystems are
presented below.

4.4.2. Swing phase model/D block
The inertia matrix (D) is a three by three matrix, which is presented below:

Figure 4.4. Implementation of the inertia matrix (D) in Simulink
Each column is represented by a sub block, which is defined as:

Figure 4.5. Sub block for a column in D
Each element in a column is represented by a function as it is defined in Eq. (2.22).


Figure 4.6. Fcn1 block parameters

42

4.4.3. Swing phase model/C block
The Coriolis matrix (C) is a three by three matrix, which is presented below:


Figure 4.7. Implementation of the Coriolis matrix (C) in Simulink
Each column is represented by a sub block, which is defined as:


Figure 4.8. Sub block for a column in C
Each element in a column is represented by a function as it is defined in Eq. (2.23).
Below one can find a single element of a matrix.


Figure 4.9. Fcn2 block parameters




43

4.4.4. Swing phase model/I ntegrator blocks
The initial values are set for the integrator blocks are set according to the first state of
the Bernstein-Bhat controller, presented in 3.4
Firstly it is presented the first Integrator block from Figure 4.10. The inputs are the
accelerations, the outputs are the velocities.


Figure 4.10. First integrator block
Below is the Integrator block, which has as input velocities,and as output positions.


Figure 4.11. Second integrator block


44

4.4.5. Swing phase model/Output block
The block presented in this section involves the use of optimisation parameters.
According to [21], are defined the optimised outputs in order to derive them to zero, which is
the basic idea of zero dynamics.


Figure 4.12. Output together with the optimization parameters

4.5. Linearization
In order to be able to apply linear MPC on a nonlinear system, it is a necessarily to
linearize the plant at some operating point.
The linearization around a certain point according to Taylors formula is applied.
Linear approximations for vector functions of a vector variable are obtained in the same way
as in the general case, but the derivative at a point is replaced by the Jacobian matrix. For
example, given a differentiable function with real values, one can approximate
for close to the point

by the formula:


45

(4.19)
But since

is an equilibrium point,

.
Extending this to a system to two equations, for example to this autonomous system

{

(4.20)
Then find the partial derivative which constructs the Jacobian:


[

(4.21)
And the least but not last step is finding the eigenvalues of the Jacobian matrix and
deduce the fate of the solutions around the equilibrium point.
The theory presented seems to be easy, and straightforward in simple cases, but is not
the case here (several equations due to the fact that in it are three by three matrices). It is a
possibility to do it by hand, as you can see in the following equations, but is time consuming.

(4.22)

(4.23)

]
(4.24)
Another way would be: use specific computer programs to do this job for you. It was
chosen MATLAB, and one of the toolboxes provided by the company, namely Simulink
Control Design.

46

Simulink Control Design lets you design and analyse control systems modelled in
Simulink. With this product you can also find operating points and compute exact
linearization of Simulink model at various operating conditions. Simulink Control Design
provides tools for computing simulation-based frequency responses without modifying your
model. A graphical user interface (GUI) lets you design and analyse arbitrary control
structures modelled in Simulink.
In order to be able to linearize with the Simulink Control Design, the model
introduced in Chapter 2, one needs a Simulink model whit the exact representation of the
swing phase, introduced before. There was applied two step input 20 respectively -20 (Figure
4.13).

Figure 4.13. Nonlinear model plant
The operating point was selected at the simulation snapshot time (seconds).
The linearized model is given in state space representation below:

[

+

The same model represented as transfer functions in a decoupled way:
Transfer function from input "u1" to output:

(4.25)

(4.26)
Transfer function from input "u2" to output:

(4.27)

(4.28)

47

The state space model will be later used in the feedback simulation together with the
MPC controller.
Simulink Control Design computes the linearized model and visualizes the results in a
step response plot or Bode diagram. A Linearization Inspector is provided to visualize the
impact of each block in the Simulink model on the linearization. In order to be able to
investigate and analyse the behaviour of the linearized system one could apply a step. In
Figure 4.14 it can be seen the responses. It can be observed that oscillations are not present.
Although in the case of highly nonlinear systems it is expectable.


Figure 4.14. Step response generated

4.6. MPC controller design

Model Predictive Control Toolbox provides MATLAB functions, a GUI, and Simulink
blocks for designing and simulating model predictive controllers in MATLAB and Simulink.
These controllers optimize the performance of MIMO systems that are subject to input and
output constraints.
In the toolbox one can use a linearized Simulink model, or specify it directly as a
linear time invariant object, such as a state space model, which is the present case.
So the first step would be to import the linearized model, then to design a controller
for it, and finally make a pre-simulation, in order to see the response.

Step Response
Time (sec)
A
m
p
l
i
t
u
d
e
0 0.2 0.4 0.6 0.8 1
From: u2 load
0 0.2 0.4 0.6 0.8 1
0
0.1
0.2
0.3
0.4
T
o
:

n
o
n
l
i
n
e
a
r
s
w
i
n
g
m
o
d
e
l
/
2
-1
-0.8
-0.6
-0.4
-0.2
0
From: u1 load
T
o
:

n
o
n
l
i
n
e
a
r
s
w
i
n
g
m
o
d
e
l
/
1

48


Figure 4.15. MPC scheme of the Model Predictive Control Toolbox

The input (torques) and output (positions) signals and their properties are represented
below, as it can be seen in the toolbox:


Figure 4.16. Signal properties in MPC toolbox

In the following section it is presented the controller design in a few steps. One of the
most important parameters of the MPC controller is the control interval, prediction horizon
and control horizon. These parameters can be varied until one has a better performance
through simulations.
The values used in the MPC controller design are the following:
control interval (the sampling time): 0.004 (time units);
prediction horizon : 120 (intervals);
control horizon : 60 (intervals).
The prediction horizon of an MPC needs to be long enough to capture the major
dynamics of the plant. In practice typically the prediction horizon approximately coincides
with the settling time of the system. As it is known from the studied controllers and analysis
in Chapter 3, the simulation duration of one step is 1.128 seconds. And according to Figure
3.4 the settling time of the outputs it is around 0.45 seconds. That is the reason for why the
horizons were chosen in such way (the prediction horizon times control interval equals 0.48).
After choosing these parameters, it is the time to talk about the constraints. The
constraints of the manipulated variables were set as a minimum of -500 and as a maximum of
500, of course after doing some simulations.

49


Figure 4.17. Simulated MPC controller inputs
Constrains of the input variables were chosen also according to the preliminary study
in Chapter 3, as it never goes below -0.5, and above 0.5. To have some more freedom it was
used -1 as minimum, and 1 as maximum.
Least but not last, the weight of the controller is 0.95, which is the ratio between
robustness and faster response. It was tuned according the preliminary simulations which
were taken place in the Model Predictive Control Toolbox.
Preliminary simulations take place in a time interval of 0-1.2, which is approximately
equal with one step duration.
Concluding the fact that simulated MPC is fast enough to keep our system stable, one
can see in Figure 4.17 that the MPC controller behaves normally, meaning that it has a peak
input in the beginning, then reaches a low input then it settles.
In Figure 4.18 one can see the two outputs of the simulated MPC controller, with the
set points: 0.5236 and 0.3927. As it is visible, in order to keep the settling time as low as
possible (around 0.45 seconds) the responses have some considerable low overshoots, the first
approximately 3 % and the second is around 8%.

Figure 4.18. Simulated MPC controller outputs

-100
-50
0
50
100
150
u
1
0 0.2 0.4 0.6 0.8 1 1.2
-30
-20
-10
0
10
20
30
u
2
Plant Inputs
Time (sec)
0
0.2
0.4
0.6
0.8
o
u
t
p
u
t
1
0 0.2 0.4 0.6 0.8 1 1.2
0
0.1
0.2
0.3
0.4
0.5
o
u
t
p
u
t
2
Plant Outputs
Time (sec)

50

4.7. Simulation of the nonlinear model with MPC controller
As it was mentioned before the Matlabs control toolbox was used for linearization.
The hybrid model introduced in Eq. (2.29) was used, more specifically the state space
representation of the swing phase model (SSP), Eq. (2.21), which is implemented in the
noninear_swing_model in Figure 4.19.
The MPC designed in 4.6 was used for simulation. The purpose of the present
simulation is to obtain valid inputs which are introduced later on in a graphical simulation,
which simulates the biped in two dimensions, along with some useful graphs (position and
velocity changes, forces acting on the end of the leg along with the torques applied).
The model introduced in Simulink can be seen in Figure 4.19. In 4.4 is represented
more in detail the noninear_swing_model. Simulink predefined blocks were used to save the
data (inputs, outputs of the system) to the workspace.
When the simulation starts, MATLAB converts the model to discrete time, and also it
is assumed white noise on each measured output channel.


Figure 4.19. Simulink representation of swing phase dynamics together with MPC controller

In order to be able to run the model it is necessary to set some initial parameters (the
length and mass of the leg, mass of the hips and torso, the distance between the hips and torso,
and the acceleration due to gravity are defined according to Table 1) and initial positions and
velocities of the swing phase model defined in Eq. (2.21), as it can be seen in Figure 4.20.
The Simulink has a built in call-back functions, which are a way of customize
Simulink models. A call-back is a function that executes when you perform actions on your
model, such as starting a simulation. One can use call-backs to execute an M-file or other
MATLAB command. InitFcn is called at start of the model simulation.
If the sampling time would be lower, for example 0.0014, the simulation takes about
three hours on a Dell notebook which has Windows 7. It can be said that the computation time
rises when the sampling time is decreased, which is normal. That is why the sampling time
was picked to be 0.004 seconds. In this case the simulation takes about five minutes, which is
more preferable.
The discussion of results and the consequences are discussed in the next subchapter.
Please note that the resulting figures are visible also there.


51


Figure 4.20. Initial call-back function associated with model, with initial parameters

4.8. Discussion of the results
In this particular section will be discussed the results of the model predictive control
applied to the linearized plant. To be able to simulate the walking of a biped in two
dimensions it was necessary a simulation program. In order to be able to produce results in
such a short time, it was used a program package written originally by Eric Westervelt. The
package contains eight m files, mainly functions, which can be used to represent the robot in
the 2D workspace. During the work I carried out, the program package suffered slight
modifications. It can be consulted in Appendix A.

Figure 4.21. The two inputs generated by the simulation, after resampling
0 200 400 600 800 1000 1200
-150
-100
-50
0
50
100
150
Number of inputs
t
o
r
q
u
e


Input1
Input2

52

So, as this was said, lets move on to discuss the obtained results in the simulation.
Firstly, in Figure 4.21, it can be seen the two torque inputs expressed in Nm (Newton-meter).
These inputs were generated by the simulation discussed in 4.7. These vectors were applied as
input to the simulation in the program package, mentioned earlier. Please note that the inputs
were resampled, as it will be debated later.
The resulting behaviour of the three link biped was a stable one step walk. Please note
that in the following all the figures are referring to a single step, from a walk cycle, in order to
be suggestive.

Figure 4.22. Resulting outputs during one step after the 2D simulation
The resulting outputs are on Figure 4.22. Settling time for the first output is around
0.11 second, for the second its 0.28. Which are much better results obtained whit feedback
control discussed in Chapter 3. More specifically one should notice the obvious differences
between Figure 3.4 and Figure 4.22. It is clear that the MPC controlled MIMO system
behaves much better.
On the next picture (Figure 4.23) it is visualised the forces acting on the stance leg.
For calculations it was used Eq. (2.32) introduced in Chapter 2.
One should notice that in the beginning of a step it is clearly visible two peaks, on both
forces, namely the tangential and normal forces. This is due to the fact that at the beginning of
the first step the biped mechanics changes from the double support phase to a single support
phase. And his whole body weight concentrates on only one leg. Moreover to be able to move
a greater torque is applied between the leg and the hip. As it follows the forces acting on the
leg are greater at beginning, as it is visible as peaks.
Following the states of the biped is crucial, more specifically the joint positions and
their velocities. One can observe it in Figure 4.24. The meanings of the notations are taken
from Figure 2.4. Actually

parameterizes the stance leg,

the swing leg and

the torso.
As it is noticeable that the torso position does not vary, which is desired. The position of the
stance and swing leg, namely

and

are complementing each other. Meaning that when

is rising in a specific slope,

decreases with approximately the same slope. Which is not

exactly the case,

has a greater slope. That is due to the fact that it is the position of the
swing leg, and because it advances faster to the next position. This fact is obvious: the
0 0.2 0.4 0.6 0.8 1
-0.04
-0.03
-0.02
-0.01
0
0.01
y
1
Outputs
0 0.2 0.4 0.6 0.8 1
-0.02
-0.015
-0.01
-0.005
0
y
2
time (sec)

53

walking would not be possible with only one rigid link legs if both are advancing at the same
speed (Figure 4.24 second part).

Figure 4.23. Forces calculated on the stance leg

Figure 4.24. Graphical representation of the states (positions and states)
As a conclusion it can be stated that the designed MPC is a better controller. It has the
required stability, much lower settling time, and a perfect response, meaning that during
walking the simulated model maintains his semi erect position in a perfect manner.
0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 1.1
-400
-200
0
200
400
600
800
1000
1200
Forces on End of Stance Leg


F
tan
(N)
F
norm
(N)
0.2 0.4 0.6 0.8 1 1.2
-0.4
-0.2
0
0.2
0.4
0.6
Joint Positions

3
0 0.2 0.4 0.6 0.8 1
-2
-1
0
1
time (sec)
Joint Velocities

1
(dot)

2
(dot)

3
(dot)

54

Least but not last the biped simulator package is presented in a few world. The
MATLAB code can be consulted in Appendix A. It contains of the following: dynamic model
of the biped; the impact model; and finally controllers. The simulator is able to calculate the
positions, the velocities, the force on the stance leg, along with the inputs, and outputs of the
system. It also consist a real time simulation of the biped represented by Figure 4.25.

Figure 4.25. Three link biped representation in the simulator


55

Chapter 5. Conclusions
The present work proposed a model predictive controller to regulate a highly nonlinear
system, namely walking motion of a three link humanoid robot with point feet.
The control problem of the biped was defined as choosing a proper input such that
the system behaves stable with semi erect torso, and two walking legs.
As the preliminary studies revealed in Chapter 3 the problem of these kinds of MIMO
systems are the strong coupling between the inputs and outputs. In order to have a stable
walking biped one needs a master controller, it is not possible to control the system at low
level each joint separately. While the stability was not considered in this work as a main
objective one should not diminish it.
It was tested the classical control (computed torque with feedback linearization) along
with the MPC control. The comparison was made in a biped simulator package. It can be
stated, as a conclusion, that the proposed controller drives the outputs quicker to zero. In the
cases of the classical controls the settling time of the outputs was around 30% respectively
34% of one step duration. With the improved model predictive controller the output settles
within 8.4% (in the case of the first output) respectively 21% (in the case of the second
output). Note that the outputs were defined in Chapter 3.2. It is a major improvement in
controlling a highly nonlinear system which has multiple inputs multiple outputs. The
preliminary studies were necessary in order to compare those types of controllers which exist
till date with the implemented one.
The goal of the work was successfully fulfilled and implemented. Although a three
link biped with point feet was used, the most simple biped model after the compass one, it
was complicated enough to be able to make investigations and research in this direction.
Future work may aim the extension of the tree link model into five one with knee
(point feet), or a seven link model (with regular feet). That would complicate the things;
mainly the mathematical computations will be intricate. So the calculation time would be
much longer on a personal computer. Thats why is advised to use a more powerful computer,
if the present work will be a basis of a research in the humanoid robot controlling topic.



56

References
[1] B. Siciliano, O. Khatib et al., Springer Handbook of Robotics, Springer,
2008.
[2] M. Vukobratovic, B. Borovac, Zero-moment point - Thirty five years of its
life, International Journal on Humanoid Robot, 2004.
[3] T. McGeer, Passive dynamic walking, Int. J. Robot. Res. 9(2), 1990.
[4] C. Chevallereau, G. Abba, Y. Aoustin, F. Plestan, E.R. Westervelt, C.
Canudas-de-Wit and J.W. Grizzle, RABBIT: a testbed for advanced
control theory, IEEE Contr. Syst. Mag., 2003.
[5] K. Hirai, M. Hirose, Y. Haikawa and T. Takenaka, The Development of
Honda Humanoid Robot, Proc. International Conference on Robotics &
Automation, 1998.
[6] A. Takanishi, M. Ishida, Y. Yamazaki, I. Kato, The realization of
dynamic walking by the biped walking robot WL-10RD, Int. Conf. Adv.
Robot, 1985.
[7] M. Hirose, Y. Haikawa, T. Takenaka, K. Hirai, Development of
humanoid robot ASIMO, IEEE/RSJ Int. Conf. Intell. Robots Syst., 2001.
[8] M. Gienger, K. Lffler, F. Pfeiffer, Towards the design of a biped jogging
robot, IEEE Int. Conf. Robot. Autom., 2001.
[9] K. Kaneko, S. Kajita, F. Kanehiro, K. Yokoi, K. Fujiwara, H. Hirukawa,
T. Kawasaki, M. Hirata, T. Isozumi, Design of advanced leg module for
humanoid robotics project of METI, IEEE Int. Conf. Robot. Autom., 2002.
[10] Y. Sugahara, M. Kawase, Y. Mikuriya, T. Hosobata, H. Sunazuka, K.
Hashimoto, H. Lim, A. Takanishi, Support torque reduction mechanism
for biped locomotor with parallel mechanism, IEEE/RSJ Int. Conf. Intell.
Robots Syst., 2004.
[11] I.W. Park, J.Y. Kim, J. Lee, J.H. Oh, Online free walking trajectory
generation for biped humanoid robot KHR-3(HUBO), IEEE Int. Conf.
Robot. Autom., 2006.
[12] M. Vukobratovic, B. Borovac, D. Surla, and D. Stokic, Biped
locomotion, Springer-Verlag, Berlin, 1990.
[13] R. Katoh and M. Mori, Control method of biped locomotion giving
asymptotic stability of trajectory, Automatica, 1984
[14] G. Cabodevilla, G. Abba, and H. Sage, Energetically near optimal gait for
a biped robot with double supporting phases, Proc. of the Third France-
Japan Congress and First Europe-Asia Congress on Mechatronics, 1996.
[15] G. Cabodevilla, N. Chaillet, and G. Abba,Energy-minimized gait for a
biped robot , Autonome Mobile Systeme, Springer-Verlag, 1995.
[16] J. Furusho and M. Masubuchi, Control of a dynamical biped locomotion
system for steady walking, Journal of DynamicSystems, Measurement, and
Control, 1986.
[17] C. Chevallereau, A. Formal'sky, and B. Perrin, Control of a walking
robot with feet following a reference trajectory derived from ballistic
motion, Proc. of the IEEE International Conference on Robotics and
Automation, 1997.

57

[18] C. Canudas, L. Roussel, and A. Goswani, Periodic stabilization of a 1-dof
hopping robot on nonlinear compliant surface, Proc. of IFAC Symposium
on Robot Control, 1997.
[19] B. Thuilot, A. Goswani, and B. Espiau,Bifurcation and chaos in a simple
passive bipedal gait, Proc. of the IEEE International Conference on
Robotics and Automation, 1997.
[20] J. W. Grizzle, G. Abba and F. Plestan, Asymptotically Stable Walking for
Biped Robots: Analysis via Systems with Impulse Effects, IEEE Transactions
on Automatic Control, 2001.
[21] E. R. Westervelt, J. W. Grizzle, C. Chevallereau,J. H. Choi, and B.
Morris, Feedback Control of Dynamic Bipedal Robot Locomotion, CRC
Press, 2007.
[22] K. Berns, The Walking Machine Catalogue. http://www.walking-
machines.org/.
[23] D. H. Sutherland, K. R. Kaufman, and J. R. Moitoza, Kinematics of
normal human walking, Williams and Wilkins, 1994.
[24] Y. Hrmzl and D. B. Marghitu,Rigid body collisions of planar
kinematic chains with multiple contact points, International Journal of
Robotics Research, 1994.
[25] C.-L. Shih and W. A. Gruver,Control of a biped robot in the double
support phase, IEEE Transactions on Systems, Man and Cybernetics, 1992.
[26] Y. Hurmuzlua, F. Gnot and B. Brogliato, Modeling, stabilityand control
of biped robotsa general framework, Automatica, 2004.
[27] R. Findeisen et al., An Introduction to Nonlinear Model Predictive
Control, 21st Benelux Meeting on Systems and Control, 2002 .
[28] A. Bemporad, M. Morari and N. L. Ricker , Model Predictive Control
Toolbox Users Guide, Online only, 2010.
[29] The Mathworks, SIMULINK: Model-Based and System-Based Design,
Online only, 2001.
[30] J. W. Grizzle, G. Abba, and F. Plestan, Proving asymptotic stability of a
walking cycle for a five DOF biped robot model. In Proc. of the 1999 Int.
Conf. on Climbing and Walking Robots, 1999.
[31] E.R. Westervelt, J.W. Grizzle and D.E. Koditschek, Hybrid Zero
Dynamics of Planar Biped Walkers, IEEE Transactions on Automatic
Control, 2001.
[32] Liuping Wang, Model Predictive Control System Design and
implementation using MATLAB, Springer, 2009.
[33] R. De Keyser, A Gentle Approach to Predictive Control, UNESCO
Encyclopaedia of Life Support Systems, 2003.
[34] Rbert MARC, Abhishek DUTTA and Robin DE KEYSER,
Sophisticated humanoid robot control- poster, Dynamical systems, control
and optimization Study Day, 2010.




58

Appendix A: Biped simulator
% A MATLAB script to simulate a three-link, planar biped walker.
%
% This file is associated with the book Feedback Control of Dynamic
% Bipedal Robot Locomotion by Eric R. Westervelt, Jessy W. Grizzle,
% Christine Chevallereau, Jun-Ho Choi, and Benjamin Morris published
% by Taylor & Francis/CRC Press in 2007.
%
% Copyright (c) 2007 by Eric R. Westervelt, Jessy W. Grizzle, Christine
% Chevallereau, Jun-Ho Choi, and Benjamin Morris. This code may be
% freely used for noncommercial ends. If use of this code in part or in
% whole results in publication, proper citation must be included in that
% publication. This code comes with no guarantees or support.
%
% Eric Westervelt
% 20 February 2007
%
% Robert Marc
% 11 July 2010
% Contributions:
% implementing feedback feedback linearization with parameter tuning
% using MPC inputs for simulation
% optimisation for Matlab 2009b

function varargout = walker_main(t,x,flag,opt_param)

if nargin == 0
flag = 'demo';
end

switch flag
case '' % Return dx/dt = dynamics(t,x).
varargout{1} = f(t,x,opt_param);
case 'events' % Return
[value,isterminal,direction].
[varargout{1:3}] = events(t,x);
case 'demo' % Run a demo.
demo;
otherwise
error(['Unknown flag ''' flag '''.']);
end
%% ------------------------------------------------------------------------
--
%% This is the system dynamics function

function dx = f(t,x,a)

global t_2 torque y force

[D,C,G,B,~,~,~,~,~,H,LfH,dLfH] = dynamics_three_link(x(1:6),a);
Fx = D\(-C*x(4:6)-G);
Gx = D\B;
%
% % Bernstein-Bhat controller (uses feedback linearization)
% v = control_three_link(H,LfH);
%
% % Used for controller that use feedback linearization
% u = (dLfH*[zeros(3,2);Gx])\(v-dLfH*[x(4:6);Fx]);


59

% Used for controller with Linear Feedback Control(P,PD)
epsi=0.1;

% feedback linearization only with kp gain
% kp=1.95; %kp=1.5 - unstable; best solution kp=1.95
% KP=kp*eye(2,2);
% u = -(inv(dLfH*[zeros(3,2);Gx])*(dLfH*[x(4:6);Fx]+1/(epsi^2)*KP*H));

%feedback linearization
kd=10; %to be more robust kp=200 kd=25
kp=15; %the best possible solution without overshoot kd=10, kp=15
KD=kd*eye(2,2);
KP=kp*eye(2,2);
u = -
((dLfH*[zeros(3,2);Gx])\(dLfH*[x(4:6);Fx]+1/epsi*KD*LfH+1/(epsi^2)*KP*H));

%loading MPC inputs from Simulink simulation
load('U_good.mat');
if isempty(U)
u = -
((dLfH*[zeros(3,2);Gx])\(dLfH*[x(4:6);Fx]+1/epsi*KD*LfH+1/(epsi^2)*KP*H));
else
temp =U;
u = temp(1,:);
u=u';
temp(1,:) = [];
U = temp;
save('U_good.mat','U');
temp=[];

end

dx(1:3) = x(4:6);
dx(4:6) = Fx+Gx*u;
dx = dx';

torque = [torque ; u.'];
t_2 = [t_2 ; t];
y = [y ; H.'];
[f_tan,f_norm] = stance_force_three_link(x(1:6),dx(1:6),u);
force = [force ; f_tan f_norm];

%% ------------------------------------------------------------------------
--
%% Locate the time when critical angle of stance leg minus stance leg angle
%% passes through zero in a decreasing direction and stop integration.

function [value,isterminal,direction] = events(t,x)

persistent control_call_cnt
if isempty(control_call_cnt) || (t == 0)
control_call_cnt = 0;
else
control_call_cnt = control_call_cnt + 1;
end

%% th1d -- is the switching angle
%
[~,th1d,~,~] = control_params_three_link;
[r,~,~,~,~,~] = model_params_three_link;
value(1) = th1d-x(1); % when stance leg attains angle of th1d

60

value(2) = r*cos(x(1))-0.5*r; % hips get too close to ground--kill
simulation
isterminal = [1,1]; % stop when this event occurs
direction = [-1,-1]; % decreasing direction detection

%% ------------------------------------------------------------------------
--
%% a demo function

function demo

global t_2 torque y force

torque = [];
t_2 = [];
y = [];
force = [];
Xout=[];
tstart = 0;
tfinal = 6;

%% The optimization parameters
%
a = [0.512 0.073 0.035 -0.819 -2.27 3.26 3.11 1.89];

omega_1 = 1.55;
x0 = sigma_three_link(omega_1,a);
x0 = transition_three_link(x0).';
x0 = x0(1:6);

options = odeset('Events','on','Refine',4,'RelTol',10^-5,'AbsTol',10^-6);

tout = tstart;
xout = x0.';
teout = []; xeout = []; ieout = [];

disp('(impact ratio is the ratio of tangential to normal');
disp('forces of the tip of the swing leg at impact)');

for i = 1:1 % run five steps
% Solve until the first terminal event.
[t,x,te,xe,ie] = ode45('walker_main',[tstart tfinal],x0,options,a);

% Accumulate output. tout and xout are passed out as output arguments
nt = length(t);
tout = [tout; t(2:nt)];
Xout = [Xout;x];
%save('x.mat','Xout');
xout = [xout;x(2:nt,:)];
teout = [teout; te]; % Events at tstart are never reported.
xeout = [xeout; xe];
ieout = [ieout; ie];

% Set the new initial conditions (after impact).
x0=transition_three_link(x(nt,:));

% display some useful information to the user
disp(['step: ',num2str(i),', impact ratio: ',num2str(x0(7)/x0(8))])

% Only positions and velocities needed as initial conditions

61

x0=x0(1:6);

tstart = t(nt);
if tstart>=tfinal
break
end
end

disp('by Eric R. Westervelt, Jessy W. Grizzle,');
disp('Christine Chevallereau, Jun-Ho Choi, and Benjamin Morris');


%% Draw some useful graphs
%
fig_hl=figure(2);
set(fig_hl,'Position', [200 100 400 450]);
set(fig_hl,'PaperPosition',[1.25 1.5 6 8])
subplot(2,1,1)
plot(tout,xout(:,1),'-',tout,xout(:,2),'--',tout,xout(:,3),'-.')
legend('\theta_1','\theta_2','\theta_3',-1)
grid
title('Joint Positions')
subplot(2,1,2)
plot(tout,xout(:,4),'-',tout,xout(:,5),'--',tout,xout(:,6),'-.')
legend('\theta_1 (dot)','\theta_2 (dot)','\theta_3 (dot)',-1);
xlabel('time (sec)')
grid
title('Joint Velocities')

fig_hl=figure(3);
set(fig_hl,'Position', [220 120 400 450])
set(fig_hl,'PaperPosition',[1.25 1.5 6 8])
subplot(2,1,1)
plot(t_2,force(:,1),'-b',t_2,force(:,2),'--r')
legend('F_{tan} (N)','F_{norm} (N)',-1)
grid
title('Forces on End of Stance Leg')
subplot(2,1,2)
plot(t_2,force(:,1)./force(:,2))
ylabel('F_{tan}/F_{norm}');
xlabel('time (sec)')
grid

%plot the input
fig_hl=figure(4);
set(fig_hl,'Position', [240 140 400 450]);
set(fig_hl,'PaperPosition',[1.25 1.5 6 8])
subplot(2,1,1)
plot(t_2,torque(:,1))
ylabel('\tau_1 (Nm)')
grid
title('Control Signals')
subplot(2,1,2)
plot(t_2,torque(:,2))
ylabel('\tau_2 (Nm)');
xlabel('time (sec)')
grid

%plot the outputs
fig_hl=figure(5);
set(fig_hl,'Position', [260 160 400 450]);
set(fig_hl,'PaperPosition',[1.25 1.5 6 8])

62

subplot(2,1,1)
plot(t_2,y(:,1))
ylabel('y_1')
title('Outputs')
grid
subplot(2,1,2)
plot(t_2,y(:,2))
ylabel('y_2');
xlabel('time (sec)')
grid

% % save u,y to file
save('u_new.mat','torque','t_2');
%save('y.mat','y','t_2');

% Run the animation
anim(tout,xout,1/30,1);

%% ------------------------------------------------------------------------
--
%% the animation function

function anim(t,x,ts,speed)

[n,~]=size(x);
[~,vH]=hip_vel(x); % convert angles to horizontal position of hips
pH_horiz = zeros(n,1);

% Estimate hip horizontal position by estimating integral of hip velocity
%(sebesseg*ido=tavolsag)
for j=2:n
pH_horiz(j)=pH_horiz(j-1)+(t(j)-t(j-1))*vH(j-1,1);
end

[~,pH_horiz]=even_sample(t,pH_horiz,1/ts);
[te,xe]=even_sample(t,x,1/ts);
[n,~]=size(xe);

q=xe(1,1:3);
[pFoot1,pFoot2,pH,pT]=limb_position(q,pH_horiz(1));

fig_hl=figure(1);
set(fig_hl,'Position', [280 180 400 350]);
set(fig_hl,'PaperPosition',[0 0 6 5]);
clf
anim_axis=[-2.2 2.2 -2.2 2.2];
axis off
axis(anim_axis)
grid

% Use actual relations between masses in animation
[~,m,~,Mt,~,~]=model_params_three_link;
scl=0.04; % factor to scale masses
mr_legs=m^(1/3)*scl; % radius of mass for legs
mr_torso=Mt^(1/3)*scl; % radius of mass for torso
leg1_color='g';
leg2_color='r';
torso_color='b';
ground_color='k'; % a.k.a. black

% Approximate circular shape of mass

63

param=linspace(0,2*pi+2*pi/50,50);
xmass_legs=mr_legs*cos(param);
ymass_legs=mr_legs*sin(param);

xmass_torso=mr_torso*cos(param);
ymass_torso=mr_torso*sin(param);

% Draw ground
buffer=5;
ground=line([-buffer pH_horiz(n)+buffer],[0 0]);
set(ground,'Color',ground_color,'LineWidth',2);
for k=-buffer:floor(pH_horiz(n)+buffer)
ref_tick(k+buffer+1)=line([k k],[-0.1 0]);
set(ref_tick(k+buffer+1),'Color',ground_color);
ref_label(k+buffer+1)=text(-0.03+k,-0.2,num2str(k));
end

% Draw leg one
leg1=line([pFoot1(1) pH(1)],[pFoot1(2) pH(2)]);
mass1=patch(xmass_legs+(pH(1)-pFoot1(1))/2,...
ymass_legs+(pH(2)-pFoot1(2))/2,leg1_color);
set(mass1,'EdgeColor',leg1_color)
set(leg1,'LineWidth',2,'Color',leg1_color);

% Draw leg two
leg2=line([pFoot2(1) pH(1)],[pFoot2(2) pH(2)]);
mass2=patch(xmass_legs+pH(1)-(pH(1)-pFoot2(1))/2,...
ymass_legs+pH(2)-(pH(2)-pFoot2(2))/2,leg2_color);
set(mass2,'EdgeColor',leg2_color)
set(leg2,'LineWidth',2,'Color',leg2_color);

% Draw torso
torso=line([pH(1) pT(1)],[pH(2)*2 pT(2)*2]);
torso_mass=patch(xmass_torso+pT(1),ymass_torso+pT(2),torso_color);
set(torso_mass,'EdgeColor',torso_color)
set(torso,'LineWidth',2,'Color',torso_color);

for k=2:n
q=xe(k,1:3);
[pFoot1,pFoot2,pH,pT]=limb_position(q,pH_horiz(k));

set(leg1,'XData',[pFoot1(1) pH(1)],'YData',[pFoot1(2) pH(2)]);

set(mass1,'XData',xmass_legs+(pH(1)-pFoot1(1))/2+pH_horiz(k),...
'YData',ymass_legs+(pH(2)-pFoot1(2))/2);

set(leg2,'XData',[pFoot2(1) pH(1)],'YData',[pFoot2(2) pH(2)]);

set(mass2,'XData',xmass_legs+pH(1)-(pH(1)-pFoot2(1))/2,...
'YData',ymass_legs+pH(2)-(pH(2)-pFoot2(2))/2);

set(torso,'XData',[pH(1) pT(1)+(pT(1)-pH(1))],...
'YData',[pH(2) pT(2)+(pT(2)-pH(2))]);

set(torso_mass,'XData',xmass_torso+pT(1),'YData',ymass_torso+pT(2));

title(['T_{est} = ',num2str(te(k),'%.1f')])

new_axis=anim_axis+[pH(1) pH(1) 0 0];
axis(new_axis);

64


for j=1:length(ref_label)
if (j-buffer-1.05<new_axis(1)) || (j-buffer-1>new_axis(2))
set(ref_label(j),'Visible','off')
set(ref_tick(j),'Visible','off')
else
set(ref_label(j),'Visible','on');
set(ref_tick(j),'Visible','on')
end
end

drawnow;
pause(ts*speed);
end


%% ------------------------------------------------------------------------
--
%% a function to calculate hip velocity

function [vV,vH] = hip_vel(x)

vV=zeros(length(x),1);
vH=cos(x(:,1)).*x(:,4); % estimate of horizontal velocity of hips

%% ------------------------------------------------------------------------
--
%% a function to calculate the limb position

function [pFoot1,pFoot2,pH,pT] = limb_position(q,pH_horiz)

% Use position of hips as location of stance leg foot.
[r,~,~,~,L,~]=model_params_three_link;

pFoot1=[pH_horiz; 0];
pH=[pFoot1(1)+r*sin(q(1)); pFoot1(2)+r*cos(q(1))];
pFoot2=[pH(1)-r*sin(q(2)); pH(2)-r*cos(q(2))];
pT=[pH(1)+L*sin(q(3)); pH(2)+L*cos(q(3))];

%% ------------------------------------------------------------------------
--
%% CONVERTS A RANDOMLY SAMPLED SIGNAL SET INTO AN EVENLY SAMPLED SIGNAL SET
%% (by interpolation)
%%
%% written by Haldun Komsuoglu, 7/23/1999

function [Et, Ex] = even_sample(t, x, Fs)

% Obtain the process related parameters
N = size(x, 2); % number of signals to be interpolated
M = size(t, 1); % Number of samples provided
t0 = t(1,1); % Initial time
tf = t(M,1); % Final time
EM = (tf-t0)*Fs; % Number of samples in the evenly sampled case with
% the specified sampling frequency
Et = linspace(t0, tf, EM)';

% Using linear interpolation (used to be cubic spline interpolation)
% and re-sample each signal to obtain the evenly sampled forms
for s = 1:N,

65

Ex(:,s) = interp1(t(:,1), x(:,s), Et(:,1));
end

function [th3d,th1d,alpha,epsilon]=control_params_three_link
%control_params_three_link.m
%
% Control parameters for three-link legged biped.
%
% Eric Westervelt
% 20 February 2007

th3d=pi/6; % desired angle of torso
th1d=pi/8; % impact occurs with walking surface
alpha=0.9; % see page 14 of Grizzle paper
epsilon=0.1; % see page 16 of Grizzle paper


function [v]=control_three_link(H,LfH)
% CONTROL_THREE_LINK Calculate the control.
% [V] = CONTROL_THREE_LINK(X,H,LFH) is the control for the
% feedback linearized biped walking model.
%

% Eric Westervelt
% 03-May-2010 15:28:26

[th3d,th1d,alpha,epsilon]=control_params_three_link;

% LfH scaling
LfH=epsilon*LfH;

% phi fcns
phi1=H(1)+1/(2-alpha)*sign(LfH(1))*abs(LfH(1))^(2-alpha);
phi2=H(2)+1/(2-alpha)*sign(LfH(2))*abs(LfH(2))^(2-alpha);

% psi fcns
psi(1,1)=-sign(LfH(1))*abs(LfH(1))^alpha...
-sign(phi1)*abs(phi1)^(alpha/(2-alpha));
psi(2,1)=-sign(LfH(2))*abs(LfH(2))^alpha...
-sign(phi2)*abs(phi2)^(alpha/(2-alpha));

% calculate control
v=1/epsilon^2*psi;

function [D,C,G,B,K,dV,dVl,Al,Bl,H,LfH,dLfH]=dynamics_three_link(x,a)
% DYNAMICS_THREE_LINK Model of three-link biped walker model.
% [D,C,G,B,K,dV,dVl,Al,Bl,H,LfH,DLFH] = DYNAMICS_THREE_LINK(X,
% A) is the three-link
% biped walking model. (x is of dimension 6)

% Eric Westervelt
% 03-May-2010 15:28:24

[r,m,Mh,Mt,L,g]=model_params_three_link;

[th3d,th1d,alpha,epsilon]=control_params_three_link;

th1=x(1); th2=x(2); th3=x(3);
dth1=x(4); dth2=x(5); dth3=x(6);


66

% D matrix
D=zeros(3);
D(1,1)=Mh*r^2 + Mt*r^2 + (5*m*r^2)/4;
D(1,2)=-(m*r^2*cos(th1 - th2))/2;
D(1,3)=L*Mt*r*cos(th3 - th1);
D(2,1)=-(m*r^2*cos(th1 - th2))/2;
D(2,2)=(m*r^2)/4;
D(3,1)=L*Mt*r*cos(th3 - th1);
D(3,3)=L^2*Mt;

% C matrix
C=zeros(3);
C(1,2)=-(dth2*m*r^2*sin(th1 - th2))/2;
C(1,3)=-L*Mt*dth3*r*sin(th3 - th1);
C(2,1)=(dth1*m*r^2*sin(th1 - th2))/2;
C(3,1)=L*Mt*dth1*r*sin(th3 - th1);

% G matrix
G=zeros(3,1);
G(1)=- Mh*g*r*sin(th1) - Mt*g*r*sin(th1) - (3*g*m*r*sin(th1))/2;
G(2)=(g*m*r*sin(th2))/2;
G(3)=-L*Mt*g*sin(th3);

% B matrix
B=zeros(3,2);
B(1,1)=-1;
B(2,2)=-1;
B(3,1)=1;
B(3,2)=1;

% K matrix
K=zeros(2,4);
K(1,1)=156;
K(1,3)=25;
K(2,2)=110;
K(2,4)=21;

% dV matrix
dV=zeros(1,6);
dV(1,1)=(184*th1)/77 - 10*dth2 - 10*dth1 + (184*th2)/77;
dV(1,2)=(184*th1)/77 - 10*dth2 - 10*dth1 + (184*th2)/77;
dV(1,3)=(4515147318722739*th3)/2251799813685248 - 10*dth3 -
(4515147318722739*th3d)/4503599627370496 -
(4515147318722739*conj(th3d))/4503599627370496;
dV(1,4)=(370*dth1)/7 + (370*dth2)/7 - 10*th1 - 10*th2;
dV(1,5)=(370*dth1)/7 + (370*dth2)/7 - 10*th1 - 10*th2;
dV(1,6)=(4419157134357289*dth3)/70368744177664 - 10*th3 + 5*th3d +
5*conj(th3d);

% dVl matrix
dVl=zeros(1,4);
dVl(1,1)=(4515147318722739*th3)/2251799813685248 - 10*dth3 -
(4515147318722739*conj(th3d))/2251799813685248;
dVl(1,2)=(184*th1)/77 - 10*dth2 - 10*dth1 + (184*th2)/77;
dVl(1,3)=(4419157134357289*dth3)/70368744177664 - 10*th3 + 10*conj(th3d);
dVl(1,4)=(370*dth1)/7 + (370*dth2)/7 - 10*th1 - 10*th2;

% Al matrix
Al=zeros(4,4);
Al(1,3)=1;
Al(2,4)=1;


67

% Bl matrix
Bl=zeros(4,2);
Bl(3,1)=1;
Bl(4,2)=1;

%optimisation parameters
a01=a(1); a11=a(2); a21=a(3); a31=a(4);
a02=a(5); a12=a(6); a22=a(7); a32=a(8);

% Ha matrix
H=zeros(2,1);
H(1,1)=- a31*th1^3 - a21*th1^2 - a11*th1 - a01 + th3;
H(2,1)=th1 + th2 - (th1 + th1d)*(th1 - th1d)*(a32*th1^3 + a22*th1^2 +
a12*th1 + a02);

% LfHa matrix
LfH=zeros(2,1);
LfH(1,1)=dth3 - dth1*(3*a31*th1^2 + 2*a21*th1 + a11);
LfH(2,1)=dth2 - dth1*((th1 - th1d)*(a32*th1^3 + a22*th1^2 + a12*th1 + a02)
+ (th1 + th1d)*(a32*th1^3 + a22*th1^2 + a12*th1 + a02) + (th1 + th1d)*(th1
- th1d)*(3*a32*th1^2 + 2*a22*th1 + a12) - 1);

% dLfHa matrix
dLfH=zeros(2,6);
dLfH(1,1)=-dth1*(2*a21 + 6*a31*th1);
dLfH(1,4)=- 3*a31*th1^2 - 2*a21*th1 - a11;
dLfH(1,6)=1;
dLfH(2,1)=-dth1*(2*a02 + 2*(th1 + th1d)*(3*a32*th1^2 + 2*a22*th1 + a12) +
2*a12*th1 + 2*(th1 - th1d)*(3*a32*th1^2 + 2*a22*th1 + a12) + 2*a22*th1^2 +
2*a32*th1^3 + (th1 + th1d)*(2*a22 + 6*a32*th1)*(th1 - th1d));
dLfH(2,4)=1 - (th1 + th1d)*(a32*th1^3 + a22*th1^2 + a12*th1 + a02) - (th1 +
th1d)*(th1 - th1d)*(3*a32*th1^2 + 2*a22*th1 + a12) - (th1 -
th1d)*(a32*th1^3 + a22*th1^2 + a12*th1 + a02);
dLfH(2,5)=1;

function [r,m,Mh,Mt,L,g]=model_params_three_link
%model_params_three_link.m
%
% Model parameters for three-link legged biped.
%
% Eric Westervelt
% 20 February 2007

r=1; % length of a leg 1.15
m=5; % mass of a leg
Mh=15; % mass of hips
Mt=10; % mass of torso
L=0.5; % distance between hips and torso
g=9.8; % acceleration due to gravity

function [x]=sigma_three_link(omega_1_minus,a)
% SIGMA_THREE_LINK Maps velocity of stance leg just before
% impact to state of the system just before impact.
% [X] = SIGMA_THREE_LINK(OMEGA_1_MINUS,A)

% Eric Westervelt
% 03-May-2010 15:28:26

[th3d,th1d,alpha,epsilon]=control_params_three_link;


68

a01=a(1); a11=a(2); a21=a(3); a31=a(4);
a02=a(5); a12=a(6); a22=a(7); a32=a(8);

th1=th1d;
dth1=omega_1_minus;

th3 = a31*th1^3 + a21*th1^2 + a11*th1 + a01;
dth2 = 5*a32*dth1*th1^4 + 4*a22*dth1*th1^3 - 3*a32*dth1*th1^2*th1d^2 +
3*a12*dth1*th1^2 - 2*a22*dth1*th1*th1d^2 + 2*a02*dth1*th1 - a12*dth1*th1d^2
- dth1;
dth3 = dth1*(3*a31*th1^2 + 2*a21*th1 + a11);

x = [th1,-th1,th3,dth1,dth2,dth3];

function [f_tan,f_norm]=stance_force_three_link(x,dx,u)
% STANCE_FORCE_THREE_LINK Calculate the forces on the stance
% leg during impact.
% [F_TAN,F_NORM] = STANCE_FORCE_THREE_LINK(X,DX,U) are the forces on the
% stance leg at impact.

% Eric Westervelt
% 03-May-2010 15:28:26

[r,m,Mh,Mt,L,g]=model_params_three_link;

[th3d,th1d,alpha,epsilon]=control_params_three_link;

th1=x(1); th2=x(2); th3=x(3);
dth1=x(4); dth2=x(5); dth3=x(6);

% De11 matrix
De11=zeros(3,3);
De11(1,1)=(r^2*(4*Mh + 4*Mt + 5*m))/4;
De11(1,2)=-(m*r^2*(cos(th1)*cos(th2) + sin(th1)*sin(th2)))/2;
De11(1,3)=L*Mt*r*(cos(th1)*cos(th3) + sin(th1)*sin(th3));
De11(2,1)=-(m*r^2*(cos(th1)*cos(th2) + sin(th1)*sin(th2)))/2;
De11(2,2)=(m*r^2)/4;
De11(3,1)=L*Mt*r*(cos(th1)*cos(th3) + sin(th1)*sin(th3));
De11(3,3)=L^2*Mt;

% De12 matrix
De12=zeros(3,2);
De12(1,1)=(r*cos(th1)*(2*Mh + 2*Mt + 3*m))/2;
De12(1,2)=-(r*sin(th1)*(2*Mh + 2*Mt + 3*m))/2;
De12(2,1)=-(m*r*cos(th2))/2;
De12(2,2)=(m*r*sin(th2))/2;
De12(3,1)=L*Mt*cos(th3);
De12(3,2)=-L*Mt*sin(th3);

% De22 matrix
De22=zeros(2,2);
De22(1,1)=Mh + Mt + 2*m;
De22(2,2)=Mh + Mt + 2*m;

% Ce11 matrix
Ce11=zeros(3,3);
Ce11(1,2)=(dth2*m*r^2*(cos(th1)*sin(th2) - cos(th2)*sin(th1)))/2;
Ce11(1,3)=-L*Mt*dth3*r*(cos(th1)*sin(th3) - cos(th3)*sin(th1));
Ce11(2,1)=-(dth1*m*r^2*(cos(th1)*sin(th2) - cos(th2)*sin(th1)))/2;
Ce11(3,1)=L*Mt*dth1*r*(cos(th1)*sin(th3) - cos(th3)*sin(th1));


69

% Ce21 matrix
Ce21=zeros(2,3);
Ce21(1,1)=-(dth1*r*sin(th1)*(2*Mh + 2*Mt + 3*m))/2;
Ce21(1,2)=(dth2*m*r*sin(th2))/2;
Ce21(1,3)=-L*Mt*dth3*sin(th3);
Ce21(2,1)=-(dth1*r*cos(th1)*(2*Mh + 2*Mt + 3*m))/2;
Ce21(2,2)=(dth2*m*r*cos(th2))/2;
Ce21(2,3)=-L*Mt*dth3*cos(th3);

% Ge1 matrix
Ge1=zeros(3,1);
Ge1(1,1)=- Mh*g*r*sin(th1) - Mt*g*r*sin(th1) - (3*g*m*r*sin(th1))/2;
Ge1(2,1)=(g*m*r*sin(th2))/2;
Ge1(3,1)=-L*Mt*g*sin(th3);

% Ge2 matrix
Ge2=zeros(2,1);
Ge2(2,1)=Mh*g + Mt*g + 2*g*m;

% B matrix
B=zeros(3,2);
B(1,1)=-1;
B(2,2)=-1;
B(3,1)=1;
B(3,2)=1;

% See my notes, 2/16/200 for equations...
DD=inv((De12*inv(De22)).'*De12*inv(De22))*(De12*inv(De22)).';
F=DD*(-(De11-De12*inv(De22)*De12.')...
*dx(4:6)+(De12*inv(De22)*Ce21-Ce11)...
*dx(1:3)+De12*inv(De22)*Ge2-Ge1+B*u);

f_tan=F(1);
f_norm=F(2);

function [x_new,z2_new]=transition_three_link(x)
% TRANSITION_THREE_LINK Calculate the state of the system after impact.
% (Last two entries are the forces at the toe.
% [X_NEW,Z2_NEW] = TRANSITION_THREE_LINK(X) is the transition function
for
% biped walking model. (x is of dimension 8)

% Eric Westervelt
% 03-May-2010 15:28:25

[r,m,Mh,Mt,L,~]=model_params_three_link;

th1=x(1); th2=x(2); th3=x(3);

% De matrix
De=zeros(5,5);
De(1,1)=(r^2*(4*Mh + 4*Mt + 5*m))/4;
De(1,2)=-(m*r^2*(cos(th1)*cos(th2) + sin(th1)*sin(th2)))/2;
De(1,3)=L*Mt*r*(cos(th1)*cos(th3) + sin(th1)*sin(th3));
De(1,4)=(r*cos(th1)*(2*Mh + 2*Mt + 3*m))/2;
De(1,5)=-(r*sin(th1)*(2*Mh + 2*Mt + 3*m))/2;

De(2,1)=-(m*r^2*(cos(th1)*cos(th2) + sin(th1)*sin(th2)))/2;
De(2,2)=(m*r^2)/4;

70

De(2,4)=-(m*r*cos(th2))/2;
De(2,5)=(m*r*sin(th2))/2;

De(3,1)=L*Mt*r*(cos(th1)*cos(th3) + sin(th1)*sin(th3));
De(3,3)=L^2*Mt;
De(3,4)=L*Mt*cos(th3);
De(3,5)=-L*Mt*sin(th3);

De(4,1)=(r*cos(th1)*(2*Mh + 2*Mt + 3*m))/2;
De(4,2)=-(m*r*cos(th2))/2;
De(4,3)=L*Mt*cos(th3);
De(4,4)=Mh + Mt + 2*m;

De(5,1)=-(r*sin(th1)*(2*Mh + 2*Mt + 3*m))/2;
De(5,2)=(m*r*sin(th2))/2;
De(5,3)=-L*Mt*sin(th3);
De(5,5)=Mh + Mt + 2*m;

% E matrix
E=zeros(2,5);
E(1,1)=r*cos(th1);
E(1,2)=-r*cos(th2);
E(1,4)=1;
E(2,1)=-r*sin(th1);
E(2,2)=r*sin(th2);
E(2,5)=1;

% See Grizzle's paper, page 28 for equation...Eq.(2.32))
tmp_vec=([De -E';E zeros(2)])\[De*[x(4:6)';zeros(2,1)];zeros(2,1)];

x_new(1)=x(2);
x_new(2)=x(1);
x_new(3)=x(3);
x_new(4)=tmp_vec(2);
x_new(5)=tmp_vec(1);
x_new(6)=tmp_vec(3);
x_new(7)=tmp_vec(6);
x_new(8)=tmp_vec(7);
z2_new=tmp_vec(5);






71

Appendix B: Script to obtain variables from Simulink
% A MATLAB script to get the inputs and outputs from the Simulink
%
% Robert Marc
% 25 May 2010

%inputs
temp=simout_u1.signals.values(:);
U1=[];

for i=1:length(temp)
U1=[U1 temp(i)];
end
U1=U1';
temp=simout_u2.signals.values(:);
U2=[];

for i=1:length(temp)
U2=[U2 temp(i)];
end
U2=U2';

%outputs
temp=simout_y1.signals.values(:);
Y1=[];

for i=1:length(temp)
Y1=[Y1 temp(i)];
end

Y1=Y1';
temp=simout_y2.signals.values(:);
Y2=[];

for i=1:length(temp)
Y2=[Y2 temp(i)];

end
Y2=Y2';