## 3 Motion Planning
### Three-Dimensional Path Search Using Modified \(A-\)Star Approach  
**Definition 1. **_A heuristic refers to an estimate of the cost with the least value from any arbitrary current vertex to the goal vertex._  
Mathematically, a simplified version of the heuristic function can be written as follows:  
\[f(n)=g(n)+h(n) \tag{33}\]  
where \(g(n)\) is cost of the path from starting point to current node \(n\), and \(h(n)\) is the estimate of the cost from current node to goal node. **Remark 1. **_In case, if \(h(n)=0\), then only \(g(n)\) plays a role in the algorithm, and \(A^{*}\) is rendered to the Dijkstra algorithm. On the other hand, if the value of \(h(n)\) is substantially large, the \(A^{*}\) will behave like the greedy best-first search algorithm. The value of \(h(n)\) plays a substantial role in expanding the nodes. If its value is relatively smaller, the algorithm will expand more nodes, which will make the path search slower. Contrarily, if its value is exactly equal to the cost of reaching from current vertex to goal, it finds the best and shortest possible path without expanding extra nodes. This ideality is difficult to achieve in three-dimensional (3D) cases, yet this algorithm is used in most applications due to its substantial fast path search ability._  
In the abovementioned perspective, a heuristic is a method of providing the algorithm with some extra evaluative statistics in order to find a better path without thoroughly searching every possible vertex. The Dijkstra algorithm does not incorporate any heuristic function; it grows outwards from the start node and scans every vertex in the map to discover the shortest path. This is why it can be computationally expensive. Therefore, specifically for a 3D search problem, an admissible heuristic is central. In this work, the adopted heuristic relies on Euclidean distance estimation. **Definition 2. **_A Euclidean heuristic gives a direct distance between current and goal points. For a three-dimensional case, it can be written as:_  
\[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}\]  
Another formalism known as the Manhattan heuristic gives summation of absolute differences in current and goal coordinates. For three-dimensional cases, we can write this mathematically as:  
\[h=abs\Big{(}v_{x(n)}-v_{x(g)}\Big{)}+abs\Big{(}v_{y(n)}-v_{y(g)}\Big{)}+abs \Big{(}v_{z(n)}-v_{z(g)}\Big{)} \tag{35}\]  
where \(v_{i(n)}\) and \(v_{i(g)}\) denote current and goal nodes, respectively. Figure 2: View of expanded voxels (\(6-\)sided search vs. \(18-\)sided search). **Definition 3**. : _A heuristic function is known as monotone or consistent if its estimate for any current node is equal to or less than the estimated cost of reaching the goal from any neighbor of the current node plus the cost consumed in reaching that neighboring vertex from current node._  
Mathematically, for any current node \(n\) and neighboring node \(i\), we can write a consistent heuristic function as follows:  
\[h(n)\leq\text{cost}(n,i)+h(i) \tag{36}\]  
\[h(g)=0 \tag{37}\]  
where \(\text{cost}(n,i)\) is the step cost of reaching vertex \(i\) from \(n\). It is important to note that a heuristic function must essentially be admissible, i.e., it should never overestimate the cost of reaching the goal point. A consistent or monotonic function is always admissible. However, an admissible heuristic is not always consistent and therefore, it is made a consistent heuristic using the path-max equation as follows:  
\[h^{\prime}(i)=\text{max}(h(i),\ h(n)-\text{cost}(n,i)) \tag{38}\]  
An A* algorithm for three-dimensional path search is given in Algorithm 1. For 3D cases, it expands more nodes than the required ones and this ultimately increases the path length. Therefore, we have proposed another algorithm to cope with this problem in order to generate an accurate and shortest possible path.