## 3 Motion Planning
### Three-Dimensional Path Search Using Modified \(A-\)Star Approach
The lower values of safe margins corresponding to lower boundaries can be calculated as follows:  
\[\left[S_{min}\right]=\left[\eta_{x}\quad\eta_{y}\quad\eta_{z}\right]_{min}- \left[\delta_{x}\quad\delta_{y}\quad\delta_{z}\right]-\left[\Gamma_{x}\quad \Gamma_{y}\quad\Gamma_{z}\right] \tag{40}\]  
Similarly, upper safe margins are  
\[h=sqrt\left[\left(v_{x(n)}-v_{x(g)}\right)^{2}+\left(v_{y(n)}-v_{y(g)}\right)^ {2}+\left(v_{z(n)}-v_{z(g)}\right)^{2}\right] \tag{34}\]0  
where \(\left[\delta_{x}\quad\delta_{y}\quad\delta_{z}\right]\) denotes safety margins, which must be kept between the UAV and the obstacles during flight, while \(\left[\Gamma_{x}\quad\Gamma_{y}\quad\Gamma_{z}\right]\) is UAV size. The boundary of the three-dimensional environment can be defined as \(\left[B_{xyz}\right]=\left[B_{x}\quad B_{y}\quad B_{z}\right]\). The lower and upper boundaries of the three-dimensional environment can be written as \(\left[B_{xyz}\right]_{min}=\left[B_{x}\quad B_{y}\quad B_{z}\right]_{min}\) and \(\left[B_{xyz}\right]_{max}=\left[B_{x}\quad B_{y}\quad B_{z}\right]_{max}\), respectively. Lemma 1: _If \(\left[C_{x}(n)\ C_{y}(n)\ C_{z}(n)\right]\) is greater than or equal to \(\left[S_{min}\right]\) and \(\left[C_{x}(n)\ C_{y}(n)\ C_{z}(n)\right]\) is less than or equal to \(\left[S_{max}\right]\), the collision will occur. \(\left[C_{x}(n)\quad C_{y}(n)\quad C_{z}(n)\right]\) represents the coordinates of current vertex._  
Lemma 2: _If the coordinates of current vertex \(\left[C_{x}(n)\quad C_{y}(n)\quad C_{z}(n)\right]\) are less than the lower boundaries of the three-dimensional environment coordinates \(\left[B_{xyz}\right]_{min}\), the collision will occur._  
Lemma 2 specifically defines the limits of the environment. A UAV is considered in collision if it is outside the lower limits of the environment. Therefore, a mission is considered successful only when a UAV stays inside its defined environment. Lemma 3: _If current vertex \(\left[C_{x}(n)\quad C_{y}(n)\quad C_{z}(n)\right]\) is greater than the upper boundaries of the environment \(\left[B_{xyz}\right]_{max}\), the collision will occur._  
With the exceptions of Lemmas 1-3, the collision will not take place. Therefore, it is necessary for any safe flight of quadrotor UAV in a three-dimensional obstacle-cluttered environment to not lie among the abovementioned lemmas. Based on these lemmas, a collision detection algorithm is presented in Algorithm 2. ```
1:input (3D_Environment, current vertex)
2:E = 3D Environment;
3:C(n) = current vertex;
4:E_min = min(E.obstacles); %lower boundary of the obstacles
5:E_max = max(E.obstacles); %upper boundary of the obstacles
6:minEo = E_min - margins - UAV_size;
7:maxEo = E_max + margins + UAV_size;
8:minEb = lower boundary of 3D Environment;
9:maxEb = upper boundary of 3D Environment;
10:if (C\({}_{x}\)(n), C\({}_{y}\)(n), C\({}_{z}\)(n) \(\geq\) minEo && C\({}_{x}\)(n), C\({}_{y}\)(n), C\({}_{z}\)(n) \(\leq\) maxEo)
11:return collision;
12:elseif (C\({}_{x}\)(n), C\({}_{y}\)(n), C\({}_{z}\)(n) \(\leq\) minEb)
13:return collision;
14:elseif (C\({}_{x}\)(n), C\({}_{y}\)(n), C\({}_{z}\)(n) \(>\) maxEb)
15:return collision;
16:else
17:return collision_free;
18:endif
```  
**Algorithm 2** Collision detection algorithm. The rule of selecting the nodes, which constitutes the shortest path from the nodes explored by the heuristic search algorithm, follows the equation:  
\[h=sqrt\left[\left(v_{x(n)}-v_{x(g)}\right)^{2}+\left(v_{y(n)}-v_{y(g)}\right)^ {2}+\left(v_{z(n)}-v_{z(g)}\right)^{2}\right] \tag{34}\]1  
where \(dP\) denotes distance between the consecutive nodes, \(p(k)\) is the current node and \(p(k-1)\) is the previous node. **Remark 2**. : _If \(dP\) follows either of the three lemmas mentioned previously, the node \(p(k-1)\) lies on the shortest path and hence will be used to generate the shortest trajectory._  
**Remark 3**.