Navigation of Multiple Disk-Shaped Robots with Independent Goals within Obstacle-Cluttered Environments

Abstract

In this work, we propose a hybrid control scheme to address the navigation problem for a team of disk-shaped robotic platforms operating within an obstacle-cluttered planar workspace. Given an initial and a desired configuration of the system, we devise a hierarchical cell decomposition methodology which is able to determine which regions of the configuration space need to be further subdivided at each iteration, thus avoiding redundant cell expansions. Furthermore, given a sequence of free configuration space cells with an arbitrary connectedness and shape, we employ harmonic transformations and harmonic potential fields to accomplish safe transitions between adjacent cells, thus ensuring almost-global convergence to the desired configuration. Finally, we present the comparative simulation results that demonstrate the efficacy of the proposed control scheme and its superiority in terms of complexity while yielding a satisfactory performance without incorporating optimization in the selection of the paths.

1. Introduction

The autonomous operation of robotic platforms inside cluttered environments constitutes an actively studied research topic, with autonomous navigation undeniably being a fundamental aspect of it. Additionally, as the tasks that robots are entrusted with grow in complexity, the employment of multi-robot systems, which generally exhibit a higher robustness and versatility than their single-robot alternatives, progressively increases. Thus, the multi-robot navigation problems to be addressed become more challenging day by day, raising the need for more efficient path and motion planning schemes.

Several methodologies can be found in the literature for coordinating the motion of two or more robots such that a desired configuration is reached under standard collision avoidance specifications. Combinatorial and algebraic approaches, such as the ones presented in [1,2], were among the earliest to be considered. However, despite their elegance, their complexity renders them impractical for addressing cases with non-trivial sizes of robotic teams. On the other side, probabilistic sampling methods, such as Rapidly Exploring Random Trees [3] and Probabilistic Roadmaps [4], constitute a popular solution employed in the recent literature due to their simplicity and their ability to efficiently handle large configuration spaces while being subjected to a variety of constraints. Nevertheless, these methodologies are generally having a hard time addressing problems with constricted configuration spaces (e.g., workspaces densely occupied by obstacles or narrow corridors). To alleviate these issues, various attempts were made. In [5], a novel sensory steering algorithm was designed which used the local Voronoi decomposition of the workspace to significantly improve the path-planning performance of sampling-based algorithms near difficult regions, such as narrow passages. In [6,7], the authors proposed a scheme that samples entire manifolds instead of isolated configurations, which are, in turn, used for approximating the configuration space’s connectivity graph, thus allowing the planner to perform significantly better even in tight workspaces. Alternatively, when a common graph representation of the workspace is shared among the agents, efficient methodologies for coordinating their transitions were proposed in [8,9,10,11,12]. On the other hand, a methodology was presented in [13,14], which addresses cases where the motion of each robot is restricted to a distinct graph by building a composite roadmap (i.e., the Cartesian product of the individual graphs). Furthermore, more efficient extensions of this approach, which work on implicitly defined composite roadmaps and, potentially, lower-dimensional configuration spaces, can be found in [15,16,17,18]. The approximate cell decomposition [19] and Slice Projection [20,21,22] constitute alternative methodologies which were successfully employed for tackling robot navigation problems with complex configuration spaces [23,24,25] and generally exhibit fast exploration capabilities when coupled with hierarchical adaptive subdivision schemes guided by suitable heuristics. For more details on the related literature, the reader may refer to the following recent comprehensive review paper [26].

In this work, we address the navigation problem for a team of disk-shaped robots that operate within an arbitrary, obstacle-cluttered, planar and connected workspace (see Figure 1). Given an initial and desired configuration for the robotic team, we design a high-level hierarchical cell decomposition planner which is tasked with the exploration of the system’s configuration space to discover a sequence of cells that the robots can safely traverse toward the desired configurations. One of the strong points of the proposed algorithm is the use of a suitable labeling mechanism for selecting the regions of the configuration space to be subdivided at each iteration. Particularly, by computing an over- and an under-approximation of each robot’s footprint, our algorithm can determine which cells may contain feasible configurations of the system while automatically discarding the cells that are determined to contain none. Finally, having obtained a sequence of traversable cells, we equip each robot with decentralized low-level control laws based on harmonic maps and adaptive harmonic potential fields, originally introduced in [27], which guarantee the safe and almost-global convergence to the goal. It should be noted that the high-level planner produces a series of cells through which the multi-robot system has to go in order to attain the final desired configuration. The aforementioned low-level controller, which was extracted by our previous work [27], simply guarantees a safe transition between successive cells. Thus, any other motion planning algorithm would also suffice.

The contribution of this work is the adaptive subdivision algorithm for obtaining a sequence of traversable cells that connect the given initial and final configurations or determining that no solution exists. Unlike standard grid/cell-based methods, the proposed algorithm subdivides the cells adaptively, requiring no selection of some arbitrary resolution by the user. Moreover, unlike probabilistic sampling methods, the proposed method does not generate paths of configuration but rather sequences of safe sets, allowing the user to make use of a larger set of controllers in addition to standard trajectory tracking schemes. Additionally and contrary to the probabilistic sampling methods, which are inherently incapable of determining the infeasibility of a given problem (such algorithms require the user to specify an arbitrary upper bound of samples after which the algorithms abort) as a direct consequence of their probabilistic completeness nature, our algorithm partitions the configuration space into cells (dense subsets) of the configuration space and is capable of determining when no admissible path exists in finite time (see [19]).

The outline of this article is as follows. At the end of this section, we define some preliminary notions and the notation used throughout this work. In Section 2, we formulate the problem addressed in this work. In Section 3, we elaborate on the proposed planner’s design as well as the velocity control scheme employed for safely executing the computed plan. Finally, the simulation results verifying the efficacy of the proposed control scheme are presented in Section 4.

Notation: Throughout this work, we shall use ${\mathfrak{I}}_N\triangleq \{1,2,\dots ,N\}$ (resp., ${\mathfrak{I}}_N^{★}\triangleq \left\{0\right\}\cup {\mathfrak{I}}_N$) to denote the set consisting of all natural numbers up to N, starting from 1 (resp., 0). Additionally, given sets A and B, we use $\partial A$, $\mathrm{int}\left(A\right)$ and $\mathrm{cl}\left(A\right)$ to denote the boundary, interior and closure of A, respectively, and $A\setminus B$ to denote the complement of B with respect to A.

2. Problem Formulation

We consider a team of $N_{\mathcal{R}}$ robots operating within a compact planar workspace $\mathcal{W}\subseteq {\mathbb{R}}^2$ occupied by a set of $N_o$ disjoint, fixed inner obstacles ${\mathcal{O}}_i,i\in {\mathfrak{I}}_{N_o}$. We assume that each robot i has a disk-shaped body ${\mathcal{R}}_i\subset {\mathbb{R}}^2$ with radius $r_i>0$. Let ${\mathcal{F}}_w$ and ${\mathcal{F}}_i,i\in {\mathfrak{I}}_{N_{\mathcal{R}}}$ be the coordinate frames arbitrarily embedded in $\mathcal{W}$ and ${\mathcal{R}}_i,i\in {\mathfrak{I}}_{N_{\mathcal{R}}}$, respectively. We shall refer to the origin of each ${\mathcal{F}}_i,i\in {\mathfrak{I}}_{N_{\mathcal{R}}}$ as the reference point of the corresponding robot. Moreover, without loss of generality, we assume that the reference point of each robot coincides with the center of its body. Let $p_i\triangleq {\left[x_i,y_i\right]}^T\in {\mathbb{R}}^2$ denote the relative position of i-th robot’s reference point with respect to the workspace’s coordinate frame ${\mathcal{F}}_w$, and let ${\mathcal{R}}_i\left(p\right)$ denote its footprint, i.e., the space occupied by robot i when placed at position p. Throughout this work, we shall use $\mathfrak{C}\subseteq {\mathbb{R}}^{2N_{\mathcal{R}}}$ to denote the robotic system’s configuration space and $P\triangleq {\left[p_1^T,p_2^T,\dots ,p_{N_{\mathcal{R}}}^T\right]}^T\in \mathfrak{C}$ to denote the stacked vector of robot positions. For the sake of brevity, we shall also use $P\left[i\right]$ to denote the i-th component of P, i.e., $P\left[i\right]=p_i$. Let ${\mathcal{W}}^o$ denote the complement of $\mathcal{W}$, i.e., ${\mathcal{W}}^o\triangleq {\mathbb{R}}^2\setminus \mathcal{W}$. We also define a configuration P as feasible iff the following conditions hold:

$$\begin{array}{cc}\hfill {\mathcal{R}}_i\left(P\right[i\left]\right)\cap {\mathcal{R}}_j\left(P\right[j\left]\right)=\varnothing ,& \forall i\ne j\in {\mathfrak{I}}_{N_{\mathcal{R}}}\hfill \\ \hfill {\mathcal{R}}_i\left(P\right[i\left]\right)\cap {\mathcal{W}}^o=\varnothing ,& \forall i\in {\mathfrak{I}}_{N_{\mathcal{R}}}\hfill \end{array}$$

(1)

and we shall use  ${\mathfrak{C}}^f\subset \mathfrak{C}$ to denote the set of all feasible configurations of the robotic system, whereas its complement ${\mathfrak{C}}^o\triangleq \mathfrak{C}\setminus {\mathfrak{C}}^f$ corresponds to the set all infeasible configurations. Finally, we assume that the motion of each robot i obeys the single-integrator kinematic model:

$${\dot{p}}_i=u_i,i\in {\mathfrak{I}}_{N_{\mathcal{R}}}$$

(2)

where $u_i$ denotes the control input.

Problem:Let $P_{\mathrm{init}}$ and $P_{\mathrm{des}}$ be two given feasible configurations of the multi-robot system that belong to the same segment of  ${\mathfrak{C}}^f$. Our goal is to design a control scheme that drives any robot i, initialized at $p_{\mathrm{init},i}=P_{\mathrm{init}}\left[i\right]$, to the specified desired position $p_{\mathrm{des},i}=P_{\mathrm{des}}\left[i\right]$, while avoiding inter-robot and robot–workspace collisions, i.e., $P\left(t\right)\in {\mathfrak{C}}^f$ for all $t\ge 0$.

3. Control Design

To address the aforementioned problem, first, we employ a hierarchical cell decomposition scheme for partitioning the configuration space of the multi-robot system $\mathfrak{C}$ into cells, as described in Section 3.1. Then, we design a high-level planner, in Section 3.2, which recursively expands the aforementioned structure until a sequence of adjacent cells connecting $P_{\mathrm{init}}$ and $P_{\mathrm{des}}$ is found. Finally, the low-level control scheme that ensures safe transition between cells until the goal configuration is reached is presented in Section 3.3.

3.1. Configuration Space Decomposition

In this subsection, we present the hierarchical cell decomposition scheme that will be employed in our approach. We begin with disregarding inter-robot collisions and considering the configuration space of each individual robot. Particularly, the configuration space of robot i, denoted herein by ${\mathcal{A}}_i\left(\mathcal{W}\right)$, corresponds to the largest subset of $\mathcal{W}$ where the reference point of robot i can be placed such that ${\mathcal{R}}_i\left(p_i\right)\cap {\mathcal{W}}^o=\varnothing$, for all $p_i\in {\mathcal{A}}_i\left(\mathcal{W}\right)$. Moreover, given a subset $\mathcal{Z}$ of $\mathcal{W}$, we shall use ${\mathcal{A}}_i\left(\mathcal{Z}\right)$ to denote the set of feasible positions of robot i which belong to $\mathcal{Z}$, i.e.,

$${\mathcal{A}}_i\left(\mathcal{Z}\right)\triangleq \left\{p|p\in \mathcal{Z}\mathrm{and}{\mathcal{R}}_i\left(p\right)\cap {\mathcal{W}}^o=\varnothing \right\},\forall i\in {\mathfrak{I}}_{N_{\mathcal{R}}}.$$

(3)

In addition to ${\mathcal{A}}_i\left(·\right)$, which corresponds to the set of feasible positions of robot i, we also consider two estimations of the area that is potentially occupied by ${\mathcal{R}}_i$ when $p_i$ is restricted in a subset $\mathcal{Z}$ of $\mathcal{W}$. Particularly, given a robot $i\in {\mathfrak{I}}_{N_{\mathcal{R}}}$ and a set $\mathcal{Z}\subseteq {\mathcal{A}}_i\left(\mathcal{W}\right)$, let $\overline{{\mathcal{R}}_i}\left(\mathcal{Z}\right)$ and $\underline{{\mathcal{R}}_i}\left(\mathcal{Z}\right)$ be an over-approximation and under-approximation, respectively, of the footprint of ${\mathcal{R}}_i$ when robot i is swept over $\mathcal{Z}$ such that:

$$\begin{array}{cc}\hfill \overline{{\mathcal{R}}_i}\left(\mathcal{Z}\right)& \supseteq \bigcup _{p\in \mathcal{Z}}^{}{\mathcal{R}}_i(p)\hfill \\ \hfill \underline{{\mathcal{R}}_i}(\mathcal{Z})& \subseteq \bigcap _{p\in \mathcal{Z}}^{}{\mathcal{R}}_i(p)\hfill \end{array}$$

(4)

and

$$\begin{array}{c}\hfill \overline{{\mathcal{R}}_i}\left(\mathcal{Z}\right)\subseteq \overline{{\mathcal{R}}_i}(\mathcal{Z}\prime ),\forall \mathcal{Z}\subseteq {\mathcal{Z}}^{\prime }\\ \hfill \underline{{\mathcal{R}}_i}\left(\mathcal{Z}\right)\supseteq \underline{{\mathcal{R}}_i}(\mathcal{Z}\prime ),\forall \mathcal{Z}\subseteq {\mathcal{Z}}^{\prime }.\end{array}$$

(5)

An example of such approximations can be seen in Figure 2. (for disk-shaped robots, a valid over-approximation $\overline{{\mathcal{R}}_i}\left(\mathcal{Z}\right)$ can be computed by offsetting $\mathcal{Z}$ by $r_i$, whereas $\underline{{\mathcal{R}}_i}\left(\mathcal{Z}\right)$ can be calculated by ${\cap }_{p\in \partial \mathcal{Z}}{\mathcal{R}}_i\left(p\right)$).

We now consider a set $\mathcal{S}\subseteq {\mathbb{R}}^2$ that has the form $[x_1,x_2]\times [y_1,y_2]$. We shall refer to such a set as a simple slice of ${\mathbb{R}}^2$. Given a simple slice $\mathcal{S}$ and a robot $i\in {\mathfrak{I}}_{N_{\mathcal{R}}}$, we will use ${\mathcal{W}}_{\mathcal{S}}^i\triangleq {\mathcal{A}}_i\left(\mathcal{W}\right)\cap \mathcal{S}$ to denote the set of feasible positions of robot i (neglecting inter-robot collisions) that are contained in $\mathcal{S}$. A set $\mathfrak{S}=\left\{{\mathcal{S}}_i|i\in {\mathfrak{I}}_{N_{\mathcal{S}}}\right\}$ of $N_{\mathcal{S}}$ simple slices shall be called a cover of $\mathcal{W}$ iff

$$\begin{array}{c}\mathcal{W}={\displaystyle \bigcup _{j\in {\mathfrak{I}}_{N_{\mathcal{S}}}}}{\mathcal{S}}_j\cap \mathcal{W}.\end{array}$$

(6)

We note that a cover $\mathfrak{S}$ partitions ${\mathcal{A}}_i\left(\mathcal{W}\right)$ into a set of regions ${\mathcal{W}}_{\mathcal{S}}^i,\mathcal{S}\in \mathfrak{S}$ each of which consists of zero or more individually connected but pairwise disjoint subsets ${\mathcal{C}}_{\mathcal{S},j}^i,j\in {\mathfrak{I}}_{N_{\mathfrak{L}}\left({\mathcal{W}}_{\mathcal{S}}^i\right)}$, which shall be referred to as workspace (or simple) cells. A cover $\widehat{\mathfrak{S}}=\left\{(i,{\mathfrak{S}}_i)|i\in {\mathfrak{I}}_{N_{\mathcal{R}}}\right\}$ of the configuration space $\mathfrak{C}$ is, respectively, defined by assigning a cover to each robot. Accordingly, a configuration space (or compound) slice $\widehat{\mathcal{S}}$ is defined as $\widehat{\mathcal{S}}=\left\{(i,{\mathcal{S}}_i)|i\in {\mathfrak{I}}_{N_{\mathcal{R}}}\right\}$, where ${\mathcal{S}}_i,i\in {\mathfrak{I}}_{N_{\mathcal{R}}}$ is a set of simple slices. Likewise, a configuration space cover $\widehat{\mathfrak{S}}=\left\{(i,{\mathfrak{S}}_i)|i\in {\mathfrak{I}}_{N_{\mathcal{R}}}\right\}$ induces a partitioning of $\mathfrak{C}$ into regions ${\widehat{\mathcal{W}}}_{\widehat{\mathcal{S}}}\triangleq {\mathcal{W}}_{{\mathcal{S}}_1}^1\times {\mathcal{W}}_{{\mathcal{S}}_2}^2\times \dots \times {\mathcal{W}}_{{\mathcal{S}}_{N_{\mathcal{R}}}}^{N_{\mathcal{R}}}$, where $\widehat{\mathcal{S}}=\left\{(i,{\mathcal{S}}_i)|i\in {\mathfrak{I}}_{N_{\mathcal{R}}}\right\}$ is an element of $\widehat{\mathfrak{S}}$. We note that each of these regions may consist of zero or more individually connected but pairwise disjoint subsets ${\widehat{\mathcal{C}}}_{\widehat{\mathcal{S}},i}^{},i\in {\mathfrak{I}}_{N_{\mathfrak{L}}\left({\mathfrak{C}}_{\widehat{\mathcal{S}}}\right)}$, which shall be referred to herein as configuration space (or compound) cells. Given the compound cell ${\widehat{\mathcal{C}}}_{\widehat{\mathcal{S}},i}^{}$, we will use ${\widehat{\mathcal{C}}}_{\widehat{\mathcal{S}},i}^{\left[j\right]}$ to denote its j-th component, i.e., ${\widehat{\mathcal{C}}}_{\widehat{\mathcal{S}},i}^{\left[j\right]}\triangleq {\mathcal{C}}_{{\mathcal{S}}_j,i}^j$, for all $j\in {\mathfrak{I}}_{N_{\mathcal{R}}}$. We remark that, unlike hierarchical decomposition schemes commonly encountered in the literature, which use cells of simple geometries (e.g., hypercubes or hyperrectangles), the configuration space cells considered in this work have, in general, arbitrary geometries because their components do not possess a pre-specified shape (see Figure 3). Although this choice renders navigation within a cell $\widehat{\mathcal{C}}$ more complicated, it generally results in coarser partitions because because each component ${\widehat{\mathcal{C}}}^{\left[i\right]}$ of $\widehat{\mathcal{C}}$ belongs in ${\mathcal{A}}_i\left(\mathcal{W}\right)$ by construction; thus, the subdivision scheme has to accommodate only for potential inter-robot collisions.

Regarding now the transition between configuration space cells, we introduce some required notions of connectedness. We begin with considering two distinct simple slices ${\mathcal{S}}_i$ and ${\mathcal{S}}_j$ which shall be called adjacent iff their intersection ${\mathcal{S}}_i\cap {\mathcal{S}}_j$ is not empty. Moreover, let ${\mathcal{C}}_{{\mathcal{S}}_m,i}^{}$ and ${\mathcal{C}}_{{\mathcal{S}}_n,j}^{}$ be two distinct workspace cells. We define these simple cells as adjacent iff ${\mathcal{C}}_{{\mathcal{S}}_m,i}^{}\cap {\mathcal{C}}_{{\mathcal{S}}_n,j}^{}\ne \varnothing$. Apparently, ${\mathcal{C}}_{{\mathcal{S}}_m,i}^{}$ and ${\mathcal{C}}_{{\mathcal{S}}_n,j}^{}$ being adjacent implies that ${\mathcal{S}}_m$ and ${\mathcal{S}}_n$ are also adjacent. The aforementioned definitions can be naturally extended to compound slices and cells, as well. Particularly, two compound slices ${\widehat{\mathcal{S}}}_m=\left\{(i,{\mathcal{S}}_{m,i})|i\in {\mathfrak{I}}_{N_{\mathcal{R}}}\right\}$ and ${\widehat{\mathcal{S}}}_n=\left\{(i,{\mathcal{S}}_{n,i})|i\in {\mathfrak{I}}_{N_{\mathcal{R}}}\right\}$ are adjacent iff ${\mathcal{S}}_{m,i},{\mathcal{S}}_{n,i}$ are adjacent, for all $i\in {\mathfrak{I}}_{N_{\mathcal{R}}}$, whereas two compound cells ${\widehat{\mathcal{C}}}_i^{}$ and ${\widehat{\mathcal{C}}}_j^{}$ are adjacent iff ${\widehat{\mathcal{C}}}_i^{\left[k\right]}$ and ${\widehat{\mathcal{C}}}_j^{\left[k\right]}$ are adjacent, for all $k\in {\mathfrak{I}}_{N_{\mathcal{R}}}$. A path $\mathsf{\Pi }$ of configuration space cells is defined as any finite string of sequentially adjacent compound cells. Obviously, a path $\mathsf{\Pi }$ consisting of cells that lie entirely in ${\mathfrak{C}}^f$ and contain both $P_{\mathrm{init}}$ and $P_{\mathrm{des}}$ is a valid solution to our path finding sub-problem. In order to discover such a path, we build a hierarchical decomposition $\mathfrak{H}=\left\{\left(i,\mathcal{H}\right)|i\in {\mathfrak{I}}_{N_{\mathcal{R}}}\right\}$ of the configuration space $\mathfrak{C}$ by assigning to each robot i a hierarchical partitioning of the workspace $\mathcal{W}$, represented as a connected, directed tree $\mathcal{H}\triangleq \left({\mathcal{N}}_{\mathcal{H}},{\mathcal{E}}_{\mathcal{H}}\right)$ such that:

• Each node $\mathcal{S}\in {\mathcal{N}}_{\mathcal{H}}$ is a simple slice.
• Every child ${\mathcal{S}}_j$ of a given node ${\mathcal{S}}_i$ (i.e., $\left({\mathcal{S}}_i,{\mathcal{S}}_j\right)\in {\mathcal{E}}_{\mathcal{H}}$) is a strict subset of ${\mathcal{S}}_i$.
• The set of leaf nodes must form a cover of $\mathcal{W}$.

Finally, an algorithm was devised for appropriately expanding $\mathfrak{H}$ until a solution is found, as described in the following subsection.

3.2. High-Level Planner

In this subsection, we present a high-level planner for finding a sequence $\mathsf{\Pi }$ of adjacent cells in ${\mathfrak{C}}^f$ connecting the initial $P_{\mathrm{init}}$ and goal $P_{\mathrm{des}}$ configurations. One of the main advantages of the proposed algorithm is the use of a suitable labeling scheme, which allows it to recursively subdivide, at each iteration, configuration space cells that lie on the boundary between ${\mathfrak{C}}^f$ and ${\mathfrak{C}}^o$ while ignoring cells that lie completely inside ${\mathfrak{C}}^f$ or ${\mathfrak{C}}^o$. To do so, this labeling scheme exploits the over- and under-approximations $\overline{{\mathcal{R}}_i}$ and $\underline{{\mathcal{R}}_i}$ of each robot’s footprint, defined in Section 3.1, to determine whether a robot may collide with another one while each robot navigates independently within its respective workspace cell. More specifically, given a compound cell ${\widehat{\mathcal{C}}}_{}^{}$, the employed cell labeling scheme works as follows:

• If the intersection of all $\overline{{\mathcal{R}}_i}\left({\widehat{\mathcal{C}}}^{\left[i\right]}\right),i\in {\mathfrak{I}}_{N_{\mathcal{R}}}$ is empty, then, by virtue of (4), no robot may come across another while $P\in {\widehat{\mathcal{C}}}_{}^{}$; thus, ${\widehat{\mathcal{C}}}_{}^{}$ is entirely contained in ${\mathfrak{C}}^f$. Such a compound cell is marked as admissible.
• If the intersection of all $\underline{{\mathcal{R}}_i}\left({\widehat{\mathcal{C}}}^{\left[i\right]}\right),i\in {\mathfrak{I}}_{N_{\mathcal{R}}}$ is non-empty, then, by virtue of (4), for every $P\in {\widehat{\mathcal{C}}}_{}^{}$ there exists at least one pair of intersecting robots; thus, ${\widehat{\mathcal{C}}}_{}^{}$ is entirely contained in ${\mathfrak{C}}^o$. Such a compound cell is marked as inadmissible.
• If ${\widehat{\mathcal{C}}}_{}^{}$ is neither admissible nor inadmissible, it is marked as mixed.

In general, mixed cells encapsulate both feasible and infeasible configurations and expanding them (recursively) should yield admissible and inadmissible subcells. On the other hand, by virtue of (5), the subdivision of admissible (resp., inadmissible) cells yields only admissible (resp., inadmissible) cells, without contributing any further in the configuration space’s exploration.

The planner’s main search algorithm is described in Algorithm 1, which initially constructs a coarse compound slice hierarchical partitioning made of each robot’s feasible set ${\mathcal{A}}_i\left(\mathcal{W}\right)$, thus enclosing all ${\mathfrak{C}}^f$ (functions InitializeHierarchy and InitializeCCells). Then, the initially computed compound cells get expanded until admissible ones, containing $P_{\mathrm{init}}$ and $P_{\mathrm{des}}$, are found (function FindEnclosingACCell), whereas the inability to find such compound cells indicates infeasibility of the given problem and the algorithm terminates. Next, an initial path $\mathsf{\Pi }$ connecting ${\widehat{\mathcal{C}}}_{\mathrm{init}}$ or ${\widehat{\mathcal{C}}}_{\mathrm{goal}}$ made of compound cells belonging to the exploration’s frontier set $S_{\widehat{\mathcal{C}},F}$ (i.e., the set of unexpanded admissible and mixed cells) is built (function ConnectStrings). At each iteration, the first mixed compound cell of $\mathsf{\Pi }$ (function GetFirstMixedCCell) is removed from the frontier and is expanded (function ExpandCCell) by subdividing the widest simple cell $\mathcal{C}$ whose over-approximation $\overline{{\mathcal{R}}_i}\left(\mathcal{C}\right)$ intersects with another (functions GetConflictingSCells and SelectSCellWithWidestSSlice) into smaller ones, as seen in Algorithm 2. Finally, a new path is constructed using standard back-tracking techniques until either $\mathsf{\Pi }$ consists only of admissible cells or no new path of mixed and admissible cells leading to $P_{\mathrm{des}}$ can be found.

Algorithm 1 Planner’s Main Algorithm

functionFindAPath( $P_{\mathrm{init}}$, $P_{\mathrm{des}}$ )     $\mathfrak{H}\leftarrow$ InitializeHierarchy     $S_{\widehat{\mathcal{C}}}\leftarrow$InitializeCCells($\mathfrak{H}$)     $S_{\widehat{\mathcal{C}},F}\leftarrow S_{\widehat{\mathcal{C}}}$     ${\widehat{\mathcal{C}}}_{\mathrm{init}}$, $\mathfrak{H}$, $S_{\widehat{\mathcal{C}}}$, $S_{\widehat{\mathcal{C}},F}$ ← FindEnclosingACCell( $P_{\mathrm{init}}$, $\mathfrak{H}$, $S_{\widehat{\mathcal{C}}}$, $S_{\widehat{\mathcal{C}},F}$)     ${\widehat{\mathcal{C}}}_{\mathrm{goal}}$, $\mathfrak{H}$, $S_{\widehat{\mathcal{C}}}$, $S_{\widehat{\mathcal{C}},F}$ ← FindEnclosingACCell( $P_{\mathrm{des}}$, $\mathfrak{H}$, $S_{\widehat{\mathcal{C}}}$, $S_{\widehat{\mathcal{C}},F}$)     if ${\widehat{\mathcal{C}}}_{\mathrm{init}}$ is $null$ or ${\widehat{\mathcal{C}}}_{\mathrm{goal}}$ is $null$ then         return $null$     end if     $\mathsf{\Pi }\leftarrow$ ConnectStrings( $\left[{\widehat{\mathcal{C}}}_{\mathrm{init}}\right]$, $\left[{\widehat{\mathcal{C}}}_{\mathrm{goal}}\right]$, $S_{\widehat{\mathcal{C}},F}$ )     while not (($\mathsf{\Pi }$ is $null$) or IsAdmissibleP($\mathsf{\Pi }$)) do         $\mathsf{\Pi }$, $\mathfrak{H}$, $S_{\widehat{\mathcal{C}}}$, $S_{\widehat{\mathcal{C}},F}$ ← ExpandPath( $\mathsf{\Pi }$, $\mathfrak{H}$, $S_{\widehat{\mathcal{C}}}$, $S_{\widehat{\mathcal{C}},F}$ )     end while     return $\mathsf{\Pi }$ end function functionFindEnclosingACCell( P, $\mathfrak{H}$, $S_{\widehat{\mathcal{C}}}$, $S_{\widehat{\mathcal{C}},F}$ ) Given a point P in the configuration space of the robotic system, this function identifies the cell in the hierarchy $\mathfrak{H}$ that contains that point and, if $S_{\widehat{\mathcal{C}}}$ is mixed, it iteratively subdivides $S_{\widehat{\mathcal{C}}}$ (modifying the hierarchy $\mathfrak{H}$ in the process) until it obtains a subcell $S_{\widehat{\mathcal{C}},F}$ of $S_{\widehat{\mathcal{C}}}$ that contains P and is admissible.     $\widehat{\mathcal{C}}\leftarrow$FindCCellContainingPos(P, $S_{\widehat{\mathcal{C}},F}$)     while $\widehat{\mathcal{C}}$ is not admissible) do         $i,\mathcal{S},\mathcal{C}\leftarrow$ SelectSCellWithWidestSSlice($\widehat{\mathcal{C}}$)         $S_{{\widehat{\mathcal{C}}}_R}$, $\mathfrak{H}$, $S_{\widehat{\mathcal{C}}}$, $S_{\widehat{\mathcal{C}},F}$ ←          ExpandCCell( $\widehat{\mathcal{C}}$, i, $\mathcal{S}$, $\mathcal{C}$, $\mathfrak{H}$, $S_{\widehat{\mathcal{C}}}$, $S_{\widehat{\mathcal{C}},F}$ )         $\widehat{\mathcal{C}}\leftarrow$FindCCellContainingPos(P, $S_{\widehat{\mathcal{C}},F}$)     end while     return $\widehat{\mathcal{C}}$, $\mathfrak{H}$, $S_{\widehat{\mathcal{C}}}$, $S_{\widehat{\mathcal{C}},F}$ end function functionConnectStrings( $\left[{\widehat{\mathcal{C}}}_{\mathrm{init}}\right]$, $\left[{\widehat{\mathcal{C}}}_{\mathrm{goal}}\right]$, $S_{\widehat{\mathcal{C}},F}$ ) This function corresponds to some standard graph search algorithm (e.g., depth-first, breadth-first, A*) which, given: (a) the initial $\left[{\widehat{\mathcal{C}}}_{\mathrm{init}}\right]$ and desired $\left[{\widehat{\mathcal{C}}}_{\mathrm{goal}}\right]$ compound cells (in case two paths of cells are passed as arguments, the last cell of the first path and the first cell of the second path are used as $\left[{\widehat{\mathcal{C}}}_{\mathrm{init}}\right]$ and $\left[{\widehat{\mathcal{C}}}_{\mathrm{goal}}\right]$, respectively), (b) a set of compound cells (nodes) $S_{\widehat{\mathcal{C}},F}$ and their adjacencies (edges), yields either a sequence of admissible and mixed cells connecting $\left[{\widehat{\mathcal{C}}}_{\mathrm{init}}\right]$ and $\left[{\widehat{\mathcal{C}}}_{\mathrm{goal}}\right]$ if such a path exists or null otherwise. end function

Algorithm 2 Path Expansion at Mixed Compound Cell

functionExpandPath( $\mathsf{\Pi }$, $\mathfrak{H}$, $S_{\widehat{\mathcal{C}}}$, $S_{\widehat{\mathcal{C}},F}$ )     $\widehat{\mathcal{C}}\leftarrow$GetFirstMixedCCell($\mathsf{\Pi }$)     ${\mathcal{L}}_{\mathrm{pre}}$, ${\mathcal{L}}_{\mathrm{suf}}$ ← SplitString($\mathsf{\Pi }$, $\widehat{\mathcal{C}}$)     $S_{\mathcal{C}}\leftarrow$ GetConflictingSCells($\widehat{\mathcal{C}}$)     $i,\mathcal{S},\mathcal{C}\leftarrow$ SelectSCellWithWidestSSlice($S_{\mathcal{C}}$)     $S_{{\widehat{\mathcal{C}}}_R}$, $\mathfrak{H}$, $S_{\widehat{\mathcal{C}}}$, $S_{\widehat{\mathcal{C}},F}$ ← ExpandCCell( $\widehat{\mathcal{C}}$, i, $\mathcal{S}$, $\mathcal{C}$, $\mathfrak{H}$, $S_{\widehat{\mathcal{C}}}$, $S_{\widehat{\mathcal{C}},F}$ )     $\mathsf{\Pi }\leftarrow$ ConnectStrings$({\mathcal{L}}_{\mathrm{pref}},{\mathcal{L}}_{\mathrm{suf}},S_{\widehat{\mathcal{C}},F})$     return $\mathsf{\Pi }$, $\mathfrak{H}$, $S_{\widehat{\mathcal{C}}}$, $S_{\widehat{\mathcal{C}},F}$ end function functionSplitString($\mathsf{\Pi }$, $\widehat{\mathcal{C}}$) Given a path $\mathsf{\Pi }$ and a specified cell $\widehat{\mathcal{C}}$ in $\mathsf{\Pi }$, return two subsequences ${\mathcal{L}}_{\mathrm{pre}}$ and ${\mathcal{L}}_{\mathrm{suf}}$ of $\mathsf{\Pi }$ such that $\mathsf{\Pi }$ = (${\mathcal{L}}_{\mathrm{pre}}$, $\widehat{\mathcal{C}}$, ${\mathcal{L}}_{\mathrm{suf}}$). end function functionSelectSCellWithWidestSSlice($S_{\mathcal{C}}$) Given a set of simple cells $S_{\mathcal{C}}$, return the simple cell in $S_{\mathcal{C}}$, the bounding box of which has the largest length or width. end function

3.3. Velocity Control Law

Given now a path $\mathsf{\Pi }$ consisting of $N_{\mathsf{\Pi }}$ admissible configuration space cells, we present a distributed control law for safely navigating from one cell to the next until the goal configuration $P_{\mathrm{des}}$ is reached. First, we consider two consecutive compound cells ${\widehat{\mathcal{C}}}_{\ell }^{}$ and ${\widehat{\mathcal{C}}}_{\ell +1}^{}$ in $\mathsf{\Pi }$, for which we compute the goal set ${\mathcal{G}}_{i,\ell }\triangleq {\widehat{\mathcal{C}}}_{\ell }^{\left[i\right]}\cap {\widehat{\mathcal{C}}}_{\ell +1}^{\left[i\right]}$ of each robot i, which contains feasible configurations in both ${\widehat{\mathcal{C}}}_{\ell }^{\left[i\right]}$ and ${\widehat{\mathcal{C}}}_{\ell +1}^{\left[i\right]}$ and is non-empty by construction. Respectively, the goal set corresponding to the last cell of $\mathsf{\Pi }$ consists of just the desired configuration $P_{\mathrm{des}}$, i.e., ${\widehat{\mathcal{C}}}_{N_{\mathsf{\Pi }}}^{}=\left\{P_{\mathrm{des}}\right\}$ (see Figure 4). Furthermore, let ${\mathcal{F}}_{\ell ,i}\triangleq {\widehat{\mathcal{C}}}_{\ell }^{\left[i\right]}\setminus \mathrm{int}\left({\mathcal{G}}_{\ell ,i}\right)$ and ${\mathcal{G}}_{\ell ,i}^{\prime }\triangleq {\mathcal{G}}_{\ell ,i}\cap {\mathcal{F}}_{\ell ,i}$, for all $k\in {\mathfrak{I}}_{N_{\mathsf{\Pi }}}$. Notice that ${\mathcal{G}}_{\ell ,i}^{\prime }$ is generally made of one or more pairwise disjoint subsets of arbitrary connectedness as well as that robot i should navigate to any of these regions without escaping ${\widehat{\mathcal{C}}}_{\ell }^{\left[i\right]}$ in order to successfully traverse to the next specified workspace cell. When more than one of such disjoint goal subsets are reachable from the connected component of ${\mathcal{F}}_{\ell ,i}$ that contains $p_i$, one of them is arbitrarily (though, deterministically) selected and assigned as the goal set; a more sophisticated approach for choosing goal regions could be employed, but it exceeds the scope of the current work. Respectively, the transition from ${\widehat{\mathcal{C}}}_{\ell }^{}$ to ${\widehat{\mathcal{C}}}_{\ell +1}^{}$ is considered complete after every robot i reaches ${\widehat{\mathcal{C}}}_{\ell +1}^{\left[i\right]}$. We also remark that when ${\widehat{\mathcal{C}}}_{\ell }^{\left[i\right]}\subseteq {\widehat{\mathcal{C}}}_{\ell +1}^{\left[i\right]}$, robot i simply needs to retain its current position during step ℓ.

In order to fulfill the aforementioned specifications, we equip each robot i with a controller $u_i$ based on suitable workspace transformations and adaptive artificial potential fields, which were originally presented in [27] and possess guaranteed domain invariance and almost-global convergence properties. More specifically, we build a diffeomorphic transformation $q_i^{\ell }=T_i^{\ell }\left(p_i\right)$ that maps ${\mathcal{F}}_{\ell ,i}$ to the unit disk $\mathcal{D}$, the outer boundary of ${\mathcal{F}}_{\ell ,i}$ to the unit circle $\partial \mathcal{D}$ and collapses all inner boundaries to distinct points $q_{i,j}^{\ell },j\in {\mathfrak{I}}_{N_i^{\ell }}$, where $N_i^{\ell }$ is the genus of ${\mathcal{F}}_{\ell ,i}$. We now distinguish the following two cases of possible goal sets: (a) ${\mathcal{G}}_{\ell ,i}^{\prime }$ being an inner boundary of ${\mathcal{F}}_{\ell ,i}$, and (b) ${\mathcal{G}}_{\ell ,i}^{\prime }$ being part of the outer boundary of ${\mathcal{F}}_{\ell ,i}$. Depending on the case, $T_i^{\ell }$ must be appropriately adapted to simplify the subsequent potential field’s design. Particularly, case (a) can be accommodated by modifying $T_i^{\ell }$ such that ${\mathcal{G}}_{\ell ,i}$ collapses to an inner point $q_{\mathrm{des},i}^{\ell }$ of $\mathcal{D}$, whereas case (b) is addressed by designing $T_i^{\ell }$ such that ${\mathcal{G}}_{\ell ,i}^{\prime }$ collapses to a single point $q_{\mathrm{des},i}^{\ell }$ on $\partial \mathcal{D}$. Next, we define the harmonic potential field ${\varphi }_i^{\ell }$ used by robot i during step ℓ by placing point harmonic sources upon the corresponding goal configuration $q_{\mathrm{des},i}^{\ell }$ and the transformed inner obstacles $q_{i,j}^{\ell }$, given by:

$${\varphi }_i^{\ell }=k_{i,d}^{\ell }ln\left(\frac{\parallel q_i^{\ell }-q_{\mathrm{des},i}^{\ell }\parallel }{2}\right)-\sum _{j\in {\mathfrak{I}}_{N_i^{\ell }}}{k}_{i,j}^{\ell }ln\left(\frac{\parallel q_i^{\ell }-q_{i,j}^{\ell }\parallel }{2}\right)$$

(7)

where $k_{i,d}^{\ell }>0$ and $k_{i,j}^{\ell }\ge 0$ are adaptively varying parameters. Finally, the control law $u_i^{\ell }$ of robot i during step ℓ is given by

$$u_i^{\ell }=-Ks(q_i^{\ell },k_{v,i}^{\ell }){\left(J_i^{\ell }\left(q_i^{\ell }\right)\right)}^{-1}{\nabla }_{q_i^{\ell }}{\psi }_i^{\ell }(q_i^{\ell },k_{v,i}^{\ell })$$

(8)

where K is a positive control gain, $J_i^{\ell }$ is the Jacobian matrix of $T_i^{\ell }$, s is a factor ensuring collision avoidance with the outer boundary and ${\psi }_i^{\ell }=1+tanh\left(w{\varphi }_i^{\ell }\right)/2$, with w a positive constant.

4. Simulation Results

In this section, we present the simulation results demonstrating the efficacy of the proposed methodology. As the contribution of this work is a path-planning algorithm, simulations were deemed sufficient to validate the efficacy of the proposed method. To conduct these simulations, the algorithm described in this work was implemented in C++, using Boost Geometry and CGAL for performing the geometric operations, such as the configuration computation, subdivision of cells, etc. The implementation can be found at the following url: https://github.com/maxchaos/mrnav, accessed on 20 December 2022. As described, the configuration space was partitioned into a tree of cells and a standard graph search algorithm was employed for finding admissible paths of mixed cells connecting the initial and goal configurations. After such a path was obtained, for each compound cell in the sequence, we extracted the simple cells corresponding to each robot, and for each robot, we employed the low-level control scheme to drive it to the border of the next cell or its goal configuration in case the current cell was the last of the path. Particularly, we consider five scenarios where a system consisting of 2, 4, 6, 8 and 10 robots, respectively, initialized within the workspace depicted in Figure 5 is requested to reach a specified final configuration. The time required by the proposed planner, as well as the total amount of compound cells generated during the solution of each case, are shown in

Table 1. Execution time, total length of the robots’ paths and number of generated compound cells required by the high-level planner for solving each scenario.

Number of Robots	2	4	6	8	10
Time (sec)	0.088	0.240	0.845	1.36	31.1
Length (dm)	404.94	1076.23	1396.20	1739.53	2125.36
Compound Cells	51	348	823	1014	3363

. We remark that the planner expanded the mixed compound cells by subdividing the corresponding conflicting simple slice into four identical overlapping sub-slices. The motion profiles executed by the robots in each corresponding case can be seen in Figure 5, Figure 6, Figure 7, Figure 8 and Figure 9. Additionally, Figure 10 and Figure 11 depict the initial and goal configurations as well as the computed enclosing cells, respectively, for the eight-robot scenario. As one can verify from the figures, the robots navigate successfully to their individual goals. A video of the aforementioned simulated scenarios can be found at the following url: https://youtu.be/asWnKLNX2Eg, accessed on 20 December 2022.

To strengthen our theoretical findings, we also conducted a comparative simulation study over the same scenarios employing a state-of-the-art high-level planner called RRT* [4] that is based on a probabilistic sampling algorithm to extract feasible paths toward the goal configuration. Because RRT* is probabilistic, we ran 50 trials on each scenario and extracted the mean value of the execution time and total path as described in

Table 2. Average and standard deviation for the execution time and total length of the robots’ paths over the number of successful runs yielded by the RRT* planner [4] for solving each scenario over 50 runs.

Number of Robots	2	4	6	8	10
Time-avg (sec)	0.041	0.124	12.375	724.136	961.230
Time-std (sec)	0.001	0.045	0.454	30.032	41.211
Length-avg (dm)	380.15	1112.21	1256.82	1712.45	1995.63
Length-std (dm)	1.25	5.18	7.32	15.82	17.31
Successful runs	50	50	49	36	5

. We have to stress that owing to the narrow passages of the workspace, as the number of robots increased (particularly for the cases of 8 and 10 robots), the RRT* algorithm reached the maximum terminating condition (an execution time more than 1000 s) very frequently (many runs did not discover feasible paths) without providing feasible solutions. Notice that our algorithm is superior in terms of the execution time and completeness without however sacrificing the performance in terms of the total path length. It should be pointed out that although our algorithm does not incorporate any optimization in either the path calculation or the transition execution, the achieved performance is comparable to the RRT* algorithm, which finds the shortest path among multiple feasible ones.

5. Conclusions

In this work, we presented a hybrid control scheme for addressing the navigation problem of a team of robots operating within an obstacle-cluttered planar workspace. Particularly, a high-level planner was designed for computing a sequence of feasible cells by adaptively subdividing the system’s configuration space using a hierarchical cell decomposition scheme. In addition, a low-level control law based on harmonic potentials and workspace transformations was employed to implement the given plan, safely navigating each robot to its specified goal. The contribution lies on the adaptive subdivision algorithm for obtaining a sequence of traversable cells connecting the given initial and final configurations or determining that no solution exists. Unlike standard grid/cell-based methods, the proposed algorithm adaptively subdivides the cells, requiring no selection of some arbitrary resolution by the user. Moreover, unlike probabilistic sampling methods, the proposed method does not generate paths of configuration but rather sequences of safe sets, allowing the user to make use of a larger set of controllers in addition to standard trajectory tracking schemes. In addition, unlike probabilistic sampling methods which are inherently incapable of determining the infeasibility of a given problem (such algorithms require the user to specify an arbitrary upper bound of samples after which the algorithms abort) as a consequence of their probabilistic completeness nature and the fact that sample points should lie within the configuration space, our algorithm partitions the configuration space into the cells (dense subsets) of the configuration space and is capable of determining when no admissible path exists in finite time. Finally, comparative simulation studies were conducted for various scenarios with a state-of-the-art algorithm that validates the proposed scheme’s efficacy and demonstrates its superiority in terms of complexity while yielding a satisfactory performance in terms of path lengths.

Regarding the limitations of the proposed approach and future research directions toward healing them, we aim to improve the proposed planner by accommodating for redundant steps appearing in the computed path and adapt it for use in a more distributed fashion, i.e., resolve the conflicts between antagonistically operating robots as they appear. Additionally, introducing optimality in the selection of the sequence of cells as well as in the execution of the transitions deserves further investigation to improve the efficiency of the proposed method. In the same vein, increasing the dimensionality of the problem (considering 3D workspaces and potentially the orientation) would also favor its applicability. Alternatively, a modern approach such as deep reinforcement learning [28] will be sought to examine new ways of solving the multi-robot coordination problem.