Fig. 5. (a) Trajectory of the leader obtained with VFM. (b) Robot formation (rectangles) with the corresponding partial objectives (circles) and the partial trajectories of the followers. (c), (d) Metrics matrixWt i for each of the follower robots in a particular step using the method proposed in this paper: the other robots are treated as obstacles in the map, then the metrics matrixWti is calculated over this map, and finally the distance matrix Dt i is calculated usingWt i. (e), (f) The distance matrix Dti measured with themetricsWti for each of the follower robots.
3.1.2. Adding springs
When there are highly symmetric situations, i.e., the robots have to traverse a small door and they are initially placed orthogonally to the wall, the two follower robots have problems in passing together through the door. In order to solve this problem, it is necessary to introduce a precedence order. For example, if there is no room for the two robots to pass, the second one has to pass first, and then the third (the first is the leader).
The solution proposed to this problem is to calculate first the complete trajectory for the first follower and then the complete trajectory for the last follower. This provides an effect similar to using springs with different stiffnesses between the robots. This effect can be seen in Figs. 7 and 9. Another possible solution to solve these highly symmetrical situations is to find the VFM joined trajectory for two followers in four-dimensional configuration space resulting from the union of the two followers.
3.1.3. Maximum energy configuration
Another interesting problem refers to what the formation must do if the nearest passage is narrower than the width of the robot formation, where the width of the formation is the orthogonal diameter to the movement direction. The solution is to specify the smallest possible formation size (or
the maximum energy configuration) beforehand and search for another possible passage larger than this size. The solution proposed is to dilate the walls and obstacles with the minimum radius of the robot formation. Then, the trajectory of the leader is found using this dilated map. In this way, it is ensured that the formation passes through the passage used by the leader trajectory. Using this trajectory, the rest of the algorithm is similar to the main method:
1. The partial goal points are calculated using the leader path and the desired formation. The distance from a partial goal to the leader path is proportional to the grey level of the partial goal’s position. 2. The distance matrices D are calculated using the metrics matrix W for each robot, using as goal the partial goal of the previous step.
3. The minimum distance path to the partial goal is found using the potential D. 4. All the robots move a fixed number of steps on the corresponding path. 5. The leader advances its path position a fixed number of steps. 3.2. Reduction of the operational cost
Although the FM method is very fast (the computation time is 0.2 sec for Figs. 6 and 7, where the map has a size of 628 × 420 cells), the proposed algorithm for robot formations has to calculate the FM wave propagation for each follower robot at each cycle of the algorithm. For this reason some techniques should be used to make the algorithm faster.
Since the FM method can be considered as the continuous version of Dijkstra’s algorithm, our goal is to turn it into an almost one-dimensional algorithm. To achieve this, the VFM method is applied in a tube around the trajectory calculated for the leader. Thus, the propagation of the FM wave across the map is calculated only the first time to find the trajectory for the leader; the other times (once per cycle and follower robot) it is calculated in a tube around this trajectory, which drastically reduces the computation time. The steps are:
(1) Enlargement of the trajectory calculated for the leader to get a tube. To achieve this, this trajectory is dilated to get a tube, and the intersection between this tube and the map obtained from the environment (walls and obstacles) is used as the starting matrixWoti(see Fig. 10).
(2) VFM—1st step. Using the map obtained in the previous step, a wave is propagated starting from the points representing the obstacles and walls. This is done with the Extended Voronoi Transform (also called Distance Transform in Computer Vision). The result is a potential map, which can be interpreted as a velocity map (or slowness map), as shown in Fig. 10(c).
(3) VFM—2nd step. Based on the previous slowness map, the FM method creates a new potential Dt I that represents the propagation of the electromagnetic wave from the goal to the robot position. An even more important reduction of the computation time is related to the matrix Dt i, as follows. As the vehicles are very close to the partial objectives, the expansion of the wave over the whole map is not necessary, but just its expansion into the tube, from the partial goal point to the corresponding vehicle. With this change the computation time of the matrix Dti, which is the most time consuming part of the algorithm, goes from 0.2 s to 0.016 s, which implies an algorithm cycle of about 0.5 s without parallelization (using a MacBook Pro platform at 3.06 GHz). The parallelized version of the algorithm has an algorithm cycle of about 0.3 s and permits the use of many followers without increasing the computation time.
Fig. 6. Consecutive steps of the robots’ formation traveling around the maze using the main method.
Fig. 7. Consecutive steps of the robots’ formation traveling around the maze using the method with springs.
Fig. 9. Consecutive steps of the robots’ formation using the maximum energy configuration with springs.
The formation does not use passages narrower than its maximum energy configuration.
The method has been designed for holonomic robots, but it is possible to apply the techniques that we used for non-holonomic robots in [18]. The method has been used giving a sequence of points, joining these points with lines to obtain the leader’s trajectory. Using this trajectory, the followers’ trajectories are calculated in the same way as in the main proposed method. In this case, the direction in the given points changes abruptly. It is important to study the behaviour of the method at these points, and we can observe that it behaves very well and even smoothes the followers’ trajectories in comparison with the leader’s one, as shown in the figures.
The inter-vehicle desired distance has a maximum value in open areas and is proportional to the refraction index in the rest. In this way, in small corridors the followers can be near each other. Fig. 6 shows an example of a team composed by two vehicles following a moving leader using the main proposed method. The lines connecting each of the three vehicles represent the formation links between them. The circles represent the partial objectives that change at each step of the algorithm. The lines from the vehicles to the circles are the partial paths calculated using VFM. Fig. 7 shows a similar case using the proposed method with springs of different stiffnesses. With this improvement the symmetry of the followers’ paths is broken and it is easier to pass trough small passages.
Fig. 8 shows consecutive steps of the robots’ formation traveling around the maze using the main method, with a formation of six robots protecting the centre of the formation. The EVT can be