Multi-UAV system for human support in inspection operations with multiple views

Proyecto Fin de Carrera Ingeniería de Telecomunicación Formato de Publicación de la Escuela Técnica Superior de Ingeniería Autor: F. Javier Payán Somet Tutor: Juan José Murillo Fuentes Dep. Teoría de la Señal y Comunicaciones Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2013 Trabajo Fin de Máster Máster en Ingeniería Electrónica, Robótica y Automática Multi-UAV system for human support in inspection operations with multiple views Autor: Damián Jesús Pérez Morales Tutor: Jesús Capitán Fernández, Alfonso Ángel Alcántara Marín Dpto. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2021 Trabajo Fin de Máster Máster en Ingeniería Electrónica, Robótica y Automática Multi-UAV system for human support in inspection operations with multiple views Autor: Damián Jesús Pérez Morales Tutor: Jesús Capitán Fernández, Alfonso Ángel Alcántara Marín Professor Contratado Doctor, Estudiante de Doctorado Dpto. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2021 Trabajo Fin de Máster: Multi-UAV system for human support in inspection operations with multiple views Autor: Damián Jesús Pérez Morales Tutor: Jesús Capitán Fernández, Alfonso Ángel Alcántara Marín El tribunal nombrado para juzgar el trabajo arriba indicado, compuesto por los siguientes profesores: Presidente: Vocal/es: Secretario: acuerdan otorgarle la calificación de: El Secretario del Tribunal Fecha: Acknowledgement Before anyone else, I would like to thank some members of the GRVC. Dr. Jesús Capitán Fernández, my tutor, for his assistance, for his kindness and for his valuable contribution to this project. I would also like to thank Alfonso Alcántara Marín, my teammate project, for his boundless patience, for his attention and for helping me to learn much more about this world. And lastly, I would like to thank Álvaro Calvo Matos, my schoolmate and my friend, for his endurance to stand my most difficult times with the project and along the school year. I do not forget to express my gratitude to my whole family. Their incredible hope for me has encouraged me to be where I am now and I will be eternally thankful for it. Last but not least, I have to show my infinite happiness and luck for having such good friends, to all of them. Thanks for everything Damián Jesús Pérez Morales Sevilla, 2021 I Abstract This thesis presents a trajectory generation method for a multi-UAV team performing inspection operations in a wind turbine. The main objective is to provide different views of the wind turbine to a ground operator, giving better perspectives of what is being supervised. To make this possible, a formation is used that follows a leader-follower scheme in a decentralized way. Each UAV calculates an initial trajectory according to the operator’s commands, maintaining visual properties. Afterwards, these paths are automatically optimized, minimizing accelerations in order to obtain smooth paths avoiding jerky movements. The presented method is evaluated in SITL (Software In The Loop) simulations. III XContents 4.1 Simulation environment 25 4.2 Simulations 26 5 Conclusions and future work 33 Appendix A Alternative method to generate reference paths 35 A.1 VXY cruising speed 35 A.1.1 Advantages and disadvantages 36 List of Figures 39 List of Tables 41 List of Codes 43 Bibliography 45 1 Introduction 1.1 Motivation Unmanned Aerial Vehicles (UAVs) have been recently growing for a wide spectrum of applications such as cinematographic shots [ 1 ], forest fire sighting [ 2 ], breakdown detection or inspection tasks [ 3 ] (see Figure 1.1). They stand out for their reduced size and weight, allowing them to be used under challenging scenarios that require high manoeuvrability. Besides, UAVs can integrate different sensors onboard, such as a gimbal with RGB or thermal cameras, laser sensors, etc. In principle, taking video shots with a UAV requires a well-trained operator to fly the vehicle and handle the camera movement and framing. The idea of conceiving UAVs that can accomplish these tasks autonomously is interesting to reduce human operators’ burden. Besides, structural repair activities in complex areas (wind turbines, electrical towers) are carried out by humans. They usually need a supervisor on the ground who monitors the mission and commands high-level decisions. The images provided by UAVs could be of great use to the supervisor, for instance, in an inspection performed in a complex access area. Multi-UAV teams can play an essential role in that application, having more advantages than a single UAV. A formation could increase the covering scene, providing images from different angles and reducing mission time, or increasing the robustness to robot failures. Teams of UAVs are already being used in multiple applications like target search [ 4 ], exploration [ 5 ], or surveillance [ 6 ], where operational times can be significantly reduced using multiples UAVs. Figure 1.1 UAVs performing inspection tasks of wind turbines. 1 2Chapter 1. Introduction Figure 1.2 DURABLE logo. This thesis focuses on a multi-UAV formation that gives extra visual information to operators inspecting the structure themselves or supervising the inspection operation to help and lead the process from the ground. The obtained images could even be integrated with Augmented Reality (AR) technologies in the future, creating a user interface that gives relevant information about the inspection operation to the ground operator. Through that interface, the operator could handle some functionalities or change the behaviour of the formation if needed during the inspection operation. Augmented Reality is being used in robotics as a new medium for interaction and information exchange with autonomous systems, increasing the Human-Robot Interaction (HRI) efficiency [ 7 ]. However, autonomous multi-UAV systems present several issues. One of them is collisionavoidance, with several vehicles in the air, it is necessary that they do not go into each others’ safe zones. Besides, most coordination strategies are centralized, making them computationally intractable in real-time, so a decentralized approach is often necessary. In applications with cameras, it is also necessary not to overlap others’ field of view. Due to these problems, there is still a need for multi-UAV autonomous systems that are intelligent enough to execute tasks coordinately, for instance, filming multiple action points in real-time. If we add the problems mentioned above to the fact that UAVs with cameras need to navigate smoothly, the trajectory planning problem becomes complex. Traditional path planning techniques are not sufficient to satisfy the requirements of the aforementioned applications, since the paths generated only rely on spatial information, and they may not actually be performed due to the dynamics of the robot, or may not achieve the smoothness requirements of applications with cameras. A trajectory is a time parameterized function of robot states that contains both spatial and temporal information. Trajectories recover the full states of a dynamic system such that they are adequate to guarantee the system’s feasibility, safety, and smoothness if it is required. Since system dynamics and time-varying states are taken into account, trajectory generation is more complicated than path planning. This thesis focuses on the trajectory planning problem for multi-UAV wind turbine inspection, where multiple cameras are involved. The thesis is motivated by the objectives set forth in the scope of the EU-funded project DURABLE 1 (see Figure 1.2). The objective of DURABLE is the development of autonomous technologies concerned with inspection tasks on renewable energy sources, as wind turbines or solar panels. 1The main webpage of DURABLE is the following: www.durableproject.eu 1.2 Objectives 3 1.2 Objectives The global objective of this thesis is to develop a multi-UAV system which supports a wind turbine inspection operation providing different views of the wind turbine to an operator who is on the ground. The operator can send high-level commands to the formation in order to film different views of the inspected structure. For this purpose, the following sub-objectives are defined: • Generating reference trajectories according to the high-level commands commanded by an operator. These trajectories should maintain certain formation and visual properties for the camera images. • Generating optimal trajectories by defining an optimization problem that minimizes the accelerations in order to achieve smooth trajectories. • Running the system online in real time. For this purpose, a receding horizon approach is used, and the system runs in a decentralized way. This allows recalculating trajectories at a high rate, to take into account disturbances. •Developing a multi-UAV software architecture based on ROS. • Performing Software In The Loop (SITL) simulations in a realistic environment, to prove that the method is able to generate trajectories for a formation of UAVs inspecting a wind turbine. 2 Preliminaries This chapter focuses on the evolution of UAVs and renewable energy, two key technologies in this project. Both technologies have evolved quite fast over the last years and have significantly brought several benefits, especially in inspection applications. Besides, we present a related work citing publications about the technologies used in this thesis, i.e., inspection applications, trajectory planning in filming applications, trajectory planning with multi-UAV systems and Augmented Reality. 2.1 Technology evolution 2.1.1 Renewable energy sources It is known that renewable energy sources have existed for a long time to generate electrical energy from natural resources (not fossil fuels): solar energy, wind energy, potential energy, biomass, among others. One of the most symbolic renewable energy sources in history could be the windmill or the watermill. These sources generate electrical energy from making rotations around the mill, either with wind energy or potential energy. With the evolution of technology and the concern about climate change, renewable energy sources have improved very fast, being the primary energy source in many countries. Nowadays, the most used sources are: •Solar energy: solar panels •Wind energy: wind turbines •Potential energy: hydroelectric power plant •Biomass: biomass plant In the last year, 2020, wind energy has got the record of renewable energy production in Spain according to Red Eléctrica Española (REE). This entity manages the electrical energy in Spain, producing 21’7% of the total energy. In Figure 2.1, a graph with the energy produced in Spain is presented. Due to recent technological improvements, it is possible to increasingly reduce the use of fossil fuels and, in the following years, green energy could cover almost the 100% of the whole consumption. 2.1.2 UAVs Over the last decade, the use of UAVs has been continuously growing for a wide spectrum of applications [ 8 , 9 ]. In some of these applications, UAVs collect data by using cameras or sensors, 5 6Chapter 2. Preliminaries Figure 2.1 Total energy produced (in percent) in Spain (2020). Source: energias-renovables.com. Figure 2.2 GB-1 Glide air bomb. Source: eldrone.es. e.g., for filming or remote sensing [ 10 , 11 , 12 , 13 ]; whereas in others, they are equipped with end-effectors to accomplish aerial manipulation tasks [14, 15, 16, 17]. The beginning of UAVs dates back to the XIX century, where UAVs were mainly used for military tasks, for instance, to bombard a target spot or for vigilance (see Figure 2.2). Basically, as a weapon. When Nikola Tesla discovered and created radio control in 1898, the applications diversified and got bigger, due to the interaction with the UAV via radio control. However, even though the main uses throughout history were military, UAVs have also shown their positive side. Nowadays, UAVs’ technology has evolved quite fast compared to what UAVs were before. Being now relatively small and light, UAVs have become the most powerful tool in many varied applications (see Figure 2.3). For instance, UAVs are now used in research [ 9 ], rescue [4], conservation [3], infrastructure inspection [18, 19], forest fire detections [2], among others. 2.2 Related work Inspection tasks with UAVs can help to check the status of the object/building with the maximum number of points of view possible. This has a wide variety of applications. This section surveys different aspects related to UAV inspection technologies: inspection applications (Section 2.2.1), UAV filming (Section 2.2.2), trajectory planning (Section 2.2.3) and Augmented Reality (Section 2.2.4). 2.2 Related work 7 Figure 2.3 Parrot Anafi 4K Quadrotor. Source: parrot.com. 2.2.1 Inspection applications with UAVs In the construction field, inspection of bridges is essential to keep its structural health and check wears in general. In [ 18 ], UAVs at bridge inspection perform a very important role as they can check every corner and every spot due to its manoeuvrability. UAVs also develop an important role in renewable energy sources. As mentioned before, solar panels are one of the most important renewable sources that we have, at least for now, and it is crucial to keep all PV modules working and detecting broken down cells as soon as possible. For instance, [ 19 ] developed a fault detection system using a single UAV and machine vision, which is basically working with a thermal camera and a Charge-Coupled Device mounted on a single UAV, that detects these faults and gives location information in terms of panel location by longitude and latitude. Also, in [ 20 ] faults on solar panels are detected with thermal infrared hitched on a single UAV that generates an orthographic image to make the inspection easier and distinguish accurately between normal and abnormal panel points. As wind turbines are a large-sized structure, they need to be periodically checked, and the vibrations of the blades can determine the structural health. These vibrations are detected using Digital Image Correlation (DIC) and prevent catastrophic failures. UAVs have a remarkable potential for this work. In [ 21 ], the vibration of rotating wind turbines was detected using a single semiautonomous vehicle. Surface damages in wind turbines have also been detected using UAVs with cameras and deep learning [ 22 ]. Figure 2.4 shows an example of such detection. In [ 23 ], they inspect blades using a single UAV with a LiDAR with minimal operator involvement. It is known that thunderstorms may affect in a bad way to all kind of electrical equipment, especially to those exposed outdoors, for instance, voltage towers and electrical transport elements in general. Wind turbines can collect a huge load of electrostatic discharge (ESD), making the turbines not work right because of the induced electromotive force acting on the electrical system. In [ 24 ], this problem is contemplated and studied using tethered UAVs, but it is still being investigated. Regarding localization, GPS is not always available close to wind turbines. In [ 25 ], they perform UAV localization around wind turbines using monocular model-based tracking . This is possible by making a 3D skeleton of the wind turbine and matching image data by means of Convolutional Neural Network (CNN) and, then, fusing this information with GPS and IMU data to improve its accuracy. In many of the aforementioned works, the UAV inspection is done by the vehicle itself in an autonomous/semi-autonomous way (let us call that direct-inspect UAV), without the necessity of having a ground operator supervising its flight, as for instance in [ 21 , 23 ]. However, UAVs could also give extra information to a ground operator that is supervising the operation from the ground [ 18 , 22 ]. In such cases, human intervention is necessary and the UAV can play a key role monitoring the operation and allowing the operation to be supervised from ground. In this thesis, we focus on that type of application. 8Chapter 2. Preliminaries Figure 2.4 Detection of sensors and erosions of wind turbine’s blades. Source: [ 22 ]. They are able to detect the erosion of the blades, the VG panels even if there are missing teeths in them, the different sensors integrated in the wind turbine, etc. All this is detected with a high rate of success. Most of the applications already mentioned are operated with a single UAV. However, a multi-UAV formation can offer multiple images from different points of view at the same time, giving extra information to a ground operator. In [ 26 ], they operate on a cinematography shot application with multiple UAVs that grants the director to have multiple views at the same time. Teams with multiple UAVs can provide alternative points of view, but also supplementary illumination. For instance, in [ 27 ], efficient variable illumination plays a key role for documentation of historical buildings interiors. 2.2.2 Trajectory planning in filming applications In the computer animation community, there are several works related with trajectory planning for the motion of virtual cameras [ 28 ]. They typically use offline optimization to generate smooth trajectories that are visually pleasant and comply with certain cinematographic aspects, like the rule of thirds. However, many of them do not ensure physical feasibility to comply with UAV dynamic constraints and they assume full knowledge of the environment map. In terms of optimization functions, several works consider similar terms to achieve smoothness. For instance, authors in [ 29 ] model trajectories as polynomial curves whose coefficients are computed to minimize snap (fourth derivative). They also check dynamic feasibility along the planned trajectories, and the user is allowed to adjust the UAV velocity at execution time. A similar application to design UAV trajectories for outdoor filming is proposed in [ 30 ]. Timed reference trajectories are generated from 3D positions specified by the user, and the final timing of the shots is addressed by designing easing curves that drive the UAV along the planned trajectory (i.e., curves that modify the UAV velocity profile). In [ 31 ], aesthetically pleasant footage is achieved by penalizing the snap of the UAV trajectory and the jerk (third derivative) of the camera motion. An iterative quadratic optimization problem is formulated to compute trajectories for the camera and the look-at point (i.e., the place where the camera is pointing at). They also include collision avoidance constraints, but the method is only tested indoors. Although these articles on computer graphics approach the problem mainly through offline optimization, some of them have proposed options to achieve real-time performance, like planning in a toric space [ 32 ] or interpolating polynomial curves [ 33 , 30 ]. In general, these works present 2.2 Related work 9 interesting theoretical properties, but they are restricted to offline optimization with a fully known map of the scenario and static or close-to-static guided tour scenes, i.e., without moving actors. In the robotics literature, there are works focusing more on filming dynamic scenes and complying with physical UAV constraints. For example, authors in [ 34 ] propose to detect limbs movement of a human for outdoor filming. Trajectory planning is performed online with polynomial curves that minimize snap. In [ 35 , 12 ], they present an integrated system for outdoor cinematography, combining vision-based target localization with trajectory planning and collision avoidance. For optimal trajectory planning, they apply gradient descent with differentiable cost functions. Smoothness is achieved minimizing trajectory jerk; and shot quality by defining objective curves fulfilling cinematographic constraints associated with relative angles w.r.t. the actor and shot scale. Cinematography optimal trajectories have also been computed in real time through receding horizon with non-linear constraints [ 36 ]. The user inputs framing objectives for one or several targets on the image, and errors of the image target projections, sizes and relative viewing angles are minimized; satisfying collision avoidance constraints and target visibility. The method behaves well for online numerical optimization, but it is only tested in indoor settings. Some of the aforementioned authors from robotics have also approached UAV cinematography applying machine learning techniques. In particular, learning from demonstration to imitate professional cameraman’s behaviors [ 10 ] or reinforcement learning to achieve visually pleasant shots [ 11 ]. In general, most of these cited works on robotics present results quite interesting in terms of operation outdoors or online trajectory planning, but they always restrict to a single UAV. Regarding methods for multiple UAVs, there is some related work which is worth mentioning. In [ 37 ], a non-linear optimization problem is solved in a receding horizon fashion, taking into account collision avoidance constraints with the filmed actors and between the UAVs. Aesthetic objectives are introduced by the user as virtual reference trails. Then, UAVs receive current plans from all others at each planning iteration and compute collision-free trajectories sequentially. A UAV toric space is proposed in [ 38 ] to ensure that cinematographic properties and dynamic constraints are ensured along the trajectories. Non-linear optimization is applied to generate polynomial curves with minimum curvature variation, accounting for target visibility and collision avoidance. The motion of multiple UAVs around dynamic targets is coordinated by means of a centralized master-slave approach to solve conflicts. Even though these works present promising results for multi-UAV teams, they are only demonstrated at indoor scenarios where a Vicon motion capture system provides accurate positioning for all targets and UAVs. These works present quite valuable contributions for cinematography with multiple UAVs, but they are evaluated in indoor settings. The specifics of the outdoor scenarios considered in our work are different in several aspects, as the environment is less controlled: UAVs require more payload to carry onboard cameras with better lenses and equipment for larger range communication; achieving smooth trajectories is more complex due to external factors such as wind gusts or communication delays; UAV positioning is less accurate in general; and so on. 2.2.3 Trajectory planning with multi-UAV systems Multi-UAV trajectory planning aims to provide guidance information such as the optimized trajectories for the formation to benefit the coordination of multiple vehicles [ ? ]. In addition, when computing the plan, apart from the costs considered in conventional planning, constraints specifically related to the formation need to be considered to facilitate the formation control. Thus, internal collision avoidance has to be considered to ensure the safety of the formation, and some formation behaviours like shape keeping is often desired. Moreover, it is usually necessary to achieve cooperative behaviour depending on the application. In [ 39 ], they provide a survey of multi-UAV trajectory planning for formation control and cooperative planning. Regarding the formation control strategies, works can be classified in leader-follower formations, virtual formations or behaviour-based formations [ 39 ]. In the leader-follower approach, one vehicle 16 Chapter 3. Methodology Figure 3.1 Top view of the leader-Follower scheme. Graphical representation of the relative angle, inspection distance and inspection point.. leader attempts to compute a reference trajectory formulated in polar coordinates, whereas the followers maintain a surrounding formation with the desired configuration. On the one hand, the trajectory generation procedure of the leader is shown in Algorithm 1. On the other hand, the followers calculate their trajectories taking into account the leader trajectory and the formation parameters, in order to maintain the desired formation. This procedure is shown in Algorithm 2. Algorithm 1 Leader UAV 1: procedure Leader(Waypoint to reach, inspected point, inspection distance) 2: System Initialization 3: while Exist waypoint to reach && Mission is started do .For each cycle 4: Reference path ←Waypoint to reach, inspected point, inspection distance 5: Optimal path ←Reference path 6: Output: Leader trajectory Algorithm 2 Follower UAV 1: procedure Follower(Leader’s reference path, formation angle) 2: System Initialization 3: while Exist leader’s reference path do .For each cycle 4: Input: Leader’s reference path, formation angle 5: Follower’s reference path ←Rotate ±formation angle Leader’s reference path 6: Optimal path ←Reference path 7: Output: Follower trajectory The entire pipeline shown in Figure 3.2 (except for the human operator interface) runs on board each UAV in a receding horizon manner. This enables online planning to react properly to changes 3.2 Problem formulation 17 in the operator inputs, as well as to malfunctioning team-members or previously unseen obstacles. Trajectory Planner infers a planned trajectory that either the leader trajectory generator mentioned in Algorithm 1 or the follower trajectory generator mentioned in Algorithm 2 is activated on each UAV, depending on its role. The Trajectory Follower calculates 3D velocity commands at a higher rate so that the UAV follows the planned optimal trajectory, which is updated any time the Trajectory Planner generates a new solution. The UAV Abstraction Layer (UAL) is a software component developed by our lab [ 73 ] to interface with the position and velocity controllers of the UAV autopilot. It provides a common interface abstracting the user from the protocol of each specific autopilot hardware. Finally, recall that each UAV has a communication link with other teammates in order to share their current computed trajectories, which are used for multi-UAV coordination by the Trajectory Planner. Figure 3.2 System overview scheme. 3.2 Problem formulation This section describes the formulation of the optimization problems that are solved for trajectory generation, including the cost functions to be minimized and the involved constraints. 3.2.1 Dynamic model Since we have a trajectory follower that allows us to follow the desired paths with enough accuracy, we formulate our trajectory optimization using a simple kinematic UAV model (using position, velocity and acceleration), as more complex non-linear models would increase the computational cost. In addition, the camera’s orientation needs to be modelled. For that, we assume the existence of a gimbal mechanism to compensate angle deviations due to changes in UAV attitude. Therefore, it is assumed that camera roll is negligible and we only control pitch and heading. Since the heading of a multi-rotor vehicle can be controlled independently of its position, we command the gimbal’s pitch and the UAV heading to point the camera to the inspected point. The positional part of the dynamic model is defined as a linear double integrator: ˙ p=v, ˙v=,(3.1) 18 Chapter 3. Methodology where p= [ppypz]T∈R3 is the UAV position, v= [yz]T∈R3 the linear velocity, and = [yz]T∈R3 the linear acceleration. The camera position is assumed to have the same position as the UAV. The orientation of the camera may be modelled similarly: ˙ o=ω ω ω, ˙ ω ω ω=θ θ θ, (3.2) where o= [φ ξ]T represents an orientation with respect to a global frame given by its heading/yaw and pitch angles, ω ω ω∈R2 are the corresponding angular rates, and θ θ θ∈R2 the angular accelerations. For the description of the proposed method, we define a full positional state of the UAV xp= [pTvT]T∈R6 , a vector of positional control inputs p= , an orientation state xo= [oTω ω ωT]T∈R4, and a vector of orientation control inputs o=θ θ θ. 3.2.2 Generation of reference paths The implemented formulation to generate the leader reference paths in this work is going to be explained in this section, but there is another formulation that was tested before and also works correctly that can be seen in Appendix A. are generated by means of a geometric procedure given the next waypoint to reach, the inspected point and the inspection distance, as mentioned in Algorithm 1, step 4. As a result, the path will fit a cylinder of radius equal to the inspection distance and centered in the inspected point. Thus, the leader will orbit around the inspected point, passing through the desired waypoints commanded by the human operator and maintaining visual properties: having the most stable camera vision possible and pointing at the inspected point. For that purpose, the reference trajectory is parametrized in cylindrical coordinates. A parametric interpolation is computed given a waypoint 0= [r0θ0z0] and 1= [r1θ1z1] and a parameter t∈[0,1] . 0 represents the current position and 1 the desired waypoint to reach, both in cylindrical coordinates. The parametrized equations of the curve are: r=r0+ (r1−r0)∗t θ=θ0+ (θtot)∗t z=z0+ (z1−z0)∗t (3.3) θtot =         θ2−θ1−2π, if θ2−θ1> π θ2−θ1,if θ2−θ1< π nd θ2−θ1<0 θ2−θ1+2π, if θ2−θ1<−π θ2−θ1,if θ2−θ1< π nd θ2−θ1>0 (3.4) We know that if t=1, the curve length Lis: L=q(R2(θtot)2+ (z1−z0)2)(3.5) If we want to interpolate N points separated by tstep seconds at a cruising velocity Vc , applying the rule of three, for each point k we have its corresponding tk: tk=Vc∗tstep ∗k Æ(Rθtot)2+ (z1−z0)2(3.6) 3.2 Problem formulation 19 In the case of the followers, their reference path is a rotation of relative_angle radians around the Z axis of the leader UAV’s path, as mentioned in Algorithm 2, step 5: PRF = (PRL −p)∗RZ(rete_nge),(3.7) where PRF is the reference path of the follower, which is a list of the reference points that the follower has to reach; PRL is the reference path (list) of the leader, p is the inspected point and RZ is: RZ(θ) =   cos(θ)−sn(θ)0 sn(θ)cos(θ)0 0 0 1 (3.8) The reason of doing the subtraction of p to PRL is to do a rotation of each point from the leader’s reference path around p (relative_angle radians around the Z axis) to obtain PRF . In other words, it is the translated list of points what is rotated by relative_angle radians around the Z axis. 3.2.3 Trajectory optimization Our main objective is to generate optimal trajectories taking into account smoothness, as mentioned in Algorithm 1, step 5. Thus, we formulate an optimization problem minimizing the deviations with respect to the reference trajectory and accelerations in order to avoid jerky movements. Moreover, we add the UAV kinematic model and dynamic limitations as constraints. The whole problem formulation is the following where, for the sake of simplicity, we denoted x:=xpand :=p: minimize x0,...,xN 0,...,N N X k=1 (||xd,k −xk||2+β||k−1||2),(3.9) subject to x0=x0(3.9.a) xk+1=ƒ(xk,k)k=0,...,N −1(3.9.b) mn ≤k≤m (3.9.c) vmn ≤vk≤vm (3.9.d) where the optimization variables xk = [ pkvk ] and uk = ak are the discretized states (position and velocity) and the control inputs (acceleration) of the system in each tk . xd,k is a list of the desired reference points to reach by the UAV that comes from Section 3.2.2 and its size depends on the receding horizon length. In case there is a predefined no-fly zone (e.g., around the wind turbine), a new constraint could be added to the optimization problem. However, this constraint is non-convex, which complicates quite a lot the optimization problem. Fortunately, we have an external module that generates reference paths (see Section 3.2.2) and this constraint can be considered implicitly in the generation of the reference paths. This way, the optimization solver does not consider a non-convex constraint and makes the optimization problem quicker and more robust to find solutions. In 3.9.a, x’ is the current observed state value of the UAV, that is considered the initial state for the formulation x0 . The system dynamics are expressed in 3.9.b, while 3.9.c and 3.9.d indicate bounds for the UAV velocity and acceleration, respectively. In this application, cameras need to always be pointing at the filmed target. Hence, their desired orientation is given by: od=φdξdT=arctan(qy,q)sinqz ||q|| T.(3.10) 20 Chapter 3. Methodology where q= [qqyqz] = p−p , that is the relative position between the position of the camera (assuming that is the same of the UAV) and the inspected point, being q the relative position in X axis, qy the relative position in Y axis and qz the relative position in Z axis. With this information, the desired heading/yaw (φd) and pitch (ξd) orientation can be inferred. Orientation control is also formulated as a constrained quadratic optimization problem in receding horizon in order to achieve smoother orientation changes. For simplicity of the description, x:=xo and :=oin the following problem formulation: minimize 0,...,N−1 N X k=1 (||od,k −ok||2+γ||k−1||2),(3.11) subject to x0=x0,(3.11.a) xk+1=fo(xk,k)∀k∈{0,...,N −1},(3.11.b) ω ω ωmn ≤ω ω ωk≤ω ω ωm ∀k∈{1,...,N},(3.11.c) ξmn ≤ξk≤ξm ∀k∈{1,...,N},(3.11.d) mn ≤k≤m ∀k∈{0,...,N −1},(3.11.e) where fo(·) represents the orientation aspect of the dynamic model defined in Section 3.2.1; ωmn , ωm are limitations on the angular velocities; mn , m control inputs limitations; and ξmn , ξm represent hardware limitations of the gimbal to adjust pitch angles. The heading and pitch angles of the camera can be controlled independently. Thus, Problem (3.11) was decoupled into two simpler problems. The Trajectory Follower, which is the responsible of sending velocity commands to the Autopilot (see Figure 3.2), has a rate of 100 Hz; and Trajectory Planning is executed with a rate of 1 Hz, having a planned trajectory 4-second long, due to the receding horizon implemented (40 steps as receding horizon, N , with each step as 0.1 seconds). The receding horizon approach allows the UAV to have a trajectory to follow for the next 4 seconds, which leads to smoother movements, as this trajectory is replanned each second. The followers’ optimal paths, as mentioned in Algorithm 2, step 6, are computed in a similar manner. We use the same optimization problem, but using the follower’s reference paths instead. 3.3 Implementation In this section, we provide more details about the software implementation of the method: class diagram, pseudocodes and communications between modules. All code is open-source and available online1, and it was developed under Ubuntu 18.04 Operating System and ROS Melodic. 3.3.1 Class diagram Figure 3.3 depicts the class diagram for the software architecture developed in this project. It has four classes in total: Trajectory Planner , Trajectory Planner Leader (which inherits from Trajectory Planner ), Trajectory Planner Follower (which inherits from Trajectory Planner ), and Trajectory Planner Ros , which wraps the code adding ROS communications. The idea behind defining the base class Trajectory Planner is that every UAV can inherit from it and implement its own version of the initial trajectory calculation and, if necessary, its own version of the optimization phase. Most of the methods needed for the planning part are common and implemented in the Trajectory Planner class, therefore, we do not duplicate code with this implementation. 1https://github.com/grvcTeam/inspection_trajectory_planning 3.3 Implementation 21 Figure 3.3 Class diagram. The blue colour on the edge symbolizes the base classes and the red colour symbolizes inherited classes. The leader node instantiates a Trajectory Planner Leader object. This node executes several methods that check if there are waypoints to reach and the mission is started to generate a reference trajectory and later optimize it, sending optimized trajectories to the followers. In the case of the follower node, it instantiates a Trajectory Planner Follower object which has its own implementation of the initial trajectory calculation method. The follower object calculates a rotation of the previous reference path done by the leader UAV around the Z axis to maintain the formation angle, and then, executes the optimization problem. We have implemented the Trajectory Planner Ros class which wraps the code explained before adding ROS communication properties to each node (topics and services). This class contains a Trajectory Planner member that use virtual functions to wrap a leader or a follower, depending on the node executed. 3.3.2 Pseudocodes In this section, we show some pseudocodes describing the global mission process (Code 3.1), how the initial trajectories are inferred (Code 3.2), the leader node (Code 3.3), the follower node (Code 3.4) and the steps that the solver does to get the optimal path (Code 3.5). 1. Launch the UAV nodes and simulation (.launch) 2. Setup the configuration of the mission (parameters) 3. Wait until UAVs are armed and ready to start the mission 4. Take off all the UAVs 5. Start the mission 6. While (True) 6.1. Read the waypoints not reached yet 6.2. While there are waypoints yet to reach and the mission is started 6.2.1. Calculate the UAVs’ initial trajectory initial_traj_to _follow 6.2.2. Calculate the UAVs’ optimal trajectory solved_ trajectory 6.3. Wait until the mission is resumed Code 3.1 Overall code functionality. In Code 3.2, it is described the process of getting the initial trajectories explained in Section 3.2.2, as done in in the leader UAV node. 22 Chapter 3. Methodology 1. Get the current position pc_xyz (Xc, Yc, Zc) 2. Get the desired position pd_xyz (Xd, Yd, Zd) 3. Transform both current and desired position in cylindrical coordinates: pc_rtz (rho_c, theta_c, z_c), pd_rtz (rho_d, theta_d, z_d) 4. Get the total theta_angle to go down from current position to desired position 5. Get the total curve length L 6. For loop from parameter k = 0 to horizon_length - 1: 6.1. Get parameter t_k in each iteration 6.2. Get pk_rtz = (rho_k, theta_k, z_k) 6.3. Transform pk_rtz to pk_xyz (Xk, Yk, Zk) 6.4. Stack/push to the back pk_xyz to the vector initial_traj_to_ follow Code 3.2 Initial trajectory pseudocode. Once the initial trajectory is calculated, the leader UAV node only has left to optimize the path and send it to the autopilot (see Code 3.3). 1. Infer the initial trajectory 2. Send initial_traj_to_follow to optimize the path (solved_traj) optimal_traj = solved_traj 3. Send solved_traj to autopilot Code 3.3 Optimal trajectory pseudocode for leader UAV. As mentioned in Algorithm 2, the follower UAV’s node gets the initial_traj_to_follow path (Code 3.2), rotates it relative_angle radians and, afterwards, it optimizes the path. The pseudocode is represented in Code 3.4. 1. Get leader’s UAV reference_trajectory 2. Get the current relative_angle 3. Rotate relative_angle rad around Z axis leader’s UAV reference_ trajectory 4. Send reference_trajectory to optimize the path optimal_traj = solved_traj 5. Send solved_traj to autopilot Code 3.4 Optimal trajectory pseudocode for follower UAV. It is important to insist that the UAVs’ nodes are run in a decentralized way. Also, it is important to recall how the optimal paths for the UAVs are generated in order to avoid the jerky movements that the reference trajectory path could have, minimizing the accelerations and generating velocities to send to each autopilot. The steps of the optimization problem are shown in Code 3.5. 1. Declaration of DifferentialStates, Controls and the DifferentialEquation 2. Define the model 3. Add constraints (maximum and minimum velocities/accelerations) 3.3 Implementation 23 4. Define a start position, velocity and acceleration (got from the first element from initial_traj_to_follow) 5. Setup the reference trajectory to follow 6. Define the weights of the costs to minimize 7. Minimize LSQ 8. Setup the parameters of the solver 9. Solve the optimization problem 10. Get the solved_traj and send it back to the UAV Code 3.5 Optimal trajectory (ACADO solver). 3.3.3 ROS: possible interactions with the operator The ROS nodes implement several services to interact with the ground operator: •Take off: it lets the UAV take off, giving a desired height to be set. • Add waypoint: it adds a waypoint given by the console with (X, Y, Z) coordinates and is sent to a queue of goals to reach orderly. •Clear waypoints: it cleans up the whole queue of goals mentioned before. • Changing the point to inspect: it lets you change the point to inspect. It could be useful if there are more than one wind turbine on the powerplant that need to be checked sequentially. Besides, we implement topics to publish commands with a joystick at high rate: •Distance to inspection point (inspection distance): it lets you increase/decrease the distance to the inspection point. • Relative angle: it lets you increase/decrease the relative angle between the leader UAV and the followers. These two topics are controlled by a virtual joystick that has been implemented in the operator interface. These services, in real life, are thought to be implemented with some kind of PAD or tablet set on the arm with a real joystick to operate those topics of distance and relative angles as well. The idea of the implementation is depicted in Figures 3.4 and 3.5. 24 Chapter 3. Methodology Figure 3.4 Operator interface aspect. Figure 3.5 Joystick aspect. 4 Results In this chapter, we are going to show the results of this work. In Section 4.1, we are going to explain how the simulation environment is, showing the scenario where the simulations are executed and the tools that were used. Later, in Section 4.2 we are going to talk about how the simulations are executed and the obtained results in our experiments. 4.1 Simulation environment We used the simulator Gazebo that was mentioned in Section 2.3.2. We created a simulation environment of the place where we will carry out our real experiments, which is a basketball court next to our lab (ETSI, Seville), to ease the transition to the real-world trials testing the algorithms in this simulator beforehand. Figure 4.1 shows the simulated environment. We have also implemented a realistic model of a wind turbine that can be seen in Figure 4.2, but this scenario is only used to develop a promotional video. We use RViz to visualize the reference and optimized trajectories of each UAV, the pose of the UAVs, the waypoints to inspect and the inspection structure. The appearance of the RViz interface can be seen in Figure 4.3: Figure 4.1 Our field experiment facility simulated in Gazebo. 25 5 Conclusions and future work This work has presented a method for autonomous multi-UAV trajectory planning for human support in inspection operations. We have proposed a novel methodology for multi-UAV trajectory planning, maintaining a desired formation depending on the operator commands and taking into account visual properties in order to get images from different points of view. We have demonstrated that the method minimizes the accelerations, obtaining smooth camera movements. Besides, we have performed Software In The Loop simulations, showing that we can plan trajectories in a distributed and online manner. As future work, we are currently working to verify the correct functionality of the multi-UAV system, by means of our first real experiments. We also plan to address synchronization issues of the trajectories that are received by the followers, in order to improve the tracking of the formation angles by advancing forward or backward the leader’s path (to take into account reception delays). Besides, we plan to implement obstacle avoidance in our trajectory optimization process, in order to avoid collisions with the blades in a turbine (or other obstacles in the scenario), when the operator wants to have a closer view of the infrastructure. We think that this method could also be integrated with an Augmented Reality device to provide extra information to the inspection operator on the ground. Thus, we plan to connect our system with an AR/VR headset to test this human-machine interaction. The idea is to design a virtual reality scenario where the operator can visualize several aspects of the mission, such as UAVs positions, camera images, inspected positions etc. Finally, this method for trajectory planning could be used to carry out other kinds of inspection tasks, not only for cylindrical structures, just with minor adjustments. 33 Appendix A Alternative method to generate reference paths In this appendix, we explain an alternative method to generate the reference paths with a cruising speed on the plane XY, VXY , instead of the different one included in Section 3.2.2, which is working with a cruising speed on XYZ, VXYZ . This means that the one exposed in this section has to describe a constant amount of angle in each tstep, which we called θpss, and has the Z speed component decoupled from the cruising speed. A.1 VXY cruising speed Recall that our formulation works with a constant-speed model. An interesting detail to mention of this formulation is that keeps the direction of the previous path unless the UAV is stopped. This aspect of the formulation lets the formation have no sudden changes of direction while they are flying, but means that they have to fly a larger path to reach the desired point in some cases. In order to determine which direction a UAV is following (clockwise or counter-clockwise), Equations A.1 and A.2 together with Figure A.1 can be used: cockse =         qdrnt 1if >0nd y<0 qdrnt 2if >0nd y>0 qdrnt 3if <0nd y>0 qdrnt 4if <0nd y<0 (A.1) conter −cockse =         qdrnt 1if <0nd y>0 qdrnt 2if <0nd y<0 qdrnt 3if >0nd y<0 qdrnt 4if >0nd y>0 (A.2) We formulate the problem to work with cylindrical coordinates: =ρ cos(θ) y=ρ sn(θ) z=z (A.3) 35 36 Chapter A. Alternative method to generate reference paths Figure A.1 Clockwise/counter-clockwise graphic. In this case, as we are working with a constant VXY cruising speed, θpss would be constant, which is the amount of angle that the UAVs describe in each tstep , that is the amount of time (in seconds) that is between each generated point of the path generator to make the optimization problem later. θpss =VXY tstep 2π R ∗2π(A.4) If the UAV is flying clockwise, θpss has to be subtracted to the current angle, otherwise, θpss has to be added to the current angle: θnet =¨θcrrent +θpss if conter −cockse θcrrent −θpss if cockse (A.5) A.1.1 Advantages and disadvantages There are two possibilities to approach this problem: considering the XYZ components on the maximum velocity or breaking it down into a maximum XY velocity and another maximum velocity for the Z component. A.1 VXY cruising speed 37 The main problems with taking independently the velocity on the XY plane and the Z axis are the following: VUAV =ÇV2 y +V2 z VXY =VmXY (A.6) • The UAV is forced to fly at a constant speed on the XY plane, which means that in each timestep, it will have the same pass angle. This may be controversial, as it seems to be advantageous because the UAV is more stable, causing smoother trajectories. • However, the UAV may have some difficulties if the next waypoint to reach is close on the XY plane from the previous waypoint (or even in the same point on XY, but having different Z coordinates) and in the same direction (clockwise/counter-clockwise) that the UAV has been flying. It will make the UAV go on a spiral path until it reaches the waypoint (which is not a bad solution though). • If that were not enough, the velocity on Z cannot go at the maximum speed of its component in almost any cases and this, considering the velocity on XYZ from the module of XY and Z velocity, does not approve the requisite of going through the different points with a cruising speed. Nevertheless, not everything is bad for the problem set out on independent velocity of XY and Z: • The fact that the UAV has a constant pass angle, due to the XY constant speed, causes throughout the UAV’s flight quite smooth changes between waypoints, as mentioned before. • In some cases, it would be interesting to be going across the waypoints on spirals to have a full view of the whole inspection structure. List of Figures 1.1 UAVs performing inspection tasks of wind turbines 1 1.2 DURABLE logo 2 2.1 Total energy produced (in percent) in Spain (2020). Source: energiasrenovables.com 6 2.2 GB-1 Glide air bomb. Source: eldrone.es 6 2.3 Parrot Anafi 4K Quadrotor. Source: parrot.com 7 2.4 Detection of sensors and erosions of wind turbine’s blades. Source: [ 22 ]. They are able to detect the erosion of the blades, the VG panels even if there are missing teeths in them, the different sensors integrated in the wind turbine, etc. All this is detected with a high rate of success 8 2.5 3D map construction with AR. Source: [68] 12 2.6 ROS logo. Source: ros.org 13 2.7 Gazebo simulator with an inspection scenario 13 2.8 RViz visualization tool 14 3.1 Top view of the leader-Follower scheme. Graphical representation of the relative angle, inspection distance and inspection point. 16 3.2 System overview scheme 17 3.3 Class diagram. The blue colour on the edge symbolizes the base classes and the red colour symbolizes inherited classes 21 3.4 Operator interface aspect 24 3.5 Joystick aspect 24 4.1 Our field experiment facility simulated in Gazebo 25 4.2 Simulated scenario with wind turbine 26 4.3 Snapshot of our RViz interface during a simulation 26 4.4 Operator’s interface, selection of the menu 27 4.5 Operator’s interface, joystick simulator 27 4.6 Simulation experiment. The UAVs’ trajectories are shown. Drone 1 (leader) is between the other two UAVs, which are following (Drone 2, Drone 3). They all describe a clockwise circle around the wind turbine (black star). Notice that there is a change in ralong the first turn over the inspection point 28 4.7 Simulation experiment. The UAVs’ distance to the inspection point are shown. It can be seen how the formation tracks the reference with some delay 29 39 40 List of Figures 4.8 Simulation experiment. The UAVs’ relative angles are shown. In the top plot, it is depicted the evolution of θr between the leader UAV (Drone 1) and the first follower UAV (Drone 2); while in the bottom plot, it is shown the evolution of θr between the leader UAV and the other follower (Drone 3). Both plots show the effect of having a lack of synchronization of the trajectories, with an angle offset 29 4.9 Simulation experiment. The UAVs’ linear accelerations are depicted for the optimized trajectories. There are some significant peaks for the followers (for instance, at t=32s or at t=48s ), which are due to changes on the reference ror θr30 4.10 Simulation experiment. The UAVs’ linear accelerations are depicted without optimization phase for the trajectories. Comparing with the experiment with the optimized trajectory (see Figure 4.9), there are more significant peaks on the UAVs’ accelerations 30 A.1 Clockwise/counter-clockwise graphic 36 List of Tables 4.1 Parameters of the simulation 27 4.2 Waypoints of the simulation 28 4.3 Average values of the accelerations with UAV trajectory optimization 31 4.4 Average values of the accelerations without UAV trajectory optimization 31 41 48 Bibliography [40] V. Krátký, P. Petráček, V. Spurný, and M. Saska, “Autonomous reflectance transformation imaging by a team of unmanned aerial vehicles,” IEEE Robotics and Automation Letters, vol. 5, no. 2, pp. 2302–2309, 2020. [41] H. G. Tanner and A. Kumar, “Towards decentralization of multi-robot navigation functions,” in Proceedings of the 2005 IEEE International Conference on Robotics and Automation. IEEE, 2005, pp. 4132–4137. [42] R. Tallamraju, S. Rajappa, M. J. Black, K. Karlapalem, and A. Ahmad, “Decentralized mpc based obstacle avoidance for multi-robot target tracking scenarios,” 2018 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), pp. 1–8, 2018. [43] E. Price, G. Lawless, R. Ludwig, I. Martinovic, M. Black, and A. Ahmad, “Deep Neural Network-based Cooperative Visual Tracking through Multiple Micro Aerial Vehicles,” IEEE Robotics and Automation Letters, vol. 3, no. 4, pp. 3193–3200, Oct. 2018. [Online]. Available: https://ieeexplore.ieee.org/document/8394622/ [44] A. Bucker, R. Bonatti, and S. Scherer, “Do you see what i see? coordinating multiple aerial cameras for robot cinematography,” arXiv preprint arXiv:2011.05437, 2020. [45] I. Maza, J. Capitán, L. Merino, and A. Ollero, Multi-UAV Cooperation, 12 2015. [46] I. Maza, A. Ollero, E. Casado, and D. Scarlatti, Classification of Multi-UAV Architectures. Dordrecht: Springer Netherlands, 2015, pp. 953–975. [Online]. Available: https: //doi.org/10.1007/978-90-481-9707-1_119 [47] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in IEEE International Conference on Robotics and Automation (ICRA), 2011, pp. 2520–2525. [48] F. Augugliaro, A. P. Schoellig, and R. D’Andrea, “Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach,” in 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2012, pp. 1917–1922. [49] M. Čáp, P. Novák, A. Kleiner, and M. Selecký, “Prioritized planning algorithms for trajectory coordination of multiple mobile robots,” IEEE Transactions on Automation Science and Engineering, vol. 12, no. 3, pp. 835–849, 2015. [50] C. Yu, J. Wang, J. Shan, and M. Xin, “Multi-uav uwa video surveillance system,” in 2016 14th International Conference on Control, Automation, Robotics and Vision (ICARCV), 2016, pp. 1–6. [51] D. R. Robinson, R. T. Mar, K. Estabridis, and G. Hewer, “An efficient algorithm for optimal trajectory generation for heterogeneous multi-agent systems in non-convex environments,” IEEE Robotics and Automation Letters, vol. 3, no. 2, pp. 1215–1222, 2018. [52] I. K. Erunsal, R. Ventura, and A. Martinoli, “Nonlinear Model Predictive Control for 3D Formation of Multirotor Micro Aerial Vehicles with Relative Sensing in Local Coordinates,” 2019 International Symposium on Multi-Robot and Multi-Agent Systems (MRS), 2019. [Online]. Available: http://arxiv.org/abs/1904.03742 [53] J. Alonso-Mora, T. Naegeli, R. Siegwart, and P. Beardsley, “Collision avoidance for aerial vehicles in multi-agent scenarios,” Autonomous Robots, vol. 39, no. 1, pp. 101–121, jun 2015. [Online]. Available: https://doi.org/10.1007/s10514-015-9429-0 Bibliography 49 [54] J. Li, M. Ran, and L. Xie, “Efficient Trajectory Planning for Multiple Non-Holonomic Mobile Robots via Prioritized Trajectory Optimization,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 405–412, 2021. [55] C. Ho, A. Jong, H. Freeman, R. Rao, R. Bonatti, and S. Scherer, “3D Human Reconstruction in the Wild with Collaborative Aerial Cameras,” 2021. [56] A. Mondal, C. Bhowmick, L. Behera, and M. Jamshidi, “Trajectory Tracking by Multiple Agents in Formation With Collision Avoidance and Connectivity Assurance,” IEEE Systems Journal, 2017. [Online]. Available: https://www.scopus.com/inward/record.uri?eid=2-s2. 0-85040082928{&}doi=10.1109{%}2FJSYST.2017.2778063{&}partnerID=40{&}md5= e40bf3aca4c1659f494cb6e394b3be32 [57] J. Alonso-Mora, S. Baker, and D. Rus, “Multi-robot navigation in formation via sequential convex programming,” IEEE International Conference on Intelligent Robots and Systems, vol. 2015-December, pp. 4634–4641, 2015. [58] J. Alonso-Mora, P. Beardsley, and R. Siegwart, “Cooperative Collision Avoidance for Nonholonomic Robots,” IEEE Transactions on Robotics, vol. 34, no. 2, pp. 404–420, 2018. [59] J. Park and H. J. Kim, “Online Trajectory Planning for Multiple Quadrotors in Dynamic Environments Using Relative Safe Flight Corridor,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 659–666, 2021. [60] M. Kamel, J. Alonso-Mora, R. Siegwart, and J. Nieto, “Robust collision avoidance for multiple micro aerial vehicles using nonlinear model predictive control,” in IEEE International Conference on Intelligent Robots and Systems, vol. 2017-Septe, 2017, pp. 236–243. [Online]. Available: https://www.scopus.com/inward/record.uri?eid=2-s2. 0-85041954247{&}doi=10.1109{%}2FIROS.2017.8202163{&}partnerID=40{&}md5= 5ad0219879fbdbd1dbb39c702613232b [61] C. E. Luis and A. P. Schoellig, “Trajectory Generation for Multiagent Point-To-Point Transitions via Distributed Model Predictive Control,” IEEE Robotics and Automation Letters, vol. 4, no. 2, pp. 375–382, 2019. [62] J. Alonso-Mora, A. Breitenmoser, M. Rufli, P. Beardsley, and R. Siegwart, Optimal Reciprocal Collision Avoidance for Multiple Non-Holonomic Robots. Berlin, Heidelberg: Springer Berlin Heidelberg, 2013, pp. 203–216. [Online]. Available: https://doi.org/10.1007/978-3- 642-32723-0_15 [63] B. Gopalakrishnan, A. K. Singh, M. Kaushik, K. M. Krishna, and D. Manocha, “PRVO: Probabilistic Reciprocal Velocity Obstacle for multi robot navigation under uncertainty,” in IEEE International Conference on Intelligent Robots and Systems, vol. 2017-September, 2017, pp. 1089–1096. [Online]. Available: https://www.scopus.com/inward/record.uri?eid=2-s2. 0-85041949927{&}doi=10.1109{%}2FIROS.2017.8202279{&}partnerID=40{&}md5= 20e7bcc5599c8ce06191ee67796b551e [64] M. P. Das, Z. Dong, and S. Scherer, “Joint point cloud and image based localization for efficient inspection in mixed reality,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 6357–6363. [65] J. Huuskonen and T. Oksanen, “Soil sampling with drones and augmented reality in precision agriculture,” Computers and Electronics in Agriculture, vol. 154, pp. 25–35, 2018. [Online]. Available: https://www.sciencedirect.com/science/article/pii/S0168169918301650 50 Bibliography [66] S. Ruano, C. Cuevas, G. Gallego, and N. García, “Augmented reality tool for the situational awareness improvement of uav operators,” Sensors, vol. 17, no. 2, p. 297, 2017. [67] B. Huang, D. Bayazit, D. Ullman, N. Gopalan, and S. Tellex, “Flight, camera, action! using natural language and mixed reality to control a drone,” in 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019, pp. 6949–6956. [68] C. Liu and S. Shen, “An augmented reality interaction interface for autonomous drone,” arXiv preprint arXiv:2008.02234, 2020. [69] K. Chandan, V. Kudalkar, X. Li, and S. Zhang, “ARROCH : Augmented Reality for Robots Collaborating with a Human,” no. Icra, pp. 3787–3793, 2021. [70] K. Shoemake, “Arcball: A user interface for specifying three-dimensional orientation using a mouse,” in Graphics interface, vol. 92, 1992, pp. 151–156. [71] Q. Galvane, C. Lino, M. Christie, J. Fleureau, F. Servant, F. o.-l. Tariolle, and P. Guillotel, “Directing cinematographic drones,” ACM Transactions on Graphics (TOG), vol. 37, no. 3, pp. 1–18, 2018. [72] C. Lino and M. Christie, “Intuitive and efficient camera control with the toric space,” ACM Transactions on Graphics (TOG), vol. 34, no. 4, pp. 1–12, 2015. [73] F. Real, A. Torres-González, P. R. Soria, J. Capitán, and A. Ollero, “Unmanned aerial vehicle abstraction layer: An abstraction layer to operate unmanned aerial vehicles,” International Journal of Advanced Robotic Systems, vol. 17, no. 4, pp. 1–13, 2020. Proyecto Fin de Carrera Ingeniería de Telecomunicación Formato de Publicación de la Escuela Técnica Superior de Ingeniería Autor: F. Javier Payán Somet Tutor: Juan José Murillo Fuentes Dep. Teoría de la Señal y Comunicaciones Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2013 Trabajo Fin de Máster Máster en Ingeniería Electrónica, Robótica y Automática Sistema multi-UAV para apoyo humano en operaciones de inspección con múltiples vistas Autor: Damián Jesús Pérez Morales Tutor: Jesús Capitán Fernández, Alfonso Ángel Alcántara Marín Dpto. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2021 Trabajo Fin de Máster Máster en Ingeniería Electrónica, Robótica y Automática Sistema multi-UAV para apoyo humano en operaciones de inspección con múltiples vistas Autor: Damián Jesús Pérez Morales Tutor: Jesús Capitán Fernández, Alfonso Ángel Alcántara Marín Profesores Titulares Dpto. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2021 Trabajo Fin de Máster: Sistema multi-UAV para apoyo humano en operaciones de inspección con múltiples vistas Autor: Damián Jesús Pérez Morales Tutor: Jesús Capitán Fernández, Alfonso Ángel Alcántara Marín El tribunal nombrado para juzgar el trabajo arriba indicado, compuesto por los siguientes profesores: Presidente: Vocal/es: Secretario: acuerdan otorgarle la calificación de: El Secretario del Tribunal Fecha: Resumen Se presenta un generador de trayectorias para formaciones multi-UAV que monitorizan la misión de la inspección de una turbina eólica. El objetivo principal es ofrecer diferentes puntos de vista de la turbina de viento a un operario de tierra, ofreciendo mejores perspectivas de lo que se está supervisando. Para hacer esto posible, se emplea una formación que sigue un esquema líder-seguidor de forma descentralizada. Cada UAV calcula una trayectoria inicial a partir de los comandos del operario que mantienen las propiedades visuales. Seguidamente, estas trayectorias son optimizadas, minimizando aceleraciones para obtener caminos suaves y, de esta manera, evitar movimientos bruscos. Este método es evaluado con simulaciones. I 4Capítulo 2. Metodología Figura 2.1 Esquema líder-seguidor en 2D. Algorithm 1 UAV líder 1: procedure Leader(waypoints, punto a inspeccionar, distancia de inspección) 2: Inicialización del sistema 3: while Existan waypoints && Misión empezada do .Por cada ciclo 4: Trayectoria de referencia ← Punto a alcanzar, punto a inspeccionar, distancia de inspección 5: Trayectoria óptima ←Trayectoria de referencia 6: Salida: Trayectoria del líder Algorithm 2 UAV seguidor 1: procedure Seguidor(Trayectoria de referencia del líder, ángulo de formación) 2: Inicialización del sistema 3: while Exista trayectoria de referencia del líder do .Por cada ciclo 4: Entrada: Trayectoria de referencia del líder, ángulo de formación 5: Trayectoria de referencia del seguidor ← Rotar ± ángulo de formación la trayectoria de referencia del líder 6: Trayectoria óptima ←Trayectoria de referencia del seguidor 7: Salida: Trayectoria del seguidor 2.2 Formulación del problema 2.2.1 Modelo dinámico Debido a que se dispone de un seguidor de trayectorias, se formula el modelo dinámico del UAV empleando únicamente su modelo cinemático (posición, velocidad y aceleración). Para el modelado de la orientación de la cámara, se asume que existe un mecanismo para el gimbal en el que se 2.2 Formulación del problema 5 Figura 2.2 Esquema de software del sistema. desprecia las variaciones en roll de la cámara y que solo se controla el cabeceo y la guiñada (pitch y yaw). La parte posicional del UAV del modelo dinámico se define como un doble integrador: ˙ p=v, ˙v=,(2.1) donde p= [ppypz]T∈R3 is es la posición del UAV, v= [yz]T∈R3 la velocidad lineal y = [yz]T∈R3 la aceleración lineal. La posición de la cámara se asume que es la misma que la del UAV. La orientación de la cámara se define de la misma manera: ˙ o=ω ω ω, ˙ ω ω ω=θ θ θ, (2.2) donde o= [φ ξ]T representa la orientación del gimbal respecto al sistema de referencias global por sus ángulos de pitch y yaw, ω ω ω∈R2 son las correspondientes velocidades angulares, y θ θ θ∈R2 las aceleraciones angulares. Para la descripción del método, se define un estado posicional del UAV xp= [pTvT]T∈R6 , un vector de entradas de control posicional p= , un estado de orientación xo= [oTω ω ωT]T∈R4, y un vector de entradas de control o=θ θ θ. 2.2.2 Generación de trayectorias de referencia La referencia del líder se genera a partir del punto a inspeccionar (waypoint), el punto a inspeccionar y la distancia a inspeccionar. Como resultado, se ofrece una trayectoria que encaja con un cilindro de radio igual a la distancia de inspección pasando por los waypoints comandados por el operario y manteniendo las restricciones visuales de los UAV para las imágenes. Para este propósito, es necesario realizar la parametrización en coordenadas polares y se aplica interpolación. Dado un waypoint 0= [r0θ0z0] y 1= [r1θ1z1] y un parámetro t∈[0,1] . 0 representa la posición actual y 1el siguiente punto a alcanzar, ambas en coordenadas cilíndricas. r=r0+ (r1−r0)∗t θ=θ0+ (θtot)∗t z=z0+ (z1−z0)∗t (2.3) 6Capítulo 2. Metodología θtot =          θ2−θ1−2π, if θ2−θ1> π θ2−θ1,if θ2−θ1< π y θ2−θ1<0 θ2−θ1+2π, if θ2−θ1<−π θ2−θ1,if θ2−θ1< π y θ2−θ1>0 (2.4) Se sabe que si t=1, la longitud de la curva L es: L=q(R2(θtot)2+ (z1−z0)2)(2.5) Para interpolar N puntos separados por tstep segundos a una velocidad de crucero Vc , aplicando regla de tres, para cada punto k se tiene la correspondiente tk: tk=Vc∗tstep ∗k Æ(Rθtot)2+ (z1−z0)2(2.6) En el caso de los seguidores, la trayectoria de referencia se calcula con una rotación alrededor del eje Z de relative_angle rad a partir de la trayectoria de referencia del líder: PRF = (PRL −p)∗RZ(rete_nge)(2.7) Donde PRF es una lista de puntos donde se almacena la trayectoria de referencia del follower, PRL es una lista de puntos donde se almacena la trayectoria de referencia del líder, p el punto a inspeccionar y RZes: RZ(θ) =   cos(θ)−sn(θ)0 sn(θ)cos(θ)0 0 0 1  (2.8) 2.2.3 Optimización de la trayectoria El objetivo principal es generar trayectorias óptimas para conseguir trayectorias suaves. Para ello, se formula un problema de optimización minimizando las desviaciones respecto a la trayectoria de referencia y las aceleraciones para evitar movimientos bruscos. Además, se añade el modelo cinemático del UAV y las limitaciones dinámicas como restricciones del problema de optimización. Por simplicidad, se denota x:=xpy:=p: minimize x0,...,xN 0,...,N N X k=1 (||xd,k −xk||2+β||k−1||2),(2.9) subject to x0=x0(2.9.a) xk+1=ƒ(xk,k)k=0,...,N −1(2.9.b) mn ≤k≤m (2.9.c) vmn ≤vQ,k ≤vm (2.9.d) donde las variables de optimización xk = [ pkvk ] y uk = ak son los estados discretos (posición y velocidad) y las entradas de control (aceleración) del sistema en cada tk . xd,k es una lista de los puntos de referencia deseados a alcanzar para cada UAV que viene de la Sección 2.2.2 y su tamaño depende de la longitud del horizonte deslizante. En el caso de que haya una zona de no vuelo predefinida (por ejemplo, alrededor de una turbina eólica), una nueva restricción puede ser añadida al problema de optimización. Sin embargo, esta 2.2 Formulación del problema 7 restricción es no convexa, lo que complica bastante el problema de optimización. Afortunadamente, se dispone de un módulo externo que genera las trayectorias de referencia (véase en la Sección 2.2.2) y esta restricción puede ser considerada implícitamente en la generación de la trayectoria de referencia. De esta manera, el optimizador no tiene por qué considerar una restricción no convexa, haciendo el problema más rápido y robusto para encontrar soluciones. En 2.9.a, x’ es el valor del estado observado del UAV actual, que es considerado el estado inicial para la formulación x0 . Las dinámicas del sistema están expresadas en 2.9.b, mientras que en 2.9.c y en 2.9.d se expresan las limitaciones de velocidad y aceleración del UAV respectivamente. En esta aplicación, las cámaras necesitan estar siempre apuntando al objetivo a filmar. Por ello, la orientación se obtiene: od=φdξdT=arctan(qy,q)sinqz ||q|| T.(2.10) donde q= [qqyqz] = p−p , son las posiciones relativas entre la posición de la cámara (asumiendo que es la misma que la del UAV) y la del punto a inspeccionar. Con esta información, la orientación deseada de yaw (φd) y de pitch (ξd) se puede inferir. El control de orientación es también definido como una problema de optimización cuadrática con restricciones con horizonte deslizante para conseguir cambios de orientación suaves: minimize 0,...,N−1 N X k=1 (||od,k −ok||2+γ||k−1||2),(2.11) subject to x0=x0,(2.11.a) xk+1=fo(xk,k)∀k∈{0,...,N −1},(2.11.b) ω ω ωmn ≤ω ω ωk≤ω ω ωm ∀k∈{1,...,N},(2.11.c) ξmn ≤ξk≤ξm ∀k∈{1,...,N},(2.11.d) mn ≤k≤m ∀k∈{0,...,N −1},(2.11.e) donde fo(·) representa la orientación del modelo dinámico definido en la Sección 2.2.1; ωmn , ωm son las limitaciones de las velocidades angulares; mn , m son las limitaciones de las entradas de control; y ξmn , ξm representan las limitaciones de hardware del gimbal para ajustar los ángulos de pitch. Los ángulos de yaw y de pitch de la cámara pueden ser controlados de forma independiente. De esta manera, el problema (2.11) se ha desacoplado en dos problemas más simples. El módulo Trajectory Follower tiene una tasa de 100 Hz, la cuale s responsable de mandar los comandos de velocidad al autopiloto (mirar la Figura 2.2) y el módulo de Trajectory Planning se ejecuta a 1 Hz, teniendo un camino planificado durante 4 segundos gracias al horizonte deslizante implementado (40 steps de horizonte deslizante, N , siendo cada step de 0.1 segundos). La implementación del horizonte deslizante permite al UAV tener un camino que seguir durante los próximos 4 segundos y permite tener movimientos más suaves, ya que el camino a seguir se vuelve a recalcular cada segundo. Para conseguir las trayectorias óptimas, se emplea el mismo problema para el líder que para el seguidor. 3 Resultados 3.1 Entorno Para realizar las simulaciones se emplea el simulador Gazebo, cuyo mundo es la cancha de baloncesto en "La Plaza del Agua", (ETSI, Sevilla). Se empleará este mundo para las simulaciones debido a que los experimentos en la vida real se van a realizar en ese lugar en un futuro. La interfaz de RViz proporciona información acerca de cómo está yendo la misión. Arroja información acerca de las trayectorias de referencia y reales de cada UAVs, las orientaciones de los UAVs, los puntos a inspeccionar, entre otras cosas. La apariencia del entorno de RViz se muestra en la Figura 3.2. 3.2 Interfaz del operario La interfaz del operario tiene la posibilidad de ejecutar un archivo de autoconfiguración donde se establecen ciertos parámetros por defecto, aunque se pueden modificar posteriormente a través de la consola que se le proporciona a través de comandos de alto nivel. El aspecto de la interfaz de la Figura 3.1 Escenario de simulación en Gazebo. 9 10 Capítulo 3. Resultados Figura 3.2 RViz en simulación. Figura 3.3 Operator’s interface, selection of the menu. que dispone el operario se representa en la Figura 3.3, a través de la cual puede interaccionar con la formación de UAVs. 3.3 Simulación En estos experimentos simulados, se inspecciona una hipotética turbina eólica en el escenario planteado en la Figura 3.1. El operario de tierra cambiará θr y r con un simulador de joystick para obtener diferentes puntos de vista, acercándose y alejándose de la estructura. El operario de tierra tiene el control de la formación y tiene unos parámetros de simulación preestablecidos, que se muestran en la Tabla 3.1 y los waypoints comandados se muestran en la Tabla 3.2. Es importante mencionar que dichos waypoints se adaptan automáticamente al cilindro. En la Figura 3.4 se representa la evolución de las trayectorias por cada UAV. Se puede observar que hay un cambio en la distancia de inspección observando los instantes de tiempo t = 30s y t = 40s. En la Figura 3.5, se representa la evolución de la distancia de inspección para cada UAV. Se puede observar que hay un cambio de referencia en t = 32s de 6.5m a 7.35m y empieza a seguir a 3.3 Simulación 11 Tabla 3.1 Parámetros de la simulación. Parameter Value Unit Horizon length 40 steps Step size 0.1 seconds Planning rate 1 seconds Cruising speed 0.7 m/s Max acceleration 1 m/s2 Inspection distance 6.5 m Relative angle 1 radian Inspection point [0, 0, 3.5] m Tabla 3.2 Waypoints deseados en la simulación. Waypoint Position [x, y, z] (m) Waypoint 1 [0, 2, 5] Waypoint 2 [2, 0, 5] Waypoint 3 [0, -2, 5] Waypoint 4 [-2, 0, 5] Waypoint 1 [0, 2, 5] Waypoint 2 [2, 0, 5] Waypoint 3 [0, -2, 5] Waypoint 4 [-2, 0, 5] Figura 3.4 Simulación. Mosaico de las trayectorias descritas por los UAVs. Notar que el Drone 1 (líder) se encuentra entre dos UAV, que son los seguidores (Drone 2, Drone 3) y están describiendo un camino siempre en dirección horaria alrededor de la turbina eólica (estrella negra). 12 Capítulo 3. Resultados la referencia en t = 36s. Más tarde, en t = 110s, hay otro cambio de referencia de 7.35m a 6.7m y empieza a seguir la trayectoria en t = 115s. El comportamiento de "overshooting" viene dado por estar generando trayectorias de referencia que son algo más largas de lo que deberían, sobrepasando ligeramente el waypoint. Sin embargo, tampoco excede de manera notoria. Figura 3.5 Simulación. Seguimiento del cambio de referencia de la distancia de inspección. En la Figura 3.6 se muestra la evolución de los ángulos de formación. Fijándose en el problema de seguimiento del ángulo de formación, se puede observar que se siguen correctamente los cambios de referencia, pero hay un offset inevitable de algunos grados ( ∼ 5-8 º ). Esto se debe a una mala sincronización de las trayectorias del líder y del seguidor, apareciendo un delay al mandar la trayectoria del líder a los seguidores ejecutándose las de estos últimos algo más tarde de lo que deberían. Para solucionarlo, es necesario descartar los puntos de la trayectoria del líder cuyo Timestamp sea más antiguo que el instante de ejecución de la misión. Este cambio está pendiente de implementarse. La aceleración lineal de los UAVs se muestran en la Figura 3.7. Como es de esperar, estas Figura 3.6 Simulación. Seguimiento del ángulo relativo de los UAVs. 3.3 Simulación 13 Figura 3.7 Simulación. Aceleración lineal de los UAV con trayectoria optimizada. aceleraciones no pueden ser 0, ya que los UAV están describiendo una circunferencia, lo que significa que deben tener algo de aceleración para que no sigan una línea recta. Cuando hay un cambio en el ángulo de la formación, las aceleraciones de los UAV seguidores pueden tener algunos picos para acercarse o alejarse del UAV líder. Incluso sin tener variaciones en los ángulos de la formación o en la distancia de inspección, las aceleraciones de los seguidores puede ser algo más irregular que las del líder, debido a que las trayectorias de los seguidores dependen directamente de la trayectoria del líder y deben adaptarse para mantener siempre el ángulo de formación con el líder. Sin embargo, gracias al solver, estos picos son minimizados gracias al problema de optimización que minimiza las aceleraciones, haciendo que estos picos sean poco notorios. Para ver la diferencia entre tener un camino óptimo de un camino no óptimo, en la Figura 3.8 se muestran las aceleraciones lineales de los UAV de otro experimento donde las trayectorias no han sido optimizadas y tienen cambios de referencia similares al experimento que se está exponiendo. En estos resultados, se puede apreciar que la aceleración lineal para el UAV líder no varía mucho respecto al experimento con MPC. Sin embargo, para los UAV seguidores sí que se puede apreciar bastante, teniendo picos bastante significativos cuando hay un cambio de referencia tanto en la distancia de inspección como en el ángulo relativo, incluso cuando no lo hay porque la trayectoria de los seguidores dependen de la trayectoria del líder y esto hace que esté constantemente acelerando o frenando. Si se hace el contraste con la Figura 3.7 que tiene el camino óptimo, se puede apreciar la mejoría que hay al tener un MPC. Se añaden además unas tablas donde se recopilan los valores medios de las aceleraciones medias lineales, de pitch y de yaw para cuando se intenta seguir una trayectoria optimizada (mirar Tabla 3.3) y para una trayectoria no optimizada (mirar Tabla 3.4). Puede observarse una mejoría significativa para el caso de los seguidores tanto por la aceleración lineal como por el ángulo de pitch (también mejora un poco el yaw) cuando se emplea la trayectoria óptima. A pesar de no estar minimizando las aceleraciones de pitch y de yaw, se puede observar que, con el problema de optimización planteado, estas se aminorar significativamente de manera indirecta, teniendo así un comportamiento más estable por parte de los seguidores. Un vídeo de demostración en el entorno de simulación se ha subido a la plataforma de YouTube 1 a modo de hacer promoción del proyecto. Es necesario aclarar que el mundo en el que se ha realizado 1https://www.youtube.com/watch?v=pXQHwkDVizw