# Path Planning and Real-Time Collision Avoidance Based on the Essential Visibility Graph
## 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._  
Figure 1: 2D Dubins paths from the point \(A=(-1\) km; \(-1\) km) to \(B=(1\) km; \(1\) km) with initial heading angle \(\psi_{A}=-2\pi/3\) rad, final heading angle \(\psi_{B}=-\pi/3\) rad, and turning radius \(R_{min}=250\) m.  
**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,\ldots,N_{o}\)) and build a graph whose node set is \(\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\Omega:V_{i}\to V_{j}\right\} \tag{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,\ldots,N_{o}\right\} :\mathcal{M}_{\mathfrak{P}_{l},V_{i}}\
eq\left\{0\right\}\right\} \tag{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\{\operatorname*{argmin}_{V_{j}\in \mathcal{I}_{l}}\operatorname*{GE}_{l}\left(r_{V_{j},V_{i}}\right);\ \operatorname*{argmax}_{V_{j}\in \mathcal{I}_{l}}\operatorname*{GE}_{l}\left(r_{V_{j},V_{i}}\right)\ \right\}\cap\mathcal{M}_{\mathfrak{P}_{l},V_{i}} \tag{3}\]  
\(\operatorname*{GE}_{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 \(\Lambda(\mathfrak{P}_{I})\)\(<\Lambda(\mathfrak{P}_{E})\), \(\Lambda(\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,\ldots,m\)) and \(Y_{i}\) (with \(j=1,2,\ldots,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},\ldots,\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}\).