## 3 The Navigation Strategy with Path Priority
### Step 3: Path Planning  
This subsection desires to find the efficient path for each robot in the space. In the beginning, the initial starting points and target points for all robots are set. Each robot moves from the starting point to the target point along links of the navigation points in the divided map, many feasible paths may be reached. Here, a Dijkstra algorithm [30] is used to calculate the lengths of all possible paths and find the shortest path between the starting point and target point for each robot using the links between navigation points on the map. As shown in Figure 5, if the robot has to pass between two obstacles, by using GVD, the feasible path between the two obstacles will pass through the midpoint of two obstacles with equal distance to both obstacles i.e., O1 = O2, where O1 and O2 are the distances between the path line and the two obstacles, respectively. In Figure 6a, the red line indicates a feasible path for robot-1 to reach the target point using the navigation points generated by the GVD. However, it is obvious that this path is not a short enough path to the target. To shorten the length of the red path, we may reduce the number of navigation points to find another shorter path using the algorithm in Algorithm 1. Note that both the starting and target points are navigation points that the robot can follow. Algorithm 1 defines two parameters, \"scan\" and \"check\", to simplify the path that is generated by Dijkstra algorithm. The \"scan\" begins from the starting point to the previous navigation point from target, but the \"check\" starts from the target point to the next numbers of \"scan\". For each iteration of \"scan\" and \"check\" parameters, we need to detect the obstacle on the connecting line between two navigation points corresponding to the \"scan\" and \"check\" numbers, respectively. If there is no obstacle on the connecting line, then the connecting line is a shortcut. Once a shortcut generates, the \"scan\" number will be changed to the number of \"check\" and the \"check\" will return to the beginning point (target point). Then repeat the above operations until the \"scan\" number reaches the target point. Then the final paths for robot-1, robot-2, and robot-3 are shown as the green paths in Figure 6a-c, respectively.  
Figure 4: (**a**) The map of robot-1, (**b**) the map of robot-2, and (**c**) The map of robot-3.  
```
1:Begin
2:\(\text{BNP}_{1}=\text{NP}_{1},r=2\) # Save the target point
3: For \(scan=1\) to \(n-1\) # \"scan\" from the starting point to the previous navigation point from target
4: For \(check=n\) to \(scan+1\) # \"check\" from the target point to the next number of \"scan\"
5: If (there is no obstacle on the connecting line between \(\text{NP}_{scan}\) and \(\text{NP}_{check}\))
6: BNP\({}_{r}=\text{NP}_{check}\) # save the shortcut point number
7:\(scan=check\), \(r=r+1\) # the next \"scan\" number is from the \"check\" numbers
8: Break # force into next round
9: End If
10: End For
11: End For
12: End
```  
**Algorithm 1**Path planning algorithm.  
Figure 5: All feasible paths generated by the GVD.  
Figure 6: (**a**) Path planning for robot-1 (the red line is the shortest path based on the GVD, and the green line is the result of path planning), (**b**) path planning for robot-2, and (**c**) path planning for robot-3.