Local Stability and Ultimate Boundedness in the Control of Robot Manipulators 3030859797, 9783030859794

Citation preview

Lecture Notes in Electrical Engineering 798

Marco A. Arteaga Alejandro Gutiérrez-Giles Javier Pliego-Jiménez

Local Stability and Ultimate Boundedness in the Control of Robot Manipulators

Lecture Notes in Electrical Engineering Volume 798

Series Editors Leopoldo Angrisani, Department of Electrical and Information Technologies Engineering, University of Napoli Federico II, Naples, Italy Marco Arteaga, Departament de Control y Robótica, Universidad Nacional Autónoma de México, Coyoacán, Mexico Bijaya Ketan Panigrahi, Electrical Engineering, Indian Institute of Technology Delhi, New Delhi, Delhi, India Samarjit Chakraborty, Fakultät für Elektrotechnik und Informationstechnik, TU München, Munich, Germany Jiming Chen, Zhejiang University, Hangzhou, Zhejiang, China Shanben Chen, Materials Science and Engineering, Shanghai Jiao Tong University, Shanghai, China Tan Kay Chen, Department of Electrical and Computer Engineering, National University of Singapore, Singapore, Singapore Rüdiger Dillmann, Humanoids and Intelligent Systems Laboratory, Karlsruhe Institute for Technology, Karlsruhe, Germany Haibin Duan, Beijing University of Aeronautics and Astronautics, Beijing, China Gianluigi Ferrari, Università di Parma, Parma, Italy Manuel Ferre, Centre for Automation and Robotics CAR (UPM-CSIC), Universidad Politécnica de Madrid, Madrid, Spain Sandra Hirche, Department of Electrical Engineering and Information Science, Technische Universität München, Munich, Germany Faryar Jabbari, Department of Mechanical and Aerospace Engineering, University of California, Irvine, CA, USA Limin Jia, State Key Laboratory of Rail Traffic Control and Safety, Beijing Jiaotong University, Beijing, China Janusz Kacprzyk, Systems Research Institute, Polish Academy of Sciences, Warsaw, Poland Alaa Khamis, German University in Egypt El Tagamoa El Khames, New Cairo City, Egypt Torsten Kroeger, Stanford University, Stanford, CA, USA Yong Li, Hunan University, Changsha, Hunan, China Qilian Liang, Department of Electrical Engineering, University of Texas at Arlington, Arlington, TX, USA Ferran Martín, Departament d’Enginyeria Electrònica, Universitat Autònoma de Barcelona, Bellaterra, Barcelona, Spain Tan Cher Ming, College of Engineering, Nanyang Technological University, Singapore, Singapore Wolfgang Minker, Institute of Information Technology, University of Ulm, Ulm, Germany Pradeep Misra, Department of Electrical Engineering, Wright State University, Dayton, OH, USA Sebastian Möller, Quality and Usability Laboratory, TU Berlin, Berlin, Germany Subhas Mukhopadhyay, School of Engineering & Advanced Technology, Massey University, Palmerston North, Manawatu-Wanganui, New Zealand Cun-Zheng Ning, Electrical Engineering, Arizona State University, Tempe, AZ, USA Toyoaki Nishida, Graduate School of Informatics, Kyoto University, Kyoto, Japan Federica Pascucci, Dipartimento di Ingegneria, Università degli Studi “Roma Tre”, Rome, Italy Yong Qin, State Key Laboratory of Rail Traffic Control and Safety, Beijing Jiaotong University, Beijing, China Gan Woon Seng, School of Electrical & Electronic Engineering, Nanyang Technological University, Singapore, Singapore Joachim Speidel, Institute of Telecommunications, Universität Stuttgart, Stuttgart, Germany Germano Veiga, Campus da FEUP, INESC Porto, Porto, Portugal Haitao Wu, Academy of Opto-electronics, Chinese Academy of Sciences, Beijing, China Walter Zamboni, DIEM - Università degli studi di Salerno, Fisciano, Salerno, Italy Junjie James Zhang, Charlotte, NC, USA

The book series Lecture Notes in Electrical Engineering (LNEE) publishes the latest developments in Electrical Engineering - quickly, informally and in high quality. While original research reported in proceedings and monographs has traditionally formed the core of LNEE, we also encourage authors to submit books devoted to supporting student education and professional training in the various fields and applications areas of electrical engineering. The series cover classical and emerging topics concerning: • • • • • • • • • • • •

Communication Engineering, Information Theory and Networks Electronics Engineering and Microelectronics Signal, Image and Speech Processing Wireless and Mobile Communication Circuits and Systems Energy Systems, Power Electronics and Electrical Machines Electro-optical Engineering Instrumentation Engineering Avionics Engineering Control Systems Internet-of-Things and Cybersecurity Biomedical Devices, MEMS and NEMS

For general information about this book series, comments or suggestions, please contact [email protected]. To submit a proposal or request further information, please contact the Publishing Editor in your country: China Jasmine Dou, Editor ([email protected]) India, Japan, Rest of Asia Swati Meherishi, Editorial Director ([email protected]) Southeast Asia, Australia, New Zealand Ramesh Nath Premnath, Editor ([email protected]) USA, Canada: Michael Luby, Senior Editor ([email protected]) All other Countries: Leontina Di Cecco, Senior Editor ([email protected]) ** This series is indexed by EI Compendex and Scopus databases. **

More information about this series at https://link.springer.com/bookseries/7818

Marco A. Arteaga · Alejandro Gutiérrez-Giles · Javier Pliego-Jiménez

Local Stability and Ultimate Boundedness in the Control of Robot Manipulators

Marco A. Arteaga Departamento de Control y Robótica, DIE. Facultad de Ingeniería UNAM Coyoacán, Mexico City, Mexico

Alejandro Gutiérrez-Giles Centro de Estudios en Computación Avanzada (CECAv) UNAM Mexico City, Mexico

Javier Pliego-Jiménez Departamento de Electrónica y Telecomunicaciones, División de Física Aplicada Centro de Investigación Científica y de Educación Superior de Ensenada Ensenada, Baja California, Mexico

ISSN 1876-1100 ISSN 1876-1119 (electronic) Lecture Notes in Electrical Engineering ISBN 978-3-030-85979-4 ISBN 978-3-030-85980-0 (eBook) https://doi.org/10.1007/978-3-030-85980-0 © The Editor(s) (if applicable) and The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 This work is subject to copyright. All rights are solely and exclusively licensed by the Publisher, whether the whole or part of the material is concerned, specifically the rights of translation, reprinting, reuse of illustrations, recitation, broadcasting, reproduction on microfilms or in any other physical way, and transmission or information storage and retrieval, electronic adaptation, computer software, or by similar or dissimilar methodology now known or hereafter developed. The use of general descriptive names, registered names, trademarks, service marks, etc. in this publication does not imply, even in the absence of a specific statement, that such names are exempt from the relevant protective laws and regulations and therefore free for general use. The publisher, the authors and the editors are safe to assume that the advice and information in this book are believed to be true and accurate at the date of publication. Neither the publisher nor the authors or the editors give a warranty, expressed or implied, with respect to the material contained herein or for any errors or omissions that may have been made. The publisher remains neutral with regard to jurisdictional claims in published maps and institutional affiliations. This Springer imprint is published by the registered company Springer Nature Switzerland AG The registered company address is: Gewerbestrasse 11, 6330 Cham, Switzerland

To Heidrun, with everlasting love. —Marco A. Arteaga To my beloved family, —Alejandro Gutiérrez-Giles To my beloved family, —Javier Pliego-Jiménez

Preface

There is a wide variety of applications for industrial robot manipulators, such as welding, painting, coating, gluing, sealing, milling, drilling, grinding, screwing, wiring, fastening or assembling of devices. To carry out any of the different tasks a manipulator is supposed to do, the design of a high-performance control law is advisable. Therefore, the control of robot manipulators has received a lot of attention for the last decades. Although inherently industrial robots are thought for practical applications, many researchers have also considered them as high nonlinear systems whose study from a pure theoretical point of view is quite challenging. Problems like trajectory tracking, state observation, force control, telemanipulation, etc., are just some examples of a wide variety of subjects under study. One of the main challenges always to be considered is global stability. Roughly speaking, it means that no matter how large the initial error state may be, it ultimately will vanish. In fact, theoretically the initial error could be just infinite. Achieving this goal may be at the cost of very complex controllers and/or observers, or at the cost of making many unrealistic assumptions, such as the perfect knowledge of different robot model parameters. But, is it really necessary to invest such a huge effort to prove that the initial error can be infinite for a physical system that is confined in few cubic meters and cannot move with infinite velocity nor apply infinite forces on the environment? Another issue is that even though from a theoretical point of view all errors tend to zero exactly, in practice residual errors arise due to a variety of causes such as sensors resolution, model inaccuracies or the discretization process for computer implementation. Although global asymptotic stability is quite desirable, this book is devoted to the design of control schemes and state observers based on the premise of desirable local stability. Furthermore, ultimate boundedness is considered as an acceptable alternative to asymptotic stability. Part I contains the most basic and well-known concepts of robot manipulators. On its own, the first part of the book can be used for a postgraduate course on robot control theory. Chapter 1 provides the technical definition of industrial robot manipulators, the only kind of systems under study, and makes a summary of the most employed concepts. The usual categories of robot manipulators are given, and it is explained why it makes just sense to employ the customary kinematic arrangements. vii

viii

Preface

Chapter 2 deals with the robot’s kinematics, and it comprises the development of basic rotations among two coordinate frames as well as the definition of homogeneous transformations. Then, the three kinds of kinematics are described in full detail for serial links manipulators, i.e. direct kinematics, inverse kinematics and differential kinematics. By taking advantage of all the previous concepts, Chap. 3 develops the dynamic model of robot manipulators based on the Euler–Lagrange methodology, including the description of external forces by means of the Lagrange multipliers. Furthermore, some of the most useful physical and structural properties of the model are obtained, which prove to be important for control design. Chapter 4 presents some basic concepts and results on control theory. It is not aimed at giving a deep insight, but at making the book self-contained. However, the concepts that are inspected with more detail are precisely those that give name to this book, i.e. local stability in the sense of Lyapunov and ultimate boundedness. A rather colloquial development instead of a strict analytical approach is used for the statement of the main theorems employed for the design of control and observers schemes. Finally, Chap. 5 provides the most elemental control laws for robot manipulators. Part II is devoted to recapitulate more than two decades of experience by the authors in the development of different control laws and approaches for robot manipulators, as well as pointing out some very well-known schemes that comply with the main goal of this book, i.e. either local stability or ultimate boundedness. Chapter 6 deals with the fact that most industrial robots are not really equipped with velocity sensors, while this quantity is necessary for control laws implementation. Therefore, it is necessary to produce an estimate of joint velocities. Although this can readily be done by numerical differentiation in a digital computer, it is preferable for many reasons the design of an observer. Model-based observers of the Luenberger type are a very common solution even for highly nonlinear systems, but it is explained how much more simpler options can be used to get just as a good performance as for model-based solutions without needing at all the robot dynamic model for implementation. In fact, although theoretically it is possible to obtain a very accurate dynamic model for industrial robots, in practice this is usually not the case because many physical parameters are not known perfectly or they are even unknown. Therefore, Chap. 7 discusses two of the most common solutions to compensate robot models uncertainties, namely adaptive and robust control. The former makes use of the very well-known property of model parameters linearity, while the latter deals with the uncertainties by introducing some extra control terms to the nominal law. Pros and cons are discussed. Chapter 8 introduces some solutions to robot force control. As explained before, these are based on the authors’ own experiences, and therefore, the reader will not find common solutions just as impedance or admittance control. However, the proposed schemes take into account a wide variety of situations, just like the possible lack of both velocities and force measurements, accurate robot models or an implementation without computing inverse kinematics. Chapters 9 and 10 study the intercommunication of different robot manipulators. The former deals only with the bilateral teleoperation of two devices, while the latter takes into account three or more, i.e. robot networks. In both cases, it is assumed that time-varying delays are present in the communication channels, which constitutes a more realistic as well as

Preface

ix

more challenging situation. Also, velocity observers are designed, and for the bilateral teleoperation the concept on delayed kinematic correspondence is introduced in contrast to the usual concept of transparency when no time delays are considered at all. One of the key characteristics of all the chapters of Part II is that experimental results are shown. Much unfortunately, the COVID-19 pandemic that humanity has endured during the writing of this book kept the National Autonomous University of Mexico closed, so that instead of using the industrial robots A255 and A465 for some experiments, the also reliable but simpler Geomagic Touch manipulators were employed. While the outcomes are still valid, these smaller robots are easier to control, and it is worthy to point this out. Part III discusses the modeling of three of the robots available at the Laboratory for Robotics of the School of Engineering of the National Autonomous University of Mexico. Chapters 11 and 12 study the kinematic and dynamic modeling of the industrial robots A465 and A255 by CRS Robotics, respectively. While the kinematic models can be gotten with high accuracy, the dynamic ones are computed by making many simplifications. This has to be so because the links geometries are not symmetric, their masses densities are not uniform, and the manufacturer provides little information altogether. One of the main drawbacks of using industrial robots for testing control–observer approaches is that sometimes they do not allow to program them since the manufacturer includes its own control algorithms. Therefore, Chap. 13 makes a proposal about how some hardware devices can be built to replace the original control units, thus allowing the implementation of newly developed control schemes. Finally, Chap. 14 computes the kinematic and dynamic models of the haptic device Geomagic Touch, employed as mentioned before in all the experiments of the second part of the book. Mexico City, Mexico Mexico City, Mexico Ensenada, Mexico June 2021

Marco A. Arteaga Alejandro Gutiérrez-Giles Javier Pliego-Jiménez

Acknowledgements The first author would like to thank the support given by the DGAPA–UNAM under grant IN117820. The second author thanks to the DGAPA–UNAM postdoctoral scholarships program. The third author thanks to the Cátedra CONACYT 1030.

Contents

Part I

Preliminaries

1

A General Overview of Robot Manipulators . . . . . . . . . . . . . . . . . . . . . 1.1 Brief History of Robot Manipulators . . . . . . . . . . . . . . . . . . . . . . . . 1.2 Industrial Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3 Common Kinematic Arrangements . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.1 Articulated Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.2 Spherical Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.3 SCARA Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.4 Cylindrical Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.5 Cartesian Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.4 Wrists and End-Effectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.4.1 Spherical Wrist . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.4.2 Common End-Effectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.5 Some Other Important Issues to Take into Account . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3 3 5 6 7 7 8 9 11 12 12 13 13 14

2

Position, Orientation and Velocity of Rigid Robot Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1 Rigid Motions and Homogeneous Transformations . . . . . . . . . . . . 2.1.1 Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1.2 Composition of Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1.3 Different Parametrizations for Rotation Matrices . . . . . . 2.1.4 Unit Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1.5 Homogeneous Transformations . . . . . . . . . . . . . . . . . . . . . 2.1.6 Skew Symmetric Matrices . . . . . . . . . . . . . . . . . . . . . . . . . 2.1.7 Angular Velocity and Acceleration . . . . . . . . . . . . . . . . . . 2.2 Direct Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2.1 Kinematic Chains . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2.2 The Denavit-Hartenberg Representation . . . . . . . . . . . . . . 2.3 Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

15 15 15 21 25 29 31 34 37 41 41 43 50 50 xi

xii

Contents

2.3.2 Kinematic Decoupling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.3.3 Inverse Position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.4 Differential Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.4.1 Analytic Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.4.2 Geometric Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.4.3 Singularities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

51 53 58 59 61 68 69

3

Dynamics of Rigid Robot Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . 71 3.1 Dynamic Modeling of Rigid Robot Manipulators . . . . . . . . . . . . . 71 3.1.1 Euler-Lagrange Equations of Motion . . . . . . . . . . . . . . . . 71 3.1.2 Kinetic Energy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72 3.1.3 Potencial Energy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77 3.2 Equations of Motion of a Robot Manipulator . . . . . . . . . . . . . . . . . 78 3.2.1 Generalized Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78 3.3 Inclusion of Environmental Forces . . . . . . . . . . . . . . . . . . . . . . . . . . 86 3.4 Model Properties . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90 3.4.1 Vectors and Matrices Properties . . . . . . . . . . . . . . . . . . . . . 90 3.4.2 Norm Related Properties . . . . . . . . . . . . . . . . . . . . . . . . . . . 93 3.4.3 Whole Model Related Properties . . . . . . . . . . . . . . . . . . . . 96 3.4.4 Holonomic Constraints Properties . . . . . . . . . . . . . . . . . . . 98 3.5 Inclusion of DC Motors in the Robot Dynamic Model . . . . . . . . . 99 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101

4

Mathematical Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1 Basic Definitions and Lemmas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Stability in the Sense of Lyapunov . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2.1 Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2.2 Main Stability Theorem . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2.3 Complementary Results . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3 Ultimate Boundedness . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3.1 Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3.2 An Ultimate Boundedness Theorem . . . . . . . . . . . . . . . . . 4.4 Sliding Surfaces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

103 103 107 107 111 119 120 120 121 126 128

5

Common Control Approaches for Robot Manipulators . . . . . . . . . . . 5.1 PD and PD+ Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2 PID Control of Robot Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . 5.3 Computed Torque Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4 Exploiting the Passive Structure of Robot Manipulators . . . . . . . . 5.5 Design in Work Space Coordinates . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

129 129 131 135 136 137 139

Contents

Part II

xiii

Looking for Semiglobal Stability or Ultimate Boundedness

6

Velocity Observer Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.1 The Nicosia and Tomei Observer . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2 Non Model Based Observer Design . . . . . . . . . . . . . . . . . . . . . . . . . 6.3 Non Model Based Observer and Control Design . . . . . . . . . . . . . . 6.4 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

143 143 149 153 161 164

7

Adaptive and Robust Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.1 The Adaptive Law by Slotine and Li . . . . . . . . . . . . . . . . . . . . . . . . 7.2 Adaptive Scheme with Velocity Observers . . . . . . . . . . . . . . . . . . . 7.3 Robust Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.4 Control-Observer Robust Scheme . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.5 Generalized Proportional Integral (GPI) Observer . . . . . . . . . . . . . 7.6 GPI Observer Without Inertia Matrix . . . . . . . . . . . . . . . . . . . . . . . . 7.7 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.7.1 Performance Comparison . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

165 165 168 179 182 187 191 194 207 212

8

Force Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.1 Robot Force Control Without Dynamic Model . . . . . . . . . . . . . . . . 8.2 Velocity and Force Observers for the Control of Robot Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.3 GPI Based Velocity/Force Observer Design for Robot Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.4 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

215 215

237 244 255

Bilateral Teleoperation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.1 Fundamental Concepts of Bilateral Manipulators Systems . . . . . . 9.2 Control and Observer Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.3 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

257 257 260 271 280

10 Robot Networks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.1 Basic Characteristic of Robot Networks . . . . . . . . . . . . . . . . . . . . . 10.2 Leaderless Consensus Problem (LCP) . . . . . . . . . . . . . . . . . . . . . . . 10.3 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.3.1 Leader-Follower Consensus Problem . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

283 283 285 298 301 310

9

230

xiv

Contents

Part III Different Testbeds and the Adaptation of Industrial Robots for Practical Implementation 11 The Robot CRS 465 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.1 Characteristics of the Robot CRS A465 . . . . . . . . . . . . . . . . . . . . . . 11.2 Kinematics of the Robot A465 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.3 Dynamics of the Robot A465 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

313 313 313 316

12 The Robot CRS 255 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.1 Characteristics of the Robot CRS A255 . . . . . . . . . . . . . . . . . . . . . . 12.2 Kinematics of the Robot A255 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.3 Dynamics of the Robot A255 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

329 329 329 332

13 Adapting the Robots CRS 465 and 255 for Original Control Laws Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13.1 Original System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13.2 Hardware Modification . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13.2.1 Signal Routing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13.2.2 Digital Stage for the A465 Manipulator . . . . . . . . . . . . . . 13.2.3 Digital Stage for the A255 Manipulator . . . . . . . . . . . . . . 13.2.4 Analog Stage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13.2.5 Power Stage and Electric Protections . . . . . . . . . . . . . . . . 13.3 Software Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Reference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

341 341 345 346 346 350 353 356 356 360

14 The Geomagic Touch Haptic Device . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14.1 Characteristics of the Geomagic Touch Manipulator . . . . . . . . . . 14.1.1 Kinematics of the Full Five DoF Robot . . . . . . . . . . . . . . 14.1.2 Direct Kinematics of the Full Five DoF Robot . . . . . . . . 14.1.3 Differential Kinematics of the Full Five DoF Robot . . . . 14.1.4 Dynamics of the Full Five DoF Robot . . . . . . . . . . . . . . . 14.2 Simplified Three DoF Geomagic Touch . . . . . . . . . . . . . . . . . . . . . . 14.2.1 Kinematics of the Three DoF Robot . . . . . . . . . . . . . . . . . 14.2.2 Direct Kinematics of the Three DoF Robot . . . . . . . . . . . 14.2.3 Differential Kinematics of the Three DoF Robot . . . . . . . 14.2.4 Dynamics of the Three DoF Robot . . . . . . . . . . . . . . . . . . 14.2.5 Linear Parametrization of the Three DoF Robot . . . . . . .

361 361 361 363 364 365 368 368 369 370 371 372

Part I

Preliminaries

Chapter 1

A General Overview of Robot Manipulators

Abstract Robot manipulators are a very particular kind of robots. They are mainly employed in the industry and therefore they are also known as industrial robots. Although they can be used for very specialized tasks, their main characteristic is the reprogrammability, i.e. the possibility of using the same manipulator for different tasks just by changing the control program code. Great accuracy and repeatability are expected and necessary to accomplish the desired jobs, and these depend on a combination of physical mechanical characteristics as well as on the design of appropriate control schemes. Due to the high nonlinear nature of robot manipulators, control design represents on its own quite a challenge.

1.1 Brief History of Robot Manipulators Robot manipulators were created in the twentieth century, although some kind of automated mechanical machines existed as early in history as in ancient Greece and Babylon. It is well known that the term robot comes from the Czech word robota and means subordinated work. It was first used in the science fiction play ˘ Rossum’s Universal Robots by Karel Capek. Outside science fiction, the first robots were created after the Second World War as a solution to the problem of handling hazardous materials, with the seminal works of Goertz (published several years after their development). Those first robots were of the master-slave type with a mechanical coupling. The objective of these mechanisms was to mimic the arm and hand movements of the operator. Later, the mechanical coupling was substituted by electrical and hydraulic actuators. In the same decade, George C. Devol developed a fully automated mechanism called programmed articulated transfer device. The main novelty of this device was its capability to follow a set of instructions that could be reprogrammed. Following this idea, the first industrial robot was presented in 1959 by Unimation, Inc. The manipulator called Unimate, designed by Joseph F. Engelberger, was successfully installed in a General Motors assembly line in 1961. After these first experimental designs, a boom of commercial robotic arms occurred in the 1960s. The American Machine and Foundry (AMF) company introduced the programmable cylindrical manipulator VERSATRAN in 1962, designed by Harry © The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control of Robot Manipulators, Lecture Notes in Electrical Engineering 798, https://doi.org/10.1007/978-3-030-85980-0_1

3

4

1 A General Overview of Robot Manipulators

Johnson and Veljko Milenkovic. In the same decade, the Freddy arm, which was pioneer in integrating the manipulator itself with vision and intelligent systems, was developed at the University of Edinburgh. In 1969, Victor Scheinman invented the Stanford arm, which was the first six-axes manipulator with a spherical wrist, following the suggestions for easier arm solutions proposed by Donald L. Pieper in his PhD thesis. Years later, Scheinman contributed to the design of the also famous MIT and PUMA arms. In the next two decades, computer-controller robotic manipulators were widely adopted in the industry, at first mainly in the automotive, but latter became gradually employed also in different areas such as chemical, electronics, and metal industries. The theory of robotic manipulation was developed at the same time as its physical counterpart. In 1955, Jacques Denavit and Richard S. Hartenberg proposed a systematic method to analyze the kinematics of a general open-chain mechanisms. By employing matrix algebra, they were able to represent the position and orientation of any two coordinate frames as an homogeneous transformation matrix. These coordinate frames can be in turn attached to any link of the manipulator, thus solving the direct kinematics problem. Inverse kinematics turned out to be more involved, even for open-chains, and several methods were developped to obtain a closed or an approximate solution. As mentioned above, in 1968 Donald L. Pieper proposed a design method for robotic manipulators called 321 kinematic structure, which always leads to a closed form solution for the inverse kinematics problem. This kind of structure is still used in most of the commercially available robotic arms. The study of the dynamics and its application to the nonlinear control problem was a very intensive research topic in the 1980s and early 1990s, resulting in most of the well known nonlinear control techniques for industrial robots such as sliding modes, adaptive, and passivity-based controllers. With the millennium change, a new perspective in robotic manipulators arose as well. While in the 20th century the great majority of the installed robots were intended for industrial purposes, in the first two decades of the 21th century an increasing number of applications are focused on human-centered systems and consequently on safe human-robot interaction. One example of such devices is the da Vinci Surgical System, developed by Intuitive Surgery Inc. Approved for its usage in human interventions in 2000, nowadays around 5000 units are installed worldwide. Some other examples of robots outside industry are the ones employed in hazardous environments (field robotics), those used to enhance human capabilities (human augmentation) and those designed to improve the quality of life (service robotics). All these trends have in common that the manipulators are no longer located in highly structured environments. Such unstructured nature represents a great challenge for the research community in a number of different areas, which in turn makes multidisciplinary and collaborative research mandatory for the upcoming years.

1.2 Industrial Robots

5

Fig. 1.1 Robots A255 and A465 by CRS Robotics and the OMNI Phantom at the Laboratory for Robotics of the National Autonomous University of Mexico

1.2 Industrial Robots Robot manipulators are also known as industrial robots. Figure 1.1 shows those available at the Laboratory for Robotics of the School of Engineering of the National Autonomous University of Mexico. First of all, it is necessary to define what a robot manipulator is. The International Organization for Standardization provides Definition 1 of manipulating industrial robots in ISO 8373. Definition 1 An industrial robot is an automatically controlled, reprogrammable, multipurpose, manipulator programmable in three or more axes, which may be either fixed in place or mobile for use in industrial automation applications.  One of the key elements of the previous definition is the reprogrammability since it implies that the very same manipulator can be used for many possible applications, and that with good accuracy and flexibility. To understand how this can be done, it is better to begin with the most basic elements of a robot manipulator: 1. Links (connected by joints) 2. Joints: a. Revolute (R) b. Prismatic (P) A revolute joint allows to rotate two links around the joint axis, while the prismatic joint allows a linear displacement of the links as depicted in Fig. 1.2. The joint variables are usually denoted by θi for revolute joints and by di for the prismatic ones, where i = 1, . . . , n. n is the total number of joints of the manipulator and it is known as the number of Degrees of Freedom (DOF) of the manipulator. A

6

1 A General Overview of Robot Manipulators

Fig. 1.2 Revolute and prismatic joints connecting two robot links

Prismatic

3D

2D

Revolute

robot should have 6 DOF because 3 of them are necessary for arbitrary positioning and 3 more for arbitrary orientation. A smaller number of DOF prevents achieving any arbitrary position and orientation of the robot’s end-effector, while a bigger number makes the manipulator to be kinematically redundant. Roughly speaking, the manipulator’s reachable workspace is the total volume spanned by the robot’s end-effector when it executes all possible movements. On its own this definition may just be of little utility, except for avoiding collisions. More important is the dexterous workspace, within which the manipulador can reach any position with arbitrary orientation. But reaching a position is kind of fuzzy. The precision of a manipulator is a measure of how close the robot’s end-effector can reach a point within its workspace. The repeatability on the other hand is a measure of how close it can reach a point taught with anteriority. Usually, digital encoders are employed to measure the value of a joint variable, while the end-effector position and orientation are computed by taking into consideration the kinematic structure of the manipulator. For that reason, industrial robots are designed to be rigid.

1.3 Common Kinematic Arrangements As it will be discussed later in full detail in Chap. 2, the inverse kinematics problem can be splitted in two easier challenges, inverse position and inverse orientation. To achieve this, the last three joints of industrial robots are chosen to have the structure of a spherical wrist, while for the first three joints there are usually five categories: articulated, spherical, SCARA, cylindrical and Cartesian manipulators.

1.3 Common Kinematic Arrangements

7

1.3.1 Articulated Manipulator This arrangement is also known as revolute or anthropomorphic. The three robots in Fig. 1.1 are of this kind, while Fig. 1.3 shows that this arrangement is made up of three revolute joints (RRR) and depicts the so-called elbow manipulator, where both axes z 1 and z 2 are parallel and perpendicular to z 0 . The workspace of an articulated manipulator is shown in Fig. 1.4.

1.3.2 Spherical Manipulator If in the previous configuration the third joint is replaced by a prismatic one (RRP), then the spherical manipulator is gotten as shown in Fig. 1.5. The name of this configuration comes from the fact that spherical coordinates can straightforwardly be employed to define the position of the end-effector or the wrist center with respect to a frame fixed at the intersection of the z 1 and z 2 axes. The corresponding workspace can be seen in Fig. 1.6.

Fig. 1.3 Articulated elbow manipulator

z0 z1 θ2

z2 θ3

shoulder

elbow θ1 body

base

8

1 A General Overview of Robot Manipulators

θ3

θ2 θ1

side view

top view

Fig. 1.4 Workspace of an articulated elbow manipulator Fig. 1.5 Spherical manipulator

z0 z1 θ2

z2

θ1

d3

1.3.3 SCARA Manipulator The Selective Compliant Articulated Robot for Assembly (SCARA) manipulator does also have a RRP arrangement, but with a different design (the joint axes z 0 , z 1 and z 2 are parallel) as shown in Fig. 1.7. This configuration is widely used in assembly tasks and it is different in appearance to the spherical manipulator despite having both a RRP structure. The corresponding workspace can be seen in Fig. 1.8.

1.3 Common Kinematic Arrangements

9

Fig. 1.6 Workspace of a spherical manipulator

Fig. 1.7 SCARA manipulator

z0

z1

z2 θ2

θ1 d3

1.3.4 Cylindrical Manipulator The cylindrical manipulator has a RPP arrangement, as shown in Fig. 1.9. Since the first joint is revolute and the last two are prismatic, this configuration owns its name due to the fact that cylindrical coordinates can be employed to describe the endeffector position with respect to the base. The corresponding workspace is shown in Fig. 1.10.

10

1 A General Overview of Robot Manipulators

Fig. 1.8 Workspace of a SCARA manipulator

Fig. 1.9 Cylindrical manipulator

d3

z2

z1

d2

z0 θ1

1.3 Common Kinematic Arrangements

11

Fig. 1.10 Workspace of the cylindrical manipulator

1.3.5 Cartesian Manipulator The Cartesian manipulator has all of its joints prismatic (PPP), as it is shown in Fig. 1.11. For this kind of manipulators, the end-effector position, or the center of the wrist, are directly the Cartesian coordinates given by the joint variables leading to the simplest of all configurations. The corresponding workspace is shown in Fig. 1.12.

Fig. 1.11 Cartesian manipulator

d2 z1

d3

z0 z2

d1

12

1 A General Overview of Robot Manipulators

Fig. 1.12 Workspace for a Cartesian manipulator

1.4 Wrists and End-Effectors 1.4.1 Spherical Wrist The connection between the arm and the end-effector is called the wrist. In the previous section the spherical wrist was mentioned many times because, while the manipulator’s main body can take any of the five basic configurations shown before, the last three joints are usually chosen to form precisely a spherical wrist, where its three axes intersect at one single point known as its center. This is shown in Fig. 1.13. The particular arrangement of the spherical wrist largely simplifies the inverse kinematic analysis as explained in detail in Sect. 2.3. It is worthy pointing out that the wrist may just have less than three joints for some manipulators, which diminishes the capability of reaching any arbitrary orientation.

Fig. 1.13 Spherical wrist pitch

roll

yaw

1.4 Wrists and End-Effectors

13

Fig. 1.14 Gripper as end-effector of an industrial robot

1.4.2 Common End-Effectors The main tool of the manipulator is located at the end of the wrist and it is called end-effector. Perhaps the most usual one is a simple gripper as shown in Fig. 1.14. Since the gripper can just open and close, a more sophisticated end-effector can be a mechanical hand, for example. Some other important tools are soldering irons, spray guns, cameras and some times even just a single finger (see Fig. 1.1).

1.5 Some Other Important Issues to Take into Account Other important aspects to consider when working with robot manipulators are the following: 1. To move each of the joints some actuators are needed, being the most commonly used: a. Electric actuators, as DC, AC or induction motors can be employed in robot manipulators. They are usually cheap, clean and silence, which explains their popularity. Also, DC motors dynamics can be straightforwardly included in the general robot model, thus allowing an easier design and implementation of control laws. b. Hydraulic actuators are fast and they can generate big torques, which allows the manipulator to move heavy loads. However, they require much more peripheral equipment like pumps, there may be some hydraulic fluid leaks and they are noisy. c. Pneumatic actuators are cheap, but they are difficult to control, so that its applicability in robotics is not too much. 2. Application area: Depending on the tasks, the robot manipulators can be classified as assembly or non-assembly. The first class corresponds usually to small electrically driven robots, while to the second one belong those manipulators capable of moving huge heavy loads or those employed for handling different items. 3. Control approach: The precision that a robot manipulator can reach depends both on physical mechanical issues and on the control technique employed for the actuators. Choosing a control scheme depends on many factors, but specially on the task to be accomplished and the information available to implement the

14

1 A General Overview of Robot Manipulators

control law. The easiest, yet not meaningless, task is position regulation, while some of the most challenging ones is bilateral teleoperation with time varying delays. Other needs can be the reconstruction of non available measurements or model parameter estimation. This books deals with all these topics.

References Fu KS, Gonzalez RC, Lee CSG (1987) Robotics: control, sensing, vision, and intelligence. McGrawHill, USA Goertz RC, Thompson WM (1954) Electronically controlled manipulator. Nucleonics (US) Ceased publication 12 Goertz RC (1954) Mechanical master-slave manipulator. Nucleonics (US) Ceased publication 12 Murray RM, Li Z, Sastry SS (1994) A mathematical introduction to robotic manipulation. CRC Press, Boca Raton, FL Ortega R, Spong MW (1989) Adaptive motion control of rigid robots: a tutorial. Automatica 25(6):877–888 Sciavicco L, Siciliano B (2000) Modeling and control of robot manipulators, 2nd edn. Springer, London Siciliano B, Khatib O (2016) Springer handbook of robotics. Springer Slotine JJE, Li W (1987) On the adaptive control of robot manipulators. Int J Robot Res 6(3):49–59 Slotine J-JE (1985) The robust control of robot manipulators. Int J Robot Res 4(2):49–64 Spong MW, Hutchinson S, Vidyasagar M (2006) Robot modeling and control. Wiley, USA

Chapter 2

Position, Orientation and Velocity of Rigid Robot Manipulators

Abstract In order to assign a task to a robot manipulator, it is a key matter to describe the position, orientation and velocity of its tool with respect to an inertial frame, usually located at the base of the robot. Although the end result may be involved, the procedure to obtain such a description is rather direct just by defining as many extra coordinate frames as degrees of freedom the manipulator has and by describing the relationship between couples of coordinate systems until arriving to the one fixed at the robot end-effector, where the working tool should be. This allows to define the orientation and position of the end-effector as a function of joint coordinates. Also in a systematic fashion it is possible to compute the linear and angular velocities of the end-effector as a function of both joints positions and velocities.

2.1 Rigid Motions and Homogeneous Transformations Regulating the position and orientation of a rigid robot manipulator may be quite a big challenge, beginning with how to describe both quantities in the first place. A common solution consists in establishing as many coordinate frames as degrees of freedom the robot has, plus one for the end-effector, where the operating tool is assumed to be fixed. This allows to compute in a rather direct way the relationship between two coordinate systems and then just to accumulate the effect of all frames.

2.1.1 Rotations Consider Fig. 2.1, where a point p ∈ R3 is represented in the coordinate frame ox0 ,y0 ,z0 as a vector. This is a very common representation in Cartesian coordinates where each axis, x0 , y0 and z 0 , has associated a unit vector in its own direction, i0 , j 0 and k0 respectively. Therefore, the vector p can be obtained as the following sum: p = px0 i0 + p y0 j 0 + pz0 k0 . © The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control of Robot Manipulators, Lecture Notes in Electrical Engineering 798, https://doi.org/10.1007/978-3-030-85980-0_2

(2.1) 15

16

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Fig. 2.1 Representation of a point p in the coordinate frame ox0 ,y0 ,z 0

z0

p k0 j0 i0

O

y0

x0

Since the coordinate system ox0 ,y0 ,z0 is right handed, the vectors i0 , j 0 and k0 satisfy k0 = i0 × j 0 j 0 = k0 × i0 and i0 = j 0 × k0 ,

(2.2)

i0 · i0 = 1 j0 · j 0 = 1 and k0 · k0 = 1.

(2.3)

and

The last relationships imply of course that i0 , j 0 and k0 are unit vectors. Also, the dot product of different vectors is always zero, i.e. i0 · j 0 = 0 j0 · k0 = 0 and k0 · i0 = 0.

(2.4)

Recall that the dot product is commutative. Instead of using (2.1), a more practical notation is the following ⎡

⎤ px0 p = ⎣ p y0 ⎦ . pz0

(2.5)

Consider now Fig. 2.2, where the very same vector is depicted, but this time together with another coordinate frame ox1 ,y1 ,z1 . Clearly, p can be expressed as p = px1 i1 + p y1 j 1 + pz1 k1 ,

(2.6)

2.1 Rigid Motions and Homogeneous Transformations Fig. 2.2 Representation of a point p in the coordinate frame ox1 ,y1 ,z 1

17 z0

x1

p

z1

k1

i1 O

y0

j1

x0 y1

or ⎡

⎤ px1 p = ⎣ p y1 ⎦ . pz1

(2.7)

While (2.1) and (2.6) are still perfectly compatible, (2.5) and (2.7) make little sense, and the reason may be rather obvious. The first couple of equations provide a sum of vectors, while the second one provide how those vectors are expressed in different coordinate frames. Therefore, it would be more convenient to rewrite (2.1) and (2.6) as ⎡ ⎡ ⎤ ⎤ px0 px1 0 p = ⎣ p y0 ⎦ and 1 p = ⎣ p y1 ⎦ , (2.8) pz0 pz1 respectively, where in general 0 p = 1 p despite they describe the very same vector p. The reason for defining two different coordinate frames is that this might constitute an advantage for the representation of a particular point or vector in the space, but now it is necessary to find the relationship between 0 p and 1 p. In fact, from (2.1) and (2.6) one gets p = px0 i0 + p y0 j 0 + pz0 k0 = px1 i1 + p y1 j 1 + pz1 k1 .

(2.9)

This relationship can better be understood when both coordinates systems are removed as shown in Fig. 2.3.

18

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Fig. 2.3 Representing p purely as sum of vectors p px1 i1 pz0 k0 O

px0 i0 + py0 j 0

py1 j 1 + pz1 k1

By using the properties given in (2.3) and (2.4), one can easily get px0 = p · i0 = px1 i1 · i0 + p y1 j 1 · i0 + pz1 k1 · i0 .

(2.10)

In the very same fashion it is possible to obtain p y0 = px1 i1 · j 0 + p y1 j 1 · j0 + pz1 k1 · j 0 pz0 = px1 i1 · k0 + p y1 j 1 · k0 + pz1 k1 · k0 .

(2.11) (2.12)

The last relationships can be written in compact form 0

p = 0 R1 1 p,

(2.13)

where ⎡

⎤ i1 · i0 j 1 · i0 k1 · i0 0 R1 = ⎣ i1 · j 0 j 1 · j 0 k1 · j 0 ⎦ . i1 · k0 j 1 · k0 k1 · k0

(2.14)

R1 ∈ R3×3 represents a transformation which maps the vector p expressed in ox1 ,y1 ,z1 , i.e. 1 p, to its representation in the frame ox0 ,y0 ,z0 , i.e. 0 p. Note that the very same procedure can be employed to find the transformation from ox0 ,y0 ,z0 to ox1 ,y1 ,z1 :

0

1

p = 1 R0 0 p,

(2.15)

where ⎡

⎤ i0 · i1 j 0 · i1 k0 · i1 1 R0 = ⎣ i 0 · j 1 j 0 · j 1 k 0 · j 1 ⎦ . i0 · k1 j 0 · k1 k0 · k1

(2.16)

2.1 Rigid Motions and Homogeneous Transformations

19

Quite obviously, 1 R0 is the inverse transformation of 0 R1 . By taking into account that the dot product is commutative, one can conclude that 1

0 T R0 = 0 R−1 1 = R1 .

(2.17)

A matrix whose inverse is its own transpose is said to be orthogonal and this implies that the columns of 0 R1 must be unit vectors orthogonal to each other. Furthermore, in general the determinant of an orthogonal matrix takes the only two possible values ±1 since the determinant of a matrix equals that of its transpose. Now, consider writing the columns of 0 R1 as r1 , r2 , r3 ∈ R3 . The meaning of these columns is easy to understand. Just choose 1 p = i1 = [1 0 0]T , which means that r1 describes the direction of the x1 -axis but in ox0 ,y0 ,z0 . The same can be said for r2 and r3 respectively for the y1 and z 1 axes. But since 0 R1 does not change neither the orientation nor the magnitude of a vector but just the coordinate frame where the vector is expressed, then r1 , r2 and r3 must own the very same properties as i1 , j 1 and k1 . On the other hand, it is possible to compute the determinant of 0 R1 as: det

0

 R1 = rT1 (r2 × r3 ) .

(2.18)

Since ox1 ,y1 ,z1 is aright handed coordinate system, then one must have r1 = r2 × r3 . Therefore it is det 0 R1 = rT1 r1 = 1. This means that this particular set of orthogonal matrices have only +1 as determinant and they are called rotation matrices and they are said to belong to the group S O(3), standing for Special Orthogonal Group of dimension 3. Example

Understanding the meaning of the columns of 0 R1 actually allows to compute it in a more simple way than using (2.14). Consider a basic rotation, i.e. a rotation around a main axis x, y or z. The most common one takes place around the z-axis, as shown in Fig. 2.4. Note that the right hand rule defines the positive value of θ . At the beginning one may think that ox0 ,y0 ,z0 and ox1 ,y1 ,z1 completely coincide, so that any vector would take the very same values in each of them. But when frame ox1 ,y1 ,z1 is rotated by an angle θ around z 0 so that (x0 , y0 ) and (x1 , y1 ) are no longer aligned, on the contrary to z 0 and z 1 . Therefore, the vector k1 in ox0 ,y0 ,z0 is still k0 . Otherwise said it is ⎡ ⎤ 0 r3 = k 1 = k 0 = k = ⎣ 0 ⎦ . (2.19) 1 Keep in mind that the regular definition of i, j and k, i.e.

20

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Fig. 2.4 Basic rotation around the z axis

y0 y1

sin(θ) cos(θ) θ

x1

cos(θ) sin(θ) θ x0

z0 ,z1

⎡ ⎤ 1 i = ⎣0⎦ 0

⎡ ⎤ 0 j = ⎣1⎦ 0

and

⎡ ⎤ 0 k = ⎣0⎦ 1

(2.20)

is used and recall that r1 and r2 are actually i1 and j 1 but expressed in ox0 ,y0 ,z0 . Trivially from Fig. 2.4 one gets ⎡

⎤ cos(θ ) r1 = ⎣ sin(θ ) ⎦ 0

(2.21)

and ⎡

⎤ − sin(θ ) r2 = ⎣ cos(θ ) ⎦ . 0

(2.22)

This way, the rotation matrix for this example is given by ⎡ ⎤ cos(θ ) − sin(θ ) 0  0 R1 = r1 r2 r3 = ⎣ sin θ cos(θ ) 0 ⎦ . 0 0 1

(2.23)

Finally, since the rotation takes place around a main axis it is customary to use the notation Rz,θ to designate the corresponding rotation matrix.

2.1 Rigid Motions and Homogeneous Transformations

21

Note that when θ = 0◦ then it holds Rz,0 = I

(2.24)

because in the very end no rotation is taking place. On the other hand, if after rotating around z by an angle θ , a second rotation takes place also around z by a new angle φ, then one actually has one single basic rotation around z but by a total angle θ + φ. In fact, it is not difficult to show that the following relationship is satisfied Rz,θ+φ = Rz,θ Rz,φ .

(2.25)

Combining (2.24) with (2.25) yields to the conclusion that R−1 z,θ = Rz,−θ .

(2.26)

This result was expected since it is equivalent to undo the rotation of θ to return ox1 ,y1 ,z1 to its original position coincident with ox0 ,y0 ,z0 . Finally, the other two basic rotations can easily be computed as: ⎡

Rx,ψ

R y,φ

⎤ 1 0 0 = ⎣ 0 cos(ψ) − sin(ψ) ⎦ 0 sin(ψ) cos(ψ) ⎡ ⎤ cos(φ) 0 sin(φ) 0 1 0 ⎦. =⎣ − sin(φ) 0 cos(φ)

(2.27)

(2.28)

Quite obviously Rx,ψ and R y,φ share the same properties as Rz,θ .

2.1.2 Composition of Rotations Recall that the rotation matrix 0 R1 in (2.13): 0

p = 0 R1 1 p,

(2.29)

represents a rotational transformation between the coordinate systems ox0 ,y0 ,z0 and ox1 ,y1 ,z1 . As seen for basic transformations, rotating twice around the very same axis results in a new basic transformation with the sum of two angles as total rotation. However, as shown in (2.25) this is equivalent to multiplying the basic rotations. It turns out that this fact is valid in general as long as the new rotation is carried out with respect to the current frame. To check this out, suppose there is a third coordinate frame ox2 ,y2 ,z2 , so that the vector p has now three representations 0 p, 1 p and 2 p, depending on the particular coordinate frame. There must exist rotation matrices so

22

2 Position, Orientation and Velocity of Rigid Robot Manipulators

that the following relationships hold p = 0 R1 1 p 0 p = 0 R2 2 p 0

1

(2.30) (2.31)

p = 1 R2 2 p,

(2.32)

where i R j ∈ S O(3). Note that 0 R1 and 0 R2 represent rotations with respect to ox0 ,y0 ,z0 , while 1 R2 does with respect to ox1 ,y1 ,z1 . Combining (2.32) and (2.30) yields 0

p = 0 R1 1 R2 2 p.

(2.33)

Therefore, from (2.31) and (2.33) one concludes that 0

R2 = 0 R1 1 R2 .

(2.34)

As a matter of fact, it is easy to show that this is a general rule, i.e. for n + 1 coordinate systems it holds 0

Rn = 0 R1 · · · n−1 Rn .

(2.35)

Note, however, that the key assumption to get (2.35) is that every rotation of the chain is carried out with respect to the current frame. The question is what happens whenever the rotations are carried out with respect to a fixed frame (e.g. the inertial one). Example Suppose that a 90◦ rotation is carried around y0 followed by another 90◦ rotation around z 0 (not around z 1 !). The result can be appreciated in Fig. 2.5. Just as explained before, rotations matrices can be obtained by inspection. Based on the figure one has

z0 y2 y1

y0 x0

z2

z1 x1

Fig. 2.5 Rotations with respect a fixed frame

x2

2.1 Rigid Motions and Homogeneous Transformations

23

y2

z0

x2

y1

y0 x0

z2

z1 x1

Fig. 2.6 Rotations with respect to current frames

⎡

⎤ 0 01 0¯ R1 = ⎣ 0 1 0 ⎦ , −1 0 0

(2.36)

and ⎡

⎤ 1 0 0 1¯ R2 = ⎣ 0 0 1 ⎦ , 0 −1 0

(2.37)

where the notation (¯·) is used to distinguish the rotations with respect to current frames (although the first one is the very same). This means that ⎡

⎤⎡ ⎤ ⎡ ⎤ 0 01 1 0 0 0 −1 0 0¯ R2 = 0 R¯ 1 1 R¯ 2 = ⎣ 0 1 0 ⎦ ⎣ 0 0 1 ⎦ = ⎣ 0 0 1 ⎦ . −1 0 0 0 −1 0 −1 0 0

(2.38)

Also by inspection it can be appreciated that the result is correct. However, this approach becomes rather impractical and very involved for arbitrary rotation angles and multiple coordinate frames. It would be for sure more desirable to carry out the rotations with respect to current frames, but the result is quite different if one just write down the basic rotation matrices and does the computation, as shown in Fig. 2.6. Based on the figure one gets ⎡

⎤ 0 01 0 R1 = ⎣ 0 1 0 ⎦ −1 0 0

⎡

and

⎤ 0 −1 0 1 R2 = ⎣ 1 0 0 ⎦ . 0 0 1

(2.39)

Therefore, the total rotation is ⎡

⎤⎡ ⎤ ⎡ ⎤ 0 01 0 −1 0 001 0 R2 = 0 R1 1 R2 = ⎣ 0 1 0 ⎦ ⎣ 1 0 0 ⎦ = ⎣ 1 0 0 ⎦ . −1 0 0 0 0 1 010

(2.40)

24

2 Position, Orientation and Velocity of Rigid Robot Manipulators

z1

z0

y1

y0

y1 x1

z1

x0

x1 z1 y2

y1 x1

z2

x2 Fig. 2.7 Substituting rotations with respect to a fixed frame by rotations with respect to current frames

Since 0 R2 in (2.40) is not the same as that in (2.38), a natural question is whether it is possible to get the same result as in (2.38) but with rotations with respect to current frames. The answer to this question is yes. Consider Fig. 2.7 where the same end result as in Fig. 2.5 is gotten but with a set of rotations with respect to the corresponding current frames. Based on the figure it is easy to see that 0 R2 can be obtained by ⎡

⎤⎡ ⎤⎡ ⎤⎡ ⎤ ⎡ ⎤ 0 01 0 0 −1 0 −1 0 0 01 0 −1 0 0¯ R2 = ⎣ 0 1 0 ⎦ ⎣ 0 1 0 ⎦ ⎣ 1 0 0 ⎦ ⎣ 0 1 0 ⎦ = ⎣ 0 0 1 ⎦ .(2.41) −1 0 0 10 0 0 0 1 −1 0 0 −1 0 0



I

The key point is that the first two rotations produce an identity matrix, so that in the very end 0 R¯ 2 can be obtained by carrying out rotations with respect to the current frame but in reverse order, i.e. 0

R¯ 2 = 1 R2 0 R1 .

(2.42)

Despite the example is quite trivial, this result can be shown to be general, no matter how many rotations are considered. In general, if the actual coordinate frame oxb ,yb ,zb with respect to the frame oxa ,ya ,za is expressed through the rotation matrix a Rb and one wants to perform a rotation Rα over another coordinate frame oxc ,yc ,zc , the resulting rotation a Rd can be computed by the composition

2.1 Rigid Motions and Homogeneous Transformations a

25

Rd = a Rb c RTb Rα c Rb ,

(2.43)

where the last three terms represent a similarity transformation, which is a well known result of linear algebra. In the example, a = c = 0, b = 1, 0 R1 = Rx,90◦ and Rα = Rz0 ,90◦ .

2.1.3 Different Parametrizations for Rotation Matrices The nine elements of a rotation matrix R ∈ S O(3) are not independent. Both columns and rows are unit vectors representing the orientation of the axes x, y and z of a righthanded coordinate system. Therefore, in the end only three parameters are necessary to characterize R. However, by no means these three parameters are unique and there exist multiple options.

2.1.3.1

Euler Angles

Since only three parameters are necessary to specify any arbitrary rotation, a natural way to describe them is by means of the so called Euler Angles (φ, θ, ψ). For this case, the total rotation is the result of three consecutive rotations with respect to the current frame. Note that it makes no sense at all to carry out two consecutive rotations around the same axis, so that the set of possible combinations reduces to 12 (XYX, XYZ, XZX, XZY, YXY, YXZ, YZX, YZY, ZXY, ZXZ, ZYX, and ZYZ). Figure 2.8 shows the most employed set of Euler Angles, which consists first of a basic rotation around the z-axis, followed by another around y and the last one also around z, resulting in

Fig. 2.8 Euler angles representation ZYZ

z0 , z0

z1

φ y1

ψ

θ θ

y0 , y0

φ y0 φ θ

x0 x0

ψ

x0

x1

26

2 Position, Orientation and Velocity of Rigid Robot Manipulators 0

R1 = Rz,φ R y,θ Rz,ψ ⎡ ⎤⎡ ⎤⎡ ⎤ cφ −sφ 0 cψ −sψ 0 cθ 0 sθ = ⎣ sφ cφ 0 ⎦ ⎣ 0 1 0 ⎦ ⎣ sψ cψ 0 ⎦ 0 0 1 −sθ 0 cθ 0 0 1 ⎡ ⎤ cφ cθ cψ − sφ sψ −cφ cθ sψ − sφ cψ cφ sθ = ⎣ sφ cθ cψ + cφ sψ −sφ cθ sψ + cφ cψ sφ sθ ⎦ . −sθ cψ sθ sψ cθ

(2.44)

A more complex problem is the inverse one, i.e. given a rotation matrix R ∈ S O(3), with components ri j , i, j = 1, 2, 3, compute the angles φ, θ , and ψ, such that the composition (2.44) results in the given matrix. To do this, consider first the function atan2 = atan2(y, x), which is the arc tangent of two arguments returning an angle which corresponds to the given quadrant, i.e. is the angle θ such that cos θ = 

x x2

+

y2

and

sin θ = 

y x2

+ y2

.

(2.45)

With this in mind, from 0 R1 it can be seen that if cθ = r33 = ±1 =⇒ sθ = 0, these angles can be computed as θ = atan2



 1

2 − r33 , r33

φ = atan2 (r23 , r13 ) ψ = atan2 (r32 , −r31 ) .

(2.46) (2.47) (2.48)

On the other hand, in the degenerated case when r33 = ±1, there are infinitely many solutions. In fact, it is easy to see that for r33 = 1 the composition degenerates into two consecutive rotations about the z axis. Therefore, a solution is θ =0 ψ + φ = atan2(r21 , r11 ).

(2.49) (2.50)

For the case when r33 = −1, a solution is given by θ =π ψ − φ = atan2(r21 , −r11 ).

(2.51) (2.52)

As in these two cases the solution involves the sum or the difference of φ and ψ, it is common to set φ = 0 to obtain a single solution.

2.1 Rigid Motions and Homogeneous Transformations

2.1.3.2

27

Yaw–Pitch–Roll Angles

Another common way to specify orientation are the so called Yaw–Pitch–Roll Angles, which consist of three consecutive rotations around x0 , y0 and z 0 , resulting in 0

R1 = Rz,φ R y,θ Rx,ψ ⎡ ⎤⎡ ⎤ ⎤⎡ cφ −sφ 0 1 0 0 cθ 0 sθ = ⎣ sφ cφ 0 ⎦ ⎣ 0 1 0 ⎦ ⎣ 0 cψ −sψ ⎦ 0 sψ cψ 0 0 1 −sθ 0 cθ ⎡ ⎤ cφ cθ −sφ cψ + cφ sθ sψ sφ sψ + cφ sθ cψ sφ cθ cφ cψ + sφ sθ sψ −cφ sψ + sφ sθ cψ ⎦ . =⎣ −sθ cθ sψ cθ cψ

(2.53)

For this representation, the inverse problem solution can be found if r31 = −sθ = ±1 =⇒ cθ = 0, as    2 θ = atan2 −r31 , 1 − r31

(2.54)

φ = atan2 (r21 , r11 ) ψ = atan2 (r32 , r33 ) .

(2.55) (2.56)

In the case of r31 = 1 a solution is θ = −π/2 φ + ψ = atan2 (−r12 , r22 ) ,

(2.57) (2.58)

whereas, if r31 = −1 a suitable solution is θ = π/2 φ − ψ = atan2(r23 , r22 ).

(2.59) (2.60)

Once again, a closed solution can be found by setting the value of one of the two angles a priori, e.g. ψ = 0.

2.1.3.3

Axis/Angle Representation

An arbitrary rotation matrix R ∈ S O(3) can always be represented by one single rotation around an appropriate unitary axis a ∈ R3 expressed in ox0 ,y0 ,z0 :

28

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Fig. 2.9 Axis/angle representation

z0 az θ β a

ay y0 ax

α

x0

⎡

⎤ ax a = ⎣ ay ⎦ , az

(2.61)

by an angle θ , as shown in Fig. 2.9. The properties of the rotations with respect to a fixed frame can be exploited to compute the total rotation of θ degrees around an arbitrary unit vector. This rotation matrix can be denoted as Ra,θ . According to Fig. 2.9, Ra,θ can be computed by carrying out five rotations around the main axes of ox0 ,y0 ,z0 . First rotate a around z 0 by −α degrees, followed by a rotation by −β degrees around y0 . These two rotations align a with the z 0 axis, where a regular basic rotation of θ degrees can be performed. The last step is to undo the first two rotations. The desired rotation matrix can therefore be computed as Ra,θ = Rz,α R y,β Rz,θ R y,−β Rz,−α . By taking into account that a is unitary, one can conclude that ay sin α =  2 ax + a 2y ax cos α =  2 ax + a 2y  sin β = ax2 + a 2y cos β = az .

(2.62)

2.1 Rigid Motions and Homogeneous Transformations

29

The final result is given by ⎡

Ra,θ

⎤ ax2 vθ + cθ ax a y vθ − az sθ ax az vθ + a y sθ a 2y vθ + cθ a y az vθ − ax sθ ⎦ , = ⎣ ax a y vθ + az sθ ax a y vθ − a y sθ a y az vθ + ax sθ az2 vθ + cθ

(2.63)

where cθ = cos(θ ), sθ = sin(θ ) and vθ = vers(θ ) = 1 − cθ . On the other hand, given a particular rotation matrix R, the challenge consists in finding both a and θ such that R = Ra,θ

(2.64)

holds. Let’s denote the elements R as ri j for i, j = 1, 2, 3 as before. Then it is possible to compute θ = cos−1



Tr(R) − 1 2



= cos−1



r11 + r22 + r33 − 1 2

 (2.65)

and ⎤ ⎡ r32 − r23 1 ⎣ r13 − r31 ⎦ , a= 2 sin(θ ) r − r 21

(2.66)

12

where Tr(·) denotes the trace operator. Note that the representation is not unique since the very same result can be gotten by using −a as axis and performing −θ degrees of rotation, i.e. Ra,θ = R−a,−θ .

(2.67)

Furthermore, if θ = 0◦ then R becomes the identity and any vector a can be considered as the axis of rotation.

2.1.4 Unit Quaternion The disadvantages of the axis/angle representation can be compensated by means of the four parameters representation known as the unit quaternion, denoted as Q = {η, }, where   θ η = cos 2   θ a,  = sin 2

(2.68) (2.69)

30

2 Position, Orientation and Velocity of Rigid Robot Manipulators

where θ and a are the same values as those employed in the axis/angle representation in Fig. 2.9. η ∈ R is known as the scalar part of the quaternion, while T   = x y z ∈ R3 is the vectorial part. These quantities are related by η2 + x2 + y2 + z2 = η2 +  T  = 1.

(2.70)

Note that unlike the axis/angle representation, a rotation of −θ around −a delivers the very same quaternion. It is then possible to compute      ⎤ ⎡  2 2 η + x2 −1 2  x y − η 2  x z + η y  z  ⎦. R(η, ) = ⎣ 2  x y + η z  2 η2 + y2 −1 2  y z − η  x 2 2 2 x z − η y 2 y z + η x 2 η + z − 1

(2.71)

On the contrary, given an arbitrary rotation matrix R ∈ S O(3) with elements ri j for i, j = 1, 2, 3, it holds 1 r11 + r22 + r33 + 1 2⎡ ⎤ √ sign(r32 − r23 )√ r11 − r22 − r33 + 1 1⎣ sign(r13 − r31 )√−r11 + r22 − r33 + 1 ⎦ , = 2 sign(r21 − r12 ) −r11 − r22 + r33 + 1

η=

(2.72) (2.73)

where sign(x) = 1 for x ≥ 0 and sign(x) = −1 for x < 0. Note that in (2.72) and (2.73) it is assumed that η ≥ 0, meaning that θ ∈ [−π π ]. The unit quaternion gotten using R−1 = RT is denoted by Q−1 and it is given by Q−1 = {η, −}.

(2.74)

Some other useful relationships are 1 η˙ = −  T ω 2 1 ˙ = E(η, )ω, 2

(2.75) (2.76)

where ω is the angular velocity ω = θ˙ a, and E(η, ) = ηI − S() ⎡ ⎤ 0 − z y S() = ⎣ z 0 − x ⎦ . − y x 0

(2.77) (2.78)

2.1 Rigid Motions and Homogeneous Transformations

31

Fig. 2.10 Parallel translation of a coordinate frame

z1 1

0

O1

p

z0 0

O0

p y1

x1 d1

y0

x0

2.1.5 Homogeneous Transformations Till now it has been shown how a vector can be represented in more than one coordinate frame by means of rotation matrices as long as the origins of all coordinate frames are coincident. Consider now Fig. 2.10. Since the coordinate frames are parallel, the unit vectors i0 , j 0 , k0 are necessarily parallel to i1 , j 1 , k1 , respectively. The vector 0 d 1 ∈ R3 goes from the origin o0 of ox0 ,y0 ,z0 to the origin o1 of ox1 ,y1 ,z1 , expressed in ox0 ,y0 ,z0 . This way, any point p represented as 0 p and 1 p in the different coordinate frames as before can be related as a regular sum of vectors, i.e. 0

p = 1p + 0d1.

(2.79)

The most general situation arises when the coordinate frame ox1 ,y1 ,z1 is not only translated but also rotated with respect to ox0 ,y0 ,z0 , as shown in Fig. 2.11. For that case the relationship (2.79) makes no sense because 1 p is still expressed in ox1 ,y1 ,z1 , while 0 p and 0 d 1 are expressed in ox0 ,y0 ,z0 . However, by comparing Fig. 2.10 with Fig. 2.11 it can be seen that in the very end one has a regular sum of vectors, which makes only sense if and only if all vectors are expressed in the same coordinate frame. It turns out that expressing 1 p in ox0 ,y0 ,z0 is trivial if the rotation matrix 0 R1 is known, so that instead of (2.79) one can employ 0

p = 0 R1 1 p + 0 d 1 .

(2.80)

The transformation given by 0 d 1 ∈ R3 and 0 R1 ∈ S O(3) represents a rigid motion. The set of all rigid motions is known as the Special Euclidean Group S E(3). If an additional third coordinate frame ox2 ,y2 ,z2 is added so that there is a further rotation and translation with respect to ox1 ,y1 ,z1 , then the following relationship is also valid 1

p = 1 R2 2 p + 1 d 2 .

(2.81)

32

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Fig. 2.11 Sum of vectors for two coordinate frames

x1 1 0

p

O1

z0

z1 0

O0

p y1

d1

y0

x0

To express 2 p in the base frame, substitute 1 p from (2.81) in (2.80) to get 0

p = 0 R1

1

 R2 2 p + 1 d 2 + 0 d 1

= 0 R1 1 R2 2 p + 0 R1 1 d 2 + 0 d 1 .

(2.82)

But, on the other hand, since the relationship between 0 p and 2 p is a rigid motion, then it can be described by 0

p = 0 R2 2 p + 0 d 2 .

(2.83)

By comparing (2.82) with (2.83), one directly gets 0

R2 = 0 R1 1 R2

(2.84)

d 2 = 0 d 1 + 0 R1 1 d 2 .

(2.85)

0

Equation (2.84) shows that, as before, the total rotation can be obtained by multiplying R1 by 1 R2 , while Eq. (2.85) shows that the position vector from o0 to o2 is the sum of the vectors from o0 to o1 , i.e. 0 d 1 , and then the vector from o1 to o2 but expressed in ox0 ,y0 ,z0 , i.e. 0 R1 1 d 2 as it had been deducted before. This can also be clearly appreciated in Fig. 2.12. Quite naturally it would be desirable to have only expressions of the form (2.84) rather than of the form (2.85), i.e. products of matrices resulting in another matrix of the same kind (a group structure) instead of a sum of them. This goal can be accomplished by putting all the information of rotation and translation in a single transformation matrix H ∈ R4×4 of the form   R d , (2.86) H= T 0 1

0

known as homogeneous transformation. Since it is assumed that R ∈ S O(3), then it is rather straightforward to compute:

2.1 Rigid Motions and Homogeneous Transformations

33

Fig. 2.12 Sum of vectors for three coordinate frames

z2 2

y2

x2

p

1

O2 1 0

d2

p

O1

z0 0

O0

p x1

z1

y1

d1

y0

x0

H −1 =



 RT −RT d . 0T 1

(2.87)

This way, Eqs. (2.84) and (2.85) can be written in a more compact form as 0

H2 = 0H11H2 0 0 1 1  R1 d 1 R2 d 2 = 0T 1 0T 1  0 0  0 1 0 1 R1 R2 R1 d 2 + 0 d 1 R2 d 2 = , = 0T 1 0T 1

(2.88)

 T where 0T = 0 0 0 . However, the relationship between 0 p and 1 p given by 0

p = 0 R1 1 p + 0 d 1 ,

cannot be carried out directly because 0 p, 1 p ∈ R3 . A simple solution consists in defining the homogeneous representation of these vectors, i.e. 0

p¯ =

0  p , 1

1

p¯ =

1  p . 1

(2.89)

and (2.90)

It is also said that 0 p¯ and 1 p¯ are expressed in homogeneous coordinates. It is easy to see that the transformation (2.80) is equivalent to

34

2 Position, Orientation and Velocity of Rigid Robot Manipulators 0

p¯ = 0 H 1 1 p¯ .

(2.91)

The set of all 4 × 4 matrices of the form (2.86) is denoted by E(3) (Euclidean group of order 3). The basic homogeneous transformations are ⎡

T x,a

1 ⎢0 =⎢ ⎣0 0

0 1 0 0

for translations, and ⎡ 1 0 0 ⎢0 cα −sα Rx,α = ⎢ ⎣0 sα cα 0 0 0

0 0 1 0

⎡ ⎤ a 1 ⎢0 0⎥ ⎥ , T y,b = ⎢ ⎣0 0⎦ 1 0

0 1 0 0

⎡ ⎤ 0 cφ ⎢ 0 0⎥ ⎥ , R y,φ = ⎢ ⎣−sφ 0⎦ 1 0

⎡ ⎤ 00 1 ⎢0 0 b⎥ ⎥ , T z,c = ⎢ ⎣0 1 0⎦ 01 0

0 1 0 0

sφ 0 cφ 0

0 1 0 0

0 0 1 0

⎡ ⎤ 0 cθ ⎢ sθ 0⎥ ⎥ , Rz,θ = ⎢ ⎣0 0⎦ 1 0

⎤ 0 0⎥ ⎥, c⎦ 1

−sθ cθ 0 0

0 0 1 0

(2.92)

⎤ 0 0⎥ ⎥ ,(2.93) 0⎦ 1

for rotations, regarding the axes x, y and z, respectively. It is customary to set the notation   nsad H= , (2.94) 0001 where in view of the definition of H, n, s and a represent the directions of the axes x1 , y1 and z 1 with respect to ox0 ,y0 ,z0 , while d represents the position of the origin of o1 expressed in the same coordinate frame. Most important is that the same rules of composition of rotations matrices are valid for homogeneous transformations, both with respect to current or fixed frames.

2.1.6 Skew Symmetric Matrices A square matrix S is said to be skew symmetric if and only if ST + S = O.

(2.95)

The set of all 3 × 3 skew symmetric matrices is denoted by SS(3). If S ∈ SS(3) and it has si j , i, j = 1, 2, 3 elements, then (2.95) is equivalent to nine equations of the form si j + s ji = 0.

(2.96)

From this relationship it is obvious that sii = 0, while the rest of the elements of S must satisfy si j = −s ji (for i = j). This means that S has only three free elements.

2.1 Rigid Motions and Homogeneous Transformations

35

T  Let’s say that those three parameters are put together in a vector a = ax a y az to define a particular skew symmetric matrix of the form ⎡

⎤ 0 −az a y S(a) = aˆ = (ˆa) = ⎣ az 0 −ax ⎦ . −a y ax 0

(2.97)

S(a) can be seen as a linear operator since given any two vectors a, b ∈ R3 , and any two scalars α, β ∈ R it holds S(αa ± βb) = αS(a) ± βS(b).

(2.98)

But perhaps more important is that it is always true S(a)b = a × b,

(2.99)

where a × b is the regular vector cross product. Consider now a R ∈ S O(3) and recall that this kind of matrices can represent two vectors a and b in other coordinate frames without changing the vectors themselves. Therefore, a cross product can be carried out before or after changing the representation without affecting the end results, i.e. the following holds always R(a × b) = Ra × Rb.

(2.100)

This property can be proved by direct computation, but care should be taken since this relationship is not true in general but only for orthogonal matrices. Another interesting property for any R ∈ S O(3) and any a, b ∈ R3 , is that after (2.99) and (2.100) one has   RS(a)RT b = R a × RT b   = (Ra) × RRT b = (Ra) × b = S (Ra) b.

(2.101)

Since b is completely arbitrary one must have as well RS(a)RT = S (Ra) .

(2.102)

Now, suppose that the rotation matrix R is a function of the single variable θ , i.e. R = R(θ ) ∈ S O(3). Since R is orthogonal for all θ it follows R(θ )RT (θ ) = I.

(2.103)

36

2 Position, Orientation and Velocity of Rigid Robot Manipulators

By taking the derivative with respect to θ one obtains dR T dRT R (θ ) + R(θ ) = O. dθ dθ

(2.104)

If it is defined S=

dR T R (θ ), dθ

(2.105)

then one has  ST =

dR T R (θ ) dθ

T = R(θ )

dRT . dθ

(2.106)

Consequently, Eq. (2.104) can be written as S + ST = O.

(2.107)

In other words, the matrix S defined by (2.105) is skew-symmetric. By multiplying both sides of (2.105) by R and recalling that R is orthogonal, it is dR = SR(θ ). dθ

(2.108)

Equation (2.108) is very important, since it says that computing the rotation matrix derivative equals to multiply the rotation matrix R by a skew-symmetric matrix S. A common case occurs when R is a basic rotation or a composition of basic rotations. Example Consider R = R y,θ , the basic rotation matrix ⎡

R y,θ

⎤ cos(θ ) 0 sin(θ ) 0 1 0 ⎦. =⎣ − sin(θ ) 0 cos(θ )

By direct computation it is straightforward to show that

2.1 Rigid Motions and Homogeneous Transformations

37

⎡ ⎤⎡ ⎤ − sin(θ ) 0 cos(θ ) cos(θ ) 0 − sin(θ ) dR T ⎦⎣ 0 1 ⎦ 0 0 0 0 S= R (θ ) = ⎣ dθ − cos(θ ) 0 − sin(θ ) sin(θ ) 0 cos(θ ) ⎡ ⎤ 0 01 = ⎣ 0 0 0 ⎦ = S(j). (2.109) −1 0 0 Therefore dR y,θ = S(j)R y,θ . dθ

(2.110)

In a similar manner, it can be shown that dRx,θ dRz,θ = S(i)Rx,θ , and = S(k)Rz,θ . dθ dθ

(2.111)

Example Let Ra,θ be a rotation matrix of an angle θ over the axis vector a given in (2.66): ⎤ ⎡ r32 − r23 1 ⎣ r13 − r31 ⎦ . a= 2 sin(θ ) r − r 21

12

It can be shown that dRa,θ = S(a)Ra,θ . dθ

(2.112)

2.1.7 Angular Velocity and Acceleration In the last section, the relationship between the position and orientation of several coordinate systems was established by introducing homogeneous transformations. In this section, it will be discussed how to represent angular velocities and accelerations in the same context. Suppose that a rotation matrix R is time-dependent, so R = R(t) ∈ S O(3) for all t ∈ R. By a similar argument as the employed to compute the derivative of R with respect to θ , it can be computed ˙ R(t) = S(t)R(t),

(2.113)

38

2 Position, Orientation and Velocity of Rigid Robot Manipulators

where S(t) is a skew-symmetric matrix. Moreover, S(t) can be represented as S(ω(t)), for a unique vector ω(t). Such vector ω(t) is known as the angular velocity of the rotated frame with respect to a frame fixed in t. Example ˙ Suppose that R(t) = R y,θ(t) . Then R(t) = as

dR(t) dt

can be gotten by using the chain rule

dR dθ ˙ = θ˙ S(j)R(t) = S(ω(t))R(t), R(t) = dθ dt

(2.114)

where ω = θ˙ j is the angular velocity over the y axis.  Consider a position vector 1 p of a point fixed in the frame ox1 ,y1 ,z1 , which rotates with respect to the fixed frame ox0 ,y0 ,z0 . The coordinates of 1 p with respect ox0 ,y0 ,z0 are given by 0

p = R(t)1 p.

(2.115)

The corresponding linear velocity 0 p˙ is given by 0

p˙ = S(ω)R(t)1 p = S(ω)0 p = ω × 0 p,

(2.116)

which is the well known expression of the linear velocity as the cross product of two vectors. Now, suppose a more general motion of the frame ox1 ,y1 ,z1 with respect to the frame ox0 ,y0 ,z0 represented by the time-dependent homogeneous transformation ⎡0 0

H 1 (t) = ⎣

R1 (t) 0 d 1 (t) 0T

⎤ ⎦.

(2.117)

1

For the sake of simplicity, time dependency and the indexes 0, 1 will be omitted, i.e. 0

p = R1 p + d.

(2.118)

By taking the time derivative of the previous relation and taking into account that 1 p is constant 0

p˙ = R˙ 1 p + d˙

= S(ω)R1 p + d˙ = ω × r + v,

(2.119)

2.1 Rigid Motions and Homogeneous Transformations

39

where r = R1 p is the vector from the origin o1 to p, expressed in the frame ox0 ,y0 ,z0 , and v is the velocity of the origin o1 with respect to ox0 ,y0 ,z0 . If the vector 1 p is not constant with respect to the frame ox1 ,y1 ,z1 , then one must add the term R1 p˙ , which is the velocity of 1 p expressed in the frame ox0 ,y0 ,z0 . An expression to compute the relative acceleration of two coordinate frames can be found as follows. First, notice that: d da db a×b= ×b+a× . dt dt dt

(2.120)

˙ 1 p + R1 p˙ = ω × (R1 p) + R1 p˙ . p˙ − d˙ = R

(2.121)

and rewrite (2.119) as 0

To compute the second derivative consider that R˙ = S(ω)R to get 0

p¨ − d¨ = ω˙ × ( R1 p ) + ω × ( R˙ 1 p ) + ω × (R1 p˙ ) + R˙ 1 p˙ + R1 p¨



 ω×r

r

˙ R

 = ω˙ × r + ω × (ω × r) + ω × (R1 p˙ ) + (ω × R)1 p˙ + R1 p¨ = ω˙ × r + ω × (ω × r) + 2ω × (R1 p˙ ) + R1 p¨ . (2.122) Therefore, it is obtained 0

p¨ = ω˙ × r + ω × (ω × r) + 2ω × (R1 p˙ ) + a,

(2.123)

where the term ω˙ × r is known as the transverse acceleration and ω × (ω × r) is known as the centripetal acceleration of the particle, which points always to the rotation axis and is orthogonal to it. The term 2ω × (R1 p˙ ) represents the Coriolis acceleration, while a = R1 p¨ + d¨ is the linear acceleration. Very often, it is necessary to compute the angular velocity due to the rotation of several frames. In what follows, the expressions of the composition of angular velocities of two frames ox1 ,y1 ,z1 and ox2 ,y2 ,z2 with respect to a fixed frame ox0 ,y0 ,z0 will be computed. Given a point p represented in different frames as 0 p, 1 p, and 2 p, the following relations are satisfied: 0

p = 0 R1 1 p + 0 d 1

(2.124)

1

p = R2 p + d 2 p = 0 R2 2 p + 0 d 2 ,

(2.125) (2.126)

0

1

2

1

40

2 Position, Orientation and Velocity of Rigid Robot Manipulators

where R2 = 0 R1 1 R2 ,

(2.127)

d 2 = 0 d 1 + 0 R1 1 d 2 .

(2.128)

0

and 0

As seen before, all the previous relations are time-dependent functions. On the one hand, by taking the time derivative of both sides of (2.127), one obtains 0

R˙ 2 = 0 R˙ 1 1 R2 + 0 R1 1 R˙ 2 .

(2.129)

On the other hand, 0 R˙ 2 must satisfy 0

R˙ 2 = S(0 ω2 )0 R2 .

(2.130)

Now, while the first term in the right hand side of (2.129) is given by 0

R˙ 1 1 R2 = S(0 ω1 )0 R1 1 R2 = S(0 ω1 )0 R2 ,

(2.131)

the second term in the right hand side of (2.129) must be analyzed more thoroughly. By employing (2.102) RS(a)RT = S (Ra) , the term 0 R1 1 R˙ 2 can be written as 0

R1 1 R˙ 2 = 0 R1 S(1 ω2 )1 R2 = 0 R1 S(1 ω2 )0 RT1 0 R1 1 R2 = S(0 R1 1 ω2 )0 R2 .

(2.132)

By combining the above expressions it is obtained S(0 ω2 )0 R2 = S(0 ω1 )0 R2 + S(0 R1 1 ω2 )0 R2 .

(2.133)

Due to the linearity of the operator S(·), it is concluded that 0

ω2 = 0 ω1 + 0 R1 1 ω2 .

(2.134)

In other words, angular velocities can be summed as long as they are expressed in the same coordinate frame, in this case ox0 ,y0 ,z0 . Furthermore, the above representation can be extended to any number of frames, e.g. if

2.1 Rigid Motions and Homogeneous Transformations

41

Rn = 0 R1 1 R2 · · · n−1 Rn ,

(2.135)

R˙ n = S(0 ωn )0 Rn ,

(2.136)

ωn = 0 ω1 + 0 R1 1 ω2 + 0 R2 2 ω3 + · · · + 0 Rn−1 n−1 ωn .

(2.137)

0

then 0

where 0

2.2 Direct Kinematics In this section, the direct kinematics for rigid robots will be developed. The direct kinematics problem can be formulated as follows: given the values for the joint variables, compute the position and the orientation of the end-effector. The joint variables are the angles for the case of revolute joints and the linear displacements for the case of prismatic joints.

2.2.1 Kinematic Chains To analyze the kinematics, it could be considered that a robot is composed of rigid elements connected by joints. Under the assumption that each joint has one degree of freedom, the effect of each of them can be described by a single real scalar: the rotation angle for revolute joints and the linear displacement for prismatic joints. The objective of the direct kinematics is to determine the total effect of all the manipulator joints in the position and orientation of the end-effector. But first, some conventions must be adopted to make this analysis in a more systematic way. Suppose that a robot is composed of n + 1 rigid elements, numbered from 0 to n, starting at the robot’s base, which is considered to be the 0 rigid element, usually fixed at the floor or at a table. Suppose it has n joints, numbered from 1 to n. The ith joint is at the point where the elements i − 1 and i are connected. The corresponding ith joint variable is denoted as qi , which can be an angle or a displacement, depending on the type of joint. Fix a coordinate frame to each rigid element, in particular fix the inertial system ox0 ,y0 ,z0 at the robot’s base. Therefore, the n + 1 frames are defined in such a way that the ith frame is fixed to the ith element. This means that for every motion of the robot, the coordinates of any point of the ith element are constant with respect to the ith frame. This is shown in Fig. 2.13. Now, suppose that i−1 Ai is the homogeneous transformation matrix which drives the coordinates of a point on the ith frame to the (i − 1)th frame. The matrix i−1 Ai is not constant, but it is a function of one single variable qi , i.e.

42

2 Position, Orientation and Velocity of Rigid Robot Manipulators

y3

y1 d2 link 2

link 3

x1

z3 z1

joint 2

y3 z2

link 1 z0

x2

joint 3

θ3

θ1

joint 1

x3

y0 x0

base (link 0)

Fig. 2.13 Definition of frame coordinates on the different elements of a robot manipulator

i−1

Ai = i−1 Ai (qi ).

(2.138)

By convention, the homogeneous matrix which transforms the coordinates of a point on the jth frame to the ith frame is called transformation matrix and it is commonly denoted by i T j , which after the discussion of Sect. 2.1.5 is given by T j = i Ai+1 i+1 Ai+2 i+2 Ai+3 · · · j−2 A j−1 j−1 A j if i < j Tj = I if i = j  j −1 i T j = Ti if i > j i i

From the above definitions, it follows that the position of any point of the endeffector is constant when expressed in frame n as mentioned before, independently of the robot configuration. Denote the position and orientation of the end-effector with respect to the base system as a vector 0 d n ∈ R3 and a rotation matrix 0 Rn ∈ S O(3), respectively, and define the homogeneous matrix 0

Tn =

 Rn 0 d n , 0T 1

0

(2.139)

so that the position and orientation of the end-effector with respect to the base frame are given by 0

T n = 0 A1 (q1 ) · · · n−1 An (qn ).

(2.140)

2.2 Direct Kinematics

43

Each homogeneous transformation i−1 Ai (qi ) has the form i−1

Ai =

 i−1

Ri

 di , 1

i−1

0T

(2.141)

from which one has i

T j = Ai+1 i

i+1

Ai+2 · · ·

j−1

Aj =

i

 Rj id j . 0T 1

(2.142)

The rotation matrix i R j expresses the orientation of the jth frame with respect to the ith frame and it is given by i

R j = i Ri+1 · · · j−1 R j .

(2.143)

The vector i d j can be computed recursively by i

d j = i d j−1 + i R j−1 j−1 d j .

(2.144)

2.2.2 The Denavit-Hartenberg Representation 2.2.2.1

Denavit-Hartenberg Algorithm

Although the n + 1 coordinate frames could in principle be chosen completely arbitrarily and the different homogeneous matrices could be computed just by inspection, it appears to be more useful to have some convention to choose all of them if possible by following simple rules. This matter is discussed in this section. In fact, perhaps the most widely employed convention in robotics is the one proposed by Denavit and Hartenberg (D-H). So as to simplify the discussion, consider first Algorithm 2.1 to choose the location of the n + 1 coordinate frames attached to the robot links. Algorithm 2.1 Given a robot manipulator with n joints, the following algorithm assigns the coordinate frames such that the Denavit-Hartenberg convention is satisfied: 1. Locate and enumerate the joint axes z0 , . . . , zn−1 . Align the zi axis with the axis of motion, rotational or linear, of the (i + 1)th joint. 2. Assign an orthonormal right-handed frame ox0 ,y0 ,z0 to the robot base, with the z0 axis along the axis of motion of joint 1. Locate the origin o0 in a convenient point over z0 . The axes x0 and y0 can then be located conveniently as long as they remain orthogonal to the z0 axis and form a right-handed coordinate frame. 3. Locate the origin oi , for i = 1, . . . , n − 1, on the intersection of the zi axis and the common normal to zi and zi−1 . If zi intersects zi−1 , locate the origin oi on

44

2 Position, Orientation and Velocity of Rigid Robot Manipulators

the intersection. If zi and zi−1 are parallel, locate the origin oi on any convenient point over zi . 4. Set the axis xi , for i = 1, . . . , n − 1, along the common normal to zi and zi−1 passing through the origin oi , or along the normal direction to the plane zi –zi−1 if zi intersects zi−1 . 5. Set the axis yi , for i = 1, . . . , n − 1, to form a right-handed coordinate frame. 6. Establish the end-effector coordinate frame oxn ,yn ,zn . Locate the origin on in the most important point of the end-effector, e.g. center of a gripper, tip of the tool, etc. Set the axis zn parallel to zn−1 passing through on . Set yn conveniently, e.g. on the direction in which the gripper closes. Finally, set xn to form a right-handed coordinate frame. Algorithm 2.1 provides an easy way to locate the different coordinate frames necessary to compute the complete direct kinematics of a robot manipulator. It remains but to obtain the corresponding homogeneous transformation matrices for each couple of links i − 1 and i, for i = 1, . . . , n. While this could be done by inspection as mentioned before, it turns out that the particular frames locations allow to describe each transformation as the product of four basic transformations. This can better be understood by analyzing Fig. 2.14. The two coordinate frames are not really independent since the xi axis is perpendicular both to zi−1 and zi . This does not mean that xi−1 and xi point in the same direction but they differ by an angle θi which represents a rotation around zi−1 . Once this rotation is carried out and xi−1 and xi point in the same direction, then to align them a translation by di along zi−1 has to be performed, while to make the origins coincide a new translation by ai along xi has to be done. Finally, to align zi−1 and zi , a new rotation around xi by an angle αi has to be carried out. This leads to the following homogeneous transformation:

αi

joint i

zi Oi ai di

1 link i −

link i

zi−1

yi−1 Oi−1 xi−1

θi

Fig. 2.14 The four parameters of the Denavit-Hartenberg convention

xi

joint i + 1

2.2 Direct Kinematics i−1

45

Ai = Rz,θi T z,di T x,ai Rx,αi ⎤⎡ ⎡ ⎤⎡ 1 cθi −sθi 0 0 100 0 ⎢ sθi cθi 0 0 ⎥ ⎢ 0 1 0 0 ⎥ ⎢ 0 ⎥⎢ ⎥⎢ =⎢ ⎣ 0 0 1 0 ⎦ ⎣ 0 0 1 di ⎦ ⎣ 0 000 1 0 0 01 0 ⎡ ⎤⎡ ⎤⎡ cθi −sθi 0 0 1 100 0 ⎢ sθi cθi 0 0 ⎥ ⎢ 0 1 0 0 ⎥ ⎢ 0 ⎥⎢ ⎥⎢ =⎢ ⎣ 0 0 1 0 ⎦ ⎣ 0 0 1 di ⎦ ⎣ 0 000 1 0 0 0 01 ⎡ ⎤ cθi −sθi cαi sθi sαi ai cθi ⎢ sθi cθi cαi −cθi sαi ai sθi ⎥ ⎥, =⎢ ⎣ 0 sαi cαi di ⎦ 0 0 0 1

⎤ ⎤⎡ 0 ai 1 0 0 0 ⎥ ⎢ 0 0⎥ ⎥ ⎢ 0 cαi −sαi 0 ⎥ ⎦ ⎣ 1 0 0 sαi cαi 0 ⎦ 0 0 0 1 0 1 ⎤⎡ ⎤ 0 0 0 1 0 0 ai ⎢ ⎥ cαi −sαi 0 ⎥ ⎥⎢0 1 0 0 ⎥ ⎦ ⎣ sαi cαi 0 001 0⎦ 0 0 1 000 1 0 1 0 0

(2.145)

where the four quantities θi , di , ai , and αi are parameters of the ith element and of the ith joint. These quantities are known as angle, distance, offset distance, and offset angle, respectively. Since the matrix i−1 Ai is a function of one single variable, necessarily three of these parameters are constant. In fact, θi is variable for a revolute joint and di is variable for a prismatic joint, whereas the remaining three are constant. For the sake of easiness, the following algorithm is introduced. Algorithm 2.2 Computation of the homogeneous transformations using the D-H convention 1. Create a table of the parameters ai , di , αi and θi ai is the distance from the origin oi to the intersection of xi with zi−1 , along the axis xi . When measuring the distance from the intersection of zi−1 with xi to the origin oi , the sign of ai is the opposite of the direction of xi . di is the distance from the origin oi−1 to the intersection of the axes zi−1 and xi , along the axis zi−1 . θi is the angle from xi−1 to xi measured with respect to the axis zi−1 , where the sense of rotation is obtained by employing the right hand rule. αi is the angle from zi−1 to zi , with respect to the axis xi , where the sense of rotation is obtained by employing the right hand rule. Figure 2.15 shows how to measure θi and αi . 2. Compose the n homogeneous transformation matrices by substituting the above parameters in the equation: ⎤ cθi −sθi cαi sθi sαi ai cθi ⎢ sθi cθi cαi −cθi sαi ai sθi ⎥ i−1 ⎥ Ai = ⎢ ⎣ 0 sαi cαi di ⎦ 0 0 0 1 ⎡

(2.146)

46

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Fig. 2.15 Rotation angles θi and αi

zi−1

zi

αi zi−1

xi−1 θi

xi xi

3. Compute the transformation matrix 0 T n = 0 A1 · · · n−1 An , which expresses the position and the orientation of the end-effector frame with respect to the base frame. It is worth to point out that, contrarily to what is expected, the D-H representation does not require 6 parameters (3 for position and 3 for orientation) to relate each pair of coordinate frames, but only 4. This is possible due to the constraints imposed on the frames assignment.

2.2.2.2

Examples

Example

Consider the two degrees of freedom planar robot shown in Fig. 2.16 Let the axes z0 and z1 be normal to the page. The coordinate system ox0 ,y0 ,z0 is located as shown in the figure, i.e. the origin o0 is established at the intersection Fig. 2.16 Two degrees of freedom planar robot

y0

y2

y1

a2

x2

q2 x1

q1 a1

x0

2.2 Direct Kinematics

47

Table 2.1 D-H parameters of the two degrees of freedom planar manipulator Joint ai αi di θi 1 2

0◦ 0◦

a1 a2

0 0

θ1 (t) θ2 (t)

of the axis z0 with the page, whereas x0 is arbitrarily chosen and the system is accomplished by locating y0 according to the right hand rule. Once the base system is fixed, the procedure described in the Denavit-Hartenberg algorithm above is followed to establish the frames ox1 ,y1 ,z1 and ox2 ,y2 ,z2 . The obtained parameters are shown in Table 2.1: Now, the matrices 0 A1 and 1 A2 can be computed by taking into account (2.146) and the table parameters as ⎤ c1 −s1 0 a1 c1 ⎢ s1 c1 0 a1 s1 ⎥ 0 ⎥ A1 = ⎢ ⎣ 0 0 1 0 ⎦ and 0 0 0 1 ⎡

⎤ c2 −s2 0 a2 c2 ⎢ s2 c2 0 a2 s2 ⎥ 1 ⎥ A2 = ⎢ ⎣ 0 0 1 0 ⎦. 0 0 0 1 ⎡

The matrix 0 T 1 is simply given by 0 T 1 = 0 A1 , whereas 0 T 2 is computed as ⎤⎡ ⎤ c2 −s2 0 a2 c2 c1 −s1 0 a1 c1 ⎢ s1 c1 0 a1 s1 ⎥ ⎢ s2 c2 0 a2 s2 ⎥ 0 ⎥⎢ ⎥ T 2 = 0 A1 1 A2 = ⎢ ⎣ 0 0 1 0 ⎦⎣ 0 0 1 0 ⎦ 0 0 0 1 0 0 0 1 ⎤ ⎡ c1 c2 − s1 s2 −c1 s2 − s1 c2 0 a2 c1 c2 − a2 s1 s2 + a1 c1 ⎢ s1 c2 + c1 s2 −s1 s2 + c1 c2 0 a2 s1 c2 + a2 c1 s2 + a1 s1 ⎥ ⎥ =⎢ ⎦ ⎣ 0 0 1 0 0 0 0 1 ⎡ ⎤ c12 −s12 0 a2 c12 + a1 c1 ⎢ s12 c12 0 a2 s12 + a1 s1 ⎥ ⎥, =⎢ ⎣ 0 ⎦ 0 1 0 0 0 0 1 ⎡

where s12 = sin(θ1 + θ2 ) and c12 = cos(θ1 + θ2 ). Note that the first two elements on the fourth column of this matrix are the components of the origin o2 with respect to the base frame, i.e. x = a1 c1 + a2 c12 y = a1 s1 + a2 s12 .

48

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Fig. 2.17 Spherical wrist configuration

θ6 z6

z3

θ5

z5 z4

d6

oc

θ4 Table 2.2 D-H parameters for a spherical wrist Joint ai αi 4 5 6

0 0 0

−90◦ 90◦ 0◦

di

θi

0 0 d6

θ4 (t) θ5 (t) θ6 (t)

In other words, (x, y) are the coordinates of the end effector in the base frame. On the other hand, the upper-left part of this matrix, i.e. the rotation part of 0 T 2 , gives the orientation of the frame ox2 ,y2 ,z2 with respect to the base frame. Example

Consider the spherical wrist configuration shown in Fig. 2.17. Since the spherical wrist is attached to the robot’s main body made usually up of three joints, then in the figure the axes are numbered as z3 , z4 and z5 and they intersect each other at the center of the wrist denoted by oc . The corresponding Denavit-Hartenberg parameters are summarized in Table 2.2. It can be easily shown that the joint variables θ4 , θ5 , and θ6 are the Euler angles φ, θ , and ψ for the Z Y Z configuration, respectively, with respect to the frame ox3 y3 z 3 . In order to see this compute the matrices 3 A4 , 4 A5 , and 4 A6 , which according to Table 2.2 are

2.2 Direct Kinematics

49

⎡

⎡

⎤ c4 0 −s4 0 ⎢ s4 0 c4 0 ⎥ 3 ⎥ A4 = ⎢ ⎣ 0 −1 0 0 ⎦ 0 0 0 1

c5 ⎢ s5 4 A5 = ⎢ ⎣0 0

0 s5 0 −c5 1 0 0 0

⎤ 0 0⎥ ⎥ 0⎦ 1

⎤ c6 −s6 0 0 ⎢ s6 c6 0 0 ⎥ 5 ⎥ A6 = ⎢ ⎣ 0 0 1 d6 ⎦ . 0 0 0 1 ⎡

By performing the matrix product, it is 3

T 6 = A4 A5 A6 = 3

4

5

3 ⎡

R6 3 d 6 0T 1



c4 c5 c6 − s4 s6 −c4 c5 s6 − s4 c6 ⎢ s4 c5 c6 + c4 s6 −s4 c5 s6 + c4 c6 =⎢ ⎣ −s5 c6 s5 s6 0 0

⎤ c4 s5 c4 s5 d6 s4 s5 s4 s5 d6 ⎥ ⎥. c5 c5 d6 ⎦ 0 1

By comparing the rotation matrix 3 R6 of 3 T 6 with the Euler angles composition (2.44) ⎡

Rz,φ R y,θ Rz,ψ

⎤ cφ cθ cψ − sφ sψ −cφ cθ sψ − sφ cψ cφ sθ = ⎣ sφ cθ cψ + cφ sψ −sφ cθ sψ + cφ cψ sφ sθ ⎦ , −sθ cψ sθ sψ cθ

it can be seen that θ4 , θ5 , and θ6 are exactly the Euler angles φ, θ , and ψ with respect to the coordinate frame ox3 y3 z 3 . Example

Consider the robot manipulator with two revolute joints shown in Fig. 2.18. This robot has the same kinematic structure of the Furuta pendulum when the parameter

1 is set to zero. The coordinate frames which are also shown in the figure were

Fig. 2.18 Serial manipulator with two revolute joints

x2 y1 x1

z2

y2 1 b1

2 z1

z0

b2

y0 x0 θ1

θ2

50

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Table 2.3 D-H parameters of the two degrees of robot shown in Fig. 2.18 Joint ai αi di 1 2

90◦

1

2

θ1 (t) θ2 (t)

b1 b2

0◦

θi

assigned according to the D-H convention. Notice that the origin of the coordinate frame x1 y1 z 1 is placed outside of the body of the robot. The D-H parameters of the robot are shown in Table 2.3 and the homogeneous transformation matrices are given by ⎡

c1 ⎢ s1 0 A1 = ⎢ ⎣0 0

0 0 1 0

s1 −c1 0 0

⎤

1 c1

1 s1 ⎥ ⎥, b1 ⎦ 1

⎡

c2 ⎢ s2 1 A2 = ⎢ ⎣0 0

−s2 c2 0 0

0 0 1 0

⎤

2 c2

2 s2 ⎥ ⎥. b2 ⎦ 1

Finally, the orientation and end-effector position of the robot are given by ⎡

⎤ c1 c2 −c1 s2 s1 c1 ( 1 + 2 c2 ) + b2 s1 0 T 2 = 0 A1 1 A2 = ⎣ s1 s2 −s1 s2 −c1 s1 ( 1 + 2 c2 ) − b2 c1 ⎦ . s2 c2 0 b1 + 2 s2

2.3 Inverse Kinematics 2.3.1 Introduction It has already been shown how to determine the pose of the end-effector in terms of the joint variables. In this section, the inverse problem will be studied, i.e. how to find the joint variables given the pose of the end-effector. This problem is known as inverse kinematics, which is, in general, more difficult to solve than the direct kinematics problem. The inverse kinematics problem can be established as follows: given a 4 × 4 homogeneous transformation by 0

Hd =

0

Rd od 0T 1

 ∈ S E(3),

(2.147)

with 0 Rd ∈ S O(3), find if exists the solution (or multiple solutions) of the equation 0

T n (q1 , . . . , qn ) = 0 H d ,

(2.148)

2.3 Inverse Kinematics

51

where 0

T n (q1 , . . . , qn ) = 0 A1 · · · n−1 An .

(2.149)

Equation (2.148) gives 12 nonlinear equations with n variables to solve, which can be written as ti j (q1 , . . . , qn ) = h i j , i = 1, 2, 3,

j = 1, 2, 3, 4,

(2.150)

where ti j and h i j are the 12 nontrivial elements of 0 T n and 0 H d , respectively. The solution of these equations in closed form is very difficult and in some cases even impossible. Therefore, more efficient and systematic techniques were developed by exploiting the kinematic structures of each manipulator in particular. While the direct kinematics has always a close solution which can be obtained simply by evaluating directly (2.146) with the information of the table of D-H parameters, the kinematics inverse problem might have a single solution, multiple solutions or no solution at all. Even if this solution is unique, it could be very difficult to obtain given the nonlinear nature of the inverse kinematics equations. Of course the solution can be found numerically, but this has the drawback of high computational costs, which makes it not suitable in a vast number of practical applications. Instead, a closed form solution is preferred, i.e. a solution of the form qk = f k (h 11 , . . . , h 34 ), k = 1, . . . , n.

(2.151)

The closed form of solutions are preferred not only for their low computational cost, but because they allow to choose between different valid solutions if necessary. The existence of solutions of the inverse kinematics problem depends not only on mathematical considerations but also depends on physical limitations. One can assume that at least one solution of (2.151) exists if the given pose for the end-effector lies in the workspace of the particular manipulator, i.e. 0 H d in (2.151) corresponds to a valid configuration in the dexterous workspace of the manipulator.

2.3.2 Kinematic Decoupling Although the inverse kinematics problem is very difficult to solve for robots with many degrees of freedom, when the last three joint axes intersect each other at one point it is possible to decouple the inverse kinematics problem into two simpler cases, known in the literature as inverse position and inverse orientation. In other words, for a manipulator with six or more degrees of freedom with their last three joints forming a spheric wrist, the inverse kinematics can be solved in two easier steps: first to find the position of the wrist axes intersection point and then the orientation of the wrist itself.

52

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Fig. 2.19 Kinematic decoupling

z4 z3

d6 z5

oc z0

o6

y6

z6

x0

For simplicity, assume that the robot has exactly six degrees of freedom and that the last three joint axes intersect at a point oc . Then Eq. (2.148) is equivalent to solve simultaneously 0

R6 (q1 , . . . , q6 ) = 0 Rd

(2.152)

o6 (q1 , . . . , q6 ) = od ,

(2.153)

where od and 0 Rd are the given position and orientation for the end-effector, respectively. The fact that the axes z4 , z5 , and z6 intersect at oc implies that the origins o4 and o5 coincide at this position as well. An important consequence is that the movement of the last three joints does not change at all the position of oc , so that the center of the wrist depends exclusively on the first three joints. By taking into consideration that the origin o6 fixed at the end-effector is simply a translation from oc along z5 by the distance d6 , as shown in Fig. 2.19, it can be concluded that o6 specified in the frame ox0 ,y0 ,z0 is given by o6 = oc + d6 z6 .

(2.154)

Therefore, to achieve o6 = od it is necessary to comply with oc = od − d6 0 Rd k.

(2.155)

 T Note that 0 Rd k = z6 because k = 0 0 1 . As mentioned before, the position of the wrist center oc does not depend on the last three joints, so that the first goal is to determine the values of the first three joints which satisfy (2.155). Recall that the orientation objective is to drive 0 R6 = 0 Rd , i.e. 0

R6 = 0 Rd = 0 R3 3 R6 ,

(2.156)

2.3 Inverse Kinematics

53

from which one can compute 3

R6 =

0

R3

−1 0

Rd = 0 RT3 0 Rd ,

(2.157)

once the first three joints values have been determined by solving (2.155). The importance of the spheric wrist is now evident, since the matrix 3 R6 is a function of the angles θ4 , θ5 , and θ6 with the same composition of the Euler angles ZYZ, as it was shown in the direct kinematics of the wrist example. Note also that the right hand side of (2.157) is a product of quantities known at this point. Therefore, to obtain the solution for the last three joints, it is sufficient to extract the ZYZ Euler angles from the matrix 3 R6 in (2.157).

2.3.3 Inverse Position For the common kinematic configurations, a geometric method will be studied to find the solution to the inverse position problem, i.e. to find the joint variables q1 , q2 , q3 corresponding to the center of the wrist oc given by (2.155). The reason for using a geometric method instead of an analytic one is, on the one hand, the difficulty to obtain such analytic solution and, on the other hand, most of the commercial manipulators are of one of the five basic kinematic configurations: Cartesian, cylindrical, spherical, SCARA, and anthropomorphic, along with a spheric wrist attached to them. In general, the complexity of the inverse kinematic problem increases with the number of D-H parameters different from zero.

2.3.3.1

Anthropomorphic Manipulator

Consider the anthropomorphic configuration shown in Fig. 2.20, where the D-H coordinate frames assignment is already shown. The configuration after the kinematic decoupling is shown in Fig. 2.21. To apply the geometric method for solving the inverse kinematics, it is convenient to redraw the manipulator in a configuration in which all the revolute joints have values different from 0, ±π/2, ±π, . . ., and the prismatic joints have values different from zero. For the present example, such configuration is displayed in Fig. 2.22. After driving the manipulator into this convenient configuration, the components oc , denoted by ocx , ocy , ocz , can be projected on the plane x0 y0 as shown in Fig. 2.23. In this figure, it can be seen that θ1 = β + γ .

(2.158)

The geometric method basically relies on forming triangles with some of their sides or angles known, and solve them. In this example, to find β, consider the right

54

2 Position, Orientation and Velocity of Rigid Robot Manipulators

a1

a2 y1

d6

d4 x4 , x5

x3 z1

d2

o1 x1

x2

z3 o6 z , z 5 6

o4,5

o2,3

z4

z2 d1

x6

y6

z0 y0 o0

x0

Fig. 2.20 Anthropomorphic configuration Fig. 2.21 Anthropomorphic configuration after kinematic decoupling

a1

a2 y1

d4 x3

z1

d2

o1 x1 oc

o2,3 z2 d1

z0 y0 x0

o0 Fig. 2.22 Anthropomorphic manipulator with joint valued different from 0, ±π/2, ±π

oc α

z3 α x2

x3 y1 x1 z1 z0

y0 x0

θ1

θ2

z2

2.3 Inverse Kinematics

55

Fig. 2.23 Projection on the x0 y0

y0

ocy x1 θ1

oc

r

γ β

ocx

x0

d2

triangle with sides ocx and ocy . Thus, β can be computed as β = atan2(ocy , ocx ).

(2.159)

On the other hand, γ can  be found by forming the triangle with a side given by d2 2 + o2 . This angle can be computed as and hypotenuse r = ± ocx cy       2 2 2 + o2 − d 2 γ = atan2 d2 , ± r − d2 = atan2 d2 , ± ocx . cy 2

(2.160)

Gathering all together, the solution for the first joint is      2 + o2 − d 2 θ1 = atan2 ocy , ocx + atan2 d2 , ± ocx . cy 2

(2.161)

Notice that this solution exists only if (ocx , ocy ) = (0, 0), i.e. when the wrist center is not located exactly above the first joint axis. For the manipulator presented in this example, this case is not possible, because of the offset d2 . Nevertheless, there exist many manipulators without this offset, for which such situation is possible. This is known as singular configuration or singularity, and will be addressed in the next section. To find the angles θ2 and θ3 , consider the projection of the robot on the plane x1 y1 , which is shown in Fig. 2.24. First, notice that θ3 = α + π/2. Now, by applying the law of cosines m 2 = (r − a1 )2 + (ocz − d1 )2 = a22 + d42 − 2a2 d4 cos(π − α).

(2.162)

Because cos(π − α) = cos(π ) cos(α) + sin(π ) sin(α) = − cos(α), one gets cos(α) =

m 2 − a22 − d42 = D. 2a2 d4

(2.163)

56

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Fig. 2.24 Projection on the x1 y1 plane

y1 oc

ocz − d1

m α θ3

x3

d4 α

ϕ δ

a2

x2

θ2

x1

r − a1 Fig. 2.25 Spheric manipulator after kinematic decoupling

d3

d4 x3

x2 x1

z2

z3

oc

z1 d1

z0

y0 x0

Thus, the solution for the third joint is given by    θ3 = α + π/2 = atan2 ± 1 − D 2 , D .

(2.164)

From Fig. 2.24 one can see that θ2 = δ − ϕ. The angle δ can be computed as δ = atan2 (ocz − d1 , r − a1 ) .

(2.165)

To get ϕ one can use α and form a right triangle with sides a2 + d4 cos(α) and d4 sin(α). Hence, this angle can be computed as ϕ = atan2 (d4 sin(α), a2 + d4 cos(α)) .

(2.166)

Finally, the solution for the second joint is given by θ2 = atan2 (ocz − d1 , r − a1 ) − atan2 (d4 sin(α), a2 + d4 cos(α)) .

(2.167)

Note that there are four valid solutions for the inverse kinematics, as a result of combining the two solutions given by choosing the signs in (2.161) and in (2.164). In (2.161), the signs + and − correspond to the right arm and left arm solutions, while in (2.164) correspond to the elbow down and elbow up solutions, respectively.

2.3 Inverse Kinematics

57

Fig. 2.26 Spheric manipulator in a convenient configuration for solving the inverse kinematics

d4 x3

oc

d3

z3 o3

x2 β y1 θ 2 z2

β x1

θ1

x0

z1 d1

z0

y0 x0

Fig. 2.27 Projection of the spheric manipulator on the x0 y0 plane

y0

oc

ocy

θ1 ocx

2.3.3.2

x0

Spheric Configuration

Figure 2.25 shows a spheric configuration after kinematic decoupling. Redraw it with all the revolute joint variables different from 0, ±π/2, ±π and prismatic joint variables different from zero, as displayed in Fig. 2.26. From the figure it can be seen that θ2 = β + π/2. Now, to find the solution for θ1 , it is convenient to project the manipulator on the x0 y0 plane, as shown in Fig. 2.27. From the figure, the solution for the first angle is easily found to be θ1 = atan2(ocy , ocx ),

(2.168)

58

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Fig. 2.28 Projection of the spheric manipulator on the x1 y1 plane

y1 x3 β d3

x2

z2 β

θ2 r

d4 z3 ocz − d1 x1

as long as (ocx , ocy ) = (0, 0). The case (ocx , ocy ) = (0, 0) is a singular point and θ1 can take any value. To find the solution for the second and third joints, it is convenient to project the robot on the x1 y1 plane, as shown in Fig. 2.28. 2 2 + ocy , and by the Pythagorean theorem it is In the figure, r is defined by r 2 = ocx 2 2 (d3 + d4 )2 = ocx + ocy + (ocz − d1 )2 ,

(2.169)

from which the solution for the third joint is obtained as d3 =



2 + o2 + (o − d )2 − d . ocx cz 1 4 cy

(2.170)

Finally, the solution for the second joint is    2 + o2 θ2 = β + π/2 = atan2 ocz − d1 , ocx cy + π/2.

(2.171)

2.4 Differential Kinematics In this section, the relationship between joint velocities and end-effector velocities will be developed. Mathematically speaking, the direct kinematics equations define a function that maps the joint variables to the position and orientation of the end-effector in the Cartesian space. The relationship between the velocities is thus naturally given by taking the time derivative of the direct kinematics function. Since it depends only on the joint positions, by using the chain rule the relationship between the joint and Cartesian velocities is given by a matrix, which is called the Jacobian matrix, or simply Jacobian. This matrix is one of the most important and useful quantities in robotics, with a great number of applications from dynamic modeling to manipulability analysis, path planing, force control, visual servoing, etc. The same approach can be used to relate the velocity of any point on the robot and the joint velocities.

2.4 Differential Kinematics

59

In the most general case, the Cartesian linear and angular velocities can be represented by a vector of dimension 6. In this section, first for a manipulator with n joints, the corresponding 6 × n configuration-dependent Jacobian matrix will be developed, which maps the joint velocities to end-effector Cartesian velocities. Second, the same procedure will be employed to compute the Jacobian for any arbitrary point on the robot. Later, the configurations which make the Jacobian to lose rank, known as singularities will be studied. Finally, the relationship between the joint and end-effector accelerations will be discussed.

2.4.1 Analytic Jacobian There are basically two important types of Jacobians that are useful for robotics, the analytic Jacobian and the geometric Jacobian. To derive the first one, let us bundle the Cartesian position and the orientation of the end-effector in the vector x=

0

 dn , φ

(2.172)

where φ ∈ R p is a parametrization of the orientation. Recall that the direct kinematics is expressed as a homogeneous transformation of the form ⎡0 0

T n (q) = ⎣

Rn (q) 0 d n (q) 0

T

⎤ ⎦.

(2.173)

1

Thus, the vector φ is just a parametrization of the rotation matrix 0 Rn (q). For the general case of the 3D Cartesian space, a vector-valued function f (q) : Rn → R3+ p of the end-effector pose in terms of the n joint positions can be constructed, where p is the dimension of the orientation parametrization, e.g. p = 3 for Euler angles and p = 4 for unit quaternions. In the particular case of planar robots, the position and orientation can be described by a vector φ ∈ R3 . Therefore, the direct kinematics can be expressed as x = f (q). (2.174) By taking the time derivative of this equation, one has x˙ =

0

 ∂f (q) d˙ n ˙φ = ∂q q˙ = J A (q)˙q.

(2.175)

The quantity J A (q) = ∂f (q)/∂q ∈ R(3+ p)×n is the analytic Jacobian of the manipulator.

60

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Fig. 2.29 Three degrees of freedom planar robot

y0 q3 φ q2

q1 x0

Example

Consider the 3 degrees of freedom planar manipulator shown in Fig. 2.29. After a standard D-H assignment, it is not difficult to obtain the direct kinematics in the form ⎡ ⎤ c123 −s123 0 a1 c1 + a2 c12 + a3 c123 ⎢ −s123 c123 0 a1 s1 + a2 s12 + a3 s123 ⎥ 0 ⎥. T3 = ⎢ (2.176) ⎣ 0 ⎦ 0 1 0 0 0 0 1 For representing the position, it is sufficient to take the first two components of the end-effector position vector o3 , i.e. 0 d 3 . On the other hand, the angle φ can be employed to represent the orientation, as shown in Fig. 2.29. This angle can be computed as φ = q1 + q2 + q3 and it is simply the angle between the end-effector and the horizontal line. Now, the direct kinematics can be written as ⎡ ⎤ ⎡ ⎤ ox a1 c1 + a2 c12 + a3 c123 x = ⎣ oy ⎦ = ⎣ a1 s1 + a2 s12 + a3 s123 ⎦ = f (q). (2.177) φ q1 + q2 + q3 Finally, the analytic Jacobian can be computed as ⎡

⎤ −a1 s1 − a2 s12 − a3 s123 −a2 s12 − a3 s123 −a3 s123 ∂f (q) ⎣ a1 c1 + a2 c12 + a3 c123 a2 c12 + a3 c123 a3 c123 ⎦(2.178) J A (q) = = . ∂q 1 1 1 In the above example, the computation of the analytic Jacobian was quite easy, since the direct kinematics function (2.177) is rather simple. In general, the compu-

2.4 Differential Kinematics

61

tation of the analytic Jacobian is more difficult for non planar robots with revolute joints, because the function representing the orientation part is not as simple as in the example.

2.4.2 Geometric Jacobian In contrast with the analytic Jacobian, the geometric Jacobian relates the joint velocities q˙ (t) with the linear and angular velocities of the end-effector. Let 0

vn = 0 d˙ n

(2.179)

denote the end-effector linear velocity and let 0 ωn be its angular velocity. As explained in Sect. 2.1.7, this angular velocity is related to the rotation matrix by   S 0 ωn = 0 R˙ n 0 RTn .

(2.180)

The objective is to find expressions of the form vn = J v q˙ ωn = J ω q˙ ,

0 0

(2.181) (2.182)

where J v and J ω are 3 × n matrices. Obviously, (2.181) and (2.182) can be written in compact form as x˙ =

0

vn 0 ωn

 = 0 J n q˙ ,

(2.183)

where x˙ bundles both linear and angular end-effector velocities and 0 J n is composed as   Jv 0 . (2.184) Jn = Jω The matrix 0 J n above is the geometric Jacobian of the manipulator, or simply Jacobian. Notice that 0 J n is always a 6 × n matrix, where n is the number of manipulator joints, as usual. However, if n < 6 it is customary to choose only those rows of 0 J n which are more relevant in order to have a n × n Jacobian. In what follows, the relationship between joint velocities and end-effector velocities will be studied in order to develop a method to compute the Jacobian. For simplicity’s sake instead of 0 J n throughout the book the notation J or J(q) will be employed.

62

2.4.2.1

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Linear Velocity

The end-effector linear velocity is simply 0 d˙ n , which is given by the chain rule as 0˙

dn =

n  ∂ 0dn i=1

∂qi

q˙i .

(2.185)

Accordingly, the ith column of J v is given by J vi =

∂ 0dn . ∂qi

The above expression can be interpreted as the value of the end-effector linear velocity when q˙i = 1, and q˙ j = 0, with j = i, for i, j = 1, . . . , n. In other words, the ith column of J v can be generated by making all the joint velocities zero, except for that of the ith joint, which is made exactly one. The joints can be either prismatic or revolute and, since their contribution to the motion of the end-effector is different, will be analyzed separately as follows. Prismatic joints If the ith joint is prismatic, then 0 R j is independent of qi = di , for all j. Let us express 0 d n as 0

d n = 0 d i−1 + 0 Ri−1 i−1 d n .

(2.186)

If all joints are fixed except for the ith, it is 0˙

∂ 0dn q˙i ∂qi ∂ 0 d i−1 ∂ 0 Ri−1 i−1 d n = q˙i + q˙i . ∂qi ∂qi

dn =

(2.187)

Since 0 d i−1 and 0 Ri−1 do not depend on qi , one has 0˙

d n = 0 Ri−1

∂

i−1

dn q˙i = 0 Ri−1 i−1 d˙ n . ∂qi

(2.188)

Now, i−1 d˙ n = q˙i k since it is a prismatic joint and the velocity is expressed with respect to the frame i − 1 and as usual k = [0 0 1]T . Thus, it is satisfied 0˙

d n = q˙i 0 Ri−1 k = q˙i zi−1 ,

(2.189)

where zi represents the orientation of the z-axis of the coordinate system oxi ,yi ,zi but expressed in ox0 ,y0 ,z0 . Since the ith column of J v is obtained when all joint velocities

2.4 Differential Kinematics

63 joint i

Fig. 2.30 Effect of a prismatic joint motion on the end-effector linear velocity

xn zi−1 q˙i z0

on yn

y0

zn

x0

o0

are zero but q˙i = 1, one can conclude that such column is given by j vi =

∂ 0dn = zi−1 . ∂qi

(2.190)

The contribution of a prismatic joint motion in the end-effector linear velocity is graphically represented in Fig. 2.30. Revolute joints Consider now the case in which the ith joint is revolute, for which it is still valid 0

d n = 0 d i−1 + 0 Ri−1 i−1 d n ,

(2.191)

which by a common vectorial subtraction can be expressed as 0

d n − 0 d i−1 = 0 Ri−1 i−1 d n .

(2.192)

Since the motion of the ith joint only affects the frames i, . . . , n, then both 0 d i−1 and 0 Ri−1 are constant in such case. Hence, the velocity of the end-effector is given by 0˙

d n = 0 Ri−1 i−1 d˙ n .

(2.193)

Now, since the motion of the ith joint is composed of a rotation over the zi−1 axis by the quantity qi , the resulting linear velocity on the end-effector is i−1 ˙

d n = q˙i k × i−1 d n ,

(2.194)

as can be seen in Fig. 2.31. Therefore, from (2.193) it is   d n = 0 Ri−1 q˙i k × i−1 d n

0˙

= q˙i 0 Ri−1 k × 0 Ri−1 i−1 d n   = q˙i zi−1 × 0 d n − 0 d i−1 ,

(2.195)

64

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Fig. 2.31 Effect of a revolute joint motion on the end-effector linear velocity

0

z0

0 dn

q˙i y0

o0

zi−1

0 −1 − di

d˙ n

xn

on yn

zn

oi−1 joint i

x0

where (2.192) has been used. Notice that it is also employed (2.100), i.e. R(a × b) = Ra × Rb. Accordingly, for this case the ith column of J v is given by jvi

  ∂ 0dn = zi−1 × 0 d n − 0 d i−1 . ∂qi

(2.196)

Summarizing, the upper part of the Jacobian J v is given by  J v = jv1 · · · j vn ,

(2.197)

where the ith column jvi is given by  zi−1 if joint i is prismatic   j vi = zi−1 × 0 d n − 0 d i−1 if joint i is revolute

2.4.2.2

(2.198)

Angular Velocity

To derive the procedure for obtaining the relation between joint velocities and the end-effector angular velocity, first recall (2.137) from Sect. 2.1.7, i.e. 0

ωn = 0 ω1 + 0 R1 1 ω2 + 0 R2 2 ω3 + · · · + 0 Rn−1 n−1 ωn ,

which basically states that angular velocities can be directly added as long as they are expressed in the same frame. Following this, it is possible to obtain the end-effector angular velocity with respect to the base frame by expressing the angular velocity of each element with respect to such frame and then adding these velocities. If the ith joint is a revolute one, then the rotation axis is zi−1 and the rotation angle is qi . Hence, the angular velocity of the ith element, expressed in the i − 1 frame is given by i−1

ωi = q˙i k.

(2.199)

2.4 Differential Kinematics

65

On the other hand, if the ith joint is prismatic, then the motion of the ith frame with respect to the (i − 1)th frame is a linear translation and thus i−1

ωi = 0.

(2.200)

As a result, the angular velocity of the end-effector 0 ωn in the base frame is given by 0

ωn = ρ1 q˙1 k + ρ2 q˙2 0 R1 k + · · · + ρn q˙n 0 Rn−1 k =

n 

ρi q˙i zi−1 ,

(2.201)

i=1

where zi−1 = 0 Ri−1 k,

(2.202)

where ρi , i = 1, . . . , n is equal to 1 if the joint is revolute and equal to 0 if it is prismatic. Clearly, ⎡ ⎤ 0 z0 = k = ⎣ 0 ⎦ . 1 Thereupon, the lower part J ω of the Jacobian (2.184), is given by  J ω = ρ1 z0 · · · ρn zn−1 .

(2.203)

Putting it all together, the manipulator Jacobian has the form 0

 J n = J = j1 · · · jn ,

(2.204)

where the ith column ji of J can be computed by ⎡ ji = ⎣

zi−1 × (on − oi−1 )

⎤ ⎦,

(2.205)

zi−1 if the ith joint is revolute and by ⎡ ji = ⎣

zi−1

⎤ ⎦,

(2.206)

0 if the ith joint is prismatic. These equations are useful to find the geometric Jacobian of any manipulator for which the Denavit-Hartenberg methodology is valid, for which

66

2 Position, Orientation and Velocity of Rigid Robot Manipulators

such computations are very straightforward since all quantities are available from the D-H algorithm. Finally, it is worth to point out that the procedure above can be mimicked to obtain the linear and angular velocity of any element of the manipulator different from the end-effector. Example

Consider the 3 degrees of freedom planar manipulator of example of Sect. 2.4.1 displayed in Fig. 2.29. By following the D-H algorithm, one can find the transformation matrices ⎡ ⎡ ⎤ ⎤ c1 −s1 0 a1 c1 c12 −s12 0 a1 c1 + a2 c12 ⎢ −s1 c1 0 a1 s1 ⎥ 0 ⎢ −s12 c12 0 a1 s1 + a2 s12 ⎥ 0 ⎢ ⎥ ⎥ , and T1 = ⎢ ⎣ 0 0 1 0 ⎦ , T2 = ⎣ 0 ⎦ 0 1 0 0 0 0 1 0 0 0 1 ⎡ ⎤ c123 −s123 0 a1 c1 + a2 c12 + a3 c123 ⎢ −s123 c123 0 a1 s1 + a2 s12 + a3 s123 ⎥ 0 ⎥. T3 = ⎢ ⎣ 0 ⎦ 0 1 0 0 0 0 1 Since the first joint is revolute, the first column of J v is computed as   j v1 = z0 × 0 d 3 − 0 d 0 ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 0 a1 c1 + a2 c12 + a3 c123 −a1 s1 − a2 s12 − a3 s123 = ⎣0⎦ × ⎣ a1 s1 + a2 s12 + a3 s123 ⎦ = ⎣ a1 c1 + a2 c12 + a3 c123 ⎦ . 1 0 0 In the same manner, the remaining columns are given by ⎡

j v2

⎡ ⎤ ⎤ −a2 s12 − a3 s123 −a3 s123 = ⎣ a2 c12 + a3 c123 ⎦ , and j v3 = ⎣ a3 c123 ⎦ . 0 0

On the other hand, the columns of the lower part J ω are simply jω1

⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 0 0 0 = z0 = ⎣0⎦ , j ω2 = z1 = ⎣0⎦ , and j ω3 = z2 = ⎣0⎦ . 1 1 1

Finally, the complete Jacobian matrix is

(2.207)

2.4 Differential Kinematics

67

⎡

⎤ −a1 s1 − a2 s12 − a3 s123 −a2 s12 − a3 s123 −a3 s123 ⎢ a1 c1 + a2 c12 + a3 c123 a2 c12 + a3 c123 a3 c123 ⎥ ⎢ ⎥ ⎢ 0 0 0 ⎥ ⎥. J(q) = ⎢ ⎢ 0 0 0 ⎥ ⎢ ⎥ ⎣ 0 0 0 ⎦ 1 1 1

(2.208)

As expected, the upper part, the linear velocity of the end-effector, is the same as that in (2.178), if the third row is obviated. However, despite the last row of (2.178) and (2.208) are the same, this is not true in general. It is important to notice that in (2.175) and in (2.183) the same notation x˙ has been employed, though representing different quantities. Although, the first three components of both Jacobians are the same, the last three ones are in general different. 0 ωn in (2.183) represents the angular velocity of the end-effector frame with respect to the base and it has a precise physical meaning. However, its integral over time does not have it unless the rotation takes place around one single axis, where then it represents precisely the angle of rotation. On the other hand, φ˙ in (2.175) does not own any physical meaning, while φ represents some orientation parameters, e.g. the Euler angles. Example  For the unaware reader it may be surprising that 0 ωn dt angular velocities does not have a physical meaning in general since when rotating around one single axis it represents the angle of rotation. To see this, suppose the following angular velocity is chosen from 0 ≤ t ≤ 1 s: ⎡ ⎤  0  rad ω1 = ⎣ 0 ⎦ s π 2

This is equivalent to a basic rotation around the z0 axis by 90◦ , i.e. Rz,90◦ . Then from 1 < t ≤ 2 s choose ⎡ ⎤  0  π ⎦ rad ⎣ , ω2 = 2 s 0 which is equivalent to a basic rotation around the y0 axis by 90◦ , i.e. R y,90◦ . Therefore, the total rotation is given by 0

R1 = R y,90◦ Rz,90◦ .

(2.209)

68

2 Position, Orientation and Velocity of Rigid Robot Manipulators

Now, suppose it is chosen first ω2 for 0 ≤ t ≤ 1 s, and ω1 for 1 < t ≤ 2 s. For that case the total rotation is given by 0

R1 = Rz,90◦ R y,90◦ .

(2.210)

R1 = 0 R1 .

(2.211)

Clearly 0

However, by computing the integral over 0 ≤ t ≤ 2 one gets ⎡ 1

2 ω1 dt +

0

1 ω2 dt =

1

2 ω2 dt +

0

1

0

⎢ ⎢π ω1 dt = ⎢ ⎢2 ⎣ π 2

⎤ ⎥ ⎥ ⎥. ⎥ ⎦

Therefore, the result is the same and it cannot be related at all with (2.211).

2.4.3 Singularities The Jacobian matrix J(q) of dimension 6 × n, defines the mapping x˙ = J(q)˙q,

(2.212)

from the joint velocities vector q˙ to the end-effector velocities one   v x˙ = . ω This equation represents also a linear transformation between differentials dx and dq, as follows dx = J(q)dq.

(2.213)

One can see such differentials as possible directions of motions in Rn and in R6 , respectively. Since the Jacobian is a function of the joint position vector q, it is important to analyze those configurations for which J(q) loses rank. Such configurations are called singularities or singular configurations. The relevance of analyzing them arises due to several reasons: • They represent configurations from which certain directions of motion cannot be reached.

References

69

• In a singular configuration, bounded end-effector velocities can correspond to unbounded joint velocities. • Most of the times, singularities are found in the border of the robot workspace (maximum reachable points). • In a singular configuration, bounded forces and moments at the end-effector can correspond to unbounded joint torques and forces. • At some singularities, points in the manipulator workspace are no longer reachable under slight modifications of the robot kinematic parameters. • Near singular configurations, a unique solution for the inverse kinematics might not exist or there could be an infinite number of solutions.

Example

Consider again the example of the three degrees of freedom planar manipulator shown in Fig. 2.29. Since its analytic Jacobian (2.178) is a square matrix, its singular points can be obtained by computing its determinant given by det (J A (q)) = a1 a2 sin (q2 ) .

(2.214)

Hence, the analytic Jacobian loses rank when q2 = kπ/2, for any integer k. These points correspond to the configurations for which the arm formed by the first two links of the manipulator is either full-stretched or full-contracted. As mentioned above, such configurations correspond to points on the border of the manipulator workspace.

References Beer FP, Johnston ER Jr, Mazurek DF, Cornwell PJ (2013) Vector mechanics for engineers: statics and dynamics, 10th edn. McGraw-Hill, USA Fu KS, Gonzalez RC, Lee CSG (1987) Robotics: control, sensing, vision, and intelligence. McGrawHill, USA Murray RM, Li Z, Sastry SS (1994) A mathematical introduction to robotic manipulation. CRC Press, Boca Raton, FL Sciavicco L, Siciliano B (2000) Modeling and control of robot manipulators, 2nd edn. Springer, London Siciliano B, Villani L (1999) Robot force control. Kluwer Academic Publishers, The Netherlands Spong MW, Hutchinson S, Vidyasagar M (2006) Robot modeling and control. Wiley, USA Strang G (2006) Linear algebra and its applications, 4th edn. Thompson Brooks, USA

Chapter 3

Dynamics of Rigid Robot Manipulators

Abstract As for most physical systems, control-observer design of robot manipulators can take advantage of the knowledge of a dynamic model. It turns out that their usual analytical description owns many structural properties due to the physical characteristics of mechanical systems, while some other properties arise from the modeling methodology employed. In this chapter, the modeling of n degrees of freedom rigid robot manipulators on the basis of the Lagrange’s equations of motion is briefly discussed. Several notable properties are presented and their impact on control-observer design is underlined.

3.1 Dynamic Modeling of Rigid Robot Manipulators The equations of motion of rigid robot manipulators are a set of analytical equations to describe their dynamics that are useful for many purposes, e.g. control-observer design, computer simulation, parameter estimation, robot design, etc. It is possible to obtain a dynamic model by using physical laws leading to a set description based on the number of joints of the manipulator as well as geometrical and inertial characteristics of the links. Perhaps the two most employed methodologies are the Euler-Lagrange and the Newton-Euler equations. In this chapter the former will be presented in detail.

3.1.1 Euler-Lagrange Equations of Motion The general equations of motion of a robot manipulator can be obtained by the direct application of the Euler-Lagrange formulation for non conservative systems, resulting in d dt



∂L ∂ q˙i

 −

∂L = τi ∂qi

i = 1, . . . , n,

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control of Robot Manipulators, Lecture Notes in Electrical Engineering 798, https://doi.org/10.1007/978-3-030-85980-0_3

(3.1)

71

72

3 Dynamics of Rigid Robot Manipulators

where L K P qi τi

= Lagrangian function = kinetic energy K − potential energy P = total kinetic energy of the manipulator = total potential energy of the manipulator = generalized coordinate of the manipulators = generalized force or torque applied at the ith joint to move the ith link.

For robot manipulators the generalized coordinates are joint angles or displacements, depending on whether the ith joint is revolute or prismatic.

3.1.2 Kinetic Energy Let the ith link of a robot be made up of a set of particles and let ρi be the mass density, for i = 1, . . . , n. Let Bi be the tridimensional space region occupied by the link, such that  ρi (x, y, z)dx dy dz = m i , (3.2) Bi

where m i is the total mass of the ith link. The corresponding kinetic energy is then given by 1 Ki = 2

 viT (x, y, z)vi (x, y, z)ρi (x, y, z)dx dy dz Bi

=

1 2

 viT (x, y, z)vi (x, y, z)dm,

(3.3)

Bi

where dm stands for the infinitesimal mass of the particle located at the coordinates (x, y, z). When a body moves in the space not all particles move with the same velocities. Therefore, it is more convenient to use the link’s center of mass, which can be expressed as: 0

1 r¯i = mi

 0

ri dm,

(3.4)

Bi

where 0 r¯i stands for the position of the ith center of mass and 0 ri represents any point on the object. Note that 0 r¯i is fixed on the ith link and therefore it is independent of the mass differential, which means that the previous equation is equivalent to  Bi

0

 r¯i − 0 ri dm = 0,

(3.5)

3.1 Dynamic Modeling of Rigid Robot Manipulators

73

zci xci

nk th li ) 1 (i

ci

ri

yci

0

i-th link

r¯ i 0

ri

z0 y0 x0 Fig. 3.1 An extra coordinate frame can be attached to the center of mass of the ith link

because 

 0

Bi

r¯i dm −

 0

ri dm =

Bi

r¯i m i −  

0 r¯i does not depend on m i Bi 0

0

ri dm = 0.

(3.6)

This property of the center of mass allows to attach a new coordinate frame in the ith link as shown in Fig. 3.1. When the link moves, the velocity of a point on it with respect to the base coordinate frame and expressed on the same coordinate frame can be computed by using (2.119) 0

vi = 0 v¯ i + 0 ω¯ i × 0 Rci ci ri ,

(3.7)

where ci ri is the position of the same point but with respect to the center of mass frame. Note that it is also possible to express 0 vi with respect to the coordinate frame fixed at the ith center of mass, oxci ,yci ,zci , just by multiplying by the corresponding rotation matrix ci R0 = 0 RTci . Then, the velocity of the particle located at 0 ri with respect to the base frame but express in oxci ,yci ,zci is given by

74

3 Dynamics of Rigid Robot Manipulators ci

 v¯ i + 0 ω¯ i × 0 Rci ci ri     = 0 RTci 0 v¯ i + 0 RTci 0 ω¯ i × 0 RTci 0 Rci ci ri     = ci R0 0 v¯ i + ci R0 0 ω¯ i × ci R0 0 Rci ci ri

vi = ci R0 0 vi = 0 RTci 0 vi = 0 RTci

0

= ci v¯ i + ci ω¯ i × ci ri .

(3.8)

By recalling that the cross product can be carried out by using a skew symmetric matrix one gets ci

vi = ci v¯ i + S(ci ω¯ i )ci ri .

(3.9)

This relationship is useful because the kinetic energy of the link has to be the same in any frame, so that by substituting (3.9) in (3.3) yields Ki =

1 2



ci

v¯ i + S(ci ω¯ i )ci ri

T ci

 v¯ i + S(ci ω¯ i )ci ri dm

Bi

1 = 2



ci

v¯ iT ci v¯ i + ci v¯ iT S(ci ω¯ i )ci ri + ci riT S(ci ω¯ i )Tci v¯ i

Bi

+ci riT S(ci ω¯ i )T S(ci ω¯ i )ci ri dm = Ki1 + Ki2 + Ki3 + Ki4 ,

(3.10)

where the last four terms represent the four integrals to be solved. The first one can be calculated as  1 ci T ci 1 Ki1 = (3.11) v¯ i v¯ i dm = m i ci v¯ iT ci v¯ i , 2 2 Bi

where it has been taken advantage of the fact that ci v¯ i is independent of the link mass. Ki1 represents the translational part of the kinetic energy. As to Ki2 it is 1 Ki2 = 2

 ci T ci v¯ i S( ω¯ i )ci ri dm

1 = ci v¯ iT S(ci ω¯ i ) 2

Bi

 ci

ri dm = 0,

(3.12)

Bi

because  ci

ri dm = m i ci r¯i = 0,

(3.13)

Bi

in view of the fact that ci r¯i is independent of the mass and it represents the i center of mass position with respect to the frame fixed precisely at that center of mass. For the same reason one also has

3.1 Dynamic Modeling of Rigid Robot Manipulators

Ki3 =

1 2

75

 ci T T ci ri S ( ω¯ i )ci v¯ i dm

= 0.

(3.14)

Bi

It remains to compute Ki4 1 Ki4 = 2

 ci T T ci ri S ( ω¯ i )S(ci ω¯ i )ci ri dm,

(3.15)

Bi

which is not as direct as the first three terms. First of all consider rewriting Ki4 =

1 2

 Bi

=

  Tr S(ci ω¯ i )ci ri ci riT ST (ci ω¯ i ) dm ⎛

1 ⎝ ci Tr S( ω¯ i ) 2

⎞

 ci

ri ci riT dmST (ci ω¯ i )⎠

Bi

 1  = Tr S(ci ω¯ i )J i ST (ci ω¯ i ) , 2

(3.16)

where it has been taken advantage that ci ω¯ i is associated to the center of mass position and thus it can be taken outside the integral, while ci ri represents the position of all particles of the link. The matrix J i ∈ R3×3 is given by  Ji =

ci

ri ci riT dm,

(3.17)

Bi

or   ⎤ ⎡  2 ⎤ ⎡ j11 j12 j13  xi dm  xi y2 i dm  xi z i dm J i = ⎣  xi yi dm  yi dm  yi z i dm ⎦ = ⎣ j12 j22 j23 ⎦ . z i2 dm j13 j23 j33 xi z i dm yi z i dm

(3.18)

Say that ci ω¯ i = [ω¯ 1i ω¯ 2i ω¯ 3i ]T , and recall ⎡

⎤ 0 −ω¯ 3i ω¯ 2i S(ci ω¯ i ) = ⎣ ω¯ 3i 0 −ω¯ 1i ⎦ , −ω¯ 2i ω¯ 1i 0 so that S(ci ω¯ i )J i ST (ci ω¯ i ) can be computed as

(3.19)

76

3 Dynamics of Rigid Robot Manipulators ⎤⎡ ⎤ ⎤⎡ ω¯ 2i j11 j12 j13 0 ω¯ 3i −ω¯ 2i 0 −ω¯ 3i ⎣ ω¯ 3i 0 −ω¯ 1i ⎦ ⎣ j12 j22 j23 ⎦ ⎣ −ω¯ 3i 0 ω¯ 1i ⎦ −ω¯ 2i ω¯ 1i 0 j13 j23 j33 ω¯ 2i −ω¯ 1i 0 ⎤⎡ ⎡ ⎤ 0 ω¯ 3i −ω¯ 2i −ω¯ 3i j12 + ω¯ 2i j13 −ω¯ 3i j22 + ω¯ 2i j23 −ω¯ 3i j23 + ω¯ 2i j33 ω¯ 3i j12 − ω¯ 1i j23 ω¯ 3i j13 − ω¯ 1i j33 ⎦ ⎣ −ω¯ 3i 0 ω¯ 1i ⎦ = ⎣ ω¯ 3i j11 − ω¯ 1i j13 −ω¯ 2i j11 + ω¯ 1i j12 −ω¯ 2i j12 + ω¯ 1i j22 −ω¯ 2i j13 + ω¯ 1i j23 ω¯ 2i −ω¯ 1i 0 ⎡

= (ci ω¯ i )

where the elements of (ci ω¯ i ) are given by  11 = −ω¯ 3i (−ω¯ 3i j22 + ω¯ 2i j23 ) + ω¯ 2i (−ω¯ 3i j23 + ω¯ 2i j33 )  12 = ω¯ 3i (−ω¯ 3i j12 + ω¯ 2i j13 ) − ω¯ 1i (−ω¯ 3i j23 + ω¯ 2i j33 )  13 = −ω¯ 2i (−ω¯ 3i j12 + ω¯ 2i j13 ) + ω¯ 1i (−ω¯ 3i j22 + ω¯ 2i j23 )  21 = −ω¯ 3i (ω¯ 3i j12 − ω¯ 1i j23 ) + ω¯ 2i (ω¯ 3i j13 − ω¯ 1i j33 )  22 = ω¯ 3i (ω¯ 3i j11 − ω¯ 1i j13 ) − ω¯ 1i (ω¯ 3i j13 − ω¯ 1i j33 )  23 = −ω¯ 2i (ω¯ 3i j11 − ω¯ 1i j13 ) + ω¯ 1i (ω¯ 3i j12 − ω¯ 1i j23 )  32 = ω¯ 3i (−ω¯ 2i j11 + ω¯ 1i j12 ) − ω¯ 1i (−ω¯ 2i j13 + ω¯ 1i j23 )  33 = −ω¯ 2i (−ω¯ 2i j11 + ω¯ 1i j12 ) + ω¯ 1i (−ω¯ 2i j12 + ω¯ 1i j22 ). Therefore Tr(S(ci ω¯ i )J i ST (ci ω¯ i )) = −ω¯ 3i (−ω¯ 3i j22 + ω¯ 2i j23 ) + ω¯ 2i (−ω¯ 3i j23 + ω¯ 2i j33 ) +ω¯ 3i (ω¯ 3i j11 − ω¯ 1i j13 ) − ω¯ 1i (ω¯ 3i j13 − ω¯ 1i j33 ) −ω¯ 2i (−ω¯ 2i j11 + ω¯ 1i j12 ) + ω¯ 1i (−ω¯ 2i j12 + ω¯ 1i j22 ) ⎡ ⎤⎡ ⎤ ω¯ 1i − j13  T j22 + j33 − j12 j11 + j33 − j23 ⎦ ⎣ ω¯ 2i ⎦ = ω¯ 1i ω¯ 2i ω¯ 3i ⎣ − j12 − j13 − j23 j11 + j22 ω¯ 3i 

 Ii

I i ∈ R3×3 is an inertia matrix, which according to (3.18) is given by ⎡ 

   ⎤ yi2 + z i2 dm − xi yidm −  xi z i dm ⎦ xi2+ z i2 dm  − I i = ⎣ −  xi yi dm  2 yi z i2dm  − xi z i dm − yi z i dm xi + yi dm   ⎡ ⎤ − xi yi dm −  xi z i dm  Ixxi − yi z i dm ⎦ = ⎣ −  xi yi dm  Iyyi Izzi − xi z i dm − yi z i dm ⎡ ⎤ Ixxi −Ixyi −Ixzi = ⎣ −Ixyi Iyyi −Iyzi ⎦ , −Ixzi −Iyzi Izzi

(3.20)

where Ixxi , Iyyi , Izzi are moments of inertia and Ixzi , Ixyi , Iyzi are products of inertia computed with respect to oxci ,yci ,zci . From (3.16) it is

3.1 Dynamic Modeling of Rigid Robot Manipulators

77

1 ci T ci ω¯ I i ω¯ i , 2 i

Ki4 =

(3.21)

which represents the rotation part of the kinetic energy of the ith link. After (3.11) and (3.21) the total kinetic energy is given by 1 ci T ci 1 m i v¯ i v¯ i + ci ω¯ iT I i ci ω¯ i 2 2 1 ci 0 T ci 0  1 ci 0 T ci 0  = m i R0 v¯ i R0 v¯ i + R0 ω¯ i I i R0 ω¯ i 2 2 1 1 = m i 0 v¯ iT ci RT0 ci R0 0 v¯ i + 0 ω¯ iT ci RT0 I i ci R0 0 ω¯ i . 2 2

Ki =

(3.22)

Finally, it is Ki =

1 0 T0 1 m i v¯ i v¯ i + 0 ω¯ iT 0 Rci I i 0 RTci 0 ω¯ i . 2 2

(3.23)

To compute the lineal and angular velocities of the ith center of mass, Jacobian matrices can be computed to get 0

v¯ i = J vci (q)˙q

and

0

ω¯ i = J ωci (q)˙q.

(3.24)

This way, the manipulator’s total kinetic energy is given by K=

n 

Ki ,

(3.25)

i=1

which from (3.23) and (3.24) becomes K=

1 T q˙ H(q)˙q, 2

(3.26)

where H(q) =

n  

m i J Tvci (q)J vci (q) + J Tωci (q)0 Rci (q)I i 0 RTci (q)J ωci (q) .

(3.27)

i=1

3.1.3 Potencial Energy Let P be the total potential energy of a robot manipulator and Pi the potential energy of the ith link, which can straightforwardly be computed as

78

3 Dynamics of Rigid Robot Manipulators

Pi = −m i g¯ T0 r¯i , for i = 1, 2, . . . , n, and

(3.28)

⎤ gx g¯ = ⎣ g y ⎦ gz ⎡

is a gravity vector expressed in the base coordinate frame. For example, for the link depicted in Fig. 3.1 it is ⎡

⎤ 0 g¯ = ⎣ 0 ⎦ , −g0 where g0 = 9.8062 m/s2 is the gravitational constant. Finally P=

n 

Pi = −

i=1

n 

m i g¯ T0 r¯i .

(3.29)

i=1

3.2 Equations of Motion of a Robot Manipulator 3.2.1 Generalized Model By taking into account (3.26) and (3.29), the Lagrangian function L = K − P is given by  1 m i g¯ T0 r¯i . L = q˙ T H(q)˙q + 2 i=1 n

(3.30)

Express (3.1) in matrix form to get1 d ∂L ∂L − = τ, dt ∂ q˙ ∂q

(3.31)

where τ ∈ Rn is the generalized force/torque vector acting at the joints.

Technically, ∂(·) ∂ q˙ produces a row vector; however, for simplicity’s sake it is considered here a column vector although this is an abuse of notation.

1

3.2 Equations of Motion of a Robot Manipulator

79

The next step is to compute a model in matrix form by developing the left hand , from (3.30) and (3.31) one gets side of (3.31). Beginning with ∂L ∂q   n  1 ∂  T ∂L ∂  T0 q˙ H(q)˙q . m i g¯ r¯i − − =− ∂q ∂q i=1 2 ∂q

(3.32)

The first term of the right hand side of (3.32) represents the gravity vector g(q) ∈ Rn , whose jth element can be obtained by: ∂ gj = − ∂q j

 n 

 m i g¯ r¯i , T0

(3.33)

i=1

and that can be written in a compact form as g(q) =

∂ P . ∂q

(3.34)

The second term of the right hand side of (3.32) belongs to the vector of Coriolis and centrifugal torques to be obtained later. Prior to it, the first part of (3.31) is developed: d ∂L d ˙ q. = H(q)˙q = H(q)¨q + H(q)˙ dt ∂ q˙ dt

(3.35)

˙ q ∈ Rn belongs as well to the The vector H(q)¨q is already in its final form, while H(q)˙ vector of Coriolis and centrifugal torques denoted here by hc (q, q˙ ) ∈ Rn . Therefore, it is ˙ q− hc (q, q˙ ) = H(q)˙

 1 ∂  T q˙ H(q)˙q . 2 ∂q

(3.36)

Using (3.36) to compute hc (q, q˙ ) is not practical. Instead it is a common and better practice to write hc (q, q˙ ) as hc (q, q˙ ) = C(q, q˙ )˙q,

(3.37)

for a matrix C(q, q˙ ) ∈ Rn×n . To find the elements of this matrix consider that the ith element of hc (q, q˙ ), h ci is given by

80

3 Dynamics of Rigid Robot Manipulators n 

1   ∂h k j q˙k q˙ j h˙ i j q˙ j − 2 k=1 j=1 ∂qi j=1  n  n n n   ∂h i j 1   ∂h k j = q˙k q˙ j − q˙k q˙ j ∂qk 2 k=1 j=1 ∂qi j=1 k=1  n  n   ∂h i j 1 ∂h k j q˙k q˙ j . = − ∂qk 2 ∂qi k=1 j=1

h ci =

n

n

(3.38)

In view of the fact that ⎞ ⎛ n n n n   ∂h i j 1 ⎝  ∂h i j q˙k q˙ j = q˙k q˙ j + q˙k q˙ j ⎠ ∂qk 2 k=1 j=1 ∂qk ∂q k k=1 j=1 ⎞ ⎛ n n  n n  1 ⎝  ∂h i j ∂h ik = q˙k q˙ j + q˙k q˙ j ⎠ , (3.39) 2 k=1 j=1 ∂qk ∂q j k=1 j=1

n n   ∂h i j k=1 j=1

one gets h ci =

n  n    1 ∂h i j k=1 j=1

+

∂qk

2

∂h k j ∂h ik − ∂q j ∂qi

 q˙k q˙ j

i = 1, . . . , n.

(3.40)

Let ck ji =

1 2



∂h i j ∂h k j ∂h ik + − ∂qk ∂q j ∂qi

 = c jki ,

(3.41)

where ck ji are the Christoffel symbols of the first kind. Based on them, the i jth element of C(q, q˙ ) for i, j = 1, . . . , n is given by ci j =

n 

ck ji q˙k

i, j = 1, . . . , n.

(3.42)

k=1

Finally, (3.31)–(3.42) lead to the following equations of motion: H(q)¨q + C(q, q˙ )˙q + g(q) = τ .

(3.43)

Note that, if desired, friction effects can be added, e.g. H(q)¨q + C(q, q˙ )˙q + D˙q + g(q) = τ ,

(3.44)

3.2 Equations of Motion of a Robot Manipulator

81 y0

Fig. 3.2 Two degrees of freedom planar robot

m2



q2

/2 m1 q1 /2

x0



where D ∈ Rn×n is a diagonal positive semidefinite matrix accounting for viscous friction. Part III is devoted to the modeling of some industrial robots, but for the reader’s comfort a simple example is provided in the following. Example

Consider the 2-DOF planar robot analyzed in the first example of Sect. 2.2.2.2 and that it is shown in Fig. 3.2. For simplicity’s sake both links are considered to be slender rods with the same length , but different masses m 1 and m 2 , respectively. No friction effects are included. The first step is to compute the inertia matrix according to (3.27). In fact, it is not difficult to get from the first example of Sect. 2.4.2.2 ⎡ J vc1

⎢ ⎢ ⎢ =⎢ ⎢ ⎣

⎤

⎡

⎥ ⎥ c 0⎥ ⎥ 2 1 ⎥ ⎦ 0 0

⎢ ⎢ =⎢ ⎢ ⎣

− 2 s1 0

J vc2

−s1 − 2 s12 − 2 s12 c1 + 0

 c 2 12

⎤ ⎥ ⎥

 c ⎥, 2 12 ⎥

⎦

0

(3.45)

82

3 Dynamics of Rigid Robot Manipulators

and ⎡ J wc1

00

⎢ ⎢ =⎢ ⎢0 ⎣ 1

⎤

⎥ ⎥ 0⎥ ⎥ ⎦ 0

⎡ J wc2

00

⎢ ⎢ =⎢ ⎢0 ⎣ 1

⎤

⎥ ⎥ 0⎥ ⎥. ⎦ 1

(3.46)

Besides it is ⎡

⎡

⎤ c1 −s1 0 0 Rc1 = ⎣ s1 c1 0 ⎦ 0 0 1

⎤ c12 −s12 0 0 Rc2 = ⎣ s12 c12 0 ⎦ . 0 0 1

(3.47)

It remains to compute I 1 and I 2 in (3.20). Since slender rods are considered and the moments and products of inertia are computed with respect to the center of mass of each link, then it is quite direct to get by using available relationships in basic vector mechanical books that: Ixxi = 0, Iyyi = Izzi =

1 m i 2 12

and

Ixyi = Ixzi = Iyzi = 0.

Therefore one gets ⎤ 0 0 0 ⎥ ⎢ 1 0 I i = ⎣ 0 12 m i 2 ⎦. 1 2 0 0 12 m i  ⎡

(3.48)

By direct computation it is ⎡

J Tvc2 J vc2

⎤

⎡ 2 ⎤ ⎥ l 0 ⎥ ⎢ 4 ⎥ c 0⎥ (3.49) ⎥=⎣ ⎦ 2 1 ⎥ ⎦ 0 0 0 0 ⎡ ⎤ −s1 − 2 s12 − 2 s12 ⎤ ⎡ ⎥ −s1 − 2l s12 c1 + 2l c12 0 ⎢ ⎢ ⎥   ⎢ ⎦ ⎣ = c1 + 2 c12 c ⎥ ⎢ 2 12 ⎥ l ⎦ − 2l s12 c 0 ⎣ 2 12 0 0 ⎡ ⎤ 2 2 5 2   2 2 c2 + 4 ⎥ ⎢ 4  +  c2 ⎥, =⎢ (3.50) ⎣ ⎦ 2 2 c + 2 2 2 4 4 ⎤⎢ − 2 s1 2 c1 0 ⎢ ⎦⎢ =⎣ ⎢ ⎢ 0 0 0 ⎣ ⎡

J Tvc1 J vc1

− 2 s1 0

3.2 Equations of Motion of a Robot Manipulator

83

and ⎤ ⎡  c −s1 0   001 ⎣ 1 001 ⎦ s1 c1 0 = = 000 000 0 0 1 ⎤ ⎡   c −s12 0   0 0 1 ⎣ 12 001 s12 c12 0 ⎦ = = . 001 001 0 0 1 

J Twc1 0 Rc1

J Twc2 0 Rc2

(3.51)

(3.52)

Thus it is ⎤⎡ 0 0 0 0 0 0 1 ⎢ 0 1 m 2 ⎥⎣ 0 = 1 ⎦ 0 0 0 0 ⎣ 12 1 2 1 0 0 12 m 1  ⎤ ⎡ 1 m i 2 0 12 ⎦, =⎣ 0 0 

J Twc1 0 Rc1 I 1 0 RTc1 J wc1



⎡

⎤ 0 0⎦ 0 (3.53)

and

J Twc2 0 Rc2 I 2 0 RTc2 J wc2

⎡

⎤⎡ 0 0 0 0 0 0 1 ⎢ 0 1 m l2 ⎥⎣ 0 0 = 2 ⎣ ⎦ 12 001 1 2 1 m 2l 0 0 ⎤ 12 ⎡ 1 m 2 1 m 2 2 12 2 ⎥ ⎢ 12 =⎣ ⎦. 1 m 2 1 m 2 12 2 12 2 



⎤ 0 0⎦ 1

(3.54)

Finally the inertia matrix can be gotten as H(q) = m 1 J Tvc1 J vc1 + m 2 J Tvc2 J vc2 + J Twc1 0 Rc1 I 1 0 RTc1 J wc1 + J Twc2 0 Rc2 I 2 0 RTc2 J wc2 ⎤ ⎡ 1 m 2 + 4 m 2 + m l 2 c 1 m 2 + 1 m 2 c 1 2 2 2 2 2 2 3 3 2 ⎥ ⎢3 =⎣ ⎦. 1 m 2 + 1 m 2 c 1 m 2 3 2 2 2 2 3 2

(3.55)

Once the inertia matrix is known it is straightforward to get the matrix C(q, q˙ ). Firstly, the Christoffel symbols are calculated using (3.41):

84

3 Dynamics of Rigid Robot Manipulators

c111 = c211

1 = 2

c121 = c221

1 2

1 2

1 = 2

c112 =

1 2

c212

1 = 2

c122

1 = 2

c222 =

1 2

 

     

∂h 11 ∂h 11 ∂h 11 + − ∂q1 ∂q1 ∂q1 ∂h 11 ∂h 12 ∂h 21 + − ∂q2 ∂q1 ∂q1 ∂h 12 ∂h 11 ∂h 12 + − ∂q1 ∂q2 ∂q1 ∂h 12 ∂h 12 ∂h 22 + − ∂q2 ∂q2 ∂q1 ∂h 21 ∂h 21 ∂h 11 + − ∂q1 ∂q1 ∂q2 ∂h 21 ∂h 22 ∂h 21 + − ∂q2 ∂q1 ∂q2 ∂h 22 ∂h 21 ∂h 12 + − ∂q1 ∂q2 ∂q2 ∂h 22 ∂h 22 ∂h 22 + − ∂q2 ∂q2 ∂q2

 = 

 

0

1 = − m 2 2 s2 2

1 = − m 2 2 s2 2 1 = − m 2 2 s2 2

 =

1 m 2 2 s2 2

=

0

=

0

=

0.

  

Then the elements of C(q, q˙ ) are obtained by employing (3.42) c11 =

c12 =

c21 =

n 

1 ck11 q˙k = c111 q˙1 + c211 q˙2 = − m 2 2 s2 q˙2 2 k=1

n 

1 1 ck21 q˙k = c121 q˙1 + c221 q˙2 = − m 2 2 s2 q˙1 − m 2 2 s2 q˙2 2 2 k=1

n 

ck12 q˙k = c112 q˙1 + c212 q˙2 =

1 m 2 2 s2 q˙1 2

ck22 q˙k = c122 q˙1 + c222 q˙2 =

0.

k=1

c22 =

n  k=1

To compute the gravitational vector it is necessary to get the potential energy according to (3.29), where

3.2 Equations of Motion of a Robot Manipulator

⎡

⎡

⎤ 0 g¯ = ⎣ −g0 ⎦ 0

⎤ 1 c 1 ⎢2 ⎥ ⎢ ⎥ ⎢ ⎥ 0 r¯1 = ⎢ 1 s1 ⎥ ⎢2 ⎥ ⎣ ⎦ 0

85

⎡

⎤ c1 + 21 c12 ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ 0 r¯2 = ⎢ s1 + 1 s12 ⎥ , ⎢ ⎥ 2 ⎣ ⎦ 0

so that P = −m 1 g¯ T0 r¯1 − m 2 g¯ T0 r¯2 =

1 1 m 1 g0 ls1 + m 2 g0 ls1 + m 2 g0 ls12 . 2 2

(3.56)

By using (3.34) one gets the elements g1 and g2 of g(q) as g1 =

1 1 m 1 g0 c1 + m 2 g0 c1 + m 2 g0 c12 . 2 2

and g2 =

1 m 2 g0 c12 . 2

Summarizing the complete robot model is given by H(q)¨q + C(q, q˙ )˙q + g(q) = τ ,

(3.57)

where ⎤ 1 m 2 + 4 m 2 + m l 2 c 1 m 2 + 1 m 2 c 1 2 2 2 2 2 2 3 3 2 ⎥ ⎢3 H(q) = ⎣ ⎦ 1 m 2 + 1 m 2 c 1 m 2 3 2 2 2 2 3 2 ⎤ ⎡ 1 1 1 2 2 − m 2  s2 q˙2 − 2 m 2  s2 q˙1 − 2 m 2 2 s2 q˙2 ⎢ 2 ⎥ C(q, q˙ ) = ⎣ ⎦ 1 m 2 s q˙ 0 2 2 1 ⎡ 2 ⎤ 1 m g c + m g c + 1 m g c 1 0 1 2 0 1 2 0 12 2 2 ⎢ ⎥ g(q) = ⎣ ⎦. 1 m g c 2 2 0 12 ⎡

As mentioned before, friction effects can be added if desired.

(3.58)

(3.59)

(3.60)

86

3 Dynamics of Rigid Robot Manipulators

3.3 Inclusion of Environmental Forces Suppose that the robot end-effector gets in touch with the environment as shown in Fig. 3.3. The contact with the object or surface can generate both forces and torques at the end-effector with respect to the base frame given by ⎤ fx ⎢ fy ⎥ ⎢ ⎥ ⎢ fz ⎥ ⎥ Fe = ⎢ ⎢ τx ⎥ . ⎢ ⎥ ⎣ τy ⎦ τz ⎡

(3.61)

For simplicity’s sake throughout the book the vector Fe ∈ R6 will be referred to as external forces only and it does not need to be of dimension 6 if n < 6 or if the dimension of motion constraints, say m, is smaller than n. For that case n − m elements of Fe will be zero. To find out how these forces are reflected at the generalized joints, the principle of virtual work can be employed. If δq represents the virtual joint displacement caused by Fe , then according to (2.183) the virtual end-effector displacement must satisfy

f

Fig. 3.3 Robot manipulator in contact with its environment

3.3 Inclusion of Environmental Forces

87

δx = J(q)δq.

(3.62)

Since the force opposes to the movement of the end-effector caused by the input torque, then the virtual work can be computed as   δW = FTe δx − τ Te δq = FTe J(q) − τ Te δq = 0,

(3.63)

from which the following relationship can be obtained τ e = J T (q)Fe .

(3.64)

This relationship can be directly inserted in (3.44) to get H(q)¨q + C(q, q˙ )˙q + D˙q + g(q) = τ − J T (q)Fe .

(3.65)

An issue of particular interest is how to obtain, compute or represent Fe . Assume for example that the surface shown in Fig. 3.3 can be represented by a set of holonomic constraints given by ϕ(q) = 0,

(3.66)

with ϕ(q) ∈ Rm and m represents the number of constraints as mentioned before. If ϕ j (q) is the jth element of ϕ(q) for j = 1, . . . , m, then its derivative satisfies  d d ϕ j (q) = a ji qi = 0, dt dt i=1 n

ϕ˙ j (q) =

(3.67)

for i = 1, . . . , n and where a ji =

∂ϕ j (q) . ∂qi

(3.68)

In terms of virtual displacements one must therefore have n 

a ji δqi = 0,

(3.69)

i=1

while once again the effect on the system dynamics is that of forces which do not do work. Let’s denote the generalized torque associated to δqi as τ¯i (might just be a force as well), so that in view of the Principle of Virtual Work it must hold n  i=1

τ¯i δqi = 0

(3.70)

88

3 Dynamics of Rigid Robot Manipulators

if the virtual displacements are consistent with the constraints. Now, suppose that each equation (3.69) is multiplied by a so called Lagrange multiplier λ j , i.e. λj

n 

a ji δqi = 0

j = 1, . . . , m.

(3.71)

i=1

Quite obviously it is m  j=1

λj

n 

a ji δqi =

n  m 

i=1

λ j a ji δqi = 0,

(3.72)

i=1 j=1

where it has been taken advantage of the fact that the sums are interchangeable. Also, it is possible to add this relationship to (3.70) in order to get n  i=1

τ¯i δqi +

n  m 

λ j a ji δqi =

i=1 j=1

n 

⎛ ⎝τ¯i +

i=1

m 

⎞ λ j a ji ⎠ δqi = 0.

(3.73)

j=1

At this point the multipliers are completely arbitrary while on the contrary the virtual displacements δqi must comply with (3.69). However, if the multipliers are chosen to satisfy τ¯i = −

m 

λ j a ji

i = 1, . . . , n,

(3.74)

j=1

then (3.73) holds for any set δqi . Therefore, since τ¯i are generalized torques acting at the joints, they can be included directly in (3.44) as ⎡

a11 · · · ⎢ .. H(q)¨q + C(q, q˙ )˙q + D˙q + g(q) = τ − ⎣ .

⎤⎡ ⎤ am1 λ1 .. ⎥ ⎢ .. ⎥ . . ⎦⎣ . ⎦

a1n · · · amn

(3.75)

λm

By defining the vector of Lagrange multipliers λ ∈ Rm as ⎤ λ1 ⎢ ⎥ λ = ⎣ ... ⎦ , ⎡

(3.76)

λm

then it is possible to write (3.75) as H(q)¨q + C(q, q˙ )˙q + D˙q + g(q) = τ − J Tϕ (q)λ,

(3.77)

3.3 Inclusion of Environmental Forces

89

where J ϕ (q) = ∇ϕ(q).

(3.78)

∇ϕ(q) ∈ Rm×n denotes the gradient of the surface vector ϕ(q), which maps a vector onto the normal plane at the tangent plane that arises at the contact point described by (3.66). J ϕ (q) is assumed to be full rank. This must indeed be the case because J ϕ (q) provides the directions where it is possible to apply forces and having Rank(J ϕ (q)) < m would mean that the constraints are redundant because for that case at least two rows of J ϕ (q) would be linearly dependent. Also note that usually it is possible to normalize the constraints to get the rows of J ϕ (q) with unitary magnitude, so that each element of λ physically represents the force magnitude applied at the contact point in the direction given by the corresponding row of J ϕ (q). It is worthy to point out that λ can be computed in the following way. From (3.66) it is ˙ ϕ(q) = J ϕ (q)˙q = 0,

(3.79)

¨ ϕ(q) = J ϕ (q)¨q + J˙ ϕ (q)˙q = 0.

(3.80)

and

But from (3.77) one has from   q¨ = H −1 (q) τ − τ¯ − J Tϕ (q)λ ,

(3.81)

with τ¯ = C(q, q˙ )˙q + D˙q + g(q). Substituting in (3.80) it is   J ϕ (q)H −1 (q) τ − τ¯ − J Tϕ (q)λ + J˙ ϕ (q)˙q = 0.

(3.82)

Finally, the Lagrange multipliers are given by −1   J ϕ (q)H −1 (q){τ − τ¯ } + J˙ ϕ (q)˙q . λ = J ϕ (q)H −1 (q)J Tϕ (q)

(3.83)

Writing the constraints in joint space coordinates allows to develop in a direct way the computation of Lagrange multipliers, but in fact it is more straightforward to write them in work space coordinates as ϕ(x) = 0,

(3.84)

so that one also has J ϕx =

∂ϕ(x) ∂x

(3.85)

90

3 Dynamics of Rigid Robot Manipulators

meaning that J ϕ (q) = J ϕx J(q).

(3.86)

Therefore, model (3.77) can be rewritten as H(q)¨q + C(q, q˙ )˙q + D˙q + g(q) = τ − J T (q)J Tϕx λ.

(3.87)

Just by comparing with (3.65) it can be concluded that Fe = J Tϕx λ.

(3.88)

3.4 Model Properties Some useful properties regarding model (3.44) are presented in this section

3.4.1 Vectors and Matrices Properties One of the most important and better known properties of model (3.44) is the following. Property 3.1 Positive definiteness of H(q). The generalized inertia matrix H(q) is symmetric positive definite. Proof From (3.27) it is evident that H(q) is symmetric. On the other hand, the kinetic energy of any mechanical system can be zero only if it is at rest, i.e. if q˙ ≡ 0, so that from (3.26) the positive definiteness is granted.  Another property that can be exploited for control and observer design is the following. Property 3.2 By using the Christoffel symbols of the first kind to compute C(q, q˙ ), ˙ the matrix N(q, q˙ ) = H(q) − 2C(q, q˙ ) ∈ Rn×n is skew symmetric. ˙ Proof Each element of H(q) satisfies h˙ i j =

n  ∂h i j k=1

∂qk

q˙k

i, j = 1, . . . , n.

(3.89)

By taking into account (3.41) and (3.42) the elements of N(q, q˙ ) can be computed as

3.4 Model Properties

91 n   ∂h i j



∂h i j ∂h k j ∂h ik − + − ∂qk ∂qk ∂q j ∂qi k=1   n  ∂h k j ∂h ik q˙k . = − ∂q ∂q j i k=1

n i j = h˙ i j − 2ci j =

 q˙k

Since n ji =

n   ∂h ki

∂q j

k=1

−

∂h jk ∂qi

 q˙k ,

and h i j = h ji then it is n i j = −n ji , which concludes the proof.



It should be noted that Property 3.2 is based on the employment of the Christoffel symbols to compute C(q, q˙ ). Note that in general there may exist many matrices which satisfy (3.37) and for which the previous property does not hold. However, the following relationship is always satisfied: ˙ q˙ T (H(q) − 2C(q, q˙ ))˙q = 0.

(3.90)

To show this Hamilton’s equations of motion can be used. The first step consists in writing (3.31) and (3.44) as d ∂L ∂L − = ψ = H(q)¨q + C(q, q˙ )˙q + g(q), dt ∂ q˙ ∂q

(3.91)

ψ = τ − D˙q.

(3.92)

where

The Hamiltonian or Hamiltonian function is given by H = pT q˙ − L,

(3.93)

where the generalized momentum p is p=

∂L . ∂ q˙

(3.94)

From (3.30) it is ∂L = H(q)˙q. ∂ q˙

(3.95)

92

3 Dynamics of Rigid Robot Manipulators

Therefore, from (3.93) and (3.94), the Hamiltonian is given by 1 1 H = q˙ T H(q)˙q − q˙ T H(q)˙q + P = q˙ T H(q)˙q + P = K + P. 2 2

(3.96)

On the other hand, Hamilton’s canonical equations of motion can be expressed as ∂H ∂ pi ∂H p˙ i = − + ψi , ∂qi

q˙i =

(3.97) i = 1, . . . , n.

(3.98)

From (3.97) and (3.98) the time derivative of H is  ∂H  ∂H dH = q˙i + p˙ i dt ∂qi ∂ pi i=1 i=1 n

=

n

n n n  ∂H ∂H  ∂H ∂H  ∂H − + ψi ∂qi ∂ pi ∂ pi ∂qi ∂ pi i=1 i=1 i=1

= q˙ T ψ.

(3.99)

But the derivative can also be computed from (3.91) and (3.96): ∂P 1 ˙ dH = q˙ T H(q)¨q + q˙ T H(q)˙ q + q˙ T dt 2 ∂q 1 ˙ q + q˙ T g(q) = q˙ T (ψ − C(q, q˙ ) − g(q)) + q˙ T H(q)˙ 2  1 ˙ = q˙ T ψ + q˙ T H(q) − 2C(q, q˙ ) q˙ . 2

(3.100)

By comparing (3.99) and (3.100) one gets (3.90). In conclusion, Property 3.2 is a particular case of (3.90). Property 3.3 By using the Christoffel symbols of the first kind to compute C(q, q˙ ), ˙ the derivative of the inertia matrix satisfies H(q) = C(q, q˙ ) + CT (q, q˙ ). 

Proof It can be shown by direct computation as done for Property 3.2. Property 3.4 The vector of Coriolis and centrifugal torques hc (q, q˙ ) satisfies: hc (q, x, y) = C(q, x)y = C(q, y)x = hc (q, y, x)

∀x, y ∈ Rn .

(3.101)

3.4 Model Properties

93

Proof From (3.40) and (3.41), the i-element of hc (q, x, y) can be expressed as ⎛ ⎞ n n   ⎝ h ci (q, x, y) = ck ji xk ⎠ y j k=1

=

j=1

n 

 n 

j=1

k=1

 c jki y j

xk = h ci (q, y, x).



(3.102) 

3.4.2 Norm Related Properties Finding norm bounds for the different matrices and vectors of model (3.44) plays an important role in proving closed loop stability for control-observer design. In fact, for any physical system both q and q˙ must be bounded. Therefore, for simplicity’s sake this section makes the assumption that the manipulator owns only revolute joints, although the properties are also valid for robots with prismatic joints. Property 3.5 The inertia matrix H(q) satisfies λmin (H(q))y2 ≤ yT H(q)y ≤ λmax (H(q))y2

∀y ∈ Rn .

(3.103)

Proof Since H(q) is positive definite, each vector y in Rn can be expressed in terms of an orthonormal basis (y1 , . . . , yn ) as y = c1 y1 + · · · + cn yn which satisfies: yT H(q)y = c12 λ1 (H(q)) + · · · + cn2 λn (H(q))

(3.104)

yT y = y2 = c12 + · · · + cn2 ,

(3.105)





and

from where (3.103) holds.

Property 3.6 The inverse H−1 (q) exists and satisfies −1 2 T −1 2 λ−1 max (H(q))y ≤ y H (q)y ≤ λmin (H(q))y

∀y ∈ Rn .

Proof This property follows directly from Property 3.5.

(3.106) 

Property 3.7 The inertia matrix satisfies 0 < λh ≤ H(q) ≤ λH < ∞. Proof Since revolute joints are considered it is possible to conclude from (3.103) that

94

3 Dynamics of Rigid Robot Manipulators

λh = minn λmin (H(q))

λH = maxn λmax (H(q)).

q∈R

q∈R

(3.107) 

Property 3.8 H−1 (q) satisfies 0 < σh ≤ H−1 (q) ≤ σH < ∞. Proof The proof is the same as for Property 3.7 with σH = maxn λ−1 min (H(q)).

σh = minn λ−1 max (H(q)) q∈R

q∈R

(3.108) 

Property 3.9 The matrix C(q, y) satisfies C(q, y) ≤ kc y, kc > 0. Proof From (3.42) it can be seen that the matrix C(q, y) can be written as 1 Bk (q)yk , 2 k=1 n

C(q, y) =

(3.109)

where the i jth element of Bk (q) ∈ Rn×n is defined as ∂h i j ∂h k j ∂h ik + − i, j = 1, . . . , n. ∂qk ∂q j ∂qi Computing the norm of C(q, y) leads to   n n n  1 1 1   Bk (q)yk  ≤ Bk (q)yk  ≤ Bk (q) y. C(q, y) =   2 2  k=1 2 k=1 k=1 Since the matrices Bk (q) are functions of bounded variables, they are bounded so that Property 3.9 holds by defining: kc =

n  1 maxn Bk (q). 2 q∈R k=1

(3.110) 

Property 3.10 The matrix D is diagonal positive semidefinite and satisfies: λmin (D)y2 ≤ yT Dy ≤ λmax (D)y2

∀y ∈ Rn .

(3.111)

Proof D is positive semidefinite because it is defined based on the Rayleigh dissipation function:

3.4 Model Properties

95

D=

1 T q˙ D˙q. 2

(3.112)

 Assuming D to be diagonal is actually a particular but important case of this function. Equation (3.111) is valid because D is positive semidefinite.  Property 3.11 The vector of gravitacional torques satisfies g(q) ≤ kg , kg > 0. Proof The property follows directly from (3.33) by recalling that revolute joints are being considered.  Property 3.12 The vector of gravitational torques g(q) satisfies    ∂g(q)     ∂q  ≤ αg .

(3.113)

Proof Since revolute joints are being considered, the generalized coordinates q appear in g(q) only as argument of (bounded) sine and cosine functions, and that ∂g(q) must be the case for ∂q , from where the property follows.  Property 3.13 The vector of gravitational torques g(q) satisfies g(q1 ) − g(q2 ) ≤ αg q1 − q2  ∀q1 , q2 ∈ Rn .

(3.114)

Proof Property 3.13 follows from Property 3.12 by applying the Mean Value Theorem. Since g(q) is a continuous differentiable function and its partial derivatives with respect to q are bounded, then the following relationship must be satisfied for any two vectors q1 and q2 : g(q1 ) − g(q2 ) =

∂g(q) ∂q

(q1 − q2 ),

(3.115)

q12

where q12 is a point between q1 and q2 . By taking norms one gets:   ∂g(q)  g(q1 ) − g(q2 ) =   ∂q

q12

    ∂g(q)   (q1 − q2 ) ≤    ∂q

q12

    q1 − q2 , (3.116) 

so that (3.114) holds by choosing    ∂g(q)    αg = maxn  ∀q∈R ∂q 

(3.117) 

96

3 Dynamics of Rigid Robot Manipulators

3.4.3 Whole Model Related Properties This section provides two properties of model (3.44) as a whole. The first one is the based of adaptive control for robot manipulators, while the second one exploits a key physical characteristic. Property 3.14 With a proper choice of parameters, model (3.44) can be rewritten as H(q)¨q + C(q, q˙ )˙q + D˙q + g(q) = τ = Y(q, q˙ , q¨ )ϕ,

(3.118)

where Y(q, q˙ , q¨ ) ∈ Rn× p is known as the regressor, and ϕ ∈ R p is a vector of parameters.  The proof of Property 3.14 is omitted because it is based on the Newton-Euler equations of motion. Instead, consider the following example. Example

Consider the robot model given in (3.57)–(3.60) written as ! " 1 m l 2 + 4 m l 2 + m l 2c 1 m l 2 + 1 m l 2c   q¨1 1 2 2 2 2 2 2 3 3 3 2 + 1 m l 2 + 1 m l 2c 1 m l2 q ¨2 3 2 2 2 2 3 2 ⎡

⎤ − 21 m 2 l 2 s2 q˙2 − 21 m 2 l 2 s2 q˙1 − 21 m 2 l 2 s2 q˙2   ⎢ ⎥ q˙1 ⎣ ⎦ q˙ + 2 1 m l 2 s q˙ 0 2 2 1 2 ⎡

⎤ 1 m g lc + m g lc + 1 m g lc   1 0 1 2 0 1 2 0 12 2 τ1 ⎢2 ⎥ ⎣ ⎦= τ 2 1 2 m 2 g0 lc12 First of all it is necessary to find a vector ϕ. In general there are many possible choices of parameters, but the less the better. For instance it can be chosen ⎤ ⎡ ⎤ ⎡1 m 1l 2 + 43 m 2 l 2 ϕ1 3 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎢ ⎥ 2 ⎢ ϕ2 ⎥ ⎢ m l ⎥ 2 ⎢ ⎥ ⎢ ⎥ ⎥=⎢ ϕ=⎢ ⎥, ⎢ ⎥ ⎢ ⎥ ⎢ ϕ3 ⎥ ⎢ 1m g l ⎥ ⎢ ⎥ ⎢ 1 0 ⎥ 2 ⎣ ⎦ ⎣ ⎦ ϕ4 m 2 g0 l

3.4 Model Properties

97

so that the robot model can be rewritten as ⎡

⎤ ⎤ ⎡ 1ϕ + 1ϕ c   − 1 ϕ2 s2 q˙2 − 21 ϕ2 s2 q˙1 − 21 ϕ2 s2 q˙2   3 2 2 2 2 ⎥ q¨1 ⎥ q˙1 ⎢ 2 ⎦ q¨ + ⎣ ⎦ q˙ 2 2 1ϕ + 1ϕ c 1ϕ 1 ϕ s q˙ 0 2 2 2 2 2 2 1 3 2 3 2 "   ! τ ϕ3 c1 + ϕ4 c1 + 21 ϕ4 c12 = 1 . 1 τ2 ϕ c 4 12 2

⎢ ⎣

ϕ1 + ϕ2 c2

Finally, the regressor can be computed as ⎡ ⎢ ⎢ ⎣ 

q¨1 0

$ $ # # c2 q¨1 + 31 + 21 c2 q¨2 − 21 s2 q˙2 q˙1 − 21 s2 q˙1 + 21 s2 q˙2 q˙2 # $ 1 + 1 c q¨ + 1 q¨ + 1 s q˙ q˙ 2 1 3 2 3 2 2 2 1 1  Y(¨q,˙q,q)

c1 0

⎤ c1 + 21 c12 ⎥ ⎥ ϕ = τ. ⎦ 1c 12 2

Property 3.15 The dynamic equation of motion of a robot manipulator given by (3.44) defines a passive mapping ψ → q˙ , i.e. T < q˙ , ψ >T =

q˙ T ψdt ≥ −β,

(3.119)

0

for some β > 0 and ∀ T > 0, with ψ = τ − D˙q ∈ Rn . Proof Recall that the derivative of the system’s Hamiltonian is given by (3.99) dH = q˙ T ψ ⇒ < q˙ , ψ >T = dt

T

T q˙ ψdt =

dH = H(T ) − H(0) ≥ −H(0).

T

0

0

Since H(T ) is not negative for all T , Property 3.15 follows with β = H(0). Note that alternatively it can be done T q˙ T ψdt

< q˙ , ψ >T = 0

=

T #

$ q˙ T H(q)¨q + q˙ T C(q, q˙ )˙q + q˙ T g(q) dt

0

T  =

  1 d T 1 ˙ q˙ H(q)˙q − q˙ T H(q) − 2C(q, q˙ ) q˙ + q˙ T g(q) dt 2 dt 2

0

T  = 0

d dt



  1 T q˙ H(q)˙q + gT (q)˙q dt. 2

98

3 Dynamics of Rigid Robot Manipulators

Note that to arrive to the last relationship (3.90) was used, showing that (3.90) is closely related to the passivity property. On the other hand, from (3.34) it is known that2 gT (q) =

∂ P . ∂q

Therefore it is gT (q)˙q =

∂ P ˙ q˙ = P. ∂q

By considering (3.26), it can be written T < q˙ , ψ >T =

d (K + P) dt dt

0

= (K + P)|0T = H|0T = H(T ) − H(0) ≥ −H(0).



3.4.4 Holonomic Constraints Properties When the robot end-effector is in contact with its environment and this can be described by the holonomic constraint (3.84), then for model (3.87) the following property holds. Property 3.16 The vector x˙ can be written as x˙ = Qx (x)˙x + Px (x)˙x = Qx (x)˙x,

(3.120)

  + T T −1 where Qx (x) = (In×n − Px (x)), Px (x) = J+ ∈ Rn×m ϕx Jϕx , and Jϕx = Jϕx Jϕx Jϕx stands for the Penrose’s pseudoinverse and Qx ∈ Rn×n satisfies rank (Qx ) = n − m. These two matrices are orthogonal, i.e. Qx Px = O (and in fact Qx JTϕx = O and 2

Recall that in (3.34) some abuse of notation was made by omitting the transpose which is included here for convenience.

3.4 Model Properties

99

˙ Jϕx Qx = O). Note that the last equality in (3.120) is due to the fact that ϕ(x) = Jϕx x˙ = 0 in view of constraint (3.84). Finally, it holds Qx (x)Qx (x) = Qx (x)

and

Px (x)Px (x) = Px (x).

(3.121) 

3.5 Inclusion of DC Motors in the Robot Dynamic Model The generalized input torque τ given in models (3.44) or (3.65) is given without considering at all the physical actuators. In this section, the inclusion of DC motors dynamics will be considered because this is the kind of actuators provided in all the robots employed for experiments in this book. Figure 3.4 shows how a DC motor can be connected to the ith joint of a robot manipulator, for i = 1, . . . , n. The physical quantities involved in the ith motor dynamics are the following: K ai Rai L ai K bi τmi i ai ebi qmi qi ri Vi

motor-torque constant (Nm/A) armature resistence () armature inductance (H) back emf constant (Vs/rad) torque at the motor axis (Nm) armature current (A) back emf (V) angular position of the motor axis (rad) angular position of the mechanical load axis (rad) gears reduction ratio (in general ri >> 1) armature voltage (V)

The mechanical load for the present study is the ith link of the robot. The different variables and constants can be related through the following relationships

Gearbox

Fig. 3.4 Schematic connection of a DC motor used as actuator for a robot joint

JL Link qi fLi

Jm qmi Motor

fmi

1 : ri

100

3 Dynamics of Rigid Robot Manipulators

τmi = K ai i ai

(3.122)

di ai + ebi Vi = Rai i ai + L ai dt ebi = K bi q˙mi qmi = ri qi .

(3.123) (3.124) (3.125)

Note that the armature inductance L ai is usually assumed to be negligible (L ai ≈ 0), so that (3.123) can be simplified. On the one hand, the motor’s equation of motion can be written as τi (3.126) Jmi q¨mi = τmi − f mi (q˙mi ) − , ri where τi is the applied net torque after the gear reduction on the mechanical load axis (i.e. the generalized input torque in the robot modeling), Jmi is the rotor inertia and f mi (q˙mi ) represents friction effects. By recalling that L ai ≈ 0, i ai can be substituted from (3.122) into (3.123) and the resulting τmi can be substituted from (3.123) into (3.126) to get K ai τi K ai ebi + = Vi . Rai ri Rai

(3.127)

K ai τi K ai K bi ri q˙i + = Vi , Rai ri Rai

(3.128)

1 K ai K bi τi K ai f mi (ri q˙i ) + q˙i + 2 = Vi . ri Rai ri Rai ri

(3.129)

Jmi q¨mi + f m (q˙mi ) + Using (3.124) and (3.125) leads to ri Jmi q¨i + f mi (ri q˙i ) + or Jmi q¨i +

Assume now that only joint viscous friction effects are present so that it is f mi (q˙mi ) = f mi q˙mi ,

(3.130)

where f mi is the joint viscous constant that can eventually be neglected. With this simplification (3.129) becomes % Jmi q¨i +

f mi

K ai K bi + Rai

& q˙i +

τi K ai = Vi , 2 r ri i Rai

(3.131)

for i = 1, . . . , n. In compact matrix form one has Dj q¨ + Df q˙ + Dn τ = DK V, with

(3.132)

References

101

Dj = diag{J $( ' mi } # Df = diag f mi + K aiRK bi ai % & 1 Dn = diag 2 i ( '# r$ K ai 1 DK = Rai ri Therefore, the generalized input torque vector is given by   τ = D−1 DK V − Dj q¨ − Df q˙ , n

(3.133)

which can be substituted in the robot model. For example, consider (3.65) to obtain   DK V − Dj q¨ − Df q˙ − J T (q)Fe . (3.134) H(q)¨q + C(q, q˙ )˙q + D˙q + g(q) = D−1 n It is possible to rewrite (3.134) as     ¨ + C(q, q˙ )˙q + D + D−1 ˙ + g(q) = H(q) + D−1 n Dj q n Df q T D−1 n DK V − J (q)F e .

(3.135)

It is not difficult to show that all the properties given in Sect. 3.4 still hold, although care must be taken since for Property 3.14 the generalized input has become D−1 n DK V and, in general, Dn and DK must be assumed to be known since different constant values contained there do not form part of the parameter vector ϕ.

References An CH, Atkeson CG, Hollerbach JM (1985) Estimation of inertial parameters of rigid body links of manipulators. In: Proceedings of 24th IEEE conference on decision and control, Ft. Lauderdale, FL, USA, Dec 1985, pp 990–995 Arteaga-Pérez MA (1998) On the properties of a dynamic model of flexible robot manipulators. ASME J Dyn Syst Meas Control 120:8–14 Atkeson CG, An CH, Hollerbach JM (1985) Rigid body load identification for manipulators. In: Proceedings of 24th IEEE conference on decision and control, Ft. Lauderdale, FL, USA, Dec 1985, pp 996–1002 Beer FP, Johnston ER Jr, Mazurek DF, Cornwell PJ (2013) Vector mechanics for engineers: statics and dynamics, 10th edn. McGraw-Hill, USA Feng W, Postlethwaite I (1993) A simple robust control scheme for robot manipulators with only joint position measurements. Int J Robot Res 12(5):490–496 Fu KS, Gonzalez RC, Lee CSG (1987) Robotics: control, sensing, vision, and intelligence. McGrawHill, USA Greenwood DT (1997) Classical dynamics. Dover Publications, INC, Mineola, New York Kelly R, Santibáñez V, Loria A (2005) Control of robot manipulators in joint space. Springer, London Meirovitch L (1967) Analytical methods in vibrations. The Macmillan Company, New York, USA

102

3 Dynamics of Rigid Robot Manipulators

Murray RM, Li Z, Sastry SS (1994) A mathematical introduction to robotic manipulation. CRC Press, Boca Raton, FL, USA Ortega R, Spong MW (1989) Adaptive motion control of rigid robots: a tutorial. Automatica 25(6):877–888 Spong MW, Hutchinson S, Vidyasagar M (2006) Robot modeling and control. Wiley, USA Strang G (2006) Linear algebra and its applications, 4th edn. Thompson Brooks, USA Wellstead PE (2005) Introduction to physical system modelling. Control systems principles. https:// www.control-systems-principles.co.uk

Chapter 4

Mathematical Background

Abstract Robot manipulators are highly nonlinear dynamic systems. When a desired task has to be accomplished, there are many issues to be solved to get the best possible performance. First of all the task has to be planned and expressed in such a way that it can be executed by the robot. Perhaps the most common practice consists in defining particular desired trajectories or references for each robot joint and then implement a control algorithm to follow these trajectories. This may well imply the computation of inverse kinematics online, but assuming that this is done, then the most relevant challenge becomes the design of a control scheme. Also the most common approach is to define an error between desired and actual trajectories, so that if it vanishes then the task is accomplished. There are many analytical tools for control design, each of them with pros and cons, e.g. passivity or Lyapunov theory. In this section, we focus on the latter and provide the reader with the most basic definitions and theorems that will be employed in the rest of the book in the solution of many different problems.

4.1 Basic Definitions and Lemmas The goal of this chapter is to provide the basic theory necessary to develop all the control and observer schemes in the next chapters. Although this makes the book self contained, by no means it can be considered as a complete background of analytical tools since most of the results are given without proof and the interested reader should look into the references for a deeper insight. However, sketches of the proofs of the main theorems are provided in order to better understand the control-observer design proposed in this work. This section lists some very well known basic definitions, concepts and important remarks. Definition 4.1 Consider the n-dimensional vector x=[x1 · · · xn ]T , where the scalars x1 , . . . , xn belong to R, then the set of all vectors define the n-dimensional Euclidean space denoted by Rn .  Definition 4.2 A set of vectors xi (t) ∈ Rn , i = 1, . . . , p are said to be linearly independent on the interval [t1 , t2 ] if and only if © The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control of Robot Manipulators, Lecture Notes in Electrical Engineering 798, https://doi.org/10.1007/978-3-030-85980-0_4

103

104

4 Mathematical Background p 

xi (t)αi = 0, t ∈ [t1 , t2 ],

(4.1)

i=1

holds only for the trivial solution αi = 0, ∀ i = 1, . . . , p. Otherwise, the vectors are  said to be linearly dependent on the interval [t1 , t2 ]. According to this definition, the vectors xi ∈ Rn are linearly independent if it is not possible to express any of them as a combination of the others. Also, if they are constant the interval [t1 , t2 ] can be dropped in Definition 4.2. This concept can be expressed in a more compact form by using a matrix X ∈ Rn× p   X = x1 , . . . , x p

(4.2)

and a vector α ∈ R p . According to Definition 4.2, the columns of X(t) are linearly independent on the interval [t1 , t2 ] if and only if X(t)α = 0

⇒

α = 0, t ∈ [t1 , t2 ].

(4.3)

For a constant matrix X their columns are linearly independent if and only if Xα = 0

⇒

α = 0.

(4.4)

Theorem 4.1 The columns of a matrix X ∈ Rn× p are linearly independent on the interval t ∈ [t1 , t2 ] if and only if the matrix 1 B= t2 − t1

t2 XT (t)X(t)dt

(4.5)

t1

is not singular. Additionally, the rows of X ∈ Rn× p are linearly independent on the interval t ∈ [t1 , t2 ] if and only if the matrix B=

1 t2 − t1

t2 X(t)XT (t)dt

(4.6)

t1

is not singular.



Definition 4.3 The norm x of a vector is a real-value function satisfying: • x ≥ 0 ∀x ∈ Rn , with x = 0 if and only if x = 0 • Triangle inequality: x + y ≤ x + y ∀x, y ∈ Rn • αx = |α|x ∀α ∈ R, ∀x ∈ Rn In this book, x stands for the standard Euclidean norm if x is a vector, i.e.



4.1 Basic Definitions and Lemmas

105

1   1 x = x12 + · · · + xn2 2 = xT x 2 ,

(4.7)

while ||A|| denotes the corresponding induced 2-norm for a matrix A    1 A = max λi AT A 2 ,

(4.8)

i

where λi denotes the i-th eigenvalue for i = 1, . . . , n. For any function f (t) : [0, ∞) → Rn , the L∞ -norm is defined as f (t)∞ = sup f (t).

(4.9)

t≥0

The L2 -norm is defined as 

∞

f (t)2 =

f (t) dt 2

21

.

(4.10)

0

Definition 4.4 Let f (t, x) be piecewise continuous in t. f (t, x) is locally Lipschitz in x on [t1 , t2 ] × D ⊂ R × Rn if each point x ∈ D has a neighborhood D0 such that f (t, x1 ) − f (t, x2 ) ≤ L 0 x1 − x 2 

(4.11)

holds on [t1 , t2 ] × D0 with some Lipschitz constant L 0 . f (t, x) is locally Lipschitz in x on [t0 , ∞) × D if it is locally Lipschitz in x on [t1 , t2 ] × D for every compact interval [t1 , t2 ] ⊂ [t0 , ∞). f (t, x) is Lipschitz in x on [t0 , t1 ] × D if (4.11) is satisfied with the same Lipschitz constant L for all points x ∈ D and for all t ∈ [t1 , t2 ]. The  term globally Lipschitz is used in case D = Rn . The Lipschitz condition (4.11) is related to the existence and uniqueness of the solution of differential equations, but for the systems under study in this book such solutions always exist. However, this condition is also necessary to guarantee stability in the sense of Lyapunov. Definition 4.5 It is said that a continuous function α : [0, a) → [0, ∞) belongs to 1. The class K if it is strictly increasing and α(0) = 0 2. The class K∞ if a = ∞ and α(r ) = ∞ if r → ∞



Definition 4.6 It is said that a continuous function β : [0, a) × [0, ∞) → [0, ∞) belongs to the class KL, if for each fixed q, β( p, q) belongs to the class K with respect to p and, for each fixed p, β( p, q) is decreasing with respect to q and β( p, q) → 0 as q → ∞.  Remark 4.1 It can be shown that if α1 (·) is a class K function on [0, a), then the inverse α1−1 (·) exists, it is defined on [0, α1 (a)) and belongs to the class K as well. Furthermore if α2 (·) is another class K function on [0, a), then the composition α1 ◦

106

4 Mathematical Background

α2 belongs to the class K as well. The very same conclusions can be drawn for class K∞ functions. Finally, if β(·, ·) is a class KL function σ ( p, q) = α1 (β(α2 ( p), q)) belongs to the class KL as well.  Definition 4.7 A scalar continuous function V (t, x) is said to be 1. Locally positive definite in the domain D ⊂ Rn which contains the origin if V (t, 0) = 0

(4.12)

holds for all t ≥ t0 , and there exits a class K function α1 (x) such that α1 (x) ≤ V (t, x)

(4.13)

holds for all x ∈ D and for all t ≥ t0 . 2. Decrescent if there exits a class K function α2 (x) such that V (t, x) ≤ α2 (x)

(4.14)

holds for all x ∈ D and for all t ≥ t0 . 3. Positive definite if (4.12)–(4.13) hold for D = Rn and α1 (x) belongs to the class K∞ . 4. Negative definite in the domain D if −V (t, x) is positive definite. 5. Positive semi-definite in the domain D if it is non-negative there. 6. Negative semi-definite in the domain D if it is non-positive there. 7. Indefinite in the domain D if it does not have definite sign.  Remark 4.2 A much simpler definition of a positive definite function implies changing the class K function α1 (x) by a function W1 (x) satisfying i) W1 (0) = 0, ii) W1 (x) > 0 for x = 0. However, it is always possible to find a class K function α1 (x) so that α1 (x) ≤ W1 (x) holds. A similar W2 (x) can be employed in (4.14), but for that case a class K function can always be found so that W2 (x) ≤ α2 (x) holds. Therefore, conditions (4.13) and (4.14) could be redefined in terms of W1 (x) and W2 (x), respectively. Finally, if V (t, x) is positive definite as given in Definition 4.7.3 then it is also said to be radially unbounded. In terms of W1 (x) one would have  W1 (x) → ∞ as x → ∞. Lemma 4.1 (Comparison lemma) Let x˙ = f (t, x) x(t0 ) = x0 ,

(4.15)

where f (t, x) is continuous in t and locally Lipschitz in x, for all t ≥ t0 and all x ∈ D ⊂ R. Assume that [t0 , t1 ) is the maximal interval of existence of the solution x(t) such that x(t) ∈ D. Let y(t) be a continuous function whose upper right-hand derivative D + y(t) satisfies

4.1 Basic Definitions and Lemmas

107

D + y(t) ≤ f (t, y(t)) y(t0 ) ≤ x0

(4.16)

with y(t) ∈ D for all t ∈ [t0 , t1 ). Then, y(t) ≤ x(t) for all t ∈ [t0 , t1 ).



Lemma 4.2 Consider the scalar differential equation x(t) ˙ = −α1 (x(t)) x(t0 ) = x0 ,

(4.17)

where α1 (x) is a locally Lipschitz class K function defined on [0, a). For any x0 ∈ [0, a), the unique solution x(t) of (4.17) satisfies x(t) = β(x0 , t − t0 ) ∀ t ≥ t0 ,

(4.18)

where β(·, ·) is a class KL function [0, a) × [0, ∞).



Lemma 4.3 For any vector signals x, y, any variable time delay 0 ≤ T (t) ≤ T < ∞ and any constant α > 0, it holds t

σ xT (σ )

− 0

2

y(θ ) dθ dσ ≤

σ −T (σ )

T α x22 + y22 . 2 2α

(4.19)

 Lemma 4.4 For any x(t) : R ≥ t0 → Rn , if x(t) ∈ L∞ ∩ L2 and x˙ (t) ∈ L∞ , then lim x(t) = 0.

t→∞

(4.20) 

4.2 Stability in the Sense of Lyapunov 4.2.1 Definition Consider the vector differential equation x˙ (t) = f (t, x(t)) t ≥ t0 ,

(4.21)

where x(t) ∈ Rn is the state and f : R+ × Rn → Rn . It is understood that a solution of (4.21) in an interval [t0 , t1 ] is a continuous function x : [t0 , t1 ] → Rn such that x˙ (t) is defined and x˙ (t) = f (t, x(t)) for all t ∈ [t0 , t1 ]. We assume that there exist a unique solution over [t0 , ∞) for each initial condition x0 = x(t0 ) and that it depends continuously on x0 , as for f satisfying a global Lipschitz condition. Figure 4.1 shows a phase-plane plot of the solution of a vector differential equation for n = 2, i.e.

108

4 Mathematical Background

x2

Fig. 4.1 Possible trajectory for the solution of a vector differential equation

x(t0 ) x1 x(t1 )

x(t) ∈ R2 . Throughout the rest of the chapter all phase-plane are depicted for R2 as representative examples, but the theory is completely valid for Rn . A particular x0 for which f (t, x0 ) ≡ 0,

(4.22)

is called an equilibrium point. Both for simplicity’s sake and because it is conceptually the most important case for the development of the rest of the book, we assume that 0 is an equilibrium point of (4.21). Note that this does not imply any lost of generality and that there may well exist more than one single equilibrium point for a particular system. The name of equilibrium point is well chosen because if x(t0 ) = 0 then x(t) ≡ 0 for all t ≥ t0 . But the natural question is, what happens if x(t0 ) = 0? Consider the following definition. Definition 4.8 The equilibrium point 0 at time t0 of (4.21) is said to be 1. Stable at time t0 if, for each  > 0, there exists a δ(t0 , ) > 0 such that x(t0 ) < δ(t0 , ) ⇒ x(t) < , ∀t ≥ t0

(4.23)

2. Uniformly stable over [t0 , ∞) if, for each  > 0, there exists a δ() > 0 such that x(t0 ) < δ() ⇒ x(t) < , ∀ t ≥ t0 3. Asymptotically stable at time t0 if 1. It is stable at time t0 , and 2. There exists a δ1 (t0 ) > 0 such that

(4.24)

4.2 Stability in the Sense of Lyapunov

109

x(t0 ) < δ1 (t0 ) ⇒ x(t) → 0 as t → ∞

(4.25)

 Bδ1 (t0 ) = x ∈ Rn : x < δ1 (t0 )

(4.26)

The set

is called a region of attraction for the equilibrium 0. 4. Uniformly asymptotically stable over [t0 , ∞) if 1. It is uniformly stable over [t0 , ∞), and 2. There exists a δ1 > 0 such that x(t0 ) < δ1 ⇒ x(t) → 0 as t → ∞

(4.27)

Moreover, the convergence is uniform with respect to t0 . 5. Globally asymptotically stable if x(t) → 0 as t → ∞ (regardless of what x(t0 ) is, i.e. the region of attraction is the whole Rn ).  6. Unstable if it is not stable at time t0 . Remark 4.3 Let us try to understand the previous definitions for our advantage in the control-observer design of robot manipulators. First of all, the very first definition should be understood in the following way: if it is not wished the norm of the solution of (4.21), i.e. x(t), to become larger than a predefined value  at any time, does there exist a bound for the initial state x(t0 ) so that if smaller than this value the norm of the state will never be larger than ? This bound is designated as δ(t0 , ) in Definition 4.8.1, and it is very important to understand that it is not chosen first and then  is computed, but exactly the other way around, if possible, i.e. if the equilibrium is stable. Therefore, it is possible to propose a priori a region D as 

D = x ∈ Rn \ x <  ,

(4.28)

as a region of interest to work in it and then to try to find the initial conditions for which the equilibrium is stable. Note that it is rather a common practice to infer  as a function of x(t0 ) and that this does not necessarily imply a mistake but usually it proves to be a more involved procedure. Figure 4.2 illustrates the stability concept.  The bound δ used to define stability in the sense of Lypaunov depends in general not only on the predefined value  but also on the initial time t0 . However, if it is possible to find a bound for the initial condition which does not depend on t0 for any t1 ≥ t0 , then Definition 4.8.2 holds, i.e. the equilibrium point is uniformly stable. This is illustrated in Fig. 4.3. Till now all what stability of an equilibrium point implies is that the state will be bounded for all time as long as it is possible to find a bound for the initial condition small enough according with the desired bound for the state. However, Definition 4.8.3 makes a stronger implication in case the system is stable (not yet

110

4 Mathematical Background

x2 x(t) δ(t0 , ) x1

x(t0 ) 

Fig. 4.2 Stable equilibrium point

uniformly), and that is that the trajectories eventually will go back to the equilibrium point. Condition 32 of Definition 4.8.3 defines the very important concept of regions of attraction and it states that if the initial state belongs to the region of attraction, then it ultimately will return to the origin. Therefore, intuitively Condition 31 appears to be unnecessary. However, this intuition is wrong. This can easily be understood by considering a system whose trajectories behave as shown in Fig. 4.4. In fact, just because x0 ∈ Bδ1 (t0 ) it does not mean that x(t) ∈ Bδ1 (t0 ) for all t ≥ t0 . The trajectory perfectly well can leave the region, but it will ultimately reenter it and tend to the origin. If it is chosen  = δ1 then the system will not be stable but x(t) will remain bounded and tend to zero anyway. Of course, for some applications this might just be good enough, but still technically speaking the equilibrium point will not be stable. Definition 4.8.4 makes the stability concept independent of t0 as before and it is easy to understand in view of the previous item. One of the most beloved concepts in control theory is global asymptotic stability, given in Definition 4.8.5. It simply states that the trajectory will tend to the origin regardless the initial value x0 . There are many reasons for which global stability is desirable. First of all it implies that the system has one single equilibrium point and second all the system trajectories tend to it. This is in contrast to the previous definitions, which deal just with the trajectories in a vicinity of the equilibrium point, however large this vicinity may be. When it comes to design a controller or an observer for robot manipulators (or any other physical system) it can prove to be very hard to get global stability. The main premise of this book is that stability, or local stability as it is also called, can be good enough in practical applications and thus is a valuable goal on its own that can be easier to achieve.

4.2 Stability in the Sense of Lyapunov

111

x2 x(t) δ() x1

x(t0 ) 

Fig. 4.3 Uniformly stable equilibrium point

Finally, Definition 4.8.6 states that if none of the previous definitions hold, then the equilibrium point is unstable.

4.2.2 Main Stability Theorem This section presents a very well known result regarding stability in the sense of Lyapunov. As usual, only the information necessary for the rest of the book is provided so that many important results are excluded. Consider once again the vector differential equation (4.21) x˙ (t) = f (t, x(t)) t ≥ t0 , and suppose that for a domain D ⊂ Rn which contains the origin it is possible to find a continuously differentiable function V (t, x) : [0, ∞) × D → R satisfying α1 (x) ≤ V (t, x) ≤ α2 (x),

(4.29)

∀ t ≥ t0 and ∀ x ∈ D, where α1 and α2 are class K functions. According to Definition 4.7, V (t, x) is both (locally) positive definite and decrescent. Note that while V (t, x) may or may not depend explicitly on time t, neither α1 (x) nor α2 (x)

112

4 Mathematical Background

x2

x(t) x(t0 ) x1 δ=

Fig. 4.4 A bounded state tending to the equilibrium point does not necessarily means asymptotic stability

V (t, x)

x2

α2 (x) x

D

V (t, x)

x1 α1 (x) t0

t1

t

Fig. 4.5 V (t, x) bounded by α1 (x) and α2 (x) without considering (4.21) and the equilibrium x=0

do. This is a key characteristic. Figure 4.5 depicts the concept of V (t, x) bounded by α1 (x) and α2 (x). It is important to note that Fig. 4.5 is not completely correct because it shows bounds without making any association between (4.21) and (4.29) at all. In fact, by assumption x = 0 is an equilibrium point of (4.21), which means that if at t = t1 x ≡ 0 then x ≡ 0 for all t ≥ t1 .

4.2 Stability in the Sense of Lyapunov

113

V (t, x)

x2

α2 (x) x V (t, x)

D

x1 α1 (x) t1

t0

t

Fig. 4.6 V (t, x) bounded by α1 (x) and α2 (x) by considering (4.21) and the equilibrium x = 0 Fig. 4.7 The derivative of V˙ (t, x) indicates whether V (t, 0) increases, stays or decreases

V (t, x) V˙ > 0

V˙ = 0 V˙ < 0

t0

t1

t

Figure 4.6 shows the relationship between (4.21) and (4.29) correctly. Note that for that case α1 (0) = V (t, 0) = α2 (0) = 0 for all t ≥ t1 . The question arises whether one can conclude any kind of stability from this relationship. In the figures, V (t, 0) going down to zero apparently indicates asymptotic stability, but how to know whether V (t, 0) is going down? Recall that V (t, 0) is never negative. Of course, by paying attention to the derivative V˙ (t, x) as shown in Fig. 4.7. The derivative of V (t, x) can be computed in general as ∂V ∂V + f (t, x), V˙ (t, x) = ∂t ∂x

(4.30)

where the dependence on (4.21) becomes evident. Prior to drawing out a conclusion about stability, it should be recalled that just because x → 0 it does not mean that the equilibrium is stable as defined in Definition 4.8 (see Fig. 4.4). Therefore, the stability analysis is more complex than just considering the sign of the derivative V˙ (t, x). The first step is to take into account that the region D does not need to be a ball as given in (4.28) because in fact it is always possible to take an  > 0 to define a ball contained in D as

114

4 Mathematical Background

x2

B  x1 D

Fig. 4.8 B ∈ D

 B = x ∈ Rn | x ≤  ⊂ D.

(4.31)

This is shown in Fig. 4.8. In practice uniform stability is sought. This means that it must be possible to find a δ() such that (4.24) holds. Choose c < α1 (),

(4.32)

i.e. choose c strictly smaller than α1 (x) evaluated at the boundary of the ball B . Since α1 (x) is a class K function, then the region defined as D1,c = {x ∈ B | α1 (x) ≤ c}

(4.33)

must be contained in B as depicted in Fig. 4.9. In the same fashion a time-dependent region can be defined as Dt,c = {x ∈ B | V (t, x) ≤ c} .

(4.34)

But if V (t, x) ≤ c then α1 (x) ≤ c, so that it can be concluded that Dt,c ⊂ D1,c because every x ∈ Dt,c is also in D1,c but not necessarily vice versa. This is shown in Fig. 4.10. Finally, another region can be defined as

4.2 Stability in the Sense of Lyapunov

115

x2

B

x1 D1,c

D

Fig. 4.9 D1,c ⊂ B ∈ D

x2

B

Dt,c x1 D1,c

Fig. 4.10 Dt,c ⊂ D1,c ⊂ B ∈ D

D

116

4 Mathematical Background

x2

B Dt,c x1

D2,c D1,c

D

Fig. 4.11 D2,c ⊂ Dt,c ⊂ D1,c ⊂ B ⊂ D

D2,c = {x ∈ B | α2 (x) ≤ c} .

(4.35)

For this case, if α2 (x) ≤ c then of course V (t, x) ≤ c, which means that D2,c ⊂ Dt,c as shown in Fig. 4.11. Therefore, it is possible to arrive to the following conclusion D2,c ⊂ Dt,c ⊂ D1,c ⊂ B ⊂ D,

(4.36)

for all t ≥ t0 just based on (4.29). Note that the time-dependent region Dt,c is surrounded by two time-independent surfaces. Assume now that V˙ (t, x) ≤ 0

(4.37)

in D. Then, any trajectory beginning in Dt0 ,c must remain in Dt,c for all t ≥ t0 because for that case V (t, x) ≤ V (t0 , x0 ) ≤ c ∀t ≥ t0 .

(4.38)

Therefore, any solution starting in D2,c remains in Dt,c for all t ≥ t0 as well and, as a direct consequence, in D1,c ⊂ B for all t ≥ t0 . This necessarily implies that x(t) <  for all t ≥ t0 , which will comply with Definition 4.8.2 as long as a δ() > 0 can

4.2 Stability in the Sense of Lyapunov

117

be found to have x(t0 ) < δ() as well. By noting that the condition to define D2,c in (4.35) can be changed to x ≤ α2−1 (c), choosing δ() = α2−1 (c) < α2−1 (α1 ())

(4.39)

implies that x(t0 ) ∈ D2,c . Although this choice excludes the boundary of D2,c , this is of little relevance. Therefore, it has been shown that the equilibrium x = 0 is uniformly stable. To prove asymptotic stability, the assumption given in (4.37) should be changed by V˙ (t, x) ≤ −W3 (x),

(4.40)

where W3 (x) is a continuous positive definite function on D. Note that in fact there exists a class K function α3 (x) such that V˙ (t, x) ≤ −W3 (x) ≤ −α3 (x),

(4.41)

holds, as highlighted in Remark 4.2. From (4.29) one gets V (t, x) ≤ α2 (x) ⇒ α2−1 (V (t, x)) ≤ x,

(4.42)

from where it is possible to get α3 (α2−1 (V (t, x))) ≤ α3 (x)

(4.43)

because α3 (x) is a class K function. Define a new class K function as α4 (·) = α3 (α2−1 (·)) to obtain from (4.41) V˙ (t, x) ≤ −W3 (x) ≤ −α4 (V (t, x)).

(4.44)

Without any loss of generality it can be assumed that α4 (·) is locally Lipschitz. If it were not the case, then another class K function α5 (·) satisfying α5 (·) ≤ α4 (·) can always be found which is locally Lipschitz. Consider now the following differential equation y˙ = −α4 (y) y0 = V (t0 , x0 ),

(4.45)

y(t) = β1 (V (t0 , x0 ), t − t0 ) ∀ t ≥ t0 ,

(4.46)

whose solution satisfies

according to Lemma 4.2. β1 (·, ·) is a class KL function defined on [0, c] × [t0 , ∞), where c has been used to define Dt,c in (4.34). By the Comparison Lemma 4.1 it is V (t, x) ≤ β1 (V (t0 , x0 ), t − t0 ) ∀ t ≥ t0 ,

(4.47)

118

4 Mathematical Background

with V (t0 , x0 ) ∈ [0, c]. Therefore, any solution starting in D2,c satisfies x ≤ α1−1 (V (t, x)) ≤ α1−1 (β1 (V (t0 , x0 ), t − t0 )) ≤ α1−1 (β1 (α2 (x0 ), t − t0 )) = β(x0 , t − t0 )

(4.48)

for all t ≥ t0 and where β(·, ·) is a class KL function as pointed out in Remark 4.1. According to Definition 4.8.4 the equilibrium is uniformly asymptotically stable if it is uniformly stable over [t0 , ∞), which has already been shown, and if there exists a δ1 > 0 such that x(t0 ) < δ1 ⇒ x(t) → 0 as t → ∞. By choosing also δ1 = α2−1 (c) as in (4.39) and by noting that β(x0 , t − t0 ) → 0 as t → ∞ because it is a class KL function, it can be concluded that the equilibrium is uniformly asymptotically stable. Finally, if D = Rn and if the functions α1 (·) and α2 (·) belong to the class K∞ , then the equilibrium is globally uniformly asymptotically stable, because α1 (·) is radially unbounded and c in (4.32) can be defined arbitrarily large because  can also be considered arbitrarily large, so that δ() = α2−1 (c) becomes arbitrarily large as well to include any initial condition x(t0 ). Note that α3 (·) becomes a class K∞ function because W3 (x) is defined in Rn . The following theorem summarizes the preceding discussion. Theorem 4.2 Consider the nonautonomous system (4.21), i.e. x˙ (t) = f(t, x(t)) t ≥ t0 , where f : [0, ∞) × D → Rn is piecewise continuous in t and locally Lipschitz in x on [0, ∞) × D, where D ⊂ Rn is a domain that contains the origin x = 0, which is assumed to be an equilibrium point of (4.21). Let V (t, x) : [0, ∞) × D → R be a continuously differentiable function satisfying (4.29), i.e. α1 (x) ≤ V (t, x) ≤ α2 (x), where α1 (·), α2 (·) are class K functions. Then, the equilibrium is 1. Uniformly stable if (4.37) holds, i.e. if V˙ (t, x) ≤ 0. 2. Uniformly asymptotically stable if (4.40) holds, i.e. if V˙ (t, x) ≤ −W3 (x), where W3 (x) is a continuous positive definite function on D. Furthermore, if  is chosen such that B ⊂ D and c is chosen such that c < α1 (), then for any x(t0 ) < α2−1 (c) the solution x(t) satisfies (4.48), i.e.

4.2 Stability in the Sense of Lyapunov

119

x(t) ≤ β(x0 , t − t0 ) for all t ≥ t0 , where β(·, ·) is a class KL function. 3. Globally uniformly asymptotically stable if additionally D = Rn and α1 (·), α2 (·)  are class K∞ functions. Remark 4.4 Defining a positive definite function V (t, x) satisfying (4.29) can prove to be rather trivial. Therefore, such a function is called Lyapunov candidate function. If the derivative of the candidate function actually satisfies (4.37) or (4.40) then it is called a Lyapunov function. Note also that the class K functions in (4.29) can be replaced by two positive definite functions W1 (x) and W2 (x) leading to the same conclusions, but making the stability analysis slightly more complex. Since for the positive definite functions employed is this book it is quite easy to satisfy (4.29), it has been preferred to use these bounds from the very beginning. 

4.2.3 Complementary Results This section presents also some very well known stability results whose proofs are omitted for brevity’s sake. First of all, the previous section presented one of the main results to prove stability in the sense of Lyapunov for nonautonomous systems. It turns out that establishing stability for autonomous system is even more direct. Corollary 4.1 Consider the autonomous system x˙ = f(x),

(4.49)

where f : D → Rn is locally Lipschitz in x on D, and D ⊂ Rn is a domain that contains the origin x = 0, which is assumed to be an equilibrium point of (4.49). Let V (x) : D → R be a continuously differentiable positive definite function. Then, the equilibrium is 1. Stable if V˙ (x) is negative semidefinite in D. 2. Asymptotically stable if V˙ (x) is negative definite in D. 3. Globally asymptotically stable if additionally V (x) is radially unbounded and  D = Rn . Note that the term uniform can be dropped for autonomous systems since they do not depend explicitly on time t. It can be seen that the condition to achieve asymptotic stability is still that the derivative of f (x) be negative definite. The following theorem is based on La Salle’s invariance principle and relaxes this condition. Theorem 4.3 Consider the autonomous system (4.49), where f : D → Rn is locally Lipschitz in x on D, and D ⊂ Rn is a domain that contains the origin x = 0, which is assumed to be an equilibrium point of (4.49). Let V (x) : D → R be a continuously differentiable positive definite function. Let M = {x ∈ D | V˙ (x) = 0}. If the

120

4 Mathematical Background

only solution x that can stay identically in M is the equilibrium x ≡ 0, then the equilibrium is 1. Asymptotically stable if V˙ (x) is negative semidefinite in D. 2. Globally asymptotically stable if additionally V (x) is radially unbounded and D = Rn .  One of the most important particular cases of autonomous systems are time invariant linear systems. The following theorem provides a very well known result. Theorem 4.4 Consider the linear time invariant autonomous system x˙ = Ax,

(4.50)

where A ∈ Rn×n is a constant matrix. The equilibrium point x = 0 is globally asymptotically stable if and only if all the eigenvalues of A have negative real part, i.e.  Re(λi (A)) < 0 for i = 1, . . . , n. Finally, since the previous results applies only for autonomous systems, a very important and well known result is given below. Lemma 4.5 (Barbalat’s) If a differentiable function f (t) has a finite limit as t → ∞, and if f¨(t) is bounded, then f˙(t) → 0 as t → ∞.  The usefulness of Barbalat’s Lemma may not be evident at first sight. However, on the contrary to what intuition might just say, the fact that lim f (t) exists does t→∞

2 not necessarily imply that lim f˙(t) = 0. A common example is f (t) = sin(tt ) . The t→∞ contrary is also true, i.e. if lim f˙(t) = 0 then lim f (t) does not necessarily exists,

e.g. f (t) = sin(ln(t)).

t→∞

t→∞

4.3 Ultimate Boundedness 4.3.1 Definition Lyapunov stability intrinsically implies boundedness of the state. In the previous section it was mentioned that choosing x0 = 0 as equilibrium point did not only not imply any loss of generality, but it is even convenient because one of the most important goals in control design is to make the error state to tend to zero. However, what if this is not possible or if there is no equilibrium point? Is x0 ≈ 0 acceptable instead? In practice, due to a wide variety of reasons, one may actually get x0 ≈ 0 even if the equilibrium is (theoretically) asymptotically stable. On the other hand, if achieving x0 → 0 is not possible, then obtaining on purpose x0 ≈ 0 in a finite time can just be acceptable. But x0 ≈ 0 is by no means an accurate concept, so that it is better to use the expression x0  ≤ bf for some positive scalar bf which does not need to satisfy bf ≈ 0. With this is mind, consider the following definition.

4.3 Ultimate Boundedness

121

x2

Fig. 4.12 A uniform ultimate bounded trajectory with ultimate bound bf

x(t)

x(t0 )

bf x1

a b

Definition 4.9 The solutions of (4.21), i.e. x˙ (t) = f (t, x(t)), are said to be 1. Uniformly bounded if there exist a constant c > 0 such that for every a ∈ (0, c) exists a positive constant b depending only on a, i.e. b = b(a) > 0, such that x(t0 ) ≤ a ⇒ x(t) ≤ b ∀ t ≥ t0

(4.51)

2. Globally uniformly bounded if (4.51) holds for every a ∈ (0, ∞). 3. Uniformly ultimately bounded with ultimate bound bf if besides c > 0 exists a positive constant bf > 0, such that for every a ∈ (0, c) exists a T = T (a, bf ) ≥ 0, independent of t0 , such that x(t0 ) ≤ a ⇒ x(t) ≤ bf ∀ t ≥ t0 + T

(4.52)

4. Globally uniformly ultimately bounded if (4.52) holds for every a ∈ (0, ∞). 

4.3.2 An Ultimate Boundedness Theorem For the purpose of the present book, Definition 4.9.3 is of particular interest. Figure 4.12 depicts the concept, where it is desirable to make b arbitrarily large and bf arbitrarily small by design. Although at first sight these two goals may appear to be easier to achieve than asymptotic stability, they represent an important challenge on its own. Note that in fact the region D in (4.28) could be also used here but with the bound b instead of . Since there is also a bound for the initial condition involved in (4.51), this looks very similar to the stability definition. However, it is conceptually different since no equilibrium point is involved.

122

4 Mathematical Background

V (t, x)

x2

α2 (x)

x

V (t, x)

D

x1

Vb

α1 (x) t0

t

Fig. 4.13 V (t, x) bounded by α1 (x) and α2 (x) under condition (4.53)

In order to get some insight about the conditions for a trajectory to be uniformly ultimately bounded, the concept of positive definite functions used to prove stability can further be exploited. Indeed, consider once again a continuously differentiable function V (t, x) : [0, ∞) × D → R satisfying (4.29), i.e. α1 (x) ≤ V (t, x) ≤ α2 (x), ∀ t ≥ t0 and ∀ x ∈ D, where as before D ⊂ Rn is a domain that contains the origin. Consider also the vector differential equation (4.21), i.e. x˙ (t) = f (t, x(t)) t ≥ t0 , whose behavior should be related to V (t, x). Note that this time it is not assumed that the origin is an equilibrium of (4.21), although this could well be the case. Consider a similar condition to that given in (4.40), namely V˙ (t, x) ≤ −W3 (x), ∀ x ≥ μ > 0,

(4.53)

where W3 (x) is a continuous positive definite function on D. As can be appreciated, the one single difference between (4.40) and (4.53) is that the derivative of V (t, x) is not negative for every x ∈ D, but only for those solutions satisfying x ≥ μ > 0. By taking into consideration (4.29), the behavior of V (t, x) should be similar to that shown in Fig. 4.13. Whenever V˙ (t, x) is negative the magnitude of V (t, x) decreases. On the contrary, in general the magnitude of x may not decrease immediately just because V˙ (t, x) < 0, but eventually it will if V˙ (t, x) is negative for enough time. Therefore, eventually x < μ and for that case V˙ (t, x) becomes indefinite. However, there must exist a value Vb which once reached can never be exceeded, as depicted in Fig. 4.13. Since from (4.29) it is known that α1 (x) ≤ V (t, x) then necessarily x becomes ultimately bounded. Note that a similar reasoning led to Theorem 4.2 (just by setting μ ≡ 0

4.3 Ultimate Boundedness

123

would be the very same), so that some steps of the proof can be employed to get a result for uniform ultimate boundedness. First of all, let’s make a small change in the definition of the constant c given in (4.32) c = α1 (),

(4.54)

where as before the constant  is used to define the region B in (4.31) so that B ⊂ D. As mentioned before, just because V˙ (t, x) is negative it does not mean that x(t) decreases immediately and in fact x could leave D if x0 lies too close to the boundary. Therefore, assume that α2 (μ) < c

⇒

μ < α2−1 (c)

⇒

μ < α2−1 (α1 ()).

(4.55)

In the very same fashion, assume that α2 (x0 ) ≤ c

⇒

x0  ≤ α2−1 (α1 ()).

(4.56)

It is not difficult to show that if (4.29) and (4.53)–(4.56) hold, then any solution of (4.21) remains in D for all t ≥ t0 . To show this, note that if x0  < μ at t = t0 then (4.53) does not hold and V˙ (t, x) is indefinite. However, it would be of advantage if it were positive or even zero, so that the worst possible situation is that V˙ (t, x) > 0 and V (t, x) grows until x(t1 ) ≡ μ at some finite time t1 > t0 . At that moment (4.53) holds and one has V˙ (t, x) ≤ −W3 (x) so that V (t, x) decreases until once again x(t) < μ and V˙ (t, x) becomes indefinite, and the sequence starts over. On the other hand, if x0  ≥ μ then V (t, x) decreases until x(t) < μ and the sequence starts over as well. Therefore, one can fairly assume that x0  = α2−1 (α1 ()) so that (4.53) holds at t = t0 and that would be the worst possible case because V (t, x) begins decreasing its value. In view of the previous discussion, a region can be defined as done in (4.34) Dt,c = {x ∈ B | V (t, x) ≤ c = α1 ()} , where it is guaranteed that any trajectory starting in Dt,c remains inside because V˙ (t, x) is negative at the boundary. Note that in order for x to be in Dt,c then x ≤  as can be concluded when V (t, x) = c = α1 () holds. Indeed, by considering (4.56) α2 (x0 ) ≤ c = α1 ()

⇒

x0  ∈ Dt0 ,c ⊂ Dt,c ∀t ≥ t0 .

(4.57)

It is also important that if x0  = α2−1 (α1 ()) then from (4.29) it is α1 (x(t)) ≤ V (t, x) ≤ V (t, x0 ) ≤ α2 (x 0 ) = α1 (),

(4.58)

which implies that x(t) ≤ .

(4.59)

124

4 Mathematical Background

As explained before V (t, x) decreases until x(t) < μ, but if (4.53) holds again because x(t) ≥ μ, then for that case μ can be considered as a new initial condition norm value and one would have α1 (x(t)) ≤ V (t, x) ≤ α2 (μ),

(4.60)

x(t) ≤ α1−1 (α2 (μ)) < .

(4.61)

and consequently

The last inequality follows after (4.55). In the end this implies that x(t) ≤  for all t ≥ t0 . This comply with Definition 4.9.1 (uniform boundedness) by choosing b =  and a = α2−1 (α1 ()). Note that if D = Rn and α1 (·) and α2 (·) are class K∞ functions, then Definition 4.9.2 (global uniform boundedness) holds as well. It remains to show ultimate boundedness. Consider η = α2 (μ) < c,

(4.62)

Bμ = {x ∈ B | α2 (x) ≤ α2 (μ)} Dt,η = {x ∈ B | V (t, x) ≤ η = α2 (μ)} D1,η = {x ∈ B | α1 (x) ≤ η < α1 ()} .

(4.63)

and define

(4.64) (4.65)

By noting that D1,c = B since c = α1 (), then one can conclude that Bμ ⊂ Dt,η ⊂ D1,η ⊂ Dt,c ⊂ D1,c = B ⊂ D,

(4.66)

just by following the same arguments to get (4.36), where Bμ takes the place of D2,c and implies x ≤ μ and therefore at the boundary and outside this region (4.53) holds. This means that every trajectory starting in Dt,η remains inside, but since Dt,η is a subset of Dt,c , then any trajectory starting inside Dt,c but outside Dt,η must enter in this last region in a finite time because for that case V˙ (t, x) ≤ −W3 (x).

(4.67)

Otherwise said, (4.67) holds in the region Dt,c − Dt,η . But in view of the definition of Dt,η , any x in this region must comply with x ≤ μ. Therefore, by recalling that any x in Dt,c satisfies x ≤ , (4.67) holds for μ ≤ x ≤ , and over this set it is possible to define k = min(W3 (x)), such that V˙ (t, x) ≤ −k < 0 holds. By integrating one gets

(4.68)

4.3 Ultimate Boundedness

125

V (t, x) ≤ V (t, x0 ) − k(t − t0 ) ≤ c − k(t − t0 ).

(4.69)

This last relationship will be valid until V (t, x) = η, which must be reached in a finite time tη obtained below η ≤ c − k(tη − t0 )

⇒

tη ≤ t0 +

c−η . k

(4.70)

Assume that T is the first time that x enters in Dt,η , so that in the time interval [t0 , T ] must hold V˙ (t, x) ≤ −W3 (x) ≤ −α3 (x) ≤ −α3 (α2−1 (V (t, x)))

(4.71)

for a class K function α3 (·) and where V (t, x) ≤ α2 (x) has been taken into account. Therefore, it is possible to recover (4.44), i.e. V˙ (t, x) ≤ −α4 (V (t, x)), with α4 (·) a class K function. Also as before one recovers (4.47), i.e. V (t, x) ≤ β1 (V (t0 , x0 ), t − t0 ) but for t ∈ [t0 , t0 + T ] and (4.48) as direct consequence, i.e. x ≤ β(x0 , t − t0 ) ∀ t ∈ [t0 , t0 + T ],

(4.72)

where β1 (·, ·) and β(·, ·) are class KL functions. Since once in Dt,η the solution remains inside, then for t ≥ t0 + T , relation (4.61) holds, i.e. x(t) ≤ α1−1 (α2 (μ)) = α1−1 (η).

(4.73)

Note that if x0 ∈ Dt,η then (4.61) holds for all t ≥ t0 . Therefore, this complies with Definition 4.9.3 (uniform ultimate boundedness) by choosing bf = α1−1 (α2 (μ)) while (4.70) shows that T must exist. Finally, if D = Rn and α1 (·) and α2 (·) are class K∞ functions, then Definition 4.9.4 (global uniform ultimate boundedness) holds as well. The previous discussion is summarized in the following theorem. Theorem 4.5 Consider the nonautonomous system (4.21), i.e. x˙ (t) = f(t, x(t)) t ≥ t0 , where f : [0, ∞) × D → Rn is piecewise continuous in t and locally Lipschitz in x on [0, ∞) × D, where D ⊂ Rn is a domain that contains the origin x = 0. Let D ⊂ Rn be a domain that contains the origin and V : [0, ∞) × D → R be a continuously differentiable function such that (4.29) and (4.53) hold, i.e.

126

4 Mathematical Background

α1 (x) ≤ V (t, x) ≤ α2 (x) V˙ (t, x) ≤ −W3 (x), ∀ x ≥ μ > 0, ∀ t ≥ 0 and ∀ x ∈ D, where α1 and α2 are class K functions, W3 (x) is a continuous positive definite function. Take  > 0 such that B = {x ∈ Rn | x ≤ } ⊂ D and suppose that (4.55) holds, i.e. μ < α2−1 (α1 ()). Assume that (4.56) holds, i.e. x(t0 ) = x0  ≤ α2−1 (α1 ()). Then, there exits a class KL function β(·, ·) and a T ≥ 0 such that the solution of (4.21) satisfies (4.72) and (4.73), i.e. x ≤ β(x(t0 ), t − t0 ), ∀ t0 ≤ t ≤ t0 + T x ≤ α1−1 (α2 (μ)), ∀ t ≥ t0 + T. Moreover, if D = Rn and α1 belongs to class K∞ , then (4.72)–(4.73) hold for any  initial state x(t0 ), with no restriction on how large μ is.

4.4 Sliding Surfaces One of the main goals of robot control theory is trajectories tracking. The idea is to provide a path and orientation for the robot end-effector which has to be followed as close as possible. The desired values can be denoted as qd and q˙ d , so that the tracking error can be defined as e = q − qd .

(4.74)

Therefore, to apply the theory of the previous section the state of the error dynamics can be defined as e x= . (4.75) e˙ Alternatively, a sliding surface s(t) = 0 in Rn can be defined. For simplicity’s sake assume first that n = 1, so that the sliding surface is given by  s(x, t) =

d +λ dt

m−1 e,

(4.76)

4.4 Sliding Surfaces

127

where λ is a positive constant and m ≥ 2 is a positive integer. For this book, the case of special interest is m = 2 to get s = e˙ + λe.

(4.77)

Clearly, this represents a stable linear system in the form of (4.50) but with an external input, i.e. e˙ = −λe + s,

(4.78)

where A = −λ and therefore it is stable. By properties of stable linear systems, if s is bounded, then so are e and e. ˙ Furthermore, if s → 0 then e, e˙ → 0 as well. In fact, if s ≡ 0, then system (4.78) is autonomous and evidently e, e˙ → 0. Thus, one can replace the problem of achieving e, e˙ → 0 by getting s → 0 as t → ∞ or if possible s ≡ 0 from a finite time tr . It can be shown that the latter is possible if the so called sliding condition is matched, i.e. 1 d 2 s ≤ −η|s|, 2 dt

(4.79)

where η > 0. Satisfying this condition makes the surface an invariant set and the behavior of the system once the sliding surface is reached is known as sliding mode. Condition (4.79) guarantees that s ≡ 0 in a finite time tr in case s(t0 ) = 0. To show this, assume without lost of generality that t0 = 0 and that s(t = 0) > 0, so that from (4.79) one gets between t = t0 and s(t = tr ) = 0 s s˙ ≤ −ηs ⇒ s˙ ≤ −η.

(4.80)

By integrating it is t t s 0r ≤ −ηt 0r

⇒

s(t = tr ) − s(t = 0) = 0 − s(t = 0) ≤ −ηtr ,

(4.81)

which in turn implies that ηtr ≤ s(t = 0),

(4.82)

s(t = 0) . η

(4.83)

and therefore tr ≤

Note that the result would be completely similar if s(t = 0) < 0, from which it is concluded tr ≤

|s(t = 0)| . η

(4.84)

128

4 Mathematical Background

Just as explained before, s ≡ 0 implies that e, e˙ → 0, but satisfying the sliding condition may require the use of discontinuities in the control law, which is prone to cause chattering. Finally, the result can easily be extended to the vector case s = e˙ + e,

(4.85)

with  ∈ Rn×n a diagonal positive definite matrix.

References Desoer CA, Vidyasagar M (1975) Feedback systems: input-output properties. Academic, New York Hahn W (1963) Theory and applications of Liapunov’s direct method. Prentice-Hall, Englewood Cliffs Hahn W (1967) Stability of motion. Springer, New York Khalil HK (1996) Nonlinear sstems, 2nd edn. Prentice-Hall, Englewood Cliffs Khalil HK (2002) Nonlinear systems, 3rd edn. Prentice-HallPrentice-Hall, Upper Saddle River Nuño E, Basañez L, Ortega R, Spong MW (2009) Position tracking for non-linear teleoperators with variable time delay. Int J Robot Res 28(7):895–910 Nuño E, Basañez L, López-Franco C, Arana-Daniel N (2014) Stability of nonlinear teleoperators using PD controllers without velocity measurements. J Franklin Inst 351:241–258 Skelton RE (1988) Dynamic systems control: linear systems analysis and synthesis. Wiley, New York Slotine JJE, Li W (1991) Applied nonlinear control. Prentice-Hall, Englewood Cliffs Vidyasagar M (1978) Nonlinear systems analysis. Prentice-Hall, Englewood Cliffs

Chapter 5

Common Control Approaches for Robot Manipulators

Abstract Robot manipulators are designed to carry out specific tasks but at the same time to be flexible enough to allow reprogramming if necessary. Since the variety of possible works is too wide, the most direct way to obtain a general solution to carry them out is to propose a time varying desired trajectory in terms of joint or work space coordinates and then design a control law able to follow it. From a pure theoretical point of view achieving this goal can even be trivial despite the high nonlinear dynamics of robot manipulators. However, when it comes to implementation many practical issues arise that make the problem much more involved than it appears to be at the beginning. This chapter recapitulates the most basic control techniques for robot manipulators.

5.1 PD and PD+ Control Despite the high nonlinear dynamic nature of robot manipulators, it turns out that some tasks can be carried out rather straightforwardly and let’s say easily. Assume that it is desired to take the robot manipulator to its initial position q(0) = q 0 to a final constant position q d ∈ Rn . The position error is therefore here and for the rest of the book given by e = q − qd.

(5.1)

If velocity measurements are available, then a PD control law can be implemented as τ = −K d e˙ − K p e,

(5.2)

where K d , K p ∈ Rn×n are diagonal positive definite matrices. Now, for simplicity’s sake assume that g(q) = 0 and D = O in model (3.44). For that case one has the closed loop dynamics given by ˙ e + K d e˙ + K p e = 0, H(q)¨e + C(q, q)˙ © The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control of Robot Manipulators, Lecture Notes in Electrical Engineering 798, https://doi.org/10.1007/978-3-030-85980-0_5

(5.3) 129

130

5 Common Control Approaches for Robot Manipulators

˙ Consider the where it has been taken advantage of the fact that q˙ d ≡ 0 ⇒ e˙ = q. following Lyapunov candidate function V (e, e˙ ) =

1 1 T e˙ H(q)˙e + eT K p e. 2 2

(5.4)

The corresponding derivative along (5.3) is then given by 1 ˙ e + e˙ T K p e V˙ (e, e˙ ) = e˙ T H(q)¨e + e˙ T H(q)˙ 2

(5.5)

 1 ˙ ˙ e˙ + e˙ T K p e − e˙ T K p e = −˙eT K d e˙ + e˙ T H(q) − 2C(q, q) 2  1 ˙ ˙ e˙ . = −˙eT K d e˙ + e˙ T H(q) − 2C(q, q) 2 According to Property 3.2 one has V˙ (e, e˙ ) = −˙eT K d e˙ .

(5.6)

Since V˙ (e, e˙ ) is only negative semidefinite, Theorem 4.3 can be used. In fact, V˙ (e, e˙ ) ≡ 0 in the set M = {(e, e˙ ) : e˙ ≡ 0}.

(5.7)

Therefore, one only needs to show that the only solution (e, e˙ ) that can stay identically in M is the equilibrium (e, e˙ ) ≡ (0, 0). This is carried out by substituting M, i.e. e˙ ≡ 0, in (5.3) to get K p e = 0. Since K p is positive definite its inverse exists and therefore the only solution for the previous relationship is precisely e = 0, which shows that the origin is asymptotically stable. But since furthermore V (e, e˙ ) is radially unbounded in view of Property 3.7, then one can conclude from Theorem 4.3 that it is globally asymptotically stable. Note that neither K d nor K p need to be diagonal, but this is a common practice and also simplifies gain tuning. Although regulation on its own may be a very important goal, trajectory tracking is also essential. For that case q˙ d = 0. Also, in general viscous friction can be neglected, but gravity effects cannot if they exist. Assuming that the whole robot model is known, then a control law that can achieve exact tracking is given by ˙ q˙ d + D q˙ d + g(q) − K d e˙ − K p e, τ = H(q)q¨ d + C(q, q)

(5.8)

5.1 PD and PD+ Control

131

which is referred to as augmented PD or PD+. Substituting (5.8) in (3.44) leads practically to the same closed loop equation (5.3), i.e. ˙ e + K D e˙ + K p e = 0, H(q)¨e + C(q, q)˙

(5.9)

where K D = D + K d > O. Therefore, the proof to show that e → 0 and e˙ → 0 is the same as before.

5.2 PID Control of Robot Manipulators Suppose that the gravity vector g(q) is not well known and that it is desired to achieve position regulation. Consider once again the error e given in (5.1) with q d constant. Since q˙ d = q¨ d = 0, one also has e˙ = q˙ and e¨ = q¨ . For that case a PD controller is not good enough, but it can be shown that a PID of the form τ = −K p e − K d e˙ − K i ξ ξ˙ = e,

(5.10) (5.11)

can fulfill this goal, as long as the (diagonal) positive definite matrices K p , K d , K i ∈ Rn×n are properly chosen. The result turns out to be only locally uniformly asymptotically stable although the region of attraction can be arbitrarily enlarged. The usual approach to prove stability is rather complex in the sense that the Lyapunov candidate function contains both cross terms and the robot’s potential energy; in fact, it is even involved to find conditions to guarantee that it is positive definite in the first place. Roughly speaking, the proportional and derivative gains need to be large enough, and the integral gain needs to be small enough. In this book, an alternative approach is provided to arrive basically to the same result. Define a vector version of s given in (4.77) as s = e˙ + e ⇒

e˙ = −e + s,

(5.12)

where  ∈ Rn×n is a positive definite diagonal matrix. The elements of s can be considered as the inputs of n stable independent linear filters of the form (4.77) whose outputs are the corresponding elements of e, so that if it is bounded and tends to zero, so will do e and e˙ , i.e. s→0

⇒

e, e˙ → 0.

(5.13)

In terms of s, the regular PID control law (5.10)–(5.11) can be rewritten as τ = −K d s − K ω ω − K ps e ω˙ = s,

(5.14) (5.15)

132

5 Common Control Approaches for Robot Manipulators

as long as the following gains equivalence is used K p = K d  + K ω + K ps Kd = Kd

(5.16) (5.17)

K i = K ω .

(5.18)

Note that given any arbitrary set K p , K d and K i it is not always possible to get an equivalent set of gains K d ,  and K ω . For that reason, the latter have to be chosen first in order to get the former for implementation. Substituting (5.14) in (3.44) yields ˙ + K d s + K ω ω + K ps e + g(q) = δ a , H(q)˙s + C(q, q)s

(5.19)

δ a = H(q)˙e + C(q, e˙ )e,

(5.20)

with

and where D = O has been taken for simplicity. By choosing ⎡ ⎤ ω y = ⎣ e ⎦, s

(5.21)

where ω is defined in (5.15), it is possible to rewrite (5.19) as ⎤ s ⎦ −e + s ˙y = ⎣ .  ˙ + K d s + K ω ω + K ps e + g(q) − δ a −H −1 (q) C(q, q)s ⎡

(5.22)

It can be seen that the equilibrium point of (5.22) is e = s = 0 and ω = −K −1 ω g(q d ). A change of coordinates can be done in the form x 1 = ω + K −1 ω g(q d ),

(5.23)

to have an equivalent system for (5.22) as ⎤ s ⎦ −e + s x˙ = ⎣ ,  ˙ + K d s + K ω x 1 + K ps e + g(q) − g(q d ) − δ a −H −1 (q) C(q, q)s (5.24) ⎡

5.2 PID Control of Robot Manipulators

133

with ⎡

⎤ ⎡ ⎤ x1 x1 x = ⎣ x2 ⎦ ≡ ⎣ e ⎦ . x3 s

(5.25)

Clearly, the equilibrium point of (5.24) is x = 0 and it is unique. Also note that δ a in (5.20) can be considered as a vanishing perturbation at the origin. To prove that x → 0 it is taken advantage of the fact that it is already known that the origin is uniformly asymptotically stable but not globally. Therefore, the region of interest (4.28) mentioned in Remark 4.3 can be used to simplify the stability analysis, i.e.  D = x ∈ Rn \ x <  , for an arbitrarily large  > 0. Consider the following positive definite function V (x) with respect to the state variables of system (5.24) V (x) =

1 1 1 T x K ω x 1 + eT K ps e + sT H(q)s. 2 1 2 2

(5.26)

V (x) satisfies (4.29) with α1 (x) = λ1 x2 and α2 (x) = λ2 x2 ,

(5.27)

  λ1 = 21 min λmin (K ω ), λmin (K ps ), λh , λ2 = 21 max λmax (K ω ), λmax (K ps ), λH , and λh , λH given in Property 3.7. The derivative of (5.26) along (5.24) can easily be computed as V˙ (x) = −eT K ps e − sT K d s − sT (g(q) − g(q d )) + sT δ a ,

(5.28)

just by using Property 3.2. To further manipulate (5.28), take Properties 3.9 and 3.11 into account to get by using (5.20) V˙ (x) ≤ − λmin (K ps )e2 − λmin (K d )s2 + kg es

(5.29)

+ λH λmax ()˙es + kc λmax ()˙ees. Define the auxiliary vector xa =

  e x2 . = x3 s

(5.30)

134

5 Common Control Approaches for Robot Manipulators

Then, directly from (5.12) one has ˙e ≤ λmax ()e + s, so that (5.29) becomes V˙ (x) ≤ − λmin (K ps )e2 − λmin (K d )s2 + kg es

(5.31)

+ λH λmax () es + λH λmax ()s 2

2

+ kc λmax ()2 ees + kc λmax ()ses ≤ − λmin (K ps )e2 + γ2 es − λmin (K d )s2 + γ1 s2 + γ3 x a 3 , where γ1 = λH λmax ()

(5.32)

γ2 = kg + λH λmax ()

2

(5.33)

γ3 = kc λmax () + kc λmax ().

(5.34)

λmin (K d ) ≥ γ1 + γ + 1, 1 λmin (K ps ) ≥ γ + γ22 , 4

(5.35)

2

Assume now that

(5.36)

for some γ > 0, so that V˙ (x) satisfies V˙ (x) ≤ −γ e2 − γ s2 + γ3 x a 3 .

(5.37)

V˙ (x) ≤ −γ x a 2 + γ3 x a 3 = −W3 (x).

(5.38)

Therefore it is

To show that W3 (x) is positive semidefinite in D, rewrite (5.38) as

γ3 V˙ (x) ≤ −γ x a 2 1 − x a  = −W3 (x). γ

(5.39)

Then, W3 (x) ≥ 0 and V˙ (x) ≤ 0 as long as x a  ≤

γ . γ3

(5.40)

Finally assume that γ < , γ3

(5.41)

5.2 PID Control of Robot Manipulators

135

so that V˙ (x) ≡ 0 only in the set M = {(x 1 , x a ) : x a ≡ 0 ⇒ (e, s) ≡ (0, 0)}.

(5.42)

Then, Theorem 4.3 can be used to show that the origin is (locally uniformly) asymptotically stable by noting that in M the only solution x of (5.24) that can stay identically is precisely the equilibrium x ≡ 0. Note that despite V (x) is radially unbounded the result cannot be global because D cannot be equal to Rn . By using the proposed approach it can be concluded, roughly speaking, that stability will be guaranteed if the derivative and the proportional gains are big enough and gains are chosen according to (5.16)–(5.18). Note that technically this does not necessarily mean that the equilibrium of the original error state (ξ , e, e˙ ) is asymptotically stable. However, it can be shown that indeed this is the case by using directly Definition 4.8 and some equivalences. This proof is omitted because in the end the main goal has already been shown, i.e. (e, e˙ ) → (0, 0).

5.3 Computed Torque Control While PID control can compensate the gravity term for position regulation, there will always be a residual error for trajectory tracking. It has been already shown that a PD+ control law can achieve this goal, but it is not the only model based technique which can be employed. One of the earliest and better known results is the computed torque control law given by   ˙ q˙ + D q˙ + g(q), τ = H(q) q¨ d − K d e˙ − K p e + C(q, q)

(5.43)

with e given in (5.1) and K d , K p ∈ Rn×n are as before diagonal positive definite gain matrices. Substituting (5.43) into (3.44) yields   H(q) e¨ + K d e˙ + K p e = 0.

(5.44)

But since from Property 3.1 the inverse of H(q) always exists and hence e¨ + K d e˙ + K p e = 0.

(5.45)

To analyze the closed loop stability properties of control law (5.43), one just needs to take advantage of the fact that K d , K p > O and diagonal, so that there are n independent second order linear differential equations of the form e¨i + kdi e˙i + kpi ei = 0,

(5.46)

where ei is the ith element of e and kdi , kpi are the ith elements of the diagonals of K d and K p , respectively, for i = 1, . . . , n. The associated characteristic equations

136

5 Common Control Approaches for Robot Manipulators

of (5.46) are of the form λ2 + kdi λ + kpi = 0.

(5.47)

Since the eigenvalues of (5.47) do have negative real part because kdi , kpi > 0, then it is clear that e˙i , ei → 0 or e˙ , e → 0 for any initial condition. It is worthy pointing out that K d and K p do not need to be diagonal to obtain the same result. Finally, it is possible to write (5.43) as   ˙ q˙ + D q˙ + g(q) + H(q) −K d e˙ − K p e . τ = H(q)q¨ d + C(q, q)       τ ff

(5.48)

τ fb

The term τ ff is known as the predictive part and it is aimed at providing the necessary input torque to keep the system along the nominal trajectory. τ fb is the feedback part, which provides the necessary input torque to reduce tracking errors.

5.4 Exploiting the Passive Structure of Robot Manipulators

The main disadvantage of the computed torque control law is that it relies on an exact knowledge of the robot model, which makes it little robust against model uncertainties. This effect is accentuated because the predictive term ideally cancels out the robot nonlinear dynamics. Instead, the passive structure of robot manipulators can be exploited to achieve trajectory tracking in a more robust way. Consider the definition of the reference velocity as q˙ r = q˙ d − e,

(5.49)

so that the vector s in (5.12) can be obtained as s = q˙ − q˙ r = e˙ + e.

(5.50)

˙ q˙ r + D q˙ r + g(q) − K d s, τ = H(q)q¨ r + C(q, q)

(5.51)

Then, define the control law

with K d ,  ∈ Rn×n diagonal and positive definite as usual. Substituting in (3.44) leads to ˙ + K D s = 0, H(q)˙s + C(q, q)s

(5.52)

5.4 Exploiting the Passive Structure of Robot Manipulators

137

where K D = D + K d . Consider the following candidate Lyapunov function1 V (t) =

1 T s H(q)s, 2

(5.53)

whose derivative along (5.52) is trivially given by 1 ˙ V˙ (t) = sT H(q)˙s + sT H(q)s 2 = −sT K D s +

(5.54)

 1 T ˙ ˙ s s H(q) − 2C(q, q) 2

= −sT K D s ≤ 0. Since K D is positive definite, this means that V˙ (t) is negative definite. Therefore, by applying Corollary 4.1 it can be concluded that the equilibrium s = 0 of (5.52) is globally asymptotically stable because additionally V (t) is radially unbounded. This in turn means that e, e˙ → 0.

5.5 Design in Work Space Coordinates In the previous sections, the desired trajectories for the robot manipulators are designated as q d . However, when it comes to choose these desired values, in practice they are given in work space coordinates and therefore inverse kinematic online has to be implemented together with the control law as shown in Fig. 5.1. The computation of inverse kinematics is explained in Section 2.3. An alternative to computing the inverse kinematic consists in rewriting model (3.44) directly in work space coordinates. To do that, assume that f : Q → R p is a smooth and invertible map from joint variables q ∈ Q to work space variables x ∈ R p . To simplify the discussion, assume that n = p so that the number of degrees of freedom of the robot matches the dimension of the work space. If n < 6, then the work space variables provide only a partial parametrization of S E(3). The transformation is carried out by using the Jacobian of the manipulator, either the analytic or the geometric. Consider for example the relationship (2.183) x˙ = J(q)q˙

1

J(q) =

∂f , ∂q

Care should be taken! V (t) in (5.53) is a candidate Lyapunov function for s, but not for (e, e˙ ).

138

5 Common Control Approaches for Robot Manipulators

from which it can be inferred that q˙ = J −1 x˙

⇒

q¨ = J −1 x¨ +

d  −1  J x˙ . dt

(5.55)

The previous relationship is valid if and only if J −1 (q) exits, which implies that the robot does not reach any singularity. This is hardly a disadvantage because singularities should also be avoided to compute the inverse kinematics and therefore this  T assumption is always made. By defining J −T = J −1 , one can get from (3.44): J −T H J −1 x¨ + J −T



C J −1 + H

  d  −1  J x˙ + J −T D J −1 x˙ + g = J −T τ , dt (5.56)

˙ has been omitted for simplicity. Define where the dependence on (q, q) ¯ H(q) = J −T (q)H(q) J −1 (q)

d  −1  ¯ ˙ = J −T (q) C(q, q) ˙ J −1 (q) + H(q) C(q, q) J (q) dt ¯D(q) = J −T (q) D J −1 (q)

(5.57) (5.58) (5.59)

g¯ (q) = J

−T

(q)g(q)

(5.60)

F= J

−T

(q)τ ,

(5.61)

so that the robot dynamics expressed in work space coordinates is given by ¯ ¯ ¯ ˙ x˙ + D(q) H(q) x¨ + C(q, q) x˙ + g¯ (q) = F.

(5.62)

This model is mainly used for control design and therefore it is unnecessary to make the substitution of q and q˙ in terms of x and x˙ . On the other hand, it is not difficult ¯ to show by direct computation that H(q) is symmetric positive definite, while as ˙ n×n ¯ ¯ ˙ ∈R is skew symmetric. This is very important because before H(q) − 2 C(q, q) these were two of the key properties used to prove stability in the proposed control schemes of this section and they could be implemented just by rewriting them in terms of model (5.62). For example, the computed torque control law can be rewritten as   ¯ ¯ x˙ + g¯ (q), ¯ ˙ x˙ + D q) F = H(q) x¨ d − K d  x˙ − K p x + C(q,

(5.63)

where K d , K p ∈ Rn×n are diagonal positive definite gain matrices and x d ∈ Rn is the desired trajectory in work space coordinates. Recall that x d has to be provided anyway as shown in Fig. 5.1. The corresponding tracking error is defined as x = x − x d .

(5.64)

5.5 Design in Work Space Coordinates Fig. 5.1 Implementation of a control law together with inverse kinematics online computation

139

Controller

¨d q d , q˙ d , q

τ

Inverse kinematics

Robot

q, q˙

¨d xd , x˙ d , x Desired workspace coordinates

It is important to note that for implementation it is the input torque which has to be programmed as τ = J T (q)F.

(5.65)

The stability proof is omitted in view of its obvious similarity with the regular computed torque law.

References 1. Arteaga-Pérez MA (2019) An alternative proof to the asymptotic stability of PID controllers for regulation of robot manipulators. IFAC J. Syst. Control 9, Article 100066 2. Canudas de Wit C, Siciliano B, Bastin G (eds) (1996) Theory of robot control. Springer, London 3. Kelly R, Santibáñez V, Loria A (2005) Control of robot manipulators in joint space. Springer, London, Great Britain 4. Murray RM, Li Z, Sastry SS (1994) A mathematical introduction to robotic manipulation. CRC Press, Boca Raton 5. Siciliano B, Khatib O (eds) (2008) Springer handbook of robotics. Springer, Berlin 6. Slotine JJE, Li W (1987) On the adaptive control of robot manipulators. Int J Robot Res 6(3):49– 59

Part II

Looking for Semiglobal Stability or Ultimate Boundedness

Chapter 6

Velocity Observer Design

Abstract As seen in Chap. 5, robot control design may be quite direct under ideal circumstances. Particularly, an exact robot model and joint velocities measurements should be available in order to implement different effective control schemes. However, in practice neither of them are available. This chapter deals with velocity estimation. It is worthy to point out that due to digital implementation in computers an acceptable estimate of joint velocities can be obtained online by numerical differentiation, for example. Nevertheless, it is always advisable to design an observer for this goal because even after digital implementation the system performance is usually higher.

6.1 The Nicosia and Tomei Observer This section reproduces one of the first joint velocity observers for robots manipulators. However, the original stability proof is changed to propose a new one based on the general methodology of this book, i.e. define a region of interest a priori were the results are valid instead of inferring such a region as a function of different control/observer gains. For the robot manipulator dynamics given by (3.44), assume that no velocities measurements are available, i.e. q˙ , and the only measurable system outputs are joint positions, i.e. q. Let qˆ be the estimated value of q and define the observation error as z = q − qˆ .

(6.1)

Consider the following observer q˙ˆ = ξ + kd z

  ξ˙ = H (q) −C(q, q˙ˆ )q˙ˆ − Dq˙ˆ − g(q) + K o z + τ , −1

(6.2) (6.3)

where qˆ is obtained by integrating (6.2), K o ∈ Rn×n is a positive definite diagonal matriz and kd is a positive constant. To compute the observer error dynamics combine (6.2) and (6.3) to get © The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control of Robot Manipulators, Lecture Notes in Electrical Engineering 798, https://doi.org/10.1007/978-3-030-85980-0_6

143

144

6 Velocity Observer Design

  H(q) q¨ˆ − kd z˙ + C(q, q˙ˆ )q˙ˆ + Dq˙ˆ + g(q) − K o z = τ ,

(6.4)

and then subtract this relationship from (3.44) to get H(q)¨z + kd H(q)˙z + C(q, q˙ )˙q − C(q, q˙ˆ )q˙ˆ + D˙z + K o z = 0.

(6.5)

In view of Property 3.4 one has C(q, q˙ )˙q − C(q, q˙ˆ )q˙ˆ = C(q, q˙ )˙q − C(q, q˙ − z˙ )q˙ˆ = C(q, q˙ )˙q − C(q, q˙ )qˆ˙ + C(q, z˙ )q˙ˆ = C(q, q˙ )˙z + C(q, z˙ )q˙ˆ .

(6.6)

Therefore, (6.5) can be rewritten as H(q)¨z + C(q, q˙ )˙z + D˙z + K o z + kd H(q)˙z + C(q, q˙ˆ )˙z = 0.

(6.7)

To prove stability, define the observation error state for (6.7) as   z x2 = , z˙

(6.8)

and assume that the velocity vector q˙ is bounded for all t ≥ t0 by a constant vm , i.e. ˙q(t) ≤ vm ∀t ≥ t0 .

(6.9)

This assumption is necessary because at this point no control law is under consideration. Note that it is quite reasonable from a physical practical point of view. For this state define a region of interest as done in (4.28)   Dz = x2 ∈ R2n \ x2  <  ,

(6.10)

and consider the following Lyapunov candidate function V (x2 ) =

1 T 1 z˙ H(q)˙z + zT K o z, 2 2

(6.11)

which complies with (4.29), i.e. α1 (x2 ) ≤ V (t, x2 ) ≤ α2 (x2 ), by using α1 (x2 ) =

1 1 Lm x2 2 and α2 (x2 ) = LM x2 2 , 2 2

(6.12)

6.1 The Nicosia and Tomei Observer

145

where Lm = min{λmin (K o ), λh } and LM = max{λmax (K o ), λH }.

(6.13)

λh and λH are given in Property 3.7. By using Properties 3.2 and 3.4, the derivative of V (x2 ) along (6.7) can straightforwardly be computed as 1 ˙ V˙ (x2 ) = z˙ T H(q)¨z + z˙ T H(q)˙ z + z˙ T K o z 2 = −˙zT D˙z − z˙ T K o z − z˙ T kd H(q)˙z − z˙ T C(q, q˙ˆ )˙z + z˙ T K o z   = −˙zT D + kd H(q) + C(q, q˙ˆ ) z˙ = −˙zT (D + kd H(q) + C(q, q˙ ) − C(q, z˙ )) z˙ .

(6.14)

By using once again Property 3.7 together with Properties 3.9 and 3.10, together with (6.9), one gets V˙ (x2 ) ≤ − ˙z2 (λmin (D) + kd λh − kc vm − kc ˙z) ≤ − ˙z2 (kd λh − kc vm − kc ˙z) .

(6.15)

Clearly, V˙ (x2 ) is negative semidefinite as long as ˙z ≤

kd λh − vm < , kc

(6.16)

kc vm . λh

(6.17)

and assuming that kd >

Since V˙ (x2 ) is only negative semidefinite, Theorem 4.3 can be used. In fact, V˙ (z, z˙ ) ≡ 0 in the set M = {(z, z˙ ) : z˙ ≡ 0}.

(6.18)

Therefore, it is necessary to show that the only solution (z, z˙ ) that can stay identically in M is the equilibrium (z, z˙ ) ≡ (0, 0). This is carried out by substituting M, i.e. z˙ ≡ 0, in (6.7) to get K o z = 0. Since K o is positive definite its inverse exists and as a consequence the only solution for the previous relationship is precisely z = 0, which shows that the origin is uniformly asymptotically stable. As noted in Definition 4.8, (local) asymptotic stability

146

6 Velocity Observer Design

implies the existence of a region of attraction. This turns out to be important because should the initial error state x2 (t0 ) be too large, then it will not necessarily vanish even if x2 (t0 ) ∈ Dz . As can be inferred from (6.16), setting gains larger makes  larger and thus the region of attraction should be larger as well. It can be said that the equilibrium is semi globally uniformly asymptotically stable. To check this intuition out, a region of attraction can be found by using (4.29), (6.12) and the fact that V˙ ≤ 0 for x2 ∈ Dz to get 1 1 Lm x2 (t)2 ≤ V (x2 (t)) ≤ V (x2 (t0 )) ≤ LM x2 (t0 )2 , 2 2 which means that

(6.19)

 x2 (t) ≤

LM x2 (t0 ) < , Lm

(6.20)

where the last inequality is mandatory to get x2 (t) ∈ Dz for all t ≥ t0 . Therefore, a region of attraction is given by 

Lm 2n  . (6.21) A = x2 (t0 ) ∈ R \ x2 (t0 ) < LM One of the main reasons to design a velocity observer is to employ the estimates in the robot control. Consider the control law (5.8) by making the substitution of actual for estimated velocities τ = H(q)¨qd + C(q, q˙ˆ )˙qd + D˙qd + g(q) − K d (q˙ˆ − q˙ d ) − K p e,

(6.22)

where the usual error definition e = q − qd in (5.1) has been employed and qd is assumed to be bounded with at least first and second derivatives bounded as well. It is not difficult to compute the closed loop dynamics as H(q)¨e + C(q, q˙ )˙e + K D e˙ + K p e = K d z˙ + C(q, e˙ )˙z − C(q, q˙ )˙z,

(6.23)

where K D = D + K d , C(q, q˙ˆ )˙qd = C(q, q˙ − z˙ )˙qd and Property 3.4 have been used. Note the similarity with (5.9). Define the state for the system closed loop dynamics (6.7) and (6.23) as  x=

x1 x2



⎡ ⎤ e ⎢ e˙ ⎥ ⎥ =⎢ ⎣z⎦, z˙

(6.24)

for which a region of the form (4.28) can be defined as   D = x ∈ R4n \ x <  .

(6.25)

6.1 The Nicosia and Tomei Observer

147

Note that  does not need to be the very same as in (6.10) because the state is different and the observer gains will have to be retuned. Consider the following Lyapunov candidate function V (x) =

1 1 1 1 T e˙ H(q)˙e + eT K p e + z˙ T H(q)˙z + zT K o z, 2 2 2 2

(6.26)

which complies with (4.29), i.e. α1 (x) ≤ V (t, x) ≤ α2 (x), by using α1 (x) =

1 1 Mm x2 and α2 (x) = MM x2 , 2 2

(6.27)

where Mm = min{λmin (K p ), λmin (K o ), λh }

(6.28)

MM = max{λmax (K p ), λmax (K o ), λH }.

(6.29)

and

λh and λH are given in Property 3.7. By using Properties 3.2 and 3.10, the derivative of (6.26) along (6.7) and (6.23) satisfies 1 ˙ 1 ˙ e + e˙ T K p e + z˙ T H(q)¨z + z˙ T H(q)˙ z + z˙ T K o z V˙ (x) = e˙ T H(q)¨e + e˙ T H(q)˙ 2 2 = −˙eT K D e˙ + e˙ T K d z˙ − e˙ T C(q, q˙ d )˙z − z˙ T (D + kd H(q))˙z − z˙ T C(q, q˙ˆ )˙z   ≤ −˙eT K d e˙ + e˙ T K d z˙ + e˙ T C(q, e˙ ) − C(q, q˙ ) z˙ − kd z˙ T H(q)˙z   (6.30) −˙zT C(q, q˙ ) − C(q, z˙ ) z˙ . Since e˙ is bounded in D and one can always define q˙ d bounded, there exists vm , defined in (6.9). After Properties 3.7 and 3.9 and the definition of the region D in (6.25), one gets V˙ (x) ≤ − λmin (K d )˙e2 − kd λh ˙z2 + kc (vm + )˙z2 + λmax (K d )˙e˙z + kc (vm + )˙e˙z.

(6.31)

To achieve asymptotic stability gains have to be tuned properly, in particular kd must be set large enough. To get insight about how large kd must be, choose δ > 0 and assume that

148

6 Velocity Observer Design

λmin (K d ) ≥ δ + 1 kd ≥

(6.32)

δ + kc (vm + ) + (λmax (K d ) + kc (vm + )) , λh 2

1 4

(6.33)

so that (6.31) satisfies V˙ (x) ≤ − δ˙e2 − δ˙z2 1 − ˙e2 + (λmax (K d ) + kc (vm + )) ˙e˙z − (λmax (K d ) + kc (vm + ))2 ˙z2 4  2 1 = − δ˙e2 − δ˙z2 − ˙e − (λmax (K d ) + kc (vm + )) ˙z 2 ≤ − δ˙e2 − δ˙z2 .

(6.34)

Once again V˙ (x) is only negative semidefinite and Theorem 4.3 can be used by considering that V˙ (e, e˙ , z, z˙ ) ≡ 0 only in the set M = {(e, e˙ , z, z˙ ) : e˙ ≡ z˙ ≡ 0}.

(6.35)

To show that the only solution (e, e˙ , z, z˙ ) that can stay identically in M is the equilibrium (e, e˙ , z, z˙ ) ≡ (0, 0, 0, 0), substitute M in (6.7) and (6.23) to get K o z = 0 and K p e = 0. Since K o and K p are positive definite matrices the only solution for the previous relationships to hold are z = 0 and e = 0, which shows that the origin is uniformly asymptotically stable. A region of attraction can be found by using (4.29), (6.27) and the fact that V˙ ≤ 0 for x ∈ D to get 1 1 Mm x(t)2 ≤ V (x(t)) ≤ V (x(t0 )) ≤ MM x(t0 )2 , 2 2

(6.36)

which means that  x(t) ≤

MM x(t0 ) < , Mm

(6.37)

where the last inequality is mandatory to get x(t) ∈ D for all t ≥ t0 . Therefore, a region of attraction is given by A = x(t0 ) ∈ R4n \ x(t0 )
0 to get V˙ (x) ≤ − δs2 − δr2 = −δx2 ≤ 0.

(6.71)

This means that Condition 2 of Corollary 4.1 is met so that the origin x = 0 of (6.50) and (6.53) is asymptotically stable. Note that as made before a region of attraction can be computed to be 

A = x(t0 ) ∈ R

2n

\ x(t0 ) <

λ1  . λ2

(6.72)

Finally, the result cannot be made global despite V (x) is radially unbounded, although the region D can be made arbitrarily large.

6.3 Non Model Based Observer and Control Design

153

6.3 Non Model Based Observer and Control Design As shown in the previous section, it is possible to design a velocity observer without using the robot model. With some modifications, it is also possible to do the same for the controller. Consider once again the observer error definition z in (6.1) and the following two new errors eo = qˆ − qd e˙ o = qˆ˙ − q˙ . d

(6.73) (6.74)

Then, define the following velocity observer q˙ˆ = ζ + z z + kd z ζ˙ = q¨ d − ˙eo + kd z z,

(6.75) (6.76)

where as usual qˆ is obtained by integrating q˙ˆ , qd is a bounded desired trajectory for q, with at least first and second derivatives bounded, , z ∈ Rn×n are diagonal positive definite matrices and kd is a positive constant. Consider now instead of the usual definition of s in (5.50) the following one s = q˙ˆ − q˙ d + (ˆq − qd ) = e˙ o + eo .

(6.77)

Also, instead of q˙ r in (5.49) define q˙ r = q˙ d − eo − K γ σ ,

(6.78)

where K γ ∈ Rn×n is a diagonal positive definite matrix and t σ =

  K β s(ϑ) + sign(s(ϑ)) dϑ σ (t0 ) = 0,

(6.79)

0

where K β ∈ Rn×n is a diagonal positive definite matrix and  T sign(s) = sign(s1 ) · · · sign(sn ) ,

(6.80)

with si element of s, i = 1, . . . , n. Alternatively to (6.79) one can use d σ = K β s + sign(s). dt

(6.81)

154

6 Velocity Observer Design

Finally, use again (6.43) and (6.44), i.e. q˙ o = q˙ˆ − z z so = q˙ o − q˙ r , to define the control law τ = −K p so ,

(6.82)

with K p ∈ Rn×n a positive definite matrix. It is possible to show that gains can always be found to achieve e, e˙ , z, z˙ → 0, but before carrying out a mathematical analysis, let get some insight about what is the basis of such a simple control law. Assume that the observation errors are zero, meaning that qˆ = q and q˙ˆ = q˙ and therefore q˙ o = q˙ , e˙ o = e˙ , eo = e and the usual definition s = e˙ + e is recovered. In this case, the control law (6.82) can be written as   τ = −K p e˙ + e + K γ σ .

(6.83)

In view of the definition of σ in (6.79), then it can be written τ = − K p e − K p K γ σ − K p e˙ ,        P

I

(6.84)

D

which shows that the control law is a nonlinear PID but with the employment of an observer and the user can tuned gains according to his/her own experience. Before carrying out the stability proof, the following lemma is presented. Lemma 6.1 Consider (6.79)–(6.81), and assume that it is possible to form the following relationship u = s + Kγ σ ,

(6.85)

for some bounded input u, i.e. u ≤ u¯ < ∞ for all t ≥ t0 . Then σ and s are bounded for all time. Proof Define V (t, σ ) = 21 σ T σ , so that (4.29) holds with α1 (σ ) = α2 (σ ) =

1 σ 2 . 2

(6.86)

Compute the derivative of V (t, σ ) using (6.81) to get by using (6.85) d V˙ = σ T σ = σ T K β s + σ T sign(s) dt = −σ T K β K γ σ + σ T K β u + σ T sign(s).

(6.87)

6.3 Non Model Based Observer and Control Design

But sign(s) =



sign(s1 )2 + · · · + sign(sn )2 ≤

155

√

n, so that it holds

 √  V˙ ≤ − λmin (K β K γ )σ 2 + σ  λmax (K β )¯u + n    √  1 1 2 ≤ − λmin (K β K γ )σ  − σ  λmin (K β K γ )σ  − λmax (K β )¯u + n . 2 2 (6.88) Then, if  √  2 λmax (K β )¯u + n , σ  ≥ μ = λmin (K β K γ )

(6.89)

1 V˙ ≤ − λmin (K β K γ )σ 2 = −W3 (σ ). 2

(6.90)

one has

Equations (6.87)–(6.90) hold for D = Rn , and therefore a positive value  can always be found to satisfy (4.55). Furthermore, since σ (t0 ) = 0 as given in (6.79), it is easy to show that σ ≤ μ = σmax . Finally, since σ is bounded, from (6.85) s must be bounded as well.  To prove that both tracking and observation errors tend to zero, use r given in (6.45), i.e. r = q˙ − q˙ o = z˙ + z z, and define based on (6.73) and (6.78) and sr = q˙ − q˙ r = e˙ + e − z + K γ σ .

(6.91)

Then, one can rewrite (3.44) as H(q)(˙sr + q¨ r ) + C(q, q˙ )(sr + q˙ r ) + D(sr + q˙ r ) + g(q) = τ ,

(6.92)

or H(q)˙sr + C(q, q˙ )sr + Dsr = τ − ya ,

(6.93)

ya = H(q)¨qr + C(q, q˙ )˙qr + D˙qr + g(q).

(6.94)

where

156

6 Velocity Observer Design

Note that as before so can be rewritten as so = sr − r, so that (6.82) and (6.93) become τ = −K p sr + K p r,

(6.95)

H(q)˙sr + C(q, q˙ )sr + K Dp sr = K p r − ya ,

(6.96)

and

respectively, where K Dp = D + K p . As to the observer error dynamics, first of all note that (6.75) can be written as q¨ˆ = ζ˙ + z z˙ + kd z˙ ,

(6.97)

so that by substituting in (6.76) one gets q¨ˆ = q¨ d − ˙eo + kd z z + z z˙ + kd z˙ = q¨ d − ˙eo + z z˙ + kd r.

(6.98)

Equivalently one has q¨ − q¨ d + ˙eo + K γ

d d σ = q¨ − q¨ˆ + z z˙ + kd r + K γ σ , dt dt

(6.99)

so that from (6.43), (6.45), (6.78) and (6.91) it is r˙ + kd r = s˙r − K γ

d σ. dt

(6.100)

Define x=

  sr r

(6.101)

as state for (6.96) and (6.100) and choose for this state a region D similar to that given in (4.28), i.e.   D = x ∈ R2n \ x ≤ b , for some arbitrarily large 0 < b < ∞. Although the notation  could have been employed as usual, this time it is not asymptotic stability but ultimate boundedness of the state what it is sought for the reasons that will be explained later, so that the notation complies with Definition 4.9.1. Note also that ≤ is used instead of 0 then condition (4.53) holds with V˙ ≤ −δx2 = −W3 (x).

(6.119)

To apply Theorem 4.5 it is only necessary to find  so that (4.55) is satisfied, i.e.  μ
0. This can be done by choosing λmin (K γ ) large enough. Therefore, there exists a finite time tr so that s(t) ≡ 0 for all t ≥ tr ≥ t0 according to the discussion of Section 4.4. This means that from t ≥ tr one has s(t) = 0, so that (6.77) becomes e˙ o + eo ≡ 0.

(6.124)

e˙ o + eo = e˙ + e − z˙ − z = 0,

(6.125)

Since

equation (6.91) can be rewritten as sr = e˙ o + eo + z˙ + z − z + K γ σ = r − z z + K γ σ .

(6.126)

Compute its derivative to get s˙r − r˙ = −z z˙ + K γ

d σ. dt

(6.127)

But from (6.100) it is also s˙r − r˙ = kd r + K γ

d σ. dt

(6.128)

Then, from (6.127)–(6.128) one has (kd I + z )˙z + kd z z = 0.

(6.129)

Equation (6.129) represents a stable linear filter for z, so that z and z˙ tend to zero. This in turn means from (6.125) that e and e˙ do tend to zero as well.

6.4 Experimental Results

161

6.4 Experimental Results In this section, a set of experiments is presented to show the performance of the above observers. For this purpose, the Phantom Omni device described in Chapter 14 is employed. For the experiments below, only the first three degrees of freedom are used, while the remaining three are mechanically blocked, as explained in Sect. 14.2. The dynamic model (14.3) along with the parameters shown in Sect. 14.2.5 are employed. The experiments presented below are: 1. The Nicosia-Tomei (NT) observer (6.2)–(6.3) along with the control law (6.22) 2. The non-model based observer (NMO) in (6.39)-(6.40) with the control law (6.41) 3. The non model based observer and controller (NMOC) given in (6.75)–(6.76) and (6.82) For all the experiments, the desired joint trajectories are qdi (t) = ai + bi (1 − cos(ωd t)) ,

(6.130)

where (a1 , a2 , a3 ) = (−30◦ , 20◦ , −60◦ ), (b1 , b2 , b3 ) = (30◦ , 30◦ , −30◦ ), and ωd = π . The gains chosen for the experiments are shown in Table 6.1. Figures 6.1 and 6.2 show the tracking behavior. It is important to recall that this is the main objective and it can be concluded that the three control-observer approaches deliver similar outcomes. Regarding the observers performances, they are difficult to evaluate because the actual velocity q˙ is not known. Therefore, Fig. 6.3 shows the estimated values vs the desired ones, while Fig. 6.4 shows the corresponding errors. Finally, the input torques are shown in Fig. 6.5. Certainly, one could have expected the worst behavior for the NT scheme because of its complete dependence on the robot model, but as a matter of fact the discretization used to implement the different algorithms plays an important role. Therefore, even a relatively small sampling time can largely increase the error in calculating the integral term of the NMOC, which explains why a zero tracking or observation error is not achieved as foreseen theoretically. Still, perhaps this is the best option simple because it does not need any robot model at all and it is easier to program.

Table 6.1 Gains chosen for the experiments Scheme Observers’ gains NT NMO NMOC

kd = 100, K o = 0.001I kd = 100, z = I kd = 100, z = I,  = 50I

Controllers’ gains K p = 4I, K d = 0.08I K d = 0.1I,  = 25I K β = I, K γ = I, K o = 0.1I

162

6 Velocity Observer Design

Fig. 6.1 Joint position tracking: desired (· · · ), NT (- . -), NMO (—), NMOC (– –)

Fig. 6.2 Joint position error: NT (- . -), NMO (—), NMOC (– –)

6.4 Experimental Results

163

Fig. 6.3 Joint velocities estimation and tracking: desired (· · · ), NT (- . -), NMO (—), NMOC (– –)

Fig. 6.4 Joint velocities error: NT (- . -), NMO (—), NMOC (– –)

164

6 Velocity Observer Design

Fig. 6.5 Input torques: NT (- . -), NMO (—), NMOC (– –)

References Arteaga-Pérez MA, Castillo-Sánchez AM, Parra-Vega V (2006) Cartesian control of robots without dynamic model and observer design. Automatica 42:473–480 Arteaga-Pérez MA, Ortiz-Espinoza A, Romero JG, Espinoza-Pérez G (2020) On the adaptive control of robot manipulators with velocity observers. Int J Rob Nonlinear Control 30:4371–4396 Nicosia S, Tomei P (1990) Robot control by using only joint position measurements. IEEE Trans Autom Control 35(9):1058–1061

Chapter 7

Adaptive and Robust Control

Abstract In the previous two chapters it was shown that the control of robot manipulators is quite direct despite the high nonlinear nature of these systems as long as the complete robot model is available. It was also shown that some techniques based on sliding mode control theory can be employed to overcome the complete lack of the robot analytical model and even the lack of velocity measurements. A possible drawback of these control laws is that the discretization process for computer implementation tends to diminish the overall system performance. This chapter presents other well known control techniques employed to deal with a poor knowledge of the robot model parameters.

7.1 The Adaptive Law by Slotine and Li Most of the control laws studied in Chaps. 5 and 6 rely on the exact knowledge of the robot manipulators. However, in practice there is usually some parameter uncertainty that might just decrease performance. For example, consider the nominal control law given by (5.51), i.e. τ n = H(q)¨qr + C(q, q˙ )˙qr + D˙qr + g(q) − K d s, where (5.1), (5.49) and (5.50) are used, i.e. e = q − qd q˙ r = q˙ d − e s = q˙ − q˙ r = e˙ + e, and K d ,  ∈ Rn×n are positive definite diagonal matrices. However, the actual controller is rather given by ˆ ˆ qr + gˆ (q) − K d s, ˆ qr + C(q, q˙ )˙qr + D˙ τ = H(q)¨

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control of Robot Manipulators, Lecture Notes in Electrical Engineering 798, https://doi.org/10.1007/978-3-030-85980-0_7

(7.1)

165

166

7 Adaptive and Robust Control

where (ˆ·) means that estimated values instead of the actual model parameters are being used. At this point it is worthy to point out that implicitly it is assumed that the robot model structure is well known and what is uncertain are constant parameters just as moments of inertia, link masses or lengths, for example. This is reasonable because the robot kinematic is usually well known, but masses and center of masses are hard to compute exactly due to a variety of factors, like the links shapes and non uniform masses densities. In the last part of the book the computation of many industrial robots is described, but for the moment let’s take advantage of Property 3.14 to rewrite (7.1) as ˆ ˆ qr + gˆ (q) − K d s ˆ qr + C(q, q˙ )˙qr + D˙ τ = H(q)¨ = Y(q, q˙ , q˙ r , q¨ r )ϕˆ − K d s,

(7.2)

where ϕˆ represents an estimated or nominal value of the actual vector of parameters ϕ. An acceptable value of ϕˆ can provide a good control performance and nothing else is advisable to do, but if uncertainties are not negligible, then it may be necessary to increase performance by somehow compensating them explicitly. Two common options are adaptive control and Lyapunov redesign. The former estimates ϕˆ online until the performance is adequate and the latter adds a second term to the nominal control law meant to deal with uncertainties and perturbations. It is also possible to combine both techniques. One of the best known adaptive approaches is the one introduced by Slotine and Li. For simplicity define Y a = Y(q, q˙ , q˙ r , q¨ r ),

(7.3)

ˆ ˆ qr + gˆ (q) − K d s = Y a ϕˆ − K d s. ˆ qr + C(q, q˙ )˙qr + D˙ τ = H(q)¨

(7.4)

so that (7.2) can be written as

In fact, the nominal control law can be written in the very same fashion as τ n = H(q)¨qr + C(q, q˙ )˙qr + D˙qr + g(q) − K d s = Y a ϕ − K d s,

(7.5)

where it can clearly be appreciated that the difference between the actual and the nominal laws lies in the parameter vector. Add and subtract Y a ϕ from (7.4) to get τ = Y a ϕˆ − K d s + Y a ϕ − Y a ϕ   = Y a ϕ − K d s + Y a ϕˆ − ϕ ˜ = τ n + Y a ϕ,

(7.6)

ϕ˜ = ϕˆ − ϕ

(7.7)

where

7.1 The Adaptive Law by Slotine and Li

167

is the parameter error. Therefore, by substituting (7.6) in the robot model (3.44) the closed loop error dynamics can be straightforwardly computed as ˜ H(q)˙s + C(q, q˙ )s + K d s = Y a ϕ,

(7.8)

where K D = D + K d . Also this equation allows to see that if ϕ˜ ≡ 0 then s → 0 and e, e˙ → 0 as shown in Sect. 5.4. Assume now that ϕˆ varies so that a vector error state x ∈ Rn+p can be introduced as   s x= , (7.9) ϕ˜ although ϕ˙˜ is still undefined. Consider the following Lyapunov candidate function for s and ϕ˜ V (x) =

1 1 T ˜ s H(q)s + ϕ˜ T  −1 ϕ, 2 2

(7.10)

where  ∈ Rp×p is a diagonal positive definite matrix. The derivative of V (x) along (7.8) is given by 1 ˙ + ϕ˙˜ T  −1 ϕ˜ V˙ (x) = sT H(q)˙s + sT H(q)s 2 ˜ = −sT K D s + sT Y a ϕ˜ + ϕ˙˜ T  −1 ϕ.

(7.11)

As usual, Property 3.2 has been employed. Note that ϕ is a constant vector and therefore from (7.7) one gets ˙ˆ ϕ˙˜ = ϕ.

(7.12)

Choosing ϕ˙ˆ implies the design of an adaptive law. The most obvious choice is ϕ˙˜ = ϕ˙ˆ = −Y Ta s,

(7.13)

V˙ (x) = −sT K D s.

(7.14)

so that (7.11) becomes

Clearly, V˙ (x) is negative semidefinite and V˙ (x) ≡ 0 only in the set ˜ : s ≡ 0}, M = {(s, ϕ)

(7.15)

168

7 Adaptive and Robust Control

so that it is tempting to use Theorem 4.3. However, substituting s ≡ 0 in (7.8) leads to Y a ϕ˜ = 0.

(7.16)

Unfortunately, x ≡ 0 is not the only solution x that can stay identically in M since ϕ˜ may happen to lie in the null space of Y a and Theorem 4.3 cannot be employed. This does not only imply that the equilibrium x ≡ 0 of (7.8) and (7.13) cannot be shown to be asymptotically stable, but in fact it cannot even be shown that lim s = 0.

t→∞

(7.17)

The fact that V˙ (x) ≤ 0 while V (x) is lower bounded by 0 implies that x is bounded for all t ≥ t0 and that V (x) tends to a constant value. Technically, this does not mean that V˙ (x) → 0 and therefore (7.17) does not hold necessarily. However, this can be proven by using Barbalat’s Lemma 4.5, for which it only has to be shown that V¨ (x) is bounded. By direct computation it is V¨ (x) = −2sT K D s˙ ˜ , = 2sT K D H −1 (q) (C(q, q˙ )s + K D s − Y a ϕ)

(7.18)

which shows that V¨ (x) is bounded and as stated by Barbalat’s Lemma V˙ (x) → 0 and as consequence (7.17) holds, so that in the end e and e˙ tend to zero as well, which is the main control objective. A natural question is whether it is also possible to have ϕ˜ → 0. In fact, it can be shown that if the regressor Y a is persistently exciting, i.e. if there exist positive constants T1 and α1 such that for all t ≥ t0 it holds t+T  1

Y Ta (t)Y a (t)dt ≥ α1 I,

(7.19)

t

then additionally to e, e˙ → 0 it is also ϕ˜ → 0. A formal technical proof is out of the scope of this book but it can be found in the references provided at the end of this chapter.

7.2 Adaptive Scheme with Velocity Observers Assume now that joint velocities are not available. For that case the use of an observer for velocity estimation is advisable. However, the problem becomes more involved if the estimated parameters are used both for the observer and the controller. A possibility is to employ the observer introduced in Sect. 6.2 in Eqs. (6.1) and (6.39)– (6.40), i.e.

7.2 Adaptive Scheme with Velocity Observers

169

z = q − qˆ ξ˙ = z q˙ˆ = q˙ r + kd (z + z ξ ) + z z, where kd is a positive real constant, z ∈ Rn×n is a diagonal positive definite matrix, and qˆ is obtained by integrating q˙ˆ . For that case the control law (6.41) becomes ˆ ˆ qr + gˆ (q) − K d so , ˆ q¨ˆ r + C(q, q˙ r )˙qr + D˙ τ = H(q)

(7.20)

where (6.42)–(6.44) have been employed, i.e.   q¨ˆ r = q¨ d −  q˙ o − q˙ d q˙ = q˙ˆ − z z o

so = q˙ o − q˙ r . Note the similarity of (7.1) with (7.20). The latter can be rewritten as ˜ τ = H(q)q¨ˆ r + C(q, q˙ r )˙qr + D˙qr + g(q) − K d so + Y a ϕ,

(7.21)

where the parameter error defined in (7.7) is used and ˜ q¨ˆ r + C(q, ˜ ˜ qr + g˜ (q). q˙ r )˙qr + D˙ Y a ϕ˜ = Y(q, q˙ r , q¨ˆ r )ϕ˜ = H(q)

(7.22)

After (6.49), i.e. τ o = τ n + H(q)r − C(q, q˙ r )s + K d r, Equation (7.21) can be rewritten as ˜ τ = τ n + H(q)r − C(q, q˙ r )s + K d r + Y a ϕ, where (6.45)–(6.47) have been used, i.e. r = q˙ − q˙ o = z˙ + z z q¨ˆ = q¨ + r r

r

so = s − r. By considering (6.50), i.e. H(q)˙s + C(q, q˙ )s + K D s = H(q)r − C(q, q˙ r )s + K d r,

(7.23)

170

7 Adaptive and Robust Control

with K D = D + K d , the closed loop dynamics for s is given by ˜ H(q)˙s + C(q, q˙ )s + K D s = H(q)r − C(q, q˙ r )s + K d r + Y a ϕ,

(7.24)

while instead of (6.53), i.e. r˙ + K H r = Bs, one has ˜ r˙ + K H r = Bs + H −1 (q)Y a ϕ,

(7.25)

where (6.54) and (6.55) are considered, i.e. K H = kd I −  − H −1 (q)K d   B = −H −1 (q) C(q, q˙ ) + K D + C(q, q˙ r ) . It is easy to note that the only differences between (6.50) and (7.24) and ˜ respectively. In between (6.53) and (7.25) are the terms Y a ϕ˜ and H −1 (q)Y a ϕ, Sect. 6.2 it was shown that for ϕ˜ ≡ 0 then s, r → 0 and consequently e, e˙ , z, z˙ → 0. ˜ ≤ ϕmax < ∞ Intuitively in general this will not be the case, but also intuitively if ϕ ∀ t ≥ t0 , the tracking and observation errors will not tend to zero but will remain ultimately bounded. To check this intuition out and to find out whether the ultimate bound can be made arbitrarily small or not, consider the following state defition y=

  s , r

(7.26)

for (7.24) and (7.25). Note that this definition excludes ϕ˜ as part of the state, but ˜ ≤ ϕmax < ∞ this is on purpose to treat it rather as a perturbation. Note also that ϕ ∀ t ≥ t0 can be achieved simply by considering ϕˆ to be constant or by design of an adaptive law specially for this goal. Then consider once again a region Dy as in (4.28), say   Dy = y ∈ R2n \ y <  , for some arbitrarily large 0 <  < ∞. With this background Theorem 4.5 can be employed by using V (y) =

1 1 T s H(q)s + rT r, 2 2

(7.27)

7.2 Adaptive Scheme with Velocity Observers

171

which satisfies (4.29), i.e. α1 (y) ≤ V (t, y) ≤ α2 (y), by considering (6.62) and (6.63), i.e. 1 λ1 y2 2

and

α2 (y) =

λ1 = min {λH , 1}

and

λ2 = max {λH , 1} ,

α1 (y) =

1 λ2 y2 , 2

and

where λh and λh are given in Property 3.7. To show that condition (4.53) also holds for some μ > 0, it is not difficult to get from (6.64)–(6.71) that the derivative of V (y) along (7.24)–(7.25) satisfies   ˜ V˙ (y) ≤ −δy2 + sT + rT H −1 (q) Y a ϕ.

(7.28)

Note that from (6.46) the boundedness of q¨ˆ r in Dy can be concluded by considering that the derivative of q˙ r = q˙ d − e must be bounded in Dy . This means that there exists some yam > 0 so that Y a  ≤ yam < ∞ for y ∈ Dy . Therefore, (7.28) satisfies

1 2 ˙ yam ϕmax V (y) ≤ − δy + y 1 + λh 1 = − δy2 2 

1 1 yam ϕmax , δy − 1 + − y 2 λh

(7.29)

where λH is given in Property 3.7. Clearly, whenever

1 2 yam ϕmax = μ, 1+ y ≥ δ λh

(7.30)

V˙ (y) ≤ −δy2 = −W3 (y).

(7.31)

it holds

To comply with condition (4.55), i.e. μ < α2−1 (α1 ()),

172

7 Adaptive and Robust Control

one must have that

λ1 , λ2

(7.32)



1 1+ yam ϕmax < δ, λh

(7.33)

μ< which will be satisfied as long as

2 

λ2 λ1

for δ > 0. Furthermore, note that (4.56), i.e. y(t0 ) = y0  ≤ α2−1 (α1 ()) actually complies with the region A given in (6.72), i.e.

 A = y(t0 ) ∈ R2n \ y(t0 )
0 must be found so that a combination of gains to get another Vϕ (ϕ) x → 0 can be found. This positive definite function must exist if Yˆ f is persistently exciting as said before, but it appears to be a waste of time to look for it because in the end Yˆ f satisfying this condition is only an assumption which may be hard to enforce. On the contrary, consider the more reasonable following assumption.

176

7 Adaptive and Robust Control

Assumption 7.1 For each element ϕi of the parameter ϕ with i = 1, . . . , p, lower and upper bounds, ϕmi and ϕMi , are known which satisfy ϕmi ≤ ϕi ≤ ϕMi .

(7.54)

This assumption is more relaxed than it appears to be because at least the sign of the parameter ϕi is always known (say it is positive), so that the lower bound is 0. The upper bound can be considered any constant value arbitrarily large. Then, instead of (7.47) consider   T ˙ˆ ϕ(t) = − β Yˆ f  + Y Ta so + f b

ϕmi ≤ ϕˆi (0) ≤ ϕMi ,

(7.55)

where for i = 1, . . . , p, each element of f b ∈ Rp is given by ⎧ T ϕ − ϕˆ ⎪ ⎪ − ϕ 2i − ϕ i ρi |β Yˆ fi  + yTai so | if ϕ1i ≤ ϕˆi < ϕ2i ⎪ 2i 1i ⎪ ⎪ ⎪ ⎨ 0 if ϕ2i ≤ ϕˆi ≤ ϕ3i , fbi = ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ϕˆ i − ϕ3i ρ |β Yˆ T  + yT s | if ϕ < ϕˆ ≤ ϕ 3i i 4i fi ai o ϕ4i − ϕ3i i

(7.56)

T T where ρi > 1, Yˆ fi is the ith row of Yˆ f , yTai is the ith row of Y Ta , and ϕ1i < ϕ2i ≤ ϕmi < ϕMi ≤ ϕ3i < ϕ4i . The effect of (7.56) is to keep the estimated parameters bounded for all t ≥ t0 , which as discussed before allows at the very least to get ultimate boundedness for s and r. To show this note that the ith element of ϕ˙˜ satisfies

 T  ϕ˙˜ i = ϕ˙ˆi = −γi β Yˆ fi  + yTai so + fbi ,

(7.57)

where γi is the ith element of the diagonal of  for i = 1, . . . , p. Define the positive definite function V =

1 2 ϕ˜ , 2γi i

(7.58)

whose derivative along (7.57) satisfies T V˙ ≤ |ϕ˜ i | · |β Yˆ fi  + yTai so |

(7.59)

if ϕ2i ≤ ϕˆ i ≤ ϕ3i , but quite obviously ϕˆ i is bounded. On the contrary, if ϕˆi < ϕ2i ≤ ϕmi then sign(ϕ˜i ) = −1, while if ϕMi ≤ ϕ3i < ϕˆi then sign(ϕ˜ i ) = +1, so that one has

7.2 Adaptive Scheme with Velocity Observers

177

ϕ2i − ϕˆi T − 1 |ϕ˜i | · |β Yˆ fi  + yTai so |, V˙ ≤ − ρi ϕ2i − ϕ1i

(7.60)

for ϕ2i ≤ ϕˆi ≤ ϕ3i and

ϕˆi − ϕ3i T − 1 |ϕ˜ i | · |β Yˆ fi  + yTai so |, V˙ ≤ − ρi ϕ4i − ϕ3i

(7.61)

for ϕMi ≤ ϕ3i < ϕˆi . But, due to the restriction on the initial condition, the worst possible case for (7.60) and (7.61) can be ϕˆi = ϕ1i or ϕˆi = ϕ4i , respectively, so that they become V˙ ≤ −(ρi − 1)|ϕ˜i | · |βwi + yTai so |.

(7.62)

Since ρi > 1 then V˙ < 0 implying that |ϕ˜i | diminishes its value and thus it has to hold ϕ1i ≤ ϕˆi (t) ≤ ϕ4i . This shows the boundedness of the estimated parameters gotten from (7.55) and as a direct consequence the possibility of achieving ultimate boundedness with arbitrarily small bound for s and r. Finally, for the unaware reader it may be difficult to understand at first sight why filtering the regressor eliminates acceleration terms. The following example is intended to clarify this issue. Example

Consider the regressor presented in the example after Property 3.14: ⎡ ⎢ ⎢ ⎣ 

q¨ 1 0

    c2 q¨ 1 + 13 + 21 c2 q¨ 2 − 21 s2 q˙ 2 q˙ 1 − 21 s2 q˙ 1 + 21 s2 q˙ 2 q˙ 2   1 + 1 c q¨ + 1 q¨ + 1 s q˙ q˙ 2 1 3 2 3 2 2 2 1 1  Y(¨q,˙q,q)

c1 0

⎤ c1 + 21 c12 ⎥ ⎥ ϕ = τ, ⎦ 1c 12 2 

with s2 = sin(θ2 ), c1 = cos(θ1 ), c2 = cos(θ2 ), c12 = cos(θ1 + θ2 ) and the parameter vector given by ⎡

⎤ 1 m l2 + 4 m l2 1 2 3 3 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ϕ2 ⎥ ⎢ m2 l 2 ⎥ ⎢ ⎥ ⎢ ⎥ ⎥ ϕ=⎢ ⎥. ⎢ ⎥=⎢ ⎢ ⎥ ⎢ ϕ3 ⎥ ⎢ 1m g l ⎥ ⎢ ⎥ ⎢ 1 0 ⎥ 2 ⎣ ⎦ ⎣ ⎦ ϕ4 m2 g0 l ⎡

ϕ1

⎤

178

7 Adaptive and Robust Control

If yij is the ijth element of Y(q, q˙ , q¨ ), for i = 1, 2 and j = 1, . . . , 4, then according to (7.41) every element yfij of Y f (q, q˙ ) can be computed by solving y˙ fij = −λf yfij + λf yij , whose solution is given by yfij (t) = yfij (0)e

−λf t

t

e−λf (t−ϕ) yij (ϕ)dϕ

+ λf 0

= yfij (0)e

−λf t

+ λf e

−λf t

t

eλf ϕ yij (ϕ)dϕ.

0

Without lost of generality it can be chosen yfij (0) = 0 to get yfij (t) = λf e−λf t

t

eλf ϕ yij (ϕ)dϕ.

0

In general, it is not possible to compute a closed form solution of the previous relationship and it has to be solved online, but to eliminate acceleration terms integration by parts can be used according to 

 vdw = vw −

wdv.

(7.63)

For example, for the element y11 = q¨ 1 it has to be solved yf11 (t) = λf e

−λf t

t

eλf ϕ q¨ 1 dϕ.

0

By choosing dw = q¨ 1 dϕ

v = e λf ϕ ,

and

to get ⎛ yf11 = λf e

−λf t

⎝e q˙ 1 − λf t

⎞

t λf e 0

λf ϕ

q˙ 1 dϕ ⎠ = λf q˙ 1 −

t λ2f 0

e−λf (t−ϕ) q˙ 1 dϕ,

7.2 Adaptive Scheme with Velocity Observers

179

it can be appreciated that q¨ 1 has disappeared. But consider also the factor c2 q¨ 1 in yf22 . For that case t e

λf ϕ

cos(q2 )¨q1 dϕ = e

λf t

t cos(q2 )˙q1 −

0

 λf ϕ  λf e cos(q2 ) − eλf ϕ sin(q2 )˙q2 q˙ 1 dϕ.

0

Therefore the partial solution for c2 q¨ 1 is given by t λf cos(q2 )˙q1 − λf

e−λf (t−ϕ) (λf cos(q2 )˙q1 − sin(q2 )˙q2 q˙ 1 ) dϕ,

0

where once again the acceleration has disappeared. The rest of yf22 and of the elements of Y f (q, q˙ ) can be computed in the same way.

7.3 Robust Control Adaptive control is a good option to compensate the lack of an accurate model parameter vector. However, an alternative consists in working with a constant parameter vector and to add a robust term to the nominal control law aimed at compensating the inaccuracies. Similar to Assumption 7.1 it is made: Assumption 7.2 For a nominal constant value of the parameter vector ϕ, denoted as ϕ 0 , an upper bound ρ < ∞ for the parameter error ϕ˜ is known, i.e. ˜ = ϕ 0 − ϕ ≤ ρ ϕ

(7.64)

holds. This way, instead of (7.1) one has a nominal control law given by τ 0 = H 0 (q)¨qr + C 0 (q, q˙ )˙qr + D0 q˙ r + g0 (q) − K d s = Y a ϕ 0 − K d s,

(7.65)

with K d > O as usual, Y a = Y(q, q˙ , q˙ r , q¨ r ) as in (7.3), and (5.1), (5.49) and (5.50) have been employed. Since in general ϕ 0 = ϕ, the control law (7.65) can be complemented in the following fashion τ = τ 0 + Y a f = Y a (ϕ 0 + f ) − K d s,

(7.66)

where f ∈ Rp is designed to compensate the error ϕ˜ = ϕ 0 − ϕ defined similar to (7.7). According to (7.8), it is not difficult to show that the closed loop dynamics for s is given by

180

7 Adaptive and Robust Control

H(q)˙s + C(q, q˙ )s + K D s = Y a (ϕ˜ + f ),

(7.67)

where K D is defined as before. The last equation shows that the term f can directly ˜ A possibility is to choose be used to deal with the error ϕ. ⎧ Y Ta s ⎪ T ⎪ ⎨ −ρ Y T s if Y a s > η a f = ⎪ ⎪ ⎩ ρ − η Y Ta s if Y Ta s ≤ η

(7.68)

Note that this term is continuous for η > 0. To show the effect of f consider once again V (t, s) =

1 T s H(q)s, 2

which satisfies (4.29), i.e. α1 (s) ≤ V (t, s) ≤ α2 (s), with α1 (s) =

1 λh s2 2

and

α2 (s) =

1 λH s2 , 2

where λh and λH are given in Property 3.7. The derivative of V (s) along (7.67) is given by V˙ (t) = −sT K D s˙ + sT Y a (ϕ˜ + f ) ≤ −λmin (K d )s2 + sT Y a (ϕ˜ + f ). Assume now that Y Ta s > η. This means that f = −ρ

Y Ta s , Y Ta s

and therefore it is

 T T Y Ta s s Y a (ϕ˜ + f ) = Y a s ϕ˜ − ρ T Y a s T

˜ −ρ ≤ Y Ta sϕ

Y Ta s2 Y Ta s

≤ Y Ta sρ − ρY Ta s = 0.

(7.69)

7.3 Robust Control

181

On the contrary, if Y Ta s ≤ η, then ρ f = − Y Ta s, η and therefore it is

ρ T ϕ˜ − Y a s s Y a (ϕ˜ + f ) = η ρ T 2 T ≤ Y a sρ − Y a s η

1 = ρ Y Ta s − Y Ta s2 η

1 T T = ρY a s 1 − Y a s , η 

T

T Y Ta s

(7.70)

which is valid for 0 ≤ Y Ta s ≤ η and it is always positive except for the border values where it becomes zero. The worst possible case is given at the maximum with respect to Y Ta s, which can be computed according to

2 1 T 2 ∂ T = 1 − Y Ta s. Y s − s Y a a T η η ∂Y a s Therefore, the maximum is at Y Ta s =

η . 2

By substituting in (7.70) one has η sT Y a (ϕ˜ + f ) ≤ ρ . 4

(7.71)

Of course, (7.71) can be considered valid even if η < Y Ta s, so that substituting in (7.69) yields to η V˙ ≤ −λmin (K d )s2 + ρ 4 1 η 1 2 ≤ − λmin (K d )s − λmin (K d )s2 + ρ . 2 2 4 This way it is easy to see that if ρ

η 1 ≤ λmin (K d )s2 , 4 2

182

7 Adaptive and Robust Control

then V˙ < 0. This is equivalent to ρη ≤ s2 , 2λmin (K d ) or $ μ=

ρη ≤ s. 2λmin (K d )

Therefore, if s ≥ μ it is 1 V˙ ≤ − λmin (K d )s2 = −W3 (s), 2 which complies with condition (4.53) of Theorem 4.5. Note that this time it was not really necessary to define a region D because D ≡ Rn and the result holds for any initial condition s(t0 ) or equivalently for any e(t0 ) and e˙ (t0 ). Finally, the ultimate bound can be computed using (4.73), which for this case is

bf =

λH μ= λH

ρ ηλH . 2λmin (K d )λH

Clearly, bf can be made arbitrarily small by setting η also arbitrarily small. As a direct consequence, both e and e˙ can be made arbitrarily small.

7.4 Control-Observer Robust Scheme Just as made before for the adaptive scheme, in this section the tracking control problem of rigid robot arms with model parameter uncertainty and without velocity measurements is studied. Consider the usual definitions (5.1), (6.1), (6.43), (6.45) and (6.73) i.e. e = q − qd z = q − qˆ q˙ = q˙ˆ − z z o

r = q˙ − q˙ o = z˙ + z z eo = qˆ − qd , ˆ denotes the estiwith z ∈ Rn×n a diagonal positive definite matrix and where (·) mated (or nominal) value of (·). It is introduced

7.4 Control-Observer Robust Scheme

183

q˙ r = q˙ d − eo = q˙ d − e + z s = q˙ − q˙ r = e˙ + e − z,

(7.72) (7.73)

with  ∈ Rn×n a diagonal positive definite matrix. Let ϕ 0 be the nominal (constant) value of the robot model parameters vector, given by ϕ, while for the parameter error vector ϕ˜ = ϕ 0 − ϕ, the following assumption is made: ˜ For i = 1, . . . , p bounds are known Assumption 7.3 Let ϕ˜i be the ith element of ϕ. such that it holds ϕ˜i  ≤ ϕmi .

(7.74)

As before, the bounds ϕmi can be set arbitrarily large if necessary. Similar to (6.39) and (6.40), the following linear observer is proposed: q˙ˆ = ξ + z z + kd z ξ˙ = q¨ r + kd z z,

(7.75) (7.76)

where kd is a positive real constant and qˆ is obtained by integrating q˙ˆ . The proposed controller is given by   τ = H 0 (q)¨qr + C 0 (q, q˙ r )˙qr + D0 q˙ r + g0 (q) − K d q˙ o − q˙ r + f   = Y a ϕ 0 − K d q˙ o − q˙ r + f ,

(7.77)

where K d , ∈ Rn×n is a diagonal positive definite matrix and as before Y a = Y(q, q˙ r , q¨ r ). To design the robust term f , denote the columns of Y a as yai ∈ Rn , for i = 1, . . . , p. In view of Assumption 7.3, the vector f can be defined as f =−

p % i=1

  ϕmi yTai sˆ + rˆ   yai , yTai sˆ + rˆ  + ηi

(7.78)

where sˆ + rˆ = q˙ˆ − q˙ d + e, and ηi , i = 1, . . . , p, are positive constants. Since q˙ o − q˙ r = s − r, (7.77) can be written as τ = H(q)¨qr + C(q, q˙ r )˙qr + D˙qr + g(q) + Y a ϕ˜ − K d (s − r) + f .

(7.79)

184

7 Adaptive and Robust Control

By substituting (7.79) in the robot model (3.44) and using Property 3.4 to have C(q, q˙ r )˙qr = C(q, q˙ )˙qr + C(q, q˙ r )˙qr − C(q, q˙ )˙qr = C(q, q˙ )˙qr − C(q, q˙ r )s,

(7.80)

the tracking error closed loop dynamics satisfies H(q)˙s + C(q, q˙ )s + K D s = K d r − C(q, q˙ r )s + Y a ϕ˜ + f ,

(7.81)

where K D = D + K d . As to the observer error dynamics from (7.75) one has q¨ˆ = ξ˙ + z z˙ + kd z˙ ,

(7.82)

which substituted in (7.76) leads to r˙ + kd r = s˙.

(7.83)

By multiplying this equation by H(q) and using (7.81), one gets H(q)˙r + kd H(q)r = −C(q, q˙ )s − K D s + K d r − C(q, q˙ r )s + Y a ϕ˜ + f .

(7.84)

Using once again Property 3.4 to have C(q, q˙ )s + C(q, q˙ r )s = C(q, q˙ )r − C(q, q˙ )r + C(q, q˙ + q˙ r )s,

(7.85)

yields to H(q)˙r = − C(q, q˙ )r − H rd r − K D s + C(q, q˙ )r − C(q, q˙ + q˙ r )s + Y a θ˜ + f ,

(7.86)

where H rd = kd H(q) − K d . Define for (7.81) and (7.86) the state x as x=

  s , r

(7.87)

for which a region of the form (4.28) is defined as   D = x ∈ R2n \ x <  , for some 0 <  < ∞. As usual, the key idea is that whenever x ∈ D then the boundedness of r implies that of z and z˙ due to z˙ = −z z + r. On the other hand, the boundedness of s and z implies from (7.73) that e and e˙ must be bounded. Also, since qd and its derivatives are bounded by assumption then so are q and q˙ . Therefore, from (7.72) q˙ r and q¨ r must be bounded, and as direct consequence Y a (q, q˙ r , q¨ r ) is

7.4 Control-Observer Robust Scheme

185

bounded as well. This in turn means that f is bounded according to (7.78) and so must be then s˙ in (7.81) in view of Property 3.1. From (7.86) this means that r˙ is bounded as well. Consider V (x) =

1 1 T s H(q)s + rT H(q)r, 2 2

(7.88)

which satisfies (4.29), i.e. α1 (x) ≤ V (t, x) ≤ α2 (x), with α1 (x) =

1 λh x2 2

α2 (x) =

and

1 λh x2 . 2

(7.89)

λh and λH are given in Property 3.7. By taking into account Property 3.2, compute the derivative of V (x) along (7.81) and (7.86) to get V˙ = − sT K D s + sT K d r − sT C(q, q˙ r )s + (s + r)T (Y a ϕ˜ + f ) − rT H rd r − rT K D s + rT C(q, q˙ )r − rT C(q, q˙ + q˙ r )s. For x ∈ D there must exist bounds such that ˙q ≤ vm ˙qr  ≤ vrm hold. Furthermore, define μa = 2

p %

max yai ϕmi ,

i=1

so that ˜ ≤ Y a ϕ

μa , 2

and   p % ϕmi yai yTai sˆ + rˆ  μa   , f  ≤ ≤ T 2 yai sˆ + rˆ  + ηi i=1

(7.90)

186

7 Adaptive and Robust Control

hold. Then, by using K D = D + K d and taking into account that Properties 3.7, 3.9 and 3.10 one has V˙ ≤ − λmin (K d )s2 + kc vrm s2 + μa (s + r) − (kD λh − λmax (K d ))r2 + λmax (D)sr + kc vm r2 + kc (vm + vrm )rs.

(7.91)

Therefore, by choosing λmin (K d ) ≥ 2δ + 1 + kc vrm kd ≥

(7.92)

2δ + λmax (K d ) + kc vm +

1 (λ (D) 4 max

+ kc (vm + vrm ))

λH

2

,

(7.93)

one gets from (7.91) V˙ ≤ −δx2 − x(δx − μa ).

(7.94)

For x ≥

μa =μ δ

(7.95)

it is V˙ ≤ −δx2 = −W3 (x),

(7.96)

subject to

μ
0. At this point, it is evident that the smaller one wishes bf to be the larger control gains must be. However, f tries to avoid using large gains by mimicking up to some point (7.68). To understand this, for a small bf it holds sˆ + rˆ ≈ s + r and therefore q˙ˆ ≈ q˙ . Make this substitution in (7.78) to have the term (s + r)T (Y a ϕ˜ + f ) in (7.90) approximated by

7.4 Control-Observer Robust Scheme

187

(s + r)T (Y a ϕ˜ + f )

p % ϕmi yT (s + r) 2 ≤ yTai (s + r) ϕmi − T ai yai (s + r)  + ηi i=1 ≤

p %

ϕmi ηi = η.

(7.99)

i=1

In other words, if q˙ˆ ≈ q˙ , f becomes a robust term like the one given in (7.68), where joint velocities are available. Using the same arguments as in Sect. 7.3, it is straightforward to show that making the parameters ηi small enough may do x tend to an even smaller region than the nominal one given by (7.98). While this is not a formal proof, it helps to understand how f works.

7.5 Generalized Proportional Integral (GPI) Observer In the previous sections it was taken advantage of Property 3.14 to compensate model parameters inaccuracies. The main disadvantage of the approaches considered before is that the model structure should be well known. This does not allow to deal explicitly with non modeled dynamics. The Generalized Proportional Integral (GPI) observers attempt to compensate non modeled dynamics with p-degrees polynomials and they represent an alternative. To illustrate this technique, consider the computed torque control law seen in Sect. 5.3 rewritten in the following form ' & τ = H(q) q¨ d − 2ζ ωn e˙ − ω2n e + C(q, q˙ )˙q + D˙q + g(q),

(7.100)

where e = q − qd is the tracking error given in (5.1), and ζ and ωn ∈ Rn×n are positive definite diagonal matrices. This leads to the closed loop dynamics e¨ + 2ζ ωn e˙ + ω2n e = 0,

(7.101)

which guarantees that tracking errors will tend to zero. Although theoretically the control problem is exactly solved, cancelling C(q, q˙ )˙q + D˙q + g(q) is not really possible. A basic GPI observer requires only the inertia matrix H(q) at the cost of not achieving exact tracking but ultimate boundedness. To understand this approach, assume that for any desired trajectory qd exists an ideal yet unknown control input τ ∗ achieving exact tracking. For that case it must hold H(qd )¨qd + C(qd , q˙ d )˙qd + D˙qd + g(qd ) = τ ∗ .

(7.102)

Then one can form the closed loop tracking error dynamics as e¨ = q¨ − q¨ d = H −1 (q)τ + z,

(7.103)

188

7 Adaptive and Robust Control

where z ∈ Rn is defined as & ' z = − H −1 (q) C(q, q˙ )˙q + D˙q + g(q) & ' + H −1 (qd ) C(qd , q˙ d )˙qd + D˙qd + g(qd ) − H −1 (qd )τ ∗ .

(7.104)

To recover (7.101) the ideal control law should be ' & τ = H(q) −2ζ ωn e˙ − ω2n e − z .

(7.105)

But (7.105) is not implementable because z is unavailable. The goal of the GPI design is to estimate z and e˙ (not q˙ !) simultaneously so that (7.105) becomes ) ( τ = H(q) −2ζ ωn eˆ˙ − ω2n e − zˆ ,

(7.106)

where (ˆ·) is the estimate of (·). The closed loop dynamics is then given by e¨ + 2ζ ωn e˙ + ω2n e = 2ζ ωn e˜ 2 + z˜ ,

(7.107)

with the estimation errors defined as e˜ 2 = e˙ − eˆ˙ z˜ = z − zˆ .

(7.108) (7.109)

After (7.107), if both e˜ 2 and z˜ can be made arbitrarily small, then e and e˙ can also become arbitrarily small. The main problem is that z in (7.104) actually lumps all unknown or uncertain terms and therefore it does not even own a well defined structure. To find a substitute for the dynamic behavior of z assume that it has a smooth enough behavior so that in any time interval [t1 , t2 ] it can be approximated by a polynomial of the form z=

p−1 %

ai t i + r,

(7.110)

i=0

with each ai being a n-vector of constant coefficients for i = 0, . . . , p − 1. For smooth enough it is meant that at least the first p derivatives of the residual term r exist (i.e. up to r(p) ). This is illustrated in Fig. 7.1. The disadvantages of using (7.110) appear to be evident. First of all the coefficients of the polynomials might change for two consecutive time intervals [t1 , t2 ] and [t2 , t3 ] and this for t ∈ [t0 , ∞). Second, the larger the time interval the bigger the order of the polynomial. But the main disadvantage is that z is unknown so that the polynomials cannot be computed anyway.

7.5 Generalized Proportional Integral (GPI) Observer

z(t) z(t) =

p−1 

189

ai ti + r()

i=0

t

t2

t1

Fig. 7.1 Approximation of z by a polynomial on the time interval [t1 , t2 ].

Now, consider the fact that the constant coefficients in (7.110) allow to get the following equivalent description to describe z which is valid for t ∈ [t0 , ∞): z˙ = z˙ 1 = z2 .. .

(7.111)

z˙ p−1 = zp = z˙ (p−1) z˙ p = r

(p)

(p)

= z˙ .

(7.112) (7.113)

Recall that it has been assumed that r(p) exists. On the other hand, the polynomial description in (7.110) is observable from the measurable position coordinate. Indeed, from (7.103) it is z = e¨ − H −1 (q)τ = q¨ − q¨ d (t) − H −1 (q)τ ,

(7.114)

meaning that z can be written in terms of the output q, a finite number of its time derivatives, the known quantity qd (t), and the control input, τ . Hence, z is observable. Therefore, a linear Luenberger observer can be designed to estimate z, for which it is necessary to rewrite model (7.103) as e˙ = e2

(7.115) −1

e˙ 2 = H (q)τ + z1 ,

(7.116)

190

7 Adaptive and Robust Control

where it has been set z1 = z and e2 = e˙ . Based on (7.111)–(7.113) and (7.115)– (7.116) and by defining eˆ˙ = eˆ 2 zˆ = zˆ 1 e˜ = e − eˆ ,

(7.117) (7.118) (7.119)

the following linear observer can be introduced e˙ˆ = eˆ 2 + λp+1 e˜

(7.120)

e˙ˆ 2 = H −1 (q)τ + zˆ 1 + λp e˜ z˙ˆ 1 = zˆ 2 + λp−1 e˜ .. .

(7.121) (7.122)

z˙ˆ p−1 = zˆ p + λ1 e˜ zˆ˙ p = λ0 e˜ ,

(7.123) (7.124)

where λ0 , . . . , λp+1 ∈ Rn×n are diagonal positive definite matrices. System (7.111)– (7.113) and (7.115)–(7.116) in closed loop with the GPI control–observer scheme given by (7.106) and (7.120)–(7.124) delivers the error dynamics e˙˜ = e˜ 2 − λp+1 e˜

(7.125)

e˙˜ 2 = z˜ 1 − λp e˜ z˙˜ 1 = z˜ 2 − λp−1 e˜ .. .

(7.126) (7.127)

z˙˜ p−1 = z˜ p − λ1 e˜ z˙˜ p = r(p) − λ0 e˜ .

(7.128) (7.129)

By combining (7.125)–(7.126) one gets e¨˜ + λp+1 e˙˜ + λp e˜ = z˜ 1 ,

(7.130)

z˜ 1 = r(p) − λ0 e˜ − · · · − λp−1 e˜ (p−1) .

(7.131)

and from (7.127)–(7.129) it is (p)

By recalling that z1 = z, one gets after (7.130) and (7.131) e˜ (p+2) + λp+1 e˜ (p+1) + · · · + λ0 e˜ = r(p) =

dp z. dt p

(7.132)

7.5 Generalized Proportional Integral (GPI) Observer

191

Rewrite (7.132) as x˙ = Ax + Br(p) ,

(7.133)

where ⎡ ⎢ x=⎣

e˜ .. .

⎤ ⎥ ⎦

(7.134)

(p+1)

e˜

is the error state and A ∈ Rn(p+1)×n(p+1) and B ∈ Rn(p+1)×n are constant matrices given by ⎡

⎤ O I ··· O ⎢ .. .. ⎥ ⎢ . ⎥ A=⎢ . ⎥ ⎣ O O ··· I ⎦ −λ0 −λ1 · · · −λp+1 & 'T B = O ··· O I .

(7.135)

(7.136)

Then, choose {λ0 , . . . , λp+1 } so that polynomials of degree p + 2 ρ(s) = sp+2 I + λp+1 sp+1 + · · · + λ1 s + λ0

(7.137)

be Hurwitz. Note that the scalar s to denote the Laplace variable should not be confused with the vector sliding variable s. This renders system (7.133) stable. Define the following region of the form (4.28), i.e.   D = x ∈ Rn(p+1) \ x <  ,

(7.138)

where  is a positive constant that can be arbitrarily large. By using a rather involved procedure it can be shown that whenever x ∈ D, then any signal of interest is bounded and the state x is ultimately bounded with a final bound that can be made arbitrarily small by choosing the roots of (7.137) as far on left hand side of the s plane as possible. The proof is skipped because it does not rely directly on Theorem 4.5.

7.6 GPI Observer Without Inertia Matrix It is interesting to note that the observer (7.120)–(7.124) needs only the inertia matrix to be known for implementation, but since in the end there is no reason at all to assume it well known while the rest of the model is not, a possibility to omit it as well is the following. When the robot model and velocity measurements are available, it has been shown that the control law (5.51), i.e.

192

7 Adaptive and Robust Control

τ = H(q)¨qr + C(q, q˙ )˙qr + D˙qr + g(q) − K d s, is a very good option and where (5.49) and (5.50), i.e. q˙ r = q˙ d − e s = q˙ − q˙ r = e˙ + e, have been used with K d ,  ∈ Rn×n diagonal positive definite matrices. In closed loop one gets H(q)˙s + C(q, q˙ )s + K D s = 0,

(7.139)

where K D = D + K d . Instead of (7.103) one gets e¨ = −˙e + H −1 (q)(τ − C(q, q˙ )s) + z,

(7.140)

z = ˙e − q¨ r − H −1 (q){C(q, q˙ )˙qr + D˙q + g(q)}.

(7.141)

with

However, the control law can no longer be given by (7.106). Instead, it can be used τ = −K d (e˙ˆ + e) − H(q)ˆz = −K d s + K d e˙˜ − H(q)ˆz,

(7.142)

where eˆ˙ has been replaced by e˙ˆ . Substituting (7.142) in (7.140) leads to H(q)˙s + C(q, q˙ )s + K d s = K d e˙˜ + H(q)˜z,

(7.143)

so that if both e˙˜ and z˜ can be made arbitrarily small, then it can easily be shown that s (and as a direct consequence e and e˙ ) can be made arbitrarily small as well. However, the control law (7.142) still relies on the exact knowledge of the inertia matrix H(q). Suppose that it is unknown and that it is approximated by the simple substitution H(q) ≡ I.

(7.144)

Then, the control law (7.142) becomes τ = −K d (e˙ˆ + e) − zˆ = −K d s + K d e˙˜ − zˆ .

(7.145)

In closed loop with (7.140) one gets H(q)˙s + C(q, q˙ )s + K d s = K d e˙˜ + H(q)z − zˆ .

(7.146)

7.6 GPI Observer Without Inertia Matrix

193

This last equation makes apparent that zˆ tries to compensate H(q)z rather than z alone. For this reason it is more convenient to redefine z in (7.141) as   z = H(q) ˙e − q¨ r − H −1 (q){C(q, q˙ )˙qr + D˙q + g(q)}   = H(q) ˙e − q¨ r − C(q, q˙ )˙qr − D˙q − g(q).

(7.147)

This makes the corresponding error dynamics be e˙ = e2

(7.148) −1

−1

e˙ 2 = H (q)(τ − C(q, q˙ )s) − ˙e + H (q)z1 z˙ 1 = z2 .. . z˙ p−1 = zp

(7.149) (7.150)

(7.151)

(p)

z˙ p = r ,

(7.152)

where z1 = z has been used as before and it is assumed that r(p) exists. The observer is now given by e˙ˆ = eˆ 2 − ˆe + (λp+1 − )˜e e˙ˆ 2 = λp e˜ z˙ˆ 1 = zˆ 2 + λp−1 e˜ .. .

(7.153) (7.154) (7.155)

z˙ˆ p−1 = zˆ p + λ1 e˜ zˆ˙ p = λ0 e˜ ,

(7.156) (7.157)

where τ is not employed in (7.154) because it cannot be canceled exactly and thus its introduction does not represent any advantage. Also, instead of (7.146) one has H(q)˙s + C(q, q˙ )s + K d s = K d e˙˜ + z˜ .

(7.158)

On the other hand, from (7.150)–(7.152) and (7.155)–(7.157) one can compute z˜ (p) = r(p) − λ0 e˜ − · · · − λp−1 e˜ (p−1) ,

(7.159)

while from (7.148)–(7.149) and (7.153)–(7.154) one gets e˙˜ = e˜ 2 + ˆe − (λp+1 − )˜e

(7.160)

e˙˜ 2 = H (q)(τ − C(q, q˙ )s) − ˙e + H (q)z1 − λp e˜ , −1

−1

(7.161)

194

7 Adaptive and Robust Control

or e¨˜ − e˙ˆ + (λp+1 − )e˙˜ =H −1 (q)(τ − C(q, q˙ )s) − ˙e + H −1 (q)z1 − λp e˜ .

(7.162)

This means that e¨˜ + λp+1 e˙˜ + λp e˜ = H −1 (q)(τ − C(q, q˙ )s) + H −1 (q)z1 .

(7.163)

By substituting τ from (7.145) one has e¨˜ + λp+1 e˙˜ + λp e˜ = − H −1 (q)(K d s + C(q, q˙ )s) + H −1 (q)˜z + H −1 (q)K d e˜˙ ,

(7.164)

which can be rewritten as ¯ z + z˜ , e¨˜ + λp+1 e˙˜ + λp e˜ = H −1 (q)(−K d s + K d e˙˜ − C(q, q˙ )s) + H(q)˜

(7.165)

¯ = H −1 (q) − I. By defining with H ¯ z, f = H −1 (q)(−K d s + K d e˙˜ − C(q, q˙ )s) + H(q)˜

(7.166)

and by computing the pth derivative of (7.165) and substituting in (7.159) one gets e˜ (p+2) + λp+1 e˜ (p+1) + · · · + λ0 e˜ = r(p) + f (p) (t).

(7.167)

This means that instead of using (7.133) one has x˙ = Ax + B¯rf ,

(7.168)

with r¯f = r(p) + f (p) . As before, if a region of interest of the form (4.28) is defined, then it can be shown that a combination of roots for the polynomials in (7.137) can always be chosen to make the sliding variables (s, s˙) and the tracking errors (e, e˙ , e¨ ) in (7.158) and the observation errors x in (7.134) arbitrarily small. Furthermore, the estimate zˆ 1 becomes arbitrarily close to the real value z. Once again, the proof is omitted because it is very involved and not based on Theorem 4.5.

7.7 Experimental Results In this section, an experimental evaluation of the control schemes described in the sections above is presented. For such evaluation, the Phantom Touch robot described in Chap. 14 is employed. The 3 degrees-of-freedom configuration characterized in Sect. 14.2 is utilized, i.e. the last three non-actuated joints are mechanically blocked.

7.7 Experimental Results

195

The desired trajectory in joint coordinates for all the experiments below is given by

⎤ −30 ◦ + 30 ◦ (1 − cos(π t)) qd = ⎣ 20 ◦ + 30 ◦ (1 − cos(π t)) ⎦ , −60 ◦ − 30 ◦ (1 − cos(π t)) ⎡

(7.169)

whereas the desired velocities and accelerations, q˙ d and q¨ d , are obtained by taking the time derivative of the desired joint position. Slotine–Li Adaptive Controller Tuning Guide The necessary equations to implement the Slotine–Li adaptive controller are (5.1), (5.49), (5.50), (7.4), and (7.13). To tune the controller consider the following suggestions. ˆ 0 ), it can be the zero vector if no • Set an initial guess for the parameters vector, ϕ(t knowledge of the parameters is available. ˆ by setting  = O, and tune the remaining • At first, neglect the adaptive term, Y a ϕ, one as a standard PD controller by setting K d equal to the derivative gain matrix, and set  = K −1 d K p , where K p is the proportional gain matrix. • Now, set  as a diagonal matrix, for which every element on the diagonal corresponds to the adaptation gain of a particular parameter. Start by setting very small adaptation gains and gradually increase them until the parameters tend to a constant value within a desired settling time. Experiment The gains chosen for the implementation of the Slotine–Li algorithm by following the tuning guide are displayed in Table 7.1. The position tracking in joint coordinates is shown in Fig. 7.2, and the corresponding tracking error in Fig. 7.3. The joint velocities estimated by means of a derivative filter are shown along with the desired joint velocities in Fig. 7.4. The time evolution of the estimated parameters is shown in Fig. 7.5. The initial guess for the parameters vector was set to & 'T ϕˆ = 0 0 0 0.02 0.02 0.02 0 0 . The first three parameters correspond to the inertia matrix and Coriolis vector, the next three correspond to viscous friction and the last two to the gravity vector. Table 7.1 Chosen gains for the Slotine–Li controller

Gain

Value

Kd  

diag{0.01, 0.01, 0.01} diag{50, 50, 25} 1 × 10−5 diag{1, 1, 1, 1, 1, 1, 10, 10}

196

7 Adaptive and Robust Control

Fig. 7.2 Slotine–Li controller, joint position tracking: desired (· · · ), real (—)

Adaptive Scheme with Velocity Observers Tuning Guide The equations needed to implement the adaptive scheme of Sect. 7.2 are the tracking variables (5.1), (5.49), (5.50), the observer Eqs. (6.1), (6.39)–(6.40), and (6.42)– (6.44), the filter (7.40), the prediction error (7.44), the adaptation law (7.47), and the control law (7.4). Note that the analytic filtered version of the regressor, obtained through (7.42), and computed as in the example at the end of Sect. 7.2, is also needed. ˆ 0 ), it can be the zero vector if no • Set an initial guess for the parameters vector, ϕ(t knowledge of the parameters is available. • As in the case of the Slotine–Li controller, first neglect the adaptive term by setting  = O, and tune the remaining one as a standard PD controller by setting K d equal to the derivative gain matrix, and  = K −1 d K p , where K p is the proportional gain matrix. • Set the observer gains kd and z satisfying (6.65)–(6.67) and (6.69)–(6.70), to minimize the observation error z. • Now, set β = 0, and proceed to tune the adaptation gains matrix  as in the case of the Slotine–Li algorithm. • Finally, increase β gradually to improve the performance of the position tracking.

7.7 Experimental Results

197

Fig. 7.3 Slotine–Li controller, joint position tracking error Table 7.2 Chosen gains for the adaptive scheme of Sect. 7.2. Gain Value Kd   kd z β

diag{0.01, 0.01, 0.01} diag{50, 50, 25} 1 × 10−5 diag{1, 1, 1, 1, 1, 1, 10, 10} 200 diag{1, 1, 1} 10

Experiment The gains chosen for the implementation of the scheme of Sect. 7.2 by following the tuning guide above are displayed in Table 7.2. The position tracking is displayed in Fig. 7.6, whereas the corresponding tracking error is shown in Fig. 7.7. The joint velocities estimated by the proposed observer are shown along with the desired joint velocities in Fig. 7.8. The time evolution of the estimated parameters is shown in Fig. 7.9. The initial guess for the parameters vector was set to & 'T ϕˆ = 0 0 0 0.02 0.02 0.02 0 0 as in the Slotine–Li scheme.

198

7 Adaptive and Robust Control

Fig. 7.4 Slotine–Li controller, joint velocities: desired (· · · ), numerical derivative (—)

Robust Controller Tuning Guide The equations required to implement the robust controller of Sect. 7.3 are the tracking variables (5.1), (5.49), (5.50), the control law (7.66), and the function defined in (7.68). For this scheme, a nominal parameters vector ϕ 0 is also required. The following directions might be useful when implementing this controller. • At first, set ρ = 0 in (7.68), and tune the remaining one as a standard PD plus Feedforward controller. • Set ρ to satisfy (7.64). • Finally, set η > 0, perhaps after looking at the time-plot of Y Ta s to obtain the desired performance.

7.7 Experimental Results

199

Fig. 7.5 Slotine–Li controller, estimated parameters: θˆ1 (—), θˆ2 (—), θˆ3 (—), θˆ4 (—), θˆ5 (– –), θˆ6 (– –), θˆ7 (– –), θˆ8 (– –) Table 7.3 Chosen gains for the robust controller of Sect. 7.3 Gain Value Kd  η ρ

diag{0.01, 0.01, 0.01} diag{50, 50, 25} 40 0.0005

Experiment By following the above tuning guide, the gains chosen for the implementation of the robust controller described in Sect. 7.3 are displayed in Table 7.3. The nominal value for the parameters vector was set to & 'T ϕ 0 = 0.0045 0.0003 0.0045 0.0014 0.0012 0.0006 0.0049 0.013 , after the estimation reported in Sect. 14.2.5. The position tracking is displayed in Fig. 7.10, whereas the corresponding tracking error is shown in Fig. 7.11. The joint velocities obtained by a derivative filter are shown along with the desired joint velocities in Fig. 7.12.

200

7 Adaptive and Robust Control

Fig. 7.6 Adaptive scheme with velocity observers, joint position tracking: desired (· · · ), real (—)

Control-Observer Robust Scheme Tuning Guide The equations required to implement the control-observer robust scheme of Sect. 7.4 are the tracking variables (5.1), (7.72), the observer equations (6.42)–(6.44), and (6.1), (7.75)–(7.76), the control law (7.77), and the function defined in (7.78). The tuning procedure can be carried out as follows. • As in the previous controller, first neglect the robustification term given by the function f defined in (7.78), and tune the remaining ones as a standard PD plus Feedforward controller (Kelly et al., 2005). • Set the observer gains kd and z satisfying (6.65)–(6.67) and (6.69)–(6.70), to minimize the observation error z. • Set η very small, just to avoid the indeterminacy of f in (7.78). • Set each ϕmi to satisfy (7.74).

7.7 Experimental Results

201

Fig. 7.7 Adaptive scheme with velocity observers, joint position tracking error

Experiment The gains chosen for the implementation of the scheme of Sect. 7.4 by following the tuning guide above are displayed in Table 7.4. The nominal value for the parameters vector was set to & 'T ϕ 0 = 0.0045 0.0003 0.0045 0.0014 0.0012 0.0006 0.0049 0.013 , after the estimation reported in Sect. 14.2.5. The position tracking is displayed in Fig. 7.13, and the corresponding tracking error is shown in Fig. 7.14. The joint velocities estimated by the proposed observer are shown along with the desired joint velocities in Fig. 7.15. GPI Observer Tuning Guide To implement the GPI observer controller of Sect. 7.5, the required equations are the tracking error (5.1), the observer Eqs. (7.117)–(7.124), and the control law (7.106). The tuning procedure can be carried out as follows.

202

7 Adaptive and Robust Control

Fig. 7.8 Adaptive scheme with velocity observers, joint velocities: desired (· · · ), observer (—) Table 7.4 Chosen gains for the control-observer scheme of Sect. 7.4 Gain Value Kd  kd z η ϕmi

diag{0.01, 0.01, 0.01} diag{50, 50, 25} 200 diag{1, 1, 1} 1 × 10−6 5 × 10−5 , i = 1, 2, . . . , 8

• Select the degree of the polynomial p in (7.110) to define the order of the dynamic extension that represents the internal model for the unknown dynamics. As a rule, the higher p the accurate the signals the controller can reject, at the cost of increasing undesired effects like the well known peaking phenomena arising when employing high-gain observers. For the common tasks of robotic manipulation, usually p = 2 is a good choice. • Set the pole observers of the diagonal matrix polynomial (7.138) as far in the left of the complex plane as the bandwidth allows it, and compute the observer gains

7.7 Experimental Results

203

Fig. 7.9 Adaptive scheme with velocity observers, estimated parameters: θˆ1 (—), θˆ2 (—), θˆ3 (—), θˆ4 (—), θˆ5 (– –), θˆ6 (– –), θˆ7 (– –), θˆ8 (– –) Table 7.5 Chosen gains for the GPI observer scheme of Sect. 7.5 Gain Value p λ0 λ1 λ2 λ3 ζ ωn 

2 8.1 × 105 I 10.8 × 104 I 5.4 × 103 I 120I I 40I 0.4 s

λ0 , . . . , λp+1 . As a rule, the higher the sampling frequency, the larger can be these poles in magnitude. • Choose the constant matrices ζ and ωn according to a desired second-order system response.

204

7 Adaptive and Robust Control

Fig. 7.10 Robust controller, joint position tracking: desired (· · · ), real (—)

Remark 7.1 To reduce the undesired peaking phenomena, it is convenient to implement a suitable clutch, which can be of the form:  sin8 ( πt ) if t ≤  2 fclutch = 1 if t >  where  is the time duration of the peaking. Finally, multiply eˆ 2 and zˆ i , i = 1, . . . , p,

by fclutch . Experiment The gains chosen for the implementation of the scheme of Sect. 7.5 by following the tuning guide above are displayed in Table 7.5. The position tracking is displayed in Fig. 7.16, whereas the corresponding tracking error is shown in Fig. 7.17. The joint velocities estimated by the proposed observer are shown along with the desired joint velocities in Fig. 7.18.

7.7 Experimental Results

205

Fig. 7.11 Robust controller, joint position tracking error

GPI Observer Without Inertia Matrix Tuning Guide The necessary equations to implement the GPI observer without inertia matrix scheme of Sect. 7.6, are the tracking error (5.1), the observer equations (7.117)– (7.119) and (7.153)–(7.157), and the control law (7.145). The following directives might be useful when tuning the controller. • Select the degree of the polynomial p in (7.110) to define the order of the dynamic extension that represents the internal model for the unknown dynamics. As a rule, the higher p the more accurately the signals the controller can reject, at the cost of increasing undesired effects like the well known peaking phenomena arising when employing high-gain observers. For the common tasks of robotic manipulation, usually p = 2 is a good choice. • Set the pole observers of the diagonal matrix polynomial (7.138) as far in the left of the complex plane as the bandwidth allows it, and compute the observer gains λ0 , . . . , λp+1 . As a rule, the higher the sampling frequency, the larger can be these poles in magnitude.

206

7 Adaptive and Robust Control

Fig. 7.12 Robust controller, joint velocities: desired (· · · ), numerical derivative (—)

• At first, neglect the zˆ term in the control law (7.145), and tune the remaining one as a standard PD controller by setting K d equal to the derivative gain matrix, and  = K −1 d K p , where K p is the proportional gain matrix. Remark 7.2 To improve the performance of the controller, another constant matrix different from the identity in (7.144) can be used, which better approximates H(q), e.g. H(q0 ), where q0 is any point on the desired joint trajectory.

Experiment The gains chosen for the implementation of the scheme of Sect. 7.6 by following the tuning guide above are displayed in Table 7.6. To improve the performance, the constant matrix ⎡ ⎤ 0.0049 0 0 H 0 = H(0) = ⎣ 0 0.0097 0.0048⎦ , 0 0.0048 0.0045 obtained with the parameters estimated in Sect. 14.2.5, was employed instead the identity in (7.144). The position tracking is displayed in Fig. 7.19, whereas the corresponding tracking error is shown in Fig. 7.20. The joint velocities estimated by the proposed observer are shown along with the desired joint velocities in Fig. 7.21.

7.7 Experimental Results

207

Fig. 7.13 Control-observer robust scheme, joint position tracking: desired (· · · ), real (—) Table 7.6 Chosen gains for the GPI observer without inertia matrix scheme of Sect. 7.6 Gain Value p λ0 λ1 λ2 λ3 Kd 

2 6.25 × 106 I 5 × 105 I 1.5 × 104 I 200I diag{0.05, 0.05, 0.05} diag{20, 40, 20}

7.7.1 Performance Comparison Since the desired trajectories are the same for all the above experiments, it is worth to compare the performance of the controllers. Notice however, that two of the controllers require velocity measurements, which are actually obtained through positions by means of filter derivatives. For this reason, only the performance of the position tracking error is taken into account, by employing the Root-Square-Mean

208

7 Adaptive and Robust Control

Fig. 7.14 Control-observer robust scheme, joint position tracking error Table 7.7 Comparison of the controllers’ position tracking performance Scheme Joint 1 RMSE Joint 2 RMSE Joint 3 RMSE Sum Slotine-Li Adaptive + Observers Robust Control-Observer robust GPI Observer GPIO without inertia matrix

0.7352 0.9993 0.5601 0.7242 0.5164 0.3081

1.1019 1.2612 0.4679 0.6082 0.8808 0.4652

1.4429 1.5033 0.3938 0.5117 1.0819 0.3750

3.28 3.7638 1.4218 1.8441 2.4791 1.1483

Error (RMSE) index. The results of this comparison for each joint, along with the sum of all joints RMSE, are displayed in Table 7.7. It is worth to point out that the RMSE of the GPI observer described in Sect. 7.5 is greatly affected by the peaking at the start of the trajectories. In fact, if the first 1 s of the experiment is not taken into account to compute the RMSE, the GPI observer gets second best performance, just below the GPIO without inertia matrix of Sect. 7.6.

7.7 Experimental Results

Fig. 7.15 Control-observer robust scheme, joint velocities: desired (· · · ), observer (—)

Fig. 7.16 GPI Observer, joint position tracking: desired (· · · ), real (—)

209

210

7 Adaptive and Robust Control

Fig. 7.17 GPI Observer, joint position tracking error

Fig. 7.18 GPI Observer, joint velocities: desired (· · · ), observer (—)

7.7 Experimental Results

211

Fig. 7.19 GPI Observer without inertia matrix, joint position tracking: desired (· · · ), real (—)

Fig. 7.20 GPI Observer without inertia matrix, joint position tracking error

212

7 Adaptive and Robust Control

Fig. 7.21 GPI Observer without inertia matrix, joint velocities: desired (· · · ), observer (—)

References Anderson BDO (1977) Exponential stability of linear equations arising in adaptive identification. IEEE Trans Autom Control 22(1):83–88 Anderson BDO, Moore JB (1969) New results in linear system stability. SIAM J Control 7(3):398– 414 Arteaga-Pérez MA, Gutiérrez-Giles A (2014) On the GPI approach with unknown inertia matrix in robot manipulators. Int J Control 87(4):844–860 Arteaga-Pérez MA, Kelly R (2004) Robot control without velocity measurements: new theory and experimental results. IEEE Trans Robotics Autom 20(2):297–308 Arteaga-Pérez MA, Ortiz-Espinoza A, Romero JG, Espinoza-Pérez G (2020) On the adaptive control of robot manipulators with velocity observers. Int J Robust Nonlinear Control 30:4371–4396 Diop S, Fliess M (1991) Nonlinear observability, identifiability and persistent trajectories. In: Proceedings of the 36th IEEE conference on decision and control, Brighton, UK, Dec 1991 Fliess M, Marquez R, Delaleau E, Sira-Ramírez H (2002) Correcteurs proportionnels intègraux généralisés. ESAIM: Control Optim Calc Var 7(2):23–41 Ioannou PA, Sun J (1996) Robust adaptive control. Prentice-Hall, USA Kalman RE (1960) Contributions to the theory of optimal control. Boletín de la Sociedad Matemática Mexicana 5:102–119 Kelly R, Santibáñez V, Loria A (2005) Control of robot manipulators in joint space. Springer, London Khalil HK (2002) Nonlinear systems, 3rd edn. Prentice-Hall, Upper Saddle River, NJ Morgan AP, Narendra KS (1977) On the uniform asymptotic stability of certain linear nonautonomous differential equations. SIAM J Control Optim 15(1):5–24

References

213

Slotine JJE, Li W (1987) On the adaptive control of robot manipulators. Int J Robotics Res 6(3):49– 59 Slotine JJE, Li W (1989) Composite adaptive control of robot manipulators. Automatica 25(4):509– 519 Slotine JJE, Li W (1991) Applied nonlinear control. Prentice-Hall, Englewood Cliffs, NJ Spong MW (1992) On the robust control of robot manipulators. IEEE Trans Autom Control 37(11):1782–1786 Tang Y, Arteaga-Pérez MA (1994) Adaptive control of robot manipulators based on passivity. IEEE Trans Autom Control 39(9):1871–1875

Chapter 8

Force Control

Abstract When a robot gets in contact with its environment, represented by a rigid or flexible surface for example, then the manipulators’ movements become constrained. For that case it is advisable to implement directly or indirectly a force control strategy to avoid damages both on the robot and to the environment. If the contact surface is well known then it is also possible to change the control objective not only to get position tracking but also force tracking or regulation. Some of the new arising challenges are the possible lack of both velocities and force measurements, a poor model of the environment or high friction effects. This chapter is meant to deal with the force tracking control when the contact surface is assumed to be rigid and frictionless so that the force and velocity components of the end-effector can be split into two tangent planes.

8.1 Robot Force Control Without Dynamic Model Assume that the end-effector of a robot manipulator gets in touch with its environment represented in work space coordinates as in (3.84), i.e. ϕ(x) = 0, where the vector x ∈ Rn can be defined as in (2.172), i.e. x=

0

 pn , 0 φn

where 0 φ n ∈ Rν is a parametrization of the orientation and 0 pn ∈ R(n−ν) is the endeffector position,1 or it can be defined according to (2.183), i.e.

1

The notation 0 pn is preferred for this chapter instead of 0 d n for being more congruent with the environment description. © The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control of Robot Manipulators, Lecture Notes in Electrical Engineering 798, https://doi.org/10.1007/978-3-030-85980-0_8

215

216

8 Force Control

x˙ =

0

p˙ n 0 ωn

 = J(q)˙q,

where J(q) ∈ R6×n is the geometric Jacobian of the manipulator and 0 ωn ∈ Rν is the angular velocity of the end-effector. For simplicity’s sake assume that n = 6 and ν = 3 and that end-effector rotations are not constrained, so that any of the two definitions of x can be employed to express (3.84). In fact, it can be rewritten as ϕ(0 pn ) = ϕ(x) = 0,

(8.1)

to make explicit that ϕ(x) = 0 depends indeed only on the upper part of x. This turns out to be relevant if (2.183) is preferred because for that case x is given by

x=

0 0

pn φn



⎡

0

= ⎣ t

0

pn

ωn dϑ

⎤ ⎦.

(8.2)

t0

As explained in Sect. 2.4.2, 0 φ n in (8.2) does not have any physical meaning. Still, there is no analytical reason at all not to employ it as long as only force and not torque control of the end-effector is pursued, i.e. only unconstrained orientation is considered. On the other hand, since 0 φ n does not have any physical meaning it cannot be measured, but for implementation online it can be approximated by employing 0

φ n (kT ) = 0 φ n ((k − 1)T ) + J ω (q){q(kT ) − q((k − 1)T )}

(8.3)

where J ω (q) ∈ R3×n is the lower part of the geometric Jacobian J(q) and T is the sampling time. One can further set2 ⎤ x ⎢ y ⎥ ⎢ ⎥ ⎢ z ⎥ ⎥ x=⎢ ⎢ φx ⎥ . ⎢ ⎥ ⎣ φy ⎦ φz ⎡

(8.4)

Consider now the robot model of a manipulator in contact with its environment given by (3.87), i.e. H(q)¨q + C(q, q˙ )˙q + D˙q + g(q) = τ − J T (q)J Tϕx λ,

2

Using x as vector and x as scalar (to designate the x-coordinate) should not cause any confusion.

8.1 Robot Force Control Without Dynamic Model

217

where (3.85) is used, i.e. J ϕx =

∂ϕ(x) . ∂x

The goal of this section is to avoid using inverse kinematics by proposing a desired position trajectory xd ∈ Rn and then defining a tracking error x = x − xd

(8.5)

directly in work space coordinates. It is assumed that xd and its first two derivatives are bounded. Suppose that velocity measurements are not available. Then, an estimate of x is given by xˆ , and the observation error is defined in the same fashion as for joint coordinates in (6.1), i.e. z = x − xˆ .

(8.6)

Also, as done in (6.73), define the auxiliary error variable eo = xˆ − xd = x − z.

(8.7)

By considering a normalization of J Tϕx the Lagrange multipliers represent physically the force applied to the surface, so that the force error can be defined as λ = λ − λd ,

(8.8)

where λd ∈ Rm is the desired bounded force, with at least its first derivative bounded. Consider Property 3.16 to define the sliding variable s = Qx (˙eo + x eo ) + J + ϕx ξ 2 F = sp + sf ,

(8.9)

where x ∈ Rn×n and ξ 2 ∈ Rm×m are diagonal positive definite matrices and t λdϑ.

F =

(8.10)

t0

As made before, the design approach is based on making s tend to zero. Note that sp + and sf are orthogonal vectors, and that P x J + ϕx = J ϕx . Thus, they both become zero if T s → 0. But recall that J ϕx is assumed to be full rank and so must be J + ϕx , so that F = 0

(8.11)

218

8 Force Control

Fig. 8.1 Planes tangent to the surface at actual and desired positions. a Large tracking error; b small tracking error

holds because ξ 2 is a positive definite matrix. However, the fact that sp becomes zero does not necessarily implies that e˙ o and x eo will do it as well. To achieve this goal, once again it will be taken advantage of a local stability analysis. For that instance, rewrite first the tracking error as x = x − xd = Qx (x)x + P x (x)x − Qx (xd )xd − P x (xd )xd .

(8.12)

In Fig. 8.1 it can be appreciated that the smaller the tracking error, the more it tends to belong to the plane tangent to the contact surface, which can be expressed as x ≈ Qx (x)(x − xd ) = Qx (x)x.

(8.13)

How small the error must be for (8.13) to hold depends on how smooth the surface is. After Property 3.16, if (8.13) is valid then one also has

8.1 Robot Force Control Without Dynamic Model

˙x ≈ Qx (x)(˙x − x˙ d ) = Qx (x)˙x.

219

(8.14)

Assumption 8.1 The approximations (8.13) and (8.14) are valid if x ≤ ψ holds for some ψ > 0.

(8.15) 

The previous assumption may be appear to be restrictive and somehow it is indeed so. Basically, it requires a zero initial error and then an accurate tracking to keep (8.15) holding. However, the main disadvantage is that the usual region of interest defined to carry out a local stability analysis cannot be made arbitrarily large. A case of particular interest is that of a rigid plane as contact surface. In fact, any vector x can be split in the two orthogonal spaces described by Qx and P x given in Property 3.16, i.e. x = Qx x + P x x,

(8.16)

where in general P x x = 0. This case is depicted in Fig. 8.2, where it can be appreciated that the vector x lying in the space spanned by P x must be constant. This is easy to understand because P x x represents the minimal distance to the surface. Note also that for the plane J ϕx in (3.85) is constant and therefore so are Qx and P x . This in turn from (8.16) implies that x˙ = Qx x˙ ,

(8.17)

which fulfills (3.120) as well. As to xd , it must comply with (8.16), so that it is chosen xd = Qx xd + P x xd ≡ Qx xd + P x x x˙ d = Qx x˙ d .

Fig. 8.2 Rigid plane as contact surface

(8.18) (8.19)

220

8 Force Control

Assume that x˙ is not available and thus it has to be estimated. But the estimated xˆ has to comply with (8.16) as well, otherwise said, the estimated variable must have form similar to (8.18)–(8.19), i.e. xˆ = Qx xˆ + P x x xˆ˙ = Q x˙ˆ ,

(8.20) (8.21)

x

where P x x is constant and known. To make sure that this goal is achieved, it can be used t xˆ = Qx

x˙ˆ dϑ + P x x

(8.22)

t0

⎛ x˙ˆ = Qx ⎝x˙ d − x eo + kd z

t

⎞ z(ϑ)dϑ + z z + kd z⎠

(8.23)

t0

to enforce x˙ˆ to lie in the space spanned by Qx , while z ∈ Rn×n is a diagonal positive definite matrix and kd is a positive constant. Obviously, one has P x x˙ˆ = 0 and since P x x = P x xˆ = P x xd , it holds z = Qx z eo = Qx eo .

(8.24)

In other words P x z = P x eo = 0. Since Qx x˙ d = x˙ d , then z˙ = Qx z˙ e˙ o = Qx e˙ o .

(8.25)

To take advantage of the previous relationships assume that Qx x = x Qx (for example by setting x = kx I, with kx > 0). Then sp in (8.9) can be rewritten as sp = Qx (˙eo + x eo ) = e˙ o + x eo ,

(8.26)

from which it can be concluded that both eo and e˙ o tend to zero if sp tends to zero, and for that it is enough that s does. Should that be the case, this can be used to show that x, ˙x, z and z˙ do tend to zero. The design approach is based on the controller described in Sect. 6.3, for which consider x˙ r = Qx (˙xd − x eo ) − J + ϕx ξ 2 F − K γ σ ,

(8.27)

8.1 Robot Force Control Without Dynamic Model

221

where K γ ∈ Rn×n is a diagonal positive definite matrix and σ ∈ Rn , with t σ =



 K β s(ϑ) + sign(s(ϑ)) dϑ

(8.28)

t0

where σ (0) = 0 and K β ∈ Rn×n is a diagonal positive definite matrix, and sign(s) = [sign(s11 ) · · · sign(s1n )]T ,

(8.29)

with s1i element of s, i = 1, . . . , n. Equivalently it can be written d σ = K β s + sign(s). dt

(8.30)

sx = x˙ − x˙ r = Qx (˙x + x eo ) + J + ϕx ξ 2 F + K γ σ .

(8.31)

Then, consider

The previous definitions allow to design the control-observer scheme directly in work space coordinates, but instead of rewriting model (3.87) in those coordinates, it is more convenient to define sr = q˙ − q˙ r = J −1 (q) (˙x − x˙ r ) = J −1 (q)sx ,

(8.32)

and to rewrite (3.87) in terms of sr as follows H(q)˙sr + C(q, q˙ )sr + Dsr = τ − J T (q)J Tϕx λ − ya ,

(8.33)

ya = H(q)¨qr + C(q, q˙ )˙qr + D˙qr + g(q).

(8.34)

where

Prior to continue, it is made the following: Assumption 8.2 The robot manipulator does not reach any singularity, so that J −1 (q) always exits and it holds q˙ = J −1 (q)˙x.

(8.35) 

This assumption is not only necessary for (8.32) to be implementable, but it is actually mandatory to guarantee that the end-effector can apply forces in the desired directions. Now one can define the auxiliary variables

222

8 Force Control

x˙ o = x˙ˆ − z z r = x˙ − x˙ o = z˙ + z z so = x˙ o − x˙ r .

(8.36) (8.37) (8.38)

Based on all previous definitions, the proposed control law is τ = −K p J −1 (q)so + J T (q)J Tϕx λd − J T (q)J Tϕx ξ 1 F,

(8.39)

with K p ∈ Rn×n and ξ 1 ∈ Rm×m positive definite diagonal matrices. By substituting (8.39) in (8.33) and by taking into account that so = sx − r, one gets H(q)˙sr + C(q, q˙ )sr + K DP sr = −J T J Tϕx λ + K p J −1 r − J T J Tϕx ξ 1 F − ya , (8.40) where K DP = D + K p . Equation (8.40) is related to the boundedness of the tracking and force errors, but to describe the dynamics of the observation error, first of all assume that z Qx = Qx z (once again, this can be done by choosing z = kz I, with kz > 0). Then rewrite (8.23) as x˙ˆ − z Qx z − kd Qx z = Qx (˙xd − x eo ) + kd z Qx

t z(ϑ)dϑ.

(8.41)

0

From (8.24) and (8.27), it is x˙ − x˙ r = x˙ − x˙ˆ + z z + kd z + kd z

t

z(ϑ)dϑ + J + ϕx ξ 2 F + K γ σ .

(8.42)

0

By taking into account (8.31), (8.36) and (8.37), it is t sx = r + kd

r(ϑ)dϑ + J + ϕx ξ 2 F + K γ σ .

(8.43)

0

But, from (8.31) one has t r(ϑ)dϑ = Qx (˙x + x eo ) .

r + kd

(8.44)

0

Alternatively, one can also write r˙ + kd r = Qx (¨x + x e˙ o ) .

(8.45)

8.1 Robot Force Control Without Dynamic Model

223

Now consider the following definition for the state of the error dynamics (8.10), (8.40) and (8.45) ⎡

⎤ sr y = ⎣ r ⎦, F

(8.46)

for which a region of the form (4.28), i.e.    D = y ∈ R2n+m \ y < , is defined for some 0 < < ∞. On the contrary to the previous chapters, cannot be made arbitrary large because it must be chosen small enough to comply with (8.15). Certainly, for a plane ψ can be fairly large, but still is not really free. A stability proof can be carried out following three basic steps: (a) The boundedness of y implies the boundedness of any other closed loop variable. (b) With a proper choice of gains, the observer (8.22)–(8.23) and the control law (8.39) guarantee the boundedness of the state y in closed loop. (c) If all closed loop variables are bounded, then the inclusion of the sign function in (8.28) make all errors tend to zero. (a) It has to be shown that if y is bounded by , then any other signal is bounded. This is completely similar to the procedure given in Sect. 6.3 and it proceeds as follows. From (8.27) one has xr − K γ σ . − Qx (˙xd − x eo ) + J + ϕx ξ 2 F = −˙

(8.47)

Then, from (8.9), (8.25) and (8.31) one gets after some manipulation u = s + Kγ σ ,

(8.48)

u = sx − z˙ ,

(8.49)

with

an equation of the form (6.85). Since y is bounded, so are sr , r and F. Then, from (8.32), (8.37) and the assumption that J −1 (q) exists, sx and z˙ must be bounded, which in turn implies that u is bounded. By applying Lemma 6.1, one concludes that s and σ must be bounded in D. From (8.9) and (8.26), this implies that both eo and e˙ o are bounded. Then, by using (8.5), (8.6), (8.24)–(8.25), one can rewrite (8.31) as ˙x + x x = sx + x z − J + ϕx ξ 2 F − K γ σ .

(8.50)

Note that F is bounded because y is bounded, and thus the righthand side of (8.50) must be bounded. Now, since the lefthand side represents a stable

224

8 Force Control

linear filter, x and ˙x are bounded too. This in turn implies that x and x˙ are bounded, because xd and x˙ d are so. Since q˙ = J(q)−1 x˙ , q˙ must be bounded. Furthermore, q must be bounded since no singularity has been reached. Consider now q˙ r = J −1 (q)˙xr , which is bounded from (8.27); its derivative is given by   d −1 −1 + ˙ q¨ r = J (q)˙xr + J (q) Qx (¨xd − x e˙ o ) − J ϕx ξ 2 λ − K γ σ . dt

(8.51)

Note that from (8.30) it is known that dtd σ is bounded, but it has to be shown that λ in (8.8) is bounded, where λd is bounded by assumption.The boundedness of the Lagrange multiplier λ can be shown by using (3.83), i.e. −1    J ϕ (q)H −1 (q){τ − τ¯ } + J˙ ϕ (q)˙q , λ = J ϕ (q)H −1 (q)J Tϕ (q) where τ¯ = C(q, q˙ )˙q + D˙q + g(q). Therefore, λ will be bounded as long as the input torque τ is bounded because it has been shown that all the other variables are bounded in D. By taking into account the fact that after (8.31)–(8.32) and (8.36)– (8.38) one has so = J(q)sr − r, from (8.39) one has τ = −K p (sr − J −1 (q)r) + J T (q)J Tϕx λd − J T (q)J Tϕx ξ 1 F,

(8.52)

which is bounded for y ∈ D. This shows the boundedness of λ, which in turn implies that of q¨ r , meaning that ya in (8.34) is bounded as well. Thus, s˙r in (8.33) ˙ sr , as a consequence. Now, from (8.43) is bounded, and so is s˙x = J(q)s r + J(q)˙ one gets r˙ + kd r = s˙x − J + ϕx ξ 2 λ − K γ

d σ, dt

(8.53)

which means that r˙, and thus ¨x in (8.45), must be bounded. Finally, note that from (8.37) and (8.49), u˙ = s˙x − z¨ is bounded. (b) The next step is to show that, with a proper choice of gains, one can actually achieve y < , whenever xd and at least its first and second derivatives are bounded. Theorem 4.5 is employed as usual. However, this time the ultimate bound for y given by (4.73) is not important, but rather it is sought that y never leaves D as long as y(t0 ) satisfies (4.56). Assume that this is the case. If (4.29) and (4.53) are satisfied in D for some V (y), then y will never leave D and thus one concludes that y < for all t ≥ t0 as desired. Now define V (y) =

1 T y My, 2

(8.54)

8.1 Robot Force Control Without Dynamic Model

225



with M = block diag {H(q) I ξ 2 }. Clearly (4.29) is satisfied, i.e. α1 (y) ≤ V (y) ≤ α2 (y), with α1 (y) = λ1 y2 and α2 (y) = λ2 y2 , and λ1 =

    1 1 min λh , 1, λmin (ξ 2 ) and λ2 = max λH , 1, λmax (ξ 2 ) , 2 2

where Property 3.7 has been used. After Property 3.2, the derivative of V along (8.10), (8.40) and (8.45) is given by V˙ = − sTr K DP sr − sTr J T J Tϕx λ + sTr K p J −1 r − sTr J T J Tϕx ξ 1 F − sTr ya − kd rT r + rT Qx (¨x + x e˙ o ) + FT ξ 2 λ.

(8.55)

From (8.32) it is sr = J −1 (q)sx ⇒ sTr J T = sTx which combined with (8.31) and Property 3.16 gives J ϕx Jsr = J ϕx sx = J ϕx J + ϕx ξ 2 F + J ϕx K γ σ = ξ 2 F + J ϕx K γ σ ,

(8.56)

so that one has V˙ = − sTr K DP sr − σ T K γ J Tϕx λ − FT ξ 2 λ + sTr K p J −1 r − σ T K γ J Tϕx ξ 1 F − FT ξ 2 ξ 1 F − sTr ya − kd rT r + rT Qx (¨x + x e˙ o ) + FT ξ 2 λ,

(8.57)

or V˙ = − sTr K DP sr − kd rT r − FT ξ 1 ξ 2 F   − σ T K γ J Tϕx λ + ξ 1 F − sTr ya + rT Qx (¨x + x e˙ o ) + sTr K p J −1 r.

(8.58)

While V is positive definite for all y ∈ R(2n+m) , to apply Theorem 4.5 the region D is necessary to show that (4.53) holds. According to the discussion given in item a), every signal in D is bounded, so that bounds can be found to satisfy

226

8 Force Control

   α¯ 1 = max σ T K γ J Tϕx ξ 1  + ya  + Qx (¨x + x e˙ o )  ∀y∈D



α¯ 2 = max σ T K γ J Tϕx λ

(8.59) (8.60)

∀y∈D



c2 = max J −1 (q).

(8.61)

∀y∈D

Then, one gets from (8.58) and Property 3.10 V˙ = − λmin (K p )sr 2 − kd r2 − λmin (ξ 1 ξ 2 )F2 + α¯ 1 y + α¯ 2 + λmax (K p )c2 sr r,

(8.62)

To achieve (4.53) choose λmin (K p ) = 1 + δ 1 kd = δ + λ2max (K p )c22 4 λmin (ξ 1 ξ 2 ) = δ,

(8.63) (8.64) (8.65)

for some δ > 0, to get 1 V˙ ≤ − δy2 − 2



 1 δy2 − α¯ 1 y − α¯ 2 . 2

(8.66)

A conservative value for μ can be defined as 

μ=

α¯ 1 +



α¯ 12 + 2δ α¯ 2 δ

> 0,

(8.67)

so that for y ≥ μ > 0 it is 1  V˙ = − δy2 = −W3 (y). 2

(8.68)

Finally, to apply Theorem 4.5 it should be noted that condition (4.55) is satisfied just by choosing μ arbitrarily small. (c) Showing that x, ˙x, z, z˙ and F tend to zero can be done as shown in Step c) of the proof in Sect. 6.3. Combine From (8.7), (8.9) and (8.31) to get an equation of the form (6.85), i.e. u = s + Kγ σ ,

8.1 Robot Force Control Without Dynamic Model

227

with u = sx + Qx z˙ .

(8.69)

By computing its derivative and using (8.30) one gets an equation of the form (6.121), i.e. ˙ s˙ = −K γ K β s − K γ sign(s) + u. Since u˙ is bounded, then by using the same arguments as for Step c) of the proof in Sect. 6.3 one concludes that in a finite time from (8.9) and (8.24)–(8.25) it holds 0 = e˙ o + x eo + J + ϕx ξ 2 F = sp + sf . Since sp and sf are orthogonal then e˙ o + x eo = 0, meaning that eo , e˙ o → 0 while F = 0. But e˙ o + x eo = ˙x + x x − z˙ − x z = 0,

(8.70)

so that (8.31) and (8.24)–(8.25) can be rewritten as sx = ˙x + x x − x z + J + ϕx ξ 2 F + K γ σ = e˙ o + x eo + z˙ + x z − x z + J + ϕx ξ 2 F + K γ σ = r − z z + J + ϕx ξ 2 F + K γ σ . So that one concludes that s˙x − r˙ = −z z˙ + J + ϕx ξ 2 λ + K γ

d σ. dt

On the other hand, from (8.53) it is s˙x − r˙ = kd r + J + ϕx ξ 2 λ + K γ

d σ. dt

Therefore it holds kd r + z z˙ = (kd I + z )˙z + z z = 0,

(8.71)

which represents a stable linear filter for z, meaning that z, z˙ → 0. But from (8.70) x, ˙x → 0.

228

8 Force Control

It only remains to show that λ → 0, for which Lemma 4.5 can be used. First of all, F is bounded and has a limit because F → 0 as t → ∞, so that in order for λ to tend to zero, it only has to be shown that it is uniformly continuous, or equivalently, that dtd λ is bounded. But, since λ = λ − λd and λd and its derivative are assumed to be bounded, it remains only to show that dtd λ is bounded. From (3.83), it can be seen that the derivative of λ must indeed be bounded because after the discussion of Step a) all variables have been proven to be bounded. Therefore, λ → 0. Finally, force control performance can be improved for unconstrained orientation by keeping the end-effector perpendicular to the contact surface. The best option is to employ the unit quaternion. Define  x˙ d =

 p˙ d , 0 ωd − kε 0 Rd d ε n 0

(8.72)

t with xd = t0 x˙ d dϑ. kε is a positive scalar gain, 0 pd is the desired position of the endeffector as before, 0 ωd ∈ R3 is the desired angular velocity, and 0 Rd ∈ R3×3 is the desired rotation matrix, i.e. it represents the desired orientation. d ε n ∈ R3 is the vector part of the unit quaternion associated to the rotation matrix given by d Rn = 0 RTd 0 Rn , where 0 Rn ∈ R3×3 is the rotation matrix between the end-effector frame and the base frame. Note that the desired position 0 pd has to be chosen to satisfy (8.1), and that care should be taken to avoid getting a not bounded xd . Also, the desired orientation 0 Rd has to be chosen perpendicular to the surface at ϕ(0 pd ) = 0. It has already been shown that x → 0 and ˙x → 0, so that the position error requires no extra analysis, but the orientation case does. When x = 0, the orientation error dynamics is easily computed as  ωn + kε 0 Rd d ε n = 0,

(8.73)

where  ωn = 0 ωn − 0 ωd . The stability analysis can be carried by multiplying (8.73) by d R0 = 0 RTd style to have the orientation error expressed in the desired coordinated frame as  d ωn + kε d ε n = 0.

(8.74)

Then, it should be noted that the unit quaternion is given by a scalar and a vectorial part by Q = {d ηn , d  n } as explained in Sect. 2.1.4. These should be considered as the state related to the orientation error whose derivatives can be computed as 1d T d   ωn 2 n   1 d d ˙ n = ηn I − S d  n  d ω n , 2

d

η˙ n = −

(8.75) (8.76)

8.1 Robot Force Control Without Dynamic Model

229

by combining (2.75)–(2.77). There is not an unique equilibrium point for (8.75) and (8.76) but two equilibria points given by Qe1 = Qe2 =

d d

ηn = 1,  n = 0

 

ηn = −1,  n = 0 .

(8.77) (8.78)

Note that for d  n = 0 one has  d ωn = 0 after (8.74). Consider the following positive definite function Vε =

d

2 ηn − 1 + d ε Tn d εn .

(8.79)

The derivative of Vε along (8.74) can be computed as   V˙ε = 2 d ηn − 1 d η˙ n + 2d εTn d ε˙ n      = − d ηn − 1 d  Tn  d ωn + d ε Tn d ηn I − S d  n  d ωn    = d  Tn  d ωn + d ε Tn −S d  n  d ωn   = −kε d  Tn d ε n +  d ωTn S d  n d ε n = −kε d  Tn d ε n ,

(8.80)

    where taken advantage of the fact that −ST d  n = S d  n and that d itd has been S  n  n = d  n × d  n = 0 (see (2.99)). Now, Theorem 4.3 can be used by noting that V˙ε is negative semidefinite and that V˙ε ≡ 0 in the set M given by the union of Qe1 and Qe2 , i.e. at the equilibria. This is easy to understand by taking into consideration (2.70), i.e. η2 + x2 + y2 + z2 = η2 +  T  = 1. Since for d  n = 0 one has  d ωn = 0, then from (8.75) and (8.76) it is not possible to know to which equilibrium the trajectory is tending. To get an insight about this issue, substitute (2.70) in (8.79) to get Vε =

d

 2  ηn − 1 + 1 − d ηn2 = 2 1 − d ηn .

(8.81)

Substitute now Qe2 to get Vε = 4.

(8.82)

Since η ∈ [−1, 1], then a perturbation σ > 0 can be introduced as d ηn = −1 + σ to get from (8.81) Vε = 2(2 − σ ) < 4.

(8.83)

230

8 Force Control

    Since V˙ε ≤ 0 and Vε d ηn = −1 + σ < Vε d ηn = −1 this means that the trajectory never returns to Qe2 . In turn this means that the equilibrium Qe1 is asymptotically stable. Finally, this implies that 0 Rn → 0 Rd and ωn = 0 ωn − 0 ωd → 0.

8.2 Velocity and Force Observers for the Control of Robot Manipulators Consider once again the relationship between workspace coordinates and joint coordinates given by (2.175) and (2.183), i.e. x˙ =

0

p˙ n 0˙ φn

 = J(q)˙q or x˙ =

0

p˙ n 0 ωn

 = J(q)˙q,

(8.84)

where for the first case J(q) represents the analytic Jacobian and for the second one t it stands for the geometric Jacobian. In general, for n = 6 it is 0 φ n = t0 0 ωn dϑ, but for simplicity’s sake an abuse of notation is made by using (8.2). This is done because the approach of this section can be employed with any of the two Jacobians and for which the design of a force control-observer scheme where the desired task is directly given in workspace coordinates is introduced. Say that Assumption 8.2 holds and use (3.84), i.e. ϕ(x) = 0, and model (3.87), i.e. H(q)¨q + C(q, q˙ )˙q + D˙q + g(q) = τ − J T (q)J Tϕx λ, to get the robot dynamics in workspace coordinates as H c x¨ + C c x˙ + Dc x˙ + gc = f − J Tϕx λ,

(8.85)

H c = J −T (q)H(q)J −1 (q)   −1 C c = J −T (q) C(q, q˙ )J −1 (q) + H(q)J˙ (q)

(8.86)

Dc = J −T (q)DJ −1 (q)

(8.88)

where

(8.87)

gc = J

−T

(q)g(q)

(8.89)

f =J

−T

(q)τ ,

(8.90)

8.2 Velocity and Force Observers for the Control of Robot Manipulators

231

  −1 and J˙ (q) = dtd J −1 (q) . The first step is the velocity observer design directly in workspace coordinates. On the contrary to the observer introduced in the last section, it is no longer assumed that the contact surface is a plane, but all the errors used before are employed again, i.e. (8.5)–(8.8). Consider now t xˆ = xˆ (t0 ) +

x˙ˆ (ϑ) dϑ

(8.91)

t0

x˙ˆ = Qx (˙xd − x eo ) + K d z

t z(ϑ)dϑ + z z + K d z,

(8.92)

t0

where z , K d ∈ Rn×n are diagonal positive definite matrices. At this point, it is still valid that if Assumption 8.1 holds, then the approximations (8.13) and (8.14) also hold. However, since the constraint is not necessarily a plane anymore, in order to make a similar assumption for the observation errors one should have z ≤ ψ

(8.93)

with the same ψ > 0 and make the following assumption. Assumption 8.3 The robot end-effector begins its movement at the constraint sur face at rest so that x˙ (t0 ) = 0 holds, and it is set (ˆx(t0 ), x˙ˆ (t0 )) = (x(t0 ), 0). Assuming the robot end-effector to be in contact with the environment at t = t0 and at rest is not unrealistic, but it may require another free movement control algorithm to take it to that position. Having zero initial observation error guarantees that (8.24)– (8.25) and x˙ˆ ≈ Qx x˙ˆ

(8.94)

hold. Certainly, these relationships are in principle only valid at t = t0 , but it will be shown that it is possible to design a control law to make them valid for all time. To describe the observer error dynamics, it is taken advantage of a local analysis where (8.15) and (8.93) hold and it is defined similar to (6.43) and (6.45) the following variables x˙ o = x˙ˆ − z z r = x˙ − x˙ o = z˙ + z z.

(8.95) (8.96)

Now, rewrite (8.92) as x˙ − x˙ˆ + K d z

t z(ϑ)dϑ + z z + K d z = Qx (˙x − x˙ d + x eo ) , t0

(8.97)

232

8 Force Control

where (3.120) has been used. Then compute the derivative to get ˙ x (˙x + x eo ) . r˙ + K d r = Qx (¨x + x e˙ o ) + Q

(8.98)

For control design assume that the vector of Lagrange multipliers λ ∈ Rm is not measurable so that an estimated value has to be computed as λˆ ∈ Rm . Of course, the regular force errors (8.8) and (8.10) can no longer be used and the following auxiliary errors have to be introduced λ¯ = λˆ − λd ,

(8.99)

and F¯ =

t

¯ λdϑ,

(8.100)

t0

where λd ∈ Rm is the desired bounded force, with at least its first derivative bounded. Instead of (8.9) one has ¯ s = Qx (˙eo + x eo ) + J + ϕx ξ 2 F = sp + sf ,

(8.101)

but as before sp and sf are orthogonal vectors, and they both must become zero if s vanishes. In that case, since J Tϕx and J + ϕx are assumed to be full rank, one has s = 0 ⇒ sf = 0 ⇒ F¯ = 0,

(8.102)

because ξ 2 ∈ Rm×m is a positive definite diagonal matrix. On the other hand, note that by assuming as before Qx x = x Qx (e.g. by setting x = kx I, with kx > 0), then in view of (8.13) and (8.14) it is sp = Qx (˙eo + x eo ) ≈ e˙ o + x eo .

(8.103)

From (8.103) one can conclude that both eo and e˙ o tend to zero if sp → 0, i.e. if s → 0. Furthermore, consider once again (8.28), or (8.30), and alike to (8.27) and (8.31) define ¯ x˙ r = Qx (˙xd − x eo ) − J + ϕx ξ 2 F − K γ σ sx = x˙ − x˙ r = Qx (˙x + x eo ) +

¯ J+ ϕx ξ 2 F

(8.104) + Kγ σ .

(8.105)

This allows to rewrite the robot dynamics in (8.85) as follows H c s˙x + C c sx + Dc sx = f − J Tϕx λ − ya ,

(8.106)

8.2 Velocity and Force Observers for the Control of Robot Manipulators

233

where ya = H c x¨ r + C c x˙ r + Dc x˙ r + gc . Based on all previous definitions, the proposed control law is so = x˙ o − x˙ r f = −K p so +

J Tϕx

  λd − ξ 1 F¯ − ξ 3 λ¯ ,

(8.107) (8.108)

with ξ 1 , ξ 3 ∈ Rm×m diagonal positive definite matrices. Note that for implementation it must be employed τ = J T (q)f

(8.109)

after (8.90). By substituting (8.108) in (8.106) and by noting that so = sx − r, one gets   H c s˙x + C c sx + K DP sx = K p r − J Tϕx λ + ξ 1 F¯ + ξ 3 λ¯ − ya ,

(8.110)

where K DP = Dc + K p . Equation (8.110) describes the dynamics of tracking errors. It remains to design the force observer. In order to do so, from (3.84) it is ¨ ϕ(x) = J ϕx x¨ + J˙ ϕx x˙ = 0,

(8.111)

so that by taking into account (8.85) it is       T −1 J ϕx H −1 f − C c x˙ − Dc x˙ − gc + J˙ ϕx x˙ . λ = J ϕx H −1 c J ϕx c

(8.112)

An observer is then given by       T −1 −1 ˙ˆ )x˙ˆ − Dc x˙ˆ − g + J˙ ϕx (x, x˙ˆ ) . J f − C J H (x, x λˆ = J ϕx H −1 ϕx c c c ϕx c (8.113) Note that C c becomes dependent on x˙ˆ through q˙ˆ = J −1 (q)x˙ˆ . By substituting λ¯ = λˆ − λd and (8.108), λˆ can be implemented as     T −1 J f λˆ = λd − (I + ξ 3 )−1 ξ 1 F¯ + J ϕx H −1 x c ϕx   ˙ K p so +  f x = J ϕx H −1 C c x˙ˆ + Dc x˙ˆ + gc − Jˆ ϕx x˙ˆ , c

(8.114) (8.115)

where it has been defined  C c = C c (x, x˙ˆ ) J˙ˆ ϕx = J˙ ϕx (x, x˙ˆ ).

(8.116) (8.117)

234

8 Force Control

Then, the state of the errors dynamics in (8.98), (8.100) and (8.110) can be defined as ⎤ ⎡ sx (8.118) y = ⎣ r ⎦. F¯ As usual define a region of interest of the form (4.28), i.e.   D = y ∈ R(2n+m) |y < , but with the constraint that cannot be made arbitrary large. On the contrary, it has to be chosen small enough for (8.15) and (8.93) to hold. Recall that for that it is mandatory to choose xd satisfying (3.84) but also far away enough from any singularity and with bounded first and second derivatives. In the same fashion, the desired bounded force λd must be bounded as well as its first derivative. To employ Theorem 4.5, consider the following positive definite function for y in (8.118) V (y) =

1 T y My, 2

(8.119)

with M = block diag {H(q) I ξ 2 ξ 3 }. Clearly, it satisfies (4.29), i.e. α1 (y) ≤ V (y) ≤ α2 (y), with α1 (y) = λ1 y2 and α2 (y) = λ2 y2 , and λ1 =

    1 1 min λh , 1, λmin (ξ 2 ξ 3 ) and λ2 = max λH , 1, λmax (ξ 2 ξ 3 ) , 2 2

where Property 3.7 has been used. As done in the previous section, three steps are necessary to prove local asymptotic stability in D. (a) First of all, it can be shown that whenever y is bounded by 0 < < ∞, then any other signal related to the closed loop system is bounded. This step is ommited because it is completely similar to that shown in the previous section. (b) The next step is to set gains to enforce y < for all t ≥ t0 . This can be done by using Theorem 4.5. By direct computation the derivative of V in (8.119) along (8.98), (8.100) and (8.110) is given by

8.2 Velocity and Force Observers for the Control of Robot Manipulators

235

1 T V˙ = sTx H˙ c sx + sTx H c s˙x + rT r˙ + F¯ ξ 2 ξ 3 λ¯ 2 = − sTx K DP sx + sTx K p r − sTx ya − sTx J Tϕx λ − sTx J Tϕx ξ 1 F¯ ˙ x (˙x + x eo ) − sTx J Tϕx ξ 3 λ¯ + rT Qx (¨x + x e˙ o ) + rT Q T ¯ − rT K d r + F¯ ξ 2 ξ 3 λ.

(8.120)

From (8.105) it is sTx J Tϕx = F¯ ξ 2 + σ T K γ J Tϕx , T

(8.121)

  T T −1 because after Property 3.16 it is J ϕx J + = I and J ϕx Qϕx = ϕx = J ϕx J ϕx J ϕx J ϕx O. By substituting (8.121) in (8.120) T V˙ = − sTx K DP sx + sTx K p r − sTx ya − rT K d r + F¯ ξ 2 ξ 3 λ¯

− F¯ ξ 2 λ − σ T K γ J Tϕx λ − F¯ ξ 1 ξ 2 F¯ − σ T K γ J Tϕx ξ 1 F¯ T

T

T − F¯ ξ 2 ξ 3 λ¯ − σ T K γ J Tϕx ξ 3 λ¯ ˙ x (˙x + x eo ) . + rT Qx (¨x + x e˙ o ) + rT Q

(8.122)

While V is positive definite for all y ∈ R(2n+m) , to apply Theorem 4.5 it is necessary to show that (4.53) holds in D. But, according to the discussion given in Step a), every signal in D is bounded, so that bounds can be found to satisfy 

α¯ 1 = max ya 

(8.123)

   ˙ x (˙x + x eo ) α¯ 2 = max Qx (¨x + x e˙ o ) + Q

(8.124)

∀y∈D ∀y∈D



α¯ 3 = max σ T K γ J Tϕx ξ 1 + λT ξ 2  ∀y∈D



α¯ 4 = max σ T K γ J Tϕx (ξ 3 λ¯ + λ). ∀y∈D

(8.125) (8.126)

Keeping definitions (8.123)–(8.126) in mind, one gets from (8.122) ¯ 2 V˙ = − λmin (K p )sx 2 − λmin (K d )r2 − λmin (ξ 1 ξ 2 )F ¯ + α¯ 1 sx  + α¯ 2 r + α¯ 3 F + α¯ 4 + λmax (K p )sx r ¯ 2 − λmin (K d )r2 = − λmin (K p )sx 2 − λmin (ξ 1 ξ 2 )F + αy + α¯ 4 + λmax (K p )sx r,

(8.127)

236

8 Force Control 

where α = α¯ 1 + α¯ 2 + α¯ 3 . To achieve (4.53), gains can be chosen as λmin (K p ) = 1 + δ 1 λmin (K d ) = δ + λ2max (K p ) 4 λmin (ξ 1 ξ 2 ) = δ,

(8.128) (8.129) (8.130)

with δ > 0, to get V˙ = −δsx 2 − δr2 − δF2 + αy + α¯ 4   1 1 δy2 − αy − α¯ 4 . = − δy2 − 2 2

(8.131)

A conservative value for μ in (4.53) can be found to be 

α 2 + 2δ α¯ 4 > 0, δ

(8.132)

1 V˙ = − δy2 = −W3 (y). 2

(8.133)

μ=

α+

so that if y ≥ μ one has

Condition (4.55), i.e. μ < α2−1 (α1 ( )), can be achieved by tuning ξ 1 , ξ 2 , K p and K d large enough. Finally, by assuming that condition (4.56) holds, i.e. y(t0 ) = y0  ≤ α2−1 (α1 ( )), then it can be concluded that y ∈ D for all t ≥ t0 , which allows to conclude this part of the proof. (c) Till now it has been proven that y is bounded by as long as the initial condition ¯ x and ˙x, z, z˙ , x is small enough and gains are properly set. Showing that F, and ˙x tend to zero can be done as shown in Step c) of the proof of the previous section. To show that λ¯ → 0, once again Lemma 4.5 can be used. First of all, it is known that F¯ is bounded and has a limit (F¯ → 0) as t → ∞. Then, in order for λ¯ to tend to zero, dtd λ¯ must be bounded. Since λ¯ = λˆ − λd and λd and its derivative are assumed to be bounded, it remains only to show that d ˆ λ is bounded. From (8.100) and (8.114) one has dt

8.2 Velocity and Force Observers for the Control of Robot Manipulators

d ˆ d ¯ λ = λd − (I + ξ 3 )−1 ξ 1 λ+ dt dt     d  −1 T −1 −1 T −1 d J ϕx H c J ϕx f x + J ϕx H c J ϕx f . dt dt x

237

(8.134)

From Step a), the right hand side of (8.134) must be bounded. This in turn means that dtd λˆ and dtd λ¯ are bounded. After the aforementioned lemma λ¯ tends to zero. On the other hand, since x˙ˆ → x˙ one must have from (8.112) and (8.113) that λˆ → λ. Finally, this necessarily means that λ = λ − λd → 0.

8.3 GPI Based Velocity/Force Observer Design for Robot Manipulators As it was shown in Sect. 7.5 a possibility to deal with uncertain model parameters are the Generalized Proportional Integral (GPI) observers. Consider model (3.77), i.e. H(q)¨q + C(q, q˙ )˙q + D˙q + g(q) = τ − J Tϕ (q)λ, where J ϕ (q) = ∇ϕ(q) ∈ Rm×n as given in (3.78) with respect to the holonomic constraint given in (3.66), i.e. ϕ(q) = 0 , which arises when the manipulator is in contact with the environment. Therefore, J ϕ (q) maps a vector into the normal plane at the contact point. It is assumed that J ϕ (q) is full rank, i.e. rank(J ϕ (q)) = m. Note that usually J ϕ (q) can be normalized to get J ϕ  = 1.

(8.135)

In the following it will be assumed that this is the case. If the position tracking error is defined by e = q − qd = Q(q)q + P(q)q − Q(qd )qd − P(qd )qd ,

(8.136)

where qd fulfils (3.66), then completely similar to (8.13), if e is small enough it holds e ≈ Q(q)(q − qd ) = Q(q)e,

(8.137)

because the error tends to be tangent to the surface. But in fact Property 3.16 is valid also for joint space coordinates, i.e. q˙ = Q(q)˙q so that it holds as well

238

8 Force Control

q˙ d ≈ Q(q)˙qd ⇒ e˙ ≈ Q(q)(˙q − q˙ d ) = Q(q)˙e.

(8.138)

Indeed, “small enough” does not necessarily mean e ≈ 0, so that it is more convenient to make the following assumption equivalent to (8.15). Assumption 8.4 The approximations (8.137) and (8.138) are valid if e ≤ ψ holds for some ψ > 0.

(8.139) 

In order to design a velocity/force observer for tracking control, first consider rewriting (3.77) as ! q¨ = H −1 (q) τ − C(q, q˙ )˙q − D˙q − g(q) − H −1 (q)J Tϕ (q)λ.

(8.140)

Define z1 = −H −1 (q)J Tϕ (q)λ

(8.141)

N(q, q˙ ) = C(q, q˙ )˙q + D˙q + g(q),

(8.142)

! q¨ = H −1 (q) τ − N(q, q˙ ) + z1 .

(8.143)

to have

Just like in the previous section, in order to design a force control law it will be necessary to estimate both the force λ and the state q˙ . However, this time the same approach as that of Sect. 7.5 will be employed, for which it is necessary to assume that the vector z1 can be expressed as in (7.110), i.e. z1 (t) =

p−1 "

ai t i + r(t) ,

(8.144)

i=0

where each ai is a n–vector of constant coefficients and r(t) represents a residual term whose first p time derivatives are assumed to exist. This is necessary to rewrite z1 (t) in the following form z˙ 1 = z2 z˙ 2 = z3 .. . z˙ p−1 = z p z˙ p = r( p) (t) .

(8.145)

(8.146)

8.3 GPI Based Velocity/Force Observer Design for Robot Manipulators

239

On the contrary to Sect. 7.5, the following GPI observer directly estimates velocities instead of errors q˙ˆ = qˆ 2 + λ p+1 e˜ ! qˆ˙ 2 = H −1 (q) τ − N(q, qˆ 2 ) + zˆ 1 + λ p e˜ z˙ˆ 1 = zˆ 2 + λ p−1 e˜

(8.147) (8.148) (8.149)

z˙ˆ 2 = zˆ 3 + λ p−2 e˜ .. . z˙ˆ p−1 = zˆ p + λ1 e˜ z˙ˆ p = λ0 e˜ ,

(8.150) (8.151)

where e˜ = q − qˆ .

(8.152)

Also, qˆ 2 is an estimated value for q˙ . In order to compute the system closed loop dynamics, consider the following error definitions z˜ k = zk − zˆ k ,

(8.153)

for k = 1, . . . , p. Then, from (8.143), (8.145)–(8.146) and (8.147)–(8.151) one gets directly e˙˜ = q˙ − q˙ˆ = e˜ 2 − λ p+1 e˜

!

e˙˜ 2 = −H (q) N(q, q˙ ) − N(q, qˆ 2 ) + z˜ 1 − λ p e˜ z˜˙ 1 = z˜ 2 − λ p−1 e˜ z˙˜ 2 = z˜ 3 − λ p−2 e˜ −1

(8.154) (8.155) (8.156)

.. . z˙˜ p−1 = z˜ p − λ1 e˜ z˙˜ p = r( p) (t) − λ0 e˜ ,

(8.157)

where e˜ 2 = q˙ − qˆ 2 .

(8.158)

It is rather direct to get from (8.156)–(8.157) the following relationship ( p)

z˜ 1 = r( p) (t) − λ0 e˜ − · · · − λ p−1 e˜ ( p−1) ,

(8.159)

240

8 Force Control

while from (8.142) and (8.155) it is N(q, q˙ ) − N(q, qˆ 2 ) = C(q, q˙ )˙q − C(q, qˆ 2 )ˆq2 + D˜e2 .

(8.160)

Combining (8.154) and (8.158) it is e˜ 2 = q˙ − qˆ 2 = e˙˜ + λ p+1 e˜ ,

(8.161)

qˆ 2 = q˙ − e˙˜ − λ p+1 e˜ .

(8.162)

which implies that

By applying Property 3.4 and after a rather direct computation one gets C(q, qˆ 2 )ˆq2 = C(q, q˙ − e˙˜ − λ p+1 e˜ )(˙q − e˙˜ − λ p+1 e˜ ) = C(q, q˙ )˙q − 2C(q, q˙ )(e˜˙ + λ p+1 e˜ ) + C(q, e˙˜ + λ p+1 e˜ )(e˙˜ + λ p+1 e˜ ), (8.163) which means that by taking this relationship and (8.161) into account, (8.160) can be rewritten as N(q, q˙ ) − N(q, qˆ 2 ) =2C(q, q˙ )(e˙˜ + λ p+1 e˜ ) − C(q, e˜˙ + λ p+1 e˜ )(e˜˙ + λ p+1 e˜ ) + D(e˙˜ + λ p+1 e˜ ), (8.164) which in turn means that (8.155) can be expressed as  e˙˜ 2 = z˜ 1 − λ p e˜ − H −1 (q) 2C(q, e˙ + q˙ d )(e˙˜ + λ p+1 e˜ )

 − C(q, e˙˜ + λ p+1 e˜ )(e˙˜ + λ p+1 e˜ ) + D(e˙˜ + λ p+1 e˜ ) ,

(8.165)

where the equality q˙ = e˙ + q˙ d has been employed. Combine with (8.161) to have e¨˜ + λ p+1 e˙˜ + λ p e˜ = z˜ 1 + f (t),

(8.166)

with  f (t) = −H −1 (q) 2C(q, e˙ + q˙ d )(e˙˜ + λ p+1 e˜ )

 −C(q, e˙˜ + λ p+1 e˜ )(e˙˜ + λ p+1 e˜ ) + D(e˙˜ + λ p+1 e˜ ) .

(8.167)

Finally, by computing the p-th derivative of (8.166) and substituting in (8.159) one gets

8.3 GPI Based Velocity/Force Observer Design for Robot Manipulators

e˜ ( p+2) + λ p+1 e˜ ( p+1) + · · · + λ0 e˜ = r( p) (t) + f ( p) (t).

241

(8.168)

Equation (8.168) represents the observer closed loop dynamics and it can be rewritten in the form (7.133)–(7.136) as x˙ o = Axo + Brf ,

(8.169)

with rf = r( p) + f ( p) and ⎡ ⎢ xo = ⎣

e˜ .. .

⎤ ⎥ ⎦

(8.170)

( p+1)

e˜ ⎡

⎤ O I ··· O ⎢ .. .. ⎥ ⎢ . ⎥ A=⎢ . ⎥ ⎣ O O ··· I ⎦ −λ0 −λ1 · · · −λ p+1 !T B = O ··· O I .

(8.171)

(8.172)

The next step is to design the force observer. Consider that after (8.141) it is J Tϕ (q)λ = −H(q)z1 .

(8.173)

Therefore, λ can be obtained by doing T λ = (J + ϕ ) H(q)z1 ,

(8.174)

because J ϕ is full rank. But since an estimated value of z1 is known, then it is quite direct to get an estimated value for λ as T z1 , λˆ = (J + ϕ ) H(q)ˆ

(8.175)

which means that the force observation error is given by T + T z1 λ˜ = λ − λˆ = (J + ϕ ) H(q)z1 − (J ϕ ) H(q)ˆ T = (J + z1 , ϕ ) H(q)˜

(8.176)

where z˜ 1 = z1 − zˆ 1 is the observation error for z1 . Note that as mentioned before it is assumed that J ϕ  = 1 and that the robot pushes on the surface so that the force

242

8 Force Control

is always positive or zero. For that case, when the constraint in (3.66) has dimension one, i.e. m = 1, so that λˆ is a scalar value, then it holds λˆ = J ϕ λˆ  = H(q)ˆz1 .

(8.177)

Finally, the control law is chosen as a simple PID given by t τ = − K v (ˆq2 − q˙ d ) − K p e − K i Q

e dϑ t0

¯ − J Tϕ (q)λd + kFi J + ϕ (q)F ,

(8.178)

where K v , K p ∈ Rn×n are positive definite diagonal matrices and as usual (·)d denotes the desired value of (·), which is assumed to be bounded, as well as its derivatives, both for forces and positions. Also, (8.99) and (8.100) are used again, i.e. λ¯ = λˆ − λd t F¯ = λ¯ dϑ . t0

To compute the closed loop system dynamics consider first rewriting (8.178) as τ = − K v (˙e − e˙˜ − λ p+1 e˜ ) − K p e − K i Q

t e dϑ t0

−

J Tϕ (q)λd

+

¯ kFi J + ϕ (q)F t

= − K v e˙ − K p e − K i Q

e dϑ t0

˙˜ + λ p+1 e˜ ), ¯ − J Tϕ (q)λd + kFi J + ϕ (q)F + K v (e

(8.179)

where (8.162) was taken into account. Define the auxiliary variable w˙ = e˙ + e

(8.180)

with  ∈ Rn×n a diagonal positive definite matrix. As before, the goal is to carry out a local stability analysis where (8.139) is valid, so that e = Qe and e˙ = Q˙e hold. Furthermore, by assuming Q = Q (e.g. by setting  = kλ I, with kλ > 0) then ˙ so that the control law in (8.179) is equivalent to also holds w˙ = Qw, ˙˜ + λ p+1 e˜ ), ¯ τ = −K v w˙ − K¯ i Qw − J Tϕ λd + kFi J + ϕ (q)F + K v (e

(8.181)

8.3 GPI Based Velocity/Force Observer Design for Robot Manipulators

243

with K p = K v  + K¯ i K i = K¯ i .

(8.182) (8.183)

By substituting (8.181) in (3.77) one gets 1 H(q)¨q = − C(q, q˙ )˙q − D˙q + g(q) − K v w˙ − K¯ i Qw + J Tϕ λ 2 1 1 1 + ¯ ˙˜ + λ p+1 e˜ ), ¯ + J Tϕ λ + kFi J + ϕ F + kFi J ϕ F + K v (e 2 2 2

(8.184)

where λ = λ − λd is the force error as given in (8.8). The previous expression may not be adequate to carry out the stability analysis, but consider defining 1 −1 T 1 −1 + ¯ ¯ q˙ r = q˙ d − e − K −1 v K i Qw + K v J ϕ λ + kFi K v J ϕ F 2 2 s = q˙ − q˙ r   1 −1 T 1 −1 ¯ −1 + ¯ = (w˙ + K v K i Qw) + − K v J ϕ λ − kFi K v J ϕ F 2 2 = sp + s f .

(8.185)

(8.186)

It is desired as before that sp and sf lie in orthogonal subspaces. To achieve this it is T −1 T necessary that PK −1 v J ϕ = K v J ϕ holds. This can be done by setting K v = kv I, with kv > 0. Therefore, one can rewrite (8.184) as H(q)˙s + C(q, q˙ )s + K d s =

1 T 1 ¯ J ϕ λ + kFi J + ϕ F + ya , 2 2

(8.187)

where K d = D + K v and   ya = K v (e˙˜ + λ p+1 e˜ ) − H(q)¨qr + C(q, q˙ )˙qr + D˙qr + g(q) .

(8.188)

Then, the complete closed loop system dynamics is described by equations (8.100), (8.169) and (8.187), for which the corresponding state vector is defined as ⎤ xo x = ⎣ s ⎦. F¯ ⎡

For this state it is always possible to define a region of the form (4.28), i.e.   D = x ∈ Rn( p+2)+m |x < ,

(8.189)

244

8 Force Control

where is a positive constant chosen small enough for (8.15) to hold. Then, it can be shown that for qd a desired bounded trajectory fulfilling constraint (3.66) with at least their first p + 2 derivatives bounded and a bounded desired force λd whose derivatives up to p + 2 are also bounded, a combination of gains always exist to make the tracking errors (e, e˙ ), the observation errors xo , and the force errors ˜ λ, ¯ F) ¯ arbitrarily small as long as the initial condition x(t0 ) lies in a (λ, λ, subset of D chosen small enough to avoid the trajectory x leaving D during the transient response. Furthermore, the estimate zˆ 1 becomes arbitrarily close to the real value z1 . The proof is omitted because it is rather involved and not directly based in the theory presented in Chap. 4. Note that it may be required that the initial velocity be zero because for that case the initial observation and tracking errors can easily be set to zero.

8.4 Experimental Results In this section, an experimental evaluation of the force controllers described above is presented. The experimental setup consisted of the Geomagic Touch robot described in Chap. 14 in contact with a horizontal-flat surface, and the ATI Nano17 force sensor attached at the tip of the manipulator. The three degrees of freedom configuration characterized in Sect. 14.2.5 is employed, i.e. the last three joints are mechanically blocked. The desired position trajectory for all the experiments below is given by ⎡

⎤ 0.12 m pd = ⎣0.02 sin (0.25π t) m⎦ , −0.1425 m i.e. a sinusoidal over the Cartesian axis y0 . The reason to select such trajectory is the limitation imposed by keeping the end-effector perpendicular to the surface, when using the three-degrees-of-freedom configuration of the Phantom robot. The desired Cartesian velocities are obtained by taking the time-derivative of the desired positions. In turn, the desired force is defined by λd = 0.5 − 0.3 cos (π t) N . Robot force control without dynamic model Tuning guide The equations required to implement the scheme of Sect. 8.1 are the tracking and observation errors (8.5)–(8.7), the force error definitions (8.8) and (8.10), the velocity observer (8.23), the auxiliary variables (8.9), (8.26)–(8.28) and (8.36)–(8.38), and the control law (8.39). The tuning of this controller-observer can be carried out as follows.

8.4 Experimental Results

245

Table 8.1 Chosen gains for the force control without dynamic model scheme of Sect. 8.1 Gain Value Kp x kd z Kβ Kγ ζ1 ζ2

0.2I 20I 10 10I 5I 0.1I 0.5I I

• At first, neglect the force control terms as well as the sliding mode ones, by setting λd = 0 and ζ 1 = ζ 2 = K β = K γ = O. • Tune the observer gains, kd and z , satisfying (6.65)–(6.67) and (6.69)–(6.70), to minimize the observation error z. • Tune the remaining position control tracking as a standard PD controller, with K p as the derivative gain and x = K −1 p K px , where K px is the proportional gain matrix. • Increase K γ slowly to improve the tracking performance, but preventing the actuators to present chattering. • Increase K β slowly in the same manner. • Include the force control terms and increase ζ 1 and z2 until obtaining the desired force tracking performance. Experiment The selected gains for the implementation of the controller presented in Sect. 8.1, by following the tuning guide above are displayed in Table 8.1. The Cartesian position tracking is shown in Fig. 8.3, whereas the corresponding tracking error is displayed in Fig. 8.4. The Cartesian velocities estimated by the proposed observer, displayed along with the desired velocities, are presented in Fig. 8.5. In Fig. 8.6, the force tracking and the force tracking error are shown as well. Velocity and Force Observers Tuning Guide The equations needed to implement the controller based on the velocity and force observers of Sect. 8.2 are the tracking and observation errors (8.5)–(8.7), the force error definitions (8.8) and (8.10), the force observation errors (8.99)–(8.100), the velocity observer (8.92), the dynamics in workspace coordinates (8.86)–(8.90), the auxiliary variables (8.101), (8.103)–(8.105), and (8.107), the control law (8.108)– (8.109), and the force observer (8.114). The tuning of this controller-observer can be carried out as follows.

246

8 Force Control

Fig. 8.3 Robot force control without dynamic model, Cartesian position tracking: desired (· · · ), real (—)

• At first, neglect the force control terms as well as the sliding mode ones, by setting λd = 0 and ζ 1 = ζ 2 = ζ 3 = K β = K γ = O. • Tune the observer gains, kd and z , satisfying (6.65)–(6.67) and (6.69)–(6.70), to minimize the observation error z. • Tune the remaining position tracking control as a standard PD controller, with K p as the derivative gain and x = K −1 p K px , where K px is the proportional gain matrix. • Increase K γ slowly to improve the tracking performance, but preventing the actuators to present chattering. • Increase K β slowly in the same manner. • Include the force control terms and increase ζ 1 and z2 until obtaining the desired force tracking performance. • Finally, increase ζ 3 slowly to improve the force tracking. Note that ζ 3 = I for (8.114) to be well defined. Experiment The selected gains for the implementation of the controller presented in Sect. 8.2, by following the tuning guide above are displayed in Table 8.2.

8.4 Experimental Results

247

Fig. 8.4 Robot force control without dynamic model, Cartesian position tracking error

Remark 8.1 Because of the extra weight added by the force sensor, the parameters obtained in Sect. 14.2.5 are no longer valid. Instead, a new identification was carried out following the same methods of Sect. 14.2.5, giving the new parameters vector θ 0 = 0.0011 0.0044 0.0045 0.0457 0.0831 0.0771 0.0125 0.0242

!T

. 

The Cartesian position tracking is shown in Fig. 8.7, whereas the corresponding tracking error is displayed in Fig. 8.8. The Cartesian velocities estimated by the proposed observer, displayed along with the desired velocities, are presented in Fig. 8.9. The force tracking and force estimation, as well as the corresponding force tracking error and force observation error, are shown in Fig. 8.10.

248

8 Force Control

Fig. 8.5 Robot force control without dynamic model, Cartesian velocities: desired (· · · ), observer (—) Table 8.2 Chosen gains for the velocity and force observers based controller of Sect. 8.2 Gain Value Kp x kd z Kβ Kγ ζ1 ζ2 ζ3

20I 20I 10 10I 5I 0.1I 0.5I I 0.01I

8.4 Experimental Results

249

Fig. 8.6 Robot force control without dynamic model, contact forces (up) and force error (down): desired (· · · ), measured (—)

GPI based velocity and force observers Tuning guide The required equations to implement the GPI force/velocity observers controller of Sect. 8.3 are the tracking error (8.136), the observer equations (8.147)–(8.151), the observation errors (8.152) and (8.153), the force error definitions (8.8) and (8.10), the force observation errors (8.99)–(8.100), the force observer (8.175), and the control law (8.178). This controller can be tuned by following the suggestions below. • Choose the degree of the polynomial, p, in (8.144) to define the order of the state extension, which in turn is the internal model for the unknown signals. As a rule, the higher this value, the more accurate will be the estimation of the unknown terms lumped in z1 , but the peaking phenomena is worsen. For most manipulation tasks, p = 2 is a good choice. • Compute the matrix gains λ0 in (8.168), by defining the poles of the polyinomial matrix in the left-hand side of this equation. Locate the poles as far on the open left-half complex plane as the bandwidth of the system allows it. As a rule, the more on the left the poles are located, the more accurate the observer estimation, but at the expense of augmenting the peaking phenomena and the sensitivity to noise.

250

8 Force Control

Fig. 8.7 Velocity and force observers, Cartesian position tracking: desired (· · · ), real (—)

• Now, neglect temporarily the force control terms by making λd = 0 and kFi = 0, and tune the remaining ones as a standard PID controller to get the desired position tracking performance. • Finally, include the force control terms and increase slowly kFi to obtain the desired force tracking performance. Remark 8.2 Note that since this controller is designed in joint coordinates, the inverse and differential kinematics are required for the implementation. 

Remark 8.3 Due to the high gain nature of GPI observers, sometimes it may be advisable to filter zˆ 1 in the following form z˙ f = −ωf zf + ωf zˆ 1 ,

(8.190)

with ωf > 0. In that case the filtered estimated force is given by T λˆ f = (J + ϕ ) H(q)zf .

(8.191) 

8.4 Experimental Results

251

Fig. 8.8 Velocity and force observers, Cartesian position tracking error

Remark 8.4 To reduce the undesired peaking phenomena, it is convenient to implement a suitable clutch, which can be of the form: # sin8 ( πt ) if t ≤ 2 f clutch = , 1 if t > where is the time duration of the peaking. Finally, multiply eˆ 2 and zˆi , i = 1, . . . , p,  by f clutch . Experiment The gains chosen for the implementation of the GPI force/velocity observers based force controller of Sect. 8.3, by following the tuning guide above, are shown in Table 8.3. The Cartesian position tracking is shown in Fig. 8.11, whereas the corresponding tracking error is displayed in Fig. 8.12. The Cartesian velocities estimated by the proposed observer, displayed along with the desired velocities, are presented in Fig. 8.13. The force tracking and force estimation, as well as the corresponding force tracking error and force observation error, are shown in Fig. 8.14.

252

8 Force Control

Fig. 8.9 Velocity and force observers, Cartesian velocities: desired (· · · ), observer (—)

Fig. 8.10 Velocity and force observers, contact forces (up), force tracking error (middle), and force observation error (down): desired (· · · ), measured (—), observer (- . -)

8.4 Experimental Results

253

Table 8.3 Chosen gains for the GPI force/velocity observers based controller of Sect. 8.3 Gain Value λ0 λ1 λ2 λ3 Kp Ki Kv kFi

1 × 108 I 4 × 106 I 6 × 104 I 400I 5I 0.1I 0.05I 0.5 0.8 s

Fig. 8.11 GPI velocity/force observers, Cartesian position tracking: desired (· · · ), real (—)

254

8 Force Control

Fig. 8.12 GPI velocity/force observers, Cartesian position tracking error

Fig. 8.13 GPI velocity/force observers, Cartesian velocities: desired (· · · ), observer (—)

References

255

Fig. 8.14 GPI velocity/force observers, contact forces (up), force tracking error (middle), and force observation error (down): desired (· · · ), measured (—), observer (- . -)

References Arimoto S (1995) Joint-space orthogonalization and passivity for physical interpretations of dexterous robot motions under geometric constraints. Int J Robust Nonlinear Control 5(4):259–284 Arteaga-Pérez MA, Castillo-Sánchez AM, Parra-Vega V (2006) Cartesian control of robots without dynamic model and observer design. Automatica 42:473–480 Arteaga-Pérez MA, Rivera-Dueñas JC (2007) Force control without inverse kinematics nor robot model. In: Proceedings CD ROM. European Control Conference ECC07. Kos Island, Greece, pp 4385–4392 Arteaga-Pérez MA, Rivera-Dueñas JC, Gutiérrez-Giles A (2013) Velocity and force observers for the control of robot manipulators. ASME J Dyn Syst Meas Control 135:064502.1–064502.7 Gutiérrez-Giles A, Arteaga-Pérez MA (2014) GPI based velocity/force observer design for robot manipulators. ISA Trans 53(4):929–938 Rivera-Dueñas JC, Arteaga-Pérez MA (2013) Robot force control without dynamic model: theory and experiments. Robotica 31:149–171 Siciliano B, Villani L (1999) Robot force control. Kluwer Academic Publishers, The Netherlands Slotine JJE, Li W (1991) Applied nonlinear control. Prentice-Hall, Englewood Cliffs, NJ

Chapter 9

Bilateral Teleoperation

Abstract Bilateral local-remote, or master-slave, teleoperation systems make a combination of human skills with the benefits of precise, repetitive and cost-effective robotic manipulation. Telepresence, task performance and transparency should be optimized. A good and robust performance are ideal characteristics because the human operator may use the bilateral system without any special training. The presence of time varying delays makes the achievement of these objectives not possible and some new performance parameters should be introduced.

9.1 Fundamental Concepts of Bilateral Manipulators Systems Figure 9.1 shows the five basic elements of a bilateral teleoperation system. The first component is a human being, the operator, who directly interacts with the second element, the local or master manipulator. The third component is a communication channel for the exchange of different signals, among other positions, velocities, forces, video, etc. The fourth element is the remote or slave robot which interacts with the fifth component, the environment. It is not difficult to get a dynamic model for the system depicted in Fig. 9.1 based on model (3.44), where the notation local (l) and remote (r) is preferred over the common one of master-slave. It is assumed that both manipulators have n DOF and the same kinematic configuration, although a scale factor might just be necessary if they have different sizes. H l (ql )¨ql + C l (ql , q˙ l )˙ql + Dl q˙ l + gl (ql ) = τ l − τ h H r (qr )¨qr + C r (qr , q˙ r )˙qr + Dr q˙ r + gr (qr ) = τ r + τ e .

(9.1) (9.2)

τ h ∈ Rn represents the torque applied by the human operator to the local robot and τ e ∈ Rn is the environment interaction. Figure 9.1 also shows a particular case, i.e. when time varying delays are present in the communication channel. Note that not necessarily all the signals shown in the © The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control of Robot Manipulators, Lecture Notes in Electrical Engineering 798, https://doi.org/10.1007/978-3-030-85980-0_9

257

258

9 Bilateral Teleoperation

Fig. 9.1 Components of a bilateral teleoperation system

figure are usually transferred. For instance, velocities are most likely not available. This does not only make the control, and perhaps the observer, design more complex because its stability is difficult to be proven analytically, but it makes less clear what are the main objectives to pursue. And that for a good reason. The usual concepts of telepresence and transparency are defined for systems without communication delays. The former is rather a subjective objective that means that the operator has the feeling of being in the remote environment, while the latter implies that the physical medium between he/she and the environment does not impose any dynamics, i.e. that local and remote robots dynamics are cancelled out. The concept of transparency is not only fundamental, but also objectively quantifiable in two usual ways. The first one is the following: Criterion Transparency based on impedance 1. The impedance felt by the operator, say Zh , must be equal to the task impedance, say Ze , i.e. Zh ≡ Ze , where the following relationships hold τ h (s) = Zh (˙ql (s)) and τ e (s) = Ze (˙qr (s)) as well.  2. Velocities must be equal, i.e. q˙ l (t) = q˙ r (t).

9.1 Fundamental Concepts of Bilateral Manipulators Systems

259

A second possibility is to define three ideal responses for kinematic correspondence regardless the operator and environment dynamics: Criterion Transparency based on kinematic correspondence 1. ql (t) = qr (t). 2. τ h (t) = τ e (t). 3. Both ql (t) = qr (t) and τ h (t) = τ e (t) hold.



Extending the concepts of telepresence and specially that of transparency for systems with communications delays is not a direct task and there is actually no general consensus regarding this particular issue. However, in this chapter an attempt to introduce an objective criterion based on the concept of delayed kinematic correspondence and on the second criterion for systems without time delays is made. In fact, it would be straightforward to define: 1. qi (t) = q j (t − T j (t)). 2. τ i (t) = τ j (t − T j (t)). 3. Both qi (t) = q j (t − T j (t)) and τ j (t) = τ i (t − Ti (t)) hold. Certainly, if i = l, then j = r and vice versa. Also, τ l = τ h and τ r = τ e . Both notations are used throughout the chapter and this should not be a source of confusion. The previous criterion makes evident that it is not possible in general to get ql (t) = qr (t − Tr (t)) and vice versa, i.e. qr (t) = ql (t − Tl (t)), simultaneously. Indeed, let’s choose the desired trajectory for a robot as the delayed one of the other, i.e. qdi (t) = q j (t − T j (t)),

(9.3)

and define as usual the tracking error as ei = qi − qdi .

(9.4)

Assume now that, somehow, it is possible to get ei ≡ 0 in a finite time. Then, according to (9.3) and (9.4) one must have qi (t) = q j (t − T j (t)) = qi (t − T j (t) − Ti (t)).

(9.5)

This relationship must be valid despite any human torque τ h and any environmental force τ e , which allows only two possibilities: either qi (t) = q j (t) = qc for some constant vector, perhaps satisfying a constraint on the remote side, or a synchronized movement arises when the sum of the time delays is constant. Therefore, it turns out that with this particular tracking errors definitions, making them exactly zero prevents the human operator to cause any desired motion on the local side other than constant regulation. The same can be said for forces. Therefore, one of the two possibilities should be chosen in advanced. When the remote manipulator is in free motion it makes just sense to choose i = r and j = l, but if it is in constrained motion then it makes more sense to choose i = l and j = r. The reason is that for the first case the

260

9 Bilateral Teleoperation

corresponding choice implies that when the remote manipulator is in free motion it should track the (delayed) trajectory commanded by the human operator at the local side. In contrast, for the second situation the corresponding choice implies that the remote surface is being reconstructed at the local side for the human operator. But regarding the force applied by the remote robot to the environment, it is important that this be commanded by the human operator, which implies that one should choose i = r and j = l. This choice appears to be the best because even if the operator does not know when the remote robot will apply the desired force, he/she knows it simply because he/she is commanding it. This can be summarized as: Criterion Delayed kinematic correspondence 1. When τ h (t) = τ e (t) = 0, it should be achieved that ql (t) = qr (t − Tr (t)) and qr (t) = ql (t − Tl (t)), meaning that either a consensus arises reaching a constant equal position or a synchronized periodic movement takes place. This corresponds to the free motion case. 2. For free movement of the remote manipulator, i.e. τ h (t) = 0 but τ e (t) = 0, then it should be achieved qr (t) = ql (t − Tl (t)), meaning that the remote manipulator should track the delayed position of the local one. 3. For constrained motion, i.e. τ h (t) = 0 and τ e (t) = 0, then one of the three ideal responses should be achieved: a. ql (t) = qr (t − Tr (t)) b. Fe (t) = Fh (t − Tl (t)) c. Both ql (t) = qr (t − Tl (t)) and Fe (t) = Fh (t − Tl (t)) hold, where relationship (3.64) has been used, i.e. τ i = J iT (qi )Fi ,

(9.6)

with J i (qi ) ∈ Rn×n the robot’s geometric Jacobian. 

9.2 Control and Observer Design Note that the term transparency is excluded in the previous criterion on purpose, but when Tl (t) = Tr (t) ≡ 0 then Criterion 9.1 is recovered. With this in mind the design of a control-observer scheme for a system like that depicted in Fig. 9.1 is introduced. First of all, note that under definition (9.3) the derivative of the desired trajectory is given by   q˙ di (t) = 1 − T˙ j (t) q˙ j (t − T j (t)).

(9.7)

9.2 Control and Observer Design

261

Unless programmed, it is not possible to known T˙ j (t). Assume also that no velocities measurements are available, so that q˙ j is not known for j = r, l. For that reason an observer is necessary to obtain an estimate q˙ˆ j ; but even with that estimate q˙ di (t) would not be available, so that it can be used qˆ˙ di = q˙ˆ j (t − T j (t))

(9.8)

as a substitution of the not available q˙ di , where as usual if i = l, then j = r and vice versa. Based on the control-observer scheme of Section 6.3, the following velocity observer is proposed ξ˙ i = zi

(9.9)

ζ i = qˆ˙ di − xi ei + K di zi ξ i q˙ˆ = ζ + zi zi + K di zi , i

i

(9.10) (9.11)

where zi , xi , K di ∈ Rn×n are positive definite diagonal matrices, with the usual observer error zi = qi − qˆ i ,

(9.12)

where qˆ i is obtained by computing the integral of q˙ˆ i in (9.11). The associated control scheme requires of the following definitions si = q˙ˆ i − qˆ˙ di + xi ei d σ i = K βi si + sign(si ) dt q˙ oi = q˙ˆ i − zi zi q˙ ri = q˙ˆ di − xi ei − K γ i σ i soi = q˙ oi − q˙ ri ,

(9.13) (9.14) (9.15) (9.16) (9.17)

where K βi , K γ i ∈ Rn×n are positive definite diagonal matrices and sign(si ) = [sign(si1 ), . . . , sign(sin )]T with si j element of si for j = 1, . . . , n. When the remote robot is in free motion, according to (6.82) the input torques should be given by τ l = −K al q˙ˆ l − K pl sol τ r = −K ar qˆ˙ − K pr sor r

(9.18) (9.19)

respectively, where K al , K ar , K pr , K f l ∈ Rn×n are positive definite diagonal matrices. Note that the main difference with (6.82) is that some damping is added. The reason is that when the operator drops the local end-effector then the robots should stop its movement. On the other hand, if the remote robot gets in touch with a surface then it

262

9 Bilateral Teleoperation

is more advisable to include some force feedback so that a force tracking error can be defined as Fi = Fi − Fdi ≡ Fi − F j (t − T j (t)).

(9.20)

A word of caution has to be made. Usually, in practical implementations the human operator pulls the local end-effector while the remote robot pushes on the environment. This way, the force sign in the measurements may be the opposite and this has to be taken into account if necessary. The corresponding integral represents the linear and angular momenta errors as t pi =

Fi dt,

(9.21)

0

with Fe or Fr for i = r and Fh or Fl for i = l employed indistinctly, and as usual if i = l then j = r and vice versa. Instead of (9.18)–(9.19), consider the following control laws τ l = −K al q˙ˆ l − K pl sol + J Tl (ql )(Fdh − K f l pl ) τ r = −K ar q˙ˆ − K pr sor − J T (q )(Fde − K fr p ), r

r

r

r

(9.22) (9.23)

where K fi ∈ Rn×n are positive definite diagonal matrices. One of the main characteristics of the previous approach is its simplicity. In fact, as made clear by Eq. (6.84), at heart there is a PID controller for each robot for position tracking, and from (9.22)– (9.23) it is also clear that one has a PI controller for force tracking. Another key characteristic is that they are the same both for the local and the remote manipulator. This is one of the reasons why this notation is preferred over the more usual of master-slave, because any of the robots can be moved by a human operator while the other will try to follow the command given on the other side. Therefore, even if no force feedback is available, the human operator will have some sense of telepresence, so that at least Criterion 9.3.1 and Criterion 9.3.3a can be achieved. To show this, assume that that is precisely the case and compute the error dynamics of system (9.1)–(9.2) in closed loop with observer (9.9)–(9.11) and the control laws (9.18) and (9.19). Define for i = l, r sqi = q˙ i − q˙ ri ri = q˙ i − q˙ oi = z˙i + zi zi .

(9.24) (9.25)

Now, consider system (9.1) in closed loop with the control law (9.18), and (9.2) in closed loop with (9.19). By using (9.17), (9.24), (9.25) and by taking into account that soi = q˙ oi − q˙ ri = sqi − ri it can be obtained H i s˙qi + C i sqi + K vi sqi = K ai z˙i + K pi ri + yai − gi + τ pi ,

(9.26)

9.2 Control and Observer Design

263

  where τ pl = −τ h , τ pr = τ e , yai = − H i q¨ ri + C i q˙ ri + Di q˙ ri + K ai q˙ ri , and K vi = Di + K ai + K pi . For the observers, from (9.10)–(9.11) one gets q˙ˆ i = qˆ˙ di − xi ei + K di zi ξ i + zi zi + K di zi ,

(9.27)

q˙ˆ i = q˙ ri + K γ i σ i + K di zi ξ i + zi zi + K di zi .

(9.28)

and from (9.16)

By taking into account (9.24) sqi = z˙i + K γ i σ i + K di zi ξ i + zi zi + K di zi .

(9.29)

By computing the derivative of this expression and by using (9.9) and (9.25) one gets the observer closed loop dynamics as r˙i + K di ri = s˙qi − K γ i

d σi. dt

(9.30)

Define the error state for the closed loop dynamics (9.26) and (9.30) as ⎤ sql ⎢ sqr ⎥ ⎥ x=⎢ ⎣ rl ⎦ . rr ⎡

(9.31)

Furthermore, define as usual a region of interest of the form (4.28)  D = x ∈ R4n |x <  , for some 0 <  < ∞. It can be shown in the very same fashion as done in Section 6.3 that whenever x is bounded by  (i.e. x ∈ D), then any other signal related to the closed loop system dynamics is bounded, as long as the following assumption is made. Assumption 9.1 The human and environment torques as well as their derivatives are bounded for all time, i.e. there exist positive constants bh , bdh , be and bde such that Fh  ≤ bh < ∞,  dtd Fh  ≤ bdh < ∞, Fe  ≤ be < ∞, and  dtd Fe  ≤ bde < ∞ ∀ t ≥ t0 . Furthermore, the robot manipulators do not reach any singularity so that the  inverse matrices J i−1 (qi ) always exist. With this in mind, the following procedure is completely similar to that given in Section 6.3: 1. The first step is to show how to set gains to make x(t) bounded for all time, i.e. x ∈ D ∀ t ≥ t0 , as long as the initial condition x(t0 ) is also contained in D. In order to do this, consider

264

9 Bilateral Teleoperation

V (x) =

1 T x Mx, 2

(9.32)

with M = block diag {H l (ql ), H r (qr ), I, I}, which satisfies (4.29) after Property 3.7, i.e. α1 (x) = λ1 x2 ≤ V (x) ≤ α2 (x) = λ2 x2 ,

(9.33)

with λ1 = 21 λmin (M) and λ2 = 21 λmax (M). By taking into account Property 3.2, the derivative of V (x) along (9.26) and (9.30) is given by   V˙ (x) = − sTql K vl sql + sTql K al z˙ l + K pl rl + yal − gl − τ h   − sTqr K vr sqr + sTqr K ar z˙ r + K pr rr + yar − gr + τ e

 d T T − rl K dl rl + rl s˙ql − K γ l σ l dt

 d T T − rr K dr rr + rr s˙qr − K γ r σ r dt ≤ − λmin (K vl )sql 2 − λmin (K vr )sqr 2   + sql  λmax (K al )˙zl  + λmax (K pl )rl  + sql yal − gl − τ h    + sqr  λmax (K ar )˙zr  + λmax (K pr )rr  + sql yar − gr + τ e     d   ˙ s σ − K − λmin (K dl )rl 2 + rl   γl l  ql dt     d   ˙ s − λmin (K dr )rr 2 + rr   σ − K γr r .  qr dt 

(9.34)

Since the stability analysis is valid only in D, there must exist positive constants such that αl = max yal 

(9.35)

αr = max yar  ∀x∈D    d   ˙ s al = max  σ − K ql γl l ∀x∈D  dt     d   ˙ s ar = max  σ − K qr γr r . ∀x∈D  dt 

(9.36)

zi  ≤ z max i ,

(9.39)

∀x∈D

(9.37) (9.38)

Also, in D it is satisfied

9.2 Control and Observer Design

265

for some positive constant z max i , while directly from (9.25) one gets ˙zi  ≤ λmax (zi )z imax + ri .

(9.40)

Then, by taking into account (9.35)–(9.40), Property 3.11 and Assumption 9.1, it is possible to rewrite V˙ (x) in (9.34) as V˙ (x) ≤ − λmin (K vl )sql 2 + βl sql rl  − λmin (K dl )rl 2 − λmin (K vr )sqr 2 + βr sqr rr  − λmin (K dr )rr 2 + βx,

(9.41)

where βl , βr and β are defined as   βl = λmax (K al ) + λmax (K pl )   βr = λmax (K ar ) + λmax (K pr )

(9.42) (9.43)

β =αl + αr + al + ar + kgl + kgr + bh + be + λmax (K al )λmax (zl )z max l + λmax (K ar )λmax (zr )z max r .

(9.44)

Suppose gains are chosen to satisfy λmin (K vl ) ≥ 1 + 2δ

(9.45)

λmin (K vr ) ≥ 1 + 2δ

(9.46)

λmin (K dl ) ≥

βl2

+ 2δ 4 β2 λmin (K dr ) ≥ r + 2δ, 4

(9.47) (9.48)

where δ is a positive constant. Then, it is straightforward to get V˙ (x) ≤ −2δx2 + βx = −δx2 − x(δx − β).

(9.49)

Define μ=

β . δ

(9.50)

Then, whenever x ≥ μ one has V˙ (x) ≤ −δx2 = −W3 (x).

(9.51)

By taking into account (9.33), conditions (4.29) and (4.53) of Theorem 4.5 are fulfilled in D. According to the same theorem, the initial condition must comply with (4.56), i.e.

266

9 Bilateral Teleoperation

 x(t0 ) ≤

λ1 , λ2

and after (4.73), the final state will be ultimately bounded by  x(t) ≤

λ2 μ = bf . λ1

(9.52)

 Note that one must enforce μ < λλ21  by setting gains large enough, but it is not necessary to make the ultimate bound bf arbitrarily small. This proves that all tracking and observation errors remain bounded for all time. 2. Once it has been shown that x is bounded (and thus every single error), the next step is to show that the observation errors tend to zero. Consider (9.16) and (9.24) to get sqi = q˙ i − qˆ˙ di + xi ei + K γ i σ i , and after (9.13) si + K γ i σ i = sqi − z˙i .

(9.53)

By computing its derivative, it is after (9.14) and (9.53)   s˙i = −K γ i K βi si + sign(si ) + s˙qi − z¨i .

(9.54)

By using this equation and the fact that s˙qi − z¨i is bounded in D, it can be shown that si ≡ 0 in a finite time tr ≥ 0 as shown in Section 6.3. Then, take into account (9.9)–(9.13) to get si = (zi + K di ) zi + K di zi ξ i .

(9.55)

Once si ≡ 0 one can compute (zi + K di ) z˙i + K di zi zi = 0,

(9.56)

which represents a stable linear filter for zi with input zero, so that zi , z˙i → 0. 3. While the observations errors tend to zero, the tracking errors can only be shown to be bounded in a finite time. To prove this, recall that si = 0 in a finite time tr ≥ t0 , so that from (9.8) and (9.13) one has q˙ i − q˙ j (t − T j (t)) + xi ei = z˙i − z˙ j (t − T j (t)).

(9.57)

Consider now that from (9.7) one has e˙ i + xi ei = z˙i − z˙ j (t − T j (t)) + T˙ j (t)˙q j (t − T j (t)),

(9.58)

9.2 Control and Observer Design

267

which is a stable linear filter of the form e˙ i = −xi ei + ui ,

(9.59)

ui = z˙i − z˙ j (t − T j (t)) + T˙ j (t)˙q j (t − T j (t)).

(9.60)

with

At this point it is necessary to make the following assumption. Assumption 9.2 The time delays derivatives are bounded by Ti∗ , i.e. 0 ≤ |T˙i (t)| ≤  Ti∗ < ∞, for i = l, r. Note that Assumption 9.2 requires the time delays derivatives to be bounded, but not the delays themselves. This implies that ui must be bounded for all t ≥ t0 since it has been shown that x ∈ D for all t ≥ t0 . Since system (9.59) is time invariant it must hold e(t−t0 )xi  ≤ ki e−λi (t−t0 ) ,

(9.61)

and the state, i.e. ei , is bounded by ei  ≤ ki e−λi (t−t0 ) ei (t0 ) +

ki sup ui (ϑ), λi 0≤ϑ≤t

(9.62)

for some positive constants ki and λi . Clearly, the first term will vanish as t → ∞, but it is necessary to show that λkii can be made arbitrarily small. To simplify the discussion, suppose that the eigenvalues of xi are chosen all real and distinct, satisfying λin < λi(n−1) < · · · < λi1 < 0.

(9.63)

Then, the right eigenvectors d i j with j = 1, . . . , n satisfy xi d i j = λi j d i j ,

(9.64)

while for the corresponding left eigenvector ηi j with j = 1, . . . , n it holds ηiTj xi = λi j ηiTj .

(9.65)

Note that it is assumed that d i j  = ηi j  = 1 ∀ i, j = 1, . . . , n and that it holds T ηik d il = δkl ,

(9.66)

for k, l = 1, . . . , n, where δkl is the Kronecker symbol satisfying δkl = 0 if k = l and δkk = 1. Under these conditions, it can be shown that

268

9 Bilateral Teleoperation

e(t−t0 )xi =

n 

eλi j (t−t0 ) d i j ηiTj .

(9.67)

j=1

By taking norms one has e(t−t0 )xi  ≤

n 

eλi j (t−t0 ) d i j ηi j  =

n 

eλi j (t−t0 ) ,

(9.68)

eλi1 (t−t0 ) = neλi1 (t−t0 ) = ne−λi (t−t0 ) ,

(9.69)

j=1

j=1

and from (9.63) it is e(t−t0 )xi  ≤

n  j=1

with λi = |λi1 |. By setting ki = n one gets (9.61). Recalling that every variable is bounded in D and taking into account Assumption 9.2, there must exist a positive constant u max i so that ui  ≤ u max i < ∞ ∀ t ≥ t0 . Since e−λi t → 0, then for a large enough time the ultimate bound for ei  will satisfy according to (9.62) ei (t) ≤

ki u max i = δmax i . λi

(9.70)

Since λi = min |λ(xi )|, by choosing λi large enough then δmax i becomes arbitrarily small for i =l, r. 4. Suppose that τ h = τ e = 0, then all tracking errors tend to zero and consensus is achieved, i.e. ql (t) = qr (t) = qc for some constant qc . This behavior corresponds to Criterion 9.3.1 and to prove it assume that the bounds δmax i in (9.70) are chosen small enough to make the tracking error negligible, i.e. el ≈ 0 and er ≈ 0, so that according to (9.3) and (9.4) the following holds qi (t) ≈ q j (t − T j (t)) ≈ qi (t − Ti (t) − T j (t)),

(9.71)

which means that qi (t) ≈ qi (t − Ti (t) − T j (t)).

(9.72)

Equation (9.72) is valid for any unrelated variable time delays Ti (t) and T j (t) and for an arbitrarily small tracking error, which in general can only be satisfied if and only if qi (t) ≡ qc for some constant vector qc . This also implies that q˙ i (t) → 0 so that the input of the filter in (9.58) becomes zero even if time delays are variable, meaning that one has e˙ i , ei → 0 exactly, not only approximately. For that reason one must have qr (t) = ql (t) ≡ qc , i.e. consensus is achieved. Of course, it is worthy to mention that the robots can also have a synchronized movement in the sense that they track each other with a periodic behavior if the round delay

9.2 Control and Observer Design

269

is constant, i.e. the sum Ti (t) + T j (t) is constant. The reason is that (9.72) can also be fulfilled if the manipulators track each other with a periodic behavior. This, however, depends on the particular characteristics of the time delays and it is a behavior that cannot be induced by gain tuning. Note that consensus is guaranteed for τ h = τ e = 0 without needing the exact knowledge of the gravity vector, which represents an advantage over previous approaches. 5. Suppose that τ h = 0 but τ e = 0 but one still has el  ≈ 0. For that case the human operator will not be able to move the local manipulator and the previous item 4 will still be valid because (9.70) holds regardless τ h and τ e , i.e. regardless the values of bh and be , although according to (9.45)–(9.48), δ has to be chosen larger to keep the same value of μ in (9.50) as for the case when τ h = τ e = 0. Thus, el  ≈ 0 means that the human operator will be treated as a perturbation and rejected by the controller, so that he/she will not be able to move the local endeffector according with (9.72). In order to avoid this undesirable effect el  ≈ 0 should not hold so that the human operator be able to move the local end-effector, while the remote tracking error er  can as before be made arbitrarily small. This corresponds to Criterion 9.3.2. 6. Suppose that τ h = 0 and τ e = 0 and assume that el  ≈ 0 does not hold, so that the human operator is able to move the local end-effector. Then, once the movement of the remote robot is constrained, he/she will undergo delayed kinematic correspondence in the sense that he/she will no longer be able to freely move the local end-effector in the direction the remote manipulator is applying the contact force and the remote contact surface will locally be reproduced with an error no larger than δmax l . This is true because the value of el  cannot be larger than δmaxl in (9.70), meaning that the remote surface is being reproduced with an error no larger than δmax l . This corresponds to Criterion 9.3.3a. 7. Item 6 makes clear that force feedback is mandatory to achieve Criterion 9.3.3c. To show this, under the control laws (9.22)–(9.23), the closed loop dynamics is once again given by (9.26), but this time with τ pi = ∓J iT (qi )(Fi + K fi pi ),

(9.73)

where the − sign is for i = l and the + sign is for i = r. Note that (9.6) has been used to get (9.73). Thus, should τ pl and τ pr be bounded, then exactly the very same conclusions can be drawn as before, i.e. items 1–6 hold. However, while Assumption 9.1 guarantees the boundedness of Fi , that of pi cannot be assumed and it has to be enforced by gain tuning. To do so, it is convenient to rewrite (9.26) as H i s˙pi + C i spi + K vi spi = K ai z˙i + K pi ri + ybi − gi ,

(9.74)

  −1 T spi = sqi − K −1 vi τ pi = sqi ± K vi J i (qi ) Fi + K fi pi ,

(9.75)

where

270

9 Bilateral Teleoperation

and   ˙ ri + K ai q˙ ri . qri + K −1 ybi = − H i (¨qri + K −1 vi τ˙ pi ) + C i (˙ vi τ pi ) + Di q (9.76) Now, change the definition given in (9.31) by ⎤ spl ⎢ spr ⎥ ⎥ x=⎢ ⎣ rl ⎦ . rr ⎡

(9.77)

Clearly, the positive definite function V (x) in (9.32) can be employed once again, with a proper new region D. Note that new but fully equivalent equations of the form (9.33) to (9.52) can be gotten as long as pi is bounded because d Fi is bounded by Assumptions 9.1 and 9.2. Certainly, the values of the dt constants in (9.35)–(9.38) and (9.42)–(9.44) would be different, but that remains a minor detail since ultimate boundedness of the system trajectories could still be guaranteed by gain tuning just as before. Therefore, only the boundedness of pi needs to be proven. Recall that by Assumption 9.1 the inverse J i−T (qi ) always exists. Then, to ensure the boundedness of pi , eq. (9.75) can be rewritten as   Fi = −K fi pi ∓ J i−T (qi )K vi spi − sqi

(9.78)

to allow to describe the system’s closed loop dynamics by (9.30), (9.74) and (9.78) and to define a new closed loop state as ⎤ spl ⎢ spr ⎥ ⎥  ⎢  ⎢ rl ⎥ x ⎥ ⎢ y=⎢ ⎥ = p . ⎢ rr ⎥ ⎣ pl ⎦ pr ⎡

(9.79)

As done before for the original state x, a region of interest  Dy = y ∈ R6n |y <  , for some 0 <  < ∞ can be defined for the state y, so that the boundedness of every signal of interest is guaranteed in Dy as before. The usefulness of (9.30), (9.74) and (9.78) is that being pi part of the state, then conditions can be found to enforce the boundedness of y by using Theorem 4.5. The main goal is to obtain an equation of the form (9.49) but for the state y. As a matter of fact, as said before and without any lost of generality V (x) in (9.32) can be used to arrive exactly to (9.49) (in terms of the new x) just by properly changing the

9.2 Control and Observer Design

271

constants definitions in (9.35)–(9.38) and (9.42)–(9.44) for the new region Dy and of course making the corresponding change of notation for the elements of x defined in (9.77). For simplicity’s sake the same notation of the different constants β is kept. On the other hand, define Vp (p) =

1 T 1 1 p p = pTl pl + pTr pr , 2 2 2

(9.80)

and Vy (y) = V (x) + Vp (p) =

1 T y Ny, 2

(9.81)

where N = block diag {H l (ql ), H r (qr ), I, I, I, I}. As before, it satisfies condition (4.29) after Property 3.7 with α1 (y) = λy1 y2 , and α2 (y) = λy2 y2 for λy1 = 21 λmin (N) and λy2 = 21 λmax (N). Then, by direct computation it can be shown that V˙y (y) ≤ −δy2 holds for some δ > 0 by setting gains large enough whenever y ≥ μ with μ arbitrary small. This allows to employ once again Theorem 4.5. Certainly, this proves that it is always possible to find a set of gains to keep p bounded. As previously discussed, just the boundedness of p ensures that items 1–6 still hold. From (9.78) it is possible to obtain d Fi = −K fi Fi ∓ ωi dt

(9.82)

d J −T (q )K s − s  bounded in D . Since (9.82) represents a with ωi = dt vi pi qi y i i stable linear filter, large values of K fi imply that the ultimate bound of Fi can be made arbitrarily small. However, as explained before for position tracking, due to the time delays this leads to a contradiction unless the human and contact forces tend to be constant and equal. In particular, an equation of the form (9.72) would arise but for human and environment forces. On the contrary, setting only K fr large enough will allow the remote manipulator to track the delayed force applied by the operator, which corresponds to Criterion 9.3.3c.

9.3 Experimental Results In this section, an experimental evaluation of the scheme developed in this chapter is presented. The constant and time-varying delay cases are considered. The experimental setup consists of two identical Geomagic Touch manipulators, equipped each one with an ATI Nano17 force sensor. The three-degree-of-freedom configuration described in Section 14.2.5 is employed, i.e. the last three joints are mechanically blocked.

272

9 Bilateral Teleoperation

Tuning guide The equations required to implement the bilateral teleoperation scheme presented in this chapter are the tracking error definitions (9.3) and (9.4), the relationship (9.6), the observer (9.9)–(9.12, the auxiliary variables (9.13)–(9.17), the force error definitions (9.20) and (9.21), and the control laws (9.22) and (9.23). The following suggestions can be followed to tune the controller. • At first, neglect the force control terms as well as the sliding mode ones, by setting K fi = K fi = K βi = K γ i = O, i = l, s. • Tune the observer gains, kd and z , satisfying (6.65)–(6.67) and (6.69)–(6.70), to minimize the observation error z. • Tune the remaining position tracking controllers as standard PD ones, with K pi as the derivative gain matrices and xi = K −1 pi K pxi , where K pxi are the proportional gain matrices. • Add damping by setting K al and K ar to obtain a stable teleoperation in presence of time-delays. Note that the larger these gains, the more stable will be the teleoperator, but at the expense of making it stiffer. • Increase the gains K γ i slowly to improve the tracking performance, but preventing the actuators to present chattering. • Increase the gains K βi slowly in the same manner. • Include the force control terms and increase K fl and K fr until obtaining the desired force tracking performance. Experiments The gains employed to evaluate the scheme of this chapter for both constant and time-varying delays, obtained by following the suggestions above are summarized in Table 9.1. Constant time-delay For this scenario, asymmetrical delays are artificially created for each robot, being Tl = 0.5 s for the local manipulator and Tr = 0.4 s for the remote one. The experiment starts in free motion for about 40 s, then enters in contact with a rigid surface, and finally returns to free motion again.

Table 9.1 Chosen gains for the bilateral teleoperation controller Gain Value K pl , K pr xl , xr K dl , K dr zl , zr K βl , K βr Kγ l, Kγ r K al , K ar K fl , K fr

0.02I 10I 100 10I 0.5I 4I 0.02I 0.001I

9.3 Experimental Results

273

Fig. 9.2 Constant time-delay, joint position tracking: ql (t − Tr (t)) (—), qr (– –)

The tracking position in joint coordinates for this case is shown in Fig. 9.2, whereas the corresponding tracking error is displayed in Fig. 9.3. A comparison between the joint velocities obtained through a numerical derivative and the ones estimated by the observer is presented in Fig. 9.4 for the local, and in Fig. 9.5 for the remote manipulator. The contact force along with the force reflected to the local operator is shown in Fig. 9.6, with their corresponding force tracking errors displayed in Fig. 9.7. Time-varying delay In this case, again, asymmetrical delays are artificially created for each robot, but time-varying. The time-delay for the local manipulator is created through a random function taking values in the interval Tl ∈ [0.45, 0.55] s, whereas the timedelay random function for the remote manipulator takes values in the interval Tr ∈ [0.65, 0.75] s. A plot of the mentioned time-varying delays is shown in Fig. 9.8. The experiment starts in free motion for about 30 s, then enters in contact with a rigid surface, and finally returns to free motion again.

274

9 Bilateral Teleoperation

Fig. 9.3 Constant time-delay, joint position tracking error

The tracking position in joint coordinates for this case is shown in Fig. 9.9, with their corresponding tracking errors displayed in Fig. 9.10. A comparison between the joint velocities obtained through a numerical derivative and the ones estimated by the observer is presented in Fig. 9.11 for the local, and in Fig. 9.12 for the remote manipulator. The contact force along with the force reflected to the local operator is shown in Fig. 9.13, with their corresponding force tracking errors displayed in Fig. 9.14. When comparing the graphics of the constant delay case with those of the timevarying delay, it can be seen that even the relatively low time-variation of 0.1 s, affects in a great manner the performance of the position tracking, particularly when interacting with the environment. Nevertheless, it can be also appreciated that the teleoperator remained stable during all the experiments.

9.3 Experimental Results

275

Fig. 9.4 Constant time-delay, joint velocity estimation for the local manipulator: observer (—), numerical (– –)

Fig. 9.5 Constant time-delay, joint velocity estimation for the remote manipulator: observer (—), numerical (– –)

276

9 Bilateral Teleoperation

Fig. 9.6 Constant time-delay, Force tracking: Fl (t − Tr (t)) (—), Fr (– –)

Fig. 9.7 Constant time-delay, Force tracking error

9.3 Experimental Results

Fig. 9.8 Variable time-delay, time delays: Tl (t) (up), Tr (t) (down)

Fig. 9.9 Variable time-delay, joint position tracking: ql (t − Tr (t)) (—), qr (– –)

277

278

9 Bilateral Teleoperation

Fig. 9.10 Variable time-delay, joint position tracking error.

Fig. 9.11 Variable time-delay, joint velocity estimation for the local manipulator: observer (—), numerical (– –)

9.3 Experimental Results

279

Fig. 9.12 Variable time-delay, joint velocity estimation for the remote manipulator: observer (—), numerical (– –)

Fig. 9.13 Variable time-delay, Force tracking: Fl (t − Tr (t)) (—), Fr (– –)

280

9 Bilateral Teleoperation

Fig. 9.14 Variable time-delay, Force tracking error

References Aldana CI, Romero E, Nuño E, Basañez L (2015) Pose consensus in networks of heterogeneous robots with variable time delays. Int J Robust Nonlinear Control 25(14):2279–2298 Arteaga-Pérez MA, Castillo-Sánchez AM, Parra-Vega V (2006) Cartesian control of robots without dynamic model and observer design. Automatica 42:473–480 Arteaga-Pérez MA, López M, Nuño E, Hernández-Ortiz O (2019) On the delayed kinematic correspondence with variable time delays for the control of the bilateral teleoperation of robots. Int J Control. https://doi.org/10.1080/00207179.2019.1707287 Arteaga-Pérez MA, Morales M, López M, Nuño E (2018) Observer design for the synchronization of bilateral delayed teleoperators. Eur J Control 43:20–32. https://doi.org/10.1016/j.ejcon.2018. 06.001 Callier FM, Desoer CA (1991) Linear system theory. Springer, New York Chopra N, Spong MW, Lozano R (2008) Synchronization of bilateral teleoperators with time delay. Automatica 44:2142–2148 Hashtrudi-Zaad K, Salcudean SE (2002) Transparency in time-delayed systems and the effect of local force feedback for transparent teleoperation. IEEE Trans Robot Autom 18(1):108–114 Khalil HK (2002) Nonlinear systems, 3rd edn. Prentice-Hall, Upper Saddle River Lawrence DA (1993) Stability and transparency in bilateral teleoperation. IEEE Trans Robot Autom 9(5):624–637

References

281

Nuño E, Basañez L, Ortega R (2011) Passivity-based control for bilateral teleoperation: a tutorial. Automatica 47(3):485–495 Passenberg C, Peer A, Buss M (2010) A survey of environment-, operator-, and task-adapted controllers for teleoperation systems. Mechatronics 20:787–801 Slawiñski E, Mut VA, Fiorini P, Salinas LR (2012) Quantitative absolute transparency for bilateral teleoperation of mobile robots. IEEE Trans Syst Man Cybern Part A Syst Humans 42(2):430–442 Yokokohji Y, Yoshikawa T (1994) Bilateral control of master-slave manipulators for ideal kinesthetic coupling: formulation and experiments. IEEE Trans Robot Autom 10(5):605–620

Chapter 10

Robot Networks

Abstract The consensus problem for networks of multiple agents consists in reaching an agreement between certain coordinates of interest using a distributed controller. For a robot network, there are multiple communication channels, each of them possibly with a time varying delay, which makes the stability analysis and the control design more complex. A common way to analyze the consensus problem for this case is by employing algebraic graphs. Usually, the graph is assumed to be undirected and connected. This chapter deals with the design of a control law with a velocity observer to solve the Leader Follower Consensus Problem and the Leaderless Consensus Problem in networks of fully actuated robots modeled as undirected weighted graphs.

10.1 Basic Characteristic of Robot Networks When three or more robot manipulators are interconnected instead of a bilateral system one has a robot network like that depicted in Fig. 10.1. The robots may or may not have the same kinematic configuration, but for simplicity’s sake it is assumed that all the manipulators of the network are similar. Just as for bilateral control, one of the main problems to be solved is that of consensus. For instance, it may be desirable that all the agents find a consensus at a given leader coordinates (Leader Follower Consensus Problem LFCP), or it may be only necessary that the agents agree at a certain coordinates value (Leaderless Consensus Problem LCP). In any case, to find a solution it is first necessary to establish some of the characteristics of the network. The interconnection of N manipulators can be modeled using graph theory. Note that a deep description of graph theory is out of the scope of this book and therefore strictly only the concepts necessary to solve the LFCP and the LCP are given. First of all each manipulator of the network is considered a node of the graph, i.e. there are N nodes. The ordered pairs of nodes are called edges. For an undirected graph the edge (i, j) denotes that agents i and j can obtain information from each other, roughly meaning that when two manipulators of the network have communication, then this is bilateral (in contrast to a directed graph). Besides the assumption that the graph is undirected, it is assumed that it is weighted, meaning that a weight is © The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control of Robot Manipulators, Lecture Notes in Electrical Engineering 798, https://doi.org/10.1007/978-3-030-85980-0_10

283

284

10 Robot Networks

Fig. 10.1 Components of a bilateral teleoperation system

associated to every edge in the graph. An undirected path is a sequence of edges of the form (i 1 , i 2 ), (i 2 , i 3 ), . . ., while the undirected graph is said to be connected if there is an undirected path between every pair of distinct nodes. Assumption 10.1 The network graph is undirected and connected.



Let n i denote the number of robots transmitting information to the i-th manipulator and Ni the set containing them. This can be described by N × N constants ai j for i, j = 1, . . . , N , so that ai j > 0 if j ∈ Ni and ai j = 0 otherwise. Furthermore, aii = 0. Then, the network interconnection can be described by using the so-called Laplacian matrix L ∈ R N ×N , whose elements are defined by

li j =

⎧ N ⎪ ⎪  aik i = j ⎨ ⎪ ⎪ ⎩

k=1

(10.1)

−ai j i = j

Assumption 10.1 ensures that the Laplacian matrix L is symmetric because ai j = a ji ⇒ li j = l ji . Also, it can be shown that all its eigenvalues are positive but one single zero eigenvalue, i.e. L is positive semidefinite. In fact, by the definition given in (10.1) any eigenvector associated to that single zero eigenvalue is given by α1 N ∈ R N with α any real value different from zero and 1 N a column vector filled with ones. Finally, the dynamics of the ith manipulator is given by (3.44), i.e. H i (qi )¨qi + C i (qi , q˙ i )˙qi + Di q˙ i + gi (qi ) = τ i , for i = 1, . . . , N .

(10.2)

10.2 Leaderless Consensus Problem (LCP)

285

10.2 Leaderless Consensus Problem (LCP) For the leaderless consensus problem (LCP) the control objective is that all robot positions reach a consensus with zero velocity, i.e. for all i = 1, . . . , N there exists qc ∈ Rn such that lim q˙ i (t) = 0.

lim qi (t) = qc and

t→∞

t→∞

(10.3)

One of the most common approaches to achieve this goal is to use a PD control law with damping inclusion and gravity cancellation of the form τ i = gi (qi ) − pi

N 

ai j (qi − q¯ j ) − di q˙ i

(10.4)

j=1

q¯ j = q j (t − T ji (t)),

(10.5)

where pi , di ∈ R are positive gains, for i, j = 1, . . . , N . However, if velocity measurements are not available a conceptually similar observer to that given in Section 6.2 is introduced here as q˙ˆ i = zi zi + K di zi + ξ i

(10.6) N 

ξ˙ i = K di zi zi − pi H i−1 (qi )

ai j (qi − q¯ j ),

(10.7)

j=1

where zi , K di ∈ Rn×n are positive definite and diagonal matrices and (ˆ·) is the estimated value of (·). The usual observation error definition (6.1) zi = qi − qˆ i , is employed. Note that the observer (10.6)–(10.7) requires the knowledge of the N inertia matrices, while the control law (10.4) becomes τ i = gi (qi ) − pi

N 

ai j (qi − q¯ j ) − di q˙ˆ i .

(10.8)

j=1

To compute the closed loop dynamics, from (10.2), (10.8) and (6.1) it is H i (qi )¨qi + C i (qi , q˙ i )˙qi + K vi q˙ i + pi

N  j=1

ai j (qi − q¯ j ) − di z˙i = 0, (10.9)

286

10 Robot Networks

where K vi = Di + di I.

(10.10)

As to the observer closed loop dynamics, once again a sliding variable of the form (6.45) is employed, i.e. ri = z˙i + zi zi .

(10.11)

Then, combine (10.6)–(10.7) to get q¨ˆ i = zi z˙i + K di z˙i + K di zi zi − pi H i−1 (qi )

N 

ai j (qi − q¯ j ),

(10.12)

j=1

and use r˙i = q¨ i − q¨ˆ i + zi z˙i to have q¨ i = r˙i + K di ri − pi H i−1 (qi )

N 

ai j (qi − q¯ j ),

(10.13)

j=1

Finally, use (10.9) to get  r˙i = −K di ri − H i−1 (qi ) C i (qi , q˙ i )˙qi + K vi q˙ i − di z˙i .

(10.14)

To show that the control objective (10.3) can be achieved with this control-observer scheme define the error ei j = qi − q j ,

(10.15)

for i, j = 1, . . . , N . Recall that ai j > 0 if and only if j ∈ N , so that ei j doesn’t exist if j ∈ / Ni . As it is the main proposal of this book, local stability is pursued, for which it is necessary to define the following error state ⎤ ei1 ⎢ .. ⎥ ⎢ . ⎥ ⎥ ⎢ ⎥ ⎢ xi = ⎢ ei N ⎥ , ⎢ q˙ ⎥ ⎢ i ⎥ ⎣ ri ⎦ zi ⎡

(10.16)

for each subsystem dynamics (10.9), (10.11) and (10.14) and where xi ∈ Rn(3+ni ) . Then consider also for each manipulator the following positive definite function

10.2 Leaderless Consensus Problem (LCP)

287

1 T 1 1 1 q˙ i H i (qi )˙qi + ai j qi − q j 2 + riT ri + γi ziT −1 zi zi , (10.17) 2 pi 4 j=1 2 2 N

Vi =

with γi > 0. As usual, by taking Property 3.7 into account, condition (4.29) holds for each Vi , i.e. αi1 (xi ) = λi1 xi 2 ≤ Vi ≤ λi2 xi 2 = αi2 (xi ),

(10.18)

with 

γi λhi 1 1 1 , , min ai j , λi1 = min 2 pi 4 j∈Ni 2 2 λmax (zi )

 (10.19)

and  γi λHi 1 1 1 , . λi2 = max , max ai j , 2 pi 4 j∈Ni 2 2 λmin (zi ) 

(10.20)

This way, for the whole robot network the following positive definite function can be used V (x) =

N 

Vi ,

(10.21)

i=1

with ⎤ x1 ⎢ ⎥ x = ⎣ ... ⎦ . xN ⎡

(10.22)

Define a domain of interest D as in (4.28), i.e.   D = x ∈ Rn(3N +n 1 +...+n N ) \x <  , for any  > 0. Now, by setting λ1 = min{λi1 } and λ2 = max{λi2 }, it is possible to get condition (4.29) for the whole system as well, i.e. α1 (x) = λ1 x2 ≤ V (x) ≤ λ2 x2 = α2 (x).

(10.23)

By using Property 3.2, the derivative of Vi in (10.17) along (10.9), (10.11) and (10.14) can directly be computed as

288

10 Robot Networks

⎛ ⎞ N  1 ai j (qi − q¯ j ) − di z˙i ⎠ V˙i = − q˙ iT ⎝K vi q˙ i + pi pi j=1 1 ai j (qi − q j )T (˙qi − q˙ j ) 2 j=1  ˙i − riT K di ri − riT H i−1 (qi ) C i (qi , q˙ i )˙qi + K vi q˙ i − di z˙i + γi ziT −1 zi z di di ≤ − ˙qi 2 + q˙ iT (−zi zi + ri ) − riT K dhi ri pi pi N N  1 − q˙ iT ai j (qi − q¯ j ) + ai j (qi − q j )T (˙qi − q˙ j ) 2 j=1 j=1  T −1 − ri H i (qi ) C i (qi , q˙ i )˙qi + K vi q˙ i + di zi zi − γi ziT zi + γi ziT −1 zi ri , (10.24) N

+

where K dhi = K di − di H i−1 (qi ),

(10.25)

and (10.10) has been used. Since t q˙ j (θ )dθ,

q j − q¯ j = q j − q j (t − T ji (t)) =

(10.26)

t−T ji (t)

one gets ⎛ q˙ iT

N 

ai j (qi − q¯ j ) = q˙ iT

j=1

N 

⎜ ai j ⎝qi − q j +

j=1

=

q˙ iT

N 

t

⎞ ⎟ q˙ j (θ )dθ ⎠

t−T ji (t)

ai j (qi − q j ) +

j=1

q˙ iT

N  j=1

t q˙ j (θ )dθ.

ai j

(10.27)

t−T ji (t)

Then one has N N  1 ai j (˙qi − q˙ j )T (qi − q j ) − q˙ iT ai j (qi − q j ) = 2 j=1

j=1

N  j=1

 ai j

 N 1 1 (˙qi − q˙ j )T − q˙ iT (qi − q j ) = − ai j (˙qi + q˙ j )T (qi − q j ). 2 2 j=1

(10.28)

10.2 Leaderless Consensus Problem (LCP)

289

Therefore, (10.24) can be rewritten as di di V˙i ≤ − ˙qi 2 + q˙ iT (−zi zi + ri ) − riT K dhi ri pi pi t  N N  1 T q˙ j (θ )dθ − − ai j q˙ i ai j (˙qi + q˙ j )T (qi − q j ) 2 j=1 j=1 t−T ji (t)

−

riT H i−1 (qi )

 C i (qi , q˙ i )˙qi + K vi q˙ i + di zi zi − γi ziT zi + γi ziT −1 zi ri . (10.29)

The next step consists in adding all the derivatives V˙i to get the derivative of V (x). However, prior to do it consider the following N N  

ai j (˙qi + q˙ j )T (qi − q j ) =

i=1 j=1 N N   i=1 j=1

  ai j q˙ iT qi − q˙ iT q j + q˙ Tj qi − q˙ Tj q j =

N     ai1 q˙ iT qi − q˙ iT q1 + q˙ T1 qi − q˙ T1 q1 + · · · + ai N q˙ iT qi − q˙ iT q N + q˙ TN qi − q˙ TN q N

=

i=1

  a11 q˙ T1 q1 − q˙ T1 q1 + q˙ T1 q1 − q˙ T1 q1 + · · · + a1N q˙ T1 q1 − q˙ T1 q N + q˙ TN q1 − q˙ TN q N + .. .

  a N 1 q˙ TN q N − q˙ TN q1 + q˙ T1 q N − q˙ T1 q1 + · · · + a N N q˙ TN q N − q˙ TN q N + q˙ TN q N − q˙ TN q N = 0.

The last result holds because ai j = a ji and aii = 0 for i, j = 1, . . . , N . Therefore, the derivative of V (t) in (10.21) satisfies V˙ ≤

N   di di − ˙qi 2 + q˙ iT (−zi zi + ri ) pi pi i=1

−

N  j=1

t ai j q˙ iT

q˙ j (θ )dθ − riT K dhi ri

t−T ji (t)

−riT H i−1 (qi )



 C i (qi , q˙ i )˙qi + K vi q˙ i + di zi zi − γi ziT zi + γi ziT −1 zi ri . (10.30)

Since the stability analysis is only intended to be valid in D, then from the state definitions (10.16) and (10.22) it holds ˙qi  < .

(10.31)

By taking into account K dhi = K di − di H i−1 (qi ), and Properties 3.7 and 3.9 one gets from (10.30)

290

10 Robot Networks

V˙ ≤ −

N   di di di − ˙qi 2 + λmax (zi )˙qi zi  + ˙qi ri  p p pi i i i=1 N 

t ai j q˙ iT

j=1

q˙ j (θ )dθ

t−T ji (t)

− λmin (K di )ri 2 +

di ri 2 λhi

kci  + λmax (K vi ) di ˙qi ri  + λmax (zi )zi ri  λhi λhi  γi zi ri  . − γi zi 2 + λmin (zi )

+

(10.32)

For simplicity define βi =

di kci  + λmax (K vi ) + , pi λhi

(10.33)

to rewrite V˙ as V˙ ≤

N   di di − ˙qi 2 + βi ˙qi ri  + λmax (zi )˙qi zi  pi pi i=1

−

N 

t ai j q˙ iT

j=1

! −

q˙ j (θ)dθ t−T ji (t)

λmin (K di ) −

! "  " di di γi λmax (zi ) + ri 2 + zi ri  − γi zi 2 . λhi λhi λmin (zi )

(10.34) Choose gains as di ≥ pi (2 + δ¯di ) ! "2 1 di λmax (zi ) + 1 γi ≥ 4 pi ! "2 β2 di 1 di γi λmin (K di ) ≥ + λmax (zi ) + + i + δri λhi 4 λhi λmin (zi ) 4

(10.35) (10.36) (10.37)

where δri and δ¯di are positive constants. With this choice it is quite direct to get V˙ ≤

⎧ N ⎪ ⎨  ⎪ i=1 ⎩

−δ¯di ˙qi 2 − δri ri 2 −

N  j=1

t ai j q˙ iT t−T ji (t)

q˙ j (θ )dθ

⎫ ⎪ ⎬ ⎪ ⎭

.

(10.38)

10.2 Leaderless Consensus Problem (LCP)

291

Despite it was possible to define as usual a region of interest D, it is however not possible to employ Theorem 4.5 as for some other schemes described in this book. Instead, it is preferable to use Lemma 4.3, for which it is necessary to make the following: Assumption 10.2 The information exchanged from the j-th robot to the i-th one is subject to a variable time delay T ji (t) with a known upper bound T ji , i.e. 0 ≤  T ji (t) ≤ T ji < ∞. Moreover, dtd T ji (t) is bounded. By taking into account Assumption 10.2, it is possible to integrate V˙ to get t t0

V˙ (σ )dσ = V (t) − V (t0 ) ⎧ & '⎫ 2 N ⎨ N ⎬   T α ji i ≤ ˙qi 22 + ai j ˙q j 22 , −δ¯di ˙qi 22 − δri ri 22 + ⎭ ⎩ 2 2αi i=1 j=1 (10.39)

where αi is any positive constant. Since evidently N N  

2

ai j

i=1 j=1

T ji 2αi

˙q j 22 =

N N  

2

a ji

i=1 j=1

T ij

˙qi 22 ,

2α j

(10.40)

then (10.39) becomes V (t) − V (t0 ) ≤

⎧ N ⎨  i=1

⎩

−δ¯di ˙qi 22 − δri ri 22 + ˙qi 22

N 

& ai j

j=1

2

'⎫ ⎬

αi + a ji . 2 2α j ⎭ T ij

(10.41) By choosing δ¯di according to δ¯di ≥ δdi +

N 

&

j=1

2

T ij αi ai j + a ji 2 2α j

' ,

(10.42)

where δdi is a positive constant, one gets V (t) +

N    δdi ˙qi 22 + δri ri 22 ≤ V (t0 ).

(10.43)

i=1

Since this relationship is valid only for x ∈ D, it is necessary to guarantee that x will never leave this region. After (10.23) and (10.43) one must have

292

10 Robot Networks

λ1 x2 ≤ V (t) ≤ V (t0 ) ≤ λ2 x(t0 )2 ,

(10.44)

which implies that x is bounded by ( x ≤

λ2 x(t0 ). λ1

(10.45)

Thus, in order to enforce x ∈ D for all time it should hold (

λ2 x(t0 ) <  λ1

( ⇒

x(t0 ) < 

λ1 . λ2

(10.46)

Satisfying (10.46) makes (10.43) valid for all time t ≥ t0 , which implies that • q˙ i , ri ∈ L2 • V (t) ∈ L∞

∀ i = 1, . . . , N

After (10.17), the latter fact ensures that • q˙ i , qi − q j , ri , zi ∈ L∞

∀ i, j = 1, . . . , N

From (10.11) one necessarily has • z˙i ∈ L∞

∀ i = 1, . . . , N

and consequently • q˙ˆ i ∈ L∞

∀ i = 1, . . . , N

and from (10.14) • r˙i ∈ L∞

∀ i = 1, . . . , N

because only revolute joints are considered for simplicity. Since ri ∈ L2 ∩ L∞ Lemma 4.4 can be employed to show that • ri → 0

∀ i = 1, . . . , N

But from (10.11) this proves that • zi , z˙i → 0, and qˆ i → qi , q˙ˆ i → q˙ i

∀ i = 1, . . . , N

On the other hand, from (10.5) it is qi − q¯ j = qi − q j (t − T ji (t)) = qi − q j + q j − q j (t − T ji (t)),

(10.47)

where it has been shown that qi − q j is bounded. As for the second term, Schwartz’s inequality, i.e.

10.2 Leaderless Consensus Problem (LCP)

t

293

⎡ t ⎤ 21 ⎡ t ⎤ 21   aT (θ )b(θ )dθ ≤ ⎣ a(θ )2 dθ ⎦ ⎣ b(θ )2 dθ ⎦ ,

t0

t0

(10.48)

t0

can be applied for any vector signals a, b. To do this consider ) )2 ) t ) ) ) ) ) 2 q j (t) − q j (t − T ji (t)) = ) q˙ j (θ )dθ ) , ) ) ) t−T ji (t) )

(10.49)

and set a = 1 and b = q˙ j to apply (10.48) to get t q j − q j (t − T ji (t)) ≤ T ji (t) 2

) ) )q˙ j (θ ))2 dθ

t−T ji (t)

t ≤ T ji

) ) )q˙ j (θ ))2 dθ ≤ T ji ˙q j 2 , 2

(10.50)

t0

where Assumption 10.2 and the fact that t

) ) )q˙ j (θ ))2 dθ ≤

t−T ji (t)

t

) ) )q˙ j (θ ))2 dθ

(10.51)

t0

have been taken into account. Since q˙ j ∈ L2 , it can be concluded that • qi − q¯ j ∈ L∞

∀ i, j = 1, . . . , N

In turn, after (10.9) and Property 3.7 it is • q¨ i ∈ L∞

∀ i = 1, . . . , N

By taking into account this last fact and in view that q˙ i ∈ L2 ∩ L∞ , Lemma 4.4 can be employed to show that • q˙ i → 0

∀ i = 1, . . . , N

This fact shows from (10.14) that • r˙i → 0

∀ i = 1, . . . , N

This in turn implies that • z¨i → 0

∀ i = 1, . . . , N

Now, by computing the derivative of (10.9)

294

H i (qi )

10 Robot Networks

d ˙ i (qi )¨qi + C˙ i (qi , q˙ i , q¨ i )˙qi + C i (qi , q˙ i )¨qi q¨ + H dt i N  + K vi q¨ i − di z¨i + pi ai j (˙qi − (1 − T˙ ji (t))˙q j (t − T ji (t))) = 0, j=1

(10.52) it is possible to show that dtd q¨ i must also be bounded ∀ i = 1, . . . , N since H i (qi ) is full rank and T˙ ji (t) is bounded by Assumption 10.2. Furthermore, one can conclude from Lemma 4.5 (Barbalat’s Lemma) that • q¨ i → 0

∀ i = 1, . . . , N

because ∞ q¨ i (θ )dθ = q˙ i (∞) − q˙ i (t0 ) = −˙qi (t0 ).

(10.53)

t0

Since q˙ i , z˙i → 0, the equilibrium point of (10.9) satisfies pi

N 

ai j (qi − q¯ j ) = 0.

(10.54)

j=1

The fact that velocities are zero at the equilibrium makes q¯ j = q j (t − T ji (t)) ≡ q j (t), and the following holds pi

N 

ai j (qi − q j ) = 0,

(10.55)

j=1

which can be rewritten as N 

ai j qi − ai1 q1 − · · · − aii qi − · · · − ai N q N = 0.

(10.56)

j=1

According with (10.1), li j = −ai j unless i = j, for which case aii = 0 but li j = N k=1 aik , which means that from (10.56) one gets li1 q1 + · · · + li N q N = 0, for i, j = 1, . . . , N . In a compact form one can write

(10.57)

10.2 Leaderless Consensus Problem (LCP)

295

⎤ q1 ⎢ ⎥ [li1 I n · · · li N I n ] ⎣ ... ⎦ = 0. qN ⎡

(10.58)

Define ⎡

⎤ q1 ⎢ ⎥ q = ⎣ ... ⎦ qN

(10.59)

to use the standard Kronecker product ⊗ as ⎤ l11 I n · · · l1N I n ⎢ .. ⎥ q = 0. (L ⊗ I n )q = ⎣ ... . ⎦ l N 1In · · · l N N In ⎡

(10.60)

Recall now that the Laplacian L matrix has almost only positive eigenvalues with the exception of one single zero eigenvalue and that any eigenvector associated to that single zero eigenvalue is given by α1 N ∈ R N where α is any real value different from zero and 1 N is a column vector filled with ones. This means that (L ⊗ I n )q = 0

(10.61)

q1 = · · · = q N ≡ qc ,

(10.62)

can hold if and only if

with qc a constant vector, precisely because L has one single zero eigenvalue associated to the eigenvector 1n . Leader follower consensus problem (LFCP) The constant vector qc in (10.62) can in fact be any constant vector. This represents a serious drawback in practical situations because physically a robot manipulator can only reach those positions in its work space. Therefore, it is more desirable that the network can be regulated at a given constant leader pose qle ∈ Rn . Of course, it makes little sense to give this information to all of the robots of the network, so that qle is only available to a certain set of them, i.e. instead of (10.3) one has to accomplish lim qi (t) = qle and

t→∞

lim q˙ i (t) = 0.

t→∞

(10.63)

It turns out that the control law (10.8) needs a minimal change to achieve this goal, since it has to be modified as

296

10 Robot Networks

τ i = gi (qi ) − pi

N 

ai j (qi − q¯ j ) − di q˙ˆ i − pi bli (qi − qle ),

(10.64)

j=1

where bli is positive for n l < N manipulators, while the rest of them are zero. This modification needs also to be included in the observer, so that (10.7) needs to be changed as ξ˙ i = K di zi zi − pi H i−1 (qi )

⎧ N ⎨ ⎩

j=1

⎫ ⎬

ai j (qi − q¯ j ) + bli (qi − qle ) . ⎭

(10.65)

To analyze the stability properties of the new scheme an extra error definition only for those robots where bli > 0 is given by ei = qi − qle ,

(10.66)

while at the same time the definition of xi in (10.16) has to be expanded as ⎤ ei ⎢ ei1 ⎥ ⎥ ⎢ ⎢ .. ⎥ ⎢ . ⎥ ⎥ ⎢ xi = ⎢ ei N ⎥ , ⎥ ⎢ ⎢ q˙ ⎥ ⎢ i ⎥ ⎣ ri ⎦ zi ⎡

(10.67)

while the definition of x in (10.22) is still valid. Finally, the positive definite functions Vi in (10.17) have also to be modified as 1 T bli 1 q˙ i H i (qi )˙qi + |qi − qle |2 + ai j qi − q j 2 2 pi 2 4 j=1 N

Vi = +

1 T 1 ri ri + γi ziT −1 zi zi . 2 2

(10.68)

Note that (10.18) still holds, but with 

bli 1 γi λhi 1 1 , min , λi1 = min , min ai j , j∈N 2 pi 4 2 2 2 λmax (zi ) i

 (10.69)

and  bli 1 γi λHi 1 1 , max , . λi2 = max , max ai j , 2 pi 4 j∈Ni 2 2 2 λmin (zi ) 

(10.70)

10.2 Leaderless Consensus Problem (LCP)

297

It is now possible to use Vi in (10.68) together with V (x) in (10.21) to arrive by direct computation to the very same conclusions as before, i.e. • q˙ i , ri , qi − q¯ j , q¨ i ∈ L∞ • q˙ i , ri , zi , z˙i → 0, r˙i → 0, z¨i → 0 • qi − qle ∈ L∞ Most important, this can be done under the same stability conditions (10.35)–(10.37) and (10.42) for i, j = 1, . . . , N . However, equation (10.9) changes to H i (qi )¨qi + C i (qi , q˙ i )˙qi + K vi q˙ i − di z˙i + pi

N 

ai j (qi − q¯ j ) + pi bli (qi − qle ) = 0

(10.71)

j=1

when necessary. As before, the computation of the derivative of this equation allows to arrive to the conclusion that • q¨ i → 0

∀ i = 1, . . . , N

As to the equilibrium point instead of (10.55) one has N 

ai j (qi − q j ) + bli (qi − qle ) = 0.

(10.72)

j=1

The second term can be expressed for ∀ i = 1, . . . , N as ⎡

⎤⎡ ⎤ O q1 − qle ⎥ .. .. ⎥ ⎢ ⎦. . . ⎦⎣ q N − qle O · · · blN I n

bl1 I n ⎢ .. ⎣ .

··· .. .

(10.73)

By defining B = diag (bl1 , . . . , blN ) with bli = 0 when necessary and using q in (10.59), the last equation can be rewritten using the Kronecker product as (B ⊗ I n )(q − (1 N ⊗ qle )).

(10.74)

Therefore, using this expression and (10.60) one can write (10.72) as (B ⊗ I n )(q − (1 N ⊗ qle )) + (L ⊗ I n )q = 0.

(10.75)

Recall as explained before that L1 N = 0, which also implies that (L ⊗ I n )(1 N ⊗ qle ) = 0, so that equivalently one has (B ⊗ I n )(q − (1 N ⊗ qle )) + (L ⊗ I n )q − (L ⊗ I n )(1 N ⊗ qle ) = 0, (10.76)

298

10 Robot Networks

or (Ll ⊗ I n )(q − (1 N ⊗ qle )) = 0,

(10.77)

by using the properties of the Kronecker product and by defining Ll = B + L.

(10.78)

But since L is a positive semidefinite matrix with one single zero eigenvalue, the addition of B is enough to make the matrix Ll symmetric and positive definite. This means that (10.77) can hold if and only if q − (1 N ⊗ qle ) = 0,

(10.79)

qi ≡ qle ,

(10.80)

which in turn means

for i = 1, . . . , N . This shows that (10.63) has been achieved.

10.3 Experimental Results Leaderless Consensus Problem Tuning guide To implement the LCP algorithm described in Sect. 10.2, one needs only the observer equations (10.6)-(10.7) and the control law (10.8). The following suggestion can be useful when implementing the controller. • Tune the observer gains, kd and z , satisfying (6.65)–(6.67) and (6.69)–(6.70), to minimize the observation error z. • Set the interconnection gains pi > 0. • Add damping by setting the di gains to satisfy (10.35). Note that the larger these gains, the more stable will be the robots’ motion, but at the expense of making them stiffer. Experiments Two experiments are carried out to test the LCP controller of Sect. 10.2, one with constant time-delays in the communication channel and the other one with variable time-delays. For the first case, different time-delays are considered for each robot, i.e. T12 = T13 = 0.45 s, T21 = T23 = 0.65 s and T31 = T32 = 0.85 s. In the time-varying delays case, there are considered T12 = T13 ∈ [0.4, 0.5] s, T21 = T23 ∈ [0.6, 0.7] s, and T31 = T32 ∈ [0.8, 0.9] s. A plot of the time delays is shown in Fig. 10.2. The interconnection constants are assumed to be ai j = 5, i = j. Each robot is manually drove to an arbitrary initial position, aided by a gravity compensation

10.3 Experimental Results

299

Fig. 10.2 Artificially implemented time-delays: robot 1 (up),robot 2 (middle), robot 3 (down) Table 10.1 Chosen gains for the LCP controller Gain Value p1 , p2 , p3 d1 , d2 , d3 K d1 , K d2 , K d3 z1 , z2 , z3

0.1 0.25 100I 0.1I

torque, before running the controller. The chosen gains for the implementation of the LCP controller for both constant and time-varying delays are shown in Table 10.1. Constant time-delay The time evolution of the joint positions for all three robots is displayed in Fig. 10.3. From this graphic, it can be seen that the robots reach a consensus position in steady state, after about 3 s. The joint velocities estimated by the observer along with the ones obtained by a derivative filter are shown in Figs. 10.4, 10.5 and 10.6, for robots 1,2, and 3, respectively.

300

10 Robot Networks

Fig. 10.3 LCP under constant time-delays, joint positions: robot 1 (—), robot 2 (—), robot 3 (—)

Fig. 10.4 LCP under constant time-delays, joint velocity estimation for robot 1: observer (—), numerical (– –)

10.3 Experimental Results

301

Fig. 10.5 LCP under constant time-delays, joint velocity estimation for robot 2: observer (—), numerical (– -)

Time-varying delay The time evolution of the joint positions for all three robots is shown in Fig. 10.7. As in the constant delay case, the robots reach a consensus position after about 3 s. The joint velocities estimated by the observer along with the ones obtained by a derivative filter are shown in Figs. 10.8, 10.9 and 10.10, for robots 1, 2, and 3, respectively.

10.3.1 Leader-Follower Consensus Problem Tuning guide The required equations for the implementation of the LFCP scheme of Sect. 10.2 are the observer equations (10.6) and (10.65), and the control law (10.64). To tune the controller, the following suggestions might be useful. • Tune the observer gains, kd and z , satisfying (6.65)–(6.67) and (6.69)–(6.70), to minimize the observation error z. • Set the interconnection gains pi > 0.

302

10 Robot Networks

Fig. 10.6 LCP under constant time-delays, joint velocity estimation for robot 3: observer (—), numerical (– –)

• Add damping by setting the di gains to satisfy (10.35). Note that the larger these gains, the more stable will be the robots’ motion, but at the expense of making them stiffer. Experiments In order to test the LFCP controller of Sect. 10.2, the same two scenarios as in the LCP problem are considered, i.e. constant and variable time-delays in the communication channel. For these case, the same delays of the LCP problem are considered, i.e. the ones depicted in Fig. 10.2. The interconnection constants are assumed to be ai j = 5, i = j. It is considered that only the robot 1 receives information from the leader, by setting bl1 = 15. The leader position is set to  T qle = −30◦ 45◦ −90◦ . Each robot is manually driven to an arbitrary initial position, aided by a gravity compensation torque, before running the controller. The chosen gains for the implementation of the LFCP controller for both constant and time-varying delays are shown in Table 10.2.

10.3 Experimental Results

303

Fig. 10.7 LCP, under variable time-delays, joint positions: robot 1 (—), robot 2 (—), robot 3 (—) Table 10.2 Chosen gains for the LFCP controller Gain Value p1 , p2 , p3 d1 , d2 , d3 K d1 , K d2 , K d3 z1 , z2 , z3

0.1 0.25 100I 0.1I

Constant time-delay The joint positions for all three robots are shown in Fig. 10.11. In this figure, it can be seen that all the three robot positions approach to the leader reference in steady state after about 3 s, though some oscillations are still present in joint 1 of robots 1 and 3. The joint velocities estimated by the observer along with the ones obtained by a derivative filter are shown in Figs. 10.12, 10.13, and 10.14, for robots 1,2, and 3, respectively.

304

10 Robot Networks

Fig. 10.8 LCP under variable time-delays, joint velocity estimation for robot 1: observer (—), numerical (– –)

Time-varying delay The time evolution of the joint positions for all three robots is shown in Fig. 10.15. As in the constant delay case, the robots approach to the leader position after about 3 s, with less oscillations than in the constant delay case. The joint velocities estimated by the observer along with the ones obtained by a derivative filter are shown in Figs. 10.16, 10.17, and 10.18, for robots 1,2, and 3, respectively.

10.3 Experimental Results

305

Fig. 10.9 LCP under variable time-delays, joint velocity estimation for robot 2: observer (—), numerical (– –)

Fig. 10.10 LCP under variable time-delays, joint velocity estimation for robot 3: observer (—), numerical (– –)

306

10 Robot Networks

Fig. 10.11 LFCP under constant time-delays, joint positions: robot 1 (—), robot 2 (—), robot 3 (—)

Fig. 10.12 LFCP under constant time-delays, joint velocity estimation for robot 1: observer (—), numerical (– –)

10.3 Experimental Results

307

Fig. 10.13 LFCP under constant time-delays, joint velocity estimation for robot 2: observer (—), numerical (– -)

Fig. 10.14 LFCP under constant time-delays, joint velocity estimation for robot 3: observer (—), numerical (– –)

308

10 Robot Networks

Fig. 10.15 LFCP, under variable time-delays, joint positions: robot 1 (—), robot 2 (—), robot 3 (—)

Fig. 10.16 LFCP under variable time-delays, joint velocity estimation for robot 1: observer (—), numerical (– –)

10.3 Experimental Results

309

Fig. 10.17 LFCP under variable time-delays, joint velocity estimation for robot 2: observer (—), numerical (– –)

400 200 0 -200 0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

100 0 -100 0 400 200 0 0

Fig. 10.18 LFCP under variable time-delays, joint velocity estimation for robot 3: observer (—), numerical (– –)

310

10 Robot Networks

References Abdessameud A, Tayebi A, Polushin IG (2016) Leader-follower synchronization of Euler-Lagrange systems with time-varying leader trajectory and constrained discrete-time communication. IEEE Trans Autom Control. https://doi.org/10.1109/TAC.2016.2602326 Aldana CI, Romero E, Nuño E, Basañez L (2015) Pose consensus in networks of heterogeneous robots with variable time delays. Int J Robust Nonlinear Control 25(14):2279–2298 Anderson RJ, Spong MW (1989) Bilateral control of teleoperators with time delay. IEEE Trans Autom Control 34(5):494–501 Arteaga-Pérez MA, Kelly R (2004) Robot control without velocity measurements: new theory and experimental results. IEEE Trans Robot Autom 20(2):297–308 Arteaga-Pérez MA, Nuño E (2018) Velocity observer design for the consensus in delayed robot networks. J Franklin Instit 355:6810–6829. https://doi.org/10.1016/j.jfranklin.2018.07.001 Nuño E (2016) Consensus of Euler-Lagrange systems using only position measurements’. IEEE Trans Control Network Syst 2620806:2016. https://doi.org/10.1109/TCNS Nuño E, Basañez L, Ortega R, Spong MW (2009) Position tracking for non-linear teleoperators with variable time delay. Int J Robot Res 28(7):895–910 Nuño E, Basañez L, Ortega R (2011) Passivity-based control for bilateral teleoperation: a tutorial. Automatica 47(3):485–495 Nuño E, Sarras I, Basañez L (2013) Consensus in networks of nonidentical Euler-Lagrange systems using P+d controllers. IEEE Trans Robot 29(6):1503–1508 Ren W, Cao Y (2011) Distributed coordination of multi-agent networks: emergent problems, models, and issues. Springer, London, Great Britain Sarras I, Nuño E, Basañez L, Kinnaert M (2016) Position tracking in delayed bilateral teleoperators without velocity measurements. Int J Robust Nonlinear Control 26(7):1437–1455 Xi J, He M, Liu H, Zheng J (2016) Admissible output consensualization control for singular multiagent systems with time delays. J Franklin Inst 353(16):4074–4090

Part III

Different Testbeds and the Adaptation of Industrial Robots for Practical Implementation

Chapter 11

The Robot CRS 465

Abstract Parts I and II deal mainly with analytical results for the modeling and control of robot manipulators. Although the main algorithms are expressed in a rather generic way so that they are useful for any robot manipulator, in practice each of them has its own particularities. The last part of the book deals with the mathematical description of some of the robots present at the Laboratory for Robotics of the School of Engineering at the National Autonomous University of Mexico, UNAM. This chapter is devoted to developing the kinematic and dynamic models of the robot A465 by CRS Robotics.

11.1 Characteristics of the Robot CRS A465 The robot A465 is a serial-link manipulator manufactured by the Canadian company CRS Robotics. It is shown in Figs. 1.1 and 11.1. The main characteristics of the CRS A465 are the following: • 6 degrees of freedom industrial robot made up of rigid links and revolute joints. • The actuators are DC motors equipped with harmonic drive gears. The reduction gears provide high input torques. • Each joint is equipped with a high-resolution incremental encoder that measures the joint position. • The last three joints form a spherical wrist. • The maximum payload is 2 kg. • The total weight of the robot is 31 kg.

11.2 Kinematics of the Robot A465 To be able to work with the A465, the first step is to describe its direct kinematic as explain in Sect. 2.2. First of all, seven coordinate frames are assigned according to the Denavit-Hartenberg Algorithm 1 (see Fig. 11.2). Then, Algorithm 2 is employed to © The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control of Robot Manipulators, Lecture Notes in Electrical Engineering 798, https://doi.org/10.1007/978-3-030-85980-0_11

313

314

11 The Robot CRS 465

Fig. 11.1 Serial-link manipulator A465

Table 11.1 Denavit-Hartenberg parameters (θi , αi , di , ai ) for the robot CRS A465 Joint θi (◦ ) αi (◦ ) di (m) ai (m) 1 2 3 4 5 6

q1 q2 q3 q4 q5 q6

90 0 90 −90 90 0

0.33 0 0 0.33 0 0.076

0 0.305 0 0 0 0

compute the different homogeneous transformations, while the different parameters are given in Table 11.1. Based on Table 11.1 and (2.146), i.e. ⎤ cθi −sθi cαi sθi sαi ai cθi ⎢ sθi cθi cαi −cθi sαi ai sθi ⎥ i−1 ⎥, Ai = ⎢ ⎣ 0 sθi cθi di ⎦ 0 0 0 1 ⎡

the different homogenous transformation matrices associated with each coordinate frame are given by ⎡

c1 ⎢ s1 0 A1 = ⎢ ⎣0 0

0 0 1 0

s1 −c1 0 0

⎤ 0 0⎥ ⎥ d1 ⎦ 1

(11.1)

11.2 Kinematics of the Robot A465

315

d4 x2 .x3

q4

q3 z1 q2

y1

q5

x4 , x5

x6

q6

y4

z3

y2 z2 .y3

d6

a2

z5 , z6

z4 , y5

y6

x1 z0

y0

d1

x0 q1

Fig. 11.2 Coordinate frames for the robot A465 assigned according to the Denavit-Hartenberg Algorithm 1.

⎡

c2 ⎢ s2 1 A2 = ⎢ ⎣0 0 ⎡

c3 ⎢ s3 2 A3 = ⎢ ⎣0 0 ⎡

c4 ⎢ s4 3 ⎢ A4 = ⎣ 0 0 ⎡

c5 ⎢ s5 4 A5 = ⎢ ⎣0 0

−s2 c2 0 0 0 0 1 0

s3 −c3 0 0

0 0 −1 0 0 0 1 0

0 0 1 0

⎤ a2 c2 a 2 s2 ⎥ ⎥ 0 ⎦ 1 ⎤ 0 0⎥ ⎥ 0⎦ 1

−s4 c4 0 0

s5 −c5 0 0

⎤ 0 0⎥ ⎥ d4 ⎦ 1

⎤ 0 0⎥ ⎥ 0⎦ 1

(11.2)

(11.3)

(11.4)

(11.5)

316

11 The Robot CRS 465

⎡

c6 ⎢ s6 5 A6 = ⎢ ⎣0 0

−s6 c6 0 0

0 0 1 0

⎤ 0 0⎥ ⎥ d6 ⎦ 1

(11.6)

where ci = cos(qi ) and si = sin(qi ) with i = 1, 2, . . . , 6. By taking into account the previous equations, the homogeneous transformation 0 T 6 that relates the base coordinate frame ox0 y0 z0 with the end-effector frame ox6 y6 z6 is given by 0

T6 =

0

R 6 0 p6 0T 1

= 0 A1 1 A2 2 A3 3 A4 4 A5 5 A6 .

(11.7)

The position of the end-effector can then be calculated as ⎡

0

⎤ a2 c1 c2 + d4 c1 s12 + d6 (c1 (c23 c4 s5 + s23 c5 ) + s1 s4 s5 ) p6 = ⎣ a2 s1 c2 + d4 s1 s23 + d6 (s1 (c23 c4 s5 + s23 c5 ) − c1 s4 s5 ) ⎦ d1 + a2 s2 − d4 c23 + d6 (s23 c4 s5 − c23 c5 )

(11.8)

where c23 = cos(q2 + q3 ) and s23 = sin(q2 + q3 ). On the other hand, the orientation of the robot 0 R6 is expressed as follows 0

R6 = 0 R3 3 R6

(11.9)

where the rotation matrices 0 R3 and 3 R6 are given by ⎡

⎤ c1 c23 s1 c1 s23 0 R3 = ⎣ s1 c23 −c1 s1 s23 ⎦ s23 0 −c23

(11.10)

⎡

⎤ c4 c5 s6 − s4 s6 −c4 c5 s6 − s4 c6 c4 s5 3 R6 = ⎣ s4 c5 c6 + c4 s6 −s4 c5 s6 + c4 c6 s4 s5 ⎦ . −s5 c6 s5 c6 c5

(11.11)

Notice that the rotation matrix 3 R6 matches the rotation matrix obtained with the Euler angles Z Y Z , i.e. the joints q4 , q5 and q6 constitute a set of Euler angles Z Y Z with respect to the coordinate frame x3 y3 z 3 .

11.3 Dynamics of the Robot A465 In order to get a dynamic model, it is necessary to compute different moments and products of inertia with respect to new coordinate frames fixed at the different centers of mass of the links. The first challenge, however, is to deal with the evident lack of symmetry of the links as well as with a non-uniform mass density. For that reason,

11.3 Dynamics of the Robot A465

317

x y 

a

y

z

r

z c

x

b

Fig. 11.3 Inertia tensor of a rectangular prism and cylinder used to approximate the links of the robot A465

the links are approximated by rectangular prisms and cylinders with uniform mass density as shown in Fig. 11.3. Recall that the inertia tensors of a rectangular prism and of a cylinder, denoted by I p and I c respectively, are given by ⎡ Ip = ⎣ ⎡ Ic = ⎣

1 m(b2 12

+ c2 )

0 0 1 m(3r 2 12

0 1 2 m(a + c2 ) 12 0

+ 2 )

0 0

⎤ 0 ⎦ 0 1 2 2 m(a + b ) 12

⎤ 0 0 1 m(3r 2 + 2 ) 0 ⎦ 12 1 mr 2 0 2

where m is the mass of the object and the geometric parameters a, b, c, d and r are shown in Fig. 11.3. The inertia matrix of the manipulator is given by (3.27), i.e. H(q) =

6



m i J Tvci (q) J vci (q) + J Tωci (q)0 Ri I i 0 RiT J ωci (q) ,

i=1

where the Jacobian matrices J vci (q) ∈ R3×6 can be computed as follows 

J vc1 (q) = z 0 × 0 r¯ 1 O 3×5 

J vc2 (q) = z 0 × 0 r¯ 2 z 1 × (0 r¯ 2 − 0 p1 ) O 3×4

(11.12)

318

11 The Robot CRS 465



J vc3 (q) = z 0 × 0 r¯ 3 z 1 × (0 r¯ 3 − 0 p1 ) z 2 × (0 r¯ 3 − 0 p2 ) O 3×3

J vc4 (q) = z 0 × 0 r¯ 4 z 1 × (0 r¯ 4 − 0 p1 ) z 2 × (0 r¯ 4 − 0 p2 ) · · ·  z 3 × (0 r¯ 4 − 0 p3 ) O 3×2

J vc5 (q) = z 0 × 0 r¯ 5 z 1 × (0 r¯ 5 − 0 p1 ) z 2 × (0 r¯ 5 − 0 p2 ) · · ·  z 3 × (0 r¯ 5 − 0 p3 ) z 4 × (0 r¯ 5 − 0 p4 ) 0

J vc6 (q) = z 0 × 0 r¯ 6 z 1 × (0 r¯ 6 − 0 p1 ) z 2 × (0 r¯ 6 − 0 p2 ) · · · z 3 × (0 r¯ 6 − 0 p3 ) z 4 × (0 r¯ 6 − 0 p4 ) z 5 × (0 r¯ 6 − 0 p5 )



T where z 0 = 0 0 1 and z j = 0 R j z 0 ; these vectors can be extracted from the homogeneous transformation matrix 0 A j with j = 1, . . . , 6. The vectors 0 r¯ j ∈ R3 denotes the position of the center of mass of the jth link and 0 p j is the position vector of the origin of the frame j. The Jacobian matrices associated with the angular velocities are given by ⎡

J ωc1

⎤ 0 0 ··· 0 = ⎣0 0 ··· 0⎦ 1 0 ··· 0 ⎡

J ωc2

0 s1 · · · = ⎣ 0 −c1 · · · 1 0 ···

⎤ 0 0⎦ 0

⎡

J ωc3

0 s1 s1 · · · = ⎣ 0 −c1 −c1 · · · 1 0 0 ···

⎡

J ωc4

⎤ 0 s1 s1 c1 s23 · · · 0 = ⎣ 0 −c1 −c1 s1 s23 · · · 0 ⎦ 1 0 0 −c23 · · · 0 ⎡

J ωc5

⎤ 0 s1 s1 c1 s23 s1 c4 − c1 c23 s4 0 = ⎣ 0 −c1 −c1 s1 s23 −s1 c23 s4 − c1 c4 0 ⎦ −s23 s4 0 1 0 0 −c23

⎤ 0 0⎦ 0

11.3 Dynamics of the Robot A465

319

⎤ 0 s1 s1 c1 s23 s1 c4 − c1 c23 s4 s5 (s1 s4 + c1 c23 c4 ) + c1 s23 c5 = ⎣ 0 −c1 −c1 s1 s23 −s1 c23 s4 − c1 c4 s1 s23 c5 − s5 (c1 s4 − s1 c23 c4 ) ⎦ . −s23 s4 s23 c4 s5 − c23 c5 1 0 0 −c23 ⎡

J ωc6

To proceed, let us compute the center of mass and inertia tensor of the robot’s links. The position of the center of mass of each link can be computed using the homogeneous transformation matrices 0 A1 . . . . , 0 A6 as follows 0

r¯ j = 0 A j j r¯ j ,

j = 1, . . . , 6

(11.13)

T T

where 0 r¯ j = 0 r¯ j 1 ∈ R4 and j r¯ j = j r¯ j 1 ∈ R4 . The constant vector j r¯ j ∈ R3 is the position of the link’s center of mass expressed with respect to frame j. Now, it is possible to begin with the computation of the different moments of inertia. First of all, link 1 is approximated according to Fig. 11.4 and where the coordinate frames ox0 y0 z0 and ox1 y1 z1 are shown. The total mass of the link is given y1

Fig. 11.4 Coordinate frames and geometry of link 1 for the robot A465

z1 m11

m13

m12

d1 c1

z0 y0

y11 z11

r13

m11

z13

yc1

c11 zc1 r12 c12

y13

13

11 r11

z12

y12

m12

c13 12

m13

320

11 The Robot CRS 465

Table 11.2 Parameters of the link 1 Parameter Value c11 c12 c13 r11 r12 r13 c1 11 12 13 m 11 m 12 m 13

Units

0.2 0.2 0.2 0.076 0.1 0.076 0.295 0.257 0.05 0.257 3.0 5.0 3.0

m m m m m m m m m m kg kg kg

by m 1 = m 11 + m 12 + m 13 . To simplify calculations it is assumed that the center of mass of the link is located along the y1 axis. The different parameters of link 1 are summarized in Table 11.2. It is worthy to point out that these parameters are just approximated values since the actual ones are not provided by the manufacturer. From Fig. 11.4, the position of the center of mass expressed with respect to frame 1 is given by ⎡

⎤ 0 1 r¯ 1 = ⎣ c1 − d1 ⎦ . 0

(11.14)

By using Steiner’s theorem (parallel-axis theorem) the diagonal elements of the inertia tensor are computed as follows  2     1 1  2 2 2 m 11 r11 3r12 + c12 + c11 + 2m 11 211 + (d1 − c1 )2 + 6 12  2  1 1 2 2 = m 11 3r11 + c11 + m 12 r12 6 2    2  1 2 2 = m 11r11 + 2m 11 d1 − 2c1 + m 12 3r12 + c12 + m 12 212 12

Ixx1 = Iyy1 Izz1

where it is assumed that m 11 = m 13 , r11 = r13 and 11 = 13 . The parameters of the first link are shown in Table 11.2. The second link is approximated by a rectangular prism. Figure 11.5 shows the coordinate frames and parameters of link 2. It is assumed that the center of mass is located along the x2 -axis, i.e.

11.3 Dynamics of the Robot A465

321

Fig. 11.5 Coordinate frames and geometry of link 2 for the robot A465

x2

y2 z2

a2 c2

y1 z1

x2

y2

b2

z2 yc2

xc2

zc2 y1 z1

2

c2

⎡

⎤ c2 − a2 2 ⎦. 0 r¯ 2 = ⎣ 0

(11.15)

The diagonal elements of the inertia tensor computed with respect to frame 2 are given by   1 m 2 b22 + c22 12   1 m 2 c22 + 22 + m 2 22 = 12   1 m 2 b22 + 22 + m 2 22 = 12

Ix x2 = I yy2 Izz2

322

11 The Robot CRS 465

Table 11.3 Parameters of the links 2 and 3 Link 2 Parameter Value Units b2 c2 2 c2 m2

0.6 0.6 0.26 0.155 5.0

m m m m kg

Link 3 Parameters

Value

Units

r3 3 c3 m3 –

0.051 0.12 0.04 4.0 –

m m m kg –

where the numerical values are shown in Table 11.3. The third link is approximated by a cylinder and it is assumed that the position of its center of mass has only a component in the z 3 -axis (see Fig. 11.6). The position

Top view

Fig. 11.6 Coordinate frames and geometry of link 3 for the robot A465

z3 y3 c3 x3

xc3 3 r3 c3

y3

yc3

zc3

11.3 Dynamics of the Robot A465

323

of the center of mass expressed with respect to the frame 3 is given by ⎡

⎤ 0 3 r¯ 3 = ⎣ 0 ⎦ . c3

(11.16)

The diagonal elements of the inertia tensor I 3 are given by  1  2 m 3 3r3 + 23 + m 3 2c3 2  1  = m 3 3r32 + 23 + m 3 2c3 2 1 = m 3r32 . 2

Ix x3 = I yy3 Izz3

Another reasonable assumption is to locate the center of mass at the origin of the frame 3, which leads to c3 = 0. The parameters of the third link are given in Table 11.3.

Top view

y4

y3

z3

z4

c4 d4 4 y3

z3

r4

yc4

zc4

Fig. 11.7 Coordinate frames and geometry of link 4 for the robot A465

324

11 The Robot CRS 465

Top view

Fig. 11.8 Coordinate frames and geometry of link 5 for the robot A465

y4

z5 c5 y5 5 xc5

x5

r5

z5 y5

yc5

zc5

The coordinate frames and the different parameters of the fourth link are shown in Fig. 11.7. In this case, the position of the center of mass is given by ⎡

⎤ 0 4 r¯ 4 = ⎣ d4 − c4 ⎦ . 0

(11.17)

The numerical values of the parameters of link 4 are given in Table 11.4. Since it is approximated by a cylinder, the diagonal elements of its inertia tensor are   1 m 4 3r42 + 24 + m 4 (d4 − c4 )2 12 1 = m 4 r42 2   1 m 4 3r42 + 24 + m 4 (d4 − c4 )2 . = 12

Ix x4 = I yy4 Izz4

Figure 11.8 shows the coordinate frame and the different parameters of link 5. Similar to links 3 and 4, the fifth link is approximated by a cylinder and the position of its center of mass expressed with respect to frame 5 is given by

11.3 Dynamics of the Robot A465

325

Table 11.4 Parameters of links 4, 5 and 6 for the robot A465 Link 4

Link 5

Link 6

Parameter Value

Units

Parameter Value

Units

Parameter Value

Units

r4

0.051

m

r5

0.051

m

r6

0.0335

m

4

0.259

m

5

0.11

m

6

0.036

m

c4

0.24

m

c5

0.015

m

c6

0.0785

m

m4

2.0

kg

m5

0.5

kg

m6

0.2

kg

x5 x6 z5

y5 c6

d6

y6

z6

Fig. 11.9 Coordinate frames and geometry of link 6 for the robot A465.

⎡

⎤ 0 5 r¯ 5 = ⎣ 0 ⎦ . c5

(11.18)

The elements of the inertia tensor of the fifth link are given by   1 m 5 3r52 + 25 + m 5 2c5 12   1 m 5 3r52 + 25 + m 5 2c5 = 12 1 = m 5r52 . 2

Ix x5 = I yy5 Izz5

The numerical values of the parameters are shown in Table 11.4. The last link is approximated by a cylinder as shown in Fig. 11.9. The position of the center of mass of the sixth link is given by ⎡

⎤ 0 6 ⎦. 0 r¯ 6 = ⎣ c6 − d6

(11.19)

326

11 The Robot CRS 465

On the other hand, the diagonal elements of the inertia tensor are   1 m 6 r62 + 26 + m 6 (c6 − d6 )2 12   1 = m 6 r62 + 26 + m 6 (c6 − d6 )2 12 1 = m 6r62 . 2

Ix x6 = I yy6 Izz6

The different parameters of link 6 are given in Table 11.4. Finally, the elements of the inertia matrix H(q) ∈ R6×6 can be computed as H11 = m 2 2c2 c22 + m 3 (a2 c2 + c3 s23 )2 + m 4 (a2 c2 + c4 s23 )2 + m 5 ((a2 c2 + c5 s23 c5 + d4 s23 + c5 c23 c4 s5 )2 + 2c5 s24 s25 ) + m 6 ((a2 c2 + c6 s23 c5 + d4 s23 + c6 c23 c4 s5 )2 + 2c6 s24 s25 ) + I yy1 + Ix x2 s22 + I yy2 c22 + Ix x3 s223 + Izz3 c223 + Ix x4 s223 c24 + I yy4 c223 + Izz4 s223 s24 + Ix x5 (s23 c4 c5 + c23 s5 )2 + I yy5 s223 s24 + Izz5 (s23 c4 s5 − c23 c5 )2 + Ix x6 (s23 c4 c5 c6 + c23 s5 c6 − s23 s4 s6 )2 + I yy6 (s23 c4 c5 s6 + c23 s5 s6 + s23 s4 c6 )2 + Izz6 (s23 c4 s5 − c23 c5 )2 H12 = m 5 c5 s4 s5 (c23 (d4 + c5 c5 ) − a2 s2 − c5 s23 c4 s5 ) + m 6 c6 s4 s5 (c23 (d4 + c6 c5 ) − a2 s2 − c6 s23 c4 s5 ) + s23 c4 s4 (I x x4 − Izz4 ) + I x x5 s4 c5 (s23 c4 c5 + c23 s5 ) + I x x6 (s23 c4 c5 c6 + c23 s5 c6 − s23 s4 s6 )(s4 c5 c6 + c4 s6 ) + I yy6 (s23 c4 c5 s6 + c23 s5 s6 + s23 s4 c6 )(s4 c5 s6 − c4 c6 ) + Izz6 s4 s5 (s23 c4 s5 − c23 c5 ) + Izz5 s4 s5 (s23 c4 s5 − c23 c5 ) − I yy5 s23 s4 c4 H13 = −m 5 c5 s4 s5 (c5 s23 c4 s5 − c23 (d4 + c5 c5 )) − m 6 2c6 s4 s5 s23 c4 s5 + m 6 c6 c23 s4 s5 (d4 + c6 c5 ) + I x x5 s4 c5 (s23 c4 c5 + c23 s5 ) + (I x x4 − Izz4 )s23 c4 s4 − I yy5 s23 s4 c4 + Izz5 s4 s5 (s23 c4 s5 − c23 c5 ) + I yy6 (s23 c4 c5 s6 + c23 s5 s6 + s23 s4 c6 )(s4 c5 s6 − c4 c6 ) + Izz6 s4 s5 (s23 c4 s5 − c23 c5 ) H14 = −m 5 c5 s5 (c5 c23 s5 + a2 c2 c4 + s23 c4 (d4 + c5 c5 )) − I yy4 c23 m 6 c6 s5 (c6 c23 s5 + a2 c2 c4 + s23 c4 (d4 + c6 c5 )) − I x x5 s5 (s23 c4 c5 + c23 s5 ) + Izz5 c5 (s23 c4 s5 − c23 c5 ) − I x x6 s5 c6 (s23 c4 c5 c6 + c23 s5 c6 − s23 s4 s6 ) − I yy6 s5 s6 (s23 c4 c5 s6 + c23 s5 s6 + s23 s4 c6 ) + Izz6 c5 (s23 c4 s5 − c23 c5 ) H15 = −m 5 c5 s4 (s23 (c5 + d4 c5 ) + a2 c2 c5 ) − I yy5 s23 s4 − m 6 c6 s4 (s23 (c6 + d4 c5 ) + a2 c2 c5 )

11.3 Dynamics of the Robot A465

327

+ I x x6 s6 (s23 c4 c5 c6 +c23 s5 c6 − s23 s4 s6 ) − I yy6 c6 (s23 c4 c5 s6 + c23 s5 s6 + s23 s4 c6 ) H16 = Izz6 (s23 c4 s5 − c23 c5 ) H22 = m 2 2c2 + (m 5 + m 6 )a22 + m 3 (a22 + 2c3 + 2a2 c3 s3 ) + m 4 (a22 + 2c4 + 2a2 c4 s3 ) + m 5 ((s5 c4 c5 )2 + (d4 + c5 c5 )2 + 2a2 s3 (d4 + c5 c5 + 2c5 a2 c3 c4 s5 )) + m 6 ((s5 c4 c6 )2 + (d4 + c5 c6 )2 + 2a2 s3 (d4 + c5 c6 ) + 2c6 a2 c3 c4 s5 ) + Izz2 + I yy3 + I x x4 s24 + Izz4 c24 + I x x5 s24 c25 + I yy5 c24 + Izz5 s24 s25 + I x x6 (s4 c5 c6 + c4 s6 )2 + I yy6 (s4 c5 s6 −c 4c6 )2 + Izz6 s24 s25 H23 = m 3 c3 (c3 + a2 s3 ) + m 4 c4 (c4 + a2 s3 ) + m 5 (d4 + c5 c5 )2 + I yy3 + m 5 (a2 s3 (d4 + c5 c5 ) + (c5 c4 s5 )2 + c5 a2 c3 c4 s5 )) + m 6 (d4 + c6 c5 )2 + m 6 (a2 s3 (d4 + c6 c5 ) + (c c4 s5 )2 + c6 a2 c3 c4 s5 ) + I x x4 s24 + Izz4 c24 + Izz4 c24 + I x x5 s24 c25 + I yy5 c24 + Izz5 s24 s25 + I x x6 (s4 c5 c6 + c4 s6 )2 + I yy6 (s4 c5 s6 − c4 c6 )2 + Izz6 s24 s25 H24 = −m 5 c5 s4 s5 (d4 + c5 c5 + a2 s3 ) − m 6 c6 s4 s5 (d4 + c6 c5 + a2 s3 ) + (Izz5 − I x x5 )s4 c5 s5 − I x x6 s5 c6 (s4 c5 c6 + c4 s6 ) − I yy6 s5 s6 s4 c5 s6 + I yy6 s5 s6 c4 c6 + Izz6 s4 s5 c5 H25 = m 5 c5 c4 (c5 + d4 c5 ) + m 5 c5 a2 (s3 c4 c5 + c3 s5 ) + m 6 2c6 c4 + m 6 c6 d4 c4 c5 + m 6 c6 a2 (s3 c4 c5 + c3 s5 ) + I yy5 c4 + I x x6 s6 (s4 c5 c6 + c4 s6 ) − I yy6 c6 (s4 c5 s6 − c4 c6 ) H26 = Izz6 s5 s4 H33 = m 3 2c3 + m 4 2c4 + m 5 (d4 + c5 c5 )2 + m 5 (c5 c4 s5 )2 + m 6 (d4 + c6 c5 )2 + m 6 (c6 c4 s5 )2 + I yy3 + I x x4 s24 + Izz4 c24 + I x x5 s24 c25 + I yy5 c24 + Izz5 s24 s25 + I x x6 (s4 c5 c6 + c4 s6 )2 + I yy6 (s4 c5 s6 − c4 c6 )2 + Izz6 s24 s25 H34 = −m 5 c5 s4 s5 (d4 + c5 c5 ) − m 6 c6 s4 s5 (d4 + c6 c5 ) + (Izz5 − I x x5 )s4 c5 s5 − I x x6 s5 c6 (s4 c5 c6 + c4 s6 ) − I yy6 s5 s6 (s4 c5 s6 − c4 c6 ) + Izz6 s4 s5 c5 H35 = m 5 c5 c4 (c5 + d4 c5 ) + m 6 c6 c4 (c6 + d4 c5 ) + I yy5 c4 + I x x6 s6 (s4 c5 c6 + c4 s6 ) − I yy62 c6 (s4 c5 s6 − c4 c6 ) H36 = Izz6 s4 s5 H44 = m 5 (c5 s5 )2 + m 6 (c6 s5 )2 + I yy4 + I x x5 s25 + Izz5 c25 + I x x6 s25 c26 + I yy6 s25 s26 + Izz6 c25 H45 = (I yy6 − I x x6 )s5 c6 s6 H46 = Izz6 c5 H55 = m 5 2c5 + m 6 2c6 + I yy5 + I x x61 s26 + I yy6 c26 H56 = 0 H66 = Iz66

328

11 The Robot CRS 465

˙ are computed Recall that as explained in Sect. 3.2, the elements of the matrix C(q, q) as a function of the elements of H(q) and they are omitted here simplicity’s sake. As to the gravity vector g(q) ∈ R6 , its elements are given by g1 = 0 g2 = m 2 gc2 c2 + (m 3 + m 4 + m 5 + m 6 )ga2 c2 + (m 4 c4 + (m 5 + m 6 )d4 )gs23 + (m 5 c5 + m 6 c6 )(s23 c5 + c23 c4 s5 ) g3 = (m 3 c3 + m 4 c4 + (m 5 + m 6 )d4 )gs23 + (m 5 c5 + m 6 c6 )g(s23 c5 + c23 c4 s5 ) g4 = −(m 5 c5 + m 6 c6 )gs23 s4 s5 g5 = (m 5 c5 + m 6 c6 )g(c23 s5 + c23 c4 s5 ) g6 = 0 where g = 9.8(m/s2 ) is the gravity constant. As it is mentioned at the beginning of the chapter the robot A465 is equipped with DC motors and harmonic drive gears. The main characteristics of these devices such as model, mechanical and electrical constants, gear ratio, etc. are shown in Tables 11.5 and 11.6.

Table 11.5 Main characteristic of the DC motors for the robot A465 Motor Brand Model Motor 1 Motor 2 Motor 3 Motor 4 Motor 5 Motor 6

Cleveland motion controls Cleveland motion controls Cleveland motion controls Cleveland motion controls Tamagawa Tamagawa

Table 11.6 Parameters of the DC motors for the robot A465 Motor Mechanical constant, Electrical constant, K a (Nm/A) K b (V rad/s) Motor 1 Motor 2 Motor 3 Motor 4 Motor 5 Motor 6

0.14234 0.14234 0.14234 0.0053 0.0053 0.0392

Gear ratio

M H 3515 M H 3515 M H 3515 M H 3515 T S3253N T S908N 7

0.14229 0.14229 0.14229 0.0534 0.0534 0.0392

r1 r2 r3 r4 r5 r6

= 100 = 100 = 100 = 101 = 100 = 101

Armature resistance, Ra () 0.84 0.84 0.84 2.7 2.7 6.9

Chapter 12

The Robot CRS 255

Abstract The robot A255 by CRS Robotics is a smaller industrial robot than the A465 but with only five degrees of freedom. It owns also a special kinematic arrangement which makes it an interesting case of study.

12.1 Characteristics of the Robot CRS A255 This chapter is devoted to developing the kinematics and dynamic models of the robot manipulator A255 by CRS Robotics shown in Figs. 1.1 and 12.1. The robot A255 is a serial-link manipulator manufactured by the Canadian company CRS Robotics. Its main characteristics are the following: • Five degrees of freedom made up of rigid links and revolute joints. • The actuators are DC motors equipped with harmonic drive gears. • Each joint is equipped with a high resolution encoder that measures the joint angular position. • The nominal payload is 1 kg. • The total weight of the robot is 17 kg.

12.2 Kinematics of the Robot A255 The robot A255 is an open-chain serial manipulator made up of five joints, labeled as qi with i = 1, . . . , 5. The main difference between the robot A465 is that the joints 2, 3 and 4 of the robot A255 are remotely driven by timing belts. Therefore, the joint angle qi is not affected by the previous joint angles qi−1 (i = 3, 4) and Algorithm 2 cannot be used. These three joints are measured with respect to the same axis of rotation. However, it is still possible to employ the Denavit-Hartenberg Algorithm 1 to assign the coordinate frames for each joint. Figure 12.2 shows the result in doing so.

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control of Robot Manipulators, Lecture Notes in Electrical Engineering 798, https://doi.org/10.1007/978-3-030-85980-0_12

329

330

12 The Robot CRS 255

Fig. 12.1 Serial manipulator robot A255

a3

y3 , x4

y2

d5 q4

x5

q5

x2 z2 q3 z1 q2

y1

a2

z3

x3 , z4

z5 y5

x1 z0

y0

d1

x0 q1 Fig. 12.2 Coordinate frames of the robot A255 assigned according to the Denavit-Hartenberg Algorithm 1

12.2 Kinematics of the Robot A255

331

Table 12.1 Kinematic parameters of the robot CRS A255 Joint θi (◦ ) αi (◦ ) di (m) 1 2 3 4 5

q1 q2 q3 q4 + 90 q5

90 – – 90 0

0.254 0 0 0 0.0508

ai (m) 0 0.254 0.254 0 0

The kinematic parameters of the robot are shown in Table 12.1. Although Algorithm 2 could be used for some joints, all the homogeneous transformation matrices are computed by inspection and they are given by ⎡

c1 ⎢ s1 0 A1 = ⎢ ⎣0 0

0 0 1 0

s1 −c1 0 0

⎤ 0 0⎥ ⎥ d1 ⎦ 1

0 1 0 0

0 0 1 0

⎤ a2 c2 a 2 s2 ⎥ ⎥ 0 ⎦ 1

0 1 0 0

0 0 1 0

⎤ a3 c2 a 3 s3 ⎥ ⎥ 0 ⎦ 1

−s4 ⎢ c4 3 A4 = ⎢ ⎣ 0 0

0 0 1 0

c4 s4 0 0

−s5 c5 0 0

0 0 1 0

⎡

1 ⎢0 1 A2 = ⎢ ⎣0 0 ⎡

1 ⎢0 2 A3 = ⎢ ⎣0 0 ⎡

⎡

c5 ⎢ s5 4 A5 = ⎢ ⎣0 0

⎤ 0 0⎥ ⎥ 0⎦ 1 ⎤ 0 0⎥ ⎥. d5 ⎦ 1

(12.1)

(12.2)

(12.3)

(12.4)

(12.5)

332

12 The Robot CRS 255

By taking into account the previous equations the homogeneous transformation matrix 0 T 5 that relates the base frame ox0 y0 z0 with the end-effector frame ox5 y5 z5 is given by 0

T 5 = A1 A2 A3 A4 A5 = 0

1

2

3

4

R 5 0 p5 , 0T 1

0

(12.6)

where the position of the end-effector 0 p5 is given by ⎡

⎤ c1 (a2 c2 + a3 c3 + d5 c4 ) 0 p5 = ⎣ s1 (a2 c2 + a3 c3 + d5 c4 ) ⎦ d1 + a2 s2 + a3 s3 + d5 s4

(12.7)

and its orientation by ⎡

⎤ s1 s5 − c1 s4 c5 s1 c5 + c1 s4 s5 c1 c4 0 R5 = ⎣ −c1 s5 − s1 s4 c5 s1 s4 s5 − c1 c5 s1 c4 ⎦ c4 c5 −c4 s5 s4

(12.8)

where ci = cos(qi ) and si = sin(qi ) with i = 1, 2, . . . , 5.

12.3 Dynamics of the Robot A255 Similar to the robot A465, the links of the robot A255 are approximated by regular prisms, cylinders and solid disks. In this case, the inertia matrix is given after (3.27) by 5

 m i J Tvci (q) J vci (q) + J Tωci (q)0 Ri I i 0 RiT J ωci (q) (12.9) H(q) = i=1

where the Jacobian matrices associated with the linear velocities J vci ∈ R3×5 are computed as follows J vci (q) =

∂ r¯ i , i = 1, . . . , 5 ∂q

(12.10)

where r¯ i ∈ R3 is the center of mass of the i-th link, while the Jacobian matrices associated to the angular velocities are given by

12.3 Dynamics of the Robot A255

333

⎡

J ωc1

J ωc2

0 = ⎣0 1 ⎡ 0 = ⎣0 1

⎤ 0000 0 0 0 0⎦ 0000

⎤ s1 0 0 0 −c1 0 0 0 ⎦ 0 000

⎡

J ωc3

⎤ 0 s1 s1 0 0 = ⎣ 0 −c1 −c1 0 0 ⎦ 1 0 0 00 ⎡

J ωc4

⎤ 0 s1 s1 s1 0 = ⎣ 0 −c1 −c1 −c1 0 ⎦ 1 0 0 0 0 ⎡

J ωc5

⎤ 0 s1 s1 s1 c1 c4 = ⎣ 0 −c1 −c1 −c1 s1 c4 ⎦ . 1 0 0 0 s4

The next step is to compute the center of mass and the inertia tensor of each link. The first one is divided in three different bodies approximated by two cylinders and a rectangular prism as shown in Fig. 12.3. For simplicity, it is assumed that the center of mass of the link is located along the z 0 -axis, i.e.

y1

a13 x1

z1

d1

c1

y1

m12

13

m13 m11 r11

b13

c12 x1

z1 11 z0

z0 y0

y0

x0

x0

Fig. 12.3 Coordinate frames and geometry of link 1 for the robot A255

334

12 The Robot CRS 255

⎤ 0 0 r¯ 1 = ⎣ 0 ⎦ . c1 ⎡

On the other hand, by applying the parallel axis theorem, the diagonal elements of inertia matrix I 1 are given by Ixx1 =

2

1 i=1

Iyy1 =

2

1 i=1

Izz1 =

2



2  1 2 2 2 m 1i 3r1i2 + c1i + m 1i 21i + m 13 a13 + c13 2 12 i=1 3



2  1 2 2 2 m 1i 3r1i2 + c1i + m 1i 21i + m 13 a13 + b13 2 12 i=1

2

1 i=1

2

m 1i r1i2 +

2  1 2 m 13 b13 + m 13 213 + c13 12

The parameters of link 1 for the robot A255 are shown in the Table 12.2. The second link is approximated by a rectangular prism. Figure 12.4 shows the coordinate frames and the kinematic parameters associated with link 2. It is assumed that the center of mass is along the y2 -axis, thus the vector position expressed with respect to the base frame is given by ⎤ c c2 0 r¯ 2 = ⎣ c s2 ⎦ . d1 ⎡

Table 12.2 Parameters of the link 1 for the robot A255 Parameter Value c11 c12 c13 r11 r12 c1 11 12 13 m 11 m 12 m 13

0.15 0.15 0.1 0.05 0.05 0.2 0.1 0.1 0.08 2.0 2.0 3.0

Units m m m m m m m m m kg kg kg

12.3 Dynamics of the Robot A255

335

y2

y2

x2 z2

x2

z2

c2

a2 y1

y1

a2

b2 z1

x1

z1

c2

x1

Fig. 12.4 Coordinate frames and geometry of link 2 for the robot A255

The elements of the inertia matrix of the link 2 are given by

 1 m 2 a22 + b22 + 12

 1 m 2 b22 + c22 = 12

 1 m 2 a22 + b22 + = 12

Ixx2 = Iyy2 Izz2

1 m 2 a22 2

(12.11) (12.12)

1 m 2 a22 , 2

(12.13)

whose values can be seen in Table 12.3. In a similar way, the third link is approximated by a rectangular prism and its assumed that the center of mass is located along the x3 -axis as shown in Fig. 12.5. This assumption yields to the following expression of its center of mass ⎡

⎤ c1 (a2 c2 + c3 c3 ) 0 r¯ 3 = ⎣ s1 (a2 c2 + c3 c3 ) ⎦ . d1 + a2 s2 + c3 s3

(12.14)

336

12 The Robot CRS 255

Table 12.3 Parameters of the links 2 and 3 for the second link for the robot A255 Link 2 Link 3 Parameter Value Units Parameter Value Units a2 b2 c2 c2 m2

0.254 0.1 0.1 0.15 3

m m m m kg

a3 b3 c3 c3 m3

0.254 0.1 0.1 0.2 2

m m m m kg

a3

y2 c3

y3

z2 x2 z3

x3

y2 a3

y3

x2

z2

b3 z3

c3

x3

Fig. 12.5 Coordinate frames and geometry of link 3 for the robot A255

In this case, the diagonal elements of the inertia matrix I 3 computed with respect to frame ox3 y3 z3 are given by

 1 m 3 b32 + c32 12

 1 1 m 3 a32 + c32 + m 3 a32 = 12 2

2  1 1 m 3 a3 + b32 + m 3 a32 . = 12 2

Ix x3 = I yy3 Izz3

12.3 Dynamics of the Robot A255

337

Fig. 12.6 Coordinate frames and geometry of link 4 for the robot A255

y3 , x4

z3 x3 , z4 y3

z3 r3

x3

The kinematic and dynamic parameters of link 3 are shown in Table 12.3. The coordinate frames and geometry of the fourth link is shown in Fig. 12.6. The link is approximated by a solid disk and it is assumed that the center of mass is located at the origin of frame ox4 y4 z4 . Therefore, the position vector of the center of mass is given by ⎡

⎤ c1 (a2 c2 + a3 c3 ) 0 r¯ 4 = ⎣ s1 (a2 c2 + a3 c3 ) ⎦ d1 + a2 s2 + a3 s3 and the inertia tensor computed with respect to frame ox4 y4 z4 is given by ⎡1

⎤ m 4 r42 0 0 I 4 = ⎣ 0 41 m 4 r42 0 ⎦ , 0 0 21 m 4 r42 4

whose values can be seen in Table 12.4. The last link is also approximated by a solid disk as shown in Fig. 12.7.

(12.15)

338

12 The Robot CRS 255

Table 12.4 Parameters of the links 4 and 5 for the robot A255 Link 4 Link 5 Parameter Value Units Parameter r4 d4 c4 m4

0.04 0 0 1

m m m kg

r5 d5 c5 m5

Value

Units

0.03 0.0508 0.045 0.5

m m m kg

c5

Fig. 12.7 Coordinate frames and geometry of link 5 for the robot A255

y4

x5

z4

x4

z5 y5 x5 r5 z5

y5 The position of the center of mass of link 5 is then given by ⎤ c1 (a2 c2 + a3 c3 + c5 c4 ) 0 r¯ 5 = ⎣ s1 (a2 c2 + a3 c3 + c5 c4 ) ⎦ . d1 + a2 s2 + a3 s3 + c5 s5 ⎡

(12.16)

On the other hand, the inertia tensor I 5 is given by ⎤ m 5r52 0 0 I 5 = ⎣ 0 41 m 5r52 0 ⎦ . 0 0 21 m 5r52 ⎡1 4

Finally, the kinematic and dynamic parameters of the link are given in Table 12.4.

12.3 Dynamics of the Robot A255

339

With the previous information, the elements of the inertia matrix for the robot A255 are given by H11 = m 2 2c2 c2 2 + m 3 (a2 c2 + c3 c3 )2 + m 5 (a2 c2 + a3 c3 + c5 c4 )2

H12 H13 H14 H15

+ m 4 (a2 c2 + a3 c3 )2 + (Izz4 − Ixx4 ) s4 2 + Iyy1 + Iyy2 + I yy3 + Ixx4  

+ Ixx5 c5 2 + Iyy5 s5 2 c4 2 + Izz5 s4 2 + Izz4 − Iyy5 + Izz5 s4 2 

= Ixx5 − Iyy5 c4 c5 s5

 = Ixx5 − Iyy5 c4 c5 s5

 = Ixx5 − Iyy5 c4 c5 s5 =0

H22 = (m 3 + m 4 + m 5 ) a22 + m 2 2c2 + Izz2 + Izz3 + Iyy4 + Iyy5 c5 2 H23

+ Ixx5 s5 2 = (m 3 a2 c3 + (m 4 + m 5 )a2 a3 ) cos(q2 − q3 ) + Izz3 + Iyy4 + Ixx5 s5 2 + Iyy5 c25

H24 = m 5 a2 c5 cos(q2 − q4 ) + Iyy4 + Ixx5 s5 2 + Iyy5 c5 2 H25 = 0 H33 = m 3 2c3 + (m 4 + m 5 )a32 + Iyy4 + Izz3 + Ixx5 s5 2 + Iyy5 c5 2 H34 = m 5 a3 c5 cos(q3 − q4 ) + Ixx5 s5 2 + Iyy5 c5 2 + Iyy4 H35 = 0 

H44 = m 5 2c5 + Ixx5 − Iyy5 s5 2 + Iyy4 + Iyy H45 = 0 H55 = Izz5 . The matrix C(q, q) can be computed according to the procedure described in Sect. 3.2 once the inertia matrix H(q) is known and it is omitted for simplicity. The elements of the gravity vector g(q) ∈ R5 are given by g1 = 0 g2 = c2 gc2 m 2 + a2 gc2

5

mj

j=3

g3 = c3 gc3 m 3 + a2 gc3

5

j=4

g4 = c5 m 5 gc4 g5 = 0

mj

340

12 The Robot CRS 255

Table 12.5 Main characteristic of the DC motors for the robot A255 Motor Brand Model Motor 1 Motor 2 Motor 3 Motor 4 Motor 5

Cleveland motion controls Cleveland motion controls Cleveland motion controls Cleveland motion controls Tamagawa

M H 3515

r1 = 72

M H 3515

r2 = 72

M H 3515

r3 = 72

M H 3515

r4 = 16

T S3253N

r5 = 8

Table 12.6 Parameters of the DC motors for the robot A255 Motor Mechanical constant, Electrical constant, K a (Nm/A) K b (V rad/s) Motor 1 Motor 2 Motor 3 Motor 4 Motor 5

0.0657 0.0657 0.0657 0.0053 0.0053

Gear ratio

0.0657 0.0657 0.0657 0.0534 0.0534

Armature resistance, Ra () 2.4 2.4 2.4 2.7 2.7

where g = 9.8 (m/s2 ) is the gravity constant. Finally the parameters of the different DC motors of the robot are shown in Tables 12.5 and 12.6.

Chapter 13

Adapting the Robots CRS 465 and 255 for Original Control Laws Implementation

Abstract The robots A465 and A255 by CRS Robotics are industrial manipulators whose controllers are provided by the manufacturer and therefore they cannot be programmed by the user. When it comes to the implementation of different control laws or observers it is necessary to buy additional data acquisition boards if available or, if these special boards are not available, then it is mandatory to adapt the devices for this particular goal. This is not a minor challenge and this chapter makes a proposal about how this can be done.

13.1 Original System Figure 13.1 depicts a typical arrangement of an industrial robot and its control unit as provided by the manufacturer. This chapter introduces hardware level modifications of the CRS A465 and CRS A255 robots (in what follows, A465 and A255 for short) and their corresponding control modules, the CRS-C500C units (C500C for short), in order to have an open architecture. Rather than a closed architecture, an open architecture allows to implement advanced control and estimation algorithms like those developed in Part II of the book. Each industrial robot is actuated through an amplification stage with electromechanical protection mechanisms. The central control units process the signals coming from the robots, i.e. joint encoders, which are employed in the default motion control algorithm. The original system for the A255 and A465 industrial manipulators is made up of the manipulators themselves along with a control unit (C500C) and a teach pendant, as shown in Figs. 13.2 and 13.3, respectively. Some important mechanical characteristics of the manipulators are summarized in Table 13.1. The C500C unit has a 486-based embedded computer with an embedded operating system (CROS) developed by the manufacturer. This unit also has a power supply stage for all the input and output devices, an input/output board to acquire the encoder signals and to send the output voltages to the manipulator motors through an amplification stage. The electrical characteristics of the amplification stage are listed in Table 13.2. © The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control of Robot Manipulators, Lecture Notes in Electrical Engineering 798, https://doi.org/10.1007/978-3-030-85980-0_13

341

342

13 Adapting the Robots CRS 465 and 255 …

Fig. 13.1 Original communication of the industrial CRS manipulators with a PC Fig. 13.2 Original components of the CRS A255 manipulator

Fig. 13.3 Original components of the CRS A465 manipulator

13.1 Original System

343

Table 13.1 Specifications of the CRS A465 and CRS A255 manipulators Specification A465 robot A255 robot Weight Load (max) Load (nominal) Reach (X − Y ) Actuators Transmission

32 kg 3 kg 2 kg 711.2 mm DC servo motors Armonic gears

17 kg 2 kg 1 kg 560 mm DC servo motors Armonic gears and straight/conic gears

Joint motion range Waist Shoulder Elbow Wrist 1 Wrist 2 Wrist 3

+175◦ to −175◦ + 90◦ to −90◦ +110◦ to −110◦ +180◦ to −180◦ +105◦ to −105◦ +180◦ to −180◦

+175◦ to −175◦ +110◦ to 0◦ 0◦ to −125◦ – +110◦ to -110◦ +180◦ to −180◦

Table 13.2 Electrical limit values at the power amplification stage: input voltage (vi ), output voltage (vo ), and output current (i o ) Joint A465 robot A255 robot vi (V) vo (V) i o (A) Gain vi (V) vo (V) i o (A) Gain Waist Shoulder Elbow Wrist 1 Wrist 2 Wrist 3

± 10 ± 10 ± 10 ± 10 ± 10 ± 10

± 70 ± 70 ± 70 ± 30 ± 30 ± 30

12 12 12 3 3 3

7 7 7 3 3 3

± 10 ± 10 ± 10 – ± 10 ± 10

± 25 ± 25 ± 25 – ± 25 ± 25

2 2 2 – 2 2

2.5 2.5 2.5 – 2.5 2.5

The joints are actuated by DC motors with different kinds of mechanical transmissions. The manufacturer controller consists of a closed-architecture PID position/velocity control, which receives the encoder signals and computes the output voltages. A diagram of this controller is shown in Fig. 13.4. As mentioned above, the joint positions are measured by means of optical encoders. The encoders provided in both the A255 and the A465 robots are of the incremental type with two channels (A and B), i.e. are quadrature encoders. There is a third channel that serves as a mark to calibrate the position measurement (channel Z). A typical plot of these signals is shown in Fig. 13.5. The quadrature encoders provided in both robots are the LDA-051-1000 by SUMTAK, powered by 5 V and with a current consumption of 140 mA. Along with the three above channel signals, ¯ B, ¯ and Z¯ signals. the encoders also provide their logic negatives, i.e. the A,

344

13 Adapting the Robots CRS 465 and 255 …

Fig. 13.4 Block diagram of the manufacturer’s controller

Fig. 13.5 Quadrature encoder signals

The C500C hardware is divided into three sections: 1. The upper section has the controller electronics 2. The lower one has the power stage 3. The emergency brake system and the signal acquisition stage. The DC motor drivers are located in the remaining space at the side of the cabinet, as can be seen in Fig. 13.6. In this figure, the back view of the C500C controller is shown, where the connectors Robot Power and Robot Feedback can be distinguished. The Robot Power connector is the motor power output, whereas the Robot Feedback one has the encoders input signals. The E-Stop is an emergency break button included in the original system as a part of the C500C module as shown in Fig. 13.7. When the E-Stop is pushed, it inhibits the power stage which drives the motors of the A255 and A465 robots. They are blocked by means of electric relays, which in turn must be liberated by a specific button for each robot. A copy of the E-Stop button is located also in the Teach Pendant. A diagram of the emergency stop system is displayed in Fig. 13.7. The connector SYSIO included in the C500C module allows to control the signals for breaking/unbreaking. The ports 9, 10, 22, and 23 of this connector where employed to implement a portable emergency button for each robot.

13.2 Hardware Modification

345

Fig. 13.6 Back view of the C500C controller

Fig. 13.7 Emergency break system of the C500C module

13.2 Hardware Modification To open the controller architecture in order to program control algorithms different from that of the manufacturer, the original PID controller must be inhibited. To carry out this task, the encoder signals and the output voltages to the motors are in turn processed by a personal computer (PC), and thus an acquisition stage is implemented as follows. To process the input and output signals, an input/output acquisition unit, in this case the CompactRIO module of National Instruments, is employed. The CompactRIO has a programmable FPGA for processing high-demanding tasks. In turn, the connection with the PC is made through fast-speed Ethernet communication. The particular NI-cRIO 9014 model employed in this work has eight expansion

346

13 Adapting the Robots CRS 465 and 255 …

slots, which can be connected to one of the several I/O modules offered by National Instruments. In the present modification, two types of I/O modules were employed: the models NI-9401 and NI-9263. The NI-9401 module is a general purpose digital input/output board, with 8 configurable channels. This module supports TTL signals and has a sampling rate of 9 MHz. The input voltages interval is 2–5.25 V. These modules are employed to process the A, B, and Z encoder signals for a total of 11 joints, i.e. 6 and 5 for the A465 and the A255 robots, respectively. Thus, five NI-9401 modules are employed in this adaptation. In turn the NI-9263 can generate up to four analog signals by means of isolated 16-bits digital to analog converters (DAC). These channels have also built-in shortcircuit and over-voltage protectors. The output voltage limits are ±10 V, with load currents up to 1 m A. The slew-rate for each channel is 4 V/μs. Since the total number of joints for both manipulators is 11, three NI-9263 modules are employed for the current modification. The maximum electric power required by the CompactRIO and the eight aforementioned modules is of 20 W. The source voltage must be between 19 and 30 V, although the manufacturer recommends a power source of 24 V that supports at least 2 A. In order to satisfy these requirements, a 24 V Quint power source by Phoenix Contact is employed, which is capable of delivering up to 5 A.

13.2.1 Signal Routing Three DB25 connectors are employed to manage the signals coming to and from the robots, namely [AB], [Z], and [VCOM], for each robot. The [AB] and [Z] wires carry the digital signals coming from the encoders, while the [VCOM] send the commanded voltages to the amplification stage which in turn sends the amplified voltages to the motors. These wires are connected to the acquisition hardware by an ad-hoc designed PCB board. The trace widths proposed for this board are of 24 mils (0.609 mm), supporting a maximum of 460 mA and a temperature rise of 1 ◦ C. A ground plane with a thickness of 35 mm is also included in the PCB, to avoid crosstalk between adjacent traces. See Figs. 13.8 and 13.9.

13.2.2 Digital Stage for the A465 Manipulator For each robot, the connections are divided into two parts, namely a digital and an analog stage. The signals transmitted by the A465 manipulator encoders are the ¯ B, ¯ and Z¯ , respectively. digital channels A, B, and Z , and their digital negatives A, All these signals present a drop in the transmitted voltage as well as added noise due to the imperfections on the transmission wires, as shown in Fig. 13.10.

13.2 Hardware Modification

347

Fig. 13.8 Upper view of the C500C module showing the added DB-25 [AB], [Z], and [VCOM] connectors

As mentioned above, the NI-9401 modules employed for the acquisition of these signals work with TTL signals, which is not the case for the unprocessed signals as the one shown in Fig. 13.10. To solve this problem, the integrated circuit 26LS32 is employed, which is a quadruple line receiver with three state outputs. The output of this circuit is shown in Fig. 13.11, where it can be appreciated that the noise has been significantly diminished. Note that the number of signals is reduced to half after this circuit, since it com¯ B, ¯ and Z¯ into one single bines channels A, B, and Z with their logic negatives A, signal for each channel, which by abuse of notation will be called A, B, and Z . To connect the 26LS32 output signals to the NI-9401 modules, an isolation stage is proposed by means of HCPL2630 optocouplers. Each DIP package has two outputs with a maximum speed of 10 Mb/s. For example, two channels for a joint encoder can be connected to a single DIP as shown in Fig. 13.12. R1 and R2 are set to 220  to have a polarization current for the internal led of about 1.5 mA. The pull-up resistors R3 and R4 are set to 350  in order to minimize the signal propagation time and the signal deformation. Finally, 15 pF capacitors are included at the output side as recommended in the HCPL2630 datasheet.

348

13 Adapting the Robots CRS 465 and 255 …

Fig. 13.9 Back view of the C500C module showing the added DB-25 [AB], [Z], and [VCOM] connectors

Fig. 13.10 Signal measured directly from channel A of the A465 first joint encoder

The HCPL2630 optocoupler output, which is connected to the NI-9401 module, is shown in Fig. 13.13. The diagram containing all the encoder channels for the six joints of the A465 manipulator is displayed in Figs. 13.14 and 13.15. Additionally to the encoder signals, the robot A465 employs the H SW binary signals that indicates the home position for each joint (see Fig. 13.16).

13.2 Hardware Modification

Fig. 13.11 Output of the 26LS32 line receiver for channel A of the A465 first joint

Fig. 13.12 Connection of the HCPL2630 optocouplers

Fig. 13.13 HCPL2630 optocoupler output for the channel A of the A465 first joint

349

350

13 Adapting the Robots CRS 465 and 255 …

Fig. 13.14 Signal conditioning circuit for the A465 manipulator encoders

13.2.3 Digital Stage for the A255 Manipulator The A255 manipulator does not transmit neither the logic negative signals of the encoders nor the H SW bits. Therefore, in this case the use of the 26LS32 line receiver is omitted, and only the optocoupler stage is employed by means of the same HCPL2630 DIP circuits mentioned before. The schematics of the circuit employed

13.2 Hardware Modification

Fig. 13.15 Signal conditioning circuit for the A465 manipulator encoders

351

352

13 Adapting the Robots CRS 465 and 255 …

Fig. 13.16 Signal conditioning circuit for the A465 manipulator H SW bits

13.2 Hardware Modification

353

Fig. 13.17 Signal measured directly from channel A of the A255 first joint encoder

Fig. 13.18 HCPL2630 output optocoupler for channel A of the A255 first joint

for this stage is displayed in Fig. 13.17. The output of the optocoupler is shown in Fig. 13.18. The connection diagram of the incremental encoders for the robot A255 are depicted in Figs. 13.19 and 13.20.

13.2.4 Analog Stage The NI-9263 modules have three stages: 1. A digital to analog (DAC) converter 2. A power amplifier 3. An over-voltage and short-circuit protection. Therefore, no additional conditioning electronics is needed and the output of the NI-9263 are connected directly to the DB25 [VCOM] connector that goes to the C500C power amplifier. The connection of these three modules is shown in

354

13 Adapting the Robots CRS 465 and 255 …

Fig. 13.19 Signal conditioning circuit for the A255 manipulator encoders

13.2 Hardware Modification

Fig. 13.20 Signal conditioning circuit for the A255 manipulator encoders

355

356

13 Adapting the Robots CRS 465 and 255 …

Fig. 13.21 Analog stage wiring

Fig. 13.21. The modules share the same ground of the C500C module, but they are isolated from the CompactRIO and the NI-9401 modules. Since each module has four analog channels, i.e. a total of 12 analog channels are available, and the total number of motors is 11, there is a remaining analog output channel. This channel is employed to control the emergency brake, labeled as BT N _C T R L in Fig. 13.21.

13.2.5 Power Stage and Electric Protections Figure 13.22 shows the final modified hardware for the main power source of the CompactRIO and the designed electronic board delivers 24 V. To obtain a 5 V power source, the Recom R785.0-0.5 integrated circuit is used. A 1N4004 diode is included at the input to prevent undesirable backward currents. Also, a single-pole doublethrow relay is included, activated by two jumpers JP1 and JP2 (see Fig. 13.23).

13.3 Software Implementation The encoder signals A, B, and Z of the 11 joint encoders and the output voltages to the 11 motors are processed by the embedded system CompactRIO FPGA, which is programmed according to the National Instruments software LabVIEW. Three main components are employed in this program, and they are repeated for each joint.

13.3 Software Implementation

357

Fig. 13.22 Modified hardware of the C500C module Table 13.3 Description of the virtual instruments

Therefore, three main virtual instruments are created to simplify the main program, as shown in Table 13.3. By taking into account the virtual instruments of Table 13.3, the encoder total count and the output voltages generation for each joint is carried out according to the flow diagram of Fig. 13.24. Next, this same flow diagram is repeated for all eleven joints. Finally, the eleven blocks are included into a single Do-While cycle. A Tick Count block was added to have control of the sample period, which is 5 μs. Therefore, the obtained sample rate for the whole system is 200 kS/s.

358

Fig. 13.23 Protection circuits

13 Adapting the Robots CRS 465 and 255 …

13.3 Software Implementation

359

Fig. 13.24 Flow diagram of the encoder counter and the output voltage generator for one joint

360

13 Adapting the Robots CRS 465 and 255 …

Reference CRS Robotics Corporation (2000) C500 controller user guide. CRS Robotics Corporation, Burlington, Ontario, Canada

Chapter 14

The Geomagic Touch Haptic Device

Abstract The robots A465 and A255 by CRS Robotics are industrial manipulators able to carry out a wide variety of tasks. However, for haptic purposes their inertias can be too high, for which it is desirable to have available low inertia manipulators. This is the case of he Geomagic Touch haptic device. This chapter develops its kinematic and dynamic models.

14.1 Characteristics of the Geomagic Touch Manipulator The Geomagic Touch haptic device, shown in Figs. 1.1 and 14.1, is an upgraded version of the Phantom Omni device and it has become very popular in the last few years since great number of researchers have employed it for the experimental validation of different control algorithms. Such popularity is due to several factors, in particular to its very high cost/benefit ratio. Additionally, its well developed API makes it a very attractive choice for students and practitioners searching for a readyto-use platform. However, since this device is intended for haptics and virtual graphics applications, there is not much information available about the kinematics from the manufacturer. Moreover, despite its wide use, there are very few kinematic and dynamic models available in the literature.

14.1.1 Kinematics of the Full Five DoF Robot The kinematic model of the Geomagic Touch by considering all its five degrees of freedom (DoF) is developed in this section. A Denavit-Hartenberg assignment according to Algorithm 1 is shown in Fig. 14.2. The propose of this assignment is to have the minimal number of parameters. For this reason, the ox0 ,y0 ,z0 frame is located at the intersection of the first two joint axes. There are some other practical considerations to be made, namely:

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control of Robot Manipulators, Lecture Notes in Electrical Engineering 798, https://doi.org/10.1007/978-3-030-85980-0_14

361

362

14 The Geomagic Touch Haptic Device

Fig. 14.1 Geomagic Touch haptic device by 3D Systems

z0 , y1

a2

x3 , y2

y0 z1

x0 , x1

d4

x4

a5

y5

y4 z2

x2 , z3

x5 z4

z5

Fig. 14.2 Denavit-Hartenberg assignment for the 5 DoF Geomagic Touch configuration

• The rotation axes of joints 1 and 4, i.e. z 0 and z 3 are opposed to those defined by the manufacturer. Therefore, the sign of the positions read from these joints must be changed (multiplied by −1). • As the rotation axis of the first joint is in the opposite direction, the torque commanded to the first joint must be also multiplied by −1. Since the 4th joint is not actuated, this adjustment is not necessary for the fourth joint. • The assignment of the x5 axis is also in the opposite direction. Therefore, an offset of −180◦ must be added to the 5-th joint measurement. • The actuator and the position sensor for the third joint are physically located at the second joint. Thus, strictly speaking the Geomagic Touch robot is not a serial manipulator. Nevertheless, it can be seen as a serial one, by making the adjustment q3 = q3read − q2read ,

(14.1)

14.1 Characteristics of the Geomagic Touch Manipulator

363

Table 14.1 Adjustments of joint measurements Joint Adjustment 1 2 3 4 5

q1 q2 q3 q4 q5

= −q1read = q2read = q3read − q2read = −q4read = q5read − π

Table 14.2 Denavit-Hartenberg parameters of the 5 DoF Geomagic Touch configuration Joint ai [m] di [m] αi [rad] θi [rad] 1 2 3 4 5 ∗

0 0.145 0 0 0.040

0 0 0 0.138 0

π/2 0 π/2 −π/2 0

θ1 ∗ θ2 ∗ θ3 ∗ θ4 ∗ θ5 ∗

variable quantity

where qiread , i = 1, . . . , 5, is the i-th joint value measured by employing the manufacturer API. A summary of the above adjustments is presented in Table 14.1.

14.1.2 Direct Kinematics of the Full Five DoF Robot Given the above considerations, one can apply the Denavit-Hartenberg Algorithm 2 to obtain the parameters given in Table 14.2. The values for a2 and d4 shown in Table 14.2 were obtained through a standard kinematic calibration. Notice that the values employed by the manufacturer in the API are a2 = 0.13335 [m] and d4 = 0.13335 [m], which are not physically accurate. Now, it is possible to compute the homogeneous transformation from the ox0 y0 z0  frame to the ox5 y5 z5 frame, i.e. 0 T 5 = 0 R5 0 p5 . The end effector position is given by ⎤ ⎡ a5 c5 (c1 c4 c23 + s1 s4 ) − a5 c1 s5 s23 + c1 (a2 c2 + d4 s23 ) 0 p5 = ⎣ a5 c5 (c4 c23 s1 − c1 s4 ) − a5 s1 s5 s23 + s1 (a2 c2 + d4 s23 ) ⎦ , −c23 d4 + a2 s2 + a5 (c23 s5 + c4 c5 s23 ) whereas the components of the rotation matrix 0 R5 are

364

14 The Geomagic Touch Haptic Device

r11 = c5 s1 s4 + c1 (c4 c5 c23 − s5 s23 ) r12 = −s1 s4 s5 − c1 (c4 c23 s5 + c5 s23 ) r13 = c4 s1 − c1 c23 s4 r21 = c5 (c4 c23 s1 − c1 s4 ) − s1 s5 s23 r22 = (c1 s4 − c4 c23 s1 ) s5 − c5 s1 s23 r23 = −c1 c4 − c23 s1 s4 r31 = c23 s5 + c4 c5 s23 r32 = c5 c23 − c4 s5 s23 r33 = −s4 s23 . There are two other coordinate frames that can be of interest, namely, the base and the default frames. The base frame is the one located at the base of the robot, i.e. at the intersection of the z 0 axis with the table that supports the manipulator. Accordingly, this frame is just a translation over the z 0 axis of the ox0 y0 z0 frame in Fig. 14.2. On the other hand, the default frame is the one defined by the manufacturer, related with the ox0 y0 z0 frame in Fig. 14.2 by both a translation and a rotation. The homogeneous transformations relating these two frames with the ox0 y0 z0 frame described above are ⎡

1 ⎢ 0 Base T0 = ⎢ ⎣0 0

0 1 0 0

0 0 1 0

⎤ 0 0⎥ ⎥ and d⎦ 1

⎡

0 ⎢ 1 Default T0 = ⎢ ⎣0 0

0 0 1 0

1 0 0 0

⎤ tx 0 ⎥ ⎥, −tz ⎦ 1

where d ≈ 0.17 [m], tx ≈ 0.1425 [m], and tz ≈ 0.0383 [m].

14.1.3 Differential Kinematics of the Full Five DoF Robot As stated in Sect. 2.4, the manipulator geometric Jacobian J(q) can be decomposed into two parts, namely

 J v (q) J(q) = , (14.2) J ω (q)

14.1 Characteristics of the Geomagic Touch Manipulator

365

where the components of the upper part are jv11 = −a2 c2 s1 − d4 s23 s1 + a5 (−c4 c5 c23 s1 + s5 s23 s1 + c1 c5 s4 ) jv12 = a2 c1 c2 + a5 c1 c4 c5 c23 + a5 c5 s1 s4 + c1 (d4 − a5 s5 ) s23 jv13 = 0 jv21 = −c1 (a2 s2 + c23 (a5 s5 − d4 ) + a5 c4 c5 s23 ) jv22 = −s1 (a2 s2 + c23 (a5 s5 − d4 ) + a5 c4 c5 s23 ) jv23 = a2 c2 + a5 c4 c5 c23 + (d4 − a5 s5 ) s23 jv31 = c1 (c23 (d4 − a5 s5 ) − a5 c4 c5 s23 ) jv32 = s1 (c23 (d4 − a5 s5 ) − a5 c4 c5 s23 ) jv33 = a5 c4 c5 c23 + (d4 − a5 s5 ) s23 jv41 = a5 c5 (c4 s1 − c1 c23 s4 ) jv42 = −a5 c5 (c1 c4 + c23 s1 s4 ) jv43 = −a5 c5 s4 s23 jv51 = −a5 (s1 s4 s5 + c1 (c4 c23 s5 + c5 s23 )) jv52 = −a5 ((c4 c23 s1 − c1 s4 ) s5 + c5 s1 s23 ) jv53 = a5 (c5 c23 − c4 s5 s23 ) , whereas the lower part is given by ⎡

⎤ 0 s1 s1 c1 s23 c4 s1 − c1 c23 s4 J ω (q) = ⎣ 0 −c1 −c1 s1 s23 −c1 c4 − c23 s1 s4 ⎦ . 1 0 0 −c23 −s4 s23 .

14.1.4 Dynamics of the Full Five DoF Robot The dynamic model below was obtained by employing the Euler–Lagrange equations of motion, as described in Sect. 3.1.1. These equations result in the well known Lagrangian model ˙ q˙ + D q˙ + g(q) = τ . H(q)q¨ + C(q, q)

(14.3)

˙ can be obtained from the matrix H(q) by computing the Since the matrix C(q, q) Christoffel symbols of the first kind and D is a diagonal matrix accounting for the viscous friction coefficients, only the H(q) matrix and the g(q) vector are given below. The components of the matrix H(q) are given by

366

14 The Geomagic Touch Haptic Device

h 11 = m 2 2c2 c22 + m 3 (a2 c2 + s23 c3 )2 + m 4 (a2 c2 + s23 c4 )2

+ m 5 (a2 c1 c2 + c1 s23 (d4 − s5 c5 ) + c5 s1 s4 c5 + c1 c4 c5 c23 c5 )2  2 + (a2 c2 s1 + (c4 c5 c23 s1 − c1 c5 s4 − s5 s23 s1 ) c5 + d4 s23 s1 ) 2 + Ixx5 (c23 s5 + c4 c5 s23 )2 + Iyy5 (c5 c23 − c4 s5 s23 )2 + Iyy2 c22 + Ixx4 c42 s23

h 12

2 2 2 2 2 + Iyy4 c23 + Izz3 c23 + Ixx2 s22 + Ixx3 s23 + Izz4 s42 s23 + Izz5 s42 s23 + Iyy1 = −m 5 c5 s4 c5 (a2 s2 + (c3 (c4 c5 s2 + c2 s5 ) + s3 (c2 c4 c5 − s2 s5 )) c5 − c23 d4 )

+ Ixx4 c4 s4 s23 + Ixx5 c5 s4 (c23 s5 + c4 c5 s23 ) − Iyy5 s4 s5 (c5 c23 − c4 s5 s23 ) − Izz4 c4 s4 s23 − Izz5 c4 s4 s23 h 13 = −m 5 c5 s4 c5 ((c3 (c4 c5 s2 + c2 s5 ) + s3 (c2 c4 c5 − s2 s5 )) c5 − c23 d4 ) + Ixx4 c4 s4 s23 + Ixx5 c5 s4 (c23 s5 + c4 c5 s23 ) − Iyy5 s4 s5 (c5 c23 − c4 s5 s23 ) − Izz4 c4 s4 s23 − Izz5 c4 s4 s23 h 14 = −m 5 c5 c5 (a2 c2 c4 + (c2 (c3 c5 − c4 s3 s5 ) − s2 (c5 s3 + c3 c4 s5 )) c5 + c4 d4 s23 ) h 15 h 22

h 23

− Ixx5 s5 (c23 s5 + c4 c5 s23 ) − Iyy5 c5 (c5 c23 − c4 s5 s23 ) − Iyy4 c23 = m 5 s4 c5 (a2 c2 s5 + s23 (d4 s5 − c5 )) − s4 s23 Izz5



= m 2 2c2 + m 3 2a2 s3 c3 + a22 + 2c3 + m 4 2a2 s3 c4 + a22 + 2c4  + m 5 (a2 s2 + c23 (s5 c5 − d4 ) + c4 c5 s23 c5 )2  + (a2 c2 + c4 c5 c23 c5 + s23 (d4 − s5 c5 ))2 + Ixx5 c52 s42 + Izz4 c42 + Izz5 c42 + Ixx4 s42 + Iyy5 s42 s52 + Iyy3 + Izz2 = m 3 c3 (a2 s3 + c3 ) + m 4 c4 (a2 s3 + c4 )

+ m 5 a2 ((c3 c4 c5 − s3 s5 ) c5 + d4 s3 ) + c42 c52 + s52 2c5 − 2d4 s5 c5 + d42 + Izz4 c42 + Izz5 c42 + Ixx4 s42 + c52 Ixx5 s42 + Iyy5 s42 s52 + Iyy3

h 24 = m 5 c5 s4 c5 (−a2 s3 + s5 c5 − d4 ) − Ixx5 c5 s4 s5 + Iyy5 c5 s4 s5 h 25 = m 5 c5 (a2 (c3 c5 − c4 s3 s5 ) + c4 (c5 − d4 s5 )) + Izz5 c4

h 33 = m 3 2c3 + m 4 2c4 + m 5 c42 c52 + s52 2c5 − 2d4 s5 c5 + d42 + c52 s42 Ixx5 h 34

+ c42 Izz4 + c42 Izz5 + s42 Ixx4 + s42 s52 Iyy5 + Iyy3 = m 5 c5 s4 c5 (s5 c5 − d4 ) − Ixx5 c5 s4 s5 + Iyy5 c5 s4 s5

h 35 = m 5 c4 c5 (c5 − d4 s5 ) + Izz5 c4 h 44 = m 5 c52 2c5 + Iyy5 c52 + Ixx5 s52 + Iyy4 h 45 = 0 h 55 = m 5 2c5 + Izz5 .

14.1 Characteristics of the Geomagic Touch Manipulator

367

The inertia tensors are computed by assuming that all the off-diagonal terms, i.e. the cross products of inertia, are zero. The first body is a hollow sphere of radius r1 and mass m 1 , with principal inertia moments given by Ixx1 = Iyy1 = Izz1 =

2 m 1r1. 3

The second and third bodies are considered solid cuboids, with sides b2 and c2 , and b3 and c3 , respectively, and lengths a2 and l3 . While a2 is a Denavit-Hartenberg parameter, l3 is the part of d4 that corresponds to the third link, so that when summed to the part corresponding to the fourth link it satisfies d4 = l3 + l4 . The principal moments of inertia for the second and third link, after applying the parallel axis theorem, yields

m2 2 b2 + c22 Ixx2 = 12

m2 2 4a2 + c22 Iyy2 = 12

m2 2 4a2 + b22 Izz2 = 12

m3 2 4l3 + c32 Ixx3 = 12

m3 2 4l3 + b32 Iyy3 = 12

m3 2 b3 + c32 . Izz3 = 12 The fourth and fifth bodies are assumed to be cylindrical, with radii r4 and r5 , and lengths l4 and a5 , with l4 as explained above. For these two links, the principal moments of inertia are

m4 2 3r4 + l42 Ixx4 =Izz4 = 12 m4 2 r Iyy4 = 2 4

m5 2 3r5 + a52 + m 5 δ52 Iyy5 =Izz5 = 12 m5 2 r5 , Ixx5 = 2 where δ5 is the distance from the center of mass of the fifth link to the z 5 axis. On the other hand, the components of the vector g(q) are computed as g1 = 0

 g2 = g c2 c2 m 2 + c4 c5 c23 c5 m 5 + a2 c2 (m 3 + m 4 + m 5 )  + c3 m 3 s23 + c4 m 4 s23 + d4 m 5 s23 − c5 m 5 s5 s23

368

14 The Geomagic Touch Haptic Device

g3 = g (c3 m 3 s23 + c4 m 4 s23 + m 5 (d4 s23 + c5 (c4 c5 c23 − s5 s23 ))) g4 = −gc5 c5 m 5 s4 s23 g5 = gc5 m 5 (c5 c23 − c4 s5 s23 ) , where g is the acceleration of gravity constant.

14.2 Simplified Three DoF Geomagic Touch A widely used configuration for the Geomagic Touch consists in a simplified fullyactuated three DoF robot by blocking the last two joints. As shown in Fig. 14.3, the last link is not straight because the fifth joint cannot be fixed at −90◦ .

14.2.1 Kinematics of the Three DoF Robot The Denavit-Hartenberg assignment for this configuration is shown in Fig. 14.4. The ox0 ,y0 ,z0 frame is located at the intersection of the first two joint axes. For this configuration, some practical considerations are: • The rotation axis of joint 1, i.e. z 0 , is opposed to that considered by the manufacturer. Therefore, the sign of the position read from this joint must be changed (multiplied by −1). • The torque commanded to the first joint must be also multiplied by −1. • The actuator and the position sensor for the third joint are physically located at the second joint. Thus, strictly speaking the Geomagic Touch robot is not a serial

Fig. 14.3 Mechanical blocking of the last three joints of the Touch robot

14.2 Simplified Three DoF Geomagic Touch

a2

z0 , y1

369

x3 , y2

a3

y3

y0 z1

x0 , x1

x3

x2 , z3

z2

z3

Fig. 14.4 Denavit-Hartenberg assignment for the 3 DoF configuration Table 14.3 Adjustments of joint measurements

Joint

Adjustment

1 2 3

q1 = −q1read q2 = q2read q3 = q3read − q2read

manipulator. Nevertheless, it can be seen as a serial one, by making the adjustment q3 = q3read − q2read ,

(14.4)

where qiread , i = 1, . . . , 3, is the i-th joint value measured by employing the manufacturer API. A summary of the above adjustments is presented in Table 14.3.

14.2.2 Direct Kinematics of the Three DoF Robot By applying the Denavit-Hartenberg Algorithm 2 one obtains the parameters given in Table 14.4. The distance a3 is computed by means of the law of cosines as a3 =



0.1382 + 0.042 − 2(0.138)(0.04) cos(150∗ )

The homogeneous transformation from the ox0 y0 z0 frame to the ox3 y3 z3 frame is denoted   by 0 T 3 = 0 R3 0 p3 , from which the end effector position is given by

370

14 The Geomagic Touch Haptic Device

Table 14.4 Denavit-Hartenberg parameters of the 3 DoF configuration Joint ai [m] di [m] αi [rad] 1 2 3 ∗

0 0.145 0.1738

π/2 0 0

0 0 0

θi [rad] θ1 ∗ θ2 ∗ θ3 ∗

variable quantity

⎡

⎤ (a2 c2 + a3 c23 ) c1 0 p3 = ⎣ (a2 c2 + a3 c23 ) s1 ⎦ , a2 s2 + a3 s23 whereas the rotation matrix 0 R3 is computed to be ⎡

⎤ c1 c23 −c1 s23 s1 0 R3 = ⎣ c23 s1 −s1 s23 −c1 ⎦ . s23 c23 0 The same coordinate frames transformations Base T0 and Default T0 of Sect. 14.1.2 can be employed for this case to find the end-effector coordinates with respect to the base and default frames.

14.2.3 Differential Kinematics of the Three DoF Robot As stated in Sect. 2.4, the manipulator geometric Jacobian J(q) can be decomposed into two parts, namely

 J v (q) J(q) = , (14.5) J ω (q) where the upper part is ⎡

⎤ − (a2 c2 + a3 c23 ) s1 −c1 (a2 s2 + a3 s23 ) −a3 c1 s23 J v (q) = ⎣ c1 (a2 c2 + a3 c23 ) −s1 (a2 s2 + a3 s23 ) −a3 s1 s23 ⎦ , 0 a2 c2 + a3 c23 a3 c23 and the lower part is given by ⎡

⎤ 0 s1 s1 J ω (q) = ⎣ 0 −c1 −c1 ⎦ . 1 0 0

14.2 Simplified Three DoF Geomagic Touch

371

14.2.4 Dynamics of the Three DoF Robot The corresponding Lagrangian model, similar to that given in Sect. 14.1.4 is developed below for this configuration. Since for the present case the dynamic model is ˙ much shorter, along with the matrix H(q) and the vector g(q), the matrix C(q, q) is given as well, computed by means of the Christoffel symbols of the first kind. The components of the matrix H(q) are 2 2 Iyy3 + s22 Ixx2 + s23 Ixx3 + Iyy1 h 11 = m 2 c22 2c2 + m 3 (a2 c2 + c23 c3 ) 2 + c22 Iyy2 + c23 h 12 = 0

h 13 = 0



h 22 = m 2 2c2 + m 3 2a2 c3 c3 + a22 + 2c3 + Izz2 + Izz3 h 23 = m 3 c3 (a2 c3 + c3 ) + Izz3 h 33 = m 3 2c3 + Izz3 . The inertia tensors are computed by assuming that all the off-diagonal terms, i.e. the cross products of inertia, are zero. The first body is a hollow sphere of radius r1 and mass m 1 , with principal inertia moments given by Ixx1 = Iyy1 = Izz1 =

2 m 1r12 . 3

The second and third bodies are considered as solid cuboids, with sides b2 and h 2 , and b3 and h 3 , respectively, and lengths a2 and a3 . The principal moments of inertia for the second and third link, after applying the parallel axis theorem, yields m2 12 m2 = 12 m2 = 12 m3 = 12 m3 = 12 m3 = 12

Ixx2 = Iyy2 Izz2 Ixx3 Iyy3 Izz3

2

b2 + h 22 2

4a2 + h 22 2

4a2 + b22 2

b3 + h 23 2

4a3 + h 23 2

4a3 + b32 .

˙ are given by1 The components of the 3 × 3 matrix C(q, q)

1

The notation ceij is used to avoid confusion with the shorthand notation for the cosine function.

372

14 The Geomagic Touch Haptic Device ce11 = c2 q˙2 s2 Ixx2 + c23 q˙2 s23 Ixx3 + c23 q˙3 s23 Ixx3 − c2 q˙2 s2 Iyy2 − c23 q˙2 s23 Iyy3  − c23 q˙3 s23 Iyy3 − m 2 c2 q˙2 s2 2c2 + m 3 − a2 c23 q˙2 s2 c3 − a2 c2 q˙2 s23 c3  − a2 c2 q˙3 s23 c3 − a22 c2 q˙2 s2 − c23 q˙2 s23 2c3 − c23 q˙3 s23 2c3 ce12 = c2 q˙1 s2 Ixx2 + c23 q˙1 s23 Ixx3 − c2 q˙1 s2 Iyy2 − c23 q˙1 s23 Iyy3 − c2 q˙1 m 2 s2 2c2  + m 3 − 2a2 c2 c3 q˙1 s2 c3 − a2 c22 q˙1 s3 c3 − a22 c2 q˙1 s2 + a2 q˙1 s22 s3 c3 + c2 q˙1 s2 s32 2c3  − c2 c32 q˙1 s2 2c3 − c22 c3 q˙1 s3 2c3 + c3 q˙1 s22 s3 2c3   ce13 = c23 q˙1 s23 Ixx3 − c23 q˙1 s23 Iyy3 + m 3 −a2 c2 q˙1 s23 c3 − c23 q˙1 s23 2c3 ce21 = −c2 q˙1 s2 Ixx2 − c23 q˙1 s23 Ixx3 + c2 q˙1 s2 Iyy2 + c23 q˙1 s23 Iyy3 + m 2 c2 q˙1 s2 2c2  + m 3 2a2 c2 c3 q˙1 s2 c3 + a2 c22 q˙1 s3 c3 + a22 c2 q˙1 s2 − a2 q˙1 s22 s3 c3 − c2 q˙1 s2 s32 2c3  + c2 c32 q˙1 s2 2c3 + c22 c3 q˙1 s3 2c3 − c3 q˙1 s22 s3 2c3 ce22 = −m 3 a2 q˙3 s3 c3 ce23 = −m 3 (a2 q˙2 s3 c3 + a2 q˙3 s3 c3 ) ce31 = c23 q˙1 s23 Iyy3 − c23 q˙1 s23 Ixx3 + m 3 q˙1 s23 c3 (a2 c2 + c23 c3 ) ce32 = m 3 a2 q˙2 s3 c3 ce33 = 0.

Finally, the vector g(q) can be computed to be ⎡

⎤ 0 g(q) = ⎣ gc2 c2 m 2 + g (a2 c2 + c23 c3 ) m 3 ⎦ . gc23 c3 m 3

14.2.5 Linear Parametrization of the Three DoF Robot A linear parametrization and a model identification of the three DoF robot is carried out in this section in order to identify the constant quantities in the dynamic model. In order to obtain a more compact model, in this section it is assumed that the second and third bodies are slender rods instead of solid cuboids, as considered before. This simplification implies that the distances r1 , b2 , h 2 , b3 , and h 3 can be assumed to be zero and in consequence Iyy1 = Ixx2 = Ixx3 = 0, and Iyy2 = Izz2  I2 and Iyy3 = Izz3  I3 . Recall, from Property 3.14 that the left hand side of the Lagrangian model

14.2 Simplified Three DoF Geomagic Touch

373

˙ q¨ ) ∈ Rn× p and a vector of can be expressed as a product of a regressor Y (q, q, p constant parameters ϕ ∈ R , where p is the number of parameters, i.e. ˙ q˙ + D q˙ + g(q) = Y (q, q, ˙ q¨ )ϕ. H(q)q¨ + C(q, q) For the present case, a possible vector of constant parameters is ⎤ ⎡ ⎤ m 2 c22 2c2 + m 3 a22 + I2 π1 ⎥ ⎢π2 ⎥ ⎢ m 3 a2 c3 ⎥ ⎢ ⎥ ⎢ 2 ⎥ ⎢π3 ⎥ ⎢ m 3 c3 + I3 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢π4 ⎥ ⎢ cf1 ⎥=⎢ ⎥, ϕ=⎢ ⎥ ⎢π5 ⎥ ⎢ cf2 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢π6 ⎥ ⎢ cf3 ⎥ ⎢ ⎥ ⎢ ⎦ ⎣π7 ⎦ ⎣ m 2 c2 π8 m 3 c3 ⎡

and the components of the regressor are given by y11 = c22 q¨1 − 2c2 s2 q˙1 q˙2 y12 = c2 c23 q¨1 − (c2 s23 (q˙2 + q˙3 ) + s2 c23 q˙2 )q˙1 2 y13 = s23 q¨1 + c23 s23 (q˙2 + 2q˙3 )q˙1 + s23 c23 q˙1 q˙2

y14 = q˙1 y15 = y16 = y17 = y18 = 0 y21 = q¨2 + c2 s2 q˙12 1 y22 = 2c3 q¨2 + c3 q¨3 + (s2 c23 + c2 s23 )q˙12 − 2s3 q˙2 q˙3 − s3 q˙32 2 y23 = q¨2 + q¨3 − s23 c23 q˙12 y24 = 0 y25 = q˙2 y26 = 0 y27 = gc2 y28 = gc23 y31 = 0 1 y32 = c2 q¨2 + c2 s23 q˙12 + s3 q˙22 2 y33 = q¨2 + q¨3 − c23 s23 q˙12 y34 = y35 = 0 y36 = q˙3 y37 = 0 y38 = gc23 .

(14.6)

374

14 The Geomagic Touch Haptic Device

Table 14.5 Parameters estimation for the 3 DOF configuration Parameter Meaning m 2 c22 2c2

π1 π2 π3 π4 π5 π6 π7 π8

+ m 3 a22

+ I2

m 3 a2 c3 m 3 2c3 + I3 cf1 cf2 cf3 m 2 c2 m 3 c3

Estimated value 0.00453641 0.000341864 0.004503919 0.001373760 0.001188021 0.000553657 0.004948273 0.012955526

The above parameters were experimentally obtained by a standard least squares off-line algorithm and the results are shown in Table 14.5. In terms of the parameters in Table 14.5, the inertia matrix H(q) and the gravity vector g(q) are given by:

⎤ 2 c22 π1 + c2 c23 π2 + s23 π3 0 0 0 π1 + 2c3 π2 + π3 c3 π2 + π3 ⎦ H(q) = ⎣ 0 c3 π2 + π3 π3 ⎡ ⎤ 0 g(q) = ⎣g (c2 π7 + c23 π8 )⎦ , gc23 π8 ⎡

(14.7)

(14.8)

˙ matrix are whereas the components of the C(q, q) 1 (c2 s23 (q˙2 + q˙3 ) + s2 c23 q˙2 ) π2 + c23 s23 (q˙2 + q˙3 )π3 2 1 = −c2 s2 q˙1 π1 − (s2 c23 + c2 s23 ) q˙1 π2 + s23 c23 q˙1 π3 2 1 = − c2 s23 q˙1 π2 + c23 s23 q˙1 π3 2 1 = c2 s2 q˙1 π1 + (s2 c23 + c2 s23 ) q˙1 π2 − s23 c23 q˙1 π3 2 = −s3 q˙3 π2 = −s3 (q˙2 + q˙3 ) π2 1 = c2 s23 q˙1 π2 − c23 s23 q˙1 π3 2 = s3 q˙2 π2 = 0.

ce11 = −c2 s2 q˙2 π1 − ce12 ce13 ce21 ce22 ce23 ce31 ce32 ce33