System Architectures for Cooperative Teams of Unmanned Aerial Vehicles Interacting Physically with the Environment

DEPARTAMENTO DE INGENIER´ IA DE SISTEMAS Y AUTOM´ ATICA ESCUELA SUPERIOR DE INGENIER´ IA UNIVERSIDAD DE SEVILLA System Architectures for Cooperative Teams of Unmanned Aerial Vehicles Interacting Physically with the Environment por Francisco J. Real P´erez Ingeniero Industrial PROPUESTA DE TESIS DOCTORAL PARA LA OBTENCI ´ ON DEL T´ ITULO DE DOCTOR POR LA UNIVERSIDAD DE SEVILLA SEVILLA, 2022 Directores Dr.-Ing. Jes´us Capit´an Fern´andez, Profesor Titular Dr.-Ing. ´ Angel Rodr´ıguez Casta˜no, Profesor Contratado Doctor ii UNIVERSIDAD DE SEVILLA Memoria para optar al grado de Doctor por la Universidad de Sevilla Autor: Francisco J. Real P´erez T´ıtulo: System Architectures for Cooperative Teams of Unmanned Aerial Vehicles Interacting Physically with the Environment Departamento: Departamento de Ingenier´ıa de Sistemas y Autom´atica V◦B◦Director: Jes´us Capit´an Fern´andez V◦B◦Director: ´ Angel Rodr´ıguez Casta˜no El autor: Francisco J. Real P´erez iii iv A mi se˜nal, ira y fuego v vi Agradecimientos Me siento muy afortunado por haber podido compartir esta aventura con un mont´on de personas preciosas a las que les debo, como m´ınimo, este agradecimiento. A mis padres, Enrique y Paquita, que lo dan todo para ayudarme siempre, tejiendo la red de seguridad m´as impenetrable jam´as conocida. A mis hermanos, Alberto y Quique, mis primeros maestros, y que me han regalado algunas de las lecciones m´as importantes de la vida. A B´arbara, la mejor de las compa˜neras posibles, tambi´en en este viaje, y cuyo resultado –como todo lo que tengo desde que la conoc´ı– es 50% suyo. A mis hijos, Olivia, Ethan y Norah, que me muestran el rumbo a seguir cuando me desv´ıo de la ruta de las cosas de verdad importantes. A Jean-Michel, mon papaye reviewer. A mis hermanos del desierto, V´ıctor, Arturo, Pedro, Manu (I), Manu (II), Honorio, Rafa, Ale, que me hicieron sentir un gladiador llegado del futuro, y a mis compa˜neros en otras trincheras, de un pasado m´as o menos remoto: Pablo (S), Luis, Fernando, Iv´an, Fili, Juanbe, Jes´us, AE, Ricky, Edu, Pablo (R), Juli, Luisma, ´ Alvaro, Mar´ıa, Ale (S), Ale (B), H´ector, Carlos, Rebeca, Roberto, V´ıctor (Q). Gran parte del trabajo presentado en esta tesis es fruto de un esfuerzo colaborativo con todos ellos. A Capi y ´ Angel, que adem´as de hermanos del desierto, han dedicado de su tiempo a dirigir esta tesis; sin su paciencia y empuje no estar´ıa escribiendo estas l´ıneas. A An´ıbal, que tras reunir una gran cantidad de talento en un grupo de rob´otica, me ha permitido ser parte de ´el. A todos aquellos a los que seguro he olvidado mencionar, tambi´en mis disculpas, lo arreglamos en mi pr´oxima tesis :) Gracias. vii El trabajo presentado en esta tesis ha sido parcialmente respaldado por los proyectos: AERIAL-CORE (Id. 871479) y PILOTING (Id. 871542), ambos del Programa HORIZON 2020 de la Uni´on Europea, y HAERA (Ref. PID2020-119027RB-I00) del Programa Estatal de I+D+i. viii Resumen Los veh´ıculos a´ereos no tripulados (UAVs, del ingl´es Unmanned Aerial Vehicles) se han convertido en herramientas muy valiosas para un amplio espectro de aplicaciones, como inspecci´on y mantenimiento, u operaciones de rescate, entre otras. Las capacidades de un ´unico UAV pueden verse extendidas o complementadas al utilizar varios de estos veh´ıculos simult´aneamente, por lo que la tendencia actual es el uso de equipos cooperativos con m´ultiples UAVs. Para ello, es fundamental la integraci´on de diferentes autopilotos, plataformas heterog´eneas, y componentes software –que dependen de la aplicaci´on–, por lo que se requieren arquitecturas multi-UAV que sean flexibles y adaptables a las necesidades del equipo. En esta tesis, se desarrollan arquitecturas para equipos cooperativos de UAVs, prestando una especial atenci´on a aplicaciones que requieran de interacci´on f´ısica con el entorno, cuya naturaleza es t´ıpicamente no estructurada. Primero se proponen capas para abstraer a los componentes de alto nivel de las particularidades del hardware. Luego se desarrollan arquitecturas cada vez m´as avanzadas, desde una arquitectura de navegaci´on para un ´unico UAV, hasta una para un equipo cooperativo de UAVs heterog´eneos. Todo el trabajo ha sido minuciosamente probado, tanto en simulaci´on como en experimentos reales, en diferentes y complejos escenarios motivados por proyectos de investigaci´on y competiciones de rob´otica. En la mayor´ıa de las aplicaciones se requer´ıa de interacci´on f´ısica con el entorno, que es normalmente un escenario en exteriores no estructurado. A lo largo de la tesis, se comparten todo el conocimiento adquirido y las lecciones aprendidas en el proceso, y el c´odigo relevante est´a publicado como open-source. ix xvi Contents 5.6.3 System architecture and integration . . . . . . . . . . . . . . . 116 5.6.4 System coordinates and geofencing . . . . . . . . . . . . . . . 117 5.6.5 Communication and network configuration . . . . . . . . . . . 118 5.6.6 Multi-UAV object estimation and allocation . . . . . . . . . . 119 5.6.7 Picking up objects . . . . . . . . . . . . . . . . . . . . . . . . 121 5.7 Conclusions ................................ 122 6 A team of heterogeneous UAVs for missions with physical interaction 127 6.1 Introduction................................ 128 6.2 Relatedwork ............................... 130 6.2.1 Multi-UAV architectures . . . . . . . . . . . . . . . . . . . . . 131 6.2.2 UAV teams for construction-like applications . . . . . . . . . . 132 6.2.3 UAV teams for search & rescue applications . . . . . . . . . . 133 6.3 Problemstatement ............................ 134 6.3.1 Challenge 2: cooperative construction . . . . . . . . . . . . . . 135 6.3.2 Challenge 3: autonomous fire-fighting . . . . . . . . . . . . . . 137 6.4 Software architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . 139 6.4.1 Components............................ 143 6.4.2 Actions .............................. 156 6.4.3 Tasks................................ 163 6.4.4 Behavior dispatcher . . . . . . . . . . . . . . . . . . . . . . . . 165 6.4.5 Communication.......................... 169 6.5 Hardwaresystems............................. 171 6.5.1 Aerialplatform .......................... 171 6.5.2 Grasping mechanism . . . . . . . . . . . . . . . . . . . . . . . 173 6.5.3 Blanket deployment system . . . . . . . . . . . . . . . . . . . 175 6.5.4 Water jet extinguishing system . . . . . . . . . . . . . . . . . 178 6.6 Experimental results of Challenge 2 . . . . . . . . . . . . . . . . . . . 180 6.6.1 Simulation experiments . . . . . . . . . . . . . . . . . . . . . . 181 6.6.2 Mock-up experiments . . . . . . . . . . . . . . . . . . . . . . . 182 Contents xvii 6.6.3 Results from the MBZIRC competition . . . . . . . . . . . . . 186 6.7 Experimental results of Challenge 3 . . . . . . . . . . . . . . . . . . . 188 6.7.1 Simulations ............................ 188 6.7.2 Mock-up experiments . . . . . . . . . . . . . . . . . . . . . . . 189 6.7.3 Results from the MBZIRC competition . . . . . . . . . . . . . 195 6.8 Lessonslearned .............................. 197 6.9 Conclusions ................................ 200 7 Conclusions and future developments 203 7.1 Conclusions ................................ 203 7.2 Future developments . . . . . . . . . . . . . . . . . . . . . . . . . . . 206 References 209 xviii Contents List of Figures 1.1 Classification of the contents of each chapter, according to the nature of the main proposed solution and the number of UAVs involved. . . 5 1.2 Timeline of the thesis with the main related projects and publications. 9 1.3 Experiments for aerial manipulation in a mockup scenario emulating pipes in a plant. A real and a simulated environment are depicted. . . 9 1.4 UAV measuring beam deflection in a bridge. Left, UAV trajectory during the mission with a point-cloud captured by the Total Station. Right, snapshot of the UAV stuck to the beam. . . . . . . . . . . . . 10 1.5 Simulated mockup scenario for MULTIDRONE where two UAVs follow a car taking different types of shots. Views from the two onboard camerascanbeseen. ........................... 11 1.6 Left, competition arena for MBZIRC 2017. A multi-UAV team must find, pick, and place a set of objects. Right, a rehearsal of the Iberian Robotics team in MBZIRC 2020. . . . . . . . . . . . . . . . . . . . . 12 2.1 Examples of a foldable quadcopter (left), a hexacopter with tilted rotors (center), and a coaxial octocopter (right). . . . . . . . . . . . . . . . . 16 2.2 Flight mechanics for a quadcopter. . . . . . . . . . . . . . . . . . . . 17 2.3 Ardupilot attitude control scheme (source: https://ardupilot.org). 20 2.4 Position controller diagram in PX4 (source: https://docs.px4.io). 20 2.5 Examples of a hexacopter frame (left) and a landing gear (right). . . 22 2.6 Examples of two different Power Distribution Boards. . . . . . . . . . 23 2.7 A brushed (left) and a brushless outrunner motor (right). . . . . . . . 24 xix xx List of Figures 2.8 A typical Electronic Speed Controller (ESC). . . . . . . . . . . . . . . 24 2.9 A pair of CW/CCW 15 ×5 carbon-fiber propellers. . . . . . . . . . . 25 2.10 Typical UAV batteries, from left to right: LiPo, LiFe, Li-ion, and NiMh. 26 2.11 Communication device examples, from left to right: Futaba RC transmitter, a pair of 900 MHz radio modems, and the Ubiquiti Bullet M. ..................................... 27 2.12 A snapshot of the graphical interface of the QGroundControl GCS. . 30 3.1 Scheme with the different layers to implement the back-ends in UAL. UAL and Backend layers are common, while DerivedBackend is autopilot/protocolspecific. ........................... 42 3.2 UAL offers a double interface with the user, either as a class or as a ROS server responding to requests. . . . . . . . . . . . . . . . . . . . 43 3.3 Results of benchmark experiment to compare time performance for the class and service UAL interfaces. Median values are indicated in red within blue boxes delimited by the 25th and 75th percentiles. The whiskers represent the extreme values and the red crosses outliers. . . 44 3.4 Scheme with the existing back-ends for UAL. Each implemented backend can communicate with different autopilots using the same protocol. Additional back-ends can be added easily to extend UAL reachability. 45 3.5 Screenshot of the output of the ROS command rqt tf tree when running an example test of UAL with 3 UAVs. . . . . . . . . . . . . . 46 3.6 Layout for a standard joystick. Left, the 6 axes used; right, the 12 buttonsused. ............................... 47 3.7 Interaction diagram with the modules required to integrate UAL with UE...................................... 50 3.8 Realistic simulation using Unreal Engine in a mountainous environment. 51 3.9 Interaction diagram with the modules required to integrate UAL with theDJIHITL................................ 52 3.10 Flowchart of the UAL state update function for the MAVROS back-end. 55 List of Figures xxi 4.1 Oil and gas refinery used as test scenario for inspection and maintenance operations.................................. 62 4.2 ARS platform designed for the AEROARMS project. . . . . . . . . . 63 4.3 ARS-LRM platform designed for the AEROARMS project in simulation (left), and while performing a real experiment (right). . . . . . . . . . 63 4.4 Scheme with the three layers to implement back-ends in ACI. . . . . 65 4.5 ACI offers a double interface: it can be used as a class or as a ROS server. 65 4.6 Scheme with the existing back-ends for ACI. Additional back-ends can beeasilyadded............................... 66 4.7 Overall navigation architecture for aerial manipulators. The plan computed by the Planner is carried out by a Reactive Collision Avoidance module. .................................. 67 4.8 Discrete configurations for the dual-arm of the ARS-LRM. As it navigates, the system can switch between these postures for obstacle avoidance. 68 4.9 The control commands provided by the navigation architecture are translated for the specific autopilot and manipulation device through UAL and ACI, respectively. . . . . . . . . . . . . . . . . . . . . . . . 69 4.10 Left, desired velocity vector vp with its azimuth and polar angles. Right, triangular probability distribution used to sample velocity module with Kv= 0.8. ................................. 71 4.11 Experimental data comparing actual (blue) and modeled (red) forward speed ( vx ) and yaw rate of the ARS. The dynamic model is computed by system identification and used to predict the UAV trajectory. It can be seen that predictions fairly match the actual values. . . . . . . . . 72 xxii List of Figures 4.12 Trajectory evaluation. Collision volumes in the current and goal positions are represented as red and blue cylinders, respectively. If the UAV executed vp (blue arrow), it may lead to a collision with the obstacles perceived as a point-cloud (red squares). N velocity vectors ( vi ) are sampled. In black, predicted trajectories leading to collision; in red, trajectories with high values of Jik ; in green, trajectories with low values of Jik . The velocity vi corresponding to the lowest Jik is selected as velocity command (green arrow). . . . . . . . . . . . . . . . . . . . 74 4.13 Simulated scenario with several pipes emulating an industrial facility for inspection. The initial plan colliding with unexpected obstacles is shown.................................... 76 4.14 Simulation with the ARS-LRM switching to home configuration (left) and zero configuration (right) while navigating. . . . . . . . . . . . . 77 4.15 Real indoor scenario with parallel pipes (left) and static map without parallelpipes(right)............................ 77 4.16 Start and goal waypoints for the indoor validation experiment, together withtheinitialplan. ........................... 78 4.17 Arms postures for the ARS platform in the indoor tests: home (left) and zero (right), which is the preferred one by the Planner. . . . . . . 78 4.18 The system surrounds the obstacle with the arms down. A picture of the experiment (left) and the executed trajectory (right). Red dots represent the point-cloud map. . . . . . . . . . . . . . . . . . . . . . . 79 4.19 Executed x−ytrajectory by the UAV in the indoor test 1. . . . . . . 80 4.20 The system goes above the obstacle with the arms up. A picture of the experiment (left) and the executed trajectory (right). . . . . . . . . . 80 4.21 Executed x−y trajectory by the UAV in the indoor test 2 (left) and evolution of z in time (right), with posture switches depicted in yellow. 81 4.22 Scenario for field experiments. The start position is depicted in red and the goal position in blue. A non-mapped U-shaped pipe is placed between both locations. . . . . . . . . . . . . . . . . . . . . . . . . . . 82 List of Figures xxiii 4.23 The ARS goes above the obstacle and switches arms configuration. In blue the initial planned trajectory, in green the executed one. Top view (left) and altitude evolution (right) of the trajectory, indicating in yellow the kindex of the arms posture. . . . . . . . . . . . . . . . . . 83 4.24 The ARS goes around the obstacle and keeps the same arms configuration. In blue the initial planned trajectory, in green the executed one. Top view (left) and altitude evolution (right) of the trajectory, indicating in yellow the kindex of the arms posture. . . . . . . . . . 84 5.1 Competition site in Abu Dhabi. Left, top view of one of the arenas. The dropping box is white and lies in the middle of one of the track laces. Right, a UAV trying to pick up one of the moving objects. In the background, a green static object has fallen down from its pedestal. Images from the MBZIRC organization. ................. 93 5.2 Block diagram of the proposed system architecture. Left, modules on the Ground Control Station and communication links with the UAVs. Right, detailed view of the blocks on board each UAV. . . . . . . . . 94 5.3 Left, color space in RGB and HSV. Right, division of the HSV color space into clusters. For instance, the cluster depicted in red can represent a blue color in a simple manner. However, in RGB, that color has to be defined by a region separated with a tilted plane. . . . . . . . . 96 5.4 Output of the vision-based object detector. Left, original image from the arena with a red and an orange object. Bounding boxes and a text displaying extra information overlay the image. Right, processed image after the color segmentation. The two objects appear segmented and the background as black. . . . . . . . . . . . . . . . . . . . . . . . . . 97 xxiv List of Figures 5.5 Scheme of the arena for MBZIRC: DZ represents the Dropping Zone, where the box is placed; LZ represents the Landing Zone, where UAVs start the mission. Left, an example of the paths followed by three UAVs during the search phase to cover the whole arena (they go and return to the start position). Right, in red the roundabout around the DZ with the six waiting spots for the UAVs loaded with objects. . . . . . 103 5.6 Diagram of the UAV State Machine. States are represented by rectangles and transitions by arrows. The circles represent UAL commands to the UAV with specific parameters. . . . . . . . . . . . . . . . . . . . . . . 106 5.7 Aerial platform developed for the Challenge. Left, airframe without the mission electronics and the pickup mechanism. Right, fully equipped hexacopter. ................................ 108 5.8 Front view (left) and side view (right) of the aerial platform with all the sensors and electronic devices on board. The spatial distribution of the equipment is indicated. . . . . . . . . . . . . . . . . . . . . . . . . 109 5.9 Detailed view of the core of the aerial platform. Left, Intel NUC computer with plastic case. Right, Pixhawk autopilot on top of the dampedbase. ............................... 110 5.10 Prototype of the pickup mechanism. Left, detail of the damped platform. The EPM is grabbing a metallic disk and it has a certain degree of flexibility. Right, the holder with a single EPM and a contact sensor. 113 5.11 Preliminary tests in Seville (Spain). A DJI F550 aerial platform with our pickup mechanism transporting a red object (top) and our final custom-made hexacopter transporting a blue object (bottom). . . . . 114 5.12 Final version of the pickup mechanism. In the middle of the carbon lattice, the red holder is mounted on the damped structure. The holder has four magnets on top, two contact sensors, and a lever in the middle actuated by a servomechanism. . . . . . . . . . . . . . . . . . . . . . 115 List of Figures xxv 5.13 The MBZIRC arena with the [arena] coordinate system. It can be seen that the coordinates are aligned with the arena and can have a rotation with respect to the north. In gray, the valid area for the geofencing tool is also shown. Objects out of that area were not considered for estimation nor collection. . . . . . . . . . . . . . . . . . . . . . . . . . 117 5.14 Results of the Object Estimator during a trial in Abu Dhabi with two UAVs. In the middle, some images taken from the UAVs during the experiment (each row comes from a UAV). Green marks indicate detections from the Vision Module. On top, the objects estimations after the search phase of both UAVs and with the Estimator parameters properly adjusted. At the bottom, the same without adjusting the parameters correctly. Each object has a number associated and a circle with the estimated position covariance. The color of the circle represents the most likely color according to the filter. . . . . . . . . . . . . . . . 124 5.15 Results of a successful operation to pick up a red object autonomously. On top left, a frame of the original image with the results of the vision detector. On top right, the frame segmented. At the bottom, the evolution of the horizontal position errors. Those errors are measured with respect to the image center and normalized. The yellow line indicates the time instant corresponding to the example frames. . . . 125 6.1 View of the arena for the MBZRIC Challenge 2. The structure to build Wall 2 has four segments, and the piles of bricks for the UAVs can be seen in front of the wall. . . . . . . . . . . . . . . . . . . . . . . . . . 135 6.2 Pictures of the MBZIRC arena for Challenge 3. Left, general view with ground fires. Right, high-rise building with windows and facade fires. 137 6.3 Scheme of the proposed multi-agent architecture. There are three different levels of software modules above the hardware level. Each Agent offers its own Actions and Tasks. Optionally, one of the Agents could be a GCS running centralized components for coordination. . . 141 2Introduction distributing complexity in layers and hoping that each layer removes the unnecessary underlying details and nuances while maintaining the functionalities required by the layers above. They give structure to the way we solve a problem, organizing the pieces of the final puzzle in such a way that it is easier and faster to add, remove, or modify any of them, improving the overall solution while minimizing side effects. This thesis proposes the application of such architectures and abstractions in order to solve some of the problems that arise in complex robotic systems, like those composed by teams of Unmanned Aerial Vehicles (UAVs), especially in applications that require interacting physically with the environment. Throughout the document, by physical interaction we mean any interaction that goes beyond sensing and implies some actuation on the environment, modifying it in some way. For instance, moving objects from a start point to a different location, or throwing water to extinguish a fire, imply physical interaction of a robot with its surroundings. In the last decades, the use of UAVs is becoming quite popular for a wide range of applications. Many of these applications, like for instance, autonomous construction (Augugliaro et al., 2014), contact-based inspection (Tognon et al., 2019), or package delivery (Grzybowski et al., 2020; Das et al., 2020), require physical interaction with the environment. In many others, like fire-fighting (Merino et al., 2012), search & rescue (Alotaibi et al., 2019), or goods delivery in disaster situations, the operation requiring physical interaction has also to be executed in a hazardous environment, and UAVs could be key to reach places of difficult access more efficiently. Multirotors are particularly suited for all these tasks, as they can hover in-place and present Vertical Take-Off and Landing (VTOL) capabilities. Another recent trend is using cooperative teams of multiple UAVs for the aforementioned applications, as a way to optimize execution time and operational possibilities by running multiple tasks in parallel. It is especially interesting the cooperation between heterogeneous vehicles with complementary sensing and actuation capabilities. This can be helpful to minimize the time to complete the task and to extend the range of possibilities of the system, which is essential in these applications. However, the use of multi-UAV teams entails additional issues, such as inter-UAV communication or multi-UAV conflict resolution. Moreover, operating heterogeneous 1.1 Motivation 3 teams of UAVs in outdoor and unstructured environments is quite challenging. First, there is a need for specific actuation systems that are efficient and accurate enough to perform the given tasks reliably. Second, the UAVs have to act in a coordinated fashion, even in situations with unreliable communication and faulty systems. Therefore, to achieve an efficient, safe, and reliable coordination, there is a need for multi-UAV architectures that are: (i) robust to account for faulty teammates; and (ii) flexible enough to integrate heterogeneous platforms and easily accommodate new software modules. Additionally, there are several UAVs vendors, and different autopilots may use different low-level interfaces. For example, there is a very interesting proposal for communication standardization called Mavlink (Meier et al., 2013), and it is followed –with subtle differences– by two of the most used open-source autopilots (PX4 (Meier et al., 2018) and ArduPilot (ArduPilot Community, 2021)), but not by one of the top consumer drone manufacturers: DJI (DJI, 2021). Also, depending on the application, different software components and hardware capabilities of the platforms are needed, and most of the multi-UAV systems in the state of the art provide architectures that are application-specific. In this thesis, we contribute to the robotics community by providing a set of abstraction layers and multi-UAV architectures that have been developed and heavily tested in the context of several research projects. They try to help solve the problems related to the cooperation of teams of UAVs in applications requiring some kind of physical interaction with the environment. Besides, our system architectures focus on unstructured and outdoor scenarios, having been tested both in simulation and field experiments. In fact, a relevant part of the thesis is devoted to explain the experiences, results, and lessons learned during our field experimental campaigns, motivated by diverse projects of the GRVC Robotics Lab 1from the University of Seville. 1https://grvc.us.es 4Introduction 1.2 Objectives The main objective of this thesis is to develop architectures for cooperative teams of UAVs in applications that require physical interaction with unstructured environments . This is achieved by means of abstraction layers at different levels. Like other tools, abstractions and system architectures require an investment of effort and time that, hopefully, is compensated with benefits afterwards. This effort and time can be fully invested at the beginning of the project, or it can be distributed alongside the development, as the understanding of the problem evolves. In this thesis, we followed the latter strategy, which is somehow a more organic approach for system design, where the way to find a solution is not always a straight line. Thus, we approached the problem following several development phases, in order to build a sequence of architectures with an increasing level of complexity. We can split the main objective of this thesis into the following sub-objectives: • Proposing a common framework to operate different types of UAVs and aerial manipulation devices. This software framework should provide an abstraction layer that enables to interface the low-level control without dependencies on the actual low-level hardware system. For example, it should allow us to take-off or land UAVs regardless of their autopilot system on board. • Developing a flexible and adaptable multi-UAV architecture for diverse applications implying physical interaction; flexible to accommodate different types of new components, and adaptable to be reusable in different applications. • Addressing the problem of integrating heterogeneous sensors, vehicles, and actuators into the architecture. In the context of multiple UAVs, also designing the architecture to be reliable and robust to communications issues and robot failures. • Validating all our developments through extensive campaigns of field experiments, given that we focus on applications in unstructured and outdoor environments. 1.3 Contributions 5 Figure 1.1: Classification of the contents of each chapter, according to the nature of the main proposed solution and the number of UAVs involved. • Sharing with other practitioners our conclusions and lessons learned throughout the development and experimentation process of the architecture. To sum up, we want to build system architectures that allow us to integrate heterogeneous aerial platforms mounting different autopilots, and that are flexible and adaptable enough to suit multiple applications for multi-UAV teams. We achieve that by abstracting components and tasks, so that the high-level system requirements can be adapted to particular low-level specifics. Furthermore, we have to make sure that the integrated UAVs, with different sensors and actuation capabilities, can interact physically with the environment. Last, we aim to test all the developments in real multi-UAV teams for different outdoor applications. 1.3 Contributions In this section, we describe the main contributions of this thesis, classified by chapter, and along with the associated publications. Figure 1.1 depicts a scheme with the focus of each chapter. In Chapter 2 , we describe some background needed to better understand the opportunities and challenges that arise from the use of multi-UAV teams in applications with physical interaction. The typical structure and components of UAVs from the control, hardware, and software points of view are discussed. From flight control mechanics to autopilots, from propellers to payloads, and from localization systems to planning software modules, basic descriptions of the pieces that build up the UAV puzzle are introduced. Finally, we try to understand why the requirements of 6Introduction multi-UAV teams and physical interaction may render some applications especially challenging. In Chapter 3 , we present our UAV Abstraction Layer (UAL) as a tool to operate with heterogeneous autopilots. The key contribution of this work is to abstract highlevel algorithms from the specifics and low-level nuances of different autopilots. This is motivated by the spirit of reuse and improvement, and it tries to save hours of tedious job adapting software to different autopilots. After distilling the most-used functionalities of a UAV and their parameters, a common interface is defined, and different back-ends for different autopilots are implemented. The main results of this chapter have been published in: • Fran Real, Arturo Torres-Gonz´alez, Pablo Ram´on-Soria, Jes´us Capit´an, and An´ıbal Ollero. UAL: an abstraction layer for unmanned aerial vehicles. In International Symposium on Aerial Robotics, 2018. • Fran Real, Arturo Torres-Gonz´alez, Pablo Ram´on-Soria, Jes´us Capit´an, and An´ıbal Ollero. Unmanned aerial vehicle abstraction layer: An abstraction layer to operate unmanned aerial vehicles. In International Journal of Advanced Robotic Systems, 17, pages 1-13, 2020. In Chapter 4 , we describe an architecture proposed for an aerial manipulator platform. Focusing on a single-robot system, this architecture includes a dual-arm manipulator that enables physical interaction with the environment. For such a device, the Arms Control Interface (ACI) abstraction layer is proposed as a software tool to simplify the interaction between high-level manipulation algorithms and the manipulators themselves. Also, a reactive algorithm for collision avoidance with the aerial manipulator is presented. The results of this chapter have been published in: • Fran Real, ´ Angel Rodr´ıguez-Casta˜no, and Jes´us Capit´an. A Montecarlo Reactive Navigation Algorithm for a Dual Arm Aerial Robot. In Robot 2017: Third Iberian Robotics Conference, pages 780-790, Springer International Publishing, Advances in Intelligent Systems and Computing 693, 2017. 1.3 Contributions 7 •´ Alvaro Caballero, Alejandro Su´arez, Fran Real, V´ıctor M. Vega, Manuel B´ejar, ´ Angel Rodr´ıguez-Casta˜no, and An´ıbal Ollero. First Experimental Results on Motion Planning for Transportation in Aerial Long-Reach Manipulators with Two Arms. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 8471-8477, 2018. • Elisabetta Cataldi, Fran Real, Alejandro Su´arez, Paolo Di Lillo, Francesco Pierri, Gianluca Antonelli, Fabrizio Caccavale, Guillermo Heredia, and An´ıbal Ollero. Set-based inverse kinematics control of an anthropomorphic dual arm aerial manipulator. In International Conference on Robotics and Automation (ICRA), pages 2960-2966, 2019. • Alejandro Su´arez, Fran Real, V´ıctor M. Vega, Guillermo Heredia, ´ Angel Rodr´ıguez- Casta˜no, and An´ıbal Ollero. Compliant Bimanual Aerial Manipulation: Standard and Long Reach Configurations. In IEEE Access, 8, pages 88844-88865, 2020. In Chapter 5 , we present a first architecture for multi-UAV missions with physical interaction. This first approach was developed and tested in the context of the 2017 edition of the Mohamed Bin Zayed International Robotics Challenge (MBZIRC), and it allowed us to lay the foundations, both from the hardware and software points of view, for many projects that came after. MBZIRC 2017 proposed a use case where a team of homogeneous UAVs had to cooperate to search, track, pick, and drop a set of static or moving objects. However, we contribute a multi-UAV architecture that could also suit other related applications, due to its flexibility and adaptability. The main contributions of this chapter have been published in: •´ Angel Rodr´ıguez-Casta˜no, Fran Real, Pablo Ram´on-Soria, Jes´us Capit´an, V´ıctor M. Vega, Bego˜na C. Arrue, Arturo Torres-Gonz´alez, and An´ıbal Ollero. Al- Robotics team: A cooperative multi-unmanned aerial vehicle approach for the Mohamed Bin Zayed International Robotic Challenge. In Journal of Field Robotics, 36: 104-124, 2019. In Chapter 6 , we derive a second architecture for multi-UAV missions with physical interaction. This architecture was developed and tested in the context of 8Introduction MBZIRC 2020, for use case applications related to cooperative construction and fire-fighting. The architecture implies more heterogeneity and modularity, as the considered team of UAVs is heterogeneous. In this second architecture, we leverage the lessons learned throughout the work in Chapter 5, in order to achieve a system that is more robust to UAV failures and communication issues. The main results of this chapter have been published in: • Fran Real, ´ Angel Rodr´ıguez-Casta˜no, Arturo Torres-Gonz´alez, Jes´us Capit´an, Pedro J. S´anchez-Cuevas, Manuel J. Fern´andez, Manuel Villar, and An´ıbal Ollero. Experimental Evaluation of a Team of Multiple Unmanned Aerial Vehicles for Cooperative Construction. In IEEE Access, 9, pages 6817-6835, 2021. • Fran Real, ´ Angel Rodr´ıguez-Casta˜no, Arturo Torres-Gonz´alez, Jes´us Capit´an, Pedro J. S´anchez-Cuevas, Manuel J. Fern´andez, Honorio Romero, and An´ıbal Ollero. Autonomous Fire-fighting with Heterogeneous Team of Unmanned Aerial Vehicles. In Field Robotics, 1, pages 158-185, 2021. Finally, Chapter 7 summarizes the conclusions of this thesis, and discusses directions for future research. 1.4 Thesis framework The work in this thesis followed a multi-step design process. The proposed abstraction solutions and architectures were not strictly designed in a single process, but on the contrary, they have evolved in the context of different related projects through the selective pressure of reuse and improvement. In fact, these projects have served as a framework for the developments in this thesis, and for their experimental validation. In the following, we briefly describe the main applications and related projects that inspired this thesis, and which framed the development and experimental evaluation of the proposed UAV solutions. Figure 1.2 sketches a timeline of the thesis with the main related projects and publications. 1.4 Thesis framework 9 Figure 1.2: Timeline of the thesis with the main related projects and publications. 1.4.1 Aerial manipulation Aerial manipulation is a relatively new and rather active research field in robotics where physical interaction is key. In the European project AEROARMS 2 , a UAV equipped with a dual robotic arm operated in complex industrial environments for inspection and maintenance. Within the framework of this project, we used UAL to operate that aerial platform, and we developed ACI as an extension interface to operate the robotic arms. Figure 1.3 shows some example experiments with the aerial dual-arm system in a mockup scenario. Furthermore, most of the contributions in Chapter 4 were experimentally validated in the framework of AEROARMS. Figure 1.3: Experiments for aerial manipulation in a mockup scenario emulating pipes in a plant. A real and a simulated environment are depicted. UAL was also tested in another European project on aerial manipulation, the HYFLIERS project 3 . This project aimed to develop teams of UAVs to perform 2https://aeroarms-project.eu 3https://www.oulu.fi/hyfliers 10 Introduction autonomous inspection of industrial environments. Particularly, UAL was used to control a UAV to autonomously perch on pipes using a zenithal camera. Eventually, the project objective was to inspect pipes using contact sensors and even to interact with them. 1.4.2 Autonomous inspection Figure 1.4: UAV measuring beam deflection in a bridge. Left, UAV trajectory during the mission with a point-cloud captured by the Total Station. Right, snapshot of the UAV stuck to the beam. The European project AEROBI 4 aimed to automate the inspection of bridge concrete beams and piers by using flying unmanned robots equipped with manipulators. For instance, measuring the deflection of bridges is a tedious operation in which an operator places a tool on the beam with a pole or aided with a crane. The tool is a prism which is used by a Total Station 5to accurately measure positions. One of the objectives of the project was to use a UAV equipped with such a prism. As described in S´anchez-Cuevas et al. (2017b), the UAV can use the drag forces generated by the propellers in the proximity of the ceiling to remain stuck to the beam. Thus, the Total Station can measure any deformation of any beam at any bridge without putting at risk neither any human operator nor machine. In that context, the UAL framework was used to automate the control and movement of the aerial platform during experimental missions. Many of the lessons 4http://www.aerobi.eu 5https://leica-geosystems.com/products/total-stations 1.4 Thesis framework 11 Figure 1.5: Simulated mockup scenario for MULTIDRONE where two UAVs follow a car taking different types of shots. Views from the two onboard cameras can be seen. learned in the design process of the AEROARMS architecture were also useful in this project. Figure 1.4 shows an experiment where the UAV follows a trajectory with three contact points chosen to measure the deflection of a bridge beam. Again, physical interaction is crucial for this application. The Spanish project INSPECTOR is another project to use UAVs for inspection and maintenance of unattended facilities. In one of the particular use cases, we developed an outdoor platform to inspect large areas of gas pipelines searching for leaks. We based our system on commercial platforms by DJI and integrated our DJI UAL back-end with an A3 autopilot. 1.4.3 Multiple drones for media production The European project MULTIDRONE 6 aimed at building teams of multiple UAVs to cover outdoor sports events, such as cycling or rowing races. This is an example of a multi-UAV application. The MULTIDRONE architecture was inspired by the one proposed in Chapter 5, and UAL was a core component. The MULTIDRONE consortium developed algorithms that translated the ideas of the media production team into autonomous plans and control actions, so that the UAVs shot the event with their onboard cameras (Torres-Gonz´alez et al., 2017). All partners adopted UAL to interface with the UAVs and they used it to simulate 6https://multidrone.eu 18 Background Let us explain the flight mechanics for an idealized model of a quadcopter (see Figure 2.2). It has four aligned rotors distributed in four arms whose tips, where the rotors are located, lie in a common plane. The arms are equally long and form a perfect cross at 90 ◦ . Rotors on parallel arms rotate in the same direction, but two of them do it clockwise (CW), and the other two counter-clockwise (CCW). We also assume that the forward direction is one of the arms 90 ◦ bisector. If, as previously stated, each rotor is capable of exhorting some thrust and some torque, what we have is four forces and four torques acting on four perfectly symmetrical points of a plane which, in this idealized case, represents the rigid body of our quadcopter. If all identical rotors spin at the same speed, the torques are all compensated, as two rotors spin CW and the other two CCW (all with equal thrusts), so the aircraft hovers statically on air as long as the sum of all thrusts is equal to the weight of the whole quadcopter. Making the CCW rotors spin faster than the CW rotors while preserving the overall thrust, induces a net CW torque on the aircraft that makes it spin in yaw, also CW. If the rotors on the right side of the aircraft spin faster than those on the left while keeping the sum of the rotors’ torque to zero, the right side tends to ascend while the left side descends, inducing a spin on the roll angle of the overall aircraft. For the same reason, if the front rotors spin faster than the back rotors while keeping the sum of rotors’ torque to zero, a movement in pitch is induced. All these types of movements are depicted in Figure 2.2 as hover, yaw, pitch, and roll control. These movements combined enable not only rotations but also translations. The overall thrust is always perpendicular to the plane where the rotors lie, but we can tilt this plane at will by means of roll and pitch control. Once tilted, the thrust is no more aligned with the weight, and the resulting force induces a translation that we can also control. Thus, multirotors can not only stay flying in a static point (hover) but also move in any 3D direction, which results in an ideal maneuverability for many applications. This model of a quadcopter is an idealization that can be easily generalized to other types of multirotors. More complex geometries, where not all the arms have the same length, the arms are not symmetrical, or the rotors’ axes are tilted, are also 2.1 UAV low-level control 19 possible, but the same principles of forces and torques apply (Hamandi et al., 2021). Of course, in the real world other complexities arise, such as rotor dynamics control, propeller deformation, and aerodynamic effects caused by nearby objects or the UAV motion itself (S´anchez-Cuevas et al., 2017a; Fumagalli and Carloni, 2013). 2.1.2 Autopilot control stack A typical autopilot control scheme involves at least four different controllers that are usually implemented in cascade: • Angular velocity controllers, also known as angular rate or rate controllers. Through the flight mechanics explained earlier, these controllers receive the desired rate for an angle (yaw, pitch, or roll) and generate a different reference for each of the rotors, taking into account the estimated current angle rate. The rotors’ speed control is typically performed in open loop, as the speed of the propellers is usually not measured. • Angle controllers. Given a desired angle value (for yaw, pitch, or roll), these controllers generate a reference for the corresponding angular velocity controller, taking into account the current attitude estimation. • Velocity controllers. Given the desired linear velocity of the UAV and the estimation of its current velocity, these controllers generate references for the angle controllers, following the logic previously described for the flight mechanics of translation. • Position controllers. Given the desired 3D position in space and taking into consideration the estimated current position of the aircraft, these controllers generate references for the velocity controllers. Both velocity and position must be defined with respect to some frame of reference. Such frames can be attached to the aircraft, like the so-called body frame, or to some external reference, like the NED (xis North, yis East, zis Down) or ENU (East, North, Up) frames. Figures 2.3 and 2.4 depict some control schemes implemented by two different autopilots, Ardupilot and PX4. In both cases, the controllers used are PID, which is 20 Background a common choice for UAV autopilot controllers. Normally, and through the implementation of different flight modes, autopilots allow the external user to give references to any of the controllers, although position and velocity are usually enough for most applications and safer to use; especially in manual modes, where it is a human who generates the control references. Figure 2.3: Ardupilot attitude control scheme (source: https://ardupilot.org). Figure 2.4: Position controller diagram in PX4 (source: https://docs.px4.io). Finally, it is important to remark that there are two different approaches for autopilot developers. On the one hand, there is the open-hardware and open-software approach, where both the hardware design and the software are publicly available. This allows anyone to replicate, customize, or improve the solution. Still, knowledge, skills, and time are required from the user to build, test, and tune the platform. An advantage for open-source autopilots is that there is usually a lot of support coming from the corresponding developers community. Regarding open-hardware autopilots, 2.2 Hardware components 21 the Pixhawk FCU is one of the most used autopilots, and they are produced by different manufacturers, such as 3DR or CUAV. Regarding open-software autopilots, the PX4 and the ArduPilot projects have currently rather stable products with seriously active communities. On the other hand, there is what we can call the closed-solution approach, where usually not only the autopilot, both hardware and software, but also the whole aerial platform is sold as a ready-to-fly product, and little integration work is required from the final user. In that case, UAV market is currently dominated by the Chinese manufacturer DJI. Although it is also possible to buy standalone FCUs from DJI, like the A3 or the NAZA series (which sometimes enable some level of behavior customization through a Software Development Kit), most DJI products are ready-to-fly multirotors that integrate some kind of camera or enable the integration of some external payload for user applications. 2.2 Hardware components This section describes the main hardware components for multirotors. 2.2.1 Autopilots From a control point of view, an autopilot follows a classical paradigm: it is composed of sensors, actuators, and some intelligence in between that read the sensors and commands the actuators to drive the system to some desired state. From a hardware point of view: • The intelligence in between is usually implemented in a Micro-Controller Unit (MCU), which has the required input/output peripherals and is fast enough to perform the low-level flight control. • The sensors can be internal or external to the autopilot. Internal means they are integrated physically in the same piece of hardware as the MCU. This is typically the case of the Inertial Measurement Unit (IMU) or the barometer. External means they are placed somewhere else, and then connected to the MCU. This is 22 Background typically the case of the Global Navigation Satellite System (GNSS) or the laser altimeter. • The actuators are the rotors. At each control step, some signals, typically Pulse Width Modulated (PWM), are sent from the MCU to each of the rotors’ Electronic Speed Controllers (ESC). Then the ESCs generate the power signals to drive the motors to some speed. This is usually done in open loop, i.e., the actual rotation speed is never returned as a measurement to the MCU. 2.2.2 Frame The frame is like the skeleton of the multirotor, and it has to be light enough to enable longer flight times, and robust enough to enable the geometrical consistency required for stable flight. Typical materials used for its construction are carbon fiber, plastics, and aluminum. The frame determines the number and position of the rotors, and the way the multirotor stands on the floor while not flying. Examples of a hexacopter frame and a landing gear are depicted in Figure 2.5. Figure 2.5: Examples of a hexacopter frame (left) and a landing gear (right). A typical frame can be divided into a center plate (that may also integrate some power distribution traces), the arms, and the landing gear (that sometimes can also be integrated with the arms themselves). All these parts are joined through diverse means, like clamping blocks or glue, and such unions can be fixed or foldable. This foldable characteristic eases transportation while not flying (e.g., foldable arms) or, in the case of a landing gear, may be automatically activated during flight in order 2.2 Hardware components 23 not to occlude the field of view of the onboard sensors. Fixed or foldable, all joints and parts must convey some level of structural stiffness that will be challenged by the forces, torques, and vibrations that characterize the flight of a multirotor. 2.2.3 Power distribution board In order to distribute the power from the flight battery to all the different components of a multirotor, a printed circuit board known as the Power Distribution Board (PDB) is commonly used. They are usually designed to reduce cable requirements, thus saving weight for the overall UAV. They may also integrate some voltage regulators, as the onboard electronics can require different voltage levels to function. Figure 2.6 shows two different PDB. Figure 2.6: Examples of two different Power Distribution Boards. 2.2.4 Motors Motors are a key element in every UAV, and in the case of multirotors, they are usually the only moving parts of the flying system. In most cases, they are electrical brushless motors. Electrical because they use electricity as their power source, and brushless because they do not use brushes to commute the coil that is being energized while the rotor spins. Brushless motors save friction, but they require the commutation to occur somewhere else, adding a new component to the system: the Electronic Speed Controller (ESC). ESCs take the control signal from the FCU, the power from the battery, and some signal from the motor itself (indicating the position of the rotor with respect to the stator), and generate the commuted power signals to energize the proper coil and make the motor spin at a certain speed. During the design process, 24 Background batteries, ESCs, motors, and propellers should be carefully selected not only to have enough thrust to enable flight but also to be electrically compatible, in order to avoid dangerous overvoltages or overcurrents. Furthermore, brushless motors can be outrunners, meaning that the rotor is outside and surrounds the stator, or inrunners, meaning that the rotor is inside and surrounded by the stator. Figure 2.7 depicts a brushed and and a brushless outrunner motor, while Figure 2.8 depicts a typical ESC. Figure 2.7: A brushed (left) and a brushless outrunner motor (right). Figure 2.8: A typical Electronic Speed Controller (ESC). 2.2.5 Propellers Propellers are devices that, when rotated by some external agent (e.g., a motor) in some fluid (e.g., the air), generate a linear force called thrust. In the case of fixed-wing aircraft, thrust forces typically move the whole platform forward, inducing an air-wing relative speed that results in some lift forces that enable flight. In the case of rotary-wing aircraft, thrust forces literally lift the platform into the air. Propellers are in fact little wings that achieve air-wing relative speed by spinning, and the same wing aerodynamics apply to them. Thus, if the lift force results in the aforementioned thrust, the aerodynamic drag results in a torque that is also transmitted to the aircraft. In the particular case of multirotors, these torques are compensated by spinning 2.2 Hardware components 25 rotors in different directions, some clockwise (CW), some counter-clockwise (CCW), as explained in Section 2.1.1. Although other configurations are possible, propellers mounted on multirotors are typically fixed-pitch, meaning that they do not include any mechanism to actively change the blades’ angle of attack. These propellers can have two or more symmetrically distributed blades, that are built with rigid materials such as wood, plastic, or carbon fiber. The main parameters of such propellers are their size and their blades’ angle of attack, commonly indicated as the product of two numbers: D×p , where D is the diameter of the imaginary disc conformed by the propeller while rotating, and p is the advance pitch, or the theoretical axial displacement in one turn if the propeller were screwed on a solid. Bigger D means bigger propeller, and bigger p bigger angle of attack. Both parameters are usually measured in inches. Figure 2.9 depicts a pair of CW and CCW 15 ×5 carbon-fiber propellers. Figure 2.9: A pair of CW/CCW 15 ×5 carbon-fiber propellers. 2.2.6 Batteries Batteries provide power for the flying system of the UAV, and they can also be used to power other components from the payload, such as computers or sensors. They can be classified depending on the components involved in the chemical reaction that leads to their electrical capacity, for example: Lithium Polymer (LiPo), Lithium iron phosphate (LiFe or LiFePO4), Lithium-ion (Li-ion), or Nickel-metal hydride (NiMH). 26 Background The chemical reaction determines the cell voltage, and cells and batteries may be connected in series or parallel in order to achieve the required capacity, voltage, and discharge current. Figure 2.10 depicts some typical UAV batteries. Figure 2.10: Typical UAV batteries, from left to right: LiPo, LiFe, Li-ion, and NiMh. 2.2.7 Communication devices There might be different wireless communication channels simultaneously opened between a UAV and the systems on the ground: • Backup pilot link. Even if the UAV is able to perform a whole mission, from take-off to landing, autonomously, due to safety and legal reasons, a human backup pilot is always required on the ground. This pilot has usually a direct Radio Control (RC) link with the autopilot, which can be used to override automatic references and command the UAV in manual mode. This same link, or a similar one, may also be used to stop all rotors in critical situations, by a mechanism that is commonly known as the kill-switch. The interface on the ground is usually some kind of RC transmitter (see Figure 2.11) operating in the range of 2.4GHz, with some kind of Frequency-Hopping Spread Spectrum (FHSS) technology that adds robustness against interferences. • Autopilot telemetry link. The autopilot has usually an additional communication channel with the ground in order to send telemetry and receive additional commands. It can use some kind of proprietary system, like DJI’s Lightbridge, or some defined communication protocol, such as MAVLink (Meier et al., 2013), over a Universal Asynchronous Receiver-Transmitter (UART). In the latter case, a radio modem can be used for wireless communication. Frequencies suitable for long distances, such as 900 and 433 MHz, are commonly preferred for this kind 2.2 Hardware components 27 of link. An example of a pair of 900 MHZ long-range radio modems is depicted in Figure 2.11. • Onboard computer link. As we will discuss later, UAVs may carry other computers on board to add some high-level intelligence on top of the autopilot. These computers usually communicate with the autopilot itself, for example through a wired USB-to-UART connection, and with other computers on the ground, like a Ground Control Station (GCS). Network connections are usually used for this computer-to-computer communication. In cases where standard Wi-Fi does not comply with range requirements, other enhanced wireless connection technologies are used. One example is the Rocket Prism AC technology from the manufacturer Ubiquiti. It allows fine control of the data transmission over 5 and 2.4 GHz full-band. As in the RC transmitter, a precise frequency hop algorithm gives the system an improved performance in noisy environments. The Ubiquiti Bullet M, depicted in Figure 2.11, is an example of a lightweight data communication device implementing this technology. Figure 2.11: Communication device examples, from left to right: Futaba RC transmitter, a pair of 900 MHz radio modems, and the Ubiquiti Bullet M. 34 Background trajectory planners (Tordesillas and How, 2022) or by means of reactive collision avoidance (Ferrera et al., 2018). Additionally, the high-level planner should handle UAV failures, to replan the mission if needed, implementing the desired behavior of the team for each possible contingency. Although there are many options, the most common approaches for behavior implementation in multi-UAV teams are Finite State Machines (Real et al., 2021b) and Behavior Trees (Best et al., 2022). Finally, from the planning point of view, it is relevant to differentiate between homogeneous and heterogeneous teams of UAVs. Homogeneous means that all the UAVs have the same capabilities, so the planner does not need to distinguish them to allocate tasks. Heterogeneous means that there are UAVs with different capabilities, and the planner should consider these circumstances not to allocate tasks to UAVs that are not capable. This thesis consider both homogeneous and heterogeneous multi-UAV system architectures in Chapters 5 and 6, respectively. 2.4 Multi-UAV physical interaction As this thesis focuses on applications with teams of UAVs performing physical interaction with their environment, it is important to highlight the specific challenges for this kind of multi-robot systems. The use of a cooperative team of UAVs for a given task brings two main advantages. First, it opens new possibilities, as the team may carry out missions that are not feasible with a single UAV. UAVs could offer complementary capabilities if the team is heterogeneous (Real et al., 2021a), or they could just join forces to accomplish more complex tasks (Sanalitro et al., 2020). Second, a team could perform a mission faster and more efficiently than a single UAV. Even if a single robot is capable of carrying out the mission, it is more efficient using a coordinated team (Real et al., 2021b). However, the integration of multiple UAVs in a system also implies additional complexities. We need to establish protocols and methods to coordinate the team, i.e., to decide who does what. This needs to be done in such a way that the final performance is as efficient as possible. Besides, additional sensing or communication channels are necessary to discover or exchange information for coordination and 2.4 Multi-UAV physical interaction 35 conflict resolution. If the team is heterogeneous, the planning part needs to consider specific capability constraints too. In order to cope with all these issues, and also with hardware integration, there exist system architectures specific for multi-UAV teams, which is one of the final goals of this thesis. There are elaborated and complex frameworks to deal with teams of multiple aerial vehicles (Baca et al., 2021; S´anchez-Lopez et al., 2016). Some solutions even try to expand the heterogeneity concept and bring up teams of both ground and aerial robots (Zhou et al., 2015; Krizmancic et al., 2020), or handle a whole swarm of UAVs (Varadharajan et al., 2017; Chung et al., 2018). Other existing solutions, like those proposed in this thesis, pay particular attention to the open-source characteristic (S´anchez-Lopez et al., 2016; Cervera, 2019) and to the relevance of simulation (Day et al., 2015; Xiao et al., 2020). The higher-level layers in all the software solutions presented in this thesis are based on the Robot Operating System (ROS) framework (Quigley et al., 2009), and all the code is publicly available as open-source. Our system architectures are intended to be simple and flexible, by trying not to impose any particular implementation requirements beyond ROS, which is currently a de facto standard for the robotics community. Finally, as this thesis addresses multi-UAV applications involving physical interaction with the environment, there are additional challenges that need to be considered in the proposed system architectures. Interacting physically with the environment with UAVs is a complex task. Recently, several approaches have been proposed for aerial manipulation (Ruggiero et al., 2018; Ollero et al., 2022) and aerial interaction with humans (Tognon et al., 2021). This physical interaction with UAVs is particularly challenging from two main perspectives. First, from the control point of view (Sanalitro et al., 2022; Afifi et al., 2022). Small deviations from the desired trajectory, that may be acceptable in navigation when moving far from obstacles, may in this case result in a collision. Moreover, the instant the UAV touches, or even approaches, any external object, the flight dynamics –and thus the flight controllability– change drastically (Fumagalli and Carloni, 2013; S´anchez-Cuevas et al., 2017a). Second, from the hardware point of view, the physical interaction device must be adapted to the UAV requirements. Not only weight is penalized, but also how changes in inertia are 36 Background transferred as undesired torques to the aerial platform. For robotic arms, for example, the result is that they are often designed specifically to be mounted on UAVs (Su´arez et al., 2018b), and compliance (Su´arez et al., 2018a) and dynamic coupling with the aerial platform can be taken into account (Caballero et al., 2018). Chapter 3 An abstraction layer for Unmanned Aerial Vehicles This chapter presents a software layer to abstract users of UAVs from the hardware specifics of each platform and autopilot. The main objective of our UAV Abstraction Layer (UAL) is to simplify the development and testing of higher-level algorithms in aerial robotics, by standardizing the interfaces with the UAVs. UAL supports operation with PX4 and DJI autopilots (among others), who are current leading manufacturers. Besides, UAL can work seamlessly with simulated or real platforms, and it provides calls to issue standard commands such as taking-off, landing, or pose and velocity controls. Even though UAL is under continuous development, a stable version is available for public use. We showcase the use of UAL with a set of applications coming from several research projects, where different academic and industrial entities have adopted UAL as a common development framework. 3.1 Introduction In the last decades, there has been an outstanding increase in the number of applications for UAVs (Visiongain, 2011). Their current levels of autonomy and cognition make them suitable to perform many different tasks, which are usually implemented by high-level algorithms. UAV technology is advancing fast and, hence, there is a wide 37 38 An abstraction layer for Unmanned Aerial Vehicles spectrum of platforms and autopilots available for the community. This variability is also due to the specific constraints associated with each application. Platforms with different payload capacity, maneuverability, or autopilot functionalities may be required depending on the context. Nonetheless, high-level algorithms should be able to operate UAVs in a transparent manner, regardless of the autopilot or platform underneath. Otherwise, it would become too complex to maintain multiple versions of the application software depending on the particular communication protocols for each autopilot. Several frameworks for UAV operation, both proprietary and open-source, have been developed over the past few years (Sabikan and Nawawi, 2016). Some open-source organizations like Dronecode 1 have proposed standard communication protocols for UAVs, as for instance MAVLink (Meier et al., 2013). However, despite the increasing use of this protocol, there are still many autopilots that do not support it. For example, the industrial leader in UAV manufacturing DJI 2 works with its own proprietary autopilot framework. There are also elaborated and complex frameworks to deal with teams of multiple aerial (Varadharajan et al., 2017; S´anchez-Lopez et al., 2016) or heterogeneous (Michael et al., 2011) robots, or to propose complete solutions, including hardware platforms (Baca et al., 2021; Spica et al., 2013). However, our focus is more on abstracting the autopilot to make easier the development of higher-level algorithms, i.e., navigation and decision-making algorithms that operate with UAVs in a transparent manner. Royo et al. (2008) proposed a Service Abstraction Layer to ease application development with UAVs. This is a quite complete architecture that copes with every possible application and functionality related with a UAV. Inside this complex architecture, they proposed a Virtual Autopilot System (Royo et al., 2011) that tried to abstract and standardize the autopilot interface. However, there is no publicly available implementation of this work. Instead, we present a ROS-based layer which is public, open-source, ready to use, and easy to expand to other autopilots. More recently, it is also worth mentioning the work in Baca et al. (2021), as an open-source 1https://www.dronecode.org 2https://www.dji.com 3.1 Introduction 39 multi-UAV software system. This system represents a quite valuable work for UAV practitioners, as it is a complex software architecture beyond autopilot abstraction, including many modules for localization, trajectory planning, navigation, etc. Even though the system is quite interesting, it is difficult to configure for non-expert users. Instead, the objective of UAL is just implementing a simpler abstraction layer, easily configurable by external users. Additionally, it is essential to integrate UAV software frameworks with simulation environments, in order to switch from simulated to real platforms with minor efforts. Due to the fact that UAVs are more sensitive and fragile than common ground vehicles, preliminary simulations to verify the correct operation of the whole system become even more critical. For that, there are widespread simulators such as Gazebo (Koenig and Howard, 2004), V-REP (E. Rohmer, 2013), or AirSim (Shah et al., 2017), that allow researchers to test UAV and multi-UAV systems for task allocation (Kurdi and How, 2017) or path planning algorithms (Real et al., 2017), among others. MATLAB Simulink has also been proposed as a rapid prototyping environment for UAVs (Fulford et al., 2008). Simulators with realistic 3D engines (e.g., AirSim) can also be used for testing computer vision algorithms. In this chapter, we introduce our software framework for UAV operation, which provides users with an abstract interface independent of the autopilot. First, we describe a relevant set of functionalities that every UAV should implement. From this set, we define a common interface that we call UAL (UAV Abstraction Layer). For each supported autopilot, a back-end dealing with its specific features must then be created. Our framework aims to ease the development pipeline of UAV algorithms and their deployment into real platforms. The main contribution of this chapter is to explain the design of UAL and the details of its implementation, which is publicly available (GRVC Robotics Lab, 2022). We also describe how UAL provides a simple way to simulate multiple UAVs, and how its interfaces make use of real or simulated robots in a transparent way to the user. The current implementation addresses mainly multirotor platforms, but it is general enough to be extended to fixed-wing vehicles. Last, we showcase the usability and versatility of our framework by means of several use cases. UAL has been in a 40 An abstraction layer for Unmanned Aerial Vehicles continuous development process for more than 5 years, and it has been successfully tested in multiple experiments within the context of different international research projects, as it was mentioned in Chapter 1. The remainder of this chapter is structured as follows: Section 3.2 details the architecture of UAL and its core functionalities; Section 3.3 describes its simulation functionalities; Section 3.4 discusses valuable lessons learned during our field tests with UAL; Section 3.5 shows use cases on the use of UAL for some research projects; and finally Section 3.6 provides conclusions and future work. 3.2 UAL architecture and implementation This section describes the design and implementation of the main features of UAL. For more details, a thorough wiki with instructions for installation and use is available online 3. 3.2.1 Preliminaries Although their implementation details may vary drastically, all autopilots share similar functionalities. First, they are all meant to provide some level of autonomous flight. In order to achieve that, they typically implement a cascade of modules for estimating and controlling angle rate, angle, velocity, and position. Some autopilots accept external references in any of the controllers, but the most common and useful controls for high-level users are velocity, position, and yaw control. Another common concept for autopilots is the flight mode. Depending on its current state, the task that is being executed, or the set of controllers that are handling the flight, the autopilot declares to be in a defined flight mode. Typically, each mode provides some level of control to the human backup pilot (so-called safety pilot), and at least one of them permits autonomous control from an external computer. We generalize and refer to the former set as manual modes, and the latter as auto modes. Moreover, in order to 3https://github.com/grvcTeam/grvc-ual/wiki 3.2 UAL architecture and implementation 41 provide complete autonomous flights, autopilots usually implement additional basic maneuvers, such as take-off, and landing. UAL tries to abstract the user-programmer from the platform’s autopilot. With that in mind, we defined a common interface with a collection of the most used UAV functionalities: •Performing a take-off maneuver to a given height. • Going from the current position to a specified waypoint in geographical (or any other global) coordinates with a specified yaw. •Setting linear velocities and yaw rate. •Landing on the current position. •Recovering from manual flight mode. •Setting home position to the current position. •Getting the latest UAV pose estimation. •Getting the latest UAV velocity estimation. •Getting the latest UAV coordinate transform estimations. UAL builds on top of ROS, which provides libraries and tools to help software developers to create robot applications. ROS provides hardware abstraction, sensor drivers, dedicated libraries, visualizers, message-passing communication, package management, etc. The main advantages of using ROS are its widespread use among the community, and the fact that the communication between different processes and machines is easily solved. UAL was first developed on ROS Kinetic Kame, but it has been later successfully ported to Melodic Morenia, and it could be used in other versions with minor adaptation. 42 An abstraction layer for Unmanned Aerial Vehicles 3.2.2 Core functionalities Our framework consists of three basic layers (see Figure 3.1). The first layer is UAL itself and it is implemented in C++ language. The second layer is the Backend class, which establishes a common interface to UAL and is also implemented in C++. Last, in order to support each particular autopilot, a specifically derived back-end for that autopilot must be implemented in C++. This back-end communicates with the autopilot and handles specific details, offering a common interface on the user side. For instance, any autopilot that uses MAVLink as communication protocol (e.g., PX4 and Ardupilot), is currently supported via our MAVROS back-end. The MAVROS back-end communicates via MAVROS with the autopilot, and handles specific issues such as mode switching and pose reference smoothing. Figure 3.1: Scheme with the different layers to implement the back-ends in UAL. UAL and Backend layers are common, while DerivedBackend is autopilot/protocol specific. 3.2 UAL architecture and implementation 43 Additionally, UAL offers a double interface (see Figure 3.2) to be accessed by external users: • Class: the developer can instantiate an object of the class UAL , and access data and functionalities via its class interface, directly calling its member functions. • Server: at the same time, inside the instance of the class and in a separate thread, UAL can be continuously publishing data and responding to service calls, as any other node inside the ROS network. Figure 3.2: UAL offers a double interface with the user, either as a class or as a ROS server responding to requests. While the class interface is middleware independent, it is always available, and introduces no delay; the server interface depends on the middleware (ROS in our case), is available only if the server mode is enabled (it is by default), and may introduce network delays. Only one process can implement one (and only one) class interface for a certain UAV. This may be an issue to handle multiple robots. However, the server interface can be reached from any host in the network and has been successfully tested with multiple UAVs. The two interfaces are not exclusive in design nor in implementation (every function in UAL interface is thread-safe), and it might be convenient to use both of them, profiting from the advantages of each one. For example, delay-sensitive functionalities 50 An abstraction layer for Unmanned Aerial Vehicles •typhoon h480 . A small quadcopter with an RGB camera mounted on a gimbal and a depth camera pointing forwards. 3.3.2 Integration with UE The Gazebo simulator is quite extended in the robotics community for UAV simulation. However, it lacks for a realistic graphics engine, which can be essential for testing computer vision and machine learning algorithms. Therefore, we have also enhanced UAL with a back-end that provides an interface to simulate in realistic environments, using Unreal Engine (UE) (Sanders, 2016). UE is a game engine with a long tradition in the world of computer games. Its source code is publicly available and written in C++, which eases integration with UAL. A few years ago, Microsoft published AirSim (Shah et al., 2017), which is a plugin for UE so that users can simulate robots. Figure 3.7 depicts the involved modules and their interactions to integrate UAL with UE. Figure 3.7: Interaction diagram with the modules required to integrate UAL with UE. With this enhancement, UAL is also able to provide more realistic simulations to test computer vision algorithms. Thanks to the powerful rendering engine of UE, it is possible to acquire high-quality footage that can be used online to feed those algorithms. Figure 3.8 shows three example platforms that have been tested in simulation via UAL. The UAL back-end for UE includes an interface for the AirSim UE plugin, enabling UAV control in a similar manner as it does with Gazebo. Although the visual realism of the simulations is remarkably enhanced with UE, the downside is that it also increases the rendering time with respect to Gazebo. In 3.3 Simulation functionalities 51 Figure 3.8: Realistic simulation using Unreal Engine in a mountainous environment. principle, this higher computational load may jeopardize simulations to keep up with real-time requirements. Nevertheless, UE is a professional framework that is extremely optimized, making it versatile and adaptable to the existing hardware. In fact, we ran some tests with standard desktop computers to verify that simulations with UE integrated into UAL can hold with reasonable rendering times. Table 3.1 summarizes the average frequency update with different hardware configurations. The update rate of the physics engine remains stable regardless of the computer, whereas the rendering engine speed, though always slower, increases notably with the GPU power. 52 An abstraction layer for Unmanned Aerial Vehicles GTX 980 GTX 1070 RTX 2080 Physics Engine ∼300 ∼300 ∼300 Render Engine ∼25 ∼50 ∼60 Table 3.1: Average frequency (Hz) of the physics and rendering engines in UE with three different GPUs. All of them used an Intel i7 CPU. 3.3.3 Integration with DJI HITL The UAV manufacturer DJI provides a set of tools that enable Hardware-In-The-Loop (HITL) simulation for their proprietary autopilots. In the development of the DJI SDK back-end for UAL, these tools were intensively used for testing before flying real platforms with our abstraction software. Figure 3.9: Interaction diagram with the modules required to integrate UAL with the DJI HITL. In Figure 3.9, the setup required for the DJI HITL simulation and its integration with UAL are depicted. There are three major components involved: • A computer running the DJI simulation software, currently available only for Windows and OS X. It simulates sensors and actuators within a simple empty simulated world, and it communicates with the autopilot as if they were real. • A DJI autopilot hardware running the DJI autopilot firmware (currently the A3 and N3 models are compatible with HITL). These are exactly the same pieces of hardware and software that would fly on the real platform. • A computer running UAL and its DJI SDK back-end, currently only available for Linux-based operating systems. This setup has two advantages. First, it uses real autopilot hardware, so simulation is closer to reality. Second, it moves the often computationally heavy simulation, from 3.4 Lessons learned from field experimentation 53 the computer running UAL to a dedicated computer. However, these features come at a high cost: three pieces of hardware are required for a single UAV simulation. This is more complex than the aforementioned PX4 SITL solution, which allows running simulations with a single computer, even if there are multiple UAVs. Furthermore, the DJI simulation environment is too simple, and it does not support the addition or edition of scenarios, so it cannot be used to simulate missions where UAVs have to interact with the environment. 3.4 Lessons learned from field experimentation This section discusses some lessons we learned during the process of development of UAL, and throughout the multiple field experiments that we performed testing UAL with different UAVs. 3.4.1 SITL simulation for integration In general, the UAL functionality to run SITL tests with PX4 proved to be quite relevant for system integration. This ability to simulate complete multi-UAV missions is a remarkable feature to test and debug the interfaces and functionalities of all highlevel modules involved in any UAV application. Although DJI autopilots offer highly stable controllers, we consider the lack of SITL functionality a major disadvantage for this reason. In the case of platforms with PX4 embedded, our systems cannot distinguish a simulation from real-flight behavior, which accelerates the process of integration before actual flights. Once the system has been integrated via SITL, in order to jump into experiments with actual UAVs, only the gains of the low-level controllers for the aerial platforms need an additional adjustment. 3.4.2 UAL state handling It is usual practice trying to summarize the UAV state with a single variable that takes values from a finite set. For example, the set of possible states could be defined as {landed , flying} . As we wanted this concept of state to be also abstracted from 54 An abstraction layer for Unmanned Aerial Vehicles the autopilot specifics, we decided to give UAL the responsibility of both defining the set of possible states and keeping updated the current state of the UAV. In the first software versions, we tried to implement the UAL state update with a simple state machine. The initial state was landed and each of the following function calls caused a logical change in state. For example, calling the take-off function changed the state to flying , and then calling the land function turned the state again to landed . However, this approach did not work properly in field tests. Anytime the system needed to restart while flying, or when the human safety pilot had to take control to take off or land, the UAL state did not correspond to reality. This led us to the conclusion that it was more realistic to estimate the state at each update, instead of keeping a state machine running. As the function in charge of estimating the state has to deal with some autopilot specifics, it is implemented at back-end level. The states considered by UAL are the following: •uninitialized: The system is not initialized and cannot perform any task. •landed disarmed : The system is landed and the safety pilot has not armed it yet. •landed armed : The system is landed and the safety pilot has already armed it. •taking off : The system is taking off automatically. Only the safety pilot can abort this maneuver. •flying auto: The system is flying autonomously. •flying manual: The system is being flown by the safety pilot. •landing : The system is landing autonomously. Only the safety pilot can abort this maneuver. The arming process occurs before taking off and it consists of sending PWM references to the ESCs of the UAV motors, so that they start to move and enable flight. The opposite process, when the autopilot stops sending commands to the ESCs of the motors, is called disarming. 3.4 Lessons learned from field experimentation 55 As an illustrative example, the flowchart of the state update function for the MAVROS back-end is depicted in Figure 3.10. The state update is not governed by a state machine anymore, as it does not take into account previous states. In contrast, at each iteration of the update loop, it estimates the UAV state by asking the system the proper questions in the proper order. This approach ended up being more robust to system failures. Finally, UAL uses its state to check consistency during function calls. For instance, the system must be landed armed before a take-off function can be called. Figure 3.10: Flowchart of the UAL state update function for the MAVROS back-end. 56 An abstraction layer for Unmanned Aerial Vehicles 3.4.3 Go to waypoint When issuing go-to-waypoint commands with UAL, we realized that the autopilot translated them automatically into a set of set-points (references) for each of the axis (x-y-z) and yaw. As a result, not desired behaviors could arise: • If the reference is far from the current value, the autopilot controller saturates and the movement is harsh. • If we consider axis and yaw as four independent systems (as the controller usually does), each of the axes may have different dynamics. This means that each system evolves differently in time, and the path is not a straight line as one would expect. Those harsh movements could be avoided by implementing any method for reference smoothing on top of UAL, i.e., algorithms for trajectory generation and tracking. Users could implement their own algorithms for trajectory tracking and use the setPose interface to send the smoothed pose references to the autopilot. Apart from that, within the goToWaypoint interface, we implemented a default method for trajectory tracking, based on a pure pursuit algorithm with look-ahead. This method can be tuned to avoid abrupt movements due to controller saturation. Even though there are alternative methods that may work better, we implemented this one in order to provide a simple, functional, and default trajectory tracker for users not interested in using more elaborate algorithms. 3.5 Use cases In Chapter 1, we already introduced the main research projects that served as framework for this thesis. In this section, we showcase the capabilities of UAL through a series of use cases, which we extracted from those projects. In these compiled use cases, UAL was used as a core component for project integration and experimental validation. 3.5 Use cases 57 3.5.1 Aerial manipulation UAL has been used for aerial manipulation with UAVs. In the AEROARMS project, a reactive navigation algorithm (Real et al., 2017) was developed to operate UAVs with collision avoidance in confined industrial scenarios. UAL was integrated with this algorithm for UAV state estimation and velocity control, both in simulation and field experiments. In this case, this module for reactive navigation made use of the UAL class interface, calling the pose method to update the current UAV pose (and hence the map of the perceived world), and the setVelocity method to command desired velocities for obstacle avoidance. More details on this reactive navigation algorithm, and an introduction to a similar abstraction layer (in this case for operating the dual-arm manipulator), will be given in Chapter 4. 3.5.2 Autonomous inspection UAL has been adopted for autonomous inspection with UAVs in different projects and with several autopilots. In the AEROBI project, we used it with a PX4 autopilot, whereas in the INSPECTOR project, the DJI A3 autopilot was used. In the AEROBI use case, a UAV had to perform contact-based inspection to measure the deflection of bridge beam. The UAL class interface was used to make a collection of goToWaypoint calls and send the UAV to the desired measuring points. Once there, the setVelocity method was used to establish contact with the beam. In the INSPECTOR use case, UAL was used to carry out high-level exploration missions outdoors, by means of the goToWaypoint service. The DJI HITL functionality was used for software integration before field experiments. 3.5.3 Multiple UAVs for media production In the MULTODRONE project, UAL was used through its server interface. The project developed a system architecture for media production with UAVs, and there was a component in charge of controlling each UAV, gimbal, and camera. This component used a UAL server to get UAV poses and velocities, and to send velocity control commands to the autopilot. In this use case, extensive field experimental campaigns 58 An abstraction layer for Unmanned Aerial Vehicles were carried out, demonstrating the reliability of UAL in aerial cinematography scenarios (Alc´antara et al., 2020). 3.5.4 Robot competitions Our lab participated in the two first editions of the MBZIRC competition. In both editions, a team of UAVs had to perform cooperative missions to search for objects of interest: colored disks in 2017, colored bricks and fires in 2020. Planning techniques were used to generate actions to interact with these objects for collection, or extinction in the case of fires. The whole architecture was built on top of UAL, so these high-level algorithms were able to operate the UAVs by sending them position waypoints, or controlling them in velocity to collect objects. For object collection tasks, UAVs required high precision in velocity control, so we used the UAL class interface through its setVelocity method. Given the multi-robot character of this use case, the UAL server interface was also rather useful. In order to face both competitions, we first implemented simulations of all the challenges, using the UAL MAVROS back-end and the PX4 SITL functionality. Then we ran preliminary tests with our custom UAVs, which were based on a Pixhawk autopilot (version 1 in 2017 and version 2 in 2020). For that, we used the MAVROS back-end too. In 2020, we detected hardware issues with sensor readings in some Pixhawk 2 units, which made our controllers unstable. Therefore, we decided to switch the faulty UAVs to Ardupilot, which seemed to better handle this issue. Thanks to UAL, switching between autopilots was straightforward and did not affect the overall project architecture. 3.6 Conclusions This chapter has presented UAL, a framework to abstract high-level software development in UAVs, allowing users to work with different autopilots and platforms by means of common interfaces. After using UAL within the context of several research 3.6 Conclusions 59 projects, we can conclude that it eases the development of high-level algorithms. It allowed us to operate in a transparent manner a wide variety of aerial platforms with autopilots from the major manufacturers. Researchers from different organizations have provided positive feedback and found useful UAL as an enhanced middleware. Moreover, the functionality to interface simulated or real UAVs, in the same way, has proved to be quite helpful. UAL has a stable version that is publicly available as open-source software (GRVC Robotics Lab, 2022). However, UAL is in continuous development, adding new features and fixing issues as they are detected. This development is profiting from the use of UAL by the robotics community. In fact, UAL was conceived as a modular and adaptable framework, so that users from the community could extend it easily with their own back-ends. As future work, we are constantly working to update UAL to newer versions of the already supported autopilots. For instance, MAVROS updates are very frequent and we maintain backward compatibility with several previous versions. Besides, we plan to migrate UAL to ROS 2. Last, we would also like to explore possibilities to enhance the functionalities of UAL to simulate DJI platforms, mainly for multi-UAV environments. 66 System architecture for an aerial manipulator Figure 4.6: Scheme with the existing back-ends for ACI. Additional back-ends can be easily added. We designed ACI so that new back-ends could be easily included, just by offering the common interfaces specified in Figure 4.5. Currently, there are three different back-ends implemented for ACI (see Figure 4.6): • The Gazebo Animator back-end was designed to enable simulation with the Gazebo simulator, but without simulating Gazebo physics. Thus, the manipulator follows any reference perfectly, without any dynamics involved. This is especially useful for debugging, because if we observe perturbations in movement, it can be only caused by perturbations in the references. • The ros control back-end was designed to use the ros control interface and conventions. ros control is a widespread set of ROS packages implementing ROS-community standards for robot control. This framework tries to make controllers generic to all robots using ROS. In our implementation, this back-end can also be used with the Gazebo simulator, through the gazebo ros control plugin. • The UDP back-end was designed to directly communicate with the custom manipulation hardware used in the ARS and ARS-LRM platforms for AEROARMS project. The back-end implements a custom protocol based on UDP (User Datagram Protocol). 4.3 Navigation architecture 67 Figure 4.7: Overall navigation architecture for aerial manipulators. The plan computed by the Planner is carried out by a Reactive Collision Avoidance module. Additionally, teleoperation functionalities are available for ACI. In particular, a joystick interface is implemented: each joystick axis moves a joint, increasing or decreasing the reference value, and some buttons are used to reset all joints to zero or to move a different set of joints in case there are more joints than joystick axes. 4.3 Navigation architecture In general, the physical interaction applications considered in this thesis require UAVs to navigate through unknown environments visiting several locations where the interaction takes place. In the particular use case studied in this chapter, for inspection and maintenance operations with dual-arm UAVs in industrial facilities, the scenario may be cluttered, and the state of the manipulation device needs to be explicitly considered for safe navigation (arms may collide with the surroundings). As previous works from the literature (Bialkowski et al., 2016; Nuske et al., 2015; Nieuwenhuisen et al., 2016), we propose a hierarchical navigation approach for our aerial manipulator. As shown in Figure 4.7, we assume the existence of a Planner that provides an initial path using a static map of the scenario. A Reactive Collision Avoidance module is then in charge of navigating the UAV safely to carry out the plan. This module takes care of local navigation and reactive collision avoidance, by using readings from onboard sensors in order to cope with dynamic and unexpected obstacles not considered in the original plan. The Planner outputs a sequence of 3D goal positions pg for the aerial platform, the goal vehicle speed sg along the path, and a sequence of goal joint angles qgfor the manipulation device. 68 System architecture for an aerial manipulator (a) Zero (k= 0) (b) Bent (k= 1) (c) Home (k= 2) (d) Forward (k= 3) Figure 4.8: Discrete configurations for the dual-arm of the ARS-LRM. As it navigates, the system can switch between these postures for obstacle avoidance. In order to reduce the complexity of motion planning in cluttered environments and alleviate the planning space, we assume that the aerial manipulator only admits a set of discrete configurations for the dual-arm device, while the aerial platform can travel freely in the 3D space. This discretization of the configuration space for the arms is necessary to reduce the number of possible solutions and obtain fast responses, given the high number of degrees of freedom of the dual manipulator. In the case of the ARS-LRM, for example, we considered in our implementation the four different configurations depicted in Figure 4.8 for collision avoidance. Thus, the Reactive Collision Avoidance component sends 3D velocity commands ( v ) to the aerial platform to navigate, and commands to the dual-arm so that it adopts one of the discrete arm postures, which we denote by an index k . For each pair of commands, the abstraction interfaces UAL and ACI are responsible for transforming the corresponding references for the aerial vehicle and the dual-arm, respectively (see Figure 4.9). 4.4 Reactive collision avoidance Many reactive collision avoidance methods have been proposed in the literature, see Garrido et al. (2007) for a thorough review or Goerzen et al. (2010) for an introduction to the terminology and classification of these methods. However, those 4.4 Reactive collision avoidance 69 Figure 4.9: The control commands provided by the navigation architecture are translated for the specific autopilot and manipulation device through UAL and ACI, respectively. methods typically consider the movement of the robot as a whole, i.e., there is no separate motion planning of an aerial platform and its manipulator onboard. Traditional methods for global path planning have also been enhanced to cope with dynamic environments in a reactive manner. In this sense, probabilistic sample-based planners like RRT (Rapidly-exploring Random Tree) are commonplace (Elbanhawi and Simic, 2014). For example, RRTX (Otte and Frazzoli, 2016) is an asymptotically optimal sampling-based motion planner for real-time navigation in dynamic environments. Their main issue is that these methods are not well suited when a fast response is needed and the number of degrees of freedom is high, like in our dual-arm configurations (4 degrees of freedom each arm). One of the main reasons is the computational bottleneck in the collision-checking phase, see Bialkowski et al. (2016) and Pan and Manocha (2016) for approaches to address that computation burden. We propose a reactive navigation algorithm that takes into account not only the aerial platform itself but also the constraints imposed by the onboard manipulation device. Our approach is sampling-based and considers the aerial platform and the manipulator in an integrated manner, predicting trajectories ahead and checking for collisions. The configuration of the dual-arm manipulator can be modified as the UAV navigates. In order to simplify the joint solution space, the configuration space for the arms is discretized, i.e., only a finite set of configurations are allowed. A key point of our algorithm is that it is well suited for parallelization (Lee and Kim, 2016; Park et al., 2017), which is a promising line for future work. 70 System architecture for an aerial manipulator 4.4.1 Algorithm The algorithm takes as input the initial path provided by the Planner, and for each 3D goal waypoint pg for the UAV, it computes the desired velocity vector vp , pointing in a straight line from the current UAV position to the goal position (see Equation 4.1). For the dual-arm, the closest discrete configuration to the goal qg provided by the Planner is set as the preferred posture kp. vp=sg·ug, ug=pg−p kpg−pk,(4.1) where sg: goal vehicle speed provided by the Planer, pg: 3D goal position provided by the Planner, p: current 3D vehicle position. A Monte-Carlo approach is followed to sample randomly the solution space (i.e., continuous 3D velocity vectors and discrete arms configurations) and simulate the trajectories ahead, checking for collisions. For each simulation i , a velocity vector vi with all dual-arm configurations is tested, taking into account the point-cloud readings from an onboard 3D sensor. In particular, the algorithm executes the following steps for each simulation: 1. Select the control input vi , sampled with a bias around vp . The azimuth and polar angles of the vector are sampled using Gaussian distributions with standard deviations σθ and σϕ , and the module is sampled using a triangular probability distribution. Figure 4.10 shows the angle coordinates of a velocity vector and the triangular distribution for the module, which has as lower limit 0, as upper limit kvpk , and as most likely value Kv· kvpk , where Kv∈ [ 0 , 1] is a parameter. 2. Given vi , for each configuration k , predict the UAV trajectory for a time horizon T with time steps of duration ts , using a velocity dynamic model. For our aerial 4.4 Reactive collision avoidance 71 Figure 4.10: Left, desired velocity vector vp with its azimuth and polar angles. Right, triangular probability distribution used to sample velocity module with Kv= 0.8. manipulators, we obtained these models from experimental data by means of system identification. Figure 4.11 depicts a comparison of the estimations from our ARS-LRM model and the actual values in terms of forward velocity and yaw rate. 3. Check for collisions: • The aerial platform hull for collision is modeled as a cylinder. The size of the cylinder depends on the selected configuration k , since different configurations produce different shapes for collision checking. • Trajectories where point-cloud particles lie within the object volume (at any time step) are discarded and trajectories without collisions are further evaluated. 4. Evaluate the trajectory for vi and all possible k configurations using a cost index Jik (see Equation 4.2) that takes into account the distance from the trajectory endpoint to the goal, the minimum distance from the point-cloud to the trajectory, changes of arm configurations, and the preferred configuration. Figure 4.12 shows an example of the whole procedure. The above steps are repeated to perform N random simulations and select the pair ( vi, k ) with the lowest index Jik , which is defined by the following equation: 72 System architecture for an aerial manipulator 0 20 40 60 80 100 120 time [s] -3 -2 -1 0 1 2 3 vx [m/s] real simulated 0 20 40 60 80 100 120 time [s] -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1 yawrate [rad/s] real simulated Figure 4.11: Experimental data comparing actual (blue) and modeled (red) forward speed ( vx ) and yaw rate of the ARS. The dynamic model is computed by system identification and used to predict the UAV trajectory. It can be seen that predictions fairly match the actual values. Jik =d(pg,pT i) d(pg,p0 i)+1 M M X j=1 λc d∗(oj,p0→T ik )+λp·[k6=kp] + λs·[k6=kc],(4.2) where 4.4 Reactive collision avoidance 73 pg: goal position from the Planner pt i: position at time tof trajectory simulated with vi M: number of points in the point-cloud oj: 3D point from the point-cloud pt1→t2 ik : trajectory from time t1to time t2simulated with viand configuration k d(p1,p2) : euclidean distance between p1and p2 d∗(oj,p0→T ik ) : minimal distance from point ojto collision volume describing trajectory pik λc: weight for closeness to point-cloud penalty λp: weight for non-preferred configuration penalty λs: weight for switching configuration penalty kc, kp: current and preferred arms configurations [P] : Iverson bracket, 1 if the proposition Pis satisfied, 0 otherwise. The first term of the cost index evaluates how far from the goal the UAV arrives, relative to the current distance to the goal. It is dimensionless, with value 1 meaning the distance to goal remains the same, and we have not considered any weight for simplicity, as all other terms do have a weight to tune relative importance in the desired behavior. The second term takes into account how close to obstacles the UAV moves, averaging the minimum distance from every point in the point-cloud to the collision volume describing the trajectory. The weight λc , besides giving relative importance to the term, may be interpreted as an average security distance and its value depends on the magnitude units of d∗ (meters in our implementation case). A negative value of d∗ means that the point lies inside the collision volume, so every trajectory with any d∗≤0 is considered in collision and thus immediately discarded. The third term considers a penalty for every arm configuration that is not the preferred one. Some configuration may be preferred to others because it requires minimal energy for the arms actuators, because it is imposed by an object that is being transported, or for other reasons. In any case, it is given by the Planner ( kp ) and its importance is 74 System architecture for an aerial manipulator Figure 4.12: Trajectory evaluation. Collision volumes in the current and goal positions are represented as red and blue cylinders, respectively. If the UAV executed vp (blue arrow), it may lead to a collision with the obstacles perceived as a point-cloud (red squares). N velocity vectors ( vi ) are sampled. In black, predicted trajectories leading to collision; in red, trajectories with high values of Jik ; in green, trajectories with low values of Jik . The velocity vi corresponding to the lowest Jik is selected as velocity command (green arrow). weighted by λp . The last term considers a penalty for any change in arms configurations ( λs ). It is included to take into account the cost of moving the arms and to avoid high-frequency switching caused by relatively small variations in the rest of the terms. Finally, a PID controller commanding yaw rates is also used to control the UAV yaw. We make the UAV face the direction of motion to maximize the information of obstacles ahead, assuming this is the main direction of the sensing system. However, this could be configured in a different manner. 4.5 Validation We have carried out simulations and real experiments to validate our system with actual aerial manipulators. In particular, we used the ARS and ARS-LRM platforms, which are built on top of an hexacopter (see Figures 4.2 and 4.3) with the characteristics listed in Table 4.1. They both have several onboard processors: a Pixhawk with 4.5 Validation 75 PX4 for control and state estimation, an Odroid board to interface with the arms, a general-purpose Intel NUC i7 processor, and a GPU Jetson TX1 for sensor processing. The aerial robot can be commanded in velocity through the Pixhawk board, and each joint of the dual-arm can be commanded in position through the Odroid board. The UAVs also carry a ZED stereo camera for the construction of the 3D point-cloud. Size 14.4×8.5cm Maximum Take-Off Weight 10.0 kg Weight 7.5 Kg Flight time 20 minutes Propellers CF 15 ×5 Motors T-Motor MN4006 380KV Table 4.1: Specifications of the UAVs used in the field experiments. Both the ARS and the ARS-LRM make use of the PX4 autopilot stack (Meier et al., 2018) with some custom software modifications. We used UAL and ACI during all experiments, profiting from their simulation and integration capabilities for the first steps. 4.5.1 Simulations We carried out experiments with different simulated scenarios. In this section, we present results for an example simulation of the ARS-LRM platform, in order to demonstrate the proposed architecture for the aerial manipulator and show the behavior of the collision avoidance algorithm. Figure 4.13 depicts the simulated scenario with two small U-shaped pipes and a large pipe on the right. The large pipe is included in the static map but the small pipes are not included, so they must be detected by onboard sensors and avoided. The ARS-LRM starts at the position marked in red in Figure 4.13 with zero as its preferred arm configuration. The Planner provides a straight-line trajectory to the goal location (depicted in blue in Figure 4.13) while keeping the zero posture, since it is unaware of the small pipes. This trajectory would collide with the small 82 System architecture for an aerial manipulator Figure 4.22: Scenario for field experiments. The start position is depicted in red and the goal position in blue. A non-mapped U-shaped pipe is placed between both locations. 4.5.3 Outdoor experiments We also designed an outdoor testing scenario for field experiments, both to validate the proposed architecture and to showcase the integration of the reactive collision avoidance algorithm with the actual ARS platform. In this case, a relatively inaccurate GNSS-based localization (non-RTK) was used, and the arms were simulated during these experiments for safety reasons. As shown in Figure 4.22, a U-shaped pipe was placed between the starting point and the goal point assigned to the ARS. The preferred arms configuration was zero and the algorithm parameters were the following: N = 10, T = 1 s , ts = 10 ms , σθ = π 8 , σϕ = π 4 , Kv = 0 . 9, λc = 1 . 0, λp = 0 . 1, λs = 0 . 05. These values for the parameters were handcrafted to achieve an adequate behavior and being able to process in real time. Results The results of two tests are shown in Figure 4.23 and Figure 4.24. In both cases, the trajectory provided by the Planner is a straight line (depicted in blue) that would cause a collision with the pipe (represented as a point-cloud with red marks) because 4.5 Validation 83 -4 -2 0 2 4 x [m] -4 -3 -2 -1 0 1 2 3 4 y [m] point cloud straight path executed path 0 5 10 15 20 t [s] 0 0.5 1 1.5 2 2.5 3 3.5 z [m] straight path straight collision executed path executed posture Figure 4.23: The ARS goes above the obstacle and switches arms configuration. In blue the initial planned trajectory, in green the executed one. Top view (left) and altitude evolution (right) of the trajectory, indicating in yellow the k index of the arms posture. it is not included in the map and the preferred configuration for the manipulator is zero (k= 0). In the first test (see Figure 4.23), the ARS trajectory provided by the Planner goes through the middle of the pipe. The reactive algorithm navigates the UAV above the pipe and changes the arms to a more compact configuration. When the ARS approaches the pipe, the reactive algorithm starts to increase significantly the altitude, up to 3 . 4 meters, while switching the arms to the home configuration ( k = 2) at t = 8 . 5 s . When the ARS has moved past the pipe, the altitude is decreased to reach the goal position and the arms are switched back to the zero configuration ( k = 0) at t= 13.5s. In the second test (see Figure 4.24), the ARS trajectory provided by the Planner also goes through the non-mapped pipe. The ARS avoids that obstacle mainly navigating to the right side of the pipe (Figure 4.24, left). When the ARS approaches the pipe, the reactive algorithm only increases the altitude up to 2 . 2 meters and keeps the arms in their initial zero configuration (Figure 4.24, right). 4 4An excerpt of these field experiments is available at https://youtu.be/1LWhuB-PTE4. 84 System architecture for an aerial manipulator -4 -2 0 2 4 x [m] -4 -3 -2 -1 0 1 2 3 4 y [m] point cloud straight path executed path 0 5 10 15 20 t [s] 0 0.5 1 1.5 2 2.5 z [m] straight path straight collision executed path executed posture Figure 4.24: The ARS goes around the obstacle and keeps the same arms configuration. In blue the initial planned trajectory, in green the executed one. Top view (left) and altitude evolution (right) of the trajectory, indicating in yellow the k index of the arms posture. We can conclude that, in this case, the behavioral change is caused by the subtle difference between the two initial trajectories provided by the Planner, as the algorithm parameters remained constant. Our algorithm adapts to the circumstances reactively trying to minimize Jik. 4.6 Conclusions In this chapter, we have presented a software architecture for an aerial manipulator in the context of inspection and maintenance operations. The UAL architecture in Chapter 3 has been extended with the ACI abstraction layer for the operation of the manipulation device on board the UAV, which in our case is a dual-arm. Moreover, we have presented a hierarchical navigation architecture with a planning layer with a reactive collision avoidance algorithm underneath. The latter addresses obstacle avoidance by dealing with motion for the aerial platform together with changes in the configuration of the dual-arm manipulator. We validated the overall solution with the ARS and the ARS-LRM platforms developed within the framework of the 4.6 Conclusions 85 AEROARMS project, in both realistic simulations and field experiments in indoor and outdoor scenarios. Even though the number of evaluated trajectories and arms configurations is relatively moderate for collision checking, our current implementation for reactive collision avoidance demonstrated performance in real time for cluttered environments resembling industrial plant facilities. An interesting line for future work is a parallel implementation of the reactive collision avoidance algorithm, which is well suited for parallelization. This could be done by using CUDA and an NVIDIA Jetson GPU, which is already mounted on the tested aerial platforms. This would allow us to significantly increase the number of evaluated arms configurations and UAV trajectories, without jeopardizing the real-time performance. Also, a dynamic model of the manipulation device could be easily integrated for collision checking while the arms are switching from one configuration to another. 86 System architecture for an aerial manipulator Chapter 5 A team of homogeneous UAVs for missions with physical interaction This chapter presents an architecture for a team of homogeneous UAVs that execute missions with physical interaction. This architecture was motivated by the 2017 edition of the MBZIRC competition in Abu Dhabi, which proposed different challenges for multi-UAV applications. We describe the overall software architecture, together with the different modules involved, and we explain the techniques that we used to develop the functionalities of the system. We also describe the procedure that we followed to design the aerial platforms, as well as all their onboard components. Finally, we discuss our experimental results and the lessons learned in the MBZIRC competition. The cooperative multi-UAV architecture was validated with fully autonomous missions in experiments previous to the actual competition. We analyze the results that we obtained both during those previous experiments and during the competition trials in 2017. 5.1 Introduction Robot competitions are becoming popular, as they have proved to be helpful speeding up technological advances in certain robotics tasks. The idea is to replicate conditions from real life in simulated or testbed scenarios and push the community to propose 87 88 A team of homogeneous UAVs for missions with physical interaction efficient algorithms to solve specific challenges. Since all participants are forced to operate their robotic systems in the same controlled and standardized testbeds, competitions are also interesting in terms of robot benchmarking. They foster the replicability of results in robotics research and allow researchers to compare different approaches and methods under similar conditions. Particularly, due to the recent advances in multi-UAV systems, there is an increasing need for testbed facilities and methodologies to compare existing methods in that field. Since competitions are a remarkable vehicle to develop specific technologies, aerial robot competitions are specially trending. The Mohamed Bin Zayed International Robotic Challenge (MBZIRC) 1 is a competition which focuses on aerial robots operating outdoors, and cooperating between them and with ground robots. In its first edition, which took place in March 2017 in Abu Dhabi, MBZIRC gathered 143 applications from teams all around the world, out of which 25 top-teams were selected as finalists to compete in several outdoor challenges. In particular, the competition consisted of three challenges and a Grand Challenge integrating all together: Challenge 1 required a UAV to locate, track, and land on a moving vehicle; Challenge 2 required a ground autonomous robot to locate and navigate to a panel, and physically operate a valve stem on it with the appropriate tool; Challenge 3 required a team of UAVs to cooperate to search, track, pick up, and drop a set of static and moving objects; and the Grand Challenge required the team of UAVs and the ground robot to coordinate in order to solve Challenges 1, 2 and 3 simultaneously. The Al-Robotics team was one of the 25 finalists selected to participate in the first edition of MBZIRC. The author of this thesis was part of the team with other researchers from the GRVC at the University of Seville, from the Centro Avanzado de Tecnolog´ıas Aeroespaciales (CATEC) 2 , and from the company GMV 3 . Even though the team participated in all the challenges, this chapter focuses on the cooperative multi-UAV architecture that was designed and implemented to address Challenge 3, which was the key contribution of the author of this thesis. This task is particularly challenging for several reasons. From the point of view of the system architecture, a 1http://www.mbzirc.com 2http://www.catec.aero 3http://www.gmv.com 5.2 Related work 89 team of UAVs have to operate together in an outdoor scenario, showing cooperative behavior. From the perception point of view, the UAVs must be able to locate and track different types of objects. Last but not least, they must interact physically with the environment by picking up, transporting, and dropping static and moving objects. In this chapter, we describe in detail the design of the system architecture for a team of homogeneous UAVs, motivated by the participation of the Al-Robotics team in MBZIRC 2017 Challenge 3. First, a data fusion approach is used to integrate observations from all the UAVs in the team and compute a probabilistic estimation of the objects’ positions. These observations come from a vision-based algorithm aimed at detecting the known colors and sizes of the Challenge objects. Given that communication issues were not expected (there are only few UAVs working in a short range), we opted for a centralized stochastic filter due to its robustness. Second, a mission planner is used for cooperative decision-making, sending the UAVs to search for objects, and later to perform pickup and dropping operations. After a detection phase, the UAVs are assigned objects heuristically so that hypothetical conflicts, i.e., UAVs with crossing paths, are minimized. The pickup operations are carried out by means of a magnetic tool and a vision-based controller, since the positioning systems of the UAVs are not accurate enough to pick up the objects. In summary, the main contributions of this chapter are: (i) to present our software architecture for a homogeneous team of multiple UAVs, motivated by MBZIRC 2017 Challenge 3, combining multi-UAV data fusion and decision-making algorithms; (ii) to detail the design and implementation of our hardware and software architectures; and (iii) to describe our results and the lessons we learned before and during the competition, some of them even leading to system redesigns. 5.2 Related work This section presents a brief summary of the state of the art with respect to robot competitions, which is a motivation for this chapter, and also related work relevant for the data fusion and decision-making components of our multi-UAV system. 90 A team of homogeneous UAVs for missions with physical interaction 5.2.1 Robot competitions In the last years, competitions have turned out to be a noticeable way of promoting the development of new technologies to cope with present robotics problems and benchmarking (Stuckler et al., 2012). Such competitions allow roboticists to test methods and compare them under the same conditions, since they provide controlled testbeds where specific robotics challenges need to be solved. In this sense, there are initiatives like RoCKIn Amigoni et al. (2015) to develop competitions where the focus is on coming closer to scientific experiments and enabling the replicability and repeatability of experimental results. The Defense Advanced Research Projects Agency (DARPA) is one of the most active actors organizing robot competitions. Their DARPA Grand Challenge (Buehler et al., 2007) and Urban Challenge (Buehler et al., 2009) were devoted to autonomous driving, but more recent editions have focused on search & rescue. Thus, the DARPA Robotics Challenge (Krotkov et al., 2017) proposed the development of humanoid robots to solve complex tasks in disaster management scenarios, and the DARPA Subterranean Challenge 4 the exploration of underground tunnels and mines. Other competitions, like RoboCup (Rodr´ıguez et al., 2019), or RoboCup Rescue Robot League (Pellenz et al., 2015), propose indoor scenarios to pursue reproducibility and repeatability in robotics benchmarks. Regarding multi-robot systems operating outdoors, euRathlon (Winfield et al., 2016) was a European competition combining marine, aerial, and ground robots for search & rescue tasks. 5.2.2 Multi-robot object tracking and decision-making The use of multiple cooperative UAVs for missions where the positions of some objects or targets must be estimated and tracked is commonplace. These vehicles can provide enhanced sensing capabilities, faster dynamics, wider fields of view, and they can access more hazardous areas; all of which are remarkable advantages for applications like surveillance and situational awareness in rescue robotics (Burdakov et al., 2010; Hsieh et al., 2007; Beard et al., 2006). 4https://www.subtchallenge.com 5.2 Related work 91 From the point of view of perception, the problem of target tracking by means of a team of UAVs has been extensively studied. An estimation of the targets’ positions and their associated uncertainties can be maintained by using different types of stochastic filters, which fuse observations coming from multiple sensors on board the team-members. Depending on the models and sensors involved, some works assume Gaussian probability distributions and propose Kalman Filters (Morbidi and Mariottini, 2011) or Information Filters (Capit´an et al., 2011); whereas others deal with multi-modal distributions through Bayes Filters (Cook et al., 2014), Particle Filters (Ong et al., 2006), or Gaussian Mixture Models (He et al., 2010). Besides the estimation problem, a decision-making problem needs to be solved, so that each UAV knows which are its best actions during the mission in order to locate the targets. One approach is to use stochastic optimal control to formulate the problem, trying to optimize some utility function based on the targets’ uncertainties (Anderson and Milutinovic, 2013; Morbidi and Mariottini, 2011). These uncertainties can be quantified by means of different metrics, such as entropy or mutual information. For instance, Charrow et al. (2014) develop a control law which maximizes the mutual information between the robot’s measurements and their current belief of the target position. Kassir et al. (2015) propose algorithms to optimize multi-robot information gain, but minimizing communication rates at the same time. Another relevant approach for decision-making in this kind of missions is to split the scenario into survey areas or points to visit and assign them to the UAVs in an efficient manner. In these sense, coverage path planning algorithms can be useful, i.e., algorithms to cover a certain area efficiently with a team of robots. Galceran and Carreras (2013) present an extensive survey of those algorithms, providing a categorization for decomposition and coverage techniques in the literature. Task allocation techniques also play a key role in multi-UAV cooperation. Korsah et al. (2013) provide an extensive literature review and propose a novel taxonomy. Traditionally, those algorithms allocate tasks to UAVs in an efficient manner, being typical tasks the points to visit for searching targets, or the targets to be tracked themselves. However, these tasks can vary depending on the techniques used. For instance, some people have proposed auctions to allocate behavior-based policies (Capit´an et al., 2016) or 98 A team of homogeneous UAVs for missions with physical interaction We propose a centralized stochastic filter running on the GCS and receiving observations from all the Vision Modules in the team, and integrating them into a single data structure. The fact that there are only three UAVs operating in a relatively short range, makes advisable to use this approach instead of a decentralized estimator. The communication bandwidth to send all observations to the GCS is not critical, and at the same time, possible issues with inter-robot transmissions and delays are avoided. Therefore, we selected a centralized multi-track filter due to its simplicity and efficiency. The Object Estimator maintains a belief over the pose and color of each object in the arena (i.e., a track). Anytime a new object is detected by any of the UAVs, this is communicated to the Estimator, and a new track for that object is created. For each track, the state is composed of several factored variables associated with the object ( x, y, vx, vy, c ). The 2D position and velocity of the object are continuous variables, whereas the color is a discrete variable c∈ {red, blue, green, yellow, orange} . The former is updated by means of a Kalman Filter (KF), and the latter with a discrete Bayes Filter. We use a Kalman Filter to estimate object positions because it is simpler for data fusion from different sources and less computationally costly than a Particle Filter. The main problem is that we cannot deal with multimodal distributions, but we alleviate that issue by reducing the integration of false positive observations into the filter. For that, we develop a technique to solve the data association problem between the observed objects and the current tracks, combining both color and distance information. The following sections give more details about the different probabilistic models used for the prediction step, the update step, and data association. Initialization As stated above, the initialization of a track occurs whenever a new object is detected by the team and must be incorporated into the filter. This happens when an observation received from some UAV is not associated with any previous track. Hence, a new track is created, initializing the position and velocity according to the observation received from the UAV, and the color to a uniform probability distribution. The color 5.4 Software architecture 99 variable is then updated with the information contained within the UAV observation, increasing the probability for the value of the observed color co. Prediction A Kalman Filter is used to maintain the belief over the position and velocity of the object. Therefore, if x = ( x, y, vx, vy ) T is the state vector and Σ the covariance matrix, a linear kinematic model is used to predict this state from one time step to another separated a time interval ∆t. The prediction and noise matrices are the following: F=      1 0 ∆t0 0 1 0 ∆t 0 0 1 0 0 0 0 1       ,Q=      0 0 0 0 0 0 0 0 0 0 σ2 v∆2 t0 0 0 0 σ2 v∆2 t       .(5.1) The parameter σv indicates the level of noise for the object velocity; the higher, the more the uncertainty grows after a prediction 7 . The belief color factor is never predicted, since the color is fixed for all objects. Moreover, the KF is not predicted for the static obstacles, only for the moving ones. This is determined by means of the color: an object whose probability of being yellow is higher than its probability of not being yellow, is labeled as a moving obstacle. Otherwise, the object is considered static. The same reasoning is applied to the probability of being orange in order to label each object as large or small. Update If a UAV observes an object and this observation gets associated with a specific track, the belief of that track must be updated with this new information. The observation coming from the Vision Module consists of a 2D position in global coordinates of the 7 We set this value to σ2 v = 0 . 2 m2/s2 and checked during the experimental trials that was reasonable for the moving objects. 100 A team of homogeneous UAVs for missions with physical interaction object z and its corresponding covariance matrix R ; and an observed color co . To incorporate the position information, the KF is updated with a simple linear model: H= 1 0 0 0 0 1 0 0!.(5.2) The color belief is also updated with the observation co by means of the equations of a standard Bayes Filter: p(ct=i) = η·p(co|ct=i)·p(ct=i),∀i(5.3) where η is a normalizing constant and p ( co|ct = i ) is the probability of observing co given a color value ct = i . We estimated empirically that the probability of detecting the actual color of an object with our vision algorithm was of 0 . 9, which is the value that we used to compute the previous probability. If the vision algorithm provides no information about the color because it could not be observed with enough certainty, the color belief is not updated. Finally, the belief is only updated with recent observations. If an observation that is too old reaches the filter, it is discarded. In this way, we avoid spoiling the estimations with observations that were delayed too long due to network communication issues. This value was adjusted during the experimental trials after evaluating communication delays. Data association A data association problem has to be solved when new object observations arrive at the Object Estimator. Multiple tracks are maintained and it needs to be determined to which track the observations correspond, or whether new tracks should be created. We define a couple of heuristics based on probability to measure how close an observation is to the current estimation of each track. Those heuristics are then used to create associations. First, we define a probabilistic distance of the observed position: 5.4 Software architecture 101 dp=p(z−Hx)TS−1(z−Hx).(5.4) Given a track with position belief ( x,Σ ) (mean and covariance matrix), the heuristic in Equation 5.4 measures the Mahalanobis distance between an observed object position z and the probability distribution of the predicted observation. With the current belief, the probability distribution of the predicted observations can be computed by projecting ( x,Σ ) into the observation space, i.e., the probability distribution of observations would have mean and covariance matrix ( Hx,S ), with S = HΣHT + R . The lower dp , the higher the probability that the observation corresponds to that track. In order to take into account the information about the color, we also compute the probability of the color observation cofor each track: p(c=co) = X i p(co|c=i)·p(c=i).(5.5) At each iteration of the Estimator, the set of received observations is processed to associate them with the existing tracks. First, the heuristic dp is computed for all possible pairs observation/track. Second, the best pair with minimum distance value is selected for association. If dp≤dth , the observation is likely enough 8 and the track is updated with that observation. Otherwise, the observation is not close enough to any of the existing tracks, so a new one is created and initialized with that observation. The same procedure is repeated until there are no more observations to associate. Note that more than one observation could be associated with the same track, since those may be observations of the same object coming from different UAVs. Finally, when a best pair is selected but the probability of its color observation is too low (Equation 5.5), the association is discarded. We experimented with our color detection algorithm and estimated that this happened when p(c=co)<0.15. 8 The threshold dth is a parameter to adjust how flexible the associations are. Its value will be discussed later. 102 A team of homogeneous UAVs for missions with physical interaction Additional information Besides the belief over the position and the color of each object, the Estimator keeps additional information useful for other modules. First, for each object (track) a unique identifier is stored. This is useful for logging and visualization, and also to assign them to different UAVs. Second, each object has a status within the tuple {UNASSIGNED,ASSIGNED,CAUGHT,DEPLOYED,LOST,FAILED} . ASSIGNED and UNASSIGNED indicate whether the object has a UAV assigned to be picked up or not, respectively; CAUGHT means that the object has been picked up successfully; DEPLOYED that the object has been transported and dropped; an object is LOST when a UAV goes to pick it up and cannot find it; and an object is set to FAILED when a UAV goes to pick it up and the action is aborted after failing. The Cooperative Planner and the UAV State Machine use this status to keep a track of the objects’ situation, and they are the ones in charge of modifying the values, as it will be explained in Section 5.4.3. Furthermore, CAUGHT and DEPLOYED objects are not considered by the Estimator for prediction nor update; whereas LOST objects are removed from the filter. Finally, it is relevant to mention that the Estimator also removes objects that are not observed for two long or were observed spuriously. After the experimental trials, we determined that an object that had been detected in less than 5 frames and had not been detected for 20 seconds, was a false positive and had to be removed. We adjusted those values during the trials not to have many spurious objects and to focus on those detections more likely to be real. 5.4.3 Cooperative planning The Cooperative Planner is in charge of planning paths and actions for the UAVs in a coordinated manner. It consists of a centralized module that runs on the GCS and that receives the current position from each UAV and object estimations from the Object Estimator module. Then it allocates different objects to the UAVs, that should go to their estimated positions, pick them up, and drop them back into the dropping box. 5.4 Software architecture 103 Figure 5.5: Scheme of the arena for MBZIRC: DZ represents the Dropping Zone, where the box is placed; LZ represents the Landing Zone, where UAVs start the mission. Left, an example of the paths followed by three UAVs during the search phase to cover the whole arena (they go and return to the start position). Right, in red the roundabout around the DZ with the six waiting spots for the UAVs loaded with objects. Due to the fact that the arena can be covered fairly fast with the UAVs, and to avoid too many conflicts between the different vehicles collecting objects, we proposed a novel cooperative strategy with two phases. With this algorithm, objects are allocated to UAVs as tasks heuristically and the potential conflicts are minimized. First, the UAVs fly covering non-overlapping zones of the whole arena and searching for the maximum number of objects. Second, once this search has ended, the Planner starts to assign the UAVs objects that they should collect and drop. During the search phase, the arena is divided into three longitudinal sectors, and each of them is covered by a different UAV with a straight-line path (return trip). Figure 5.5 depicts an example of the division and the paths followed, which can be computed geometrically so that all the segments are equally distributed. Also, in case that there were only two UAVs available (e.g., because one of them failed), the scenario would be split into two equal sectors to be covered in the same fashion as before. Note that the UAVs fly at the same height during this search phase, since their paths are non-overlapping and no conflicts need to be solved 9. 9 We flew with a height of 10 meters during our trials, since we tested that that height was adequate for detecting most objects with our vision algorithm and covering a third of the arena. 104 A team of homogeneous UAVs for missions with physical interaction After the search phase, the UAVs should have a good estimation of most object locations. A collecting phase starts afterwards, where the Cooperative Planner assigns them different objects to collect and drop. These assignments are asynchronous, i.e., anytime a UAV is idle, it asks for a new task (object) and the Planner decides the best one to be picked up, given the current situation. During this phase, a different height is assigned to each UAV for navigation, so that they can traverse the arena without colliding with each other 10 . Note that the Vision Modules and the Object Estimator are still running during the second phase, so new objects could be detected (or information from previous ones updated) as the UAVs navigate collecting and dropping objects. When the Planner needs to assign an object to a UAV, it follows several rules. First, it only considers objects that are not assigned to another UAV. Second, it prioritizes according to the color and assigns first those unassigned with the color of highest priority. In particular, we focused first on the static, small ones (the higher its score, the higher its priority); then on the moving ones (yellow); and finally on the large ones (orange). Given our aerial platforms, we estimated that scale of difficulty and decided to go from easier to harder. Last, in order to discriminate between obstacles with the same priority, the Planner rates them with a heuristic based on distance, opting for the closest one. We also tested another heuristic weighting object scores and distances, but it turned out to be more effective the priority rule, i.e., to pick up always the easiest ones first. Although each UAV flies at a different horizontal plane while collecting objects, they may still collide when one of them is descending to pick up an object, since it could traverse others’ planes. In order to minimize those situations, when assigning an object i to a UAV 1, the Planner checks whether the straight line from that UAV 1 to the object i lies too close (distance measured on the horizontal plane) from any other object j assigned to some other UAV 2. In that case, this assignment is discarded because UAV 1 could cause a potential conflict while UAV 2 is descending to pick up its object j . Nonetheless, note that some conflicting situations may still arise, but 10 We selected heights of 3, 7 and 11 meters for the three UAVs during our trials, since we tested that those were still adequate to detect objects and keep a safety distance between the UAVs. 5.4 Software architecture 105 their probability is significantly reduced. Indeed, being conservative and discarding assignments whose corresponding paths passed closer than 5 meters to other assigned objects, we did not come across any conflict during all our experiments. Finally, another source of conflict should be taken into account: the dropping zone. We solve this issue by treating that zone as a centralized shared resource where only one UAV can enter at a time. When a UAV enters that zone, it takes a token that needs to be freed before someone else uses it. Thus, as shown in Figure 5.5, an imaginary roundabout with six waiting positions is designed around the dropping zone. Any time a UAV has picked up an object and needs to drop it, it asks the Cooperative Planner for a waiting spot. The Planner will assign to that UAV the closest spot not already assigned to another UAV also dropping. Afterwards, the UAV will navigate there and wait until the token of the dropping zone is freed. Note that UAVs navigating across the dropping zone towards their assigned objects (before picking them up) would still be conflictive, but this situation is highly unlikely, since the dropping zone is placed in one of the extremes of the arena with no much space behind. 5.4.4 UAV state machine Each UAV runs a State Machine on board that deals with all the tasks assigned by the Cooperative Planner. The UAV State Machine is depicted in Figure 5.6 and it governs the UAV behavior by issuing commands through UAL (see Chapter 3). State transitions may be triggered by the completion of UAL commands, by service calls from the Planner, or by other external events (e.g., activation of the contact sensor in the pickup mechanism). The State Machine starts in REPOSE and it waits until the Planner begins the mission by calling a service to take off the UAV. Then it switches to TAKING OFF and issues a TAKE OFF command through UAL with the corresponding height for the search phase (z searching). When the take-off is completed, the UAV goes directly to the SEARCHING state, where it is issued a GOTO WP command. This UAL command navigates the UAV to a single waypoint or through a list of waypoints. The Cooperative 106 A team of homogeneous UAVs for missions with physical interaction Figure 5.6: Diagram of the UAV State Machine. States are represented by rectangles and transitions by arrows. The circles represent UAL commands to the UAV with specific parameters. Planner indicates the specific search path for each UAV (search path), as explained in Section 5.4.3. After finishing the path, the UAV goes to an idle state called HOVERING . As explained in Section 5.4.3, the Planner assigns asynchronously objects to the UAVs as they become idle and ask for new tasks. Hence, any time a UAV is HOVERING , the Planner selects the best object to collect (if there is any) and calls a service of the UAV State Machine indicating information about that object. In the GOTO PICKUP state, the State Machine uses the object position (object xy) to send the UAV there with a GOTO WP command. Note that the navigation height z uav during this phase varies from one UAV to another. 5.4 Software architecture 107 Once arrived at the object position, a pickup operation is attempted. The candidates generated by the Vision Module are explored to find one that matches the color of the assigned object. The best match is selected, i.e., the closest one with the same color as the assigned object and inside the arena (see the discussion about geofencing in Section 5.6.4). If a match is found, the State Machine transitions to PICKING UP . Otherwise, it goes back to HOVERING and that assigned object is set to LOST. In the PICKING UP state, the UAL command VEL CTRL is activated. This command controls the UAV in velocity (horizontally) in order to center the candidate position candidate xy on the image, at the same time that the UAV descends to get closer. This visual servoing is based on a PID controller that works with local position errors, since global object positions are not accurate enough. Thus, the object position on the image (candidate xy) with respect to the image center is used to center the UAV by means of horizontal movements. Different values for the controller gains are used to pick up static or moving objects. We tuned those values empirically to achieve a more aggressive control with the dynamic objects. As the UAV descends, the corresponding candidate may be lost by the Vision Module. In that case, the UAV ascends back up to a maximum height or until the object is detected again ( GOTO WP command). The same procedure is repeated up to a maximum number of attempts, after which the object is set to FAILED and the UAV returns to HOVERING . Since FAILED objects are not considered again for assignment, in case there were not remaining objects, the Object Estimator would reset the ones FAILED to UNASSIGNED in order to attempt them over again. On the contrary, if the contact sensor of the pickup mechanism is activated, the object is set to CAUGHT and the State Machine switches to GOTO DROP. In the GOTO DROP state, the UAV goes back to its navigation height, and asks for the closest free waiting spot in the roundabout ( GOTO WP command). Once arrived, it waits until the dropping zone is free, then it enters, descends to a dropping altitude, drops down the object, and sets it to DEPLOYED . Afterwards, the UAV returns to its original position at the roundabout at its navigation height and transitions back to HOVERING. 114 A team of homogeneous UAVs for missions with physical interaction Figure 5.11: Preliminary tests in Seville (Spain). A DJI F550 aerial platform with our pickup mechanism transporting a red object (top) and our final custom-made hexacopter transporting a blue object (bottom). ours, while others used smaller and lighter UAVs. These lighter platforms present advantages in terms of maneuverability and stability, but they were not able to pick up the large objects. However, there were two relevant facts during the competition that affected significantly the payload requirements. First, even though the weight of the small objects was originally specified as “less than 500 g ”, the actual weight of those used in the competition was 350 g . Second, the organization allowed all teams to change batteries during the trials without penalty. Thus, it was feasible to fly with smaller batteries, having more payload available for the objects. Given the above premises we would have probably used a different platform. Actually, we also performed some experiments in Seville to test our software and pickup mechanism with the smaller and lighter DJI F550 airframe, as shown in Figure 5.11. Although this aircraft was more controllable and stable at low altitude, we originally considered that it would not offer enough flight time and payload. Nonetheless, the performance of our aerial platforms during the competition was excellent in terms of endurance. They behaved as expected according to the original design, being able to fly during 20 minutes and to perform complete missions. Hence, we never had to change the batteries during any of the trials. Finally, we discarded more precise localization devices for the UAVs, such as RTK GNSS receivers due to their price. Those devices provide more accuracy in 5.6 Experimental results and lessons learned 115 Figure 5.12: Final version of the pickup mechanism. In the middle of the carbon lattice, the red holder is mounted on the damped structure. The holder has four magnets on top, two contact sensors, and a lever in the middle actuated by a servomechanism. the measurements and increase clearly the stability of the aircraft. However, we experienced that the level of precision provided by our autopilot (it achieved errors below 2 meters by filtering GNSS and IMU measurements) was enough to perform the missions, since we were using visual servoing with local coordinates to pick up the objects. 5.6.2 Pickup mechanism In our first rehearsal trials in Abu Dhabi, we experienced issues with our pickup mechanism described in Section 5.5.2, since the UAVs were not able to grab any of the competition objects. The issue was related to the layer of color paint that the official objects had. Our previous and successful tests in Seville were with similar mock-up metallic objects, since the organization did not send instances of the competition objects. There were other teams experiencing the same critical issue, but instead of quitting the competition, we improvised a new design on site. In particular, we used the same holder, but replaced the EPM with an array of four permanent magnets, which had enough power to pick up the objects. We also included a radio-control servomechanism with a lever that was used to release the 116 A team of homogeneous UAVs for missions with physical interaction objects. Moreover, we mounted two contact sensors to detect the pieces. Figure 5.12 shows the new design of the whole mechanism. The contact sensors were in charge of confirming that an object had been caught, and the release action triggered the movement of the lever to push the object downwards. We tested our new device during the rehearsal trials successfully and were able to pick up eventually the official objects. 5.6.3 System architecture and integration We integrated all the modules of our architecture with ROS Kinetic Kame. We also developed a simulated version of the MBZIRC arena and our aerial platforms based on the robotics simulator Gazebo 14 . An SITL scheme was used to integrate the actual software of our autopilot into the simulation, what allowed us to perform quite realistic simulations. Our platforms use PX4 as autopilot software, so we used an SITL module of the PX4 for Gazebo (Furrer et al., 2016) integrated into our software architecture. This allowed us to implement a Gazebo model for our aerial platforms, which ran the same software as the actual autopilot. The camera to feed the Vision Module and the pickup devices were also integrated into the simulation by means of Gazebo plugins. This simulation is not very reliable in terms of flight control, as we did not invest time identifying a dynamic model of the platform, but it is definitely accurate with respect to the autopilot behavior. In general, our UAL and the SITL simulator proved to be quite relevant for the integration of the whole system. The ability to simulate complete multi-UAV missions became a remarkable feature to test and debug the interfaces and functionalities of all modules. Indeed, the whole system could not distinguish simulation from real flight behavior. Only the gains of the low-level controllers for the aerial platforms needed an additional adjustment in order to jump into experiments with the actual systems. 14http://gazebosim.org 5.6 Experimental results and lessons learned 117 5.6.4 System coordinates and geofencing The autopilot provides the location of each UAV in global geodesic coordinates (latitude and longitude) and in local coordinates (in meters) with the origin in the place where the autopilot is booted and the axis aligned with the north. Since we have multiple UAVs which cooperate, we need a common coordinate system. Geodesic coordinates are global and seem to be an obvious option. However, our platforms had not the global precision of an RTK-GNSS receiver and the PX4 implements an enhancing filter (based on GNSS readings) to estimate its pose on its own local coordinate system. Therefore, we preferred those local coordinates rather than the global ones, due to their accuracy and stability. Figure 5.13: The MBZIRC arena with the [arena] coordinate system. It can be seen that the coordinates are aligned with the arena and can have a rotation with respect to the north. In gray, the valid area for the geofencing tool is also shown. Objects out of that area were not considered for estimation nor collection. We defined a global coordinate system called [arena] (see Figure 5.13) relative to the scenario map and we learned that specifying coordinates for the high-level modules in this common system avoided many issues. With this new system, we could also maintain the same configuration (in terms of UAV waypoints) for every arena 118 A team of homogeneous UAVs for missions with physical interaction (there were two). The only requirements to transform between the local geodesic coordinates of the UAVs and the [arena] system were to know the start UAV positions (we placed them in known points of the landing zone, e.g., the squares); and the arena rotation with respect to the north, because the local coordinates are defined in ENU (East-North-Up). Furthermore, we designed our UAL to deal with different coordinate systems, abstracting the end-user from that. Besides defining different coordinate systems and managing them transparently, we implemented a geofencing tool. This tool is in charge of checking whether an hypothetical object is within the physical limits of the arena (see Figure 5.13). We discovered during the competition that this was quite relevant for safety reasons, in order to prevent the UAVs from attempting to pick up things out of the arena (i.e., false positives), or inside the dropping zone (i.e., dropped objects), where trying to catch an object could interfere with other UAV dropping. The geofencing tool solved the above issues in a simple fashion, double-checking object positions before creating them in the Estimator. Also, objects not holding the geofencing constraints were not considered by the UAVs during pickup operations. 5.6.5 Communication and network configuration The network configuration and devices turned out to be critical for the competition. Although we had tested the wireless communication devices on board our aerial platforms extensively in the experiments previous to the competition, we experienced many communication issues during the trials in Abu Dhabi. We discovered eventually that it was a problem with the setup of our wireless links on board the UAVs and we solved our connectivity issues by updating the firmware of the Ubiquiti Rocket devices. In addition, since the system is distributed and there are processes running on the UAVs and on the Ground Control Station, time synchronization is essential, specially for the algorithms of data fusion. Delays in the network communications led to situations where the Object Estimator discarded many observations for being too old or where those estimations were inconsistent. Therefore, we solved this issue by using 5.6 Experimental results and lessons learned 119 the Network Time Protocol (NTP) and a server configured on the Ground Control Station. NTP allows timing information to be distributed in local area networks with errors below one millisecond, which satisfies the time constraints of our distributed architecture. 5.6.6 Multi-UAV object estimation and allocation We integrated and tested successfully and repetitively our Object Estimator in the trials in Abu Dhabi, fusing information from the three UAVs. The navigation heights selected were appropriate to detect the objects in the arena and estimate their positions with enough accuracy to be found later by a UAV trying to collect them. The main source of error for the objects’ positions came from the UAV positioning systems, since those were used to project the image detections onto the 3D global coordinate system. Moreover, the time synchronization to match image detections and UAV telemetry was not perfect. Overall, with our GNSS-based positioning system, we achieved an accuracy with errors below 2 meters for the UAVs, and hence, for object estimations. We did not use RTK GNSS and we had no ground truth either, so we could only get an empirical estimation of the UAV localization error. For that, we took large sets of measurements of a UAV at different static positions and compared them with the average value to extract a standard deviation. The UAV altitude was provided by a highly accurate laser altimeter, and hence it had a much lower vertical uncertainty. The Cooperative Planner worked also properly during the competition trials, performing the search phase and distributing later the objects between the three UAVs. We were able to detect most of the objects in the arena, but we observed that two parameters were critical for the Estimator performance: the error covariance for the observed positions R and the association threshold dth . As expected, it is essential to adjust those parameters adequately so that the filter is not overconfident (what may make it diverge at some point) and the associations are reasonable. On the one hand, if the distance threshold for association is decreased, the filter considers many observations as new different objects instead of integrating them within previously 120 A team of homogeneous UAVs for missions with physical interaction existing estimations. On the other hand, increasing this threshold too much could cause that close objects are seen as the same. Figure 5.14 shows some experimental results in Abu Dhabi 15 for the Object Estimator with and without adjusting the above parameters. When everything is tweaked correctly (top view), the system outputs estimations for the actual objects, whereas too many spurious objects appear without the correct parameters (bottom view). As explained above, this is due to the fact that many observations corresponding to the same objects are not associated well but seen as new objects. Moreover, the video of this experiment shows how a moving object (yellow) is detected within the dropping zone (second 50) but not included in the Estimator due to the geofencing tool. The idea is to avoid the UAVs from going toward dropped objects again. Also, there are some false positive detections during the video that create new objects which are erased later (e.g., objects 1 and 6), as they are considered spurious after some time without detection. This is done for objects that are detected just in a couple of image frames. Another interesting discovery during the competition trials was that the movement of the yellow objects was quite restrictive, since they moved around the same zone where they started. At the beginning, our Estimator was configured to removed moving objects that had not been detected for a while. That made sense because those predicted estimations were not reliable anymore after some time. However, we ended up treating them in the Estimator as static, bounding their predictions and not removing them when not seen for a while. Regarding the cooperative behavior, it turned out to be wise the strategy of focusing first on the small, static objects and then the moving ones, since those were harder to pick up and there was no team collecting all the static ones. Moreover, even though we managed to run missions with the three UAVs, many times we ended up with fewer due to hardware, software or communication failures. This could lead to some issues due to synchronization constraints between the UAVs. For instance, originally the UAVs were waiting for each other after the search phase, in order to move together to the collecting phase. 15A video of the experiment can be seen at https://youtu.be/38PnmsH4jOk. 5.6 Experimental results and lessons learned 121 In the end, we removed UAV synchronization to make the distributed system more robust. 5.6.7 Picking up objects In our previous experiments in Seville, we tested the software architecture to pick up our mock-up objects autonomously. Even though we did not have time to test the system extensively under a wide variety of conditions, we performed successful experiments, including autonomous complete missions picking up several pieces. For instance, Figure 5.15 shows the results of an experiment where one of our UAVs attempts to pick up a red object with the autonomous visual servoing 16 . In this experiment, the UAV centers the object on the image plane by means of its velocity control, at the same time that it descends gradually. The visual detector is stable enough and the UAV is able to recover when the object gets out of the field of view. This is done by ascending back slightly until the object is seen again. After the second attempt (second 70 of the video), the object is caught by the magnetic device successfully. This is noticed by the UAV, that starts going up again. We also run some repeatability tests to assess the overall performance of the system 17 . In particular, we repeated multiple autonomous pickups of different static objects an evaluated the success rate and the duration of each trial. On average, 20% of the trials failed, i.e., the UAV was not able to pick up the object; a 30% of the trials were partially successful, i.e., the UAV picked up the object but it fell down when returning to the dropping area; and a 50% of the trials were totally successful, with the UAV picking up and dropping the object correctly. Moreover, the average duration of each trial was 40 ± 4 seconds and the number of attempts 2 . 1 ± 0 . 34. In each trial, a single UAV started the pickup operation always at the same height, and performed several attempts (as explained in Figure 5.6) until either it picked up the object successfully or it made it fall down from its pedestal. We also picked up moving objects successfully, but we did not have enough time before the competition to run similar repeatability tests with moving objects. 16A video of the complete experiment is available at https://youtu.be/NQLvokGbVzM. 17 A video showing an excerpt of these experiments is available at https://youtu.be/0n2B0wOoOZI . 122 A team of homogeneous UAVs for missions with physical interaction During the first trials in Abu Dhabi, the performance of our controller was not satisfactory and we did not achieve the same successful results picking up objects autonomously. There were windy conditions and it turned out that our system was not robust enough to cope with that. We thought of tuning the controller to make it more aggressive, but it was too risky because we were not allowed to fly the UAVs for testing out of the trials. Instead, we decided to modify the final behavior, including a free fall of the aircraft (until the contact sensor was activated) when it managed to have the object centered and close enough. After the free-fall implementation, we performed the last competition trials where our UAVs attempted to pick up several objects autonomously. However, they did not fall down with enough accuracy to contact the objects. Any subtle delay in the free-fall decision resulted in blindly trying to pick up a nonexistent object near the actual one. The problem may have been solved by tuning and testing better the controller, but we had no time available for that. 5.7 Conclusions In this chapter, we have presented a system architecture for a homogeneous team of UAVs in cooperative missions. In particular, we have explained the implementation of the architecture to address MBZIRC 2017 Challenge 3. This Challenge took place in an outdoor arena and it consisted of searching, collecting, and transporting to a dropping box a set of static and moving colored objects. First, we have presented the software architecture, describing the techniques used to develop all the software functionalities. After that, we have detailed the hardware used to implement the multi-UAV system. Finally, we have discussed our results before and during the first edition of the competition in Abu Dhabi, as well as all the lessons learned during the process. In terms of hardware, our aerial platforms performed well with all the devices correctly integrated. However, provided that battery replacement was not penalized eventually and that the payload requirements from the objects were not so high as 5.7 Conclusions 123 expected, we conclude that we could have used UAVs with less payload. These would have been lighter and more agile platforms, and hence easier to control and stabilize. Regarding the software architecture, our participation in the competition entailed a tremendous and fruitful integration effort. As a result, our team managed to perform cooperative missions with the three UAVs and all modules working together. In the competition trials, we always started in Autonomous Mode and flew simultaneously our three UAVs, except for one of the trials, where we lost communication with a UAV from the beginning. Our team always completed the search phase autonomously, finding most of the objects on the arena. Afterwards, the team was also able to allocate objects to the UAVs autonomously, and the UAVs attempted to collect their assignments navigating without collisions in a coordinated manner. In our experiments previous to the competition, we managed to pick up mock-up objects with an acceptable success rate, which we did not achieve with worse windy conditions in Abu Dhabi. We conclude that our system was more sensitive than others to the external conditions, since it required to have the UAV stabilized to make contact with the pieces. We strongly believe that the system would have worked fully autonomous with some more time for a proper calibration and tunning process. Overall, it seems that the first MBZIRC edition was more focused on hardware issues. Designing reliable aerial platforms and pickup mechanisms was the most crucial part. On the contrary, there was less focus on the implementation of cooperative and efficient strategies. Nonetheless, the development of this architecture for homogeneous multi-UAV systems was quite helpful for us, as it paved the way for more complex and flexible architectures considering heterogeneous capabilities in the UAVs. Thus, Chapter 6 will present a more advanced multi-UAV architecture, using all the lessons learned during the work described in this chapter. 226 Bibliography Su´arez, A., Soria, P. R., Heredia, G., Arrue, B. C., and Ollero, A. (2017b). Anthropomorphic, compliant and lightweight dual arm system for aerial manipulation. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 992–997. Su´arez, A., Vega, V. M., Fern´andez, M., Heredia, G., and Ollero, A. (2020). Benchmarks for aerial manipulation. IEEE Robotics and Automation Letters, 5(2):2650– 2657. Tardioli, D., Parasuraman, R., and Ogren, P. (2019a). Pound: A multi-master ROS node for reducing delay and jitter in wireless multi-robot networks. Robotics and Autonomous Systems, 111:73–87. Tardioli, D., Riazuelo, L., Sicignano, D., Rizzo, C., Lera, F., Villarroel, J. L., and Montano, L. (2019b). Ground robotics in tunnels: Keys and lessons learned after 10 years of research and experiments. Journal of Field Robotics, 36(6):1074–1101. Thomas, J., Loianno, G., Sreenath, K., and Kumar, V. (2014). Toward image based visual servoing for aerial grasping and perching. In IEEE International Conference on Robotics and Automation (ICRA), pages 2113–2118. Tognon, M., Alami, R., and Siciliano, B. (2021). Physical human-robot interaction with a tethered aerial vehicle: Application to a force-based human guiding problem. IEEE Transactions on Robotics, 37(3):723–734. Tognon, M., Ch´avez, H. A. T., Gasparin, E., Sabl´e, Q., Bicego, D., Mallet, A., Lany, M., Santi, G., Revaz, B., Cort´es, J., and Franchi, A. (2019). A truly-redundant aerial manipulator system with application to push-and-slide inspection in industrial plants. IEEE Robotics and Automation Letters, 4(2):1846–1851. Tordesillas, J. and How, J. P. (2022). MADER: Trajectory Planner in Multiagent and Dynamic Environments. IEEE Transactions on Robotics, 38(1):463–476. Torres-Gonz´alez, A., Capit´an, J., Cunha, R., Ollero, A., and Mademlis, I. (2017). A multidrone approach for autonomous cinematography planning. In Iberian Robotics conference, pages 337–349. Springer. Bibliography 227 Umili, E., Tognon, M., Sanalitro, D., Oriolo, G., and Franchi, A. (2020). Communication-based and communication-less approaches for robust cooperative planning in construction with a team of uavs. In International Conference on Unmanned Aircraft Systems (ICUAS), pages 279–288. Varadharajan, V. S., St-Onge, D., ˇ Svogor, I., and Beltrame, G. (2017). A software ecosystem for autonomous UAV swarms. In International Symposium on Aerial Robotics. Philadelphia, PA, USA. Vatavuk, I., Poli´c, M., Hrabar, I., Petric, F., Orsag, M., and Bogdan, S. (2022). Autonomous, mobile manipulation in a wall-building scenario: Team LARICS at MBZIRC 2020. Field Robotics, 2(1):201–221. Visiongain (2011). The Unmanned Aerial Vehicles (UAV) Market 2011-2021: Technologies for ISR and Counter-Insurgency. Technical report, Visiongain. Vrba, M. and Saska, M. (2020). Marker-less micro aerial vehicle detection and localization using convolutional neural networks. IEEE Robotics and Automation Letters, 5(2):2459–2466. Walter, V., Spurny, V., Petrlik, M., Baca, T., Zaitlik, D., Demkiv, L., , and Saska, M. (2022). Extinguishing real fires by fully autonomous multirotor uavs in the mbzirc 2020 competition. Field Robotics, 2:406–436. Whitaker, T. M. and Corson, M. (2017). Tethered unmanned aerial vehicle fire fighting system. US Patent 9,764,839. Winfield, A. F. T., Franco, M. P., Brueggemann, B., Castro, A., Limon, M. C., Ferri, G., Ferreira, F., Liu, X., Petillot, Y., Roning, J., Schneider, F., Stengler, E., Sosa, D., and Viguria, A. (2016). euRathlon 2015: A multi-domain multi-robot grand challenge for search and rescue robots. In Alboul, L., Damian, D., and Aitken, J. M., editors, Towards Autonomous Robotic Systems, pages 351–363, Cham. Springer International Publishing. 228 Bibliography Xiao, K., Tan, S., Wang, G., An, X., Wang, X., and Wang, X. (2020). XTDrone: A Customizable Multi-rotor UAVs Simulation Platform. In International Conference on Robotics and Automation Sciences, pages 55–61. Yuan, C., Liu, Z., and Zhang, Y. (2016). Vision-based forest fire detection in aerial images for firefighting using uavs. In 2016 International Conference on Unmanned Aircraft Systems (ICUAS), pages 1200–1205. Yuan, C., Liu, Z., and Zhang, Y. (2019). Learning-based smoke detection for unmanned aerial vehicles applied to forest fire surveillance. Journal of Intelligent & Robotic Systems, 93(1-2):337–349. Yuen, H., Princen, J., Illingworth, J., and Kittler, J. (1990). Comparative study of hough transform methods for circle finding. Image and Vision Computing, 8(1):71–77. Y¨uksel, B., Buondonno, G., and Franchi, A. (2016). Differential flatness and control of protocentric aerial manipulators with any number of arms and mixed rigid- /elastic-joints. In Intelligent Robots and Systems (IROS), IEEE/RSJ International Conference on, pages 561–566. IEEE. Zhou, B., Pan, J., Gao, F., and Shen, S. (2021). RAPTOR: Robust and Perception- Aware Trajectory Replanning for Quadrotor Fast Flight. IEEE Transactions on Robotics, 37(6):1992–2009. Zhou, Y., Cheng, N., Lu, N., and Shen, X. (2015). Multi-uav-aided networks: Aerialground cooperative vehicular networking architecture. IEEE Vehicular Technology Magazine, 10:36–44.