Path Planning and Real-Time Collision Avoidance Based on the Essential Visibility Graph

Abstract

This paper deals with a novel procedure to generate optimum flight paths for multiple unmanned aircraft in the presence of obstacles and/or no-fly zones. A real-time collision avoidance algorithm solving the optimization problem as a minimum cost piecewise linear path search within the so-called Essential Visibility Graph (EVG) is first developed. Then, a re-planning procedure updating the EVG over a selected prediction time interval is proposed, accounting for the presence of multiple flying vehicles or movable obstacles. The use of Dubins curves allows obtaining smooth paths, compliant with flight mechanics constraints. In view of possible future applications in hybrid scenarios where both manned and unmanned aircraft share the airspace, visual flight rules compliant with International Civil Aviation Organization (ICAO) Annex II Right of Way were implemented. An extensive campaign of numerical simulations was carried out to test the effectiveness of the proposed technique by setting different operational scenarios of increasing complexity. Results show that the algorithm is always able to identify trajectories compliant with ICAO rules for avoiding collisions and assuring a minimum safety distance as well. Furthermore, the low computational burden suggests that the proposed procedure can be considered a promising approach for real-time applications.

1. Introduction

In the last few years, improvements in technology have led to an increasing use of unmanned aerial vehicles, especially for the reduction of costs and human risks they can assure. Many researchers are currently focusing on the perspective of hybrid airspaces, in which both manned and unmanned aircraft share the environment and information, to prepare scenarios implementing effective and general purpose unmanned missions [1,2,3,4,5].

Unmanned Traffic Management (UTM) depends on the ability to ensure safe and efficient paths to every aircraft in the airspace [6]. In this context, the research on path planning becomes central, with the need for ever faster and more effective algorithms to be used in real-time applications.

Path planning has been studied using several approaches in the literature [7,8,9]. One of the most effective categories of path planning techniques is based on geometrical methods, which can be implemented in real time due to their low computational burden. The most famous approach is probably the one based on Dubins’ car [10,11,12,13,14], in which planned paths are made up as a sequence of straight lines and circular arcs.

However, pure geometrical approaches can be hardly used in the presence of obstacles without any amendment. In some cases, they are used to smooth trajectories generated with other methods, including graph based paths. Graphs are widely used in trajectory optimization thanks to their capability of discretizing and modeling the continuum by means of a certain number of nodes and arcs: probabilistic roadmaps [15,16], Voronoi diagrams [17,18,19,20], cell decomposition [21,22,23,24,25], and polynomial and spline parameterizations [26,27,28] are examples of methods based on graphs.

The Visibility Graph (VG) [29,30,31] can be a very effective approach in the case of polygonal obstacles. Several approaches based on VGs have been developed to reduce the computational burden, as the Reduced Visibility Graph (RVG) [32,33,34] and the Essential Visibility Graph (EVG) [34,35,36].

Other popular techniques in path planning make use of potential fields for both offline nominal trajectory optimization [37,38] and online collision avoidance [39,40,41]. More sophisticated approaches are specifically aircraft oriented, integrating flight dynamics constraints into the problem [42,43,44]. Trajectory optimization with constraints can be also based on nature inspired optimization techniques [45], but they are usually too burdensome to be implemented in real-time applications.

The available approaches for conflict resolution can be grouped into three different categories. Prescribed methods [46,47,48] are based on rules and protocols, and they are very effective in the case of integration in a preexisting airspace, but hardly capable of facing unexpected conditions. Optimization based methods [49,50,51,52,53,54,55] inherit several techniques from offline path planning and obstacle avoidance. Optimization makes these methods more versatile, but it increases the computational burden; for this reason, they are difficult to implement in real-time applications. Potential field based techniques [56,57,58,59,60,61] are based on the concept of considering aircraft as charged particles repelled by any other vehicle or obstacle. While the computational burden of these approaches are very low, sometimes they can result in non-feasible paths and possible deadlocks, limiting the capability to make optimal decisions independently. In the literature [62,63], some works exist that try to solve the problem of escaping from deadlocks and local minimums.

In order to optimize flight paths in real-time collision avoidance problems, in the presence of other aircraft or newly detected obstacles, this paper deals with the design of a reactive distributed guidance system. The proposed algorithm considers independent aircraft, sharing information only about position, speed, and direction within their communication/sensor range.

One of the original contributions of this work is the use of the Essential Visibility Graph (EVG). While the EVG was previously described in [34,36], in this paper, a complete proof of the optimality of the resulting path under some assumptions is presented. The EVG concept is extended in the presence of unknown or newly detected obstacles in both static and dynamic scenarios. Although the application of the velocity obstacles concept is shared with other papers [64,65,66,67,68], the applicability with obstacles of a general shape and the efficient use of the visibility graph concept coupled with Dubins’ paradigm in finding the optimum flyable path place the proposed approach a further step ahead in the field of aircraft collision avoidance algorithms; finally, the further implementation of the visual flight rules, compliant with the International Civil Aviation Organization (ICAO) Annex II Right of Way rules, makes our proposed collision avoidance strategy suitable for future applications in hybrid scenarios where both manned and unmanned aircraft share the airspace.

The article is organized as follows: In Section 2, the geometrical approach based on Dubins’ car is recalled, aimed at providing smooth trajectories compliant with the aircraft minimum turn radius constraint; in Section 3, a graph based approach based on the Essential Visibility Graph (EVG) is presented, with the proof of the optimality of the resulting path. The extension of such a method to dynamic scenarios with the implementation of the ICAO Right of Way rules is presented in Section 4. Finally, numerical results are shown and discussed in Section 5.

2. Dubins’ Path Generation in the Absence of Obstacles

Consider two points $A=(x_A;y_A)$ and $B=(x_B;y_B)$, namely the starting and target point, and two heading angles ${\psi }_A$ and ${\psi }_B$ used to define the current starting point and the desired final direction, respectively.

To obtain a path compliant with aircraft performance limits, connecting A with B with given heading angles, the trajectory must be smooth and continuous with bounds on curvature to account for the minimum flight turning radius $R_{min}$. The quality of the path is measured in terms of path length, in order to rate the fuel consumption.

In 2D unconstrained environments, Dubins’ algorithm [10] is able to provide optimal and smooth paths, which are composed of straight lines and circular arcs only. Optimality, in the absence of obstacles, is guaranteed under the assumption that the distance between A and B is more than $2R_{min}$. Consequently the path is composed of three pieces: an arc of circumference with the radius equal to $R_{min}$, a straight line, and another circular arc with the same radius.

The optimal path is obtained with the following geometrical construction.

• Consider the circumferences, ${{\mathfrak{C}}_A}_{Left}$ and ${{\mathfrak{C}}_A}_{Right}$ (${{\mathfrak{C}}_B}_{Left}$ and ${{\mathfrak{C}}_B}_{Right}$), with radius $R_{min}$ to the left and right of the starting (final) point A (B) and having at this point a tangent described by ${\psi }_A$ (${\psi }_B$).
• For each pair of circumferences (${{\mathfrak{C}}_A}_{Right}$, ${{\mathfrak{C}}_B}_{Right}$), (${{\mathfrak{C}}_A}_{Right}$, ${{\mathfrak{C}}_B}_{Left}$), (${{\mathfrak{C}}_A}_{Left}$, ${{\mathfrak{C}}_B}_{Left}$), and (${{\mathfrak{C}}_A}_{Left}$, ${{\mathfrak{C}}_B}_{Right}$), four common tangents can be used to connect one circumference to the other, but only one is compatible with the starting direction ${\psi }_A$ and the final direction ${\psi }_B$.
• Along the above tangents compatible with ${\psi }_A$ and ${\psi }_B$, four feasible paths can be computed, namely RSR (Right Straight Right) for $({\mathfrak{C}}_{A_{Right}},{\mathfrak{C}}_{B_{Right}})$, RSL (Right Straight Left) for $({\mathfrak{C}}_{A_{Right}},{\mathfrak{C}}_{B_{Left}})$, LSL (Left Straight left) for $({\mathfrak{C}}_{A_{Left}},{\mathfrak{C}}_{B_{Left}})$, and LSR (Left Straight Right) for $({\mathfrak{C}}_{A_{Left}},{\mathfrak{C}}_{B_{Right}})$.
• The optimal path is the shortest among RSR, RSL, LSL, and LSR.

Figure 1 provides an example of how Dubins’ concept is applied. Assuming $A=$ (−1 km; −1 km) and $B=$ (1 km; 1 km) with ${\psi }_A=-2\pi /3$ rad and ${\psi }_B=-\pi /3$ rad and a turning radius $R_{min}=250$ m it shows the four feasible paths, namely RSR, RSL, LSL, and LSR. In this case, the shortest one is the RSR path.

3. Obstacle Avoidance Using EVG

Consider the path planning problem between A and B described in the previous section. In the presence of obstacles, it is possible to convert the trajectory planning problem into a minimum cost path search within a so-called Essential Visibility Graph (EVG) as presented in [35], under the following main assumptions:

Hypothesis 1 (H1).

Obstacles can be approximated by polygons.

Hypothesis 2 (H2).

Minimum turn radius $R_{min}$ is much shorter than both the distance between any two obstacle vertices and the distance between any obstacle vertex and the A/B points.

Hypothesis 3 (H3).

Desired starting and ending heading ${\psi }_A$ and ${\psi }_B$ can be locally achieved by using Dubins’ circles with a negligible increase of the total path length.

The flight path optimization problem is then formulated as follows.

Problem 1.

Given starting and target points and directions, say A and B, and ${\psi }_A$ and ${\psi }_B$, respectively, and $N_o$ polygonal obstacles, find the shortest piecewise linear path connecting A to B.

Let us denote with ${\mathcal{I}}_l$ the set of vertices of obstacle ${\mathfrak{P}}_l$ ($l=1,\cdots ,N_o$) and build a graph whose node set is $\mathsf{\Omega }=\left\{A,B,{\bigcup }_{l=1}^{N_o}{\mathcal{I}}_l\right\}$.

Definition 1.

The set of visible vertices from a node $V_i$ is defined as:

$${\mathcal{C}}_{V_i}=\left\{V_j\in \mathsf{\Omega }:V_i\to V_j\right\}$$

(1)

where $V_i\to V_j$ means that there exists a segment $r_{V_j,V_i}$ connecting $V_i$ to $V_j$, which does not intersect any obstacle.

Definition 2.

The set of obstacles reachable from $V_i$ is:

$${\mathcal{O}}_{V_i}=\left\{{\mathfrak{P}}_l,l\in \left\{1,\cdots ,N_o\right\}:{\mathcal{M}}_{{\mathfrak{P}}_l,V_i}\ne \left\{0\right\}\right\}$$

(2)

where ${\mathcal{M}}_{{\mathfrak{P}}_l,V_i}={\mathcal{I}}_l\bigcap {\mathcal{C}}_{V_i}$ is the set of the l-th obstacle vertices reachable from $V_i$.

Definition 3.

The set of transition nodes through the obstacle ${\mathfrak{P}}_l$ boundary, from a point $V_i$, is:

$${\mathcal{T}}_{{\mathfrak{P}}_l,V_i}=\left\{\underset{V_j\in {\mathcal{I}}_l}{\mathrm{argmin}}{Œ}_l\left(r_{V_j,V_i}\right);\underset{V_j\in {\mathcal{I}}_l}{\mathrm{argmax}}{Œ}_l\left(r_{V_j,V_i}\right)\right\}\cap {\mathcal{M}}_{{\mathfrak{P}}_l,V_i}$$

(3)

${Œ}_l\left(r_{V_j,V_i}\right)$ being the phase angle of vector $r_{V_j,V_i}$ assuming a reference null angle in correspondence with any obstacle node belonging to ${\mathcal{M}}_{{\mathfrak{P}}_l,V_i}$ (see Figure 2).

Lemma 1.

Given two polygons ${\mathfrak{P}}_E$ and ${\mathfrak{P}}_I$, with ${\mathfrak{P}}_I$ convex and ${\mathfrak{P}}_I\subset {\mathfrak{P}}_E$, then Λ(${\mathfrak{P}}_I$)<$\Lambda$(${\mathfrak{P}}_E$), Λ(${\mathfrak{P}}_k$) being the perimeter length of polygon ${\mathfrak{P}}_k$.

Proof. 

Assume that ${\mathfrak{P}}_E$ and ${\mathfrak{P}}_I$ have m and n sides respectively and vertices $X_i$ (with $i=1,2,\cdots ,m$) and $Y_i$ (with $j=1,2,\cdots ,n$) respectively. By prolonging in sequence each side of ${\mathfrak{P}}_I$, we can build a sequence of n polygons ${\mathfrak{P}}_1,{\mathfrak{P}}_{2,}\cdots ,{\mathfrak{P}}_{n-1},{\mathfrak{P}}_n$ with ${{\mathfrak{P}}_I\equiv \mathfrak{P}}_n\subseteq {\mathfrak{P}}_{n-1}\subseteq \cdots \subseteq {\mathfrak{P}}_2\subseteq {\mathfrak{P}}_1\subset {\mathfrak{P}}_E$ characterized by a decreasing perimeter length, ${\mathfrak{P}}_k$ having k sides in common with ${\mathfrak{P}}_I$.

If ${\mathfrak{P}}_I\subset {\mathfrak{P}}_E$, ${\mathfrak{P}}_I$ has at least one side that is not coincident with any side of ${\mathfrak{P}}_E$. Without loss of generality, we start prolonging this side, to intersect two sides of ${\mathfrak{P}}_E$. With reference to Figure 3, let us consider the prolongation of sides $Y_1{Y}_2$ that intersect the sides $X_1{X}_6$ and $X_2{X}_3$ of ${\mathfrak{P}}_E$ at points $H_1$ and $H_2$, respectively. Polygon ${\mathfrak{P}}_1$, defined by vertices $\{H_1{H}_2{X}_3{X}_4{X}_5{X}_6\}$ in a clockwise sense, is such that ${\mathfrak{P}}_I\subseteq {\mathfrak{P}}_1\subset {\mathfrak{P}}_E$.

Since $d_{H_1{H}_2}<d_{H_1{X}_1}+d_{X_1{X}_2}+d_{X_2{H}_2}$ ($d_{X_j{X}_k}$ is the length of segment $\overline{X_j{X}_k}$), then $\Lambda$(${\mathfrak{P}}_1$)$<\Lambda$(${\mathfrak{P}}_E$). With the same procedure, we can generate polygon ${\mathfrak{P}}_{k+1}\subseteq {\mathfrak{P}}_k$ prolonging the other sides of ${\mathfrak{P}}_I$ to intersect the sides of ${\mathfrak{P}}_k$ and demonstrate that $\Lambda$(${\mathfrak{P}}_k$)$\ge \Lambda$(${\mathfrak{P}}_{k+1}$).

Therefore, we can build a sequence of n polygons such that ${\Lambda (\mathfrak{P}}_1)\ge {\Lambda (\mathfrak{P}}_2)\ge \cdots \ge {\Lambda (\mathfrak{P}}_{n-1})\ge {\Lambda (\mathfrak{P}}_n)$ with ${{\mathfrak{P}}_I\equiv \mathfrak{P}}_n$. □

Theorem 1.

Shortest path connecting two points in the presence of one obstacle: Given two points A and B, B being not reachable from A, and one single obstacle ${\mathfrak{P}}_l$, whose vertices are also nodes of the graph (set of nodes ${\mathcal{I}}_l$), then the optimal path connecting node ${\overline{V}}_0=A$ to ${\overline{V}}_{n+1}=B$ is a piecewise linear path defined by the sequence of $n+2$ nodes ${\overline{V}}_0,{\overline{V}}_1,\cdots ,{\overline{V}}_{n+1}$ with ${\overline{V}}_i\in {\mathcal{T}}_{{\mathfrak{P}}_l,V_{i-1}}\cap {\mathcal{T}}_{{\mathfrak{P}}_l,V_{i+1}},\forall i=1,\cdots ,n$.

Proof. 

Let us prove by contradiction, assuming that there exists an optimal piecewise linear path defined by the sequence of nodes ${\overline{V}}_0,\cdots ,{\overline{V}}_{i-1},{\overline{V}}_i^{*},{\overline{V}}_{i+1},\cdots ,{\overline{V}}_{n+1}$ with ${\overline{V}}_i^{*}\notin {\mathcal{T}}_{{\mathfrak{P}}_l,V_{i-1}}\cap {\mathcal{T}}_{{\mathfrak{P}}_l,V_{i+1}}$.

Case 1. ${\overline{V}}_i^{*}\notin {\mathcal{I}}_l$.

Case 1a. ${\overline{V}}_i^{*}$ is reachable from nodes ${\overline{V}}_{i-1}\notin {\mathcal{I}}_l$ and ${\overline{V}}_{i+1}\notin {\mathcal{I}}_l$ (Figure 4a).

Let us define triangular polygon ${\mathfrak{Q}}_1$ with vertices $\left\{{\overline{V}}_{i-1},{\overline{V}}_i,{\overline{V}}_{i+1}\right\}$ and triangular polygon ${\mathfrak{Q}}_2$ with vertices $\left\{{\overline{V}}_{i-1},{\overline{V}}_i^{*},{\overline{V}}_{i+1}\right\}$. Segment ${\overline{V}}_{i-1}{\overline{V}}_{i+1}$ necessarily cuts the obstacle, otherwise the optimal sequence of nodes would have been ${\overline{V}}_0,\cdots ,{\overline{V}}_{i-1},{\overline{V}}_{i+1},\cdots ,{\overline{V}}_n,{\overline{V}}_{n+1}$, then ${\mathfrak{Q}}_1\subset {\mathfrak{Q}}_2$, and by virtue of Lemma 1, $\Lambda$(${\mathfrak{Q}}_1$)$<\Lambda$(${\mathfrak{Q}}_2$). Since ${\mathfrak{Q}}_1$ and ${\mathfrak{Q}}_2$ share one side, we have:

$$d_{{\overline{V}}_{i-1},{\overline{V}}_i}+d_{{\overline{V}}_i,{\overline{V}}_{i+1}}\le d_{{\overline{V}}_{i-1},{\overline{V}}_i^{*}}{+d}_{{\overline{V}}_i^{*},{\overline{V}}_{i+1}}$$

(4)

which implies that the length of the piecewise linear path through the sequence of nodes ${\overline{V}}_0,\cdots {\overline{V}}_{i-1},{\overline{V}}_i^{*},{\overline{V}}_{i+1},\cdots ,{\overline{V}}_{n+1}$ is not shorter than the length of the piecewise linear path through nodes ${\overline{V}}_0,\cdots {\overline{V}}_{i-1},{\overline{V}}_i,{\overline{V}}_{i+1},\cdots ,{\overline{V}}_{n+1}$, contradicting the optimality of the first sequence.

Case 1b. ${\overline{V}}_i^{*},{\overline{V}}_{i+1}^{*},\cdots ,{\overline{V}}_{i+p}^{*}$ is a sequence of nodes $\notin {\mathcal{I}}_l$ replacing ${\overline{V}}_i,{\overline{V}}_{i+1},\cdots ,{\overline{V}}_{i+m-1}$${\in \mathcal{I}}_l$, ${\overline{V}}_{i-1}$, ${\overline{V}}_{i+m}\notin {\mathcal{I}}_l$ (Figure 4b).

Let us define polygon ${\mathfrak{Q}}_1$ with vertices $\left\{{\overline{V}}_{i-1},{\overline{V}}_i,\cdots ,{\overline{V}}_{i+m-1},{\overline{V}}_{i+m}\right\}$ and polygon ${\mathfrak{Q}}_2$ with vertices $\left\{{\overline{V}}_{i-1},{\overline{V}}_i^{*},\cdots ,{\overline{V}}_{i+p}^{*},{\overline{V}}_{i+m}\right\}$. As for Case 1a, ${\mathfrak{Q}}_1\subset {\mathfrak{Q}}_2$, and by virtue of Lemma 1, $\Lambda$(${\mathfrak{Q}}_1$)$<\Lambda$(${\mathfrak{Q}}_2$). We have:

$$\begin{array}{cc}{d}_{{\overline{V}}_{i-1},{\overline{V}}_i}+d_{{\overline{V}}_i,{\overline{V}}_{i+1}}\hfill & +\cdots \hfill \\ \hfill & +d_{{\overline{V}}_{i+m-1},{\overline{V}}_{i+m}}<d_{{\overline{V}}_{i-1},{\overline{V}}_i^{*}}+\hfill \\ \hfill & +d_{{\overline{V}}_i^{*},{\overline{V}}_{i+1}^{*}}+\cdots +d_{{\overline{V}}_{i+p}^{*},{\overline{V}}_{i+m}}\hfill \end{array}$$

(5)

contradicting the optimality of path ${\overline{V}}_0,\cdots ,{\overline{V}}_{i-1},{\overline{V}}_i^{*},\cdots ,{\overline{V}}_{i+p}^{*},{\overline{V}}_{i+m},{\cdots V}_n$.

Case 2. ${\overline{V}}_i^{*}\in {\mathcal{I}}_l$ (Figure 4c).

Assume that ${\overline{V}}_i^{*}\in {\mathcal{T}}_{{\mathfrak{P}}_l,{\overline{V}}_{i-1}}$ and ${\overline{V}}_i^{*}\notin {\mathcal{T}}_{{\mathfrak{P}}_l,{\overline{V}}_{i+1}}$ (or vice versa). Without loss of generality, assume that ${\overline{V}}_i^{*}$ lies on the same side that ${\overline{V}}_i$ belongs to and that ${\overline{V}}_i$ is reachable from ${\overline{V}}_i^{*}$. Since ${\overline{V}}_{i+1}$ is not reachable from ${\overline{V}}_i^{*}$, at least one additional node belonging to ${\mathcal{M}}_{{\mathfrak{P}}_l,{\overline{V}}_{i+1}}\cap {\mathcal{M}}_{{\mathfrak{P}}_l,{\overline{V}}_i^{*}}$ must be added to the sequence ${\overline{V}}_0,\cdots {\overline{V}}_{i-1},{\overline{V}}_i^{*},{\overline{V}}_{i+1},\cdots ,{\overline{V}}_{n+1}$. This is by construction vertex ${\overline{V}}_i$. It turns out that the new sequence of nodes ${\overline{V}}_1,\cdots {\overline{V}}_{i-1},{\overline{V}}_i^{*},{\overline{V}}_i,{\overline{V}}_{i+1},\cdots ,{\overline{V}}_{n+1}$ defines a trajectory that is longer than that one identified by ${\overline{V}}_0,\cdots ,{\overline{V}}_{i-1},{\overline{V}}_i,{\overline{V}}_{i+1},\cdots ,{\overline{V}}_{n+1}$. □

The following two straightforward corollaries of Theorem 1 hold.

Corollary 1.

Given an optimal sequence of nodes ${\overline{V}}_0,\cdots ,{\overline{V}}_{n+1}$, with (${\overline{V}}_i\in {\mathcal{I}}_l,i=1,\cdots ,n$), then this sequence must be a set of clockwise or counterclockwise ordered and adjacent vertices of the l-th obstacle’s convex hull.

Corollary 2.

Shortest path connecting two points in the presence of $N_o$ obstacles: given two points A and B, B being not reachable by A, and $N_o$ obstacles, the optimal piecewise linear trajectory connecting $A={\overline{V}}_0$ to $B={\overline{V}}_{n+1}$ is defined by the sequence of $n+2$ nodes ${\overline{V}}_0,\cdots ,{\overline{V}}_{n+1}$, where each intermediate node ${\overline{V}}_i$, with $i=1,\cdots ,n$, belongs to ${\mathcal{T}}_{{\mathfrak{P}}_l,{\overline{V}}_{i-1}}\cap {\mathcal{T}}_{{\mathfrak{P}}_l,{\overline{V}}_{i+1}}$ with obstacle ${\mathfrak{P}}_l\in {\mathcal{O}}_{{\overline{V}}_{i-1}}\cap {\mathcal{O}}_{{\overline{V}}_{i+1}}$.

The Essential Visibility Graph (EVG) $\mathcal{G}=\left\{\mathcal{W},\mathcal{E}\right\}$ can be then computed using a branching algorithm reported in the form of pseudo-code in Algorithm 1.

Algorithm 1 EVG building procedure.

The shortest path connecting two points in the presence of polygonal obstacles can be then calculated as a minimum cost search [69] over the EVG.

Once a piecewise linear optimal flight path has been found, an optimal flyable path, i.e., compatible with the flight mechanics constraints like minimum turn radius, can be computed using Dubins’ circles, as summarized in the following Procedure 1.

Procedure 1.

Find a (sub-)optimal flyable path

• Step 1. Find an optimal piecewise linear path over the EVG described by $n+2$ nodes including A and B, called $\overline{\mathcal{V}}=\left\{{\overline{V}}_0,\cdots ,{\overline{V}}_n+1\right\}$.
• Step 2. For each intermediate node h = 2, ..., n of the optimal sequence, define a circle ${\mathfrak{C}}_h$, centered in the corresponding polygon vertex.
• Step 3. Define Dubins’ circles ${{\mathfrak{C}}_A}_{Right}$ and ${{\mathfrak{C}}_A}_{Left}$, ${{\mathfrak{C}}_B}_{Right}$ and ${{\mathfrak{C}}_B}_{Left}$, for the starting and target points A and B.
• Step 4. Build 2D Dubins trajectories based on four possible sequences of circles, namely:
  • (${{\mathfrak{C}}_A}_{Right},{\mathfrak{C}}_2\cdots ,{\mathfrak{C}}_h,\cdots ,{\mathfrak{C}}_{n+1}{,\mathfrak{C}}_{B_{Left}}$),
  • (${{\mathfrak{C}}_A}_{Left},{\mathfrak{C}}_2\cdots ,{\mathfrak{C}}_h,\cdots ,{\mathfrak{C}}_{n+1}{,\mathfrak{C}}_{B_{Left}}$),
  • (${{\mathfrak{C}}_A}_{Left},{\mathfrak{C}}_2\cdots ,{\mathfrak{C}}_h,\cdots ,{\mathfrak{C}}_{n+1}{,\mathfrak{C}}_{B_{Right}}$),
  • (${{\mathfrak{C}}_A}_{Right},{\mathfrak{C}}_2\cdots ,{\mathfrak{C}}_h,\cdots ,{\mathfrak{C}}_{n+1},{\mathfrak{C}}_{B_{Right}}$),

As a numerical example, Figure 5 shows the optimal flyable path from point $A=$ (−3 km; 3 km) to $B=$ (3 km; −3 km) with initial heading angle ${\psi }_A=\pi /2$ rad, final heading angle ${\psi }_B=-\pi /2$ rad, and turning radius $R_{min}=80$ m, in the presence of $N_o=14$ polygonal obstacles. The EVG is composed of 486 arcs and 76 vertices, whereas the standard visibility graph has 1322 arcs and 100 vertices. As expected, the construction of the weighted EVG provides an improvement with respect to the visibility graph in terms of the number of nodes and arcs, leading to a lower computational burden.

Algorithm 1, combined with Procedure 1, allows computing a starting nominal path, called ${\Gamma }_0$. However, during the flight, if one or more unknown obstacles are detected, the vehicle must change its trajectory to avoid possible collisions. The new flight path can be computed online in an efficient way by updating the nominal EVG to include the newly detected obstacles. The updated EVG can be used to find the shortest path between the current position of the aircraft, becoming also a new additional node of the graph, and the target point.

If the current position of the aircraft, say $P^{*}$, belongs to the nominal path ${\Gamma }_0$ and no new obstacles are detected, the optimal flyable path between the starting point $P^{*}$ and the target point B is a sub-path of ${\Gamma }_0$ according to the optimality principle.

If $N_n$ new obstacles are detected, let be ${\mathcal{I}}_l$ the subset of nodes being vertices of the new l-th obstacle, with $l=N_o+1,\dots ,N_o+N_n$. Algorithm 2 is run to update the nominal EVG with a lower computational cost compared to the cost required to compute the nominal starting EVG.

Algorithm 2 EVG updating procedure.

4. Collision Avoidance with EVG

In a dynamic environment, the UAV must prevent collisions also with moving obstacles and other vehicles called intruders. The proposed collision avoidance algorithm consists of a path re-planning procedure based on the EVG and Dubins’ paths, to avoid UAVs that are on a collision course. The algorithm is computationally efficient as it makes use of the procedures previously described.

When an aircraft predicts multiple future collisions, it selects the nearest one in accordance with a typical reactive scheme [39], and it calls the EVG updating procedure assuming a fictitious additional obstacle centered at the predicted collision position. To avoid ambiguity (i.e., it is possible to overcome the obstacle on the left or on the right), the EVG is cut according to the Right of Way rules prescribed in the Annex II of International Civil Aviation Organization (ICAO).

4.1. Collisions Prediction

In our work, we assume that UAVs know the position, speed, and heading of neighboring aircraft, within a given sensing range given by ADS-B, TCAS, or other sensing devices.

Consider the i-th controlled aircraft and the set ${\mathcal{S}}_i$ of vehicles in the sensor range. Let $P_i(t)=(x_i(t),y_i(t))$ and $P_j(t)=(x_j(t),y_j(t))\in {\mathcal{S}}_i$ be the position of the i-th and j-th UAV, respectively. The line of sight vector between aircraft i and j is defined as $LO{S}_{i,j}=P_j-P_i$ (see Figure 6).

${\theta }_{i,j}^1\in [0,2\pi [$ is the angle between the velocity vector of aircraft i and $LO{S}_{i,j}$, and ${\theta }_{i,j}^2\in [0,2\pi [$ is the angle between the velocity vector of the vehicle located in $P_j$ and $LO{S}_{i,j}$ (see Figure 6).

The ICAO Right of Way rules can be formalized by defining an operator ${\mathsf{\Phi }}_{i,j}$, such that ${\mathsf{\Phi }}_{i,j}=1$, if the j-th vehicles has the right of way, and ${\mathsf{\Phi }}_{i,j}=0$ in other cases:

$$\left\{\begin{array}{cc}{\mathsf{\Phi }}_{i,j}=1\hfill & \forall {\theta }_{i,j}^2\cap \frac{3}{2}\pi \le {\theta }_{i,j}^1<2\pi \hfill \\ {\mathsf{\Phi }}_{i,j}=1\hfill & 0\le {\theta }_{i,j}^2\le \frac{\pi }{2}\cap 0\le {\theta }_{i,j}^1\le \frac{\pi }{2}\hfill \\ {\mathsf{\Phi }}_{i,j}=1\hfill & \pi \le {\theta }_{i,j}^2<2\pi \cap 0\le {\theta }_{i,j}^1\le \frac{\pi }{2}\hfill \\ {\mathsf{\Phi }}_{i,j}=0\hfill & \mathrm{otherwise}.\hfill \end{array}\right.$$

(6)

At any time t, by evaluating the operator ${\mathsf{\Phi }}_{i,j}(t)$, $\forall j\in {\mathcal{S}}_i(t)$, it is possible to determine the list ${\overline{\mathcal{S}}}_i(t)$ of vehicles taking priority in accordance with the ICAO rules:

$${\overline{\mathcal{S}}}_i(t)=\left\{j\in {\mathcal{S}}_i(t):{\mathsf{\Phi }}_{i,j}(t)=1\right\}$$

(7)

In the prediction time horizon $\left[t,t+T_d\right]$, the i-th aircraft control system computes the predicted distance ${\widehat{d}}_{ij}(\tau |t)$, with $\tau \in \left[t,t+T_d\right]$ for each vehicle $j\in {\overline{\mathcal{S}}}_i(t)$, assuming that the intruders fly on a straight path at constant speed $v_j$. If ${\widehat{d}}_{ij}(t)$ is less than the minimum separation distance $d_{min}$, a collision can occur, and vehicle j is added to the set of colliding aircraft, namely ${\mathcal{S}}_i^{*}\subseteq {\overline{\mathcal{S}}}_i\subseteq {\mathcal{S}}_i$. Assuming that $t_{i,j}^{*}$ is the time of collision between i and j, predicted at time t, the i-th aircraft control system selects the nearest collision, i.e., aircraft $j^{*}$ such that $t_{i,j^{*}}^{*}$ is minimum, say $t^{*}$ this time.

4.2. Anti-Collision Flight Path Update

Consider that the i-th aircraft is following its nominal path ${\Gamma }_0$, and at time t, it predicts a collision with aircraft $j^{*}$ at time $t^{*}$. Let ${\widehat{P}}_{j^{*}}(t^{*})$ be the position of aircraft $j^{*}$ at collision time $t^{*}$. Assume a fictitious obstacle of m vertices circumscribed to a circumference with the center at ${\widehat{P}}_{j^{*}}(t^{*})$ and radius $d_{min}$ (see Figure 7). If the obstacle overlaps with other existing obstacles in the scenario, consider the convex hull of the vertices of the overlapped obstacles plus the fictitious obstacle. The nominal EVG is then updated using Algorithm 2.

The Right of Way rules prescribe that “An aircraft that is obliged by the following rules to keep out of the way of another shall avoid passing over, under or in front of the other, unless it passes well clear and takes into account the effect of aircraft wake turbulence” (Section 3.2.2.1 in [70]). For this reason, a cut of the EVG is needed to plan a path compliant with the rules.

Consider a straight line $r_1$ passing through $P_i(t)$ and ${\widehat{P}}_{j^{*}}(t^{*})$:

$$r_1:\beta ·\left(x-x_i(t)\right)-\alpha ·\left(y-y_i(t)\right)=0$$

(8)

where $\alpha$ and $\beta$ are the components of the vector ${\widehat{P}}_{j^{*}}(t^{*})-P_i(t)={[\alpha ,\beta ]}^T=[{\widehat{x}}_{j^{*}}(t^{*})-x_i(t)$, ${\widehat{y}}_{j^{*}}(t^{*})-y_i(t){]}^T$.

Consider a straight line $r_2$ passing through ${\widehat{P}}_{j^{*}}(t^{*})$ and the target point $B=(x_B,y_B)$:

$$r_2:\nu ·\left(x-{\widehat{x}}_{j^{*}}(t^{*})\right)-\mu ·\left(y-{\widehat{y}}_{j^{*}}(t^{*})\right)=0$$

(9)

where $\mu$ and $\nu$ are the components of the vector $B-{\widehat{P}}_{j^{*}}(t^{*})={[\mu ,\nu ]}^T={[x_B-{\widehat{x}}_{j^{*}}(t^{*}),y_B-{\widehat{y}}_{j^{*}}(t^{*})]}^T$.

To follow the Right of Way rules, the optimization algorithm over the EVG can choose only vertices in the semi-plane defined using $r_1$ and $r_2$ and the following conditions:

$$\left\{\begin{array}{c}-{\Sigma }_{i,j^{*}}·\left[\beta ·\left(x-x_i(t)\right)-\alpha ·\left(y-y_i(t)\right)\right]\le 0\hfill \\ -{\Sigma }_{i,j^{*}}·\left[\nu ·\left(x-{\widehat{x}}_{j^{*}}(t^{*})\right)-\mu ·\left(y-{\widehat{y}}_{j^{*}}(t^{*})\right)\right]\le 0\hfill \end{array}\right.$$

(10)

where the operator ${\Sigma }_{i,j}=1$ if the UAV i must turn to the left in accordance with Right Of Way rules, otherwise ${\Sigma }_{i,j}=-1$ if i must turn to the right:

$$\left\{\begin{array}{cc}{\Sigma }_{i,j}=1\hfill & 0\le {\theta }_{ij}^1\le \frac{\pi }{2}{\theta }_{ij}^1<{\theta }_{ij}^2\le \frac{\pi }{2}\hfill \\ {\Sigma }_{i,j}=1\hfill & \frac{3}{2}\pi <{\theta }_{ij}^1<2\pi 0\le {\theta }_{ij}^2\le {\theta }_{ij}^1-\pi \hfill \\ {\Sigma }_{i,j}=1\hfill & \frac{3}{2}\pi <{\theta }_{ij}^1<2\pi {\theta }_{ij}^1\le {\theta }_{ij}^2<2\pi \hfill \\ {\Sigma }_{i,j}=-1\hfill & \mathrm{otherwise}.\hfill \end{array}\right.$$

(11)

Figure 7 shows the nominal path of two UAVs. UAV i predicts a collision with UAV $j^{*}$ at time $t^{*}$ in ${\widehat{P}}_{j^{*}}(t^{*})$. Since UAV i does not have the right of way, it must re-plan its trajectory. Before updating the graph, a fictitious obstacle is added to the environment, centered at the point ${\widehat{P}}_{j^{*}}(t^{*})$ where the collision is expected. In order to force the UAV i to avoid the collision with the UAV $j^{*}$, passing on the right of fictitious obstacle, all arcs of the updated graph in the gray area are considered unfeasible and deleted from he graph.

5. Numerical Results

An extensive campaign of numerical simulations was carried out whose results are presented in this section. Several operational scenarios were set to verify the effectiveness of the proposed technique. The selected scenarios are among the most complex ones to suitably test our anti-collision algorithm. Both the planned paths and the actual trajectories, as modified by the collision avoidance system, are shown. Mutual distances between UAVs over time are also shown for scenarios with few aircraft. For the sake of clarity, by increasing the number of aircraft involved, only the minimum distances reached between UAVs are reported in tabular format. Finally, all the vehicles have the same cruise speed and minimum turn radius.

Table 1. Simulation parameters of Scenarios #1–#6.

$d_{min}$	200 m
Turning Radius $R_{min}$	100 m
Cruise Speed $v_c$	10 m/s
Sample Time $\Delta t$	0.5 s
Prediction Time Horizon $T_d$	20 s

summarizes the simulation parameters used for Scenarios #1–#6.

5.1. Scenario #1: Two Converging UAVs

In this scenario, two UAVs follow perpendicular flight paths intersecting at one point. When UAV 2 detects a possible collision at time $t^{*}$, in order to respect the right of way rules (see 3.2.2.3, in ICAO Annex II), it re-plans its path, giving the way to UAV 1, which does not change its planned trajectory. Figure 8 shows the planned paths (dotted lines) and the actual trajectories followed (solid lines) of both UAVs. As can be seen, UAV 2 passes on the right of the expected collision point, in order to pass behind UAV 1, according to the right of way rules. In Figure 9, the distance between the aircraft over time is reported. As we can see, the collision avoidance system is able to guarantee a mutual distance not below the prescribed minimum value of 200 m.

5.2. Scenario #2: Two Opposite UAVs

In this scenario, two aircraft are approaching from opposite directions following the same path. As prescribed by ICAO Rule 3.2.2.2, in Annex II [70], both UAVs must deviate, re-planning their trajectory in order to turn to the right. Figure 10 shows the vehicles’ planned paths (dotted lines) and the actual trajectories followed (solid lines). As can be seen, both UAVs turn on the right near the expected collision point according to the right of way rules. Figure 11 shows the distance between UAVs over time. As we can see, the collision avoidance system is able to guarantee a mutual distance not below the prescribed minimum value of 200 m.

5.3. Scenario #3: Eight Converging UAVs

In this scenario, eight aircraft are uniformly distributed in the airspace as a circular pattern and follow trajectories all converging towards the center, which is the potential collision point. Figure 12 shows the UAVs’ planned paths (dotted lines) and the actual trajectories followed (solid lines). As can be seen, each aircraft avoids a collision by re-planning its own nominal path making a turn maneuver to the right near the center of the scenario and reaching the destination point on the opposite side. Circular markers indicate the UAVs’ positions at the time the collision is predicted. Note that the aircraft are placed all around a sort of fictitious traffic circle with a radius that depends on the number of vehicles, centered at the point where the collision is expected to occur. As previously said, due to the higher number of aircraft involved in this scenario, only the minimum distances between UAVs are summarized in

Table 2. Scenario #3: Minimum distances between UAVs (m).

	1	2	3	4	5	6	7	8
1	-	389	719	939	1017	939	719	389
2		-	389	719	939	1017	939	719
3			-	389	719	939	1017	939
4				-	389	719	939	1017
5					-	389	719	939
6						-	389	719
7							-	389
8								-

. As we can see, they are always greater than the required minimum value.

5.4. Scenario #4: Twenty Converging UAVs

In this scenario, twenty aircraft are uniformly distributed in the airspace as a circular pattern and follow trajectories all converging towards the center, which is the potential collision point. Figure 13 shows the UAVs’ planned paths (dotted lines) and the actual trajectories followed (solid lines). Despite the even higher number of aircraft, also in this test case, the UAVs are able to avoid collisions. Once a potential collision point is detected, each aircraft re-plans its own path making a turn maneuver to the right compliant with the ICAO rules. Circular markers indicate the UAVs’ positions at the time the collision is predicted. As in the previous scenario, the aircraft avoid a sort of fictitious traffic circle having a radius that depends on the number of vehicles, centered at the point where the collision is expected, before reaching their own destination points on the opposite side. For this test case, given the large number of aircraft, we preferred not to report the entire table with the minimum distances between aircraft, which, anyway, were never below 221 m.

5.5. Scenario #5: Four Converging Pairs of UAVs

In this scenario, eight vehicles grouped into four pairs are involved. All the aircraft follow parallel nominal paths pointing to their own destination points on the opposite side. The distance between parallel paths is 0.3 km. This scenario represents a challenging test case for our collision avoidance algorithm as any turn maneuver of one vehicle can affect the behavior of the others. The planned (dotted lines) and actual trajectories (solid lines) are shown in Figure 14. As can be seen, compliance with the ICAO right of way rules forces all the UAVs to turn right of the colliding aircraft near the center of the airspace. This makes the actual trajectories possibly deviate greatly from the planned ones. Again, as previously done, only the minimum distances between aircraft are resumed in

Table 3. Scenario #5: Minimum distances between UAVs (m).

	1	2	3	4	5	6	7	8
1	-	294	643	526	221	372	372	702
2		-	930	643	658	702	221	658
3			-	294	658	221	702	658
4				-	702	372	372	221
5					-	294	643	930
6						-	526	643
7							-	294
8								-

. In this case as well, the collision avoidance algorithm is able to assure the required minimum value.

5.6. Scenario #6: Sixteen Converging UAVs

In this scenario, to further test our anti-collision algorithm, sixteen aircraft, grouped into four formations of four UAVs each, are involved following parallel paths pointing to their own destination points on the opposite side. The distance between parallel paths is 0.3 km. Planned paths (dotted lines) and actual trajectories (solid lines) are shown in Figure 15. As we can see, with the exception of UAVs on the right side of the flight formations (UAVs 4, 8, 12, and 16), all the others are forced to deviate strongly from their planned trajectories to avoid a collision with other vehicles while complying with the ICAO rules. In this test case as well, given the large number of aircraft, we preferred not to report the entire table with the minimum distances between aircraft, which, anyway, were never below 213 m.

5.7. Scenario #7: Four Converging UAVs in the Presence of Fixed Obstacles

In this scenario, four UAVs are involved following perpendicular flight paths in the presence of four fixed obstacles placed near the center point of the airspace where collision is expected. All the aircraft have to reach their own destination points placed on the opposite side. In this scenario, the desired minimum distance was chosen equal to $d_{min}=$ 4 km and the minimum turn radius $R_{min}=$ 400 m. All nominal paths (dotted lines) converge towards the center and pass through the two narrow perpendicular channels created by the four obstacles. Each vehicle must re-plan its trajectory to avoid a collision by performing a turn maneuver in order to fly out of the area occupied by the obstacles (see Figure 16). Each aircraft, in its prediction phase, considers a fictitious obstacle centered at the predicted collision point that overlaps with the existing real obstacles. Therefore, as stated in Section 4.2, a unique larger obstacle is considered by merging the existing ones. For this reason, the resulting path does not pass in the center of the scenario. The distances over time between UAVs are shown in Figure 17. As we can see, mutual distances never fall below the minimum required value. Due to the symmetry of the scenario, the distances between Aircraft 1–3 and 2–4 are superimposed, the same as the distances between Aircraft 1–2, 1–4, 2–3, and 3–4.

Finally, to update the aircraft trajectories, the proposed algorithm never took more than 0.01 s for the scenarios without obstacles and 0.03 s for the scenario with static obstacles by using an Intel i7 based laptop. This feature suggests that the proposed procedure can be considered a promising approach for real-time applications.

6. Conclusions

This paper describes a novel procedure to generate optimum flight paths in dynamic environments featuring the simultaneous presence of multiple flying vehicles and fixed or moving obstacles. A real-time collision avoidance algorithm was developed based on the Essential Visibility Graph (EVG) solving the optimization problem as a minimum cost piecewise linear path search. It was assumed that all the air vehicles present in the airspace share information about their position, speed, and direction. This way, if a possible collision is detected within a fixed time horizon, a re-planning procedure is called that updates the EVG and changes the aircraft’s current trajectory, avoiding UAVs that are on a collision course. The use of Dubins’ curves allows obtaining smoothed paths compliant with the flight mechanics constraints. In view of possible future applications in hybrid scenarios where both manned and unmanned aircraft share the same airspace, visual flight rules compliant with the ICAO Annex II Right of Way were implemented. To test the effectiveness of the proposed algorithm, different operational scenarios of increasing complexity were defined. All the numerical simulations proved the algorithm’s capability to avoid possible collisions by quickly changing the aircraft’s current trajectories, assuring a minimum safety distance between air vehicles as well. Furthermore, the low computational burden suggests that the proposed procedure can be considered a promising approach for real-time applications. Future research activities will be directed toward the implementation of this new anti-collision algorithm in a guidance and navigation system taking into account possible atmospheric disturbances and model uncertainties. This will assure the identification of real flyable trajectories.