# 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.2 Search Direction Restricted Evaluation Method  
According to previous analyses, in this paper, both the distance and search direction were used as heuristic information to design the valuation function as follows:  
\[h(i)=h_{r}(i)\times h_{d}(i), \tag{9}\]  
where \(h_{r}(i)\) evaluates the influence of search direction, while \(h_{d}(i)\) evaluates the influence of distance. Here, distance is measured as the Euclidean distance. If the coordinate of current node is \((x,y)\), and the coordinate of the destination is \((x_{end},y_{end})\), then \(h_{d}(i)\) is calculated as  
Figure 6: Cosine theorem.  
\[h_{d}(i)=\sqrt{(x-x_{end})^{2}+(y-y_{end})^{2}}.\] (10) In order to design \(h_{r}(i)\), two conditions need to be satisfied: (1) \(h_{r}(i)\) needs to be a function increasing monotonously with the angle. According to the previous discussion, if we choose a search direction with a smaller angle to the current node and the end point (set the angle to \(\alpha\)), we can find the shortest path as soon as possible. In addition, the smaller the angle (\(\alpha\)), the smaller the search area is. Therefore, it is necessary to design the \(h_{r}(i)\) function as a function that increases monotonously with \(\alpha\). Thus, the less \(\alpha\) is, less the \(h_{r}(i)\) is, and the less \(h\) is. The algorithm can select a smaller search area with a greater opportunity to limit the direction of search; (2) The value of the evaluation function for the current node must be less than or equal to the actual distance from the current node to the destination which can be expressed as \(h_{r}(i)\times h_{d}(i)\leq h_{d}(i)\). That is, the value of \(h_{r}(i)\) must be less than or equal to 1, and since the estimated distance cannot be negative, the range of value of \(h_{r}(i)\) must fall within the interval \([0,1]\). If we set the value of the angle in \([0,\pi]\) interval, we can choose the monotone increasing trigonometric function in that interval: \[f(\alpha)=-\cos(\alpha). \tag{11}\]  
This function satisfies the first condition of monotone increment on \([0,\pi]\), and its value range is \([-1,1]\), which is shown in Figure 7.  
However, this function does not satisfy the second condition. So, we use min-max normalization method to map \(f(\alpha)\) onto the interval of \([0,1]\):  
\[\begin{array}{rl}g(\alpha)=&\frac{f(\alpha)-min(f(\alpha))}{max(f(\alpha))- min(f(\alpha))}\\ &\times[max(g(\alpha)-min(g(\alpha))+min(g(\alpha)]\\ =&\frac{-\cos(\alpha)-\cos(0)}{\cos(\pi)-\cos(0)}\times 1+0\\ =&\frac{1-\cos(\alpha)}{2}\end{array}. \tag{12}\]  
This function is shown in Figure 8. It can be seen from Figure 8 that the \(g(\alpha)\) function does not only satisfy the monotone increments in the \([0,\pi]\) interval, but also satisfies the requirement that the value in this interval be located in \([0,1]\) interval. Therefore, this function can be used as \(h_{r}(i)\).  
Thus, the evaluation function for our algorithm is  
\[h(i)=\frac{1-\cos(\alpha)}{2}\sqrt{(x-x_{end})^{2}+(y-y_{end})^{2}}. \tag{13}\]