# Obstacle Avoidance in a Three-Dimensional Dynamic Environment Based on Fuzzy Dynamic Windows
## 3 Proposed Dynamic Window Approach
### The Evaluation Function for DF_DWA
#### 3.3.4 Distance-to-the-Obstacle Term  
This term prizes that the agent travels far from the obstacles. The distance to obstacles includes the distance to static obstacles and the predicted distance to moving obstacles. If the value of this term is small, the agent will be close to obstacles. In contrast, if the value of this term is big, the agent will be far from obstacles.  
\[dist_{obstacle}(i)=norm(x(t)-x_{obstacle}(t),y(t)-y_{obstacle}(t),z(t)-z_{obstacle }(t))-r_{0} \tag{25}\]  
\[dist_{obstacle}=dist_{static}+dist_{dynamic}-r_{0} \tag{26}\]  
Figure 6: Angle \(\theta_{error}\) and \(\phi_{error}\) to the goal.  
where (\(x_{obstacle}\), \(y_{obstacle}\), \(z_{obstacle}\)) are the coordinates of obstacles. Distance to obstacles includes distance to static obstacles (\(dist_{static}\)) and distance to dynamic obstacles (\(dist_{dynamic}\)).  
For the dynamic obstacle, the position and velocity direction of the dynamic obstacle are measured by the sensor in real time, and then the position that the dynamic obstacle can reach in each time interval (\(\Delta\)t) in the next prediction time is calculated as the predicted position of the dynamic obstacle. Then, among these positions, the shortest distance to the agent is used as the dynamic obstacle distance (\(dist_{dynamic}\)), as shown in Figure 7. By considering this distance, the agent plans the next motion command. In actual operation, the polynomial fitting algorithm [19] and extended Kalman filtering method [33] can be used to predict the trajectory of obstacles. In addition, choosing the shortest distance as the distance of the dynamic obstacles increases the threat posed by the dynamic obstacle to the agent. Therefore, fuzzy rules can increase the weight of distance to the obstacle item and the agent can avoid obstacle as quickly as possible.  
\[dist_{dynamic}=\min\Big{(}norm\Big{(}x(t)-x_{prob}(t_{i}),y(t)-y_{prob}(t_{i}),z(t)-z_{prob}(t_{i})\Big{)}\Big{)} \tag{27}\]  
where (\(x_{prob}\), \(y_{prob}\), \(z_{prob}\)) are the predicted position of the dynamic obstacle.  
\[dist_{obstacle}(i)=\frac{dist_{obstacle}(i)}{\sum_{i=1}^{n}dist_{obstacle}(i)} \tag{28}\]  
The reward value of the iteration is calculated using Equation (29). Among all the speed groups being evaluated, the largest speed pair among the evaluation function values is selected as the motion instruction.  
\[\begin{array}{ll}f(v_{t},\omega_{t})&=\varepsilon_{spcat}\bullet speed(v_{t}, \omega_{t})+\varepsilon_{goal}\bullet distgoal(v_{t},\omega_{t})\\ &+\varepsilon_{pitchreading}\bullet heading_{pitch}(v_{t},\omega_{t})+ \varepsilon_{synchroning}\bullet heading_{syww}(v_{t},\omega_{t})\\ &+\varepsilon_{obstacle}\bullet dist_{obstacle}(i)\end{array} \tag{29}\]  
where \(\varepsilon_{spcat}\) is the reward value of speed, \(\varepsilon_{goal}\) is the reward value of the distance from the candidate points to the goal. \(\varepsilon_{obstacle}\) is the reward value of the distance from the agent to the obstacles, and (\(\varepsilon_{pitchreading}\), \(\varepsilon_{synchroning}\)) is the heading reward value.