Motion Control Systems

MOTION CONTROL SYSTEMS Motion Control Systems, First Edition. Asif S Šabanovic´ and Kouhei Ohnishi. © 2011 John Wiley ...

Author: Asif Šabanović | Kouhei Ohnishi

297 downloads 3027 Views 3MB Size Report

This content was uploaded by our users and we assume good faith they have the permission to share this book. If you own the copyright to this book and it is wrongfully on our website, we offer a simple DMCA procedure to remove your content from our site. Start by pressing the button below!
Report copyright / DMCA form

DOWNLOAD PDF

MOTION CONTROL SYSTEMS

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 CONTROL SYSTEMS Asif Sˇabanovic´

Sabancı University, Turkey

Kouhei Ohnishi

Keio University, Japan

This edition first published 2011 Ó 2011 John Wiley & Sons (Asia) Pte Ltd

Registered office John 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 any means, 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, John Wiley & 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 Sˇabanovic´, Asif. Motion control systems / Asif Sˇabanovic´. 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 About the Authors

ix xi

PART ONE – BASICS OF DYNAMICS AND CONTROL 1

Dynamics of Electromechanical Systems 1.1 Basic Quantities 1.1.1 Elements and Basic Quantities in Mechanical Systems 1.1.2 Elements and Basic Quantities in Electric Systems 1.2 Fundamental Concepts of Mechanical Systems 1.2.1 The Principle of Least Action 1.2.2 Dynamics 1.2.3 Nonpotential and Dissipative Forces 1.2.4 Equations of Motion 1.2.5 Properties of Equations of Motion 1.2.6 Operational Space Dynamics 1.3 Electric and Electromechanical Systems 1.3.1 Electrical Systems 1.3.2 Electromechanical Systems 1.3.3 Electrical Machines References Further Reading

3 3 3 5 7 7 8 9 10 14 18 20 20 21 24 27 27

2

Control System Design 2.1 Basic Concepts 2.1.1 Basic Forms in Control Systems 2.1.2 Basic Relations 2.1.3 Stability 2.1.4 Sensitivity Function 2.1.5 External Inputs 2.2 State Space Representation 2.2.1 State Feedback 2.2.2 Stability 2.2.3 Observers 2.2.4 Systems with Observers 2.2.5 Disturbance Estimation

29 30 31 35 36 37 38 39 40 44 45 48 49

vi

Contents

2.3

Dynamic Systems with Finite Time Convergence 2.3.1 Equivalent Control and Equations of Motion 2.3.2 Existence and Stability 2.3.3 Design 2.3.4 Control in Linear Systems 2.3.5 Sliding Mode Based Observers References Further Reading

51 52 53 53 55 56 59 59

PART TWO – ISSUES IN MOTION CONTROL 3

Acceleration Control 3.1 Plant 3.2 Acceleration Control 3.2.1 Formulation of Control Tasks 3.2.2 Equivalent Acceleration and Equivalent Force 3.3 Enforcing Convergence and Stability 3.3.1 Convergence for Bounded Control Input 3.3.2 Systems with Finite-Time Convergence 3.3.3 Equations of Motion 3.3.4 General Structure of Acceleration Control 3.4 Trajectory Tracking References Further Reading

63 63 67 68 74 85 90 94 97 105 107 114 114

4

Disturbance Observers 4.1 Disturbance Model Based Observers 4.1.1 Velocity Based Disturbance Observer 4.1.2 Position Based Disturbance Observer 4.2 Closed Loop Disturbance Observers 4.2.1 Internal and External Forces Observers 4.3 Observer for Plant with Actuator 4.3.1 Plant with Neglected Dynamics of Current Control Loop 4.3.2 Plant with Dynamics in Current Control Loop 4.4 Estimation of Equivalent Force and Equivalent Acceleration 4.5 Functional Observers 4.6 Dynamics of Plant with Disturbance Observer 4.6.1 Disturbance Estimation Error 4.6.2 Dynamics of Plant With Disturbance Observer 4.7 Properties of Measurement Noise Rejection 4.8 Control of Compensated Plant eq 4.8.1 Application of Estimated ^t eq and q^€ References Further Reading

115 118 119 121 127 128 132 133 136 140 144 149 150 151 160 164 167 172 173

Contents

vii

5

Interactions and Constraints 5.1 Interaction Force Control 5.1.1 Proportional Controller and Velocity Feedback 5.1.2 Environment with Losses 5.1.3 Lossless Environment 5.1.4 Control of Push Pull Force 5.2 Constrained Motion Control 5.2.1 Modification of Reference 5.2.2 Modification by Acting on Equivalent Acceleration 5.2.3 Motion Modification while Keeping Desired Force Profile 5.2.4 Impedance Control 5.2.5 Force Driven Systems 5.2.6 Position and Force Control in Acceleration Dimension 5.3 Interactions in Functionally Related Systems 5.3.1 Grasp Force Control 5.3.2 Functionally Related Systems References Further Reading

175 176 178 182 187 191 193 195 201 205 209 210 211 215 215 225 232 232

6

Bilateral Control Systems 6.1 Bilateral Control without Scaling 6.1.1 Bilateral Control Design 6.1.2 Control in Systems with Scaling in Position and Force 6.2 Bilateral Control Systems in Acceleration Dimension 6.3 Bilateral Systems with Communication Delay 6.3.1 Delay in Measurement Channel 6.3.2 Delay in Measurement and Control Channels 6.3.3 Closed Loop Behavior of System with Observer 6.3.4 Bilateral Control in Systems with Communication Delay References Further Reading

233 234 238 247 251 256 257 263 267 270 274 274

PART THREE – MULTIBODY SYSTEMS 7

Configuration Space Control 7.1 Independent Joint Control 7.2 Vector Control in Configuration Space 7.2.1 Selection of Desired Acceleration 7.3 Constraints in Configuration Space 7.3.1 Enforcement of Constraints by Part of Configuration Variables 7.4 Hard Constraints in Configuration Space References Further Reading

279 280 281 282 290 303 304 311 312

viii

Contents

8

Operational Space Dynamics and Control 8.1 Operational Space Dynamics 8.1.1 Dynamics of Nonredundant Tasks 8.1.2 Dynamics of Redundant Tasks 8.2 Operational Space Control 8.2.1 Nonredundant Task Control 8.2.2 Redundant Task Control References Further Reading

313 314 314 315 318 319 328 336 336

9

Interactions in Operational Space 9.1 Task–Constraint Relationship 9.2 Force Control 9.3 Impedance Control 9.4 Hierarchy of Tasks 9.4.1 Constraints in Operational Space 9.4.2 Enforcing the Hierarchy of Tasks 9.4.3 Selection of Configuration Space Desired Acceleration References Further Reading

337 337 341 345 347 347 352 357 358 358

Index

361

Preface This book is concerned with the development of an understanding of the design issues in controlling motion within mechanical 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 AMC Workshops) 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. Time flows 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

x

Preface

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 and give essential methods which 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 actuated multibody 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 from us. We would 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 c Kouhei Ohnishi

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. Eugene Mittelmann Achievement Award 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 Dynamics and Control No mathematical representation can precisely model a real physical system. One cannot predict exactly what the output of a real physical system will 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 we will 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.

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

1 Dynamics of Electromechanical Systems 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 electromechanical 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 using Lagrange 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):


4

Motion Control Systems

.

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.

. . .

In mechanics, a body receives work 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 Ð x moving from position x1 along a translational path G to position x2 is defined by WF12 ¼ x12 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 Ðu position u1 to angular position u2 is given by Wt12 ¼ u12 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 _ ¼ p_ Fðx; xÞ

ð1:1Þ

_ is the force, p ¼ mx_ ¼ mv is the linear momentum. Here Fðx; xÞ The kinetic energy of nonrotating rigid body with mass m and velocity x_ is given by 1 1 1 _ x_ ¼ vmv ¼ mv2 T ¼ xm 2 2 2

ð1:2Þ

_ The work done by changing The change in kinetic energy with time is dT=dt ¼ p_ x_ ¼ F x. position from x1 ðt1 Þ to x2 ðt2 Þ can be expressed as ð t2 ð x2 Tðt1 Þ Tðt2 Þ ¼ ðdT=dtÞdt ¼ Fdx. t1

x1

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 potential Uð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 Þ ¼

ð t2 t1

ð q2 dT qUðqÞ dt ¼ dq ¼ Uðt2 Þ Uðt1 Þ dt qq q1

ð1:3Þ

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

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

5

Dynamics of Electromechanical 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 3n we say that the system is constrained. A system of free particles constrained to move 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 of magnetic field or electric field energy will 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 associated with physical objects consisting of one or more 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 either f ¼ 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 Ð t energy. It is a function of the flux-linkage and the current and can be expressed by Te ¼ t0 iedj ¼ Ðf Ðt t0 iðdf=djÞdj ¼ f0 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 relation f ¼ f ðiÞ is linear, then f ¼ Li, where L is defined as the inductance of the inductor, and the stored magnetic energy becomes ðf Te ðfÞ ¼

ðf idf ¼

f0

f0

f 1 f2 1 f20 df ¼ L 2 L 2 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 if for any given point on the Ðcurve f ¼ fðiÞ can be expressed as the sum of the integral Te ðfÞ and i its dual integral Te* ðiÞ ¼ i0 fdi representing the so-called magnetic coenergy ðf Te ðfÞ þ Te* ðiÞ ¼

idf þ f0

ði i0

fdi ¼ fi

ð1:5Þ

It is easy to show that a change in the magnetic coenergy dTe* ðiÞ can be expressed as dTe* ðiÞ ¼ fdi þ idf dTe ðfÞ ¼ fdi |fflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflffl} ¼0

ð1:6Þ

6


The dual integral Te* ðiÞ allows the flux to be determined as f ¼ dTe* ð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 ði ði 1 1 Te* ðiÞ ¼ fdi ¼ Lidi ¼ Li2 Li02 2 2 i0

ð1:7Þ

i0

The flux can be determined from f ¼ dTe* ð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 dðLiÞ di ¼ ¼L dt dt dt

ð1:8Þ

Note that for a linear inductor the magnetic energy and magnetic coenergy are equal, that is, Te ¼ Te* . 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 Ð t charge Ð t Q and electricÐ Qpotential e) can be expressed as potential energy Ue ¼ eidj ¼ t0 t0 eðdQ=djÞdj ¼ Q0 edQ. 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 ðQ Ue ðQÞ ¼

ðQ edQ ¼

Q0

Q0

Q 1 Q2 1 Q20 dQ ¼ C 2C 2C

ð1:9Þ

Ðe The electric coenergy Ue* ðeÞ ¼ e0 Qde allows the charge to be determined as Q ¼ dUe* ðeÞ=de and consequently dQ=dt ¼ i. For a linear capacitor one can find ðe Ue* ðeÞ ¼

ðe Qde ¼

e0

e0

1 1 Cede ¼ Ce2 Ce20 2 2

ð1:10Þ

The charge can be determined from Q ¼ dUe* ð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 d dUe* ðeÞ dðCeÞ de ¼ ¼ ¼C dt dt de de dt

ð1:11Þ

7


In general the relation between stored electric energy U and electric coenergy Ue* is given by U þ Ue* ¼ Qe. For a linear capacitor the electric energy and electric coenergy are equal, that is, U ¼ Ue* . Consider a system with 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 q_ 1 ; . . . ; q_ n . The kinetic energy is a function of the generalized coordinates qi ; i ¼ 1; 2; . . . ; n and generalized velocities q_ i ; 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 a way 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Þ ¼ G L½qðtÞ; qðtÞ; tdt. 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 Þ. The principle of least action states that the actual path taken by the system is an extreme of SðGÞ. The solution is a so-called Euler–Lagrange equation which (loosely speaking) states that if qðtÞ is _ the 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 qL qL ¼ dt qq_ qq

ð1:12Þ

_ The Lagrangian L½qðtÞ; qðtÞ; t is not unique. We can make the transformation L0 ¼ 0 bL; b 2 R or L ¼ 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 a way 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.

8


1.2.2 Dynamics The trajectories that satisfy the least action principle for conservative systems give a solution of the Euler–Lagrange equation d qL qL ¼0 dt qq_ qq

ð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 qUðxÞ _ ¼ ðmxÞ dt 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 qL qU ¼ qx qx qL ¼ mx_ qx_

ð1:15Þ

Integration of the second equation with respect to x_ yields L ¼ 1=2 mx_ 2 þ cðxÞ and substituting it to the first equation in (1.15), the constant of integration can be derived as qL q 1 2 qcðxÞ qU ¼ mx_ þ cðxÞ ¼ ¼ ) cðxÞ ¼ UðxÞ qx qx 2 qx qx

ð1:16Þ

Consequently Lagrangian can be expressed as 1 _ UðxÞ L ¼ mx_ 2 þ cðxÞ ¼ TðxÞ 2

ð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 2 easy to confirm by taking T ¼ 1=2 Lf_ and U ¼ 1=2 ð1=LÞf2 for flux linkage as a generalized 2 1= 1 coordinate or T ¼ 2 LQ_ U ¼ =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 the mechanical and electrical system motion. 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.

9


The Lagrangian, in a sense, measures the system’s activity: when there is more kinetic energy, the Lagrangian is greater, but when there is more 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 energy U (nonpotential forces) can be included in the Lagrange equation by adding the work of these forces into the energy expression. For a system with n-dof with generalized coordinates q1 ; . . . ; qn , a component-wise Euler–Lagrange equation with forces Fi ; i ¼ 1; 2; . . . ; n not included in the potential energy can be expressed as d dt

qL qq_ i

qL ¼ Fi ; qqi

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 socalled Raleigh dissipative function R R ¼

n X n 1X brs q_ r q_ s 2 r¼1 s¼1

ð1:19Þ

qR FiR ¼ qq_ i

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 P expressing change of work due to the action of nonpotential forces as dW ¼ Fi dqi qR=qq_ i into Euler–Lagrange equations yields d dt d dt

qL qq_ i qT qq_ i

qL qR þ ¼ Fi ; qqi qq_ i

qT qU qR þ þ ¼ Fi ; qqi qqi qq_ i

i ¼ 1; 2; . . . ; n ð1:20Þ i ¼ 1; 2; . . . ; n

This formulation includes kinetic energy T, potential energy U, dissipative forces ðqR=qq_ i Þ and nonpotential external forces Fi .

10


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 Fjcst ; 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 Fjcst ; l ¼ 1; 2; . . . ; m 0 1 d @ qL A qL qR þ ¼ Fi þ Fjcst ; dt qq_ i qqi qq_ i

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

j ¼ 1; 2; . . . ; m

ð1:22Þ

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

m X qhj j¼1

qqi

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 0 1 d @ qL A qL qR þ ¼ Fi þ Fjcst ; dt qq_ i qqi qq_ i Ficst

¼

m X qhj j¼1

qqi

i ¼ 1; 2; . . . ; n

lj ; hj ðq1 ; . . . ; qn Þ ¼ 0;

ð1:24Þ j ¼ 1; . . . ; m

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 ¼ q_ 1 ; v2 ¼ q_ 1 þ q_ 2

11


Figure 1.1

Planar elbow manipulator

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 the links, Ii ; i ¼ 1; 2 stands for the moment of inertia of the links around axis coming out of the plane 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 ¼ T ¼

1 T 1 v mi vmi þ vTmi Ii vmi ; 2 mi 2 2 X 1 i¼1

2

vTmi mi vmi þ

2 X 1 i¼1

2

i ¼ 1; 2 ð1:25Þ

vTmi Ii vmi

From geometrical considerations the linear velocities of the center of the mass are " vm1 ¼

" vm2 ¼

vxm2 vym2

"

# ¼

vxm1 vym1

"

# ¼

lm1 sinðq1 Þ

0

lm1 cosðq1 Þ

0

#"

q_ 1 q_ 2

# ¼ Jv1 q_

l1 sin ðq1 Þ lm2 sin ðq1 þ q2 Þ

lm2 sin ðq1 þ q2 Þ

l1 cos ðq1 Þ þ lm2 cos ðq1 þ q2 Þ

lm2 cos ðq1 þ q2 Þ

#"

ð1:26Þ

q_ 1 q_ 2

# ¼ Jv2 q_

ð1:27Þ

Here Jv1 2 R22 and Jv2 2 R22 are the so-called Jacobian matrices relating the velocities of the center of the mass for each link to the joint velocities and q_ T ¼ ½ q_ 1 q_ 2 is the vector of joint velocities.

12


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

1 T 1 1 v _ T mi Jvi q_ ¼ q_ T ðJvT _ i ¼ 1; 2 v mi vmi ¼ ðJvi qÞ i mi Ji Þq; 2 mi 2 2 2 X 1 i¼1

Tv ¼

2

vTmi mi vmi ¼

2 X 1 i¼1

2

v _ q_ T ðJvT i mi Ji Þq

ð1:28Þ

1 T vT v _ q_ ðJ1 m1 Jv1 Þ þ ðJvT 2 m2 J2 Þ q 2

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

q_ 1

#

q_ 2 "

v2 ¼ ½ 1 1

q_ 1

_ ¼ Jv1 q;

ð1:29Þ

¼ Jv2 q_

ð1:30Þ

#

q_ 2

Here Jv1 2 R12 and Jv2 2 R12 are Jacobian matrices relating the angular velocities of the links to the joint velocities. The rotational kinetic energy is Tri ¼ Tr ¼

1 T 1 1 v _ T Ii ðJvi qÞ _ ¼ q_ T ðJvT _ v Ii vi ¼ ðJvi qÞ i Ii Ji Þq 2 i 2 2 2 X 1 i¼1

Tr ¼

2

vTi Ii vi ¼

2 X 1 i¼1

2

v _ q_ T ðJvT i Ii Ji Þq

ð1:31Þ

1 T vT v v _ q_ ðJ1 I1 J1 Þ þ ðJvT 2 I2 J2 Þ q 2

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 1 1 1 v _ Tr1 ¼ vT1 I1 v1 ¼ q_ T ðJvT 1 I1 J1 Þ ¼ ½ q 2 2 2 1

" q_ 2

1 ¼ I1 q_ 1 q_ 1 2 1 1 1 v _ Tr2 ¼ vT2 I2 v2 ¼ q_ T ðJvT 2 I2 J2 Þ ¼ ½ q 2 2 2 1 1 1 ¼ I2 q_ 1 q_ 1 þ I2 q_ 2 q_ 2 þ I2 q_ 1 q_ 2 2 2 " #" # I1 þ I2 I2 q_ 1 1 Tr ¼ ½ q_ 1 q_ 2 2 q_ I2 I2 2

" q_ 2

#"

I1

0

0

0

I2

I2

I2

I2

q_ 1

#

q_ 2

#"

q_ 1 q_ 2

# ð1:32Þ

13


The total kinetic energy of the manipulator becomes T ¼ Tv þ Tr ¼ ¼

AðqÞ ¼

1 T vT 1 v v vT v _ þ q_ T ðJvT _ q_ ðJ1 m1 Jv1 Þ þ ðJvT 2 m2 J2 Þ q 1 I1 J1 Þ þ ðJ2 I2 J2 Þ q 2 2 1 T q_ AðqÞq_ 2 2 X i¼1

ð1:33Þ

v vT v ðJvT i mi Ji þ Ji Ii Ji Þ

Matrix AðqÞ incorporates all the mass properties of the whole manipulator 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 1 1 T ¼ q_ T AðqÞq_ ¼ ½ q_ 1 2 2

" q_ 2

a11

a12

a21

a22

#"

q_ 1

#

q_ 2

ð1:34Þ

Where the elements aij ; i; j ¼ 1; 2 of matrix AðqÞ are 2 2 a11 ¼ m1 lm1 þ I1 þ m2 ½l12 þ lm2 þ 2l1 lm2 cos ðq2 Þ þ I2 ¼ a11 ðq2 Þ |fflfflfflfflfflffl{zfflfflfflfflfflffl} |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} axis 1

a12 ¼ a21 ¼

2 m2 ½lm2

a22 ¼

þ I2

2 m2 lm2

axis 2

þ 2l1 lm2 cos ðq2 Þ þ I2 ¼ a12 ðq2 Þ ¼ a21 ðq2 Þ ð1:35Þ

The potential energy stored in the links is given by U ¼ m1 glm1 sin ðq1 Þ þ m2 g ½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 q_ T AðqÞq_ UðqÞ. From Euler–Lagrange equations the components describing motion of the first joint can be determined as follows qL qU ¼ ¼ m1 glm1 cos ðq1 Þ m2 g½l1 cos ðq1 Þ þ lm2 cos ðq1 Þ ¼ g1 ðq1 ; q2 Þ qq1 qq1 qL ¼ a11 q_ 1 þ a12 q_ 2 qq1 d qL qa11 qa12 ¼ a11 q€1 þ a12 q€2 þ q_ q_ þ q_ q_ qq2 2 1 qq2 2 2 dt qq1 d qL qL qa11 qa12 ¼ a11 q€1 þ a12 q€2 þ q_ q_ þ q_ q_ þ g1 ðq1 Þ qq2 2 1 qq2 2 2 dt qq1 qq1

ð1:37Þ

14


For the second joint, we can write qL qU ¼ ¼ m2 glm2 cos ðq1 þ q2 Þ ¼ g2 ðq1 ; q2 Þ qq2 qq1 qL ¼ a21 q_ 1 þ a22 q_ 2 qq2 d qL qa21 qa22 q_ q_ þ q_ q_ ¼ a21 q€1 þ a22 q€2 þ dt qq1 qq2 2 1 qq2 2 2

ð1:38Þ

d qL qL qa21 qa22 ¼ a21 q€1 þ a22 q€2 þ q_ q_ þ q_ q_ þ g2 ðq1 ; q2 Þ dt qq2 qq2 qq2 2 1 qq2 2 2

By inserting (1.37) and (1.38) into (1.24) and taking into account the work of the torques t1 and t2 developed by actuators, the dynamics of the planar elbow manipulator yields a11 q€1 þ a12 q€2 þ

qa11 qa12 q_ q_ þ q_ q_ þ g1 ðq1 Þ ¼ t1 qq2 2 1 qq2 2 2

qa21 qa22 q_ q_ þ q_ q_ þ g2 ðq1 ; q2 Þ ¼ t2 a21 q€1 þ a22 q€2 þ qq2 2 1 qq2 2 2

ð1:39Þ

These equations can be simplified by merging components that depend on the velocities _ ¼ b1 ðq; qÞ

qa11 qa12 q_ 2 q_ 1 þ q_ q_ qq2 qq2 2 2

qa qa _ ¼ 21 q_ 2 q_ 1 þ 22 q_ 2 q_ 2 b2 ðq; qÞ qq2 qq2

ð1:40Þ

The final form of the equations describing the dynamics of a planar elbow manipulator can now be written as _ þ gðqÞ ¼ s AðqÞ q€ þ bðq; qÞ

ð1:41Þ

_ ¼ ½ b1 ðq; qÞ _ _ is the Here AðqÞ 2 R22 is the kinetic energy matrix, bT ðq; qÞ b2 ðq; qÞ vector representing terms depending on velocities, gT ðqÞ ¼ ½ g1 ðqÞ g2 ðqÞ is the vector of the gravitational 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 multibody mechanical 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

15


Figure 1.2

Multibody system

energy matrix or inertia matrix should be examined first and then conservative and nonconservative 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 the mass Tv . 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 ¼ system Tvi þ Tvi ¼ 1=2 vTmi mi vmi þ 1=2 vTi Ii vi and the total kinetic energy of the multibody P becomes just the sum of the kinetic energies of the individual bodies T ¼ ni¼1 Ti . Here vmi is the velocity vector of the body, vmi stands for the rotational speed vector, mi is the mass 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 Jacobian matrix relating the velocity of the center of the mass vmi and the angular velocities vi of the i-th link to _ For three-dimensional multibody systems we can write joint velocities q. _ vmi ¼ Jvi q;

Jvi 2 R3n

_ vi ¼ Jvi q;

Jvi 2 R3n

ð1:42Þ

The total energy of the n-dof multibody system can be expressed as T¼ AðqÞ ¼

n n X X 1 T vT 1 1 v _ ¼ q_ T AðqÞq_ ðTvi þ Tvi Þ ¼ q_ ðJi mi Jvi Þq_ þ q_ T ðJvT i Ii Ji Þq 2 2 2 i¼1 i¼1 n X v vT v ðJvT i mi Ji þ Ji Ii Ji Þ; i¼1

AðqÞ 2 Rnn

ð1:43Þ

16


Matrix AðqÞ 2 Rnn incorporates the mass properties of the multibody system and is called a multibody kinetic energy matrix. 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 configurationdependent; thus it represents the instantaneous composite mass properties of the whole system. Due to the boundedness of the generalized coordinates and its structure the kinetic energy matrix is bounded. Expressing AðqÞ ¼ faij ðqÞg; i; j ¼ 1; 2; . . . ; n yields the total kinetic energy as a quadratic form n X n 1 1X T ¼ q_ T AðqÞq_ ¼ aij ðqÞq_ i q_ j 2 2 i¼1 j¼1

ð1:44Þ

The elements of the Euler–Lagrange equations contributed by the kinetic energy can be now written as n d qT d X ¼ aij q_ j dt qq_ i dt j¼1

! ¼

n n X X daij aij q€j þ q_ ; i ¼ 1; 2; . . . ; n dt j j¼1 j¼1

ð1:45Þ

The terms aij q€j ; i 6¼ j represent dynamic interaction among i-th and j-th joints and satisfy aij ¼ aji due P to symmetry of the kinetic energy matrix. The term nj¼1 ðdaij =dtÞq_ j is function of the generalized coordinates and their derivatives and in general, by applying chain rule of differentiation, can be expressed as n X daij j¼1

dt

q_ j ¼

n n X X qaij j¼1

k¼1

qqk

! q_ k q_ j ¼

n X n X qaij j¼1 k¼1

qqk

q_ k q_ j

ð1:46Þ

The term qL=qqi in the Euler–Lagrange equation yields 0 1 n X n n X n qajk qL qT q @1 X 1X ¼ ¼ ajk q_ j q_ k A ¼ q_ q_ ; qqi qqi qqi 2 j¼1 k¼1 2 j¼1 k¼1 qqi j k

i ¼ 1; 2; . . . ; n

ð1:47Þ

The overall contribution of the kinetic energy to the Euler–Lagrange equations is n n X n n X n X X qaij qajk d qT qT 1X ¼ aij q€j þ q_ k q_ j q_ q_ dt qqi qqi 2 qq qqi j k k j¼1 j¼1 k¼1 j¼1 k¼1

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

n X n X qaij j¼1 k¼1

qqk

q_ k q_ j

ð1:48Þ n X n qajk 1X q_ q_ ; 2 j¼1 k¼1 qqi j k

i ¼ 1; 2; . . . ; n

_ is partitioned on the centrifugal (proportional to the square of Sometimes the term bi ðq; qÞ the 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.

17


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¼

n X

mi gT rmi 0 ðqÞ

i¼1

ð1:49Þ

1n Here, rmi is the vector of position of the center of mass of the i-th link. The 0 ðqÞ 2 R substitution into Euler–Lagrange equation yields

gi ðqÞ ¼

n n qU X qrmj ðqÞ X ¼ mj gT 0 ¼ mi gT coli ðJvj Þ; qqi qqi j¼1 i¼1

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 _ þ gðqÞ ¼ s þ Fcst AðqÞ q€ þ bðq; qÞ

ð1:51Þ

Here AðqÞ 2 Rnn is a symmetric positive definite uniformly bounded matrix given in (1.43). The other terms can be expressed as: 1. Corriolis and centrifugal terms

2

_ _ ¼ AðqÞ bðq; qÞ q_

q_ T Aq1 q_

3

7 16 6 7 6... 7; 5 24 q_ T Aqn q_

Aqi ¼

qA qqi

ð1:52Þ

2. Gravity term gðqÞ ¼

3. Constraint forces

con

F

¼

qhðqÞ qq

T

qUðqÞ 2 Rn1 qq

‚ ¼ JTh ‚;

Jh ¼

ð1:53Þ

qhðqÞ 2 Rmn qq

ð1:54Þ

Here Jh 2 Rmn 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.

18


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 socalled 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 R1m

ð1:55Þ

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

qfðqÞ _ q; qq

J¼

qfðqÞ 2 Rmn qq

ð1:56Þ

Here, matrix JðqÞ 2 Rmn 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 1 T ¼ x_ T LðxÞx_ 2

ð1:57Þ

Here LðxÞ 2 Rmm stands for the operational space kinetic energy matrix. The potential energy is assumed to be function of operational space generalized coordinates UðxÞ. Taking into account nonconservative operational space forces F, Euler–Lagrange equations representing the motion in operational space can be written as _ UðxÞ L ¼ Tðx; xÞ d qL qL ¼F _ dt qx qx

ð1:58Þ

The particular terms in Euler–Lagrange equations are qL ¼ LðxÞx_ qx_ d qL _ x_ ¼ LðxÞ x€ þ LðxÞ dt qx_

ð1:59Þ

19

Dynamics of Electromechanical Systems 21

x_ T Lx1 x_

3

62 7 6 7 6 7 6 7 _ qL qTðx; xÞ qUðxÞ 6 . . . 7 ¼ ¼6 7 pðxÞ 6 7 qx qx qx 6 7 61 7 4 x_ T L x_ 5 xm 2 Lxi ¼

qLðxÞ ; qxi

ð1:60Þ

i ¼ 1; 2; . . . ; m

These terms yield operational space dynamics _ þ pðxÞ ¼ F LðxÞ x€ þ mðx; xÞ 21 3 _ x1 x_ xL 62 7 6 7 6 7 6 7 6 7 _ 7 ... _ ¼ LðxÞ mðx; xÞ x_ 6 6 7 6 7 6 7 61 7 4 _ 5 xLxm x_ 2

ð1:61Þ

The forces in operational space F 2 Rm1 can be implemented only by applying the configuration space forces s 2 Rn1 , 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 of motion (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 1 1 T ¼ q_ T AðqÞq_ ¼ x_ T LðxÞx_ 2 2

ð1:62Þ

The operational space velocities and configuration space velocities are related by the _ thus invariance of kinetic energy yields Jacobian matrix x_ ¼ Jq, 1 T 1 1 1 _ T LJq_ ¼ q_ T ðJT LJÞq_ q_ AðqÞq_ ¼ x_ T LðxÞx_ ¼ x_ ¼ ðJqÞ 2 2 2 2

ð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 JT LJ into the dynamical model of the multibody system in operational space yields [3] _ þ gðqÞ ¼ s _ þ pðxÞ ¼ JT F ¼ AðqÞ q€ þ bðq; qÞ JT ½LðxÞ x€ þ mðx; xÞ

ð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

20


operational space velocities and forces with the configuration space generalized velocities and torques are x_ ¼ Jq_ s ¼ JT F

ð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 space will 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, electromechanical 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 simple R L C circuit with constant parameters R; L; C supplied from voltage source vðtÞ, shown in Figure 1.3. The capacitor C stores charge Q 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 charge Q as the generalized coordinate, equations of motion can be written as Q_ ¼ i d Q ðLiÞ þ þ Ri ¼ vðtÞ dt C

Figure 1.3

R L C circuit

ð1:66Þ


21

For the given circuit, Lagrangian and the Rayleigh dissipative functions are 2 _ ¼ 1 LQ_ 2 1 Q ¼ T * ðQÞ _ Ue ðQÞ Le ðQ; QÞ e 2 2C

1 1 2 Re ¼ RQ_ ¼ Ri2 2 2

ð1:67Þ

The R L C circuit equations of motion can be derived from Euler–Lagrange equations d qLe qLe qRe ¼ Ie ðtÞ þ dt qi qQ qQ_

ð1:68Þ

Here Ie ðtÞ stands for the generalized force, which in our case is voltage Ie ðtÞ ¼ vðtÞ. The first 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 0 1 d qLe d q @1 2 1 Q2 A d ¼ ðLiÞ Li ¼ dt qi dt qi 2 2C dt 0 1 qLe q @1 2 1 Q2 A Q ¼ Li ¼ qQ 2 2C C qQ

ð1:69Þ

qRe ¼ Ri qQ_ Ie ðtÞ ¼ vðtÞ

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

22


Figure 1.4 Ball suspended by an electromagnet with a voltage supply

electromagnet. Most of the magnetic 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 with mass m 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 _ ¼ 1 mx_ 2 þ mgx þ 1 LðxÞQ_ 2 _ Q; QÞ Lðx; x; 2 2 1 2 Re ¼ RQ_ 2

ð1:70Þ

Here g is the gravity constant, x is the position of the center of the ball, and LðxÞ is the inductance of the magnetic circuit. Direct application of the Euler–Lagrange equations leads to the following equations of motion for mechanical and electromagnetic subsystems d qL qL qR i2 qLðxÞ þ ¼ Ix ðtÞ ) m x€ mg ¼0 2 qx dt qx_ qx qx_

ð1:71Þ

d qL qL qR d þ ¼ IQ ðtÞ ) LðxÞi þ Ri ¼ vðtÞ _ _ dt qQ qQ qQ dt

ð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 qLðxÞ qT * ðxÞ ¼ e 2 qx qx

ð1:73Þ

23


Figure 1.5

Representation of an electromechanical converter with a magnetic coupling field

The voltage e acting on the electric circuit due to the change of the electromagnetic energy (electromotive force) is d qL d qTe* d ¼ ¼ ½LðxÞi ¼ e dt qQ_ dt qi dt

ð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 qLðxÞ qTe* ¼f ¼ 2 qx qx

d d vðtÞ Ri ¼ ½LðxÞi ¼ dt dt

qTe* qi

ð1:75Þ

¼e

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 stored magnetic 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 electromechanical coupling, thus represents the coupling field as a load for both electric and mechanical systems.

24


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 equal windings on stator and rotor. Windings on stator and rotor are placed symmetrically with angular shift of the magnetic 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;s a;b ¼ p=2 and rotor windings ur;r ¼ p=2. The shift of magnetic axes between stator winding a and the rotor winding a is a;b us;r ¼ u. The windings are supplied from external voltage sources vsT ¼ ½ vs1 . . . vsn and a;a vrT ¼ ½ vr1 . . . vrn and are assumed of carrying currents isT ¼ ½ i1s . . . ins and irT ¼ ½ i1r . . . inr 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

25


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;r a;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 s;r Ls;r a;a . The difference is in the space angle which is now ua;k ¼ u kp=n, thus the mutual inductance can be expressed as p

Ls;r a;k ¼ Lsr cos u k n

ð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–rotor windings respectively. Matrices Lr;s ðuÞ and Ls;r ðuÞ are matrices of mutual inductances of the stator–rotor and rotor–stator windings respectively. Both Lr;s ðuÞ and Ls;r ðuÞ depend on the rotor angular position u. For magnetically linear systems magnetic coenergy Te* is 1 Te* ¼ iT LðuÞi 2

ð1:79Þ

Here iT ¼ ½ i1s . . . ins i1r . . . inr stands for the current vector. The extended Lagrangian, which includes both mechanical and electromagnetic energy storage, and the Rayleigh dissipative function can be written as _ Q; iÞ ¼ 1 aðuÞu_ 2 þ 1 iT LðuÞi Lem ðu; u; 2 2 Rem

1 2 1 ¼ Bu_ þ iT Ri 2 2

ð1:80Þ

Here aðuÞ is the kinetic energy coefficient (here equal to moment of inertia). The Rayleigh 2 dissipative function is a sum of the mechanical system R ¼ 1=2 Bu_ and the electric system 1= T R ¼ 2 i Ri 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.

26


Direct application of Euler–Lagrange equations yields the following components of the equations of motion: 1. Mechanical system

d qLem d q 1 _2 1 T d _ da au þ i LðuÞi ¼ ðauÞ ¼ a€ u þ u_ ¼ dt qu_ dt qi 2 2 dt dt

qLem q 1 _2 1 T 1 qLðuÞ au þ i LðuÞi ¼ iT i ¼ qQ 2 2 2 qu qu

ð1:81Þ

qRem ¼ Bu_ qu_ Ie ðtÞ ¼ text ðtÞ

2. Electromagnetic system d qLem d q 1 _2 1 T d di dLðuÞ ¼ au þ i LðuÞi ¼ ðLðuÞiÞ ¼ LðuÞ þ i dt qi dt qi 2 2 dt dt dt

qLem q 1 _2 1 T au þ i LðuÞi ¼ 0 ¼ qQ 2 2 qQ

ð1:82Þ

qRem ¼ Ri qQ_ Ie ðtÞ ¼ vðtÞ

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 iT qLðuÞ þ B u_ i ¼ text dt 2 qu

di qLðuÞ i þ Ri ¼ vðtÞ LðuÞ þ u_ dt qu

ð1:83Þ

Term 1=2 iT ½qLðuÞ=qui represents the force due to the electromechanical conversion. _ Similarly u½qLðuÞ=qui stands for electromotive force due to the interaction of the mechanical systems on the electromagnetic energy storage. Force due to electromechanical conversion t ¼ 1=2 iT ½qLðuÞ=qui 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 3 qLs;r ðuÞ 6 " # qu 7 7 is 6 7 rT 6 i 6 7 7 ir 6 qLr;s ðuÞ 5 4 0 qu 2

0

t ¼ isT

ð1:84Þ

27


Here, the current vector is expressed as iT ¼ isT irT with isT ¼ ½ i1s i2s . . . ins and irT ¼ ½ i1r i2r . . . inr . 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 ¼ kf Fr is ¼ kf jjFr jjjjis jjsin ðwÞ s ¼ kf Fr jji?

ð1:85Þ

Here jjFr jj stands for the magnitude of the rotor flux vector, jjis jj stands for the magnitude of stator current vector and w stands for the angle between rotor flux and stator current vectors s s and i? stands for the i? ¼ jjis jjsin ðwÞ. The last expression in (1.85) shows that, if the rotor flux is kept constant, then we can write kf jjFr jj ¼ KT ðFr Þ and force (or torque) is expressed as s t ¼ KT i? . This allows us to write the dynamics of the mechanical motion of electrical machine as _ þ text ¼ tðtÞ a q€ þ bðq; qÞ s tðtÞ ¼ KT ðFÞi? ¼ KT ðFÞis

ð1:86Þ

_ includes all components that depend on position and velocity. To avoid Here b ðq; qÞ complicated notation, the component of the stator current orthogonal to rotor flux is written as s i? ¼ 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. Feedback may be used as a tool to modify the properties of a system, it can reduce the effects of disturbances and process variations, and it allows a well defined relationship to be maintained between variables or between systems. The process of designing a control system generally involves many 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


30


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 that must 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 systems which can be viewed in many 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. Systems with 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

31

Control System Design

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 dn y dn 1 y dm u dm 1 u þ a1 n 1 þ þ an y ¼ m þ b1 m 1 þ þ bm u n dt dt dt dt

ð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 dn y dn 1 y þ a1 n 1 þ þ an y ¼ 0 n dt dt

ð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 þ a1 ln 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Þ ¼

n X

Ck eak t

ð2:4Þ

k¼1

Here Ck are arbitrary constants, thus the solution (2.4) has n free parameters Ck , k ¼ 1; 2; . . . ; n. The functions eak t are sometimes called modes of the system. The solution consists of monotone functions that decrease if ak is negative or increase if ak is positive. If the roots are complex ak ¼ sk jv, the solution (2.4) is composed of the functions esk t sin ðvk tÞ and esk t cos ðvk tÞ. These functions have an oscillatory behavior but their amplitudes decrease if sk < 0 or increase if sk > 0.

32


The solution is stable if all functions eak t and esk t 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Þ ¼

n X

Ck eak t þ

k¼1

ðt gðt qÞuðqÞdq

ð2:5Þ

0

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

dm f dm 1 f þ b1 m 1 þ þ bm f m dt dt

ð2:6Þ

Here f ðtÞ is solution of equation dn f =dtn þ a1 dn 1 f =dtn 1 þ þ an f ¼ 0 with initial conditions f ð0Þ ¼ f_ ð0Þ ¼ ¼ f ðn 2Þ ð0Þ ¼ 0, f ðn 1Þ ð0Þ ¼ 1. The behavior of a LTI multiple input multiple output (MIMO) system may be described by a vector differential equation dxðtÞ ¼ AxðtÞ þ BuðtÞ dt

ð2:7Þ

Here x 2 Rn1 stands for the so-called state vector and u 2 Rm1 stands for the control vector. For time-invariant systems matrices A 2 Rnn , B 2 Rnm are constant. Matrix A 2 Rnn is called the dynamics matrix and matrix B 2 Rnm 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Þ ¼ eAt xð0Þ þ

ðt

e Aðt qÞ BuðqÞdq

ð2:8Þ

0

The matrix exponential is defined as eAt ¼ I þ At þ

1 1 ðAtÞ2 þ þ ðAtÞn 2 n!

ð2:9Þ

The characteristic equation of n n matrix satisfies the following relations det ðlI AÞ ¼ ln þ a1 ln 1 þ a2 ln 2 þ þ an ¼ 0 ¼ An þ a1 An 1 þ a2 An 2 þ þ an I ¼ 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 are negative, or equivalently that all the roots of the characteristic Equation (2.10) have negative real parts.

33


2.1.1.2 Laplace Transform, Transfer Functions The Laplace transform is very convenient for dealing with 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Þ defined on t < ¥ which grows slower than est for a real finite number s > 0 thus Ð ¥ 0 st f ðtÞjdt < ¥. Laplace transformation FðsÞ of f ðtÞ is then defined by 0 je ð¥ FðsÞ ¼ e st f ðtÞdt

ð2:11Þ

0

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Þ BðsÞ ¼ uðsÞ AðsÞ

¼

sm þ b1 sm 1 þ þ bm sn þ a1 sn 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 a transfer 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 ÞCj ebj t ¼ 0 and the transmission of the signal u ¼ Cj ebj 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Þ BðsÞ BðsÞ ¼ ¼ uðsÞ AðsÞ ðs a1 Þðs a2 Þ . . . ðs an Þ

C1 C2 Cn þ þ þ ¼ ðs a1 Þ ðs a2 Þ ðs an Þ

ð2:13Þ

34


Figure 2.2

Simple control system

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Þ ¼

n X

Ck eak t

ð2:14Þ

k¼1

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 jGðjvÞ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.

35


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 and measured 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 between variables can be expressed in terms of the transfer functions. The following relations are obtained from the block diagram in Figure 2.2. 2

FCP 6 1 þ HCP 2 3 6 6 xðsÞ 6 FCP 6 7 6 6 yðsÞ 7 ¼ 6 4 5 6 6 1 þ HCP 6 6 uðsÞ 6 FC 4 1 þ HCP

3 HCP 1 þ HCP 7 72 7 yref ðsÞ 3 7 76 P 1 7 76 dðsÞ 7 5 1 þ HCP 1 þ HCP 7 74 7 7 nðsÞ HCP HC 7 5 1 þ HCP 1 þ HCP

P 1 þ HCP

ð2:15Þ

The control error can be determined from the above relations in the following form eðsÞ ¼

F HP H yref ðsÞ þ dðsÞ nðsÞ 1 þ HCP 1 þ HCP 1 þ HCP

ð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

36


xðsÞ PCF ¼ yref ðsÞ 1 þ PCH uðsÞ CF ¼ yref ðsÞ 1 þ PCH

xðsÞ yref ðsÞ uðsÞ yref ðsÞ

F¼1

PC ¼ 1 þ PCH

xðsÞ dðsÞ

C 1 þ PCH

yðsÞ nðsÞ

¼ F¼1

¼ F¼1

¼ F¼1

P 1 þ PCH

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 systems without 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Þ PC ¼ yref ðsÞ F ¼ 1 1 þ PC

xðsÞ P ¼ dðsÞ F ¼ 1 1 þ PC

uðsÞ C ¼ yref ðsÞ F ¼ 1 1 þ PC

yðsÞ 1 ¼ nðsÞ F ¼ 1 1 þ PC

H¼1

H¼1

H¼1

H¼1

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 system without a feedforward term (F ¼ 1; H ¼ 1). Then the overall behavior of the system (2.15) can be written as

37

Control System Design 2

xðsÞ

3

2

CP

7 6 6 6 yðsÞ 7 ¼ 1 6 CP 5 1 þ CP 4 4 uðsÞ

C

P

CP

P

1

CP

C

32

yref ðsÞ

76 76 dðsÞ 54

3 7 7 5

ð2:17Þ

nðsÞ

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 2

xðsÞ

2

3

6 7 4 yðsÞ 5 ¼ uðsÞ

NP NC

1 6 4 NP NC NP NC þ MP MC MP N C

NP MC

NP NC

NP MC

MP MC

NP NC

MP NC

32 3 yref ðsÞ 74 5 dðsÞ 5 nðsÞ

ð2:18Þ

The system will be stable if the characteristic polynomial M ¼ NP NC þ MP MC 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 polynomials NP ; MP ; NC ; MC assures that there is no common factor between the characteristic polynomials and all other polynomials NP NC ; NP MC ; . . . 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 not have 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 system behavior. 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 C 1 CP 1 1 T ¼ ¼ ¼ dP ð1 þ HCPÞ2 1 þ HCP 1 þ HCP P 1 þ HCP P dT P dðlog TÞ 1 ¼ ¼ ¼S dP T dðlog PÞ 1 þ HCP

ð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. This means that S and T cannot be made 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.

38


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 system performance it is important to look at system behavior when 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 a function 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 P nðsÞ dðsÞ ¼ S½nðsÞ PdðsÞ ¼ Syol ðsÞ 1 þ HCP 1 þ HCP

ð2:21Þ

The sensitivity function directly shows the effect of the load disturbance and the measurement 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. There may 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Þ ¼ F

CP yref ðsÞ ¼ FTyref ðsÞ 1 þ HCP

FC yref ðsÞ ¼ SFCyref ðsÞ ucl ðsÞ ¼ 1 þ HCP

ð2:22Þ

The response to the input signal is given as the product of the feedforward transfer function F 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

39


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. This makes 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 space model in which the state variables are represented by a state vector x 2 Rn1, the control variables by a control vector u 2 Rm1, the measured signal by the vector y 2 Rp1 and the exogenous disturbances are represented by vector d 2 Rl1. 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 Rn1 (linear or nonlinear) which specifies the rate of change (velocity) of the state vector and (ii) a function gðx; u; tÞ 2 Rp1 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Þ do not 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 Rnm ; rank ðBÞ ¼ m can be constant or variable. Matrix D 2 Rpm is assumed constant. System (2.23) can be regarded as a nonlinear system which is linear in control input. Mathematical models of most motion control systems can be represented as timeinvariant 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Þ

40


Here A 2 Rnn , B 2 Rnm , H 2 Rnl , C 2 Rpn and D 2 Rpm are constant matrices. Matrix A 2 Rnn is called the dynamics matrix, matrix B 2 Rnm is called the control distribution or gain matrix, matrix H 2 Rnl is called the disturbance matrix, matrix C 2 Rpn is called the sensor matrix and matrix D 2 Rpm 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. Matrix B 2 Rnm shows 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 Rnl 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 Rm1 . Then Equation (2.25) can be rewritten as x_ ¼ Ax þ Bðu þ lÞ ¼ Ax þ Bu0 with u0 ¼ u þ l – thus the influence of the disturbance can be compensated by selecting u ¼ u0 l. Note that l does not stand for the original disturbance vector d 2 Rl1 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Þ

ð2:26Þ

yðsÞ ¼ CxðsÞ þ DuðsÞ

The matrix transfer functions then can be derived in the following form xðsÞ ¼ ðsI AÞ 1 BuðsÞ þ ðsI AÞ 1 HdðsÞ yðsÞ ¼ ðCðsI AÞ 1 B þ 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 Rn1 tracks its reference xref 2 Rn1

Figure 2.3

Diagram of a linear time-invariant system with state feedback

41


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

ð2:29Þ

y ¼x

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 of matrix ðA BFÞ need to be set by appropriate selection of the feedback matrix F 2 Rmn – 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 Þ to any final state xðt1 Þ within a finite time t1 t0 . The necessary and sufficient conditions for controllability of the system (2.25), with disturbance satisfying matching conditions, is that the controllability matrix =x ¼ B AB A2 B

...

An 1 B

ð2:30Þ

has full row rank. Equivalent is the condition that matrix ½ sI A B has full rank for all s. The pair fA; Bg is said to be fully controllable if (2.30) is satisfied. Consequently the eigenvalues of matrix ð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]. Output controllability can be defined similarly. If output controllability matrix is =y ¼ ½ CB CAB CA2 B . . . CAn 1 BD has full row rank the system (2.25) is output controllable. Here we would 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 Rnn is regular matrix. Assuming that matching conditions Hd ¼ Bl hold, then Bu þ Hd ¼ Bðu þ lÞ. Then the dynamics of (2.25) in a new set of variables can be written as x_ 0 ¼ TAT 1 x0 þ 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 transformation matrix T 2 Rnn is selected such that the following condition holds " TAT

1

¼

A11

A12

A21

A22

#

2

3 ðT1 Þðn mÞn 5)T¼4 5; T1 B ¼ 0ðn mÞm ; TB ¼ 4 T mn mm T B ðB BÞ 0ðn mÞm

3

2

ð2:32Þ

42


With such coordinates, the transformation system (2.31) can be rewritten as x_ 1 ¼ A11 x1 þ A12 x2 x_ 2 ¼ A21 x1 þ A22 x2 þ ðBT BÞð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 ; BT BÞ 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 Rm1 with m-dimensional external input ðu þ lÞ 2 Rm1 and square full rank gain matrix BT B with rank ðBT BÞ ¼ 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 x_ 1 ¼ A11 x1 þ A12 x2 and linear feedback x2 ¼ Lx1 is selected such that x1 has the desired dynamics. Since the pair ðA11 ; A12 Þ is controllable this step yields the desired matrix L from the assignment of eigenvalues of matrix ðA11 A12 LÞ. In the second step, the control ðu þ lÞ in x_ 2 ¼ A21 x1 þ A22 x2 þ ðBT BÞð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 ¼ ðBT BÞðu þ lÞ, the second row in (2.33) may be rewritten as x_ 2 ¼ A21 x1 þ A22 x2 þ v

ð2:34Þ

All components of the state vector are assumed measured 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_ ¼ x_ 2 þ Lx_ 1 ¼ Kðx2 þ Lx1 Þ ¼ Ks 0 s ¼ x2 þ Lx1 ! K>0

ð2:36Þ

Lx1 x2 t! !¥

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 x_ 1 ¼ ðA11 A12 LÞx1

ð2:37Þ

In Section 2.3 we will 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

43


v ¼ ðBT BÞðu þ lÞ ) u ¼ l þ ðBT BÞ 1 v u ¼ l ðBT BÞ 1 ½A21 x1 þ A22 x2 þ Lx_ 1 þ 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 x_ 1 ¼ f 1 ðx1 ; x2 Þ x_ 2 ¼ f 2 ðx1 ; x2 Þ þ B2 ðu þ lÞ

ð2:39Þ

Matrix B2 2 Rmm has full rank. By assigning a new control input v ¼ B2 ðu þ lÞ, the second row in Equation (2.39) may be written as x_ 2 ¼ f 2 ðx1 ; x2 Þ þ v and by selecting _ 1 Þ K½x2 þ sðx1 Þ; K > 0 v ¼ f 2 ðx1 ; x2 Þ sðx

ð2:40Þ

the dynamics of the closed loop will be reduced to _ 1 Þ ¼ K½x2 þ sðx1 Þ x_ 2 þ sðx 0 x2 þ sðx1 Þ ! K>0

ð2:41Þ

x2 t! sðx1 Þ !¥

By applying the methods of singularly perturbed systems, slow dynamics (2.39) and (2.41) can be found in the following form x_ 1 ¼ f 1 ½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 þ B2 1 v _ 1 Þ þ K½x2 þ sðx1 Þg u ¼ l B2 1 ff 2 ðx1 ; x2 Þ þ sðx

ð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.

44


2.2.2 Stability The question of stability arises in examining what happens near, but not at, the equilibrium. The question is can we 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 often means that for any bounded input over any amount of time, the output will also be bounded. This is known as BIBO stability. If a system is BIBO stable 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

45


_ 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 ð2:44Þ

qV dx _ 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 kxðtÞk g kxð0Þke ht ; t > 0. For LTI system x_ ¼ Ax, the Lyapunov function candidate can be selected as a quadratic form V ¼ xT Px. Here P ¼ PT > 0 is a positive definite symmetric matrix, thus V_ ¼ xT ðAT P þ PAÞx. The system x_ ¼ Ax is said to be stable if there exists a solution P of the so-called Lyapunov equation AT P þ 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 Rnn have a negative real part, then the equation AT P þ 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 from measurements ð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

AT CT

ðAT Þ2 CT . . . ðAT Þn 1 CT

ð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.

46


In further analysis we will 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Þ are known – 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 constructed 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 the constructed 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_ ¼ A^x þ 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_ ¼ A^x þ Bu þ Lðy ^yÞ

Figure 2.5

) ^_ ¼ ðA LCÞðx ^ ) x_ x xÞ

Structure of the state observer and its relation to the plant

ð2:47Þ

47


In Equation (2.47), matrix L 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 CT LT Þ has given eigenvalues. Note that in dealing with control we concluded that, for controllable systems, the eigenvalues of the matrix ðA BFÞ could be selected if the pair ðA; BÞ is controllable. It then follows that the eigenvalues of the matrix ðAT CT LT Þ can be replaced by an appropriate selection of matrix L if the pair ðAT ; CT Þ is controllable, 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

# ;

ð2:48Þ

C1 2 Rmðn mÞ ; C2 2 Rmm ; rank ðC2 Þ ¼ m

As shown in Equation (2.39), the transformation matrix T as in (2.49) would transform a LTI system ðA; B; CÞ into two subsystems (2.50) " T¼

#

In m

0

C1

C2 " TB ¼

" ;

B1 B2

A11

A12

A21 " # x1

A22

TAT 1 ¼ # ;

Tx ¼

# ; ð2:49Þ

y

dx1 ¼ A11 x1 þ A12 y þ B1 u dt dy ¼ A21 x1 þ A22 y þ B2 u dt

ð2:50Þ

In Equation (2.50) vector x1 2 Rðn mÞ1, x2 2 Rm1 and since rankðC2 Þ ¼ m from (2.48) follows x2 ¼ C2 1 ðy C1 x1 Þ. 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 controllability 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 8 z_ ¼ ðA11 þ LA21 Þz þ Az y þ ðB1 þ LB2 Þu > > < z ¼ x1 þ Ly ) y_ ¼ A21 z þ ðA22 LA21 Þy þ B2 u > > : Az ¼ ðA12 þ LA22 Þ ðA11 þ LA21 Þ

ð2:51Þ

48


Matrix L 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 system will 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 þ Az y þ ð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 ¼ C1 ^x1 þ C2 x2 ; rank ðC2 Þ ¼ m the remaining m components of the state vector can be determined as ^x2 ¼ C2 1 ½y C1 ð^z LyÞ. This demonstrates the effectiveness of using measurement information in lowering the order of the observer. The full order observer (2.46) is based on enforcing tracking of measured 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] x^_ 1 ¼ A11 ^x1 þ A12 ^y þ B1 u Lv ^y_ ¼ A21 ^x1 þ A22 ^y þ B2 u þ v

ð2:54Þ

Here v 2 Rm1 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 e_ x1 ¼ A11 ex1 þ A12 ey Lv e_ y ¼ A21 ex1 þ A22 ey þ v

ð2:55Þ

Assume that control v 2 Rm1 is enforcing ey ¼ 0 and e_ y ¼ 0. Then from e_ y ¼ 0 the control v ¼ veq can be derived as vj ey ¼ 0 ¼ A21 ex1 . Then the error in estimation of x1 2 Rðn mÞ1 e_ y ¼ 0 becomes e_ x1 ¼ A11 ex1 þ A12 ey Lveq ¼ ðA11 þ LA21 Þex1 y ¼ C1 ^x1 þ C2 ^x2 ) ^x2 ¼ C2 1 ðy C1 ^ x1 Þ

ð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)

49


with D ¼ 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 BF^x þ 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 ðA BFÞ BF x_ BFxref þ ¼ 0 ðA LCÞ e e_ 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 with matched 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_ ¼ Al l;

Al 2 Rmm ;

l 2 Rm1

ð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

#" # x

0

Al

l

" þ

B 0

# u;

y ¼ Cx þ 0l " # " # A B x ; z ¼ ; Az ¼ 0 Al l z_ ¼ Az z þ Bz u y ¼ Cz z

" Bz ¼

B 0

# ;

Cz ¼ ½ C

0

ð2:62Þ

50


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 ¼ Fz z þ Fr xref ¼ Fx x Fl l þ Fr xref

ð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 þ BFr xref l_ ¼ Al l

ð2:64Þ

y ¼ Cx þ 0l

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 Rmm , 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_ ¼ Az^z þ Bz u þ Lxy y # ^x_ ^x B Lx A B u þ þ ¼ ðy C^ xÞ ^ ^_ 0 Ll 0 Al l l ^x A Lx C B B Lx ¼ þ u þ y ^ Al l Ll 0 Ll C

ð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

#

¼

e_ x e_ l

¼

A Lx C Ll C

B 0

ex el

ð2:66Þ

With proper selection of matrices Lx and Ll both ex ! 0 and el ! 0, thus estimations tend to ^ þ Fr xref for systheir real values. Full state feedback u ¼ Fz^z þ Fr xref ¼ Fx ^x Fl l tem (2.59) yields x_ ¼ ðA BFx Þx þ ðB BFl Þl þ BðFx ex þ Fl el Þ þ BFr xref

ð2:67Þ

51


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 þ BFr xref .

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 ¼

uiþ ðx; tÞ si ðxÞ > 0 ui ðx; tÞ si ðxÞ < 0 i ¼ 1; . . . ; m

ð2:68Þ

x 2 Rn1 ; u 2 Rm1 ; h 2 Rp1 ; s 2 Rm1

It is assumed that fðxÞ; uiþ ; ui 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. The motion 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 substantiation of a so-called equivalent control method which provides a simple procedure for finding equations of motion in sliding mode.

52


Figure 2.6

Illustration of motion in the vicinity of the discontinuity surface

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Þ 2 Rmn qx

ð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Þ 1 Gð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Þ 1 Gð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Þ 1 G has very interesting properties with regard to matrices B; G

53


G½I BðGBÞ 1 G ¼ 0 ½I BðGBÞ 1 GB ¼ 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Þ 1 Gj. Sliding mode 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Þ ¼

sT s 2

ð2:73Þ

The stability requirement, for appropriate function zðV; s; xÞ > 0, is expressed as _ Vðs; xÞ ¼ s T 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Þ 1 Gð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Þ 1 MðxÞ sign ðsÞ; MðxÞ > 0 ½sign ðsÞT ¼ ½ sign ðs1 Þ . . . sign ðsm Þ ( 1 for si > 0 sign ðsi Þ ¼ 1 for si < 0

ð2:75Þ

_ Plugging the selected control into the derivative Vðs; xÞ yields _ Vðs; xÞ ¼ s T GBðueq ðGBÞ 1 MðxÞ sign ðsÞ ueq Þ ¼ MðxÞs T sign ðsÞ < 0

ð2:76Þ

The Lyapunov stability conditions (2.74) are satisfied and consequently a sliding mode exists in manifold sðxÞ ¼ 0. The dynamics of the system state are governed by Equation (2.71) and sðxÞ ¼ 0. From sðxÞ ¼ 0, one may determine m 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:

54 . .


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 2 Rn m

x_ 1 ¼ f 1 ðx1 ; x2 Þ

x_ 2 ¼ f 2 ðx1 ; x2 Þ þ B2 ðx1 ; x2 Þðu þ lÞ x2 2 Rm

ð2:77Þ

Matrix B2 2 Rmm is regular. The first equation is of ðn mÞ order and it does not depend on control. In this equation vector x2 may be handled as a virtual control. Assume that x*2 ¼ s 0 ðx1 Þ is selected such that x_ 1 ¼ f 1 ð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 x_ 1 ¼ f 1 ðx1 ; x*2 Þ ¼ f 1 ½x1 ; s 0 ðx1 Þ

ð2:78Þ

In the second step, discontinuous control u should be selected to enforce sliding mode in the manifold s ¼ x2 x*2

ð2:79Þ

¼ x2 þ s 0 ðx1 Þ ¼ 0

The projection of the system (2.77) motion in the manifold (2.79) is s_

¼ ðf 2 þ G0 f 1 þ B2 lÞ þ B2 u

G0 ¼

qs 0 qx1

ð2:80Þ

Stability in a manifold (2.79) will be enforced if control is selected as u

¼ ueq B2 1 Mðx1 ; x2 Þ sign ðsÞ;

ueq ¼

B2 1 ðf 2

Mðx1 ; x2 Þ > 0

þ G0 f 1 þ B2 lÞ

ð2:81Þ

Here Mð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 êq and matrix B2 are available, the structure of the control estimation of the equivalent control u may be modified to the form êq M * ðx1 ; x2 Þ sign ðsÞ; u¼u

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 discontinuous term M * ðx1 ; x2 Þ should now cover the error in the equivalent control estimation.

55


2.3.4 Control in Linear Systems Linear systems are most 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 Rnn ; B 2 Rnm ; D 2 Rnp

ð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 Rmm with rankðB2 Þ ¼ m. As shown in Equation (2.32), there exists a nonsingular transformation xT ¼ Tx such that " TB ¼

B1 B2

#

" ¼

#

0

ð2:84Þ

B2

and consequently the transformed system can be written in the following form x1 2 Rn m

x_ 1 ¼ A11 x1 þ A12 x2

x_ 2 ¼ A21 x1 þ A22 x2 þ B2 ðu þ lÞ x2 2 Rm ; u 2 Rm

ð2:85Þ

Here Aij ði; j ¼ 1; 2Þ are constant matrices and the pair ðA11 ; A12 Þ is controllable. As shown in 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 x_ 1 ¼ ðA11 A12 GÞx1 , G 2 Rmðn mÞ . In the next step a control should be selected to enforce the desired change of x*2 ¼ Gx1 rank G ¼ 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 aB2 1

ueq ¼

s ; ks k

l B2 1 ½ðA21

a > 0; ks k ¼

pffiffiffiffiffiffiffiffiffiffiffiffiffi ðs T sÞ ð2:87Þ

þ GA11 Þx1 þ ðA22 þ GA12 Þx2

Control (2.87) is continuous everywhere except in s ¼ 0. The closed loop transient is defined by x_ 1 ¼ ðA11 A12 GÞx1 and clearly depends on the selection of a manifold (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 A12 GÞ. This is an interesting feature of 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).

56


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 are well detailed in the available literature [3]. They are mostly 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 output x measured, f ðx; aÞ unknown continuous scalar function of 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 ^_ ¼ fo ð^ x; ao Þ þ u þ uo x

ð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 real plant 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. Let ^_ ¼ fx ð^ us construct a model x xÞ þ fa ð^ xÞuo þ u. If sliding mode is established on ^ ¼ 0 then x ¼ x ^ and consequently fx ð^ s ¼ xx 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 the measurement y 2 Rm1 and another describing the dynamics of the remaining ðn mÞ variables represented by x1 2 Rn m1. By expressing the measured variable estimation error as ey ¼ ^ y y the control input v 2 Rm1 can be selected to ensure a sliding mode

57


in the manifold ey ¼ ^ y y ¼ 0. The dynamics of the measured output control error (distance from sliding mode manifold) can be expressed as e_ y ¼ A21 ð^x1 x1 Þ þ A22 ey þ v

ð2:90Þ

Control input v ¼ Msignð^ y yÞ enforces a sliding mode in ey ¼ 0. Here M > 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 Þ ¼ A21 ex1 . Insertion veq into ^ x1 þ A12 ^ y þ B1 u Lveq yields an estimation error of x_ 1 ¼ A11 ^ n m1 x1 2 R e_ x1 ¼ A11 ex1 Lveq ¼ A11 ex1 Lð A21 ex1 Þ ¼ ðA11 þ LA21 Þex1

ð2:91Þ

In comparison with a continuous system framework design, here zero control error in tracking measurement 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 Rn1 ; u 2 Rm1 ; d 2 Rp ; rankðBÞ ¼ m; m < n

ð2:92Þ

The nominal plant dynamics is described by x^_ ¼ f 0 ð^x; a0 Þ þ Bu þ Buo ^x 2 Rn1 ; 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 Rm1 ; rankðGÞ ¼ m. If detðGBÞ 6¼ 0 then from sðG; x; ^xÞ ¼ 0 the equivalent control can be evaluated as uoeq ¼ ðGBÞ 1 GfDhðx; tÞ þ ½fðx; aÞ f 0 ð^ x; a0 Þg uoeq ¼ l þ ðGBÞ 1 GDfð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Þ 1 GÞfðx; aÞ BðGBÞ 1 Gf 0 ðx; aÞ þ Buc

ð2:95Þ

58


As result, the disturbance Dhðx; tÞ ¼ Bl is canceled. If fðx; aÞ is known and f 0 ð^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 f 0 ð^ x; a0 Þ ¼ 0 is selected, then GDfðx; aÞ ¼ Gfðx; aÞ and Equation (2.95) becomes x_ ¼ ðI BðGBÞ 1 GÞfðx; aÞ þ Buc

ð2:97Þ

If fðx; aÞ ¼ Bl þ jðx; aÞ, then Equation (2.97) yields x_ ¼ Buc þ ðI BðGBÞ 1 GÞ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 model (2.93). The matched plant disturbance and matched component of the vector fðx; aÞ can be 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_ ¼ f 0 ðx; a0 Þ þ B0 u. The real plant components can be expressed as fðx; aÞ ¼ f 0 ðx; a0 Þ þ Dfðx; aÞ, B ¼ B0 þ DB and by assumption, the unmeasured disturbance Dhðx; tÞ satisfies the matching conditions. Vector f 0 ð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^_ ¼ f 0 ð^x; a0 Þ þ B0 u þ B0 uo

ð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 Rm1 and rank ðGÞ ¼ m. The equivalent control can be determined as GB0 uoeq ¼ G½fðx; aÞ f 0 ð^x; a0 Þ þ GðB B0 Þu þ GDh uoeq ¼ ðGB0 Þ 1 GDfðx; aÞ þ ðGB0 Þ 1 GDBu þ ðGB0 Þ 1 GDh

ð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Þ ¼ B0 uc þ f 0 ð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


59

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 Kuo, B.C. (1982) Control Systems, Prentice Hall, Englwood Cliffs, New Jersey. Klailath, T. (1980) Linear Systems, Prentice Hall, Englwood Cliffs, New Jersey. Utkin, V.I. (1992) Sliding Modes in Control and Optimization, Springer-Verlag, New York. 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. 1. 2. 3. 4.

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., and Kokotovic, P.V. (1995) Nonlinear and Adaptive Control Design, John Wiley & 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 Motion Control 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 implementation issues will be treated in order to make a flavor of the complexities arising in




62


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 term environment is used for a body with which controlled system is in interaction. Making a mechanical system insert a specified force while in interaction with the environment. Making a mechanical system react as a specified combination of mass, spring and damper on an external force acting on the system. Enforcing a virtual interaction like the master–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 we will 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. The main topic in Chapter 5 is the control of motion in the presence of an interaction with 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 one of the central issues in design. In order to avoid a discussion of many different tasks and their peculiarities, in this chapter, design will be shown for a tracking problem in which output defined as a function of system coordinates is required to track its reference. Such a formulation allows within the same framework a design of control for problems in which 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 problem would open a way 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 task within 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 in Chapter 1, a mathematical model 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 ¼ τ 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

ð3:1Þ


64

Here q and q_ stand for the state variables – position and velocity respectively; aðqÞ is a continuous strictly positive bounded function amin aðqÞ amax , 8q representing inertia of _ bmax is a nonlinear bounded function representing Coriolis forces the system; bmin bðq; qÞ 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 some means according to demand – thus it can be interpreted as the control input to the system. The min and max 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, we will 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. Here we will use simplified models able to reflect the nature of interaction in terms of the system coordinates and the position of the body with which the system is interacting. A general secondorder 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 ¼

_ q€ ; qe ; q_ e ; q€ e Þ if there is interaction wðq; q; 0

Figure 3.1

if there is no interaction

Structure of the 1-dof system

ð3:2Þ

Acceleration Control

65

_ q€ ; qe ; q_ e ; q€ e Þ is a linear or nonlinear function of the system coordinates and Here wðq; q; the position qe , the velocity q_ e 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 τext in (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 system motion. 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 _ þ gðqÞ þ τext τd ¼ bðq; qÞ

ð3:3Þ

Here acceleration induced forces aðqÞ€ q are analyzed with respect to the control input τ _ þ gðqÞ and external forces and algebraic sum of all other known and unknown dynamics bðq; qÞ τ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 an stands 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 _ þ gðqÞ þ τext τdis ¼ DaðqÞ€ q þ bðq; qÞ

ð3:4Þ

¼ DaðqÞ€ q þ τd

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 variable while in (3.4) force DaðqÞ€ q is included in the generalized system input


66

Figure 3.2 disturbance

Representation of system (3.4) with constant system inertia and generalized input

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Þ€ q on system dynamics is compensated. Then, description (3.4) may acquire the physical meaning 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 or matched disturbances and, as shown in Chapter 2, they can be compensated by appropriate selection of the control input. At the same time one may look at external force as an additional input which may change the system motion in the same way 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 both models (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Þ€ q mot ¼ ðτ τd Þ and in (3.4) as an q€ mot ¼ ðτ τdis Þ, then models (3.3) and (3.4) can be 1 2 written in the following form q€ ¼ q€ mot i ; i ¼ 1; 2

ð3:5Þ

System dynamics are expressed as a double integrator with the acceleration q€ mot i ; 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.3 Representation of system (3.3) in (a) and system (3.4) in (b) and their common representation as a double integrator with acceleration as the input


67

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 q€ mot ¼ q€ des ; i ¼ 1; 2 by applying input forces. The linearity of the control plant in control input i and in disturbance allows application of the superposition principle in selecting the structure of additional forces inserted into the system by the controller. In mechanical 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 a way 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 the system and both can be changed by some means then either may change the state of the system in the same way. This means 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 cases may be best understood if we 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 position may 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.


68

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 a motion 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 to maintain different compliances for different control tasks. From a control 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 interaction) 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 some outputs, 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. That means, controlling a system to have the desired reaction on an external force while 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) (ii) (iii) (iv)

Linear or nonlinear continuous function of position yðqÞ, _ Linear or nonlinear continuous function of velocity yðqÞ, _ Linear or nonlinear continuous function of position and velocity yðq; qÞ, _ q€ Þ. Linear or nonlinear continuous function of position, velocity and acceleration yð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


69

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 the output of one system should track the output of another system, the reference will 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 the equilibrium state. Detailed requirements on these functions will be presented later in conjunction with particular control problems. In the motion control system design, we will 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, or manifold Sy ðy; yref Þ in system state space. For tasks (i)–(iv), the domain Sy ðy; yref Þ is defined as: .

if output is defined as function of position only _ ¼ q : eðqÞ ¼ yðqÞ yref ¼ 0 S1 ðq; qÞ

.

if output is defined as function of velocity only _ ¼ q_ : eðqÞ _ ¼ yðqÞ _ yref ¼ 0 S2 ðq; qÞ

.

ð3:7Þ

if output is defined as function of position and velocity _ ¼ q; q_ : eðq; qÞ _ ¼ yðq; qÞ _ yref ¼ 0 S3 ðq; qÞ

.

ð3:6Þ

ð3:8Þ

for output defined as function of position, velocity and acceleration _ ¼ q; q; _ q€ : eðq; q; _ q€ Þ ¼ yðq; q; _ q€ Þ yref ¼ 0 S4 ðq; qÞ

ð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

70


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 _ ¼ yref . Enforcement of (3.9) can be interpreted as control of operational constraints yðq; qÞ 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 specific manifold 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 output function 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, _ motion will be constrained to an elliptic trajectory in system state space ðq; qÞ, q ¼ q0 sinðvtÞ; q_ ¼ vq0 cos ðvtÞ. More complex trajectories may be obtained if yðqÞ is nonlinear. _ equilibrium solution eðq; _ yref Þ ¼ 0 means that yðqÞ _ tracks its reference. For output yðqÞ ref _ ¼ y should yield a unique solution for q_ ¼ qðy; _ yref Þ. The Equilibrium solution yðqÞ velocity 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, _ are constrained to satisfy equation yðq; qÞ _ ¼ yref . The position is detercoordinates ðq; qÞ ref _ ¼ y . In this case, actual dynamics mined as a solution of the differential equation yðq; qÞ 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 _ q€ Þ ¼ 0. Function yðq; q; _ q€ Þ should guarantee a unique solution for the acceleration eðq; q; _ q€ Þ ¼ yref . The position is then determined from the solution of the second order from yðq; q; _ q€ Þ ¼ yref . The simplest case is if yðq; q; _ q€ Þ is linear then differential equation yðq; q; 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), system motion is constrained to the manifold in state space. In order to fully describe the behavior of the system the dynamics of the constraint equations must be taken into account along with 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 acceleration 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.


71

This suggests a two-step design. In the first step, the desired acceleration q€ des 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 q€ des in the closed loop system 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 q€ des is known. Inserting the known desired acceleration q€ des into (3.3) and (3.4) yields the control force in the following forms τ ¼ τd þ aðqÞ€ q des

ð3:10Þ

τ ¼ τdis þ an q€ des

The applied force has two components – the disturbance τd or τdis and the force induced by the desired acceleration aðqÞ€ q des or an q€ des. The first expression is used in plants with a known inertia 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€ ¼ q€ des, 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 the motion 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 _ ) e€ðqÞ ¼ c_ q_ þ c€ S1 ðq; qÞ q y€ref ;

.

c¼

qyðqÞ 6¼ 0 qq

ð3:11Þ

for output defined as function of velocity only _ ) e_ ðqÞ _ ¼ c1 q€ y_ ref ; S2 ðq; qÞ

c1 ¼

_ qyðqÞ 6¼ 0 qq_

ð3:12Þ


72

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 þ an q€des

.

for output defined as function of position and velocity _ ) e_ ðq; qÞ _ ¼ c2 q_ þ c3 q€ y_ ref ; S3 ðq; qÞ

.

c2 ¼

_ qyðq; qÞ 6¼ 0; qq

c3 ¼

_ qyðq; qÞ 6¼ 0 qq_

ð3:13Þ

for output defined as function of position, velocity and acceleration _ ) eðq; q; _ q€ Þ ¼ yðq; q; _ q€ Þ yref S4 ðq; qÞ

ð3:14Þ

Here ci 6¼ 0; i ¼ 1; 2; 3 are assumed constants or known continuous functions of time or plant _ q€ Þ is coordinates. Equations (3.11)–(3.14) are linear in acceleration. If the function yðq; q; _ ¼ yðq; q; _ q€ Þ; c4 6¼ 0 where linear in acceleration, then it can be expressed as c4 q€ þ y1 ðq; qÞ 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 in which 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: . .

_ q€ Þ and its reference yref to be continuous, if relative degree is zero, we assume function yðq; q; _ or yðqÞ _ and its reference yref must be continuous if relative degree is one, function yðq; qÞ along with their first-order time derivatives,

Acceleration Control .

73

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 convergence to equilibrium solution if initial conditions are not consistent with equilibrium ejt¼0 6¼ 0. The first term is the so-called equivalent acceleration q€ eq . The force [determined from Equation (3.10) with q€ des ¼ q€ eq ] enforcing equivalent acceleration will be called equivalent force τeq .

Figure 3.5 Output error dynamics (3.11)–(3.14) for a system with 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)


74

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 _ _ ¼ const and deðq; qÞ=dt _ _ ¼ const, respectively – error deðqÞ=dt ¼ 0 ) eðqÞ ¼ 0 ) eðq; qÞ thus the motion will remain equidistant from the equilibrium solution. _ q€ Þ ¼ 0. It will set the From Equation (3.14) acceleration can be derived directly from eðq; q; 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 error will exhibit a different behavior – from being 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 solution would allow all these problems to be treated in the same way. In addition, it may allow the development of a unified control system design procedure valid for all of these control problems. What we would like to do is to find a way 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Þ e_ 1 ðqÞ ¼ c_ q_ þ c€ q y€ref

ð3:15Þ

As discussed in Chapter 2, in such systems one can treat e1 ðqÞ as a virtual control input in the first equation of (3.15) and select it to ensure the desired behavior of the control error e. Let eref 1 ¼ 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 s_ z ¼ e_ 1 z_ ¼ c_ q€ þ cq_ y€ref z_ ðqÞ

ð3:16Þ


75

_ For initial conditions sz ½qð0Þ; qð0Þ ¼ 0, acceleration q€ eq ¼ c 1 ½€ y ref 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. _ ¼ yðq; q; _ q€ Þ; c4 6¼ 0, we For the control problems with a zero relative degree, c4 q€ þ y1 ðq; qÞ _ þ yref holds. Then for constant c4 6¼ 0 can define a new variable z such that z_ 1 ¼ y1 ðq; qÞ the dynamics of the tracking error ez ¼ c4 q_ z1 can be expressed in the following form _ yref e_ z ¼ c4 q€ z_ 1 ¼ c4 q€ þ y1 ðq; qÞ

ð3:17Þ

The relative degree in (3.17) is one and the acceleration derived from e_ z ¼ 0 is equal to the _ q€ Þ ¼ 0. This can be confirmed by inspection of (3.17) acceleration derived from eðq; q; 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 q€ eq . 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) s_ z ðtÞ ¼ 0 ) q€ eq ¼

.

c_ q_ z_ y€ref c

for control output defined as function of velocity only with error dynamics as in (3.12) _ ¼ 0 ) q€ eq ¼ e_ ðqÞ

.

y_ ref c1

ð3:19Þ

for control output defined as function of velocity and position with error dynamics as in (3.13) _ ¼ 0 ) q€ eq ¼ e_ ðq; qÞ

.

ð3:18Þ

c2 q_ y_ ref c3

ð3:20Þ

for control output defined as function of position, velocity and acceleration with augmented error dynamics as in (3.17) e_ z ¼ 0 ) q€ eq ¼

_ yref y1 ðq; qÞ c4

ð3:21Þ


76

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 q€ eq Þ; 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 in in in

(3.16) (3.12) (3.13) (3.17)

s s s s

¼ sz ¼ e_ zðeÞ and g ¼ c _ and g ¼ c1 ¼ eðqÞ _ and g ¼ c3 ¼ eðq; qÞ ¼ 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 solution s ¼ 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 q€ eq 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Þjtt0 ¼ q€ eq ð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)


77

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)

Assume g and q€ eq are known. Let the desired rate of change of the distance from manifold s ¼ 0 is specified as s_ ¼ s_ des

ð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 q€ des ¼ q€ eq þ g 1 s_ des

ð3:24Þ

The desired acceleration (3.24) enforces the dynamics of the distance from the equilibrium solution is reduced to a single integrator with s_ des 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 s_ des 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. _ þ gðqÞ þ τext Þ ¼ τ aðqÞ€ q þ ðbðq; qÞ |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl ffl}

ð3:25Þ

system disturbance¼τd

Here variables have the following meaning and, depending on the motion (translational or rotational), are expressed in appropriate units: . . .

position q (in m or rad), velocity q_ (in m/s or rad/s), acceleration q€ (in m/s2 or rad/s2).

System parameters are selected as follows and, depending on the translational or rotational motion, are expressed in the following units:


78

. . . .

inertia coefficient aðqÞ ¼ an ½1 þ a1 sin ðqÞ, in kg or kg m2, _ ¼ b0 q þ b1 q_ þ an a1 q_ cos ðqÞ, in N or Nm, nonlinear forces bðq; qÞ nonlinear force gðqÞ ¼ g0 q2 , in N or Nm, external force, in N or Nm, τext ¼

τ0 ½1 þ cos ðvτ tÞ þ sin ð3vτ tÞ; 0

0:12 < t < 0:8 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 Nm), 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 _ þ gðqÞ þ τext ¼ τ an q€ þ Da€ q þ bðq; qÞ _ þ gðqÞ þ τext q þ bðq; qÞ τdis ¼ Da€

ð3:26Þ

In modeling 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 _ ¼ cðqÞq þ c1 q_ yðq; 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. _ is considered just as a variable representing a given In all examples the output yðq; qÞ functional relationship without any physical meaning, thus having no unit associated with it.


79

The reference input yref ðtÞ is assumed a bounded continuous function of time ref yref ðtÞ ¼ yref 0 ½1 þ y1 sin ðvr tÞ

ð3:28Þ

ref Coefficients yref 0 ; y1 ; 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Þ. _ yref ðtÞ are The dynamics of the output error ey ¼ yðq; qÞ ref e_ y ¼ cðqÞq_ þ c_ ðqÞq þ c1 q€ yref 0 y1 vr cos ðvr tÞ

ð3:29Þ

Solving e_ ¼ 0 for q€ ¼ q€ eq yields equivalent acceleration ref _ c_ ðqÞq q ¼ q€ eq Þ ¼ 0 ) q€ eq ¼ c1 1 ½yref e_ y ð€ 0 y1 vr cos ðvr tÞ cðqÞq

ð3:30Þ

In this chapter, as the plant outputs we will consider the following functions of position and velocity _ ¼ cq þ c1 q_ yl ðq; qÞ

ð3:31Þ

_ ¼ ðcjqjÞ q þ c1 q_ yn ðq; qÞ

ð3:32Þ

_ ¼ c½1 þ 0:8 sin ðqÞ q þ c1 q_ yn1 ðq; qÞ

ð3:33Þ

For these outputs and selected control error, the equivalent acceleration is 1 ref ref _ q€ eq l ¼ c1 ½y0 y1 vr cos ðvr tÞ cq

ð3:34Þ

1 ref ref _ q€ eq n ¼ c1 ½y0 y1 vr cos ðvr tÞ 2cjqjq

ð3:35Þ

1 ref ref _ 0:8cqq_ cos ðqÞg q€ eq n1 ¼ c1 fy0 y1 vr cos ðvr tÞ c½1 þ 0:8 sin ðqÞq

ð3:36Þ

As an illustration, plant (3.25) with the parameters listed above is simulated for outputs (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 of acceleration 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Þ€ q eq where τd and aðqÞ are eq assumed known and q€ 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Þ and _ are determined from the equation yl ðq; qÞ _ ¼ cq þ c1 q_ ¼ 24 þ yref ðtÞ – thus it is velocity qðtÞ 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 e_ y ð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 þ 5q_ ¼ 24 þ yref ðtÞ] and the difference between these two variables


80

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 q€ eq and disturbance τd for plant (3.25) with output (3.31) and the equivalent acceleration q€ eq 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Þ

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 þ 5q. In the third row, the equivalent acceleration q€ eq 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 _ rad/s2}. q€ eq l ¼ 0:2½35 12:56 cos ð12:56tÞ 50q 3.2.2.2 Equivalent Force Insertion of acceleration derived from (3.1) into (3.22) yields 8 9 > > > > = g < _ þ gðqÞ þ τext aðqÞ€ s_ ¼ q eq ; g 6¼ 0 τ ½bðq; qÞ > > aðqÞ > > : ; |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} τd

s_ ¼ ga 1 ðqÞðτ τeq Þ

ð3:37Þ


81

Here τeq stands for the value of the input force for which the rate of change of the distance from 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 _ þ gðqÞ þ τext þ aðqÞ€ τeq ¼ ½bðq; qÞ q eq ¼ τd þ aðqÞ€ q eq

ð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) _ þ gðqÞ þ τext Þ aðqÞc 1 ð_cq_ z_ y€ref Þ s_ z ¼ 0 ) τeq ¼ ðbðq; qÞ

.

for control tasks defined as function of velocity only _ þ gðqÞ þ τext þ aðqÞc1 1 y_ ref _ ¼ 0 ) τeq ¼ ½bðq; qÞ e_ ðqÞ

.

ð3:40Þ

for control tasks defined as function of position and velocity _ þ gðqÞ þ τext aðqÞc3 1 ðc2 q_ y_ ref Þ _ ¼ 0 ) τeq ¼ ½bðq; qÞ e_ ðq; qÞ

.

ð3:39Þ

ð3:41Þ

for control tasks with error dynamics as in (3.17) _ q€ eq Þ ¼ 0 ) τeq ¼ ½bðq; qÞ _ þ gðqÞ þ τext þ aðqÞc4 1 ½yref y1 ðq; qÞ _ e_ 1 ð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 q€ eq _ þ gðqÞ þ τext τdis ¼ ½DaðqÞ€ q þ bðq; qÞ

ð3:43Þ

The difference from (3.38) is in force induced by the variable part of inertia Dað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 ¼ 0 will guarantee s ¼ 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 s_ des

ð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.


82

Figure 3.9 The control loop for system (3.1) and equivalent structure

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 s_ des 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 q€ eq _ þ gðqÞ þ τext ¼ Da€ q þ bðq; qÞ

ð3:45Þ

ref _ c_ ðqÞq þ an c1 1 ½yref 0 y1 vr cosðvr tÞ cðqÞq

Here τdis is generalized disturbance (3.26). In this example the outputs and control errors are selected as _ eyn ¼ yn yref _ ¼ ðcjqjÞq þ c1 q; yn ðq; qÞ

ð3:46Þ

_ ¼ cð1 þ 0:8 sin ðqÞÞq þ c1 q; _ eynl ¼ ynl yref yn1 ðq; qÞ

ð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 þ 5q_ ¼ 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Þ.


83

For outputs (3.46) and (3.47) end selected control errors the equivalent torque is as in (3.48) and (3.49) respectively. τeq q eq n n ¼ τdis þ 0:1€

ð3:48Þ

_ Nm ¼ τdis þ 0:02½35 12:56 cos ð12:56tÞ 100jqjq q eq τeq n1 ¼ τdis þ 0:1€ n1 ¼ τdis þ 0:02

35 12:56 cos ð12:56tÞ 50ð1 þ 0:8 sin ðqÞÞq_ 40qq_ 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. _ ¼ ð50jqjÞq þ 5q_ with initial Figure 3.10 illustrates system behavior for output yn ðq; qÞ 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 þ 5q_ ¼ 11:5 þ yref ðtÞ), equivalent torque τeq and _ ¼ ð50jqjÞq þ 5q_ and the equivalent torque as the disturbance τd is for plant (3.25) with output yn ðq; qÞ 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Þ

84


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 þ 5q_ ¼ 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 behavior is 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 is _ ¼ 33:488 þ yref ðtÞ and the difference between the solution of the differential equation yn1 ðq; qÞ these 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 þ 5q_ ¼ 33:488 þ yref ðtÞÞ, equivalent torque τeq and disturbance τd for plant (3.25) with output (3.47) and the equivalent torque as the 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Þ


85

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 s_ des needs to be selected. It is easy to verify that selection s_ des ¼ ks; k > 0 in (3.23) enforces convergence to the equilibrium solution s ¼ 0 and the stability of the equilibrium. In this section, we would 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 > 0; 2

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_ ¼ ss_ ¼ sgð€ q q€ eq Þ ¼ sga 1 ðqÞðτ τeq Þ

ð3:51Þ

_ q eq Þ ¼ Vðτ _ eq Þ ¼ 0, thus For q€ ¼ q€ eq or τ ¼ τeq ; the Laypunov function derivative is Vð€ eq eq _ Vð€ q Þ and Vðτ Þ 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_ ¼ ss_ lðVÞ

ð3:52Þ

By selecting control from V_ ¼ lðVÞ; a specific convergence of the Lyapunov function is enforced. 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 be selected 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 ss_ þ lðVÞ sgð€ q q€ eq Þ þ lðVÞ 0 ½ gs€ q eq þ lðVÞ þ gs€ q0

ð3:53Þ


86

.

for force as the control input ss_ þ lðVÞ s

g ðτ τeq Þ þ lðVÞ 0 aðqÞ

g g sτeq þ lðVÞ þ sτ 0 aðqÞ aðqÞ

ð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Þ

For s 6¼ 0, by inserting (3.50) and (3.24) into (3.55) yields the acceleration enforcing (3.55) as q€ ¼ q€ eq q€

con

1 lðVÞ sign ðsÞ ¼ q€ eq þ q€ con g jsj

1 lðVÞ sign ðsÞ; ¼ g jsj

ð3:56Þ s 6¼ 0

The acceleration (3.56) enforces the convergence to manifold s ¼ 0. It is a sum of equivalent acceleration q€ eq and the q€ con . Since equivalent acceleration enforces no changes _ q eq Þ ¼ ssð€ in the Lyapunov function derivative (sð€ _ q ¼ q€ eq Þ ¼ 0 and Vð€ _ q eq Þ ¼ 0) the concon vergence must be enforced by q€ , 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 q€ eq , thus the influence of q€ con 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 τcon ¼

aðqÞ lðVÞ ¼ τeq þ τcon ¼ τeq þ aðqÞ€ q con gs

aðqÞ lðVÞ sign ðsÞ; g js j

ð3:57Þ s 6¼ 0

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 q€ con and the convergence force τcon (or at least their average values) vanish when motion reaches the equilibrium solution s ¼ 0.


87

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 q€ con and τcon have finite value as jsj ! 0. From (3.56) the convergence acceleration q€ con is bounded if there exists a strictly positive K > 0 such that, for s 6¼ 0 0 lðVÞ Kjgjjsj

ð3:58Þ

If the rate of change of the Lyapunov function satisfies (3.58) then q€ con K sign ðgsÞ and τ ¼ aðqÞ€ q con . For a finite error both q€ con 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 con

V_ lðVÞ ¼ k2a V a ;

k > 0;

1 a1 2

ð3:59Þ

Integration of V_ hV a ; h ¼ 2a k > 0 over the ½0; t interval via separation of variables yields for a 6¼ 1 V 1 a ðtÞ V 1 a ð0Þð1 aÞht

ð3:60Þ

For a ¼ 1; integration of V_ hV a ; h ¼ 2a k > 0 over the ½0; t interval yields VðtÞ Vð0Þe ht

Thus, for

1= 2

ð3:61Þ

a 1, the convergence VðtÞ ! 0 is guaranteed. t!¥

From (3.60), finite-time convergence is achieved for 1=2 a < 1; thus s reaches zero value in time tr tr V 1 a ð0Þ=hð1 aÞ

ð3:62Þ

For 1=2 ¼ a the fastest finite-time convergence is achieved tr 21 a V 1 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 convergence acceleration the convergence coefficient a of the Lyapunov function candidate must be selected.


88

The control system specification introduced in Section 3.2 can be satisfied if exponential convergence is guaranteed. Because of this, we will 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 q€ eq Þ þ 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Þ ! 0 from initial state consistent with operational t!¥ bounds sð0Þ 6¼ 0, to the equilibrium sðtÞ ¼ 0, is obtained from (3.64) as q€ ¼ q€ des ¼ q€ eq kg 1 s ¼ q€ eq þ q€ con q€ con ¼ kg 1 s

ð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 s_ des ¼ kg 1 s is selected, the convergence term is consistent with stability enforcement in (3.24). It is also consistent with (3.56). If jsj is bounded 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

des

q€ q€ þ k g 1 s q€ eq max

ð3:67Þ

The force that enforces the desired acceleration (3.65) is τ ¼ τd þ a€ q eq akg 1 s ¼ τeq akg 1 s ¼ τeq þ τcon con

τ

¼ a€ q conv ¼ akg 1 s

ð3:68Þ

Here τeq stands for the equivalent force and τcon stands for the convergence force. Further decomposition on the disturbance, the equivalent acceleration and the convergence acceleration 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, q€ eq or τeq, the control component enforcing the convergence to the equilibrium q€ con or τcon.


89

Figure 3.12 Illustration of the control forces and the motion enforced in a closed loop

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 q€ des ¼ q€ eq þ q€ con . Example 3.1 has shown the system behavior with equivalent acceleration as input. It enforces a constant control error. The convergence acceleration q€ con is selected to enforce specific dynamics of the output control error. The desired dynamics of the control error are e_ y þ 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€ q des ¼ ðτdis þ 0:1€ q eq Þ þ 0:1€ q con ¼ τeq ey

ð3:70Þ


90

With control input (3.70), the output tracking error is expected to converge to zero on the trajectory e_ y ¼ 50ey in the ðey ; e_ y Þ plane. _ plane, behavior is more complex and depends on the structure of the output In the ðq; qÞ function and it should be evaluated for each output separately. For a fast convergence rate, the duration of motion along line e_ y ¼ 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_ ¼ yref ðtÞ and can imation. For ey ðtÞ ¼ 0 change in position and velocity is governed by yðq qÞ be 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. _ ¼ 50½1 þ 0:8 sin ðqÞq þ 5q_ to track Figure 3.13 illustrates control of the output yn1 ðq; qÞ 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 _ ¼ yref ðtÞ] and the difference reference qref [solution of the differential equation yn1 ðq; qÞ 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 τ, the equivalent torque τeq , the convergence torque τcon ¼ an q€ con and the e_ y ð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 e_ y ðey Þ diagram illustrates the enforcement of the transient e_ y þ 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 ¼ fq; q_ : s_ þ ks ¼ 0; τmin τ < τmax g

ð3:71Þ

Here, τmin and τmax are lower and upper bounds of input force. For simplicity we will assume jτmin j ¼ jτmax 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


91

Figure 3.13 Changes in reference yref , output y, output error ey, position q, reference position qref {solution of linear differential equation 50½1 þ 0:8 sin ðqÞq þ 5q_ ¼ yref ðtÞ}, equivalent torque τeq , convergence torque τcon and control input τ. The output is yn1 ¼ 50 ½1 þ 0:8 sin ðqÞ q þ 5q_ with an exponential convergence of the output error e_ y þ 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

reference is selected consistent with control input resources, thus if the equilibrium is reached the equivalent force needed to maintain equilibrium satisfies jτeq j < M. This implies that plant generalized disturbance force satisfies jτdis j < M. To take available control resources into account, the control inputs (3.65) and (3.68) can be written in the following form q€ des ¼ satð€ q eq kg 1 sÞ q eq Þ akg 1 s ¼ satðτeq akg 1 sÞ τ ¼ sat½ðτd þ a€

ð3:72Þ

Here satð Þ stands for the saturation function: 8 > <x satðxÞ ¼ L x > : jxj

if

jxj < L

if

jxj L

ð3:73Þ


92

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_ ¼

g g τ ðτ τeq Þ ¼ M τeq aðqÞ aðqÞ jτj

ð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 convergence force input. Inserting (3.73) yields g τ M g eq M eq M τ τ ks ¼ 1 s_ ¼ aðqÞ jτj jτj aðqÞ jτj

ð3:75Þ

By adding ks to both sides, (3.75) can be rearranged into M g eq M g τ aðqÞg 1 ks ¼ 1 τ s_ þ ks ¼ 1 jτj aðqÞ jτj aðqÞ

ð3:76Þ

! 0. The time period during which control Motion is directed towards domain s_ þ ksjτj¼M 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 disturbances. 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 q€ des ¼ satð€ q eq kg 1 sÞ τ ¼ sat½ðτdis þ an q€ eq Þ an g 1 ks ¼ satðτeq an g 1 ksÞ

ð3:77Þ

Bounded control input enforces convergence to domain (3.71) in which the condition jτeq 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 of the 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 system with 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 in Example 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


93

convergence force are applied while equivalent acceleration and equivalent force as components of the control input are not separately limited. In order to facilitate these requirements, the control input is implemented as τ ¼ satT ½ðτdis þ an q€ eq Þ þ satTC ðan q€ con Þ

ð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 jτcon ¼ an q€ con jmax ¼ 4 N:m. The control is selected to enforce control error dynamics e_ y ¼ key ; k ¼ 50, thus the convergence torque is selected as τcon ¼ an kc1 1 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 > jτdis j and τ ¼ τmax sign ðτÞ. Motion is directed towards the domain jτj < jτmax j and, after reaching it, motion in the ðey ; e_ y Þ plane is constrained to line e_ y ¼ key ; k > 0. In Figure 3.14, simulations are shown for output yl ¼ 50q þ 5q_ 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 _ ¼ yref ðtÞ] and the difference between these two variables eq are shown. equation yn1 ðq; qÞ In the third row, the input torque τ, the equivalent torque τeq , the convergence torque τcon ¼ an q€ con and the e_ y ð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 ; e_ y Þ plane. Motion along line e_ y ¼ 50ey has two segments – in the initial stage the control input needed to maintain e_ y ¼ 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 < jτmax j. After reaching jτj ¼ τmax sign ðτÞ motion along e_ y ¼ 50ey requires higher resources, thus it cannot be enforced. The motion control input jτj ¼ jτmax j deviates from e_ y ¼ 50ey until resources are sufficient to enforce such a motion. After reaching a region in which e_ y ¼ key and jτj jτmax 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 e_ y ¼ key . In Figure 3.15, the same diagrams as in Figure 3.14 are shown. Output is selected as _ ¼ ð50jqjÞq þ 5q. _ Here, both the input torque and the nonlinear function of position yn ðq; qÞ convergence torque are limited on jτjmax ¼ 10 N:m and jτcon jmax ¼ 4 N:m; respectively. In this case 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 saturated but the convergence torque is saturated and for 0:17 s t 0:25 s motion in ðey ; e_ y Þ is along e_ y ¼ const. For t 0:25 s both input and convergence torques are not bounded and motion is in

94


Figure 3.14 Changes in reference yref , output y, output error ey, position q, reference position qref [solution of linear differential equation 50q þ 5q_ ¼ yref ðtÞ], input torque τ, equivalent torque τeq , convergence torque τcon and e_ y ðey Þ diagram. Output is defined as yl ¼ 50q þ 5q_ 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 is enforced. Reference output is yref ðtÞ ¼ 1 þ 35 sin ð12:56tÞ

domain (3.71). The motion along trajectory e_ y þ 50ey ¼ 0 is enforced for

ey τcon ðan kc 1 Þ 1 ¼ 4. The diagrams illustrate dependence of the reaching transient max 1 on the available control resources. For given resources, the region in which desired motion e_ y þ 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 kV a ; a ¼ 1. Similarly, the stability conditions V_ ¼ 2a kV a ; 1 2 a < 1 enforcing finite-time convergence to the equilibrium s ¼ 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


95

Figure 3.15 Changes in reference yref , output y, output error ey, position q, reference position qref [solution of linear differential equation ð50jqjÞq þ 5q_ ¼ yref ðtÞ], input torque τ, equivalent torque τeq , _ ¼ ð50jqjÞq þ 5q_ and converconvergence torque τcon and e_ y ðey Þ diagram. Output is defined as yn ðq; qÞ gence gain is k ¼ 50. The limit on input torque is jτjmax ¼ 10 N:m. The limit on the acceleration torque is jτcon jmax ¼ 4 N:m

some of the solutions may yield imaginary values if s < 0. For simplicity, values of a which 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_ ¼ ss_ ¼ 2a kV a ) s½s_ þ kjsj2a 1 signðsÞ ¼ 0; s6¼0

1 a 0 sign s ¼ 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 discontinuous 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 1 sign ðsÞ can be removed and the control (3.82) with kg 1 jsjb sign ðsÞ; 0 < b < 1 instead of kg 1 sign ðsÞ can be directly applied. In this case s_ ¼ kjsjb sign ð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 q€ eq ¼ τeq

ð3:83Þ


97

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 in manifold s ¼ 0. The control (3.81) is continuous while control in (3.82) or (3.83) are discontinuous. The application of sliding mode 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 q€ eq þ an q€ con with convergence torque

2a 1 τcon ¼ an q€ con ¼ an kc1 1 ey

signðey Þ;

k > 0;

1 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Þ þ ke_ ð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Þ ¼ k1 e 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. _ the closed loop dynamics is For generalized error s ¼ eðqÞ _ þ keðqÞ _ ¼ 0; e_ ðqÞ

k>0

ð3:88Þ

Consequently, the generalized error converges to zero s ! 0. The equilibrium solution for t!¥ _ ¼ yref and selection of output function yðqÞ _ should velocity is then determined from yðqÞ 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 ¼ c2 q þ c3 q_ _ yref into (3.86) with c2 ; c3 > 0. Let the reference output be yref . Insertion of the s ¼ yðq; qÞ yields c3 q€ þ ðc2 þ kc3 Þq_ þ kc2 q ¼ y_ ref þ 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 Example 3.3 for unbounded control input and in Example 3.4 for bounded control input.


100

Inserting s ¼ ez from (3.17) into (3.86) yields e_ z þ 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 _ yref ¼ 0. The selection of output function yð€ _ qÞ is then determined by c4 q€ þ y1 ðq; qÞ q ; q; _ qÞ ¼ yref guarantees the stability and is restricted by the requirements that equation yð€ q ; q; 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 finitetime 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 sliding mode [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 _ þ gðqÞ þ τext ¼ τeq aðqÞ€ q þ bðq; qÞ

)

s¼0

ð3:91Þ

Insertion of (3.38) into (3.91) yields aðqÞ€ q ¼ aðqÞ€ q eq

)

s¼0

Insertion of the equivalent acceleration from (3.18)–(3.21) into (3.92) yields:

ð3:92Þ

Acceleration Control .

101

for control output with dynamics of augmented error (3.16) q€ ¼ c 1 f_cq_ z_ ½eðqÞ y€ref g;

.

_ ¼ yðqÞ _ yref ¼ 0 s ¼ eðqÞ

ð3:94Þ

for control output defined as a function of velocity and position with error dynamics as in (3.13) q€ ¼ c3 1 ðc2 q_ y_ ref Þ;

.

ð3:93Þ

for control output defined as a function of velocity only with error dynamics as in (3.12) q€ ¼ c1 1 y_ ref ;

.

s ¼ e_ ðqÞ z½eðqÞ ¼ 0

_ ¼ yðq; qÞ _ yref ¼ 0 s ¼ eðq; qÞ

ð3:95Þ

for control output defined as a function of position, velocity and acceleration with augmented error dynamics as in (3.17) _ q€ ¼ c4 1 ½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 ¼ 0; dt

s¼0

ð3:97Þ

Thus the dynamics for all of the analyzed problems (3.6)–(3.9), if manifold s ¼ 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 _ ¼ yðqÞ _ yref ¼ 0 eðqÞ

in ð3:7Þ

_ ¼ yðq; qÞ _ yref ¼ 0 eðq; qÞ

in ð3:8Þ

_ y _ qÞ ¼ c4 q€ þ y1 ðq; qÞ eð€ q ; q;

ref

ð3:99Þ

¼ 0 in ð3:9Þ

That defines the properties of the output function to be continuous and differentiable for _ ¼ yðqÞ _ yref ¼ 0 the velocity is the solution of an appropriate number of times. For eðqÞ


102

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 the Closed Loop Dynamics for Different Convergence 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 q€ con ¼ 10ey ¼ 10ðy yref Þ

ð3:100Þ

0:5 q€ con ¼ 10 ey signðey Þ

ð3:101Þ

for finite-time convergence

The closed loop motion of the system is expected to satisfy: .

.

for exponential convergence e_ y þ 50ey ¼ 0

ð3:102Þ

0:5 e_ y þ 50 ey sign ðey Þ ¼ 0

ð3:103Þ

for finite-time convergence

The convergence time is an important feature of the closed loop. For linear dependence of output in both position and velocity yl ¼ 50q þ 5q_ 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 convergence 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.


103

Figure 3.17 Changes in reference yref , output y, output error ey, e_ y ðey Þ diagram (in the left column) for _ ¼ 50q þ 5q_ and the reference yref ðtÞ ¼ ½1 þ 35 sin ð12:56tÞ, position q, reference the output yl ðq; qÞ position qref (solution of differential equation 50q þ 5q_ ¼ yref ðtÞ), position error eq and e_ q ðeq Þ diagram (in the right column). The convergence gain is k ¼ 50 and a ¼ 1

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:75 is 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 þ 5q_ and k ¼ 50 for exponential convergence a ¼ 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 þ 5q_ ¼ yref ðtÞ, thus from y ¼ 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 e_ y ðey Þ diagram is a straight line with slope k ¼ 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.

104


Figure 3.18 Changes in reference yref , output y, output error ey, e_ y ðey Þ diagram for the output _ ¼ 50q þ 5q_ and the reference yref ðtÞ ¼ ½1 þ 35 sin ð12:56tÞ are shown in the left column. The yl ðq; qÞ right column shows changes in position q, reference position qref [solution of differential equation 50q þ 5q_ ¼ yref ðtÞ], position error eq and e_ q ðeq Þ diagram. The convergence gain is k ¼ 50 and a ¼ 0:75

That is clearly illustrated in both time change of eq and the e_ q ðeq Þ diagram. The reaching time try ¼ 0:1 s is equal to the convergence time for output tracking error. For try > 0:1 s the motion 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 The e_ y ðey Þ

predicted.

0:5 diagram illustrates that motion constrained to the curve e_ y þ 50 ey signð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 þ 5q_ yref ðtÞ ¼ 0; thus the changes in position are governed by solution 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


105

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 specifications along with proper selection of the references include most if not all common problems in control of mechanical 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 q€ des – is selected as the sum of the equivalent acceleration q€ eq and the convergence acceleration q€ con q€

des

¼ sat

equivalent

þ

acceleration

convergence

ð3:104Þ

acceleration

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 specified by the desired convergence law. The saturation function in (3.104) just reflects the fact that the control input is bounded, thus the desired motion can be enforced in the bounded domain within the system state space. The force that enforces the desired acceleration (3.104) in system (3.1) is

τ ¼ sat

8 > > > : disturbance

þq

9 > > =

equivalent convergence þ ; > acceleration acceleration ; |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} >

q ¼ aðqÞ

or

an

desired acceleraion

ð3:105Þ

The disturbance is added to the desired acceleration (3.104) as a way to compensate system disturbance and make 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.


106

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

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 candidate VðsÞ depends on both position and velocity and convergence acceleration depends only on the selection of the derivative V_ ¼ lðVÞ of the Lyapunov function and can be expressed as

convergence acceleration

¼ q€ con ¼

1 lðVÞ sign ðsÞ; g js j

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 kV a ;1=2 a < 1 leads to the finite-time convergence to the equilibrium. Selection lðVÞ ¼ 2a kV a ;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 q€ des ðsÞ _ þ gðqÞ þ τext þ an q€ des ðsÞ ¼ ½DaðqÞ€ q þ bðq; qÞ

ð3:107Þ


107

Here q€ des ð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 is known exactly yields _ þ gðqÞ þ τext ¼ τ an q€ þ DaðqÞ€ q þ bðq; qÞ _ þ gðqÞ þ τext q þ bðq; qÞ ¼ an q€ des ðsÞ þ ½DaðqÞ€

ð3:108Þ

The closed loop dynamics reduce to q€ ¼ q€ des ð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 q€ des q . Assume acceleration is measured. Let us introduce a new variable as in (3.110) s_ q€ð€ q ; q€ des Þ ¼ q€ q€ des q

ð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 sq€ð€ q ; q€ des Þ ! 0 is enforced if control input is selected as eq τ ¼ τd þ a€ q des q aksq€ ¼ τ aksq€

ð3:111Þ

Thus, after transient s_ q€ð€ q ; q€ des Þ þ ksq€ð€ q ; q€ des Þ ¼ 0, the equivalent force enforces motion des q€ ¼ q€ q . 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.


108

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 q_ ref ðtÞ and q€ ref ð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€ q€ ref , thus the acceleration that will maintain e€ðqÞ ¼ 0 is q€ ¼ q€ ref . Let us first find the desired acceleration. The plant dynamics with the control input q€ des are defined as the double integrator q€ ¼ q€ des. 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 q€ des ¼

q€ ref |{z} equivalent acceleration

½KD e_ ðqÞ þ KP eðqÞ |fflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflffl}

ð3:112Þ

convergence acceleration

The closed loop transient is e€ðqÞ þ KD e_ ðqÞ þ KP eðqÞ ¼ 0

ð3:113Þ

Selection of the controller gains as KD ¼ ðm1 þ m2 Þ > 0 and KP ¼ m1 m2 > 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 feedforward terms. Another design can be implemented by introducing a new variable s ¼ c3 e_ ðqÞ þ c2 eðqÞ. Then, by selecting the Lyapunov function as in (3.50) and its derivative as in (3.55), the


Figure 3.20

109

Cascade structure of a trajectory tracking system with an inner acceleration control loop

structures discussed so far can be directly applied. The equivalent acceleration enforcing zero rate of change s_ ¼ 0 is sj _ q¼ €ðqÞ þ c2 e_ ðqÞ ¼ 0 ) q€ eq ¼ q€ ref € q€eq ¼ c3 e

c2 e_ ðqÞ c3

ð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 q€ con ¼ 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 q€ des ¼ q€ eq þ q€ con ¼ q€ eq ks c2 e_ ðqÞ kc2 eðqÞ ¼ q€ ref kc3 þ c3

ð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 ¼ c3 1 m1 and c2 c3 1 ¼ 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 c2 c3 1 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. If m1 >> m2 then closed loop motion 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 q€

des

τ ¼ satfτdis þ an q€

des

g ¼ sat τdis þ an g ¼ satfτdis þ an ½€ q

q€ ref

ref

c2 c2 e_ kc3 e_ þ e c3 c3

ðKD e_ þ KP eÞg

ð3:116Þ


110

Figure 3.21

Structure of a trajectory tracking system with an acceleration control framework

For given bounds of disturbance and desired acceleration bound of control can be determined as minðτÞ minðτdis þ an q€ des Þ maxðτÞ maxðτdis þ an q€ des Þ

ð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 ¼ c3 1 m1 , c2 c3 1 ¼ m2 for the system shown in Figure 3.21 and KP ¼ m1 m2 , 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 error sð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 kV a ;1=2 < a < 1. The reaching time tr from the initial value Vð0Þ 6¼ 0 is determined by tr ¼ V 1 a ð0Þ=2a kð1 aÞ. The convergence acceleration

Figure 3.22 Common realization of the systems depicted in Figures 3.20 and 3.21


111

1 q€ con ¼ kjsj2a 1 signðsÞ; < a < 1 2

ð3:118Þ

enforces motion s_ þ kjsj2a 1 sign ðsÞ ¼ 0 if the desired acceleration and consequently the control input force are selected as q€ des ¼ sat½€ q eq kjsj2a 1 sign ðsÞ; τ ¼ sat½τeq an kjsj2a 1 sign ðsÞ

ð3:119Þ

For the desired reaching time and convergence rate, the parameter k > 0 should be selected from tr ¼ V 1 a ð0Þ=2a kð1 aÞ. The control (3.119) enforces closed loop dynamics _ ¼ 0 ) c3 e_ ðqÞ þ c2 eðqÞ ¼ 0; t tr sðq; qÞ

ð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Þ þ c2 eð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 _ ¼ yref ðtÞ. In this example, our primary goal is to enforce it converges to the solution yðq; qÞ 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Þ _ c2 ¼ 25, c3 ¼ 1. Plant model and the parameters are the same as in Example 3.6. c2 q þ c3 q, _ 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 jτmax j ¼ 100 N:m, and the 2

convergence acceleration q€ con max ¼ 150 rad=s . 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

Figure 3.23

Position control system with finite-time convergence for 0:5 a < 1

ð3:121Þ


112

_ Figure 3.24 Changes in reference yref , output y, output error ey, e_ y ðey Þ diagram, output yl ¼ 25q þ q, reference yref ¼ 25qref þ q_ ref , qref ¼ 0:1½1 þ 0:25 sin ð12:56tÞ rad as position reference, position q, reference position qref , position error eq and e_ q ðeq Þ diagram. The convergence gain is k ¼ 100 and a ¼ 0:75

The output and its reference are selected as _ ¼ c2 q þ c3 q_ ¼ 25q þ q_ yðq; qÞ y ðq ; q_ ref Þ ¼ c2 qref þ c3 q_ ref ¼ 25qref þ q_ ref ref

ref

ð3:122Þ

For selected output the control error becomes _ yref ðqref ; q_ ref Þ ey ¼ yðq; qÞ ¼ 25ðq qref Þ þ ðq_ q_ ref Þ ¼ 25eq þ e_ q

ð3:123Þ

The desired acceleration and the control torque are q€ des ¼ q€ eq þ q€ con τ ¼ τdis þ an q€ des

ð3:124Þ


113

_ Figure 3.25 Changes in reference yref , output y, output error ey, e_ y ðey Þ diagram for output yl ¼ 25q þ q, its reference yref ¼ 25qref þ q_ ref , qref ¼ 0:1½1 þ 0:25 sin ð12:56tÞ rad as position reference, position q, reference position qref , position error eq and e_ q ðeq Þ diagram. The convergence gain is k ¼ 100, a ¼ 0:5 and convergence acceleration as in (3.128).

The equivalent acceleration is q€ eq ¼ y_ ref 25q_

ð3:125Þ

For finite-time convergence a ¼ 0:75 the convergence acceleration is given by

0:5 q€ con ¼ 100 ey sign ðey Þ

ð3:126Þ

The closed

0:5loop motion of the system is expected to satisfy one of the following constraints e_ y þ 100 ey sign ðey Þ ¼ 0 For a ¼ 0:5 convergence acceleration reduces to q€ con ¼ 100 sign ðey Þ 1 for ey > 0 sign ðey Þ ¼ 1 for ey < 0

ð3:127Þ


114

Convergence acceleration (3.127) undergoes discontinuity while enforcing condition ey e_ y < 0. For control q€ des ¼ q€ eq þ q€ con after ey ¼ 0 is reached the control is equal to q€ eq , 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 con and average q€~ ¼ 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 q€ con ¼ kc1 1 ðjey j 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 e_ y ðey Þ diagram. In the right column, the position reference qref and its actual value q, the position control eq error and the e_ q ð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 e_ q ðeq Þ diagram. The motion towards trajectory 25eq þ e_ q ¼ 0 is short and then motion is tracking the straight line e_ q ¼ 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. 6. Utkin, V.I. (1992) Sliding Modes in Control and Optimization, Springer-Verlag, New York.

Further Reading Katsura, S. (2004) Advanced Motion Control Based on Quarry of Environmental Information, Ph.D. Dissertation, Keio University, Japan. Katsura, S., Matsumoto, Y., and Ohnishi, K. (2005) Realization of “Law of action and reaction” by multilateral control, Industrial Electronics. IEEE Transactions on Industrial Electronics, 52(5), 1196–1205. € Onal, C.D. (2005) Bilateral Control – A Sliding Mode 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 design method based on functionality. IEEE Transactions 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 understanding 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, we will assume that the disturbance is bounded and can be modeled as output of known dynamical system with unknown initial _ are measured, the conditions. A further assumption is that the plant coordinates ðq; qÞ 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 _ þ gðqÞ þ text td ¼ bðq; qÞ ¼ t aðqÞ€ q


ð4:1Þ

116


or generalized disturbance force tdis _ þ gðqÞ þ text þ Da€ tdis ¼ bðq; qÞ 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, at least 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 _ a; tÞtd . Similarly the estimated disturbance is the output of a dynamic structure ^td ¼ Qd ðq; q; _ an ; tÞtdis . Here, ^td the estimated generalized disturbance will be expressed as ^tdis ¼ Qdis ðq; q; stands for the estimated value of the disturbance, ^tdis stands for the estimation of the _ a; tÞ and Qdis ðq; q; _ an ; tÞ stand for some linear or nonlinear generalized disturbance, Qd ðq; q; 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 position may 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 _ a; tÞ or Qdis ðq; q; _ an ; tÞ and the disturbance. In order to simplify observer dynamics Qd ðq; q; _ a; tÞ and Qdis ðq; q; _ an ; tÞ notation in further text here Q will be used instead of Qd ðq; q; 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 in Chapter 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 disturbance would 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 ) d2 qðtÞ _ ¼ 0; qð0Þ ¼ d; qð0Þ ¼ d1 dt2 td ¼ qðtÞ ¼ d þ d1 t

ð4:3Þ

Disturbance Observers

117

In another representation it could be written as

d q 0 1 q ¼ ; 0 0 q1 dt q1 q td ¼ ½ 1 0 q1

d qð0Þ ¼ d1 q1 ð0Þ

ð4:4Þ

Similarly a disturbance represented as a ðm 1Þ order polynomial can be modeled by 9 dm qðtÞ > ðm 1Þ _ = ¼ 0; qð0Þ ¼ d; qð0Þ ¼ d ; q ð0Þ ¼ d ðm 1Þ! 1 m1 dtm > ; qðtÞ ¼ d þ d1 t þ þ dm 1 tm 1

ð4:5Þ

In the state space form (4.5) is written as 2

q

3

2

0

d6 7 6 4... 5 ¼ 4... dt 0 qm 1 td ¼ ½ 1

32

q

1

0

...

76 . . . 54 . . .

0

2

0 q

3

2

7 5;

6 7 6 4... 5 ¼ 4...

qð0Þ

3

ð0Þ

qm 1 3

2

d

dm 1 ðm 1Þ!

3 7 5 ð4:6Þ

7 5

6 0 0 4 . . . qðm 1Þ

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 q 0 ¼ v dt q1

v q ; 0 q1 q td ¼ ½ 1 0 q1

0 qð0Þ ¼ d q1 ð0Þ

ð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 ¼ An q; dt

td ¼ Cq q;

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 observer may 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. Many works are based on state observer design for augmented system

118


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Þ ¼ t_ d ð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

0 1 q_ q 0 ¼ Y ðt td Þ þ 1 v_ 0 0 v a |fflfflfflffl{zfflfflfflffl} |fflfflfflffl{zfflfflfflffl} B

A

q_ ¼ |{z} 0 ;

ð4:9Þ

Aq q

^td ¼ q

System (4.9) can be written as 32 3 2 3 2 3 2 0 q 0 1 0 q_ 76 7 6 7 6 7 6 6 v_ 7 ¼ 6 0 0 a 1 76 v 7 þ 6 a 1 7t 54 5 4 5 4 5 4 q 0 0 0 0 q_ " # " # A B B z_ ¼ zþ t; z ¼ ½ q v 0 Aq 0

ð4:10Þ q T

^td ¼ ½ 0 0 1 z

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_ ¼ |{z} 0 v þ a 1 ðt td Þ |{z} A

B

1 v_ 0 a1 v a ¼ þ t q_ q 0 0 0 B A B zþ t z_ ¼ 0 0 Aq z ¼ ½ v q T ^td ¼ ½ 0 1 z

ð4:11Þ


119

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

h z¼ q

#

" zþ

B 0

# t ¼ Az z þ Bt

v q . . . . . . qðm 1Þ

iT

ð4:12Þ

td ¼ Cz

The selection of measurements and the model of disturbance are such that augmented plantdisturbance 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 l z_ ¼ q_ þ l v_ ¼ ðt z þ lvÞ a

ð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) l ^z_ ¼ ðt ^z þ lvÞ; a

l>0

ð4:15Þ

^ ¼ ^z lv q

Structure of the disturbance observer (4.15) is depicted in Figure 4.1(a). Plugging (4.15) into ^_ ¼ ^z_ l v_ yields q l l ^ l2 ^_ ¼ _ þ t ðq þ l qÞ q_ l€ q q aðqÞ aðqÞ aðqÞ ^_ þ q

l ^ l l ½t aðqÞ€ q ¼ td q¼ aðqÞ aðqÞ aðqÞ

ð4:16Þ

^ ¼ ^td q

^ output of the nonlinear first-order filter (4.16). With assumption that Estimated disturbance is q disturbance is modeled by t_ ¼ 0 the estimation error can be expressed as

120

Motion Control Systems l ^ e ¼ tq ^ ^_ ¼ ðt qÞ; t_ q aðqÞ e_ þ

ð4:17Þ

l e¼0 aðqÞ

Since aðqÞ > 0 is strictly positive then, by selecting l > 0, the estimation error tends to zero as ^ ! td . The convergence rate depends on plant inertia. t ! ¥ and consequently ^td ¼ q ^_ þ gq ^ ¼ gðt aðqÞ€ Selecting l ¼ gaðqÞ > 0 allows rewrite structure (4.16) as q q Þ ¼ gtd and by taking Laplace transformation we obtain: ^ ¼ gtd ðs þ gÞq ^ ¼ ^td ¼ q

g td ¼ QðsÞtd sþg

ð4:18Þ

^ ¼ ^td ¼ QðsÞtd modified by simple Observer (4.16) provides estimation of the disturbance q 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 ðt ^z þ lvÞ; an

l>0

ð4:19Þ

^ ¼ ^z lv q

^_ ¼ ^z_ l v_ the output By selecting observer gain l ¼ gan , inserting (4.19) and (3.4) into q ^ ¼ ^tdis becomes q l l ^ l2 ^_ ¼ t ðq _ þ q_ l€ q þ l qÞ q an an an ^_ þ gq ^ ¼ gðt an q€ Þ ¼ gtdis q

ð4:20Þ

^ ¼ ^tdis q

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 ^ ¼ ^tdis is the same as the one shown in Equation estimating the generalized disturbance q (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)].


121

Figure 4.1 Disturbance observer for: (a) a plant with known parameters and (b) a plant with nominal parameters

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 the output of the inverse plant P 1 ¼ aðqÞ€ q or nominal plant Pn 1 ¼ 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Þ. In contrast, 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 an svÞ ¼ Qt an gð1 QÞv ^tdis ¼ Qðt þ van gÞ van g;

ð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_ ¼ t_ dis ¼ 0. Since the position is measured the reduced

122


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Þ

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 tdis ¼ t* þ q an

q_ ¼ 0 ^tdis ¼ an q

ð4:22Þ

t* ¼ an 1 t 8 2 39 < t = t t d 5 ¼ td þ Da€ 4 q tdis ¼ an :aðqÞ aðqÞ an ;

The scaling t* ¼ an 1 t is used just for more compact writing. The disturbance q 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.


123

The intermediate variables should be specified to allow estimation of the velocity and the disturbance. They can be selected as z1 ¼ q l1 q;

q ¼ an 1 tdis ;

z2 ¼ v l2 q;

l2 ¼ const

l1 ¼ const

ð4:23Þ

The dynamics of these intermediate variables on the trajectories of system (4.22) are z_ 1 ¼ l1 ðz2 þ l2 qÞ z_ 2 ¼ z1 l2 z2 þ ðl1 l22 Þq þ t*

ð4:24Þ

t* ¼ an 1 t

The observer should have the same dynamics as the intermediate variables (4.24), thus it can be written in the following form ^z_ 1 ¼ l1 ð^z2 þ l2 qÞ ^z_ 2 ¼ ^z1 l2^z2 þ ðl1 l22 Þq þ t*

ð4:25Þ

Taking Laplace transformation of (4.25) and solving for ^z1 yields ^z1 ¼

l2 v þ l1 q þ t* l1 s2 þ l2 s þ l1

ð4:26Þ

^ ¼ ^z1 þ l1 q yields the disturbance estimation as Plugging (4.26) into q 1 2 l1 t an s2 q ^ ¼ an t s q l1 ¼ q 2 2 s þ l2 s þ l1 an s þ l2 s þ l1

l1 tdis ^ ¼ ^tdis ¼ q an s2 þ l2 s þ l1 an ^¼ ^tdis ¼ an q

ð4:27Þ

l1 tdis s2 þ l2 s þ l1

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 þ l2 q ^v ¼

l1 v s2 þ l 2 s þ l 1

ð4:28Þ

The structures of the disturbance observer as described in (4.25) is shown in Figure 4.3. More complex disturbance models 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€ ¼ t€dis ¼ 0. Let measurement be the input force and the velocity ðt; vÞ

124


Figure 4.3

Disturbance observer with position and force as inputs

and let the nominal system inertia is known. The augmented system can be written as v_ ¼

t tdis ¼ t* þ q an

q_ ¼ q1 q_ 1 ¼ 0

ð4:29Þ

t* ¼ an 1 t ^tdis ¼ an q

Let the intermediate variables be z1 ¼ q1 l1 v; z2 ¼ q l2 v;

l1 ¼ const l2 ¼ const

ð4:30Þ

The observer dynamics can be written in the following form ^z_ 1 ¼ l1 ð^z2 þ l2 v þ t* Þ ^z_ 2 ¼ ^z1 l2^z2 þ ðl1 l22 Þv l2 t*

ð4:31Þ

Taking a Laplace transformation of (4.31) and solving it for ^z2 yields ^z2 ¼

ðl1 l22 Þs þ l1 l2 l2 s þ l1 v 2 t* ; s2 þ l2 s þ l1 s þ l2 s þ l1

ð4:32Þ

^ ¼ ^z2 þ l2 v can be expressed as The estimation of disturbance q ^¼ q

l2 s þ l1 l2 s þ l1 tdis ðsv t* Þ s2 þ l2 s þ l1 |fflfflfflfflffl{zfflfflfflfflffl} ¼ s2 þ l2 s þ l1 an q

^¼ ^tdis ¼ an q

l2 s þ l1 tdis s2 þ l2 s þ l1

ð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.4

125

€¼0 Disturbance observer with velocity measurement and model of disturbance q

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 socalled 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 _ þ gðqÞ þ text ¼ t. The control will be either with exponential (as in (3.26) an q€ þ Da€ q þ bðq; qÞ shown in Example 3.3) or finite-time convergence (as shown in Example 3.5). _ þ gðqÞ þ text with: The external force is modeled as td ¼ bðq; qÞ . . .

_ ¼ b0 q þ b1 q_ þ an a1 q_ cos ðqÞ, in N m, nonlinear forces bðq; qÞ nonlinear force gðqÞ ¼ g0 q2 , in N m, t0 ½1 þ cos ðvt tÞ þ sin ð3vt tÞ; 0:12 < t < 0:8s Nm external force text ¼ 0 elsewhere

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 _ and the second based on the position measurements ðt; qÞ. Our goal is not to show the best ðt; qÞ performance but rather, to illustrate the structures and show a possible simple implementation. _ measurements is realized as The disturbance observer with input force and velocity ðt; qÞ dz ¼ gðt þ gan q_ zÞ dt

ð4:34Þ

^td ¼ z gan q_

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

126

Motion Control Systems dz1 ¼ l1 ðz2 l2 qÞ dt dz2 ¼ z1 l2 z2 þ ðl1 l22 Þq þ an 1 t dt

ð4:35Þ

^td ¼ an ðz1 þ ln qÞ

The parameters of observer (4.35) are selected as l1 ¼ g2 and l2 ¼ 2g. This way the comparison of the observers may be more meaningful. 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 _ ¼ 50q þ 5q_ are simulated. yl ðq; qÞ In Figure 4.5, in the first row the reference yref , the output y and the system disturbance td are shown. 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 þ 5q_ is controlled to track the reference yref ¼ 1 þ 3:5 sin ð12:56tÞ and the controller parameters are k ¼ 50 and a ¼ 0:8


127

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 in Chapter 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 tracks measured plant output v. From plant dynamics (3.4) and model (4.36) the dynamics of the tracking error ez ¼ z v is obtained as e_ z ¼ z_ v_ ¼

1 ðtdis uÞ an

ð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 e_ z þ

l 1 ez ¼ tdis an an

ð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 þ gu ¼ gtdis dt

ð4:39Þ

uav ðtÞ ¼ ^tdis

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 e_ z ¼ l2a V a ; 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 > jtdis j

ð4:40Þ

enforces sliding mode in ez ¼ 0 and tracking error, after initial transient, will reach zero value in finite time. From e_ z ðu ¼ ueq Þ ¼ 0 equivalent control is derived as ueq ¼ tdis

ð4:41Þ

128


Figure 4.6

Topology of velocity based closed loop disturbance observer

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 u_ av ¼ 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 jez j2a 1 sign ðez Þ;

1 0 and assuming the disturbance model q_ ¼ 0, for plant (4.54), results in the disturbance observer ^z_ ¼

l ðKn iref ^z þ lvÞ; an

l>0

^ ¼ ^z lv q

ð4:55Þ

^_ ¼ ^z_ l v_ yields Plugging (4.55) into q l ^ l ^_ þ q ¼ ðKn iref an q€ Þ q an an

ð4:56Þ

^ ¼ ^tdis q

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).

134


Figure 4.9 Disturbance observer for plant with actuator without dynamics in the current loop

In comparison with observer (4.15) in (4.55) the current reference Kn iref 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 the setting t* ¼ Kn iref =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 ¼ Kn iref . 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 _ þ gðqÞ þ text ¼ t aðqÞ€ q þ ½bðq; qÞ |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} system disturbance¼td

t ¼ KT iref 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


135

_ þ gðqÞ þ text 0:2Kn iref cos ð128:28tÞ ¼ Kn iref an q€ þ ½Da€ q þ bðq; qÞ |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}

ð4:58Þ

generalized disturbance¼tdis

The generalized disturbance to be estimated is given by _ þ gðqÞ þ text 0:2Kn iref cos ð128:28tÞ tdis ¼ 0:25an q€ sin ðqtÞ þ bðq; qÞ

ð4:59Þ

The disturbance observer (4.56) and the observer (4.35) with input t ¼ Kn iref 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 þ _ ¼ 50q þ 5q. _ The gain of the filter is g ¼ 600 for easier 3:5 sin ð12:56tÞ and output yl ðq; qÞ comparison of the effects of the additional terms in the generalized disturbance. In Figure 4.10, the first row the actuator gain KT and the system generalized disturbance tdis are 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 ¼ Kn iref . 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

136


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 ¼ Kn iref 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 now higher 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 _ þ gðqÞ þ text ðq; tÞ ¼ ½Kn þ DKT ðq; iÞi ½an þ DaðqÞ€ q þ ½bðq; qÞ di ¼ gc ðiref iÞ dt

ð4:60Þ

t ¼ KT ðq; iÞ i ¼ Kn i þ DKT ðq; iÞi KT min < KT ¼ Kn þ DKT ðq; iÞ < KT max

By adding and subtracting Kn iref to the right hand side (4.60) can be rearranged into an q€ ¼ Kn iref tdis _ þ gðqÞ þ text þ Da€ tdis ¼ ½bðq; qÞ q þ Kn iref KT i

ð4:61Þ

di ¼ gc ðiref iÞ dt

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_ ¼ lan 1 ðKn iref ^z þ lvÞ; ^ ¼ ^z lv q

l>0 ð4:62Þ


137

^_ ¼ ^z_ l v_ it is easy to show that the output of observer (4.62) can be expressed Plugging ^z_ into q ^ ¼ ^tdis . Disturbance observer (4.62) includes force ^ ¼ la 1 ðKn iref an q€ Þ, q ^_ þ la 1 q as q n n i ref tdis ¼ Kn i KT i 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 systems with 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 is the gain of the nominal current loop filter. By adding and subtracting Kn in in the right hand side in Equation (4.60) yields an q€ ¼ Kn in tdis di ¼ gc ðiref iÞ dt ð4:63Þ

din ¼ gcn ðiref in Þ dt _ þ gðqÞ þ text þ Da€ tdis ¼ ½bðq; qÞ q þ Kn in KT i

Here, disturbance tdis includes the difference between nominal and actual current loop dynamics and the variation in actuator gain. Just by replacing Kn iref with Kn in the observer (4.62) can be modified to include nominal current loop dynamics ^z_ ¼ lan 1 ðKn in ^z þ lvÞ; ^ ¼ ^z lv q

l>0 ð4:64Þ

^_ ¼ ^z_ l v_ it is easy to show that output of observer (4.64) can be By plugging ^z_ into q ^ ¼ ^tdis . ^ ¼ la 1 ðKn in an q€ Þ, q ^ expressed as d q=dt þ lan 1 q n 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=an s2 stands for the nominal plant dynamics. This structural arrangement points to another way 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 disturbance 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 gain KT ¼ Kn ½1 þ 0:2cos ð128:28tÞ N m=A with Kn ¼ 0:85 N m=A. The current control loop dynamics is modeled as a first-order system.

138


Figure 4.11 Disturbance observer with current control loop dynamics: (a) nominal structure and (b) representation by transfer functions from iref and q

The observer is designed as in (4.62) ^z_ ¼ lan 1 ðKn iref ^z þ lvÞ;

l>0

^ ¼ ^z lv q l ¼ gan ; g ¼ 600

ð4:65Þ

The output of the observer is a generalized disturbance _ þ gðqÞ þ text þ KT i Kn iref tdis ¼ Da€ q þ bðq; qÞ di ¼ gc ðiref iÞ dt

ð4:66Þ

gc ¼ 300

_ ¼ 50q þ 5q_ Simulation is presented for a closed loop control system with output yl ðq; qÞ and 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.


139

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

In the first row in Figure 4.12, the change in actuator gain KT 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.

140


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

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 some properties of the control plant or its interaction with the environment. In this section we would like to discuss estimation of 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 acceleration and the equivalent force can be treated in the same way as the disturbance 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


141

q_ ¼ 0. Then the dynamics of the generalized control error (3.25) augmented by model of equivalent force yield s_ ¼ an 1 gðt teq Þ q_ ¼ 0

ð4:67Þ

eq

^t ¼ q

By selecting intermediate variable z ¼ q þ ls; l > 0 the equivalent force observer can be constructed as ^z_ ¼ lgan 1 ðt þ lsÞ lgan 1^z ^ ¼ ^z ls q

ð4:68Þ

^_ ¼ ^z_ l v_ it is easy to show that the output of observer (4.68) can be By inserting ^z_ into q ^ ¼ ^teq , thus expressed as q ^ ¼ lga 1 ðt an g 1 sÞ ^_ þ lgan 1 q _ ¼ lgan 1 teq q n ^ ¼ ^teq q

ð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 ¼ lgan 1 . The equivalent acceleration observer can be constructed in a very similar way. From (3.22) the equivalent acceleration can be expressed as q€ eq ¼ q€ des g 1 s, _ thus the information needed for the equivalent acceleration estimation are the desired acceleration q€ des and the generalized control error s. The desired acceleration is the output of the outer loop controller and s is the control error, so both are readily available within the controller. Assume the pair ð€ q des ; sÞ is measured, the coefficient g is known and the equivalent acceleration is modeled by ^ ¼ q€êq . first-order dynamics q_ ¼ 0 and q Let intermediate variable be z ¼ q þ ls; l > 0. From (3.22) and the model of the disturbance q_ ¼ 0 the estimation of intermediate variable ^z can be then constructed as ^z_ ¼ lgð€ q des þ lsÞ lg^z ^ ¼ ^z ls q

ð4:70Þ

^_ ¼ ^z_ l s_ yields Inserting ^z_ into q ^_ þ lg q ^ ¼ lgð€ _ ¼ lg€ q eq q q des g 1 sÞ ^ ¼ q€^ eq q

ð4:71Þ

142


Figure 4.14 Equivalent force observer teq and its implementation with first-order filter

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 linked with 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 of Equivalent Acceleration and Equivalent 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 loop dynamics 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 _ ¼ 50q þ 5q_ and its reference yref ðtÞ ¼ ½1 þ 3:5 sin ð12:56tÞ. The generalized error is yl ðq; qÞ ey ¼ yref 5ðq_ þ 10qÞ. The parameters of the controller are a ¼ 0:80; k ¼ 50 and initial


143

conditions in position qð0Þ ¼ 0:15 rad. The output controller is designed for a nominal plant without taking current loop dynamics into account q€ eq ¼ 0:2ð3:5 12:56 cosð12:56tÞ 50q_ Þ l 2a 1 sign ey q€ con ¼ 10ey

ð4:72Þ

tdis ¼ Da€ q þ bðq; q_ Þ þ gðqÞ þ text

ð4:73Þ

The disturbance is

For equivalent acceleration (4.72) and disturbance (4.73) the equivalent force becomes teq € eq l ¼ tdis þ an q l

ð4:74Þ

For equivalent acceleration structure (4.71) and for equivalent force estimation structure (4.68) is used. The equivalent acceleration observer is dz ¼ gð€ q des þ 0:02 gey zÞ dt eq q€^ ¼ z 0:02 gey ;

g>0

ð4:75Þ

ey ¼ yl yref

The equivalent force observer is written in the following form dzt ¼ gðt þ 0:02 gey zt Þ dt teq l ¼ zt 0:02 gey ;

g>0

ð4:76Þ

ey ¼ yl yref

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 _ ¼ 50q þ 5q_ and its reference yref ðtÞ ¼ ½1 þ 3:5 sin ð12:56tÞ. The generalized output yl ðq; qÞ 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 q€ eq , the convergence acceleration q€ con and the eq estimated equivalent acceleration q€^ are 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.

144


Figure 4.15 Changes in output reference yref , output y, disturbance tdis , equivalent acceleration q€ eq , eq convergence acceleration q€ con , estimated equivalent acceleration q€^ , 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

4.5 Functional Observers In this section we will show a slightly different way to design observers within 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 ¼ Kn Wcn Qiref P 1 Qq. The transfer functions Kn Wcn and P are defined by the plant dynamics and the only design parameter is the filter Q. Here, we would 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 ¼ WN q be the desired output. The transfer function WN may or may not be proper. Indeed, if the desired output is proportional to acceleration


145

z ¼ h2 s2 q or a linear combination of acceleration and velocity z ¼ ðh2 s2 þ h1 sÞq or linear combination of acceleration, velocity and position z ¼ ðh2 s2 þ h1 s þ h0 Þq, the resulting WN 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 ¼ h2 s2 þ h1 s þ h0. Inserting reference current iref , plant disturbance td and the plant dynamics the desired output z can be expressed as z ¼ WN q ¼ WN PðKT Wc iref td Þ ¼ WN PKT Wc iref WN Ptd

ð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 ¼ W1 iref þ W2 q, 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 WL td stands for the error in estimation of the output z due to the presence of the unmeasured inputtd . Ideally WL should be zero, but that is not feasible. The estimation error is determined by the structure of WL and the properties of the disturbance input, thus it is natural to seek WL td 0 in the selected frequencies range.

Figure 4.16 Estimation of the desired output z ¼ WN q by adding the correction input W1 iref such that the error due to the disturbance input has specified properties. (a) Desired relation z ¼ WN q. (b) Realization of the observer ^z ¼ WN q þ WL td

146


From Figure 4.16(b) and (4.78) the estimated output ^z can be expressed as ^z ¼ W1 iref þ W2 q ¼ W1 iref þ W2 PðKT Wc iref td Þ ¼ ðW1 þ W2 PKT Wc Þiref W2 Ptd

ð4:79Þ

Now the problem is to select transfer functions W1 and W2 such that output ^z is the estimation of 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 WL td ¼ ½PKT Wc ðWN W2 Þ W1 iref þ PðWN W2 Þtd

ð4:80Þ

From (4.80) the following relations are obtained PKT Wc ðWN W2 Þ W1 ¼ 0 PðWN W2 Þ WL ¼ 0

) ð4:81Þ

Solving (4.81) for W1 and W2 , with the assumption that KT Wc ; WL and WL ; P do not have common poles and zeroes, yields W1 ¼ KT Wc WL W2 ¼ WN WL P 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 KT Wc are assumed known. The error transfer function WL is a design parameter and it should be selected such that W1 and W2 are realizable. It is a matter 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 dynamics (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 Section 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 ¼ KT Wc their nominal values will be used. Then (4.82) can be written as (

W2 ¼ WN WL Pn 1 W1 ¼ Kn Wcn WL

ð4:83Þ

Here WN stands for the desired transfer function from output to measurement, Pn 1 ¼ an s2 stands for the inverse nominal plant – disturbance to output and force to output transfer function, Win ¼ Kn Wcn stands for the approximation of the current control loop and actuator gain, W1 stands for the input to estimated variable transfer function and W2 stands for the output to estimated variable transfer function.


147

In the design the actuator current and the position ðiref ; qÞ are assumed measured and the nominal values of the inertia an ¼ 0:1 kg 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 WL tdis ¼

1 g2 sðgs þ dÞ tdis ; an ðs þ gÞ2

ð4:84Þ

g; g; d > 0

Coefficients g; 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 Kn Wcn WL ¼ W1 ¼

Kn g2 sðgs þ dÞ ¼ Kn Pn ðWN W2 Þ an ðs þ gÞ2

ð4:85Þ

For WL as in (4.84) transfer function W2 can be expressed as W2 ¼ WN Pn 1 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 g2

s3 ðgs þ dÞ ðs þ gÞ

2

¼

ðas2 þ bsÞðs þ gÞ2 g2 s3 ðgs þ dÞ ðs þ gÞ2

ða g2 gÞs4 þ ðb þ 2ga g2 dÞs3 þ ð2gb þ g2 aÞs2 þ g2 bs

ð4:87Þ

ðs þ gÞ2

The values of g; d can be determined if W1 is required to be a proper transfer function (both nominator and denominator as second-order polynomials). Then (4.87) yields )

a g2 g ¼ 0 b þ 2ga g2 d ¼ 0

Yg¼

a b þ 2ga ; d¼ g2 g2

ð4:88Þ

Finally, after some algebra, the observer transfer functions can be determined as W1 ¼

Kn as2 þ ðb þ 2gaÞs an ðs þ gÞ2

W2 ¼ g

ðga þ 2bÞs2 þ gbs ðs þ gÞ2

WN ¼ as2 þ bs Wcn ¼ 1

ð4:89Þ

148


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. 2 3 2 Kn as2 þ ðb þ 2gaÞs b g b g 2 1 5 þ W1 ¼ ¼ b0 4b3 þ s þ g ðs þ gÞ2 an ðs þ gÞ2 2

W2 ¼ gs

ðga þ 2bÞs þ gb ðs þ gÞ2

3 2 a g a g 2 1 5 þ ¼ a0 4a3 þ s þ g ðs þ gÞ2

ð4:90Þ

z_ 1 ¼ gða1 a0 q þ b1 b0 iref z1 Þ z_ 2 ¼ gðy1 þ a2 a0 q þ b2 b0 iref z2 Þ z ¼ z2 þ a3 a0 q þ b3 b0 i

ð4:91Þ

ref

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;

.

Kn ; b1 ¼ 1; b2 ¼ 1; b3 ¼ 0 gan

acceleration estimation z ¼ q€ aa ¼ g2 ; a1 ¼ 1; a2 ¼ 2; a3 ¼ 1;

.

b0 ¼

b0 ¼

Kn ; b1 ¼ 1; b2 ¼ 0; b3 ¼ mn > 1 an

disturbance estimation z ¼ tdis aa ¼

g2 an ; a1 ¼ 1; a2 ¼ 2; a3 ¼ 1; Kn

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 ¼ KT i. The input to the observer is t ¼ Kn iref . 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.


149

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 ¼ KT i. The input to the observer is t ¼ Kn iref The control loop parameters are a ¼ 0:75; k ¼ 50

4.6 Dynamics of Plant with Disturbance Observer The control input required to enforce the desired acceleration q€ des is t ¼ an q€ des þ tdis . Since tdis is not measured in the implementation estimated disturbance ^tdis is used, thus the realizable control input is t ¼ an q€ des þ ^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 system with disturbance observer feedback (4.92). The term an q€ des ¼ 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

150


Figure 4.18

Acceleration control with estimated disturbance feedback

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

t ^t_ dis ¼ gðt ^ttdis Þ q ^t_ dis ¼ gð an q€ ^tqdis Þ

ð4:93Þ

Inserting (4.93 ) into (4.92) yields ðt t ¼ an q€ des þ gan ð€ q des q€ Þdz

ð4:94Þ

0

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 q€ des þ ^tdis _ þ gðqÞ þ text tdis ¼ Dan q€ þ bðq; qÞ

ð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 q€ des as a new variable, (4.95) can be rearranged into two equivalent forms


151

an q€ ¼ an q€ des ð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 q€ des 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 second representation. The dynamics of the plant (3.1) with the disturbance feedback (4.92) reduces to a double integrator with 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 the disturbance 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 plant with an estimated disturbance feedback (4.96) can be approximated by an q€ ¼ an q€ des

ð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.

152


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

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, we will 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Þ Pn 1 qQ ¼ z

ð4:99Þ

Here P ¼ 1=ðas2 Þ stands for plant (3.1) with the control input t ¼ tdes þ z and the _ þ gðqÞ þ text , Pn ¼ 1=ðan s2 Þ stands for the nominal plant, Q stands disturbance td ¼ bðq; qÞ 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 ½Pn 1 Q þ P 1 ð1 QÞq ¼ tdes ð1 QÞtd

ð4:100Þ

Rearranging (4.100) into q ¼ Pn Qqr tdes PQqt td , yields

q ¼ Pn

1 P 1 Pn ð1 QÞ þ Q

tdes P

P 1 Pn ð1 QÞ td P 1 Pn ð1 QÞ þ Q

Qqr ¼


Qqt ¼

P 1 Pn ð1 QÞ P 1 Pn ð1 QÞ þ Q

ð4:101Þ


Figure 4.20 in (4.103).

153

System with (a) disturbance observer feedback and (b) its equivalent modification as

Here, the control to output transfer function Pn Qqr and the disturbance to output transfer function PQqt are selected to allow a comparison with a system representation as in (4.96). The coefficient P 1 Pn ¼ aan 1 ¼ 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 q€ des , 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 1 Pn ð1 QÞ and 1 P Pn ð1 QÞ þ Q can be expressed as P 1 Pn ð1 QÞ ¼ P 1 Pn ð1 QÞ þ Q ¼

1 s a sþg 1 s þ ag a sþg

ð4:102Þ

Insertion of (4.102) into (4.101) yields q¼a

sþg s Pn tdes P td s þ ag s þ ag

ð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.

154


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 kgm2 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 ¼ Kn ides . The system inertia variation associated force Da€ q and the actuator gain variation induced force DKT iref 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 Pn 1 qQ ¼ Kn z

ð4:104Þ

Here, P 1 ¼ as2 stands for the plant with control input t ¼ KT ðides þ zÞ and disturbance z ¼ ^tdis . Pn 1 ¼ an s2 stands for the inverse of the nominal plant, Q stands for


155

Figure 4.22 Plant (4.53) with disturbance observer feedback (a) and its equivalent modification as in (4.106) (b).

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 ¼ Pn Qqr Kn ides PQqt td yields q ¼ Pn

1 Kn KT 1 P 1 Pn ð1 QÞ þ Q

Kn ides P

Kn KT 1 P 1 Pn ð1 QÞ td Kn KT 1 P 1 Pn ð1 QÞ þ Q

Qqr ¼

1 Kn KT 1 P 1 Pn ð1 QÞ þ Q

Qqt ¼

Kn KT 1 P 1 Pn ð1 QÞ Kn KT 1 P 1 Pn ð1 QÞ þ Q

ð4:105Þ

Since Kn KT 1 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 Kn KT 1 P 1 Pn ¼ b 1 into (4.105) yields q¼b

sþg s Pn tdes P td ; s þ bg s þ bg

b¼

an KT 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

156


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 ¼ KT Wc ðides þ zÞ. The forces induced 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ÞKT Wc td P ¼ q ½ðides þ zÞKn Pn 1 qQ ¼ Kn z

ð4:107Þ

Elimination of z from (4.107) and rearranging the solution into the form q ¼ Pn Qqr Kn ides PQqt td yields q ¼ Pn

1 Kn KT 1 P 1 Pn Wc 1 ð1 QÞ þ Q

Kn ides P

Kn KT 1 P 1 Pn Wc 1 ð1 QÞ td Kn KT 1 P 1 Pn Wc 1 ð1 QÞ þ Q

Qqr ¼

1 Kn KT 1 P 1 Pn Wc 1 ð1 QÞ þ Q

Qqt ¼

Kn KT 1 P 1 Pn Wc 1 ð1 QÞ Kn KT 1 P 1 Pn Wc 1 ð1 QÞ þ Q

ð4:108Þ

Here P and Pn 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 and Kn 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


157

If nominal current loop dynamics Wcn is included within the structure of the disturbance observer, the input to the disturbance observer is then block Kn Wcn and the second equation in (4.107) changes to ½ðides þ zÞKn Wcn Pn 1 qQ ¼ Kn z while the rest of the system remains the same. This change yields the dynamics of the compensated system q ¼ Pn Qqr Kn ides PQqt td 1 Qqr ¼ Kn KT 1 P 1 Pn Wc 1 ð1 Wcn QÞ þ Q Qqt

Kn KT 1 P 1 Pn Wc 1 ð1 Wcn QÞ ¼ Kn KT 1 P 1 Pn Wc 1 ð1 Wcn QÞ þ 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 ¼ Pn Qqr Kn ides PQqt td 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 ¼ Pn Qqr tdes PQqt td , 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. Blocks W1 and W2 are inserted into the system structure as shown in Figure 4.24. The design goal is selection of at least proper transfer functions W1 and W2 , 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

158


From the block diagram in Figure 4.24 the following relations can be obtained KT Wc iref td ¼ P 1 q Kn W1 iref W2 q ¼ Kn z i

ref

¼i

des

ð4:110Þ

þz

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¼

KT Wc 1 W1 Pides Ptd ð1 W1 Þ þ KT Kn 1 Wc W2 P ð1 W1 Þ þ KT Kn 1 Wc W2 P

ð4:111Þ

The representation of the plant output in the form q ¼ Pn Qqr Kn ides PQqt td is a starting point in the selection of the transfer functions W1 and W2 . By inspection from (4.111) we can write Qqr ¼

KT Kn 1 Wc PPn 1 ð1 W1 Þ þ KT Kn 1 Wc W2 P

Qqt ¼

1 W1 ð1 W1 Þ þ KT Kn 1 Wc W2 P

ð4:112Þ

Solving (4.112) for W1 and W2 yields 1 W1 ¼ 1 KT Kn 1 PPn 1 Wc Qqt Qqr 1 1 W2 ¼ Qqr ð1 Qqt ÞPn

ð4:113Þ

Expressing z from (4.110) and (4.112) yields Kn z ¼ Kn W1 iref W2 q Qqr ð1 Qqt Þ 1 ¼ tdis þ Pn q ðKT Wc Kn W1 Þiref Qqr

ð4:114Þ

The output z stands for the estimation of disturbance and the difference in current controller transfer function. Ideally, for full compensation one would desire Qqr ¼ 1 and Qqt ¼ 0. Then, z is reduced to a generalized disturbance in the plant with partially compensated dynamics of the current control loop Kn z ¼ tdis ðKT Wc Kn W1 Þ iref . In Equation (4.113), the transfer functions Qqr and Qqt are design parameters. It is realistic to require that W1 and W2 are at least proper stable transfer functions. From the input output relation q ¼ Pn Qqr Kn ides PQt td 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 function close to zero in frequency bandwidth determined by desired disturbance compensation. The transfer functions Qqr and Qqt should not have common poles and zeroes.


159

For example, let Wc , Qqr and Qqt be selected as Wc ¼

g sþg

Qqr ¼ 1 Qqt ¼ 1

ð4:115Þ

g2 s2 þ g1 s þ g2

Insertion of these values into (4.113) yields 2

0 13 g g 2 @1 A5 PPn 1 41 sþg s2 þ g1 s þ g2

W1 ¼ KT Kn 1

0 W2 ¼

1

ð4:116Þ

g2 s an g1 s þ g2 A an ¼ g2 @1 2 s2 þ g1 s þ g2 Kn s þ g1 s þ g2 Kn 2

If the parameters of the plant are known, W1 and W2 in (4.116) can be easily realized. In general, if the current loop dynamics Wc , the exact actuator gain and the plant dynamics P are not known, then, in calculating W1 and W2 the nominal current loop dynamics Wcn , the nominal plant dynamics Pn and the nominal actuator gain Kn are used. Insertion of Wcn , Kn and Pn instead of the Wc , KT and P into (4.113) yields 1 W1 ¼ 1 Wcn Qqt Qqr 1 W2 ¼ Qqr ð1 Qqt ÞKn 1 Pn 1

ð4:117Þ

Plugging (4.117) into the first row of (4.111) yields q ¼ Pn

bWC Qqr Qqt Kn ides P td bWC ð1 Qqt Þ þ Qqt bWC ð1 Qqt Þ þ Qqt

ð4:118Þ

bWC ¼ KT Wc PðKn Wcn Pn Þ 1

Here bWC stands for the compensation ratio – in this case a transfer function. The poles of this transfer function are poles of Wc and zeros of Wcn – thus if Wcn 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 ¼ Pn Q*qr Kn ides PQ*qt td

ð4:119Þ

The transfer functions Q*qr and Q*t characterizing deviation of the compensated plant from its ideal structure are

Q*qr ¼

bWC Qqr bWC ð1 Qqt Þ þ Qqt

Q*t ¼

Qqt bWC ð1 Qqt Þ þ Qqt

ð4:120Þ

160


Figure 4.25 Bode plots of transfer functions Q*qr and Q*qt for PPN 1 ¼ 5:0 and an ¼ 0:1 kgm2 (a) for PPN 1 ¼ 0:2 (b) with mismatch in the current loop dynamics gcn =gc ¼ 0:5

The result is similar to other cases discussed so far. If Qqr 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 and mismatch 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 ; Pn 1 ¼ an s2 ; Kn ¼ 0:85 N m=A; an ¼ 0:1 kg m2 ; g ¼ 600; gc ¼ g; gcn ¼ 300 and transfer functions Wc ¼

gc gcn g 2 ; Wcn ¼ ; Qqt ¼ 1 ; Qqr ¼ 1 sþg s þ gc s þ gcn

The results show that the change in current loop dynamics 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 position must 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Þ.


161

Figure 4.26 Bode plots of transfer functions Q*qr and Q*qt for PPN 1 ¼ 5:0 and an ¼ 0:1 kgm2 (a) for PPN 1 ¼ 0:2 (b) with mismatch in the current control loop dynamics gcn =gc ¼ 1:5

Assume the position information is corrupted by noise and ½qðtÞ þ jðtÞ is used instead of the exact 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Þ Pn 1 ðq þ jÞQ ¼ z

ð4:121Þ

Expressing z ¼ P 1 q þ td tdes and inserting it into the second expression in (4.121) yields q ¼ Pn Qqr tdes PQqt td Qqj j Qqr ¼


Qqt ¼

P 1 Pn ð1 QÞ P 1 Pn ð1 QÞ þ Q

Qqj ¼

Q P 1 Pn ð1 QÞ þ 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

162


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)

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, thus Qqt 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 ¼ Qj ¼

Kn KT 1 P 1 Pn ð1 QÞ Kn KT 1 P 1 Pn ð1 QÞ þ Q Q

ð4:123Þ

Kn KT 1 P 1 Pn ð1 QÞ þ Q

These two transfer functions also satisfy Qqt þ Qj ¼ 1. As an illustration the transfer functions Qqt and Qj for the position or the velocity measurement based disturbance observers with the disturbance models t_ d ¼ 0 and t€d ¼ 0 are shown in Table 4.1, where they are given as functions of the parameter mismatch P 1 Pn ¼ a 1 . Just by replacing the inertia mismatch ratio a by the inertia and actuator gain mismatch ratio Kn KT 1 P 1 Pn ¼ 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 functions Qqt and Qj 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Þ, PPN 1 ¼ 0:2, an ¼ 0:1 kg m2 , t_ d ¼ 0]. In the right column in


163

Figure 4.28 Bode plots of Qqt and Qj with measurements t; v and disturbance model t_ d ¼ 0 in (a) and for measurements t; q and disturbance model t_ d ¼ 0 in (b). In both diagrams g ¼ 600

Figure 4.28 Qqt and Qj are shown for the conditions in the third row of Table 4.1 [Q ¼ l1 =ðs2 þ l2 s þ l1 Þ, PPN 1 ¼ 0:2, an ¼ 0:1 kg m2 , measurement ðt; qÞ, disturbance model t_ d ¼ 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:

Table 4.1.

Disturbance and noise rejection for the system shown in Figure 4.27(a) and P 1 Pn ¼ a 1

Measured variables

Disturbance model

Observer filter TF Q g Q¼ sþg

t; v

t_ d ¼ 0

t; v

t€d ¼ 0

Q¼

t; q

t_ d ¼ 0

Q¼

Disturbance transfer function Qqt s Qqt ¼ s þ ag

l2 s þ l1 s2 þ l2 s þ l1

Qqt ¼

l1 þ l2 s þ l1

Qqt ¼

s2

Noise transfer function Qj ag Qj ¼ s þ ag

s2 s2 þ al2 s þ al1

Qj ¼

al2 s þ al1 s2 þ al2 s þ al1

s2 þ l 2 s þ l2 s þ al1

Qj ¼

al1 s2 þ l2 s þ al1

s2

164 . .


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 section we will 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 design will follow the same procedure as in Chapter 3. The dynamics of the generalized control error s 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 q€ eq Þ þ ks ¼ sfg€ q des g½€ q eq þ an 1 pðQ; tdis Þ þ ksg ¼ 0

ð4:124Þ

For s 6¼ 0 the desired acceleration q€ des is derived as q€ des ¼ q€ eq

g 1 ks |fflfflffl{zfflfflffl} convergence acc:

þ

an 1 pðQ; tdis Þ |fflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflffl}

ð4:125Þ

acc due to estimation error

The desired acceleration has three components: (i) the equivalent acceleration q€ eq , (ii) the convergence acceleration q€ con ¼ g 1 ks and (iii) the disturbance estimation error induced acceleration q€ error ¼ an 1 pðQ; tdis Þ. The equivalent acceleration and the convergence acceleration 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 q€ des is tdes ¼ an ½€ q eq þ an 1 pðQ; tdis Þ an g 1 ks

ð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 q€ des ¼ q€ eq g 1 ks.


165

This implementation yields closed loop dynamics s_ þ ks ¼ an 1 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 1 an 1 pðQ; tdis Þ, thus the disturbance observer and/or the convergence 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 _ ¼ c2 q þ c3 q_ with c2 ; c3 > 0 or c2 ; c3 < 0, substituting s ¼ y yref into (4.127) is yðq; qÞ yields c3 q€ þ ðc2 þ kc3 Þ q_ þ kc2 q ¼ ð_yref þ kyref Þ þ an 1 pðQ; tdis Þ

ð4:128Þ

The disturbance estimation error dependent term an 1 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)

166


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 , _ þ gðqÞ þ text . The input torque is t ¼ KT i with current loop dynamics tdis ¼ Da€ q þ bðq; qÞ di=dt ¼ gc ðiref iÞ, gc ¼ 360. The initial condition in position is qð0Þ ¼ 0:025 rad. The _ ¼ 25q þ q. _ reference position is qref ðtÞ ¼ 0:1½1 þ 0:25 sin ð12:56tÞ rad and output yðq; qÞ The reference output is yref ¼ 25qref þ q_ ref and the control error ey ¼ 25eq þ e_ q, eq ¼ q qref . The equivalent acceleration q€ eq ¼ y_ ref 25q_ is calculated exactly and applied as a part of the control input t ¼ ^tdis þ an ð€ q eq þ q€ con Þ

ð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€ q des j 150 rad=s2 ; jtj 100 N m. In Figure 4.30 the transients with convergence acceleration q€ con ¼ 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, e_ y ðey Þ diagram for output _ reference yref ¼ 25qref þ q_ ref , position q, reference position qref , position error eq and y ¼ 25q þ q, e_ q ðeq Þ diagram. The convergence gain is k ¼ 100 and a ¼ 1


167

Figure 4.31 Changes in reference yref , output y and output error ey, e_ y ðey Þ diagram for output _ reference yref ¼ 25qref þ q_ ref , position q, reference position qref , position error eq and y ¼ 25q þ q, e_ q ðeq Þ diagram. The convergence gain is k ¼ 100 and a ¼ 0:75

e_ y ðey Þ diagram. The right column shows the position reference qref , its actual value q, the position control eq error and the e_ q ðeq Þ diagrams. Figure 4.31 shows 0:5 the same transients as in Figure 4.30 but with convergence acceleration q€ con ¼ 100ey sign ðey Þ. The left column shows the output reference yref , the output y, the output control error ey and the e_ y ðey Þ diagram. The right column shows the position reference qref , position actual value q, the position control error eq and the e_ q ð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. eq 4.8.1 Application of Estimated ^teq and q€^

If the control input is implemented with the equivalent force estimation ^teq instead of its exact value, the input force and the closed loop dynamics can be expressed as t ¼ ^teq an g 1 ks ¼ ^teq þ tcon eq s_ þ ks ¼ gan 1 ð^teq teq Þ ¼ gan 1 peq t ðQ; t Þ

ð4:130Þ

168


The closed loop dynamics now depend on the equivalent force estimation error peq t ð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 gan t ¼ ðs_ þ ksÞ ðs_ þ ksÞ dz g g

ð4:131Þ

0

The integral action would 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 eq acceleration q€^ ) are applied and the control input is eq t ¼ ^tdis þ an q€^ an g 1 ks

ð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)


169

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)

Inserting (4.132) into (4.95) yields the dynamics of the closed loop s_ þ ks ¼ gan 1 peq € eq Þ q€ ðQ; q

ð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 € eq Þ estimation error since the disturbance estimation error q€ ðQ; q 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.

170


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 q€ des þ ^tdis Þ

^tdis ¼ Qtdis ; q€ des ¼ an q€ eq an g 1 ks

eq

t ¼ sat½an ð€ q con þ q€^ Þ þ ^tdis

^tdis ¼ Qtdis ; q€^

eq

¼ Q€ q eq ;

q€ des ¼ an g 1 ks t ¼ satðan q€ con þ ^teq Þ

^teq ¼ Qteq ; q€ des ¼ an g 1 ks

ð4:134Þ

ð4:135Þ ð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 q€ des þ ^tdis Þ ^tdis ¼ Qtdis ; q€ des ¼ KP eq þ KD e_ q þ y€ref

ð4:137Þ

Here eq ¼ yref yðqÞ is the control error, e_ q 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 eq t ¼ ^tdis þ an ðq€^ þ q€ con Þ

ð4:138Þ

The disturbance ^tdis and equivalent acceleration q€êq are estimated by observers discussed in detail in Examples 4.1 and 4.5. Filter gain is g ¼ 600.


171

Figure 4.34 Changes in reference yref , output y and output error ey, e_ y ðey Þ diagram for output _ reference yref ¼ 25qref þ q_ ref , position reference qref , position q, position error eq and y ¼ 25q þ q, e_ q ðeq Þ diagram. The convergence gain is k ¼ 100 and a ¼ 0:75. Both the equivalent acceleration and the disturbance torque are estimated

0:5 In Figure 4.34, the transients with convergence acceleration q€ con ¼ 100ey signðey Þ are shown. In the left column, the reference yref , the output y, the control error ey and the e_ y ðey Þ diagram are shown. In the right column, the position reference qref , the position q, the position control eq error and the e_ q ð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Þ rad _ ¼ 25q þ q. _ The reference output is yref ¼ 25qref þ q_ ref and the control and the output is yðq; qÞ ref error ey ¼ 25eq þ e_ q, eq ¼ q q . In Figure 4.35, the transients with convergence acceleration q€ con ¼ kc1 1 ey m sign ðey Þ; m > 0 are shown. In the left column, the reference yref , the output y, the control error ey and the e_ y ðey Þ diagram are shown. In the right column, the position reference qref , the position q, the position control eq error and the e_ q ð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.

172


Figure 4.35 Evolution of reference yref , output y and output error ey, e_ y ðey Þ diagram for output _ reference yref ¼ 25qref þ q_ ref , position reference qref , position q, position error eq and e_ q ðeq Þ y ¼ 25q þ q, diagram. The controller parameters are k ¼ 100 and m ¼ 0:1. Both the equivalent acceleration and the disturbance 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) Sliding Mode 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.


173

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 and manipulation 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 force may 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.


176


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 for motion without 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 and De 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 parameters Me , Ke and De are assumed unknown with known upper and lower bounds. The interaction force can be modeled as ( τe ¼

Me ðq€ q€ e Þ þ De ðq_ q_ e Þ þ 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 we will 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

177

Interactions and Constraints

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 Kemin Ke Kemax and 0 De Dmax may vary significantly depending on the e 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 spring will represent the contact properties and the model (5.1) will degenerate to τe ðq; qe Þ ¼

Ke ðq qe Þ if bodies are in contact 0

if bodies are not 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 Þ ¼ τ τ ¼ KT iref

ð5:3Þ

The meaning of the parameters and the variables in model (5.3) is already given in Chapter 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 external force 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 generalized plant disturbance has been discussed in Chapters 3 and 4. The structures of the disturbance observer discussed in Chapter 4 are directly applicable in this case also. The closed loop motion is robust against all disturbances and thus rejects the influence of the interaction force as well. In many 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 system with 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 in most 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].

178


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€ ¼ q€ des þ an 1 pðQ; τdis Þ with the desired acceleration q€ des as the output of the force controller and the disturbance compensation error pðQ; τdis Þ. From (5.3), the input force is τ ¼ an q€ des þ ^τdis ¼ Kn iref . 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 ¼ Kn ides ¼ 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 τref e 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_ q_ e Þ. In further analysis it will be assumed that the interaction force is measured or estimated and qe and q_ e are bounded continuous functions. This allows us to treat the force control problem in more general way assuming contact with a body moving 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 gain Cf > 0 and desired acceleration q€ des ¼ 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 τ ¼ Kn iref ¼ ^τdis þ an q€ des F ¼ ^τdis þ an Cf τref τe ; Cf > 0 ¼ ^τdis þ an Cf τref Ke qe De q_ e an Cf ðKe q þ De q_ Þ

ð5:4Þ

Insertion of control force (5.4) into the plant dynamics (5.3) yields q€ þ Cf De q_ þ Cf Ke q ¼ Cf τref þ Cf De q_ e þ Cf Ke qe

ð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

179


Figure 5.2

Structure of the interaction force control system

_ q€ des ¼ Cf τref τe Cv q; τ

des

¼

ides ¼

Cf ; Cv > 0

an q€ des F an des q€ Kn F

ð5:6Þ

results in the closed loop dynamics q€ þ Cf De þ Cv q_ þ Cf Ke q ¼ Cf τref þ Cf De q_ e þ Cf Ke qe

ð5:7Þ

Now damping is determined by Cf De þ 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 2

eF

3 De s þ Ke 5τref s2 þ Cf De þ Cv s þ Cf Ke 2 3 D s þ K e e 5ðDe s þ Ke Þqe ¼ þ 41 Cf 2 s þ Cf De þ Cv s þ Cf Ke ¼ 41 C f

ð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 common with reference to the error transfer function. A third zero at Ke De 1 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 general Ke is high and consequently Cf 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 motion will be used in

180


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

τ ¼ KT i di ¼ gc iref i dt

ð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€ q€ e Þ þ De ðq_ q_ e Þ þ Ke ðq qe Þ if bodies are in contact 0 if bodies are not 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. Coefficients Memin Me Memax , Kemin Ke Kemax and 0 De Dmax may vary significantly depending on the mass and properties of the bodies e 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 vqe t m

ð5:11Þ

The coefficients qe0 , qe1 and vqe will be set for particular experiments. The reference force is defined as h i ref τref ðtÞ ¼ τref 0 1 þ τ1 sin ðvτ tÞ N

ð5:12Þ

ref Coefficients τref 0 ; τ1 ; 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)

τ ¼ Kn iref ¼ ^τdis þ an q€ des F ; des ref q€ F ¼ Cf τe τ þ Cv q_

ð5:13Þ

181


Cf ; Cv > 0 are design parameters. Here q€ des F stands for the desired acceleration enforcing ^τdis is estimated by disturbance observer as the interaction force tracking. The disturbance 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 Kn iref þ an gq_ z ; ^τdis ¼ z an gq_

g>0

ð5:14Þ

For a system with actuator, having nominal force constant Kn , the reference current can be determined as iref ¼ Kn 1 τ ¼ Kn 1 ^τdis þ an q€ des F

ð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 ¼ 0 m, Ke ¼ 250 000 N m–1 and De ¼ 5 kg s–1. Controller parameters are Cf ¼ 20, Cv ¼ 100, τmax ¼ 25 N

182


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 ¼ 0 m, Ke ¼ 250 000 N m–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 τref and 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 and the 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_ q_ e Þ, 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 system motion is constrained to satisfy ref SF ¼ q; q_ : ½Ke ðq qe Þ þ De ðq_ q_ e Þ τref ¼0 e ðtÞ ¼ eF τe ; τe

ð5:17Þ

Plant (5.3) with disturbance feedback reduces to double integrator (4.96) with desired acceleration q€ des 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 system position 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 e_ F ¼ De q€ De

1 ref Ke Ke τ_ e þ q_ e þ q€ e q_ De De De

ð5:18Þ

183


The equivalent acceleration enforcing zero rate of change of force control error is q€ eq €e þ Fn ¼ q

τ_ ref Ke ðq_ e q_ Þ þ e De De

ð5:19Þ

The equivalent acceleration depends on the system velocity, the parameters of the environment, and the derivative of the desired force. For De ! 0 (5.19) gives large value for q€ eq Fn and for systems having De ¼ 0 expression (5.19) is not applicable. Parameters of the environment Ke and De are not known, thus q€ eq Fn 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_ q_ e Þ can be treated as a part of the equivalent acceleration. Then the derivative of the force tracking error on the trajectories of the system (5.3) with compensated disturbance could be rearranged into

τ_ ref Ke DDe De _eF ¼ Den q€ des q€ e _ _ ðq qe Þ þ ðq€ q€ e Þ þ pðQ; τdis Þ Den Den Den Den an

ð5:20Þ

From (5.20) the equivalent acceleration is q€ eq €e F ¼q

τ_ ref Ke DDe De ðq_ q_ e Þ þ ðq€ q€ e Þ þ pðQ; τdis Þ Den Den Den Den an

ð5:21Þ

Now estimation of the equivalent acceleration is straightforward. Insertion of the desired acceleration q€ des and the force control error eF into the equivalent acceleration observer (4.70) yields 1 z_ ¼ gdF q€ des þ Den gdF eF z eq 1 q€^ F ¼ z Den gdF eF ; 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_ 2kF V a ; kF > 0; a ¼ 1, from eF e_ F ¼ 2kF V a ¼ kF e2F yields eF ðe_ F þ kF eF Þ ¼ 0

ð5:23Þ

For eF 6¼ 0 the convergence acceleration can be from (5.23) determined as 1 q€ con F ¼ Den kF eF

ð5:24Þ

184


and the desired acceleration becomes

eq 1 q€ des ¼ satq q€^ F Den kF eF

ð5:25Þ

The control force and the actuator reference current are h

eq i 1 τ ¼ satT ^τdis þ an q€^ F Den kF eF 2 3

eq ^ τ a dis n 1 q€^ Den þ kF eF 5 iref ¼ satI 4 Kn Kn F

ð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 ¼ fq; q_ : e_ F þ kF eF ¼ 0g 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 ¼ fq; q_ : e_ F þ kF eF ¼ 0g

eq e_ F ¼ Den q€ des q€ eq €^F q€ eq F ¼ Den q F kF e F e_ F þ kF eF ¼ Den pF ðQdF ; q€ eq FÞ

:

ð5:27Þ

Dynamics (5.27) is enforced if control variables do not saturate. Here pF ðQdF ; q€ eq F Þ 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_ q_ e Þ τref e ¼ eF into (5.27) expressed as De D€ q e þ ðKe þ kF De ÞDq_ e þ kF Ke Dqe ¼ τ_ ref þ kF τref Dqe ¼ q qe

ð5:28Þ

Roots of the characteristic equation s2 þ ðKe =De þ kF Þs þ kF Ke =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 for Den 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.

185


Figure 5.4

Structure of a force control system with Den > 0

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 disturbance ^τ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 q€ des F and the input force are selected as

eq q€ des ¼ satq q€^F þ q€ con F F τ ¼ satT ^τdis þ an q€ des F

ð5:29Þ

186


The equivalent acceleration is estimated by observer (5.22) with observer filter gain gdF ¼ 600 and Den ¼ 5 kg s–1. The convergence acceleration q€ con is selected to enforce finite-time F convergence 1 2a 1 1 q€ con sign ðeF Þ; < a < 1 F ¼ kDen jeF j 2

ð5:30Þ

ref

eF ¼ τe τ

The reference actuator current is calculated as iref ¼ Kn 1 τ ¼ Kn 1 ^τdis þ an q€ des F

ð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 000 N m–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 000 N m–1 and De ¼ 5[1 þ 0.5 sin (6.28t)] kg s–1. Controller parameters are a ¼ 0:80, k ¼ 50, τmax ¼ 25 N.

187


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 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 Þ τref e ðtÞ ¼ eF ðq; qe Þ ¼ 0

ð5:32Þ

ref The control error in (5.32) can be rewritten as eF ¼ Ke q τref e þ Ke qe ¼ Ke q Ke qe , 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 e€F þ k1 e_ F þ k2 eF ¼ 0;

k1 ; k2 > 0

ð5:33Þ

The desired acceleration which enforces transient (5.33) on the trajectories of the system (5.3) with disturbance compensation and dynamics (4.96) is q€ des ¼ q€ e þ

τ€ref k1 e_ F þ k2 eF e an 1 pðQ; τdis Þ Ke Ke

ð5:34Þ

The exact value of the spring constant Ke is not known in most practical cases. In further development we will assume the nominal value of the spring constant Ken known and variation of Ke determined by continuous function DKe with known lower and upper bounds. Then (5.34) can be rearranged into q€ des ¼ q€ e þ

ref τ€ ðtÞ DKe ðq€ q€ e Þ k1 e_ F þ k2 eF an 1 pðQ; τdis Þ Ken 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 q€ eq F

ref τ€ ðtÞ DKe ðq€ q€ e Þ ¼ q€ e þ an 1 pðQ; τdis Þ Ken

ð5:36Þ

188


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 e€F ¼ 0 Y e_ F ¼ 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 q€ des ¼ q€ eq € con F þq F q€ con ¼ Ken 1 ðk1 e_ F þ k2 eF Þ F

ð5:37Þ

In constructing the equivalent acceleration observer the desired acceleration q€ des 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 Ken 1 e_ F ¼ j j_ ¼ q€ des þ q q_ ¼ 0

ð5:38Þ

System (5.38) with inputs q€ des ; eF and q ¼ q€ eq F as output is observable. Let introduce new variables z1 and z2 z1 ¼ q l1 e*F ;

e*F ¼ Ken 1 eF ;

z2 ¼ j l2 e*F ;

l2 ¼ const

l1 ¼ const

ð5:39Þ

Differentiation of z1 and z2 on the trajectories of system (5.38) yields z_ 1 ¼ l1 z2 þ l2 e*F z_ 2 ¼ z1 l2 z2 þ l1 l22 e*F þ q€

ð5:40Þ

The observer should have the same dynamics as in (5.39) ^z_ 1 ¼ l1 ^z2 þ l2 e*F ^z_ 2 ¼ ^z1 l2^z2 þ l1 l22 e*F þ q€ des ^ ¼ q

ð5:41Þ

^z1 þ l1 e*F

From (5.41) the estimated equivalent acceleration is governed by ^€ þ l2 q ^_ þ l1 q ^ ¼ l1 q€ des e€* q F eq ^ q€^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 1 eq q€ des ¼ satq q€^F ðk1 e_ F þ k2 eF Þ Ken

ð5:43Þ

189


and, the input force and the actuator reference current can be obtained as 8 9 < = a eq n τ ¼ satT ^τdis þ an q€^F ðk1 e_ F þ k2 eF Þ : ; Ken iref ¼ satI

8 < :

0 eq Kn 1 @^τdis þ an q€^F

19 = an ðk1 e_ F þ k2 eF ÞA ; Ken

ð5:44Þ

Due to error in the equivalent acceleration estimation peq € eq Þ, with QeF standing F ðQeF ; q for equivalent acceleration filter in (5.42), the closed loop dynamics of the force tracking error are

e€F þ k1 e_ F þ k2 eF ¼ Ken peq € eq ; F QeF ; q

k1 ; k 2 > 0

ð5:45Þ

The result coincides with position control. Formally position q is forced to track position qe with 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 is modeled as in (5.10) with Me ¼ 0 kg, Ke ¼ 250 000 N m–1 and De ¼ 0 kg m–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Þ

190


Figure 5.6 Transients in force control with the environment model as an ideal spring Me ¼ 0 kg, Ke ¼ 250 000 N m–1 and De ¼ 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 plant input force τ are shown. Controller parameters are a ¼ 0:75, k ¼ 50, τmax ¼ 25 N

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 e_ qF ¼ ðτ_ e þ Cv q€ Þ τ_ ref ¼ Cv q€ des Cv 1 τ_ ref Ke ðq_ q_ e Þ pðQ; τdis Þ

ð5:47Þ

1 ref The equivalent acceleration is q€ eq τ_ Ke ðq_ q_ e Þ pðQ; τdis Þ . The convergence F ¼ Cv q 1 acceleration q€ con F ¼ kCv eF ; k > 0 is enforcing exponential convergence. Finite-time

191


convergence is enforced by q€ con ¼ kCv 1 jeqF j F

2a 1

1 sign ðeqF Þ; < a < 1 2

ð5:48Þ

eqF ¼ ðτe þ Cv q_ Þ τref

_ For small Cv and low velocities In a steady state eqF ¼ 0 and the force control error is eF ¼ Cv q. this error may be acceptable. The desired acceleration, the input force and the actuator current can be expressed as

eq q€ des ¼ satq q€^F þ q€ con F F τ ¼ satT ^τdis þ an q€ des F iref ¼ satI Kn 1 τ ¼ satI Kn 1 ^τdis þ an q€ des F

ð5:49Þ

eq The acceleration q€^F is estimated by observer (5.22) with filter gain gdF ¼ 600, desequivalent q q€ ; eF 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 observer parameters, 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 jτmax 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 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 q_ max ¼ 0:1 0:5 10 3 ¼ 5 10 5 N 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_ q_ e Þ 0 in push mode Ke2 ðq qe Þ þ De2 ðq_ q_ e Þ 0 in pull mode τe ¼

Ke1 ðq qe Þ 0 Ke2 ðq qe Þ 0

in push mode in pull mode

ð5:50Þ

ð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

192


Figure 5.7 Illustration of body deformation by push pull forces

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_ q_ e Þ ¼ 0 Y D1 ¼ ðq1 qe Þ ¼

De1 ðq_ q_ e Þ Ke1

ð5:52Þ

Ke2 ðq qe Þ þ De2 ðq_ q_ e Þ ¼ 0 Y D2 ¼ ðq2 qe Þ ¼

De2 ðq_ q_ e Þ Ke2

ð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 _ q_ e Þ þ Ke2 ðq qe Þ þ De2 ðq_ q_ e Þ τfb e ¼ Ke1 ðq qe Þ þ De1 ðq

ð5:54Þ

Now the problem of the push pull force can be formulated as keeping the state of the system in the manifold pp ref SF ¼ q; q_ : τfb e ðq; qe Þ τ ðtÞ ¼ eF ð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). If D1 > 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 same value for Den 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.


193

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

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 of moving 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

194


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 usually measured – thus available during whole motion 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. In motion 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 that means making 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 function qðτe Þ. Since the validity of the specification of the control problems

195


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: _ q€ : eðqÞ ¼ y yref þ qðτe Þ Sðq; τe Þ ¼ q; q;

ð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 n o _ q€ : q€ des Sðq; τe Þ ¼ q; q; € des € eq € con q;F ¼ q q þ qðτe Þ ¼ q q þq q þ qðτe Þ

ð5:57Þ

Here q€ des q stands for the desired acceleration of the free motion (no interaction with the environment), q€ des q;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 in Chapters 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 then no 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

196


Figure 5.9

Modification of the position reference proportional to the interaction force

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 in Figure 5.9. The output tracking is modified 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 between modes 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 s_ q;F ¼ s_ q þ l_τe

ð5:59Þ

The derivative of the generalized output tracking error is given in (3.22). Inserting (3.22) into (5.59) yields s_ q;F ¼ g

q€ q€ eq q

l eq _ þ l_τe ¼ g q€ q€ q τe g

ð5:60Þ

Here q€ eq q 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.10

Interpretation of manifold shift in a modification of output tracking by the interaction force

197


only a change in the equivalent acceleration q€ eq € eq q;F ¼ q q

l τ_ e g

ð5:61Þ

The equivalent acceleration q€ eq q;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 s_ q;F ¼ ks2q;F . For sq;F 6¼ 0 the convergence acceleration can be found as 1 q€ con sq;F ; q;F ¼ kg

k>0

ð5:62Þ

The desired acceleration, desired force and desired actuator current can be expressed as 1 q€ des € eq sq;F ; k > 0 q;F kg q;F ¼ q

1 € eq sq;F τdes ¼ an q€ des q;F kg q;F ¼ an q

1 1 € eq sq;F ides ¼ Kn 1 an q€ des q;F kg q;F ¼ Kn an q

ð5:63Þ

This acceleration will enforce sq;F ðtÞ t !! ¥ 0 from initial state s q;F ð0Þ 6¼ 0 consistent with operational bounds on control input and system variables. The desired acceleration (5.63) enforces the closed loop transient s_ q;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 s_ q þ 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 s_ q þ 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_ q_ e Þ þ Ke ðq qe Þ and the output generalized tracking error be sq ¼ cDq þ c1 Dq; c; c1 > 0 and position tracking error Dq ¼ q qref . Then (5.65) yields c1 D€ q þ ðc þ c1 kÞDq_ þ kcDq ¼ ½lDe ðq€ q€ e Þ þ lðKe þ kDe Þðq_ q_ e Þ þ 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,

198


by separation of variables Equation (5.66) can be rearranged into 2 0

1 0 13 c K e ðc1 þ lDe Þ€ q þ 4c1 @ þ kA þ lDe @ þ kA5q_ þ kðc þ lKe Þq De c1 0

2 ¼ c1 4q€

ref

1 0 1 3 2 3 c c K K e e ref ref þ @ þ kAq_ þ k q 5 þ lDe 4q€ e þ @ þ kAq_ e þ k qe 5 c1 c1 De De

ð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 in contact point Ke ; De and the compliance coefficient l. The parameters c; c1 ; k; l can be selected such 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 ¼

Zc* lZ * qref þ * e * qe Zc* þ lZe* Zc þ lZe

Zc* ¼ c1 s2 þ ðc þ c1 kÞs þ kc

ð5:68Þ

Ze* ¼ De s2 þ ðKe þ kDe Þs þ kKe

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

ref

kðc þ lKe Þq ¼ k cq 0

ref cq þ lKe qe þ lKe qe Y q ¼ ðc þ lKe Þ 1

@ c qref þ lqe A q¼ c þ l Ke Ke 1

ð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_ q_ e Þ and (5.69) as Ke c ref q qe ðc þ lKe Þ qe

τe ¼ Ke ðq qe Þ ¼ c ref q τe ¼ c þl Ke

ð5:70Þ

199


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 (high Ke ) 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 the case 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 by sq ¼ 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 000 N m–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_ q_ ref

ð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

eq q€ des ¼ satq q€^ þ q€ con τ ¼ satT ^τdis þ an q€ des iref ¼ satI Kn 1 τ ¼ satI Kn 1 ^τdis þ an q€ des

ð5:73Þ

eq

Here q€^ stands for equivalent acceleration estimated by observer (5.22). The observer filter _ iref gain is set at gdF ¼ 600. The disturbance ^τdis is estimated by disturbance observer with q; 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 jτmax j ¼ 25 N. The convergence acceleration is selected as 0:6 q€ con ¼ 50eq sign eq

ð5:74Þ

200


Figure 5.11 Trajectory tracking modulation by an interaction force with object Me ¼ 0.25 kg, Ke ¼ 250 000 N m–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 τdis and control input force τ. The controller parameters are k ¼ 50 and a ¼ 0:8.

The trajectory modification due to interaction force is realized by modification of the control tracking error e ¼ 100 q qref þ q_ q_ ref þ 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

201


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 1 q€ des € eq sq q ¼q q kg

ð5:76Þ

Here q€ eq q is the equivalent acceleration determined from the position tracking task, s q 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τe into (5.76) to obtain 1 q€ des € eq sq q;F ¼ q q kg |fflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflffl} trajectory tracking

Figure 5.12

lτe |{z} modification

Modification of the desired acceleration by interaction force

ð5:77Þ

202


The coefficient l is assumed strictly positive constant. The desired input force or input current is then expressed as

des eq 1 an lτe τdes ¼ a q € ¼ a q € kg s n n q q;F q;F q

des 1 1 iq;F ¼ Kn 1 an q€ des € eq sq Kn 1 an lτe q;F ¼ Kn an q q kg

ð5:78Þ

The insertion of the modification term as acceleration or as the force does not have any difference. The input force τ ¼ ^τdis þ τdes τdis and q;F is just a sum of the estimated disturbance ^ the desired force. If interaction force is zero, the position tracking loop operates without any change and the closed loop dynamics are s_ q þ k sq ¼ 0. When the interaction force is different from a zero closed loop the dynamics enforced by the desired acceleration (5.77) become s_ q þ 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_ q_ e Þ þ Ke ðq qe Þ and the generalized position tracking error sq ¼ cDq þ c1 Dq; 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Þq_ ref þ kcqref þ lðDe q_ e þ Ke qe Þ

ð5:80Þ

If the parameters in (5.80) are constant by taking a Laplace transformation and solving it for q yields q ¼

Zc*

Zc* lZe qref þ * qe þ lZe Zc þ lZe

ð5:81Þ

Zc* ¼ c1 s2 þ ðc þ c1 kÞs þ kc Ze ¼ De s þ Ke

The characteristic equation Zc* þ 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 þ lKe qe kc þ lKe k

¼

k

c Ke

ref

q c þl Ke

þ

l

k

qe c þl Ke

ð5:82Þ

203


and the interaction force at position (5.82) becomes τe ¼ Ke ðq qe Þ ¼ ¼

Ke kc ref q qe kc þ lKe kc ref q qe kc þl Ke

ð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 interaction may require a high force feedback gain l and then the force measurement noise may 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. That makes 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 account while designing the controller since a limit on the convergence acceleration may 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 N m–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).

204


The trajectory modification due to interaction force is realized by modification of the desired acceleration

eq

eq q€ des ¼ satq q€^ þ q€ con lτe τ ¼ satT ^τdis þ an q€ des iref ¼ satI Kn 1 τ ¼ satI Kn 1 ^τdis þ an q€ des

ð5:84Þ

Here q€^ 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 _ iref as inputs and ðan ; Kn ; gÞ as observer parameters, thus it observer (4.34) with q; includes inertia variation force, actuator dynamics induced forces and the interaction force. The observer filter gain is g ¼ 600. The control force 0:6 is limited by jτmax j ¼ 25 N. The convergence acceleration is q€ con ¼ 50eq sign eq , and the generalized trajectory tracking error is eq ¼ 100ðq qref Þ þ ðq_ q_ ref Þ. 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 000 N m–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 τ. The convergence gain is kept constant k ¼ 50 and a ¼ 0:8.


205

environment qe and the position control error eq. In the right column the force τe , the disturbance τ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 modification: 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 it if 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 1 q€ des € eq sq where sq ¼ sðDq; Dq_ Þ, Dq ¼ q qref stands for the generalized output q ¼ q q kg tracking error, k is the convergence coefficient and g is gain defined in (3.22); q€ eq q 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 1 model the desired acceleration in the force tracking loop is q€ des € eq € eq F kF Den eF . Here, q F F ¼q stands for the equivalent acceleration in the force control loop [see Equation (5.21)], kF stands 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.

206


The position tracking and force tracking algorithms are well established solutions. The force is not measured (the signal from force measurement 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 1 q€ des € eq sq . Let us find additional acceleration input q€ cmp such that system remains F q ¼ q q kg 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 q€ des € cmp due to interaction force can be F q and additional acceleration q expressed as q€ ¼ q€ des € cmp F q þq 1 ¼ q€ eq sq þ q€ cmp F q kg |ffl{zffl} |fflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflffl} trajectory tracking

ð5:85Þ

modification

The interaction force is modeled by τe ¼ De ðq_ q_ e Þ þ Ke ðq qe Þ and the desired closed loop transient in force control is e_ F þ kF eF ¼ 0. Then the desired acceleration in force control 1 _ F þ kF eF ¼ 0 the compensating term is q€ des ¼ q€ eq F Den kF eF . By inserting (5.85) into e cmp q€ F is determined in the following form

€ des € cmp € eq e_ F þ kF eF ¼ Den ðq€ q€ eq F Þ þ kF eF ¼ Den q F q F þ kF eF ¼ 0 q þq 1 € des ¼ q€ eq € des € des q€ cmp F F Den kF eF q q ¼q F q q

ð5:86Þ

The overall control is defined by the following relation q€ des ¼ q€ des € cmp F q þq ( 0 q€ cmp ¼ F 1 € des q€ eq F q q Den kF eF

motion without contact

ð5:87Þ

motion in contact

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 are Me ¼ 0 kg, Ke ¼ 5000 N m–1 and De ¼ 5 kg s–1. The reference position is qref ¼ 0:01½1 þ 0:3 sin ð6:28tÞ m. The position of environment ref is qe ðtÞ ¼ 0:0065 m and the reference Þ¼ force is τ ðtref 15½1 þ 0:25 sin ð12:56tÞ N. The ref þ q_ q_ and the force tracking error is position tracking error is eq ¼ 100 q q eF ¼ τe τref .

207


Figure 5.14

Trajectory tracking modification and interaction force control

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 modification of the desired acceleration

eq q€ des ¼ satq q€^ þ q€ con τ ¼ satT ^τdis þ an q€ des iref ¼ satI Kn 1 τ ¼ satI Kn 1 ^τdis þ an q€ des

ð5:88Þ

0:6 sign eq . Here q€êq stands for The convergence acceleration is selected q€ con q ¼ 50 eq the estimated equivalent acceleration. The disturbance ^τdis is estimated by the disturbance ref _ i as inputs and ðan ; Kn ; gÞ as observer parameters, thus it includes observer (4.34) with q; inertia variation force, actuator dynamics induced forces and the interaction force. The observer filter gain is g ¼ 600. The control force is limited by jτmax j ¼ 25 N. The control input in the force control loop is selected as

eq €^ F þ q€ con q€ des F ¼ satq q F τ ¼ satT ^τdis þ an q€ des F iref ¼ satI Kn 1 τ ¼ satI Kn 1 ^τdis þ an q€ des F

ð5:89Þ

eq

Here q€ des €^F is estimated F stands for the desired acceleration, the equivalent acceleration q 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 0:6 q€ con F ¼ 10jeF j signð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 solution which involves switching between two tasks. Let us look here at a slightly different approach. Motion along the desired trajectory is performed with the

208


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 free movement or in contact with the environment. The interaction force is determined by the relative position of the system and the environment. In a mathematical 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 jqj > jqe j and is equal to zero for jqj jqe j. The desired acceleration follows the same relationship. This allows the application of a very simple algorithm in the selection of the desired acceleration

q€ des ¼ min q€ des € des ;q > 0 F ;q q

ð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 , force error eF and control input force τ

209


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 q€ des þ pðQ; τdis Þ such that the closed loop system exhibits impedance Z¼

τe K ¼ Ms þ D þ s v

ð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 q€ des ¼ q€ ref M 1 D q_ q_ ref þ 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 q€ des þ pðQ; τdis Þ to obtain M q€ q€ ref þ D q_ q_ ref þ K q qref ¼ τe þ Man 1 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.

210


Figure 5.16

Structure of impedance control based on a model following trajectory tracking

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€ ¼ q€ des þ an 1 p Q; τdis into (5.95) yields the desired acceleration q€ des ¼ am 1 ðτe bm q_ Þ an 1 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.

211


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€ ¼ q€ des þ am 1 bm q_ þ p Q; τdis . Then by selecting q€ des ¼ am 1 τ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 to mismatch 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 ðt d am q_ þ bm q τe dz ¼ 0 am q€ þ bm q_ τe ¼ dt 0

ð5:97Þ

Ðt The eq ¼ am q_ þ bm q 0 τe dz can be interpreted as a position control error and the standard position controller Ð t design procedure can be applied. This solution has a problem with open loop integration 0 τe dz 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 q€ des . Then the velocity is integral to the desired acceleration and the position can be expressed as an integral of velocity ð v ¼ q€ dj;

ðð q¼

q€ djdz

ð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 q€ des þ pðQ; τdis Þ τe

ð5:99Þ

From (5.98) the external force is readily expressed in the acceleration dimension as q€ ¼ q€ des an 1 τe ¼ q€ des q€ int þ an 1 pðQ; τdis Þ τe ¼ an q€ int

ð5:100Þ

The interaction force is now represented in terms of acceleration τe ¼ an q€ int . Expressions (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 specified up 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

212


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€ q€ ref ð5:101Þ

q_ ð0Þ ¼ vð0Þ ¼ 0 qð0Þ ¼ 0

Let the desired acceleration be ðt

ðt

0

q€ des ¼ q€ ref Kd e d j Kp @0 þ 0

0

ðt

1 e d jAdz

ð5:102Þ

0

Then the motion of the system is governed by the following dynamics q€ ¼ q€ ref Kd q_ q_ ref 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 in which there are some other means 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 Ðt the acceleration dimension leads to an q€ ¼ an q€ des þ gan q€ des q€ dz, thus the acceleration 0

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 of motion specification in the acceleration dimension. We will 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 q€ int ¼ an 1 τint tracking its smooth reference 1 ref q€ ref int ¼ an τint .

213


Assume the interaction force is modeled as a spring–damper system, thus the interaction force induced acceleration is q€ int ¼

τint Kp ðq qe Þ þ Kd ðq_ q_ e Þ ¼ an an

ð5:104Þ

Then the acceleration tracking error is eint ¼ q€ int q€ ref int . 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

q€ ref ¼ Cf eint ¼ Cf q€ int q€ ref int

ð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 q€ des ¼ Cff q€ mes q€ ref Cfd q_

ð5:106Þ

Here q€ mes stands for the measurement of the actual acceleration and Cfd q_ stands for velocity induced 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 q€ mes ¼ q€ þ an 1 pðQ; τdis Þ

ð5:107Þ

The reference acceleration (5.105) with measured acceleration (5.107) yields closed loop dynamics of compensated system q€ ¼ q€ des þ an 1 pðQ; τdis Þ ¼ Cff q€ mes q€ ref Cfd q_ þ an 1 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€ þ an Cfd þ Cff Cf Kd q_ þ Cff Cf Kp q ¼ _e ¼ Cff Cf τref int þ 1 þ Cff pðQ; τdis Þ þ Cff Cf Kp qe þ Kd q

ð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 steadystate solution for the interaction force and the position are τe ¼ Ke ðq qe Þ ¼ τref þ 1 þ Cff pðQ; τdis Þ 1 þ Cff τref þ qe þ pðQ; τdis Þ q ¼ Ke Ke

ð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.

214


Example 5.7 Force Control in Acceleration Dimension Here we would 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€ ¼ q€ des þ an 1 pðQ; τdis Þ. The acceleration due to the interaction force τe can be expressed as q€ int ¼ an 1 τe . Similarly the reference force 1 ref induced acceleration can be expressed as q€ ref int ¼ an τ . Assume that plant acceleration is ^ measured or estimated and is represented by q€ ¼ q€ mes. Under these conditions the desired acceleration which will enforce convergence of the interaction force induced acceleration q€ int to the reference force induced acceleration q€ ref int can be expressed as

q€ ref ¼ Cf eint ¼ Cf q€ int q€ ref int q€ des ¼ Cff q€ mes q€ ref Cfd q_

ð5:111Þ

Figure 5.17 Force control in the acceleration dimension. The transients in reference force τref and measured force τe , position difference eqe ¼ q qe , estimated acceleration q€^ ¼ q€ mes , force control error eF ¼ τe τref and the desired acceleration q€ des are shown. The controller gains are Cf ¼ 0:75, Cff ¼ 0:85, Cfd ¼ 250

215


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. The interaction force is modeled as in (5.10) with Me ¼ 0.5 kg, Ke ¼ 250 000 N m–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 qe0 ¼ 0:05 m. The disturbance ^τdis is estimated by a disturbance constant _ iref as inputs and ðan ; Kn ; gÞ as observer parameters. The observer filter observer with q; gain is g ¼ 600. The input force is τ ¼ KT i 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€^ ¼ q€ mes ¼ an 1 Kn iref ^τ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 , the position difference eqe ¼ q qe , the estimated acceleration q€^ ¼ q€ mes , the force control error eF ¼ τe τref and the desired acceleration q€ des . Force tracking with 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 two motion 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 systems we will 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€q m ¼ am€q des m þ pm ðQ; τmdis Þ as€q s

¼ as€q des s þ ps ðQ; τsdis Þ

ð5:113Þ

216


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 characterizing the body be Ke ; 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 ðq_ m q_ me Þ

ð5:114Þ

Fs ¼ Ke ðqse qs Þ þ De ðq_ se q_ s Þ

The model for the grasp force can be expressed as τgrasp ¼ Ke ðqm qs Þ þ De ðq_ m q_ s Þ þ dgrasp ðqme ; qse Þ dgrasp ðqme ; qse Þ ¼ Ke ðqse qme Þ þ De ðq_ se q_ me Þ

ð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, q_ me and q_ se are corresponding velocities. 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 τref grasp ðtÞ with continuous and bounded first time derivative. The grasp force error is then expressed as egrasp ¼ ðFm þ Fs Þ τref grasp

ð5:116Þ

In calculating the grasp force error (5.116), the grasp force is assumed measured or estimated. The dynamics of the grasp force control error may be expressed in the following form _ _ _ ref e_ grasp ¼ τ_ grasp τ_ ref grasp ¼ F m þ F s τ grasp

eq ¼ De q€ grasp q€ grasp ; q€ grasp ¼ ðq€ m q€ s Þ h i 1 ref τ_ grasp Ke ðq_ m q_ s Þ d_ grasp q€ eq grasp ¼ De

ð5:117Þ

Taking into account the equations of motion (5.113) the dynamic of the grasp force becomes

e_ grasp ¼ De q€ des € eq grasp q grasp þ De pgrasp q€ des € des € des grasp ¼ q m q s 0 1 pm ps A @ pgrasp ¼ am as

ð5:118Þ

217


Here the desired acceleration q€ des grasp is expressed as a difference between the ‘m’ side and ‘s’ side desired accelerations. System (5.118) describes first-order dynamics with desired acceleration q€ des € eq grasp as the input and ð q grasp þ 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 e_ grasp þ kgrasp egrasp ¼ 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 1 q€ des € eq grasp ¼ q grasp kgrasp De 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 q€ eq grasp 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 Þ. The control error (5.116) can be expressed as egrasp ¼ Ke ðqm qs Þ τref grasp . The resulting error dynamics (5.121) has relative degree two h i e€grasp ¼ τ€grasp τ€ref € m q€ s Þ τ€ref € ðqm ; qse Þ grasp ¼ Ke ðq grasp d |fflfflfflfflffl ffl{zfflfflfflfflfflffl} |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}

¼ Ke q€ grasp q€ eq grasp

q€grasp

Ke q€grasp eq

ð5:121Þ

With the dynamics of the compensated systems as in (5.113) the grasp force control error dynamics become

e€grasp ¼ τ€grasp τ€ref € des € eq grasp ¼ Ke q grasp q grasp € des € des q€ des grasp ¼ q m q s h i eq 1 ref q€ grasp ¼ Ke τ€grasp d€ ðqme ; qse Þ

ð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 q€ des grasp as input and q€ eq grasp as disturbance. Let the desired closed loop dynamics in grasp force tracking be e€grasp þ k1 e_ grasp þ k2 egrasp ¼ 0; k1 ; k2 > 0

ð5:123Þ

218

Figure 5.18


Grasp force error dynamics with interaction model (a) as in (5.115) and (b) as in (5.120)

For Ke 6¼ 0, from the desired closed loop dynamics (5.123) and (5.122), the desired acceleration can be determined as 1 q€ des € eq k1 e_ grasp þ k2 egrasp grasp ¼ q grasp Ke

ð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 q€ des is the result of s 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 1 q€ des € des € des € eq grasp ¼ q m q s ¼q grasp kgrasp De egrasp 1 1 q€ des € des € eq € eq m ¼ q s þq grasp kgrasp De egrasp ¼ q m kgrasp De egrasp

q€ eq m

¼

q€ des s

þ q€ eq grasp

ð5:125Þ

219


Consequently, the control forces on the ‘m’ and the ‘s’ systems are n

eq o τm ¼ sat ^τdism þ anm q€^m kgrasp De 1 egrasp n

eq o τs ¼ sat ^τdiss þ ans q€^ qs ks g 1 sqs

ð5:126Þ

Here anm ; ans stand for the nominal inertia of the ‘m’ and ‘s’ systems respectively, ^τdism ; ^τdiss are 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 the work 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 qref c with a smooth bounded first-order derivative q_ ref . Concurrently let us require the grasp force to track its smooth c

Figure 5.19 force

Grasp force control. System ‘s’ controls the movement and system ‘m’ controls the grasp

220


_ ref bounded reference τref grasp with a smooth bounded first-order derivative τ grasp . Let the position tracking error and force tracking error be as defined in (5.127) and (5.128) respectively þ q_ c q_ ref ; c>0 ec ¼ c qc qref c c

ð5:127Þ

_ d τref egrasp ¼ τgrasp τref grasp ¼ Ke qd þ De q grasp þ dgrasp ðqme ; qse Þ

ð5:128Þ

qd ¼ qm qs

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 ; q_ s Þ; ðqm ; q_ m Þ : 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

" þ q_ c q_ ref c qc qref c c

# ð5:130Þ

Ke qd þ De q_ d τref grasp þ dgrasp

with derivative " s_ c;grasp ¼

e_ c

#

e_ grasp

¼

" q€ ref c q_ c q_ ref c c

#

_ Ke q_ d τ_ ref grasp þ d grasp

" þ

0:5 0:5 De

De

#"

q€ m q€ s

# ð5:131Þ

To simplify the notation let us introduce the equivalent acceleration vector q € eq c;grasp and the ctr control vector q € c;grasp " q € eq c;grasp

¼ "

q € ctr c;grasp

¼

q€ eq c q€ eq grasp q€ ctr c

2

#

¼4 #

q€ ctr grasp

¼

_ c q_ ref q€ ref c c c q

_ d d_ grasp τ_ ref grasp Ke q " #" # 0:5 0:5 q€ m De

De

3 5 ð5:132Þ

q€ s

Insertion of (5.132) into (5.131) yields € eq € ctr s_ c;grasp ¼ q c;grasp þ q c;grasp

ð5:133Þ

The dynamics (5.133) describe two first-order systems € eq e_ c ¼ q€ ctr c q c e_ grasp ¼ q€ ctr € eq grasp q grasp

ð5:134Þ

with control inputs q€ ctr € m þ q€ s Þ and q€ ctr € m q€ s Þ. Selection of the control in c ¼ 0:5ðq grasp ¼ De ðq the systems in (5.134) is straightforward. Proportional controllers are sufficient to enforce

221


convergence to a zero tracking error eq q€ ctr € des €^ c kc ec c ¼q c ¼q eq

q€ ctr € des €^grasp kgrasp egrasp grasp ¼ q grasp ¼ q

ð5:135Þ

eq Here q€ des € des €^c and c and q grasp stand for the desired acceleration in motion and force control, q 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 kc and 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 eq q€^grasp

q€ des € des m ¼q c þ € des q€ des s ¼q c

q€ des grasp 2De q€ des grasp 2De

ð5:136Þ ð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

222


As expected, real control inputs depend on the object damping coefficient, as shown in the force control structure. on the ‘m’ To enforce the desired acceleration and ‘s’ side input forces des ^ and τ respectively. have form τm ¼ sat ^τdism þ anm q€ des ¼ sat τ þ a q € s diss ns s m 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 dynamics may be determined from enforced error dynamics e_ c þ kc ec ¼ 0 and e_ grasp þ kgrasp egrasp ¼ 0.

Figure 5.21

Control of grasp and position of the center of geometry

223


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 ref τref grasp ¼ Ke ðqm qs Þ and qm þ qs ¼ 2qc holds. The steady-state positions of the ‘m’ and ‘s’ systems may be derived as qm ¼ qref c þ

1 τref grasp 2 Ke

ð5:138Þ

qs ¼ qref c

1 τref grasp 2 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 same model, 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 ðq_ m q_ s Þ þ Me ðq€ m q€ s Þ 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 qref c ¼ 0:1½1 þ 0:2 sin ð6:28tÞ m. With these selections the control errors are ec ¼ qc qref c eF ¼ τe τref

ð5:141Þ

Since the control of qc ¼ qm þ qs is defined as position tracking, a generalized error s 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. _ ref ; c ¼ 100; c1 ¼ 1 s ¼ cqc þ c1 q_ c cqref c þ c1 q c

ð5:142Þ

224


The position tracking controller is designed as

eq q€ des €^ c þ q€ con c ¼ sat q c

ð5:143Þ eq

Here q€ des €^c is estimated c stands for the desired acceleration. The equivalent acceleration q by an observer. The observer filter gain is set at g ¼ 600. The convergence acceleration is selected as 0:6 q€ con c ¼ 100js j sign ðs Þ

ð5:144Þ

The force control error can be expressed as eF ¼ De q_ F þ Ke qF τref with qF ¼ qm qs . The desired acceleration in the grasp force control loop q€ des F can be expressed as

eq q€ des ¼ sat q€^F þ q€ con F F q€ con ¼ 100jeF j0:6 signðeF Þ F

ð5:145Þ

eq The equivalent acceleration q€^F can be estimated by observer. The observer filter gain is set at gdF ¼ 800. The desired accelerations q€ des € des c and q F enforce the center of geometry and force tracking. The plant accelerations can be determined from functional relations between the task accelerations q€ des € des € m and q€ s . From the c and q F and the configuration space accelerations q definition of the task space variables qc and qF it is easy to find

q€ des m ¼

q€ des € des c þq F 2

q€ des ¼ s

q€ des € des c q F 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-called modal 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 jmax τj ¼ 25N. Figure 5.22 shows the transients in the reference position of the center of geometry qref c , 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 q€ con € con control loops and the control errors ec F , the center of geometry q c and 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.


225

Figure 5.22 Control of grasp force and center of geometry, showing transients in the reference position ref of the center of geometry qref c , the position of the center of geometry qc, reference force τ , measured force τe , the position of the ‘m’ plant qm , the position of the ‘s’ plant qs , the convergence accelerations in the grasp force q€ con € con control loops and the control errors ec and eF F , the center of geometry q c

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 a wide range of applications – from remote repetition of movement and/or forces to training systems in which one master and more 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.

226


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 q€ des € des m and q s , 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 Þ

ð5:147Þ

x2 ¼ f2 ðqs ; qm Þ

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 q€ des € des m and q s such that motion is stable in the manifold Sx ¼ fqs ; qm : x1 ðqs ; qm Þ ¼ 0 and x2 ðqs ; qm Þ ¼ 0g

ð5:148Þ

A second time derivative of functions f1 ðqs ; qm Þ and f2 ðqs ; qm Þ can be derived as x € 1 ¼ ðg1s q€ s þ g1m q€ m Þ þ ðg_ 1s q_ s þ g_ 1m q_ m Þ x € 2 ¼ ðg2s q€ s þ g2m q€ m Þ þ ðg_ 2s q_ s þ g_ 2m q_ m Þ gis ¼

qxi 6¼ 0; qqs

gim ¼

qxi 6¼ 0; qqm

ð5:149Þ

i ¼ 1; 2

Formally, on the trajectories of system (5.113), we can introduce the following new variables q€ des € des € des x1 ¼ g1s q s þ g1m q m € des € des q€ des x2 ¼ g2s q s þ g2m q m ; 0 qeq x1

1 p p s mA ¼ g_ 1s q_ s g_ 1m q_ m @g1s þ g1m ans anm 0

q€ eq x2

1 p p s m A ¼ g_ 2s q_ s g_ 2m q_ m @g2s þ g2m ans anm

ð5:150Þ

227


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 x € 1 ¼ q€ des € eq x1 q x1

ð5:151Þ

x € 2 ¼ q€ des € eq x2 q x2

Dynamics (5.150) and (5.151) describe two virtual, decoupled second-order systems with equivalent accelerations q€ eq € eq € des € des x1 and q x2 , x1 and q x1 and desired accelerations q des des respectively. The desired accelerations q€ x1 and q€ x2 are linear in the desired accelerations q€ des € des of the ‘m’ and ‘s’ systems. The equivalent accelerations q€ eq € eq m and q s x1 and q x1 are linear in the velocities q_ m and q_ s 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 q€ des m and q€ des if q€ des € des s x1 and q x2 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 x € 1 þ k11 x_ 1 þ k12 x1 ¼ 0;

k11 ; k12 > 0

x € 2 þ k21 x_ 21 þ k22 x2 ¼ 0;

k21 ; k22 > 0

ð5:152Þ

By inserting (5.151) into (5.152) the desired accelerations can be selected in the form _ 1 þ k12 x1 Þ q€ des € eq x1 ¼ q x1 ðk11 x

ð5:153Þ

_ 2 þ k22 x2 Þ € eq q€ des x2 ¼ q x2 ðk21 x

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 q€ des € des must be m and q s derived. From (5.150) the transformation matrix "

q€ des x1 q€ des x2

#

" ¼

g1s

g1m

g2s

g2m

#"

q€ des s q€ des m

#

" ¼

Js;m x1;x2

q€ des s q€ des m

# ð5:154Þ

relates accelerations q€ des q des € des € des m ,€ s in the configuration space with accelerations q x1 , q x2 in the task space. Here Js;m stands for a Jacobian matrix relating the velocities in the task x1 ;x2 space x_ T ¼ ½x_ 1 ; x_ 2 and the velocities in the configuration space q_ T ¼ ½q_ s ; q_ m . In order to

228


determine q€ des € des

matrix Js;m m and q s from (5.154) x1 ;x2 must be nonsingular in the domain of change s;m of system variables. Assuming det Jx1 ;x2 6¼ 0, the inverse transformation can be written as

Js;m x1 ;x2

1

" g2m 1 ¼ g1s g2m g1m g2s g2s

det Js;m x1 ;x2 ¼ g1s g2m g1m g2s 6¼ 0

g1m g1s

# ð5:155Þ

The desired accelerations in the ‘m’ and ‘s’ systems enforcing the transients (5.153) can be expressed in the following form q€ des ¼ s q€ des m ¼

det

1 Js;m x1 ;x2 1

det Js;m x1 ;x2

h i g2m q€ des € des x1 g1m q x2 h i g2s q€ des € des x1 þ g1s q x2

ð5:156Þ

des The control the forces ondes ‘m’ and ‘s’ sides can be expressed as τm ¼ sat ^τdism þ anm q€ m and τs ¼ sat ^τdiss þ ans q€ s 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 Þ with f T ¼ ½ f1 f2 . Then a projection of the system motion in the manifold (5.149) can be written as s;m

x€

¼ Js;m € s;m þ J_ x1 ;x2 q_ s;m x1 ;x2 q

xT

¼ ½ x1

x2

qTs;m

¼ ½ qs

qm

ð5:157Þ

On the trajectories of compensated systems (5.113) the equations of motion have the form _ s;m _ s;m þ Js;m ps;m x€ ¼ Js;m € des x1 ;x2 q s;m þ Jx1 ;x2 q x1 ;x2 pTs;m ¼ ½ ps

pm

ð5:158Þ

The Jacobean matrix Js;m x1 ;x2 must be regular and if it is not constant it introduces an additional _ s;m specific to the system representation in the task space. By selecting a disturbance Js;m x1 ;x2 q s;m vector of the desired accelerations in the task space as q € des € des x1 ;x2 ¼ Jx1 ;x2 q s;m and a vector of the _ s;m q_ s;m Js;m ps;m the dynamics (5.158) reduce equivalent accelerations as q € eq ¼ J x1 ;x2 x1 ;x2 x1 ;x2 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 a whole new set of possibilities in motion control design by introducing

229


Figure 5.23

Control structure in functionally related systems

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 the motion 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

230


Figure 5.24 Synchronized motion of two plants. The diagrams show reference position qref , reference position qref c , 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, q€ des € des m and q s . The control parameters in both loops are k ¼ 150 and a ¼ 0:75

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 _ iref as inputs and disturbance ^τdis is estimated by a disturbance observer with q; ð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 q€ m ¼ q€ des m þ pm and q€ s ¼ q€ des € des € des s þ ps . Here q m and q s 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 ref define variables qc ¼ qm þ qs and qd ¼ qm qs . If qref and qref c ¼ 2q d ¼ 0 then enforcing ref ref qc ¼ qref c and qd ¼ qd gives the unique solution qm ¼ qs ¼ q . This indicates synchronization of the movement of the two plants formulated as a tracking problem with tracking errors

ref ec ¼ qc qref c ¼ qc 2q

ed ¼ qd qref d ¼ qd 0

ð5:159Þ

231


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 _ ref ; c ¼ 100; c1 ¼ 1 sc ¼ cqc þ c1 q_ c 2 cqref c þ c1 q c sd ¼ cqd þ c1 q_ d 0

ð5:160Þ

The derivatives of errors (5.160) on the trajectories of compensated plants, with neglected disturbance estimation error, can be written as ¼ q€ des s_ c ¼ cq_ c þ q€ c 2 cq_ ref € ref € eq c þq c c q c ; _ ref cq_ c € ref q€ eq c ¼ 2 cq c c þq s_ d ¼ cq_ d þ q€ d 0 ¼ q€ des € eq d q d ;

ð5:161Þ

_d q€ eq d ¼ 0 cq

Here q€ des € m þ q€ s and q€ des € m q€ s are control variables in the ec and ed control loops c ¼ q d ¼q respectively. The position tracking controllers for qc and qd can be designed as

eq q€ des ¼ sat q€^c þ q€ con c c

eq ¼ sat q€^d þ q€ con q€ des d d

ð5:162Þ

The acceleration enforcing finite-time convergence in trajectory tracking can be determined as q€ con ¼ kjsi j2a 1 signðsi Þ; i

1 < a < 1; 2

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 q€ c ¼ q€ m þ q€ s and q€ d ¼ q€ m q€ s and the dynamics of the compensated systems, it follows that q€ des m ¼

q€ des € des c þq d 2

¼ q€ des s

q€ des € des c q d 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 qref c , 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 q€ des m and

232


q€ des respectively. The control force has been limited by jτmax j ¼ 25 N.m. The synchros 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., and Ohnishi, K. (2005) Realization of “Law of action and reaction” by multilateral 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 developments in robotics, microparts handling, control theory and virtual reality systems. Its potential applications include network robotics, telesurgery, space and seabed telemanipulation, micronanoparts 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]:


234


Dynamic touch is the ability to sense certain physical properties of an object by moving 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 in which the slave device replicates operator motion at the remote site and the master 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 on master 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 on master 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 are many 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 be manipulated. 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 system with the operator force top and the interaction force with master 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

235

Bilateral Control Systems

as in (6.4). am ðqm Þ€q m þ bm ðqm ; q_ m Þ þ gm ðqm Þ ¼ tm tmext fh

ð6:1Þ

as ðqs Þ€q s þ bs ðqs ; q_ s Þ þ gs ðqs Þ ¼ ts tsext fs

ð6:2Þ

ah ðqh Þ€q h þ bh ðqh ; q_ h Þ þ gh ðqh Þ ¼ top fh Dh ðq_ m q_ h Þ þ Kh ðqm qh Þ ¼ fh Me ð€q s q€e Þ þ De ðq_ s q_ e Þ þ Ke ðqs qe Þ ¼ fs

ð6:3Þ ð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 systems with tm and ts as the control forces and with fh and 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 q_ e and acceleration € q e . Interaction force fs exists only if there is contact between slave device and environment. In our analysis we will assume fs 0. The properties of the objectenvironment in the contact point are defined by Me ; 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 q_ h and acceleration €q h then the force due to motion of the master 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 objectenvironment to be manipulated and the operator’s hand are in general modeled as full secondorder 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 system but 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 the magnitude 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 fh which 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.

236


Thus system has three active command inputs top ; tm ; ts , where top is treated as an input that cannot be changed by the control – thus it dictates motion on the master 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 the master device (robot) acts as an environment with 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 the master side and in that way causing motion within 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)

237


Figure 6.2 Structure and components of a bilateral control

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]

fh qs

¼

h11 h21

h12 h22

qm fs

q ¼H m fs

ð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

fh qs

¼

0 1 1 0

qm fs

ð6:6Þ

If conditions (6.6) are satisfied, the operator feels the environmental impedance. Indeed if fs ¼ Zs qs is inserted in (6.5) then the operator force becomes fh ¼ h11 qm Ze qs h12 h21 = ð1 h22 Ze Þ. Under conditions (6.6) the interaction force is obtained as fh ¼ Ze qm , thus the operator feels directly the environmental impedance. Ideal bilateral system conditions (6.6) or bilateral system operational requirements may be formulated as in (6.7). Control inputs ðtm ; ts Þ should be selected in such a way 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

238


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 ) is an 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 dis amn€q m ¼ amn€q des m pm Qdm ; tm dis asn€q s ¼ asn€q des s ps Qds ; ts

ð6:8Þ

The desired accelerations € q des q des m and € s 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 Þ€q m þ bm ðqm ; q_ m Þ þ gm ðqm Þ þ fh þ text tsdis ¼ ðas asn Þ€q s þ bs ðqs ; q_ s Þ þ gs ðqs Þ þ fs pm ðQdm ; tmdis Þ ¼ tmdis ^tmdis

ð6:9Þ

ps ðQds ; tsdis Þ ¼ tsdis ^tsdis

Disturbance observers on the master and slave side provide a reasonable accuracy of compensation of master 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.

239


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 €q des q des m and € s . 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 with Me ¼ 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 €e qb

¼ €q m €q s ¼ €q des q des m € s

pm ps amn asn

¼€ q des qb pqb

q€ des q des €q des qb ¼ € s ; m pm ps pqb ¼ amn asn

ð6:10Þ

The position tracking control input€q des qb 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 q€ des pqb KD e_ qb KP eqb qb ¼ ^ the dynamics in the position tracking loop will be forced to satisfy

pqb pqb ffi 0 €e qb þ KD e_ qb þ KP eqb ¼ ^

ð6:11Þ

ð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 þ e_ qb with dynamics s_ qb ¼ c_eqb þ€e qb ¼ € q des q eq qb € qb

ð6:13Þ

Here € q des q des q eq q eq qb ¼ € m € s stands for the desired acceleration and € qb ¼ pqb c_eqb stands for the equivalent 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 e_ Fb ¼ f_ h þ f_ s ¼ Dh q€ des q eq Fb € Fb q€ des q des q des Fb ¼ € m þ€ s

1 _ _ h Þ Dh 1 f_ s þ pFb €q eq ¼ q € þ€ q h s Kh Dh ðq m q Fb pm ps pFb ¼ þ amn asn

ð6:14Þ

240


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

Here Dh stands for the nominal damping coefficient of the operator and is assumed known. The € q eq q des Fb stands for the Fb stands for the equivalent acceleration in the force control loop and € desired acceleration input in force control loop. The error dynamics is described by two firstorder 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 eq

q€ des q^ qb kqb sqb ; qb ¼ €

kqb > 0

eq

q€ des q^Fb kFb Dh 1 eFb ; kFb > 0 Fb ¼ €

ð6:15Þ

eq eq Here € q^qb and € q^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

2a 1 €q des q^ eq sign sqb ; qb ¼ € qb kqb sqb eq

q^Fb kFb jeFb j2a 1 signðeFb Þ; q€ des Fb ¼ €

kqb > 0 1 < a < 1; kFb > 0 2

ð6:16Þ

Equivalent accelerations in both loops can be estimated by observers. From measurement of the desired acceleration and the generalized error ð€q des qb ; sqb Þ the equivalent acceleration in the position tracking loop can be estimated by

z_ q ¼ g €q des qb þ gsqb zq eq q€^qb ¼ zq gsqb ; g > 0

ð6:17Þ

The same structure of the observer can be estimation in applied for equivalent acceleration q eq the force control loop. From measurement €q des Fb ; eFb and dynamics (6.14) the€ Fb observer can be written in the following form 1 z_ F ¼ g €q des Fb þ gDh eFb zF eq q€^Fb ¼ zF gDh 1 eFb ; g > 0

ð6:18Þ

241


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)

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 eq

q eq s_ qb þ kqb sqb ¼ €q^ qb € qb ffi 0 eq

e_ Fb þ kFb eFb ¼ €q^Fb € q eq Fb ffi 0

ð6:19Þ

Dynamics in both loops are described by first-order transients. From (6.19) and sqb ¼ ceqb þ e_ qb the dynamics of the position tracking error can be determined as eq €e qb þ kqb þ c e_ qb þ kqb ceqb ¼ € q^qb € q eq qb ffi 0

ð6:20Þ

The equilibrium solution is qm ¼ qs – thus the position of the master and slave devices is the same. The force balance established by the force tracking loop enforces the following dynamics Dh€q m þ ðKh þ kFb Dh Þq_ m þ kFb Kh qm þ De€ q s þ ðKe þ kFb De Þq_ s þ kFb Ke qs ¼ Dh€q h þ h ðKh þ kFb Dh Þq_ h þ kFb Kh qh þ De€ q e þ ðKe þ kFb De Þq_ e þ kFb Ke qe

ð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

242


F¼

Ke Kh ðqh qe Þ Kh þ Ke

ð6:22Þ

For interaction force (6.22) steady-state solution for the master and slave side positions qm ¼ qs is qm ¼ qs ¼

Kh Ke qh þ qe Kh þ Ke Kh þ Ke

ð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 Ke Kh 1 : 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 the master 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 € q des q des q des q des q des q des Fb ¼ € m þ€ s and € qb ¼ € m € s the desired accelerations 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

243

Bilateral Control Systems 1 des €q Fb þ€ q des qb 2

1 des €q Fb € q des €q des m ¼ qb 2 q€ des m ¼

ð6:24Þ

The corresponding control forces realizing the desired accelerations can be expressed as tm ¼ sat ^tdism þ anm€ q des and ts ¼ sat ^tdiss þ ans€q des for the master and slave devices, m s 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 ¼ e_ qb þ 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 Þ€q m þ bm ðqm ; q_ m Þ þ gm ðqm Þ þ tmext ¼ tm fh

ð6:25Þ

tm ¼ am€q des m ¼ KTm im as ðqs Þ€q s þ bs ðqs ; q_ s Þ þ gs ðqs Þ þ tsext ¼ ts fs

ð6:26Þ

ts ¼ as€q des s ¼ KTs is aop ðqh Þ€q h þ bop ðqh ; q_ h Þ þ gop ðqh Þ ¼ top fh top ¼ aop€q des op ¼ KTop iop

ð6:27Þ

Mh ð€q m €q h Þ þ Dh ðq_ m q_ h Þ þ Kh ðqm qh Þ ¼ fh Me ð€q s €q e Þ þ De ðq_ s q_ e Þ þ 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

as ¼ 0:2½1 þ 0:5 cos ð12tÞ

kg

bm ðqm ; q_ m Þ ¼ 16qm 8q_ m

N

bs ðqs ; q_ s Þ ¼ 5qs 2q_ s

N

gm ðqm Þ ¼ 0

N

gs ðqs Þ ¼ 0

N

tmext ¼ 0

N

tsext ¼ 0

N

KTm ¼ 0:95½1 þ 0:2 sin ð21tÞ

N=A

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 Mh ¼ 0 .

.

Dh ¼ 5 Kh ¼ 150 000 Me ¼ 0 for environment De ¼ 5 Ke ¼ 225 000 for operator

kg kg=s kg=s2 kg kg=s kg=s2

244


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 vqe t 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 qref ð t Þ ¼ q þ q sin vqh t m, h0 h1 h where parameters qh0 ; qh1 ; vqh are selected in each experiment. The operator motion is the result of the compliant control

h

i _ ref _ h þ kh ch qref _ h þ lh fh _ ref €q des q ref h ¼€ h þ ch q h q h qh þ q h q top ¼ ah€q des h

ð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 as eqb ¼ qm qs eFb ¼ fh þ fs

ð6:31Þ

The generalized position tracking error s is selected as sqb ¼ ceqb þ e_ qb ;

c ¼ 100

ð6:32Þ

The position tracking controller is designed as in (6.15) eq

q€ des q^qb þ€q con qb ¼ € qb

ð6:33Þ

eq Here € q des q^qb stands for qb stands for the desired acceleration in position tracking, € equivalent acceleration estimated by observer (6.17) with observer filter gain g ¼ 1200 and q con € qb ¼ kqb s qb ; kqb ¼ 100 is the convergence acceleration. The desired acceleration in the force control loop €q des Fb is selected as eq

q€ des q^Fb þ€q con Fb ¼ € Fb q€ con Fb ¼ 100eFb

ð6:34Þ

eq Equivalent acceleration € q^Fb is estimated by a observer (6.18) with observer filter gain g ¼ 1200.

245


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:0045 m; 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 q€ des m ¼

q€ des q des Fb þ€ qb 2

q€ des s ¼

q€ des q des Fb € qb 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 qref h , the position of the environment qe , position of master device qm , position of slave device qs and the desired accelerations in the position tracking € q des qb . The right column shows the difference between the positions of operator and master device eh ¼ qh qm , the interaction forces of 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€q des Fb . 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

246


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 and all 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 both motion 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 qref h , the position of the environment qe , position of master device qm , position of slave device qs and the desired accelerations in the position tracking € q des qb . The right column shows the difference between positions of the operator and the master device eh ¼ qh qm , the interaction forces of 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€q des Fb . 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

247


As shown in (6.22) and (6.23) the steady-state operation satisfies transparency conditions (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

fh qs

0 1 ¼ 1 0

qm p þ m fs 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 ¼ e_ qb þ ceqb ¼ 0. After the position converges, the transient (6.21) governs the changes in the position of the master and slave devices with the transient as expressed by 0

1 K þ K Kh þ Ke h e ðDh þ De Þ€q m þ @ þ kFb Aq_ m þ kFb qm Dh þ De Dh þ De Dh€q h þ ðKh þ kFb Dh Þq_ h þ kFb Kh qh De€ q e þ ðKe þ kFb De Þq_ e þ kFb Ke qe ¼ þ Dh þ De 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 € q des q des such that the motion of system (6.3), (6.4) m and € s and (6.8) is stable in the manifold (6.38).

248


The derivative of the position generalized error sqb ¼ ceqb þ e_ qb on the trajectories of system (6.8) can be expressed as s_ qb ¼ €e qb þ c_eqb ¼ €q des q eq qb € qb q€ des q des q des qb ¼ € m a€ s q€ qb _ Þq_ s þ caq _ s cq_ m þ pqb eq ¼ ðca þ 2a pm aps pqb ¼ amn asn

ð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 _ s ¼ Dh q€ des €q eq e_ Fb ¼ f_ h þ bf_ s þ bf Fb Fb q des q des €q des Fb ¼ € m þ b€ s

_ þ pFb q€ eq q s þ€ q h Þ Dh 1 Kh ðq_ m q_ h Þ þ bDe ð€q s € q e Þ þ bKe ðq_ s q_ e Þ þ bf Fb ¼ ðb€ pm bps pqb ¼ þ amn asn

ð6:40Þ

€ des q q eq Fb stands for the desired acceleration, and € Fb stands for the equivalent acceleration in the force tracking loop. The dynamics of the control errors (6.39) and (6.40) are expressed in the same way 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 finitetime convergence will be enforced if control is selected as in (6.16). Observers (6.17) and (6.18) can be directly applied for the estimation of € q eq q eq qb and € Fb , respectively. Consequently the transients of the transformed system may be expressed as in (6.19). The desired acceleration inputs for master € q des q des can be m and slave manipulators € s determined as

1 q des a€q des Fb þ b€ qb aþb

1 q des €q des € €q des s ¼ qb a þ b Fb €q des m ¼

ð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 þ e_ qb the dynamics of the position tracking error can be determined as eq €e qb þ kqb þ c e_ qb þ kqb ceqb ¼ € q eq q^qb 0 qb €

ð6:42Þ

249


Figure 6.8 Structure of a bilateral system with scaling in position and force loops

The equilibrium solution is qm ¼ aqs . The steady-state solution for a scaled bilateral system gives 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¼

bKe Kh ðqh aqe Þ aKh þ bKe

ð6:43Þ

For force (6.43) steady-state solution for the master and slave side positions qm ¼ aqs qs ¼

Kh bKe qh þ qe aKh þ bKe aKh þ bKe

qm ¼

aKh abKe qh þ qe aKh þ bKe aKh þ bKe

ð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. We will illustrate bilateral control with position tracking and the force tracking errors as eqb ¼ qm aqs eFb ¼ 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

250


q€ des m ¼

1 des a€q Fb þ b€ q des qb aþb

ð6:46Þ

1 des €q Fb € q des qb aþb

ð6:47Þ

q€ des s ¼

Figure 6.9 shows the transients in the bilateral control system with 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 are a ¼ 0:5; b ¼ 1:5 while all other conditions are the same. Both scaled position tracking and force tracking are achieved.

Figure 6.9 Transients in a bilateral control system with force and position scaling a ¼ 1:5; b ¼ 1:5 and operator compliance coefficient lh ¼ 1:85. The left column shows reference position qref h , 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 €q des qb . 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 €q des Fb

251


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 qref h , 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 €q des qb . 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 €q des Fb

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 ¼ €e qb ¼ € q qm € q qs

ð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 € q fma ¼ fh =anh and € q fsa ¼ fs =ans . Here the indexes ‘ma’ and ‘sa’ are

252


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 fs þ ¼ €q fma þ€ q fsa anh ans

ð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 fh ans ¼ fs anh , 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 q€ qqb ¼ eqqb ¼ €q qm €q qs q€ fFb ¼ efFb ¼ €q fma þ€ q fsa

ð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 n q Sqb q qqb ¼ €q qm €q qs ¼ 0 € ¼ qm ; qs : eqb ¼ €

o and q€ fFb ¼ € q fma þ€ q fsa ¼ 0

ð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 € q qqb ¼ € q des q des m € s ¼ 0 is enforced the equilibrium q des des des solution does not enforce q_ qb ¼ q_ m q_ s ¼ 0 and qqqb ¼ qdes m qs ¼ 0, thus the equilibrium 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 q_ m ð0Þ ¼ q_ s ð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 €q fma and €q fsa 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 position will be enforced if the initial conditions on the master and slave devices are the same. In design we will assume that the acceleration loop on the master and slave side are compensating all disturbances with negligible error, thus ensuring perfect acceleration tracking qm ¼ € € q des q s ¼ €q des m and € s . In the design we will assume two possibilities. In one position, the

253


velocity and forces will be assumed as measured 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 we would 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 s_ qb þ kqb sqb ¼ 0 for generalized error eq q^qb kqb sqb . From sqb ¼ ceqb þ e_ qb and the desired acceleration being expressed as €q des qb ¼ € s_ qb ¼ € q qb € q eq q eq q qb s_ qb . In systems qb the equivalent acceleration can be expressed as € qb ¼ € des with acceleration controller the relationship € q qb ¼ €q qb þ pqb Qqb ; tdis holds and we can eq

des

des

q^qb s_ qb, where €q^qb can be calculated simply as approximate equivalent acceleration by € q^qb ¼ € des q des q^qb s_ qb þ kqb sqb : a filtered € q des qb . Then the desired acceleration can be expressed as € qb ¼ € The desired acceleration, with the assumption that control error, position and velocity are measured, can be expressed as h i des q€ des q^qb eqqb þ KD ðq_ m q_ s Þ þ KP ðqm qs Þ qb ¼ € eqqb ¼ €q qm €q qs

ð6:52Þ

des

d €q^qb ^des ¼ g €q des q € qb qb dt

The transient in motion tracking is then governed by eqqb þ KD ðq_ m q_ s Þ þ KP ðqm qs Þ ¼ pqb

ð6:53Þ

The solution based only on the acceleration measurement leads to the desired acceleration being expressed as [7] €q des qb

des ¼ €q^qb

8 2 3 9 ð ðt ðt t0 ¼ 0 with initial conditions ez ðt0 Þ ¼ 0, can be determined as des

€ ðtÞ ueq e_ z ðtÞ ¼ v_ mes ðtÞ z_ ðtÞ ¼ v_ mes ðtÞ q z ðez Þ ¼ 0 ueq q des ðtÞ v_ mes ðtÞ z ðez Þ ¼ €

ð6:60Þ

Equivalent control ueq z ð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€q des ðtÞ. Inserting tðtÞ ¼ an€ q des ðtÞ into (6.57) results in an€q des ðtÞ ¼ an v_ ðtÞ þ tdis ðtÞ

ð6:61Þ

Plugging (6.61) into the expression for equivalent control in (6.60) yields ueq z ½ez ðtÞ ¼

tdis ½v_ mes ðtÞ v_ ðtÞ an

ð6:62Þ

The equivalent control ueq z ½ez ðtÞ is a function of the generalized disturbance induced acceleration and the difference between the measured v_ mes ðtÞ and the plant acceleration v_ ðtÞ. From (6.62), if tracking in (6.59) is enforced, the average control is equal to ueq z ½ez ðtÞ and an1 tdis ½v_ mes ð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 in measurement. From (6.62) an observer-predictor that estimates the plant output at time ðtÞ can be constructed as an z_ ðtÞ ¼ an€q des ðtÞ an uz ½ez ðtÞ tdis ^v_ ðtÞ ¼ v_ mes ðtÞ þ ueq z ðez Þ an _^qðtÞ ¼ ^vðtÞ ez ðtÞ ¼ vmes ðtÞ zðtÞ

ð6:63Þ

259


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 the measured inputs €q des ðtÞ; vmes ðtÞ and the plant nominal inertia an the plant generalized disturbance tdis is required. That prevents the direct application 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€q des ðtÞ an uz ½ez ðtÞ; ez ðtÞ ¼ vmes ðtÞ zðtÞ pðQ; tdis Þ ^v_ ðtÞ ¼ v_ mes ðtÞ þ ueq z ðe z Þ an ^q_ ðtÞ ¼ ^vðtÞ

ð6:64Þ

Integration of ^v_ ðtÞ over time interval [0,t] yields ^vðtÞ ¼ vmes ðtÞ þ

ðt uzeq ½ez ðjÞd j 0

ðt 1 pðQ; tdis Þd j þ ^vð0Þ vmes ð0Þ an

ð6:65Þ

0

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 Þ ¼ KD vðtÞ þ KP qðtÞ. Here KD ; 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Þ KD vðtÞ KP qðtÞ

ð6:66Þ

In order to reflect the structure of the nominal plant observer (6.59) needs to be modified. The modification yields qðtÞ an uz ½ez ðtÞ an z_ ðtÞ ¼ an€q des ðtÞ KD^vðtÞ KP ^ ez ðtÞ ¼ vmes ðtÞ zðtÞ

ð6:67Þ

The equivalent control can be expressed from the tracking conditions in the observer (6.67) an ueq q des ðtÞ an v_ mes ðtÞ KD^vðtÞ KP ^ qðtÞ z ðez Þ ¼ an€

ð6:68Þ

Calculation of an€q des ðtÞ from the second equation in (6.66) and inserting it into (6.68) yields _ mes ðtÞ ¼ an v_ ðtÞ þ KD ½vðtÞ ^vðtÞ þ KP ½qðtÞ ^ an ueq qðtÞ z ðez Þ þ an v

ð6:69Þ

260


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 qðtÞ an uz ½ez ðtÞ an z_ ðtÞ ¼ an€q des ðtÞ KD^vðtÞ KP ^ ez ðtÞ ¼ vmes ðtÞ zðtÞ an^v_ ðtÞ ¼ an v_ mes ðtÞ þ an ueq z ð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Þ an D€q ðtÞ þ KD Dq_ ðtÞ þ KP DqðtÞ ¼ 0

ð6:71Þ

DqðtÞ ¼ qðtÞ ^qðtÞ

The convergence of error between the exact and estimated plant position depends on an uncompensated disturbance in plant pn ðQ; tdis Þ ¼ KD vðtÞ þ KP qð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 Þ ¼ KD vðtÞ þ KP qðtÞ to the plant input as shown in Figure 6.14, the compensated plant dynamics can be adjusted to (6.66). Insertion of KP qð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 delayed measurement 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

261


convergence depend on the selection of parameters KD ; 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 system motion (by applying operator force on the master 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~uz ¼ 400ðuz ~ uz Þ dt

ð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 qmes and the delay in measurement channel Tm . The right column shows the actual slave s position qs , the estimated slave position ^qs , the master position qm and the force due to the convergence term tcon ¼ KD vðtÞ þ KP qðtÞ (the spikes on this force are caused by the way delay is implemented in simulation)

262


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 þ e_ qm ; c ¼ 100 and sqs ¼ ceqs þ e_ qs ; c ¼ 100 with eqm ¼ qh qm and eqs ¼ qm ^ qs . The position tracking controller is designed as in (6.15) eq êq þ€ q des € ¼ q € q con q des stands for the desired acceleration, €q^qi the qi qi qi ; i ¼ m; s. Here € qi equivalent acceleration estimated by observer (6.17) with filter gain g ¼ 1200 and q con € qi ¼ kqi s qi ; kqi ¼ 100; i ¼ m; s as the convergence acceleration. The results with a constant delay in the measurement channel are shown in Figure 6.15 and for a variable delay in the measurement 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 the measurement channel. The left column shows transients in the master position qm , the slave position qs , the measured slave position qmes and the delay in the measurement channel Tm . The right column shows s the actual slave position qs , the estimated slave position ^qs , the master position qm and the force due to the convergence term tcon ¼ KD vðtÞ þ KP qðtÞ (the spikes on this force are caused by the way delay is implemented in simulation)

263


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Þ ¼ Kn iðtÞ ¼ an€q

ð6:73Þ des

ðt Tc Þ þ ^tdis ðtÞ

As a reference we will take time ‘t’ at which control signal €q des ðtÞ is generated and entered into 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Þ, the control input € q des ð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 v_ mes ðtÞ ¼ v_ ðt Tc Tm Þ. Thus it can be rewritten as ueq z ½ez ðtÞ ¼

tdis ½v_ ðt Tc Tm Þ v_ ðtÞ an

ð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 an z_ ðtÞ ¼ an€q des ðtÞ an uz ½ez ðtÞ tdis ^v_ ðtÞ ¼ v_ mes ðtÞ þ ueq z ðez Þ an _^qðtÞ ¼ ^vðtÞ

ð6:76Þ

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 and measurement channels. This expression has the same form as (6.63) and all remarks

264


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 convergence we may take the same approach as for an observer with only delay in the measurement channel. The input to the plant is composed as tðtÞ ¼ an€q des ðt; Tc Þ ½KD vðt; Tc Þ þ KP qð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 Þ KD vðt; Tc Þ KP qðt; Tc Þ

ð6:78Þ

The observer (6.76) can be then modified as follows an z_ ðtÞ ¼ an€q des ðtÞ KD^vðtÞ KP ^ qðtÞ an uz ½ez ðtÞ ez ðtÞ ¼ vmes ðtÞ zðtÞ

ð6:79Þ

In (6.79) the desired acceleration € q des ðtÞ is used as the observer input. From tracking conditions in observer (6.79) equivalent control ueq z ½ez ðtÞ can be expressed as an ueq q des ðtÞ KD^vðtÞ KP ^ qðtÞ an v_ mes ðtÞ z ½ez ðtÞ ¼ an€

ð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€q des ðtÞ ¼ an v_ ðtÞ þ KD vðtÞ þ KP qðtÞ

ð6:81Þ

Insertion of an€ q des ðtÞ from (6.81) into (6.80) yields an v_ ðtÞ þ KD ½vðtÞ ^vðtÞ þ KP ½qðtÞ ^qðtÞ ¼ an v_ mes ðtÞ þ an ueq z ½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€q des ðtÞ KD^vðtÞ KP ^ qðtÞ uz ½ez ðtÞ ez ðtÞ ¼ vmes ðtÞ zðtÞ an^v_ ðtÞ ¼ an v_ mes ðtÞ þ ueq ðez Þ q^_ ðtÞ ¼ ^vðtÞ

ð6:83Þ

z

From (6.82) and (6.83) the estimation error may be expressed in the following form an D€q ðtÞ þ KD Dq_ ðtÞ þ KP DqðtÞ ¼ 0 DqðtÞ ¼ qðtÞ ^qðtÞ

ð6:84Þ


265

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 KD vðtÞ þ KP qð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 disturbance may 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 on master 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 the measurement and the control channels and the disturbance observer as the estimator of the equivalent control

266


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 example 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 the delay 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 system with a constant delay in the control and the measurement channels Tc ¼ 0:2 s, Tm ¼ 0:3 s. The left column shows the master position qm , the slave position qs , the actual qs , the measured qmes and the delay in the measurement channel. The right column shows the actual s position qs , the estimated position ^qs , the estimated slave position ^ qs , the master position qm and the force due to the convergence term tcon ¼ KD vðtÞ þ KP qðtÞ (the spikes on this force are caused by the way delay is implemented in simulation)


267

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 between maximum 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 qmes and the delay in the measurement channel. The right s 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 ¼ KD vðtÞ þ KP qðtÞ (the spikes on this force are caused by the way delay is 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 steadystate 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 2 N.

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€q des ð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€q des ðtÞ ¼ €q ref ðtÞ þ KDC vref ðtÞ ^vðtÞ þ KPC qref ðtÞ ^ qðtÞ

ð6:85Þ

268


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 shows master position qm , slave position qs , the actual qs , the measured qmes and the delay s qs , the in the measurement channel. The right column shows the actual position qs , the estimated position ^ estimated slave position ^qs , the master position qm and the force due to the convergence term tcon ¼ KD vðtÞ þ KP qðtÞ (the spikes on this force are caused 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; tc Þ €q ðtÞ þ KD q_ ðtÞ þ Kp qðtÞ þ KDC ^vðt; tc Þ þ KPC ^ ¼ €q ref ðt; tc Þ þ KDC q_ ref ðt; tc Þ þ KPC qref ðt; tc Þ

ð6:86Þ

Having convergence of the estimated values defined by (6.84) we can write ^qðtÞ ¼ qðtÞ þ j and ^vðtÞ ¼ vðtÞ þ z with j; z t ! 0. Then (6.86) can be rearranged into !¥ €q ðtÞ þ KD q_ ðtÞ þ KP qðtÞ þ KDC q_ ðt; tc Þ þ KPC qðt; tc Þ ¼ €q ref ðt; tc Þ þ KDC q_ ref ðt; tc Þ þ KPC qref ðt; tc Þ þ eðj; zÞ

ð6:87Þ

269


After the convergence error diminishes eðj; zÞ ¼ 0 the closed loop behavior is governed by €q ðtÞ þ KD q_ ðtÞ þ KP qðtÞ þ KDC q_ ðt tc Þ þ KPC qðt tc Þ

ð6:88Þ

¼ €q ref ðt tc Þ þ KDC q_ ref ðt tc Þ þ KPC qref ðt tc Þ

The closed loop system (6.88) may be written in the form of a LTI system " " xðtÞ ¼ " A1 ¼

qðtÞ vðtÞ

# ;

0 KPC

0 0

#

x_ ref ðt tc Þ þ Bxref ðt tc Þ 0 1 " # " # 0 1 qref ðtÞ ref ; A0 ¼ x ðtÞ ¼ ref ; KP KD v ðtÞ # " # 0 1 0 ; B¼ KDC KPC KDC

x_ ðtÞ ¼ A0 xðtÞ þ A1 xðt tc Þ þ

ð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Þ ¼ A1 xðtÞ þ A2 xðt tÞ, stability requires . .

A1 þ A2 to be Hurwitz, that there exist positive definite AT1 P þ PA1 þ PA2 S 1 AT2 P þ S þ R ¼ 0.

symmetric

matrices

P; S; R

such

that

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

270


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 systems with a communication delay (constant or variable) are shown in previous sections. Here we would like to discuss merging 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 motion we 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 parameters Me ; 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

8 9 < qm ; qs : eqbT ðqm ; qs Þ ¼ qm ðt Tc Þ aqs ðtÞ ¼ 0 = ¼ and : ; eFbT ðfh ; fs Þ ¼ fh ðtÞ þ bfs ðt Tm Þ ¼ 0

ð6:90Þ

Such a formulation allows operations in which a motion 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 in motion tracking as a function of the estimated motion at the slave side and the motion on the master side eqbT ðqm ; qs Þ ¼ qm ðtÞ a^ qs ðtÞ ¼ 0

ð6:91Þ

Thus the motion bilateral relationship is formulated in the same way as in Equation (6.7) if the master motion and the slave estimated motion are taken as functionally related coordinates.


271

eq This allows a direct application of the motion controller as in (6.15) €q des q^qbT kqbT sqbT qbT ¼ € 2a 1 eq or (6.16) € q des ¼ € q^ kqbT sqbT sign sqbT , kqbT > 0 depending on the selected conqbT

qbT

vergence with generalized error sqbT ¼ ceqbT þ e_ qbT . The force control error eFbT ðfh ; fs Þ ¼ fh ðtÞ þ bfs ðt Tm Þ formally can be written as eFbT ðfh ; fs Þ ¼ fh ðtÞ þ bfsref ðtÞ with fsref ðtÞ ¼ fs ðt Tm Þ and consequently the force tracking eq q^FbT kFbT Dh 1 eFbT for exponential convercontroller can be selected as in (6.15) € q des FbT ¼ € eq q^FbT kFbT jeFbT j2a 1 signðeFbT Þ; 12 < a < 1, kkFbT > 0 for gence or as in (6.16) € q des FbT ¼ € finite-time convergence. eq eq The equivalent accelerations € q^qbT and €q^FbT can be estimated by simple disturbance observer-like structures (6.17) and (6.18). Thus, the structure of controller did not change as compared with systems without 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 systems without scaling), or (6.41) for systems with 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 system with a constant delay Tm ¼ 0:3 s in the measurement channel. The left column shows the master position qm , the slave position qs , the actual qs , the measured qmes and s 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 ¼ KD vðtÞ þ KP qðtÞ. The scaling coefficients are a ¼ 0:5; b ¼ 1:5

272


control loop can be closed locally on the master side and then the master and slave desired accelerations can be expressed as 1 des a€q FbT þ b€ q des qbT aþb 1

0 €q des €q des qbT s ¼ aþb

q€ des m ¼

ð6:92Þ

Example 6.6 Bilateral Control with Delay in Control and Measurement 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 systems with a delay in the control and measurement 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) with KD ¼ 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 qmes and the delay in the measurement channel. The right column shows the actual position qs , s 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 ¼ KD vðtÞ þ KP qð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)


273

The bilateral control errors are defined as in (6.90) and (6.91) eqbT ¼ qm ðtÞ a^qs ðtÞ and eFbT ¼ fh ðtÞ þ bfs ðt Tm Þ. The generalized position tracking error is selected as sqbT ¼ ceqbT þ e_ qbT ; c ¼ 100. The position tracking controller is designed as in (6.15) eq eq q^qbT þ€ q con q des q^qbT is the equivalent q des € qbT ¼ € qbT . Here € qbT stands for the desired acceleration, € acceleration estimated by observer (6.17) with observer filter gain g ¼ 1200 and q con € qbT ¼ kqbT s qbT ; kqbT ¼ 100 is the convergence acceleration. The desired acceleration eq q des q^FbT þ€q con q con in the force control loop € q des FbT is selected as € FbT ¼ € FbT ;€ FbT ¼ 100eFbT . The master and slave desired accelerations are determined as in (6.92). In experiments the interaction force with the environment is limited to fsmax ¼ 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 the delay 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 between maximum 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 qmes and the delay in the measurement channel. The right s 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 ¼ KD vðtÞ þ KP qð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)

274


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 steadystate 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 the measured 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 2 N.

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 nominal mass design in disturbance observer. Industrial Electronics Society 32nd Annual Conference 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: and overview of some recent advances and open problems. 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) Sliding Mode Control 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.


275

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 systems with highly nonlinear dynamics. In general the number of actuators may 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 Rn1 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 here we 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 related motion. 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 system we will 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 obtained within an acceleration control framework. Thus, we will not touch many important issues that can be found in a wide literature on robotics and the dynamics and control of multibody systems. Rather, we will 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




278


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. Further we will discuss the dynamics and control of multibody 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 a multibody system with 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 Rn1 denotes the configuration vector, AðqÞ 2 Rnn stands for the positive definite kinetic energy matrix (sometimes termed inertia matrix) with bounded strictly positive elements 0 < aij aij ðqÞ aijþ [hence A k AðqÞk A þ , where A ; A þ are two known scalars with bounds 0 < A A þ ] bðq; q_ Þ 2 Rn1 stands for the vector Coriolis forces, viscous friction and centripetal forces and is bounded by kbðq; q_ Þk b þ gðqÞ 2 Rn1 stands for the vector of gravity terms bounded by kgðqÞk g þ ,s 2 Rnx1 stands for the vector of generalized joint forces bounded by ksk t þ (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 energy matrix and the forces on the system coordinates will not be denoted in some expressions. Matrix A 1 ðqÞ 2 Rnn 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 distribution 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.


280


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) 3

2

n 7 6X aii ðqÞ€q i þ 6 aij€q j þ bi ðq; q_ Þ þ gi ðqÞ7 5 ¼ ti ; 4

i ¼ 1; . . . ; n

ð7:2Þ

j¼1 j 6¼ i

q_ i ¼ vi

2 6 aii ðqÞ_vi þ 6 4

n X j¼1 j 6¼ i

3

7 aij v_ j þ bi ðq; vÞ þ gi ðqÞ7 5 ¼ 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 €q i þ Daii ðqÞ€q i þ

n X j¼1

aij€q j þ bi ðq; q_ Þ þ gi ðqÞ ¼ ti ; |fflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflffl}

i ¼ 1; 2; . . . ; n

disturbance¼tdi

j 6¼ i

|fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} generalized disturbance¼tdisi

anii

_ q Þ ¼ ti €q i þ tdisi ðq; q;€

ð7:4Þ

tdi ðq; q_ Þ ¼ bi ðq; q_ Þ þ gi ðqÞ _ q Þ ¼ Daii ðqÞ€q i þ tdisi ðq; q;€

n X

aij€q j

j¼1 j 6¼ i

þ bi ðq; q_ Þ þ gi ðqÞ |fflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflffl} tdi

|fflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} acceleration induced disturbance

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 _ q Þ; i ¼ 1; 2; . . . ; n. tdi ðq; q_ Þ; i ¼ 1; 2; . . . ; n form a generalized disturbance tdisi ðq; q;€ 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€ q des tdisi , where € q des stands for the i-th joint desired acceleration and ^tdisi stands i þ^ i for the estimated i-th joint disturbance, system (7.4) can be reduced to n decoupled secondorder systems of the form €q i ¼ €q des i

tdisi ^tdisi pi ðQd ; tdisi Þ ¼ €q des ; i anii anii

i ¼ 1; 2; . . . ; n

ð7:5Þ

pi ðQd ; tdisi Þ ¼ tdisi ^tdisi

The disturbance estimation error pi ðQd ; tdisi Þ ¼ tdisi ^tdisi is discussed in detail in Chapter 4. Structure (7.5) is identical with compensated 1-dof systems discussed in detail in Chapters 3

281

Configuration Space Control

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 the variation of the kinetic energy matrix on the compensated system dynamics. By selecting desired acceleration € q des as a function of the error in position and velocity one can i stabilize the motion 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) we would like to apply the same ideas discussed in Chapter 3. Thus for system (7.1) the desired acceleration €q des 2 Rn1 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 ^ ðqÞ€q des þ b ^ ðqÞ€ ^ðq; q_ Þ þ ^gðqÞ ¼ A s¼A q des þ ^sd

ð7:6Þ

^ ðqÞ stands for the estimation of inertia matrix and ^sd ¼ b ^ðq; q_ Þ þ ^gðqÞ stands for the Here A estimation of disturbance vector sd ¼ bðq; q_ Þ þ gðqÞ. Inserting control (7.6) into plant dynamics (7.1) yields ^q des A 1 ðqÞpðQd ; sd Þ €q ¼ A 1 A€ 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 ^ For A 1 A ^ ¼ E being a unity matrix, a full decoupling is obtained. distribution matrix A 1 A. 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 inertia matrix as AðqÞ ¼ An þ DAðqÞ where An > 0; 8q stands for the nominal inertia matrix selected as a positive definite nonzero matrix and DAðqÞ stands for 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 in (7.6), the dynamics (7.7) can An€ q þ DAðqÞ€ q þ bðq; q_ Þ þ gðqÞ ¼ s and replacing A be expressed as €q ¼ €q des An 1 pðQd ; sdis Þ pðQd ; sdis Þ ¼ sdis ^sdis

ð7:8Þ

282


Figure 7.1

Structure of control system in configuration space

Here sdis ¼ DAðqÞ€ q þ bðq; q_ Þ þ gðqÞ stands for the generalized disturbance vector and ^sdis stands 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 a more formal approach based on Lyapunov stability methods. Let us first look at a configuration tracking problem in which configuration q 2 Rn1 is required to track a sufficiently smooth reference qref 2 Rn1 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 system motion to stay in the manifold S ¼ q; q_ : s q; qref ¼ CDq þ Dq_ ¼ 0;

C 2 Rnn ;

C>0

ð7:9Þ

283


Here s q; qref 2 Rn1 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 beselected 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 ref q € q CDq_ s_ q; qref ¼ CDq_ þ D€q ¼ € |fflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflffl} €q

eq

¼ €q ref CDq_

equivalent acceleration

ð7:10Þ

Here €q eq 2 Rn1 stands for the equivalent acceleration, thus enforcing a zero rate of change of the generalized control error s_ q; qref jq¼€ € q eq ¼ 0. Inserting (7.1) into (7.10) yields s_ q; qref ¼ A 1 s b þ g þ A €q ref CDq_ ¼ A 1 ðs seq Þ q eq seq ¼ b þ g þ A €q ref CDq_ ¼ sd þ A€

ð7:11Þ

The equivalent force seq 2 Rn1 , which enforces equivalent acceleration in system (7.1) consists of the disturbance force sd and the equivalent acceleration induced force A€q eq 2 Rn1 . 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¼

sT s > 0; 2

V ð0Þ ¼ 0;

ð7:12Þ

_ Let us select a The time derivative of the Lyapunov function V can be expressed as V_ ¼ sT s. control input which will, for a given linear or nonlinear function YðsÞ 2 Rn1 , enforce a derivative of the Lyapunov function in the form V_ ¼ sT s_ ¼ sT YðsÞ < 0

ð7:13Þ

Here YT ðsÞ ¼ ½ Y1 . . . Yn 2 Rn1 stands for a vector function satisfying condition sign½YðsÞ ¼ signðsÞ where signðxÞ stands for a componentwise function with element signðxi Þ being þ 1 for xi > 0 and 1P for xi < 0, i ¼ 1; 2; . . . ; n. Thus the Lyapunov function derivative can be expressed as V_ ¼ n1 jYi jsi sign ðsi Þ. This leads for a wide range of possibilities for selecting 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 s_ T ½s_ þ YðsÞ ¼ 0 and for s 6¼ 0 desired acceleration des q 2 Rn1 can be determined as € € des ¼ €q eq YðsÞ q

ð7:14Þ

284


The acceleration enforcing the desired structure of the Lyapunov function derivative consists of the equivalent acceleration €q eq and the convergence acceleration q con ¼ 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€q eq AYðsÞ ¼ seq AYðsÞ ¼ sd þ A€q des

ð7:15Þ

¼ b þ g þ A€q des

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 componentwise using observers. Then (7.14) can be expressed as eq

€ des ¼ €q^ YðsÞ q dzi ¼ g €q des i þ gs i zi ; dt eq q€î ¼ zi gsi ;

ð7:16Þ

i ¼ 1; . . . ; n

The force input (7.15) realizes the desired acceleration (7.16) thus enforcing closed loop behavior € q ffi€ q des which, by inserting (7.10) and (7.16) becomes 9 €q ¼ €q ref CDq_ YðsÞ pð€q eq Þ =

s_ þ YðsÞ ¼ pð€ q eq Þ eq ; pð€q eq Þ ¼ €q eq €q^

ð7:17Þ

The closed loop behavior depends on the selection of the convergence acceleration € con ¼ YðsÞ. For a small equivalent acceleration estimation error pð€q eq Þ 0, Equation (7.17) q reduces to the ideal system s_ þ YðsÞ ¼ 0 or componentwise s_ i þ Ci ðsÞ ¼ 0; i ¼ 1; . . . ; n. This result is the same as for 1-dof systems. that obtained Assume available s_ q; qref . Then, by expressing €q eq from (7.10) and inserting it into (7.14) yields € des q q € des s_ q; qref þ Y s q; qref |{z} |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} control

ð7:18Þ

desired dynamics

In continuous time (7.18) cannot be implemented directly. In a discrete time realization (7.18) can be implemented as € q des ðk þ 1Þ ffi €q des ðkÞ fs_ ðkÞ þ Y½sðkÞg with approximation error oðT Þ for a system with a sampling interval T. Here the continuity of configuration and its reference along with the desired acceleration €q des is used in justifying the approximation. In continuous time (7.18) can be approximated by replacing the desired acceleration in des the right hand side expression by an approximation €q~

285

Configuration Space Control des des q ;d €~ ¼ €q des þ « € q des « €q ; d ! 0

ð7:19Þ

d!0

des

The € q~ is designed in such a way that the approximation error is uniformly bounded and tends to zero if the nonideality d introduced by the approximation mechanism tends to zero. For example, a componentwise simple first-order filter satisfies such requirements. The approximation mechanism can be selected in many different ways; for example, just a componentwise fast dynamics would serve. Insertion of (7.19) into (7.18) yields realization in the form € des ¼ €q~ q

des

s_ q; qref þ YðsÞ

ð7:20Þ

n des o Inserting (7.20) into (7.15) yields s ¼ sd þ A €q~ s_ q; qref þ YðsÞ . Plugging this control input into (7.1) yields closed loop dynamics s_ þ YðsÞ ¼ « €q des ; d þ A 1 pðQd ; sd Þ ffi 0

ð7:21Þ

The right hand side can be made small by an appropriate design of the disturbance observer des and an approximation of the desired acceleration €q~ . If theconvergence term is selected as YðsÞ ¼ Ds; D > 0 then, for generalized error _ C > 0, the control (7.20) takes the form s q; qref ¼ CDq þ Dq; €q des ¼ €q~

des

ðs_ þ DsÞ ¼ €q~

des

½D€ q þ ðC þ DÞDq_ þ DCDq |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}

ð7:22Þ

desired dynamics

and the closed loop dynamics (7.21) reduces to D€ q þ ðC þ DÞDq_ þ 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. Insertion of (7.20) into (7.15) yields s ¼ A€q des þ sd ¼ sd þ A€q~

des

A s_ q; qref þ YðsÞ

ð7:24Þ

des Introducing ~s ¼ sd þ A€ q~ the expression (7.24) can be simplified to

~s A s_ q; qref þ YðsÞ s ¼ |{z} |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} control input

ð7:25Þ

desired dynamics

From (7.19) the approximation ~s in (7.25) can be determined as ~s ¼ s þ A« €q des ; d ;

des «€ q ; d ! 0 d!0

ð7:26Þ

286


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Þ ! 0, then d!0 inserting (7.25) into (7.1) yields s_ þ YðsÞ ¼ A 1 «ðs; dÞ ! 0

ð7:27Þ

d!0

_ C > 0 and convergence term YðsÞ ¼ Ds; D > 0 For generalized error s ¼ CDq þ Dq; control (7.25) takes the form s ¼ ~s Aðs_ þ DsÞ ¼ ~s A½D€q þ ðC þ DÞDq_ þ DCDq |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}

ð7:28Þ

desired dynamics

Insertion of (7.28) into (7.1) yields closed loop dynamics s_ þ Ds ¼ D€q þ ðC þ DÞDq_ þ DCDq ¼ A 1 «ðs; dÞ ! 0 d!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 €q con ¼ YðsÞ defines the convergence to the manifold (7.9). The desired acceleration (7.20) or input force (7.25) enforces s_ þ YðsÞ ffi 0, thus motion is in the «-vicinity of the ideal closed loop dynamics. If the convergence acceleration is proportional to the generalized error €q con ¼ 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 € q con ¼ YðsÞ is selected to enforce a finite-time convergence [for example, con q ¼ 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 and motion for t t0 is in manifold (7.9). The closed loop dynamics is defined by (7.1) with input seq and the equilibrium solution s ¼ 0. Inserting (7.11) into (7.1) yields d ðDq_ þ CDqÞ ¼ 0; dt

ð7:30Þ

s¼0

The closed loop dynamics is reduced to s ¼ 0 – thus (7.9) is enforced and the equations of motion are of lower order that system (7.1). So far in our realization of the control input the disturbance and system kinetic energy matrix 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€q des ¼ ^seq An YðsÞ eq

s ¼ sdis þ An€q

eq

ð7:31Þ

287


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 ¼ An 1 ð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 An ½s_ þ YðsÞ s ¼ |{z} |fflfflfflfflfflfflffl{zfflfflfflfflfflfflffl} control

ð7:32Þ

desired dynamics

For YðsÞ ¼ Ds and control input (7.32) the closed loop dynamics (7.29) becomes s_ þ Ds ¼ An 1 «ð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 des has been expressed as € q des ¼ € q eq YðsÞ in (7.14) or as €q des ¼ €q~ 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)

288


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)

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 dependencies 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 energy matrix is expressed as in Equation (1.34) and the elements aij ; i; j ¼ 1; 2 of matrix AðqÞ are 2 2 a11 ðq2 Þ ¼ m1 lm1 þ I1 þ m2 l12 þ lm2 þ 2l1 lm2 cos ðq2 Þ þ I2 |fflfflfflfflfflffl{zfflfflfflfflfflffl} |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} axis 1

axis 2

2 a12 ðq2 Þ ¼ a21 ðq2 Þ ¼ m2 lm2 þ 2l1 lm2 cos ðq2 Þ þ I2

ð7:33Þ

2 a22 ¼ m2 lm2 þ I2

The potential energy stored in the links is given by U ¼ m1 glm1 sin ðq1 Þ þ m2 g½l1 sin ðq1 Þ þ lm2 sin ðq1 þ q2 Þ

ð7:34Þ

289


The dynamics of the planar elbow manipulator can be written as AðqÞ€q þ bðq; q_ Þ þ gðqÞ ¼ s " # " # b1 ðq; q_ Þ a11 a12 ; bðq; q_ Þ ¼ ; AðqÞ ¼ b2 ðq; q_ Þ a21 a22

" gðqÞ ¼

g1 ðqÞ

#

g2 ðqÞ

b1 ðq; q_ Þ ¼ m2 2l1 lm2 sinðq2 Þq_ 2 q_ 1 m2 2l1 lm2 sinðq2 Þq_ 2 q_ 2

ð7:35Þ

b2 ðq; q_ Þ ¼ m2 2l1 lm2 sinðq2 Þq_ 2 q_ 1 g1 ðqÞ ¼ m1 glm1 cosðq1 Þ þ m2 g½l1 cosðq1 Þ lm2 cosðq1 þ q2 Þ g2 ðqÞ ¼ m2 glm2 cosðq1 þ q2 Þ

Here AðqÞ 2 R22 is the kinetic energy matrix with elements as in (7.33), b ð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 T

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Þ

We would like to illustrate configuration T control where the configuration q is forced to track a smooth reference qref ¼ qref . For the controller design the manipulator descripqref 1 2 tion (7.35) is rearranged into a11n€q 1 ¼ t1 t1dis a22n€q 2 ¼ t2 t2dis t1dis ¼ b1 ðq; q_ Þ g1 ðqÞ ða11 a11n Þ€ q1

ð7:37Þ

t2dis ¼ a21€q 1 b2 ðq; q_ Þ g2 ðqÞ ða22 a22n Þ€ q2

The controller is now designed for tracking of q1 and q2 separately with input forces t1 ¼ t1dis þ a11n€q des 1 t2 ¼ t2dis þ a22n€q des 2 a11n ¼ 4:5 kg m2 ;

ð7:38Þ

a22n ¼ 0:55 kg m2

The generalized disturbances are estimated using observers with velocity and input forces as inputs, thus dz1 ¼ gðt1 þ ga11n q_ 1 z1 Þ; dt ^t1dis ¼ z1 ga11n q_ 1 dz2 ¼ gðt2 þ ga22n q_ 2 z2 Þ; dt ^t2dis ¼ z2 ga22n q_ 2 g ¼ 800

ð7:39Þ

290


The generalized control errors are determined as

¼ c1 e1 þ e_ 1 ; s1 q1 ; qref 1

¼ c2 e2 þ e_ 2 ; s2 q2 ; qref 2

e1 ¼ q1 qref 1 e2 ¼ q2 qref 2

ð7:40Þ

c1 ¼ c2 ¼ 25

The desired accelerations are calculated as

ref q€ des q ref 1 ¼€ 1 d1 s1 q1 ; q1

ref q ref €q des 2 ¼€ 2 d2 s2 q2 ; q2

ð7:41Þ

d1 ¼ d2 ¼ 125

The reference trajectories are selected as qref 1 ðtÞ ¼ 0:1 þ 0:15 sin ð3:14tÞ rad qref 2 ð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 Rn1 in system (7.1) needs to be controlled to satisfy algebraic equation fðqÞ ¼ 0, where fðqÞ 2 Rm1 ; m < n stands for a vector valued continuous 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 Rm1

ð7:43Þ

This means, the control needs to be selected to constrain the motion of system (7.1) in hypersurface fðqÞ ¼ 0 2 Rm1 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

291


Figure 7.4 Transients in 2-dof manipulator control. The left column shows position q1 ðtÞ; qref 1 ðtÞ, positions q2 ðtÞ; qref 2 ðtÞ and generalized errors s 1 ; 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)

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 0 1 qf ð q Þ Aq_ ¼ Fq_ f_ ¼ @ qq _ q_ € ¼ F€q þ F f

ð7:44Þ

_ q_ € ¼ FA 1 ½s ðb þ gÞ þ F f

In (7.44) F€ q stands for projection of the system acceleration in constrained direc_ q_ stands for velocity induced acceleration. The ½qfðqÞ=qq ¼ F 2 Rmn stands tions and F for a full raw rank constraint Jacobian matrix.

292


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 f F 2 Rm1 . These forces are acting in the constraint direction and, as shown in Chapter 1, the configuration space force s corresponding to f F can be expressed by sF ¼ FT f F. Let the component of the configuration space force sG 2 Rn1 which complements sF to realize arbitrary force be expressed as sG ¼ GT s0 where matrix G is yet to be determined and s0 is an arbitrary force vector in Rn . Then the input arbitrary force generated by these two components can be expressed as s ¼ FT f F þ GT s0

ð7:45Þ

_ q_ € ¼ FA 1 FT f F þ FA 1 GT s0 FA 1 ðb þ gÞ þ F f

ð7:46Þ

Inserting (7.45) into (7.44) yields

By collecting the position- and velocity-dependent terms into vector mðq; q_ Þ 2 Rm1 and terms depending only on position into vector rðqÞ 2 Rm1, Equation (7.46) can be rearranged in the following form € þ mðq; q_ Þ þ rðqÞ LF FA 1 GT s0 ¼ f F LF f 1 LF ¼ FA 1 FT _ q_ mðq; q_ Þ ¼ LF FA 1 bðq; q_ Þ F

ð7:47Þ

rðqÞ ¼ LF FA 1 gðqÞ

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 f F 2 Rm1 is the control input force acting in the constrained direction, matrix LF 2 Rmm can be interpreted as a pseudokinetic energy matrix consistent with the constraints. The control distribution matrix FA 1 FT 2 Rmm 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 _ q. _ Disturbances are expressed as functions of configuration space position and velocity F in order to clearly show their relationship. Vector LF FA 1 GT s0 stands for the projection of s0 in the constrained direction. Acceleration in the constrained direction induced by force s0 is given by FA 1 GT s0, 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 € des f F ¼ f Fdis þ LF f f Fdis ¼ mðq; q_ Þ þ rðqÞ LF FA 1 GT s0

ð7:48Þ

293


Here f Fdis is the disturbance in the constrained direction. The simplest structure of des the desired acceleration in the constrained direction is f€ ¼ KD f_ KP f with positive definite matrices KD ; KP > 0. Similarly the application of structure (7.32) yields f F ¼ ~f F LF f€ þ KD f_ þ KP f

ð7:49Þ

Here ~f F is an approximate value of the control force in the constrained direction. Both control inputs enforce the same dynamics € þ KD f_ þ KP f ffi 0 f

ð7:50Þ

The component of the configuration space force corresponding to the control force in the constrained direction is € des sF ¼ FT f F ¼ FT f Fdis þ FT LF f

ð7:51Þ

Since control (7.49) enforces stable equilibrium fðqÞ ¼ 0 2 Rm1 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 ) €des F q_ ¼ I FT LF FA 1 GT s0 A€q þ I FT LF FA 1 ðb þ gÞ FT LF f € þ KD f_ þ KP f ffi 0 f

ð7:52Þ

€ þ KD f_ þ KP f ¼ 0 should be selected fast as compared with the rest of the Dynamics f 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 Rm1 . From fðqÞ ¼ 0, m components of the configuration vector q 2 Rn1 can be determined and the resulting dynamics describe changes in n m components 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 in the constrained direction f_ f_ des ¼ 0. velocity eq eq des T € ¼ 0 in (7.46) and inserting seq ¼ FT f eq Indeed, calculating f F from f€ f F ¼ f F f F þ G s0 into (7.1) yields the first row in (7.52). For consistent lower level initial conditions the equilibrium solution fðqÞ ¼ 0 2 Rm1 is enforced. The dependence of the control in a constrained direction (7.48) on the force s0 introduces a coupling 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 1 GT s0 ¼ 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

294


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 FT F#T

ð7:53Þ

1 FA 1 GT ¼ 0YF# ¼ A 1 FT FA 1 FT

Matrix F# stands for a weighted right generalized inverse of the constraint Jacobian F, G is a null space projection matrix which satisfies FG ¼ 0; and GT FT ¼ 0, FA 1 GT ¼ 0, GA 1 FT ¼ 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. € þ m þ r ¼ f F . Control force in Selecting such a generalized inverse reduces (7.47) to LF f des ^ þ ^r þ LF f€ . The closed loop dynamthe constrained direction (7.48) reduces to f F ¼ m ics (7.52) can be now expressed as A€ q þ GT ðb þ gÞ FT LF f€des F q_ ¼ GT s0 € þ KD f_ þ KP f ¼ 0 f

GT ¼ I FT F#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_ ¼ Fq_ 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_ ¼ F1 Gq_ 2 Rðn mÞ1 with a full row rank matrix F1 2 Rðn mÞn and G 2 Rnn null space projection matrix defined in (7.53). Matrix F1 G describes two consecutive transformations. Let forces f F 2 Rm1 and f G 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 ðF1 GÞT .

295


_ w_ and the configuration By such a selection of new coordinates, new set of velocities f; space force vector can be expressed as " z_ FG ¼

f_ w_ T

s¼F

#

" ¼

F

#

G1

_ q_ ¼ JFG q;

G1 ¼ F1 G

ð7:55Þ

f F þ GT1 f G

nn

Here JFG 2 R stands for the Jacobian matrix. Due to the selection of the matrices F and F1 G, Jacobian JFG has full row rank. The acceleration €z FG can be expressed as €z FG ¼ JFG€q þ J_ FG q_

ð7:56Þ

Inserting (7.1) and (7.55) into (7.56) yields system dynamics

€ f €w

¼

FA 1 FT G1 A 1 FT

FA 1 GT1 G1 A 1 GT1

_ q_ fF FA 1 ðb þ gÞ F þ fG G1 A 1 ðb þ gÞ G_ 1 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 1 GT1 and G1 A 1 FT . Having F1 G ¼ G1 and G as in (7.53) we can verify FA 1 GT1 ¼ FA 1 GT FT1 ¼ 0mðn mÞ and G1 A 1 FT ¼ F1 GA 1 FT ¼ 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 1 GT1 ¼ 0 and G1 A 1 FT ¼ 0, the dynamics (7.57) can be rearranged into _ q_ ¼ f F LF f€ þ LF FA 1 ðb þ gÞ LF F 1 LG1€w þ LG1 G1 A ðb þ gÞ LG1 G_ 1 q_ ¼ f G 1 LF ¼ FA 1 FT 1 LG1 ¼ G1 A 1 GT1

ð7:58Þ

The projections of the system motion into constrained direction and the posture of the system are dynamically decoupled and described as m- 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

296


Let us now look at a slightly different control problem in which constraint fðqÞ is required to track 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 _ q_ þ LF f € LF€e f þ LF FA 1 ðb þ gÞ LF F

ref

¼ fF

ð7:59Þ

The force input enforcing zero acceleration in the constrained direction is 1 _ q_ þ LF f€ ref f eq ðb þ gÞ LF F F ¼ LF FA

ð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) eq f F ¼ ^f F LF KD e_ f þ KP ef

ð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 G1 A 1 ðb þ gÞ and the velocity induced _ Assume reference wref 2 Rðn mÞ1 to be tracked by w 2 Rðn mÞ1. For a acceleration G_ 1 q. error in the posture eG ¼ w wref , the control input f G 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 €e G þ KDG e_ G þ KPG eG ¼ 0;

eG ¼ w wref

ð7:62Þ

The control input f G 2 Rn m1 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 eq

f G ¼ ^f G LG1 ðKDG e_ G þ KPG eG Þ ^f eq ¼ LG1 G1 A 1 ðb þ gÞ LG1 G_ 1 q_ þ LG€ w ref G

ð7:63Þ

sG ¼ GT1 f G

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 ¼ GT FT1 f G . 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

297


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 projection matrices 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-dof manipulator is used. As shown in Chapter 1, the dynamics of the manipulator can be written in the form AðqÞ€q þ bðq; q_ Þ þ gðqÞ ¼ s

ð7:64Þ

Here AðqÞ 2 R33 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 01 cosðq2 Þ þ d02 cosðq3 Þ þ d03 cosðq2 þ q3 Þ a12 ¼ c21 þ

1 ½d 01 cosðq2 Þ þ d02 cosðq3 Þ þ d03 cosðq2 þ q3 Þ 2

a13 ¼ c31 þ

1 ½d 02 cosðq3 Þ þ d03 cosðq2 þ q3 Þ 2

a21 ¼ a12

a31 ¼ a13

a22 ¼ c22 þ d 02 cosðq3 Þ

a32 ¼ a23

a23 ¼ c32 þ

1 d 02 cosðq3 Þ 2

a33 ¼ c33

ð7:65Þ

298


Here the parameters and variables are expressed as 2 2 c11 ¼ I1 þ I2 þ I3 þ m1 lm1 þ mlm2 þ m3 lm3 2 þ ðm1 þ m2 Þl12 þ m3 l22 2 2 c22 ¼ I2 þ I3 þ m2 lm2 þ m3 lc3 þ m3 l22 2 c33 ¼ I3 þ m3 lm3 2 2 c21 ¼ I2 þ I3 þ m2 lm2 þ m3 lm3 þ m3 l22

(7.66)

2 c31 ¼ I3 þ m3 lm3

c32 ¼ c31 d01 ¼ 2m2 l1 lm2 þ 2m3 l1 l2 ; d02 ¼ 2m3 l2 lm3 ; d03 ¼ 2m3 l1 lm3 I1 ¼ m1 l12 =12; I2 ¼ m2 l22 =12; I3 ¼ m3 l32 =12

The elements of vector bT ðq; q_ Þ ¼ ½ b1

b2

b3 are expressed as

b1 ¼ 2m3 l 1 l2 q_ 1 q_ 2 sinðq2 Þ 2m2 l 1 lm2 q_ 1 q_ 2 sinðq2 Þ 2m3 l 1 lm3 q_ 1 q_ 2 sinðq2 þ q3 Þ ¼ 2m3 l 2 lm3 q_ 1 q_ 3 sinðq3 Þ 2m3 l 1 lm3 q_ 1 q_ 3 sinðq2 þ q3 Þ m2 l 1 lm2 q_ 22 sinðq2 Þ ¼ m3 l 1 lm3 q_ 22 sinðq2 þ q3 Þ m3 l 1 l2 q_ 22 sinðq2 Þ 2m3 l 2 lm3 q_ 2 q_ 3 sinðq2 þ q3 Þ ¼ m3 l2 lm3 q_ 23 sinðq3 Þ m3 l1 lm3 q_ 23 sinðq2 þ q3 Þ

(7.67)

b2 ¼ m3 l 1 l2 q_ 21 sinðq2 Þ þ m2 l 2 lm2 q_ 21 sinðq2 Þ þ m3 l 1 lm3 q_ 21 sinðq2 þ q3 Þ ¼ 2m3 l 2 lm3 q_ 1 q_ 3 sinðq3 Þ 2m3 l 2 lm3 q_ 2 q_ 3 sinðq3 Þ m3 l 2 lm3 q_ 23 sinðq3 Þ b3 ¼ m3 l 2 lm3 q_ 21 sinðq3 Þ þ m2 l 1 lm3 q_ 21 sinðq2 þ q3 Þ þ 2m3 l 2 lm3 q_ 1 q_ 2 sinðq3 Þ ¼ þ m3 l 2 lm3 q_ 221 sinðq3 Þ

The gravitational terms gT ðqÞ ¼ ½ g1

g2

g3 are

g1 ¼ gm1 lm1 cosðq1 Þ þ gm2 lm2 cosðq1 þ q2 Þ þ gm2 l1 cosðq1 Þ þ gm3 lm3 cosðq1 þ q3 Þ þ m3 l2 cosðq1 þ q2 Þ þ gm3 l1 cosðq1 Þ

(7.68)

g2 ¼ gm2 lm2 cosðq1 þ q2 Þ þ gm3 lm3 cosðq1 þ q3 Þ þ gm3 l2 cosðq1 þ q2 Þ g3 ¼ gm3 lm3 cosð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Þ

299


The constraint is selected as f ¼ q1 þ aq2 þ 0q3 ref

f

¼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 ref

€ LF FA 1 GT s0 ¼ f F LF f€ þ mðq; q_ Þ þ rðqÞ þ LF f 1 LF ¼ FA 1 FT _ q_ mðq; q_ Þ ¼ LF FA 1 bðq; q_ Þ F

ð7:72Þ

rðqÞ ¼ LF FA 1 gðqÞ

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 2

A1

ai12 ai22 ai32

ai11 6 ¼ 4 ai21 ai31

3 ai13 7 ai23 5 ai33

ai11 ¼ ða22 a33 a32 a23 Þ=detðAÞ








ai22 ¼ ða11 a33 a31 a13 Þ=detðAÞ detðAÞ ¼ a11 a22 a33 þ a12 a23 a31 þ a21 a32 a13 a13 a22 a31 a21 a12 a33 a32 a2 3a11 1 LF ¼ FA 1 FT ¼

1 ¼l ai11 þ ðai12 þ ai2 1Þa þ ai22 a2

3 2 #3 f11 ai11 þ aai12 6 #7 1 T # 4 5 F ¼ A F LF ¼ l ai21 þ aai22 ¼ 4 f21 5 ai31 þ aai32 f# 2

.

31

300


2 .

.

f# 11

3

2

6 7 ½1 G ¼ I F# F ¼ I33 4 f# 21 5

LF FA 1 ¼ l½ 1

a

f# 31 2 ai11 6 0 6 4 ai21 ai31

a ai12

1 f# 11

6 0 ¼ 4 f# 21 ai13

3

f# 31

ai22

7 ai23 7 5 ¼ l½ w11

ai32

ai33

af# 11

0

3

1 af# 21

7 05

af# 31

0

w12

w13

.

w11 ¼ ai11 þ aai21 ; w12 ¼ ai12 þ aai22 ; w13 ¼ ai13 þ aai23 _ q_ ¼ lðw11 b1 þ w12 b2 þ w13 b3 Þ l013 q mðq; q_ Þ ¼ LF FA 1 bðq; q_ Þ F

.

rðqÞ ¼ LF FA 1 gðqÞ ¼ lðw11 g1 þ w12 g2 þ w13 g3 Þ

Now (7.72) can be written as a scalar equation € þ mðq; q_ Þ þ rðqÞ ¼ fF lf

ð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 _ q€ des F ¼ 0 dF cF f þ 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 cF and dF can be selected larger. Then the control force in the constrained direction can be expressed as fF ¼ f

F dis

þ L F €q des F ¼f

F dis

þ l€ q des F

ð7:75Þ

The disturbance in the constrained direction fFdis ¼ mðq; q_ Þ þ rðqÞ is estimated by a simple observer dz ¼ g fF þ glf_ z ; dt ^f Fdis ¼ z glf_ g ¼ 600

ð7:76Þ

The projection of the constraint control into configuration space can be expressed as sF ¼ FT fF 2 3 2 3 2 3 fF t1F 1 6 7 6 7 6 7 4 t2F 5 ¼ 4 a 5fF ¼ 4 afF 5 t3F

0

0

ð7:77Þ

301


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Þ are derived 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 we will 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 qref 1 ¼ 0:5 þ 0:15 sin ð3:14tÞ rad qref 2 ¼ not specified qref 3

ð7:78Þ

¼ 0 þ 0:25 sin ð6:28tÞ rad

The controller is now designed for tracking q1 and q3 separately with input forces t1q ¼ t1dis þ a11€ q des 1 t2q ¼ t2dis t3q ¼ t3dis þ a33€ q des 3

ð7:79Þ

The configuration space input force in q2 axis is just a disturbance compensation. Generalized disturbances are estimated using observers with velocity and input forces as inputs dz1 ¼ gðt1 þ ga11 q_ 1 z1 Þ; dt ^t1dis ¼ z1 ga11 q_ 1 dz2 ¼ gðt2 þ ga22 q_ 2 z2 Þ; dt ^t2dis ¼ z2 ga22 q_ 2 dz3 ¼ gðt3 þ ga33 q_ 3 z3 Þ; dt

ð7:80Þ

^t3dis ¼ z3 ga33 q_ 3 g ¼ 600

The generalized control errors are determined as s1 ¼ c1 e1 þ e_ 1 ;

e1 ¼ q1 qref 1

s3 ¼ c3 e3 þ e_ 3 ;

e3 ¼ q3 qref 3

ð7:81Þ

c1 ¼ 5; c3 ¼ 25

The desired acceleration is calculated as q€ des q ref 1 ¼€ 1 d1 s1 q€ des q ref 3 ¼€ 3 d3 s3 d1 ¼ 25; d3 ¼ 75

ð7:82Þ

302


The configuration space forces, enforcing the desired accelerations (7.82), can be expressed as

t1q ¼ t1dis þ a11€ q des 1 ð7:83Þ

t2q ¼ t2dis þ a22 0 t3q ¼ t3dis þ a33€ q des 3

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 3 3 2 3 2 3 2 3 2 t1q t1q t1 t1F fF 7 6 7 6 7 6 7 6 7 6 4 t2 5 ¼ 4 t2q 5 þ 4 t2F 5 ¼ 4 t2q 5 þ 4 afF 5 2

t3

t3q

t3F

t3q

ð7:84Þ

0

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Þ; qref 1 ð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)


303

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Þ; qref 1 ð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 positions as in (7.78)

position q1 ðtÞ; qref 1 ð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 in the 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 can be used for the enforcement of constraints. Let the axes used to maintain constraint (7.43) be selected by the designer and specified by selection matrix S 2 Rnn Yq* ¼ 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

304


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 " z_ FS ¼

f_ S w_ S

#

¼

FS q_ ¼ JSFG q_ GS

ð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 f FG ¼ FTS f FS þ GTS f GS where f FS 2 Rm1 stands for forces in the constrained direction, and f GS stands for forces that enforce unconstrained motion. Differentiation of (7.85) on the trajectories of system (7.1) yields S

€z SFG ¼ JSFG €q þ J_ FG q_ # " # " #" # " # " _ S q_ €f S fF FS A 1 FTS FS A 1 GTS FS A 1 ðb þ gÞ F ¼ þ fG GS A 1 FTS GS A 1 GTS €w S GS A 1 ðb þ gÞ G_ S q_

ð7:86Þ

ðn mÞn Similarly as in (7.57), selection is a full row rank

GS ¼ F1 GFS where F121 RT 1 # 1 T T T #T matrix and GFS ¼ I FS FS yields FS A 1 GTS ¼ 0, with FS ¼ A FS FS A FS GS A 1 FTS ¼ 0; and dynamics (7.86) can be rearranged into

_ S q_ ¼ f FS € S þ LFS FS A 1 ðb þ gÞ LFS F LFS f LGS€w S þ LGS GS A 1 ðb þ gÞ LGS G_ S q_ ¼ f GS 1 LFS ¼ FS A 1 FTS 1 LGS ¼ GS A 1 GTS

ð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. We will 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 satisfy m < n hard holonomic constraints fðqÞ ¼ 0 2 Rm1 ; m < n. The Jacobian associated with the constraints is defined as F ¼ ½qfðqÞ=qq 2 Rmn ; m < n and is assumed to have full row rank. As a result of interaction, additional constraint forces will appear and they should be

305


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Þ FT k ¼ s

ð7:88Þ

Here k 2 Rm1 stands for the vector of Lagrange multipliers. It needs to be determined from the requirements that system (7.88) satisfies the constraint equation. Lagrange multipliers stand for the force vector needed to maintain system (7.88) in the constraint manifold 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 ¼ FT k. 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Þ ¼ Fq_ ¼ 0 2 Rm1 ; 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 Rm1 _ q_ s_ ðq; q_ Þ ¼€f ðqÞ ¼ F€q þ F _ q_ s_ ðq; q_ Þ ¼€f ðqÞ ¼ FA 1 ½s bðq; q_ Þ gðqÞ þ FA 1 FT k þ F |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} unconstrained acceleration¼€ q*

ð7:90Þ

_ q_ s_ ðq; q_ Þ ¼€f ðqÞ ¼ F€q * þ FA 1 FT k þ F

Acceleration in the constrained direction is the sum of the projected acceleration of the _ q_ in the constraint direction and unconstrained system F€ q * , the velocity induced acceleration F the constraint force induced acceleration FA 1 FT k. 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 * _ q_ are determined by the system motion, thus the balance forces – the constraint forces that and F will appear due to the interaction of the unconstrained system with constraints – are represented by Lagrange multipliers. With €q * ¼ A 1 ðs b gÞ, from (7.90) the Lagrange multipliers € ½qðtÞ ¼ 0 can be determined as satisfying s_ ðq; q_ Þ ¼ f _ q_ ¼ 0 €f ðqÞ ¼ F€q * þ FA 1 FT k þ F 1 _ q_ F€ q* þ F k ¼ FA 1 FT 1 _ q_ FA 1 ðs b gÞ þ F k ¼ FA 1 FT _ q_ k ¼ F#T ðs b gÞ LF F

ð7:91Þ

306


1 Here F#T ¼ FA 1 FT FA 1 stands for the transpose 1 of the right weighted pseudoinverse of the constraint Jacobian and LF ¼ FA 1 FT can be interpreted as the kinetic energy matrix. The structure of the pseudoinverse is the same as in (7.43). Lagrange multipliers _ q. € ðqÞ ¼ F€q þ F q_ ¼ 0 or F€q ¼ F _ (7.91) enforce constraint conditions s_ ðq; q_ Þ ¼ f Note that if the constraint equations are linear with respect to velocity _ f q_ þ Ff€q ¼ 0 using the same sðq; q_ Þ ¼ fðqÞ þ Ff ðqÞq_ ¼ 0. then from s_ ðq; q_ Þ ¼ Fq_ þ F procedure as in (7.90) one can determine the constraint forces – the Lagrange multiplier k 2 Rm1 – in a very similar form by taking matrix Ff 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 _ q_ and a projection of the joint force F#T s. It is very similar to the structure of direction F 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Þ ¼ 0 can 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 _ q_ ¼ GT s A€q þ GT ðb þ gÞ þ FT LF F fðqÞ ¼ 0 GT ¼ I FT F#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 _ q_ ¼ f F þ k € þ LF FA 1 ðb þ gÞ LF F LFf LG1€w G þ LG1 G1 A 1 ðb þ gÞ LG1 G_ 1 q_ ¼ f G

ð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

307


_ q. _ q, _ If the pro_ as shown in f F þ k ¼ F#T ðb þ gÞ LF F velocity induced acceleration F 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 f F . If the control forces can be changed, then by selecting a particular structure of f F , 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€w G þ LG G1 A 1 ðb þ gÞ LG1 G_ 1 q_ ¼ f G fðqÞ ¼ 0 2 Rm1 1 LG1 ¼ G1 A 1 GT1

ð7:94Þ

Structurally it is the same as system (7.1) with the inertia matrix being LG and the disturbance term being the sum of the projected system disturbance and the weighted constraint velocity induced acceleration. The input is represented by f G. Equation (7.94) describes the dynamics of system (7.1) consistent with holonomic constraint fðqÞ ¼ 0 2 Rm1 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Þ FT k ¼ s. Taking into account #T _ q_ we can, under assumption that constraint the constraint force k ¼ F A€ q * LF F consistent initial conditions are satisfied, write _ q_ €q ¼ I F# F €q * F# F fðqÞ ¼ 0 2 Rm1

ð7:95Þ

The derivation of (7.95) uses the identity A 1 FT F#T A ¼ F# F for the dynamically consistent right pseudoinverse F# ¼ A 1 FT LF . 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 Lagrange multipliers – are additionally acting in the system. From (7.93) it follows that the configuration forces sF ¼ FT f F corresponding to the force in the constrained direction f F can be applied to compensate for the interaction force FT k. 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 illustrate 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.

308


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Þ FT k ¼ s

ð7:96Þ

Here AðqÞ 2 R33 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Þ

The constraint Jacobian is

For a given constraint Jacobian the Lagrange multiplier that enforces zero acceleration in the constraint direction is determined as in (7.91) 1 _ q_ ¼ F#T ðs b gÞ LF F _ q_ k ¼ FA 1 FT FA 1 ðs b gÞ þ F

ð7:99Þ

The pseudoinverse F# is calculated in Example 7.2 as F#T ¼ f# . f# f# 11 21 31 _ q_ ¼ 0. By denoting the The constraint Jacobian is constant, thus the term LF F components of the force vector ðs b gÞ as sbqi ; i ¼ 1; 2; 3 the Lagrange multipliers can be expressed as

# # k ¼ f# 11 sbq1 þ f21 sbq2 þ f31 sbq3

ð7:100Þ

Insertion of force sl ¼ FT l 2 3 3 2 3 l 1 t1l 6 7 6 7 6 7 6 t2l 7 ¼ 6 a 7l ¼ 6 al 7 4 5 4 5 4 5 0 0 t3l 2

ð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 qref 3 ¼ 0 þ 0:25 sin ð6:28tÞ rad by applying configuration space input t3q ¼ t3dis þ a33€ q des , with the desired acceleration as in (7.82) and the generalized error as 3

309


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

in (7.81) s3 ¼ c3 e3 þ e_ 3 ; e3 ¼ q3 qref 3 ; c3 ¼ 25. The positions q1 ðtÞ and q2 ðtÞ are not controlled. Their dynamics are the result of the interaction forces. The configuration space input forces in all three degrees of freedom are set as sF 2 3 t1 6 7 6 t2 7 4 5 t3

¼

¼

sq þ FT l 2 tdis1 6 6 tdi2 4 t3dis þ a33€ q des 3

3

2

l

3

7 6 7 7 þ 6 al 7 5 4 5

ð7:102Þ

0l

The initial conditions are set at q1 ð0Þ ¼ 0:05; q2 ð0Þ ¼ q3 ð0Þ ¼ 0:3 rad and the constraint Jacobian is FT ¼ ½ 1 1 0 . In Figure 7.9, the left column shows positions q1 ðtÞ, q2 ðtÞ and q3 ðtÞ. The right column shows the constraint variable fðqÞ, the Lagrange multiplier l and the configuration 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

310


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) € q des ¼ 0 d c f þ f to the constraint force F F F [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 2 3 t1 6 7 6 t2 7 4 5 t3

¼

¼

sq þ FT p 2 3 2 3 l þ€ q des tdis1 F 6 7 6 7 6 tdi2 7 þ 6 a l þ€ 7 q des F 4 5 4 5 des des t3dis þ a33€q 3 0 l þ€ qF

ð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 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Þ rad and q3 ð0Þ ¼ 0:3 rad and the disturbance observer gain is g ¼ 600


311

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 conditions are q1 ð0Þ ¼ 0:05 rad; q2 ð0Þ ¼ q3 ð0Þ ¼ 0:3 rad and the 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 1. Utkin, V.I. (1992) Sliding Modes in Control and Optimization, Springer-Verlag, New York. 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.

312


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. and Khatib, O. (2005) Operational space control of multibody systems with 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., and Warren, 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-3540-23957-4. Spong, M.W., Hutchinson, S., and Vidyasagar, M. (2006) Robot Modeling and Control, John Wiley & Sons, Inc., New York.

8 Operational Space Dynamics and 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 Rp1 ; p n involves mapping the configuration space dynamics into operational space. The set of all permissible values of coordinates xðqÞ 2 Rp1 defines the operational space for a given task. In general, operational space coordinates may represent any set of coordinates defining kinematic mapping between configuration space and operational space. Assume the task vector is given by x ¼ xðqÞ;

x 2 Rp1 ;

q 2 Rn1 ;

pn

ð8:1Þ

By taking the derivative of xðqÞ 2 Rp1 , the following relationship is obtained 2 3 qx ð q Þ 5q_ ¼ JðqÞq_ x_ ¼ 4 qq

ð8:2Þ

€x ¼ J€q þ J_ q_

Here €x 2 Rp1 stands for the vector of operational space accelerations and JðqÞ 2 Rpn 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 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

314


space is defined by the transpose of the constraint Jacobian. In Chapter 1, mapping of the operational space force f x 2 Rp1 into configuration space is found to be expressed as [3] s ¼ JT f x

ð8:3Þ

If the dimension of the task vector is equal to the dimension of the configuration vector 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 Rn1 , 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 derived within 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 Jacobian matrix has full row rank 8x, outside singularity points in operational space. In the 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 Rn1 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 1 b J_ q_ þ Lx JA 1 g ¼ f x 1 Lx ¼ JA 1 JT

ð8:6Þ

315

Operational Space Dynamics and Control

Matrix Lx 1 ¼ JA 1 JT 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Þ ¼ f x lðq; q_ Þ ¼ Lx JA 1 bðq; q_ Þ J_ q_ n ðqÞ ¼ Lx JA

1

ð8:7Þ

gðqÞ

Here lðq; q_ Þ 2 Rn1 stands for the projection of centrifugal and Coriolis forces and n ðqÞ 2 Rn1 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 correspondence 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 T AJ 1 x€ þ lðq; q_ Þ þ n ðqÞ ¼ f x lðq; q_ Þ ¼ J T b J T AJ 1 J_ q_

n ðqÞ ¼ J

T

ð8:8Þ

g

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 unique way the motion 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 Rpn 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 by matrices JT ðqÞ and GT ðqÞ respectively. The structure of the projection matrix GðqÞ will be determined below. Let sx ¼ JT f x be vector of configuration space forces generated by operational space forces f x 2 Rp1 and vector that complements sx be sG ¼ GT s0 , with s0 2 Rn1 . Then the configuration space force can be expressed as s ¼ JT f x þ GT s0

ð8:9Þ

316


Insertion of (7.1) and (8.9) into €x ¼ J€ q þ J_ q_ yields Lx€x þ lx ðq; q_ Þ þ n x ðqÞ ¼ f x þ Lx JA 1 GT s0 1 Lx ¼ JA 1 JT lx ðq; q_ Þ ¼ Lx JA 1 b J_ q_

ð8:10Þ

n x ðqÞ ¼ Lx JA 1 g

1 Here Lx ¼ JA 1 JT 2 Rpp can be interpreted as the operational space kinetic energy matrix associated with selected task, lðq; q_ Þ 2 Rp1 stands for the projection of centrifugal and Coriolis forces, n ðqÞ 2 Rp1 stands for the projection of gravitational forces. The Lx JA 1 GT s0 stands for the interaction force coupling motion in operational space and its orthogonal complement space. For dynamical decoupling, s0 should not influence operational space acceleration, thus the following should be satisfied JA 1 GT s0 ¼ 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 the matrix G should be selected as null space projection matrix G ¼ I J# J GT ¼ I JT J#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 1 J# ¼ A 1 JT JA 1 JT ¼ A 1 JT Lx 1 J#T ¼ JA 1 JT JA 1 ¼ Lx JA 1

ð8:13Þ

Comparison of (7.53) with (8.13) shows identical structures, with the role of the constrained Jacobian F in (7.53) and the role of the operational space Jacobian J in (8.13) being the same. This shows first similarity in the treatment of the constraints and the tasks in multibody systems. Now the operational space dynamics can be expressed as Lx€x þ lx ðq; q_ Þ þ n x ðqÞ ¼ f x 1 Lx ¼ JA 1 JT lx ðq; q_ Þ ¼ J#T bðq; q_ Þ Lx J_ q_

ð8:14Þ

n x ðqÞ ¼ J#T gðqÞ J#T ¼ Lx JA 1

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.


317

In model (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 space velocities 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_ þ Gq_ 0 €q ¼ J# €x J_ q_ þ G€ q0

ð8:15Þ

Here G stands for the null space projection matrix (8.12) and q0 is any vector in Rn . 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 ¼ 12 q_ T Aq_ of system (7.1) under operational space velocity mapping con_ By introducing Lagrange multipliers l and forming the augmented function straints x_ ¼ Jq. _ x; _ x; _ lÞ ¼ 12 q_ T Aq_ þ lT ðx_ Jq_ Þ the minimization min Pðq; _ lÞ becomes a straightforward Pðq; q_ task which yields a Lagrange multiplier as 1 qP ¼ q_ T A lT J ¼ 0 Y lT ¼ q_ T JT JA 1 JT qq_

ð8:16Þ

Insertion l from Equation (8.16) into q_ ¼ A 1 JT l yields a configuration space velocity that minimizes kinetic energy with constraint x_ ¼ Jq_ 1 q_ ¼ A 1 JT JA 1 JT þ Gq_ 0 ¼ J# x_ þ Gq_ 0 1 J# ¼ A 1 JT JA 1 JT

ð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Þ ¼ 12 €q T A€q þ lT €x J_ q_ J€q it can be shown that the pseudoinverse as in Equation (8.17) minimizes ‘acceleration energy’ _ thus yielding configuration space acceleration as Tq€ ¼ 12 € q T A€ q under constraints J€ q ¼€ x J_ q, €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 1 T * ¼ 12 q_ T Wq_ and Tq€* ¼ 12 € q T W€ q would lead to pseudoinverse J# ¼ W 1 JT JW 1 JT , 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 acceleration vector are uniquely defined by the operational space variables. The task dynamics is described by (8.14).

318


Similarly to the procedure applied in the analysis of motion in unconstrained directions of constrained systems, let the internal configuration (we will also use the term posture) consistent with given task be described by a minimal 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_ ¼ Jq_ _ where JP ¼ ðqxP =qqÞ 2 Rðn pÞn stands and the task consistent posture vector x_ P ¼ ðJP GÞq, for the posture Jacobian and G stands for the task consistent null space projection matrix. Similarly, f P 2 Rðn pÞ1 is the posture related force vector. The posture force vector induces the configuration space force sP ¼ ðJP GÞT f P . 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 " x_ JP ¼

x_ x_ P

"

# ¼

J ðJP GÞ

#

" q_ ¼

J GP

# q_ ¼ JJP q_

ð8:19Þ

Here GP ¼ ðJP GÞ and JJP 2 Rnn are, by selection of the task and posture, full row rank Jacobian matrices. The acceleration € x JP ¼ JJP € q þ J_ JP q_ can be written in the following form "

€x €x P

#

" ¼

JA 1 JT

JA 1 GTP

GP A 1 JT

GP A 1 GTP

#"

fx fP

#

"

JA 1 ðb þ gÞ GP A 1 ðb þ gÞ

"

# þ

J_ q_ G_ P q_

# ð8:20Þ

With matrix G as in (8.12) and pseudoinverse as in (8.13) it is easy to verify the relationship JA 1 GTP ¼ 0pðn pÞ and GP A 1 JT ¼ 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 f x and f P can be selected separately. Note the similarities between the transformations and the constrained systems discussed in Chapter 7. Lx€x þ Lx JA 1 ðb þ gÞ Lx J_ q_ ¼ f x LP€x P þ LP GP A 1 ðb þ gÞ LP G_ P q_ ¼ f P 1 Lx ¼ JA 1 JT 1 LP ¼ GP A 1 GTP

ð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.

319


8.2.1 Nonredundant Task Control Let us analyze a problem in which task vector x ¼ xðqÞ 2 Rn1 with x_ ¼ Jq_ 2 Rn1 , detðJÞ 6¼ 0 is to be steered to track its vector reference xref 2 Rn1 . 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 Rn1

ð8:22Þ

The formulation is the same as discussed in Section 7.1 for enforcing motion in configuration space. Projection of the system (8.7) motion in manifold (8.22) is described by €e x ¼ €x ðqÞ €x ref ¼ J€q þ J_ q_ € x ref 1 ¼ Lx ff x ½lx ðq; q_ Þ þ n x ðqÞg Lx€ x ref

ð8:23Þ

T

s ¼ J fx

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 f x or accelerations can be 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 then mapping 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 secondorder error dynamics (8.23) can be stabilized if the desired acceleration in operational space is selected to enforce closed loop transients €e x þ KxD e_ x þ KxP ex ¼ 0

ð8:24Þ

Here matrices KxD 2 Rnn and KxP Rnn 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 x€ des ¼ €x ref ðKxD e_ x þ KxP ex Þ

ð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 f x. This procedure yields f x ¼ lx ðq; q_ Þ þ n x ðqÞ þ Lx€x des f x ¼ lx ðq; q_ Þ þ n x ðqÞ þ Lx €x ref ðKxD e_ x þ KxP ex Þ

ð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 operational space disturbance f d ¼ lx ðq; q_ Þ þ n x ðqÞ and the operational space inertia matrix Lx . By writing the inertia matrix as Lx ¼ Lxn þ DLx where Lxn is nominal inertia matrix, the

320


operational space dynamics (8.7) can be rearranged into Lxn€x þ DLxn€x þ lx ðq; q_ Þ þ n x ðqÞ ¼ f x

ð8:27Þ

Then (8.26) can be rewritten as ref f x ¼ ½lx ðq; q_ Þ þ n x ðqÞ þ DLxn€x þ Lxn x € ðKxD e_ x þ KxP ex Þ

ð8:28Þ

The generalized operational space disturbance f disx ¼ f d þ DLxn€x can be estimated componentwise using methods discussed in Chapter 4. Then the control force in operational space and its projection to configuration space can be expressed as f x ¼ ^f disx þ Lxn€x des s ¼ JT f x

ð8:29Þ

Inserting (8.29) into (8.7) yields the closed loop dynamics in operational space Lxn€x ¼ Lxn€x des px ðf disx Þ €e x þ KxD e_ x þ KxP ex ¼ px ðf disx Þ px ðf disx Þ ¼ f disx ^f disx

ð8:30Þ

The dynamics are governed by (8.24), and the generalized operational space disturbance estimation error px ðf disx Þ 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 € des ¼ J 1 x€ des J_ q_ ¼ J 1 €x ref ðKxD e_ x þ KxP ex Þ J_ q_ q

ð8:31Þ

The configuration space control enforcing (8.31) is des s ¼ ^sdis þ A€q des ¼ ^sdis þ AJ 1 x € 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 f x ¼ JT lx ðq; q_ Þ þ n x ðqÞ þ Lx€ x des

ð8:33Þ

_ The error dynamics (8.23) can be rewritten Insertion of (8.33) into (8.8) yields J€ q ¼€ x J_ q. in the following form e_ x ¼ x_ ðqÞ x_ ref ¼ Jq_ x_ ref ¼ ez e_ z ¼ €x ðqÞ €x ref ¼ J€q þ J_ q_ € x ref

ð8:34Þ

321


The control goal is to enforce the stability of equilibrium solution ex ¼ 0. By selecting ez ¼ Cex as a virtual control in the first equation of (8.34) the control error will be governed by e_ x ¼ 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 e_ xi ¼ ci exi ; 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 2 2 a11 ðq2 Þ ¼ m1 lm1 þ I1 þ m2 l12 þ lm2 þ 2l1 lm2 cosðq2 Þ þ I2 |fflfflfflfflfflffl{zfflfflfflfflfflffl} |fflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl{zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl} axis 1

a12 ðq2 Þ ¼ a21 ðq2 Þ ¼

2 m2 lm2

axis 2

þ 2l1 lm2 cosðq2 Þ þ I2

2 a22 ¼ m2 lm2 þ I2

ð8:35Þ

Figure 8.1 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 generalized errors sx ðtÞ; sy ðtÞ. The right column shows the desired accelerations €x des ðtÞ;€ y des ð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)

322


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 e_ x ðex Þ diagram. The right column shows the desired operational space accelerations € x des ðtÞ; € y des ðtÞ, the configuration des des space desired accelerations €q 1 ðtÞ;€ q 2 ð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 positions as in (8.47)

The potential energy stored in the links is given by U ¼ m1 glm1 sinðq1 Þ þ m2 g½l1 sinðq1 Þ þ lm2 sinð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€q 1 þ a12€q 2 m2 2l1 lm2 sinðq2 Þq_ 2 q_ 1 m2 2l1 lm2 sinðq2 Þq_ 2 q_ 2 þ g1 ðq1 Þ ¼ t1 a21€q 1 þ a22€q 2 m2 2l1 lm2 sinðq2 Þq_ 2 q_ 1 þ g2 ðq1 ; q2 Þ ¼ t2 g1 ðq1 ; q2 Þ ¼ m1 glm1 cosðq1 Þ þ m2 g½l1 cosðq1 Þ lm2 cosðq1 þ q2 Þ g2 ðq1 ; q2 Þ ¼ m2 glm2 cosðq1 þ q2 Þ

ð8:37Þ

323


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 ¼ l1 cosðq1 Þ þ l2 cosðq1 þ q2 Þ y ¼ l1 sinðq1 Þ þ l2 sinðq1 þ q2 Þ

ð8:38Þ

Taking the derivative of (8.38) and rearranging the equations yields the Jacobian matrix " J¼

l1 sinðq1 Þ l2 sinðq1 þ q2 Þ

l2 sinðq1 þ q2 Þ

l1 cosðq1 Þ þ l2 cosðq1 þ q2 Þ

l2 cosðq1 þ q2 Þ

# ð8:39Þ

The operational space acceleration can be expressed as €x o ¼ J€q þ J_ q_

ð8:40Þ

T We would like to illustrate the control where the end effecter position xo ¼ ½ x y is forced ref ref T . Let the generalized control errors be to track the smooth reference xref ¼ x y o defined as

sx ¼ c1 ex þ e_ x ;

ex ¼ x xref

sy ¼ c2 ey þ e_ y ; c1 ¼ c2 ¼ 50

ey ¼ y yref

ð8:41Þ

The desired acceleration can be calculated similarly as in (8.25) € des ¼ €x ref d1 sx x ð8:42Þ

y€des ¼ €y ref d2 sy d1 ¼ d2 ¼ 250

Outside singularities, the desired configuration space acceleration is then determined as in (8.31) __ €q des ¼ J 1 ð€x des o JqÞ

ð8:43Þ

and the configuration space forces are expressed as in (8.32) s ¼ ^sdis þ A€q des . 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 t1 ¼ t1dis þ a11n€q des 1 t2 ¼ t2dis þ a22n€q des 2 a11n ¼ 4:5 kg m ; 2

ð8:44Þ a22n ¼ 0:55 kg m

2

The generalized configuration space disturbances are estimated using observers with velocity and input forces as inputs

324


dz1 ¼ gðt1 þ ga11n q_ 1 z1 Þ; dt ^t1dis ¼ z1 ga11n q_ 1 dz2 ¼ gðt2 þ ga22n q_ 2 z2 Þ; dt ^t2dis ¼ z2 ga22n q_ 2 g ¼ 800

ð8:45Þ

ð8:46Þ

Reference trajectories are selected as xref ¼ 0:707 þ 0:2 cosð3:14tÞ m yref ¼ 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 enforce s x ¼ e_ x þ 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¼

s Tx s x > 0; 2

V ð0Þ ¼ 0

ð8:48Þ

The selection of control to enforce V_ ¼ s Tx Yðs x Þ < 0 will guarantee the stability of solution s x ¼ e_ x þ Cex ¼ 0, thus ex ¼ 0. Here YT ðs x Þ ¼ ½ Y1 . . . Yn 2 Rn1 stands for a vector function satisfying the condition sign½Yðs x Þ ¼ signðs x Þ where signðxÞ stands for a componentwise signð.Þ function, being 1 for xi > 0 and 1 for xi P < 0, i ¼ 1; 2; . . . ; n; thus the Lyapunov function derivative can be expressed as V_ ¼ n1 jYi jsxi signðsxi Þ. For example, if Yðs x Þ is discontinuous, then finite-time convergence to equilibrium and sliding mode motion may be enforced [6]. By selecting Yðs x Þ ¼ Ds x , 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 s Tx ½s_ x þ Yðs x Þ ¼ 0. For s x 6¼ 0 the desired operational space acceleration can be determined as x€ des ¼ €x eq Yðs x Þ x€ eq ¼ €x ref C_ex

ð8:49Þ

The desired acceleration consists of the equivalent acceleration €x eq and the convergence acceleration € x con ¼ Yðs x Þ. 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 s Tx ½s_ x þ Yðs x Þ ¼ 0 for s x 6¼ 0 yields f x ¼ lx ðq; q_ Þ þ n x ðqÞ þ Lx€x des ¼ f eq x Lx Yðs x Þ

_ Þ þ n x ðqÞ þ Lx€x eq f eq x ¼ lx ðq; q

ð8:50Þ

325


Figure 8.3

Structure of a control system for nonredundant systems

Here f eq x 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 same way as in configuration space with the necessary changes of measured variables. If Yðs x Þ ¼ Ds x 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 €e x þ ð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 €x ref þ C_ex þ Yðs x Þ ¼ s_ x þ Yðs x Þ ¼ 0

ð8:52Þ

The convergence depends on the structure of Yðs x Þ. For finite time convergence one can select, for example, Yðs x Þ ¼ Ds x þ hsignðs x Þ < 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 Planar Manipulator Control – Operational Force Mapping 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 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 end effecter position ref the ref T xTo ¼ ½ x y is forced to track a given smooth reference xref ¼ . The generalized x y o control errors are selected in the following form sx ¼ c1 ex þ e_ x ;

ex ¼ x xref

sy ¼ c2 ey þ e_ y ; c1 ¼ c2 ¼ 50

ey ¼ y yref

ð8:53Þ

326


Figure 8.4 Transients in a planar elbow manipulator 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 right column shows the desired accelerations €x des ðtÞ;€y des ð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 is g ¼ 800 and the reference operational space positions as in (8.58)

The desired operational space acceleration is calculated as € des ¼ €x ref d1 sx x ð8:54Þ

y€des ¼ €y ref d2 sy d1 ¼ d2 ¼ 250

The operational space control forces are determined as in (8.51) f ¼ l þ n þ L€x des o with components fx ¼ fxdis þ l11€x des fy ¼ fydis þ l22€y des l11n ¼ 10 kg m ; 2

ð8:55Þ l22n ¼ 10 kg m

2

327


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 e_ x ðex Þ. The right column shows the desired accelerations in operational space €x des ðtÞ;€y des ðtÞ, the desired accelerations in configuration q des € q des 1 ðtÞ;€ 2 ð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 positions as in (8.58)

In determining control forces (8.55) the component induced by the product of the extra 1 diagonal elements of matrix L ¼ JA 1 JT and acceleration €x des o are attributed to the generalized disturbance in the operational space f dis ¼ lðq; q_ Þ þ n ðqÞ þ ðL Ln Þ€x des o . 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 ¼ JT " s¼

fx

#

fy

l1 sinðq1 Þ l2 sinðq1 þ q2 Þ l1 cosðq1 Þ þ l2 cosðq1 þ q2 Þ l2 sinðq1 þ q2 Þ

l2 cosðq1 þ q2 Þ

#"

fx

#

ð8:56Þ

fy

The components of the generalized disturbance in operational space are estimated using observers with velocity and input forces as inputs

328

Motion Control Systems dz1 ¼ gðfx þ gl11n x_ z1 Þ; dt ^f xdis ¼ z1 gl11n x_ dz2 ¼ g fy þ gl22n y_ z2 ; dt ^f ydis ¼ z2 gl22n y_

ð8:57Þ

g ¼ 800

Reference trajectories are selected as xref ¼ 0:707 þ 0:2 cosð3:14tÞ m yref ¼ 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 €x des ðtÞ;€y des ð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 e_ x ðex Þ are shown. In the right column the desired accelerations in operational space €x des ðtÞ;€ y des ðtÞ, the desired accelerations in configuration €q des q des 1 ðtÞ;€ 2 ðtÞ and the yðxÞ diagram are 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 ;

¼ xP ðqÞ xref eP xP ; xref P P ;

ex x; xref 2 Rp1

2 Rðn pÞ1 eP xP ; xref P

ð8:59Þ

Here xref and xref P stand or the task and posture references. Then the dynamics of the task and posture errors can be expressed as x ref €e x ¼ €x €x ref ¼ Lx 1 f x Lx JA 1 ðb þ gÞ Lx J_ q_ € n h io 1 x ref €e P ¼ €x P €x ref f P LP GP A 1 ðb þ gÞ LP G_ P q_ € P ¼ LP P

ð8:60Þ

Let the closed loop task and posture control error dynamics be selected as €e x þ KDX e_ x þ KPX ex ¼ 0pp €e P þ KDP e_ P þ KPP eP ¼ 0ðn pÞðn pÞ ;

ð8:61Þ

329


Here, KDX ; KPX 2 Rpp 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 x€ des ¼ €x ref ðKxD e_ x þ KxP ex Þ _ P þ KPP eP Þ x€ des x ref P ðKDP e P ¼€

ð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 f x ¼ Lx JA 1 ðb þ gÞ Lx J_ q_ þ Lx€ x des ¼ f xdes þ Lx€ x des

ð8:63Þ

The posture control force can be expressed as h i f P ¼ LP GP A 1 ðb þ gÞ LP G_ P q_ þ LP€ x des x des P ¼ f Pdes þ LP€ P

ð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 the matrices Lx and LP , 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 the task control forces could be expressed as f x ¼ Lx J_ q_ þ Lx€ x des

ð8:65Þ

And the posture control force can be expressed as f P ¼ LP G_ P q_ þ LP€ x des P

ð8:66Þ

The configuration space force can be expressed as s ¼ JT f x þ GTP f P :

ð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

_ _ € des ¼ J# x€ des J_ q_ þ G#P € q x des P GP q

ð8:68Þ

Here J# ¼ A 1 JT Lx stands for the task Jacobian pseudoinverse and G#P ¼ A 1 GTP LP stands for the posture Jacobian ðJP GÞ 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.

330


Figure 8.6

Structure of operational space control for task redundant systems

The structure of the control system in task space for redundant systems is shown in 8.6. The implementation for the f xdis ¼ hof disturbance observers provides i Figure Lx JA 1 ðb þ gÞ Lx J_ q_ and f Pdis ¼ LP GP A 1 ðb þ gÞ LP G_ P q_ components of the control 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 the manipulator can be written in the form AðqÞ€q þ bðq; q_ Þ þ gðqÞ ¼ s

ð8:69Þ

The structure of the kinetic energy matrix AðqÞ 2 R33 and the force vectors b ð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 T

331

Operational Space Dynamics and Control m1 ¼ 10 kg m2 ¼ 8 kg m3 ¼ 6 kg

l1 ¼ 1 m l2 ¼ 1 m l3 ¼ 1 m

lm1 ¼ 0:5 m lm2 ¼ 0:5 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 ¼ 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 Þ

ð8:71Þ

The task Jacobian can be calculated as " J¼ j11 j12 j13 j21 j22 j23

j11

j12

j13

j21

j22

j23

#

¼ l1 sinðq1 Þ l2 sinðq1 þ q2 Þ l3 sinðq1 þ q2 þ q3 Þ ¼ l2 sinðq1 þ q2 Þ l3 sinðq1 þ q2 þ q3 Þ ¼ l3 sinðq1 þ q2 þ q3 Þ ¼ l1 cosðq1 Þ þ l2 cosðq1 þ q2 Þ þ l3 cosðq1 þ q2 þ q3 Þ ¼ l2 cosðq1 þ q2 Þ þ l3 cosðq1 þ q2 þ q3 Þ ¼ l3 cosðq1 þ q2 þ q3 Þ

ð8:72Þ

For the operational control force design the operations space mass matrix 1 Lx ¼ JA 1 JT is needed. For its calculation the inverse of configuration space kinetic energy matrix and the Jacobian and its transpose should be calculated. As mentioned 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 2

A

1

ai11 ¼ 4 ai21 ai31

3 ai13 ai23 5 ai33

ai12 ai22 ai32

ai11 ¼ ða22 a33 a32 a23 Þ=detðAÞ ai23 ¼ ða12 a21 a23 a11 Þ=detðAÞ ai12 ¼ ða13 a32 a33 a12 Þ=detðAÞ ai31 ¼ ða21 a32 a31 a22 Þ=detðAÞ ai13 ¼ ða12 a23 a22 a13 Þ=detðAÞ ai32 ¼ ða12 a31 a32 a11 Þ=detðAÞ ai21 ¼ ða22 a31 a33 a21 Þ=detðAÞ ai33 ¼ ða11 a22 a12 a21 Þ=detðAÞ ai22 ¼ ða11 a33 a31 a13 Þ=detðAÞ detðAÞ ¼ a11 a22 a33 þ a12 a23 a31 þ a21 a32 a13 a13 a22 a31 a21 a12 a33 a32 a2 3a11 .

A 1 JT can be determined as 2

A

aj11 J ¼ 4 aj21 aj31

1 T

3 aj12 aj22 5 aj32

aj11 aj12 aj21 aj22 aj31 aj32

¼ ai11 j11 þ ai12 j12 þ ai13 j13 ¼ ai11 j21 þ ai12 j22 þ ai13 j23 ¼ ai21 j11 þ ai22 j12 þ ai23 j13 ¼ ai21 j21 þ ai22 j22 þ ai23 j23 ¼ ai31 j11 þ ai32 j12 þ ai33 j13 ¼ ai31 j21 þ ai32 j22 þ ai33 j23

332 .


The operational space control distribution matrix Lx 1 ¼ JA 1 JT Lx 1

.

¼ JA 1 JT ¼

"

l11

l12

l21

l22

#

l11 l12 l21 l22

¼ j11 aj11 þ j12 aj21 þ j13 aj31 ¼ j11 aj12 þ j12 aj22 þ j13 aj32 ¼ j21 aj11 þ j22 aj21 þ j23 aj31 ¼ j21 aj12 þ j22 aj22 þ j23 aj32

1 The operational space mass matrix Lx ¼ JA 1 JT 1 al Lx ¼ JA 1 JT ¼ al

al al

al11 ¼ l22 =det Lx 1 al12 ¼ l21 =det Lx 1 al21 ¼ l12 =det Lx 1 1 al22 ¼ l11 =det Lx det Lx 1 ¼ l11 l22 l21 l12

T The end ref effecter T position xo ¼ ½ x y is required to track smooth reference ref ref xo ¼ x . The same problem is discussed in Examples 8.1 and 8.2 for a nonrey 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 sx ¼ c1 ex þ e_ x ;

ex ¼ x xref

sy ¼ c2 ey þ e_ y ; c1 ¼ c2 ¼ 25

ey ¼ y yref

ð8:75Þ

The desired operational space acceleration is calculated as € des ¼ €x ref d1 sx x y€des ¼ €y ref d2 sy d1 ¼ d2 ¼ 375

ð8:76Þ

The operational space control forces are determined as in (8.63) f ¼ f dis þ L€x des o with components y des fx ¼ fxd þ al11€x des þ al12€ fy ¼ fyd þ al21€x des þ al22€ y des

ð8:77Þ

333


The corresponding configuration space forces are determined as in (8.33) s ¼ JT f ¼ JT

fx fy

2

3 2 3 j11 fx þ j21 fy t1 ¼ 4 t2 5 ¼ 4 j12 fx þ j22 fy 5 t3 j13 fx þ j23 fy

ð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Þ, the desired accelerations € x des ðtÞ; € y des ðtÞ and the yðxÞ diagrams are shown. Tracking in operational space is illustrated. The operational space dynamics depend on the additional forces Lx JA 1 GT s0 and only for selection of the projection matrix such that JA 1 GT s0 ¼ 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 GT s0 . 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 € x des ðtÞ;€ y des ðtÞ, the operational space forces fx ðtÞ; fy ðtÞ and the yðxÞ diagrams

334


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 € x des ðtÞ;€ y des ðtÞ, the configuration space forces due to vector s0 and the projection of force vector s0 as given in (8.80) into the operational space

space coordinates due to changes in this input. To see the changes due to the force GT s0 the T matrix G ¼ I JT J#T should be calculated first. In (8.13) the pseudoinverse is defined as J#T ¼ Lx JA 1 .

T Due to the symmetry of kinetic energy matrix A the product JA 1 ¼ A 1 JT thus above result for matrix A 1 JT 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 ja12 ja13 ja21 ja22 ja23

¼ j11 ai11 þ j12 ai21 þ j13 ai31 ¼ j11 ai12 þ j12 ai22 þ j13 ai32 ¼ j11 ai13 þ j12 ai23 þ j13 ai33 ¼ j21 ai11 þ j22 ai21 þ j23 ai31 ¼ j21 ai12 þ j22 ai22 þ j23 ai32 ¼ j21 ai13 þ j22 ai23 þ j23 ai33

335

Operational Space Dynamics and Control .

The pseudoinverse J#T ¼ Lx JA 1 can be calculated as

J#T ¼ Lx JA 1 ¼

.

al11 al21

al12 al22

"

aj11

aj21

aj31

aj12

aj22

aj32

#

The GT ¼ I JT J#T can be then determined as 2

T #T

J J

T

¼ J Lx JA

1

g J11 ¼ j11 pJ11 þ j21 pJ21 g J12 ¼ j11 pJ12 þ j21 pJ22 g J13 ¼ j11 pJ13 þ j21 pJ23 T

3 j21 pJ11 j22 5 pJ21 j23

j11 ¼ 4 j12 j13

pJ12 pJ22

pJ13 pJ23

g J21 ¼ j12 pJ11 þ j22 pJ21 g J22 ¼ j12 pJ12 þ j22 pJ22 g J23 ¼ j12 pJ13 þ j22 pJ23 T #T

G ¼ IJ J

2

1 g J11 ¼ 4 g J21 g J31

g J12 1 g J22 g J32

pJ11 pJ12 pJ13 pJ21 pJ22 pJ23

¼ al11 ja11 þ al12 ja21 ¼ al11 ja12 þ al12 ja22 ¼ al11 ja13 þ al12 ja23 ¼ al21 ja11 þ al22 ja21 ¼ al21 ja12 þ al22 ja22 ¼ al21 ja13 þ al22 ja23

g J31 ¼ j13 pJ11 þ j23 pJ21 g J32 ¼ j13 pJ12 þ j23 pJ22 g J33 ¼ j13 pJ13 þ j23 pJ23 3 g J13 g J23 5 1 g J33

Now the configuration space control force can be expressed as s ¼ JT f þ GT s0

ð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 2

3 10 sinð2tÞ 5 s0 ¼ 4 5 1

ð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 € x des ðtÞ;€ y des ð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 f t0 ¼ s ¼ JA 1 GT s0 is small in comparison with operational space force (less than 0.01%).

336


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. and Khatib, O. (eds) (2008) Springer Handbook of Robotics, Springer-Verlag, New York, ISBN: 978-3540-23957-4. 6. Utkin, V.I. (1992) Sliding Modes in Control and Optimization, Springer-Verlag, New York.

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. 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 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. Automatica, 12, 123–126. 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 space motion for a constrained system is shown in Equation (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 vector fðqÞ 2 RmC 1 with hard constraints fðqÞ ¼ 0. Let ðmT þ mC Þ < n where 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 can define a new operational space vector in a form similar to merging 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Þ 2 RðmT þ mC Þ1 fðqÞ

ð9:1Þ

By differentiating Equation (9.1) the velocity vector can be written as 2 x_ T ðqÞ ¼4 x_ ðqÞ ¼ _ fðqÞ

@xT ðqÞ @q @fðqÞ @q

3

5q_ ¼ JT q_ ¼ Jq_ F


ð9:2Þ


338

The task Jacobian JT 2 RmT n and the constraint Jacobian F 2 RmC n are by assumption 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 f T þ FT f C þ GTJ s0

ð9:3Þ

The meaning of the components is as follows: f T 2 RmT 1 stands for the operational space task force component, f C 2 RmC 1 is a component of the operational space force acting in the constraint direction and s0 2 Rn1 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 ¼

0 F k T

JTT

ð9:4Þ

From (9.2), the operational space dynamics can be determined as " # x€ T ðqÞ JT J_ T €x ðqÞ ¼ ¼ q_ qþ € _ f€ ðqÞ F F

ð9:5Þ

¼ J€q þ J_ q_

By inserting (7.88) into (9.5) yields

€T x € f

JT 1 ¼ s b g þ JTT A F

T

F

0 k

"

# J_ T q_ þ _ F

ð9:6Þ

Insertion of (9.3) into (9.6) after some algebra yields

€T x f€

"

¼

JT A 1 JTT FA 1 JTT

JT A 1 FT FA 1 FT

#

fT fC

þ

0 k

"

þ JA 1 GTJ s0

# " # JT A 1 J_ T ðb þ g Þ þ q_ 1 _ FA F ð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

Interactions in Operational Space

339

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 1 J# ¼ A 1 JT JA 1 JT

ð9:8Þ

JA 1 GTJ s0 ¼ 0

ð9:9Þ

Then

holds. Matrices JT A 1 JTT 2 RmT mT and FA 1 FT 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 " # # 0 fT JT A 1 g JT A 1 b J_ T q_ ¼ þ L _ q_ k fC FA 1 g FA 1 b F " #1 L11 ðqÞ L12 ðqÞ JT A 1 JTT JT A 1 FT ¼ LðqÞ ¼ L21 ðqÞ L22 ðqÞ FA 1 JTT FA 1 FT "

LðqÞ€x þ L

ð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 interaction with the constraints is on the left hand side as € ¼ f_ ¼ 0, a part of the system disturbance. Taking into account the constraint conditions f 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 € ¼ 0 and L22 ðqÞ f € ¼ 0. depending on € x T since products L12 ðqÞ f 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 of motion are as follows: .

Task dynamics L11 ðqÞ€x T þ lT ðq; q_ Þ þ n T ðqÞ ¼ f T

_ q_ lT ðq; q_ Þ ¼ L11 JT A 1 b J_ T q_ þ L12 FA 1 b F n T ðqÞ ¼ L11 JT A

.

1

g þ L12 FA

1

ð9:11Þ

g

Projection into constraint direction L21 ðqÞ€x T þ lF ðq; q_ Þ þ n F ðqÞ ¼ f C þ k

_ q_ lF ðq; q_ Þ ¼ L21 JT A 1 b J_ T q_ þ L22 FA 1 b F

ð9:12Þ

n F ðqÞ ¼ L21 JT A 1 g þ L22 FA 1 g

€ ¼ f_ ¼ f ¼ 0. For the soft constraints Note that this partition is valid for hard constraints f discussed in Chapter 7 with closed loop control decomposition (9.11), then (9.12) will be valid


340

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. Dynamics (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 operational 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 design may follow the same procedure as applied in Chapters 7 and 8. The task force f T 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 €e x þ KDX e_ x þ KPX ex ¼ 0;

ex ¼ xT xref T

ð9:13Þ

is enforced, and consequently the desired acceleration is expressed as x€ des x ref ðKDX e_ x þ KPX ex Þ T ¼€

ð9:14Þ

Here KDX ; KPX are positive definite diagonal matrices of proper dimension. Task control force f T and the corresponding component of the configuration space force can be expressed as ^ 11€ f T ¼ ½l ^T ðq; q_ Þ þ n^T ðqÞ þ L x des T ^ 11 €x ref ðKDX e_ x þ KPX ex Þ ^ þ n^T Þ þ L ¼ ðl sT ¼

T JTT f T

ð9:15Þ

^ 11 stands for estimation of the task inertia matrix and l ^T ; n^T stand for estimation of Here L 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 f C 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 f C 2 RmC 1 as in (9.16) one can control the constraint force ^ F ðq; q_ Þ þ n^F ðqÞ f C þ k ¼ L21€x des T þ ½l

ð9:16Þ

The corresponding component of the configuration space force becomes sC ¼ FT f C . 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 system dynamics in the proposed way decouples the system from the point of the control forces, but interconnection forces do appear as a result. By treating these forces as a


Figure 9.1

341

Operational space task control in the presence of constraints

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 constraints 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

342


configuration space degrees of freedom are needed to enforce the interaction force and thus force in certain directions and motion 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 R66 to have refis assumed 61 full rank. Errors in position and force are determined as e ¼ x x 2 R and x ef ¼ f ref f 2 R61 respectively. The first step involves the selection of: (i) operational space directions in which the position will be controlled exh ¼ Sx ex and (ii) directions in which the forces will be controlled efh ¼ Sf ef . The selection is defined by a diagonal matrix Sx 2 R66 with elements being either one for position control or zero for no position control directions, thus the vector exh 2 R61 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 R66 . The elements of matrix Sf ¼ ðI Sx Þ 2 R66 are one in the direction where 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 1 exh and eqf ¼ JT efh 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 ¼ ðSx JÞ# exh and the force control error is transformed by a transpose Jacobian eqf ¼ JT Sf 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 in Chapter 8, the operational space dynamics can be written by a simple projection of the configuration space dynamics (7.1) into the operational space. By defining the Jacobian


343

matrix related to the end effecter frame Oe as J 2 R66 the projection of the dynamics (7.1) can be written 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 1 L ¼ JA 1 JT lðq; q_ Þ ¼ J#T bðq; q_ Þ LJ_ q_

ð9:17Þ

n ðqÞ ¼ J gðqÞ J#T ¼ LJA 1 #T

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 € x des ¼ € x ref ðKD e_ þ KP eÞ, e ¼ x ¼ xref and the corresponding control force f ¼ lðq; q_ Þ þ n ðqÞ þ f int þ L€ x des

ð9:18Þ

Here € x des stands for the desired operational space acceleration. In realization, disturbances can be compensated in configuration space. Velocity induced acceleration LJ_ q_ may be also compensated in the configuration space if the term sq_ ¼ JT LJ_ 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 € x des . Force control or position tracking can be treated just by establishing a functional dependence of the desired acceleration €x des from the force or position control error. Position tracking has been already discussed in Chapter 8 and in Section 9.1. Here, we would like to address force control and then the integration of position and force control. As shown in Chapter 7, constraints on motion due to interaction with 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 ¼ Ka xf

ð9:19Þ

Here Ka 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€ x f . For reference of the contact force f ref int 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 f ref int can be expressed as ref

€e f ¼ Ka€x f € f int

ð9:20Þ


344

Figure 9.2

Selection of the compliant frame

By selecting the desired acceleration as h i _ f þ KPF ef x€ des ¼ Ka 1 f€ref f int KDF e

ð9:21Þ

the transient in the force control loop, under the assumption that €f ref int and Ka are known, can be determined as €e f þ KDF e_ f þ KPF ef ¼ 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 constraints with a selected set of configuration variables 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 frame Of needs to be aligned with this direction. Now we 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 reference Of there is a straightforward partition of: (i) the directions in which motion is constrained – and thus forces must 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 R66 with diagonal elements having binary


345

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 R66 where I66 is the identity matrix. Let T 2 R66 be the matrix describing the transformation of a generalized task (position and orientation) from the end effecter frame of reference to the constraint consistent frame of reference T : Oe ! Of . Now the partition of the operational space variables consistent with the constraint can be represented by generalized specification matrices: Tx ¼ TT Sx T 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 acceleration in the motion control loop and in the force control loop can be selected as des x€ des € x ðKDX e_ x þ KPX ex Þ x ¼ x h i _ f þ KPF ef €x des ¼ Ka 1 €f ref f int KDF e

ð9:24Þ

The operational space desired acceleration can be expressed as x€ des ¼ Tx€x des x des x þ Tf € f

ð9:25Þ

The operational space force and its projection to configuration space is f ¼ ðl þ n þ f int Þ þ L€ x des T s ¼J f

ð9:26Þ

The presented method assumes a description of the force and motion 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 perceived model 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Þ


346

Here q 2 Rn1 stands for the configuration vector, f int 2 Rm1 stands for the operational space external force and JT 2 Rnm is the transpose of a Jacobian matrix corresponding to task _ The xðqÞ 2 Rm1 . The acceleration in operational space can be expressed as €x ¼ J€q þ J_ q. operational 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 Rm1 as in (9.18). Let the desired reaction of the system on external force f int 2 Rm1 in operational space be specified as a mass–spring–damper system with parameters M; KP and; KD . Here M 2 Rmm stands for the desired operational space inertia matrix, KD 2 Rmm stands for the desired operational space dissipation matrix and KP 2 Rmm stands for the desired operational space stiffness matrix. Let the operational space reference trajectory be defined as €x ref and let the operational space desired acceleration be selected as x€ des ¼ €x ref M 1 ðKD e_ x þ KP ex 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€e x þ KD e_ x þ KP ex ¼ 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 x des € x ref M 1 ðKD e_ x þ KP ex Þ for an amount proportional to the external force tr ¼ € 1 x€ des ¼ €x des f int tr þ M

ð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 in Chapter 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 þ GT s0 , where G is a null space projection matrix and s0 stands for any vector Rn. Insertion of s ¼ JT f þ GT s0 into configuration space dynamics (9.27) yields configuration space acceleration as €q ¼ A 1 JT L x€ des J_ q_ þ A 1 GT ½s0 ðb þ gÞ

ð9:31Þ

The configuration space acceleration is equal to a projection of the task space acceleration € des and x velocity induced acceleration J_ q_ by weighted right pseudoinverse J# ¼ A 1 JT L, the 1 T 1 L ¼ JA J and uncompensated disturbance. Compensation of disturbance in the configuration space results in the impedance control system, as depicted in Figure 9.3.


Figure 9.3

347

Structure of the impedance control system

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 specification. 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 system with requirements to control impedance (or force) in the constraint direction and motion 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 des control can be realized if the desired acceleration is expressed as € q des ¼ A 1 JT L € x J_ q_ þ A 1 GT s0 . 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 in multibody systems will 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 ndof multibody system as in (9.27), a task defined by xðqÞ 2 Rm1 and an operational space Jacobian matrix Jx 2 Rmn ; 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 Rp1 ; p < m < n

ð9:32Þ


348

The fref ð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) € ½xðqÞ ¼ Jf€x þ J_ f x_ f @f 2 Rpm Jf ¼ @x

ð9:33Þ

Here Jf 2 Rpm stands for a constraint Jacobian in operational space for constraint (9.32). The operational space dynamics can be written as Lx€x þ lx þ n x ¼ f x . The relationship between operational space force f xf and the forces in the constrained direction can be expressed as f xf ¼ JTf f f . Now a projection of the operational space dynamics in the constrained direction can be written as € þ lf ðq; q_ Þ þ n f ðqÞ ¼ f f Lf f

1 h 1 Ti1 Lf ¼ Jf Lx 1 JTf ¼ Jf Jx A 1 JTx Jf 1 lf ðq; q_ Þ ¼ Lf Jf Lx lx J_ f x_

ð9:34Þ

n f ðqÞ ¼ Lf Jf Lx 1 n x f xf ¼ JTf f f

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 ¼ðJf Lx 1 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 f f acting in the constrained direction as € des ¼ f € ref KDF e_ f þ KPF ef f € f f ¼ ðlf þ n f Þ þ Lf f

des

ð9:35Þ

ref

ef ¼ f f des

€ stands for the desired acceleration in the constrained direction, ef ¼ f fref Here f € ref stands for the reference acceleration and stands for the constraint tracking error, f KDF ; KPF > 0 are positive definite matrices. As shown in Chapter 7 the decoupling of the remaining m 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_ ¼ Jz Gxf x_ with Jz 2 Rðm pÞm as a full row rank matrix and Gxf 2 Rmm as a null space projection matrix Gxf ¼ I J#f Jf associated with the constraint Jacobian in the operational space. 1 J#w ¼ Lx 1 JTf Jf Lx 1 JTf stands for a dynamically consistent pseudoinverse. The dynamics _ x þ Jzf x_ can be expressed as z ¼ Jzf€ €


349

Lzf€z þ lzf ðq; q_ Þ þ n zf ðqÞ ¼ f zf

1 h 1 T i1 Lzf ¼ Jzf Lx 1 JTzf ¼ Jzf Jx A 1 JTx Jzf lzf ðq; q_ Þ ¼ Lzf Jzf Lx 1 lx J_ zf x_

ð9:36Þ

n zf ðqÞ ¼ Lzf Jzf Lx 1 n x f xz ¼ JTzf f zf

Tracking of reference zref can be enforced by selecting the desired acceleration and corresponding force as z€des ¼ €z ref ðKDZ e_ z þ KPZ ez Þ

z des f zf ¼ lzf þ n zf þ Lzf€

ð9:37Þ

ez ¼ z zref

with tracking error ez ¼ z zref and positive definite matrices KDZ ; KPZ > 0. Inserting the operational space force f x ¼ JTf f f þ JTzf f zf along with (9.35) and (9.36) into Lx€ x þ lx ðq; q_ Þ þ n x ðqÞ ¼ f x yields the operational space acceleration

des € des J_ f x_ þ J#zf € €x ¼ J#f f z J_ zf x_

ð9:38Þ

where matrices J#f and J#zf can be expressed as

1 J#f ¼ Lx 1 JTf Jf Lx 1 JTf

1 J#zf ¼ Lx 1 JTzf Jzf Lx 1 JTzf

ð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 € des is selected as a control in the constrained direction force tracking loop, acceleration f 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 transformations 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 ¼ JTf l where l stands for the Lagrange multiplier. These multipliers can be determined using the same idea as applied in Chapter 7.


350

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 AðqÞ€q þ bðq; q_ Þ þ gðqÞ ¼ s

ð9:40Þ

The structure of the kinetic energy matrix AðqÞ 2 R33 and the force vectors b ð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 T

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 1 L ¼ JA 1 JT lðq; q_ Þ ¼ J#T bðq; q_ Þ LJ_ q_ n ðqÞ ¼ J#T gðqÞ

ð9:42Þ

J#T ¼ LJA 1

The Jacobian matrix is given as in Equation (8.72) and the operational 1 space control distribution matrix and its inverse are Lx 1 ¼ JA 1 JT , Lx ¼ JA 1 JT , respectively. The structure of these matrices is shown in Example 8.3. ref T In Example 8.3, task control is illustrated. For a given reference xref yref as in o ¼ x (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 f ¼ 0:1 þ 0:125 sin ð6:28tÞ ref

ð9:43Þ

The constraint Jacobian is F ¼ ½1 a

0

ð9:44Þ

Projection of the constraints into the task Jacobian null space yields € þ mðq; q_ Þ þ rðqÞ ¼ fF LF f 1 LF ¼ ðFGÞA 1 ðFGÞT h

i _ G_ q_ mðq; q_ Þ ¼ LF ðFGÞA 1 b F rðqÞ ¼ LF ðFGÞA 1 g

ð9:45Þ


351

Let the desired acceleration in the constrained direction be selected as in Example 7.2 € ref dF cF f þf_ q€ des F ¼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 as fF ¼ fFdis þ LF€ q des F

ð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 JT J#T can be calculated as in Example 8.3 by calculating the pseudoinverse matrix J#T ¼ Lx JA 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 space position, reference xð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 € x des ðtÞ; € y des ðtÞ and end effecter motion yðxÞ. The constraint equation is f ¼ q1 2q2 þ 0q3 ¼ 0:1 þ 0:125 sin ð6:28tÞ


352

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 constraint and its tracking error fðtÞ; ef ¼ f fref ðtÞ, the operation space desired accelerations x des ðtÞ;€ € y des ðtÞ and the end effecter motion yðxÞ. The reference constraint is given by ref f ð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 Rnn and the primary goal related Jacobian matrix JP 2 Rmn ; m < n the dynamic decoupling is achieved if the weighted pseudoinverse is selected as 1 J#P ¼ A 1 JTP JP A 1 JTP

ð9:49Þ

and the null space projection matrix is GP ¼ I J#P JP

ð9:50Þ

The dynamics and control of the secondary goal has been solved in the primary goal null space using a projection matrix JSP ¼ JS GP 2 Rðn mÞn

ð9:51Þ

where JS 2 Rðn mÞn is the Jacobian matrix 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 des _ _ þ J# € _ _ €q ¼ J#P x€ des P JP q SP x S JSP q

ð9:52Þ

Here the desired accelerations € x des x des P and € S are determined as _ P þ KPP eP Þ; x€ des x ref P ðKDP e P ¼€

eP ¼ xref P xP

_ SP þ KPSP eSP Þ; x€ des x ref SP ¼ € SP ðKDSP e

eSP ¼ xref SP xSP

ð9:53Þ

The operational space forces associated with the primary and secondary goals are determined as


353

f P ¼ f Pdis þ LP€x des P ;

f SP ¼ f SPdis þ LSP€x des SP ;

1 LP ¼ JP A 1 JTP 1 LSP ¼ JSP A 1 JTSP

ð9:54Þ

where f Pdis and f SPdis are disturbances associated with the primary and secondary goals. The configuration space forces are determined as s ¼ JTP f P þ JTSP f S

ð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 matrices F 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 2 3 3 F f_ 6 7 4 x_ 5 ¼ 6 J1 7q; 4 5 _ y_ J2 2

2

3 f h ¼ 4 x 5; y

2

F

6

3

7 7 J¼6 1 5 4 J

ð9:56Þ

J2

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 Rnn has full rank detðJÞ 6¼ 0. Formally constraint and task attributed velocities and accelerations can be expressed as h_ ¼ Jq_ _ €h ¼ J€q þ J_ q;

ð9:57Þ


354

Configuration space force is s ¼ FT f F þ JT1 f x þ JT2 f y ¼ JT f . . JT ¼ FT .. JT1 .. JT2 fT ¼ ½ fF mc 1

fx

ð9:58Þ

fy

mx 1

Here f F 2 R ; fx 2 R ; f y 2 Rmy 1 are control forces associated with the constraint and operational spaces. Let the interaction forces in the constraint and operational spaces be f Fc ; f xc and f yc , respectively. The configuration space dynamics are then described by A€q þ bðq; q_ Þ þ gðqÞ þ JT f c ¼ s JT f c ¼ FT f Fc þ JT1 f xc þ JT2 f yc

ð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 f c hT ¼ ½ f x y fT ¼ ½ fF fx fy f Tc ¼ ½ f Fc f xc f yc 1 L ¼ JA 1 JT lðq; q_ Þ ¼ LJA 1 b LJ_ q_

ð9:60Þ

n ðqÞ ¼ LJA 1 gðqÞ

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 control design, 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 22

L1

3 2 FA 1 FT 7 7 66 7 1 T .. T .. T 7 6 ¼ 66 J1 7A F .J1 .J2 7 ¼ 4 J1 A 1 FT 44 5 5 J2 A 1 FT J2 F

3

66

FA 1 JT1 J1 A 1 JT1 J2 A 1 JT1

3 FA 1 JT2 7 J1 A 1 JT2 5 J2 A 1 JT2

ð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 control distribution 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 1 JT1 ¼ 0mc mx FA 1 JT2 ¼ 0mc my J1 A 1 FT ¼ 0mx mc J1 A 1 JT2 ¼ 0mx my J2 A 1 FT ¼ 0my mc J2 A 1 JT1 ¼ 0my mx

ð9:62Þ


355

These conditions will ensure the constraints and tasks are dynamically Recalling decoupled. 1 the structure of the weighted pseudoinverse J# ¼ A 1 JT JA 1 JT and its orthogonal complement G ¼ I J# J we can verify that conditions (9.62) are met by selecting either J1 and J2 proportional to the orthogonal complement of the constraint Jacobian F. Under assumption that task xðqÞ has higher priority then matrix J1 , associated to this task should be selected as J1 ¼ Jx GF 2 Rmx n . Then the following is satisfied FA 1 JT1 ¼ FA 1 GTF JTx ¼ FA 1 I FT F#T JTx ¼ FA 1 I FT FA 1 FT FA 1 JTx ¼ 0mc n JTx ¼ 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 consistent with 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) J2 A 1 FT ¼ 0my mc and J2 A 1 JT1 ¼ 0my mx lead to the selection of matrix J2 as J2 A 1 FT ¼ 0my mc J2 A 1 JT1 ¼ 0my mx

Y J2 ¼ Jy GF 2 Rmy n Y J2 ¼ Jy GJ1 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 J2 A 1 FT ¼ Jy Inn F# F A 1 FT ¼ 0my mc J2 A 1 JT1 ¼ Jy Inn J#1 J1 A 1 JT1 ¼ 0my mx

ð9:65Þ

Matrices GF and GJ1 have the same dimension but are not the same, thus the structure of matrix 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 1 FT and Jy VA 1 JT1 , thus expressing the matrix as J2 ¼ Jy V. By adding Jy J#1 J1 A 1 FT ¼ 0my mc to the first row in (9.65), and Jy F# FA 1 JT1 ¼ 0my mx to the second row and taking into account FA 1 JT1 ¼ 0mc mx yields J2 A 1 FT ¼ Jy Inn F# F J#1 J1 A 1 FT ¼ 0my mc J2 A 1 JT1 ¼ Jy Inn F# F J#1 J1 A 1 JT1 ¼ 0my mx

ð9:66Þ

Thus we can write V ¼ Inn F# F J#1 J1 and matrix J2 ¼ Jy V becomes J2 ¼ Jy I F# F J#1 J1

ð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 1 bðq; q_ Þ LJ_ q_ and n ðqÞ ¼ LJA 1 gðqÞ become a projection of the configuration space disturbance to the appropriate operational spaces.


356

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, J1 A 1 and J2 A 1 respectively and recalling s ¼ FT f F þ JT1 f x þ JT2 f y to obtain € þ lF ðq; q_ Þ þ n F ðqÞ ¼ f F f Fc LF f Lx€x þ lx ðq; q_ Þ þ n x ðqÞ ¼ f x f xc Ly€y þ ly ðq; q_ Þ þ n y ðqÞ ¼ f y f yc

ð9:68Þ

1 Here FA 1 FT inertia matrix in the constraint direction, LF ¼ stands for the 1 1 Lx ¼ J1 A 1 JT1 and Ly ¼ J2 A 1 JT2 stand for the inertia matrices in the operational spaces. li ðq; q_ Þ þ n i ðqÞ; i ¼ F; x; y stand for the projections of the configuration space disturbance bðq; q_ Þ þ gðqÞ and the velocity induced forces. f F ; f x ; f y 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 des ref f€ ¼ f€ ðKDF e_ F þ KPF eF Þ; eF ¼ f fref €x des ¼ €x ref ðKDX e_ X þ KPX eX Þ; eX ¼ x xref €y des ¼ €y ref ðKDY e_ Y þ KPY eY Þ; eY ¼ y yref

ð9:69Þ

The corresponding operational space and configuration space forces can be expressed as des f F ¼ ðlF þ n F þ f Fc Þ þ LF f€ f x ¼ ðlx þ n x þ f xc Þ þ Lx€ x des f y ¼ ðly þ n y þ f yc Þ þ Ly€ y des

ð9:70Þ

s ¼ FT f F þ JT1 f x þ JT2 f y

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.


357

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€ q des . In such a realization, the closed loop dynamics reduce des to A€ q ¼ A€ q þ pðQd ; sdis Þ, where pðQd ; sdis Þ stands for the disturbance estimation error. Appropriate selection of the desired acceleration would guarantee the system specification. For a negligible disturbance estimation error the closed loop dynamics can be expressed as A€q ¼ A€q des ¼ 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 space acceleration is given in (9.57). Insertion of (9.71) with hT ¼ ½ f x y , the operational des T h des des des i T des des fy s ¼ J f and f ¼ fF fx into (9.57) yields J€q des ¼ h € des J_ q_ 1 des h J_ q_ € f des ¼ JA 1 JT

ð9:72Þ

Insertion of (9.72) into (9.71) yields the configuration space desired acceleration des 1 des € des ¼ AJT JA 1 JT q €h J_ q_ ¼ J# € h J_ q_

ð9:73Þ

As shown, the selection of the matrices J1 ¼ Jx GF 2 Rmx n and J2 as in (9.67) yields 1 1 with diagonal block matrices LF ¼ FA 1 FT , block diagonal matrix JA 1 JT 1 T 1 1 T 1 # L x ¼ J1 A J1 and Ly ¼ J2 A J2 . Then the right pseudoinverse J can be calculated as

A1

2 LF . . 6 FT .. JT1 .. JT2 4 0 0

0 Lx 0

3 0 . . 7 0 5 ¼ A 1 FT LF .. A 1 JT1 Lx .. A 1 JT2 Ly Ly

ð9:74Þ

Insertion of (9.74) into (9.73) yields the configuration space desired acceleration

des des _ q_ þ J# €x des J_ 1 q_ þ J# € €q ¼ F# €f F J_ 2 q_ 1 2 y F# ¼ A 1 FT LF J#1 ¼ A 1 JT1 Lx J#2 ¼ A 1 JT2 Ly

ð9:75Þ


358

Figure 9.6

Multiple task acceleration controller

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., and Warren, 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. Blajer, W. (1997) A geometric unification of constrained system dynamics. Multibody System Dynamics, 1, 3–21. de Sapio, V. and Khatib, O. (2005) Operational space control of multibody systems with 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. Automatica, 12, 123–126. 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.


359

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-3540-23957-4. Spong, M.W., Hutchinson, S., and Vidyasagar, M. (2006) Robot Modeling and Control, John Wiley & 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 desired acceleration, 282 dynamics, 279 generalized control error, 282 independent joint control, 280 recursive control, 287 system structure, 282 vector control, 281


362

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

Index

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

Index

control, 215 control error dynamics, 216 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 desired acceleration, 208 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 desired acceleration, 67 generalized disturbance, 65 interaction force, 65 matched disturbance, 49, 66 output tracking, 71, 76

363

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 desired acceleration, 319 control force, 320 control system structure, 325 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

364

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 error dynamics, 329 control force, 329 control system structure, 330 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

Index

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

Motion Control Systems

Uncertain dynamical systems. Stability and motion control

Uncertain Dynamical Systems: Stability and Motion Control

Motion control handbook

Robot Motion and Control

Introduction to Structural Motion Control

High Speed Precision Motion Control

Robot Motion Planning and Control

Control Systems

Motion Control in Offshore and Dredging

Precision Motion Control: Design and Implementation (Advances in Industrial Control)

Robot Motion and Control: Recent Developments

Nonlinear Control Systems

Nonlinear control systems 2

Neural Systems for Control

Fuzzy Control Systems

Optimal Control Systems

Nonlinear Dynamical Control Systems

Fuzzy Control Systems

Boiler Control Systems Engineering

Adaptive Control Systems

Neural Systems for Control

Advanced Train Control Systems

Control Systems Engineering

Automatic Control Systems

Hydraulic Control Systems

Nonlinear Control Systems

Automatic Flight Control Systems

Time lag control systems

Neural Systems for Control

Control Systems - Classical Controls

Motion Control Systems

Uncertain dynamical systems. Stability and motion control

Uncertain Dynamical Systems: Stability and Motion Control

Motion control handbook

Robot Motion and Control

Introduction to Structural Motion Control

High Speed Precision Motion Control

Robot Motion Planning and Control

Control Systems

Motion Control in Offshore and Dredging

Precision Motion Control: Design and Implementation (Advances in Industrial Control)

Robot Motion and Control: Recent Developments

Nonlinear Control Systems

Nonlinear control systems 2

Neural Systems for Control

Fuzzy Control Systems

Optimal Control Systems

Nonlinear Dynamical Control Systems

Fuzzy Control Systems

Boiler Control Systems Engineering

Adaptive Control Systems

Neural Systems for Control

Advanced Train Control Systems

Control Systems Engineering

Automatic Control Systems

Hydraulic Control Systems

Nonlinear Control Systems

Automatic Flight Control Systems

Time lag control systems

Neural Systems for Control

Control Systems - Classical Controls

Recommend Documents