Technical Report: Reactive Navigation in Partially Known Non-Convex Environments

07/23/2018 ∙ by Vasileios Vasilopoulos, et al. ∙ University of Pennsylvania 0

This paper presents a provably correct method for robot navigation in 2D environments cluttered with familiar but unexpected non-convex, star-shaped obstacles as well as completely unknown, convex obstacles. We presuppose a limited range onboard sensor, capable of recognizing, localizing and (leveraging ideas from constructive solid geometry) generating online from its catalogue of the familiar, non-convex shapes an implicit representation of each one. These representations underlie an online change of coordinates to a completely convex model planning space wherein a previously developed online construction yields a provably correct reactive controller that is pulled back to the physically sensed representation to generate the actual robot commands. We extend the construction to differential drive robots, and suggest the empirical utility of the proposed control architecture using both formal proofs and numerical simulations.

READ FULL TEXT VIEW PDF
POST COMMENT

Comments

There are no comments yet.

Authors

page 13

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

1 Introduction

1.1 Motivation and Prior Work

Recent advances in the theory of sensor-based reactive navigation [2] and its application to wheeled [3] and legged [35] robots promote its central role in provably correct architectures for increasingly complicated mobile manipulation tasks [36, 37]. The advance of the new theory [2] over prior sensor-based collision avoidance schemes [23, 16, 33, 12, 6, 5, 10, 8, 7] was the additional guaranteed convergence to a designated goal which had theretofore only been established for reactive planners possessing substantial prior knowledge about the environment [21, 28]. A key feature of these new (and other recent parallel [24, 15]) approaches is that they trade away prior knowledge for the presumption of simplicity: unknown obstacles can be successfully negotiated in real time without losing global convergence guarantees if they are “round” (i.e., very strongly convex in a sense made precise in [3]). The likely necessity of such simple geometry for guaranteed safe convergence by a completely uninformed “greedy” reactive navigation planner is suggested by the result that a collision avoiding, distance-diminishing reactive navigation policy can reach arbitrarily placed goals in an unknown freespace only if all obstacles are “round” [3, Proposition 14].

This paper offers a step toward elucidating the manner in which partial knowledge may suffice to inform safe, convergent, reactive navigation in geometrically more interesting environments. Growing experience negotiating learned [13]

or estimated

[18, 34] environments suggests that reasonable statistical priors may go a long way toward provable stochastic navigation. But in this work we are interested in what sort of deterministic guarantees may be possible. Recent developments in semantic SLAM [9] and object pose and triangular mesh extraction using convolutional neural net architectures [17, 19, 25]

now provide an avenue for incorporating partial prior knowledge within a deterministic framework well suited to the vector field planning methods reviewed above.

1.2 Contributions and Organization of the Paper

We consider the navigation problem in a 2D workspace cluttered with unknown convex obstacles, along with “familiar” non-convex, star-shaped obstacles [27] that belong to classes of known geometries, but whose number and placement are unknown, awaiting discovery at execution time. We assume a limited range onboard sensor, a sufficient margin separating all obstacles from each other and the goal, and a catalogue of known star-shaped sets, along with a “mapping oracle” for their online identification and localization in the physical workspace. These ingredients suggest a representation of the environment taking the form of a “multi-layer” triple of topological spaces whose realtime interaction can be exploited to integrate the geometrically naive sensor driven methods of [2] with the offline memorized geometry sensitive methods of [28]. Specifically, we adapt the construction of [27] to generate a realtime smooth change of coordinates (a diffeomorphism) of the mapped layer of the environment into a (locally) topologically equivalent but geometrically more favorable model layer relative to which the reactive methods of [2] can be directly applied. We prove that the conjugate vector field defined by pulling back the reactive model space planner 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. 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 [2] 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 [2]. Finally, to better support online implementation of these constructions, we adopt modular methods for implicit description of geometric shape [32].

The paper is organized as follows. Section 2 describes the problem and establishes our assumptions. Section 3 describes the physical, mapped and model planning layers used in the constructed diffeomorphism between the mapped and model layers, whose properties are established next. Based on these results, Section 4 describes our control approach both for fully actuated and differential drive robots. Section 5 presents a variety of illustrative numerical studies and Section 6 concludes by summarizing our findings and presenting ideas for future work. Finally, Appendix 0.A includes the proofs of our main results, Appendix 0.B sketches the ideas from computational geometry [32] underlying our modular construction of implicit representations of polygonal obstacles, and Appendix 0.C includes some technical details on the calculation of the diffeomorphism jacobian for differential drive robots.

2 Problem Formulation

We consider a disk-shaped robot with radius , centered at , navigating a closed, compact workspace , with known convex boundary . The robot is assumed to possess a sensor with fixed range , capable of recognizing “familiar” objects, as well as estimating the distance of the robot to nearby obstacles111We refer the reader to an example of existing technology [1] generating 2D LIDAR scans from 3D point clouds for such an approach..

The workspace is cluttered by an unknown number of fixed, disjoint obstacles, denoted by . We adopt the notation in [2] and define the freespace as

(1)

where is the open ball centered at with radius , and denotes its closure. To simplify our notation, we neglect the robot dimensions, by dilating each obstacle in by , and assume that the robot operates in . We denote the set of dilated obstacles by .

Although none of the positions of any obstacles in are à-priori known, a subset of these obstacles is assumed to be “familiar” in the sense of having an à-priori known, readily recognizable star-shaped geometry [27] (i.e., belonging to a known catalogue of star-shaped geometry classes), which the robot can efficiently identify and localize instantaneously from online sensory measurement. Although the implementation of such a sensory apparatus lies well beyond the scope of the present paper, recent work on semantic SLAM [9] provides an excellent example with empirically demonstrated technology for achieving this need for localizing, identifying and keeping track of all the familiar obstacles encountered in the otherwise unknown environment. The à-priori unknown center of each catalogued star-shaped obstacle is denoted . Similarly to [28], each star-shaped obstacle can be described by an obstacle function, a real-valued map providing an implicit representation of the form

(2)

which the robot must construct online from the catalogued geometry, after it has localized . The remaining obstacles are 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 [2, Assumption 2].

For the obstacle functions, we require the technical assumptions introduced in [28, Appendix III], outlined as follows. The obstacle functions satisfy the following requirements

  1. For each , there exists such that for any two obstacles

    (3)

    i.e., the “thickened boundaries” of any two stars still do not overlap.

  2. For each , there exists such that the set does not contain the goal and does not intersect with any other obstacle in .

  3. For each obstacle function , there exists a pair of positive constants satisfying the inner product condition222A brief discussion on this condition is given in Appendix 0.B.

    (4)

    for all such that .

For each obstacle , we then define . Finally, we will assume that the range of the sensor satisfies for all .

Based on these assumptions and further positing first-order, fully-actuated robot dynamics , the problem consists of finding a Lipschitz continuous controller , that leaves the freespace positively invariant and asymptotically steers almost all configurations in to the given goal .

3 Multi-layer Representation of the Environment and Its Associated Transformations

In this Section, we introduce associated notation for, and transformations between three distinct representations of the environment that we will refer to as planning “layers” and use in the construction of our algorithm. Fig. 1 illustrates the role of these layers and the transformations that relate them in constructing and analyzing a realtime generated vector field that guarantees safe passage to the goal. The new technical contribution is an adaptation of the methods of [28] to the construction of a diffeomorphism, , where the requirement for fast, online performance demands an algorithm that is as simple as possible and with few tunable parameters. Hence, since the reactive controller in [2] 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 to resolve to the identity transformation in the neighborhood of “unfamiliar” objects) and the control response defaults to that prior construction.

Figure 1: Snapshot Illustration of Key Ideas. The robot in the physical layer (left frame, depicting in blue the robot’s placement in the workspace along with the prior trajectory of its centroid) containing both familiar objects of known geometry but unknown location (dark grey) and unknown obstacles (light grey), moves towards a goal and discovers obstacles (black) with an onboard sensor of limited range (orange disk). These obstacles are localized and stored permanently in the mapped layer (middle frame, depicting in blue the robot’s placement as a point in freespace rather than its body in the workspace) if they have familiar geometry or temporarily, with just the corresponding sensed fragments, if they are unknown. An online map is then constructed (Section 3), from the mapped layer to a geometrically simple model layer (right frame, now depicting the robot’s placement and prior tractory amongst the -deformed convex images of the mapped obstacles). A doubly reactive control scheme for convex environments [2] defines a vector field on the model layer which is pulled back in realtime through the diffeomorphism to generate the input in the physical layer (Section 4).

3.1 Description of Planning Layers

3.1.1 Physical Layer

The physical layer 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 actual workspace , punctured with the obstacles . This gives rise to the freespace , given in (1), consisting of all placements of the robot’s centroid that entail no intersections of its body with any obstacles. The robot navigates this layer, and discovers and localizes new obstacles, which are then stored in its semantic map if their geometry is familiar.

3.1.2 Mapped Layer

The mapped layer has the same boundary as (i.e. ) and records the robot’s evolving information about the environment aggregated from the raw sensor data about the observable portions of unrecognized (and therefore, presumed convex) obstacles , together with the inferred star centers and obstacle functions of star-shaped obstacles

, that are instantiated at the moment the sensory data triggers the “memory” that identifies and localizes a familiar obstacle. It is important to note that the star environment is constantly updated, both by discovering and storing new star-shaped obstacles in the semantic map and by discarding old information and storing new information regarding obstacles in

. In this representation, the robot is treated as a point particle, since all obstacles are dilated by in the passage from the workspace to the freespace representation of valid placements.

3.1.3 Model Layer

The model layer has the same boundary as (i.e. ) and consists of a collection of Euclidean disks, each centered at one of the mapped star centers, , and copies of the sensed fragments of the unrecognized visible convex obstacles in . The radii of the disks are chosen so that , as in [28].

This metric convex sphere world comprises the data generating the doubly reactive algorithm of [2], which will be applied to the physical robot via the online generated change of coordinates between the mapped layer and the model layer to be now constructed.

3.2 Description of the Switches

In order to simplify the diffeomorphism construction, we depart from the construction of analytic switches [27] and rely instead on the function [14] described by

(5)

with derivative

(6)

Based on that function, we can then define the switches for each star-shaped obstacle in the semantic map as

(7)

with and given according to Assumption 2. The gradient of the switch is given by

(8)

Finally, we define

(9)

Using the above construction, it is easy to see that on the boundary of the -th obstacle and when for each . Based on Assumption 2 and the choice of for each , we are, therefore, led to the following results.

Lemma 1.

At any point , at most one of the switches can be nonzero.

Corollary 1.

The set defines a partition of unity over .

3.3 Description of the Star Deforming Factors

The deforming factors are the functions , responsible for transforming each star-shaped obstacle into a disk in . Once again, we use here a slightly different construction than [27], in that the value of each deforming factor at a point does not depend on the value of . Namely, the deforming factors are given based on the desired final radii as

(10)

We also get

(11)

3.4 The Map Between the Mapped and the Model Layer

3.4.1 Construction

The map for star-shaped obstacles centered at is described by a function given by

(12)

Note that the visible convex obstacles are not considered in the construction of the map. Since the reactive controller used in the model space can handle convex obstacles and there is enough separation between convex and star-shaped obstacles according to Assumption 2-(b), we can “transfer” the geometry of those obstacles directly in the model space using the identity transformation.

Finally, note that Assumption 2-(b) implies that , since the target location is assumed to be sufficiently far from all star-shaped obstacles.

Based on the construction of the map , the jacobian at any point is given by

(13)

3.4.2 Qualitative Properties of the Map

We first verify that the construction is a smooth change of coordinates between the mapped and the model layers.

Lemma 2.

The map from to is smooth.

Proof.

Included in Appendix 0.A.1. ∎

Proposition 1.

The map is a diffeomorphism between and .

Proof.

Included in Appendix 0.A.1. ∎

3.4.3 Implicit representation of obstacles

To implement the diffeomorphism between and , shown in (12), we rely on the existence of a smooth obstacle function for each star-shaped obstacle stored in the semantic map. Since recently developed technology [25, 17, 19] provides means of performing obstacle identification in the form of triangular meshes, in this work we focus on polygonal obstacles on the plane and derive implicit representations using so called “R-functions” from the constructive solid geometry literature [32]. In Appendix 0.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. For the construction, we assume that the sensor has already identified, localized and included each discovered star-shaped obstacle in ; i.e., it has determined its pose in , given as a rotation of its vertices on the plane followed by a translation of its center , and that the corresponding polygon has already been dilated by for inclusion in .

4 Reactive Controller

4.1 Reactive Controller for Fully Actuated Robots

4.1.1 Construction

First, we consider a fully actuated particle with state , whose dynamics are described by

(14)

The dynamics of the fully actuated particle in with state are described by with the control given in [2] as333Here denotes the metric projection of on a convex set .

(15)

Here, the convex local freespace for , , is defined as in [2, Eqn. (30)]. Using the diffeomorphism construction in (12) and its jacobian in (13), we construct our controller as the vector field given by

(16)

4.1.2 Qualitative Properties

First of all, if the range of the virtual LIDAR sensor used to construct in the model layer is smaller than , the vector field is Lipschitz continuous since is shown to be Lipschitz continuous in [2] and is a smooth change of coordinates. We are led to the following result.

Corollary 2.

The vector field generates a unique continuously differentiable partial flow.

To ensure completeness (i.e. absence of finite time escape through boundaries in ) we must verify that the robot never collides with any obstacle in the environment, i.e., leaves its freespace positively invariant.

Proposition 2.

The freespace is positively invariant under the law (16).

Proof.

Included in Appendix 0.A.2. ∎

Lemma 3.
  1. The set of stationary points of control law (16) is given as , where444Here denotes the distance between two sets .

    (17a)
    (17b)

    with spanning the star-shaped obstacles in and spanning the convex obstacles in .

  2. The goal is the only locally stable equilibrium of control law (16) and all the other stationary points , each associated with an obstacle, are nondegenerate saddles.

Proof.

Included in Appendix 0.A.2. ∎

Proposition 3.

The goal location is an asymptotically stable equilibrium of (16), whose region of attraction includes the freespace excepting a set of measure zero.

Proof.

Included in Appendix 0.A.2. ∎

We can now immediately conclude the following central summary statement.

Theorem 4.1.

The reactive controller in (16) leaves the freespace positively invariant, and its unique continuously differentiable flow, starting at almost any robot placement , asymptotically reaches the goal location , while strictly decreasing along the way.

4.2 Reactive Controller for Differential Drive Robots

In this Section, we extend our reactive controller to the case of a differential drive robot, whose state is , and its dynamics are given by555We use the ordered set notation and the matrix notation for vectors interchangeably.

(18)

with and with and the linear and angular input respectively. We will follow a similar procedure to the fully actuated case; we begin by describing a smooth diffeomorphism and then we establish the results about the controller.

4.2.1 Construction and Properties of the Diffeomorphism

We construct our map from to as

(19)

with , and

(20)

Here, and

(21)

with denoting the projection onto the first two components. The reason for choosing as in (20) will become evident in the next paragraph, in our effort to control the equivalent differential drive robot dynamics in .

Proposition 4.

The map in (19) is a diffeomorphism from to .

Proof.

Included in Appendix 0.A.2. ∎

4.2.2 Construction of the Reactive Controller

Using (19), we can find the pushforward of the differential drive robot dynamics in (18) as

(22)

Based on the above, we can then write

(23)

with , and the inputs related to through

(24)
(25)

with . The calculation of can be tedious, since it involves derivatives of elements of , and is included in Appendix 0.C.

Hence, we have found equivalent differential drive robot dynamics, defined on . The idea now is to use the control strategy in [2] for the dynamical system in (23) to find reference inputs , and then use (24), (25) to find the actual inputs that achieve those reference inputs as

(26a)
(26b)

Namely, our reference inputs and inspired by [2, 4] are given as666In (19), we construct a diffeomorphism between and . However, for practical purposes, we deal only with one specific chart of in our control structure, described by the angles . As shown in [4], the discontinuity at does not induce a discontinuity in our controller due to the use of the atan function in (27b). On the contrary, with the use of (27b) as in [4, 2], the robot never changes heading in , which implies that the generated trajectories both in and (by the properties of the diffeomorphism ) in have no cusps, even though the robot might change heading in because of the more complicated nature of the function in (20).

(27a)
(27b)

with a fixed gain, the convex polygon defining the local freespace at , and and the lines defined in [2] as

(28)
(29)

4.2.3 Qualitative Properties

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

Theorem 4.2.

The reactive controller for differential drive robots, given in (26), leaves the freespace positively invariant, and its unique continuously differentiable flow, starting at almost any robot configuration , asymptotically steers the robot to the goal location , without increasing along the way.

Proof.

Included in Appendix 0.A.2. ∎

5 Numerical Experiments

In this Section, we present numerical experiments that verify our formal results. All simulations were run in MATLAB using ode45, with control gain and for the R-function construction. The reader is also referred to our video attachment for a visualization of the examples presented here and more numerical simulations.

Figure 2: Navigation around a U-shaped obstacle: 1) Fully actuated particle: (a) Original doubly reactive algorithm [2], (b) Our algorithm, 2) Differential drive robot: (a) Original doubly reactive algorithm [2], (b) Our algorithm.
Figure 3: Navigation in a cluttered environment with U-shaped obstacles. Top - Trajectories in the physical, mapped and model layers from a particular initial condition. Bottom - Convergence to the goal from several initial conditions: left - fully actuated robot, right - differential drive robot.
Figure 4: Navigating a room cluttered with known star-shaped and unknown convex obstacles. Top - Trajectories in the physical, mapped and model layers from a particular initial condition. Bottom - Convergence to the goal from several initial conditions: left - fully actuated robot, right - differential drive robot. Mapped obstacles are shown in black, known obstacles in dark grey and unknown obstacles in light grey.

5.1 Comparison with Original Doubly Reactive Algorithm

We begin with a comparison of our algorithm performance with the standalone version of the doubly reactive algorithm in [2], that we use in our construction. Fig. 2 demonstrates the basic limitation of this algorithm; in the presence of a non-convex obstacle or a flat surface, whose curvature violates [2, Assumption 2], the robot gets stuck in undesired local minima. On the contrary, our algorithm is capable of overcoming this limitation, on the premise that the robot can recognize the obstacle with star-shaped geometry at hand. The robot radius is m and the value of used for the obstacle is .

5.2 Navigation in a Cluttered Non-Convex Environment

In the next set of numerical experiments, we evaluate the performance of our algorithm in a cluttered environment, packed with instances of the same U-shaped obstacle, with star-shaped geometry, we use in Fig. 2. Both the fully actuated and the differential drive robot are capable of converging to the desired goal from a variety of initial conditions, as shown in Fig. 3. In the same figure, we also focus on a particular initial condition and include the trajectories observed in the physical, mapped and model layers. The robot radius is m and value of used for all the star-shaped obstacles in the environment is .

5.3 Navigation Among Mixed Star-Shaped and Convex Obstacles

Finally, we report experiments in an environment cluttered with both star-shaped obstacles (with known geometry) and unknown convex obstacles. We consider a robot of radius m navigating a room towards a goal. The robot can recognize familiar star-shaped obstacles (e.g., the couch, tables, armchair, chairs) but is unaware of several other convex obstacles in the environment. Fig. 4 summarizes our results for several initial conditions. We also include trajectories observed in the physical, mapped and model layers during a single run. The value of used for all the star-shaped obstacles in the environment is .

6 Conclusion and Future Work

In this paper, we present a provably correct method for robot navigation in 2D environments cluttered with familiar but unexpected non-convex, star-shaped obstacles as well as completely unknown, convex obstacles. The robot uses a limited range onboard sensor, capable of recognizing, localizing and generating online from its catalogue of the familiar, non-convex shapes an implicit representation of each one. These sensory data and their interpreted representations underlie an online change of coordinates to a completely convex model planning space wherein a previously developed online construction yields a provably correct reactive controller that is pulled back to the physically sensed representation to generate the actual robot commands. Using a modified change of coordinates, the construction is also extended to differential drive robots, and numerical simulations further verify the validity of our formal results.

Experimental validation of our algorithm with deep learning techniques for object pose and triangular mesh recognition

[25] is currently underway. Next steps target environments presenting geometry more complicated than star-shaped obstacles, by appropriately modifying the purging transformation algorithm for trees-of-stars, presented in [28]. Future work aims to relax the required degree of partial knowledge and the separation assumptions needed for our formal results, by merging the “implicit representation trees” (e.g. see Fig. 5 in Appendix 0.B) online, when needed.

References

  • [1] http://wiki.ros.org/pointcloud_to_laserscan
  • [2] Arslan, O., Koditschek, D.E.: Sensor-Based Reactive Navigation in Unknown Convex Sphere Worlds. In: The 12th International Workshop on the Algorithmic Foundations of Robotics (2016)
  • [3] Arslan, O., Koditschek, D.E.: Sensor-Based Reactive Navigation in Unknown Convex Sphere Worlds. International Journal of Robotics Research p. (to appear) (Jul 2018)
  • [4] Astolfi, A.: Exponential Stabilization of a Wheeled Mobile Robot Via Discontinuous Control. Journal of Dynamic Systems, Measurement, and Control 121(1), 121–126 (1999)
  • [5] van den Berg, J., Lin, M., Manocha, D.: Reciprocal Velocity Obstacles for real-time multi-agent navigation. In: IEEE International Conference on Robotics and Automation. pp. 1928–1935 (2008)
  • [6] van den Berg, J., Guy, S.J., Lin, M., Manocha, D.: Reciprocal n-Body Collision Avoidance, pp. 3–19. Springer Berlin Heidelberg (2011)
  • [7] Borenstein, J., Koren, Y.: Real-time obstacle avoidance for fast mobile robots. IEEE Transactions on Systems, Man, and Cybernetics 19(5), 1179–1187 (1989)
  • [8] Borenstein, J., Koren, Y.: The vector field histogram-fast obstacle avoidance for mobile robots. IEEE Transactions on Robotics and Automation 7(3), 278–288 (1991)
  • [9] Bowman, S.L., Atanasov, N., Daniilidis, K., Pappas, G.J.: Probabilistic data association for semantic SLAM. In: IEEE International Conference on Robotics and Automation. pp. 1722–1729 (May 2017)
  • [10] Brock, O., Khatib, O.: High-speed navigation using the global dynamic window approach. In: IEEE International Conference on Robotics and Automation. pp. 341–346 (1999)
  • [11] Chaney, R.W.: Piecewise functions in nonsmooth analysis. Nonlinear Analysis: Theory, Methods & Applications 15(7), 649–660 (1990). https://doi.org/10.1016/0362-546X(90)90005-2
  • [12] Fiorini, P., Shiller, Z.: Motion Planning in Dynamic Environments Using Velocity Obstacles. The International Journal of Robotics Research 17(7), 760–772 (1998)
  • [13] Henry, P., Vollmer, C., Ferris, B., Fox, D.: Learning to navigate through crowded environments. In: IEEE International Conference on Robotics and Automation. pp. 981–986 (2010)
  • [14] Hirsch, M.W.: Differential Topology. Springer (1976)
  • [15] Ilhan, B.D., Johnson, A.M., Koditschek, D.E.: Autonomous legged hill ascent. Journal of Field Robotics 35(5), 802–832 (Aug 2018). https://doi.org/10.1002/rob.21779
  • [16] Johnson, A.M., Hale, M.T., Haynes, G.C., Koditschek, D.E.: Autonomous legged hill and stairwell ascent. In: IEEE International Symposium on Safety, Security, and Rescue Robotics. pp. 134–142 (2011)
  • [17]

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

  • [18] Karaman, S., Frazzoli, E.: High-speed flight in an ergodic forest. In: IEEE International Conference on Robotics and Automation. pp. 2899–2906 (2012)
  • [19] Kong, C., Lin, C.H., Lucey, S.: 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 (2017)
  • [20] Kuntz, L., Scholtes, S.: Structural Analysis of Nonsmooth Mappings, Inverse Functions, and Metric Projections. Journal of Mathematical Analysis and Applications 188(2), 346–386 (1994). https://doi.org/10.1006/jmaa.1994.1431
  • [21] Majumdar, A., Tedrake, R.: Funnel Libraries for Real-Time Robust Feedback Motion Planning. The International Journal of Robotics Research 36(8), 947–982 (2017)
  • [22] Massey, W.S.: Sufficient conditions for a local homeomorphism to be injective. Topology and its Applications 47, 133–148 (1992)
  • [23] Paranjape, A.A., Meier, K.C., Shi, X., Chung, S.J., Hutchinson, S.: Motion primitives and 3D path planning for fast flight through a forest. The International Journal of Robotics Research 34(3), 357–377 (2015)
  • [24] Paternain, S., Koditschek, D.E., Ribeiro, A.: Navigation Functions for Convex Potentials in a Space with Convex Obstacles. IEEE Transactions on Automatic Control (2017). https://doi.org/10.1109/TAC.2017.2775046
  • [25] Pavlakos, G., Zhou, X., Chan, A., Derpanis, K.G., Daniilidis, K.: 6-DoF object pose from semantic keypoints. In: IEEE International Conference on Robotics and Automation. pp. 2011–2018 (May 2017)
  • [26] Rimon, E.: Exact robot navigation using artificial potential functions. Ph.D. thesis, Yale University (1990)
  • [27] Rimon, E., Koditschek, D.E.: The Construction of Analytic Diffeomorphisms for Exact Robot Navigation on Star Worlds. Transactions of the American Mathematical Society 327(1), 71–116 (1989)
  • [28] Rimon, E., Koditschek, D.E.: Exact Robot Navigation Using Artificial Potential Functions. IEEE Transactions on Robotics and Automation 8(5), 501–518 (1992)
  • [29]

    Russell, S.J., Norvig, P.: Artificial Intelligence: A Modern Approach. Prentice Hall (2009)

  • [30] Rvachev, V.L.: An analytic description of certain geometric objects. In: Doklady Akademii Nauk. vol. 153, pp. 765–767. Russian Academy of Sciences (1963)
  • [31] Shapiro, A.: Sensitivity Analysis of Nonlinear Programs and Differentiability Properties of Metric Projections. SIAM Journal on Control and Optimization 26(3), 628–645 (1988). https://doi.org/10.1137/0326037
  • [32] Shapiro, V.: Semi-analytic geometry with R-functions. Acta Numerica 16, 239–303 (2007)
  • [33] Simmons, R.: The curvature-velocity method for local obstacle avoidance. In: IEEE International Conference on Robotics and Automation. vol. 4, pp. 3375–3382 (1996)
  • [34] Trautman, P., Ma, J., Murray, R.M., Krause, A.: 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 (2015)
  • [35] Vasilopoulos, V., Arslan, O., De, A., Koditschek, D.E.: Sensor-Based Legged Robot Homing Using Range-Only Target Localization. In: IEEE International Conference on Robotics and Biomimetics. pp. 2630–2637 (2017)
  • [36] Vasilopoulos, V., Vega-Brown, W., Arslan, O., Roy, N., Koditschek, D.E.: Sensor-Based Reactive Symbolic Planning in Partially Known Environments. In: IEEE International Conference on Robotics and Automation. pp. 5683–5690 (2018)
  • [37] Vasilopoulos, V., Topping, T.T., Vega-Brown, W., Roy, N., Koditschek, D.E.: 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 (2018)

Appendix 0.A Proofs

0.a.1 Proofs of results in Section 3

Proof of Lemma 2.

Since both the switches and the deforming factors are smooth, for , the only technical challenge here is introduced by the fact that the number of discovered star-shaped obstacles in is not constant and changes as the robot navigates the workspace.

Notice from (6) that all the derivatives of used in the construction of the switch for any are zero if and only if is zero. Therefore, in order to guarantee smoothness of , we just have to ensure that when a new obstacle is added to the semantic map, the value of will be zero. This follows directly from the assumption that the sensor range is much greater than , which implies that when obstacle is discovered, the robot position will lie outside the set and therefore the value of will be zero. ∎

Proof of Proposition 1.

First of all, the map is smooth as shown in Lemma 2. Therefore, in order to prove that is a diffeomorphism, we will follow the procedure outlined in [22], also followed in [27], to show that

  1. has a non-singular differential on

  2. preserves boundaries, i.e., .777Here we denote by the -th connected component of the boundary of (that corresponding to ), with the outer boundary of .

  3. the boundary components of and are pairwise homeomorphic, i.e. .

We begin with property 1. Using Lemma 1 and observing from (7) and (8) that a switch is zero if and only if its gradient is zero, we observe from (13) that is either the identity map (which is non-singular) or depends only a single switch when . In that case, we can isolate the -th term in (13) and write the map differential as

(30)

From this expression, we can find with some computation

(31)

However, we know that

(32)

since , giving . Also, by construction (since ), and in the set , because of Assumption 2-(c). Therefore, we get for all such that . Also, since , we can similarly compute

(33)

which leads to for all such that . Since and , we conclude that

has two strictly positive eigenvalues in the set

. Since this is true for any , it follows that has two strictly positive eigenvalues in and, thus, is non-singular in .

Next, pick a point for any . This point could lie on the outer boundary of , on the boundary of one of the unknown but visible convex obstacles, or on the boundary of one of the star-shaped obstacles. In the first two cases, we have , while in the latter case

(34)

for some , sending to the boundary of the -th disk in . This shows that we always have and, therefore, the map satisfies property 2.

Finally, property 3 derives from above and the fact that each boundary segment is an one-dimensional manifold, the boundary of either a convex set or a star-shaped set, both of which are homeomorphic to the corresponding boundary . ∎

0.a.2 Proofs of results in Section 4

Proof of Proposition 2.

Since is just the identity transformation away from any star-shaped obstacle and the control law guarantees collision avoidance in that case, as shown in [2], it suffices to show that the robot can never penetrate any star-shaped obstacle, i.e., for any such that for some , we have . For such a point , we get from (10) and (13)

(35)

since . Since , we can explicitly compute the inverse of the 2x2 matrix from its four elements , , , as

(36)

and after some simple computations, we can eventually find

(37)

On the other hand,

(38)

Since