# Application of Angle Related Cost Function Optimization for Dynamic Path Planning Algorithm
## 4 The Spatial Optimized Dynamic Path Planning Algorithm
### Restrict Search Direction  
#### 4.2.1 Analysis of the A* Algorithm Evaluation Method  
The evaluation function of the A* algorithm (\(f(i)\)) is defined as  
\[f(i)=g(i)+h(i), \tag{1}\]  
where \(g(i)\) is the actual cost of searching from the source to the current node (\(i\)), while \(h(i)\) is the estimated cost from the current node (\(i\)) to the destination. As a heuristic search based on empirical information judgment, the more heuristic information included in the valuation function (\(h(i)\)), the smaller the search space is, and the more efficient the search algorithm is. On the one hand, the more information contained in the evaluation function, the more nodes that can be excluded and the smaller the search scale of the algorithm is. On the other hand, an increase in heuristic information will increase the amount of computation required and reduce the time efficiency of the algorithm. Thus, the design of the evaluation function is of critical importance [22]. In the A* algorithm, the evaluation function is designed to estimate the distance between the current node and the target node, as shown in Figure 4.  
If the current node is \(V_{i}\), when picking next node, the evaluation function needs to be performed for nodes \(V_{j0}\) and \(V_{j1}\). If it is assumed that the Euclidean distance is used as a measurement for distance, then the evaluation functions for those two nodes are calculated with  
\[f(V_{j0})=g(V_{j0})+h(V_{j0})=(3+4)+(2+11)=20 \tag{2}\]  
\[f(V_{j1})=g(V_{j1})+h(V_{j1})=(3+8)+11=22. \tag{3}\]  
In this case, the algorithm would pick \(V_{j0}\) as next node, thus \(V_{j1}\) and all its successors would be excluded from searching list, thus restricting the search range. However, this method only takes into account the distance relationships between nodes, and does not take into account the directional relationships between nodes. According to actual life experience, if the chosen path is closer to the direction of the ray formed by the starting point and the end point, the smaller the angle is, the shorter the distance of the final path is. We can prove this. As shown in Figure 5, the current node is \(V_{i}\). Suppose the angle between the rays from \(V_{i}\) to \(V_{j0}\) and the rays from \(V_{i}\) to \(V_{end}\) is \(\beta\), and the angle between the rays from \(V_{i}\) to \(V_{j1}\) and the rays from \(V_{i}\) to \(V_{end}\) is \(\alpha\). The distance from \(V_{i}\) to \(V_{j0}\) is \(g_{0}\), while the distance from \(V_{i}\) to \(V_{j1}\) is \(g_{1}\). The estimated distance from \(V_{i}\) to \(V_{end}\) is \(p\). Figure 4: Illustration of the A* evaluation function. Figure 5: The influence of the search direction on the evaluation function. According to the cosine theorem, for the following triangles (shown in Figure 6), there are  
\[b^{2}=a^{2}+c^{2}-2 ac\times\cos(B). \tag{4}\]  
Thus, we have  
\[F(\beta)=h_{0}^{2}=g_{0}^{2}+p^{2}-2g_{0}p\times\ \cos(\beta). \tag{5}\]  
The derivation of function \(F(\beta)\) is  
\[F^{\prime}(\beta)=2g_{0}p\times\ \sin(\beta). \tag{6}\]  
Since the value of function \(\sin(\beta)\) is \([0,1]\) in the \([0,\pi]\) interval, the value of derivative \(F^{\prime}(\beta)\) in the \([0,\pi]\) interval is also greater than or equal to 0. That is,  
\[F^{\prime}(\beta)=2g_{0}p\times\ \sin(\beta)\geq 0. \tag{7}\]  
From the nature of the derivative of the function, we can conclude that function \(F^{\prime}(\beta)\) in the interval \([0,\Pi]\) monotonically increases, and we can prove that when there is a common edge (p) and \(\alpha\leq\beta\), there must be a third side (\(h_{1}\leq\ h_{0}\)) which is proved as follows:  
Figure 4.0  
It can be concluded that when the cost is equal for \(g_{0}\) and \(g_{1}\), the search can find a shorter path by choosing the direction closer to the current node and the end line.