A path planning algorithm for a marsupial robotic system (Algoritmo de planificación de trayectorias para un sistema robótico marsupial)

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 Proyecto Fin de Carrera Grado en Ingeniería de Tecnologías Industriales A path planning algorithm for a marsupial robotic system (Algoritmo de planificación de trayectorias para un sistema robótico marsupial) Autor: Adam Rodríguez González Tutor: José Miguel Díaz Báñez Dpto. de matemática aplicada II Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2023 Proyecto Fin de Carrera Grado en Ingeniería de Tecnologías Industriales A path planning algorithm for a marsupial robotic system (Algoritmo de planificación de trayectorias para un sistema robótico marsupial) Autor: Adam Rodríguez González Tutor: José Miguel Díaz Báñez Catedrático de universidad Dpto. de matemática aplicada II Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2023 Proyecto Fin de Carrera: A path planning algorithm for a marsupial robotic system (Algoritmo de planificación de trayectorias para un sistema robótico marsupial) Autor: Adam Rodríguez González Tutor: José Miguel Díaz Báñez 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: Agradecimientos Am is padres por ayudarme en todo lo que han podido. A mi familia, por proporcionarme bastantes momentos de alegría y diversión. En especial a mi hermano, que siempre está conmigo y compartimos la vida. A mi abuela, que ha hecho todo lo que ha estado en su mano por estar conmigo día a día y ha escuchado todos los problemas a los que me he enfrentado durante estos años de universidad sin entender ni la mitad. A mi perra a la que parece que le falta medio cerebro y sus pocas neuronas no son capaces de entender más de dos palabras, y a mi gato Mino, que no para de pisarme el teclado mientras escri/O?Eo7w47ovr3nt. I Resumen El uso de UAVs y AGVs durante los últimos años ha permitido disminuir el número de tareas potencialmente peligrosas que debían ser realizadas por humanos. Las tareas de inspección que deben realizar son extensas en el tiempo, por lo que se recurre al uso de drones marsupial. En este trabajo se desarrolla un algoritmo capaz de encontrar la posición en la que debe colocarse la parte terrestre del dron marsupial para poder desplegar a la aérea sin producir colisiones con el entorno. Para lograr este objetivo, el entorno se divide en planos bidimensionales en los que se estudia la viabilidad de la posición, evitando colisiones en el vehículo terrestre, el aéreo y el cable de alimentación que permite aumentar la autonomía. Conocidos el punto inicial y el punto objetivo, así como el entorno, el algoritmo calcula una solución válida en los planos que permitan encontrar la solución de mínimo desplazamiento en el menor tiempo posible. Una vez lograda esta función, se introducen cambios en el comportamiento del algoritmo, los parámetros del sistema y la definición del entorno para estudiar los efectos que produce en la eficiencia del algoritmo y cómo estos pueden ser subsanados en el caso de que no sean beneficiosos. III XContents 3.5 Initial length value 32 4 Extensions 35 4.1 Exploring around the target point 35 4.2 Floating obstacles 38 4.3 Search for the optimal solution taking into account the obstacles 40 5 Experiments 43 5.1 First scenario: Simple scenario 43 5.2 Second scenario: Tunnel 44 5.3 Third scenario: Zig-Zag Tunnel 45 5.4 Fourth scenario: Walls 47 5.5 Adding obstacles 49 6 Conclusiones y trabajo futuro 51 7 Conclusions and future work 53 8 Appendix 57 8.1 Main and auxiliary algorithms 57 8.2 Visualisation 61 8.3 Auxiliary algorithm with movement at the target point. Version 1 61 8.4 Auxiliary algorithm with movement at the target point. Version 2 62 8.5 Algorithms with floating obstacles 62 8.6 Auxiliary algorithm with floating obstacles. Version 2 64 8.7 Creation of planes 65 8.8 Edge detection algorithm 65 8.9 Edge detection algorithm. Version 2 66 List of Figures 1.1 Worker and drone in building inspection 1 1.2 Marsupial drone 2 1.3 Aerial vehicle characteristics 2 2.1 Planes and obstacle 1 6 2.2 Planes and obstacle 2 6 2.3 Class obs 7 2.4 Function creaobs 7 2.5 Creation of ground obstacles 8 2.6 Obstacles example 8 2.7 Catenaries of different lengths 9 2.8 Function that implements the catenary 9 3.1 Example of a two dimensional instance 11 3.2 Minimum distance by length 12 3.3 Minimum distance by aerial obstacle 13 3.4 Implementation of the initial point 14 3.5 Lmax and length loop 14 3.6 Catenary’s lengths 15 3.7 Function choque 15 3.8 Representation of obstacles and wire 17 3.9 Auxiliary algorithm flowchart 18 3.10 Example of planes 19 3.11 Plane creation 19 3.12 Solutions in 90 random planes 20 3.13 360 planes solutions 20 3.14 360 planes solutions with a shorter wire’s length 21 3.15 Solutions area 21 3.16 Reduction of the solutions area 22 3.17 New range of planes 22 3.18 d(A,Ci)22 3.19 Function nonlinear 22 3.20 Calculation of P1 23 3.21 Calculation of P2 23 3.22 List of planes 23 3.23 Point to degree conversion 23 3.24 Main algorithm flowchart 24 3.25 Plane solution 1 25 3.26 Local solution 1 26 3.27 Plane solution 2 26 3.28 Average time vs obstacles 27 XI XII List of Figures 3.29 Average time vs position iterations 28 3.30 Solutions vs position iterations 28 3.31 Solutions vs modifications 30 3.32 Solutions vs catenary 31 3.33 Time vs catenary 31 3.34 Cost function 32 3.35 Solutions 32 3.36 Least square line 33 3.37 Initial point in algorithm 33 3.38 Time and methods 34 4.1 Function vertical position 35 4.2 Wire’s length and vertical position 1 36 4.3 Wire’s length and vertical position 2 36 4.4 Complete position function 1 37 4.5 Wire’s length and positions 1 37 4.6 Complete position function 2 38 4.7 Wire’s length and positions 2 38 4.8 Floating obstacle 39 4.9 Two floating obstacles 39 4.10 Two floating obstacles and wire 40 4.11 New Xmin 40 4.12 Obstacle algorithm 1 41 4.13 Obstacle algorithm 2 41 4.14 Obstacle algorithm. Version 2 42 4.15 Point selection. Version 2 42 5.1 Scenario 1 43 5.2 Scenario 1 solution 44 5.3 Scenario 2 44 5.4 Scenario 2 solution 45 5.5 Scenario 2 plane solutions 45 5.6 Scenario 3 45 5.7 Scenario 3 solution 46 5.8 Edge detection algorithm point 46 5.9 Edge detection algorithm solution 47 5.10 Edge detection algorithm point, version 2 47 5.11 Scenario 4 47 5.12 Scenario 4 plan view 48 5.13 Scenario 4 Edge detection algorithm 48 5.14 Scenario 4 Edge detection algorithm solution 2 49 5.15 Scenario 4 Edge detection algorithm solution 1 49 List of Tables 3.1 Wire’s length and collisions 16 3.2 Obstacles and time 26 3.3 Time, solutions and position iterations 1 27 3.4 Time, solutions and position iterations 2 28 3.5 Time, solutions and catenary iterations 1 29 3.6 Time, solutions and catenary iterations 2 29 3.7 Time, solutions and catenary’s length 30 3.8 Time, obstacles and methods 1 33 3.9 Time, obstacles and methods 2 34 4.1 Time, obstacles and methods 2 40 5.1 Average times and planes. Second column without obstacles. Third column with land obstacles. Fourth column with aerial obstacles. Fifth column with both obstacles 49 XIII 1 Introduction 1.1 Problem description and motivation Th e goal of this project is to develop an algorithm capable of enabling the arrival of a marsupial drone from an initial point to a target point on a known obstacle map. This technology allows reducing the danger to which a worker must be exposed and replacing it with autonomous or remotely controlled drones, which are able to obtain more features of the building in a shorter time and have greater manoeuvrability than the worker (see Figure 3.35). Drones simplify the work to be done, but their autonomy (tens of minutes) is not enough to do the job without having to use a new drone to replace the old one or wait for the battery to be recharged. The solution to the problem of autonomy, which greatly reduces the applicability of drones, is the use of marsupial drones. A marsupial drone is a technology that began to be developed in the 1990s [1] and consists of making use of a team of drones: a ground vehicle and an aerial vehicle (see Figure 1.2). The ground vehicle will be able to transport the aerial vehicle, allow the aerial vehicle to be deployed, and continuously power it through the use of a wire linking the two vehicles, which solves the problem of autonomy. Although using a marsupial drone solves the time of use of the aerial vehicle, it introduces a new problem in the inspection, the wire. The wire that connects the two vehicles is a source of collisions with obstacles in the terrain to be inspected and limits the range of the aerial vehicle with respect to the position of the ground Figure 1.1 Worker and drone in building inspection. 1 2Chapter 1. Introduction Figure 1.2 Marsupial drone. Figure 1.3 Aerial vehicle characteristics. vehicle. In this study, it will be assumed that it is possible to change the length of this wire to a proportionate maximum length. Depending on the type of aerial vehicle, a number of features are available that make it stand out from other types of configurations (see Figure 3.38). The most useful type of aerial drone in this case is the helicopter type, but it allows less maneuverability than the multirotor. If a marsupial drone is chosen, the time of use can be increased and the distance of travel will be limited by the power wire. It makes more sense to use a multirotor, which are easier to maneuver and cheaper to build. Once a configuration has been chosen which allows inspection, an algorithm has to be developed to calculate the position of the ground vehicle which enables the aerial vehicle to be deployed to the target point while avoiding collisions between the aerial vehicle and the wire with obstacles in the environment. In other works, the ground vehicle remains static [16] and the focus is on the calculation of the time position of the tether and the aerial vehicle. Configurations consisting of an unmanned ground vehicle (UGV) carrying the unmanned aerial vehicle (UAV) are becoming more noticeable in recent years. Some of the most noteworthy examples using this type of technology are Team CSIRO Data61 [11], the DARPA 2021 Subterranean Challenge [12] and, as an 1.2 Related work 3 honourable mention, Mars 2020 rover and helicopter [13]. 1.2 Related work It should be noted that not all studies of marsupial robotic systems are based on the use of a hybrid UGV-UAV system, on which this study will be based. There are other typologies worthy of study, such as marsupial systems of unmanned underwater vehicles (UUVs), linked by a cable whose primary function is to serve both as a means of communication between vehicles and as a means of data transmission [14]. In other approaches, as [17], the marsupial system is formed by the aerial vehicle and a human, connected by a cable. Another interesting use of this type of system is the use of the mooring as an aid in the exploration of difficult terrain or in maneuvers. In [15], the tether is attached from an UGV to an object in the environment, allowing for greater exploration capability. In [18], the authors propose the using of a rigid tether connected to an aerial vehicle to increase the stability in landing. The use of the tether can provide stability not only in landing, but in flights, as in [19]. In this study, the tether improves the stability of a UAV in confined spaces. In [22], a marsupial system formed by an UAV, an UGV and a tight tether, navigates through confined spaces using the UAV as a visual assistant. This study does not take into account the trajectory that the aerial vehicle must follow once deployed, but rather considers that by following the path formed by the wire between the two vehicles and not colliding with any obstacle, the vehicle will be able to reach the target point. In [20], the aerial vehicle follows the same type of trajectory, but in this case, the cable is considered as a taut tether. Because of that, the UAV can only reach targets with a direct line of sight from the point it takes off. In order to solve this problem, in [21], the tether is modeled as a catenary, which allows to find solutions away from the direct line of sight, but the starting point of the tether still static. Most of the studies developed are based on the deployment of the aerial vehicle and use Djikstra, A* and Theta* algorithms to avoid collisions [2], dividing the three-dimensional environment in a meshed way. There are also algorithms capable of calculating the areas accessible by each vehicle, as in [3], but they are not able to consider the case where both are joined, so it may help to find some initial position points, but it does not ensure an optimal solution search. Studies of aerial robots that make use of wires are mostly focused on the control of these robots [4] and consider that the wire cannot cause collisions with the environment, which is not in line with the research conducted in this study, which is based on the influence of the wire and puts the vehicles to which it is anchored in the background. In [23], the approach considers the same marsupial system as this study, but the cable is a tight tether, only providing ray-tracing solutions, but with a dynamic system. The most common trajectory algorithms calculate the path to be followed by the ground vehicle and constrain it by using the position in which the aerial must be placed and the length of the wire [5], but still do not consider collisions that occur between the wire and the environment. As can be seen, although there are studies that make it possible to determine the position of the catenary that forms the wire between the two vehicles [6], this is not an aspect that is usually considered when carrying out the collision study and which can make many of the solutions found inaccessible. An approach that consider the same marsupial system, a tether modeled by a catenary and considers the collisions with the environment is [24], being the most similar to this study. In this case, the marsupial system moves in a coordinated way, unlike this study, where the UAV only moves when the UGV can provide a trajectory to the target. 1.3 Scope of this work The collisions that exist between the catenary and the environment are very little developed in the academic work that can be found. This work seeks to find a point, or several of them, from which it is possible to deploy the aerial vehicle in the shortest possible time. Once the various points have been found, the ground vehicle must proceed to one of them, and trajectory tracking can be used. When it has reached the deployment point, a real-time system can be incorporated into the air vehicle in case there may be unanticipated obstacles, but from the outset, the wire will not cause any collisions. The work is carried out in Python and by means of the available libraries, the aim is to achieve an algorithm capable of implementing the above. The work is developed as follows: 4Chapter 1. Introduction • Creation of the different elements necessary to build the environment and the catenary formed by the wire. The chapter 2 explains in detail the study to be carried out and the objects to be created. In addition, the way in which the obstacles are going to be found and how the map is different in each simulation will also be explained, in order to obtain unbiased data. • In chapter 3 it will be explained how the created algorithms work, where there is a main algorithm that will call an auxiliary one as many times as necessary. These algorithms are in charge of finding a solution that allows the deployment of the aerial vehicle avoiding collisions of the aerial vehicle and the wire with obstacles in the shortest possible time. •In order to obtain the best possible solutions and to observe how the different variables of the system affect the performance of the algorithms, in the chapter 3.2.3 the algorithms will have the fundamental parameters developed in the previous chapter modified, looking for the most efficient solution. • To make use of more complex functionalities and new obstacle definitions, extensions to the initial problem are developed in chapter 4 and the results are studied. • Once the most optimal possible algorithm has been obtained, different cases are studied in chapter 5, testing the correct operation in different environments. • In chapter 7, the development of the work is discussed, the objectives achieved, the usefulness of the study is checked and the future applications it can have are considered. 2 Description of the problem under study Th e problem under study is defined as follows: given an initial position of the marsupial system as a whole A= (xA,yA,zA) , a target position B= (xB,yB,zB) , where the air vehicle should be placed, the maximum length of the tether connecting the two vehicles L and a three-dimensional map made up of obstacles, find the nearest point of deployment C= (xC,yC,zA) to which the ground vehicle must travel in order to deploy the air vehicle, enabling it to reach the target point as well as the length of wire that allows such deployment while avoiding collisions with the obstacles. The movement of the marsupial system will not take place simultaneously, but there will be a first move, in which the ground vehicle travels from A to C, and a second move, in which the aerial vehicle flies from C to B. To perform this function, the point C to be found in each configuration must be calculated from the target point, and once found, movement will be allowed. Point C must satisfy the following conditions: 1. The position of C must be accessible to the marsupial system, that is, it cannot be positioned on an obstacle and there must be an accessible path between A and C. 2. The position of C must allow for a wire of less than the maximum specified length connecting points C and B without collision between surrounding obstacles and the wire. Before developing the algorithm, it is necessary to know how some parameters that belong both to the marsupial drone and to the three-dimensional space behave and how they will be treated. 2.1 Treatment of obstacles and workspace An obstacle is a set of three-dimensional points defined by three components: 1. Height. 2. Horizontal distance to the target point. 3. The angle it forms with respect to the union of the target point with the initial point. We propose a strategy to obtain an approximate solution the optimization problem defined above. The approach considers a range of planes passing through the target point and perpendicular to the ground. Thus, it will be assumed that the obstacle points are given within a plane, as in Figure 2.1. Each plane is defined by the angle it makes with respect to the plane perpendicular to the ground passing through the initial and target points. The three-dimensional points of the obstacle must be translated into two-dimensional positions within a plane. The workspace is divided into M planes with a centre at the target point, as shown in figure 2.1, and identified by a degree between 0ºand 360ºwith respect to the plane joining the target point and the initial point. To each plane corresponds a degree i, so that: 5 12 Chapter 3. Algorithms •Xmin is the first point to the right of Ax at which there is a (non necessarily collision free) catenary connecting that point and B. •lmax and lmin are the maximum and minimum catenary’s lengths. The detailed explanation of how each parameter is calculated will be described later. The steps to be followed by the algorithm are the following: •Step 1: Generate the range of horizontal positions [Xmin,Bx]by calculating the value of Xmin. •Step 2: Uniformly discretise the interval I= [Xmin,Bx]into hposition points. •Step 3: Move to point aof Iclosest to Axthat has not been evaluated. •Step 4: Calculate lmin(a), corresponding to the position of the point to be evaluated. •Step 5: Generate the range of catenary’s lengths [lmin,lmax]. •Step 6: Uniformly discretise the interval [lmin,lmax]into plength values. • Step 7: Test all catenary’s lengths to be evaluated and check for solutions, adjusting the value of lmax in the process. • Step 8: If a collision free solution is found, the value of the evaluated position point (point C ) is stored and the algorithm is finished. If no solution is found in the length’s range, go to step 3. 3.1.1 Detailed description of the algorithm Position points The first parameter to be calculated, Xmin , is the first location which allows a possible solution. The value of Xmin is determined by the maximum catenary’s length, the shorter the catenary’s length, the closer Xmin will be to Bx. Figure 3.2 Minimum distance by length. In Figure 3.2 if the target point is B and the maximum catenary’s length is L , the minimum distance where the aerial vehicle can be deployed is the intersection of the circumference of radius L and centre B with the straight line on the ground (D). This way, there can be zero, one or two solutions. 1. L<By. There is no solution for Xmin. It is to be assumed that this case will not take place in practice. 2. L=By. There is only one solution: Xmin =Bx, and the catenary must be an straight line. 3. L>By. There are two solutions, the point nearest to Ais chosen. L=q(Bx−Xmin)2+ (By)2 L2= (Bx−Xmin)2+ (By)2 Bx−Xmin =q(L2−(By)2) Xmin =Bx−q(L2−(By)2) 3.1 Auxiliary algorithm 13 4. L>> By. If the solution calculated before returns a negative value, Xmin =Ax. Additionally, note that if there exists a low aerial obstacle near to B , it can limit the number of valid solutions far from that obstacle point. In this case, we consider the length of two line segments of lengths L1 and L2 , where L1+L2=L . In this case, the length of the line connecting the points B and the lowest point of the obstacle ( P ) must be calculated, say L1 . Once obtained L1 , Xmin is determined by the circle centered at P of radius L2=L−L1. Figure 3.3 Minimum distance by aerial obstacle. L1+L2=L L1=q((By−Py)2+(Bx−Px)2) L2=L−q((By−Py)2+(Bx−Px)2) = q((Py)2+ (Px−Cx)2) (Py)2+(Px−Cx)2=L2−2Lq((By−Py)2+(Bx−Px)2)+(By−Py)2+(Bx−Px)2 X1=−2Px X2= (Py)2+(Px)2(−L)2+2Lq((By−Py)2+(Bx−Px)2)−(By−Py)2−(Bx−Px)2 (Cx)2+X1∗Cx+X2=0 Cx=−X1±p((X1)2−4∗X2)) 2 Xmin =1 2∗(−X1−q((X1)2−4∗X2)) To recapitulate, there are three solutions to choose as the initial search point: Ax , the point of maximum catenary’s length, which may return an invalid negative position, and the point calculated from an aerial obstacles, which may return no solution. The algorithm selects the maximum value of the three independent solutions. Xmin =MAX{Xmin(1),Xmin(2),Xmin(3)} where: Xmin(1) = Ax Xmin(2) = Bx−q(L2−(By)2) Xmin(3) = 1 2∗(−X1−q((X1)2−4∗X2)) 14 Chapter 3. Algorithms Figure 3.4 Implementation of the initial point. In the code of Figure 3.4, Zx and Zy act as Px and Py , the lowest point of the aerial obstacle in the plane. C1 and C2 are the constants of the quadratic equation. Xm1 is the solution of the third method, Xm2 is the solution of the second method and Axis the solution of the first method. Catenary’s length This is one of the most important parameters of the proposed approach. As with the position, there are restrictions, but they are much simpler. 1. The length must be in the interval [lmin,lmax]. 2. The length can not exceed L. The values of lmin and lmax are updated at each position point. The value of lmin is defined as: lmin =q((Bx−Xix)2+ (By−Xiy)2) where Xi= (Xix,Xiy)is the evaluated point. lmax is calculated as follows. A first estimation is to choose the maximum length allowed by the user L , but it will only work at large distances from the target points ( L≈Bx ). Once the points are closer, the length values near to L will not be able to solve the collision problem (specifically with land and ground obstacles). Thus, we tune lmax at every step. For the first position point, the value of lmax is obtained using the following procedure: 1. The maximum permitted length Lis set as lmax. 2. The algorithm enters the decrementing length loop. 3. The first length that does not collide with the ground is stored as lmax for the next step. In this way, Note that lmax will decrease as the points get closer and cases impossible to solve will not be calculated. In addition, lmax can not exceed L, fulfilling the second restriction. Figure 3.5 Lmax and length loop. Once obtained the values of lmin and lmax , the next question is how to traverse the interval [lmin,lmax] . Since we do not know if the solution will be close to lmax or lmin , the values to be tested will "jump" from values close to lmax to values close to lmin starting at the middle point. When a catenary’s length avoids collision with obstacles, the solution Cx (distance from the origin to point Xi) will be saved (see Figure 3.5) and returned for future calculations with the Main Algorithm. 3.1 Auxiliary algorithm 15 Number of positions and lengths The number of position points and lengths will be experimentally studied in Section 3.2.3. At the this point, we take starting values. The initial value of number of position points will be the number of obstacle points of the largest obstacle between the straight lines x=Axand x=Bx. Set this number as N. Apart from that, the number of lengths will be variable depending on the position. The closer the search position is to x=Bx , the closer the values of lmax and lmin will be. In order to prevent similar searches, an initial value for the number of lengths is the following: 2+N∗(Bx−Xi) (Bx−Xmin), which decreases as the position point moves away from Xmin. Figure 3.6 Catenary’s lengths. Collision with obstacles The catenary has to avoid collision with obstacles. In the same two-dimensional plane, it is necessary to check that each point defining the aerial obstacle is above the catenary and those defining the terrestrial obstacles are below. Once a point that does not verify this rule is found, the catenary deployed is invalid. To implement this simple algorithm, the function choque is created (see Figure 3.7). This function acts as a module of the auxiliary algorithm. Figure 3.7 Function choque. 16 Chapter 3. Algorithms The function needs the following parameters that act as local variables: •Obs: Matrix which stores the coordinates of the obstacle points. •A: Ground vehicle’s position. •B: Target point. •L: Wire’s length. The algorithm will evaluate every obstacle point between the tie points of the catenary. The collision will be determined by subtracting the height of the catenary with respect the height of the obstacle point. Depending on the type of obstacle, the value of the subtraction will be: •Greater than zero for terrestrial obstacles. •Less than zero for aerial obstacles. If it exists at least one obstacle point which does not verify these conditions, the catenary will collide with the environment and it is not a valid solution for the auxiliary algorithm. In order to check the correct functioning of the algorithm, a test will be done following the next steps: 1. Create a random aerial obstacle, a random land obstacle and the ground. 2. Determinate if exists a collision between the wire and the obstacles individually. 3. Determinate if it is a valid solution for the auxiliary algorithm. Table 3.1 is created using the results of the test with some lengths between 5 and 10 metres. Furthermore, in order to verify these results, Figure 3.8 shows a visual representation of each try. Table 3.1 Wire’s length and collisions. Wire’s length Aerial collision Land collision Ground collision Solution 5 YES NO NO NO 6.25 YES NO NO NO 7.5 NO YES YES NO 8.75 NO YES YES NO 10 NO YES YES NO The code is able to interpret the information of the different types of existing obstacles and identify the collision with the catenary. With this information, it is possible to start searching the optimal position point of the ground vehicle. 3.1.2 Processing time Taux =O(h∗n∗p) where: h=Number of position points. n=Number of obstacle points. p=Number of catenary’s lengths. Both h and p are user-defined variables, while n is an immutable input, thus the auxiliary algorithm scales linearly with respect to the obstacle points. 3.1.3 Auxiliary algorithm flowchart In order to understand the functioning of the algorithm in a more visual way, the flowchart is illustated in Figure 5.2: 3.2 Main algorithm 17 Figure 3.8 Representation of obstacles and wire. 3.2 Main algorithm The problem is as follows. Given an initial position of the marsupial system as a whole, a target position, a maximum length of catenary and a three-dimensional workspace, find the nearest point of deployment to which the ground vehicle must travel in order to deploy the air vehicle, avoiding collisions with the obstacles. The three-dimensional space is discretised using two-dimensional planes as explained in Section 3.1 and then the auxiliary algorithm is applied to every plane. Let π(αj) , j=1,··· ,k be a bundle of planes passing through B and perpendicular to the ground and π∗(αj) be the set of points Cj on the ground in π(αj) so that there exists a free collision catenary ζjconnecting Cjand B. The overview of the main algorithm is as follows: •Input: A,B, planes π(αj)and the maximum length L. •Output: Ci∈π∗(αi)such that d(A,Ci) = minCj∈π∗(αj)d(A,Cj). 1. Find the solution C0 in π∗(α0) , where π(α0) is the plane passing through points A and B and perpendicular to the ground. 18 Chapter 3. Algorithms Figure 3.9 Auxiliary algorithm flowchart. 2. Calculate the distance d(A,C0). 3. From the calculated distance, determine the range of planes [π(αµ),π(αη)] in which it is convenient to search for a solution, where αµ≤0≤αη. 4. If a solution is found in π(αi)∈[π(αµ),π(αη)] such that d(A,Ci)<d(A,C0) , go to step 5, otherwise go to step 6. 3.2 Main algorithm 19 5. Recalculate [π(αµ),π(αη)] according to d(A,Ci) and return to step 4, comparing the new solutions with d(A,Ci). 6. The output is C0 if no solution of smaller displacement has been found or, otherwise, the last point Ci obtained in step 4 such that d(A,Cj)<d(A,Ci). 3.2.1 Detailed explanation of the steps Figure 3.10 Example of planes. As explained above, a plane is defined by the obstacles (aerial, land and ground obstacles) and a degree. The degree is determined with the current obstacle plane with respect to the plane that connects A and B in a counterclockwise direction (looking in a plan view) as illustrated in Figure 3.10 where the plane connecting points A and B, π(α0), and the obstacle plane to be studied, π(αi), are represented in a circle of radius: r=d(Ax,Bx) Creation of the planes Figure 3.11 Plane creation. Once the number of planes to be used is known, they are created as shown in Figure 3.11. The created planes are stored in an ordered vector from 0°to 360°with a total number of M planes. The degree of the plane depends on the number of planes, the lesser the number of planes, the more distance there is between them. If the auxiliary algorithm is applied to a different number of planes, the distance between planes can be seen (see Figure 3.12). If the solutions obtained are counted, 57 of the 90 existing planes are obtained, this is due to the fact that many of the randomly generated obstacles do not allow a solution. The blue square corresponds to the initial position of the drone and the origin of coordinates is the target position. If 360 planes are used, one for each degree, Figure 3.13 is obtained. In this case, 220 solutions have been found. In addition to changing the number of planes, another important aspect in the solutions is the length of the catenary. It is assumed that, if it is reduced, the solutions should be found at points closer to the target 20 Chapter 3. Algorithms Figure 3.12 Solutions in 90 random planes. Figure 3.13 360 planes solutions. position. To determinate if this theory is valid, a new simulation where the wire length is reduced is done, obtaining the results in Figure 3.14. Indeed, solutions are grouped more according around the target, as expected. Finding the optimal location C We now elaborate on finding to the plane the ground vehicle should be placed in order to arrive as quickly as possible. In order to perform this action, three conditions will be assumed: 1. The ground vehicle is slower than the aerial vehicle, so its movement determines the speed at which the drone reaches the target point. 2. The building is accessible on one side, i.e., only obstacle planes between -90°and 90°are to be considered. Otherwise, the algorithm can be applied using different intervals of 180°, exploring all the possibilities. 3. The ground vehicle can reach any point using a linear trajectory, i.e., there is no ground obstacle in the planes. Since the goal is the ground vehicle moves at little as possible, once the first solution for a plane has been determined, the system should not search at points further away from this solution. 3.2 Main algorithm 21 Figure 3.14 360 planes solutions with a shorter wire’s length. Figure 3.15 Solutions area. With A being the initial point and B the target point (see Figure 3.15), the solutions are in the circle of centre Band radius: S=q(L2−(B.y−A.y)2) If the algorithm finds a first solution in the plane π(α0) , C0 , the solutions that allow a displacement less than or equal to the one found are found inside the black circle in Figure 3.16. To find the planes in which a solution can be found, the intersection points of both circles must be determined and, from these, the plane to which they belong (see Figure 3.17). The algorithm creates an interleaved vector, like the one used for the catenary’s lengths, which makes use of all the planes between π(αµ) and π(αη) . The search will be done from the extremes of the plane interval to π(α0). The first plane to be evaluated is π(α0) . Once the plane has been evaluated, it is saved in a list of evaluated planes, and it is checked if the number of planes has been limited according to the distance obtained; in other case, the search interval would be [-90°,90°]. When the first plane has been evaluated, the search of a new valid solution begins. Once a solution to the plane position problem has been obtained, it is checked if it is valid, and if so, the distance d(A,Ci) is calculated (see Figure 3.18). If it is less than the distance calculated in the previous points, Ci is taken as the 28 Chapter 3. Algorithms Table 3.4 Time, solutions and position iterations 2. I Tmed Solutions 16 0.0388 224 13 0.0286 218 10 0.0281 210 7 0.0212 208 Figure 3.29 Average time vs position iterations. Figure 3.30 Solutions vs position iterations. 3.4.3 Variation with respect to the number of catenary iterations Once it has been verified that the number of divisions on the displacement axis is not significant, there is one more loop to inspect, the one searching for the catenary’s length. As indicated above, the number of iterations this loop performs is 2+N∗(B.x−A.x) (B.x−Xmin) 3.4 Study of the processing time 29 To increase the number of iterations, the independent term of the expression is going to be modified, being in this case 2, and it is going to be done in an odd way, as this number is introduced in two length vectors. Starting from the data of the previous example and with N equal to the independent term, the following table of results is obtained. Table 3.5 Time, solutions and catenary iterations 1. N Tmed Solutions 2 0.0398 221 4 0.0421 212 6 0.0461 226 8 0.0521 215 10 0.0502 225 12 0.0591 231 14 0.0487 215 16 0.0703 221 18 0.0612 223 20 0.0769 234 As before, the number of solutions found oscillates at a constant point, probably due to the randomness of the obstacles created, which only allows for that number of solutions. If, by increasing the number of iterations, it appears that the only factor that fluctuates is time, which must not increase, the behaviour of the algorithm if the number of iterations is reduced will be simulated. To perform this action, acting on the independent term does not have a great effect, as once it is less than two, the choice of the integer of the expression will not take it into account. Then the action will occur on N and the expression will be as follows: 2+N U∗(B.x−A.x) (B.x−Xmin), where Uis a factor that will decrease the number of searches in each loop. Table 3.6 Time, solutions and catenary iterations 2. U Tmed Solutions 1 0.0368 212 1.2 0.0280 210 1.4 0.0278 206 1.6 0.0291 190 1.8 0.0285 191 2 0.0216 197 2.2 0.0241 191 2.4 0.0201 179 2.6 0.0227 187 2.8 0.0197 178 3 0.0182 161 By plotting the data obtained in both tables (see Tables 3.5 and 3.6), the behaviour of the algorithm can be understood. Figure 3.31 is not a direct input-output relationship. Changes from 0 to 1 represent the value of 1/U and changes from 2 to 20 represent the value of the independent term. The number of solutions grows rapidly as the factor multiplying the average number of obstacles found in the plane segment to be examined increases, but once it becomes equal to the total number of obstacles ( U=1 ), the number of solutions found grows slowly, increasing the execution time considerably and, depending on the shape of the points, tending to a maximum average value. 30 Chapter 3. Algorithms Figure 3.31 Solutions vs modifications. Surprisingly, it appears that both the position loop and the catenary’s length choice loop were already in the optimal number of iterations for the solution search algorithm. 3.4.4 Variation with respect to maximum catenary’s length Another variable that can influence the performance of the solution search is the maximum length of the catenary, since the distance between points is kept constant for all tests, allowing to observe the changes in behaviour with the same initial conditions. The distance at which the initial and target points are located is 8 metres on the X-axis and 2.8 metres on the Y-axis, since the catenary has been given an initial height of 0.2 metres. The maximum length with which the simulations of the previous points have been made is 6 metres. Changing this, and simulating the same number of planes, the following results are obtained: Table 3.7 Time, solutions and catenary’s length. L Tmed Solutions 3 0.0013 156 3.5 0.0029 224 4 0.0056 205 4.5 0.0140 209 5 0.0220 211 5.5 0.0339 239 6 0.0370 209 6.5 0.0451 225 7 0.0551 225 7.5 0.0528 226 8 0.0610 256 8.5 0.0710 283 9 0.0340 291 9.5 0.0228 318 10 0.0220 331 Breaking with the previous points, the number of solutions found and the average calculation time are highly dependent on the maximum catenary’s length. To get a better idea of this variation, the data obtained are shown (see Figures 3.32 and 3.33). Knowing that the search point is not smaller than the origin point of the plane in any way, it is possible to make use of catenary’s lengths greater than the distance between points without worrying about obtaining invalid solutions. What can be observed in the results is that the number of solutions found by the algorithm 3.4 Study of the processing time 31 Figure 3.32 Solutions vs catenary. Figure 3.33 Time vs catenary. grows significantly when the maximum catenary’s length is equal to or greater than 90% of the length between the origin point and the target, and the processing time has a maximum when the length is equal to the distance. Creating a very simple cost function: J=cs +ct Where cs is the cost with respect to the solutions and ct is the average time cost. To obtain the solution of the cost function that minimises the processing and maximises the number of solutions obtained, cs will be defined as the percentage of solutions not found and ct as the percentage of time with respect to the maximum time. Smaller values are given for small variations to the right side or large variations to the left side (see Figure 3.34). If a catenary’s length capable of operating on the right side of the maximum of the cost function is available, it is possible to obtain many solutions in a small time interval. If, on the other hand, not so much catenary’s length is available, one option is to use 40% of the distance between points, which allows 60% of solutions to be found with much shorter times than those to the right of the maximum. 32 Chapter 3. Algorithms Figure 3.34 Cost function. 3.5 Initial length value As mentioned in Chapter 3.1.1 the loop of lengths is traversed from values close to lmax to values close to lmin starting at the middle point. This middle point will be name as lmed. The reason to use lmed is to avoid making searches near the extremes of the length interval, increasing the possibilities of finding a solution in the first searches. It is for this reason that lmed must be adjust to its optimal value. This value will be obtained empirically, making use of the solution found in the evaluated planes. Plotting the length that allows the solution of the auxiliary algorithm against the horizontal distance between the position point and the target, the Figure 3.35 is obtained. Figure 3.35 Solutions. The solutions for each distance seem to vary up to 0.5 metres from each other as maximum. This variation should change depending on the characteristics of the environment, but it can help to provide an initial value for lmed , since this value will be the mean length for each distance. The simplest way to obtain this value is to fit the data by using least squares, in this case, with a straight line (see Figure 3.36). The solution obtained was as follows: lmed =0.6597 ∗d+2.5307 3.5 Initial length value 33 Figure 3.36 Least square line. Introducing this value into the algorithm is as simple as doing as shown in Figure 3.37. Figure 3.37 Initial point in algorithm. To find out if there is a significant variation with respect to the value used originally, it is used a table of times according to the number of obstacles and compared with the new results (see Table 3.8). Table 3.8 Time, obstacles and methods 1. L Tprevious Tnew 5 0.0021 0.0012 8 0.0054 0.0041 11 0.0114 0.0106 14 0.0284 0.0227 17 0.0435 0.0404 20 0.0624 0.0816 23 0.1021 0.1063 26 0.1724 0.1718 The processing time is smaller until 17 intermediate obstacles are reached, thereafter, the algorithm works worse. This is because the data from which the least squares line is derived comes from simulations with 16 intermediate obstacle points. To solve this error, the number of intermediate obstacles can be introduced as a new variable and the above calculations can be repeated, obtaining a new formula to lmed. lmed =0.9981 ∗d−0.0017 ∗z+2.5090 Where z is the number of obstacles between A and B . Now, Table 4.1 is created with the same purpose as the last one, to see if there is a significant variation in the time of the auxiliary algorithm. 34 Chapter 3. Algorithms Table 3.9 Time, obstacles and methods 2. L Tprevious Tnew 5 0.0021 0.0017 8 0.0054 0.0032 11 0.0114 0.0087 14 0.0284 0.0139 17 0.0435 0.0238 20 0.0624 0.0381 23 0.1021 0.0438 26 0.1724 0.0578 To get a clearer idea of the solutions obtained, the average processing times obtained are plotted (see Figure 3.38). Figure 3.38 Time and methods. Making use of distance has no significant effect when increasing the number of obstacles, but taking the obstacles into account to calculate lmed reduces the time needed to find a solution by almost half. 4 Extensions In the above Chapters we showed that the algorithm is capable of finding a solution to the proposed problem and its parameters have been modified to obtain the solution in the shortest possible time as possible. In this Chapter, we explore how to adapt the method to find solutions for some extensions of the problem. 4.1 Exploring around the target point Once the aerial vehicle has reached the target point, it may need to be able to move in different directions to complete the inspection. If it is assumed that the obstacles will not be highly variable and that the ground vehicle is able to increase the initial height of the catenary to a specific point, it is possible to create an algorithm that allows the aerial vehicle to move. By making a few small changes to the function that previously calculated the position in the plane (auxiliary algorithm), it can be converted into one that calculates the initial vertical position of the catenary (see Figure 4.1). Figure 4.1 Function vertical position. This new function requires 2 more inputs than before: 35 36 Chapter 4. Extensions 1. Pos: This is the maximum vertical position of the catenary starting point. 2. Pos2: This is the vertical position with respects to point By . If positive, the drone has a pos2 value greater than By, otherwise, in the negative case, it has a pos2 value less than By. What this algorithm allows to do is to find a catenary’s length that allows the drone to move vertically. Figure 4.2 shows the value of the vertical position (of the initial point) against the catenary’s length that allows the solution of the problem. Figure 4.2 Wire’s length and vertical position 1. The orange graph represents the catenary’s length and the blue graph the vertical position. For a movement from -1 metre to 1 metre with respect to the target point, it is able to find a solution up to a point quite close to one metre high, since there is always a collision of an obstacle with the catenary. Repeating the simulation with other randomly generated obstacles, the aerial vehicle is able to perform the complete movement. In this case, the catenary’s length does not continue to increase, as it has reached its defined maximum (see Figure 4.3). Figure 4.3 Wire’s length and vertical position 2. If in addition to the ability for vertical positions, it is also able to move horizontally, it is possible to implement an algorithm that allows the complete movement of the starting point of the catenary (see Figure 4.4). 4.1 Exploring around the target point 37 Figure 4.4 Complete position function 1. Basically, it consists of adding the horizontal position loop as the last position option, since it takes the longest time (ground vehicle movement). If the results are plotted, the Figure 4.5 is obtained. Figure 4.5 Wire’s length and positions 1. The blue curve represents the vertical position, the orange curve the horizontal position and the green curve the length of the catenary. Notice that the length is always the maximum possible; this is because the length loop is the main one, to obtain something more realistic, the function is changed as follows: Now, let’s consider the vehicle with a horizontal movement. If the results of the new function are plotted, the Figure 4.7 is obtained. It does not make much difference, the other simulated cases are quite similar, the system always dispenses with one of the variables, which seems to be fixed, and the others vary. Then, it is not worth using all three 44 Chapter 5. Experiments Figure 5.2 Scenario 1 solution. 5.2 Second scenario: Tunnel The tunnel consists of a straight road from which the ground vehicle cannot exit and a road going upwards, where the target point is located. The solutions to be found by the algorithm must be inside the yellow zone (see Figure 5.3). Figure 5.3 Scenario 2. The tunnel walls are interpreted as high ground obstacles and the roof as aerial obstacles. There are no intermediate obstacles, so the processing time should not vary too much from the first scenario. Once the algorithm is executed, the planes in which it has searched for a solution and the solution it has found in each one are obtained. Since there are no obstacles inside the tunnel, the first search position of the algorithm only depends on the maximum wire’s length, so if this does not allow the aerial vehicle to be deployed from the initial position, the first and optimal solution will always be the one in the plane connecting the initial and target points (see Figure 5.4), with all other solutions being at a greater distance. To check that the algorithm is fully considering the tunnel, the shortest length solutions corresponding to each plane are plotted in Figure 5.5. The tunnel is being studied correctly. The algorithm searched for a solution in 43 planes and took 1.5477 seconds to determine the optimal position. Calculating the processing time required per plane gives 0.035993 5.3 Third scenario: Zig-Zag Tunnel 45 Figure 5.4 Scenario 2 solution. Figure 5.5 Scenario 2 plane solutions. seconds per plane, five times more than the scenario without obstacles (0.006697 seconds per plane), which is not a bad result. 5.3 Third scenario: Zig-Zag Tunnel Figure 5.6 Scenario 3. 46 Chapter 5. Experiments The Zig-Zag tunnel is a more complex version of the previous scenario, as it does not allow linear movement between the starting point and the target point (see Figure 5.6). As in the previous scenario, movement is only allowed in the yellow zone. Once the algorithm is executed, the following solution map is obtained (see Figure 5.7) after searching within 99 planes and an average time of 3.69349 seconds (0.037308 seconds per plane, an result similar to the previous scenario). Figure 5.7 Scenario 3 solution. The chosen solution is the closest to the initial point, but it is not the closest to the path of the ground vehicle, since it is not considering the obstacles to calculate the solutions. In order to obtain a more realistic solution, the edge detection algorithm developed in the previous chapter will be used. When used, it finds the black spot as a source for finding solutions (see Figure 5.8). Figure 5.8 Edge detection algorithm point. The following solutions have been obtained: The algorithm finds the same number of solutions, but the optimal solution chosen is closer to the path that the ground vehicle must follow. The number of evaluated planes and the average solution search time is equal to the case without edge detection algorithm. If version 2 of the edge detection algorithm is used, the same solutions are obtained, but the initial search point is as shown in Figure 5.10. 5.4 Fourth scenario: Walls 47 Figure 5.9 Edge detection algorithm solution. Figure 5.10 Edge detection algorithm point, version 2. 5.4 Fourth scenario: Walls Figure 5.11 Scenario 4. 48 Chapter 5. Experiments This scenario will allow the study of the functioning and efficiency of the edge detection algorithm. The scenario consists of three walls that block the passage of the ground vehicle. For a better view, Figure 5.12 shows a plan view of the scenario. Figure 5.12 Scenario 4 plan view. When running the edge detection algorithm, the following points are identified: Figure 5.13 Scenario 4 Edge detection algorithm. Recall that the exact position of these points is not what the algorithm is identifying, since each plane is being discretised. Having found the points from which a solution can be found, two simple solutions are possible: 1. Choose the solution closest to the point closest to the origin of the ground vehicle. 2. Choose the solution closest to the point furthest from the origin of the ground vehicle and closest to the target point. If the second option is chosen, the results shown in Figure 5.14 are obtained. 130 plans were evaluated in an average time of 0.92667 seconds (0.00713 seconds per plane). If, on the other hand, the first method is used, the solutions in Figure 5.15 are obtained. This time, the algorithm searches only 5 planes in an average time of 0.22247 seconds (0.044494 seconds per plane). It increases the number of seconds needed per plane, but drastically decreases the number of planes evaluated, making this option more convenient. 5.5 Adding obstacles 49 Figure 5.14 Scenario 4 Edge detection algorithm solution 2. Figure 5.15 Scenario 4 Edge detection algorithm solution 1. 5.5 Adding obstacles Finally, some obstacles are added to make the marsupial drone’s movement even more difficult. To obtain the comparison, simulations are done by adding land obstacles, then aerial obstacles (both randomly generated) and, finally, the case where both obstacles are present is simulated. A large number of simulations are run. Table 5.1 shows the average processing time and the average number of planes searched. Table 5.1 Average times and planes. Second column without obstacles. Third column with land obstacles. Fourth column with aerial obstacles. Fifth column with both obstacles. Scenario Time/Planes Time/Planes Time/Planes Time/Planes Simple 0.04688 / 7 0.35510 / 31 2.50390 / 39 2.75723 / 73 Tunnel 1.54770 / 43 1.35981 / 78 0.62844 / 26 1.58846 / 61 Zig-Zag Tunnel 3.69349 / 99 1.98436 / 101 4.0088 / 101 3.66593 / 108 Walls 0.22247 / 5 0.56187 / 66 1.8837 / 71 2.29530 / 87 6 Conclusiones y trabajo futuro Es te estudio propone un método de planificación del movimiento para un sistema robótico marsupial. El sistema consta de un vehículo terrestre y un vehículo aéreo conectados por un cable, que se modela mediante una catenaria. El vehículo terrestre tiene que moverse hasta una posición que permita al vehículo aéreo alcanzar un objetivo dado sin colisiones. El movimiento del vehículo aéreo no se produce hasta que se garantiza que el vehículo de tierra está en la posición correcta. El objetivo es mover el vehículo terrestre lo menos posible, es decir, alcanzar el objetivo en el menor tiempo posible. Nuestro enfoque busca una solución de aproximación al problema de optimización. Para ello, el espacio tridimensional se ha discretizado considerando un conjunto discreto de planos perpendiculares al suelo y que pasan por el objetivo. El algoritmo principal se basa en un algoritmo auxiliar que resuelve la versión bidimensional del problema correspondiente a cada plano. Tras ajustar los parámetros correspondientes, ha sido posible diseñar un algoritmo eficiente de planificación de trayectorias que puede utilizarse en línea. La eficiencia del método utilizado ha sido probada utilizando diferentes escenarios, obteniéndose trayectorias eficientes incluso en situaciones complejas. Como trabajo futuro, queda pendiente la comparación de nuestro planificador con otros existentes en la literatura aplicados al mismo sistema marsupial, así como la posibilidad de diseñar un nuevo método de planificación en el que ambos vehículos se muevan al mismo tiempo. Esto permitiría la posibilidad de extender el enfoque para otras aplicaciones como la exploración del escenario en lugar de alcanzar el objetivo lo antes posible. 51 7 Conclusions and future work Th is study proposes a motion planning method for a marsupial robotic system. The system consists of a ground vehicle and an aerial vehicle connected by a wire, which is modelled by a catenary. The ground vehicle has to move to a position that allows the aerial vehicle to reach a given target without collisions. The movement of the air vehicle does not take place until it is guaranteed that the ground vehicle is in the correct position. The goal is to move the ground vehicle as little as possible, that is, to reach the target in the shortest possible time. Our approach looks for an approximation solution to the optimization problem. Then, the three-dimensional space has been discretised by considering a discrete set of planes perpendicular to the ground and passing through the target. The main algorithm is based on an auxiliary algorithm that solves the two-dimensional version of the problem corresponding to each plane. After adjusting the corresponding parameters, it has been possible to design an efficient path planning algorithm that can be used online. The efficiency of the method used has been tested using different scenarios, obtaining efficient trajectories even in complex situations. As future work, it remains to compare our planner with others in the literature applied to the same marsupial system, as well as the possibility of designing a new planning method in which both vehicles move at the same time. This would allow the possibility of extending the approach for other applications as the exploration of the scenario instead of reaching the target as soon as possible. 53 60 Chapter 8. Appendix r=0 m=0 m2="No value" Da=100000 W=0 Lista=[] while(r<4): This number can be changed x=np.linspace(int(Pizq*360/(2*pi)),int((Pder+Pizq)/2),int((Pder-Pizq)*M/(4*pi))) y=np.linspace(int(Pder*360/(2*pi)),int((Pder+Pizq)/2),int((Pder-Pizq)*M/(4*pi))) Gxs=vectbisect(x,y) for grado in Gxs: ||Between -90ºand 90º if grado < 0: i=int((grado+360)*M/360) else: i=int(grado*M/360) print(i) if i in Lista: r+=1 print("Lista") else: if W==0: i=0 grado=0 print("At 0ºplane") Lista += [i] if grado < Pizq*360/(2*pi) or grado > Pder*360/(2*pi): Lista += [i] break A=punto() calculo(A, B, L, planos[i]) if planos[i].dmin <=L: D = math.sqrt((B.x - (planos[i].dmin) * math.cos(planos[i].grado * 2 * pi / 360)) ** 2 + ( (planos[i].dmin) * math.sin(planos[i].grado * 2 * pi / 360)) ** 2) if D<=Da: m=i m2 = D x, y, z = scipy.optimize.fsolve(nonlinear, [1,1,1]) print("P1= ", y,",",z) if k>0: if math.asin(y / L)<Pder: Pder = math.asin(y / math.sqrt(L**2-B.y**2)) r=0 else: r+=1 else: Pder = math.asin(y / math.sqrt(L**2-B.y**2)) x, y, z = scipy.optimize.fsolve(nonlinear, [-1, -1, 1]) print("P2= ", y, ",", z) if k > 0: if math.asin(y / math.sqrt(L**2-B.y**2)) > Pizq: Pizq = math.asin(y / math.sqrt(L**2-B.y**2)) else: Pizq = math.asin(y / math.sqrt(L**2-B.y**2)) print("Pder=",int(Pder*360/(2*pi))) print("Pizq=", int(Pizq*360/(2*pi))+360) print("Distancia=",B.x-planos[i].dmin) print("Grado=",planos[i].grado) k+=1 8.2 Visualisation 61 Da=D if W!=0: break r+=1 if W==0: W=1 break 8.2 Visualisation fig = matplotlib.pyplot.figure() ax = fig.add-subplot(111, projection="polar") for i in range(M): if planos[i].dmin <= L and planos[i].dmin >= -L: ax.plot(i*(360/M)*2*pi/360,planos[i].dmin,’o’,mfc=’black’) ax.plot(0,B.x,’s’,mfc=’blue’) ax.plot((m*2*pi)/360,B.x-m2,’D’,mfc=’red’) plt.show() A.x=B.x-planos[m].dmin dibujacat(A,B,calculo(A,B,L,planos[m])) dibujaobs(planos[m].obs1) dibujaobs(planos[m].obs2) dibujaobs(Suelo) show() 8.3 Auxiliary algorithm with movement at the target point. Version 1 def calculo(A,B,L,plano,xmin,pos,pos2): try: obs2=plano.obs1 obs4=plano.obs2 suelo=plano.suelo lmax=L B.y=B.y+pos2 for A.y in np.linspace(0.2,pos, 100): flag=0 lmin=math.sqrt(((B.x-A.x)**2)+((B.y-A.y)**2)) Pint=int((2+N*((B.x-A.x)/(B.x-xmin)))/2) x=np.linspace(lmax, (lmax+lmin)/2, Pint) y=np.linspace(lmin,(lmax+lmin)/2, Pint) for l in vectbisect(x,y): if(flag==0 and choque(suelo,A,B,l)==0): lmax=l flag=1 elif(obs4.pol(A.x)<=0 and choque(obs4,A,B,l)==0 and choque(suelo,A,B,l)==0 and choque(obs2,A,B,l)==0): plano.dmin=B.x-A.x return l return "Fallo" except ValueError: return "Fallo" 62 Chapter 8. Appendix 8.4 Auxiliary algorithm with movement at the target point. Version 2 def calculo(A,B,L,plano,pos,pos2): try: obs2=plano.obs1 obs4=plano.obs2 suelo=plano.suelo B.y=B.y+pos2 Zx=obs2.pos[0,np.where(obs2.pos[1,]==np.amin(obs2.pos[1,]))[0]] Zy=np.amin(obs2.pos[1,]) C1=-2*Zx C2=(Zy**2)+(Zx**2)-(L**2)+2*L*math.sqrt(((B.y-Zy)**2)+((B.x-Zx)**2))-((B.y-Zy)**2)-((B.x-Zx)**2) xm1=(-C1-math.sqrt((C1**2)-4*C2))/2 xm2=B.x-math.sqrt((L**2)-((B.y-A.y)**2)) xmin=max(xm1,xm2,A.x) for A.y in np.linspace(0.2, pos, 100): for A.x in np.linspace(xmin, B.x - ((B.x - xmin) / N), N - 1): lmax = L lmin = math.sqrt(((B.x - A.x) ** 2) + ((B.y - A.y) ** 2)) Pint = int((2 + N * ((B.x - A.x) / (B.x - xmin))) / 2) for l in np.linspace(lmin, lmax, Pint * 2): if(obs4.pol(A.x)<=0 and choque(obs4,A,B,l)==0 and choque(suelo,A,B,l)==0 and choque(obs2,A,B,l)==0): plano.dmin=B.x-A.x return l return "Fallo" except ValueError: return "Fallo" 8.5 Algorithms with floating obstacles Creation of floating obstacles: def creaobsflot(): obsx=obsflot() a=B.x/2 a=a-2+4*np.random.rand(1) b=B.y/3 b = b - 0.5 + 1 * np.random.rand(1) obsx.pos=np.array([[a+a/10,a+a/6,a+a/4,a+a/10],[b+b/10,b+b/3,b+b/6,b+b/10]]) obsx.sit="Flotante" return obsx class plano(): obs1=creaobs("Aereo") obs2=creaobs("Terrestre") suelo=Suelo obsf=creaobsflot() obsf2=creaobsflot() obsf3=creaobsflot() grado=1 dmin=10000 Collision function: 8.5 Algorithms with floating obstacles 63 def choque(Obs,A,B,L): try: k=0 if Obs.sit == "Aereo": for i in Obs.pos[0,]: if i>=A.x and i<B.x: if catenaria(A,B,L,i)-Obs.pos[1,k] > 0: return 1 k+=1 return 0 elif Obs.sit == "Terrestre": k=0 for i in Obs.pos[0,]: if i>=A.x and i<=B.x: if catenaria(A,B,L,i)-Obs.pos[1,k] < 0: return 1 k+=1 return 0 elif Obs.sit == "Flotante": k=0 i=Obs.pos[0,0] if catenaria(A,B,L,i)-Obs.pos[1,k] > 0: m=1 else: m=2 if m==1: for i in Obs.pos[0,]: if i >= A.x and i < B.x: if catenaria(A, B, L, i) - Obs.pos[1, k] < 0: return 1 k += 1 return 0 else: for i in Obs.pos[0,]: if i >= A.x and i < B.x: if catenaria(A, B, L, i) - Obs.pos[1, k] > 0: return 1 k += 1 return 0 except np.core.-exceptions.UFuncTypeError: return 1 Auxiliary algortihm: def calculo(A,B,L,plano): try: obs2=plano.obs1 obs4=plano.obs2 obsf=plano.obsf obsf2=plano.obsf2 obsf3 = plano.obsf3 suelo=plano.suelo Zx=obs2.pos[0,np.where(obs2.pos[1,]==np.amin(obs2.pos[1,]))[0]] Zy=np.amin(obs2.pos[1,]) C1=-2*Zx C2=(Zy**2)+(Zx**2)-(L**2)+2*L*math.sqrt(((B.y-Zy)**2)+((B.x-Zx)**2))-((B.y-Zy)**2)-((B.x-Zx)**2) 64 Chapter 8. Appendix xm1=(-C1-math.sqrt((C1**2)-4*C2))/2 xm2=B.x-math.sqrt((L**2)-((B.y-A.y)**2)) xp1=B.x-B.y*((B.x-obsf.pos[0,2])/(B.y-obsf.pos[1,2])) xp2=B.x-B.y*((B.x-obsf2.pos[0,2])/(B.y-obsf2.pos[1,2])) xp3 = B.x - B.y * ((B.x - obsf3.pos[0, 2]) / (B.y - obsf3.pos[1, 2])) xmin=max(xm1,xm2,A.x,(xp1+xp2+xp3)/3) lmax=L for A.x in np.linspace(xmin, B.x-((B.x-xmin)/N), N-1): lmin=math.sqrt(((B.x-A.x)**2)+((B.y-A.y)**2)) lmax=(3*L+lmin)/4 Pint=int((2+N*((B.x-A.x)/(B.x-xmin)))/2) Pmed = 0.9981 * (B.x - A.x) - 0.0017 * N + 2.5090 x=np.linspace(lmax,Pmed, Pint) y=np.linspace(lmin,Pmed-1/Pint, Pint) for l in vectbisect(x,y): if(obs4.pol(A.x)<=0 and choque(obs4,A,B,l)==0 and choque(suelo,A,B,l)==0 and choque(obs2,A,B,l)==0 and choque(obsf,A,B,l)==0 and choque(obsf2,A,B,l)==0 and choque(obsf3,A,B,l)==0): plano.dmin=B.x-A.x return l return "Fallo" except ValueError: return "Fallo" 8.6 Auxiliary algorithm with floating obstacles. Version 2 def calculo(A,B,L,plano): try: obs2=plano.obs1 obs4=plano.obs2 obsf=plano.obsf obsf2=plano.obsf2 obsf3 = plano.obsf3 suelo=plano.suelo Zx=obs2.pos[0,np.where(obs2.pos[1,]==np.amin(obs2.pos[1,]))[0]] Zy=np.amin(obs2.pos[1,]) C1=-2*Zx C2=(Zy**2)+(Zx**2)-(L**2)+2*L*math.sqrt(((B.y-Zy)**2)+((B.x-Zx)**2))-((B.y-Zy)**2)-((B.x-Zx)**2) xm1=(-C1-math.sqrt((C1**2)-4*C2))/2 xm2=B.x-math.sqrt((L**2)-((B.y-A.y)**2)) xp1=B.x-B.y*((B.x-obsf.pos[0,2])/(B.y-obsf.pos[1,2])) xp2=B.x-B.y*((B.x-obsf2.pos[0,2])/(B.y-obsf2.pos[1,2])) xp3 = B.x - B.y * ((B.x - obsf3.pos[0, 2]) / (B.y - obsf3.pos[1, 2])) xmin=max(xm1,xm2,A.x,(xp1+xp2)/2,(xp2+xp3)/2,(xp1+xp3)/2) //New lmax=L for A.x in np.linspace(xmin, B.x-((B.x-xmin)/N), N-1): lmin=math.sqrt(((B.x-A.x)**2)+((B.y-A.y)**2)) lmax=(3*L+lmin)/4 Pint=int((2+N*((B.x-A.x)/(B.x-xmin)))/2) Pmed = 0.9981 * (B.x - A.x) - 0.0017 * N + 2.5090 x=np.linspace(lmax,Pmed, Pint) y=np.linspace(lmin,Pmed-1/Pint, Pint) for l in vectbisect(x,y): if(obs4.pol(A.x)<=0 and choque(obs4,A,B,l)==0 and choque(suelo,A,B,l)==0 and choque(obs2,A,B,l)==0 and choque(obsf,A,B,l)==0 and choque(obsf2,A,B,l)==0 and choque(obsf3,A,B,l)==0): plano.dmin=B.x-A.x 8.7 Creation of planes 65 return l return "Fallo" except ValueError: return "Fallo" 8.7 Creation of planes M=1000 //Number of planes planos=[] for i in range(M): planos+=[plano()] planos[i].obs1=creaobs("Aereo") planos[i].obs2=creaobs("Terrestre") planos[i].obsf1 = creaobsflot() planos[i].obsf2 = creaobsflot() planos[i].obsf3 = creaobsflot() planos[i].grado=i*360/M 8.8 Edge detection algorithm aa=0 points=[] gs=[] for i in range(90): ls=[1000] for j in range(N-2): if (planos[i].obs2.pos[1,j]==0 and planos[i].obs2.pos[1,j+1]>0 and planos[i].obs2.pos[1,j+2]==0): ls2 = [planos[i].obs2.pos[0, j + 1]] ls += ls2 a=min(ls) if a - aa > (B.x / N + 0.2) and i!=0 and i!=1 and a!=1000 and aa!=1000: points+=[a] gs+=[i] elif a - aa < (-B.x / N - 0.2) and i!=0 and i!=1 and a!=1000 and aa!=1000: points+=[aa] gs += [i-1] aa = a for i in range(90): i=i+270 ls=[1000] for j in range(N-2): if (planos[i].obs2.pos[1,j]==0 and planos[i].obs2.pos[1,j+1]>0 and planos[i].obs2.pos[1,j+2]==0): ls2=[planos[i].obs2.pos[0, j+1]] ls += ls2 a = min(ls) if a - aa > (B.x / N + 0.2) and i!=270 and i!=271 and a!=1000 and aa!=1000: points+=[a] gs += [i] elif a-aa<(-B.x/N-0.2) and i!=270 and i!=271 and a!=1000 and aa!=1000: points+=[aa] gs += [i-1] aa=a 66 Chapter 8. Appendix P=np.array(points) P=np.amax(P) G=np.array(gs) G=G[np.where(points==np.amax(points))[0]] d=B.x-P Ps=np.array([d*math.sin(G*pi/180),B.x-(B.x-d)*math.cos(G*pi/180)]) 8.9 Edge detection algorithm. Version 2 aa=0 points=[] gs=[] sm=0 for i in range(90): ls=[1000] for j in range(N-2): j=N-1-j if (planos[i].obs2.pos[1,j]>0 and planos[i].obs2.pos[1,j+1]==0 ): ls2 = [planos[i].obs2.pos[0, j + 1]] ls += ls2 sm=0 for j in range(N): sm+=planos[i].obs2.pos[1,j] if sm==0: ls2 = [B.x] ls += ls2 a=min(ls) if a - aa > (B.x / N + 0.2) and i!=0 and i!=1 and a!=1000 and aa!=1000: points+=[aa] gs+=[i-1] elif a - aa < (-B.x / N - 0.2) and i!=0 and i!=1 and a!=1000 and aa!=1000: points+=[a] gs += [i] aa = a for i in range(90): i=i+270 ls=[1000] for j in range(N-2): j=N-1-j if (planos[i].obs2.pos[1, j] > 0 and planos[i].obs2.pos[1, j + 1] == 0 ): ls2 = [planos[i].obs2.pos[0, j + 1]] ls += ls2 sm=0 for j in range(N): sm += planos[i].obs2.pos[1, j] if sm == 0: ls2 = [B.x] ls += ls2 a = min(ls) if a - aa > (B.x / N + 0.2) and i!=270 and i!=271 and a!=1000 and aa!=1000: points+=[aa] gs += [i-1] 8.9 Edge detection algorithm. Version 2 67 elif a-aa<(-B.x/N-0.2) and i!=270 and i!=271 and a!=1000 and aa!=1000: points+=[a] gs += [i] aa=a try: px=points.index(min(points)) P=(points[px]) G =(gs[px]) Gs=G d=B.x-P Ps = np.array([d * math.sin(G * pi / 180), B.x - (B.x - d) * math.cos(G * pi / 180)]) except ValueError: Ps=np.array([0,0]) Gs=0