# UWB-Based Human-Following System with Obstacle and Crevasse Avoidance for Polar-Exploration Robots
## 5 Local Path Planning for Collision Avoidance  
In this section, we generate a path that allows for movement without collision using the local obstacle map created in the previous section while achieving control objectives. Path planning involves creating a sub-goal within the obstacle-detection range, and then generating a path (hereafter referred to as the pre-path) from the center of the local obstacle map (the robot's position) to the sub-goal using the wave-front algorithm [23]. Subsequently, a straight line is drawn from the center of the local obstacle map to the farthest point on the pre-path that can be reached without passing through an obstacle. This straight line is selected as the final local path. Figure 8 illustrates the process of generating a collision-avoidance path. As shown in Figure 8, the point where the line from the robot to the goal (dash-dot line) intersects with the outer boundary of the local obstacle map is selected as the sub-goal for creating the local path. Specifically, even if there is an obstacle at the sub-goal location during sub-goal generation, it is possible to generate the sub-goal due to the creation of a non-obstacle area based on the previous assumptions, as illustrated in Figure 7. This sub-goal is marked on the grid map, and starting from this pixel coordinate, the wave-front algorithm is used to propagate and increase the cost until it reaches the center of the map. This process is depicted in Algorithm 2. Using the wave-front cost map generated by Algorithm 2, a path from the center of the map, where the robot is located, to the sub-goal can be created. By incorporating the grid coordinates in the direction of the sub-goal, with decreasing cost from the center of the map, a path represented by a set of grid coordinates connected to the sub-goal (hereafter referred to as the pre-path, dotted line in Figure 8) can be generated. After generating the pre-path, starting from the center of the map, which is the initial point of the pre-path, move along the pre-path one cell at a time. Draw a line segment connecting each point to the center of the map. This process continues until the point just before the line segment intersects an obstacle indicated on the map. The straight path to this point is then determined as the final path (line in Figure 8). This process is outlined in Algorithm 3. Figure 7: Comparison of local obstacle maps (resolution: 0.1 m, 12 m \(\times\) 12 m): (**a**) Previous obstacle map; (**b**) Proposed obstacle map. ```
1. Achieve sub-goal
2. Achieve obstacle_map M\({}_{\text{O}}\)
3. Set initial cost
4. Set empty wave-front cost map W\({}_{\text{C}}\)
5. Set Q to the empty queue
6. Add sub-goal (\(x_{g},y_{g}\)) to Q
7. Set initial value to W\({}_{\text{C}}\) (\(x_{g},y_{g}\))
8. While Q is not empty:
9. \(\text{p}\) (\(x_{p},y_{p}\)) = pop Q
10. If p (\(x_{p},y_{p}\)) is center of M\({}_{\text{O}}\):
11. break
12. If W\({}_{\text{C}}\) (\(x_{p}-1,y_{p}\)) is zero:
13. If there is no obstacle at M\({}_{\text{O}}\) (\(x_{p}-1,y_{p}\))
14. Add (\(x_{p}-1,y_{p}\)) to Q
15. W\({}_{\text{C}}\) (\(x_{p}-1,y_{p}\)) = W\({}_{\text{C}}\) (\(x_{p}-1,y_{p}\)) + 1
16. Repeat lines 12-15 while changing (\(x_{p}+1,y_{p}\)), (\(x_{p},y_{p}-1\)), (\(x_{p},y_{p}+1\))
```  
**Algorithm 2** Wave-front cost-propagation algorithm  
```
# pre-path generation
1. Achieve wave-front cost map W\({}_{\text{C}}\)
2. Set pre-path P
3. Add center position \(p_{c}\) (\(x_{c},y_{c}\)) to P\({}_{\text{P}}\)
4. Set \(p_{min}\) (\(x_{min},y_{min}\)) as \(p_{c}\) (\(x_{c},y_{c}\))
5. Set min_value as W\({}_{\text{C}}\) (\(x_{c},y_{c}\))
6. While \(p_{min}\) is not sub-goal:
7. Find minimum value in 8 positions (\(x_{min}-1\) to \(x_{min}+1\), \(y_{min}-1\) to \(y_{min}+1\)), except \(p_{min}\) and \(p_{min}\) (\(x_{min},y_{min}\)) = arg_min W\({}_{\text{C}}\) (\(x,y\))
8. Add \(p_{min}\) to P\({}_{\text{P}}\)
9. Final straight local path generation
10. Set final path P\({}_{\text{F}}\)
11. Set start_point to P\({}_{\text{P}}\)[0], center of W\({}_{\text{C}}\)
12.