Path Planning for a Formation of Mobile Robots with Split and Merge

TL;DR

This paper introduces a deterministic, optimal path planning algorithm for robot formations that allows splitting and merging, improving multi-robot navigation efficiency in complex environments.

Contribution

It presents a novel extension of Dijkstra's algorithm to handle formation split and merge scenarios with optimality guarantees.

Findings

The algorithm finds optimal paths in complex environments.

It effectively manages formation split and merge situations.

Experimental results confirm its efficiency and optimality.

Abstract

A novel multi-robot path planning approach is presented in this paper. Based on the standard Dijkstra, the algorithm looks for the optimal paths for a formation of robots, taking into account the possibility of split and merge. The algorithm explores a graph representation of the environment, computing for each node the cost of moving a number of robots and their corresponding paths. In every node where the formation can split, all the new possible formation subdivisions are taken into account accordingly to their individual costs. In the same way, in every node where the formation can merge, the algorithm verifies whether the combination is possible and, if possible, computes the new cost. In order to manage split and merge situations, a set of constraints is applied. The proposed algorithm is thus deterministic, complete and finds an optimal solution from a source node to all other nodes in the graph. The presented solution is general enough to be incorporated into high-level tasks as well as 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 presented experimental results demonstrate the ability of the method to find the optimal solution for a formation of robots in environments with varying complexity.

Equations

$$p_i=\{(e_{i1},r_{e_{i1}}),(e_{i2},r_{e_{i2}}),\dots ,(e_{ik},r_{e_{ik}})\},$$

$$e_{12}$$

$$e_{14}$$

$$e_{16}$$

$$e_{27}$$

$$e_{23}$$

$$e_{37}$$

$$p_1$$

$$p_2$$

$$p_3$$

$$p_4$$

$$p_1$$

$$p_2$$

$$p_3$$

$$p_4$$

$$p_5$$

Full text

1 Introduction

Recent advances in mobile robotics and deployment of robotic systems in many practical applications lead to intensive research of multi-robot systems and robot formations as their special case. One of the most studied topic deals with planning trajectory/path of a formation 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 [4, 20, 18, 23] 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 [20]), 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 [3], genetic algorithms [19] or ant colony optimization [15]. 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 [2] 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 [11].

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 [5, 6] or coordinate motion of robots on a preplanned paths [17, 13], a big class of algorithms employs a concept of artificial potential fields. As classical potential fields [22] tend to find a local optimum, Garrido et al. [9] 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 minima, but generated paths are not smooth and go too close to obstacles. This can be solved by Fast Marching Square (FM2) [10], which modifies wave expansion by incorporating velocity maps. Moreover, FM2 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 [14]. Application of the Frenet-Serret frame to control orientation of a formation enabled path planning for formations of unmanned aerial vehicles in 3D environments [1].

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 [21] 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 [16], while extension of flocking behavior [20] with game-theoretic based reconfiguration is presented in [8].

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 primary 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 minima and enables to compute feasible trajectories fast. Similar approach is described in [12], 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 the well known Dijkstra’s algorithm to be applicable for multi-robot systems and which considers possible split and merge of a formation. This is in contrast with [12], 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 directly used for it.

The rest of the paper is organized as follows. The general overview of the approach is described in Section 2, while the proposed extension of Dijkstra’s algorithm for robot formations is introduced in Section 3. In Section 4 we present and discuss some experimental results. Finally, Section 5 is dedicated to concluding remarks and future directions of the research.

2 Framework

The planning problem for a fleet of mobile robots can be generally understood as search for a continuous sequence of feasible configurations of the fleet from a start configuration to a given goal configuration. A feasible configuration is such a configuration, in which a fleet does not collide with a surrounding environment as well as robots in the fleet do not collide with each other. In many scenarios we don’t ask for arbitrary sequence of configurations (trajectory), an optimal trajectory 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 neighbours.

Given a polygonal representation of the environment, start and goal positions, and the number of robots $R$, the proposed solution works in several steps, as described in Algorithm 1. It starts with construction of a graph $G(V,E)$ (line 1), which is done in three steps. A Voronoi diagram of obstacles is generated first. As illustrated in Fig. 1LABEL:sub@fig:vd-a, the original Voronoi diagram contains edges, which are inside obstacles or which are connected to some obstacle vertex. These edges are thus 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 red in Fig. 1LABEL:sub@fig:vd-a). The graph after this removal is shown in Fig. 1LABEL:sub@fig:vd-b. If the goal or final positions are not in the graph, they are added into it finally.

All edges of the graph are evaluated in the second step (line 1). A cost of a particular edges is a vector $\boldsymbol{c}=(c_{1},c_{2},\dots c_{R})$, where $c_{k}$ is the cost for a formation of $k$ 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. [10]) in simulation or it can be approximated based on distance of the edge from the obstacle nearest to it. Some penalty can be also added to the cost for $k<R$ expressing that the formation is split.

For simplicity the algorithm processes all nodes in the graph assuming that they have a degree less or equal to three, $\rho\leq 3$, that is, they have two or three connected edges. Nodes with $\rho>3$ are substituted by an equivalent set of nodes-edges that meet this constraint. Nodes with $\rho=4$ are substituted with two nodes, which are connected with an edge with cost equal to zero. Similarly, nodes with $\rho=5$ are substituted with three nodes. Some examples of this substitution are shown in Fig. 2.

The proposed algorithm for formations, described later in Section 3, is run next (line 1), which for a given start computes shortest paths to all other nodes in $G(V,E)$ and all possible sizes of a formation. In other words, it will be possible to reconstruct an optimal path from the start to an arbitrary node $Q_{goal}$ and the given number of robots $r\in\left<1,R\right>$ after application of this step. This path is a sequence of nodes $p=\{v_{0}=v_{ini},v_{1},v_{2},\dots,v_{n}=Q_{goal}\}$ with the properties and constrains detailed in 2.1. A complete motion of a formation is generated finally, given the path $p$ (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 $p$.

2.1 Path properties and constrains

Given a connected graph $\cal G=(V,E)$ representing the working environment, with a set of vertices ${\cal V}=\{v_{1},v_{2},\dots,v_{i}\}$ and edges ${\cal E}=\{e_{1},e_{2},\dots,e_{j}\}$, and a fleet of robots ${\cal R}=\{r_{1},r_{2},\dots,r_{R}\}$, the corresponding collision-free paths for the fleet in the graph are denoted as ${\cal P}=\{p_{1},p_{2},\dots,p_{R}\}$, with $p_{i}:\mathbb{Z}^{+}\to\cal V$. Moreover, a path of the individual robot $r_{i}$ is a sequence of vertices $p_{i}=\{v_{i_{1}},v_{i_{2}},\dots v_{i_{k}}\}$ such that $(v_{i_{j}},v_{i_{j+1}})$ is an edge of the graph. In order to compute the cost of the path another representation of the same path is used, where the number of robots passing through each edge is considered. In such representation, a path of the individual robot $r_{i}$ is a sequence of tuples, edges and a number of robots sharing each edge, $p_{i}=\{(e_{i1},r_{e_{i1}}),(e_{i2},r_{e_{i2}}),\dots(e_{ik},r_{e_{ik}})\}$, where $(e_{ij},e_{i(j+1)})$ are connected edges of the graph.

The feasibility of the path $p_{i}$ is conditioned upon the following constrains: 1) initially, all the robots in the fleet are in the start position: $p_{i}(0)=Q_{\rm ini},\,\forall p_{i}\in\cal P$. 2) there exists a state in the path $k_{\rm min}\in\mathbb{Z}^{+}$ such that $p_{i}(k_{\rm min})=Q_{\rm goal}$, meaning that the robot $r_{i}$ reaches the goal on the shortest possible path. 3) any two paths from ${\cal P}$ have not to be in a collision, i.e. given any pair of states $m,l\in\left<0,k_{\rm min}\right>$, two paths $p_{i},p_{j}$ are in collision if $(p_{i}(m),p_{i}(m+1))=p_{j}(l+1),p_{j}(l))$. 4) given two paths $p_{i},p_{j}$ and two states $m,l\in\left<0,k_{\rm min}\right>$, if $p_{i}(m)=p_{j}(l)$ then $m=l\leftarrow max(m,l)$, 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.

3 Multi-robot path planning algorithm

Standard Dijkstra’s algorithm finds the cheapest paths together with their costs from the given source node $Q_{\rm ini}$ to all other nodes in the graph ${\cal G}$ given costs of all graph edges. The algorithm stores for each node $v\in{\cal G}$ the minimum cost $C_{v_{\rm min}}$ to reach this node and the predecessor $v_{\rm prev}$ from where the node is reached. The shortest path for the given node $v$ can be then easily determined by walking consecutively over predecessors starting in $v$. In the initialization stage, costs $C_{v}$ of all the nodes are set to infinity and their predecessor $v_{\rm prev}$ to a fictive node $None$, what means that the shortest path has not been found yet. The only exception is the start node, whose cost value is set to [math]. The start node is then put into a priority queue, which is sorted according to $C_{v_{i}}$. The algorithm then consecutively takes the nodes from the priority queue and for the current node $u$, their neighbors $v_{i}$ are processed. For each neighbor $v_{i}$, the total cost $C_{v_{i}}$ of arriving to it from $u$ is computed. If $v_{i}$ is reached for the first time by the algorithm, it is added into the priority queue with its total cost $C_{v_{i}}$ corresponding of arrive to it from $u$, and $u$ is assigned as its predecessor, $v_{\rm prev}=u$. If it is not the case, the computed cost $C_{v_{i}}$ is compared with the cost stored in the node and, if it is smaller, the staged cost is updated to $C_{v_{i}}$, $v_{\rm prev}$ is set to $u$, and $v_{i}$ 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 traversing the edge, and thus, all possible combinations of formation sizes 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. The proposed algorithm for a formation is depicted in Algorithm 2. Here, a formation of $R$ robots, a connected graph $\cal G(V,E)$, and a vector of costs ${\boldsymbol{c}}^{e}=\left(c^{e}_{1},c^{e}_{2},\dots,c^{e}_{R}\right)$ for each edge $e$, where $c^{e}_{r}$ is the cost paid for traversing $e$ with $r\in\left<1,R\right>$ robots are considered.

Similarly to the standard Dijkstra’s algorithm, the algorithm uses a priority queue $\cal H$ to select the next state to be processed. In our approach, a state is a data structure associated to a node $v$, given by $s_{\rm vr}=\left<v,r_{\rm v},c_{\rm vr},p_{\rm vr}\right>$, where $r_{\rm v}$ is the number of robots arriving to the node, $p_{\rm vr}$ are the paths for each of this robots computed from $Q_{\rm ini}$ to $v$, and $c_{\rm vr}$ is the highest cost corresponding to the worst paths of $p_{\rm vr}$. A node can have a lot of associated states for the same number of robots, depending on the combination of paths used for each robot in the node. All of these states are stored in a state table $\cal S$ which will be used later to combine paths in the merge operation.

The algorithm starts with initialization of data structures (lines 2 – 2). First, all nodes in the graph are stored in a dynamic list $\cal N$. Then, the states associated to each neighbour of $Q_{\rm ini}$ considering a different number of robots $r\in\left<1,R\right>$ are generated (line 2) and stored in the queue (line 2) and in the state table (line 2). After that, the algorithm loops processing each of the states from the priority queue (line 2) until the optimal paths for the $R$ robots from $Q_{\rm ini}$ to each node in the graph are computed.

The algorithm processes each state as follows. It takes the next state $s_{\rm ur}$ from the queue (line 2). If this state corresponds to an arrival of $r$ robots to the node $u$ from $Q_{\rm ini}$ for the first time, what means that it is the cheapest solution so far, it is stored in the set of optimal paths $P$ (line 2). Then, considering the possibility of formation split, for each number of robots $r<r_{\rm u}$, and for each neighbor $v$ of $u$, a new state $s_{\rm vr}$ is generated (line 2). If this state does not exist in the state table $\cal S$, it is combined if possible with other states associated with the node $v$ and stored in $\cal S$. In this step, the possibility of formation merge is considered by means of combination of states. Two states are combinable if they meet a set of rules, which will be described in section 3.1. Finally, if $r_{\rm u}=R$, the node $u$ is removed from the list $\cal N$ (line 2).

3.1 Merge of the formation

The process of states combination allows the algorithm to deal with formation merge. A state $s_{\rm vk}$ represents the arrival of $k$ robots to the node $v$ by means of their $p_{\rm vk}$ paths. The existence of a second state $s_{\rm vq}$ associated to the same node $v$, such that $k+q<=R$, allows the combination of both states which leads to generation of a new state $s_{\rm v(k+q)}$. The new state represents the arrival of $k+q$ robots to the node $v$ by means of a combination, that is a merge of $k+q$ robots of the formation. In order to evaluate whether a merge is possible, a set conditions must be satisfied. The fuction to combine two sattes is depicted in Algorithm 3. Given a state $s_{\rm vr}$, the function tries to combine it with all other states in the state table $S$, ensuring that the number of robots after the combination remains less or equal to $R$ (lines 3 - 3). Each possible combination is evaluated by three rules (lines 3 - 3), and if all of them are satisfied the new combined state is added to the queue $\cal H$ and state table $\cal S$ (lines 3 - 3).

Using the edge representation of paths,

[TABLE]

two paths $p_{1}$ and $p_{2}$ must fulfill the following rules in order to be combined:

Shared edges rule This rule looks for edges common to both paths, and verifies whether those shared edges contain equal number of robots.

Shared nodes rule This rule looks for nodes common to both paths, and verifies whether the number of robots arriving to the node is higher than or equal to the number of robots leaving the node.

Cross edges rule This rule avoids combinations where both paths contain the same edge but in opposite direction, given that these combinations don’t meet the no collision constrain for paths (Section 2.1).

4 Experimental evaluation

The proposed algorithm for multi-robot path planning has been implemented in Python 2.7. 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. Given that the algorithm is based in a Voronoi diagram of the map, it is complete; and since it is based on the standard Dijkstra, it is optimal too. In order to prove its optimality, the experiments were performed on a set of maps with various complexities from http://imr.ciirc.cvut.cz/planning/maps.xml. Nevertheless, because of the high complexity of the problem, the optimality of the solutions was possible to verify manually only for maps with a low number of nodes and with a low number of robots.

In the Fig. 3 an example of a path planning in a simple map with $8$ vertices is shown. For each edge of the graph a vector of costs is determined by the following equations,

[TABLE]

where $r\in{1,2,\dots,R}$ is the number of robots travelling through the edge. All the robots of the formation are initially at the blue start node, the formation splits and merges at different nodes, following the green paths. The arrows in the graph represent the number of robots moving through each edge and the direction of the movement.

Fig. 3a shows the solution for four robots. Note the number in square brackets drawn near each edge which express vectors of costs computed by the equations presented before for $r=1$ to $r=4$. The optimal paths given by the algorithm are

[TABLE]

and the formation cost is then given by the worst cost of the set, that is the cost of the path $p_{1}$, $c_{1}=449$. In the Fig. 3b the solution for ten robots formation is shown, in this case the solution found is

[TABLE]

Although the complexity of this map is relatively low, the problem becomes quickly very complex as the number of robots grows and therefore it is not easy to prove the optimality of the generated solutions. In the same way, if the complexity of the map (i.e. the number of vertices) grows, the optimality of the solutions is very difficult to verify even with a low number of robots. For example, Fig. 4 shows a solution for a three robots formation in four complex maps, the map on 4a has a total of $77$ nodes, the map on 4b has $62$ nodes and the maps on 4c and 4d have $45$ nodes.

5 Conclusion

A novel algorithm for multi-robot path planning was presented. The algorithm finds a path for each robot of a formation, from a given node to all other nodes in the graph, considering the possibility of split and merge of the formation. It is an extended version of the standard Dijkstra’s algorithm, and it is therefore deterministic, complete and finds an optimal solution. The algorithm is computationally heavy, and finding solutions for more than ten robots in maps with more than twenty nodes is very time consuming.

In the future work, improvements in realization of the constrains for formation merge will be considered, because this is the most time consuming part of the algorithm. Naturally, some aforementioned motion planning approach for formations will be employed to form, together with the proposed solution, the integrated approach for formation planning. This will allow us to perform experiments in realistic setups in simulation and then with real robots.

ACKNOWLEDGMENT

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 European Community’s HORIZON 2020 Programme under grant agreement No. 688117 “SafeLog: Safe human-robot interaction in logistic applications for highly flexible warehouses”.