Reactive navigation in partially familiar planar environments using semantic perceptual feedback

Abstract

This article solves the planar navigation problem by recourse to an online reactive scheme that exploits recent advances in simultaneous localization and mapping (SLAM) and visual object recognition to recast prior geometric knowledge in terms of an offline catalog of familiar objects. The resulting vector field planner guarantees convergence to an arbitrarily specified goal, avoiding collisions along the way with fixed but arbitrarily placed instances from the catalog as well as completely unknown fixed obstacles so long as they are strongly convex and well separated. We illustrate the generic robustness properties of such deterministic reactive planners as well as the relatively modest computational cost of this algorithm by supplementing an extensive numerical study with physical implementation on both a wheeled and legged platform in different settings.

Keywords

Reactive motion planning collision avoidance vision and sensor-based control

1. Introduction

This article advances the formally demonstrable capabilities of online reactive motion planners by disentangling the topology of navigation from the geometry of perception. Specifically, we appeal to a semantically aware perceptual oracle for the instantaneous recognition and localization of previously memorized objects and abstract away their geometric details in realtime as they are encountered, yielding a sequence of topologically representative motion planning problems solvable by recourse to purely reactive online methods. Recent advances in simultaneous localization and mapping (SLAM) and visual object recognition afford a working empirical realization of this provably correct scheme in both quasi-static and highly dynamic physical planar robots.

1.1. Motivation and prior work

Even as legged (Ilhan et al., 2018; Johnson et al., 2011; Wooden et al., 2010) and aerial (Amazon, Inc., 2014; Gao et al., 2019; Mohta et al., 2018; Tang and Kumar, 2018) robots engage increasingly realistic, unstructured environments, intuition suggests that prior experience ought to yield deterministic navigation guarantees, postponing statistical predictions of performance to estimated (Trautman et al., 2015), learned (Henry et al., 2010), or simulated (Karaman and Frazzoli, 2012) characterizations of truly bewilderingly dense or moving environments. Similarly, sampling-based methods, motivated by the typically high-dimensional configuration spaces arising from combined task and motion planning (Garrett et al., 2018), can achieve asymptotic optimality (Vega-Brown and Roy, 2018), but no guarantee of convergence (or task completion) under partial prior knowledge or limited sampling, and their probabilistic completeness guarantees can be slow to be realized in practice when confronting settings with narrow passages (Noreen et al., 2016), even in 2D environments. More importantly, our recent parallel work (Vasilopoulos et al., 2020), that uses the reactive planning principles presented in this article, shows that existing state-of-the-art path replanning algorithms for unknown 2D environments (Otte and Frazzoli, 2015) can cycle repeatedly in the presence of both unforeseen obstacles and narrow passages as they search for alternative openings, before eventually (and after protracted cycling) reporting failure (incorrectly) and halting.

1.1.1. Reactive navigation

Heretofore, deterministically safe, convergent reactive methods have required substantial prior knowledge of a static environment, whether encoded using navigation functions (Filippidis and Kyriakopoulos, 2012; Lionis et al., 2008; Rimon and Koditschek, 1992), harmonic potential functions (Conner et al., 2011; Vlantis et al., 2018), or pre-computed sequences of “funnels” (Majumdar and Tedrake, 2017). In contrast, sensor-driven planners in this general tradition (Borenstein and Koren, 1989, 1991; Brock and Khatib, 1999; Fiorini and Shiller, 1998; Johnson et al., 2011; Khatib, 1986; Paranjape et al., 2015; Simmons, 1996; van den Berg et al., 2011, 2008) have guaranteed collision avoidance but have offered no assurance of convergence to a designated goal.

Recent advances in the theory of sensor-based navigation (Arslan and Koditschek, 2016a,b, 2017) relying on the properties of metric projections on convex sets (Kuntz and Scholtes, 1994) (and other parallel approaches (Ataka et al., 2018; Chang and Marsden, 2003; Ilhan et al., 2018; Paternain et al., 2017)) add the key feature of guaranteed convergence to a designated goal, by trading away prior knowledge for the presumption of simplicity: unknown obstacles can be successfully negotiated in realtime without losing global convergence guarantees if they are “round” (i.e., very strongly convex in a sense made precise in Arslan and Koditschek (2018)).

However, this presumption, along with the additional requirement for enough separation between the obstacles in the workspace, limit the domain of application for such methods to geometrically simple environments and might prohibit successful navigation in complicated, unstructured environments with non-convex geometry. Hence, other reactive approaches either seek to appropriately modify the input reference signal to account for unanticipated (potentially non-convex) disturbances (Revzen et al., 2012), or rely on stochastic frameworks that are empirically shown to improve performance with non-convex obstacles (Reverdy et al., 2015), with no guarantees of convergence.

1.1.2. Realtime perception

In this work, we address these shortcomings by appeal to an agent’s memory evoked by execution-time perceptual cues. Recent advances in semantic SLAM (Atanasov et al., 2016; Bowman et al., 2017) and object pose extraction using convolutional neural net architectures (Kar et al., 2015; Kong et al., 2017; Pavlakos et al., 2017) now provide an avenue for systematically composing partial prior knowledge about the robot’s workspace within a deterministic framework well suited to the vector field planning methods reviewed above.

Contrasting recent work has recruited end-to-end learning to achieve obstacle avoiding reactions within semantically labeled representations of familiar environments (Gupta et al., 2017), or supplemented such deep-learned representations with reference paths (Kumar et al., 2018), or optimally generated waypoint sequences (Bansal et al., 2019) that guide the robot to its destination. Although such approaches cannot guarantee safe convergence to the robot’s destination, they promote the importance of landmark-based navigation, already highlighted by parallel work in biology (Jayakumar et al., 2019). However, characteristically, the input to such architectures is raw visual data thereby generating egocentric reactions that are hostage to the experience of one particular environment. In contrast, our compositional use of semantically tagged, learned-object recognizers affords systematic re-use across many different environments and achieves formal deterministic guarantees as well, at least up to their (admittedly still far from formally justifiable) idealization as perfect realtime perceptual oracles.

1.1.3. Topologically informed navigation

Work on the topology of motion planning (Farber, 2006; Farber et al., 2019) has overtaken earlier investigation of reactive (i.e., vector field) navigation planners (Rimon, 1990; Rimon and Koditschek, 1989) to the point that, comparatively, only preliminary results on their intrinsic limitations have been reported (Baryshnikov and Shapiro, 2014). It seems clear that our success in achieving such strong results for a broad class of partially known environments is due to the simplicity of the problem class (punctured 2D manifolds have the homotopy type of a bouquet of circles), but we are not in a position to opine firmly on the likely limitations of this approach in higher-dimensional settings.

Recently, several contributions have focused on either finding invariants for homology classes to facilitate optimal path search in known environments (Bhattacharya et al., 2015), exploiting data to enforce topological constraints (Pokorny and Kragic, 2015), or conceptualizing sensor measurements related to the shape of an object in a topologically meaningful way using persistent homology (Mueller and Birk, 2018). In contrast, we extract geometric and topological information about the robot’s workspace at execution time in order to construct a map between a geometrically complicated mapped space and a (topologically equivalent but geometrically simple) model space that can be used for planning purposes. To this end, we employ methods from the field of computational geometry for implicit description of geometric shape using R-functions (Shapiro, 2007), convex decomposition (Keil, 1985), and logic operations with polygons (Clementini et al., 1993; Douglas and Peucker, 1973; Egenhofer and Herring, 1991).

1.2. Summary of contributions

We consider the navigation problem in a 2D workspace cluttered with unknown convex obstacles, along with “familiar” non-convex obstacles that belong to classes of known geometries, but whose number and placement are a priori unknown. We assume a limited-range onboard sensor and a catalog of known obstacles, along with a “mapping oracle” for their online identification and localization in the physical workspace. This framework allows the robot to explore the geometry and topology of its workspace in realtime as it navigates toward its goal, by recognizing and incorporating in its stored semantic map “familiar” obstacles, whose number and placement are otherwise unknown, awaiting discovery at execution time.

Based on the aforementioned description, we propose a representation of the environment taking the form of a “multi-layer” collection of topological spaces whose realtime interaction can be exploited to integrate the geometrically naive sensor-driven methods of Arslan and Koditschek (2016b) with the offline geometrically sensitive methods of Rimon and Koditschek (1992).

Specifically, we adapt the construction of Rimon and Koditschek (1989) to generate a realtime smooth change of coordinates (a diffeomorphism) of the mapped space of the environment into a (locally) topologically equivalent but geometrically more favorable model space, relative to which the sensor-based reactive methods of Arslan and Koditschek (2016b) can be applied directly. We prove that the conjugate vector field defined by appropriately transforming the reactive model space back through this diffeomorphism induces a vector field on the robot’s physical configuration space that inherits the same formal guarantees of obstacle avoidance and convergence. As the robot’s knowledge about the geometry and topology of its workspace at execution time is constantly updated, we adopt a hybrid dynamical systems description of our navigation framework, and show that the resulting hybrid system both inherits the consistency properties outlined in Johnson et al. (2016) and safely drives the robot to the goal without violating given command limits.

We extend the construction to the case of a differential drive robot, by pulling back the extended field over planar rigid transformations introduced for this purpose in Arslan and Koditschek (2016b) through a suitable polar coordinate transformation of the tangent lift of our original planar diffeomorphism and demonstrate, once again, that the physical differential drive robot inherits the same obstacle avoidance and convergence properties as those guaranteed for the geometrically simple model robot (Arslan and Koditschek, 2016b).

We believe that this is the first doubly reactive controller (Arslan and Koditschek, 2016b) (i.e., a navigation framework wherein not only the robot’s trajectory but also the control vector field that generates it are computed online at execution time) that can handle arbitrary polygonal shapes in realtime without the need for specific separation assumptions between the familiar obstacles, by combining perception and object recognition for the familiar obstacles with local range measurements (e.g., LIDAR) for the unknown obstacles, to yield provably correct navigation in geometrically complicated environments. Furthermore, unlike Rapidly-exploring Random Trees (RRT)-based (LaValle and Kuffner, 2001) or Probabilistic RoadMap (PRM)-based (Kavraki et al., 1996) algorithms, and similarly to other vector-field based approaches, our framework is capable of solving the overall “kinodynamic” problem online, instead of executing separate trajectory and motion planning, for both a fully actuated particle and a differential drive robot.

Finally, by coupling the semantic SLAM framework of (Bowman et al., 2017) and the object detection pipeline of (Pavlakos et al., 2017) with our reactive planning architecture, we are able to localize against isolated semantic cues while navigating, instead of localizing against entire scenes (Gupta et al., 2017) or visual geometric features (Hesch et al., 2014). Therefore, by training just on data from the objects the robot is expected to encounter, we introduce modularity and robustness in our approach, while simultaneously performing online planning that does not rely on specific features of a deep network architecture (e.g., number or type of layers) (Bansal et al., 2019; Gupta et al., 2017; Kumar et al., 2018).

1.3. Organization of the article

The article is organized as follows. Section 2 describes the problem and establishes our assumptions. Section 3 describes the physical, semantic, mapped, and model planning spaces (summarized in Figure 1) used in the diffeomorphism construction between the mapped and model spaces, whose properties are established next in Section 4. Section 5 provides the formal hybrid systems description framework and the correctness proofs for both a fully actuated (Theorem 3) and differential drive (Theorem 4) velocity-controlled planar robot, comprising the central theoretical contribution of this article.

Based on these results, Section 6 continues with a description of the implemented mapped space recovery and reactive planning algorithms, for both a fully actuated and a differential drive robot, shown in Figure 2(d) and (e). Section 7 presents a variety of illustrative numerical studies, and Section 8 continues with a brief description of the experimental setup, realizing the deployed perception (relying on prior work and shown in Figure 2(a) and (c)) and motion planning (Figure 2(d) and (e)) algorithms on both the Turtlebot (TurtleBot2, 2019) and the Minitaur (Ghost Robotics, 2016) robot. Section 9 continues with our experimental results, and Section 10 concludes by summarizing our findings and sketching some of the future work now in progress building on these results. Finally, the appendix provides details of computational methods and proofs.

2. Problem formulation

Similarly to Vasilopoulos and Koditschek (2018), we consider a disk-shaped robot with radius $r > 0$ , centered at $x \in R^{2}$ , navigating a closed, compact, polygonal, potentially non-convex workspace $W \subset R^{2}$ , with known outer boundary $\partial W$ , towards a target location $x_{d} \in W$ . The robot is assumed to possess a sensor with fixed range $R$ , capable of recognizing “familiar” objects, as well as estimating the distance of the robot to nearby obstacles.¹ We also define the enclosing workspace, as the convex hull of the closure of the workspace $W$ :

W_{e} : = {x \in R^{2} | x \in Conv (\bar{W})}

(1)

The workspace is cluttered by a finite, unknown number of fixed, disjoint obstacles, denoted by $\tilde{O} : = {{\tilde{O}}_{1}, {\tilde{O}}_{2}, \dots}$ . By convention, the set $\tilde{O}$ also includes potentially non-convex “intrusions” of the boundary of the physical workspace $W$ into the enclosing workspace $W_{e}$ , that can be described as the connected components of $W_{e} \ W$ . We use the notation in Arslan and Koditschek (2016b) and define the freespace as

F : = {x \in W_{e} | \bar{B (x, r)} \subseteq W_{e} \ ⋃_{i} {\tilde{O}}_{i}}

(2)

where $B (x, r)$ is the open ball centered at $x$ with radius $r$ , and $\bar{B (x, r)}$ denotes its closure. Similarly to the enclosing workspace, $W_{e}$ , we define the enclosing freespace, $F_{e}$ as

F_{e} : = {x \in R^{2} | x \in Conv (\bar{F})}

(3)

Although none of the positions of any obstacles in $\tilde{O}$ are a priori known, a subset $\tilde{P} : = {{\tilde{P}}_{i}}_{i \in N_{P}} \subseteq \tilde{O}$ of these obstacles, indexed by $N_{P} : = {1, \dots, N_{P}} \subset N$ , is assumed to be “familiar” in the sense of having an a priori known, readily recognizable, potentially non-convex, polygonal geometry (i.e., belonging to a known catalog of geometry classes), which the robot can identify and localize instantaneously from online sensory measurement, as described in Section 8. We require that this subset also includes all connected components of $W_{e} \ W$ . The remaining obstacles in $\tilde{C} : = \tilde{O} \ \tilde{P}$ , indexed by $N_{C} : = {1, \dots, N_{C}} \subset N$ are assumed to be strictly convex but are in all other regards (location and specific shape) completely unknown to the robot, while nevertheless satisfying a curvature condition given in Arslan and Koditschek (2016b: Assumption 2).This condition is interpreted in Arslan and Koditschek (2016b) as the requirement for each convex obstacle $\tilde{C} \in \tilde{C}$ to be sufficiently “round”, because mere obstacle convexity with flat edges can still generate undesired local minima when using a greedy reactive controller.

To simplify our notation, we neglect the robot dimensions, by dilating each obstacle in $\tilde{O}$ by $r$ , and assume that the robot operates in $F$ . We denote the set of dilated obstacles derived from $\tilde{O}, \tilde{P}$ and $\tilde{C}$ , by $O, P$ and $C$ , respectively. As obstacles in $\tilde{P}$ are polygonal, and dilations of polygonal obstacles are not in general polygonal, we approximate obstacles in $P$ with conservative polygonal supersets. Note that because the set $\tilde{P}$ is required to contain all connected components of $W_{e} \ W$ , that describe non-convex “intrusions” of the boundary of the physical workspace $W$ into the enclosing workspace $W_{e}$ , the set $P$ is similarly required to contain the dilations of these intrusions. For obstacles in $C$ we require the following separation assumptions, introduced in Arslan and Koditschek (2016b).

Assumption 1. Each obstacle $C_{i} \in C$ has a positive clearance ² $d (C_{i}, C_{j}) > 0$ from any obstacle $C_{j} \in C$ , with $i \neq j$ , and a positive clearance $d (C_{i}, \partial F) > 0$ from the boundary of the freespace $F$ .

Then, similarly to (Rimon and Koditschek, 1992), we describe each polygonal obstacle $P_{i} \in P \subseteq O$ by an obstacle function, a real-valued map providing an implicit representation of the form

P_{i} = {x \in R^{2} | β_{i} (x) \leq 0}

(4)

that the robot can construct online from the cataloged geometry after it has localized $P_{i}$ , as detailed in Appendix B. We also require the following technical assumption.

Assumption 2. For each $P_{i} \in P$ , there exists $ϵ_{i} > 0$ such that the set $S_{β_{i}} : = {x | β_{i} (x) \leq ϵ_{i}}$ has a positive clearance $d (S_{β_{i}}, C) > 0$ from any obstacle $C \in C$ .

Note that Assumptions 1 and 2 constrain the shape (convex) and placements (sufficiently separated) only of obstacles that have never previously been encountered. Familiar (polygonal, dilated by $r$ ) obstacles $P_{i} \in P$ , while fixed, can be placed completely arbitrarily with no further prior information: in particular, they can overlap unrestrictedly, with no jeopardy to our formal results, because we rely on the sensor oracle to recognize and locate them in realtime. Obstacles in $P$ are similarly allowed to overlap with the boundary of the enclosing freespace $\partial F_{e}$ . To control the scope of the present article, we simply assume that a path to the goal always exists, i.e., the robot operates in a non-adversarial environment.

Assumption 3. The freespace $F$ is path-connected.

Finally, in Section 5.2, we impose the technical Assumption 4 precluding the possibility that any of the (topologically unavoidable) unstable saddle points of our control law coincide with a cataloged “knot point” of any familiar obstacle (a condition that we conjecture should be generic in the configuration space of obstacle placements).

Based on these assumptions and further positing first-order, fully actuated robot dynamics $\overset{\cdot}{x} = u (x)$ , the problem consists of finding a Lipschitz continuous controller $u : F \to R^{2}$ , that leaves the freespace $F$ positively invariant and asymptotically steers almost all configurations in $F$ to the given goal $x_{d} \in F$ . We have also summarized key symbols used throughout this article in Table 1.

Table 1.

Key symbols used throughout this article, associated with the problem formulation in Section 2. See also Table 2 for notation associated with the environment representation in Section 3, Table 3 for notation associated with the diffeomorphism construction in Section 4, and Table 4 for notation associated with our reactive controller in Section 5.

$W \subset R^{2}$	Closed, compact, polygonal, potentially non-convex workspace
$W_{e} \subset R^{2}$	Enclosing workspace (1)
$F \subset W$	Freespace (2)
$F_{e} \subset W_{e}$	Enclosing freespace (3)
$r \in R$	Robot radius
$R \in R$	Sensor range
$x_{d} \in F$	Goal location
$\tilde{O} : = {{\tilde{O}}_{1}, {\tilde{O}}_{2}, \dots} \subseteq R^{2}$	Set of fixed, disjoint obstacles
$\tilde{P} : = {{\tilde{P}}_{i}}_{i \in N_{P}} \subseteq \tilde{O}$	Set of “familiar,” polygonal obstacles, indexed by the set $N_{P} : = {1, \dots, N_{P}} \subset N$
$\tilde{C} : = \tilde{O} \ \tilde{P} = {{\tilde{C}}_{i}}_{i \in N_{C}}$	Set of completely unknown obstacles, indexed by the set $N_{C} : = {1, \dots, N_{C}} \subset N$
$O, P, C$	Set of obstacles in $\tilde{O}, \tilde{P}, \tilde{C}$ respectively, dilated by the robot radius, $r$

3. Navigational representation of the environment

In this section, we introduce associated notation for the four distinct representations of the environment that we will refer to as planning spaces and use in the construction of our algorithm. Figure 1 illustrates the role of these spaces and the transformations that relate them in constructing and analyzing a realtime generated vector field that guarantees safe passage to the goal. The notation related to the environment representation in this section is listed in Table 2. The new technical contribution is an adaptation of the methods of Rimon and Koditschek (1989) to the realtime construction of a diffeomorphism, $h$ , where the requirement for fast, online performance demands an algorithm that is as simple as possible and with few tunable parameters. Hence, because the reactive controller in Arslan and Koditschek (2016b) is designed to (provably) handle convex shapes, sensed obstacles not recognized by the semantic SLAM process are simply assumed to be convex (implemented by designing $h$ to resolve to the identity transformation in the neighborhood of “unfamiliar” objects) and the control response defaults to that prior construction.

Table 2.

Key symbols related to the environment representation in Section 3.

${\tilde{P}}_{I} : = {{\tilde{P}}_{i}}_{i \in I} \subseteq \tilde{P}$	Set of (constantly updated) instantiated familiar polygonal obstacles, Section 3.1
$I \subseteq N_{P}$	Index set of the $\| I \|$ presently instantiated obstacles in ${\tilde{P}}_{I}$ , Section 3.1
$F_{s em}^{I}$	Semantic space corresponding to $I \in 2^{N_{P}}$ , Section 3.2
$F_{map}^{I}$	Mapped space corresponding to $I \in 2^{N_{P}}$ , Section 3.3
$F_{model}^{I}$	Model space corresponding to $I \in 2^{N_{P}}$ , Section 3.4
$P_{s em}^{I} : = \underset{i \in I}{∐} P_{i}$	Set of familiar, polygonal obstacles instantiated in the semantic space, Section 3.2
$C_{s em} : = {C_{i}}_{i \in J_{C}} \subseteq C$	Set of unknown obstacles in the semantic space, indexed by $J_{C} \subseteq N_{C}$ , Section 3.2
$P_{map}^{I} : = ⋃_{i \in I} P_{i} = {P_{i}}_{i \in J^{I}}$	Set of consolidated familiar obstacles in the mapped space, indexed by $J^{I}$ , Section 3.3
$C_{map} : = C_{s em}$	Set of unknown obstacles in the mapped space, indexed by $J_{C} \subseteq N_{C}$ , Section 3.3
$B_{map}^{I} : = {B_{i}}_{i \in J_{B}^{I}}$	Connected components of $P_{map}^{I}$ to be merged into $\partial F_{e}$ , indexed by $J_{B}^{I}$ , Section 3.3
$D_{map}^{I} : = {D_{i}}_{i \in J_{D}^{I}}$	Connected components of $P_{map}^{I}$ to be deformed into disks, indexed by $J_{D}^{I}$ , Section 3.3
$x_{i}^{*} \in R^{2}, i \in J_{D}^{I}$	Centers of the $\| J_{D}^{I} \|$ disks in the model space, Section 3.4
$ρ_{i} \in R, i \in J_{D}^{I}$	Radii of the $\| J_{D}^{I} \|$ disks in the model space, Section 3.4

3.1. Physical space

The physical space is a complete description of the geometry of the unknown actual world and while inaccessible to the robot is used for purposes of analysis. It describes the enclosing workspace $W_{e}$ , punctured with the obstacles $\tilde{O}$ . This gives rise to the freespace $F$ , given in (2), consisting of all placements of the robot’s centroid that entail no intersections of its body with any interior obstacles or intrusions from the boundary. The robot navigates this space toward the goal, discovering and localizing new obstacles along the way. Those discovered obstacles which are not convex are (by assumption) “familiar” and are then “instantiated” (recalled, and registered from memory to populate the accumulating record of discovery in the semantic space) as we next discuss. Those which are “unfamiliar” are presumed convex and registered as such in the companion spaces next to be presented.³

We denote by ${\tilde{P}}_{I} : = {{\tilde{P}}_{i}}_{i \in I} \subseteq \tilde{P}$ the finite set of (constantly updated) physically “instantiated” familiar objects, indexed by $I \subseteq N_{P}$ , that drives the construction of the semantic, mapped and model spaces described next. As explained in Section 5.1, such elements $I$ of the power set $2^{N_{P}}$ also index the modes of our hybrid system.

3.2. Semantic space

The semantic space $F_{s em}^{I}$ records the robot’s evolving information about the environment aggregated from the raw sensor data about the observable portions of a subset of unrecognized (and, therefore, presumed convex) obstacles from $C$ , together with the polygonal boundaries of the $| I |$ familiar obstacles, that are instantiated at the moment the sensory data triggers the identification and localization a familiar obstacle.

Definition 1. A familiar obstacle $\tilde{P} \in \tilde{P}$ is considered to be “instantiated,” if it has been sensed using the robot’s sensor of fixed range $R$ , recognized, localized, and its dilation $P \in P$ is permanently included in the semantic space. ⁴

We denote the set of unrecognized obstacles in the semantic space by $C_{s em} : = {C_{i}}_{i \in J_{C}}$ , indexed by $J_{C} \subseteq N_{C}$ , and the set of familiar obstacles in the semantic space by $P_{s em}^{I} : = \underset{i \in I}{∐} P_{i}$ .

It is important to note that this environment is constantly updated, both by discovering and storing new familiar obstacles in the semantic map and by discarding old information and storing new information regarding obstacles in $C$ .³ Here, the robot is treated as a point particle, since all obstacles are dilated by $r$ in the passage from the workspace to the freespace representation of valid placements.

3.3. Mapped space

Although the semantic space contains all the relevant geometric information (identity and pose) about the obstacles the robot has encountered, it does not explicitly contain any topological information about the explored environment, as represented by the disjoint union operation in the definition of $P_{s em}^{I}$ . This is because Assumption 2 does not exclude overlaps between obstacles in $P$ . Their algorithmically effective consolidation in realtime reduces the number while increasing the geometric complexity of the actual freespace obstacles the robot must negotiate along the way to the goal. To do so, we need therefore to take unions of overlapping obstacles in $P_{s em}^{I}$ , making up $P_{map}^{I} : = ⋃_{i \in I} P_{i} = {P_{i}}_{i \in J^{I}}$ (i.e., a new set of consolidated familiar obstacles indexed by $J^{I}$ with $| J^{I} | \leq | I |$ ), as well as copies of the sensed fragments of unknown obstacles from $C_{s em}$ (i.e., $C_{map} : = C_{s em}$ ) to form the mapped space $F_{map}^{I}$ as

F_{map}^{I} : = F_{e} \ (P_{map}^{I} \cup C_{map})

(5)

Note that, by Assumption 2, the convex obstacles are assumed to be far enough away from the familiar obstacles, such that no overlap occurs in the above union.

Next, we focus on the connected components of $P_{map}^{I}$ ; because Assumption 2 allows overlaps between obstacles in $P$ and the boundary of the enclosing freespace $\partial F_{e}$ , for any connected component $P$ of $P_{map}^{I}$ such that $P \cap \partial F_{e} \neq \emptyset$ , we take $B : = P \cap F_{e}$ and include $B$ in a new set $B_{map}^{I}$ , indexed by $J_{B}^{I}$ . The rest of the connected components in $P_{map}^{I}$ , which do not intersect $\partial F_{e}$ , are included in a separate set $D_{map}^{I}$ , indexed by $J_{D}^{I}$ . The idea here is that obstacles in $B_{map}^{I}$ should be merged to the boundary of the enclosing freespace $\partial F_{e}$ , and obstacles in $D_{map}^{I}$ should be deformed to disks, because $F_{map}^{I}$ and $F_{model}^{I}$ need to be diffeomorphic.

3.4. Model space

The model space $F_{model}^{I}$ has the same boundary as $F_{e}$ (i.e., $\partial F_{model}^{I} : = \partial F_{e}$ ) and consists of copies of the sensed fragments of the $| J_{C} |$ unrecognized visible convex obstacles in $C_{map}$ , and a collection of $| J_{D}^{I} |$ Euclidean disks corresponding to the $| J_{D}^{I} |$ consolidated obstacles in $D_{map}^{I}$ that are deformed to disks. The centers⁵ ${x_{i}^{*}}_{i \in J_{D}^{I}}$ and radii ${ρ_{i}}_{i \in J_{D}^{I}}$ of the $| J_{D}^{I} |$ disks are chosen so that $\bar{B (x_{i}^{*}, ρ_{i})}$ is contained in the interior of $D_{i} \in D_{map}^{I}$ , as required in Rimon and Koditschek (1989). The obstacles in $B_{map}^{I}$ are merged into $\partial F_{e}$ , to make $F_{map}^{I}$ and $F_{model}^{I}$ topologically equivalent, through a map $h^{I} : F_{map}^{I} \to F_{model}^{I}$ . We can, therefore, write $F_{model}^{I}$ as

F_{model}^{I} = F_{e} \ (⋃_{i \in J_{D}^{I}} \bar{B (x_{i}^{*}, ρ_{i})} \cup C_{map})

(6)

Note here that we need to distinguish between familiar but unanticipated obstacles in $D_{map}^{I}$ and $B_{map}^{I}$ , and completely unknown convex obstacles in $C_{map}$ , because our algorithm handles them quite differently. From a practical point of view, the robot instantiates familiar obstacles using its full sensor suite equipped with a learned catalog, while completely unknown obstacles that the robot can only partially observe are handled solely based on distance measurements (e.g., LIDAR). More importantly, from a formal point of view, this distinction is needed in the definition of our global diffeomorphism (to be presented next in Section 4), because only the familiar obstacles affect its construction. Moreover, the previously unseen unfamiliar obstacles cannot be simply registered in the model space, but they must be formally mapped there through that diffeomorphism, which is constructed so that it defaults to the identity transform on their boundaries.

3.5. Implicit representation of obstacles

We note here that the construction of the map $h^{I}$ between the mapped space $F_{map}^{I}$ and the model space $F_{model}^{I}$ , described next in Section 4 and shown in Figure 1, relies on the existence of smooth implicit functions $β : R^{2} \to R$ for polygons, constructed such that $β (x) = 0$ implies that $x$ lies on the boundary of the polygon. Although the construction of such functions is a separate problem on its own, here we derive implicit representations using so-called “R-functions” from the constructive solid geometry literature (Shapiro, 2007). In Appendix B, we describe the method used for the construction of such implicit functions for polygonal obstacles that have the desired property of being analytic everywhere except for the polygon vertices. We have already successfully implemented such constructions in a similar setting for star-shaped obstacles in Vasilopoulos and Koditschek (2018).

4. The diffeomorphism between the mapped and model spaces

In this section, we describe our method of constructing the diffeomorphism, $h^{I}$ , between the mapped space $F_{map}^{I}$ and the model space $F_{model}^{I}$ . We assume that the robot has already recognized, localized, and stored the $| J^{I} |$ consolidated familiar polygonal obstacles in $P_{map}^{I}$ in its map, and has subsequently identified obstacles to be merged to the boundary of the enclosing freespace $\partial F_{e}$ , stored in $B_{map}^{I}$ , and obstacles to be deformed to disks, stored in $D_{map}^{I}$ .

The idea is then to compose a sequence of “purging” diffeomorphisms, which coincide with the identity map except on a small “collar” around each component of $P_{map}^{I}$ , that produce successively less-complicated isolated shapes. The final simplified shapes are then conveniently deformed into a disk if the corresponding obstacle belongs in $D_{map}^{I}$ , or into the boundary of $F_{e}$ if the corresponding obstacle belongs in $B_{map}^{I}$ , in order to generate the model space $F_{model}^{I}$ . Before describing these transformations, we first provide some background on the used obstacle representation methodology in Section 4.1, and provide associated notation in Table 3.

Table 3.

Key symbols related to the diffeomorphism construction from $F_{map}^{I}$ to $F_{model}^{I}$ , described in Section 4.

$T_{P_{i}}$	Tree of triangles, constructed from the dual graph of the triangulation of $P_{i}$ , Section 4.1
$V_{P_{i}}$	Set of vertices of $T_{P_{i}}$ identified with triangles in the triangulation of $P_{i}$ , Section 4.1
$E_{P_{i}}$	Set of edges of $T_{P_{i}}$ encoding triangle adjacency in the triangulation of $P_{i}$ , Section 4.1
$F_{map, j_{i}}^{I} \subset R^{2}$	Mapped space before the purging of leaf triangle $j_{i} \in V_{P_{i}}$ , Section 4.2
$F_{map, p (j_{i})}^{I} \subset R^{2}$	Mapped space after the purging of leaf triangle $j_{i} \in V_{P_{i}}$ , Section 4.2
${\hat{F}}_{map}^{I} \subset R^{2}$	Mapped space after the purging of all leaf triangles, Sections 4.2 and 4.3
$x_{j_{i}}^{*} \in R^{2}$	Admissible center for the purging transformation of a leaf triangle $j_{i} \in V_{P_{i}}$ , Definition 2
$x_{i}^{*} \in R^{2}$	Admissible center for the transformation of a root triangle $r_{i} \in V_{P_{i}}$ , Definitions 4 and 6
$x_{1 j_{i}}, x_{2 j_{i}}, x_{3 j_{i}}$	Vertices of a leaf triangle $j_{i} \in V_{P_{i}}$ , Section 4.2
$x_{1 r_{i}}, x_{2 r_{i}}, x_{3 r_{i}}$	Vertices of a root triangle $r_{i} \in V_{P_{i}}$ , Section 4.3
$Q_{j_{i}} \subset R^{2}$	Quadrilateral $x_{3 j_{i}} x_{1 j_{i}} x_{j_{i}}^{*} x_{2 j_{i}} x_{3 j_{i}}$ associated with a leaf triangle $j_{i} \in V_{P_{i}}$ , Section 4.2
$Q_{r_{i}} \subset R^{2}$	Quadrilateral $x_{3 r_{i}} x_{1 r_{i}} x_{i}^{*} x_{2 r_{i}} x_{3 r_{i}}$ associated with a root triangle $r_{i} \in V_{P_{i}}$ , Section 4.3
${\bar{Q}}_{j_{i}} \subset R^{2}$	Admissible polygonal collar associated with a leaf triangle $j_{i} \in V_{P_{i}}$ , Definition 3
${\bar{Q}}_{r_{i}} \subset R^{2}$	Admissible polygonal collar associated with a root triangle $r_{i} \in V_{P_{i}}$ , Definitions 5 and 7
$γ_{j_{i}}, γ_{r_{i}} : R^{2} \to R$	Implicit function associated with $Q_{j_{i}}$ (7) or $Q_{r_{i}}$ (17)
$δ_{j_{i}}, δ_{r_{i}} : R^{2} \to R$	Implicit function associated with ${\bar{Q}}_{j_{i}}$ (8) or ${\bar{Q}}_{r_{i}}$ (18)
$σ_{γ_{j_{i}}}, σ_{γ_{r_{i}}} : R^{2} \to [0, 1]$	Auxiliary $C^{\infty}$ switch associated with $Q_{j_{i}}$ (11) or $Q_{r_{i}}$ (19)
$σ_{δ_{j_{i}}}, σ_{δ_{r_{i}}} : R^{2} \to [0, 1]$	Auxiliary $C^{\infty}$ switch associated with ${\bar{Q}}_{j_{i}}$ (12) or ${\bar{Q}}_{r_{i}}$ (20)
$σ_{j_{i}}, σ_{r_{i}} : R^{2} \to [0, 1]$	$C^{\infty}$ switch of the transformation of a leaf triangle $j_{i}$ (13) or a root triangle $r_{i}$ (21)
$ν_{j_{i}}, ν_{r_{i}} : R^{2} \to [0, 1]$	Deforming factor for a leaf triangle $j_{i}$ (14) or a root triangle $r_{i}$ (22) and (23)
$h_{j_{i}}^{I} : F_{map, j_{i}}^{I} \to F_{map, p (j_{i})}^{I}$	Purging transformation mapping a leaf triangle $j_{i}$ to its parent $p (j_{i})$ (16)
$g^{I} : F_{map}^{I} \to {\hat{F}}_{map}^{I}$	Composition of purging transformations mapping $F_{map}^{I}$ to ${\hat{F}}_{map}^{I}$ , Section 4.2.6
${\hat{h}}^{I} : {\hat{F}}_{map}^{I} \to F_{model}^{I}$	Diffeomorphism between ${\hat{F}}_{map}^{I}$ and $F_{model}^{I}$ (26)
$h^{I} : F_{map}^{I} \to F_{model}^{I}$	Diffeomorphism between $F_{map}^{I}$ and $F_{model}^{I}$ (27)

4.1. Obstacle representation

In order to construct the map $h^{I}$ between the mapped space and the model space, we assume that the robot has access to the triangulation of each one of the obstacles stored in both $D_{map}^{I}$ and $B_{map}^{I}$ . This triangulation can be efficiently constructed online upon recognition of each obstacle, using the Two Ears Theorem (Meisters, 1975), and the associated Ear Clipping Method (ElGindy et al., 1993).⁶

Briefly, an ear of a simple polygon is a vertex of the polygon such that the line segment between the two neighbors of the vertex lies entirely in the interior of the polygon. The Two Ears Theorem guarantees that every simple polygon has at least two such ears, and the Ear Clipping Method uses this result to efficiently construct polygon triangulations in $O (n^{2})$ time. Namely, an ear and its two neighbors form a triangle that is not crossed by any other part of the polygon and can, therefore, be safely removed. Removing a triangle of this type produces a polygon with one less vertex than the original polygon; we can repeat the process to eventually get a single triangle and complete the triangulation. An example is shown in Figure 3.

Except for its utility in constructing triangulations, the Two Ears Theorem guarantees that the dual graph of the triangulation of a simple polygon with no holes constructed with the Ear Clipping Method (i.e., a graph with one vertex per triangle and one edge per pair of adjacent triangles) is, in fact, a tree (O’Rourke, 1987).

Therefore, in order to construct a tree of triangles $T_{P_{i}} : = (V_{P_{i}}, E_{P_{i}})$ corresponding to a polygon $P_{i}$ , with $V_{P_{i}}$ a set of vertices identified with triangles (i.e., vertices of the dual of the formal triangulation) and $E_{P_{i}}$ a set of edges encoding triangle adjacency, we can triangulate $P_{i}$ using the Ear Clipping Method, pick any triangle as root, and construct $T_{P_{i}}$ based on the adjacency properties induced by the dual graph of the triangulation, as shown in Figure 3. If $P_{i} \in D_{map}^{I}$ , we pick as root the triangle with the largest surface area, whereas if $P_{i} \in B_{map}^{I}$ , we pick as root a triangle adjacent to $\partial F_{e}$ . This will give us a tree of triangles for $P_{i}$ in a notion similar to Rimon and Koditschek (1992). Our goal is then to successively “purge” this tree, triangle by triangle, in order of descending depth, until we reach the root triangle. Then, we can use a diffeomorphism similar to Vasilopoulos and Koditschek (2018) to map the exterior and boundary of the root triangle onto the exterior and boundary of a topologically equivalent disk if $P_{i} \in D_{map}^{I}$ , or merge the root triangle into $\partial F_{e}$ if $P_{i} \in B_{map}^{I}$ . These operations are all performed online; we provide some computational performance metrics with our experiments in Section 9.

We describe the algorithm for each purging transformation of the leaf nodes in Section 4.2 and the (final) root triangle purging transformation in Section 4.3. Finally, Section 4.4 defines the diffeomorphism between the mapped and model spaces, along with associated qualitative properties.

4.2. Intermediate spaces related by leaf purging transformations

In this section, we describe the purging transformation that maps the boundary of a leaf triangle $j_{i} \in V_{P_{i}}$ onto the boundary of its parent $p (j_{i}) \in V_{P_{i}}$ , as shown in Figure 4(1a) and (2a). This gives rise to a composition of transformations between a succession of intermediate spaces, each including the triangle $j_{i}$ , and $F_{map, p (j_{i})}^{I}$ , where $j_{i}$ has been mapped onto the boundary of its parent. Each of these transformations is in principle similar and performing a role analogous to the corresponding purging transformation in Rimon and Koditschek (1992), with two important differences. First, it deforms space only “locally” around the triangle, without taking into consideration other triangles or polygons, affording better numerical stability because only one triangle is considered at a time. Second, the method presented in Rimon and Koditschek (1992) is limited to parent–child pairs that strongly overlap instead of just being adjacent, which makes the method impractical for the arbitrary polygonal shapes and meshes involved in this work.

4.2.1. Center of the transformation and surrounding polygonal collars

Let the vertices of the triangle $j_{i} \in V_{P_{i}}$ be $x_{1 j_{i}}$ , $x_{2 j_{i}}$ , and $x_{3 j_{i}}$ in counterclockwise order, with $x_{1 j_{i}} x_{2 j_{i}}$ the common edge between $j_{i}$ and $p (j_{i})$ .

Definition 2. An admissible center for the purging transformation of the leaf triangle $j_{i} \in V_{P_{i}}$ , denoted by $x_{j_{i}}^{*}$ , is a point along the triangle median from $x_{3 j_{i}}$ and in the interior of its parent $p (j_{i}) \in V_{P_{i}}$ .

Such a point is always possible to find, because the two triangles share a common edge. By picking an admissible center, we ensure that the quadrilateral $Q_{j_{i}}$ with boundary $x_{3 j_{i}} x_{1 j_{i}} x_{j_{i}}^{*} x_{2 j_{i}} x_{3 j_{i}}$ is convex, as shown in Figure 4(1a) and (2a).

Definition 3. An admissible polygonal collar for the purging transformation of the leaf triangle $j_{i}$ is a convex polygon ${\bar{Q}}_{j_{i}}$ such that:

the edges $x_{1 j_{i}} x_{j_{i}}^{*}$ and $x_{j_{i}}^{*} x_{2 j_{i}}$ are edges of ${\bar{Q}}_{j_{i}}$ ;

${\bar{Q}}_{j_{i}}$ does not intersect the interior of any triangle $k \in V_{P}$ with $k \neq j_{i}, k \neq p (j_{i}), \forall P$ ;

${\bar{Q}}_{j_{i}}$ does not intersect any obstacle $C \in C_{map}$ ;

${\bar{Q}}_{j_{i}}$ does not intersect the boundary of $F_{map, j_{i}}^{I}$ ;

$Q_{j_{i}} \subset {\bar{Q}}_{j_{i}}$ .

Examples of such polygons are shown in Figure 4(1a) and (2a). This polygon is responsible for limiting the effect of the purging transformation in its interior, while keeping its value equal to the identity everywhere else. Intuitively, the requirements in Definition 3 will limit the effect of the purging transformation in a region that encloses the triangle $j_{i}$ and is away from the boundary of any other obstacle. The importance of enforcing the edges $x_{1 j_{i}} x_{j_{i}}^{*}$ and $x_{j_{i}}^{*} x_{2 j_{i}}$ to be edges of ${\bar{Q}}_{j_{i}}$ will become evident in the construction of the provable properties of the diffeomorphism, summarized in Proposition 1. We provide more details about the construction of admissible collars in Appendix C.

For the following, we also construct implicit functions $γ_{j_{i}} (x)$ and $δ_{j_{i}} (x)$ corresponding to the leaf triangle $j_{i} \in V_{P_{i}}$ , as described in Appendix B, such that

Q_{j_{i}} = {x \in R^{2} | γ_{j_{i}} (x) \leq 0}

(7)

{\bar{Q}}_{j_{i}} = {x \in R^{2} | δ_{j_{i}} (x) \geq 0}

(8)

4.2.2. Description of the $C^{\infty}$ switches

In order to simplify the diffeomorphism construction, we depart from the construction of analytic switches (Rimon and Koditschek, 1989) and rely instead on the $C^{\infty}$ function $ζ_{μ} : R \to R$ (Hirsch, 1976) described by

ζ_{μ} (χ) = {\begin{matrix} e^{- μ / χ}, & χ > 0 \\ 0, & χ \leq 0 \end{matrix}

(9)

and parametrized by $μ > 0$ , that has derivative

ζ_{μ'} (χ) = {\begin{matrix} \frac{μ ζ_{μ} (χ)}{χ^{2}}, & χ > 0 \\ 0, & χ \leq 0 \end{matrix}

(10)

Based on that function, we can then define the auxiliary $C^{\infty}$ switches

σ_{γ_{j_{i}}} (x) : = η_{μ_{γ_{j_{i}}}, ϵ_{j_{i}}} ° γ_{j_{i}} (x)

(11)

σ_{δ_{j_{i}}} (x) : = ζ_{μ_{δ_{j_{i}}}} ° \frac{δ_{j_{i}} (x)}{∥ x - x_{j_{i}}^{*} ∥}

(12)

with $η_{μ, ϵ} (χ) : = ζ_{μ} (ϵ - χ) / ζ_{μ} (ϵ)$ , and $μ_{γ_{j_{i}}}, μ_{δ_{j_{i}}}, ϵ_{j_{i}} > 0$ tunable parameters. Note that $σ_{γ_{j_{i}}}$ is exactly equal to 1 on the boundary of $Q_{j_{i}}$ and equal to 0 when $γ_{j_{i}} (x) \geq ϵ_{j_{i}}$ , whereas $σ_{δ_{j_{i}}}$ is 0 outside ${\bar{Q}}_{j_{i}}$ . The parameters $μ_{γ_{j_{i}}}$ and $μ_{δ_{j_{i}}}$ are used to tune the “slope” of $σ_{γ_{j_{i}}}$ on the boundary of $Q_{j_{i}}$ and how fast $σ_{δ_{j_{i}}}$ approaches 1 in the interior of ${\bar{Q}}_{j_{i}}$ , respectively.

Based on the above, we define the $C^{\infty}$ switch of the purging transformation for the leaf triangle $j_{i} \in V_{P_{i}}$ as a function $σ_{j_{i}} : F_{map, j_{i}}^{I} \to R$ , defined by

σ_{j_{i}} (x) : = {\begin{matrix} \frac{σ_{γ_{j_{i}}} (x) σ_{δ_{j_{i}}} (x)}{σ_{γ_{j_{i}}} (x) σ_{δ_{j_{i}}} (x) + (1 - σ_{γ_{j_{i}}} (x))}, & x \neq x_{1 j_{i}}, x_{2 j_{i}} \\ 1, & x = x_{1 j_{i}}, x_{2 j_{i}} \end{matrix}

(13)

In this way, we see that $σ_{j_{i}} (x) = 0$ when $σ_{γ_{j_{i}}} (x) = 0$ or $σ_{δ_{j_{i}}} (x) = 0$ (i.e., when $γ_{j_{i}} (x) \geq ϵ_{j_{i}}$ or outside ${\bar{Q}}_{j_{i}}$ ), $σ_{j_{i}} (x) = 1$ when $σ_{γ_{j_{i}}} (x) = 1$ (i.e., on the boundary of $Q_{j_{i}}$ ) and $σ_{j_{i}}$ varies between 0 and 1 everywhere, because $σ_{γ_{j_{i}}}$ and $σ_{δ_{j_{i}}}$ also vary between 0 and 1. Based on Definitions 2 and 3, it is straightforward to show the following lemma.

Lemma 1. The function $σ_{j_{i}} : F_{map, j_{i}}^{I} \to R$ is smooth away from the triangle vertices $x_{1 j_{i}}, x_{2 j_{i}}, x_{3 j_{i}}$ , none of which lies in the interior of $F_{map, j_{i}}^{I}$ .□

Proof. See Appendix E.1.

4.2.3. Description of the deforming factors

The deforming factors are the functions $ν_{j_{i}} : F_{map, j_{i}}^{I} \to R$ , responsible for mapping the boundary of the leaf triangle $j_{i} \in V_{P_{i}}$ onto the boundary of its parent $p (j_{i})$ . Based on Definitions 2 and 3 and as shown in Figure 4(1a) and (2a), this implies that the functions $ν_{j_{i}}$ are responsible for mapping the polygonal chain $x_{2 j_{i}} x_{3 j_{i}} x_{1 j_{i}}$ onto the shared edge $x_{2 j_{i}} x_{1 j_{i}}$ between $j_{i}$ and $p (j_{i})$ . For this reason, we construct $ν_{j_{i}}$ as follows:

ν_{j_{i}} (x) : = \frac{{(x_{1 j_{i}} - x_{j_{i}}^{*})}^{⊤} n_{j_{i}}}{{(x - x_{j_{i}}^{*})}^{⊤} n_{j_{i}}}

(14)

with

n_{j_{i}} : = R_{\frac{π}{2}} \frac{x_{2 j_{i}} - x_{1 j_{i}}}{∥ x_{2 j_{i}} - x_{1 j_{i}} ∥}, R_{\frac{π}{2}} : = [\begin{matrix} 0 & - 1 \\ 1 & 0 \end{matrix}]

(15)

the normal vector corresponding to the shared edge between $j_{i}$ and $p (j_{i})$ .

4.2.4. The map between $F_{map, j_{i}}^{I}$ and $F_{map, p (j_{i})}^{I}$

Based on the above, we then construct the map between $F_{map, j_{i}}^{I}$ and $F_{map, p (j_{i})}^{I}$ with the $j_{i}$ th leaf triangle of $P_{i}$ purged, as

h_{j_{i}}^{I} (x) : = σ_{j_{i}} (x) (x_{j_{i}}^{*} + ν_{j_{i}} (x) (x - x_{j_{i}}^{*})) + (1 - σ_{j_{i}} (x)) x

(16)

4.2.5. Qualitative properties of the map between $F_{map, j_{i}}^{I}$ and $F_{map, p (j_{i})}^{I}$

We first verify that the construction is a smooth change of coordinates between the intermediate mapped spaces.

Lemma 2. The map $h_{j_{i}}^{I} : F_{map, j_{i}}^{I} \to F_{map, p (j_{i})}^{I}$ is smooth away from the triangle vertices $x_{1 j_{i}}, x_{2 j_{i}}, x_{3 j_{i}}$ , none of which lies in the interior of $F_{map, j_{i}}^{I}$ .

Proof. See Appendix E.1.□

Proposition 1. The map $h_{j_{i}}^{I}$ is a $C^{\infty}$ diffeomorphism between $F_{map, j_{i}}^{I}$ and $F_{map, p (j_{i})}^{I}$ away from the triangle vertices $x_{1 j_{i}}, x_{2 j_{i}}, x_{3 j_{i}}$ , none of which lies in the interior of $F_{map, j_{i}}^{I}$ .

Proof. See Appendix E.1.□

4.2.6. Composition of leaf purging transformations

The application of the purging transformation described above will result in a tree for $P_{i}$ with one less vertex and one less edge. Therefore, similarly to Rimon and Koditschek (1992), we can keep applying such purging transformations by composition, during execution time, for all leaf triangles of all obstacles $P$ in $B_{map}^{I}$ and $D_{map}^{I}$ (in any order), until we reach their root triangles. We denote by ${\hat{F}}_{map}^{I}$ this final intermediate space, where all obstacles in $F_{map}^{I}$ have been deformed to their root triangles ${r_{i}}$ , and by $g^{I} : F_{map}^{I} \to {\hat{F}}_{map}^{I}$ the map between $F_{map}^{I}$ and ${\hat{F}}_{map}^{I}$ , arising from this composition of purging transformations. Since $g^{I}$ is a composition of diffeomorphisms, we immediately obtain the following result.

Corollary 1. The map $g^{I} : F_{map}^{I} \to {\hat{F}}_{map}^{I}$ is a $C^{\infty}$ diffeomorphism between $F_{map}^{I}$ and ${\hat{F}}_{map}^{I}$ away from sharp corners, none of which lie in the interior of freespace for any of the intermediate or final spaces.

4.3. Purging of root triangles

After the successive application of the leaf purging transformations presented in Section 4.2, familiar obstacles in $F_{map}^{I}$ are reduced to triangles in ${\hat{F}}_{map}^{I}$ . These triangles either are homeomorphic to a disk in the interior of ${\hat{F}}_{map}^{I}$ if they correspond to obstacles in $D_{map}^{I}$ , or have a common edge with $\partial F_{e}$ if they correspond to obstacles in $B_{map}^{I}$ . Therefore, the final step to generate the model space $F_{model}^{I}$ is to transform each of the root triangles corresponding to obstacles in $D_{map}^{I}$ to disks, following a procedure similar to Vasilopoulos and Koditschek (2018), and merge the root triangles corresponding to obstacles in $B_{map}^{I}$ to $\partial F_{e}$ .

4.3.1. Center of the transformation and surrounding polygonal collars for obstacles in $D_{map}^{I}$

Here we assume that $P_{i} \in D_{map}^{I}$ . Let the vertices of the root triangle $r_{i} \in V_{P_{i}}$ be $x_{1 r_{i}}$ , $x_{2 r_{i}}$ and $x_{3 r_{i}}$ in counterclockwise order, as shown in Figure 4(1b).

Definition 4. An admissible center for the transformation of the root triangle $r_{i}$ , corresponding to a polygon $P_{i} \in D_{map}^{I}$ , is a point $x_{i}^{*}$ in the interior of $r_{i}$ .

Without loss of generality, we pick $x_{i}^{*}$ to be the barycenter of $r_{i}$ , and the radius of the transformation to be a number $ρ_{i} < d (x_{i}^{*}, \partial r_{i})$ , following the admissibility assumptions made in Rimon and Koditschek (1992). We also set $Q_{r_{i}}$ to be the closure of $r_{i}$ itself, and define a polygonal collar ${\bar{Q}}_{r_{i}}$ for the root triangle transformation as follows.

Definition 5. An admissible polygonal collar for the transformation of the root triangle $r_{i}$ , corresponding to a polygon $P_{i} \in D_{map}^{I}$ , is a convex polygon ${\bar{Q}}_{r_{i}}$ such that:

${\bar{Q}}_{r_{i}} \cap {\bar{Q}}_{r_{j}} = \emptyset$ , with $i \neq j$ ;

${\bar{Q}}_{r_{i}}$ does not intersect any obstacle $C \in C_{map}$ ;

${\bar{Q}}_{r_{i}}$ does not intersect $\partial {\hat{F}}_{map}^{I}$ ;

$Q_{r_{i}} \subset {\bar{Q}}_{r_{i}}$ .

An example of such a polygon is shown in Figure 4(1b). Again, this polygon is responsible for limiting the effect of the transformation in its interior, while keeping it equal to the identity map everywhere else. Similarly to Section 4.2, we also construct implicit functions $γ_{r_{i}} (x)$ and $δ_{r_{i}} (x)$ for each root triangle $r_{i}$ , such that

Q_{r_{i}} = {x \in R^{2} | γ_{r_{i}} (x) \leq 0}

(17)

{\bar{Q}}_{r_{i}} = {x \in R^{2} | δ_{r_{i}} (x) \geq 0}

(18)

4.3.2. Description of the $C^{\infty}$ switches for obstacles in $D_{map}^{I}$

Following the notation of Section 4.2, we can define the auxiliary $C^{\infty}$ switches

σ_{γ_{r_{i}}} (x) : = η_{μ_{γ_{r_{i}}}, ϵ_{r_{i}}} ° γ_{r_{i}} (x)

(19)

σ_{δ_{r_{i}}} (x) : = ζ_{μ_{δ_{r_{i}}}} ° \frac{δ_{r_{i}} (x)}{∥ x - x_{i}^{*} ∥}

(20)

with $η_{μ, ϵ} (χ) : = ζ_{μ} (ϵ - χ) / ζ_{μ} (ϵ)$ , $ζ$ defined as in (9) and $μ_{γ_{r_{i}}}, μ_{δ_{r_{i}}}, ϵ_{r_{i}} > 0$ tunable parameters.

Based on the above, we then define the $C^{\infty}$ switch of the transformation of the root triangle $r_{i}$ as the function $σ_{r_{i}} : {\hat{F}}_{map}^{I} \to F_{map}^{I}$ given by

σ_{r_{i}} (x) : = \frac{σ_{γ_{r_{i}}} (x) σ_{δ_{r_{i}}} (x)}{σ_{γ_{r_{i}}} (x) σ_{δ_{r_{i}}} (x) + (1 - σ_{γ_{r_{i}}} (x))}

(21)

It can be seen that the function $σ_{r_{i}}$ will be 0 outside ${\bar{Q}}_{r_{i}}$ , exactly equal to 1 on the boundary of $Q_{r_{i}}$ (i.e., on the boundary of the root triangle $r_{i}$ ) and varies smoothly between 0 and 1 everywhere else.

We can easily show the following lemma, as the function $ζ$ eliminates all the singular points of $δ_{r_{i}}$ that correspond to the vertices of ${\bar{Q}}_{r_{i}}$ .

Lemma 3. The switch $σ_{r_{i}} : {\hat{F}}_{map}^{I} \to R$ is smooth away from the triangle vertices $x_{1 r_{i}}, x_{2 r_{i}}, x_{3 r_{i}}$ , none of which lies in the interior of ${\hat{F}}_{map}^{I}$ .

4.3.3. Description of the deforming factors for obstacles in $D_{map}^{I}$

Here, the deforming factors are the functions $ν_{r_{i}} : {\hat{F}}_{map}^{I} \to R$ , responsible for transforming each root triangle corresponding to an obstacle in $D_{map}^{I}$ to a disk in $R^{2}$ . The deforming factors we use are inspired by those in Rimon and Koditschek (1992), but do not depend on the values of the implicit functions $γ_{r_{i}}$ . Namely, the deforming factors are given based on the desired final radii $ρ_{i}$ as

ν_{r_{i}} (x) : = \frac{ρ_{i}}{∥ x - x_{i}^{*} ∥}

(22)

4.3.4. Center of the transformation and surrounding polygonal collars for obstacles in $B_{map}^{I}$

Next we focus on obstacles in $B_{map}^{I}$ . The procedure here is slightly different, because we want to merge the root triangle $r_{i}$ to the boundary of the enclosing freespace $F_{e}$ . Namely, we assume that the vertices of the triangle $r_{i}$ are $x_{1 r_{i}}, x_{2 r_{i}}, x_{3 r_{i}}$ in counterclockwise order, with $x_{1 r_{i}} x_{2 r_{i}}$ the common edge between $r_{i}$ and $\partial F_{e}$ , as shown in Figure 4(2b).⁷ Then, we pick an admissible center similarly to Definition 2, as follows.

Definition 6. An admissible center for the transformation of the root triangle $r_{i}$ , corresponding to a polygon $P_{i} \in B_{map}^{I}$ , denoted by $x_{i}^{*}$ , is a point along the triangle median from $x_{3 r_{i}}$ , such that $x_{i}^{*} \in R^{2} \ F_{e}$ .

We also define $Q_{r_{i}}$ to be the convex quadrilateral with boundary $x_{3 r_{i}} x_{1 r_{i}} x_{i}^{*} x_{2 r_{i}} x_{3 r_{i}}$ . The collars used are defined similarly to Definition 3, as the transformation itself is designed to be quite similar with the purging transformation.

Definition 7. An admissible polygonal collar for the transformation of the root triangle $r_{i}$ , corresponding to a polygon $P_{i} \in B_{map}^{I}$ , is a convex polygon ${\bar{Q}}_{r_{i}}$ such that:

the edges $x_{1 r_{i}} x_{i}^{*}$ and $x_{i}^{*} x_{2 r_{i}}$ are edges of ${\bar{Q}}_{r_{i}}$ ;

${\bar{Q}}_{r_{i}} \cap {\bar{Q}}_{r_{j}} = \emptyset$ , with $i \neq j$ ;

${\bar{Q}}_{r_{i}}$ does not intersect any obstacle $C \in C_{map}$ ;

$Q_{r_{i}} \subset {\bar{Q}}_{r_{i}}$ .

4.3.5. Description of the $C^{\infty}$ switches for obstacles in $B_{map}^{I}$

With the definition of $Q_{r_{i}}$ and ${\bar{Q}}_{r_{i}}$ as described above, we associate implicit functions $γ_{r_{i}} (x)$ and $δ_{r_{i}} (x)$ as in (17), (18), auxiliary switches $σ_{γ_{r_{i}}}$ and $σ_{δ_{r_{i}}}$ as in (19) and (20), and overall $C^{\infty}$ switch of the transformation of the root triangle $r_{i}$ as the function $σ_{r_{i}} : {\hat{F}}_{map}^{I} \to F_{map}^{I}$ given in (21).

4.3.6. Description of the deforming factors for obstacles in $B_{map}^{I}$

Finally, in order to merge the root triangle into the boundary $\partial F_{e}$ , we define the deforming factors similarly to (14), as the functions $ν_{r_{i}} : {\hat{F}}_{map}^{I} \to R$ , given by

ν_{r_{i}} (x) : = \frac{{(x_{1 r_{i}} - x_{i}^{*})}^{⊤} n_{r_{i}}}{{(x - x_{i}^{*})}^{⊤} n_{r_{i}}}

(23)

with

n_{r_{i}} : = R_{\frac{π}{2}} \frac{x_{2 r_{i}} - x_{1 r_{i}}}{∥ x_{2 r_{i}} - x_{1 r_{i}} ∥}, R_{\frac{π}{2}} : = [\begin{matrix} 0 & - 1 \\ 1 & 0 \end{matrix}]

(24)

the normal vector corresponding to the shared edge between $r_{i}$ and $\partial F_{e}$ .

4.3.7. The map between ${\hat{F}}_{map}^{I}$ and $F_{model}^{I}$

First of all, we define

σ_{d} (x) : = 1 - \sum_{i \in J_{ℬ}^{ℐ} \cup J_{D}^{ℐ}} σ_{r_{i}} (x)

(25)

Using the above constructions and Definitions 4, 5, 6, and 7 we are led to the following results.

Lemma 4. At any point $x \in {\hat{F}}_{map}^{I}$ , at most one of the switches ${σ_{r_{i}}}_{i \in J_{B}^{I} \cup J_{D}^{I}}$ can be non-zero.

Corollary 2. The set ${σ_{r_{i}}}_{i \in J_{B}^{I} \cup J_{D}^{I}} \cup {σ_{d}}$ defines a partition of unity over ${\hat{F}}_{map}^{I}$ .

With the construction of $σ_{r_{i}}$ (as in (21)) and $ν_{r_{i}}$ (as in either (22) or (23), depending on whether $i$ belongs to $J_{D}^{I}$ or $J_{B}^{I}$ , respectively) for each root triangle, we can now construct the map ${\hat{h}}^{I} : {\hat{F}}_{map}^{I} \to F_{model}^{I}$ given by

{\hat{h}}^{ℐ} (x) : = \sum_{i \in J_{ℬ}^{ℐ} \cup J_{D}^{ℐ}} σ_{r_{i}} (x) [x_{i}^{*} + ν_{r_{i}} (x) (x - x_{i}^{*})] + σ_{d} (x) x

(26)

4.3.8. Qualitative properties of the map between ${\hat{F}}_{map}^{I}$ and $F_{model}^{I}$

We can again verify that the construction is a smooth change of coordinates between ${\hat{F}}_{map}^{I}$ and $F_{model}^{I}$ . Using Lemma 3 and the fact that the deforming factors $ν_{r_{i}}$ are smooth in ${\hat{F}}_{map}^{I}$ (because the centers $x_{i}^{*}$ do not belong in ${\hat{F}}_{map}^{I}$ ) for all $i$ , we get the following result.

Lemma 5. The map ${\hat{h}}^{I} : {\hat{F}}_{map}^{I} \to F_{model}^{I}$ is smooth away from any sharp corners, none of which lie in the interior of ${\hat{F}}_{map}^{I}$ .

Proposition 2. The map ${\hat{h}}^{I}$ is a $C^{\infty}$ diffeomorphism between ${\hat{F}}_{map}^{I}$ and $F_{model}^{I}$ away from any sharp corners, none of which lie in the interior of ${\hat{F}}_{map}^{I}$ .

Proof. See Appendix E.1.□

4.4. The map between the mapped space and the model space

Based on the construction of $g^{I} : F_{map}^{I} \to {\hat{F}}_{map}^{I}$ in Section 4.2.6 and ${\hat{h}}^{I} : {\hat{F}}_{map}^{I} \to F_{model}^{I}$ in Section 4.3.7, we can finally write the map between the mapped space and the model space as the function $h^{I} : F_{map}^{I} \to F_{model}^{I}$ given by

h^{ℐ} (x) = {\hat{h}}^{ℐ} ° g^{ℐ} (x)

(27)

It is straightforward to obtain the following result, because both $g^{I}$ and ${\hat{h}}^{I}$ are $C^{\infty}$ diffeomorphisms away from sharp corners.

Corollary 3. The map $h^{I}$ is a $C^{\infty}$ diffeomorphism between $F_{map}^{I}$ and $F_{model}^{I}$ away from any sharp corners, none of which lie in the interior of $F_{map}^{I}$ .

An illustration of the behavior of the map $h^{I}$ through a visualization of the values of $\det (D_{x} h^{I})$ for a specific example with a single polygon is included in Figure 5.

5. Reactive controller

The preceding analysis in Section 4 describes the diffeomorphism construction between $F_{map}^{I}$ and $F_{model}^{I}$ for a given index set $I$ of instantiated familiar obstacles. However, the onboard sensor might discover new obstacles and, subsequently, incorporate them in the semantic map, updating the set $I$ . Therefore, we have to provide a hybrid systems description of our reactive controller, where each mode is defined by an index set $I \in 2^{N_{P}}$ of familiar obstacles stored in the semantic map, the guards describe the sensor trigger events where a previously “unexplored” obstacle is discovered and incorporated in the semantic map (thereby changing $P_{map}^{I}$ , along with $D_{map}^{I}$ , $B_{map}^{I}$ ), and the resets describe transitions to new modes that might result in discrete “jumps” of the robot position in the model space. We then need to show that the resulting hybrid controller, for both the fully actuated robot and the differential drive robot, must succeed in the navigation task.

In the following, Section 5.1 provides the hybrid systems description, Section 5.2 describes the reactive controller applied in each mode of the hybrid system, Section 5.3 summarizes the qualitative properties of our hybrid controller, and Section 5.4 describes our method of generating bounded inputs, each time for both the fully actuated and the differential drive robot. Table 4 summarizes associated notation used throughout this Section.

Table 4.

Key symbols related to the hybrid systems formulation (top, Section 5.1) and the reactive controller construction in each mode of the hybrid system (bottom, Section 5.2) for both a fully actuated robot and a differential drive robot.

$I \in 2^{N_{P}}$	Mode of the hybrid system, Sections 3.1, 5.1
$F^{I}, F_{s em}^{I}, F_{map}^{I}, F_{model}^{I} \in R^{2}$	Physical, semantic, mapped, model freespace for $I$ , Sections 3, 5.1
$h^{I} : F_{map}^{I} \to F_{model}^{I}$	Map between $F_{map}^{I}$ and $F_{model}^{I}$ for mode $I$ , Section 4
$Γ \subset 2^{N_{P}} \times 2^{N_{P}}$	Directed graph of discrete transitions over the modes, Section 5.1
$D : = ∐_{I \in 2^{N_{P}}} F^{I}$	Collection of domains for a fully actuated robot, Section 5.1
$G : = ∐_{(I, I^{'}) \in Γ} G^{I, I^{'}}$	Collection of guards for a fully actuated robot (29)
$R : G \to D$	Continuous reset map for a fully actuated robot (30)
$U : D \to T D$	Hybrid vector field for a fully actuated robot (31)
$H : = (2^{N_{P}}, Γ, D, U, G, R)$	Hybrid system for a fully actuated robot, Theorem 3
$\bar{D} : = ∐_{I \in 2^{N_{P}}} (F^{I} \times S^{1})$	Collection of domains for a differential drive robot, Section 5.1
$\bar{G} : = ∐_{(I, I^{'}) \in Γ} {\bar{G}}^{I, I^{'}}$	Collection of guards for a differential drive robot (33)
$\bar{R} : \bar{G} \to \bar{D}$	Continuous reset map for a differential drive robot (34)
$\bar{U} : \bar{D} \to T \bar{D}$	Hybrid vector field for a differential drive robot (35)
$\bar{H} : = (2^{N_{P}}, Γ, \bar{D}, \bar{U}, \bar{G}, \bar{R})$	Hybrid system for a differential drive robot, Theorem 4
$x \in F_{map}^{I}$	Fully actuated robot position in $F_{map}^{I}$ , Section 5.2
$y : = h^{I} (x) \in F_{model}^{I}$	Fully actuated robot position in $F_{model}^{I}$ , Section 5.2
$y_{d} : = h^{I} (x_{d})$	Goal position in $F_{model}^{I}$ , Section 5.2
$L F (y) \subset F_{model}^{I}$	Local freespace at $y \in F_{model}^{I}$ (37)
$v^{I} : F_{model}^{I} \to T F_{model}^{I}$	Vector field controller for a fully actuated robot in $F_{model}^{I}$ (36)
$u^{I} : F_{map}^{I} \to T F_{map}^{I}$	Vector field controller for a fully actuated robot in $F_{map}^{I}$ (38)
${\bar{h}}^{I} : F_{map}^{I} \times S^{1} \to F_{model}^{I} \times S^{1}$	Diffeomorphism between $F_{map}^{I} \times S^{1}$ and $F_{m odel}^{I} \times S^{1}$ (41)
$\bar{x} : = (x, ψ) \in F_{m ap}^{I} \times S^{1}$	Differential drive robot state in $F_{m ap}^{I} \times S^{1}$ , Section 5.2
$\bar{y} : = (y, φ) = {\bar{h}}^{I} (\bar{x}) \in F_{m odel}^{I} \times S^{1}$	Differential drive robot state in $F_{m odel}^{I} \times S^{1}$ , Section 5.2
$ξ^{I} : S^{1} \to S^{1}$	Angle transformation between $F_{m ap}^{I} \times S^{1}$ and $F_{m odel}^{I} \times S^{1}$ (42)
$y_{d, ∥} (\bar{y}), y_{d, G} (\bar{y}) \in F_{m odel}^{I}$	Linear (57) and angular (58) local goals for $\bar{y} \in F_{m odel}^{I} \times S^{1}$
${\bar{v}}^{I} : = ({\hat{v}}^{I}, {\hat{ω}}^{I}) \in R^{2}$	Linear and angular inputs for a unicycle robot in $F_{m odel}^{I} \times S^{1}$ (53)
${\bar{u}}^{I} : = (v^{I}, ω^{I}) \in R^{2}$	Linear and angular inputs for a unicycle robot in $F_{m ap}^{I} \times S^{1}$ (52)

5.1. Hybrid systems description of navigation framework

5.1.1. Fully actuated robots

First, we consider a fully actuated particle with state $x \in F$ , and dynamics

\overset{\cdot}{x} = u

(28)

As different subsets of instantiated obstacles in $\tilde{P}$ , indexed by $I$ , result in different consolidated polygonal obstacles stored in $P_{map}^{I}$ , it is natural to index the modes of the hybrid controller according to elements $I$ of the power set $2^{N_{P}}$ . Every execution, from any initial state, is required to start in the initial mode, indexed by $I = \emptyset$ . We also define a terminal mode as follows.

Definition 8. The terminal mode of the hybrid system is indexed by the improper subset, $I = N_{P}$ , where all familiar obstacles in the workspace have been instantiated in the set $P_{s em}^{I}$ , in the sense of Definition 1.

We denote the freespace in the semantic, mapped, and model spaces, associated with a unique subset $I$ of $N_{P}$ , by $F_{s em}^{I}, F_{map}^{I}, F_{model}^{I}$ , respectively, as in Section 3. We also denote the corresponding perceived physical freespace by $F^{I}$ , with $F^{I} : = F_{map}^{I}$ , because the dilation of obstacles by $r$ in the passage from the physical to the semantic space and the obstacle merging in the passage from the semantic to the mapped space do not alter the freespace description. The domain $D$ of our hybrid system is then defined as the collection $D : = \underset{I \in 2^{N_{P}}}{∐} F^{I}$ .

Following the notation in Johnson et al. (2016), we can then denote by $Γ \subset 2^{N_{P}} \times 2^{N_{P}}$ the set of discrete transitions for the hybrid system, forming a directed graph structure over the set of modes $2^{N_{P}}$ . The collection of guards associated with $Γ$ can be described as $G : = \underset{(I, I') \in Γ}{∐} G^{I, I'}$ , with $G^{I, I'} \subset F^{I}$ given by

\begin{matrix} G^{I, I'} : = {x \in F^{I} | I' = I \cup I_{u} \\ I_{u} \neq \emptyset, I_{u} \cap I = \emptyset, \\ \bar{B (x, R)} \cap {\tilde{P}}_{i} \neq \emptyset \forall i \in I_{u}, \\ \bar{B (x, R)} \cap {\tilde{P}}_{N_{P} \ (I \cup I_{u})} = \emptyset} \end{matrix}

(29)

with ${\tilde{P}}_{N_{P} \ (I \cup I_{u})} : = {{\tilde{P}}_{i}}_{i \in N_{P} \ (I \cup I_{u})}$ .

In addition, the reset $R : G \to D$ is the continuous map that restricts simply as $R^{I, I'} : = R |_{G^{I, I'}} : G^{I, I'} \to F^{I'}$ , with

R^{I, I'} (x) = x

(30)

the identity map. Note, however, that although the robot cannot experience discrete jumps in the physical space, the model space $F_{model}^{I'}$ is likely to be a discontinuously different space from $F_{model}^{I}$ (i.e., there is no guaranteed inclusion from $F_{model}^{I'}$ into $F_{model}^{I}$ ), hence the model position in the new space bears no obvious relationship to that in the prior. Namely, the position of the robot in the model space after a transition from mode $I$ to mode $I'$ will be given by $h^{I'} ° (h^{I})^{- 1} (y)$ , with $y \in F_{model}^{I}$ .

Finally, we can construct the hybrid vector field $U : D \to T D$ that restricts to a vector field $U^{I} : = U |_{F^{I}} : F^{I} \to T F^{I}$ , that can be written as

U^{I} (x) : = u^{I} (x)

(31)

with $u^{I}$ given in (38) and described in the next section.

Based on the above definitions, we define the navigational hybrid system for fully actuated robots as the tuple $H : = (2^{N_{P}}, Γ, D, U, G, R)$ describing the modes, discrete transitions, domains, associated vector fields, guards and resets.

5.1.2. Differential drive robots

Next, we focus on a differential drive robot, whose state is $\bar{x} : = (x, ψ) \in F \times S^{1} \subset SE (2)$ , and its dynamics are given by⁸

\bar{x} = B (ψ) \bar{u}

(32)

with $B (ψ) : = {[\begin{matrix} \cos ψ & \sin ψ & 0 \\ 0 & 0 & 1 \end{matrix}]}^{⊤}$ and $\bar{u} : = (v, ω)$ , with $v, ω \in R$ the linear and angular input, respectively.

The analysis here is fairly similar; the modes and discrete transitions are identical. However, the robot operates on a subset of $SE (2)$ and, therefore, the domains must be described as $\bar{D} : = \underset{I \in 2^{N_{P}}}{∐} (F^{I} \times S^{1})$ . Consequently, the collection of guards that will result in transitions between different modes according to $Γ$ are described as $\bar{G} : = \underset{(I, I') \in Γ}{∐} {\bar{G}}^{I, I'}$ , with ${\bar{G}}^{I, I'} \subset (F^{I} \times S^{1})$ given by

\begin{matrix} {\bar{G}}^{I, I'} : = {\bar{x} = (x, ψ) \in F^{I} \times S^{1} | I' = I \cup I_{u} \\ I_{u} \neq \emptyset, I_{u} \cap I = \emptyset, \\ \bar{B (x, R)} \cap {\tilde{P}}_{i} \neq \emptyset for all i \in I_{u}, \\ \bar{B (x, R)} \cap {\tilde{P}}_{N_{P} \ (I \cup I_{u})} = \emptyset} \end{matrix}

(33)

In addition, the reset $\bar{R} : \bar{G} \to \bar{D}$ is the continuous map that restricts simply as ${\bar{R}}^{I, I'} : = \bar{R} |_{{\bar{G}}^{I, I'}} : {\bar{G}}^{I, I'} \to (F^{I'} \times S^{1})$ , with

{\bar{R}}^{I, I'} (\bar{x}) = \bar{x}

(34)

the identity map.

Finally, the fact that the robot operates in $SE (2)$ gives rise to a new hybrid vector field $\bar{U} : \bar{D} \to T \bar{D}$ , that restricts to a vector field ${\bar{U}}^{I} : = \bar{U} |_{F^{I} \times S^{1}} : F^{I} \times S^{1} \to T (F^{I} \times S^{1})$ , that can be written as

{\bar{U}}^{I} (\bar{x}) : = B (ψ) {\bar{u}}^{I}

(35)

with the inputs ${\bar{u}}^{I} = (v^{I}, ω^{I})$ given as in (52) and described in the next section.

Based on the above definitions, we define the navigational hybrid system for differential drive robots as the tuple $\bar{H} : = (2^{N_{P}}, Γ, \bar{D}, \bar{U}, \bar{G}, \bar{R})$ describing the modes, discrete transitions, domains, associated vector fields, guards, and resets.

5.2. Reactive controller in each hybrid mode

The preceding analysis of the hybrid system allows us to now describe the constituent controllers in each mode $I$ of the hybrid system, for both the fully actuated and the differential drive robot. For the results pertaining to each separate mode, we are going to assume that $I$ describes the terminal mode of the hybrid system, in the notion of Definition 8.

With this assumption, we can arrive to Theorems 1 and 2, that allow us to establish the main results about our hybrid controller in Theorems 3 and 4. We assume that the robot operates in $F_{map}^{I}$ ,⁹ and the set of consolidated obstacles $P_{map}^{I}$ in mode $I$ has been identified.

5.2.1. Fully actuated robots

The dynamics of the fully actuated particle in $F_{model}^{I}$ with state $y = h^{I} (x) \in F_{model}^{I}$ can be described by $\overset{\cdot}{y} = v^{I} (y)$ with the input $v^{I} (y)$ given in (Arslan and Koditschek, 2016b) as¹⁰

v^{I} (y) = - (y - Π_{L F (y)} (y_{d}))

(36)

with $y_{d} = h^{I} (x_{d})$ , and the convex local freespace for $y$ , $L F (y)$ , defined as the Voronoi cell in Arslan and Koditschek (2016b: Equations (7) and (24)):

\begin{matrix} L F (y) : = {q \in F_{model}^{I} | ∥ q - y ∥ \leq ∥ q - Π_{{\bar{O}}_{i}} (y) ∥, \forall i} \\ \cap \bar{B (y, \frac{R_{model}}{2})} \end{matrix}

(37)

Here, $i$ spans the obstacles in both $D_{map}^{I}$ (represented in $F_{model}^{I}$ by $\bar{B (x_{i}^{*}, ρ_{i})}$ ) and $C_{map}$ (transferred to $F_{model}^{I}$ with an identity map), and $R_{model}$ is the range of the virtual sensor used for obstacle detection in $F_{model}^{I}$ . Similarly to Vasilopoulos and Koditschek (2018), using the diffeomorphism construction in (27), we construct our controller as the vector field $u^{I} = : F_{map}^{I} \to T F_{map}^{I}$ given by

u^{I} (x) = k {[D_{x} h^{I}]}^{- 1} \cdot (v^{I} ° h^{I} (x))

(38)

with $k > 0$ . Note here that the strategy employed never requires the explicit computation of $(h^{I})^{- 1}$ , which would make our numerical realization quite difficult; instead, it merely requires inversion of $[D_{x} h^{I}]$ .

We note that if the range of the virtual sensor $R_{model}$ used to construct $L F (y)$ in the model space is smaller than the range of our sensor $R$ , the vector field $u^{I}$ is Lipschitz continuous because $v^{I} (y)$ is shown to be Lipschitz continuous in Arslan and Koditschek (2016b), $y = h^{I} (x)$ is a smooth change of coordinates away from sharp corners, and the robot discovers obstacles before actually using them for navigation, because $R_{model} < R$ . We are led to the following result.

Corollary 4. With $I$ the terminal mode of the hybrid controller, the vector field $u^{I} : F_{map}^{I} \to T F_{map}^{I}$ generates a unique continuously differentiable partial flow.

To ensure completeness (i.e., absence of finite time escape through boundaries in $F_{map}^{I}$ ) we must verify that the robot never collides with any obstacle in the environment, i.e., leaves its freespace positively invariant. However, this property follows almost directly from the fact that the vector field $u^{I}$ on $F_{map}^{I}$ is the pushforward of the complete vector field $v^{I}$ through $(h^{I})^{- 1}$ , guaranteed to insure that $F_{model}^{I}$ remain positively invariant under its flow as shown in Arslan and Koditschek (2016b), away from sharp corners on the boundary of $F_{map}^{I}$ . Therefore, we immediately obtain the following result.

Proposition 3. With $I = N_{P}$ the terminal mode of the hybrid controller, the freespace interior $F_{map}^{I}$ is positively invariant under the law (38).

Next, we focus on the stationary points of $u^{I}$ .

Lemma 6. With $I = N_{P}$ the terminal mode of the hybrid controller:

1. the set of stationary points of control law (38) is given as ${x_{d}} ⋃_{{(h^{I})^{- 1} (s_{i})}_{i \in J_{D}^{I}}} ⋃_{{G_{k}}_{k \in J_{C}}}$ , where

s_{i} = x_{i}^{*} - ρ_{i} \frac{h^{I} (x_{d}) - x_{i}^{*}}{∥ h^{I} (x_{d}) - x_{i}^{*} ∥}

(39a)

G_{k} = {q \in F_{map}^{I} | d (q, C_{k}) = r, κ (q) = 1}

(39b)

with

κ (q) : = \frac{{(q - Π_{{\bar{C}}_{k}} (q))}^{⊤} (q - h^{I} (x_{d}))}{∥ q - Π_{{\bar{C}}_{k}} (q) ∥ \cdot ∥ q - h^{I} (x_{d}) ∥}

2. the goal $x_{d}$ is the only locally stable equilibrium of control law (38) and all the other stationary points ${(h^{I})^{- 1} (s_{i})}_{i \in J_{D}^{I}} ⋃_{{G_{k}}_{k \in J_{C}}}$ , each associated with an obstacle, are non-degenerate saddles.

Proof. See Appendix E.2.□

Note that there is a slight complication here; each stationary point $s_{i}, i \in J_{D}^{I}$ lies on the boundary of the corresponding ball $\bar{B (x_{i}^{*}, ρ_{i})}$ in the model space and, thus, by construction of the diffeomorphism $h^{I}$ , it might not lie in the domain of $(h^{I})^{- 1}$ because it could correspond to a sharp corner (i.e., a polygon vertex) in the mapped space. Although such problems can only occur for a thin subset of obstacle placements, we explicitly impose the following assumption to facilitate our formal results.

Assumption 4. The stationary points of control law (36) in the model space lie in the domain of the map $(h^{I})^{- 1}$ between $F_{model}^{I}$ and $F_{map}^{I}$ .

As such pathological cases can only occur for a “thin” (empty interior) subset of obstacle placements and the considered stationary points are shown to be non-degenerate saddles, it should be highlighted that Assumption 4 has only theoretical and no practical implications, and does not affect the controller’s performance in any way.

Then, using Lemma 6, we arrive at the following result, that establishes (almost) global convergence to the goal $x_{d}$ .

Proposition 4. With $I$ the terminal mode of the hybrid controller, the goal $x_{d}$ is an asymptotically stable equilibrium of (38), whose region of attraction includes the freespace $F_{map}^{I}$ except a set of measure zero.

Proof. See Appendix E.2.□

We can now immediately conclude the following central summary statement.

Theorem 1. With $I$ the terminal mode of the hybrid controller, the reactive controller in (38) leaves the freespace $F_{map}^{I}$ positively invariant, and its unique continuously differentiable flow, starting at almost any robot placement $x \in F_{map}^{I}$ , asymptotically reaches the goal location $x_{d}$ , while strictly decreasing $∥ h^{I} (x) - h^{I} (x_{d}) ∥$ along the way.

A depiction of the vector field in (38) for the terminal mode $I$ (Definition 8) from one of our numerical examples presented in Section 7 is included in Figure 6.

5.2.2. Differential drive robots

As the robot operates in $SE (2)$ instead of $R^{2}$ , we first need to come up with a smooth diffeomorphism ${\bar{h}}^{I} : F_{map}^{I} \times S^{1} \to F_{model}^{I} \times S^{1}$ away from sharp corners on the boundary of $F_{map}^{I} \times S^{1}$ , and then establish the results about our controller.

Following our previous work (Vasilopoulos and Koditschek, 2018), we construct our map ${\bar{h}}^{I}$ from $F_{map}^{I} \times S^{1}$ to $F_{model}^{I} \times S^{1}$ as

\bar{y} = (y, φ) = {\bar{h}}^{I} (\bar{x}) : = (h^{I} (x), ξ^{I} (\bar{x}))

(40)

with $\bar{x} = (x, ψ) \in F_{map}^{I} \times S^{1}$ , $\bar{y} : = (y, φ) \in F_{model}^{I} \times S^{1}$ and

φ = ξ^{I} (\bar{x}) : = ∠ (e (\bar{x}))

(41)

Here, $∠ e : = atan 2 (e_{2}, e_{1})$ and

e (\bar{x}) = Π_{y} \cdot D_{\bar{x}} {\bar{h}}^{I} \cdot B (ψ) \cdot [\begin{matrix} 1 \\ 0 \end{matrix}] = D_{x} h^{I} [\begin{matrix} \cos ψ \\ \sin ψ \end{matrix}]

(42)

with $Π_{y}$ denoting the projection onto the first two components. The reason for choosing $φ$ as in (42) will become evident later, in our effort to control the equivalent differential drive robot dynamics in $F_{model}^{I}$ .

Proposition 5. The map ${\bar{h}}^{I}$ in (40) is a $C^{\infty}$ diffeomorphism from $F_{map}^{I} \times S^{1}$ to $F_{model}^{I} \times S^{1}$ away from sharp corners, none of which lie in the interior of $F_{map}^{I} \times S^{1}$ .

Proof. See Appendix E.2.□

Then, using (40), we can find the pushforward of the differential drive robot dynamics in (32) as

\begin{matrix} \bar{y} = \frac{d}{dt} [\begin{matrix} h^{I} (x) \\ ξ^{I} (\bar{x}) \end{matrix}] \\ = [D_{\bar{x}} {\bar{h}}^{I} ° {({\bar{h}}^{I})}^{- 1}] \cdot (B ° {({\bar{h}}^{I})}^{- 1} (\bar{y})) \cdot {\bar{u}}^{I} \end{matrix}

(43)

Based on the above, we can then write

\bar{y} = [\begin{matrix} \overset{\cdot}{y} \\ \overset{\cdot}{φ} \end{matrix}] = \frac{d}{dt} [\begin{matrix} h^{I} (x) \\ ξ^{I} (\bar{x}) \end{matrix}] = B (φ) {\bar{v}}^{I}

(44)

with ${\bar{v}}^{I} = ({\hat{v}}^{I}, {\hat{ω}}^{I})$ , and the inputs $({\hat{v}}^{I}, {\hat{ω}}^{I})$ related to $(v^{I}, ω^{I})$ through

{\hat{v}}^{I} = ∥ e (\bar{x}) ∥ v^{I}

(45)

{\hat{ω}}^{I} = v^{I} D_{x} ξ^{I} [\begin{matrix} \cos ψ \\ \sin ψ \end{matrix}] + \frac{\partial ξ^{I}}{\partial ψ} ω^{I}

(46)

with $D_{x} ξ^{I} = [\begin{matrix} \frac{\partial ξ^{I}}{\partial x} & \frac{\partial ξ^{I}}{\partial y} \end{matrix}]$ . Here, we can calculate

D_{x} ξ^{I} [\begin{matrix} \cos ψ \\ \sin ψ \end{matrix}] = \frac{α_{1} (\bar{x}) α_{3} (\bar{x}) + α_{2} (\bar{x}) α_{4} (\bar{x})}{∥ e (\bar{x}) ∥^{2}}

(47)

with the auxiliary terms $α_{1}, α_{2}, α_{3}, α_{4}$ defined as

α_{1} (\bar{x}) : = - ({[D_{x} h^{I}]}_{21} \cos ψ + {[D_{x} h^{I}]}_{22} \sin ψ)

(48)

α_{2} (\bar{x}) : = [D_{x} h^{I}]_{11} \cos ψ + [D_{x} h^{I}]_{12} \sin ψ

(49)

\begin{matrix} α_{3} (\bar{x}) : = \frac{\partial {[D_{x} h^{I}]}_{11}}{\partial {[x]}_{1}} \overset{2}{\cos} ψ + \frac{\partial {[D_{x} h^{I}]}_{12}}{\partial {[x]}_{2}} \overset{2}{\sin} ψ \\ + (\frac{\partial {[D_{x} h^{I}]}_{11}}{\partial {[x]}_{2}} + \frac{\partial {[D_{x} h^{I}]}_{12}}{\partial {[x]}_{1}}) \sin ψ \cos ψ \end{matrix}

(50)

\begin{matrix} α_{4} (\bar{x}) : = \frac{\partial {[D_{x} h^{I}]}_{21}}{\partial {[x]}_{1}} \overset{2}{\cos} ψ + \frac{\partial {[D_{x} h^{I}]}_{22}}{\partial {[x]}_{2}} \overset{2}{\sin} ψ \\ + (\frac{\partial {[D_{x} h^{I}]}_{21}}{\partial {[x]}_{2}} + \frac{\partial {[D_{x} h^{I}]}_{22}}{\partial {[x]}_{1}}) \sin ψ \cos ψ \end{matrix}

(51)

We provide more details about the calculation of partial derivatives for elements of $D_{x} h^{I}$ used above in Section 6 and in Appendix D.

Hence, we have found equivalent differential drive robot dynamics, defined on $F_{model}^{I} \times S^{1}$ . The idea now is to use the control strategy in Arslan and Koditschek (2016b) for the dynamical system in (44) to find inputs ${\hat{v}}^{I}, {\hat{ω}}^{I}$ in $F_{model}^{I} \times S^{1}$ , and then use (45), (46) to find the actual inputs $v^{I}, ω^{I}$ in $F_{map}^{I} \times S^{1}$ that achieve ${\hat{v}}^{I}, {\hat{ω}}^{I}$ as

v^{I} = \frac{k_{v} {\hat{v}}^{I}}{∥ e (\bar{x}) ∥}

(52a)

ω^{I} = {(\frac{\partial ξ^{I}}{\partial ψ})}^{- 1} (k_{ω} {\hat{ω}}^{I} - v^{I} D_{x} ξ^{I} [\begin{matrix} \cos ψ \\ \sin ψ \end{matrix}])

(52b)

with $k_{v}, k_{ω} > 0$ fixed gains.

Namely, inspired by Arslan and Koditschek (2016b) and Astolfi (1999), we design our inputs ${\hat{v}}^{I}$ and ${\hat{ω}}^{I}$ as¹¹

{\hat{v}}^{I} = - {[\begin{matrix} \cos φ \\ \sin φ \end{matrix}]}^{⊤} (y - y_{d, ∥} (\bar{y}))

(53a)

{\hat{ω}}^{I} = atan (\frac{{[\begin{matrix} - \sin φ \\ \cos φ \end{matrix}]}^{⊤} (y - y_{d, G} (\bar{y}))}{{[\begin{matrix} \cos φ \\ \sin φ \end{matrix}]}^{⊤} (y - y_{d, G} (\bar{y}))})

(53b)

with $L F (y) \subset F_{model}^{I}$ the convex polygon defining the local freespace at $y = h^{I} (x)$ , and linear and angular local goals $y_{d, ∥} (\bar{y}), y_{d, G} (\bar{y})$ given by

y_{d, ∥} (\bar{y}) : = Π_{L F (y) \cap H_{∥}} (y_{d})

(54)

y_{d, G} (\bar{y}) : = \frac{Π_{L F (y) \cap H_{G}} (y_{d}) + Π_{L F (y)} (y_{d})}{2}

(55)

with $H_{∥}$ and $H_{G}$ the lines defined in Arslan and Koditschek (2016b) as

H_{∥} = {z \in F_{model}^{I} | {[\begin{matrix} - \sin φ \\ \cos φ \end{matrix}]}^{⊤} (z - y) = 0}

(56)

H_{G} = {α y + (1 - α) y_{d} \in F_{model}^{I} | α \in R}

(57)

The properties of the differential drive robot control law given in (52) can be summarized in the following theorem.

Theorem 2. With $I$ the terminal mode of the hybrid controller, the reactive controller for differential drive robots, given in (52), leaves the freespace $F_{map}^{I} \times S^{1}$ positively invariant, and its unique continuously differentiable flow, starting at almost any robot configuration $(x, ψ) \in F_{map}^{I} \times S^{1}$ , asymptotically steers the robot to the goal location $x_{d}$ , without increasing $∥ h^{I} (x) - h^{I} (x_{d}) ∥$ along the way.

Proof. See Appendix E.2.□

5.3. Qualitative properties of the hybrid controller

5.3.1. Fully actuated robots

First, we show that the navigational hybrid system $H$ inherits the fundamental consistency properties outlined in Johnson et al. (2016: Theorems 5–9), in order to establish that the hybrid system is well-behaved in the sense of being both deterministic and non-blocking (i.e., generating executions defined for all future times).

Lemma 7. The hybrid system $H$ has disjoint guards.

Proof. See Appendix E.2.□

An immediate result following Lemma 7, that does not allow a robot state $x \in F^{I}$ to be contained in more than one guard $G^{I, I'}$ , and the nice properties of the flow in each separate mode, summarized in Corollary 4, is the following important consistency property.

Corollary 5. The hybrid system $H$ is deterministic.

Next, we focus on the non-blocking property. As stated in Johnson et al. (2016), a hybrid execution might be blocked either by conventional finite escape through the boundary of the hybrid domain at a point in the complement of all the guards, by escape through a point in the guard whose reset lies outside of the hybrid domain, or by hybrid ambiguity, i.e., by arriving at a point through the continuous flow that lies in the complement of the guard $G$ and yet still on the boundary of $G$ . We eliminate all cases in the proof of the following result.

Lemma 8. The hybrid system $H$ is non-blocking.

Proof. See Appendix E.2.□

Finally, using the last part of the proof of Lemma 8 which shows that the (identity) reset from a given mode cannot lie in the guard of the next mode, we arrive at the following result about the discrete transitions of the hybrid system $H$ .

Corollary 6. An execution of the hybrid system $H$ undergoes no more than one hybrid transition at a single time $t$ .

Based on the above, the central result about the hybrid controller for a fully actuated robot can be summarized in the following theorem.

Theorem 3. For a fully actuated robot with dynamics defined in (28), the deterministic, non-blocking navigational hybrid system $H : = (2^{N_{P}}, Γ, D, U, G, R)$ , with the restrictions of guards $G$ , resets $R$ and vector fields $U$ defined as in (29), (30), and (31), respectively, leaves the free space $F$ positively invariant under the Lipschitz continuous, piecewise smooth flow associated with each of its hybrid domains, and, starting at almost any robot placement $x \in F$ at time $t_{0}$ with an initial mode $I = \emptyset$ , asymptotically reaches a designated goal location $x_{d} \in F$ , in a previously unexplored environment satisfying Assumptions 1–4, with a uniquely defined (in both state and mode) execution for all $t > t_{0}$ .

Proof. See Appendix E.2.□

5.3.2. Differential drive robots

We can then follow exactly the same procedure to prove the following statement for the hybrid controller for differential drive robots.

Theorem 4. For a differential drive robot with dynamics defined in (32), the deterministic, non-blocking navigational hybrid system $\bar{H} : = (2^{N_{P}}, Γ, \bar{D}, \bar{U}, \bar{G}, \bar{R})$ , with the restrictions of guards $\bar{G}$ , resets $\bar{R}$ and vector fields $\bar{U}$ defined as in (33), (34), and (35), respectively, leaves the free space $F \times S^{1}$ positively invariant under the Lipschitz continuous, piecewise smooth flow associated with each of its hybrid domains, and, starting at almost any robot placement $\bar{x} \in F \times S^{1}$ at time $t_{0}$ with an initial mode $I = \emptyset$ , asymptotically reaches a designated goal location $x_{d} \in F$ , in a previously unexplored environment satisfying Assumptions 1–4, with a uniquely defined (in both state and mode) execution for all $t > t_{0}$ .

5.4. Generating bounded inputs

Although the control inputs for both a fully actuated robot and a differential drive robot, described in (38) and (52), respectively, can be used in the hybrid systems description of the controller (see (31) and (35)) to yield the desired results of Theorems 3 and 4, we have so far implicitly assumed that there is no bound in the magnitude of $u^{I}$ in (38) or the magnitudes of $v^{I}, ω^{I}$ in (52) for each separate mode $I$ . In this section, we show how to generate bounded inputs without affecting the results of Theorems 3 and 4.

5.4.1. Fully actuated robots

We focus on fully actuated robots first. Let $u_{nom}^{I} : F_{map}^{I} \to T F_{map}^{I}$ denote the nominal input for mode $I$ , defined using (38) as

u_{nom}^{I} (x) : = {[D_{x} h^{I}]}^{- 1} \cdot (v^{I} ° h^{I} (x))

(58)

We can then easily satisfy the requirement $∥ u^{I} ∥ \leq u_{\max}$ by picking a gain $k$ such that $0 < k \leq u_{\max}$ and defining our controller as

u^{I} (x) : = k \frac{u_{nom}^{I} (x)}{∥ u_{nom}^{I} (x) ∥ + ϵ_{u}}

(59)

with $ϵ_{u} > 0$ a small number. The modified (bounded) controller in (59) does not affect the results of Theorem 3, because it maintains the heading direction of the original (unbounded) controller in (38), and just limits its magnitude.

5.4.2. Differential drive robots

The analysis is slightly more complicated for differential drive robots, because we have to respect the fact that the actual inputs $v^{I}$ , $ω^{I}$ are related to the inputs ${\hat{v}}^{I}$ , ${\hat{ω}}^{I}$ through (52). However, an important observation, deriving from the proof of Theorem 2, is that the choice of gains $k_{v}, k_{ω} > 0$ in (52) does not affect the positive invariance or convergence properties of the controller, which rely entirely on ${\hat{v}}^{I}$ , ${\hat{ω}}^{I}$ , given in (53).

Therefore, the main idea is to adaptively change the gains online, in order to satisfy the constraints $| v^{I} | \leq v_{\max}$ , $| ω^{I} | \leq ω_{\max}$ . Namely, using (52), we look for gains $k_{v} (\bar{x}), k_{ω} (\bar{x})$ such that

\begin{matrix} k_{v} (\bar{x}) \frac{| {\hat{v}}^{I} (\bar{x}) |}{∥ e (\bar{x}) ∥} \leq v_{\max} \\ | k_{ω} (\bar{x}) {\hat{ω}}^{I} (\bar{x}) - k_{v} (\bar{x}) \frac{{\hat{v}}^{I} (\bar{x})}{∥ e (\bar{x}) ∥} ϑ (ψ) | \leq \frac{\partial ξ^{I}}{\partial ψ} ω_{\max} \end{matrix}

with $ϑ (ψ) : = D_{x} ξ^{I} {[\begin{matrix} \cos ψ & \sin ψ \end{matrix}]}^{⊤}$ , because $\frac{\partial ξ^{I}}{\partial ψ} > 0$ , as shown in the proof of Proposition 5. A conservative selection of gains that satisfies the above constraints can then be extracted using the triangle inequality as follows:

\begin{matrix} k_{v} (\bar{x}) = min (k_{v, nom}, \frac{∥ e (\bar{x}) ∥}{| {\hat{v}}^{I} (\bar{x}) |} v_{\max}, \\ λ \frac{\partial ξ^{I}}{\partial ψ} \frac{∥ e (\bar{x}) ∥}{| {\hat{v}}^{I} (\bar{x}) | | ϑ (ψ) |} ω_{\max}) \end{matrix}

(61)

k_{ω} (\bar{x}) = min (k_{ω, nom}, (1 - λ) \frac{\partial ξ^{I}}{\partial ψ} \frac{ω_{\max}}{| {\hat{ω}}^{I} (\bar{x}) |})

(62)

with $k_{v, nom}, k_{ω, nom} > 0$ initially provided nominal gains and $λ \in (0, 1)$ a tuning parameter. It can be seen that $k_{v} (\bar{x}), k_{ω} (\bar{x})$ are always positive because $∥ e (\bar{x}) ∥, \frac{\partial ξ^{I}}{\partial ψ}$ are always positive.

6. Online reactive planning algorithms

With the description of the diffeomorphism construction and the overall hybrid controller, we are now ready to describe the algorithm we use during execution time to generate our control inputs. As shown in Figure 2 that summarizes the whole architecture, we divide the main algorithm that communicates with the semantic mapping and the perception pipelines¹² in two distinct components. First, the mapped space recovery component, described in Section 6.1, is responsible for keeping track of all encountered objects, and extracting the sets of obstacles $D_{map}^{I}$ , $B_{map}^{I}$ . Next, the reactive planning component, described in Section 6.2, uses the input from the mapped space recovery component to generate the diffeomorphism $h^{I}$ (described in Section 4) between the mapped space and the model space during execution time, and provide the commands for the robot according to the hybrid controller (described in Section 5).

6.1. Mapped space recovery

Given as input the aggregated set of localized, recognized familiar obstacles ${\tilde{P}}_{I}$ , we first dilate all these elements of ${\tilde{P}}_{I}$ by the robot radius $r$ , to form the components of $P_{s em}^{I}$ , and consolidate the connected components resulting from their union into a new set of merged obstacles to form $P_{map}^{I}$ . Then, for each connected component $P$ of $P_{map}^{I}$ that intersects the boundary of the enclosing freespace $F_{e}$ , we take $B = P \cap F_{e}$ , as described in Section 3.3, and include $B$ in the list of obstacles to be merged into $\partial F_{e}$ , $B_{map}^{I}$ ; the rest of the components of $P_{map}^{I}$ are included in the list of obstacles to be deformed into disks, $D_{map}^{I}$ .¹³ All these computational steps rely on underlying polygon operations (unions, intersections, differences); the development of such algorithms has been heavily explored in the computational geometry literature (Clementini et al., 1993; Douglas and Peucker, 1973; Egenhofer and Herring, 1991), and here we rely on their efficient implementations, either in the open-source C++ Boost library (Schling, 2011), or in the open-source Shapely package (Shapely, 2019) in Python.

The next step is to triangulate every obstacle $P_{i}$ in both $D_{map}^{I}$ and $B_{map}^{I}$ using the Ear Clipping Method, find its root triangle $r_{i}$ and extract the corresponding tree of triangles $T_{P_{i}} : = (V_{P_{i}}, E_{P_{i}})$ , as described in Section 4.1. For the implementation of the Ear Clipping Method, we use either the open-source Boost library (Schling, 2011), for our C++ implementation, or the open-source tripy package (linuxlewis, 2019), for our Python implementation.

The final operation of the mapped space recovery algorithm is to extract the admissible centers of transformation, $x_{j}^{*}$ , according to Definitions 2–7, the corresponding radius of transformation, $ρ_{i}$ (if $P_{i} \in D_{map}^{I}$ ), and the admissible polygonal collars, ${\bar{Q}}_{j}$ for all triangles $j \in V_{P_{i}}$ and polygons $P_{i}$ in $D_{map}^{I}$ and $B_{map}^{I}$ . There is not a unique method of performing this operation, and we provide our implemented method along with other details in Appendix C. The mapped space recovery algorithm is summarized in Algorithm 1.

Algorithm 1 Derivation of the sets of obstacles $D_{map}^{I}, B_{map}^{I}$ , used in the diffeomorphism construction, and their associated properties, from the aggregated list of known obstacles in the physical space ${\tilde{P}}_{I}$ .
Function MappedSpaceRecovery ${\tilde{P}}_{I}$ $P_{map}^{I} \leftarrow Union (dilate ({\tilde{P}}_{I}, r))$ do $P \leftarrow pop (P_{map}^{I})$ ▹Pop next component if $P \cap \partial F_{e} \neq \emptyset$ then $B . geometry \leftarrow P \cap F_{e}$ $B . tree \leftarrow EarClipping (B . geometry)$ Find root of tree $B . root$ as in Section 4.1 Restructure $B . tree$ around $B . root$ for $j \in B . tree . vertices$ do ▹Definitions 2–7 $B . tree . vertices (j) . append (x_{j}^{})$ $B . tree . vertices (j) . append ({\bar{Q}}_{j})$ end for $B_{map}^{I} . append (B)$ else $D . geometry \leftarrow P$ $D . tree \leftarrow EarClipping (D . geometry)$ Find root of tree $D . root$ as in Section 4.1 Restructure $D . tree$ around $D . root$ for $j \in D . tree . vertices$ do ▹Definitions 2–7 $D . tree . vertices (j) . append (x_{j}^{})$ $D . tree . vertices (j) . append ({\bar{Q}}_{j})$ end for Find $ρ = D . radius$ as in Section 4.3 $D_{map}^{I} . append (D)$ end if while $P_{map}^{I} \neq \emptyset$ $return D_{map}^{I}, B_{map}^{I}$ end function

Algorithm 1 Derivation of the sets of obstacles

D_{map}^{I}, B_{map}^{I}

, used in the diffeomorphism construction, and their associated properties, from the aggregated list of known obstacles in the physical space

{\tilde{P}}_{I}

Function MappedSpaceRecovery

{\tilde{P}}_{I}

P_{map}^{I} \leftarrow Union (dilate ({\tilde{P}}_{I}, r))

P \leftarrow pop (P_{map}^{I})

▹Pop next component
if

P \cap \partial F_{e} \neq \emptyset

then

B . geometry \leftarrow P \cap F_{e}

B . tree \leftarrow EarClipping (B . geometry)

Find root of tree

B . root

as in Section 4.1
Restructure

B . tree

around

B . root

for

j \in B . tree . vertices

do ▹Definitions 2–7

B . tree . vertices (j) . append (x_{j}^{*})

B . tree . vertices (j) . append ({\bar{Q}}_{j})

end for

B_{map}^{I} . append (B)

else

D . geometry \leftarrow P

D . tree \leftarrow EarClipping (D . geometry)

Find root of tree

D . root

as in Section 4.1
Restructure

D . tree

around

D . root

for

j \in D . tree . vertices

do ▹Definitions 2–7

D . tree . vertices (j) . append (x_{j}^{*})

D . tree . vertices (j) . append ({\bar{Q}}_{j})

end for
Find

ρ = D . radius

as in Section 4.3

D_{map}^{I} . append (D)

end if
while

P_{map}^{I} \neq \emptyset

return D_{map}^{I}, B_{map}^{I}

end function

6.2. Reactive planning component

The mapped space recovery algorithm described above just informs the robot about its surroundings, by post-processing aggregated information from the semantic mapping pipeline. In this section, we describe the algorithm for generating actual robot inputs, that closes our control loop.

Given the robot state in the mapped space ( $x$ for a fully actuated robot or $\bar{x}$ for a differential drive robot), and the list of obstacles in the mapped space, $D_{map}^{I}, B_{map}^{I}$ , along with their associated triangulation trees and their properties as computed with Algorithm 1, the first step of the reactive planning algorithm is to compute the state of the robot in the model space ( $h^{I} (x)$ for a fully actuated robot (27) or ${\bar{h}}^{I} (\bar{x})$ for a differential drive robot (40)), the diffeomorphism Jacobian $D_{x} h^{I}$ , and partial derivatives of the terms of the jacobian $\frac{\partial {[D_{x} h^{I}]}_{ij}}{\partial {[x]}_{k}}$ (needed in (52) for a differential drive robot), following the methods outlined in Section 4. We show in Appendix D how to perform this operation inductively, given the general form of $h^{I}$ in (27).

Next, we need to properly populate the model space with obstacles, in order to compute the input (59) for a fully actuated robot, or the inputs (52) (using (61) and (62)) for a differential drive robot. This procedure is straightforward for familiar obstacles; obstacles in $B_{map}^{I}$ are not taken into account in the model space, because they are merged into the boundary $\partial F_{e}$ , and obstacles in $D_{map}^{I}$ are represented in the model space as disks with radius $ρ_{i}$ centered at $x_{i}^{*}$ , with $i$ spanning the elements of $D_{map}^{I}$ . For unknown obstacles in $C_{map}$ , we use the LIDAR measurements (see Figure 2). Namely, we first pre-process the 2D LIDAR pointcloud by disregarding points that correspond to obstacles in $D_{map}^{I}$ or $B_{map}^{I}$ , because those have already been considered. The pointcloud with the remaining points is then transferred with an identity transform to the model space; this is allowed because, by construction, the diffeomorphism $h^{I}$ between $F_{map}^{I}$ and $F_{model}^{I}$ defaults to the identity transform (i.e., $h^{I} (x) = x$ ) on the boundary of any unknown obstacle, provided that this obstacle is sufficiently separated from any obstacle in $P_{map}^{I}$ (see Assumption 2).

With the (“virtual”) model space constructed, we can then construct the local freespace (37), as in Arslan and Koditschek (2016b: Equation (24)), and, subsequently, compute the input $v^{I}$ (36) for a fully actuated robot or the inputs ${\bar{v}}^{I}$ (53) for a differential drive robot. The final step is to compute the “pull-backs” of these inputs in the physical space and enforce bounds, by using (59) for a fully actuated robot, or (52) along with (61) and (62) to adaptively modify the input gains for a differential drive robot. The reactive planning module functionality is summarized in Algorithm 2.

Algorithm 2 Description of the online reactive planning module that uses the state of the robot, LIDAR input, and $D_{map}^{I}, B_{map}^{I}$ .
functionReactiveplanning ( $State$ , $LIDAR$ , $D_{map}^{I}, B_{map}^{I}$ ) if $RobotType$ is $FullyActuated$ then $x \leftarrow State$ $y \leftarrow h^{I} (x)$ ▹Section 4, Appendix D Compute $D_{x} h^{I}$ ▹Appendix D Populate $F_{model}^{I}$ using $LIDAR, D_{map}^{I}, B_{map}^{I}$ Construct $L F (y)$ ▹ (37) Compute input $v^{I}$ ▹(36) Compute input $u^{I}$ ▹(59) $RobotInput \leftarrow u^{I}$ else if $RobotType$ is $DiffDrive$ then $\bar{x} \leftarrow State$ $\bar{y} \leftarrow {\bar{h}}^{I} (\bar{x})$ ▹Section 5.2, (40), Appendix D Compute $D_{x} h^{I}, \frac{\partial {[D_{x} h^{I}]}_{ij}}{\partial {[x]}_{k}}$ ▹Appendix D Populate $F_{model}^{I}$ using $LIDAR, D_{map}^{I}, B_{map}^{I}$ Construct $L F (y)$ ▹(37) Compute inputs ${\bar{v}}^{I}$ ▹(53) Compute input ${\bar{u}}^{I}$ ▹(52) using (61) and (62) $RobotInput \leftarrow {\bar{u}}^{I}$ end if $return RobotInput$ end function

Algorithm 2 Description of the online reactive planning module that uses the state of the robot, LIDAR input, and

D_{map}^{I}, B_{map}^{I}

functionReactiveplanning (

State

LIDAR

D_{map}^{I}, B_{map}^{I}

)
if

RobotType

FullyActuated

then

x \leftarrow State

y \leftarrow h^{I} (x)

▹Section 4, Appendix D
Compute

D_{x} h^{I}

▹Appendix D
Populate

F_{model}^{I}

using

LIDAR, D_{map}^{I}, B_{map}^{I}

Construct

L F (y)

▹ (37)
Compute input

v^{I}

▹(36)
Compute input

u^{I}

▹(59)

RobotInput \leftarrow u^{I}

else if

RobotType

DiffDrive

then

\bar{x} \leftarrow State

\bar{y} \leftarrow {\bar{h}}^{I} (\bar{x})

▹Section 5.2, (40), Appendix D
Compute

D_{x} h^{I}, \frac{\partial {[D_{x} h^{I}]}_{ij}}{\partial {[x]}_{k}}

▹Appendix D
Populate

F_{model}^{I}

using

LIDAR, D_{map}^{I}, B_{map}^{I}

Construct

L F (y)

▹(37)
Compute inputs

{\bar{v}}^{I}

▹(53)
Compute input

{\bar{u}}^{I}

▹(52) using (61) and (62)

RobotInput \leftarrow {\bar{u}}^{I}

end if

return RobotInput

end function

It must be highlighted that the presented reactive planning pipeline (summarized in Figure 2) runs at 10 Hz online and onboard our physical robots’ Nvidia Jetson TX2 modules, during execution time.

7. Numerical results

In this section, we present numerical simulations that illustrate our formal results. Our simulations are run in MATLAB using ode45, and $p = 20$ for the R-function construction, as described in Appendix B. Our mapped space recovery (Section 6.1) and reactive planning (Section 6.2) algorithms are implemented in Python and communicate with MATLAB using the standard MATLAB–Python interface. For our numerical results, we assume perfect robot state estimation and localization of obstacles, using a fixed range sensor that can instantly identify and localize either the entirety of familiar obstacles that intersect its footprint, or the corresponding fragments of unknown obstacles within its range.

7.1. Comparison with original doubly reactive algorithm

We begin with a comparison of our algorithm performance with the original version of the doubly reactive algorithm in Arslan and Koditschek (2016b)), that we use in the model space computed at each instant from the perceptual inputs as depicted in Figure 2(e) and described in Section 6.2. Figure 7 (Extension 1) demonstrates the well-understood limitations of this algorithm (limitations of all online (Borenstein and Koren, 1991) or offline (Filippidis and Kyriakopoulos, 2012) reactive schemes we are aware of). Namely, in the presence of a flat surface or a non-convex obstacle, or when separation assumptions are violated, the robot gets stuck in undesired local minima, which are locally stable and trap a set of initial conditions whose area becomes arbitrarily large as their “shadows” (i.e., the corresponding basins of attraction) grow; see, e.g., Vasilopoulos et al. (2020: Figure 6(b)). Absent the new methods introduced in this article, handling these spurious basins of attraction would require complex dynamic replanning algorithms (Reverdy et al., 2015; Revzen et al., 2012), whose presentation falls beyond the scope of the present article. In contrast, our algorithm overcomes this limitation, by recourse to the robot’s ability to recognize obstacles at hand (documented empirically in Section 9) and transform them appropriately (as detailed in Section 4) for both a fully actuated and a differential drive robot. The robot radius used in our simulation studies is 0.2 m, the control gains are $k = k_{v} = k_{ω} = 0.4$ , and the values of $μ_{γ_{j_{i}}} = μ_{γ_{r_{i}}}$ , $μ_{δ_{j_{i}}} = μ_{δ_{r_{i}}}$ and $ϵ_{j_{i}} = ϵ_{r_{i}}$ used in the diffeomorphism construction are 4.0, 0.05, and 2.0, respectively. Finally, the maximum input $u_{\max}$ for the fully actuated robot as well as the maximum linear and angular inputs $v_{\max}, ω_{\max}$ for the differential drive robot are limited to 0.4.

7.2. Navigation in a cluttered environment with obstacle merging

For the next set of numerical studies, we focus on environments cluttered with several instances of the same familiar obstacle, in different, a priori unknown poses. We illustrate the concept in Figure 8 (Extension 1). The robot abstracts away the familiar geometry to explore the unknown topology of the workspace online during execution time. In this particular example, the robot first adopts the hypothesis that an “opening” exists above the initially observed obstacle. With the observation and instantiation of the second obstacle in the semantic map, it is then capable of correcting this hypothesis by merging the obstacle to the boundary of $F_{e}$ . The properties of the hybrid controller presented in Section 5 guarantee convergence to the goal for both the fully actuated and the differential drive robot, as shown in Figure 9 and the accompanying Extension 1.

We further illustrate the scope of formal results by presenting numerical simulations where the constellation of fixed obstacles incurs the need for multiple mergings between obstacles or between obstacles and the boundary of the enclosing freespace $F_{e}$ . As guaranteed, both the fully actuated (Theorem 3) and the differential drive (Theorem 4) robots converge to the desired goal from a variety of initial conditions (all but a set of measure zero must converge), as shown in Figure 10 (Extension 1). The robot radius used in our simulations is 0.25 m, the control gains are $k = k_{v} = k_{ω} = 0.4$ , and the values of $μ_{γ_{j_{i}}} = μ_{γ_{r_{i}}}$ , $μ_{δ_{j_{i}}} = μ_{δ_{r_{i}}}$ and $ϵ_{j_{i}} = ϵ_{r_{i}}$ used in the diffeomorphism construction are 2.0, 0.05, and 1.0, respectively. The maximum input $u_{\max}$ for the fully actuated robot as well as the maximum linear and angular inputs $v_{\max}, ω_{\max}$ for the differential drive robot are limited to 0.4.

7.3. Navigation among mixed known and unknown obstacles

Finally, Figure 11 (Extension 1) illustrates the convergence guarantees for both a fully actuated as well as a differential drive robot when confronted both by familiar obstacles (with a priori unknown pose) as well as completely unknown obstacles (presumed to satisfy the convexity and separation assumptions of Arslan and Koditschek (2016b)), as outlined in Section 2. The robot radius used in our simulations is 0.25 m, the control gains are $k = k_{v} = k_{ω} = 0.4$ , and the values of $μ_{γ_{j_{i}}} = μ_{γ_{r_{i}}}$ , $μ_{δ_{j_{i}}} = μ_{δ_{r_{i}}}$ , and $ϵ_{j_{i}} = ϵ_{r_{i}}$ used in the diffeomorphism construction are 1.6, 0.05, and 0.8 respectively. The maximum input $u_{\max}$ for the fully actuated robot as well as the maximum linear and angular inputs $v_{\max}, ω_{\max}$ for the differential drive robot are limited to 0.4.

8. Experimental setup

Because the reactive planners introduced in this article take the form of first order vector fields (i.e., issuing velocity commands at each state), we use a quasi-static platform, the Turtlebot robot (TurtleBot2, 2019), for the bulk of physical experiments reported next. With the aim of merely suggesting the robustness of these feedback controllers, we also repeat two of those experiments using the highly dynamic Minitaur robot (Ghost Robotics, 2016), whose rough approximation to the quasi-static differential drive motion model is adequate to yield nearly indistinguishable navigation behavior. In the conclusion, we list further factors highlighting the importance of this legged implementation by sketching a longer term agenda emerging from recent results in Vasilopoulos et al. (2018) for integrating this planner into a more complicated system that achieves mobile manipulation tasks with legged robots in dynamic environments.

The experimental setups for our robots are depicted in Figure 12. In both cases, the main computer is an Nvidia TX2 GPU unit (NVIDIA, 2019), responsible for running our mapped space recovery and reactive planning algorithms online, during execution time, according to Figure 2. The GPU unit communicates with a Hokuyo LIDAR (Hokuyo, 2019), used to detect unknown obstacles, and a ZED Mini stereo camera (StereoLabs, 2019b), used for visual–inertial state estimation and for detecting familiar obstacles. As shown in Figure 2, we choose to run our perception and semantic mapping pipelines described next either onboard (using the same Nvidia TX2 GPU unit) or offboard (on a desktop computer with an Nvidia GeForce RTX 2080 GPU), for faster inference and improved performance. We also assume that the differential drive robot model, presented in (32), is the most suitable motion model for both robots. This is indeed the case for Turtlebot, and an extensive discussion on the empirical anchoring (Full and Koditschek, 1999) of the unicycle template on Minitaur is included in Vasilopoulos et al. (2017).

As the main focus of this article is not the development of new perception or state estimation algorithms, but rather the development of a provably correct planning architecture for partially known environments, we rely to as great an extent as possible on off-the-shelf perception algorithms, implemented in ROS (Quigley et al., 2009), and couple them with our motion planner for the hardware experiments. We are further motivated by the intent for our accompanying software to be modular and easily integrated to existing perception pipelines for future users. We briefly describe the perception and semantic mapping algorithms employed in this article in the following sections, and refer the reader once more to the summary illustration of the whole navigation stack in Figure 2.

8.1. Object detection and keypoint localization

The pipeline we use to detect the objects in the scene and extract the geometric properties needed in order to estimate their 3D pose relies on Pavlakos et al. (2017). The two components involved in this procedure are:

object detection, which returns 2D bounding boxes for each object;

keypoint localization, which estimates the 2D locations for a set of predefined keypoints for the specific object instance and class.

The algorithm is described in detail in Pavlakos et al. (2017), but here we give a brief overview of each step in Sections 8.1.1 and 8.1.2, and provide training details for our neural networks in Section 8.1.3.

8.1.1. Object detection

For the task of object detection, we only require the estimation of a 2D bounding box for each object that is visible on the image. We use the YOLOv3 detector (Redmon and Farhadi, 2018) which offers a good trade-off between detection accuracy and inference speed. Given a single RGB image as input, the output of the detector is a 2D bounding box for each object instance, along with the estimated class for this bounding box.

8.1.2. Keypoint localization

For the keypoint localization task, we use a convolutional neural network to accurately estimate the 2D location of the keypoints within the object’s bounding box. The keypoints are defined on the 3D model of the object and are selected in advance for each object instance. The keypoint localization network uses as input an RGB image of a specific object, which is cropped using the bounding box information from the detection step. The output of the network is a set of 2D heatmaps. Assuming we select $k$ keypoints for an object, each heatmap is responsible for the localization of the corresponding keypoint. To estimate the locations $W \in R^{2 \times k}$ of the $k$ keypoints on the image, we use the 2D heatmap location with the maximum activation for each heatmap as the detected location for the corresponding keypoint. We also consider the value of the activation at this location as the detection confidence $d_{i}$ for keypoint $i$ . The architecture for this network follows the Stacked Hourglass design (Newell et al., 2016). In practice, we train a single network for all objects of interest and at test time we use only the heatmaps for the specific class, which is already known from the detection step.

8.1.3. Training details

The aforementioned neural networks are trained to detect a predefined set of object instances visualized in Figure 13. The object classes represented for our experiments are chair, table, ladder, cart, gascan, and pelican case. Our goal is to include a variety of instances in terms of the size, shape and visual appearance, in an attempt to simulate the variety of objects that can be encountered in a partially familiar environment. The training data for the particular instances of interest are collected with a semi-automatic procedure, similarly to Pavlakos et al. (2017). Given the bounding box and keypoint annotations for each image, the two networks were trained with their default configurations until convergence.

8.2. Semantic mapping

Our semantic mapping infrastructure relies on the algorithm presented in Bowman et al. (2017), and implemented in C++ using GTSAM (Dellaert, 2012) and its iSAM2 implementation (Kaess et al., 2012) as the optimization back-end. Briefly, this algorithm fuses inertial information (here simply provided by the position tracking implementation from StereoLabs on the ZED Mini stereo camera (StereoLabs, 2019a)), and semantic information (i.e., the detected keypoints and the associated object labels as described in Section 8.1) to provide a posterior estimate for both the robot state and the associated poses for all tracked objects, by simultaneously solving the data association problem arising when several objects of the same class exist in the map. As described in Bowman et al. (2017), except for providing an estimate for all poses tracked in the environment, this algorithm facilitates loop-closure recognition based on viewpoint-independent semantic information (i.e., tracked objects), rather than low-level geometric features such as points, lines, or planes.

For a single frame detection, the 3D pose of each object with respect to the camera is recovered using the estimated 2D locations of the associated object keypoints. By denoting with $S \in R^{3 \times k}$ the 3D locations of the keypoints in the canonical pose of an object instance with $k$ keypoints, the goal is to estimate the rotation $R \in R^{3 \times 3}$ and translation $T \in R^{3 \times 1}$ of the object, such that the distance of the projected 3D keypoints from their corresponding detected 2D locations is minimized. To incorporate the detection confidence for each keypoint in the optimization, we define the matrix $D \in R^{k \times k}$ . This is a diagonal matrix that features the detection confidences $d_{i}$ for each keypoint $i$ in its diagonal. The optimization problem is then formulated as

min_{R, T} \frac{1}{2} ‖ (\tilde{W} Z - RS - T I^{⊤}) D^{\frac{1}{2}} ‖_{F}^{2}

(63)

where $\tilde{W} \in R^{3 \times k}$ represents the normalized homogeneous coordinates of the 2D keypoints and $Z \in R^{k \times k}$ is a diagonal matrix, that features the depths $z_{i}$ for each keypoint $i$ in its diagonal.

After the estimation of the object’s 3D pose from a single frame measurement as described above, the 3D positions of its corresponding semantic keypoints are then independently tracked and the object’s pose is appropriately updated, as more frame measurements are added. Once a sufficient number of frame measurements¹⁴ has been incorporated so that the 3D keypoint positions can be triangulated, the object is considered to be localized and is permanently added to the map. The reader is referred to Bowman et al. (2017) for more details. Figure 14 shows an example of this localization process. It should be noted that for our onboard implementation, where inference using the object detection and keypoint estimation neural networks is slower, we include in the semantic map both the localized objects, after several frame measurements, and objects resulting from a single frame measurement pose estimation, to allow for faster response to sensory input.

As shown in Figure 2, the meshes of the objects in the semantic map, defined by the corresponding keypoint adjacency properties and the extracted 3D pose, are projected on the robot’s plane of motion to provide the aggregated list of known obstacles in the physical space ${\tilde{P}}_{I}$ , forwarded to our mapped space recovery module (described in Section 6.1). On the other hand, the posterior estimate of the robot pose on the plane, extracted by the semantic mapping module, is forwarded to our reactive planning module (described in Section 6.2).

9. Experimental results

In this section, we provide our experimental results using both the Turtlebot and the Minitaur robot, and the setup described in Section 8. We begin with experiments run using Turtlebot and offboard (Section 9.1) or onboard (Section 9.2) perception, and continue with Minitaur experiments using offboard perception (Section 9.3), to demonstrate the robustness of our method on a more dynamic legged platform. It should be noted that although the perception algorithms, described in Section 8, are run either offboard or onboard, our mapped space recovery and reactive planning modules, described in Algorithms 1 and 2, respectively, are always run onboard each robot’s Nvidia TX2 module. The control gains used in our experiments are $k_{v} = k_{ω} = 0.4$ , and the maximum linear and angular inputs $v_{\max}, ω_{\max}$ are set to 0.4. The reader is referred to Extension 2 for more examples and illustrations.

9.1. Experiments with Turtlebot and offboard perception

9.1.1. Comparison with the original doubly reactive algorithm

In this section, we demonstrate experiments similar to the simulations reported in Section 7.1. We first illustrate various well-understood failures of the original version of the doubly reactive algorithm in (Arslan and Koditschek, 2016b). Collisions result from the presence of short obstacles that cannot be detected by the 2D LIDAR (Figure 15(a)). Confronted by obstacles with flat surfaces (Figure 15(b)), or when separation assumptions are violated (Figure 15(c)), the original algorithm gets stuck in undesired local minima (Figure 15(b) and (c)). In contrast, our new algorithm guarantees safe convergence to the goal in all these cases: short but familiar obstacles (in this case the gascan in column 3 of Figure 13) are recognized by the camera system and localized; once localized, these known geometries can then be appropriately abstracted into the model space (Section 4) which is topologically equivalent but geometrically simplified to meet the requirements of Arslan and Koditschek (2016b). Figure 15 (Extension 2) shows the groundtruth trajectory of the robot, recorded using Vicon, along with 2D projections on the horizontal plane of the obstacles’ keypoint meshes, that were used for the construction of the semantic space (Section 3.2). The values of $μ_{γ_{j_{i}}} = μ_{γ_{r_{i}}}$ , $μ_{δ_{j_{i}}} = μ_{δ_{r_{i}}}$ and $ϵ_{j_{i}} = ϵ_{r_{i}}$ used in the diffeomorphism construction are 4.0, 0.05, and 2.0, respectively.

9.1.2. Navigation in a cluttered environment with obstacle merging

We begin the second set of experiments by demonstrating the merging process and the properties of the hybrid controller, reported in Section 5, in a physical setting. As shown in Figure 16 (Extension 2), the robot starts navigating toward its target and localizing obstacles in front of it, until it converges to its target; at the same time, by incorporating more information in its semantic map, it experiences transitions to different modes of the (previously unknown) hybrid system. The values of $μ_{γ_{j_{i}}} = μ_{γ_{r_{i}}}$ , $μ_{δ_{j_{i}}} = μ_{δ_{r_{i}}}$ and $ϵ_{j_{i}} = ϵ_{r_{i}}$ used in this experiment are 4.0, 0.05, and 2.0, respectively.

Finally, Figure 17 (Extension 2) demonstrates navigation in environments cluttered with multiple familiar obstacles. In the first illustration, the robot reactively chooses to navigate through a gap between the gascan and a chair. Despite the blockage of this gap by another familiar obstacle (pelican case) in the second illustration, the robot reactively chooses to follow another safe and convergent trajectory (as guaranteed by the theorems of Section 5), by merging the set gascan–pelican case–chair, and considering them as a single obstacle. The values of $μ_{γ_{j_{i}}} = μ_{γ_{r_{i}}}$ , $μ_{δ_{j_{i}}} = μ_{δ_{r_{i}}}$ and $ϵ_{j_{i}} = ϵ_{r_{i}}$ used in this experiment are 1.6, 0.05, and 0.8, respectively.

9.1.3. Navigation among mixed known and unknown obstacles

In the next set of experiments, we consider navigation among multiple familiar and unknown obstacles. Figure 18 (Extension 2) shows that the robot safely converges to the goal from multiple initial conditions, using vision and the setup described in Section 8 for familiar obstacle detection and localization, and the onboard 2D LIDAR for all the unknown obstacles. In Figure 18, we also overlay trajectories from a MATLAB simulation of a differential drive robot with the same initial conditions and similar control gains; the simulated and physical platform follow similar trajectories in all three cases. The values of $μ_{γ_{j_{i}}} = μ_{γ_{r_{i}}}$ , $μ_{δ_{j_{i}}} = μ_{δ_{r_{i}}}$ and $ϵ_{j_{i}} = ϵ_{r_{i}}$ used in these experiments are 2.0, 0.05, and 1.0.

It should be highlighted that even when the object localization process fails, collision avoidance is still guaranteed with the use of the onboard LIDAR. Nevertheless, collisions could result with obstacles that cannot be detected by the 2D horizontal LIDAR (e.g., see Figure 15(a)). One could still think of extensions to the presented sensory infrastructure (e.g., the use of a 3D LIDAR) that could still guarantee safety under such circumstances.

9.2. Experiments with Turtlebot and onboard perception

This section briefly reports on experiments using onboard perception; the reader is referred to Extension 2 for more examples. As described in Section 8.2, here we use both the localized obstacles by the semantic mapping pipeline and raw, not permanently localized obstacles, resulting from a single semantic frame measurement and the optimization problem given in (63). Figure 19 (Extension 2) illustrates an example; the robot detects and avoids the two chairs in front of it, even if they are only temporarily included in the semantic map (in the absence of more frame measurements). The robot then proceeds to localize and avoid the gascan and the two tables and safely converge to the designated goal. The values of $μ_{γ_{j_{i}}} = μ_{γ_{r_{i}}}$ , $μ_{δ_{j_{i}}} = μ_{δ_{r_{i}}}$ , and $ϵ_{j_{i}} = ϵ_{r_{i}}$ used in this experiment are 2.0, 0.05, and 1.0, respectively.

It should be noted that the object impermanence in the semantic map violates the formal assumptions of Theorems 3 and 4; without permanently localizing an object, the robot could get stuck in an endless loop trying to avoid obstacles that it then “forgets,” in unfavorable workspace configurations (such as those reported in Figure 9).

9.3. Experiments with Minitaur

Finally, Figure 20 (Extension 2) presents illustrative snapshots of two navigation examples on the much more dynamic Minitaur platform. Despite the fact that Minitaur is an imperfect kinematic unicycle and the overall shakiness of the platform, the robot is capable of detecting and localizing familiar obstacles of interest and using that information to safely converge to the target. The values of $μ_{γ_{j_{i}}} = μ_{γ_{r_{i}}}$ , $μ_{δ_{j_{i}}} = μ_{δ_{r_{i}}}$ , and $ϵ_{j_{i}} = ϵ_{r_{i}}$ used in this experiment are 2.0, 0.05, and 1.0, respectively. The reader is referred to Extension 2 for more examples and illustrations.

10. Conclusion and future work

10.1. Conclusion

This article presents a reactive navigation scheme for robots operating in planar workspaces, cluttered with obstacles of familiar geometry but a priori unknown placement, and completely unknown, but strongly convex and well-separated obstacles. To the best of the authors’ knowledge, this is the first doubly reactive navigation framework (i.e., a scheme where not only the robot’s trajectory but also the vector field that generates it are computed online at execution time) that can handle arbitrary polygonal shapes in realtime without the need for specific separation assumptions between the familiar obstacles. The resulting algorithm combines state-of-the-art perception and object recognition techniques (based on neural network architectures) for familiar obstacles, with local range measurements (e.g., LIDAR) for the unknown obstacles, to yield provably correct navigation in geometrically complicated environments. We illustrate the practicability of this approach by reporting empirical results using modest computational hardware on a wheeled robot, and the intrinsic robustness of such reactive schemes by a second implementation on a dynamic legged platform, exhibiting imperfect fidelity to the differential drive model assumed in the formal results.

10.2. Future work

Figures 21 and 22 present snapshots from experiments in settings falling outside the scope of our formal results to illustrate some of the future directions opened up by the reactive planner presented in this article. Using the feature of semantic inference provided by the semantic mapping pipeline described in Section 8.2, the user can command the robot to target a semantic goal, instead of a merely geometric one (considered in this article). Namely, in Figure 21 (Extension 3), we command the Turtlebot robot to move to a geometrically predefined target, unless it sees and localizes a cart; in that case, it is tasked with approaching and facing the cart with its camera. As shown in the bottom row of Figure 21, the robot avoids familiar obstacles, localizes the cart and proceeds to properly approach it, with the right orientation. We take this approach one step further with the example shown in Figure 22 (Extension 3), using the Minitaur platform. Using the mobile manipulation primitives developed in Topping et al. (2019), we task the robot by not only localizing and approaching the cart, but also jumping to grab and mount it. This is a first step toward integrating the reactive planning architecture developed in this work in the multi-layer architecture presented in Vasilopoulos et al. (2018), for accomplishing increasingly complicated mobile manipulation tasks with underactuated legged robots in environments that are semantically partially known and geometrically unknown. Parallel work, relying on the formal guarantees presented in this article, has already demonstrated how the same vector field planning principles and our semantic inference capabilities can be exploited in order to perform more complex missions with predefined logic that involve human following and pose tracking (Vasilopoulos et al., 2020), tasks such as navigation among movable obstacles (Vasilopoulos et al., 2021), using a linear temporal logic planner (Kantaros et al., 2020) and an interface layer that translates symbolic commands to point navigation tasks, or complicated mobile manipulation tasks with legged robots in unexplored 2.5D environments, using an external geometric planner to rearrange semantically tagged objects of interest (Vega-Brown et al., 2021).

Moreover, a remaining challenge is to generalize the present framework beyond the current restriction to 2D environments, in order to address the challenge of navigating unknown or partially known environments in higher dimension. Even though the currently presented algorithm would be restricted to shapes with genus zero (no holes), one could develop algorithms that “patch” the holes of shapes with non-zero genus when they are not important, affording the use of the same reactive principles for navigation. Work currently in progress investigates whether concepts from the literature on convex decomposition of polyhedra (Lien and Amato, 2007) could afford such generalization to the problem of navigating 3D workspaces with aerial drones.

Finally, we believe the methods we develop here for generating in realtime simple, topologically equivalent model spaces and pulling back the model controller through the corresponding diffeomorphism can be applied to diverse, philosophically alternative approaches to our purely reactive formulation of motion planning. For example, sampling-based (probabilistically complete) offline planners have been shown to benefit from integration with even geometrically naive locally reactive methods (Arslan et al., 2017) that can mitigate difficulties such as finding paths through narrow passages. We imagine that even greater simplification of the steering and collision-checking issues arising from sampling-based methods in partially “familiar” geometrically complicated environments (Bialkowski et al., 2012; LaValle and Kuffner, 2001) might be achieved by shifting the problem of finding a feasible path to a topologically equivalent, metrically simple abstracted model wherein planning might be significantly faster. The robot could then be tasked to follow a generated path in the abstract space (e.g., along the lines of Arslan and Koditschek (2017)) and the associated commands can be pulled back to the physical space through the diffeomorphism. Careful future inquiry will be needed to explore such deliberative–reactive hybrid uses for the online topological abstraction of familiar geometry developed here.

Footnotes

Appendix A. Index to Multimedia Extensions

Archives of IJRR multimedia extensions published prior to 2014 can be found at http://www.ijrr.org, after 2014 all videos are available on the IJRR YouTube channel at http://www.youtube.com/user/ijrrmultimedia

SAGE-Journals-Accessible-Video-Player 10.1177/02783649211048931.M1 sj-vid-1-ijr-10.1177_02783649211048931 Extension 1

Numerical studies

SAGE-Journals-Accessible-Video-Player 10.1177/02783649211048931.M2 sj-vid-2-ijr-10.1177_02783649211048931 Extension 2

Experimental results

SAGE-Journals-Accessible-Video-Player 10.1177/02783649211048931.M3 sj-vid-3-ijr-10.1177_02783649211048931 Extension 3

Navigation toward a semantic target

Appendix B. Implicit representation of obstacles with R-functions

In this work, looking ahead toward handling in a more modular fashion the general class of obstacle shapes encompassed by the star-tree methods from the traditional navigation function literature (Rimon and Koditschek, 1989, 1992), we depart from individuated homogeneous implicit function representation of our memorized catalog elements in favor of the R-function compositions (Rvachev, 1963), explored by Rimon (1990) and explicated within the field of constructive solid geometry by Shapiro (2007). We believe that this modular representation of shape will be helpful in the effort now in progress to instantiate the posited mapping oracle for obstacles with known geometry, whose triangular mesh can be identified in realtime using state-of-the-art techniques (Kar et al., 2015; Kong et al., 2017; Pavlakos et al., 2017) in order to extract implicit function representations for polygonal obstacles.

Appendix C. Construction of polygonal collars

Definitions 3, 5, and 7 provide the basic guidelines for constructing admissible polygonal collars that fit our formal results. However, there is not a unique way of performing this operation. Here, we describe the method employed in this article for a single polygon $P$ , contained in either $D_{map}^{I}$ or $B_{map}^{I}$ , whose triangulation tree $T_{P} : = (V_{P}, E_{P})$ has already been constructed, according to Section 4.1, and the corresponding centers of transformation $x_{j}^{*}$ have already been identified, according to Definitions 2, 4, and 6 for all triangles $j \in V_{P}$ . We assume that the value of the corresponding clearance $ϵ_{P}$ , according to Assumption 2, is also known.

Based on the above, the first step is to stack all triangles in $V_{P}$ in order of descending depth, as prescribed by the sequence of purging transformations. Then, for each triangle $j \in V_{P}$ in the stack, we first dilate the polygon $Q_{j}$ by $ϵ_{P}$ and take the intersection with $F_{map, j}^{I}$ . If the triangle $j$ is the root triangle of $P$ and $P \in D_{map}^{I}$ , this is enough to give an admissible polygonal collar. In any other case, we have to take the intersection of the generated dilated polygon with the half spaces $H_{1 j}$ and $H_{2 j}$ , defined as the sets

\begin{matrix} H_{1 j} : = {z | {(z - x_{j}^{*})}^{⊤} ([\begin{matrix} 0 & - 1 \\ 1 & 0 \end{matrix}] \frac{x_{j}^{*} - x_{1 j}}{∥ x_{j}^{*} - x_{1 j} ∥}) \geq 0} \\ H_{2 j} : = {z | {(z - x_{j}^{*})}^{⊤} ([\begin{matrix} 0 & - 1 \\ 1 & 0 \end{matrix}] \frac{x_{2 j} - x_{j}^{*}}{∥ x_{2 j} - x_{j}^{*} ∥}) \geq 0} \end{matrix}

i.e., the half spaces defined by hyperplanes passing through the center $x_{j}^{*}$ and vertex $x_{1 j}$ , and the center $x_{j}^{*}$ and vertex $x_{2 j}$ , respectively. The resulting polygon is guaranteed to be convex, but might intersect with triangles in $V_{P}$ that will succeed $j$ in the purging transformation. To solve that problem, we can take the difference of this polygon with all triangles that will succeed $j$ in the purging transformation (except for its parent $p (j)$ ), decompose the final resulting polygon into its convex pieces using a variant of Keil’s algorithm (Keil, 1985) (implemented in the C++ library CGAL (The CGAL Project, 2019) and in the Python package $poly_decomp$ (PolyDecomp, 2019)), and use as ${\bar{Q}}_{j}$ the convex piece that includes $Q_{j}$ , as prescribed by Definitions 3, 5, and 7. The whole procedure is shown in Algorithm 3.

Appendix D. Inductive computation of the diffeomorphism at execution time

From the description of the diffeomorphism $h^{I}$ in Section 4, we see that $h^{I}$ is constructed in multiple steps by composition. Therefore, we can compute the value of $h^{I} (x)$ at $x \in F_{map}^{I}$ inductively, by setting $h_{0}^{I} (x) = x$ and computing $h_{k}^{I} (x) = h_{k, k - 1}^{I} ° h_{k - 1}^{I} (x)$ , with $k$ spanning all triangles in $V_{P}$ for all known obstacles $P$ in both $D_{map}^{I}$ and $B_{map}^{I}$ , and $h_{k, k - 1}^{I}$ given either in (16) or (26). We can then see that, due to Lemma 4, $h_{k, k - 1}^{I}$ can be generally written in the following form

(70)

\begin{matrix} h_{k, k - 1}^{I} (x) = σ_{k, k - 1} (x) [x_{k, k - 1}^{*} + ν_{k, k - 1} (x) (x - x_{k, k - 1}^{*})] \\ + (1 - σ_{k, k - 1} (x)) x \end{matrix}

with the switch $σ_{k, k - 1}$ (see (13) and (21)), deforming factor $ν_{k, k - 1}$ (see (14), (22), and (23)) and center of the transformation $x_{k, k - 1}^{*}$ (see Definitions 2, 4, and 7) depending on the particular triangle being purged.

We can, therefore, set $D_{x} h_{0}^{I} : = I$ , compute

(71)

\begin{matrix} D_{x} h_{k, k - 1}^{I} = (ν_{k, k - 1} (x) - 1) (x - x_{k, k - 1}^{*}) \nabla σ_{k, k - 1} (x)^{⊤} \\ + σ_{k, k - 1} (x) (x - x_{k, k - 1}^{*}) \nabla ν_{k, k - 1} (x)^{⊤} \\ + [1 + σ_{k, k - 1} (x) (ν_{k, k - 1} (x) - 1)] I \end{matrix}

and use the chain rule to write

(72)

D_{x} h_{k}^{I} = (D_{x} h_{k, k - 1}^{I} ° h_{k - 1}^{I} (x)) \cdot D_{x} h_{k - 1}^{I}

Finally, because (47) requires partial derivatives of $D_{x} h^{I}$ , we can follow a similar procedure and the chain rule to compute the partial derivatives $\frac{\partial {[D_{x} h_{k}^{I}]}_{ml}}{\partial {[x]}_{n}}$ , as functions of $\frac{\partial {[D_{x} h_{k - 1}^{I}]}_{ml}}{\partial {[x]}_{n}}$ and $\frac{\partial {[D_{x} h_{k, k - 1}^{I}]}_{ml}}{\partial {[x]}_{n}} |_{h_{k - 1}^{I} (x)}$ , after initially setting all partial derivatives to zero: $\frac{\partial {[D_{x} h_{0}^{I}]}_{ml}}{\partial {[x]}_{n}} = 0$ , with the indices $m, l, n \in {1, 2}$ . Namely:

(73)

\begin{matrix} \frac{\partial {[D_{x} h_{k}^{I}]}_{ml}}{\partial {[x]}_{n}} = \sum_{r = 1}^{2} ({[D_{x} h_{k, k - 1}^{I} ° h_{k - 1}^{I} (x)]}_{mr} \cdot \\ \cdot \frac{\partial {[D_{x} h_{k - 1}^{I}]}_{rl}}{\partial {[x]}_{n}} \\ + [D_{x} h_{k - 1}^{I}]_{rl} \sum_{s = 1}^{2} [D_{x} h_{k - 1}^{I}]_{sn} \cdot \\ \cdot \frac{\partial {[D_{x} h_{k, k - 1}^{I}]}_{mr}}{\partial {[x]}_{s}} |_{h_{k - 1}^{I} (x)}) \end{matrix}

where, from (71), we can compute

(74)

\begin{matrix} \frac{\partial {[D_{x} h_{k, k - 1}^{I}]}_{mr}}{\partial {[x]}_{s}} = (ν_{k, k - 1} - 1) \frac{\partial σ_{k, k - 1}}{\partial {[x]}_{r}} δ_{ms} \\ + ([x]_{m} - [x_{k, k - 1}^{*}]_{m}) \frac{\partial σ_{k, k - 1}}{\partial {[x]}_{r}} \frac{\partial ν_{k, k - 1}}{\partial {[x]}_{s}} \\ + (ν_{k, k - 1} - 1) ([x]_{m} - [x_{k, k - 1}^{*}]_{m}) \frac{\partial^{2} σ_{k, k - 1}}{\partial {[x]}_{r} \partial {[x]}_{s}} \\ + ([x]_{m} - [x_{k, k - 1}^{*}]_{m}) \frac{\partial σ_{k, k - 1}}{\partial {[x]}_{s}} \frac{\partial ν_{k, k - 1}}{\partial {[x]}_{r}} \\ + σ_{k, k - 1} \frac{\partial ν_{k, k - 1}}{\partial {[x]}_{r}} δ_{ms} \\ + σ_{k, k - 1} ([x]_{m} - [x_{k, k - 1}^{*}]_{m}) \frac{\partial^{2} ν_{k, k - 1}}{\partial {[x]}_{r} \partial {[x]}_{s}} \\ + σ_{k, k - 1} \frac{\partial ν_{k, k - 1}}{\partial {[x]}_{s}} δ_{mr} + (ν_{k, k - 1} - 1) \frac{\partial σ_{k, k - 1}}{\partial {[x]}_{s}} δ_{mr} \end{matrix}

by using elements of the Hessians $\nabla^{2} σ_{k, k - 1}, \nabla^{2} ν_{k, k - 1}$ .

Appendix E. Proofs

Acknowledgements

The authors thank Dr. Omur Arslan for many formative discussions and for sharing his simulation and presentation infrastructure, Prof. Elon Rimon for many interesting discussions pertaining to results from the field of computational geometry, Prof. George Pappas and Sean Bowman for sharing their semantic mapping framework, and T. Turner Topping for assistance with the Minitaur hardware experiments.

Funding

The author disclosed receipt of the following financial support for research, authorship, and/or publication of this article: This work was supported in part by the AFRL (grant number FA865015D1845; subcontract 669737-1), in part by the ONR (grant number N00014-16-1-2817), and by a Vannevar Bush Fellowship held by the last author, sponsored by the Basic Research Office of the Assistant Secretary of Defense for Research and Engineering.

ORCID iDs

Vasileios Vasilopoulos

Karl Schmeckpeper

Notes

References

Amazon, Inc. (2014) Amazon Prime Air. http://www.amazon.com/b?node=8037720011 (accessed 12 September 2021).

Arslan

Koditschek

(2016a) Exact robot navigation using power diagrams. In: IEEE International Conference on Robotics and Automation, pp. 1–8.

Arslan

Koditschek

(2016b) Sensor-based reactive navigation in unknown convex sphere worlds. In: The 12th International Workshop on the Algorithmic Foundations of Robotics.

Arslan

Koditschek

(2017) Smooth extensions of feedback motion planners via reference governors. In: IEEE International Conference on Robotics and Automation, pp. 4414–4421.

Arslan

Koditschek

(2018) Sensor-based reactive navigation in unknown convex sphere worlds. The International Journal of Robotics Research 38(1–2): 196–223.

Arslan

Pacelli

Koditschek

(2017) Sensory steering for sampling-based motion planning. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3708–3715.

Astolfi

(1999) Exponential stabilization of a wheeled mobile robot via discontinuous control. Journal of Dynamic Systems, Measurement, and Control 121(1): 121–126.

Ataka

Lam

Althoefer

(2018) Reactive magnetic-field-inspired navigation for non-holonomic mobile robots in unknown environments. In: IEEE International Conference on Robotics and Automation, pp. 6983–6988.

Atanasov

Zhu

Daniilidis

Pappas

(2016) Localization from semantic observations via the matrix permanent. The International Journal of Robotics Research 35(1–3): 73–99.

10.

Bansal

Tolani

Gupta

Malik

Tomlin

(2019) Combining optimal control and learning for visual navigation in novel environments. arXiv preprint https://arxiv.org/abs/1903.02531.

11.

Baryshnikov

Shapiro

(2014) How to run a centipede: A topological perspective. In: Geometric Control Theory and Sub-Riemannian Geometry ( Springer INdAM Series, Vol. 5.). Cham: Springer, pp. 37–51.

12.

Bhattacharya

Ghrist

Kumar

(2015) Persisent homology for path planning in uncertain environments. IEEE Transactions on Robotics 31(3): 578–590.

13.

Bialkowski

Karaman

Otte

Frazzoli

(2012) Efficient collision checking in sampling-based motion planning. In: The 10th International Workshop on the Algorithmic Foundations of Robotics.

14.

Borenstein

Koren

(1989) Real-time obstacle avoidance for fast mobile robots. IEEE Transactions on Systems, Man, and Cybernetics 19(5): 1179–1187.

15.

Borenstein

Koren

(1991) The vector field histogram - fast obstacle avoidance for mobile robots. IEEE Transactions on Robotics and Automation 7(3): 278–288.

16.

Bowman

Atanasov

Daniilidis

Pappas

(2017) Probabilistic data association for semantic SLAM. In: IEEE International Conference on Robotics and Automation, pp. 1722–1729.

17.

Branicky

(1998) Multiple Lyapunov functions and other analysis tools for switched and hybrid systems. IEEE Transactions on Automatic Control 43(4): 475–482.

18.

Brock

Khatib

(1999) High-speed navigation using the global dynamic window approach. In: IEEE International Conference on Robotics and Automation, pp. 341–346.

19.

Chaney

(1990) Piecewise C^k functions in nonsmooth analysis. Nonlinear Analysis: Theory, Methods and Applications 15(7): 649–660.

20.

Chang

Marsden

(2003) Gyroscopic forces and collision avoidance with convex obstacles. In: New Trends in Nonlinear Dynamics and Control and their Applications (Lecture Notes in Control and Information Sciences, Vol. 295). Berlin: Springer, pp. 145–159.

21.

Clementini

Felice

van Oosterom

(1993) A small set of formal topological relationships suitable for end-user interaction. In: Proceedings of the 3rd International Symposium on Advances in Spatial Databases, pp. 277–295.

22.

Conner

Choset

Rizzi

(2011) Integrating planning and control for single-bodied wheeled mobile robots. Autonomous Robots 30(3): 243–264.

23.

Dellaert

(2012) Factor graphs and GTSAM: A hands-on introduction. Technical Report GT-RIM-CP&R-2012-002. Available at: https://research.cc.gatech.edu/borg/sites/edu.borg/files/downloads/gtsam.pd (accessed 12 September 2021).

24.

Douglas

Peucker

(1973) Algorithms for the reduction of the number of points required to represent a digitized line or its caricature. Cartographica: The International Journal for Geographic Information and Geovisualization 10: 112–122.

25.

Egenhofer

Herring

(1991) Categorizing binary topological relations between regions, lines, and points in geographic databases. International Series of Monographs on Computer Science.

26.

ElGindy

Everett

Toussaint

(1993) Slicing an ear using prune-and-search. Pattern Recognition Letters 14(9): 719–722.

27.

Farber

(2006) Topology of robot motion planning. In: Biran

Cornea

Lalonde

(eds) Morse Theoretic Methods in Nonlinear Analysis and in Symplectic Topology (NATO Science Series II: Mathematics, Physics and Chemistry, Vol. 217). Dordrecht: Springer, pp. 185–230.

28.

Farber

Grant

Lupton

Oprea

(2019) An upper bound for topological complexity. Topology and its Applications 255: 109–125.

29.

Filippidis

Kyriakopoulos

(2012) Navigation functions for everywhere partially sufficiently curved worlds. In: IEEE International Conference on Robotics and Automation, pp. 2115–2120.

30.

Fiorini

Shiller

(1998) Motion planning in dynamic environments using velocity obstacles. The International Journal of Robotics Research 17(7): 760–772.

31.

Full

Koditschek

(1999) Templates and anchors: Neuromechanical hypotheses of legged locomotion on land. Journal of Experimental Biology 202(23): 3325–3332.

32.

Gao

Shen

(2019) Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments. Journal of Field Robotics 36(4): 710–733.

33.

Garrett

Lozano-Perez

Kaelbling

(2018) Sampling-based methods for factored task and motion planning. The International Journal of Robotics Research 37(13–14): 1796–1825.

34.

Ghost Robotics (2016) Ghost Robotics Minitaur. https://www.ghostrobotics.io/about (accessed 21 September 2021).

35.

Gupta

Davidson

Levine

Sukthankar

Malik

(2017) Cognitive mapping and planning for visual navigation. In: IEEE Conference on Computer Vision and Pattern Recognition, pp. 7272–7281.

36.

Hartley

(1997) Triangulation. Computer Vision and Image Understanding 68(2): 146–157.

37.

Henry

Vollmer

Ferris

Fox

(2010) Learning to navigate through crowded environments. In: IEEE International Conference on Robotics and Automation, pp. 981–986.

38.

Hesch

Kottas

Bowman

Roumeliotis

(2014) Consistency analysis and improvement of vision-aided inertial navigation. IEEE Transactions on Robotics 30(1): 158–176.

39.

Hirsch

(1976) Differential Topology. Berlin: Springer.

40.

Hokuyo (2019) Scanning Rangefinder UTM-30LX. https://www.hokuyo-aut.jp/search/single.php?serial=169 (accessed 12 September 2021).

41.

Ilhan

Johnson

Koditschek

(2018) Autonomous legged hill ascent. Journal of Field Robotics 35(5): 802–832.

42.

Jayakumar

Madhav

Savelli

Blair

Cowan

Knierim

(2019) Recalibration of path integration in hippocampal place cells. Nature 566(7745): 533–537.

43.

Johnson

Burden

Koditschek

(2016) A hybrid systems model for simple manipulation and self-manipulation systems. The International Journal of Robotics Research 35(11): 1354–1392.

44.

Johnson

Hale

Haynes

Koditschek

(2011) Autonomous legged hill and stairwell ascent. In: IEEE International Symposium on Safety, Security, and Rescue Robotics, pp. 134–142.

45.

Kaess

Johannsson

Roberts

Ila

Leonard

Dellaert

(2012) ISAM2: Incremental smoothing and mapping using the Bayes tree. The International Journal of Robotics Research 31(2): 216–235.

46.

Kantaros

Malencia

Kumar

Pappas

(2020) Reactive temporal logic planning for multiple robots in unknown environments. In: IEEE International Conference on Robotics and Automation, pp. 11479–11485.

47.

Kar

Tulsiani

Carreira

Malik

(2015) Category-specific object reconstruction from a single image. In: IEEE International Conference on Computer Vision and Pattern Recognition, pp. 1966–1974.

48.

Karaman

Frazzoli

(2012) High-speed flight in an ergodic forest. In: IEEE International Conference on Robotics and Automation, pp. 2899–2906.

49.

Kavraki

Svestka

Latombe

Overmars

(1996) Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation 12(4): 566–580.

50.

Keil

(1985) Decomposing a polygon into simpler components. SIAM Journal on Computing 14(4): 799–817.

51.

Khatib

(1986) Real-time obstacle avoidance for manipulators and mobile robots. The International Journal of Robotics Research 5(1): 90–98.

52.

Kong

Lin

Lucey

(2017) Using locally corresponding CAD models for dense 3D reconstructions from a single image. In: IEEE International Conference on Computer Vision and Pattern Recognition, pp. 5603–5611.

53.

Kumar

Gupta

Fouhey

Levine

Malik

(2018) Visual memory for robust path following. Advances in Neural Information Processing Systems 31: 765–774.

54.

Kuntz

Scholtes

(1994) Structural analysis of nonsmooth mappings, inverse functions, and metric projections. Journal of Mathematical Analysis and Applications 188(2): 346–386.

55.

LaValle

Kuffner

(2001) Randomized kinodynamic planning. The International Journal of Robotics Research 20(5): 378–400.

56.

Lien

Amato

(2007) Approximate convex decomposition of polyhedra. In: ACM Symposium on Solid and Physical Modeling. New York: ACM Press, pp. 121–131.

57.

linuxlewis (2019) TriPy: Simple polygon triangulation algorithms in pure Python. https://github.com/linuxlewis/tripy.

58.

Lionis

Papageorgiou

Kyriakopoulos

(2008) Towards locally computable polynomial navigation functions for convex obstacle workspaces. In: IEEE International Conference on Robotics and Automation, pp. 3725–3730.

59.

Majumdar

Tedrake

(2017) Funnel libraries for real-time robust feedback motion planning. The International Journal of Robotics Research 36(8): 947–982.

60.

Massey

(1992) Sufficient conditions for a local homeomorphism to be injective. Topology and its Applications 47: 133–148.

61.

Meisters

(1975) Polygons have ears. American Mathematical Monthly 82: 648–651.

62.

Mohta

Watterson

Mulgaonkar

, et al. (2018) Fast, autonomous flight in GPS-denied and cluttered environments. Journal of Field Robotics 35(1): 101–120.

63.

Mueller

Birk

(2018) Conceptualization of object compositions using persistent homology. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 1095–1102.

64.

Newell

Yang

Deng

(2016) Stacked hourglass networks for human pose estimation. In: European Conference on Computer Vision. Berlin: Springer, pp. 483–499.

65.

Noreen

Khan

Habib

(2016) Optimal path planning using RRT* based approaches: A survey and future directions. International Journal of Advanced Computer Science and Applications 7(11): 97–107.

66.

NVIDIA (2019) Jetson TX2 Module. https://developer.nvidia.com/embedded/jetson-tx2 (accessed 12 September 2021).

67.

O’Rourke

(1987) In: Art Gallery Theorems and Algorithms (International Series of Monographs on Computer Science). Oxford: Oxford University Press.

68.

Otte

Frazzoli

(2015) RRT^X: Asymptotically optimal single-query sampling-based motion planning with quick replanning. The International Journal of Robotics Research 35(7): 797–822.

69.

Paranjape

Meier

Shi

Chung

Hutchinson

(2015) Motion primitives and 3D path planning for fast flight through a forest. The International Journal of Robotics Research 34(3): 357–377.

70.

Paternain

Koditschek

Ribeiro

(2017) Navigation functions for convex potentials in a space with convex obstacles. IEEE Transactions on Automatic Control 63(9): 2944–2959.

71.

Pavlakos

Zhou

Chan

Derpanis

Daniilidis

(2017) 6-DoF object pose from semantic keypoints. In: IEEE International Conference on Robotics and Automation, pp. 2011–2018.

72.

Pokorny

Kragic

(2015) Data-driven topological motion planning with persistent cohomology. In: Robotics: Science and Systems.

73.

PolyDecomp (2019) poly_decomp: Decompose 2D polygons into convex pieces. https://github.com/wsilva32/poly_decomp.py (accessed 12 September 2021).

74.

Quigley

Conley

Gerkey

, et al. (2009) ROS: An open-source robot operating system. In: ICRA Workshop on Open Source Software.

75.

Redmon

Farhadi

(2018) YOLOv3: An incremental improvement. arXiv Preprint arXiv:1804.02767.

76.

Reverdy

Ilhan

Koditschek

(2015) A drift-diffusion model for robotic obstacle avoidance. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 6113–6120.

77.

Revzen

Ilhan

Koditschek

(2012) Dynamical trajectory replanning for unknown environments. In: IEEE Conference on Decision and Control, pp. 3476–3483.

78.

Rimon

(1990) Exact Robot Navigation Using Artificial Potential Functions. PhD Thesis, Yale University.

79.

Rimon

Koditschek

(1989) The construction of analytic diffeomorphisms for exact robot navigation on Star Worlds. Transactions of the American Mathematical Society 327(1): 71–116.

80.

Rimon

Koditschek

(1992) Exact robot navigation using artificial potential functions. IEEE Transactions on Robotics and Automation 8(5): 501–518.

81.

Russell

Norvig

(2009) Artificial Intelligence: A Modern Approach. Englewood Cliffs, NJ: Prentice Hall.

82.

Rvachev

(1963) An analytic description of certain geometric objects. Doklady Akademii Nauk 153: 765–767.

83.

Schling

(2011) The Boost C++ Libraries. XML Press.

84.

Shapely (2019) Shapely: Manipulation and analysis of geometric objects. https://github.com/Toblerity/Shapely (accessed 12 September 2021).

85.

Shapiro

(1988) Sensitivity analysis of nonlinear programs and differentiability properties of metric projections. SIAM Journal on Control and Optimization 26(3): 628–645.

86.

Shapiro

(2007) Semi-analytic geometry with R-functions. Acta Numerica 16: 239–303.

87.

Simmons

(1996) The curvature-velocity method for local obstacle avoidance. In: IEEE International Conference on Robotics and Automation, Vol. 4, pp. 3375–3382.

88.

StereoLabs (2019a) Positional tracking with the ZED Mini stereo camera. https://www.stereolabs.com/docs/positional-tracking/ (accessed 12 September 2021).

89.

StereoLabs (2019b) ZED Mini stereo camera. https://www.stereolabs.com/zed-mini/ (accessed 12 September 2021).

90.

Tang

Kumar

(2018) Autonomous flight. Annual Review of Control, Robotics and Autonomous Systems 1(1): 29–52.

91.

The CGAL Project (2019) CGAL User and Reference Manual. 4.14 edition. CGAL Editorial Board. https://doc.cgal.org/4.14/Manual/packages.html (accessed 12 September 2021).

92.

Topping

Vasilopoulos

Koditschek

(2019) Composition of templates for transitional pedipulation behaviors. In: The International Symposium on Robotics Research.

93.

Trautman

Murray

Krause

(2015) Robot navigation in dense human crowds: Statistical models and experimental studies of human–-robot cooperation. The International Journal of Robotics Research 34(3): 335–356.

94.

TurtleBot2 (2019) TurtleBot2: Open-source robot development kit for apps on wheels. https://www.turtlebot.com/turtlebot2/.

95.

van den Berg

Guy

Lin

Manocha

(2011) Reciprocal n-Body Collision Avoidance. Berlin: Springer, pp. 3–19.

96.

van den Berg

Lin

Manocha

(2008) Reciprocal velocity obstacles for real-time multi-agent navigation. In: IEEE International Conference on Robotics and Automation, pp. 1928–1935.

97.

Vasilopoulos

Arslan

Koditschek

(2017) Sensor-based legged robot homing using range-only target localization. In: IEEE International Conference on Robotics and Biomimetics, pp. 2630–2637.

98.

Vasilopoulos

Kantaros

Pappas

Koditschek

(2021) Reactive planning for mobile manipulation tasks in unexplored semantic environments. In: IEEE International Conference on Robotics and Automation.

99.

Vasilopoulos

Koditschek

(2018) Reactive navigation in partially known non-convex environments. In: 13th International Workshop on the Algorithmic Foundations of Robotics (WAFR).

100.

Vasilopoulos

Pavlakos

Bowman

, et al. (2020) Reactive semantic planning in unexplored semantic environments using deep perceptual feedback. IEEE Robotics and Automation Letters 5(3): 4455–4462.

101.

Vasilopoulos

Topping

Vega-Brown

Roy

Koditschek

(2018) Sensor-based reactive execution of symbolic rearrangement plans by a legged mobile manipulator. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3298–3305.

102.

Vega-Brown

Roy

(2018) Admissible abstractions for near-optimal task and motion planning. In: International Joint Conference on Artificial Intelligence.

103.

Vega-Brown

Vasilopoulos

Castro

Koditschek

Roy

(2021) An hierarchical deliberative–reactive architecture for task and motion planning. Mobile Robot Laboratory Publications, Georgia Tech. Available at: https://smartech.gatech.edu/handle/1853/22224 (accessed 12 September 2021).

104.

Vlantis

Vrohidis

Bechlioulis

Kyriakopoulos

(2018) Robot navigation in complex workspaces using harmonic maps. In: IEEE International Conference on Robotics and Automation, pp. 1726–1731.

105.

Wooden

Malchano

Blankespoor

Howardy

Rizzi

Raibert

(2010) Autonomous navigation for BigDog. In: IEEE International Conference on Robotics and Automation, pp. 4736–4741.

Reactive navigation in partially familiar planar environments using semantic perceptual feedback

Abstract

Keywords

1. Introduction

1.1. Motivation and prior work

1.1.1. Reactive navigation

1.1.2. Realtime perception

1.1.3. Topologically informed navigation

1.2. Summary of contributions

1.3. Organization of the article

2. Problem formulation

3. Navigational representation of the environment

3.1. Physical space

3.2. Semantic space

3.3. Mapped space

3.4. Model space

3.5. Implicit representation of obstacles

4. The diffeomorphism between the mapped and model spaces

4.1. Obstacle representation

4.2. Intermediate spaces related by leaf purging transformations

4.2.1. Center of the transformation and surrounding polygonal collars

4.2.2. Description of the C ∞ switches

4.2.3. Description of the deforming factors

4.2.4. The map between F map , j i I and F map , p ( j i ) I

4.2.5. Qualitative properties of the map between F map , j i I and F map , p ( j i ) I

4.2.6. Composition of leaf purging transformations

4.3. Purging of root triangles

4.3.1. Center of the transformation and surrounding polygonal collars for obstacles in D map I

4.3.2. Description of the C ∞ switches for obstacles in D map I

4.3.3. Description of the deforming factors for obstacles in D map I

4.3.4. Center of the transformation and surrounding polygonal collars for obstacles in B map I

4.3.5. Description of the C ∞ switches for obstacles in B map I

4.3.6. Description of the deforming factors for obstacles in B map I

4.3.7. The map between F ^ map I and F model I

4.3.8. Qualitative properties of the map between F ^ map I and F model I

4.4. The map between the mapped space and the model space

5. Reactive controller

5.1. Hybrid systems description of navigation framework

5.1.1. Fully actuated robots

5.1.2. Differential drive robots

5.2. Reactive controller in each hybrid mode

5.2.1. Fully actuated robots

5.2.2. Differential drive robots

5.3. Qualitative properties of the hybrid controller

5.3.1. Fully actuated robots

5.3.2. Differential drive robots

5.4. Generating bounded inputs

5.4.1. Fully actuated robots

5.4.2. Differential drive robots

6. Online reactive planning algorithms

6.1. Mapped space recovery

6.2. Reactive planning component

7. Numerical results

7.1. Comparison with original doubly reactive algorithm

7.2. Navigation in a cluttered environment with obstacle merging

7.3. Navigation among mixed known and unknown obstacles

8. Experimental setup

8.1. Object detection and keypoint localization

8.1.1. Object detection

8.1.2. Keypoint localization

8.1.3. Training details

8.2. Semantic mapping

9. Experimental results

9.1. Experiments with Turtlebot and offboard perception

9.1.1. Comparison with the original doubly reactive algorithm

9.1.2. Navigation in a cluttered environment with obstacle merging

9.1.3. Navigation among mixed known and unknown obstacles

9.2. Experiments with Turtlebot and onboard perception

9.3. Experiments with Minitaur

10. Conclusion and future work

10.1. Conclusion

10.2. Future work

Footnotes

Appendix A. Index to Multimedia Extensions

Appendix B. Implicit representation of obstacles with R-functions

Appendix C. Construction of polygonal collars

Appendix D. Inductive computation of the diffeomorphism at execution time

Appendix E. Proofs

Acknowledgements

Funding

4.2.2. Description of the $C^{\infty}$ switches

4.2.4. The map between $F_{map, j_{i}}^{I}$ and $F_{map, p (j_{i})}^{I}$

4.2.5. Qualitative properties of the map between $F_{map, j_{i}}^{I}$ and $F_{map, p (j_{i})}^{I}$

4.3.1. Center of the transformation and surrounding polygonal collars for obstacles in $D_{map}^{I}$

4.3.2. Description of the $C^{\infty}$ switches for obstacles in $D_{map}^{I}$

4.3.3. Description of the deforming factors for obstacles in $D_{map}^{I}$

4.3.4. Center of the transformation and surrounding polygonal collars for obstacles in $B_{map}^{I}$

4.3.5. Description of the $C^{\infty}$ switches for obstacles in $B_{map}^{I}$

4.3.6. Description of the deforming factors for obstacles in $B_{map}^{I}$

4.3.7. The map between ${\hat{F}}_{map}^{I}$ and $F_{model}^{I}$

4.3.8. Qualitative properties of the map between ${\hat{F}}_{map}^{I}$ and $F_{model}^{I}$