# Sequential path planning for a formation of mobile robots with split and merge

###### Abstract

An algorithm for robot formation path planning is presented in this paper. Given a map of the working environment, the algorithm finds a path for a formation taking into account possible split of the formation and its consecutive merge. The key part of the solution works on a graph and sequentially employs an extended version of Dijkstra’s graph-based algorithm for multiple robots. It is thus deterministic, complete, computationally inexpensive, and finds a solution for a fixed source node to other node in the graph. Moreover, the presented solution is general enough to be incorporated into high-level tasks like cooperative surveillance and it can benefit from state-of-the-art formation motion planning approaches, which can be used for evaluation of edges of an input graph. The performed experimental results demonstrate behavior of the method in complex environments for formations consisting of tens of robots.

M. Estefanía Pereyra, R. Gastón Araguás, and Miroslav Kulich \publishedIEEE Latin American Conference on Computational Intelligence (LA-CCI). Arequipa. New Jersey, 2017. ISBN 978-1-5386-3734-0. \DOI10.1109/LA-CCI.2017.8285722 \originalhttps://ieeexplore.ieee.org/document/8285722 \coverpage

## I Introduction

Recent advances in mobile robotics and deployment of robotic systems in many practical applications have lead to intensive research of multi-robot systems and robot formations as their special case. One of the most studied topic deals with trajectory/path planning of a formation, known as multi-robot path planning (MPP) in an environment with obstacles, i.e. the problem, how to find a continuous collision-free motion of the formation through a known environment from a current configuration to a given final configuration. Besides optimization of some parameters of the solution (e.g., path distance, energy consumption, mission time), a shape and size of the formation is constrained and violation of these constrains is penalized.

Approaches to motion and path planning for multi-robot systems and robot formations can be classified into several categories. Behavior-based algorithms [1, 2] are decentralized and reactive, i.e. each robot is controlled individually, using only local information about its neighborhood. Robot’s behavior is typically composed of several simple behaviors (e.g., separation, alignment, and cohesion in [3]), which describe basic actions. These approaches are easy to implement and applicable to large swarms. They, on the other hand, fail in finding a plan in complex environments and do not guarantee precise formation control. To deal with the first problem, several heuristic search based algorithms were introduced based on particle swarm optimization [4], genetic algorithms [5] or ant colony optimization [6]. Nevertheless, precise formation shape can not be still maintained.

In contrast, centralized approaches consider a formation as a single body and plan trajectories in a high-dimensional composite configuration space (CCS). Exact solutions are complete, but their complexity is exponential in the dimension of CCS and therefore, methods based on sampling CSS were introduced. For example, a probabilistic road map with sampling strategies designed especially for multi-robot systems that enable fast coverage of the configuration space was presented in [7], while a generalization of rapidly exploring random trees (RRT) to a graph structure is introduced in [8].

Another research stream considers a leader-follower architecture. Besides other approaches, which compute leader’s trajectory and find trajectories of followers relative to this trajectory [9, 10] or coordinate motion of robots on a preplanned paths [11, 12], a big class of algorithms employs a concept of artificial potential fields. As classical potential fields [13] tend to find a local optimum, Garrido et al. [14] employed the Voronoi Fast Marching method, which propagates a wave over a viscosity map for a leader. Trajectories of followers are then dynamically computed to keep desired nominal inter-robot distances using Fast Marching (FM) with incorporated potentials reflecting leader’s path, obstacles and positions of other robots. The method produces paths with minimal Euclidean lengths and avoids local minimum, but generated paths are not smooth and go too close to obstacles. This can be solved by Fast Marching Square (FM) [15], which modifies wave expansion by incorporating velocity maps. Moreover, FM manages uncertainties in robot’s positions, sensor noise, and moving obstacles. This was recently applied for formations of unmanned surface vehicles (ships) allowing to model their dynamic behavior [16]. Application of the Frenet-Serret frame to control orientation of a formation enabled path planning for formations of unmanned aerial vehicles in 3D environments [17].

The works mentioned above do not explicitly address splitting and merging of a formation during movement, although this is possible with some approaches. While centralized exact methods are computationally demanding and thus practically inapplicable for larger formations, random sampling are more promising. For example, the authors in [18] present combination of RRT and particle swarm optimization for cooperative surveillance and demonstrate splitting of a formation in a simplified scenario with a single obstacle. A reactive obstacle avoidance with added rules for split and merge are introduced in [19], while extension of flocking behavior [3] with game-theoretic based reconfiguration is presented in [20].

The proposed approach can be seen as a part of a general hierarchical planning algorithm, which consists of a global path planner and a local motion planner. While the the global planner searches for a topological path of a formation and thus generates primarily movement directions, the motion planner determines (based on a path found by the global planner) motion for particular robots in a formation. This combination prevents the whole planner to be trapped at a local minimum and enables to compute feasible trajectories fast. Similar approach is described in [21], where the global planner constructs a partial Voronoi diagram on the fly and a memetic evolution algorithm is employed for motion generation along this diagram. The key contribution of the paper lies in design of a novel algorithm for global path planning, which extents well known Dijkstra’s algorithm to be applicable for MPP and which considers possible split and merge of a formation. This is in contrast with [21], which is primarily focused on a solution of local motion planning, while the global planner is simplified to assume a formation as a single point robot. On the other hand, motion planning is not addressed in the presented paper as some of the aforementioned approaches can be used for it. The rest of the paper is organized as follows. The general overview of the approach is described in Section II, while the proposed algorithm for robot formations is introduced in Section III. In Section IV we present and discuss some experimental results. Finally, Section V is dedicated to concluding remarks and future directions of the research.

## Ii Multi-robot path planning

The planning problem for a fleet of mobile robots, or MPP, can be generally understood as a search of a continuous sequence of feasible fleet configurations from a start configuration to a given goal configuration. A feasible configuration for a fleet is such configuration that robots in the fleet collide neither with the surrounding environment nor collide with each other. In many scenarios we don’t ask for an arbitrary sequence of configurations (trajectory), an optimal path with respect to some criteria (e.g., a mission time) is required instead. Moreover, additional constrains to fleet’s geometry may be applied. For example, robots may be requested to form and keep a specified shape or lattice, or to move close together in order to ensure visibility or communication to their neighbors.

In this work, we investigate the problem of MPP, from a common start point to a common end point of a known map. Each robot path is obtained by minimizing a cost function, which is proportional to the path length, the number of robots moving in the same path, and inversely proportional to the path width (i.e. space between obstacles). The formation can split if necessary but it is forced to merge as soon as possible, what means that each robot arriving to a common point should wait to all other robots of the formation that include this point on their paths. Consequently, the cost of the complete solution is given by the most expensive individual path. A valid solution of MPP is that which avoids collisions with obstacles in the map and collisions between robots. A collision between robots occurs when two or more robots share the same part (edge) of the path but in the opposite direction. To avoid collisions between robots a set of constrains for resulting paths are imposed, as described in Section II-B. To avoid collisions with obstacles, we construct a connected graph representing topology of collision free space of the working area as described in the next section and perform MPP on this graph.

### Ii-a General framework

Given a polygonal representation of the environment (Fig. 1LABEL:sub@fig:vor-a), start and goal positions, and the number of robots , the proposed solution depicted in Algorithm 1 works as follows.

^{1}

^{1}1We use implementation of a Voronoi Diagram (VD) from the Boost Polygon Library (http://www.boost.org/doc/libs/1_60_0/libs/polygon/doc/voronoi_diagram.htm). The image was generated directly from the output of the library. Note that a Voronoi Diagram for polygons contains generally parabolic segments which are approximated with line segments in the image.. The red line represents a tail to be filtered. LABEL:sub@fig:vor-b The same after filtering of edges and vertices.

It starts with the construction of a connected graph of vertex and edges (line 1), which is done in three steps.

A Voronoi diagram of all obstacles in the environment is generated (Fig. 1LABEL:sub@fig:vor-a), resulting in a set of polygons rounding the obstacles, and edges which are inside obstacles or which are connected to some obstacle vertex. These edges are then removed together with nodes and edges forming tails, i.e. components of the graph which can not be a part of any shortest path except paths originating or ending in these components (one of these components is highlighted in red in Fig. 1LABEL:sub@fig:vor-a). The graph after this removal is shown in Fig. 1LABEL:sub@fig:vor-b. Finally, if the goal or final positions are not in the graph, they are added into it. All remaining edges of the graph are evaluated furthermore (line 1). The cost of particular edges is a vector , where is the cost of a formation of robots to traverse the edge.

The cost of an edge can be either determined as the time needed to traverse the edge with some motion planning algorithm (e.g. [15]) in simulation or it can be approximated based on the length of the edge and the distance of the edge to the nearest obstacle. Some control coefficient can also be used to modify the cost for and thus to manage the formation splits. For simplicity the algorithm processes all nodes in the graph assuming that they have a degree less or equal to three, , that is, they have two or three connected edges. Nodes with are substituted by an equivalent set of nodes-edges that meet this constraint. Some examples of this substitution are shown in Fig. 2.

The proposed algorithm for formations is run next (line 1), which for a given start node computes shortest paths to the goal node in and all possible sizes of the formation. Each of the computed paths is a sequence of nodes . The algorithm is described in Section III in details. A complete motion of a formation is generated finally, given the path (line 1). Trajectories of robots in the formation are determined in this step so that relative positions of robots are computed with respect to geometrical constrains on the formation and to avoid nearby obstacles. Again, some motion planning algorithm can be employed to plan a motion between each two consecutive nodes of .

### Ii-B Path properties and constrains

Given a connected graph representing the working environment, with a set of vertices and edges , and a fleet of robots , the corresponding collision-free paths for the fleet in the graph are denoted as , with . Moreover, a path of the individual robot is a sequence of vertices such that is an edge of the graph.

The feasibility of a path is conditioned upon the following constrains: 1) initially, all the robots in the fleet are in the start position: . 2) there exists a state in the path such that , meaning that the robot reaches the goal on the shortest possible path. 3) any two paths from have not to be in a collision, i.e. given any pair of states , two paths are in collision if . 4) given two paths and two states , if then , i.e. the robot that arrives first to a vertex waits for the second one, in order to keep the formation joint as much time as possible.

## Iii Sequential path planning for formations

The proposed algorithm for MPP is explained in details in this section. Basically, the algorithm finds paths for a multi-robot team running sequentially an extended version of the well known Dijkstra’s algorithm for one robot on a tailored graph. The standard Dijkstra’s algorithm finds the cheapest paths together with their costs from the given source node to all other nodes in the graph . The algorithm stores for each node the minimum cost to reach this node and the predecessor from where the node is reached. The shortest path (in terms of cost) for the given node can be then easily determined by walking consecutively over predecessors starting in . In the initialization stage, costs of all the nodes are set to infinity and their predecessor to a fictive node , what means that the shortest path has not been found yet. The only exception is the start node, whose cost value is set to . The start node is then put into a priority queue, which is sorted according to . The algorithm then consecutively takes the nodes from the priority queue and for the current node , their neighbors are processed. For each neighbor , the total cost of arriving to it from is computed. If is reached for the first time by the algorithm, it is added into the priority queue with its total cost corresponding of arrive to it from , and is assigned as its predecessor, . If it is not the case, the computed cost is compared with the cost storaged in the node and, if it is smaller, the storaged cost is updated to , is set to , and with its new cost is added to the priority queue.

For the multi-robot case, the cost of an edge can vary with the size of the formation, and thus, all possible combinations given by the number of robots traveling through the node must be considered. This means that a node can not be processed at once, because it is not guaranteed that all needed information is available until all the robots are processed. Based on the standard Dijkstra’s algorithm, path planning for a multi-robot formation can be computed in a semi-decoupled way, considering split and rejoin of áºhe formation. The path planning is performed sequentially for all robots, revisiting each node accordingly to the number of robots that will travel trough it.

Given a formation of robots, a connected graph , and a vector of costs for each edge , where is the cost paid for traversing with robots, the sequential algorithm for a formation is depicted in Algorithm 2. Similarly to the standard Dijkstra’s algorithm, the proposed sequential algorithm starts with initialization of data structures (lines 2 – 2). Here, for each node of the graph, all predecessors are set to and the corresponding costs are initialized to infinity, except the cost to reach , which is set to . After that, all the nodes are pushed into the queue (line 2).

The algorithm then loops through all nodes in the queue and processes them in a similar way to the original Dijkstra’s algorithm (line 2). When the map is processed completely, the path for the first robot is generated at line 2, and added to the set of paths at the next line. Next, the path for the second robot in the formation is planned, updating costs of those edges in the graph that are a part of the previously computed robot path (lines 2 – 2). The algorithm ends whenever the paths for all robots are computed.

### Iii-a Path optimization

The paths for robots computed as explained in the previous section are optimal if the paths calculated for robots are a part of the optimal solution for robots, i.e . For example, in Fig. 3LABEL:sub@fig:example-a the paths obtained for two robots are shown. The paths are represented by green lines and the arrows over each edge indicate the number of robots traversing it. The cost vector for each edge is shown nearby the edge. In this case, the paths found by the algorithm from to are , with cost and , with cost . The optimal paths for two robots between these nodes are nevertheless and , with costs . These two optimal paths are shown in Fig. 3LABEL:sub@fig:example-b.

This problem can be partially solved by means of an optimization process applied in the second stage of the algorithm. For each new calculated path , the function (line 2) performs the following optimization process. Once a new path is calculated, all the previous paths are recalculated sequentially, beginning from the first one. If a recalculated path results in a lower cost of the formation, the original path is replaced by this new one. The optimization process performed by this function is detailed in Algorithm 3.

Given a set of paths , the algorithm runs over all previously calculated paths (line 3). A copy of the current set of paths is stored to be processed (line 3), and the maximum cost of the set is computed at line 3. A subset is extracted then (line 3) and a new path is computed (line 3), in a similar way as in the Algorithm 2 but considering the subset to determine the edges costs. This new path is added to the subset , and the maximal cost of is computed (lines 3 and 3). If the cost is lower than the lod one, the new computed path is preserved (lines 3 and 3).

### Iii-B Formation control coefficient

To keep control over the joint of the formation a coefficient named formation control coefficient is used. This coefficient modifies the cost of each edge proportionaly to the number of robots. The paths computed using different values of the coefficient are shown in Fig. 4. For a low the formation tends to stay together as shown in the left map (), while a high value of makes the formation to split over all the map and cover big areas, as shown in the right map of Fig. 4.

## Iv Experimental evaluation

All the experiments were evaluated under the same conditions, using a notebook with an Intel CORE i5 processor and 4GB of RAM, running a Debian GNU/Linux. The proposed algorithm has been implemented in Python 2.7. The maps used in the experiments are gaps, dense, staggered_brick_wall, potholes, var_density, var_density2 and var_density3 from http://imr.ciirc.cvut.cz/planning/maps.xml. An optimal path for each configuration (map, , , , ) is obtained by an exhaustive search method, which is used for comparison. The exhaustive search has a high computational complexity, which grows exponentially with the number of robots and the number of nodes in the map, and it is thus very difficult to obtain results for more than three robots in maps with more than ten nodes. Each test is performed by running the Algorithm 2, with and without the optimization process given as described in the Algorithm 3, and both results are compared with optimal results generated by the exhaustive search algorithm. Fig. 5 shows a typical test, where the optimization process performs a paths correction resulting in the same solution as the one provided by the exhaustive search.

Fig. 6 shows a comparison of the optimal paths (black lines) and the paths generated by the proposed algorithm (red lines): without the optimization process in Fig. (a)a and with optimization in Fig. (b)b. The plots are sample vs cost, where a sample refers to a path returned by the algorithm for each (map, , , , ) configuration, and cost is the cost of the returned path. From a total of computed paths, about solutions generated without optimization is optimal, while about solutions is optimal after optimization is applied. Note that only 10% of samples is drawn in Figs. (a)a and (b)b and the data are sorted by cost ascending for better visualization. In order to describe the quality of the found solution relative to the optimum, a gap is calculated for each sample, where is the cost of the found solution and is the cost of the optimal solution. Fig. (c)c shows the gap between each sequential solution and the optimal path for both non-optimized and optimized cases.

Table I presents the runtime of the proposed path planning algorithm for various numbers of robots and various maps with optimization (opt) and without it (nonopt). We set , since the process time is barely affected by this value. The most left-bottom node of the map was chosen as while the most right-top node was set to be in all the tests. Side-by-side comparison with the state-of-the-art methods is not presented due to (seemingly small but) important differences in the problem formulation. Nevertheless, the computation time of the proposed algorithm including the optimization process appears comparable with results from other authors, while the computation time of the non-optimized version of the algorithm is several times lower.

Number of robots | |||||||||||||||||||||

5 | 10 | 20 | 50 | 100 | |||||||||||||||||

maps | (nodes) | opt | - | nonopt | opt | - | nonopt | opt | - | nonopt | opt | - | nonopt | opt | - | nonopt | |||||

gaps | (12) | 0.002 | - | 0.0004 | 0.013 | - | 0.001 | 0.097 | - | 0.002 | 1.157 | - | 0.011 | 9.166 | - | 0.036 | |||||

staggered_bk | (33) | 0.006 | - | 0.001 | 0.045 | - | 0.003 | 0.263 | - | 0.010 | 3.291 | - | 0.042 | 26.780 | - | 0.125 | |||||

potholes | (43) | 0.007 | - | 0.002 | 0.047 | - | 0.005 | 0.373 | - | 0.016 | 4.622 | - | 0.067 | 33.429 | - | 0.198 | |||||

var_density2 | (45) | 0.010 | - | 0.003 | 0.058 | - | 0.006 | 0.310 | - | 0.016 | 4.613 | - | 0.066 | 39.447 | - | 0.202 | |||||

var_density3 | (45) | 0.012 | - | 0.002 | 0.058 | - | 0.006 | 0.353 | - | 0.017 | 5.495 | - | 0.070 | 39.152 | - | 0.205 | |||||

dense | (62) | 0.010 | - | 0.002 | 0.054 | - | 0.007 | 0.391 | - | 0.021 | 5.425 | - | 0.083 | 37.789 | - | 0.233 | |||||

var_density | (77) | 0.019 | - | 0.004 | 0.114 | - | 0.012 | 0.609 | - | 0.035 | 10.438 | - | 0.141 | 76.157 | - | 0.409 |

## V Conclusion

A new decoupled approach for multi-robot path planning with split and merge capability was presented. The algorithm is deterministic, complete and computationally inexpensive. The performed tests on various maps show that the algorithm provides the optimal solution in approximately of cases which can be increased to when a simple optimization procedure is applied. Solutions that remain suboptimal have nevertheless a low gap, which means that resulting paths are close to the optimum. Time complexity of the algorithm is very low as shown in the experiments, what makes the algorithm suitable for real-time (re-)planning or its deployment on on-board computers of real robots. The future work will be focused on improvement of the optimization process.

## Acknowledgments

The work of M. Estefanía Pereyra and R. Gastón Araguás has been supported by the “Multirrotores Autónomos para Aplicaciones en Ambientes Exteriores” project, U.T.N. PID UTI4534. The work of Miroslav Kulich has been supported by the European Union’s Horizon 2020 research and innovation programme under grant agreement No. 688117.

## References

- [1] G. A. S. Pereira, V. Kumar, and M. F. M. Campos, “Closed loop motion planning of cooperating mobile robots using graph connectivity,” Robotics and Autonomous Systems, vol. 56, no. 4, pp. 373–384, Apr. 2008.
- [2] X. Zhong, X. Zhong, and X. Peng, “VCS-based motion planning for distributed mobile robots: collision avoidance and formation,” Soft Computing, Mar. 2015.
- [3] C. W. Reynolds, “Steering behaviors for autonomous characters,” in Game developers conference, vol. 1999, 1999, pp. 763–782.
- [4] C. Bai, H. Duan, C. Li, and Y. Zhang, “Dynamic multi-uavs formation reconfiguration based on hybrid diversity-pso and time optimal control,” in Intelligent Vehicles Symposium, 2009 IEEE, June 2009, pp. 775–779.
- [5] H. Qu, K. Xing, and T. Alexander, “An improved genetic algorithm with co-evolutionary strategy for global path planning of multiple mobile robots,” Neurocomputing, vol. 120, pp. 509–517, 2013.
- [6] A. Noormohammadi Asl, M. B. Menhaj, and A. Sajedin, “Control of leader-follower formation and path planning of mobile robots using asexual reproduction optimization (aro),” Appl. Soft Comput., vol. 14, pp. 563–576, Jan. 2014.
- [7] C. M. Clark, “Probabilistic Road Map sampling strategies for multi-robot motion planning,” Robotics and Autonomous Systems, vol. 53, no. 3-4, pp. 244–264, Dec. 2005.
- [8] R. Kala, “Rapidly exploring random graphs: motion planning of multiple mobile robots.” Advanced Robotics, vol. 27, no. 14, pp. 1113–1122, 2013.
- [9] T. Barfoot and C. Clark, “Motion planning for formations of mobile robots,” Robotics and Autonomous Systems, vol. 46, no. 2, pp. 65–78, Feb. 2004.
- [10] J. Chen, D. Sun, J. Yang, and H. Chen, “A leader-follower formation control of multiple nonholonomic mobile robots incorporating receding- horizon scheme,” The International Journal of Robotics Research, 2009.
- [11] R. Olmi, C. Secchi, and C. Fantuzzi, “Coordination of multiple agvs in an industrial application,” in Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on. IEEE, 2008, pp. 1916–1921.
- [12] S. Liu, D. Sun, and C. Zhu, “Coordinated Motion Planning for Multiple Mobile Robots Along Designed Paths With Formation Requirement,” IEEE/ASME Transactions on Mechatronics, vol. 16, no. 6, pp. 1021–1031, Dec. 2011.
- [13] M. Zhang, Y. Shen, Q. Wang, and Y. Wang, “Dynamic artificial potential field based multi-robot formation control,” in Instrumentation and Measurement Technology Conference (I2MTC), 2010 IEEE, May 2010, pp. 1530–1534.
- [14] S. Garrido, L. Moreno, and P. U. Lima, “Robot formation motion planning using Fast Marching,” Robotics and Autonomous Systems, vol. 59, no. 9, pp. 675–683, Sept. 2011.
- [15] J. V. Gómez, A. Lumbier, S. Garrido, and L. Moreno, “Planning robot formations with fast marching square including uncertainty conditions,” Robotics and Autonomous Systems, vol. 61, no. 2, pp. 137–152, Feb. 2013.
- [16] Y. Liu and R. Bucknall, “Path planning algorithm for unmanned surface vehicle formations in a practical maritime environment,” Ocean Engineering, vol. 97, pp. 126–144, Mar. 2015.
- [17] D. Álvarez, J. V. Gómez, S. Garrido, and L. Moreno, “3D Robot Formations Path Planning with Fast Marching Square,” Journal of Intelligent & Robotic Systems, vol. 80, no. 3-4, pp. 507–523, Feb. 2015.
- [18] M. Saska, J. Chudoba, L. Přeučil, J. Thomas, G. Loianno, A. Třešňák, V. Vonásek, and V. Kumar, “Autonomous deployment of swarms of micro-aerial vehicles in cooperative surveillance,” in Unmanned Aircraft Systems (ICUAS), 2014 International Conference on, May 2014, pp. 584–595.
- [19] P. Ogren, “Split and join of vehicle formations doing obstacle avoidance,” in IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA ’04. 2004, vol. 2. IEEE, 2004, pp. 1951–1955 Vol.2.
- [20] P. Dasgupta and K. Cheng, “Robust multi-robot team formations using weighted voting games,” in Distributed Autonomous Robotic Systems. Springer, 2013, pp. 373–387.
- [21] C.-C. Lin, K.-C. Chen, and W.-J. Chuang, “Motion Planning Using a Memetic Evolution Algorithm for Swarm Robots,” International Journal of Advanced Robotic Systems, p. 1, May 2012.