Quadruped Walking Running Simulation

Autonomous Systems LabProf. Roland Siegwart

Semester-Thesis

Supervised by: Author:Marco Hutter Dominik NfDavid Remy

Quadruped walking/runningsimulation

Spring Term 2011

Contents

Abstract iii

Symbols v

1 Introduction 11.1 About the Project . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Task . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2 Model 32.1 The Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.2 The Quadruped . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.3 SimMechanics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42.4 Literature Research . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.4.1 Hard Contact versus Soft Contact . . . . . . . . . . . . . . . 52.5 Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2.5.1 2D Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.5.2 3D Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.6 Ground Contact . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.7 Visualization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

3 Controller 133.1 Literature Research . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

3.1.1 Raibert Control . . . . . . . . . . . . . . . . . . . . . . . . . . 133.1.2 Virtual Model Control . . . . . . . . . . . . . . . . . . . . . . 17

3.2 Implementation of the Controller . . . . . . . . . . . . . . . . . . . . 173.2.1 General Architecture . . . . . . . . . . . . . . . . . . . . . . . 183.2.2 Force Calculation . . . . . . . . . . . . . . . . . . . . . . . . . 193.2.3 Forward Velocity Control . . . . . . . . . . . . . . . . . . . . 203.2.4 Roll and Pitch Control . . . . . . . . . . . . . . . . . . . . . . 203.2.5 Energy Shaping . . . . . . . . . . . . . . . . . . . . . . . . . . 22

4 Results 234.1 General Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 234.2 Eigenvalue Optimization . . . . . . . . . . . . . . . . . . . . . . . . . 234.3 Forward Trotting . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

5 Interpretation 295.1 Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

5.1.1 To do . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295.2 Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

5.2.1 To do . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

6 Acknowledgment 31

i

A Files 33

B How to use 34

C Results 35

Bibliography 41

ii

Abstract

This report describes the work that has been done for the semester projectQuadrupedwalking/running simulation. During the work a 3 dimensional simulation of a trot-ting four legged robot was implemented in the MATLAB SimMechanics environ-ment.The work was split into two phases: First, a model of the quadruped was build withthe main attention put on the creation of the ground contact model. A soft contactapproach with a linear spring damper systems was chosen. During the second phasea simple and robust controller for the robot in a trotting gait was created. For that,a Raibert three-part controller was applied. To be able to do that for our fourlegged robot, a virtual model had to be defined for the legs.For the determination of the best fitted parameters for the control algorithm, as wellan empirical and an optimization method based on an eigenvalue analysis were used.The created controller delivered satisfying results in its reactions to disturbances.Specially roll and pitch angles could be corrected successfully, whereas stabilizationof disturbances in horizontal velocities might be improved.

iii

Symbols

Symbolsfx, fy, fz ground reaction force in x, y and z-directionTst duration of stance phasexf position of the foot relative to the hip in x-directionr,p,y roll, pitch and yaw angleT vector of actuator torquesF force applied to CoGFpitch, Froll vertical force for pitch and roll control

Acronyms and AbbreviationsCoG Center of Gravity of the main body of the quadrupedFL Front Left (leg)FR Front Right (leg)RL Rear Left (leg)RR Rear Right (leg)VL Virtual Leg (leg)TD Touchdown pointETH Eidgenssische Technische Hochschule

v

Chapter 1

Introduction

The control of legged locomotion is one of the most challenging fields in roboticsresearch today. Even though wheeled locomotion typically is very energy efficientand easy to control, legged locomotion is better suited when it comes to roughand unstructured terrain. While in the early years the focus was mostly laid in thedefinition of static controllers, the focus nowadays moved on to dynamic and energyefficient locomotion.

1.1 About the ProjectThis work is part of a research project of the ASL (Autonomous System Lab) atthe ETH in Zurich. The main goal of this project is to build a prototype of afour legged robot which is specialized in energy efficient locomotion. This efficiencyshould be achieved by using passive dynamics for different dynamic motions suchas fast walking, trotting, bounding and galloping. A CAD rendering of the robotdeveloped for this purpose can be seen in Figure 1.1.

Figure 1.1: CAD rendering of the StarlETH robot.

1

Chapter 1. Introduction 2

1.2 TaskThe task of this thesis is to implement a 3 dimensional model of the quadrupedused in the project with MATLAB SimMechanics. The model should reflect thephysical properties of the robot and its environment, be computationally efficient,and should allow the integration of different control algorithms in a simple way.Further, a control algorithm should be implemented which controls the createdmodel in a trotting gait. The controller should be robust, i.e. it should be able toreact to different disturbances in a stabilizing ways.

Chapter 2

Model

2.1 The Model

In order to create a controller that regulates the quadruped, a reliable model isrequired. The main requirements to build this model were:

- a realistic reproduction of the multi body structure

- an acceptable calculation time

- suitable for simple integration of several controllers

- easily understandable and adjustable

The quadruped modeled is described in the next section.

2.2 The Quadruped

The robot modeled in this work is a four legged machine with three actuated jointsper leg (Figure 2.1).The three joint actuators are considered as optimal torque sources for hip abduction,hip flection, and knee flection. Six different coordinate systems are defined and usedat the robot:

- The world coordinate system (CSw).

- The main body fixed system originated at the CoM of the main body (CSmb).

- Four local coordinate systems originated in the hip joints of the legs (CSloc)with a fix offset to CSmb.

The main body had a length of 0.5 m and a mass of 10 kg. For a more detailedspecification please consult the following table.

3

Chapter 2. Model 4

Figure 2.1: Definition of the Legs and Coordinate Systems

Parameter Value Descriptionparam.mainBody.m 10 main body massparam.mainBody.I diag([0.128 0.171 0.9

0.171])main body inertia

param.mainBody.b 0.3 main Body widthparam.mainBody.l 0.5 main Body lengthparam.thigh.l 0.2 thigh lengthparam.thigh.s 0.03 CoG position in z direction

from hipparam.thigh.m 1.78 thigh massparam.thigh.I diag([1e-3 1e-3 1e-5]) thigh inertiaparam.shank.l 0.2 shank lengthparam.shank.s 0.1 CoG position in z direction

from hipparam.shank.m 0.548 shank massparam.shank.I diag([3e-3 3e-3 1e-5]) shank inertiaparam.hip.l 0.05 distance hip abduction to hip

flexion axis in neg z directionparam.hip.s 0.025 distance hip abduction to

CoG in neg z directionparam.hip.m 0.5 hip massparam.hip.I 1e-3*eye(3) hip inertia

2.3 SimMechanics

To achieve the attributes described in Section 2.1 MATLAB Simulink and the Sim-Mechanics toolbox were chosen as simulation environment. This approach providesa straight forward way to create the model, which is important for later correctionsor improvements of the model.

5 2.4. Literature Research

MATLAB SimMechanics is an additional toolbox in the MATLAB Simulink en-vironment, which is specially developed for the modeling of 3-dimensional physicalproblems. A library providing a wide range of body-, joint-, constraint-, and otherblocks representing the physical structure of the model is offered to provide an in-tuitive approach. The blocks represent physical body parts, such as the main bodyor the hip to which you can assign certain dimensions, inertias and masses, but alsojoints or actuators to which you can assign angles or torques. Furthermore, you canalso use blocks that serve as sensors and let you create an interaction point betweenyour model and the control algorithm. Like that it is possible to build a modeljust as you would assemble a physical system in reality. SimMechanics calculatesthe mechanical motions using forward dynamics, inverse dynamics and kinematics,trimming and linearization.By the fact that the SimMechanics environment is based on MATLAB Simulink, youcan build subsystems based on Simulink models or even embedded functions. Likethat, the model can be created entirely in MATLAB Simulink and SimMechanics,but the controller can easily be integrated as an ordinary MATLAB function. Moreabout this topic can be seen in the Section 2.5.For more information about MATLAB SimMechanics please consult [1].

2.4 Literature ResearchBefore starting to build the model, a literature research was done. For the mainarchitecture of the model not a lot of information was accessible, since a very par-ticular model was planned here. Some insights were gained from Philipp Omlinswork [2], which addresses a similar problem with MATLAB Simulink. The mainissue of the research was to find the most suitable way to implement the groundcontact forces. In the papers [2], [3], [4], [5] and [6] several different solutions arediscussed using soft contact properties.

2.4.1 Hard Contact versus Soft ContactIn the literature mainly two different approaches for the modeling of contact prob-lems can be found - hard and soft contact models. The difference between hardcontact and soft contact modeling is that in a hard contact model a geometric con-straint is used (i.e. the body cant penetrate the ground), whereas in a soft contactmodel a constraining force is applied.

Hard Contact: For the simulation that means that in case of a hard contact model,the ground contact is modeled as an instantaneous discrete transition between thetwo states no contact and contact. That enables us to use different coordinatesduring different contact situations and hereby to use minimal coordinates [6].To doso, a hybrid state vector (xc, xd), which consists of the continuous state vector

xc = (qT , qT )T Rn

and the discrete statexd Z

, is defined. Depending on the discrete state the continuous differential equationsfor the state vector are of a different form.

x = f(xd, xc)

To define the discrete state, guard conditions are needed which tell us when a dis-crete transition takes place (in our case the event when the foot touches/leaves the

Chapter 2. Model 6

ground). Furthermore we need reset conditions for our state vector (e.g. the ve-locity in vertical direction xz = 0 at touchdown). This method is mainly used insimulations with few interaction points between body and ground ([6]).

Soft Contact: The soft contact is in our case modeled as a spring damper systembetween the first touchdown point of the foot on the floor and the foot itself (Figure2.2). Since the force is applied between the initial position of the foot and the actualposition, this method is also called penalty method. For the spring damper systemas well linear and non-linear systems were used.

Figure 2.2: Soft Contact modeling in 2D

The non-linear form of the system is mainly used to prevent from two major weak-nesses the system has with linear spring damper systems. The first problem is thediscontinuity of the forces at impact. Whereas the force generated by the springis zero right after the impact, the force generated by the damper then is at itsmaximum. Physically, the force should grow continuously starting at the momentof impact. The second problem occurring are the so called sticking forces. Thesticking forces basically are the opposite of the first problem mentioned above; theyarise when the foots velocity points in positive y-direction (Figure 2.2). In this casea force in negative z-direction is possible or even bound to happen shortly beforethe foot leaves the ground. The sticking forces are tensile forces holding the footand the ground together, which is physically not correct. To prevent these two phe-nomena from happening, different approaches of non-linear spring damper systemscan be found in the literature. Because both problems mainly affect the forces invertical direction, we well look here solely at spring damper system with a linearpart in the horizontal directions and a non-linear part in the vertical one. Two ofthem are presented below.

[5] and [6] choose a spring damper system where the damper is also dependenton the displacement of the foot.

fy = (xn)x kxn , (2.1)where n is often close to one and is depending on the geometry of the surface.Easily it can be seen that this approach suppresses the problems mentionedabove. The drawback of this approach is that the system gets more complex

7 2.5. Architecture

and more uncertain parameters will have to be found to make it work.

[3] and [4] choose a different kind of non-linear spring damper system of theform

fy =

{kyy dy y if kyy dy y 00 else

. (2.2)

As can be seen this definition of the forces also gets rid of the sticking forces,but the discontinuity at impact is still given. The big advantages of this ap-proach are first its simple structure - no variables or multiplications are added- and second, the fact that a lot of literature is available where this methodwas successfully implemented.

Additional to the two fundamental concepts mentioned above there are also somealternative contact models existing using a mixture between soft and hard contactmodels [7]. A hard contact model is used to describe the impact whereas the overallbehavior is modeled as a compliant model. In this work these possibilities wont bediscussed.

2.5 Architecture

The main goal of the general architecture was that it should allow the implemen-tation of different controllers for the same model. Because of that, the idea was toseparate the control algorithm from the model (Figure 2.3). The discrete controllerreceives the entire state vectors q and dq from the model at a constant time intervalt and sends back the calculated torques T.

Figure 2.3: General architecture

The state variables q and dq consist of

q = (qb, qr) R18

dq = (dqb, dqr) R18

Chapter 2. Model 8

where qb is the 1x6 base coordinate vector. The entries qb(1), qb(2) and qb(3) arethe x,y and z-coordinates of the position of the CoG of the main body. The entries4 to 6 are used to describe the rotation of the quadruped about the according axesof the global coordinate system with Euler angles. The vector qr contains the jointangles as a 12x1 vector, where the first three entries are the angles for hip abduction,hip flection and knee flection of the first leg (front-left, Figure 2.1), the next threefor the second leg, and so on. The vector dq has the same structure as q, exceptthat its entries stand for the velocities, respectively the rotation rates. The inputvariable T consist of the torque vector, which has the same structure as the vectorfor the joint angles:

T R12This separation of control algorithm and model enables us, to use the same controllerto control the robot in the simulation and in reality, since the same in- and outputsare available. By defining the parameters of the model in an external file, the modelcan easily be adapted or modified. The control parameters and the utilities will bediscussed in Section 3.2.

2.5.1 2D ModelBefore creating the 3 dimensional model of the quadruped, a simpler 2 dimensionalone of a single leg was created. Like that it was possible to get used to the SimMe-chanics toolbox by implementing the ground contact model on a smaller scale first.On top of that it was possible to get a first insight about how to choose the rightparameters.

Like in the final simulation, the model was split into two parts, the physical model-ing with SimMechanics and the control algorithm as an embedded function. Figure2.4 shows the SimMechanics model in a simplified version.

Figure 2.4: Simplified Model of the 2D leg

The red arrows show the state variable consisting of the joint angles, joint velocity,vertical position and vertical velocity, whereas the green arrows show the input sig-nals to the actuators in the joint. To get the state variables from the model and

9 2.5. Architecture

to be able to apply the desired torques, additional blocks in the model are needed.Figure 2.5 shows the knee joint in detail with the additional sensor, actuator andinitial condition blocks.

Figure 2.5: Knee joint with sensor, actuator and initial condition blocks.

In the blocks for the hips, shanks or thighs the properties of these body parts (i.e.mass, length, inertia, etc.) are assigned. Also in these blocks, the definition ofthe coordinate systems was done. It was crucial to stick strictly to the predefinedorder of coordinate systems defined in Section 2.2. The initial conditions, as wellas the parameters for the body parts and the ground contact model, are definedin the parameter file. The ground contact model itself was designed based on thebouncing ball MATLAB tutorial as a separate block. Details to the ground contactmodel can be found in the Section 2.6.

2.5.2 3D ModelAs introduced in Section 2.5 the model was split in two parts: the physical modeland the control function. In Figure 2.6 this distinction can be seen.

Figure 2.6: Structure of 3D model.

The model in 3 dimensions was build using the already built model for a single foot.For that matter it was necessary to allow motion and calculate the forces in thethird dimension. Further, the robot had to be composed by combining 4 legs, add

Chapter 2. Model 10

the physics of the main body, and implement the hip abduction joints between themain body and the legs. In Figure 2.7 this structure can be seen.

The block subsystem containes the whole model of the quadruped. On a first levelthe connection between the main body and the environment and the connectionbetween the main body and the legs was modeled (Figure 2.7). Again the inputtorques were colored green and the output state variables were colored red.

Figure 2.7: Quadruped model - Main body level.

On the next level the dynamics of the legs (cian blocks in Figure 2.7) were definedin a similar way as shown in the 2 dimensional example (Figure 2.8).

Figure 2.8: Quadruped model - Leg level.

2.6 Ground ContactWe decided to use a soft contact model of the form

11 2.6. Ground Contact

fx = kx(x x0) dxx (2.3)

fy = ky(y y0) dy y (2.4)

fz =

{kzz dz z if kzz dz z 00 else

. (2.5)

To implement this approach in the model, we needed to hand over the actual positionof the foot and the velocity of the foot to the ground contact subsystem. By checkingthe vertical position, two different discrete states were defined:

State =

{Contact if z 0No contact else

. (2.6)

If the discrete state was No contact, the ground reaction force immediately wasdefined as zero. In the case Contact the resulting forced had to be calculated.While the hight in vertical direction always was compared with zero (the hight ofthe ground), the forces in the horizontal direction were compared with the touch-down point. For that, first the touchdown point was stored (Figure 2.9). Oncecalculated a saturation block was used to make sure the force in vertical direction isgreater than zero. The last step was to hand back the discrete state to the model,since this variable is also used in the control algorithm.


There occurred two main difficulties while implementing this ground contact model.The first one was to find the right parameters to get the desired results. Only acertain penetration dept was wanted and the damping rate shouldnt be to higheither. In addition to that, when the spring and damping constants were chosentoo high, the differential equations got very stiff, which caused the next problem:to find the right solver. The same parameters can result in completely different be-haviours just by changing the solver used. It was even possible to observe that withone solver the simulation terminates, and with another one singularities occurre.The best solver found was the ode15s solver, which is specially fitted for ordinarydifferential equations which are stiff.

Chapter 2. Model 12

2.7 VisualizationThe visualization of the model was done automatically by MATLAB SimMechanics,another great advantage of this toolbox. The model in its intial configuration inflight phase can be seen in Figure 2.10.


Chapter 3

Controller

The second big part of this work was the implementation of a control algorithmwhich stabilizes the quadruped while trotting. In a trotting gait the left forelegand the right hind leg touch the ground in unison, followed by a flight phase whereall four legs are in the air, followed by the stance phase of the remaining two legs(Figure 3.1).

Figure 3.1: Different phases of a trotting gait. FL = Front Left, FR = Front Right,RL = Rear Left, RR = Rear Right

Since trotting is a dynamic gait and the support polygon in stance phase was re-duced to a 1 dimensional line connecting the two feet, a simple static locomotioncontrol or a ZMP (Zero Moment Point) based control strategy was not possible.We were looking for a control algorithm for dynamic locomotion, which consideredactuation and sensor feedback, so another literature research was necessary.

3.1 Literature ResearchThe literature research turned out to be more time consuming than expected, be-cause there are many papers about legged locomotion control algorithms, but onlya few examples of trotting quadrupeds where the control strategies are easily ac-cessible. Most of the papers found were dealing with four legged static locomotionwhere always at least three legs are in contact with the ground. Since we werelooking for a robust, but also a simple algorithm, the work of M. H. Raibert turnedout to be highly interesting ([8], [9]).

3.1.1 Raibert ControlIn [8] a one legged robot which contained of a springy leg and a gimbals-type hip,which allowed to control the orientation of the leg, was examined (Figure 3.2).The advantage of this model is that it combines the relatively simple dynamics of

13

Chapter 3. Controller 14

a spring-mass model with additional control possibilities. For further informationabout the spring-mass theory, please refer to [10]. Later in his research, he extendedthese principles to control the locomotion of a four legged robot, which makes itinteresting for our project.

Figure 3.2: Raibert Mono hopper from [8].

The controller used to stabilize the hopper contained of three parts:

- The velocity in horizontal direction was controlled by an appropriate footplacement during the flight phase. The right position of the foot relative tothe hip at touchdown is important since it controls in which direction thethrust applied in the leg acts. To calculate the right position two factors wereconsidered. First the actual forward velocity. For a constant forward move-ment, spring-mass model theory dictates a symmetrical stance phase. To findthe right touchdown point for a symmetrical stance phase, an estimation forthe next stance phase has to be made by considering the predicted mean for-ward velocity and the predicted stance time (Figure 3.3).

Figure 3.3: Spring-Mass Model during stance phase with s = xTst and x = forwardvelocity and Tst = Duration of stance phase.

15 3.1. Literature Research

The hereby found touchdown point will be called TD in the next subsection.

The second factor considered was the error in forward speed. To be ableto control the forward velocity, the offset of the foot at touchdown relativeto the foot placement calculated before (TD) can be used. By making thestance phase asymmetrical, different behaviours can be caused; E.g. if thefoot is placed in front of TD, the period of time where the mass deceleratesin horizontal direction is smaller than the period where the mass accelerates.Like that an overall acceleration can be caused. Three different scenarios arepossible, illustrated in Figure 3.4. The offset is calculated using the differencebetween the actual forward velocity and the desired one, multiplied with anexperimentally found gain kfoot > 0.

Figure 3.4: Case A: Foot placed in front of TD, the velocity will increase. CaseB: Foot placed at TD, no change in velocity. Case C: Foot placed behind TD, theforward velocity will decrease.

Combining these two factors results in the following equation for the footplacement, valid in x- and y-direction:

xf =xTst

2+ kfoot(x xdes) (3.1)

- The hopping height, respectively the energy niveau, was controlled by theamount of thrust inserted in the legs actuator. This is the most intuitivepart of the controller and hereby only discussed briefly. Meanwhile the footplacement was adjusted during the flight phase, the hopping height has tobe controlled during the stance phase. Since Raibert used in his work aspringy leg, the robot would also hop without insertion of any thrust in theleg. However due to not negligible losses, an additional thrust is necessary tocompensate these. Since these losses are monotonic with the hopping height,a fixed thrust could be found which stabilizes the robot at a certain hoppingheight.

- The balance of the body (attitude) was controlled by applying torques betweenthe leg and the body in stance phase. Since the angular momentum duringthe flight is conserved theres no possibility to control the attitude duringthis phase. The torques applied during stance were calculated considering theactual roll and pitch angles and the velocities. The resulting equation can beseen here:

x = kr(rdes r) dr(r) (3.2)y = kp(pdes p) dp(p) (3.3)


Where is the torque applied about the x- respectively y-axis, stands forthe roll respectively pitch angle and for the angle rate. The parameters kand d had to be found again experimentally.

To adapt the control algorithm used for the one legged robot to a four legged one,Raibert introduced the principle of the virtual leg [9]. The main idea of introducinga virtual leg is to treat two legs as if it were one. In our case treat the two legsFL and RR in stance as if it were only one centered leg VL (Virtual Leg). Thisapproach lets us reducing any quadruped pair gate to better known virtual bipedgaits. In our case even a reduction to an one legged gait is possible (Figure 3.5).

Figure 3.5: The legs FR and RL can be reduced to VL.

To achieve the same behavior with two legs instead of one, some rules for thedefinition of the virtual leg had to be set:

- The amount of axial thrust exerted by each leg should equal half of the axialthrust exerted by the virtual leg.

- The amount of torque applied between each hip and leg should equal half ofthe torque around the virtual hip.

- The feet should hit and leave the ground in unison.

- The horizontal position of each foot relative to its hip should equal the positionof the virtual foot relative to the virtual hip.

Whereas the first two points were important during the stance phase, the latter twodealt with the foot positioning during the flight phase.

By applying these rules, the three-part locomotion algorithm mentioned above couldalso be realized for a four legged robot. The only extension necessary is to includea yaw control in the algorithm. Because the yaw angle was also controlled by thefoot placement, this enhancement only affected the first part of the controller.

To control the yaw angle, a torque about the z-axis has to be applied. To cause atorque about the z-axis, a force tangential to a circle with center at the virtual hipand through the real hips has to be created (Figure 3.6). For that, the feet have tobe misplaced by an angle . The mechanism of this action is the same as can beseen in Figure 3.4.Including the adjustments for the yaw control in equation 3.1 the following finalsolution for the foot placement relative to the virtual hip results:

xf =xTst

2+ k(x xdes) +Dcos( + 0) (3.4)

yf =yTst

2+ k(y ydes) +Dsin( + 0) (3.5)

17 3.2. Implementation of the Controller

Figure 3.6: Top view of forces created by misplacing the legs. The foot (green) ismisplaced by an angle relative to its old position (red).

with 0 = arctan(W/L), D =W 2/2 + L2/2 and L an W are the length and the

width of the main body. was calculated as following:

= ky(ydes y) dy(y) (3.6)

3.1.2 Virtual Model ControlIn contrast to the quadruped Raibert simulated, our model doesnt have prismaticlegs. In order to apply his method nevertheless, we had to use a virtual modeltoconvert the wanted axial forces in the legs into actuator torques in our joints [12].In order to calculate the torques applied in the joint we had to use equation 3.7.

~T = JT ~F (3.7)

With ~F the desired forces andJ =

~x

~qL(3.8)

the Jacobian matrix with ~qL = [qHAA, qHFE , qKFE ]T the joint angles for hip abduc-tion, hip flection and knee flection and ~x = [x, y, z]T the vector from the hip to thefoot as function of the joint angles ~qL in the local coordinate system. To transformthe position of the foot with respect to the hips from cartesian coordinates to bodyjoint angles, inverse kinematics were used.

3.2 Implementation of the ControllerTo implement the algorithm mentioned above it was split in different MATLABfiles. While the main control was implemented in one file, some parameters andsome important utilities were distributed in several other files. By using the exist-ing utilities a new algorithm can be included more simply in the existing environ-ment. The parameters stored in the external parameters files are the ones used byevery controller, such as initial conditions etc.. The parameters used in the controlalgorithm were stored in the same file as the algorithm itself.


3.2.1 General ArchitectureThe control algorithm itself was divided in 6 parts:

- Definition of persistent variables which must be available during the wholesimulation.

- Two different flight phases.

- Two different stance phases.

- Calculation of the torques according to the forces calculated earlier.

In the following, the defined persistent variables wont be discussed explicitly.

The definition of four different phases was important since we are dealing witha hybrid system and the control algorithm looks completely different for the twodiscrete states flight or stance. The stance phase has to be further distinguishedbetween stance FLRR (front left leg and rear right leg are in contact) and stanceFRRL (front right leg and rear left leg are in contact). Here the differences in thecontrol algorithm are limited to the annotation of the legs but are still important.The same differentiation has to be made for the flight phase; its a different be-havior for a leg whether it prepares for a stance phase or not. For making thisdifferentiation, a discrete state variable which can have the states stanceFLRR,stanceFRRL, flightFLRR and flightFLRR (Figure 3.7) was introduced. Thisvariable was named ctrl.phase and was of course a persistent one. Depending onthe discrete state, the according control actions were taken as discussed in the pre-vious section.

Figure 3.7: Different discrete states during simulation.

The moment of transition from one discrete state to another is defined by the guardconditions G. The transition requirement from stance to flight phase is

G(stance, flight) = KFE,stance < 0.9 Fz,stance < 0 isContactstance == 0 .(3.9)

In words this means, if either the knee angle of a leg in stance KFE,stance is smallerthan 0.9 rad (i.e. extended to a desired value), the vertical force applied to the leg


is negative or one of the stance legs lost contact with the ground, a transition istaken and the discrete state switches to flight. The guard condition to switch fromflight phase to stance phase looks like

G(flight, stance) = any(KFE,stance 1)any(isContactstance == 1) . (3.10)

In words, if any of the legs preparing for stance phase is in contact and one ofthe legs preparing for stance phase has a knee flection angle greater or equal to 1rad, the transition is taken. Both guards were treated with an as-soon-as semantic,which means that as soon as a transition was possible it was taken.

3.2.2 Force Calculation

To calculate the forces acting on the legs, one has to differentiate between legsin contact with the ground and legs not in contact with the ground. The actuatortorques of the legs which have no interaction with the ground are calculated directlyby comparing the desired joint angles and velocities with the actual ones, i.e. a PD-Position control was applied.

T = kq (qDes qr) + dq (dqr) (3.11)

The parameters kq and dq are experimentally found gains. qDes is the desired angleand qr and dqr are the actual joint angles and rotaion rates.In order to get a spring-mass system similar to the one simulated by Raibert, theaxial force exerted by the leg is calculated by assuming a spring between the virtualhip and the virtual foot. Whereas the virtual hip is simply the position of themain bodys CoM, the position of the virtual foot had to be calculated first. Sincethe position of each leg relative to its hip should be the same in order to fulfillthe conditions for the virtual leg, the position of the virtual foot was calculated asthe middle between the two legs in stance. The position of the virtual hip at thebeginning of the stance phase was used to calculate the initial length of the springwhich stayed constant throughout the whole phase, however the actual length wascalculated continuously as the distance between the virtual hip and the virtual foot.The resulting force was:

F = k (||~r0|| ||~r||) (3.12)

Where ~r0 = V irtualHiptouchdownV irtualFoot and ~r = V irtualHipV irtualFoot.Since were only interested in the length of the spring and not in the direction, thenorm of the vectors was taken. To calculate the direction of the force we simplyhad to point the force in the direction of the spring, since only axial thrusts weredesired.

~F = k (||~r0|| ||~r||) ~r||~r|| (3.13)

k = 12000[N/m] was found experimentally by looking for a realistic behavior of thequadruped.

In the controller, the forces were calculated with the function getVirtForceAtCoGfor all 4 legs. Because of that, in the next step we had to choose to which legs theseforces are assigned according to the discrete state and then calculate the actuatortorques with the virtual model introduced in 3.1.2. Since the forces are chosen in away that they point in the direction of the legs, the choice of the coordinate systemwas not important yet as long as it was constistent.


3.2.3 Forward Velocity Control

In this subsection the implementation of the correct foot placement is discussed. Toplace the foot on the right position, first the vector between the hip and the foot iscalculated in world coordinates. This position gets transformed in the local coor-dinate system, out of which it is possible to calculate the joint angles with inversekinematics.

The foot placement on the one side controls the forward velocity and the yaw angleof the quadruped, on the other side it is also in charge to make sure that the rules ofthe virtual leg are not violated. In other words, the foot has to be placed in a waythat the horizontal displacement relative to the hip of the foot hitting the groundis equal for all the legs and that the legs hit the ground in unison. The equal dis-placement is achieved by simply calculate the displacement for the virtual foot andthen assign it to the individual ones. To hit the ground in unison, the displacementof each foot relative to the virtual hip in vertical direction has to be the same. Veryimportant here is, that the relative displacement in vertical direction is measuredand applied in world coordinates (Figure 3.8)! Like that, the z-coordinate of ourfoot in world coordinates was set.

Figure 3.8: Same displacement in z-direction in local (left) and world coordinateframe (right).

For the forward velocity control, the desired foot position was again adjusted inworld coordinates. If we would have done that in local coordinates, the positioningin horizontal direction would as well affect the vertical position in the world frame.For the displacement relative to the hip we had to consider the actual forward veloc-ity in main body coordinates, the desired forward velocity and the duration of thestance phase. While the forward velocity was handed over from the model and justhad to be transformed into main body coordinates and the desired forward velocitywas given, the duration of the stance phase had to be identified. On that accountthe time between the discrete transitions from flight to stance and from stanceto flight was measured each time the simulation performed these transitions. Sincelike that, the first stance phase couldnt rely on any measured data, an educatedinitial guess had to be made.

3.2.4 Roll and Pitch Control

For the roll and pitch control a slightly different approach was chosen than the oneintroduced by Raibert, the main principle allthough stayed the same: In order tobalance the roll respectively pitch angle a momentum about the according axis hadto be applied during the stance phase. This momentum was caused by additional


vertical forces applied in the hips of the stance legs. In case of pitch control, thisforce was depending on the actual pitch angle and the pitch angle rate in main bodycoordinates. Why the body fixed coordinate system was chosen becomes clear whenyou revolve the quadruped around the z-axis for pi/2. In that case the roll angle inthe world coordinates describes the pitch angle in body fixed coordinates and viceversa. The desired force calculated itself as:

Fpitch = kpitch pitch + dpitch pitch (3.14)Froll = kroll roll + droll roll (3.15)

The forces found were added to the forces calculated in Section 3.2.2.

Figure 3.9: An added force creates a momentum about both axis.

Unfortunately, if you just add vertical forces you will also cause a momentum aboutthe axis you dont want to influence. In Figure 3.9 an example is shown: We as-sume that we want to correct an existing positive pitch angle by adding respectivelysubtracting forces to the stance legs. We succeed in creating a momentum aboutthe y-axis and will probably be able to correct the pitch angle. Unfortunately alsoa momentum about the x-axis arises which causes an unwanted roll movement.

Figure 3.10: An added torque equalizes the created momentum about the roll axis.

To counteract this unwanted momentum, an additional torque was added to the hipswhich equalized the unintended angular acceleration (Figure 3.10). The quantitycould easily be calculated:

Mpitch = bmb F/2 (3.16)


Mroll = lmb F/2 (3.17)If a Force F created to act against a pitch movement is applied, a Torque Mpitchmust be applied about the x-axis to equalize the angular momentum. The sameholds for compensating the momentum caused by a force acting against a roll move-ment and the y-axis, see equation 3.17. The torques are applied between the legand the hip and were directly added to the hip abduction respectively hip flectionactuator torques calculated earlier.

To spot the right parameter was crucial for this method to work. Only a verysmall range of values provided a satisfying result. Since at the beginning no sat-isfying results were found a different attempt with a quadratic approach for theangular velocity was tried. With the quadratic approach it was easier to find valueswhich kept the quadruped trotting for a few secondes, but no good values whichstabilized the robot could be found; so we went back to the linear approach (seemore in the Section 4). The values found to be the best fitted are kpitch = 20,kroll = 15, dpitch = 13 and droll = 7.

3.2.5 Energy ShapingSince the system only contains of a spring-mass like model trotting on a groundmodeled as a spring damper system, the system constantly loses energy. To actagainst this phenomena an additional force in vertical direction was added, whichwas based on a simplified energy shaping approach. The energy shaping methodassures that always the same amount of energy is in the system. To do so, the po-tential and the kinetic energy always were calculated at touchdown. The rotationalenergy and the kinetic energy of the legs were hereby neglected, since theyre verysmall compared to the total energy.

E = m g z + m2v2 (3.18)

The first calculation served as an initial value for the system. Later measurementsgot compared with the initial one and the difference is calculated, to determine thefortitude of the forces added.

FE = E (||~r0|| ||~r||) c , (3.19)

with E the calculated energy difference and c an experimentally determined con-stant. The force was added to the hips in stance phase in vertical direction.

Chapter 4

Results

4.1 General Results

The results presented in this section are mainly about stabilizing the robot aroundthe equilibrium point x = y = pitch = roll = 0. Therefore the reaction of thecontroller on several different disturbances was observed. To optimize the controllerand especially the parameters used, two different approaches were used. First qual-itative judging was done: By looking at the simulation you could easily tell whetherit was stable or not. This approach was used in the early phase of the optimization,when the scales of the parameters werent known yet and quite often the model wasunstable and crashed. The hereby found parameters already delivered satisfyingresults, it was possible to trot without forward velocity and also disturbances weresuccessfully corrected. In the Figures 4.1, 4.2, 4.3 and 4.4 you can see how themodel reacts on disturbances in certain directions. Figure 4.1 shows the roll angleover time, with an initial disturbance of 0.1 rads. You can see that the systemstabilizes pretty well. In the Figure 4.2 the pitch angle is shown with a disturbanceof 0.1 rads, in 4.3 the velocity in x-direction and in 4.4 the velocity in y-direction,always with a disturbance of 0.1 m/s. The parameters used here were:

Parameter Empirical Resultsdpitch 13kpitch 20droll 7kroll 15kfoot 0.044

For the complete results with all the graphs, please consult appendix C.

Once satisfying values were found with this method an Eigenvalue Analysis wasdone to optimize specific parameters further.

4.2 Eigenvalue Optimization

We decided to look at the parameters for the pitch and the roll control, kpitch, dpitch,kroll and droll. The system was linearized around the equilibrium point x = y =pitch = roll = pitch = roll = 0. To optimize the stability, the six state vari-ables mentioned above were considered, the yaw angle and the position of the mainbody, as well as the vertical velocity were not considered here. The two discretestates regarded were defined as the initial position and the after next apex height

23

Chapter 4. Results 24

Figure 4.1: Disturbance in the roll angle of 0.1 rad.

Figure 4.2: Disturbance in the pitch angle of 0.1 rad.

Figure 4.3: Disturbance in the forward velocity of 0.1 m/s.

25 4.2. Eigenvalue Optimization

Figure 4.4: Disturbance in the sidewards velocity of 0.1 m/s.

(Figure 4.5). Like that it was assured that the model was in the same discrete stateagain and the two positions are comparable.

Figure 4.5: Only every other apex point is considered.

Since we linearized around the zero point, the problem could be stated as:

xk+1 = A xk (4.1)

A is hereby the linearization matrix and x is a vector containing the differenceof our state variables to our equilibrium point. If the eigenvalues of A have amagnitude smaller than 1, the system is considered to be stable. The smaller theeigenvalues the better, so the goal of the optimization was to get the eigenvalues assmall as possible. We were looking for the configuration where the spectral radiusis the smallest.

To be able to test a big amount of different parameters a little script was writ-ten which ran the simulation under different condition and calculated the eigenval-ues independently. The best configuration found after several loops was with theparameters:


Parameter Empirically found parameters After EV - optimizationdpitch 13 3kpitch 20 20droll 7 2kroll 15 8 2.5760 0.9349

The biggest Eigenvalue with these parameters was max = 0.7719+0.5274i. As canbe seen from the results above, since the biggest Eigenvalue has a magnitude smallerthan one, the system is considered to be stable. Running the simulation with thesevalues with a disturbance of 0.1 rad in the pitch angle, delivers the results shownin the Figures 4.6 and 4.7.



As can be seen in the graphs, the angles still tend to oscillate heavily. To fix thisissue, we also tried to optimize the parameters for the forward velocity control kfoot(see Section 3.2.3). Since also with the optimization of the parameters for the for-ward velocity no better values could be found, we went back to using the parametersempirically found, which deliver by far the best results.

27 4.3. Forward Trotting

There might be several reason why the eigenvalue analysis didnt work out. Firstof all the equilibrium point is not validated: Even though the system stays stablewhen starting at this point, a slight oscillation can still be observed. Other reasonscould be the insufficient accuracy of the solvers and time steps used or the too biginterval of linearization.

4.3 Forward TrottingWith the parameters found in the section 4.1 also the behaviour while forward trot-ting was examined. The fastest stable velocity in forward direction was found tobe around 0.2 m/s. At this speed the control of the quadruped was still possible,eventhought with a certain bias. In Figure 4.8 a graph is illustrated where you cansee the forward velocity plotted over time. The controller was set to stay at zerovelocity for 2 seconds, then walk forward with a speed of 0.2 m/s for 10 seconds andthe go back to zero velocity. Since the yaw angle was not controlled, the velocityin y-direction was controlled in a way that it keeped on walking in a straight linewith respect to the world coordinate system.

Figure 4.8: Forward trotting for 10 seconds.

A big oscillation of the forward velocity can be observed, but it can be explained asthe natural behaviour of a spring mass model. Unfortunately, on top of this fluc-tuation also a quite big drift is observable, the longer the quadruped trots forward,the higher the velocity.

Chapter 5

Interpretation

5.1 Model

The model worked reliable during the simulations, and the simulation time wasreasonable. It was made simple to implement several different controllers to themodel, so the overall judgment was positive. Some points that might be improvedcan be seen in the next subsection.

5.1.1 To do

Due to the fact that a lot of simulations with different parameters were done, thesimulations tolerance was held pretty high in order to keep the simulation timeshort. To make more diagnostically conclusive simulations, the tolerance has to beadjusted more precicly, which would on the other hand encrease the simulation timedrastically.

Another point of the model which might still be improved is the ground contactmodel. Since a simple approach for the nonlinear spring damper system was chosenhere, the discontinuities of the ground contact forces at impact are still existent.With a different approach, as it was introduced in Section 2.4 this inaccuracy mightbe inhibited.

5.2 Controller

The performance of the controller was satisfying, specially because it is at the sametime a very simple, but still robust controller. The implementation of the Raibertthree part controller was possible with a reasonable amount of work. The biggestproblem that occured was the determination of the best fitted parameters for thecontrol algorithm.

The parameters found empirically stabilize the model very good in case of theroll, pitch and altitude control. The model can be held in a stable position for anarbitrary long time and it reacts appropriate on disturbances and brings the modelback to the equilibrium point within only a few steps.

The forward velocity control worked satisfying. The model can be kept in thesame position for a long time and only a small drift of the position can be observed.Unfortunately the control of the velocity when trotting forward might still be im-proved. There a visible difference between the desired and the actual velocity can

29

Chapter 5. Interpretation 30

be observed.

The yaw control was not implemented during this work.

With the eigenvalue analysis, satisfying parameters could not be found. Possiblereasons for this failure are listed in section 4.2.

5.2.1 To doAs a first step I would suggest to implement the yaw control. The implementationof the yaw control is already done to a certain point and a completion of this algo-rithm seems promising, since its based on the same method as the forward velocitycontrol. Again, enough time should be calculated for the determination of the rightparameters.

The completion of the control for a stable forward trotting might also be donein a reasonable amount of time and also seems very promising. The biggest prob-lem one might face there, are the occurring additional momentums related with thefoot placement, which might make an additional control component inevitable.

Chapter 6

Acknowledgment

With the completion of this project, I like to express my sincere thanks to all personswho contributed to this work. First of all Id like to thank my supervisors MarcoHutter and David Remy for their competent support at all times throughout thewhole work. I specially appreciate their flexibility in rescheduling my work due tomy temporary disability. I also like to thank Roland Siegwart and the AutonomousSystem Lab for making this project possible and allowing me to get new insightsand knowledge in the field of robotics.

31

Chapter 6. Acknowledgment 32

Appendix A

Files

The final simulation consited of the following files:

- ScarpETH.mdl: In this file the complete model is defined, inclusive groundcontact forces and wrapper for the control algorithm.

- ctrl_VM_trot_dn.m: In this file the control algorithm is defined, inclu-sive all the parameters needed for the controller, such as initial guess for thestance time duration, several gains for the pitch, roll and forward velocity con-trol, constants for the energy shaping method, desired foot positions duringthe flight phase, etc.. The explaination to the variables and function executedare written as comments in the file.

- param_all_dn.m: This is the file to call the parameter files param_mech_dn.mand param_ctrl_dn.m.

- param_mech_dn.m: In this file the physical properties of the quadruped(as introduced in section 2.2) are described. Furthermore also the parametersfor the ground contact model and the physical enviroment are defined here.

- param_ctrl_dn: In this file the initial condition for the simulation aredefined. Further the sampling frequenz of the controller is defined here.

- The utilities folder: In this folder external function for coordinate transfor-mations and inverse kinematics are stored. These utilities were implementedby M. Hutter and were only slightly adapted.

33

Appendix B

How to use

To run the simulation with the implemented controller these steps have to be fol-lowed:

1. Open the file ScarpETH.mdl.

2. Open the file param_all_dn.m and press execute.

3. Press the Start simulation button in the model ScarpETH.mdl.

To implement your own controller in the model follow these steps:

1. Open the file ScarpETH.mdl.

2. Open the block ctrlWrapper. (colored in magenta)

3. Follow the instruction in line 5 and 30.

4. Open the file param_all_dn.m and press execute.

5. Press the Start simulation button in the model ScarpETH.mdl.

34

Appendix C

Results

For reasons of completness, here are the graphs for the roll and pitch angle, aswell as forward and sidewards velocity reacting to different disturbances with theparameters introduced in Section 4.1.

Figure C.1: Disturbance in the roll angle of 0.1 rad.


35

Appendix C. Results 36



Figure C.5: Disturbance in the pitch angle of 0.1 rad.

37





Figure C.9: Disturbance in the forward velocity of 0.1 m/s.



39


Figure C.13: Disturbance in the sidewards velocity of 0.1 m/s.


Bibliography

[1] MathWorks Official Homepage: http://www.mathworks.com/products/simmechanics/

[2] P. Omlin, C. D. Remy, M. Hutter: Implementation of Optimal ControlStrategies for a Hopping Leg, 2010.

[3] M. Silva, J. A. Tenreiro Machado, I. S. Jesus: Modeling and simulationof walking robots with 3 DOF legs, Institute of Engineering Porto, 2006.

[4] M. Silva, J. A. Tenreiro Machado, A. Lopes: Modelling and simulationof artificial locomotion systems, Cambridge University Press, 2005.

[5] D. W. Marhefka, D. E. Orin: Simulation of Contact Using a NonlinearDamping Model, International Conference on Robotics and Automation, 1996.

[6] M. Sobotka: Hybrid Dynamical System Methods for Legged Robot Locomotionwith Variable Ground Contact, Lehrstuhl fr Steuerungs- und RegelungstechnikTechnische Universitt Mnchen, 2007.

[7] J. K. Mills, C. V. Nguyen: Robotic Manipulator COLLISIONS: modellingand Simulation, ASME Journal of Dynamic Systems, Measurement and Con-trol, 1992.

[8] M. H. Raibert, H. B. Brown, Jr. M. Chepponis, E. Hastings, J.Koechling, K. N. Murphy, S. S. Murthy, A. J. Stentz: DynamicallyStable Legged Locomotion, The Robotics Institute and Department of Com-puter Science Carnegie-Mellon University Pittsburg, 1983.

[9] M. H. Raibert: Trotting, Pacing and Bounding by a Quadruped Robot, Artifi-cial Intelligence Laboratory, Massachusetts Institute if Techology, Cambridge,1990.

[10] H. Geyer: Simple Models of Legged Locomotion based on Compliant LimbBehavior, Fakultt fr Sozial- und Verhaltenswissenschaften, Friedrich-Schiller-Universitt, Jena, 2005.

[11] L. R. Palmer III: Intelligent Control and Force Redistribution for a high-speedQuadruped Trot, The Ohio state university, 2007.

[12] A. Lauber: Virtual Model Control on ALoF, ETH Zurich, 2011.

41

AbstractSymbolsIntroductionAbout the ProjectTask

ModelThe ModelThe QuadrupedSimMechanicsLiterature ResearchHard Contact versus Soft Contact

Architecture2D Model3D Model

Ground ContactVisualization

ControllerLiterature ResearchRaibert ControlVirtual Model Control

Implementation of the ControllerGeneral ArchitectureForce CalculationForward Velocity ControlRoll and Pitch ControlEnergy Shaping

ResultsGeneral ResultsEigenvalue OptimizationForward Trotting

InterpretationModelTo do

ControllerTo do

AcknowledgmentFilesHow to useResultsBibliography

Quadruped Walking Running Simulation

Documents

Transcript of Quadruped Walking Running Simulation