# RRT*-Fuzzy Dynamic Window Approach (RRT*-FDWA) for Collision-Free Path Planning
## 4 Dynamic Path Planning Algorithm Based on RRT*-FDWA
### RRT*-FDWA Local Minima and Pseudocode  
In the DWA algorithm, the overall generation value increases as the value of its weight coefficient increases the closer it gets to the obstacle. This will cause the robot to fall into local minima during local path planning. In the RRT*-FDWA algorithm, to avoid local minima, the robot can quickly enter global path planning once it has successfully avoided an obstacle, as shown in Equation (13). K is the reward and punishment function of the DWA algorithm. \[G^{\prime}(\
u,\omega)=K\cdot G(\
u,\omega) \tag{13}\]  
\[\Delta t(\
u,\omega)=\Delta S/\Delta V \tag{14}\]  
\[K= \begin{cases}a&t<\ 0.5\\ 1&t=0.5\\ b&0.5<t<1\\ 0&t>\ 1\end{cases}  \tag{15}\]  
In Equation (14), \(\Delta t(\
u,\omega)\) is the time function with the obstacle. In Equation (15), \(a\) and \(b\) represent the reward and punishment function \(K\), where \(a<1<b\). When the value of \(t\) is less than 0.5, it is not caught in the local minimum; when \(t\) is greater than 0.5, it is regarded that the robot is caught in the local minimum. Global path planning uses the RRT* [25] algorithm, while local path planning uses the FDWA algorithm. The pseudo code of the RRT*-FDWA algorithm is shown in Algorithm 1; Algorithm 2 is the _Rewire_ in Algorithm 1. Figure 5: Path planning diagram. ```
input:\(p_{init}\), \(p_{goal}\), Map, \(v\), \(w\), \(K\) output:\(p_{init}\)-\(p_{goal}\) path  WHILE Target not reach DO  FOR i in range(\(n\)): \(p_{rand}\gets Sample(Map\) \(p_{nearest}\gets Node_{list}(p_{rand},p_{ini})\) \(p_{new}\gets Steer(p_{nearest},p_{rand})\) \(Cost(x_{new})\leftarrow\)-\(d(p_{nearest},p_{new})+Cost(p_{nearest})\) IF (\(Check\_collision(p_{nearest},p_{new})\)) THEN \(p_{near}\gets findnear_{nodes(p_{new})}\) \(p_{new}\leftarrow\) C_parent(\(p_{near},p_{new}\)) \(Node\_list\gets append(p_{new})\) \(p_{parent}\gets Revenue(p_{new},p_{near})\) IF (\(Check\_ Moving obstacle(p_{nearest},p_{new})\)) THEN \(DWA(p_{near},p_{new})\gets G^{\prime}(v,w),K\) \(G^{\prime}(v,w),K\gets Rule(\alpha,\beta)\) \(\alpha,\beta\gets clarification\) IF (\(Check\_collision(p_{nearest},p_{new})\)) THEN  end IF \(p_{new}\to p_{goal}\) THEN \(IF(Check\_collision(p_{new},p_{goal}))\) THEN \(Temppath\gets currrent\Big{(}p_{ini}\to p_{goal}\Big{)}\) Return path End while
```  
**Algorithm 1** RRT\({}^{*}\)-FDWA Algorithm. In Algorithms 1 and 2 in this paper, the following terms are used:  
_Sample (Map)_: The number of sampling points generated randomly on the map towards the target node is in the range [0, 100]. _Node_list \((p_{1},p_{2})\)_: The set of Euclidean distances between the sampling point and the parent node, where the coordinates of the random sampling point and the parent node are \(p_{s}\) (\(x_{1},y_{2}\)) and \(p_{p}\) (\(x_{2},y_{2}\)), respectively. \(p_{nearest}\): Minimum of \(Node\_list\) (\(p_{1},p_{2}\)) min (\(Node\_list\))  
_Steer \((p_{1},p_{2})\)_: The angle between \(p_{1}\) and the line connecting \(p_{1}\) and \(p_{2}\) of its parent node; multiply the steering angle by the step size \(\beta\) to get \(p_{new}\). Let the steering angle be \(\theta\); \(p_{new}\) is obtained by the following Equations (16) and (17). \[p_{new,x}=\beta\cos\theta \tag{16}\]  
\[p_{new,y}=\beta\sin\theta \tag{17}\]  
_Check_collision \((p_{1},p_{2})\)_: Check if there are obstacles between \(p_{1}\) and \(p_{2}\). _Target_not_reach_: If or not the target point is reached: the return value is 1 if the target point is reached and 0 if the opposite is true. _Cost(x)_: Return along the total path length of the target node from node \(p\). _find near_nodes_ (\(p_{1}\)): Find a new parent node near node \(p\) again at random. _C_parent_ (\(p_{1}\), \(p_{2}\)): Add a new optional parent node and define its length, angle, and generation value. \(D\) (\(p_{1}\), \(p_{2}\)): Denotes the Euclidean distance between nodes \(p_{1}\) and \(p_{2}\). _Rewire_ (\(p_{1}\), \(p_{2}\)): Determine whether \(p_{2}\) can replace \(p_{1}\) as the new parent node.