Next Article in Journal
Group Target Tracking for Highly Maneuverable Unmanned Aerial Vehicles Swarms: A Perspective
Next Article in Special Issue
A Convex Optimization Approach to Multi-Robot Task Allocation and Path Planning
Previous Article in Journal
Computationally Efficient Continuous-Time Model Predictive Control of a 2-DOF Helicopter via B-Spline Parameterization
Previous Article in Special Issue
Multi-Agent Deep Reinforcement Learning for Multi-Robot Applications: A Survey
 
 
Font Type:
Arial Georgia Verdana
Font Size:
Aa Aa Aa
Line Spacing:
Column Width:
Background:
Article

Robot Navigation in Complex Workspaces Employing Harmonic Maps and Adaptive Artificial Potential Fields

by
Panagiotis Vlantis
1,
Charalampos P. Bechlioulis
1,* and
Kostas J. Kyriakopoulos
2
1
Department of Electrical and Computer Engineering, University of Patras, 26504 Patras, Greece
2
School of Mechanical Engineering, National Technical University of Athens, 15780 Athens, Greece
*
Author to whom correspondence should be addressed.
Sensors 2023, 23(9), 4464; https://doi.org/10.3390/s23094464
Submission received: 17 April 2023 / Revised: 29 April 2023 / Accepted: 2 May 2023 / Published: 3 May 2023
(This article belongs to the Special Issue Mobile Robots: Navigation, Control and Sensing)

Abstract

:
In this work, we address the single robot navigation problem within a planar and arbitrarily connected workspace. In particular, we present an algorithm that transforms any static, compact, planar workspace of arbitrary connectedness and shape to a disk, where the navigation problem can be easily solved. Our solution benefits from the fact that it only requires a fine representation of the workspace boundary (i.e., a set of points), which is easily obtained in practice via SLAM. The proposed transformation, combined with a workspace decomposition strategy that reduces the computational complexity, has been exhaustively tested and has shown excellent performance in complex workspaces. A motion control scheme is also provided for the class of non-holonomic robots with unicycle kinematics, which are commonly used in most industrial applications. Moreover, the tuning of the underlying control parameters is rather straightforward as it affects only the shape of the resulted trajectories and not the critical specifications of collision avoidance and convergence to the goal position. Finally, we validate the efficacy of the proposed navigation strategy via extensive simulations and experimental studies.

1. Introduction

The navigation of autonomous robots in cluttered environments is a widely studied topic in the field of robotics. Popular methodologies that have been employed in the related literature to address it include, but are not limited to, configuration space decomposition approaches [1,2]; probabilistic sampling methods such as rapidly exploring random trees [3,4], probabilistic roadmaps [5,6] and manifold samples [7,8]; and optimal control strategies such as receding horizon control [9,10] and path homotopy invariants [11,12]. Apart from the aforementioned discrete methods regarding the workspace and/or the decision domain, Artificial Potential Fields (APFs) that were originally introduced in [13] generally provide a simpler means of encoding collision avoidance specifications, with their negated gradient functioning as a reference motion direction that drives the robot towards the desired goal configuration. As shown in [14], despite their intuitive nature, this class of controllers suffers unavoidably from the presence of unwanted equilibria induced by the workspace’s topology, whose region of attraction may not be trivial. In their seminal work [15], Rimon and Koditschek presented a family of APFs called Navigation Functions (NFs) for point and sphere worlds, as well as a constructive transformation for mapping workspaces cluttered by sequences of star-shaped obstacles into such worlds. However, certain design parameters require tedious tuning to eliminate unwanted local minima and render the transformation a diffeomorphism. In practice, this solution suffers by the fact that the allowable values of the design parameters may cause both the potential and the corresponding transformation to vary too abruptly close to the obstacles (the issue of “disappearing valleys” [15]), thus pushing the trajectories of the robot very close to them. Density functions for remedying such drawbacks or adjustable NFs for relaxing some generally conservative requirements are presented in [16,17]. Additionally, attempts to extend the NF framework directly to non-sphere worlds can be found in [18,19]. Finally, a novel approach based on power diagrams which can be used for designing tune-free vector fields for navigation within convex workspaces is also presented in [20].
Artificial Harmonic Potential Fields (AHPFs) constitute an interesting subclass of APFs, since they are free of unwanted local minima by construction. However, no simple method exists for constructing safe (with respect to obstacle avoidance), harmonic potentials even for simple workspaces. AHPFs suitable for navigation in realistic environments were originally utilized in [21], where computationally expensive numerical techniques were employed to solve the associated Dirichlet and Neumann problems. Several extensions of the aforementioned methodology followed [22,23], addressing issues such as numerical precision and computation, dynamic environments, etc. The panel method was also employed in [24,25,26] to build harmonic potentials to coordinate the motion of single and multiple robots in polygonal environments. In [27,28], well-known closed-form solutions of the uncompressed fluid flow around simple geometries was used in order to safely drive a robot among moving obstacles. Harmonic potential fields have also been used in [29,30] to address the Simultaneous Localization and Mapping problem (SLAM) by coordinating the robot motion in unknown environments. Moreover, a methodology based on the evaluation of the harmonic potential field’s streamlines was used in [31,32] for mapping a multiply connected workspace to a disk, collapsing inner obstacles to line segments or arcs. In a recent work [33], the problem of designing closed form harmonic potentials in sphere worlds was addressed by the introduction of a diffeomorphism [34], which allows mapping such workspaces to the euclidean plane with some of its points removed. Finally, extensions of this work addressing topologically complex three-dimensional workspaces or multi-robot scenarios by introducing appropriate constructive workspace transformations can be found in [35,36], respectively.

1.1. Contributions

We address the navigation problem for a robot operating within a static, compact, planar workspace of arbitrary connectedness and shape by designing a control law that safely drives the robot to a given goal position from almost any initial feasible configuration. The goal of this work is twofold. (A) To cope with the topological complexity of the workspace, we employed numerical techniques in order to build a transformation that maps the workspace onto a punctured disk and delved into the respective construction in detail. We remark that, although the transformation constructed using this method is an approximation of a harmonic map ideal for navigation, our solution benefits from the fact that it only needs a sufficiently fine polygonal workspace description that can be easily acquired in practice (e.g., through SLAM), contrary to [15,34,36] that require an explicit representation of the workspace boundaries (i.e., as the level sets of sufficiently smooth functions). Moreover, unlike the solutions proposed in [15,36], our approach does not require the decomposition of the workspace obstacles into sequences of simpler overlapping shapes and computes the desired transformation in one step. (B) To steer the robot to its desired configuration, we employed a control law based on closed-form AHPFs coupled with adaptive laws for their parameters to eliminate the necessity of explicitly defined local activation neighborhoods around the workspace boundaries for ensuring collision avoidance. Our approach is reactive (closed loop) since it selects the velocity of the robot based on the positions of the robot, the desired goal and the workspace boundary. As such, it is more robust against position measurement errors than other open loop approaches such as configuration space decomposition approaches [2] or probabilistic sampling methods such as rapidly exploring random trees [4], probabilistic roadmaps [6] and manifold samples [8], where an open loop path is initially extracted and executed by a trajectory tracking controller. In this way, even small position errors risk the safe execution of the calculated plan. We remark that our overall control scheme only requires solving a computationally expensive problem once for a given static workspace, independent of the robot’s initial and goal configurations, in contrast to the solutions presented in [21,22]. Finally, we adapt our methodology to the class of differential drive robots, which are commonly encountered in real-world applications and propose an algorithm that decomposes the overall workspace into small neighbouring subsets to render the problem of addressing large workspaces tractable. An overview of the proposed methodology’s pros and cons compared to alternative transformations and potential fields can be seen in Table 1 and Table 2, respectively.
Preliminary results were included in our conference paper [37]. We have to stress though that the algorithmic calculation of the harmonic map is given in the present work, along with a rigorous formulation of the panel method. A modification of the adaptive laws for the parameters of the underlying potential field is also introduced to simplify the tuning process by eliminating the necessity of heuristically defined local activation neighborhoods around the workspace boundaries for ensuring collision avoidance. Moreover, an extension for tackling the navigation problem under unicycle kinematics is also provided. Finally, new comparative simulation results are provided to highlight the strong points of the proposed method with respect to other related works, accompanied by an experiment employing an actual robot navigating within a complex office workspace.

1.2. Preliminaries

We use D r ( x ) to denote an open disk with radius r > 0 centered at x R 2 . Additionally, D and D denote the closed disk and circle with unit radii centered at the origin of R 2 , respectively. Furthermore, let I N { 1 , 2 , , N } and I N { 0 } I N . Given sets A , B R n , we use cl A , A , int A and A ¯ to denote the closure, boundary, interior and complement of A with respect to R n , respectively, and A \ B to denote the complement of B with respect to A. Furthermore, we use 0 N and 1 N to denote the all-zeros and all-ones column vectors of length N, respectively, and 0 N × M to denote the N × M zero matrix. We also define 1 N × M k , k I M as the N × M matrix whose k-th column is equal to 1 N and every other column is equal to 0 N . Given a vector function f ( x ) , we use x f to denote its Jacobian matrix. Furthermore, given an arc C, we use | C | to denote its length. We will also say that a set A is attractive (repulsive) under a potential function ψ when there exists a point p 0 cl A such that if we initialize at p 0 and move along the negated gradient of ψ , we will converge (not converge) to A . Finally, a potential function ψ is called harmonic if it satisfies the Laplace equation, i.e., 2 ψ = 0 , where 2 denotes the Laplacian operator. An important property of harmonic functions is the principle of superposition, which follows from the linearity of the Laplace equation. Moreover, the extrema of a non-constant harmonic function occur on the boundary of the domain of definition, thus excluding any local minima/maxima within it (a desirable property for motion planning).

2. Problem Formulation

We consider a robot operating within a compact workspace W R 2 bounded by a single outer and a finite set of inner disjoint Jordan curves (a Jordan curve is a non-self-intersecting continuous planar closed curve), which correspond to the boundaries of static obstacles. It is assumed that W can be written as:
W = W 0 ¯ \ i I N W i
where W i , i I N denote regions of R 2 that the robot cannot occupy (see left subplot in Figure 1). Particularly, the complement of W 0 is considered to be a bounded, simply connected region that may also include a strict subset of its own boundary (this corresponds to cases when we wish to place the robot’s goal configuration on some part of the workspace outer boundary which is not physically occupied by an actual obstacle, e.g., the door of a compartment (refer to Section 5.2 for more details)) and W 1 , W 2 , …, W N are assumed to be closed, simply connected compact sets that are contained in W 0 ¯ and are pairwise disjoint. Let p = [ x , y ] T R 2 denote the robot’s position and assume that the robot’s motion is described by the single integrator model:
p ˙ = u
where u R 2 is the corresponding control input vector.
Problem 1. 
Our goal is to design a control law to successfully drive a robot with kinematics (1) towards a given goal configuration p d W from almost any feasible initial configuration p init W , while ensuring collision avoidance, i.e., p ( t ) W for all t 0 .
Remark 1. 
The results presented in this work can be readily employed for the navigation of disk robots with radius R > 0 by appropriately augmenting the workspace boundaries with the robot’s size.

3. Harmonic Maps for Planar Navigation

In this section, we present a methodology that maps the robot’s workspace onto a punctured unit disk, over which the robot’s control law is designed. Particularly, our goal is to construct a transformation, T : cl W D , from the closure of the robot’s configuration space cl W onto the unit disk D with the following properties:
  • T ( · ) maps the outer boundary W 0 to the unit circle D ;
  • T ( · ) maps the boundary W i , i I N of each obstacle to a distinct point q i = [ u i , v i ] T int D ;
  • T ( · ) is a diffeomorphism for all p int W .
To that end, we compute a transformation T ˜ ( p ) = [ u ˜ ( p ) , v ˜ ( p ) ] T , with u ˜ ( p ) and v ˜ ( p ) being harmonic functions with respect to p, by approximating an ideal harmonic map T that meets the aforementioned properties and the existence of which was proven in Theorem 2 of [38], accompanied by sufficient conditions that render it a diffeomorphism as outlined in the corresponding proof.
Particularly, this theorem implies that given an orientation-preserving, weak homeomorphism T : W 0 D (such a transformation can be easily obtained for any given planar Jordan curve C by (1) arbitrarily selecting a point p o on C, (2) defining ( p ) , p C as the length of the arc p o p ^ , assuming one travels from p o to p on C while having the curve’s interior to its left and (3) choosing T ( p ) = [ cos ( 2 π ( p ) / L ) , sin ( 2 π ( p ) / L ) ] T , where L = | C | ) from the workspace outer boundary W 0 to the boundary of the unit disk, the harmonic map T that satisfies the conditions:
T ( p ) = T ( p ) [ u ¯ ( p ) , v ¯ ( p ) ] T , p W 0 ,
W i T n p d s = 0 , i I N
with n p denoting the unit vector that is normal to the boundary at the point p W i , i I N , is a diffeomorphism that maps cl W to the target set and collapses each inner obstacle W i onto a distinct point q i within its interior (see Figure 1). Note that the coordinates of q i , i.e., the images of the internal obstacles, are not explicitly specified but are computed as part of the solution, such that the aforementioned constraints are satisfied.
Given that closed-form solutions to the aforementioned problem are generally not available for non-trivial domains, in this work, we employed numerical techniques and particularly the Panel Method [24,39,40] (similar formulations can be obtained by employing other numerical techniques such as the Boundary Element Method (BEM), the Finite Element Method (FEM) or the Finite Differences Method (FDM)) in order to construct a harmonic map T ˜ that sufficiently approximates T. As such, by subdividing separately the workspace’s outer and inner boundaries into M ˜ 0 , M ˜ 1 , , M ˜ N number of elements (see Figure 2), we define the components of T ˜ ( p ) = [ u ˜ ( p ) , v ˜ ( p ) ] T as follows:
u ˜ ( p ) = i = 0 N j = 1 M ˜ i l = 1 M ˜ C C ˜ i j l x H ˜ i j l ( p ) v ˜ ( p ) = i = 0 N j = 1 M ˜ i l = 1 M ˜ C C ˜ i j l y H ˜ i j l ( p )
H ˜ i j l ( p ) = E ˜ i j G ˜ i j l s ln ( p p ˜ i , j ( s ) ) d s
where M ˜ C is the number of control parameters per element, E ˜ i j denotes the j-th element of the i-th boundary’s approximation, p ˜ i , j ( s ) : [ 0 , | E ˜ i j | ] E ˜ i j is a bijective parameterization of E ˜ i j , G ˜ i j l : [ 0 , | E ˜ i j | ] R is the shape function corresponding to the l-th control parameter of E ˜ i j and C ˜ i j l x , C ˜ i j l y R are control parameters that need to be appropriately selected so that T ˜ satisfies properties 1–3 for all l I M ˜ C , j I M ˜ i and i I N . It is worth noting that for common choices of G ˜ i j l (e.g., constant or linear shape functions) and simple types of E ˜ i j (e.g., line segments), the integral in (4) can be easily evaluated to obtain a closed-form expression for H ˜ i j l . As an illustration, for a line segment element E ˜ i j with two control parameters (i.e., M ˜ C = 2 ), a typical choice for linear shape functions (see Figure 2) is G ˜ i j 1 ( s ) = s / | E ˜ i j | , G ˜ i j 2 ( s ) = 1 s / | E ˜ i j | and p ˜ i , j ( s ) = p ˜ i , j , A + p ˜ i , j , B p ˜ i , j , A s / | E ˜ i j | for the corresponding parameterization, where p ˜ i , j , A , p ˜ i , j , B are the element’s end-points. To obtain the unknown control parameters as well as the images of the workspace’s inner obstacles, one needs to solve two independent linear systems of equations:
A ˜ X ˜ = B ˜ x , A ˜ Y ˜ = B ˜ y
for the unknown vectors:
X ˜ = C ˜ 0 , 1 , 1 x , , C ˜ 1 , 1 , 1 x , , C ˜ N , M ˜ N , M ˜ C x , u 1 , , u N T Y ˜ = C ˜ 0 , 1 , 1 y , , C ˜ 1 , 1 , 1 y , , C ˜ N , M ˜ N , M ˜ C y , v 1 , , v N T .
The matrix A ˜ and the right hand side vectors B ˜ x and B ˜ y are constructed by selecting a set of i I N m ˜ i arbitrary points p ˜ i , j (a typical strategy is to select the points p ˜ i , j uniformly on the outer and inner boundaries of the given domain) such that (a) p ˜ i , j W i for all j I m ˜ i and i I N and (b) i I N m ˜ i = M ˜ C i I N M ˜ i , and evaluating (2) and (3) at those points as follows:
A ˜ = A ˜ 0 , 0 m ˜ 0 × N A ˜ 1 , 1 m ˜ 1 × N 1 A ˜ N , 1 m ˜ N × N N A ˜ , 0 N × N , B ˜ x = B ˜ x , 0 0 m ˜ 1 0 m ˜ N 0 N , B ˜ y = B ˜ y , 0 0 m ˜ 1 0 m ˜ N 0 N
A ˜ k = H ˜ 0 , 1 , 1 ( p ˜ k , 1 ) H ˜ N , M ˜ N , M ˜ C ( p ˜ k , 1 ) H ˜ 0 , 1 , 1 ( p ˜ k , 2 ) H ˜ N , M ˜ N , M ˜ C ( p ˜ k , 2 ) H ˜ 0 , 1 , 1 ( p ˜ k , m ˜ k ) H ˜ N , M ˜ N , M ˜ C ( p ˜ k , m ˜ k ) , k I N
A ˜ = k = 1 m ˜ 1 H ˜ 0 , 1 , 1 n 0 , 1 ( p ˜ 1 , k ) k = 1 m ˜ 1 H ˜ N , M ˜ N , M ˜ C n N , M ˜ N ( p ˜ 1 , k ) k = 1 m ˜ 2 H ˜ 0 , 1 , 1 n 0 , 1 ( p ˜ 2 , k ) k = 1 m ˜ 2 H ˜ N , M ˜ N , M ˜ C n N , M ˜ N ( p ˜ 2 , k ) k = 1 m ˜ N H ˜ 0 , 1 , 1 n 0 , 1 ( p ˜ N , k ) k = 1 m ˜ N H ˜ N , M ˜ N , M ˜ C n N , M ˜ N ( p ˜ N , k )
B ˜ x , 0 = u ¯ ( p ˜ 0 , 1 ) u ¯ ( p ˜ 0 , 2 ) u ¯ ( p ˜ 0 , m ˜ 0 ) B ˜ y , 0 = v ¯ ( p ˜ 0 , 1 ) v ¯ ( p ˜ 0 , 2 ) v ¯ ( p ˜ 0 , m ˜ 0 ) .
Notice that by discretizing the workspace boundaries into a large number of sufficiently small elements, the overall approximation error between the solution T ˜ of the aforementioned linear problem and the exact transformation T can be rendered arbitrarily small (see [39,40]). However, the complexity of constructing the mapping is of order O ( M ¯ 3 ) , where M ¯ denotes the number of total elements of the mapping (i.e., the complexity of the solution of the dense system of linear Equation (5)). Nevertheless, the construction of the transformation, which is the main computational bottleneck, is performed only once at the beginning. Additionally, apart from the straightforward user-defined homeomorphism T on the workspace boundary, no tedious trial and error tuning is needed to extract the diffeomorphic transformation T ˜ , in contrast to other related works such as the Star-to-Sphere Transformation (SST) [15], the Multi-Agent Navigation Transformation (MANT) [36] and the Navigation Transformation (NT) [34].

4. Control Design

To address Problem 1, we equip the robot with the aforementioned transformation q = T ( p ) from the closure of its configuration space W onto the unit disk D and an artificial potential ψ ( q , k ) augmented with an adaptive control law k ˙ = f k ( q , k ) for its parameters k = [ k d , k 1 , k 2 , , k N ] T . The robot velocity control law is calculated as follows:
u = K u s ( q , k ) J 1 ( p ) q ψ ( q , k )
where J ( p ) denotes the Jacobian matrix of T ( p ) , s ( q , k ) 0 is a continuously differentiable gain function given by:
s ( q , k ) = γ σ p 1 q ϵ p + ( 1 γ ) σ v q ψ T q ϵ v + q ψ q
with
σ p x = x 2 ( 3 2 x ) , if x 1 1 , if x > 1 ,
σ v x = x 2 , if x 0 0 , if x < 0
and K u , γ , ϵ p and ϵ v are scalar constants such that K u , ϵ v > 0 and γ , ϵ p ( 0 , 1 ) . More specifically, s ( q , k ) consists of two individual terms, with the first vanishing as the robot approaches the workspace’s outer boundary (and its distance from the unit circle is less than ϵ p ) and the second vanishing when the robot’s velocity points away from the disk’s center. The scalar parameter γ can be used for adjusting the contribution of each respective term of s ( q , k ) . Finally, ψ is a harmonic artificial potential field defined on the image T ( W ) of the workspace W and whose negated gradient q ψ ( q , k ) defines the direction of the robot’s motion in the real workspace W via the inverse Jacobian J 1 ( p ) . By design, the resultant vector field precludes collisions between the robot and the workspace’s inner obstacles and renders the goal configuration almost globally attractive except for a set of measure zero initial configurations. However, since W 0 may not be repulsive under ψ for an arbitrary, fixed selection of k, we also introduce the adaptive law f k ( q , k ) which, along with s ( q , k ) , guarantees forward invariance of the workspace without compromising the convergence and stability properties of the overall system. The following subsections elaborate on each component of the proposed control law individually.

4.1. Artificial Harmonic Potential Fields

We construct an artificial harmonic potential field on the disk space D employing point sources placed at the desired configuration q d = T ( p d ) as well as at the points q i = T ( W i ) , i I N that correspond to the inner obstacles, as follows:
ϕ ( q , k ) = k d ln q q d 2 i = 1 N k i ln q q i 2
where k d > 0 and k i 0 denote harmonic source strengths which vary according to adaptive laws that are presented later. An interesting property of the above potential field, which stems from the maximum principle for harmonic functions, is that, for fixed k, the only minima of ϕ are located at q d and, possibly, at infinity. As a direct consequence of this property, the Hessian q 2 ϕ computed at a non-degenerate critical point of ϕ in our domain’s interior has one positive and one negative eigenvalue with the same magnitude, e.g., λ and λ with λ > 0 .
Next, we define a reference potential ψ based on ϕ , which is given by:
ψ ( q , k ) = 1 + tanh ( ϕ ( q , k ) / w ϕ ) 2
where w ϕ is a positive scaling constant. Note that ψ maps the extended real line to the closed interval [ 0 , 1 ] . As tanh ϕ / w ϕ is a strictly increasing function, the only critical points of ψ are the ones inherited from ϕ with their indices preserved. Furthermore, the gradient of ψ with respect to q, given by
q ψ = 1 tanh ϕ / w ϕ 2 2 w ϕ q ϕ ,
is well defined and bounded for all q D .
If the workspace was radially unbounded, selecting k fixed with k d > i = 1 N k i would render the potential field (10) sufficient for navigation. The author of [33] addresses bounded workspaces that are diffeomorphic to sphere worlds by simply mapping the outer bounding circle to infinity. In this work, we would like to be able to place q d on regions of D that are not physically occupied by obstacles (such as passages to other compartments, see, for example, Section 5.2); thus, we cannot follow the same procedure since that would render the effect of the sole attractor on the robot null. Instead, we design appropriate adaptive laws for the parameters k of ϕ to render the outer boundary repulsive and establish the forward completeness of the proposed scheme at all times.
Before proceeding with the definition of the adaptive law, we first state two propositions that will be used in the subsequent analysis, the proofs of which can be found in the Appendix A.
Proposition 1. 
Let k d > 0 and q D \ { q d } . There exists k > 0 such that if k i < k , i I N , then q is repulsive under ψ.
Proposition 2. 
If k i are non-negative and bounded, there exists k d > 0 such that ψ is Morse for all k d k d .

4.2. Adaptive Laws

We now present the adaptive law k ˙ = f k ( q , k ) that updates the parameters of the potential field ψ . Its primary goal is to render (a) the workspace outer boundary repulsive and (b) any critical point of ϕ in the vicinity of the robot non-degenerate, a property that will be used later in the analysis. In particular, we consider f k of the form:
k ˙ d = ξ 1 ( λ + q ϕ ; ϵ 1 ) k ˙ i = k ¯ i k i w i i g i K k k i h i w 0 g 0 + ξ 1 ( s ; ϵ 2 ) , i I N
where w i and g i , i I N , as well as h i , i I N , are functions to be defined later, k ¯ i , i I N are desired upper bounds for k i , λ denotes the non-negative eigenvalue of q 2 ϕ , K k is a positive control gain and ϵ 1 and ϵ 2 are small positive constants. The continuously differentiable switch ξ 1 ( x ; ϵ ) and functions i ( q ) are, respectively, given by:
ξ 1 ( x ; ϵ ) = 1 σ p x / ϵ
i ( q ) = K u s ( q , k ) ln q q i 2 .
According to Proposition 1, our first requirement can be accomplished by designing f k to reduce k i as the robot approaches D . To do so without compromising the inherent inner obstacle collision avoidance properties of ϕ , we need to also ensure that each k i does not vanish within some neighborhood of q i for all i I N . To that end, firstly we define g i , employing the smoothly vanishing function defined in (8) to serve as pseudo-metrics of the alignment between the robot’s velocity and the directions towards the goal and inner obstacles, respectively, given by:
g i = σ v g ¯ i , i I N
with
g ¯ 0 = 1 4 α q ψ q q d q ψ T ( q q d ) g ¯ i = 1 2 q ψ T ( q q i ) , i I N
where α ( 0 , 1 ] is a fixed constant that is used for selecting the desired alignment between the robot’s motion and the direction to the goal. We also define the accompanying weights w i as follows to ensure that only one term of (12) dominates as the robot approaches a particular boundary of W :
w 0 = ξ 2 ( w ¯ 0 ; ϵ 3 ) w ¯ 0 + j = 1 N ( k ¯ j w ¯ j ) w i = w ¯ i w ¯ 0 + j = 1 N ( k ¯ j w ¯ j ) , i I N
with
w ¯ i = r i ¯ / ( r i + r i ¯ ) , i I N ,
r 0 = 1 q 2 , r i = q q i 2 , i I N
r i ¯ = j i ( r j ) m m , i I N
ξ 2 ( x ; ϵ ) = 0 , if x < ϵ x ϵ 1 ϵ 2 3 2 x ϵ 1 ϵ , if ϵ x 1 1 , otherwise
for a scalar constant ϵ 3 ( 0 , 1 ) in (15) and some integer m < 1 in (16) that serves as a smooth under-approximation of min j i ( r j ) , i I N . Finally, the weights h i , i I N are defined as follows:
h i = 1 + σ v h ¯ i 1 + j I N σ v h ¯ j
with
h ¯ i = k i 1 tanh ϕ / w ϕ 2 2 q d q q d q 2 T q i q q i q 2
whose purpose is to accelerate the decay of those k i that contribute the most to the component of q ψ that pushes the robot toward the workspace’s outer boundary.
Regarding the second requirement, as shown in Proposition 1, selecting a k d above a certain threshold is sufficient to render ϕ free of degenerate equilibria. On the other hand, for a given k ¯ i , increasing k d steers the robot closer to the workspace’s inner obstacles. Nevertheless, since the robot may never actually enter the vicinity of a degenerate equilibrium, instead of setting k d sufficiently large a priori, the adaptive law for the parameter k d is introduced to increase k d only when it is actually needed, thus alleviating the aforementioned shortcoming.

4.3. Stability Analysis

Let us consider the overall system:
z ˙ = f z ( z )
where z = ( q , k ) and f z ( z ) = ( f q , f k ) with f q = J u . Furthermore, let Ω denote the image of W under T, i.e., Ω = T ( W ) . Note that Ω consists of int D , possibly with a subset of D , with the points q i removed. In this section, we elaborate on the stability properties of (17) under the proposed control scheme (6) and (12). First, we formalize the safety properties of the closed-loop system dynamics, which guarantee that our robot does not collide with any obstacle.
Proposition 3. 
The workspace W is invariant under the dynamics (17) with control laws (6) and (12), i.e., p ( t ) W for all t 0 .
Proof. 
For the proof, refer to the Appendix A.    □
Having eliminated the possibility of the robot colliding with the workspace’s boundaries, we proceed by showing that all critical points of ψ , where (17) may converge to, are either non-degenerate saddles or q d . Additionally, we show that the latter is a stable equilibrium.
Proposition 4. 
The artificial potential ψ decreases along the trajectories of the closed-loop system and its time derivative vanishes only at its critical points. Additionally, the preimage of q d is a set of stable equilibria of (1).
Proof. 
For the proof, refer to the Appendix A.    □
Proposition 5. 
Let z = ( q , k ) be a critical point of the closed-loop system dynamics with q Ω \ { q d } . Then, q is a non-degenerate saddle point of ψ.
Proof. 
For the proof, refer to the Appendix A.    □
Finally, we conclude this section with the main theoretical findings.
Theorem 1. 
System (1) under the control law (6) and (12) converges safely to p d , for almost all initial configurations, thus addressing successfully Problem 1.
Proof. 
For the proof, refer to the Appendix A.    □
Remark 2. 
Owing to the adaptive laws (12) that modify the harmonic source strengths online to secure the safety and convergence properties at all times, the selection of the fixed control parameters in the proposed scheme, i.e., K u , γ, ϵ p , ϵ v , w ϕ , K k , ϵ 1 , ϵ 2 , α and ϵ 3 , is straightforward as it affects only the trajectory evolution within the workspace and not the aforementioned critical properties. Consequently, their values should be set freely as opposed to NFs, where the selection of the main parameters severely affects the convergence properties of the adopted scheme and cannot be conducted constructively for generic workspaces of arbitrary topology.

5. Extensions

In this section, we present certain extensions of the proposed approach to (a) address the safe navigation problem for unicycle robots which are frequently encountered in many application domains and (b) tackle computational complexity issues that affect the numerical computation of the harmonic map presented in Section 3 as the size of the workspace increases.

5.1. Unicycle Robot Kinematics

In this subsection, we consider robots whose motion is subjected to Pfaffian constraints of the form:
p ˙ = n θ v θ ˙ = ω
where θ [ 0 , 2 π ) denotes the robot’s orientation, n θ = [ cos ( θ ) , sin ( θ ) ] T , and v , ω R are control inputs corresponding to the robot’s linear and angular velocities, respectively. First, let us define the robot’s kinematics in the image of the configuration space via the proposed transformation as follows:
q ˙ = n θ ^ v ^ θ ^ ˙ = ω ^ .
Note that the orientations θ and θ ^ are related by:
n θ ^ = J p n θ J p n θ
where J p = J ( p ) . To safely drive the robot to its goal configuration, we consider the following control laws:
v ^ = K v s v ( q , θ ^ , k ) n θ ^ T q ψ ( q , k ) ω ^ = K ω n θ ^ T q ψ ( q , k )
with K v , K ω R positive constant gains, n θ = [ sin ( θ ) , cos ( θ ) ] T and
s v ( q , θ ^ , k ) = γ σ p 1 q ϵ p + ( 1 γ ) σ v n θ ^ T q ψ n θ ^ T q ϵ v + n θ ^ T q ψ q .
Additionally, we need to employ a modified version of the adaptive law for the potential field parameters, which is obtained by substituting s with s v in (12) and (13) and g ¯ i , i I N , with
g ¯ v , 0 = 1 4 α n θ ^ T q ψ q q d n θ ^ T q ψ n θ ^ T ( q q d ) g ¯ v , i = 1 2 n θ ^ T q ψ n θ ^ T ( q q i ) , i I N
respectively, in (14). Finally, by expressing the aforementioned control laws to the robot’s actual configuration space, we obtain:
v = ν v ^ ω = ω d q + ω d θ ^
where ω d q and ω d θ ^ are terms corresponding to angular velocities induced by translational and rotational motion of the robot in the workspace’s image, respectively, given by:
ω d q = v ^ ν 2 J p n θ ν n θ 1 n θ J p n θ T 0 1 ω d θ ^ = ω ^ J p n θ ν n θ 1 n θ ^ T 0 1
with ν = J p 1 n θ ^ and n θ J p denoting the directional derivative of J p along n θ .
The stability properties of the aforementioned closed-loop system dynamics are formalized below.
Theorem 2. 
The workspace W is invariant under the dynamics of (18) equipped with the proposed control law. Additionally, the robot will asymptotically converge either to an interior critical point of ϕ or to the pre-image of q d , which is stable.
Proof. 
For the proof, refer to the Appendix A.    □
Remark 3. 
The result of Theorem 2 is weaker compared to that of Theorem 1, since there is no guarantee that the set of configurations which converge to a critical point of ϕ (other than the pre-image of q d ) has Lebesgue measure zero.

5.2. Atlas of Harmonic Maps

As the size of the workspace increases, the problem of computing the transformation T grows in complexity as well, because the resources required by commonly employed numerical techniques that can solve the problem presented in Section 3 are polynomial in the number of elements used for representing W . Alternatively, to cope with large workspaces efficiently, we propose instead the construction of an atlas A { P i , T i | i I N A } obtained by separating the workspace W into N A overlapping subsets P i W , such that i I N A P i = W and constructing a separate harmonic map T i for each P i (see Figure 3).
This essentially allows us to solve many small (and computationally less intensive) problems instead of a large one, thus reducing the overall resources required for addressing a given workspace. Therefore, given such a partitioning of W , we define the graph G = ( V , E ) , where V = { P i | i I N A } denotes the set of corresponding nodes (workspace partitions) and E denotes the set of edges between the elements of V , with each edge indicating a feasible transition from one partition to another, i.e., ( i , j ) E if and only if cl P i cl P j . Note that G is undirected by definition, i.e., ( i , j ) E only if ( j , i ) E . Additionally, since the workspace is connected, G should also be connected. Thus, for a given atlas A , an initial configuration p init and a final configuration p d , we can employ standard graph search algorithms to obtain a sequence of indices S = { s 1 , s 2 , s n } corresponding to partitions that the robot can tranverse to reach its goal. (In general, more than one such sequence of partitions may exist connecting the initial and the final configurations. However, the selection of one that corresponds to some sort of “optimal” path is beyond the scope of this work.) Additionally, note that since the partitioning of W does not need to be fine, the size of G will generally be small, rendering the cost of finding S negligible.
We now concentrate on how the transition between two consecutive elements of S is implemented. Let C i , j cl P i cl P j denote the common region of cl P i and cl P j and let B i , j P i P j denote the set of points on the boundary of P i that also belong to P j and are not occupied by obstacles for all i I N A and all j such that ( i , j ) E . Without loss of generality, we assume that A is constructed such that the sets B , i B , j are either empty or consist of isolated points. We note that in order to successfully complete the transition between two consecutive nodes P i and P j of S , it suffices for the robot to reach any single point of B i , j from P i . We also observe that each B i , j may consist of one or more disjoint components B i , j , L ( i , j ) , with L ( i , j ) being some valid indexing of those. By exploiting the fact that Theorem 2 [38] imposes a weak homeomorphism requirement on T ¯ i , we can construct each T i such that each disjoint subset of P i collapses into a separate point, i.e., T i ( B i , j ) = q i , j D (see Figure 3), which, in turn, implies that selecting q i , j as an intermediate goal configuration suffices to render the entire B i , j attractive. Building upon this fact, for each consecutive pair of P i and P j in S , we (arbitrarily) select a B i , j and we construct a transformation T i : P i D , with q [ i ] = T i ( p ) , and artificial potential field ϕ i ( q [ i ] , k [ i ] ) with goal configuration q d [ i ] = q i , j . Additionally, to smooth the transition between consecutive partitions, when they overlap, we propose the following modified control law for the robot:
u = u [ i ] + η c , i , j · η t , i , j · u [ j ] , p C i , j
where u [ i ] and u [ j ] denote the control inputs as defined in (6) and evaluated using ψ i , T i and ψ j , T j , respectively; the function η t , i , j : C i , j [ 0 , 1 ] is any smooth bump function such that
η t , i , j ( p ) = 0 , if p B j , i 1 , if p B i , j
and
η c , i , j ( p , k [ i ] , k [ j ] ) = ζ i , j 2 ϵ 4 + ζ i , j 2 , if ζ i , j 0 0 , if ζ i , j < 0
with ζ i , j = p ψ i T · p ψ j ; and ϵ 4 > 0 is a fixed parameter. What this modification essentially does is incrementally add an extra component, with the direction of p ψ j , to the robot’s velocity when that component is cosine similar (two vectors u and v are cosine similar if their inner product is positive) with p ψ i . We note that η c , i , j 1 and η t , i , j 1 as the robot approaches the boundary of the corresponding partition. We also remark that once the robot has completed its transition to P j , we do not concern ourselves with u [ i ] anymore, i.e., u = u [ j ] even if p returns to C i , j . The overall scheme employed for navigating a holonomic robot to its goal configuration using an altas constructed as described above can be found in Algorithm 1.
Regarding the stability analysis of the modified system, by following the same procedure as in Section 4.3 and by virtue of η c , i , j , it is trivial to verify the following statement.
Theorem 3. 
System (1) equipped with Algorithm 1 converges safely to a given goal configuration p d W from almost all initial configurations p init W .
Proof. 
For the proof, refer to the Appendix A.    □
Algorithm 1 Altas-based motion planning scheme for a holonomic robot
Require:  A , p init , p d
  • S FindPathToGoal( G , p init , p d )
  • Initialize k [ s ] for all s S .
  • for all i in I n 1  do
  •      s , s s i , s i + 1
  •     Select (arbitrary) such that L ( s , s ) .
  •     Place goal configuration of ψ s at q s , s .
  • end for
  • Place goal configuration of ψ s n at T s n ( p d ) .
  • 1
  • loop
  •     if  = n or p P s \ P s + 1  then
  •         Update p using (6) and k [ s ] using (12).
  •     else if  p C s , s + 1  then
  •         Update p using (22) with i = s and j = s + 1 .
  •         Update k [ s ] and k [ s ] using (12).
  •     else
  •          + 1
  •     end if
  • end loop

6. Simulations and Experimental Results

In order to demonstrate the efficacy of the proposed control scheme, we have conducted various simulation and experimental studies, the results of which are presented in this section. The algorithm that computes the harmonic transformation and its Jacobian was implemented in C++, while the proposed control protocols were implemented in Python. Code implementations can be accessed at https://github.com/maxchaos/hntf2d (accessed on 16 April 2023). All simulations were carried out on a PC with an Intel i5 processor operating at 2.2 Ghz, with 4 GB RAM and running a GNU/Linux operating system. For more details regarding both simulations and experiments, the reader may refer to the accompanying video material at https://youtu.be/I6WUS81iDh4 (accessed on 16 April 2023).

6.1. Simulations—Full Workspace Transformation

In the first case study, a single transformation of the entire 8 m × 5 m workspace (see Figure 3) was constructed and the robot was instructed to navigate to various goal configurations starting from the same initial position. The initial configuration and the parameters of our controller were selected such as to better demonstrate the guaranteed collision avoidance properties of our scheme. Particularly, the initial values for the parameters of the adaptive law were selected as k d = 20 , k i = 1 and k ¯ i = 20 for all i I 10 . The values of the remaining parameters were K u = 100 , w ϕ = 20 , K k = 100 , α = 1 , ϵ p = 0.025 , ϵ v = 0.1 , γ = 0.7 , ϵ 1 = 0.01 , ϵ 2 = 0.1 and ϵ 3 = 0.1 . The goal configurations and the trajectories executed by the robot, both in the real and transformed workspace, are illustrated in Figure 4.
The simulations were conducted using the Euler method with 10 ms steps. Regarding the computational complexity of the control scheme, the construction of the harmonic transformation for this large workspace that was carried out offline once required 5.4 s to complete for a sufficient approximation of the workspace boundary with 3680 segments. Finally, the online computation of the transformation T ( p ) and its Jacobian J ( p ) required an average of 6.0 ms per step.

6.2. Simulations—Atlas of Harmonic Maps

In this case study, we decomposed the aforementioned workspace into separate partitions (see Figure 3) and constructed a harmonic transformation T i for each one (we adopted the door of each room as the common boundary between neighboring partitions). The robot was initialized at the same position as the previous study and it was instructed to navigate towards the same set of individual goal configurations. The initial values selected for the parameters of the adaptive law were k [ i ] = N [ i ] + 3 , k j [ i ] = 1 and k ¯ j [ i ] = k [ i ] for all j I N [ i ] and i I N A , where N [ i ] denotes the amount of obstacles inside the corresponding partition. All remaining control parameters were selected as in Section 6.1. The trajectories of the robot are depicted in Figure 5. The time spent to construct the corresponding harmonic transformations varied from 0.019 s to 0.211 s to (depending on the amount of elements required for sufficiently approximating each room, ranging between 320 and 1000 segments) and was significantly much less than the full map construction of the previous case (5.4 s). Additionally, the online computation of T i ( p ) and J i ( p ) in each of these rooms required an average time between 1.0 ms and 2.2 ms per step, respectively. Finally, it should be noted that in this case, the workspace inner obstacles were mapped to points further away from the boundaries of the partitions, which is an interesting result as it alleviates possible numerical issues that may arise in the computation of the transformation near the obstacles (the condition number of the Jacobian of the transformation is improved). It should be stressed that the length of the paths in the second case was less (improvement of 0.5 m on average), owing to the fact that the robot gets closer to the workspace boundary since the individual transformations in each room obtain a better conditioned Jacobian (condition number 0.212 against 0.093) and thus are more fine than the first approach, where a transformation is built for the whole workspace.

6.3. Comparative Study—Workspace Transformation

In this subsection, we provide a comparative study of the harmonic map presented in this work against readily available workspace transformation methods employed in the motion planning literature. Particularly, we consider four 4 m × 4 m compact workspaces, each associated with a pair of initial and goal positions, and construct appropriate transformations for each one by employing the methodology presented in this work (HM), as well as (i) the Star-to-Sphere Transformation (SST) [15], (ii) the Multi-Agent Navigation Transformation (MANT) [36] and (iii) the Navigation Transformation (NT) [34] (with the aforementioned Star-to-Sphere transformation serving as the underlying map). The trajectories of the robot executed while tracing the line segment connecting the initial and goal configurations in the images of each domain can be seen in Figure 6. We note that manual tuning of the compared transformations was necessary in order to render each a diffeomorphism but without making them too steep around the obstacles. Furthermore, the domain boundaries considered here had to be sufficiently smooth in order for methodologies such as MANT to be applicable. Finally, we remark that the trajectories corresponding to the proposed transformation are, in general, less abrupt compared to the rest, a property attributed to the fact that our approach is global as opposed to the other transformations, i.e., the distortion caused by each obstacle is not limited to some narrow neighborhood around it. The total length, maximum curvature and distance from the obstacles of each executed trajectory can be seen in Table 3, Table 4 and Table 5, respectively. We can see from these values that the actual trajectories yielded using harmonic maps are among the shorter and smoother ones, although they tend to approach the obstacles more than the rest.

6.4. Comparative Study—Control Law

In this subsection, we provide a comparative study of our control scheme against other motion planning methodologies.

6.4.1. APF-Based Schemes

To demonstrate the efficacy of the proposed control scheme compared to other APF-based schemes, we considered the 12 m × 16 m workspace depicted in Figure 7, for which we constructed a harmonic map as described in Section 3. Next, we equipped a holonomic robot with three alternative control laws and instructed it to visit four distinct goal positions using these controllers, starting each time from a fixed initial configuration. Particularly, we considered a conventional navigation function-based controller (NF) [15] augmented by [17], for the selection of its notorious parameter, and a harmonic navigation function-based controller (HNF) [33], in addition to our adaptive control scheme (AHNF) described in Section 4. We note that all three control laws considered here make use of the same underlying harmonic map T constructed as described above in order to drive the robot to its instructed goal positions. The trajectories executed by the robot can be seen in Figure 7. We remark that, in general, our approach steers the robot away from inner obstacles that lie between its initial and goal configurations, unlike “greedy” schemes such as the conventional NF-based controller, while keeping the traced paths shorter compared to HNFs with fixed source weights, a property attributed to the proposed adaptive laws (12) which penalize misalignment between the robot’s velocity and the direction towards the goal configuration.
The total length and distance from the obstacles of each executed trajectory can be seen in Table 6 and Table 7, respectively. First, we have to stress that the length trajectory corresponds to the travelled path towards the goal configuration and thus needs to be small, whereas the minimum distance to the workspace boundary refers to the closest point of the trajectory to the workspace boundary and thus needs to be large to have a safe trajectory. Consequently, note from Table 6 that the NF scheme yielded shorter path lengths than the proposed method in two cases (blue and yellow); nevertheless, such paths approach closer to the workspace boundary as indicated in Table 7, thus resulting in more risky paths. On the other hand, the Adaptive Harmonic Potential Field yields a good trade-off between path length and minimum distance to the boundary, since it achieves the shortest paths for two cases without compromising them, as is the case with the NF. On the other hand, the HPF tend to travel around the obstacle closer to the outer workspace boundary and hence exhibit more safe trajectories but they are significantly longer than the other two schemes.

6.4.2. Sampling-Based Scheme

To compare the control scheme proposed in this work against sampling-based methods, we considered a holonomic point-sized robot positioned inside a 6 m × 8 m compact workspace and a desired goal configuration. To complete this task, we employed two different controllers, namely the one proposed in this work and an admissible planner based on an improved probabilistic roadmap method (PRM) [6]. The trajectories executed by the robot using our control law as well as two of the trajectories generated by the PRM-based planner can be seen in Figure 8. The construction of the associated transformation took 31 s to complete for a given boundary approximation made of 7842 elements, whereas the PRM-based planner required approximately 24 s on average over 10 successful runs to yield a solution (we have to stress that we ran 14 trials to get 10 solutions, since four runs did not complete as they exceeded the 500 s calculation time), using the same boundary approximation for collision checking. The robot trajectories exhibited similar lengths in both algorithms (22.5 m for our method against 21.8 m on average), although no path optimization was employed in our case. Additionally, the proposed scheme resulted in a smoother robot trajectory (based on the resulting sequence of points in both cases, we calculated the minimum curvature radius as 0.23 m for our method against 0.12 m on average for the PRM method). On the other hand, note that our approach solves the motion planning problem for any pair of initial and final configurations within the workspace, whereas the sampling-based scheme considers only one go-to problem. Thus, a different initial or final configuration would require a new solution with the PRM method. On the contrary, the proposed transformation needs to be calculated only once to solve the motion planning problem for any pair of initial and final configurations. Finally, it should be noted that for a narrower corridor in Figure 8, the sampling-based approach failed to derive a solution with a reasonable execution time (no solution was calculated within 500 s), since the probability of sampling connected points within this snaky passage reduces drastically. On the contrary, the proposed transformation took 38 s to complete for the same number of elements (i.e., 7842 elements).

6.5. Experiments

In order to verify the results presented in Section 5.1, real experiments were conducted on a non-holonomic robotic platform (Robotnik Summit-XL) operating within the 10 m × 25 m compact workspace that is depicted in Figure 9 and Figure 10. The boundaries of the workspace were obtained using readily available SLAM algorithms and were later augmented with the robot’s shape (approximated by a disk). The workspace was partitioned into six overlapping subsets and the robot was instructed to visit three different goal configurations, each located in a different room. An off-the-shelf localization algorithm was employed for estimating the robot’s position and orientation using its on-board sensors (laser scanners and RBG-D cameras), providing feedback at approximately 5 HZ to the robot’s linear and angular commanded velocities. The construction of the associated transformations over the six subsets of the workspace took from 1.3 s for the simple and smaller partitions with 800 elements to 3.1 s for the more complex ones employing 1500 elements. On the other hand, the evaluation of the mapping as well as its Jacobian took less than 6 ms on average, which was satisfactory given the low position update rate. Note that our algorithm successfully managed to drive the robot safely (the minimum distance to the workspace boundary was 0.15 m when passing through the doors) to its specified goal configurations, as one can verify from the trajectories (see Figure 9, Figure 11 and the accompanying video material). However, an issue that needs to be pointed out is the oscillating behavior that the robot exhibited in the configuration space’s image, particularly in subsets p 1 and p 2 as depicted in Figure 11. Such behavior is attributed both to (a) the relative slow update of the robot’s pose estimation and (b) the inversion of the Jacobian which is ill-conditioned close to extremely narrow passages of the domain. Nevertheless, such shortcomings can be alleviated by a better choice of partitions, e.g., by partitioning the domain into more subsets with less complex shapes. As a future research direction, we shall investigate whether the condition number of the Jacobian of the transformation is a fine criterion, since the condition number is usually used to measure how sensitive a function is to changes or errors in the input, and the output error results from an error in the input via the Jacobian.

7. Conclusions and Future Work

In this work, we employed harmonic map theory to devise a transformation of complex workspaces directly to point worlds that are appropriate for robot navigation. Subsequently, we presented a novel motion planning control scheme based on closed-form harmonic potential fields equipped with appropriate adaptive laws for their parameters, which can safely navigate a robot to its goal state from almost all initial configurations. Additionally, we extended our approach to accommodate the navigation problem of non-holonomic robots and kept the numeric computations tractable for large workspaces.
Regarding future directions, our aim is first to increase the applicability of the proposed navigation framework by addressing partially known dynamic workspaces, which is far from being straightforward. To remedy the issue of calculation time in this case, we shall adopt a sensitivity analysis approach so that we do not solve the whole problem from scratch, but find how the solution deviates when a small change in the workspace occurs. In this way, we envision a reasonable calculation time (except from the first calculation) that would result in an almost real-time calculation of the transformation and thus allow us to consider even moving obstacles in dynamic environments. However, critical issues have to be studied concerning cases where the workspace changes topologically (e.g., in the case of antagonistically moving obstacles) and this results in significant changes in the transformation. In the same vein, switching in the transformation output might raise practical issues such as chattering that have to be carefully considered. Note that the aforementioned research direction could also serve as a first step towards the solution of the multi-robot motion planning problem, where for each robot all other robots should be considered as moving obstacles, operating antagonistically to achieve their goal configurations. Finally, another challenging research direction concerns the extension to 3D workspaces. Unfortunately, the harmonic maps have been studied only for 2D workspaces, since they rely heavily on complex analyses. Nevertheless, we propose to decompose the 3D motion planning problem into several 2D sub-problems, where the proposed solution works, and then combine them (e.g., decompose the motion along the z-axis and on the x-y plane).

Author Contributions

Methodology, P.V. and C.P.B.; Validation, P.V.; Formal analysis, P.V. and C.P.B.; Writing—original draft, P.V.;Writing—review & editing, C.P.B. and K.J.K.; Supervision, C.P.B. and K.J.K. All authors have read and agreed to the published version of the manuscript.

Funding

The publication fees of this manuscript have been financed by the Research Council of the University of Patras.

Institutional Review Board Statement

Not applicable.

Informed Consent Statement

Not applicable.

Data Availability Statement

Not applicable.

Conflicts of Interest

The authors declare no conflict of interest.

Appendix A

Appendix A.1. Proof of Proposition 1

By construction, it holds that 1 tanh ϕ / w ϕ 2 > 0 for all q D \ { q d } . The gradient of ϕ with respect to q is given by
q ϕ = k d q q d q q d 2 i = 1 N k i q q i q q i 2 .
Computing the inner product of q ϕ and q yields:
q ϕ T q = k d q 2 q d T q q q d 2 i = 1 N k i ( q q i ) T q q q i 2 k d 1 q d T q q q d 2 max i ( k i ) i = 1 N 1 q q i .
Given that all q i lie within int D , the second term on the right-hand side of (A2) is finite for all q D . Similarly, the first term on the right-hand side of (A2) is positive for all q q d . Let q D \ { q d } . Additionally, the continuity of 1 q d T q / q q d 2 and ( 1 tanh ϕ / w ϕ 2 ) / ( 2 w ϕ ) implies that there exists a closed neighborhood F ( q ) of q , not containing q d , where both are positive. Hence, selecting
k = k d min q F ( q ) 1 q d T q q q d 2 1 i = 1 N 1 q q i
ensures that ( q ϕ ) T q > 0 for all q F ( q ) . Moreover, computing the derivative of d = 1 q 2 with respect to time for all q F ( q ) and assuming k i < k , i I N yields d ˙ = 2 K u s q ψ T q > 0 ; thus, the distance from the workspace boundary increases, which concludes the proof.

Appendix A.2. Proof of Proposition 2

Similarly to the proof of Proposition 3 in [33], we proceed by defining q ˜ d q q d , q ˜ i q q i for all i I N . Let also q ^ d q ˜ d / q ˜ d and q ^ i q ˜ i / q ˜ i . Accordingly, the Hessian of ϕ can be computed by:
q 2 ϕ = k d q ˜ d 2 I 2 2 q ^ d q ^ d T i I N k i q ˜ i 2 I 2 2 q ^ i q ^ i T .
Note that at a critical point of ϕ it holds that:
k d q ^ d q ˜ d = i I N k i q ^ i q ˜ i k d 2 q ˜ d 2 q ^ d q ^ d T = i I N k i 2 q ˜ i 2 q ^ i q ^ i T + i I N j I N \ { i } k i k j q ˜ i q ˜ j q ^ i q ^ j T + q ^ j q ^ i T .
Substituting (A4) into (A3) and re-arranging the terms yields:
p 2 ϕ = k d q ˜ d 2 i I N k i q ˜ i 2 I + 2 ( i I N k i ( k d k i ) k d 1 q ˜ i 2 q ^ i q ^ i T 1 k d i I N j I N \ { i } k i k j q ˜ i q ˜ j q ^ i q ^ j T + q ^ j q ^ i T ) .
Next, we argue that for any given set of radii ρ i > 0 such that D ρ i ( q i ) , i I N are disjoint disks that lie entirely within our domain, there exists k d > 0 such that no critical point of ϕ exists within D \ i I N D ρ i ( q i ) for all k d > k d . This implies that, by choosing a sufficiently large k d , each critical point of ϕ belongs to a single D ρ i ( q i ) . Let q be a critical point and = argmin i I N q q i . To show that q 2 ϕ ( q ) is not degenerate, it suffices to show that its eigenvalue λ ( q ) is positive. We recall that λ is lower bounded by the quadratic form x ^ T q 2 ϕ x ^ for all x ^ = 1 . By considering the direction of q ˜ and after some tedious calculations, we obtain:
q ^ T q 2 ϕ ( q ) q ^ = k d q ˜ d 2 i I N \ { } k i q ˜ i 2 + k q ˜ 2 k d 2 k k d 4 q ˜ k d i I N \ { } k i q ˜ i 2 ( q ^ T q ^ i ) + 2 i I N \ { } k i ( k d k i ) k d 1 q ˜ i 2 q ^ T q ^ i 2 2 k d i I N \ { } j I N \ { i , } k i k j q ˜ i q ˜ j 2 ( q ^ T q ^ i ) ( q ^ T q ^ j ) .
The first right-hand side term of (A5) is strictly positive. Since all k i are bounded and non-negative, choosing a sufficiently large k d renders the second and third right-hand side terms non-negative. Furthermore, note that the fourth and fifth right-hand side terms are bounded for all q D ρ ( q ) . Thus, by choosing a sufficiently large k d , the first three terms of (A5) can be made dominant, thus rendering q ^ T q 2 ϕ q ^ positive at q , which concludes the proof.

Appendix A.3. Proof of Proposition 3

Firstly, we will show that the robot cannot escape through the workspace’s outer boundary. Let us assume that q q D \ { q d } . Then, q ˙ 0 by virtue of (7), since s ( q , k ) = 0 for all q = 1 with q ϕ T q 0 . Additionally, w 0 1 and w i 0 , for all i I N . Thus, k ˙ i < 0 holds within a neighborhood of D , while k i > 0 , which implies that k i 0 for all i I N . Moreover, Proposition 1 dictates that there exists k > 0 for which any point in D \ { q d } is repulsive under ψ . Since (12) dictates that all k i become less than k in finite time, this contradicts our supposition.
Next, we consider collision avoidance between the robot and the inner obstacles. Let us assume that the robot approaches obstacle i. By construction, w i 1 while q ψ 0 and w ¯ j 0 for all j I N \ { i } . Note that there exists a neighborhood N i of q i such that w 0 = 0 for all q N i due to continuity of w ¯ 0 and ξ 2 ( w ¯ 0 ; ϵ 3 ) . Additionally, since the robot is assumed to approach q i , q ˙ T ( q q i ) cannot be identically zero inside N i . As such, as long as k i < k ¯ i , k ˙ i 0 inside N i without k ˙ i = 0 for all q N i . This implies that k i ¬ 0 as q q i , thus rendering q i a local maximum of ψ . Thus, there exists a neighborhood of q i inside which ( q ψ ) T ( q q i ) > 0 , which contradicts our assumption.

Appendix A.4. Proof of Proposition 4

Let V ψ ( q , k ) , as defined in (10), be a candidate Lyapunov function, which is non-negative and vanishes only when q = q d . Differentiating V along the system’s trajectories yields:
V ˙ = 1 tanh ϕ / w ϕ 2 2 w ϕ q ϕ T q ˙ + ln q q d 2 k ˙ d i I N ln q q i 2 k ˙ i .
Given that q ˙ = J p ˙ , the first term of (A6) can be further expanded as follows:
q ϕ T q ˙ = K u s 1 tanh ϕ / w ϕ 2 2 w ϕ J T q ϕ 2 ,
which is non-positive for all q Ω and becomes zero only at the critical points of ψ . The second term of (A6) is non-positive since k ˙ d 0 by construction and invariance of W (see Proposition 3) implies q q d 2 which, in turn, implies ln q q d 2 0 . Similarly, the sign of each term of the sum is determined solely by the sign of the corresponding k ˙ i . Substituting (12) yields:
i I N ln q q i 2 k ˙ i i I N ln q q i 2 k ¯ i k i w i i g i .
Given that g i q ψ 2 and i I N k ¯ i w i 1 by construction, expanding i into the right-hand side of (A8) leads to:
i I N ln q q i 2 k ¯ i k i w i i g i K u s q ψ 2 i I N k ¯ i k i w i K u s 1 tanh ϕ / w ϕ 2 2 w ϕ q ϕ 2 .
Thus, (A6) is non-positive. Therefore, by invoking Lyapunov’s Stability Theorem (Theorem 3.1 [41]) we may conclude that q d is stable. Finally, LaSalle’s Theorem (Theorem 3.4 [41]) dictates that the system will converge to the largest invariant set, which, in our case, consists of the critical points of ψ , thus concluding the proof.

Appendix A.5. Proof of Proposition 5

At the critical point z of system (17), the Hessian q 2 ϕ of ϕ is non-degenerate, since otherwise k ˙ d 0 . Additionally, q Ω \ { q d } implies that 1 tanh ϕ / w ϕ 2 0 . These two facts mean that q 2 ψ has two eigenvalues at z , namely λ and λ , with λ > 0 . Computing the Jacobian of f q with respect to q at z yields:
q f q = K u q ψ T q s K u s q 2 ψ = K u s q 2 ψ
since q ψ ( q ) = 0 . Furthermore, by construction of the adaptive law (12), the Jacobian of f k with respect to z at z is 0 ( 1 + N ) × ( 3 + N ) . Thus, linearization of the system f z at z yields
z f z ( z ) = K u s 1 tanh ϕ / w ϕ 2 2 w ϕ q 2 ϕ ϕ k d ϕ k 1 ϕ k N 0 ( 1 + N ) × ( 3 + N ) .
Since the top-left block q 2 ϕ is invertible at z , using the well-known property of block matrix determinants, we can see that z f z has two non-zero eigenvalues, particularly the eigenvalues of q 2 ψ and a zero eigenvalue with multiplicity 1 + N . Thus, z f z ( z ) has exactly one positive eigenvalue, rendering z a saddle point of (17) (Theorem 3.7 [41]).

Appendix A.6. Proof of Theorem 1

In Proposition 4, we have proven that ψ ˙ < 0 for all q Ω \ { q d } , except for the critical points of ϕ that lie in it. Lasalle’s Invariance Theorem (Theorem 3.4 [41]) dictates that system (17) will converge to either (a) the desired configuration q d , (b) the obstacles q i or (c) a critical point z = ( q , k ) with q Ω \ { q d } . We know from Proposition 3 that the critical points of case (b) are repulsive; therefore, no trajectory of the system may converge to them. Regarding the critical point z corresponding to case (c), Proposition 5 dictates that it must be a non-isolated, degenerate equilibrium of the whole of system (17), since z f z has one positive, one negative and several zero eigenvalues. Let k ¯ d be the upper bound of k d that the closed-loop system can possibly attain, as indicated by Proposition 2. In order to prove that the set of initial conditions leading to these points has zero Lebesgue measure, we will study the properties of the gradient-like system (by definition, a gradient-like system is a pair of a scalar cost functions and a dynamical system for which each non-equilibrium initial condition moves the state towards a new one whose cost is less than that of the initial state) ( ψ ( z ) , F z , τ ( z ) ) in the domain S z , where the scalar potential ψ ( z ) is treated as a function to be minimized, the map F z , τ ( z ) : S z R N + 3 is given by
F z , τ ( z ( t ) ) z ( t + τ ) = z ( t ) + t t + τ f z ( z ( s ) ) d s
for any τ > 0 and S z D × [ 1 , k ¯ d ] × [ 0 , k ¯ 1 ] × [ 0 , k ¯ N ] . Note that S z is convex and closed. Additionally, the map F z , τ ( z ) is a locally Lipschitz diffeomorphism in S z and S z is forward invariant under F z , τ ( z ) (by virtue of Proposition 3 and design of adaptive law (12)) for all τ > 0 . Furthermore, the unwanted equilibria of F z , τ are strict saddles. Thus, following similar arguments as the proof of Theorem 3 in [42], we conclude that the set of all initial conditions that converge to these saddles has zero Lebesgue measure, which implies that almost every trajectory of the system converges to q d , i.e., the only stable equilibrium of (17), thus completing the proof.

Appendix A.7. Proof of Theorem 2

We begin by noting that, by virtue of (21), we only need to study the trajectories of (19) in the workspace’s image, since that motion is traced exactly by our robot. Considering the first part of the Theorem 2, we note that by following the same arguments as in the proof of Proposition 3, we may conclude that the robot cannot escape throught the workspace’s outer boundary. Likewise, assuming that q q i for some i I N implies that ( n θ T J T q ψ ) T n θ ^ T ( q q i ) cannot be identically zero in a neighborhood of q i . As such, since k ˙ i 0 in the neighborhood of q i , k i cannot vanish as the robot approaches q i , which contradicts our original supposition.
To prove the second part of the Theorem 2, first we show that the only equilibria of the closed-loop system coincide with the critical points of ψ . Assuming that s v 0 , it is readily seen that both inner products in (20) vanish simultaneously only when q ψ = 0 . Considering now the case when s v 0 , we note that this can only happen when q D and n θ ^ is tangent to D . For ω ^ to also vanish when s v 0 , the gradient q ψ should also be tangent to D . Recalling that the adaptive laws for k ensure that q ψ will eventually point inwards, we conclude that no equilibria other than the critical points of ψ exist.
Next, we consider ψ as a lyapunov candidate function, whose derivative along the systems trajectories is given by (A6) (note that ψ does not depend on θ ). Substituting (20) into the first term of (A6) yields:
q ϕ T q ˙ = K v s v 1 tanh ϕ / w ϕ 2 2 w ϕ n θ ^ T q ϕ 2 .
Regarding the remaining terms of (A6), given that g v , i n θ ^ T q ψ 2 , one can readily verify that:
i I N ln q q i 2 k ˙ i K v s v 1 tanh ϕ / w ϕ 2 2 w ϕ n θ ^ T q ϕ 2 .
Thus, invoking Lyapunov’s Stability Theorem (Theorem 3.1 [41]) and LaSalle’s Theorem (Theorem 3.4 [41]) concludes the proof similarly to Proposition 4.

Appendix A.8. Proof of Theorem 3

Regarding the robot’s safety under the closed-loop system, we note that when p P s \ P s + 1 or p P s n for all < n , the individual control laws render every point on the corresponding partition boundaries repulsive. When p C s , s + 1 , we note that, by construction, both u [ s ] and u [ s + 1 ] vanish when the robot approaches any point of P s P s + 1 , preventing the robot from escaping. Additionally, the adaptive laws of each individual potential field will eventually render both p ψ s and p ψ s + 1 inward-looking with respect to W , rendering P s P s + 1 repulsive.
While p P s , we consider V ψ s as a Lyapunov function candidate and we examine its time derivative along the system’s trajectories when p C s , s + 1 :
V ˙ = p ψ s T p ˙ + k [ s ] ψ s T k ˙ [ s ] = p ψ s T u [ s ] + η c , s , s + 1 η t , s , s + 1 u [ s + 1 ] + k ˙ [ s ] ψ s T k [ s ] = p ψ s T u [ s ] + k ˙ [ s ] ψ s T k [ s ] + η c , s , s + 1 η t , s , s + 1 p ψ s T u [ s + 1 ] .
We recall that the first two right-hand side terms of (A12) are non-positive, as shown in Proposition 4. Likewise, the last term is rendered non-positive by virtue of η c , s , s + 1 . Additionally, we note that the equilibrium points of the system in p C s , s + 1 correspond only to critical points of ψ s . By virtue of η c , s , s + 1 , which vanishes at a critical point of ψ s , along with its derivative, one can easily verify that the Jacobian of (22) is equal to the one of u [ s ] , whose properties were studied in Proposition 2. Finally, following a similar procedure as in the proof of Theorem 1, we conclude that the system will converge to the specified goal configuration for almost all initial configurations.

References

  1. Canny, J. The Complexity of Robot Motion Planning; MIT Press: Cambridge, MA, USA, 1988. [Google Scholar]
  2. Mesesan, G.; Roa, M.A.; Icer, E.; Althoff, M. Hierarchical Path Planner Using Workspace Decomposition and Parallel Task-Space RRTs. In Proceedings of the IEEE International Conference on Intelligent Robots and Systems, Madrid, Spain, 1–5 October 2018; pp. 6524–6531. [Google Scholar]
  3. Karaman, S.; Frazzoli, E. Sampling-based algorithms for optimal motion planning. Int. J. Robot. Res. 2011, 30, 846–894. [Google Scholar] [CrossRef]
  4. Chi, W.; Wang, C.; Wang, J.; Meng, M.Q. Risk-DTRRT-Based Optimal Motion Planning Algorithm for Mobile Robots. IEEE Trans. Autom. Sci. Eng. 2019, 16, 1271–1288. [Google Scholar] [CrossRef]
  5. Kavraki, L.E.; Svestka, P.; Latombe, J.; Overmars, M.H. Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Trans. Robot. Autom. 1996, 12, 566–580. [Google Scholar] [CrossRef]
  6. Chen, G.; Luo, N.; Liu, D.; Zhao, Z.; Liang, C. Path planning for manipulators based on an improved probabilistic roadmap method. Robot. Comput.-Integr. Manuf. 2021, 72, 102196. [Google Scholar] [CrossRef]
  7. Salzman, O.; Hemmer, M.; Halperin, D. On the Power of Manifold Samples in Exploring Configuration Spaces and the Dimensionality of Narrow Passages. IEEE Trans. Autom. Sci. Eng. 2015, 12, 529–538. [Google Scholar] [CrossRef]
  8. Kingston, Z.; Moll, M.; Kavraki, L.E. Sampling-Based Methods for Motion Planning with Constraints. Annu. Rev. Control Robot. Auton. Syst. 2018, 1, 159–185. [Google Scholar] [CrossRef]
  9. Ogren, P.; Leonard, N.E. A convergent dynamic window approach to obstacle avoidance. IEEE Trans. Robot. 2005, 21, 188–195. [Google Scholar] [CrossRef]
  10. Wen, J.; Zhang, X.; Gao, H.; Yuan, J.; Fang, Y. E3MoP: Efficient Motion Planning Based on Heuristic-Guided Motion Primitives Pruning and Path Optimization With Sparse-Banded Structure. IEEE Trans. Autom. Sci. Eng. 2021, 19, 2762–2775. [Google Scholar] [CrossRef]
  11. Bhattacharya, S.; Ghrist, R. Path Homotopy Invariants and their Application to Optimal Trajectory Planning. Ann. Math. Artif. Intell. 2018, 84, 139–160. [Google Scholar] [CrossRef]
  12. Diaz-Arango, G.; Vázquez-Leal, H.; Hernandez-Martinez, L.; Pascual, M.T.S.; Sandoval-Hernandez, M. Homotopy Path Planning for Terrestrial Robots Using Spherical Algorithm. IEEE Trans. Autom. Sci. Eng. 2018, 15, 567–585. [Google Scholar] [CrossRef]
  13. Khatib, O. Real-time obstacle avoidance for manipulators and mobile robots. In Proceedings of the IEEE International Conference on Robotics and Automation, St. Louis, MO, USA, 25–28 March 1985; pp. 500–505. [Google Scholar]
  14. Koditschek, D. Exact robot navigation by means of potential functions: Some topological considerations. In Proceedings of the IEEE International Conference on Robotics and Automation, Raleigh, NC, USA, 31 March–3 April 1987; pp. 1–6. [Google Scholar]
  15. Rimon, E.; Koditschek, D. Exact Robot Navigation Using Artificial Potential Functions. IEEE Trans. Robot. Autom. 1992, 8, 501–518. [Google Scholar] [CrossRef]
  16. Loizou, S.G.; Jadbabaie, A. Density Functions for Navigation-Function-Based Systems. IEEE Trans. Autom. Control 2008, 53, 612–617. [Google Scholar] [CrossRef]
  17. Filippidis, I.; Kyriakopoulos, K.J. Adjustable navigation functions for unknown sphere worlds. In Proceedings of the IEEE Conference on Decision and Control and European Control Conference, Orlando, FL, USA, 12–15 December 2011; pp. 4276–4281. [Google Scholar]
  18. Filippidis, I.F.; Kyriakopoulos, K.J. Navigation Functions for everywhere partially sufficiently curved worlds. In Proceedings of the IEEE International Conference on Robotics and Automation, St Paul, MN, USA, 14–18 May 2012; pp. 2115–2120. [Google Scholar]
  19. Paternain, S.; Koditschek, D.E.; Ribeiro, A. Navigation Functions for Convex Potentials in a Space With Convex Obstacles. IEEE Trans. Autom. Control 2018, 63, 2944–2959. [Google Scholar] [CrossRef]
  20. Arslan, O.; Koditschek, D.E. Sensor-based reactive navigation in unknown convex sphere worlds. Int. J. Robot. Res. 2019, 38, 196–223. [Google Scholar] [CrossRef]
  21. Connolly, C.; B Burns, J.; Weiss, R. Path Planning Using Laplace’s Equation. In Proceedings of the IEEE International Conference on Robotics and Automation, Cincinnati, OH, USA, 13–18 May 1990; pp. 2102–2106. [Google Scholar]
  22. Mantegh, I.; Jenkin, M.R.M.; Goldenberg, A.A. Path Planning for Autonomous Mobile Robots Using the Boundary Integral Equation Method. J. Intell. Robot. Syst. 2010, 59, 191–220. [Google Scholar] [CrossRef]
  23. Golan, Y.; Edelman, S.; Shapiro, A.; Rimon, E. Online Robot Navigation Using Continuously Updated Artificial Temperature Gradients. IEEE Robot. Autom. Lett. 2017, 2, 1280–1287. [Google Scholar] [CrossRef]
  24. Kim, J.O.; Khosla, P.K. Real-time obstacle avoidance using harmonic potential functions. IEEE Trans. Robot. Autom. 1992, 8, 338–349. [Google Scholar] [CrossRef]
  25. Merheb, A.; Gazi, V.; Sezer-Uzol, N. Implementation Studies of Robot Swarm Navigation Using Potential Functions and Panel Methods. IEEE/ASME Trans. Mechatron. 2016, 21, 2556–2567. [Google Scholar] [CrossRef]
  26. Velagić, J.; Vuković, L.; Ibrahimović, B. Mobile Robot Motion Framework Based on Enhanced Robust Panel Method. Int. J. Control Autom. Syst. 2020, 18, 1264–1276. [Google Scholar] [CrossRef]
  27. Feder, H.J.S.; Slotine, J.J.E. Real-time path planning using harmonic potentials in dynamic environments. In Proceedings of the IEEE International Conference on Robotics and Automation, Albuquerque, NM, USA, 20–25 April 1997; pp. 874–881. [Google Scholar]
  28. Marchidan, A.; Bakolas, E. A local reactive steering law for 2D collision avoidance with curvature constraints and constant speed. Robot. Auton. Syst. 2022, 155, 104156. [Google Scholar] [CrossRef]
  29. e Silva, E.P.; Engel, P.M.; Trevisan, M.; Idiart, M.A. Exploration Method Using Harmonic Functions. Robot. Auton. Syst. 2002, 40, 25–42. [Google Scholar] [CrossRef]
  30. Maffei, R.; Souza, M.P.; Mantelli, M.; Pittol, D.; Kolberg, M.; Jorge, V.A.M. Exploration of 3D terrains using potential fields with elevation-based local distortions. In Proceedings of the IEEE International Conference on Robotics and Automation, Paris, France, 31 May–31 August 2020; pp. 4239–4244. [Google Scholar]
  31. Voruganti, H.; Dasgupta, B.; Hommel, U. A novel potential field based domain mapping method. In Proceedings of the 10th WSEAS Conference on Computers (ICCOMPP06), Athens, Greece, 13 July 2006; pp. 655–661. [Google Scholar]
  32. Gautam Pradeepkumar, B.; Gondegaon, S.; Voruganti, H.K. Domain Mapping for Path Planning and Mesh Generation. In Proceedings of the International Conference on Theoretical, Applied, Computational and Experimental Mechanics, Kharagpur, India, 29–31 December 2014. [Google Scholar]
  33. Loizou, S.G. Closed form Navigation Functions based on harmonic potentials. In Proceedings of the IEEE Conference on Decision and Control and European Control Conference, Orlando, FL, USA, 12–15 December 2011; pp. 6361–6366. [Google Scholar]
  34. Loizou, S.G. The Navigation Transformation. IEEE Trans. Robot. 2017, 33, 1516–1523. [Google Scholar] [CrossRef]
  35. Loizou, S.G. Navigation functions in topologically complex 3-D workspaces. In Proceedings of the American Control Conference, Montreal, QC, Canada, 27–29 June 2012; pp. 4861–4866. [Google Scholar]
  36. Loizou, S.G. The Multi-Agent Navigation Transformation: Tuning-Free Multi-Robot Navigation. In Proceedings of the Robotics: Science and Systems, Berkeley, CA, USA, 14–16 July 2014; pp. 1–9. [Google Scholar]
  37. Vlantis, P.; Vrohidis, C.; Bechlioulis, C.P.; Kyriakopoulos, K.J. Robot Navigation in Complex Workspaces Using Harmonic Maps. In Proceedings of the IEEE International Conference on Robotics and Automation, Brisbane, Australia, 21–25 May 2018; pp. 1726–1731. [Google Scholar]
  38. Duren, P.; Hengartner, W. Harmonic mappings of multiply connected domains. Pac. J. Math. 1997, 180, 201–220. [Google Scholar] [CrossRef]
  39. Biringen, S.; Chow, C.Y. An Introduction to Computational Fluid Mechanics by Example; John Wiley & Sons, Inc.: Hoboken, NJ, USA, 2011. [Google Scholar]
  40. Kuethe, A.M.; Chow, C.Y.; Fung, Y.C. Foundations of Aerodynamics: Bases of Aerodynamics Design, 5th ed.; John Wiley & Sons, Inc.: Hoboken, NJ, USA, 1997. [Google Scholar]
  41. Khalil, H.K. Nonlinear Systems; Prentice-Hall: Hoboken, NJ, USA, 1996. [Google Scholar]
  42. Panageas, I.; Piliouras, G. Gradient Descent Only Converges to Minimizers: Non-Isolated Critical Points and Invariant Regions. arXiv 2016, arXiv:1605.00405. [Google Scholar]
Figure 1. Transformation of a workspace onto a punctured disk.
Figure 1. Transformation of a workspace onto a punctured disk.
Sensors 23 04464 g001
Figure 2. Discretization of a given domain’s boundary using line segment elements. By convention, the outer boundary is considered to be clockwise oriented, whereas inner boundaries are counter-clockwise oriented. The normal direction of each element is depicted using green colored vectors. Furthermore, the values of the two linear shape functions G ˜ 0 , 3 , 1 and G ˜ 0 , 3 , 2 are plotted along the associated element E ˜ 0 , 3 .
Figure 2. Discretization of a given domain’s boundary using line segment elements. By convention, the outer boundary is considered to be clockwise oriented, whereas inner boundaries are counter-clockwise oriented. The normal direction of each element is depicted using green colored vectors. Furthermore, the values of the two linear shape functions G ˜ 0 , 3 , 1 and G ˜ 0 , 3 , 2 are plotted along the associated element E ˜ 0 , 3 .
Sensors 23 04464 g002
Figure 3. The partition of a complex workspace into overlapping subsets along with the corresponding graph and the tranformation T 2 of the second partition P 2 .
Figure 3. The partition of a complex workspace into overlapping subsets along with the corresponding graph and the tranformation T 2 of the second partition P 2 .
Sensors 23 04464 g003
Figure 4. Robot trajectories illustrated with various colors in both the actual workspace (upper plot) and their image (bottom plot) using a full workspace transformation. The robot starts from an arbitrary location at the bottom left part of the workspace (black circle) and navigates to different goal configurations (colored crosses). Note that the ten black dots in the lower plot correspond to the points where the internal obstacles of the actual workspace have been transformed.
Figure 4. Robot trajectories illustrated with various colors in both the actual workspace (upper plot) and their image (bottom plot) using a full workspace transformation. The robot starts from an arbitrary location at the bottom left part of the workspace (black circle) and navigates to different goal configurations (colored crosses). Note that the ten black dots in the lower plot correspond to the points where the internal obstacles of the actual workspace have been transformed.
Sensors 23 04464 g004
Figure 5. The resulting robot trajectories for various goal configurations depicted with colored crosses, using an atlas of the workspace.
Figure 5. The resulting robot trajectories for various goal configurations depicted with colored crosses, using an atlas of the workspace.
Sensors 23 04464 g005
Figure 6. Robot trajectories from initial configuration (black circle) straight to the desired configuration (black cross) by employing various domain transformation methods in each workspace (ad).
Figure 6. Robot trajectories from initial configuration (black circle) straight to the desired configuration (black cross) by employing various domain transformation methods in each workspace (ad).
Sensors 23 04464 g006
Figure 7. Trajectories of the robot navigating to four distinct goal configurations (black crosses) with red, green, yellow and blue color starting from the same initial position (black circle) while using various alternative APF-based controllers.
Figure 7. Trajectories of the robot navigating to four distinct goal configurations (black crosses) with red, green, yellow and blue color starting from the same initial position (black circle) while using various alternative APF-based controllers.
Sensors 23 04464 g007
Figure 8. Trajectories of the robot navigating to its goal configuration (black cross) generated using the proposed control law and a PRM-based planner.
Figure 8. Trajectories of the robot navigating to its goal configuration (black cross) generated using the proposed control law and a PRM-based planner.
Sensors 23 04464 g008
Figure 9. Trajectories of the unicycle-like robot in the real workspace, with each color (red, green and blue) corresponding to a different goal configuration. Dark gray regions indicate areas where partitions overlap.
Figure 9. Trajectories of the unicycle-like robot in the real workspace, with each color (red, green and blue) corresponding to a different goal configuration. Dark gray regions indicate areas where partitions overlap.
Sensors 23 04464 g009
Figure 10. Snapshots of the robot navigating through the workspace. The figures on the right illustrate the position and orientation of the robot with respect to the workspace map.
Figure 10. Snapshots of the robot navigating through the workspace. The figures on the right illustrate the position and orientation of the robot with respect to the workspace map.
Sensors 23 04464 g010
Figure 11. Robot image trajectories within each partition of the workspace for three experiments in red, green and blue color.
Figure 11. Robot image trajectories within each partition of the workspace for three experiments in red, green and blue color.
Sensors 23 04464 g011
Table 1. Comparison between the Harmonic Transformation (HM) proposed in this work and the (i) Star-to-Sphere Transformation (SST) [15], (ii) Multi-Agent Navigation Transformation (MANT) [36] and (iii) the Navigation Transformation (NT) [34]. Although HMs require global knowledge of the workspace’s geometry to be constructed, HMs are infinitely differentiable and require the domain to be represented by closed polygonal curves (which can be easily obtained using SLAM methodologies), unlike the alternatives that require the domain boundaries to be represented as sets of sufficiently differentiable implicit equations.
Table 1. Comparison between the Harmonic Transformation (HM) proposed in this work and the (i) Star-to-Sphere Transformation (SST) [15], (ii) Multi-Agent Navigation Transformation (MANT) [36] and (iii) the Navigation Transformation (NT) [34]. Although HMs require global knowledge of the workspace’s geometry to be constructed, HMs are infinitely differentiable and require the domain to be represented by closed polygonal curves (which can be easily obtained using SLAM methodologies), unlike the alternatives that require the domain boundaries to be represented as sets of sufficiently differentiable implicit equations.
Geometry RepresentationGlobalAnalytic
HMPoints on the boundaryYesYes
SSTTrees of StarsYesYes
NT C 2 -manifoldsNoNo
MANTTrees of C 2 -manifoldsNoNo
Table 2. Comparison of Adaptive Harmonic Potential Fields (proposed herein) with common alternatives, specifically Rimon–Koditchek Navigation Functions (RKNF) [15], Harmonic Navigation Functions [33] and approximate Harmonic Potential Fields obtained using numerical techniques [21]. Unlike RKNFs that require tuning for ensuring convergence to the goal from almost all initial configurations and HNFs that require tuning for guaranteeing collision avoidance with the workspace boundaries, the proposed control law enjoys both properties by design.
Table 2. Comparison of Adaptive Harmonic Potential Fields (proposed herein) with common alternatives, specifically Rimon–Koditchek Navigation Functions (RKNF) [15], Harmonic Navigation Functions [33] and approximate Harmonic Potential Fields obtained using numerical techniques [21]. Unlike RKNFs that require tuning for ensuring convergence to the goal from almost all initial configurations and HNFs that require tuning for guaranteeing collision avoidance with the workspace boundaries, the proposed control law enjoys both properties by design.
ConvergenceCollision AvoidanceComputational Cost
AHPFBy designBy designCheap
HPFBy designBy designExpensive
RKNFRequires tuningBy designCheap
HNFBy designRequires tuningCheap
Table 3. Trajectory lengths (m) executed by employing the four alternative transformations in each workspace displayed in Figure 6.
Table 3. Trajectory lengths (m) executed by employing the four alternative transformations in each workspace displayed in Figure 6.
abcd
SST3.634.182.124.09
MANT4.264.692.344.45
NT3.354.302.184.22
HM3.194.212.054.32
Table 4. Maximum value of curvature (m−1) associated with each trajectory displayed in Figure 6.
Table 4. Maximum value of curvature (m−1) associated with each trajectory displayed in Figure 6.
abcd
SST4.225.4386.932.16
MANT1.231.4713.562.06
NT66.9725.2314.896.92
HM2.472.4914.762.77
Table 5. Minimum distance (m) between each robot trajectory and the corresponding workspace boundaries displayed on Figure 6.
Table 5. Minimum distance (m) between each robot trajectory and the corresponding workspace boundaries displayed on Figure 6.
abcd
SST0.03030.02830.01590.0063
MANT0.06440.12530.18700.0648
NT0.13860.05060.09150.0058
HM0.03350.03770.01030.0181
Table 6. Length of trajectories (m) executed by each controller (Rimon–Koditchek Navigation Functions, Harmonic Navigation Functions and Adaptive Harmonic Potential Fields) in Figure 7.
Table 6. Length of trajectories (m) executed by each controller (Rimon–Koditchek Navigation Functions, Harmonic Navigation Functions and Adaptive Harmonic Potential Fields) in Figure 7.
RedGreenBlueYellow
NF19.78120.42722.09018.397
HNF18.22422.53826.95920.062
AHNF17.87419.41923.36418.595
Table 7. Minimum distance (m) between the corresponding workspace boundaries and each trajectory displayed in Figure 7.
Table 7. Minimum distance (m) between the corresponding workspace boundaries and each trajectory displayed in Figure 7.
RedGreenBlueYellow
NF0.11580.01020.12100.1103
HNF0.33470.21350.25910.2166
AHPF0.13100.03520.20430.1854
Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content.

Share and Cite

MDPI and ACS Style

Vlantis, P.; Bechlioulis, C.P.; Kyriakopoulos, K.J. Robot Navigation in Complex Workspaces Employing Harmonic Maps and Adaptive Artificial Potential Fields. Sensors 2023, 23, 4464. https://doi.org/10.3390/s23094464

AMA Style

Vlantis P, Bechlioulis CP, Kyriakopoulos KJ. Robot Navigation in Complex Workspaces Employing Harmonic Maps and Adaptive Artificial Potential Fields. Sensors. 2023; 23(9):4464. https://doi.org/10.3390/s23094464

Chicago/Turabian Style

Vlantis, Panagiotis, Charalampos P. Bechlioulis, and Kostas J. Kyriakopoulos. 2023. "Robot Navigation in Complex Workspaces Employing Harmonic Maps and Adaptive Artificial Potential Fields" Sensors 23, no. 9: 4464. https://doi.org/10.3390/s23094464

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Metrics

Back to TopTop