en français

. On dit que l'ingénieur montra au roi sa construction mécanique, d'aspect et de taille comparables à celles d'un humain. Cet exemple témoigne de l'intérêt immémorial de l'Homme pour les créations mimant le comportement humain, primant même sur son désir de reproduire le comportement des animaux ou des insectes. Selon moi, l'exemple le plus fascinant est celui des animaux volants et des insectes. Depuis ses débuts, l'Homo Sapiens ne cesse d'observer des corps volants et de s'imaginer voler. Il lui fallut beaucoup de temps pour réaliser ce rêve, mais, désormais, nous vivons dans un monde où se déplacer en avion est devenu monnaie courante et où nous ne sommes qu'à un pas de faire voler nos voitures. Dans cette optique, il semble que la robotique soit entrée dans une nouvelle ère, dans laquelle les robots sont pleinement intégrés à nos sociétés. Le lecteur de ce manuscrit pourra d'ailleurs faire le parallèle avec la révolution numérique du 21ième siècle. Cela fait d'ailleurs plusieurs décennies que les robots sont utilisés dans des applications civiles et militaires. Aujourd'hui, ils sont principalement employés par l'industrie, l'armée et l'agriculture, mais ils sont aussi présents en médecine, sous la forme de nanorobots, dans nos foyers etc. La liste ne cesse de s'allonger, et, malgré cela, chaque année, de nouvelles manières d'utiliser les robots, insoupçonnées encore il y a vingt ans, émergent. C'est le cas, par exemple, des robots cuisiniers 1 , serveurs, ou bien de ceux qui seront utilisés pour administrer des médicaments plus efficacement à des patients [2, 3].

Imiter le comportement d'un seul animal ou insecte est encore le centre d'attention de grands sujets de recherche [4][5][6]. Cependant, depuis des siècles, les chercheurs se sont aussi intéressés au comportement collectif des animaux, des insectes et des bactéries [7]. Pour cette raison, l'un des objectifs de la communauté robotique

est de produire des robots capables d'imiter des comportements présents dans la nature, tels que l'essaimage (en anglais swarming), le rassemblement (flocking), l'attroupement (herding), et le rassemblement en bancs (shoaling/schooling) [START_REF] Dorigo | Ant system: optimization by a colony of cooperating agents[END_REF][START_REF] Martinoli | Understanding collective aggregation mechanisms: From probabilistic modelling to experiments with real robots[END_REF][START_REF] Bonabeau | Swarm intelligence: from natural to artificial systems[END_REF][START_REF] Mondada | Swarm-bot: A new distributed robotic concept[END_REF][START_REF] Martinoli | Modeling swarm robotic systems: A case study in collaborative distributed manipulation[END_REF][START_REF] Dorigo | Ant colony optimization[END_REF][START_REF] Halloy | Social integration of robots into groups of cockroaches to control self-organized choices[END_REF][START_REF] Karaboga | A survey: algorithms simulating bee swarm intelligence[END_REF]. Même en dehors de la robotique, de nombreux chercheurs étudient ces comportements pour pouvoir les répliquer. Un aspect fascinant de ces derniers est que les agents de tels groupes semblent souvent agir en suivant seulement leur propre intention, alors qu'une ruche d'abeilles, une colonie de fourmis ou une migration d'oiseaux apparaissent particulièrement organisées, comme si leurs membres agissaient dans un but commun. Les chercheurs comprirent que dans ces situations, les agents suivent quelques règles simples, le plus souvent sans avoir besoin d'un chef. En suivant celles-ci, le comportement du groupe semble alors harmonieux. Dans la Fig. 1.1 sont présentés deux exemples de coopération chez les fourmis de feu qui surgissent grâce aux principes mentionnés. Dans ces situations, et dans bien d'autres également, les (a) (b)

Figure 1 -Exemples de comportements collectifs exposés par des fourmis de feu. Fig. 1(a): un tas de 500 fourmis de feu, composées d'une couche partiellement mouillée de fourmis sur le fond et les fourmis sèches sur sommet, [START_REF] Mlot | Fire ants self-assemble into waterproof rafts to survive floods[END_REF]. Fig. 1.1(b): flottabilité et élasticité du radeau de fourmis, comme indiqué par immersion essayée par une brindille, [START_REF] Mlot | Fire ants self-assemble into waterproof rafts to survive floods[END_REF].

agents (animaux ou insectes) ne disposent que des informations qu'ils ressentent, et ne peuvent agir que localement (c'est-à-dire par rapport à eux-mêmes). Il n'existe pas d'information partagée par tous les agents, et aucune forme d'encadrement.

Durant la dernière décennie, un intérêt croissant fut porté aux applications multirobots. En particulier, la recherche sur la coordination multi-véhicules débuta vers la fin des années 1980, dans le domaine de la robotique mobile (se référer à [START_REF] Parker | Current state of the art in distributed autnomous mobile robotics[END_REF][START_REF] Murray | Recent research in cooperative control of multi-vehicle systems[END_REF] pour un état de l'art dans ce domaine, valable jusqu'à 2006). La plupart des scénarios multi-robots sont similaires aux comportements de groupe adoptés par certains animaux et insectes, abordés précédemment. D'ailleurs, l'un des éléments communs à toute formation organisée est l'objectif principal (par exemple, la construction d'une ruche, le transport d'un morceau volumineux de nourriture d'un endroit à un autre, la migration de tout un groupe d'oiseaux etc) à atteindre de manière décentralisée (chaque agent ne connaît qu'en partie l'état de ses voisins). Pour atteindre cet objectif, un élément que l'on retrouve dans chaque formation organisée est la faculté de chaque agent à interagir seulement avec une sous-partie du groupe. Ce sous-partie du groupe est d'habitude appelé les voisins de l'agent considéré. Certains agents sont même capables de se repérer par rapport aux autres et de prendre des décisions, en se basant uniquement sur des informations locales.

En résumé, il arrive souvent que des agents de la formation doivent effectuer des actions nécessitant des mesures relatives (relatives aux autres agents de la formation, et non à un référentiel central au groupe). Parmi ces mesures relatives on trouve les distances relatives, les bearings 2 relatives, les positions relatives et, bien sûr, des combinaisons des trois. En robotique, ce type de mesure peut être obtenu à partir de capteurs embarqués, comme, par exemple, des capteurs ultrasonores, des scanners lasers, des caméras ou des émetteurs/récepteurs radiofréquences. Leur utilisation, pour extraire des mesures relatives, permet à la structure de contrôle de s'affranchir de la présence de systèmes de localisation centralisés, comme le dispositif de capture de mouvement proposé par Vicon, ou le GNSS et l'utilisation de, par exemple, algorithmes SLAM [START_REF] Howard | Experiments with a large heterogeneous mobile robot team: Exploration, mapping, deployment and detection[END_REF][START_REF] Yan | A survey and analysis of multi-robot coordination[END_REF]. Ces concepts nous amènent à un des motifs principaux du développement des systèmes de multi-agent qui est la possibilité de décentralisation.

iii d'un robot terrestre est que son champ d'action est, le plus souvent, limité à deux dimensions. Ceci, combiné à l'intérêt croissant des chercheurs pour les drones, a suscité un fort intérêt pour l'application des concepts des formations multi-robots terrestres à leurs homologues aériens et, en particulier, ceux capables de décoller et d'atterrir verticalement (en anglais VTOL UAVs) [START_REF] Schwager | Eyes in the sky: Decentralized control for the deployment of robotic camera networks[END_REF].

Dans cette thèse, nous nous intéressons plus particulièrement aux quadrotors, des drones de taille réduite, et à l'utilisation d'informations visuelles pour les coordonner. Cette technologie a d'ailleurs été appliquée avec succès à la fouille de sites dévastés par des catastrophes naturelles, telles que les séismes de Chirstchurch en Nouvelle-Zélande (2011), Emilia-Romagna (2012) et Amatrice (2016), ou bien après le désastre nucléaire de Fukushima Daiichi, au Japon (2011). De plus, les drones sont fréquemment utilisés pour filmer des événements sportifs comme les Jeux Olympiques d'hiver de Sotchi, en Russie, ou la coupe d'Europe 2016, ayant eu lieu en France. Un autre exemple est celui du spectacle de Lady Gaga, organisé pendant la mi-temps du Super Bowl (2017), et pour lequel 300 drones Shooting Star de chez Intel furent déployés. Il s'agissait de l'une des premières fois que les drones étaient utilisés pour un événement télévisuel. Selon l'institut de recherche Gartner 3 , l'industrie des drones personnels ou commerciaux devrait générer des revenus de l'ordre de 5 milliards d'euros en 2017 et plus de 9,4 milliards d'euros en 2020 [START_REF]Forecast: Personal and Commercial Drones[END_REF].

Malgré cette croissance exponentielle du domaine de la robotique, l'utilisation d'un dispositif entièrement autonome dans un environnement inconnu et déstructuré constitue toujours, aujourd'hui, un sujet actif de recherche. Il n'est donc pas surprenant que trois ambitieux projets aient été financés à hauteur de 22,2 millions d'euros ces six dernières années pour approfondir la thématique de la coordination multi-robots [START_REF]ARCAS: Aerial Robotics Cooperative Assembly System[END_REF][START_REF]SHERPA: Smart collaboration between Humans and ground-aErial Robots for imProving rescuing activities in Alpine environments[END_REF][START_REF]AEROWORKS: Collaborative Aerial Workers[END_REF]. De la même manière, les compétitions de robotique dédiées aux applications multi-robots sont de plus en plus fréquentes et attirent toujours plus d'investissements 4 .

Contributions de la Thèse

L'objectif de la thèse est de proposer des innovations pour résoudre les problèmes mentionnés auparavant (en particulier, le contrôle de la formation et la localisation par coopération), dans le cas d'un groupe de drones de type quadrotor, équipés de caméras monoculaires. Un intérêt est porté sur la mise en place d'un contrôle de la formation entièrement décentralisé et de techniques d'estimation reposant sur (i) des mesures relatives, obtenues à partir de caméras embarquées, et (ii) une communication locale utilisant le standard Wi-Fi. Pour que les résultats obtenus soient les plus réalistes possibles, certaines limitations classiques des capteurs sont prises en considération. Une approche alternative permettant de résoudre le problème de localisation dans un environnement inconnu, à l'aide de capteurs embarqués, est également envisageable avec des techniques de mapping/SLAM [START_REF] Howard | Experiments with a large heterogeneous mobile robot team: Exploration, mapping, deployment and detection[END_REF]. Cependant, cela nécessiterait soit une carte détaillée de l'environnement, soit la possibilité d'exécuter des algorithmes complexes de type SLAM en temps réel, directement sur les robots. Par ailleurs, nous pensons que la résolution du problème de localisation par coopération, uniquement à partir d'informations locales (capteurs et communication) aux drones, offrirait au système plus de souplesse, lui permettrait d'évoluer dans des environnements inconnus et n'impliquerait pas qu'il faille embarquer de nombreux capteurs ni une grande puissance de calcul sur chaque drone.

Pour contrôler une formation de drones, il est important d'avoir une quantité métrique de quel récupérer les distances entre les robots. Cependant, cette information ne peut être obtenue uniquement à partir d'informations visuelles (non-métriques) d'entrée. Ainsi, dans ce manuscrit, nous présentons également une méthode permettant d'extraire en temps réel l'échelle de la formation (et donc les distances entre les robots) à l'aide de mesures de bearing et en supposant que les robots ont connaisv sance des vitesses linéaires et angulaires exprimées dans leur repère (body-frame). La dernière hypothèse est, selon nous, assez faible puisque chaque drone doit avoir connaissance de ces grandeurs, afin de contrôler son vol.

Les résultats théoriques présentés dans ce manuscrit ont été validés de manière approfondie en simulation et durant des expérimentations. Ces dernières ont été réalisées avec un groupe de drones de type quadrotors (Annexe B).

Structure de la thèse

Le présent manuscrit est divisé en trois grandes parties. La première (I) contient une courte introduction sur la robotique qui détaille plus particulièrement les applications multi-robots, en insistant sur celles ayant recours à des drones. Une synthèse de ces thématiques est proposée, et plusieurs états de l'art issus de la littérature sont cités pour appréhender le contexte dans lequel s'inscrivent les travaux. La deuxième partie (II), quant à elle, constitue le coeur de la thèse. Elle en présente les contributions principales. Les résultats étayés dans cette seconde partie ont fait l'objet des publications suivantes : [START_REF] Schiano | A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs[END_REF][START_REF] Schiano | Bearing rigidity maintenance for formations of quadrotor uavs[END_REF][START_REF] Schiano | The Dynamic Bearing Observability Matrix, Nonlinear Observability and Estimation for Multi-Agent Systems[END_REF]. La troisième (III), et dernière, partie conclut le manuscrit, expose les futurs travaux à mener et s'achève sur deux annexes, permettant au lecteur d'approfondir certains aspects techniques. Un sommaire est détaillé ci-dessous.

Aperçu de la Partie I

La première partie contient un état de l'art exhaustif sur le contrôle et la localisation des formations multi-robots, et introduit des fondamentaux sur la théorie algébrique des graphes et de la rigidité.

Le Chapitre 2 donne un aperçu des concepts portant sur les formations multirobots, et explicite ceux appliqués aux drones. Il inclut notamment une brève description des problèmes majeurs inhérents au contrôle des formations multi-agents, en se focalisant sur les problèmes de consensus et de contrôle de la formation. Dans sa conclusion, ce chapitre décrit le problème de localisation par coopération à partir de mesures relatives.

Le Chapitre 3 propose une introduction sur la théorie algébrique des graphes. Celle-ci est fondamentale pour, ensuite, exposer les principes de la théorie de la rigidité. La dernière partie de ce chapitre approfondit la théorie de la rigidité, appliquée aux cas de la distance et des contraintes de bearing. Cela permettra d'aboutir, par la suite, au concept clé de cette thèse, à savoir la matrice de rigidité bearing (BRM).

Le Chapitre 4 décrit les équations modélisant un seul drone et fournit quelques détails sur la rigidité bearing en R 3 ×S 1 , qui seront utilisés dans les chapitres suivants. vi Aperçu de la Partie II La seconde partie de ce manuscrit décrit les contributions apportées par les travaux conduits dans le cadre de cette Thèse. Il s'agit de techniques permettant de contrôler et de localiser, de manière décentralisée, une formation de drones de type quadrotors dans R 3 × S 1 . Ensuite, cette méthode est combinée avec un algorithme assurant la rigidité de la formation, une propriété essentielle pour la convergence de cette dernière et pour les modèles de localisation. La dernière contribution repose sur une analyse non-linéaire de l'observabilité d'un système multi-agents composé de plusieurs quad-rotors. Cette étude montre qu'il est possible d'estimer l'échelle de la formation uniquement à partir de mesures de bearing et des vitesses linéaires et angulaires des robots, exprimées dans leur repère.

Le Chapitre 5 aborde la conception d'un algorithme permettant de contrôler et de localiser, de manière décentralisée, une formation. Il est inspiré de [START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF]. Cet algorithme de localisation est capable d'estimer les positions et le lacet des agents. Cependant, il est important de garder en mémoire que ce dernier n'a pas connaissance des poses absolues des robots. Par conséquent, il existera systématiquement un décalage lié à une rototranslation globale du système. Il subsiste également une ambiguïté d'échelle, liée à l'absence de données de distance (métriques). Celle-ci est levée par l'introduction d'une unique mesure de distance. Des expérimentations avec cinq quad-rotors sont présentées à la fin du chapitre.

Le Chapitre 6 expose le problème de maintenance de la rigidité bearing d'une formation de quadrotors. Cette propriété est d'une importance capitale pour résoudre les problèmes de contrôle et de localisation de la formation, présentés dans les chapitres précédents. La stratégie de maintenance mise en place est robuste à des limitations des capteurs telles que le champ de vision réduit des caméras, leur faible portée, ainsi que d'éventuelles occlusions entre les agents lors du mouvement de la formation.

Le Chapitre 7 détaille l'analyse de l'observabilité du système non-linéaire constitué par plusieurs drones. Celle-ci est préliminaire à la réalisation d'un filtre de Kalman étendu, qui est implémenté directement dans SE(3), ceci lui permettant d'estimer les positions et les orientations des agents de la formation. Il est important de noter que, par rapport à l'algorithme de localisation présenté au Chapitre 5, les positions sont toujours estimées sans avoir connaissance de la rototranslation globale du système, mais avec la bonne échelle. Ce résultat est obtenu sous l'hypothèse (réaliste) que les valeurs d'entrée transmises à chaque agent, dans le repère qui leur est propre (body-frames), sont connues.

Aperçu de la Partie III

Le Chapitre 8 présente les conclusions des travaux de Thèse et en résume les vii contributions. De plus, des pistes de recherche pertinentes à explorer sont proposées et discutées. Celles-ci sont d'ailleurs actuellement étudiées par l'auteur de ce manuscrit.

L'Annexe A détaille certains concepts mathématiques présentés au Chapitre 6.

L'Annexe B décrit brièvement l'architecture (hardware et software) utilisée durant les expérimentations présentées dans les Chapitres 5,6,7. viii Abstract Since humans exist, they have been witnessing the great power of nature. Among the many fascinating behaviors we find in nature, one which has inspired the work of researchers from all over the world is the show offered every day by insects and vertebrates with their collective behaviors. Indeed, one aspect common to all previous categories is that they give birth to complex cooperative behaviors through really simple actions. If, to this concept, we add also the environment variable we are talking about a really specific social network mechanism which is called stigmergy. Stigmergy is the phenomenon of indirect communication mediated by modifications of the environment. As an example, it is intriguing to think about ants. Each ant is mainly able to do two simple actions: (i) leave traces of different pheromone perfumes in the environment and (ii) follow these traces. Relying only on these local capabilities ants are capable to give birth to exceptionally complex behaviors such as the construction of anthills, transportation of food (alone or in groups), sticking together to form a whole and float in water or resist to an external force.

Since many years researchers from all over the world are trying to understand the very small details of these behaviors in order to replicate them. Human beings try to replicate nature because they are aware that nature often presents them with efficient solutions to very specific problems (e.g., flying between two places, flocking or moving an object from one point to another). This is made possible by the fact that nature is constantly solving an optimization problem. This sophisticated process is usually known as evolution. Among the many disciplines tackling these problems, we are interested in robotics and specifically in multi-robot applications.

In this broad context, the aim of this Thesis is to give contributions to the state of the art on the collective behavior of a group of flying robots, specifically quadrotor UAVs, which can only rely on their onboard capabilities and not on a centralized system (e.g., Vicon or GNSS) in order to safely navigate in the environment. We achieve this goal by giving a possible solution to the problems of formation control ix and localization from onboard sensing and local communication. We tackle these problems exploiting mainly concepts from algebraic graph theory and the so-called theory of rigidity. This allows us to solve these problems in a decentralized fashion, and propose decentralized algorithms able to also take into account some typical sensory limitations. The onboard capabilities we referred to above are represented by an onboard monocular camera and an inertial measurement unit (IMU) in addition to the capability of each robot to communicate (through RF) with some of its neighbors. This is due to the fact that an IMU and a camera represent a possible minimal, lightweight and inexpensive configuration for the autonomous localization and navigation of a quadrotor UAV. Notice that sensor limitations are present both in robotics and in nature (e.g., ants have a limited sensing range of the pheromone traces, birds have a limited field of view and so on).

A first contribution of this Thesis is the design of a formation control technique that allows the robots to achieve a certain shape only through bearing measurements coming from onboard monocular cameras and, at least, one distance measurement (e.g., coming from a rangefinder). In addition to this, the bearing formation can also be steered in 3D space without changing the bearings between the robots. We also couple this control algorithm with an estimation of the relative poses between the robots of the formation which is able to converge also for non-stationary agents. A second contribution of this Thesis consists in a strategy able to maintain formation rigidity over time against sensing limitations (limited field of view of the camera, maximum/minimum range of the camera, and occluded visibility).

Finally, in order to cope with the missing scale information from pure camera measurements, a third contribution of this Thesis consists in a technique able to estimate the scale of a formation of quadrotor UAVs only through bearing measurements and known agent ego-motion (body-frame linear/angular velocity).

All the theoretical developments discussed in this Thesis are corroborated by simulations and experiments run by using a group of quadrotor UAVs.

The reported results show the effectiveness of proposed techniques in controlling the motion of multiple quadrotor UAVs only relying on (constrained) onboard sensing/communication capabilities.

Keywords: multi-robot, formation control, cooperative localization, aerial robotics, rigidity theory.

x Here I am, at the end of another journey of my life trying to thank all the people who have contributed to it. For sure it was the most tiring (my first white hairs are the proof) but also a fascinating journey of my life. It was the one which changed the most my way of working and approaching different problems both in my professional and personal life.

For this, first of all, I should thank my supervisor: Paolo. Before starting the Ph.D., or sometimes at its beginning, I read that the admiration for your supervisor is inversely proportional to time, but for me was exactly the opposite. The more I went through my Ph.D. the more I recognized how lucky I was to be advised, encouraged and shaken up by him. As any Ph.D., mine was full of highs and lows and mainly thanks to his advice I was able to get across the main obstacle and learn from every error. I truly appreciated his ability to always be there when I asked for his help and to give me more space when I wanted to count mainly on my forces.

Then, I would like to thank Roberto, who guided me for 6 months (and more) in Boston during my visiting period. When I arrived in Boston I thought it would have been really hard to end up again with an amazing advisor but it (luckily) happened again. We addressed difficult topics which I thought to be impossible for me to deal with, but thanks to his patience I was able to get really a lot from those six months. As Paolo, he was incredibly available when I needed him and this helped a lot in giving my Ph.D. a final important boost. The decision of spending six months working with him was among the best decisions I have made during my entire Ph.D.

Another big thank you goes to the head of the Lagadic team: Francois. He gave me the opportunity to be part of an amazing, talented team. Thanks for our occasional, always interesting, talks. With him too, I was truly impressed that whenever I knocked at his door he was available to listen and to help me.

Thanks to my parents I became who I am right now. For this, I will be always xi grateful to you both. I could not ask for something better than you. During the years of my Ph.D., besides being the father that everyone would ask for, he was a priceless source of professional advice. I do not think I would be here without him conveying to me all the passion for math, engineering and so on. My mother is one (of the two) anchors for stability and balance in my life. She can understand my mood from the first ten words even if we are speaking over the phone, thousands of kilometers away from each other.

Thanks to my brother who has always believed in me and who, I know, was secretly my first supporter.

My grandmother would have been proud of me finishing a Ph.D. even without knowing what a Ph.D. is, thanks for your continuous admiration and love. I am grateful that I have many friends in my life but thanking all of them one by one would be quite hard. Therefore I say already sorry if I forgot you. Thanks to Philip, one of my best friends. Thanks for listening to my highs, but mainly to my lows during these three years. For me, you are the proof that friendship does not need 10 years to be built. Thanks to Bruno, Arturo, Anna, Luca, Mirko for your support during these years. We are definitely sure that our friendship is immune to distance. Thanks to Riccardo for his invaluable advice regarding my Ph.D. but mainly for his friendship. I am really glad to have met you during my Ph.D. Thanks to Francesco for introducing me to the Ferrari oven and for his friendship which brought a lot of interesting things in my life and I am sure it will continue to do so. Thanks to Giovanni for having listened to several problems during my Ph.D. At some points knocking at the door of the robotics room was almost the normal consequence of my frustration during experiments. Thanks to Rosa, Fausta and Riccardo for our serate tranquille at home with lots of food and less of Ph.D.-related-stuff. Thanks to Carlotta, Luiz, Roberto, Antonella, Claudia and Melania for our amazing times in Rennes. Thanks to the most recent Italian-Lagadic-subgroup: Marco, Marco and Claudio. To all of you, I hope that our friendship, started thanks to my period in Rennes will not end even if our ways will separate us. Thanks to all my friends in Naples, even talking to you for just 5 minutes helped me a lot sometimes. Thanks a lot to my Bostonian friends: Claudia, Alessandra, Caterina, Morgane, Johnny, Fernando, Mirza and Jamie. Without you, my experience in the US would have been way harder. The same goes for the Ph.D. students in the robotics lab in Boston, in particular Zac and Bee. Thanks to Nicola for having helped me with a session of experiments which finished really early (in the morning). Thanks to Pedro for the lots of coffees shared which brought a lot of nice and interesting conversations. Thanks to Muhammad for his help in the last session of experiments of my Ph.D. and for the interesting chats. Thanks to all the members of the Lagadic team, Fabien, Aurelien, Nicolas, Pol, Bryan, Hadrien, Axel, Firas, Louise, Suman, Aly, Vishnu and xii much more who were present in this amazing journey.

The last paragraph is to say GRAZIE to the most important person and the love of my life, Ida. Without you, I honestly think that my Ph.D. would have been ten times harder. You are the main source of balance/happiness/joy of my life and the person who every night listened to me in these years. You motivated me with your sincere and unconditional love. For this reason, I am and will be immensely grateful. You all made one of my dreams come true. Thank you.

Fabrizio. xiii

Acronyms and abbreviations

Notation

General notation conventions

Unless otherwise stated, the main conventions used in the notation of this Thesis are the following

• Scalar quantities are represented by lowercase symbols such as u, v, and so on.

• Elements of R n and similar sets are interpreted as column vectors and represented by bold lowercase symbols such as u, v, and so on.

• e d represents the d-th element of the standard basis in R n .

• We use the notation (a, b, c) to indicate a vertical concatenation of elements (scalars, vectors or matrices) and [a b c] for horizontal concatenations.

• I n is used to represent the identity matrix of dimension n × n.

• 0 n×m is used to represent the n × m matrix with all elements equal to zero. If m = 1 we also use 0 n .

• 1 n represents a vector of all ones of dimension n × 1.

• Matrixes of real numbers, i.e. elements of R n×m , are indicated with capital letters such as A, B and so on.

• A T denotes the matrix transpose of A.

• sym(A) is an operator that extracts the symmetric component of a matrix A, i.e., sym(A) = 1 2 (A + A T ).

• skew(A) is an operator that extracts the skew-symmetric component of a matrix A, i.e., skew(A) = 1 2 (A -A T ).

xix

• A † indicates the pseudoinverse of A.

• ⊗ denotes the Kronecker product.

• tr(A) is the trace of a matrix A.

• ḟ (•), f (•) are the first and second derivative of a scalar function f .

• stack(•) is an operator which returns a matrix containing a vertical stacking of the arguments.

• S n represents the n-dimensional sphere defined as

S n = {v ∈ R n+1 : v T v = 1}.
• •, • represents the Riemannian metric.

• M represents a Riemannian manifold and T x M is the tangent space of a manifold M at a point x ∈ M.

• SO(3) denotes the special orthogonal group of dimension three, i.e. the space

of 3-D rotations SO(3) = {R ∈ R 3×3 : R T R = I, det(R) = 1}.
• SE(3) denotes the special Euclidean group of dimension three, i.e. the space

of 3-D poses SE(3) = {(p, R) : R ∈ SO(3), p ∈ R 3 }. the space of 3-D poses SE(3) = {(p, R) : R ∈ SO(3), p ∈ R 3 }
• W represents an absolute 3-D world reference frame.

• A i represents a body reference frame attached to the i-th agent.

• R i ∈ SO(3) represents a rotation matrix transforming from A i to W.

• Let f : R n → R, p → f (p) be a generic scalar function of a vector argument.

We indicate with

∇ p f (p) =             ∂f (p) ∂p 1 p ∂f (p) ∂p 2 p . . . ∂f (p) ∂pn p            
the column vector built by stacking the partial derivatives of f with respect to the elements of p. This vector is also called the gradient of f w.r.t p.

xx

Graph theory

• V represents the vertex set which is a finite set of elements: V = {v 1 , . . . , v N }.

• E represents the edge set which is a subset of ordered pairs of V 2 , the

2-element subsets of V: V 2 = {(v i , v j ) , i = 1, . . . , N, j = 1, . . . , N, i = j}. E ⊆ V 2 and G is said to be undirected if (v i , v j ) ∈ E =⇒ (v j , v i ) ∈ E, while it is said to be directed if (v i , v j ) ∈ E =⇒ (v j , v i ) ∈ E.
• G = (V, E) represents a graph G with vertex set V and edge set E.

• e k represents the k-th edge associated to the graph G.

• N i = {j ∈ V| (i, j) ∈ E} ⊂ V represents the set of neighbors of an agent i with respect to a graph G.

• O i = {j ∈ V| (j, i) ∈ E} represents the set of agents j for which i is a neighbor (this distinction is important for directed graphs).

• p i represents the position of agent i in a world frame (usually in R 2 or R 3 )

• ϕ i , ϑ i , ψ i represent the respectively the roll, pitch and yaw associated to R i

• q i represents the configuration of the agent i which can be either the pair

(p i , ψ i ) or (p i , R i ) depending on the context • p ij represents the vector (p j -p i ) ∈ R 3
• d ij represents the actual distance between agents i and j computed as p j -p i

• β ij represents the bearing unit-norm vector between the agent i and j expressed in the body frame of the agent i, and it is defined as ) and Yan Shi, a mechanical engineer. It seems that the engineer showed the king a life-size, human-shaped figure of his mechanical creation [START_REF] Joseph | Science and Civilization in China[END_REF]. This is an example to show that humans have been interested in replicating human behaviors for a long time and, maybe, even before they tried to replicate animal/insect behaviors. Regarding the latter, the best and most fascinating example is, in my opinion, the one involving flying animals and insects. Homo Sapiens has seen flying bodies since the beginning of his time and since then he dreamed of flying. It took some time for Homo Sapiens to figure out how to fly but right now we live in a world in which airplanes are a completely normal thing and flying cars are likely to come next. It seems indeed that robotics is now entering a new era in which robots are more commonly accepted as an integral part of our society. It can be considered similar to what happened to the digital revolution at the beginning of the 21st century. Robots are being employed both in civil and military applications since decades now. At the current time, some of their main applications are industrial robots, military robots, agricultural robots, household robots, nanorobots, medical robots and so on. This list seems to expand year after year and robots are starting to be used in scenarios which we did not even imagine of two decades ago. Examples of this trend are represented by the robots that now are used as waiters, cooks 1 or the ones which will be used to deliver drugs in human bodies efficiently [START_REF] Douglas | A logic-gated nanorobot for targeted transport of molecular payloads[END_REF]3].

β ij = R T i p j -p i p j -p i = R T i Introduction Contents

1

Bearing-based localization and control for multiple quadrotors Replicating a single animal/insect behavior is still the focus of great research [4][5][6] but, since decades now, researchers are also being interested in replicating the collective behaviors of multiple animals, insects and bacteria [7]. For this reason, one of the goals of the robotic community is to make robots able to mimic behaviors present in nature such as swarming, flocking, herding and shoaling (schooling) [START_REF] Dorigo | Ant system: optimization by a colony of cooperating agents[END_REF][START_REF] Martinoli | Understanding collective aggregation mechanisms: From probabilistic modelling to experiments with real robots[END_REF][START_REF] Bonabeau | Swarm intelligence: from natural to artificial systems[END_REF][START_REF] Mondada | Swarm-bot: A new distributed robotic concept[END_REF][START_REF] Martinoli | Modeling swarm robotic systems: A case study in collaborative distributed manipulation[END_REF][START_REF] Dorigo | Ant colony optimization[END_REF][START_REF] Halloy | Social integration of robots into groups of cockroaches to control self-organized choices[END_REF][START_REF] Karaboga | A survey: algorithms simulating bee swarm intelligence[END_REF]. Several researchers, not only in robotics, are interested in studying, understanding and replicating these behaviors. One of the fascinating aspects of these behaviors is that the agents of these groups seem often to act only by following their own plan but yet it seems that a beehive, an ant colony or a bird migration are incredibly organized as if the agents are pursuing a master plan. Researchers understood that in these cases it happens that all the agents of the group are following some easy rules without, usually, the need of a supervisor. By following these rules, they are capable of giving birth to some specific group behaviors. In Fig. 1.1 there are some examples of cooperative behaviors exhibited by fire ants which arise thanks to the mentioned principles. In the case highlighted in Fig. 1.1, and in many others, the

agents (animals/insects) can use only what they sense and act locally (with respect to themselves).

There is neither a global information shared between the agents nor some sort of supervision happening.

The last decades have witnessed a growing interest in multi-robot applications, in particular, research about multi-vehicle coordination started around the end of the 1980s in the field of mobile robotics (refer to [START_REF] Parker | Current state of the art in distributed autnomous mobile robotics[END_REF][START_REF] Murray | Recent research in cooperative control of multi-vehicle systems[END_REF] for the state of the art up to 2006). Between the multi-robot scenarios, many of them are dealing with cases that are similar to the formation-type behaviors mentioned above for animals and insects. We can say that one common aspect of formation-type behaviors is that they have a particular centralized goal (i.e., build the anthill/beehive, transport a big piece of food from one point to another, migration of a whole group from one place to another and so on) to achieve in a decentralized manner (each agent knows only part of the state of its neighbors). In order to do that, one aspect that is common to all these groups is the ability, for each agent, of sensing and communication with a limited subset of the whole group. This subgroup is usually referred as the neighbors of the considered agent. Some agents are even able to localize themselves with respect to other agents of the formation and make decisions based only on local information.

To summarize, often it happens that the agents of the formation have to perform actions which involve relative measurements (relative to the different agents of the formation and not to a central unit/reference frame). Examples of relative measurements are relative distances, relative bearings2 or positions and of course combinations of the previous are possible. In robotics, this kind of measurements can be retrieved directly from onboard sensors; some examples are ultrasound sensors, laser scanners, cameras and radio-frequency transmitters/emitters. Using this type of sensors to extract relative measurements allows freeing the whole control structure from the presence of centralized localization systems as Vicon/GNSS and the use of, for example, SLAM algorithms [START_REF] Howard | Experiments with a large heterogeneous mobile robot team: Exploration, mapping, deployment and detection[END_REF][START_REF] Yan | A survey and analysis of multi-robot coordination[END_REF]. These concepts bring us to one of the main reasons for the development of multi-agent systems which is the possibility of decentralization.

Decentralized solutions play a significant role in multi-agent applications since they allow for scalable algorithms (in the sense of computational and communicational loads) with respect to the group size. However, even if the importance and the advantages of decentralization are quite clear to the multi-robot community it seldom happens to come across purely decentralized solutions for multi-robot systems. Indeed, this is the case of all those applications which are heavily relying on centralized localization systems such as the Vicon motion capture system (Fig. 1.2(a)) or the GNSS (Fig. 1.2(b)). These applications have a high value, but they would need substantial adjustments to be deployed in a real-world scenario where centralized systems are not available (e.g., inside a collapsed building after an earthquake, underwater, underground, in weak-GNSS locations or even in deep space).

In robotics, ground (mobile) robots continue to be widely used for multi-robot applications [START_REF] Howard | Experiments with a large heterogeneous mobile robot team: Exploration, mapping, deployment and detection[END_REF][START_REF] Charrow | Cooperative multi-robot estimation and control for radio source localization[END_REF], (see Fig. 1.2(c)). The use of ground robots is mostly due to their relatively low cost, their ease of use and control and they are (usually) intrinsic safeness. For all these reasons, at the beginning of multi-robot applications, they helped understand the main strengths of multi-agent systems along with the challenges present in their design. The main limitation of a ground robot is its pervasiveness limited mainly to 2D scenarios. This, and the fast-paced growth of the UAV technology, induced a strong interest in applying the multi-robot concepts apprehended through ground robot applications, to aerial robots and in particular to Vertical Take-Off and Landing (VTOL) UAVs [START_REF] Schwager | Eyes in the sky: Decentralized control for the deployment of robotic camera networks[END_REF].

In this Thesis, we are particularly interested in small-size UAVs, specifically quadrotors and in their coordination using mainly visual information. These objects have been successfully used in different scenarios like the investigation of sites after catastrophic events like the earthquakes of Christchurch (2011) in New Zealand, Emilia-Romagna (2012) and Amatrice (2016) in Italy or after the Fukushima Daiichi (2011) nuclear disaster in Japan. They are massively used to film sportive events like the winter Olympics in Sochi, Krasnodar Krai, Russia and the UEFA Euro 2016 held in France. A recent example which involved 300 Shooting Star drones by Intel and that has been literally celebrated in the news was the one during Lady Gaga's performance for the halftime show at the Super Bowl (2017). This event marked the first time in which drones have been used in a televised event. According to the research firm Gartner3 , globally the market revenue coming from the production of commercial and personal drones should hit 5 billion euros in 2017 and grow more than 9.4 billion euros in 2020 [START_REF]Forecast: Personal and Commercial Drones[END_REF].

Despite the exponential progress in the robotics field, the use of a fully autonomous system in a real-world unknown and unstructured environment is still a subject of active research. For this reason, it is not surprising that three very ambitious research projects [START_REF]ARCAS: Aerial Robotics Cooperative Assembly System[END_REF][START_REF]SHERPA: Smart collaboration between Humans and ground-aErial Robots for imProving rescuing activities in Alpine environments[END_REF][START_REF]AEROWORKS: Collaborative Aerial Workers[END_REF] about multi-robot coordination have been funded by the European Union for a total of about 22.2 millions of Euros in the last six years. Similarly, robotics competitions focused on multi-robot application are receiving exponential interest, and funding, all over the world4 .

Thesis contributions

Keeping these considerations in mind, this Thesis aims at advancing the state-of-theart in overcoming the problems mentioned above (in particular formation control and cooperative localization) for the case of a group of quadrotor UAVs equipped with monocular cameras. We address the design of fully-decentralized formation control and estimation techniques based on (i) relative measurements retrievable through onboard cameras and (ii) local communication through Wi-Fi. To make the results of this work even closer to a real-world deploying, we also deal with some typical sensor limitations. An alternative approach to tackle the problem of localization in unknown environments through onboard sensors is an absolute localization through mapping/SLAM techniques [START_REF] Howard | Experiments with a large heterogeneous mobile robot team: Exploration, mapping, deployment and detection[END_REF]. This would require though either a detailed map of the environment or the possibility of running complex SLAM algorithms in real-time onboard the robots. On the other hand, we believe that solving the cooperative localization problem relying only on the local skills (local sensing and local communication) of the quadrotor UAVs provides our system with a better flexibility allowing it to work in entirely unknown environments without having to embed on each robot significant sensing/computational capabilities.

In order to solve the formation control for a group of robots, it is important to have a metric quantity from which to retrieve the distances between the robots. As well-known, this is not retrievable solely from the non-metric visual inputs. Therefore, in this Thesis, we also present a method for retrieving online the scale of the formation (and therefore all the distances between the robots) through bearing measurements and assuming that the robots know their body-frame linear/angular velocities. The last assumption is, in our opinion, quite mild since any UAV needs knowledge of these body-frame quantities in order to control its flight.

All the theoretical results presented throughout the Thesis have been validated through extensive sets of simulations and experiments. The experiments were performed using as a platform a group of quadrotor UAVs (Appendix B).

Structure of the Thesis

This Thesis is divided into three main parts. The first part (Part I) contains a short introduction about robotics going more in details into multi-robot applications and especially the ones involving unmanned aerial vehicles (UAVs). A state of the art for these topics is provided, and several survey papers are referenced to help the reader in building up a well-defined context. The second part (Part II) represents the core of the Thesis and its main contributions. The results illustrated in this part correspond to the following author's publications: [START_REF] Schiano | A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs[END_REF][START_REF] Schiano | Bearing rigidity maintenance for formations of quadrotor uavs[END_REF][START_REF] Schiano | The Dynamic Bearing Observability Matrix, Nonlinear Observability and Estimation for Multi-Agent Systems[END_REF]. The third and last part (Part III) of the Thesis contains its conclusions, future work and two appendices which will help the reader in the understanding of some details of the Thesis. Below there is a summary of the content of each part of the Thesis.

Outline of part Part I

This part contains an extensive state-of-the-art for the topics of multi-robot formation control and localization and the fundamentals of algebraic graph theory and the theory of rigidity.

Chapt. 2 provides an overview of the multi-robot concepts and specifically of the ones applied to UAVs. There is a brief description of the main multi-agent control problems focusing more on the consensus and the formation control problems. This chapter ends with the description of the cooperative localization problem from relative measurements.

Chapt. 3 gives an introduction on algebraic graph theory. This is fundamental for the explanation of the concepts of rigidity theory. The last part of this chapter is devoted to explaining more the theory of rigidity both in the case of distance and bearing constraints. This will allow us to arrive at the essential concept, for this Thesis, of bearing rigidity matrix (BRM).

Chapt. 4 describes the equations regarding the model of the single UAV and we give some details about bearing rigidity in R 3 × S 1 which are instrumental for the following chapters.

Outline of Part II

The second part of this Thesis presents the contributions of this work related to a technique used for the decentralized formation control and localization of quadrotor UAVs in R 3 × S 1 . Then, we couple this method with an algorithm able to maintain rigidity of the formation, an essential property for the convergence of the formation control and localization schemes. The last contribution consists in a nonlinear observability analysis of a multi-agent system composed of multiple quadrotor UAVs. This study shows that it is possible to estimate the scale of a formation only from bearing measurements and linear/angular velocities of the robots in their body frames.

In Chapt. 5 is addressed a decentralized formation control and localization algorithm which takes inspiration from [START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF]. Note that the localization algorithm can estimate the positions and the yaw angle of the agents up to a global rototranslation of the whole framework. There is also an ambiguity related to the scale due to the absence of a metric information 5 . This ambiguity is removed with the introduction of a single distance measurement. Experiments with five quadrotors are presented at the end of the chapter.

In Chapt. 6 we deal with the problem of maintaining bearing rigidity of a formation of quadrotor UAVs. This property is of paramount importance to solve the problems of formation control and localization which we addressed in the previous chapter. The maintenance strategy can cope with some sensor limitations as limited field of view of the cameras, their limited range and the possibility of occlusions between the agents occurring during motion.

In Chapt. 7 is presented an observability analysis of the nonlinear system made of different UAVs. This analysis is preliminary to the design of an extended Kalman filter (EKF), implemented directly on SE(3) which is able to estimate the positions and orientations of the agents of the formation. It is important to note that, with respect to the localization algorithm proposed in Chapt. 5, the positions are always estimated up to a rototranslation of the whole framework but with the right scale without, for the latter, depending on the availability of a distance measurement. This is obtained by taking advantage of the (realistic) assumption of knowing the inputs given to each agent in their own body frame.

Outline of Part III

In Chapt. 8 are reported the conclusions of the Thesis and the main contributions brought to the state-of-the-art are summarized. Moreover, some open issues are listed and we discuss future directions which would be worth exploring. Note that some of the proposed future directions are the subject of the author's current research.

In Appendix A there are some mathematical details relative to Chapt. 6.

In Appendix B it is briefly described the hardware and software architecture used to carry out the experiments described in Chapts. 5

Introduction

A

S pointed out in the introduction of this Thesis (Chapt. 1), research on multiagent systems, and in particular multi-robot systems (e.g., quadrotors), has flourished over the last decades with a number of theoretical and experimental results. The improvements in this field were also made possible by the constant technological advancements in onboard sensing, communication and computing power [START_REF] Fox | Distributed multirobot exploration and mapping[END_REF][START_REF] Michael | Cooperative manipulation and transportation with aerial robots[END_REF][START_REF] Giordano | A passivitybased decentralized strategy for generalized connectivity maintenance[END_REF][START_REF] Hausman | Cooperative multi-robot control for target tracking with onboard sensing[END_REF][START_REF] Montijano | Visionbased distributed formation control without an external positioning system[END_REF]. These systems have many potential advantages with respect to single-agent systems (e.g., decentralized processing, resilience against failures of individual agents, faster task completion times and so on).

In a lot of multi-robot scenarios robots are acting either by themselves (with respect to other robots) or just with a limited awareness of the presence of other robots in their environment (Fig. 2.1). Since many years the robotics community has been attracted by the so called multi-agent systems (MASs) [START_REF] Murray | Recent research in cooperative control of multi-vehicle systems[END_REF]. A MAS usually denotes a group of several agents which are able to communicate and sense the whole group or part of it. Some multi-agent systems can also give birth to collective behaviors in order to fulfil different tasks and/or interact with the environment. Several reasons pushed researchers in developing such systems in lieu of single agent systems. One of them is that a MAS could be helpful in solving complex tasks in a robust (to single robot failures) and highly flexible way. The tasks addressed by multi-robot groups are usually the ones which would be hard or even impossible for a single agent and where the use of multiple robots translates in a drastic reduction of time for the completion of the task. An example is the PATH project at the University of Berkeley1 which demonstrated multiple cars driving together in platoons. This project served as inspiration for many projects which followed. as the one undertaken by the VISLAB2 which, between many achievements, completed the Vislab Intercontinental Autonomous Challenge (VIAC), see Fig. 2.2. More recent examples of multi-robot applications which have a big impact on the current society are the ones given by the mobile robots used to move shelves around warehouses at Amazon and Alibaba, see Fig. 2.3 while refer to [START_REF] Wurman | Coordinating hundreds of cooperative, autonomous vehicles in warehouses[END_REF] for an early description of the Kiva Systems which is behind the robots at Amazon warehouses. Within the scope of robotics, the following are between the most promising multi-robot fields of application: autonomous search and rescue, precision agriculture3 , military systems, mobile sensor networks, transportation systems (Fig. 2.2), firefighting, cooperative localization, surveillance [START_REF] Schwager | Eyes in the sky: Decentralized control for the deployment of robotic camera networks[END_REF][START_REF] Jaimes | An approach to surveillance an area using swarm of fixed wing and quad-rotor unmanned aerial vehicles uav (s)[END_REF] 4 , medical robotics, exploration, intervention and mapping in dangerous or inaccessible/unknown areas [START_REF] Howard | Experiments with a large heterogeneous mobile robot team: Exploration, mapping, deployment and detection[END_REF][START_REF] Fox | Distributed multirobot exploration and mapping[END_REF][START_REF] Franchi | The sensor-based random graph method for cooperative robot exploration[END_REF][START_REF] Renzaglia | Multi-robot three-dimensional coverage of unknown areas[END_REF], target tracking [START_REF] Hausman | Cooperative multi-robot control for target tracking with onboard sensing[END_REF][START_REF] Rafi | Autonomous target following by unmanned aerial vehicles[END_REF], large antenna synthetization with multiple robots, cooperative building of 3D structures [START_REF] Lindsey | Construction of cubic structures with quadrotor teams[END_REF][START_REF] Joo | A study on the advantages on high-rise building construction which the application of construction robots take (iccas 2007)[END_REF], complex cooperative manipulation and transportation [START_REF] Michael | Cooperative manipulation and transportation with aerial robots[END_REF][START_REF] Berman | Study of group food retrieval by ants as a model for multi-robot collective transport strategies[END_REF][START_REF] Sreenath | Dynamics control and planning for cooperative manipulation of payloads suspended by cables from multiple quadrotor robots[END_REF][START_REF] Wang | Force-amplifying n-robot transport system (forceants) for cooperative planar manipulation without communication[END_REF] Anywhere on page where float appears Another reason for the development of MASs is the understanding that it would be easier to control ten agents with limited capabilities than embed one agent with the capabilities of the aggregate ten-agents-system 6 . When designing a control algorithm for a group of agents it is important to make them rely on their onboard capabilities and not on a central unit otherwise a single failure could compromise the functioning of the whole system. In brief, the goal of research about MASs is to design control techniques which allow these systems to exploit all the strengths mentioned above 7 .

Among the multi-robot community, at the end of 1990s and beginning of 2000s, 5 www.flashhold.com 6 It is interesting to notice that interest in MASs really blossomed at the beginning of the 2000s. This was also the time in which it happened the switch, in microprocessor manufacturing, from single-core CPUs to multi-core CPUs. The reason of going from single-core to multi-core CPUs is the same which drove the development of MASs: researchers realized that it was easier to build a processor with multiple cores than one core with the same capabilities of a multi-core processor. 7 In this work the term multi-agent system is often used as a synonym of multi-robot system. If one wants to be more precise a multi-robot system can be seen in general as a subset of multi-agent systems.

research about the coordination of multiple aircraft and especially UAVs started to become really active [START_REF] Chandler | Uav cooperative control[END_REF]. As already said, a central part of this work is about the decentralized formation control and estimation for a group of quadrotor UAVs. For this reason the following sections describe more into details the state-of-theart of these topics, exploring first the concept of decentralization and giving a classification of the different UAVs, pointing out the differences between fixed-wing and rotary-wing UAVs.

However, just before diving into these notions, we will shortly describe a concept which is instrumental for the rest of this work: decentralization.

Decentralization

Beside multi-agent systems and robotics, decentralization plays a fundamental role in many domains such as economics, finance, politics and society. One basic and practical example of decentralization is the one of a group of people transporting a heavy load which would be impossible to even lift for one person (see Fig. 2.4(a)). Another example would be the one in which ants cooperate to transport a piece of food [START_REF] Berman | Study of group food retrieval by ants as a model for multi-robot collective transport strategies[END_REF][START_REF] Wang | Force-amplifying n-robot transport system (forceants) for cooperative planar manipulation without communication[END_REF], (see Fig. 2.4(b)). In these cases usually there is no central unit and there are communication capabilities but usually communication is not used for the whole task but only in its critical parts (e.g., people are not talking all the time while transporting a heavy load). In the same way there are sensing capabilities but usually they are not exploited for the whole duration of the task and/or they are changing over time depending on the relative configuration of the agents (e.g. people are not looking all the time at the other members of the group while transporting a heavy load). The key feature in all these cases is decentralization. On the other hand, according to [START_REF] Antonelli | Interconnected dynamic systems: An overview on distributed control[END_REF], a centralized system is such when at least one agent needs to sense a global information or communicate with all the other agents at once. In control theory terms a centralized system is one in which the controller is able to communicate and control with all the agents of the formation [START_REF] Belta | Abstraction and control for groups of robots[END_REF][START_REF] Antonelli | Kinematic control of platoons of autonomous vehicles[END_REF][START_REF] Antonelli | Stability analysis for prioritized closed-loop inverse kinematic algorithms for redundant robotic systems[END_REF]. This kind of structure (see Fig. 2.5(a) for an example) is usually easier to implement than a decentralized structure but it carries with it different problems:

• The complexity of the algorithm scales with the number of agents of the formation • The control structure is not robust, in the sense that the presence of a central control unit identifies also a single point of failure.

• The agents of the formation need to deal with a quantity of information that grows with the size of the agent group.

A decentralized system (see Fig. 2.5(b) for an example) instead scales well against the number of agents, it is more resilient to external threats and allows the agents to deal only with a limited quantity of information which, in general, does not grow with the group size [START_REF] Korchenko | The generalized classification of unmanned air vehicles[END_REF]. An example of classification upon weight is reported in table 2.1. However, classification of UAVs is outside of the scope of this work and therefore we only deal with the difference between UAVs which are defined fixed-wing (e.g., Fig. 2.6(a)) and rotary-wing (e.g., Fig. 2.6(b)) and we do it from an high level perspective.

Multi-aerial vehicles

Each of the two categories has its own benefits and downsides. The advantages and disadvantages of fixed-wing UAVs, with respect to rotary-wing UAVs, are reported in table 2.2.

Advantages

Disadvantages Simpler structure and therefore less maintenance required Usual need of a runway or a launcher to take-off and landing More efficient aerodynamics which translates in longer flight times at higher speeds

No hovering capabilities

Capability of carrying greater payloads for longer distances

Normally not suited for indoor applications due to their low degree of manoeuvrability when compared to rotary wing UAVs In both categories there is ongoing research to overcome the listed disadvantages. Examples include fully-actuated multi-rotor UAVs [START_REF] Ryll | Modeling and control of a quadrotor uav with tilting propellers[END_REF] as also VTOL fixed-wing UAVs [START_REF] Vuruskan | Dynamic modeling of a fixed-wing vtol UAV[END_REF] 10 . There are works, as [START_REF] Jaimes | An approach to surveillance an area using swarm of fixed wing and quad-rotor unmanned aerial vehicles uav (s)[END_REF], which envision to use swarms of both fixed-wing and rotary-wing UAVs. However, even if UAVs are being applied in many fields the regulations related to these objects are still at an embryological state, refer to [START_REF] Stöcker | Review of the current state of uav regulations[END_REF] for a survey and to Fig. 2.7 for a global overview of the UAV regulations up to October 2016. In addition to Fig. 2.7 it is also interesting to have a look at the global distribution of first releases of UAV regulations in the world in Fig. 2.8 to understand the pioneering countries of the world in the field of UAV regulations. Based on the advantages of rotary-wing UAVs listed in table 2.3 and especially because of their relatively low-cost and high pervasiveness in the 3D space we chose to use quadrotor UAVs for our research. The spectrum of application of quadrotor UAVs is really broad and, according to [START_REF] Stöcker | Review of the current state of uav regulations[END_REF], beside many military applications, between the quadrotor UAVs civil application there are: high-resolution surface reconstruction in the geosciences, documentation of cultural heritage and archaeological sites, precision agriculture and forest change detection, support for disaster management, surveying and mapping, land administration and wildlife observation. Beside these we can add package delivery, events filming (e.g., concerts, sport events) and the film industry.

The fields of application of single UAVs can be seen as a subset of the ones of group of UAVs. Indeed, multiple UAVs can fulfil the tasks assigned to a single UAV in a fraction of the time a single UAV would take [START_REF] Augugliaro | The flight assembled architecture installation: Cooperative construction with flying machines[END_REF]. Beside these tasks there are others which are only suitable for multiple UAVs and that are therefore impossible for a single UAVs. These are all the tasks in which cooperation is not only better but fundamental such as aerial mobile manipulation [START_REF] Gioioso | The flying hand: A formation of UAVs for cooperative aerial tele-manipulation[END_REF], cooperative load-carrying [START_REF] Mellinger | Cooperative grasping and transport using multiple quadrotors[END_REF] and cooperative construction [START_REF] Lindsey | Construction of cubic structures with quadrotor teams[END_REF][START_REF] Lindsey | Construction with quadrotor teams[END_REF][START_REF] Mirjan | Building a bridge with flying robots[END_REF] or really specific cooperative tasks [START_REF] Ritz | Cooperative quadrocopter ball throwing and catching[END_REF].

(a) (b) (c) (d)
Figure 2.9 -2.9(a): two quadrotors assembling a structure, from [START_REF] Augugliaro | The flight assembled architecture installation: Cooperative construction with flying machines[END_REF]. 2.9(b): concept of flying-hand for transportation of a rigid object with four UAVs commanded through human hand movements, from [START_REF] Gioioso | The flying hand: A formation of UAVs for cooperative aerial tele-manipulation[END_REF]. 2.9(c): two quadrotors tying a knot to build a o build a 7.4 m long rope bridge one can walk on, from [START_REF] Mirjan | Building a bridge with flying robots[END_REF]. 2.9(d): three quadrotors attached to a net used to juggle and catch a ball through, from [START_REF] Ritz | Cooperative quadrocopter ball throwing and catching[END_REF] All the works referenced in Fig. 2.9, together with others such as [START_REF] Giordano | A passivitybased decentralized strategy for generalized connectivity maintenance[END_REF][START_REF] Giordano | Bilateral teleoperation of groups of UAVs with decentralized connectivity maintenance[END_REF][START_REF] Lee | Semiautonomous haptic teleoperation control architecture of multiple unmanned aerial vehicles[END_REF], achieve quite impressive results but they are all relying on a centralized system to retrieve the poses of the involved agents. Again, the aim of this Thesis is to find decentralized algorithms for (i) the formation control and (ii) cooperative localization of a group of quadrotor UAVs without the need of a centralized system and therefore relying only on the local skills of the robots. Some results in this sense are starting to appear in the literature [START_REF] Krajnik | A practical multirobot localization system[END_REF][START_REF] Loianno | Estimation, control, and planning for aggressive flight with a small quadrotor with a single camera and imu[END_REF]. Hence, in the following sections a comprehensive state-of the-art of these two topics, specifically for UAVs, is given.

Different classifications have been proposed over the years to put some order within the realm of multi-agent systems (e.g., [START_REF] Oh | A survey of multi-agent formation control[END_REF] focuses on the sensing capabilities and the interaction topology of the agents while [START_REF] Olfati-Saber | Consensus and cooperation in networked multi-agent systems[END_REF] ). For the next sections we adopted a terminology (and a structure) similar to [START_REF] Antonelli | Interconnected dynamic systems: An overview on distributed control[END_REF].

Multi-agent control problems

A good starting point to have an idea about some of the most common multi-agent control problems in the literature is Fig. 2.10. Following the structure of [START_REF] Antonelli | Interconnected dynamic systems: An overview on distributed control[END_REF], we go through the same problems but focusing more on the problems of formation control and localization which are instrumental for this work.

Note that, when dealing with the control of multi-agent systems, due to the already intrinsic complexity of the problem, it is common to start the design of the control algorithm by considering the nodes/agents/robots as having simple dynamics (e.g., simple/double integrators). Therefore, usually the dynamics of the agents is not taken into account. This does not prevent the addition of the dynamics of the agent in a second stage of the design of the algorithm.

Consensus and rendezvous

Consensus-based algorithms have their roots in computer science [START_REF] Lynch | Distributed algorithms[END_REF] but, since many years now, they are playing a central role in the multi-robot literature [START_REF] Olfati-Saber | Consensus and cooperation in networked multi-agent systems[END_REF][START_REF] Olfati-Saber | Consensus problems in networks of agents with switching topology and time-delays[END_REF][START_REF] Freeman | Stability and convergence properties of dynamic average consensus estimators[END_REF], refer to [START_REF] Ren | A survey of consensus problems in multi-agent coordination[END_REF] for a survey. The consensus problem, according to [START_REF] Olfati-Saber | Consensus problems in networks of agents with switching topology and time-delays[END_REF], is the process of reaching an agreement concerning a certain quantity of interest depending on the state of all the agents. Without going too much into details we can think about the consensus protocol as a solution to the following problem. We have a set of N agents with an internal state x i ∈ R m . As we said above, the internal dynamics for the state evolution of each agent is considered to be the one of a single integrator and therefore: ẋi = u i (with u i the input to agent i). Let us also associate an interaction graph G to the formation, where the agents are the vertexes of G. The problem is to design the control inputs u i so that all the states x i converge to the same common value x:

lim t→∞ x i (t) = x, ∀i (2.1)
The main characteristic of the consensus protocol is that it achieves (2.1) by making use only of relative information with respect to the state of the neighbors, therefore basing everything on relative sensing and on a decentralized structure of the control algorithm. A basic solution to this problem is achieved by choosing the inputs u i such that:

u i = j∈N i (x j (t) -x i (t)) (2.2)
where N i is the set of neighbors of agent i. It is also possible to express (2.2) in a compact form for all the agents as:

ẋ (t) = -(L ⊗ I m )x (t) (2.3)
where x ∈ R N m is the stack of all the x i , and L ∈ R N ×N is the Laplacian associated to the graph G. More details about the Laplacian matrix can be found in Sect. 3.2.

Another key feature of the consensus protocol is that the convergence is assured if the graph G is connected (i.e., the second smallest eigenvalue λ 2 of the Laplacian matrix L is non-null, see Sect. 3.3.

The consensus protocol has several applications and depending on them it gets different names. For example:

• Rendezvous: a consensus protocol to meet at a common point. Its goal is to uniform the positions of the agents.

• Alignment: a consensus protocol to point in the same direction. Its goal is to uniform the angles of the agents.

• Distributed estimation: a consensus protocol to agree on the estimation of some distributed quantity (e.g., average humidity/temperature in a room)

• Synchronization: a consensus protocol to agree on the same time regardless of phase shifts or different rates in the clocks.

The consensus protocol is the model on which are based many decentralized laws for control and/or estimation. A more generic form of the (2.2), neglecting the time dependence, is:

ẋi = u i = j∈N i k i f (x j -x i ) (2.4)
where f is a local action function of the relative state between the agent i and its neighbors. For instance, assume that p i ∈ R m is the agent position in some m-dimensional space and let p * = (p * 1 , . . . , p * N ) a set of desired positions. The dynamics of the agent i is such that ṗi = u i . Then the following law converges to the desired shape up to, of course, a global translation:

u i = k i j∈N i (p i -p j ) -p * i -p * j (2.5)
It is important to note that in order to implement the (2.5) there is the need for the agent i to know its relative position with respect to its neighbors in a common reference frame.

Formation control

Another possibility of multi-agent control is the so called formation control, which is of big interest for this work. In general, formation control is the problem of controlling a group of agents by imposing geometric constraints on their relative poses. Depending on the states of the robots and on their sensing capabilities one can impose certain constraints which result in different behaviors (e.g., through interdistances [START_REF] Anderson | Rigid graph control architectures for autonomous formations[END_REF][START_REF] Hou | Control of formations with non-rigid and hybrid graphs[END_REF] or angles [START_REF] Basiri | Distributed control of triangular formations with angle-only constraints[END_REF][START_REF] Bishop | A very relaxed control law for bearing-only triangular formation control[END_REF][START_REF] Bishop | Stabilization of rigid formations with direction-only constraints[END_REF][START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF] between the agents). For example, in [START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF] it is presented a formation controller which is able to keep a formation defined in terms of bearing constraints and which requires only bearing measurements. The controller converges almost globally and maintains limited inter-agent distances even without metric informations. Regarding specifically UAVs, one of the first works about formation flight control is represented by [START_REF] Parker | Designing control laws for cooperative agent teams[END_REF].

Formation control can be also seen as a specialized version of (2.2) and (neglecting the time-dependence) can be expressed with the following, which is a sort of generalization of the (2.5)

u i = j∈N i o i ij -o id ij (2.6)
where o i ij ∈ R m is the current relative measurement between i and j and o id ij ∈ R m is an offset which encloses the desired formation information. Note that this approach can be applied easily to both bearing and distance cases. In the first case o i ij , o id ij would be a bearing while in the second case a distance. Of course other options are possible depending on the application.

Formation control is still a really active field of research, for example in [START_REF] Fathian | Distributed formation control under arbitrarily changing topology[END_REF] the desired formation is defined in terms of both inter-agent distances and angles and there is no need of a global or common reference frame.

Formation control can be implemented through different architectures depending on the requirements. Some renowned implementations of formation control are the leader-follower and flocking paradigms.

Leader-follower, as its name suggests, consists of a type of formation control in which there is one agent which acts as a leader and the remaining agents are the followers, see Fig. 2.11(a). In the literature there is plenty of applications to this paradigm (e.g., target tracking, environment patrolling) but one of its main disadvantages is that it creates a system with a single point of failure (the leader).

Flocking takes inspiration from nature and it refers to a certain behavior exhibited by birds. A similar behaviour is showed in groups of fishes and it is referred as schooling (if the group is swimming in the same direction in a coordinated manner) or shoaling (if the group of fishes stay together for social reasons). The difference with respect to the leader-follower behavior is that in this case there is no leader. On the other side, the thing in common with the leader-follower architecture is that the agents cannot see all the other agents of the formation. In the case of birds the formation changes over time as the birds in front get tired they leave their place to birds which are behind them optimizing the efficiency of the migration or other goals. [START_REF] Francis | Flocking and rendezvous in distributed robotics[END_REF] represents a recent and complete work specifically on flocking and rendezvous behaviors.

Cooperative localization algorithms

As the problem of formation control, cooperative localization is another fundamental problem in the multi-robot applications. In this context, with cooperative localization we refer to the process of estimating the configuration 12 of the agents in a common reference frame from a sparse set of relative measurements between the robots. As in the rest of the Thesis, the aim is to solve the localization problem without relying on a centralized localization system as a GNSS or the Vicon motion capture system.

The topic of cooperative localization was firstly investigated in [START_REF] Kurazume | Cooperative positioning with multiple robots[END_REF]. Many more followed based on different approaches for the localization, such as probabilistic approaches [START_REF] Fox | Collaborative multi-robot localization[END_REF][START_REF]A probabilistic approach to collaborative multi-robot localization[END_REF], based on Kalman filters [START_REF] Panzieri | Multirobot localisation using interlaced extended kalman filter[END_REF][START_REF] Roumeliotis | Distributed multirobot localization[END_REF][START_REF] Kia | A centralized-equivalent decentralized implementation of extended kalman filters for cooperative localization[END_REF] or particle filters [START_REF] Prorok | Low-cost collaborative localization for large-scale multi-robot systems[END_REF]. Note that cooperative localization plays a central role not only in multi-robot systems but also in many others multi-agent systems as sensor networks [START_REF] Shirmohammadi | Self localizing smart camera networks[END_REF][START_REF] Eren | Graph invariants for unique localizability in cooperative localization of wireless sensor networks: rigidity index and redundancy index[END_REF][START_REF] Williams | Evaluating network rigidity in realistic systems: Decentralization, asynchronicity, and parallelization[END_REF][START_REF] Shames | Analysis of Noisy Bearing-Only Network Localization[END_REF], Structure from Motion (SfM) [START_REF] Brand | Spectral solution of large-scale extrinsic camera calibration as a graph embedding problem[END_REF][START_REF] Li | Multi-view structure computation without explicitly estimating motion[END_REF] in computer vision, and graph drawings [START_REF] Servatius | Constraining plane configurations in computeraided design: Combinatorics of directions and lengths[END_REF] 13 . This problem is of paramount importance in all the scenarios highlighted in Sect. 2.1 such as the ones in which the robots need to navigate in an unknown environment and therefore they need to know their relative configuration. In this cases usually the absolute configurations (with respect to a centralized system) of the robots is not needed but the relative one (with respect to a reference frame attached to the formation) would be enough. The solution to the localization problem can follow different approaches, which strongly depend on the sensors carried onboard by the different agents used for the relative measurements. Some work has been done in position-based localization [START_REF] Calafiore | A distributed gradient method for localization of formations using relative range measurements[END_REF], bearing-based localization [START_REF] Eren | Using angle of arrival (bearing) information for localization in robot networks[END_REF][START_REF] Ye | Bearing-only measurement self-localization, velocity consensus and formation control[END_REF] and a mix of the two [START_REF] Martinelli | Observability analysis for mobile robot localization[END_REF][START_REF] Zhou | Determining the robot-to-robot 3d relative pose using combinations of range and bearing measurements: 14 minimal problems and closed-form solutions to three of them[END_REF]. Our interest lies in the second category because, as we said already, we assume the robots equipped with monocular cameras.

There is one thing though which is seldom taken into account in the literature mentioned above. Cooperative localization algorithms need, together with relative sparse measurements, also the identity associated to these measurements. This means that each robot when it acquires a measurement it needs also to be capable to associate the measurement to a specific robot. This operation is often referred as tagging [START_REF] Stegagno | Mutual localization from anonymous measurements in multirobot systems[END_REF]. The identities of the robots need to be coherent between the robots of the formation. Here we want to stress out that this operation, according to the robot sensing capabilities, could be not trivial but it is not in the scope of our work. Therefore, all the algorithms described in the rest of the Thesis assume that tagging is an intrinsic capability of the robots along with their sensing capabilities, and consequently we do not deal explicitly with the measurement-anonymousity problem. Freeing the proposed algorithms from the need of identities is left as a possible interesting future work. About this topic, addressed as anonymous localization, research has been done both for the case of position measurements [START_REF] Franchi | Probabilistic mutual localization in multi-agent systems from anonymous position measures[END_REF][START_REF] Franchi | Mutual localization in a multi-robot system with anonymous relative position measures[END_REF] (in 2D) and for the one of bearing measurements [START_REF] Cognetti | 3-D mutual localization with anonymous bearing measurements[END_REF][START_REF] Stegagno | Mutual localization using anonymous bearing measurements[END_REF][START_REF] Stegagno | Ground and aerial mutual localization using anonymous relative-bearing measurements[END_REF] (respectively in 2D and 3D).

In this chapter we gave some details about multi-agent systems, focusing on multi-robot systems (e.g., UAVs) and going through the most relevant problems to this field of research. Deliberately, we gave more emphasis to two of these problems: formation control and cooperative localization. The reason for this choice is simple: we believe that the theory of rigidity together with algebraic graph theory represents the perfect tool to tackle both problems. Therefore, in the following Chapt. 3 we give some details about these topics which represents the theoretical core on which this work is based.

Chapter 3

Algebraic graph theory and graph rigidity

Introduction

I

n the previous Chapt. 2 we gave a brief overview about multi-robot systems and we understood that an ideal scenario for multi-robot applications would be one which is embedded with the following characteristics. There should be availability of relative poses with respect to any other robot in the group in a common reference frame. There should be the possibility to communicate with any other robot in the group with no delays. Each agent should have unlimited memory and processing power (onboard/offboard). In real-world scenarios this is seldom the case. Instead, in many realistic applications, these systems are subject to several limitations, requirements and constraints such as:

• limited sensing: partial measurement of the other robot states (e.g., distance, bearing), lack of a common shared frame, occlusions, limited field of view, limited range

• limited communication: occlusions, limited range, delays, maximum data rate

• limited memory and processing power.

Graphs represent a powerful mathematical structure to encode, in a compact manner, these limitations. Indeed, they are built to describe and model pairwise relationship between specific objects. For this reason graph theory plays a central role in this Thesis and lays the groundwork for the so-called rigidity theory which helps us solving the problems of formation control and cooperative localization. Hence, the following sections describe more in details graph theory and the theory of rigidity, two topics which are broadly used throughout the rest of the Thesis.

Graph theory

According to [START_REF] Biggs | Graph Theory[END_REF], the first appearance of graph theory can be found in a paper by Leonhard Euler (Fig. 3.1) on the Seven Bridges of Königsberg and published in 1736. His work was then followed by Augustin-Louis Cauchy [START_REF] Cauchy | Recherche sur les polyèdres-premier mémoire[END_REF] and Gergonne L'Huilier [START_REF] Huilier | Recherche sur les polyèdres-premier mémoire[END_REF] and gave birth to the mathematical field of topology. Since then, graph theory has been massively applied to many distinct areas such as sociology, computer science, mathematics, biology, chemistry and physics (refer to [START_REF] Mesbahi | Graph theoretic methods in multiagent networks[END_REF] for a thorough analysis of graph theoretic methods and their application in the analysis and design of multi-agent systems).

First of all, it is important to understand that several graphs can be associated to a single group of robots to encode different capabilities/properties of the group [START_REF] Franchi | Decentralized control of parallel rigid formations with direction constraints and bearing measurements[END_REF] and the information/action flow among the robots, such as: More graphs can be defined depending on the application. In the following section we go through some definitions of graph theory which are of help for the next developments.

•

Graph theory definitions

Let us define:

• V as the vertex set which is a finite set of elements:

V = {v 1 , . . . , v N } • V 2 as the 2-element subsets of V defined as V 2 = {(v i , v j ) , i = 1, . . . , N, j = 1, . . . , N, i = j} • E as the edge set which is a subset of ordered pairs of V 2 such as E ⊆ V 2
Then we indicate with G = (V, E) a graph G with vertex set V and edge set E. In general a graph G can be directed1 or undirected Fig. 3

.2. G is said to be undirected if (v i , v j ) ∈ E =⇒ (v j , v i ) ∈ E,
while it is said to be directed if

(v i , v j ) ∈ E =⇒ (v j , v i ) ∈ E
An undirected graph can be thought of as a special case of a directed graph where if an edge e ij ⊆ E then e ji ⊆ E. An undirected graph is said to be complete if for every pair of vertexes there is an edge connecting them. If the graph is directed, it is complete if each pair of vertexes is connected by a unique edge, one in each direction. In this Thesis we indicate with K the complete graph associated to a set of nodes. A node v j is said adjacent (neighbor ) of v i if (v i , v j ) ∈ E and therefore, given a node v i , the set N i defined as

N i = {j ∈ V| (i, j) ∈ E} ⊂ V (3.1)
represents the set of neighbors2 of an agent i with respect to a graph G. With O i we represent the set of agents j for which i is a neighbor defined as

O i = {j ∈ V| (j, i) ∈ E}, (3.2)
we note that the distinction between N i and O i is only relevant in case of directed graphs.

We can also define the degree of a node v i , indicated as deg(v i ), as the number of edges to which v i is connected, therefore it holds, for a graph G = (V, E), the so-called handshaking lemma3 :

v∈V deg(v) = 2|E|.
For a directed graph there is the need, with respect to a generic vertex v i , to define the in-degree (defined as deg -(v i )) and the out-degree (defined as deg + (v i )) respectively as the number of edges entering and exiting v i . Therefore it holds the so-called degree sum formula:

v∈V deg -(v) = v∈V deg + (v) = |E|. If, for every vertex v ∈ V, deg -(v) = deg + (v) the graph is called a balanced directed graph.
We can also define a path as a finite or infinite sequence of edges which connect a sequence of distinct vertices. If a path starts and ends in the same vertex the path is called cycle. An undirected graph is said connected if there exists a path joining any two vertexes in V. On the other side a directed graph is said strongly connected if there exists a directed path joining any two vertexes, while it is said to be weakly connected if there exists an undirected graph connecting any two vertexes in V.

An extension to the concept of graph explained so far can be the one of weighted graph. Usually the weight is associated to each edge of the graph. The easiest way to think about weighted graphs is with respect to sensing graphs and a weight associated to an edge in order to encode, e.g., how reliable is the corresponding measurement. Usually to each edge is associated a weight between 0 and 1 (where 0 indicates a loss of measurement and 1 maximum reliability) but other choices are completely legit. The concepts of weighted graphs are heavily exploited in this Thesis in Chapt. 6.

Algebraic graph theory

Graphs are great tools to encode, in a compact way, different information but it is possible to boost their power if we associate to them algebraic methods. This, gives birth to a branch of mathematics called algebraic graph theory. This is probably the feature which made graphs so widespread in so many different fields of engineering. One of the most investigated branches of algebraic graph theory is the one which links graphs to linear algebra. Regarding this, it is important to introduce three fundamental matrixes: adjacency matrix, degree matrix, incidence matrix.

Given a graph G = (V, E) the adjacency matrix A ∈ R |V|×|V| (where |V| is the number of nodes) has elements A ij defined as:

A ij = 1 if the edge e ij ∈ E 0 otherwise. (3.
3)

The degree matrix D ∈ R |V|×|V| has elements D ij defined as:

D ij = deg(v i ) if i = j 0 otherwise. (3.4)
Note that to a directed graph are associated an in-degree and an out-degree matrix.

The incidence matrix E, for directed graphs, has elements

E ij ∈ R |V|×|E| defined as 4 E ij =      -1 if the edge e ij exits from vertex v i +1 if the edge e ij enters into vertex v i 0 otherwise. (3.5)
Another fundamental matrix in algebraic graph theory is the so-called Laplacian matrix L ∈ R |V|×|V| (refer to [START_REF] Fiedler | Algebraic connectivity of graphs[END_REF][START_REF]Laplacian of graphs and algebraic connectivity[END_REF]) which can be built in two different ways starting from the matrixes defined above

L = D -A (3.6) L = EE T . (3.7)
The two definitions are equivalent and the latter is not dependent on the particular labelling or orientation chosen for the graph. For an undirected graph G the matrix L results symmetric and positive semi-definite. This means that all its |V| eigenvalues λ i are real and non-negative. If we order them as

0 = λ 1 ≤ λ 2 ≤ . . . ≤ λ |V| (3.8)
it holds that the graph G is connected if and only if λ 2 > 0 [START_REF] Fiedler | Algebraic connectivity of graphs[END_REF][START_REF]Laplacian of graphs and algebraic connectivity[END_REF] (e.g., refer to Fig. 3.4 for some examples).

The definitions given above are the most common ones in the literature. There are different definitions of these matrices but, in this work, when we refer to the adjacency, degree, incidence and Laplacian matrices we are referring to the definitions in (3.3) to (3.7)

Laplacian matrix and connectivity

The Laplacian matrix defined in (3.6-3.7) represents therefore a link between combinatorial properties of a graph (existence of a path among two agents) and spectral properties of a matrix associated to a graph (condition on λ 2 ). This means that there is therefore an equivalence between the combinatorial and spectral properties of graphs which plays a fundamental role for control and estimation purposes. The power of the Laplacian matrix is that its second smallest eigenvalue λ 2 offers a measure of the degree of connectivity of the graph to which the matrix is associated. For this reason λ 2 is often referred as the connectivity eigenvalue (or algebraic connectivity). It is important to highlight that λ 2 is a global property of the graph which means that it is influenced by all the nodes (agents) of the graph. For example, the Laplacian matrix can be used to study the consensus protocol (refer to Sect. 2.4.1), one of the major problems in multi-agent applications. It can be shown, by analyzing (2.3), that the convergence to an arbitrary equilibrium is related to the spectral properties of the matrix L. In particular L represents the state-transition matrix in the closed-loop dynamics of the system. Basically the consensus converges faster as the λ 2 increases and vice versa.

λ 2 = 4 λ 2 = 2 λ 2 = 0.58 λ 2 = 0
Previously, at the beginning of Sect. 3.2, we mentioned that to a specific group of robots we can associate different graphs. From this we can understand that the Laplacian matrix can be useful to model the connectivity of different properties of the group such as sensing, communication or action. For example, if the underlying communication graph associated to the group of robots is connected it means that there can be a flow of information between each pair of robots.

Analogously to connectivity and taking inspiration from structural mechanics one can define a stronger property which allows to study different properties of a group of agents and it involves not only the graph associated to the agents but also their relative position in space (2D or 3D). This property is called rigidity and it is discussed in the following section.

Rigidity theory

Rigidity has its roots in mechanics, this theory was born as a combinatorial theory for characterizing the stiffness or flexibility of structures made of rigid bodies connected by flexible linkages or hinges. One of the first works considered related to the theory of rigidity and graphs is the so-called Euler-conjecture about polyhedra.

Rigidity theory has found a large number of applications in the context of formation control (see Sect. 2.4.2) based on, e.g., relative distance measurements [START_REF] Anderson | Rigid graph control architectures for autonomous formations[END_REF][START_REF] Krick | Stabilisation of infinitesimally rigid formations of multi-robot networks[END_REF][START_REF] Anderson | Formal theory of noisy sensor network localization[END_REF][START_REF] Garcia De Marina | Controlling rigid formations of mobile agents under inconsistent measurements[END_REF][START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF] and relative bearing measurements [START_REF] Bishop | Stabilization of rigid formations with direction-only constraints[END_REF][START_REF] Franchi | Decentralized control of parallel rigid formations with direction constraints and bearing measurements[END_REF][START_REF] Eren | Formation shape control based on bearing rigidity[END_REF][START_REF] Cornejo | Scale-free coordinates for multi-robot systems with bearing-only sensors[END_REF][START_REF] Zhao | Bearing rigidity and almost global bearing-only formation stabilization[END_REF][START_REF]Translational and scaling formation maneuver control via a bearingbased approach[END_REF], mainly differing in the assumptions and/or simplifications taken at the design stage. For instance, regarding the bearing case, the authors of [START_REF] Bishop | Stabilization of rigid formations with direction-only constraints[END_REF][START_REF] Franchi | Decentralized control of parallel rigid formations with direction constraints and bearing measurements[END_REF] have considered the formation control of planar kinematic agents by assuming a common reference frame and an undirected topology for the measurement graph (i.e., all measurements are assumed to be reciprocal), and [START_REF] Zhao | Bearing rigidity and almost global bearing-only formation stabilization[END_REF] has extended these results to arbitrary dimensions. The work [START_REF] Cornejo | Scale-free coordinates for multi-robot systems with bearing-only sensors[END_REF] has instead dropped the assumption of a common reference frame while, however, retaining that of an undirected measurement topology. Another example is [START_REF] Aspnes | A theory of network localization[END_REF] in which they apply distance-based rigidity to beacon localization.

As mentioned several times, rigidity theory plays a pivotal role also in the dual problem of cooperative localization (see Sect. 2.5) from local relative measurements [START_REF] Calafiore | A distributed gradient method for localization of formations using relative range measurements[END_REF][START_REF] Eren | Using angle of arrival (bearing) information for localization in robot networks[END_REF][START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF][START_REF] Shames | Analysis of noisy bearingonly network localization[END_REF][START_REF] Tron | Rigid components identification and rigidity enforcement in bearing-only localization using the graph cycle basis[END_REF]. Indeed, the rigidity of the formation is a necessary requirement for recovering, from the available relative measurements, a consistent solution of the localization problem in a common shared frame.

It is interesting to recognize how (mechanical) rigidity played some role in the design of the first working aircraft of history, the one flown by Wright's brothers at Kill Devil Hills, North Carolina on December 17, 1903. Indeed, at the Smithsonian National Air and Space Museum, a small demo is offered to the visitors for understanding how, without the rigidity of some parts of the plane the airplane would have never taken off, see Fig. 3.5. The Wrights were aware of earlier biplane designs and especially the 1896 Chanute-Herring glider which had a special bracing. Steel wires crisscrossed between vertical wooden struts that supported the upper and lower wings, creating a simple, rigid structure. This structure was tested by the Wrights for the first time on a kite they built in 1899. Rigidity can also play a combining role between two usually separated communities, the multi-robot and the parallel robots ones. This has been a topic of discussion in the workshop Rigidity Theory for Multi-agent Systems Meets Parallel Robots held during the 2017 IFAC congress in Toulouse (France) 5 . Intuitively each robot of a multi-agent system could be seen as a passive joint of a virtual mechanical parallel robot and each measurement between them as a rigid connection.

As aforementioned, the theory of rigidity can be formulated in case of any kind of geometrical constraints on the edges, distances and bearings are two possible cases. In this section, we focus on the case of distance constraints because, for the sake of illustration, they are more intuitive to deal with. Therefore, each edge in the following graphs represents the distance between the interested nodes. From here we gradually evolve towards the case of bearing constraints in 3D space, which is the case considered in all the scenarios of this Thesis and is detailed in Sect. 4.3. Note that the main results, theorems, and definitions described below for the case of distance constraints usually apply flawlessly to the case of bearing constraints.

In order to define and understand rigidity we need to introduce the concept of framework (or formation). A framework is identified, in the case of distanceconstraints in 2D, as a pair (G, p), where p = stack (p 1 , ..., p N ), p i ∈ R 2 is the position of the i-th robot and N is the total number of robots of the formation. In the previous Sect. 3.3.1 we gave some details about connectivity. Rigidity is a stronger property and it is possible to prove that if a framework is rigid, the underlying graph is also connected [START_REF] Zelazo | Rigidity maintenance control for multi-robot systems[END_REF], while the vice versa is not true. Roughly speaking, a framework (G, p) is rigid if satisfying the distance constraints associated with the edges of G univocally determines the shape (spatial arrangement of the positions p) of all the agents of the formation up to a roto-translation. An example to understand better this concept is reported in Fig. 3.6.

Another intuitive way to define rigidity is the following. A framework (G, p) is rigid if the only motions satisfying all the constraints over the edges in E are those which would be allowed in case G was the complete graph K. 6 This leads us to a great advantage of rigid graphs which is related to the complexity of a hypothetical formation control algorithm associated with a group of agents with an underlying rigid topology. Shortly, one way of controlling N agents is by regulating all the distances among N agents to some desired values. This would yield to a complete graph K which has N (N -1) edges. This translates into a complexity of the formation control algorithm of O(N 2 ). Instead, if a graph is rigid in two dimensions, only 2N -3 edges7 are needed to control all the distances between the agents and therefore the complexity decreases to O(N ), i.e., linear versus quadratic.

As stated in [START_REF] Anderson | Rigid graph control architectures for autonomous formations[END_REF], if a graph is rigid for a certain set of positions, it will be rigid for almost all other positions. The adverb almost refers to some nongeneric sets of positions as the ones corresponding to collinear agents8 . This is also why in order to study rigidity we can employ two different approaches:

• Combinatorial approach: through Laman's theorem [START_REF] Laman | On graphs and rigidity of plane skeletal structures[END_REF]. This approach does not take into account the positions of the agents and is only function of the topology of G. Indeed, it relies on the previous concept whose idea is that if a formation is rigid in a set of positions, it will be rigid almost anywhere else.

• Algebraic approach: through linear algebra and the concept of rigidity matrix [START_REF] Tay | Generating isostatic frameworks[END_REF][START_REF] Whiteley | Some matroids from discrete applied geometry[END_REF]. This approach takes also into account the positions of the agents.

The theory of rigidity brings with it several definitions and theorems and therefore in the following part of this section, we briefly go through some of them, refer to [START_REF] Anderson | Rigid graph control architectures for autonomous formations[END_REF] and its bibliography for more details.

The Laman's theorem and some definitions for the case of distance constraints

In order to introduce the Laman's theorem we need the concept of induced subgraph of a graph G = (V, E) [START_REF] Anderson | Rigid graph control architectures for autonomous formations[END_REF]. Let V be a subset of V, then the subgraph of G induced by V is the graph G = (V , E ) where E includes all the edges of E that are incident on a vertex pair in V .

The Laman's theorem states the following: a graph G = (V, E) modeling a formation in two dimensions with |V| vertexes and |E| edges is rigid if and only if there exists a subgraph G = (V, E ) with 2|V| -3 edges such that for every subgraph

V of V, the induced subgraph G = (V , E ) of G has a number of edges |E | which is: |E | ≤ 2|V | -3.
Laman's theorem provides a test, applicable to graphs in two dimensions and for the case of distance constraints, to check whether a graph is rigid. This is why rigid graphs with a number of edges equal to 2|V| -3 are sometimes called Laman graphs. 9A theorem for graphs in three dimensions does not exist but a partial extension of the Laman's theorem to three dimensions is discussed in [START_REF] Anderson | Rigid graph control architectures for autonomous formations[END_REF] and it requires the graph G of the considered framework G = (V, E) to have 3|V| -6 edges.

Another important concept in the theory of rigidity is the one of minimal rigidity [START_REF] Tay | Generating isostatic frameworks[END_REF]. Intuitively, a framework (G, p) is minimally rigid if removing any of its edges will cause the loss of rigidity, see Fig. 3.7. Furthermore, we can refer to rigid graphs which are rigid but not minimally rigid as redundantly rigid graphs [START_REF] Williams | Redundantly rigid topologies in decentralized multi-agent networks[END_REF], and an edge is called a redundant edge if the graph remains rigid after its removal [START_REF] Eren | Combinatorial measures of rigidity in wireless sensor and robot networks[END_REF]. Minimally rigid graphs have been described in details in several works such as [START_REF] Krick | Stabilisation of infinitesimally rigid formations of multi-robot networks[END_REF]. A procedure to build minimally rigid graphs in two dimensions is represented by the so-called Henneberg construction [START_REF] Tay | Generating isostatic frameworks[END_REF][START_REF] Henneberg | Die graphische Statik der starren Systeme[END_REF][START_REF] Bereg | Certifying and constructing minimally rigid graphs in the plane[END_REF]. As stated in [START_REF] Anderson | Rigid graph control architectures for autonomous formations[END_REF], for graphs in three dimensions there exist some operations that are analogous to the Henneberg construction, but they are in the form of conjectures [START_REF] Tay | Generating isostatic frameworks[END_REF] and it is therefore not sure that they represent necessary and sufficient conditions to build and deconstruct all minimally rigid graphs.

There are several and equivalent formal definitions of rigidity, for example in [START_REF] Krick | Stabilisation of infinitesimally rigid formations of multi-robot networks[END_REF][START_REF] Asimow | The Rigidity of Graphs, II[END_REF] the following definition is given. Let us define a rigidity function associated with the framework (G, p) as the function g G : R 2|V| → R |E| given by g G (p) := stack(..., ||p kp j || 2 , ...).

(3.9)

The ith component of g G (p) corresponds to the value of the geometric constraint (distance in this case) over the edge e i ∈ E. After defining the rigidity function it is possible to give the following definition. A framework (G, p) is rigid if there exists a neighborhood U ⊂ R 2|V| of p such that

g -1 G (g G (p)) ∩ U = g -1 K (g K (p)) ∩ U (3.10)
where K is the complete graph with the same vertices as G. The definition (3.10) asserts that, in a neighborhood U of p, the possible shapes corresponding to the values of the constraints over the edges E in G are the same that one would obtain by considering the edges of the complete graph K. Moreover, if this is true for all positions p, and not only for a neighborhood of p, the framework is globally rigid and therefore the following definition.

A framework (G, p) is globally rigid if

g -1 G (g G (p)) = g -1 K (g K (p)) (3.11)
for all possible values of p. Global rigidity, as it comes out of the last definition, is a property which is stronger than rigidity. Moreover, in [START_REF] Eren | Combinatorial measures of rigidity in wireless sensor and robot networks[END_REF] it is shown that redundant rigidity is a necessary (but not sufficient) condition for global rigidity. A typical example to understand the difference between a rigid and a globally rigid graph is reported in Fig. 3.8. 10 Strictly speaking, it is important to highlight that rigidity (minimal or not) does not uniquely define the shape of a formation but it ensures that if the formation assumes a specific shape, it will not be able to deform from that shape smoothly. On the other hand, global rigidity ensures that, if the formation satisfies a set of distance constraints, the shape is univocally (globally) defined.

In order to have a better understanding of the definition of rigidity in (3.10) we can refer to Fig. 3.9 which depicts how the rigidity function works in the case of a rigid graph. Let us assume that we have a certain set p of positions of the agents. If we apply the function g G to p, from the set of R 2|V| we will go to the set of R |E| , specifically we will end up in g G ( p), that is, the value of the distances over the edges for the particular configuration p. following concept. For a rigid graph, the set that one gets from the counter-image g -1 G ( p) (the shaded area) is the same set that one would get by replacing G with K, at least in a neighborhood U p of p. On the other hand, in the case of global rigidity this holds for all p (not just in a neighborhood of p).

The distance-rigidity matrix

As pointed out above, an alternative approach to the combinatorial one provided by the Laman's theorem for the study the rigidity of a formation is the algebraic one based on the Rigidity Matrix.

Again, let us consider a framework (G, p) where p is the stack of all the positions p i ∈ R 2 of the agents. We can define a rigidity function, which is slightly different from (3.9)

g G (p) := 1 2 stack(..., ||p k -p j || 2 , ...). (3.12)
The rigidity matrix is defined as the Jacobian J g G of the rigidity function with respect to the positions of the agents. This matrix is useful to study some concepts of graph rigidity which would be impossible to study only through combinatorial approaches (refer to [START_REF] Anderson | Rigid graph control architectures for autonomous formations[END_REF][START_REF] Krick | Stabilisation of infinitesimally rigid formations of multi-robot networks[END_REF] and related works for more details). Thanks to the introduction of the rigidity matrix, and therefore within the algebraic approach, one can study the so-called infinitesimal rigidity. The idea of infinitesimal rigidity plays a central role in the design of decentralized formation controllers and localization algorithms, both in the case of distance constraints and in the one of bearing ones.

Infinitesimal rigidity has the goal of studying the flexibility of a framework under instantaneous (infinitesimal) motions of the agents. It is discussed, with respect to distance constraints, in [START_REF] Krick | Stabilisation of infinitesimally rigid formations of multi-robot networks[END_REF].

The idea of infinitesimal rigidity is to move infinitesimally the agents while keeping the rigidity function (3.12) constant up to the first order. Let δp be an infinitesimal motion of the framework (G, p). Then the Taylor expansion of g G about p is

g G (p + δp) = g G (p) + J g G δp + h.o.t.,
therefore the rigidity matrix can be also seen as the linear term in the Taylor expansion of the rigidity function. The rigidity function stays constant up to first order when J g G δp is null, which is when δp is in the null-space of the rigidity matrix.

In [START_REF] Asimow | The Rigidity of Graphs, II[END_REF] it is stated: a framework with N agents (G, p) is infinitesimally rigid in the plane if dim(Ker(J g G )) = 3, or equivalently

rank(J g G ) = 2N -3 (3.13)
Note that infinitesimal rigidity implies rigidity but the vice versa is not true. In order to understand better these concepts an example of infinitesimally and not infinitesimally rigid graph is reported in Fig. 3.10. Note that the two graphs are both rigid and globally rigid. In particular the second graph (Fig. 3.10(b)) is rigid because of the Laman's theorem. Indeed the graph has three edges which is equal to 2|V| -3. The same graph is not rigid because if one would build the rigidity matrix, in the specific configuration of Fig. 3.10(b), it would be possible to check that the rank of the matrix is equal to two (instead of three) and therefore (3.13) would not be satisfied. Geometrically, this (point-wise) loss of rank is due to the fact that, at first order, agents 1 and 3 can infinitesimally move orthogonally to the edge (1, 3) without changing their distance, thus (again infinitesimally) deforming the shape of the formation. Finally, bringing together the definitions of infinitesimal and minimal rigidity one can define a minimally infinitesimally rigid framework as one which is both infinitesimally and minimally rigid.

In the case of distance constraints on the plane, if the rank of the rigidity matrix is 2|V| -3 (or equivalently 2N -3) its null-space will be of dimension 3. This, in two dimensions for distance constraints, makes sense because it means that in the null-space of the rigidity matrix there are only three vectors which correspond to agent velocities which do not change the distance constraints. These vectors correspond to the planar translation (two vectors) and planar rotation (one vector) of the whole framework in two dimensions.

In [START_REF] Anderson | UAV formation control: Theory and application[END_REF] it is also specified that the result in (3.13) extends easily to graphs in three dimensions. In this case, it is possible to prove that a framework is rigid if and only if the rigidity matrix has a rank of 3|V| -6. This means that the null-space has dimension 6 and its vectors correspond to three translations and three rotations of the whole framework in three dimensions.

We said several times that rigidity plays an essential role in solving the problem of formation control. Indeed, in [START_REF] Krick | Stabilisation of infinitesimally rigid formations of multi-robot networks[END_REF], after defining the rigidity matrix they design a gradient-based control law based on this matrix. Thanks to the gradient-like structure of the controller and on the intrinsic decentralized structure of the rigidity matrix (each column corresponds to an agent and contains information which is relative only to the neighbor of the considered agent) the result is a controller which has a decentralized structure (i.e., the controller for each agent relies just on quantities dependent on itself or its neighbors). Furthermore, in [START_REF] Krick | Stabilisation of infinitesimally rigid formations of multi-robot networks[END_REF] they prove that infinitesimal rigidity is a sufficient condition for local asymptotic stability of a gradient-based controller, for multi-vehicle systems, which uses only local information. 11Note that the choice of a controller based on the rigidity matrix and which is gradient-based is not casual. Indeed, if instead of a gradient-based method, it would have been chosen, e.g., a Gauss-Newton approach the controller would end up being dependent not simply on the rigidity matrix J g G (or more precisely on its transpose) but on the so-called pseudoinverse of the rigidity matrix which can be defined as

J † g G = J T g G J g G -1
J T g G . Without going too much into the details, the inverse operation of a matrix in some way mixes the elements of the matrix to which is applied. Then, because the pseudoinverse uses the inverse operation, designing a controller built on this matrix would yield a loss of its decentralized structure. However, gradient-based methods have also some drawbacks. One of them is the speed of convergence which is one of the worse among the optimization methods. The discussion about this topic is not within the scope of this Thesis but interesting details can be found in [START_REF] Calafiore | A distributed gradient method for localization of formations using relative range measurements[END_REF], where they try to speed up a gradient-based method by choosing a better, decentralized, stepsize.

Thanks to the introduction of the rigidity matrix we can understand a difference between connectivity and infinitesimal rigidity. Connectivity and rigidity are both global properties of the whole group of agents to which the considered graph is associated. However, connectivity is associated to a graph while infinitesimal rigidity takes into account also the particular configuration of the robots. Note that our algorithms rely on the concept of infinitesimal rigidity (for bearing constraints). 12This property becomes evident by looking at two rigidity matrices corresponding to a group of agents in two dimensions, always in the case of distance constraints. The matrix in (3.14) corresponds to a graph like the one in Fig. 3.11(a) which is not rigid and indeed it has a rank which is less than 2|V| -3. Instead, the following matrix corresponds to a rigid graph like the one in Fig. 3.

      x 1 -x 2 y 1 -y 2 x 2 -x 1 y 2 -y 1 0 0 0 0 0 0 x 2 -x 3 y 2 -y 3 x 3 -x 2 y 3 -y 2 0 0 0 0 0 0 x 3 -x 4 y 3 -y 4 x 4 -x 3 y 4 -y 3 x 1 -x 4 y 1 -y 4 0 0 0 0 x 4 -x 1 y 4 -y 1       (3.14) (a) (b)
11(b)            x 1 -x 2 y 1 -y 2 x 2 -x 1 y 2 -y 1 0 0 0 0 0 0 x 2 -x 3 y 2 -y 3 x 3 -x 2 y 3 -y 2 0 0 0 0 0 0 x 3 -x 4 y 3 -y 4 x 4 -x 3 y 4 -y 3 x 1 -x 4 y 1 -y 4 0 0 0 0 x 4 -x 1 y 4 -y 1 x 1 -x 3 y 1 -y 3 0 0 x 3 -x 1 y 3 -y 1 0 0 0 0 x 2 -x 4 y 2 -y 4 0 0 x 4 -x 2 y 4 -y 2            (3.15)
The matrix in (3.15) seems to have a rank always greater or equal than 2|V| -3.

But, if we look closer it is possible to understand that some terms of (3.15) can become equal to zero even if the underlying graph contains the edge corresponding to that element. This happens, for example, when agent 1 has the same x-coordinate (or y-coordinate) of agent 2 and, in general, when two or more agents are collinear or coincident with each other. In this case the graph would be rigid but not infinitesimally rigid, as for the example in Fig. 3.10(b). This leads to the conclusion that infinitesimal rigidity is a stronger property than rigidity. Indeed, global rigidity can be seen as a sort of rigidity which ensures that the positions of the robot are nondegenerate (e.g., not all aligned).

In this section, we went through the primary definitions and results about the theory of rigidity focusing our attention on the case of distance constraints. We concentrated on distance constraints for two main reasons. The first one is a historical reason: distance constraints, because of their analogy with rigidity in mechanical systems, were the first ones to be tackled and investigated by the multi-robot community. The second reason is that we simply believe that this case is more intuitive than the bearing one.

The case of bearing constraints

However, since some years there has been an interest also in applying the theory of rigidity to scenarios with bearing constraints [START_REF] Bishop | Stabilization of rigid formations with direction-only constraints[END_REF][START_REF] Eren | Using angle of arrival (bearing) information for localization in robot networks[END_REF][START_REF] Franchi | Decentralized control of parallel rigid formations with direction constraints and bearing measurements[END_REF][START_REF] Cornejo | Scale-free coordinates for multi-robot systems with bearing-only sensors[END_REF][START_REF] Zhao | Bearing rigidity and almost global bearing-only formation stabilization[END_REF][START_REF] Eren | Using angle of arrival (bearing) information in network localization[END_REF][START_REF] Eren | Formation shape control based on bearing rigidity[END_REF]. This happened mainly because bearing measurements are often retrievable from cheaper and more accessible sensors, such as vision-based sensors (e.g., cameras) and angleof-arrival sensor (e.g., wireless network devices 13 ). Moreover, a formation based on angles can be scaled up easier than one based only on distances. A difference between distance-rigidity and bearing-rigidity is that the rigid body motions in the second case also include a contraction/expansion of the whole frame along with the usual roto-translation. Indeed, only with bearing measurements there is no control over the scale of the formation and therefore the size of the desired shape cannot be controlled. This is one of the reasons why Chapt. 7 deals with the scale estimation through bearing measurements and known agent ego-motion (body-frame linear/angular velocity). Contrary to distance measurements (which are scalar quantities), bearing measurements are vector quantities. Therefore, one has to consider in which frame these measurements are expressed.

In the literature, two main cases have been considered: all bearing measurements expressed in a common (world) frame [START_REF] Bishop | Stabilization of rigid formations with direction-only constraints[END_REF][START_REF] Franchi | Decentralized control of parallel rigid formations with direction constraints and bearing measurements[END_REF], and all bearing measurements expressed in the local body frames of the agents [START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF][START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF][START_REF] Tron | A distributed optimization framework for localization and formation control[END_REF]. This Thesis, as stated several times, lies in the second group because this is the kind of measurement retrievable with a monocular camera. A third category could be represented by the works which analyze both cases, such as [START_REF] Zhao | Bearing rigidity and almost global bearing-only formation stabilization[END_REF] in which there is an extension of a bearing-based formation control from two to arbitrary dimensions. In [START_REF] Zhao | Bearing rigidity and almost global bearing-only formation stabilization[END_REF] there is also the study of some of the connections between distance and bearing rigidity showing that a framework in R 2 is infinitesimally bearing-rigid if and only if is also infinitesimally distance-rigid 14 .

In the case of a bearing measurement β ij (between the agents i and j) expressed with respect to a common reference frame, the vector β ij is the 3D unit-norm vector expressed as

β ij = p j -p i p j -p i = p ij d ij ∈ S 2 , (3.16)
while in the second case of measurements expressed in the body-frames, the bearing is expressed as

β ij = R T i p j -p i p j -p i = R T i p ij d ij ∈ S 2 , (3.17)
where p ij = p jp i , d ij = p jp i and R i is a rotation matrix that rotates a vector from the world frame to the body frame. Regarding the second case, more details are given in Sect. 4.2 while in this section we will continue to focus on the first case where the considered framework for bearing rigidity is still the pair (G, p).

Note that bearing rigidity is often addressed as parallel rigidity. This is due to an alternative but equivalent definition of rigidity given by introducing the parallelism between vectors (as done in [START_REF] Zhao | Bearing-constrained formation control using bearing measurements[END_REF]). Let us define the following orthogonal projector matrix for any non-null vector x ∈ R d (d ≥ 2) as

P x := I d - x x x T x . (3.18)
P x projects the vector x onto the orthogonal complement of x. Thanks to the definition of this projector matrix we can define two vectors x, y ∈ R d as parallel to each other if and only if P x y = 0 or P y x = 0.

Two frameworks (G, p) and (G, p ) are bearing equivalent if

P (p i -p j ) p i -p j = 0 ∀e i ∈ E.
Two frameworks (G, p) and (G, p ) are bearing congruent if

P (p i -p j ) p i -p j = 0 ∀e i ∈ K.
Therefore, it is possible to give an alternative definition, to the ones given in Sect. 3.4.1, of rigidity and global rigidity.

A framework (G, p) is bearing/parallel rigid if there exists a constant > 0 such that any framework (G, p ) that is bearing equivalent to (G, p), and satisfies pp < is also bearing congruent to (G, p).

A framework (G, p) is globally bearing/parallel rigid if every framework which is bearing equivalent to (G, p) is also bearing congruent to (G, p).

Similarity between the rigidity matrix and the Jacobian of robotic manipulators

To make the understanding of the rigidity matrix easier, in this section we want to point out to the reader a useful similarity between the rigidity matrix and the well-known Jacobian in the case of robotic manipulators. A typical task in robotics consists in reaching a certain desired position p d of the end effector. Therefore the classical task could be written as p(q) → p d . Where p is the position of the end-effector of the considered robot and q is a vector which contains all the joints variables. In this case, one would differentiate the p and have the well-known equation

ṗ = J (q) q (3.19)
where the matrix J is the Jacobian associated to the manipulator and it contains the dynamics of the robot.

In our case, the task is to bring the robots from a certain configuration, with certain bearings β * ij , to a desired one which will have β d ij . In the same way, in the case of distances, the task would be to steer the agents from a configuration with some distances d * ij to a desired configuration with d d ij . The dual equation of (3.19) in our case is, using the same notation used so far:

ġG = J g G ṗ (3.20)
where J g G is the Jacobian of the function g G and therefore the rigidity matrix.

In this chapter, the theory of rigidity has been addressed to give the reader an introduction to this vast topic. In the next Chapt. 4 we will specialize the theory of rigidity for the case of bearing rigidity in R 3 × S 1 (which is the case considered in this Thesis) also giving an explicit expression of the bearing rigidity matrix which has a fundamental role in the whole Thesis.

Part II

Contributions

Chapter 4

Main modeling assumptions

I

N this chapter we bring together the main modeling assumptions and definitions which are common to the chapters of this part of the Thesis. More specifically, Chapts. 5 and 6 share the same modeling assumptions listed in this chapter while in Chapt. 7 there are some differences which are specifically addressed and explained in details.

Agent model

As we said several times, in this Thesis we want to be able to control a group of N quadrotor UAVs in an unstructured environment only relying on their onboard capabilities. To fulfill this task, the robots are required to be able to localize themselves with respect to other agents of the formation. The literature shows how this is possible relying on different sensing capabilities and on the capability of communication among the agents (e.g., by exchanging data over a radio communication channel). From a theoretical point of view, the most important aspect of these different sensing modalities is the quantity of information they provide. For instance, the sensors could provide estimates of distance (e.g., from wireless signal strength), bearing directions (e.g., with monocular cameras), translations (e.g., with stereo or depth cameras), or both rotations and translations (e.g., using cameras and two-view Structure from Motion [START_REF] Ma | An invitation to 3-D vision: from images to geometric models[END_REF][START_REF] Hartley | Multiple View Geometry in Computer Vision[END_REF]). In this Thesis, we assume that each agent can measure the direction (but not the distance) of a subset of neighboring agents in its reference frame1 . This setup is arguably the most practical with today's most popular hardware, which, due to weight, cost, and power consumption considerations, is usually limited to an Inertial Measurement Unit (IMU) and a monocular camera [START_REF] Jones | Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[END_REF][START_REF] Scaramuzza | Visioncontrolled micro flying robots: from system design to autonomous navigation and mapping in GPS-denied environments[END_REF].

Let W : {O W , X W , Y W , Z W } represent a world frame with Z W aligned with the vertical (gravity) direction and A i : {O A i , X A i , Y A i , Z A i } the i-th body frame attached to each quadrotor UAV. Following the modeling assumptions of [START_REF] Schiano | A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs[END_REF][START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF][START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF][START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF], we consider a group of N 'velocity-controlled' quadrotor UAVs with agent dynamics

ṗi ψi = R i 0 0 1 u i w i (4.1)
where p i ∈ R 3 is the quadrotor 3D position in W, ψ i ∈ S 1 the yaw angle about Z W , and

R i = R z (ψ i ) ∈ SO(3)
is the canonical rotation around the world z-axis. The quantities u i ∈ R 3 and w i ∈ R represent the body-frame linear velocity and yaw rate which are assumed to be known and controllable. The configuration of the i-th quadrotor is then denoted with

q i = (p i , ψ i ) ∈ R 3 ×S 1 , while q = (p, ψ) ∈ (R 3 ×S 1 ) N
is the configuration of the whole formation, with p = (p 1 , . . . , p N ) ∈ R 3N and ψ = (ψ 1 , . . . , ψ N ) ∈ S N . We stress that, as in [START_REF] Schiano | A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs[END_REF][START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF][START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF][START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF], the (absolute) yaw angle ψ i is not considered as an available quantity to the i-th quadrotor. Indeed, additional sensors, such as compasses, would be needed for obtaining a consistent yaw measurement for all the UAVs of the group with, however, a typically limited reliability (e.g., compasses would fail to operate soundly indoor or close to strong magnetic fields). As a consequence, the N quadrotors are not assumed to share, as a group, a common (global) reference frame where to express local measurements and control inputs. The reader is referred to Figs. 4.1 and 4.2 for a better understanding of the introduced quantities. In the first one, for simplicity, the origin of the body frame of the quadrotor O A i coincides with the origin of the world frame O W while in the second one a group of three quadrotor UAVs is illustrated.

Each quadrotor is also assumed equipped with an onboard calibrated camera that allows an agent i to measure the relative bearing vector with respect to an agent j in visibility, i.e., the 3D unit-norm vector

β ij = R T i p j -p i p j -p i = R T i p ij d ij ∈ S 2 , (4.2)
where p ij = p jp i , d ij = p jp i and S 2 is the unit sphere, i.e. the space of 3-D unit-norm vectors defined as

S 2 = {v : v ∈ R 3 , v = 1} (4.3)
This agent relative bearing β ij can be retrieved by 'derotating' the actual bearing measurement among quadrotors i and j (e.g., from an onboard camera) by the roll and pitch angles which can be typically measured by exploiting the built-in IMU, see also [START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF]. Indeed, by exploiting the known roll/pitch angles (φ A i , θ A i ) the unit vector β ij can be obtained from the measured

β A i ,A j as β ij = R y (θ A i )R x (φ A i )β A i ,A j
and can, thus, be treated as a quantity available to agent i.

The control strategies discussed in the next Chapts. 5 and 6 are based on model (4.1) and assume that each agent i can measure the (body-frame) bearing vector β ij with respect to other neighboring agents together with its own (bodyframe) velocity commands (u i , w i ) [START_REF] Schwager | Eyes in the sky: Decentralized control for the deployment of robotic camera networks[END_REF][START_REF] Fink | Planning and control for cooperative manipulation and transportation with aerial robots[END_REF]. Instead, Chapt. 7 considers a more general model which takes into account a rotation matrix which is a function of the full orientation of the agent and not only of the yaw angle ψ i described above. Moreover, with special focus on the quadrotor platform, we assume that the UAV is able (through a low-level controller such as, for instance, the one proposed in [START_REF] Lee | Geometric tracking control of a quadrotor UAV on SE(3)[END_REF]) to track a smooth reference trajectory (p i (t) , w i (t)) in the four-dimensional space

R 3 × S 1 .

Directed bearing rigidity in

R 3 × S 1
In this section there is a specialization of the theory of rigidity, explained in Sect. 3.4, for the case of bearing constraints. We also give relevant definitions and properties of directed bearing formations and bearing rigidity in R 3 × S 1 which are useful in the next chapters. Many of the introduced concepts are an extension of the SE(2) case treated in [START_REF] Schiano | A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs[END_REF][START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF] to which the interested reader is referred for full details.

Let G = (V, E) be a directed graph, where V = {1 . . . N } is the vertex set and E ⊆ V × V the edge set [START_REF] Godsil | Algebraic Graph Theory[END_REF]. Presence of an edge e k = (i, j) in E represents the possibility for agent i to measure the relative bearing β ij (4.2) to agent j. Graph G is designed as directed as we do not require, in general, reciprocity of the relative bearing measurements. (i.e., agent i may measure agent j but not be measured by agent j). This way, the typical visibility constraints of onboard cameras due to, e.g., limited field of view or occluded line-of-sight, can be directly accommodated at the modeling stage. We assume, however, that agents i and j can communicate if either (i, j) ∈ E or (j, i) ∈ E (i.e., the communication graph is taken as the undirected counterpart of the directed sensing graph G). We let N i = {j ∈ V| (i, j) ∈ E} represent the usual set of neighbors of an agent i, and O i = {j ∈ V| (j, i) ∈ E} the set of agents j for which i is a neighbor2 . We also let 1 N and I N represent a vector of all ones and the identity matrix of dimension N , respectively.

A slightly different way of defining bearing rigidity, with respect to the definitions given in Sect. 3.4.3, is the one which involves the concepts of framework congruency and equivalence. In this case, the difference is that a framework (or also formation) [START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF][START_REF] Eren | Sensor and network topologies of formations with direction, bearing, and angle information between agents[END_REF] is the pair (G, q) where q follows the definition given in Sect. 4.2.

Two frameworks (G, q) and (G, q ) are denoted bearing equivalent if

β ij (q) = β ij (q ) ∀(i, j) ∈ E and bearing congruent if β ij (q) = β ij (q ) ∀ i, j ∈ V, i = j. A framework (G, q) is
defined bearing rigid (or simply rigid in the following) if there exists a neighborhood U of q such that any framework (G, q ), q ∈ U, that is bearing equivalent to (G, q) is also bearing congruent to (G, q). Also in this case, logically, bearing congruency implies bearing equivalence, but the opposite is not true (see Fig. 4.3). By extension, a framework is instead defined globally bearing rigid if U = (R 3 × S 1 ) N , i.e., if all frameworks which are bearing equivalent to (G, q) are also bearing congruent to (G, q). A non-rigid framework is also termed roto-flexible, and a framework (G, q) is said to be minimally rigid if (G, q) is rigid and the removal of any edge yields a roto-flexible framework.

As for the rigidity in the case of distance constraints, the notion of bearing rigidity can also be characterized from an infinitesimal perspective by introducing the so-called directed bearing function and corresponding directed bearing rigidity matrix. The directed bearing function (bearing function from now on) associated to a framework (G, q) is the map

β G (q) : (R 3 × S 1 ) N → (S 2 ) |E| β G (q) = stack β e 1 , . . . , β e |E| (4.4)
where the notation e i ∈ E is used to represent a directed edge in the graph G according to any chosen labeling. The world-frame (directed) bearing rigidity matrix (BRM) is the Jacobian of the bearing function with respect to the agent configuration q, that is, the matrix

B W G (q) = ∂β G (q) ∂q ∈ R 3|E|×4N . (4.5)
Let N (•) represent the span of the null-space of a matrix. A framework (G, q) is said to be infinitesimally bearing rigid at some point

q if N (B W G (q)) = N (B W K N (q)
), with K N being the complete directed graph. Otherwise a framework is said to be infinitesimally roto-flexible. Since it can be shown that dim N (B W K N (q)) = 5 (see, e.g., [START_REF] Schiano | A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs[END_REF][START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF]), it follows that a framework (G, q) in R 3 × S 1 is infinitesimally rigid if and only if the following is satisfied

rank(B W G (q)) = 4N -5. (4.6)
For infinitesimally rigid frameworks in R 3 × S 1 , the 5-dimensional null-space of the bearing rigidity matrix (BRM) is also well-understood: it corresponds to the three rigid-body translations, a dilation relative to a reference point, and a coordinated rotation about a vertical axis passing through a reference point [START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF][START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF]. Equivalently, we can say that the 5-dimensional null-space of matrix B W G (q) is spanned by those velocities q that do not change the bearing function β G (q) (i.e., the value of the measured bearing vectors).

The notion of bearing infinitesimally rigidity and, in particular, the properties of the bearing rigidity matrix and its null-space play a central role in the design of decentralized bearing formation controller and localization schemes. A role analogous to the (more common case of) distance-constrained infinitesimal rigidity and distanceconstrained rigidity matrix [START_REF] Calafiore | A distributed gradient method for localization of formations using relative range measurements[END_REF][START_REF] Krick | Stabilisation of infinitesimally rigid formations of multi-robot networks[END_REF][START_REF] Garcia De Marina | Controlling rigid formations of mobile agents under inconsistent measurements[END_REF][START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF][START_REF] Oh | Formation control of mobile agents based on interagent distance dynamics[END_REF]. Note that in [START_REF] Zhao | Bearing rigidity and almost global bearing-only formation stabilization[END_REF] there is a theorem that expresses a relationship between the three kinds of bearing rigidity which is summarized in Fig. 4.4. Extending the results of [START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF] to the case of frameworks in R 3 × S 1 , the k-th row block of the world-frame bearing rigidity matrix B W G in (4.5) associated to edge e k = (i, j) has expression

∂β ij ∂q =    -0-- P ij R T i d ij i -0- P ij R T i d ij j -0-. . . -Sβ ij 3N +i -0-    ∈ R 3×4N . (4.7)
Here, We then note that the bearing rigidity matrix B W G is a function of interdistances, relative bearings, and absolute yaw rotations. Indeed, the bearing rigidity matrix relates changes in the bearing function β G to the world-frame velocities q = ( ṗ, ψ) of the framework

P ij = I 3 -β ij β T ij is
βG = B W G (q) ṗ ψ . ( 4.8)
The world-frame rigidity matrix relates variations in the bearing function to the world-frame agent velocities ( ṗ, ψ). Exploiting (4.1), one can also define a body-frame bearing rigidity matrix B G (q) as

βG =B W G (q) diag(R i ) 0 0 I N u w = B W G T (ψ) u w = B G (q) u w , (4.9)
which explicits the dependance on the body-frame velocity inputs u = stack (. . . u i . . .) ∈ R 3N and w = stack (. . . w i . . .) ∈ R N , respectively the linear and angular velocities. The (3×4N ) k-th row block of B G (q) associated to an edge e k = (i, j) has expression

   -0-- P ij d ij i -0- P ij i R j d ij j -0--Sβ ij 3N +i -0-    , (4.10)
where

i R j = R z (ψ j -ψ i ) = R z (ψ ij ).
It is worth noting that the body-frame bearing rigidity (4.10) matrix is, again, a function of measured bearings β ij and interdistances d ij . However, contrary to the previous case, it is not a function of absolute yaw rotations ψ i , but, instead, of relative orientations ψ ij = ψ jψ i among neighboring agents. This fact will be important for the next developments.

Note that the same considerations we did for the world-frame bearing rigidity matrix about infinitesimal rigidity extend to the body-frame bearing rigidity matrix since rank(B G (q)) = rank(B W G (q)), being T (ψ) in (4.9) a square non-singular matrix.

Chapter 5

Formation control and localization in R 3 × S 1

I

n this chapter we consider the problem of controlling a formation of quadrotor UAVs equipped with onboard cameras able to measure relative bearings in their local body frames with respect to neighboring UAVs. The control goal of this work is twofold: (i) steering the agent group towards a formation defined in terms of desired bearings, and (ii) actuating the group motions in the 'null-space' of the current bearing formation. The proposed control strategy relies on an extension of the rigidity theory to the case of directed bearing frameworks in R 3 ×S 1 (see Sect. 4.3). This extension allows to devise a decentralized bearing controller which does not need presence of a common reference frame or of reciprocal bearing measurements for the agents.

Introduction

As stated in the previous chapters, the problem of formation control of multiple mobile robots has been extensively studied over the last decade in the robotics and control communities. The basic goal of most formation controllers is to coordinate a robot team in order to achieve some desired spatial arrangement. Most formation control schemes differ in their assumptions about, for example:

• the robot mobility: planar robots, aerial vehicles, possible presence of nonholonomic constraints or underactuation

• the employed sensing technology: local measurements from onboard sensors such as cameras, lidars, sonars, radio transmitters/receivers, or absolute/global measurement from centralized facilities as motion capture systems (mainly for indoor applications) and GNSS (for outdoor applications).

• the overall architecture: centralized versus decentralized coordination schemes.

A challenging scenario that still motivates considerable research efforts is that of decentralized formation control of mobile robots based only on local sensing, in which the robots are assumed to only be able to obtain relative measurements with respect to other robots in the group. Typical examples of relative sensing include range sensors (such as lidars and ultrasound) for retrieving inter-robot distances, or bearing sensors (such as cameras) for obtaining the bearing angle to other robots within visibility. The use of local sensing also entails the possible lack of a common reference frame to which all the individually collected measurements and control actions can be expressed, a limitation that poses additional challenges for solving the formation control problem [START_REF] Olfati-Saber | Consensus and cooperation in networked multi-agent systems[END_REF][START_REF] Cortés | Global and robust formation-shape stabilization of relative sensing networks[END_REF][START_REF] Montijano | Distributed formation control without a global reference frame[END_REF]. As we already mentioned, these scenarios are motivated by the goal of deploying highly autonomous multi-robot teams in 'non-trivial' environments (e.g., inside buildings, underwater, underground, or even in deep space). Usually, in these scenarios centralized sensing facilities, such as GNSS, are not available, and the robots can only rely on their 'local skills' (local sensing and computing power, and local communication with neighboring agents).

In Sect. 3.4 we explained that, in all these cases, the correct theoretical framework for analyzing and controlling the geometrical properties of robot formations defined in terms of relative measurements has proven to be the theory of formation rigidity [START_REF] Anderson | Rigid graph control architectures for autonomous formations[END_REF] described already in Chapt. 3.

Cameras are a widespread sensing technology which is often found onboard quadrotor UAVs mainly because of their low weight and limited power consumption [START_REF] Scaramuzza | Visioncontrolled micro flying robots: from system design to autonomous navigation and mapping in GPS-denied environments[END_REF]. It is then interesting to study how to design suitable vision-based cooperative control strategies to be exploited in more complex applications such as exploration, coverage, surveillance or target tracking.

Nevertheless, when addressing visual-based formation control in GNSS-denied environments, the assumptions of a common reference frame and of an undirected topology for the measurement graph can be hard to realize. For instance, the requirement of keeping constant mutual visibility among all robot pairs can easily become unfeasible because of the limited camera field of view, and likewise for the possible presence of a common reference frame shared by a group of robots that has only access to local relative measurements. It is then essential to investigate possible bearing formation control strategies that can relax as much as possible these assumptions.

In this respect, to the best of our knowledge, the only decentralized bearing formation controller that does not require the presence of a common reference frame and of an undirected sensing topology has been proposed in [START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF] by exploiting the (body-frame) measured bearings and a single distance measurement among an arbitrary pair of robots. However, the machinery presented in [START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF] requires a very special structure for the chosen measurement graph (the one in Fig. 5.1(a)) which, among others, must contain two special agents able to measure and to be measured by any other agent in the group. While instrumental, this choice is also unnecessarily overconstraining since any bearing rigid topology (with, in general, far less constraining requirements than the ones exploited in [START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF]), for example the graphs in Figs. Our contribution in this context has been the generalization of the ideas of [START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF]. Indeed, we proposed a fully decentralized bearing formation controller that only requires the presence of a generic (directed) bearing rigid topology. Furthermore, as in [START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF], the proposed control strategy is also complemented with the possibility of steering the quadrotor group along all the bearing-preserving motion directions: these can be shown to consist of a collective translation, an expansion with respect to a reference point, and a coordinated rotation relative to a vertical reference axis. This possibility is particularly useful when, for instance, we would need to collectively steer the quadrotor group for navigation or exploration purposes while maintaining the desired bearing formation optimized for the task at hand. The two goals discussed above of bearing formation stabilization and group collective steering are here achieved under a minimal number of assumptions compared to the existing literature. In particular:

• the quadrotors are only assumed able to collect bearing measurements (through onboard cameras) and to impose motion commands in their (local) body-frames (which do not need to be aligned or 'coordinated' in some special ways),

• the bearing measurements are not necessarily required to be reciprocal, and the resulting (directed) sensing topology has no special constraints (apart from the 'necessary condition' of yielding a rigid bearing formation),

• a single (but arbitrary) quadrotor pair is additionally assumed able to measure its inter-distance (needed to retrieve the correct scale of formation which would be, otherwise, unobservable without employing estimation strategies such as those described in Chapt. 7 or in [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF]). Note that, besides the intrinsic importance of knowing the scale of the formation, this quantity is then needed for correctly implementing the coordinated rotation in the null-space of the bearing rigidity matrix (BRM).

Finally, we also provide an experimental validation of the overall approach employing a group of four quadrotor UAVs and simulations with the help of Matlab/SIMULINK and the V-REP simulator.

Chapter overview

The rest of the chapter is organized as follows: Sect. 5.2 illustrates the proposed bearing formation control strategy for robots evolving in R 3 × S 1 , which can be representative of the quadrotor case. Subsequently, Sect. 5.3 reports the simulation results obtained by considering a group of quadrotor UAVs while Sect. 5.4 presents the experiments with real robots. Finally, Sect. 6.5 concludes the chapter and discusses some future directions.

Decentralized formation control

Consider a bearing rigid framework (G, q) in R 3 × S 1 consisting of N agents with dynamics (4.1). Let q d be a desired configuration such that (G, q d ) is bearing rigid, and let b

d G = β G (q d ) = (β d e 1 . . . β d e | E|
) be the corresponding desired value for the bearing function. Our goal is to design a decentralized bearing formation controller able to accomplish two distinct objectives.

A first objective is the bearing formation stabilization: by acting on the control inputs (u i , w i ), the controller should steer the N agents towards a configuration q * equivalent to q d , i.e., such that β G (q * ) = b d G . Because of the framework rigidity, equivalence will also imply congruency with q d . Hence, the fulfilment of this first objective will ensure that q(t) reaches the (correct) desired shape modulo a possible translation, vertical rotation, and scaling (i.e., the motions spanning the null-space of the bearing rigidity matrix).

A second independent objective, 'orthogonal' to the first one, is the possibility to steer the agent group along the motion directions that do not affect the bearing rigidity function (for not interfering with the previous formation control goal), i.e., such that β G (q(t)) = const. These are spanned by the null space of the rigidity matrix which, for a rigid framework in R 3 × S 1 , consists of the aforementioned collective translation, expansion and coordinated rotation.

Fulfillment of this second objective will then allow to, for instance, collectively steer the agent group while maintaining a desired bearing formation for navigation or exploration purposes.

Finally, the formation controller should be decentralized and only based on information locally available or communicated by 1-hop neighbors.

As explained above, a complete solution to this control problem has been already proposed in [START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF] by, however, exploiting the structure of a very special interaction graph G and a single distance measurement d ij = p ip j among an arbitrary pair of robots. Indeed, measurement of this distance allows to fix (without ambiguities) the scale of the formation, which is then needed for correctly implementing the coordinated rotation in the null-space of the bearing rigidity matrix. Coping with this requirement may be quite challenging from a sensing/technological point of view because of, e.g., mutual occlusions (for large quadrotor groups or in cluttered environments) and/or limited camera field of view (with respect to the last two problems the reader is referred to Chapt. 6).

Therefore, the goal of the rest of the section is to generalize the machinery presented in [START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF] to the case of a generic bearing rigid framework. This will be achieved by suitably combining and extending the results of [START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF] which have addressed, in a separated way, the problems of rigidity-based decentralized localization and control for bearing formations for frameworks in SE(2).

Rigidity-based control of bearing frameworks in R 3 × S 1

A first contribution of this work is the following.

Proposition 5.1. The null-space of the bearing rigidity matrix can be explicitly characterized as

N (B W G (q)) = span 1 N 3 0 , p 0 , p ⊥ 1 N = span {n 1 , n 2 n 3 } (5.1)
where 1 N is a vector of all ones of dimension N , 1 N 3 = 1 N ⊗ I 3 , p ⊥ = (I N ⊗ S)p, and ⊗ denotes the matrix Kronecker product.

Proof. The proof of Prop. 5.1 that vectors [1 T N 3 0 T ] T and [p T 0 T ] T belong to N (B W G ) can be found in [START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF]. The explicit expression for the last null-space vector in (5.1) (not present in [START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF]) can be shown as follows: consider the k-th element of

B W G p ⊥ 1 N
, which has the following expression

P ij R T i S(p j -p i ) d ij -Sβ ij = P ij R T i SR i R T i (p j -p i ) d ij -Sβ ij = P ij Sβ ij -Sβ ij = Sβ ij -Sβ ij = 0, (5.2)
where the properties R T i SR i = S and P ij Sβ ij = Sβ ij were used (the last one exploits the fact that Sβ ij ⊥ β ij ). Each null-space vector of this particular basis for N (B W G (q)) represents one of the coordinated motions discussed above, namely, three translations along the world axes, an expansion about O W , and a rotation about a vertical axis passing through O W .

Remember that, as pointed out in Sect. 4.3, it is also possible to define a bodyframe bearing rigidity matrix B G (q). Let now e F (q) = b d Gβ G (q) denote the bearing formation control error to be regulated to zero for solving the first objective (formation stabilization). As shown in [START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF], minimization of e F can be obtained by implementing the following scale-free controller based on the body-frame rigidity matrix,

u w = k c diag(d ij ) 0 0 I N B G (q) T b d G , k c > 0 (5.3)
that results in the i-th agent velocity command

         u i = -k c (i, j)∈E P ij β d ij + k c (j, i)∈E i R j P ji β d ji w i = k c (i, j)∈E β T ij Sβ d ij .
(5.4)

The reader is referred to [START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF] for an almost global stability proof for frameworks in SE(2) that can be directly extended to the case under consideration. Furthermore, the centroid p = 1 T N 3 p/N and 'scale' p T p of the formation can be shown to be invariant under the action of (5.4).

It is worth noting that controller (5.4) has a decentralized structure depending only on the interaction graph G and on relative quantities. In particular, it does not require knowledge of any distance measurement (from which the term scale-free), nor knowledge of any common reference frame shared by the agent group. However, controller (5.4) requires communication among agents since, if there exists an edge (j, i) ∈ E (i.e., an agent j is measuring agent i), agent i needs to receive the bearing measurement β ji and desired bearing β d ji from agent j (second term of u i ). Furthermore, controller (5.4) also needs access to the relative orientation i R j among neighboring pairs which is a quantity not available from direct measurements and here, again, bearing rigidity comes in handy. Indeed, if a framework is bearing rigid, all the relative orientations among agent pairs are univocally fixed by the existing inter-agent bearings constraints. Therefore, it is in principle conceivable to recover/estimate the relative rotations i R j by processing the measured inter-agent bearings.

This insight has, indeed, been exploited in [START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF][START_REF] Cornejo | Scale-free coordinates for multi-robot systems with bearing-only sensors[END_REF]. In [START_REF] Cornejo | Scale-free coordinates for multi-robot systems with bearing-only sensors[END_REF] the assumption of reciprocal measurements for all agent pairs (undirected sensing graph G) allows for an algebraic computation of all the needed relative orientations. An analogous solution is exploited in [START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF] which, instead, relies on a very special construction of the (directed) sensing graph G. The procedure of [START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF] cannot be, however, generalized to generic bearing rigid frameworks such as those considered in this venue. In order to cope with this problem, we now detail an extension of the localization algorithm introduced in [START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF] for obtaining a (decentralized) estimation of the relative orientations i R j in presence of a generic bearing rigid graph and of non-stationary agents.

Rigidity-based localization of time-varying bearing frameworks in

R 3 × S 1
The decentralized localization algorithm proposed in [START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF] allows the agent group to estimate their unscaled positions1 and orientations with respect to a common reference frame by only exploiting the available relative bearing measurements. The localization algorithm exploits the world-frame bearing rigidity matrix B W G (q).

Let q = ( p, ψ) be an estimation of the true q and define the bearing estimation error as e L (q, q) = β G (q)β G ( q). Assuming β G (q(t)) = const, minimization of e L can be obtained by this gradient descent based on the bearing rigidity matrix,

ṗ ψ = k e B W G ( q) T β G (q)
, k e > 0.

(5.5)

Under the action of (5.5), the estimation q(t) will converge towards a configuration equivalent to q. Bearing rigidity of the framework (G, q) will also imply congruency with q. Therefore, at convergence (e L = 0), the estimated q will reach a configuration such that p = s(

I N ⊗ R z ( ψ))p + 1 N ⊗ t ψ = ψ + 1 N ψ (5.6)
for an arbitrary translation t ∈ R 3 , rotation angle ψ ∈ S 1 and scaling factor s ∈ R + . Any neighboring pair can then replace the unknown i R j with the estimated îR j = R z ( ψjψi ) by exchanging the two estimates ( ψi , ψj ) over local communication.

The estimator (5.5) is fully decentralized and only requires the bearings in β G (q) as measured quantities. However, the estimator (5.5) also assumes β G (q(t)) = const while the inter-agent relative bearings will be in general time-varying under the action of controller (5.4)2 . Presence of a time-varying β G (q(t)) can clearly prevent convergence of the estimation error. This issue can be, however, addressed by adding to (5.5) the following feedforward term for taking into account the agent motion,

ṗ ψ = k e B W G ( q) T β G (q) + diag(R z ( ψi )) 0 0 I N u w .
(5.7)

Proposition 5.2. If the initial estimation error e L (t 0 ) is small enough and s = 1 then (5.7) will guarantee e L (t) → 0 in case of time-varying bearings β G (q(t)) = const.

Proof. The closed-loop dynamics of the estimation error is

ėL = B G (q) u w -B W G ( q) ṗ ψ = = -k e B W G ( q)B W G ( q) T β G (q) + (B G (q) -B G ( q)) u w .
(5.8)

The first term of (5.8) represents the (nominal) closed-loop dynamics of the constant bearing case, while the second term of (5.8) is a perturbation due to the agent motion.

Since the nominal closed-loop dynamics is asymptotically stable [START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF], one can resort to the theory of perturbed systems [START_REF] Khalil | Nonlinear Systems[END_REF] for analyzing the stability of (5.8). In particular, if the perturbation term can be shown to be vanishing with respect to the estimation error e L , one can conclude local stability of the overall system (5.8) under mild conditions. Consider the k-th row of the body-frame rigidity matrix (4.10): this depends on the quantities β ij , d ij and i R j . By inspection one can then verify that, when e L = 0 (i.e., when (5.6) holds), i R j = i Rj , β ij = βij and d ij = s dij .

Assuming s = 1 in (5.6) then results in e L → 0 =⇒ B G (q) -B G ( q) → 0 which concludes the proof.

A correct scale (s = 1) of the estimated formation q is then necessary in order to compensate for the effects of the agent motion in the estimation dynamics. It is wellknown that the formation scale cannot be retrieved from only bearing measurements without introducing the concepts of nonlinear observability of Chapt. 7 already or the ones illustrated in [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF]. Therefore, here we exploit the presence of the single pair of agents, indexed as ι and κ, which is assumed able to also measure its relative distance d ικ . One can then consider the following 'augmented' cost function,

1 2 (k e e T L e L + k d ( pT ικ pικ -d 2 ικ ) 2 ), k d > 0 (5.9)
meant to enforce the constraint pικ = pιpκ = d ικ in the estimated q. As shown in [START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF], minimization of (5.9) is obtained by complementing the update law (5.7) with the additional (decentralized) terms ∓k d ( pT ικ pικd 2 ικ ) pικ in the ι-th and κ-th entries of ṗ, respectively.

If a distance measurement is not available, the estimator (5.7) will not be able to recover the correct scale factor s, but it will still track changes in the formation scale thanks to the feedforward term. If the initial mismatch between actual and estimated scale is small enough, this is often enough for allowing (5.7) to still provide a consistent estimation of q. Sect. 5.3 will indeed show results in this sense, by comparing the control/estimation performance when including/not-including the single distance measurement d ικ .

Coordinated motions in the null-space of the bearing rigidity matrix

As a final step, we address the fulfilment of the second control objective, that is, the implementation of the null-space motions spanned by (5.1).

This can be achieved by realizing the world-frame velocity qs = n 1 ν + n 2 λ + n 3 w which imposes to the framework a common linear velocity ν ∈ R 3 , an expansion rate λ ∈ R about O W , and a coordinated rotation with angular speed w about a vertical axis passing through O W . The corresponding body-frame velocities (u s , w s ) to be added to the formation control inputs (u, w) in (5.3) are then

u s w s = diag(R T i ) 0 0 I N qs .
(5.10) While (5.10) realizes the second control objective, it is usually more interesting to implement an expansion rate and coordinated rotation about a specific point of interest attached to the formation itself, rather than about the (arbitrary) origin of the world frame O W . For instance, an often convenient choice is to implement these motions relative to the formation centroid p = 1 T N 3 p/N . This can be obtained by using as basis for N (B W G (q)) the set {n 1 , n 2n 1 p, n 3n 1 S p} which eventually results in the i-th agent velocity commands

u s i = R T i (ν + λ(p i -p) + wS(p i -p)) w s i = w . (5.11)
An actual implementation of (5.11) would require the (non-available) quantities (p i , ψ i , p): exploiting the estimator (5.7), each agent can replace the true (p i , ψ i ) with the estimated ( pi , ψi ). The corresponding average p = 1 T N 3 p/N can then be obtained by resorting to any distributed averaging filtering technique such as the well-known PI average consensus filter (PI-ACE) [START_REF] Freeman | Stability and convergence properties of dynamic average consensus estimators[END_REF]. Indeed, given a (time-varying) vector quantity x(t) ∈ R N with each component x i (t) locally available to agent i, the PI-ACE filter allows every agent to distributedly build an estimation converging to the average x(t) = N i=1 x i (t)/N with a tunable dynamics that can be made faster than the underlying dynamics of each agent in the system, see also [START_REF] Giordano | A passivitybased decentralized strategy for generalized connectivity maintenance[END_REF][START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF][START_REF] Yang | Decentralized estimation and control of graph connectivity for mobile sensor networks[END_REF] for some applications in the context of multi-robot distributed control.

It is worth noting that knowledge of the correct scale factor (s = 1) in the estimated q is not required for implementing the null-space motions associated to vectors n 1 and n 2 (translation and expansion), but it is instead required for correctly implementing the coordinated rotation associated to vector n 3 . Indeed, n 1 does not depend on p and n 2 is homogeneous in p, with its direction thus unaffected by any scaling of the agent positions. This is, however, not the case for vector n 3 which is not homogeneous in p. Similarly to the estimation case, if a distance measurement is not available for fixing the scale of q, the coordinated rotation will not be exactly implemented.

Discussion

We conclude by emphasizing that, as stated at the beginning of the section, the proposed control/estimation scheme does not require a special topology for the interaction graph (besides being bearing-rigid); the bearing controller (5.4), the localization algorithm (5.7), and the null-space motions (5.11) have the same (decentralized) expression for all agents only as a function of the measured bearings and body-frame linear/angular velocities. The only exception is the inclusion of the distance measurement d ικ which adds an additional control term to agents ι and κ. In this sense, we believe that the work presented in this chapter represents a significant generalization of the strategy reported in [START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF] which, to the best of our knowledge, is the closest related work to our setting, and relied on a much more constrained design of the agent group.

We also note that the correct formation scale could be retrieved without assuming the presence of an (additional) distance measurement d ικ (and, thus, presence of two 'special agents' in the group). Indeed, the unknown robot inter-distances could be estimated online by processing the measured inter-robot bearings and the (known) robot own motions similarly to what done in the context of scale estimation for point features [START_REF] De Luca | Feature depth observation for image-based visual servoing: Theory and experiments[END_REF][START_REF] Spica | Active Structure from Motion: Application to Point, Sphere and Cylinder[END_REF]. In this respect, a possible solution will be presented in Chapt. 7. A different approach has been instead followed by [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF] which proposes an active scale estimation strategy for bearing formations of quadrotor UAVs.

We finally wish to briefly discuss the practical implications of requiring directed (bearing) rigidity for the robot formation (which, as often stated, is an underlying necessary condition of the proposed machinery). As well-known, minimal rigidity requires presence of a |E| = O(N ) number of edges (i.e., of inter-robot measurements/constraints) in the framework vs. the quadratic complexity of the complete (directed) graph K N (for which the complexity would be N (N -1) = O(N 2 )). As illustration, Fig. 5.2 shows some possible minimal bearing rigid topologies for the case of N ∈ {3 . . . 6} agents for which |E| = 4, 6, 8, 10, respectively.

Simulation results

In this section we present some simulation results involving N = 6 quadrotor UAVs. The simulation is run by considering the full dynamics of 6 quadrotor UAVs simulated via the 3D physical simulator V-REP [START_REF] Rohmer | V-rep: a versatile and scalable robot simulation framework[END_REF], for more details about the software architecture the reader is referred to Appendix B. In this case, the robustness of the bearing controller is tested against the discrepancies between the nominal agent (4.1) and the actual quadrotor flight dynamics, as well as against noise and discretization in the measured bearings (which are sampled at 60 Hz for mimicking an actual onboard camera).

The initial configuration q(t 0 ) and estimated q(t 0 ) were generated by adding to the desired q d a uniformly distributed random perturbation of amplitude 1 m for the positions and 120 deg for the orientation. A graph G with |E| = 20 directed edges was then randomly generated under the constraint of guaranteeing bearing rigidity at q(t 0 ), q(t 0 ) and q d , and the following gains were used: k c = 1 in (5.4), k e = 5 in (5.7) and k d = 10 in (5.9). Finally, in all simulations the null-space velocity commands (5.11) were activated, for convenience of illustration, only after convergence to the desired formation b d G : the five coordinated motions were first actuated one at the time and then all together. Figs. 5.3 and 5.4 report the simulation results. In particular, the bottom Fig. 5. 3(a) shows the behavior of e F (t) (the bearing control error, solid blue line) and of e L (t) (the estimation error, solid red line), while the top Fig. 5.3(a) depicts the five null-space velocity commands (ν, λ, w) in (5.11) given by a human operator through a joypad. It is then possible to verify how (i) both the control and estimation bearing errors converge to zero (despite their initial large value and despite the presence of time-varying bearings β(q(t))) and how (ii) the implementation of the coordinated motions (5.11) has no disturbing effect on the bearing errors (as expected).

Let λ 6 (q) ≥ 0 represent the sixth smallest eigenvalue of the following square matrix, which we refer to as symmetric bearing rigidity matrix3

B W G (q) = B W G (q) T B W G (q).
(5.12)

Since for an infinitesimal rigid framework in R 3 × S 1 it holds rank(B W G (q)) = 4N -5, the quantity λ 6 (q) can be taken as a measure of the framework bearing rigidity. Indeed, λ 6 (q) > 0 if and only if (G, q) is infinitesimal rigid and λ 6 (q) = 0 otherwise. Fig. 5.3(b) then reports the behavior of λ C 6 (t) = λ 6 (q(t)) and λ E 6 (t) = λ 6 ( q(t)), that is, the rigidity measures for the 'control' framework (G, q) and the 'estimation' framework (G, q). One can then check how both frameworks remained rigid throughout the motion of the agents, thus confirming congruency between q and q d (correct agent formation), and between q and q (correct agent localization).

As an additional measure of the localization performance, we considered the quantity e ψ = I N -1 N 1 T N /N (ψ -ψ): this represents the disagreement between the orientation estimation error and its mean value, and it should vanish in presence of a correct localization4 as, indeed, reported in Fig. 5.4(b). A converging e ψ (t) then allows to correctly compute the missing terms i R j in the bearing controller (5.4). Finally, Fig. 5.4(b) depicts the behavior of the 'formation scale error' defined as e s (t) = p(t) -1 N ⊗ p(t)p(t) -1 N ⊗ p(t) which, again, converges to zero, as expected, thanks to the additional distance constraint in (5.9).

It is worth noting that the 'distortions' present in Fig. 5.4(b) are mainly due to the higher-order quadrotor dynamics neglected by model (4.1) which, roughly speaking, introduces an unmodeled lag between commanded and actual velocities. The proposed control strategy is nevertheless robust enough for coping with these model inaccuracies.

Experimental results

This section discusses the experiment conducted for validating the formation controller described in Sect. 5.2.

For the experiment illustrated in this section we used a group of four quadrotor of the ones described in Sect. B.2. The TeleKyb framework [START_REF] Grabe | The TeleKyb framework for a modular and extendible ros-based quadrotor control[END_REF] was used for The onboard low-level controller is the one described in [START_REF] Spica | An opensource hardware/software architecture for quadrotor uavs[END_REF] and it is in charge of letting the orientation of the quadrotor track a desired reference. The experiments were performed in our flying arena which is equipped with a Vicon motion capture system, employed for reconstructing the body-frame bearing measurements β ij that would have been obtained by an onboard camera (refer to Sect. B.2.2 for more details).

The reported experiment followed a pattern similar to the simulation results described in the previous section: (i) regulation towards a desired bearing formation, (ii) actuation of the null-space motions (5.11), (iii) regulation towards a different desired bearing formation, (iv) actuation of the null-space motions (5.11). Additionally, we implemented, at every 6 seconds, a random switch among all the possible rigid topologies for the sensing graph in order to show the robustness of our approach also against possible topology changes during motion. Fig. 5.6-5.7 report the results of the experiment. The UAV formation starts far from the desired configuration but, after about 20 seconds, the norm of the formation control error e F (t) drops below 4% of its initial value (Fig. 5.6(a)). On the other hand, convergence of the estimator error e L (t) is quite fast even though the initial estimated q(t 0 ) was generated by adding to the real q(t 0 ) a uniformly distributed random perturbation of amplitude 1.5 m for the positions p(t 0 ) and 80 deg for the orientations ψ(t 0 ). Convergence of the estimated q(t) towards a configuration congruent with q(t) (and with the correct scale) can also be appreciated in Fig. 5.7 where the orientation estimation error e ψ (t) and the formation scale error e s (t) are shown. One can then verify, again, how a consistent estimation of the orientations ψ and of the formation scale s could be obtained despite the (unavoidable) non-idealities present in any real implementation.

Finally, as stated before, the underlying graph G switches randomly at every 6 seconds across all the possible rigid topologies for the N = 4 quadrotors (in particular, we allowed switches among graphs with |E| ∈ {6, 7, 8, 9, 10, 11, 12}). Fig. 5.6(b) shows the behavior of the rigidity measures λ C 6 (t) and λ E 6 (t) which 'jump' at every 6 seconds (as expected) because of the topology switches. Nevertheless, these topology changes (and associated increases/decreases of the control/estimation framework rigidity) did not negatively affected the overall performance of the proposed bearing control strategy.

Conclusions

In this chapter we have considered the problem of devising a decentralized control strategy for controlling a group of quadrotor UAVs able to measure relative bearings in their own body frames. In particular, we considered two control objectives: (i) stabilization of the quadrotor formation towards a desired bearing configuration, and (ii) steering of the whole formation along the motion directions in the nullspace of the bearing rigidity matrix. To this end, a suitable R 3 × S 1 extension and combination of the SE(2) directed bearing rigidity control/localization algorithms introduced in [START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF] has been developed for the case of non-stationary agents, together with a full explicit characterization of the null-space of the bearing rigidity matrix. This allowed to devise a decentralized bearing controller able to meet the two control objectives without the need of a common reference frame for the agent group, nor the requirement of reciprocal bearing measurements (i.e., of an undirected measurement topology). Simulation and experimental results on real quadrotors have been proposed to illustrate the various features of the approach.

A fully-onboard implementation of the proposed ideas by equipping the UAVs with onboard cameras for retrieving relative bearings and estimating the quadrotor body-frame linear/angular velocities (u i , w i ) represents a fundamental future work of this work and, in general, of this Thesis. By doing this we will be able to free ourselves from the need of an external motion capture system. Refer to Chapt. 8 for more details and a future direction regarding this issue. Another logical consequence of this work is the extension of the ideas presented in [START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF] for dealing with the issue of bearing rigidity maintenance in the presence of sensor constraints. Some of these constraints are the limited field of view and range of the onboard cameras and the possible occlusions generated by different UAVs. A possible solution to this problem is given in Chapt. 6 and it would then allow the quadrotor group to flexibly navigate in cluttered environment under the (controlled) possibility of losing/gaining neighbors while ensuring a minimum level of bearing rigidity for the formation. Another interesting extension, as we pointed out several times, is the (decentralized) integration of Structure from Motion (SfM) schemes, such as [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF], able to recover online the missing scale information by processing the measured bearings and known agent motion (and, thus, avoiding the requirement of a special agent pair able to additionally measure its inter-distance). An alternative solution to this problem is presented in Chapt. 7.

Chapter 6

Rigidity maintenance

I

N the previous chapters we considered the concept of rigidity and its properties with respect to the problems of formation control and estimation when applied to a group of quadrotor UAVs. In this chapter of the Thesis we are concerned with a distributed technique which is able to maintain rigidity while moving a formation of quadrotor UAVs. We asked ourselves the question: what are the challenges to maintain bearing rigidity in a real-world scenario? The answer to this question is strictly related to the aforementioned concepts of bearing rigidity and therefore to the sensors used to retrieve the bearing measurements (e.g., monocular cameras) and to their limitations. Nevertheless, in this chapter we present the details of a strategy we adopted to maintain bearing rigidity of a formation of quadrotor UAVs. The goal is to maintain the rigidity during motion despite the presence of several sensing constraints. This strategy can be coupled with a formation controller like the one presented in Chapt. 5. This would allow to have a distributed control strategy which is able to reach a desired formation and/or steering in the 3D space, all this while coping with sensors limitations. The sensing constraints we explicitly took into account were:

• minimum and maximum range of the camera • limited field of view of the camera

• possible occlusions caused by the agents of the formation

To this end, a decentralized gradient-based control action was developed, based on a suitable degree of infinitesimal rigidity linked to the spectral properties of the bearing rigidity matrix. The approach was first validated in a simulation environment and then through real experiments employing five quadrotor UAVs.

Introduction

The use of local sensing (such as onboard cameras or range sensors), however, entails many challenges. For instance, in the absence of centralized aids, each robot is typically only able to collect measurements and impose control actions in its local body-frame: when exchanging information over communication, the group then faces the need of, e.g., agreeing over some common shared frame where to express any quantity of interest. Furthermore, local (onboard) sensing also forces to cope with any sensing limitation (such as limited range/field of view, or occluded visibility) that can prevent retrieving the needed measurements with respect to neighboring robots.

While a number of rigidity-based formation control/localization schemes has been proposed over the years, to the best of our knowledge only a few previous works (e.g., [START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF]) have considered the issue of maintaining/preserving rigidity of the formation (which is a global property like graph connectivity [START_REF] Fiedler | Algebraic connectivity of graphs[END_REF][START_REF] Sabattini | Decentralized connectivity maintenance for networked lagrangian dynamical systems[END_REF]) during the agent motion despite the possible presence of sensing constraints. Rigidity maintenance is a fundamental problem since, as explained, losing formation rigidity prevents convergence of any formation control/localization scheme run by the robot group.

In this respect, in this chapter we present a work which considers the problem of bearing rigidity maintenance for a robot group equipped with onboard monocular cameras able to measure relative bearings with respect to other robots in visibility. Indeed, cameras are a widespread sensor modality for mobile robots, and the problem of coordinating the motion/formation of a robot group from only camera (bearing) measurements has attracted large attention in the robotics and control community [77, 120-122, 125, 126, 162]. Cameras, however, also suffer from all the

Cooperative localization from bearing measurements

The modeling assumptions of this work are exactly the same of the previous Chapt. 5 and they are specified in Chapt. 4. Moreover, as suggested by (4.10), the implementation of bearing formation control algorithms for agents evolving in R 3 × S 1 requires the availability of the relative orientation i R j and, possibly, distance d ij among neighboring robots (see, e.g., [START_REF] Schiano | A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF][START_REF] Franchi | Modeling and control of UAV bearing-formations with bilateral highlevel steering[END_REF]). These quantities, in our context, are not assumed to be directly measurable. A possible workaround is to then estimate these (needed) quantities via some bearing-based cooperative localization scheme.

In this respect, in this chapter we use the decentralized localization algorithm for rigid frameworks in R 3 × S 1 presented in Sect. 5.2.2. Therefore, letting qi = ( pi , ψi ) represent the estimation of the true agent i configuration q i , we assume, from now on, that the agent group runs the localization algorithm (of Sect. 5.2.2), and treat qi as a sufficiently good approximation of the true q i (always up to a roto-translation of the whole framework).

Note that the localization algorithm presented in Sect. 5.2.2 is entirely decentralized and can cope with any time-varying bearing function β G (q(t)) (i.e., the agents do not need to remain stationary or move in 'special' ways) as long as the formation is infinitesimally bearing rigid. It does, however, require the availability of at least one inter-agent distance d ικ among an arbitrary agent pair (ι, κ) for fixing the scale of the formation (which, otherwise, would be unobservable only from bearing measurements). This (possibly single) inter-agent distance can be either considered as an additional measurement among a special agent pair equipped with a distance sensor, or it can be estimated online by processing the measured bearings and the known agent body-frame velocities (u i , w i ) as extensively explained in Chapt. 7 and [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF] (thus, without requiring, in this case, any additional sensing capability).

A bearing rigidity maintenance strategy

The goal of this section is to present an algorithm for bearing rigidity maintenance able to also take into account some typical limitations of the employed onboard sensors (cameras), that is, minimum/maximum range, limited field of view, and possible line-of-sight (visibility) occlusions due to the alignments of multiple robots in the camera field of view. Our strategy is inspired by the connectivity/(distance) rigidity maintenance controllers presented in [START_REF] Giordano | A passivitybased decentralized strategy for generalized connectivity maintenance[END_REF][START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF][START_REF] Yang | Decentralized estimation and control of graph connectivity for mobile sensor networks[END_REF] and can be summarized as follows: the sensing constraints affecting a pair of agents (i, j) are encoded in a suitable scalar weight1 w ij = w k associated to the edge e k = (i, j), with w k = 0 if the constraint is not satisfied, and 0 < w k ≤ 1 otherwise2 . Let W = diag(w k I 3 ) ∈ R 3|E|×3|E| be a diagonal matrix collecting all the |E| weights w k = w ij , and define the weighted body-frame rigidity matrix as W B G = W B W G T . Any weight w k → 0 will cause the corresponding k-th row block of W B G , associated to edge e k = (i, j), to vanish, potentially diminishing the rank of W B G (and, thus, possibly threatening bearing rigidity of the framework). The agents will then implement a gradient-based controller able to preserve infinitesimal bearing rigidity of the formation by ensuring fulfilment of the rank condition rank(W B W G ) = 4N -5 for the weighted bearing rigidity matrix. The next sections detail the various steps needed to implement this gradient control action.

Design of the inter-agent weights

We start addressing the design of the inter-agent weights w ij associated to each edge e k = (i, j) that will encode the sensing constraints among agent pairs. Since, as explained in Sect. 6.1, in this work we consider the three constraints (C1) minimum/maximum range, (C2) limited field of view and (C3) occluded visibility. The weights w ij will then be designed as the product of three individual terms

w ij = w R ij w F ij w V ij (6.1)
accounting for (C1)-(C3), respectively.

Minimum/maximum range

As first constraint, we consider that the employed cameras have a minimum/maximum range beyond which no detection of neighboring quadrotors is possible (refer to Fig. 6.1 for a real-world scenario associated to this particular constraint). In particular d min , d max represent respectively the minimum and maximum range of the camera and, obviously, d min < d max . The weight w R ij (d ij ) is then designed as a function that smoothly vanishes (with vanishing derivative) when d ij → d min and d ij → d max , and has a maximum at the midpoint between d min and d max . To this end, we choose the following function

w R ij (d ij ) =        0 d ij < d min 1 2 + 1 2 cos π 2d ij -(d min +dmax) dmax-d min d min ≤ d ij ≤ d max 0 d ij > d max (6.2)
for which a representative shape is shown in Fig. 6.2a.

Limited field of view

A second constraint is related to the limited field of view of the onboard cameras (refer to Fig. 6.3 for a real-world scenario associated to this particular constraint).

Let o C ∈ S 2 be the (constant and known) direction of the camera optical axis in the quadrotor body-frame and consider the scalar product

α ij = o T C β ij .
Clearly, α ij = 1 when β ij and o C are perfectly aligned, and α ij → -1 as the angle between β ij and o C increases. Letting -1 ≤ α min < 1 represent the camera field of view, we then design weight w F ij as the following function with a maximum for α max ≤ α ij ≤ 1 and smoothly vanishing with vanishing derivative for A representative shape for the (6.3) is shown in Fig. 6.2b.

α ij → α min w F ij (α ij ) =        0 α ij < α min 1 2 -1 2 cos π α ij -α min αmax-α min α min ≤ α ij ≤ α max 1 α ij > α max (6.3) (a) (b)

Occluded visibility

A final constraint considered in this work is that of possible occluded visibility because of two (or more) quadrotors (close to be) aligned in front of a camera (with, thus, the closest quadrotor occluding the visibility of the farthest ones). The design of the weight w V ij meant to encode this constraint requires some care and, thus, we proceed by steps (Fig. 6.4(a) gives the reader a better understanding of this constraint through a real-world scenario).

Let us first focus on the situation depicted in Fig. 6.5 in which an agent i is measuring exactly two agents j and k. Our goal is to define a function w V ijk that captures the possible occlusions of agent k (third subscript) on the edge (i, j) (first two subscripts), and, analogously, a function w V ikj capturing the occlusions of j on

i j k ⌘ ijk 1 g(⌘ ijk ) ijk ⌧ 0 (a) i j k ⌘ ijk ijk = 0 g(⌘ ijk ) g(⌘ ijk ) (b) i j k ⌘ ijk 1 g(⌘ ijk ) ijk 0 (c)
Figure 6.5 -Scheme which depicts the weights associated to the occluded visibility between a triple of agents.

the edge (i, k). To this end, let η ijk = β T ij β ik represent the (cosine of the) angle between the two bearing measurements of j and k by agent i, and δ ijk = d ikd ij the relative distance of k and j with respect to i. Note that η ijk = η ikj and δ ijk = -δ ikj .

Since, unlike the previous case, we now wish to penalize small angular displacements between j and k with respect to i (which could lead to a possible occlusion), we introduce the following penalty function

g(η ijk ) =        0 η ijk ≥ η max 1 2 -1 2 cos π η ijk -ηmax ηmax-η min η min ≤ η ijk ≤ η max 1 η ijk ≤ η min (6.4)
where -1 ≤ η min < η max ≤ 1 are suitable parameters representing the activation/deactivation of function g(•). Therefore, g(η ijk ) = 0 if agents j and k are close to be aligned from the vantage point of i (η ijk ≥ η max ), while g(η ijk ) = 1 if agents j and k are far from being aligned (η ijk ≤ η min ). Fig. 6.2c shows a representative plot of function g(η ijk ).

The weight w V ijk , associated to the edge (i, j), should be clearly based on the 'angular penalty' function g(η ijk ). However, weight w V ijk should also take into account the fact that, regardless of the value of η ijk , agent j can be occluded by agent k only if agent k is front of j (i.e., if δ ijk < 0), and symmetrically for the weight w V ikj associated to the edge (k, j). As depicted in Fig. 6.5, a possibility is to have:

1) w V ijk = g(η ijk ) and w V ikj = 1 if δ ijk 0: agent k is in front of j and, thus,
only the edge (i, j) is penalized by g(η ijk ) (Fig. 6.5(a));

2) w V ijk = 1 and w V ikj = g(η ijk ) if δ ijk 0: agent j is in front of k and, thus, only the edge (i, k) is penalized by g(η ijk ) (Fig. 6.5(c));

3) w V ijk = w V ikj = g(η ijk ) if δ ijk = 0: agents j and k are at the same distance from i and, therefore, the two edges (i, j) and (i, k) are equally penalized by g(η ijk ) (Fig. 6.5(b))

In order to obtain this goal we then introduce a second penalty function

h(δ ijk ) =        0 δ ijk ≤ 0 1 2 + 1 2 cos π δ ijk -δmax δmax 0 ≤ δ ijk ≤ δ max 1 δ max ≤ δ ijk (6.5)
where δ max > 0 is an activation parameter. Fig. 6.2d shows a representative plot of function h(δ ijk ). With this choice of h(δ ijk ), weight w V ijk can then be chosen as

w V ijk (δ ijk , η ijk ) = (1 -h(δ ijk ))g(η ijk ) + h(δ ijk ), (6.6)
and symmetrically for w V ikj (δ ikj , η ikj ). Because of (6.4-6.5), one can easily verify that the expression in (6.6) correctly realizes the above-mentioned requirements 1)-3) for weights w V ijk and w V ikj .

The last step is to generalize this procedure for producing the (cumulative) weight w V ij on the edge (i, j) to be plugged in (6.1). The previous w V ijk accounts for the occlusions on edge (i, j) by a single specific agent k ∈ N i /{j}. Clearly, if |N i | < 2 then one should have w V ij ≡ 1 (no occlusions are possible if agent i has less than two neighbors). If, on the other hand, |N i | ≥ 2 then, in order to take into account all the possible occlusions on edge (i, j) by any neighbor k ∈ N i /{j}, one can just take the product sequence

w V ij = k∈N i /{j} w V ijk (δ ijk , η ijk ).
(6.7)

The bearing rigidity eigenvalue

Having introduced the weighting machinery meant to encode the sensing constraints considered in this work, we now discuss a suitable measure of bearing rigidity that is exploited by the rigidity maintenance controller. We first consider the unweighted case (W = I 3|E| ) and then explicitly introduce the weights w ij in the design.

As discussed, a framework is infinitesimally bearing rigid if and only if rank(B W G (q)) = rank(B G (q)) = 4N -5. This rank condition can be translated into an equivalent spectral condition on the eigenvalues of two suitable corresponding matrices. To this end, define

B W G (q) = B W G (q) T B W G (q) and B G (q) = B T G (q)B G (q) as the 4N × 4N
world-frame and body-frame (weighted) symmetric rigidity matrices, respectively. These two matrices are similar3 since from (4.9) it follows

B G (q) = T T (ψ)B W G (q)T (ψ) (6.8)
and T (ψ) is orthonormal 4 . Letting λ i (A) represent the i-th smallest eigenvalue of a square symmetric matrix A, infinitesimal bearing rigidity then translates into the condition λ 6 (B W G (q)) = λ 6 (B G (q)) > 0 (where the similarity between B W G (q) and B G (q) has been used). Let then λ B 6 (q) represent the sixth smallest eigenvalue of the world/body-frame symmetric rigidity matrix: λ B 6 (q) is a natural measure of infinitesimal rigidity and is denoted from now on as the bearing rigidity eigenvalue. The purpose of this section is to detail the main properties and explicit expressions of λ B 6 (q) and of its gradient with respect to the i-th agent configuration q i , which is later used in Sect. 6.3.3.

A first observation is formalized by the following Proposition. Proposition 6.1. The bearing ridigity eigenvalue does not depend on the agent orientations ψ, i.e., λ B 6 (q) = λ B 6 (p).

Proof. The proof exploits the definition λ B 6 (q) = λ 6 (B W G (q)) based on the worldframe symmetric rigidity matrix. Let pij = p ij /d ij and Pij = I npij pT ij . Exploiting (4.2) and the identity SR i = R i S, it follows that

P ij R T i = (I N -β ij β T ij )R T i = R T i Pij , Sβ ij = R T i S pij (6.9) (6.10)
The k-th row block of B W G (q) in (4.7) associated to an edge e k = (i, j) can then be factorized as

R T i (ψ i )    -0-- Pij d ij i -0- Pij d ij j -0--S pij 3N +i -0-    = R T i (ψ i ) Bk (p ij ), (6.11)
where Bk (p ij ) ∈ R 3×4N does not depend on ψ. If we define

B = stack( B1 , . . . , Bk ) ∈ R 3|E|×4N , (6.12)
bearing rigidity matrix with a weighted counterpart B T G (q)W B G (q) (and analogously for the world-frame symmetric bearing rigidity matrix B W ). Since W is a diagonal matrix, repeating the previous steps simply results into

λ B 6 = (i, j)∈E w ij l ij (6.17)
in place of (6.16). Expression (6.17) can be leveraged for obtaining the gradient of λ B 6 with respect to the configuration q υ = (p υ , ψ υ ) of a specific agent υ in the group. Recalling the definition of set O i in (3.2), we first note that the only terms l ij that depend on p υ are those associated to the edges (υ, j), ∀j ∈ N υ and (j, υ), ∀j ∈ O υ . The computation of the gradient of these terms with respect to p υ is based on the general formula for expressing the derivative of an eigenvalue with respect to a parameter (see, e.g., [START_REF] Yang | Decentralized estimation and control of graph connectivity for mobile sensor networks[END_REF]) dλ 6 = ν T 6 dB G ν 6 (for more details refer to Appendix A.2). Therefore, it can be shown, after some (tedious) steps, that

∇ pυ l υj = 2R υ 1 d 3 υj (β υj ν T p υj + ν T p υj β υj I 3 )P υj ν p υj + + ν ψυ d 2 υj (P υj -β υj β T υj )Sν p υj + ν 2 ψυ d υj P υj S 2 β υj = 2R υ L υj (6.18)
and ∇ pυ l jυ = -2R j L jυ .

The gradient of weights w ij is, however, more involved since the weight design introduces additional dependencies which must be correctly taken into account. Some details about the gradient of the weights are given in Appendix A.3. Consider again the configuration q υ = (p υ , ψ υ ) of a specific agent υ and the weight partition (6.1): clearly, the three subweights w R ij in (6.2), w F ij in (6.3) and w V ij in (6.7) depend on p υ over the same set of edges (υ, j), ∀j ∈ N υ and (j, υ), ∀j ∈ O υ as for the terms l ij . However, because of its definition, and unlike the terms l ij , the third subweight w V ij depends on p υ also over all the edges (j, m), ∀j ∈ O υ , ∀m ∈ N j . Furthermore, weight w F ij depends on ψ υ over all the edges (υ, j), ∀j ∈ N υ , while weights w R ij and w V ij do not depend on ψ υ (w R ij and w V ij are functions of interdistances, and w V ij is also function of the angular quantity η ijk which, however, does not depend on ψ).

Therefore, after some (again tedious) algebra, and by exploiting (6.18), the gradient of λ B 6 with respect to q υ can be expanded as follows

                     ∇ pυ λ B 6 = R υ j∈Nυ (l υj ∇ pυ w υj + 2L υj w υj )+ + j∈Oυ   -2R j w jυ L jυ + m∈N j R m l jm ∇ pυ w jm   ∇ ψυ λ B 6 = j∈Nυ l υj ∇ ψυ w υj . ( 6.19)

The bearing rigidity maintenance controller

Having obtained an explicit expression for the gradient ∇ qυ λ B 6 , we can now present the proposed bearing rigidity maintenance controller. Similarly to [START_REF] Giordano | A passivitybased decentralized strategy for generalized connectivity maintenance[END_REF][START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF], we assume that the agents need to maintain a minimum level of bearing rigidity during motion, i.e., guarantee that λ B 6 (t) > λ min 6 > 0 where λ min 6 is a suitable lower threshold depending on the particular application. We then introduce a potential function

V λ (λ B 6 ) ≥ 0 such that V λ (λ B 6 ) → ∞ as λ B 6 → λ min 6 and V λ (λ B 6 ) → 0 (with vanishing derivative) as λ B 6 → λ max 6 > λ min 6 .
In particular the expression of V λ (λ B 6 ) is the following Each agent υ will then follow the anti-gradient of V λ (λ B 6 ) with respect to q υ which, using (4.1), yields the following body-frame velocity commands

V λ (λ B 6 ) =      0 0≤ λ B 6 ≤ λ min 6 K V tan aλ B 6 + b 2 λ min 6 ≤ λ B 6 ≤ λ max 6 0 λ max 6 ≤ λ B 6 (6.20)
   u υ = -R T υ ∇ pυ V λ (λ B 6 ) w υ = -∇ ψυ V λ (λ B 6 ) (6.22)
By plugging (6.19) in (6.22), we then obtain

                           u υ = - ∂V λ ∂λ B 6   j∈Nυ (l υj ∇ pυ w υj + 2L υj w υj )+ + j∈Oυ   -2 υ R j 2w jυ L jυ + m∈N j υ R m l jm ∇ pυ w jm     w υ = - ∂V λ ∂λ B 6 j∈Nυ l υj ∇ ψυ w υj . (6.23)

Discussion

We conclude with some remarks about the bearing rigidity maintenance controller (6.23). Let us first consider the issue of decentralization and scalability: by analyzing the explicit expressions of the various terms in (6.23) (given in the previous sections), it is possible to conclude that, in order to implement the rigidity maintenance action, agent υ needs knowledge of λ B 6 (for evaluating ∂V λ /∂λ B 6 ), of (ν pυ , ν ψυ ), and of

1) β υj , d υj , ψ υj , ν p j , ν ψ j , ∀j ∈ N υ ; 2) β jυ , d jυ , ψ jυ , ν p j , ν ψ j , ∀j ∈ O υ ; 3) β jm , d jm , ψ jm , ν pm , ν ψm , ∀j ∈ O υ , ∀m ∈ N j /{υ}.
Let us assume, for now, that each agent has access to the value of λ B 6 and to its own components of ν 6 : one can then verify that 1)-3) consist of (measured) bearings, interdistances, relative orientations, and components of the eigenvector ν 6 , i.e., all quantities available to the robot group. In particular, the quantities in items 1)-2) are either locally available to agent υ, or can be computed 5 by exploiting communication with its 1-hop (communication) neighbors j ∈ N υ (the agents measured by υ) and j ∈ O υ (the agents measuring υ). The quantities in item 3) (which are ultimately due to weights w V ij in (6.7)) are instead relative to the 1-hop neighbors m ∈ N j /{υ} of any agent j ∈ O υ . Therefore, agent υ needs to receive this information from some of its 2-hop communication neighbors. In any case, since the amount of information in 1)-3) is constant per neighbor, an upper bound of the communication complexity for an agent

υ is O(|N υ | + |O υ | • max j∈Oυ |N j |).
We can then conclude that the rigidity maintenance controller (6.23) admits a decentralized implementation by, however, assuming a 2-hop communication model.

A second remark is about the availability, for each agent υ, of λ B 6 (a global quantity) and of its own eigenvector components (ν pυ , ν ψυ ): although these quantities are not directly measurable, it is in principle conceivable to estimate them in a decentralized way by adapting the estimators presented in [START_REF] Giordano | A passivitybased decentralized strategy for generalized connectivity maintenance[END_REF][START_REF] Yang | Decentralized estimation and control of graph connectivity for mobile sensor networks[END_REF] for the connectivity case, and ported in [START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF] to the (distance) rigidity case. Indeed, these methods essentially require an explicit characterization of the null-space of the bearing rigidity matrix (which is well understood, see [START_REF] Schiano | A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs[END_REF]), the use of some PI consensus filters, and the suitable exploitation of the structure of the symmetric bearing rigidity matrix. The estimation of λ B 6 and of (ν pυ , ν ψυ ) is, however, left for future extensions of this work and in the following results we just assume availability of these quantities.

A final remark is about the well-posedness of controller (6.23): as well-known, the derivative of an eigenvalue is not well-defined for multiplicities larger than one (repeated eigenvalues), since in these cases one cannot reliably find/estimate a unique eigenvector to be plugged in the derivative computation [START_REF] Friswell | The derivatives of repeated eigenvalues and their associated eigenvectors[END_REF]. This difficulty, which affects any method based on the optimization of a single eigenvalue associated to some matrix of interest, has already been recognized in [START_REF] Giordano | A passivitybased decentralized strategy for generalized connectivity maintenance[END_REF][START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF][START_REF] Yang | Decentralized estimation and control of graph connectivity for mobile sensor networks[END_REF] without, however, proposing an explicit solution to deal with it. A possible workaround is to replace the eigenvalue to be optimized with a 'smoothed' version [START_REF] Chen | Smooth convex approximation to the maximum eigenvalue function[END_REF] which is well-behaved for multiplicities larger than one. Following [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF], a possibility is to replace λ B 6 with the quantity λB = 4N i=6 p (λ B i ) p where λ B i is the i-th eigenvalue of the symmetric bearing rigidity matrix, and p 0. Indeed, when λ

B 6 λ B 7 one has λB ≈ λ B 6 while when, instead, λ B 6 ≈ λ B 7 ≈ λ B m λ B m+1 it is λB ≈ p √ m -6λ B 6 .
Therefore, maximization of λB results into maximization of λ B 6 with however the advantage of λB being always differentiable. By evaluating V λ (•) on λB , the controller (6.22) then becomes

         u υ = - ∂V λ ∂ λB R T υ 4N i=6 λ B i λB p-1 ∇ pυ λ B i w υ = - ∂V λ ∂ λB 4N i=6 λ B i λB p-1 ∇ ψυ λ B i . (6.24)
Evaluation of (6.24) by an agent υ would require the same quantities listed in 1)-3) and, in order to evaluate ∂V λ /∂ λB and ∇ qυ λ B i , the additional availability of λ B i and of (ν i pυ , ν i ψυ ) (the υ-th components of the eigenvector ν i associated to λ i ) for i = 6 . . . 4N . Therefore any implementation of (6.24) would be decentralized but not scalable, as the amount of information per agent would increase with the size of the whole agent group.

Experimental results

We employed a group of five quadrotors of the ones described in Sect. B.2. The TeleKyb framework [START_REF] Grabe | The TeleKyb framework for a modular and extendible ros-based quadrotor control[END_REF] was used for implementing the low-level flight control receiving the body-frame velocity commands (6.24), and for exchanging data via Wi-Fi with the other robots in the group and the ground station. Each quadrotor was also equipped with an onboard camera, refer to Fig. B.5(b). The UAVs were flying in an indoor room equipped with the Vicon motion capture system (Fig. 6.7), refer to Sect. B.2 for more details. This was used for obtaining the ground truth and for reconstructing the body-frame bearing measurements β ij that would have been obtained by the onboard cameras. Indeed, as others [START_REF] Franchi | Mutual localization in a multi-robot system with anonymous relative position measures[END_REF], we found the problem of detecting and tracking (in a reliable way) each quadrotor during flight to be non-trivial, and then exploited the onboard cameras only for the sake of verifying that, during motion, all the neighboring pairs remained indeed well visible in the corresponding camera field of view as predicted by the proposed machinery.

The on-board low-level controller ensures that the orientation of the UAV tracks the desired reference. As usual, this is accomplished by neglecting the couplings among the three body axes, and by treating each individual rotation (roll, pitch, yaw) as a separate channel modeled as a double integrator with input the correspondent body torque. The adopted controller is then a simple PID with saturated integral term in order to prevent wind-up issues. Readings from the on-board gyros are exploited as velocity feedback, while a complementary filter provides estimates of the UAV attitude by fusing together accelerometer and gyro readings from the IMU.

The experimental results are shown in Figs. 6.8 and 6.9: at the beginning the UAVs just followed the control action (6.24) and, as a consequence, the value of λB (t) was maximized (until the vertical dashed line in Fig. 6.8(a)). Then two human operators started acting on two UAVs by sending two velocity commands added to (6.24) with the aim of triggering as much as possible the loss/gain of neighbors because of the considered sensing constraints. One can then note how presence of these two additional velocity inputs (which are representative of any external planner/algorithm in charge of steering the robot formation in the environment) did not threaten bearing rigidity of the formation thanks to the maintenance action (6.24) (indeed, as shown in Fig. 6.8(a), λ(t) remains larger than the chosen threshold λmin = 0.01). This is also confirmed in Fig. 6.9(a) where the evolution of λ B 6 (t) for the unweighted framework is reported. Finally, Fig. 6.8(b) shows the number of edges |E| over time, thus confirming the (intentionally induced) time-varying nature of the sensing topology during motion, and Fig. 6.9(b) reports two of the graph topologies encountered during the experiment. The formation also goes through a rearranging process of its topology (Fig. 6.8(b)) by virtue of the always continuous evolution of the weights associated to its edges (Fig. 6.10).

Conclusions

In this chapter we illustrated a decentralized gradient-based controller able to enforce bearing rigidity maintenance for a group of quadrotor UAVs while coping with three typical sensing constraints of onboard cameras: minimum/maximum range, limited field of view, and possible occluded visibility. The proposed control action exploited a 6 , the sixth smallest eigenvalue of the symmetric bearing rigidity matrix, and of its gradient with respect to the agent configuration. Moreover, a proof of independence of the the bearing rigidity eigenvalue with respect to the agent orientations ψ was given. To the best of our knowledge, this represents the first work addressing the problem of bearing rigidity maintenance for a formation of UAVs by also explicitly taking into account (complex) sensing constraints. The reported experimental results showed the effectiveness of the proposed control strategy.

Several extensions of this work are possible: as pointed out in the previous chapter, a fully onboard implementation is still missing (refer to Chapt. 8 for more details). Furthermore, we would like to address the open points listed in Sect. 6.3.4, that is, decentralized estimation of the components of the body-frame eigenvector ν 6 , and the scalability issue of exploiting the p-norm λ in place of λ B 6 . Finally, we are also considering the possibility of extending the proposed weighting machinery for including a collision avoidance action among quadrotors (which is now not guaranteed), similarly to what done in [START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF].

This chapter shows that current results represent an incomplete view of the problem. In particular, we show that • current methods are a particular instantiation of nonlinear observability theory

• we can introduce the concept of the dynamic bearing observability matrix from higher order derivatives to study the observability of dynamic formations

• the global scale is, in fact, generally observable when the agents move according to known inputs.

We use tools from Riemannian geometry and Lie group theory to tackle, in a general and principled way, the general formulation of the localization problem with states that include both rotations and translations. Finally, we verify our theoretical results by deriving and applying, in both simulations and real experiments on UAVs, a centralized Extended Kalman Filter on Lie groups that is able to estimate the global scale of a moving formation.

Introduction

As already discussed in Chapt. 5 and Sect. 2.5, a fundamental problem in the multirobot applications is the one of localization. 1 This problem becomes of paramount importance, for instance, when a team of robots has to navigate autonomously in an unknown environment, or needs to collaborate on a physical task (e.g., transporting a load [START_REF] Gioioso | The flying hand: A formation of UAVs for cooperative aerial tele-manipulation[END_REF][START_REF] Sreenath | Geometric control and differential flatness of a quadrotor uav with a cable-suspended load[END_REF][START_REF] Wang | Kinematic multi-robot manipulation with no communication using force feedback[END_REF]). In this case, the robots need a precise localization with respect to other robots of the formation. In this chapter, as in the whole Thesis, we consider the case of bearing measurements coming from onboard calibrated monocular cameras. Our goal is to show that, despite the very limited information provided by this type of measurements (direction of relative translations alone), it is possible to reconstruct the full 3-D pose (rotation and translation) of the agents, including the global scale, up to a global gauge ambiguity. We demonstrate this 1) theoretically through an application of nonlinear observability analysis and Riemannian geometry, and 2) practically by applying an Extended Kalman Filter in both simulations and experiments.

Prior work

The problem of localization from bearing-only measurements has appeared in a variety of domains, such as (to cite a few) sensor network localization [START_REF] Shirmohammadi | Self localizing smart camera networks[END_REF][START_REF] Aspnes | A theory of network localization[END_REF][START_REF] Eren | Using angle of arrival (bearing) information in network localization[END_REF] and formation control [START_REF] Bishop | Stabilization of rigid formations with direction-only constraints[END_REF][START_REF] Franchi | Decentralized control of parallel rigid formations with direction constraints and bearing measurements[END_REF][START_REF] Zhao | Distributed control of angle-constrained cyclic formations using bearing-only measurements[END_REF] in controls and robotics, Structure from Motion [START_REF] Brand | Spectral solution of large-scale extrinsic camera calibration as a graph embedding problem[END_REF][START_REF] Li | Multi-view structure computation without explicitly estimating motion[END_REF] in computer vision, and graph drawings [START_REF] Servatius | Constraining plane configurations in computeraided design: Combinatorics of directions and lengths[END_REF] in discrete mathematics. Most of the literature has focused on the development of distributed algorithms (especially in the sensor network and robotics communities), but centralized solutions have also been considered (mostly in the computer vision community). In this chapter, rather than specific algorithms, we are interested in analyzing the fundamental aspects of the localization problem. In this direction, for our case of interest involving bearingonly measurement, there has been a considerable amount of work for developing a theory of rigidity [START_REF] Schiano | A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs[END_REF][START_REF] Schiano | Bearing rigidity maintenance for formations of quadrotor uavs[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF][START_REF] Anderson | Rigid graph control architectures for autonomous formations[END_REF][START_REF] Tron | Rigid components identification and rigidity enforcement in bearing-only localization using the graph cycle basis[END_REF][START_REF] Tron | Rigid components identification and rigidity enforcement in bearing-only localization using the graph cycle basis[END_REF], which can predict what information can be recovered from the available measurements (i.e., whether the solution is "unique"). While most of these works considered only agents in 2-D, recent work has also studied the 3-D case [START_REF] Michieletto | Bearing rigidity theory in SE(3)[END_REF]. The commonly accepted result is that when the number and connectivity of the measurement graph is sufficiently high (that is, when the graph is rigid ), then, for static agents, the solution to the bearing-only localization problem is unique up to a rototranslation and a contraction/expansion of the whole formation. This is determined by considering the nullspace of a so-called rigidity matrix. Most of the existing works, however, do not explicitly consider the case of dynamic agents. At a high level, one could expect that if the agents know their own velocities in their own local frames (e.g., because they control them, or measure them using the onboard IMU), then they could use this metric information to avoid the scale ambiguity. Recent works [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF] have pursued this idea, but do not provide a full, rigorous analysis rooted in nonlinear observability analysis and Riemannian geometry. One disadvantage of [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF] is the presence of two estimators in series for estimating the scale of the formation (one filter estimates the distances over a selection of edges, and the second filter recovers a correctly scaled estimation of the formation configuration). Stability of this cascaded structure is difficult to prove (indeed, nothing is said in [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF]), while our algorithm achieves the same result with only one single EKF. On the other hand, [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF] is able to determine the optimal motion for the agents in order to maximize the observability of the scale factor. We plan to exploit the ideas in [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF] for a similar characterization in the context of our EKF estimation. Nonetheless, ideas related to this approach have been successfully explored in the context of single-agent Simultaneous Localization and Mapping (SLAM) [START_REF] Jones | Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[END_REF][START_REF] Martinelli | Vision and IMU data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination[END_REF] and localization from distance measurements [START_REF] Williams | Observability in topology-constrained multi-robot target tracking[END_REF].

In this regard, nonlinear observability (the problem of determining if the state of a nonlinear dynamical system can be reconstructed by knowing its inputs and outputs) is a classical topic in automatic controls [START_REF] Martinelli | Observability analysis for mobile robot localization[END_REF][START_REF] Khalil | Nonlinear Systems[END_REF][START_REF] Hermann | Nonlinear controllability and observability[END_REF], and it now constitutes textbook material [START_REF] Isidori | Nonlinear control systems[END_REF]. However, it has never been explicitly applied to the problem of localization from bearing-only measurements. On the other hand, Riemannian geometry has been applied in the context of geometric control and estimation of mechanical systems in general [START_REF] Bullo | Geometric control of mechanical systems: modeling, analysis, and design for simple mechanical control systems[END_REF][START_REF] Markley | Fundamentals of spacecraft attitude determination and control[END_REF], and quadrotors in particular [START_REF] Sreenath | Geometric control and differential flatness of a quadrotor uav with a cable-suspended load[END_REF][START_REF] Lee | Nonlinear robust tracking control of a quadrotor UAV on SE(3)[END_REF][START_REF] Loianno | Visual inertial odometry for quadrotors on se(3)[END_REF]. A Riemannian geometry formulation has also been used for multiagent localization with unscaled relative poses [START_REF] Tron | Distributed 3-D localization of camera sensor networks from 2-D image measurements[END_REF], but it has never been applied together with nonlinear observability analysis to the bearing-only case.

Finally, in this work, we propose a validation of our theoretical results using an Extended Kalman Filter (EKF) for statistical filtering of states evolving on Riemannian manifolds. The main advantage of the EKF formulation is that it is relatively easy to derive; in fact, both centralized [START_REF] Roumeliotis | Propagation of uncertainty in cooperative multirobot localization: Analysis and experimental results[END_REF] and decentralized [START_REF] Luft | Recursive Decentralized Collaborative Localization for Sparsely Communicating Robots[END_REF] implementations have been proposed specifically for multiagent systems, although without considering states evolving on Riemannian manifolds. On the other hand, it is known that the EKF is not optimal for nonlinear systems. Developing filtering techniques with optimality guarantees on Riemannian manifolds (and Lie groups in particular) is still an active field of research [START_REF] Lee | Geometric control of multiple quadrotor uavs transporting a cablesuspended rigid body[END_REF][START_REF] Barrau | The invariant extended kalman filter as a stable observer[END_REF]. Since the goal of this work is merely to use filtering as a validation of the theoretical derivations, we opted for a straight (although suboptimal), centralized (as opposed to distributed) application of the EKF, albeit with the explicit consideration of the Riemannian geometry of the states.

Main contributions

In this work, we make several contributions to the state of the art

• we show how the study of rigidity is, in fact, a particular instance of classical nonlinear observability analysis;

• using this insight, we propose the notion of dynamic bearing observability matrix (DBOM), which extends the standard notion of rigidity matrix for the case of moving agents with known inputs;

• by (numerically) analyzing the rank of the DBOM, we show that the global scale of the formation is generally observable;

• we show how tools from Riemannian geometry can be employed to carry out the observability analysis for states evolving in the space of rigid body motions SE(3);

• we derive and apply a centralized Extended Kalman Filter (EKF) on Riemannian manifolds that empirically verifies the theory (i.e., that shows that the global scale can be indeed recovered).

Overall, we show how bearing-only measurements (which, taken individually, do not contain any scale information) and local linear and angular velocity information can be used to recover the entire state of the agents. The recovered state is up to a common rotation and translation (since all measurements are relative and do not have any relation to external reference systems, this last ambiguity appears to be unavoidable, even with dynamic agents). Moreover, by explicitly using the Riemannian geometry of the space of poses (which is based, among other elements, on the use of rotations matrices) throughout the chapter (both for the observability analysis and the statistical filter), we avoid the problems given by other representations (e.g., the singularities of Euler angles, and the nonuniqueness of quaternions). Finally, in this work, we do not perform a full, analytical characterization of the nullspace of the DBOM, and we do not consider distributed filtering solutions. However, these are interesting future directions that are enabled by the present work.

Chapter overview

This chapter is organized as follows. Sect. 7.2 reviews notions from several areas that are necessary to carry out our analysis. In Sect. 7.3 we introduce the novel concept of Dynamic Bearing Observability Matrix. Sect. 7.4 illustrates our EKF design. Finally, Sect. 7.5 reports experimental results with a group of quadrotor UAVs, followed by Sect. 7.6 that concludes the chapter and gives possible future directions.

Preliminaries

General notation

The localization problem was solved in the previous Chapts. 5 and 6 through the estimator described in Sect. 5.2.2 and by assuming that at least one distance was known (either measured or estimated through [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF]). As shown in Sects. 5.3, 5.4 and 6.4 this solution works well, but something better can be done if we think of employing only one EKF which would estimate all the poses of the agents of the formation with the right scale and not the cascade of two estimators like in [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF].

To design this EKF, as we said in the introduction of this chapter, we conducted an observability analysis which shows that the poses of the agents (with the right scale) are actually observable. This will then motivate the convergence of the designed EKF. We decided to conduct the observability analysis directly on the Lie group SE(3). This was done for the sake of completeness, to avoid singularity problems related to the orientation and to use a representation of the orientation which is also global. Moreover, this will allow us to extend this analysis to complete models of the quadrotors (also with their full dynamics) and/or to other robots too.

Note that the representation of the pose of the quadrotor as an element of SE( 3) is done both for the observability analysis and for the localization while the control can still be done through the model detailed in Sect. 4.2 once the localization phase is over. Then, as usual, let W represent an absolute 3-D world reference frame, and A i represent a body reference frame attached to the i-th agent.

Formation, agent and measurement model

This chapter shares the same formation model in terms of graph theory of the previous Chapts. 5 and 6 which is described in Sect. 4.3. But, as just said, in this chapter there is a different modeling assumption with respect to Chapt. 4. Indeed, here we represent the orientation of the single agents in a different way. More specifically, we model the state of an agent i ∈ V as a pose q i = (p i , R i ), where p i ∈ R 3 represents the translation of the origin of A i expressed in W, and R i ∈ SO(3) represents the rotation transforming directions from A i to W. Note that here R i is a function of the full orientation of the rigid body and not only of the yaw angle ψ i as in Sect. 4.2. We denote the space of rigid poses as SE(3) (the detailed definition of SO(3) and its geometry is postponed to Sect. 7.2.3). Let us denote with {e m } 3 m=1 the standard R 3 basis. We also let 1 N and I N represent a vector of all ones and the identity matrix of dimension N , respectively. The operator stack(•) returns a matrix containing a vertical stacking of the arguments. We assume a simple first order model for the 6-D dynamics of each agent

qi = ṗi , Ṙi = R i v i , R i ŵi = = 3 k=1 R i e k , 0 v i k + 3 k=1 0, R i êk w i k , (7.1) (7.2)
where v i , w i ∈ R 3 represent, respectively, the linear and angular velocities expressed in A i , and v i k , w i k represent their components along the e k basis vector. We use this model for generality, but the results of this work could be easily specialized to other cases (e.g., considering only the positions of the agents, or just the 2-D yaw angle, as done in the majority of previous works).

In this work, as in the previous chapters, we assume that each robot is equipped with a sensor (onboard calibrated camera) that allows it to measure the relative bearing vector (4.2) where the R i is now the full rotation matrix.

As in [START_REF] Schiano | A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs[END_REF][START_REF] Zelazo | Rigidity Theory in SE(2) for Unscaled Position Estimation using only Bearing Measurements[END_REF][START_REF] Zelazo | Bearing-Only Formation Control Using an SE(2) Rigidity Theory[END_REF][START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF], we assume that we have only available the inputs {v i , w i } i∈V , and the measurements {β ij } (i,j)∈E . In particular, we do not have access neither to the absolute states q i , nor to the global reference frame W. Throughout the nonlinear observability analysis we will refer to the different components of the vector β ij as:

β ijm = e T m β ij ∈ R, m ∈ {1, 2, 3}. (7.3)
Remark 7.1. While we will individually consider each one of the three elements of each bearing β ij as a separate output, in reality, the fact that β ij ∈ S 2 implies that, in general, only two outputs are algebraically independent. The effect of this is that the bearing rigidity matrix that we will derive will contain more rows than strictly needed (i.e., some rows will be automatically linearly dependent). However, this does not change the result of the rank-based observability test.

Elements of Riemannian geometry

This section covers the basic Riemannian geometry notions that are used in the derivations below. We will be mostly concerned with three manifolds: the Euclidean space R 3 , the space of 3-D rotations SO(3) = {R ∈ R 3×3 : R T R = I, det(R) = 1}, the space of 3-D poses SE(3) = {(p, R) : R ∈ SO(3), p ∈ R 3 }, and the space of N 3-D poses SE(3) N . These manifold are in fact Lie groups, but we will not make use of this fact.

Tangent spaces We denote as T x M the tangent space of a manifold M at a point x ∈ M. The tangent space at a point can be identified as the vector space spanned by the tangents of the curves passing through that point; for instance, if R(t) : I → SO(3) is a parametrized curve in SO(3) defined on some interval I ⊂ R around zero, then Ṙ(0) ∈ T R(0) SO(3). For R 3 , the tangent space at each point can be identified with R 3 itself. For SO(3) however, we first need to define the usual hat (•) ∧ and vee (•) ∨ operators between R 3 and the set of skew-symmetric matrices in

R 3×3 as follows, with v = [v 1 , v 2 , v 3 ] T : v R∧ = R    0 -v 3 v 2 v 3 0 -v 1 -v 2 v 1 0    = Rv, V R∨ = R T V ∨ (7.4)
where V is any vector V ∈ T R SO(3). Note that, if R is not present in the superscript, or if the R = I, the definitions of hat and vee operators are the classical ones present in the literature. It can be shown that the tangent space of SO(3) is given by

T R SO(3) = {Rv : v ∈ R 3 }. (7.5)
We define a basis for T R SO(3) as {e R∨ m } 3 m=1 ; it follows that any vector V ∈ T R SO(3) can be expressed as a vector v ∈ R 3 of local coordinates in this basis with the relation v = V R∨ . For instance, for the curve R(t) defined above, letting R 0 = R(0) we will have Ṙ(0) = w R 0 ∧ for some w ∈ R 3 , or, equivalently, w = Ṙ(0) R 0 ∨ . In the case where R(t) represents a physical time-varying rotation, using the convention given in Sec. 7.2.2, the vector w coincides with a vector of angular velocities expressed in the body frame (see also (7.1)). The tangent space of T q SE(3) can be identified with the direct sum R 3 ⊕ T R SO(3) (i.e., a tangent for SE(3) is simply a tangent for R 3 together with a tangent for SO(3)). Similarly, the tangent space of SE(3) N is simply the direct sum of N copies of T q SE(3). A representation in local coordinates of a vector T q SE(3) can be obtained by stacking the local coordinate representation of each rotational component (as discussed above), with the translational components.

Riemannian metrics A Riemannian metric , smoothly assigns an inner product to each tangent space. The standard Riemannian metric for R 3 is the usual inner product. The standard Riemannian metric for SO(3) is defined as

Rv 1 , Rv 2 = 1 2 tr(v T 1 v2 ) = v T 1 v 2 , (7.6)
where Rv 1 , Rv 2 ∈ T R SO(3) are two tangent vectors. For SE(3), we use the metric given by the sum of the two previous metrics, and for SE(3) N , the sum of the metrics for each copy of SE(3).

Gradients and how to compute them The gradient of a differentiable function f (x), f : M → R computed at a point x 0 on a manifold M is defined as the unique tangent vector ∇ x f (x 0 ) such that, for all curves x(t) with x(0) = x 0 ,

∇ x f (x 0 ), ẋ(0) = d dt f x(t) t=0 . (7.7)
For R 3 , it can be shown that this definition coincides with the more common definition as a vector of partial derivatives. For SO(3), we can use (7.7) to compute gradients of any arbitrary function f in a few steps. First, we consider a fictitious parametrized curve R(t) in SO(3). Then we use the chain rule to compute d dt f R(t) , the derivative of the function along the curve. It can be shown that this derivative (when it exists) can always be written as

d dt f (R) = tr(M T Ṙ) = tr skew(R T M ) T R T Ṙ , (7.8)
where M is some matrix in R 3×3 which in general depends on R, skew(A) = 1 2 (A -A T ) is an operator that extracts the skew-symmetric component of a matrix, and the explicit dependency on t has been omitted for brevity. The first equality in (7.8) comes from the fact that the differential of a map (gradients are a particular case of differentials) are always linear maps [START_REF] Carmo | Riemannian geometry[END_REF], and they can be expressed as linear functionals using the trace operator [START_REF] Horn | Matrix analysis[END_REF]. The second equality in (7.8) comes from the characterization of T R SO(3) given in (7.5) and the fact that tr(S v) = 0 for any symmetric matrix S ∈ R 3×3 and v ∈ R 3 . Comparing (7.8) with (7.7), we then obtain ∇ R f (R 0 ) = 2R skew(R T M ) (this is equivalent to the formula given, e.g., in [START_REF] Absil | Optimization Algorithms on Matrix Manifolds[END_REF]); in local coordinates, this becomes 2 skew(R T M ) ∨ . Informally, we refer to this set of steps as the trace trick. For SE(3) and SE(3) N , the computation of the gradient reduces to a separate computation for each component.

Remark 7.2. In this chapter we represent rotations using rotation matrices. Compared to other representations (such as Euler angles or quaternions), this representation is unambiguous, does not have singularities, and, as shown above, provides a relatively straightforward way to compute gradients (see, e.g., [START_REF] Loianno | Visual inertial odometry for quadrotors on se(3)[END_REF] for additional insights).

Elements of local nonlinear observability

Let q = (q 1 , . . . , q N ) ∈ SE(3) N (according to remark 2), β = stack({β ij }) ∈ R 3|E| , and the input vector fields g v i k , g w i k on SE(3) N which are obtained by appropriately padding with zeros the corresponding vectors in (7.2). Then, the dynamical model of the entire network can be considered as a nonlinear system with affine inputs:

q = i∈V 3 k=1 (g v i k v i k + g w i k w i k ), β = h(q), (7.9)
where h : SE(3) N → R 3|E| , h = stack({h ij }).

The goal of nonlinear observability theory [START_REF] Martinelli | Vision and IMU data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination[END_REF] applied to problem (7.9) is to determine what parts of the state q can be reconstructed from the outputs β and the inputs {v i , w i }. Following the notation of [START_REF] Martinelli | Vision and IMU data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination[END_REF], we indicate the kth order Lie derivative of a function h along the vector fields f 1 , . . . , f k as L k f 1 ,...,f k h. The definition of Lie derivative is given by letting L 0 h = h, and then, recursively:

dL k f 1 ,...,f k h = ∇ q L k f 1 ,...,f k h, L k+1 f 1 ,f 2 ,...,f k+1 h = dL k f 1 ,...,f k h, f k+1 , (7.10)
(7.11)
where , is the Riemannian metric on SE(3) N described in Sect. 7.2.3, and dL k f 1 ,...,f k h is a shorthand notation for the gradient of a Lie derivative. In nonlinear observability, the function h is set to be an output of the system, and the vector fields f 1 , . . . , f k are taken to be the input vector fields.

In our case, we consider each β ijm (that is, each element of β) as a separate output of the system. In order to carry out the local nonlinear observability analysis at a particular configuration q, it is necessary to define the subspace

dΩ = span {dL k f 1 ,...,f k β ijm } , (7.12a)
where k ∈ {0, 1, . . .}, (i, j) ∈ E, m ∈ {1, 2, 3}, (7.12b)

f 1 , . . . , f k ∈ {g v i k , g w i k }. (7.12c)
As discussed in [177, Sect. 1.9], it is sufficient to consider Lie derivatives up to the order k = 6N -1 (where 6N is the dimension of the system (7.9)). In practice, we will numerically verify that k = 1 is already sufficient under general conditions to show that the global scale of the system can be recovered. We define dΩ ⊥ to be the annihilator of dΩ, that is, the subspace of T q SE(3) N such that v, n = 0 ∀v ∈ dΩ, n ∈ N (7.13)

The annihilator tells us the locally unobservable modes of the system [START_REF] Martinelli | Vision and IMU data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination[END_REF][START_REF] Isidori | Nonlinear control systems[END_REF], that is, what variations of the state q cannot be observed under any choice of the inputs (and their Lie brackets). In practice, to find dΩ ⊥ and compute its dimension, we need to switch to a local coordinate representation. To avoid introducing additional notation, we redefine dL k f 1 ,...,f k h to be the vector in R 6N of local coordinates (as opposed to an abstract tangent vector in T q SE(3) N ). Similarly, we redefine dΩ as a matrix (the original subspace dΩ is given by the row span of this matrix):

dΩ = stack {dL k f 1 ,...,f k β ijm } , (7.14)
where the indexes are the same as in (7.12). Intuitively, the matrix dΩ generalizes the classical observability matrix used for linear systems [START_REF] Khalil | Nonlinear Systems[END_REF][START_REF] Hermann | Nonlinear controllability and observability[END_REF][START_REF] Isidori | Nonlinear control systems[END_REF]. Finally, the annihilator dΩ ⊥ is redefined to be the nullspace of dΩ, dΩ ⊥ = null(dΩ).

In the next section we will give the details of the computation of dΩ for Lie derivatives of order up to k = 1 for our system (7.9).

Dynamic Bearing Observability Matrix

In this section we introduce the notion of Dynamic Bearing Observability Matrix2 (DBOM). We define the DBOM R to be equal to the matrix dΩ computed with the gradients of Lie derivatives of order up to k = 1. More explicitly:

R = stack({dL 0 β ijm }, {dL 1 f 1 β ijm }) = stack( RA , RB ), (7.15)
where the matrix RA and RB contain the gradients of the Lie derivative of order, respectively, k = 0 and k = 1, and will be described in detail in Sects. 7.3.1 and 7.3.2.

We anticipate here that the matrix RA is equivalent to the traditional bearing rigidity matrix first introduced in [START_REF] Eren | Sensor and network topologies of formations with direction, bearing, and angle information between agents[END_REF], and then expanded upon in various works (e.g., [START_REF] Schiano | A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs[END_REF][START_REF] Schiano | Bearing rigidity maintenance for formations of quadrotor uavs[END_REF][START_REF] Michieletto | Bearing rigidity theory in SE(3)[END_REF]). However, here we 1. derive it for full 6-D states in SE(3), using a clear interpretation with respect to the Riemannian geometry of the space, and 2. give it the interpretation of a first step in a full nonlinear observability analysis.

Intuitively, since this matrix includes only zeroth order Lie derivatives, its properties tell us which parts of the state can be estimated without providing any input (static agents). As expect from previous works, and compatibly with the intuition, global scaling (contraction/expansion) of the formation generate tangent vectors that are in the nullspace of RA , meaning that the global scale is not observable.

The matrix RB constitutes the main novelty in our analysis. Intuitively, since this matrix includes first order Lie derivatives, its properties tell us what can be estimated by moving the agents with constant inputs. As we will numerically verify in the following, this matrix contributes to reducing the dimension of the nullspace of R by one. The direction that is removed corresponds exactly to the contraction/expansion motion. This is compatible with the intuition above: when agents move, there is a parallax effect that can be exploited to get an estimate of the unknown scales.

Matrix RA

In order to compute the matrix RA , since the zeroth-order Lie derivatives L 0 β ijm are simply equal to the function themselves, we can directly focus on computing their gradients ∇ q L 0 β ijm . For this purpose, we will use the trace-trick method. Assuming that β ij moves along a fictitious curve β ij (t), we compute the following:

d dt L 0 β ijm = ∇ p i L 0 β ijm , ṗi + ∇ p j L 0 β ijm , ṗj ∇ R i L 0 β ijm , Ṙi + ∇ R j L 0 β ijm , Ṙj = e T m βij (7.16)
which can be written also as

e T m βij = e T m ṘT i p ij d ij + e T m d dt p ij d ij . (7.17)
The ∇ p i L 0 β ijm comes from the computation of ∂β ij ∂p i which is reported for clarity here 7.18) where P ij = I 3β ij β T ij is the orthogonal projector onto the orthogonal complement of β ij . In the same way one can compute ∂β ij ∂p j which gives birth to ∇ p j L 0 e T m β ij . On the other hand, regarding the gradient ∇ R i L 0 β ijm we need to focus our attention on the part of e T m βij which multiplies the Ṙi .

∂β ij ∂p i = ∂ ∂p i R T i p ij d ij = R T i d 2 ij - ∂p i ∂p i d ij - ∂p i ∂d ij p ij = -P ij R T i d ij (
Before going into the details of the computation of ∇ R i L 0 β ijm we want to highlight some useful identities. Let u, v ∈ R d represent two generic vectors and A, B ∈ R d×d two generic matrices. Then, the following identities are satisfied

u T v = tr(u T v) = tr(vu T ) tr( ûT v) = -tr( ûv) tr(AB) = tr(BA) (7.19) (7.20) (7.21) and that Ṙi = R i ŵi =⇒ ṘT i = ŵT i R T i =⇒ ṘT i R i = ŵT i . (7.22)
Therefore, we can write

e T m ṘT i p ij d ij = tr e T m ṘT i p ij d ij = tr ṘT i p ij d ij e T m = tr ṘT i R i R T i p ij d ij e T m = = tr ŵT i R T i R i β ij e T m = tr ŵT i β ij e T m = -tr ŵi β ij e T m = -tr β ij e T m ŵi (7.23)
The last term of (7.23) does not represent an inner product because it is missing a 1 2 . We can therefore rewrite the (7.23) as

-tr β ij e T m ŵi = - 1 2 tr 2β ij e T m ŵi (7.24)
the argument of the trace in the (7.24) has the shape of a tangent vector of SO(3) and therefore we can finally extract the gradient ∇ R i L 0 β ijm through the trace trick described in Sect. 7.2.3.

So, we can extract the desired gradients (in local coordinates):

∇ p i L 0 e T m β ij = - 1 d ij e T m P ij R T i , ∇ p j L 0 e T m β ij = + 1 d ij e T m P ij R T i , ∇ R i L 0 e T m β ij = -skew 2β ij e T m ∨ T , ∇ R j L 0 e T m β ij = 0, (7.25)
RA ijm = -0-- e T m P ij R T i d ij -skew 2β ij e T m ∨ T -0- e T m P ij R T i d ij -0--0-, (7.29)
where the blocks are ordered following the ordering of the translational and rotational states in q. Therefore the matrix RA can be seen as

RA = stack ∇ q L 0 β ijm = stack RA ijm , (i, j) ∈ E, m ∈ {1, 2, 3} (7.30)
Notice that the block row in (7.29) is expressed in the frame W. It is also possible to express it in the local frame using the same ideas as [START_REF] Schiano | Bearing rigidity maintenance for formations of quadrotor uavs[END_REF].

Matrix RB

In order to compute the matrix RB , it is necessary to first compute the first-order Lie derivatives L 1

f 1 β ijm = ∇ q L 0 β ijm , f 1 , where f 1 ∈ {g vı k , g wı k } ı∈V .
Note that, for a given i, j, all these Lie derivative are zero except for f 1 ∈ {g v i k , g w i k , g v j k } (for f 1 = g w j k , the Lie derivative is zero due to (7.28)). We can therefore consider only the latter ones. For instance, let us focus on the case f 1 = g v i k . To compute the gradient of L 1 gv i k β ijm we can employ again the trace-trick method described in Sect. 7.2.3.

d dt L 1 gv i k β ijm = ∇ p i L 1 gv i k β ijm , ṗi + ∇ p j L 1 gv i k β ijm , ṗj + + ∇ R i L 1 gv i k β ijm , Ṙi + ∇ R j L 1 gv i k β ijm , Ṙj (7.31)
Notice that, however, while expanding above we will obtain terms that depend on βij .

Similarly to what we mentioned in Sect. 7.2.3, one can show that this dependency is linear. More explicitly, we can rewrite (7.31) as

d dt L 1 gv i k β ijm = ∇p i L 1 gv i k β ijm , ṗi + ∇p j L 1 gv i k β ijm , ṗj + + ∇R i L 1 gv i k β ijm , Ṙi + ∇R j L 1 gv i k β ijm , Ṙj + K ij βij , (7.32)
where K ij is a matrix in R 3×3 . We can exploit (7.32) to compute the rank of the DBOM R, while avoiding the explicit computation of the terms in K ij , thus simplifying the analytical expressions involved; this is because we can collect all the {K ij } (i,j)∈E into a 3|E| × 3|E| matrix K = diag {K ij } , and then rewrite (7.15) as

R = RA RC + K RA , (7.33)
where RC is defined in the same way as RB , but by using the modified (and analytically simpler) gradients ∇q instead of the full gradients ∇ q . Eq. (7.33) Hence, for our purposes, we can compute the rows of RC instead of those of RB . As for the RA in the (7.30), the RC can be written as

RC = stack ∇q L 1 f 1 β ijm = stack RC ijm , where (i, j) ∈ E, m ∈ {1, 2, 3}, f 1 ∈ {g vı k , g wı k } ı∈V (7.35)
where RC ijm is

RC ijm = stack ∇q L 1 gv i 1 β ijm , ∇q L 1 gv i 2 β ijm , ∇q L 1 gv i 3 β ijm , ∇q L 1 gv j 1 β ijm , ∇q L 1 gv j 2 β ijm , ∇q L 1 gv j 3 β ijm , ∇q L 1 gw i 1 β ijm , ∇q L 1 gw i 2 β ijm , ∇q L 1 gw i 3 β ijm , ∇q L 1 gw j 1 β ijm , ∇q L 1 gw j 2 β ijm , ∇q L 1 gw j 3 β ijm .
(7.36) Just to give an example, the first argument of the stack(•) operator in the (7.36) would be

∇ q L 1 gv i k β ijm = stack -0 -∇p i L 1 gv i k β ijm -0 -∇p j L 1 gv i k β ijm -0 -∇R i L 1 gv i k β ijm -0 -∇R j L 1 gv i k β ijm T . (7.37)
The other terms of the (7.36), of course, will have the same structure.

In order to show an analytical expression of the matrix RC we need to rewrite the (7.32) as

d dt L 1 gv i k β ijm = d dt - 1 d ij e T m P ij R T i R i e k = - ∂ ∂p i 1 d ij • ṗi + ∂ ∂p j 1 d ij • ṗj e T m P ij R T i R i e k + 1 d ij e T m βij β T ij + β ij βT ij Ṗij R T i R i e k + - 1 d ij e T m P ij ṘT i R i e k - 1 d ij e T m P ij R T i Ṙi e k = - R i β ij d 2 ij • ṗi + - R i β ij d 2 ij • ṗj e T m P ij R T i R i e k + + 1 d ij e T m βij β T ij R T i R i e k + 1 d ij e T m β ij βT ij R T i R i e k + - 1 d ij e T m P ij ṘT i R i e k - 1 d ij e T m P ij R T i Ṙi e k . (7.38)
Focusing on the right hand side of the (7.38)

• from the first two terms, which multiply respectively ṗi , ṗj , we will extract

∇p i L 1 gv i k β ijm , ∇p j L 1 gv i k β ijm
• the third and fourth term, which multiply respectively βij , βT ij , are the terms which give birth to the part of the matrix K corresponding to g v i k

• from the fifth and sixth term, which multiply respectively ṘT i , Ṙi , we will extract the ∇R i L 1 [START_REF] Jaimes | An approach to surveillance an area using swarm of fixed wing and quad-rotor unmanned aerial vehicles uav (s)[END_REF]) is straightforward and therefore we will focus on the computation of ∇R i L 1

gv i k β ijm Extracting ∇p i L 1 gv i k β ijm , ∇p j L 1 gv i k β ijm from (7.
gv i k β ijm . The first term to consider is - 1 d ij e T m P ij ṘT i R i e k = - 1 2d ij tr 2e T m P ij ŵT i R T i R i e k = - 1 2d ij tr 2e k e T m P ij ŵT i = - 1 2d ij tr 2 ŵT i e k e T m P ij = 1 2d ij tr 2 ŵi e k e T m P ij = 1 2d ij tr 2e k e T m P ij ŵi . (7.39)
The second term to consider is Thus, from (7.39-7.40) it is possible to notice that

- 1 d ij e T m P ij R T i Ṙi e k = - 1 2d ij tr 2e T m P ij ŵi e k = - 1
- 1 d ij e T m P ij ṘT i R i e k - 1 d ij e T m P ij R T i Ṙi e k = 0 =⇒ ∇R i L 1 gv i k β ijm = 0 (7.41)
and therefore, for f 1 = g v i k , we have:

∇p i L 1 gv i k β ijm = - 1 d 2 ij e T m P ij R T i R i e k R i β ij T ∇p j L 1 gv i k β ijm = 1 d 2 ij e T m P ij R T i R i e k R i β ij T ∇R i L 1 gv i k β ijm = 0 T ∇R j L 1 gv i k β ijm = 0 T (
β ijm = d dt 1 d ij e T m P ij R T i R j e k = R i β ij d 2 ij • ṗi + - R i β ij d 2 ij • ṗj e T m P ij R T i R j e k + - 1 d ij e T m βij β T ij R T i R j e k - 1 d ij e T m β ij βT ij R T i R j e k + + 1 d ij e T m P ij ṘT i R j e k + 1 d ij e T m P ij R T i Ṙj e k . (7.46)
Focusing on the right hand side of the (7.46)

• from the first two terms, which multiply respectively ṗi , ṗj , we will extract

∇p i L 1 gv j k β ijm , ∇p j L 1 gv j k β ijm
• the third and fourth term, which multiply respectively βij , βT ij , are the terms which give birth to the part of the matrix K corresponding to g v j k

• from the fifth and sixth term, which multiply respectively ṘT i , Ṙj , we will extract the ∇R i L 1

gv j k β ijm , ∇R j L 1 gv j k β ijm .
Also in this case, extracting ∇p i L 1 gv j k β ijm , ∇p j L 1 gv j k β ijm from (7.46) is straightforward and therefore we will focus on the computation of ∇R i L 1

gv j k β ijm , ∇R j L 1 gv j k β ijm .
For this purpose, let us write the following (with β ijm . Accordingly, for f 1 = g v j k , we have: After building the matrix (7.33) we found that, for a formation of 3 agents in random positions, the rank of the RA is equal to 11 (6N -7) while the one of the R to 12 (6N -6). As an example, we plot in Fig. 7.1 the last 8 singular values of the two matrices as a function of time along the trajectories of the experiment which will be discussed in Sect. 7.5. Note that, as with any dynamic observability result, this result does not imply that the global scale can be recovered under any arbitrary choice of inputs. Instead, it is necessary to choose inputs that "excite" this mode (intuitively, where the agents move approximately perpendicularly to the bearing). This is in line with what was found in [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF] and in any other work concerned with Structure from Motion.

R T i R j = i R j ) 1 d ij e T m P ij ṘT i R j e k = 1 2d ij tr 2e T m P ij ṘT i R j e k = 1 2d ij tr 2 ṘT i R j e k e T m P ij = = 1 2d ij tr 2 ŵT i i R j e k e T m P ij = 1 2d ij tr 2 i R j e k e T m P ij ŵT i (7.47) and
1 d ij e T m P ij R T i Ṙj e k = 1 2d ij tr 2e T m P ij R T i Ṙj e k = 1 2d ij tr 2e k e T m P ij R T i R j ŵj = = 1 2d ij tr 2 ŵj e k e T m P ij i R j = 1 2d ij tr 2 i R T j P ij e m e T k ŵT j . ( 7
∇p i L 1 gv j k β ijm = R i β ij d 2 ij e T m P ij R T i R j e k T ∇p j L 1 gv j k β ijm = - R i β ij d 2 ij e T m P ij R T i R j e k T ∇R i L 1 gv j k β ijm = 1 d ij skew 2R T i R j e k e T m P ij ∨ T ∇R j L 1 gv j k β ijm = 1 d ij skew 2R T j R i P ij e m e T k ∨ T ( 7
Note that at the beginning of this chapter we claimed that the machineries exploited in this work are based only on bearing measurements while in the previous terms, almost everywhere, it appears the distance d ij . This quantity is retrievable through the EKF which is the topic of next Sect. 7.4 exp R k ( ŵk ) (this notation simplifies the filter equations). Note that (7.53) is used for i > 1; for agent i = 1 we have q i,k = 0, given our choice of the reference frame. Note also that the output function h ij in (4.2), corresponding to the edge (i, j), should be redefined to explicitly take into account the measurement noise (denoted with o

ij ) hij (q i , q j , o ij ) = R T i p j -p i + o ij p j -p i + o ij (7.54)
Similarly, we define fi (q i , u i , n i ) = f (q i , u i + n i ), where n i ∈ T q i SE(3) is the noise.

The overall system for the entire network is then

q k = f (q k-1 , u k , n k ), β ij = h(q j , q j , o k ), (7.55)
with u k = stack ({u i,k }), and where we added process and measurement noises

n k , o k (o k = stack ({o ij }) introduced in (7. 54
)), which are assumed to be zero mean multivariate Gaussian with covariance (block diagonal) matrixes

Q k , R k .
The EKF is based on the linearization of the system (7.55):

F k = ∂f ∂q xk-1|k-1 , u k ,n k =0 , H k = ∂h ∂q xk|k-1 ,o k =0 , L k = ∂f ∂n xk-1|k-1 , u k-1 ,n k =0 , M k = ∂h ∂o xk|k-1 ,o k =0 . (7.56) (7.57)
In our specific case, the matrix F is block diagonal with blocks given by .58) where D R i = D R i exp R i (dtR i ŵi ) and D w i = D w i exp R i (dtw i ) denote the differentials of the exponential map with respect to R and w, respectively; details on the implementation of these differential can be found in [START_REF] Tron | Distributed optimization on manifolds for consensus algorithms and camera network localization[END_REF]. The matrix H k is actually the same as the matrix RA computed in Sect. 7.3.1. The matrices L k and M k can be computed similarly to F k and H k .

F i qi = ∂f i ∂q i qi = ṗi + dt Ṙi v i + dtR i vi D R i Ṙ∨ i + dtD w i ẇi . ( 7
The prediction step of the EKF is given by the state estimate

qk|k-1 = f ( qk-1|k-1 , u k-1 ) (7.59)
and the covariance matrix estimate

P k|k-1 = F k-1 P k-1|k-1 F T k-1 + L k-1 Q k-1 L T k-1 . (7.60)
The update step of the filter consists of the state estimate

qk|k = exp qk|k-1 P k|k-1 H T k + + H k P k|k-1 H T k + M k R k M T k S k -1 ỹk (7.61)
where ỹk is the difference between the measured and estimated bearings.

Moreover, the covariance matrix estimate is given by

P k|k = I -P k|k-1 H T k + S -1 k H k P k|k-1 (7.62)
In the next Sect. 7.5 it is shown a result of our (centralized) EKF.

Experimental results

In order to validate the presented ideas, extensive simulations in a Matlab/Simulink environment have been performed considering cases with both process and measurement noise. In addition to simulations also experiments have been performed with real quadrotor UAVs. In this section an experiment with three agents (two real quadrotor UAVs and a fixed virtual agent) will be presented. The experiments were performed in the flying arena described in Sect. B.2.2. Furthermore, we used two of the quadrotors described in Sect. B.2 and a third virtual agent of the formation which fix the ambiguity discussed in Sect. 7.4 has been placed, for convenience, in the origin of the motion capture system reference frame. This agent will be the one with respect to which the other two UAVs will refer their measurements.

During the experiment reported in Fig. 7.2 the agent 2 was moving mainly back and forth on the x axis while the agent 3 was following an ellipsoidal trajectory. A user was also able to give velocity inputs to the UAVs through a joystick. The trajectories described above allow to highlight the fact that, in order to collectively estimate the scale, the agents need to move along exciting trajectories [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF]. Indeed, from Figs. 7.2(a) and 7.2(b) it is possible to notice that the variance associated to the coordinates which are only ligthly excited (y, z coordinates on the agent 2 and 3) remains higher than the ones which are strongly excited (x coordinates on the agents 2 and 3). Fig. 7.2(c) shows the behavior of the true and estimated distance between the agents 2 and 3 and it is clear that the EKF, even starting far from the true values (both for positions and orientations (Fig. 7.2(d))) it is able to recover the true distance d 23 , hence the scale of the formation.

The orientation error showed in Fig. 7.2(d) is defined as:

R err i = 1 2 tr I -RT i R i , ∀i ∈ V (7.63)
where Ri represents the estimation of the real rotation matrix R i corresponding to the agent i.

Conclusions and future works

In this chapter we considered the problem of scale estimation and localization with bearing-only measurements and known agent velocities. By applying nonlinear observability theory and Riemannian geometry, we extended existing results on the theory of rigidity and introduced the notion of Dynamic Bearing Observability Matrix (DBOM). Using experiments with two quadrotor UAVs, we have shown that the global scale is indeed observable and that it can be recovered by employing a centralized EKF implemented directly on SE(3).

The preliminary results of this work open several interesting future research direction, such as3

• providing a full analytical characterization of the nullspace of the Dynamic Bearing Observability Matrix and critical agent configurations • verifying if it is possible to achieve additional insights by including higher order Lie derivatives

• designing input signals that minimize the uncertainty in localization (using [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF] as inspiration),

• studying distributed implementations of the EKF (building upon, e.g., [START_REF] Luft | Recursive Decentralized Collaborative Localization for Sparsely Communicating Robots[END_REF][START_REF] Lendek | Distributed Kalman filtering for multiagent systems[END_REF]) and, finally,

• applying similar ideas to different types of agents (e.g. UAVs with full dynamics, or unicycle dynamics) and measurements (e.g., distance-only),

• extending the proposed ideas to the case of multi-agent systems with unknown input as in [START_REF] Martinelli | Nonlinear Unknown Input Observability: Extension of the Observability Rank Condition[END_REF]. automated landing on a recharging platform as presented in [START_REF] Cocchioni | Visual based landing for an unmanned quadrotor[END_REF]. We believe that improvements on this side of the technology are also fundamental for deploying a swarm of UAVs in real-world missions.

On the other hand, the numerous technological advancements regarding vision sensors, computational power and miniaturization of the electronics give big hopes of seeing these systems becoming a robust reality in the not-so-far future.

Open issues and future perspectives

However, as we mentioned, even if our approach presented many promising results it also highlighted some limitations both theoretical and practical. Note that some difficulties are specific to flying robots and especially quadrotor UAVs and this is the reason why in a part of the multi-agent community they are not even mentioned.

Entirely onboard implementation

As we pointed out in the conclusions of Chapts. 5 to 7 one limitation of this work (and in our opinion the most important) is that we did not implement a fully onboard solution to retrieve bearing measurements from onboard cameras. This obstacle was due to the complex nature of the problems of robust detection, tracking, and identification, in real-time of the agents of the formation. However, we believe to have found the right tool to tackle this problem: the AprilTag detector [START_REF] Olson | AprilTag: A robust and flexible visual fiducial system[END_REF][START_REF] Wang | AprilTag 2: Efficient and robust fiducial detection[END_REF]. This tool is used extensively in different applications (such as exploration and mapping for ground robots [START_REF] Olson | Exploration and mapping with autonomous robot teams[END_REF], camera calibration [START_REF] Richardson | Aprilcal: Assisted and repeatable camera calibration[END_REF][START_REF] Burri | The euroc micro aerial vehicle datasets[END_REF] and autonomous landing on moving platforms for UAVs [START_REF] Kyristsis | Towards autonomous modular uav missions: The detection, geo-location and landing paradigm[END_REF]) but not yet, up to our knowledge, in scenarios like the one described in this Thesis. An alternative to the AprilTag fiducial markers is to use the WhyCon localization system proposed in [START_REF] Krajnik | A practical multirobot localization system[END_REF] and already successfully employed in several works as [START_REF] Saska | Autonomous deployment of swarms of microaerial vehicles in cooperative surveillance[END_REF][START_REF] Saska | System for deployment of groups of unmanned micro aerial vehicles in gps-denied environments using onboard visual relative localization[END_REF]. For all these reasons, during the experiments described in Chapts. 5 to 7 the bearing measurements were retrieved from the Vicon motion capture system. Note that the development of a fully onboard implementation is the topic of current research of the author (see Fig. 8.1). The previous problem is also related to the one of onboard computational power of each quadrotor. At the time of writing, as explained in Appendix B, we equipped our robots with an ODROID-XU4. However, we think that the new, and more powerful, NVIDIA JETSON TX2 module (Fig. 8.2) could represent a significant improvement to our system and this is why we are looking into switching to a platform with this computer module onboard.

Collision avoidance

Another aspect which we did not take into account and plays a pivotal role in all kinds of formation control tasks is the one of collision avoidance. It is impossible to think about a real-world application which is not embedded with a collision avoidance technique. However, in this work collision avoidance is not taken into account in the design of the controller. Despite the capability of the maintenance strategy (Chapt. 6) of making an edge disappear when two agents become too close, this (unfortunately) does not ensure (in general) the avoidance of collision between the robots. This is because the interested edge could be one which is not necessary to maintain the rigidity of the formation over a certain threshold. Indeed, consider the scenario in which two agents start to become too close, but the rigidity of the framework is still well over the desired threshold. In this case, the weight associated with the corresponding edge will gradually vanish leaving the considered agents without any mean of avoiding each other. This is an intrinsic property of our design of the weights. Indeed, it encodes the concept that the disappearance of an edge corresponds to the vanishing of the capability of a robot to sense the other robot to which it is connected. For this reason, we believe that our framework needs an explicit consideration of the collision avoidance problem. In the literature, there are many works [START_REF] Zhao | Control and navigation of multi-vehicle systems using visual measurements[END_REF] which deal with this problem, and we realized that finding the right method is of paramount importance. As a starting point towards this aim, we plan to adapt to our scenario the collision avoidance strategies used in [START_REF] Giordano | A passivitybased decentralized strategy for generalized connectivity maintenance[END_REF][START_REF] Giordano | Bilateral teleoperation of groups of UAVs with decentralized connectivity maintenance[END_REF][START_REF] Lee | Semiautonomous haptic teleoperation control architecture of multiple unmanned aerial vehicles[END_REF][START_REF] Nestmeyer | Decentralized simultaneous multi-target exploration using a connected network of multiple robots[END_REF] which allow preventing inter-agent and obstacle collisions. Another choice would be following up what proposed in [START_REF] Zhao | Bearing rigidity and almost global bearing-only formation stabilization[END_REF] where, besides extending their results of stabilization of bearing-only formations to arbitrary dimensions, they also provide a sufficient condition to ensure collision avoidance between any pair of agents under the action of their controller.

Stochasticity of the control system

Another interesting direction is the explicit modeling of the effect of noise on our algorithms. Noise was added in our simulations and it was intrinsically present in all the experiments, but no formal study was conducted to claim specific robustness properties of the proposed methods. This direction was partially followed in Chapt. 7 in which we took into account (see the equation (7.55)) both process and measurement noises. Therefore, we are aware that to go towards a real-world application we need to deal with the stochasticity of the overall system which could include not entirely reliable communication, tracking of the other robots through cameras and so on. Regarding the communication, during our experiments, we experienced several problems with the WiFi interferences, and therefore an excellent future direction would be to investigate alternatives to this communication technology. Regarding the problem of tracking other quadrotors, one of the dilemmas we would need to deal with is the presence of outliers and therefore choose the right algorithm (between the many which are present in the literature) is essential. A well-known way to deal with outliers, which would be our first shot at tackling these problems is the random sample consensus (RANSAC) algorithm [START_REF] Fischler | Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography[END_REF] (and its extensions), which has been successfully explicitly used for AprilTags in [START_REF] Nissler | Evaluation and improvement of global pose estimation with multiple apriltags for industrial manipulators[END_REF]. All this could compromise the designed controller and therefore study the robustness of the controller is essential. A specific study of the gradient-based control law used to steer a formation to the desired bearing formation which could be taken as a starting point for future developments is [START_REF] Garcia De Marina | Controlling rigid formations of mobile agents under inconsistent measurements[END_REF]. In this work, they try to give a solution to the specific problem of measurements discrepancies. It is highlighted that this issue can sometimes produce undesirable collective motions of the formation, and therefore dealing with this problem is also of paramount importance.

Multiplicity of the rigidity eigenvalue

Both in Chapts. 5 and 6 we relied on the sixth smallest eigenvalue λ 6 of the 123 Bearing-based localization and control for multiple quadrotors symmetric bearing rigidity matrix B W G (q) as a measure of rigidity of the considered framework (G, q). Already in Sect. 6.3.4 we alluded to the fact that what we said is valid as soon as the rigidity eigenvalue has multiplicity equal to one. This is easy to understand if we remember that we exploited, in our computations, several times the well-known formula

λ i = v T i Av i (8.1)
where λ i is an eigenvalue of the matrix A and v i is the corresponding eigenvector. Now suppose that we design a controller which is based on the λ i and that this eigenvalue has multiplicity equal to two. This means that there are two different eigenvectors v i 1 , v i 2 corresponding to this eigenvalue and this would mean that even a linear combination of these two vectors is an eigenvector corresponding to the considered eigenvalue λ i . Therefore, it will likely happen that the controller, at each time step, would switch between different eigenvectors, with all the possible instabilities causes by this discontinuing switching. This is a very relevant problem which was already encountered, as we already said, in [START_REF] Giordano | A passivitybased decentralized strategy for generalized connectivity maintenance[END_REF][START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF][START_REF] Yang | Decentralized estimation and control of graph connectivity for mobile sensor networks[END_REF] without proposing a solution. To find a solution, we believe that it could be helpful to understand, in the scenarios analyzed in this Thesis, what motivates the occurrence of multiple eigenvalues and if it is associable to a specific structure of the framework and/or a specific topology of the graph G. For this reason, we believe that this problem represents an additional future direction of this work.

Agent dynamics

In the multi-robot literature, as in this Thesis, we often encounter works1 in which the dynamics of the single agent is neglected and therefore each agent is seen as a simple-integrator. Hence, we believe that another interesting future direction, also highlighted in Sect. 7.6, is extending our results to systems with different types of agents, especially second-order integrators, in order to be able to include also the dynamics associated to each agent.

Final remarks

A last open issue, which is a result of the solution of the open directions mentioned above, is the need of tackling more complex problems than the simple steering of the formation by a human operator in an indoor room equipped with a Vicon motion capture system. As we pointed out many times, this would include, but it is not limited to, exploration and mapping in collapsed buildings, cooperative transportation, surveillance, and so forth. Furthermore, moving outdoor, in an uncontrolled environment is a big step which is needed to make these systems undoubtedly more useful for humankind. A first step towards all these aims would be to enhance the framework described in Chapt. 6 with the one of Chapt. 7 for the scale estimation.

Overall, we believe that the contributions of this Thesis allow the field of multirobot coordination in unknown environments to take a step further in the understanding of the concepts of theory of rigidity and its application to the decentralized formation control and localization. These problems revealed to be challenging and at the same time full of potential for real-world applications which we sincerely hope to see in the near future. therefore the ∇ q i λ B 6 would be equal to

∇ q i λ B 6 = (i, j)∈E ∇ q i w R ij w F ij w V ij l ij + w R ij ∇ q i w F ij w V ij l ij + w R ij w F ij ∇ q i w V ij l ij + w R ij w F ij w V ij (∇ q i l ij ) . (A.19)
From the (A. [START_REF] Howard | Experiments with a large heterogeneous mobile robot team: Exploration, mapping, deployment and detection[END_REF]) it is then clear that to compute the gradient of the λ B 6 when the weights are involved, we need to compute the terms ∇ q i w R ij , ∇ q i w F ij , ∇ q i w V ij . Remember that q i = (p i , ψ i ) and therefore each of the previous terms will break down to a ∇ p i (•) and a ∇ ψ i (•).

As an example, let us look at the gradient of the weight w F ij (α ij ) defined in (6.3). In particular it will be

∂ ∂p i w F ij (α ij ) = ∂ ∂α ij w F ij ∂ ∂p i α ij . (A.20)
The first term of the right-hand-side of the (A.20) depends just from the shape we decided for the function expressed by ( 6.3) and it involves really easy derivatives.

On the other side, the second term of the right-hand-side of the (A.20), because α ij = o T C β ij , it will be just equal to the first equation of (A.15)1 . The same thing goes for the derivative with respect to p j and for ψ i it holds

∂ ∂ψ i w F ij (α ij ) = ∂ ∂α ij w F ij ∂ ∂ψ i α ij . (A.21)
where the second term of the right-hand-side of the (A.21) is equal to the third equation of (A.15).

• the formation control and estimation algorithms described in Chapts. 5 and 6

• the observability analysis (with the corresponding EKF) described in Chapt. 7.

In the case (2) above TeleKyb was used to interface the control/estimation algorithms with the V-REP simulator. Using TeleKyb also in simulations allowed us to have a similar architecture between simulations and real experiments. This enabled us to switch easily between the simulation and experimental architectures. The module which provides the state of the formation is different, of course, between the simulation architecture and the experimental one. In the first case, it is the V-REP simulator (or Matlab) which provides the state, while during real experiments is the Vicon motion capture system which tracks the pose of real quadrotor UAVs.

The communication between the modules present in Fig. B.1 is made possible through the Robotic Operating System (ROS1 ) and, more in details:

• the bridge described in [START_REF] Spica | Interfacing Matlab/Simulink with V-REP for an easy development of sensor-based control algorithms for robotic platforms[END_REF] has been used to build a communication between

V-REP and ROS

• another bridge 3 it is also possible to notice that we used a Vicon motion capture system (see Sect. B.2.2) to track the pose of the quadrotor UAVs. The Vicon was used for obtaining a reliable ground truth with a sub-millimeter accuracy, to retrieve the linear/angular velocities of the real robots and, when an edge was existing, the body-frame bearing measurements β ij between the considered robots which would have been obtained by the onboard cameras. To mimic bearing measurements coming from a camera we also exploited the option to decrease the frequency at which the Vicon was giving data.

B.2.1 The quadrotor

The quadrotor platform used for the experiments of this work consists of a MK-Quadro by Mikrokopter Note also that in order to have a smooth trajectory and to match better the dynamics capabilities of the UAVs we used a fourth order linear filter for the inputs given to each UAV: ....

p f i = -k 1 ... p f i -k 2 pf i -k 3 ( ṗf i -ṗi ) -k 4 (p f i -p i ) (B.1)
where with .... p f i , ... p f i , pf i , ṗf i we indicate respectively the filtered snap, jerk, acceleration and velocity given to the i-th UAV. The gains k 1 , k 2 , k 3 , k 4 were chosen in order to have the poles in -7,-6,-5,-4 and therefore have a convenient settling time.

Moreover, note that towards the end of this work we decided to upgrade our system from using TeleKyb to a set of packages based on GenoM3 [START_REF] Mallet | GenoM3: Building middleware-independent robotic components[END_REF] for implementing the low-level flight control. This set of packages is mainly developed at LAAS-CNRS of Toulouse. This choice was done in order to build a common better tool for flying UAVs and mainly because of the many improvements that were made over the TeleKyb framework. This new framework has been successfully used for the experiments showed in Chapt. 7. However, the high level part of the algorithm described in Chapt. 7 (as in the other chapters) was still implemented through Matlab/Simulink.

B.2.2 The Vicon motion capture system

During the experiments, in order to get the pose (position and orientation) of the robots in the environment we used a Vicon motion capture system. This system is made of multiple infrared cameras (Fig. B.7), a router to receive all the camera measurements and a software (Vicon Tracker) to analyze these measurements. We used this system in two different configurations according to the room in which the experiments were conducted:

• for early-stage tests we used a room with a flying volume of about 7m x 4.5m

Abstract

The aim of this Thesis is to give contributions to the state of the art on the collective behavior of a group of flying robots, specifically quadrotor UAVs, which can only rely on their onboard capabilities and not on a centralized system (e.g., Vicon or GNSS) in order to safely navigate in the environment. We achieve this goal by giving a possible solution to the problems of formation control and localization from onboard sensing and local communication. We tackle these problems exploiting mainly concepts from algebraic graph theory and the so-called theory of rigidity. This allows us to solve these problems in a decentralized fashion, and propose decentralized algorithms able to also take into account some typical sensory limitations. The onboard capabilities we referred to above are represented by an onboard monocular camera and an inertial measurement unit (IMU) in addition to the capability of each robot to communicate (through RF) with some of its neighbors. This is due to the fact that an IMU and a camera represent a possible minimal, lightweight and inexpensive configuration for the autonomous localization and navigation of a quadrotor UAV.

3Figure 2

 2 Figure 2 -1.2(a) : Transport coopératif d'un objet rigide avec quatre UAVs. Copyright d'Université de la Pennsylvanie (USA). 1.2(b) : un drapeau américain fait de 300 drones Shooting Star par Intel au spectacle de mi-temps du Super Bowl (2017). Copyright d'Intel. 1.2(c) : un groupe de robots terrestres naviguants et déplacent des objets autour d'un entrepôt. Copyright de systèmes Kiva. 1.2(d) : structure de filament extensible construite par des robots. Copyright de l'Institute for Computational Design and Construction à l'Université de Stuttgart.

Figure 1 . 1 -

 11 Figure 1.1 -Examples of collective behaviors exhibited by fire ants. Fig. 1.1(a): a raft of 500 fire ants, composed of a partially wetted layer of ants on the bottom and dry ants on top, from [16]. Fig. 1.1(b): buoyancy and elasticity of the ant raft, as shown by attempted submersion by a twig, from [16].

Figure 1

 1 Figure 1.2 -1.2(a): cooperative transportation of a rigid object with four UAVs. Copyright of University of Pennsylvania (USA). 1.2(b): an American flag made of 300 Shooting Star drones by Intel at the halftime show of the Super Bowl (2017). Copyright of Intel. 1.2(c): a group of ground robots navigating and moving objects around a warehouse. Copyright of Kiva systems. 1.2(d): tensile filament structure built by multiple robots. Copyright of the Institute for Computational Design and Construction at the University of Stuttgart.

Figure 2 . 1 -

 21 Figure 2.1 -An example of four robots working together, in an automotive factory, on the same car. The robots are fulfilling different tasks and they have a really limited awareness of other robots and of their environment (from which they are separated through a cage).

  .

Figure 2 . 2 -Figure 2 . 3 -

 2223 Figure 2.2 -Fig. 2.2(a): convoy of vehicles driving for the Vislab Intercontinental Autonomous Challenge (VIAC) which one of the first examples of autonomous driving challenges. Fig. 2.2(b): the route covered during the challenge which lasted 100 days in which 15.926 km were covered, from Parma (Italy) to Shanghai (China).

Figure 2 . 4 -

 24 Figure 2.4 -Examples of decentralized behaviors in humans and ants. Fig. 2.4(a): a group of people performing in the 25th castells competition at Tarraco Arena ring in Tarragona, Spain. Fig. 2.4(b): cockerelli ants retrieving a piece of fig, from[START_REF] Berman | Study of group food retrieval by ants as a model for multi-robot collective transport strategies[END_REF]

8

 8

  .

Figure 2 . 5 -

 25 Figure 2.5 -Examples of schemes of a centralized (Fig. 2.5(a)) and decentralized (Fig. 2.5(a)) system

Figure 2 . 6 -

 26 Figure 2.6 -Examples of fixed and rotary wing UAVs. Fig. 2.6(a): the MQ-9 Reaper, from General Atomics. Fig. 2.6(b): the Mavic Pro, from DJI.

Figure 2 . 7 -

 27 Figure 2.7 -Global overview of current status of UAV regulations on a country-level resolution (status: October 2016), from[START_REF] Stöcker | Review of the current state of uav regulations[END_REF]

Figure 2 . 8 -

 28 Figure 2.8 -Global distribution of first releases of UAV regulations on country level resolution (status: October 2016), from[START_REF] Stöcker | Review of the current state of uav regulations[END_REF]

Figure 2 . 10 -

 210 Figure 2.10 -Main multi-agent control problems

Figure 2 . 11 -

 211 Figure 2.11 -Example of leader-follower, flocking and shoaling. Fig. 2.11(a): leader-follower happening between a mother duck and her ducklings, Fig. 2.11(b): example of flocking of birds, Fig. 2.11(c): example of shoaling of fishes 11 .

Contents 3 . 1 27 3. 2 28 3. 2 . 1 29 3. 3 31 3. 3 . 1 32 3. 4 33 3. 4 . 1 36 3. 4 . 2

 312722821293313132433413642 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Graph theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Graph theory definitions . . . . . . . . . . . . . . . . . . Algebraic graph theory . . . . . . . . . . . . . . . . . . . . . . . Laplacian matrix and connectivity . . . . . . . . . . . . Rigidity theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . The Laman's theorem and some definitions for the case of distance constraints . . . . . . . . . . . . . . . . . . . The distance-rigidity matrix . . . . . . . . . . . . . . . . 39 3.4.3 The case of bearing constraints . . . . . . . . . . . . . . 44 3.4.4 Similarity between the rigidity matrix and the Jacobian of robotic manipulators . . . . . . . . . . . . . . . . . . 45

Figure 3 . 1 -

 31 Figure 3.1 -Leonhard Euler, Swiss mathematician (1707-1783)

  Sensing graph: it encodes what agents can be sensed by each agent • Communication graph: it encodes to what agents each agent can communicate some sort of information • Action graph: for each control action, encodes what robots are (locally) affected .

Figure 3 . 2 -

 32 Figure 3.2 -Examples of undirected (Fig. 3.2(a)) and directed (Fig. 3.2(b)) graphs

Figure 3 . 3 -

 33 Figure 3.3 -Fig. 3.3(a): connected graph, Fig. 3.3(b): weakly connected graph, Fig. 3.3(c): strongly connected graph, Fig. 3.3(d): disconnected graph

Figure 3 . 4 -

 34 Figure 3.4 -Graphs with different connectivity eigenvalues λ 2

Figure 3 . 5 -

 35 Figure 3.5 -Wright brothers and rigidity. Fig. 3.5(a): shows some drafts by Wilbur Wright about a kite to test warping for roll control. Fig. 3.5(b): shows a demo available at the Smithsonian National Air and Space Museum to stress out the difference that a rigid structure made in the design of the the 1903 Wright Flyer, the craft that ushered in the age of flight. Fig. 3.5(c): at the same museum, shows the 1903 Wright Flyer, the craft that ushered in the age of flight.

Figure 3 . 6 -

 36 Figure 3.6 -Examples of rigid and nonrigid frameworks. The framework in Fig. 3.6(a) is rigid because it cannot be deformed while satisfying the distance constraints over the edges. The framework in Fig. 3.6(b) is not rigid because it can be deformed by a smooth motion without violating the distance constraints over the edges.

Figure 3 . 7 -

 37 Figure 3.7 -Examples of minimally rigid (Fig. 3.7(a)) and redundantly rigid (Fig. 3.7(a)) graphs.

Figure 3 . 8 -

 38 Figure 3.8 -Global rigidity: Fig. 3.8(a) is rigid but not globally rigid because the same distance constraints can be satisfied, e.g., also by the graph in Fig. 3.8(b). On the contrary, the graph in Fig. 3.8(c) is globally rigid. Note also that Fig. 3.8(a) cannot be continuously deformed into Fig. 3.8(b). Therefore, locally, the graphs in Figs. 3.8(a) and 3.8(b) are rigid graphs, although not in a global sense.

Figure 3 . 9 -

 39 Figure 3.9 -The rigidity function g G in the case of a rigid framework

Figure 3 . 10 -

 310 Figure 3.10 -Examples of infinitesimally rigid (Fig. 3.10(a)) and not infinitesimally rigid (Fig. 3.10(b)) graphs. Note that the red color is used to highlight that there is still an edge between the agents 1 and 3.

Figure 3 . 11 -

 311 Figure 3.11 -Example of not-rigid (Fig. 3.11(a)) and rigid (Fig. 3.11(b)) graphs in the case of distance constraints in two dimensions for four agents.

Contents 4 . 1

 41 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 4.2 Agent model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 4.3 Directed bearing rigidity in R 3 × S 1 . . . . . . . . . . . . . . . . 52 4.1 Introduction

Figure 4 . 1 -Figure 4 . 2 -

 4142 Figure 4.1 -A quadrotor UAV with the origin of its body frame A i which is coincident with the origin of the world reference frame W

Figure 4 . 3 -

 43 Figure 4.3 -Examples of two frameworks which are bearing equivalent but not bearing congruent

Figure 4 . 4 -

 44 Figure 4.4 -Relationship between the three kinds of bearing rigidity introduced in this section, from[START_REF] Zhao | Bearing rigidity and almost global bearing-only formation stabilization[END_REF]

  the orthogonal projector onto the orthogonal complement of β ij , and S = [0 0 1] T × where [•] × indicates the usual skew-symmetric matrix operator.

Contents 5 . 1 58 5. 1 . 1 60 5. 2 61 5. 2 . 2 63 5. 2 . 3 66 5. 3 68 5. 4

 51581160261226323663684 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Chapter overview . . . . . . . . . . . . . . . . . . . . . . Decentralized formation control . . . . . . . . . . . . . . . . . . 60 5.2.1 Rigidity-based control of bearing frameworks in R 3 × S 1 Rigidity-based localization of time-varying bearing frameworks in R 3 × S 1 . . . . . . . . . . . . . . . . . . . . . . Coordinated motions in the null-space of the bearing rigidity matrix . . . . . . . . . . . . . . . . . . . . . . . 65 5.2.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . Experimental results . . . . . . . . . . . . . . . . . . . . . . . . . 69 5.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72

Figure 5 . 1 -

 51 Figure 5.1 -Three examples of rigid graphs with 5 agents. Fig. 5.1(a) is the overconstrained graph used in [77]. Fig. 5.1(b) is another example of rigid graph which does not require all the constraints of the previous one and Fig. 5.1(c) is yet another rigid graph chosen randomly.

Figure 5 . 2 -

 52 Figure 5.2 -Possible minimal bearing rigid topologies for N ∈ {3, 4, 5, 6} (note that some arrows are bi-directional)

Figure 5 . 3 -

 53 Figure 5.3 -Results of the simulation. 5.3(a)-top: behavior of the five null-space motion commands ν(t) (blue, purple, yellow) λ(t) (green) and w(t) (red). 5.3(a)-bottom: behavior of the bearing control error e F (t) and of the localization control error e L (t) . 5.3(b): behavior of the rigidity eigenvalues λ C 6 (t) (control -blue) and λ E 6 (t) (estimation -red)

Figure 5 . 4 -Figure 5 . 5 -

 5455 Figure 5.4 -Results of the first simulation. 5.4(a): behavior of the orientation estimation error e ψ (t). 5.4(b): behavior of the formation scale error e s (t)

Figure 5 . 6 -

 56 Figure 5.6 -Results of the experiment. Fig. 5.6(a)-top: behavior of the five null-space motion commands ν(t) (blue, purple, yellow) λ(t) (green) and w(t) (red). Fig. 5.6(a)bottom: behavior of the bearing control error e F (t) and of the localization control error e L (t) . Fig. 5.6(b): behavior of the rigidity eigenvalues λ C 6 (t) (control -blue) and λ E 6 (t) (estimation -red)

Figure 5 . 7 -

 57 Figure 5.7 -Results of the experiment. Fig. 5.7(a): behavior of the orientation estimation error e ψ (t). Fig. 5.7(b): behavior of the formation scale error e s (t)

Contents 6 . 1

 61 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76 6.1.1 Chapter overview . . . . . . . . . . . . . . . . . . . . . . 77 6.2 Cooperative localization from bearing measurements . . . . . . . 78 6.3 A bearing rigidity maintenance strategy . . . . . . . . . . . . . . 78 6.3.1 Design of the inter-agent weights . . . . . . . . . . . . . 79 6.3.2 The bearing rigidity eigenvalue . . . . . . . . . . . . . . 83 6.3.3 The bearing rigidity maintenance controller . . . . . . . 87 6.3.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . 88 6.4 Experimental results . . . . . . . . . . . . . . . . . . . . . . . . . 90 6.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91

Figure 6 . 1 -

 61 Figure 6.1 -Fig. 6.1(a) is a snapshot from the onboard camera mounted on a quadrotor. It is taken when the robot is close to lose a link (with respect to the robot highlighted in yellow) due to the maximum range constraint. Fig. 6.1(b) is the graph view corresponding to the case of of Fig. 6.1(a).

Figure 6 . 2 -

 62 Figure 6.2 -Representative shapes for the weights w Rij (d ij ) (a), w Fij (α ij ) (b), g(η ijk ) (c), and h(δ ijk ) (d)

Figure 6 . 3 -Figure 6 . 4 -

 6364 Figure 6.3 -Fig. 6.3(a) is a snapshot from the onboard camera mounted on a quadrotor. It is taken when the robot is close to lose a link (with respect to the robot highlighted in yellow) due to the limited field of view constraint. Fig. 6.3(b) is the graph view corresponding to the case of of Fig. 6.3(a).

Fig. 6 .

 6 Fig. 6.6 shows a possible shape for the potential function V λ (λ B 6 ).

Figure 6 . 6 -

 66 Figure 6.6 -A representative shape for the function V λ (λ B 6 ) with λ min

Figure 6 .

 6 Figure 6.7 -A group of five quadrotor UAVs flying in our flying arena at INRIA Rennes, Bretagne Atlantique, France

Figure 6

 6 Figure 6.9 -6.9(a): behavior of the unweighted rigidity eigenvalue λ B 6 , 6.9(b): two graph topologies corresponding to the maximum (left, |E| = 17) and minimum (right, |E| = 14) number of edges during the experiment.

Figure 6 . 10 -

 610 Figure 6.10 -Behavior of the weights over the edges in E during the reported experiment. The reader should notice the continuous nature of the weights

2d ij tr 2

 2 ŵi e k e T m P ij = -1 2d ij tr 2e k e T m P ij ŵi . (7.40)

7. 3 . 3

 33 Numerical verification of the ranks of RA and R Sects. 7.3.1 and 7.3.2 provided a complete analytical expression of the different terms of the matrix R. Notice that these matrices (and their ranks) depend only on the position and rotations of the agents and on their velocities (i.e., inputs).

Figure 7 . 2 -

 72 Figure 7.2 -Fig. 7.2(a) shows the behavior of the real position (x, y, z) (in meters) of the agent 2, in black, and the estimated one with the associated covariance respectively in red (x), green (y) and blue (z); Fig. 7.2(b) is the dual of Fig. 7.2(a) for the agent 3; Fig. 7.2(c) shows the behavior of the true (blue) and estimated (red) distance d 23 between the agents 2 and 3. Fig. 7.2(d) shows the behavior of the orientation error introduced in (7.63)

Figure 8 . 1 -

 81 Figure 8.1 -First tests with the AprilTag fiducial markers. Fig. 8.1(a): two quadrotor UAVs flying in our flying arena with some Apriltag[START_REF] Wang | AprilTag 2: Efficient and robust fiducial detection[END_REF] mounted on them. Note that we plan to reduce the size of the fiducial markers used in our applications to make them more compact and easy to embed on each UAV. Fig.8.1(b): a snapshot of an onboard camera which shows the detection of an AprilTag fiducial marker mounted on another quadrotor UAV and the consequent translation to a bearing vector.

Figure 8 . 2 -

 82 Figure 8.2 -The NVIDIA JETSON TX2 Module, from http://www.nvidia.com

2

 2 has been used to build a communication between Matlab, Simulink and ROS B.2 Real experiments architecture As explained in Appendix B.1 the overall architecture used for the real experiments is similar to the one used in simulations. It is possible to appreciate this similarity by comparing Fig. B.1 with Fig. B.3. From Fig. B.

3 .

 3 The usual Mk-Quadro was extended with an ODROID-XU4 computing device (Fig. B.4) running Ubuntu (14.04 or 16.04) ROS and TeleKyb.

Figure B. 1 -

 1 Figure B.1 -Scheme of the software architecture used during a simulation when the V-REP robot simulator is used to model the quadrotor UAVs

Figure B. 2 -

 2 Figure B.2 -Screenshot of a simulation of six quadrotor UAVs in the V-REP robot simulator

x 2 .

 2 5m of about 7 m x 4.5 m x 2.5 m (Fig. B.6(a)) equipped with a Vicon system made of eight Bonita 10 cameras (Fig. B.7(a)) • for final experiments we used a room with a bigger flying volume (with respect to the previous one) of about 6.5 m x 6.5 m x 3 m (Fig. B.6(b)) equipped with

Figure B. 3 -

 3 Figure B.3 -Scheme of the overall architecture used during the experiments with real quadrotor UAVs and the Vicon motion capture system

  Contributions de la Thèse . . . . . . . . . . . . . . . . . . . . . . . . . . . iv

	AGL Above Ground Level
	BLOS Beyond Line of Sight
	BRM Bearing Rigidity Matrix
	DBOM Dynamic Bearing Observability Matrix
	EKF Extended Kalman Filter	Contents
	GNSS Global Navigation Satellite System
	IMU Inertial Measurement Unit
	LOS Line of Sight
	Notation	xix
	MAS Multi-Agent System
	MSL Median Sea Level Chapter 1 Introduction
	RF Radio Frequency
	ROS Robot Operating System
	SfM Structure from Motion
	SLAM Simultaneous Localization and Mapping
	UAV Unmanned Aerial Vehicle
	VTOL Vertical Take-off and Landing
	xiv

1.1 Thesis contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2 Structure of the Thesis . . . . . . . . . . . . . . . . . . . . . . . . . . Part I Preliminaries and state of the art Chapter 2 Multi-robot systems 2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2 Decentralization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.3 Multi-aerial vehicles . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.4 Multi-agent control problems . . . . . . . . . . . . . . . . . . . . . . 2.4.1 Consensus and rendezvous . . . . . . . . . . . . . . . . . . . . 2.4.2 Formation control . . . . . . . . . . . . . . . . . . . . . . . . 2.5 Cooperative localization algorithms . . . . . . . . . . . . . . . . . . . Chapter 3 Algebraic graph theory and graph rigidity 3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2 Graph theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2.1 Graph theory definitions . . . . . . . . . . . . . . . . . . . . . 3.3 Algebraic graph theory . . . . . . . . . . . . . . . . . . . . . . . . . . 3.3.1 Laplacian matrix and connectivity . . . . . . . . . . . . . . . 3.4 Rigidity theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.4.1 The Laman's theorem and some definitions for the case of distance constraints . . . . . . . . . . . . . . . . . . . . . . . xv

  to 7.

		Chapter 2
		Multi-robot systems
	Contents	Part I
	2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
	2.2 Decentralization . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
	Preliminaries and 2.3 Multi-aerial vehicles . . . . . . . . . . . . . . . . . . . . . . . . . 15
	2.4 Multi-agent control problems . . . . . . . . . . . . . . . . . . . . 20
		state of the art 2.4.1 Consensus and rendezvous . . . . . . . . . . . . . . . . . 21
		2.4.2 Formation control . . . . . . . . . . . . . . . . . . . . . . 22
	2.5 Cooperative localization algorithms . . . . . . . . . . . . . . . . 24

Table 2 .

 2

	When we refer to an Unmanned Aerial Vehicle (UAV) we indicate a flying vehicle
	which does not have a human pilot onboard 9 . UAVs can be classified according to

Table 2 .

 2

	2 -Some of the advantages and disadvantages of fixed-wing UAVs
	Regarding rotary-wing UAVs, their advantages and disadvantages, with respect
	to fixed-wing UAVs, are reported in table 2.3.
	Advantages	Disadvantages
	Ability of vertical take-off and landing (VTOL)	Limited flight time
		More complex mechanics and elec-
	Hovering capability	tronics and therefore more mainte-
		nance needed
	Higher manoeuvrability, especially in-door	Lower cruising speed
	Table 2.3 -Some of the advantages and disadvantages of rotary-wing UAVs

  .48) from the(7.47-7.48), through the trace-trick described in Sect. 7.2.3, we can then extract the ∇R i L 1

	gv j k	β ijm , ∇R j L 1 gv j k

  Regarding the case f 1 = g w i k , the Lie derivative L 1 gw i k β ijm depends on the states only through β ij . Since we have separated the contribution of βij in (7.32), we have that ∇q L 1

	.49)
	(7.50)
	(7.51)
	(7.52)
	gw i k

β ijm = 0. Finally, as already mentioned, all the other first order Lie derivatives are zero, so they cannot contribute to the rank of R.

•

  • angle of view narrow: 26.41 • x 26.41 • , while a Vero v1.3 camera has the following specifications • frame rate: 250 fps • resolution: 1.3 megapixel • angle of view wide: 60.8 • x 50.3 • • angle of view narrow: 32.7 • x 26.4 • .RésuméLe but de cette thèse est d'étendre l'état de l'art par des contributions sur le comportement collectif d'un groupe de robots volants, à savoir des quadrirotors UAV. Afin de pouvoir sûrement naviguer dans un environnement, ces derniers peuvent se reposer uniquement sur leurs capacités à bord et non sur des systèmes centralisés (e.g., Vicon ou GNSS). Nous réalisons cet objectif en offrant une possible solution aux problèmes de contrôle en formation et de localisation à partir de mesures à bord et via une communication locale. Nous abordons ces problèmes exploitant différents concepts provenant de la théorie des graphes algébriques et de la théorie de la rigidité. Cela nous permet de résoudre ces problèmes de façon décentralisée et de proposer des algorithmes décentralisés capables de prendre en compte également des limites sensorielles classiques. Les capacités embarquées que nous avons mentionnées plus tôt sont représentées par une caméra monoculaire et une centrale inertielle (IMU) auxquelles s'ajoute la capacité de chaque robot à communiquer (par RF) avec certains de ses voisins. Cela est dû au fait que l'IMU et la caméra représentent une possible configuration économique et légère pour la navigation et la localisation autonome d'un quadrotor UAV.

http://www.moley.com/ i

Les solutions décentralisées jouent un rôle significatif dans des applications de multi-agent puisqu'elles permettent d'utiliser des algorithmes indépendement de la taille de groupe (scalable algorithms). Cependant, même si l'importance et les avantages de décentralisation sont evidentes à la communauté de multi-robot, il est rare de trouver des solutions purement décentralisées pour des systèmes de multirobot. En effet, c'est le cas de toutes ces applications qui comptent lourdement sur des systèmes de localisation centralisés comme le système de capture de mouvement de Vicon ( Fig.1.2(a) ) ou le GNSS ( Fig.1.2(b) ). Ces applications sont très intéressantes, mais nécessitent des ajustements substantiels afin d'être déployées dans un scénario réaliste, pour lequel les systèmes centralisés ne sont pas disponibles (par exemple, dans un bâtiment détruit par un séisme, sous l'eau, sous terre, dans l'espace, ou dans un endroit où le signal GPS est faible).En robotique, les robots mobiles terrestres sont encore largement employés dans les applications multi-robots[START_REF] Howard | Experiments with a large heterogeneous mobile robot team: Exploration, mapping, deployment and detection[END_REF][START_REF] Charrow | Cooperative multi-robot estimation and control for radio source localization[END_REF], (voir Fig.1.2(c) ). Cela est principalement dû à leur coût relativement faible, leur facilité d'utilisation, mais aussi leur sécurité (d'habitude) intrinsèque. Pour toutes ces raisons, à leurs débuts, les applications multi-robots permirent de mettre en avant les atouts des systèmes multi-agents, ainsi que les verrous technologiques inhérents à leur élaboration. Le principal inconvénient 2 Dans cette Thèse le terme anglais bearing se réfère à une mesure non métrique, obtenue en temps-réel via l'utilisation d'une caméra montée sur le drone et visualisant les autres drones de la formation. Cette mesure est alors représentée par le vecteur unitaire pointant vers le drone visualisé.

In this Thesis the term bearing refers to the non-metric information that can be instantaneously recovered from an onboard camera looking at other UAVs in the scene (that is, the unit bearing vector pointing towards a UAV).

www.gartner.com

www.mbzirc.com, www.robocup.org, www.eurathlon.eu

Visual inputs, and therefore bearing measurements are non-metric measurements.

California Partners for Advanced Transit and Highways, www.path.berkeley.edu

www.vislab.it

http://www.precisionhawk.com/

https://www.aeryon.com/

Note that decentralized is often used as a synonym of distributed.

International Civil Aviation Organization (ICAO). Unmanned Aircraft Systems (UAS); ICAO: Montreal, QC, Canada, 2011.

When a UAV is a hybrid between a fixed-wing and rotary-wing UAV is usually addressed as compound .

www.octavioaburto.com/cabo-pulmo

The meaning of the term configuration depends on the application. It can refer to positions (in R 2 or R 3 ), positions and orientations and so on.

Note that some works[START_REF] Franchi | Probabilistic mutual localization in multi-agent systems from anonymous position measures[END_REF][START_REF] Cognetti | 3-D mutual localization with anonymous bearing measurements[END_REF] use the specific term relative mutual localization (RML) to refer the cooperative localization in scenarios in which the measurements are expressed with respect to the body frames of the agents[START_REF] Zhou | Determining 3-d relative transformations for any combination of range and bearing measurements[END_REF][START_REF] Martinelli | Closed-form solutions for attitude, speed, absolute scale and bias determination by fusing vision and inertial measurements[END_REF], like the one of this Thesis.

Directed graphs are often referred as digraphs.

More formal definitions of neighbors, in different senses, can be found in[START_REF] Bullo | Distributed control of robotic networks: a mathematical approach to motion coordination algorithms[END_REF].

This formula was first introduced by L. Euler in Seven Bridges of Königsberg.

Some authors use the opposite notation.

For more details refer to the website of the workshop: https://parrigidwrkshp.sciencesconf. org/.

Note that these concepts are formalized in(3.10-3.11).

This precise number of edges comes from the Laman's theorem, defined in Sect. 3.4.1.

In the literature, these cases are sometimes referred as degenerate cases.

Sometimes they are also called plane isostatic graphs.

In accordance to the definition of global rigidity given above sometimes rigidity is referred to as local rigidity in order to highlight that it is a property local to the current positions of the agents. In this thesis we refer to it simply as rigidity.

In[START_REF] Krick | Stabilisation of infinitesimally rigid formations of multi-robot networks[END_REF], the definition of infinitesimal rigidity is the one coming from[START_REF] Asimow | The Rigidity of Graphs, II[END_REF].

This can be summarized by saying that infinitesimal rigidity is not a purely topological property as connectivity.

Indeed, the direction of arrival of a radio signal can be estimated through different kinds of antennas and used for radio source localization[START_REF] Twigg | Rss gradient-assisted frontier exploration and radio source localization[END_REF].

This result cannot be generalized to R 3 or higher dimensions.

We refer to this reference frame as the body-frame of the robot

This distinction between sets Ni and Oi, needed because of the directed nature of graph G, will play an important role in Sects. 6.3.2 and 6.3.3.

Here, the term 'unscaled' means that the agent positions are estimated up to a common scale factor.

Indeed, βG(q(t)) = const only for stationary agents or for agents moving along the directions (5.1).

Note the notation difference between B and B

Indeed, a correct localization implies that ψ(t) → ψ(t) + 1N ψ (see(5.6)).

With a small abuse of notation, we use either w k or wij to indicate the weight associated to an edge e k = (i, j).

We also note that the introduction of time-varying weights wij(t) naturally induces a corresponding time-varying edge set E(t) (the set of all edges e k for which w k (t) > 0).

Two matrixes A, B ∈ R n×n are similar if there exists a matrix P ∈ R n×n such that B = P -1 AP . One of the (several) properties of similar matrixes which is exploited in the math of this chapter is that the two matrixes have the same eigenvalues (but, in general, not the same eigenvectors).

An orthonormal matrix A ∈ R n×n is such that A T A = AA T = In and equivalently A T = A -1 .

= 0.01 and λ max 6 = 0.42

We recall that, as discussed in Sect.

6.2, the interdistances dij and relative orientations ψij can be computed by any neighboring pair by exchanging the local estimates pi, pj, ψi, ψj.

Remember that in this Thesis with localization we refer to the process of determining the configuration of the agents in a common reference frame from a sparse set of relative measurements between them, and without the aid of an external centralized system (such as Vicon/GNSS).

We chose this name to stress that the scale of a formation based on bearing measurements is retrievable through dynamic information.

As for the previous chapters, a fully onboard implementation is missing, refer to Chapt. 8 for more details.

Especially when dealing with multiple quadrotor UAVs.

Where oC ∈ S

is the (constant and known) direction of the camera optical axis in the quadrotor body-frame

http://www.ros.org/

http://wiki.ros.org/matlab_ros_bridge

http://www.mikrokopter.de/en/home

https://www.ptgrey.com/

The experiments described in Chapts. 5 to 7 were performed in the room with a bigger flying volume, in Fig. B.6(b).

Acknowledgements

Part III Conclusions and Future Work

Bibliography

Vita xvii Now we are finally ready to present to the reader the main contributions of this Thesis which are divided into three chapters:

• Chapt. 5 presents a formation control and localization algorithm for a group of N quadrotor UAVs based on the theory of rigidity.

• Chapt. 6 presents a solution to the problem of maintaining rigidity in spite of sensing limitations like (i) limited field of view of the onboard cameras, (ii) limited range of the onboard cameras, (iii) possibility of occlusions between the agents of the formation during motion.

• Chapt. 7 presents a nonlinear observability analysis of the system composed of N quadrotor UAVs. The observability analysis is the first step towards the design of an Extended Kalman Filter which can also estimate the scale of the formation (and therefore the real distances between the robots) using only bearing measurements and the linear/angular body-frame velocities of the robots. shortcomings listed above: they only provide relative (and unscaled) measurements in the local body-frame of the measuring agent, and are affected by several sensing constraints such as limited field of view, limited range, and occluded visibility. Another big limitation of most of the commercial cameras is the ability to work only in environments with an appropriate luminosity. Some recent work is being conducted to solve these problems on quadrotor UAVs [START_REF] Vidal | Hybrid, frame and event based visual inertial odometry for robust, autonomous navigation of quadrotors[END_REF] by using event-based cameras (as [START_REF] Lichtsteiner | A 128 × 128 120 db 15µs latency asynchronous temporal contrast vision sensor[END_REF]) but it is outside the scope of this work.

Taking inspiration from [START_REF] Zelazo | Decentralized Rigidity Maintenance Control with Range-only Measurements for Multi-Robot Systems[END_REF], in this work we then propose a control strategy able to ensure maintenance at all times of a minimum level of bearing rigidity for the robot formation during motion. The controller consists of a decentralized gradient descent action based on a suitable "degree of bearing rigidity" which is directly related to the spectral properties of the so-called bearing rigidity matrix already introduced in Chapt. 4. Furthermore, we assume a directed measurement topology (no need of reciprocal measurements among neighboring pairs), and explicitly consider three typical sensing constraints affecting onboard cameras: Finally, we experimentally validate the proposed machinery with five quadrotor UAVs navigating in an indoor flying arena. To the best of our knowledge, the problem of bearing rigidity maintenance and the use of the complex (but quite realistic) sensing model (C1)-(C3) for camera-based formation control purposes are two novel contributions of this work.

Chapter overview

The rest of this chapter is organized as follows: Sect. 6.2 quickly recaps the localization algorithm described in Sect. 5.2.2. Then Sect. 6.3 presents the various details of the proposed bearing rigidity maintenance strategy together with a discussion on the issue of decentralization and scalability. Subsequently, Sect. 5.4 reports the results of our experimental validation with five quadrotor UAVs, and Sect. 6.5 concludes the chapter and discusses some future directions. it follows that the world-frame symmetric bearing rigidity matrix can be actually rewritten as

This shows that B W G (q) = B W G (p) and, as a consequence, that λ B 6 (q) = λ B 6 (p), thus concluding the proof.

We now provide two explicit expressions of λ B 6 (p) that is instrumental in the following developments. Let v 6 ∈ R 4N represent the world-frame normalized eigenvector associated to λ B 6 , i.e., such that (using (6.13))

, where v p i ∈ R 3 and v ψ i ∈ R are the eigenvector components associated to the position and orientation of agent i. By exploiting (6.11), the properties Pij Pij = Pij and Pij Sp ij = Sp ij (see the proof of Prop. 6.1 and [START_REF] Schiano | A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs[END_REF]), it is possible to obtain the following expression for λ B 6 (applying (6.14))

where

The expression (6.15) obviously depends on world-frame quantities. An equivalent expression in terms of body-frame quantities can be obtained as follows (look at Appendix A.1 for more details): let ν 6 represent the body-frame normalized eigenvector, i.e., such that λ B 6 = ν T 6 B G ν 6 . From (6.8) it follows that v 6 = T ν 6 and, therefore,

1 and (4.2)), the expression (6.15) can be reformulated as [START_REF] Mlot | Fire ants self-assemble into waterproof rafts to survive floods[END_REF]) is now rewritten in terms of only body-frame quantities relative to agents i and j (measured bearings β ij , interdistances d ij , relative orientations i R j and relative components of the body-frame rigidity eigenvector ν 6 ). Note that, in general, l ij = l ji . Furthermore, by looking at the (world-frame) expression of l ij in (6.15), it follows that ∂l ij /∂ψ k = 0, ∀k (in accordance with Prop. 6.1).

Let us now introduce the weights w ij into the rigidity eigenvalue λ B 6 and proceed to obtain a closed-form expression for its gradient. As explained at the beginning of Sect. 6.3, the weights w ij can be included by replacing the body-frame symmetric Chapter 7

Nonlinear observability and estimation for multi-agent systems

I

n this chapter we still consider the problem of localization in multi-agent formations with bearing only measurements but specifically analyze the fundamental observability properties for dynamic agents. The current well-established approach is based on the so-called rigidity matrix, and its algebraic properties (e.g., its rank and nullspace). This method is typically motivated using first-order derivatives, and shows, among other facts, that the global scale of the formation is not observable.

A multi-agent Extended Kalman Filter

This section describes the design of an Extended Kalman Filter (EKF) on Lie groups to empirically verify the ideas of the previous sections. In particular, the EKF will provide an estimate qi = ( pi , Ri ) of the configuration q i of each agent. There are, however, three issues that need to be addressed. First, as already seen, the state of the system can be estimated only up to a global rotation and translation. We fix this ambiguity by choosing a moving reference frame that moves with the first agent. Of course, the uncertainty associated with this agent will always be zero by construction. A more representative choice would probably be to consider an "average" reference frame placed at the centroid of the formation; this, however, would significantly complicate the derivation of the filter, and it is therefore outside the scope of this work. Second, actual implementations of any filter must work in discrete time. We therefore need to discretize our dynamical model (7.1). As in [START_REF] Loianno | Visual inertial odometry for quadrotors on se(3)[END_REF], we use a simple 1-step Euler forward approach, which is equivalent to assuming constant input velocites v i , w i between discretization instants. With this assumption, our system (7.1) becomes

where u i,k = stack(v i,k , w i,k ), dt is the length of time of the discretization interval, and exp R (•) is the exponential map at R ∈ SO(3); the exponential map can be computed as exp R (R ŵ) = R exp I ( ŵ), where the exponential at the identity exp I , which corresponds to the matrix exponential, can be computed using the Rodriguez's formula [START_REF] Ma | An invitation to 3-D vision: from images to geometric models[END_REF]. Moreover, we define an operator exp q k (τ k ) with

. This operator for the position part of q k corresponds to the operation q k + v k dt and for the rotation part of q k corresponds to the usual

Summary

The material presented in the previous chapters has been published to different international conferences:

• The work presented in Chapt. 5 has been published in [START_REF] Schiano | A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs[END_REF] and a related video can be found at https://goo.gl/ZwwMGa

• The work presented in Chapt. 6 has been published in [START_REF] Schiano | Bearing rigidity maintenance for formations of quadrotor uavs[END_REF] and a related video can be found at https://goo.gl/z4v7Bu

• The work presented in Chapt. 7 has been published in [START_REF] Schiano | The Dynamic Bearing Observability Matrix, Nonlinear Observability and Estimation for Multi-Agent Systems[END_REF] and a related video can be found at https://goo.gl/jqB1kX

Part III

Conclusions and Future Work

Chapter 8

Conclusions and future work

I

N this last chapter, we wish to review the main theoretical and experimental results achieved in the Thesis and point out some issues which are still left open. Regarding the latter, we also intend to indicate possible directions to follow for further investigation and research.

Summary and contributions

The goal of this Thesis was mainly to study the problems of decentralized formation control and localization of a group of quadrotor UAVs through relative measurements. The relative measurements on which we focused our attention were 3D bearings, that is, what one can recover from monocular cameras mounted onboard the robots. Note that in the whole Thesis we assumed that the underlying topology of the sensing graph associated with the formation is of a directed type to make our approach more prone to a real-world application. On the other hand, the underlying communication graph is supposed to be undirected, that is, if there is an edge, we assume that the robots connected through that edge are able to communicate with each other.

In Chapt. 5 we showed how to stabilize, through a decentralized controller, a group of quadrotor UAVs towards a desired formation specified in terms of bearing constraints. This was done by extending the theory of rigidity from SE(2) to the case of R 3 × S 1 and by combining some control and estimation algorithms. All this was done, up to our knowledge for the first time, in the case of non-stationary agents under the assumption that the formation remains infinitesimally bearing rigid. Furthermore, we fully characterized the null-space of the BRM. This allowed us to steer the group of quadrotors in 3D space through motions which do not alter the specified bearing constraints. Specifically, these motions consist of the so-called rigidbody-motions which consist of collective roto-translation and expansion/contraction of the whole formation.

Having understood the importance of bearing rigidity in the formation control and localization problems, we worked on the maintenance of this essential property. Therefore, in Chapt. 6, we proposed a bearing maintenance strategy which required the analytical expression of the rigidity eigenvalue and of its derivatives with respect to the configuration which we detailed in closed-form in the Thesis. This strategy was built on our previous results and was able to cope with some (realistic) sensor limitations as limited field of view of the camera, limited range of the camera and lastly the possibility of occlusions between the quadrotors during motion (a complex constraint which is quite seldom considered in the literature).

Bearing-only localization intrinsically does not allow to remove the scale ambiguity because bearings consist of non-metric information. Indeed, our localization scheme through bearing measurements, used for both the contributions mentioned above, suffered of the assumption that at least one agent needs to be able to measure its distance with respect to another agent. Because of this and under the (realistic) assumption that each robot can know its ego-motion (body frame linear/angular velocity) we decided to conduct an observability analysis of the whole system. This showed that the scale of the formation is indeed observable. To this end, we designed an EKF, directly on SE(3) which was able to estimate the poses of the agents of the formation and therefore delete the "scale ambiguity". Note that the finding that the scale is observable is in line with other works such as [START_REF] Spica | Active decentralized scale estimation for bearing-based localization[END_REF] and with the literature of Structure from Motion [START_REF] Brand | Spectral solution of large-scale extrinsic camera calibration as a graph embedding problem[END_REF][START_REF] Li | Multi-view structure computation without explicitly estimating motion[END_REF]. However, unlike the majority of other works, we tackled these problems with tools from nonlinear observability and Riemannian geometry.

Again, all the presented theoretical claims were supported by extensive simulations and experimental campaigns. The experiments were run in our flying arena (see Sect. B.2) with multiple quadrotor UAVs.

Note that, during his Ph.D., the author extensively contributed to the setup of all the needed software/hardware infrastructure which was not in place at the beginning of his Ph.D. These experimental activities allowed him to get in contact with several implementation issues, such as network issues, limited flying time and the choice of the right software infrastructure which is responsible for the overall control of the UAVs. Due to this experience, the author believes that substantial work is still needed to allow a group of flying robots for robust autonomous navigation in an unknown environment without a centralized localization system and relying only on their local skills. As we mentioned, a good example of the implementation issues which continues to be a significant bottleneck is the limited flying time of quadrotor UAVs. Indeed, current battery technology only enables flights of 10-30 min, depending on many factors such as wind and payload. There is much research going on with battery refueling on UAVs with some solutions being battery swapping [START_REF] Toksoz | Automated battery swap and recharge to enable persistent uav missions[END_REF][START_REF] Ure | An automated battery management system to enable persistent missions with multiple aerial vehicles[END_REF]

T

HIS appendix includes additional technical details about the derivations relative to Chapt. 6. The mathematical details presented here are not essential for the understanding of the maintenance strategy but give to the interested reader and idea on how we proceeded to arrive to certain results.

A.1 How to go from (6.15) to (6.16) In this section we want to show how to go from (6.15), which is reported below for convenience,

to the (6.16) reported below

This is really important because, as explained in Sect. 6.3.2, the (A.1) contains quantities expressed in the world frame (not available to the i-th agent) while (A.2) contains quantities only relative to agents i and j.

Bearing-based localization and control for multiple quadrotors Therefore, we can rewrite (A.1), knowing that v p i = R i ν p i and v ψ i = ν ψ i , as

where the term v 2 ψ i pT ij S 2 pij has been rewritten as

and with ν p ij = ν p i -i R j ν p j we have that (A.1) is equal to the (A.2) as wanted.

A.2 Useful derivatives for the computation of the derivative of λ B

6

This section contains some useful derivatives for the computation of the λ B 6 in Chapt. 6. The distance d ij between two robots can be written as

therefore, its derivative will be

(A.7) follows from (A.6) by an easy application of the chain rule. We further have

A. Additional technical details associated to Chapt. 6

If we multiply (on the left) (A.9) by R i R T i we have

and therefore it follows

In the same way we can find

Furthermore, to compute Ṗij we need to use the following

and combine it with the (A.14-A.15).

A.3 Towards the derivatives of the weights

In Sect. 6.3.2, specifically in (6.19), we refer to the gradients of the weights w ij with respect to p i , p j , ψ i , ψ j without giving further details. This section is intended to give some more details about the math involved in the computation of these weights. This should give the interested reader a hint of the rationale behind the math exploited by the controller proposed in (6.23).

Let us start from the (6.17) which is reported below for convenience

Remember that w ij is composed of different weights

I

n this appendix we present the overall software/hardware architecture used throughout this work both for the simulations and for the experiments. Note that, during his Ph.D., the author has been extensively involved in the setup of the whole architecture both on the software and on the hardware level.

B.1 Simulations architecture

The simulation architecture can be divided into two main parts depending on how the single quadrotor UAV is modeled:

(1) Quadrotor UAV modeled as a simple integrator using only Matlab and Simulink to run each simulation

(2) Quadrotor UAV modeled with a more complex model through the V-REP robot simulator [START_REF] Rohmer | V-rep: a versatile and scalable robot simulation framework[END_REF] and by using the TeleKyb framework to run each simulation (Figs. B.1 and B.2). For more details about TeleKyb the reader is referred to [START_REF] Grabe | The TeleKyb framework for a modular and extendible ros-based quadrotor control[END_REF].

In both cases Matlab and Simulink represent the main programming environment where we implemented: