# Path Planning and Real-Time Collision Avoidance Based on the Essential Visibility Graph
## 3 Obstacle Avoidance Using EVG
```
1 Data: Starting point \(A\), target point \(B\), obstacles' vertices \(\mathcal{I}_{1},\dots,\mathcal{I}_{N_{o}}\) Result: EVG \(\mathcal{G}=\{\mathcal{W},\mathcal{E}\}\) Add \(A\) and \(B\) to \(\mathcal{W}\); \(n=1\); \(\mathcal{K}_{n}=\{A\}\); while\(\mathcal{K}_{n}\
eq\emptyset\)do
2\(n=n+1\); \(\mathcal{K}_{n}=\mathcal{O}\); foreach\(X\in\mathcal{K}_{n-1}\)do
3 if\(B\) is visible from \(X\)then
4 add \(xc\) (\(X,B\)) to \(\mathcal{E}\);
5
6 else
7 compute the set of obstacle reachable from \(X\), \(\mathcal{O}_{X}\); foreach\(\mathfrak{P}\in\mathcal{O}_{X}\)do
8 compute the set of transition nodes \(\mathcal{T}_{\mathfrak{P},X}\); foreach\(Y\in\mathcal{T}_{\mathfrak{P},X}\)do
9 if\(Y\
otin W\)then
10 add \(Y\) to \(\mathcal{W}\);
11 add \((Y,X)\) to \(\mathcal{E}\);
12 add \(Y\) to \(\mathcal{K}_{n}\);
13
14 end if
15
16 end if
17
18 end for
19
20 end for
```  
**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}}=\{\overline{V}_{0},\dots,\overline{V}_{n}+1\}\)_._
* _Step 2. For each intermediate node_ \(h=2\)_,_ \(\dots\)_,_ \(n\) _of the optimal sequence, define a circle_ \(\mathfrak{C}_{h}\)_, centered in the corresponding polygon vertex._
* _Step 3. Define Dubins' circles_ \(\mathfrak{C}_{ARight}\) _and_ \(\mathfrak{C}_{ALeff}\)_,_ \(\mathfrak{C}_{BRight}\) _and_ \(\mathfrak{C}_{BLft}\)_, for the starting and target points_ \(A\) _and_ \(B\)_._
* _Step 4. Build 2D Dubins trajectories based on four possible sequences of circles, namely:_ 1. \((\mathfrak{C}_{ARight},\mathfrak{C}_{2}\dots,\mathfrak{C}_{h},\dots,\mathfrak{C }_{n+1},\mathfrak{C}_{B_{Left}})\)_,_ 2. \((\mathfrak{C}_{ALeff},\mathfrak{C}_{2}\dots,\mathfrak{C}_{h},\dots,\mathfrak{C }_{n+1},\mathfrak{C}_{B_{Left}})\)_,_ 3. \((\mathfrak{C}_{ALeff},\mathfrak{C}_{2}\dots,\mathfrak{C}_{h},\dots,\mathfrak{C }_{n+1},\mathfrak{C}_{B_{Right}})\)_,_ 4. \((\mathfrak{C}_{ARight},\mathfrak{C}_{2}\dots,\mathfrak{C}_{h},\dots,\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.