## 3 Motion Planning
### Three-Dimensional Path Search Using Modified \(A-\)Star Approach
In Algorithm 1, fewer than six vertices have been visited in the neighborhood of the current vertex in determining the next vertex on the route. This strategy allows us to restrain the search algorithm from entering into a long computational mode, as there is a possibility of visiting diagonal vertices in the neighborhood of current vertex. Diagonal search will enable us to find a path that is closer to the exact solution, i.e., the shortest possible path, at the cost of higher computation. However, paying a higher computational cost does not guarantee an exact solution to our problem. Contrarily, nondiagonal searching affects the searched path substantially, but a tradeoff has been determined between computational cost and extra nodes. The computed path generated by the search algorithm is not fully optimal and not even the shortest one. This can be seen in Figure 3, where path ABCD searched by the augmented A* is not the shortest; path ACD is shorter compared to path ABCD. After further iterations, we see that we can construct another route (the path AD) which is the shortest one. The feasibility of all these iterated paths has been checked via a collision detection algorithm (Algorithm 2) presented subsequently. Performing so few iterations can provide an even shorter path, and we can skip some of the extra path nodes. For this purpose, we have developed a truncation algorithm (Algorithm 3). Refined path nodes can then be connected using any interpolation method. Any order of the polynomial can be used for the purpose just conferred depending on the dynamical constraints of the quadrotor [44]. We, in this work, have used differential flatness theory to build the smooth trajectory. ```
1:Function P(n) = Astar3D(3D_Environment, start, goal)
2:list_open = nodes to be evaluated; %initially only starting node is present
3:list_closed = nodes which have been examined; %initially empty
4:list_open (start_node) = 0;
5:while (list_open =! empty) do
6: search the node with least f in list_open % the value of h(n) is same only for 6 neighbors, here one can plan the strategy for 18-sided search as well. 7: n = node with least value of f;
8: remove n from the list_open
9: search the adj_cells of n %adj_cells infer adjacent cells
10:for (each adj_cell) % for three-dimensional case 6 adjacent cells have been visited in this work, however these can be up to 18 adjacent cells including the cells at the edges of a voxel holding the current vertex. 11:if (adj_cell = goal) then
12: stop the search
13:adj_cell_h = n.g + dis_adj_cell;
14: adj_cell_h = distance between adj_cell and goal;
15: adj_cell_f = Adj_cell_g + Adj_cell_h;
16:elseif (node with same position as of adj_cell in list_open)
17: if (node.f < adj_cell_f)
18: skip adj_cell
19:endif
20:elseif (node with same position as of adj_cell in list_closed)
21: if (node.f < adj_cell.f)
22: skip adj_cell
23:else
24: add node to list_open
25:endif
26:endforloop
27: add \(n\) to list_closed
28:endwhile
29:P(n)= list_closed
30:return P(n)
31:endfunction
```  
**Algorithm 1** A* algorithm for three-dimensional path search (with 6-sided search). Obstacles cluttered in a three-dimensional environment can be defined as follows:  
\[\left[\eta_{xyz}\right]=\left[\eta_{x}\quad\eta_{y}\quad\eta_{z}\right] \tag{39}\]  
Figure 3: Various possible paths. where \(\left[\eta_{xyz}\right]_{min}=\left[\eta_{x}\quad\eta_{y}\quad\eta_{z}\right]_{min}\) and \(\left[\eta_{xyz}\right]_{max}=\left[\eta_{x}\quad\eta_{y}\quad\eta_{z}\right]_{ max}\) represent lower and upper boundaries of obstacles in a three-dimensional environment, respectively.