Motion Control Systemsprasadpawaskar.weebly.com/uploads/6/0/2/9/6029691/... · Sˇabanovic´, Asif....

MOTION CONTROLSYSTEMS

Motion Control Systems, First Edition. Asif SŠabanovic and Kouhei Ohnishi.

© 2011 John Wiley & Sons (Asia) Pte Ltd. Published 2011 by John Wiley & Sons (Asia) Pte Ltd. ISBN: 978-0-470-82573-0

MOTION CONTROLSYSTEMS

Asif SabanovicSabancı University, Turkey

Kouhei OhnishiKeio University, Japan

This edition first published 2011

� 2011 John Wiley & Sons (Asia) Pte Ltd

Registered officeJohn Wiley & Sons (Asia) Pte Ltd, 2 Clementi Loop, # 02-01, Singapore 129809

For details of our global editorial offices, for customer services and for information about how to apply for permission

to reuse the copyright material in this book please see our website at www.wiley.com.

All Rights Reserved. No part of this publication may be reproduced, stored in a retrieval system or transmitted, in any

form or by anymeans, electronic, mechanical, photocopying, recording, scanning, or otherwise, except as expressly permitted

by law, without either the prior written permission of the Publisher, or authorization through payment of the appropriate

photocopy fee to the Copyright Clearance Center. Requests for permission should be addressed to the Publisher, JohnWiley&

Sons (Asia) Pte Ltd, 2 Clementi Loop, #02-01, Singapore 129809, tel: 65-64632400, fax: 65-64646912, email:

[email protected].

Wiley also publishes its books in a variety of electronic formats. Some content that appears in print may not be available

in electronic books.

Designations used by companies to distinguish their products are often claimed as trademarks. All brand names and product

names used in this book are trade names, service marks, trademarks or registered trademarks of their respective owners.

The Publisher is not associated with any product or vendor mentioned in this book. This publication is designed to

provide accurate and authoritative information in regard to the subject matter covered. It is sold on the understanding

that the Publisher is not engaged in rendering professional services. If professional advice or other expert assistance is

required, the services of a competent professional should be sought.

MATLAB� is a trademark of The MathWorks, Inc. and is used with permission. The MathWorks does not warrant the

accuracy of the text or exercises in this book. This book’s use or discussion of MATLAB� software or related products does

not constitute endorsement or sponsorship by The MathWorks of a particular pedagogical approach or particular use of the

MATLAB� software.

Library of Congress Cataloging-in-Publication Data

Sabanovic, Asif.

Motion control systems / Asif Sabanovic.

p. cm.

Includes bibliographical references and index.

ISBN 978-0-470-82573-0 (hardback)

1. Motion control devices. I. Title.

TJ214.5.S33 2011

621.4–dc22

2010041054

Print ISBN: 978-0-470-82573-0

ePDF ISBN: 978-0-470-82574-7

oBook ISBN: 978-0-470-82575-4

ePub ISBN: 978-0-470-82829-8

Set in 10/12pt Times by Thomson Digital, Noida, India.

Contents

Preface ix

About the Authors xi

PART ONE – BASICS OF DYNAMICS AND CONTROL

1 Dynamics of Electromechanical Systems 3

1.1 Basic Quantities 3

1.1.1 Elements and Basic Quantities in Mechanical Systems 3

1.1.2 Elements and Basic Quantities in Electric Systems 5

1.2 Fundamental Concepts of Mechanical Systems 7

1.2.1 The Principle of Least Action 7

1.2.2 Dynamics 8

1.2.3 Nonpotential and Dissipative Forces 9

1.2.4 Equations of Motion 10

1.2.5 Properties of Equations of Motion 14

1.2.6 Operational Space Dynamics 18

1.3 Electric and Electromechanical Systems 20

1.3.1 Electrical Systems 20

1.3.2 Electromechanical Systems 21

1.3.3 Electrical Machines 24

References 27

Further Reading 27

2 Control System Design 292.1 Basic Concepts 30

2.1.1 Basic Forms in Control Systems 31

2.1.2 Basic Relations 35

2.1.3 Stability 36

2.1.4 Sensitivity Function 37

2.1.5 External Inputs 38

2.2 State Space Representation 39

2.2.1 State Feedback 40

2.2.2 Stability 44

2.2.3 Observers 45

2.2.4 Systems with Observers 48

2.2.5 Disturbance Estimation 49

2.3 Dynamic Systems with Finite Time Convergence 51

2.3.1 Equivalent Control and Equations of Motion 52

2.3.2 Existence and Stability 53

2.3.3 Design 53

2.3.4 Control in Linear Systems 55

2.3.5 Sliding Mode Based Observers 56

References 59

Further Reading 59

PART TWO – ISSUES IN MOTION CONTROL

3 Acceleration Control 63

3.1 Plant 63

3.2 Acceleration Control 67

3.2.1 Formulation of Control Tasks 68

3.2.2 Equivalent Acceleration and Equivalent Force 74

3.3 Enforcing Convergence and Stability 85

3.3.1 Convergence for Bounded Control Input 90

3.3.2 Systems with Finite-Time Convergence 94

3.3.3 Equations of Motion 97

3.3.4 General Structure of Acceleration Control 105

3.4 Trajectory Tracking 107

References 114

Further Reading 114

4 Disturbance Observers 115

4.1 Disturbance Model Based Observers 118

4.1.1 Velocity Based Disturbance Observer 119

4.1.2 Position Based Disturbance Observer 121

4.2 Closed Loop Disturbance Observers 127

4.2.1 Internal and External Forces Observers 128

4.3 Observer for Plant with Actuator 132

4.3.1 Plant with Neglected Dynamics of Current Control Loop 133

4.3.2 Plant with Dynamics in Current Control Loop 136

4.4 Estimation of Equivalent Force and Equivalent Acceleration 140

4.5 Functional Observers 144

4.6 Dynamics of Plant with Disturbance Observer 149

4.6.1 Disturbance Estimation Error 150

4.6.2 Dynamics of Plant With Disturbance Observer 151

4.7 Properties of Measurement Noise Rejection 160

4.8 Control of Compensated Plant 164

4.8.1 Application of Estimated teq and €qeq

167

References 172

Further Reading 173

vi Contents

5 Interactions and Constraints 175

5.1 Interaction Force Control 176

5.1.1 Proportional Controller and Velocity Feedback 178

5.1.2 Environment with Losses 182

5.1.3 Lossless Environment 187

5.1.4 Control of Push Pull Force 191

5.2 Constrained Motion Control 193

5.2.1 Modification of Reference 195

5.2.2 Modification by Acting on Equivalent Acceleration 201

5.2.3 Motion Modification while Keeping Desired Force Profile 205

5.2.4 Impedance Control 209

5.2.5 Force Driven Systems 210

5.2.6 Position and Force Control in Acceleration Dimension 211

5.3 Interactions in Functionally Related Systems 215

5.3.1 Grasp Force Control 215

5.3.2 Functionally Related Systems 225

References 232

Further Reading 232

6 Bilateral Control Systems 233

6.1 Bilateral Control without Scaling 234

6.1.1 Bilateral Control Design 238

6.1.2 Control in Systems with Scaling in Position and Force 247

6.2 Bilateral Control Systems in Acceleration Dimension 251

6.3 Bilateral Systems with Communication Delay 256

6.3.1 Delay in Measurement Channel 257

6.3.2 Delay in Measurement and Control Channels 263

6.3.3 Closed Loop Behavior of System with Observer 267

6.3.4 Bilateral Control in Systems with Communication Delay 270

References 274

Further Reading 274

PART THREE – MULTIBODY SYSTEMS

7 Configuration Space Control 279

7.1 Independent Joint Control 280

7.2 Vector Control in Configuration Space 281

7.2.1 Selection of Desired Acceleration 282

7.3 Constraints in Configuration Space 290

7.3.1 Enforcement of Constraints by Part of Configuration Variables 303

7.4 Hard Constraints in Configuration Space 304

References 311

Further Reading 312

Contents vii

8 Operational Space Dynamics and Control 313

8.1 Operational Space Dynamics 314

8.1.1 Dynamics of Nonredundant Tasks 314

8.1.2 Dynamics of Redundant Tasks 315

8.2 Operational Space Control 318

8.2.1 Nonredundant Task Control 319

8.2.2 Redundant Task Control 328

References 336

Further Reading 336

9 Interactions in Operational Space 337

9.1 Task–Constraint Relationship 337

9.2 Force Control 341

9.3 Impedance Control 345

9.4 Hierarchy of Tasks 347

9.4.1 Constraints in Operational Space 347

9.4.2 Enforcing the Hierarchy of Tasks 352

9.4.3 Selection of Configuration Space Desired Acceleration 357

References 358

Further Reading 358

Index 361

viii Contents

Preface

This book is concerned with the development of an understanding of the design issues in

controllingmotionwithinmechanical systems. There seems to be a never-ending discussion on

what motion control is – a new field or an extension or a combination of existing fields. Despite

this, both industry and academia have been involved in fulfilling real-world needs in

developing efficient design methods that will support never-ending requirements for faster

and accurate control of mechanical motion. High-precision manufacturing tools, product

miniaturization, the assembly of micro- and nanoparts, a need for high accuracy and fidelity of

motion in robot-assisted surgery – in one way or another all these employ motion control.

Looking back at its brief history, the concept of motion control was not well established in

the 1970s and 1980s. Many people still believed that controlling the torque needed for a load

should be achieved through velocity control. However, we found that torque and velocity could

be separately controlled. This was very effective for dexterous motion in robotics. We were

very excited and naturally wanted to announce this interesting finding and create a new field.

Meetings and discussions with other researchers and students encouraged us to create a new

workshop covering the problems of motion control. In March 1990, the first workshop

dedicated only to motion control (the IEEE International Workshop on Advanced Motion

Control – later known as AMCWorkshops) was held at Keio University. To our surprise, there

were more than 100 papers presented at the workshop. Since then, many ideas, concepts and

results have come out. Subsequently, motion control gained visibility and attracted many

researchers. Timeflows very fast and now it is time to summarize the results, particularly for the

new students coming into this field. We hope readers enjoy this book.

The intention of this book is to present material that is both elementary and fundamental,

but at the same time to discuss the solution of complex problems in motion control systems.

We recognize that the motion control system as an entity, separable from the rest of the

universe (the environment of the systems) by a conceptual or physical boundary, is composed

of interacting parts. This allows treatment of simple single degree of freedom systems as

well as complex multibody systems in a very similar if not identical way. By considering

complex motion control systems as physically or conceptually interconnected entities,

design ideas applied to single degree of freedom systems can also be applied with small

changes to complex multibody systems. Material in this book is treated in such a way that

the complexity of a system is gradually increased, starting from fundamentals shown in

the framework of single degree of freedom systems and ending with a treatment for the

control of complex multibody systems. Mathematical complexities are kept to a required

minimum so that practicing engineers as well as students with a limited background in

control may use the book.

This book has nine chapters, divided into three parts. The first part serves as an overview of

dynamics and control. It is intended for those who would like to refresh ideas related to

mathematical modeling of electromechanical systems and control. The first chapter is related

to the dynamics of mechanical and electromechanical systems. It presents basic ideas for

deriving equations of motion in mechanical and electromechanical systems. The second

chapter gives fundamental concepts in the analysis and design of control systems. Design is

discussed for systems with continuous and discontinuous control.

In the second part we discuss fundamentals of acceleration control framework for motion

control systems andgive essentialmethodswhich are used in the third part of the book.Chapter 3

deals with single degree of freedom motion control system with asymptotic or finite time

convergence to the equilibrium. The design is based on the assumption that any disturbance due

to a change in parameters and interaction with environment should be rejected. In the fourth

chapter the design of a disturbance observer and the dynamics of a system with a disturbance

observer is discussed. Chapter 5 discusses the behavior of single degree of freedom motion

systems in interaction with the environment.While rejections of the interaction forces is a basic

requirement in Chapter 3, Chapter 5 considers modification of motion due to interaction. Such a

modification introduces a more natural behavior of the motion control system. The interaction

control is extended to controlling systems that need to maintain some functional relationship,

thus introducing a conceptual functional relationship between physically separated systems.

This serves as a background for a discussion of specific relationships – bilateral control –

discussed in chapter six for systems without and with a delay in the communication channels.

The third part extends the results obtained in part two to controlling fully actuatedmultibody

mechanical systems. Chapter 7 discusses the control of constrained systems in configuration

space and the enforcement of constraints by a selected group of degrees of freedom. In

Chapter 8 control design in operational space is carried out for nonredundant and redundant

tasks. The relationship between task and constraint is discussed and the similarities and

differences between the two are investigated. Chapter 9 discusses problems related to the

concurrent realization of multiple redundant tasks for constrained or unconstrained systems.

Problems in the hierarchy of the execution of multiple tasks are described.

The idea of writing this book stems from a long-term collaboration between the authors. It

began in early 1980s when we met at a conference in Italy and developed during Asif’s stay at

Keio. The book is the result of our discussions and common understanding of problems and

control methods applicable in the field of motion control. Obviously we do not pretend that it is

a final world; rather it is just a beginning, maybe a first step in establishing motion control as a

stand alone academic discipline. Results produced by many other authors are included in the

book in one way or another. Many authors, and especially our students, influenced our way of

treating certain material.

We would like to thank our numerous students, from whom we have learned a lot, and we

hope that they have learned something fromus.Wewould like to express our sincerest thanks to

our families for their support during many years of research and especially during the

preparation of the manuscript.

Asif �Sabanovi�cKouhei Ohnishi

x Preface

About the Authors

Asif �Sabanovi�c is Professor of Mechatronics at Sabancı University, Istanbul, Turkey.

He received undergraduate and graduate education in Bosnia and Herzegovina, at the Faculty

of Electrical Engineering, University of Sarajevo. From 1970, for 20 years he was with

ENERGOINVEST-IRCA, Sarajevo, where he was head of research in sliding mode control

applications in power electronics and electric drives. He was Visiting Professor at Caltech,

USA, at Keio University, Japan, and at Yamaguchi University, Japan. He was Head of the

CAD/CAM and Robotics Department at Tubitak – MAM, Turkey. He has received Best Paper

Awards from the IEEE. His fields of interest include motion control, mechatronics, power

electronics and sliding mode control.

Kouhei Ohnishi is Professor of the Department of System Design Engineering at Keio

University, Yokohama, Japan. After receiving a PhD in electrical engineering from the

University of Tokyo in 1980, he joined Keio University and has been teaching, conducting

research and educating students for more than 30 years. His research interests include motion

control, haptics and power electronics. He received Best Paper Awards and a Distinguished

Achievement Award from the Institute of Electrical Engineers of Japan. He received the

Dr.-Ing. EugeneMittelmannAchievementAward from the IEEE Industrial Electronics Society

(IES). He is an IEEE Fellow and served as President of the IEEE IES in 2008 and 2009.

He enjoys playing clarinet on holidays.

Part One

Basics of Dynamicsand Control

No mathematical representation can precisely model a real physical system. One cannot

predict exactly what the output of a real physical systemwill be even if the input is known, thus

one is uncertain about the system. Uncertainty arises from unknown or unpredictable inputs

(disturbance, noise, etc.), unpredictable dynamics and unknown or disregarded dynamics and

change of parameters. Yet, to design control systems one need a mathematical description of

the physical systems – plants – that will allow the application of mathematical tools to predict

the output response for a defined input, so that it can be used to design a controller. The models

should allow a design which leads to a control that will work on the real physical system. This

limits the details needed to describe the system and the scope of the details wewill be including

in the mathematical models of physical systems.

Generally speaking the objective in a control system is to make some output behave in a

desired way by manipulating some inputs. The output of design is a mathematical model of a

controller that must be implemented. Motion control involves assisting in the choice and

configuration of the overall system or, in short, taking a system view of the overall

performance. For this reason it is important that an applied control framework not only leads

to good and consistent designs but also indicates when the performance objectives cannot be

met. In order to make sense of the issues involved in the design of a motion control system, a

short overview of the control methods for analysis and design are presented in Chapter 2. In

addition to classical frequency and state space methods, systems with finite-time convergence

are treated.



1

Dynamics of ElectromechanicalSystems

In this chapter we will discuss methods of deriving equations of motion for mechanical and

electromechanical systems. We use the term equations of motion to understand the relation

between accelerations, velocities and the coordinates of mechanical systems [1]. For elec-

tromechanical systems the equations of motion, in addition to mechanical coordinates, also

establish the relationship between electrical system coordinates and their rate of change.

Traditionally, introductory mechanics begins with Newton’s laws of motion which relate

force, momentum and acceleration vectors. Analytical mechanics in the form of Lagrange

equations provides an alternative and very powerful tool for obtaining the equations of motion.

The Lagrange equations employ a single scalar function, and there are no annoying vector

components or associated trigonometric manipulations. Moreover, analytical approaches

using Lagrange equations provide other capabilities that allow the analysis of a wide range

of systems.

The advantage of usingLagrange equations is that they are applicable to an extensive field of

particle and rigid body problems, including electromechanical systems, by reducing derivation

to a single procedure while repeating the same basic steps. The procedure is based on scalar

quantities such as energy, work and power, rather than on vector quantities.

In this chapter only basic ideas will be discussed, without detailed and long derivations. Our

goal is to show ways of deriving the equations of motion as a first step for the later design of

control systems. The scope is to show basic procedures and their application to different plants

(mechanical, electrical, electromechanical) often used in motion control.

1.1 Basic Quantities

1.1.1 Elements and Basic Quantities in Mechanical Systems

Mechanics is based on the notion that the measure of mechanical interactions between systems

is force and/or torque (the turning effect of forces):



. Force is related to deformation by material properties (elasticity, viscosity, etc.) and to

motion by the laws of mechanics.. Every action has an equal and opposite reaction.. The net force on a system causes a net linear acceleration and the net turning effect of forces

on a system causes it to rotationally accelerate.. The change of energy of a system is due to energy flowing into the system.

Inmechanics, a body receiveswork from a force or a torque that acts on it if the body undergoes

a displacement in the direction of the force or torque, respectively, during the action. It is the

force or torque, not the body, which does the work. Basic quantities and their relations will be

given here for a rigid body with pure translational motion (with position x and velocity _x ¼ v)

or pure rotational motion (with angular position u and angular speed _u ¼ v).The work done by a force F on a rigid body moving from position x1 along a translational

path G to position x2 is defined byWF12 ¼Ð x2x1

Fdx. Here dx is the differential displacement of

the body moving along the path G.The work done by a torque t on a body during its finite rotation, parallel to t, from angular

position u1 to angular position u2 is given by Wt12 ¼Ð u2u1

tdu, where du is the angular

differential displacement.

The motion of a mass m at the position x is governed by Newton’s second law

Fðx; _xÞ ¼ _p ð1:1Þ

Here Fðx; _xÞ is the force, p ¼ m _x ¼ mv is the linear momentum.

The kinetic energy of nonrotating rigid body with mass m and velocity _x is given by

T ¼ 1

2_xm _x ¼ 1

2vmv ¼ 1

2mv2 ð1:2Þ

The change in kinetic energy with time is dT=dt ¼ _p _x ¼ F _x. The work done by changing

position from x1ðt1Þ to x2ðt2Þ can be expressed as

Tðt1Þ� Tðt2Þ ¼ðt2t1

dT=dtð Þdt ¼ðx2x1

Fdx.

For a conservative force (depending only on positions and not on velocities, thus the work

done is independent on the path taken) the closed path work is zero. This force can appear as a

result of a potentialUðxÞ and can be expressed as F ¼ � qUðqÞ=qq. This property implies the

law of conservation of energy expressed as

Tðt1Þ� Tðt2Þ ¼ðt2t1

�dT

dt

�dt ¼ �

ðq2q1

qUðqÞqq

dq ¼ Uðt2Þ�Uðt1Þ

Tðt1ÞþUðt1Þ ¼ Tðt2ÞþUðt2Þ ¼ E

ð1:3Þ

The number of degrees of freedom of a system is the number of coordinates that can be

independently varied, that is, the number of ‘directions’ a system can move in small

displacements from any initial configuration. A configuration of rigid multibody system

with n degrees of freedom (n-dof) is described by a vector completely specifying the position

4 Motion Control Systems

of each point of multibody system. The set of all admissible configurations is called the

configuration space. If the number of degrees of freedom of a system of n particles is less than

3nwe say that the system is constrained. A systemof free particles constrained tomove in two

dimensions has 2n-dof. The number of degrees of freedom is equal to the number of

independent generalized coordinates.

1.1.2 Elements and Basic Quantities in Electric Systems

In this section, electric energy storage and flow will be shown and the fundamental relations

related to the electric energy storage in the form ofmagnetic field or electric field energywill be

derived. Basic quantities and relations are shown for systems with concentrated parameters

which allow the space changes of the quantities to be neglected thus dynamics can be

represented by ordinary differential equations instead of partial differential equations.

1.1.2.1 Inductance and Magnetic Field Energy

The concept of inductance is associatedwith physical objects consisting of one ormore loops of

conducting material. An ideal inductor is associated with three variables: current i, flux f and

voltage e. The constitutive relationship between the flux-linkage and the current is given as

eitherf ¼ fðiÞ or i ¼ iðfÞ, respectively. There also exists a dynamic relationship between the

flux-linkage and voltage (Faraday’s law) described by df=dt ¼ e.

The work done in establishing a flux-linkage in an inductor is the stored magnetic energy.

It is a function of the flux-linkage and the current and can be expressed by Te ¼Ð tt0iedj ¼Ð t

t0i df=djð Þdj ¼ Ð ff0

idf. Here the subscript ‘e’ is used to distinguish electromagnetic energy

functions from the mechanical systems energy functions. The current can be determined from

i ¼ dTeðfÞ=df. If the constitutive relationf ¼ f ðiÞ is linear, then f ¼ Li, where L is defined

as the inductance of the inductor, and the stored magnetic energy becomes

TeðfÞ ¼ðff0

idf ¼ðff0

f

Ldf ¼ 1

2

f2

L� 1

2

f20

Lð1:4Þ

It is interesting to note the role of the energy and the coenergy variables. Let the constituent

relation describing flux linkage f ¼ fðiÞ and its inverse i ¼ iðfÞ be known. Then product iffor any given point on the curvef ¼ fðiÞ can be expressed as the sum of the integral TeðfÞ andits dual integral T*

e ðiÞ ¼Ð ii0fdi representing the so-called magnetic coenergy

TeðfÞþ T*e ðiÞ ¼

ðff0

idfþðii0

fdi ¼ fi ð1:5Þ

It is easy to show that a change in the magnetic coenergy dT*e ðiÞ can be expressed as

dT*e ðiÞ ¼ fdiþ idf� dTeðfÞ|fflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflffl}

¼0

¼ fdi ð1:6Þ

Dynamics of Electromechanical Systems 5

The dual integralT*e ðiÞ allows the flux to be determined asf ¼ dT*

e ðiÞ=di and, consequently,df=dt ¼ e. Equation (1.5) allows a change in the independent variables without explicitly

using the constituent relationship.

For a linear inductor f ¼ Li and df=dt ¼ e one will find

T*e ðiÞ ¼

ðii0

fdi ¼ðii0

Lidi ¼ 1

2Li2 � 1

2Li20 ð1:7Þ

The flux can be determined from f ¼ dT*e ðiÞ=di for iðt0Þ ¼ i0 ¼ 0. If the constitutive

relation i ¼ iðfÞ is linear, then dT*ðiÞ=di ¼ f ¼ Li and consequently for constant L

e ¼ df

dt¼ dðLiÞ

dt¼ L

di

dtð1:8Þ

Note that for a linear inductor the magnetic energy andmagnetic coenergy are equal, that is,

Te ¼ T*e .

1.1.2.2 Capacitance and Electric Field Energy

The concept of capacitance is associated with physical objects consisting of isolated plates

that are capable of storing charge. An ideal capacitor is associated with three variables: charge

Q, voltage e and current i. Similarly as for the inductor, the static relationship between charge

and voltage can be given either by Q ¼ QðeÞ or e ¼ eðQÞ. The dynamic relationship between

charge and current is dQ=dt ¼ i.

Work done while moving a charge in an electric field is the stored electric energy, which

(for charge Q and electric potential e) can be expressed as potential energy Ue ¼Ð tt0eidj ¼ Ð t

t0e dQ=djð Þdj ¼ ÐQ

Q0edQ. The electric potential can be determined from

e ¼ dUeðQÞ=dQ. If the constitutive relation Q ¼ QðeÞ is linear, then Q ¼ Ce. Here C is

defined as the capacitance and the stored electric energy is

UeðQÞ ¼ðQQ0

edQ ¼ðQQ0

Q

CdQ ¼ 1

2

Q2

C� 1

2

Q20

Cð1:9Þ

The electric coenergy U*e ðeÞ ¼

Ð ee0Qde allows the charge to be determined as Q ¼

dU*e ðeÞ=de and consequently dQ=dt ¼ i.

For a linear capacitor one can find

U*e ðeÞ ¼

ðee0

Qde ¼ðee0

Cede ¼ 1

2Ce2 � 1

2Ce20 ð1:10Þ

The charge can be determined from Q ¼ dU*e ðeÞ=de for eðt0Þ ¼ e0 ¼ 0. If the constitutive

relation e ¼ eðQÞ is linear, then dU*ðeÞ=de ¼ Q ¼ Ce and, consequently, for constant C

i ¼ dQ

dt¼ d

dt

dU*e ðeÞde

¼ dðCeÞde

¼ Cde

dtð1:11Þ


In general the relation between stored electric energyU and electric coenergyU*e is given by

UþU*e ¼ Qe. For a linear capacitor the electric energy and electric coenergy are equal, that is,

U ¼ U*e .

Consider a systemwith n-dof. The dynamical equations of such a system can be represented

in terms of n so-called generalized displacement coordinates q1; . . . ; qn. For example, in the

translational mechanical domain, these coordinates represent the actual position of the bodies,

in the rotational domain they represent the angles, and in the electrical domain the charge.

Similarly, we can speak about the generalized velocity coordinates _q1; . . . ; _qn. The kinetic

energy is a function of the generalized coordinates qi; i ¼ 1; 2; . . . ; n and generalized

velocities _qi; i ¼ 1; 2; . . . ; n. The potential energy is a function of generalized coordinates.

1.2 Fundamental Concepts of Mechanical Systems

1.2.1 The Principle of Least Action

The principle of least action [1], known as Hamilton’s principle, is a general and universally

applied method which permits constructing mathematical models of heterogeneous physical

structures, composed of electrical, mechanical, pneumatic and hydraulic elements. Assume

any two fixed points q1ðt1Þ and q2ðt2Þ in space and time.A body that travels without losses from

the point q1ðt1Þ at time t1 to the point q2ðt2Þ at time t2 may take any path that connects these two

points. From the principle of least action the body travels in one specific path.We need away to

single out, from all the other possible paths, the unique path along which the body will travel.

That may be done using the value SðGÞ – called action – defined as the integral of a scalar

function along the path SðGÞ ¼ ÐGL½qðtÞ; _qðtÞ; t�dt. Here L½qðtÞ; _qðtÞ; t� is the so-called

Lagrangian and G is the path the system takes while moving from q1ðt1Þ to q2ðt2Þ. Theprinciple of least action states that the actual path taken by the system is an extreme of SðGÞ. Thesolution is a so-called Euler–Lagrange equation which (loosely speaking) states that if qðtÞ isthe extreme of SðGÞ with Lagrangian L½qðtÞ; _qðtÞ; t� within interval t 2 ½t1; t2� then, on that

interval, the following so-called Euler–Lagrange equation (1.12) holds on qðtÞ for all t 2 t1; t2½ �

d

dt

qLq _q

� �¼ qL

qqð1:12Þ

The Lagrangian L½qðtÞ; _qðtÞ; t� is not unique. We can make the transformation L0 ¼bL;b 2 R or L0 ¼ Lþ dw=dt for any function w and the equations of motion remain

unchanged. Proof of the Euler–Lagrange equation (1.12) is beyond the scope of this work.

The value of the Lagrangian at a given instant of time is a function given by the state of

the system at that time, and does not depend on the system’s history. Such functions are called

state functions. In general, state is a collection of variables that summarize the past of

a system in such away that allows the prediction of the future state of the system. Examples of

state functions are the total energy of the system and other closely related functions. The state

functions are of central importance in the characterization of physical systems. Some

physical effects (dissipation, hysteresis, inputs) cannot be included in system state functions.

Effect of these properties and quantities can be included in the system description. Here

conservative systems will be discussed first and then other effects will be included in the

system equations of motion.


1.2.2 Dynamics

The trajectories that satisfy the least action principle for conservative systems give a solution of

the Euler–Lagrange equation

d

dt

qLq _q

� �� qL

qq¼ 0 ð1:13Þ

So far we know the conditions which must be satisfied and that Lagrangian is not unique.

The question is how to find Lagrangian for mechanical and electrical systems. The simplest

way is to do it by comparison with a known law of motion – namely Newton’s second law for

the conservative system. Let a body with mass m has linear motion in a potential field. Then

motion is described by the equilibrium of forces

d

dtðm _xÞ ¼ � qUðxÞ

qxð1:14Þ

The same motion can be described by the Euler–Lagrange equation (1.12). These two

descriptions must be equivalent, thus their left hand sides and right hand sides must coincide.

That condition yields

qLqx

¼ � qUqx

qLq _x

¼ m _x

ð1:15Þ

Integration of the second equation with respect to _x yields L ¼ 1=2m _x2 þ cðxÞ and substi-

tuting it to the first equation in (1.15), the constant of integration can be derived as

qLqx

¼ qqx

1

2m _x2 þ cðxÞ

� �¼ qcðxÞ

qx¼ � qU

qx) cðxÞ ¼ �UðxÞ ð1:16Þ

Consequently Lagrangian can be expressed as

L ¼ 1

2m _x2 þ cðxÞ ¼ Tð _xÞ�UðxÞ ð1:17Þ

Thus, by selecting L ¼ T �U for mechanical system the Euler–Lagrange equation is

equivalent to Newton’s second law.

For electric circuits, Lagrangian can be expressed in the same form as L ¼ T �U. That is

easy to confirm by taking T ¼ 1=2L _f2and U ¼ 1=2ð1=LÞf2 for flux linkage as a generalized

coordinate or T ¼ 1=2L _Q2U ¼ 1=2ð1=CÞQ2 as charge as a generalized coordinate. Note here the

consistent selection of coordinates and their velocities (derivatives). This shows that the

Euler–Lagrange equation captures the behavior of themechanical and electrical systemmotion.

For mechanical systems the Lagrangian is expressed as the difference of the kinetic energy

and the potential energy L ¼ T �U. Using the law of energy conservation in conservative

systems E ¼ T þU, quantitatively, the Lagrangian of the system can be expressed as

L ¼ E� 2U ¼ 2T �E. The kinetic energy T measures the motion in the system and the

potential energy measures how much energy is stored in the system at given position.


The Lagrangian, in a sense, measures the system’s activity: when there is more kinetic energy,

the Lagrangian is greater, butwhen there ismore potential energy, it is smaller. The Lagrangian

L ¼ T �U is capturing only the conservative forces and the energy storing process. In order to

include the interaction with other systems and dissipative forces in the system description the

Lagrangian must be changed.

1.2.3 Nonpotential and Dissipative Forces

The forces that are not included in potential energyU (nonpotential forces) can be included in

the Lagrange equation by adding the work of these forces into the energy expression. For a

systemwith n-dof with generalized coordinates q1; . . . ; qn, a component-wise Euler–Lagrange

equationwith forcesFi; i ¼ 1; 2; . . . ; n not included in the potential energy can be expressed as

d

dt

�qLq _qi

�� qL

qqi¼ Fi; i ¼ 1; 2; . . . ; n ð1:18Þ

The forces Fi; i ¼ 1; 2; . . . ; n stand for the interaction forces with rest of the world, the

dissipative forces in the system, the forces that can depend on position and velocity and the

input forces. Some of these forces can have a specific way of their insertion in the system

Euler–Lagrange description.

1.2.3.1 Dissipative Forces

The dissipative forces in nature depend on velocity and thus represent dissipation (loss of

energy) in the system. These forces can be included in the Lagrange equations by using the so-

called Raleigh dissipative function R

R ¼ 1

2

Xnr¼1

Xns¼1

brs _qr _qs

FiR ¼ � qRq _qi

ð1:19Þ

Here, coefficient brs stands for the dissipation coefficient of interaction between the r-th and

s-th kinetic energy storing elements and coefficient FiR stands for the dissipative force. By

expressing change of work due to the action of nonpotential forces as dW ¼PFidqi � qR=q _qiinto Euler–Lagrange equations yields

d

dt

�qLq _qi

�� qL

qqiþ qR

q _qi¼ Fi; i ¼ 1; 2; . . . ; n

d

dt

�qTq _qi

�� qT

qqiþ qU

qqiþ qR

q _qi¼ Fi; i ¼ 1; 2; . . . ; n

ð1:20Þ

This formulation includes kinetic energy T, potential energyU, dissipative forces qR=q _qið Þand nonpotential external forces Fi.


1.2.3.2 Constraints

Systems are often subject to the so-called holonomic constraints

hjðq1; . . . ; qnÞ ¼ 0; j ¼ 1; 2; . . . ;m ð1:21Þ

The enforcement of constraints (1.21) results in appearance of the m constraint forces

Fcstj ; j ¼ 1; 2; . . . ;m. These forces can be treated as external forces and included within

Euler–Lagrange equation by partitioning the external forces into actuator forces

Fi; i ¼ 1; 2; . . . ; n and constraint forces Fcstj ; l ¼ 1; 2; . . . ;m

d

dt

qLq _qi

0@

1A� qL

qqiþ qR

q _qi¼ Fi þFcst

j ; i ¼ 1; 2; . . . ; n; j ¼ 1; 2; . . . ;m ð1:22Þ

The constraint forces are formulated via Lagrange multipliers l [2]

Fcsti ¼ �

Xmj¼1

qhjqqi

lj ; i ¼ 1; . . . ; n; j ¼ 1; 2; . . . ;m ð1:23Þ

Where lj ; j ¼ 1; . . . ;m stand for new variables – Lagrange multipliers that must be

determined in order to calculate all forces and the motion of the system.

Now Equation (1.22) can be expressed in the following form

d

dt

qLq _qi

0@

1A� qL

qqiþ qR

q _qi¼ Fi þFcst

j ; i ¼ 1; 2; . . . ; n

Fcsti ¼ �

Xmj¼1

qhjqqi

lj ; hjðq1; . . . ; qnÞ ¼ 0; j ¼ 1; . . . ;m

ð1:24Þ

This structure of the Euler–Lagrange equation allows energy storage to be handled in the

form of kinetic energy and potential energy, where dissipative forces are expressed through

the Raleigh dissipative function, nonpotential active forces, that is, inputs or control forces and

the constraint forces. The variable to be solved in order to determine system dynamics are n

generalized coordinates and m Lagrange multipliers.

The structure of (1.24) is illustrative in how to apply the Euler–Lagrange equation. The

conservative (lossless) energy storage elements are first determined and the kinetic and

potential energy are written, thus the Lagrangian is formulated. Then dissipative elements

are detected and the Raileigh dissipative function is formulated. The inputs are included

and at the end the constraints are detected and included into the system description.

1.2.4 Equations of Motion

As an example of using the Euler–Lagrange equations for mechanical systems, we will

consider a planar manipulator as depicted in Figure 1.1. The generalized coordinates are

the joint angles qi ¼ ui; i ¼ 1; 2, with angular velocities being v1 ¼ _q1; v2 ¼ _q1 þ _q2


respectively. The mi; i ¼ 1; 2 stands for the mass of links assumed to be concentrated at the

distance lmi; i ¼ 1; 2 from the corresponding joint. Here li; i ¼ 1; 2 stands for the length of thelinks, Ii; i ¼ 1; 2 stands for themoment of inertia of the links around axis comingout of theplane

x; y through the center of the mass of the corresponding link. The center of the mass of link i

moves with translational velocity vmi; i ¼ 1; 2 (vector with components vxmi; vymi; i ¼ 1; 2).In order to apply Euler–Lagrange equations, kinetic and potential energies should be

determined first. The total kinetic energy T is composed of the translational and rotational

kinetic energy of the links Ti; i ¼ 1; 2 is

Ti ¼ 1

2vTmimivmi þ 1

2vTmiIivmi; i ¼ 1; 2

T ¼X2i¼1

1

2vTmimivmi þ

X2i¼1

1

2vTmiIivmi

ð1:25Þ

From geometrical considerations the linear velocities of the center of the mass are

vm1 ¼vxm1

vym1

" #¼

� lm1sinðq1Þ 0

lm1cosðq1Þ 0

" #_q1

_q2

" #¼ Jv1 _q ð1:26Þ

vm2 ¼vxm2

vym2

" #¼

� l1sin ðq1Þ� lm2sin ðq1 þ q2Þ � lm2sin ðq1 þ q2Þ

l1cos ðq1Þþ lm2cos ðq1 þ q2Þ lm2cos ðq1 þ q2Þ

" #_q1

_q2

" #¼ Jv2 _q ð1:27Þ

Here Jv1 2 R2�2 and Jv2 2 R2�2 are the so-called Jacobianmatrices relating the velocities of

the center of the mass for each link to the joint velocities and _qT ¼ _q1 _q2½ � is the vector ofjoint velocities.

Figure 1.1 Planar elbow manipulator


Now the translational kinetic energy of each link and their sum are

Tvi ¼ 1

2vTmimivmi ¼ 1

2ðJvi _qÞTmiJ

vi _q ¼ 1

2_qTðJvTi miJ

vi Þ _q; i ¼ 1; 2

Tv ¼X2i¼1

1

2vTmimivmi ¼

X2i¼1

1

2_qTðJvTi miJ

vi Þ _q

Tv ¼ 1

2_qT ðJvT1 m1J

v1Þþ ðJvT2 m2J

v2Þ

� �_q

ð1:28Þ

The angular velocity terms depend on the moment of inertia of links and the angular

velocities of around axes passing through center of the mass. These velocities are

v1 ¼ 1 0½ �_q1

_q2

" #¼ Jv1 _q; ð1:29Þ

v2 ¼ 1 1½ �_q1

_q2

" #¼ Jv2 _q ð1:30Þ

Here Jv1 2 R1�2 and Jv2 2 R1�2 are Jacobian matrices relating the angular velocities of the

links to the joint velocities.

The rotational kinetic energy is

Tri ¼ 1

2vTi Iivi ¼ 1

2ðJvi _qÞT IiðJvi _qÞ ¼

1

2_qTðJvTi IiJ

vi Þ _q

Tr ¼X2i¼1

1

2vTi Iivi ¼

X2i¼1

1

2_qTðJvTi IiJ

vi Þ _q

Tr ¼ 1

2_qT ðJvT1 I1J

v1 Þþ ðJvT2 I2J

v2 Þ

� �_q

ð1:31Þ

Having moments of inertia for each link Ii; i ¼ 1; 2 allows determination of the rotational

kinetic energy of each link and their sum as

Tr1 ¼ 1

2vT1 I1v1 ¼ 1

2_qTðJvT1 I1J

v1 Þ ¼

1

2_q1 _q2½ �

I1 0

0 0

" #_q1

_q2

" #

¼ 1

2I1 _q1 _q1

Tr2 ¼ 1

2vT2 I2v2 ¼ 1

2_qTðJvT2 I2J

v2 Þ ¼

1

2_q1 _q2½ �

I2 I2

I2 I2

" #_q1

_q2

" #

¼ 1

2I2 _q1 _q1 þ

1

2I2 _q2 _q2 þ I2 _q1 _q2

Tr ¼ 1

2_q1 _q2½ �

I1 þ I2 I2

I2 I2

" #_q1

_q2

" #

ð1:32Þ


The total kinetic energy of the manipulator becomes

T ¼ Tv þ Tr ¼ 1

2_qT ðJvT1 m1J

v1Þþ ðJvT2 m2J

v2Þ

� �_qþ 1

2_qT ðJvT1 I1J

v1 Þþ ðJvT2 I2J

v2 Þ

� �_q

¼ 1

2_qTAðqÞ _q

AðqÞ ¼X2i¼1

ðJvTi miJvi þ JvTi IiJ

vi Þ

ð1:33Þ

MatrixAðqÞ incorporates all themass properties of thewholemanipulator as reflected to the

joint axes. It is referred as multibody kinetic energy matrix or multibody inertia matrix.

The total kinetic energy of the manipulator can be written as

T ¼ 1

2_qTAðqÞ _q ¼ 1

2_q1 _q2½ �

a11 a12

a21 a22

" #_q1

_q2

" #ð1:34Þ

Where the elements aij ; i; j ¼ 1; 2 of matrix AðqÞ are

a11 ¼ m1l2m1 þ I1|fflfflfflfflfflffl{zfflfflfflfflfflffl}axis 1

þm2½l21 þ l2m2 þ 2l1lm2 cos ðq2Þ� þ I2|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}axis 2

¼ a11ðq2Þ

a12 ¼ a21 ¼ m2½l2m2 þ 2l1lm2 cos ðq2Þ� þ I2 ¼ a12ðq2Þ ¼ a21ðq2Þa22 ¼ m2l

2m2 þ I2 ð1:35Þ

The potential energy stored in the links is given by

U ¼ m1glm1sin ðq1Þþm2g ½l1 sin ðq1Þþ lm2 sin ðq1 þ q2Þ� ð1:36Þ

With kinetic energy (1.34) and potential energy (1.36) the Lagrangian can be expressed as

L ¼ 1=2 _qTAðqÞ _q�UðqÞ.

From Euler–Lagrange equations the components describing motion of the first joint can be

determined as follows

qLqq1

¼ � qUqq1

¼ �m1glm1cos ðq1Þ�m2g l1cos ðq1Þþ lm2cos ðq1Þ½ � ¼ � g1ðq1; q2Þ

qLqq1

¼ a11 _q1 þ a12 _q2

d

dt

qLqq1

¼ a11 €q1 þ a12 €q2 þqa11qq2

_q2 _q1 þqa12qq2

_q2 _q2

d

dt

qLqq1

� qLqq1

¼ a11 €q1 þ a12 €q2 þqa11qq2

_q2 _q1 þqa12qq2

_q2 _q2 þ g1ðq1Þ

ð1:37Þ


For the second joint, we can write

qLqq2

¼ � qUqq1

¼ �m2glm2cos ðq1 þ q2Þ ¼ � g2 ðq1; q2Þ

qLqq2

¼ a21 _q1 þ a22 _q2

d

dt

qLqq1

¼ a21 €q1 þ a22 €q2 þqa21qq2

_q2 _q1 þqa22qq2

_q2 _q2

d

dt

qLqq2

� qLqq2

¼ a21 €q1 þ a22 €q2 þqa21qq2

_q2 _q1 þqa22qq2

_q2 _q2 þ g2ðq1; q2Þ

ð1:38Þ

By inserting (1.37) and (1.38) into (1.24) and taking into account the work of the torques t1and t2 developed by actuators, the dynamics of the planar elbow manipulator yields

a11 €q1 þ a12 €q2 þqa11qq2

_q2 _q1 þqa12qq2

_q2 _q2 þ g1ðq1Þ ¼ t1

a21 €q1 þ a22 €q2 þqa21qq2

_q2 _q1 þqa22qq2

_q2 _q2 þ g2ðq1; q2Þ ¼ t2

ð1:39Þ

These equations can be simplified by merging components that depend on the velocities

b1ðq; _qÞ ¼ qa11qq2

_q2 _q1 þqa12qq2

_q2 _q2

b2ðq; _qÞ ¼ qa21qq2

_q2 _q1 þqa22qq2

_q2 _q2

ð1:40Þ

The final form of the equations describing the dynamics of a planar elbow manipulator can

now be written as

AðqÞ€qþ bðq; _qÞþ gðqÞ ¼ s ð1:41Þ

Here AðqÞ 2 R2�2 is the kinetic energy matrix, bTðq; _qÞ ¼ b1ðq; _qÞ b2ðq; _qÞ½ � is the

vector representing terms depending onvelocities, gTðqÞ ¼ g1ðqÞ g2ðqÞ½ � is the vector of thegravitational forces and sT ¼ t1 t2½ � is the vector of joint torques generated by actuators.

1.2.5 Properties of Equations of Motion

Essential to the Euler–Lagrange formulation for mechanical systems is the derivation of the

total kinetic energy stored in all of the rigid bodies, the determination of the potential energy

and the nonconservative external forces. This section briefly discusses an extension of the

Euler–Lagrange’s approach in obtaining the equations of motion for fully actuated rigid n-dof

multibodymechanical system, as shown in Figure 1.2. The general structure of the equations of

motion is as in (1.41). Storing kinetic energy is a property of the body in motion thus kinetic


energy matrix or inertia matrix should be examined first and then conservative and noncon-

servative forces should be derived.

1.2.5.1 Inertia Matrix

The kinetic energy stored in an individual arm link consists of two terms; one is kinetic energy

attributed to the translational motion of mass Tv and the other is kinetic energy due to rotation

about the center of themassTv. Treating i� th link as a general three-dimensional rigid body, the

total kinetic energy can be expressed as sum of translational and rotational kinetic energy Ti ¼Tvi þ Tvi ¼ 1=2v

Tmimivmi þ 1=2v

Ti Iivi and the total kinetic energy of the multibody system

becomes just the sum of the kinetic energies of the individual bodies T ¼Pni¼1 Ti. Here vmi

is thevelocity vector of the body,vmi stands for the rotational speed vector,mi is themass and Ii is

the inertia matrix.

The expression for the kinetic energy is written in terms of the linear and angular velocities

of each link member, which are not independent variables. As shown in the previous section

velocities can be expressed in terms of a set of generalized coordinates, namely joint

coordinates qT ¼ q1 . . . qn½ �. The functional dependence is defined by the Jacobianmatrix

relating the velocity of the center of the mass vmi and the angular velocitiesvi of the i-th link to

joint velocities _q. For three-dimensional multibody systems we can write

vmi ¼ Jvi _q; Jvi 2 R3�n

vi ¼ Jvi _q; Jvi 2 R3�nð1:42Þ

The total energy of the n-dof multibody system can be expressed as

T ¼Xni¼1

ðTvi þ TviÞ ¼Xni¼1

1

2_qTðJvTi miJ

vi Þ _qþ

1

2_qTðJvTi IiJ

vi Þ _q ¼ 1

2_qTAðqÞ _q

AðqÞ ¼Xni¼1

ðJvTi miJvi þ JvTi IiJ

vi Þ; AðqÞ 2 Rn�n

ð1:43Þ

Figure 1.2 Multibody system


MatrixAðqÞ 2 Rn�n incorporates the mass properties of the multibody system and is called

a multibody kinetic energymatrix. This matrix is symmetric positive definite (since the kinetic

energy of the multibody system is strictly positive if the body is not at rest). It is configuration-

dependent; thus it represents the instantaneous composite mass properties of thewhole system.

Due to the boundedness of the generalized coordinates and its structure the kinetic energy

matrix is bounded.

ExpressingAðqÞ ¼ faijðqÞg; i; j ¼ 1; 2; . . . ; n yields the total kinetic energy as a quadraticform

T ¼ 1

2_qTAðqÞ _q ¼ 1

2

Xni¼1

Xnj¼1

aijðqÞ _qi _qj ð1:44Þ

The elements of the Euler–Lagrange equations contributed by the kinetic energy can be now

written as

d

dt

qTq _qi

¼ d

dt

Xnj¼1

aij _qj

!¼Xnj¼1

aij €qj þXnj¼1

daij

dt_qj ; i ¼ 1; 2; . . . ; n ð1:45Þ

The terms aij €qj; i 6¼ j represent dynamic interaction among i-th and j-th joints and satisfy

aij ¼ aji due to symmetry of the kinetic energy matrix.

The termPn

j¼1ðdaij=dtÞ _qj is function of the generalized coordinates and their derivatives

and in general, by applying chain rule of differentiation, can be expressed as

Xnj¼1

daij

dt_qj ¼

Xnj¼1

Xnk¼1

qaijqqk

_qk

!_qj ¼

Xnj¼1

Xnk¼1

qaijqqk

_qk _qj ð1:46Þ

The term qL=qqi in the Euler–Lagrange equation yields

qLqqi

¼ qTqqi

¼ qqqi

1

2

Xnj¼1

Xnk¼1

ajk _qj _qk

0@

1A ¼ 1

2

Xnj¼1

Xnk¼1

qajkqqi

_qj _qk; i ¼ 1; 2; . . . ; n ð1:47Þ

The overall contribution of the kinetic energy to the Euler–Lagrange equations is

d

dt

qTqqi

� qTqqi

¼Xnj¼1

aij €qj þXnj¼1

Xnk¼1

qaijqqk

_qk _qj �1

2

Xnj¼1

Xnk¼1

qajkqqi

_qj _qk

¼ aTi ðqÞ€qþ biðq; _qÞ

biðq; _qÞ ¼Xnj¼1

Xnk¼1

qaijqqk

_qk _qj �1

2

Xnj¼1

Xnk¼1

qajkqqi

_qj _qk; i ¼ 1; 2; . . . ; n

ð1:48Þ

Sometimes the term biðq; _qÞ is partitioned on the centrifugal (proportional to the square ofthe joint velocities j ¼ k) and Coriolis terms (proportional to the product of the joint velocities

for j 6¼ k). These two terms are a result of the configuration dependence of the kinetic energy

matrix.


1.2.5.2 Conservative and Nonconservative Forces

The conservative forces are given as partial derivative of the potential energy stored in the links

of the multibody system. That energy is given as

U ¼Xni¼1

migTrmi

0 ðqÞ ð1:49Þ

Here, rmi0 ðqÞ 2 R1�n is the vector of position of the center of mass of the i-th link. The

substitution into Euler–Lagrange equation yields

giðqÞ ¼ qUqqi

¼Xnj¼1

mjgT qr

mj0 ðqÞqqi

¼Xni¼1

migTcoliðJvj Þ; i ¼ 1; 2; . . . ; n ð1:50Þ

Here, coli ðJvj Þis the i-th column of the linear velocity Jacobian matrix for the j-th

link.

Nonconservative forces acting on the multibody system can be incorporated into the

Euler–Lagrange equations by determining the work related to these forces. The assignment

of the nonconservative forces depend on the selection of the generalized coordinates. In the

case that work is the inner product of joint torques and joint displacements, the joint torques

represent generalized forces.

The vector of constraint forces Fcst are formulated via Lagrange multipliers method

in (1.23).

Now the configuration space dynamics of a multibody system can be expressed in the

following form

AðqÞ€qþ bðq; _qÞþ gðqÞ ¼ sþFcst ð1:51Þ

Here AðqÞ 2 Rn�n is a symmetric positive definite uniformly bounded matrix given

in (1.43). The other terms can be expressed as:

1. Corriolis and centrifugal terms

bðq; _qÞ ¼ _AðqÞ _q� 1

2

_qTAq1 _q

. . .

_qTAqn _q

26664

37775; Aqi ¼

qAqqi

ð1:52Þ

2. Gravity term

gðqÞ ¼ � qUðqÞqq

2 Rn�1 ð1:53Þ

3. Constraint forces

Fcon ¼ ��qhðqÞqq

�T

‚ ¼ � JTh‚; Jh ¼ qhðqÞqq

2 Rm�n ð1:54Þ

Here Jh 2 Rm�n is the so-called constraint Jacobian. Model (1.51) describes dynamics of the

rigid fully actuated multibody system in configuration space. In this model forces reflecting

interaction with surroundings – external forces except the constraint forces are not included.


In order to include these forces one has first find the relation between forces originating in

configuration space and the forces originating in the space in which the operation is

performed – so-called operational space. In order to do so, first the model in the operational

space must be derived.

1.2.6 Operational Space Dynamics

Let us find equations for the motion of the multibody system shown in Figure 1.2 in the so-

called operational space defined by a set of m parameters x1; x2; . . . ; xm representing the

position and orientation of point P in operational space. One can select another point on the

multibody system, so this selection is arbitrary. The operational space configuration vector

xT ¼ x1 . . . xm½ � can be, from the system kinematics, determined as function of the

generalized configuration space coordinates q1; q2; . . . ; qm, and in general is given by

x ¼ fðqÞ 2 R1�m ð1:55Þ

By differentiating (1.55) the operational space velocities are expressed as

_x ¼ qfðqÞqq

_q; J ¼ qfðqÞqq

2 Rm�n ð1:56Þ

Here,matrix JðqÞ 2 Rm�n is a so-called kinematic Jacobian. It depends on the configuration

space generalized coordinates – thus on the instantaneous configuration of the multibody

system. For some configurations, its determinant may become zero and the system appear to

have a singular position.

The kinetic energy, similarly as in (1.44), can be expressed as

T ¼ 1

2_xTLðxÞ _x ð1:57Þ

Here LðxÞ 2 Rm�m stands for the operational space kinetic energy matrix. The potential

energy is assumed to be function of operational space generalized coordinates UðxÞ. Takinginto account nonconservative operational space forces F, Euler–Lagrange equations repre-

senting the motion in operational space can be written as

L ¼ Tðx; _xÞ�UðxÞd

dt

�qLq _x

�� qL

qx¼ F

ð1:58Þ

The particular terms in Euler–Lagrange equations are

qLq _x

¼ LðxÞ _x

d

dt

�qLq _x

�¼ LðxÞ€xþ _LðxÞ _x

ð1:59Þ


qLqx

¼ qTðx; _xÞqx

� qUðxÞqx

¼

1

2_xTLx1 _x

. . .

1

2_xTLxm _x

26666666664

37777777775� pðxÞ

Lxi ¼ qLðxÞqxi

; i ¼ 1; 2; . . . ;m

ð1:60Þ

These terms yield operational space dynamics

LðxÞ€xþmðx; _xÞþ pðxÞ ¼ F

mðx; _xÞ ¼ _LðxÞ _x�

1

2_xLx1 _x

. . .

1

2_xLxm _x

266666666664

377777777775

ð1:61Þ

The forces in operational space F 2 Rm�1 can be implemented only by applying the

configuration space forces s 2 Rn�1, so one has to find functional relation between forces in

operation and configuration spaces.

In order to find the correspondence between operational space equations of motion (1.61)

and the configuration space equations ofmotion (1.51), one has to exploit the known functional

relation between coordinates and their derivatives and the invariance of the kinetic and

potential energy with a coordinate change.

Invariance of the kinetic energy in operational and configuration spaces leads to

T ¼ 1

2_qTAðqÞ _q ¼ 1

2_xTLðxÞ _x ð1:62Þ

The operational space velocities and configuration space velocities are related by the

Jacobian matrix _x ¼ J _q, thus invariance of kinetic energy yields

1

2_qTAðqÞ _q ¼ 1

2_xTLðxÞ _x ¼ _x ¼ 1

2ðJ _qÞTLJ _q ¼ 1

2_qTðJTLJÞ _q ð1:63Þ

For a nonredundant presentation (equal dimension of configuration space and operational

space), the kinetic energy matrix can be expressed as a function of the configuration space

kinetic energy matrix and the Jacobian. By inserting JTLJ into the dynamical model of the

multibody system in operational space yields [3]

JT ½LðxÞ€xþmðx; _xÞþ pðxÞ� ¼ JTF ¼ AðqÞ€qþ bðq; _qÞþ gðqÞ ¼ s ð1:64Þ

The relation between operational space forces F and the configuration space joint

torques s is defined by the transpose of the Jacobian matrix. The expressions relating the


operational space velocities and forces with the configuration space generalized velocities

and torques are

_x ¼ J _q

s ¼ JTFð1:65Þ

These two relations are very important since they allow mapping the velocities and the

forces between configuration and the operational spaces. If in Equation (1.65) the Jacobian

matrix is singular, then finite velocities in the operational space will require infinite speeds in

the configuration space and, similarly, finite forces in the operational spacewill require infinite

forces in the configuration space.

1.3 Electric and Electromechanical Systems

One of the main advantages of the Euler–Lagrange equations is that mechanical and electrical

systems are treated analogously. This makes this method particularly suitable for describing

dynamics of systems that have both electrical and mechanical components, that is, electro-

mechanical systems like transducers and actuators. Here we will first discuss the electric

systems and then turn to the analysis of electromechanical systems.

1.3.1 Electrical Systems

In what we call electric circuits, the spatial behavior of electromagnetic systems is

determined by elements confined in space so the dependence on the spatial coordinates

can be disregarded and the resulting dynamics can be described by ordinary differential

equations.

Let us first look at a simpleR� L�C circuit with constant parametersR; L;C supplied from

voltage source vðtÞ, shown in Figure 1.3. The capacitor C stores chargeQ and the electric field

energy. Inductance L stores the magnetic field energy. The resistance R is the dissipative

element in the electric circuit. Selecting the chargeQ as the generalized coordinate, equations

of motion can be written as

_Q ¼ i

d

dtðLiÞþ Q

CþRi ¼ vðtÞ ð1:66Þ

Figure 1.3 R� L�C circuit


For the given circuit, Lagrangian and the Rayleigh dissipative functions are

LeðQ; _QÞ ¼ 1

2L _Q

2 � 1

2

Q2

C¼ T*

e ð _QÞ�UeðQÞ

Re ¼ 1

2R _Q

2 ¼ 1

2Ri2

ð1:67Þ

The R� L�C circuit equations of motion can be derived from Euler–Lagrange equations

d

dt

qLeqi

� qLeqQ

þ qRe

q _Q¼ IeðtÞ ð1:68Þ

Here IeðtÞ stands for the generalized force, which in our case is voltage IeðtÞ ¼ vðtÞ. Thefirst term in Lagrangian (1.67) is the magnetic coenergy and the second term is just the electric

energy function.

The particular terms of the Euler–Lagrange Equation (1.68) are

d

dt

qLeqi

¼ d

dt

qqi

1

2Li2 � 1

2

Q2

C

0@

1A ¼ d

dtðLiÞ

� qLeqQ

¼ � qqQ

1

2Li2 � 1

2

Q2

C

0@

1A ¼ Q

C

qRe

q _Q¼ Ri

IeðtÞ ¼ vðtÞ

ð1:69Þ

This illustration shows a complexity of the direct application of the Lagrangian to electric

circuits. It would be hard to justify its application in problems like this. The derivations just

serve as illustration of the applicability of the Euler–Lagrange formulation to electric circuits.

In the following section we will discuss electromechanical systems as a new class of

systems in which energy is stored and exchanged in both mechanical and electromagnetic

forms, thus the equations of motion depend on the coordinates of the mechanical and

electromagnetic systems. The underlying phenomenon is in the nature of the electromagnetic

energy storage which depends on the primary electrical coordinates (charge, current) and on

the geometric attributes of electromagnetic energy-storing elements (capacitors, inductors).

As a consequence, change in either primary electrical or geometric (mechanical) coordinates

will change the stored energy. The dependence of the stored electromagnetic energy on the

mechanical coordinates points out to a possibility of energy exchange between mechanical

and electromagnetic systems – thus the electromechanical energy conversion.

1.3.2 Electromechanical Systems

The single-axis magnetic suspension system shown in Figure 1.4 will be used as introductory

example for the analysis of systems with both electromagnetic and mechanical energy storing

elements. The winding with inductance LðxÞ, supplied from the voltage source vðtÞ, forms an


electromagnet.Most of themagnetic energy is stored in the air gap between ball and the body of

inductance. The losses in electric circuits are represented by the resistance R. Mechanical

system consists of a ball withmassm suspended in the air by the attractive force pulling the ball

towards the body of the electromagnet.

The extended Lagrangian, which includes both mechanical and electromagnetic energy

storage. The Rayleigh dissipative function, can be written as

Lðx; _x;Q; _QÞ ¼ 1

2m _x2 þmgxþ 1

2LðxÞ _Q2

Re ¼ 1

2R _Q

2ð1:70Þ

Here g is the gravity constant, x is the position of the center of the ball, and LðxÞ is theinductance of the magnetic circuit.

Direct application of the Euler–Lagrange equations leads to the following equations of

motion for mechanical and electromagnetic subsystems

d

dt

qLq _x

� qLqx

þ qRq _x

¼ IxðtÞ ) m€x�mg� i2

2

qLðxÞqx

¼ 0 ð1:71Þ

d

dt

qL

q _Q� qL

qQþ qR

q _Q¼ IQðtÞ ) d

dtLðxÞiþRi ¼ vðtÞ ð1:72Þ

In writing (1.71) and (1.72) equality _Q ¼ i is used. The force f acting on the mechanical

subsystem due to the change of the stored electromagnetic energy is

f ¼ � i2

2

qLðxÞqx

¼ � qT*e ðxÞqx

ð1:73Þ

Figure 1.4 Ball suspended by an electromagnet with a voltage supply


The voltage e acting on the electric circuit due to the change of the electromagnetic energy

(electromotive force) is

d

dt

qL

q _Q

� �¼ d

dt

qT*e

qi

� �¼ d

dtLðxÞi½ � ¼ e ð1:74Þ

Now we are able to write equations of motion for both mechanical and electromagnetic

system by separating all the terms that depend only on mechanical coordinates, the terms that

depend only on electric coordinates and the terms that depend on both mechanical and electric

coordinates. For Equations (1.71) and (1.72) that separation procedure yields

m€x�mg¼ i2

2

qLðxÞqx

¼ qT*e

qx¼ f

vðtÞ�Ri ¼ d

dtLðxÞi½ � ¼ d

dt

�qT*

e

qi

�¼ e

ð1:75Þ

The separation procedure allows partitioning the system as shown in Figure 1.5. into three

separate blocks:

. electric circuit with voltage as external input,

. mechanical system without external input,

. conservative electromechanical coupling network.

Stored energy can be changed by the energy flow from the electric or from the mechanical

sources, that is, the magnetic field energy storing is connected to both electric and mechanical

terminals and acts as the coupling elements (coupling field).

The change in the storedmagnetic energy in the coupling field results in electromotive force

e on the electric terminals and in mechanical force f on the mechanical terminals. If there is no

change in the amount of the energy stored in the coupling field, then the power inputs to the

coupling field from electric and mechanical connections are balanced. In such operation either

electric or mechanical input power to the coupling field has to be negative (the sink) and other

has to be positive (the source), thus the coupling field transforms the electric energy into

mechanical energy and vice versa.

The separation of the system allows finding the interaction forces due to electromechan-

ical coupling, thus represents the coupling field as a load for both electric and mechanical

systems.

Figure 1.5 Representation of an electromechanical converter with a magnetic coupling field


1.3.3 Electrical Machines

Converters with magnetic field coupling are of particular interest in motion control systems

since they encompass most electrical machines. For such systems, mechanical motion is either

rotational or translational.

Elementary electrical machines (with a magnetic energy coupling field) consist of two

members, each carrying windings, as shown in Figure 1.6. One of these members is

stationary (the stator) and the other one is moving (the rotor). The space between stator and

rotor forms a cylinder. The thickness of that cylinder is called the air gap and, in our

analysis, is assumed constant. Windings are placed on the stator and the rotor and are

magnetically coupled. Most of the magnetic energy is stored in the air gap between stator

and rotor.

A moving part can either rotate around its axis or have translational motion along its axis –

thus an elementary machine has one mechanical degree of freedom.

For the analysis, let the structure consist of n equalwindings on stator and rotor.Windings on

stator and rotor are placed symmetricallywith angular shift of themagnetic axes of the adjacent

windings for equal angle usj;k ¼ urj;k ¼ p=n. Here, superscripts (s) and (r) are used to assign

stator or rotor, respectively. The structure in Figure 1.6, for simplicity, is shown with only two

windings on the stator and two windings on the rotor. It illustrates the geometrical relations of

the windings. Here angular shift between stator windings is us;sa;b ¼ p=2 and rotor windings

ur;ra;b ¼ p=2. The shift of magnetic axes between stator winding a and the rotor winding a is

us;ra;a ¼ u. The windings are supplied from external voltage sources vsT ¼ vs1 . . . vsn½ � andvrT ¼ vr1 . . . vrn½ � and are assumed of carrying currents isT ¼ is1 . . . isn½ � and

irT ¼ ir1 . . . irn½ � on the stator and rotor windings, respectively.

The arrangement of the windings is such that their magnetomotive forces can be

approximated by space sinusoids. The self inductances of stator and rotor windings are

constant. The same is true for the stator–stator and the rotor–rotor mutual inductances.

The stator–rotor and rotor–stator mutual inductances depend on the angle between the

magnetic axes of the windings on the stator and the rotor for which they are calculated.

Figure 1.6 Representation of an elementary cylindrical electrical machine


These inductances have an extreme value when the axes of corresponding windings are

aligned and are zero when these axes are orthogonal. Mutual inductance of the winding on

the stator with the magnetic axis aligned on u ¼ 0 and the corresponding winding on the

rotor, can be expressed as

Ls;ra;a ¼ Lsr cos ðuÞ ð1:76Þ

Here Lsr is the value of the mutual inductance when magnetic axes of windings are aligned

and u is the space angle measured from the magnetic axis of the stator winding to the magnetic

axis of the rotor winding or rotor rotation angle. The mutual inductance of the a-th winding on

the stator and the k-th winding on the rotor has the same properties as the mutual inductance

Ls;ra;a. The difference is in the space angle which is now us;ra;k ¼ u� kp=n, thus the mutual

inductance can be expressed as

Ls;ra;k ¼ Lsr cos u� k

pn

ð1:77Þ

Similar relations can be found for all other windings, the difference will be in the relative

angular shift between windings.

The overall inductance matrix can be expressed in the following form

LðuÞ ¼Ls;s Ls;rðuÞLr;sðuÞ Lr;r

" #ð1:78Þ

Here Ls;s and Lr;r are constant matrices with elements being the self inductances of the

stator–stator and rotor–rotorwindings respectively.MatricesLr;sðuÞ andLs;rðuÞ arematrices of

mutual inductances of the stator–rotor and rotor–stator windings respectively. BothLr;sðuÞ andLs;rðuÞ depend on the rotor angular position u. For magnetically linear systems magnetic

coenergy T*e is

T*e ¼ 1

2iTLðuÞi ð1:79Þ

Here iT ¼ is1 . . . isn ir1 . . . irn½ � stands for the current vector. The extended Lagrang-ian, which includes both mechanical and electromagnetic energy storage, and the Rayleigh

dissipative function can be written as

Lemðu; _u;Q; iÞ ¼ 1

2aðuÞ _u2 þ 1

2iTLðuÞi

Rem ¼ 1

2B _u

2 þ 1

2iTRi

ð1:80Þ

Here aðuÞ is the kinetic energy coefficient (here equal to moment of inertia). The Rayleigh

dissipative function is a sum of the mechanical system R ¼ 1=2B _u2and the electric system

R ¼ 1=2iTRi Rayleigh dissipative functions. B stands for the friction coefficient and R is a

diagonal matrix of ohmic resistances of the stator and the rotor windings. The external

nonconservative force text is assumed to be acting on the rotor.


Direct application of Euler–Lagrange equations yields the following components of the

equations of motion:

1. Mechanical system

d

dt

qLemq _u

¼ d

dt

qqi

�1

2a _u

2 þ 1

2iTLðuÞi

�¼ d

dtða _uÞ ¼ a€uþ da

dt_u

� qLemqu

¼� qqQ

�1

2a _u

2 þ 1

2iTLðuÞi

�¼ � 1

2iTqLðuÞqu

i

qRem

q _u¼ B _u

IeðtÞ ¼ � textðtÞ

ð1:81Þ

2. Electromagnetic system

d

dt

qLemqi

¼ d

dt

qqi

�1

2a _u

2 þ 1

2iTLðuÞi

�¼ d

dtðLðuÞiÞ ¼ LðuÞ di

dtþ dLðuÞ

dti

� qLemqQ

¼ � qqQ

�1

2a _u

2 þ 1

2iTLðuÞi

�¼ 0

qRem

q _Q¼ Ri

IeðtÞ ¼ vðtÞ

ð1:82Þ

Here, vT ¼ vs1 . . . vsn vr1 . . . vrn� �

stands for the vector of voltage sources on the stator

and the rotor, respectively. Insertion of (1.81) and (1.82) into Euler–Lagrange equations yields

a€uþ�da

dtþB

�_u� iT

2

qLðuÞqu

i ¼ � text

LðuÞ didt

þ _uqLðuÞqu

iþRi ¼ vðtÞð1:83Þ

Term 1=2iT ½qLðuÞ=qu�i represents the force due to the electromechanical conversion.

Similarly _u½qLðuÞ=qu�i stands for electromotive force due to the interaction of the mechanical

systems on the electromagnetic energy storage.

Force due to electromechanical conversion t ¼ 1=2iT ½qLðuÞ=qu�i causes motion of the rotor

when electrical power is fed into the electromechanical converter. It is a scalar quantity. Since

Ls;s and Lr;r are constant matrices this force can be expressed as

t ¼ isT irT� � 0

qLs;rðuÞqu

qLr;sðuÞqu

0

2666664

3777775

is

ir

" #ð1:84Þ


Here, the current vector is expressed as iT ¼ isT irT� �

with isT ¼ is1 is2 . . . isn½ � andirT ¼ ir1 ir2 . . . irn½ �. This expression is complex for practical usage. What can be

concluded is that force is function of the stator and the rotor currents and the relative position

of the windings.

In the design of electromechanical actuators based on the magnetic coupling fields [4], the

structures and relative positions of the magnetic energy storing elements are selected in such a

way that the force (or torque) due to the electromechanical conversion, in suitable frame of

references, can be expressed as proportional to the vector product of the flux linkage on the

rotor and stator current

t ¼ kfFr � is

¼ kf Frjj isjjsin ðwÞjjjj¼ kf Frjjis?

�� ð1:85Þ

Here Frjjjj stands for the magnitude of the rotor flux vector, isjjjj stands for the magnitude

of stator current vector andw stands for the angle between rotor flux and stator current vectors

and is? stands for the is? ¼ isjjsin ðwÞjj . The last expression in (1.85) shows that, if the rotor flux

is kept constant, then we can write kf Frjj ¼ KTðFrÞjj and force (or torque) is expressed as

t ¼ KTis?. This allows us to write the dynamics of the mechanical motion of electrical

machine as

a€qþ bðq; _qÞþ text ¼ tðtÞ

tðtÞ ¼ KTðFÞis? ¼ KTðFÞisð1:86Þ

Here b ðq; _qÞ includes all components that depend on position and velocity. To avoid

complicated notation, the component of the stator current orthogonal to rotor flux is written as

is? ¼ is.

References

1. Landau, L.D. and Lifshitz, E.M. (1976) Mechanics, Butterworth-Heinemann, Oxford.

2. Arnold, V.I., Weinstein, A., and Vogtmann, K. (1997) Mathematical Methods of Classical Mechanics, 2nd edn,

Springer-Verlag, New York.

3. Khatib, O. (1987) A united approach to motion and force control of robot manipulators: The operational space

formulation. International Journal of Robotics Research, 3(1), 43–53.

4. Leonhard, W. (1985) Control of Electrical Drives, Springer-Verlag, New York.

Further Reading

Goldstein, H., Poole, C.P., and Safko, J.L. (2002) Classical Mechanics, 3rd edn, Prentice Hall, Upper Saddle River,

New Jersey.

Hite, D.C. and Woodson, H.H. (1959) Electromechanical Energy Conversion, John Wiley & Sons, Inc., New York.

Isermann, R. (2003) Mechatronics Systems Fundamentals, Springer-Verlag, New York.

Kugi, A. (2001) Nonlinear Control Based on Physical Models, Springer-Verlag, New York.

MacFarlane, A.G.J. (1970) Dynamical System Models, Harrap, London.

Nijmeijer, H. and van der Schaft, A.J. (1990) Nonlinear Dynamical Control Systems, Springer-Verlag, New York.

Paynter, H.M. (1960) Analysis and Design of Engineering Systems, M.I.T. Press, Cambridge, Massachusetts.


2

Control System Design

In order to make sense of issues involved in the design of motion control system, a short

overview of control system analysis and design methods is given in this chapter. First, we

will review basic issues relevant for understanding the ideas involved in control and then

we will briefly present design and analysis methods which can be effectively used in

motion control.

The main idea in control systems is related to feedback, which is used extensively in natural

and technical systems. The principle of feedback is very simple: corrective actions are taken

based on the difference between the desired and the actual performance. Feedback has some

remarkable properties and its use in motion control systems has resulted in dramatic

improvements in system performance. Feedbackmay be used as a tool tomodify the properties

of a system, it can reduce the effects of disturbances and process variations, and it allows awell

defined relationship to be maintained between variables or between systems.

The process of designing a control system generally involvesmany steps. A typical scenario

consists of the following steps:

. Model the system to be controlled and simplify the model so that it is tractable,

. Decide on performance specifications and control framework to be used,

. Design a controller to meet the specifications if possible,

. Simulate the resulting controlled system, either on a computer or in a pilot plant,

. Repeat the procedure if necessary,

. Choose hardware and software and implement the controller,

. Tune the controller online if necessary.

Motion control is not designing a control system for a fixed plant, or simply setting a feedback

around an already fixed physical system. It involves assisting in the choice and configuration of

the overall system, the placement and selection of transducers and actuators, the selection of

hardware for control implementation, in short, taking a system view of overall system

performance. For this reason it is important that the applied control framework will not only

lead to good and consistent designs when these are possible, but will also indicate when the

performance objectives cannot be met. It is important to take into account that practical



problems have uncertain parameters, unmodelled dynamics that may produce substantial

uncertainty. The specific nature of sensors and actuators and sensor noise and input signal level

constraints will limit the achievable specifications.

Generally speaking the objective in a control system is to make some output behave in

a desired way by manipulating some inputs. The simplest objective might be to keep the

output close to some equilibrium point, or to keep the output close to a reference or

command signal.

Motion control systems may appear complicated due to their complex structure, involving

many different subsystems and many different technologies. For control purposes, the use of

simplified mathematical models allows reduction of the complexity in system representation.

In general the model should predict the output response for a defined input in such a way that it

can be used to design a control system. The output of design is a mathematical model of a

controller thatmust be implemented in hardware. Design should lead to a control that will work

on the real physical system.

An issue in control system design and analysis is the specification of the properties of a

control system. This is important because it gives the goals and allows users to specify, evaluate

and test a system. Specifications for a control system typically include: stability of the closed

loop system, robustness to model uncertainty, attenuation of measurement noise and ability to

follow reference signals.

2.1 Basic Concepts

Dynamics is a key element of control because both processes and controllers are dynamical

systemswhich can be viewed inmany different ways. An internal view attempts to describe the

internal workings of the system, taking into account the energy-storing elements and their

interactions. A detailed analysis of structure and changes in internal variables describing the

state of the system is important in this approach.

Another way is to look at the relationship between input and output of the system and

determine features like linearity, time-invariance and so on. Successful application of control

needs to merge these two views and add the external elements that allow measurement of the

variables of interest and change control inputs to the system. The presence of interactions with

other systems may be added as disturbance to complete the picture (Figure 2.1).

This chapter will review basic concepts of control system analysis and design. Systemswith

a single input single output (SISO) as well as systems with multiple input and multiple output

(MIMO) will be discussed. Linear time-invariant (LTI) systems and some aspects of the

nonlinear systems will be analyzed.

Figure 2.1 Representation of a dynamic system in input–output form


2.1.1 Basic Forms in Control Systems

Standard models are very useful to make it easier when dealing with the complexities of

dynamic systems control. Four basic standard forms are used in control and they interplay in

analysis and design. These are:

. ordinary differential equations,

. transfer functions,

. frequency responses,

. state equations.

Ordinary differential equations and transfer functions are primarily used for LTI systems. The

state equations also apply to nonlinear systems.

2.1.1.1 Ordinary Differential Equations

Let the dynamics of a LTI system be described by the following differential equation

dny

dtnþ a1

dn� 1y

dtn� 1þ � � � þ any ¼ dmu

dtmþ b1

dm� 1u

dtm� 1þ � � � þ bmu ð2:1Þ

Here u is the input and y the output of the system and disturbance is assumed to be zero. The

highest derivative of y defines the system order. The right hand side depends not only on the

control input but also on its derivatives, which is an interesting feature of dynamical systems.

If the input to the system is zero, then the homogeneous equation associated with

Equation (2.1) is obtained as

dny

dtnþ a1

dn� 1y

dtn� 1þ � � � þ any ¼ 0 ð2:2Þ

Solution of Equation (2.2) plays an important role in the control systems analysis. The

characteristic polynomial of Equation (2.2) is

AðlÞ ¼ ln þ a1ln� 1 þ � � � þ an ð2:3Þ

The roots of the characteristic equation AðlÞ ¼ 0 determine the properties of the solution of

a differential equation. If the characteristic equation has distinct roots ak the solution is

yðtÞ ¼Xnk¼1

Ckeakt ð2:4Þ

Here Ck are arbitrary constants, thus the solution (2.4) has n free parameters Ck,

k ¼ 1; 2; . . . ; n. The functions eakt are sometimes called modes of the system. The solution

consists ofmonotone functions that decrease ifak is negative or increase ifak is positive. If the

roots are complex ak ¼ sk � jv, the solution (2.4) is composed of the functions esktsin ðvktÞand esktcos ðvktÞ. These functions have an oscillatory behavior but their amplitudes decrease if

sk < 0 or increase if sk > 0.

Control System Design 31

The solution is stable if all functions eakt and eskt go to zero as time increases. A system is

thus stable if the real parts of all ak are negative, or equivalently if all the roots of the

characteristic polynomial (2.3) have negative real parts.

The full solution of Equation (2.1) depends on the control input to the system and can be

expressed as

yðtÞ ¼Xnk¼1

Ckeakt þ

ðt0

gðt�qÞuðqÞdq ð2:5Þ

Here gðtÞ stands for impulse response of the system defined by

gðtÞ ¼ dmf

dtmþ b1

dm� 1f

dtm� 1þ � � � þ bmf ð2:6Þ

Here f ðtÞ is solution of equation dnf=dtn þ a1dn� 1f=dtn� 1 þ � � � þ anf ¼ 0 with initial

conditions f ð0Þ ¼ _f ð0Þ ¼ � � � ¼ f ðn� 2Þð0Þ ¼ 0, f ðn� 1Þð0Þ ¼ 1.

The behavior of a LTImultiple input multiple output (MIMO) systemmay be described by a

vector differential equation

dxðtÞdt

¼ AxðtÞþBuðtÞ ð2:7Þ

Here x 2 Rn�1 stands for the so-called state vector and u 2 Rm�1 stands for the control

vector. For time-invariant systems matrices A 2 Rn�n, B 2 Rn�m are constant. Matrix

A 2 Rn�n is called the dynamicsmatrix andmatrixB 2 Rn�m is called the control distribution

matrix or simply the gain matrix. Similarly as for a system with one input and one output (2.1)

the solution of the vector, Equation (2.7), can be written in the form

xðtÞ ¼ eAtxð0Þþðt0

eAðt�qÞBuðqÞdq ð2:8Þ

The matrix exponential is defined as

eAt ¼ IþAtþ 1

2ðAtÞ2 þ � � � þ 1

n!ðAtÞn ð2:9Þ

The characteristic equation of n� n matrix satisfies the following relations

det ðlI�AÞ ¼ ln þ a1ln� 1 þ a2l

n� 2 þ � � � þ an ¼ 0

¼ An þ a1An� 1 þ a2A

n� 2 þ � � � þ anI ¼ 0ð2:10Þ

Similarly as for single input single output systems the solution is stable if all solutions in

Equation (2.8) go to zero as time increases. Thus, a system is stable if the real parts of all lk arenegative, or equivalently that all the roots of the characteristic Equation (2.10) have negative

real parts.


2.1.1.2 Laplace Transform, Transfer Functions

TheLaplace transform is very convenient for dealingwith LTI systems. It gives a natural way to

introduce transfer functions and opens the way for using the powerful tools of the theory of

complex variables in the control system analysis and design. Consider a function f ðtÞ definedon 0 � t < ¥ which grows slower than est for a real finite number s > 0 thusÐ¥0

e�stf ðtÞj jdt < ¥. Laplace transformation FðsÞ of f ðtÞ is then defined by

FðsÞ ¼ð¥0

e� stf ðtÞdt ð2:11Þ

The transfer function of an LTI system with input u and output y is the ratio of the

transforms of the output and the input where the Laplace transforms are calculated under

the assumption that all initial values are zero. For system (2.1), transfer function can be

expressed as

GðsÞ ¼ yðsÞuðsÞ ¼

BðsÞAðsÞ

¼ sm þ b1sm� 1 þ � � � þ bm

sn þ a1sn� 1 þ � � � þ an

¼ ðs�b1Þðs�b2Þ . . . ðs�bmÞðs�a1Þðs�a2Þ . . . ðs�anÞ

ð2:12Þ

If the degree of numerator BðsÞ does not exceed the degree of denominator AðsÞ then such atransfer function is proper deg½BðsÞ� � deg½AðsÞ�. Proper transfer function will never grow

unbounded as s tends to infinity. If the degree of numerator is lower than the degree of

denominator then such a transfer function is strictly proper deg½BðsÞ� < deg½AðsÞ�. As a

consequence, strictly proper transfer function will tend to zero as s!¥.The roots of AðsÞ are called the poles of the system. The roots of BðsÞ are called the zeros of

the system. The poles of the system are roots of the characteristic equation, thus they determine

the homogenous solution and impulse response of the system. As can be easily verified from

Equation (2.12), the zeros block inputs – thus if BðbjÞ ¼ 0 then BðbjÞCjebj t ¼ 0 and the

transmission of the signal u ¼ Cjebj t is blocked.

Systems that do not have poles and/or zeros in the right half plane are called minimum

phase systems. Systems that have zeros in the right half plane are called nonminimum

phase systems.

Inverse transform allows finding time functions corresponding to a rational transfer

function. Just by applying a partial fraction expansion transfer function (2.12), if all poles

are distinct, can be written in the form

GðsÞ ¼ yðsÞuðsÞ ¼

BðsÞAðsÞ ¼

BðsÞðs�a1Þðs�a2Þ . . . ðs�anÞ

¼ C1

ðs�a1Þ þC2

ðs�a2Þ þ � � � þ Cn

ðs�anÞ

ð2:13Þ


Here Ci; i ¼ 1; . . . ; n are residues which show relative contribution of the corresponding

fraction to the overall response of the system. The corresponding time function is

f ðtÞ ¼Xnk¼1

Ckeakt ð2:14Þ

By substituting s ¼ jv into the transfer function GðsÞ one can determine the response of a

system to sinusoidal input. The frequency response GðjvÞ may be represented by magnitude

GðjvÞj j and phase f ¼ arg½GðjvÞ�. The amplitude and phase diagrams are basis for many

methods (Nyquist, Bode plot, etc.) in the analysis and design of LTI systems.

The simplest control system (Figure 2.2) consists of the plant (object selected output of

which should be controlled), the transducers for measurement of the variables of interest and

the controller. In most cases the actuator is lumped with the plant. The signals for the system in

Figure 2.2 have the following interpretation:

. yref reference or command input

. e error

. x plant output (often system state)

. y sensor, measured output

. u control, plant input

. d disturbance

. n noise signal.

The system closed loop is composed of three blocks: the process plant P, the controller with

feedback blocks C and H, the feedforward block F and the sensor. There are three inputs: the

reference yref and two exogenous disturbances acting on the system, the load or input

disturbance d and the measurement noise n. The load disturbance (from now on the term

‘load’ will be omitted whenever it is clearly understood) is assumed acting on the plant input,

which may not be the case in all of the plants but it is applicable to motion control systems.

The plant variable x is the real physical variable that needs to be controlled. Control is based on

the measured signal y corrupted by measurement noise. The output of the system is the

measured signal y.

Disturbances are typically dominant at low frequencies. Step signals or ramp signals are

commonly used as prototypes for load disturbances. Measurement noise typically has high

frequencies and it corrupts the information about the plant variable. The average value of the

noise is typically zero. There may also be dynamics in the sensor but this is disregarded in

Figure 2.2.

Figure 2.2 Simple control system


The plant is influenced by actuators which in motion systems are typically driven

electrically, pneumatically, or hydraulically. There are often actuator local feedback loops

and the control signals can also be the reference variables for these loops. When the dynamics

of the actuators are significant it is convenient to lump them with the dynamics of the plant.

Many issues have to be considered in the analysis and design of control systems. The basic

requirements are:

. stability,

. ability to track reference signals,

. reduction of effects of disturbances,

. reduction of effects of measurement noise,

. reduction of effects of model uncertainties.

Avoiding instability is a primary goal due to the unwanted feature of feedback, reflected in the

possibility to cause instability. The reference signal tracking is second to the stability issue. The

system should also be able to reduce the effect of disturbances. The reaction on measurement

noise, the system uncertainties and the change in system dynamics are important issues. The

control system should be able to cope with moderate changes in the plant. In a motion control

system, the ability to follow a reference signal, reduce the influence of load disturbance and

cope with parameters changes are of primary concern.

2.1.2 Basic Relations

The feedback loop in Figure 2.2 is influenced by three external signals and there are at least

three signals (plant output, measured output, control) that are of interest for control – thus nine

relations between the inputs and the outputs should be evaluated. In addition difference

between reference andmeasured signal – the control error – is important. Let us assume that the

system under analysis is a LTI plant. Then blocks in Figure 2.2 can be interpreted as transfer

functions – thus relations betweenvariables can be expressed in terms of the transfer functions.

The following relations are obtained from the block diagram in Figure 2.2.

xðsÞyðsÞuðsÞ

2664

3775 ¼

FCP

1þHCP� P

1þHCP� HCP

1þHCP

FCP

1þHCP� P

1þHCP

1

1þHCP

FC

1þHCP� HCP

1þHCP� HC

1þHCP

2666666666664

3777777777775

yref ðsÞdðsÞnðsÞ

2664

3775 ð2:15Þ

The control error can be determined from the above relations in the following form

eðsÞ ¼ F

1þHCPyref ðsÞþ HP

1þHCPdðsÞ� H

1þHCPnðsÞ ð2:16Þ

These relations allow investigation of interaction between selected variables. Analysis

shows that all relations are defined by the following set of transfer functions


xðsÞyref ðsÞ ¼

PCF

1þPCH

uðsÞyref ðsÞ ¼

CF

1þPCH

xðsÞyref ðsÞ

��F¼1

¼ PC

1þPCH

uðsÞyref ðsÞ

��F¼1

¼ C

1þPCH

xðsÞdðsÞ

��F¼1

¼ � P

1þPCH

yðsÞnðsÞ

��F¼1

¼ 1

1þPCH

It follows that four transfer functions are required to describe system reaction to disturbance

and the measurement noise. Two transfer functions are required to describe system response to

set point changes. In the systemswithout feedforward term (F ¼ 1) only four different transfer

functions (second and third column) describe the behavior of the system. Unity feedback

systems without fedforward term (F ¼ 1; H ¼ 1) are described by the following four transfer

functions

xðsÞyref ðsÞ

��F ¼ 1

H ¼ 1

¼ PC

1þPC

uðsÞyref ðsÞ

��F ¼ 1

H ¼ 1

¼ C

1þPC

xðsÞdðsÞ

��F ¼ 1

H ¼ 1

¼ � P

1þPC

yðsÞnðsÞ

��F ¼ 1

H ¼ 1

¼ 1

1þPC

Four unity feedback and unity feedforward term these transfer functions are known as:

S ¼ 1=ð1þPCÞ the sensitivity function

xðsÞ=dðsÞ ¼ P=ð1þPCÞ ¼ SP the disturbance sensitivity function

xðsÞ=nðsÞ ¼ PC=ð1þPCÞ ¼ SPC the noise sensitivity function

xðsÞ=yref ðsÞ ¼ PC=ð1þPCÞ ¼ SPC the complementary sensitivity function.

A good insight into the properties of these transfer functions is essential for understanding the

behavior of feedback systems. The fact that six relations are required to capture the properties

of a basic feedback loop is often neglected in the literature. In most texts, the discussion is

concentrated on the response of the process variable to set point changes. Such analysis gives

only partial information. It is essential to look at all six transfer functions (four if F ¼ 1) in

order to fully show the behavior of the system. This is especially important in setting the

specification of the system.

2.1.3 Stability

Stability may be regarded either as a property of the plant or as a consequence of introducing

feedback into a dynamic system. Stability analysis of a feedback system can be done by

investigating the roots of the equations describing the dynamics of a closed loop. In order to

make analysis simpler and without losing any generality we will concentrate on the unity

feedback systemwithout a feedforward term (F ¼ 1; H ¼ 1). Then the overall behavior of the

system (2.15) can be written as


xðsÞyðsÞuðsÞ

2664

3775 ¼ 1

1þCP

CP �P �CP

CP �P 1

C �CP �C

2664

3775

yref ðsÞdðsÞnðsÞ

2664

3775 ð2:17Þ

By writing the plant and controller transfer functions as a ratio of polynomials with no

common factors P ¼ NP=MP and C ¼ NC=MC (2.17) can be expressed as

xðsÞyðsÞuðsÞ

264

375 ¼ 1

NPNC þMPMC

NPNC �NPMC �NPNC

NPNC �NPMC MPMC

MPNC �NPNC �MPNC

264

375 yref ðsÞ

dðsÞnðsÞ

24

35 ð2:18Þ

The system will be stable if the characteristic polynomial M ¼ NPNC þMPMC does not

have roots with a positive real part. In general this does not guarantee that some internal

variable is not unstable. In our case, the assumption that there are no common factors between

polynomialsNP;MP;NC;MC assures that there is no common factor between the characteristic

polynomials and all other polynomials NPNC;NPMC; . . . and so on – thus assuring that all

transfer functions are stable. A system with such a property is called internally stable. The

internal stability can be extended to system (2.15) if the transfer function ð1þHCPÞ does nothave zeros with a positive real part and that there is no pole-zero cancellation in the right half

plane when HCP is formed.

2.1.4 Sensitivity Function

Changes in the plant dynamics exist in control systems and it is important to analyze the

influence of these changes on systembehavior. Denoting the transfer function from reference to

output (so-called loop transfer function) as T ¼ ðx=yref Þ ¼ CP=ð1þHCPÞ for F ¼ 1 one can

find its change due to the variation in plant just by differentiating it with respect to the plant

transfer function. The transfer function derived in such a way has the following form

dT

dP¼ C

ð1þHCPÞ2 ¼1

1þHCP

CP

1þHCP

1

P¼ 1

1þHCP

T

P

dT

dP

P

T¼ dðlog TÞ

dðlog PÞ ¼1

1þHCP¼ S

ð2:19Þ

The relative error in the closed loop transfer function is small when the value of the

sensitivity function S is small. This is one of the very useful properties of feedback systems. A

small value of the sensitivity function means that the effect of plant perturbations is small or

negligible. For unity feedback systems without a feedforward term (F ¼ 1; H ¼ 1) the

sensitivity transfer function S and the loop transfer function T satisfy relation Sþ T ¼ 1.

Thismeans that S andT cannot bemade small at the same frequency. The loop transfer function

CP is typically large for small frequencies (thus T !s! 0 1) and it goes to zero as the frequency

goes to infinity (thus T !s!¥ 0). This means that S is typically small for low and close to 1 for

high frequencies.


2.1.5 External Inputs

The behavior of the system with respect to external signals is a vital part of the controller

design. For overall systemperformance it is important to look at systembehaviorwhen external

inputs are changing.

2.1.5.1 Disturbance and Measurement Noise

The easiest way to understand the behavior of a feedback system with changes or disturbances

is to compare it with an open loop. The measured output for open loop system yolðsÞ as afunction of the load disturbance and measurement noise can be expressed as

yolðsÞ ¼ nðsÞ�PdðsÞ ð2:20Þ

The measured output for closed loop system yclðsÞ due to disturbances (noise, input

disturbance) is

yclðsÞ ¼ 1

1þHCPnðsÞ� P

1þHCPdðsÞ ¼ S nðsÞ�PdðsÞ½ � ¼ SyolðsÞ ð2:21Þ

The sensitivity function directly shows the effect of the load disturbance and the measure-

ment noise on the measured output. The maximum value of the amplitude of the sensitivity

function shows the largest magnification of disturbances and the corresponding frequency on

which it occurs. Typically disturbances are attenuated at low frequencies what may not be the

best performance regarding the suppression of measurement noise.

Using a feedback system to compensate disturbance has one important disadvantage. In

order to react, a controller needs an error input – thus full elimination of the disturbance is not

possible. Theremay be some other structural ways to deal with disturbances outside a feedback

loop which can bring different system behavior regarding disturbance compensation. Some

methods related to such compensation of disturbances are discussed later in this chapter.

2.1.5.2 Reference Input

The reference signal defines the tracking goal of the system. The closed loop transfer functions

from reference input to output and to control are given as

yclðsÞ ¼ FCP

1þHCPyref ðsÞ ¼ FTyref ðsÞ

uclðsÞ ¼ FC

1þHCPyref ðsÞ ¼ SFCyref ðsÞ

ð2:22Þ

The response to the input signal is given as the product of the feedforward transfer functionF

and loop transfer function T . If the closed loop transfer function T gives a satisfactory transient

on both the reference and the disturbance (which is a rare case) then the feedforward term can

be kept equal to one (F ¼ 1). In most systems a closed loop transfer function is selected to

ensure a good disturbance rejection and the feedforward transfer function is then used to obtain


the desired transient on the reference input. For the desired closed loop dynamics from

reference input to output ycl=yref ¼ G and given a closed loop transfer function T from (2.22), a

feedforward term F ¼ G=Tcan be derived. Feedforward transfer function must be stable since

F is not taken into account in determining the system stability. That requires all right half plane

zeros of C;P to be taken as zeros of G.

There are many other very interesting and important issues related to control system design

using transfer functions, but they are out of the scope of this text. Good references treating in

details control system design can be found in [1].

2.2 State Space Representation

In dynamical systems, the state is represented by a set of variables that permits prediction of the

future behavior of a system if inputs to the system are known. In general for an engineering

system, the state is composed of the variables required to account for the storage of mass,

momentum and energy, as shown in Chapter 1. Thismakes it natural to use state variables in the

analysis and design of control systems.

For control purposes the behavior of a system is represented by a state spacemodel in which

the state variables are represented by a state vector x 2 Rn�1, the control variables by a control

vector u 2 Rm�1, the measured signal by the vector y 2 Rp�1 and the exogenous disturbances

are represented by vector d 2 Rl�1. The dimension of the state vector is called the order of

the system.

In general a dynamic system can be described by two functions: (i) a vector function

fðx; u; d; tÞ 2 Rn�1 (linear or nonlinear)which specifies the rate of change (velocity) of the state

vector and (ii) a function gðx; u; tÞ 2 Rp�1 specifying the measured output of the system

_x ¼ fðx; u; d; tÞy ¼ gðx; u; tÞ

ð2:23Þ

The system in Equation (2.23) is called time-invariant if functions fðx; u; dÞ and gðx; uÞ donot depend explicitly on time. A time-invariant system is called linear in control if functions

fðx; u; dÞ and gðx; uÞ are linear in u. A system linear in control can be represented by

_x ¼ fðx; dÞþBu

y ¼ gðxÞþDuð2:24Þ

Here matrix B 2 Rn�m; rank ðBÞ ¼ m can be constant or variable. Matrix D 2 Rp�m is

assumed constant. System (2.23) can be regarded as a nonlinear system which is linear in

control input.Mathematicalmodels ofmostmotion control systems can be represented as time-

invariant systems linear in control.

A system is called linear time-invariant (LTI) if the functions fðx; u; dÞ and gðx; uÞ do not

depend on time and are linear in x and in u. A LTI system can be represented by

_x ¼ AxþBuþHd

y ¼ CxþDuð2:25Þ


Here A 2 Rn�n, B 2 Rn�m, H 2 Rn�l , C 2 Rp�n and D 2 Rp�m are constant matrices.

Matrix A 2 Rn�n is called the dynamics matrix, matrix B 2 Rn�m is called the control

distribution or gain matrix, matrix H 2 Rn�l is called the disturbance matrix, matrix

C 2 Rp�n is called the sensor matrix and matrix D 2 Rp�m is called the direct term.

Frequently, systems will not have a direct term which indicates that the control signal does

not influence the output directly. MatrixB 2 Rn�mshows how controls are distributed among

states and thus energy-storage elements. Its rank shows how many independent control

actions are in the systems. Very similar is the role of the matrix H 2 Rn�l which shows the

distribution of the disturbances to the system states, thus it determines the effect of the

external inputs to the states of the system.

Further we will assume that disturbance satisfies matching conditions Hd ¼ Bl – in other

words, disturbance may be expressed as a product of gain matrix and unknown multiplier

l 2 Rm�1. Then Equation (2.25) can be rewritten as _x ¼ AxþBðuþ lÞ ¼ AxþBu0 withu0 ¼ uþ l – thus the influence of the disturbance can be compensated by selecting u ¼ u0 � l.Note thatl does not stand for the original disturbancevectord 2 Rl�1 but for its projection into

the range space of control distribution matrix B.In the Laplace domain system Equation (2.25) is described by

ðsI�AÞxðsÞ ¼ BuðsÞþHdðsÞ

yðsÞ ¼ CxðsÞþDuðsÞð2:26Þ

The matrix transfer functions then can be derived in the following form

xðsÞ ¼ ðsI�AÞ� 1BuðsÞþ ðsI�AÞ� 1

HdðsÞ

yðsÞ ¼ ðCðsI�AÞ� 1BþDÞuðsÞþCðsI�AÞ� 1

HdðsÞð2:27Þ

2.2.1 State Feedback

To make analysis simpler assume all components of the state vector are measured

C ¼ I ) y ¼ x and D ¼ 0, as shown in Figure 2.3. Control should be selected such that

state vector x 2 Rn�1 tracks its reference xref 2 Rn�1

Figure 2.3 Diagram of a linear time-invariant system with state feedback


For the LTI system (2.25), it is natural to assume that feedback is restricted to be linear –

thus it can be written in the following form

u ¼ Fðxref � xÞ ð2:28Þ

By plugging Equation (2.28) and D ¼ 0 into Equation (2.25) the behavior of closed loop

system is obtained as

_x ¼ ðA�BFÞxþBFxref þHd

y ¼ xð2:29Þ

The dynamics of the system has been changed and is now determined by matrix ðA�BFÞ.In order to set the desired behavior of the closed loop system all eigenvalues ofmatrix ðA�BFÞneed to be set by appropriate selection of the feedback matrix F 2 Rm�n – or in other words

system (2.25) should be controllable.

A linear system is said to be controllable if it can be transferred from any initial state xðt0Þ toany final state xðt1Þ within a finite time t1 � t0. The necessary and sufficient conditions for

controllability of the system (2.25), with disturbance satisfyingmatching conditions, is that the

controllability matrix

=x ¼ B AB A2B . . . An� 1B� � ð2:30Þ

has full row rank. Equivalent is the condition thatmatrix sI�A B½ � has full rank for all s. Thepair fA;Bg is said to be fully controllable if (2.30) is satisfied. Consequently the eigenvalues ofmatrix ðA�BFÞ, describing closed loop dynamics of system (2.25) with control (2.28), can be

assigned. There are many algorithms for assigning eigenvalues of matrix ðA�BFÞ [2]. Outputcontrollability can be defined similarly. If output controllability matrix is =y ¼ CB CAB½CA2B . . .CAn� 1BD� has full row rank the system (2.25) is output controllable. Herewewould

like to discuss some other properties of system (2.25) that may be directly applicable in motion

control systems.

In control systems change of variables is often required for simpler description of the task.

For system (2.25) the new set of variables is expressed as x0 ¼ Tx where T 2 Rn�n is regular

matrix. Assuming that matching conditions Hd ¼ Bl hold, then BuþHd ¼ Bðuþ lÞ. Thenthe dynamics of (2.25) in a new set of variables can be written as

_x0 ¼ TAT� 1x0 þTBðuþlÞ ð2:31Þ

It is easy to show that controllability of the system has been preserved [1] and disturbance

matching conditions are preserved in the transformed system as well.

The specific structure of system (2.31) can be obtained if transformationmatrixT 2 Rn�n is

selected such that the following condition holds

TAT� 1 ¼A11 A12

A21 A22

" #; TB ¼

0ðn�mÞ�m

ðBTBÞm�m

24

35 ) T ¼

T1ð Þ n�mð Þ�n

BT� �m�n

24

35;T1B ¼ 0 n�mð Þ�m ð2:32Þ


With such coordinates, the transformation system (2.31) can be rewritten as

_x1 ¼ A11x1 þA12x2

_x2 ¼ A21x1 þA22x2 þðBTBÞðuþ lÞð2:33Þ

Structure (2.33) is presented in so-called regular form. It has been proven in [3] that, if pair

ðA;BÞ is fully controllable, then ðA11;A12Þ and ðA22;BTBÞ are also fully controllable. In (2.33)

the first row describes a n�m dimensional system independent of the external inputs to the

system (control, disturbance) with vector x1 2 Rðn�mÞ�1 as output. The second row describes

the dynamics of m-dimensional vector x2 2 Rm�1 with m-dimensional external input

ðuþ lÞ 2 Rm�1 and square full rank gain matrix BTB with rank ðBTBÞ ¼ m.

The control design for system (2.33) is arranged in two steps. In the first step, state x2 is

treated as a virtual control input in _x1 ¼ A11x1 þA12x2 and linear feedback x2 ¼ �Lx1 is

selected such that x1 has the desired dynamics. Since the pair ðA11;A12Þ is controllable this stepyields the desired matrix L from the assignment of eigenvalues of matrix ðA11 �A12LÞ. In thesecond step, the control ðuþ lÞ in _x2 ¼ A21x1 þA22x2 þðBTBÞðuþ lÞ can be selected to

enforce x2 ¼ �Lx1, obtained in the first step.

Since the second equation has the same number of control inputs as its number of states, it is

easy to transform it into a system with a diagonal control matrix. By defining a new set of

control variables v ¼ ðBTBÞðuþ lÞ, the second row in (2.33) may be rewritten as

_x2 ¼ A21x1 þA22x2 þ v ð2:34Þ

All components of the state vector are assumedmeasured so both x1 and x2 are available. Let

control vector v in (2.34) be

v ¼ �ðA21 þLA11Þx1 �ðA22 þLA12Þx2 �Kðx2 þLx1Þ;K > 0 ð2:35Þ

Then insertion of control (2.35) into (2.34) leads to the following dynamics

_s ¼ _x2 þL _x1 ¼ �Kðx2 þLx1Þ ¼ �Ks

s ¼ x2 þLx1 !K>00

x2 !t!¥ �Lx1

ð2:36Þ

By selecting K > 0, sufficiently large transients in system consisting of the first row

in Equations (2.33) and (2.36) could be analyzed as a singularly perturbed system [4].

Equation (2.36) represents fast motion then, after a steady state is reached in (2.36),

slow motion [represented by the first row in Equation (2.33) and x2 ¼ �Lx1] can be

written as

_x1 ¼ ðA11 �A12LÞx1 ð2:37Þ

In Section 2.3 wewill show in more detail the behavior of systems that guarantee finite time

convergence to solution x2 ¼ �Lx1.

Note that the selection of control in Equations (2.34) and (2.37) is for systems of lower order

than the original system. The original control input can be expressed as


v ¼ ðBTBÞðuþlÞ ) u ¼ �lþðBTBÞ� 1v

u ¼ �l�ðBTBÞ� 1½A21x1 þA22x2 þL _x1 þKðx2 þLx1Þ�ð2:38Þ

Dynamics [Equations (2.36), (2.37)] do not depend on the disturbance and matrices

ðA21;A22Þ. Slow motion is determined by selection of feedback matrix L.

Nonlinear systems that are linear in control can be analyzed in a very similar way. Assuming

that disturbance satisfies the matching conditions, then system (2.24) can be represented in a

regular form

_x1 ¼ f1ðx1; x2Þ_x2 ¼ f2ðx1; x2ÞþB2ðuþlÞ

ð2:39Þ

Matrix B2 2 Rm�m has full rank. By assigning a new control input v ¼ B2ðuþ lÞ, thesecond row in Equation (2.39) may be written as _x2 ¼ f2ðx1; x2Þþ v and by selecting

v ¼ � f2ðx1; x2Þ� _sðx1Þ�K½x2 þsðx1Þ�;K > 0 ð2:40Þ

the dynamics of the closed loop will be reduced to

_x2 þ _sðx1Þ ¼ �K½x2 þsðx1Þ�x2 þsðx1Þ !K>0

0

x2 !t!¥ �sðx1Þ

ð2:41Þ

By applying the methods of singularly perturbed systems, slow dynamics (2.39) and (2.41)

can be found in the following form

_x1 ¼ f1½x1; �sðx1Þ� ð2:42Þ

In Equation (2.42) sðx1Þ should be selected to ensure stability and the desired dynamics.

The original control input can be expressed as

v ¼ B2ðuþlÞ ) u ¼ � lþB� 12 v

u ¼ �l�B� 12 ff2ðx1; x2Þþ _sðx1ÞþK½x2 þsðx1Þ�g

ð2:43Þ

Similarly as in the LTI system (2.38), the control consists of the disturbance, part of the

system dynamics and the desired dynamics of the subsystem represented by the second row

in Equation (2.39). The procedure used in control design is simple. It consists of transforming

the system in a regular form and then designing the control for two subsystems of a lower

order. The structure obtained as the result of such a design, in a sense, stands for a cascade

control system in which the term Lx1 or sðx1Þ is selected to enforce the desired dynamics of

slow motion.

In previous analyses the availability of all states and a full knowledge of the system structure

and parameters have been assumed. Neither of these assumptions is realistic. Due to this,

the presented methods in an application to a real system require additional efforts to acquire

the necessary information and to cope with system uncertainties.


2.2.2 Stability

The question of stability arises in examining what happens near, but not at, the equilibrium.

The question is canwe be sure that in the vicinity of equilibrium there are no initial conditions

such that the system would be drawn to a different steady state. Loosely speaking can we at

least guarantee that nothing too peculiar happens at only a short distance away from the

equilibrium?

As shown in Section 2.1 and illustrated in Figure 2.4, if the pole ak has a real part which is

strictly negative, we speak about asymptotic stability. The variables of an asymptotically stable

control system always decrease to their equilibrium and do not show permanent oscillations.

Permanent oscillations are present when a pole has a real part exactly equal to zero. If a stable

system response neither decays nor grows over time and has no oscillations, it is referred to as

marginally stable. Oscillations are present when poles with a real part equal to zero also have a

complex part not equal to zero.

Stability in control systems oftenmeans that for any bounded input over any amount of time,

the outputwill also be bounded. This is known asBIBO stability. If a system isBIBOstable then

the output cannot ‘blow up’ if the input remains finite. Mathematically, this means that, for a

continuous LTI system to be BIBO stable, all of the poles of its transfer function must lie in the

closed left half of the complex plane if the Laplace transform is used (i.e., its real part is less

than or equal to zero).

The stability analysis of nonlinear systems is much more complicated than that for LTI

systems. In 1892 the Russian mathematician Alexander Mikhailovitch Lyapunov introduced

his famous stability theory for nonlinear and linear systems. The stability used in this text

corresponds to the Lyapunov stability definition, so that ‘stable’ used in this text also means

‘stable in the sense of Lyapunov’. According to this idea the stability of a LTI system can be

checked if one can find some function VðxÞ called the Lyapunov function that shows how far

the solution is from equilibrium, which satisfies both (a) VðxÞ > 0; Vð0Þ ¼ 0 and (b)_VðxÞ ¼ ðqV=qxÞðdx=dtÞ � 0. Condition (b) says that the derivative of VðxÞ computed along

the trajectories of system is nonpositive – thus VðxÞ does not increase. In other words if the

solution is close to equilibrium it must remain close to equilibrium. For nonlinear systems there

is no general procedure for finding Lyapunov functions. If Lyapunov stability is achieved, then

the system, while not necessarily settling on any one value, stays bounded. Lyapunov stability is

not a very strong stability requirement; it does not mean that solutions ‘near’ to the equilibrium

are ‘pulled’ to the equilibrium over time. This stronger notion is asymptotic stability.

The Lyapunov stability theorem states for a nonlinear system _x ¼ fðxÞ with equilibrium

fð0Þ ¼ 0, the solution of equilibrium is asymptotically stable if there exists a Lyapunov

Figure 2.4 Illustration of system behavior from the stability point of view


function candidate VðxÞ such that _VðxÞ is strictly negative definite along solutions of

_x ¼ fðxÞðaÞ VðxÞ > 0; Vð0Þ ¼ 0

ðbÞ _VðxÞ ¼ qVqx

dx

dt< 0

ð2:44Þ

The strict inequality means VðxÞ is actually decreasing along system trajectories. The

VðxÞ ¼ c > 0 defines the so-called level surface of VðxÞ. The solution xðtÞ of system _x ¼ fðxÞis uniformly bounded with respect to the level surface VðxÞ ¼ c > 0 if_VðxÞ ¼ ðqV=qxÞðdx=dtÞ � 0 for x outside of VðxÞ ¼ c > 0. In other words under the above

conditions trajectories starting outside level surface VðxÞ ¼ c > 0 are pointing towards the

level surface VðxÞ ¼ c > 0.

The global stability in a Lyapunov sense is achieved if the Lyapunov function is radially

unbounded or if VðxÞ!¥ as x!¥.The exponential stability in a Lyapunov sense is guaranteed if there are positive real

constants g and h such that xðtÞk k � g xð0Þk ke�ht; t > 0.

For LTI system _x ¼ Ax, the Lyapunov function candidate can be selected as a qua-

dratic form V ¼ xTPx. Here P ¼ PT > 0 is a positive definite symmetric matrix, thus_V ¼ xTðATPþPAÞx. The system _x ¼ Ax is said to be stable if there exists a solution P of

the so-called Lyapunov equation ATPþPA ¼ �Q for a positive definite symmetric

Q ¼ QT > 0. This can be expressed in a different way by looking at the solution of the

system _x ¼ Ax. If all eigenvalues of matrix A 2 Rn�n have a negative real part, then the

equation ATPþPA ¼ �Q has a unique positive definite solution for P, therefore the LTI

system is stable.

2.2.3 Observers

While discussing control system design in previous sections, the assumption has been readily

used that the system states are available. Measurement of the system states may not be an easy

task and during design, one often has to rely on estimates. That brings up the question how

system state can be estimated by using the available information: measurements, control input

and the plant model.

Before beginning to discuss structures and design methods one should make sure that the

task in hand can be realized or that the system is observable. The question is related to the

possibility to compute the state of the system frommeasurements ðy; uÞ and known parameters

of the system ðA;B;CÞ.It may be shown that the state could be computed from the output if the system is observable.

LTI system is observable if the observability matrix

CT ATCT ðATÞ2CT . . . ðATÞn� 1CT

� � ð2:45Þ

has full row rank. Observability is a property of the system related to the state dynamics matrix

A and the measurement matrix C. Essentially it determines the minimum set of measured

variables necessary to calculate (estimate) the state of the system.


In further analysis wewill assume that the systems under consideration are observable. The

task in this section is to show some methods for determining the state of the system from

measured inputs and outputs. In developing this, we will assume that parameters ðA;B;CÞ areknown – thus we are able to establish a dynamic structure that plays a role in the plant. The

structure allowing state estimation from available measurements and, additionally, the con-

structed dynamical system of an observable system are called observers.

In order to simplify analysis, we assume a LTI system without direct term D ¼ 0 and

without exogenous disturbance input H ¼ 0. In constructing a dynamical system based on

available measurements ðy; uÞ and known plant parameters ðA;B;CÞ, it is natural that theconstructed system has the same structure as the plant. What we are doing is establishing a

virtual plant or an ‘image’ of the plant. Since the parameters and inputs to the real plant and

its ‘image’ are the same, one could expect that changes in the state and the output of the plant

and the ‘image’ are the same. That does not necessarily mean that the measured output of the

plant y and the output of the ‘image’ system are the same. Since these outputs are available

one can compare them – find the error – and act on the ‘image’ system so that the error is

driven to zero. Loosely speaking this is the basic idea of state observers. They incorporate a

system ‘image’ and the feedback from a measured output in order to enforce tracking of the

real output by the output of the ‘image’ system. The structure showing just such a described

situation is depicted in Figure 2.5.

The observer can be described by

_x ¼ AxþBuþLðy� yÞ ð2:46Þ

It is easy to find the error between output of the plant and the observer

_x ¼ AxþBu

_x ¼ AxþBuþLðy� yÞ

)) _x� _x ¼ ðA�LCÞðx� xÞ ð2:47Þ

Figure 2.5 Structure of the state observer and its relation to the plant


In Equation (2.47), matrixL shows the distribution of measurements between system states

(similarly as control matrix B shows the distribution of control among states). It should be

selected to ensure the desired placement of the eigenvalues of the error system. This points to

the fact that the problem of determining the matrix L such that ðA�LCÞ has prescribed

eigenvalues is very similar to the pole placement problem that was discussed in Section 2.2.1.

Since the eigenvalues of a matrix and its transpose are the same it follows that L should be

determined such that ðAT �CTLTÞ has given eigenvalues. Note that in dealing with control

we concluded that, for controllable systems, the eigenvalues of the matrix ðA�BFÞ could beselected if the pair ðA;BÞ is controllable. It then follows that the eigenvalues of the matrix

ðAT �CTLTÞ can be replaced by an appropriate selection of matrix L if the pair ðAT ;CTÞ iscontrollable, thus if condition (2.45) are satisfied.

Observer (2.46) has the order of the state vector. From the measurement equation y ¼ Cx,

rank ðCÞ ¼ m, m components of the state vector can be determined as a function of the

remaining n�m components. Let the measurement vector be partitioned as

y ¼ Cx ¼ C1 C2½ �x1

x2

" #;

C1 2 Rm�ðn�mÞ;C2 2 Rm�m; rank ðC2Þ ¼ m

ð2:48Þ

As shown inEquation (2.39), the transformationmatrixT as in (2.49)would transform aLTI

system ðA;B;CÞ into two subsystems (2.50)

T ¼In�m 0

C1 C2

" #; TAT� 1 ¼

A11 A12

A21 A22

" #;

TB ¼B1

B2

" #; Tx ¼

x1

y

" # ð2:49Þ

dx1

dt¼ A11x1 þA12yþB1u

dy

dt¼ A21x1 þA22yþB2u

ð2:50Þ

In Equation (2.50) vector x1 2 Rðn�mÞ�1, x2 2 Rm�1 and since rankðC2Þ ¼ m from (2.48)

follows x2 ¼ C� 12 ðy�C1x1Þ. In order to estimate the whole state vector it is enough to

estimate only x1.

As shown in Section 2.2.1 regular transformation of the variables preserves the control-

lability and observability features of the system. It can be proven that pair ðA11;A21Þ is

observable if pair ðA;CÞ is observable [3]. The dynamics (2.50) in a new set of variables ðz; yÞwith z ¼ x1 þLy can be written as

z ¼ x1 þLy )_z ¼ ðA11 þLA21ÞzþAzyþðB1 þLB2Þu_y ¼ A21zþðA22 �LA21ÞyþB2u

Az ¼ ðA12 þLA22Þ� ðA11 þLA21Þ

8>><>>: ð2:51Þ


MatrixL 2 Rðn�mÞ�m shows the distribution of the measurement to the components of new

variable z 2 Rðn�mÞ�1. Since the output is known, the observer can be built only for variable

z 2 Rðn�mÞ�1. So the constructed systemwill be of a lower dimension than the original system

for the number of linearly independent measurements and can be written is the following form

_z ¼ A11 þLA21ð ÞzþAzyþ B1 þLB2ð Þu ð2:52Þ

Consequently the observer error is

ð _z� _zÞ ¼ ðA11 þLA21Þðz� zÞ ð2:53Þ

The observability of pair ðA11;A21Þ allows selection of observer gain matrix L so that the

eigenvalues of matrix ðA11 þLA21Þ are placed in accordance with design requirements – thus

L can be selected such that estimation error converges to zero. From z, n�m components of

state vector can be determined as x1 ¼ z�Ly and from y ¼ C1x1 þC2x2; rank ðC2Þ ¼ m the

remainingm components of the state vector can be determined as x2 ¼ C� 12 ½y�C1ðz�LyÞ�.

This demonstrates the effectiveness of usingmeasurement information in lowering the order of

the observer.

The full order observer (2.46) is based on enforcing tracking ofmeasured output of the plant

by observers output. Application of the same idea to system (2.50) would lead to designing an

observer in the following form [3]

_x1 ¼ A11x1 þA12yþB1u�Lv

_y ¼ A21x1 þA22yþB2uþ v ð2:54Þ

Here v 2 Rm�1 is the observer control input to be selected in such a way that y tracks the

measured output y of the plant.

From Equations (2.50) and (2.54) estimation errors can be derived as

_ex1 ¼ A11ex1 þA12ey �Lv

_ey ¼ A21ex1 þA22ey þ vð2:55Þ

Assume that control v 2 Rm�1 is enforcing ey ¼ 0 and _ey ¼ 0. Then from _ey ¼ 0 the control

v ¼ veq can be derived as vj ey ¼ 0_ey ¼ 0

¼ �A21ex1. Then the error in estimation of x1 2 Rðn�mÞ�1

becomes

_ex1 ¼ A11ex1 þA12ey �Lveq ¼ ðA11 þLA21Þex1y ¼ C1x1 þC2x2 ) x2 ¼ C� 1

2 ðy�C1x1Þð2:56Þ

This works very well if ey ¼ 0 is reached in finite time. If control is designed with

asymptotic convergence then singularly perturbed systems methods can be used to verify

the validity of the result.

2.2.4 Systems with Observers

Let us now return to the control problem for a LTI plant assuming that feedback is based on the

estimated state. Assume the controllable and observable system as described in Equation (2.25)


withD ¼ 0,H ¼ 0, full state observer (2.46) and observer error e ¼ x� x. Then control (2.28)

can be written as

u ¼ Fðxref � xÞ ð2:57ÞThe closed loop dynamics together with observer error dynamics can be expressed as

_x ¼ Ax�BFxþBFxref ¼ ðA�BFÞxþBFeþBFxref

_e ¼ ðA�LCÞe ð2:58Þ

System (2.58) describes 2n-dimensional augmented system dynamics (n-dimensional

closed loop plant dynamics, n-dimensional observer error dynamics)

_x_e

� �¼ ðA�BFÞ BF

0 ðA�LCÞ� �

xe

� �þ BFxref

0

� �ð2:59Þ

In Equation (2.59) the eigenvalues of the augmented system are defined by both feedback

gain F and observer gain L. By separating the dynamics of the observer and the feedback

system into fast (observer dynamics) and slow (feedback dynamics), one can design observer

and feedback gains separately. That does not change the fact that augmented system will have

eigenvelues set by both the observer and the feedback. It is just a way of making design easier.

2.2.5 Disturbance Estimation

Until now in all designs of observers we have assumed that disturbance is not acting on the

system. Let us now return to systems withmatched disturbance – the same states are influenced

by the disturbance and by the control input Hd ¼ Bl. Then a model of the controllable and

observable system with D ¼ 0 can be written as

_x ¼ AxþBðuþlÞy ¼ Cx

ð2:60Þ

Assume that matched input disturbance can be modeled as the output of a known stable

dynamical system

_l ¼ All; Al 2 Rm�m; l 2 Rm�1 ð2:61Þ

Here Al is a known full row rank matrix. By combining Equations (2.60) and (2.61) the

dynamics of the augmented system with a new state variable zT ¼ x l½ � yields

_x

_l

" #¼

A B

0 Al

" #x

l

" #þ

B

0

" #u;

y ¼ Cxþ 0l

z ¼x

l

" #; Az ¼

A B

0 Al

" #; Bz ¼

B

0

" #; Cz ¼ C 0½ �

_z ¼ AzzþBzu

y ¼ Czz

ð2:62Þ


The augmented system is of nþm order. In order to look at ideal behavior, let the state

vector and the projected disturbance vector x;l are available. Full state feedback for

system (2.62) can be written as

u ¼ �FzzþFrxref ¼ �Fxx�FllþFrx

ref ð2:63Þ

Here Fz ¼ Fx½ jFl�and Fr are matrices of appropriate dimensions. The dynamics of a closed

loop for system (2.62) with state feedback (2.63) yields

_x ¼ ðA�BFxÞxþðB�BFlÞlþBFrxref

_l ¼ All

y ¼ Cxþ 0l

ð2:64Þ

As expected, l is not controllable. Its influence on x can be eliminated if the disturbance

feedback matrix Fl is selected to satisfy ðB�BFlÞ ¼ 0, thus Fl ¼ I 2 Rm�m, i.e., if one is

able to determine not necessarily original disturbances but their projections into the range space

of the control distribution matrix, then such disturbance can be compensated just by feeding it

into the system input with the appropriate sign. Compensation requires estimation of the

projection of the disturbance instead of the original disturbance. This plays a very important

role in the motion control systems due to the complex disturbance distribution matrix H in

mechanical multibody systems.

In Equation (2.63), measurement of both state vector and the exogenous disturbance is

assumed, which is an unlikely situation in most systems. A more realistic assumption is that

both the state vector and the disturbance vector are not known – thus the state vector of an

augmented system (2.62) should be estimated. With the assumption that pair ðAz;CzÞ is

observable and pair ðu; yÞ is available (measured), using methods discussed in previous

sections the augmented full state observer can be designed in the following form

_z ¼ AzzþBzuþLxyy

_x

_l

" #¼ A B

0 Al

� �x

l

� �þ B

0

� �uþ Lx

Ll

� �ðy�CxÞ

¼ A�LxC B

�LlC Al

� �x

l

� �þ B

0

� �uþ Lx

Ll

� �y ð2:65Þ

The observer feedback matrix Lxy should be selected to provide the desired eigenvalues of

matrix Az. Observation errors can be expressed as

_x� _x

_l� _l

" #¼ _ex

_el

� �¼ A�LxC B

LlC 0

� �exel

� �ð2:66Þ

With proper selection of matrices Lx and Ll both ex ! 0 and el ! 0, thus estimations tend to

their real values. Full state feedback u ¼ �FzzþFrxref ¼ �Fxx�FllþFrx

ref for sys-

tem (2.59) yields

_x ¼ ðA�BFxÞxþðB�BFlÞlþBðFxex þFlelÞþBFrxref ð2:67Þ


If B�BFl ¼ 0 is satisfied, plant motion does not depend on the disturbance. The

augmented system is ð2nþmÞ order with dynamics (2.66) and (2.67).

In previous sections we discussed systems with continuous control. In the next section we

will look at systems which do not necessarily have continuous control input and which may

have finite time convergence of the control error. The application of such systems, especially in

designing the observers, may lead to very interesting advantages. This can be seen from the last

problem. If in Equation (2.66) the estimation errors in state and disturbance observers converge

to zero in finite time, the closed loop system (2.67) would be driven by n-th order dynamics

_x ¼ ðA�BFxÞxþBFrxref .

2.3 Dynamic Systems with Finite Time Convergence

The systems analyzed so far have a continuous control input. A large class of systems has a

discontinuity of some sort in generating control inputs, the prominent example being power

converters applied in the control of electrical machines. In this section specific methods

related to systems with discontinuous control will be reviewed in some detail, with the aim to

point out their peculiarities. Such systems may exhibit motion in a selected manifold within a

state space – so-called sliding mode – characterized by high frequency switching of the

control input and finite time convergence to such a manifold. The fact that motion is

constrained in a sliding manifold opens a range of possibilities in shaping the system behavior.

Our presentation will be limited to systems linear in discontinuous parameter – control – as

in Equation (2.68)

_x ¼ fðxÞþBðxÞuðxÞþDhðx; tÞ

ui ¼(uþi ðx; tÞ siðxÞ > 0

u�i ðx; tÞ siðxÞ < 0 i ¼ 1; . . . ;m

x 2 Rn�1; u 2 Rm�1; h 2 Rp�1;s 2 Rm�1

ð2:68Þ

It is assumed that fðxÞ; uþi ; u�

i and si are continuous functions and the control matrix has

rank BðxÞj8x ¼ m. Control undergoes discontinuity in discontinuity surfaces siðxÞ ¼ 0 and is

not defined in each of the discontinuity surfaces and in manifold sðxÞ ¼ 0. Under certain

conditions the system may be forced to exhibit motion in manifold sðxÞ ¼ 0, thus it is of

interest to find conditions for which such motion can be enforced and methods to analyze the

dynamics of system (2.68) in manifold sðxÞ ¼ 0 in which control is not defined.

Loosely speaking, if initial conditions xð0Þ are consistent with si½xð0Þ� ¼ 0, then motion

will remain in discontinuity surface siðxÞ ¼ 0 if the control maintains opposite signs of the

distance from surface and its rate of change. Themotion in discontinuity surfaces is a so-called

sliding mode motion. The problem of reaching and remaining in the discontinuity surface is

closely related to stability (Figure 2.6).

For systems linear in control, the so-called regularization method [3] allows the substan-

tiation of a so-called equivalent control method which provides a simple procedure for finding

equations of motion in sliding mode.


2.3.1 Equivalent Control and Equations of Motion

The equivalent control is defined as the solution to

_sðx; tÞ ¼ GfþGBueq þGDh ¼ 0; G ¼ qsðx; tÞqx

2 Rm�n ð2:69Þ

The rows of matrix G are gradients of functions siðxÞ. If detðGðxÞBðxÞÞ 6¼ 0 for all x the

equivalent control in manifold sðxÞ ¼ 0 can be determined as

ueq ¼ �ðGBÞ� 1GðfþDhÞ ð2:70Þ

The equivalent control (2.70) is continuous and it could be interpreted as the input needed to

maintain system motion in manifold sðxÞ ¼ c where all components of vector c are constant.

If 8t � t0 the control has value uðtÞ ¼ ueqðtÞ and s½xðt0Þ� ¼ c then s½xðtÞ� ¼ c, 8t � t0. If the

initial conditions are such that s½xðt0Þ� ¼ 0 and uðtÞ ¼ ueqðtÞ, 8t � t0 the motion will remain

in manifold s½xðt0Þ� ¼ 0.

Substitution of control (2.69) into (2.68) yields

_x ¼ fþBueq þDh

¼ ½I�BðGBÞ� 1G�ðfþDhÞð2:71Þ

If at t ¼ t0 the condition s½xðt0Þ� ¼ 0 is held then Equation (2.71), along with the equation

of sliding mode manifold sðxÞ ¼ 0, describe the ideal sliding mode motion 8t � t0. Matrix

½I�BðGBÞ� 1G� has very interesting properties with regard to matrices B;G

Figure 2.6 Illustration of motion in the vicinity of the discontinuity surface


G½I�BðGBÞ� 1G� ¼ 0

½I�BðGBÞ� 1G�B ¼ 0

ð2:72Þ

From Equation (2.72) it follows that, if vector ðfþDhÞ can be partitioned as

fþDh ¼ Blþ j, then Equation (2.71) can be rewritten as _x ¼ ½I�BðGBÞ� 1G�j. Slidingmode equations (2.71) are invariant with respect to components Bl of vector ðfþDhÞ [5].

2.3.2 Existence and Stability

The stability of nonlinear dynamics (2.68) in a sliding mode manifold can be analyzed in the

Lyapunov stability framework.Without loss of generality, let the Lyapunov function candidate

be selected as

Vðs; xÞ ¼ sTs

2ð2:73Þ

The stability requirement, for appropriate function zðV ;s; xÞ > 0, is expressed as

_Vðs; xÞ ¼ sT _s � � zðV ;s; xÞ ð2:74Þ

The equation describing projection of the system motion in a sliding mode manifold can be

rearranged into _s ¼ GB½uþðGBÞ� 1GðfþDhÞ� ¼ GBðu� ueqÞ. Control may be selected as

in (2.75) [with MðxÞ being a strictly positive scalar function]

u ¼ ueq �ðGBÞ� 1MðxÞ sign ðsÞ; MðxÞ > 0

½sign ðsÞ�T ¼ sign ðs1Þ . . . sign ðsmÞ½ �

sign ðsiÞ ¼(1 for si > 0

� 1 for si < 0

ð2:75Þ

Plugging the selected control into the derivative _Vðs; xÞ yields_Vðs; xÞ ¼ sTGBðueq �ðGBÞ� 1

MðxÞ sign ðsÞ� ueqÞ

¼ �MðxÞsTsign ðsÞ < 0

ð2:76Þ

The Lyapunov stability conditions (2.74) are satisfied and consequently a sliding mode

exists inmanifoldsðxÞ ¼ 0. The dynamics of the system state are governed by Equation (2.71)

and sðxÞ ¼ 0. From sðxÞ ¼ 0, one may determinem components of state vector as function of

the remaining ðn�mÞ components and then by substituting that into (2.71) the resulting

dynamics is of a reduced order.

2.3.3 Design

As shown, both the equations of motion in sliding mode and the projection of the system

dynamics in a sliding mode manifold are of lower order than the original system dynamics.

That permits us to decouple the design problem into two independent steps:


. In the first step, the desired closed loop dynamics is determined from Equation (2.71) and

selection of manifold sðxÞ ¼ 0.. In the second step, the control input needs to be selected to enforce motion in selected

manifold.

In order to make selection of manifold simpler it is useful to transform system (2.68) with

matched disturbance Dh ¼ Bl in regular form

_x1 ¼ f1ðx1; x2Þ x1 2 Rn�m

_x2 ¼ f2ðx1; x2ÞþB2ðx1; x2ÞðuþlÞ x2 2 Rmð2:77Þ

Matrix B2 2 Rm�m is regular. The first equation is of ðn�mÞ order and it does not dependon control. In this equation vector x2 may be handled as a virtual control. Assume that

x*2 ¼ �s0ðx1Þ is selected such that _x1 ¼ f1ðx1; x*2Þ satisfies given design criteria. Then the

dynamics described by the first equation in (2.77) can be rewritten in the following form

_x1 ¼ f1ðx1; x*2Þ ¼ f1½x1; �s0ðx1Þ� ð2:78Þ

In the second step, discontinuous control u should be selected to enforce slidingmode in the

manifold

s ¼ x2 � x*2

¼ x2 þs0ðx1Þ ¼ 0ð2:79Þ

The projection of the system (2.77) motion in the manifold (2.79) is

_s ¼ ðf2 þG0f1 þB2lÞþB2u

G0 ¼ qs0

qx1ð2:80Þ

Stability in a manifold (2.79) will be enforced if control is selected as

u ¼ ueq �B� 12 Mðx1; x2Þ sign ðsÞ; Mðx1; x2Þ > 0

ueq ¼ �B� 12 ðf2 þG0f1 þB2lÞ

ð2:81Þ

HereMðx1; x2Þ > 0 is strictly positive continuous function of the system states or constant.

To realize control (2.81), one needs to know the equivalent control, that is, full information on

the system’s structure and parameters. This is unlikely in most real control systems. If some

estimation of the equivalent control ueq andmatrixB2 are available, the structure of the control

may be modified to the form

u ¼ ueq �M*ðx1; x2Þ sign ðsÞ; M*ðx1; x2Þ > 0 ð2:82Þ

Here the estimate of equivalent control is used instead of the exact value and the

discontinuous part does not depend on the unknown gain matrix. The magnitude of discon-

tinuous term M*ðx1; x2Þ should now cover the error in the equivalent control estimation.


2.3.4 Control in Linear Systems

Linear systems aremost illustrative to demonstrate the potential of a particular control method.

Assume the task of enforcing motion in manifold sðxÞ ¼ 0 for a LTI system

_x ¼ AxþBuþDh x 2 Rn; u 2 Rm; h 2 Rp

A 2 Rn�n;B 2 Rn�m;D 2 Rn�pð2:83Þ

We further assume that pair ðA;BÞ is controllable, rank ðBÞ ¼ m and disturbance satisfies

matching conditions Dh ¼ Bl. By reordering components of vector x, matrix B can be

partitioned into two matrices B1 2 Rðn�mÞ�m and B2 2 Rm�m with rankðB2Þ ¼ m. As shown

in Equation (2.32), there exists a nonsingular transformation xT ¼ Tx such that

TB ¼ B1

B2

" #¼ 0

B2

" #ð2:84Þ

and consequently the transformed system can be written in the following form

_x1 ¼ A11x1 þA12x2 x1 2 Rn�m

_x2 ¼ A21x1 þA22x2 þB2ðuþlÞ x2 2 Rm; u 2 Rmð2:85Þ

HereAijði; j ¼ 1; 2Þ are constant matrices and the pair ðA11;A12Þ is controllable. As shownin Section 2.2.1 x2 can be treated as a virtual control in the first equation in (2.85). x

*2 ¼ �Gx1

satisfies the desired closed loop system dynamics. Thus the first equation in (2.85) becomes

_x1 ¼ ðA11 �A12GÞx1, G 2 Rm�ðn�mÞ.In the next step a control should be selected to enforce the desired change of

x*2 ¼ �Gx1rankG ¼ m, or in other words, to enforce a sliding mode in a manifold

s ¼ x2 � x*2 ¼ x2 þGx1 ¼ 0 ð2:86Þ

One of the solutions for control that will enforce a sliding mode in a manifold (2.86) is

u ¼ ueq �aB� 12

s

sk k ; a > 0; sk k ¼ffiffiffiffiffiffiffiffiffiffiffiffiffiðsTsÞ

pueq ¼ �l�B� 1

2 ½ðA21 þGA11Þx1 þðA22 þGA12Þx2�ð2:87Þ

Control (2.87) is continuous everywhere except in s ¼ 0. The closed loop transient is

defined by _x1 ¼ ðA11 �A12GÞx1 and clearly depends on the selection of amanifold (2.86). The

finite time convergence to a manifold (2.86) is enforced but overall dynamics have a

convergence defined by the eigenvalues of matrix ðA11 �A12GÞ. This is an interesting featureof systems with a sliding mode. The functional relation between system coordinates – sliding

mode manifold – (2.86) is reached within finite time but the overall dynamics are then defined

by selection of the manifold (2.86). This property may be very effectively used in the design of

observers or for example in establishing a functional relation between motion control systems

(like bilateral control).


2.3.5 Sliding Mode Based Observers

The idea of a sliding mode application in observer design rests on designing a control that

forces the output of the nominal plant (or the plant ‘image’) to track the corresponding output of

the real plant in sliding mode. Then an equivalent control can be used to determine some

parameters reflected in the difference between the nominal and real plant. Sliding mode based

state observers arewell detailed in the available literature [3]. They aremostly based on the idea

of reduced order Luenberger observers. In order to illustrate some other ideas related to sliding

mode based observers, let us look at a first-order system with scalar control input

_x ¼ f ðx; aÞþ u ð2:88Þ

Assume control input u and outputxmeasured, f ðx; aÞ unknown continuous scalar functionof state, time and parameter a. Assume a nominal plant as integrator with an input consisting of

the known part of the system structure foðx; aoÞ, a control input of the actual plant and the

additional observer control input uo

_x ¼ foðx; aoÞþ uþ uo ð2:89Þ

The observer control input uo is selected such that sliding mode is enforced in s ¼ x� x.

Equivalent control can be expressed as uoeq ¼ f ðx; aÞ� foðx; aoÞ. The equivalent control is

equal to the difference between real plant f ðx; aÞ and assumed plant foðx; aoÞ, thus the realplant unknown structure may be estimated as f ðx; aÞ ¼ foðx; aoÞþ uoeq. This estimation is

valid after a sliding mode is established. It is obvious that f ðx; aÞ ¼ foðx; aoÞþ uoeq allows

estimation of the complete function f ðx; aÞ but not its components separately.

Let Equation (2.88) be linear in the unknown parameter a and can be expressed as

f ðx; aÞ ¼ fxðxÞþ faðxÞa. Assume ðx; uÞ measured and functions fxðxÞ and faðxÞ known. Letus construct a model _x ¼ fxðxÞþ faðxÞuo þ u. If sliding mode is established on

s ¼ x� x ¼ 0 then x ¼ x and consequently fxðxÞ ¼ fxðxÞ and faðxÞ ¼ faðxÞ. In sliding

mode equivalent control becomes uoeq ¼ a, thus such a structure can be used to estimate the

system parameters.

Estimation of the system parameters can also be extended to MIMO systems. It is well

known that the dynamics of robotic manipulators can be linearly parametrized with some sets

of parameters. The selection of these parameters is neither simple nor unique. If the number of

such parameters is lower or equal to the order of the system, then application of the parameter

estimation result is straightforward. In the case that the number of parameters is higher than the

order of the system then additional constraint equations should be found. An interesting

application of this idea for vision based motion estimation is presented in [6].

2.3.5.1 State Observers

Extension of the observer design in the framework of sliding mode systems is based on the

structure (2.50) in which the original system is partitioned into two blocks – one describing the

dynamics of themeasurement y 2 Rm�1 and another describing the dynamics of the remaining

ðn�mÞ variables represented by x1 2 Rn�m�1. By expressing the measured variable esti-

mation error as ey ¼ y� y the control input v 2 Rm�1 can be selected to ensure a sliding mode


in the manifold ey ¼ y� y ¼ 0. The dynamics of the measured output control error (distance

from sliding mode manifold) can be expressed as

_ey ¼ A21ðx1 � x1ÞþA22ey þ v ð2:90Þ

Control input v ¼ �Msignðy� yÞ enforces a slidingmode in ey ¼ 0. HereM > 0 is a large

enough constant and signðy� yÞ is a component-wise sign function. After tracking of the

measured output is reached the equivalent control becomes veq ¼ �A21ðx1 � x1Þ ¼�A21ex1. Insertion veq into _x1 ¼ A11x1 þA12yþB1u�Lveq yields an estimation error of

x1 2 Rn�m�1

_ex1 ¼ A11ex1 �Lveq ¼ A11ex1 �Lð�A21ex1Þ¼ ðA11 þLA21Þex1

ð2:91Þ

In comparison with a continuous system framework design, here zero control error in

trackingmeasurement is reached within a finite time, so the dynamics (2.91) is valid after some

finite time, as opposed to solution (2.56) which guarantees exponential stability.

2.3.5.2 Disturbance Observers

Assume a MIMO system with measurements ðx; uÞ and unknown vector valued function

fðx; aÞ, matched unknown and unmeasured disturbance Dhðx; tÞ ¼ Bl such that the system

dynamics is given by

_x ¼ fðx; aÞþBuþDhðx; tÞ

x 2 Rn�1; u 2 Rm�1; d 2 Rp; rankðBÞ ¼ m;m < nð2:92Þ

The nominal plant dynamics is described by

_x ¼ f0ðx; a0ÞþBuþBuo

x 2 Rn�1; u 2 Rm�; uo 2 Rm; rankðBÞ ¼ m ð2:93ÞThe observer control input uo is selected to enforce a sliding mode in a manifold

s ¼ Gðx� xÞ ¼ 0 ; s 2 Rm�1; rankðGÞ ¼ m. If detðGBÞ 6¼ 0 then from _sðG; x; xÞ ¼ 0 the

equivalent control can be evaluated as

uoeq ¼ ðGBÞ� 1GfDhðx; tÞþ ½fðx; aÞ� f0ðx; a0Þ�g

uoeq ¼ lþðGBÞ� 1GDfðx; aÞ

ð2:94Þ

The equivalent control consists of the system matched disturbance l and projection of the

difference between the actual plant and its nominal model GDfðx; aÞ. The control input in

Equation (2.92) is u ¼ uc � uoeq, where uc is to be determined later and uoeq is defined in

Equation (2.94). Then the dynamics (2.92) yields

_x ¼ ðI�BðGBÞ� 1GÞfðx; aÞ�BðGBÞ� 1

Gf0ðx; aÞþBuc ð2:95Þ


As result, the disturbance Dhðx; tÞ ¼ Bl is canceled. If fðx; aÞ is known and

f0ðx; a0Þ ¼ fðx; aÞ is selected, then GDfðx; aÞ ¼ 0 and (2.95) reduces to

_x ¼ fðx; aÞþBuc ð2:96Þ

If fðx; aÞ is unknown and f0ðx; a0Þ ¼ 0 is selected, then GDfðx; aÞ ¼ Gfðx; aÞ and Equa-

tion (2.95) becomes

_x ¼ ðI�BðGBÞ� 1GÞfðx; aÞþBuc ð2:97Þ

If fðx; aÞ ¼ Blþ jðx; aÞ, then Equation (2.97) yields

_x ¼ Buc þðI�BðGBÞ� 1GÞjðx; aÞ ð2:98Þ

The dynamics of plant (2.92) with equivalent control (2.94) fed to the system input

could be adjusted to different forms depending on the selection of the nominal mod-

el (2.93). The matched plant disturbance and matched component of the vector fðx; aÞ canbe compensated by the same mechanism. That is very important since it allows the

matched disturbance and the matched variation of the unknown system dynamics to be

handled in the same way.

Until now the system control distribution matrix B had been assumed known. Let us now

look at system (2.92) with the assumption that the pair ðx; uÞ is measured and the nominal plant

can be described by dynamics _x ¼ f0ðx; a0ÞþB0u. The real plant components can be

expressed as fðx; aÞ ¼ f0ðx; a0ÞþDfðx; aÞ, B ¼ B0 þDB and by assumption, the unmeasured

disturbance Dhðx; tÞ satisfies the matching conditions. Vector f0ðx; a0Þ and nominal control

distribution matrix B0 are assumed known and rank ðB0Þ ¼ m.

The control input uo in the nominal plant dynamics

_x ¼ f0ðx; a0ÞþB0uþB0uo ð2:99Þ

is selected to enforce tracking in the sliding mode of the output of the real plant by the output

of the plant model (2.99). The sliding mode manifold is s ¼ Gðx� xÞ ¼ 0, s 2 Rm�1 and

rank ðGÞ ¼ m. The equivalent control can be determined as

GB0uoeq ¼ G½fðx; aÞ� f0ðx; a0Þ� þGðB�B0ÞuþGDh

uoeq ¼ ðGB0Þ� 1GDfðx; aÞþ ðGB0Þ� 1

GDBuþðGB0Þ� 1GDh

ð2:100Þ

Feeding u ¼ uc � uoeq, with equivalent control as in Equation (2.100), into the dynamics of

the system (2.92) yields

_x ¼ fðx; aÞþ ½B0 þDB�ðuc � uoeqÞþDhðx; tÞ¼ B0uc þ f0ðx; a0Þþ ½I�BðGBÞ� 1

G�½Dfðx; aÞþDBuþDh�ð2:101Þ

If conditions ½Dfðx; aÞþDBuþDh� ¼ Blþ jðx; aÞ hold, the dynamics (2.101) reduce

to (2.98) with control input uc and control distribution matrix B0. This shows an effective way


of rejecting all components of the system [the internal dynamics fðx; aÞ, the plant parameters

DB and the exogenous inputs Dh] which satisfy the matching conditions.

References

1. Kuo, B.C. (1982) Control Systems, Prentice Hall, Englwood Cliffs, New Jersey.

2. Klailath, T. (1980) Linear Systems, Prentice Hall, Englwood Cliffs, New Jersey.

3. Utkin, V.I. (1992) Sliding Modes in Control and Optimization, Springer-Verlag, New York.

4. Kokotovic, P.V., O’Malley, R.B., and Sannuti, P. (1976) Singular perturbations and order reduction in control theory.

Automatica, 12, 123–126.

5. Drazenovic, B. (1969) The Invariance conditions for variable structure systems. Automatica, 5, 287–295.

6. Unel, M., Sabanovic, A., Yilmaz, B., and Dogan, E. (2008) Visual motion and structure estimation using sliding

mode observers. International Journal of Systems Science, 39(2), 149–161.

Further Reading

Filipov, A.F. (1988) Differential Equations with Discontinuous Right Hand Sides, Kluwer, Dordrecht.

Franklin, G.F., Powel, J.D., and Emami-Naeini, A. (2002) Feedback Control of Dynamics Systems, Prentice Hall,

Englwood Cliffs, New Jersey.

Krstic,M.,Kanellakopulos, I., andKokotovic, P.V. (1995)Nonlinear andAdaptiveControlDesign, JohnWiley&Sons,

Inc., New York.

Luenberger, D.G. (1964) Observing the state of a linear system. Transactions in Military Electronics, 8, 74–80.

Lukyanov, A. and Utkin, V. (1981) Method of reducing equations of dynamic systems to a regular form. Automation

and Remote Control, 42, 413–420.


Part Two

Issues in MotionControl

Motion control encompasses a wide range of issues related to the control of mechanical

systems. In free motion, trajectory or velocity tracking is the task required most often. In

contact with the environment, interaction forces act on all bodies in interaction and modify

their motion. Another situation may appear if the systems in interaction are constrained to

maintain a certain relationship. The functional relationship may be the result of mechanical

constraints (for example, parallel mechanisms or cooperative work) or they may be

specified by the operation requirements to be maintained by otherwise mechanically not

interconnected systems (for example, in bilateral control systems).

The presence of human interaction with system adds another dimension to motion

control. Humans may require the establishment of a certain functional relation with respect

to the system by having direct contact (human operator) or having no direct mechanical

contact (remote operation). In physical contact, the human operator has a direct sensation

of the interaction force – the sensation of touch. In some systems the operator may need a

functional relation to the remote system in which a sense of the interaction forces

generated on a remote site is transmitted to the human operator with high fidelity. That

requires the establishment of a functional relation between the operator’s side (master

side) and the remote side (slave side) in such a way that the operator has a sense that a real

interaction force like a direct connection exists – either rigid or modulated by a feeling of

some mechanical impedance. Such a functional relation requires coordinated control

action on both the master and slave sides of the interconnected systems. As opposed to

mechanically interconnected systems, in this case a functional relation enforced by control

will result in interaction forces which enable the behavior of the controlled system’s acts

because it has a prescribed interaction.

In this part of the book, our aim is to discuss selected concepts in motion control. In

order to highlight the design and structural issues in Chapter 3, we assume that all

necessary information is available. In subsequent chapters and in the examples imple-

mentation issues will be treated in order to make a flavor of the complexities arising in



motion control. Our goal is to clarify our approach to motion control by discussing

problems related to:

. Forcing a mechanical system to track predefined trajectory or velocity without contact or in

contact with the environment. The termenvironment is used for a bodywithwhich controlled

system is in interaction.. Making a mechanical system insert a specified force while in interaction with the

environment.. Making amechanical system react as a specified combination ofmass, spring and damper on

an external force acting on the system.. Enforcing a virtual interaction like themaster–slave functional relation requires that systems

at the master and slave sides track the trajectory and force (functionally scaled due to the

difference in the systems) from each other.

In Chapter 3 wewill present motion control design within the acceleration control framework.

The presentation concentrates on the realization of a cascade structure, with the inner loop

realizing acceleration controller and the outer loop realizing a task controller.

In Chapter 4 we will focus on the design of the disturbance observers and their application

in realization of the control structures discussed in Chapter 3. The emphasis is on salient

properties and application issues. The design and analysis are based on single degree of

freedom system.

Themain topic in Chapter 5 is the control ofmotion in the presence of an interactionwith the

environment and the associated constraints. Force control and the enforcement of functional

relations between systems is discussed in detail.

The subject in Chapter 6 is bilateral control with and without a delay in the control loop. In

all chapters, examples are given to illustrate the salient properties of the applied methods.


3

Acceleration Control

In general, motion is defined in terms of position and/or velocity or forces. Often, due to

interaction with environment, modification of the main task may be required, thus a transition

between different tasks may be needed. An unstructured environment and the complexity of

tasks require a control system to enable a simple transition from one task to another and

modification of the motion due to interaction with the environment.

In a motion control system the role of a controller is to impose the desired acceleration by

inserting additional forces onto the system input, thus the synthesis of the desired system

acceleration is oneof the central issues indesign. In order to avoid adiscussionofmanydifferent

tasks and their peculiarities, in this chapter, design will be shown for a tracking problem in

whichoutput definedas a functionof systemcoordinates is required to track its reference. Sucha

formulation allowswithin the same framework a design of control for problems inwhich output

is either the function of acceleration, position and velocity or the function of velocity or the

function of position only. The solution of such a problemwould open away of unified design of

motion control for numerous specific tasks–position and velocity tracking, interaction force

control, establishment of a functional relation between different systems, to name just some of

them.Control plant analysis and formulation of the control taskwithin a suitable framework are

the key issues in such an approach to motion control system design. In order to avoid

complexities, the design will be shown for a single degree of freedom mechanical system.

The control tasks will be defined to allow simple and direct usage of Lyapunov stability criteria

as a starting point in selecting the control system structure.

3.1 Plant

As shown inChapter 1, amathematicalmodel of a single degree of freedom (1-dof)mechanical

system (either translational or rotational) can be written in the following forms

a qð Þ€qþ b q; _qð Þþ g qð Þþ τext ¼ τ

or

_q ¼ v

a qð Þ _vþ b q; vð Þþ g qð Þþ τext ¼ τ

ð3:1Þ



Here q and _q stand for the state variables – position and velocity respectively; aðqÞ is acontinuous strictly positive bounded function amin � aðqÞ � amax, 8q representing inertia of

the system; bmin � bðq; _qÞ � bmax is a nonlinear bounded function representing Coriolis forces

and friction force; τe min � τext � τe max stands for bounded external forces acting on system;

gmin � gðqÞ � gmax is a bounded continuous function representing gravitational forces;

τmin � τ � τmax stands for the force applied to the system input – which can be changed by

somemeans according to demand – thus it can be interpreted as the control input to the system.

Themin andmax values for all parameters and variables are assumed known. For a given range

of control input, other acting forces and change of system parameter, change in system

acceleration and velocity are also bounded. The change of the system position may be

unbounded (for example angular position in the case of a rotational actuator). If otherwise

not stated,wewill assume that the operation of the system (3.1) is restricted to a bounded region

consistent with limits on the system variables, parameters and inputs. The structure of the

system (3.1) is depicted in Figure 3.1.

Two forms shown in (3.1) will be used in different parts of the text and both will be assigned

as system (3.1). The usage of either particular form will be clear from the context. Since

model (3.1) is valid for both translational and rotational motion the term force will be used

within the text. Through the text the terms system and plant will be used interchangeably

when referring to (3.1). The consistent set of units for parameters and variables will be used in

all examples.

Forces are the measure of interaction between mechanical systems and they exist if bodies

are in contact. The interaction forces depend on the relative positions of the interacting bodies

and on the deformation properties of the bodies in the contact point. The peculiarities of the

interaction force modeling are many due to the complexity of motion and body deformation.

Herewewill use simplifiedmodels able to reflect the nature of interaction in terms of the system

coordinates and the position of the bodywithwhich the system is interacting.Ageneral second-

order mass–spring–damper model is the most complete description of the dynamics of the

interaction force. In many applications, contact is assumed with a stationary environment, thus

interaction forces reflect only the body deformation and can be modeled as a spring–damper

system, thus assuming no acceleration induced force involved. In some cases, interaction may

be modeled as ideal, lossless spring – thus depending only on the relative positions and

properties of the bodies in the interaction point.

For the needs of control system analysis and design, we will model interaction force in its

most general form as

τint ¼wðq; _q; €q; qe; _qe; €qeÞ if there is interaction

0 if there is no interaction

�ð3:2Þ

Figure 3.1 Structure of the 1-dof system


Here wðq; _q; €q; qe; _qe; €qeÞ is a linear or nonlinear function of the system coordinates and

the position qe, the velocity _qe and the acceleration q€e of the environment. A model of

interaction force in a particular case may not include all variables. The external force τextin (3.1) for the general case includes, but may not be equal to, the interaction force.

The system structure illustrates the features of a single degree of freedom mechanical

system – motion is the result of the algebraic sum of all forces acting on the system and

influence of a particular force cannot be determined by analyzing only the systemmotion. This

feature also plays a role in control – the influence of any of the forces or a combination of some

of the them can be compensated by other forces acting on the system. That allows aggregate

forces in two groups:

. the control forces – may be changed according to requirements,

. the disturbances – all other known and unknown forces except acceleration induced force.

With such a partition of forces, the dynamics of system (3.1) can be represented in the

following, more compact way

aðqÞ€q ¼ τ� τdτd ¼ bðq; _qÞþ gðqÞþ τext

ð3:3Þ

Here acceleration induced forces aðqÞ€q are analyzed with respect to the control input τand algebraic sumof all other known and unknown dynamics bðq; _qÞþ gðqÞ and external forcesτext – including interaction forces if there is contact with the environment. In further analysis

τd will be referred as the system input disturbance. It is obvious that such a division of

forces is arbitrary. The acceleration induced force is a result of the algebraic summation of all

forces acting on the system – thus acceleration is linear in any of them.

Assume that varying inertia coefficient can be represented as aðqÞ ¼ an þDaðqÞ, where anstands for known nominal value and DaðqÞ is a smooth bounded function with known upper

and lower bounds. In accordance with equation of motion (3.1) the force associated with a

variation of inertia DaðqÞ can be treated as a part of the overall disturbance. In this case

disturbance τd will be augmented by the inertia variation induced force DaðqÞ€q and will be

referred to as the generalized input disturbance τdis. In the further text when the context is clear,both the disturbance τd and the generalized input disturbance τdis will be referred to just as

disturbance. By introducing a generalized input disturbance the dynamics for system (3.1) can

be written as

an€q ¼ τ� τdisτdis ¼ DaðqÞ€qþ bðq; _qÞþ gðqÞþ τext

¼ DaðqÞ€qþ τd

ð3:4Þ

Possibility to attribute forces induced by variable part of inertia to disturbance is a clear

consequence of system property – motion is determined by the algebraic sum of all forces

and their internal distribution is not reflected in the overall system motion. The structure of

system (3.4) is shown in Figure 3.2.

Models (3.3) and (3.4) describe the same system and one should note the difference. Inertia

in (3.3) may be variablewhile in (3.4) force DaðqÞ€q is included in the generalized system input

Acceleration Control 65

disturbance – thus the inertia of the system appears to be constant. This may be achieved only if

one can implement control input such that the influence of force due to inertia variation DaðqÞ€qon system dynamics is compensated. Then, description (3.4)may acquire the physicalmeaning

of a system with constant inertia. This opens a possibility of manipulating at least some of the

parameters of the system by adding appropriate forces to the system input.

In models (3.3) and (3.4) input force τ and disturbances are acting the same way on the

system acceleration. Both τd and τdis are so-called input ormatched disturbances and, as shown

inChapter 2, they can be compensated by appropriate selection of the control input. At the same

time onemay look at external force as an additional input whichmay change the systemmotion

in the sameway as control input does. This property will allow treatment of interaction force as

an input, which may modify the system motion in a desired way. This illustrates the arbitrary

nature of the attribution of forces in (3.3) and (3.4).

Formally the right hand side in bothmodels (3.3) and (3.4) can be interpreted as a product of

the system inertia and acceleration. By expressing the right hand side in (3.3) as

aðqÞ€qmot1 ¼ ðτ� τdÞ and in (3.4) as an€q

mot2 ¼ ðτ� τdisÞ, then models (3.3) and (3.4) can be

written in the following form

€q ¼ €qmoti ; i ¼ 1; 2 ð3:5Þ

System dynamics are expressed as a double integrator with the acceleration €qmoti ; i ¼ 1; 2

as a system input.

Structures corresponding to the system description in (3.3)–(3.5) – as their common

representation – are depicted in Figure 3.3. The difference between the two systems is the

way the force induced by the inertia variations is treated. In Figure 3.2 the inertia variation

Figure 3.2 Representation of system (3.4) with constant system inertia and generalized input

disturbance

Figure 3.3 Representation of system (3.3) in (a) and system (3.4) in (b) and their common represen-

tation as a double integrator with acceleration as the input


induced force is treated as a part of the generalized disturbance, thus leading to a representation

of the system as a plant with constant parameters in which parameter variation is included in

the generalized system disturbance.

Description of the system dynamics as in (3.5) is taken as a starting point in the so-called

acceleration control framework [1,2]. In this framework, synthesis of control of mechanical

system is a two-step procedure. In the first step, acceleration is determined as a virtual control

input, and in the second step, the force necessary to enforce the desired acceleration is selected.

From (3.5) motion control tasks can be interpreted as imposing the desired acceleration

€qmoti ¼ €qdes; i ¼ 1; 2 by applying input forces. The linearity of the control plant in control input

and in disturbance allows application of the superposition principle in selecting the structure of

additional forces inserted into the system by the controller. Inmechanical systems, these forces

may be introduced by: (i) making the control input a function of the difference between the

desired and actual output or the state of the system, or (ii) by establishing a system interaction

with the environment in such away that interaction force τint (as a part of the external force τext)makes the desired influence on system dynamics. The difference between these two cases lies

in the source of the force relative to the system. If τ and τext are treated as external inputs to thesystem and both can be changed by somemeans then either may change the state of the system

in the sameway. Thismeans that partition, as already stated, of components in (3.3) and (3.4) on

disturbance and control is arbitrary. For a long time trajectory tracking and velocity control of

actuators have been basic issues in motion control and, in both of these tasks, interaction with

the environment is a disturbance that must be rejected.

3.2 Acceleration Control

A general requirement for a control system is to have a robust behavior – thus keeping control

error as close as possible to zero in the presence of changes in the plant and/or interaction with

other systems. Control problems (alternatively we will use the term control tasks) in

mechanical systems are numerous and diverse. As examples, high performance trajectory

tracking or control of the interaction force in the presence of a change in the environment can be

considered. In the high precision machining of parts, the position of cutting tool should be

maintained despite changes in material properties and variation in the cutting forces.

Contrarily, while handling fragile objects, contact force should be maintained carefully to

avoid damage. The opposing requirements in these two casesmay be best understood ifwe look

at compliance (defined as a relative change in position due to a change in interaction force

j ¼ qq=qτint) [3]. In position control compliance should be as small as possible – thus rejecting

the influence of all disturbances. But in force control, the positionmay change in order to satisfy

the desired force and compliance should be as large as possible. Ideal position control and force

control are equivalent to realizing zero and infinite compliance, respectively.

The same can be concluded if stiffness (inverse of compliance – defined as the relative

change in a force generated to resist a change in position) is used to describe the reaction of a

motion control system to a change in the external force acting on the system. In these terms, the

ideal position tracking requires infinite stiffness. Contrarily – force control requires zero

stiffness. Systems may be required to react in a certain way to external forces by modifying

their motion. In this case the dynamical reaction of the system on the external force needs to be

controlled.


This indicates the need to control compliance (or stiffness) in motion control systems over

the full range between zero and infinity – or more realistically from very small to very large

values. In addition, the transition between high compliance to low compliance tasks and vice

versa needs to be resolved in order to achieve natural behavior in a controlled system.

The characterization of amotion control system in terms of compliance or stiffness (despite a

clear physical meaning) has a drawback as a control task specification. That drawback is the

result of a need tomaintain different compliances for different control tasks. Fromacontrol point

of view it is more natural to define a common goal for all control tasks. Such a goal is in the core

of control systems design –maintain control error zero despite changes in system dynamics and

interactions with other systems. This requires, as discussed in Section 3.1, selection of the

desired system acceleration (and consequently input force) such that zero control error is

maintained after being reached from some initial state. In order to do this we need to relate

the dynamics of control task and the desired acceleration. It would be of advantage if that

relationship can be specified in a unified way for at least common motion control tasks. That

would allow a consistent control system design procedure. This requires solutions for

. consistent specification of diverse control tasks in terms of the system coordinates,

. selection of the control inputs to enforce stability of the equilibrium solution and ensure

closed loop dynamics robust with respect to change in system parameters and disturbances.

3.2.1 Formulation of Control Tasks

For mechanical systems the control tasks include free motion (without environment interac-

tion) and motion in interaction with other systems. Thus, in motion control systems position or

velocity tracking, force control and enforcing the desired dynamic reaction of the system in

interaction are basic tasks. In some cases, position or velocity are not directly controlled.

Instead someoutputs, represented by a function of position and/or velocity, are required to track

their references. As an example, interaction force control, if contact is modeled by a linear or

nonlinear spring, may be considered. In the most general case, the reaction of a system to

external force can be described as a function of position, velocity and acceleration. Thatmeans,

controlling a system to have the desired reaction on an external forcewhile interacting with the

environment involves tracking an output that depends on acceleration, velocity and position.

Taking into account all these considerations, in a most general way, control output may be

expressed as one of the following:

(i) Linear or nonlinear continuous function of position yðqÞ,(ii) Linear or nonlinear continuous function of velocity yð _qÞ,(iii) Linear or nonlinear continuous function of position and velocity yðq; _qÞ,(iv) Linear or nonlinear continuous function of position, velocity and acceleration yðq; _q; €qÞ.

In real applications, these outputs can be measured by transducers, or synthesized using

information from available transducers (estimated by using observers, for example). Essential

in our formulation is the assumption that the form of the functional dependence of these outputs

on the system state is known. In other words, models which generate these outputs, but not

necessarily all parameters, are assumed to be known. These models may be a mathematical

representation of the real processes (like the modeling of interaction forces) or they may


represent a virtual relationship between coordinates needed to be maintained by the control

(for example, the virtual forces assumed in the mobile robot obstacle avoidance tasks). The

properties of these models [or output functions yð � Þ] are important for the proper formulation

of the motion control design.

Let reference yref be a linear or nonlinear function of time or some other variables (for

example, if theoutput of one systemshould track the output of another system, the referencewill

depend on the coordinate of the leading system). Functions y and yref are assumed differentiable

for the appropriate number of times. In order to have a meaningful task specification, both the

function describing task y and its reference yref should guarantee a unique solution for system

coordinate(s) in transients and in theequilibriumstate.Detailed requirementson these functions

will be presented later in conjunction with particular control problems.

In themotion control system design, wewill consider necessary information [disturbance as

expressed in (3.3) or (3.4) and the system coordinates (position and velocity)] available by

measurement or estimation. This would allow analysis of the salient properties of algorithms

and control system structures. Errors due to imperfections in obtaining necessary information

encountered in practical implementation, unmodelled dynamics and other constraints will be

taken into account later in the text.

Tracking error e ¼ y� yref is a measure of the distance from desired system behavior

described by equilibrium solution y¼ yref. In equilibrium, the system coordinates are forced to

satisfy relations eðy; yref Þ ¼ 0 – or in other words, the system state is constrained to the domain,

ormanifoldSyðy; yref Þ in systemstatespace.For tasks (i)–(iv), thedomainSyðy; yref Þ isdefinedas:

. if output is defined as function of position only

S1ðq; _qÞ ¼ q : eðqÞ ¼ yðqÞ� yref ¼ 0� � ð3:6Þ

. if output is defined as function of velocity only

S2ðq; _qÞ ¼ _q : eð _qÞ ¼ yð _qÞ� yref ¼ 0� � ð3:7Þ

. if output is defined as function of position and velocity

S3ðq; _qÞ ¼ q; _q : eðq; _qÞ ¼ yðq; _qÞ� yref ¼ 0� � ð3:8Þ

. for output defined as function of position, velocity and acceleration

S4ðq; _qÞ ¼ q; _q; €q : eðq; _q; €qÞ ¼ yðq; _q; €qÞ� yref ¼ 0� � ð3:9Þ

Now the control task can be formulated as a requirement to enforce equilibrium eðy; yref Þ ¼ 0.

Stated differently, control should be selected to enforce convergence to the domain Syðy; yref Þand the stability of the equilibrium. The state will be then forced to reach domain Syðy; yref Þ,from initial conditions consistent with limits on plant parameters and variables, and to remain

within domain Syðy; yref Þ after reaching it.

Depending on the nature of the output function y; these requirements may be interpreted

in different ways. For example, enforcement of (3.6) and (3.7) can mean tracking in position

and velocity, respectively. Enforcement of (3.6) can also be interpreted as force control, if


interaction is modeled as an ideal spring (linear or nonlinear). Enforcement of (3.8) can be

interpreted as enforcing the interaction force with the environment, if interaction is modeled

as a spring–damper system, or just as a requirement that position and velocity satisfy

operational constraints yðq; _qÞ ¼ yref . Enforcement of (3.9) can be interpreted as control of

the system reaction as a specific mass–spring–damper system to the external force, or just as a

requirement that position and velocity acceleration satisfy operational constraints. This variety

of interpretations shows a possibility to include very diverse problems into formulations

(3.6)–(3.9).

When conditions (3.6)–(3.9) are met, the system coordinates satisfy equilibrium solution

eðy; yref Þ ¼ 0, thus they are constrained in specificmanifold in system state space. For selected

outputs functions yð � Þ requirements (3.6)–(3.9) can be interpreted as:

. If the output is selected as in (3.6) then in equilibrium yðqÞ tracks its reference. The outputfunction needs to be selected such that yðqÞ ¼ yref guarantees the unique solution

q ¼ qðy; yref Þ. If for simplicity yðqÞ ¼ q and, for example, the reference is sinusoidal then,

in system state space ðq; _qÞ, motion will be constrained to an elliptic trajectory

q ¼ q0 sinðvtÞ; _q ¼ vq0 cos ðvtÞ. More complex trajectories may be obtained if yðqÞ is

nonlinear.. For output yð _qÞ equilibrium solution eð _q; yref Þ ¼ 0 means that yð _qÞ tracks its reference.

Equilibrium solution yð _qÞ ¼ yref should yield a unique solution for _q ¼ _qðy; yref Þ. Thevelocity will track its reference and the position will be just the result of velocity

integration.. If the task is a function of position and the velocity as in (3.8), then in equilibrium system,

coordinates ðq; _qÞ are constrained to satisfy equation yðq; _qÞ ¼ yref . The position is deter-

mined as a solution of the differential equation yðq; _qÞ ¼ yref . In this case, actual dynamics

of the state coordinates must be determined taking into account the equilibrium solution

and full dynamics of the system.. In the case of constraint (3.9), the behavior of the system in steady state is described by

eðq; _q; €qÞ ¼ 0. Function yðq; _q; €qÞ should guarantee a unique solution for the acceleration

from yðq; _q; €qÞ ¼ yref . The position is then determined from the solution of the second order

differential equation yðq; _q; €qÞ ¼ yref . The simplest case is if yðq; _q; €qÞ is linear then

satisfying requirements for the existence and the uniqueness of the solution can be verified

easily.

Having control task formulated as in (3.6)–(3.9), systemmotion is constrained to the manifold

in state space. In order to fully describe the behavior of the system the dynamics of the

constraint equationsmust be taken into account alongwith dynamics of the system. In addition,

closed loop system dynamics will depend on convergence to the equilibrium solution.

It follows from Equation (3.5) that control system design consists of selection of accel-

eration input such that:

. Equilibrium solutions (3.6)–(3.9) are reached from initial states, consistent with bounded

control and changes in plant parameters. In general, one may require specific way of

convergence (asymptotic, exponential or finite-time, for example) from initial state to

equilibrium solution.. Equilibrium solution is stable on the system trajectories.


This suggests a two-step design. In the first step, the desired acceleration €qdes needs to be

selected to enforce the output convergence and stability of equilibrium. This first step is related

to the output – task – control and it requires selection of the acceleration consistent with task

specification. As shown in Equations (3.6)–(3.9) tasks can be specified in many different ways.

This step involves finding relationship between acceleration as the control input and the

enforcement of the stability of the equilibrium solution.

In the second step, the input force that enforces desired acceleration €qdes in the closed loopsystem needs to be derived. This step is related to the structure of the plant and is not related

to the control task. Synthesis of the force input that effectively will establish a closed loop

acceleration controller is performed in this step.

This leads to the cascade structure of the output control. The outer loop is related to the task

control and the inner loop realizes the acceleration controller.

Since the second task is only related to the plant structure let us address it first. Assume the

desired acceleration €qdes is known. Inserting the known desired acceleration €qdes into (3.3)

and (3.4) yields the control force in the following forms

τ ¼ τd þ aðqÞ€qdesτ ¼ τdis þ an€q

desð3:10Þ

The applied force has two components – the disturbance τd or τdis and the force induced bythe desired acceleration aðqÞ€qdes or an€qdes. The first expression is used in plants with a knowninertia aðqÞ and the second is applicable in plants where only the nominal value an of inertia is

known. Force (3.10) cancels plant input disturbance and makes the plant a simple double

integrator €q ¼ €qdes, thus robust against changes of system parameters and external forces.

Realization of the acceleration controller (3.10) requires information on disturbance and plant

inertia. In Chapter 4, we will discuss estimation of the plant disturbance or generalized

disturbance in detail. Here we will assume that disturbance is known.

The structure of themotion controller with a desired acceleration generator – the output task

controller – in the outer loop and the acceleration controller in inner loop are shown in

Figure 3.4(a) for realization of the acceleration controller as in the first row of (3.10) and in

Figure 3.4(b) for realization of the acceleration controller as in the second row of (3.10).

In order to derive the desired acceleration, the dynamics of control error on the trajectories of

the system should be evaluated. Functions y and yref are assumed differentiable for the

appropriate number of times and consecutive differentiation of control error in (3.6)–(3.9) until

the coefficient multiplying acceleration is different from zero yields:

. for output defined as function of position only

S1ðq; _qÞ ) €eðqÞ ¼ _c _qþ c€q� €yref ; c ¼ qyðqÞqq

6¼ 0 ð3:11Þ

. for output defined as function of velocity only

S2ðq; _qÞ ) _eð _qÞ ¼ c1€q� _yref ; c1 ¼ qyð _qÞq _q

6¼ 0 ð3:12Þ


. for output defined as function of position and velocity

S3ðq; _qÞ ) _eðq; _qÞ ¼ c2 _qþ c3€q� _yref ; c2 ¼ qyðq; _qÞqq

6¼ 0; c3 ¼ qyðq; _qÞq _q

6¼ 0 ð3:13Þ

. for output defined as function of position, velocity and acceleration

S4ðq; _qÞ ) eðq; _q; €qÞ ¼ yðq; _q; €qÞ� yref ð3:14Þ

Here ci 6¼ 0; i ¼ 1; 2; 3 are assumed constants or known continuous functions of time or plant

coordinates. Equations (3.11)–(3.14) are linear in acceleration. If the function yðq; _q; €qÞ islinear in acceleration, then it can be expressed as c4€qþ y1ðq; _qÞ ¼ yðq; _q; €qÞ; c4 6¼ 0 where

c4 6¼ 0 is assumed known continuous function or constant.

Equations (3.11)–(3.14) show the system relative degree with acceleration as the control

input. (The relative degree is taken as the order of the lowest derivative of control error inwhich

a nonzero coefficient appears in front of the acceleration term). The relative degree of the

control error allows the establishment of additional requirements on functions specifying

control output in (3.6)–(3.9) by the following form:

. if relative degree is zero,we assume function yðq; _q; €qÞ and its reference yref to be continuous,

. if relative degree is one, function yðq; _qÞ or yð _qÞ and its reference yref must be continuous

along with their first-order time derivatives,

Figure 3.4 Output control realized as a cascade structure of the task controller and the acceleration

controller. (a) Realization of the acceleration controller as τ ¼ τd þ aðqÞq€des. (b) Realization of the

acceleration controller as τ ¼ τdis þ anq€des


. if relative degree is two, function yðqÞ and its reference yref must have continuous first- and

second-order time derivatives.

If these requirements are satisfied the dynamics described by (3.11)–(3.14) are continuous and

linear in control (linear or nonlinear depending on the specification of the control output).

Depending on the structure of the output, appropriate continuous control system design

methods can be applied in selecting the desired acceleration. Many solutions have been

presented in the vast literature on motion control and mechatronics. A cascade structure with

nested loops for force and velocity is a commonly accepted solution [5].

The dynamic structures described by Equations (3.11)–(3.14) are respectively shown in

Figure 3.5(a)–(d). Here we assume that the acceleration control loop is realized, thus the

desired acceleration is the control input. Similarities between them are apparent. With

acceleration as the control input, other elements, similar to the description of the control

plant, may be formally treated as input disturbance in the error dynamics. Thus all systems

appear as continuous SISO systems linear in control input. All disturbances are input

disturbances – thus they can be compensated by appropriate selection of the desired

acceleration.

The dynamics of output control error (3.11)–(3.14) is a starting point in selection of desired

acceleration enforcing convergence to and stability of the equilibrium. In the selection of

desired acceleration, we can apply superposition method and design it to be composed of two

components. The first component is selected to enforce equilibrium solution for all initial

conditions consistent with equilibrium ejt¼0 ¼ 0. The second is selected to guarantee con-

vergence to equilibrium solution if initial conditions are not consistent with equilibrium

ejt¼0 6¼ 0. The first term is the so-called equivalent acceleration €qeq. The force [determined

from Equation (3.10) with €qdes ¼ €qeq] enforcing equivalent acceleration will be called

equivalent force τeq.

Figure 3.5 Output error dynamics (3.11)–(3.14) for a systemwith the desired acceleration as the control

input. (a) Error dynamics as in (3.11); (b) Error dynamics as in (3.12); (c) Error dynamics as in (3.13) and

(d) Error dynamics as in (3.14)


3.2.2 Equivalent Acceleration and Equivalent Force

Acceleration which enforces zero of the right hand side in differential Equations (3.11)–(3.14)

can be easily derived. From the dynamics point of view, acceleration determined by equating

the right hand side in (3.11)–(3.14) to zero will not have the same meaning for all cases, due to

differences in relative degree:

. Acceleration determined from €eðqÞ ¼ 0 will enforce a constant rate of change of

error d _eðqÞ=dt ¼ 0 ) _eðqÞ ¼ const, thus the error can diverge from the equilibrium

solution.. Acceleration derived from Equations (3.12) and (3.13) will enforce a zero rate of change of

error deð _qÞ=dt ¼ 0 ) eð _qÞ ¼ const and deðq; _qÞ=dt ¼ 0 ) eðq; _qÞ ¼ const, respectively –

thus the motion will remain equidistant from the equilibrium solution.. From Equation (3.14) acceleration can be derived directly from eðq; _q; €qÞ ¼ 0. It will set the

control error to zero – thus it will enforce an equilibrium solution.

If acceleration derived from Equations (3.11)–(3.14) is applied to the system, the input control

errorwill exhibit a different behavior – frombeing set to zero [for (3.14)], having some constant

value [for (3.12) and (3.13)] or having a constant rate of change [for (3.11)]. Different

additional terms should be added to system input in order to guarantee the convergence and

stability of the equilibrium solution. Finding a way of making the same relative degree in

control tasks specified by (3.6)–(3.9) while enforcing desired equilibrium solutionwould allow

all these problems to be treated in the sameway. In addition, it may allow the development of a

unified control system design procedure valid for all of these control problems.

What wewould like to do is to find away to express the control system requirements in such

a form that the relative degree in new coordinates is one, while ensuring the same equilibrium

solution as in (3.6)–(3.9). This would open a way of unified treatment of the tasks (3.6)–(3.9).

In such case, the acceleration enforcing a zero rate of change of the control error, and

consequently, the force enforcing such acceleration will have the same meaning in all of the

problems. Equations (3.12) and (3.13) have relative degree one so there is no need to change

them. That leaves requirements (3.6) and (3.9) for the analysis.

For a control task defined as in (3.6) the error dynamics (3.11) can be rearranged as

_eðqÞ ¼ e1ðqÞ_e1ðqÞ ¼ _c _qþ c€q� €yref

ð3:15Þ

As discussed in Chapter 2, in such systems one can treat e1ðqÞ as a virtual control input in thefirst equation of (3.15) and select it to ensure the desired behavior of the control error e. Let

eref1 ¼ zðeÞ enforces the desired dynamics in the output control error e (for example, selecting

z ¼ � ke gives _eþ ke ¼ 0). In the next step, the acceleration should be selected to enforce

tracking e1 ¼ z. The dynamics of the tracking error sz ¼ e1 � z on the trajectories of

system (3.15) is

_sz ¼ _e1 � _z

¼ _c€qþ c _q� €yref � _zðqÞ ð3:16Þ


For initial conditions sz½qð0Þ; _qð0Þ� ¼ 0, acceleration €qeq ¼ c� 1½€yref � _c _q� _zðqÞ� will

enforce motion szðtÞ ¼ 0; 8t � 0. Tracking e1 ¼ z is enforced 8t � 0. As a result, tracking

_eðqÞ ¼ zðeðqÞÞ is enforced and the desired dynamics of the control error are reached. Thus

problem (3.6) can be formulated as enforcing stability of sz(q,z) ¼ 0.

For the control problemswith a zero relative degree, c4€qþ y1ðq; _qÞ ¼ yðq; _q; €qÞ; c4 6¼ 0, we

can define a new variable z such that _z1 ¼ � y1ðq; _qÞþ yref holds. Then for constant c4 6¼ 0

the dynamics of the tracking error ez ¼ c4 _q� z1 can be expressed in the following form

_ez ¼ c4€q� _z1 ¼ c4€qþ y1ðq; _qÞ� yref ð3:17ÞThe relative degree in (3.17) is one and the acceleration derived from _ez ¼ 0 is equal to the

acceleration derived from eðq; _q; €qÞ ¼ 0. This can be confirmed by inspection of (3.17)

and (3.13).

Now, the error dynamics in (3.12), (3.13), (3.16) and (3.17) are described by first-order

differential equations. This allows us to treat modified problems (3.6)–(3.9) in the same way.

In the first step acceleration is selected by setting the right hand side in (3.12), (3.13), (3.16)

and (3.17) to zero. The acceleration determined in such a way is the so-called equivalent

acceleration €qeq. When applied to the system input the equivalent acceleration will ensure

a zero rate of change of the corresponding error.

3.2.2.1 Equivalent Acceleration

From Equations (3.12), (3.13), (3.16) and (3.17) the equivalent acceleration can be

expressed as:

. for control output with dynamics of augmented error as in (3.16)

_szðtÞ ¼ 0 ) €qeq ¼ _c _q� _z� €yref

cð3:18Þ

. for control output defined as function of velocity only with error dynamics as in (3.12)

_eð _qÞ ¼ 0 ) €qeq ¼ _yref

c1ð3:19Þ

. for control output defined as function of velocity and position with error dynamics as

in (3.13)

_eðq; _qÞ ¼ 0 ) €qeq ¼ � c2 _q� _yref

c3ð3:20Þ

. for control output defined as function of position, velocity and acceleration with augmented

error dynamics as in (3.17)

_ez ¼ 0 ) €qeq ¼ yref � y1ðq; _qÞc4

ð3:21Þ


The uniqueness of each of these solutions is guaranteed by the properties of output functions

and their references. It is interesting to note that, except in (3.21), the equivalent acceleration

depends on the derivative of the reference and the velocities and does not depend on position

and actual reference.

The dynamics defined by Equations (3.16), (3.12), (3.13) and (3.17) can be written in the

following form

_s ¼ gð€q� €qeqÞ; g 6¼ 0 ð3:22Þ

Here s stands for generalized error for problems (3.6)–(3.9) selected to have relative degree

one with respect to control input (either acceleration of the force) and at the same time

guaranteeing the desired output tracking if the equilibrium s ¼ 0 is enforced. The generalized

error and gain g stand for:

. in (3.16) s ¼ sz ¼ _e� zðeÞ and g ¼ c

. in (3.12) s ¼ eð _qÞ and g ¼ c1

. in (3.13) s ¼ eðq; _qÞ and g ¼ c3

. in (3.17) s ¼ ez ¼ c4 _q� z1 and g ¼ c4

The dynamics (3.22) describe a virtual plant which unifies all of the control tasks specified in

Equations (3.6)–(3.9). The control goals for these tasks will be achieved if control can be

selected such that equilibrium solutions ¼ 0 is stable and is reached from the initial conditions

consistent with plant parameters and the bounded control input. This condition is equivalent

to stability of the projection of the system motion in manifold Ss ¼ fq; _q : s ¼ 0g. Such a

definition includes all the tasks as specified in Equations (3.6)–(3.9) thus, it may serve as a

starting point for designing the control.

The dynamics (3.22) is shown in Figure 3.6. It is a simple integrator with acceleration and

the equivalent acceleration as inputs and generalized error sðtÞ as the output. In the

acceleration control the control input is desired acceleration and, similarly as in the analysis

of the control plant, the equivalent acceleration can be treated as input disturbance.

The equivalent acceleration has a very specific meaning. If the acceleration is equal to the

€qeq then the rate of change of the distance from the equilibrium solution s ¼ 0 is zero. That

means if sðt0Þ 6¼ 0 at t ¼ t0 and acceleration €qðtÞjt�t0¼ €qeqðtÞ is applied then the error will

remain equal to sðt0Þ for t � t0. Equivalent acceleration (3.18)–(3.21) is a function of

reference and control output, thus if these are known it can be calculated and can feed forward

to the system input. Due to assumptions of the continuity of system states, reference and their

derivatives the equivalent acceleration is a continuous function.

Figure 3.6 Dynamics of the generalized control error (3.22)


Assume g and €qeq are known. Let the desired rate of change of the distance from manifold

s ¼ 0 is specified as

_s ¼ _sdes ð3:23Þ

From (3.22) the acceleration input which enforces the dynamics of the distance from the

manifold Ss ¼ fq; _q : s ¼ 0g as in (3.23) is

€qdes ¼ €qeq þ g � 1 _sdes ð3:24Þ

The desired acceleration (3.24) enforces the dynamics of the distance from the equilibrium

solution is reduced to a single integrator with _sdes as the control input. The structure may be

presented as in Figure 3.7. The signals represented by the doted lines cancel each other, and the

resulting system is a single integrator.

Selection of _sdes is clearly related to the convergence and the stability of the closed loop

and will be discussed in detail later in the text.

Example 3.1 System Motion with Equivalent Acceleration as Input Examples are

provided to illustrate application of the methods and algorithms discussed in the text. The

control plant used in all examples in Part Two of this book is a single degree of freedom

mechanical system (3.25) with variable parameters and varying external load. It can represent

either translational or rotational motion.

aðqÞ€qþðbðq; _qÞþ gðqÞþ τextÞ|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}system disturbance¼τd

¼ τ ð3:25Þ

Here variables have the following meaning and, depending on the motion (translational or

rotational), are expressed in appropriate units:

. position q (inm or rad),

. velocity _q (inm/s or rad/s),

. acceleration €q (inm/s2 or rad/s2).

System parameters are selected as follows and, depending on the translational or rotational

motion, are expressed in the following units:

Figure 3.7 Structure of the output control system with acceleration as in (3.24) and its equivalent

reduction to the structure described in (3.23)


. inertia coefficient aðqÞ ¼ an½1þ a1sin ðqÞ�, in kg or kg �m2,

. nonlinear forces bðq; _qÞ ¼ b0qþ b1 _qþ ana1 _q cos ðqÞ, in N or N�m,

. nonlinear force gðqÞ ¼ g0q2, in N or N�m,

. external force, in N or N�m,

τext ¼τ0½1þ cos ðvτtÞþ sin ð3vτtÞ�; 0:12 < t < 0:8

0 elsewhere:

�

This external force does not include the force due to interaction with environment. That force

will be added to the model whenever appropriate.

These units will be consistently applied and will not be explicitly written in each of the

examples. In the drawings units will be given depending on the selected type of motion

(translational or rotational).

Coefficients an; b0; b1; g0; τ0 and vτ are assumed constant in each of the examples. All

parameters and components of the disturbance are bounded:

. inertia coefficient an min � an � an max (in kg or kg �m2),

. spring constant b0 min � b0 � b0 max (in kg/s2 or kg �m2/s2),

. damping coefficient b1 min � b1 � b1 max (in kg/s or kg �m2/s),

. nonlinear gain gmin � g0 � gmax [in kg/(m � s2) or kg �m2/s2],

. force amplitude τ0 min � τ0 � τ0 max (in N or N�m),

. angular frequency vr min � vr � vr max (in rad/s).

The system parameters (in appropriate units) are: an ¼ 0:1, a1 ¼ 0:5, b0 ¼ 15, b1 ¼ 0:02,g0 ¼ 9:81, τ0 ¼ 0:5 and vτ ¼ 12:56.

The dynamics of plant (3.25) with nominal inertia can be rewritten as

an€qþDa€qþ bðq; _qÞþ gðqÞþ τext ¼ ττdis ¼ Da€qþ bðq; _qÞþ gðqÞþ τext

ð3:26Þ

Inmodeling this system, Equation (3.25)will be used. Equation (3.26) shows partition of the

terms and will be used to show the structure and calculation of the generalized disturbance τdis.As the modeling environment for all examples, both Simnon� (version 3.0) and MATLAB�

are used. Simnon is a trademark of Department of Automatic Control (Lund, Sweden) and is

a product of SSPA Maritime Consulting AB. MATLAB� is a registered trademark of The

MathWorks, Inc.

In this example system motion with equivalent acceleration as input is illustrated. The

general structure of the system output is

yðq; _qÞ ¼ cðqÞqþ c1 _q

cðqÞ; c1 > 0ð3:27Þ

In order to avoid unnecessary complexities, the output is linear in velocity. Coefficients cðqÞand c1 have the same sign.

In all examples the output yðq; _qÞ is considered just as a variable representing a given

functional relationship without any physical meaning, thus having no unit associated with it.


The reference input yref ðtÞ is assumed a bounded continuous function of time

yref ðtÞ ¼ yref0 ½1þ y

ref1 sin ðvrtÞ� ð3:28Þ

Coefficients yref0 ; yref1 ;vr are assumed constant. The unit for reference and its parameters

depend on the unit of output. In most of the examples in this chapter, the reference is selected

as yref ðtÞ ¼ 1þ 35 sin ð12:56tÞ.The dynamics of the output error ey ¼ yðq; _qÞ� yref ðtÞ are

_ey ¼ cðqÞ _qþ _cðqÞqþ c1€q� yref0 y

ref1 vr cos ðvrtÞ ð3:29Þ

Solving _e ¼ 0 for €q ¼ €qeq yields equivalent acceleration

_eyð€q ¼ €qeqÞ ¼ 0 ) €qeq ¼ c� 11 ½yref0 y

ref1 vr cos ðvrtÞ� cðqÞ _q� _cðqÞq� ð3:30Þ

In this chapter, as the plant outputs we will consider the following functions of position and

velocity

ylðq; _qÞ ¼ cqþ c1 _q ð3:31Þynðq; _qÞ ¼ ðcjqjÞ qþ c1 _q ð3:32Þ

yn1ðq; _qÞ ¼ c½1þ 0:8 sin ðqÞ� qþ c1 _q ð3:33Þ

For these outputs and selected control error, the equivalent acceleration is

€qeql ¼ c� 11 ½yref0 y

ref1 vr cos ðvrtÞ� c _q� ð3:34Þ

€qeqn ¼ c� 11 ½yref0 y

ref1 vr cos ðvrtÞ� 2cjqj _q� ð3:35Þ

€qeqn1 ¼ c� 11 fyref0 y

ref1 vr cos ðvrtÞ� c½1þ 0:8 sin ðqÞ� _q� 0:8cq _q cos ðqÞg ð3:36Þ

As an illustration, plant (3.25) with the parameters listed above is simulated for out-

puts (3.31) with c ¼ 50; c1 ¼ 5, initial conditions in position qð0Þ ¼ 0:5 rad and equivalent

acceleration as input and the reference output yref ðtÞ ¼ 1þ 35 sin ð12:56tÞ. [The realization ofacceleration as input will be discussed in Chapter 4; here we are assuming that we can apply

such an input to the plant (3.25) by giving input force as τ ¼ τd þ aðqÞ€qeq where τd and aðqÞ areassumed known and €qeq is calculated as in (3.34)]. In this example a rotational motion for

plant (3.25) is assumed.

For qð0Þ ¼ 0:5 rad; _qð0Þ ¼ 0 rad/s the output initial value is ylð0Þ ¼ 25. The output

tracking error is expected to remain constant for t � 0 s. The changes in position qðtÞ andvelocity _qðtÞ are determined from the equation ylðq; _qÞ ¼ cqþ c1 _q ¼ 24þ yref ðtÞ – thus it is

the solution of a linear differential equation.

Figure 3.8 illustrates the system behavior for output linear in position and velocity (3.31).

In the first row, the reference output yref , the output y and the output error ey ¼ y� yref are

shown. The output error is eyð0Þ ¼ ylð0Þ� yref ð0Þ ¼ 24 and it remains constant since

equivalent acceleration enforces _eyðtÞ ¼ 0 ) eðtÞ ¼ const.

In the second row, the change in position q, the reference position qref [the solution of the

differential equation 50qþ 5 _q ¼ 24þ yref ðtÞ] and the difference between these two variables


eq ¼ q� qref is shown. The position converges to the reference qref . The convergence rate is

defined by the ratio c=c1 ¼ 10 – as expected from the structure of the output y ¼ 50qþ 5 _q.In the third row, the equivalent acceleration €qeq and plant disturbance τd are shown.

The equivalent acceleration in steady state is a harmonic function {due to the linear

dependence of output on system variables and harmonic change in reference output

€qeql ¼ 0:2½35 � 12:56 cos ð12:56tÞ� 50 _q� rad/s2}.

3.2.2.2 Equivalent Force

Insertion of acceleration derived from (3.1) into (3.22) yields

_s ¼ gaðqÞ τ� ½bðq; _qÞþ gðqÞþ τext�|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}

τd

� aðqÞ€qeq

8>><>>:

9>>=>>;; g 6¼ 0

_s ¼ ga� 1ðqÞðτ� τeqÞ

ð3:37Þ

Figure 3.8 Changes in reference yref , output y, output error ey, position q, reference position qref

[solution of linear differential equation cqþ c1 _q ¼ 24þ yref ðtÞ], equivalent acceleration €qeq and distur-

bance τd for plant (3.25) with output (3.31) and the equivalent acceleration €qeq as control input.

Parameters c ¼ 50; c1 ¼ 5 and initial conditions in position qð0Þ ¼ 0:5 rad and reference output

yref ðtÞ ¼ 1þ 35 sin ð12:56tÞ


Here τeq stands for the value of the input force for which the rate of change of the distancefrom the equilibrium s ¼ 0 is zero. This value of the input force will be called the equivalent

force. From Equation (3.25) the equivalent force τeq can be derived as

τeq ¼ ½bðq; _qÞþ gðqÞþ τext� þ aðqÞ€qeq¼ τd þ aðqÞ€qeq ð3:38Þ

By comparison with Equation (3.10) we conclude that the equivalent force is enforcing the

desired acceleration equal to the equivalent acceleration. Thus, the system dynamics enforced

by the equivalent force are the same as those enforced by the equivalent acceleration.

For control tasks with error dynamics as in Equations (3.12), (3.13), (3.16) and (3.17), the

equivalent force can be expressed as:

. for control tasks having error dynamics as in (3.16)

_sz ¼ 0 ) τeq ¼ ðbðq; _qÞþ gðqÞþ τextÞ� aðqÞc� 1ð _c _q� _z� €yref Þ ð3:39Þ

. for control tasks defined as function of velocity only

_eð _qÞ ¼ 0 ) τeq ¼ ½bðq; _qÞþ gðqÞþ τext� þ aðqÞc� 11 _yref ð3:40Þ

. for control tasks defined as function of position and velocity

_eðq; _qÞ ¼ 0 ) τeq ¼ ½bðq; _qÞþ gðqÞþ τext� � aðqÞc� 13 ðc2 _q� _yref Þ ð3:41Þ

. for control tasks with error dynamics as in (3.17)

_e1ðq; _q; €qeqÞ ¼ 0 ) τeq ¼ ½bðq; _qÞþ gðqÞþ τext� þ aðqÞc� 14 ½yref � y1ðq; _qÞ� ð3:42Þ

By inspection, it can be verified that in (3.39)–(3.42) the equivalent force is expressed as

in (3.38). If the force induced by the variable part of inertia is added to disturbance as in (3.4)

then the equivalent force may be expressed as

τeq ¼ τdis þ a n€qeq

τdis ¼ ½DaðqÞ€qþ bðq; _qÞþ gðqÞþ τext�ð3:43Þ

The difference from (3.38) is in force induced by thevariable part of inertiaDaðqÞ€q. In (3.43)this force is included in the generalized system input disturbance τdis. Here gain for the

equivalent acceleration is equal to an. If the initial conditions are consistent with

s½qð0Þ; _qð0Þ; yref ð0Þ� ¼ 0 the equivalent force applied at t ¼ 0will guarantees ¼ 0 for 8t � 0.

It can be easily verified that insertion of the desired acceleration (3.24) into (3.10) yields

τ ¼ τeq þ aðqÞg � 1 _sdes ð3:44Þ

and the dynamics of generalized error s is then as in (3.23). The dynamics (3.22) with

force (3.44) as control input is shown in Figure 3.9. The signals shown by dotted lines

cancel each other and the resulting dynamics are just a single integrator.


Till now we have derived inputs that, if applied at t ¼ t0, will enforce motion sðtÞ ¼ sðt0Þ8t � t0. In other words, we have determined input that will enforce constant output control

error. If at t ¼ t0 the generalized error is sðt0Þ ¼ 0 the equivalent acceleration or equivalent

force as control input for t � t0 will guarantee equilibrium solution sðt0Þ ¼ 0; 8t � t0.

Thus we have found the control input that will ensure equilibrium solution for consistent

initial conditions. This reduces the control system design to the selection of the input _sdes

which will enforces the convergence from the initial state sðt0Þ to the equilibrium and

the stability of the equilibrium s ¼ 0. This problem will be examined in the following

section.

Example 3.2 System Motion with Equivalent Force as Input Here we would like to

illustrate the plant (3.1) behavior with equivalent force τeq as control input. For easy

comparison with results shown in Example 3.1 the plant, parameters, the reference and the

initial conditions are the same as in Example 3.1.

For a general structure of output (3.27) the equivalent torque is

τeq ¼ τdis þ an€qeq

¼ Da€qþ bðq; _qÞþ gðqÞþ τext

þ anc� 11 ½yref0 y

ref1 vrcosðvrtÞ� cðqÞ _q� _cðqÞq�

ð3:45Þ

Here τdis is generalized disturbance (3.26).

In this example the outputs and control errors are selected as

ynðq; _qÞ ¼ ðcjqjÞqþ c1 _q; eyn ¼ yn � yref ð3:46Þ

yn1ðq; _qÞ ¼ cð1þ 0:8 sin ðqÞÞqþ c1 _q; eynl ¼ ynl � yref ð3:47Þ

The initial conditions in position and velocity are the same as in Example 3.1 – qð0Þ ¼0:5 rad, _qð0Þ ¼ 0 rad=s and the coefficients c ¼ 50; c1 ¼ 5. The output initial values are

ynð0Þ ¼ 12:5 and yn1ð0Þ ¼ 34:488. The output initial errors are eynð0Þ ¼ 11:5 and

eyn1ð0Þ ¼ 33:488; respectively. The changes in the position and velocity is determined from

the nonlinear differential equation ð50jqjÞqþ 5 _q ¼ 11:5þ yref ðtÞ and c½1þ 0:8 sinðqÞ�qþc1 _q ¼ 33:488þ yref ðtÞ with reference output yref ðtÞ ¼ 1þ 35 sin ð12:56tÞ.

Figure 3.9 The control loop for system (3.1) and equivalent structure


For outputs (3.46) and (3.47) end selected control errors the equivalent torque is as in (3.48)

and (3.49) respectively.

τeqn ¼ τdis þ 0:1€qeqn

¼ τdis þ 0:02½35 � 12:56 cos ð12:56tÞ� 100jqj _q�Nm ð3:48Þ

τeqn1 ¼ τdis þ 0:1€qeqn1

¼ τdis þ 0:0235 � 12:56 cos ð12:56tÞ�

� 50ð1þ 0:8 sin ðqÞÞ _q� 40q _q cos ðqÞ

!Nm

ð3:49Þ

Application of the equivalent torque instead of the equivalent acceleration is expected to

enforce the same motion of the plant if the parameters and the desired motion are the same.

Figure 3.10 illustrates system behavior for output ynðq; _qÞ ¼ ð50jqjÞqþ 5 _q with initial

conditions in position qð0Þ ¼ 0:5 rad and equivalent torque input (3.48). In the first row,

the output reference yref ðtÞ ¼ 1þ 35 sin ð12:56tÞ, the actual output y and the output error

Figure 3.10 Changes in reference output yref , output y, output error ey, position q, reference position

qref (solution of linear differential equation ð50jqjÞqþ 5 _q ¼ 11:5þ yref ðtÞ), equivalent torque τeq and

disturbance τd is for plant (3.25) with output ynðq; _qÞ ¼ ð50jqjÞqþ 5 _q and the equivalent torque as the

control input. Parameters c ¼ 50; c1 ¼ 5 and initial conditions in position qð0Þ ¼ 0:5 rad and referenceoutput yref ðtÞ ¼ 1þ 35 sin ð12:56tÞ


ey ¼ y� yref are shown. The output error is eyð0Þ ¼ 11:5. In the second row, the change of

the position q and its reference qref as the solution of the differential equation

ð50jqjÞqþ 5 _q ¼ 11:5þ yref ðtÞ and the difference eq ¼ q� qref between these two variables

is shown. In the third row, the equivalent torque τeq and disturbance τd are shown. The behavioris the same as for the equivalent acceleration as the control input. This illustrates the

equivalence of equivalent acceleration and equivalent torque as inputs – both enforces tracking

with an error determined by the initial conditions.

In Figure 3.11, illustration of system behavior for output (3.47) with torque input as in (3.49)

is shown. The plots are showing the same variables as in Figure 3.10. The output error is

eyð0Þ ¼ 33:488. In the second row, the change of the position q and its reference qref which isthe solution of the differential equation yn1ðq; _qÞ ¼ 33:488þ yref ðtÞ and the difference betweenthese two variables is shown. As expected the convergence is nonlinear. In the third row, the

equivalent torque and disturbance are shown.

Figure 3.11 Changes in reference output yref , output y, output error ey, position q, reference position

qref ðsolution of linear differential equation 50ð1þ 0:8 sin ðqÞÞqþ 5 _q ¼ 33:488þ yref ðtÞÞ, equivalenttorque τeq and disturbance τd for plant (3.25) with output (3.47) and the equivalent torque as the controlinput. Parameters c ¼ 50; c1 ¼ 5 and initial conditions in position qð0Þ ¼ 0:5 rad and reference output

yref ðtÞ ¼ 1þ 35 sin ð12:56tÞ


Examples 3.1 and 3.2 illustrate important features of the equivalent acceleration and

equivalent torque as control inputs:

. Output error remains constant with either equivalent acceleration of equivalent torque as

control inputs to the plant.. Convergence of the position towards its ‘reference’ is determined by equation yðq; _qÞ ¼yð0Þþ yref ðtÞ and it does not depend on the control input.

3.3 Enforcing Convergence and Stability

To complete the design, the rate of change of the generalized error _sdes needs to be selected. It

is easy to verify that selection _sdes ¼ � ks; k > 0 in (3.23) enforces convergence to the

equilibrium solution s ¼ 0 and the stability of the equilibrium.

In this section, wewould like to confirm results obtained so far in more formal way and look

at wider range of solutions. Error dynamics (3.22) or (3.37) allow us to write the Lyapunov

function candidate as

V ¼ s2

2> 0; Vð0Þ ¼ 0 ð3:50Þ

The time derivative of V can be determined as function of the acceleration of the input force

as shown in (3.51)

_V ¼ s _s¼ sgð€q� €qeqÞ¼ sga� 1ðqÞðτ� τeqÞ

ð3:51Þ

For €q ¼ €qeq or τ ¼ τeq; the Laypunov function derivative is _Vð€qeqÞ ¼ _VðτeqÞ ¼ 0, thus

Vð€qeqÞ and _VðτeqÞ are constant. In order to satisfy Lyapunov stability conditions _V must be

negative definite on the system trajectories. Stability requirements, for the appropriate scalar

function lðVÞ > 0, can be expressed as

_V ¼ s _s � � lðVÞ ð3:52Þ

By selecting control from _V ¼ � lðVÞ; a specific convergence of the Lyapunov function isenforced. For known lðVÞ the acceleration and the input force can be determined from (3.51)

and (3.52).

When selecting the right hand side as in (3.52) it should be taken into account that control

input in (3.1) is bounded. In order to complete the design, it has to be shown that lðVÞ can beselected so that conditions (3.52) are satisfied for bounded control input. When acceleration

or force provide the control input conditions, Equation (3.52) takes the following forms:

. for acceleration as the control input

s _sþ lðVÞ � sgð€q� €qeqÞþ lðVÞ � 0

� ½� gs€qeq þ lðVÞ� þ gs€q � 0ð3:53Þ


. for force as the control input

s _sþ lðVÞ � s

�g

aðqÞ ðτ� τeqÞ�þ lðVÞ � 0

�� g

aðqÞsτeq þ lðVÞþ g

aðqÞsτ � 0

ð3:54Þ

For s 6¼ 0, the acceleration and the force can be calculated from expressions (3.53) and (3.54),

respectively. These control inputs will enforce Lyapunov stability conditions (3.52) outside

manifold s ¼ 0 thus will enforce convergence to manifold s ¼ 0. The control and system

dynamics in manifold must be separately investigated.

In order to make derivations simpler let us investigate the case described by

_V ¼ � lðVÞ ð3:55Þ

Fors 6¼ 0,by inserting(3.50) and (3.24) into (3.55)yields theaccelerationenforcing (3.55)as

€q ¼ €qeq � 1

glðVÞjsj sign ðsÞ ¼ €qeq þ €qcon

€qcon ¼ � 1

glðVÞjsj sign ðsÞ; s 6¼ 0

ð3:56Þ

The acceleration (3.56) enforces the convergence to manifold s ¼ 0. It is a sum of

equivalent acceleration €qeq and the €qcon. Since equivalent acceleration enforces no changes

in the Lyapunov function derivative ( _sð€q ¼ €qeqÞ ¼ 0 and _Vð€qeqÞ ¼ s _sð€qeqÞ ¼ 0) the con-

vergence must be enforced by €qcon, thus we will call it convergence acceleration. The sign of

the convergence acceleration is opposite to the sign of the distance from the equilibrium thus

it is directing motion towards s ¼ 0. If the equilibrium solution is reached the acceleration

must be equal to €qeq, thus the influence of €qcon should vanish for s ¼ 0. This would require

particular structure of function lðVÞ in (3.55).

Before discussing the structure of lðVÞ let us derive an expression for the input force

satisfying (3.55). For s 6¼ 0, insertion of (3.50) and (3.51) into (3.55) yields

τ ¼ τeq � aðqÞgs

lðVÞ ¼ τeq þ τcon ¼ τeq þ aðqÞ€qcon

τcon ¼ � aðqÞg

lðVÞsj j sign ðsÞ; s 6¼ 0

ð3:57Þ

Similar to (3.56), the input force depends on the equivalent force and the convergence

force τcon.For system (3.1) both acceleration and force are assumed bounded. The function lðVÞ

should be selected appropriately in order to guarantee the boundedness of the control

inputs (3.56) and (3.57). At the same time, it should ensure that both the convergence

acceleration €qcon and the convergence force τcon (or at least their average values) vanish

when motion reaches the equilibrium solution s ¼ 0.


If lðVÞ remains finite and nonzero when jsj! 0, from (3.56) and (3.57) it follows that the

convergence acceleration and the convergence force will tend to infinity. Thus, function lðVÞmust tend to zero with a particular rate in order to ensure that €qcon and τcon have finite value asjsj! 0. From (3.56) the convergence acceleration €qcon is bounded if there exists a strictly

positive K > 0 such that, for s 6¼ 0

0 � lðVÞ � Kjg jjsj ð3:58Þ

If the rate of change of theLyapunov function satisfies (3.58) then €qcon � �K sign ðgsÞ andτcon ¼ aðqÞ€qcon. For a finite error both €qcon and τcon are finite and since the equivalent

acceleration and the equivalent force are bounded, the acceleration in (3.56) and the force

in (3.57) are bounded, also.

Selection of the Lyapunov function rate of convergence is restricted by (3.58). It is

straightforward to show that above requirements are met if the derivative of the Lyapunov

function candidate is selected as

_V � � lðVÞ ¼ � k2aVa; k > 0;1

2� a � 1 ð3:59Þ

Integration of _V � �hVa; h ¼ 2ak > 0 over the ½0; t� interval via separation of variablesyields for a 6¼ 1

V1�aðtÞ � V1�að0Þð1�aÞht ð3:60Þ

For a ¼ 1; integration of _V � �hVa; h ¼ 2ak > 0 over the 0; t½ � interval yields

VðtÞ � Vð0Þe�ht ð3:61Þ

Thus, for 1=2 � a � 1, the convergence VðtÞ��!t!¥ 0 is guaranteed.

From (3.60), finite-time convergence is achieved for 1=2 � a < 1; thus s reaches zero value

in time tr

tr � V1�að0Þ=hð1�aÞ ð3:62Þ

For 1=2 ¼ a the fastest finite-time convergence is achieved

tr � 21�a V1�að0Þ=k ð3:63Þ

For a ¼ 1 the exponential convergence is obtained. Consequently for a ¼ 1 a d– vicinity

of the equilibrium solution s ¼ 0 is reached in finite time, but s exponentially tends to

equilibrium s ¼ 0.

Control input (3.56) or (3.57) enforces Lyapunov stability for the selected structure of the

Laypunov function candidate and its time derivative. Analysis shows that this structure allows

exponential or finite-time convergence. In order to derive particular solutions for the conver-

gence acceleration the convergence coefficient a of the Lyapunov function candidate must be

selected.


The control system specification introduced in Section 3.2 can be satisfied if exponential

convergence is guaranteed. Because of this, wewill first complete the design for a convergence

rate with a ¼ 1. Later in the text we will consider systems with finite-time convergence.

Without loss of generality, all further derivations will be handled for system dynamics as

in (3.3), the Lyapunov function candidate as in (3.50) and the derivative of the Lyapunov

function as in (3.59) with a ¼ 1. Acceleration will be taken as the control input and force will

be treated as the mean to implement the desired acceleration or in other words to realize the

acceleration controller.

For the Lyapunov function candidate V ¼ s2=2 conditions (3.59) for a ¼ 1 are enforced if

the control input is selected from

sð _sþ ksÞ ¼ s½gð€q� €qeqÞþ ks� ð3:64Þ

For s 6¼ 0, the Lyapunov stability conditions sð _sþ ksÞ ¼ 0 reduce to _sþ ks ¼ 0. Then

the acceleration input enforcing sðtÞ��!t!¥ 0 from initial state consistent with operational

bounds sð0Þ 6¼ 0, to the equilibrium sðtÞ ¼ 0, is obtained from (3.64) as

€q ¼ €qdes ¼ €qeq � kg � 1s ¼ €qeq þ €qcon

€qcon ¼ � kg � 1sð3:65Þ

The change of sðtÞ is governed by

sðtÞ ¼ sð0Þe� kt ð3:66Þ

The convergence rate is defined by k > 0. If _sdes ¼ � kg � 1s is selected, the convergence

term is consistent with stability enforcement in (3.24). It is also consistent with (3.56). If jsj isbounded the desired acceleration is bounded. Therefore it will enforce the desired transient

within the bounded region in state space. This can be easily seen from (3.65) by taking into

account the bounds on acceleration

€qdes � €qeq

þ k g � 1s � €qmax ð3:67Þ

The force that enforces the desired acceleration (3.65) is

τ ¼ τd þ a€qeq � akg � 1s ¼ τeq � akg � 1s ¼ τeq þ τcon

τcon ¼ a€qconv ¼ � akg � 1sð3:68Þ

Here τeq stands for the equivalent force and τcon stands for the convergence force. Furtherdecomposition on the disturbance, the equivalent acceleration and the convergence acceler-

ation just points out a possible implementation.

The structure of control input naturally reflects the physical operation of the system. It

consists of three terms:

. the system disturbances τd or τdis, depending on the selection of the nominal plant,

. the control component needed to maintain the equilibrium sðtÞ ¼ 0, €qeq or τeq,

. the control component enforcing the convergence to the equilibrium €qcon or τcon.


The illustration of the forces and the motion enforced by the control input (3.68) is shown in

Figure 3.12. With τ ¼ τeq as input, the motion is in manifold sjτ¼τeq ¼ const equidistant from

s ¼ 0. The convergence force τcon directs the system motion towards the equilibrium state, so

the resulting motion is along the dashed line.

The composition of control as in (3.65) and (3.68) allows a deeper understanding of the

influence and the role of every component and thus leads to a simpler implementation. Stability

is related only to the convergence acceleration while the other two terms can be treated as

feedforward components. This opens the possibility of disturbance estimation and its direct

application to the system input. That allows the controller design to consider only the desired

motion and convergence to the equilibrium.

Example 3.3 Enforcing Exponential Convergence Enforcement of the exponential

convergence of the output control error is illustrated in this example. For easy comparison

with results shown in Examples 3.1 and 3.2, the plant, parameters, reference output and initial

conditions are the same as in Example 3.1.

The general structure of the desired acceleration is €qdes ¼ €qeq þ €qcon. Example 3.1 has

shown the system behavior with equivalent acceleration as input. It enforces a constant control

error. The convergence acceleration €qcon is selected to enforce specific dynamics of the output

control error. The desired dynamics of the control error are

_ey þ key ¼ 0; k ¼ 50 ð3:69Þ

For unbounded control, assumed in this example, the selection of a closed loop bandwidth

is not restricted. Since our intention is to show the functional relations and not design for

performance we selected slow convergence with a time constant 1/50 s.

From (3.46) and the selected parameters an¼ 0.1 kg �m2, c¼ 50, c1¼ 5, k¼ 50, the torque

input guaranteeing the desired convergence of the tracking error is

τ ¼ τdis þ 0:1€qdes ¼ ðτdis þ 0:1€qeqÞþ 0:1€qcon

¼ τeq � eyð3:70Þ

Figure 3.12 Illustration of the control forces and the motion enforced in a closed loop


With control input (3.70), the output tracking error is expected to converge to zero on the

trajectory _ey ¼ � 50ey in the ðey; _eyÞ plane.In the ðq; _qÞ plane, behavior is more complex and depends on the structure of the output

function and it should be evaluated for each output separately. For a fast convergence rate, the

duration of motion along line _ey ¼ � 50ey is short and then most of the transients in position

and velocity are restricted to a small vicinity of the equilibrium solution eyðtÞ ¼ 0. Due to the

exponential convergence, the solution eyðtÞ ¼ 0 is reached as t!¥ but, for engineering

evaluation solutions dictated by eyðtÞ ¼ 0 for t > ð5=kÞ ¼ 0:1 s, can be used as good approx-

imation. For eyðtÞ ¼ 0 change in position and velocity is governed by yðq � _qÞ ¼ yref ðtÞ and canbe evaluated for the given output and reference.

Note here the existence of two transients – one defined by the output convergence eyðtÞ! 0

and another describing the convergence of the plant position to the ‘reference’

qðtÞ! qref ðy ¼ yref Þ. These two processes define the overall dynamics of the closed loop

system.

Figure 3.13 illustrates control of the output yn1ðq; _qÞ ¼ 50½1þ 0:8 sin ðqÞ�qþ 5 _q to track

reference yref ðtÞ ¼ 1þ 35 sin ð12:56tÞ. The initial conditions are qð0Þ ¼ 0:5 rad,_qð0Þ ¼ 0 rad=s. The convergence acceleration gain is k ¼ 50 and the output reference

yref ðtÞ ¼ 1þ 35 sin ð12:56tÞ. The control input is not limited.

In the first row, the output reference yref , output y and the output error ey are shown. The

convergence is clearly shown in the tracking error diagram. The output y converges to the

reference yref with exponential rate 1/50 s. The steady-state output error is eyð1Þ ¼yn1ð1Þ� yref ð1Þ ¼ 1:544 � 10� 14. In the second row, the change of the position q and its

reference qref [solution of the differential equation yn1ðq; _qÞ ¼ yref ðtÞ] and the difference

between these two variables eq are shown. As expected the convergence is defined by the

structure of the output function. The transient in the position after output error converges to a

vicinity of equilibrium solution eyðtÞ ¼ 0 is clearly visible in the diagram. The error in position

reaches eqð1Þ ¼ qð1Þ� qref ð1Þ ¼ 2:689 � 10� 5 rad. In the third row, the input torque τ, theequivalent torque τeq, the convergence torque τcon ¼ an€q

con and the _eyðeyÞ diagram are shown.

The exponential decay of the convergence torque is clearly indicated. The convergence of the

input torque to the equivalent torque is also shown. This illustrates remarks on the role of

equivalent torque in overall control. It enforces output tracking. The _eyðeyÞ diagram illustrates

the enforcement of the transient _ey þ 50 ey ¼ 0 by the selected control law.

3.3.1 Convergence for Bounded Control Input

As shown in Section 3.1, the control forces, parameters and state coordinates are bounded, thus

the desired dynamics can be enforced within a bounded domain in the state space

Dy ¼ q; _q : _sþ ks ¼ 0; τmin � τ < τmaxf g ð3:71Þ

Here, τmin and τmax are lower and upper bounds of input force. For simplicity wewill assume

τminj j ¼ τmaxj j ¼ M > 0 and τmin ¼ � τmax.

Assume that for given initial conditions and references the desired dynamics (3.71) cannot

be enforced with available resources and consequently, control is saturating on either τmin or

τmax. In this situation, we need to investigate if the proposed control guarantees convergence

toward domain (3.71) and regain the desired closed loop dynamics. It is natural to assume that


reference is selected consistent with control input resources, thus if the equilibrium is reached

the equivalent force needed to maintain equilibrium satisfies jτeqj < M. This implies that plant

generalized disturbance force satisfies jτdisj < M.

To take available control resources into account, the control inputs (3.65) and (3.68) can be

written in the following form

€qdes ¼ satð€qeq � kg � 1sÞτ ¼ sat½ðτd þ a€qeqÞ� akg � 1s� ¼ satðτeq � akg � 1sÞ

ð3:72Þ

Here satð � Þ stands for the saturation function:

satðxÞ ¼x if jxj < L

Lx

jxj if jxj � L

8><>: ð3:73Þ


{solution of linear differential equation 50½1þ 0:8 sin ðqÞ�qþ 5 _q ¼ yref ðtÞ}, equivalent torque τeq,convergence torque τcon and control input τ. The output is yn1 ¼ 50 ½1þ 0:8 sin ðqÞ� qþ 5 _q with an

exponential convergence of the output error _ey þ 50ey ¼ 0. The reference output is yref ðtÞ ¼1þ 35 sin ð12:56tÞ and the initial conditions are qð0Þ ¼ 0:5 rad, _qð0Þ ¼ 0 rad=s


where L > 0 is strictly positive constant or continuous function. In our case L ¼ M > 0.

Assume that for given operational point, required equivalent acceleration is such that the

control resources enforcing the operational conditions are exceeding the available limits of

control. In this case, control is taking one of the extremes þM or �M and consequently the

rate of change of the control error _s becomes

_s ¼ gaðqÞ ðτ� τeqÞ ¼ g

aðqÞ Mτjτj � τeq

� ð3:74Þ

Note that, for unbounded control and s 6¼ 0 conditions sign ð _sÞ ¼ sign ½gðτ� τeqÞ=a� ¼� sign ðksÞ hold, thus for any τeq; the convergence conditions are enforced by the convergenceforce input. Inserting (3.73) yields

_s ¼ gaðqÞ M

τjτj � τeq

� ¼ � 1� M

jτj�

gaðqÞ τeq � M

jτj ks ð3:75Þ

By adding ks to both sides, (3.75) can be rearranged into

_sþ ks ¼ � 1� M

jτj�

gaðqÞ τeq � aðqÞg � 1ks

� � ¼ � 1� M

jτj�

gaðqÞ τ ð3:76Þ

Motion is directed towards domain _sþ ks��!jτj¼M 0. The time period during which control

input is equal to its extreme value τ ¼ Mðτ=jτjÞ is essentially determined by open loop plant

behavior, thus the dynamics for τ ¼ Mðτ=jτjÞ depend on all system parameters and distur-

bances. In general, the duration of such a motion is limited and for most of the time the closed

loop dynamics are governed by the desired dynamics.

By applying the same procedure the control input can be derived if system representation is

as in Equation (3.4), in which the variable part of the inertia force is treated as part of the

generalized disturbance. Inserting (3.4) into (3.22) yields the desired acceleration and the force

needed to enforce the desired acceleration as

€qdes ¼ satð€qeq � kg � 1sÞτ ¼ sat½ðτdis þ an€q

eqÞ� ang � 1ks� ¼ satðτeq � ang � 1ksÞ ð3:77Þ

Bounded control input enforces convergence to domain (3.71) in which the condition

τeqj j < M holds. Motion outside of domain Dy is governed by the extreme values τmin or

τmax of the control input and depends on the parameters of the system. After reaching the

domain (3.71), the convergence to the equilibrium is governed by _s ¼ � ks. The evolution ofthe system state is defined by the convergence dynamics on the system trajectories. The form of

implementation can be in either (3.68) or (3.77).

Example 3.4 Exponential Convergence with Bounded Control Input In this example

the behavior of a systemwith bounded control torque and/or bounded convergence acceleration

is illustrated. The plant, parameters, reference and initial conditions are the same as in

Example 3.3.

As shown inExample 3.3, for unbounded control the rate of convergence is defined by k > 0

in the entire state space. In this example, the limits on the control input force and/or


convergence force are applied while equivalent acceleration and equivalent force as compo-

nents of the control input are not separately limited. In order to facilitate these requirements, the

control input is implemented as

τ ¼ satT ½ðτdis þ an€qeqÞþ satTCðan€qconÞ� ð3:78Þ

Here saturation function satTð � Þ enforces limits on the control input and saturation function

satTC �ð Þ enforces limits on the convergence force.

Here we assume rotational motion described by model (3.25). The input torque is limited

on the max value equal to τj jmax ¼ 10 N:m and the convergence torque is bounded as

τcon ¼ an€qconj jmax ¼ 4 N:m. The control is selected to enforce control error dynamics

_ey ¼ � key; k ¼ 50, thus the convergence torque is selected as τcon ¼ � ankc� 11 ey.

With control input (3.78) control error ey is expected to converge to zero exponentially in

the bounded domain in state space. During the time for which control input is on its extreme

value (maximum or minimum) – thus having a constant value – the dynamics are described by

the open loop equations with constant control input. As τj j > τdisj j and τ ¼ τmaxsign ðτÞ.Motion is directed towards the domain τj j < τmaxj j and, after reaching it, motion in the ðey; _eyÞplane is constrained to line _ey ¼ � key; k > 0.

In Figure 3.14, simulations are shown for output yl ¼ 50qþ 5 _q and its reference

yref ðtÞ ¼ 1þ 35 sin ð12:56tÞ. The input torque (3.78) is limited by τj jmax ¼ 10 N:m but no

limit on the convergence torque is applied. The convergence gain is k ¼ 50, and initial

conditions are qð0Þ ¼ 0:5 rad, _qð0Þ ¼ 0 rad=s.In the first row, the output reference yref , the output y and the output error ey are shown. In the

second row, the change of the position q and its reference qref [solution of the differential

equation yn1ðq; _qÞ ¼ yref ðtÞ] and the difference between these two variables eq are shown.

In the third row, the input torque τ, the equivalent torque τeq, the convergence torque

τcon ¼ an€qcon and the _eyðeyÞ diagram are show. The dynamics with constant control input

are noticeable on all diagrams.

Perhaps it is best to analyze motion in the ðey; _eyÞ plane. Motion along line _ey ¼ � 50ey has

two segments – in the initial stage the control input needed tomaintain _ey ¼ � key is lower than

the available resources (clearly shown on the diagram in lower left corner) with control input

being in the region τj j < τmaxj j. After reaching τj j ¼ τmax sign ðτÞ motion along _ey ¼ � 50eyrequires higher resources, thus it cannot be enforced. The motion control input τj j ¼ τmaxj jdeviates from _ey ¼ � 50ey until resources are sufficient to enforce such a motion. After

reaching a region in which _ey ¼ � key and τj j � τmaxj j further motion is the same as discussed

and illustrated in Example 3.3. The convergence of the control torque to the equivalent torque is

clearly illustrated on diagram in third row. The limit on the available resources if not properly

managed can cause a large overshoot. There are many possibilities to avoid overshooting. The

simplest is to change the slope of the line _ey ¼ � key.

In Figure 3.15, the same diagrams as in Figure 3.14 are shown. Output is selected as

nonlinear function of position ynðq; _qÞ ¼ ð50jqjÞqþ 5 _q. Here, both the input torque and the

convergence torque are limited on τj jmax ¼ 10 N:m and τconj jmax ¼ 4 N:m; respectively. In thiscase the control and the convergence torque are reaching saturation at t ¼ 0 s. Effectively the

dynamics is governed by limited input torque. For t � 0:17 s the input torque is not saturatedbut the convergence torque is saturated and for 0:17 s� t � 0:25 s motion in ðey; _eyÞ is along_ey ¼ const. For t � 0:25 s both input and convergence torques are not bounded andmotion is in


domain (3.71). The motion along trajectory _ey þ 50ey ¼ 0 is enforced for

ey � τconmaxðankc� 1

1 Þ� 1 ¼ 4. The diagrams illustrate dependence of the reaching transient

on the available control resources. For given resources, the region in which desired motion

_ey þ key ¼ 0 can be enforced depends on both limits on input and the limits of the convergence

torque.

3.3.2 Systems with Finite-Time Convergence

Selection of a Lyapunov function derivative as in Equation (3.59) guarantees convergence to

the equilibrium solution. In previous sections the control input has been selected to enforce the

stability conditions _V ¼ � 2a kVa; a ¼ 1. Similarly, the stability conditions _V ¼ � 2a kVa;12� a < 1 enforcing finite-time convergence to the equilibriums ¼ 0 can be used for selection

of the control input. These conditions for s 6¼ 0 yield _s ¼ � ks2a� 1. For 12� a < 1 the

change of the exponent is bounded by 0 � 2a� 1 � 1. Since s can be positive or negative


[solution of linear differential equation 50qþ 5 _q ¼ yref ðtÞ], input torque τ, equivalent torque τeq,convergence torque τcon and _eyðeyÞ diagram. Output is defined as yl ¼ 50qþ 5 _q and convergence gain

is k ¼ 50. The limit on input torque is τj jmax ¼ 10 N:m.No separate limit on the acceleration torque τcon isenforced. Reference output is yref ðtÞ ¼ 1þ 35 sin ð12:56tÞ


some of the solutionsmay yield imaginary values ifs < 0. For simplicity, values ofawhich for

s < 0 generate imaginary solutions will be excluded from further analysis. In the following

analysis only a that yields real values for s2a� 1; 0 � ð2a� 1Þ < 1 and satisfy s2a� 1 ¼jsj2a� 1

sign ðsÞ will be considered. Selection of a ¼ p=q; p < q and 2p � q yields

ð2a� 1Þ � 0 and division by zero in _s ¼ � ks2a� 1 is avoided. Under these assumptions,

the finite-time convergence stability conditions, for s 6¼ 0 yield

_V ¼ s _s ¼ � 2a kVa )s 6¼0

s½ _sþ kjsj2a� 1signðsÞ� ¼ 0; s 6¼ 0

1

2� a < 1

ð3:79Þ

For the control tasks specified in (3.6)–(3.9) and the generalized error dynamics as in

(3.22), the conditions in (3.79) yield the following solutions for the desired acceleration and the

control force


[solution of linear differential equation ð50jqjÞqþ 5 _q ¼ yref ðtÞ], input torque τ, equivalent torque τeq,convergence torque τcon and _eyðeyÞ diagram. Output is defined as ynðq; _qÞ ¼ ð50jqjÞqþ 5 _q and conver-

gence gain is k ¼ 50. The limit on input torque is τj jmax ¼ 10 N:m. The limit on the acceleration torque

is τconj jmax ¼ 4 N:m


€qdes ¼ satð€qeq þ €qconÞ;€qcon ¼ � kg � 1jsj2a� 1

sign ðsÞτ ¼ satðτeq þ τconÞ;

τcon ¼ � akg � 1jsj2a� 1sign ðsÞ

ð3:80Þ

The saturation satð � Þ is used to indicate that the acceleration and the control input are

bounded. Differences between the control inputs as expressed in (3.80) and (3.72) are in the

convergence term only. The equivalent acceleration and the equivalent force do not depend on

the way system converges to the equilibrium thus difference in the desired convergence is

reflected only on the convergence acceleration.

If the system is described as in Equation (3.4), then appropriate expressions for the desired

acceleration and the input control force are

€qdes ¼ satð€qeq þ €qconÞ;€qcon ¼ � kg � 1jsj2a� 1

sign ðsÞτ ¼ satðτeq þ τconÞ;

τcon ¼ � ankg � 1jsj2a� 1sign ðsÞ

ð3:81Þ

For a ¼1=2 control takes a specific form

€qdes ¼ sat½€qeq � kg � 1signðsÞ�τ ¼ satðτeq � ang � 1k sign sÞ

sign s ¼ þ 1 if s > 0

� 1 if s < 0

( ð3:82Þ

Control in (3.82) is discontinuous in manifold s ¼ 0. The discontinuity comes from

selection of the convergence acceleration while other components of the control are the same

as for the system with exponential convergence. Motion generated by control (3.82) exhibits a

high frequency (theoretically infinite) oscillation around equilibrium and is known as sliding

mode motion [6]. Application of algorithm (3.82) in motion control system is delicate due to

discontinuity in control and high frequency oscillation. Unmodeled dynamics present in many

systems may be excited by this oscillation or oscillation may cause excessive activity of

actuator. Since motion in the manifold is driven by equivalent acceleration and the discon-

tinuous term defines only the convergence, in general the convergence term can be selected

with variable amplitude. That would allow adjustment of a discontinuous term amplitude

similar to that in (3.80) and (3.81). In that case the restriction s2a� 1 ¼ jsj2a� 1sign ðsÞ can be

removed and the control (3.82) with kg � 1 sj jbsign ðsÞ; 0 < b < 1 instead of kg � 1sign ðsÞcan be directly applied. In this case _s ¼ � k sj jbsign ðsÞ; k > 0 guarantees the convergence

and stability of the equilibrium s ¼ 0.

If information on disturbance and equivalent acceleration needed to implement the

control (3.82) is not available, then the amplitude of the discontinuous term can be adjusted

to be equal to the maximum available control resources

τ ¼ �M � sign ðgsÞ; M > τdis þ an€qeq ¼ τeq

ð3:83Þ


In order to ensure motion in manifold (3.71) the average force input (3.83) must be equal to

the equivalent force. This may be easily verified from the reasoning applied in establishing

equivalent control method as discussed in Chapter 2.

The finite-time convergence to and the stability of the equilibrium result in a sliding mode

motion inmanifolds ¼ 0. The control (3.81) is continuous while control in (3.82) or (3.83) are

discontinuous. The application of slidingmode control to electromechanical systems is studied

in detail in [4].

Example 3.5 Finite-Time Convergence In this example we would like to illustrate the

dynamics of systems with control enforcing output finite-time convergence. The plant,

parameters, reference and initial conditions qð0Þ ¼ 0:5 rad, _qð0Þ ¼ 0 rad=s are the same as

in Example 3.4. The units are consistent with rotational motion.

The control input is τ ¼ τdis þ an€qeq þ an€q

con with convergence torque

τcon ¼ an€qcon ¼ � ankc

� 11 ey 2a� 1

signðeyÞ; k > 0;1

2< a < 1 ð3:84Þ

In all experiments the convergence exponent is a ¼ 0:80, with k ¼ 50 and an¼ 0.1 kg �m2.

The convergence time is determined by

tr ¼ V1�að0Þ2akð1�aÞ ¼

e2ð0Þ2

� �1�a1

2akð1�aÞ ¼ ½e2ð0Þ�1�a 1

2kð1�aÞ s ð3:85Þ

For a given output ynðq; _qÞ ¼ ð50jqjÞqþ 5 _q and with initial conditions in position and

velocity the initial control error is eyð0Þ ¼ 12:5 and the convergence time for a ¼ 0:80 and

k ¼ 50 is trðynÞ ¼ 0:137 s.Figure 3.16 shows transients for output nonlinear in position ynðq; _qÞ ¼ ð50jqjÞqþ 5 _q

controlled to track yref ðtÞ ¼ ½1þ 35 sin ð12:56tÞ�. The composition of diagrams is the same as

in the figures in Example 3.4. In the first row the output y, the output reference yref and the

output tracking error ey are shown. It can beverified that predicted reaching time trðynÞ ¼ 0:137is achieved. In the second row the change in the position q, the position reference [solution

of linear differential equation ð50jqjÞqþ 5 _q ¼ yref ðtÞ], and the error eq are shown. In the last

row the input torque τ, the equivalent torque τref and the convergence torque τref are shown. Thetransients in _eyðeyÞ clearly show that motion is constrained on the trajectory

_ey þ 50 ey 0:60sign ðeyÞ ¼ 0. The slope of that trajectory in the vicinity of the origin is high

and theoretically tends to infinity.

This example illustrates the consistency of system behavior enforced by acceleration

control. Equilibrium is enforced by the equivalent acceleration (or equivalent force) while

convergence towards equilibrium is enforced by the convergence acceleration (or convergence

force). Such a design allows effective usage of the resources and a clear understanding of

the role of each component.

3.3.3 Equations of Motion

The dynamics of a closed loop is one of the key issues in control system design. In this section

we will examine equations of motion for system (3.1) with control input (3.65) enforcing


asymptotic convergence of output and input (3.80) with finite-time convergence of the output

to its reference. Of particular interest are the dynamics of the system state coordinates in these

two cases.

3.3.3.1 Systems with Asymptotic Convergence

Let us first examine the equations of motion for systems with control input (3.65) or (3.68).

Control inputs (3.65) and (3.68) are continuous, thus the methods of continuous control

systems can be directly applied. Without any loss of generality, we may assume initial

conditions consistent with domain (3.71). Inserting the input force from (3.68) into (3.1)

yields closed loop dynamics of the generalized error

d

dtsþ ks ¼ 0 ) sðtÞ ¼ sð0Þe� kt ��!

t!¥k > 0

0 ð3:86Þ

Figure 3.16 Changes in reference yref , output y, output error ey, position q, reference position qref , input

torque τ, equivalent torque τeq, convergence torque τcon and _eyðeyÞ diagram. Output is defined as

ynðq; _qÞ ¼ ð50jqjÞqþ 5 _q, convergence gain is k ¼ 50, a ¼ 0:8 and reference input yref ðtÞ ¼½1þ 35 sin ð12:56tÞ�


The distance from equilibrium s ¼ 0 is governed by first-order differential Equation (3.86)

with constant parameter k > 0. The convergence is a design parameter and it can be selected in

the design process. This behavior is illustrated in Example 3.3 for unbounded control and in

Example 3.4 for bounded control. Output convergence to its reference and stability of the

equilibrium solution is guaranteed for all initial conditions consistent with bounds on plant

parameters and inputs.

The dynamics of the system state coordinates can be derived from (3.86) by using an

appropriate expression to replace the generalized error s. This has been illustrated in all

examples so far. In a more general framework let us find equations of motion for the tasks

specified in Euqations (3.6)–(3.9) and the corresponding dynamics of the state coordinates.

For task (3.6) the manifold S1 ¼ fq; _q : sðqÞ ¼ _eðqÞ� z½eðqÞ� ¼ 0g specifies the desired

equilibrium solution. By inserting s ¼ _e� z into (3.86) the closed loop dynamics of the

system (3.6) become

€eðqÞþ k _eðqÞ� _z½eðqÞ� � kz½eðqÞ� ¼ 0 ð3:87Þ

Equation (3.87) describes second-order dynamics in the output control error

eðqÞ ¼ yðqÞ� yref . The motion depends on the selection of z½eðqÞ� – the desired change in

the control error derivative. For example, the selection of z½eðqÞ� ¼ � k1e with k1 > 0 yields

€eþðkþ k1Þ _eþ kk1 e ¼ 0. The roots of the characteristic equation are determined by the

selection of the Lyapunov function convergence rate m1 ¼ � k and the second m2 ¼ � k1 by

the selection of z½eðqÞ�. Since k1; k > 0 the dynamics in (3.87) are stable. The dynamics of the

position can be calculated from the specification of the output variable yðqÞ and its reference

just by inserting e ¼ y� yref into (3.87). If output yðqÞ is a nonlinear function the change

of plant position will be then driven by a nonlinear differential equation.

The equilibrium solution yðqÞ ¼ yref yields the plant position. This restricts the selection of

the output function yðqÞ. It must guarantee that the solution q ¼ fqðyref ; yÞ is unique and

continuous with continuous first-order time derivative.

For generalized error s ¼ eð _qÞ the closed loop dynamics is

_eð _qÞþ keð _qÞ ¼ 0; k > 0 ð3:88Þ

Consequently, the generalized error converges to zeros !t!¥

0. The equilibrium solution for

velocity is then determined from yð _qÞ ¼ yref and selection of output function yð _qÞ should

guarantee that _q ¼ fvðyref ; yÞ is unique.Systems with the control error as a function of position and velocity can be analyzed in the

same way. Without losing generality in approach, let the output be defined by y ¼ c2qþ c3 _qwith c2; c3 > 0. Let the reference output be yref . Insertion of the s ¼ yðq; _qÞ� yref into (3.86)

yields

c3€qþðc2 þ kc3Þ _qþ kc2q ¼ _yref þ kyref ð3:89Þ

The closed loop dynamics are defined as a second-order system with poles of characteristic

equation � k and �ðc2=c3Þ. Both are real negatives. For known yref ðtÞ the equilibrium

solution for position can be determined from (3.89). The dynamics are illustrated in Exa-

mple 3.3 for unbounded control input and in Example 3.4 for bounded control input.


Inserting s ¼ ez from (3.17) into (3.86) yields

_ez þ kez ¼ 0; k > 0 ð3:90Þ

The control error ez asymptotically tends to the equilibrium solution ez ¼ 0. The closed loop

dynamics is described by a first-order differential Equation (3.90). If k > 0 is selected high

enough the transient (3.90) quickly converges towards ez ¼ 0 and the closed loop motion

is then determined by c4€qþ y1ðq; _qÞ� yref ¼ 0. The selection of output function yð€q; _q; qÞis restricted by the requirements that equation yð€q; _q; qÞ ¼ yref guarantees the stability and

uniqueness of the steady-state solution.

The dynamic of control error and the system coordinates for the control tasks specified in

Equations (3.6)–(3.9) can be derived from (3.86). This provides a tool for assessment of the

closed loop dynamics prior to the selection of the actual control input. In addition, it allows us to

determine the domain (3.71) in which, for given system resources, the desired dynamics can be

enforced. Such a possibility is of importance for systems that may require specific dynamics.

Examination of the equations of motion allows a proper selection of control parameters so that

the desired dynamics are achieved within the specified domain of the change of the state

coordinates and the available resources.

3.3.3.2 Systems with Finite-Time Convergence

The equilibrium s ¼ 0 is reached at finite time t ¼ t0. Since the equilibrium is stable the

system state is constrained to satisfy s ¼ 0, 8t � t0. As shown in Chapter 2, the same

behavior is encountered for systems with sliding modes. Indeed, finite-time convergence and

the stability of the equilibrium solution is guaranteed, thus sliding mode motion is enforced

in manifold S1 ¼ ðq; _q : s ¼ 0Þ. Consequently, equations of motion for systems with finite-

time convergence can be derived by applying methods of sliding mode control systems

analysis.

Accordingly methods of systems with sliding mode, equivalent control (obtained as the

solution of the algebraic equation _s ¼ 0 on the system trajectories), should be substituted into

the system dynamics. The resulting equations together with s ¼ 0 are taken as the equation of

motion in slidingmode [6]. Solution of the equation _s ¼ 0 is discussed in detail in Section 3.2.2

and the solutions for the equivalent acceleration and equivalent force are given. The equivalent

control method procedure yields

aðqÞ€qþ bðq; _qÞþ gðqÞþ τext ¼ τeq

s ¼ 0

)ð3:91Þ

Insertion of (3.38) into (3.91) yields

aðqÞ€q ¼ aðqÞ€qeqs ¼ 0

)ð3:92Þ

Insertion of the equivalent acceleration from (3.18)–(3.21) into (3.92) yields:


. for control output with dynamics of augmented error (3.16)

€q ¼ � c� 1f _c _q� _z½eðqÞ� � €yref g; s ¼ _eðqÞ� z½eðqÞ� ¼ 0 ð3:93Þ

. for control output defined as a function of velocity only with error dynamics as in (3.12)

€q ¼ c� 11 _yref ; s ¼ eð _qÞ ¼ yð _qÞ� yref ¼ 0 ð3:94Þ

. for control output defined as a function of velocity and position with error dynamics as

in (3.13)

€q ¼ � c� 13 ðc2 _q� _yref Þ; s ¼ eðq; _qÞ ¼ yðq; _qÞ� yref ¼ 0 ð3:95Þ

. for control output defined as a function of position, velocity and accelerationwith augmented

error dynamics as in (3.17)

€q ¼ c� 14 ½yref � y1ðq; _qÞ�; s ¼ c4 _q� z ¼ 0 ð3:96Þ

Rearranging the equations of motion (3.93)–(3.96) yields a description in the following form

ds

dt¼ 0; s ¼ 0 ð3:97Þ

Thus the dynamics for all of the analyzed problems (3.6)–(3.9), ifmanifolds ¼ 0 is reached

at t ¼ t0, are governed by the equilibrium solution for generalized error

s ¼ 0; 8t � t0 ð3:98Þ

The dynamics (3.98) yield first-order dynamics for system specification in (3.6). The desired

velocity z½eðqÞ� in (3.15) is reached in finite time but the transient in position is governed by

the selection of z½eðqÞ�. If z½eðqÞ� is selected as a nonlinear function such that solution of the

_eðqÞþ z½eðqÞ� ¼ 0 gives finite-time convergence in eðqÞ then, in such a special case, the

solution eðqÞ ¼ 0 will be reached in finite time.

For control tasks as defined in Equations (3.7) and (3.8) the equilibrium state is reached in

finite time. That means, tracking in the velocity, or the force control loop can be enforced in

finite time. The change of the system coordinates is then governed by the first order [for (3.7)

and (3.8)] These equations are defined in terms of the output and its reference. Consequently,

for a stable solution in the system coordinates the selection of output and its reference must

guarantee unique and stable solutions for the system coordinates from

eð _qÞ ¼ yð _qÞ� yref ¼ 0 in ð3:7Þeðq; _qÞ ¼ yðq; _qÞ� yref ¼ 0 in ð3:8Þeð€q; _q; qÞ ¼ c4€qþ y1ðq; _qÞ� yref ¼ 0 in ð3:9Þ

ð3:99Þ

That defines the properties of the output function to be continuous and differentiable for

an appropriate number of times. For eð _qÞ ¼ yð _qÞ� yref ¼ 0 the velocity is the solution of


algebraic equations. In all other cases, these equations are linear or nonlinear differential

equations and they must satisfy the conditions for uniqueness and stability of the solution of

differential equations.

Equations (3.96) and (3.98) show that, if generalized error is selected as in (3.17), the

finite-time convergence enforces motion that is described by the first-order differential

equation s ¼ c4 _q� z ¼ 0. This has to be taken into account if the desired dynamics is as

in (3.9). Then for finite-time convergence, desired acceleration should be determined

from (3.9).

Example 3.6 Comparison of theClosedLoopDynamics forDifferentConvergence In

this example we would like to compare system behavior with exponential and finite-time

convergence and give a more detailed illustration on the changes in position and velocity

transient for different structures of the convergence force. To concentrate only on the control

properties, simulations are shown for unbounded control, thus motion with saturated control

input is avoided. That allows illustrating the intrinsic properties of the closed loop system

behavior. The plant and the parameters are the same as in Examples 3.1 and 3.5.

The initial conditions in position qð0Þ ¼ 0:5 rad, _qð0Þ ¼ 0 rad=s and the coefficient

c ¼ 50; c1 ¼ 5 are applied, and the control error is ey ¼ y� yref .

The exponential and finite-time convergence acceleration are defined as:

. for exponential convergence

€qcon ¼ � 10ey ¼ � 10ðy� yref Þ ð3:100Þ

. for finite-time convergence

€qcon ¼ � 10 ey 0:5signðeyÞ ð3:101Þ

The closed loop motion of the system is expected to satisfy:

. for exponential convergence

_ey þ 50ey ¼ 0 ð3:102Þ

. for finite-time convergence

_ey þ 50 ey 0:5sign ðeyÞ ¼ 0 ð3:103Þ

The convergence time is an important feature of the closed loop. For linear dependence of

output in both position and velocity yl ¼ 50qþ 5 _q with initial conditions qð0Þ ¼ 0:5 rad,_qð0Þ ¼ 0 rad=s the initial value of error is eyð0Þ ¼ 24. For convergence coefficient k ¼ 50 and

exponential convergence a ¼ 1, the time constant is l ¼ 0:02 s and the time to reach

eyðtÞ ¼ 0 is approximately try ¼ 0:1 s. The time constant is directly defined by the conver-

gence coefficient k and the reaching time is approximately tr ¼ 5 k� 1 s. For large initial

errors control tends to be large. That may cause some problems in systems with bounded

control.


For the finite-time convergence, reaching time tr ¼ e2ð1�aÞ ð0Þ=2kð1�aÞ is function of

the initial error, the convergence coefficient k and the convergence exponent a. For the same

initial error eð0Þ ¼ 24 and convergence coefficient k ¼ 50 the convergence time for a ¼ 0:75is trðylÞ ¼ 0:19595. It is almost twice longer than for the exponential convergence. If needed it

can be adjusted by selecting either a or k.

Transients, for output yl ¼ 50qþ 5 _q and k ¼ 50 for exponential convergencea ¼ 1 and the

reference yref ðtÞ ¼ ½1þ 35 sin ð12:56tÞ�, are shown in Figure 3.17. In Figure 3.18, transients

for the same output and the same reference but with a ¼ 0:75 are shown. In the left column of

each figure, the behavior of the output is shown and in the right column the behavior of position

is shown. The reference position is calculated as the solution of 50qþ 5 _q ¼ yref ðtÞ, thus fromy ¼ yref .

The enforcement of the output transient as in Equation (3.102) is clearly illustrated in

diagrams shown in the left column. The exponential change in output control error (second row

in first column) is shown and convergence time is as predicted. The _eyðeyÞ diagram is a straight

linewith slopek ¼ 50 and, due to unbounded control it is enforced for t � 0. In the right column,

the convergence of the position is shown. The position error eq exponentially tends to zero.

Figure 3.17 Changes in reference yref , output y, output error ey, _eyðeyÞ diagram (in the left column) for

the output ylðq; _qÞ ¼ 50qþ 5 _q and the reference yref ðtÞ ¼ ½1þ 35 sin ð12:56tÞ�, position q, reference

position qref (solution of differential equation 50qþ 5 _q ¼ yref ðtÞ), position error eq and _eqðeqÞ diagram(in the right column). The convergence gain is k ¼ 50 and a ¼ 1


That is clearly illustrated in both time change of eq and the _eqðeqÞ diagram. The reaching time

try ¼ 0:1 s is equal to the convergence time for output tracking error. For try > 0:1 s themotion is

constrained to e-vicinity of trajectory 50eq þ 5_eq ¼ 0. Change in position is close to solution of

differential equation 50eq þ 5 _eq ¼ 1þ 35 sinð12:56tÞ. This can be observed in the diagram in

second row of right column.

In the Figure 3.18 enforcement of the closed loop dynamics (3.103) is illustrated for the

same output and the same reference as in Figure 3.17. The composition of the diagrams is the

same for easy comparison. The output error convergence time is as predicted. The _eyðeyÞdiagram illustrates that motion constrained to the curve _ey þ 50 ey

0:5signðeyÞ ¼ 0. The

convergence to the equilibrium ey ¼ 0 at try ¼ 0:19595 is shown. The change of the position

for t � tr is governed by 50qþ 5 _q� yref ðtÞ ¼ 0; thus the changes in position are governed bysolution of differential equation 50eq þ 5 _eq ¼ yref ðtÞ and is the same as in Figure 3.17.

The transients for exponential and finite-time convergence clearly show two separate

dynamics: (i) the dynamics of the output variable described by the design curves (3.102)

or (3.103) and (ii) the dynamics of the position (state coordinate) defined by the structure of

the output function. This fact is very important in motion control systems since many tasks are

Figure 3.18 Changes in reference yref , output y, output error ey, _eyðeyÞ diagram for the output

ylðq; _qÞ ¼ 50qþ 5 _q and the reference yref ðtÞ ¼ ½1þ 35 sin ð12:56tÞ� are shown in the left column. The

right column shows changes in position q, reference position qref [solution of differential equation

50qþ 5 _q ¼ yref ðtÞ], position error eq and _eqðeqÞ diagram. The convergence gain is k ¼ 50 and a ¼ 0:75


related to the force control, thus will involve control of the output which may be nonlinear

function of state.

3.3.4 General Structure of Acceleration Control

Control tasks discussed so far are expressed as function of: (i) position, (ii) velocity,

(iii) position þ velocity and (iv) position þ velocity þ acceleration. Such tasks specifica-

tions along with proper selection of the references include most if not all common problems in

control ofmechanical system.The design had been presented at two different levels – one based

on intuitive reasoning and another based on the enforcement of the desired Lyapunov stability

conditions with asymptotic or finite-time convergence. Both solutions are shown to lead to

acceleration control structure. Formally, control is selected to enforce stability of the

equilibrium solution for a generalized control error sðeÞ. A generalized output control error

is selected with relative degree one for acceleration of force as the control input.

The dynamics to the equilibrium solution is a part of the design specification. In general,

control can be required to enforce asymptotic or finite time convergence. The selection of

convergence determines the dynamics of the closed loop system. If asymptotic convergence is

enforced the generalized error reaches equilibrium sðeÞ ¼ 0 as time tends to infinity. The

closed loop system dynamics are determined by the convergence law _sðeÞþ ksðeÞ ¼ 0. If

finite-time convergence is enforced then, after reaching manifold sðeÞ ¼ 0, further motion

is constrained by the constraint equation sðeÞ ¼ 0.

System (3.1) is linear in control input (acceleration or force depending on choice of the

control input). This allows the control design to be solved in a two-step procedure. In the first

step, the desired acceleration is selected. In our design the control – desired acceleration €qdes –is selected as the sum of the equivalent acceleration €qeq and the convergence acceleration €qcon

€qdes ¼ satequivalent

acceleration

� þ convergence

acceleration

� � �ð3:104Þ

Equivalent acceleration is task-specific, thus it can be derived from a known control output,

its reference and structure of generalized error sðeÞ. The convergence acceleration is specifiedby the desired convergence law. The saturation function in (3.104) just reflects the fact that the

control input is bounded, thus the desiredmotion can be enforced in the bounded domainwithin

the system state space.

The force that enforces the desired acceleration (3.104) in system (3.1) is

τ ¼ satsystem

disturbance

� þq

equivalent

acceleration

� þ convergence

acceleration

� � �|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}

desired acceleraion

8>><>>:

9>>=>>;; q ¼ aðqÞ or an

ð3:105Þ

The disturbance is added to the desired acceleration (3.104) as a way to compensate system

disturbance andmake the system acceleration equal to the desired acceleration. The input force

as in (3.105) stands for the acceleration controller. The desired acceleration enforces the

control task and the disturbance feedback enforces acceleration tracking.


For a given system, disturbance is a property of the system structure and forces due to

interactions with environment. In contrast, the equivalent acceleration and convergence

acceleration are determined by the control task and the desired rate of convergence of the

Lyapunov function. Thus, the cascade structure as shown in Figure 3.19 is the best for reflecting

the control system structure. The desired acceleration is determined in the outer, control

task-dependent loop. The role of the inner loop is disturbance compensation.

A peculiarity of the system lies in the composition of the block calculating the control error.

In general structure, this consists of the control output error e ¼ y� yref and the block for

calculation of generalized error sðeÞ. In this structure, the control variable appears to be sðeÞ.The output control is then enforced by virtue of the equilibrium solution sðeÞ ¼ 0 and by

selection of the reference.

Convergence acceleration is the most specific term of the control. Its role is to enforce the

desired convergence rate and the stability of the equilibrium solution. In position control

both position and velocity feedback are required to guarantee the convergence and

stability of equilibrium. That dictates a difference in the solutions for tasks that depend

on the velocity and those that are only a function of the position. For the tasks that depend on

velocity the convergence acceleration can be selected proportional to control error. For tasks

that do not depend on velocity the convergence acceleration must be created as a function of

the control error and its derivative. Here, these differences are resolved by selecting a

structure of the generalized error sðeÞ to include the velocity. Then the Lyapunov function

candidateVðsÞ depends on both position and velocity and convergence acceleration dependsonly on the selection of the derivative _V ¼ � lðVÞ of the Lyapunov function and can be

expressed as

convergence

acceleration

� ¼ €qcon ¼ � 1

glðVÞsj j sign ðsÞ; s 6¼ 0 ð3:106Þ

This formulation includes both systems with asymptotic and finite-time convergence.

Selection of the function lðVÞ ¼ 2 kV leads to exponential convergence to the equilibrium.

Selection of the function lðVÞ ¼ 2a kVa;1=2 � a < 1 leads to the finite-time convergence

to the equilibrium. Selection lðVÞ ¼ 2a kVa;1=2 ¼ a leads discontinuous control.

Before turning to the implementation issues, let us take more detailed look at each of the

components forming the control input. The control input (3.105) can be expressed as

τ ¼ τdis þ an€qdesðsÞ

¼ DaðqÞ€qþ bðq; _qÞþ gðqÞþ τext½ � þ an€qdesðsÞ

ð3:107Þ

Figure 3.19 Structure of an output control system with the desired acceleration determined as the

output of the outer loop and with disturbance feedback as a way to implement acceleration control


Here €qdesðsÞ stands for the desired acceleration as a function of the generalized error.

Insertion of the control input (3.107) into system model (3.4) with an assumption that τdis isknown exactly yields

an€qþDaðqÞ€qþ bðq; _qÞþ gðqÞþ τext ¼ τ

¼ an€qdesðsÞþ DaðqÞ€qþ bðq; _qÞþ gðqÞþ τext½ � ð3:108Þ

The closed loop dynamics reduce to

€q ¼ €qdesðsÞ ð3:109Þ

Dynamics (3.109) describe a double integrator. The role of the disturbance feedback is to

enforce the tracking of the desired acceleration in inner loop. Then in the outer loop design the

desired acceleration, as a function of the task and the desired convergence to equilibrium,

is determined.

In the proposed structure, the desired acceleration is enforced due to the assumption that

inertia and the disturbance are exactly known, thus selection of force as in (3.68) essentially

stands for desired acceleration controller. This structure is used very often in the literature

related to application of disturbance compensation by a disturbance observer. Since that is the

inner loop of the system, small errors can be compensated by the outer loop. In some cases

the control goal is defined in terms of acceleration only and then such an open loop structure

must be very carefully implemented to minimize errors.

To complete this discussion on acceleration control let us look at the possibility to design a

closed loop controller that will enforce a desired acceleration in a system (3.1). The desired

acceleration is derived from (3.109) or some other system requirements or the system task is

specified in acceleration dimension as €qdesq . Assume acceleration is measured. Let us introduce

a new variable as in (3.110)

_s €qð€q; €qdesÞ ¼ €q� €qdesq ð3:110Þ

Now, acceleration control is specified in a similar way as tasks (3.6)–(3.9) and the methods

discussed in previous sections can be directly applied. The asymptotic convergence

s €qð€q; €qdesÞ! 0 is enforced if control input is selected as

τ ¼ τd þ a€qdesq � aks €q ¼ τeq � aks€q ð3:111Þ

Thus, after transient _s €qð€q; €qdesÞþ ks €qð€q; €qdesÞ ¼ 0, the equivalent force enforces motion

€q ¼ €qdesq . This is the same result as in an open loop acceleration controller (3.107). The

difference is in the closed loop and the need for acceleration measurement. Open loop

realization is in many cases easier to implement.

3.4 Trajectory Tracking

In order to demonstrate application of the design method discussed so far, let us turn to one

of the most often explored issues in motion control systems, the trajectory tracking problem.


We would like to demonstrate the similarities and the differences in the control system

structures without discussing details related to implementation.

The design in the acceleration control framework can be split into the following steps:

. Specification of the control output, the reference, the requirements and desired closed loop

dynamics.. Evaluation of the equivalent acceleration and the convergence accelerations as part of the

desired acceleration.. Selection of control input to enforce the desired acceleration. In this step the domain

of attractiveness and stability must be evaluated by taking into account the control

resources.

The first issue is to decide the control goal. For trajectory tracking let us require the output qðtÞtracks smooth bounded reference qref ðtÞwith smooth bounded first and second time derivatives

_qref ðtÞ and €qref ðtÞ respectively.Plant dynamics are defined as in Equation (3.1). Let us require the convergence to zero

tracking error to be exponential without overshoot with the poles of the closed loop be

�m1; �m2 � c0 < 0. The control input is bounded – thus τj j � M > 0. The reference and the

initial conditions are selected consistent with control bounds, thus the control input needed to

maintain equilibrium solution is within available resources. Assume the position, velocity and

disturbance to be known.

The control requirement could be defined as in (3.6) with yðqÞ ¼ q and the control error

eðqÞ ¼ q� qref . A second derivative of the control error gives €eðqÞ ¼ €q� €qref , thus the

acceleration that will maintain €eðqÞ ¼ 0 is €q ¼ €qref .Let us first find the desired acceleration. The plant dynamics with the control input €qdes are

defined as the double integrator €q ¼ €qdes. In order to guarantee the stability of equilibrium

eðqÞ ¼ 0 the desired acceleration must depend on the error and its derivative. The simplest and

most widely used solution is

€qdes ¼ €qref|{z}equivalent

acceleration

� ½KD _eðqÞþKPeðqÞ�|fflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflffl}convergence

acceleration

ð3:112Þ

The closed loop transient is

€eðqÞþKD _eðqÞþKPeðqÞ ¼ 0 ð3:113Þ

Selection of the controller gains as KD ¼ ðm1 þm2Þ > 0 and KP ¼ m1m2 > 0 guarantees

stability and satisfies design specification. The error asymptotically converges to zero. Note

that, in general, m1 and m2 may be complex if the specification allows. The structure of the

control is shown in Figure 3.20. The structure is easy for implementation. It is essentially a

proportional-derivative controller with the disturbance and reference acceleration as feedfor-

ward terms.

Another design can be implemented by introducing a new variable s ¼ c3 _eðqÞþ c2eðqÞ.Then, by selecting the Lyapunov function as in (3.50) and its derivative as in (3.55), the


structures discussed so far can be directly applied. The equivalent acceleration enforcing zero

rate of change _s ¼ 0 is

_sj€q¼€qeq¼ c3€eðqÞþ c2 _eðqÞ ¼ 0 ) €qeq ¼ €qref � c2

c3_eðqÞ ð3:114Þ

The system specification requires asymptotic convergence. That can be guaranteed if

convergence acceleration is selected as in Equation (3.65). Let the convergence acceleration be

€qcon ¼ � ks. Then, the closed loop dynamics are _sþ ks ¼ 0. The desired acceleration, as

a sum of the equivalent and convergence accelerations, can be now expressed as

€qdes ¼ €qeq þ €qcon ¼ €qeq � ks

¼ €qref ��kc3 þ c2

c3

�_eðqÞ� kc2eðqÞ ð3:115Þ

The desired acceleration (3.115) has the same structure as desired acceleration in (3.112).

In order to have the same closed loop transient parameters in (3.115) must be selected as

k ¼ c� 13 m1 and c2c

� 13 ¼ m2. The difference between controls (3.112) and (3.115) is in the

distribution of the components between the equivalent and the convergence accelerations.

In (3.115) the term c2c� 13 eðqÞ ¼ m2 _eðqÞ is attributed to the equivalent acceleration instead of

the convergence acceleration as in (3.115). For the same gains the closed loop dynamics

enforced by both solutions are the same. The equivalent acceleration will enforce the

equilibrium solution and theoretically the convergence acceleration is zero at equilibrium.

Another difference is in the roots of a characteristic equation. Solution (3.115) is valid only

for real m1 > 0 and m2 > 0 while solution (3.112), depending on the system specification, can

result in complex roots. Ifm1 >> m2 then closed loopmotion is mostly defined by the dynamics

determined by m2 (analysis can be easily performed using methods of singularly perturbed

systems).

For a selected desired acceleration the force input is

τ ¼ satfτdis þ an€qdesg ¼ sat

�τdis þ an

��€qref � c2

c3_e

� kc3

�_eþ c2

c3e

�

τ ¼ satfτdis þ an€qdesg ¼ satfτdis þ an½€qref �ðKD _eþKPeÞ�g

ð3:116Þ

Figure 3.20 Cascade structure of a trajectory tracking system with an inner acceleration control loop


For given bounds of disturbance and desired acceleration bound of control can be

determined as

minðτÞ � minðτdis þ an€qdesÞ

maxðτÞ � maxðτdis þ an€qdesÞ

ð3:117Þ

If the actuator is known then the bounds for input force and disturbance are known, thus the

achievable desired acceleration can be determined. From (3.117), it is obvious that a bound

on the control input, for a given disturbance, limits the achievable acceleration, thus limits the

convergence rate and the admissible dynamics of the closed loop. Selection of fast convergence

may result in prolonged oscillations.

The structure of a trajectory tracking system with control (3.115) is shown in Figure 3.21.

With the selection of parameters as k ¼ c� 13 m1, c2c

� 13 ¼ m2 for the system shown in

Figure 3.21 andKP ¼ m1m2,KD ¼ m1 þm2 for the system shown in Figure 3.20, the dynamics

and behavior of the two realizations are the same. This can be easily verified by manipulation

of the blocks and their arrangements, as shown in Figure 3.22.

Selection of the desired acceleration as in (3.112) does not allow us to design a finite-time

convergence of the generalized errorsðe; _eÞ. If finite-time convergence of the generalized error

sðe; _eÞ is required the design should follow the steps discussed in Section 3.3.2. Recall

conditions for the finite-time convergence with the continuous control input_V ¼ � 2a kVa;1=2 < a < 1. The reaching time tr from the initial valueVð0Þ 6¼ 0 is determined

by tr ¼ V1�að0Þ=2akð1�aÞ. The convergence acceleration

Figure 3.21 Structure of a trajectory tracking system with an acceleration control framework

Figure 3.22 Common realization of the systems depicted in Figures 3.20 and 3.21


€qcon ¼ � k sj j2a� 1signðsÞ; 1

2< a < 1 ð3:118Þ

enforces motion _sþ k sj j2a� 1sign ðsÞ ¼ 0 if the desired acceleration and consequently the

control input force are selected as

€qdes ¼ sat½€qeq � kjsj2a� 1sign ðsÞ�;

τ ¼ sat½τeq � ankjsj2a� 1sign ðsÞ�

ð3:119Þ

For the desired reaching time and convergence rate, the parameter k > 0 should be selected

from tr ¼ V1�að0Þ=2akð1�aÞ. The control (3.119) enforces closed loop dynamics

sðq; _qÞ ¼ 0 ) c3 _eðqÞþ c2eðqÞ ¼ 0; t � tr ð3:120Þ

The control error exponentially decays to equilibrium eðqÞ ¼ 0.

The desired acceleration can be determined knowing the reference and the control error.

The input enforcing the desired acceleration needs information on system disturbance and

inertia. The structure of the position tracking controller with finite-time convergence of

generalized error s ¼ c3 _eðqÞþ c2eðqÞ is shown in Figure 3.23.

Example 3.7 Position Control – Exponential and Finite-Time Convergence In

Examples 3.3–3.6 position output has been evaluated just for the purpose of confirming that

it converges to the solution yðq; _qÞ ¼ yref ðtÞ. In this example, our primary goal is to enforce

convergence of the position to the reference trajectory. This will require formulation of the

reference output in terms of the reference position yref ðqref Þ.In order to demonstrate the behavior, system (3.1) is simulated for the output ylðq; _qÞ ¼

c2qþ c3 _q, c2 ¼ 25, c3 ¼ 1. Plant model and the parameters are the same as in Example 3.6.

The initial conditions are qð0Þ ¼ 0:025 rad, _qð0Þ ¼ 0 rad=s, nominal inertia is an ¼ 0:1 kg �m2

the convergence gain is k ¼ 100. The limits on the input torque is τmaxj j ¼ 100 N:m, and the

convergence acceleration €qconmax

¼ 150 rad=s2. The convergence coefficients are selected

a ¼ 3=4 (in Figure 3.24) and a ¼ 1=2 (in Figure 3.25).

The reference position is selected as

qref ðtÞ ¼ 0:1½1þ 0:25 sin ð12:56tÞ� rad ð3:121Þ

Figure 3.23 Position control system with finite-time convergence for 0:5 � a < 1


The output and its reference are selected as

yðq; _qÞ ¼ c2qþ c3 _q ¼ 25qþ _q

yref ðqref ; _qref Þ ¼ c2qref þ c3 _q

ref ¼ 25qref þ _qrefð3:122Þ

For selected output the control error becomes

ey ¼ yðq; _qÞ� yref ðqref ; _qref Þ¼ 25ðq� qref Þþ ð _q� _qref Þ ¼ 25eq þ _eq

ð3:123Þ

The desired acceleration and the control torque are

€qdes ¼ €qeq þ €qcon

τ ¼ τdis þ an€qdes

ð3:124Þ

Figure 3.24 Changes in reference yref , output y, output error ey, _eyðeyÞ diagram, output yl ¼ 25qþ _q,reference yref ¼ 25qref þ _qref , qref ¼ 0:1½1þ 0:25 sin ð12:56tÞ� rad as position reference, position q,

reference position qref , position error eq and _eqðeqÞ diagram. The convergence gain is k ¼ 100 and

a ¼ 0:75


The equivalent acceleration is

€qeq ¼ _yref � 25 _q ð3:125Þ

For finite-time convergence a ¼ 0:75 the convergence acceleration is given by

€qcon ¼ � 100 ey 0:5sign ðeyÞ ð3:126Þ

The closed loop motion of the system is expected to satisfy one of the following constraints

_ey þ 100 ey 0:5sign ðeyÞ ¼ 0

For a ¼ 0:5 convergence acceleration reduces to

€qcon ¼ � 100 sign ðeyÞ

sign ðeyÞ ¼1 for ey > 0

� 1 for ey < 0

� ð3:127Þ

Figure 3.25 Changes in reference yref , output y, output error ey, _eyðeyÞ diagram for output yl ¼ 25qþ _q,its reference yref ¼ 25qref þ _qref , qref ¼ 0:1½1þ 0:25 sin ð12:56tÞ� rad as position reference, position q,

reference position qref , position error eq and _eqðeqÞ diagram. The convergence gain is k ¼ 100, a ¼ 0:5and convergence acceleration as in (3.128).


Convergence acceleration (3.127) undergoes discontinuity while enforcing condition

ey _ey < 0. For control €qdes ¼ €qeq þ €qcon after ey ¼ 0 is reached the control is equal to €qeq,thus convergence acceleration should diminish to zero. Since (3.127) cannot have an

instantaneous zero value the resulting motion is characterized by high frequency switching

and average ~€qcon ¼ 0 on manifold ey ¼ 0. To avoid large amplitude of the discontinuous input

the control (3.127) can be modified by effectively making combination of the exponential

convergence and finite-time convergence. This gives control input

€qcon ¼ � kc� 11 ðjeyj �mÞsign ðeyÞ; m > 0 ð3:128Þ

In (3.128) m can be selected small in order to avoid excessive amplitude of the switching

component of the control input. This solution guarantees finite-time convergence to

and consequently a sliding mode motion in ey ¼ 0.

In Figure 3.24, the left column shows the output reference yref , actual output y, control error

ey and _eyðeyÞ diagram. In the right column, the position reference qref and its actual value q,

the position control eq error and the _eqðeqÞ diagrams are shown with a ¼ 3=4 and k ¼ 100.

The same diagrams for a ¼ 1=2 are shown in Figure 3.25. The peculiar property of systems

with sliding mode is clearly shown in the _eqðeqÞ diagram. The motion towards trajectory

25eq þ _eq ¼ 0 is short and then motion is tracking the straight line _eq ¼ � 25eq.

References

1. Ohishi, K., Ohnishi, K., and Miyachi, K. (1983) Torque-speed regulation of dc motor based on load torque

estimation. IEEJ International Power Electronics Conference, Tokyo 2, pp. 1209–1216.

2. Ohnishi, K., Shibata, M., and Murakami, T. (1996) Motion control for advanced mechatronics, Mechatronics.

IEEE/ASME Transactions on Mechatronics 1(1), 56–67.

3. Salisbury, J.K. (1980) Active stiffness control of a manipulator in cartesian coordinates. Proceedings of the 19th

IEEE Conference on Decision and Control, pp. 95–100.

4. Utkin, V., Guldner, J., and Shi, J. (1999) Sliding Mode Control in Electromechanical Systems, Taylor & Francis,

London.

5. Isermann, R. (2003) Mechatronics Systems Fundamentals, Springer.


Further Reading

Katsura, S. (2004)AdvancedMotionControl Based onQuarry of Environmental Information, Ph.D.Dissertation, Keio

University, Japan.

Katsura, S.,Matsumoto,Y., andOhnishi, K. (2005)Realization of “Lawof action and reaction” bymultilateral control,

Industrial Electronics. IEEE Transactions on Industrial Electronics, 52(5), 1196–1205.€Onal, C.D. (2005) Bilateral Control – A SlidingMode Control Approach, M. Sc. Thesis, Sabanci University, Istanbul,

Turkey.

Sabanovic, A. (2007) SMC framework in motion control systems. International Journal of Adaptive Control and

Signal Processing, 21(8/9), 731–744.

Shimono, T., Katsura, S., and Ohnishi, K. (2005) Improvement of operationality for bilateral control based on nominal

mass design in disturbance observer. IECON 2005, 32nd Annual Conference of the IEEE, vol. 1, pp. 6–10.

Tsuji, T. (2005) Motion Control for Adaptation to Human Environment, PhD Thesis, Keio University, Yokohama,

Japan.

Tsuji, T. and Ohnishi, K. (2004) Position/force scaling of function-based bilateral control system. Industrial

Technology, IEEE International Conference on Industrial Electronics, vol. 1, pp. 96–101.

Tsuji, T., Ohnishi, K., and Sabanovic, A. (2007)A controller designmethod based on functionality. IEEETransactions

on Industrial Electronics, 54(6), 3335–3343.


4

Disturbance Observers

In this chapter we will focus on the design of disturbance observers and their application in the

realization of the control structures discussed so far. There are many possibilities to approach

issues of disturbance estimation. Our intention is not to exhaust all possibilities but rather, by

discussing some, to show the basic features of the problem and point to the most often used

solutions. The emphasis is on salient properties and application issues. In our view under-

standing the role of the disturbance observer and the changes in the dynamics of the plant when

disturbance observers are applied is one of the central issues in motion control. The design and

analysis are based on a single degree of freedom system (3.1).

As a starting point in the disturbance observer design, wewill assume that the disturbance is

bounded and can be modeled as output of known dynamical system with unknown initial

conditions. A further assumption is that the plant coordinates ðq; _qÞ are measured, the

parameter aðqÞ or its nominal value an and the plant control input t are known. The design

and implementation of the acceleration control in systems with disturbance observer will be

discussed later in this chapter. That will allow a comparison of the results obtained in Chapter 3

for an ideal system with more realistic solutions based on the application of estimated

disturbance and state variables and outputs.

For plant (3.1) the disturbance td is an algebraic sum of different forces. At system input all

these forces act as a scalar quantity, thus summary action of all components and not necessarily

each component separately needs to be estimated. If a particular component of the disturbance

is of interest, then the structure of the estimation should be adopted for that particular case.

The structure of an observer suitable for the purpose of being used in acceleration control

implementation should be applicable for estimation of either disturbance force td

td ¼ bðq; _qÞþ gðqÞþ text¼ t� aðqÞ€q ð4:1Þ



or generalized disturbance force tdis

tdis ¼ bðq; _qÞþ gðqÞþ text þDa€q¼ t� an€q

ð4:2Þ

Synthesis of the disturbance observer involves the design of a stable linear or nonlinear

dynamic system that will, by taking measurements from the plant as inputs, generate an output

signal linear in the disturbance td or generalized disturbance tdis. The output of the observer, atleast in some range of frequencies, must be close to the real disturbance. From (4.1) and (4.2) it

follows that, for calculating the disturbance the input force, the acceleration and system inertia

must be known. The exact measurement of any of these variables and parameters is unlikely,

thus a direct calculation of the disturbance from (4.1) or (4.2) is not feasible. The direct

acceleration measurement is complicated (and expensive). This results in the application of

linear or nonlinear dynamic structures to circumvent the acceleration measurement and use

position and/or velocity as the measurement variables in the disturbance estimation. Then

the estimated disturbance is the output of a dynamic structure td ¼ Qdðq; _q; a; tÞtd . Similarly

the estimated generalized disturbance will be expressed as tdis ¼ Qdisðq; _q; an; tÞtdis. Here, tdstands for the estimated value of the disturbance, tdis stands for the estimation of the

generalized disturbance, Qdðq; _q; a; tÞ and Qdisðq; _q; an; tÞ stand for some linear or nonlinear

dynamics with position and/or velocity and the input force as inputs and the plant inertia or its

nominal value as the parameter. The dependence of the plant inertia from the positionmay lead

to nonlinear gains thus nonlinear dependence on the position. Such a structure does not reflect

the implementation issues. It rather shows that whatever particular implementation of the

disturbance observer is applied the observer output should be linear in disturbance. The

estimation error pd ¼ td � td or pdis ¼ tdis � tdis is then function of the properties of the

observer dynamics Qdðq; _q; a; tÞ or Qdisðq; _q; an; tÞ and the disturbance. In order to simplify

notation in further text here Q will be used instead of Qdðq; _q; a; tÞ and Qdisðq; _q; an; tÞwhenever it is clear from the context. In cases when the linear system methods of analysis and

design are used the plant inertia aðqÞ is assumed an unknown constant, the disturbance td is

treated as an external signal. Then the plant could be represented by the transfer function

P ¼ 1/ as2 with inputs t and td .As discussed inChapter 2, in order to estimate system state or disturbance one has to know at

least some properties of the system and the disturbance. A natural assumption is that some

output and input variables are measured and the structure of the system is known. In addition at

least some characteristics of the disturbance are assumed to be known. Prior information on the

disturbancewould suggest properties of the structure of the system that can generate the model

which best represents the disturbance. Here, we will assume that the disturbance can be

modeled by a polynomial of time, thus it can be mathematically modeled as the output of

a linear system with unknown initial conditions. For example if the disturbance is assumed to

be an unknown constant qðtÞ ¼ td ¼ d then it can be modeled by a first-order system_qðtÞ ¼ 0; qð0Þ ¼ d with output function td ¼ qðtÞ. Similarly, disturbance as a first-order

polynomial can be modeled as

d2qðtÞdt2

¼ 0;qð0Þ ¼ d; _qð0Þ ¼ d1

td ¼ qðtÞ ¼ dþ d1t

)ð4:3Þ


In another representation it could be written as

d

dt

qq1

� �¼ 0 1

0 0

� �qq1

� �;

qð0Þq1ð0Þ

� �¼ d

d1

� �

td ¼ 1 0½ � qq1

� � ð4:4Þ

Similarly a disturbance represented as a ðm� 1Þ order polynomial can be modeled by

dmqðtÞdtm

¼ 0;qð0Þ ¼ d; _qð0Þ ¼ d1;qðm� 1Þð0Þ ¼ dm� 1ðm� 1Þ!

qðtÞ ¼ d þ d1tþ � � � þ dm� 1tm� 1

9>=>; ð4:5Þ

In the state space form (4.5) is written as

d

dt

q

. . .

qm� 1

264

375¼

0 1 0

. . . . . . . . .

0 0 0

264

375

q

. . .

qm� 1

264

375;

qð0Þ. . .

0ð Þ

264

375 ¼

d

. . .

dm� 1ðm� 1Þ!

264

375

td ¼ 1 0 0½ �q

. . .

qðm� 1Þ

264

375

ð4:6Þ

If disturbance has some other structure, model generating such a signal needs to be applied.

For example if disturbance is represented by q ¼ c sin ðvtÞ then mathematical model

generating such a signal can be written as

d

dt

qq1

� �¼ 0 v

�v 0

� �qq1

� �;

qð0Þq1ð0Þ

� �¼ 0

d

� �

td ¼ 1 0½ � qq1

� � ð4:7Þ

The same model with a different output function may simulate a disturbance

q ¼ c sin ðvtÞþ cv cos ðvtÞ.In general a disturbance can be described as an output of a linear system

dq

dt¼ Anq; td ¼ Cqq; qð0Þ ¼ d ð4:8Þ

Structure of matrices Aq and Cq determine the shape, and the initial conditions qð0Þ ¼ d

determine the amplitude of signal qðtÞ generated by system (4.8). As shown in Chapter 2,

augmented plant-disturbance model allows estimation of the augmented state vector and thus

estimation of the disturbance. The order of observermay be lowered for the number of available

measurements.

The disturbance observer is a robust compensator, proposed by K. Ohnishi, and is widely

used in real applications.Manyworks are based on state observer design for augmented system

Disturbance Observers 117

in which disturbances are taken as states that append to the system. In designing the dynamic

structure that will estimate input disturbance many different methods can be applied. Some are

already discussed in Chapter 2. Each application of the model reference adaptive control [1]

requires extensive calculations. The Kalman filter based approaches [2] are used to reject

disturbances and noise. Variable structure disturbance observers and high gain observers are

designed based on this method [3,4].

4.1 Disturbance Model Based Observers

We will begin with the simple problem of estimating a bounded disturbance modeled as the

output of a first-order linear system. The disturbance input to plant (3.1) changes slowly relative

to the state variables of the system, thus it can be modeled as _qðtÞ ¼ _tdðtÞ ¼ 0. Assume the

plant inertia coefficient aðqÞ is known. This augmented system includes the model of the plant

and the disturbance model

_q ¼ v

aðqÞ _v ¼ t� td

�Y

_q_v

� �¼ 0 1

0 0

� �|fflfflfflffl{zfflfflfflffl}

A

q

v

� �þ 0

a� 1

� �|fflfflfflffl{zfflfflfflffl}

B

ðt� tdÞ

_q ¼ 0|{z}Aqq

; ð4:9Þ

td ¼ q

System (4.9) can be written as

_q

_v

_q

2664

3775 ¼

0 1 0

0 0 � a� 1

0 0 0

2664

3775

q

v

q

2664

3775þ

0

a� 1

0

2664

3775t

_z ¼A �B

0 Aq

" #zþ

B

0

" #t; z ¼ q v q½ �T

td ¼ 0 0 1½ �z

ð4:10Þ

The augmented plant-disturbance system (4.10) is third-order dynamics with state vector

z ¼ q v q½ �T . If the velocity is measured the system output augmented plant-disturbance

system becomes

_v ¼ 0|{z}A

vþ a� 1|{z}B

ðt� tdÞ

_v_q

� �¼ 0 � a� 1

0 0

� �v

q

� �þ a� 1

0

� �t

_z ¼ A �B

0 Aq

� �zþ B

0

� �t

z ¼ v q½ �T

td ¼ 0 1½ �z

ð4:11Þ


With the introduction of a more complex model of the disturbance like in (4.8), the augmented

system (4.10) can be represented by

_z ¼ A �B

0 Aq

" #zþ B

0

" #t ¼ AzzþBt

z ¼ q v q . . . . . .qðm� 1Þh iT

td ¼ Cz

ð4:12Þ

The selection of measurements and the model of disturbance are such that augmented plant-

disturbance model dynamics are observable.

4.1.1 Velocity Based Disturbance Observer

Design of disturbance observer for system (4.9) is based on the transformation of variables in

which intermediate variable z is selected as a linear combination of the unknown disturbance

q ¼ td and the measured output v

z ¼ qþ lv; l ¼ const > 0 ð4:13Þ

The dynamics of intermediate variable z ¼ qþ lv on the trajectories of augmented system (4.9)

can be written as

_z ¼ _qþ l _v ¼ l

aðt� zþ lvÞ ð4:14Þ

Since the inertia of system is assumed known and the pair ðt; vÞ is measured, the observer for

intermediate variable z can be designed by taking the same structure as in (4.14) and the

estimated disturbance can be determined from (4.13)

_z ¼ l

aðt� zþ lvÞ; l > 0

q ¼ z� lv

Structure of the disturbance observer (4.15) is depicted in Figure 4.1(a). Plugging (4.15) into_q ¼ _z� l _v yields

_q ¼ l

aðqÞ t�l

aðqÞ ðqþ l _qÞþ l2

aðqÞ _q� l€q

_qþ l

aðqÞ q ¼ l

aðqÞ t� aðqÞ€q½ � ¼ l

aðqÞ td

q ¼ td

Estimated disturbance is q output of the nonlinear first-order filter (4.16).With assumption that

disturbance is modeled by _t ¼ 0 the estimation error can be expressed as

ð4:16Þ

ð4:15Þ


_t� _q ¼ l

aðqÞ ðt� qÞ; e ¼ t� q

_eþ l

aðqÞ e ¼ 0

Since aðqÞ > 0 is strictly positive then, by selecting l > 0, the estimation error tends to zero as

t!¥ and consequently td ¼ q! td . The convergence rate depends on plant inertia.

Selecting l ¼ ga qð Þ > 0 allows rewrite structure (4.16) as_qþ gq ¼ g t� a qð Þ€qð Þ ¼ gtd

and by taking Laplace transformation we obtain:

sþ gð Þq ¼ gtd

q ¼ td ¼ g

sþ gtd ¼ Q sð Þtd

ð4:18Þ

Observer (4.16) provides estimation of the disturbance q ¼ td ¼ Q sð Þtd modified by simple

first order filter. If one requires constant bandwidth of the low pass filter the gain of the observer

should be selected as l ¼ ga qð Þ. If the system inertia is not known, than this requirement cannot

be met with selection of the observer as in (4.15).

In writing the plant model as in (3.4), inertia is assumed to be an unknown bounded function

aðqÞ ¼ an þDaðqÞ, where an > 0 stands for so-called nominal value and DaðqÞ stands for

bounded variation of plant inertia. Since only an is known, in actual implementation of

observer (4.15), the nominal value an instead of the exact value aðqÞ can be used. In that case,structure of intermediate variable z as in (4.15) can be applied by replacing plant inertia by its

nominal value to obtain

_z ¼ l

anðt� zþ lvÞ; l > 0

q ¼ z� lv

By selecting observer gain l ¼ gan, inserting (4.19) and (3.4) into_q ¼ _z� l _v the output

q ¼ tdis becomes

_q ¼ l

ant� l

anðqþ l _qÞþ l2

an_q� l€q

_qþ gq ¼ g t� an€qð Þ ¼ gtdis

q ¼ tdis

The topology of the generalized disturbance observer is shown in Figure 4.1(b). There is

no topological difference in comparison with the structure shown in Figure 4.1(a), just a

change in the observer gain. From Equation (4.20) it appears that the structure of an observer

estimating the generalized disturbance q ¼ tdis is the same as the one shown in Equation

(4.16). The difference is only in the dependence of the observer gain on the plant inertia [in

(4.16)] or on the nominal plant inertia [in (4.20)].

ð4:17Þ

ð4:20Þ

ð4:19Þ


Now we can in more details look at the topological properties of the disturbance observers

discussed above. In both Equations (4.16) and (4.20) the right hand side appears to be

proportional to the actual disturbance expressed as a difference between plant input t and theoutput of the inverse plant P� 1 ¼ a qð Þ€q or nominal plant P� 1

n ¼ an€q. This can be easily

verified from plant description shown in Equations (3.1) or (3.4). It appears that structures

shown in Figures 4.1 (a) and (b) in a certain way determine plant acceleration.

In systems with variable plant inertia, observer (4.15) is having variable gain l=a qð Þ. Incontrast, the observer described by Equation (4.19) is having constant parameters. By selecting

gain l ¼ ga qð Þ in (4.15) or l ¼ gan in (4.19) the dynamics of both observers is the same and is

defined by first order filter Q ¼ g= sþ gð Þ. The general representation of the disturbance

observer (4.20) is depicted in Figure 4.2(a). The observer (4.16) can be shown in the same

way just by changing the structure of the inverse plant.

With filter Q ¼ g= sþ gð Þ, g ¼ const the disturbance observer (4.20) can be rearranged

into

tdis ¼ Qðt� ansvÞ ¼ Qt� angð1�QÞvtdis ¼ Qðtþ vangÞ� vang;

ð4:21Þ

Implementation of the disturbance observer given by (4.21) is shown in Figure 4.2(b). In

applying a Laplace transformation on the plant (3.1) the inertia is assumed to be an unknown

constant and the disturbance input is represented by signal td whose components are shown in

figures just for the illustration of its composition.

The disturbance observer shown in Figure 4.2(b) is very widely used in motion

control systems literature [5–7]. It is very simple for implementation and yet a very versatile

structure.

4.1.2 Position Based Disturbance Observer

So far, in constructing the disturbance observer the velocity and the input force were assumed

measured. This section discusses disturbance observer design with the pair ðt; qÞ assumed

measured and the disturbance model _q ¼ _tdis ¼ 0. Since the position is measured the reduced

Figure 4.1 Disturbance observer for: (a) a plant with known parameters and (b) a plant with nominal

parameters


order observer should be constructed to estimate velocity v as well. The dynamics of system

(3.4) along with the disturbance model can be expressed as

_q ¼ v

_v ¼ t� tdisan

¼ t* þq

_q ¼ 0

tdis ¼ � anq

t* ¼ a� 1n t

tdis ¼ antdaðqÞ �

taðqÞ �

tan

24

35

8<:

9=; ¼ td þDa€q

ð4:22Þ

The scaling t* ¼ a� 1n t is used just for more compact writing. The disturbanceq is assumed

to be an unknown constant and a disturbance model is added to the system description

in (4.22). The augmented system dynamics (4.22) with pair ðt; qÞ as inputs and q as output is

observable.

Figure 4.2 Implementation of a velocity based disturbance observer: (a) general representation with

filter Q and (b) realization with first-order filter Q ¼ g =ðsþ gÞ


The intermediate variables should be specified to allow estimation of the velocity and the

disturbance. They can be selected as

z1 ¼ q� l1q; q ¼ � a� 1n tdis; l1 ¼ const

z2 ¼ v� l2q; l2 ¼ constð4:23Þ

The dynamics of these intermediate variables on the trajectories of system (4.22) are

_z1 ¼ � l1ðz2 þ l2qÞ

_z2 ¼ z1 � l2z2 þðl1 � l22Þqþ t*

t* ¼ a� 1n t

ð4:24Þ

The observer should have the same dynamics as the intermediate variables (4.24), thus it can

be written in the following form

_z1 ¼ � l1ðz2 þ l2qÞ

_z2 ¼ z1 � l2z2 þðl1 � l22Þqþ t* ð4:25Þ

Taking Laplace transformation of (4.25) and solving for z1 yields

z1 ¼ � l2vþ l1qþ t*

s2 þ l2sþ l1l1 ð4:26Þ

Plugging (4.26) into q ¼ z1 þ l1q yields the disturbance estimation as

q ¼ � a� 1n t� s2q

s2 þ l2sþ l1l1 ¼ � l1

s2 þ l2sþ l1

t� ans2q

an

q ¼ � tdisan

¼ � l1

s2 þ l2sþ l1

tdisan

tdis ¼ � anq ¼ l1

s2 þ l2sþ l1tdis

ð4:27Þ

The selection of gains l1; l2 determines the desired bandwidth of the observer. From (4.23)

and (4.25) the velocity estimation is v ¼ z2 þ l2q

v ¼ l1

s2 þ l2sþ l1v ð4:28Þ

The structures of the disturbance observer as described in (4.25) is shown in Figure 4.3.

More complex disturbancemodels can be treated in the same framework.General solution is

given in Chapter 2. Here we would like to show procedure for the system (4.24) with

disturbance model €q ¼ €tdis ¼ 0. Let measurement be the input force and the velocity ðt; vÞ


and let the nominal system inertia is known. The augmented system can be written as

_v ¼ t� tdisan

¼ t* þq

_q ¼ q1

_q1 ¼ 0

t* ¼ a� 1n t

tdis ¼ � anq

ð4:29Þ

Let the intermediate variables be

z1 ¼ q1 � l1v; l1 ¼ const

z2 ¼ q� l2v; l2 ¼ constð4:30Þ

The observer dynamics can be written in the following form

_z1 ¼ � l1ðz2 þ l2vþ t*Þ_z2 ¼ z1 � l2z2 þðl1 � l22Þv� l2t* ð4:31Þ

Taking a Laplace transformation of (4.31) and solving it for z2 yields

z2 ¼ ðl1 � l22Þsþ l1l2

s2 þ l2sþ l1v� l2sþ l1

s2 þ l2sþ l1t*; ð4:32Þ

The estimation of disturbance q ¼ z2 þ l2v can be expressed as

q¼ l2sþ l1

s2 þ l2sþ l1ðsv� t*Þ|fflfflfflfflffl{zfflfflfflfflffl}

q

¼ � l2sþ l1

s2 þ l2sþ l1

tdisan

tdis¼ � anq ¼ l2sþ l1

s2 þ l2sþ l1tdis ð4:33Þ

The structure of the disturbance observer (4.31) is shown in Figure 4.4. It is very similar to

the one shown in Figure 4.3. The difference is in the inputs and assumed model of the

disturbance.

Figure 4.3 Disturbance observer with position and force as inputs


Topologies of observers discussed in this section are based on the known structure of the

control plant and the assumed dynamics of linear system which generates the disturbance

signal. In addition to measured inputs it requires data on the parameters of the plant. In 1-dof

motion control systems the input disturbance may include the effects of variation of the system

inertia, thus exact information on system inertia may not be required. This allows usage of a so-

called nominal plant as the basis for the disturbance observer design.

Example 4.1 Comparison of Disturbance Observers (4.16) and (4.24) The goal of the

examples in this chapter is to illustrate the disturbance observer design and application. Along

with this realization of the acceleration controller and investigation properties of system with

disturbance observer feedback is illustrated.

For easy comparison with results shown in examples in Chapter 3 plant and all parameters,

reference and initial conditions are selected the same as in Example 3.1.

In the examples in Chapter 4 the rotational motion is examined and the plant has structure as

in (3.26) an€qþDa€qþ bðq; _qÞþ gðqÞþ text ¼ t. The control will be either with exponential (asshown in Example 3.3) or finite-time convergence (as shown in Example 3.5).

The external force is modeled as td ¼ bðq; _qÞþ gðqÞþ text with:

. nonlinear forces bðq; _qÞ ¼ b0qþ b1 _qþ ana1 _q cos ðqÞ, in N �m,

. nonlinear force gðqÞ ¼ g0q2, in N �m,

. external force text ¼ t0½1þ cos ðvttÞþ sin ð3vttÞ�; 0:12 < t < 0:8s0 elsewhere

N �m�

In this example, plant inertia is assumed constant and equal to its nominal value

a ¼ an ¼ 0:1kg �m2. Two designs are compared, one based on the velocity measurements

ðt; _qÞ and the second based on the positionmeasurements ðt; qÞ. Our goal is not to show the best

performance but rather, to illustrate the structures and show a possible simple implementation.

The disturbance observer with input force and velocity ðt; _qÞ measurements is realized as

dz

dt¼ gðtþ gan _q� zÞ

td ¼ z� gan _q

ð4:34Þ

Here g > 0 is the filter gain and z is just intermediate variable. The disturbance observer

based on input force and position ðt; qÞ measurements is realized as

Figure 4.4 Disturbance observer with velocity measurement and model of disturbance €q ¼ 0


dz1

dt¼� l1ðz2 � l2qÞ

dz2

dt¼ z1 � l2z2 þðl1 � l22Þqþ a� 1

n t

td ¼� anðz1 þ lnqÞ

ð4:35Þ

The parameters of observer (4.35) are selected as l1 ¼ g2 and l2 ¼ 2g. This way the

comparison of the observersmay bemoremeaningful. The simulations for g ¼ 600 are plotted.

The selection of the filter gain is not tuned for the best performance but rather to illustrate

properties of the disturbance observer.

The closed loop control with a ¼ 0:80; k ¼ 50, initial conditions in position

qð0Þ ¼ 0:15 rad, the output reference yref ¼ ½1þ 3:5 sin ð12:56tÞ� and the output

ylðq; _qÞ ¼ 50qþ 5 _q are simulated.

In Figure 4.5, in the first row the reference yref , the output y and the system disturbance td areshown. In the second row the disturbance estimation error pðQ; tdÞ and the disturbance,

Figure 4.5 Changes in reference output yref , output y, disturbance td , disturbance estimation error

pðQ; tdÞ and estimated disturbance td for observer (4.34), in left column, and observer (4.35), in right

column, with g ¼ 600. The output y ¼ 50qþ 5 _q is controlled to track the reference

yref ¼ 1þ 3:5 sin ð12:56tÞ and the controller parameters are k ¼ 50 and a ¼ 0:8


estimated by the observer (4.34), td are depicted. In the third row, the estimation error pðQ; tdÞand the output of the observer (4.35) are shown.

As expected the estimation error is almost two times higher that for the observer (4.34). The

highest errors are at the points of the discontinuity of the disturbance force.

4.2 Closed Loop Disturbance Observers

As shown inChapter 2, input disturbance observers can be designed by forcing the output of the

plant model to track the measured output of the real plant. Let the pair ðt; vÞ be measured and

nominal value of the inertia an be known. Let the velocity dynamics be modeled by

an _z ¼ t� u ð4:36ÞHere z stands for the output of the model and u stands for the model control input to be

selected such that z tracksmeasured plant output v. Fromplant dynamics (3.4) andmodel (4.36)

the dynamics of the tracking error ez ¼ z� v is obtained as

_ez ¼ _z� _v ¼ 1

anðtdis � uÞ ð4:37Þ

Selecting control u for system (4.37) is straight forward. The simplest way is to apply

proportional controller u ¼ lez ¼ lðz� vÞ; l > 0. Then (4.37) becomes

_ez þ l

anez ¼ 1

antdis ð4:38Þ

By inspection we can confirm that disturbance observer (4.16) and the dynamics of the

control error (4.38) are similar. Replacing u ¼ lez and l ¼ gan in (4.38) yields

du

dtþ gu ¼ gtdis

uavðtÞ ¼ tdis

ð4:39Þ

The average of the control output is equal to the estimated disturbance. As expected, for a

proportional controller, the tracking error is proportional to the disturbance and the average

value of the observer control input u is equal to the estimated disturbance. The filter dynamics

(for a selected controller gain) is the same as the one for observer (4.19). The structure of the

disturbance observer is shown in Figure 4.6.

One may use model (4.36) and design controller with finite-time convergence of the

observer. Selection of the Lyapunov function as V ¼ ðe2z=2Þ and its time derivative as_V ¼ ez _ez ¼ � l2aVa; l > 0 and 1=2 � a < 1 allows design of the control u such that the

finite-time convergence of the estimation error (4.37) is guaranteed. For a ¼ 1=2 control input

u ¼ lan sign ðezÞ; lan > tdisj j ð4:40Þ

enforces sliding mode in ez ¼ 0 and tracking error, after initial transient, will reach zero value

in finite time. From _ezðu ¼ ueqÞ ¼ 0 equivalent control is derived as

ueq ¼ tdis ð4:41Þ


Actual control (4.40) is switching between þ lan and � lan. As shown in Chapter 2, the

average control uav is equal to ueq. The average control can be obtained using low-pass filter,

thus the estimate of actual disturbance is obtained as

ueq ffi _uav ¼ gðu� uavÞuav ¼ tdis

ð4:42Þ

A low-pass filter should be selected to suppress oscillations due to the control discontinuity.

In general, a better result is obtained if a high order filter is selected.

For 1=2 < a < 1 the convergence term is proportional to the error and tends to zero as the

tracking error tends to zero. As shown in Chapter 3, this requires selecting the control input as

u ¼ ueq þ lan ezj j2a� 1sign ðezÞ; 1

2< a < 1 ð4:43Þ

Using the established relationship between the equivalent control and the average control,

algorithm (4.43) can be approximated as

u ¼ uav þ lan ezj j2a� 1signðezÞ; 1

2< a < 1 ð4:44Þ

Here the average control is the output of a low-pass filter as in (4.42). The selection of this

filter with a high bandwidth would improve estimation. Since control is continuous selecting,

a high bandwidth would not be a problem in realization. The presence of noise in the velocity

measurement is the limiting factor in the controller and filter design. The performance greatly

depends on the selected structure of the controller and low-pass filter implementation. More

details on sliding mode observers for electromechanical systems can be found in [3,4].

In the design of observers based on velocity tracking we did not assume any specific

dynamics for the disturbance. This makes such observers suitable for applications in which the

nature of the disturbance is not known in advance.

4.2.1 Internal and External Forces Observers

Observers discussed in previous sections estimate generalized input disturbance. The partic-

ular component of the disturbance cannot be separated as a direct output of these observers.

Figure 4.6 Topology of velocity based closed loop disturbance observer


Since the generalized disturbance is a linear combination of all terms the estimation of a

particular component would require a change in the observers’ input. Indeed, if the external

force is measured, then the augmented plant-observer dynamics (4.9) can be rearranged as

_q ¼ v

aðqÞ _v ¼ ðt� textÞ� td1 ¼ t1 � td1_q ¼ 0

bðq; _qÞþ gðqÞ ¼ td1

ð4:45Þ

The disturbance td1 consists of the internal forces bðq; _qÞþ gðqÞ ¼ td1. By taking the

difference of the input and the external forces and velocity as observer inputs the dynamics of

the intermediate variable z ¼ qþ lv; l ¼ const > 0 on the trajectories of system (4.45) can

be written as

_z ¼ _qþ l _v ¼ l

aðt1 � zþ lvÞ; l > 0 ð4:46Þ

The dynamics of the disturbance observer have the same structure as the dynamics of the

intermediate variable

_z ¼ l

aðt1 � zþ lvÞ; l > 0

q ¼ z� lv

Plugging (4.47) into_q ¼ _z� l _v yields

_q ¼ l

aðqÞ t1 �l

aðqÞ ðqþ l _qÞþ l2

aðqÞ _q� l€q

_qþ l

aðqÞ q ¼ l

aðqÞ t1 � aðqÞ€q½ �

q ¼ td1

The estimated disturbance q is the output of a filter as in (4.16). It is easy to show that the

estimation error has dynamics and convergence as observer (4.17).

Comparison of the observers (4.15) and (4.47) shows a difference only in observer input.

Instead of the control force t as in (4.15), the input in (4.47) is composed of measured forces

ðt� textÞ. The output of the observer now includes a linear combination of unknown forces.

Thus, the linear combination of known (or measured) components can be assigned as the input

to the observer and then the observer output will correspond to the linear combination of

unknown forces.

If, for example, the sum of internal forces ½bðq; _qÞþ gðqÞ� are known, structure (4.47) can beused for estimating the external force text just by replacing input t1 ¼ t� text by

t1 ¼ ½t� bðq; _qÞ� gðqÞ�. The structures for estimating the internal forces and the external

forces are shown in Figure 4.7(a) and (b), respectively.

ð4:47Þ

ð4:48Þ


The same structures can be used for the estimation of the plant acceleration just by plugging

the estimated disturbance into the plant model to obtain €q ¼ ðt� tdisÞ=an.As analysis shows, the observers discussed so far include filters in one way or another, thus

having the observer output smoothed. As a result, the discontinuous disturbances cannot be

estimated exactly. At the same time, disturbance observer-like structures can be used to

estimate a particular component of the overall input disturbance. This feature can be applied in

specially designed experiments for estimation of the plant parameters. Indeed, if for example

velocity is kept constant, then forces induced by acceleration will be zero and thus it will not

appear in the estimated disturbance.

Figure 4.7 Disturbance observer based internal force estimation (a) and external force estimation (b)


Example 4.2 Comparison of Disturbance Observers (4.15) and (4.36) This example

shows a comparison between the disturbance observer as in (4.16) and the closed loop

disturbance observer (4.36). The plant and the control loop parameters are the same as in

Example 4.1. The disturbance observer with ðt; _qÞ measurements and known nominal inertia

a ¼ an ¼ 0:1 kg �m2 is implemented as in Equation (4.34).

The closed loop observer with measurement ðt; _qÞ is designed with a finite-time

convergence

dz

dt¼ 1

anðt� uzÞ

uz ¼ 0:4g ð _q� zÞþ 0:5 sign ð _q� zÞdtddt

¼ 5g ðuz � tdÞ

ð4:49Þ

Figure 4.8 Changes in output and disturbance. Top row: reference yref , output y and disturbance td .Middle row: disturbance estimation error pðQ; tdÞ and estimated disturbance td for observer (4.34).

Bottom row: disturbance estimation error pðQ; tdÞ, estimated disturbance td for observer (4.49). The filtergain is g ¼ 600. The output controller parameters are k ¼ 50 and a ¼ 0:8


Here z is an intermediate variable. The discontinuous term in control is selected to be small

in order to allow a high bandwidth of the additional filter.

The plant output closed loop control is the same as in Example 4.1 [a ¼ 0:80; k ¼ 50,

initial conditions in position qð0Þ ¼ 0:15 rad], with output reference yref ðtÞ ¼ ½1þ 3:5 sinð12:56tÞ� and output yl ¼ 50qþ 5 _q. Simulations with g ¼ 600 are plotted.

The diagrams are composed in the sameway as in Example 4.1. Inertia is constant and equal

to its nominal value a ¼ an.

In Figure 4.8 in the first row the reference output yref , the output y and the systemdisturbance

td are shown. In the second row the disturbance estimation error pðQ; tdÞ and the disturbance tdestimated by the observer (4.34) are depicted. In the third row the estimation error pðQ; tdÞ andthe output td of the observer (4.49) are shown. As it can be observed, the error in the observer(4.49) is lower than for observer (4.34).

4.3 Observer for Plant with Actuator

In real systems an actuator is applied to generate a control force. Measurement of the force is

not easy – thus closure of the control loop based on direct measurement of the force is not very

practical. Because of this, in most actuators some other variable, related to force in a known

way, is controlled. The force is then determined as a function of a controlled variable. Most

commonly used actuators in motion control systems are electromechanical converters –

electrical machines – for which, as shown in Chapter 1, force depends on the actuator current

and, in general, can be expressed in the following form

t ¼ KTðq; iÞi; KT min < KTðq; iÞ ¼ Kn þDKTðq; iÞ < KT max ð4:50Þ

The actuator gain KT is assumed to be a smooth bounded function of time and system

coordinates. The nominal valueKn of the actuator gain is assumed known. Thevariation of gain

is assumed to be described by continuous function DKTðq; iÞ with known upper and lower

bounds, input current i is assumed continuous and bounded. The dependence of gain on current

may represent features like magnetic saturation in electrical machines or some other phe-

nomenon of the actuator. As a means of controlling the actuator force the current control loop

is employed. That introduces additional dynamics in the model of the plant with actuator. In

most actuators the control loop dynamics can be approximated by a first-order filter. Then, the

force generated by actuator can be expressed as a function of the reference current

di

dt¼ gcðiref � iÞ

t ¼ KTðq; iÞiKT min < KT ¼ Kn þDKTðq; iÞ < KT max

ð4:51Þ

Here gc is the filter gain and iref stands for the reference current. By taking a Laplace

transformation the actuator current can be expressed as i ¼ Wc iref where Wc stands for the

transfer function of the actuator current control loop [in (4.51) just a first-order filter].

The actuator current may or may not be available for measurement and for the disturbance

observer design, pair ðiref ; vÞ is assumed measured. The nominal value of inertia an is assumed

known. Two possibilities will be discussed: (i) a system with perfect tracking in the current


loop thus neglecting the dynamics of the current control loop and (ii) taking the dynamics of

the current loop into account.

4.3.1 Plant with Neglected Dynamics of Current Control Loop

In general iref is output of outer loop controller and is assumed known without dynamical

distortions. Assume no dynamics in current control loop. Consequently, the actual current and

the reference current are equal i ¼ iref . Then the actuator’s force can be expressed as

t ¼ KTðq; iÞirefKT min < KT ¼ Kn þDKTðq; iÞ < KT max

ð4:52Þ

Insertion of (4.52) into (3.1) yields the dynamics of a plant with actuator

½an þDaðqÞ�€qþ ½bðq; _qÞþ gðqÞþ textðq; tÞ� ¼ ½Kn þDKTðq; iÞ� iref ð4:53Þ

Collecting all the unknown terms the dynamics (4.53) can be rewritten as

an€q ¼ Kniref � ½bðq; _qÞþ gðqÞþ text�

zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{td

þDaðqÞ€q�DKTðq; iÞi ref|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}tdis

26664

37775

an€q ¼ Kniref � tdis

Here tdis stands for all unknown forces acting in the plant input – generalized disturbancesfor plant (4.53). In comparison with the generalized disturbance introduced for the description

of the plant as in (3.4) tdis now includes a forceDKTiref induced by thevariation of actuator gain.

Let pair ðiref ; vÞ be measured and let parameters an;Kn be known. Disturbance observer

can be designed by following the procedure discussed in Section 4.1. Selection of an inter-

mediate variable z ¼ qþ lv; l ¼ const > 0 and assuming the disturbance model _q ¼ 0, for

plant (4.54), results in the disturbance observer

_z ¼ l

anðKni

ref � zþ lvÞ; l > 0

q ¼ z� lv ð4:55Þ

Plugging (4.55) into_q ¼ _z� l _v yields

_q þ l

anq ¼ l

anðKni

ref � an€qÞ

q ¼ tdis

Observer (4.55) has the same structure as observer (4.15). The output of a linear first-order

filter (4.56) is equal to the estimated disturbance. The estimation error has dynamics as

in (4.17).

ð4:54Þ

ð4:56Þ


In comparison with observer (4.15) in (4.55) the current reference Kniref stands for the

observer input and the block KT appears in the structure of the plant, while the corresponding

block Kn appears in the structure of the observer, as shown in Figure 4.9.

Inclusion of a fraction of the input force corresponding to the difference between actual and

nominal actuator gain in a generalized disturbance shows a possibility to set the desired

‘nominal plant’ dynamics by selecting forces to be included into the generalized disturbance.

The disturbance observer can be used as a tool to manipulate the dynamics of the plant. For

example, if a system with a particular value of inertia (constant or variable) is desired, then by

changing the parameters of the disturbance observer the desired nominal dynamics of the plant

can be obtained.

It is easy to show that a disturbance observer with a measurement pair ðiref ; qÞ and with thesetting t* ¼ Kni

ref =an has the same structure as observer (4.24). The closed loop observers

discussed in Section 4.2 can be directly applied by setting the input variable as t ¼ Kniref .

Example 4.3 Disturbance Observer for System with Variable Actuator Gain In this

example the disturbance observer for a system with variable actuator gain is illustrated. The

plant used in this example is

aðqÞ€qþ ½bðq; _qÞþ gðqÞþ text�|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}system disturbance¼td

¼ t

t ¼ KTiref

KT ¼ Kn½1þ 0:2 cos ð128:28tÞ�; Kn ¼ 0:85 N �m=A

a ¼ an½1þ 0:25 sin ðqÞ�; an ¼ 0:1 kg �m2 ð4:57Þ

Other parameters are the same as in Example 4.1. The dynamics of plant (4.57) can be

rewritten in the form

Figure 4.9 Disturbance observer for plant with actuator without dynamics in the current loop


an€qþ ½Da€qþ bðq; _qÞþ gðqÞþ text � 0:2Kniref cos ð128:28tÞ�|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}

generalized disturbance¼tdis

¼ Kniref ð4:58Þ

The generalized disturbance to be estimated is given by

tdis ¼ 0:25an€q sin ðqtÞþ bðq; _qÞþ gðqÞþ text � 0:2Kniref cos ð128:28tÞ ð4:59Þ

The disturbance observer (4.56) and the observer (4.35) with input t ¼ Kniref are applied for

estimation of the disturbance (4.59).

The plant output closed loop control is the same as in Example 4.1 [a ¼ 0:80; k ¼ 50,

initial conditions in position qð0Þ ¼ 0:15 rad] with output reference yref ðtÞ ¼ ½1þ3:5 sin ð12:56tÞ� and output ylðq; _qÞ ¼ 50qþ 5 _q. The gain of the filter is g ¼ 600 for easier

comparison of the effects of the additional terms in the generalized disturbance.

In Figure 4.10, the first row the actuator gainKT and the system generalized disturbance tdisare shown. In the second row, the disturbance estimation error pðQ; tdisÞ and the disturbance tdis

Figure 4.10 Top row: actuator gain KT and disturbance tdis with input t ¼ Kniref . Middle row:

disturbance estimation error pðQ; tdisÞ and estimated disturbance tdis for observer (4.56). Bottom row:

pðQ; tdisÞ and tdis for observer (4.35). The filter gain is g ¼ 600. The output control parameters are k ¼ 50

and a ¼ 0:8


estimated by the observer (4.56) are depicted. In the third row, the estimation error pðQ; tdisÞand the output tdis of the observer (4.35)with input t ¼ Kni

ref are shown.All components of the

generalized disturbance are now included in the estimation. Diagrams illustrate the possibility

of using the same structure of the observer to estimate not only the exogenous disturbance but

the forces induced by the variation of parameters as well.

The estimation errors are nowhigher due to the presence of the high frequency component of

the changes in actuator gain. The bandwidth of the disturbance observer filter should be

selected higher, but we kept it the same as in the previous case in order to illustrate its influence

on the estimation error. The closed loop control of the system output is not influenced by the

error in the disturbance estimation.

This example illustrates applicability of rather simple structure for estimating a very

complex disturbance signal. Selection of a higher gain would make the disturbance estimation

error smaller. In this example the bandwidth of the disturbance observer is only about four

times higher than the changes in actuator gain, thus the dynamic error due to the first-order filter

is high.

4.3.2 Plant with Dynamics in Current Control Loop

Let ðiref ; vÞ be measured and parameters an;Kn be known. Plant model (3.1) with current loop

dynamics as in (4.51) yields system description

½an þDaðqÞ�€qþ ½bðq; _qÞþ gðqÞþ textðq; tÞ� ¼ ½Kn þDKTðq; iÞ�idi

dt¼ gc ðiref � iÞ

t ¼ KTðq; iÞ i ¼ KniþDKTðq; iÞi

KT min < KT ¼ Kn þDKTðq; iÞ < KT max

ð4:60Þ

By adding and subtracting Kniref to the right hand side (4.60) can be rearranged into

an€q ¼ Kniref � tdis

tdis ¼ ½bðq; _qÞþ gðqÞþ text� þDa€qþKniref �KTi

di


ð4:61Þ

The generalized disturbance tdis includes the dynamics of the current control loop. Selection

of an intermediate variable z ¼ qþ lv; l ¼ const > 0 and the disturbance model _q ¼ 0

with measurement pair ðiref ; vÞ and known parameters ðan;KnÞ results in the dynamics of

generalized disturbance observer

_z ¼ la� 1n ðKni


q ¼ z� lv ð4:62Þ


Plugging _z into_q ¼ _z� l _v it is easy to show that the output of observer (4.62) canbe expressed

as_qþ la� 1

n q ¼ la� 1n ðKni

ref � an€qÞ, q ¼ tdis. Disturbance observer (4.62) includes force

tidis ¼ Kniref �KTi as part of the generalized disturbance. This is a result of neglecting the

current loop dynamics in designing the observer. In some applications the force tidis may not be

large. In most industrial systems such an approximation is expected to yield satisfactory results.

In systemswith a high bandwidth of the acceleration loop, the current loop dynamics cannot

be neglected. Then structure (4.62) can be rearranged to reflect the dynamics of the current

controller. Let ðiref ; vÞ be measured, the parameters ðan;KnÞ and the nominal dynamics

of current loop known. Then the input to the observer could include the nominal dynamics

of the current loop din=dt ¼ gcnðiref � inÞ, where in is the current input to the observer and gcn isthe gain of the nominal current loop filter. By adding and subtractingKnin in the right hand side

in Equation (4.60) yields

an€q ¼ Knin � tdisdi


din

dt¼ gcnðiref � inÞ

tdis ¼ ½bðq; _qÞþ gðqÞþ text� þDa€qþKnin �KTi

ð4:63Þ

Here, disturbance tdis includes the difference between nominal and actual current loop

dynamics and the variation in actuator gain.

Just by replacing Kniref with Knin the observer (4.62) can be modified to include nominal

current loop dynamics

_z ¼ la� 1n ðKnin � zþ lvÞ; l > 0

q ¼ z� lv ð4:64Þ

By plugging _z into_q ¼ _z� l _v it is easy to show that output of observer (4.64) can be

expressed as dq=dtþ la� 1n q ¼ la� 1

n Knin � an€qð Þ, q ¼ tdis.Observer (4.64) is shown in Figure 4.11(a) with the assumption that plant inertia and

actuator gain are constants. The actuator current loop dynamics are Wc, Wcn stands for the

nominal current loop dynamics and the observer gain is l ¼ gan. The actuator gain is

represented by block KT and its nominal value by block Kn. In Figure 4.11(b) the topology

is rearranged to show transfer functions from the measured variables iref and v to the observer

output. The transfer function P ¼ 1=as2 stands for the plant dynamics and Pn ¼ 1=ans2 stands

for the nominal plant dynamics. This structural arrangement points to anotherway of designing

disturbance observers. This will be discussed in detail in Section 4.5.

Example 4.4 Disturbance Observer with Current Control Loop Dynamics The dis-

turbance observer for a system with dynamics in the actuator current control loop is illustrated

in this example. The plant is modeled as in (4.60) with the same parameters as in Example 4.3,

moment of inertia a ¼ an ¼ 0:1 kg �m2, actuator gainKT ¼ Kn½1þ 0:2cos ð128:28tÞ�N �m=AwithKn ¼ 0:85 N �m=A. The current control loop dynamics is modeled as a first-order system.


The observer is designed as in (4.62)

_z ¼ la� 1n ðKni


q ¼ z� lv

l ¼ gan; g ¼ 600 ð4:65Þ

The output of the observer is a generalized disturbance

tdis ¼ Da€qþ bðq; _qÞþ gðqÞþ text þKTi�Kniref

di

dt¼ gc ðiref � iÞ

gc ¼ 300

ð4:66Þ

Simulation is presented for a closed loop control system with output ylðq; _qÞ ¼ 50qþ 5 _qand reference yref ðtÞ ¼ ½1þ 3:5 sin ð12:56tÞ�. The parameters of the controller are a ¼ 0:80;k ¼ 50, with initial conditions in position qð0Þ ¼ 0:15 rad. The output controller is designed

for nominal plant without taking the current loop dynamics into account.

Figure 4.11 Disturbance observer with current control loop dynamics: (a) nominal structure and (b)

representation by transfer functions from iref and q


In the first row in Figure 4.12, the change in actuator gainKT and the generalized disturbance

tdis are shown. In the second row, the disturbance estimation error pðQ; tdisÞ and the

disturbance tdis estimated by the observer (4.34) are shown. In the third row, the difference

pðQ; iÞ ¼ i� iref between actual actuator current i and the reference current input to the

disturbance observer iref are shown.

In Figure 4.13, the same diagrams as in Figure 4.12 are shown. Here the disturbance

observer is implemented as in (4.64) with nominal dynamics of the current control loop

din=dt ¼ gcnðiref � inÞ and gcn ¼ 450. All other conditions are kept the same as in Figure 4.12.

The example illustrates the applicability of the observer in estimating the forces induced by

the uncompensated actuator gain and current loop dynamics. The design parameter is the

observer gain, while nominal inertia and nominal actuator gain define the desired nominal

plant. A mismatch of these parameters with their actual values and guidance for their selection

will be discussed later in the text.

Figure 4.12 Top row: actuator gain KT and disturbance tdis. Middle row: disturbance estimation error

pðQ; tdisÞ and estimated disturbance tdis for observer (4.65). The dynamics in the current loop have filter

gain gc ¼ 300 and the disturbance observer gain g ¼ 600. Bottom row: actuator current i and the

difference between actuator current and its reference pðQ; iÞ ¼ i� iref . The output controller parameters

are k ¼ 50 and a ¼ 0:8


4.4 Estimation of Equivalent Force and Equivalent Acceleration

Till now we have discussed the estimation of the disturbance and its components. All of these

quantities represent someproperties of the control plant or its interactionwith the environment. In

this sectionwewould like to discuss estimationof the quantities related to the control task. For the

control tasks discussed in Chapter 3, the dynamics of the generalized control error with the

desired acceleration as the control input are given in (3.22) and with force as the control input in

(3.25). In both cases the changes are described by first-order dynamics. The equivalent

acceleration can be interpreted as the disturbance in (3.22). The same interpretation can

be applied to the equivalent force in (3.25). This suggests that estimation of the equivalent

accelerationand the equivalent force canbe treated in the samewayas thedisturbance estimation.

Let us first discuss the design of the equivalent force observer. Let the generalized control

error s and the input force t be measured and the parameters ðan; gÞ known. Since the

equivalent force is treated as a disturbance let teq be modeled as the output of linear system

Figure 4.13 Top row: actuator gain KT and disturbance tdis. Middle row: disturbance estimation error

pðQ; tdisÞ and estimated disturbance tdis for observer (4.65). The dynamics in the current loop are with

filter gain gc ¼ 300, the nominal dynamics of the current control loop have gcn ¼ 450 and disturbance

observer gain g ¼ 600. Bottom row: actuator current i and the difference between actuator current and its

reference pðQ; iÞ ¼ i� iref . The output controller parameters are k ¼ 50 and a ¼ 0:8


_q ¼ 0. Then the dynamics of the generalized control error (3.25) augmented by model of

equivalent force yield

_s ¼ a� 1n gðt� teqÞ

_q¼ 0

teq ¼ q

ð4:67Þ

By selecting intermediate variable z ¼ qþ ls; l > 0 the equivalent force observer can be

constructed as

_z ¼ lga� 1n ðtþ lsÞ� lga� 1

n z

q ¼ z� ls ð4:68Þ

By inserting _z into_q ¼ _z� l _v it is easy to show that the output of observer (4.68) can be

expressed as q ¼ teq, thus

_q þ lga� 1

n q ¼ lga� 1n ðt� ang

� 1 _sÞ ¼ lga� 1n teq

q ¼ teq ð4:69Þ

The estimated equivalent force is the output of a low-pass filter with corner frequency

defined by design parameter l, nominal systems inertia an and the generalized error input

gain coefficient g. The equivalent force observer has the same structure as the disturbance (or

generalized disturbance) observer (4.16). The difference is in the inputs to the observer. The

inputs to the equivalent force observer are the input force and the generalized control error, thus

this can be applied only in the closed loop systems. The structure of the equivalent force

observer is shown in Figure 4.14 with g ¼ lga� 1n .

The equivalent acceleration observer can be constructed in a very similar way. From (3.22)

the equivalent acceleration can be expressed as €qeq ¼ €qdes � g� 1 _s, thus the information

needed for the equivalent acceleration estimation are the desired acceleration €qdes and the

generalized control error s. The desired acceleration is the output of the outer loop controllerand s is the control error, so both are readily available within the controller. Assume the pair

ð€qdes;sÞ is measured, the coefficient g is known and the equivalent acceleration is modeled by

first-order dynamics _q ¼ 0 and q ¼ €qeq.

Let intermediate variable be z ¼ qþ ls; l > 0. From (3.22) and the model of the distur-

bance _q ¼ 0 the estimation of intermediate variable z can be then constructed as

_z ¼ lgð€qdes þ lsÞ� lgz

q ¼ z� lsð4:70Þ

Inserting _z into_q ¼ _z� l _s yields

_qþ lgq ¼ lgð€qdes � g� 1 _sÞ ¼ lg€qeq

q ¼ €qeq

ð4:71Þ


Observers (4.68), (4.70) and disturbance observer (4.20) have the same structure. Thus the

same observer can be applied to estimate the vital components of the control input: (i) the

generalized disturbance, (ii) the equivalent acceleration and (iii) the equivalent force

(a linear combination of the disturbance and the equivalent acceleration). The versatility

of this structure is apparent. It can be used with an open loop plant (disturbance observer)

or a closed loop (equivalent acceleration, equivalent force observers). The closed loop

application estimates the control inputs which enforce zero rate of change of the control

error.

The disturbance observers discussed so far require a model of the plant and a model of the

disturbance. The resulting topologies are essentially the same – the inversed plant and the

appropriate filter are part of all of them. In realization the inverse plant dynamics are linkedwith

the filter in order to achieve proper transfer functions thus avoid differentiation. The closed loop

observer design is not based on the disturbance model, but the dynamics of the observer greatly

depend on the selection of the controller.

Example 4.5 Estimation ofEquivalentAcceleration andEquivalent Force Estimation

of the equivalent acceleration and the equivalent force is illustrated in this example.

The plant and the parameters are the same as in Example 4.3 with inertia a ¼ an½1þ0:25 sin ðqÞ�kg �m2; an ¼ 0:1 kg �m2 and KT ¼ Kn ¼ 0:85 N �m=A. The current control loopdynamics are selected with gc ¼ 300 and the disturbance observer is simulated with filter gain

g ¼ 600.

Illustration of the estimation of equivalent acceleration and equivalent force requires closed

loop system operation. A simulation is presented for a closed loop control system with output

ylðq; _qÞ ¼ 50qþ 5 _q and its reference yref ðtÞ ¼ ½1þ 3:5 sin ð12:56tÞ�. The generalized error is

ey ¼ yref � 5ð _qþ 10qÞ. The parameters of the controller are a ¼ 0:80; k ¼ 50 and initial

Figure 4.14 Equivalent force observer teq and its implementation with first-order filter


conditions in position qð0Þ ¼ 0:15 rad. The output controller is designed for a nominal plant

without taking current loop dynamics into account

€qeql¼ 0:2 3:5 � 12:56 cos 12:56tð Þ� 50 _qð Þ

€qcon ¼ � 10 ey�� 2a� 1

sign ey� ð4:72Þ

The disturbance is

tdis ¼ Da€qþ b q; _qð Þþ g qð Þþ text ð4:73Þ

For equivalent acceleration (4.72) and disturbance (4.73) the equivalent force becomes

teql ¼ tdis þ an€qeql ð4:74Þ

For equivalent acceleration structure (4.71) and for equivalent force estimation structure

(4.68) is used. The equivalent acceleration observer is

dz

dt¼ gð€qdes þ 0:02 gey � zÞ

€qeq ¼ z� 0:02 gey; g > 0

ey ¼ yl � yref

ð4:75Þ

The equivalent force observer is written in the following form

dzt

dt¼ gðtþ 0:02 gey � ztÞ

teql ¼ zt � 0:02 gey; g > 0

ey ¼ yl � yref

ð4:76Þ

Illustration of the estimation of equivalent acceleration and equivalent force requires

closed loop system operation. A simulation is presented for a closed loop control systemwith

output ylðq; _qÞ ¼ 50qþ 5 _q and its reference yref ðtÞ ¼ ½1þ 3:5 sin ð12:56tÞ�. The generalized

error is ey ¼ yref � 5ð _qþ 10qÞ. The parameters of the controller are a ¼ 0:80; k ¼ 50,

with initial conditions in position qð0Þ ¼ 0:15 rad. The output controller is designed for

a nominal plant without taking current loop dynamics into account. The current control loop

dynamics are selected with gc ¼ 360.

Filter gain g ¼ 600 is applied to both observers. The simulation results are shown in

Figure 4.15. In the first row the output reference yref , output y and the disturbance tdis are shown.In the second row the equivalent acceleration €qeq, the convergence acceleration €qcon and the

estimated equivalent acceleration €qeqare shown. In the third row the actual teq and estimated

equivalent force teq are shown. The estimation errors in both equivalent acceleration and

equivalent force are small.


4.5 Functional Observers

In this sectionwewill showa slightly differentway to design observerswithin the framework of

linear systems. To apply a Laplace transformation and the transfer function notations the plant

is assumed to have constant, but unknown parameters. The disturbance is treated as an input

signal to the plant. The idea is based on the structure shown in Figure 4.11(b). There the

observer output is shown as a linear combination of the outputs of transfer functions from

the measured signals tdis ¼ KnWcnQiref �P� 1Qq. The transfer functions KnWcn and P are

defined by the plant dynamics and the only design parameter is the filterQ. Here, wewould like

to investigate a possibility of selecting independently transfer functions from the reference

current and from the plant output to the estimated variable [8].

Let the reference current and the position of the plant (3.1) be measured, the plant dynamics

P� 1 ¼ as2, the current controller dynamics Wc and the actuator gain KT be known.

For the system in Figure 4.16(a) let z ¼ WNq be the desired output. The transfer function

WN may or may not be proper. Indeed, if the desired output is proportional to acceleration

Figure 4.15 Changes in output reference yref , output y, disturbance tdis, equivalent acceleration €qeq,convergence acceleration €qcon, estimated equivalent acceleration €q

eq, equivalent torque teq and its

estimation teq. The observer filter gain is g ¼ 600 and the control loop parameters are a ¼ 0:75; k ¼ 50


z ¼ h2s2q or a linear combination of acceleration and velocity z ¼ ðh2s2 þ h1sÞq or linear

combination of acceleration, velocity and position z ¼ ðh2s2 þ h1sþ h0Þq, the resultingWN is

not a proper transfer function. It is realistic to assume that acceleration is the highest derivative

contained in output z, thus the transfer function of ideal output could, in general, be represented

by WN ¼ h2s2 þ h1sþ h0. Inserting reference current iref , plant disturbance td and the plant

dynamics the desired output z can be expressed as

z ¼ WNq ¼ WNPðKTWciref � tdÞ

¼ WNPKTWciref �WNPtd

ð4:77Þ

In addition to the known input iref one has to know the disturbance to generate the desired

output. The presence of the unknown input td and the structure of the transfer function WN

makes a direct calculation of the desired output from (4.77 ) unrealizable. Having position q

and reference current iref as measured variables allows calculation of the variable

z ¼ W1iref þW2q, as shown in Figure 4.16(b). Here, the transfer functions W1 and W2 are

design parameters to be selected from the requirement that z is an estimate of the desired

variable z with a specified error due to unknown input td .Since W1 cannot be selected equal to WN an estimation error will appear due to the

unmeasured input td . Let the estimation error be such that z can be expressed as

z ¼ WN qþWL td ¼ zþWL td ð4:78Þ

Here WLtd stands for the error in estimation of the output z due to the presence of the

unmeasured inputtd . IdeallyWL should be zero, but that is not feasible. The estimation error is

determined by the structure ofWL and the properties of the disturbance input, thus it is natural to

seek WLtd � 0 in the selected frequencies range.

Figure 4.16 Estimation of the desired output z ¼ WNq by adding the correction inputW1iref such that

the error due to the disturbance input has specified properties. (a) Desired relation z ¼ WNq.

(b) Realization of the observer z ¼ WNqþWLtd


From Figure 4.16(b) and (4.78) the estimated output z can be expressed as

z ¼ W1iref þW2q ¼ W1i

ref þW2PðKTWciref � tdÞ

¼ ðW1 þW2PKTWcÞiref �W2Ptdð4:79Þ

Nowtheproblem is to select transfer functionsW1 andW2 such that output z is the estimationof

the desired output z with the estimation error WL td due to the unmeasured input.

From Equations (4.77 )Equations (4.78) and (4.79) the estimation error can be derived as

WLtd ¼ ½PKTWcðWN �W2Þ�W1�iref þPðWN �W2Þtd ð4:80Þ

From (4.80) the following relations are obtained

PKTWcðWN �W2Þ�W1 ¼ 0

PðWN �W2Þ�WL ¼ 0

)ð4:81Þ

Solving (4.81) for W1 and W2, with the assumption that KTWc;WL and WL;P do not have

common poles and zeroes, yields

W1 ¼ KTWcWL

W2 ¼ WN �WLP� 1

ð4:82Þ

For the easy realization transfer functions W1 and W2 need to be at least proper. The plant

dynamics P� 1 ¼ as2 and the actuator current loop dynamics KTWc are assumed known. The

error transfer functionWL is a design parameter and it should be selected such thatW1 andW2

are realizable.

It is amatter of exercise to check that structures of the disturbance observers discussed in the

previous sections may be derived from (4.81) by proper selection of the desired dynam-

ics (4.78). As an illustration of using such a design, the following example shows the estimation

of velocity, acceleration and disturbance torque.

Example 4.6 Functional Observer One realization of the observer discussed in Sec-

tion 4.5 with the general structure shown in Figure 4.16 is illustrated in this example. For a

known plant and actuator current loop dynamics the selection of the transfer functions is shown

in (4.82). Here, instead of plant P� 1 ¼ as2 and actuator current loop dynamics W ¼ KTWc

their nominal values will be used. Then (4.82) can be written as

W2 ¼ WN �WLP� 1n

W1 ¼ KnWcnWL

(ð4:83Þ

HereWN stands for the desired transfer function from output to measurement, P� 1n ¼ ans

2

stands for the inverse nominal plant – disturbance to output and force to output transfer

function, Win ¼ KnWcn stands for the approximation of the current control loop and actuator

gain,W1 stands for the input to estimated variable transfer function andW2 stands for the output

to estimated variable transfer function.


In the design the actuator current and the position ðiref ; qÞ are assumed measured and the

nominal values of the inertia an ¼ 0:1kg �m2 and the actuator gain Kn ¼ 0:85N �m=A are

known. Here W1 and W2 will be selected by neglecting the actuator current control loop

dynamics – thus assuming Wcn ¼ 1.

The error due to unmeasured disturbance as a design parameter is selected to satisfy

WLtdis ¼ 1

an

g2sðgsþ dÞðsþ gÞ2 tdis;

g; g; d > 0

ð4:84Þ

Coefficientsg; d should be selected in the design process, and g is the filter gain. From (4.83)

the transfer function W1, with Wcn ¼ 1, becomes

KnWcnWL ¼ W1 ¼ Kn

an

g2sðgsþ dÞðsþ gÞ2 ¼ KnPnðWN �W2Þ ð4:85Þ

For WL as in (4.84) transfer function W2 can be expressed as

W2 ¼ WN �P� 1n WL ¼ WN � g2

s3ðgsþ dÞðsþ gÞ2 ð4:86Þ

Let the ideal transfer function be WN ¼ as2 þbs or in other words assume that a linear

combination of velocity and acceleration needs to be estimated. Then from (4.86) follows

W2 ¼ as2 þbs� g2s3ðgsþ dÞðsþ gÞ2 ¼ ðas2 þbsÞðsþ gÞ2 � g2s3ðgsþ dÞ

ðsþ gÞ2

¼ ða� g2gÞs4 þðbþ 2ga� g2dÞs3 þð2gbþ g2aÞs2 þ g2bs

ðsþ gÞ2ð4:87Þ

The values of g; d can be determined ifW1 is required to be a proper transfer function (both

nominator and denominator as second-order polynomials). Then (4.87) yields

a� g2g ¼ 0

bþ 2ga� g2d ¼ 0

)Y g ¼ a

g2; d ¼ bþ 2ga

g2ð4:88Þ

Finally, after some algebra, the observer transfer functions can be determined as

W1 ¼ Kn

an

as2 þðbþ 2gaÞsðsþ gÞ2

W2 ¼ gðgaþ 2bÞs2 þ gbs

ðsþ gÞ2

WN ¼ as2 þbs

Wcn ¼ 1

ð4:89Þ


The design parameters are now the desired ideal measurement transfer function WN and

filter gain g. By fraction decomposition (4.89) can be realized as in (4.90) and (4.91) using just

two first-order filters.

W1 ¼ Kn

an

as2 þðbþ 2gaÞsðsþ gÞ2 ¼ b0 b3 þ b2g

sþ gþ b1g

2

ðsþ gÞ2

24

35

W2 ¼ gsðgaþ 2bÞsþ gb

ðsþ gÞ2 ¼ a0 a3 þ a2g

sþ gþ a1g

2

ðsþ gÞ2

24

35

ð4:90Þ

_z1 ¼ gða1a0qþ b1b0iref � z1Þ

_z2 ¼ gðy1 þ a2a0qþ b2b0iref � z2Þ

z ¼ z2 þ a3a0qþ b3b0iref

ð4:91Þ

By selecting the structure of the desired output transfer function WN and the error due to

unmeasured disturbance WL the proposed structure can be applied to estimate velocity,

acceleration or their linear combination. In addition the structure may be used to estimate

disturbance.

Coefficients ai and bi i ¼ 0; 1; 2; 3 for different structures of WN and WL as in (4.84) are

listed below:

. velocity estimation z ¼ _q

aa ¼ g; a1 ¼ 1; a2 ¼ �3; a3 ¼ 2; b0 ¼ Kn

gan; b1 ¼ �1; b2 ¼ 1; b3 ¼ 0

. acceleration estimation z ¼ €q

aa ¼ g2; a1 ¼ 1; a2 ¼ �2; a3 ¼ 1; b0 ¼ Kn

an; b1 ¼ �1; b2 ¼ 0; b3 ¼ mn > 1

. disturbance estimation z ¼ tdis

aa ¼ g2an

Kn

; a1 ¼ �1; a2 ¼ 2; a3 ¼ �1; b0 ¼ 1; b1 ¼ 1; b2 ¼ 0; b3 ¼ 0

In Figure 4.17 transients are shown for closed loop control. The plant output controller is

enforcing finite-time convergence. Controller parameters are a ¼ 0:75; k ¼ 50 and the initial

conditions in position qð0Þ ¼ 0:15 rad. The actuator current control loop dynamics are

simulated by di=dt ¼ 0:6gðiref � iÞ and the plant input force is t ¼ KTi. The input to the

observer is t ¼ Kniref . In this example, Equation (4.91) is used to estimate the velocity _q ¼ v.

The equivalent torque and equivalent acceleration are estimated by observers (4.75) and (4.76),

respectively. In all observers the same filter gain is g ¼ 600.


4.6 Dynamics of Plant with Disturbance Observer

The control input required to enforce the desired acceleration €qdes is t ¼ an€qdes þ tdis. Since

tdis is not measured in the implementation estimated disturbance tdis is used, thus the

realizable control input is

t ¼ an€qdes þ tdis ð4:92Þ

As shown, the dynamics of the disturbance observer is described by a filter. In further

analysis, we will assume that estimated disturbance is the output of a linear filter Q and that

plant parameters are constant. Then linear systems methods can be applied in analyzing the

systemwith disturbance observer feedback (4.92). The term an€qdes ¼ tdes can be interpreted as

the input force induced by the desired acceleration.

The plant (3.1) with estimated disturbance feedback (4.92) is shown in Figure 4.18. The first

salient feature of this structure is the presence of the positive feedback loop formed by the force

input to the observer and the observer dynamics. Indeed, we can separate the estimated

disturbance tdis into the component induced by the control input ttdis and the component

Figure 4.17 Changes in output reference yref , output y, disturbance tdis, estimated velocity v, velocity v,

estimation of equivalent torque teq, error in velocity estimation ev ¼ v� v and estimated equivalent

acceleration €qeq. The observer filter gain is g ¼ 600. The actuator current control loop dynamics are

di=dt ¼ 0:6gðiref � iÞ;t ¼ KTi. The input to the observer is t ¼ Kniref The control loop parameters are

a ¼ 0:75; k ¼ 50


induced by the plant output tqdis. If the observer dynamics is described by a first-order filter

with gain g, for plant with disturbance feedback as in Figure 4.18 the input force t can be

expressed as

tdis ¼ ttdis þ tqdis_ttdis ¼ gðt� ttdisÞ_tq

dis ¼ gð� an€q� tqdisÞð4:93Þ

Inserting (4.93 ) into (4.92) yields

t ¼ an€qdes þ gan

ðt0

ð€qdes � €qÞdz ð4:94Þ

Observer feedback inserts an integral action in the acceleration control loop. The integral

acts on the difference between the desired and actual acceleration, thus it enforces zero tracking

error in the acceleration loop. For the more complex dynamics of the disturbance observer the

structure of the acceleration loop can be investigated using the same procedure.

4.6.1 Disturbance Estimation Error

Insertion of (4.92) into the system dynamics (3.4) yields

an€qþ tdis ¼ t ¼ an€qdes þ tdis

tdis ¼ Dan€qþ bðq; _qÞþ gðqÞþ textð4:95Þ

The right hand side in (4.95) has the same structure as the plant dynamics with the desired

acceleration and the estimated disturbance as components. This points more clearly at the

structure and components of the acceleration control. The structure of the control input (4.92) is

the same as the structure of the control plant with disturbance feedback aimed at compensating

for generalized disturbance and the desired acceleration to enforce the desired motion.

By introducing the desired force tdes ¼ an€qdes as a new variable, (4.95) can be rearranged

into two equivalent forms

Figure 4.18 Acceleration control with estimated disturbance feedback


an€q ¼ an€qdes �ðtdis � tdisÞ

an€q ¼ tdes �ðtdis � tdisÞð4:96Þ

The difference between these two representations is in the definition of the control input.

When the desired acceleration €qdes is taken as the control input into the compensated system the

first expression is used. Taking the desired force tdes as the control input results in the secondrepresentation.

The dynamics of the plant (3.1) with the disturbance feedback (4.92) reduces to a double

integratorwith nominal inertia. The desired acceleration is the control input to the compensated

plant and an error in the disturbance estimation ðtdis � tdisÞ can be treated as the input

disturbance in (4.96). If the disturbance observer dynamics is described by filter Q then the

disturbance estimation error can be expressed as

pðQ; tdisÞ ¼ tdis � tdis ¼ ð1�QÞtdis ð4:97Þ

The disturbance compensation error (4.97) gives an insight on the selection of the filter Q.

The obvious choice is to make Qtdis � 1 in the frequency range in which generalized

disturbance tdis is dominant and its compensation is of interest. The same notation for

the disturbance estimation error pðQ; tdisÞ will be used in the time and frequency domain.

Here dependence on ðQ; tdisÞ is used as an indication that error depends on the observer and thedisturbance dynamics.

In Equation (4.96) the disturbance estimation error is added to the nominal plant dynamics.

The bandwidth of the disturbance observer should be as high as possible, thus making the

disturbance compensation error pðQ; tdisÞ close to zero in the wide span of frequencies.

The compensation error depends on the span of frequency of the disturbance, thus in the

analysis the model of disturbance should be considered. With proper selection of the

disturbance observer filter the disturbance compensation error pðQ; tdisÞ will, in practical

engineering terms, be close to zero in the selected frequency range and a plantwith an estimated

disturbance feedback (4.96) can be approximated by

an€q ¼ an€qdes ð4:98Þ

The dynamics of a compensated plant (4.98) will be used whenever the dynamics of the

disturbance compensation error are not essential for analysis. That simplifies the expressions

while preserving all the essential properties of the system. System (3.1) with estimated

disturbance feedback and its approximated dynamics (4.98) are shown in Figure 4.19. The

estimated disturbance is essentially playing the role of a feedforward term and, if properly

designed, does not influence the stability of the closed loop system within the selected span of

frequencies.

4.6.2 Dynamics of Plant With Disturbance Observer

In the design of a disturbance observer the variations in plant parameters, inertia and actuator

gain or current control loop transfer function are treated as part of the generalized disturbance.


The influence of the error in compensation for the forces induced by parameter changes on the

system dynamics needs to be investigated. To show the salient properties, wewill first look at a

system with changes only in plant inertia and after that we will turn to analyze a system with

changes in other parameters. In this way, the contributions of parameters and guidance for the

design based on the compensated plant dynamics will be shown more clearly.

The dynamics of a system with disturbance feedback, shown in Figure 4.20(a), is

described by

½ðtdes þ zÞ� td �P ¼ q

½ðtdes þ zÞ�P� 1n q�Q ¼ z

ð4:99Þ

Here P ¼ 1=ðas2Þ stands for plant (3.1) with the control input t ¼ tdes þ z and the

disturbance td ¼ bðq; _qÞþ gðqÞþ text, Pn ¼ 1=ðans2Þ stands for the nominal plant, Q stands

for the disturbance observer dynamics, q is the system output and z stands for the disturbance

observer output. Elimination of z from (4.99 ) yields

½P� 1n QþP� 1ð1�QÞ�q ¼ tdes �ð1�QÞtd ð4:100Þ

Rearranging (4.100) into q ¼ PnQqrtdes �PQqttd , yields

q ¼ Pn

1

P� 1Pnð1�QÞþQtdes �P

P� 1Pnð1�QÞP� 1Pnð1�QÞþQ

td

Qqr ¼ 1

P� 1Pnð1�QÞþQ

Qqt ¼ P� 1Pnð1�QÞP� 1Pnð1�QÞþQ

ð4:101Þ

Figure 4.19 Plant (3.1) with disturbance observer feedback and its equivalent modification to a system

with desired acceleration and disturbance estimation error as inputs


Here, the control to output transfer function PnQqr and the disturbance to output transfer

functionPQqt are selected to allow a comparison with a system representation as in (4.96). The

coefficient P� 1Pn ¼ aa� 1n ¼ a� 1 > 0 stands for the ratio between the real and the nominal

plant inertia. It is easy to verify that, within the range of frequencies for which Q ¼ 1 and

tdes ¼ an€qdes, with zero initial conditions qð0Þ ¼ _qð0Þ ¼ 0, the dynamics (4.101) leads to the

same solution as represented in (4.98). This justifies the design of the closed loop system in

the given span of frequencies based on (4.98).

Let the disturbance observer filter be Q ¼ g=ðsþ gÞ. The terms P� 1Pnð1�QÞ and

P� 1Pnð1�QÞþQ can be expressed as

P� 1Pnð1�QÞ ¼ 1

a

s

sþ g

P� 1Pnð1�QÞþQ¼ 1

a

sþag

sþ g

ð4:102Þ


q ¼ asþ g

sþagPntdes �P

s

sþagtd ð4:103Þ

System (4.103) shows that, if a mismatch in inertia between the plant and the disturbance

observer exists, a lead-lag block is inserted into the control loop, as shown in Figure 4.20(b). At

low frequencies the contribution of that block is small and the control to output dynamics are

close to the nominal plant dynamics. In the disturbance path the inertia mismatch results in

changes of the disturbance compensation bandwidth.

Figure 4.20 System with (a) disturbance observer feedback and (b) its equivalent modification as

in (4.103).


Figure 4.21 Bode plot of control and disturbance transfer functions for plant with disturbance observer

transfer function Q ¼ g=ðsþ gÞ; g ¼ 600. (a) Transfer functions Qqr and Qqt for a ¼ 0:025 kg �m2 and

an ¼ 0:1 kg �m2. (b) Transfer functions Qqr and Qt for a ¼ 0:5 kg �m2 and an ¼ 0:1 kg �m2

Figure 4.21 shows the transfer functions Qqr and Qqt for plant (3.1) with nominal inertia

an ¼ 0:1 kg �m2 and actual plant inertia a ¼ 0:025 kg�m2 in the left diagram and with

a ¼ 0:5 kg �m2 in the right diagram. On both diagrams the lead-lag characteristics of the

input transfer function Qqr are clear. The shift in the disturbance compensation bandwidth is

obvious in comparison with the Qqt transfer function.

4.6.2.1 Plant with Actuator

Plant (4.53) with actuator gain KT , plant inertia a and disturbance feedback is shown in

Figure 4.22. Here, the desired force tdes is a function of the desired actuator current and the

nominal actuator gain tdes ¼ Knides. The system inertia variation associated force Da€q and

the actuator gain variation induced force DKTiref are part of the estimated disturbance z.

From Figure 4.22 the following relations can be derived

½ðides þ zÞKT � td �P ¼ q

½ðides þ zÞKn �P� 1n q�Q ¼ Knz

ð4:104Þ

Here, P� 1 ¼ as2 stands for the plant with control input t ¼ KTðides þ zÞ and

disturbance z ¼ tdis. P� 1n ¼ ans

2 stands for the inverse of the nominal plant, Q stands for


the disturbance estimation filter and q stands for the plant output. Elimination of z from

Equation (4.104) and rearranging the solution into the form q ¼ PnQqrKnides �PQqttd yields

q ¼ Pn

1

KnK� 1T P� 1Pnð1�QÞþQ

Knides �P

KnK� 1T P� 1Pnð1�QÞ


td

Qqr ¼ 1


Qqt ¼ KnK� 1T P� 1Pnð1�QÞ


ð4:105Þ

Since KnK� 1T is a real number, it follows that expression (4.105) has the same form

as (4.101), thus the same system behavior can be expected. In the span of frequencies for which

Q � 1 the dynamics (4.105) can be approximated by (4.98).

If approximation (4.98) is not used, then insertion of the dynamics of the disturbance

observer as Q ¼ g=ðsþ gÞ and KnK� 1T P� 1Pn ¼ b� 1 into (4.105) yields

q ¼ bsþ g

sþbgPntdes �P

s

sþbgtd ; b ¼ anKT

aðqÞKnð4:106Þ

Here, the lead-lag block depends on the product of the compensation ratio of the system

inertia and the compensation ration of the actuator gain. Since in the structure of the disturbance

observer the actuator gain and inertia are in different loops, structure (4.106) offers more

flexibility in shaping the characteristics of the lead-lag block. Other properties are the same as

Figure 4.22 Plant (4.53) with disturbance observer feedback (a) and its equivalent modification as

in (4.106) (b).


for system (4.103). For higher order disturbance observer filters, similar structures can be

derived by following the same procedure.

4.6.2.2 Plant with Current Loop Dynamics

A plant with actuator current loop dynamics (4.60) can be analyzed in a similar way. As shown

in Section 4.3.2 the disturbance observer can be designed by disregarding the current loop

dynamics, as shown in Figure 4.23, or by taking the nominal current loop dynamics into

consideration.

For the system in Figure 4.23 the input force is expressed as t ¼ KTWcðides þ zÞ. The forcesinduced by the inertia variation and the actuator dynamics induced forces are included in the

estimated disturbance. The dynamics of the system can be written in the following form

½ðides þ zÞKTWc � td �P ¼ q

½ðides þ zÞKn �P� 1n q�Q ¼ Knz

ð4:107Þ

Elimination of z from (4.107) and rearranging the solution into the form

q ¼ PnQqrKnides �PQqttd yields

q ¼ Pn

1

KnK� 1T P� 1PnW � 1

c ð1�QÞþQKni

des �PKnK

� 1T P� 1PnW

� 1c ð1�QÞ


c ð1�QÞþQtd

Qqr ¼ 1


c ð1�QÞþQ

Qqt ¼ KnK� 1T P� 1PnW

� 1c ð1�QÞ


c ð1�QÞþQ

ð4:108Þ

HereP andPn stand for the actual plant dynamics and the nominal plant dynamics,Q stands

for the disturbance observer filter,Wc stands for the current loop dynamics,KT andKn stand for

the actual and the nominal actuator gain.

The dynamics of a plant with compensated disturbance (4.108) depends on the actuator

current control loop and a mismatch of the system parameters. In the span of frequencies for

which Q � 1 approximation (4.105) is still valid.

Figure 4.23 System with dynamics in actuator current control loop and disturbance observer feedback


If nominal current loop dynamics Wcn is included within the structure of the disturbance

observer, the input to the disturbance observer is then block KnWcn and the second equation

in (4.107) changes to ½ðides þ zÞKnWcn �P� 1n q�Q ¼ Knz while the rest of the system remains

the same. This change yields the dynamics of the compensated system

q ¼ PnQqrKnides �PQqttd

Qqr ¼ 1


c ð1�WcnQÞþQ

Qqt ¼ KnK� 1T P� 1PnW

� 1c ð1�WcnQÞ


c ð1�WcnQÞþQ

ð4:109Þ

As expected, both the nominal current loop dynamics Wcn and the actual current loop

dynamics Wc are contributing to the dynamics of the compensated system. The compensated

system dynamics can still be expressed in the form q ¼ PnQqrKnides �PQqttd and thus the

general property of the compensated systems is preserved. The change of the disturbance

compensation bandwidth is now linked to the change of both Wc and Wcn.

4.6.2.3 Compensation of Current Loop Dynamics

The analysis done so far, shows that the compensated dynamics can be partitioned as

q ¼ PnQqrtdes �PQqttd , where the transfer function Qqr can be interpreted as the dynamic

gain in the control input path and Qqt as the disturbance input dynamic gain. Both depend on

the disturbance observer dynamics and the mismatch in the system parameters with values

used in the disturbance observer design. The design of the disturbance observer has been

guided by minimization of the estimation error in selected range of frequencies.

In this section, we would like to discuss the application of a functional observer design

leading to the desired structure of the transfer functions Qqr and Qqt. We will analyze plant

having actuator current loop dynamics as in (4.60),with known nominal parameters of the plant

and both the reference current and the output measured. BlocksW1 andW2 are inserted into the

system structure as shown in Figure 4.24. The design goal is selection of at least proper transfer

functionsW1 andW2, such that Qqr has the desired structure and that error due to unmeasured

input td has the desired dynamic properties. The 2-dof controller design approach can be

directly applied as shown in [9,10]. Here we would like to apply a slightly different idea.

Without loss of generality, we will assume actuator current loop dynamics Wc and plant

dynamics P are known.

Figure 4.24 Enforcement of the transfer function Qqr by a functional observer


From the block diagram in Figure 4.24 the following relations can be obtained

KTWciref � td ¼ P� 1q

KnW1iref �W2q ¼ Knz

iref ¼ ides þ z

ð4:110Þ

Here ides stands for input generated by outer loop not shown in Figure 4.24, iref stands for

a reference input to the actuator current controller, z stands for an intermediate variable,W1 and

W2 are the transfer functions to be determined and P� 1 ¼ as2 stands for the plant transfer

function.

By eliminating z from (4.110) and rearranging expressions, the position q as function of the

reference current ides and the disturbance td can be expressed as

q ¼ KTWc

ð1�W1ÞþKTK � 1n WcW2P

Pides � 1�W1


Ptd ð4:111Þ

The representation of the plant output in the form q ¼ PnQqrKnides �PQqttd is a starting

point in the selection of the transfer functions W1 and W2.

By inspection from (4.111) we can write

Qqr ¼ KTK� 1n WcPP

� 1n


Qqt ¼ 1�W1


ð4:112Þ

Solving (4.112) for W1 and W2 yields

W1 ¼ 1�KTK� 1n PP� 1

n WcQqtQ� 1qr

W2 ¼ Q� 1qr ð1�QqtÞP� 1

n

ð4:113Þ

Expressing z from (4.110) and (4.112) yields

Knz¼ KnW1iref �W2q

¼ tdis þ Qqr �ð1�QqtÞQqr

P� 1n q�ðKTWc �KnW1Þiref ð4:114Þ

The output z stands for the estimation of disturbance and the difference in current controller

transfer function. Ideally, for full compensation onewould desireQqr ¼ 1 andQqt ¼ 0. Then, z

is reduced to a generalized disturbance in the plant with partially compensated dynamics of the

current control loop Knz ¼ tdis �ðKTWc �KnW1Þ iref .In Equation (4.113), the transfer functionsQqr andQqt are design parameters. It is realistic to

require thatW1 andW2 areat least proper stable transfer functions.Fromthe inputoutput relation

q ¼ PnQqrKnides �PQttd follows that Qqr should be selected as a proper transfer function as

close as possible to one in the desired frequency range. The transfer function Qqt should be a

proper transfer functionclose tozero in frequencybandwidthdeterminedbydesireddisturbance

compensation. The transfer functions Qqr and Qqt should not have common poles and zeroes.


For example, let Wc, Qqr and Qqt be selected as

Wc ¼ g

sþ g

Qqr ¼ 1

Qqt ¼ 1� g2

s2 þ g1sþ g2

ð4:115Þ

Insertion of these values into (4.113) yields

W1 ¼ KTK� 1n PP� 1

n 1� g

sþ g1� g2

s2 þ g1sþ g2

0@

1A

24

35

W2 ¼ g2s2

s2 þ g1sþ g2

an

Kn

¼ g2 1� g1sþ g2

s2 þ g1sþ g2

0@

1A an

Kn

ð4:116Þ

If the parameters of the plant are known, W1 and W2 in (4.116) can be easily realized.

In general, if the current loop dynamicsWc, the exact actuator gain and the plant dynamicsP

are not known, then, in calculating W1 and W2 the nominal current loop dynamics Wcn, the

nominal plant dynamics Pn and the nominal actuator gainKn are used. Insertion ofWcn,Kn and

Pn instead of the Wc, KT and P into (4.113) yields

W1 ¼ 1�WcnQqtQ� 1qr

W2 ¼ Q� 1qr ð1�QqtÞK � 1

n P� 1n

ð4:117Þ

Plugging (4.117) into the first row of (4.111) yields

q ¼ Pn

bWCQqr

bWCð1�QqtÞþQqtKni

des �PQqt

bWCð1�QqtÞþQqttd

bWC ¼ KTWcPðKnWcnPnÞ� 1

ð4:118Þ

Here bWC stands for the compensation ratio – in this case a transfer function. The poles of

this transfer function are poles ofWc and zeros ofWcn – thus ifWcn has right half plane zeros,

then bWC is unstable. In most of the practical systems Wc and Wcn are first-order filters.

The plant output can be expressed as

q ¼ PnQ*qrKni

des �PQ*qttd ð4:119Þ

The transfer functionsQ*qr andQ

*t characterizing deviation of the compensated plant from its

ideal structure are

Q*qr ¼

bWCQqr

bWCð1�QqtÞþQqt

Q*t ¼

Qqt

bWCð1�QqtÞþQqt

ð4:120Þ


The result is similar to other cases discussed so far. IfQqr is a proper transfer function, then in

the span of frequencies in which Qqt � 0 the output does not depend on disturbance, thus the

design conditions are met. To illustrate the changes due to the mismatch of the plant and

the current loop dynamics the Bode plot diagrams of the Q*qr and Q*

qt for mismatch in plant

inertia andmismatch in current loop dynamics are shown in Figures 4.25 and 4.26, respectively.

The parameters, current loop dynamics and the desired system behavior are given

by KT ¼ aKn; an ¼ aa; P� 1 ¼ as2; P� 1n ¼ ans

2; Kn ¼ 0:85N �m=A; an ¼ 0:1 kg �m2;g ¼ 600; gc ¼ g; gcn ¼ 300 and transfer functions

Wc ¼ gc

sþ gc;Wcn ¼ gcn

sþ gcn;Qqt ¼ 1� g

sþ g

�2

; Qqr ¼ 1

The results show that thechange in current loopdynamics has very similar effects to a change in

the plant parameters.

4.7 Properties of Measurement Noise Rejection

The control input, the position and the velocity are measured variables used in the design of the

disturbance observers. The control input is generated within the controller and it can be

assumed noise-free. The position and velocity measurements present a different situation.

Position transducers mostly provide discrete position information. In order to derive a velocity

signal a rate of change of positionmust be determined – thus differentiation needs to be applied.

As a result both the position and the velocity measurement signals are corrupted by noise jðtÞ.

Figure 4.25 Bode plots of transfer functions Q*qr and Q*

qt for PP� 1N ¼ 5:0 and an ¼ 0:1 kg�m2

(a) for PP� 1N ¼ 0:2 (b) with mismatch in the current loop dynamics gcn=gc ¼ 0:5


Assume the position information is corrupted by noise and ½qðtÞþ jðtÞ� is used instead of theexact position qðtÞ during construction of the disturbance observer, as shown in Figure 4.27.

The relations describing the operation of the system shown in Figure 4.27(a) can be derived as

½ðtdes þ zÞ� td �P ¼ q

½ðtdes þ zÞ�P� 1n ðqþ jÞ�Q ¼ z

ð4:121Þ

Expressing z ¼ P� 1qþ td � tdes and inserting it into the second expression in

(4.121) yields

q ¼ PnQqrtdes �PQqttd �Qqjj

Qqr ¼ 1


Qqt ¼ P� 1Pnð1�QÞP� 1Pnð1�QÞþQ

Qqj ¼ Q


ð4:122Þ

Here Qqr stands for the input control force to the nominal plant acceleration transfer

function, Qqt stands for the disturbance to the acceleration transfer function and Qj stands for

the noise to the output transfer function. It is easy to verify that the disturbance and the noise

Figure 4.26 Bode plots of transfer functions Q*qr and Q*

qt for PP� 1N ¼ 5:0 and an ¼ 0:1 kg�m2 (a) for

PP� 1N ¼ 0:2 (b) with mismatch in the current control loop dynamics gcn=gc ¼ 1:5


transfer functions satisfy Qqt þQj ¼ 1. Due to this relation the compensation of disturbance

and the noise cannot be selected independently. This is similar to the relationship held by the

sensitivity and complementary sensitivity transfer functions. In the design a trade-off between

the disturbance rejection bandwidth and the noise rejection cannot be avoided. In most cases

the low frequency disturbances need to be compensated, thusQqt is required to be small in the

low frequency range and Qj is small in the high frequency range.

The disturbance and the noise transfer functions for a system with actuator shown in

Figure 4.27(b) can be expressed as

Qqt ¼ KnK� 1T P� 1Pnð1�QÞ


Qj ¼ Q


ð4:123Þ

These two transfer functions also satisfy Qqt þQj ¼ 1. As an illustration the transfer

functionsQqt andQj for the position or the velocity measurement based disturbance observers

with the disturbance models _td ¼ 0 and €td ¼ 0 are shown in Table 4.1, where they are given as

functions of the parameter mismatch P� 1Pn ¼ a� 1. Just by replacing the inertia mismatch

ratio a by the inertia and actuator gain mismatch ratio KnK� 1T P� 1Pn ¼ b� 1, Table 4.1 can be

used for plants with current input and mismatch of both inertia and actuator gain.

As an illustration the Bode plots for transfer functionsQqt andQj are plotted in Figure 4.28.

The left column in Figure 4.28 shows Qqt and Qj for the conditions shown in the first row of

Table 4.1 [Q ¼ g=ðsþ gÞ, PP� 1N ¼ 0:2, an ¼ 0:1 kg �m2, _td ¼ 0]. In the right column in

Figure 4.27 Noise input to plant (3.1) with disturbance observer but without actuator (a) and noise input

to plant (3.1) with actuator and disturbance observer (b)


Figure 4.28 Qqt and Qj are shown for the conditions in the third row of Table 4.1

[Q ¼ l1=ðs2 þ l2sþ l1Þ,PP� 1N ¼ 0:2, an ¼ 0:1 kg �m2,measurement ðt; qÞ, disturbancemodel

_td ¼ 0]. In both cases the parameters of the disturbance filter are g ¼ 600, l1 ¼ g2, l2 ¼ 2g.

Bode plots clearly show the dependence Qqt þQj ¼ 1 and the shift in bandwidths due to the

larger inertia of the plant.

Up to now we have discussed the observer design issues and the system dynamics for

systems with disturbance compensation. Now we are ready to discuss the implementation of

control issues in systems with compensated disturbance. Knowing that almost the same

observer structure may estimate the disturbance, the equivalent force and/or the equivalent

acceleration allows different solutions for the structure of the control system. Two solutions are

obvious:

Figure 4.28 Bode plots of Qqt and Qj with measurements t; v and disturbance model _td ¼ 0 in (a) and

for measurements t; q and disturbance model _td ¼ 0 in (b). In both diagrams g ¼ 600

Table 4.1. Disturbance and noise rejection for the system shown in Figure 4.27(a) and P� 1Pn ¼ a� 1

Measured

variables

Disturbance

model

Observer

filter TF Q

Disturbance

transfer function Qqt

Noise transfer

function Qj

t; v _td ¼ 0 Q ¼ g

sþ gQqt ¼ s

sþagQj ¼ ag

sþag

t; v €td ¼ 0 Q ¼ l2sþ l1

s2 þ l2sþ l1Qqt ¼ s2

s2 þal2sþal1Qj ¼ al2sþal1

s2 þal2sþal1

t; q _td ¼ 0 Q ¼ l1

s2 þ l2sþ l1Qqt ¼ s2 þ l2s

s2 þ l2sþal1Qj ¼ al1

s2 þ l2sþal1


. Use the observer for disturbance compensation and then design a controller for the

compensated plant.. Use the equivalent force observer for disturbance and equivalent acceleration compensation

and design a controller that will enforce convergence to the desired equilibrium.

The difference between these two solutions is not just structural. The disturbance observer

design does not require information on the control error for its realization, thus it can be used in

open loop systems.

The equivalent force observer and the equivalent acceleration observer need information

on the control error, thus they assume the closed loop system. Both have meaning only in the

context related to the closed loop system dynamics for a specified control goal.

4.8 Control of Compensated Plant

In this sectionwewill revisit controller design for output control tasks as specified in Equations

(3.6)–(3.9). Here, the acceleration controller is realized by applying estimated disturbance

feedback, thus the compensated plant is described as in (4.96). Comparison with the results

obtained in Chapter 3 illustrates the properties of the acceleration control loop implemented

using estimated disturbance and the peculiarity of controller design in such a case.

Here designwill follow the same procedure as inChapter 3. The dynamics of the generalized

control errors are as in (3.22) or (3.25) for the acceleration and the force input respectively. For

simplicity we will first analyze systems with asymptotic convergence. A Lyapunov function

candidate is selected as in (3.33)V ¼ s2=2 and control should be derived to enforce_V ¼ � 2 kV ; k > 0. Then insertion of (4.96), (4.97) and (3.22) into sð _sþ ksÞ ¼ 0 yields

sð _sþ ksÞ ¼ s½gð€q� €qeqÞþ ks�¼ sfg€qdes � g½€qeq þ a� 1

n pðQ; tdisÞ� þ ksg ¼ 0ð4:124Þ

For s 6¼ 0 the desired acceleration €qdes is derived as

€qdes ¼ €qeq � g� 1ks|fflfflffl{zfflfflffl}convergence acc:

þ a� 1n pðQ; tdisÞ|fflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflffl}

acc due to estimation error

ð4:125Þ

The desired acceleration has three components: (i) the equivalent acceleration €qeq, (ii) the

convergence acceleration €qcon ¼ � g� 1ks and (iii) the disturbance estimation error induced

acceleration €qerror ¼ a� 1n pðQ; tdisÞ. The equivalent acceleration and the convergence accel-

eration are the same as for the uncompensated system. The disturbance estimation error

induced acceleration is a component specific to the systems with disturbance observer. The

desired force tdes ¼ an€qdes is

tdes ¼ an½€qeq þ a� 1n pðQ; tdisÞ� � ang

� 1ks ð4:126Þ

The disturbance compensation error pðQ; tdisÞ is not known and the desired acceleration

can be implemented using available information on the equivalent acceleration and the

generalized control error, thus the implemented desired acceleration is €qdes ¼ €qeq� g� 1ks.


This implementation yields closed loop dynamics

_sþ ks ¼ a� 1n pðQ; tdisÞ ð4:127Þ

The left hand side in (4.127) – the dynamics of the closed loop system – is the same as in

(3.64), obtained for a system with the ideal disturbance compensation. Here, the right hand

side depends on the error in the disturbance compensation. From (4.127) the error due to the

disturbance estimation in a closed loop system can be evaluated. The steady-state value of the

generalized error is s ¼ k� 1a� 1n pðQ; tdisÞ, thus the disturbance observer and/or the conver-

gence term can be redesigned to satisfy the system specification. The integration loop inserted

by the disturbance observer, as shown in (4.94), in most of the cases is sufficient for

compensation of the disturbance estimation error in a closed loop.

The structure of the system with disturbance observer and control (4.125) is shown in

Figure 4.29(a) and its equivalent closed loop representation (4.127) is shown in Figure 4.29(b).

Transients in a generalized error s are governed by the first-order dynamics (4.127). The

dynamics of the plant state coordinates can be derived using the dependence of a generalized

error s on the output y, its reference yref and dynamics (4.127). For example, if output

is yðq; _qÞ ¼ c2qþ c3 _q with c2; c3 > 0 or c2; c3 < 0, substituting s ¼ y� yref into (4.127)

yields

c3€qþðc2 þ kc3Þ _qþ kc2q ¼ ð _yref þ kyref Þþ a� 1n pðQ; tdisÞ ð4:128Þ

The disturbance estimation error dependent term a� 1n pðQ; tdisÞ appears in right hand side of

Equation (4.128). That is the only difference from the ideal system dynamics (3.67). The other

Figure 4.29 Control system with a disturbance observer and evaluation of the equivalent acceleration.

(a) Plant with disturbance observer and calculation of the equivalent acceleration. (b) Closed loop

dynamics as shown in (4.127)


properties of the system are the same. Systems with large inertia will tend to be less sensitive to

the disturbance observer error. Equivalent results can be obtained for finite-time convergence.

Example 4.7 Position Control of Compensated Plant The plant and the parameters

are the same as in Example 4.6. The plant is simulated as a€q ¼ t� tdis,tdis ¼ Da€qþ bðq; _qÞþ gðqÞþ text. The input torque is t ¼ KTi with current loop dynamics

di=dt ¼ gcðiref � iÞ, gc ¼ 360. The initial condition in position is qð0Þ ¼ 0:025 rad. The

reference position is qref ðtÞ ¼ 0:1½1þ 0:25 sin ð12:56tÞ� rad and output yðq; _qÞ ¼ 25qþ _q.The reference output is yref ¼ 25qref þ _qref and the control error ey ¼ 25eq þ _eq, eq ¼ q� qref .

The equivalent acceleration €qeq ¼ _yref � 25 _q is calculated exactly and applied as a part of

the control input

t ¼ tdis þ anð€qeq þ €qconÞ ð4:129ÞThe disturbance observer (4.20) with g ¼ 600 is applied for a generalized disturbance tdis

estimation. The limits on control and convergence acceleration are j€qdesj � 150 rad=s2;jtj � 100N �m.

In Figure 4.30 the transients with convergence acceleration €qcon ¼ � 100ey are shown. The

left column shows the output reference yref , the output y, the output control error ey and the

Figure 4.30 Changes in reference yref , output y and output error ey, _eyðeyÞ diagram for output

y ¼ 25qþ _q, reference yref ¼ 25qref þ _qref , position q, reference position qref , position error eq and

_eqðeqÞ diagram. The convergence gain is k ¼ 100 and a ¼ 1


_eyðeyÞ diagram. The right column shows the position reference qref , its actual value q, the

position control eq error and the _eqðeqÞ diagrams.

Figure 4.31 shows the same transients as in Figure 4.30 but with convergence acceleration

€qcon ¼ � 100 ey�� 0:5sign ðeyÞ. The left column shows the output reference yref , the output y,

the output control error ey and the _eyðeyÞ diagram. The right column shows the position

reference qref , position actual value q, the position control error eq and the _eqðeqÞ diagram. The

controller parameters are a ¼ 3=4 and k ¼ 100.

The results are almost identical with those obtained for an ideal system. There is a small

difference in the reaching mode, which here is shorter due to higher initial estimates of the

disturbance force.

4.8.1 Application of Estimated teq and €qeq

If the control input is implemented with the equivalent force estimation teq instead of its exactvalue, the input force and the closed loop dynamics can be expressed as

t ¼ teq � ang� 1ks ¼ teq þ tcon

_sþ ks ¼ ga� 1n ðteq � teqÞ ¼ ga� 1

n peqt ðQ; teqÞð4:130Þ


y ¼ 25qþ _q, reference yref ¼ 25qref þ _qref , position q, reference position qref , position error eq and

_eqðeqÞ diagram. The convergence gain is k ¼ 100 and a ¼ 0:75


The closed loop dynamics now depend on the equivalent force estimation error peqt ðQ; teqÞ.The equivalent force observer, similar to the disturbance observer, inserts an integrator into the

control loop. With appropriate changes of variables the expression (4.94) can be applied to

Figure 4.32(a). For the equivalent force observer with a first-order filter with gain g the control

force can be expressed as

t ¼ � an

gð _sþ ksÞ� gan

g

ðt0

ð _sþ ksÞ dz ð4:131Þ

The integral actionwould eventually eliminate steady-state error and enforce ð _sþ ksÞ! 0,

thus the dynamics of the closed loop system are the same as in the systems with only

disturbance compensation and direct calculation of the equivalent acceleration. The difference

comes from the requirement that generalized output control error is available as the input to the

equivalent force observer.

The closed loop dynamics of with the equivalent force observer is shown in Figure 4.32(b).

The difference between the structures shown in Figure 4.29(b) and in Figure 4.32(b) is just in

the input representing estimation error.

As the last step, let us analyze implementation of the control system as shown in

Figure 4.33(a). Here, two observers (for the plant disturbance tdis and for the equivalent

acceleration €qeq) are applied and the control input is

t ¼ tdis þ an€qeq � ang

� 1ks ð4:132Þ

Figure 4.32 Structure of an output control system with an equivalent control observer. (a) Plant output

control with equivalent force observer. (b) Closed loop dynamics as in (4.130)


Inserting (4.132) into (4.95) yields the dynamics of the closed loop

_sþ ks ¼ ga� 1n p

eq€q ðQ; €qeqÞ ð4:133Þ

The left hand side in (4.133) is the same as in previous cases. The right hand side depends on

the equivalent acceleration peq€q ðQ; €qeqÞ estimation error since the disturbance estimation error

is included in the estimation of the equivalent acceleration.

From a functional point of view all three structures are resulting in the same closed loop

dynamics. Differences from application point of view are interesting to observe.

Implementation as shown in Figure 4.29 assumes a known equivalent acceleration, thus it

requires prior knowledge on the reference and its derivatives. Closed loop motion depends

only on disturbance observer error. This topology allows the application of different outer

loop controllers enforcing the convergence acceleration. From this point of view it is a basic

implementation of the acceleration control. It establishes a robust and yet very simply

implemented acceleration control loop and leaves some freedom in selecting the

convergence acceleration by applying different control system frameworks. The simplest,

yet very reliable solution is the application of a PD controller with reference acceleration as

a feedforward term.

The closed loop system with equivalent force estimation compensates both the disturbance

and the equivalent acceleration as one term. It can be realized only as part of the closed loop

structure. The relative degree of the control error dynamics would dictate the characteristics of

the equivalent force observer. The solution is applicable to problems being specified by the

generalized error having relative degree one. In addition it requires a closed loop system and its

tuning may prove complex for the inexperienced user.

Figure 4.33 Structure of a control system with a disturbance observer and an equivalent acceleration

observer. (a) System with estimation of disturbance and equivalent acceleration, and (b) closed loop

dynamics as in (4.132)


Using separate observers for the disturbance and equivalent acceleration gives flexibility

in realization. It offers a robust implementation, setting the robust acceleration loop

separately and then using the equivalent acceleration observer as a part of the outer loop

controller.

For a bounded control input, as shown in Chapter 3, the domain of validity of

transients (4.127), (4.130) and (4.133) is determined by the disturbance and the bounds

on the input force. In all three cases the bounded control input has a form as in (4.134)–

(4.136). The expression in (4.134) is a generic form of the acceleration control and (4.135)

and (4.136) are just different implementations of the same structure. Implementation as in

(4.135) derives the desired acceleration from the two terms – estimated equivalent

acceleration and convergence acceleration. In the (4.136) the estimation term includes

the equivalent acceleration so the remaining input is just the convergence acceleration.

Application of one structure or another would depend on the circumstances and the ability

of the designer.

t ¼ satðan€qdes þ tdisÞ tdis ¼ Qtdis;

€qdes ¼ an€qeq � ang

� 1ksð4:134Þ

t ¼ sat½anð€qcon þ €qeqÞþ tdis� tdis ¼ Qtdis; €q

eq ¼ Q€qeq;

€qdes ¼ � ang� 1ks

ð4:135Þ

t ¼ satðan€qcon þ teqÞ teq ¼ Qteq;

€qdes ¼ � ang� 1ks

ð4:136Þ

If control output y is a function of only position to enforce convergence, the desired

acceleration must include velocity feedback. This leads to the generic structure of the

compensated system controller as

t ¼ satðan €qdes þ tdisÞ tdis ¼ Qtdis;

€qdes ¼ KPeq þKD _eq þ €yrefð4:137Þ

Here eq ¼ yref � yðqÞ is the control error, _eq is the control error derivative, KP;KD > 0 are

design parameters and yref is the reference output. The motion of such a system is discussed in

detail in Section 3.5.

Example 4.8 Position Control with Equivalent Acceleration Observer This example

shows the behavior of the output control system with both disturbance force and equivalent

acceleration estimated by appropriate observers. The plant, reference and output are the same

as in Example 4.7. Here the control input is selected as

t ¼ tdis þ anð€qeq þ €qconÞ ð4:138Þ

The disturbance tdis and equivalent acceleration €qeq are estimated by observers discussed in

detail in Examples 4.1 and 4.5. Filter gain is g ¼ 600.


In Figure 4.34, the transientswith convergence acceleration €qcon ¼ � 100 ey�� 0:5signðeyÞ are

shown. In the left column, the reference yref , the output y, the control error ey and the _eyðeyÞdiagram are shown. In the right column, the position reference qref , the position q, the position

control eq error and the _eqðeqÞ diagrams are shown with a ¼ 3=4 and k ¼ 100. The initial

condition is qð0Þ ¼ 0:025 rad. The reference position is qref ðtÞ ¼ 0:1½1þ 0:25sin ð12:56tÞ� radand the output is yðq; _qÞ ¼ 25qþ _q. The reference output is yref ¼ 25qref þ _qref and the controlerror ey ¼ 25eq þ _eq, eq ¼ q� qref .

In Figure 4.35, the transients with convergence acceleration €qcon ¼ � kc� 11 ey

�� m�

sign ðeyÞ; m > 0 are shown. In the left column, the reference yref , the output y, the control error

ey and the _eyðeyÞ diagram are shown. In the right column, the position reference qref , the

position q, the position control eq error and the _eqðeqÞ diagrams are shown. The controller

parameters are k ¼ 100 and m ¼ 0:1.This example illustrates the applicability of the estimation of equivalent acceleration and

disturbance in closed loop control.


y ¼ 25qþ _q, reference yref ¼ 25qref þ _qref , position reference qref , position q, position error eq and

_eqðeqÞ diagram. The convergence gain is k ¼ 100 and a ¼ 0:75. Both the equivalent acceleration and thedisturbance torque are estimated


References

1. Tsai, M.C. and Tomizuka, M. (1988) Model reference adaptive control for robot manipulators-continuous time

theory and digital implementation. Symposium on Robotics, 11, 199–209.

2. Cortesao, R. (2007) On Kalman active observers. Journal of Intelligent and Robotic Systems, 48(2),

131–155.

3. Utkin, V., Guldner, J., and Shi, J.X. (1999) SlidingMode Control in Electromechanical Systems, 2nd edn, Taylor

and Francis, London.

4. Davila, J., Fridman, L., and Poznyak, A. (2006) Observation and identification of mechanical systems via second

order sliding modes. International Journal of Control, 79(10), 1251–1262.

5. Ohishi, K., Ohnishi, K., and Miyachi, K. (1983) Torque –speed regulation of DC motor based on load torque

estimation. Proceedings of the IEEJ International Power Electronics Conference, pp. 1209–1216.

6. Nakao, M., Ohnishi, K., and Miyachi, K. (1987) A robust decentralized joint control based on interference

estimation. IEEE International Conference on Robotics and Automation, 4, 326–331.

7. Ohnishi, K., Shibata, M., and Murakami, T. (1996) Motion control for advanced mechatronics, Mechatronics.

IEEE/ASME Transactions on Mechatronics, 1(1), 56–67.

Figure 4.35 Evolution of reference yref , output y and output error ey, _eyðeyÞ diagram for output

y ¼ 25qþ _q, reference yref ¼ 25qref þ _qref , position reference qref , position q, position error eq and _eqðeqÞdiagram. The controller parameters are k ¼ 100 and m ¼ 0:1. Both the equivalent acceleration and the

disturbance are estimated


8. Izosimov, D.B. (1980) Parallel correction method and its application in SMC (in Russian), in New Directions in

Theory of Variable Structure Systems, IPU, Moscow.

9. Ohishi, K., Ohba, Y., and Katsura, S. (2008) Kinematics and dynamics of motion control based on acceleration

control. Electronics and Communications in Japan, 91(6), 12–22.

10. Tasfaye, A., Lee, H.S., and Tomizuka, M. (2000) A sensitivity approach to design of a disturbance observer in

digital motioncontrol systems. IEEE/ASME Transactions on Mechatronics, 5(1), 32–38.

Further Reading

Kaneko, K., Ohnishi, K., and Komoriya, K. (1994) A design method for manipulator control based on disturbance

observer. International Conference on Intelligent Robots and Systems, 2, 1405–1412.

Katsura, S., Matsumoto, Y., and Ohnishi, K. (2006) Analysis and experimental validation of force bandwidth for force

control. IEEE Transactions on Industrial Electronics, 53(3), 922–928.

Murakami, T. and Ohnishi, K. (1990) Advanced motion control in mechatronics – a tutorial. Proceedings of the IEEE

Workshop on Intelligent Motion Control, vol. 1, pp. 9–17.

Ohnishi, K. (1995) Industry applications of disturbance observer. International Conference on Recent Advances in

Mechatronics, pp. 72–77.

Yamada, K., Komada, S., Ishida, M., and Hori, T. (1996) Characteristics of servo system using high order disturbance

observer. Proceedings of the 35th Conference on Decision and Control, pp. 3252–3257.


5

Interactions and Constraints

The measure of mechanical interaction between bodies is the force acting on the interaction

point. The geometry of the surface in the contact point determines the direction of the

interaction force. The dynamics of the interaction force depend on the properties of the bodies

in the interaction point. In further text here it will be assumed that the interaction is the result

of changes in the motion of the plant and the interaction occurs with an object situated within

the plant workspace (from now on referred to as the environment).

The control of interactions is a central point of the natural behavior of motion control

systems. Pure position control is not suitable for solving motion control problems due to the

uncertainties in location and the movement of objects. In many cases position is the result of

interaction of bodies andmanipulation needs to be guided by interaction forces. The interaction

forces may be treated in many different aspects depending on the properties of the bodies in

contact and the desired task. One may, as discussed in trajectory tracking tasks, just reject

interaction force and treat it as a part of disturbance. Another possibility is to maintain a certain

profile of the interaction force while controlling movement of the system. Depending on the

nature of the interaction, forces may be resisting motion (perception that the system is pushing

the object – body) or the forcemay be a pulling system – thus acting as a force in the direction of

the motion. Controlling a system in such a way that it behaves as a specified mass–damper–

spring (or in general as a specified mechanical impedance) against the interaction force opens

the possibility to modulate the perception of having contact with a specific environment.

Another aspect of interaction control is the establishment of a functional relation among

systems in such a way that the interaction with the environment experienced by one system is

mirrored by another system in contact with another environment. The configuration of bilateral

systems is a special case in which a human operator is on one side (called the master side). The

system in contact with the environment (called the slave side) has a role of extended tool and

it tracks motion dictated from the master side while mirroring interaction information (force)

to the master side. There are many other scenarios in which the control of interaction force is

essential in solving a specific task.



5.1 Interaction Force Control

The interaction force depends on the relative positions of the system and object–environment

and it exists only if they are in contact. In some cases the interaction force may be a fictitious

item added to the systems in order to establish the desired way of interaction. The deformation

of the object and its motion results in the interaction force as shown in Figure 5.1(a). The

variable structure of the system control plant–environment when contact exists and without

contact requires careful evaluation of the controller suitable for application in systems with

a variable structure. It seems natural to have different controllers for: (i) the position control

and (ii) the force control. Application of the position control is formotionwithout contact with

the environment (free motion), and the force control is applied while in contact with the

environment. The problem of transition between position control and force control depends

on the state of the system. Closely related to this is the control of the impact force especially if

contact is with a hard object.

In order to understand the peculiarities of force control the dynamics of the control error

must be established first. For this, a model of interaction force is needed. Any model will

require information on the relative position of the system and the interaction object in the

contact point. For the purposes of the control design a general second-order mass–spring–

damper model is the most complete description of the interaction. It includes the forces due to

motion of the object and the spring–damper component due to object deformation. Assume

Me is a mass of an object,Ke andDe are the spring and damper coefficients in the contact point;

qe is the equilibrium position of an object for which the interaction force is equal to zero.

In general a priori knowledge of the object parameters is very unlikely. The parametersMe,Ke

and De are assumed unknown with known upper and lower bounds. The interaction force

can be modeled as

τe ¼Me €q� €qeð ÞþDe _q� _qeð ÞþKe q� qeð Þ if bodies are in contact

0 if bodies are not in contact

(ð5:1Þ

In some application the position of the environment is taken as the zero position. Then

expression in (5.1) depends only on the changes of the system state and the properties of the

object in the interaction point. It should be noted here that such a definition of the interaction

force assumes that the force is the result of deformation of the object. In further analysis wewill

assume that interaction force (5.1) is bounded.

The dynamics of the 1-dof plant with actuator current as the control input, in contact with

the environment, are shown in Figure 5.1(b). Structurally the interaction force acts as an input

Figure 5.1 Interaction with compliant environment: (a) deformation and forces, (b) system structure


disturbance. Variation of the structure is shown by a block with a switch, whose on–off state

depends on the contact.

In most applications the interaction processes can be modeled as a spring–damper system.

Coefficients Kmine � Ke � Kmax

e and 0 � De � Dmaxe may vary significantly depending on the

properties of the bodies in the contact point. In general, Ke is strictly positive and could be

a function of position (contact modeled as a nonlinear spring). The damping coefficient

represents losses due to body deformation in the contact point and for some systems it may be

very close to or equal to zero – thus perfect a lossless springwill represent the contact properties

and the model (5.1) will degenerate to

τe q; qeð Þ ¼ Ke q� qeð Þ if bodies are in contact


�ð5:2Þ

In the first approximation of the complex physical nature of the interaction forces, a single

body point contact results in the interaction force being collinear with normal on the contact

surface in the contact point. For a 1-dof system having motion in a direction normal to the

contact surface the interaction force can be just algebraically added to the other acting forces

as an unknown term. The dynamics of the system shown in Figure 5.1 in contact become

a qð Þ€qþ b q; _qð Þþ g qð Þþ τext þ τe q; qeð Þ ¼ ττ ¼ KTi

refð5:3Þ

Themeaning of the parameters and thevariables inmodel (5.3) is already given inChapter 3.

In (5.3) the external force τext and the interaction force τe q; qeð Þ are treated separately. In some

cases the independent external force τext is zero, thus the interaction force is the only externalforce acting in the system. The input force is expressed as the product of the actuator gain

KT and the reference current iref . In some applications the dynamics of the current control loop

will be added to the structure. If not clear from the context we will note such a change in the

system (5.3) description.

A trajectory tracking problem by considering interaction force τe as a part of the generalizedplant disturbance has been discussed in Chapters 3 and 4. The structures of the disturbance

observer discussed inChapter 4 are directly applicable in this case also. The closed loopmotion

is robust against all disturbances and thus rejects the influence of the interaction force as well.

Inmany real situations robustness against interaction force is not a desired feature of system

behavior. In contract with fragile objects or for protection of the system, the desired system

behavior requires a modification of motion while in contact with the environment. The nature

of the modification may differ, but as a general rule, it will require an adjustment of the relative

position and velocity of the systemwith respect to the position and velocity of the environment

in such a way that the interaction force has the desired profile.

If interaction force control or modification of the system motion is required, then the

interaction force should be measurement or estimated. Most of the force sensors are strain to

signal converters and in a first approximation can be modeled as a spring with high stiffness.

Force measurement is corrupted by noise and inmost cases has a limited frequency bandwidth.

As shown in Chapter 4 the interaction force can be estimated using disturbance observer-like

structures. In some cases application of the force observer instead of a force transducer may be

preferable due to the higher bandwidth and better noise characteristics [1,4,5].

Interactions and Constraints 177

5.1.1 Proportional Controller and Velocity Feedback

We will begin analysis of interaction force control by presenting a solution that, in a sense, is

an extension of the position control discussed in Chapter 4. Plant (5.3) with disturbance

feedback reduces to €q ¼ €qdes þ a� 1n p Q; τdisð Þ with the desired acceleration €qdes as the output

of the force controller and the disturbance compensation error p Q; τdisð Þ. From (5.3), the input

force is τ ¼ an€qdes þ τdis ¼ Kni

ref . The reference current iref is the sum of the current

proportional to the estimated disturbance τdis=Kn and the current proportional to the desired

acceleration τdes ¼ Knides ¼ an€q

des. As shown in Chapter 4, disturbance compensation realizes

an acceleration tracking loop, thus the outer loop controller enforces the tracking in the outer

control loop.

Let the control task be to maintain the desired time-dependent profile of the interaction

force. Assume contact is established and is stable – thus the structure of the system is not

changing. Let the desired force be a smooth bounded function τrefe with a smooth bounded

first-order derivative.

Let the interaction be with a stationary object, thus interaction force (5.1) does not depend

on acceleration and could be expressed as τe ¼ Ke q� qeð ÞþDe _q� _qeð Þ. In further analysis itwill be assumed that the interaction force is measured or estimated and qe and _qe are boundedcontinuous functions. This allows us to treat the force control problem in more general way

assuming contact with a bodymoving on trajectory qe tð Þ, thus force control may be formulated

in a similar way as position tracking problem. Indeed, with τe being output of the system,

the structure in Figure 5.1(b) is equivalent of the structures discussed in Chapter 3 with output

defined by function y q; _qð Þ which depends on position and velocity. The difference is that

parameters Ke;De are not design parameters but part of the plant–environment model, thus

controller must compensate their uncertainties.

Trajectory tracking in an acceleration control framework requires a PD controller to

stabilize system. With a spring–damper model of the interaction force, a simple proportional

controller with gainCf > 0 and desired acceleration €qdes ¼ Cf τref � τe� �

;Cf > 0 can stabilize

the closed loop system. Having the desired acceleration proportional to the force control error

and the disturbance feedback the control input becomes

τ ¼ Kniref ¼ τdis þ an€q

desF

¼ τdis þ anCf τref � τe� �

;Cf > 0

¼ τdis þ anCf τref �Keqe �De _qe� �� anCf KeqþDe _qð Þ

ð5:4Þ

Insertion of control force (5.4) into the plant dynamics (5.3) yields

€qþCfDe _qþCfKeq ¼ Cf τref þCfDe _qe þCfKeqe ð5:5Þ

By assumption, all coefficients in Equation (5.5) are strictly positive, thus (5.5) has stable

dynamics. The roots of the characteristic equation depend on the parameters of the

environment. The damping depends on the controller gain and the environment damping

coefficient De.

By simply adding velocity feedback to the system as shown in Figure 5.2 the damping can be

changed. Then, the desired acceleration


€qdes ¼ Cf τref � τe� ��Cv _q; Cf ;Cv > 0

τdes ¼ an€qdesF

ides ¼ an

Kn

€qdesF

ð5:6Þ

results in the closed loop dynamics

€qþ CfDe þCv

� �_qþCfKeq ¼ Cf τref þCfDe _qe þCfKeqe ð5:7Þ

Now damping is determined by CfDe þCv

� �. Assuming parameters of the plant and the

environment are constant, by taking a Laplace transformation of (5.7) the force control error

can be expressed as

eF ¼ 1�Cf

DesþKe

s2 þ CfDe þCv

� �sþCfKe

24

35τref

¼ þ 1�Cf

DesþKe

s2 þ CfDe þCv

� �sþCfKe

24

35 DesþKeð Þqe

ð5:8Þ

The proportional force controller with velocity feedback has complex second-order

dynamics. The closed loop poles and zeros depend on the properties of the object in the

interaction point. The dynamics of the interaction force control error due to changes in the

position of environment have three zeros. Two of them (one at origin, other at –Cv) being

commonwith reference to the error transfer function.A third zero at �KeD� 1e is defined by the

properties of the environment in the contact point. This makes very stringent requirements

on the control gain design if the environment position is changing (contact with a moving

object). In generalKe is high and consequentlyCf is generally low in order to avoid oscillations.

Example 5.1 Force Control with Velocity Feedback The goal of the examples in this

chapter is to expand on the results from Chapters 3 and 4 and to illustrate interaction control in

motion systems. The system presented in Example 3.1 with translational motionwill be used in

Figure 5.2 Structure of the interaction force control system


all examples in this chapter. In addition the actuator with current loop dynamics and possibly

nonlinear actuator gain KT is added to the system (3.1) description. The observers (for

disturbance, equivalent acceleration, external force, etc.) developed and tested in Chapter 4

will be applied. This allows reuse of the solutions developed in Chapters 3 and 4.

Just as a reminder, the model of the plant and parameters are rewritten in (5.9). The

dynamics, parameters and other conditions used in this example are:

a qð Þ€qþ b q; _qð Þþ g qð Þþ τext þ τe½ �|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}system disturbance¼τd

¼ τ

τ ¼ KTi

di

dt¼ gc iref � i

� � ð5:9Þ

In most of the examples the actuator gain KT is treated constant equal to its nominal value

KT ¼ Kn ¼ 0:85 N=A and the dynamics of the current loop are neglected. The actuator current

control loop is assumed as a first-order system with gain gc and the reference current iref as

input. Since the actuator current may not be available for measurement in most examples

the reference current is treated as the control input. The inertia is modeled as

a qð Þ ¼ 0:1 1þ 0:5 sin qð Þ½ � kg. Interaction force is now included in the dynamics of the system

and is modeled as

τe ¼Me €q� €qeð ÞþDe _q� _qeð ÞþKe q� qeð Þ if bodies are in contact


�ð5:10Þ

Here Me stands for the mass of the object, Ke and De stand for the spring and damper

coefficients in the contact point and qe is the equilibrium position of the object for which

the interaction force is equal to zero. CoefficientsMmine � Me � Mmax

e ,Kmine � Ke � Kmax

e and

0 � De � Dmaxe may vary significantly depending on the mass and properties of the bodies

in the contact point. In order to simulate the environment we will assume an object (with

parameters Me;De;Ke in interaction points) at position

qe tð Þ ¼ qe0 þ qe1 sin vqet� �

m ð5:11Þ

The coefficients qe0, qe1 and vqe will be set for particular experiments. The reference force

is defined as

τref tð Þ ¼ τref0 1þ τref1 sin vτtð Þh i

N ð5:12Þ

Coefficients τref0 ; τref1 ;vτ will be defined for each experiment.

In this example the proportional controller with disturbance feedback and additional

velocity feedback for faster damping of the transients will be illustrated. The control input

is designed as in (5.4)

τ ¼ Kniref ¼ τdis þ an€q

desF ;

€qdesF ¼ Cf τe � τref� �þCv _q

ð5:13Þ


Cf ;Cv > 0 are design parameters. Here €qdesF stands for the desired acceleration enforcing

the interaction force tracking. The disturbance τdis is estimated by disturbance observer as

shown in (4.65) with _q ¼ v; iref� �

as inputs and an;Kn; gð Þ as observer parameters, thus it

includes inertia variation force, actuator dynamics induced forces and the interaction force

_z ¼ g Kniref þ ang _q� z

� �; g > 0

τdis ¼ z� ang _qð5:14Þ

For a system with actuator, having nominal force constant Kn, the reference current can be

determined as

iref ¼ K � 1n τ ¼ K � 1

n τdis þ an€qdesF

� � ð5:15Þ

Transients are shown in Figure 5.3. Initial conditions are q 0ð Þ ¼ 0:0499 m, position of

environment qe tð Þ ¼ 0:05 m, reference force τref tð Þ ¼ 10 1þ 0:25 sin 6:28tð Þ½ �N, controller

Figure 5.3 Force control by proportional controller and disturbance feedback. Diagram shows the

position of the environment qe, the plant position q, reference force τref , interaction force τe, plant velocity_q ¼ v, force control error eF ¼ τe � τref , estimated disturbance τdis and plant input force τ. Environment

parameters are Me ¼ 0m, Ke ¼ 250 000Nm–1 and De ¼ 5 kg s–1. Controller parameters are Cf ¼ 20,

Cv ¼ 100, τmax ¼ 25 N


parameters Cf ¼ 20, Cv ¼ 100. The initial position is taken very close to position of

environment in order to avoid high impact forces. The parameters in the interaction point

are Me ¼ 0m, Ke ¼ 250 000Nm–1 and De ¼ 5 kg s–1. The disturbance observer filter gain is

g ¼ 1000. In the first row the position of environment qe, the plant position q, the reference τrefand interaction force τe are shown. In the second row the plant velocity _q ¼ v and the force

control error eF ¼ τe � τref are shown. In the third row the estimated disturbance τdis andthe plant input force τ are shown. The estimated disturbance includes both the acceleration

induced force and the interaction force. This is an interesting difference with respect to the

output control systems discussed in Chapters 3 and 4. Here the control variable is acting as

a part of the disturbance and its influence on the system dynamics is compensated by the

disturbance observer feedback. The role of control (5.l5) is then to establish the desired

dynamics of the control error.

The closed loop behavior is defined by (5.7). The characteristic equation is

s2 þ 200sþ 5 � 106 ¼ 0 ð5:16Þ

From (5.16) the roots of the characteristic equation can be derived as m1;2 ¼ � 100�j2233:8. The high frequency oscillations of impact force with fast convergence are obtained,

as can be observed in Figure 5.3.

5.1.2 Environment with Losses

The acceleration control method is so far discussed only for a general output control and

trajectory tracking problem. For its application in force control tasks we need to formulate

a force control problem in a suitable way. If the interaction force is modeled as τe ¼Ke q� qeð ÞþDe _q� _qeð Þ, the force tracking can be formulated in the same way as a general

output control problem (3.8). The force tracking is enforced if the systemmotion is constrained

to satisfy

SF ¼ q; _q : Ke q� qeð ÞþDe _q� _qeð Þ½ � � τrefe tð Þ ¼ eF τe; τrefe

� � ¼ 0� � ð5:17Þ

Plant (5.3) with disturbance feedback reduces to double integrator (4.96) with desired

acceleration €qdes as the output of the outer loop controller. Design of the force tracking

controller reduces to the selection of the desired acceleration enforcing the stability of motion

in manifold (5.17).

Conceptually (5.17) can be regarded as a position control in which a linear function of the

difference between the environment position and the systemposition should be kept equal to τe.That means that environment position tracking error is set to be equal to the reference

interaction force. Such an interpretation allows direct application of the result discussed in

Chapters 3 and 4.

The derivative of control error eF is

_eF ¼ De€q�De

1

De

_τrefe þ Ke

De

_qe þ €qe �Ke

De

_q

� ð5:18Þ


The equivalent acceleration enforcing zero rate of change of force control error is

€qeqFn ¼ €qe þKe

De

_qe � _qð Þþ _τrefe

De

ð5:19Þ

The equivalent acceleration depends on the system velocity, the parameters of the envi-

ronment, and the derivative of the desired force. ForDe ! 0 (5.19) gives large value for €qeqFn andfor systems havingDe ¼ 0 expression (5.19) is not applicable. Parameters of the environment

Ke and De are not known, thus €qeqFn cannot be directly calculated from (5.19). In the equivalent

acceleration observer (4.71) the environment damping coefficient De is input gain. Let the

environment damping coefficient be De ¼ Den þDDe, with nominal value Den > 0 assumed

known and the uncertainty defined by a bounded continuous function. The uncertainty induced

force DDe _q� _qeð Þ can be treated as a part of the equivalent acceleration. Then the derivative ofthe force tracking error on the trajectories of the system (5.3) with compensated disturbance

could be rearranged into

_eF ¼ Den €qdes � €qe �Ke

Den

_q� _qeð Þþ _τref

Den

� DDe

Den

€q� €qeð Þþ De

Denanp Q; τdisð Þ

�� ð5:20Þ

From (5.20) the equivalent acceleration is

€qeqF ¼ €qe �Ke

Den

_q� _qeð Þþ _τref

Den

� DDe

Den

€q� €qeð Þþ De

Denanp Q; τdisð Þ ð5:21Þ

Now estimation of the equivalent acceleration is straightforward. Insertion of the desired

acceleration €qdes and the force control error eF into the equivalent acceleration observer (4.70)yields

_z ¼ gdF €qdes þD� 1en gdFeF � z

� �€qeq

F ¼ z�D� 1en gdFeF ; gdF > 0

ð5:22Þ

The observer employs a first-order low pass filter. The estimated equivalent acceleration

includes the uncompensated disturbance estimation error p Q; τdisð Þ and the acceleration

induced by the variation of the parameters of environment. In addition it includes the

motion of the environment. Note that the output of the observer (5.22) stands for the estimated

equivalent acceleration only during contact with the environment. In the period of time when

contact does not exist the system is essentially an open loop (error is equal to the interaction

force reference, feedback force is equal to zero).

Selection of the desired acceleration would complete the design of the force tracking

controller. By selecting the Lyapunov function as V ¼ e2F=2 and the stability requirements

as _V � � 2kFVa; kF > 0;a ¼ 1, from eF _eF ¼ � 2kFV

a ¼ � kFe2F yields

eF _eF þ kFeFð Þ ¼ 0 ð5:23Þ

For eF 6¼ 0 the convergence acceleration can be from (5.23) determined as

€qconF ¼ �D� 1en kFeF ð5:24Þ


and the desired acceleration becomes

€qdes ¼ satq €qeq

F �D� 1en kFeF

�ð5:25Þ

The control force and the actuator reference current are

τ ¼ satT τdis þ an €qeq

F �D� 1en kFeF

�h i

iref ¼ satIτdisKn

þ an

Kn

€qeq

F �D� 1en kFeF

�24

35 ð5:26Þ

Here saturation functions sati �ð Þ; i ¼ q; T ; I are enforcing bounds on the corresponding

variables – the acceleration, the force and the current. These functions cannot be set

independently and in actual systems they should be matched to satisfy the functional relations

between these variables. The bounded control inputs, similarly as in the output control limit the

state space domain DF ¼ q; _q : _eF þ kFeF ¼ 0f g in which the desired dynamics (5.23) can be

enforced. Additionally, the capability of the system to compensate for the fast changes in

external force (especially impact forces with a hard environment) is greatly affected by these

limits.

Insertion of the control force or control current from (5.26) into plant dynamics (5.3) yields

the dynamics of the control error in domain DF ¼ q; _q : _eF þ kFeF ¼ 0f g

_eF ¼ Den €qdes � €qeqF� � ¼ Den €q

eq

F � €qeqF

�� kFeF

_eF þ kFeF ¼ DenpF QdF ; €qeqFð Þ

: ð5:27Þ

Dynamics (5.27) is enforced if control variables do not saturate. Here pF QdF; €qeqFð Þ stands

for the equivalent acceleration estimation error expected to be small in the desired span of

frequencies. Transients in the force control loop are governed by a first-order differential

equation with constant parameters. Dependence on the equivalent acceleration estimation

errors is consistent with results presented in Chapter 4 for output control.

By neglecting error due to the equivalent acceleration estimation the change of the position

can be obtained by inserting Ke q� qeð ÞþDe _q� _qeð Þ½ � � τrefe ¼ eF into (5.27) expressed as

DeD€qe þ Ke þ kFDeð ÞD _qe þ kFKeDqe ¼ _τref þ kFτref

Dqe ¼ q� qeð5:28Þ

Roots of the characteristic equation s2 þ Ke=De þ kFð Þsþ kFKe=De ¼ 0 are l1 ¼ �Ke=De

and l2 ¼ � kF , so the transient in (5.28) is stable and has one root defined by the ratio of spring

and damper coefficients of environment and another by the desired convergence of the

Lyapunov function. Both roots are real. If root l1 ¼ �Ke=De dictates a fast transient

(environment stiffness high, damping small), control input can reach saturation and unwanted

oscillations can appear. That may be avoided by selecting different value forDen in (5.20). This

has the same effect as adding a velocity feedback as additional dissipative element. The high

impact forces may still create some short oscillation due to the boundness of the control input.

The structure of the force control system is shown in Figure 5.4.


The dynamics of the force tracking error (5.27), as the design goal, are enforced but the

dynamics of the system state depends on the parameters of the environment. The same result

was shown for the output control discussed in Chapters 3 and 4.

In this analysis the environment is assumed ‘passive’ in the sense that it does not change its

trajectory on the occurrence of an interaction force. Only a controlled plant modifies its motion

to maintain the desired force. The position of the environment is not assumed stationary so this

algorithm can be applied in maintaining the desired interaction with a moving object.

Control (5.25) and transients (5.27) and (5.28) have the same form as that obtained for

position control. That is a natural consequence of the model of the interaction force and

consequently the structure of the manifold (5.17). The reference trajectory in position tracking

and the position of the environment in (5.17) play the same role. The difference appears in

the system operation. In the trajectory tracking a virtual force is acting to pull the system to

the trajectory – in the case of an active force the control system is pushing or pulling the

environment by a defined force.

Example 5.2 Force Control for Environment with Losses While discussing the output

control in Chapter 3, two transients had been observed – in the output and in the system

coordinates. It was shown that finite-time convergence of the output control error can be

enforced if the output is a function of position and velocity. This property can be used to achieve

a finite-time convergence of the force control error if interaction with the environment is

modeled by a spring–damper.

In this example the control plant and its parameters are the same as in Example 5.1.

The actuator current control loop dynamics are modeled with gc ¼ 1200. The disturb-

ance τdis is estimated by a disturbance observer as shown in (5.14) with _q ¼ v; iref� �

as inputs and an;Kn; gð Þ as observer parameters, thus it includes inertia variation

force, actuator dynamics induced forces and the interaction force. The position of

the environment is qe tð Þ ¼ 0:05þ 0:01 sin 3:14tð Þm and the reference force is

τref tð Þ ¼ 10 1þ 0:25 sin 12:56tð Þ½ �N. The interaction force is modeled as a spring–damper

system.

The desired acceleration €qdesF and the input force are selected as

€qdesF ¼ satq €qeq

F þ €qconF

�τ ¼ satT τdis þ an€q

desF

� � ð5:29Þ

Figure 5.4 Structure of a force control system with Den > 0


The equivalent acceleration is estimated by observer (5.22) with observer filter gain gdF ¼ 600

and Den ¼ 5 kg s–1. The convergence acceleration €qconF is selected to enforce finite-time

convergence

€qconF ¼ � kD� 1en eFj j2a� 1

sign eFð Þ; 12< a < 1

eF ¼ τe � τrefð5:30Þ

The reference actuator current is calculated as

iref ¼ K � 1n τ ¼ K � 1


� � ð5:31Þ

Transients are shown in Figure 5.5. Initial conditions are q 0ð Þ ¼ 0:0499 m. In the interaction

force modeling the parameters of the environment are Me ¼ 0 kg, Ke ¼ 250 000Nm–1,

De ¼ 5 [1 þ 0.5 sin (6.28t)] kg s–1 and Den ¼ 5 kg s–1. The force controller parameters are

k ¼ 50, a ¼ 0:8. The control force is limited by τj j ¼ 25 N. In the first row the position of

environment qe, the plant position q, the reference τref and actual force τe are shown. In the

Figure 5.5 Transients in the position of the environment qe, the plant position q, reference τref ,actual force τe, velocity _q ¼ v, force control error eF ¼ τe � τref , estimated disturbance τdis and plant

input force τ are shown. Interaction object parameters are Me ¼ 0 kg, Ke ¼ 250 000Nm–1 and

De ¼ 5[1 þ 0.5 sin (6.28t)] kg s–1. Controller parameters are a ¼ 0:80, k ¼ 50, τmax ¼ 25 N.


second row the plant velocity _q ¼ v and the force control error eF ¼ τe � τref are shown. Inthe third row the estimated disturbance τdis and the plant input force τ are shown. The

finite-time convergence to zero force control error can be observed on the diagram. Since

changes in position and velocity are small the disturbance is dominated by the interaction

force. Diagrams indicate the tracking of the reference force with the nonstationary

environment.

5.1.3 Lossless Environment

If the losses in interaction are negligible or zero, the interaction force is modeled by a

lossless linear or nonlinear spring (5.2). In this case the force control task (5.17) modifies

to enforcing

SF ¼ q : Ke q� qeð Þ� τrefe tð Þ ¼ eF q; qeð Þ ¼ 0� � ð5:32Þ

The control error in (5.32) can be rewritten as eF ¼ Keq� τrefe þKeqe� � ¼ Keq�Keq

refe ,

thus it can be interpreted as the position control with reference depending on the position

of environment and the desired interaction force. This indicates that results obtained for

position control, with the output defined as in (3.6), can be applied in this case too. The

implementation of such a solution greatly depends on Ke and in practical application may not

produce the desired result.

Here we will discuss slightly different solution. Let the desired closed loop transient is

selected as

€eF þ k1 _eF þ k2eF ¼ 0; k1; k2 > 0 ð5:33Þ

The desired acceleration which enforces transient (5.33) on the trajectories of the sys-

tem (5.3) with disturbance compensation and dynamics (4.96) is

€qdes ¼ €qe þ€τrefe

Ke

� a� 1n p Q; τdisð Þ� k1 _eF þ k2eF

Ke

ð5:34Þ

The exact value of the spring constant Ke is not known in most practical cases. In further

development wewill assume the nominal value of the spring constantKen known and variation

ofKe determined by continuous functionDKe with known lower and upper bounds. Then (5.34)

can be rearranged into

€qdes ¼ €qe þ€τref tð Þ�DKe €q� €qeð Þ� �

Ken

� a� 1n p Q; τdisð Þ� k1 _eF þ k2eF

Ken

ð5:35Þ

In the solutions discussed so far the desired acceleration was expressed as the sum of the

equivalent acceleration and the convergence acceleration. Following the same pattern, let us

formally set the equivalent acceleration as

€qeqF ¼ €qe þ€τref tð Þ�DKe €q� €qeð Þ� �

Ken

� a� 1n p Q; τdisð Þ ð5:36Þ


The equivalent acceleration (5.36) does not set to a zero rate of change of the control error

but, the second derivative of the control error. It enforces €eF ¼ 0Y _eF ¼ const: Since the

parameters and the motion of environment is not known equivalent acceleration (5.36) cannot

be calculated directly, it thus needs to be estimated.

From the plant description and (5.35) and equivalent acceleration (5.36) the desired

acceleration can be determined as

€qdes ¼ €qeqF þ €qconF

€qconF ¼ K � 1en k1 _eF þ k2eFð Þ

ð5:37Þ

In constructing the equivalent acceleration observer the desired acceleration €qdes and the

control error eF are available as the measured inputs. Assume that the equivalent acceleration

can be modeled as an unknown constant, thus _q ¼ 0. The dynamics of the scaled control error,

on the trajectories of the system with compensated disturbance, can be expressed as

K � 1en _eF ¼ j

_j ¼ €qdes þq_q ¼ 0

ð5:38Þ

System (5.38) with inputs €qdes; eF� �

and q ¼ � €qeqF as output is observable.

Let introduce new variables z1 and z2

z1 ¼ q� l1e*F ; e*F ¼ K � 1

en eF ; l1 ¼ const

z2 ¼ j� l2e*F ; l2 ¼ const

ð5:39Þ

Differentiation of z1 and z2 on the trajectories of system (5.38) yields

_z1 ¼ � l1 z2 þ l2e*F

� �_z2 ¼ z1 � l2z2 þ l1 � l22

� �e*F þ €q

ð5:40Þ

The observer should have the same dynamics as in (5.39)

_z1 ¼ � l1 z2 þ l2e*F

� �_z2 ¼ z1 � l2z2 þ l1 � l22

� �e*F þ €qdes

q ¼ z1 þ l1e*F

ð5:41Þ

From (5.41) the estimated equivalent acceleration is governed by

€qþ l2

_qþ l1q ¼ � l1 €qdes � €e*F

� �€qeq

F ¼ � qð5:42Þ

The desired bandwidth can be set by selection of the observer gains l1; l2. By inserting the

estimated equivalent acceleration into (5.37) the desired acceleration

€qdes ¼ satq €qeq

F � 1

Ken

k1 _eF þ k2eFð Þ� �

ð5:43Þ


and, the input force and the actuator reference current can be obtained as

τ ¼ satT τdis þ an€qeq

F � an

Ken

k1 _eF þ k2eFð Þ8<:

9=;

iref ¼ satI K � 1n τdis þ an€q

eq

F � an

Ken

k1 _eF þ k2eFð Þ0@

1A

8<:

9=;

ð5:44Þ

Due to error in the equivalent acceleration estimation peqF ðQeF; €qeqÞ, with QeF standing

for equivalent acceleration filter in (5.42), the closed loop dynamics of the force tracking

error are

€eF þ k1 _eF þ k2eF ¼ KenpeqF QeF; €qeq

�; k1; k2 > 0 ð5:45Þ

The result coincides with position control. Formally position q is forced to track position qewith tracking error proportional to the interaction force τref . Control is essentially maintaining

the relative motion of the system and the environment such that the interaction force has the

desired profile. The plant motion tracks the motion of the environment while exerting

interaction force τe ¼ τref but rejects any other external force not included in the feedback.

The error due to the application of observers in estimation of the disturbance and the

equivalent acceleration depends on the property of the environment in the interaction point.

For a hard environment the error in force may be high, even for modest estimation errors.

Additional error in the force control system may be generated by the dynamics of the force

measurement devices. The dynamic error of the force transducer adds uncompensated

dynamics which may reduce the overall frequency bandwidth and may cause poor dynamic

performance. Application of an observer and usage of the estimated value of the external force

instead of the measurement can improve the overall bandwidth of the force control system

and improve the accuracy and stability margin ([1,5]).

Example 5.3 Force Control for Lossless Environment Here we would like to illustrate

the application of the algorithms for force control, discussed so far if the environment is

modeled as an ideal spring. The control plant and its parameters are as in Example 5.2. The

interaction force ismodeled as in (5.10)withMe ¼ 0 kg,Ke ¼ 250 000Nm–1 andDe ¼ 0 kgm–1.

The position of the environment is qe tð Þ ¼ 0:05m and the reference force is

τref tð Þ ¼ 15 1þ 0:25 sin 12:56tð Þ½ �N (Figure 5.6).

Algorithm (5.6) – essentially a PD controller – can be applied. Here we will show another

idea that is inspired by the results shown in Example 5.1 in which additional velocity feedback

is applied to enforce faster convergence to the desired force. Let us introduce a new variable

related to the force control error as

eqF ¼ τe þCv _qð Þ� τref

¼ Ke q� qeð ÞþCv _q� τref

¼ eF þCv _q

ð5:46Þ


Here eF ¼ τe � τref stands for the force control error and eqF stands for the modified force

control error. The modified error eqF depends on velocity; and that allows the algorithms

discussed in Example 5.2 to be directly applied for this case as well. The difference is that Cv

is used instead of the De coefficient. In this example Cv ¼ 0.1 kg/s–1 is selected. The dynamics

of the augmented force control error (5.46) on the trajectories of compensated system with

disturbance estimation error p Q; τdisð Þ are

_eqF ¼ _τe þCv€qð Þ� _τref

¼ Cv €qdes �C� 1v _τref �Ke _q� _qeð Þ� p Q; τdisð Þ� �� ð5:47Þ

The equivalent acceleration is €qeqF ¼ C� 1v _τref �Ke _q� _qeð Þ� p Q; τdisð Þ� �

. The convergence

acceleration €qconF ¼ � kC� 1v e

qF; k > 0 is enforcing exponential convergence. Finite-time

Figure 5.6 Transients in force control with the environment model as an ideal spring Me ¼ 0 kg,

Ke ¼ 250 000Nm–1 andDe ¼ 0 kg s–1. The position of the environment qe, the plant position q, reference

τref , actual force τe, velocity _q ¼ v, force control error eF ¼ τe � τref , estimated disturbance τdis and plantinput force τ are shown. Controller parameters are a ¼ 0:75, k ¼ 50, τmax ¼ 25 N


convergence is enforced by

€qconF ¼ � kC� 1v e

qFj j2a� 1

sign eqFð Þ; 1

2< a < 1

eqF ¼ τe þCv _qð Þ� τref

ð5:48Þ

In a steady state eqF ¼ 0 and the force control error is eF ¼ Cv _q. For smallCv and low velocities

this error may be acceptable. The desired acceleration, the input force and the actuator current

can be expressed as


F þ €qconF


desF

� �iref ¼ satI K � 1

n τ� � ¼ satI K � 1


� �� ð5:49Þ

The equivalent acceleration €qeq

F is estimated by observer (5.22) with filter gain gdF ¼ 600,

€qdes; eqF� �

as measured inputs and Cv as input gain. The disturbance τdis is estimated by a

disturbance observer as shown in (5.14) with _q ¼ v; iref� �

as inputs and an;Kn; gð Þ as observerparameters, thus it includes inertia variation force, actuator dynamics induced forces and the

interaction force. The observer filter gain is g ¼ 600.

The simulations are conducted under the same conditions as in Example 5.1. Initial

conditions are q 0ð Þ ¼ 0:0499m, parameters of environment qe tð Þ ¼ 0:05m and the reference

force is τref tð Þ ¼ 10 1þ 0:25 sin 6:28tð Þ½ �N. The convergence gain is k ¼ 50 and a ¼ 0:75,thus finite-time convergence is realized. The control force is limited by τmaxj j ¼ 25N.

In the first row the position of environment qe, the plant position q, the reference τref andactual force τe are shown. In the second row the plant velocity _q ¼ v and the force control error

eF ¼ τe � τref are shown. In the third row the estimated disturbance τdis and the plant input

force τ are shown.

The finite-time convergence of the force error is clearly shown. The maximum error due to

the additional velocity dependent term is Cv _qmax ¼ 0:1 � 0:5 � 10� 3 ¼ 5 � 10� 5N which is

negligible.

5.1.4 Control of Push Pull Force

Assume the characteristics of interaction between system and object changes if the object is

pushed or pulled by a system. Then the model of the interaction force, depending on the body

characteristics in the interaction point, can be written as in (5.50) or (5.51)

τe ¼Ke1 q� qeð ÞþDe1 _q� _qeð Þ � 0 in push mode

Ke2 q� qeð ÞþDe2 _q� _qeð Þ � 0 in pull mode

�ð5:50Þ

τe ¼Ke1 q� qeð Þ � 0 in push mode

Ke2 q� qeð Þ � 0 in pull mode

�ð5:51Þ

Here Ke1;De1 and Ke2;De2 describe the property of the body in the interaction point. The

structure of such a system is depicted in Figure 5.7. The equivalency of the acceleration control


for both models is already demonstrated. In further analysis and in the controller design,

model (5.35) will be used.

Assume push mode is active for q � q1ð Þ and pull mode is active for q < q2ð Þ where q1ð Þand q2ð Þ are the equilibrium points for the push and pull forces. From (5.35) the equilibrium

points can be derived as

Ke1 q� qeð ÞþDe1 _q� _qeð Þ ¼ 0YD1 ¼ q1 � qeð Þ ¼ � De1

Ke1

_q� _qeð Þ ð5:52Þ

Ke2 q� qeð ÞþDe2 _q� _qeð Þ ¼ 0YD2 ¼ q2 � qeð Þ ¼ � De2

Ke2

_q� _qeð Þ ð5:53Þ

If different properties in push and pull modes are assumed these two equilibrium points

are not the same. The peculiarity of this problem lies in the fact that the feedback quantity is

changing its slope in equilibrium points (5.52) and (5.53) respectively.

In the analysis of the problem the push force is assumed positive and the pull force is

assumed negative. The feedback signal can be established as a sum of these forces, so that the

feedback is determined by

τfbe ¼ Ke1 q� qeð ÞþDe1 _q� _qeð ÞþKe2 q� qeð ÞþDe2 _q� _qeð Þ ð5:54Þ

Now the problem of the push pull force can be formulated as keeping the state of the system

in the manifold

SF ¼ q; _q : τfbe q; qeð Þ� τref tð Þ ¼ eppF q; qe; tð Þ ¼ 0

� � ð5:55Þ

There are three possibilities as shown in Figure 5.8. If D1 ¼ D2 the feedback signal is

piecewise continuous, Figure 5.8(a). IfD1 > D2 then there is a dead zone as shown in Figure 5.8

(b). If D1 < D2, Figure 5.8(c), then there is discontinuity in the feedback signal. The cases

D1 < D2 and D1 ¼ D2 can be handled without problem. If the dead zone is large then control

of very small forces may cause an oscillation.

The algorithm discussed in Section 5.1.2 can be applied directly. The only precaution to be

taken is to select the samevalue forDen for the push and pull directions. Then the controller will

be the same for both the pull and push directions and the disturbance observer and the

equivalent acceleration observers will compensate for the error in damping ratio.

Figure 5.7 Illustration of body deformation by push pull forces


5.2 Constrained Motion Control

With the evolution of motion control technology and the wider use of systems in unstructured

surroundings the control of only position or only interaction force is becoming less and less

acceptable.Motion systems are required to operate in the same environment as people do. They

must move while establishing interactions with other systems and surroundings and modify

their motion in response to interaction forces. That sets another target in the motion control

system design – to create a framework and algorithms suitable for the control ofmoving objects

in unstructured surroundings and the modification of motion due to interaction with other

objects. That requires motion modification due to interaction force (or in other words due to

system action) and not based on the predefined models.

For a 1-dof system (5.3) position tracking and force control as independent tasks cannot be

enforced concurrently. This is a consequence of system (5.3) having a single control input and

force being the result of the system motion relative to the environment. When contact with

the environment is established a free trajectory tracking motion is impeded by the interaction

force. For a system in contact with an object, at any particular moment of time it is possible to

control either trajectory tracking or motion along a constrained trajectory (dictated by position

of environment) while the desired interaction force profile is maintained.

The specification for a trajectory tracking task aims to reject all motion impeding forces.

That makes position controlled systems robust on the appearance of the interaction force – thus

trajectory tracking is maintained despite any interaction. All forces that are within the bounds

of the control input are rejected.

In force control the controller enforces the system to reach a state in which interaction force

control error is zero and, if the environment is removed, then motion is in the direction to seek

contact. If the interaction force is controlled the trajectory is defined by the position of the

environment and the properties of the interaction point, and if the environment is removed the

position is undefined. Change in the trajectory of the contact point is a disturbance in the control

system and needs to be compensated by control input. If the interaction point is moving then,

due to the fact that the relative position between object and system is controlled, the system

movement is tracking the object movement while maintaining the interaction force. In a sense

we may tell that changes in the position of the contact point is rejected by the control system,

just as interaction forces are rejected in position control systems.

In real applications the system is often required to track a desired trajectory and, if an

interaction appears, to modify motion to maintain the desired interaction force as long as the

interaction exists and to transit into trajectory tracking when the interaction disappears. In this

situation a control system should resolve the contradictory requirements of position tracking

Figure 5.8 The relation of push pull forces due to a change of equilibrium points. (a) D1 ¼ D2.

(b) D1 > D2. (c) D1 < D2


and interaction force control. In addition the transition from trajectory tracking to force

control and vice versa should be controlled.

Another operational situation may also require transition between different modes of

operation. Assume a system for which the interaction force is input variable (human operated

device with human generated force being treated as input). Then, if the interaction force is

different from zero, the system should move in the direction of the force in such a way that

the acceleration and velocity induced forces are equal to the input force. When the interaction

force is zero the system should rest in the reached position.

The stated problems may be analyzed in stiffness settings also. While trajectory tracking

requires robustness of the closed loop motion against interaction forces, the force control

requires robustness against change of the interaction point trajectory. If the measure of

robustness for trajectory tracking is expressed as a high (theoretically infinite) stiffness then,

in force control, the stiffness should be small (theoretically zero).

An additional problem is minimization of the impact force occurring in the case of a

transient from position to force tracking. Fast convergence of the impact force requires a high

rate of dissipation of kinetic energy when impact occurs. All of these considerations make it

a challenging task to design a control that will ensure a stable transition from position tracking

to force tracking and vice versa. Just a combination of the two controllers may not be the best

way to solve such problems.

Chapter 3 has shown the general solution of the control problems in the acceleration

framework and the application of these general solutions to trajectory tracking and the

interaction force control have been further discussed in Chapter 4 and in this chapter. It is

shown that structurally all solutions are very similar if not the same. In both position and

interaction force control, the desired acceleration is the sum of equivalent acceleration and

convergence acceleration. The problem in force control is the system structure variation which

is actually reflected in the open feedback loop if contact does not exist. The formulation of

control in an acceleration control framework offers the possibility of uniting trajectory tracking

and force control and the transition from one to another.

Here two different approaches that lead to similar behavior but conceptually different

systems will be shown. These two approaches are:

. Since position and velocity are usuallymeasured – thus available duringwholemotion of the

system– it seems natural to assume the position tracking loop to be active all of the time. That

would require a modification of reference motion in such a way so to maintain the desired

interaction force when an interaction appears.. Inmotion tracking problems the desired acceleration – a force per unit mass – is perceived as

a control input. Force indicates an interaction with the surroundings, thus it would be natural

to merge the interaction and the trajectory tracking in the acceleration dimension. That

would lead to a modification of the desired acceleration by the interaction force.

If the internal loop is position based, a modification due to interaction means allowing an

error in trajectory tracking during contact with the environment. Effectively thatmeansmaking

a position reference function of the interaction force. The structure of the functional

dependence of the position reference would define the system behavior. Let the desired

functional relation of the changes of the system trajectory due to an interaction force be defined

by the continuous functionq τeð Þ. Since the validity of the specification of the control problems


as in (3.6)–(3.9) is already shown for both position tracking and the interaction force control let

us now look at a modified specification which includes a modification of the motion due to an

interaction force. Modified requirements (3.6)–(3.9) may be expressed in the following form:

S q; τeð Þ ¼ q; _q; €q : e qð Þ ¼ y� yref þq τeð Þ� � ð5:56Þ

Here y and yref stand for the position specified output and the reference. The nature of

modification is now defined by selection of the additional term q τeð Þ. It could be just

proportional (with linear or nonlinear gain) to interaction force or it may be a controller

setting a specific profile of the interaction force.

Merging the trajectory tracking and the force control problems in the acceleration

dimension is another possibility. The desired acceleration is sum of equivalent acceleration

and convergence acceleration. The force due to interaction with the environment can be

expressed in the acceleration dimension just by scaling it by inertia. That would allow

introducing the interaction force feedback, not in the definition of the manifold, nor in the

modification of the reference as in (5.56), but directly as acceleration induced by the interaction

force. This will directly change the acceleration and thus the motion of the system.

In the case of desired acceleration modification by the interaction force, operational

requirements may be redefined as

S q; τeð Þ ¼ q; _q; €q : €qdesq;F ¼ €qdesq þq τeð Þ ¼ €qeqq þ €qconq þq τeð Þn o

ð5:57Þ

Here €qdesq stands for the desired acceleration of the free motion (no interaction with the

environment), €qdesq;F stands for the desired acceleration of the constrained motion and q τeð Þstands for the interaction force compensation term.

5.2.1 Modification of Reference

The output y of a 1-dof system (5.3) is required to track its smooth reference yref . If during

motion the system interacts with an object moving on trajectory qe tð Þ the motion of the system

should be modified. The modification should be proportional to the interaction force where

proportionality is specified by a constant or varying coefficient l. Such an operation of the

system mathematically can be formulated as

S q; τeð Þ ¼ q; _q : sq;F ¼ sq þ lτe ¼ 0� � ð5:58Þ

Here sq stands for the generalized output tracking error and τe stands for the interaction

force. The solution of the output tracking problem is discussed in detail inChapters 3 and 4. The

rate of change of the generalized control error is defined in (3.22) or (3.37). Specification (5.58)

differs from specifications discussed in Chapter 3 by the fact that the equilibrium solution

for the output generalized tracking error sq is not zero but is proportional to the interaction

force sq ¼ � lτe; l > 0.

If no contact between the control system and the object on trajectory qe tð Þ is established thenno change in the tracking problem occurs and the equilibrium solution is sq ¼ 0. If the system

trajectory intersects with the object trajectory, an interaction force appears and consequently


the system motion is modified, allowing a shift in the system trajectory. The interaction force

dependent term lτe in (5.58) can be treated as a shift in the output tracking reference as shown inFigure 5.9. The output tracking ismodified by the interaction force and the original equilibrium

solution is shifted along the error axis for a certain value, as shown in Figure 5.10. The shift

depends on the amplitude of the external force, compliance factor l > 0 and the position

controller.

The system dynamics have two modes. In one, no contact with the environment is

established and the system is in free motion τe ¼ 0, thus behavior is determined by the

position controller. The other mode is effective when contact is established τe 6¼ 0, thus

motion is influenced by the interaction force. The transition betweenmodes is natural since it is

distinguished by the interaction force being zero.

In the second mode, the rate of change of the generalized error sq;F ¼ sq þ lτe on the

trajectories of system (5.3) is

_sq;F ¼ _sq þ l _τe ð5:59Þ

The derivative of the generalized output tracking error is given in (3.22). Inserting (3.22)

into (5.59) yields

_sq;F ¼ g €q� €qeqq

�þ l _τe ¼ g €q� €qeqq � l

g_τe

� �ð5:60Þ

Here €qeqq is the equivalent acceleration in the output tracking loop without modification

by an interaction force. Expression (5.60), as expected, has the same form as (3.22) with

Figure 5.9 Modification of the position reference proportional to the interaction force

Figure 5.10 Interpretation ofmanifold shift in amodification of output tracking by the interaction force


only a change in the equivalent acceleration

€qeqq;F ¼ €qeqq � lg_τe ð5:61Þ

The equivalent acceleration €qeqq;F in the system with reference modification depends on

the interaction force induced acceleration lg � 1 _τe proportional to the rate of change of the

interaction force. On impact, the interaction force induced acceleration may have a large value

and the control input may go to saturation.

The convergence acceleration for Equation (5.60) can be derived by selecting Lyapunov

function candidate V ¼ s2q;F=2 and its derivative sq;F _sq;F ¼ � ks2

q;F . For sq;F 6¼ 0 the

convergence acceleration can be found as

€qconq;F ¼ � kg � 1sq;F ; k > 0 ð5:62Þ

The desired acceleration, desired force and desired actuator current can be expressed as

€qdesq;F ¼ €qeqq;F � kg � 1sq;F ; k > 0

τdes ¼ an€qdesq;F ¼ an €qeqq;F � kg � 1sq;F

�ides ¼ K � 1

n an€qdesq;F ¼ K � 1

n an €qeqq;F � kg � 1sq;F

� ð5:63Þ

This acceleration will enforce sq;F tð Þ��!t!¥ 0 from initial state sq;F 0ð Þ 6¼ 0 consistent with

operational bounds on control input and system variables.

The desired acceleration (5.63) enforces the closed loop transient

_sq;F þ ksq;F ¼ 0 ð5:64Þ

Insertion of the sq;F ¼ sq þ lτe into (5.64) yields the dynamics of the output tracking

error as a function of the interaction force

_sq þ ksq ¼ � l _τe þ kτeð Þ ð5:65Þ

The reference output generalized tracking error is determined by the interaction force.

For slow changing or constant forces the change is represented just as a shift of the trajectory

_sq þ ksq ¼ 0. If the changes in the interaction force are fast then the motion is more complex.

To look at the system behavior in more detail, as an example, let interaction force be

τe ¼ De _q� _qeð ÞþKe q� qeð Þ and the output generalized tracking error be sq ¼ cDqþ c1D _q;c; c1 > 0 and position tracking error Dq ¼ q� qref . Then (5.65) yields

c1D€qþ cþ c1kð ÞD _qþ kcDq

¼ � lDe €q� €qeð Þþ l Ke þ kDeð Þ _q� _qeð Þþ klKe q� qeð Þ½ � ð5:66Þ

In free motion the right hand side is zero and selection of coefficients c; c1 > 0 guarantees

motion convergence to the reference trajectory. If the interaction force is different from zero,


by separation of variables Equation (5.66) can be rearranged into

c1 þ lDeð Þ€qþ c1c

c1þ k

0@

1Aþ lDe

Ke

De

þ k

0@

1A

24

35 _qþ k cþ lKeð Þq

¼ c1 €qref þ c

c1þ k

0@

1A _qref þ k

c

c1qref

24

35þ lDe €qe þ

Ke

De

þ k

0@

1A _qe þ k

Ke

De

qe

24

35

ð5:67Þ

All parameters and constants are strictly positive. The roots of the characteristic equation

depend on the parameters of the output tracking controller c; c1; k, the properties of bodies incontact pointKe;De and the compliance coefficient l. The parameters c; c1; k; l can be selectedsuch that the characteristic polynomial in (5.66) is Hurwitz and consequently the transient is

stable. In comparison with the control error dynamics (5.64) the dynamics of the plant position

is second order with complex dependence of the roots of the characteristic equation from the

parameters of the system.

More compact representation of the closed loop dynamics for constant parameters can be

obtained by taking a Laplace transformation of (5.67). Then the position output can be

expressed as

q ¼ Z*c

Z*c þ lZ*

e

qref þ lZ*e

Z*c þ lZ*

e

qe

Z*c ¼ c1s

2 þ cþ c1kð Þsþ kc

Z*e ¼ Des

2 þ Ke þ kDeð Þsþ kKe

ð5:68Þ

The position dynamics are determined by the reference trajectory qref and the trajectory of

contact point qe. If the reference position qref is constant and the position of the environment qe

is constant, the equilibrium position becomes

k cþ lKeð Þq ¼ k cqref þ lKeqe� �

Y q ¼ cqref þ lKeqe� �

cþ lKeð Þ

q ¼ 1c

Ke

þ l

c

Ke

qref þ lqe

0@

1A ð5:69Þ

The steady-state force inserted in the contact point can be determined from the interaction

force model τe ¼ Ke q� qeð ÞþDe _q� _qeð Þ and (5.69) as

τe ¼ Ke q� qeð Þ ¼ Kec

cþ lKeð Þ qref � qe� �

τe ¼ cc

Ke

þ lqref � qe� � ð5:70Þ


The force inserted in the contact point depends on the difference between the reference

position and the position of the environment – thus in a sense it is a force necessary to push the

system from the reference position to the environment. For a hard contact (highKe) the force is

mostly determined by the ratio c=lð Þ. Both c and l are design parameters and can be selected

during design process. The interaction force can be small if either c is small or l is large. In thecase that c is selected small the stiffness of the trajectory tracking control is made small, thus

the force to move the system back to the reference trajectory is small.

For trajectory tracking defined bysq ¼ q� qref and a lossless environment τe ¼ Ke q� qeð Þit can be shown that the steady-state position and force are determined by expression (5.69)

and (5.70) by replacing c ¼ 1. In selected structures the parameters of error equation – thus the

parameters of the manifold – determine the steady-state interaction force. The parameters of

the controller defining the convergence ratio do not contribute to the steady-state value of the

interaction force.

Example 5.4 Reference Position Modification by Interaction Force The modification

of the trajectory tracking due to the interaction force with stationary or moving object is a

basis for establishing a natural behavior of the system. The control plant is the same as in

Example 5.1. The object to be manipulated is modeled by Me ¼ 0:25 kg, De ¼ 5 kg s–1 and

Ke ¼ 250 000Nm–1. The position of environment is simulated as

qe tð Þ ¼ 0:01þ 0:0025 sin 12:56tð Þm 0:12 � t � 1

0:01 m elsewhere

(ð5:71Þ

The reference position is qref ¼ 0:01½1þ 0:3 sin 6:28tð Þ�m. Generalized position tracking

error is

eq ¼ 100 q� qref� �þ _q� _qref

� � ð5:72Þ

The position tracking controller is designed as illustrated in Example 4.8. The desired

acceleration, the input force and the reference actuator current are selected as

€qdes ¼ satq €qeq þ €qcon


des� �

iref ¼ satI K � 1n τ

� � ¼ satI K � 1n τdis þ an€q

des� �� ð5:73Þ

Here €qeqstands for equivalent acceleration estimated by observer (5.22). The observer filter

gain is set at gdF ¼ 600. The disturbance τdis is estimated by disturbance observer with _q; iref� �

as inputs and an;Kn; gð Þ as observer parameters, thus it includes inertia variation force, actuator

dynamics induced forces and the interaction force. The observer filter gain is g ¼ 600. The

control force has been limited to τmaxj j ¼ 25 N.

The convergence acceleration is selected as

€qcon ¼ � 50 eq�� 0:6sign eq

� � ð5:74Þ


The trajectorymodification due to interaction force is realized bymodification of the control

tracking error

e ¼ 100 q� qref� �þ _q� _qref

� �þ 2τe ð5:75Þ

In Figure 5.11 transients from initial position q 0ð Þ ¼ 0:009 m are shown. Diagrams in the

left column show the position q, the position reference qref , the position of environment qe and

the position control error eq. In the right column the force τe, the disturbance τdis and control

input force τ are shown. The behavior is just as expected. When contact with environment is

established the trajectory tracking is modified and motion in contact with environment is

performed till the reference motion pulls plant from the contact with environment.

The diagrams illustrate the behavior in trajectory tracking without contact the transition to

trajectory modification by interaction force and regaining the trajectory tracking when contact

Figure 5.11 Trajectory tracking modulation by an interaction force with object Me ¼ 0.25 kg,

Ke ¼ 250 000 Nm–1 and De ¼ 5 kg s–1, with force gain l ¼ 2. The reference position is

qref ¼ 0:01½1þ 0:3 sin 6:28tð Þ�m and the position of the environment as in (5.71). Diagrams in

the left column show the changes in position q, position reference qref , the position of the

environment qe and in position control error eq. The right column shows force τe, disturbance τdisand control input force τ. The controller parameters are k ¼ 50 and a ¼ 0:8.


is lost. Both the stationary and moving obstacles are shown. The capability of the system

modification if control input is saturated is also illustrated.

5.2.2 Modification by Acting on Equivalent Acceleration

Modification of motion by changing reference input to the position control loop results in

complex dependence of the closed loop system dynamics on controller parameters and

the properties of the interaction object. The advantage of such design is that output of the

position tracking controller stands for the desired acceleration of the system with and

without interaction with environment, thus the controller design is clearly defined.

Another way to integrate modification of motion due to interaction into position control

system is to merge these two tasks in the force or acceleration dimension instead in the

position dimension. The output of the position controller is the desired acceleration, or if

multiplied by the inertia, the desired input force. That makes natural to realize in

acceleration dimension the modification of motion due to interaction force. This results

in structure shown in Figure 5.12. The trajectory tracking is now in the outer loop with

desired acceleration

€qdesq ¼ €qeqq � kg � 1sq ð5:76Þ

Here €qeqq is the equivalent acceleration determined from the position tracking task, sq is

the generalized position tracking error with dynamics (3.22). The convergence acceleration

is selected to be proportional to error, thus asymptotic convergence will be enforced in the

position tracking loop. The motion is assumed consistent with bounded control so saturation is

omitted in (5.76).

Let desired acceleration is modified by inserting interaction force proportional term lτeinto (5.76) to obtain

€qdesq;F ¼ €qeqq � kg � 1sq|fflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflffl}trajectory tracking

� lτe|{z}modification

ð5:77Þ

Figure 5.12 Modification of the desired acceleration by interaction force


The coefficient l is assumed strictly positive constant. The desired input force or input

current is then expressed as

τdesq;F ¼ an€qdesq;F ¼ an €qeqq � kg � 1sq

�� anlτe

idesq;F ¼ K � 1n an€q

desq;F ¼ K � 1

n an €qeqq � kg � 1sq

��K � 1

n anlτeð5:78Þ

The insertion of the modification term as acceleration or as the force does not have any

difference. The input force τ ¼ τdis þ τdesq;F is just a sum of the estimated disturbance τdis andthe desired force.

If interaction force is zero, the position tracking loop operates without any change and the

closed loop dynamics are _sq þ k sq ¼ 0. When the interaction force is different from a zero

closed loop the dynamics enforced by the desired acceleration (5.77) become

_sq þ ksq ¼ � lτe: ð5:79Þ

The error in position tracking is now proportional to the interaction force. In order to

determine actual changes in position and interaction force, let model the interaction force be

τe ¼ De _q� _qeð ÞþKe q� qeð Þ and the generalized position tracking error sq ¼ cDqþ c1D _q;c; c1 > 0, Dq ¼ q� qref . Then, the dynamics (5.79) can be rearranged into

c1€qþ kc1 þ cþ lDeð Þ _qþ kcþ lKeð Þq¼ c1€q

ref þ kc1 þ cð Þ _qref þ kcqref þ l De _qe þKeqeð Þð5:80Þ

If the parameters in (5.80) are constant by taking a Laplace transformation and solving it for

q yields

q ¼ Z*c

Z*c þ lZe

qref þ lZeZ*c þ lZe

qe

Z*c ¼ c1s

2 þ cþ c1kð Þsþ kc

Ze ¼ DesþKe

ð5:81Þ

The characteristic equation Z*c þ lZe ¼ 0 depends on the parameters of environment. As

in previous case the dynamics in position are governed by the reference position qref and the

position qe of interaction point with object. In free motion τe ¼ 0 and the trajectory tracking is

clearly established.

If the reference trajectory and the position of environment are constant the steady-state

position error is determined as

kDq ¼ � lτe Y q ¼ kcqref þ lKeqe

kcþ lKe

¼k

c

Ke

kc

Ke

þ lqref þ l

kc

Ke

þ lqe

ð5:82Þ


and the interaction force at position (5.82) becomes

τe ¼ Ke q� qeð Þ ¼ Kekc

kcþ lKe

qref � qe� �

¼ kc

kc

Ke

þ lqref � qe� � ð5:83Þ

The interaction force depends on the convergence gain k and the difference between the

position reference qref and the environment position qe. To maintain a small force during the

interactionmay require a high force feedback gain l and then the forcemeasurement noisemay

present some problems in realization. The steady-state relation kDq ¼ � lτe shows that

position controller convergence term is balanced by the interaction force feedback. Thatmakes

these two terms possibly large but their difference – which acts as the system acceleration is

taking the value needed to make desired motion of the system. This fact must be taken into

accountwhile designing the controller since a limit on the convergence accelerationmay create

oscillations in the system.

Motion modifications by changing the reference trajectory or the acceleration demonstrate

ways of handling interactions in motion control systems. Acting on the reference position

changes the control error and as a result the closed loop dynamics. The controller is designed to

maintain a specific closed loop error convergence. The steady-state error in trajectory tracking

and the steady-state force are not functions of the rate of the convergence.

Modification by the addition of an interaction force to the desired acceleration acts

as an uncompensated disturbance in the system and shifts the overall closed loop to a

new dynamical balance. The trajectory tracking steady-state error and the steady-state

interaction force depend on the position control loop stiffness. This clearly shows the

relation between stiffness established by the position controller and the interaction force –

one may change stiffness in order to maintain a desired interaction force; and if stiffness is

kept very low then the rate of change of the position tracking error is proportional to the

interaction force.

In both solutions, adding a small velocity term would give better convergence for

systems with a small De. The reason is exactly the same as discussed for the position control

systems.

The two solutions may be combined to use the good properties of both of them. Some

additional characteristics are discussed in the following example.

Example 5.5 Modification of Trajectory Tracking by Changing Acceleration Here

we would like to illustrate a modification of the trajectory tracking due to the interaction

force with a stationary or moving object by changes in the desired acceleration. The

control plant and other conditions are the same as in Example 5.4 in which a modification

by changing the position reference is shown. The object to be manipulated is modeled by

Me ¼ 0.25 kg, Ke ¼ 250 000 Nm–1 and De ¼ 5 kg s–1. The position of the object is as

in (5.71). The trajectory reference is qref ¼ 0:01½1þ 0:3 sin 6:28tð Þ�m. The trajectory

tracking error is as in (5.72).


The trajectorymodification due to interaction force is realized bymodification of the desired

acceleration

€qdes ¼ satq €qeq þ €qcon � lτe


des� �



des� �� ð5:84Þ

Here €qeq

stands for the equivalent acceleration estimated by observer (5.22). The

observer filter gain is set at gdF ¼ 600. The disturbance τdis is estimated by disturbance

observer (4.34) with _q; iref� �

as inputs and an;Kn; gð Þ as observer parameters, thus it

includes inertia variation force, actuator dynamics induced forces and the interaction

force. The observer filter gain is g ¼ 600. The control force is limited by τmaxj j ¼ 25 N. The

convergence acceleration is €qcon ¼ � 50 eq�� 0:6 sign eq

� �, and the generalized trajectory

tracking error is eq ¼ 100ðq� qref Þþ ð _q� _qref Þ.In Figure 5.13 the initial position is q 0ð Þ ¼ 0:009 m and the parameter l ¼ 1. Diagrams

in the left column show the position q, the position reference qref , the position of the

Figure 5.13 Trajectory tracking modulation by interaction force with object Me ¼ 0.25 kg,

Ke ¼ 250 000Nm–1 and De ¼ 5 kg s–1, with force gain l ¼ 1. The reference position is

qref ¼ 0:01½1þ 0:3 sin 6:28tð Þ�m and the position of the environment as in (5.71). Diagrams in the left

column, show the changes in position q, position reference qref , the position of the environment qe and

position control error eq. The right column shows force τe, disturbance τdis and control input force τ. Theconvergence gain is kept constant k ¼ 50 and a ¼ 0:8.


environment qe and the position control error eq. In the right column the force τe, thedisturbance τdis and the control input force τ are shown. The behavior is just as expected.

When plant comes in contact with environment the trajectory tracking is modified and motion

in contact with the environment is performed till the reference motion is such that the plant

loses contact with the environment.

The diagrams illustrate the same behavior as a system with reference trajectory modifi-

cation: the trajectory tracking without contact, the transition to trajectory modification by

interaction force and regaining the trajectory tracking when contact is lost. Both the stationary

and moving obstacle are shown. The capability of the system modification if the control input

is saturated is also illustrated.

5.2.3 Motion Modification while Keeping Desired Force Profile

In the solutions presented so far the interaction force depends on the position reference – thus if

the position reference is high in comparison with the position of the point of contact the

interaction force may be unacceptable from the point of system operation. A solution that will

allow force control during interaction while maintaining position tracking if interaction does

not occur would give a more flexible system behavior in the presence of an unstructured

environment.

Both structures discussed in Sections 5.2.1 and 5.2.2 may be applied to limit the interaction

force at the desired level. In order to keep the desired profile of the interaction force in both

structures the coefficient l should be variable. Both structures have the same behavior – the

larger the l the lower the interaction force, thus the control loop that will vary l should lower itif the force is small and conversely raise it if the force is high.

The desired motion is composed of free motion in which the system is required to track

the desired trajectory qref and constrained motion in contact with the environment. During

interaction with the environment the desired τref of the interaction force should be

maintained.

The open loop behavior of system (5.3) with a disturbance feedback is given by (4.96).

Let the dynamics of trajectory tracking system be governed by the desired acceleration

€qdesq ¼ €qeqq � kg � 1sq where sq ¼ s Dq;D _qð Þ, Dq ¼ q� qref stands for the generalized output

tracking error, k is the convergence coefficient and g is gain defined in (3.22); €qeqq stands for

equivalent acceleration. As shown in Chapters 3 and 4 this solution guarantees trajectory

tracking in free motion without contact with the environment.

The force due to contact with environment τe is required to track its reference τref .The design of the interaction force control is discussed in detail in Section 5.1. Assume the

interaction is modeled as a spring–damper with spring and damper Ke;De coefficients in

the contact point. Let the reference force be a continuous, bounded function. For a given force

model the desired acceleration in the force tracking loop is €qdesF ¼ €qeqF � kFD� 1en eF . Here, €q

eqF

stands for the equivalent acceleration in the force control loop [see Equation (5.21)], kFstands for the convergence coefficient and eF stands for the force tracking error.

The position tracking controller is governing the system along the desired trajectory. At the

moment of establishing contact with the environment the trajectory tracking task should be

modified so that tracking of the desired force is realized. The force tracking mode is then active

as long as the contact exists. When the reference trajectory is such that the system is pulled out

of the contact the trajectory tracking mode should be reestablished.


The position tracking and force tracking algorithms arewell established solutions. The force

is notmeasured (the signal from forcemeasurement transducer is zero)when there is no contact

and is measured when contact exists. The position is measured during the entire motion. That

means the position tracking loop can be operational during the entire motion and the force

control loop is operational only during contact.

Assume the position tracking loop is active and the desired acceleration is

€qdesq ¼ €qeqq � kg � 1sq. Let us find additional acceleration input €qcmpF such that system remains

in force tracking mode as long as interaction with the environment exists and returns to the

position tracking mode after the system is pulled out of contact. The closed loop dynamics

with position tracking €qdesq and additional acceleration €qcmpF due to interaction force can be

expressed as

€q ¼ €qdesq þ €qcmpF

¼ €qeqq � kg � 1sq|fflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflffl}trajectory tracking

þ €qcmpF|ffl{zffl}

modification

ð5:85Þ

The interaction force is modeled by τe ¼ De _q� _qeð ÞþKe q� qeð Þ and the desired closed

loop transient in force control is _eF þ kFeF ¼ 0. Then the desired acceleration in force control

is €qdes ¼ €qeqF �D� 1en kFeF . By inserting (5.85) into _eF þ kFeF ¼ 0 the compensating term

€qcmpF is determined in the following form

_eF þ kFeF ¼ Den €q� €qeqFð Þþ kFeF ¼ Den €qdesq þ €qcmpF � €qeqF

�þ kFeF ¼ 0

€qcmpF ¼ €qeqF �D� 1

en kFeF� �� €qdesq ¼ €qdesF � €qdesq

ð5:86Þ

The overall control is defined by the following relation

€qdes ¼ €qdesq þ €qcmpF

€qcmpF ¼

0 motion without contact

€qeqF � €qdesq �D� 1en kFeF motion in contact

( ð5:87Þ

The structure of the system is shown in Figure 5.14. Algorithm (5.87) effectively changes

the trajectory tracking error when the interaction force appears and does not influence

trajectory tracking if the interaction does not exist. In the application one may encounter a

problem with noisy force measurement and fast switching between two algorithms for forces

very close to zero. Some of these issues will be discussed in Example 5.6.

Example 5.6 Transient between Position and Force Control The trajectory tracking in

free motion combined with the force control when in contact with the environment is another

issue in motion control systems. The control plant is the same as in Example 5.5. The

parameters of the object in the contact point areMe ¼ 0 kg,Ke ¼ 5000Nm–1 andDe ¼ 5 kg s–1.

The reference position is qref ¼ 0:01½1þ 0:3 sin 6:28tð Þ�m. The position of environment

is qe tð Þ ¼ 0:0065 m and the reference force is τref tð Þ ¼ 15 1þ 0:25 sin 12:56tð Þ½ �N. Theposition tracking error is eq ¼ 100 q� qref

� �þ _q� _qref� �

and the force tracking error is

eF ¼ τe � τref .


The position tracking controller is designed as shown in Chapter 4 and illustrated in

Example 4.8. The trajectory modification due to the interaction force is realized by modifi-

cation of the desired acceleration

€qdes ¼ satq €qeq þ €qcon


des� �



des� �� ð5:88Þ

The convergence acceleration is selected €qconq ¼ � 50 eq�� 0:6sign eq

� �. Here €q

eqstands for

the estimated equivalent acceleration. The disturbance τdis is estimated by the disturbance

observer (4.34) with _q; iref� �

as inputs and an;Kn; gð Þ as observer parameters, thus it includes

inertia variation force, actuator dynamics induced forces and the interaction force. The

observer filter gain is g ¼ 600. The control force is limited by τmaxj j ¼ 25 N.

The control input in the force control loop is selected as


F þ €qconF


desF

� �iref ¼ satI K � 1

n τ� � ¼ satI K � 1


� �� ð5:89Þ

Here €qdesF stands for the desired acceleration, the equivalent acceleration €qeq

F is estimated

by observer (5.22). The observer filter gain is gdF ¼ 600, De ¼ 5 kg s–1.

The convergence acceleration in the force control is selected as

€qconF ¼ � 10 eFj j0:6sign eFð ÞeF ¼ τe � τref

ð5:90Þ

The position tracking and the force control are designed as independent control tasks.

Section 5.2.3 discusses a solutionwhich involves switching between two tasks. Let us look here

at a slightly different approach. Motion along the desired trajectory is performed with the

Figure 5.14 Trajectory tracking modification and interaction force control


desired acceleration as the control input. Since the external force is compensated by the

disturbance observer the desired acceleration is compensating the changes in the position and

velocity in freemovement or in contactwith the environment. The interaction force is determined

by the relative position of the systemand the environment. In amathematical sense if qe > 0 then

the interaction force appears for q > qe > 0. In the opposite case for qe < 0 then the interaction

force appears for q < qe < 0. In both cases the interaction force appears for qj j > qej j and is

equal to zero for qj j � qej j. The desired acceleration follows the same relationship. This allows

the application of a very simple algorithm in the selection of the desired acceleration

€qdes ¼ min €qdesF ; €qdesq

�; q > 0 ð5:91Þ

In simulations the initial position is q 0ð Þ ¼ 0:009 m. The controller parameters in both

loops are the same k ¼ 50 and a ¼ 0:80, thus finite-time convergence is realized.

In Figure 5.15 the diagrams in the left column show transients in position q, position

reference qref , position of environment qe and position control error eq. In the right column the

Figure 5.15 Combination of position tracking in free motion and force tracking in an interaction with

a stationary environment. The left column shows position q, position reference qref , the position of the

environment qe and position control error eq. The right column shows force τe, force reference τref , forceerror eF and control input force τ


force τe, the force reference τref , the force error eF and the control input force τ are shown.

The behavior is just as expected. When the plant comes in contact with the environment the

trajectory tracking is modified and motion in contact with the environment is performed with

the desired profile of the interaction force till the reference trajectory is such that the plant loses

contact with the environment.

5.2.4 Impedance Control

Mechanical impedance is a measure of the way a structure resists motion when subjected to

a given force. It relates forces with velocities acting on a mechanical system. Mechanical

impedance reflected in a point on a structure is the ratio of the force applied to the point on a

structure to the resulting velocity at that point.

Assume the selection of control for compensated plant _q ¼ v; an _v ¼ an€qdes þ p Q; τdisð Þ such

that the closed loop system exhibits impedance

Z ¼ τev¼ MsþDþ K

sð5:92Þ

Here M stands for virtual mass, D stands for virtual damper, K stands for virtual spring

to be maintained in the closed loop and τe stands for external force acting on the system.

The problem of having a reaction to the interaction force defined by (5.92) and at the same

time tracking the reference trajectory qref when there is no interaction with the environment

can be solved in the acceleration control framework by selecting an appropriate desired

acceleration. The desired acceleration is selected as

€qdes ¼ €qref �M� 1 D _q� _qref� �þK q� qref

� �� τe� � ð5:93Þ

The closed loop dynamics can be determined by inserting (5.93) into the compensated

system dynamics an€q ¼ an€qdes þ p Q; τdisð Þ to obtain

M €q� €qref� �þD _q� _qref

� �þK q� qref� � ¼ τe þMa� 1

n p Q; τdisð Þ ð5:94Þ

Effectively (5.94) is setting the same dynamics response of the system on the change of

the reference trajectory and the external force. It is interesting to compare this result with

the trajectory tracking control input (3.112) and the closed loop transient (3.113). The

structures are the same and the difference lies in the fact that the trajectory tracking is

designed to reject disturbance (the interaction force is part of the disturbance). This shows

that impedance control can be obtained if the parameters of the position tracking system

are selected such that the roots of the closed loop characteristic equation are the same as

in (5.94) and that the external force is not compensated by the disturbance observer or if

anτe is added to the desired acceleration. Note that systems with finite-time convergence

cannot be designed in such a way.


The trajectory tracking is modified by external force and the motion will return on the

reference trajectory when the interaction force disappears. This is the same behavior as that

obtained in the discussion on compliant control (Section 5.2.2). These two results are the same

if the closed loop dynamics of the trajectory tracking are selected to be the same.

The structure of the system is shown in Figure 5.16. A combination of trajectory tracking

and impedance control can be obtained in a very similar way as the trajectory tracking and

force control.

Solution (5.77) essentially is the same as that obtained in (5.93). In both cases the dynamics

of the trajectory tracking loop stand for the system impedance with respect to the external

interaction force.

5.2.5 Force Driven Systems

In impedance control scheme (5.93) the system is attracted back to the reference trajectory by

the force proportional to the difference between the actual position and the reference position.

A sudden drop of the external force would cause motion of the system towards the reference

trajectory. In some applications the system should rest at the reached position if the external

force becomes zero. In such systems no reference trajectory is provided and the motion is

driven by the interaction force as the system input.

Let us design the desired acceleration in such a way that the system reacts on the external

force input τe as

am€qþ bm _q ¼ τe ð5:95Þ

Here am; bm are the desired inertia and damping coefficients of the closed loop system. The

solution in the acceleration control framework is straightforward. Insertion of compensated

system dynamics €q ¼ €qdes þ a� 1n p Q; τdis

� �into (5.95) yields the desired acceleration

€qdes ¼ a� 1m τe � bm _qð Þ� a� 1

n p Q; τdisð Þ ð5:96Þ

This implementation requires measurement of the input force. For this purpose the external

force observer, discussed in Chapter 4, can be directly applied and installation of a force

transducer can be avoided.

Figure 5.16 Structure of impedance control based on a model following trajectory tracking


There are many possibilities to implement the desired structure. One way is to treat

am€qþ bm _q as the desired plant dynamics and to design a disturbance observer structure such

that the system with a disturbance observer behaves as €q ¼ €qdes þ a� 1m � bm _qþ p Q; τdis

� �� .

Then by selecting €qdes ¼ a� 1m τe the desired behavior of the system is achieved. The complexity

of the realization is the same as in the previous case. The drawback is in the possible large

difference between the desired and actual system inertia and the lead–lag term due tomismatch

of the plant and the observer parameters, as discussed in Chapter 4.

The position control based realization can be easily derived from (5.96). It can be

rewritten as

am€qþ bm _q� τe ¼ d

dtam _qþ bmq�

ðt0

τedz�

¼ 0 ð5:97Þ

The eq ¼ am _qþ bmq�Ð t0

τedz can be interpreted as a position control error and the standardposition controller design procedure can be applied. This solution has a problem with open

loop integrationÐ t0

τedz but may have better noise rejection properties.

5.2.6 Position and Force Control in Acceleration Dimension

By applying disturbance observer feedback the dynamics of the system as reflected to the

control input can be adjusted to the desired structure. In most cases the compensated systems

are adjusted to simple second-order dynamics with nominal inertia an and desired acceleration

as the control input an€q ¼ an€qdes. Then the velocity is integral to the desired acceleration and

the position can be expressed as an integral of velocity

v ¼ð€qdj; q ¼

ðð€qdjdz ð5:98Þ

On the contrary, in Equation (5.3) the interaction force is directly acting at acceleration.

For the compensated system the interaction force modifies motion as shown in (5.99)

an€q ¼ an€qdes þ p Q; τdisð Þ� τe ð5:99Þ

From (5.98) the external force is readily expressed in the acceleration dimension as

€q ¼ €qdes � a� 1n τe ¼ €qdes � €qint þ a� 1

n p Q; τdisð Þτe ¼ an€qint

ð5:100Þ

The interaction force is now represented in terms of acceleration τe ¼ an€qint. Expres-sions (5.98) and (5.100) shows that both motion and interaction force can be defined in the

acceleration dimension. While force has a static relationship with acceleration the motion

(velocity, position) are integrals of acceleration, thus they are specifiedup to integration constants.

5.2.6.1 Position Control in Acceleration Dimension

Let acceleration and its reference be known. Assume known zero initial conditions for velocity

and position. Let us design the desired acceleration for a compensated system such that the


plant acceleration tracks its reference and the velocity and positions converge to the values

determined by the reference acceleration and the zero initial conditions in velocity and

position. The control error can be expressed as

e tð Þ ¼ €q� €qref

_q 0ð Þ ¼ v 0ð Þ ¼ 0

q 0ð Þ ¼ 0

ð5:101Þ

Let the desired acceleration be

€qdes ¼ €qref �Kd

ðt0

e d j�Kp

ðt0

0þðt0

e d j

0@

1Adz ð5:102Þ

Then the motion of the system is governed by the following dynamics

€q ¼ €qref �Kd _q� _qref� ��Kp q� qref

� �_q 0ð Þ ¼ v 0ð Þ ¼ 0; q 0ð Þ ¼ 0

ð5:103Þ

The position tracking error tends to zero. The zeroes of the characteristic equation of the

closed loop system are determined by the controller parameters Kd ;Kp.

Control is designed to maintain the acceleration tracking in double integrator system.

Any error in the initial conditions will create an error in velocity and position.

This shows the possibility of specifying and controlling motion in the acceleration

dimension. There are practical problems in reaching the desired equilibrium solution due to

the dynamic error (5.103). Due to this, the proposed solution may be applied in systems with

zero initial conditions or in systems inwhich there are some othermeans for enforcement of the

position convergence.

As shown in Section 4.6 the disturbance observer feedback inserts an integrator in

the acceleration control loop as shown in (4.94). This feature in position control in

the acceleration dimension leads to an€q ¼ an€qdes þ gan

Ðt0

€qdes � €q� �

dz, thus the acceleration

control loop will enforce the desired acceleration (5.102). The presence of a human operator

dictating the trajectory like in the bilateral control systems offers another good example of the

application ofmotion specification in the acceleration dimension.Wewill return to the analysis

of motion control in the acceleration dimension in that context.

5.2.6.2 Force Control in Acceleration Dimension

Assume the external force is measured or estimated, thus the acceleration due to the

external force can be calculated from (4.75). Let us design a control input which enforces

the interaction force induced acceleration €qint ¼ a� 1n τint tracking its smooth reference

€qrefint ¼ a� 1n τrefint .


Assume the interaction force is modeled as a spring–damper system, thus the interaction

force induced acceleration is

€qint ¼τintan

¼ Kpðq� qeÞþKdð _q� _qeÞan

ð5:104Þ

Then the acceleration tracking error is eint ¼ €qint � €qrefint . As shown in Section the force

control, with the force model as a spring–damper, can be solved by applying a proportional

controller. Following the same idea let the reference acceleration be determined as

€qref ¼ Cf eint ¼ Cf €qint � €qrefint

�ð5:105Þ

Assume that the actual plant acceleration is measured. Then the desired acceleration to be

enforced in the closed loop system can be selected as

€qdes ¼ Cff €qmes � €qref� ��Cfd _q ð5:106Þ

Here €qmes stands for the measurement of the actual acceleration andCfd _q stands for velocityinduced acceleration.

The acceleration measurement is very complex, thus it is more convenient to use an

estimation of the acceleration in the realization of the control (5.106). The straightforward

solution is to apply the estimated disturbance and to estimate acceleration from the measured

input force and the estimated disturbance as

€qmes ¼ €qþ a� 1n p Q; τdisð Þ ð5:107Þ

The reference acceleration (5.105) with measured acceleration (5.107) yields closed loop

dynamics of compensated system

€q ¼ €qdes þ a� 1n p Q; τdisð Þ ¼ Cff €qmes � €qref

� ��Cfd _qþ a� 1n p Q; τdisð Þ ð5:108Þ

Inserting (5.104 ), (5.105), (5.106) and (5.107) into (5.108) and separating variables yields

an 1�Cff

� �€q þ anCfd þ Cff Cf Kd

� �_qþ Cff Cf Kpq ¼

¼ Cff Cf τrefint þ 1þ Cff

� �p Q; τdisð Þ þ Cff Cf Kpqe þ Kd _qe

� � ð5:109Þ

To guarantee stability the coefficients must satisfy Cff < 1 and Cf ;Cfd > 0 to satisfy the

Routh–Hurwitz criterion. For constant position qe and a constant reference force, the steady-

state solution for the interaction force and the position are

τe ¼ Ke q� qeð Þ ¼ τref þ 1þCff

� �p Q; τdisð Þ

q ¼ τref

Ke

þ qe þ1þCff

� �Ke

p Q; τdisð Þ ð5:110Þ

The steady-state interaction force tracks its reference. The position tracks environment

position qe with an error proportional to the interaction force induced displacement. The

behavior of the system with position and force control in the acceleration dimension is shown

in Example 5.7.


Example 5.7 Force Control in Acceleration Dimension Herewewould like to illustrate

force control in the acceleration dimension. As in other examples we will use compensation

of disturbance by disturbance observer. For the compensated plant with disturbance observer

feedback the equations of motion are reduced to €q ¼ €qdes þ a� 1n p Q; τdisð Þ. The acceleration

due to the interaction force τe can be expressed as €qint ¼ a� 1n τe. Similarly the reference force

induced acceleration can be expressed as €qrefint ¼ a� 1n τref . Assume that plant acceleration is

measured or estimated and is represented by €q ¼ €qmes.

Under these conditions the desired acceleration which will enforce convergence of the

interaction force induced acceleration €qint to the reference force induced acceleration €qrefint

can be expressed as

€qref ¼ Cf eint ¼ Cf €qint � €qrefint

�€qdes ¼ Cff €qmes � €qref

� ��Cfd _q

ð5:111Þ

Figure 5.17 Force control in the acceleration dimension. The transients in reference force τrefand measured force τe, position difference eqe ¼ q� qe, estimated acceleration €q ¼ €qmes, force control

error eF ¼ τe � τref and the desired acceleration €qdes are shown. The controller gains are Cf ¼ 0:75,Cff ¼ 0:85, Cfd ¼ 250


The plant model and the parameters are the same as in Example 5.1 with actuator gain

KT ¼ 0:85 1þ 0:25 sin 12:56tð Þ½ �N=A and plant inertia a ¼ 0:1 1þ 0:2 sin 3:14tð Þ½ �kg. Theinteraction force is modeled as in (5.10) with Me ¼ 0.5 kg, Ke ¼ 250 000 Nm–1 and

De ¼ 5 kg s–1. The reference force is τref ¼ 15 0:6þ 0:5 sin 12:56tð Þ½ �N. The position of

the object is kept constant qe0 ¼ 0:05 m. The disturbance τdis is estimated by a disturbance

observer with _q; iref� �

as inputs and an;Kn; gð Þ as observer parameters. The observer filter

gain is g ¼ 600. The input force is τ ¼ KTi and the actuator current loop dynamics is

modeled as a first-order filter di=dt ¼ g iref � i� �

, g ¼ 600.

The acceleration is calculated using known reference current iref and the estimated

force τdis

€q ¼ €qmes ¼ a� 1n Kni

ref � τdis� � ð5:112Þ

The control force is assumed unbounded. The nominal parameters of the plant are

Kn ¼ 0:85 N=A; an ¼ 0:05 kg. The force controller is realized with Cf ¼ 0:75, Cff ¼ 0:85,Cfd ¼ 250.

Figure 5.17 shows the transients in the reference force τref and the measured force τe, theposition difference eqe ¼ q� qe, the estimated acceleration €q ¼ €qmes, the force control error

eF ¼ τe � τref and the desired acceleration €qdes.Force trackingwith a stationary environment is illustrated. The force tracking error is within

the range of 1%. The application of the disturbance observer for the acceleration estimation

is also illustrated. The acceleration estimation error seems large but is compensated by the

desired acceleration controller.

5.3 Interactions in Functionally Related Systems

Till now the interaction between the system and the passive (not necessarily stationary)

environment has been studied. In all cases the environment is treated as a disturbance or

constraint. In this section control of the interaction of twomotion systems will be studied. This

will open a new paradigm – the interaction between systems where both can change their

motion so both can influence the interaction force. As an example, let us discuss the problem

of handling an object by two actuators geometrically positioned in such a way that the force

attack line for both of them passes through the center of geometry of the object, thus no torque

is exerted onto the object and consequently the object does not have rotational motion. This

arrangement simplifies analysis on translational motion only.

5.3.1 Grasp Force Control

Assume two systems are required to act together in a way to keep a specified grasp force on an

object placed between them. Both systems are defined as in (5.3) and disturbance feedback is

applied for each of them separately. In order to distinguish the systemswewill use subscript ‘m’

for one of them and subscript ‘s’ for another. Then the dynamics of these systems, with

disturbance feedback applied on both systems separately, can be described as

am€qm ¼ am€qdesm þ pm Q; τmdisð Þ

as€qs ¼ as€qdess þ ps Q; τsdisð Þ

ð5:113Þ


Here am and as are inertia of corresponding systems. The disturbance compensation errors

pm Q; τmdisð Þ and ps Q; τsdisð Þ are assumed small. Let stiffness and damping coefficient char-

acterizing the body beKe;De, respectively. By assumption, the properties of the grasped object

are the same in both contact points. Let both ‘m’ and ‘s’ systems, be in contact with the object.

The grasp force Fgrasp can be expressed as

τgrasp ¼ Fm þFs

Fm ¼ Ke qm � qmeð ÞþDe _qm � _qmeð ÞFs ¼ Ke qse � qsð ÞþDe _qse � _qsð Þ

ð5:114Þ

The model for the grasp force can be expressed as

τgrasp ¼ Ke qm � qsð ÞþDe _qm � _qsð Þþ dgrasp qme; qseð Þdgrasp qme; qseð Þ ¼ Ke qse � qmeð ÞþDe _qse � _qmeð Þ ð5:115Þ

Here Fm and Fs are the forces in contact with the environment on both the ‘m’ and ‘s’ sides.

Both have zero value if contact is not established, qme and qse are the positions of the contact

points with the environment at the ‘m’ and ‘s’ side respectively, _qme and _qse are correspondingvelocities. Note that the grasp force is different from the external force acting on the ‘m’ and ‘s’

systems since object mass – thus the acceleration force – is not taken into account. The object

acceleration force and the grasp forces are part of the overall disturbance of the system and

are compensated by disturbance observers. The models (5.114) and (5.115) are used only to

determine the structure of the control system, thus without loss of generality the term

dgrasp qme; qseð Þ can be treated as one quantity.

Assume the grasp force reference is a smooth bounded function τrefgrasp tð Þ with continuous

and bounded first time derivative. The grasp force error is then expressed as

egrasp ¼ Fm þFsð Þ� τrefgrasp ð5:116Þ

In calculating the grasp force error (5.116), the grasp force is assumedmeasured or estimated.

The dynamics of the grasp force control error may be expressed in the following form

_egrasp ¼ _τgrasp � _τrefgrasp ¼ _Fm þ _Fs

� �� _τrefgrasp

¼ De €qgrasp � €qeqgrasp

�;

€qgrasp ¼ €qm � €qsð Þ€qeqgrasp ¼ D� 1

e _τrefgrasp �Ke _qm � _qsð Þ� _dgrasp

h ið5:117Þ

Taking into account the equations ofmotion (5.113) the dynamic of the grasp force becomes

_egrasp ¼ De €qdesgrasp � €qeqgrasp

�þDepgrasp

€qdesgrasp ¼ €qdesm � €qdess

pgrasp ¼ pm

am� ps

as

0@

1A

ð5:118Þ


Here the desired acceleration €qdesgrasp is expressed as a difference between the ‘m’ side and ‘s’

side desired accelerations. System (5.118) describes first-order dynamics with desired

acceleration €qdesgrasp as the input and ð� €qeqgrasp þ pgraspÞ as the disturbance. Assuming

pm � 0 and ps � 0 in further analysis the estimation error will be neglected, thus

pgrasp � 0. The grasp force control error dynamics (5.118) have the same structure as

dynamics (5.18) describing the dynamics of a force tracking error.

The grasp force control error dynamics (5.118) are formulated as an ordinary force control

problem and design can follow the same steps as in Section 5.1. Assume that the desired

closed loop dynamics are _egrasp þ kgraspegrasp ¼ 0; kgrasp > 0. For known De 6¼ 0, the desired

acceleration enforcing grasp force tracking can be determined from the desired closed loop

dynamics and (5.118) as

€qdesgrasp ¼ €qeqgrasp � kgraspD� 1e egrasp ð5:119Þ

Control gain depends on the properties of the objects in the contact point. The damping

coefficient De is a variable but has bounded ‘nominal’ value which, as in force control, can be

used. The equivalent acceleration €qeqgrasp can be estimated as shown in Chapter 4.

If De ¼ 0 the interaction forces become

Fm ¼ Ke qm � qmeð ÞFs ¼ Ke qse � qsð Þ ð5:120Þ

The grasp force is τgrasp ¼ Ke qm � qsð Þþ d qm; qseð Þ with d qm; qseð Þ ¼ Ke qse � qmeð Þ. Thecontrol error (5.116) can be expressed as egrasp ¼ Ke qm � qsð Þ� τrefgrasp. The resulting error

dynamics (5.121) has relative degree two

€egrasp ¼ €τgrasp � €τrefgrasp ¼ Ke €qm � €qsð Þ|fflfflfflfflfflffl{zfflfflfflfflfflffl}€qgrasp

� €τrefgrasp � €d qm; qseð Þh i|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}

Ke €qgraspeq

¼ Ke €qgrasp � €qeqgrasp

� ð5:121Þ

With the dynamics of the compensated systems as in (5.113) the grasp force control error

dynamics become

€egrasp ¼ €τgrasp � €τrefgrasp ¼ Ke €qdesgrasp � €qeqgrasp

�€qdesgrasp ¼ €qdesm � €qdess

€qeqgrasp ¼ K � 1e €τrefgrasp � €d qme; qseð Þ

h i ð5:122Þ

The dynamics of the force error with De 6¼ 0 and De ¼ 0 are shown in Figure 5.18. The

difference is in the relative degree of the dynamics (5.118) and (5.121).

The dynamics (5.122) is second order with the desired acceleration €qdesgrasp as input and

� €qeqgrasp as disturbance. Let the desired closed loop dynamics in grasp force tracking be

€egrasp þ k1 _egrasp þ k2egrasp ¼ 0; k1; k2 > 0 ð5:123Þ


For Ke 6¼ 0, from the desired closed loop dynamics (5.123) and (5.122), the desired

acceleration can be determined as

€qdesgrasp ¼ €qeqgrasp �K � 1e k1 _egrasp þ k2egrasp

� � ð5:124Þ

Both solutions (5.119) and (5.124) are treated as SISO problems and their solutions are the

same as those derived for the corresponding 1-dof force control problems. Such a treatment is

the result of the physical interaction of the systems. The control output is a linear combination

of the forces developed by the individual systems. As the scalar input to the system a linear

combination of the control inputs is selected. The control input appears to be a linear

combination of the accelerations of the systems enforcing the grasp. Control forces for the

‘m’ and ‘s’ systems cannot be uniquely determined from the selected control. That is a natural

consequence of the system structure. The grasp force can be created within a work space of

the system independently of the absolute position of the subsystems involved. The solution

establishes a functional relation between the ‘m’ and ‘s’ systems such that the grasp force has

the desired value. In a sense the relative motion of the two systems is controlled but their

absolute motion within the workspace is not controlled.

The system has a two-dimensional control input and only a one-dimensional control

error – thus it is redundant for the given task. The unused degree of freedom in the control

can be assigned to enforce some other task. The only constraint in selecting an additional

task is that it should be linearly independent from grasp force control. For example, we

may require that the system ‘s’ tracks the desired trajectory while the desired profile of the

grasp force is maintained. In this case the desired acceleration €qdess is the result of

trajectory tracking and in the error dynamics of the grasp force it acts as a disturbance.

Now the problem is reduced to ordinary force control, discussed in Section 5.1. Here the

role of the environment is taken by system ‘s’. The desired acceleration for system ‘m’

should be selected in the following form

€qdesgrasp ¼ €qdesm � €qdess ¼ €qeqgrasp � kgraspD� 1e egrasp

€qdesm ¼ €qdess þ €qeqgrasp � kgraspD� 1e egrasp ¼ €qeqm � kgraspD

� 1e egrasp

€qeqm ¼ €qdess þ €qeqgrasp

ð5:125Þ

Figure 5.18 Grasp force error dynamics with interaction model (a) as in (5.115) and (b) as in (5.120)


Consequently, the control forces on the ‘m’ and the ‘s’ systems are

τm ¼ sat τdism þ anm €qeq

m � kgraspD� 1e egrasp

�n oτs ¼ sat τdiss þ ans €q

eq

qs � ksg � 1sqs

�n o ð5:126Þ

Here anm; ans stand for the nominal inertia of the ‘m’ and ‘s’ systems respectively, τdism; τdissare estimated disturbances at the ‘m’ and ‘s’ sides and sqs is the trajectory tracking error in the

‘s’ system.

This arrangement establishes a functional relation between the two systems. System ‘s’

defines the trajectory while system ‘m’ defines the control grasp force on the object supported

by both systems. The structure of the grasp control is depicted in Figure 5.19. The two systems

interact with each other, and both react on the action of the other system as on the disturbance.

The proposed solution functionally relates the motion of two systems by enforcing control

of the interaction force. The controllers for position tracking and grasp force control are

separated and the dynamics of the corresponding closed loops are selected independently. In

each of them the forces induced by the motion of the other system are treated just as

disturbances. The assignment of one system to enforce trajectory tracking and other system

to control grasp force is arbitrary. This solution conceptually is not different from the force

control with variable environment position. Each of the systems has its own role and controller

design has been implemented for each system separately. The behavior of one system is treated

as a disturbance in the other. The compensation of error in the system is processed for each

system separately.

Since the systems are interconnected and are required to attain accurate coordination of

motion, it seems natural to seek a formulation of the control tasks in a way that reflects more

accurately the systems interaction. Grasp force is defined by the relative position of the systems

and is not influenced by their absolute position in thework space. That can serve as guidance on

problem formulation. Let the center of geometry in the grasp axis of the manipulated object

qc ¼ qm þ qsð Þ=2 be required to track its smooth bounded reference qrefc with a smooth bounded

first-order derivative _qrefc . Concurrently let us require the grasp force to track its smooth

Figure 5.19 Grasp force control. System ‘s’ controls the movement and system ‘m’ controls the grasp

force


bounded reference τrefgrasp with a smooth bounded first-order derivative _τrefgrasp. Let the position

tracking error and force tracking error be as defined in (5.127) and (5.128) respectively

ec ¼ c qc � qrefc

� �þ _qc � _qrefc

� �; c > 0 ð5:127Þ

egrasp ¼ τgrasp � τrefgrasp ¼ Keqd þDe _qd � τrefgrasp þ dgrasp qme; qseð Þqd ¼ qm � qs

ð5:128Þ

The desired system operation is guaranteed if both control errors are concurrently zero or if

the system state is forced to stay in the manifold

Sc;grasp ¼ qs; _qsð Þ; qm; _qmð Þ : ec ¼ 0 and egrasp ¼ 0� � ð5:129Þ

Vector control error, with position error and grasp force error as components, can be

written as

sc;grasp ¼ec

egrasp

" #¼

c qc � qrefc

� �þ _qc � _qrefc

� �Keqd þDe _qd � τrefgrasp þ dgrasp

" #ð5:130Þ

with derivative

_sc;grasp ¼_ec

_egrasp

" #¼

c _qc � _qrefc

� �� €qrefc

Ke _qd � _τrefgrasp þ _dgrasp

" #þ 0:5 0:5

De �De

" #€qm

€qs

" #ð5:131Þ

To simplify the notation let us introduce the equivalent acceleration vector €qeqc;grasp and the

control vector €qctrc;grasp

€qeqc;grasp ¼

€qeqc

€qeqgrasp

" #¼

€qrefc � c _qc � _qrefc

� �_τrefgrasp �Ke _qd � _dgrasp

24

35

€qctrc;grasp ¼

€qctrc

€qctrgrasp

" #¼

0:5 0:5

De �De

" #€qm

€qs

" # ð5:132Þ


_sc;grasp ¼ €qeqc;grasp þ €qctr

c;grasp ð5:133Þ

The dynamics (5.133) describe two first-order systems

_ec ¼ €qctrc � €qeqc

_egrasp ¼ €qctrgrasp � €qeqgraspð5:134Þ

with control inputs €qctrc ¼ 0:5 €qm þ €qsð Þ and €qctrgrasp ¼ De €qm � €qsð Þ. Selection of the control in

the systems in (5.134) is straightforward. Proportional controllers are sufficient to enforce


convergence to a zero tracking error

€qctrc ¼ €qdesc ¼ €qeq

c � kcec

€qctrgrasp ¼ €qdesgrasp ¼ €qeq

grasp� kgraspegrasp

ð5:135Þ

Here €qdesc and €qdesgrasp stand for the desired acceleration in motion and force control, €qeq

c and

€qeq

grasp stand for the estimated equivalent accelerations and kc,kgrasp stand for the convergence

coefficients in the position and force control loops respectively. The equivalent accelerations

can be estimated by a equivalent acceleration observer. Control errors tend toward the small

values determined by the disturbance observer error and the convergence rate given by gains kcand kgrasp.

The dynamic structure of the projected system dynamics (5.134) is depicted in Figure 5.20.

The structures in Figure 5.20(a) and (b) show a projection of the ‘m’ and ‘s’ system dynamics

into new space with coordinates ec; egrasp� �

– the generalized error in the center of geometry

position (5.127) and the grasp force error (5.128). In [1] a similar transformation is applied to

define system motion in terms of modes.

The desired accelerations (5.135) are virtual variables. They cannot be directly applied to

the system input. The desired accelerations for the ‘m’ and ‘s’ systems can be determined

from (5.134). By simple algebra the ‘m’ and ‘s’ side desired accelerations can be determined as

€qdesm ¼ €qdesc þ €qdesgrasp

2De

ð5:136Þ

€qdess ¼ €qdesc � €qdesgrasp

2De

ð5:137Þ

Figure 5.20 Trajectory tracking and grasp force control loops shown as a projection into space with

the center of geometry and the grasp force as coordinates. (a) Center of geometry control. (b) Grasp force

control


As expected, real control inputs depend on the object damping coefficient, as shown in the

force control structure. To enforce the desired acceleration on the ‘m’ and ‘s’ side input forces

have form τm ¼ sat τdism þ anm€qdesm

� �and τs ¼ sat τdiss þ ans€q

dess

� �respectively.

The solutions for grasp force control (5.134)–(5.137) in addition to the functional

differences have also a conceptual difference regarding the relationship established between

the two systems. The control (5.135) establishes a functional relation among systems ‘m’ and

‘s’. These two systems act ‘together’ in maintaining the grasp force while the position of the

geometrical center of the object tracks the desired path. The controllers are designed separately

for new variables to ensure the stability and convergence of control errors (5.127) and (5.128)

and then the selected control inputs are transformed back to the original systems.

This solution can be interpreted as the definition of a system operational task (movement of

the center of geometry, grasping an object by desired force) to be fulfilled by systems defined in

the configuration space (5.113). The selection of functional relations between coordinates of

the original system(s) describing the desired tasks defines the task space. The dynamics of the

task coordinates describe the projection of the dynamics from the configuration space into the

task space. In the task space the new control inputs are defined as functions of the controls in

configuration space. Then, control design is performed in the task space to enforce the stability

of the equilibrium solution of the task error. At the end, the control inputs for the ‘m’ and ‘s’

systems are calculated. The overall structure of the control system is depicted in Figure 5.21.

Controls (5.136) and (5.137) enforce a functional relation between the system coordinates.

For assessment of the system the behavior system dynamics need to be analyzed. As already

discussed in the sections related to position and force control, the dynamicsmay be determined

from enforced error dynamics _ec þ kcec ¼ 0 and _egrasp þ kgraspegrasp ¼ 0.

Figure 5.21 Control of grasp and position of the center of geometry


The steady-state solution can be determined from egrasp ¼ 0 and ec ¼ 0. Let the reference

grasp force and the reference position of the center of geometry be constant. Then

τrefgrasp ¼ Ke qm � qsð Þ and qm þ qs ¼ 2qrefc holds. The steady-state positions of the ‘m’ and ‘s’

systems may be derived as

qm ¼ qrefc þ 1

2

τrefgrasp

Ke

ð5:138Þ

qs ¼ qrefc � 1

2

τrefgrasp

Ke

ð5:139Þ

The equilibrium solutions for the positions of the ‘m’ and ‘s’ systems are uniquely

determined by the references.

Example 5.8 Grasp Force Control The control of grasp force and motion of the center

of geometry of two systems manipulating an object will be illustrated in this example. The

system consists of two plants, both being described by the samemodel, as in Example 5.1. The

parameters of the plant are

. plant ‘m’ am ¼ 0:1 1þ 0:25 sin qmð Þ½ � kg;KTm ¼ 0:85 1þ 0:25 cos 12:56tð Þ½ �N=A

. plant ‘s’ as ¼ 0:2 1þ 0:25 sin qsð Þ½ � kg;KTs ¼ 0:65 1þ 0:5 cos 6:28tð Þ½ �N=A

Other parameters are as in Example 5.1. For each plant the disturbance τdis is estimated by

a disturbance observer. The observer gain is the same for both plants at g ¼ 600. The grasp

force is simulated as in (5.140). The value for the spring coefficient is selected low to show the

change in position of the ‘m’ and ‘s’ plants for variable force reference.

τe ¼ Ke qm � qsð ÞþDe _qm � _qsð ÞþMe €qm � €qsð Þ

Ke ¼ 250 Nm� 1;De ¼ 5 kgs� 1;Me ¼ 0 kgð5:140Þ

The reference force is τref ¼ 10 1þ 0:2 sin 12:56tð Þ½ �N. The center of geometry is

qc ¼ qm þ qs and its reference is qrefc ¼ 0:1 1þ 0:2 sin 6:28tð Þ½ �m. With these selections the

control errors are

ec ¼ qc � qrefc

eF ¼ τe � τrefð5:141Þ

Since the control of qc ¼ qm þ qs is defined as position tracking, a generalized errors can be

defined as in (5.142) and the control task can be formulated as finding a control enforcing

convergence and the stability of s ¼ 0 and eF ¼ 0.

s ¼ cqc þ c1 _qc � cqrefc þ c1 _qrefc

� �; c ¼ 100; c1 ¼ 1 ð5:142Þ


The position tracking controller is designed as

€qdesc ¼ sat €qeq

c þ €qconc

�ð5:143Þ

Here €qdesc stands for the desired acceleration. The equivalent acceleration €qeq

c is estimated

by an observer. The observer filter gain is set at g ¼ 600.

The convergence acceleration is selected as

€qconc ¼ � 100 sj j0:6sign sð Þ ð5:144Þ

The force control error can be expressed as eF ¼ De _qF þKeqF � τref with qF ¼ qm � qs.

The desired acceleration in the grasp force control loop €qdesF can be expressed as

€qdesF ¼ sat €qeq

F þ €qconF

�€qconF ¼ � 100 eFj j0:6sign eFð Þ

ð5:145Þ

The equivalent acceleration €qeq

F can be estimated by observer. The observer filter gain is set

at gdF ¼ 800.

The desired accelerations €qdesc and €qdesF enforce the center of geometry and force tracking.

The plant accelerations can be determined from functional relations between the task

accelerations €qdesc and €qdesF and the configuration space accelerations €qm and €qs. From the

definition of the task space variables qc and qF it is easy to find

€qdesm ¼ €qdesc þ €qdesF

2

€qdess ¼ €qdesc � €qdesF

2

ð5:146Þ

This solution differs from (5.136) and (5.137). This is a consequence of the different

selection of the functional relation between the coordinates in the task and the configuration

space. The definition used in this example is very similar to the so-calledmodal decompositions

introduced in [1].

The initial positions for the ‘m’ and ‘s’ plants are qm 0ð Þ ¼ 0:05 m and qs 0ð Þ ¼ 0 m

respectively. The position of the environment is constant at qe0 ¼ 0:01 m. The convergence

gain is kept constant at k ¼ 50 and a ¼ 0:8 The control force has been limited by

maxτj j ¼ 25N.

Figure 5.22 shows the transients in the reference position of the center of geometry qrefc ,

the position of the center of geometry qc, the reference force τref , the measured force τe,the position of the ‘m’ plant qm, the position of the ‘s’ plant qs, the convergence accelerations

in the grasp force €qconF , the center of geometry €qconc control loops and the control errors ecand eF .

The diagrams illustrate the enforcement of the system task – the center of geometry tracking

and the grasp force control. Both variables track their references.


5.3.2 Functionally Related Systems

In this section we will discuss the control of systems that are ‘virtually’ interconnected. The

term ‘virtually’ is used to describe a situation in which the state or outputs of otherwise

separated systems are being functionally related to each other. Controlling systems that may

require such a functional relation opens awide range of applications – from remote repetition of

movement and/or forces to training systems in which one master andmore slaves are involved.

Grasp control describes the behavior of two motion systems that have a direct mechanical

contact by grasping an object. From the motion control point of view it is of interest to discuss

the control of systems that may or may not be in mechanical contact but need to maintain

some functional relationship. Common examples are remotely operated systems. A special

example is the so-called bilateral systems where a functional relation is established for

remotely located systems in such a way that the trajectories and the interaction forces of the

systems track each other.

Figure 5.22 Control of grasp force and center of geometry, showing transients in the reference position

of the center of geometry qrefc , the position of the center of geometry qc, reference force τref , measured

force τe, the position of the ‘m’ plant qm, the position of the ‘s’ plant qs, the convergence accelerations

in the grasp force €qconF , the center of geometry €qconc control loops and the control errors ec and eF


In order to make mathematical treatment as simple as possible in this section the problem

of functionally related systems will be examined taking the example of two 1-dof systems.

Assume two mechanical systems labeled ‘m’ and ‘s’ with a structure as in (5.3) and the

disturbance feedback applied to each of them separately. Then, the description of the systems is

as in (5.113). These two systems have independent control inputs €qdesm and €qdess , thus allowing

concurrently two independent control goals. The description of this system in form (5.3) or the

equivalent form (5.113) will be referred to here as a description in configuration space with

the coordinates being state coordinates of the ‘m’ and ‘s’ systems.

Assume the states of system ‘m’ and ‘s’ are required to maintain a linearly independent

functional relation x1 qs; qmð Þ ¼ 0 and x2 qs; qmð Þ ¼ 0 where x1; x2 are continuous bounded

functions, with continuous bounded first- and second-order derivatives and continuous and

bounded partial derivatives with respect to qm and qs

x1 ¼ f1 qs; qmð Þx2 ¼ f2 qs; qmð Þ

ð5:147Þ

We will assume that in (5.147) guarantees a unique solution for qm and qs or one-to-one

mapping (x1,x2)$(qs,qm). The functional relations in (5.147) will be called a task

description.

Now, the control problem of maintaining functional relations x1 qs; qmð Þ ¼ 0 and

x2 qs; qmð Þ ¼ 0 can be defined as the selection of control inputs €qdesm and €qdess such that motion

is stable in the manifold

Sx ¼ qs; qm : x1 qs; qmð Þ ¼ 0 and x2 qs; qmð Þ ¼ 0f g ð5:148Þ

A second time derivative of functions f1 qs; qmð Þ and f2 qs; qmð Þ can be derived as

€x1 ¼ g1s€qs þ g1m€qmð Þþ _g1s _qs þ _g1m _qmð Þ€x2 ¼ g2s€qs þ g2m€qmð Þþ _g2s _qs þ _g2m _qmð Þ

gis ¼ qxiqqs

6¼ 0; gim ¼ qxiqqm

6¼ 0; i ¼ 1; 2

ð5:149Þ

Formally, on the trajectories of system (5.113), we can introduce the following new

variables

€qdesx1 ¼ g1s€qdess þ g1m€q

desm

� �€qdesx2 ¼ g2s€q

dess þ g2m€q

desm

� �;

qeqx1 ¼ � _g1s _qs � _g1m _qm � g1s

ps

ansþ g1m

pm

anm

0@

1A

€qeqx2 ¼ � _g2s _qs � _g2m _qm � g2sps

ansþ g2m

pm

anm

0@

1A

ð5:150Þ


Now the dynamics (5.149) describing the projection of system (5.113) into a space defined

by the task variables x1 qs; qmð Þ and x2 qs; qmð Þ can be written as

€x1 ¼ €qdesx1 � €qeqx1

€x2 ¼ €qdesx2 � €qeqx2

ð5:151Þ

Dynamics (5.150) and (5.151) describe two virtual, decoupled second-order systems

with equivalent accelerations €qeqx1 and €qeqx1 and desired accelerations €qdesx1 and €qdesx2 ,

respectively. The desired accelerations €qdesx1 and €qdesx2 are linear in the desired accelerations

€qdesm and €qdess of the ‘m’ and ‘s’ systems. The equivalent accelerations €qeqx1 and €qeqx1 are

linear in the velocities _qm and _qs of the ‘m’ and ‘s’ systems and the errors in the

disturbance compensation. Later in the text we will determine the relations between

coefficients gim; i ¼ 1; 2 and gis; i ¼ 1; 2 required to ensure a unique solution for €qdesm and

€qdess if €qdesx1 and €qdesx2 are known. For now, we will assume that such a condition can be

determined and is satisfied for the selected functional relation (5.147). For properly

designed disturbance observers, the disturbance compensation errors pm and ps are small

and can be neglected.

Similarly to the procedure applied for position tracking and force control, let us select

control such that convergence to the manifold (5.148) satisfies the following dynamics

€x1 þ k11 _x1 þ k12x1 ¼ 0; k11; k12 > 0

€x2 þ k21 _x21 þ k22x2 ¼ 0; k21; k22 > 0ð5:152Þ

By inserting (5.151) into (5.152) the desired accelerations can be selected in the form

€qdesx1 ¼ €qeqx1 � k11 _x1 þ k12x1ð Þ€qdesx2 ¼ €qeqx2 � k21 _x2 þ k22x2ð Þ

ð5:153Þ

By selecting appropriate values for k11; k12 > 0ð Þ and k21; k22 > 0ð Þ the closed loop

dynamics of functions x1 qs; qmð Þ and x2 qs; qmð Þ could be adjusted to satisfy given criteria,

and convergence to x1 qs; qmð Þ ¼ 0 and x2 qs; qmð Þ ¼ 0 is guaranteed.

The dynamics of the system coordinates qs; qmð Þ can be determined from (5.152), and the

steady state can be determined from x1 qs; qmð Þ ¼ 0 and x2 qs; qmð Þ ¼ 0. For any particular

solution the dynamics and state coordinates should be verified in order to validate the range of

change in which dynamics (5.152) can be enforced.

The controls (5.153) are derived in task space – but the actual motion is generated in

configuration space – and the control inputs in configuration space €qdesm and €qdess must be

derived. From (5.150) the transformation matrix

€qdesx1

€qdesx2

" #¼ g1s g1m

g2s g2m

" #€qdess

€qdesm

" #¼ Js;mx1;x2

€qdess

€qdesm

" #ð5:154Þ

relates accelerations €qdesm ,€qdess in the configuration space with accelerations €qdesx1, €qdesx2

in the task

space. Here Js;mx1;x2 stands for a Jacobian matrix relating the velocities in the task

space _xT ¼ _x1; _x2½ � and the velocities in the configuration space _qT ¼ _qs; _qm½ �. In order to


determine €qdesm and €qdess from (5.154) matrix Js;mx1;x2 must be nonsingular in the domain of change

of system variables. Assuming det Js;mx1;x2

�6¼ 0, the inverse transformation can be written as

Js;mx1;x2

�� 1

¼ 1

g1sg2m � g1mg2s

g2m � g1m

� g2s g1s

" #

det Js;mx1 ;x2

�¼ g1sg2m � g1mg2s 6¼ 0

ð5:155Þ

The desired accelerations in the ‘m’ and ‘s’ systems enforcing the transients (5.153) can be

expressed in the following form

€qdess ¼ 1

det Js;mx1;x2

� g2m€qdesx1

� g1m€qdesx2

h i

€qdesm ¼ 1

det Js;mx1;x2

� � g2s€qdesx1

þ g1s€qdesx2

h i ð5:156Þ

The control forces on the ‘m’ and ‘s’ sides can be expressed as τm ¼ sat τdism þ anm€qdesm

� �and τs ¼ sat τdiss þ ans€q

dess

� �respectively. This completes the design of the functionally related

systems. The design procedure follows the same steps we applied in the grasp force control

problem.

The control of functionally related systems can be derived in a more compact way using

matrix–vector notation. The functional relation (5.147) can be written as x ¼ f qs; qmð Þ withfT ¼ f1 f2½ �. Then a projection of the system motion in the manifold (5.149) can be written as

€x ¼ Js;mx1;x2€qs;m þ _Js;m

x1;x2_qs;m

xT ¼ x1 x2½ �qTs;m ¼ qs qm½ �

ð5:157Þ

On the trajectories of compensated systems (5.113) the equations of motion have the form

€x ¼ Js;mx1;x2€qdess;m þ _J

s;m

x1 ;x2_qs;m þ Js;mx1;x2ps;m

pTs;m ¼ ps pm½ �ð5:158Þ

The Jacobeanmatrix Js;mx1;x2 must be regular and if it is not constant it introduces an additional

disturbance Js;mx1;x2 _qs;m specific to the system representation in the task space. By selecting a

vector of the desired accelerations in the task space as €qdesx1;x2

¼ Js;mx1;x2€qdess;m and a vector of the

equivalent accelerations as €qeqx1;x2

¼ � _Js;m

x1;x2_qs;m � Js;mx1;x2ps;m the dynamics (5.158) reduce

to (5.151). Now the control system design can follow the same steps as shown above.

Selected functional relations can be enforced in remotely placed systems (for example, in

maintaining the relationship of mobile robots), or mechanically coupled systems, as well as

systems of a different nature as long as the compatibility of coordinates is maintained. In that

sense it establishes a framework within which the interconnection is maintained due to control

action which enforces the stability of the functional dependence between the systems’

coordinates. This opens awhole new set of possibilities inmotion control design by introducing


a different definition of tasks as functions to be realized by systems [2,3] or as modes of the

system operation [1].

The structure of the control system is shown in Figure 5.23. Naturally, the structure is very

similar to the one depicted in Figure 5.24 since both deal with the same problem.

The salient feature of the design discussed in this section lies in the specification of

operational requirements in the form of functional constraints on the coordinates of the

interrelated systems. The dynamics describe ‘virtual’ systems with ‘virtual’ control inputs

being related to the control inputs of the original systems. The design is next performed for

the ‘virtual’ systems and then the control is mapped back to the original systems. This places

limitations on the specification of functional relations – effectively the transformation of

coordinates – and the transformation of the ‘virtual’ control.

In order to demonstrate the applicability of the proposed structure the synchronization of the

position of two systems described by the dynamics given in (5.3) will be shown within the

framework of functionally related systems. The solution can be analyzed in the traditional way

– both systems should be set the same references and should ensure the same initial conditions.

In such a solution, feedback from one system to another does not exist and motion may differ

for many reasons. Application of the approach discussed in this section leads to very accurate

tracking of the position of two systems to be synchronized.

Example 5.9 Functionally Related Systems – Synchronization The synchronization of

themotion of two plants on the desired trajectory is shown in this example. The system consists

of two plants. The parameters of the plants are

. plant ‘m’ am ¼ 0:1 1þ 0:25 sin qmð Þ½ � kg;KTm ¼ 0:85 1þ 0:25 cos 12:56tð Þ½ �N=A

. plant ‘s’ as ¼ 0:2 1þ 0:25 sin qsð Þ½ � kg;KTs ¼ 0:65 1þ 0:5 cos 6:28tð Þ½ �N=A

Figure 5.23 Control structure in functionally related systems


Index ‘m’ is for variables and parameters of plant ‘m’ and index ‘s’ denotes variables and

parameters of plant ‘s’. Other parameters are as in Example 5.1. For each plant the

disturbance τdis is estimated by a disturbance observer with _q; iref� �

as inputs and

an;Kn; gð Þ as observer parameters. The observer gain is the same for both plants at

g ¼ 600. The compensated plants are then described by €qm ¼ €qdesm þ pm and

€qs ¼ €qdess þ ps. Here €qdesm and €qdess are the desired accelerations; pm and ps are the disturbance

observer estimation errors.

We would like to synchronize the motion of the ‘m’ and ‘s’ plants on trajectory

qref ¼ 0:1 1þ 0:2 sin 6:28tð Þ½ �m. The positions of both plants are assumed measured.

The solution illustrated here is based on the idea of functionally related systems. Let us

define variables qc ¼ qm þ qs and qd ¼ qm � qs. If qrefc ¼ 2qref and q

refd ¼ 0 then enforcing

qc ¼ qrefc and qd ¼ qrefd gives the unique solution qm ¼ qs ¼ qref . This indicates synchro-

nization of the movement of the two plants formulated as a tracking problem with tracking

errors

ec ¼ qc � qrefc ¼ qc � 2qref

ed ¼ qd � qrefd ¼ qd � 0

ð5:159Þ

Figure 5.24 Synchronized motion of two plants. The diagrams show reference position qref , reference

position qrefc , the position of the ‘m’ and ‘s’ plants, qm and qs respectively, tracking error ec, position

qd ¼ qm � qc and the desired accelerations for the ‘m’ and ‘s’ plants, €qdesm and €qdess . The control parameters

in both loops are k ¼ 150 and a ¼ 0:75


Now synchronization is defined as a simple position control problem for the sum and the

difference of the ‘m’ and ‘s’ system positions. After notation in [1], the qc ¼ qm þ qs will be

called ‘common mode’ and qd ¼ qm � qs will be called ‘difference mode’. The solutions

discussed in Chapters 3 and 4 can be directly applied. Let the generalized errors sc and sd be

sc ¼ cqc þ c1 _qc � 2 cqrefc þ c1 _qrefc

� �; c ¼ 100; c1 ¼ 1

sd ¼ cqd þ c1 _qd � 0ð5:160Þ

The derivatives of errors (5.160) on the trajectories of compensated plants, with neglected

disturbance estimation error, can be written as

_sc ¼ c _qc þ €qc � 2 c _qrefc þ €qrefc

� � ¼ €qdesc � €qeqc ;

€qeqc ¼ 2 c _qrefc þ €qrefc

� �� c _qc

_sd ¼ c _qd þ €qd � 0 ¼ €qdesd � €qeqd ;

€qeqd ¼ 0� c _qd

ð5:161Þ

Here €qdesc ¼ €qm þ €qs and €qdesd ¼ €qm � €qs are control variables in the ec and ed control loops

respectively. The position tracking controllers for qc and qd can be designed as

€qdesc ¼ sat €qeq

c þ €qconc

�€qdesd ¼ sat €q

eq

d þ €qcond

� ð5:162Þ

The acceleration enforcing finite-time convergence in trajectory tracking can be deter-

mined as

€qconi ¼ � k sij j2a� 1sign sið Þ; 1

2< a < 1; i ¼ c; d ð5:163Þ

Equivalent accelerations in (5.162) are estimated by observer with a filter gain

g ¼ 800.

From a definition of the control variables €qc ¼ €qm þ €qs and €qd ¼ €qm � €qs and the dynamics

of the compensated systems, it follows that

€qdesm ¼ €qdesc þ €qdesd

2

€qdess ¼ €qdesc � €qdesd

2

ð5:164Þ

The plants are simulated with initial conditions qm 0ð Þ ¼ 0:05 m, qs 0ð Þ ¼ 0:15 m and

compensation of the generalized disturbance. The convergence accelerations in both loops

have k ¼ 150 and a ¼ 0:75. The diagrams show the reference position qref , the reference

position qrefc , the position of the ‘m’ and ‘s’ plants qm and qs respectively, the tracking error

ec, the position qd ¼ qm � qc and the desired accelerations for ‘m’ and ‘s’ plants €qdesm and


€qdess respectively. The control force has been limited by τmaxj j ¼ 25 N.m. The synchro-

nization is enforced.

References

1. Katsura, S. (2004) Advanced Motion Control Based on Quarry of Environmental Information, PhD Thesis, Keio

University, Yokohama, Japan.

2. Tsuji, T. (2005) Motion Control for Adaptation to Human Environment, PhD Thesis, Keio University, Yokohama,

Japan.

3. Tsuji, T., Ohnishi, K., and Sabanovic, A. (2007) A Controller design method based on functionality. IEEE

Transactions on Industrial Electronics, 54(6), 3335–3343.

4. Cortesao, R. (2002) Kalman techniques for intelligent control systems: theory and robotic experiments, PhD thesis,

University of Coimbra, Coimbra, Portugal.

5. Cortesao, R., Park, J. and Khatib, O. (2003) Real-time adaptive control for haptic manipulation with active

observers. Proceedings of the International Conference on Intelligent Robots and Systems, Las Vegas.

Further Reading

Cortesao, R. (2002) Kalman Techniques for Intelligent Control Systems: Theory and Robotic Experiments, PhD

Thesis, University of Coimbra, Coimbra, Portugal.

Cortesao, R., Park, J., and Khatib, O. (2003) Real-time adaptive control for haptic manipulation with active observers.

Proceedings of the International Conference on Intelligent Robots and Systems, Las Vegas.

Hogan, N. (1989) Controlling impedance at the man/machine interface. Proceedings of the IEEE International

Conference on Robotics and Automation, pp. 14–19.

Katsura, S., Matsumoto, Y., andOhnishi, K. (2005) Realization of “Law of action and reaction” bymultilateral control.

IEEE Transactions on Industrial Electronics, 52(5), 1196–1205.

Ohnishi, K., Shibata, M., and Murakami, T. (1996) Motion control for advanced mechatronics. IEEE/ASME

Transactions on Mechatronics, 1(1), 56–67.€Onal, C.D. (2005) Bilateral Control – A Sliding Mode Control Approach, MSc Thesis, Sabanci University, Istanbul,

Turkey.

Salisbury, J.K. (1980) Active stiffness control of a manipulator in cartesian coordinates. Proceedings of the 19th IEEE

Conference on Decision and Control, pp. 95–100.

Tsuji, T. and Ohnishi, K. (2004) Position/force scaling of function-based bilateral control system. IEEE International

Conference on Industrial Technology, pp. 96–101.

Utkin, V., Guldner, J., and Shi, J. (1999) Sliding Mode Control in Electromechanical Systems, Taylor and Francis,

New York.


6

Bilateral Control Systems

In this chapter we will examine the design of control structures enabling a human operator to

interact with a remote environment through interface devices. This problem has attracted

considerable attention recently and is expected to be an emerging point of modern develop-

ments in robotics, microparts handling, control theory and virtual reality systems. Its potential

applications include network robotics, telesurgery, space and seabed telemanipulation, micro-

nanoparts handling, inspection and assembly.

In such systems the operator’s action on the environment are realized by two functionally

related systems (devices). The operator is in contact with one of the devices (master device),

and another device (slave device) is in contact with the environment. By manipulating the

master device the operator defines the tasks to be assigned to the slave device. The master

device in addition to acquiring motion data from the operator is able to exert force on the

operator. The slave device replicates the operator motion at the remote site, senses interactions

with the environment and transmits information on its motion and interaction force to the

master device and operator. Such a functional relationship is known as a bilateral system.

Both the master and slave devices have local controllers providing at least some protection

and possibly a certain degree of autonomy. Bilateral control structure is responsible for the

appropriate behavior of the slave according to the task assignment by the operator and the

environment the slave is interacting with. The bilateral controller is also responsible for

providing the operator with appropriate information through the specific master device. Thus,

the bilateral controller is responsible for giving the user the correct perception about the task

execution. Within a bilateral system information is exchanged in two directions. The structure

enables the human operator to assign tasks to the slave and to receive information about the task

execution. Thus through communication channels all components of the system are

interconnected.

In literature there are many different definitions for bilateral control, teleoperation, and

haptics. The reason for that is the usage of the same terms for similar concepts in different

contexts by people from different areas of expertise.

1. Touch is a generic term for several senses that respond to stimuli directly in contact with the

skin. Within the touch perception, several subdomains can be identified [1]:



. Dynamic touch is the ability to sense certain physical properties of an object bymoving it

around,. Haptic touch refers to a active exploration of objects or surfaces using the sense of touch.

2. Force perception is the ability to detect external forces working on the human body. It

depends on sensitivity to the muscular opposition that resists mechanical forces.

3. Teleoperation stands for operation of system from a remote location.

4. Bilateral control stands for functionally related systems inwhich the slave device replicates

operator motion at the remote site and themaster device exert a force on the human operator

equal to the interaction force exerted on the environment by the slave device.

The ideal behavior of two functionally related systems can be defined in the following

sense [2]:

(a) The position responses onmaster side and on the slave side produced by the operators input

are identical whatever the dynamics of manipulated object is;

(b) The force responses on master side and on the slave side produced by the operators input

are identical whatever the dynamics of manipulated object is;

(c) Both the position responses onmaster side and on the slave side and the force responses on

master side and on the slave side produced by the operators input are identical whatever the

dynamics of manipulated object is.

Situation (c) describes a bilateral control system. In loose terms it gives to the operator the

ability to operate an object or perform a task at a distance while sensing the force developed on

the slave side – thus having the feeling that the operator himself is operating the object on the

slave side. Such an operation is defined as ‘ideal kinesthetic coupling.’ One has to be careful

with understanding of the definition in point (c) from the point of the capability of systems to

control concurrently the position and the force.

There aremany questions related to design of bilateral system. The basic one is related to the

possibility of designing a control that will guarantee finite-time or at least asymptotic

convergence to condition (c). In this section the first question of enforcing the convergence

and stability of condition (c) will be discussed. Other problems related to bilateral control will

be addressed later.

In a bilateral system there are four players: (i) operator, (ii) master side device (robot),

(iii) slave side device (robot) and (iv) object to bemanipulated. All of these players have their

own dynamical properties. The inputs to the system are operator’s force and motion and the

force generated due to interaction of the slave side and the object to be manipulated.

6.1 Bilateral Control without Scaling

In this section both the master side and slave side devices are assumed 1-dof systems as

described in (6.1) and (6.2), respectively. Operator motion and the interaction force between

operator and the master device are given in (6.3). The operator is modeled as a full 1-dof

mechanical systemwith the operator force top and the interaction forcewithmaster device fh as

the inputs. Interaction between the master device and operator is modeled as spring-damper

system (6.3). The interaction force between the environment and the slave device fs is modeled


as in (6.4).

am qmð Þ€qm þ bm qm; _qmð Þþ gm qmð Þ ¼ tm � tmext � fh ð6:1Þas qsð Þ€qs þ bs qs; _qsð Þþ gs qsð Þ ¼ ts � tsext � fs ð6:2Þah qhð Þ€qh þ bh qh; _qhð Þþ gh qhð Þ ¼ top � fh ð6:3ÞDh _qm � _qhð ÞþKh qm � qhð Þ ¼ fh

Me €qs � €qeð ÞþDe _qs � _qeð ÞþKe qs � qeð Þ ¼ fs ð6:4Þ

In Equations (6.1)–(6.4) the index ‘m’ is used for master side parameters and variables,

index ‘s’ is used for slave side device parameters and variables, index ‘h’ is used for human

operator related parameters and variables, index ‘e’ is used for environment parameters and

variables and text stands for the external force on the master and slave sides. Model (6.1)–(6.4)

describes a bilateral system in configuration space.

The overall system is ninth order with the dynamics of both the operator (6.3) and the

environment (6.4) assumed as moving bodies. These are the most general models and in

particular cases, as shown in Chapter 5, they can be described by simpler models. The

master (6.1) and the slave (6.2) are active systemswith tm and ts as the control forces andwith fhand fs as the interaction forces on master and slave side, respectively. The slave device is

assumed to be in contact with the environment moving on a trajectory defined by position qe,

velocity _qe and acceleration€qe. Interaction force fs exists only if there is contact between slavedevice and environment. In our analysis we will assume fs � 0. The properties of the object-

environment in the contact point are defined byMe;De and Ke. The motion of environment is

external input to the system and is assumed unknown.

The force exerted by the operator depends on the motion of operator and the motion of the

master device. Assuming that the properties of the operator in the contact point are given by

Dh;Kh and the motion is defined by position qh, velocity _qh and acceleration €qh then the forcedue tomotion of themaster device is defined as in (6.3). The position dictated by the operator is

treated in the same way as the position of the environment. Similarly as the slave side

interaction force the force fh exists only if contact is established. We assume that the contact is

such that the master device can have both pull and push forces.

The forces and positions on the master and slave sides are assumed measured. The object-

environment to be manipulated and the operator’s hand are in general modeled as full second-

order mass–spring–damper systems. Depending on the properties of the operator and the

environment the properties may be modeled as a spring–damper or as only a spring. The

structure depicted by (6.1)–(6.4) depicts four body-systems in interaction.

Control is to be selected to maintain bilateral relations between the human operator–master

and the slave–environment systems. The operator input force top is an active input to the systembut it cannot be changed by the system components – thus it plays the role of a reference. It will

be assumed that the operator himself reacts on the appearance of the interaction force on the

slave side by changing its position and thus adjusting themagnitude of the interaction force and

the movement of the slave device. The role of the master side device is two-fold – the setting of

the position reference for the slave side and the generation of force fhwhich is equal to the slave

side interaction force. Ideally, the force fh resists the operator’s motion in the same way as

the force of a direct touch on the operator hand with a manipulated environment would do.

Bilateral Control Systems 235

Thus system has three active command inputs top; tm; ts� �

, where top is treated as an input thatcannot be changed by the control – thus it dictatesmotion on themaster side, and ideally if there

is no interaction force between the slave device and the environment fh is equal to zero and the

operator position, master device position and slave device position are the same. In some cases

we may assume that the operator position qh is an input to the system, thus setting the desired

motion for the slave device. Then the operator force is changed in such a way as to generate

the desired position. Forces tm; tsð Þ are control inputs on the master and the slave side,

respectively. The interaction force fs on the slave side appears as a result of the interaction of the

slave side device and the environment. The general structure of the bilateral system is shown in

Figure 6.1.

As depicted in Figure 6.1 themaster device (robot) acts as an environmentwith respect to the

human operator; and the force due to the interaction between operator and the master device is

fh. Force fh is impressed on the operator by the master system. The force extended to the

operator is equal to the force generated by the relative motion of the operator and the master

device. That force may be controlled to be equal to the slave side interaction force, which

appears due to the interaction with the environment, as depicted in Equation (6.4). Thus if

fs ¼ � fh the operator will sense a force equal to the slave side interaction force and thus will

have a sense of direct touch.

As mentioned earlier, the operator is a part of the bilateral system structure which sets

motion by creating a force balance on themaster side and in that way causingmotionwithin the

overall system. The relationship between components (6.1)–(6.4) of a bilateral control system

is shown in Figure 6.2. The role of the bilateral controller is to determine the control forces for

master and slave systems based on the known positions of master and slave systems qm and qs,

the interaction forces between slave device and environment fs and the master device and

operator fh. This structure differs from structures based on impedance analysis [3,4] in which

velocities instead of positions stand for measured variables.

Figure 6.1 Structure of a bilateral system as described by (6.1)–(6.4)


The system shown in Figure 6.2 functionally relates the position and forces of the operator

and the environment, having master and slave devices as the interface systems. The general

relationship between position and forces in bilateral control can be described using the

so-called hybrid matrix H [2,5]

fhqs

� �¼ h11 h12

h21 h22

� �qmfs

� �¼ H

qmfs

� �ð6:5Þ

The four parameters hij ; i; j ¼ 1; 2 have specific physical meanings: h12 corresponds to the

accuracy of force reflection (the force tracking), h21 refers to the position tracking of the slave

with respect to the master, h11 denotes the master-side mechanical impedance and h22 can be

interpreted as a slave-side mechanical admittance.

The goal of bilateral control is to approximate the ‘ideal teleoperator’ as closely as possible.

This ideal functional relation provides transparency: the operator feels as if he is manipulating

the remote environment directly. The bilateral control desired (ideal) conditions can be

represented in the following H matrix

fhqs

� �¼ 0 � 1

1 0

� �qmfs

� �ð6:6Þ

If conditions (6.6) are satisfied, the operator feels the environmental impedance. Indeed

if fs ¼ Zsqs is inserted in (6.5) then the operator force becomes fh ¼ h11qm � Zeqsh12h21=1� h22Zeð Þ. Under conditions (6.6) the interaction force is obtained as fh ¼ Zeqm, thus the

operator feels directly the environmental impedance. Ideal bilateral system conditions (6.6) or

bilateral systemoperational requirementsmay be formulated as in (6.7). Control inputs tm; tsð Þshould be selected in such away that both position error eqb and force error eFb on the trajectories

of system (6.1)–(6.4) tend to zero, at least with asymptotic convergence.

Sb ¼ qm; qs : eqb qm; qsð Þ ¼ qm � qs ¼ 0 and eFb fh; fsð Þ ¼ fh þ fs ¼ 0� � ð6:7Þ

From (6.7) it appears that the slave side motion is required to track the master side motion,

while the master side will resist motion dictated by a human operator with a force equal to the

interaction force on the slave side. If there is no interaction with the environment, the force

exerted by the master device to the operator is zero. This makes operator to feel the same force

as he/she is manipulating/touching object directly or by rigid stick, as illustrated in Figure 6.1.

In further analysis it is assumed that the input generated by the operator creates a balance of the

forces on the master side in such a way that slave-side position tracking is within the

capabilities of the slave system.

The bilateral control problem can now be formulated within the framework of functionally

related systems as discussed in Chapter 5. The bilateral control as a task to maintain

Figure 6.2 Structure and components of a bilateral control


concurrently position tracking and force tracking as defined in (6.7). The description of the

system motion in configuration space is given by (6.1)–(6.4). The task coordinates and the

operational task are defined by (6.6) or (6.7).

The role of the operator is to set the movement of the system and to dictate the position and

the interaction forces. From that point of view the operator force top (or operator position qh) isan input to the system while the control input to the master and slave devices tm; ts (or

corresponding desired accelerations) enforces a functional relation between the master and

slave devices and their interactions with operator and environment. The master and the slave

devices are acting as ‘agents’ for the human operator, extending the motion dictated by the

operator to the slave side and transmitting the interaction force from the slave to the operator. If

both the position tracking error and the force tracking error are enforced to be zero functionally

the dynamics of the master and slave devices is fully compensated and the interaction realized

by the bilateral system realizes a ‘virtual stick’ – a rigid coupling interaction between operator

and environment.

6.1.1 Bilateral Control Design

Before attempting to design the control enforcing requirements (6.7) assume that

disturbance compensation is applied to both slave side and master side devices so they can

be modeled as

amn€qm ¼ amn€qdesm � pm Qdm; tdism

� �asn€qs ¼ asn€q

dess � ps Qds; tdiss

� � ð6:8Þ

The desired accelerations €qdesm and €qdess are considered as control inputs on the master and

slave side, respectively.

The disturbances on the master and slave side include parameter variations and all external

forces acting on each system

tmdis ¼ am � amnð Þ€qm þ bm qm; _qmð Þþ gm qmð Þþ fh þ texttsdis ¼ as � asnð Þ€qs þ bs qs; _qsð Þþ gs qsð Þþ fs

pm Qdm; tmdisð Þ ¼ tmdis � tmdis

ps Qds; tsdisð Þ ¼ tsdis � tsdis

ð6:9Þ

Disturbance observers on the master and slave side provide a reasonable accuracy of

compensation ofmaster and slave side disturbances. The errors in disturbance estimation on the

master and slave side are defined by the selection of disturbance observer dynamics Qdm and

Qds, respectively.

Information on position, velocity and forces on both the master and slave sides are assumed

available by measurement or estimation. Relations (6.8) assume sufficient control resources to

compensate all disturbances on the master and slave side, thus any interaction between the

master and slave systems should be established by the control inputs. In further analysis the

error due to disturbance estimation on the master and slave side will be treated small enough

pm Qdm; tmdisð Þ � 0 and ps Qds; tsdisð Þ � 0 so that their influence on the system behavior will be

neglected, if necessary.


Compensated master and slave systems (6.8) expanded by the models of the interaction

forces on the master and slave side have two control inputs€qdesm and€qdess . The task is defined by

the two linearly independent functional requirements (6.7) and thus the number of controls and

the number of the independent task variables in the system under consideration is the same.

Let us first analyze bilateral control, assuming that the properties of the environment and

human operator can be modeled by spring–damper. Then model (6.4) is valid withMe ¼ 0 and

the bilateral task specification (6.7) can be applied without changes.

Position tracking is the same as the one analyzed in Chapter 5. The second derivative of the

position error on the trajectories of system (6.8) can be written as

€eqb ¼€qm �€qs ¼ €qdesm �€qdess

� ��pm

amn

� ps

asn

¼ €qdesqb � pqb

€qdesqb ¼€qdesm �€qdess ;

pqb ¼�

pm

amn

� ps

asn

ð6:10Þ

The position tracking control input€qdesqb is the difference between the desired accelerations of

the master and slave side and the disturbance in position control is given by pqb. By selecting

desired acceleration

€qdesqb ¼ pqb �KD _eqb �KPeqb ð6:11Þ

the dynamics in the position tracking loop will be forced to satisfy

€eqb þKD _eqb þKPeqb ¼ pqb � pqb

�ffi 0 ð6:12Þ

Here KD;KP are design parameters and should be selected such that characteristic equation

in (6.12) has the desired roots. The pqb is estimated disturbance in (6.8). In properly designed

systems the disturbance compensation error ðpqb � pqbÞ in (6.12) can be neglected. For

estimation of the disturbance in (6.10), the application of observers discussed in Chapter 4

is straightforward.

Another solution is the selection of a generalized position error sqb ¼ ceqb þ _eqb with

dynamics

_sqb ¼ c _eqb þ€eqb ¼ €qdesqb �€qeqqb ð6:13Þ

Here€qdesqb ¼ €qdesm �€qeqs stands for the desired acceleration and€qeqqb ¼ pqb � c_eqb stands for theequivalent acceleration in the generalized error dynamics.

The first-order time derivative of the force tracking error eFb ¼ fh þ fs from (6.3) and (6.4)

with Me ¼ 0, and (6.8) can be expressed as

_eFb ¼ _f h þ _f s ¼ Dh €qdesFb �€qeqFb� �

€qdesFb ¼€qdesm þ€qdess

€qeqFb ¼ €qh þ€qs �KhD� 1h _qm � _qhð Þ�D� 1

h_f s

� þ pFb

pFb ¼�

pm

amn

þ ps

asn

ð6:14Þ


Here Dh stands for the nominal damping coefficient of the operator and is assumed known.

The €qeqFb stands for the equivalent acceleration in the force control loop and €qdesFb stands for the

desired acceleration input in force control loop. The error dynamics is described by two first-

order plants (6.13) and (6.14) as shown in Figure 6.3.

Both the generalized position tracking error and force tracking error dynamics have the

same form as a generalized output tracking error (3.22), thus the solutions discussed so far for

position tracking and force tracking can be directly applied.

The asymptotic convergence is enforced if the desired accelerations are selected as

€qdesqb ¼ €qeq

qb� kqbsqb; kqb > 0

€qdesFb ¼ €qeq

Fb � kFbD� 1h eFb; kFb > 0 ð6:15Þ

Here €qeq

qb and €qeq

Fb are estimated equivalent accelerations in the position and force

control loop respectively.

Finite-time convergence will be enforced is the desired accelerations are selected as

€qdesqb ¼ €q eqqb � kqb sqb

�� 2a� 1sign sqb

� �; kqb > 0

€qdesFb ¼ €qeq

Fb � kFb eFbj j2a� 1sign eFbð Þ; 1

2< a < 1; kFb > 0 ð6:16Þ

Equivalent accelerations in both loops can be estimated by observers. From measurement

of the desired acceleration and the generalized error ð€qdesqb ; sqbÞ the equivalent acceleration

in the position tracking loop can be estimated by

_zq ¼ g €qdesqb þ gsqb � zq

�€qeq

qb ¼ zq � gsqb; g > 0 ð6:17Þ

The same structure of the observer can be applied for equivalent acceleration estimation in

the force control loop. Frommeasurement €qdesFb ; eFb� �

and dynamics (6.14) the€qeqFb observer canbe written in the following form

_zF ¼ g €qdesFb þ gD� 1h eFb � zF

� �€qeq

Fb ¼ zF � gD� 1h eFb; g > 0 ð6:18Þ

Figure 6.3 Bilateral system control error dynamics as shown in (6.13) and (6.14). (a) Dynamics of the

position generalized error. (b) Dynamics of the force error


The observers in position and force loop may be selected with different bandwidths if

required. The structure of the position tracking and the force tracking loops with equivalent

acceleration observers (6.17) and (6.18) are shown in Figure 6.4. The structure is the same for

both control errors. Simple proportional controllers with estimated equivalent acceleration

enforce an exponential convergence to an equilibrium solution.

With control inputs (6.15) transients in position and force control loops are obtained as

_sqb þ kqbsqb ¼ €qeq

qb�€qeqqb ffi 0

_eFb þ kFbeFb ¼ €qeq

Fb �€qeqFb ffi 0 ð6:19Þ

Dynamics in both loops are described by first-order transients. From (6.19) and

sqb ¼ ceqb þ _eqb the dynamics of the position tracking error can be determined as

€eqb þ kqb þ c� �

_eqb þ kqbceqb ¼ €qeq

qb �€qeqqb ffi 0 ð6:20Þ

The equilibrium solution is qm ¼ qs – thus the position of themaster and slave devices is the

same. The force balance established by the force tracking loop enforces the following dynamics

Dh€qm þ Kh þ kFbDhð Þ _qm þ kFbKhqm þDe€qs þ Ke þ kFbDeð Þ _qs þ kFbKeqs

¼ Dh€qh þ h Kh þ kFbDhð Þ _qh þ kFbKhqh þDe€qe þ Ke þ kFbDeð Þ _qe þ kFbKeqe ð6:21Þ

The steady-state solution for a bilateral system (6.20) and (6.21) gives a static balance of

the forces Kh qm � qhð Þ ¼ �Ke qs � qeð Þ ¼ �F where F is the value of the interaction

force acting on the environment. The steady-state value of the interaction force can be

derived as

Figure 6.4 Structure of control systems (6.15) for virtual plants (6.13) and (6.14) with proportional

controller and equivalent acceleration observer. (a) Virtual plant (6.13) (b) Virtual plant (6.14)


F ¼ KeKh

Kh þKe

qh � qeð Þ ð6:22Þ

For interaction force (6.22) steady-state solution for the master and slave side positions

qm ¼ qs is

qm ¼ qs ¼ Kh

Kh þKe

qh þ Ke

Kh þKe

qe ð6:23Þ

The interaction force and the master and slave device positions are determined by

the properties of the operator and environment in contact points and the corresponding

motion of operator and environment. The properties of the master and slave devices do not

influence the steady-state force and position. In a steady state only the spring constants

contribute to the interaction force, thus the model simplifies to two springs with spring

constants Kh and Ke connected in series with motion qh and qe acting in opposite directions.

The deviation of the master device position from the operator position depends on the ratio

KeK� 1h : the smaller the ratio the closer the positions of the master and slave devices to the

operator position.

Control errors eqb and eFb converge asymptotically to zero and the bilateral system task

requirements (6.7) – the trajectory tracking and the force tracking are enforced concurrently.

The presence of dynamic error means that, strictly speaking, the full tracking on themaster and

slave side is an asymptotic feature of the system while the real behavior will have some

dynamic distortions. These distortions depend on the controller design and in general can be set

as part of the control system specification. A block diagram of the bilateral control system is

depicted in Figure 6.5. The structure is the same as shown for the functionally related systems.

From the desired accelerations €qdesFb ¼ €qdesm þ€qdess and €qdesqb ¼ €qdesm �€qdess the desired accel-

erations of the master and slave devices can be determined as

Figure 6.5 Structure of bilateral system (6.24) with virtual plant controllers as in Figure 6.4


€qdesm ¼ 1

2€qdesFb þ€qdesqb

�€qdesm ¼ 1

2€qdesFb �€qdesqb

� ð6:24Þ

The corresponding control forces realizing the desired accelerations can be expressed as

tm ¼ sat tdism þ anm€qdesm

� �and ts ¼ sat tdiss þ ans€q

dess

� �for the master and slave devices,

respectively.

Bilateral control with finite-time convergence will not change steady-state behavior. The

force balance will be reached in finite-time. The position tracking transient will be determined

by the equilibrium of the generalized position tracking error sqb ¼ _eqb þ ceqb ¼ 0. These

properties of systems with finite-time convergence are discussed in detail in Chapter 3.

Example 6.1 Bilateral Control without Scaling The goal of the examples in this chapter

is to expand on the results shown in previous chapters and illustrate the control of bilateral

systems without and with a delay in the communication channels. The components of the

bilateral system (6.25)–(6.28) will be modeled as:

am qmð Þ€qm þ bm qm; _qmð Þþ gm qmð Þþ tmext ¼ tm � fh

tm ¼ am€qdesm ¼ KTmim

ð6:25Þ

as qsð Þ€qs þ bs qs; _qsð Þþ gs qsð Þþ tsext ¼ ts � fs

ts ¼ as€qdess ¼ KTsis

ð6:26Þ

aop qhð Þ€qh þ bop qh; _qhð Þþ gop qhð Þ ¼ top � fh

top ¼ aop€qdesop ¼ KTopiop

Mh €qm �€qhð ÞþDh _qm � _qhð ÞþKh qm � qhð Þ ¼ fh

ð6:27Þ

Me €qs �€qeð ÞþDe _qs � _qeð ÞþKe qs � qeð Þ ¼ fs ð6:28ÞThe plants are assumed to be 1-dof devices with linear motors as actuators. The parameters

of the plants are selected as follows

am ¼ 0:1 1þ 0:5 cos 12tð Þ½ � kg

bm qm; _qmð Þ ¼ � 16qm � 8 _qm N

gm qmð Þ ¼ 0 N

tmext ¼ 0 N

KTm ¼ 0:95 1þ 0:2 sin 21tð Þ½ � N=A

as ¼ 0:2 1þ 0:5 cos 12tð Þ½ � kg

bs qs; _qsð Þ ¼ � 5qs � 2 _qs N

gs qsð Þ ¼ 0 N

tsext ¼ 0 N

KTs ¼ 0:65 1þ 0:25 sin 18tð Þ½ � N=A

The properties of the operator and the environment in the interaction points are defined by

. for operator

Mh ¼ 0 kg

Dh ¼ 5 kg=s

Kh ¼ 150 000 kg=s2

. for environment

Me ¼ 0 kg

De ¼ 5 kg=sKe ¼ 225 000 kg=s2


For both the master and slave devices the disturbance tdis is estimated by disturbance observer.

The observer gain g ¼ 1200 is the same for both plants.

The position of environment is simulated as

qe tð Þ ¼ qe0 þ qe1 sin vqet� �

m ð6:29ÞHere parameters qe0; qe1;vqe are set for every experiment.

In order to have realistic illustrations in which the operator would react on the appearance of

the interaction force by modifying its motion, the operator motion is controlled by a

compliance controller. The structure of such a controller is shown in detail in Chapter 5 and

Examples 5.4 and 5.5. The operator reference motion is qrefh tð Þ ¼ qh0 þ qh1 sin vqht

� �m,

where parameters qh0; qh1;vqh are selected in each experiment. The operator motion is the

result of the compliant control

€qdesh ¼€qrefh þ ch _qrefh � _qh

�þ kh ch q

refh � qh

�þ _qrefh � _qh

�h iþ lhfh

top ¼ ah€qdesh

ð6:30Þ

Parameters ch; kh; lh are all positive constants and will be selected in each experiment. In

some examples the operator tracking of the reference will be enforced, thus lh ¼ 0 will be

selected. In that case the interaction force between the operator and the master device will be

determined only by the motion of the master device. This will result in the error in position

tracking between the operator and the master device – thus the error in position tracking

between operator and slave device. This error is the result of the interaction force balance on the

master and slave side.

In this example we will illustrate bilateral control with position tracking and the force

tracking errors aseqb ¼ qm � qs

eFb ¼ fh þ fsð6:31Þ

The generalized position tracking error s is selected as

sqb ¼ ceqb þ _eqb; c ¼ 100 ð6:32Þ

The position tracking controller is designed as in (6.15)

€qdesqb ¼ €qeq

qb þ€qconqb ð6:33Þ

Here €qdesqb stands for the desired acceleration in position tracking, €qeq

qb stands for

equivalent acceleration estimated by observer (6.17) with observer filter gain g ¼ 1200 and

€qconqb ¼ � kqbsqb; kqb ¼ 100 is the convergence acceleration.

The desired acceleration in the force control loop €qdesFb is selected as

€qdesFb ¼ €qeq

Fb þ€qconFb

€qconFb ¼ � 100eFb

ð6:34Þ

Equivalent acceleration €qeq

Fb is estimated by a observer (6.18) with observer filter gain

g ¼ 1200.


The reference operator position parameters are qh0 ¼ 0 m; qh1 ¼ 0:005 m; vqh ¼6:28 rad=s, the operator position compliance controller parameters are ch ¼ 100; kh ¼ 25;lh ¼ 0, thus the operator position tracks its reference. The parameters of the position of

environment are qe0 ¼ 0 m; qe1 ¼ 0:0045m; vqe ¼ 1:57 rad=s.From the definition of the task variables (6.31) the desired accelerations for the

master and slave devices can be expressed as

€qdesm ¼€qdesFb þ€qdesqb

2

€qdess ¼€qdesFb �€qdesqb

2

ð6:35Þ

Transients in Figure 6.6 show that both position tracking and force tracking are achieved.

The master and slave device positions change with the appearance of the interaction forces on

Figure 6.6 Transients in bilateral control system. The left column shows transients in the position of

operator qh and its reference qrefh , the position of the environment qe, position ofmaster device qm, position

of slave device qs and the desired accelerations in the position tracking€qdesqb . The right column shows the

difference between the positions of operator and master device eh ¼ qh � qm, the interaction forces of

slave devicewith the environment fs, the interaction force induced by master device to operator fh and the

desired acceleration in the force tracking loop€qdesFb . The position of the operator is set to track its reference

so the changes in the master device position generate the master side interaction force


the master and slave side, while the operator position tracks its reference. The amplitude of the

forces on the master and slave side are not controlled and they are established by the system

balances. Force tracking is clearly illustrated. The difference in position between the operator

and the master device illustrates the changes due to the master side interaction force.

In Figure 6.7 the same transients as in Figure 6.6 are shown. Here the compliance coefficient

in the operator position control loop lh ¼ 1:85 is applied. The operator reference position andall other parameters are kept as in Figure 6.6. The result of introducing the compliance of the

operator results in changes in the operator position and changes in the amplitude of the

interaction forces. The diagrams clearly show compliance with the environment and a

modification of the operator position due to the interaction force. The error between operator

position and master device position and the amplitude of the interaction force are now smaller

than in the experiment shown in Figure 6.6. This illustrates that the interaction between the

operator and the remote environment on the slave side is clearly established and that the

operator can set bothmotion and force on the distant environment. By changing the compliance

coefficient lh the interaction force can be controlled.

Figure 6.7 Transients in a bilateral control system. The left column shows transients in the reference

position of the operator qrefh , the position of the environment qe, position of master device qm, position of

slave device qs and the desired accelerations in the position tracking €qdesqb . The right column shows the

difference between positions of the operator and the master device eh ¼ qh � qm, the interaction forces of

slave devicewith the environment fs, the interaction force induced by master device to operator fh and the

desired acceleration in the force tracking loop€qdesFb . The position of the operator is set to track its reference

with compliance lh ¼ 1:85, thus it changes at the appearance of the interaction force


As shown in (6.22) and (6.23) the steady-state operation satisfies transparency condi-

tions (6.6). The dynamic properties must be analyzed from the closed loop dynamics of the

control errors and the functional relations (6.5). Under ideal compensation of the disturbances

for control (6.15) the H matrix can be derived

fhqs

� �¼ 0 � 1

1 0

� �qmfs

� �þ pm

ps

� �ð6:36Þ

The control (6.15) enforces exponential convergence in both position and force loops. The

dynamics of the state coordinates under the assumption that zero error in bilateral control is

reached should be investigated in order to confirm the stability of the overall system. After

equilibrium (6.7) is reached the force balance is enforced but the trajectory tracking error is

governed by the transient sqb ¼ _eqb þ ceqb ¼ 0. After the position converges, the tran-

sient (6.21) governs the changes in the position of the master and slave devices with the

transient as expressed by

Dh þDeð Þ€qm þ Kh þKe

Dh þDe

þ kFb

0@

1A _qm þ kFb

Kh þKe

Dh þDe

qm

¼ Dh€qh þ Kh þ kFbDhð Þ _qh þ kFbKhqh

Dh þDe

þ De€qe þ Ke þ kFbDeð Þ _qe þ kFbKeqe

Dh þDe

ð6:37Þ

It appears that the bilateral system zero dynamics depend on the environment and the

operator’s parameters. The steady-state position is directly determined by the operator and

environment positions and by the combined parameters of the environment and the operator.

Now the role of the operator in the bilateral system is apparent. In order to generate the desired

motion the operator motion qh, and thus the operator force top, must be adjusted to compensate

the dynamical forces on the master and slave sides.

6.1.2 Control in Systems with Scaling in Position and Force

In some applications (for example, manipulation of microparts) the forces and motions at the

operator side and on the slave side may not be the same. That would require scaling of the

variables between the master and slave system by some constant or by time-varying

coefficients. In this situation the bilateral system operational requirements (6.7) should be

modified

Sb ¼ qm; qs : eqb qm; qsð Þ ¼ qm �aqs ¼ 0 & eFb fh; fsð Þ ¼ fh þbfs ¼ 0� � ð6:38Þ

Here a and b are position scaling and force scaling coefficients, respectively. In further

analysis these coefficients will be assumed to be time-varying strictly positive scalar functions.

In the same way as in a bilateral system without scaling, here control may be formulated as

the selection of the control inputs €qdesm and €qdess such that the motion of system (6.3), (6.4)

and (6.8) is stable in the manifold (6.38).


The derivative of the position generalized error sqb ¼ ceqb þ _eqb on the trajectories of

system (6.8) can be expressed as

_sqb ¼€eqb þ c _eqb ¼ €qdesqb �€qeqqb

€qdesqb ¼€qdesm �a€qdess

€qqbeq ¼ caþ 2 _að Þ _qs þ c _aqs � c _qm þ pqb

pqb ¼�

pm

amn

� apsasn

ð6:39Þ

Now the desired acceleration is a function of the scaling coefficient and the equivalent

acceleration is a function of position and velocities.

The derivative of the force tracking error eFb ¼ fh þbfs on the trajectories of system (6.8)

can be expressed as

_eFb ¼ _f h þb_f s þ _bfs ¼ Dh €qdesFb �€qeqFb� �

€qdesFb ¼€qdesm þb€qdess

€qeqFb ¼ b€qs þ€qhð Þ�D� 1h Kh _qm � _qhð ÞþbDe €qs �€qeð ÞþbKe _qs � _qeð Þþ _bf

� þ pFb

pqb ¼�

pm

amn

þ bpsasn

ð6:40Þ

€qdesFb stands for the desired acceleration, and€qeqFb stands for the equivalent acceleration in theforce tracking loop.

The dynamics of the control errors (6.39) and (6.40) are expressed in the sameway as control

errors (6.13) and (6.14) thus, selection of the desired acceleration can follow the same

procedure. Selecting control as in (6.15) will enforce exponential convergence. The finite-

time convergencewill be enforced if control is selected as in (6.16). Observers (6.17) and (6.18)

can be directly applied for the estimation of €qeqqb and €qeqFb, respectively. Consequently the

transients of the transformed system may be expressed as in (6.19).

The desired acceleration inputs for master €qdesm and slave manipulators €qdess can be

determined as

€qdesm ¼ 1

aþba€qdesFb þb€qdesqb

�€qdess ¼ 1

aþb€qdesFb �€qdesqb

� ð6:41Þ

The structure of the bilateral control with scaled positions and forces is shown in Figure 6.8.

In comparison with the system shown in Figure 6.5 the structure of the task variables and the

transformation of control from task space to master and slave control is different.

From (6.19) and sqb ¼ ceqb þ _eqb the dynamics of the position tracking error can be

determined as

€eqb þ kqb þ c� �

_eqb þ kqbceqb ¼ €qeqqb � €qeq

qb � 0 ð6:42Þ


The equilibrium solution is qm ¼ aqs. The steady-state solution for a scaled bilateral systemgives static balance of the forces Kh qm � qhð Þ ¼ �bKe qs � qeð Þ ¼ �F, where F is the value

of the interaction force acting on the environment. The steady-state value of the interaction

force is

F ¼ bKeKh

aKh þbKe

qh �aqeð Þ ð6:43Þ

For force (6.43) steady-state solution for the master and slave side positions qm ¼ aqs

qs ¼ Kh

aKh þbKe

qh þ bKe

aKh þbKe

qe

qm ¼ aKh

aKh þbKe

qh þ abKe

aKh þbKe

qe

ð6:44Þ

Example 6.2 Bilateral Control with Position and Force Scaling In this example the

same plants and under the same conditions as in Example are simulated to illustrate the scaled

operation of bilateral systems.Wewill illustrate bilateral control with position tracking and the

force tracking errors as

eqb ¼ qm �aqseFb ¼ fh þbfs

ð6:45Þ

The generalized position tracking error sqb and the desired accelerations are selected as

in (6.32), (6.34). The references and other parameters are kept the same as in Example 6.1.

Compliance on the operator side lies with the same coefficient as in Example 6.1, Figure 6.7.

From the definition of the task variables the desired accelerations for the master and slave

devices can be expressed as

Figure 6.8 Structure of a bilateral system with scaling in position and force loops


€qdesm ¼ 1

aþba€qdesFb þb€qdesqb

�ð6:46Þ

€qdess ¼ 1

aþb€qdesFb �€qdesqb

�ð6:47Þ

Figure 6.9 shows the transients in the bilateral control systemwith force and position scaling

a ¼ 1:5; b ¼ 1:5 and the operator compliance coefficient lh ¼ 1:85.The transients depict the scaling of both position and force. During interaction with the

environment the slave device tracks the environment position and the operator and master

device have larger amplitudes. The same pattern is shown by the ratio of forces. This clearly

illustrates the possibility to scale down the motion and forces on the slave side.

In Figure 6.10 the scaling parameters area ¼ 0:5; b ¼ 1:5while all other conditions are thesame. Both scaled position tracking and force tracking are achieved.

Figure 6.9 Transients in a bilateral control systemwith force and position scalinga ¼ 1:5;b ¼ 1:5 andoperator compliance coefficient lh ¼ 1:85. The left column shows reference position q

refh , operator

position qh, the position of the environment qe, position of master device qm, position of slave device qsand the desired accelerations in position tracking€qdesqb . The right column shows the difference between the

operator position and the master device position eh ¼ qh � qm, the interaction forces of the slave device

with the environment fs, the interaction force induced by master device to operator fh, and the desired

acceleration in the force tracking loop €qdesFb


6.2 Bilateral Control Systems in Acceleration Dimension

In Chapter 5 we discussed position and force control in the acceleration dimension. Here we

would like to extend these results to bilateral control. The first step is to set a problem in such a

way that it can be treated within the desired framework.

Bilateral control operational conditions (6.7) are expressed in the position and force

dimensions. The specification (6.7) can be used as a starting point in deriving a formulation

in the acceleration dimension. By taking the second derivative of the position tracking error we

obtain

eqqb ¼ €eqb ¼ €qqm �€qqs ð6:48Þ

Here the upper superscript ‘q’ stands for the motion induced acceleration.

The acceleration induced by interaction forces on the operator side and on the environment

side can be expressed as €qfma ¼ fh=anh and €qfsa ¼ fs=ans. Here the indexes ‘ma’ and ‘sa’ are

Figure 6.10 Bilateral control system with force and position scaling a ¼ 0:5; b ¼ 1:5 and operator

compliance coefficient lh ¼ 1:85. The left column shows reference position qrefh , operator position qh, the

position of the environment qe, position of master device qm, position of slave device qs and the desired

accelerations in position tracking €qdesqb . The right column shows the difference between the operator

position and the master device position eh ¼ qh � qm, the interaction forces of the slave device with the

environment fs, the interaction force induced by master device to operator fh and the desired acceleration

in the force tracking loop €qdesFb


used to denote that induced acceleration depends on the force and the scaling coefficient.Now the

total acceleration due to the interaction forces on the master and slave side can be expressed as

efFb ¼

fh

anhþ fs

ans¼ €qfmaþ€qfsa ð6:49Þ

The total interaction force induced acceleration can represent the bilateral system force

operational requirements in (6.7) if scaling coefficients for both forces are equal. That is easy to

verify from (6.49). From efFb ¼ 0 the balance of the forces gives fhans ¼ � fsanh, thus fh ¼ � fs

is satisfied if ans ¼ anh ¼ an. The force scaling coefficient plays the role of a virtual inertia in

the force control loop of the bilateral system.

The accelerations induced by errors (6.48) and (3.35) can be expressed as

€qqqb ¼ eqqb ¼ €qqm �€qqs

€qfFb ¼ efFb ¼ €qfma þ€qfsa

ð6:50Þ

Now, similar to (6.7), the bilateral control system operational conditions in terms of the

motion and force induced accelerations can be expressed in the following form

S€qb ¼ qm; qs : eqqb ¼ €qqqb ¼ €qqm �€qqs ¼ 0 and €qfFb ¼ €qfma þ€qfsa ¼ 0

n oð6:51Þ

Requirements (6.51) mean enforcing a functional relationship between the components

of acceleration of the master and slave systems. The bilateral control conditions (6.7)

and (6.51) have some functional differences and strictly speaking they are not equivalent.

The difference is in the fact that the initial conditions on position and velocity are not

reflected in (6.51). Indeed, if condition €qqqb ¼ €qdesm �€qdess ¼ 0 is enforced the equilibrium

solution does not enforce _qqqb ¼ _qdesm � _qdess ¼ 0 and qqqb ¼ qdesm � qdess ¼ 0, thus the equi-

librium solutions may differ with a change of the initial conditions. The position tracking

requirements in (6.7) and (6.51) are equivalent only if the initial conditions in position and

velocity are equal: qm 0ð Þ ¼ qs 0ð Þ and _qm 0ð Þ ¼ _qs 0ð Þ. If the initial conditions in velocity

and position are not equal they should be included in specification (6.51). If the initial

conditions in velocity are equal, then in equilibrium (6.51) the error in position of the

master and slave system is constant and equal to the difference in the initial positions of

the master and slave devices. If the initial conditions in the velocity of the master and

slave devices are not equal, then in equilibrium (6.51) the positions on the master and

slave side will diverge. As shown in Section the presence of the operator in systems with a

bilateral relationship can be used for compensation of the position difference, thus

enforcement of conditions (6.51) can be used as an alternative to (6.7). The force

tracking requirements in (6.7) and (6.51) are equivalent since €qfma and €qfsa are just scaled

interaction forces [6,7].

Setting aside the differences in position tracking requirements as specified in (6.7) and (6.51)

let us analyze the design of control that will enforce operational conditions (6.51). The tracking

in positionwill be enforced if the initial conditions on themaster and slave devices are the same.

In design we will assume that the acceleration loop on the master and slave side are

compensating all disturbanceswith negligible error, thus ensuring perfect acceleration tracking

€qm ¼ €qdesm and €qs ¼ €qdess . In the design we will assume two possibilities. In one position, the


velocity and forces will be assumed asmeasured variables. In the second, the accelerations and

forces will be assumed as measured variables.

It has been shown in Chapter 3 that convergence in the position control loop requires velocity

and position feedback. The resulting dynamics for controllers with asymptotic convergence is

second order. The control input – desired acceleration – is composed as a sum of the equivalent

acceleration and the convergence acceleration. The convergence acceleration is selected as a

sum of the velocity and position error proportional terms. Here wewould like to show a slightly

different realization of the controller enforcing a second-order transient in the position tracking

loop. The idea is based on the closed loop transient _sqbþ kqbsqb ¼ 0 for generalized error

sqb ¼ ceqbþ _eqb and the desired acceleration being expressed as €qdesqb ¼ €qeq

qb � kqbsqb. From

_sqb ¼ €qqb�€qeqqb the equivalent acceleration can be expressed as €qeqqb ¼ €qqb� _sqb. In systems

with acceleration controller the relationship €qqb ¼ €qdesqb þ pqb Qqb; tdis� �

holds and we can

approximate equivalent acceleration by €qeq

qb ¼ €qdes

qb � _sqb, where €qdes

qb can be calculated simply as

a filtered €qdesqb . Then the desired acceleration can be expressed as €qdesqb ¼ €qdes

qb � _sqb þ kqbsqb

� �:

The desired acceleration, with the assumption that control error, position and velocity are

measured, can be expressed as

€qdesqb ¼ €qdes

qb� e

qqb þKD _qm � _qsð ÞþKP qm � qsð Þ

h ieqqb ¼ €qqm �€qqs

d €qdes

qb

dt¼ g €qdesqb � €q

des

qb

� ð6:52Þ

The transient in motion tracking is then governed by

eqqb þKD _qm � _qsð ÞþKP qm � qsð Þ ¼ pqb ð6:53Þ

The solution based only on the acceleration measurement leads to the desired acceleration

being expressed as [7]

€qdesqb ¼ €qdes

qb � €qqm �€qqs� �þKD

ð€qqm �€qqs� �

d zþKP

ðt0

ðt0

€qqm �€qqs� �

d z

24

35d j

8<:

9=; ð6:54Þ

The closed loop transient is governed by

€qqm �€qqs� �þKD

ð€qqm �€qqs� �

d zþKP

ðt0

ðt0

€qqm �€qqs� �

d z

24

35 d j ¼ 0 ð6:55Þ

If the initial conditions in position and velocity on the master and slave side are equal, then

these two solutions are equivalent. With different initial conditions in the master and slave

system implementation (6.52) does not need any change, but implementation (6.54) needs the

addition of terms dependent on initial conditions. Further, in implementation (6.54) the


acceleration is assumed measured while, in the first implementation, the position and velocity

are assumed measured also.

The force tracking controller can be implemented as

€qdesFb ¼ ~€qdes

Fb � k€qmesFb

d~€qdes

Fb

dt¼ g €qdesFb � ~€q

des

Fb

� ð6:56Þ

The master and slave desired accelerations can be determined from (6.24) for systems

without scaling or (6.41) for a systemwith scaling. Expressions (6.41) can be used in both cases

just by setting a ¼ b ¼ 1 for systems without scaling.

Example 6.3 Bilateral Control in Acceleration Dimension In this example the same

plants and under the same conditions as in Example 6.2 are simulated to illustrate bilateral






environment fs, the interaction force induced by master device to operator fh and the desired acceleration

in the force tracking loop €qdesFb . Control as in (6.52)


control in the acceleration dimension with acceleration tracking errors for motion and force as

in (6.50). The control input in the position loop is as in (6.52) and (6.54) and applied with

parameters KD ¼ 80; KP ¼ 1600. The control in the force induced acceleration is as in (6.56)

with k ¼ 100 and filter g ¼ 400. The initial conditions are qm 0ð Þ ¼ 0m; qs 0ð Þ ¼ � 0:03m.

The scaling parameters are a ¼ 1:5; b ¼ 1:5 (Figure 6.11). The other operational conditionsare as in Example . The system behavior is very similar to the one shown in Figure 6.9. That

illustrates the applicability of the acceleration-based formulation in bilateral systems control.

In Figure 6.12 the scaling parameters area ¼ 0:5; b ¼ 1:5while all other conditions are thesame. Both scaled position tracking and force tracking are achieved.

These examples illustrate the operation of bilateral control systems within a framework of

acceleration control. The formulation as position and force control problems as the tasks leads

to a clear decoupling of the control actions. The formulation in the acceleration dimension

unifies the way bilateral control may be treated.






environment fs, the interaction force induced by master device to operator fh, and the desired acceleration

in the force tracking loop €qdesFb . Control as in (6.52)


6.3 Bilateral Systems with Communication Delay

Ideal bilateral control allows an extension of the operator’s force sensing to a remote

environment. The need for such indirect interaction may be due many reasons – the distance

between the operator and the site on which the task is performed, a hazardous environment, or

tasks not compatiblewith direct human senses (for example,manipulation ofmicroparts). In all

cases an exchange of data between themaster and slave side is required. The commands should

be transferred from themaster to the slave side and the measurements must be transferred from

the slave to the master side. In this section we will discuss issues related to distortion and time

delay in both measurement and control channels. The need for analysis of motion control

systems with time delay comes from the structure of remotely controlled systems. In these

systems the master and slave usually are at distance and the signals from one system to another

are subject to transmission via communication channels. Time delay and dynamical distortion

are normally present in such a situation.

In recent yearsmany interesting solutions have predominated the research field [3,4,12–14],

ranging from variations in the classic Smith predictor [8,9], control based on sliding

modes [10], m-synthesis [11] to passivity based approaches like scattering theory and wave

variables. Those approaches assure passivity aswell as stability and arevalid for constant delay.

However, those are not applicable (or only with difficulty) in time-varying delay cases. Among

the proposed methods, the communication disturbance observer (CDOB) based control of

systemswith delay [15] stands on its own as a simple design procedure based on the disturbance

observer method. It offers a framework for the application of the disturbance observer for

systems with a constant and/or time-varying delay. Experimental results have confirmed this

applicability but at the same time have revealed a problem related to the convergence of the

estimated–predicted value to the plant’s output, especially in the case of a time-varying delay.

Development will be shown for a 1-dof motion control system exposed to an unknown,

possibly time-varying delay in control and in the measurement channels (Figure 6.13).

Wewill assume the known nominal parameters of the plant andmeasurements are subject to

only network delay. The goal is to design a controller based on available data such that the

stability of a closed loop system is guaranteed and at least delay in the measurement channel is

compensated, while delay in the control channel may not be compensated, and thus the plant

output may be delayed due to the delay in the control channel.

In this section wewill be discussing two problems: (i) restoration of the system coordinates

in the presence of a network delay in the system and (ii) design of a network controller for the

system depicted in Figure 6.13. We will first address compensation of the distortions in the

Figure 6.13 A 1-dof system with a time delay in the measurement and control channels


measurement channel and thenwill return to the problemwith a delay in bothmeasurement and

control channels.

6.3.1 Delay in Measurement Channel

Assume a simple 1-dof motion control system with known nominal inertia an and torque

constant Kn

_q tð Þ ¼ v tð Þan _v tð Þ ¼ t tð Þ� tdis tð Þ

t ¼ Kni tð Þtdis tð Þ ¼ a� anð Þ _vþ b q; vð Þþ g qð Þþ text tð Þ

ð6:57Þ

As shown, the disturbance tdis can be estimated based on the measurement of v tð Þ; t tð Þ andknown nominal parameters. The general acceleration control framework for system (6.57)

leads to the implementation of control input as t ¼ tdis tð Þþ an€qdes tð Þ. The disturbance tdis tð Þ is

assumed unknown but some components of it, like spring coefficient and friction coefficients,

may be estimated if necessary. The desired acceleration€qdes tð Þ is generated by the output of theplant controller.

For the system in Figure 6.13 the time delay in the measurement channel is Tm and the time

delay in the control channel isTc. Both delaysmay be constant or time-varying and are assumed

unknown. In order to avoid long expressions, a shorthand notation for all variables

x t� Tmð Þ ¼ x t; Tmð Þ or x t� Tcð Þ ¼ x t; Tcð Þ will be used from now on. Time tð Þ is referredto time at the controller side. The measurements will be labeled by the superscript mes.

In the system under consideration the controller output is the desired acceleration €qdes. Onthe plant side a disturbance observer is applied and the control input to the plant consists of the

desired acceleration induced force an€qdes and the disturbance observer output tdis, thus

t ¼ tdis tð Þþ an€qdes tð Þ or Kni ¼ tdis þ an€q

des. Since component tdis originates on the plant

side, it is not subject to the delay in the control channel. Note that tdis can be selected to

compensate part of the disturbance, thus it allows flexibility in selecting a compensation

strategy at the plant.

Let us first analyze the problem in the presence of a measurement delay while there is no

control channel delay. The measurements available at the controller side are described as

qmes tð Þ ¼ q t� Tmð Þ ¼ q t; Tmð Þvmes tð Þ ¼ v t� Tmð Þ ¼ v t; Tmð Þ ð6:58Þ

Where qmes tð Þ stands for measured the position at time instant tð Þ received at the controllerside, vmes tð Þ stands for the measured velocity at time instant tð Þ received at the controller sideand Tm stands for unknown, possibly time-varying delay, in the measurement channel.

Our goal is to design a control enforcing the tracking of reference position qref tð Þ velocityvref tð Þ for system (6.57)with a known nominalmodel and subject tomeasurement (6.58). Since

there is no delay in the control channel, the controller output €qdes tð Þ is transferred to the plantinput without delay.

In the first step we would like to investigate the possibility of reconstructing the

plant position and/or velocity using available measurements [qref tð Þ and/or vref tð Þ] and a

known plant nominalmodel. The application of a disturbance observer is extensively discussed


in [15]. Here we will discuss the application of closed loop observers with finite-time

convergence.

The available data are velocity and desired acceleration qdes tð Þ; vmes tð Þ� measurements

along with known plant parameters an;Knð Þ. Let the observer structure be

an _z tð Þ ¼ an€qdes tð Þ� anuz ez tð Þ½ �

ez tð Þ ¼ vmes tð Þ� z tð Þð6:59Þ

Control uz ez tð Þ½ � should be selected to enforce tracking of the measured velocity vmes tð Þ bythe output z tð Þ of the nominal plantmodel. Control uz ez tð Þ½ � for first-order dynamics (6.59)may

be realized in many different ways. Assume control uz ezð Þ is selected in such a way that finite-time convergence of error ez tð Þ ¼ 0; 8t � t0 is enforced [for example, a sliding mode is

enforced if control is selected as uz ezð Þ ¼ � kez �m sign ezð Þ; with k;m being a strictly

positive constant]. Then equivalent control uezq ezð Þ which will maintain equilibrium solution

ez tð Þjt>t0¼ 0 with initial conditions ez t0ð Þ ¼ 0, can be determined as

_ez tð Þ ¼ _vmes tð Þ� _z tð Þ ¼ _vmes tð Þ� €qdes tð Þ� ueqz ezð Þ� ¼ 0

ueqz ezð Þ ¼€qdes tð Þ� _vmes tð Þð6:60Þ

Equivalent control ueqz ezð Þ is equal to the difference between the desired and measured

accelerations. Assuming no delay and distortions in the control channel, the input from

controller to the plant is equal to the desired acceleration induced force an€qdes tð Þ. Inserting

t tð Þ ¼ an€qdes tð Þ into (6.57) results in

an€qdes tð Þ ¼ an _v tð Þþ tdis tð Þ ð6:61Þ

Plugging (6.61) into the expression for equivalent control in (6.60) yields

ueqz ez tð Þ½ � ¼ tdisan

� _vmes tð Þ� _v tð Þ½ � ð6:62Þ

The equivalent control ueqz ez tð Þ½ � is a function of the generalized disturbance induced

acceleration and the difference between the measured _vmes tð Þ and the plant acceleration _v tð Þ.From (6.62), if tracking in (6.59) is enforced, the average control is equal to ueqz ez tð Þ½ � and

a�1n tdis � _vmes tð Þ� _v tð Þ½ � can be calculated as one quantity. This conclusion is very important

since it shows that acceleration due to the disturbance is transferred to the observer without

distortion despite a delay inmeasurement. From (6.62) an observer-predictor that estimates the

plant output at time tð Þ can be constructed as


_v tð Þ ¼ _vmes tð Þþ ueqz ezð Þ� tdisan

_q tð Þ ¼ v tð Þ

ez tð Þ ¼ vmes tð Þ� z tð Þ ð6:63Þ


Here _v tð Þ stands for estimated plant acceleration and v tð Þ and q tð Þ stand for estimations of

the plant velocity and position. In addition to themeasured inputs €qdes tð Þ; vmes tð Þ� and the plant

nominal inertia an the plant generalized disturbance tdis is required. That prevents the directapplication of (6.63). One of the solutions is to compensate a generalized disturbance on the

plant directly. Then the disturbance compensation error tdis � tdis ¼ p Q; tdisð Þ appears in the

plant as an uncompensated disturbance. In this case the generalized disturbance tdis in (6.63)

can be replaced by the disturbance compensation error p Q; tdisð Þ to obtain

an _z tð Þ ¼ an€qdes tð Þ� anuz ez tð Þ½ �; ez tð Þ ¼ vmes tð Þ� z tð Þ

_v tð Þ ¼ _vmes tð Þþ ueqz ezð Þ� p Q; tdisð Þan

_q tð Þ ¼ v tð Þ ð6:64Þ

Integration of _v tð Þ over time interval [0,t] yields

v tð Þ ¼ vmes tð Þþðt0

uzeq ez jð Þ½ �d j� 1

an

ðt0

p Q; tdisð Þd jþ v 0ð Þ� vmes 0ð Þ ð6:65Þ

The velocity estimation error depends on the initial conditions in the plant and the observer.

The additional error in (6.65) depends from the uncompensated generalized disturbanceÐp Q; tdisð Þd j and is determined by the accuracy of the disturbance compensation on the

plant side.

This dependence on the uncompensated plant disturbance may be used to insert a

convergence term in the otherwise open loop integration in (6.65). In order to introduce the

convergence term into the observer assume the uncompensated disturbance as

p Q; tdisð Þ ¼ KDv tð ÞþKPq tð Þ. HereKD;KP are strictly positive constants. These two constants

can be interpreted as the damper and spring coefficients of the plant. The compensated plant

dynamics can be written as

_q tð Þ ¼ v tð Þan _v tð Þ ¼ an€q

des tð Þ�KDv tð Þ�KPq tð Þ ð6:66Þ

In order to reflect the structure of the nominal plant observer (6.59) needs to be modified.

The modification yields

an _z tð Þ ¼ an€qdes tð Þ�KDv tð Þ�KPq tð Þ� anuz ez tð Þ½ �


The equivalent control can be expressed from the tracking conditions in the observer (6.67)

anueqz ezð Þ ¼ an€q

des tð Þ� an _vmes tð Þ�KDv tð Þ�KPq tð Þ ð6:68Þ

Calculation of an€qdes tð Þ from the second equation in (6.66) and inserting it into (6.68) yields

anueqz ezð Þþ an _v

mes tð Þ ¼ an _v tð ÞþKD v tð Þ� v tð Þ½ � þKP q tð Þ� q tð Þ½ � ð6:69Þ


In order to ensure the convergence of estimation error Dq tð Þ ¼ q tð Þ� q tð Þ to zero the left

hand side of (6.69) should be equal to an _v tð Þ, thus velocity observer has the following form


ez tð Þ ¼ vmes tð Þ� z tð Þan _v tð Þ ¼ an _v

mes tð Þþ anueqz ezð Þ

_q ¼ v tð Þð6:70Þ

The structure of the system is shown in Figure 6.14. From (6.69) and (6.70) the estimation

error can be expressed in the following form

an _v tð Þ ¼ an _v tð ÞþKD v tð Þ� v tð Þ½ � þKP q tð Þ� q tð Þ½ �anD€q tð ÞþKDD _q tð ÞþKPDq tð Þ ¼ 0

Dq tð Þ ¼ q tð Þ� q tð Þð6:71Þ

The convergence of error between the exact and estimated plant position depends

on an uncompensated disturbance in plant pn Q; tdisð Þ ¼ KDv tð ÞþKPq tð Þ. As shown in

Chapter 4, disturbance observers allow the selection of structure and the parameters

of pn Q; tdisð Þ, thus parameters KD;KP > 0 can be selected during the design process.

By applying a disturbance observer compensating the generalized disturbance and

inserting pn Q; tdisð Þ ¼ KDv tð ÞþKPq tð Þ to the plant input as shown in Figure 6.14, the

compensated plant dynamics can be adjusted to (6.66). Insertion of KPq tð Þ inserts a

virtual spring in the plant which limits this application to systems with a bounded range

of changes in position. Some other limitations will be discussed later in this section.

The estimated value evaluates the plant output at the current time from the current value of

the control input and the delayedmeasurement of the plant output. In a sense it plays a dual role:

(i) the estimation and (ii) the prediction of the output of the plant. The error depends on the

accuracy of the compensation of the plant generalized disturbance. The dynamics of error

Figure 6.14 Structure of the disturbance observer without a delay in the control channel


convergence depend on the selection of parametersKD;KP. Both in the plant and in the observer

the additional forces induced by the convergence terms are dependent on velocity and position

and not on the estimation error – thus they are present even when the estimation error is zero.

This limits the available resources for control and external force compensation. That should be

taken into account when selecting convergence; or convergence parameters should be designed

as a function of the estimation error.

Example 6.4 System with Delay in Measurement Channel In this example the same

plants and under the same conditions as in Example 6.1 are simulated to illustrate convergence in

the systems with a delay in the measurement channel. As a plant the slave side is used and the

master side is used to define systemmotion (by applyingoperator force on themaster side system).

The delay in the measurement channel is simulated as a constant Tm ¼ 0:3 s or time varying

Tm ¼ 0:3þ 0:06 sin 31:4tð Þ s. Reconstruction of the position and velocity of the slave side is

realized by applying observer (6.80) with KD ¼ 10; KP ¼ 25 and the tracking controller

uz ¼ ~uz � 2500ez

d~uzdt

¼ 400 uz � ~uzð Þ ð6:72Þ

Figure 6.15 Position convergence for a system with constant delay Tm ¼ 0:3s in the measurement

channel. The left column shows transients in the master position qm, the slave position qs, the measured

slave position qmess and the delay in measurement channel Tm. The right column shows the actual slave

position qs, the estimated slave position qs, themaster position qm and the force due to the convergence term

tcon ¼ KDv tð ÞþKPq tð Þ (the spikes on this force are caused by thewaydelay is implemented in simulation)


The environment position is given as qe ¼ 0:045 sin 3:785tð Þm. The operator force

is calculated as the difference between the master device position and the operator

position fh ¼ 225 000eh þ 10 _eh N; eh ¼ qh � qm. The slave side force is calculated as

fs ¼ 115 000es þ 5 _es N; es ¼ qs � qe.

The master position is used as a reference in the estimated slave position qs tracking. The

controllers in both, the master and estimated slave positions, are the same as the

position controller in Example 6.1. The generalized position tracking errors are selected as

sqm ¼ ceqmþ _eqm; c ¼ 100 and sqs ¼ ceqsþ _eqs; c ¼ 100 with eqm ¼ qh � qm and

eqs ¼ qm � qs. The position tracking controller is designed as in (6.15)

€qdesqi ¼ €qeq

qi þ€qconqi ; i ¼ m; s. Here €qdesqi stands for the desired acceleration, €qeq

qi the

equivalent acceleration estimated by observer (6.17) with filter gain g ¼ 1200 and

€qconqi ¼ � kqisqi; kqi ¼ 100; i ¼ m; s as the convergence acceleration.The results with a constant delay in themeasurement channel are shown in Figure 6.15 and

for a variable delay in themeasurement channel in Figure 6.16. In both experiments the initial

conditions in the plant are qs 0ð Þ ¼ � 0:03 m. The convergence is illustrated in both

Figure 6.16 Position convergence for a system with constant delay Tm ¼ 0:3þ 0:06 sinð31:4tÞs in themeasurement channel. The left column shows transients in the master position qm, the slave position qs,

the measured slave position qmess and the delay in the measurement channel Tm. The right column shows

the actual slave position qs, the estimated slave position qs, the master position qm and the force due to the

convergence term tcon ¼ KDvðtÞþKPqðtÞ (the spikes on this force are caused by the way delay is

implemented in simulation)


diagrams. Transients illustrate the convergence of the plant position (slave side system) to the

reference.

6.3.2 Delay in Measurement and Control Channels

A 1-dof motion control system (6.57) with delay Tc in the control channel can be

described by

_q tð Þ ¼ v tð Þan _v tð Þ ¼ t tð Þ� tdis tð Þt tð Þ ¼ Kni tð Þ ¼ an€q

des t� Tcð Þþ tdis tð Þð6:73Þ

As a reference we will take time ‘t’ at which control signal€qdes tð Þ is generated and enteredinto the control communication channel. The observer and the output from the measurement

channel are assumed to be running at time ‘t’ also. Then the input to the real plant at time ‘t’ is

delayed for ‘Tc’ and the measured signals will be then expressed as

qmes tð Þ ¼ q t� Tc � Tmð Þvmes tð Þ ¼ v t� Tc � Tmð Þ

ð6:74Þ

A shorthand notation x t� Tc � Tmð Þ ¼ x t; Tc; Tmð Þ will be used from now on.

The goal is to design a control system based on available measurements qmes tð Þ, vmes tð Þ, thecontrol input€qdes tð Þ and the nominal parameters of the plant an. The slave side tracking of the

master side position may have a time delay equal to the time delay in the control channel.

By selecting the same structure of the nominal plant and a measured velocity tracking

system as in (6.60) the equivalent control can be determined as in (6.62) with

_vmes tð Þ ¼ _v t� Tc � Tmð Þ. Thus it can be rewritten as

ueqz ez tð Þ½ � ¼ tdisan

� _v t� Tc � Tmð Þ� _v tð Þ½ � ð6:75Þ

From (6.75) the predicted plant output at time ‘t’ can be found in the same way as in (6.63),

thus we can write


_v tð Þ ¼ _vmes tð Þþ ueqz ezð Þ� tdisan

_q tð Þ ¼ v tð Þ

ez tð Þ ¼ vmes tð Þ� z tð Þ

vmes tð Þ ¼ v t� Tc � Tmð Þ

The output is a plant position and velocity corresponding to a system without a delay in the

control andmeasurement channels. This expression has the same form as (6.63) and all remarks

ð6:76Þ


related to (6.63) are valid here also. The full compensation of disturbance on the plant would

lead to an observer without a convergence term.

To introduce convergencewemay take the same approach as for an observerwith only delay

in the measurement channel. The input to the plant is composed as

t tð Þ ¼ an€qdes t; Tcð Þ� KDv t; Tcð ÞþKPq t; Tcð Þ½ � þ tdis ð6:77Þ

Here the delay in the control input is reflected in the time stamp on the measured variables.

Then the dynamics of plant (6.73) can be written as

_q t; Tcð Þ ¼ v t; Tcð Þan _v t; Tcð Þ ¼ an€q t; Tcð Þ�KDv t; Tcð Þ�KPq t; Tcð Þ ð6:78Þ

The observer (6.76) can be then modified as follows



In (6.79) the desired acceleration €qdes tð Þ is used as the observer input. From tracking

conditions in observer (6.79) equivalent control ueqz ez tð Þ½ � can be expressed as

anueqz ez tð Þ½ � ¼ an€q

des tð Þ�KDv tð Þ�KPq tð Þ� an _vmes tð Þ ð6:80Þ

The plant motion (6.78) give variables at time t� Tcð Þ and in Equation (6.80) the input is

given in time tð Þ. In order to relate the dynamics and variables of both the plant and

observer (6.79) we need to find the dynamics of the plant at time tð Þ. The second equation

in (6.78) at time tð Þ can be rearranged to

an€qdes tð Þ ¼ an _v tð ÞþKDv tð ÞþKPq tð Þ ð6:81Þ

Insertion of an€qdes tð Þ from (6.81) into (6.80) yields

an _v tð ÞþKD v tð Þ� v tð Þ½ � þKP q tð Þ� q tð Þ½ � ¼ an _vmes tð Þþ anu

eqz ez tð Þ½ � ð6:82Þ

In order to ensure convergence to zero of the estimation error Dq tð Þ ¼ q tð Þ� q tð Þ the

velocity observer has to have the following form

an _z tð Þ ¼ an€qdes tð Þ�KDv tð Þ�KPq tð Þ� uz ez tð Þ½ �

ez tð Þ ¼ vmes tð Þ� z tð Þan _v tð Þ ¼ an _v

mes tð Þþ ueqz ezð Þ_q tð Þ ¼ v tð Þ

ð6:83Þ

From (6.82) and (6.83) the estimation error may be expressed in the following form

anD€q tð ÞþKDD _q tð ÞþKPDq tð Þ ¼ 0

Dq tð Þ ¼ q tð Þ� q tð Þ ð6:84Þ


The observer plays the role of predictor and compensation of the dynamic distortion due to a

variable delay. It should be noted here that almost the same result can be obtained if, instead of

the equivalent control, the disturbance observer-like structure is used. This follows from the

nature of the information contained in the equivalent control – it is essentially the input

disturbance perceived as acting on the input of the nominal system without delays. The only

difference is in the fact that the disturbance observer would introduce additional fast dynamics

in the convergence of the observer. Those additional dynamics are defined by the disturbance

observer dynamics and may be selected to be much faster than the convergence dynamics of

the observer (6.83). The solution with a disturbance observer and without convergence term

KDv tð ÞþKPq tð Þ is detailed in [15].

The structure of the observer for a system with a delay in the control channel and a delay in

the measurement channel is shown in Figure 6.17. It is the same as the observer for a system

with delay only in the measurement channel. As mentioned earlier any structure that may

estimate the input disturbancemay be used as a part of the overall structure. The convergence is

assured by making both the plant and the nominal plant model stable.

In the observer design, no assumption has been introduced on the nature of the delay in the

sense of being constant or time varying or being equal or different in the control and

measurement channels. The elements determining the accuracy of the observer are related

to the accuracy of the nominal parameters of the plant, the accuracy of the compensation of

disturbance on the plant and the design parameters KD;KP.

An essential part of the observer design is enforcing an accurate calculation of the apparent

disturbance perceived acting on the input of the system due to the time delays and distortions in

the measurement and control channels. The usage of the finite-time convergence and the

equivalent control is not essential, as shown in Example 6.4. It has the advantage of making the

convergence dynamics simpler. If convergence of the positions onmaster and slave sides is not

required, and the error due to the initial conditions is acceptable, then the position related term

Figure 6.17 Structure of an observer with a delay in both themeasurement and the control channels and

the disturbance observer as the estimator of the equivalent control


in (6.78) and in (6.80) can be omitted. In this case, the spring like action induced by

convergence term can be avoided.

Example 6.5 System with Delay in Control and Measurement Channel In this exam-

ple the same plants and under the same conditions as in Example 6.1 are simulated to illustrate

the convergence in systems with a delay in the measurement and control channels. As a plant

the slave side is used. The delay is simulated as constant or time varying. Reconstruction of the

position and velocity of the slave side is realized by applying observer (6.80) with

KD ¼ 10; KP ¼ 25 and the tracking controller as in (6.72). Other conditions are as in

Example 6.4.

The results with a constant delay in the control and measurement channels are shown in

Figure 6.18 and for a variable delay in the control and measurement channels in Figures 6.19

and 6.20. In both experiments the initial conditions on the plant are qs 0ð Þ ¼ � 0:03 m. The

variable delay in the measurement channel is selected as Tm ¼ 0:3þ 0:06 sin 31:4tð Þ s and thedelay in the control channel is discontinuous with a maximum value Tc max ¼ 0:21 s and a

minimum value Tc min ¼ 0:14 s.

Figure 6.18 Position convergence for a systemwith a constant delay in the control and themeasurement

channels Tc ¼ 0:2 s, Tm ¼ 0:3 s. The left column shows the master position qm, the slave position qs, the

actual qs, themeasured qmess and the delay in themeasurement channel. The right column shows the actual

position qs, the estimated position qs, the estimated slave position qs, the master position qm and the force

due to the convergence term tcon ¼ KDvðtÞþKPqðtÞ (the spikes on this force are caused by theway delayis implemented in simulation)


In Figure 6.20 both a delay in the control channel and a delay in the measurement channels

are selected discontinuous. The convergence is illustrated in all diagrams. In both cases –

constant delay, variable delay – a stable motion is observed. Transients illustrate the

convergence of the plant position (slave side system) to the delayed reference. The steady-

state error in systems with a constant delay is negligible, while the error in systems with a

variable delay is about 5%. The distortion of the measured variable due to the variable delay in

the control loop is noticed. The peak uncompensated input due to the convergence in both the

observer and the slave system is about 2N.

6.3.3 Closed Loop Behavior of System with Observer

The analysis of a closed loop assumes known the structure of the controller that provides

control signal an€qdes tð Þ. In order to make the analysis simpler let the controller be selected as

PD with an acceleration feedforward term. The controller output may be written as

an€qdes tð Þ ¼ €qref tð ÞþKDC vref tð Þ� v tð Þ� þKPC qref tð Þ� q tð Þ� ð6:85Þ

Figure 6.19 Position convergence for a system with a variable delay in the measurement channel

Tm ¼ 0:3þ 0:06 sinð31:4tÞs and a discontinuous delay in the control channel varying betweenmaximum

value Tc max ¼ 0:21 s and minimum value Tc min ¼ 0:14 s. The left column shows the master position qm,

the slave position qs, the actual qs, the measured qmess and the delay in themeasurement channel. The right

column shows the actual position qs, the estimated position qs, the estimated slave position qs, the master

position qm and the force due to the convergence term tcon ¼ KDvðtÞþKPqðtÞ (the spikes on this force arecaused by the way delay is implemented in simulation)


The structure of the closed loop control system with a delay in both the measurement and

control channels is depicted in Figure 6.21.

By inserting control (6.85) into the plant description the closed loop dynamics can be

described in the following way

€q tð ÞþKD _q tð ÞþKpq tð ÞþKDCv t; tcð ÞþKPCq t; tcð Þ¼ €qref t; tcð ÞþKDC _q

ref t; tcð ÞþKPCqref t; tcð Þ

ð6:86Þ

Having convergence of the estimated values defined by (6.84) we can write q tð Þ ¼ q tð Þþ jand v tð Þ ¼ v tð Þþ z with j; z !

t!¥ 0. Then (6.86) can be rearranged into

€q tð ÞþKD _q tð ÞþKPq tð ÞþKDC _q t; tcð ÞþKPCq t; tcð Þ¼ €qref t; tcð ÞþKDC _q

ref t; tcð ÞþKPCqref t; tcð Þþ e j; zð Þ

ð6:87Þ

Figure 6.20 Position convergence for a system with variable discontinuous delays: (i) between

maximum value Tc max ¼ 0:39 s and minimum value Tc min ¼ 0:3 s in the measurement channel and

(ii) between maximum value Tc max ¼ 0:21 s and minimum value Tc min ¼ 0:14 s in the control channel.

The left column showsmaster position qm, slave position qs, the actual qs, the measured qmess and the delay

in the measurement channel. The right column shows the actual position qs, the estimated position qs, the

estimated slave position qs, the master position qm and the force due to the convergence term

tcon ¼ KDvðtÞþKPqðtÞ (the spikes on this force are caused by theway delay is implemented in simulation)


After the convergence error diminishes e j; zð Þ ¼ 0 the closed loop behavior is governed by

€q tð ÞþKD _q tð ÞþKPq tð ÞþKDC _q t� tcð ÞþKPCq t� tcð Þ¼ €qref t� tcð ÞþKDC _q

ref t� tcð ÞþKPCqref t� tcð Þ

ð6:88Þ

The closed loop system (6.88) may be written in the form of a LTI system

_x tð Þ ¼ A0x tð ÞþA1x t� tcð Þþ 0 0

0 1

" #_xref t� tcð ÞþBxref t� tcð Þ

x tð Þ ¼ q tð Þv tð Þ

" #; xref tð Þ ¼ qref tð Þ

vref tð Þ

" #; A0 ¼

0 1

�KP �KD

" #;

A1 ¼0 0

�KPC �KDC

" #; B ¼ 0 1

�KPC �KDC

" #ð6:89Þ

This form of system is much discussed in the literature related to systems with a time

delay [8]. The stability of such systems is proven in the Lyapunov stability framework. In [8]

it has been shown that, for a system represented in the form _x tð Þ ¼ A1x tð ÞþA2x t� tð Þ,stability requires

. A1 þA2 to be Hurwitz,

. that there exist positive definite symmetric matrices P; S;R such that

AT1PþPA1 þPA2S

� 1AT2Pþ SþR ¼ 0.

Due to the fact that matrices A1;A2 depend only on the design parameters (the observer

convergence gains and the controller gains) and not on the plant parameters one may use the

Figure 6.21 Structure of the closed loop control system with a delay in both the measurement and

control channels


above stability conditions to determine the range of the design parameters for which the

stability of the closed loop system is ensured for the selected matrices P; S;R.

6.3.4 Bilateral Control in Systems with Communication Delay

Reconstruction of the position and velocity in systems with a communication delay offers a

starting point in designing a bilateral relationship between the operator and the remote system.

The structure of the bilateral control and a reconstruction of the position, velocity and stability

of the position control in systemswith a communication delay (constant or variable) are shown

in previous sections.Herewewould like to discussmerging these results in the establishment of

teleoperated systems with specific bilateral functional requirements.

In the context of systems with an unknown communication delay, the bilateral control

functional requirements in Equation (6.7) need some revision. Equation (6.7) requires tracking

of the motion and the forces in current time. We have shown that, in systems with a delay, the

position and velocity can be estimated. The estimation is due to the information on the nominal

plant parameters and the ability to enforce nominal plant dynamics by selecting a proper

structure of the plant disturbance compensation.

The estimation and reconstruction of the interaction force in systems with a delay is more

complicated. The interaction force estimation by a disturbance observer-like structure, as

discussed in Chapter 4, provides correct information on the interaction force. Transmitting

force information (measured or estimated) via a communication channel may not only retard

force information in time (if the delay is strictly constant) but may also alter the information on

amplitude (if the delay is variable). The reconstruction of the correct force information would,

as in the motion reconstruction case, require additional information. For the reconstruction of

motionwe used a nominal model of the plant and information on control input. It seems natural

to use a model of the interaction force (5.1). In that model, in addition to the environment

parametersMe;De;Ke, the motion of both the plant and the environment is needed. Parameters

can be estimated using some known techniques and the plant motion can be reconstructed, but

the motion of the environment is unknown and very hard to predict. Because of this we will

modify the bilateral functional relation (6.7) in systems with an unknown possibly variable

delay in the communication channel into the following form

SbT ¼qm; qs : eqbT qm; qsð Þ ¼ qm t� Tcð Þ�aqs tð Þ ¼ 0

and

eFbT fh; fsð Þ ¼ fh tð Þþbfs t� Tmð Þ ¼ 0

8<:

9=; ð6:90Þ

Such a formulation allows operations in which amotion control delay in the control channel

and a force control delay in the measurement channel may not be compensated.

As shown the motion on the slave side can be estimated despite the unknown delay in the

communication channels. That allows us to formulate a bilateral relationship inmotion tracking

as a function of the estimated motion at the slave side and the motion on the master side

eqbT qm; qsð Þ ¼ qm tð Þ�aqs tð Þ ¼ 0 ð6:91Þ

Thus the motion bilateral relationship is formulated in the same way as in Equation (6.7) if

themaster motion and the slave estimatedmotion are taken as functionally related coordinates.


This allows a direct application of the motion controller as in (6.15) €qdesqbT ¼ €qeq

qbT � kqbTsqbT

or (6.16) €qdesqbT ¼ €qeq

qbT � kqbT sqbT

�� 2a� 1sign sqbT

� �, kqbT > 0 depending on the selected con-

vergence with generalized error sqbT ¼ ceqbT þ _eqbT.The force control error eFbT fh; fsð Þ ¼ fh tð Þþbfs t� Tmð Þ formally can be written as

eFbT fh; fsð Þ ¼ fh tð Þþbf refs tð Þ with f refs tð Þ ¼ fs t� Tmð Þ and consequently the force tracking

controller can be selected as in (6.15) €qdesFbT ¼ €qeq

FbT � kFbTD� 1h eFbT for exponential conver-

gence or as in (6.16) €qdesFbT ¼ €qeq

FbT � kFbT eFbTj j2a� 1sign eFbTð Þ; 1

2< a < 1, kkFbT > 0 for

finite-time convergence.

The equivalent accelerations €qeq

qbT and €qeq

FbT can be estimated by simple disturbance

observer-like structures (6.17) and (6.18). Thus, the structure of controller did not change

as comparedwith systemswithout a delay in the communication channels. Transformation of

the selected control into the master and slave devices control inputs in the presence of a

communication delay is the next question to be addressed. By applying transformation (6.24)

(for systemswithout scaling), or (6.41) for systemswith scaling, will have in the force control

loop an uncompensated delay of the measurement channel. In order to avoid this, the force

Figure 6.22 Bilateral control in a systemwith a constant delay Tm ¼ 0:3 s in themeasurement channel.

The left column shows the master position qm, the slave position qs, the actual qs, the measured qmess and

the delay in the measurement channel. The right column shows the actual position qs, the estimated

position qs, the interaction forcewith the environment fs, the interaction force with the operator fh and the

force due to the convergence term tcon ¼ KDvðtÞþKPqðtÞ. The scaling coefficients are a ¼ 0:5;b ¼ 1:5


control loop can be closed locally on the master side and then the master and slave desired

accelerations can be expressed as

€qdesm ¼ 1

aþba€qdesFbT þb€qdesqbT

�€qdess ¼ 1

aþb0�€qdesqbT

� ð6:92Þ

Example 6.6 Bilateral Control with Delay in Control andMeasurement Channels In

this example the same plants and under the same conditions as in Example 6.1 are simulated to

illustrate the bilateral control of systemswith a delay in the control andmeasurement channels.

The delay in the control and measurement channels is simulated as constant or time varying.

Reconstruction of the position and velocity of the slave side is realized by applying

observer (6.80) withKD ¼ 10; KP ¼ 25 and a tracking controller as in (6.72). Other conditions

are as in Example 6.4. The position of the environment is qe ¼ 0:045 sin 3:785tð Þm and the

operator position is set to qh ¼ qe þ 0:001 sin qeð Þm.

Figure 6.23 Bilateral control in a system with constant delay in control Tc ¼ 0:1 s and measurement

channel Tm ¼ 0:3 s. The left column shows the master position qm, the slave position qs, the actual qs, the

measured qmess and the delay in the measurement channel. The right column shows the actual position qs,

the estimated position qs, the interaction force with the environment fs, the interaction force with the

operator fh and the force due to the convergence term tcon ¼ KDvðtÞþKPqðtÞ. The scaling coefficients area ¼ 0:5;b ¼ 1:5 (the spikes on this force are caused by the way delay is implemented in simulation)


The bilateral control errors are defined as in (6.90) and (6.91) eqbT ¼ qm tð Þ�aqs tð Þand eFbT ¼ fh tð Þþbfs t� Tmð Þ. The generalized position tracking error is selected

as sqbT ¼ ceqbT þ _eqbT ; c ¼ 100. The position tracking controller is designed as in (6.15)

€qdesqbT ¼ €qeq

qbT þ€qconqbT . Here €qdesqbT stands for the desired acceleration, €qeq

qbT is the equivalent

acceleration estimated by observer (6.17) with observer filter gain g ¼ 1200 and

€qconqbT ¼ � kqbTsqbT ; kqbT ¼ 100 is the convergence acceleration. The desired acceleration

in the force control loop €qdesFbT is selected as €qdesFbT ¼ €qeq

FbT þ€qconFbT ;€qconFbT ¼ � 100eFbT . The

master and slave desired accelerations are determined as in (6.92). In experiments the

interaction force with the environment is limited to fmaxs ¼ 25 N.

The results with a constant delay in the control and measurement channels are shown in

Figures 6.22 and 6.23. Variable delay in the control and measurement channels is shown in

Figure 6.24. In experiments the initial conditions in the plant are qs 0ð Þ ¼ � 0:03 m. The

variable delay in the measurement channel is selected as Tm ¼ 0:3þ 0:06 sin 31:4tð Þ s and thedelay in the control channel is discontinuous with a maximum value Tc max ¼ 0:21 s and a

Figure 6.24 Bilateral control in a system with a variable delay in the measurement channel

Tm ¼ 0:3þ 0:06 sinð31:4tÞs and a discontinuous delay in the control channel varying betweenmaximum

value Tc max ¼ 0:21 s and minimum value Tc min ¼ 0:14 s. The left column shows the master position qm,

the slave position qs, the actual qs, the measured qmess and the delay in themeasurement channel. The right

column shows the actual position qs, the estimated position qs, the interaction force with the environment

fs, the interaction force with the operator fh and the force due to the convergence term

tcon ¼ KDvðtÞþKPqðtÞ. The scaling coefficients are a ¼ 0:5; b ¼ 1:5 (the spikes on this force are

caused by the way delay is implemented in simulation)


minimum value Tc min ¼ 0:14 s. The convergence is illustrated in all diagrams. In both cases –

constant delay, variable delay – a stable motion is observed. Transients illustrate the

convergence of the plant position (slave side system) to the delayed reference. The steady-

state error in systems with a constant delay is negligible while the error in the system with a

variable delay is about 5%.The distortion of themeasured variable due to a variable delay in the

control loop is noted. Uncompensated input due to a convergence in both the observer and the

slave system is about 2N.

References

1. Colman, A. (2001) A Dictionary of Psychology, Oxford University Press, Oxford.

2. Yokokohji, Y. and Yoshikawa, T. (1994) Bilateral control of master-slave manipulators for ideal kinesthetic

coupling. IEEE Transactions on Robotics and Automation, 10, 605–620.

3. Hokayem, P.F. and Spong, M.W. (2006) Bilateral teleoperation: An historical survey. Automatica, 49(12),

2035–2057.

4. Hokayem, P.F. and Spong, M.W. (2006) Bilateral teleoperation: An historical survey. Automatica, 42(12),

2035–2057.

5. Lawrence, D.A. (1993) Stability and transparency in bilateral telemanipulation. IEEE Transactions on Robotics

and Automation, 9, 624–637.

6. Shimono, T., Katsura, S., and Ohnishi, K. (2005) Improvement of operationality for bilateral control based on

nominalmass design in disturbance observer. Industrial Electronics Society 32ndAnnualConference of the IEEE,

vol. 1, pp. 6–10.

7. Katsura, S., Matsumoto, Y., and Ohnishi, K. (2005) Realization of “Law of action and reaction” by multilateral

control. IEEE Transactions on Industrial Electronics, 52(5), 1196–1205.

8. Richard, J.-P. (2003)Time-delay systems: andoverviewof some recent advances and openproblems.Automatica,

39, 1667–1694.

9. Palmor, Z.J. (1966) Time-delay compensation smith predictor and its modifications, in The Control Handbook

(ed. W.S. Levine), CRC Press, Boca Raton.

10. Utkin, V., Guldner, J., and Shi, J. (1999) SlidingModeControl in Electromechanical Systems, Taylor and Francis,

New York.

11. Abe, N. and Yamanaka, K. (2003) Smith predictor control and internal model control - a tutorial. Proceedings of

the SICE Annual Conference, Fukui, Japan, vol. 2, pp. 1383–1387.

12. Uchimura, Y. and Yakoh, T. (2004) Bilateral robot system on the real-time network structure. IEEE Transactions

on Industrial Electronics, 51(5), 940–946.

13. Anderson, R.J. and Spong,M.W. (1989) Bilateral control of teleoperators with time delay. IEEE Transactions on

Automatic Control, 34(5), 494–501.

14. Niemeyer, G. and Slotine, J.J.E. (1991) Stable adaptive teleoperation. IEEE Journal of Oceanic Engineering,

16(1), 152–162.

15. Natori, K. (2008) Time Delay Compensation for Motion Control Systems, PhD Thesis, Keio University,

Yokohama, Japan.

Further Reading

Hogan, N. (1989) Controlling impedance at the man/machine interface. Proceedings of the IEEE International

Conference on Robotics and Automation, vol. 3, pp. 14–19.

Katsura, S. (2004) Advanced Motion Control Based on Quarry of Environmental Information, PhD Thesis, Keio

University, Yokohama, Japan.€Onal, C.D. (2005) Bilateral Control – A Sliding Mode Control Approach, MSc Thesis, Sabanci University, Istanbul,

Turkey.


Tsuji, T. (2005) Motion Control for Adaptation to Human Environment, PhD Thesis, Keio University, Yokohama,

Japan.

Tsuji, T. and Ohnishi, K. (2004) Position/force scaling of function-based bilateral control system. IEEE International

Conference on Industrial Technology, vol. 1, pp. 96–101.

Tsuji, T., Ohnishi, K., and Sabanovic, A. (2007) A Controller Design Method Based on Functionality. IEEE

Transaction on Industrial Electronics, 54(6), 3335–3343.


Part Three

MultiBody Systems

Multibody mechanical systems are composed of links connected by joints to form complex

systemswith highly nonlinear dynamics. In general the number of actuatorsmay be equal to the

number of primary masses – thus the resulting multibody system is ‘fully actuated.’ The forces

(static or dynamic) arising from the interaction with other masses, together with the forces

developed by actuators, determine the motion of the multibody system. In such systems the

forces attributed to actuators are treated as control input. Links and joints may have flexible

elements which will add to the complexity of motion and control. In this text only systems with

rigid links will be treated.

The configuration of a rigid multibody system with n degrees of freedom is described by

vector q tð Þ 2 Rn�1 specifying completely the position of each point of the multibody system.

The set of all admissible configurations is called the configuration space. In the systems studied

herewe are assuming structures for which the configuration is uniquely determined by the joint

coordinates, thus we will interchangeably use the terms configuration space and joint space.

The task realized by a system is defined by a minimal set of coordinates fully describing the

task relatedmotion. All permissible values of the task coordinates define the task or operational

space of the mechanical system in concern. In general the operational space coordinates

represent any set of coordinates defining kinematic mapping between the configuration space

and the operational space.

As the state of multibody systemwewill understand the set of variables that, together with a

description of the multibody system dynamics and inputs, is enough to determine future

changes in system configuration. The state of the systems that are under consideration in this

section is defined by the configuration and its velocity q tð Þ; _q tð Þ½ �.This part discusses the control of multibody mechanical systems in configuration space and

in operational space. The aim is to show ways in which the results presented so far can be

extended to multibody systems in free motion or in contact with the environment. Our aim is

limited in scope and in the depth of the presentation to a discussion of the basic control

structures naturally obtainedwithin an acceleration control framework. Thus,wewill not touch

many important issues that can be found in a wide literature on robotics and the dynamics and

control ofmultibody systems. Rather, wewill rely on the natural extension of the results shown

for 1-dof motion control systems.

We will first discuss the control of multibody systems in configuration space. The results

presented so far for 1-dof systems can be directly applied in this case. Interactions with the



environment or the enforcement of constraints in configuration space will result in changes in

the dynamics of a multibody system. The enforcement of constraints and the dynamics of

constrained systems will be discussed.

Furtherwewill discuss the dynamics and control ofmultibody systems required to perform a

task or set of tasks. This leads to a need to discuss operational space control approaches and the

relationship between tasks and constraints. The redundancy of amultibody systemwith respect

to its task opens a way to realize more than one task concurently. That opens the question of

finding suitable transformations that would allow dynamical decoupling of the tasks and

possibly establishing a hierarchy of tasks. These questions will be addressed along with

problems related to the constraints and control of constrained systems.


7

Configuration Space Control

As shown in Chapter 1, a n-dof fully actuated multibody mechanical system with rigid links

operating freely in configuration space can be described by a set of nonlinear differential

equations

A qð Þ€qþ b q; _qð Þþ g qð Þ ¼ s ð7:1Þ

where:

. q 2 Rn�1 denotes the configuration vector,

. A qð Þ 2 Rn�n stands for the positive definite kinetic energymatrix (sometimes termed inertia

matrix) with bounded strictly positive elements 0 < a�ij � aij qð Þ � aþ

ij [hence A� � kA qð Þk � Aþ , where A� ;Aþ are two known scalars with bounds 0 < A� � Aþ ]

. b q; _qð Þ 2 Rn�1 stands for the vector Coriolis forces, viscous friction and centripetal forces

and is bounded by b q; _qð Þk � bþk. g qð Þ 2 Rn�1 stands for the vector of gravity terms bounded by g qð Þk � gþk ,s2 Rnx1 stands for the vector of generalized joint forces bounded by sk � tþk (in further text

we will sometimes refer to s as the control vector or input force vector).

The positive scalars A� ;Aþ , bþ , tþ are assumed known where any induced matrix or vector

norm may be used in their definition. The kinetic energy matrix depends on the current system

configuration, thus it reflects the current system configuration. In further text, to reduce the

notation, the dependence of the kinetic energymatrix and the forces on the system coordinates

will not be denoted in some expressions.

Matrix A� 1 qð Þ 2 Rn�n can be interpreted as the control distribution matrix. This can be

confirmed by rewriting Equation (7.1) as €q ¼ A� 1 s� b� gð Þ. It defines the current distri-

bution of joint forces among the generalized accelerations of the system. The system is linear

with respect to control but the control distribution matrix is not diagonal, thus coupling exists

not only due to disturbance sd ¼ bþ g but also due to the extradiagonal elements of the control

distribution matrix.



7.1 Independent Joint Control

Model (7.1) can be rewritten as a set of n interconnected second-order systems (7.2) or (7.3)

aii qð Þ€qi þXnj ¼ 1

j 6¼ i

aij€qj þ bi q; _qð Þþ gi qð Þ

2664

3775 ¼ ti; i ¼ 1; . . . ; n ð7:2Þ

_qi ¼ vi

aii qð Þ _vi þXnj ¼ 1

j 6¼ i

aij _vj þ bi q; vð Þþ gi qð Þ

2664

3775 ¼ ti; i ¼ 1; . . . ; n

ð7:3Þ

Here aii qð Þ stand for inertia of the i-th joint and can be expressed as aii qð Þ ¼ anii þDaii qð Þ.Term anii is a constant and represents a nominal value while term Daii qð Þ is a bounded

continuous function. Each of the n degrees of freedom in (7.2) can now be rewritten in the

following form

anii €qi þDaii qð Þ€qi þXnj ¼ 1

j 6¼ i

aij€qj þ bi q; _qð Þþ gi qð Þ|fflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflffl}disturbance¼tdi|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}

generalized disturbance¼tdisi

¼ ti; i ¼ 1; 2; . . . ; n

anii €qi þ tdisi q; _q;€qð Þ ¼ ti

tdi q; _qð Þ ¼ bi q; _qð Þþ gi qð Þ

tdisi q; _q;€qð Þ ¼ Daii qð Þ€qi þXnj ¼ 1

j 6¼ i

aij€qj

|fflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}acceleration induced disturbance

þ bi q; _qð Þþ gi qð Þ|fflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflffl}tdi

ð7:4Þ

The interaction forces induced by a change of acceleration in the other joints (or due to

extradiagonal terms in the control distribution matrix) together with the disturbance term

tdi q; _qð Þ; i ¼ 1; 2; . . . ; n form a generalized disturbance tdisi q; _q;€qð Þ; i ¼ 1; 2; . . . ; n.Description (7.4) is the same as that already shown in Chapter 3 and all results obtained

there can be applied directly. By expressing the i-th, (i ¼ 1; . . . ; n) joint input force as

ti ¼ anii€qdesi þ tdisi, where €qdesi stands for the i-th joint desired acceleration and tdisi stands

for the estimated i-th joint disturbance, system (7.4) can be reduced to n decoupled second-

order systems of the form

€qi ¼ €qdesi � tdisi � tdisianii

¼ €qdesi � pi Qd ; tdisið Þanii

; i ¼ 1; 2; . . . ; n

pi Qd ; tdisið Þ ¼ tdisi � tdisi

ð7:5Þ

The disturbance estimation error pi Qd ; tdisið Þ ¼ tdisi � tdisi is discussed in detail in Chapter4. Structure (7.5) is identical with compensated 1-dof systems discussed in detail in Chapters 3


and 4. The complexity of the dynamics of systems with a disturbance compensation having

a mismatch of parameters between the system and the observer requires analysis of the

influence of thevariation of the kinetic energymatrix on the compensated systemdynamics. By

selecting desired acceleration €qdesi as a function of the error in position and velocity one can

stabilize themotion of system (7.5) on a desired trajectory. This allows us to treat a n-dof system

as a set of n double integrator systems.

7.2 Vector Control in Configuration Space

System (7.1) has n dimensional control vector control input sT ¼ t1 . . . tn½ � with

the components being generalized forces. The dynamic coupling terms are represented

by extradiagonal elements aij qð Þ; i 6¼ j of the kinetic energy matrix A qð Þ and vectors

b q; _qð Þ, g qð Þ.In selecting a control input for system (7.1) wewould like to apply the same ideas discussed

in Chapter 3. Thus for system (7.1) the desired acceleration €qdes 2 Rn�1 is selected to ensure

the desired closed loop system dynamics. Formal application of the same structure of input

force as in 1-dof systems leads to the composition of control input as

s ¼ A qð Þ€qdes þ b q; _qð Þþ g qð Þ ¼ A qð Þ€qdes þ sd ð7:6Þ

Here A qð Þ stands for the estimation of inertia matrix and sd ¼ b q; _qð Þþ g qð Þ stands for theestimation of disturbance vector sd ¼ b q; _qð Þþ g qð Þ. Inserting control (7.6) into plant

dynamics (7.1) yields

€q ¼ A� 1A€qdes �A� 1 qð Þp Qd ; sdð Þp Qd ; sdð Þ ¼ sd � sd

ð7:7Þ

Here p Qd ; sdð Þ stands for the error in estimation of the disturbance vector with components

pi Qd ; tdisið Þ ¼ tdisi � tdisi, i ¼ 1; . . . ; n. Dependence on the observer dynamics is shown by

parameter Qd. The decoupling of dynamics (7.7) depends on the desired acceleration

distribution matrix A� 1A. ForA� 1A ¼ E being a unity matrix, a full decoupling is obtained.

That requires an accurate estimation of the inertia matrix. Since matrix A qð Þ depends on the

current system configuration, its estimation may not be an easy task. Another solution is in

migrating the problem of the inertia matrix estimation to the disturbance estimation. This can

be applied by expressing the inertiamatrix asA qð Þ ¼ An þDA qð ÞwhereAn > 0; 8q stands forthe nominal inertia matrix selected as a positive definite nonzero matrix and DA qð Þ standsfor DA qð Þ ¼ A qð Þ�An. As already mentioned, selection of An > 0; q as a diagonal matrix

may not be always the best solution due to the lead–lag dynamics inserted by a mismatch

of parameters (see Chapter 4). By rearranging the system dynamics (7.1) into

An€qþDA qð Þ€qþ b q; _qð Þþ g qð Þ ¼ s and replacing A ¼ An in (7.6), the dynamics (7.7) can

be expressed as

€q ¼ €qdes �A� 1n p Qd ; sdisð Þ

p Qd ; sdisð Þ ¼ sdis � sdisð7:8Þ

Configuration Space Control 281

Here sdis ¼ DA qð Þ€qþ b q; _qð Þþ g qð Þ stands for the generalized disturbance vector and sdisstands for its estimation. sdis may be estimated componentwise by applying the observers

discussed in Chapter 4.

The result is n decoupled double integrators driven by the desired acceleration subject to

error in the generalized disturbance estimation. This is the same result as that obtained in

Chapter 4 for 1-dof. The selection of desired acceleration depends on the control goal and the

desired transient in the closed loop control system [4].

With perfect estimation of the system parameters and the disturbances, the resulting n-dof

double integrator system should be driven to satisfy the desired closed loop dynamics.

Intuitively one can assume that a PD controller will suffice. In the following section we will

take a closer look at the problem of convergence and selection of the desired acceleration. The

structure of the vector control system is depicted in Figure 7.1.

7.2.1 Selection of Desired Acceleration

The solutions shown above are the result of a direct application of the ideas discussed in

Chapter 3. Let us take more a detailed view on selection of control in system (7.1) using amore

formal approach based on Lyapunov stability methods.

Let us first look at a configuration tracking problem in which configuration q 2 Rn�1 is

required to track a sufficiently smooth reference qref 2 Rn�1 with at least asymptotic stability

of the equilibrium solution. Let the error be Dq ¼ q� qref . The reference is assumed to be a

smooth bounded vector function with continuous and bounded first- and second-order

derivatives. The configuration tracking can be then formulated as enforcing the systemmotion

to stay in the manifold

S ¼ q; _q : s q; qref� � ¼ CDqþD _q ¼ 0; C 2 Rn�n; C > 0

� � ð7:9Þ

Figure 7.1 Structure of control system in configuration space


Here s q; qref� � 2 Rn�1 stands for a generalized control error. Note that this structure is the

same as that discussed in Chapter 3 for output control problems. Control should be selected to

drive the control error to zero and to maintain equilibrium solution s q; qref� � ¼ 0 after

reaching it. Before proceeding to control input selection, let us determine the acceleration and

force inputs which will enforce a zero rate of change of the generalized error – equivalent

acceleration and equivalent force. The derivative of the error s q; qref� �

can be expressed as

_s q; qref� � ¼ CD _qþD€q ¼ €q� €qref �CD _q

� �|fflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflffl}equivalent acceleration

€qeq ¼ €qref �CD _q� � ð7:10Þ

Here €qeq 2 Rn�1 stands for the equivalent acceleration, thus enforcing a zero rate of

change of the generalized control error _s q; qref� �j€q¼€qeq ¼ 0. Inserting (7.1) into (7.10) yields

_s q; qref� � ¼ A� 1 s� bþ gþA €qref �CD _q

� �� ¼ A� 1 s� seqð Þseq ¼ bþ gþA €qref �CD _q

� � ¼ sd þA€qeqð7:11Þ

The equivalent force seq 2 Rn�1, which enforces equivalent acceleration in system (7.1)

consists of the disturbance force sd and the equivalent acceleration induced forceA€qeq 2 Rn�1.

Let us select a control input which will enforce convergence and the stability of

the equilibrium solution s q; qref� �¼ 0. To do so, let the Lyapunov function candidate be

selected as

V ¼ sTs

2> 0; V 0ð Þ ¼ 0; ð7:12Þ

The time derivative of theLyapunov functionV can be expressed as _V ¼ sT _s. Let us select acontrol input which will, for a given linear or nonlinear function Y sð Þ 2 Rn�1, enforce

a derivative of the Lyapunov function in the form

_V ¼ sT _s ¼ �sTY sð Þ < 0 ð7:13Þ

Here YT sð Þ ¼ Y1 . . . Yn½ � 2 Rn�1 stands for a vector function satisfying condition

sign Y sð Þ½ � ¼ sign sð Þwhere sign xð Þ stands foracomponentwise functionwith element sign xið Þbeing þ 1 forxi > 0 and � 1 forxi < 0, i ¼ 1; 2; . . . ; n. Thus theLyapunov function derivativecan be expressed as _V ¼ � Pn

1 Yij jsi sign sið Þ. This leads for a wide range of possibilities forselecting the function Y sð Þ. For example, if Y sð Þ is selected discontinuous, a finite-time

convergence to equilibrium and sliding mode motion can be enforced [1]. By selecting

Y sð Þ ¼ Ds, where D > 0 is a positive definite diagonal matrix, the exponential convergence

is enforced. These results correspond to the results obtained in Chapter 3 for 1-dof systems.

From (7.13) we can write _sT _sþY sð Þ½ � ¼ 0 and for s 6¼ 0 desired acceleration

€qdes 2 Rn�1 can be determined as

€qdes ¼ €qeq �Y sð Þ ð7:14Þ


The acceleration enforcing the desired structure of the Lyapunov function derivative

consists of the equivalent acceleration €qeq and the convergence acceleration

€qcon ¼ �Y sð Þ. In realization of an estimation of the equivalent control may be used. Insertion

of (7.11) into (7.13) for s 6¼ 0 yields

s ¼ sd þA€qeq �AY sð Þ¼ seq �AY sð Þ¼ sd þA€qdes

¼ bþ gþA€qdes

ð7:15Þ

The representation of the input force (7.15) in different ways serves just as an illustration of

the possibility of different realizations.

As shown in Chapter 4 the equivalent acceleration estimation can be realized component-

wise using observers. Then (7.14) can be expressed as

€qdes ¼ €qeq �Y sð Þ

dzi

dt¼ g €qdesi þ gsi � zi

� �;

€qeq

i ¼ zi � gsi; i ¼ 1; . . . ; n

ð7:16Þ

The force input (7.15) realizes the desired acceleration (7.16) thus enforcing closed loop

behavior €q ffi €qdes which, by inserting (7.10) and (7.16) becomes

€q ¼ €qref �CD _q�Y sð Þ� p €qeqð Þp €qeqð Þ ¼ €qeq � €q

eq

9=; _sþY sð Þ ¼ p €qeqð Þ ð7:17Þ

The closed loop behavior depends on the selection of the convergence acceleration

€qcon ¼ �Y sð Þ. Fora small equivalent accelerationestimationerrorp €qeqð Þ � 0,Equation (7.17)

reduces to the ideal system _sþY sð Þ ¼ 0 or componentwise _si þCi sð Þ ¼ 0; i ¼ 1; . . . ; n.This result is the same as that obtained for 1-dof systems.

Assume available _s q; qref� �

. Then, by expressing €qeq from (7.10) and inserting it into

(7.14) yields

€qdes � €qdes|{z}control

� _s q; qref� �þY s q; qref

� �� |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}desired dynamics

ð7:18Þ

In continuous time (7.18) cannot be implemented directly. In a discrete time realiza-

tion (7.18) can be implemented as €qdes kþ 1ð Þ ffi €qdes kð Þ� _s kð ÞþY s kð Þ½ �f g with approxi-

mation error o Tð Þ for a system with a sampling interval T . Here the continuity of configuration

and its reference along with the desired acceleration €qdes is used in justifying the approxi-

mation. In continuous time (7.18) can be approximated by replacing the desired acceleration in

the right hand side expression by an approximation ~€qdes


~€qdes ¼ €qdes þ « €qdes; d

� �« €qdes; d� ��!

d! 00

ð7:19Þ

The ~€qdes

is designed in such a way that the approximation error is uniformly bounded and

tends to zero if the nonideality d introduced by the approximationmechanism tends to zero. For

example, a componentwise simple first-order filter satisfies such requirements. The approx-

imationmechanism can be selected inmany different ways; for example, just a componentwise

fast dynamics would serve. Insertion of (7.19) into (7.18) yields realization in the form

€qdes ¼ ~€qdes � _s q; qref

� �þY sð Þ� � ð7:20Þ

Inserting (7.20) into (7.15) yields s ¼ sd þA ~€qdes � _s q; qref

� �þY sð Þ� �n o. Plugging this

control input into (7.1) yields closed loop dynamics

_sþY sð Þ ¼ « €qdes; d� �þA� 1p Qd ; sdð Þ ffi 0 ð7:21Þ

The right hand side can be made small by an appropriate design of the disturbance observer

and an approximation of the desired acceleration ~€qdes.

If the convergence term is selected as Y sð Þ ¼ Ds;D > 0 then, for generalized error

s q; qref� � ¼ CDqþD _q;C > 0, the control (7.20) takes the form

€qdes ¼ ~€qdes � _sþDsð Þ ¼ ~€q

des � D€qþ CþDð ÞD _qþDCDq½ �|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}desired dynamics

ð7:22Þ

and the closed loop dynamics (7.21) reduces to

D€qþ CþDð ÞD _qþDCDq ffi 0 ð7:23Þ

Expression (7.23) describes a n-dof system with damper CþDð Þ and stiffness DC. The

closed loop dynamics is determined by the design parameters.


s ¼ A€qdes þ sd ¼ sd þA~€qdes �A _s q; qref

� �þY sð Þ� � ð7:24Þ

Introducing ~s ¼ sd þA~€qdes

the expression (7.24) can be simplified to

s ¼ ~s|{z}control

input

�A _s q; qref� �þY sð Þ� �|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}desired dynamics

ð7:25Þ

From (7.19) the approximation ~s in (7.25) can be determined as

~s ¼ sþA« €qdes; d� �

; « €qdes; d� ��!

d! 00 ð7:26Þ


Insertion of (7.25) and (7.26) into (7.1) yields closed loop dynamics (7.21). Approximation

of control ~s can be calculated without relying on an estimation of the desired acceleration. If an

approximation is determined with error « s; dð Þ, thus ~s ¼ sþ « s; dð Þ; « s; dð Þ �!d! 0

0, then

inserting (7.25) into (7.1) yields

_sþY sð Þ ¼ A� 1« s; dð Þ �!d! 0

0 ð7:27Þ

For generalized error s ¼ CDqþD _q;C > 0 and convergence term Y sð Þ ¼ Ds;D > 0

control (7.25) takes the form

s ¼ ~s�A _sþDsð Þ ¼ ~s�A D€qþ CþDð ÞD _qþDCDq½ �|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}desired dynamics

ð7:28Þ

Insertion of (7.28) into (7.1) yields closed loop dynamics

_sþDs ¼ D€qþ CþDð ÞD _qþDCDq ¼ A� 1« s; dð Þ �!d! 0

0 ð7:29Þ

The desired acceleration (7.14) or input force (7.15) enforces ideal closed loop dynamics

_sþY sð Þ ¼ 0, thus they enforce the desired structure of a first-order derivative of the

Lyapunov function (7.12). Selection of the convergence acceleration €qcon ¼ �Y sð Þ definesthe convergence to the manifold (7.9). The desired acceleration (7.20) or input force (7.25)

enforces _sþY sð Þ ffi 0, thusmotion is in the «-vicinity of the ideal closed loop dynamics. If the

convergence acceleration is proportional to the generalized error €qcon ¼ �Ds, D > 0, the

closed loop dynamics (7.29) guarantees exponential convergence in the «-vicinity of

equilibrium solution s ¼ 0. Selection of C > 0 in (7.9) guarantees exponential convergence

to Dq ¼ 0.

If €qcon ¼ �Y sð Þ is selected to enforce a finite-time convergence [for example,

€qcon ¼ �Ks�m sign sð Þ, with diagonal matrix K > 0, scalar m > 0 and sign sð Þ as compo-

nentwise sign function] then the equilibrium solution s ¼ 0 is reached at t ¼ t0 andmotion for

t t0 is in manifold (7.9). The closed loop dynamics is defined by (7.1) with input seq and theequilibrium solution s ¼ 0. Inserting (7.11) into (7.1) yields

d

d tD _qþCDqð Þ ¼ 0;

s ¼ 0

ð7:30Þ

The closed loop dynamics is reduced to s ¼ 0 – thus (7.9) is enforced and the equations ofmotion are of lower order that system (7.1).

So far in our realization of the control input the disturbance and systemkinetic energymatrix

are assumed known. Componentwise estimation of the disturbance is shown in Chapter 4. By

expressing A qð Þ ¼ An þDA qð Þ where An > 0; 8q stands for the nominal kinetic energy

matrix, system (7.1) can be rewritten as An€qþDA€qþ bþ g ¼ s. Assigning generalized

disturbance vector sdis ¼ DA€qþ bþ g, the input force (7.15) can expressed as

s ¼ sdis þAn€qdes

¼ seq �AnY sð Þseq ¼ sdis þAn€q

eq

ð7:31Þ


Here sdis stands for the estimated generalized disturbance and seq stands for an estimation of

the equivalent force. The componentwise estimation for sdis and seq is shown in Chapter 4. The

realization requires only information on the generalized error s. For Y sð Þ ¼ Ds closed loop

dynamics are described by _sþDs ¼ A� 1n sdis � sdisð Þ. The generalized disturbance estimation

error sdis � sdisð Þ depends on the observer dynamics and can be designed to satisfy the desired

specification. The implementation (7.31) leads to the same closed loop dynamics. The input

force (7.25) can be now expressed as

s ¼ ~s|{z}control

�An _sþY sð Þ½ �|fflfflfflfflfflfflffl{zfflfflfflfflfflfflffl}desired dynamics

ð7:32Þ

For Y sð Þ ¼ Ds and control input (7.32) the closed loop dynamics (7.29) becomes

_sþDs ¼ A� 1n « s; dð Þ. For systems with a finite-time convergence the closed loop dynamics

reduces to (7.30) for both control inputs. The structure of the trajectory tracking system realized

as in (7.14) and (7.20) is shown in Figure 7.2.

The desired acceleration that enforces the convergence and stability of equilibrium s ¼ 0

has been expressed as €qdes ¼ €qeq �Y sð Þ in (7.14) or as €qdes ¼ ~€qdes � _s q; qref

� �þY sð Þ� �in (7.20). The closed loop dynamics for the same structure of the convergence acceleration is

the same.

The input force can be expressed as in (7.31) or (7.32). The structural difference here is

interesting to observe. For implementation of (7.31) an estimation of the generalized

disturbance or equivalent force is needed. Implementation of (7.32) does not apply a

disturbance feedback but needs the rate of change of the generalized error – thus it requires

Figure 7.2 Structure of trajectory tracking control in configuration space with acceleration as control

input. (a) Control realization as in (7.14) and (b) control realization as in (7.20)


information on acceleration. The closed loop is the same for both implementations. The

structure of the control system is depicted in Figure 7.3.

The trajectory tracking control in configuration space shows the same structural depen-

dencies as for a 1-dof motion control system. Application of a componentwise disturbance

observer realizes the acceleration controller and selection of the desired acceleration ensures

the convergence to and stability of the equilibrium solution.

Example 7.1 2-dof Planar Manipulator Control in Configuration Space In this

example we will illustrate the control of a multibody mechanical system in configuration

space. As an example we will use the planar elbow manipulator discussed in Chapter 1. The

total kinetic energymatrix is expressed as in Equation (1.34) and the elements aij ; i; j ¼ 1; 2of matrix A qð Þ are

a11 q2ð Þ ¼m1l2m1 þ I1|fflfflfflfflfflffl{zfflfflfflfflfflffl}axis 1

þm2 l21 þ l2m2 þ 2l1lm2 cos q2ð Þ� �þ I2|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}axis 2

a12 q2ð Þ ¼ a21 q2ð Þ ¼ m2 l2m2 þ 2l1lm2 cos q2ð Þ� �þ I2

a22 ¼m2l2m2 þ I2

ð7:33Þ


U ¼ m1glm1 sin q1ð Þþm2g l1 sin q1ð Þþ lm2 sin q1 þ q2ð Þ½ � ð7:34Þ

Figure 7.3 Structure of trajectory tracking control in configuration space with force as control input.

(a) Control realization as in (7.31) and (b) realization as in (7.32)


The dynamics of the planar elbow manipulator can be written as

A qð Þ€qþ b q; _qð Þþ g qð Þ ¼ s

A qð Þ ¼a11 a12

a21 a22

" #; b q; _qð Þ ¼

b1 q; _qð Þb2 q; _qð Þ

" #; g qð Þ ¼

g1 qð Þg2 qð Þ

" #

b1 q; _qð Þ ¼ �m22l1lm2 sin q2ð Þ _q2 _q1 �m22l1lm2 sin q2ð Þ _q2 _q2b2 q; _qð Þ ¼ �m22l1lm2 sin q2ð Þ _q2 _q1g1 qð Þ ¼ m1glm1 cos q1ð Þþm2g l1 cos q1ð Þ� lm2 cos q1 þ q2ð Þ½ �g2 qð Þ ¼ m2glm2 cos q1 þ q2ð Þ

ð7:35Þ

Here A qð Þ 2 R2�2 is the kinetic energy matrix with elements as in (7.33),

bT q; _qð Þ ¼ b1 q; _qð Þ b2 q; _qð Þ½ � is the vector representing terms depending on velocities,

gT qð Þ ¼ g1 qð Þ g2 qð Þ½ � is the vector of the gravitational forces, qT ¼ q1 q2½ � is the

configuration vector and sT ¼ t1 t2½ � is the vector of joint torques generated by actuators.

The parameters of the manipulator are

m1 ¼ 1 kg l1 ¼ 1 m lm1 ¼ 0:5 m I1 ¼ 0:2 kg m2

m2 ¼ 2 kg l2 ¼ 1 m lm2 ¼ 0:5 m I2 ¼ 0:35 kg m2ð7:36Þ

Wewould like to illustrate configuration control where the configuration q is forced to track

a smooth reference qref ¼ qref1 q

ref2

� �T. For the controller design the manipulator descrip-

tion (7.35) is rearranged into

a11n€q1 ¼ t1 � t1dis

a22n€q2 ¼ t2 � t2dis

t1dis ¼ � b1 q; _qð Þ� g1 qð Þ� a11 � a11nð Þ€q1t2dis ¼ a21€q1 � b2 q; _qð Þ� g2 qð Þ� a22 � a22nð Þ€q2

ð7:37Þ

The controller is now designed for tracking of q1 and q2 separately with input forces

t1 ¼ t1dis þ a11n€qdes1


a11n ¼ 4:5 kg m2; a22n ¼ 0:55 kg m2

ð7:38Þ

The generalized disturbances are estimated using observerswith velocity and input forces as

inputs, thusdz1

dt¼ g t1 þ ga11n _q1 � z1ð Þ;

t1dis ¼ z1 � ga11n _q1

dz2



g¼ 800

ð7:39Þ


The generalized control errors are determined as

s1 q1; qref1

¼ c1e1 þ _e1; e1 ¼ q1 � q

ref1

s2 q2; qref2

¼ c2e2 þ _e2; e2 ¼ q2 � q

ref2

c1 ¼ c2 ¼ 25

ð7:40Þ

The desired accelerations are calculated as

€qdes1 ¼€qref1 � d1s1 q1; qref1

€qdes2 ¼€qref2 � d2s2 q2; q

ref2

d1 ¼ d2 ¼ 125

ð7:41Þ

The reference trajectories are selected as

qref1 tð Þ ¼ 0:1þ 0:15 sin 3:14tð Þ radqref2 tð Þ ¼ 0:1þ 0:25 sin 6:28tð Þ rad

ð7:42Þ

Transients in the configuration control for initial conditions q1 0ð Þ ¼ q2 0ð Þ ¼ 0:1 rad are

shown in Figure 7.4. The diagrams illustrate the trajectory tracking performance and the

applicability of a decentralized acceleration control.

7.3 Constraints in Configuration Space

Assume configuration vector q 2 Rn�1 in system (7.1) needs to be controlled to satisfy

algebraic equation f qð Þ ¼ 0, where f qð Þ 2 Rm�1;m < n stands for a vector valued contin-

uous function of the system configuration with continuous and bounded first- and second-order

time derivatives.

This problem can be formulated as a requirement to select a control input such that the

configuration of system (7.1) is constrained to satisfy

Sf ¼ q; _q : f qð Þ ¼ 0;f qð Þ 2 Rm�1� � ð7:43Þ

This means, the control needs to be selected to constrain the motion of system (7.1) in

hypersurface f qð Þ ¼ 0 2 Rm�1 in configuration space. When motion is constrained to the

hypersurface, m components of the configuration vector have a predefined functional

dependence governed by f qð Þ ¼ 0, thus the resulting motion in manifold (7.43) can be

described by the rest of configuration vector components. This allows decomposition of

system motion into motion in the constrained direction and motion in the unconstrained

direction. Motion in the constrained direction satisfies (7.43). Motion in the unconstrained

direction is then performed in the tangential plane to the constraint hypersurface.

From the control point of view, the requirements in (7.43) means we must find a control that

guarantees convergence and stability of equilibrium solution f qð Þ ¼ 0. Note that this


formulation is very similar to the way control problem has been treated in the sliding mode

framework. The difference here is that the dimension of the control vector is not equal to the

dimension of the constraint manifold – thus there is redundancy in the control vector with

respect to the control enforcement of the constraints.

Projection of system motion in manifold (7.43) can be written in the following form

_f ¼ qf qð Þqq

0@

1A _q ¼ F _q

€f ¼ F€qþ _F _q

€f ¼ FA� 1 s� bþ gð Þ½ � þ _F _q

ð7:44Þ

In (7.44) F€q stands for projection of the system acceleration in constrained direc-

tions and _F _q stands for velocity induced acceleration. The qf qð Þ=qq½ � ¼ F 2 Rm�n stands

for a full raw rank constraint Jacobian matrix.

Figure 7.4 Transients in 2-dof manipulator control. The left column shows position q1 tð Þ; qref1 tð Þ,positions q2 tð Þ; qref2 tð Þ and generalized errors s1;s2. The right column shows control inputs

t1 tð Þ; t2 tð Þ, generalized disturbances t1dis tð Þ; t2dis tð Þ and the q2 q1ð Þ diagram, for initial conditions

q1 0ð Þ ¼ 0:0 rad; q2 0ð Þ ¼ 0:4 rad. The observer gain is g ¼ 800, with reference positions as

in (7.42)


In order to enforce the constraints the projection of velocity and acceleration in the

constrained directions must be enforced to zero by selecting the vector of generalized input

force s. In general, the enforcement of m constraints requires m independent control inputs

fF 2 Rm�1. These forces are acting in the constraint direction and, as shown in Chapter 1, the

configuration space force s corresponding to fF can be expressed by sF ¼ FT fF. Let the

component of the configuration space force sG 2 Rn�1 which complements sF to realize

arbitrary force be expressed as sG ¼ GTs0 where matrix G is yet to be determined and s0 is anarbitrary force vector inRn. Then the input arbitrary force generated by these two components

can be expressed as

s ¼ FT fF þGTs0 ð7:45Þ

Inserting (7.45) into (7.44) yields

€f ¼ FA� 1FT fF þFA� 1GTs0 �FA� 1 bþ gð Þþ _F _q ð7:46Þ

By collecting the position- and velocity-dependent terms into vector m q; _qð Þ 2 Rm�1 and

terms depending only on position into vector r qð Þ 2 Rm�1, Equation (7.46) can be rearranged

in the following form

LF €fþm q; _qð Þþ r qð Þ�LFFA� 1GTs0 ¼ fF

LF ¼ FA� 1FT� �� 1

m q; _qð Þ ¼ LF FA� 1b q; _qð Þ� _F _q� �

r qð Þ ¼ LFFA� 1g qð Þ

ð7:47Þ

System (7.47) describes the projection of system dynamics in the constrained direction – or

loosely speaking the changes in distance from the constraint manifold. The m dimensional

force vector fF 2 Rm�1 is the control input force acting in the constrained direction, matrix

LF 2 Rm�m can be interpreted as a pseudokinetic energy matrix consistent with the con-

straints. The control distribution matrix FA� 1FT� � 2 Rm�m is a full rank matrix. Vectors

m q; _qð Þ and r qð Þ are disturbance forces in the constrained direction. They depend on

configuration space disturbances and velocity induced acceleration in the constrained direction_F _q. Disturbances are expressed as functions of configuration space position and velocity

in order to clearly show their relationship. VectorLFFA� 1GTs0 stands for the projection of s0in the constrained direction. Acceleration in the constrained direction induced by force s0 isgiven byFA� 1GTs0, and for a general selection of matrix G this force acts a disturbance force

in the constrained direction.

The structure of the projection of the system dynamics into constrained directions (7.47) has

the same form as configuration space dynamics (7.1), thus the control design can follow the

same steps as discussed in Section 7.2. Applying a control input structure as in (7.31) the

control force in the constrained direction can be selected as

fF ¼ fFdis þLF €fdes

fFdis ¼ m q; _qð Þþ r qð Þ�LFFA� 1GTs0ð7:48Þ


Here fFdis is the disturbance in the constrained direction. The simplest structure of

the desired acceleration in the constrained direction is €fdes ¼ �KD

_f�KPf with positive

definite matrices KD;KP > 0. Similarly the application of structure (7.32) yields

fF ¼ ~fF �LF €fþKD_fþKPf

� � ð7:49Þ

Here ~fF is an approximate value of the control force in the constrained direction. Both

control inputs enforce the same dynamics

€fþKD_fþKPf ffi 0 ð7:50Þ

The component of the configuration space force corresponding to the control force in the

constrained direction is

sF ¼ FT fF ¼ FT fFdis þFTLF €fdes ð7:51Þ

Since control (7.49) enforces stable equilibriumf qð Þ ¼ 0 2 Rm�1 after initial transient, the

functional relation (7.43) will be enforced – thus the constraint on the configuration vector will

be imposed by control input. Note that this relationship is very similar to the situation

encountered in control systems with sliding modes discussed in Chapter 2.

The straightforward way to determine the dynamics of the closed loop system with

dynamics of the constrain error (7.50) can be determined by inserting (7.45), (7.48) and (7.50)

into (7.1). The result is

A€qþ I�FTLFFA� 1� �

bþ gð Þ�FT LF€fdes �F _q

� � ¼ I�FTLFFA� 1� �

GTs0

€fþKD_fþKPf ffi 0

)ð7:52Þ

Dynamics €fþKD_fþKPf ¼ 0 should be selected fast as compared with the rest of the

system. Then, application of the methods of singularly perturbed systems [2] allows us to

reduce the overall system dynamics to the equation in the first row in (7.52) and the algebraic

equation f qð Þ ¼ 0 2 Rm�1. From f qð Þ ¼ 0, m components of the configuration vector

q 2 Rn�1 can be determined and the resulting dynamics describe changes in n�m compo-

nents of the configuration vector.

Note that (7.52) can be easily obtained using methods of systems with sliding mode, if the

control goal is selected to enforce zero velocity in the constrained direction _f� _fdes ¼ 0.

Indeed, calculating feqF from €f fF ¼ f

eqF

� �� €fdes ¼ 0 in (7.46) and inserting seq ¼ FT feqF þGTs0

into (7.1) yields the first row in (7.52). For consistent lower level initial conditions the

equilibrium solution f qð Þ ¼ 0 2 Rm�1 is enforced.

The dependence of the control in a constrained direction (7.48) on the force s0 introduces acoupling between the motion in constrained and the motion in unconstrained directions.

This suggests that a better design would be if matrix G is selected in such a way that

dynamics (7.47) does not depend on force s0 or if FA� 1GTs0 ¼ 0. This can be guaranteed if

G is selected as an orthogonal complement to constrained matrix F. Thus G needs to be

selected as a null space projection matrix of the form G ¼ I�F#F� �

. In general there is a

freedom in selecting a generalized inverse matrix F# such that F ¼ FF#F. Making

acceleration in the constraint direction independent of the force s0 reduces the freedom in


selecting the pseudoinverse F#. That leads to the following structure of the null space

projection matrix G and pseudoinverse F#

G ¼ I�F#F� �

GT ¼ I�FTF#T� �

FA� 1GT ¼ 0YF# ¼ A� 1FT FA� 1FT� �� 1

ð7:53Þ

MatrixF# stands for a weighted right generalized inverse of the constraint JacobianF, G is

a null space projection matrix which satisfies FG ¼ 0; and GTFT ¼ 0, FA� 1GT ¼ 0,GA� 1FT ¼ 0 and Gk ¼ G for k being any positive integer. The resulting structures of the

matrices G and F# are determined under the condition of the dynamical decoupling of

acceleration in the constrained direction from force s0, thus the selection of a pseudoinverse

as in (7.53) is consistent with dynamical decoupling in constrained systems. In this way,

the dynamical interactions between motion in a constrained direction and motion in an

unconstrained direction are eliminated in the static and dynamic states of a multibody

system.

Selecting such a generalized inverse reduces (7.47) to LF €fþmþ r ¼ fF. Control force in

the constrained direction (7.48) reduces to fF ¼ mþ rþLF €fdes. The closed loop dynam-

ics (7.52) can be now expressed as

A€qþGT bþ gð Þ�FT LF€fdes �F _q

� � ¼ GTs0

€fþKD_fþKPf ¼ 0

GT ¼ I�FTF#T ð7:54Þ

Model (7.54) describes the dynamics of the closed loop system with an explicit description

of the dynamics in the constrained direction and still implicit dynamics in the unconstrained

direction. For a better understanding of the dynamical behavior of the system an explicit

formulation is needed of the dynamics in the unconstrained direction – or the constrained

system dynamics. In deriving the error dynamics (7.47) as variable, a projection of the system

velocity in the constrained direction _f ¼ F _q is applied.

Let us now establish a description of the system dynamics in the new set of coordinates, by

selecting projections of the configuration space velocity vector. It is natural that the projection

of configuration space velocity into the constrained direction is used as one of the components.

As shown earlier from the velocity vector in a constrained direction one can determine m

components of the configuration space velocity vector. The remaining n�mð Þ components of

the configuration vector are then specifying motion in the unconstrained direction or just a

posture of the multibody system. Let motion in unconstrained direction or posture of the

multibody system be described by a minimal set of coordinates with velocity vector

_w ¼ F1G _q 2 R n�mð Þ�1 with a full row rank matrix F1 2 R n�mð Þ�n and G 2 Rn�n null space

projection matrix defined in (7.53). Matrix F1G describes two consecutive transformations.

Let forces fF 2 Rm�1 and fG 2 R n�mð Þ�1 be associated with constraint and the posture,

respectively. The forces are projected into the configuration space by transposing the

corresponding matrices FT and F1Gð ÞT .


By such a selection of new coordinates, new set of velocities _f; _w� �

and the configuration

space force vector can be expressed as

_zFG ¼_f

_w

" #¼

F

G1

" #_q ¼ JFG _q; G1 ¼ F1G

s¼ FT fF þGT1 fG

ð7:55Þ

Here JFG 2 Rn�n stands for the Jacobian matrix. Due to the selection of the matricesF and

F1G, Jacobian JFG has full row rank. The acceleration €zFG can be expressed as

€zFG ¼ JFG€qþ _JFG _q ð7:56Þ

Inserting (7.1) and (7.55) into (7.56) yields system dynamics

€f

€w

� �¼ FA� 1FT FA� 1GT

1

G1A� 1FT G1A

� 1GT1

� �fF

fG

� �� FA� 1 bþ gð Þ

G1A� 1 bþ gð Þ

� �þ _F _q

_G1 _q

� �ð7:57Þ

The dynamical decoupling in (7.57) can be verified just by analyzing the extradiagonal

elements of the control distribution matrix. These terms are FA� 1GT1 and G1A

� 1FT . Having

F1G ¼ G1 and G as in (7.53) we can verify FA� 1GT1 ¼ FA� 1GTFT

1 ¼ 0m� n�mð Þ and

G1A� 1FT ¼ F1GA� 1FT ¼ 0 n�mð Þ�m. Selection of matrix F1 is restricted by the structure

of the constraint Jacobian F. By applying a matrix inversion in block form and equalities

FA� 1GT1 ¼ 0 and G1A

� 1FT ¼ 0, the dynamics (7.57) can be rearranged into

LF €fþLFFA� 1 bþ gð Þ�LF _F _q ¼ fF

LG1€wþLG1G1A� 1 bþ gð Þ�LG1 _G1 _q ¼ fG

LF ¼ FA� 1FT� �� 1

LG1 ¼ G1A� 1GT

1

� �� 1

ð7:58Þ

The projections of the system motion into constrained direction and the posture of the

system are dynamically decoupled and described asm- and (n�m)-order dynamical systems.

Motion in constrained and unconstrained directions is illustrated in Figure 7.5.

Figure 7.5 Illustration of constraints and motion in constrained and unconstrained directions


Let us now look at a slightly different control problem inwhich constraintf qð Þ is required totrack a smooth reference fref tð Þ. The reference is assumed to have continuous and bounded

first- and second-order time derivatives. From (7.56) and (7.58) the dynamics of the error in the

constrained direction ef ¼ f�fref can be expressed as

LF€ef þLFFA� 1 bþ gð Þ�LF _F _qþLF €fref ¼ fF ð7:59Þ

The force input enforcing zero acceleration in the constrained direction is

feqF ¼ LFFA� 1 bþ gð Þ�LF _F _qþLF €f

ref ð7:60Þ

Consequently, this force guarantees that the stability of equilibrium solution ef ¼f�fref ¼ 0 can be expressed in the same form as in (7.48)

fF ¼ feq

F �LF KD _ef þKPef� � ð7:61Þ

The structure of the control input as in (7.49) can be applied without changes. Both control

inputs enforce the same dynamics as in (7.50). The component of the configuration space force

corresponding to the force in the constraint direction is as in (7.51).

The equation in the second row of (7.58) describes the posture dynamics. Acceleration

consists of the disturbance induced acceleration G1A� 1 bþ gð Þ and the velocity induced

acceleration _G1 _q. Assume reference wref 2 R n�mð Þ�1 to be tracked by w 2 R n�mð Þ�1. For a

error in the posture eG ¼ w�wref , the control input fG 2 R n�mð Þ�1 can be selected the same

way as the control in the constrained direction. Assume a stable closed loop error dynamics is

defined by

€eG þKDG _eG þKPGeG ¼ 0; eG ¼ w�wref ð7:62Þ

The control input fG 2 Rn�m�1 and corresponding component of the configuration

space force enforcing dynamics (7.62) can be expressed in the same way as in (3.55), which

yields

fG ¼ feq

G �LG1 KDG _eG þKPGeGð Þfeq

G ¼ LG1G1A� 1 bþ gð Þ�LG1 _G1 _qþLG€w

ref

sG ¼ GT1 fG

ð7:63Þ

Formally the structure of the control input (7.63) is the same as the structure of the control

input (7.61). In (7.63) the corresponding configuration space force is expressed as

sG ¼ GTFT1 fG. The decomposition of the configuration space velocity vector as in (7.55)

illustrates a way in which the system motion can be dynamically decoupled by the appropriate

selection of new variables.

The structure of the overall control system is depicted in Figure 7.6. The topology has two

structurally identical cells for determining the desired accelerations and common disturbance

compensation block implemented in the configuration space. The gains acting on the desired


Figure 7.6 The acceleration controller of a soft constrained system

accelerations are specific and stay for the product of the corresponding inertia matrix and the

transposed Jacobian.

Partition of the velocity vector (7.55) leads to decoupled dynamics if the projectionmatrices

are selected a null space projection matrix G and F1 2 R n�mð Þ�n a full row rank matrix. This

matrix can be selected in many different ways. For example, it may be selected to enforce the

desired posture of a manipulator, avoid singularities, avoid kinematic obstacles, realize a task,

and so on. More on the selection of this matrix will be discussed in Chapter 9.

Example 7.2 Enforcement of Constraints in Configuration Space In this example we

will illustrate the enforcement of constraint control of a multibody mechanical system in

configuration space. As an example, a planar 3-dofmanipulator is used. As shown inChapter 1,

the dynamics of the manipulator can be written in the form


Here A qð Þ 2 R3�3 is the kinetic energy matrix, bT q; _qð Þ ¼ b1 b2 b3½ � is the vector

representing terms depending on velocities, gT qð Þ ¼ g1 g2 g3½ � is the vector of the

gravitational forces and sT ¼ t1 t2 t3½ � is the vector of joint torques generated by

actuators. The elements of the kinetic energy matrix can be found as

a11 ¼ c11 þ d 01cos q2ð Þþ d02cos q3ð Þþ d03cos q2 þ q3ð Þ

a12 ¼ c21 þ 1

2d 01cos q2ð Þþ d02cos q3ð Þþ d03cos q2 þ q3ð Þ½ �

a13 ¼ c31 þ 1

2d 02cos q3ð Þþ d03cos q2 þ q3ð Þ½ �

a21 ¼ a12 a31 ¼ a13

a22 ¼ c22 þ d 02cos q3ð Þ a32 ¼ a23

a23 ¼ c32 þ 1

2d 02cos q3ð Þ a33 ¼ c33

ð7:65Þ


Here the parameters and variables are expressed as

c11 ¼ I1 þ I2 þ I3 þm1 l2m1 þml2m2 þm3 lm32þ m1 þm2ð Þl21 þm3l

22

c22 ¼ I2 þ I3 þm2l2m2 þm3l

2c3 þm3l

22

c33 ¼ I3 þm3l2m3

c21 ¼ I2 þ I3 þm2l2m2 þm3l

2m3 þm3l

22

c31 ¼ I3 þm3l2m3

c32 ¼ c31

d01 ¼ 2m2l1lm2 þ 2m3l1l2; d02 ¼ 2m3l2lm3; d03 ¼ 2m3l1lm3

I1 ¼ m1l21=12; I2 ¼ m2l

22=12; I3 ¼ m3l

23=12

The elements of vector bT q; _qð Þ ¼ b1 b2 b3½ � are expressed as

b1 ¼ � 2m3 l1 l2 _q1 _q2sin q2ð Þ� 2m2l1 lm2 _q1 _q2sin q2ð Þ� 2m3l1 lm3 _q1 _q2sin q2 þ q3ð Þ

¼ � 2m3l2 lm3 _q1 _q3sin q3ð Þ� 2m3l1 lm3 _q1 _q3sin q2 þ q3ð Þ�m2l1 lm2 _q22sin q2ð Þ

¼ �m3l1 lm3 _q22sin q2 þ q3ð Þ�m3l1 l2 _q

22sin q2ð Þ� 2m3l2 lm3 _q2 _q3sin q2 þ q3ð Þ

¼ �m3l2 lm3 _q23sin q3ð Þ�m3l1 lm3 _q

23sin q2 þ q3ð Þ

b2 ¼ m3l1 l2 _q21sin q2ð Þþm2l2 lm2 _q

21sin q2ð Þþm3l1 lm3 _q

21sin q2 þ q3ð Þ

¼ � 2m3l2 lm3 _q1 _q3sin q3ð Þ� 2m3l2 lm3 _q2 _q3sin q3ð Þ�m3l2 lm3 _q23sin q3ð Þ

b3 ¼ m3l2 lm3 _q21sin q3ð Þþm2l1 lm3 _q

21sin q2 þ q3ð Þþ 2m3l2 lm3 _q1 _q2sin q3ð Þ

¼ þm3l2 lm3 _q221sin q3ð Þ

The gravitational terms gT qð Þ ¼ g1 g2 g3½ � areg1 ¼ gm1 lm1cos q1ð Þþ gm2lm2cos q1 þ q2ð Þþ gm2l1cos q1ð Þ

þ gm3lm3cos q1 þ q3ð Þþm3l2cos q1 þ q2ð Þþ gm3l1cos q1ð Þg2 ¼ gm2lm2cos q1 þ q2ð Þþ gm3lm3cos q1 þ q3ð Þþ gm3l2cos q1 þ q2ð Þg3 ¼ gm3lm3cos q1 þ q3ð Þ

Here Ii;mi; li; lcm; i ¼ 1; 2; 3 are the moments of inertia, mass, length and the center of

gravity for each link respectively. The parameters are selected as

m1 ¼ 10 kg l1 ¼ 1 m lm1 ¼ 0:5 m

m2 ¼ 8 kg l2 ¼ 1 m lm2 ¼ 0:5 m

m3 ¼ 6 kg l3 ¼ 1 m lm3 ¼ 0:5 m

ð7:69Þ

(7.66)

(7.67)

(7.68)


The constraint is selected as

f ¼ q1 þaq2 þ 0q3

fref ¼ 0ð7:70Þ

The constraint Jacobian is

F ¼ 1 a 0½ � ð7:71Þ

The projection of the manipulator dynamics into the constraint direction is expressed as

LF €fþm q; _qð Þþ r qð ÞþLF €fref �LFFA� 1GTs0 ¼ fF

LF ¼ FA� 1FT� �� 1

m q; _qð Þ ¼ LF FA� 1b q; _qð Þ� _F _q� �

r qð Þ ¼ LFFA� 1g qð Þ

ð7:72Þ

In examples in Chapters 7, 8 and 9 detailed calculations, instead of the direct MATLAB�

matrix calculus solutions, of elements of matrices and vectors are given. This way users who

would like to apply direct programming may use these results. The parameters in (7.72) can be

calculated as

. Inverse kinetic energy matrix

A� 1 ¼ai11 ai12 ai13

ai21 ai22 ai23

ai31 ai32 ai33

264

375

ai11 ¼ a22a33 � a32a23ð Þ=det Að Þ ai23 ¼ a12a21 � a23a11ð Þ=det Að Þai12 ¼ a13a32 � a33a12ð Þ=det Að Þ ai31 ¼ a21a32 � a31a22ð Þ=det Að Þai13 ¼ a12a23 � a22a13ð Þ=det Að Þ ai32 ¼ a12a31 � a32a11ð Þ=det Að Þai21 ¼ a22a31 � a33a21ð Þ=det Að Þ ai33 ¼ a11a22 � a12a21ð Þ=det Að Þai22 ¼ a11a33 � a31a13ð Þ=det Að Þdet Að Þ ¼ a11a22a33 þ a12a23a31 þ a21a32a13 � a13a22a31 � a21a12a33 � a32a23a11

LF ¼ FA� 1FT� �� 1 ¼ 1

ai11 þ ai12 þ ai21ð Þaþ ai22a2¼ l

. F# ¼ A� 1FTLF ¼ lai11 þaai12

ai21 þaai22

ai31 þaai32

24

35 ¼

f#11

f#21

f#31

264

375


. G ¼ I�F#F ¼ I3�3 �f#11

f#21

f#31

264

375 1 a 0½ � ¼

1�f#11 �af#

11 0

�f#21 1�af#

21 0

�f#31 �af#

31 0

264

375

. LFFA� 1 ¼ l 1 a 0½ �ai11 ai12 ai13

ai21 ai22 ai23

ai31 ai32 ai33

2664

3775 ¼ l w11 w12 w13½ �

w11 ¼ ai11 þaai21; w12 ¼ ai12 þaai22; w13 ¼ ai13 þaai23

. m q; _qð Þ ¼ LF FA� 1b q; _qð Þ� _F _q� � ¼ l w11b1 þw12b2 þw13b3ð Þ� l01�3q

. r qð Þ ¼ LFFA� 1g qð Þ ¼ l w11g1 þw12g2 þw13g3ð Þ

Now (7.72) can be written as a scalar equation

l €fþm q; _qð Þþ r qð Þ ¼ fF ð7:73Þ

The resulting dynamics are described by a second-order differential equation. Selection of

the constraint control input fF can follow the standard steps of acceleration control design.

Let the desired acceleration in the constrained direction be selected as

€qdesF ¼ 0� dF cFfþ _f� �

cF ¼ 10; dF ¼ 125ð7:74Þ

The selection of parameters is not for best performance but rather to have diagrams that

show the salient features of the system behavior. In this particular example, both parameters cFand dF can be selected larger.

Then the control force in the constrained direction can be expressed as

fF ¼ f F dis þLF€qdesF ¼ f F dis þ l€qdesF ð7:75Þ

The disturbance in the constrained direction fFdis ¼ m q; _qð Þþ r qð Þ is estimated by a simple

observer

dz

dt¼ g fF þ gl _f� z

� �;

fFdis ¼ z� gl _fg¼ 600

ð7:76Þ

The projection of the constraint control into configuration space can be expressed as

sF ¼ FT fF

t1Ft2Ft3F

264

375 ¼

1

a

0

264

375fF ¼

fF

afF

0

264

375 ð7:77Þ


Note that, in the derivation of the control input, only the constraint Jacobian F and LF are

needed in the control input design. The other matrices and the disturbances m q; _qð Þ; r qð Þ arederived for the sake of completeness of the example.

In this example control of the unconstrained motion is not of interest. In order to make

experiments wewill assume that configuration variables q1 and q3 are controlled to track given

references. The configuration space control is selected the same way as in Example 7.1. The

reference trajectories for q1 and q3 are selected as

qref1 ¼ 0:5þ 0:15 sin 3:14tð Þ radqref2 ¼ not specified

qref3 ¼ 0þ 0:25 sin 6:28tð Þ rad

ð7:78Þ

The controller is now designed for tracking q1 and q3 separately with input forces

t1q ¼ t1dis þ a11€qdes1

t2q ¼ t2dist3q ¼ t3dis þ a33€q

des3

ð7:79Þ

The configuration space input force in q2 axis is just a disturbance compensation.

Generalized disturbances are estimated using observerswith velocity and input forces as inputsdz1

dt¼ g t1 þ ga11 _q1 � z1ð Þ;

t1dis ¼ z1 � ga11 _q1dz2

dt¼ g t2 þ ga22 _q2 � z2ð Þ;

t2dis ¼ z2 � ga22 _q2dz3

dt¼ g t3 þ ga33 _q3 � z3ð Þ;

t3dis ¼ z3 � ga33 _q3

g ¼ 600

ð7:80Þ

The generalized control errors are determined as

s1 ¼ c1e1 þ _e1; e1 ¼ q1 � qref1

s3 ¼ c3e3 þ _e3; e3 ¼ q3 � qref3

c1 ¼ 5; c3 ¼ 25

ð7:81Þ

The desired acceleration is calculated as

€qdes1 ¼€qref1 � d1s1

€qdes3 ¼€qref3 � d3s3

d1 ¼ 25; d3 ¼ 75

ð7:82Þ


The configuration space forces, enforcing the desired accelerations (7.82), can be expressed

ast1q ¼ t1dis þ a11€q

des1

t2q ¼ t2dis þ a220

t3q ¼ t3dis þ a33€qdes3

ð7:83Þ

With such selection of the configuration space forces in the q1 and q3 coordinates, the overall

control input enforcing constraints (7.70) and motion (7.78) can be expressed as

sF ¼ sq þFT fF

t1

t2

t3

264

375¼

t1q

t2q

t3q

264

375þ

t1F

t2F

t3F

264

375 ¼

t1q

t2q

t3q

264

375þ

fF

afF

0

264

375 ð7:84Þ

Transients in the configuration control for initial conditions q1 0ð Þ ¼ 0:5 rad;q2 0ð Þ ¼ q3 0ð Þ ¼ 0:1 rad and a ¼ � 0:5 are shown in Figure 7.7. The left column shows

Figure 7.7 Transients in a 3-dof manipulator constrained to maintain f tð Þ;¼ q1 tð Þþaq2 tð Þ and

a ¼ � 0:5. The left column shows position q1 tð Þ; qref1 tð Þ and positions q2 tð Þ and q3 tð Þ. The right column

shows the control force in constrained direction fF tð Þ, the constraints f tð Þ and the q2 q1ð Þ diagram, for

initial conditions q1 0ð Þ ¼ 0:5 rad; q2 0ð Þ ¼ q3 0ð Þ ¼ 0:1 rad. The observer gain is g ¼ 600 and reference

positions as in (7.78)


position q1 tð Þ; qref1 tð Þ and positions q2 tð Þ and q3 tð Þ. The right column shows the control force in

the constrained direction fF tð Þ, the constraint f tð Þ and the q2 q1ð Þ diagram.

Transients in the configuration control for initial conditions q1 0ð Þ ¼ q3 0ð Þ ¼ 0:1 rad;q2 0ð Þ ¼ 0:25 rad and a ¼ � 2 are shown in Figure 7.8.

In both diagrams the enforcement of constraints is clearly illustrated. The interaction

between constraint control and configuration control is noticable. This is the result of the fact

that the positions of q1 tð Þ and q3 tð Þ are directly analyzed in the configuration space and not inthe constraint Null space. We will return to this in Chapter 9 in which we will analyze task

control for constrained systems.

7.3.1 Enforcement of Constraints by Part of Configuration Variables

In some applications not all axes canbeused for the enforcementof constraints. Let the axes used

to maintain constraint (7.43) be selected by the designer and specified by selection matrix

S 2 Rn�nYq* ¼ Sq; with the diagonal elements being zero for axes that should not influence

constraint and being one for axes that should participate in maintaining the constraint

requirements. In this way, the configuration vector is split into two components: (i) the first

Figure 7.8 Transients in a 3-dof manipulator constrained to maintain f tð Þ;¼ q1 tð Þþaq2 tð Þ and

a ¼ � 2. The left column shows position q1 tð Þ; qref1 tð Þ and positions q2 tð Þ and q3 tð Þ. The right column

shows the control force in constrained direction fF tð Þ, the constraints f tð Þ and the q2 q1ð Þ diagram, for

initial conditions q1 0ð Þ ¼ q3 0ð Þ ¼ 0:1 rad; q2 0ð Þ ¼ 0:25 rad. The observer gain is g ¼ 600 and reference



contributes to motion in the constrained direction and (ii) the second has no contribution to that

motion. By concatenating these two motions similarly as used in (7.55) the new velocity vector

can be written as

_zFS ¼_fS

_wS

" #¼ FS

GS

� �_q ¼ JSFG _q ð7:85Þ

The second variable may be selected to enforce some additional requirements in the system

motion, consistent with constraints and the selection matrix S. Matrix FS ¼ FS stands for

the product of the constraint Jacobian and the selection matrix, and GS stands for the full

row rank matrix, yet to be determined. The forces vector can be expressed as

s ¼ JSFG� �

T fFG ¼ FTS fFS þGT

S fGS where fFS 2 Rm�1 stands for forces in the constrained

direction, and fGS stands for forces that enforce unconstrainedmotion. Differentiation of (7.85)

on the trajectories of system (7.1) yields

€zSFG ¼ JSFG€qþ _JS

FG _q

€fS

€wS

" #¼ FSA

� 1FTS FSA

� 1GTS

GSA� 1FT

S GSA� 1GT

S

" #fF

fG

" #� FSA

� 1 bþ gð ÞGSA

� 1 bþ gð Þ

" #þ

_FS _q

_GS _q

" # ð7:86Þ

Similarly as in (7.57), selection GS ¼ F1GFS where F1 2 R n�mð Þ�n is a full row rank

matrix and GTFS ¼ I�FT

SF#TS

with F#

S ¼ A� 1FTS FSA

� 1FTS

� �� 1yields FSA

� 1GTS ¼ 0,

GSA� 1FT

S ¼ 0; and dynamics (7.86) can be rearranged into

LFS€fS þLFSFSA

� 1 bþ gð Þ�LFS_FS _q ¼ fFS

LGS€wS þLGSGSA� 1 bþ gð Þ�LGS _GS _q ¼ fGS

LFS ¼ FSA� 1FT

S

� �� 1

LGS ¼ GSA� 1GT

S

� �� 1

ð7:87Þ

The dynamics (7.87) have the same form as in (7.58) (with changes in the projection

matrices) and the selection of a control can follow the same procedure as in Section 7.3. The

control inputs will have the same structure as in (7.50), (7.61) or (7.63). This solution pointing

out to the posibility to use part of the configuration space variables for enforcing some

relationship and the other part of the variables can be controlled in such a way that these two

motions are dynamically decoupled.Wewill discuss similar partition of variables in relation to

realization of redundant tasks.

7.4 Hard Constraints in Configuration Space

Let us analyze the behavior of system (7.1) under the assumption that motion is required to

satisfym < n hard holonomic constraintsf qð Þ ¼ 0 2 Rm�1; m < n. The Jacobian associated

with the constraints is defined asF ¼ qf qð Þ=qq½ � 2 Rm�n; m < n and is assumed to have full

row rank. As a result of interaction, additional constraint forces will appear and they should be


included in the system equations. As shown in Chapter 1 the dynamics of system (7.1) in

contact with constraint manifold can be described by

A€qþ b q; _qð Þþ g qð Þ�FTk ¼ s ð7:88Þ

Here k 2 Rm�1 stands for thevector of Lagrangemultipliers. It needs to be determined from

the requirements that system (7.88) satisfies the constraint equation. Lagrange multipliers

stand for the forcevector needed tomaintain system (7.88) in the constraintmanifold and in that

sense is very similar to the control input discussed in Section 7.3 enforcing motion in the

constraint manifold. There are many methods to determine Lagrange multipliers [3]. Here the

same approach as in Section 7.2 will be applied. The Lagrange multiplier k will be taken as a

virtual control input in system (7.88). This can be directly observed by rewriting (7.88) as

A€qþ bþ g� s ¼ FTk.Satisfying constraints f qð Þ ¼ 0 can be interpreted as enforcing a zero velocity in the

constrained directions, or equivalently selecting Lagrange multipliers that enforce stability in

manifold (7.89) with constraint consistent initial conditions

Ss ¼ q; _q : s q; _qð Þ ¼ _f qð Þ ¼ F _q ¼ 0 2 Rm�1; m < n� � ð7:89Þ

Projection of the motion of system (7.88) into manifold (7.89) can be written by finding the

derivative of s q; _qð Þ 2 Rm�1

_s q; _qð Þ ¼€f qð Þ ¼ F€qþ _F _q

_s q; _qð Þ ¼€f qð Þ ¼ FA� 1 s� b q; _qð Þ� g qð Þ½ �|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}unconstrained acceleration¼€q*

þFA� 1FTkþ _F _q

_s q; _qð Þ ¼€f qð Þ ¼ F€q* þFA� 1FTkþ _F _q

ð7:90Þ

Acceleration in the constrained direction is the sum of the projected acceleration of the

unconstrained systemF€q*, the velocity induced acceleration_F _q in the constraint direction and

the constraint force induced acceleration FA� 1FTk. Note that the Lagrange multiplier

distribution matrix is the same as the control distribution matrix in (7.47).

In order to satisfy conditions (7.90) with constraint consistent lower level initial conditions_f q 0ð Þ½ � ¼ 0 and f q 0ð Þ½ � ¼ 0, the acceleration in the constrained direction must be zero. F€q*

and _F _q are determined by the systemmotion, thus the balance forces – the constraint forces that

will appear due to the interaction of the unconstrained systemwith constraints – are represented

by Lagrange multipliers. With €q* ¼ A� 1 s� b� gð Þ, from (7.90) the Lagrange multipliers

satisfying _s q; _qð Þ ¼ €f q tð Þ½ � ¼ 0 can be determined as

€f qð Þ ¼ F€q* þFA� 1FTkþ _F _q ¼ 0

k ¼ � FA� 1FT� �� 1 F€q* þ _F _q

� �k ¼ � FA� 1FT

� �� 1 FA� 1 s� b� gð Þþ _F _q� �

k ¼ �F#T s� b� gð Þ�LF _F _q

ð7:91Þ


Here F#T ¼ FA� 1FT� �� 1FA� 1 stands for the transpose of the right weighted pseu-

doinverse of the constraint Jacobian andLF ¼ FA� 1FT� �� 1

can be interpreted as the kinetic

energymatrix. The structure of the pseudoinverse is the same as in (7.43). Lagrangemultipliers

(7.91) enforce constraint conditions _s q; _qð Þ ¼ €f qð Þ ¼ F€qþF _q ¼ 0 or F€q¼ � _F _q.Note that if the constraint equations are linear with respect to velocity

s q; _qð Þ ¼ f qð ÞþFf qð Þ _q ¼ 0. then from _s q; _qð Þ ¼ F _qþ _Ff _qþFf€q ¼ 0 using the same

procedure as in (7.90) one can determine the constraint forces – the Lagrange multiplier

k 2 Rm�1 – in a very similar form by taking matrixFf as a constraint Jacobian. That confirms

the remark regarding the structure of the constraint equations and the consistency of the

formulation of constraints as in (7.89) or as s q; _qð Þ ¼ f qð ÞþFf qð Þ _q ¼ 0.

The Lagrange multipliers as obtained in (7.91) consist of a projection of the system

disturbance F#T bþ gð Þ, a projection of the velocity induced acceleration in constraint

direction _F _q and a projection of the joint force F#Ts. It is very similar to the structure of

the force needed to maintain a soft constraint in manifold (7.43) with the addition of a

projection of the input force acting on the system. This shows the equivalence of the soft and

hard constraints in multibody systems. The constraints in the form s q; _qð Þ ¼ f qð Þ�f tð Þ ¼ 0can be analyzed within the same framework.

By inserting (7.91) into (7.88) the dynamics of a constrained system for motion satisfying

the constraint consistent initial conditions _f q 0ð Þ½ � ¼ 0 and f q 0ð Þ½ � ¼ 0 can be obtained in the

following form

A€qþGT bþ gð ÞþFTLF _F _q ¼ GTs

f qð Þ ¼ 0

GT ¼ I�FTF#T� � ð7:92Þ

These equations have the same structure as (7.54) obtained for soft constrained systems.

In order to simplify analysis of this system, the motion procedure used in Section 7.3 can be

directly applied. The difference here is in the physical meaning of the constraint forces. In

Section 7.3 an active control input is selected to ensure that the balance of forces projected into

the constrained directions is such that the system remains in a constructed manifold (7.43). In

the case examined here, motion in the constraint manifold is imposed by the reaction forces

resisting motion in the constrained directions. These forces, are determined from the

requirement that velocity in the constrained direction is zero. In order to determine motion,

the system dynamics (7.88) is projected into constrained and unconstrained directions.

Now projection of motion in constrained and unconstrained directions, similarly to (7.55)

leads to equations of motion in both the constrained direction and the unconstrained direction

LF€f þ LFFA� 1 bþ gð Þ�LF _F _q ¼ fF þ k

LG1€wG þ LG1G1A� 1 bþ gð Þ�LG1 _G1 _q ¼ fG

ð7:93Þ

The force acting in the constrained direction and the Lagrange multipliers appear to

balance the projection of system disturbance and velocity induced acceleration. In order to

maintain constraints, acceleration in the constrained direction _s q; _qð Þ must be zero and the

sum of the force due to the constraints and the projection of the joint force in the constrained

direction must compensate for the projection of the disturbances FA� 1 bþ gð Þ and the


velocity induced acceleration _F _q, as shown in fF þ k ¼ F#T bþ gð Þ�LF _F _q. If the pro-

jection of forces is not controlled, the configuration space control input is independent of

the interaction with constraints and the forces due to the constraint will be changed

to compensate for the force fF. If the control forces can be changed, then by selecting

a particular structure of fF, the force induced due to interaction with the constraint manifold

can be controlled.

Unconstrained motion is performed in the tangential plane to the constraint manifold in

the contact point and is described by the second equation in (7.93). That expression can be

rewritten in the following form

LG1€wG þLGG1A� 1 bþ gð Þ�LG1 _G1 _q ¼ fG

f qð Þ ¼ 0 2 Rm�1

LG1 ¼ G1A� 1GT

1

� �� 1

ð7:94Þ

Structurally it is the same as system (7.1)with the inertiamatrix beingLG and the disturbance

term being the sum of the projected system disturbance and the weighted constraint velocity

induced acceleration. The input is represented by fG. Equation (7.94) describes the dynamics of

system (7.1) consistent with holonomic constraint f qð Þ ¼ 0 2 Rm�1 under the constraint

consistent initial conditions f q 0ð Þ½ � ¼ 0 and _f q 0ð Þ½ � ¼ 0.

In the same way, as discussed for soft constrained systems, it is possible to show a relation

between the acceleration of the unconstrained system A€q* ¼ s� b q; _qð Þ� g qð Þ and the

acceleration of the constrained system A€qþ b q; _qð Þþ g qð Þ�FTk ¼ s. Taking into account

the constraint force k ¼ �F#TA€q* �LF _F _q we can, under assumption that constraint

consistent initial conditions are satisfied, write

€q ¼ I�F#F� �

€q* �F# _F _q

f qð Þ ¼ 0 2 Rm�1ð7:95Þ

The derivation of (7.95) uses the identity A� 1FTF#TA ¼ F#F for the dynamically

consistent right pseudoinverse F# ¼ A� 1FTLF. Acceleration of the constrained system is

composed of a projection of the unconstrained system acceleration into the unconstrained

direction and the velocity induced acceleration weighted by the right pseudoinverse of the

constraint Jacobian.

The equivalence of the soft and hard constrained systems is now apparent and the selection

of the control in both cases may follow the same procedure. In the case of the hard constraints

the forces due to the constraints – represented by Lagrangemultipliers – are additionally acting

in the system. From (7.93) it follows that the configuration forces sF ¼ FT fF corresponding to

the force in the constrained direction fF can be applied to compensate for the interaction force

FTk. This shows a way of controlling forces due to the interaction of a multibody system with

the environment. More on this will be discussed in Chapter 8.

Example 7.3 Hard Constraints in Configuration Space In this example we will illus-

trate the behavior of a multibody system with hard constraints in configuration space. As an

example, a planar 3-dof manipulator from Example 7.2 is used.


As shown in Chapter 1, the dynamics of a manipulator in the presence of a hard constraint

can be written in the form

A qð Þ€qþ b q; _qð Þþ g qð Þ�FTk ¼ s ð7:96Þ

Here A qð Þ 2 R3�3 is the kinetic energy matrix, bT q; _qð Þ ¼ b1 b2 b3½ � is the vector

representing the terms depending on velocities, gT qð Þ ¼ g1 g2 g3½ � is the vector of the

gravitational forces, F stands for the constraint Jacobian, k stands for the vector of Lagrange

multipliers, sT ¼ t1 t2 t3½ � is the vector of the joint torques generated by actuators and

qT ¼ q1 q2 q3½ � is the configuration vector. The structure of the kinetic energy matrix and

vectors b q; _qð Þ and g qð Þ are given in Equations (7.65)–(7.68). The parameters of the

manipulator are given in (7.69).

In this example the constraints will be selected the same as in Example 7.2, thus

f ¼ q1 þaq2 þ 0q3 ¼ 0 ð7:97Þ


F ¼ 1 a 0½ � ð7:98Þ

For a given constraint Jacobian the Lagrange multiplier that enforces zero acceleration in

the constraint direction is determined as in (7.91)

k ¼ � FA� 1FT� �� 1 FA� 1 s� b� gð Þþ _F _q

� � ¼ �F#T s� b� gð Þ�LF _F _q ð7:99Þ

The pseudoinverse F# is calculated in Example 7.2 as F#T ¼ f#11 f#

21 f#31

� �.

The constraint Jacobian is constant, thus the term LF _F _q ¼ 0. By denoting the

components of the force vector s� b� gð Þ as sbqi; i ¼ 1; 2; 3 the Lagrange multipliers can

be expressed as

k ¼ � f#11sbq1 þf#

21sbq2 þf#31sbq3

ð7:100Þ

Insertion of force

sl ¼ FTl

t1l

t2l

t3l

2664

3775¼

1

a

0

2664

3775l ¼

l

al

0

2664

3775 ð7:101Þ

into dynamics (7.96) enforces zero acceleration in the constraint direction. For constraint

consistent initial conditions _f q 0ð Þ½ � ¼ 0 and f q 0ð Þ½ � ¼ 0, the velocity in the constrained

direction will be zero, and consequently the constraint f qð Þ ¼ 0 will be enforced. If, for

example, f q 0ð Þ½ � 6¼ 0 motion will be enforced in f qð Þþf q 0ð Þ½ � ¼ 0.

This situation is illustrated in Figure 7.9 in which transients are shown which control q3 tð Þto track reference q

ref3 ¼ 0þ 0:25 sin 6:28tð Þ rad by applying configuration space input

t3q ¼ t3dis þ a33€qdes3 , with the desired acceleration as in (7.82) and the generalized error as


in (7.81) s3 ¼ c3e3 þ _e3; e3 ¼ q3 � qref3 ; c3 ¼ 25. The positions q1 tð Þ and q2 tð Þ are not con-

trolled. Their dynamics are the result of the interaction forces. The configuration space input

forces in all three degrees of freedom are set as

sF ¼ sq þFTl

t1

t2

t3

2664

3775 ¼

tdis1

tdi2

t3dis þ a33€qdes3

2664

3775þ

l

al

0l

2664

3775 ð7:102Þ

The initial conditions are set at q1 0ð Þ ¼ 0:05; q2 0ð Þ ¼ q3 0ð Þ ¼ 0:3 rad and the constraint

Jacobian isFT ¼ 1 � 1 0½ �. In Figure 7.9, the left column shows positions q1 tð Þ, q2 tð Þ andq3 tð Þ. The right column shows the constraint variable f qð Þ, the Lagrange multiplier l and theconfiguration space forces t1l and t2l.

The transient in constraint variable f tð Þ ¼ q1 þaq2 þ 0q3 show slight changes and the

influence of the force t3. The transients illustrate the fact that selection of the Lagrange

Figure 7.9 Transients in a 3-dof manipulator constrained to maintain f tð Þ ¼ q1 þaq2 þ 0q3 and

a ¼ � 1. The left column shows position q1 tð Þ and positions q2 tð Þ and q3 tð Þ. The right column shows the

constraint f tð Þ, the interaction force in constrained direction l tð Þ and the projection of l tð Þ into

configuration space t1l and t2l. Initial conditions are q1 0ð Þ ¼ 0:05 rad; q2 0ð Þ ¼ q3 0ð Þ ¼ 0:3 rad and

the disturbance observer gain is g ¼ 600


multipliers enforces zero acceleration and velocity in the constraint direction, thus the initial

error in the position is propagated without changes as a steady-state error. To eliminate the

steady state an additional term that will guarantee convergence towards the constraint should

be added to l tð Þ. The simplest way is to use the same approach as in Example 7.2. The simple

addition of the desired acceleration (7.74) €qdesF ¼ 0� dF cFfþ _f� �

to the constraint force

[thus realizing configuration space forces as in (7.103)] will enforce both the convergence of

the system motion to the convergence manifold and the stability of the constraints.

sF ¼ sq þFTp

t1

t2

t3

2664

3775 ¼

tdis1

tdi2

t3dis þ a33€qdes3

2664

3775þ

lþ€qdesF

a lþ€qdesF� �

0 lþ€qdesF� �

2664

3775 ð7:103Þ

The results are shown in Figure 7.10. Here the same transients as in Figure 7.9 are shown

with configuration space input force as in (7.103). The convergence and stability of the

constraints are clearly illustrated.

Figure 7.10 Transients in a 3-dof manipulator constrained to maintain f tð Þ ¼ q1 � q2 þ 0q3 and

control as in (7.103). The left column shows position q1 tð Þ and positions q2 tð Þ and q3 tð Þ. The right column

shows the constraintf tð Þ, the interaction force in constrained direction l tð Þ and the projection of l tð Þ intoconfiguration space t1l and t2l. Initial conditions are q1 0ð Þ ¼ 0:05 rad; q2 0ð Þ rad and q3 0ð Þ ¼ 0:3 rad andthe disturbance observer gain is g ¼ 600


Figure 7.11 illustrates enforcement of the time changing constraints. The diagrams are taken

under the same conditions as in Figure 7.10, except that here the constraints are defined as

f ¼ q1 þaq2 þ 0q3 ¼ 0:05þ 0:025 sin 3:14tð Þ ð7:104Þ

Diagrams illustrate the convergence and the stability of the constraints.

References


2. Kokotovic, P.V., O’Malley, R.B., and Sannuti, P. (1976) Singular perturbations and order reduction in control

theory. Automatica, 12, 123–126.

3. Arnold, V.I. (1989) Mathematical Methods of Classical Mechanics, 2nd edn, Springer-Verlag, New York.

4. Nakao, M., Ohnishi, K. and Miyachi, K. (1987) A robust decentralized joint control based on interference

estimation. Proceedings of the IEEE Conference on Robotics and Automation, vol. 4, pp. 326–331.

Figure 7.11 Transients in a 3-dof manipulator constrained to maintain constraint relation

f ¼ q1 � q2 þ 0q3 ¼ 0:05þ 0:025 sin 3:14tð Þ and control as in (7.103). The left column shows position

q1 tð Þ and positions q2 tð Þ and q3 tð Þ. The right column shows the constraint f tð Þ, the interaction force in

constrained direction l tð Þ and the projection of l tð Þ into configuration space t1l and t2l. Initial conditionsare q1 0ð Þ ¼ 0:05 rad; q2 0ð Þ ¼ q3 0ð Þ ¼ 0:3 rad and the disturbance observer gain is g ¼ 600


Further Reading

Asada, H. and Slotine, J.-J. (1986) Robot Analysis and Control, John Wiley & Sons, Inc., New York.

Blajer, W. (1997) A geometric unification of constrained system dynamics. Multibody System Dynamics, 1, 3–21.

Craig, J.J. and Raibert, M. (1979) A systematic method for hybrid position/force control of a manipulator. Proceedings

of the IEEE Computer Software Applications Conference, Chicago.

de Sapio,V. andKhatib, O. (2005)Operational space control ofmultibody systemswith explicit holonomic constraints.

Proceedings of the IEEE International Conference on Robotics and Automation, Barcelona, Spain.

Fisher, W.D. and Mujtaba, M.S. (1991) Hybrid Position. I Force Control: A Correct Formulation, Measurement and

Manufacturing Systems Laboratory, HPL-91–140.

Khatib, O. (1987) A unified approach for motion and force control of robot manipulators: the operational space

formulation. IEEE Journal on Robotics and Automation, RA-3(1), 43–53.

Khatib, O., Sentis, L., Park, J., andWarren, J. (2004)Whole-body dynamics behavior and control of humal-like robots.

International Journal of Humanoid Robotics, 1(1), 29–43.

Paul, R. (1982) Robot Manipulators: Mathematics, Programming and Control, MIT Press, Cambridge, Mass.

Siciliano, B. (1990) Kinematic control of redundant robot manipulators: a tutorial. Journal of Intelligent and Robotic

Systems, 3, 201–212.

Siciliano, B. and Khatib, O. (eds) (2008) Springer Handbook of Robotics, Springer-Verlag, New York, ISBN: 978-3-

540-23957-4.

Spong, M.W., Hutchinson, S., and Vidyasagar, M. (2006) Robot Modeling and Control, John Wiley & Sons, Inc.,

New York.


8

Operational Space Dynamicsand Control

In the analysis of motion system, the configuration space description of system dynamics is

readily available. The dynamics of the mechanical system associated with performing a task

defined by a set of coordinates x ¼ x qð Þ 2 Rp�1; p � n involves mapping the configuration

space dynamics into operational space. The set of all permissible values of coordinates

x qð Þ 2 Rp�1 defines the operational space for a given task.

In general, operational space coordinates may represent any set of coordinates defining

kinematicmapping between configuration space and operational space.Assume the task vector

is given by

x ¼ x qð Þ; x 2 Rp�1; q 2 Rn�1; p � n ð8:1Þ

By taking the derivative of x qð Þ 2 Rp�1, the following relationship is obtained

_x ¼ qx qð Þqq

24

35 _q ¼ J qð Þ _q

€x ¼ J€qþ _J _q

ð8:2Þ

Here€x 2 Rp�1 stands for the vector of operational space accelerations and J qð Þ 2 Rp�n is

the task associated Jacobian matrix. The task Jacobian depends on the configuration vector,

thus is an instantaneous property of the system. The Jacobian has full row rank rank J qð Þ½ � ¼ p

for all permissible values of the task vector. The details of calculating the Jacobian matrix are

not the subject of this text. The details can be found in robotics-related literature [1,2]. The

singularities are defined as configurations for which det J qð Þ½ � ¼ 0.

Since the task is described and executed in operational space and the control forces are

applied in configuration space, the forces mapping from operational to configuration space

plays an important role in describing the dynamics in the operational space control of a

multibody system. We already encountered a similar situation in Chapter 7 while analyzing

constrained systems. The projection of the forces from constraint space into configuration



space is defined by the transpose of the constraint Jacobian. In Chapter 1, mapping of the

operational space force fx 2 Rp�1 into configuration space is found to be expressed as [3]

s ¼ JT fx ð8:3Þ

If the dimension of the task vector is equal to the dimension of the configurationvector p ¼ n

then outside the singularities the inverse J� 1 qð Þ is well defined and the configuration space

velocities can be expressed as

_q ¼ J� 1 qð Þ _x ð8:4Þ

Consequently the configuration space accelerations €q 2 Rn�1, by using Equation (8.2),

can be determined as

€q ¼ J� 1 qð Þ €x� _J qð Þ _q� � ð8:5Þ

For selected task satisfying p < n the direct inverse is not applicable and relations (8.4)

and (8.5) cannot be directly applied.

The relation between velocity projection from configuration space into operational space

and the forces projection from operational space into configuration space has the same form as

the corresponding projections derivedwithin the framework of constrained systems. As shown

later in the text, this similarity would allow the application of similar, if not identical ways of

treating tasks and constraints.

8.1 Operational Space Dynamics

Before addressing the operational space control we need to develop a consistent dynamical

description of the multibody system in operational space. It is obvious that such a description

would depend on the kinematic structure of the system and the specification of the task. In

general, the dimension of the task vector (8.1) and the dimension of the configuration vector

may or may not be the same. If these dimensions are the same p ¼ n, then the Jacobian

matrix (8.2) would have full rank and is invertible outside of singularities. If p � n is satisfied,

then the Jacobianmatrix has full row rank 8x, outside singularity points in operational space. Inthe first case we are talking about a task nonredundant system and in the second case a task

redundant system. In this section we will analyze the dynamics of both cases. More about

definitions of redundancy related definitions can be found in [4,5].

8.1.1 Dynamics of Nonredundant Tasks

For the nonredundant x ¼ x qð Þ; x 2 Rn�1 case the dynamics of a multibody system in

operational space can be obtained by inserting Equations (7.1) and (8.5) into €x ¼ J€qþ _J _q.After some algebra this yields

Lx€xþLx JA� 1b� _J _q� �þLxJA

� 1g ¼ fx

Lx ¼ JA� 1JT� �� 1 ð8:6Þ


Matrix L� 1x ¼ JA� 1JT stands for the operational space force distribution matrix. This

matrix is structurally the same as the constrained force distribution matrix for constrained

systems. The difference is in the structure and meaning of the Jacobian matrix. Matrix Lx can

be interpreted as the operational space kinetic energy matrix (sometimes we will refer to it as

the operational space inertia matrix.) In a more compact form, operational space dynamics can

be expressed as

Lx€xþ l q; _qð Þþ n qð Þ ¼ fx

l q; _qð Þ ¼ Lx JA� 1b q; _qð Þ� _J _q� �

n qð Þ ¼ LxJA� 1g qð Þ

ð8:7Þ

Here l q; _qð Þ 2 Rn�1 stands for the projection of centrifugal and Coriolis forces and

n qð Þ 2 Rn�1 stands for the projection of gravitational forces. The dependence on the

configuration space variables is preserved in Equation (8.7) in order to show the correspon-

dence in configuration space and operational space and an easier establishment of the

correspondence between the domains of the change of variables

For nonredundant tasks J� 1 qð Þ is well defined everywhere except in singularity points.

Outside of singularities, Equation (8.6) can be simplified to

J�TAJ� 1� �

€xþ l q; _qð Þþ n qð Þ ¼ fx

l q; _qð Þ ¼ J� Tb� J�TAJ� 1 _J _q

n qð Þ ¼ J� Tg

ð8:8Þ

Equations (8.7) and (8.8) have the same form as the configuration space dynamics.

For motion out of the singularities, by using projection €x ¼ J€qþ _J _q one can determine

the configuration space acceleration as a function of operational space acceleration as in

Equation (8.5), thus linking in a uniqueway themotion in operational and configuration spaces.

8.1.2 Dynamics of Redundant Tasks

In the case that p < n, mapping of velocities from operational space to configuration space can

no longer be obtained just by inversion of the Jacobian matrix. This situation is similar to that

encountered in the constrained systems discussed in Chapter 7 and the approach used there can

be applied here also.

As shown in Chapter 7, for the constrained systems decomposition of the n-dimensional

configuration space into operational space with associated velocity projection matrix

J qð Þ 2 Rp�n and its orthogonal complement space allows partition of the system dynamics

into two dynamically decoupled components. The projection of forces from operational space

and its orthogonal complement into configuration space is defined bymatrices JT qð Þ andGT qð Þrespectively. The structure of the projection matrix G qð Þ will be determined below.

Let sx ¼ JT fx be vector of configuration space forces generated by operational space forces

fx 2 Rp�1 and vector that complements sx be sG ¼ GTs0, with s0 2 Rn�1. Then the config-

uration space force can be expressed as

s ¼ JT fx þGTs0 ð8:9Þ

Operational Space Dynamics and Control 315

Insertion of (7.1) and (8.9) into €x ¼ J€qþ _J _q yields

Lx€xþ lx q; _qð Þþ nx qð Þ ¼ fx þLxJA� 1GTs0

Lx ¼ JA� 1JT� �� 1

lx q; _qð Þ ¼ Lx JA� 1b� _J _q� �

nx qð Þ ¼ LxJA� 1g

ð8:10Þ

HereLx ¼ JA� 1JT� �� 1 2 Rp�p can be interpreted as the operational space kinetic energy

matrix associated with selected task, l q; _qð Þ 2 Rp�1 stands for the projection of centrifugal

and Coriolis forces, n qð Þ 2 Rp�1 stands for the projection of gravitational forces.

TheLxJA� 1GTs0 stands for the interaction force coupling motion in operational space and

its orthogonal complement space. For dynamical decoupling, s0 should not influence oper-

ational space acceleration, thus the following should be satisfied

JA� 1GTs0 ¼ 0 ð8:11Þ

This requirement is equivalent to conditions (7.53) used in establishing the dynamics of the

constrained system in configuration space. To satisfy (8.11) the structure of thematrixG should

be selected as null space projection matrix

G ¼ I� J#J� �

GT ¼ I� JTJ#T� � ð8:12Þ

Where J# stands for right pseudoinverse of the Jacobian J. Inserting (8.12) into (8.11) yields the

structure of the pseudoinverse J# ensuring dynamical decoupling as

J#¼ A� 1JT JA� 1JT� �� 1 ¼ A� 1JTLx

J#T¼ JA� 1JT� �� 1

JA� 1 ¼ LxJA� 1

ð8:13Þ

Comparison of (7.53) with (8.13) shows identical structures, with the role of the constrained

JacobianF in (7.53) and the role of the operational space Jacobian J in (8.13) being the same.

This showsfirst similarity in the treatment of the constraints and the tasks inmultibody systems.

Now the operational space dynamics can be expressed as

Lx€xþ lx q; _qð Þþ nx qð Þ ¼ fx

Lx ¼ JA� 1JT� �� 1

lx q; _qð Þ ¼ J#Tb q; _qð Þ�Lx_J _q

nx qð Þ ¼ J#Tg qð ÞJ#T ¼ LxJA

� 1

ð8:14Þ

Dynamics (8.14) allows us to uniquely specify p components of the configuration vector;

other n� p can be used to perform another task or to set the posture of the multibody

system.


Inmodel (8.14) the operational space forces act as the input to the system. Let us now look at

a way to determine the configuration space velocities and accelerations from given operational

spacevelocities or accelerations. For nonredundant systems inversion of the Jacobian is applied

to determine configuration space velocities and accelerations outside singularities. In the case

of redundant systems the direct inverse of the Jacobian matrix is not applicable. The minimum

norm solution using the right pseudoinverse matrix J#jJJ# ¼ I� �

is a way of specifying the

configuration space variables

_q ¼ J# _xþG _q0

€q ¼ J# €x� _J _q� �þG€q0

ð8:15Þ

HereG stands for the null space projectionmatrix (8.12) andq0 is any vector inRn. Selection

of the pseudoinverse is not unique and it depends on the structure of the function selected to be

minimized. A reasonable solution in (8.15) will be to find the pseudoinverse minimizing the

kinetic energy T ¼ 1

2_qTA _q of system (7.1) under operational space velocity mapping con-

straints _x ¼ J _q. By introducing Lagrange multipliers l and forming the augmented function

P _q; _x; lð Þ ¼ 1

2_qTA _qþ lT _x� J _qð Þ theminimizationmin

_qP _q; _x; lð Þ becomes a straightforward

task which yields a Lagrange multiplier as

qPq _q

¼ _qTA� lTJ ¼ 0Y lT ¼ _qTJT JA� 1JT� �� 1 ð8:16Þ

Insertion l from Equation (8.16) into _q ¼ A� 1JTl yields a configuration space velocity that

minimizes kinetic energy with constraint _x ¼ J _q

_q ¼ A� 1JT JA� 1JT� �� 1 þG _q0

¼ J# _xþG _q0

J# ¼ A� 1JT JA� 1JT� �� 1

ð8:17Þ

The previously determined dynamically consistent pseudoinverse (8.13) yields the kinetic

energy minimizing solution.

By selecting a minimization function P €q;€x; lð Þ ¼ 1

2€qTA€qþ lT €x� _J _q� J€q

� �it can be

shown that the pseudoinverse as in Equation (8.17) minimizes ‘acceleration energy’

T€q ¼ 1

2€qTA€q under constraints J€q ¼ €x� _J _q, thus yielding configuration space acceleration as

€q ¼ J# €x� _J _q� �þG€q0 ð8:18Þ

The pseudoinverse (8.17) is the same as in (8.13), thus the transformations used so far

minimize the kinetic and ‘acceleration’ energy. The selection of different energy-like functions

T* ¼ 1

2_qTW _q and T*

€q ¼ 1

2€qTW€q would lead to pseudoinverse J# ¼ W� 1JT JW� 1JT

� �� 1,

thus leading to a different configuration space velocity and acceleration.

Now we are ready to derive the complete multibody system dynamics with a redundant

task. As follows from Equations (8.17) and (8.18). Due to redundancy neither configuration

velocity or accelerationvector are uniquely defined by the operational space variables. The task

dynamics is described by (8.14).


Similarly to the procedure applied in the analysis of motion in unconstrained directions of

constrained systems, let the internal configuration (wewill also use the term posture) consistent

with given task be described by aminimal set of independent coordinates xP 2 R n� pð Þ�1. Then

we can decompose the configuration space velocity vector into the task velocity vector _x ¼ J _qand the task consistent posture vector _xP ¼ JPGð Þ _q, where JP ¼ qxP=qqð Þ 2 R n� pð Þ�n stands

for the posture Jacobian and G stands for the task consistent null space projection matrix.

Similarly, fP 2 R n� pð Þ�1 is the posture related force vector. The posture force vector induces

the configuration space force sP ¼ JPGð ÞT fP.Concatenating task and posture velocity vectors into one vector, similarly to the procedure

applied in analysis of the constrained systems in Chapter 7, leads to the task-posture velocity as

_xJP ¼_x

_xP

" #¼

J

JPGð Þ

" #_q ¼

J

GP

" #_q ¼ JJP _q ð8:19Þ

Here GP ¼ JPGð Þ and JJP 2 Rn�n are, by selection of the task and posture, full row rank

Jacobian matrices. The acceleration €xJP ¼ JJP€qþ _JJP _q can be written in the following form

€x

€xP

" #¼

JA� 1JT JA� 1GTP

GPA� 1JT GPA

� 1GTP

" #fx

fP

" #�

JA� 1 bþ gð ÞGPA

� 1 bþ gð Þ

" #þ

_J _q

_GP _q

" #ð8:20Þ

With matrix G as in (8.12) and pseudoinverse as in (8.13) it is easy to verify the relation-

ship JA� 1GTP ¼ 0p� n� pð Þ and GPA

� 1JT ¼ 0 n� pð Þ�p. The force distribution matrix in (8.20)

is a block diagonal and its inverse will be also a block diagonal. Then dynamics (8.20) can

be written as in (8.21). The task and the posture are dynamically decoupled and the forces fxand fP can be selected separately. Note the similarities between the transformations and the

constrained systems discussed in Chapter 7.

Lx€xþLxJA� 1 bþ gð Þ�Lx

_J _q ¼ fx

LP€xP þLPGPA� 1 bþ gð Þ�LP

_GP _q ¼ fP

Lx ¼ JA� 1JT� �� 1

LP ¼ GPA� 1GT

P

� �� 1

ð8:21Þ

Relations (8.20) may serve as a basis for the analysis of the influence of different errors due

to coordinate transformations.

By establishing a consistent projection of the system dynamics, the control input design for

both task and posture is now straightforward.

8.2 Operational Space Control

The operational space dynamics of the fully actuated multibody system (7.1) is described

by (8.7) for nonredundant tasks and by (8.14) for redundant tasks. It is structurally the same as

the configuration space description (7.1) so one can design the control input by applying the

same ideas as used in configuration space control – with a necessary change of variables and

taking care of the redundancy related issues.


8.2.1 Nonredundant Task Control

Let us analyze a problem in which task vector x ¼ x qð Þ 2 Rn�1 with _x ¼ J _q 2 Rn�1,

det Jð Þ 6¼ 0 is to be steered to track its vector reference xref 2 Rn�1. This requirement can

be formulated as selecting a control such that the state of the system (8.7) is enforced to reach

and maintain motion in manifold Sx

Sx ¼ x : ex x; xref� � ¼ x qð Þ� xref ¼ 0; ex x; xref

� � 2 Rn�1� � ð8:22Þ

The formulation is the same as discussed in Section 7.1 for enforcing motion in config-

uration space. Projection of the system (8.7) motion in manifold (8.22) is described by

€ex ¼€x qð Þ�€xref ¼ J€qþ _J _q�€xref

¼ L� 1x fx � lx q; _qð Þþ nx qð Þ½ �f g�Lx€x

ref� �

s ¼ JT fx

ð8:23Þ

The error dynamics (8.23) show the same structure as the configuration space position

tracking error. The difference is in the fact that operational space forces fx or accelerations canbe implemented only by determining the configuration space control forces or accelerations.

That sets the operational space control problem as a two-step procedure – designing the desired

acceleration and/or forces in the operational space and thenmapping the selected variables into

the configuration space. This procedure has been already used in constrained motion control

in Chapter 7.

Let us first look at selection of the desired acceleration and the control force in operational

space and then we will discuss mapping these variables into configuration space. The second-

order error dynamics (8.23) can be stabilized if the desired acceleration in operational space is

selected to enforce closed loop transients

€ex þKxD _ex þKxPex ¼ 0 ð8:24Þ

Here matrices KxD 2 Rn�n and KxPRn�n are design parameters and should be selected to

ensure the stability of the equilibrium solution ex ¼ 0. Closed loop transient (8.24) can be

enforced by operational space desired acceleration

€xdes ¼ €xref � KxD _ex þKxPexð Þ ð8:25Þ

The operational space force realizing the desired acceleration (8.25) can be obtained just by

plugging (8.25) into (8.23) and solving the resulting equation for fx. This procedure yields

fx ¼ lx q; _qð Þþ nx qð ÞþLx€xdes

fx ¼ lx q; _qð Þþ nx qð ÞþLx €xref � KxD _ex þKxPexð Þ� � ð8:26Þ

The structure of the operational space force (8.26) is the same as the structure of the

configuration space control force (7.6). Implementation requires estimation of the opera-

tional space disturbance fd ¼ lx q; _qð Þþ nx qð Þ and the operational space inertia matrix Lx.

By writing the inertia matrix as Lx ¼ Lxn þDLx where Lxn is nominal inertia matrix, the


operational space dynamics (8.7) can be rearranged into

Lxn€xþDLxn€xþ lx q; _qð Þþ nx qð Þ ¼ fx ð8:27Þ

Then (8.26) can be rewritten as

fx ¼ lx q; _qð Þþ nx qð ÞþDLxn€x½ � þLxn €xref � KxD _ex þKxPexð Þ� � ð8:28Þ

The generalized operational space disturbance fdisx ¼ fd þDLxn€x can be estimated com-

ponentwise using methods discussed in Chapter 4. Then the control force in operational space

and its projection to configuration space can be expressed as

fx ¼ fdisx þLxn€xdes

s ¼ JT fxð8:29Þ

Inserting (8.29) into (8.7) yields the closed loop dynamics in operational space

Lxn€x ¼ Lxn€xdes � px fdisxð Þ

€ex þKxD _ex þKxPex ¼ � px fdisxð Þpx fdisxð Þ ¼ fdisx � fdisx

ð8:30Þ

The dynamics are governed by (8.24), and the generalized operational space disturbance

estimation error px fdisxð Þ defines the control error.The configuration space desired acceleration corresponding to the operational space desired

acceleration (8.25) can be from (8.23) determined as

€qdes ¼ J� 1 €xdes � _J _q� � ¼ J� 1 €xref � KxD _ex þKxPexð Þ� _J _q

� � ð8:31Þ

The configuration space control enforcing (8.31) is

s ¼ sdis þA€qdes ¼ sdis þAJ� 1 €xdes � _J _q� � ð8:32Þ

It can be simplified if the velocity induced acceleration _J _q can be neglected. The Jacobian

J and the configuration space inertia matrix are needed for implementation of control

input (8.32).

The configuration space control input realizing the operational space force can be

expressed as

s ¼ JT fx ¼ JT lx q; _qð Þþ nx qð ÞþLx€xdes

� � ð8:33Þ

Insertion of (8.33) into (8.8) yields J€q ¼ €x� _J _q. The error dynamics (8.23) can be rewritten

in the following form

_ex ¼ _x qð Þ� _xref ¼ J _q� _xref ¼ ez

_ez ¼ €x qð Þ�€xref ¼ J€qþ _J _q�€xrefð8:34Þ


The control goal is to enforce the stability of equilibrium solution ex ¼ 0. By select-

ing ez ¼ �Cex as a virtual control in the first equation of (8.34) the control error will be

governed by _ex ¼ �Cex, thus it will be stable if C is selected as a positive definite matrix.

Selection of C > 0 as diagonal matrix would ensure componentwise closed loop dynamics

_exi ¼ � ciexi; i ¼ 1; . . . ; n which for ci > 0 guarantees stability of ei ¼ 0, thus the stability of

equilibrium solution ex ¼ 0.

Example 8.1 Planar Manipulator Control – Mapping Desired Acceleration In this

example we will illustrate control of a multibody mechanical system in configuration space.

As an example we will use the planar elbow manipulator discussed in Chapter 1. The kinetic

energy matrix A qð Þ is expressed as in (1.34) and its elements aij; i; j ¼ 1; 2 are

a11 q2ð Þ ¼ m1l2m1 þ I1|fflfflfflfflfflffl{zfflfflfflfflfflffl}axis 1

þm2 l21 þ l2m2 þ 2l1lm2cos q2ð Þ� �þ I2|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}axis 2

a12 q2ð Þ ¼ a21 q2ð Þ ¼ m2 l2m2 þ 2l1lm2cos q2ð Þ� �þ I2

a22 ¼ m2l2m2 þ I2 ð8:35Þ

Figure 8.1 Transients in control of a planar elbow manipulator. The left column shows the configura-

tions q1 tð Þ; q2 tð Þ, the operational space positions x tð Þ; y tð Þ and the generalized errors sx tð Þ;sy tð Þ. Theright column shows the desired accelerations €xdes tð Þ;€ydes tð Þ, the configuration space input forces

t1 tð Þ; t2 tð Þ and the q2 q1ð Þ. The initial conditions are q1 0ð Þ ¼ 0:25 rad; q2 0ð Þ ¼ 0:5 rad. The observer

gain is g ¼ 800 and the reference operational space positions as in (8.47)



U ¼ m1glm1sin q1ð Þþm2g l1sin q1ð Þþ lm2sin q1 þ q2ð Þ½ � ð8:36Þ

Taking into account the work of the torques t1 and t2 developed by actuators, the dynamics

of the planar elbow manipulator yield

a11€q1 þ a12€q2 �m22l1lm2sin q2ð Þ _q2 _q1 �m22l1lm2sin q2ð Þ _q2 _q2 þ g1 q1ð Þ ¼ t1

a21€q1 þ a22€q2 �m22l1lm2sin q2ð Þ _q2 _q1 þ g2 q1; q2ð Þ ¼ t2

g1 q1; q2ð Þ ¼ m1glm1cos q1ð Þþm2g l1cos q1ð Þ� lm2cos q1 þ q2ð Þ½ �g2 q1; q2ð Þ ¼ m2glm2cos q1 þ q2ð Þ

ð8:37Þ

Figure 8.2 Transients in control of a planar elbow manipulator. The left column shows the

configurations q1 tð Þ; q2 tð Þ, the operational space positions x tð Þ; y tð Þ and the _ex exð Þ diagram.

The right column shows the desired operational space accelerations €xdes tð Þ; €ydes tð Þ, the configurationspace desired accelerations €qdes1 tð Þ;€qdes2 tð Þ and the y xð Þ diagram. The initial conditions are

q1 0ð Þ ¼ 0:25 rad, q2 0ð Þ ¼ 0:5 rad. The observer gain is g ¼ 800 and the reference operational space



In simulation, the same parameters as in Example 7.1 are used. The end effecter position in

orthogonal xTo ¼ x y½ � coordinates can be expressed as

x ¼ l1cos q1ð Þþ l2cos q1 þ q2ð Þy ¼ l1sin q1ð Þþ l2sin q1 þ q2ð Þ ð8:38Þ

Taking the derivative of (8.38) and rearranging the equations yields the Jacobian matrix

J ¼� l1sin q1ð Þ� l2sin q1 þ q2ð Þ � l2sin q1 þ q2ð Þl1cos q1ð Þþ l2cos q1 þ q2ð Þ l2cos q1 þ q2ð Þ

" #ð8:39Þ

The operational space acceleration can be expressed as

€xo ¼ J€qþ _J _q ð8:40Þ

Wewould like to illustrate the control where the end effecter position xTo ¼ x y½ � is forcedto track the smooth reference xrefo ¼ xref yref

� �T. Let the generalized control errors be

defined as

sx ¼ c1ex þ _ex; ex ¼ x� xref

sy ¼ c2ey þ _ey; ey ¼ y� yref

c1 ¼ c2 ¼ 50

ð8:41Þ

The desired acceleration can be calculated similarly as in (8.25)

€xdes ¼ €xref � d1sx

€ydes ¼ €yref � d2sy

d1 ¼ d2 ¼ 250

ð8:42Þ

Outside singularities, the desired configuration space acceleration is then determined as

in (8.31)

€qdes ¼ J� 1ð€xdeso � _J _qÞ ð8:43Þ

and the configuration space forces are expressed as in (8.32) s ¼ sdis þA€qdes.

In realization, the forces induced by the extra diagonal elements of the inertia matrix are

attributed to a generalized disturbance, thus the components of the operational space input

force vector can be determined as



a11n ¼ 4:5 kgm2; a22n ¼ 0:55 kg �m2

ð8:44Þ

The generalized configuration space disturbances are estimated using observers with

velocity and input forces as inputs


dz1



ð8:45Þ

dz2


t2dis ¼ z2 � ga22n _q2g ¼ 800

ð8:46Þ

Reference trajectories are selected as

xref ¼ 0:707þ 0:2 cos 3:14tð Þmyref ¼ 1:5þ 0:2 sin 6:28tð Þm ð8:47Þ

Transients for the initial conditions q1 0ð Þ ¼ 0:25 rad; q2 0ð Þ ¼ 0:5 rad are shown in

Figures 8.1 and 8.2. They illustrate the desired behavior of the closed loop. The tracking

in the operational space position is shown. The phase diagrams illustrate convergence.

In the next step the desired operational space acceleration or operational space force should

be selected to enforce tracking ez ¼ �Cex or enforcesx ¼ _ex þCex ¼ 0. This may be solved

in many different ways.

Let the Lyapunov function candidate be selected in the same form as for configuration space

control, thus having the following form

V ¼ sTxsx

2> 0; V 0ð Þ ¼ 0 ð8:48Þ

The selection of control to enforce _V ¼ �sTxY sxð Þ < 0 will guarantee the stability of

solution sx ¼ _ex þCex ¼ 0, thus ex ¼ 0. HereYT sxð Þ ¼ Y1 . . . Yn½ � 2 Rn�1 stands for

a vector function satisfying the condition sign Y sxð Þ½ � ¼ sign sxð Þ where sign xð Þ stands for acomponentwise sign .ð Þ function, being 1 for xi > 0 and � 1 for xi < 0, i ¼ 1; 2; . . . ; n; thusthe Lyapunov function derivative can be expressed as _V ¼ � Pn

1 Yij jsxisign sxið Þ. For

example, if Y sxð Þ is discontinuous, then finite-time convergence to equilibrium and sliding

mode motion may be enforced [6]. By selecting Y sxð Þ ¼ Dsx, where D > 0 is a positive

definite diagonal matrix, the exponential convergence is enforced. These results correspond to

the results obtained in Chapter 7, for configuration space control.

As shown in Chapter 7 the control can be selected from sTx _sx þY sxð Þ½ � ¼ 0. For sx 6¼ 0

the desired operational space acceleration can be determined as

€xdes ¼ €xeq �Y sxð Þ€xeq ¼ €xref �C _ex

ð8:49Þ

The desired acceleration consists of the equivalent acceleration €xeq and the convergence

acceleration €xcon ¼ �Y sxð Þ. The structure of (8.49) is the same as structure (7.14) obtained

for position tracking control in configuration space.

Insertion of (8.7) into sTx _sx þY sxð Þ½ � ¼ 0 for sx 6¼ 0 yields

fx ¼ lx q; _qð Þþ nx qð ÞþLx€xdes ¼ feqx �LxY sxð Þ

feqx ¼ lx q; _qð Þþ nx qð ÞþLx€xeq

ð8:50Þ


Here feqx stands for the operational space equivalent force. This show full consistency with

results obtained for position tracking in configuration space. The equivalent acceleration and

equivalent force can be estimated in the sameway as in configuration space with the necessary

changes of measured variables.

If Y sxð Þ ¼ Dsx is selected, (8.49) can be rearranged to be the same as (8.25) if appropriate

matrices are selected as:KxD ¼ CþD and KxP ¼ DC. The same is true for (8.50) and (8.26).

The closed loop transient is governed by

€ex þ CþDð Þ _ex þDCex ¼ 0 ð8:51Þ

The structure of the task control system is shown in Figure 8.3.

By inserting (8.50) into (8.7) the closed loop operational space dynamics can be expressed as

€x�€xref þC _ex þY sxð Þ ¼ _sx þY sxð Þ ¼ 0 ð8:52Þ

The convergence depends on the structure of Y sxð Þ. For finite time convergence one can

select, for example, Y sxð Þ ¼ Dsx þhsign sxð Þ < 0;D > 0 where h > 0 is a small positive

constant. Such control would provide finite time convergence and sliding mode motion in

C«x þ _«x ¼ 0;C > 0.

Example 8.2 2-dof PlanarManipulator Control –Operational ForceMapping In this

examplewewill illustrate the control of amultibodymechanical system in configuration space.

As an example, we will use the planar elbow manipulator discussed in Example 8.1. The

configuration space dynamics of the planar elbow manipulator are given in (8.37) with task

coordinates as in (8.38) and the task Jacobian as in (8.39). The parameters of the manipulator

are as in (8.47).

We would like to illustrate the configuration control where the end effecter position

xTo ¼ x y½ � is forced to track a given smooth reference xrefo ¼ xref yref� �T

. The generalized

control errors are selected in the following form



c1 ¼ c2 ¼ 50

ð8:53Þ

Figure 8.3 Structure of a control system for nonredundant systems


The desired operational space acceleration is calculated as



d1 ¼ d2 ¼ 250

ð8:54Þ

The operational space control forces are determined as in (8.51) f ¼ lþ nþL€xdeso with

components

fx ¼ fxdis þ l11€xdes

fy ¼ fydis þ l22€ydes

l11n ¼ 10 kg �m2; l22n ¼ 10 kg � m2

ð8:55Þ

Figure 8.4 Transients in a planar elbowmanipulator control. The left column shows the configurations

q1 tð Þ; q2 tð Þ, the operational space positions x tð Þ; y tð Þ and the generalized errors sx tð Þ;sy tð Þ. The rightcolumn shows the desired accelerations €xdes tð Þ;€ydes tð Þ, the configuration space input forces t1 tð Þ; t2 tð Þand the q2 q1ð Þ diagram. The initial conditions are q1 0ð Þ ¼ 0:25 rad, q2 0ð Þ ¼ 0:5 rad. The observer gain isg ¼ 800 and the reference operational space positions as in (8.58)


In determining control forces (8.55) the component induced by the product of the extra-

diagonal elements of matrix L ¼ JA� 1JT� �� 1

and acceleration €xdeso are attributed to the

generalized disturbance in the operational space fdis ¼ l q; _qð Þþ n qð Þþ L�Lnð Þ€xdeso . Matrix

Ln is diagonal with elements l11 and l22. The gains l11 ¼ 10 kg �m2 and l22 ¼ 10 kg � m2 are

taken in simulation.

The configuration space forces are determined as in (8.33)

s ¼JT f ¼ JTfx

fy

" #

s ¼� l1sin q1ð Þ� l2sin q1 þ q2ð Þ l1cos q1ð Þþ l2cos q1 þ q2ð Þ

� l2sin q1 þ q2ð Þ l2cos q1 þ q2ð Þ

" #fx

fy

" # ð8:56Þ

The components of the generalized disturbance in operational space are estimated using

observers with velocity and input forces as inputs

Figure 8.5 Transients in a planar elbow manipulator control. The left column shows the positions

q1 tð Þ; q2 tð Þ, the positions x tð Þ; y tð Þ and the diagram _ex exð Þ. The right column shows the desired

accelerations in operational space €xdes tð Þ;€ydes tð Þ, the desired accelerations in configuration

€qdes1 tð Þ;€qdes2 tð Þ and the y xð Þ diagram. The initial conditions are q1 0ð Þ ¼ 0:25 rad, q2 0ð Þ ¼ 0:5 rad. Theobserver gain is g ¼ 800 and the reference operational space positions as in (8.58)


dz1

dt¼ g fx þ gl11n _x� z1ð Þ;

f xdis ¼ z1 � gl11n _x

dz2

dt¼ g fy þ gl22n _y� z2

� �;

f ydis ¼ z2 � gl22n _y

g ¼ 800

ð8:57Þ

Reference trajectories are selected as

xref ¼ 0:707þ 0:2 cos 3:14tð Þmyref ¼ 1:5þ 0:2 sin 6:28tð Þm ð8:58Þ

Transients for the initial conditions q1 0ð Þ ¼ 0:25 rad; q2 0ð Þ ¼ 0:5 rad are shown in

Figure 8.4. In the left column position q1 tð Þ; q2 tð Þ, positions x tð Þ; y tð Þ and the generalized

errors sx tð Þ;sy tð Þ are shown. In the right column the desired accelerations €xdes tð Þ;€ydes tð Þ,the configuration space input forces t1 tð Þ; t2 tð Þ and the q2 q1ð Þ diagram are shown. Position

tracking in operational space is illustrated.

In Figure 8.5 in the left column position q1 tð Þ; q2 tð Þ, positions x tð Þ; y tð Þ and the diagram

_ex exð Þ are shown. In the right column the desired accelerations in operational space

€xdes tð Þ;€ydes tð Þ, the desired accelerations in configuration €qdes1 tð Þ;€qdes2 tð Þ and the y xð Þ diagramare shown.

8.2.2 Redundant Task Control

Control in systems with a redundant task, based on the dynamics (8.14), leaves uncontrolled

the internal configuration – posture – of the system. Because of this, in the control of systems

with a redundant task is more appropriate to use dynamics (8.21). Then, since they are

dynamically decoupled, the task and the posture can be controlled independently. Let the task

and posture tracking errors be defined as

ex x; xref� � ¼ x qð Þ� xref ; ex x; xref

� � 2 Rp�1

eP xP; xrefP

¼ xP qð Þ� x

refP ; eP xP; x

refP

2 R n� pð Þ�1

ð8:59Þ

Here xref and xrefP stand or the task and posture references. Then the dynamics of the task and

posture errors can be expressed as

€ex ¼ €x�€xref ¼ L� 1x fx � LxJA

� 1 bþ gð Þ�Lx_J _q

� �� €xref

€eP ¼ €xP �€xrefP ¼ L� 1P fP � LPGPA

� 1 bþ gð Þ�LP_GP _q

h in o�€xrefP

ð8:60Þ

Let the closed loop task and posture control error dynamics be selected as

€ex þKDX _ex þKPXex ¼ 0p�p

€eP þKDP _eP þKPPeP ¼ 0 n� pð Þ� n� pð Þ;ð8:61Þ


Here, KDX;KPX 2 Rp�p are design parameters that define the closed loop task error

dynamics, and KDP;KPP 2 R n� pð Þ� n� pð Þ stand for the design parameters in the posture

control loop. These parameters can be selected independently. Having selected closed loop

design parameters, the desired accelerations can be determined from (8.61) as

€xdes ¼ €xref � KxD _ex þKxPexð Þ€xdesP ¼ €xrefP � KDP _eP þKPPePð Þ

ð8:62Þ

The desired acceleration for task and posture control has the same structure. The task control

force realizing the desired acceleration (8.62) can be expressed as

fx ¼ LxJA� 1 bþ gð Þ�Lx

_J _q� �þLx€x

des ¼ fxdes þLx€xdes ð8:63Þ

The posture control force can be expressed as

fP ¼ LPGPA� 1 bþ gð Þ�LP

_GP _qh i

þLP€xdesP ¼ fPdes þLP€x

desP ð8:64Þ

The control forces, like the desired accelerations, show a symmetry in structure for both task

and posture control. The information needed for their implementation are the corresponding

disturbances and thematricesLx andLP, respectively. As shown, disturbance can be estimated

in configuration space and then the term bþ gð Þ in both (8.63) and (8.64) will be zero. Then thetask control forces could be expressed as

fx ¼ �Lx_J _qþLx€x

des ð8:65Þ

And the posture control force can be expressed as

fP ¼ �LP_GP _qþLP€x

desP ð8:66Þ

The configuration space force can be expressed as

s ¼ JT fx þGTPfP: ð8:67Þ

Inserting (8.65), (8.66) and (8.67) into (7.1) with compensated configuration space

disturbances A€q ¼ s results in the operational space desired acceleration

€qdes ¼ J# €xdes � _J _q� �þG#

P €xdesP � _GP _q

ð8:68Þ

Here J# ¼ A� 1JTLx stands for the task Jacobian pseudoinverse and G#P ¼ A� 1GT

PLP

stands for the posture Jacobian JPGð Þ pseudoinverse. The result shows full correspondence

with the constrained system control. The solution offers a way of combining the task and the

posture control or, instead of posture, controling another task. That situation will be discussed

in more detail in Chapter 9.


The structure of the control system in task space for redundant systems is shown

in Figure 8.6. The implementation of disturbance observers provides for the fxdis ¼LxJA

� 1 bþ gð Þ�Lx_J _q

� �and fPdis ¼ LPGPA

� 1 bþ gð Þ�LP_GP _q

h icomponents of the con-

trol forces. It shows the same structural blocks as constrained system control. The disturbance

compensation is assumed in configuration space since that offers the simplest way for

implementation. This structure raises the question of the task-constraint relationship in

multibody systems. That relationship is important in the control design of a multibody system

in contact with the environment or cooperative tasks.

The structure of the control in operational space has the same form as the enforcement of the

constraints discussed in Chapter 7. It shows equivalency of the soft constraints and the

redundant task control in multibody systems. Such similarity is very important for the motion

control of systems that may be constrained and at the same time need to realize a certain task.

The dynamics of constrained systems and tasks are described by equations that have the same

form and projection of the velocities and the forces are consistent in both cases; and this fact

opens the possibility of combining the constraints and tasks into an augmented description and

treating them within the same framework.

Example 8.3 Redundant Task Control In this example we will illustrate redundant task

control of a multibody mechanical system. As an example, a planar 3-dof manipulator

described in Example 7.2 will be used. As shown in Chapter 1, the dynamics of themanipulator

can be written in the form


The structure of the kinetic energy matrix A qð Þ 2 R3�3 and the force vectors

bT q; _qð Þ ¼ b1 q; _qð Þ b2 q; _qð Þ b3 q; _qð Þ½ � iand gT qð Þ ¼ g1 qð Þ g2 qð Þ g3 qð Þ½ � are given

in Example 7.2, Equations (7.65)–(7.68). The parameters are selected as

Figure 8.6 Structure of operational space control for task redundant systems


m1 ¼ 10 kg l1 ¼ 1 m lm1 ¼ 0:5 m

m2 ¼ 8 kg l2 ¼ 1 m lm2 ¼ 0:5 m

m3 ¼ 6 kg l3 ¼ 1 m lm3 ¼ 0:5 m

ð8:70Þ

We would like to illustrate the operational space position tracking control design with the

end effecter position in orthogonal xTe ¼ x y½ � coordinates expressed as

x ¼ l1cos q1ð Þþ l2cos q1 þ q2ð Þþ l3cos q1 þ q2 þ q3ð Þy ¼ l1sin q1ð Þþ l2sin q1 þ q2ð Þþ l3sin q1 þ q2 þ q3ð Þ ð8:71Þ

The task Jacobian can be calculated as

J ¼ j11 j12 j13

j21 j22 j23

" #j11 ¼ � l1sin q1ð Þ� l2sin q1 þ q2ð Þ� l3sin q1 þ q2 þ q3ð Þj12 ¼ � l2sin q1 þ q2ð Þ� l3sin q1 þ q2 þ q3ð Þj13 ¼ � l3sin q1 þ q2 þ q3ð Þj21 ¼ l1cos q1ð Þþ l2cos q1 þ q2ð Þþ l3cos q1 þ q2 þ q3ð Þj22 ¼ l2cos q1 þ q2ð Þþ l3cos q1 þ q2 þ q3ð Þj23 ¼ l3cos q1 þ q2 þ q3ð Þ

ð8:72Þ

For the operational control force design the operations space mass matrix

Lx ¼ JA� 1JT� �� 1

is needed. For its calculation the inverse of configuration space kinetic

energymatrix and the Jacobian and its transpose should be calculated.Asmentioned earlier, the

calculations below are shown in more details than needed for Matlab users. We had in mind

users that may want to use some other simulation environments.

. Inverse kinetic energy matrix A� 1

A� 1 ¼ai11 ai12 ai13ai21 ai22 ai23ai31 ai32 ai33

24

35

ai11 ¼ a22a33 � a32a23ð Þ=det Að Þ ai23 ¼ a12a21 � a23a11ð Þ=det Að Þai12 ¼ a13a32 � a33a12ð Þ=det Að Þ ai31 ¼ a21a32 � a31a22ð Þ=det Að Þai13 ¼ a12a23 � a22a13ð Þ=det Að Þ ai32 ¼ a12a31 � a32a11ð Þ=det Að Þai21 ¼ a22a31 � a33a21ð Þ=det Að Þ ai33 ¼ a11a22 � a12a21ð Þ=det Að Þai22 ¼ a11a33 � a31a13ð Þ=det Að Þdet Að Þ ¼ a11a22a33 þ a12a23a31 þ a21a32a13 � a13a22a31 � a21a12a33 � a32a23a11

. A� 1JT can be determined as

A� 1JT ¼aj11 aj12aj21 aj22aj31 aj32

24

35

aj11 ¼ ai11j11 þ ai12j12 þ ai13j13

aj12 ¼ ai11j21 þ ai12j22 þ ai13j23

aj21 ¼ ai21j11 þ ai22j12 þ ai23j13

aj22 ¼ ai21j21 þ ai22j22 þ ai23j23

aj31 ¼ ai31j11 þ ai32j12 þ ai33j13

aj32 ¼ ai31j21 þ ai32j22 þ ai33j23


. The operational space control distribution matrix L� 1x ¼ JA� 1JT

� �

L� 1x ¼ JA� 1JT

� � ¼ l11 l12

l21 l22

" # l11 ¼ j11aj11 þ j12aj21 þ j13aj31

l12 ¼ j11aj12 þ j12aj22 þ j13aj32

l21 ¼ j21aj11 þ j22aj21 þ j23aj31

l22 ¼ j21aj12 þ j22aj22 þ j23aj32

. The operational space mass matrix Lx ¼ JA� 1JT� �� 1

Lx ¼ JA� 1JT� �� 1 ¼ al al

al al

� �al11 ¼ l22=det L

� 1x

� �al12 ¼ � l21=det L

� 1x

� �al21 ¼ � l12=det L

� 1x

� �al22 ¼ l11=det L

� 1x

� �det L� 1

x

� � ¼ l11l22 � l21l12

The end effecter position xTo ¼ x y½ � is required to track smooth reference

xrefo ¼ xref yref� �T

. The same problem is discussed in Examples 8.1 and 8.2 for a nonre-

dundant manipulator. Here we can follow the same steps since projection of the configuration

space dynamics into operational space gives the dynamics

al11€xþ al12€yþ fxd ¼ fx

al21€xþ al22€yþ fyd ¼ fyð8:73Þ

Here fdx and fdy are components of projection of the configuration space disturbance into

operational space. The exact expressions for forces fdx and fdy are not important since these

forces can be estimated by a simple disturbance observer.

The operational space reference motion is

xref ¼ 1:8þ 0:25 sin 3:14tð Þ m

yref ¼ 1:6þ 0:25 sin 1:57tð Þ mð8:74Þ

The generalized control errors are selected in the following form



c1 ¼ c2 ¼ 25

ð8:75Þ

The desired operational space acceleration is calculated as



d1 ¼ d2 ¼ 375

ð8:76Þ

The operational space control forces are determined as in (8.63) f ¼ fdis þL€xdeso with

components

fx ¼ fxd þ al11€xdes þ al12€y

des

fy ¼ fyd þ al21€xdes þ al22€y

des ð8:77Þ


The corresponding configuration space forces are determined as in (8.33)

s ¼ JT f ¼ JTfx

fy

� �¼

t1t2t3

24

35 ¼

j11fx þ j21fy

j12fx þ j22fy

j13fx þ j23fy

24

35 ð8:78Þ

The operational space disturbances and the configuration space disturbances are estimated

by disturbance observers. The filter gain is g ¼ 600.

Transients in the operational space position control for initial conditions q1 0ð Þ ¼ 0:1 rad;q2 0ð Þ ¼ 0:25 rad; q3 0ð Þ ¼ 0:2 rad are shown in Figure 8.7. In the left column the configuration

space variables q1 tð Þ, q2 tð Þ, q3 tð Þ, the operations space position and the reference x tð Þ; xref tð Þare shown. In the right column the operations space position, the reference y tð Þ; yref tð Þ, thedesired accelerations €xdes tð Þ; €ydes tð Þ and the y xð Þ diagrams are shown.

Tracking in operational space is illustrated. The operational space dynamics depend on the

additional forces LxJA� 1GTs0 and only for selection of the projection matrix such that

JA� 1GTs0 ¼ 0 dynamical decoupling is obtained. In the example shown in Figure 8.7 the

additional force is assumed to be zero s0 ¼ 0.

In Figure 8.8 the same operational space tracking control as in Figure 8.7 is shown with

additional force GTs0. The aim is to illustrate the dependence of changes in the configuration

Figure 8.7 Control of a redundant 3-dof planar manipulator. The left column shows the configuration

space variables q1 tð Þ, q2 tð Þ, q3 tð Þ, the operations space position and references x tð Þ; xref tð Þ and

y tð Þ; yref tð Þ. The right column shows the operation space desired accelerations €xdes tð Þ;€ydes tð Þ, theoperational space forces fx tð Þ; fy tð Þ and the y xð Þ diagrams


space coordinates due to changes in this input. To see the changes due to the force GTs0 the

matrix GT ¼ I� JTJ#T� �

should be calculated first. In (8.13) the pseudoinverse is defined as

J#T ¼ LxJA� 1

. Due to the symmetry of kinetic energy matrixA the product JA� 1 ¼ A� 1JT� �T

thus above

result for matrix A� 1JT can be used. Here we will not use this identity just to illustrate all

derivations step by step. Then JA� 1 can be determined as

JA� 1 ¼ ja11 ja12 ja13

ja21 ja22 ja23

" #ja11 ¼ j11ai11 þ j12ai21 þ j13ai31

ja12 ¼ j11ai12 þ j12ai22 þ j13ai32

ja13 ¼ j11ai13 þ j12ai23 þ j13ai33

ja21 ¼ j21ai11 þ j22ai21 þ j23ai31

ja22 ¼ j21ai12 þ j22ai22 þ j23ai32

ja23 ¼ j21ai13 þ j22ai23 þ j23ai33

Figure 8.8 Control of a redundant 3-dof planar manipulator. The left column shows the configuration

space variables q1 tð Þ, q2 tð Þ, q3 tð Þ, the operations space position and references x tð Þ; xref tð Þ and

y tð Þ; yref tð Þ. The right column shows the operation space desired accelerations €xdes tð Þ;€ydes tð Þ, theconfiguration space forces due to vector s0 and the projection of force vector s0 as given in (8.80) into

the operational space


. The pseudoinverse J#T ¼ LxJA� 1 can be calculated as

J#T ¼ LxJA� 1 ¼ al11 al12

al21 al22

� �aj11 aj21 aj31

aj12 aj22 aj32

" #pJ11 ¼ al11ja11 þ al12ja21

pJ12 ¼ al11ja12 þ al12ja22





. The GT ¼ I� JTJ#T� �

can be then determined as

JTJ#T ¼ JTLxJA� 1 ¼

j11 j21

j12 j22

j13 j23

24

35 pJ11 pJ12 pJ13

pJ21 pJ22 pJ23

� �

gJ11 ¼ j11pJ11 þ j21pJ21 gJ21 ¼ j12pJ11 þ j22pJ21 gJ31 ¼ j13pJ11 þ j23pJ21



GT ¼ I� JTJ#T� � ¼ 1� gJ11 � gJ12 � gJ13

� gJ21 1� gJ22 � gJ23� gJ31 � gJ32 1� gJ33

24

35

Now the configuration space control force can be expressed as

s ¼ JT fþGTs0 ð8:79Þ

Transients illustrate the changes in configuration space coordinates while the control inputs

and the changes of operational space coordinates are the same. This confirms two points

discussed above: (i) partition of control input as in (8.12) ensures the dynamic decoupling and

(ii) task redundancy allows us to add new requirements on the operational space behavior and to

solve it independently of the operational task. These features will be used in establishing the

control of multiple tasks and the constraints in multibody mechanical systems.

In Figure 8.8 the transients in the configuration and operational spaces are shown. The task is

selected the same as in Figure 8.7. The input s0 is selected as

s0 ¼10 sin 2tð Þ

5

1

24

35 ð8:80Þ

In Figure 8.8 the configuration space variables q1 tð Þ, q2 tð Þ, q3 tð Þ, the operations space

position and references x tð Þ; xref tð Þ and y tð Þ; yref tð Þ are shown. In the right column the

operations space desired accelerations €xdes tð Þ;€ydes tð Þ, the configuration space forces due to

the vector s0 and the projection of the force vector s0 as given in (8.80) into the operational

space. The projection of the s0 into operational space ft0 ¼ s ¼ JA� 1GTs0 is small in

comparison with operational space force (less than 0.01%).


References

1. Paul, R. (1982) Robot Manipulators: Mathematics, Programming and Control, MIT Press, Cambridge, Mass.

2. Spong, M.W., Hutchinson, S., and Vidyasagar, M. (2006) Robot Modeling and Control, John Wiley & Sons, Inc.,

New York.

3. Khatib, O. (1987) A unified approach for motion and force control of robot manipulators: the operational space

formulation. IEEE Journal on Robotics and Automation, 3 (1), 43–53.

4. Sahin Conkur, E. and Buckingham, R. (1997) Clarifying the definition of redundancy as used in robotics.Robotica,

15, 583–586.

5. Siciliano,B. andKhatib,O. (eds) (2008)SpringerHandbookofRobotics, Springer-Verlag,NewYork, ISBN: 978-3-

540-23957-4.


Further Reading

Arnold, V.I. (1989) Mathematical Methods of Classical Mechanics, 2nd edn, Springer-Verlag, New York.

Asada, H. and Slotine, J.-J. (1986) Robot Analysis and Control, John Willey & Sons, Inc., New York.


Craig, J.J. and Raibert, M. (1979) A systematic method for hybrid position/force control of a manipulator. Proceedings

of 1979 IEEE Computer Software Applications Conference, Chicago.

de Sapio, V. and Khatib, O. (2005) Operational space control of multibody systems with explicit holonomic

constraints. Proceedings of the 2005 IEEE International Conference on Robotics and Automation, Barcelona,

Spain.

Fisher, W.D. and Mujtaba, M.S. (1991) Hybrid Position I Force Control: a Correct Formulation, Measurement and

Manufacturing Systems Laboratory, HPL-91-140.

Khatib, O., Sentis, L., Parkand, J., and Warren, J. (2004) Whole-body dynamics behavior and control of humal-like

robots. International Journal of Humanoid Robotics, 1(1), 29–43.

Kokotovic, P.V., O’Malley, R.B., and Sannuti, P. (1976) Singular perturbations and order reduction in control theory.


Nakao, M., Ohnishi, K. and Miyachi, K. (1987) A robust decentralized joint control based on interference estimation.

Proceedings of the IEEE Conference on Robotics and Automation, vol. 4, pp. 326–331.

Siciliano, B. (1990) Kinematic control of redundant robot manipulators: a tutorial. Journal of Intelligent and Robotic

Systems, 3, 201–212.


9

Interactions in Operational Space

9.1 Task–Constraint Relationship

Till now we have discussed ways of describing the dynamics of constrained systems and the

dynamics in operational space for nonredundant and redundant tasks. In addition, the selection

of control actions to maintain constraints or to control redundant or nonredundant tasks has

been shown. All of that gives the background for task control in constrained systems.

The configuration spacemotion for a constrained system is shown inEquation (7.47) and the

projection of unconstrained system (7.1) dynamics into the task space is described by (8.14). In

our previous analysis these two descriptions are treated separately. The idea we would like to

explore here is related to the possibility of merging constraints and an operational space task

into one augmented task vector. Such a vector will define a new operational space composed of

the constraints and task and will open a possibility to relate the components of that space in a

simpler and more direct way.

Without loss of generality, assume task vector xT qð Þ 2 RmT�1 and the constraint conditions

described by vectorf qð Þ 2 RmC�1 with hard constraintsf qð Þ ¼ 0. Let mT þmCð Þ < nwhere

n stands for the dimension of the configuration space of system (7.1). These task and constraints

vectors are assumed linearly independent. By concatenating xT qð Þ and f qð Þ vectors we candefine a newoperational space vector in a form similar tomerging the constraints and posture in

Chapter 7 or the task and posture description in Chapter 8. Let the new operational space vector

be defined as

x qð Þ ¼ xT qð Þf qð Þ

� �2 R mT þmCð Þ�1 ð9:1Þ

By differentiating Equation (9.1) the velocity vector can be written as

_x qð Þ ¼ _xT qð Þ_f qð Þ

� �¼

@xT qð Þ@q

@f qð Þ@q

24

35 _q ¼ JT

F

� �_q ¼ J _q ð9:2Þ



The task Jacobian JT 2 RmT�n and the constraint Jacobian F 2 RmC�n are by as-

sumption full row rank matrices. These matrices are selected in such a way that the

augmented operational space Jacobian J 2 R mT þmCð Þ�n is a full row rank matrix. Such a

selection of matrices JT and F allows us to determine the mT þmCð Þ < n component of

the configuration velocity vector from (9.2) as a function of the remaining n�mT �mCð Þcomponents of the configuration velocity vector and the operational space velocity

vector _x qð Þ.Since the dimension of the augmented operational space is lower than the dimension of the

system mT þmCð Þ < n the configuration space force can be expressed as

s ¼ JTT fT þFT fC þGTJ s0 ð9:3Þ

The meaning of the components is as follows: fT 2 RmT�1 stands for the operational space

task force component, fC 2 RmC�1 is a component of the operational space force acting in the

constraint direction and s0 2 Rn�1 is any vector in Rn. Matrix GJ is to be selected such that

GTJ s0 does not generate acceleration in operational space.

With such definitions of force vectors the configuration space force acting only in the

direction of constraints and not appearing in the task direction can be written as

sC ¼ JTT FT� � 0

k

� �ð9:4Þ

From (9.2), the operational space dynamics can be determined as

€x qð Þ ¼ €xT qð Þ€f qð Þ

� �¼ JT

F

� �€qþ

_JT_F

" #_q

¼ J€qþ _J _q

ð9:5Þ

By inserting (7.88) into (9.5) yields

€xT€f

� �¼ JT

F

� �A� 1 s� b� gþ JTT FT

� � 0

k

� �� þ

_JT_F

" #_q ð9:6Þ

Insertion of (9.3) into (9.6) after some algebra yields

€xT€f

� �¼ JTA

� 1JTT JTA� 1FT

FA� 1JTT FA� 1FT

" #fT

fC

� �þ 0

k

� �� þ JA� 1GT

J s0 �JTA

� 1

FA� 1

" #bþ gð Þþ

_JT_F

" #_q

ð9:7Þ

The control distribution matrix is not block diagonal, thus indicating the coupling of the

dynamics in the task and constraint directions. In addition to the acceleration coupling, other


coupling forces are involved in system (9.7). This is undesirable from the point of control

synthesis and it would be natural to find transformation such that at least some coupling forces

are eliminated.

Similarly as in the analysis of constrained systems and in the development of the redundant

task dynamics, let GJ be selected as an orthogonal complement GJ ¼ I� JJ# of the augmented

Jacobian J, having generalized inverse as

J# ¼ A� 1JT JA� 1JT� � 1 ð9:8Þ

Then

JA� 1GTJ s0 ¼ 0 ð9:9Þ

holds.

Matrices JTA� 1JTT

� 2 RmT�mT and FA� 1FT 2 RmC�mC are full rank matrices and

dynamics (9.7), by applying matrix inversion in block form, can be expressed in a more

compact form

L qð Þ€xþLJTA

� 1b� _JT _q

FA� 1b� _F _q

" #þL

JTA� 1g

FA� 1g

" #� 0

k

� �¼ fT

fC

� �

L qð Þ ¼ JTA� 1JTT JTA

� 1FT

FA� 1JTT FA� 1FT

" #� 1

¼ L11 qð Þ L12 qð ÞL21 qð Þ L22 qð Þ

� � ð9:10Þ

In (9.10), the right hand side is split into active forces – the controls for the task and the

constraints. The unknown force due to interactionwith the constraints is on the left hand side as

a part of the system disturbance. Taking into account the constraint conditions €f ¼ _f ¼ 0,

dynamics (9.10) can be rearranged into two subsystems: (i) one describes the task and (ii) the

other describes the constraint forces. The product L qð Þ€x will have only two components, both

depending on €xT since products L12 qð Þ €f ¼ 0 and L22 qð Þ €f ¼ 0.

Now the dynamics of a constrained system with task xT qð Þ 2 RmT�n can be decomposed

into a projection in the constrained directions and the task dynamics. These equations ofmotion

are as follows:

. Task dynamics

L11 qð Þ€xT þ lT q; _qð Þþ nT qð Þ ¼ fT

lT q; _qð Þ ¼ L11 JTA� 1b� _JT _q

� þL12 FA� 1b� _F _q �

nT qð Þ ¼ L11JTA� 1gþL12FA� 1g

ð9:11Þ

. Projection into constraint direction

L21 qð Þ€xT þ lF q; _qð Þþ nF qð Þ ¼ fC þ k

lF q; _qð Þ ¼ L21 JTA� 1b� _JT _q

� þL22 FA� 1b� _F _q �

nF qð Þ ¼ L21JTA� 1gþL22FA� 1g

ð9:12Þ

Note that this partition is valid for hard constraints €f ¼ _f ¼ f ¼ 0. For the soft constraints

discussed in Chapter 7 with closed loop control decomposition (9.11), then (9.12) will be valid

Interactions in Operational Space 339

only if the finite time convergence to the constraints is enforced by control and €f ¼ _f ¼ f ¼ 0

is valid for t � t0.

The Lagrange multiplier k stands for the constraint induced interaction forces. Dynam-

ics (9.12) are coupled with task acceleration and may be regarded as describing the balance of

the forces in the constrained direction. The acceleration related forces are due to the task

acceleration, thus making a constraint coupled control problem which depends on the task

dynamics and the projection of the configuration space disturbances into augmented opera-

tional space. The questions of constraint–task dynamical decoupling will be discussed later in

this chapter.

With such a partitioning of the system dynamics the task control designmay follow the same

procedure as applied in Chapters 7 and 8. The task force fT 2 RmT�1 can be selected using the

results presented in Chapter 8. For system (9.11) full dynamics compensation is obtained if the

desired acceleration is selected such that transient

€ex þKDX _ex þKPXex ¼ 0; ex ¼ xT � xrefT ð9:13Þ

is enforced, and consequently the desired acceleration is expressed as

€xdesT ¼ €xref � KDX _ex þKPXexð Þ ð9:14Þ

Here KDX; KPX are positive definite diagonal matrices of proper dimension. Task control

force fT and the corresponding component of the configuration space force can be expressed as

fT ¼ lT q; _qð Þþ nT qð Þ½ � þ L11€xdesT

¼ lT þ nTð Þþ L11 €xref � KDX _ex þKPXexð Þ� �sT ¼ JTT fT

ð9:15Þ

Here L11 stands for estimation of the task inertia matrix and lT ; nT stand for estimation of

the components of the disturbance in operational space. The error due to estimation of the

inertia matrix and the disturbances has been discussed in relation to the control of multibody

systems in configuration space. The same procedure and conclusions can be applied for

systems in operational space.

The component of the operational space force consistent with the constraints

fC 2 RmC�1 does not influence the task dynamics. It influences the constraint forces

k 2 RmC�1 and can be used to maintain them at the desired values. This allows the

constraint forces to be determined consistent with the task motion. The dynamic structure

allowing selection of a operational space force consistent with the constraint is governed

by the dynamics (9.12). By selecting fC 2 RmC�1 as in (9.16) one can control the

constraint force

fC þ k ¼ L21€xdesT þ lF q; _qð Þþ nF qð Þ½ � ð9:16Þ

The corresponding component of the configuration space force becomes sC ¼ FT fC. The

null space component allows the realization of additional motion requirements and it will

interfere neither with the constraints nor with the task.

Partition of the systemdynamics in the proposedway decouples the system from the point of

the control forces, but interconnection forces do appear as a result. By treating these forces as a


disturbance the effective acceleration controllers can be designed to deal with each problem

separately. The proposed solution has some deficiencies due to the complex coupling forces.

There is still a question as to whether some other projection may provide a more suitable

structure of the projected dynamics.

The structure of the control system is depicted in Figure 9.1. The null space component is not

defined fully and is just added to the structure for completeness.

The partition of the multibody system dynamics as in (9.11)–(9.12) shows that con-

straints and motion tasks consistent with constraints can be enforced concurrently. That

shows a possibility of enforcing motion and force concurrently by selecting an appropriate

partition of the configuration vector. From the analysis of the constraint dynamics discussed

in Chapter 7, a partition into an orthogonal complement of the constraint space appears to

be a way of dynamical decoupling. We will return to this problem later in the text to discuss

the possibility of decoupling the hierarchical control of multiple tasks and constraints at the

same time.

9.2 Force Control

Very simple tasks need only trajectory tracking control, while complex tasks require the

control of system interaction with the environment by either controlling the interaction

force or forcing the system to comply with the motion of the environment. Interaction with

the environment may occur at different parts of the multibody system. In this section, we

will consider the simplest problem when interaction occurs at the end effecter and is

localized to the point contact. The issues related to the control of constrained systems and

task control are already discussed in Chapters 7 and 8. Here we will expand on these results

to interaction force control. Since forces appear due to interactions during the execution of

tasks it is natural to base the design of force control on the operational space dynamics of a

multibody system.

As shown in Chapter 5, force and position tracking as separate problems may be solved

within the acceleration framework. As shown in Chapter 7, in a multibody system not all

Figure 9.1 Operational space task control in the presence of constraints


configuration space degrees of freedom are needed to enforce the interaction force and thus

force in certain directions andmotion consistent with constraints may be realized concurrently.

This means it is feasible to use some of the degrees of freedom to control motion and some of

the degrees of freedom of a multibody system to realize the interaction force. Since the

interaction forces appear in the operational space, both the position and the force errors are

defined in the operational space. That allows us to design a force/position controller directly in

the operational space, which is consistent with the result of the analysis in Section 9.1.

As shown in Chapters 7 and 8 a natural way to analyze motion in the presence of a

constraint is to project it into the constraint and unconstrained directions. Since force appears

as a result of an interaction between a specific point in a multibody system with constraints it

seems natural to take into account the task–constraint relationship when investigating the

force control.

The solution of combining the position tracking and force control has been proposed in a

well known hybrid control scheme [1]. The main idea is related to separating the motion and

force control errors in operational space, projecting these errors into configuration space and

applying known motion and force algorithms in configuration space. The operational space

consistent force control solution [2] assumes the design of an operational space control to

maintain the desired force and motion and then projecting them into configuration space.

Central for both schemes is the formulation of the selection matrix which partitions the

operational space into: (i) directions in which force is controlled and (ii) directions in which

motion is controlled.

In the hybrid control scheme [1] the end effecter Jacbian J 2 R6�6 is assumed to have

full rank. Errors in position and force are determined as ex ¼ xref � x� 2 R6�1 and

ef ¼ fref � f� 2 R6�1 respectively. The first step involves the selection of: (i) operational

space directions in which the position will be controlled exh ¼ Sxex and (ii) directions in which

the forceswill be controlled efh ¼ Sf ef . The selection is defined by a diagonalmatrix Sx 2 R6�6

with elements being either one for position control or zero for no position control directions, thus

the vector exh 2 R6�1 has m < 6 nonzero components. The selection matrix must be selected

consistent with the constraint surface in the contact point. The directions in which the force

should be controlled are determined as an orthogonal complement of Sx 2 R6�6. The elements

ofmatrix Sf ¼ I� Sxð Þ 2 R6�6 are one in the directionwhere force is controlled and zero in the

direction where force is not controlled. By applying an inverse Jacobian and transpose Jacobian

the corresponding errors in the configuration space eqx ¼ J� 1exh and eqf ¼ JTefh can be

determined and the position and force controllers are separately applied. The position and force

control algorithms are then applied in the configuration space.

In [3] a pseudoinverse matrix is used to transform the motion control error eqx ¼ SxJð Þ#exhand the force control error is transformed by a transpose Jacobian eqf ¼ JTSf efh, as in the

original hybrid scheme. That solution gives some improvements in comparison with the

original hybrid control but still has problems of applying the transformation on the control

errors instead of on the control forces. A problem arises from incorrect mapping and

incomplete definition of the task involving position and force decoupling. Instead of a

projection of the control errors into the configuration space it seems more coherent to design

a position/force controller directly in the operational space and then transform the control

(acceleration or forces) into the configuration space.

As shown inChapter 8, the operational space dynamics can bewritten by a simple projection

of the configuration space dynamics (7.1) into the operational space. By defining the Jacobian


matrix related to the end effecter frameOe as J 2 R6�6 the projection of the dynamics (7.1) can

bewritten as a set of differential equations similar to (8.14)with an additional term representing

the interaction force – thus we can write

L€xþ l q; _qð Þþ n qð Þþ f int ¼ f

L ¼ JA� 1JT� � 1

l q; _qð Þ ¼ J#Tb q; _qð Þ�L _J _q

n qð Þ ¼ J#Tg qð ÞJ#T ¼ LJA� 1

ð9:17Þ

Here f int stands for an operational space interaction force and f is the operational space

control input. Application of the operational space acceleration control yields the desired

acceleration in a position tracking €xdes ¼ €xref � KD _eþKPeð Þ, e ¼ x ¼ xref and the corre-

sponding control force

f ¼ l q; _qð Þþ n qð Þþ fint þL€xdes ð9:18Þ

Here €xdes stands for the desired operational space acceleration. In realization, disturbancescan be compensated in configuration space. Velocity induced acceleration L _J _q may be also

compensated in the configuration space if the term s _q ¼ JTL _J _q is added to the configuration

space input.

Control (9.18) effectively linearizes system motion by rejecting disturbances up to the

accuracy of the estimation. A closed loop is then reestablished by functional dependencies

defined by selection of the desired acceleration €xdes.Force control or position tracking can be treated just by establishing a functional depen-

dence of the desired acceleration €xdes from the force or position control error. Position tracking

has been already discussed inChapter 8 and in Section 9.1. Here, wewould like to address force

control and then the integration of position and force control.

As shown in Chapter 7, constraints onmotion due to interactionwith the environment define

the unconstrained directions in which the end effector can freely move. The motion degrees of

freedom and the direction of the total interaction force lie in mutually orthogonal subspaces.

That – knowing force direction – allows us to determine the constraint consistent motion

direction and consequently, a specification of a convenient reference frame in which the task

and the force control specification is natural. From analysis of constrained systems in Chapter 7

we know that the frame concerned must be consistent with constraints (Figure 9.2).

Let the linear spring be modeling an interaction force

f int ¼ Kaxf ð9:19Þ

HereKa stands for the stiffness of the environment in the contact point and xf stands for the

components of the task vector that influence the interaction force. The second derivative of the

interaction force can be expressed as €f int ¼ Ka€xf . For reference of the contact force frefint being

selected as a smooth vector valued function with continuous first- and second-order time

derivatives, the dynamics of the error in the force control loop ef ¼ f int � frefint can be expressed as

€ef ¼ Ka€xf �€fref

int ð9:20Þ


By selecting the desired acceleration as

€xdesf ¼ K� 1a €f refint � KDF _ef þKPFef

� h ið9:21Þ

the transient in the force control loop, under the assumption that €f refint and Ka are known, can

be determined as

€ef þKDF _ef þKPFef ¼ 0 ð9:22Þ

Transient (9.22) has the same form as the one for position control and is determined by the

design parameters KDF;KPF . The dependence of closed loop control on the environmental

stiffness is the main difficulty in the application of this solution and it requires very careful

design.

The enforcement of constraintswith a selected set of configurationvariables has been shown

in Chapter 7. That establishes grounds for decomposing the operational space degrees of

freedom into: (i) those that generate motion in a constrained direction (and thus can influence

interaction force) and (ii) those that generate motion in an unconstrained direction. Knowing

the direction of the force or the constraints on motion in the operational space allows us to

decompose the task vector into the components determining the interaction force and the

components determining the unconstrained motion.

In [2] decomposition of the task vector is defined by selecting a frame of reference Of with

one axis oriented along the desired force vector specified in the end effecter frame of reference

Oe. If the motion is restricted by geometric constraints in some direction, one of the axes of the

frameOf needs to be aligned with this direction. Nowwe are dealing with two different frames

needed to specify the task (position and orientation): (i) the frame of reference Of consistent

with the constraints in which motion and forces are naturally defined and (ii) the frame of

reference of the end effecter Oe.

Within the constraint consistent frame of referenceOf there is a straightforward partition

of: (i) the directions in whichmotion is constrained – and thus forcesmust be controlled – and

(ii) the directions in which motion is not constrained – and thus motion tracking can be

applied. Let, for nonredundant 6 dof system, the diagonal matrix specifying the motion

directions (positions and orientations) be Sx 2 R6�6 with diagonal elements having binary

Figure 9.2 Selection of the compliant frame


values one or zero in a similar way as adopted in the hybrid control scheme. The nonzero

value means full freedom of motion and the zero value means constraints in that direction.

The directions of force control are then described by an orthogonal complement matrix

Sf ¼ I� Sxð Þ 2 R6�6 where I6�6 is the identity matrix.

LetT 2 R6�6 be thematrix describing the transformation of a generalized task (position and

orientation) from the end effecter frame of reference to the constraint consistent frame of

referenceT : Oe !Of . Now the partition of the operational space variables consistent with the

constraint can be represented by generalized specification matrices:

Tx ¼ TTSxT

Tf ¼ TT I� Sxð ÞT ð9:23Þ

Here Tx specifies the operational space variables consistent with constraints. (T stands for

the transformation of the operational space variables into a constraint defined frame of

reference, Sx stands for the selection matrix and TT stands for the transformation from a

constraint consistent frame of reference Of into the operational space.) The desired acceler-

ation in the motion control loop and in the force control loop can be selected as

€xdesx ¼ €xdesx � KDX _ex þKPXexð Þ� �€xdesf ¼ K� 1

a €f refint � KDF _ef þKPFef� h i ð9:24Þ

The operational space desired acceleration can be expressed as

€xdes ¼ Tx€xdesx þTf€x

desf ð9:25Þ

The operational space force and its projection to configuration space is

f ¼ lþ nþ f intð ÞþL€xdes

s ¼ JT fð9:26Þ

The presentedmethod assumes a description of the force andmotion control tasks in the end

effecter operational space. The decomposition of the operational space coordinates into

position controlled and force controlled lies in the constraint consistent frame of reference;

and the projection matrices between the end effecter frame of reference and the constraint

consistent frame of reference are used to make decisions simpler and more natural. The

structure of the force controller depends on the perceivedmodel of the interaction in the contact

point and in that sense the solution shown in this section is just a particular case.

9.3 Impedance Control

If the force in contact with the environment is controlled, the multibody system in the contact

point appears to act as a pure force source. This section will discuss the reaction of a multibody

system on an external force action.

The configuration space dynamical model for a n-dof multibody system can be written as

A qð Þ€qþ b q; _qð Þþ g qð Þþ JT f int ¼ s ð9:27Þ


Here q 2 Rn�1 stands for the configuration vector, f int 2 Rm�1 stands for the operational

space external force and JT 2 Rn�m is the transpose of a Jacobianmatrix corresponding to task

x qð Þ 2 Rm�1. The acceleration in operational space can be expressed as €x ¼ J€qþ _J _q. Theoperational space dynamics are defined as in (9.17). Application of the acceleration control in

operational space leads to selection of the control force f 2 Rm�1 as in (9.18).

Let the desired reaction of the system on external force f int 2 Rm�1 in operational space be

specified as amass–spring–damper systemwith parametersM;KP and; KD. HereM 2 Rm�m

stands for the desired operational space inertia matrix, KD 2 Rm�m stands for the desired

operational space dissipation matrix andKP 2 Rm�m stands for the desired operational space

stiffness matrix. Let the operational space reference trajectory be defined as €xref and let the

operational space desired acceleration be selected as

€xdes ¼ €xref �M� 1 KD _ex þKPex � f intð Þex ¼ x� xref

ð9:28Þ

By rearranging (9.28) the operational space reaction of the closed loop system on the

external force input f int may be written as

M€ex þKD _ex þKPex ¼ f int ð9:29Þ

The closed loop operational space reaction on the external force is fully specified by

controller parameters M; KD; KP. This describes a mass–spring–damper system acting

against external force f int. From (9.29) it follows that the system will track the reference if

the external force is zero and will change the trajectory tracking desired acceleration

€xdestr ¼ €xref �M� 1 KD _ex þKPexð Þ for an amount proportional to the external force

€xdes ¼ €xdestr þM� 1f int ð9:30Þ

The proportionality is set as the inverse of the desired operational space inertia matrix. The

deviation of the motion from the reference trajectory depends on the interaction force. The

force appearing in contact with an unknown environment will depend not only on the selected

parameters but also on the position and velocity of the environment and the reference trajectory.

Note that this result is the same as the one obtained for the 1-dof systems discussed inChapter 5.

The operational space force realizing the desired acceleration (9.30) can be expressed as

in (9.18).

The configuration space force can be determined by mapping the operational space control

input into the configuration space as s ¼ JT fþGTs0, where G is a null space projection matrix

and s0 stands for any vector Rn. Insertion of s ¼ JT fþGTs0 into configuration space

dynamics (9.27) yields configuration space acceleration as

€q ¼ A� 1JTL €xdes � _J _q� þA� 1GT s0 � bþ gð Þ½ � ð9:31Þ

The configuration space acceleration is equal to a projection of the task space acceleration

€xdes and the velocity induced acceleration _J _q by weighted right pseudoinverse J# ¼ A� 1JTL,L ¼ JA� 1JT

� � 1and uncompensated disturbance. Compensation of disturbance in the

configuration space results in the impedance control system, as depicted in Figure 9.3.


The null space input is not specified and, as shown in Chapter 8, it can be assigned to realize

another task-consistent motion.

This solution ensures trajectory tracking in free motion and the desired reaction on the

external force acting in the selected operational space. Structurally, it is the same as operational

space position control with the addition of interaction force input and the selection of controller

parameters to satisfy the impedance requirement instead of the trajectory tracking specifica-

tion. That may result in a poor performance in trajectory tracking tasks. This could be avoided

by applying an impedance control structure similar to the one discussed for a constrained

systemwith requirements to control impedance (or force) in the constraint direction andmotion

in the tangential space. The result is consistent with analysis of the 1-dof systems in Chapter 5

with a modification of the trajectory due to interaction with the environment.

The configuration space acceleration control can be realized if the desired acceleration is

expressed as €qdes ¼ A� 1JTL €xdes � _J _q� þA� 1GTs0. This shows consistency of the selected

design approach.

9.4 Hierarchy of Tasks

In this section we will return to a discussion of structural relations in systems with redundant

tasks. As shown in the analysis of force control, the selection of a frame of reference in which a

task is formulated greatly influences the selection of a control. In this section the problem of

setting a hierarchy of tasks inmultibody systemswill be discussed in some detail. Our intention

is to show that the techniques developed so far can be successfully applied to multibody

systems with multiple tasks and constraints.

9.4.1 Constraints in Operational Space

Let us first take a look at the problem of enforcing some functional relations (soft constraints)

among operational space coordinates. Assume a configuration space dynamical model for a n-

dof multibody system as in (9.27), a task defined by x qð Þ 2 Rm�1 and an operational space

Jacobian matrix Jx 2 Rm�n; m < n. Let us select an input force such that the end effecter is

constrained to a smooth surface defined by

f x qð Þ½ � ¼ fref tð Þ; f 2 Rp�1; p < m < n ð9:32Þ

Figure 9.3 Structure of the impedance control system


Thefref tð Þ stands for the time-dependent sufficiently smooth reference. A relation between

acceleration in operational space and acceleration in a constrained direction can be expressed

by twice differentiating (9.32)

€f x qð Þ½ � ¼ Jf€xþ _Jf _x

Jf ¼�@f

@x

�2 Rp�m ð9:33Þ

Here Jf 2 Rp�m stands for a constraint Jacobian in operational space for constraint (9.32).

The operational space dynamics can be written as Lx€xþ lx þ nx ¼ fx. The relationship

between operational space force fxf and the forces in the constrained direction can be

expressed as fxf ¼ JTfff.

Now a projection of the operational space dynamics in the constrained direction can be

written as

Lf€fþ lf q; _qð Þþ nf qð Þ ¼ ff

Lf ¼ JfL� 1x JTf

�� 1

¼ Jf JxA� 1JTx

� � 1JTf

h i� 1

lf q; _qð Þ ¼ Lf JfL� 1x lx � _Jf _x

� nf qð Þ ¼ LfJfL

� 1x nx

fxf ¼ JTfff

ð9:34Þ

Dynamics (9.34) have the same structure as operational space dynamics and show full

consistency with the dynamics of other problems in multibody systems discussed so far. The

structure of the matrix Lf¼ JfL� 1x JTfð Þ� 1

illustrates the result of two consecutive transformations

from configuration space – a projection into operational space and then a projection into the

constrained direction.

The simplest solution enforcing stability of (9.32) is the selection of the desired acceleration

and corresponding control force ff acting in the constrained direction as

€fdes ¼ €f

ref � KDF _ef þKPFef�

ff ¼ ðlf þ nfÞþLf€fdes

ef ¼ f�fref

ð9:35Þ

Here €fdes

stands for the desired acceleration in the constrained direction, ef ¼ f�fref

stands for the constraint tracking error, €fref

stands for the reference acceleration and

KDF;KPF > 0 are positive definite matrices.

As shown in Chapter 7 the decoupling of the remainingm� p degrees of freedom requires

projection into an orthogonal complement subspace. This can be obtained by projecting

the operational space velocity into an unconstrained direction _z ¼ Jzf _x ¼ JzGxf _x with

Jz 2 R m� pð Þ�m as a full row rank matrix and Gxf 2 Rm�m as a null space projection

matrix Gxf ¼ I� J#fJf associated with the constraint Jacobian in the operational space.

J#w ¼ L� 1x JTf

�JfL

� 1x JTf

� 1stands for a dynamically consistent pseudoinverse. The dynamics

€z ¼ Jzf€xþ _Jzf _x can be expressed as


Lzf€zþ lzf q; _qð Þþ nzf qð Þ ¼ fzf

Lzf ¼ JzfL� 1x JTzf

�� 1

¼ Jzf JxA� 1JTx

� � 1JTzf

h i� 1

lzf q; _qð Þ ¼ Lzf JzfL� 1x lx � _Jzf _x

� nzf qð Þ ¼ LzfJzfL

� 1x nx

fxz ¼ JTzffzf

ð9:36Þ

Tracking of reference zref can be enforced by selecting the desired acceleration and

corresponding force as

€zdes ¼ €zref � KDZ _ez þKPZezð Þfzf ¼ lzf þ nzf

�þLzf€z

des

ez ¼ z� zref

ð9:37Þ

with tracking error ez ¼ z� zref and positive definite matrices KDZ ; KPZ > 0.

Inserting the operational space force fx ¼ JTfff þ JTzffzf along with (9.35) and (9.36) into

Lx€xþ lx q; _qð Þþ nx qð Þ ¼ fx yields the operational space acceleration

€x ¼ J#f €fdes � _Jf _x

�þ J#zf €zdes � _Jzf _x

� ð9:38Þ

where matrices J#f and J#zf can be expressed as

J#f ¼ L� 1x JTf JfL

� 1x JTf

�� 1

J#zf ¼ L� 1x JTzf JzfL

� 1x JTzf

�� 1ð9:39Þ

This result is equivalent to (8.68) obtained for concurrent task and posture control in

operational space and in Equation (9.25) for force control in task space. If the desired

acceleration €fdes

is selected as a control in the constrained direction force tracking loop,

then (9.39) realizes concurrent force control and motion control in operational space. The

enforcement of constraints in task space establishes an algebraic relation between a certain

number of operational space coordinates and thus limits the set of motions that can be realized

by the system. The established relationship is similar to the relationship used to decompose the

degrees of freedom in the interaction force control (Section 9.2). There the dynamics of the

system in the constraint consistent frame of reference are not derived since the controller is

designed in the end effecter operational space. The correspondence between these two

solutions points out the consistency in the analysis of the multibody systems.

Till now we have found that dynamical decoupling of the constraints and task leads to a

projection of the task into a constraint orthogonal complement space with a dynamically

consistent generalized inverse. The relationships shown for configuration space transforma-

tions are also preserved in operational space.

If Equation (9.32) describes hard constraints with fref tð Þ � 0, the operational space

dynamics are described as in Equation (9.17) with f int standing for the interaction force due

to the constraints. This force can be expressed as f int ¼ JTfl where l stands for the Lagrange

multiplier. These multipliers can be determined using the same idea as applied in Chapter 7.


Example 9.1 Task Control with Constraint Enforcement This example illustrates the

enforcement of configuration space constraints concurrently with a task. It is assumed that the

task has priority and thus the constraints should be executed in the operational space orthogonal

complement space. For simulations the 3-dof system shown in Example 8.3 will be used. The

configuration space model is given by


The structure of the kinetic energy matrix A qð Þ 2 R3�3 and the force vectors

bT q; _qð Þ ¼ b1 b2 b3½ � and gT qð Þ ¼ g1 g2 g3½ � are given in Example 7.2, Equations

(7.65)–(7.68). The parameters are selected as in Equation (7.69).

The end effecter position in orthogonal xTe ¼ x y½ � coordinates can be expressed as

x ¼ l1 cos q1ð Þþ l2 cos q1 þ q2ð Þþ l3 cos q1 þ q2 þ q3ð Þy ¼ l1 sin q1ð Þþ l2 sin q1 þ q2ð Þþ l3 sin q1 þ q2 þ q3ð Þ ð9:41Þ

The operational space dynamics are given by

L€xþ l q; _qð Þþ n qð Þ ¼ f

L ¼ JA� 1JT� � 1

l q; _qð Þ ¼ J#Tb q; _qð Þ�L _J _q

n qð Þ ¼ J#Tg qð ÞJ#T ¼ LJA� 1

ð9:42Þ

The Jacobian matrix is given as in Equation (8.72) and the operational space control

distribution matrix and its inverse are L� 1x ¼ JA� 1JT

� , Lx ¼ JA� 1JT

� � 1, respectively.

The structure of these matrices is shown in Example 8.3.

In Example 8.3, task control is illustrated. For a given reference xrefo ¼ xref yref� �T

as in

(8.74) the generalized operational space control errors are defined as in (8.75) and the desired

accelerations and operational space control forces are selected as in (8.76) and (8.77),

respectively. The configuration space forces are then determined as in (8.78).

In addition to the task control, let the configuration space constraint be defined by

f ¼ q1 þaq2 þ 0q3 ¼ fref

fref ¼ 0:1þ 0:125 sin 6:28tð Þ ð9:43Þ


F ¼ 1 a 0½ � ð9:44Þ

Projection of the constraints into the task Jacobian null space yields

LF€fþm q; _qð Þþ r qð Þ ¼ fF

LF ¼ FGð ÞA� 1 FGð ÞT� �� 1

m q; _qð Þ ¼ LF FGð ÞA� 1b� _F _G �

_qh i

r qð Þ ¼ LF FGð ÞA� 1g

ð9:45Þ


Let the desired acceleration in the constrained direction be selected as in Example 7.2

€qdesF ¼ €fref � dF cFfþ _f

� cF ¼ 10; dF ¼ 125

ð9:46Þ

The selection of the parameters is not for best performance but rather to have diagrams that

show the salient features of the system behavior. In this particular example both parameters

cF and dF can be selected larger. Then a control force in the constrained direction can be

expressed asfF ¼ fFdis þLF€q

desF ð9:47Þ

The disturbance in the constrained direction fFdis ¼ mþ r can be estimated by a simple

reduced order observer, as shown in Example 7.2, Equation (7.76).

The null space projection matrix GT ¼ I� JTJ#T�

can be calculated as in Example 8.3 by

calculating the pseudoinverse matrix J#T ¼ LxJA� 1.

Now the configuration space control force can be expressed as

s ¼ JT fþ FGð ÞT fF ð9:48ÞTransients in the operational space position control for initial conditions

q1 0ð Þ ¼ 0:1 rad; q2 0ð Þ ¼ 0:25 rad; q3 0ð Þ ¼ 0:2 rad and reference operational space position

xref ¼ 1:8þ 0:25 sin 3:14tð Þm; yref ¼ 1:6þ 0:25 sin 1:57tð Þm are shown in Figure 9.4. The

Figure 9.4 Control of redundant 3-dof planar manipulator. Left column: configuration space variables

q1 tð Þ,q2 tð Þ,q3 tð Þ, operations spaceposition, referencex tð Þ; xref tð Þ and y tð Þ; yref tð Þ.Right column: constraint

and its tracking error f tð Þ; ef ¼ f�fref tð Þ, operations space desired accelerations€xdes tð Þ; €ydes tð Þ and endeffecter motion y xð Þ. The constraint equation is f ¼ q1 � 2q2 þ 0q3 ¼ 0:1þ 0:125 sin 6:28tð Þ


left column shows the configuration space variables q1 tð Þ, q2 tð Þ, q3 tð Þ, the operations spaceposition and references x tð Þ; xref tð Þ and y tð Þ; yref tð Þ. The right column shows the constraint and

its tracking error f tð Þ; ef ¼ f�fref tð Þ, the operation space desired accelerations

€xdes tð Þ;€ydes tð Þ and the end effecter motion y xð Þ. The reference constraint is given by

fref tð Þ ¼ 0:1þ 0:125 sin 6:28tð Þ.

9.4.2 Enforcing the Hierarchy of Tasks

So far we have been analyzing dynamics and control issues for multibody systems

subjected to constraints and a single task. The solution has been found in selecting the

primary goal (enforcement of the constraints) and then solving a secondary goal by using

a projection into an orthogonal complement space of the primary goal. So far the solutions

for selecting either constraints or task as the primary goal has been shown using either

acceleration or forces as the control input. It is shown that the key for the solution is the

selection of a dynamically consistent weighted pseudoinverse. For the configuration space

kinetic energy matrix A qð Þ 2 Rn�n and the primary goal related Jacobian matrix

JP 2 Rm�n; m < n the dynamic decoupling is achieved if the weighted pseudoinverse

is selected as

J#P ¼ A� 1JTP JPA� 1JTP

� � 1 ð9:49Þ

and the null space projection matrix is

GP ¼ I� J#PJP ð9:50Þ

The dynamics and control of the secondary goal has been solved in the primary goal null

space using a projection matrix

JSP ¼ JSGP 2 R n�mð Þ�n ð9:51Þ

where JS 2 R n�mð Þ�n is the Jacobianmatrix associated with the secondary goal. Subscript ‘SP’

is used to denote a secondary goal dynamically consistent with the primary goal. The desired

acceleration in the configuration space is determined as

€q ¼ J#P €xdesP � _JP _q� þ J#SP €xdesS � _JSP _q

� ð9:52Þ

Here the desired accelerations €xdesP and €xdesS are determined as

€xdesP ¼€xrefP � KDP _eP þKPPePð Þ; eP ¼ xrefP � xP

€xdesSP ¼€xrefSP � KDSP _eSP þKPSPeSPð Þ; eSP ¼ xrefSP � xSP

ð9:53Þ

The operational space forces associated with the primary and secondary goals are

determined as


fP ¼ fPdis þLP€xdesP ; LP ¼ JPA

� 1JTP� � 1

fSP ¼ fSPdis þLSP€xdesSP ; LSP ¼ JSPA

� 1JTSP� � 1 ð9:54Þ

where fPdis and fSPdis are disturbances associated with the primary and secondary goals.

The configuration space forces are determined as

s ¼ JTPfP þ JTSPfS ð9:55Þ

As shown, these solutions are applicable in the configuration space as well in the operational

space. The selection of the primary goal – the control task that has higher priority – is a guiding

point in the overall control design. After selecting it, the selection of the pseudoinverse as in

Equation (9.49) defines all other transformations.This allows a decoupled design of the control –

acceleration or force – and consistent mapping into configuration space.

Let us now address issues in the analysis and control of multibody systems with constraints

and multiple tasks. For the purpose of showing the design procedure, let us have the same

assumptions as used in the previous analysis

. A n-dof multibody system is required to maintain functional constraints while fulfilling

selected tasks.. Constraint is defined by function f qð Þ ¼ 0 2 Rmc�1 with constraint Jacobian F 2 Rmc�n.. One of the tasks is defined by x qð Þ 2 Rmx�1 with task Jacobian Jx 2 Rmx�n.. A second task is defined by y qð Þ 2 Rmy�1 with task Jacobian Jy 2 Rmy�n.. The priority of task x qð Þ is higher than the priority of task y qð Þ.. All matricesF 2 Rmc�n, Jx 2 Rmx�n and Jy 2 Rmy�n are assumed to have full row rank and

thus the constraints and tasks are linearly independent.. Without loss of generality, the allocation of available configuration space degrees of freedom

is such that the constraints and tasks can be implemented concurrently and no free degrees of

freedom are left, thus mc þmx þmy ¼ n.

With these operational requirements and the application of the so far discussed approach we

can write the constraint–task velocity mapping in the following form

_f_x_y

24

35 ¼

F��J1��J2

2664

3775 _q; h ¼

fx

y

24

35; J ¼

F��J1��J2

2664

3775 ð9:56Þ

Matrices J1 2 Rmx�n and J2 2 Rmy�n are assumed to have full row rank and should be

determined as a function of constraint and task Jacobian matrices in such a way that the

dynamics of constraints and tasks are decoupled. By assumption, the constraint–tasks Jacobian

J 2 Rn�n has full rank det Jð Þ 6¼ 0. Formally constraint and task attributed velocities and

accelerations can be expressed as

_h ¼ J _q

€h ¼ J€qþ _J _q;ð9:57Þ


Configuration space force is

s ¼FT fF þ JT1 fx þ JT2 fy ¼ JT f

JT ¼ �FT ..

.JT1

..

.JT2�

fT ¼ fF fx fy½ �

ð9:58Þ

Here fF 2 Rmc�1; fx 2 Rmx�1; fy 2 Rmy�1 are control forces associated with the con-

straint and operational spaces.

Let the interaction forces in the constraint and operational spaces be fFc; fxc and fyc,respectively. The configuration space dynamics are then described by

A€qþ b q; _qð Þþ g qð Þþ JT fc ¼ s

JT fc ¼ FT fFc þ JT1 fxc þ JT2 fycð9:59Þ

By inserting (9.58) and (9.59) into (9.57) formally, the constraint–operational space

dynamics can be expressed as

L€h þ l q; _qð Þþ n qð Þ ¼ f� fc

hT ¼ f x y½ �fT ¼ fF fx fy½ �fTc ¼ fFc fxc fyc½ �L ¼ JA� 1JT

� � 1

l q; _qð Þ ¼ LJA� 1b�L _J _q

n qð Þ ¼ LJA� 1g qð Þ

ð9:60Þ

The dynamical couplings are present in all three terms in the inertia matrix L and coupling

forces l q; _qð Þ; n qð Þwhile the constraint- and task-associated forces are decoupled. In controldesign, we would like to establish a first dynamical decoupling at least for the acceleration

induced forces and then apply acceleration control.

In order to find the decoupling conditions let us first analyze the constraint–operational

space control distribution matrix

L� 1 ¼

F��J1��J2

26664

37775A� 1 FT ..

.JT1...JT2

� �26664

37775 ¼

FA� 1FT FA� 1JT1 FA� 1JT2

J1A� 1FT J1A

� 1JT1 J1A� 1JT2

J2A� 1FT J2A

� 1JT1 J2A� 1JT2

264

375 ð9:61Þ

Matrix L� 1 shows the dynamical coupling of the acceleration terms. In order to have

dynamically decoupled acceleration terms, the extradiagonal elements Lijði 6¼ jÞ of controldistribution matrix must be zero. That gives a set of matrix equations to be solved. If the

following requirements are met, then (9.61) will be reduced into a block diagonal form

FA� 1JT1 ¼ 0mc�mx

FA� 1JT2 ¼ 0mc�my

J1A� 1FT ¼ 0mx�mc

J1A� 1JT2 ¼ 0mx�my

J2A� 1FT ¼ 0my�mc

J2A� 1JT1 ¼ 0my�mx

ð9:62Þ


These conditionswill ensure the constraints and tasks are dynamically decoupled. Recalling

the structure of the weighted pseudoinverse J# ¼ A� 1JT JA� 1JT� � 1

and its orthogonal

complementG ¼ I� J#Jwe canverify that conditions (9.62) aremet by selecting either J1 and

J2 proportional to the orthogonal complement of the constraint JacobianF. Under assumption

that task x qð Þ has higher priority then matrix J1, associated to this task should be selected as

J1 ¼ JxGF 2 Rmx�n. Then the following is satisfied

FA� 1JT1 ¼FA� 1GTFJ

Tx

¼FA� 1 I�FTF#T�

JTx

¼FA� 1 I�FT FA� 1FT�

FA� 1� �

JTx

¼ 0mc�nJTx

¼ 0mc�mx

ð9:63Þ

The projection matrix J1 reflects the requirement that the task with higher priority is to be

realized in the constraint orthogonal complement space. This is consistentwith results obtained

in Chapters 7 and 8 while analyzing the constraints–task relationship. This also reflects the

requirements that a constraint should not be violated by the task motion.

The last two equations in (9.62) J2A� 1FT ¼ 0my�mc and J2A

� 1JT1 ¼ 0my�mx lead to the

selection of matrix J2 as

J2A� 1FT ¼ 0my�mc Y J2 ¼ JyGF 2 Rmy�n

J2A� 1JT1 ¼ 0my�mx Y J2 ¼ JyGJ1 2 Rmy�n

ð9:64Þ

By using the structural properties of the pseudoinverse and its orthogonal complement, the

structure of the projection (9.64) can be rearranged into the following form

J2A� 1FT ¼ Jy In�n �F#F

� A� 1FT ¼ 0my�mc

J2A� 1JT1 ¼ Jy In�n � J#1J1

� A� 1JT1 ¼ 0my�mx

ð9:65Þ

MatricesGF andGJ1 have the same dimensionbut are not the same, thus the structure ofmatrix

J2 cannot be derived directly from (9.65). Further transformation is needed to make a structure

of the expressions in (9.65) in the form Jy VA� 1FT and Jy VA� 1JT1 , thus expressing the

matrix as J2 ¼ JyV. By adding � JyJ#1J1A

� 1FT ¼ 0my�mc to the first row in (9.65), and

� JyF#FA� 1JT1 ¼ 0my�mx to the second rowand taking into accountFA� 1JT1 ¼ 0mc�mx yields

J2A� 1FT ¼ Jy In�n �F#F� J#1J1

� A� 1FT ¼ 0my�mc

J2A� 1JT1 ¼ Jy In�n �F#F� J#1J1

� A� 1JT1 ¼ 0my�mx

ð9:66Þ

Thus we can write V ¼ In�n �F#F� J#1J1�

and matrix J2 ¼ JyV becomes

J2 ¼ Jy I�F#F� J#1J1� ð9:67Þ

A more general solution for task hierarchy can be found in [4]. That solution includes a

general case of task–constraint hierarchy.

Now we can return to the analysis of the other interconnecting terms in (9.53). Since the

inertia matrix (9.61) under conditions (9.62) is a block diagonal then the terms

l q; _qð Þ ¼ LJA� 1b q; _qð Þ�L _J _q and n qð Þ ¼ LJA� 1g qð Þ become a projection of the config-

uration space disturbance to the appropriate operational spaces.


The transformation from configuration space into the constraint and operational spaces can

be determined by premultiplying the configuration space equations of motion by FA� 1,

J1A� 1 and J2A

� 1 respectively and recalling s ¼ FT fF þ JT1 fx þ JT2 fy to obtain

LF€fþ lF q; _qð Þþ nF qð Þ ¼ fF � fFc

Lx€xþ lx q; _qð Þþ nx qð Þ ¼ fx � fxc

Ly€yþ ly q; _qð Þþ ny qð Þ ¼ fy � fyc

ð9:68Þ

Here LF ¼ FA� 1FT� � 1

stands for the inertia matrix in the constraint direction,

Lx ¼ J1A� 1JT1

� � 1andLy ¼ J2A

� 1JT2� � 1

stand for the inertia matrices in the operational

spaces. li q; _qð Þþ ni qð Þ; i ¼ F; x; y stand for the projections of the configuration space

disturbance b q; _qð Þþ g qð Þ and the velocity induced forces. fF; fx; fy stand for the control

forces in the corresponding operational spaces.

Having the dynamics of the system decomposed as in (9.68), then application of the

acceleration control method leads to the selection of the operational space desired acceleration

enforcing tracking of the constraints and the tasks as

€fdes ¼ €f

ref � KDF _eF þKPFeFð Þ; eF ¼ f�fref

€xdes ¼€xref � KDX _eX þKPXeXð Þ; eX ¼ x� xref

€ydes ¼€yref � KDY _eY þKPYeYð Þ; eY ¼ y� yref

ð9:69Þ

The corresponding operational space and configuration space forces can be expressed as

fF ¼ lF þ nF þ fFcð ÞþLF€fdes

fx ¼ lx þ nx þ fxcð ÞþLx€xdes

fy ¼ ðly þ ny þ fycÞþLy€ydes

s ¼FT fF þ JT1 fx þ JT2 fy

ð9:70Þ

The result shows that the subsequent tasks in the hierarchy are executed in the orthogonal

complement space of the preceding task. The highest priority is assumed for the constraints and

all tasks are realized in the constraint orthogonal complement space. This offers a simple, yet

effective structure of establishing a hierarchy for the execution of the tasks and their dynamical

decoupling within the framework of the acceleration control. The structure of the control

system is shown in Figure 9.5.

Figure 9.5 Structure of the multitask control system.


9.4.3 Selection of Configuration Space Desired Acceleration

As shown in Chapter 7, configuration space control is realized by applying configuration

space control force s ¼ sdis þA€qdes. In such a realization, the closed loop dynamics reduce

to A€q ¼ A€qdes þ p Qd ; sdisð Þ, where p Qd ; sdisð Þ stands for the disturbance estimation error.

Appropriate selection of the desired acceleration would guarantee the system specifica-

tion. For a negligible disturbance estimation error the closed loop dynamics can be

expressed as

A€q ¼ A€qdes ¼ sdes ð9:71Þ

For the constraint–task hierarchy discussed in Section 9.4.2 with the constrain–task

Jacobian matrix as JT ¼ �FT ..

.JT1...JT2�and the constraint–task operational space task vector as

hT ¼ f x y½ �, the operational space acceleration is given in (9.57). Insertion of (9.71) withsdes ¼ JT fdes and fdes

� T ¼ fdesF fdesx fdesy

h iinto (9.57) yields

J€qdes ¼ €hdes � _J _q

fdes ¼ JA� 1JT� � 1

€hdes � _J _q� ð9:72Þ

Insertion of (9.72) into (9.71) yields the configuration space desired acceleration

€qdes ¼ AJT JA� 1JT� � 1

€hdes � _J _q� ¼ J# €hdes � _J _q

� ð9:73Þ

As shown, the selection of the matrices J1 ¼ JxGF 2 Rmx�n and J2 as in (9.67) yields

block diagonal matrix JA� 1JT� � 1

with diagonal block matrices LF ¼ FA� 1FT� � 1

,

Lx ¼ J1A� 1JT1

� � 1and Ly ¼ J2A

� 1JT2� � 1

. Then the right pseudoinverse J# can be calcu-

lated as

A� 1 FT ...JT1

..

.JT2

� � LF 0 0

0 Lx 0

0 0 Ly

264

375 ¼ A� 1FTLF

..

.A� 1JT1Lx

..

.A� 1JT2Ly

� �ð9:74Þ

Insertion of (9.74) into (9.73) yields the configuration space desired acceleration

€q ¼F# €fdes � _F _q

�þ J#1 €xdes � _J1 _q

� þ J#2 €ydes � _J2 _q�

F# ¼ A� 1FTLF

J#1 ¼ A� 1JT1Lx

J#2 ¼ A� 1JT2Ly

ð9:75Þ


The obtained structure confirms the full correspondence of mapping the operational space

control forces and mapping the operational space desired accelerations by the right weighted

pseudoinverse matrix J# consistent with dynamical decoupling requirements. This establishes

the correspondence between acceleration control design in a constrained operational space and

the selection of the configuration space desired acceleration. The structure of the control

system is depicted in Figure 9.6.

References

1. Craig, J.J. and Raibert, M. (1979) A systematic method for hybrid position/force control of a manipulator.

Proceedings of the IEEE Computer Software Applications Conference, Chicago.

2. Khatib, O. (1987) A unified approach for motion and force control of robot manipulators: The operational space

formulation. IEEE Journal on Robotics and Automation, RA-3(1), 43–53.

3. Fisher, W.D. and Mujtaba, M.S. (1991) Hybrid Position. I Force Control: A Correct Formulation, Measurement

and Manufacturing Systems Laboratory, HPL-91-140.

4. Khatib, O., Sentis, L., Parkland, J., andWarren, J. (2004)Whole-body dynamics behavior and control of humal-like

robots. International Journal of Humanoid Robotics, 1(1), 29–43.

Further Reading

Arnold, V.I. (1989) Mathematical Methods of Classical Mechanics, 2nd edn, Springer-Verlag, New York.

Asada, H. and Slotine, J.-J. (1986) Robot Analysis and Control, John Wiley & Sons, Inc., New York.


deSapio,V. andKhatib,O. (2005)Operational space control ofmultibody systemswith explicit holonomic constraints.

Proceedings of the IEEE International Conference on Robotics and Automation Barcelona, Spain.

Kokotovic, P.V., O’Malley, R.B., and Sannuti, P. (1976) Singular perturbations and order reduction in control theory.


Nakao, M., Ohnishi, K. and Miyachi, K. (1987) A robust decentralized joint control based on interference estimation.

Proceedings of the IEEE Conference on Robotics and Automation, vol. 4, pp. 326–331.

Paul, R. (1982) Robot Manipulators: Mathematics, Programming and Control, MIT Press, Cambridge, Mass.

Figure 9.6 Multiple task acceleration controller


Siciliano, B. (1990) Kinematic control of redundant robot manipulators: A tutorial. Journal of Intelligent and Robotic

Systems, 3, 201–212.

Siciliano, B. and Khatib, O. (eds) (2008) Springer Handbook of Robotics, Springer-Verlag, New York, ISBN: 978-3-

540-23957-4.

Spong,M.W., Hutchinson, S., and Vidyasagar, M. (2006) Robot Modeling and Control, JohnWiley & Sons, Inc., New

York.

Utkin, V.I. (1992) Sliding Modes in Control and Optimization, Springer-Verlag, New York.


Index

acceleration control

acceleration controller, 62, 71, 105, 253, 288,

341, 358

boundness of control, 87, 90, 92, 102, 170,

184, 201

convergence acceleration, 86, 89, 105, 143,

170, 186, 201, 231, 253, 284, 324

desired acceleration, 67, 88, 96, 108, 149, 170,

178, 195, 203, 207, 254, 281, 319, 345, 357

error dynamics, 71, 81, 95, 101, 169, 217, 247,

296, 320

equations of motion, 97, 99, 101, 217, 228

equivalent acceleration, 73, 75, 77, 86, 140,

171, 172, 183, 239, 283, 325,

equivalent force, 73, 80, 82, 88, 140, 168, 283, 325

generalized error, 76, 82, 100, 111, 142, 165,

253, 286, 291, 328

generalized structure, 105

relative degree, 72, 74, 105

stability, 85, 87, 94, 283, 324

task controller, 70, 72, 303, 325, 340, 356

actuator, 10, 27, 35, 64, 132, 136, 140, 156, 162

augmented operational space, 338

bilateral control

closed loop dynamics, 239

control error dynamics, 240

control system structure, 241

convergence, 240

desired acceleration, 240

dynamics, 235

force error dynamics, 241

hybrid matrix, 237

ideal teleoperator, 237

master side force, 235, 242

master system, 236

operational requirements, 237

operator dynamics, 236

position error dynamics, 239

position generalized error, 239

slave system, 236

steady state force, 242

steady state position, 241

structure and components, 236

system with scaling, 248, 250

system without scaling, 235, 243

with communication delay, 270

bilateral control in acceleration dimension,

251, 254

capacitance, 6

closed loop dynamics

transfer function 38,

with observer 49,

communication delay

delay in control channel, 261, 266

delay in measurement channel, 256, 261

configuration space control

control input, 286


dynamics, 279

generalized control error, 282

independent joint control, 280

recursive control, 287

system structure, 282

vector control, 281



configuration space, 7, 18

constrained motion control

control input, 293

dynamics, 289, 292

initial conditions, 306

constraint tracking error, 348

constraints

holononic constraints, 10

constraints in configuration space, 290, 295, 297

constraint force, 10, 17, 306, 340

constraint Jacobian, 291

constraint manifold, 291

constraint null space, 303

controllability, 41

control

basic concepts, 30

design, 30

standard forms, 31

control input distribution matrix, 32, 40, 58,

280, 295, 332, 338, 354

control resources, 91, 96, 108, 238

control in acceleration dimension

bilateral control , 252, 254

force control, 212, 214

position control , 212

degree of freedom, 4, 6, 9, 273, 309, 347

differential equation

characteristic polynomial, 30, 198

roots, 31

solution, 32

disturbance,

estimation, 49, 116

exogenous disturbance, 34, 38

high order polynomial, 128

input disturbance, 31

model, 118

disturbance observer

closed loop, 128

external force observer, 128

plant with actuator, 132

position based, 121

velocity based, 119

with dynamics in current loop, 136

dynamical decoupling, 278, 294, 333, 343, 348

dynamically consistent pseudoinverse, 317, 342

dynamics of constrained system, 330, 337

electrical machine, 24

end-effecter frame, 343

energy

kinetic energy, 4, 11, 15

magnetic, 5, 23

magnetic coenergy, 5, 25

energy conversion

coupling field, 23

electromechanical, 28, 29

equilibrium solution, 69, 99, 283, 293

equivalent acceleration observer, 141, 170, 183, 241

equivalent control, 52, 56, 100, 259, 284

equivalent force observer, 142, 168

Faraday’s law, 5

feedback, 29, 36, 40

flux linkage, 5, 8, 27

force

conservative, 4, 15, 25

coriolis forces, 16, 64, 279, 315

coupling forces, 339, 354

dissipative, 9

magnetomotive, 24

non-conservative, 17

non-potential, 9, 10

force control

actuator current, 203

closed loop dynamics, 179

control error, 179

equivalent acceleration, 183

finite time convergence, 185, 191

force model, 176

interaction force control, 176

lossless environment, 189

proportional control, 178

pull push force control, 191

push pull force equilibrium points, 193

stability, 184

variable damping coefficient, 184

functional observers, 144

functionally related systems, 215, 228

common mode, 231

difference mode, 232

functional relations, 230

synchronization of motion, 230

virtual systems, 231

generalized coordinates, 7

generalized inverse, 294, 348

generalized joint forces, 279

grasp force

center of geometry control, 243

362 Index

control, 215


desired acceleration, 217, 219

desired closed loop dynamics, 218

redundancy, 218

structure of grasp force control, 222

haptics, 234

hard constraints, 306, 308

hierarchy of tasks, 347, 352

hybrid control, 342, 345

impedance control, 209, 210

inductance, 5

mutual inductances, 25

self inductances, 24

matrix, 25

inertia matrix, 13, 15, 279

Jacobian, 11, 228, 394, 306

Lagrange

Lagrange multipliers, 10, 305, 309

Lagrangian, 5, 21, 25

Euler–Lagrange equations, 7, 16, 21

Raleigh, 9, 10

Laplace transform, 33

Lyapunov function, 44

finite-time convergence, 87

manipulator, 11

matrix exponential, 32

matrix in block form, 295, 339

mechanical impedance

control, 209, 335


structure, 210

minimum norm solution, 317

motion

rotational, 4, 63, 77, 125

translational, 4, 12, 63, , 179

motion control

acceleration control, 63

compliance, 67

control tasks, 68


generalized disturbance, 65

interaction force, 65

matched disturbance, 49, 66

output tracking, 71, 76

plant description, 63

stiffness, 67

motion modification

changing equivalent acceleration, 201

equilibrium solution, 198

position dynamics, 198

requirements, 195

trajectory shift, 196

reference modification, 195, 199

desired acceleration modification, 201, 203, 212

multi-body systems

configuration, 4

configuration space, 4

joint space, 4

operational space, 18

task, 222

Newton’s second law, 4, 8

nonredundant multibody system 19

nonredundant task

dynamics, 19, 314

control, 319, 325


control force, 320


null space, 293

null space projection matrix, 294, 316, 317, 351

observability, 45

observer

full order state observer, 46

Luenberger observer, 56

reduced order, 48

disturbance observer, 49

sliding mode observer, 56

operational space

acceleration, 313

control, 313, 319

disturbance, 346, 347, 370

dynamics, 18, 315

force distribution matrix, 315

force mapping, 314

inertia matrix, 315

Jacobian, 313

nonredundant task, 315

redundant task, 316

singularities, 315

planar manipulator control, 288,

321, 325

Index 363

plant with disturbance observer

approximated presentation, 168

disturbance estimation error, 150

dynamics, 151

lead-lag characteristics, 154

noise rejection, 163

plant with actuator, 156

with current loop dynamics, 157, 159

position predictor-observer, 260

position tracking controllers, 111

posture

control force, 329

Jacobian, 318

tracking, 328

principle of least action, 7

priority of task, 353

rate of convergence

enforcing exponential convergence, 87

enforcing finite-time convergence, 87

redundant system, 219, 314

redundant task

control, 328, 330, 353


control force, 329


dynamics, 315

reference input, 38

remote operation, 225, 228, 237

right pseudoinverse, 307

selection matrix, 303, 342

sliding manifold, 51

stability

asymptotic, 44

characteristic polynomial, 37

common factors, 37

Lyapunov function, 44

Lyapunov stability theorem, 44

state feedback, 40

systems

characteristic equation, 31

characteristic polynomial, 31

conservative, 7

electomechanical, 20

electrical, 20

linear time invariant, 30

matching conditions, 40

regular form, 42

state space representation, 39

state transformation, 41

system with delay

bilateral control, 254, 270

closed loop behavior, 267

delay in measurement channel, 257

delay in control channel, 263

observer-predictor, 260

systems with sliding modes

control, 51

design, 53

disturbance observers, 57

existence conditions, 53

equations of motion, 52

equivalent control, 52

linear systems, 55

observers, 56

stability, 53

state observer, 56

task-constraint relationship, 330, 337

task-constraints hierarchy, 355

telemanipulation, 234

teleoperation, 234

time delay, 256

time-varying delay, 256, 257

touch, 233

dynamics touch, 234

haptic touch, 234

trajectory tracking, 107

transfer function

complementary sensitivity , 36

disturbance, 36

loop, 37

noise, 36, 38

poles, 34

proper, 33

sensitivity, 36, 37

strictly proper, 33

zeros, 34

translational motion, 4, 15, 180

transparency, 237, 247

unconstrained motion, 295, 301, 307

velocity in constrained direction, 291

velocity induced acceleration, 213, 296, 306,

320, 346

viscous friction, 279

weighted pseudoinverse, 306, 352

work

done by force, 3

done by torque, 3

364 Index

Motion Control Systemsprasadpawaskar.weebly.com/uploads/6/0/2/9/6029691/... · Sˇabanovic´, Asif....

Documents

Transcript of Motion Control Systemsprasadpawaskar.weebly.com/uploads/6/0/2/9/6029691/... · Sˇabanovic´, Asif....