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

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.


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 overand 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 I N {1, 2, . . . , N} (resp., I N {0} ∪ 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 ∂A, int(A) and cl(A) to denote the boundary, interior and closure of A, respectively, and A \ B to denote the complement of B with respect to A.

Problem Formulation
We consider a team of N R robots operating within a compact planar workspace W ⊆ R 2 occupied by a set of N o disjoint, fixed inner obstacles O i , i ∈ I N o . We assume that each robot i has a disk-shaped body R i ⊂ R 2 with radius r i > 0. Let F w and F i , i ∈ I N R be the coordinate frames arbitrarily embedded in W and R i , i ∈ I N R , respectively. We shall refer to the origin of each F i , i ∈ I N 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 x i , y i T ∈ R 2 denote the relative position of i-th robot's reference point with respect to the workspace's coordinate frame F w , and let R i p denote its footprint, i.e., the space occupied by robot i when placed at position p. Throughout this work, we shall use C ⊆ R 2N R to denote the robotic system's configuration space and P î p T 1 , p T 2 , . . . , p T N R ó T ∈ C to denote the stacked vector of robot positions. For the sake of brevity, we shall also use P[i] to denote the i-th component of P, i.e., P[i] = p i . Let W o denote the complement of W, i.e., W o R 2 \ W. We also define a configuration P as feasible iff the following conditions hold: and we shall use C f ⊂ C to denote the set of all feasible configurations of the robotic system, whereas its complement C o C \ 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:ṗ where u i denotes the control input. Problem: Let P init and P des be two given feasible configurations of the multi-robot system that belong to the same segment of C f . Our goal is to design a control scheme that drives any robot i, initialized at p init,i = P init [i], to the specified desired position p des,i = P des [i], while avoiding inter-robot and robot-workspace collisions, i.e., P(t) ∈ C f for all t ≥ 0.

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 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 init and P 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.

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 A i (W), corresponds to the largest subset of W where the reference point of robot i can be placed such that R i p i ∩ W o = ∅, for all p i ∈ A i (W). Moreover, given a subset Z of W, we shall use A i (Z) to denote the set of feasible positions of robot i which belong to Z, i.e., In addition to A i (·), which corresponds to the set of feasible positions of robot i, we also consider two estimations of the area that is potentially occupied by R i when p i is restricted in a subset Z of W. Particularly, given a robot i ∈ I N R and a set Z ⊆ A i (W), let R i (Z) and R i (Z) be an over-approximation and under-approximation, respectively, of the footprint of R i when robot i is swept over Z such that: and An example of such approximations can be seen in Figure 2. (for disk-shaped robots, a valid over-approximation R i (Z) can be computed by offsetting Z by r i , whereas R i (Z) can be calculated by ∩ p∈∂Z R i p ).

Figure 2.
Over-approximation R i (Z) (resp., R i (Z )) and under-approximation R i (Z) (resp., R i (Z )) of the footprint of robot i when swept over Z ⊂ R 2 (resp., Z).
We now consider a set S ⊆ R 2 that has the form [x 1 , x 2 ] × [y 1 , y 2 ]. We shall refer to such a set as a simple slice of R 2 . Given a simple slice S and a robot i ∈ I N R , we will use We note that a cover S partitions A i (W) into a set of regions W i S , S ∈ S each of which consists of zero or more individually connected but pairwise disjoint subsets C i S,j , j ∈ I N L( W i S ) , which shall be referred to as workspace (or simple) cells. A cover " S = (i, S i ) | i ∈ I N R of the configuration space C is, respectively, defined by assigning a cover to each robot. Accordingly, a configuration space (or compound) slice S is defined as S. We note that each of these regions may consist of zero or more individually connected but pairwise disjoint subsets C 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 C more complicated, it generally results in coarser partitions because because each component C [i] of C belongs in A i (W) by construction; thus, the subdivision scheme has to accommodate only for potential inter-robot collisions. Example of a hierarchical configuration space decomposition for a system of two identical robots. The green area corresponds to the configuration space A i (W) of each robot. For the sake of simplicity, we assume H 1 = H 2 . The workspace slice S 1 corresponding to robot 1 consists of a single simple cell (C S 1 ) whereas slice S 2 corresponding to robot 2 consists of two cells (C S 2 ,1 , C S 2 ,2 ). The compound cell (C S 1 , C S 2 ,1 ) is labeled as mixed because R 1 and R 2 may intersect when p 1 ∈ C S 1 and p 2 ∈ C S 2 ,2 because R 1 (C S 1 ) ∩ R 2 (C S 2 ,1 ) = ∅, whereas (C S 1 , C S 2 ,2 ) is marked as admissible.
Regarding now the transition between configuration space cells, we introduce some required notions of connectedness. We begin with considering two distinct simple slices S i and S j which shall be called adjacent iff their intersection S i ∩ S j is not empty. Moreover, let C S m ,i and C S n ,j be two distinct workspace cells. We define these simple cells as adjacent iff C S m ,i ∩ C S n ,j = ∅. Apparently, C S m ,i and C S n ,j being adjacent implies that S m and S n are also adjacent. The aforementioned definitions can be naturally extended to compound slices and cells, as well. Particularly, two compound slices S m = (i, S m,i ) | i ∈ I N R and S n = (i, S n,i ) | i ∈ I N R are adjacent iff S m,i , S n,i are adjacent, for all i ∈ I N R , whereas two compound cells C i and C j are adjacent iff C [k] i and C [k] j are adjacent, for all k ∈ I N R . A path Π of configuration space cells is defined as any finite string of sequentially adjacent compound cells. Obviously, a path Π consisting of cells that lie entirely in C f and contain both P init and P des is a valid solution to our path finding sub-problem. In order to discover such a path, we build a hierarchical decomposition H = (i, H) | i ∈ I N R of the configuration space C by assigning to each robot i a hierarchical partitioning of the workspace W, represented as a connected, directed tree H (N H , E H ) such that: • Each node S ∈ N H is a simple slice. • Every child S j of a given node S i (i.e., The set of leaf nodes must form a cover of W. Finally, an algorithm was devised for appropriately expanding H until a solution is found, as described in the following subsection.

High-Level Planner
In this subsection, we present a high-level planner for finding a sequence Π of adjacent cells in C f connecting the initial P init and goal P 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 C f and C o while ignoring cells that lie completely inside C f or C o . To do so, this labeling scheme exploits the over-and under-approximations R i and 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 C , the employed cell labeling scheme works as follows: is empty, then, by virtue of (4), no robot may come across another while P ∈ C ; thus, C is entirely contained in C f . Such a compound cell is marked as admissible.

•
If the intersection of all R i Ä C [i] ä , i ∈ I N R is non-empty, then, by virtue of (4), for every P ∈ C there exists at least one pair of intersecting robots; thus, C is entirely contained in C o . Such a compound cell is marked as inadmissible.

•
If 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 A i (W), thus enclosing all C f (functions INITIALIZEHIERARCHY and INITIALIZECCELLS). Then, the initially computed compound cells get expanded until admissible ones, containing P init and P 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 Π connecting C init or C goal made of compound cells belonging to the exploration's frontier set S 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 Π (function GETFIRSTMIXEDCCELL) is removed from the frontier and is expanded (function EXPANDCCELL) by subdividing the widest simple cell C whose over-approximation R i (C) 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 Π consists only of admissible cells or no new path of mixed and admissible cells leading to P des can be found.

Algorithm 1 Planner's Main Algorithm
Π, H, S C , S C,F ← EXPANDPATH( Π, H, S C , S C,F ) end while return Π end function function FINDENCLOSINGACCELL( P, H, S C , S C,F ) Given a point P in the configuration space of the robotic system, this function identifies the cell in the hierarchy H that contains that point and, if S C is mixed, it iteratively subdivides S C (modifying the hierarchy H in the process) until it obtains a subcell S C,F of S C that contains P and is admissible.

Velocity Control Law
Given now a path Π consisting of N Π admissible configuration space cells, we present a distributed control law for safely navigating from one cell to the next until the goal configuration P des is reached. First, we consider two consecutive compound cells C and C +1 in Π, for which we compute the goal set G i, +1 and is non-empty by construction. Respectively, the goal set corresponding to the last cell of Π consists of just the desired configuration P des , i.e., C N Π = {P des } (see Figure 4). Furthermore, let F ,i C [i] \ int G ,i and G ,i G ,i ∩ F ,i , for all k ∈ I N Π . Notice that G ,i 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 C [i] 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 F ,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 C to C +1 is considered complete after every robot i reaches C [i] +1 . We also remark that when C [i] ⊆ C [i] +1 , robot i simply needs to retain its current position during step .

Algorithm 2 Path Expansion at Mixed Compound Cell
Given a path Π and a specified cell C in Π, return two subsequences L pre and L suf of Π such that Π = (L pre , C, L suf ). end function function SELECTSCELLWITHWIDESTSSLICE(S C ) Given a set of simple cells S C , return the simple cell in S C , the bounding box of which has the largest length or width. end function Figure 4. Two adjacent compound cells C 1 = (C 1 1 , C 2 1 ) and C 2 = (C 1 2 , C 2 2 ). In order for robot 1 (resp., robot 2) to successfully move from C 1 1 to C 1 2 (resp., from C 2 1 to C 2 2 ), it has to reach any point of G 1,1 (resp., G 2,1 ).
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 = T i (p i ) that maps F ,i to the unit disk D, the outer boundary of F ,i to the unit circle ∂D and collapses all inner boundaries to distinct points q i,j , j ∈ I N i , where N i is the genus of F ,i . We now distinguish the following two cases of possible goal sets: (a) G ,i being an inner boundary of F ,i , and (b) G ,i being part of the outer boundary of F ,i . Depending on the case, T i must be appropriately adapted to simplify the subsequent potential field's design. Particularly, case (a) can be accommodated by modifying T i such that G ,i collapses to an inner point q des,i of D, whereas case (b) is addressed by designing T i such that G ,i collapses to a single point q des,i on ∂D. Next, we define the harmonic potential field φ i used by robot i during step by placing point harmonic sources upon the corresponding goal configuration q des,i and the transformed inner obstacles q i,j , given by: where k i,d > 0 and k i,j ≥ 0 are adaptively varying parameters. Finally, the control law u i of robot i during step is given by where K is a positive control gain, J i is the Jacobian matrix of T i , s is a factor ensuring collision avoidance with the outer boundary and ψ i = 1 + tanh(wφ i )/2, with w a positive constant.

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. We remark that the planner expanded the mixed compound cells by subdividing the corresponding conflicting simple slice into four identical overlapping subslices. The motion profiles executed by the robots in each corresponding case can be seen in Figures 5-9. Additionally, Figures 10 and 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. 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.

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.