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.
ditzinger@springer.com)
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,
⎡ ⎤ ⎡
ẋc
cos ψ − sin ψ
ẋ = ⎣ ẏ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)
ẋc = u cos ψ,
ẏc = u sin ψ,
(2.5b)
ψ̇ = ω,
(2.5c)
and
or, in a compact way
⎡ ⎤ ⎡
ẋc
cos ψ
ẋ = ⎣ ẏ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 ẋc and ẏc over the axis Y r , which will result in
ẋc cos(90 − ψ) − ẏ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
L
r−
(2.8a)
lw
2
and
δ2 = arctan
L
r+
,
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
ẋc = u cos(ψ),
(2.10a)
ẏ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
δ
. One important detail is that such a robot is not
by u = ψ̇r. Thus, ψ̇ = ur = u tan
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 ẋc and ẏc over such axle results in
ẋc sin ψ − ẏc cos ψ = 0,
(2.11)
since ẋc = u cos ψ and ẏc = u sin ψ. Similarly, regarding the direction of the axle
correspondent to the two front wheels one has
ẋ f sin(ψ + δ) − ẏ 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
ẋc = u cos(ψ) − v cos(90 − ψ) − aω sin(α + ψ),
(2.13a)
ẏc = u sin(ψ) + v cos(ψ) + aω cos(α + ψ),
(2.13b)
ψ̇ = ω,
(2.13c)
and
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]
⎤⎡ ⎤
⎡ ⎤ ⎡
ẋc
cos ψ − sin ψ −a sin(α + ψ)
u
⎦
⎣
⎦
⎣
⎣
ẏ
sin
ψ
cos
ψ
a
cos(α
+
ψ)
v ⎦ = Av,
=
(2.14)
ẋ =
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
⎡
⎤⎡ ⎤
ẋc
cos ψ sin ψ a sin α
(2.16)
v = A−1 ẋ = ⎣− sin ψ cos ψ −a cos α ⎦ ⎣ ẏ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
⎡ ⎤ ⎡
ẋc
cos ψ − sin ψ −a sin(α + ψ)
⎢ ẏc ⎥ ⎢ sin ψ cos ψ a cos(α + ψ)
⎥
⎢
⎢
ẋ = ⎣ ⎦ = ⎣
ψ̇
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
⎡
⎤⎡ ⎤
ẋc
cos ψ sin ψ a sin α 0
⎢− sin ψ cos ψ −a cos α 0⎥ ⎢ ẏc ⎥
−1
⎥⎢ ⎥,
(2.19)
v = A ẋ = ⎢
⎣ 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
θ̇ , φ̇ and ψ̇, can be determined.
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
ẋ w
ẋw = ⎣ ẏ w ⎦ = Rbw ⎣v by ⎦ ,
ẏ 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
ẋ 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 )
ẏ w = vxb sin ψ + v by cos ψ,
(2.25b)
ż 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
⎤ ⎡
ẋ w
cos ψ − sin ψ
w⎥
⎢
⎢
ẏ
⎥ ⎢ sin ψ cos ψ
ẋw = ⎢
⎣ ż 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ński 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
Ie d + 2r 2 Iz + mb2
ka
θ3 =
θ4 =
Ra
ka
θ5 =
θ6 =
Ra
ka
+ 2r dk D R
1
[s],
dk
(2r P R )
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
T
v vr dt −
V̇ dt =
0
T
T
0
v T Fvdt,
0
or
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
bω + aω
δ−
u
c2
Ma 2 sin δ
η(u, ω, δ) =
u−
ω +
cf
c1
c1
c1
,
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
ẋ . In addition, one can take
recall (2.22), or ẋw = Rbw vb , so that vb = Rbw
the time derivative of (2.22) to obtain ẍw = Rbw v̇b + Ṙbw vb , from where results that
−1 w
ẍ − Ṙbw vb . Similarly, regarding (2.23), or w = Hωb , one gets the
v̇b = Rbw
b
¨ w − Ḣωb ). Then, using such relationships one gets, now
relationship ω̇ = H−1 (
considering that the variables are all referred to the world frame [13], and including
the air drag effect [25],
m ẍ = (cos φ sin θ cos ψ + sin φ sin ψ) f zb − d1 ẋ,
(3.28a)
m ÿ = (cos φ sin θ sin ψ − sin φ cos ψ) f zb − d2 ẏ,
(3.28b)
m z̈ = (cos φ cos θ ) f zb − mg − d3 ż,
(3.28c)
Ix x θ̈ ≈ τφ − (Izz − I yy )θ̇ ψ̇ − d4 φ̇,
(3.28d)
I yy φ̈ ≈ τθ − (Ix x − Izz )φ̇ ψ̇ − d5 θ̇ ,
(3.28e)
Izz ψ̈ ≈ τψ − d6 ψ̇,
(3.28f)
and
where m is the mass of the vehicle, g is the gravity acceleration, ẋ, ẏ and ż are
the linear velocities along the axes X w , Y w and Z w , ẍ, ÿ and z̈ are the respective
accelerations, φ̇, θ̇ and ψ̇ are the angular velocities around the axes X w , Y w and Z w ,
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
ẋ
cos ψ − sin ψ 0 0
⎢ ẏ w ⎥ ⎢ sin ψ cos ψ 0 0⎥ ⎢ v by ⎥
w
b
⎥ ⎢
⎥⎢ ⎥,
v = Pv or ⎢
⎣ ż 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
⎡
⎤ ⎡
ẍ w
K 1 cos ψ
⎢ ÿ 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
ẍ w = K 1 cos ψu vx − K 3 sin ψu v y − K 2 cos ψvxb + K 4 sin ψv by ,
(3.32a)
ÿ 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 ż w = vzb and ψ̇ w =
T
T
vψ , so that vxb v by vzb vψb becomes vxb v by ż 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
(ẋd (t), ẏd (t)) or (ẋd (t), ẏd (t), ż 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 ẋ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)
ẋ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
ẋw = AA−1 (vdw + K p x̃w ) = vdw + K p x̃w ,
(4.5)
vdw − ẋ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 = ẋdw , so that the adopted
control law is
(4.8)
vd = A−1 (ẋdw + 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 ẋ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)
ẋdw − ẋ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 (x̃w ) = 21 x̃wT x̃w , and its time derivative V̇ (x̃w ) =
x̃wT x̃˙ w . Now, taking x̃˙ w = −K p x̃w from (4.9) one gets V̇ (x̃w ) = −x̃wT 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 (ẋdw + 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̇ (x̃w ) = −x̃wT 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̇ (x̃w ) 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 (ẋdw + 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 ẋ w , ẏ w
and ż w and the yaw velocity ψ̇ w as well, for which the respective components of the
auxiliary velocity vector ν are given by,
vd 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 = ẏdw + k py tanh ỹ w ,
(4.12b)
vdz = ż 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
ẋdw = ẋdw (t) ẏdw (t) ż dw (t) ψ̇dw (t) ,
(4.14a)
which corresponds to
ẋdw (t) = −
ẏ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)
ż 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 = ẋdw + k px x̃
(4.18)
ν y = ẏdw + k py ỹ.
(4.19)
where k px and k py are positive nonzero gains, x̃ w and ỹ w are the position errors in the
world frame, and ẋdw and ẏ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 = ẋdw + k px x̃ w ,
ν y = ẏdw + k py ỹ w ,
(4.23b)
νψ = ψ̇d + k pψ ψ̃,
(4.23c)
and
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 = ẋdw + k px x̃,
(4.29)
ν y = ẏdw + k py ỹ,
(4.30)
where k px and k py are positive nonzero gains, x̃ w and ỹ w are the position errors in the
world frame, and ẋdw and ẏ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 = ẋ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 ẋ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 ẋ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, ñ is the distance between the point of interest for
control and the point cn , whose projections on the world axes are x̃ and ỹ, 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 ñ(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
ψt (s) = arctan
dy(s)
ds
dx(s)
ds
ds
dt
ds
dt
= arctan
dy(s)
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 ỹ = Vd sin ψt + k py ỹ,
ds
and
where k px and k py are positive nonzero gains, x̃ w and ỹ 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 ñ → 0, which is equivalent
to x̃ → 0 and ỹ → 0 when t → ∞, which is going to be shown now.
Regarding Fig. 4.21, considering a coordinate frame whose origin is the point
cn , and whose axes coincide with the directions of t and ñ, the so called Frenet
coordinates, one gets that
x̃ = −ñ cos(90 − φt ) = −ñ sin ψt ,
and
ỹ = ñ sin(90 − φt ) = ñ cos ψt .
66
4 Motion Control
In addition,
ñ˙ = ẋc cos(90 − ψt ) − ẏc cos ψt = ẋc sin ψt − ẏ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 , ẏ = ν y , and ψ̇ = νψ , or
ẋ = Vd cos ψt + k px x̃,
ẏ = Vd sin ψt + k py ỹ,
and
ψ̇ =
(−Vd cos ψt + k px x̃) sin ψ
(Vd sin ψt + k py ỹ) cos ψ
+
.
a cos α
a cos α
To analyze the behavior of the error ñ, let us consider the Lyapunov candidate function
V (ñ) =
ñ 2
,
2
for which the first time derivative is
V̇ (ñ) = ñ ñ˙ = ñ ẋ cos ψt − ñ ẏ cos ψt = ñνx sin ψt − ñν y cos ψt ,
considering the previous relationships. Therefore,
V̇ (ñ) = ñ sin ψt (Vd cos ψt + k px x̃) − ñ cos ψt (Vd sin ψt + k py ỹ),
or
V̇ (ñ) = ñ sin ψt k px x̃ − ñ cos ψt k py ỹ.
As x̃ = −ñ sin ψt and ỹ = ñ cos ψt , it comes that
Ṽ (ñ) = ñ sin ψt (−k px ñ sin ψt ) − ñ cos ψt (k py ñ cos πt ),
or
Ṽ (ñ) = −k px ñ 2 sin2 ψt − k py ñ 2 cos2 ψt ,
so that V̇ (ñ) < 0 for all ñ = 0, once the gains k px and k py be selected as positive
values, thus meaning that ñ → 0when t → ∞, or the closed loop control system
is asymptotically stable. As ñ = ẋ 2 + ẏ 2 , this result means that ẋ → 0 and ẏ →
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 (ñ and t), we can note that vdw in (4.3) is on
the t direction and K p x̃w is on the direction of ñ, therefore being perpendicular vectors. 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
the path, including following of yaw angle ψd (s). On the normal direction ñ, we get
ñ = 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 ỹ
with
V =
0
Vd
for ñ > ε, 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 ỹ).
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
⎤
ẍ w
⎢ ÿ 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 ẋb ,
(4.38)
for instance, one can adopt the control law
u = F1−1 ẍdw + K D x̃˙ w + K P x̃w + F2 ẋb ,
(4.39)
when designing a dynamic controller, for which K P and K D are diagonal positive
definite gain matrices, and ẍ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 = ẋdw − ẋ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 ẋw = Avb , one gets the new state-space
description, based on the concept of equivalent systems [8],
or
ẍw = APA−1 ẋw + Abvr = APvb + Abvrb ,
(4.40b)
ẍ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 ẋdw is zero, as well as ẍ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
ẋdw = −5ω sin(ωt) m/s 10ω cos(2ωt) ) m/s 0 ) m/s
and
ẍ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( ẏdw /ẋ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
ẍw = F1 F1−1 ẍdw + K D x̃˙ w + K P x̃w + F2 ẋb − F2 ẋ b ,
or
ẍw = ẍdw + K D x̃˙ w + K P x̃w ,
or, finally,
ẍdw − ẍ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
ẋ1 = x2 ,
ẋ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 ẋ1 + x2T ẋ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 ṽ + 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 ṽ − C(v)v + C(v)v,
4.11 Cascade Dynamic Compensation
83
from which one gets
v̇ = v̇d + K D ṽ,
or
v̇d − v̇ + K P ṽ = ṽ˙ + K D ṽ = 0,
which allows concluding that ṽ(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 ṽ. 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ṽ,
(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ṽ − K P x̃w )
A sufficient condition for V̇ < 0 is
x̃wT K P x̃w > x̃wT Aṽ ,
with K P = diag(k pi ), i = 1, . . . , n. Such a condition implies that
min(k pi )||x̃w ||2 > |x̃w Aṽ|,
or
||x̃w || >
||Aṽ||
,
min(k pi )
(4.46)
guaranteeing that for great x̃w values, regarding that ṽ 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 ṽ(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 ṽ, 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 [v̇d + K D (vd − v) + f2 v] = f1−1 v̇d + K D ṽ + 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 = ṽ 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 (v̇d + K D ṽ + f2 v) − f2 v,
(4.48a)
v̇ = v̇d + K D ṽ,
(4.48b)
v̇d − v̇ + K D ṽ = ṽ˙ + K D ṽ = 0,
(4.48c)
from which it results
or
from where one can conclude that ṽ(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,
by an estimation error θ̃ , such that θ = θ̂ − θ̃, or θ̂ = θ + θ̃ . Therefore, the modeled
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(ṽ) + 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
robot, with ṽ = vd − v. As for Ĥ, Ĉ(v) and F̂(v), they are the matrices H, C(v)
and F(v) characterized by the estimate θ̂ of θ, whereas
T
,
T(ṽ) = lu tanh kluu ũ lω tanh klωω ω̃
86
4 Motion Control
with ũ = 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 ũ and
lω tanh klωω ω̃ have the same signal as ũ and ω̃, respectively.
From such a control law one can write
⎤
⎡
θ̂1 u̇ d + lu tanh kluu ũ − θ̂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
ũ
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(ṽ) + C(v)vd + F(v)vd = Gθ.
However, recalling that θ̂ = θ + θ̃, one gets vr = Gθ + Gθ̃ , or, using the model
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(ṽ). However, as ṽ = vd − v, and thus ṽ˙ = v̇d − v̇, one
gets v̇d = ṽ˙ + v̇, so that σ = ṽ˙ + v̇ + T(ṽ), and hence σ − v̇ = ṽ˙ + T(ṽ), resulting
that
H ṽ˙ + T(ṽ) + C(v)ṽ + F(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 (ṽ, θ̃ ) =
T
1 T
ṽ Hṽ + 21 θ̃ γ −1 θ̃ be the radially unbounded Lyapunov candidate function, which
2
4.12 Adaptive Control
87
is positive for all nonzero ṽ and θ̃ and zero for ṽ = 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̇ (ṽ, θ̃ ) = ṽ T Hṽ˙ + θ̃ γ −1 θ̃,
or
T
V̇ (ṽ, θ̃ ) = ṽ T −Gθ̃ − C(v)ṽ − F(v)ṽ − HT(ṽ) + θ̃ γ −1 θ̃˙ ,
taking (4.53) into account. Moreover, as C(v) is a skew symmetric matrix (see the
model properties in Chap. 3), the term ṽ T C(v)ṽ in the above equation is null, thus
resulting
T
V̇ (ṽ, θ̃ ) = −ṽ T Gθ̃ − ṽ T F(v)ṽ − ṽ T HT(ṽ) + θ̃ γ −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 ṽ = θ̃˙ ,
the time derivative of the Lyapunov candidate function becomes
V̇ (ṽ, θ̃ ) = −ṽ T F(v)ṽ − ṽ T HT(ṽ),
because the last term in the right hand compensates for the first one when introducing
(4.54). This way, V̇ (ṽ, θ̃ ) is negative semi-definite, since it is zero for any θ̃ when
ṽ = 0, and is negative for any nonzero value of ṽ, since F(v) > 0, H > 0 (see the
model properties in Chap. 3), and T(ṽ) has the same signal of ṽ. From such result,
and recalling that V is radially unbounded, it is possible to conclude that ṽ ∈ L ∞
and θ̃ ∈ L ∞ , meaning that both errors are bounded.
Now, taking the integral of V̇ (ṽ, θ̃ ) over the time interval [0, T ], with T finite,
one has
T
T
T
V (T ) − V (0) = − ṽ HT(ṽ) dt − ṽ T Fṽ dt,
0
0
which becomes, after dropping the positive terms V (T ) and the first integral,
T
−V (0) ≤ −
ṽ T Fṽ dt,
0
or
T
V (0) ≥
ṽ T Fṽ dt.
0
88
4 Motion Control
However, as F is diagonal and positive definite (see the model properties in
Chap. 3), the inequality
λmin (F)||ṽ||2 ≤ ṽ T Fṽ ≤ λmax (F)||ṽ||2
holds. Nonetheless, considering that λmin (F) and λmax (F) can vary with u, one can
write
inf[λmin (F)]||ṽ||2 ≤ ṽ T Fṽ ≤ sup[λmax (F)]||ṽ||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)]
||ṽ||2 dt.
0
As such equation is valid for all integration time T, one can write
∞
V (0) ≥ inf[λmin (F)]
||ṽ||2 dt,
0
or
∞
||ṽ||2 dt ≤
0
V (0)
,
inf[λmin (F)]
allowing concluding that ṽ is square integrable, or ṽ ∈ 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 ṽ, since ṽ ∈ L ∞ , and remembering
that ṽ = 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 ṽ˙ is limited, so that ṽ˙ ∈ L ∞ and ṽ ∈ L 2 , which allows to state, using
the Lemma of Barbalat [3], that ṽ(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
Dv̇
0
θ4
0
0
0
0
θ6
0
η
⎤
0
0⎥
⎥ v,
0⎦
θ8
or
vr = Dv̇ + η.
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 ṽ + η = D v̇d + T(ṽ) + η,
(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
ṽ = 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 ṽx , g23 = v̇dy + l y tanh lpyy ṽ y ,
k
k
.
ṽ
l z tanh lpzz ṽz , and g47 = v̇dψ + lψ tanh lpψ
ψ
ψ
with
g35 = v̇dz +
Now, considering the estimate θ̂ of θ, such that θ̃ = θ̂ − θ , the control law of
(4.58) based on the estimated parameters becomes
vr = Gθ̂ = Gθ + Gθ̃ = D v̇ + T(ṽ) + η + 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 ṽ(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 ṽw is converted to body
coordinates, resulting in ṽ. However, the parameter updating does not work continuously. Instead, when the quadratic norm of the velocity tracking error ṽ 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 = ẋdw + K p x̃w , such that ṽw = vdw − ẋw = ẋdw + K p x̃w − ẋw , resulting in
the velocity tracking error ṽw = x̃˙ w + K p x̃w , from which the value ṽ is obtained
using the matrix of inverse kinematics of the quadrotor.
The dynamic compensation is performed considering σ w = v̇dw + Kd (vdw − ẋw ).
Then, σ b and xb , in body coordinates, are obtained, and thus one gets
⎤
0
0
0
0
0
σ b (1) ẋb (1) 0
⎢ 0
0
0
0 ⎥
0 σ b (2) ẋb (2) 0
⎥,
G=⎢
⎣ 0
0 ⎦
0
0
0 σ b (3) ẋb (3) 0
0
0
0
0
0
0 σ b (4) ẋ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
λ = ṽ if ||ṽ||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) − ,
ẋ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
ẋ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,
ḋ = 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
⎡
⎤
⎡
xf
⎢
⎢
⎢y f ⎥
⎢
⎢ ⎥
⎢
⎢z f ⎥
⎥ = f (x) = ⎢
q=⎢
⎢
⎢ρ f ⎥
⎢
⎢ ⎥
⎢
⎣α f ⎦
⎢
⎣
βf
⎤
x1
⎥
y1
⎥
⎥
z1
⎥
(x2 − x1 )2 + (y2 − y1 )2 + (z 2 − z 1 )2 ⎥
⎥
⎥
z
−z
2
1
⎥
arctan √
⎥
(x2 −x1 )2 +(y2 −y1 )2
⎦
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
time, which means to determine q̇, it becomes necessary to calculate the velocities
ẋ 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
ẋ =
∂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 ẋr (t) = J−1 q̇r (t). After, such a reference velocity vector is split in two parts, ẋ1r (t) and ẋ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 ẋ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(ẋ) 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 ẋ
(5.8a)
q̇s = Js ẋ
(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 ẋr p = J†p q̇r p and ẋ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
ẋr = ẋr p + I − J†p J p ẋr s ,
(5.9)
where J†p is the right pseudo inverse of J p . The effect of pre-multiplying ẋr s by
I − J†p J p corresponds to project the velocity command ẋ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 ẋ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
ẋr = ẋr s + I − Js† Js ẋ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
ẋ = ẋ1 + I − J1† J1
ẋ2 + I − J2† J2 ẋ3 .
(5.11)
Generalizing for N subtasks one would get
ẋ = ẋ1 + I − J1† J1
ẋ2 + · · · ẋ N −1 + I − J†N −1 J N −1 ẋ 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,
ẋ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 ẋ = ẋ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 ẋ = ẋ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† ẋ = J1† ẋ, 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 ẋr p and q̇r s into ẋ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)
ẋr p = J†p q̇r p ,
and
ẋr s = Js† q̇r s ,
(5.19b)
where the superscript † means the right pseudo inverse of the matrix, and then
or
ẋr = ẋr p + I − J†p J p ẋr s
(5.19c)
ẋr = ẋr s + I − Js† Js ẋ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 (ẋr ) to the reference frame of the robots, what is
done through multiplying ẋ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
x(t)−xobs n
y(t)−yobs n
−
−
a
b
(x(t) − xobs )n−1
ẋ(t)
an
n n
x(t)−xobs
y(t)−yobs
(y(t) − yobs )n−1
−
−
a
b
+e
n
ẏ(t),
bn
V̇ = e
or
n
(y(t) − yobs )n−1
(x(t) − xobs )n−1
ẋ(t) +
ẏ(t) ,
V̇ = nV (t)
an
bn
(5.21a)
(5.21b)
136
5 Control of Multi-robot Systems
or
V̇ =
∂V ∂V
∂x ∂y
ẋ ẏ
T
= Jo ẋ,
(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)
ẋ = Jo† V̇d + kobs Ṽ
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 Ṽ = 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 ẋ = Jo Jo† V̇d + kobs Ṽ = V̇d + kobs Ṽ ,
from where
V̇d − V̇ + kobs Ṽ = 0,
and, finally
Ṽ˙ + kobs Ṽ = 0,
which allows to conclude that the robot moves causing Ṽ → 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