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

TL;DR

This paper introduces a deterministic, graph-based path planning algorithm for robot formations that handles splitting and merging, suitable for complex environments and scalable to tens of robots.

Contribution

It presents a novel, efficient extension of Dijkstra's algorithm for formation path planning with split and merge capabilities.

Findings

Algorithm is deterministic and complete.

Works efficiently in complex environments.

Scales to formations of tens of robots.

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 another 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 the behavior of the method in complex environments for formations consisting of tens of robots.

Tables

Table 1. TABLE I: Times in seconds with (opt) and without (nonopt) the optimization process, for 5 5 5 , 10 10 10 , 20 20 20 , 50 50 50 and 100 100 100 robots with k = 1000 𝑘 1000 k=1000 .

		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

Full text

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 (FM2) [15], 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 [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 $R$, the proposed solution depicted in Algorithm 1 works as follows.

It starts with the construction of a connected graph $\cal G(V,E)$ of vertex $\cal V$ and edges $\cal E$ (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 $\boldsymbol{c}=(c_{1},c_{2},\dots c_{R})$, where $c_{r}$ is the cost of a formation of $r$ 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 $r<R$ 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, $\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. 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 $\cal G(V,E)$ and all possible sizes of the formation. Each of the computed paths is a sequence of nodes $p=\{v_{0}=Q_{\rm ini},v_{1},v_{2},\dots,v_{n}=Q_{\rm goal}\}$. The algorithm is described in Section III in details. 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$.

II-B 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.

The feasibility of a 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.

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 $Q_{\rm ini}$ to all other nodes in the graph $\cal G$. 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 (in terms of cost) 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 storaged in the node and, if it is smaller, the storaged 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, 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 $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, 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 $None$ and the corresponding costs are initialized to infinity, except the cost to reach $Q_{\rm ini}$, which is set to [math]. After that, all the nodes are pushed into the queue $\cal H$ (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 ${\cal P}_{R}=\{p_{1},p_{2},\dots,p_{R}\}$ for all $R$ robots are computed.

III-A Path optimization

The paths ${\cal P}_{R}=\{p_{1},p_{2},\dots,p_{R}\}$ for $R$ robots computed as explained in the previous section are optimal if the paths ${\cal P}_{R-1}$ calculated for $R-1$ robots are a part of the optimal solution for $R$ robots, i.e ${\cal P}_{R-1}\subset{\cal P}_{R}$. 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 ${\boldsymbol{c}}^{e}=\left(c^{e}_{1},c^{e}_{2}\right)$ for each edge $e$ is shown nearby the edge. In this case, the paths found by the algorithm from $Q_{\rm ini}=1$ to $Q_{\rm goal}=4$ are $p_{1}=\{1,2,3,4\}$, with cost $C_{1}=168$ and $p_{2}=\{1,2,4\}$, with cost $C_{2}=170$. The optimal paths for two robots between these nodes are nevertheless $p_{1}=\{1,3,4\}$ and $p_{2}=\{1,2,4\}$, with costs $C_{1}=C_{2}=163$. 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 $p_{\rm r}$, the function $compute\_path()$ (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 ${\cal P}_{\rm r}=\{p_{1},p_{2},\cdots,p_{\rm r}\}$, the algorithm runs over all previously calculated paths (line 3). A copy of the current set of paths ${\cal P}_{r}$ is stored to be processed (line 3), and the maximum cost of the set is computed at line 3. A subset ${\cal P}_{\rm r-1}\subset{\cal P}_{\rm r}\setminus p_{\rm i}:i\in\left<1,\rm r-1\right>$ 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 ${\cal P}_{\rm r-1}$ to determine the edges costs. This new path is added to the subset ${\cal P}_{\rm r-1}$, and the maximal cost of $\{{\cal P}_{\rm r-1}\}\cup p_{\rm new}$ is computed (lines 3 and 3). If the cost is lower than the lod one, the new computed path $p_{\rm new}$ is preserved (lines 3 and 3).

III-B Formation control coefficient

To keep control over the joint of the formation a coefficient $k$ 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 $k$ are shown in Fig. 4. For a low $k$ the formation tends to stay together as shown in the left map ($k=10$), while a high value of $k$ 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, ${\cal{Q}}_{\rm ini}$, ${\cal{Q}}_{\rm goal}$, $R$, $k$) 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. 6a and with optimization in Fig. 6b. The plots are sample vs cost, where a sample refers to a path returned by the algorithm for each (map, ${\cal{Q}}_{\rm ini}$, ${\cal{Q}}_{\rm goal}$, $R$, $k$) configuration, and cost is the cost of the returned path. From a total of $\approx 2000$ computed paths, about $85\%$ solutions generated without optimization is optimal, while about $92\%$ solutions is optimal after optimization is applied. Note that only 10% of samples is drawn in Figs. 6a and 6b 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 $g=(c-c_{OPT})/c_{OPT}$ is calculated for each sample, where $c$ is the cost of the found solution and $c_{OPT}$ is the cost of the optimal solution. Fig. 6c 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 $k=1000$, since the process time is barely affected by this value. The most left-bottom node of the map was chosen as ${\cal Q}_{\rm init}$ while the most right-top node was set to be ${\cal Q}_{\rm goal}$ 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.

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 $84\%$ of cases which can be increased to $92\%$ 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.