Control of Ground and Aerial Robots 3031230876, 9783031230875

Citation preview

Intelligent Systems, Control and Automation: Science and Engineering

Mario Sarcinelli-Filho Ricardo Carelli

Control of Ground and Aerial Robots

Intelligent Systems, Control and Automation: Science and Engineering Volume 103

Series Editors Kimon P. Valavanis, Department of Electrical and Computer Engineering, University of Denver, Denver, CO, USA S. G. Tzafestas, National Technical University of Athens, Athens, Greece Advisory Editors P. Antsaklis, University of Notre Dame, Notre Dame, IN, USA P. Borne, Ecole Centrale de Lille, France R. Carelli, Universidad Nacional de San Juan, Argentina T. Fukuda, Nagoya University, Japan N. R. Gans, The University of Texas at Dallas, Richardson, TX, USA F. Harashima, University of Tokyo, Japan P. Martinet, Ecole Centrale de Nantes, France S. Monaco, University La Sapienza, Rome, Italy R. R. Negenborn, Delft University of Technology, The Netherlands António Pascoal, Institute for Systems and Robotics, Lisbon, Portugal G. Schmidt, Technical University of Munich, Germany T. M. Sobh, University of Bridgeport, CT, USA C. Tzafestas, National Technical University of Athens, Greece K. Valavanis, University of Denver, Denver, CO, USA

Intelligent Systems, Control and Automation: Science and Engineering book series publishes books on scientific, engineering, and technological developments in this interesting field that borders on so many disciplines and has so many practical applications: human-like biomechanics, industrial robotics, mobile robotics, service and social robotics, humanoid robotics, mechatronics, intelligent control, industrial process control, power systems control, industrial and office automation, unmanned aviation systems, teleoperation systems, energy systems, transportation systems, driverless cars, human-robot interaction, computer and control engineering, but also computational intelligence, neural networks, fuzzy systems, genetic algorithms, neurofuzzy systems and control, nonlinear dynamics and control, and of course adaptive, complex and self-organizing systems. This wide range of topics, approaches, perspectives and applications is reflected in a large readership of researchers and practitioners in various fields, as well as graduate students who want to learn more on a given subject. The series has received an enthusiastic acceptance by the scientific and engineering community, and is continuously receiving an increasing number of highquality proposals from both academia and industry. The current Series Editor is Kimon Valavanis, University of Denver, Colorado, USA. He is assisted by an Editorial Advisory Board who help to select the most interesting and cutting edge manuscripts for the series: Panos Antsaklis, University of Notre Dame, USA Stjepan Bogdan, University of Zagreb, Croatia Alexandre Brandao, UFV, Brazil Giorgio Guglieri, Politecnico di Torino, Italy Kostas Kyriakopoulos, National Technical University of Athens, Greece Rogelio Lozano, University of Technology of Compiegne, France Anibal Ollero, University of Seville, Spain Hai-Long Pei, South China University of Technology, China Tarek Sobh, University of Bridgeport, USA Springer and Professor Valavanis welcome book ideas from authors. Potential authors who wish to submit a book proposal should contact Thomas Ditzinger (thomas. [email protected]) Indexed by SCOPUS, zbMATH, SCImago.

Mario Sarcinelli-Filho · Ricardo Carelli

Control of Ground and Aerial Robots

Mario Sarcinelli-Filho Departamento de Engenharia Elétrica Universidade Federal do Espírito Santo Vitoria, Espírito Santo, Brazil

Ricardo Carelli Instituto de Automática Universidad Nacional de San Juan CONICET San Juan, Argentina

ISSN 2213-8986 ISSN 2213-8994 (electronic) Intelligent Systems, Control and Automation: Science and Engineering ISBN 978-3-031-23087-5 ISBN 978-3-031-23088-2 (eBook) https://doi.org/10.1007/978-3-031-23088-2 © The Editor(s) (if applicable) and The Author(s), under exclusive license to Springer Nature Switzerland AG 2023 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 my beloved wife, Cleonice, and my beloved “kids”, João Vitor, Maria Alice and José Mário. To the memory of my parents, Mario and Gloria, and my brother José Antônio. To my brothers, sister, nephews, nieces, daughter-in-law and son-in-law. Mario Sarcinelli-Filho To the memory of my parents. To my brothers and sister, nephews and niece. Ricardo Carelli

Preface

This book is a synthesis of several results associated to M.Sc. and Ph.D. works supervised by the authors along the last twenty years. Such works resulted of a cooperation between the groups coordinated by the two authors, the first one working as a professor at the Department of Electrical Engineering of the Federal University of Espírito Santo (UFES) and a researcher of CNPq (National Council for Scientific and Technological Development), both from Brazil, and the second one working as a professor at the Institute of Automatics of the National University of San Juan (UNSJ), and a researcher of CONICET (National Scientific and Technical Research Council), both from Argentina. The text focuses on kinematic and dynamic control of a single mobile robot or a group of them (a multi-robot system). The main topic is the use of the technique of feedback linearization (inverting the system kinematics or dynamics), regarding tasks of positioning, trajectory-tracking and path following. The robots considered are the wheeled mobile platforms, with emphasis in the differential drive vehicles, the most common of the wheeled platforms and the multirotor aerial robots, in particular the quadrotors. A general solution based on the technique of feedback linearization is proposed to guide a single robot and a homogeneous or heterogeneous group of wheeled differential drive robots and/or quadrotors. Examples of the use of the proposed controllers are also provided, for the aforementioned mobile platforms. However, other mobile platforms, as the omnidirectional and car-like robots are also addressed, although not with the same emphasis. The cooperation which has generated the results we are discussing in this book has been financed by two-partner projects and also by single-part projects. The cooperation started in 1998, with the financial support of CAPES—Coordenação de Aperfeiçoamento de Pessoal de Nível Superior, an agency of the Brazilian Ministry of Education and Fundación Antorchas, an independent Argentinian non-profit institution. From 1999 to 2002, the financial support came from a partnership between CAPES and SETCIP—Secretaria de Tecnología, Ciencia e Innovación Productiva, an agency of the Argentinian Ministry of Education. From 2004 to 2018, such support came from the partnership CAPES/Brazil-SPU/Argentina (Secretaria de Políticas Universitárias, a secretary of the Argentinian Ministry of Education), through the vii

viii

Preface

joint program Associated Graduate Centers. In 2015, FAPES—Fundação de Amparo à Pesquisa e Inovação do Espírito Santo, an agency of the State of Espírito Santo, Brazil, that supports scientific and technological development, started financing research projects that partially covered the cooperation between the authors of this book, continuing as the only financing agency nowadays. To all these agencies and foundations, and all the students who developed the researches whose results we present along this text, our thanks for allowing us to go on with our partnership. Finally, it is worth mentioning that such a long cooperation has generated a cooperation agreement between our two universities, namely UFES-Federal University of Espírito Santo, Brazil, and UNSJ—National University of San Juan, Argentina, which also includes double diplomas for M.Sc. and Ph.D. degrees. Vitoria, ES, Brazil San Juan, Argentina September 2022

Mario Sarcinelli-Filho Ricardo Carelli

Acknowledgements

The authors would like to thank UFES—Universidade Federal do Espírito Santo, Brazil, and UNSJ—Universidad Nacional de San Juan, Argentina, for supporting the long-term partnership that allowed them to share expertise and get all the results that supported the writing of this book. They also thank all their M.Sc. and Ph.D. students, whose research work allowed to collect the results here reported. Finally, they also thank CNPq—Conselho Nacional de Desenvolvimento Científico e Tecnológico, CAPES—Coordenação de Aperfeiçoamento de Pessoal de Nível Superior, and FAPES—Fundação de Amparo à Pesquisa e Inovação do Espírito Santo, the three from Brazil, as well as MinCyT—Ministerio de Ciencia, Tecnología e Innovación, CONICET—Consejo Nacional de Investigaciones Científicas y Técnicas, and SECITI—Secretaría de Ciencia, Tecnología e Innovación del Gobierno de San Juan, the three from Argentina, for the financial support granted to the partnership involving UFES and UNSJ, and thus themselves.

ix

Contents

1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1 4

2 Kinematic Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1 Wheeled Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1.1 Omnidirectional Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1.2 Unicycle (Differential Drive) Robots . . . . . . . . . . . . . . . . . . . 2.1.3 Car-like Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1.4 Extended Kinematics for Nonholonomic Robots . . . . . . . . . 2.2 Aerial Multirotor Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.3 Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

5 5 6 8 10 12 16 20 21

3 Dynamic Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1 Modeling a Unicycle Robot with Velocity Inputs . . . . . . . . . . . . . . . 3.1.1 Model Properties . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1.2 Model Parameterization and Identification . . . . . . . . . . . . . . 3.2 Dynamic Model for the Car-Like Robot . . . . . . . . . . . . . . . . . . . . . . 3.3 The Dynamic Model of a Quadrotor . . . . . . . . . . . . . . . . . . . . . . . . . . 3.4 Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

23 23 25 28 29 32 38 38

4 Motion Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Basic Control Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3 Inner-Outer Controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.4 Inverse Kinematics Technique . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.5 Trajectory-Tracking Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.6 Positioning Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.7 Path-Following Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.8 Kinematic Sliding Compensation of Non-holonomic Robots . . . . . 4.9 Dynamic Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

41 41 42 43 44 47 59 63 72 75

xi

xii

Contents

4.10 Feedback Linearization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.11 Cascade Dynamic Compensation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.12 Adaptive Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.13 Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

80 81 84 94 96

5 Control of Multi-robot Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2 Characterization of a Robot Formation . . . . . . . . . . . . . . . . . . . . . . . 5.3 Leader-Follower Paradigm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4 Virtual Structure Paradigm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4.1 Cluster Space Versus Robots Space . . . . . . . . . . . . . . . . . . . . 5.4.2 Transformations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4.3 Movement Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.5 Control in the Cluster Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.6 Control and Dynamic Compensation in the Robots Space . . . . . . . 5.7 Scalability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.8 Multiple Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.9 Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

99 99 100 102 109 109 110 111 112 114 122 125 141 142

Index . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145

About the Authors

Mario Sarcinelli-Filho was born in João Neiva, ES, Brazil. He received the B.Sc. degree in Electrical Engineering from Federal University of Espírito Santo, Brazil, in 1979, and the M.Sc. and Ph.D. degrees, also in Electrical Engineering, from Federal University of Rio de Janeiro, Brazil, in 1983 and 1990, respectively. He is currently a Professor at the Department of Electrical Engineering, Federal University of Espírito Santo, Vitória, ES, Brazil, where he coordinated the Graduate Program on Electrical Engineering from 1992 to 1998 and from 2002 to 2012. He is also a researcher of the Brazilian National Council for Scientific and Technological Development (CNPq), an agency of the Brazilian Ministry of Science, Technology, Innovations and Communications, and senior editor of the Journal of Intelligent and Robotic Systems, edited by Springer Nature. He has co-authored more than peer-reviewed in indexed journals, more than 350 peer-reviewed conference papers, and 17 book chapters. He has also advised 21 Ph.D. students and 27 M.Sc. students. His research interests are nonlinear control, mobile robot navigation, coordinated control of mobile robots, unmanned aerial vehicles and coordinated control of ground and aerial robots (multi-robot systems).

xiii

xiv

About the Authors

Ricardo Carelli was born in San Juan, Argentina. He graduated in Engineering from the National University of San Juan, Argentina, and obtained a Ph.D. degree in Electrical Engineering from the National Autonomous University of Mexico (UNAM). He is a professor at the Instituto de Automática, Faculty of Engineering, National University of San Juan (Argentina), and senior researcher by contract with the National Council for Scientific and Technical Research (CONICET, Argentina). Professor Carelli has been Director of the Instituto de Automática, National University of San Juan from 2008 to 2019, and has coordinated the Graduate Program in Control Engineering from 2002 to 2022. He has advised 27 Ph.D. students and 13 M.Sc. students. He has published more than a hundred scientific articles in indexed journals on control and robotics. His research interests are on Robotics, Manufacturing Systems, Adaptive Control and Artificial Intelligence Applied to Automatic Control.

Chapter 1

Introduction

Robots have become part of the daily life. Manipulator robots, also known as anthropomorphic robots, for instance, are quite common in the automotive and naval industries, mainly in painting and welding, and it is difficult even to imagine an automotive or naval facility without such robots. Mobile robots have emerged more recently, adding flexibility in the accomplishment of several tasks, for their capability of moving. In fact, they have become quite powerful auxiliaries in housekeeping (the robotic vacuum cleaners) and as systems for interaction with people, performing as guides in large museums, for instance. Continuing with such an evolution, aerial robots have emerged, and are becoming quite useful in an increasing number of applications, such as in agriculture, load transportation and manipulation, and in the entertainment industry, for instance. Among the aerial robots, the rotary-wings type have become much useful, and the most commonly adopted are the quadrotors, rotary-wing robots with four motors. Specifically speaking of aerial robots, the so called drones, usually quadrotors, they are present in a lot of civilian applications, ranging from agricultural [14] to inspection [3] applications, including film making, advertisement, load transportation [12], load manipulation [5], package delivery [1, 10], and so on, besides several military applications. In other words, robots are much likely to stay between human beings from now on, becoming more and more important in improving our life quality. Specifically considering smaller and cheaper robots, it may be quite interesting to deal with a group of them [4, 6], instead of a single robot, depending on the task to be accomplished. In fact, using a group of robots one can overcome the deficiencies of the individual robots and make profit of their benefits [4]. In load transportation, for instance, the load shape can demand that more than one robot be used [11]. In other cases the payload capacity of a single robot is not enough to guarantee the success of the load transportation by a single robot [7], what is much likely to occur when using micro aerial vehicles, in particular small quadrotors. In this sense, the use of multi-robot systems has become strongly frequent, including the possibility of using heterogeneous formations, like those composed by a © The Author(s), under exclusive license to Springer Nature Switzerland AG 2023 M. Sarcinelli-Filho and R. Carelli, Control of Ground and Aerial Robots, Intelligent Systems, Control and Automation: Science and Engineering 103, https://doi.org/10.1007/978-3-031-23088-2_1

1

2

1 Introduction

Fig. 1.1 The robots used in the subsequent chapters. Source Personal photos

(a) Pioneer 3-DX

(c) AR.Drone 2.0

(b) Pioneer 3-AT

(d) Bebop 2

ground vehicle and an aerial one [2, 8]. Such a particular formation have been used in applications such as warehouse automation [13], package delivery [10], specifically in last mile delivery [2], and even in space exploration [9].1 Regarding all these topics, this book will address control proposals considering a single wheeled differential drive robot, a single quadrotor, a formation of various differential drive robots, a formation of various quadrotors, and a formation of a quadrotor and a differential drive robot, in this last case dealing with the last step of the last mile delivery, when the delivery drone should land back on a terrestrial vehicle. To illustrate the use of the proposed control systems, the well known wheeled differential drive robots Pioneer 3-DX and Pioneer 3-AT manufactured by Omron Adept Mobile Robotics, Inc., and the quadrotors AR.Drone 2.0 and Bebop 2, both manufactured by Parrot Drones SAS, illustrated in Fig. 1.1, are used in this text. In the sequel, the text is distributed according to the following contents: 1. Kinematic models, containing: a. a brief introduction to wheeled robots and models of omnidirectional, unicycle, differential, and car-like robots, besides an extended kinematic model for nonholonomic robots, b. a brief introduction to aerial and multirotor vehicles, besides their kinematic model (complete and simplified). 2. Dynamic models, including: a. a dynamic model for the differential drive robot with velocity inputs, and its inverse dynamic model,

1

See also https://mars.nasa.gov/technology/helicopter//#.

1 Introduction

3

b. parameterization and parameters identification for such a model, with an example considering the Pioneer 3-AT robot, c. model for the car-like robot, d. models for multi-rotor vehicles, complete and simplified, with experimental validation using the Bebop 2 quadrotor. 3. Motion Control, including: a. an introduction to motion control, b. the concepts of control and general hierarchical control structure, c. the basic control objectives: posture control, trajectory-tracking control and path-following control., d. inner-outer controllers, e. inverse kinematics control technique, f. trajectory-tracking control, with stability proof and experimental examples. The cases of differential and car-like robots and the case of multi-rotor robots, g. positioning control, with experimental examples, h. path following control, with stability proof and experimental examples, i. kinematic slipping compensation of non-holonomic robots, with stability proof and simulated examples, j. dynamic control, k. feedback linearization, l. inverse dynamics technique. Cascaded compensation. The cases of differential and multirotor vehicles, m. adaptive control, including the linear parameterization of the inverse dynamic model, and a control law with stability proof and experimental results as well, regarding the cases of differential drive and multi-rotor robots. 4. Control of multi-robot systems, including: a. concepts of coordinated and formation control, motivation and applications with examples taken from the web (with permission if pertinent), besides discussing a classification of formations and formation control strategies, b. characterization of a robot formation, c. leader-follower paradigm, d. virtual structure paradigm, i. cluster space versus robots space, ii. transformations, iii. movement Jacobian. e. control in the cluster space, regarding trajectory-tracking and path-following formation control, f. control dealing with multiple and conflicting control objectives, regarding a robot formation, based on the null space. Characterization of rigid and flexible formations. Obstacle avoidance, using potential fields and fictitious repulsion forces, considered in the null-space. Hybrid and sub-actuated formations. Scalability. Stability proof. Experimental examples, g. control and dynamic compensation in the robots space.

4

1 Introduction

References 1. Ackerman E, Koziol M (2019) The blood is here: Zipline’s medical delivery drones are changing the game in Rwanda. IEEE Spect 56(5):24–31. https://doi.org/10.1109/MSPEC.2019.8701196 2. Bacheti VP, Brandão AS, Sarcinelli-Filho M (2021) A path-following controller for a uavugv formation performing the final step of last-mile-delivery. IEEE Access 9:142218–142231. https://doi.org/10.1109/ACCESS.2021.3120347 3. Jordan S, Moore J, Hovet S, Box J, Perry J, Kirsche K, Lewis D, Tse ZTH (2018) State-of-theart technologies for UAV inspections. IET Radar, Sonar Navig 12(2):151–164. https://doi.org/ 10.1049/iet-rsn.2017.0251 4. Kumar V, Michael N (2012) Opportunities and challenges with autonomous micro aerial vehicles. Int J Robot Res 31(11):1279–1291. https://doi.org/10.1177/0278364912455954 5. Mohiuddin A, Tarek T, Zweiri Y, Gan D (2020) A survey of single and multi-uav aerial manipulation. Unmanned Syst 8(02):119–147 6. Parker LE, Rus D, Sukhatme GS (2016) Multiple mobile robot systems. In: Siciliano B, Khatib O (eds) Springer handbook of robotics, chap 53. Springer, Oxford, pp 1335–1384 7. Pizetta IHB, Brandão AS, Sarcinelli-Filho M (2019) Cooperative load transportation using three quadrotors. In: 2019 International conference on unmanned aircraft systems (ICUAS). Atlanta, GA, USA, pp 644–650. https://doi.org/10.1109/ICUAS.2019.8798175 8. Rabelo MFS, Brandão AS, Sarcinelli-Filho M (2021) Landing a uav on static or moving platforms using a formation controller. IEEE Syst J 15(1):37–45. https://doi.org/10.1109/JSYST. 2020.2975139 9. Rice M, Horgan B (2011) Nasa Perseverance rover’s first major successes on Mars—an update from mission scientists. ScitechDaily, 11 Nov 2021. https://scitechdaily.com/nasaperseverance-rovers-first-major-successes-on-mars-an-update-from-mission-scientists/ 10. Schneider D (2020) The delivery drones are coming. IEEE Spect 57(1):28–29. https://doi.org/ 10.1109/MSPEC.2020.8946304 11. Villa DKD, Brandão AS, Carelli R, Sarcinelli-Filho M (2021) Cooperative load transportation with two quadrotors using adaptive control. IEEE Access 9:129148–129160. https://doi.org/ 10.1109/ACCESS.2021.3113466 12. Villa DKD, Brandão AS, Sarcinelli-Filho M (2020) A survey on load transportation using multirotor UAVs. J Intell Robot Syst 98(2):267–296. https://doi.org/10.1007/s10846-019-01088w 13. Wawrla L, Maghazei O, Netland T (2019) Applications of drones in warehouse operations. ETH Zurich Whitepaper 14. Zhang C, Kovacs JM (2012) The application of small unmanned aerial systems for precision agriculture: a review. Precision Agric 13(6):693–712. https://doi.org/10.1007/s11119012-9274-5

Chapter 2

Kinematic Models

Abstract This chapter discusses the kinematic models of some wheeled mobile platforms and the aerial four motors helicopter-type vehicles. Such models show that commanding suitable velocities to the robots analyzed, the wheeled omnidirectional, unicycle and car-like robots and the aerial multirotor robot as well, causes velocities in the world frame, meaning that the commanded vehicles can move to any point in the navigation plane or in the navigation 3D space, in the cases of wheeled robots or aerial robots. Therefore, such models can be used to design automatic kinematic controllers, able to guide such robots in their navigation. As such controllers deal with the velocities of the robots ignoring the inertial effects, they are called kinematic controllers, and are responsible to move the vehicles in their working space. The design of such kinematic controllers, however, is not addressed here, but in Chap. 4.

2.1 Wheeled Robots Wheeled robots are the most common ground vehicles, driven and supported by wheels. There are many wheeled structures, including the holonomic ones, such as the omnidirectional robots, with special wheels that allow them to move in any direction, as well as non-holonomic robots, which have velocity constraints. Each structure presents a kinematic model defining the relationship between the robot velocities and the corresponding velocities in the absolute Cartesian plane, in which the motion should be described. In the following sub-sections the structures and kinematic models of the most commonly used wheeled robots are described. In addition to the wheeled mobile robots, aerial robots have also been used in many applications, mainly the rotary wings multirotor ones. Such vehicles are also addressed in this book, as well as the wheeled ones, so that their kinematic model are also presented, specifically considering quadrotors.

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2023 M. Sarcinelli-Filho and R. Carelli, Control of Ground and Aerial Robots, Intelligent Systems, Control and Automation: Science and Engineering 103, https://doi.org/10.1007/978-3-031-23088-2_2

5

6

2 Kinematic Models

2.1.1 Omnidirectional Robots Omnidirectional robots are ground vehicles capable of moving in any direction in the working plane. This is achieved by means of using specially designed wheels, which allow to move in longitudinal or transversal directions. An example of omnidirectional robot is shown in Fig. 2.1, while Fig. 2.2 shows two wheel models used in omnidirectional robots, as well as some different patterns of wheel distribution in the robot platform. In addition to such figures, Fig. 2.3 shows a sketch characterizing the movement of such a kind of robot, regarding an omnidirectional platform of three wheels. Notice that there are at least two more possible options to define the axis X r of the robot frame in the left part of Figs. 2.2 and 2.3. They correspond to the one shown in Fig. 2.2 rotated 120◦ to the left or to the right. To define the axis Y r , in each case, the right hand rule should be adopted, remembering that the figures shown correspond to bottom views. Assuming a linear velocity with magnitude u 0 , as characterized in Fig. 2.3, it can be decomposed in the longitudinal and transversal components, regarding the axes X r and Y r shown, (2.1a) u = u 0 cos δ and v = u 0 sin δ,

(2.1b)

or, in other words, the linear velocity u 0 is the vector sum of the velocities u and v. Therefore,  u 0 = u 2 + v2 , (2.1c) and

v δ = arctan . u

(2.1d)

Including the rotational velocity ω, the robot kinematics, giving the velocities of the central point of the robot in the Cartesian frame, characterized as (xc , yc ) in Fig. 2.3, can be expressed as

Fig. 2.1 Examples of four-wheeled omnidirectional robots. Source https://www. neobotix-robots.com/ products/mobile-robots

2.1 Wheeled Robots

7

Fig. 2.2 The most common types of omnidirectional wheels and two possible wheel distribution for an omnidirectional robot. Sources a Imetron GmbH, https://commons.wikimedia.org/w/index. php?curid=2861318, and b http://www.arhan-japan.com/material_handling.php Fig. 2.3 Schematics of an omnidirectional robot with three wheels

8

2 Kinematic Models

x˙c = u cos ψ − v sin ψ,

(2.2a)

y˙c = u sin ψ + v cos ψ,

(2.2b)

ψ˙ = ω,

(2.2c)

or, in a compact form, ⎡ ⎤ ⎡ x˙c cos ψ − sin ψ x˙ = ⎣ y˙c ⎦ = ⎣ sin ψ cos ψ 0 0 ψ˙

⎤⎡ ⎤ 0 u 0⎦ ⎣ v ⎦ = Av, 1 ω

(2.3)

where x represents the state vector, v is the vector of input commanded velocities, and A represents the matrix of direct kinematics. There also exists a relationship between the angular rotations of the wheels, ωi , and the input velocities to the robot. Assuming the wheels have a radius r , the tangential velocity for the ith wheel will be vi = r ωi . Then, the linear velocity u0 of the robot is obtained by the vector sum of the tangential velocities vi . In addition, the angular velocity of the robot is the sum of the tangential velocities vi associated to each wheel divided by the distance d from the wheel to the center of the robot.

2.1.2 Unicycle (Differential Drive) Robots A unicycle robot is a differential drive mobile platform containing two wheels driven by independent motors plus a castor wheel, just to keep the equilibrium of the platform. The two driven wheels can develop different rotations, since each one is generally driven by an electrical motor, excited by a suitable voltage, generated by PWM signals. An additional detail is that the two wheels can rotate clockwise or counterclockwise, through using two electronic circuits called H bridge, which allow supplying current to each motor in both directions, so that the robot can move forward and backward. As a result of the rotation of the two wheels, distinct velocities are obtained in the point the wheels touch the ground, generating the ground velocities v1 and v2 for the wheels. Consequently, the mobile platform develops a linear forward velocity, u, and an angular one, ω, as illustrated in Fig. 2.4, what makes it quite similar to a real mono-cycle, also illustrated in the figure, for comparison. Such velocities are defined as v1 + v2 (2.4a) u= 2 and ω=

v2 − v1 , D

(2.4b)

2.1 Wheeled Robots

9

Fig. 2.4 Kinematics of the unicycle wheeled mobile robot: the analogy with a real mono-cycle

where D is the distance between the centers of the two driven wheels, ω is positive counterclockwise, and v1 and v2 are the tangential velocities correspondent to the left and right wheels, respectively. The movement of the point of interest for control in the X w Y w plane, in this case the point in the middle of the virtual axle linking the driven wheels, of coordinates (xc , yc ), is characterized by (2.5a) x˙c = u cos ψ,

and

y˙c = u sin ψ,

(2.5b)

ψ˙ = ω,

(2.5c)

or, in a compact way ⎡ ⎤ ⎡ x˙c cos ψ x˙ = ⎣ y˙c ⎦ = ⎣ sin ψ 0 ψ˙

⎤ 0   u 0⎦ = Av, ω 1

(2.6)

where x is the state vector, v is the vector of commanded velocities and A is the matrix of direct kinematics. As one can see, when considering just the linear velocities for control design purposes, the matrix is not invertible. Therefore, as it will be detailed ahead in this text, it prevents using a controller designed adopting the technique of inverse kinematics, because it demands the inversion of the matrix A. To overcome this drawback, next section discusses an extended kinematic model, which allows generating a matrix of direct kinematics that is always invertible, does not mattering where the point of interest for control is notice that regarding the point of interest for control as the point in the middle of the virtual axle linking the driven wheels, as

10

2 Kinematic Models

depicted in Fig. 2.4, there is no velocity in this direction, meaning that the unicycle robot is a nonholonomic one, which is a terminology used to characterize a vehicle not able to develop velocity along the axis Y r . This can be checked taking the projections of the velocities x˙c and y˙c over the axis Y r , which will result in x˙c cos(90 − ψ) − y˙c cos ψ = 0,

(2.7)

taking into account that cos(90 − ψ) = sin ψ and considering (2.5).

2.1.3 Car-like Robots A car-like robot is a wheeled mobile platform with four wheels. Two of them, located in the backward part of the chassis, as depicted in Fig. 2.5, are driven by a single motor, so that the whole platform moves ahead when the front wheels are aligned to the chassis body. The two front wheels are responsible for steering the chassis, as sketched in the figure, what is done rotating such wheels to the left and right, characterizing the robot steering. However, such angles, δ1 and δ2 , are bounded to small values, what also happens with a real automobile. Regarding Fig. 2.5b, one can see that the movement of the point of interest for control, the small yellow circle in the figure, corresponds to a forward velocity u, the velocity of the two linked back wheels, with a steering imposed by the angles δ1 and δ2 . Such angles are calculated, to have a unique rotation point P, as δ1 = arctan

and δ2 = arctan



L r−



L r+

(2.8a)

lw 2

lw 2

,

(2.8b)

showing that small values of δ1 and δ2 cause large values of r, such that one can approximate these two values by a single one, given by δ = arctan

 L . r

(2.9)

In addition, for the cases in which L >> lw one can consider the car-like robot represented as in Fig. 2.5e, which is a representation quite similar to a real bicycle, shown in Fig. 2.5f.

2.1 Wheeled Robots

11

Fig. 2.5 A real car-like robot, also known as Ackerman structure, the sketches used to develop its model, and the analogy with a bicycle

12

2 Kinematic Models

The movement of the point of interest for control of the structure of Fig. 2.5e, the small yellow circle shown, in the X w Y w plane is characterized by x˙c = u cos(ψ),

(2.10a)

y˙c = u sin(ψ),

(2.10b)

u tan(δ) , L

(2.10c)

and ψ˙ =

considering (2.9) and the fact that for δ constant the vehicle follows a circumference of radius r with the velocity u tangent to the circumference, such that u is given ˙ Thus, ψ˙ = u = u tan δ . One important detail is that such a robot is not by u = ψr. r L able to change its orientation ψ without moving ahead. In other words, it only can maneuver when moving with a nonzero u velocity. An important aspect when regarding a car-like robot is that it is also a nonholonomic robot, as well as the unicycle robots, in opposition to the omnidirectional ones. The nonholonomic constraints are characterized by null velocities in the directions normal to the directions of its wheels. Considering the two back wheels, such a direction is the one of the axle linking them, as their orientation is fixed. The sum of the projections of the velocities x˙c and y˙c over such axle results in x˙c sin ψ − y˙c cos ψ = 0,

(2.11)

since x˙c = u cos ψ and y˙c = u sin ψ. Similarly, regarding the direction of the axle correspondent to the two front wheels one has x˙ f sin(ψ + δ) − y˙ f cos(ψ + δ) = 0,

(2.12)

where the subscript f stands for the front wheels. Therefore, the car-like robot has velocity constraints, corresponding to the impossibility of moving in the direction normal to the orientation of each pair of wheels.

2.1.4 Extended Kinematics for Nonholonomic Robots The extended kinematics model here discussed applies for nonholonomic robots, such as the differential drive and car-like ones. For the differential-drive robots, for instance, it is not possible to consider the point of interest for control as being on the virtual axle linking the driven wheels, when designing a controller based on the inverse kinematics. This happens because the correspondent matrix of direct kinematics becomes is singular, not having an inverse, necessary to use the inverse kinematics approach to design a controller.

2.1 Wheeled Robots

13

Fig. 2.6 A sketch to get the extended kinematics of the nonholonomic robot

Then, a possible solution to get a nonsingular matrix of direct kinematics independently of where the point of interest for control is corresponds to suppose an holonomic platform, having linear velocities in the axes X r and Y r of the robot frame, besides angular velocity around its axis Z r , as depicted in Fig. 2.6. In such a figure the point of interest for control is defined by the coordinates (xc , yc ), and can be located at any position in the world frame X w Y w , including out of the robot body, but, in this case, rigidly coupled to it. In Fig. 2.6 it is at a distance a of the point in the middle of the virtual axis linking the driven wheels, forming an angle α with the axis X r . The robot is supposed to be able to develop linear velocities u and v, respectively in the axes X r and Y r , and an angular velocity ω around the axis Z r , positive counterclockwise (the right hand rule applies). Therefore, the movement of the point (xc , yc ) is associated to the velocities u, v and aω, the tangential velocity in such a point generated by the angular movement of the robot. As for the angle ψ, it represents the current robot orientation. It is worth noting that v is here assumed to be nonzero while formulating the extended model. It will then be set to zero or set to a measured nonzero slipping value when designing the kinematic controllers in Chap. 4. The movement of the point of interest for control, the point of coordinates (xc , yc ), is then caused by the velocity components u, v and aω, so that the velocity components in the world frame X w Y w are

and

x˙c = u cos(ψ) − v cos(90 − ψ) − aω sin(α + ψ),

(2.13a)

y˙c = u sin(ψ) + v cos(ψ) + aω cos(α + ψ),

(2.13b)

ψ˙ = ω,

(2.13c)

14

2 Kinematic Models

taking into account that the angular velocity developed by the point of interest for control is the same of the robot, the angle between the velocity vector aω and the axis Y w is the sum of ψ and α and the angle between the velocity vector v and the axis X w is 90 − ψ, both in degrees. In a compact notation one has the extended kinematic model for the differential robot [1] ⎤⎡ ⎤ ⎡ ⎤ ⎡ x˙c cos ψ − sin ψ −a sin(α + ψ) u ⎦ ⎣ ⎦ ⎣ ⎣ y ˙ sin ψ cos ψ a cos(α + ψ) v ⎦ = Av, = (2.14) x˙ = c ˙ 0 0 1 ω ψ where x is the state vector, v is the vector of commanded velocities (regarding the hypothetical possibility of a velocity component in the axis Y r of the robot frame) and ⎡ ⎤ cos ψ − sin ψ −a sin(α + ψ) A = ⎣ sin ψ cos ψ a cos(α + ψ) ⎦ (2.15) 0 0 1 is the matrix of direct extended kinematics, whose determinant is always the unity, it does not matter where the point (xc , yc ) is, since rigidly linked to the robot. From such a model one can write ⎡ ⎤⎡ ⎤ x˙c cos ψ sin ψ a sin α (2.16) v = A−1 x˙ = ⎣− sin ψ cos ψ −a cos α ⎦ ⎣ y˙c ⎦ , 0 0 1 ψ˙ ⎡

⎤ cos ψ sin ψ a sin α where A−1 = ⎣− sin ψ cos ψ −a cos α ⎦ is the matrix of inverse extended kinemat0 0 1 ics, which always exists, since A in (2.15) is nonsingular for any a, α, and ψ. It is worthwhile to stress, nonetheless, that although the point (xc , yc ) in Fig. 2.6 has velocity in the Y r direction, the mobile robot is still a nonholonomic one. Moreover, as it will be shown ahead, such an extended kinematic model includes the case of Fig. 2.4, which corresponds to a = 0 in Fig. 2.6. Now, let us consider the extended kinematic model for the car-like robot, represented in a simplified way as a bicycle, as shown in Fig. 2.7. In this figure the point of interest for control is defined by the coordinates (xc , yc ), is located at a distance a of the point in the middle of the axle linking the rear wheels, and forms an angle α with the body axis of the vehicle. The movement of this point of interest for control is then caused by the velocity components u, v and aωψ . Following a similar reasoning as for the differential-drive robot, but including the orientation δ of the driving front wheel as an additional state variable (see Fig. 2.7), the extended differential kinematics in compact form is given by

2.1 Wheeled Robots

15

Fig. 2.7 A sketch to get the extended kinematics of the car-like nonholonomic robot

⎡ ⎤ ⎡ x˙c cos ψ − sin ψ −a sin(α + ψ) ⎢ y˙c ⎥ ⎢ sin ψ cos ψ a cos(α + ψ) ⎥ ⎢ ⎢ x˙ = ⎣ ˙ ⎦ = ⎣ ψ 0 0 1 0 0 0 δ˙

⎤⎡ ⎤ 0 u ⎢ ⎥ 0⎥ ⎥ ⎢ v ⎥ = Av, 0⎦ ⎣ωψ ⎦ 1 ωδ

(2.17)

where x is the state vector, v is the vector of commanded velocities (regarding the hypothetical possibility of a velocity component in the axis Y r of the robot frame) and ⎡ ⎤ cos ψ − sin ψ −a sin(α + ψ) 0 ⎢ sin ψ cos ψ a cos(α + ψ) 0⎥ ⎥ A=⎢ (2.18) ⎣ 0 0 1 0⎦ 0 0 0 1 is the matrix of direct differential kinematics, whose determinant is always the unity, it does not matter where the point (xc , yc ) is, since rigidly linked to the robot. From such a model one can write ⎡ ⎤⎡ ⎤ x˙c cos ψ sin ψ a sin α 0 ⎢− sin ψ cos ψ −a cos α 0⎥ ⎢ y˙c ⎥ −1 ⎥⎢ ⎥, (2.19) v = A x˙ = ⎢ ⎣ 0 0 1 0⎦ ⎣ ψ˙ ⎦ 0 0 0 1 δ˙ where A−1 is the matrix of inverse extended kinematics, which always exists, since A in (2.18) is guaranteed to be nonsingular.

16

2 Kinematic Models

2.2 Aerial Multirotor Robots A multirotor aerial robot is a helicopter-type vehicle having various propellers, each one corresponding to a motor with shaft perpendicular to the vehicle plane, to which a blade is connected. This way, each set motor-blade generates a propulsion in the normal direction, with respect to the body of the vehicle. The most common of them are the quadrotors, or those possessing four motors, although configurations with six motors (hexarotor) and eight motors (octorotor) are also available. This book, however, will focus just in the four-motors configurations. Such vehicles have been designed and operated as unmanned aerial vehicles, the so called UAVs, meaning that they were thought to fly autonomously. As a consequence of the ability to fly autonomously, the use of UAVs in civil and military applications has become more and more frequent. Just to exemplify some civil applications, multirotor UAVs have been used in agriculture (pest control, plant count and growth stage monitoring), commerce (cargo delivery and warehouse automation), entertainment (film making and sports coverage), disaster response (search and rescue, fire detection and lifebuoy delivery), among many others (see, for instance, [2]). Therefore, a quadrotor is a helicopter-type aerial vehicle with four vertical propellers. To avoid the rotation of the vehicle body when the motors are working, any two adjacent motors should rotate in opposite directions, one clockwise and the other counterclockwise. As a result, the vehicle body stays in a fixed orientation, regarding the world frame, when the four motors develop the same velocity. Such a vehicle can move up- and downwards, for- and backwards, and to the left or the right, besides being able to rotate around the three axes of the Cartesian space, as illustrated in Fig. 2.8. This means that a quadrotor is a real omnidirectional vehicle, since it can move in any direction of the Cartesian 3D space (remember that the omnidirectional wheeled robots are restricted to navigate in a plane). Therefore, a quadrotor has six degrees of freedom (linear displacements in the x w , y w and z w axes, and rotations around the same axes). In such a figure one can perceive the detail of the direction of rotation of each one of the four motors, which generate the moments M1 to M4 , whose resultant is responsible for rotating the body of the robot around the axis z b , thus defining the yaw angle ψ of the vehicle. Notice that the axes x b , y b and z b correspond to the so called body frame, which is a frame of coordinates whose origin is the center of gravity of the vehicle. By their turn, the sum of the torques correspondent to the forces f 1 to f 4 defines the rotation of the vehicle body around the axes x b and y b , thus defining the pitch angle θ and the roll angle φ of the vehicle, with reference to the body frame. From Fig. 2.8 one can perceive that the torque that causes a rotation counterclockwise around the axis y b is ( f 3 + f 4 − f 1 − f 2 )lb , where lb is the distance from each propeller to the axis y b . Notice that if all the motors are with the same rotation there will be no rotation around the axis y b . Thus, to change this it is necessary to increase the velocities of the motors 3 and 4. On the other hand, the torque necessary to rotate the vehicle body clockwise around the axis x b is given by

2.2 Aerial Multirotor Robots

17

Fig. 2.8 The degrees of freedom and input commands for a quadrotor (developed over a personal photo)

( f 1 + f 4 − f 2 − f 3 )lb (lb is the same for all the propellers, considering the symmetry of the structure). For angles θ and φ different of zero there is a difference in the velocities of the four motors. Therefore, to control the quadrotor corresponds to impose a suitable rotation to each motor. For instance, to move the vehicle upwards it is necessary to increase the rotation speed of all the motors, which should also be equal, to avoid any inclination. An important detail is that all the motors generate propulsion in the direction normal to the plane x b y b , so that there is no force, therefore no acceleration, and consequently no movement, in the x b and y b directions. To create force in the x b direction, and thus move the quadrotor forward, it is necessary to make f 3 + f 4 greater than f 1 + f 2 , thus rotating the vehicle body counterclockwise around the axis y b . The way to create velocity in the y b direction is quite similar, now generating a rotation around the axis x b . This way, the linear velocity in the direction x b is coupled to the pitch angle θ, and the linear velocity in the direction y b is coupled to the roll angle φ. As a result, the quadrotor has six degrees of freedom, but only four input commands, thus being an underactuated system. For the quadrotors AR.Drone 2.0 and Bebop 2, in particular, these commands are related to the linear velocity along the axis z b (u z b ), the angular velocity around the axis z b (u ψ˙ ), the pitch angle (u θ ), and the roll angle (u φ ), which are also represented in Fig. 2.8. Regarding the quadrotors AR.Drone 2.0 and Bebop 2 used in this book, such commands are normalized to be within the interval from −1 to +1, as required by the manufacturer [3, 4]. Another detail is that for these two quadrotors the basic structure is an X, regarding the position of the four propellers with respect to the axes x b and y b . As a consequence, as one can see in Fig. 2.8, each propeller is in one of the quadrants of the plane x b y b , in an angle of 45◦ with each axis of the respective quadrant. There are other quadrotor structures for which two propellers are over the axis x b and the other two are over the axis y b , however, but they are not discussed in this text.

18

2 Kinematic Models

To represent the movement of the point correspondent to the center of gravity of the quadrotor in the axes of the world frame one needs to know its attitude, which corresponds to the instantaneous values of the angles θ, φ and ψ. With these angles the linear velocities along the axes x w , y w and z w , as well as the angular velocities ˙ can be determined. θ˙ , φ˙ and ψ, Now, assuming that the body structure of the quadrotor is symmetric and rigid, the propellers have a fixed pitch angle, and the center of mass coincides with the geometric center of the vehicle, the matrix Rbw that gives the orientation of the vehicle in the world frame as a function of the instantaneous values of the angles θ, φ and ψ can be obtained. It is calculated as the premultiplication of a sequence of the single rotation matrices Rx (φ), R y (θ ), and Rz (ψ) corresponding to the angles φ, θ and ψ (see, for instance, [5, 6]), given by ⎡

⎤ 1 0 0 Rx (φ) = ⎣0 cφ −sφ ⎦ , 0 sφ cφ ⎡

⎤ cθ 0 sθ R y (θ ) = ⎣ 0 1 0 ⎦ , −sθ 0 cθ and

⎡

⎤ cφ −sφ 0 Rz (ψ) = ⎣sφ cψ 0⎦ , 0 0 1

where cx and sx stand for cos x and sin x, respectively. Therefore, the rotation matrix Rbw is ⎤ cθ cψ cψ sθ sφ − sψ cφ cφ sθ cψ + sφ sψ Rbw = Rz (ψ)R y (θ )Rx (φ) = ⎣sψ cθ sφ sθ sψ + cφ cψ sθ sψ cφ − cψ sφ ⎦ . −sθ cθ sφ cθ cφ ⎡

(2.21)

The differential kinematics gives the relationship between velocities in the body and the world frames. When regarding the linear velocities, it is given by ⎡

⎤ ⎡ b⎤ vx x˙ w x˙ w = ⎣ y˙ w ⎦ = Rbw ⎣v by ⎦ , y˙ w vzb

(2.22)

where vxb , v by and vzb are the linear velocities in the body frame of the quadrotor. As for the angular velocities, the differential kinematics is

2.2 Aerial Multirotor Robots

19

⎡

⎤ ⎡ b⎤ φ˙ w φ˙ w w⎦ ⎣ ⎣ ˙  = θ = H θ˙ b ⎦ , w ˙ ψ ψ˙ b

(2.23)

where w is the vector of angular velocities in the world frame, and H is given by ⎡

⎤ 1 sin φ tan θ cos φ tan θ cos φ − sin φ ⎦ . H = ⎣0 0 sin φ/ cos θ cos φ/ cos θ

(2.24)

Regarding a near hover situation, which means that the quadrotor is practically hovering (small inclination angles θ and φ, corresponding to low linear velocities ahead and aside), one can consider that the quadrotor stays practically in the horizontal position, or the plane x b y b of the body frame is parallel to the plane x w y w of the world frame. In such case the axes z w and z b are in parallel, whereas the axes x w and x b establish the angle ψ, the yaw angle of the vehicle. Notice that in such a case just the angles θ and φ are close to zero, whereas the angle ψ is not bounded. Therefore, the axes y w and y b also form the angle ψ. Under such an assumption, the quadrotor can be seen as a horizontal platform able to develop linear velocities in the axes x b , y b and z b , besides angular velocity around the axis z b (yaw velocity). Notice that this is an approximation, associated to the assumption that the angles θ and φ are close to zero. Indeed, it is not possible to have velocities in the directions x b and y b for θ = 0 and φ = 0. Nonetheless, as we are limiting such angles to a value close to zero, although not exactly zero, it is acceptable to consider the body of the quadrotor always in the horizontal position, although having low velocities in x b and y b . For the examples considered in the rest of this book, either for the AR. Drone 2.0 or the Bebop 2 quadrotors, the angles θ and ψ are limited to ±15◦ , what is possible through the configurations made available in their embedded autopilot [4] (practically all the commercially available quadrotors have such an embedded autopilot, so that the simplification here considered can be adopted for the majority of the commercially available quadrotors). Besides this, as the dynamics associated to the pitch and roll angles are quite faster than the dynamics associated to the linear displacement of the robot in the correspondent directions, it is possible to consider that just velocity commands, T  grouped in the vector u = u vx u v y u vz u vψ , whose entries are all limited to the bounds ±1, are given as inputs to the quadrotor, once the pitch and roll angles be limited to small absolute values. Assuming a near-hovering navigation of the quadrotor, Fig. 2.9 depicts the configuration of the axes of the global and body frames, x w y w z w and x b y b z b . There the axes z w and z b are in parallel, both coming out of the book page, perpendicularly to it. From such a figure one can write that x˙ w = vxb cos ψ + v by cos(90 + ψ) = vxb cos ψ − v by sin(90 + ψ),

(2.25a)

20

2 Kinematic Models

Fig. 2.9 Axes configuration for the global and body frames for a quadrotor in a near-hover condition (the two black circles correspond to the arrows of the axes z b and z w )

y˙ w = vxb sin ψ + v by cos ψ,

(2.25b)

z˙ w = vzb ,

(2.25c)

ψ˙ w = ψ˙ b ,

(2.25d)

regarding that cos(90 + ψ) = − sin ψ, where the superscripts b and w refers to the body and global frames, respectively, v represents linear velocities, ψ is the yaw angle and ψ˙ is the yaw velocity. Notice that such relationships can also be deduced from (2.22) and (2.23) considering cos φ ≈ 1, cos θ ≈ 1, sin φ ≈ 0, sin θ ≈ 0, and tan θ ≈ 0. Written in a compact way, such a set of equations becomes ⎤ ⎡ x˙ w cos ψ − sin ψ w⎥ ⎢ ⎢ y ˙ ⎥ ⎢ sin ψ cos ψ x˙ w = ⎢ ⎣ z˙ w ⎦ = ⎣ 0 0 0 0 ψ˙ w ⎡

0 0 1 0

⎤⎡ ⎤ vx b 0 ⎢v y b ⎥ 0⎥ ⎥ ⎢ ⎥ = Avb , 0⎦ ⎣ v z b ⎦ 1 ψ˙ b

(2.26)

where xw is the state vector, in global coordinates, vb is the vector of velocities in the body frame, and A is the matrix of direct kinematics correspondent to the quadrotor, regarding it is flying in a near hover condition.

2.3 Concluding Remarks The kinematic models here addressed show that commanding suitable velocities to the robots analyzed, the wheeled omnidirectional, unicycle and car-like robots and the aerial multirotor robots as well, causes velocities in the world frame, meaning

2.3 Concluding Remarks

21

that the commanded vehicles can move to any point in the navigation plane or in the navigation 3D space, in the cases of wheeled robots or aerial robots. Therefore, such models can be used to design automatic kinematic controllers [7–9], or controllers able to guide such robots in their navigation. As such controllers deal with the velocities of the robots ignoring the inertial effects, they are called kinematic controllers, and are responsible to move the vehicles in their working space. The design of such kinematic controllers is the topic of Chap. 4. The technique explored to design such controllers is called feedback linearization [10, 11], also discussed in Chap. 4, whose essence is to generate a control loop whose characteristic equation describes a linear temporal evolution of the error between the current robot position and the desired one, for instance. This means that the nonlinear kinematics associated to the robots analyzed in this chapter, represented by the matrix of direct kinematics, should be inverted and incorporated in the control loop, thus compensating for the nonlinear kinematics, associated to a desired linear behavior for such error. Thus, such a control technique is also called inverse kinematics [12– 15]. A main innovation in relation to the non-holonomic wheeled robots has been the introduction in this chapter of the “extended kinematic models”, which guarantees the kinematic matrix to be always invertible, and thus allowing the inverse kinematics design for any position of the point of interest on the robot.

References 1. Montesdeoca JC, Herrera D, Toibero JM, Carelli R (2017) Controllers design for differential drive mobile robots based on extended kinematic modeling. In: 2017 European conference on mobile robots (ECMR), pp 1–6. https://doi.org/10.1109/ECMR.2017.8098661 2. Chen S, Laefer DF, Mangina E (2016) State of technology review of civilian UAVs. Recent Pat Eng 10(3):160–174. https://doi.org/10.2174/1872212110666160712230039 3. Pinto AO, Marciano HN, Bacheti VP, Moreira MSM, Brandão AS, Sarcinelli-Filho M (2020) High-level modeling and control of the Bebop 2 micro aerial vehicle. In: Proceedings of the 2020 international conference on unmanned aircraft systems. IEEE, Athens, Greece, pp 939– 947. https://doi.org/10.1109/ICUAS48674.2020.9213941 4. Santana LV, Brandão AS, Sarcinelli-Filho M (2016) Navigation and cooperative control using the AR. Drone quadrotor. J Intell Robot Syst 84(1):327–350 (2016). https://doi.org/10.1007/ s10846-016-0355-y 5. Arvo J (1992) Fast random rotation matrices. In: Kirk D (ed) Graph Gems III. Academic Press, San Diego, pp 117–120 6. Sciavicco L, Siciliano B (2000) Modelling and Control of Robot Manipulators. Springer-Verlag, London, England. https://doi.org/10.1007/978-1-4471-0449-0 7. Khai TQ, Ryoo YJ (2019) Design of adaptive kinematic controller using radial basis function neural network for trajectory tracking control of differential-drive mobile robot. Int J Fuzzy Logic Intell Syst 19(4):349–359. https://doi.org/10.5391/IJFIS.2019.19.4.349 8. Lugo-Cárdenas I, Salazar S, Lozano R (2017) Lyapunov based 3d path following kinematic controller for a fixed wing UAV. IFAC-PapersOnLine 50(1):15946–15951. https://doi.org/10. 1016/j.ifacol.2017.08.1747 20th IFAC World Congress 9. Pazderski D, Szulczy´nski P, Kozłowski KR (2009) Kinematic tracking controller for unicycle mobile robot based on polar-like representation and Lyapunov analysis. In: Kozłowski KR (ed) Robot motion and control. Springer, London, pp 45–56 https://doi.org/10.1007/978-1-84882985-5_5

22

2 Kinematic Models

10. Khalil HK (2002) Nonlinear systems, 3 edn. Prentice Hall 11. Montoya-Villegas L, Moreno-Valenzuela J, Pérez-Alcocer R (2019) A feedback linearizationbased motion controller for a UWMR with experimental evaluations. Robotica 37(6):1073– 1089. https://doi.org/10.1017/S0263574718001443 12. Antonelli G (2009) Stability analysis for prioritized closed-loop inverse kinematic algorithms for redundant robotic systems. IEEE Trans Robot 25(5):985–994. https://doi.org/10.1109/ TRO.2009.2017135 13. Geoffroy P, Mansard N, Raison M, Achiche S, Todorov E (2014) From inverse kinematics to optimal control. In: LJ, KO (eds) Advances in robot kinematics. Springer, Cham, pp 409–418. https://doi.org/10.1007/978-3-319-06698-1_42 14. Hakamata Y, Tsuichihara S, Ricardez GAG, Takamatsu J, Ogasawara T (2020) Pushing and pulling motion generation for humanoid robots using whole-body momentum control based on analytical inverse kinematics. Adv Robot 34(21–22):1442–1454. https://doi.org/10.1080/ 01691864.2020.1817778 15. Siciliano B (1990) A closed-loop inverse kinematic scheme for on-line joint-based robot control. Robotica 8(3):231–243. https://doi.org/10.1017/S0263574700000096

Chapter 3

Dynamic Models

Abstract This chapter addresses the dynamic models correspondent to various mobile vehicles. The kinematic models so far addressed do not consider the effects of the mass and inertia of the vehicles on how their velocities vary till reaching the commanded values. In other words, in the real world a velocity reference sent to a robot demands a certain time to be fully developed, what characterizes a velocity tracking error, or a difference between the commanded and the effective velocities. This is due to the dynamic behavior of the vehicle, which is here addressed.

3.1 Modeling a Unicycle Robot with Velocity Inputs Most of the literature considers the dynamic model of a differential-drive robot when the input are the torques applied to the wheels. However, commercial versions of such robots, such as the Pioneer P3 series (P3-DX and P3-AT models), usually receive velocities as inputs, which are applied to the internal servos. Specifically, they should receive linear velocity (in the axis X r ) and angular velocity (around the axis Z r ) as input commands, and with the suitable values of such commands this kind of vehicle can reach any point of its navigation plane. Considering the kinematic model described in Sect. 2.1.2, one can see that from the velocity commands applied, the position of the vehicle can be changed accordingly. However, as the robot has a nonzero mass, and consequently a meaningful inertia, the velocities really developed by the robot are normally different of the commanded ones. This characterizes what is called the velocity-tracking error. The reason for this is that the dynamics of the vehicle has not been considered. This means that it is necessary to take into account that the commanded velocity is reached through a transient process, which depends on the dynamics associated to the vehicle and its actuators. Therefore, in many applications it is necessary to consider this dynamics. The dynamic model presented here includes the dynamics of the internal servoactuators, and therefore admits reference velocities as inputs, with the advantages described above. For an electrically driven differential robot, as the Pioneer P3 family, for instance, considering the Newton dynamic equations and P-D electrically driven servos, as developed in [8, 16, 17], one can write the dynamic model of the differential robot as © The Author(s), under exclusive license to Springer Nature Switzerland AG 2023 M. Sarcinelli-Filho and R. Carelli, Control of Ground and Aerial Robots, Intelligent Systems, Control and Automation: Science and Engineering 103, https://doi.org/10.1007/978-3-031-23088-2_3

23

24

3 Dynamic Models

   θ3 2 θ4   1      ω − θ1 u 0 ur e f u˙ δ θ1 θ1 + u , = + δω ω˙ 0 θ12 ωr e f − θθ5 uω − θθ6 ω 2

(3.1)

2

 T where the vector θ = θ1 θ2 θ3 θ4 θ5 θ6 contains the parameters associated to the dynamics of the vehicle, which can be experimentally identified, as it will be discussed ahead, u and ω are the linear and angular velocities effectively developed by T  the vehicle, u r e f and ωr e f are the commanded velocities, and the vector δ = δu δω characterizes disturbances, such as misidentified parameters, bad velocity measurements and so on. As for the entries of the vector θ, they are characterized as [8]  θ1 =  θ2 =

 1 Ra  2 [s], mr + 2Ie + 2r k DT ka (2r k P T )

  Ra  2 1 2 2 [s], Ie d + 2r Iz + mb + 2r dk D R ka dk (2r P R ) θ3 =



Ra θ4 = ka θ5 = Ra θ6 = ka

Ra mbr [sm, /rad2 ], ka 2k P T ka kb + Be Ra



1 + 1, r kPT

Ra mbr [s/m], and ka dk P R



ka kb + Be Ra



d + 1, 2r k P R

which includes physical parameters of the robot, such as its mass m, inertia Iz , regarding the center of mass of the vehicle, friction coefficient Be , the radius r of the wheels, the distance from the wheels axis to the center of mass b, and others, such as the electrical resistance Ra of its motors, the electromotive constant kb of its motors multiplied by the gear ratio, the constant of torque ka of its motors multiplied by the gear ratio, and the PD servo gains k P T , k DT , k P R , k D R for translational and rotational velocities. As one can see from these equations it is extremely difficult to determine all these values, and thus the precise value of the parameters θi , i = 1, . . . , 6. This is why it is much more practical to identify such values, as it will be discussed ahead. It should be noticed that θi > 0 for i = 1, 2, 4, 6, whereas the parameters θ3 and θ5 can be negative or null, this last case corresponding to the case in which the point of interest for control is in the center of the virtual axle (which means a = 0 in Fig. 2.6). Therefore, (3.1) shows how the current velocities u and ω of the differentialdrive robot evolve along time to reach the given references, thus characterizing the dynamics of the vehicle.

3.1 Modeling a Unicycle Robot with Velocity Inputs

25

From (3.1) one can explicit the values of the reference velocities u r e f and ωr e f , obtaining the inverse dynamic model for the differential-drive robot as            −θ1 0 δu θ1 0 u˙ θ4 −θ3 ω u ur e f + , + = ωr e f 0 −θ2 δω 0 θ2 ω˙ θ5 ω θ6 ω

(3.2)

or, in a compact form,  + Hv˙ + C(v)v = vr

(3.3)

T   T where vr = u r e f ωr e f is the vector of reference velocities, v = u ω is the vector containing the current robot velocities. As for the matrices H and C(v) and the vector , they are given by  H=

 θ1 0 , 0 θ2



 θ4 −θ3 ω C(v) = , θ5 ω θ6 

and

−θ1 0 = 0 −θ2

  δu . δω

An important aspect to be stressed, regarding the vector of parameters θ characterizing the dynamic model of the differential-drive robot, is that it is not possible to consider less than six parameters, as it is shown in [17].

3.1.1 Model Properties To allow checking some properties of the model in (3.2) and (3.3), the matrix C(v) is now rewritten by adding and subtracting the term θ3 u to its entry in the second row and second column, such that it becomes   −θ3 ω θ4 C(v) = , (3.4) θ5 ω θ6 + (θ3 − θ3 )u and thus the term C(v)v of (3.3) can now be written as 

0 −θ3 ω C(v)v = θ3 ω 0

     u θ4 u 0 + . ω 0 θ6 + (θ5 − θ3 )u ω

(3.5)

26

3 Dynamic Models

Therefore, the model in (3.3) can be written as  + Hv + C(v)v + F(v)v = vr ,  θ1 0 , 0 θ2

(3.6b)

  θ4 0 0 θ6 + (θ5 − θ3 )u

(3.6c)



with H= F(v) =

(3.6a)

 0 −θ3 ω , θ3 ω 0



and C(v) =

(3.6d)

which is the one to be adopted along the next chapters of this text. For such a model, some basic properties can be checked, remembering that θi > 0 for i = 1, 2, 4 and 6. The first property is that H is a symmetric positive definite matrix, since θ1 and θ2 are positive nonzero values. Therefore, its inverse exists, and is also positive definite. Moreover, H is constant, assuming the physical parameters of the robot, such as its mass, moment of inertia, radius of the wheels, and so on, does not change. The second property refers to the matrix F(v), also a diagonal square matrix. Assuming that θ6 > −(θ5 − θ3 )u, one can conclude that it is also symmetric and positive definite, as well as its inverse. Moreover, it can be considered constant if θ6  |(θ5 − θ3 )u| and the physical parameters of the robot are constant. Actually, in [17] it is shown that such conditions are fulfilled for five distinct differential-drive robots, whose parameters have been identified. Thus, it is acceptable to consider that this property is valid for the differential-drive robot, when regarding the model in (3.6a). Another property is that C(v) is a skew symmetric matrix, since CT (v) = −C(v) [24]. The last meaningful property of the model in (3.6a) is that the mapping vr → v is output strictly passive, whenever θ6 > −(θ5 − θ3 )u and  = 0. The concept underlying this statement is the concept of passivity, as discussed in [1] and in [5]. Such a property is quite important, since passivity is a concept adopted as an approach to design control systems, such as in [5, 10, 12, 15, 18], among others. Therefore, the model in (3.6a) can be used to design passivity-based controllers to be used to control a differential-drive robot. Except for the property of being output strictly passive, all the other properties are easily checked. With regard to this property, Theorem 3.1.1 establishes the complete statement. Theorem 3.1.1 Considering  = 0 and θ6 > −(θ5 − θ3 )u, and assuming that vr ∈ L 2e and v ∈ L 2e , the mapping vr → v of the dynamic model

3.1 Modeling a Unicycle Robot with Velocity Inputs

27

Hv + C(v)v + F(v)v = vr is output strictly passive. Proof According to [23], an operator P : L 2e → L 2e is output strictly passive if, and only if, there are real constants β and δ so that ∀x ∈ L 2e ,

< Px, x > ≥ β + δPx22,T

(3.7)

where Px, x is a scalar correspondent to the inner product of Px and x, and Px22,T is the squared Euclidean norm of Px when considering a time interval T . To show that the mapping vr → v is strictly output passive, one should consider the model in (3.6a) and verify that it fulfills the condition in (3.7), considering that v corresponds to Px and vr corresponds to x. In the sequel, one should take the quadratic form [6] V = 21 v T Hv and its first time derivative V˙ = v T Hv˙ , yet taking into account that H is a constant matrix. Now, using (3.6a) and considering that C(v) is skew-symmetric and F(v) is symmetric and positive definite, V˙ becomes V˙ = v T (vr − Cv − Fv) = v T vr − v T Fv.

(3.8)

Integrating such a value along the time interval [0, T ] one gets T

V˙ dt =

0

T

T v vr dt − T

0

or

v T Fvdt, 0

T V (T ) − V (0) =

T v vr dt − T

0

v T Fvdt.

(3.9)

0

By dropping the positive term V(T), it follows that T v vr dt − λmin (F)

−V (0) ≤ 0

or

T v2 dt,

T

0

T v T vr dt ≥ −V (0) + λmin (F)v22,T . 0

Assuming that both vr and v ∈ L 2e , the previous equation can be written as v, vr ≥ −V (0) + λmin (F)v22,T ,

(3.10)

28

3 Dynamic Models

where λmin (F) is the smallest eigenvalue of F. Taking into account that F > 0, such that its eigenvalues are all positive and nonzero [6], a comparison between (3.10) and (3.7) allows concluding that the mapping vr → v is strictly output passive.

3.1.2 Model Parameterization and Identification T  To allow identifying the values of the parameters θ = θ1 θ2 θ3 θ4 θ5 θ6 , (3.2) can be written as ⎡ ⎤ θ1 ⎢θ2 ⎥    ⎢ ⎥ ⎥ u˙ 0 −ω2 u 0 0 ⎢ u ⎢θ3 ⎥ = Gθ , (3.11) vr = r e f = ⎢ ωr e f 0 ω˙ 0 0 uω ω ⎢θ4 ⎥ ⎥ ⎣θ5 ⎦ θ6 so that applying appropriate (sufficiently exciting of the dynamics) input velocities u r e f and ωr e f and measuring the velocities u and ω and the accelerations u˙ and ω˙ it is possible to get an estimate of the vector of parameters θ . To do that, known signals are applied to the inputs u r e f and ωr e f , and N samples of them are stacked, composing the vector  T vr e f = u r e f1 ωr e f1 u r e f2 ωr e f2 . . . u r e f N ωr e f N . Now, measuring the values u, ω, u˙ and ω˙ correspondent to the samples of the reference signals, the recursion matrix T  R = G1 G2 . . . G N is formed. Notice that the entries of vr e f are known, from the applied input velocities, and the entries of R are measured (the effective velocities of the robot are obtained through its oddometry, whereas the accelerations can be obtained simply numerically differentiating the velocities), so that only θ remains as unknown. With these data one can apply the least mean squares technique [9], to obtain the estimate (3.12) θˆ = (R T R)−1 R T vr e f of the vector of parameters characterizing the dynamic model of the differential-drive wheeled mobile robot. As an example, by applying linear and angular velocity references with three sinusoidal components each, it has been identified the dynamic parameters of the Pioneer 3DX robot, resulting in

3.2 Dynamic Model for the Car-Like Robot

29

θ1 = 0.3037; θ2 = 0.2768; θ3 = −0.0004018; θ4 = 0.9835; θ5 = 0.003818; θ6 = 1.0725.

3.2 Dynamic Model for the Car-Like Robot Consider the car-like robot in its simplified bicycle representation, as shown in Fig. 3.1. The dynamic model is obtained by considering the driving forces acting on both the front and rear wheels, namely F f and Fr , and the Newton forces balance equations related to the longitudinal and lateral coordinates, besides the rotation around its center of gravity G. The result is that M(u˙ − ωv) = Fr + F f cos δ − F f y sin δ,

(3.13)

M(v˙ + ωu) = Fr y + F f sin δ + F f y cos δ,

(3.14)

J ω˙ = −a Fr y + F f b sin δ + F f y b cos δ,

(3.15)

and where u is the longitudinal velocity, v is the lateral velocity, ω is the angular velocity, M is the mass, and J is the moment of inertia, all of them referred to the robot body. Notice that these equations include the Coriolis and centripetal forces on the vehicle.

Fig. 3.1 Forces acting on a car-like vehicle

30

3 Dynamic Models

The lateral or wheel-soil interaction forces F f y and Fr y can be expressed as [11, 19] F f y = c f (δ − α f ) and Fr y = cr αr where c f and cr are the so-called cornering or lateral stiffness factors, and α f and αr are the slip angles, calculated as tan(δ − α f ) = and tan(αr ) =

bω + v , u

aω − v . u

Therefore, in compact form such a dynamics is given by ⎡

⎤⎡ ⎤ ⎡ ⎤⎡ ⎤ M 0 0 u˙ 0 −Mω 0 u ⎣ 0 M 0 ⎦ ⎣ v˙ ⎦ + ⎣ Mω 0 0⎦ ⎣ v ⎦ 0 0 J ω˙ 0 0 0 ω ⎡ ⎤ ⎡ ⎤  cos δ 1   − sin δ 0  F F = ⎣ sin δ 0⎦ f + ⎣ cos δ 1 ⎦ f y , Fr Fr y b sin δ 0 b cos δ −a (3.16) or, in symbolic terms, by Du˙ + C(u)u = Bf + JT λ, where D is the inertial matrix, C is the matrix of Coriolis and centripetal forces, and J is the kinematics Jacobian matrix, which, for the non-slipping condition, verifies ⎡ ⎤ u J ⎣ v ⎦ = 0, ω which represents the non-holonomic conditions for this vehicle (null lateral velocity in both wheels). It should be emphasized that the dynamic model in (3.16) is written in terms of the driving forces acting on the vehicle. Nonetheless, depending on the kind of driving motors one can express it in terms of the desired input velocities, similarly to the case of the differential-drive robot. Taking as example a car-like robot whose rear wheels are driven by an electrical motor and in which the dominant dynamics is the longitudinal one (assuming smooth maneuvers), from (3.16) one simply obtains M u˙ − Mωv = Fr − sin δ F f y .

(3.17)

3.2 Dynamic Model for the Car-Like Robot

31

Now, considering the motor model with negligible voltage on the inductance one gets (3.18) τr = ka (vr − kb ωr )/Ra , where vr is the input voltage applied to the motor, kb is equal to the voltage constant multiplied by the gear ratio, Ra is the electric resistance, τr is the motor torque multiplied by the gear ratio, ωr is the angular velocity of the rear wheel, and ka is the torque constant multiplied by the gear ratio. As for the dynamic equation for the motor-wheel system, it is given by Ie ω˙ r + Be ωr = τr − Fr Rr

(3.19)

where Ie and Be are the moment of inertia and the viscous friction coefficients of the combined motor, rotor, gearbox, and wheel, and Rr is the nominal radius of the rear tire. Finally, most market-available robots in general have low level PID velocity controllers to track input reference velocities and do not allow the motor voltage to be applied directly. To simplify the model, a PD velocity controller is considered, which is described by (3.20) vr = k p (u r e f − u) − kd u˙ where u r e f is the reference velocity to be sent to the dynamic model, u is the current longitudinal velocity of the vehicle and k p and kd are the proportional and derivative gains of the PD controller. To further simplify the model, u˙ r e f has not been considered in (3.20). Combining (3.18), (3.19) and (3.20), and remembering that u = ωr Rr , one obtains the dynamics of the servo-motor-wheel system, given by ˙ Fr = c1 u r e f − c2 u − c3 u,

(3.21)

where the constants are given by c1 = c2 =

ka k p , Rr Ra

ka k p ka kb Be + 2 + 2, Rr Ra Rr Ra Rr

and c3 =

ka kd Ie + 2. Rr Ra Rr

Finally, combining the longitudinal rear wheel driven dynamics of the vehicle body (3.17) and the dynamics of the servo-motor (3.21) one gets the simplified inverse longitudinal dynamic model of the vehicle taking u r e f as input, which is

32

3 Dynamic Models

u r e f = D u˙ + η(u, ω, δ), where D= and

(3.22)

c3 + M , c1



c2 Ma 2 sin δ η(u, ω, δ) = u− ω + cf c1 c1 c1

 bω + aω δ− , u

for which the non-slipping condition v = aω has been considered. Equation (3.22) can be directly used for inverse dynamic compensation control, as it is explained in Chap. 4. Furthermore, such an equation can be written as a linear one, in dynamic parameters, as u r e f = , (3.23) where

T   = u˙ u −ω2 − ωu sin δ δ sin δ

and =



c3 +M c2 Ma c f c1 c1 c1 c1

L

cf c1

T

.

Therefore, using (3.23) it is possible to identify the parameters in . Such an equation also allows designing a simple adaptive dynamic control, as discussed in Chap. 4.

3.3 The Dynamic Model of a Quadrotor For a quadrotor the dynamic behavior can be described as a sequence of events, start T ing from the input commands, or the vector u = u vx u v y u vz u vψ . The velocities commanded by the a remote user, through a smartphone or a joystick, for instance, or by an automatic controller designed to autonomously guide the vehicle, running as a computer code in an external or embedded computer, are processed by the embedded autopilot of the quadrotor, which define the rotation speed of each one of the four  motors. Such motors generate the total thrust f i , i = 1, . . . , 4, in the direction normal to the body of the vehicle, as well as the torques Mi , i = 1, . . . , 4 around the axis Z b of the vehicle. The total thrust acts on the center of gravity of the vehicle, and the torques act as represented in Fig. 2.8. Then, through the attitude of the T T   vehicle, the forces and torques characterized as f = f x f y f z and τ = τx τ y τz are generated, which cause linear and angular accelerations, respectively, along and around the correspondent axes, causing the movement of the vehicle. Such sequence of events is represented in Fig. 3.2, which is based in a similar figure in [3].

3.3 The Dynamic Model of a Quadrotor

33

Fig. 3.2 Characterization of the dynamics of a quadrotor, from the commanded velocities to the position ξ and attitude η. The vectors ξ˙ and η˙ represent the linear and angular velocities.

The generation of the individual velocities for the four motors is a task accomplished by the embedded autopilot, which works as a low-level controller. The propulsion model characterizes how the propulsion forces are generated by each motor, generally assumed as proportional to the square of its rotation speed. With respect to the diagram of Fig. 3.2, these two steps are performed by the first and second blocks, which characterizes what is conventionally called the low level dynamic model. As one can see, to model these two blocks is not an easy task. Moreover, such model would be quite different for different quadrotors. However, according to [2, 14], the input control signals are approximately related to the forces/torques by a linear function, for small quadrotors, like the Bebop 2 one. Therefore, the low level dynamic model of such a quadrotor can be represented by the linear func T T  tion u vx u v y u vz u vψ = α f 1 f 2 f 3 f 4 + β, with α ∈ R4 and β ∈ R4×1 being constant matrices. Still considering Fig. 3.2, the last two blocks represent what is conventionally called the high level dynamic model. The block responsible for generating the forces and torques that will cause the movement of the quadrotor corresponds to the way the total thrust is converted in the forces f and torques τ in the three axes of the Cartesian 3D space. Regarding the forces generated in the global frame, they correspond to the T  vector f = f xw f yw f zw given by ⎡

⎤ ⎡ ⎤⎡ ⎤ 0 cθ cψ cψ sθ sφ − sψ cφ cφ sθ cψ + sφ sψ 0 f = Rbw ⎣ 0 ⎦ = ⎣sψ cθ sφ sθ sψ + cφ cψ sθ sψ cφ − cψ sφ ⎦ ⎣ 0 ⎦ , f zb f zb −sθ cθ sφ cθ cφ

(3.24)

34

3 Dynamic Models

4 where f zb = i=1 f i is the total thrust, and cx and sx correspond to cos x and sin x, respectively, such as in Sect. 2.2. As for the torques around the axes x b , y b and z b of the body of the vehicle, the result is ⎡ ⎤ f τφ l l −l −l ⎢ 1 ⎥ f 2⎥ τ = ⎣ τθ ⎦ = ⎣−l l l −l ⎦ ⎢ ⎣ f3 ⎦ , k −k k −k τψ f4 ⎡

⎤

⎡

⎤

(3.25)

where τφ is the torque around the axis X b , which varies the roll angle, τθ is the torque around the axis Y b , which varies the pitch angle, and τψ is the torque around the axis Z b , which varies the yaw angle. As for l and k, they are the distance from each rotor to the center of gravity of the quadrotor and a constant characterizing the moments Mi , i = 1, . . . , 4 illustrated in Fig. 2.8. The last block of the complete dynamic model of the quadrotor, the fourth one in Fig. 3.2, corresponds to the mechanical characterization of the movement, or to the relationship between the forces and torques and the linear and angular velocities of the quadrotor.  T  T Considering the position η1 = x y z and attitude η2 = φ θ ψ of the vehiT  cle, both represented in the world frame, and the linear velocities v = vxb v by vzb T  and angular velocities ω = ωxb ωby ωzb , both in the body reference system, one can write the correspondent Lagrangian, the difference of the kinetic and potential energies associated to the movement of the quadrotor, as

with T =

L = T − V,

(3.26a)

1 1 T mv v + ω T Iω, 2 2

(3.26b)

and V = mgz,

(3.26c)

regarding that the Z axis points upwards. The matrix I defines the inertia of the vehicle, regarding all possible rotations, and is given by I = diag(Ix x , I yy , Izz ), with the entries out of the main diagonal null, considering that the body of the quadrotor is axisymmetric, what also implies that Ix x = I yy [13, 21]. Now, applying the EulerLagrange constraint, as in [3], and expanding the result, one gets that [13] m[v˙ xb − v by ωzb + vzb ωby − g sin θ ] = 0,

(3.27a)

m[v˙ by − vzb ωxb + vxb ωzb + g cos θ sin φ] = 0,

(3.27b)

3.3 The Dynamic Model of a Quadrotor

35

m[v˙ zb − vxb ωby + v by ωxb + g cos θ cos φ] = f zb ,

(3.27c)

Ix x ω˙ xb + (Izz − I yy )ωby ωzb = τφ ,

(3.27d)

I yy ω˙ by + (Ix x − Izz )ωzb ωxb = τθ ,

(3.27e)

Izz ω˙ zb = τψ ,

(3.27f)

with all the velocities referred to the body frame. However, for the purpose of designing a controller for the quadrotor, it is more convenient to use the dynamic equations written in the world frame. To do that,  −1 w x˙ . In addition, one can take recall (2.22), or x˙ w = Rbw vb , so that vb = Rbw ˙ w vb , from where results that the time derivative of (2.22) to obtain x¨ w = Rbw v˙ b + R b   −1 ˙ w vb . Similarly, regarding (2.23), or w = Hωb , one gets the x¨ w − R v˙ b = Rbw b ¨ w − Hω ˙ b ). Then, using such relationships one gets, now relationship ω˙ b = H−1 ( considering that the variables are all referred to the world frame [13], and including the air drag effect [25],

and

˙ m x¨ = (cos φ sin θ cos ψ + sin φ sin ψ) f zb − d1 x,

(3.28a)

m y¨ = (cos φ sin θ sin ψ − sin φ cos ψ) f zb − d2 y˙ ,

(3.28b)

m z¨ = (cos φ cos θ ) f zb − mg − d3 z˙ ,

(3.28c)

˙ Ix x θ¨ ≈ τφ − (Izz − I yy )θ˙ ψ˙ − d4 φ,

(3.28d)

I yy φ¨ ≈ τθ − (Ix x − Izz )φ˙ ψ˙ − d5 θ˙ ,

(3.28e)

˙ Izz ψ¨ ≈ τψ − d6 ψ,

(3.28f)

where m is the mass of the vehicle, g is the gravity acceleration, x, ˙ y˙ and z˙ are ¨ y¨ and z¨ are the respective the linear velocities along the axes X w , Y w and Z w , x, ˙ θ˙ and ψ˙ are the angular velocities around the axes X w , Y w and Z w , accelerations, φ, ¨ ¨ and φ, θ and ψ¨ are the respective angular accelerations. As for di , i = 1, . . . , 6, and f zb , they are the air drag coefficients and the total thrust generated by the four motors of the quadrotor. Nonetheless, according to [4], the autopilot of the AR.Drone quadrotor is designed considering a similar model to achieve flight stabilization and response to the input control signals, which is also valid for the Bebop 2 quadrotor. However, the access to the firmware is restricted to end users. Regardless of this, it is possible to take advantage of the presence of the autopilot, when the vehicle is equipped with an autopilot, which is the case of the AR.Drone 2.0 and the Bebop 2 quadrotors, to

36

3 Dynamic Models

model the dynamic response  of them as a function of the high-level control signals, packed in the vector u = u vx u v y u vz u vψ . The stabilization of the quadrotor remains as a responsibility of the firmware, but the vehicle response to the input commands can be modeled using a simple mathematical description, quite useful for the purpose of designing high-level flight controllers [21, 22]. In such a context, a simpler model for a quadrotor equipped with an autopilot is v˙ xb = K 1 u vx − K 2 vxb ,

(3.29a)

v˙ by = K 3 u v y − K 4 v by ,

(3.29b)

v˙ zb = K 5 u vz − K 6 vzb ,

(3.29c)

v˙ψb = K 7 u vψ − K 8 vψb ,

(3.29d)

and

where all the velocities and input commands are in the body frame. In a compact form, such equations can be written as v˙ b = f1 u − f2 vb ,

(3.30)

where f1 = diag(K 1 , K 3 , K 5 , K 7 ) and f2 = diag(K 2 , K 4 , K 6 , K 8 ) are diagonal matrices whose entries can be identified through a procedure similar to the one described in Sect. 3.1.2. Notice that this is a fourth-order linear state equation, correspondent to a system with four inputs, for which the state variables are the linear velocities in the three axes of the body frame and the yaw velocity. Using the concept of equivalence transformation [7], one has, considering the new state vector ⎡ w⎤ ⎡ ⎤⎡ b⎤ vx x˙ cos ψ − sin ψ 0 0 ⎢ y˙ w ⎥ ⎢ sin ψ cos ψ 0 0⎥ ⎢ v by ⎥ w b ⎥ ⎢ ⎥⎢ ⎥, v = Pv or ⎢ ⎣ z˙ w ⎦ = ⎣ 0 0 1 0⎦ ⎣ vzb ⎦ 0 0 01 vψb ψ˙ w according to (2.26), which already includes the near hover condition. Notice that the matrix P is nonsingular for all values of ψ. Then, considering the new state vector one can write, according to [7], vw , v˙ w = Pf1 u − Pf2 vb , or v˙ w = F1 u − F2 vb v˙ w = Pf1 u − Pf2 P−1  Pvb

what corresponds, expanding the matrices F1 and F2 , to

3.3 The Dynamic Model of a Quadrotor

⎡

⎤ ⎡ x¨ w K 1 cos ψ ⎢ y¨ w ⎥ ⎢ K 1 sin ψ ⎢ w⎥=⎢ ⎣ z¨ ⎦ ⎣ 0 0 ψ¨ w ⎡ K 2 cos ψ ⎢ K 2 sin ψ −⎢ ⎣ 0 0

37

−K 3 sin ψ K 3 cos ψ 0 0

0 0 K5 0

−K 4 sin ψ K 4 cos ψ 0 0

0 0 K6 0

⎤⎡ ⎤ 0 u vx ⎢ u vy ⎥ 0⎥ ⎥⎢ ⎥ 0 ⎦ ⎣ u vz ⎦ u vψ K7 ⎤⎡ b⎤ vx 0 ⎢ v by ⎥ 0⎥ ⎥⎢ ⎥, 0 ⎦ ⎣ vzb ⎦ K8 vψb

(3.31)

or, in terms of the individual variables

and

x¨ w = K 1 cos ψu vx − K 3 sin ψu v y − K 2 cos ψvxb + K 4 sin ψv by ,

(3.32a)

y¨ w = K 1 sin ψu vx + K 3 cos ψu v y − K 2 sin ψvxb − K 4 cos ψv by ,

(3.32b)

z¨ w = K 5 u vz − K 6 vzb ,

(3.32c)

ψ¨ w = K 6 u vψ − K 6 vψb .

(3.32d)

An important remark, for the two last equations above, is that considering the near hover situation the axes Z b and Z w are in parallel, meaning that z˙ w = vzb and ψ˙ w = T T   vψ , so that vxb v by vzb vψb becomes vxb v by z˙ w ψ˙ w in (3.31). To conclude this section, Table 3.1 shows the constants K i , i = 1, . . . , 8, identified for the quadrotors AR.Drone 2.0 and Bebop 2, following the procedure outlined in Sect. 3.1.2 and detailed in [21] for the AR.Drone 2.0 and in [20] for the Bebop 2 quadrotor.

Table 3.1 Constants identified to model the AR.Drone 2.0 (without the indoor protective hull) and Bebop 2 quadrotors AR.Drone 2.0 Bebop 2 K1 K2 K3 K4 K5 K6 K7 K8

5.00 0.30 5.50 0.10 0.55 0.70 1.45 0.70

0.84170 0.18227 0.83540 0.17095 3.96600 4.00100 9.85240 4.72950

38

3 Dynamic Models

3.4 Concluding Remarks This chapter presents dynamic models for the differential-drive and car-like wheeled mobile robots, as well as for quadrotors equipped with an on-board autopilot. In the first cases, it was stressed the importance to include the dynamics of the actuators, thus obtaining a model whose inputs are reference velocities, as usual in commercial robots. Regarding the quadrotors, a simplified dynamic model, valid under the assumption that the quadrotor is flying in a near-hover condition, what corresponds to slight inclinations around the axes X b and Y b . Such models are quite important when designing high-level controllers to guide these vehicles autonomously, as it will be discussed in Chap. 4. The technique used to design such controllers uses a controller implemented as two control loops, an inner one and an outer one, in which the inner control loop is used exactly to compensate for the vehicle dynamics, introducing a new dynamics that is more suitable for the application. In addition, the models here presented can also be used to design controllers based on a single control loop, also based on the inverse dynamics technique, as discussed in Chap. 4. Moreover, when dealing with heavier robots, whose dynamic behavior demands more acceleration time to reach the desired velocities, which can cause meaningful differences between the commanded velocities and those effectively developed by the vehicle, such models are quite useful.

References 1. Bao J, Lee PL (2007) Dissipativity and passivity. In: Bao J, Lee PL (eds) Process control—the passive systems approach. Springer, London, pp 5–41. https://doi.org/10.1007/978-1-84628893-7_2 2. Bernard M, Kondak K, Meyer N, Zhang Y, Hommel G (2007) Elaborated modeling and control for an autonomous quad-rotor. In: Proceedings of the 22nd international unmanned air vehicle systems Cconference. Bristol, UK, pp 2375–2380 3. Brandão AS, Filho MS, Carelli R (2013) High-level underactuated nonlinear control for rotorcraft machines. In: 2013 IEEE international conference on mechatronics (ICM), pp 279– 285. https://doi.org/10.1109/ICMECH.2013.6518549 4. Bristeau PJ, Callou F, Vissiere D, Petit N (2011) The navigation and control technology inside the AR.Drone micro UAV. In: Proceedings of the 18th IFAC world congress, vol 18. Milan, Italy 5. Brogliato B, Maschke B, Lozano R, Egeland O (2007) Passivity-based control. In: Dissipative systems analysis and control. Springer, London, pp 1279–1291. https://doi.org/10.1007/9781-84628-517-2_7 6. Chen CT (1999) Linear algebra. In: Chen CT (ed) Linear systems theory and design, 3rd ed. Oxford University Press, pp 44–78 7. Chen CT (1999) State-space solutions and realizations. In: Chen CT (ed) Linear systems theory and design, 3rd ed. Oxford University Press, pp 86–116

References

39

8. de la Cruz C, Carelli R (2008) Dynamic model based formation control and obstacle avoidance of multi-robot systems. Robotica 26(3):345–356. https://doi.org/10.1017/ S0263574707004092 9. Diniz PSR (2020) The least-mean-square (lms) algorithm. In: Diniz PSR (ed) Adaptive filtering, Chap. 3. Springer International Publishing. https://doi.org/10.1007/978-3-030-29057-3_3 10. Dong Y, Chopra N (2019) Passivity-based bilateral tele-driving system with parametric uncertainty and communication delays. IEEE Control Syst Lett 3(2):350–355. https://doi.org/10. 1109/LCSYS.2018.2879952 11. Lucet E, Grand Ch, Salle D, Bidaud P (2008) Stabilization algorithm for a high speed car-like robot achieving steering maneuver. In: 2008 IEEE international conference on robotics and automation. Pasadena, GA, USA, pp 2540–2545 12. Hatanaka T, Igarashi Y, Fujita M, Spong MW (2012) Passivity-based pose synchronization in three dimensions. IEEE Trans Autom Control 57(2):360–375. https://doi.org/10.1109/TAC. 2011.2166668 13. Kim J, Kang MS, Park S (2009) Accurate modeling and robust hovering control for a quad-rotor VTOL aircraft. J Intell Robot Syst 57(1):9. https://doi.org/10.1007/s10846-009-9369-z 14. Kondak K, Bernard M, Meyer N, Hommel G (2007) Autonomously flying vtol-robots: modeling and control. In: Proceedings 2007 IEEE international conference on robotics and automation, pp 736–741. https://doi.org/10.1109/ROBOT.2007.363074 15. Lanzon A, Petersen IR (2008) Stability robustness of a feedback interconnection of systems with negative imaginary frequency response. IEEE Trans Autom Control 53(4):1042–1046. https://doi.org/10.1109/TAC.2008.919567 16. Martins FN, Celeste WC, Carelli R, Sarcinelli-Filho M, Bastos-Filho TF (2008) An adaptive dynamic controller for autonomous mobile robot trajectory tracking. Control Eng Prac 16(11):1354–1363. https://doi.org/10.1016/j.conengprac.2008.03.004 17. Martins FN, Sarcinelli-Filho M, Carelli R (2017) A velocity-based dynamic model and its properties for differential drive mobile robots. J Intell Robot Syst 85(2):277–292. https://doi. org/10.1007/s10846-016-0381-9 18. Ortega R, van der Schaft A, Castanos F, Astolfi A (2008) Control by interconnection and standard passivity-based control of port-hamiltonian systems. IEEE Trans Autom Control 53(11):2527–2542. https://doi.org/10.1109/TAC.2008.2006930 19. Pacejka HB (2006) Tyre and vehicle dynamics, 2nd edn. Elsevier, UK 20. Pinto AO, Marciano HN, Bacheti VP, Moreira MSM, Brandão AS, Sarcinelli-Filho M (2020) High-level modeling and control of the Bebop 2 micro aerial vehicle. In: Proceedings of the 2020 international conference on unmanned aircraft systems. IEEE, Athens, Greece, pp 939– 947. https://doi.org/10.1109/ICUAS48674.2020.9213941 21. Santana LV, Brandão AS, Sarcinelli-Filho M (2016) Navigation and cooperative control using the AR.Drone quadrotor. J Intell Robot Syst 84(1), 327–350. https://doi.org/10.1007/s10846016-0355-y 22. Santana LV, Brandão AS, Sarcinelli-Filho M, Carelli R (2014) A trajectory tracking and 3d positioning controller for the AR.Drone quadrotor. In: 2014 international conference on unmanned aircraft systems (ICUAS). IEEE, pp 756–767. https://doi.org/10.1109/ICUAS.2014. 6842321 23. van der Schaft A (2017) Small-gain and passivity for input-output maps. In: L2-gain and passivity techniques in nonlinear control, 3rd ed.. Springer, Cham, pp 13–32. https://doi.org/ 10.1007/978-3-319-49992-5_2 24. Szabo FE (2015) S. In: Szabo FE (ed) The linear algebra survival guide. Academic Press, Boston, pp 320–377. https://doi.org/10.1016/B978-0-12-409520-5.50026-6 25. Villa DKD, Brandão AS, Carelli R, Sarcinelli-Filho M (2021) Cooperative load transportation with two quadrotors using adaptive control. IEEE Access 9:129148–129160. https://doi.org/ 10.1109/ACCESS.2021.3113466

Chapter 4

Motion Control

Abstract This chapter explores two basic techniques to design a controller to guide differential drive wheeled robots and quadrotors, namely the single-loop dynamic control system and the inner-outer loops control system, ans shows that they can be adopted to accomplish positioning tasks, trajectory-tracking tasks and path-following tasks, with quite small adaptations. It also shows how to design an adaptive dynamic compensator to be used as the inner control loop, which is quite useful in situations in which the robot transports loads, which changes its dynamic parameters, as well as to compensate for any misidentified dynamic parameter or external disturbances. The focus, in both designs, is the inverse kinematics technique to design the outer loop correspondent to the inner-outer loops structure, and the inverse dynamics technique to design the inner loop of the inner-outer loops structure and the single-loop dynamic controller, which consists in inverting the nonlinear kinematics or dynamics, thus generating a linear closed-loop system equations.

4.1 Introduction As mentioned in the end of Chap. 2, applying suitable velocity commands to the mobile robot, either a wheeled one or an aerial one, one can guide it to reach any desired point in its navigation space. This is what the literature calls motion control. As one can suppose, there are different ways to control the movement of a robot, depending on what one wants the robot do. In this chapter the most important ways to control the movement of a mobile robot are presented, and controllers suitable to guide the vehicle in the cases addressed are designed. The focus is on the use of the technique of inverse kinematics [2] to design such controllers, taking into account the kinematic models of the vehicles. We also show that this method is generic enough to allow controlling the vehicles when executing any kind of movement. Therefore, we are going to present a unified approach to control the movement of a mobile robot.

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2023 M. Sarcinelli-Filho and R. Carelli, Control of Ground and Aerial Robots, Intelligent Systems, Control and Automation: Science and Engineering 103, https://doi.org/10.1007/978-3-031-23088-2_4

41

42

4 Motion Control

4.2 Basic Control Objectives One can classify the movement of a mobile robot in one of the following possibilities: posture control, trajectory-tracking control and path-following control. Posture control means to control the position and the orientation of the vehicle to reach a desired position and orientation (posture refers to position plus orientation). Regarding the differential drive wheeled robot, for instance, one can split the task in two subtasks, namely to guide the robot to reach a desired position (xd , yd ), in the navigation plane, starting from any initial position (x, y), in the plane, and to rotate around its Z b axis to correct the orientation, after reaching the desired position. Notice that this is possible just for this specific robot, which can turn around without changing its position. Another important aspect, in this case, is that it does not matter the path the robot followed to reach the target point. The problem specifies just the point to be reached and the desired orientation at the destination, not including any path the robot should follow. The second possibility is the so called trajectory-tracking control, in which the robot should track a given trajectory. By a trajectory we mean a vanishing point, which is a point in the plane, for a ground robot, or in the 3D space, in the case of aerial robots, whose position varies along time. This means that the desired destination is now the point (xd (t), yd (t)) or (xd (t), yd (t), z d (t)), which moves with velocities (x˙d (t), y˙d (t)) or (x˙d (t), y˙d (t), z˙ d (t)). In this case, the robot should develop velocities greater than those of the trajectory to get over it, and, in such a condition, reduce its velocity to match the velocity of the trajectory, in order to stay over it. The third possibility is to follow a given path, what is quite similar to the behavior of a person driving a car in a road. The driver should keep any desired velocity, always tangent to the path, to keep itself in the road. Otherwise, or if the velocity is not tangent to the path, the vehicle goes out of the road. Then, to follow a path means, as the first step, to consider the point over the given path that is closest to the robot, and guide the robot to reach such a point. In the sequel, the second step is to make the robot to develop a velocity that is always tangent to the path, regardless of its magnitude. This means that the magnitude of the velocity along the path is freely defined. This means that one can select any velocity, including zero, what means that the vehicle stops for a while over the path, such as when a car stops in a red traffic light. Notice the main difference, in the three cases: in posture control only a desired destination is defined, and no path or velocity is specified. On the other hand, in trajectory-tracking the velocity of the vehicle, in terms of both magnitude and direction, is defined by the trajectory being tracked, whereas in path-following the velocity of the vehicle is defined by the designer, restricted to be always tangent to the path being followed. These three cases can be exemplified by simple daily situations. When one takes any bus which passes by your working place and sleep during the trip you do not know neither how long it took to get at your work nor the route the bus followed. Notice that the objective, namely to arrive at your workplace, has been accomplished.

4.3 Inner-Outer Controllers

43

Regarding trajectory-tracking, an example can be a bus driver, who knows the exact instants the bus should arrive each stop. This means that the driver should be aware of the timetable to perfectly accomplish the task of being at each bus stop in the right time. Finally, to exemplify a situation of path-following, consider that one is driving his/her car along a road. Sometimes he/she can accelerate, to move faster, and sometimes should brake, to obey velocity restrictions along the road, or even to stop in a red traffic light. Anyway, he/she follows the path, in this case the road, in more or less time, what does not matter at all, because there is no time constraint in this case.

4.3 Inner-Outer Controllers The controllers to be designed to guide the robot in posture control, trajectorytracking control, and path-following control will use the kinematic models of Chap. 2 to command the vehicles to develop the desired velocities in the world frame. Thus, the so called kinematic controllers are obtained. However, as the kinematic models do not consider the mass and the inertia of the robots being controlled, there is no guarantee that the velocity commands the kinematic controller sends to the robots will be effectively developed. This creates what is called a velocity-tracking error: there is a difference between the velocities sent by the kinematic controller and the velocities effectively developed by the robots. To compensate for such velocity-tracking error a second controller, or a compensator, takes care of this velocity-tracking error, so that one has now two control levels, which are an internal loop to increase/decrease the acceleration to compensate for the velocity error, and an external loop to change the velocity of the vehicle to allow it to move accordingly. Therefore, we have a controller composed of two closed-loops, an inner one, the so called dynamic controller or dynamic compensator, and an outer one, the so called kinematic controller. This generates what is called an inner-outer loops controller [7, 12], also known as cascade controller [4]. The reason for this is that the outer control loop effectively generates the references (setpoint) for the inner control loop, thus configuring a cascaded structure, as depicted in Fig. 4.1. An important aspect, when using inner-outer control loops is that although having just a single output, in the case of Fig. 4.1 the position of the vehicle xw , one should also have information about the velocities x˙ b . For the case of the robots analyzed in this book this is not a problem, since in general one has access to their position and velocity. A second aspect is that two controllers need to be tuned. In this book we will adopt this strategy of designing two controllers according to the configuration of Fig. 4.1. The two controllers will be a kinematic one, the outer loop, and a dynamic one, the inner loop, this last one responsible for stabilizing the attitude of the vehicle (the orientation, when applied to a wheeled robot navigating in the X w Y w plane).

44

4 Motion Control

Fig. 4.1 A sketch of an inner-outer controller, also known as a cascade controller

4.4 Inverse Kinematics Technique When dealing with motion control one wants to control the velocity of a vehicle in order to guarantee that it reaches a desired position, tracks a specified trajectory or follows a given path, as discussed in Sect. 4.2. Anyway, the controller should generate velocity commands to be delivered to the vehicle, in order to reduce the position error. Therefore, the relation between the velocity commands and the robot position/orientation is a kinematic relationship, not taking into account the dynamics of the vehicle affecting the position error x˜ w = xdw − xw along time. This means that the temporal behavior of such an error is described by a first order differential equation, which is the characteristic equation obtained when closing the loop with a controller. Figure 4.2 shows the principle behind the technique of inverse kinematics [9, 15, 16]. The idea is that the system to be controlled is described by a nonlinear kinematic model, and the controller uses the inverse of the kinematic model of the vehicle to compensate for the nonlinearity, so that the resulting closed loop equation describes a linear behavior of the position error. In this case, the premise is that the inverse of the kinematic model exists, or that the matrix describing the kinematic model should have a nonzero determinant. Therefore, considering that the kinematic models of the robots analyzed in Chap. 2 are of the form (4.1) x˙ w = Avb , where A is the matrix of direct kinematics and vb is the vector of velocity commands to the robot, one can adopt the generic control law vd = A−1 ν,

(4.2)

4.4 Inverse Kinematics Technique

45

Fig. 4.2 Diagram illustrating a closed-loop control system designed using the inverse kinematics technique

where ν is an auxiliary velocity vector, which can be defined as ν = vdw + K p x˜ w ,

(4.3)

where vdw represents a desired velocity, which will be defined according to the control objective-position control, trajectory-tracking control or path-following control— and K p is a gain matrix arbitrarily selected among definite positive ones. A simple and intuitive selection is to take K p as a diagonal matrix with positive nonzero entries (a positive definite diagonal matrix). Notice that to implement such a control law A should be invertible. Otherwise, such a controller can not be adopted. Therefore, the inverse kinematics control law can be written as vd = A−1 (vdw + K p x˜ w ).

(4.4)

Now, assuming that the command vd is attained instantaneously, that is vd = vb , what means a perfect velocity tracking, therefore ignoring the dynamics of the vehicle, one gets the closed-loop equation

or

x˙ w = AA−1 (vdw + K p x˜ w ) = vdw + K p x˜ w ,

(4.5)

vdw − x˙ w + K p x˜ w = 0,

(4.6)

combining (4.1) and (4.4). Notice that this equation characterizes a linear behavior in terms of the control error, since K p is a constant matrix, which is a consequence of the use of the inverse kinematics to compensate for the nonlinear kinematics. This is what characterizes the feedback linearization technique [9, 11, 16].

46

4 Motion Control

Fig. 4.3 Illustrating the smooth saturation provided by the function tanh

Before discussing the application of this technique, it is worthy mentioning that the control law in (4.4) can be modified to vd = A−1 ν = A−1 (vdw + K p Ks tanh(Ks−1 x˜ w )),

(4.7)

to include a smooth saturation function, the tanh one, in order to limit the magnitude of the control signals when the error x˜ w is too large, to take into account the physical limits of the robot actuators. The matrix Ks is a real matrix, usually selected as a diagonal one with positive values. The way tanh acts is illustrated in Fig. 4.3, where one can see how this function smoothly bounds its value when the argument is too large. When the control error x˜ w is small, the term K p Ks tanh(Ks−1 x˜ w ) becomes approximately K p x˜ w , meaning that the controller has a proportional gain given by K p . On the other hand, when x˜ w is large enough to saturate the tanh function, that term becomes approximately K p Ks which represents its saturation value. Therefore, by suitably selecting the matrices K p and Ks , the proportional gains and the saturation values can be specified. Two important remarks should be emphasized, with respect to the generic control law of (4.4). The first one is that the variable vdw should be selected according to the control objective, as shown in the following sections. Therefore (4.4) represents a generalized inverse kinematics control law to satisfy different control objectives. The second remark is that it will be convenient, in the sequel, to consider the vector T  ν of (4.3) split in two sub-vectors, namely ν = ν p ν o , where ν p is the auxiliary control vector related to the robot position x p in the Cartesian space, and ν o is the

4.5 Trajectory-Tracking Control

47

one related to the orientation of the vehicle xo in the same coordinate system. This consideration will be useful when discussing the design of control laws being applied to non-holonomic robots.

4.5 Trajectory-Tracking Control When dealing with a trajectory-tracking task, the kinematic controller of the structure of Fig. 4.1 should guarantee that the velocity of the robot be high enough to reduce the error between the current robot position and the desired trajectory to zero, although the trajectory acts in the sense of increasing such an error. Therefore, if the robot is out of the desired trajectory it should develop a velocity that is the velocity of the trajectory plus an increment that is a function of the instantaneous position error. Moreover, when the robot reaches the trajectory it should continue moving, now with the same velocity of the trajectory. This way, the kinematic controller should generate velocity commands to be sent to the robot accordingly. Let us consider first the case of omnidirectional robots such as the wheeled omnidirectional ones, or the quadrotor vehicles. The velocity commands are, in this book, generated considering the technique of inverse kinematics, by taking in (4.4) the desired robot velocity for trajectory tracking to be vdw = x˙ dw , so that the adopted control law is (4.8) vd = A−1 (˙xdw + K p x˜ w ), where A−1 is the matrix of inverse kinematics of the robot, K p is a matrix of gains, diagonal and positive definite, x˜ w is the position error xdw − xw , and x˙ dw is the time derivative of the trajectory being tracked (the block “Processing’’ in Fig. 4.2 is now the time-derivative operator). This way, the velocity of the robot will be greater than that of the trajectory, if it is out of the trajectory, and equal to that of the trajectory otherwise. With respect to the stability proof for this controller, the closed loop equation of (4.6) becomes (4.9a) x˙ dw − x˙ w + K p x˜ w = 0, from where

x˙˜ w + K p x˜ w = 0,

(4.9b)

which is the equation describing the time evolution of the position error x˜ w (t). Notice that it characterizes a linear behavior of such error, since K p is a constant matrix, which is a consequence of the use of the inverse kinematics to compensate for the nonlinear kinematics, as mentioned above. An important detail of (4.9) is that selecting K p as diagonal and positive definite, all the error components are decoupled, and each one of the entries of the vector x˜ w goes to zero exponentially, showing that the kinematic controller based on inverse kinematics is asymptotically stable, in the sense of Lyapunov [9]. As a matter of

48

4 Motion Control

fact, this asymptotic stability can be proven for any K p positive definite by taking the Lyapunov candidate function V (˜xw ) = 21 x˜ wT x˜ w , and its time derivative V˙ (˜xw ) = x˜ wT x˙˜ w . Now, taking x˙˜ w = −K p x˜ w from (4.9) one gets V˙ (˜xw ) = −˜xwT K p x˜ w , which is a negative definite function, showing the asymptotic stability of the closed loop system when adopting the controller of (4.8). Now, if we use the saturation controller option given in (4.7), for trajectory tracking it becomes (4.10) vb = A−1 (˙xdw + K p Ks tanh(Ks−1 x˜ w )). Under such a modification, it is important to analyze how does tanh affect the stability of the closed-loop system. Actually, using (4.10) instead of (4.8), the stability proof is changed just when obtaining the time derivative of the Lyapunov candidate function, which will become V˙ (˜xw ) = −˜xwT K p Ks tanh(Ks−1 x˜ w ). However, as tanh is an odd function, as one can see from Fig. 4.3, the product x˜ wT K p Ks tanh(Ks−1 x˜ w ) is positive for all x˜ w = 0, so that V˙ (˜xw ) is negative definite, thus guaranteeing the asymptotic stability of the closed loop system. In the sequel two examples illustrate the application of the trajectory-tracking controller to omnidirectional robots. Example 1 Wheeled omnidirectional robot In this example a simulation is presented, considering an omnidirectional threewheels robot, as depicted in Fig. 2.3. The simulation corresponds to a desired tra T jectory defined as xd = 5 cos(2π t/60) m 5 sin(2π t/60) m , with the desired orientation ψ(t) = 0, and the robot initially positioned in the coordinates x0 = 2 m and y0 = 3 m, with orientation ψ0 = 0 rad. As for the controller gain and saturation matrices, they are set to K p = 7.5I and Ks = I, with I being the 6 × 6 unit matrix. The simulation runs for a time of 50 s, with sampling time of 50 ms. Figures 4.4 and 4.5 show the results of such a simulation. As one can see, the robot reaches the trajectory and continues tracking it during the navigation, with the error between the desired trajectory and the robot current position converging to zero as the current position of the robot becomes closer to the desired trajectory. Example 2 Quadrotor in a near-hover condition Continuing to illustrate the application of the trajectory-tracking controller to an omnidirectional robot, we consider now a quadrotor in a near-hover condition (see Sect. 2.2). Using the smooth saturation function tanh of (4.10) one gets, vd = A−1 ν = A−1 (˙xdw + K p Ks tanh(Ks−1 x˜ w )),

(4.11)

with A given by (2.26). In this case one should control the linear velocities x˙ w , y˙ w and z˙ w and the yaw velocity ψ˙ w as well, for which the respective components of the auxiliary velocity vector ν are given by, vd x = x˙dw + k px tanh x˜ w ,

(4.12a)

4.5 Trajectory-Tracking Control

49

Fig. 4.4 Desired and effective routes for a simulated omnidirectional robot when tracking a circular trajectory

Fig. 4.5 Positions and position error for the simulated omnidirectional robot when tracking a circular trajectory

50

4 Motion Control

Fig. 4.6 The experimental setup adopted to run the experiment in outdoor environment

and

vdy = y˙dw + k py tanh y˜ w ,

(4.12b)

vdz = z˙ dw + k pz tanh z˜ w ,

(4.12c)

vdψ = ψ˙ dw + k pψ tanh ψ˜ w .

(4.12d)

To illustrate this example, a real experiment correspondent to a trajectory-tracking performed by a Bebop 2 quadrotor in an outdoor environment is shown. The experimental setup adopted to run the experiment is shown in Fig. 4.6. Part of such a setup is the Bebop 2 quadrotor, running its embedded software, compatible with the Autonomy ROS (Robot Operating System)1 library. The feedback of the robot position/orientation is the result of a data fusion procedure run in the embedded firmware of the quadrotor, involving raw data provided by an inertial measurement unit (IMU) and a GPS reception module. Such data is made available in a 5 Hz rate, and is read by a laptop running a Linux operating system, over which ROS runs. Another laptop, running the Windows 10 operating system is used, in which a Matlab script correspondent to the control system runs. The two laptops 1

See https://www.ros.org/.

4.5 Trajectory-Tracking Control

51

are interconnected via RJ45 cables, so that the Matlab script receives the sensory data from the computer running ROS, and sends the calculated velocity commands to the vehicle through the ROS node (this information exchange is represented by the dashed arrows in Fig. 4.6). The router shown in the figure establishes the communication between the two laptops, though the RJ45 cables, and with the Bebop 2 through a wi-fi channel, whereas the joystick is available to allow the human operator to take control of the quadrotor in case of any trouble. Regarding the control system, an important detail is that just the outer loop of the controller shown in Fig. 4.1 is adopted, which corresponds to suppress the inner loop of such a figure. In other words, the velocity command vd is directly sent to the vehicle, without passing by a second controller. As for the correspondent gain  T values, they are selected as K p = 1, 1, 3, 1 , whereas the saturation values are  T given by Ks = 1, 1, 1, 1 . In terms of the task being accomplished, the desired trajectory is a circle described at the constant altitude of 18 m, characterized as T  xdw = xdw (t) ydw (t) z dw (t) ψdw (t) , where xdw (t) = 2.5 cos ydw (t)

2π t = 2.5 sin 60

 m,

(4.13b)

m,

(4.13c)



z dw (t) = 18 m,

(4.13d)

ψdw = 0◦ ,

(4.13e)

 T x˙ dw = x˙dw (t) y˙dw (t) z˙ dw (t) ψ˙ dw (t) ,

(4.14a)

which corresponds to x˙dw (t) = − y˙dw (t)

and

2π t 60



and

so that



(4.13a)

  2π t π sin , 12 60

  2π t π cos , = 12 60

(4.14b)

(4.14c)

z˙ dw (t) = 0,

(4.14d)

ψ˙ dw (t) = 0.

(4.14e)

52

4 Motion Control

Fig. 4.7 The path really followed by the Bebop 2 and the desired one, for the experiment of trajectory-tracking Fig. 4.8 Position/orientation errors for the Bebop 2 tracking a circular trajectory

The results of the experiment are summarized in Figs. 4.7 and 4.8, which show the path really followed by the quadrotor, besides the desired one, in a 3D and a top views and the position/orientation errors, respectively. As one can see, the quadrotor tracked the desired trajectory with small errors, after some initial delay to reach the trajectory (remember that the trajectory is 18 m above ground, and the quadrotor starts tracking it from the initial position/orientation of (0 m, 0 m, 1.2 m, 0◦ ), because the designed controller starts working after the takeoff, a procedure included in the firmware of the quadrotor, whose result is to put the quadrotor in that position, with that orientation. Now, let us consider the case of non-holonomic robots. The control law is the same (4.2) as proposed for inverse kinematics control, using the extended kinematic

4.5 Trajectory-Tracking Control

53

Fig. 4.9 A sketch of the nonholonomic differential-drive robot when the point of interest for control is a generic one, rigidly coupled to the robot platform

model as presented in 2.1.4. Now, regarding the auxiliary control signal ν, we recall  T Remark 2 in Sect. 4.4, where this vector is split in two sub-vectors ν = ν p ν o with ν p being the auxiliary control related to the robot position x p in the Cartesian space, and ν o is the one related to the orientation of the vehicle xo in the same coordinate system. In the case of non-holonomic robots, we define ν p using Eq. v v for trajectory tracking, being xdp the desired position velocities (4.3) with vdw = xdp of the robot. Regarding ν o it will be defined respecting the non-holonomic constraints of the robot. In the sequel we present examples to illustrate the design and application of the trajectory-tracking controller to a non-holonomic mobile robot. Let as first consider the case of a differential-drive wheeled mobile robot with the point of interest for control as illustrated in Fig. 2.6, repeated here for convenience. In this example two distinct cases are addressed, namely a cos α = 0 and a = 0, this last case corresponding to the point of interest for control just in the middle of the virtual axle linking the robot wheels (see Fig. 4.9). Example 3 Case 1: Differential-drive robot with a cos α = 0 For this case the point of interest for control is the point (xc , yc ) in Fig. 4.9, for which a = 0 and α = 90◦ , so that a cos α = 0. The control law of (4.2) can be written as vd = A−1 ν, ⎡

(4.15)

⎤ cos ψ sin ψ a sin α where A−1 = ⎣− sin ψ cos ψ −a cos α ⎦ is the matrix of inverse extended kinemat0 0 1 T  ics for this case (see (2.16)) and ν = νx ν y νψ is a vector of auxiliary velocities. From such control law one gets that

54

4 Motion Control

u d = νx cos ψ + ν y sin ψ + νψ a sin α,

(4.16a)

vd = −νx sin ψ + ν y cos ψ − νψ a cos α,

(4.16b)

ωd = νψ .

(4.16c)

and

However, as the vehicle is a nonholonomic one, its velocity in the axis y r must be set to zero, or vd = 0. As a consequence, νψ =

−νx sin ψ + ν y cos ψ . a cos α

(4.17)

As for νx and ν y , they can be arbitrarily selected. Therefore, such values can be determined as in (4.8), or νx = x˙dw + k px x˜

(4.18)

ν y = y˙dw + k py y˜ .

(4.19)

where k px and k py are positive nonzero gains, x˜ w and y˜ w are the position errors in the world frame, and x˙dw and y˙dw characterize the velocity of the trajectory. Therefore, the kinematic control commands are given by u d and ωd in (4.16) with the auxiliary velocities defined by the above expressions. To illustrate this example a real experiment considering a trajectory-tracking task performed by the differential robot Pioneer 3AT (see Fig. 1.1) was conducted, with the position of the point of interest at a = 0.4 m and α = π/4 rad. The reference ) and yd (t) = trajectory to be followed was characterized by xd (t) = 2.5 cos( 2πt 100 1.5 sin( 4πt ). As for the gain matrix K entries, they were set to k = k py = 0.6, p px 100 and k pω = 1.8. Fig. 4.10 shows the reference trajectory and the actual trajectory followed by the point of interest of the robot. In addition, Fig. 4.11 shows the velocity commands generated by the controller and the Cartesian errors during the trajectorytracking experiment. It is clear that the control objective is accomplished with quite small errors and good performance. Example 3 Case 2: Differential-drive robot with a = 0 For the case of a = 0 the point of interest for control is exactly in the middle of the virtual axle of the robot. The interest for this specific case is because it corresponds to the case for which the basic kinematics expressed in (2.6) prevents the use of the inverse kinematics controller. However, using the extended kinematics expressed by (2.15) it is now shown that the controller of Fig. 4.1, based on the inverse kinematics technique, can be adopted. The first step is to check the matrix A−1 , which is now

4.5 Trajectory-Tracking Control

55

Fig. 4.10 Trajectory tracking of a differential robot with point of interest for control at a generic position (a cos α = 0 and α ∈ (0◦ , 90◦ )

Fig. 4.11 Trajectory tracking of a differential robot with point of interest for control at a generic position: control commands and Cartesian errors

⎡

A−1

Now, recalling (4.25),

⎤ cos ψ sin ψ 0 = ⎣− sin ψ cos ψ 0⎦ . 0 0 1 vd = A−1 ν,

(4.20)

(4.21)

where ν is again the vector of auxiliary velocities used in the previous case. By imposing the nonholonomic restriction, one gets that vd = −νx sin ψ + ν y cos ψ = 0,

(4.22a)

νx sin ψ = ν y cos ψ,

(4.22b)

from where

56

or, finally,

4 Motion Control



νy ψd = arctan νx

 ,

(4.22c)

from which we can get the desired angular velocity ψ˙ d . Now we can set the auxiliary velocities to (4.23a) νx = x˙dw + k px x˜ w ,

and

ν y = y˙dw + k py y˜ w ,

(4.23b)

˜ νψ = ψ˙ d + k pψ ψ,

(4.23c)

thus generating the velocity commands u d = νx cos ψ + ν y sin ψ

(4.24a)

ωd = νψ .

(4.24b)

and

An important aspect to be stressed here is that the velocity commands generated by the kinematic controller can be directly applied to the robot, without considering the inner control loop of Fig. 4.1. The problem, however, is that a meaningful velocity tracking error may arise, depending on the mass/inertia of the robot. To illustrate this case a real experiment with the differential robot Pioneer 3AT (see Fig. 1.1) was conducted, with the position of the point of interest at ) and a = 0 m. The reference trajectory to be followed was given by xd (t) = 2 cos( 2πt 120 yd (t) = sin( 6πt ). As for the gain matrix K entries, they were set to k = k = 0.6, p px py 120 and k pω = 1.8. Figure 4.12 shows the reference trajectory and the trajectory effectively tracked by the point of interest during control. In addition, Fig. 4.13 gives the velocity commands generated by the controller and the Cartesian errors during the trajectory-tracking experiment. It is clear that the control objective is accomplished with quite small errors and good performance. Moreover, one can verify that the extended kinematic model allows the inverse kinematic controller to be feasible when considering the point of interest on the virtual axle of the robot. As a last example to illustrate the design of a trajectory-tracking controller for nonholonomic robots, let us consider a car-like wheeled mobile robot in its simplified structure of a bicycle like kinematics, as was shown in Fig. 2.7, which is repeated as Fig. 4.14, for convenience. Example 4 Car-like robot with generic point of interest. For this case the point of interest for control is the point (xc , yc ) in Fig. 4.14, placed at a generic position rigidly coupled to the robotic platform. The control law of (4.2) can be written as (4.25) vd = A−1 ν,

4.5 Trajectory-Tracking Control

57

Fig. 4.12 Trajectory tracking of a differential robot with point of interest exactly on the virtual axle (a = 0 and α = 90◦ )

Fig. 4.13 Trajectory tracking of a differential robot with point of interest exactly on the virtual axle: control commands and Cartesian errors Fig. 4.14 A simplified sketch of the car-like robot when the point of interest for control is a generic one, rigidly coupled to the robot platform

58

4 Motion Control

⎡

⎤ cos ψ sin ψ a sin α 0 ⎢− sin ψ cos ψ −a cos α 0⎥ ⎥ is the matrix of inverse extended kinewhere A−1 = ⎢ ⎣ 0 0 1 0⎦ 0 0 0 1 T T   u matics for this case (see (2.19)), vd = d vd ωψd ωδd and ν = νx ν y νψ νδ is a vector of auxiliary velocities. From such control law one gets that u d = νx cos ψ + ν y sin ψ + νψ a sin α,

(4.26a)

vd = −νx sin ψ + ν y cos ψ − νψ a cos α,

(4.26b)

ωψd = νψ .

(4.26c)

ωδd = νδ .

(4.26d)

and

For the kinematics of such vehicle there are two non-holonomic constraints. The constraint on the rear wheel imposes that vd = 0, so that from (4.26) one obtains νψ =

−νx sin ψ + ν y cos ψ . a cos α

(4.27)

As for the constraint on the front wheels, it imposes that the velocity normal to those wheels should be zero, or Lωψ cos δ − u sin δ = 0 (see Fig. 4.14). Therefore, Lω δd = arctan u ψ . To obtain this desired steering it is proposed the auxiliary velocity ˜ νδ = δ˙d + kδ δ,

(4.28)

where kδ is a positive gain constant. As for νx and ν y , they can be arbitrarily selected. For instance, such values can be defined as in (4.8), such that

and

˜ νx = x˙dw + k px x,

(4.29)

ν y = y˙dw + k py y˜ ,

(4.30)

where k px and k py are positive nonzero gains, x˜ w and y˜ w are the position errors in the world frame, and x˙dw and y˙dw characterize the velocity of the trajectory. Therefore, the kinematic control commands are given by u d and ωδd in (4.26) with the auxiliary velocities defined in (4.27), (4.29), and (4.30).

4.6 Positioning Control

59

4.6 Positioning Control Positioning control using inverse kinematics and regarding a final position as well as a final orientation, is possible for omnidirectional robots. The case of non-holonomic robots will be discussed at the end of this section. In this control objective, the same controller adopted to guide the robot when tracking a trajectory can be used, with quite small adaptation. Regarding the control structure of Fig. 4.1 and the control law of (4.8), the difference is simply to take vdw = x˙ dw = 0, since for this case the desired target point xdw is static. Therefore, to guide the vehicle in a positioning task the control structure of Fig. 4.15 is adopted. The difference to Fig. 4.1 is that now the entries of x˙ dw are all zero, thus not demanding to use these values in the kinematic controller. Therefore, the control law based on the inverse kinematics of (4.8) will become simply (4.31) vd = A−1 (K p x˜ w ), with the same variables involved, but the velocity x˙ dw = 0. Once again, as this controller is an extension of the general controller based on inverse kinematics, the stability proof is identical to the case of the generic inverse kinematics controller, and will not be repeated here. An example illustrating the results obtained using the controller here designed is shown now. It considers a real Bebop 2 quadrotor navigating in an outdoor environment, accomplishing a mission correspondent to successive positioning tasks, regarding some waypoints the vehicle should visit. The experimental setup is the same used in the trajectory-tracking experiment, and once more the designed controller starts working when the quadrotor is in the position (0 m, 0 m, 1.2 m), with the orientation of 0◦ , the position/orientation established by the embedded takeoff procedure. From this moment ahead, the designed controller takes the command and guides the quadrotor to visit the waypoints defined in Table 4.1, where the time value represents the instants at which each waypoint is set. Then, the quadrotor should visit the new waypoint, starting from the one it is currently in, having 25 s to get there. For this example just the kinematic controller is used, what means just the outer loop of Fig. 4.15. In other words, the velocity command vd is directly applied to

Fig. 4.15 The inner-outer control structure for the case of positioning

60

4 Motion Control

Table 4.1 Waypoints to be visited by the quadrotor Bebop 2 Waypoint x (m) y (m) z (m) P0 P1 P2 P3 P4

0 5 5 0 0

0 0 5 5 0

18 18 18 18 18

ψ (◦ )

Time (s)

0 0 0 0 0

0 25 50 75 100

Fig. 4.16 The path followed by the Bebop 2 quadrotor when visiting the waypoints of Table 4.1

the vehicle. For such a control loop the gain values are the same adopted for the  T trajectory-tracking example (K p = 1, 1, 3, 1 ). As for the necessary feedback, the position and orientation of the vehicle, it is the result of a data fusion procedure involving raw data provided by an inertial measurement unit and a GPS, procedure that is included in the firmware of the vehicle. Figures 4.16 and 4.17 show the positions of the quadrotor during its navigation, stressing the waypoints, and the position errors along navigation. From this last figure one can see that a great error appears in the coordinates x or y, when a new waypoint is defined, which asymptotically goes to zero as the quadrotor gets closer to the new waypoint. Another important detail is that a positioning control, as it is the case here, does not define the path to be followed by the robot between two consecutive waypoints. However, Fig. 4.16 shows, besides the real path the quadrotor followed, the straight line linking two consecutive waypoints, just to show that the real path is not so different. In other words, the error between the real path and the one of minimum energy, the straight line linking two waypoints, is not big. To conclude this section, a simulated experiment of positioning control is shown now, considering the omnidirectional wheeled robot illustrated in Fig. 2.3. The robot should pass by the waypoints described in Table 4.2, which shows the position and desired orientation at each waypoint, starting navigating from the position (x0 , y0 ) =

4.6 Positioning Control

61

Fig. 4.17 Position errors along the accomplishment of the positioning task

Table 4.2 Waypoints to be visited by the omnidirectional robot Waypoint x (m) y (m) ψ (rad) P1 P2 P3 P4

4 8 8 4

6 6 10 10

0 0 0 0

Time (s) 0 25 50 75

(2 m, 3 m), with initial orientation ψ0 = 0 rad. As in the previous example, each waypoint stands by 25 s. As for the controller, it was used that of (4.10) with the gains set to k px = 7.5, k py = 7.5, and k pψ = 2.5, with saturation constants ksx = 2, ksy = 2, and ksψ = 1. The results are synthesized in Figs. 4.18 and 4.19, which show the path traveled by the robot to visit the established waypoints and the respective x and y positions along the navigation time. In the second case, the switching from a given waypoint to the subsequent one is represented as a vertical segment. An important remark, besides observing that the robot effectively visited the waypoints, is that the orientation of the robot is not meaningful, and could be not considered, as the robot is an omnidirectional one, thus not needing to orientate itself suitably to go to any desired point. This is why it takes the shortest segment when going from one waypoint to the subsequent one. Notice that this means that one could accomplish any navigation task with this robot without considering its orientation. Naturally, when the orientation is an important feature (such as when the robot has a camera which should be pointed always in a given direction, for instance), the orientation ψ of the robot becomes important. To conclude the analysis of the simulated positioning task, Fig. 4.20 shows the absolute values of the position errors in the coordinates x and y along the navigation.

62

4 Motion Control

Fig. 4.18 The path traveled by the omnidirectional robot to visit the desired waypoints

Fig. 4.19 The position of the omnidirectional robot navigating to visit the desired waypoints

As one can see after each waypoint change a big position error, in x or in y, appears, and the robot resume the navigation towards the new waypoint quickly correcting the error, or quickly reaching the new waypoint (much before the next waypoint change). Finally, let us consider briefly the case of non-holonomic robots. By using the general inverse-kinematics controller of (4.2), it is not possible the positioning control of both position and orientation of the robot. This comes from the well-known

4.7 Path-Following Control

63

Fig. 4.20 The absolute value of the position errors along the navigation to visit the established waypoints

Brockett’s condition [6] which states that no smooth feedback control can stabilize the complete state of the system for systems such as non-holonomic robots. Nevertheless, it is still possible just to make position control. To do that, it is followed the same procedure as was described for designing the trajectory-tracking control, w w = 0 instead of xdp . Similar to what was done for trajectory-tracking, but taking vdp the νo values associated to the orientation of the robot, are calculated by imposing the non-holonomic constraints. It is easily proven that the position control errors converge to zero asimptotically, thus verifying the position control objective. The vehicle will end up with a non-controllable orientation. If it would be necessary to control orientation as well, it could be designed a trajectory with this final desired orientation, and make in this case a trajectory control up to the point of interest (where the desired velocity is set to zero). Other solutions not resourcing to full inverse kinematics, can be found for instance in [1, 14, 17].

4.7 Path-Following Control As mentioned before, to control a mobile robot to make it to follow a specified path is quite different of controlling it to track a trajectory. In path-following control there is not a requirement in terms of the velocity the robot should develop at every time instant. As a matter of fact, the velocity which could be constant or time-varying, is defined by the user respecting the robot bounds and any other external requirement,

64

4 Motion Control

Fig. 4.21 Illustration of the path-following control

such as safe speed limits or some optimization criteria, although keeping the direction of the tangent to the path at every time instant to guarantee the robot keeps on the path. Fig. 4.21 illustrates this, considering a differential drive robot. However, such an illustration can easily be extended to the case of a quadrotor evolving in 3D space. In such a figure (xc , yc ) is the point of interest for control, which is the point expected to follow the path defined as c(s), where s is the curvilinear abscissa, and cn is the orthogonal projection of the point of interest for control over the path, which corresponds to the point of the path that is closest to the current position of the point of interest for control. By its turn, n˜ is the distance between the point of interest for control and the point cn , whose projections on the world axes are x˜ and y˜ , whereas t is the unit vector tangent to the path at point cn , whose direction with respect to the axis X w is ψt , and V is the desired velocity of the vehicle along the path at point cn , which should be tangent to the path, that is with direction of t and module V . Therefore, the path-following control objective is to reach the path making n(t) ˜ → 0 as t → ∞ and asymptotically navigate over the path with velocity Vd (s) tangent to the path, which means that it should form the angle ψt with the axis X w , where ψt varies with the position of the robot along the path, according to dy(s) ψt (s) = arctan

ds ds dt dx(s) ds ds dt

dy(s) = arctan

ds dx(s) ds

.

As presented here, the path-following control problem is solved with the same controller structure adopted for the trajectory tracking problem, as shown in (4.4) or in (4.7), but considering the desired velocity as vdw =

dx w dx w ds = Vd . ds dt ds

(4.32)

4.7 Path-Following Control

65

In the case of omnidirectional robots, the path-following control will include the objective of following the desired orientation defined over the path. Considering the case of non-holonomic robots, as for instance the wheeled differential drive robot with the point of interest for control characterized by a cos α = 0 (see Fig. 4.21), the control law ⎡

⎤ cos ψ sin ψ a sin α vd = ⎣− sin ψ cos ψ −a cos α ⎦ ν, 0 0 1

(4.33)

which is similar to (4.25), is applied. From such control law one gets the values in (4.16), and, then, imposing the nonholonomic condition vd = 0 one gets the value for the ν signal related to the orientation, as explained for trajectory-tracking νψ =

−νx sin ψ + ν y cos ψ . a cos α

As regards for ν signal related to the position of the robot, νx and ν y , such values can be determined in the form of (4.4) or (4.7) with vdw defined by (4.32) as the vector of desired velocity on the path νx =

dx Vd + k px x˜ = Vd cos ψt + k px x˜ ds

νy =

dy Vd + k py y˜ = Vd sin ψt + k py y˜ , ds

and

where k px and k py are positive nonzero gains, x˜ w and y˜ w are the position errors in the world frame, regarding the point of the path to be reached at each instant, and Vd is the desired velocity of the robot which may be constant or, in general, a function of the position on the path Vd (s). Now the stability of the closed loop-system when adopting the path-following controller just proposed is discussed regarding the case of the non-holonomic differential robot. The objective is to show that the robot gets to the closest point in the path asymptotically, or, observing Fig. 4.21, to show that n˜ → 0, which is equivalent to x˜ → 0 and y˜ → 0 when t → ∞, which is going to be shown now. Regarding Fig. 4.21, considering a coordinate frame whose origin is the point ˜ the so called Frenet cn , and whose axes coincide with the directions of t and n, coordinates, one gets that x˜ = −n˜ cos(90 − φt ) = −n˜ sin ψt , and y˜ = n˜ sin(90 − φt ) = n˜ cos ψt .

66

4 Motion Control

In addition, n˙˜ = x˙c cos(90 − ψt ) − y˙c cos ψt = x˙c sin ψt − y˙c cos ψt . Regarding the path-following control law of (4.33), with the correspondent values of νx , ν y , and νψ , assuming a perfect velocity tracking, or v = vd , one gets, for the condition a cos α = 0, that x˙ = νx , y˙ = ν y , and ψ˙ = νψ , or ˜ x˙ = Vd cos ψt + k px x, y˙ = Vd sin ψt + k py y˜ , and ψ˙ =

˜ sin ψ (−Vd cos ψt + k px x) (Vd sin ψt + k py y˜ ) cos ψ + . a cos α a cos α

To analyze the behavior of the error n, ˜ let us consider the Lyapunov candidate function V (n) ˜ =

n˜ 2 , 2

for which the first time derivative is V˙ (n) ˜ = n˜ n˙˜ = n˜ x˙ cos ψt − n˜ y˙ cos ψt = nν ˜ x sin ψt − nν ˜ y cos ψt , considering the previous relationships. Therefore, ˜ − n˜ cos ψt (Vd sin ψt + k py y˜ ), V˙ (n) ˜ = n˜ sin ψt (Vd cos ψt + k px x) or

V˙ (n) ˜ = n˜ sin ψt k px x˜ − n˜ cos ψt k py y˜ .

As x˜ = −n˜ sin ψt and y˜ = n˜ cos ψt , it comes that V˜ (n) ˜ = n˜ sin ψt (−k px n˜ sin ψt ) − n˜ cos ψt (k py n˜ cos πt ), or

V˜ (n) ˜ = −k px n˜ 2 sin2 ψt − k py n˜ 2 cos2 ψt ,

so that V˙ (n) ˜ < 0 for all n˜ = 0, once the gains k px and k py be selected as positive values, thus meaning that n˜ → 0when t → ∞, or the closed loop control system is asymptotically stable. As n˜ = x˙ 2 + y˙ 2 , this result means that x˙ → 0 and y˙ → 0 when t → ∞. Therefore, the robot always reaches asymptotically the path and continues over it, with a desired velocity tangent to the path.

4.7 Path-Following Control

67

To complete this stability analysis, it is worth mentioning that a similar stability analysis can be performed for the condition a = 0, and the result is that the closedloop control system is effectively asymptotically stable. Now, a similar reasoning can be followed in the case of omnidirectional robots, as the case of a quadrotor evolving in 3D space. Here we take (4.4) or (4.7) again, with dx w Vd vdw = ds where now x w = [x y z ψ]T which includes the orientation of the vehicle, thus Vd ]T resulting vcw = [Vdx Vdy Vdz dψ(s) ds Considering again Frenet coordinates (n˜ and t), we can note that vdw in (4.3) is on ˜ therefore being perpendicular vecthe t direction and K p x˜ w is on the direction of n, tors. Consequently (4.3) can be splitted in two components in the Frenet coordinates. w In tangent t direction vdw = dxds Vd guarantees the path following while the robot is on ˜ we get the path, including following of yaw angle ψd (s). On the normal direction n, n˜ = x˜ w = −K p x˜ w which implies x˜ w (t) → 0 with t → ∞, and thus approaching the robot asymptotically to the path. Therefore, the path-following objective is attained. In the sequel a simulated example is presented, considering the situation of the non-holonomic differential robot depicted in Fig. 4.21 with a = 20 cm and α = 60 ◦ , with the initial position of the point of interest for control given by (6 m, 1 m) with a robot orientation of 0◦ . The path to be followed is an arc of a circle with radius of 5 m and with center at (8 m, 0 m). In this example it is considered the objective of attaining and following the path in two steps. The first step is to reach the path at the point closest to the initial robot, and the second step is the path-following at a specified velocity Vd , when the robot is on the path. Therefore, νx and ν y are taken as νx = V cos ψt + k px x˜ ν y = V sin ψt + k py y˜ 

with V =

0 Vd

for n˜ > ε, or otherwise,

(4.34)

where ε is a small positive value defining the error below which the robot is considered on the path. As for the gains k px and k py , the values adopted were both 7.5. Finally, the velocity of the robot along the path was set to 0.5 m/s, and the selected value of ε was 5 cm. To calculate the current position of the point of interest for control along the path, from the velocities in the world frame, it is adopted a sampling time of 100 ms, the sampling time for the Pioneer 3-AT robot, which is the time to update the sensory data of such vehicle. Figure 4.22 shows the path the robot followed, firstly to reach

68

4 Motion Control

Fig. 4.22 The path followed by the robot in the simulated example

the path and then to following, which corresponds to the yellow small circles. As for the proposed path, it corresponds to the blue line in the figure. To conclude the analysis of the simulated path-following task, Fig. 4.23 shows the linear and angular velocities u d and ωd commanded to the robot along its navigation. As one can see, there is an initial transient in which the point of interest for control is still not over the path. After it reaches the path, however, the commanded velocities remain constant. An important remark about such a control system is that one can use the tanh() function to limit the velocities commanded to the robot. In this case, one should take ˜ νx = Vd cos ψt + tanh (k px x) and ν y = Vd sin ψt + tanh (k py y˜ ). Figure 4.24 shows the results obtained for the same example of Fig. 4.22, now using the tanh(.) function, in terms of the path traveled by the robot. Comparing the two figures one can check that the robot took more time to get the path to be followed, what is a consequence of the velocity limitation. As a second simulation example, Fig. 4.25 shows the navigation of an omnidirectional robot such as quadrotor accomplishing a path-following task. The path is characterized as x path = 8 − 5 cos(2π s/60), y path = 5 sin(2π s/60), and z path = 2π s/60,

4.7 Path-Following Control

69

Fig. 4.23 The linear and angular velocities commanded during the accomplishment of the pathfollowing task Fig. 4.24 Path traveled by the robot when using the tanh() function to limit the velocities commanded to the vehicle

with the curvilinear abscissa s in the interval [0, 80], with 300 points. In this example the orientation of the quadrotor is not being controlled, what is possible due to the fact that it is an omnidirectional vehicle. The magnitude of the desired velocity is set constant to Vd = 0.25 m/s. The projections of the desired velocity vector on the axes of the world frame, after the quadrotor reaches the path to be followed, are characterized as Vd x = Vd cos θ cos β, Vdy = Vd cos θ sin β,

70

4 Motion Control

Fig. 4.25 A simulated path-following task accomplished by a quadrotor

and Vdz = Vd sin θ, where θ is the angle between the velocity vector Vd = [Vd x Vdy Vdz ]T and the plane X w Y w , and β is the angle between the projection of such a velocity vector on the plane X w Y w and the axis X w . As for the gains of the controller, they are set to k px = k py = k pz = 4, whereas the matrix of direct kinematics, disregarding the orientation of the quadrotor, is

4.7 Path-Following Control

71

Fig. 4.26 Path-following with a Pioneer 3-AT differential robot with point of interest for control characterized by a = 0.25 m and α = 0 ◦ (see Fig. 4.9)

Fig. 4.27 Path-following with the Pioneer 3-AT differential robot: control commands and Cartesian errors

⎡

cos ψ − sin ψ A = ⎣ sin ψ cos ψ 0 0

⎤ 0 0⎦ . 1

Once more the function tanh(.) was used to provide a smooth saturation for the velocity commands, as discussed before. As a final example in this section, it is considered an experiment of path-following with the differential robot Pioneer 3AT. In this example, the point of interest is located ahead from the center of the robot at d = 0.25 m and α = 0◦ . This point should follow an unstructured path as shown in Figure 4.26. The desired velocity is fixed at the constant value Vd = 0.2 m/s. It is used the general inverse kinematics control law of (4.4) with vdw = [Vd cos ψ Vd sin ψ] T and controller gains set to k x = k y = 0.5 and kω = 1.2. It is followed a design procedure similar to that described in relation to Fig. 4.21. Figure 4.26 shows the path

72

4 Motion Control

described by the robot and Fig. 4.27 presents the commanded velocities and the Cartesian errors correspondent to this example. These results show the good performance of the implemented control algorithm.

4.8 Kinematic Sliding Compensation of Non-holonomic Robots The extended kinematic model for non-holonomic robots presented in Sect. 2.1.4 has the advantage of offering a simple solution to the control of non-holonomic robots through inverse kinematics, for any position of the point of interest (including the case it lies on the virtual axle of the robot). Moreover, such a model has the advantage of allowing a simple design of kinematic controllers for the compensation of sliding, when due to navigation or soil conditions the robot violates the nonholonomic velocity constraints. The only condition for such a compensation design is to measure the lateral velocity v (see Figs. 4.9 and 4.14) by using an IMU mounted on the robot, for instance. The design of the sliding compensator follows the same steps described in the previous sections, but now defining the νψ value considering this lateral velocity not being set to zero, but taking the measured value of v. Let us consider the design procedures given in Sect. 4.5, Example 3, Cases 1 and 2, for the differential nonholonomic robot. For a point of interest located at any place on the robot (Case 1), we now get the value of νψ as νψ =

−νx sin ψ + ν y cos ψ − v , a cos α

(4.35)

after substituting the measured value of the lateral velocity v, instead of that of (4.17), which is valid for the non-sliding condition. With this value and those corresponding to νx and ν y as defined in the previous sections for trajectory-tracking or pathfollowing, one can calculate the control actions for the sliding compensation. Regarding the Case 2, and following the same procedure of substituting the measured value of v instead of setting it to 0, one obtains the equations which replace (4.22) and (4.23) valid for the non-sliding condition, which are now 

ν y − (v/ cos ψ) ψd = arctan νx and

˜ νψ = ψ˙d + kψ ψ.

 (4.36)

(4.37)

A completely similar design procedure is valid for trajectory or path-following control, just considering the corresponding value of vdw in the controller, as described in previous sections. Furthermore, similar arguments are considered when controlling

4.8 Kinematic Sliding Compensation of Non-holonomic Robots

73

Fig. 4.28 Path-following of differential robot with sliding compensation: the path followed

Fig. 4.29 Path-following of differential robot with sliding compensation: variables of interest

the car-like non-holonomic robot using the extended kinematics, with the lateral velocity set to the measured value due to the vehicle sliding. To illustrate the application of the sliding compensation, simulations have been made for a differential ground robot and a car-like one, navigating along a rectilinear path and subject to sliding. In the case of the differential robot, the control parameters described in Sect. 4.7 were set to k px = k py = 1.7 and Vd =0.20 m/s. As for the point of interest for control, it corresponded to α = 0rad and a = 0.1 m. Figure 4.28 shows the evolution of the robot along the path, alternating the non-sliding and sliding conditions. Notice that when sliding (shaded area), the robot navigates with an

74

4 Motion Control

Fig. 4.30 Path-following of car-like robot with sliding compensation: the path followed

Fig. 4.31 Path-following of car-like robot with sliding compensation: variables of interest

orientation that is not tangent to the path, to compensate for the sliding. Nevertheless, the point of interest remains on the path all the time, thus achieving the path-following control objective. To complement the analysis of such a simulation Fig. 4.29 shows the Cartesian error related to the point of interest for control, the lateral sliding velocity and the orientation ψ of the vehicle during path-following. Finally, a similar simulation was performed for the car-like robot, using the same control parameters adopted for the case of the differential robot, with the point of interest for control corresponding to α = 0 rad and a = 0.6 m. The corresponding simulation results are shown in Figs. 4.30 and 4.31. In this case we consider the sliding of the rear wheel, with a lateral velocity v, as indicated in Figure 4.14. Once more the Cartesian control error remains close to zero, even in the presence of the sliding, due to a correct compensation of this phenomenon.

4.9 Dynamic Control

75

4.9 Dynamic Control As mentioned in the end of Chap. 3, the dynamic model of a given robot can be used to design controllers implemented as a single control loop or as a cascade controller, the latest ones referred to as controllers implemented as inner-outer control loops. In the first case, or when designing a controller based on a single control loop, one has what is called a dynamic controller, whose inputs are the position/orientation of the robot and whose outputs are velocity commands to be sent to the robot, whereas when using the inner-outer control loops design, the inner loop is implemented with basis on the dynamic model of the vehicle. In this last case the inputs are velocity commands, as well as the outputs. Therefore, when regarding a dynamic controller the closed-loop equation corresponds to a second-order differential equation describing the temporal behavior of the position error, whereas in the case of the design of an inner-outer loops controller the dynamic loop deals with the behavior of the velocity error. This section deals with the case of a single loop controller designed considering the dynamic model of the vehicle. The control objective associated to such a dynamic controller is to make the position error converge to zero asymptotically, which, in practical terms, means to go to an acceptable small value in a finite time (ultimately bounded errors). Velocity commands are sent to the robot to make it to move accordingly, to reach desired positions in the world frame. Notice that this is valid for the differential-drive and the car-like wheeled robots, as well as for the quadrotors here considered. Considering the simplified dynamic model of the quadrotor in (3.31), or ⎤ x¨ w ⎢ y¨ w ⎥ ⎢ w⎥ ⎣ z¨ ⎦ ψ¨ w ⎡

⎡

K 1 cos ψ ⎢ K 1 sin ψ =⎢ ⎣ 0 0 ⎡ K 2 cos ψ ⎢ K 2 sin ψ −⎢ ⎣ 0 0

−K 3 sin ψ K 3 cos ψ 0 0

0 0 K5 0

−K 4 sin ψ K 4 cos ψ 0 0

0 0 K6 0

⎤⎡ ⎤ 0 u vx ⎢ u vy ⎥ 0⎥ ⎥⎢ ⎥ 0 ⎦ ⎣ u vz ⎦ u vψ K7 ⎤⎡ b⎤ vx 0 ⎢ v by ⎥ 0⎥ ⎥⎢ ⎥ 0 ⎦ ⎣ vzb ⎦ vψb K8

= F1 u − F2 x˙ b ,

(4.38)

for instance, one can adopt the control law   u = F1−1 x¨ dw + K D x˙˜ w + K P x˜ w + F2 x˙ b ,

(4.39)

when designing a dynamic controller, for which K P and K D are diagonal positive definite gain matrices, and x¨ dw is the acceleration correspondent to the trajectory being tracked, when regarding trajectory-tracking tasks, or the acceleration along

76

4 Motion Control

Fig. 4.32 The control system implemented as a single-loop dynamic controller for the quadrotors AR.Drone and Bebop 2

the path, for a path-following task. As for x˙˜ w = x˙ dw − x˙ w and x˜ w = xdw − xw , they are the velocity error and the position error. The design of a dynamic controller for a differential drive wheeled robot is quite similar to the case of a quadrotor. Taking the dynamic model of the vehicle, as in (3.3), repeated here for convenience, or vrb = Hv˙ b + C(vb )vb , where the velocities are in the body frame of the vehicle,   v˙ b = H−1 vrb − C(vb )vb = −H−1 C(vb )vb + H−1 vrb = Pvb + bvrb .

(4.40a)

Now, taking the new state vector as being x˙ w = Avb , one gets the new state-space description, based on the concept of equivalent systems [8],

or

x¨ w = APA−1 x˙ w + Abvr = APvb + Abvrb ,

(4.40b)

x¨ w = −AH−1 C(vb )vb + AH−1 vrb = F1 vrb − F2 vb ,

(4.40c)

which is quite similar to (4.38), considering F1 = AH−1 and F2 = AH−1 C(vb ), so that the same controller of (4.39) can be designed for this case. For the specific example of designing a single-loop dynamic controller for a quadrotor, remembering that the quadrotors here considered are equipped with onboard autopilot, a condition to adopt the simplified dynamic model of (3.31) and the control law of (4.39), one gets the control scheme depicted in Fig. 4.32, in which the matrix A is the matrix of direct kinematics of the quadrotor, given in (2.26).

4.9 Dynamic Control

77

Table 4.3 The waypoints the quadrotor should pass by, and the desired orientation in each one of them (each waypoint stands for 20 s) x (m) y (m) z (m) ψ (rad) 5 5 0 0 0

0 5 5 0 0

10 10 10 10 10

0 π /2 π 3π/2 0

To illustrate the implementation of a single loop-dynamic controller, two real experiments are shown in the sequel, with a Bebop 2 quadrotor. Both experiments are outdoor flights, run using the hardware described in Fig. 4.6. The first one corresponds to a controller to guide the quadrotor in a positioning task, during which the vehicle should pass by some waypoints, defined in Table 4.3. The navigation starts being controlled by such a control system when the quadrotor is in the initial position (0 m, 0 m, 1 m), with the initial orientation of 0 rad. As for the gain matrices they are set to K P = diag(1, 1, 3, 1) and K D = diag(1.5, 1.5, 2, 2). In this case, in the control law the term x˙ dw is zero, as well as x¨ dw , since xdw is constant. The result of such an experiment is synthesized in Figs. 4.33 and 4.34, which show a 3D view of the path traveled by the quadrotor moving from a waypoint to the next and the position and orientation errors along the navigation time. As one can see from them, the quadrotor managed to reach each waypoint before a new one is established, with quite small errors when arriving at each waypoint. The big errors, of about 5 m, appear when the waypoint is changed to the next one, and the controller starts working to correct them, which become close to zero when the quadrotor reaches the waypoint, where it stays hovering till the definition of the next waypoint. The second example is an experiment of trajectory-tracking, in which the trajectory to be tracked is a lemniscate of Bernoulli, characterized as   xdw = 5 cos(ωt) m 5 sin(2ωt) m 8 m , with ω = 2π/60 rad/s, for which   x˙ dw = −5ω sin(ωt) m/s 10ω cos(2ωt) ) m/s 0 ) m/s and

  x¨ dw = −5ω2 cos(ωt) ) m/s2 −20ω2 sin(2ωt) ) m/s2 0 ) m/s2 .

Notice that the desired orientation of the vehicle is defined by the orientation of the trajectory, or ψd = arctan( y˙dw /x˙dw ). The initial position and orientation of the quadrotor, as well as the gain matrices, are the same of the positioning experiment.

78

4 Motion Control

Fig. 4.33 A 3D view of the path traveled by the quadrotor when navigating through the waypoints of Table 4.3

Fig. 4.34 Position and orientation errors for the quadrotor navigating through the waypoints of Table 4.3

4.9 Dynamic Control

79

Fig. 4.35 A 3D view of the path traveled by the quadrotor when tracking the lemniscate of Bernoulli trajectory. The red line corresponds to the desired trajectory, whereas the blue line represents the path effectively traveled by the robot

Fig. 4.36 Position and orientation errors for the quadrotor when tracking the lemniscate of Bernoulli trajectory

Figures 4.35 and 4.36 show the result of such an experiment. The first one shows the path traveled by the quadrotor, whereas the second one shows the position and orientation errors along the navigation time.

80

4 Motion Control

4.10 Feedback Linearization Introducing the control law in (4.39) in the dynamic models of (3.31) and of the last subequation in (4.40) one gets   x¨ w = F1 F1−1 x¨ dw + K D x˙˜ w + K P x˜ w + F2 x˙ b − F2 x˙ b , or

x¨ w = x¨ dw + K D x˙˜ w + K P x˜ w ,

or, finally, x¨ dw − x¨ w + K D x˙˜ w + K P x˜ w = x¨˜ w + K D x˙˜ w + K P x˜ w = 0,

(4.41a)

which is a second-order linear homogeneous differential equation describing the behavior of the position error x˜ w . One should notice that both, the robot dynamic model and the dynamic control law, are nonlinear functions. However, the closed-loop equation is a linear differential equation. In other words, the dynamics of the position error is a linear one, what gives the idea of this control technique, the linearization of the error dynamics through the feedback adopted. This is what is called feedback linearization technique. To analyze the stability of the control system implemented with the single loop dynamic controller, which results in the closed-loop equation x¨˜ w + K D x˙˜ w + K P x˜ w = 0, one should take into account that the gain matrices K P and K D are diagonal matrices, so that the system is decoupled. Therefore, one can examine the stability for each component of the position error x˜ w separately. Just to exemplify, consider the variable x˜ w , the component of such an error in the axis x w . For it one gets that x¨˜ w + k Dx x˙˜ w + k P x x˜ w = 0, with k P x and k Dx both positive and nonzero. From such equation, and considering the characteristics of the gain constants, one can conclude that x˜ w (t) → 0 when t → ∞. To get that conclusion one can apply Laplace transform to the differential equation, and check that the correspondent poles have negative real parts. A similar way to prove the asymptotic stability of such a closed-loop system, or T  T  to show that the equilibrium of such a system, x˜ w x˙˜ w = 0 0 is asymptotically stable, is using the theory of Lyapunov [9, 16]. To do that, one can consider the second-order system defined as

4.11 Cascade Dynamic Compensation



81

x˙ 1 = x2 , x˙ 2 = −K D x2 − K P x1 ,

where x1 = x˜ w and x2 = x˙˜ w . Let us select the radially unbounded Lyapunov candidate function 1 1 (4.42) V (x1 , x2 ) = x1T K P x1 + x2T x2 , 2 2 which is positive definite, that is greater than zero for all x1 and x2 , and equal to zero just for x1 = x2 = 0. Taking its first time derivative one gets V˙ (x1 , x2 ) = x1T K P x˙ 1 + x2T x˙ 2 = x1T K P x2 + x2T (−K P x1 − K D x2 ) = −x2T K D x2 ≤ 0

(4.43)

which is a negative semi-definite function. Therefore, from the theory of Lyapunov one can conclude, from (4.42) and (4.43), that the equilibrium is stable, and x˜ w , x˙˜ w are bounded signals. Applying the theorem of La Salle for autonomous systems, the greatest invariant set [5, 16]  M in  R =

  x˜ w w ˙w ˙ : V (˜x , x˜ ) = 0 x˙˜ w 

is M =

   x˜ w 0 ˙x˜ w = 0 .

 T Thus, the greatest invariant set  M is the equilibrium of the system x˜ w x˜˙ w =  T 0 0 , which means that x˜ w (t) → 0 with t → ∞, resulting that the equilibrium is asymptotically stable.

4.11 Cascade Dynamic Compensation The idea to be developed in this section is the cascaded compensation of the robot dynamics using the cascade controller discussed in Sect. 4.3, using the inner control loop of Fig. 4.1 to compensate for any velocity tracking error. As it was mentioned there, the velocity commands generated by the kinematic controller, the outer control loop, can not be developed by the vehicle instantaneously, due to its mass and inertia, not considered in the kinematic analysis. Therefore, a difference between the velocities commanded by the kinematic controller and those effectively developed by the vehicle can appear. Notice that this becomes worse when considering heavier

82

4 Motion Control

Fig. 4.37 The implementation of the dynamic controller as the inner loop of Fig. 4.1

robots. Therefore, it can be mandatory to compensate for the robot dynamics, so that the velocity-tracking error goes to zero with a desired dynamics profile. The inverse dynamics technique can be used to do that, implemented as the inner control loop of Fig. 4.1, as shown in Fig. 4.37, whose inputs are the velocity commands generated by the outer loop control and whose output are different velocity commands that are sent to the robot. The objective of such a control loop is to provide a new dynamic behavior for the velocity-tracking error, guaranteeing that it goes to zero asymptotically. The method consists in canceling the dynamics of the vehicle and then introducing a linear dynamics in its place, which is easily guaranteed to be asymptotically stable. Moreover, this idea is applicable to any mobile robot, not mattering either if it is a ground or aerial robot, for instance, or which is its kinematic model, since this control loop works on the velocity commands delivered by the kinematic controller (the outer control loop). The reason is that this control loop works on the velocity commanded by the kinematic controller, therefore yet referred to the body frame of the robot. When dealing with a differential-drive wheeled robot, let us consider the inverse dynamic model of the vehicle in (3.3) without disturbances, vr = Hv˙ + C(v)v, or its direct dynamics expression v˙ = H−1 [vr − C(v)v] , with all the velocities in the body frame. The dynamic control law   vr = H v˙ d + K D v˜ + C(v)v,

(4.44)

based on the exact inverse dynamic model of the robot, is adopted, for which K D is a positive definite diagonal gain matrix. Substituting this control law in the dynamic model of the robot the resulting closed-loop equation is   v˙ = H−1 H v˙ d + K D v˜ − C(v)v + C(v)v,

4.11 Cascade Dynamic Compensation

83

from which one gets v˙ = v˙ d + K D v˜ , or

v˙ d − v˙ + K P v˜ = v˙˜ + K D v˜ = 0,

which allows concluding that v˜ (t) → 0 when t → ∞, since K D is positive definite. Now, the dynamic behavior of the position error is revisited, though relaxing the assumption of perfect velocity tracking made while designing the kinematic outerloop controller, accepting a velocity error v˜ . Under such a relaxed condition the closed-loop equation in (4.6) applied to trajectory-tracking control (4.9) becomes x˙˜ w + K P x˜ w = A˜v,

(4.45)

where A is the matrix of direct kinematics of the vehicle. Consider the Lyapunov candidate function V = 21 x˜ wT x˜ w , whose time derivative in the system trajectories of (4.45) is V˙ = x˜ wT (A˜v − K P x˜ w ) A sufficient condition for V˙ < 0 is   x˜ wT K P x˜ w > x˜ wT A˜v , with K P = diag(k pi ), i = 1, . . . , n. Such a condition implies that min(k pi )||˜xw ||2 > |˜xw A˜v|, or ||˜xw || >

||A˜v|| , min(k pi )

(4.46)

guaranteeing that for great x˜ w values, regarding that v˜ is a low value, V˙ is negative, thus causing x˜ w to decrease, till not attending the above condition. Then, V˙ would become positive, and the error x˜ w would increase, resuming the condition in (4.46), making V˙ negative once more. Therefore, the error x˜ w would have an ultimately bounded behavior, with the bound defined by (4.46). As we have proved that v˜ (t) → 0, such a bound also asymptotically converges to zero, thus showing that x˜ w (t) → 0, allowing concluding that the dynamic compensation, through the reduction in the velocity tracking error v˜ , effectively contributes to achieve the control objective of reducing the position error x˜ w . Considering a quadrotor in near hover flight, for which the dynamic model of (3.30) is valid, with the matrix f1 given by f1 = diag(K 1 , K 3 , K 5 , K 7 ) and the matrix f2 given by f2 = diag(K 2 , K 4 , K 6 , K 8 ), and remembering that all around the inner loop of Fig. 4.1 the variables are referred to the body frame, the control law

84

4 Motion Control

  vr = f1−1 [˙vd + K D (vd − v) + f2 v] = f1−1 v˙ d + K D v˜ + f2 v

(4.47)

is adopted, which is based on the inverse dynamic model of the quadrotor. For such a control law, vr is the output of the dynamic controller, to be sent to the quadrotor, vd is the velocity command generated by the kinematic controller, whose time derivative is v˙ d , K D is a positive definite diagonal gain matrix, and vd − v = v˜ is the velocitytracking error. Therefore, the way the dynamic controller works is changing the velocity commands to be sent to the quadrotor as a function of the velocity-tracking error. Now, substituting (4.47) in the dynamic model of the quadrotor, one gets the closed loop equation v˙ = f1 f1−1 (˙vd + K D v˜ + f2 v) − f2 v,

(4.48a)

v˙ = v˙ d + K D v˜ ,

(4.48b)

v˙ d − v˙ + K D v˜ = v˙˜ + K D v˜ = 0,

(4.48c)

from which it results

or

from where one can conclude that v˜ (t) → 0 when t → ∞, since K D is positive definite.

4.12 Adaptive Control As discussed in Chap. 3, the dynamic models of the differential drive robot and the quadrotor are based in a set of estimated parameters. However, parameter estimation is a process that embeds an error. In other words, the estimated parameters θˆ , in the case of the differential-drive robot, differs from the true values, which are unknown, ˜ or θˆ = θ + θ˜ . Therefore, the modeled by an estimation error θ˜ , such that θ = θˆ − θ, dynamics are wrongly estimated. For example, if the robot thus modeled is used to transport any payload, so that the total mass changes, the dynamics also changes. In both cases, when the controllers so far designed are adopted control errors certainly arise, because such controllers can not deal with the parametric uncertainties. To reduce such control errors an adaptive dynamic controller is now discussed, to perform as the inner control loop of Fig. 4.1, considering the cases of a differential-drive robot and a quadrotor [10, 13]. For the case of a differential-drive robot, the model in (3.6a) is considered. Such a model is, neglecting the vector of disturbances , Hv + C(v)v + F(v)v = vr ,

(4.49a)

4.12 Adaptive Control

85



with H=

 θ1 0 , 0 θ2

(4.49b)

 C(v) = and F(v) =

 0 −θ3 ω , θ3 ω 0

  0 θ4 , 0 θ6 + (θ5 − θ3 )u

(4.49c)

(4.49d)

which is repeated here for convenience. Now, expanding this equation one gets, T   T  T taking into account that vr = u r ωr , v = u ω , and v˙ = u˙ ω˙ 

          0 ur θ1 0 u˙ u 0 −θ3 ω u −θ4 = + + , ωr 0 θ2 ω˙ θ3 ω 0 0 θ6 + (θ5 − θ3 )u ω ω (4.50a)

or 

     θ1 u˙ − θ3 ω2 + θ4 u θ1 u˙ − θ3 ω2 + θ4 u ur = = , (4.50b) ωr θ2 ω˙ + θ3 ωu + θ6 ω + θ5 ωu − θ3 ωu θ2 ω˙ + θ6 ω + θ5 ωu

or, finally, 

   T ur u˙ 0 −ω2 u 0 0  θ1 θ2 θ3 θ4 θ5 θ6 = Gi θ , = ωr 0 ω˙ 0 0 uω ω

(4.50c)

which is the linear parameterization of the inverse dynamic model of the differentialdrive robot, with   u˙ 0 −ω2 u 0 0 Gi = . (4.50d) 0 ω˙ 0 0 uω ω However, as the true values of the entries of θ are not known, the correct values of H, C(v) and F(v) are not available. As only the estimate θˆ is available, the solution is to use such an estimate when designing the control law. Hence, the control law proposed for the inner loop of the control system is T     v˙ d + T(˜v) +  C(v)vd +  F(v)vd , vr = u r ωr = H

(4.51)

where vd is the desired velocity, provided by the kinematic controller implemented as the outer control loop, and v is the velocity vector effectively developed by the ˆ C(v) ˆ ˆ and F(v), they are the matrices H, C(v) robot, with v˜ = vd − v. As for H, ˆ and F(v) characterized by the estimate θ of θ, whereas    T  , T(˜v) = lu tanh kluu u˜ lω tanh klωω ω˜

86

4 Motion Control

with u˜ = u d − u and ω˜ = ωd − ω being the linear and angular velocity errors. By their turn, ku and kω are the controller gains, which are positive nonzeroconstants,  whereas lu and lω are real nonzero saturation constants, so that lu tanh kluu u˜ and   lω tanh klωω ω˜ have the same signal as u˜ and ω, ˜ respectively. From such a control law one can write ⎤  ⎡   θˆ1 u˙ d + lu tanh kluu u˜ − θˆ3 ωd ω + θˆ4 u d ⎦ , (4.52a)   vr = ⎣  θˆ2 ω˙ d + lω tanh klωω ω˜ + θˆ3 ωu d + θˆ6 ωd + θˆ5 uωd − θˆ3 ωd u  −ωd ω ud 0 0 ˆ σ1 0 vr = θ, 0 σ2 (u d ω − uωd ) 0 uωd ωd 

or

with σ1 = u˙ d + lu tanh



ku u˜ lu



and σ2 = ω˙ d + lω tanh



kω ω˜ lω



(4.52b)

, so that

ˆ vr = Gθ,

(4.52c)

which is the linear parameterization of the inverse dynamic control law of the differential-drive robot . Nonetheless, case the right values θ were available, the control law would be   vr = H v˙ d + T(˜v) + C(v)vd + F(v)vd = Gθ. ˜ one gets vr = Gθ + Gθ˜ , or, using the model However, recalling that θˆ = θ + θ, in (4.49a), vr = Hσ + C(v)vd + F(v)vd + Gθ˜ = Hv˙ + C(v)v + F(v)v, from where one gets H(σ − v˙ ) + C(v)(vd − v) + F(v)(vd − v) + Gθ˜ = 0,  T with σ = σ1 σ2 = v˙ d + T(˜v). However, as v˜ = vd − v, and thus v˙˜ = v˙ d − v˙ , one gets v˙ d = v˙˜ + v˙ , so that σ = v˙˜ + v˙ + T(˜v), and hence σ − v˙ = v˙˜ + T(˜v), resulting that   H v˙˜ + T(˜v) + C(v)˜v + F(v)˜v = −Gθ˜ , (4.53) which is the closed-loop equation describing the behavior of the velocity error along time. To analyze the stability of the inner control loop thus designed, let V (˜v, θ˜ ) = T 1 T v˜ Hv˜ + 21 θ˜ γ −1 θ˜ be the radially unbounded Lyapunov candidate function, which 2

4.12 Adaptive Control

87

is positive for all nonzero v˜ and θ˜ and zero for v˜ = 0 and θ˜ = 0, since H > 0 and γ is chosen as a diagonal positive definite matrix. Thus, the time derivative of such a function is T ˙˜ V˙ (˜v, θ˜ ) = v˜ T Hv˙˜ + θ˜ γ −1 θ, or

  T V˙ (˜v, θ˜ ) = v˜ T −Gθ˜ − C(v)˜v − F(v)˜v − HT(˜v) + θ˜ γ −1 θ˙˜ ,

taking (4.53) into account. Moreover, as C(v) is a skew symmetric matrix (see the model properties in Chap. 3), the term v˜ T C(v)˜v in the above equation is null, thus resulting T V˙ (˜v, θ˜ ) = −˜v T Gθ˜ − v˜ T F(v)˜v − v˜ T HT(˜v) + θ˜ γ −1 θ˙˜ . Now, considering that θ is constant, or θ˙ = 0, one gets that θ˙ˆ = θ˙˜ . Considering updating the estimated parameters used in the control law as a function of the velocity error, such that (4.54) θ˙ˆ = γ GT v˜ = θ˙˜ , the time derivative of the Lyapunov candidate function becomes V˙ (˜v, θ˜ ) = −˜v T F(v)˜v − v˜ T HT(˜v), because the last term in the right hand compensates for the first one when introducing (4.54). This way, V˙ (˜v, θ˜ ) is negative semi-definite, since it is zero for any θ˜ when v˜ = 0, and is negative for any nonzero value of v˜ , since F(v) > 0, H > 0 (see the model properties in Chap. 3), and T(˜v) has the same signal of v˜ . From such result, and recalling that V is radially unbounded, it is possible to conclude that v˜ ∈ L ∞ and θ˜ ∈ L ∞ , meaning that both errors are bounded. Now, taking the integral of V˙ (˜v, θ˜ ) over the time interval [0, T ], with T finite, one has T T T V (T ) − V (0) = − v˜ HT(˜v) dt − v˜ T F˜v dt, 0

0

which becomes, after dropping the positive terms V (T ) and the first integral, T v˜ T F˜v dt,

−V (0) ≤ − 0

or

T v˜ T F˜v dt.

V (0) ≥ 0

88

4 Motion Control

However, as F is diagonal and positive definite (see the model properties in Chap. 3), the inequality λmin (F)||˜v||2 ≤ v˜ T F˜v ≤ λmax (F)||˜v||2 holds. Nonetheless, considering that λmin (F) and λmax (F) can vary with u, one can write inf[λmin (F)]||˜v||2 ≤ v˜ T F˜v ≤ sup[λmax (F)]||˜v||2 , where inf(.) means the infimum of the value, and sup(.) means the supremum of the value. Using such result in V˙ (.) one has T V (0) ≥ inf[λmin (F)]

||˜v||2 dt. 0

As such equation is valid for all integration time T, one can write ∞ V (0) ≥ inf[λmin (F)]

||˜v||2 dt, 0

or

∞ ||˜v||2 dt ≤ 0

V (0) , inf[λmin (F)]

allowing concluding that v˜ is square integrable, or v˜ ∈ L 2 . As vd is bounded, since it comes from the kinematic controller, which configures the outer control loop in Fig. 4.1, as well as v˜ , since v˜ ∈ L ∞ , and remembering that v˜ = vd − v, v is also bounded, so that C(v) and F(v) are bounded. Assuming that v˙ d is limited, the conclusion is that G is also limited. Therefore, (4.53) allows concluding that v˙˜ is limited, so that v˙˜ ∈ L ∞ and v˜ ∈ L 2 , which allows to state, using the Lemma of Barbalat [3], that v˜ (t) → 0 when t → ∞, thus achieving the control objective of reducing the velocity error between the desired velocity and the effective one ideally to zero. Considering the case of a quadrotor, whose model, in body coordinates, as seen in Chap. 3, is (4.55) v˙ = f1 u − f2 v, with f1 = diag(k1 , k3 , k5 , k7 ) and f2 = diag(k2 , k4 , k6 , k8 ). From such a model one can write u = vr = f1 −1 v˙ + f1−1 f2 v,

4.12 Adaptive Control

89

where vr , instead of u will be used from now on, for compatibility with (4.49a), with  T v = vx v y vz vψ . From such an equation it comes that ⎡

θ1 v˙ x ⎢ θ3 v˙ y vr = ⎢ ⎣ θ5 v˙ z θ7 v˙ψ or

⎡

v˙ x ⎢0 vr = ⎢ ⎣0 0 

vx 0 0 0

0 v˙ y 0 0

0 vy 0 0

⎤ + θ2 vx + θ4 v y ⎥ ⎥, + θ6 vz ⎦ + θ8 vψ 0 0 v˙ z 0 

0 0 vz 0

0 0 0 v˙ψ

⎤ 0 0⎥ ⎥ θ, 0⎦ vψ 

(4.56)

Mθ

where

 T  θ = θ1 θ2 θ3 θ4 θ5 θ6 θ7 θ8 = k11

k2 1 k4 1 k6 1 k8 k1 k3 k3 k5 k5 k7 k7

T

.

Notice that this is the linear parameterization of the inverse dynamic model of the quadrotor. Finally, such a inverse model can also be written as ⎡

θ1 ⎢0 vr = ⎢ ⎣0 0 

0 θ3 0 0

0 0 θ5 0 

⎤ ⎡ θ2 0 ⎢0 0⎥ ⎥ v˙ + ⎢ ⎣0 0⎦ θ7 0  

D˙v

0 θ4 0 0

0 0 θ6 0  η

⎤ 0 0⎥ ⎥ v, 0⎦ θ8 

or vr = D˙v + η. Similarly to the case of the differential-drive robot, the control law adopted, based on the inverse dynamics, is      vr = D v˙ d + L tanh L−1 K p v˜ + η = D v˙ d + T(˜v) + η,

(4.57)

where K p = diag(k px , k py k pz k pψ ) is a diagonal positive gain matrix, L = diag(l x , l y , l z , lψ ) is a real diagonal matrix defining the saturation level, and v˜ = vd − v is the velocity error. The function tanh is used once more, to provide a smooth saturation of the control signal. Considering such a control law one can write vr = Gθ,

(4.58)

90

4 Motion Control

Fig. 4.38 Illustration of how to implement the adaptive dynamic control as the inner loop of the inner-outer control structure

where

⎡

g11 ⎢0 G=⎢ ⎣0 0

vx 0 0 0

0 g23 0 0

0 vy 0 0

0 0 g35 0

0 0 vz 0

0 0 0 g47

⎤ 0 0⎥ ⎥ 0⎦ vψ

    k k g11 = v˙d x + l x tanh lpxx v˜ x , g23 = v˙dy + l y tanh lpyy v˜ y , g35 = v˙dz +     k k . v ˜ l z tanh lpzz v˜ z , and g47 = v˙dψ + lψ tanh lpψ ψ ψ Now, considering the estimate θˆ of θ, such that θ˜ = θˆ − θ , the control law of (4.58) based on the estimated parameters becomes with

  vr = Gθˆ = Gθ + Gθ˜ = D v˙ + T(˜v) + η + Gθ˜ . Hereinafter the stability analysis is identical to the one developed for the case of a differential-drive robot, including using the same parameter adaptation law. The conclusion is that the velocity error v˜ (t) → 0 when t → ∞, meaning that the velocity error converges to zero asymptotically. Figure 4.38 depicts the scheme of the inner control loop implemented as an adaptive dynamic compensator, for a differential-drive robot and a quadrotor as well, as discussed above. Some examples of the use of the adaptive dynamic compensator are shown in the sequel. The first one simulates a Pioneer 3 DX robot following a trajectory. To run this simulation, the dynamic parameters of the robot, identified as the vector  T θˆ = 0.3037 0.2768 −0.0004018 0.9835 0.003818 1.0725 , are changed, reducing their value in 1/3, to simulate a bad identification. Then, after 100 s of simulation, with a sampling time of 100 milliseconds, the parameters start being updated, according to the updating law yet discussed, and the simulation continues till t = 200 s. The simulated task is a trajectory-tracking one, where the trajectory to be tracked is a lemniscate of Bernoulli characterized by xd = 2.5 sin (0.3t) and yd = 2.5 cos (0.15t).

4.12 Adaptive Control

91

Fig. 4.39 The desired and the effectively tracked trajectories for a differential-drive robot before and after the parameter updating. The robot is guided by an adaptive controller whose parameter updating law starts working at t = 100 s

The adaptive inner-outer controller is implemented to guide the robot during the simulation. The kinematic outer loop controller (4.10) is configured with the gains k px = k py = 0.75 and the saturation constants ksx = ksy = 0.1, with the point of interest for control characterized by a = 0.2 m and α = 0◦ . As for the gains of the dynamic controller (4.51) correspondent to the inner control loop, they are ku = kω = 4, with saturation constants lu = lω = 1. By its turn, the matrix γ in the parameter updating law is γ = diag(1.7, 1.1, 0.5, 0.3, 0.001, 0.5). Figures 4.39, 4.40 and 4.41 illustrate the result of the simulation. One can see from Figs. 4.39 and 4.40 that after the parameter updating law starts working the parameters are corrected, what becomes clear checking the difference in terms of the tracking error before and after the parameter updating. The position (x, y) of the robot along navigation, shown in Fig. 4.41 also allows perceiving the performance improvement. To conclude this section an experiment involving the adaptive control of a quadrotor is presented, considering a trajectory-tracking task. The Bebop 2 drone is used, for which the parameters shown in Table 3.1 are not used. Instead, the experiment starts with the parameters arbitrarily chosen as K 1 = 2.5, K 3 = 2.5, K 5 = 10, K 7 = 10, K 2 = 2.5, K 4 = 2.5, K 6 = 10, and K 8 = 10, As a consequence, the vector θ  T of (4.56) becomes θ = 0.4 1 0.4 1 0.1 1 0.1 1 , whose entries are quite different from those correspondent to the parameters of Table 3.1. Therefore it is expected

92

4 Motion Control

Fig. 4.40 Tracking error for the differential-drive robot in a trajectory-tracking task guided by an adaptive controller, for which the adaptation starts at t = 100 s

that the drone navigates with a quite big error with respect to the desired trajectory, reducing it as long as the parameters are being updated. The desired trajectory is defined as  T xd = cos(2π/T ) sin(4π/T ) 1 − 0.5 cos(2π/T ) , with T = 18 s and a sampling rate of 30 Hz. The experiment has a duration of 60 s, with the parameter adaptation starting at the instant t = 15 s. Another detail is that  T in the first 3 s the quadrotor goes to the position x0 = 0 0 1 , where its orientation should be 0 ◦ . Then, it keeps hovering until t = 15 s, when it starts tracking the desired trajectory. The control system designed is based on the inner-outer loops structure, as illustrated in Fig. 4.38. The outer loop corresponds to a kinematic controller, which generates the desired velocity vd shown in the figure. However, as the data relative to the position and velocities of the quadrotor are obtained using the OptiTrack motion capture system, all the values in the figure are in global coordinates, except by vr . In addition, to calculate the parameters update law v˜ w is converted to body coordinates, resulting in v˜ . However, the parameter updating does not work continuously. Instead, when the quadratic norm of the velocity tracking error v˜ is less than 2.5 cm/s it is considered that the parameters have reached their final values, stopping the parameters updating until an error value greater than such a value

4.12 Adaptive Control

93

Fig. 4.41 Robot position in a trajectory-tracking task guided by an adaptive controller, for which the adaptation starts at t = 100 s

is detected (this corresponds to creating a dead zone of ±2.5 cm/s for the velocity tracking error, to avoid the continuous parameter updating). Therefore, one can write vdw = x˙ dw + K p x˜ w , such that v˜ w = vdw − x˙ w = x˙ dw + K p x˜ w − x˙ w , resulting in the velocity tracking error v˜ w = x˙˜ w + K p x˜ w , from which the value v˜ is obtained using the matrix of inverse kinematics of the quadrotor. The dynamic compensation is performed considering σ w = v˙ dw + Kd (vdw − x˙ w ). Then, σ b and xb , in body coordinates, are obtained, and thus one gets ⎤ 0 0 0 0 0 σ b (1) x˙ b (1) 0 ⎢ 0 0 0 0 ⎥ 0 σ b (2) x˙ b (2) 0 ⎥, G=⎢ ⎣ 0 0 ⎦ 0 0 0 σ b (3) x˙ b (3) 0 0 0 0 0 0 0 σ b (4) x˙ b (4) ⎡

and then vr = Gθ is obtained. To update the vector of parameters, as shown in Fig. 4.38, one calculates θ m = θ m−1 + γ −1 GT λTs , where Ts is the sampling period and λ = v˜ if ||˜v||2 ≥ 2.5 cm/s, and λ = 0 otherwise (in this last case the parameters are not updated).

94

4 Motion Control

Fig. 4.42 The variation of the parameters K i , i = 1, . . . , 8, with the final values shown

As for the gains K p and Kd , they are K p = diag(4, 4, 5.4, 7.5) and Kd = diag(2, 2, 1.8, 5), whereas the matrix γ −1 of the parameter updating law is γ −1 = diag(60, 60, 1, 0.1, 20, 20, 1, 0.1). The results of such an experiment are synthesized in Figs. 4.42, 4.43, 4.44 and 4.45. In the first one it is possible to perceive that the arbitrary parameter values converged to the final values showed in the figure, as long as the position error with respect to the desired trajectory becomes quite close to zero, as Fig. 4.43 shows. As a consequence, the tracking of the desired trajectory becomes much better as long as the parameters are being updated, as Figs. 4.44 and 4.45 show. As one can see from the experimental results, the adaptive controller performed as expected, making the initial big tracking error to decrease to quite low values.

4.13 Concluding Remarks This chapter explores two basic techniques to design a controller to guide differential drive wheeled robots and quadrotors, namely the single-loop dynamic control system and the inner-outer loops control system, which can be applied to positioning tasks, trajectory-tracking tasks and path-following tasks, with quite small adaptations, as discussed and illustrated along the text. In addition to the design of controllers based on the inner-outer loops structure, in which the inner loop performs a compensation of the dynamics of the vehicle, the chapter also shows how to design an adaptive dynamic compensator to be used as the inner control loop, which is quite useful in situations

4.13 Concluding Remarks

Fig. 4.43 The position errors with respect to the desired trajectory along the experiment Fig. 4.44 The path traveled by the quadrotor along the experiment

95

96

4 Motion Control

Fig. 4.45 The position of the quadrotor with respect to the desired trajectory along the experiment

in which the robot transports loads, which changes its dynamic parameters, as well as to compensate for any misidentified dynamic parameter or external disturbances. The focus, in both designs, is the inverse kinematics technique to design the outer loop correspondent to the inner-outer loops structure, and the inverse dynamics technique to design the inner loop of the inner-outer loops structure and the singleloop dynamic controller, which consists in inverting the nonlinear kinematics or dynamics, thus generating a linear closed-loop system equation. The most important contribution is to show that the technique is powerful enough to be applied to any motion strategy, as exemplified along the chapter.

References 1. Aicardi M, Casalino G, Balestrino A (1995) Closed loop steering of unicycle-like vehicles via Lyapunov techniques. IEEE Robot Autom Mag 2(1):27–35 2. Antonelli G (2009) Stability analysis for prioritized closed-loop inverse kinematic algorithms for redundant robotic systems. IEEE Trans Robot 25(5):985–994. https://doi.org/10.1109/ TRO.2009.2017135 3. Astrom KJ, Wittenmark B Chapter 6: Model-reference adaptive systems, 2nd ed. Dover Books on Engineering. Dover Publications, Inc., Mineola, NY, USA 4. Bolton W (2004) 13-controllers. In: Bolton W (ed) Instrumentation and control systems. Newnes, Oxford, pp 290–302. https://doi.org/10.1016/B978-075066432-5/50013-9 5. Brauer F, Nohel JA (1989) The qualitative theory of ordinary differential equations: an introduction. Dover Publications Inc, New York, USA 6. Brockett R (1983) Asymptotic stability and feedback stabilization. In: Brockett RW, Millman RS, Sussmann HJ (eds) Differential geometric control theory 7. Cao N, Lynch AF (2016) Inner-outer loop control for quadrotor UAVs with input and state constraints. IEEE Trans Control Syst Technol 24(5):1797–1804. https://doi.org/10.1109/TCST. 2015.2505642 8. Chen CT (1999) State-space solutions and realizations. In: Chen CT (ed) Linear systems theory and design, 3rd ed. Oxford University Press, pp 86–116 (1999) 9. Khalil HK (2002) Nonlinear systems, 3 ed. Prentice Hall

References

97

10. Martins FN, Celeste WC, Carelli R, Sarcinelli-Filho M, Bastos-Filho TF (2008) An adaptive dynamic controller for autonomous mobile robot trajectory tracking. Control Eng Prac 16(11):1354–1363. https://doi.org/10.1016/j.conengprac.2008.03.004 11. Montoya-Villegas L, Moreno-Valenzuela J, Pérez-Alcocer R (2019) A feedback linearizationbased motion controller for a UWMR with experimental evaluations. Robotica 37(6):1073– 1089. https://doi.org/10.1017/S0263574718001443 12. Naldi R, Furci M, Sanfelice RG, Marconi L (2017) Robust global trajectory tracking for underactuated VTOL aerial vehicles using inner-outer loop control paradigms. IEEE Trans Autom Control 62(1):97–112 13. Santos MCP, Rosales CD, Sarapura JA, Sarcinelli-Filho M, Carelli R (2019) An adaptive dynamic controller for quadrotor to perform trajectory tracking tasks. J Intell Robot Syst 93(1– 2):5–16. https://doi.org/10.1007/s10846-018-0799-3 14. Secchi H, Carelli R, Mut V (2003) An experience on stable control of mobile robots. Latin Am Appl Res 33(4):379–386 15. Siciliano B (1990) A closed-loop inverse kinematic scheme for on-line joint-based robot control. Robotica 8(3):231–243. https://doi.org/10.1017/S0263574700000096 16. Slotine JJ, Li W (1991) Applied nonlinear control. Prentice Hall, Englewood Cliffs, NJ, USA 17. Toibero JM, Roberti F, Carelli R, Fiorini P (2011) Switching control approach for stable navigation of mobile robots in unknown environments. Robot Comput-Integr Manuf 27:558–68. https://doi.org/10.1016/j.rcim.2010.10.002

Chapter 5

Control of Multi-robot Systems

Abstract Two approaches to control a multi robot system, also known as a robot formation or robot cluster, namely the leader-follower and the virtual structure approaches, are dealt with in this chapter. They are discussed in details, and examples showing their implementation to control a group of two or more robots are also discussed, considering groups of two or more robots of the same kinematics and of different kinematics. The scalability of such systems to control more than two or three robots is also addressed. The chapter also discusses how to implement a hierarchical control structure, when regarding conflicting control objectives, showing how to implement a control system taking into account the different priorities assigned to the subtasks involved in the accomplishment of the whole task.

5.1 Introduction In this chapter some ideas on the control of multi robot systems, also called multi robot formations, or simply robot formations, are discussed. First of all, the main characteristic of the control of a multi robot system should be stressed. To accomplish a desired task, all the robots in the formation should move in such a way to contribute to the task accomplishment. There is no possibility of independent movement of the elements in the formation. They should move accordingly, to achieve the common objective. This introduces the key word coordination in the context. The elements of the formation should move in a coordinated way, not independently. Therefore, the control system of each robot should take into account the data relative to position and orientation of all the elements of the formation, to know the current state of the formation, and thus define the commands to be sent to each element of it. In this sense, two ways are available to implement the control system. It may be a centralized control system, gathering and processing the information of all agents, which corresponds to a control station that can be external to all the robots or can be on board of one of the agents in the formation. The centralized control system should receive information from all the agents, calculate all the velocity commands and send them to each robot. In this case, the communication mesh needs a full duplex channel © The Author(s), under exclusive license to Springer Nature Switzerland AG 2023 M. Sarcinelli-Filho and R. Carelli, Control of Ground and Aerial Robots, Intelligent Systems, Control and Automation: Science and Engineering 103, https://doi.org/10.1007/978-3-031-23088-2_5

99

100

5 Control of Multi-robot Systems

linking each robot and the control station. Case the control system be a decentralized one, meaning that each robot has its own controller, each robot should receive the position and orientation of all the other components of the formation, as well as to send its own position and orientation to all the others. The communication mesh, in this case, would involve a full duplex channel linking each robot in the formation and all the others, which is much more complex, mainly when the number of elements in the formation increases.

5.2 Characterization of a Robot Formation Two main paradigms are considered in this chapter to perform the control of a robot formation: the leader-follower and the virtual structure approaches. In the leader-follower approach one robot is the leader, whose movement should be replicated by a second robot, the follower one [13, 26]. However, the controller guiding the follower robot should preserve a previously established distance between both agents and a relative angle, defined as the angle formed by the line segment linking the two robots and the axis x w , when regarding ground vehicles. When regarding quadrotors, as the line segment linking the two agents is a straight line in the 3D space, besides the distance between the robots two angles should be established, the one between the projection of the line segment on the x w y w plane and the axis x w , and the one between the line segment and the x w y w plane. Notice that this formation, conceptually speaking, is interesting to allow two robots to carry a load in the form of a bar, for instance. In such case, the two robots should be kept apart enough to guarantee the balance of the load, and should move with close velocities, to guarantee that the bar would not fall down. Thus, the controller of the follower robot is in charge of just moving it to replicate the movement of the leader one, which is independently controlled. However, although being a quite interesting paradigm to control a multi-robot system, the leader-follower formation has a strong drawback. If the leader presents any failure preventing its movement, the whole formation stops moving, since the follower replicates the movement of the leader. Therefore, with the leader stopped the follower also stops. In the eventuality of a leader-follower chain, when the first follower, which is the leader for the third robot in the chain, stops, its follower also stops, so that the whole chain stops. Regarding the virtual structure approach, there is no leader agent anymore [18]. The whole formation is considered as a single agent, a virtual one. Therefore, the movement of the whole formation is defined by the movement of a virtual robot placed in a point of a geometric figure characterizing the formation. For instance, let us consider a three-robots formation. Naturally one thinks on a triangle composed by such robots, such as in [1, 17, 25], to mention the use of such an approach for different robot types (wheeled robots, water vessels and drones, respectively). In such a case, the virtual robot could be over the center of mass of the virtual structure (the triangle). Other example could be the virtual structure corresponding

5.2 Characterization of a Robot Formation

101

to a formation of a wheeled ground robot and a drone, such as in [7, 23], or two drones, such as in [28], for which the virtual robot can be placed at any point of the line segment corresponding to the virtual structure, such as in one of the extremities, in this case coinciding with one of the real robots in the formation. The first meaningful difference when comparing with the leader-follower structure is that the virtual robot, responsible for the motion of the formation, is not susceptible to failures, for being virtual. Therefore, the whole formation can continue moving, even when one or more robots in the formation stops due to any failure. In such a case all one needs is to reconfigure the formation. Moreover, the virtual robot demands just a kinematic controller, since it does not have mass or inertia, for being virtual. The virtual structure approach consists in designing a kinematic controller for the virtual robot to control the motion of the formation. From the velocity commands generated by such controller the velocity commands to be sent to all the robots of the formation are generated. In other words to move the formation is to move the virtual robot, whose movement defines how each robot in the formation should move, to establish/preserve the desired structure. Therefore, two control instances are considered, which are the control of the virtual robot characterizing the formation, and the control of each robot of the formation. Notice that the movement of the virtual robot characterizing the formation may accomplish a trajectory-tracking, positioning or path-following task. As for the individual robots, they always perform trajectorytracking, since each one receives references of desired position and velocity at each time instant. An important aspect, in this case, is that all the robots of the formation are controlled simultaneously, with the references generated by the controller designed for the virtual robot characterizing the formation. Notice that this is in opposition to what happens when considering a leader-follower approach, since there the controller designed for the follower does not generate any reference for the leader robot. Thus, in the leader-follower approach each robot is independently controlled, whereas in the virtual structure approach all the robots of the formation are simultaneously controlled, as it will become clearer ahead. Another meaningful aspect of multi robot systems or robot formations is their characterization with respect to the type of robot it includes. A formation is said to be an homogeneous one when all its components are vehicles of identical kinematics, such as a formation composed of only quadrotors, such as in [25, 28], or only wheeled robots, such as in [17, 24]. On the other hand, when robots of different kinematics are included in the formation, such as in [7, 23] or [16], the formation is an heterogeneous one. As for the motivation to adopt a robot formation to accomplish a given task, instead of just a single robot, there are some important aspects which deserve mentioning. One of them is that the nature of the task to be accomplished by itself can demand a group of robots, to achieve better performance. This is the case when detecting land mines in a certain terrain, for instance. More inspection robots allow to cover the same area in much less time, and the destruction of one robot of the squad does not prevent the continuity of the task, what would be the case when using just one robot. In other situations the robots included in the squad can be much cheaper than the single one necessary to accomplish the same task. In tasks such as search and rescue in disaster

102

5 Control of Multi-robot Systems

areas, the need of searching for survivors in the least time possible demands a group of robots, to reduce the time necessary to accomplish such a task. In other situations, such as in warehouse automation, the combination of a wheeled robot with a drone taking off from it and landing on it, allows to access merchandise stored in higher shelves, not accessible for the ground vehicle. Thus, in many applications the use of a robot formation allows achieving much better performance, with more reliability and, sometimes, at a lower cost [22].

5.3 Leader-Follower Paradigm In a leader-follower formation, the follower robot accomplishes the task of mimicking the movement of the leader one. This means that the two robots are independently controlled, although this may be performed by controllers running in a same computer. Another possibility is that the leader be remotely controlled by a human operator, whereas the follower is controlled by an automatic controller responsible to send velocity references for it. Notice that in this case it is possible to have an homogeneous formation, with two wheeled robots or two quadrotors, for instance, or an heterogeneous formation, composed by a wheeled robot and a quadrotor, for instance. Figures 5.1 and 5.2 illustrate two homogeneous leader-follower formations, the first one composed by two wheeled robots Pioneer 3-DX and the second one composed by two quadrotors Bebop 2. Notice that the following of the leader robot by the follower one establishes a certain distance between them, and angles characterizing the inclination of the line segment that links the point of interest for control of the two robots, as represented in the two figures. An important remark is that a leader-follower multi-robot system (MRS) may comprise more than two robots. The way to implement this is to consider the whole system as a chain, where the first robot is the leader being followed by the second robot in the chain, which is also the leader for the third robot and so on. From the Figs. 5.1 and 5.2 one can perceive that the leader robot defines a trajectory to be tracked by the follower robot. Thus, all the follower robots should do is to track a trajectory defined by xdwF (t) = xwL (t) − , x˙ dwF (t) = A L vbL (t),

(5.1)

where  defines the desired position error between the leader and follower robots, and A L is the matrix of direct kinematics of the leader robot. Thus, it does not matter which task the leader is accomplishing (a positioning, a trajectory-tracking or a pathfollowing one), the follower robot should always track a trajectory, established by the leader robot.

5.3 Leader-Follower Paradigm

103

Fig. 5.1 Illustration of a leader-follower formation of two Pioneer 3-DX robots

Fig. 5.2 Illustration of a leader-follower formation of two Bebop2 quadrotors

Therefore, the controller to be designed for the follower robot is a trajectorytracking one, as seen in Chap. 4, with the trajectory to be tracked defined as above. In the case of a wheeled robot, the variables ρ L F and β L F characterize the formation (see Fig. 5.1). From them one can get T   = ρ L F cos β L F ρ L F sin β L F .

104

5 Control of Multi-robot Systems

In the case of a quadrotor, the variables describing the formation are ρ L F , α L F and β L F , shown in Fig. 5.2. From them one gets T   = ρ cos α L F cos β L F ρ cos α L F sin β L F ρ sin α L F . To illustrate this design, consider a leader follower MRS composed of two Pioneer 3-DX robots, both with the point of interest for control at a distance of 20 cm of the point in the middle of the virtual axle, in an angle of 30◦ with the axis x r (see Fig. 5.1). The leader should track a circular trajectory characterized as  T xdw = 8 − 5 cos( 2πt ) 5 sin( 2πt ) , 60 60 for which

x˙ dw =

 10π 60

sin( 2πt ) 60

10π 60

cos( 2πt ) 60

T

,

during 55 s. Notice that the leader robot tracks a prescribed trajectory, so that two trajectory-tracking controllers similar to that described by (4.25) are designed, one for the leader robot and another for the follower one. Such a task is simulated, considering that the leader robot starts moving from the initial position (6 m, 1 m), whereas the follower robot starts moving from the position (8 m, 0 m), both with orientation 0◦ . As for the controllers, the gains are set to K P = diag(4, 4) for the leader robot, and K P = diag(7.5, 7.5) for the follower one. Finally, the desired formation shape is described by ρ L F = 4 m and β L F = π/4 rad. The graphics in Figs. 5.3 and 5.4 show the results of this simulation. The first one shows the trajectory programmed for the leader, the trajectory the leader establishes for the follower, and the paths effectively traveled by the leader and the follower, besides the initial positions of both robots (see the legend of the figure). As for the second of these figures, it shows the behavior of the variables ρ L F and β L F , which characterize the leader-follower formation, in this example, along the navigation. From the figures one can perceive that the leader, after an initial transient, managed to reach the programmed trajectory, thus causing the follower to also reach the trajectory the leader defines for it, and making the values of ρ L F and β L F to reach their programmed values, which are preserved all along the rest of the navigation. Let us now consider a leader-follower formation of two quadrotors (see Fig. 5.2). Figures 5.5 and 5.6 show the paths effectively followed by the robots against the desired trajectories, and the temporal evolution of the variables ρ L F , α L F and β L F describing the formation. The leader robot is programmed to track the same circular trajectory of the previous simulation, now at a constant altitude of 8 m. As for the initial positions of the robots, they are (5 m, 1 m, 1 m), for the leader robot, and (8 m, 0 m, 1 m), for the follower robot, both with orientation 0 rad, whereas the adopted gains are K P L = diag(4, 4, 4, 4) for the leader quadrotor, and K P F = diag(7.5, 7.5, 7.5, 7.5) for the follower quadrotor. From the figures one can perceive that the two robots, after an initial transient, effectively track the respective trajectory. However, as the leader quadrotor is quite far

5.3 Leader-Follower Paradigm

105

Fig. 5.3 The paths traveled by the leader and follower robots, considering that the leader tracks a given trajectory

Fig. 5.4 The values of the variables describing the formation along the navigation

106

5 Control of Multi-robot Systems

Fig. 5.5 The paths traveled by the leader and follower quadrotors, considering that the leader tracks a given trajectory

Fig. 5.6 The values of the variables describing the formation along the navigation

5.3 Leader-Follower Paradigm

107

Fig. 5.7 Experimental setup and schematics of the leader and follower robots

from the desired altitude, its controller starts guiding the navigation after the quadrotor takes off and starts hovering about 1 m above the ground, whereas the desired trajectory is a circle at the altitude of 8 m. As for the variables ρ L F , α + L F and β L F , one can see that they converge to the prescribed values of (4 m, 0 rad, 0 rad), respectively, confirming the effectiveness of the proposed approach to design a trajectorytracking controller for the follower robot, considering that the position and velocity of the leader robot defines the trajectory it should track. Finally, it is worthy mentioning that such an approach is also valid for an heterogeneous formation, such as one composed by a differential-drive robot and a quadrotor. The only difference, in comparison with the example just discussed, which considers two quadrotors, is in the z coordinate of the ground vehicle, which should be set to zero. The relative position between the leader and the follower can be measured with different sensors, as fixed and mobile cameras, GPS, Indoor GPS, motion capture systems, such as in [7, 23], where the OptiTrack motion capture system was used. In [12], it is used a visual camera mounted on the follower robot and looking at a visual pattern mounted on the leader robot, which gets the information about the relative follower-leader position and allows estimating the leader´s velocity, as shown in Fig. 5.7. This information is used by the proposed controller to guide the follower robot to track the leader one. Notice that the robot with the visual pattern performs like a leader for the one which is tracking the visual pattern, thus configuring a leader-follower formation. In the experiment, the desired tracking should verify a relative distance between the ground robots of d = 0.5 m and the angle φ = 0, variables which are shown in Fig. 5.7. The variables of distance d and the angles φ and θ are obtained from the processing of the images captured by the camera at a sampling period of T = 0.1 s, see details in [12]. From Fig. 5.7, it can be written the following kinematic relations, d˙ = v cos φ − v L cos θ

(5.2a)

108

5 Control of Multi-robot Systems

Fig. 5.8 Evolution of the formation variables

φ˙ = ω + v

sin φ sin θ + vL d d

(5.2b)

Now, the following inverse kinematics control laws are considered for commanding the follower robot, 1 ˜ v= (v L cos θ − kd d) (5.3a) cos φ ω = −v

sin θ sin φ −v + kφ φ d d

(5.3b)

where d˜ = d − dd . The leader velocity vT is estimated from (5.2a) considering the sampling time of T = 0.1 s, as vˆ L =

v cos φ − (dk − dk−1 )/0.1 cos θ

(5.4)

The control gains are set to kd = 200 and kφ = 10. In Fig. 5.8 it is shown the formation tracking errors from an experiment in which the leader follows an arbitrary trajectory, and the follower conforms the leader-follower formation by the visual servoing as described above.

5.4 Virtual Structure Paradigm

109

5.4 Virtual Structure Paradigm In this section three formations of two robots are considered, to design the formation controller with basis on the virtual structure paradigm. The first one is an homogeneous formation composed by two wheeled differential-drive robots, the second one is also an homogeneous formation, now composed by two quadrotors, and the third one is an heterogeneous formation composed by a wheeled differential-drive robot and a quadrotor. Figures 5.9, 5.10 and 5.11 illustrate these three two-robots formations.

5.4.1 Cluster Space Versus Robots Space In all the three previous figures illustrating two-robots formations there are two distinct groups of variables. The first group characterizes the positions of the two

Fig. 5.9 Illustration of a formation of two differential-drive robots, with the variables involved

Fig. 5.10 Illustration of a formation composed by two quadrotors, with the variables involved

110

5 Control of Multi-robot Systems

Fig. 5.11 Illustration of an heterogeneous formation composed by a wheeled differential-drive robot and a quadrotor, with the variables involved. Source [7]

  T T robots, and  is composed of the variables hi = xi yi , for ground vehicles, or hi =  xi yi z i , for the quadrotor. This set of variables is concatenated as a single vector  T x = h1T h2T , referred to as the robot variables, characterizing the so called robots space. Therefore, in a general way, T  x = x1 y1 z 1 x2 y2 z 2 , which can be particularized when one or both vehicles are ground vehicles simply taking the z coordinate as zero. In addition, the other variables present in the figures above mentioned are grouped in the vector T  q = xf yf z f ρf αf βf , which describes the position of the virtual robot characterizing the formation, or the T    position of the formation q P = x f y f z f , the shape q S = ρ f and the orien T tation q O = α f β f of the virtual structure, in this case given by the length and orientation of the line segment linking the two robots, respectively. Such variables are called formation variables, and define what is called the cluster space or formation space. Notice that the number of variables of both the robot and the cluster space should be the same.

5.4.2 Transformations Therefore, two different spaces are characterized when dealing with the virtual structure approach to control a multi robot system, the robots space and the cluster space. The desired movement of the formation, such as trajectory-tracking, positioning or path-following, as well as its desired shape and orientation along the movement, are specified through desired values of the cluster variables, or qdw and the way they

5.4 Virtual Structure Paradigm

111

should vary along time, or q˙ dw , both in the global frame. However, the velocity commands to be sent to each robot of the formation should be in the robots space, and in the body frame of each vehicle. Thus, there should be a transformation from the robots space to the cluster space, to evaluate the current state of the formation from the current robot positions, and from the cluster space to the robots space, to define where each robot of the formation should be to get the desired formation state. Without loosing generality, let us consider those transformations for the case of a two-robots formation involving two quadrotors. To particularize for the other cases, it is enough to take the corresponding z coordinate as being zero. Then, from Fig. 5.10, one gets that ⎡

⎤ x1 xf ⎢ ⎥ y1 ⎢ ⎥ ⎢y f ⎥ ⎢ ⎥ z ⎢ ⎥ 1 ⎢ ⎥

⎢z f ⎥ ⎢ ⎥ = f (x) = ⎢ (x2 − x1 )2 + (y2 − y1 )2 + (z 2 − z 1 )2 ⎥ q=⎢ ⎥ ⎢ρ f ⎥ ⎢ ⎥ ⎢ ⎥ z −z 2 1 ⎢ ⎥ arctan √ ⎣α f ⎦ ⎢ ⎥ (x2 −x1 )2 +(y2 −y1 )2 ⎣ ⎦ 

βf 1 arctan xy22 −y −x1 ⎡

⎤

transforms the robot variables to the cluster variables, whereas ⎡ ⎤ ⎡ ⎤ x1 xf ⎢ y1 ⎥ ⎢ ⎥ yf ⎢ ⎥ ⎢ ⎥ ⎢z1 ⎥ ⎢ ⎥ z f ⎢ ⎢ ⎥ ⎥ x = ⎢ ⎥ = g(q) = ⎢ ⎥ x x + ρ cos α cos β 2 f f f f ⎢ ⎥ ⎢ ⎥ ⎣ y2 ⎦ ⎣ y f + ρ f cos α f sin β f ⎦ z2 z f + ρ f sin α f

(5.5a)

(5.5b)

does the inverse transformation, or transforms the cluster variables into robot variables. An important remark about these transformations is that they can be also developed for a group of three robots constituting a triangular formation and so on. As it was done here for a formation of two quadrotors, the geometry of the formation allows to get these transformations. However, it will not be done here, since it is enough to extend the analysis already performed.

5.4.3 Movement Jacobian In terms of velocity, when one determines how the formation should evolve along ˙ it becomes necessary to calculate the velocities time, which means to determine q, x˙ to be sent to the individual robots of the formation. This is done simply taking the time derivative of the transformation x = g(q), or

112

5 Control of Multi-robot Systems

x˙ =

∂g ˙ q˙ = J−1 (q)q, ∂q

(5.6a)

where J−1 (q) is the Jacobian matrix corresponding to the transformation of the cluster space to the robots space represented by (5.5b), or ⎡ ∂ x1

J−1 (q) =

which results in ⎡ 10 ⎢0 1 ⎢ ⎢0 0 J−1 (q) = ⎢ ⎢1 0 ⎢ ⎣0 1 00

∂x f ⎢ ∂ y1 ⎢ ∂x f ⎢ ∂z1 ⎢ ∂x f ⎢ ∂x ⎢ 2 ⎢ ∂x f ⎢ ∂ y2 ⎣ ∂x f ∂z 2 ∂x f

∂ x1 ∂yf ∂ y1 ∂yf ∂z 1 ∂yf ∂ x2 ∂yf ∂ y2 ∂yf ∂z 2 ∂yf

∂ x1 ∂z f ∂ y1 ∂z f ∂z 1 ∂z f ∂ x2 ∂z f ∂ y2 ∂z f ∂z 2 ∂z f

∂ x1 ∂ρ f ∂ y1 ∂ρ f ∂z 1 ∂ρ f ∂ x2 ∂ρ f ∂ y2 ∂ρ f ∂z 2 ∂ρ f

∂ x1 ∂α f ∂ y1 ∂α f ∂z 1 ∂α f ∂ x2 ∂α f ∂ y2 ∂α f ∂z 2 ∂α f

∂ x1 ∂β f ∂ y1 ∂β f ∂z 1 ∂β f ∂ x2 ∂β f ∂ y2 ∂β f ∂z 2 ∂β f

⎤ ⎥ ⎥ ⎥ ⎥ ⎥, ⎥ ⎥ ⎥ ⎦

(5.6b)

⎤ 0 0 0 0 ⎥ 0 0 0 0 ⎥ ⎥ 1 0 0 0 ⎥ . (5.6c) 0 cos α f cos β f −ρ f sin α f cos β f −ρ f cos α f sin β f ⎥ ⎥ 0 sin α f cos β f −ρ f sin α f sin β f ρ f cos α f cos β f ⎦ 1 sin α f ρ f cos α f 0

Such a Jacobian matrix, for the case of two quadrotors, as (5.6) shows, has a dimension of 6 × 6, which corresponds to 6 robot variables and 6 cluster variables. For the case of a cluster formed by two wheeled robots it would be a 4 × 4 matrix, whereas for the case of a cluster formed by a wheeled robot and a quadrotor the Jacobian matrix would be a 5 × 5 one. Generalizing to triangular clusters, for a cluster composed by three quadrotors the dimension of such Jacobian matrix would be 9 × 9 (see, for instance, [25]), whereas for a triangular cluster composed of three wheeled robots its dimension would be 6 × 6 (see, for instance, [9]).

5.5 Control in the Cluster Space The idea of using the virtual structure approach is that the movement of the whole structure is associated to the movement of a virtual robot. Then, it is necessary to design a controller to guarantee that the virtual robot, and thus the whole formation or cluster, accomplish the desired task, which can be a trajectory-tracking one, a path-following one or a positioning one, as already discussed considering a single robot. As the whole structure is represented by a single virtual robot, it is only necessary to design a suitable controller to guide the virtual robot to accomplish the task. Thus, the designs are identical to those discussed in Chap. 4. However, there is a meaningful difference: as the virtual robot does not present any dynamic requisite,

5.5 Control in the Cluster Space

113

for being virtual and thus without either mass or inertia, just a kinematic controller is necessary, either for trajectory-tracking, positioning or path-following. An important detail, however, is that such a controller should deal with the cluster variables, what means that it implements the control law in the cluster space, thus performing cluster space control. The control objective, in such a case, is to generate a reference on how the cluster variables should vary along time to make the current state of the formation to converge to the desired one. In the sequel, such a reference should be transformed into velocity commands for the individual robots of the cluster, to guarantee that they will move accordingly to achieve the control objective. Considering that the task the formation should accomplish is a trajectory-tracking one, such a controller implements the control law ˜ q˙ r (t) = q˙ d (t) + L tanh (L−1 Kq(t)),

(5.7)

where K ∈ 6 and L ∈ 6 are positive definite diagonal gain and saturation matrices, respectively, q˙ r (t) is the way the cluster should evolve along time, qd (t) is the desired trajectory generated by a planning stage, whose time derivative is q˙ d (t), ˜ and q(t) = qd (t) − q(t) is the error between the current state of the cluster and the desired one. An important remark is that such a control law can be adapted for the tasks of positioning and path-following. For positioning it should be taken qd as constant and equal to the desired position, form and orientation to be regulated, and q˙ d should be set to zero. When considering path-following let us split the formation variables  q = q P q S q O into the position, shape and orientation variables. Then, for pathfollowing, q Pd corresponds to the point on the path closer to the position of the formation, while q Sd , q Od are the desired shape and orientation corresponding to that point on the path. Regarding q˙ d , its component q˙ Pd corresponds to the velocity adopted along the path, tangent to it, as discussed in Chap. 4. With respect to q˙ Sd and q˙ Od they are defined as dq Sd ||q˙ Pd || q˙ Sd = ds and q˙ Od =

dq Od ||q˙ Pd || ds

where s is the variable over the path, as described in Chap. 4. In terms of the stability of the control system when closing the loop with such a controller, the analysis is similar to the one already discussed in Chap. 4, and the conclusion is that the formation variables q(t) asymptotically converge to their desired values qd (t). The above remark is important, because it allows to design controllers to guide the multi-robot system to accomplish any of the control objectives (trajectory-tracking, positioning or path-following) in a unified form, preserving the same controller structure, but appropriately defining the references. This way, it represents an extension for a robot formation of the unified solution that has been presented in Chap. 4 regarding a single robot.

114

5 Control of Multi-robot Systems

5.6 Control and Dynamic Compensation in the Robots Space From the reference values q˙ r so far obtained it is necessary to generate the velocity references to be sent to the robots in the formation. Still regarding a formation com T T T v2d . posed by two quadrotors, it is necessary to obtain the velocities vd = v1d This is done in two steps, as shown in Fig. 5.12. First of all, it is necessary to use the Jacobian matrix of the transformation from the cluster space to the robots space, namely J−1 , of (5.6), which allows to get x˙ r (t) = J−1 q˙ r (t). After, such a reference velocity vector is split in two parts, x˙ 1r (t) and x˙ 2r (t), which are the velocity commands for each robot, still in the global frame. Now, the velocity commands are processed at the level of the robots, so that the control system now involves the control and dynamic compensation in the robots space, as illustrated by the rightmost red rectangle in Fig. 5.12. This involves the projection of the velocity commands into the body frames of each robot, through the matrix of inverse kinematics of each vehicle, which are, in the figure, A−1 1 and , for the first and second drones, respectively. As one can see from the figure, A−1 2 this completes the outer control loop, which consists in a kinematics controller, as already mentioned. Now, completing the inner-outer loop control scheme, a dynamic compensator, the inner loop, as already discussed, is designed for each quadrotor, as represented by the blocks DC in the figure. Such compensators are designed in the same way as discussed in Chap. 4. An important aspect to be stressed here is that the velocity commands dealt with are just those relative to the Cartesian position of each robot, or u vx , u v y and u vz . In other words, the yaw velocity is not included. However, as the quadrotor is an omnidirectional vehicle, this is not a problem. All one should do is to include a zero

Fig. 5.12 The control system for a virtual structure composed by two quadrotors Bebop 2. A trajectory-tracking task is considered

5.6 Control and Dynamic Compensation in the Robots Space

115

Fig. 5.13 Characterization of the triangular cluster composed by three Pioneer 3 robots

value as the fourth entry of the vectors v1d and v2d , before continuing processing the control algorithm. Nonetheless, as the yaw movement is decoupled of the others in the simplified kinematics model, one can design a separate kinematic yaw controller, to generate a yaw velocity command according to a desired yaw value. For instance, in [6, 7] a yaw controller is designed to keep the quadrotor oriented as the ground robot. An important remark is that this control structure at the robot space allows generating the control signals to any kind of robot by just considering their kinematics and, eventually, their dynamic model as well. Therefore, it can be considered a cluster formed by similar or different kind of mobile robots. In the first case one has an homogeneous formation, whereas in the last case the formation is an heterogeneous or hybrid one. To illustrate the use of the virtual structure control approach, four experiments are presented in the sequel. The first and the second ones consider a triangular cluster composed by two Pioneer 3-AT and one Pioneer 3-DX wheeled unicycle robots. In the first case a positioning task is accomplished, whereas the second one considers T  a trajectory-tracking task. The cluster variables, q = x F y F ψ F p F q F β F , in such a case, are the position given by the coordinates of the center of gravity of the triangle, the orientation of the triangle, defined by the angle ψ F formed by the line linking the triangle vertex corresponding to the robot R1 and the center of gravity with the axis y w , and its shape, given by the two triangle sides corresponding to the segments R1 R2 ( p F ) and R1 R3 (q F ) and the angle β F between these two triangle sides. Figure 5.13 shows such variables. For the positioning task, the initial configuration of the cluster is q(0) =  T 0.70 m 1.50 m 1.57 rad 1.58 m 1.58 m 0.64 rad , and the desired one is qd =  T 4.70 m 1.50 m 1.57 rad 1.58 m 1.58 m 1 rad . Therefore, the cluster should move ahead for 4 m to reach the desired position of its center of gravity, reach the desired orientation and increase the distance between the robots R2 and R3 , through increasing the angle β F . Such a positioning task is illustrated in Fig. 5.14, which shows

116

5 Control of Multi-robot Systems

Fig. 5.14 Positioning task performed by a triangular cluster composed by three Pioneer 3 robots

Fig. 5.15 Control system to guide a triangular cluster composed by three Pioneer 3 robots

three sketches of the cluster position/orientation/shape while navigating towards the desired final configuration, illustrated by the pink triangle at the right of the figure. As for the control system adopted, it is illustrated in Fig. 5.15. Notice that it is the same for the cases of positioning and trajectory-tracking. The difference is that for positioning q˙ d = 0. Also notice that the dynamic compensation block is not implemented for the robots, so that the whole control is based just on the kinematics of the robots, as represented by the matrices of inverse kinematics Ai−1 , i = 1, 2, 3. As for the gain and saturation matrices, they are K = 0.12I and L = 0.2I, where I is a 6 × 6 unit matrix. In the second example the same cluster should perform a trajectory-tracking task, starting from the same initial state of the positioning task, except for the cluster orientation, which is now 1.57 rad, instead of 0.64 rad. The trajectory to be tracked is  T characterized by qd = q(0) + 0.08t 0.3 sin(0.1t) 0 0 0 0 . Therefore, the cluster should move ahead with a constant velocity of 80 mm/s, with a sinusoidal oscillation

5.6 Control and Dynamic Compensation in the Robots Space

117

Fig. 5.16 Trajectory-tracking task performed by a triangular cluster composed by three Pioneer 3 robots

in the y direction. As for the gain and saturation matrices, the values adopted were K = 0.25I and L = 0.1I. Figure 5.16 presents three sketches of the formation along its navigation, showing that the task is effectively accomplished. As for the third example of using the virtual structure approach to control a multi-robot system, a hybrid formation composed by the nonholonomic Pioneer 3-DX robot and the Bebop 2 quadrotor, shown in Fig. 5.11, is considered. The formation should track a lemniscate of Bernoulli trajectory, so that the desired posiT  tion of the formation, the same of the ground vehicle, is given by xd yd ψd =   T T 0.85 sin(2ωt) 1.5 cos(ωt) 0 , where ω = 2π/25 rad/s, with ρ f α f β f =  T 1.5 m π/2 rad 0 being the desired shape of the virtual structure. Thus, the desired formation is characterized by the vector qd = [0.85 sin(2ωt)1.5 cos(ωt)01.5 m π/2 rad0]T , whose time derivative is q˙ d = [1.7ω cos(2ωt) − 1.5ω sin(ωt)0000]T . As for the point of interest for control, regarding the Pioneer 3-DX robot, it is characterized by a = 15 cm and α = 0 rad (see Sect. 2.1.4). The control system implemented is quite similar to that in Fig. 5.12, with two differences. The first one is that the virtual robot is the ground one, for which the z coordinate is always zero. The other difference is that for the Pioneer 3-DX just the kinematic controller is adopted, such that just the Bebop 2 has a dynamic compensator to reduce the velocity tracking error. Such a dynamic compensator is designed according to Chap. 4, using the parameters of Table 3.1 for the Bebop 2 quadrotor. Other important aspect is that this is an indoor flight, run in an arena based on a motion capture system to provide the positions of the vehicles. Finally, the gain and saturation matrices K and L of (5.7) are K = diag(0.2, 0.2, 0.2, 3, 1, 1.8) and L = diag(1, 1, 1, 1, 1, 1). The experiment starts with the Bebop 2 landed on the Pioneer 3-DX, from where it takes off and establishes the desired formation along navigation. After a while the quadrotor lands on the ground vehicle again, stays there for a lap in the trajectory, and takes off again, reestablishing the formation once more, and continues navigating keeping the formation, till landing definitively on the Pioneer 3-DX. Figures 5.17,

118

5 Control of Multi-robot Systems

Fig. 5.17 Paths traveled by the Pioneer 3-DX and the Bebop 2 robots in a trajectory-tracking task, navigating as a formation Fig. 5.18 Top view of the paths traveled by the Pioneer 3-DX and the Bebop 2 robots in a trajectory-tracking task, navigating as a formation

5.18, 5.19, 5.20 and 5.21 describe the experiment. They show the paths effectively traveled by the two robots, thus by the formation, in a 3D plot and in two views (a top and a side views), the formation variables (desired and effectively obtained), and the errors in the formation variables. In the first and third of such figures the path the quadrotor effectively followed is represented in blue, and two lemniscate can be perceived. The one in the top corresponds to the time along which the desired formation is kept (ρ f = 1.5 m),

5.6 Control and Dynamic Compensation in the Robots Space

119

Fig. 5.19 Side view of the paths traveled by the Pioneer 3-DX and the Bebop 2 robots in a trajectorytracking task, navigating as a formation

Fig. 5.20 Formation variables for a Pioneer 3-DX and a Bebop 2 robots navigating as a formation, in a trajectory-tracking task

whereas the one in the bottom corresponds to the time along which the quadrotor is landed on the ground vehicle, which corresponds to an altitude of about 75 cm, considering the ground robot itself plus a landing platform installed over it. The black dashed line represents two desired formations, which differs just in the value of ρ f , equal to 75 cm for the landed quadrotor. The lemniscate curve in the ground corresponds to the Pioneer 3-DX robot, which coincides with the virtual robot guiding the virtual structure. Continuing the analysis of the experiment, Fig. 5.20 shows the evolution of the formation variables during the navigation, besides their desired values. As one can see, there are meaningful variations in the variable ρ f , which correspond to the

120

5 Control of Multi-robot Systems

Fig. 5.21 Time evolution of the formation errors for the Pioneer 3-DX and the Bebop 2 robots navigating as a formation, in a trajectory-tracking task

landing and taking off moments. By its turn, Fig. 5.21 shows the evolution of the formation errors during the navigation. As one can see, all the errors converge to zero along time, except for two of them. The first one is the error in ρ f , which has positive and negative spikes corresponding to the moments of landing and taking off, respectively. The second one is the error in the angle β f , which presents quite large variations, due to the fact that the situation in which the quadrotor is exactly over the ground vehicle (α = π/2) is a singularity, since there is no projection of the straight line describing the virtual structure over the plane x w y w , thus letting β f not defined. Finally, the fourth experiment illustrating the use of the virtual structure approach to control a multi-robot system is now discussed. It considers a formation of two Bebop 2 quadrotors tracking a lemniscate of Bernoulli trajectory character T ized by qd = 1.5 cos(ωt) sin(2ωt) 1.25 + 0.3 sin(ωt) 1.5 π π/9 , flying in an indoor environment, with the positions of the two quadrotors obtained through using the OptiTrack motion capture system. For such a trajectory one has q˙ d =  T −1.5ω sin(ωt) 2ω cos(2ωt) 0.3ω cos(ωt) 0 0 0 , with ω = 2π rad/s. The quadro30  T tors start moving from x1 = 0.9m −0.28 m 0.9 m and x2 =  T −0.6 m −0.18 m 0.9 m . The gain matrix for the formation controller is K = diag(0.768, 0.768, 1.6, 3.2, 1.92, 1.92), whereas its saturation matrix is L = diag(0.96, 0.96, 2, 4, 2.4, 2.4). As for the gain matrices for the dynamic compensators associated to each quadrotor, they are diag(2, 2, 1.8, 5). Finally, the sampling time adopted is 100 ms.

5.6 Control and Dynamic Compensation in the Robots Space

121

Fig. 5.22 Paths traveled by the formation and the two quadrotors (the path the formation travels is the same of quadrotor 1)

Fig. 5.23 Errors in the formation variables

Figures 5.22 and 5.23 illustrate the result of such an experiment. The first one shows two views, a 3D one and a top one, of the path traveled by the two quadrotors, besides the path the formation (and also the first quadrotor) should travel. As one can see, after an initial transient the formation reaches the trajectory and tracks it. This can be confirmed looking at Fig. 5.23, which shows the errors in the formation variables, which becomes quite close to zero after the initial transient.

122

5 Control of Multi-robot Systems

5.7 Scalability One quite important property of the control structures associated to the leaderfollower and the virtual structure control approaches is their scalability to accommodate more robots in the cluster. The main question, in such a case, is: how to change the control structure to add a new robot in the cluster? For the case of the leader-follower approach, one can assembly the cluster as a chain of n robots as n − 1 leader-follower pairs. This can be done considering a single leader and n − 1 followers, or with the first robot in the chain being the leader for the second one, which is its follower and simultaneously is the leader for the third robot, and so on. In both cases, the leader moves according to an independent control system, whereas the followers track the trajectory corresponding to the leader, as discussed in Sect. 5.3. Regarding the virtual structure approach, the problem of including a new robot in the cluster becomes a problem of redefining the dimension and the entries of the Jacobian matrix that transforms the reference q˙ r for the movement of the cluster to the velocity references x˙ r for the robots. Each new robot included in the cluster means, in this case, a completely new and different Jacobian matrix. A solution to avoid this problem is to split the whole cluster in smaller parts, thus creating virtual substructures, which can be triangles or line segments, for instance, such as in [9] and in [14], respectively. In the case of using triangles as the virtual substructures, each new robot added to the cluster generates a new triangle, considering two other robots yet included in other triangles. This way, a cluster composed by n robots, thus generating a polygon of n vertices, can be split in n − 2 triangles. For each triangle the same control structure can be adopted, just considering the data relative to the robots of the respective triangle to generate the Jacobian matrix, which preserves its structure and dimension, and the task characterization for each substructure. This way, the whole control structure becomes a modular one, with n − 2 identical controllers, each one generating the references for the three robots of the corresponding triangular virtual substructure. In this case, one should notice, two robots of each triangle are also in another triangle, such that one robot could receive reference velocities coming from one, two or even three controllers. Figure 5.24 illustrates how to build the triangular virtual substructures, regarding a cluster of 6 robots. From the figure one can see that the triangles are built considering the sequence of the robots in the queue, or Ri , i = 1, . . . , 6. The three first robots assembly the first triangle, the second and third ones, plus the next, the fourth, assembly the second triangle and so on. Therefore, for this example, four triangles are assembled. As one can perceive, except by the two first and the two last robots, all the robots receive control references generated by three controllers, one associated to each triangle to which they belong (in this example the robots numbered as 3 and 4). In such cases it is possible to apply just one of such control references to the robot, such as in [9, 10] (although in this last case the way to build the triangles is different), or even to consider the generation of a new control signal coming from the fusion of the control

5.7 Scalability

123

Fig. 5.24 Illustration of how to build triangular virtual substructures when dealing with a cluster using the virtual structure approach

Fig. 5.25 Illustrating the split of the multi robot system in smaller virtual structures characterized as line segments

signals generated by the different controllers [15]. Notice that only the first and the last robots receive just one control reference, whereas the second and the one before the last receive, both, two control references. Finally, if we want to avoid generating commands to each robot from multiple controllers, it is also possible to consider the new triangle as having only three degrees of freedom in 3D (because constrained in two vertices) and design a controller that generates commands only to the new added robot for controlling only the three degrees of freedom of the new constrained triangle. When regarding a line segment as the virtual substructures, the possibilities are also two. The first one is to centralize all the line segments in one of the robots, thus building n − 1 virtual substructures, as it is done in [14], or to build line segments where each robot belongs just to one substructure, such as illustrated in Fig. 5.25. In this last case, if the number of robots in the cluster is odd one robot needs to build a line segment with one robot already belonging to a line segment. Therefore, the result is n/2 virtual substructures, for n even, and (n − 1)/2 + 1 virtual substructures, for n odd. Notice that in this case, for n even all the robots receive control references from just one controller, whereas for n odd one robot would receive control references of two controllers. In this case, a solution is, once more, to consider just the control reference provided by the last controller. Notice that in the case of n odd other possibility is to assembly n − 3 line segments and one triangle.

124

5 Control of Multi-robot Systems

Fig. 5.26 Illustrating the scalability idea of splitting the whole formation in triangular partial formations

The idea of splitting the virtual structure associated to the cluster into smaller virtual structures allows to use the same controller designed for a virtual line segment and for a triangular virtual substructure as many times as necessary, without needing to change the system core, but just replicating the corresponding controllers, using the corresponding data (see [9]). To illustrate such ideas, two simulations are now presented, both considering a multi-robot system described by a set of triangles, each one independently controlled, but with the task assigned according to what should be performed by the whole formation. The first one is a positioning task performed by a formation composed by four wheeled differential-drive robots. Thus, two triangles should be controlled, which are indicated in Fig. 5.26, which illustrates the task accomplishment. In the figure the pink rectangle represents the desired position, orientation and shape of the formation, as well as the position, orientation and shape of each triangle. The different sketches of the whole formation along the simulation time (0 s, 20 s, 35 s and 50 s) show how each triangle changes its position, orientation and shape, till getting its final position, orientation and shape. As a consequence, the whole formation achieves its desired position, orientation and shape. The second simulation corresponds to a trajectory-tracking performed by a multirobot system composed by eight wheeled differential-drive robots, thus characterizing six triangles, according to one of the scalability strategies discussed. The desired trajectory for the whole formation allows defining the desired trajectory for each triangle, as well as the physics involved in the whole formation allows defining the desired formation variables for each triangle. Figure 5.27 illustrates the task accomplishment. The pink rectangle characterizes the desired whole formation, which moves following the desired trajectory. The figure shows sketches of the whole formation and the desired one in various instants during the simulation, which are 0 s, 40 s, 80 s and 120 s. However, yet in the second sketch of the whole formation (simulation time of 40 s) one can see that the whole cluster has already completed the necessary reshaping, besides reaching the desired trajectory, and then continues tracking it, as the figure characterizes.

5.8 Multiple Objectives

125

5.8 Multiple Objectives Controlling a cluster through the virtual structure paradigm involves a set of formation variables that describe the current state of the formation in every sampling instant. In the case of two tridimensional vehicles, such as in Fig. 5.10, the virtual structure considered is the straight line linking the two vehicles, and the set of formation T  variables is the vector q = x f y f z f ρ f α f β f , as described in Sect. 5.4. From this vector one can get two sub-vectors, for instance, one associated to the position of T  the cluster, which would be q p = x f y f z f , and the other associated to the shape T  of the cluster, which would be qs = ρ f α f β f . Working with these two subsets of variables one can control the formation with two different control objectives, for instance. They could be to control the position of the virtual structure (and thus the position of the cluster) and to control the shape of the virtual structure. Notice that to move the virtual structure to a desired position keeping the desired shape involves to deal with these two subsets of formation variables. If only the first subset is used one manages to move the formation, but its shape would not be controlled. Other subtasks can also be added to achieve additional control objectives along the cluster navigation. A commonly adopted additional subtask is obstacle deviation, such as in [27]. Therefore, considering the possibility of various conflicting subtasks it is necessary to establish a priority list, giving the highest priority to the subtask of obstacle avoidance, and following to the other subtasks, according to the control objectives. For instance, in [20, 21] a drone is supposed to land on a ground vehicle, mimicking the last step of a last-mile delivery problem, which demands that the drone be quite well positioned with respect to the ground vehicle before landing on it. In this case, considering to move the cluster and to keep its shape as two behaviors, the highest priority is given to keep the formation shape, moving the virtual robot (in those works coinciding with the ground vehicle) just after the shape is reestablished, after the previous movement.

Fig. 5.27 Illustrating the scalability idea of splitting the whole formation in triangular partial formations

126

5 Control of Multi-robot Systems

Thus, the idea is to assign a priority level to each subtask considered, which are indeed independent behaviors, taking into account the whole control objective. This corresponds to generate velocity commands associated to each subtask, and then to compose the final velocity command considering the priorities of each subtask. For instance, in [27] two subtasks are considered, which are to guide a quadrotor to track a trajectory and to avoid obstacles. The highest priority was given to the behavior avoid obstacles, for safety reasons, and only after the quadrotor left the obstacles behind, the trajectory-tracking was resumed. Then, the strategy to solve the conflict between the independent behaviors is to activate a behavior of lower priority just when the higher priority behaviors are inactive. In [27] this means to track the desired trajectory after leaving the obstacles behind. Therefore, in the presence of obstacles the quadrotor goes out of the desired trajectory, to avoid them. A way to activate the individual behaviors according to their priority levels is to use the approach called null-space behavioral control [2, 3]. Such an approach has been adopted in several applications since the beginning of the 21st century, such as to control a single aerial robot [27], a group of them [5, 25], a group of marine vessels [19], a quadrotor carrying a manipulator [8], and a heterogeneous formation involving a quadrotor and a ground vehicle [21], showing its flexibility and generality. It is, actually, an analytical way, with stability proof, to implement the well known subsumption architecture [11]. Consider a case in which the formation control is split in two different subtasks. This is the case, for instance, for the line formation of Fig. 5.10, considering to control the position of the bottom extremity of the line as one of the subtasks, and to control the shape of the cluster as the second one. Therefore, writing the relationship between formation and robot variables, similar to what was made in (5.5a), q = f(x), taking T  time derivative, q˙ = f(˙x) and splitting the formation vector q = q p qs , with q p and qs being the position and shape formation variables subsets, one can write q˙ p = J p x˙

(5.8a)

q˙ s = Js x˙

(5.8b)

and

for each subset. What should be done now is to design individual controllers for each subtask, which would generate vectors corresponding to the desired time variation for the cluster variables, to get the desired shape and position, thus still in the cluster space. This means to generate q˙ r p and q˙ r s in the form of (5.7) but referred to the position and shape formation variables respectively. Then, it is necessary to map these command vectors to the robots space, using the inverse Jacobian matrix corresponding to each subtask, thus obtaining velocity command vectors for the robots, although still in the global reference frame. Such velocity commands would be x˙ r p = J†p q˙ r p and x˙ r s = Js† q˙ r s , which are associated to the inverse Jacobian matrices J†p and Js† , respectively, which map the corresponding set of cluster variables into the robot variables.

5.8 Multiple Objectives

127

For this example, considering the subtask of controlling the position of the bottom extremity of the virtual structure characterizing the cluster as having the highest priority, the final velocity command vector to be sent to the robots would be   x˙ r = x˙ r p + I − J†p J p x˙ r s ,

(5.9)

where J†p is the right pseudo inverse of J p . The effect of pre-multiplying x˙ r s by I − J†p J p corresponds to project the velocity command x˙ r s into the null space of the matrix J p , or to project the velocity command corresponding to a certain task into the null space of the Jacobian matrix of the task with priority immediately higher,   which can be verified pre-multiplying (5.9) by J p and checking that J p I − J†p J p x˙ r s = 0. This is why the technique is called null space-based behavioral control approach. Two important remarks should be done here. The first one is that if the highest priority is assigned to move the reference point of the formation one has what is called a flexible formation or flexible cluster, since the most important is to move the cluster accordingly, not paying much attention to its shape. In such a case the velocity command to be sent to the robot is defined as in (5.9). In opposition, if the highest priority is assigned to keep the shape of the cluster, then one will get a rigid formation, meaning that more attention is paid to guarantee the shape of the cluster, not paying so much attention to its movement. In this case, the matrix J†p should be changed to Js† in (5.9), thus resulting in the velocity command   x˙ r = x˙ r s + I − Js† Js x˙ r p .

(5.10)

The designations rigid or flexible formation becomes clear with conflicting tasks, for example when the formation avoids an obstacle. In this case it is not possible to keep tracking of both the position and the shape of the cluster while avoiding the obstacle. If priority is given to position, the cluster will distort but still can keep tracking its reference position, thus having a flexible formation. Just the opposite occurs when priority is given to the shape. The cluster will not be distorted but the entire cluster will be deviated from its route to avoid the obstacle, thus obtaining a rigid formation. For the case of three subtasks - for example to track the position and shape of the formation and to avoid an obstacle -, one would get   

x˙ = x˙ 1 + I − J1† J1 x˙ 2 + I − J2† J2 x˙ 3 .

(5.11)

Generalizing for N subtasks one would get   



x˙ = x˙ 1 + I − J1† J1 x˙ 2 + · · · x˙ N −1 + I − J†N −1 J N −1 x˙ N , for the priority order decreasing from 1 → N . Such an architecture is illustrated in Fig. 5.28, for a case of four subtasks (or behaviors).

128

5 Control of Multi-robot Systems

Fig. 5.28 The scheme implementing the null space-based behavioral control for four subtasks/behaviors

An important aspect to be considered here is the stability of the closed loop control scheme when using the null space-based behavioral control approach. To analyze its stability, let us consider the basic structure of two subtasks, for which the formation variables are split into two groups, each one associated to one of the two subtasks. T  Therefore, q = q1T q2T , and the control laws corresponding to each subtasks are

and

q˙ r 1 = q˙ d1 + L1 tanh(L1−1 K1 q˜ 1 )

(5.12a)

q˙ r 2 = q˙ d2 + L2 tanh(L2−1 K2 q˜ 2 ),

(5.12b)

similarly to (5.7). On the other hand, using the null space-based behavioral control approach the velocities to be used as reference for the agents in the formation are, for two subtasks, with highest priority given to subtask 1,   x˙ r =J1† q˙ d1 + L1 tanh(L1−1 K1 q˜ 1 ) +   (I − J1† J1 )J2† q˙ d2 + L2 tanh(L2−1 K2 q˜ 2 ) ,

(5.13)

such as in (5.9). Now, assuming perfect velocity tracking, or x˙ = x˙ r , and substituting the control law of (5.13) in (5.8a), one gets the closed-loop equation q˙ 1 = q˙ d1 + L1 tanh(L1−1 K1 q˜ 1 ),

(5.14)

taking into account that J1† is the right pseudo inverse of J1 , and, therefore, J1 (I − J1† J1 )J2† = J1 − J1 = 0. As already shown, this implies that q1 (t) → 0 as t → ∞. Similarly, taking now the relationship in (5.8b), and substituting again x˙ = x˙ r , one obtains   q˙ 2 = J2 {J1† q˙ d1 + L1 tanh(L1−1 K1 q˜ 1 ) (5.15)   + (I − J1† J1 )J2† q˙ d2 + L2 tanh(L2−1 K2 q˜ 2 ) }.

5.8 Multiple Objectives

129

Now we recall that J2 J2† = I, and assume that both subtasks are non conflicting, which means that they are orthogonal, or the range space of J1† is inside the null space of J2 . As (I − J2† J2 ) is the projection operator on the null space of J2 , the above condition implies that (I − J2† J2 )J1† x˙ = J1† x˙ , and, therefore, J2 J1† = 0. Then, from (5.15) one gets, for non-conflicting subtasks, q˙ 2 = q˙ d2 + L2 tanh(L2−1 K2 q˜ 2 ),

(5.16)

which implies that q˜ 2 (t) → 0 as t → ∞. This allows concluding that the closed-loop  T  T control system with equilibrium at q˜ q˙˜ = 0 0 is asymptotically stable. For a deeper stability analysis, regarding more than two subtasks, see [4]. There one can see that for three subtasks it is enough that two successive subtasks be orthogonal. If the null space of the Jacobian matrix of the main task has not enough dimension to keep the secondary tasks, it will not be possible to execute them exactly, because an equation similar to (5.16) is not possible to obtain. To illustrate how to implement a null space based behavioral control system, the steps to be followed to design such a control system, considering a cluster formed by two quadrotors flying in the near hover condition, are elaborated in the sequel. The illustrative design considers two subtasks, namely to move the cluster to a desired fixed position and to keep the formation shape along navigation. Therefore, two possibilities arise, the first one corresponding to giving the higher priority to the subtask of moving the cluster, and the second one corresponding to giving the highest priority to the subtask of keeping the formation shape. To do this the desired vector  T T T qds , where qdp = of formation variables is split in two parts, such that qd = qdp  T T  x f d y f d z f d and qds = ρ f d α f d β f d are the cluster variables associated to the position of the cluster and its shape, respectively. As the task to be accomplished is a positioning one, q˙ dp = 0 and q˙ ds = 0. The controllers to deal with the two subtasks, at the level of the cluster space, are characterized by q˙ r p = q˙ dp + K1 p tanh (K2 p q˜ p ) = K1 p tanh (K2 p q˜ p )

(5.17a)

q˙ r s = q˙ ds + K1s tanh (K2s q˜ s ) = K1s tanh (K2s q˜ s ),

(5.17b)

and

where the subscripts p and s stand for position and shape. Notice that such controllers are quite similar to the one in (5.7), but considering just part of the cluster variables each one. Proceeding, it is now necessary to convert the necessary changes in the cluster variables into velocities to be sent as reference for the two quadrotors, namely to convert q˙ r p into x˙ r p and q˙ r s into x˙ r s , using the corresponding Jacobian matrices. Such Jacobian matrices are obtained as the right pseudoinverses of the matrices obtained by differentiating (5.10) and partitioning the resulting matrix in two parts, considering the cluster variables qdp and qds . Proceeding this way one gets

130

5 Control of Multi-robot Systems

⎡ ∂x f Jp =

∂x ⎢ ∂ y 1f ⎣ ∂ x1 ∂z f ∂ x1

∂x f ∂x f ∂x f ∂ y1 ∂z 1 ∂ x2 ∂yf ∂yf ∂yf ∂ y1 ∂z 1 ∂ x2 ∂z f ∂z f ∂z f ∂ y1 ∂z 1 ∂ x2

∂x f ∂x f ∂ y2 ∂z 2 ∂yf ∂yf ∂ y2 ∂z 2 ∂z f ∂z f ∂ y2 ∂z 2

⎤

⎡

⎤ 100000 ⎥ ⎣ ⎦ = 0 1 0 0 0 0⎦ , 001000

(5.18a)

relative to the position variables q p , and ⎡ ∂ρ f Js =

∂ρ f ∂ρ f ∂ρ f ∂ρ f ∂ρ f ∂ x1 ∂ y1 ∂z 1 ∂ x2 ∂ y2 ∂z 2 ⎢ ∂α f ∂α f ∂α f ∂α f ∂α f ∂α f ⎣ ∂ x1 ∂ y1 ∂z1 ∂ x2 ∂ y2 ∂z2 ∂β f ∂β f ∂β f ∂β f ∂β f ∂β f ∂ x1 ∂ y1 ∂z 1 ∂ x2 ∂ y2 ∂z 2

⎤ ⎥ ⎦

⎡ ⎤ −cα f cβ f −cβ f sα f −sβ f cα f cβ f cβ f sα f sβ f −cα f −sα f cα f ⎢ sα f 0 0 ⎥ = ⎣ ρ f cβ f ⎦, ρ f cβ f ρ f cβ f ρ f cβ f cα f sβ f ρf

−cβ f ρf

sα f sβ f ρf

−cα f sβ f ρf

−sα f sβ f ρf

(5.18b)

cβ f ρf

where c stands for cos and s stands for sin, relative to the cluster variables qs . From such matrices one can get (5.19a) x˙ r p = J†p q˙ r p , and x˙ r s = Js† q˙ r s ,

(5.19b)

where the superscript † means the right pseudo inverse of the matrix, and then

or

  x˙ r = x˙ r p + I − J†p J p x˙ r s

(5.19c)

  x˙ r = x˙ r s + I − Js† Js x˙ r p ,

(5.19d)

for the cases of giving the highest priority to move the cluster to a desired position and keeping its shape, respectively. Now, it is necessary to convert such velocities from the global reference frame (˙xr ) to the reference frame of the robots, what is done through multiplying x˙ r by the matrix of inverse kinematics of the cluster, a block-diagonal matrix whose blocks are the matrices of inverse kinematic of each quadrotor, disregarding their orientations. This means that such a matrix of inverse kinematics is   −1 A1 0 , K−1 = 0 A−1 2 ⎡

with A−1 1

⎤ cos ψ1 sin ψ1 0 = ⎣− sin ψ1 cos ψ1 0⎦ 0 0 1

5.8 Multiple Objectives

131

⎡

and A−1 2

⎤ cos ψ2 sin ψ2 0 = ⎣− sin ψ2 cos ψ2 0⎦ , 0 0 1

where ψ1 and ψ2 are the orientations of the two quadrotors. This way, one gets the reference velocities for the two quadrotors, in their own reference frames, as T  T  vr = vr 1 vr 2 = u vx1 u vy1 u vz1 u vx2 u vy2 u vz2 . Before sending such values to the quadrotors, however, it is necessary to insert the yaw velocity command for each quadrotor. Nevertheless, as the quadrotors are omnidirectional vehicles, their orientation may not be relevant for the task, so that one can, in this case, select a null yaw velocity command, so that T  vr 1 = u vx1 u vy1 u vz1 u vψ1 = 0 and

T  vr 2 = u vx2 u vy2 u vz2 u vψ2 = 0 .

An important remark is that the control system whose design has been illustrated for the two quadrotors performing a positioning task can be adapted to a trajectorytracking task simply considering the values of q˙ dp and q˙ ds as being the time derivatives of the desired cluster variables. Moreover, it can also be adapted for the case of a path-following task, simply considering q˙ dp as the desired velocity along the path, tangent to it at the point closer to the cluster position, as discussed in Sect. 4.7. Also, ds (s) ||q˙ dp ||, where qds is the desired shape of the q˙ ds should be calculated as q˙ ds = ∂q∂s formation in the point of the path closer to its position. Notice that it is also possible to adapt such a control system to a heterogeneous cluster/formation involving a wheeled robot and a quadrotor, simply making z 1 = 0, since the wheeled robot, in this case considered as the reference for the cluster, navigates at zero altitude. To illustrate the performance obtained using the null space-based behavioral approach to control a cluster, in the sequence an example is considered, corresponding to a homogeneous formation involving two omnidirectional robots, whose configuration is as shown in Fig. 2.3. The characterization of such a formation is given in Fig. 5.29. Notice that the orientations of the two robots will not be considered, as they are omnidirectional vehicles. T  The cluster variables, in this case, are q = x f y f ρ f α f , whereas the robot  T variables are x = x1 y1 x2 y2 , with the cluster variables split in the vectors q p = T T   x f y f and qs = ρ f α f , corresponding to the position and shape variables, respectively. A positioning task is simulated, using such a formation, for which two controllers will be considered, both developed according to the null space-based behavioral control approach. The first one gives the highest priority to the subtask of moving the cluster to the desired position, and the second one gives the highest

132

5 Control of Multi-robot Systems

Fig. 5.29 Characterization of a homogeneous formation involving two omnidirectional robots characterized as in Fig. 2.3

priority to the subtask of keeping the cluster shape. For the simulations the gain and saturation matrices considered are K1 p = diag(5, 3), K2 p = diag(1, 1), K1s = diag(3, 5), and K2s = diag(1, 1). As for the desired position for the cluster, it is (6 m, 8 m), whereas the desired shape is characterized by ρ f = 2 m and α f = π/2 rad, with the robot numbered as 1, the reference for the virtual structure, starting from the position (0 m, 2 m), and the robot numbered as 2 starting from the position (4 m, 2 m), both with orientation 0 rad. As for the matrices J p and Js , they are obtained from the matrices in (5.18a) and (5.18b), suppressing the third row and third column plus the sixth row and sixth column, besides considering β f = 0 rad, so that both matrices become 2 × 4 matrices. Therefore,   1000 Jp = 0100 and

  − cos α f − sin α f cos α f sin α f Js = sin α f − cos α f − sin α f cos α f . ρf ρf ρf ρf

Figures 5.30 and 5.31 illustrate the results of such an example. From Fig. 5.30 one can perceive that when the highest priority is given to move the cluster to the desired position, the cluster variables x f and y f ,, that is the position variables, reach the desired values much before ρ f and α f , the shape variables. This means that the robot numbered as 1 gets its position much before the robot numbered as 2. On the other hand, if the highest priority is given to keep the shape of the cluster one can perceive that the opposite happens, or the first variables to

5.8 Multiple Objectives

133

Fig. 5.30 Evolution of the cluster/formation variables along navigation, for the cases of highest priority for position and shape of the cluster/formation

134

5 Control of Multi-robot Systems

Fig. 5.31 Paths traveled by the two robots and the initial and final cluster/formation configuration, for the cases of highest priority for position and shape of the cluster/formation

5.8 Multiple Objectives

135

converge to the desired values are the shape variables. As a consequence of these different possibilities, one can see in Fig. 5.30 that different paths are traveled by the two robots according to the priorities assigned to the subtasks/behaviors. To continue illustrating the use of the null space-based behavioral control approach, a simulation involving the same two omnidirectional robots is now considered, but an obstacle is inserted in the route of the cluster towards the desired position (still considering a positioning task). The objective is to illustrate two different behaviors when the cluster formed by the two robots deviate from an obstacle. To do that three behaviors (subtasks) are considered, namely to avoid the obstacle, to move the cluster to a desired position, and to reach a desired formation shape, which will be embedded in the null space-based behavioral control approach, such as in (5.11). Two different strategies are possible, when deviating from the obstacle, which corresponds to a flexible formation or a rigid formation. The rigid formation is characterized by the preservation of the formation shape even when deviating from the obstacle, whereas the flexible formation is characterized by the change of the formation shape when avoiding an obstacle. For the rigid formation, therefore, the highest priority is assigned to the subtask of reaching the formation shape, followed by the subtask of deviating from the obstacle and the subtask of moving the formation to the desired position. As for the priority sequence associated to the flexible formation, it is given by to deviate from the obstacle, with the highest priority, reach the formation shape and move the formation to the desired position. The obstacle considered is a static one, whose profile is a cylinder centered in the coordinates (xobs , yobs ), whose presence in the navigation environment is characterized by the potential function V (t) = e

    x(t)−xobs n y(t)−yobs n − − a b

,

(5.20)

where n is a positive even integer and a and b are parameters that allow tuning the height and spread of the curves. Notice that this potential function decreases for positions (x(t), y(t)) of the robot far from the obstacle positioned at (xobs , yobs ). The idea is to develop a controller able to guide the robot to avoid obstacles, or to move the robot in the sense of increasing the distance robot-obstacle, which means to navigate through points of low potential. Notice also that if more than one obstacle is considered, the whole  potential will be the sum of the potentials associated to each obstacle, or V (t) = Vi (t), i = 1, · · · , n, when n obstacles are present. For such a potential function one gets that 

n   y(t)−yobs n − b

(x(t) − xobs )n−1 x(t) ˙ an n  n  x(t)−xobs y(t)−yobs (y(t) − yobs )n−1 − − a b +e n y˙ (t), bn

− V˙ = e

or

x(t)−xobs a

n

  (y(t) − yobs )n−1 (x(t) − xobs )n−1 ˙ x(t) ˙ + y˙ (t) , V = nV (t) an bn

(5.21a)

(5.21b)

136

or

5 Control of Multi-robot Systems

V˙ =



∂V ∂V ∂x ∂y



x˙ y˙

T

= Jo x˙ ,

(5.21c)

where Jo is the Jacobian matrix associated to the potential variation as a function of the position of the robot. Such a result says that the robot should move towards points of zero potential (actually potentials below a small threshold ). To avoid the neighborhood of obstacles, characterized by high potential values, the control law

 (5.22) x˙ = Jo† V˙d + kobs V˜ is adopted, for which kobs is a positive nonzero gain, Vd is the desired potential value for the robot, in this case , whose time derivative is V˙d = 0, and V˜ = Vd − V, where V is the potential value for the current robot position. Notice that such a control law is quite similar to the kinematic controller discussed in Sect. 4.4, so the stability analysis is quite similar. Indeed, when applying such a control law to (5.21c) the resulting closed loop equation is

 V˙ = Jo x˙ = Jo Jo† V˙d + kobs V˜ = V˙d + kobs V˜ , from where

and, finally

V˙d − V˙ + kobs V˜ = 0, V˙˜ + kobs V˜ = 0,

which allows to conclude that the robot moves causing V˜ → 0 when t → ∞. To illustrate the cases of flexible and rigid formation behaviors, the same case illustrated in Fig. 5.29 is considered, now including an obstacle in the route of the formation, first considering a positioning task, and then considering a trajectorytracking task. For the positioning task the two robots are initially placed in the positions x10 =  T  T 1 0 and x20 = 2 −1 , with their orientation being constant and equal to zero all the time. The desired final position of the formation, given by q p = x1 , is q pd =  T  T 3 6 , with the time derivative q˙ d = 0 0 , as the task to be accomplished is a positioning one. As for the desired values of the shape variables, they are qsd =  T  T 2 0 , with q˙ sd = 0 0 . As for the control gains, they are kobs = 0.2, K1 p = K1s = diag(1, 1), K2 p = diag(5, 3), and K2s = diag(3, 5). The static obstacle is positioned at (xobs , yobs ) = (2.8, 3), and the parameters of the potential function are a = b = 0.5 and n = 2, with the value Vd = 0.2. Finally, the sampling time considered is 100 ms. Figures 5.32 and 5.33 characterize the two behaviors. The first one shows the paths the two robots follow along the navigation, whereas the second one shows the time variation of the formation variables. As one can see from the figures, in the case of

5.8 Multiple Objectives

137

Fig. 5.32 The paths traveled by the two omnidirectional robots, in the presence of an obstacle, in a positioning task, characterizing a rigid and a flexible formation

the rigid formation the formation shape is kept even during the obstacle deviation, whereas in the case of the flexible formation the formation shape is relaxed during the obstacle deviation action. To conclude this section the same formation is considered, now in a trajectorytracking task. The trajectory to be tracked is a circular one, characterized by q pd =

138

5 Control of Multi-robot Systems

Fig. 5.33 The time variation of the formation variables for the case of Fig. 5.32

5.8 Multiple Objectives

139

Fig. 5.34 The paths traveled by the two omnidirectional robots, in the presence of an obstacle, in a trajectory-tracking task, characterizing a rigid and a flexible formation

140

5 Control of Multi-robot Systems

Fig. 5.35 The time variation of the formation variables for the case of Fig. 5.34

5.9 Concluding Remarks

141

 T T  10π , with q˙ pd = −10π . As for the gains, just 5 cos 2πt 5 sin 2πt sin 2πt cos 2πt 100 100 100 100 100 100 kobs has been changed, becoming kobs = 0.5, with the obstacle now in the coordinates (xobs , yobs ) = (4, 2). The parameters associated to the potential function are the same as in the case of positioning, but the desired potential value was changed to Vd = 0.1.  T  T The initial positions of the two robots are now x10 = 5 0 and x20 = 7 0 , with their orientations starting at zero and staying in such values all the time. The desired values for the shape formation variables are also the same as in the positioning case, as well as the sampling time, which is once more 100 ms. The simulation time varies from zero to 250 ms, which corresponds to two and a half laps over the trajectory. Figures 5.34 and 5.35 show how the formation performs when accomplishing the trajectory-tracking task. As one can see, the behavior is quite similar to the ones in the case of positioning.

5.9 Concluding Remarks In this chapter two approaches to control a multi robot system, also known as a robot formation or robot cluster, namely the leader-follower and the virtual structure approaches, are dealt with. Each one was discussed in details, and examples showing their implementation to control a group of two or more robots were also discussed, which validate these approaches. Examples considering groups of two or more robots of the same kinematics and of different kinematics were dealt with. An important aspect of both approaches is their scalability for more than two or three robots, which was also explored in the chapter. In the case of the leader-follower approach, the scalability is guaranteed by the possibility of building a leader-follower chain, in which the first follower robot also performs as a leader for the third one, and so on, allowing to control a group of theoretically any number of robots. However, this is not the only way. Other possibility is that the same agent performs as the leader for all the other agents in the group. When regarding the virtual structure approach, it is necessary to deal with the Jacobian matrix which transforms the velocity commands from the cluster space to the robots space, whose dimension is directly associated to the number of robots in the cluster. Therefore, the addition of a new robot in the cluster would demand to change the dimension of such Jacobian matrix, and thus to change its entries. However, as discussed in the chapter, the scalability can be guaranteed by splitting the whole cluster in smaller parts, what corresponds to group the robots of the whole cluster in smaller clusters, thus generating various virtual structures. This allows preserving the Jacobian matrix structure of each smaller virtual structure, and simply add a new small virtual structure for each new robot added to the cluster. As commented in the text, this can be done considering subgroups of two or three robots. In the case of subgroups of three robots, the total number of robots in the cluster, n, generates n − 2 subgroups of three robots, each one containing two robots yet included in a

142

5 Control of Multi-robot Systems

previous subgroup plus one new robot. When splitting the whole cluster in subgroups of two robots, for n even one gets n/2 subgroups, each one corresponding to a virtual line segment. For n odd it would be created (n − 1)/2 + 1 subgroups, with the one before the last robot included in two subgroups. This scalability property allows designing a modular control structure, in which each module is the control system corresponding to use a triangle or a line segment as the virtual structure. In addition, the chapter also discusses how to implement a hierarchical control structure, regarding different control objectives, each one with different priority, for the virtual structure approach. For instance, one could have obstacle deviation as part of the navigation task, thus characterizing two tasks, namely to avoid obstacles and to navigate the formation accordingly. Or even three subtasks, such as to avoid obstacles, to move the formation accordingly and to keep the shape of the formation (remember that the cluster variables involve variables related to its position and its shape). In this case, the chapter shows how to implement a control system for the cluster considering the priorities given to each subtask, illustrating the flexibility of using the virtual structure approach to control the navigation of the multi robot system.

References 1. Adamek T, Kitts CA, Mas I (2015) Gradient-based cluster space navigation for autonomous surface vessels. IEEE/ASME Trans Mech 20(2):506–518. https://doi.org/10.1109/TMECH. 2013.2297152 2. Antonelli G, Arrichiello F, Chiaverini S (2005) The null-space-based behavioral control for mobile robots. In: 2005 international symposium on computational intelligence in robotics and automation, pp 15–20. https://doi.org/10.1109/CIRA.2005.1554248 3. Antonelli G, Arrichiello F, Chiaverini S (2008) The null-space-based behavioral control for autonomous robotic systems. Intell Service Robot 1(1):27–39. https://doi.org/10.1007/s11370007-0002-3 4. Antonelli G, Arrichiello F, Chiaverini S (2008) Stability analysis for the null-space-based behavioral control for multi-robot systems. In: 2008 47th IEEE conference on decision and control, pp 2463–2468. https://doi.org/10.1109/CDC.2008.4738697 5. Antonelli G, Arrichiello F, Chiaverini S (2009) Experiments of formation control with multirobot systems using the null-space-based behavioral control. IEEE Trans Control Syst Technol 17(5):1173–1182. https://doi.org/10.1109/TCST.2008.2004447 6. Bacheti VP, Brandão AS, Sarcinelli-Filho M. (2020) Path-following with a UGV-UAV formation considering that the UAV lands on the UGV. In: Proceedings of the 2020 international conference on unmanned aircraft systems (ICUAS), pp 488–497. Athens, Greece. https://doi. org/10.1109/ICUAS48674.2020.9213918 7. Bacheti VP, Brandão AS, Sarcinelli-Filho M (2021) A path-following controller for a uavugv formation performing the final step of last-mile-delivery. IEEE Access 9:142218–142231. https://doi.org/10.1109/ACCESS.2021.3120347 8. Baizid K, Giglio G, Pierri F, Trujillo MA, Antonelli G, Caccavale F, Viguria A, Chiaverini S, Ollero A (2015) Experiments on behavioral coordinated control of an unmanned aerial vehicle manipulator system. In: 2015 IEEE international conference on robotics and automation (ICRA), pp 4680–4685. https://doi.org/10.1109/ICRA.2015.7139848

References

143

9. Brandão AS, Rampinelli VTL, Martins FN, Sarcinelli-Filho M, Carelli R (2015) The multilayer control scheme: a strategy to guide n-robots formations with obstacle avoidance. J Control, Autom Electric Syst 26(3):201–214. https://doi.org/10.1007/s40313-015-0170-x 10. Brandão AS, Sarcinelli-Filho M (2016) On the guidance of multiple uav using a centralized formation control scheme and delaunay triangulation. J Intell Robot Syst 84(1):397–413. https:// doi.org/10.1007/s10846-015-0300-5 11. Brooks R (1986) A robust layered control system for a mobile robot. IEEE J Robot Autom 2(1):14–23. https://doi.org/10.1109/JRA.1986.1087032 12. Carelli R, Soria C, Morales B (2005) Vision-based tracking control for mobile robots. In: Proceedings of the 12th international conference on advanced robotics (ICAR), pp 148–152. Seattle, WA, USA. https://doi.org/10.1109/ICAR.2005.1507405 13. Consolini L, Morbidi F, Prattichizzo D, Tosques M (2008) Leader–follower formation control of nonholonomic mobile robots with input constraints. Automatica 44(5):1343–1349. https:// doi.org/10.1016/j.automatica.2007.09.019 14. Ernandes-Neto V, Sarcinelli-Filho M, Brandão AS (2019) Trajectory-tracking of a heterogeneous formation using null space-based control. In: 2019 international conference on unmanned aircraft systems (ICUAS). Atlanta, GA, USA, pp 187–195. https://doi.org/10.1109/ICUAS. 2019.8798031 15. Freire E, Bastos-Filho T, Sarcinelli-Filho M, Carelli R (2004) A new mobile robot control approach via fusion of control signals. IEEE Trans Syst Man Cyber Part B (Cyber) 34(1):419– 429. https://doi.org/10.1109/TSMCB.2003.817034 16. Gimenez J, Salinas L, Gandolfo D, Rosales C, Carelli R (2020) Control for cooperative transport of a bar-shaped payload with rotorcraft uavs including a landing stage on mobile robots. Int J Syst Sci 1–14. https://doi.org/10.1080/00207721.2020.1815096 17. Kitts C, Mas I (2009) Cluster space specification and control of mobile multirobot systems. IEEE/ASME Trans Mechatron 14(2):207–218. https://doi.org/10.1109/TMECH.2009. 2013943 18. Lewis MA, Tan KH (1997) High precision formation control of mobile robots using virtual structures. Autonom Robot 4(4):387–403. https://doi.org/10.1023/A:1008814708459 19. Marino A, Parker L, Antonelli G, Caccavale F (2009) Behavioral control for multi-robot perimeter patrol: a finite state automata approach. In: 2009 IEEE international conference on robotics and automation, pp 831–836. https://doi.org/10.1109/ROBOT.2009.5152710 20. Moreira MSM, Brandão AS, Sarcinelli-Filho M (2019) Null space based formation control for a uav landing on a ugv. In: 2019 international conference on unmanned aircraft systems (ICUAS), pp 1389–1397. https://doi.org/10.1109/ICUAS.2019.8797820 21. Pacheco Bacheti V, Santos Brandão A, Sarcinelli-Filho M (2021) Path-following by a ugvuav formation based on null space. In: 2021 14th IEEE international conference on industry applications (INDUSCON), pp 1266–1273. https://doi.org/10.1109/INDUSCON51756.2021. 9529472 22. Parker LE, Rus D, Sukhatme GS (2016) Multiple mobile robot systems. In: Siciliano B, Khatib O (eds) Springer handbook of robotics, chap 53. Springer, Oxford, pp 1335–1384 23. Rabelo MFS, Brandão AS, Sarcinelli-Filho M (2021) Landing a uav on static or moving platforms using a formation controller. IEEE Syst J 15(1):37–45. https://doi.org/10.1109/JSYST. 2020.2975139 24. Resende CZ, Carelli R, Sarcinelli-Filho M (2021) A path-following controller for guiding a single robot or a multi-robot system. J Control Autom Electric Syst 32:895–909. https://doi. org/10.1007/s40313-021-00725-w 25. Rosales C, Leica P, Sarcinelli-Filho M, Scaglia G, Carelli R (2016) 3d formation control of autonomous vehicles based on null-space. J Intell Robot Syst 84(1):453–467. https://doi.org/ 10.1007/s10846-015-0329-5 26. Santana LV, Brandão AS, Sarcinelli-Filho M (2021) On the design of outdoor leader-follower uav-formation controllers from a practical point of view. IEEE Access 9:107493–107501. https://doi.org/10.1109/ACCESS.2021.3100250

144

5 Control of Multi-robot Systems

27. Santos MCP, Rosales CD, Sarcinelli-Filho M, Carelli R (2017) A novel null-space-based uav trajectory tracking controller with collision avoidance. IEEE/ASME Trans Mechatron 22(6):2543–2553. https://doi.org/10.1109/TMECH.2017.2752302 28. Villa DKD, Brandão AS, Carelli R, Sarcinelli-Filho M (2021) Cooperative load transportation with two quadrotors using adaptive control. IEEE Access 9:129148–129160. https://doi.org/ 10.1109/ACCESS.2021.3113466

Index

A Attitude, 18

High level dynamic model of a quadrotor, 33 Homogeneous robot formation, 101

B Bicycle model of car-like robots, 11

I Inner-outer controllers, 43 Inverse dynamic model of unicycle robots, 25 Inverse dynamics, 82, 84 Inverse extended kinematics, 14 Inverse kinematics, 12, 41, 44, 47, 59

C Car-like robots, 10 Cluster space, 110 Cluster space control, 113 D Differential drive, 8 Direct extended kinematics, 14 Direct kinematics, 8, 9, 15, 20, 21 Dynamic controller, 75 Dynamic model of car-like robots, 29 Dynamic model of quadrotors, 32 Dynamic model of unicycle robots, 23 E Extended kinematics, 12, 14 F Feedback linearization, 21, 45, 80 Flexible formation, 127 Flexible structure, 135 Formation space, 110 H Heterogeneous formation, 101

K Kinematic controllers, 21 Kinematic model, 5

L Leader-follower multi-robot system, 102 Leader-follower paradigm, 100 Linear parameterization of the inverse dynamic control law of the differential-drive robot, 86 Linear parameterization of the inverse dynamic model of a quadrotor, 89 Linear parameterization of the inverse dynamic model of the differentialdrive robot, 85 Low level dynamic model of a quadrotor, 33

M Motion control, 41 Movement Jacobin, 112

© The Editor(s) (if applicable) and The Author(s), under exclusive license to Springer Nature Switzerland AG 2023 M. Sarcinelli-Filho and R. Carelli, Control of Ground and Aerial Robots, Intelligent Systems, Control and Automation: Science and Engineering 103, https://doi.org/10.1007/978-3-031-23088-2

145

146

Index

N Near hover, 19, 48 Nonholonomic robots, 5, 10, 12 Null space-based behavioral control, 126, 129

R Rigid formation, 127 Rigid structure, 135 Robots space, 110

O Omnidirectional robots, 5, 6

T Trajectory-tracking, 42

P Parameter identification, 28 Passivity, 26, 27 Path-following, 42 Path-following control, 63 Path-following with a quadrotor, 68 Path-following with a unicycle robot, 65 Positioning control, 42, 59 Propulsion, 17

U Underactuated system, 17 Unicycle model for a differential drive robot, 8 Unicycle robot, 8

Q Quadrotor, 16

V Velocity-tracking error, 23 Virtual structure paradigm, 100