Capture-Point Based Controllers for Robot Bipedal Locomotion: Analysis and Implementation on the iCub Platform.

(1)

UNIVERSITY OF PISA

D

EPARTMENT OF

I

NFORMATION

E

NGINEERING

ISTITUTO ITALIANO DI TECNOLOGIA

D

IC

Master Degree in Robotics and Automation Engineering

Capture-Point Based Controllers for Robot

Bipedal Locomotion: Analysis and

Implementation on the iCub Platform

Candidate: Giulio Romualdi

Supervisors:

Prof. Lucia Pallottino Dott. Daniele Pucci Eng. Stefano Dafarra

(2)

Acknowledgements

Prima di iniziare, vorrei spendere qualche parola per ringraziare tutte le persone che, in un modo o nell’altro, hanno contribuito al raggiungimento di questo enorme traguardo. Per motivi comunicativi i ringraziameti sarann`o scritti in due lingue, italiano ed inglese, cos`ı da rendere possibile a tutti la comprensione.

Innanzitutto verrei ringraziare la Prof.ssa Lucia Pallottino che ha deciso di accompagnarmi in questo percorso di Tesi e che ha creduto in me e nelle mie capacit`a.

Come non ringraziare la propria famiglia. Grazie Mamma, Babbo ed Elisa. Grazie per esserci sempre stati, per avermi sempre supportato e sop-portato. Non sarei nulla senza di voi.

Un ringraziamento enorme va ad Olivia, compagna, amica e faro nella notte. Grazie per avermi aiutato a credere di pi`u in me stesso, per esserci sempre stata nei momenti in cui tutto intorno a me era nero.

Ed ora `e il turno di Nicola. Quante avventure passate insieme: da com-pagni di corso, ad amici, per poi essere diventati coinquilini. Se ora sono qu`ı che sto scrivendo questi ringraziamenti `e sicuramente anche merito tuo che mi hai spinto sempre a migliorare e cercare di superare i limiti.

Un grazie a Federico, per i suoi squisiti tiramis`u che riescono sempre a motivarmi.

Un ringraziamento particolare va anche a Stefano e Carmine, amici di vecchia data, che sono la prova vivente che la vera amicizia pu`o superare mari e monti.

Ed ora `e il momento di passare in inglese per ringraziare tutti i ragazzi dell’IIT.

I would like to thank Dr. Daniele Pucci, supervisor and mentor, who, believing in me, gave me the opportunity to become part of a very close-knit research group.

I would also like to sincerely thank Stefano, for all the time spent to discuss all the topics of this work.

In addition, I would like to thank Silvio, Yue and Julien for the

(3)

II dinary experience in Trento.

Finally, I would like to extend my gratitude to the guys of the Dynamic Interaction and Control Lab for accepting me to be a part of the research group.

Sincerely, Giulio Romualdi

(4)

Abstract

Bipedal locomotion is still an open problem in the humanoid community. This thesis proposes a complete controller architecture that allows a hu-manoid robot to walk on a rigid and flat terrain. First, two controllers are developed to ensure the tracking of the desired divergent component of mo-tion and the zero momentum point. Then, an inverse kinematics algorithm is used to evaluate the desired joint positions.

As an additional contribution, a task-based velocity controller has been developed. This can be used, instead of the inverse kinematics algorithm, to reduce the computational effort. Furthermore, since the task-based velocity controller retrieves the robot signal to evaluate the desired joint values it ensures a better tracking of the desired Cartesian tasks, i.e. the desired CoM and feet position and orientation.

Experiments in the Gazebo simulator and on the iCub humanoid robot validate the proposed architecture.

(5)

Chapter 1 Introduction

From the dawn of time, human beings were fascinated by the possibility to create an intelligent robot. One of the first traces of this wish dates back to the work of Apollonius of Rhodes, an ancient Greek author. In his masterpiece, The Argonautica, he describes Talos, a giant automaton made of bronze whose purpose is to protect Europa in Crete from pirates and invaders. Here a short extract of the 4-th book, translated in English [1], is attached:

“And Talos, the man of bronze, as he broke off rocks from the hard cliff, stayed them from fastening hawsers to the shore, when they came to the roadstead of Dicte’s haven. He was of the stock of bronze, of the men sprung from ash-trees, the last left among the sons of the gods; and the son of Cronos gave him to Europa to be the warder of Crete and to stride round the island thrice a day with his feet of bronze. Now in all the rest of his body and limbs was he fashioned of bronze and invulnerable, but beneath the sinew by his ankle was a blood-red vein; and this, with its issues of life and death, was covered by a thin skin.”

The dream of creating an automaton becomes reality when, around the year 1495, Leonardo da Vinci designed and possibility constructed the so-called Leonardo’s mechanical knight, Figure 1.1. However, even if the idea behind an autonomous system that can cooperate with humans dates back to ancient times, the father of the word “robot” was the Czech writer Karel

ˇ

Capek. He used, in 1921, the term “robot” to denote an automaton in the play R.U.R. (Rossum’s Universal Robots).

It is interesting to notice that in both the works of Apollonius of Rhodes and Leonardo da Vinci the automatons have human aspect; however, the

(9)

CHAPTER 1. INTRODUCTION 2

Figure 1.1: Model of Leonardo’s robot with inner workings, on display in Berlin.

first modern robots were firstly adopted inside factories and they were not humanoid. This is due to several reasons, among which, the most important is the huge amount of complexity behind a humanoid robot, both from the electromechanical and the mathematical point of views.

To find the first robot used in industries we go back to 1961 when the first industrial robot, named Unimate, joined the assembly line at a General Motors plant to work with heated die-casting machines. Starting from that moment, robots had become a constant presence in the assembly line. The success of this kind of robots lies in their capacity to perform very accurate movements and, at the same time, to move heavy loads. In general, indus-trial manipulators are robots constituted by several links connected by joints. Their mechanical structure allows the robot to move its end-effector, e.g. a drill bit, in a desired position inside the space. Because of their capabili-ties, these robots are widely used inside the assembly line to substitute the unskilled labor, performing repetitive tasks [9].

Another emerging field is mobile robotics for intralogistics purpose. Fig-ures 1.2a and 1.2b show two examples of wheeled robots that can be adopted inside a warehouse. Indeed, a robot equipped with wheels has the ability to move in an environment without architectural barriers, such as a warehouse. A mobile robot can be used to localize objects on a shelf and, once grasped, place them precisely at their destination.

The applications of wheeled robots are not restricted to warehouse and factories. Vacuum cleaners, 1.2c, are a renowned commercial product and an example of wheeled robotics for householding. Researchers are now pursuing

(10)

CHAPTER 1. INTRODUCTION 3

(a) (b)

(c) (d)

Figure 1.2: Example of wheeled robots: the autonomous flexFELLOW, (a),

produced by KUKA R

; TORU, (b), a mobile robot produced by Magazino R for e-commerce warehouses; Roomba 960, (c), vacuuming robot produced by iRobot R

; R1, (d), humanoid robot produced by IIT.

the objective of using wheeled humanoid robots, like R1, 1.2d, for more complicated tasks like washing dishes or serve dinner.

Locomotion is becoming a fundamental requisite. In general for terrestrial robots, the available means of locomotion are wheels and legs: the former offers an intrinsic stability and simplify the control problem. On the other hand, the latter allows the robot to walk in uneven terrains. Indeed when a

(11)

CHAPTER 1. INTRODUCTION 4 wheeled robot has to move into a real environment the architectural barriers could become a problem. To overcome this issue a possible solution is to equip the robot with legs. This kind of robots, differently from wheeled platforms, has the ability to walk and to balance in almost any type of terrains. However, the introduction of legs adds several difficulties, one of the most important is maintaining the equilibrium in the upright position.

Several bipedal humanoid robots where developed, among which two ex-amples are cited: Atlas, 1.3a and iCub 1.3b. Even if both robots are equipped by legs they are very different, indeed Atlas is made by hydraulic actuator while iCub is composed only by electric motors. According to the type of actuation, Atlas and iCub have very different objectives, indeed the former was built for military purposes, and it has the ability to lift heavy loads. On the other hand, the latter was developed to study the cognitive intelligence and how robots can interact with humans.

(a) (b)

Figure 1.3: Example of bipedal robots: Atlas, (a), produced by Boston Dynamics R

; iCub, (b), a humanoid robot produced by IIT.

1.1 Thesis Contribution

The problem of legged locomotion is a fundamental requirement for a hu-manoid robot. Before entering into the details, it is important to remember that a humanoid robot is a mechanical system, whose ability to move de-pends on the motors attached to each joint. To perform the walking task

(12)

CHAPTER 1. INTRODUCTION 5 with a bipedal robot, the desired trajectories for the feet and the center of mass have to be evaluated. Then, a controller architecture has to be defined in order to ensure the tracking of the desired quantities, even if the humanoid is subjected to external disturbances, such as an unexpected contact with the environment.

According to this, the legged locomotion problem can be summarized as follows: find a control law such that a humanoid robot, equipped with legs, can walk in a human environment.

Since the problem is very general, it has been approached in several ways. Some scientists faced the problem by reducing the model of the entire robot to a simple class of systems, this was inspired by the works of Vukobratovi´c. He introduced the zero momentum point (ZMP) [44] as the point on the contact surface at which the net moment of the inertial forces and the gravity forces has no component along the horizontal axes. The definition of this particular point moved the attention of the entire complex walking problem to the study of the movement of the ZMP. Actually, if the ZMP is well defined (i.e. it is inside the support polygon) the stance foot does not rotate along the sagittal and lateral directions and the unilaterality of the contact force is guaranteed [43]. However, it is important to notice that the previous principle does not take into account the friction between the ground and the stance foot, thus the foot may rotate along the normal axis to the ground or, even worst, it may slide on the walking surface.

Above the ZMP concept, several simplified models were developed, one of the most important is the well known 3D-Linear inverted pendulum model (3D-LIPM) [25]. Using this approximation, several control strategies were developed, one of the most notable solutions is the Kajita’s ZMP preview controller [24]. In his work, he presented a technique that can be used to generate the desired center of mass trajectory in order to guarantee the fea-sibility of the gait (i.e. ZMP inside the support polygon). Also, the works of Englsberger take place in the framework of the simplified models. He introduced the divergent component of motion (DCM) [12] as the unstable dynamics of the 3D-LIPM equation and developed a controller that guaran-tees the stability of the simplified model. Several authors tried to extend the concept of DCM in order to overcome the assumptions of the 3D-LIPM (e.g. a constant height of the center of mass). Hopkins in 2015 [21] presented a framework for dynamic walking on uneven terrain using a time-varying ex-tension of the divergent component of motion, while Ashtiani in the 2017 [39] designed a robust controller for locomotion.

According to this, the aim of this work is to exploit the divergent compo-nent of motion framework as a method for implementing a walking strategy for the iCub humanoid robot.

(13)

CHAPTER 1. INTRODUCTION 6 Two controller architectures will be developed and implemented. The former is a standard linear controller, while the latter uses the Model Pre-dictive Control (MPC) approach in order to ensure the equilibrium in the upright position. Both the architecture will use the Force Torque (F/T) sen-sors attached to the ankle to evaluate the position of the ZMP. This feature allows iCub to be more compliant if subject to external disturbances such as an external force applied by a human. This, for the ultimate goal, to obtain a safe platform that can cooperate side by side with humans.

In order to validate the entire controller architecture, the algorithm is firstly tested in simulation using a full model of the robot, then on the real humanoid.

1.2 Thesis Outline

This work is structured as follows. Chapter 2 introduces the reader inside the framework necessary to understand the controller architecture developed in this work. The locomotion problem is introduced for the human beings and then is ported to the robotic platform. Later the simplified models used in the controller synthesis are presented. Chapter 3 contains a detailed de-scription of iCub, the humanoid platform used as a workbench to test the algorithms developed during the thesis. In Chapter 4 the problem of the footstep planning is discussed, however since the development of a footpath planner was not a goal of the thesis, just a summary description will be given. Chapter 5 analyzes the problem of generating a trajectory for the divergent component of motion starting from the desired footsteps positions. In Chap-ter 6 the entire controller architecture is described. Here two controllers for stabilizing the unstable dynamics of the DCM and a controller used to ensure the tracking of the desired ZMP trajectory are presented. Later the problem of the how to evaluate the desired joint values for the humanoid robot is introduced. The problem will be solved by using a nonlinear optimization and by implementing a task-based velocity controller. In Chapter 7 the re-sults of the framework developed during this work will be discussed, both in simulation and on the real platform, iCub. Finally, Chapter 8 completes the thesis.

Last but not least a set of appendices are written. Here the interested reader could find mathematical details of the analysis presented in this work.

(14)

Chapter 2 State of Art

In this Chapter the concepts that will be used to achieve the objective of the thesis are detailed introduced. The Chapter is organized in four section: in Section 2.1 the locomotion task in the human beings is analyzed, then these concepts are ported in a robotic system; in Section 2.2 the Zero Momentum Point and the Foot Rotation Indicator are introduced; in Section 2.3 the well known in 3D Linear Inverted Pendulum model is derived; finally in Section 2.4 the Divergent Component of Motion is analyzed in details.

2.1 Introduction of the Locomotion

In this Section the human locomotion is briefly introduced. Subsection 2.1.1 introduces the gait cycle. Later, in Subsection 2.1.2 the concept introduced for the human beings are ported to bipedal robots.

2.1.1 Locomotion in human beings

Before entering into the world of the locomotion the concept of the gait cycle has to be introduced. The gait cycle is defined as the time between two successive foot contact of the same limbs, it can be divided into two phases, the stance phase and the swing phase. In general, for a healthy man, the stance phase constitutes about the 60% of the gait cycle, while the remaining part, about 40%, is the swing phase [27].

Now the stance and the swing phases of one of the foot, taken as a ref-erence, will be analyzed. The stance phase is the part of a gait cycle during which the foot remains in contact with the ground and it is constituted by five sub-phases.

• initial contact: the instant when the foot contacts the ground; 7

(15)

CHAPTER 2. STATE OF ART 8 • loading response: time period between the initial contact phase and

the instant when the other foot lifts the ground;

• mid stance: time interval from the end of the loading response phase to the time when the centers of both ankles are aligned in the frontal plane;

• terminal stance: the period from ankles alignment to the contact of the swinging foot;

• pre-swing: time interval between the end of the terminal stance phase and the instant when the foot lifts from the ground.

The swing phase is the phase of the gait during which the reference foot swings and it can be distinguished into three parts:

• initial swing: phase during which the reference foot is lifted from the ground to the position of maximum knee flexion;

• mid-swing: time period between the maximum knee flexion and the instant when the tibia reaches the vertical position;

• terminal swing: following vertical tibia position to just prior to initial contact.

Last but not least the concepts of double support, DS, and single support, SS, are introduced. The first one refers to the period during which boot feet are in contact with the ground while the other refers to the interval when only one foot is in contact with the ground.

In Figure 2.1 all the phases described above are shown. For each phase, the type of support (single or double) is specified.

Figure 2.1: Classification of a normal human gait cycle. Image taken from [27].

(16)

CHAPTER 2. STATE OF ART 9 In the following Section the concepts described will be applied to the robot walking.

2.1.2 Locomotion in robotic systems

In robot locomotion the gait cycle described in Section 2.1.1 is simplified, and the phases described are condensed in more general groups. The walking task can be easily summarized using the walking cycle, Figure 2.2, it embeds all the phases of the human locomotion shown in Figure 2.1.

Single support (right leg) Single support (left leg) Swing phase Detachment Double support Impact Swing phase Detachment Double support Impact

Figure 2.2: Walking cycle state machine.

The cycle can be split in two main phases, the double support, DS, and the single support, SS, ones. The former embeds the initial contact, the load-ing response, and the pre-swload-ing phases while the latter embeds the others. During the SS phase, the movement of the swing foot is usually split into three sub-phases, detachment, swing and impact. More detailed:

• detachment: the instant when the foot lifts from the ground;

• swing: this phase embeds the initial swing, the mid-swing and the terminal swing stages of the human walking;

(17)

CHAPTER 2. STATE OF ART 10

2.2 Analysis of the rotation of the stance foot

In this Section, two concepts useful for analyzing the rotation of the stance foot are introduced. Section 2.2.1 introduces the zero momentum point. While in Section 2.2.2, the foot rotation indicator is analyzed.

Even if the zero momentum point and the foot rotation indicator are often used as tools to analyze the postural stability of biped robots it is important to notice that it is a mistake; indeed, as will be detailed discussed in the following subsections, these two quantities, if well defined, guarantee only that the stance foot does not rotate.

2.2.1 Zero Momentum Point

In order to derive a model for the robotic locomotion the zero momentum point, ZMP, concept [44] must be discussed. In the following dissertation, only the single support phase is considered and, in order to simplify the analysis, only the stance foot is considered.

In the entire following analysis the stance foot is supposed rigidly attached to the ground, i.e. it does not move.

In Figure 2.3 the free body diagram of the stance foot is depicted, here the the influence of the entire robot, due to the gravity and the acceleration of each link, is replaced by the force applied to the point A, FA, and the torque relative to A, MA. The total ground reaction consists of a force applied to the point P , R, and a torque relative to P , M , notice, in Figure 2.3, only the z component of M is represented. Finally, the weight of the foot itself is given by the mass of the stance foot, ms, times the gravitational acceleration, g, and acts at its gravity center G.

Figure 2.3: Free body diagram of the reference foot. Image edit from [43]. The horizontal components of the reaction force R, Rx and Ry, are the

(18)

CHAPTER 2. STATE OF ART 11 friction force necessary to balance the horizontal components of the force FA, while the vertical component of the reaction torque M , Mz, represents moment generated by the friction reaction forces and it balances the vertical component of the torque MA.

Since, as already mentioned above, the reference foot is assumed in con-tact with the ground without sliding, the friction compensates the horizontal force components (Rx, Ry) and vertical reaction torque, Mz. The vertical reaction force Rz compensates the vertical component of the force FA and the weight of the foot itself.

Finally, it is very interesting to study the balancing of the horizontal component of the torque MA, MAx and MAy, and understand why they are

not represented in 2.3. In general, every wrench, couple of force and torque, applied to a rigid body is equivalent to a force applied along a fixed axis plus a torque about the same axis 1_{. In this specific case, the point P is the} intersection between the ground terrain and the Poinsont axis.

In Figure 2.4 a simple planar case of the sagittal plane is presented. In this simple scenario, the component MAx is balanced by shifting the point

P .

Figure 2.4: Example of a planar case. Image taken from [43].

It is important to note that until the reaction force R is applied within

1_{This is always true and it is proved by the Poinsot Theorem [31]}

In particular, given a wrench (fA, µA_{) applied at the point A. This is equivalent to}

another wrench (fL, µL_{) applied at any point L on an axis l =}

{q∗_{: q}∗_{= q + λω,}_{∀λ ∈ R}}

and whose components fL and µL _{are parallel to l.}

More formally, if the _fA _{6= 0, f}L= λfω and µL= λµω where

ω = f A fA , q = f A × µA fA 2 , λf = fA λµ=hf A , µAi fA ,

where h., .i is the inner product in R3_.

(19)

CHAPTER 2. STATE OF ART 12 the support polygon, i.e. the intersection between the ground terrain and the Poinsont axis is inside the support polygon, the ankle moment is full compensate.

However, in the case which the sole is not large enough to allow the appropriate position of the point P the force R acts at the edge of the foot and the horizontal components Mx and My are different from zero.

The reasoning presented above can be summarized in the following prin-ciple [43].

Principle 2.1. In order to guarantee that the stance foot does not rotate, the point P , the intersection between the ground terrain and the Poinsont axis, has to belongs to the support polygon.

Zero Momentum Point evaluation

Now the position of the Zero Momentum Point is evaluated. It should be noted that the position of the ZMP is a prerequisite of the stability of the contact 2 _{between the stance foot and the ground.}

The cardinal equations for the static of the stance foot are now written: R + FA+ msg = 0, OP × R + msOG× g + OA × FA+ MA+   00 Mz   = 0, (2.1)

where R, Mz, FA, MA and ms are shown in Figure 2.3, while OP , OG and OA are respectively the position vector from the origin of the coordinate system Oxyz to the ground reaction force acting point, P ; CoM of the foot,G, and the ankle joint, A.

Projecting the second cardinal equation (2.1) onto the z-axis and placing the origin of the reference system O at P , i.e. O ≡ P , Mz can be evaluated

Mz = Mf r = [−(MA+ P A× FA)]z

where [v]z is used to indicate the scalar product between the vector v and the third vector of the canonical space in R3_, _{hv, e}

3i.

The projection of the equation (2.1) onto the horizontal plane (xy-plane) gives

[OP × R + msOG× g + OA × FA+ MA](x,y) = 0.

2_{In the following the term stability of contact is used to indicate that the stance foot}

(20)

CHAPTER 2. STATE OF ART 13 Placing the origin of the reference system O at P (i.e. O _{≡ P ) the following} relation holds

[msP G× g + P A × FA+ MA](x,y) = 0. (2.2)

An intuitive understanding is obtained by setting ms = 0 and MA = 0 in

equation (2.2)

[P A× FA](x,y) = 0

in this particular case P is the point on the ground where the line of the action of FA penetrates, Figure 2.5.

A

P FA

R

Figure 2.5: ZMP simple example. Image edit from [43].

In order to understand if stance foot does not rotate the position of P , evaluated using (2.2), has to be within the support polygon.

The following principle holds:

Principle 2.2. If the position of the point P is inside the convex hull of the foot support area, Ω, then the stance foot is rigidly attached to the ground.

However if the pointP _{∈ ∂Ω it is not possible to conclude on the stability} of the robotic system.

Where ∂Ω is the boundary of Ω (i.e. ¯Ω = Ω_{∪ ∂Ω).}

Thus, as consequent of 2.2, the position of ZMP may not be an accurate criterion to distinguish between a stable contact from an unstable one, to solve this problem a new concept must be introduced: the Fictitious Zero Momentum Point, also know with the name of Foot Rotation Indicator (FRI) [19].

2.2.2 Foot Rotation Indicator

In order to overcome the limitation of the ZMP, another point is introduced. The Foot Rotation Indicator (FRI) is a point on the ground surface, within

(21)

CHAPTER 2. STATE OF ART 14 or outside the support polygon, where the ground reaction force R would have to act in order to keep the stance foot rigidly attached to the ground. In order to formally define the FRI point the entire robot must be taken into account.

The following dynamic equation holds

M + OP _{× R +} N −1_X i=0 OGi× mig = N −1 X i=0 ˙ HGi + N −1_X i=0 OGi× miai where mi is the mass, Gi is the position of CoM, HGi is the angular

momen-tum about the CoM and ai is the CoM linear acceleration of the i-th link expressed in the inertial frame, while M and R are define in Section 2.2.1.

Following the same steps of the ZMP evaluation the dynamic of the entire robot, except from the reference foot, is completely represented by a force FA and a torque MA.

The dynamic equation of the foot is given by:

M +OP×R+OG0×m0g +MA+OA×FA= ˙HG0+OG0×mia0, (2.3)

where the index 0 correspond to the stance foot.

It important to note that the equation (2.1) can be easily obtained from 2.3 by setting the dynamic terms to 0. In the special case in which O _{≡ P} (where P is the Center of pressure) the static equilibrium of the foot becomes

M + P G0× m0g + MA+ P A× FA= 0. (2.4)

Projecting Equation (2.4) into the xy-plane (2.2) is obtained and, in the presence of unbalanced torque on the foot, and its solution may not belong to the support polygon.

The solution of (2.2) is called Foot Rotation Indicator (FRI) point and it is in general indicated with the letter F

[F G0× m0g + MA+ F A× FA](x,y) = 0. (2.5)

An explicit solution of the equation (2.5) was find by Goswami [19]; the main idea is compute the “dynamic of the robot without the foot”

−MA− F A × FA+ N −1_X i=1 F Gi× mig = N −1_X i=1 ˙ HGi+ N −1_X i=1 F Gi× miai, considering only the tangential plane and using the equation (2.5)

" F G0× m0g + N −1 X i=1 F Gi× mi(g− ai) # (x.y) = "_{N −1} X i=1 ˙ HGi # (x,y) .

(22)

Substituting F Gi with F O + OGi and OF =−F O the following equation

holds "_{N −1} X i=1 OF _{× m}i(ai− g) − OF × mig # (x,y) = " −OG0× m0g + N −1_X i=1 ˙ HGi + N −1_X i=1 OGi× mi(ai− g) # (x,y) .

The cartesian position of the FRI point OFx OFy 0 T can be computed OFx= m0OG0yg + PN −1 i=1 miOGiy(aiz + g) + PN −1 i=1 miOGizaiy + PN −1 i=1 H˙Gix m0g +PN −1_i=1 mi(aiz + g) OFy = m0OG0xg + PN −1 i=1 miOGix(aiz + g) + PN −1 i=1 miOGizaix + PN −1 i=1 H˙Giy m0g +PN −1i=1 mi(aiz + g) . In the following some properties of the FRI point are summarized. The position of the FRI indicates the occurrence of foot rotation and it also indicate the magnitude and the direction of the unbalanced moment.

MQ = Q∗F × (m1g + FA),

where Q∗ _{(shown in Figure 2.6) is the nearest point to F belonging to the} boundary of the convex hull of the foot support area.

More formally Q∗ = arg min Q∈∂ΩkQF k2. A Q∗ F −FA MA R

Figure 2.6: Unstable unbalance. Image edit from [43].

The most important result of the introduction of the FRI point can be summarized in the following principle [19]:

(23)

CHAPTER 2. STATE OF ART 16 Principle 2.3. If the position of the FRI point F is inside the convex hull of the foot support area (Ω) then the stance foot does not rotate

It is also possible to define the stability margin against the foot rotation. Indeed, it can be quantified by the minimum distance between the boundary of the support polygon ∂Ω and the location of the FRI point within the sole. Instead, when the FRI point is outside the convex hull of the foot support area the _kQ∗F_k₂ is a measure of the robot instability.

At this point, it is interesting to compare the concept of ZMP and FRI. The ZMP, because of its definition, is the solution of (2.2) and it has to be always inside the support polygon, while the FRI, is not forced to belong to support polygon and its position is a measure of the robot instability.

In the case of stable contact, i.e. the stance foot is rigidly attached to the ground, the FRI and the ZMP coincide and more important they coincide with the position of the center of pressure (CoP), the point where the sum of all forces acting between the stance foot and its supporting surface is applied. It is important to notice that, in a humanoid robot, the CoP can be easily evaluated using force torques sensors mounted on the ankle level or better using a pressure sensor mounted under the feet. Indeed, in this case, the wrench acting on the stance foot can be estimated and the position of the CoP is equal to:

rcopx =− µC y fC z rcopy = µC x fC z , (2.6) where µC

∗ is the torque evaluated in the origin of the contact frame, C. fzC is the normal force acting on the stance foot. rcopx and rcopy are the x and y

coordinates of the CoP expressed in the frame in which the contact wrench is evaluated, _C.

2.3 Derivation of the 3D-LIPM

To stabilize the robot in the upright position the study of the motion of the center of mass, CoM, is an essential requisite. In literature, there are two different approaches to achieve this goal.

In the former the precise knowledge of robot dynamics (mass of each joint, location of the center of mass of each joint, etc.) is required, while in the latter, called inverted pendulum approach, only a limited knowledge of the robot dynamics (the position of the whole-body center of mass, CoM, the

(24)

CHAPTER 2. STATE OF ART 17 entire mass of the robot, m, etc.) is used. Since the controllers developed in this work use the simplified model approach, in this Section the equation of the simplified model, 3D Inverted pendulum model, is derived.

The following dissertation will be mainly based on Kajita’s work [25].

2.3.1 Derivation of the equations of a general 3D

In-verted Pendulum

When a biped robot is supporting its body on one leg (i.e. the robot is in single support phase), its dynamics can be approximated by the 3D Inverted Pendulum Model. This approximation allows reducing the dynamics of the whole body to a simple inverted pendulum considering only the position of the whole-body center of mass, CoM, and the entire mass of the robot, m.

The model, shown in Figure 2.7, represents the entire robot as point mass on top of a telescopic leg with no inertia while the other edge of this pendulum is the “foot”, and it is set in rigid contact with the ground.

x y z r θr θp

Figure 2.7: 3D Inverted pendulum model.

The position of the mass point x =x y zT is uniquely specified by a set of the Lagrangian coordinates q

q =θr θp r T

,

where r is the length of the pendulum while the angles θr and θp are shown in Figure 2.7.

(25)

CHAPTER 2. STATE OF ART 18 The Cartesian position of the CoM can be related to Lagrangian

coordi-nates: _       x = r sθp y =_{−r s}θr z = rq1− s2 θr − s 2 θp.

In order to develop a model of the pendulum the dynamic equation can be be easily derived

m¨x = (JT)−1τ + mg, (2.7)

where g = 0 0 _−gT is the gravity vector, τ = τr τp f T

is the actu-ated force/torque vector associactu-ated to the Lagrangian coordinates and J is the Jacobian matrix

J = ∂x ∂q =    0 r cθp sθp −r cθr 0 − sθr − r cθrsθr q 1−s2 θr−s2θp − r c_θps_θp q 1−s2 θr−s2θp q 1− s2 θr − s 2 θp    . In order to erase the term (JT₎−1 _{the Equation (2.7) is multiplied by J}T

mJTx = τ + mJ¨ Tg, m   0 −r cθr − r cθrsθr D r cθp 0 − r cθpsθp D sθp − sθr D    xy¨¨ ¨ z   =  ττrp f   − mg  −r cθrsθr D −rcθpsθp D D   , (2.8) where D = q1_{− s}2 θr − s 2 θd.

Using the first row of the Equation (2.8) and multiplying by D/ cθp the

following relation is obtained D cθr m−¨yr cθr − ¨zr cθrsθr D = D cθr τr+ mgr D cθr cθrsθr D . (2.9)

Substituting kinematic relation in (2.9) the equation of the the pendulum along the y axis can be obtained

m(_{−z¨y + ¨zy) =} D cθr

τr− mgy. (2.10)

With an analogous procedure the equation of the pendulum along the x axis can be obtained

m(z ¨x_{− ¨zx) =} D cθp

(26)

2.3.2 Derivation of the equations of a 3D Linear

In-verted Pendulum

Although the moving pattern of the pendulum has many possibilities, the class of motion that would be suitable for walking is a small subset. In this specific task the trajectory of the pendulum must lie on a plane characterized by a normal vector kx ky −1

and passing through the point (0 0 zc). z = kxx + kyy + zc.

Substituting the second derivative of the plane equation in (2.10) and in (2.11) the following system is obtained

( ¨ x = _zg cx− ky zc(x¨y− ¨xy) − 1 mzcup ¨ y = _zg cy− kx zc(x¨y− ¨xy) − 1 mzcur, (2.12) where ur = D cθr τr up = D cθp τp.

In the case of the walking on a flat plane (i.e. kx = 0 and ky = 0) the Equation (2.12) becomes ( ¨ x = _zg_cx− 1 mzcup = g zc(x− rzmpx) ¨ y = _zg_cy− 1 mzcur = g zc(y− rzmpy),

where the point rzmpx rzmpy 0

is the zero momentum point.

The set of the equation above can be written in a more elegant form ¨

x = ω2(x− rzmp),

where x is the vector of the horizontal coordinates of the CoM, while rzmp is the vector of x and y position of the ZMP. The parameter ω is the time costant of the model.

ω = r

g zc

. (2.13)

Since the sagittal and logitudinal dyanamics are complitely decoupled, in the following only the x axis is analyzed. The obtained results are valid also for the y dynamics.

The system dynamics of the x axis is given by: ˙σ = 0 1 ω2 ₀ σ + 0 −ω2 rzmpx, (2.14)

(27)

CHAPTER 2. STATE OF ART 20 where σ = x ˙xT.

The explicit solution of the dynamic system is [13] σ(t) = cosh(ωt) _ω1 sinh(ωt) ω sinh(ωt) cosh(ωt) σ0+ 1_{− cosh(ωt)} −ω sinh(ωt) rzmpx (2.15) where σ = x0 ˙x0 T

is the initial condition.

2.4 Divergent Component of Motion

In this Section the concept of divergent component of motion (3D-DCM) is introduced and the equations of the DCM are derived following the works of Englsberger [13, 11].

Before starting it is important to depict a significant difference between the capture point (CP) and the 3D-DCM. The former, also known with the term 2D-divergent component of motion (2D-DCM), was defined by Pratt et al. [37] and it is the point on the ground, to which the robot has to step to come to a stop asymptotically. The latter is not restricted to the ground plane and can be interpreted as a three-dimensional state. However in the common 2D scenario with constant CoM height the CP and 3D-DCM (projected to the ground) are equivalent; this explains why in Englsberger’s work [13] the term capture point was used to indicate the 2D-DCM. In this work, the terms CP and 2D-DCM will be confused.

This Section is organized into two Subsections. In Subsection 2.4.1, the 3D-DCM concept is introduced. In Subsection 2.4.2, the projection on the ground plane of the 3D-DCM (2D-DCM) is discussed.

2.4.1 Derivation of the 3D-DCM

The three-dimensional divergent component of motion was introduced by Englsberger [11]. The main goal of this point is to overcome the limitation of taking into account only trajectories restricted to the horizontal plane (z = const) due to use the LIPM for bipedal walking control.

The 3D-DCM is defined as the vector ξ

ξ = x + b ˙x, (2.16)

where x =x y zT and ˙x =˙x ˙y ˙zT are respectively the CoM position and velocity vector expressed with respect to an inertial frame, while b is a strictly positive constant. It is important to notice that (2.16) is a general transformation and it is not restricted to the LIPM.

(28)

CHAPTER 2. STATE OF ART 21 By reordering Equation (2.16) the CoM dynamic system can be derived

˙x =₋1

b(x− ξ), (2.17)

the CoM dynamic equation is a first order stable dynamic system_{∀b > 0 (i.e.} the CoM follows the DCM, Figure 2.8).

ξ

₀

ξ

_f

x

0

˙x

0

˙ξ

₀

Figure 2.8: Two-dimensional Capture Point and CoM.

By differentiating (2.17) and using (2.16) the following DCM dynamic equation holds: ˙ξ = −1 b(x− ξ) + b¨x =₋1 b(x− ξ) + b mF , (2.18)

where F = Fg+ Fext, the total force acting in the robot CoM, can be split in gravitational force, Fg, and in external forces, Fext. The main idea is to design the external forces in order to be appropriate for the robot walking task while the feasibility constraints are satisfied (i.e. center of pressure inside the support polygon). For the sake of simplicity, a force-to-point transformation is used to express the external forces:

(29)

CHAPTER 2. STATE OF ART 22 where s is a positive constant and eCMP is the enhanced centroidal moment pivot point [11, 12]. It is important to point out that the eCMP is related to the CMP [36], indeed the first one could not belong to the ground plane while the second is the intersection point between the ground surface and the line between the CoM and the eCMP.

The total force acting on the CoM is given by

F = Fg+ Fext= mg + s(x− reCM P), (2.19)

where g is the gravitational vector 0 0 _−gT.

Combining Equation (2.18) with (2.19) the following DCM dynamic equa-tion holds ˙ξ =bs m − 1 b x + 1 bξ− bs mreCM P + bg,

this shows that the states x and ξ are in general coupled, however they can be decoupled by choosing the parameter s equal to m/b2

˙ξ = 1 bξ−

1

breCM P + bg. (2.20)

Using this particular choice for the s parameter the external force becomes Fext=

m

b2(x− reCM P).

For the sake of simplicity Equation (2.20) can be reduced using the virtual repellent point (VPR)

rvrp = reCM P +

0 0 b2_gT_.

Combining the VRP definition with Equation (2.20) the DCM dynamics can be rewritten:

˙ξ = 1

b(ξ− rvrp). (2.21)

This shows that the 3D-DCM dynamic equation is a first order unstable dynamic system _{∀b > 0.}

2.4.2 Derivation of the 2D-DCM

If the robot is modelled using the well known LIPM the following assumptions can be done:

(30)

CHAPTER 2. STATE OF ART 23 2. the angular momentum rate ˙L is zero ( ˙L = 0).

The first assumption allows to project the 3D problem on the ground surface [11, 12] because the position of the eCMP coincides with the CMP, indeed the eCMP position is given by

reCM P = x−

b2

mFext= x− 1

mω2Fext,

where in the second equality the LIPM assumption is used by substituting b with ω2 _{= g/z}

c (inverse time constant of the LIPM). Taking into account only the z coordinate of reCM P the following relation holds

reCM Pz = zc− 1 mω2Fextz = zc− mg mω2 = zc− mg m_zg c = 0,

where the external force projected on the z-axis is equal to the gravity force [12]. Note that the CMP is equivalent to the eCMP when it intersects the ground.

Furthermore the second assumption can be used to prove the equality between the CMP and the ZMP [36].

Making these assumptions the dynamic equation of the 3D-DCM can be projected on the ground plane and the following 2D-DCM equation holds:

˙˜ξ = ω(˜ξ − rzmp), (2.22)

where, ˜ξ is the vector containing only the x and y coordinates of the DCM. It is important to underline that the dynamic equation of the CoM respect the DCM (2.17) is still valid except for the fact that now it only takes into account the x and y axes.

The external forces applied to the CoM, projected on the x and y axes, becomes

Fext = mω2(x− rzmp) (2.23)

The 2D-DCM can be also derived from the solution of the LIPM equations, for further details please refers to the Appendix A.

In the following chapters, to avoid overloading the text with an extremely complex notation, the 2D-DCM will be often indicated with the DCM or CP acronyms and ˜ξ is always indicate with ξ.

(31)

Chapter 3 iCub: the Humanoid Robot

The aim of this Chapter is to introduce the platform used to test the control algorithms presented in this work: the iCub humanoid robot.

iCub, described in [42], is an entirely open-source humanoid robot born to study cognitive sciences.

This Chapter is structured as follows: in Section 3.1 the mechanical equa-tion of a floating base robot are recalled; in Secequa-tion 3.2 the mechanical struc-ture of iCub is presented; in Section 3.3 the sensor, used for the locomotion task, mounted on iCub are introduced; Section 3.4 contains a brief introduc-tion to YARP, the robot middleware used to communicate with the robot.

3.1 Model of a humanoid robot

In this Section, a brief dissertation about the kinematics of a floating base system is debated. This does not want to be a complete exposition, indeed only the concepts useful for the main objective of the thesis will be analyzed.

The interested reader can find further details in [15].

A multibody system is a mechanical system composed of n + 1 links connected by n joints furthermore, it has a floating base if none of its links has an a priori fixed position. To derive a framework to study the kinematics of a floating base system the flowing frames are defined, Figure 3.1:

I = {Oi, xi, yi, zi}, B = {Ob, xb, yb, zb},

the former is the inertial frame, i.e. a fixed frame rigidly attached to the ground. While the latter is the base frame and it is attached to one of the link of the robot named base link. The configuration of a floating base robot is determined by the position and the orientation of the base frame with respect to the inertial frame and the internal joints values.

(32)

CHAPTER 3. ICUB: THE HUMANOID ROBOT 25

y

_i

x

i

z

i

z

b

x

b

y

_b

Figure 3.1: Example of a simple floating base system. The base is represented by the link colored in blue.

A humanoid robot is an example of floating base multibody system com-posed of n + 1 links connected by n joints with one degree of freedom. Since none of the links of the robot has an a priori constant position and orienta-tion with respect to a global frame I, its configuration can be determined by the position and the orientation of the base frame _{B (e.g. the stance foot} during the locomotion) and the joints values. In this particular case, the configuration space is defined as:

Q = SE(3)× Rn,

where SE(3) is the special Euclidean group and Rn _{is used to represent the} joint angles. An element of Q is then a triplet

q = (IoB,IRB, s), where (I_o

B,IRB) ∈ SE(3) denotes the position and the orientation of the base frame, _{B, expressed with respect to the inertial frame I; and s ∈ R}n represents the joint angles.

Furthermore it is easy to show that Q is a Lie group, Definition C.3. Indeed given two element of Q,

(33)

CHAPTER 3. ICUB: THE HUMANOID ROBOT 26 the group multiplication is defined as

q_{· p = (o}q+ oq, RqRp, sq+ sq), while inverse of q is given by:

q−1 = (−oq, RTq,−sq).

Since Q is a Lie group, the velocity of the multibody system is represented by the Lie algebra V, Definition C.4. An element of the Lie algebra V is a triplet:

ν = (I˙oB,IωI,B, ˙s), whereI_ω

I,Bis the angular velocity of the base with respect the inertial frame whose coordinates are written in the inertial frame. In general A_ω

B,C is used to indicate the relative angular velocity between the frame C and the frame B, whose coordinates are written in the frame A.

More formally, the angular velocity, I_ω

I,B, is defined as: I ωI,B = S(IωI,B)∨ I ˙ RB = S(IωI,B)IRB, where S(I_ω

I,B)∈ so(3), the set of 3×3 skew-symmetric matrices, for further details please refers to Appendix C, Definition C.5.

Given a link of the floating base system its position with respect to the inertial frame is uniquely identified by an homogeneous transformation, I_H

A ∈ SE(3), between the inertial frame, I, and the frame attached to that link, A. I_H

A contains the rotation matrix, IRA, and the vector connecting the origin of the inertial frame to origin of the frame _{A expressed in the} inertial frame, I_o A, I HA = _I RA IoA 0T 1 .

Given a link of the floating base system the velocity of a rigid body can be represented by: I vA= I ˙oA I_ω I,A ,

where _{A is the frame attached to the link. The interested reader can find} further details regarding different velocity representations in [31].

In another word, the velocity of a link attached to the robot is represented by a vector containing the linear velocity of the origin of the frame attached to the link, A, expressed with respect to the inertial frame I and the angular velocity between the frameA and the inertial frame I written in the inertial frame coordinates.

(34)

CHAPTER 3. ICUB: THE HUMANOID ROBOT 27 Finally, the Jacobian for a floating base system is introduced. The Jaco-bian JA(q) is the map between the robot velocity ν ∈ V and the linear and angular velocities of the frame A. For example, given the velocity I_v

A ∈ R6 and an element of the Lie algebra ν _{∈ V:}

I

vA = JAν,

where JA can be split into two parts, one related to the velocity of the base and the other one concerned the joints velocity.

JA(q) = Jb A J j A , where Jb A(q)∈ R6×6 is JAb = I3 −S(IoA−IoB) 03 I3 .

3.2 Hardware

The iCub humanoid robot is 104 cm tall, weighting 33 kg. It is constituted by 53 degrees of freedom as you can see in Figure 3.2b, most of these located in the hands:

• 38 in the upper body: – 7 for each arm; – 9 for each hand;

– 6 for the head (three at the neck, three at the eyes). • 12 in the lower body, 6 for each leg:

– 3 for the hip; – 1 for the knee; – 2 for the ankle. • 3 for the torso.

However for the purpose of this thesis, only 23 of them are considers: • 6 DoFs for each legs;

• 4 DoFs for the upper arm, 3 of which in the shoulder and one in the elbow;

(35)

(a) (b)

Figure 3.2: iCub kinematic structure. Figure (a) shows a CAD representation of the iCub and Figure (b) of its kinematic structure. For visual clarity, the representation of the eyes and hands joints have been omitted. Image taken from [34].

• 3 DoFs for the torso.

The torso and shoulder joints are mechanically coupled and driven by ten-don mechanisms. All the 23 joints are powered by brushless electric motors equipped with harmonic drive gears.

3.3 Sensors

A peculiar feature of iCub is the huge number of sensors available. First of all, it is equipped with a distributed tactile skin that allows iCub to per-ceive if it is touched. It also has force/torque (F/T) sensors, accelerometers, gyroscopes, two cameras and microphones.

For the objectivities of this thesis, the F/T sensors cover a central role. Indeed iCub possesses 6 six-axis F/T sensors [17], depicted in Figure 3.3a. Two of them are mounted on the shoulders, the other four respectively below the hip joints and on the feet.

(36)

(a) (b)

Figure 3.3: Figure (a) shows the F/T sensors location while Figure(c) repre-sents the Inertial Measurements Units (IMUs) locations. Image taken from [32].

iCub is also equipped with an artificial skin, Figure 3.4, which provides information about both the location and the intensity of the contact forces. It is distributed on the torso, the arms, the palm and the fingertips of hands and the legs. Since iCub is not equipped with torque sensor at the joint level, the F/T sensors and the skin are used to provide an estimation of both the internal torques and external wrenches [17].

Last but not least, as can be seen from Figure 3.3b, iCub has an array of inertial sensors providing acceleration and angular velocity information. They can be used both to estimate the orientation of the humanoid robot with respect to the inertial frame and to estimate the gravity force exerted on each link.

The F/T sensors and the estimation algorithm for the external wrenches are widely used by the walking algorithm. Indeed the contact wrenches at feet level are indispensable to evaluate the position of the ZMP, by using Equation (2.6).

(37)

Figure 3.4: Tactile sensors.

3.4 Software architecture

To communicate with the robot, a network infrastructure is necessary. To this purpose, the middleware Yet Another Robot Platform (YARP) was de-veloped [28, 16]. YARP is an open-source multi-platform middleware whose main purpose is to allow communication between the applications (modules), which can run on different computers. More specifically YARP is a set of libraries, protocols and tools to keep modules and devices cleanly decoupled. Indeed, it provides an abstraction layer to interact with physical devices,

Camera Port Motors Port F/T sensors Port

iCub

Camera

Port

_module

Vision

Motors Port F/T sensors Port

Controller

module

udp tcp udp

Figure 3.5: Example of YARP network. iCub, represented with the box on the left, exposes several interface. Each interface can be used by a different module running in a different computer with different operating systems.

(38)

CHAPTER 3. ICUB: THE HUMANOID ROBOT 31 such as encoders, F/T sensors, skin, independently of their actual implemen-tation. This is useful to encourage modularity and code reuse. For example, the acquisitions from sensors and motor controllers are provided through YARP interfaces.

In Figure 3.5 an example of YARP network is depicted. The rectangles represent different modules which may run in a different computer with a different operating system. The rectangle on the left represents iCub, which exposes the interfaces related to the cameras, F/T sensors and motors. On the left two different applications are represented: one, using the images com-ing from the cameras, estimates the position of an object. The other module needs the information of the provided by F/T sensors and the encoders to evaluate a desired position for the joints. Both applications expose “ports” to receive and send data to iCub. In this specific case, every connection can take place using a different protocol and/or physical network (e.g. TCP, UDP, ...).

(39)

Chapter 4 Footstep planner

In this Chapter, the algorithm used to plan the footsteps is presented. Since the developing of the planner was not a goal of the thesis but only an instru-ment to generate the DCM trajectory, this wants to be just an introduction to the foot planning problem.

The main idea behind the footsteps planner is to generate the desired footsteps by using a simplified model, the unicycle.

This Chapter is structured as follows: in Section 4.1 the model of the unicycle is presented and a nonlinear control law used to stabilize the desired position is synthesized. In Section 4.2 the technique used to extract the footsteps from the unicycle trajectory is presented. Finally, in Section 4.3 the method used to plan the trajectory of the swing foot is analyzed.

4.1 The Unicycle Model

The unicycle is a planar robot equipped by two wheels, whose axes of rotation coincide. It has the possibility to move forward and turn, but, because of its mechanical structure, the instantaneous lateral motion is forbidden.

To derive the dynamical equations of the unicycle the following frames are defined:

I = {Oi, xi, yi} U = {Ou, xu, yu},

the former is the inertial frame while the latter is a frame attached to unicycle whose x pointing out the forward direction of the motion.

The state of the unicycle can be completely defined by the following vector q =x y θT,

where x and y are the coordinates of the vector connecting the origin of the 32

(40)

CHAPTER 4. FOOTSTEP PLANNER 33 I_o U xi yi Oi I_o F xu y_u U_o F F Ou θ

Figure 4.1: Unicycle model.

inertial frame to the center of the unicycle expressed in the inertial frame, I_o

U. θ is the angle between the unicycle frame and the inertial frame. Because of its mechanical structure, the model is subjected to a constraint

˙y

˙x = tan(θ), that can be written if Pfaffian form:

A(q) ˙q =sin(θ) _{− cos(θ) 0}  ˙x˙y

˙θ   = 0.

We can make use of the concepts of distribution and codistribution [23] to define:

Ω(q) =Rr(A(q)),

where _Rr denotes the row range and Ω(q) is a codistribution.

Now, since the dim(Ω(q)) = 1 and dim(q) = 3 the codistribution is annihilated by a distribution ∆, whose dimension is 2, and it is defined as follows:

∆(q) = Ω⊥(q) =Rc(S(q)), where _Rc denotes the column range.

(41)

CHAPTER 4. FOOTSTEP PLANNER 34 For the case of the unicycle S(q) is defined as follows

S(q) =S1(q) S2(q) =  cos(θ) 0sin(θ) 0 0 1   . Thus the dynamics equation of the unicycle is

˙q =  cos(θ) 0sin(θ) 0 0 1  ν1 ν2 ,

where ν1 and ν2 are called quasi-velocities, and in this specific case they are respectively, the forward, v, and the angular, ω, velocities.

It is important to notice that the dynamic equation of the unicycle can be also written using a different formalism [38] that can be useful for the development of the controller.

(_I

˙oU = R(θ)e1v

˙θ = ω (4.1)

where R(θ) _{∈ SO(2) is the rotation matrix between the unicycle frame and}

inertial frame and e1 is the canonical vector in R2.

4.1.1 Unicycle Controller

The control objective is to asymptotically stabilize the point, F , a fixed point with respect to the unicycle, to the desired point F∗. Thus the error e can be define e :=IoF −Io∗F, (4.2) where I_o F is I oF =IoU + R(θ)UoF.

So the control objective becomes the asymptotic stabilization of e to 0. The time derivative of the error (4.2) is given by

˙e =I_˙o U + ˙R(θ)UoF =I˙oU + ωR(θ)HUoF, (4.3) where H is H = 0 −1 1 0 .

(42)

CHAPTER 4. FOOTSTEP PLANNER 35 Substituting (4.1) into (4.3), the output dynamics can be written in the following form:

˙e = B(θ)u,

where u is the vector containing the controlled input and B(θ) has the fol-lowing structure: B(θ) = R(θ) 1 ₋U_o Fy 0 U_o Fx . Assuming that U_o

Fx 6= 0, i.e. F does not belong to the wheels axis, the

control law [38]

u = B(θ)−1(U˙o∗F − Ke), (4.4)

guarantees, for every K > 0, that 0 is an asymptotically stable equilibrium point of the error dynamical system (4.3).

4.2 Footsteps Trajectory

In this Section, the trajectory performed by the unicycle controlled by the law (4.4) is used to extrapolate the footsteps.

This approach is supported by the works of Truong [41] and Mombaur [30], indeed in their papers, the authors proved that humans, while walking, tend to keep the orientation of the body tangent to the path.

In [14], authors used the unicycle as a simplified model to extrapolate footsteps, however, They generate steps using constant velocity and a con-stant footsteps length. This drawback can be overcome by solving an opti-mization problem that allows obtaining steps with different step length and step duration.

In order to obtain the footsteps positions, the trajectory of the left foot and the right foot, represented by the wheels of the unicycle, are sampled using a sampling time T .

I oLk = I_o Uk+ R(θk) U_o L, IoRk = I_o Uk+ R(θk) U_o R,

where _{L and R are respectively the frame attached to the left and right feet} and the subscript k is used to indicate the time instant.

Given the set of unicycle position (which is finite, thanks to discretiza-tion), we can select a particular position for the feet. Each position in the set is associated with a time instant, k. This instant is considered as the moment in which the corresponding foot is expected to touch the ground and it is called impact time, timp. The impact time can be used as decision variables to select also the feet position.

(43)

CHAPTER 4. FOOTSTEP PLANNER 36 Using the impact time, the step duration can be defined:

∆t=|tlfimp− t rf imp|.

Another interesting quantity is the distance of the feet during the double support phase ∆x = I odsLk − I odsRk .

Both ∆x and ∆t are used inside the cost function in order to reduce the possibility to obtain long and fast steps

J = Kt 1 ∆2

t

+ Kxk∆xk2,

where Kt> 0 and Kx > 0 are used to allow the robot to perform long/short and fast/slow steps.

However using only the cost function too long/short or too fast/slow steps can be planned. In order to avoid this undesirable behavior, several constraints are added. First of all, the bound on the step duration is necessary to avoid too fast or too slow steps and a division by zero in the cost function:

tmin ≤ ∆t≤ tmax.

Also, the distance between the feet during the double support phase has to be bounded in order to avoid undesired collision between feet and, on the other side, step length that the robot, according to its mechanical structure cannot achieve:

dmin ≤ ∆x ≤ dmax.

Last but not least, an additional constraint has to be taken into account: the relative rotation between the two feet, _|∆θ|, has to be lower than θmax, this is necessary to take into account the mechanical limitations of the humanoid robot.

|∆θ| ≤ θmax.

Finally, the optimization problem can be summarized as follows: min timp Kt_∆12 t + Kx∆ 2 x

subject to : tmin ≤ ∆t≤ tmax dmin ≤ ∆x ≤ dmax |∆θ| ≤ θmax

(4.5)

Once the impact time of each footstep has been evaluated, the single and the double supports duration can be obtained.

(44)

CHAPTER 4. FOOTSTEP PLANNER 37

4.3 Swing Foot Trajectory

In this Section, the problem of generating the feet trajectory from the foot-steps is solved. More formally, given two consecutive footfoot-steps positions we characterized by impact times, we want to find a trajectory that connects these two poses.

In order to evaluate the desired feet trajectory, the spline interpolation approach can be used. One of the main advantages of using spline inter-polation is the possibility to avoid the so-called Runge’s phenomenon (i.e. oscillation between interpolating point using high order polynomials).

Before entering into the details of how the spline can be used to interpolate the feet trajectory a brief dissertation about cubic spline has to be done. It is important to notice that this Section does not pretend to be an exhaustive dissertation about splines. The reader interested in this topic can find more details in [4].

4.3.1 Cubic Spline

Given n + 1 points the main objective is to find a set of 3-th order poly-nomial that interpolates the points. The boundary conditions between two polynomials are chosen in order to guarantee a smooth transition. In Figure 4.2 an example of cubic spline interpolation is shown.

1 2 3 4 −10 −5 5 10 q0 q1 q2 q3 _q4 q5 q6 q7 q8 q9 q10 _t s(t)

Figure 4.2: A cubic spline trajectory through 11 points (n = 10). Each function between two consecutive points is a 3-th order polynomial, whose boundary conditions are chosen to ensure C2 _{continuity of the interpolating} function.

(45)

CHAPTER 4. FOOTSTEP PLANNER 38 Given n + 1 points, the spline function is defined as follows:

s(t) =_{qk(t), t∈ [tk, tk+1], k = 0, . . . , n− 1}, where qk(t) is a 3-th order polynomial and it is equal to:

qk(t) = ak0 + ak1(t− tk) + ak2(t− tk)

2_{+ a}

k2(t− tk)

3_.

For n+1 points, n polynomials are necessary to compute the interpolation. Furthermore, since each polynomial is a 3-th order function the total number of coefficients that has to be evaluated is 4n.

So, in order to compute the spline interpolation, 4n constraints are nec-essary. Generally, the following constraints are taken into account:

• 2n constraints related to the initial and final values of each spline, this guarantees the continuity of the spline (i.e. C0_);

• n−1 constraints related to the velocity in the via points, this guarantees the continuity of the first derivative of the spline (i.e. C1_);

• n−1 constraints related to the velocity in the via points, this guarantees the continuity of the second derivative of the spline (i.e. C2_);

These constraints allow to find 2n+2(n_{−1) coefficients, therefore 2 degrees of} freedom remain. Thus two additional constraints have to be imposed in order to evaluate the spline. Among all the possible choices, two are interesting for the objective of this Chapter. The former is related to the initial and final velocities, while the latter is related to the initial an final accelerations, in general, these two conditions are called natural conditions.

4.3.2 Spline and Feet trajectories

Using the spline is possible to interpolate the desired trajectory of the swing foot, indeed, in this specific case, n is equal to 1 for the x and y coordinate and the yaw angle, while is equal to 2 for the z direction. Indeed during the walking gait, the swing foot has to move in order to reach a desired maximum height. The instant in which foot reach its maximum altitude is called apex time.

The initial and final velocities are used as additional constraints, more specifically the initial velocity is set equal to 0, while the final velocity de-pends on the direction. Indeed for the z direction, a final velocity different from zero is desirable instead of having a zero impact velocity.

(46)

CHAPTER 4. FOOTSTEP PLANNER 39

Figure 4.3: Swing foot trajectory interpolation.

One of the main advantages of using spline is the guarantee of at least the C2 _{continuity of the interpolating function. This could be very useful in} the cases in which the desired velocity or acceleration is required.

Indeed as will be discussed later in Chapter 6, the desired feet position and velocity are required by the inverse kinematics solvers, together with the desired position and velocity of the CoM.

In Figure 4.3 an example of spline interpolation for the left foot is pre-sented.

In conclusion, the output of the planner is the desired homogeneous trans-formations and velocities of the feet with respect to the inertial frame.

I_Hd RF = I_Rd RF IodRF 0T 1 , I_Hd LF = I_Rd LF IodLF 0T 1 . (4.6)

As regards the velocity of the feet they are expressed using the formalism presented in 3.1: vd_rf = I_˙od RF I_ωd I,RF , vd_lf = I_˙od LF I_ωd I,LF , (4.7)

whereI˙o∗F andIωI,∗F are the linear and the angular velocity expressed with respect to the inertial frame.

(47)

Chapter 5 2D-DCM trajectory generator

In this Chapter the problem of generating a trajectory for the 2D-DCM is extensively presented. As can be seen in Chapter 6, and more specifically in Figure 6.1, the trajectory will be the reference signal of the entire controller architecture.

The main idea behind this trajectory generator is to use the Footpath planner presented in Chapter 4 as starting point to build a trajectory com-patible with the DCM dynamics, presented in Equation (2.22).

The Chapter is organized in two Section. Section 5.1 exposes the theory behind the generation of the DCM trajectory. In Section 5.2, some “tricks” necessary for the implementation are discussed.

5.1 Theory behind 2D-DCM trajectory

gen-erator

In this Section, the theory behind a 2D-DCM trajectory generator will be analyzed. Firstly, in Subsection 5.1.1, the assumption of the instantaneous transition between two consecutive single support phase (SS) is done. Later, in Subsection 5.1.2, it will be relaxed and the double support phases (DS) will be taken into account.

5.1.1 Generation of the Single support trajectory

The objective of this Section is to provide a simple technique to generate a DCM trajectory taking into account only SS phase (i.e. instantaneous transition between left and right SS phases).

In order to simplify the planning procedure the following assumptions are done:

Capture-Point Based Controllers for Robot Bipedal Locomotion: Analysis and Implementation on the iCub Platform.

UNIVERSITY OF PISA

D

EPARTMENT OF

I

NFORMATION

E

NGINEERING

Master Degree in Robotics and Automation Engineering

Capture-Point Based Controllers for Robot

Bipedal Locomotion: Analysis and

Implementation on the iCub Platform

Acknowledgements

Abstract

Contents

Chapter 1

Introduction

1.1

Thesis Contribution

1.2

Thesis Outline

Chapter 2

State of Art

2.1

Introduction of the Locomotion

2.1.1

Locomotion in human beings

2.1.2

Locomotion in robotic systems

2.2

Analysis of the rotation of the stance foot

2.2.1

Zero Momentum Point

2.2.2

Foot Rotation Indicator

2.3

Derivation of the 3D-LIPM

2.3.1

Derivation of the equations of a general 3D

In-verted Pendulum

2.3.2

Derivation of the equations of a 3D Linear

In-verted Pendulum

2.4

Divergent Component of Motion

2.4.1

Derivation of the 3D-DCM

ξ

ξ

x

˙x

˙ξ

2.4.2

Derivation of the 2D-DCM

Chapter 3

iCub: the Humanoid Robot

3.1

Model of a humanoid robot

y

x

z

z

x

y

3.2

Hardware

3.3

Sensors

3.4

Software architecture

iCub

module

Vision

Controller

module

Chapter 4

Footstep planner

4.1

The Unicycle Model

4.1.1

_module