Potential Gap: Using Reactive Policies to Guarantee Safe Navigation

03/21/2021
by   Ruoyang Xu, et al.
Georgia Institute of Technology
0

This paper considers the integration of gap-based local navigation methods with artificial potential field (APF) methods to derive a local planning module for hierarchical navigation systems that has provable collision-free properties. Given that APF theory applies to idealized robot models, the provable properties are lost when applied to more realistic models. We describe a set of algorithm modifications that correct for these errors and enhance robustness to non-ideal models. Central to the construction of the local planner is the use of sensory-derived local free-space models that detect gaps and use them for the synthesis of the APF. Modifications are given for a nonholonomic robot model. Integration of the local planner, called potential gap, into a hierarchical navigation system provides the local goals and trajectories needed for collision-free navigation through unknown environments. Monte Carlo experiments in benchmark worlds confirm the asserted safety and robustness properties by testing under various robot models.

READ FULL TEXT VIEW PDF
POST COMMENT

Comments

There are no comments yet.

Authors

page 1

page 2

page 3

page 4

11/23/2021

Context-based navigation for ground mobile robot in a semi-structured indoor environment

There is a growing demand for mobile robots to operate in more variable ...
03/31/2020

APPLD: Adaptive Planner Parameter Learning from Demonstration

Existing autonomous robot navigation systems allow robots to move from o...
01/10/2022

Motion Primitives-based Navigation Planning using Deep Collision Prediction

This paper contributes a method to design a novel navigation planner exp...
04/20/2021

A Deep Learning Approach To Multi-Context Socially-Aware Navigation

We present a context classification pipeline to allow a robot to change ...
05/26/2020

Adaptive Robot Navigation with Collision Avoidance subject to 2nd-order Uncertain Dynamics

This paper considers the problem of robot motion planning in a workspace...
09/12/2018

Safe Navigation with Human Instructions in Complex Scenes

In this paper, we present a robotic navigation algorithm with natural la...
02/26/2020

GLAS: Global-to-Local Safe Autonomy Synthesis for Multi-Robot Motion Planning with End-to-End Learning

We present GLAS: Global-to-Local Autonomy Synthesis, a provably-safe, au...
This week in AI

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

I Introduction

Potential methods generate guaranteed safe reactive policies for fully controlled point-mass agents going from one point in space to another, under the assumption of known collision space regions [11]. A non-trivial performance gap emerges between idealized and actual models, the latter of which occupy space, have dynamics, can be nonholonomic, and have incomplete collision space measurements (from a limited field of view) [13, 24] . A variety of other planning schemes have been derived and refined to address the performance gap [30, 33]. In the context of long distance navigation, no single strategy provides the best outcome. Hierarchical strategies permit multiple approaches that operate at different temporal and spatial scales [30, 35, 26]. More importantly, they consider the role of visual sensors in providing up-to-date information regarding navigable space that complements existing and potentially stale free space maps. Unfortunately, more research effort goes into the design of general-purpose, single strategy planning schemes, rather than into the closed-loop performance of sensing-informed hierarchical navigation methods.

The research deficit implies that there may be value in more extensive studies of hierarchical methods and how classical navigation approaches may successfully integrate into their architecture. Doing so may also suggest how to best leverage general-purpose planners within an equally general and modular hierarchical navigation framework. It also provides an excellent opportunity to reconsider how idealized approaches, or those with theoretical support but limited applicability, might provide safe local planners that operate on short temporal and small spatial scales. For these smaller domains, the fragile theoretical guarantees could translate more robustly to non-ideal settings through targeted modifications. This paper explores the value of reconsidering potential field navigation strategies for vision-based navigation through unknown environments. In the process, it creates a hierarchy of modular components that are activated as the robot model strays from the ideal one. Each component corrects for the loss of theoretical support and minimizes sensitivity to this loss. The final approach, called the potential gap local planner, is shown to inherit the navigation guarantees of potential field methods for idealized systems, to demonstrate robustness to non-ideal robot models, and to degrade gracefully for uncorrected non-ideal properties. It provides a framework for improved safety within a real-time, hierarchical navigation system.

I-a Related Work and Research Context

Visual navigation has an extensive history that bridges several core areas of robotics from perception to planning to action. This section narrowly reviews topics most related to the study at hand, starting at the top level with hierarchical navigation and descending to closed-loop execution.

I-A1 Hierarchical Navigation Systems

Navigation systems for mobile vehicles must identify a solution to the high-level task of goal attainment with realizable low-level system control laws. In the context of mobile robot navigation, bipartite hierarchical systems often consist of a global planner that generates paths with prior knowledge of the environment, and a local planner that reacts to the local information obtained from the real-time sensors [30]. They leverage the advantages of different planners while offsetting their limitations, and have long been used in robotics, as the hierarchical structure leads to higher fault tolerance and increased robustness [7, 32, 10, 31]. The global planner, which runs at a lower rate and considers all known map information to provide a global path from a given start pose to a goal pose, can be any applicable planner [14]. In contrast, the local planner executes at a higher frequency to produce kinodynamically controllable commands for driving robots and avoiding obstacles. There is flexibility to select the local planner so long as it is structured to receive sensory input and assimilate the input into its local collision-avoiding navigation decision structure [4, 8, 25, 29]. Its limited scale supports real-time, sensory-driven navigation with collision avoiding properties.

I-A2 Reactive and Potential Field Methods

The artificial potential field method and its variants are a family of planners whose instances offer simple and fast computation for mobile robot obstacle avoidance [11, 3, 9, 23, 16]. While the potential field is particularly attractive due to its elegance and simplicity, there are substantial shortcomings inherent to this method such as a lack of consideration for robot kinematics, dynamics, as well as local minimum problems with regard to world geometry [13, 33]. Significant efforts were made to alleviate those problems [12, 6]. When implemented as reactive planners, the family of APFs nevertheless directly map robot state and sensor observation to available actions, offering better computational performance than deliberative planners. They share valuable traits with perception space methods, such as minimal sensor processing and planning complexity in the egocentric robot frame. Integrating reactive methods and perception space methods can leverage their fast compute properties and the limited deliberation associated with local planning modules informed by a global planner.

I-A3 Perception-Space and Gap Navigation Methods

Recent work has explored the use of perception-space representations, inspired from Marr’s 2.5D space, and argued in favor of mixed representation hierarchical navigation strategies [30, 29]. In particular, a local planner (limited to a short time and small spatial scale) gains computational advantages by minimally processing the sensor data and recasting local navigation as an ego-centric decision process. Related work has established similar benefits for stereo MAVs [18].

For ground vehicle navigation, the topology of the space and how to interpret it from sensor data has emerged as an important local navigation decision [29]. Gap-based processing aimed at detecting passable free-space is compatible with and improves the synthesis of local optimal paths. While there is no widely agreed-upon representation–or processing approach–for gaps, gap-based methods typically reduce 1D laser scan measurements to a set of “gaps” comprised of beginning and terminating points that represent collision-free regions in the observable space [27, 19, 21, 20, 22]. A gap-based method would then generate reactive motion commands towards a selected gap, which gives favorable performance in reasonably sparse and structured environments. However existing gap representations either rely on the presence of free space to infinity [27] or dense and accurate sensor measurements for detecting discontinuities [19], leaving the representation prone to potentially false collision free spaces and errors. Gap-based methods operate semi-deliberately in that the construction and selection of free space are intentional while execution is reactive. There is an opportunity to connect perception space methods with classical potential field methods through the affordance of gaps to deliver safe planning.

The value of gap detection in terms of establishing navigation affordances for local planning was established in [29]. The intent of this study is to explore more deeply the gap representations associated with gap-based path planning and their connection to safe navigation through local space. Based on their use, gap-based methods will naturally connect the local planning module to the potential field navigation strategies reviewed above, in turn overcoming the latter’s fundamental limitations for complex worlds.

I-A4 Safety in Control

Control barrier functions (CBFs) are a class of functions whose role is to serve as instantaneous, point-wise constraints on the applied control to achieve forward invariance with respect to a safe set, in our context a collision-free set (or to achieve forward reachability then invariance with respect to a target set) [1]. By converting safety (reachability) constraints into linear-in-the-control constraint equations, CBFs simplify the certification of safety (reachability) with a trade-off in short-horizon optimality. Looking back at the history of CBFs, which derive from barrier functions, there is a connection to soft-constraint optimal control methods. Barriers were used to generate soft-constraint modifications to the optimization landscape, and could be used to generate a family of optimal control solutions from a parametrically varied barrier specification, which often had properties similar to or were potential fields. By sequentially solving and warm-starting a series of soft-constraint problems with increasing penalties, the intent was to converge to the equivalent hard-constraint solution. In connecting potential methods to hierarchical planning, we have the opportunity to translate the collision-critical potentials to CBF-like specifications for ensuring the safe execution of local paths.

For an idealized robot model (full control, 1st order, point-mass, etc.), the safe forward-invariant set from a CBF specification aligns with the safe reachable set generated using HJB methods [2]. This set is described by a barrier function level-set. For non-ideal robots a gap emerges between the idealized level-set defined by the barrier function and the safe set defined by HJB reachability/CBF feasibility analysis. Both approaches require offline analysis to characterize the gap to arrive at the correct set. This paper takes a step towards efficiently and online modifying an idealized, sensor-derived safe set and applied controls for non-ideal robots.

I-B Hierarchical Planning for Navigation

As briefly noted in §I-A1 and depicted in Figure 1

, a hierarchical navigation system decomposes its task into two parts with distinct temporal and spatial scales: a global planner and a local planner. Given a goal point and an estimated map, the global planner synthesizes a candidate path from the current robot pose to the goal. The global path is passed to the local planner, whose objective is to generate local goals and local paths that lead the robot to the final goal. The reduced temporal and spatial scale of the local planner permits real-time operation in the face of novel information generated from sensor measurements. These measurements are also passed to the global planner to enrich the global map for generating valid global paths. The objective of the local planner, as informed by the global path, is to sequentially synthesize collision-free trajectory segments whose concatenation terminates at the final goal. The

potential gap method described next will define the operation of the local planner.

Laserscan

Goal

Egocircle

World Map

Global Planner

Gap Generation and Selection

Multi-Trajectory Synthesis

Trajectory Scoring

Trajectory Controller

Local Planner

Hierarchical Navigation System

sensor info

Recent Best Trajectory

Replan

Level

High

Mid

Low
Fig. 1: Hierarchical Navigation System information flow with global planner modules (gray boxes) and local planner modules (blue boxes).

Ii The Potential Gap Local Navigation Module

This section describes the approach taken to identify gaps

for navigation purposes and how they are used to specify potential-based local navigation vector fields. By design, the vector fields have guaranteed collision-free

passage through the gap for idealized settings (first-order, point-mass, holonomic travel, full vision). For non-idealized robot models, the potential fields require modification to their construction and use as barrier functions for an additional run-time hedge against collisions.

Ii-a Gap Analysis and Gap Detection

Existing gap detection methods considered a limited set of navigation and sensing scenarios, leading to correct but not fully generalizable gap detections. Though Close Gap sought to address these circumstances, the set of scenarios covered was still limited [19]. Our conception of a gap is informed by earlier work using gaps to define local navigation goals [29]. In considering the intent behind gap-based methods and their utility for navigation, we propose a different conception of gaps that leads to a new method for detection and analysis.

Viewing gaps from a perception space approach, it is equally natural to represent them in robot-centered polar coordinates. This perspective leads to the categorization of gaps into swept and radial gaps based on their dominant direction. Conceptually, swept gaps involve curves with larger angular sweeps, while radial gaps have larger distance variation and little angular difference. The former “face” the robot and have good line-of-sight visibility properties. Swept gaps provide information both about passage to the gap, as well as through the gap. Radial gaps are oriented “sideways” to the robot and have bad line-of-sight visibility properties. They exhibit poor properties regarding passage to the gap and/or through it, see Fig. 2(b) and Fig. 3 A. The objective of a gap detection strategy should be to provide the maximal quantity of swept gaps to leverage their favorable properties.

Ii-A1 Swept and Radial Gap Detection

We define a new gap detection policy that simplifies gaps when they arise from complex world structure. The policy avoids potentially false information arising from measurement sparsity. Gap modeling hinges on classifying them as swept or radial.

Suppose that a full (360) laser scan-like measurement is available to the robot. In the instance of a limited field of view, measurement propagation using the egocircle [29] would keep track of measured parts of the surrounding collision-space. Let the polar measurement provide readings with a maximum range value . Perform a first pass through looking for the following two outcomes:

  1. [leftmargin=*]

  2. Large interval , where .

  3. Instantaneous change in range: ,

where is the inscribed radius of robot and is the distance in Cartesian space. Regions triggering the above are gaps that populate the gap set . Each gap in is then classified as swept or radial, see Figure 2. Gaps picked based on the second test are automatically radial gaps. The others are classified based on the dominant direction. Such direction is measured via the angle on the short side of the triangle formed by the robot and the two sides of the gap in the laser scan, Figure 2(d). Let , , and . Then,

(1)

The threshold differentiates radial from swept gaps. Assign the gap type as left when and right otherwise. For each detected gap , record the angular index and distance of its sides.

(a)

(b)

(c)

(d)

swept gap

radial gap

simplified gap

radial gap

to

swept gap
Fig. 2: (a,b) Visual of swept and radial gaps, (c) the radial gap conversion process, and (d) radial gap angular geometry. Black dot denotes the robot.

Ii-A2 Gap Simplification

A second pass through merges radial gaps and replaces them with swept gaps. This process is done by moving gaps into a stack and recording the last continuously mergable radial gaps before reaching a termination condition. The termination condition is defined by parameter denoting the distance between the pending gap and terminating gap. A gap is mergable with an incoming gap if there are no closer obstacle points in between. Mergability is tuned by the parameter , based on the acceptable range difference between two merging candidate gaps. By traversing every gap once when added to the final set and possibly once more when determining the need for deletion, the time complexity for removing redundant gaps is linear. We call this method Swept Gap Prioritization (SGP).

closest gap

potential gap

closest gap

potential gap

(a) Closest gap vs potential gap.

(b) Closest gap vs potential gap.

D

A

B

C

D

E

E

F

F

(c) Closest gap vs potential gap.

G

G

(d) Closest gap vs potential gap.
Fig. 3: Comparison between closest gap (CG) [19] and potential gap (PG). Dark dots represents laserscan results, red lines denote raw gaps for PG, and the teal lines denote returned gaps (CG and PG). (a) Performing SGP collapses dangerous regions as A, B, and C. (a,b) Since PG explicitly reasons about swept gaps, it simplifies regions D and E while CG does not due to boundary geometry readings. (c) PG is less sensitive to sensor noise and provides a smaller, safer set of gaps as in region F, since regions behind swept gaps are collision free. (d) CG and PG are comparable for G.

Ii-A3 Radial Gap Conversion

For holonomic point-mass robots with full () scanning the SGP strategy is sufficient for collision-free path planning. However, for robots with partial scanning, non-circular shapes, or nonholonomic constraints, radial gaps cause problems based on the limited visual information available about what lies beyond the gap. To remedy the problem, a radial gap conversion process may be added to convert all remaining radial gaps into swept gaps by rotating their gap representation about the nearest gap point. The amount of rotation is controlled by the parameters and by . Radial gaps are rotated by so that the local goal, a temporary point just away on the other side of the gap is line-of-sight visible. Figure 2(c) shows a preserved radial gap converted to a swept gap.

Ii-A4 Swept Gap Prioritization versus Other Gaps

Figure 3 visualizes some of the benefits of potential gap (PG) over closest gap (CG) [19], which attempts similar processing. The figures shows examples of gap detection where raw gaps are marked bright red, and simplified gaps marked teal. PG collapses compound situations (A, B, and C regions), reducing gaps to the ones most relevant to the robot. The simplification reduces uncertainty behind swept gaps and avoids dangerous compound scenarios such as at the tip of region A and B. While both approaches generate largely similar results in most scenarios, PG is computationally simpler. It leverages gap positioning in polar space during the reduction of gap sets and is more robust to situations where sensors readings are uncertain. Figure 3(c) illustrates where sensor uncertainty due to environment geometry causes discontinuities in range readings. PG simplifies some of the gaps and returns ones better suited to motion planning.

Ii-B Gradient Field Construction

The output of the RGP algorithm leads to a modified (and usually reduced) gap set . For each we establish a gap local goal that is at least away from the gap, and from the closest side of gap if the local goal lies outside of the (visible) angular space of the gap. The generation of a local navigation trajectory for the gap involves the creation of attractive potentials whose gradients are augmented with circulation terms [5]. Under normal circumstances the addition of circulation would have sign ambiguity. Here, the desired flow direction to the gap is known, and hence circulation sign, so that the constructed solution has guaranteed passage through the gap.

Ii-B1 Closing the Gap for Convexity

If the gap angular extent is beyond (usually or ), then the gap angle is shrunk to give a new whose angular extent is , thereby ensuring that the gap region is a polar triangle convex in Euclidean space. Euclidean convexity ensures that the gradient vectors from the potential and circulation elements will have the necessary properties to guarantee gap passage. Shrinking of the gap is biased to ensure that the local gap goal lies within the outer gap region. The set is remapped to consist of Euclidean convex gaps only.

Ii-B2 Potential Field with Circulation

After convexification, and possibly modification (see §II-D3), to an angular extent equal to or less than , the local goal is line-of-sight visible to the other side of the gap from any point within the gap region. Let the local goal point be as determined from the chosen gap . The attractive potential is

(2)

where the first distance is to the local goal point and the second is to the gap curve (then vanishes on the other side of the gap). These potentials attract the robot to the gap curve then through to the local goal. Rather than use the gradient for the flow, we will follow the normalized gradient.

The premise behind gaps is that there is an obstacle in the world that must be avoided by staying within the gap region, which is known to be collision-free. Rather than impose an obstacle avoiding potential, which could create a fixed point in the resulting vector field, a purely rotational vector field is created

(3)

where is the skew-symmetric form, and is the angular distance. The vector fields are two rotational fields anchored at the left and right gap points. Figure 4 shows an example circulation vector field.

+

=

circulation

attractor

combined
Fig. 4: Gap gradient field construction. Red triangle is robot location, blue circles are gap curve endpoints, and green circle is goal point.

Ii-B3 Guaranteed Passage

Proving that passage through the gap for the robot must happen involves showing that the boundary of the gap navigation region points inwards along the robot-to-gap-endpoint lines (or simply gap lines) and that there is no fixed point interior to the region. The only reasonable flow for any point in the region is to exit via the gap curve, e.g., gap passage. Along the gap lines, the gradients of (2) either point inwards or parallel to it, never out by virtue of gap convexity and by definition of the gap region. Thus, what must be shown is that the circulation components also point inwards. On the gap line, one circulation term has vanishing; the circulation is purely perpendicular and inward pointing. Let this vector be . Let the other circulation term contribute the vector . It satisfies

(4)

which means that restricted to gap lines is inward pointing. The only outward flow can be on the gap curve.

A similar argument as above applies to show that the interior gap region points have a non-trivial outward pointing flow. Define to be the radially directed outward vector for a point . The following properties hold:

(5)

by virtue of the gap angle being (see §II-D3 for the case and how it gets further reduced to ). The definition of is that it computes the gradient in (2) then makes it unit length. The vector field interior to the gap region always has positive outward pointing contributions, so there can be no fixed points. Strict positivity implies that any initial point in will flow out through the gap curve.

Ii-C Trajectory Synthesis and Scoring

With the generated set of gaps from Section §II-A, perform forward integration on every potential field to create a set of trajectories . Each trajectory is scored through cumulative egocentric pose cost and the terminating pose distance to the local navigation goal [30]. Let denote a single trajectory discretized into poses , denote the local navigation goal, and return the distance from to the nearest point in the egocircle . Score tuning parameters include: . The trajectory score is:

(6)

At each planning iteration, the best trajectory from current set is selected and compared against the currently executing trajectory, with an oscillation cost to limit path switching.

Ii-D Safe Extensions for Non-Ideal Robots

The mathematics and algorithms employed to now presumed the use of an idealized robot model. For deviations from this model, additional modifications to the base method are required. This section describes the modifications required for (a) robots with area/volume, (b) noholonomic robots, and (c) other deviations from the ideal.

Ii-D1 Perception-Space Collision Checking

This module is an automatic part of our perception-space navigation stack [28, 30, 29]. It takes candidate local trajectories and synthesizes simulated egocircle measurements at their future poses along the trajectory that measure the far side of the robot. If any range measurements in the simulated egocircle lie beyond the current actual egocircle estimate, then the pose is in collision and the trajectory is rejected as infeasible. The process repeats until a collision-free path is found through a gap or all are rejected. The former gets passed on for trajectory tracking while the latter triggers a global replan.

Ii-D2 Nonholonomic Vehicles

The control is not fully realizable due to vehicle movement constraints. However, most nonholonomic vehicles (and certainly all standard mobile robots) are linearly controllable along feasible trajectories. Let the vehicle be a differential drive Hilare type. The control is converted to feasible movement via:

(7)

where . The control is both realizable and guides the robot towards the local gap goal.

Ii-D3 Radial Extension of Gaps

Since nonholonomy impacts the linear controllability of robot models, the paths generated from the vector field in §II-B cannot be precisely followed, but are asymptotically tracked by (7). The robot may actually leave the gap region when attempting to do so (typically when oriented away or orthogonal to the local goal). We address this problem by expanding the gap region in the vicinity of the robot while closing it further from the robot. This process is called radial extension since the gap region vertex is shifted away from the gap curve along a radial line. Shifting the origin permits the construction of a new gap region that puts the robot inside of it. The robot may move for a non-zero distance and remain inside.

The process first finds the largest circular region centered on the robot known to be collision-free. Such a circle must exist if the robot is non-colliding. The new origin is mapped to the intersection of the circle with the gap boundary bisector that passes through the robot frame, see Figure 5. A new gap region is established from this new origin and the radial vectors that pass through the two intersecting points of the safe circle and the gap region boundary lines. Simple geometric arguments show that the angular extent of this new gap is half that of (or smaller than) the original gap. Gaps extending out to are transformed to gaps with a gap angle less than or equal to , as preferred in §II-B.

Ii-D4 The Safety Projection Operator

One final addition aims to prevent collisions, in case nonholonomic motion and the discrete time implementation of continuous control laws lead to collision for the edge cases. It implements the projection operator from adaptive control, but to prevent entrance to a region as opposed to preventing exit. It has long been used in adaptive control to prevent parameter drift and is an early example of a barrier-like function that performs command reshaping with provable forward invariance for a convex set [15]. Let be the set of collision curves to the left and to the right of the gap (in polar space) and the distance function to . Define the level-set function:

(8)

where and which is depicted in Figure 6. Far enough away, the potential is negative. At , passes through zero and at it passes through unity. These properties are essential to the projection operator, which is defined as in (9). In adaptive control, the projection operator keeps adaptive parameters from going unbounded by restricting the parameters to a compact set. Here, by definition of , the opposite holds with the intent being to drive the state away from the open, bounded set associated to . When evaluates to less than zero, no control modification occurs. When greater than zero, but with the control vectored away from the boundary, no control modification occurs. As under a violating law , the control modification increases until canceling out the component leading to the safety violation. Under full control, the projection operator prevents control dynamics that would enter the unsafe set established by the obstacles.

original polar triangle and swept gap (red)

collision free circle (green)

new polar triangle and its swept gap (green)

new polar origin
Fig. 5: Radial extension. The original swept gap polar triangle is converted to a new triangle with a shifted origin (backwards along the swept gap arc bisector). The shift distance is the radius of the largest free-space disc.
(9)

Fig. 6: Obstacle Potential

Iii Experiments, Outcomes, and Discussion

This section covers the experiments and outcomes conducted for potential gap and for the state-of-the-art method TEB. Monte Carlo rollouts are performed for navigation tests in benchmark worlds, which are populated with randomized unknown obstacles and start/goal locations.

Iii-a Benchmark Environments

The TEB and potential gap (PG) navigation planner are run in four benchmarking worlds. Dense, Campus, and Office are from [29]. The fourth, Sector world, is a single large room intended to model indoor spaces with locally untraversable regions such as chair legs and obstacles such as trash cans and cabinets. The simulation environment is a slightly modified version of the Simple Two Dimensional Robot Simulator (STDR) [17]. A very short range 360 range finder acts as a virtual bumper for collision detection. The platforms are configurable with different angular fields of view (FoV) of the laser sensor, starting from 60 to 360, for partial to full observability. Additionally, experiments are conducted in Gazebo with Turtlebots (60 FoV) for navigation performance with nonholonomic, 2 order robot.

Iii-B Experiments and Outcomes

The experiments cover a range of test cases, starting with the idealized robot model under variable field of view (FoV) settings. The idealized robot model has 1 order, fully actuated control equations (i.e., holonomic) with a circular body that operates like a point mass. The second experiment relaxes the control equations to being nonholonomic. The third experiment moves from STDR to Gazebo, where the motion model is nonholonomic, 2 order. Each setting is run with 100 different seeds in all four worlds. STDR experiments are run 25 at a time and Gazebo experiments 3 at a time on an AMD Ryzen 7 3800X processor (single-thread passmark of 2744; multi-thread score of 43,447).

PG

FoV

Percentage

TEB

FoV

Percentage

PG+RE

FoV

Percentage
Fig. 7: PG (left) and TEB (right) performance vs FoV for the idealized robot model (-order holonomic). Numbers over columns are success rate (%). PG+RE (middle) adds radial extension to address FoV issues.

Percentage

PG

TEB
Fig. 9: PG and TEB performance for fixed FoV in Gazebo.

Iii-B1 Idealized Robot Model

Figure 9 gives the failure mode vs FoV for PG and TEB implementations. Firstly, for the full FoV case, PG achieves no collisions and a nearly perfect success rate. A small quantity (1%) of runs resulted in an abort, which means that the local and global planner could not coordinate to find a feasible path to the goal. PG continues to match or outperform TEB in collisions, aborts, and time outs in lower FoV cases. PG also maintains a high success rate, much like TEB preserves it moderate success rate. However, for 60 and 90 FoV, PG has more collisions, and matches TEB’s success rate at the lowest FoV. Enabling radial extension gives a bit more navigation free-space around the robot to compensate for the limited FoV. PG+RE matches or outperforms TEB for all cases except for 60, where the collision count remains higher.

The results show that in the idealized setting, the PG navigation system matches the theoretical expectations. While TEB also has similar performance, PG employs a greedy gradient descent approach to path generation. In contrast, TEB employs a soft-constraint optimal control formulation to establish the local navigation paths and has higher computational cost. The lower FoV results involve collisions that arose from a lack of FoV but can be corrected with the RE module. The high success rate of PG and PG+RE confirms that the notion of using gaps to serve as navigation affordances is critical to achieving safe, goal attaining navigation through an unknown environment.

Bumper Collisions

FoV

Percentage

Success Rate

FoV

Percentage
Fig. 8: PG and TEB bumper collisions (left) and success rate (right) vs FOV for the order nonholonomic case.

Iii-B2 Nonholonomic, 1 order

Figure 8 provides the outcomes for bumper collisions (left) and success rate (right). Each plot contains implementations of TEB, baseline PG without projection operator (PO) and radial extend (RE), the full nonholonomic (PO,RE), and mixed activations. The baseline PG, which uses the idealized robot solutions, has the worst performance in lower FoVs. TEB maintains a low collision count and has none at 360 FoV. Of the two partially corrected PG implementations, the projection operator performs best (PO,RE). Adding radial extension improves it somewhat for higher FoV, showing that both contribute to safe navigation. The zero collision performance of TEB at full FoV is undermined by its low success rate induced by aborts. Furthermore, the PG+(PO,RE) method has more consistent success rate as a function of FoV.

Similar collision outcomes between PG+(PO,RE) and TEB show that the modified method functions as intended and that the greedily derived paths are similar to optimally derived paths from a safety perspective. They are better than the sampled and optimally derived paths of TEB from a task completion perspective. Compared to the holonomic case in Figure 9, both planners displayed an outcome less sensitive to FoV. This is caused by the motion model limiting motion to angles local to the camera optical axis.

Iii-B3 Nonholonomic, 2 order

Figure 9 displays the failure rates for PG+(PO,RE) and TEB in the Gazebo simulated Turtlebot (FoV = 60). The TEB collision rate matches the 1 order case. The 2 order nonholonomic PG collision rate is similar to the 1 order case and significantly better than TEB (6x). The success rate for PG drops from 96% to 93%, but is well above those of the 1 and 2 order TEB cases.

PG is relatively consistent when applied to the dynamic case. Again, this is an important outcome based on the fact that PG is based on APF methods that were designed for idealized robot models. As a greedy path generation approach, it outperforms the soft-constraint optimal control TEB solution. Though TEB explores a far richer trajectory-space for a given local goal, the extra degrees of freedom may not properly exploit the navigable space as well as the potential gap method does. Especially since PG typically explores 5x more trajectory options than TEB. Furthermore, the soft constraint approach cannot guarantee feasibility of the trajectories, which may result in poor sequential decision making if some of the solutions are immediately feasible but eventually infeasible. By construction PG avoids these issues and creates protective mechanisms to prevent collision while aiming to complete the task.

on Partial FoV

Percentage

on Full FoV

Percentage
Fig. 10: Safety vs Passage test for 120 and 360 FoV in nonholonomic 1 order case. Performance vs .

Iii-B4 Nonholonomic, Safety vs Passage

In §III-B2, the modified PG method with full FoV did not achieve perfect navigation as for the PG holonomic case in §III-B1. This is a function of the lack of full control, the tuning of the projection operator, and some non-determinism in the outcomes. There is a trade-off between tuning for task completion versus safety. Figure 10 shows the effect of varying the parameter in (8). For lower values there are less aborts or time outs, but at the cost of increased collisions. For higher values there are more time outs and some aborts, but no collisions. The results hold for the two FoV cases tested, which represent the lowest reasonably safe and available FoV for popular range-based sensors, and the maximal FoV settings. Future work aims to address the trade-off by exploring how to dynamically adjust the parameter.

Iii-B5 Timing

The median per trajectory compute time of potential gap was less than 1ms, while TEB was just under 10ms, leading to an order of magnitude difference. The average processing time per frame for PG was 8ms, half that of TEB (17ms). Recall that, on average, PG evaluated 5x more paths than TEB per invocation.

Iv Conclusion

Potential gap is a local planner, designed using ideas from artificial potential field theory to create a spatially localized gradient descent problem with guarantees on safe passage under an idealized robot model with full sensing. Central to the idea is the use of perception space to identify gaps for constructing the local gradient flow. Deviations from this model, such as field of view limitations and nonholonomic motion models, require modifications of the baseline method to recover lost safety and performance properties. Simulated scenarios demonstrate that the method achieves high completion rates and matches the safety of an optimal trajectory synthesis method but with faster runtime and higher completion rate. The code is open-sourced

[34].

The study is a first step towards provably safe, hierarchical navigation with realistic robot models and configurations. Additional investigation is needed to strengthen the safety properties for the nonholonomic case. Lastly, more aggressive maneuvering with strong momentum effects were not considered. Further study is needed to explore how the potential gap method may operate under these conditions.

References

  • [1] A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, and P. Tabuada (2019-06) Control barrier functions: theory and applications. In ECC, Vol. , pp. 3420–3431. External Links: ISSN Cited by: §I-A4.
  • [2] S. Bansal, M. Chen, S. Herbert, and C. J. Tomlin (2017) Hamilton-jacobi reachability: a brief overview and recent advances. In CDC, Vol. , pp. 2242–2253. External Links: ISSN Cited by: §I-A4.
  • [3] J. Barraquand, B. Langlois, and J. -. Latombe (1992) Numerical potential field techniques for robot path planning. T-SMC 22 (2), pp. 224–241. External Links: Document Cited by: §I-A2.
  • [4] J. Borenstein and Y. Koren (1991-06) The vector field histogram-fast obstacle avoidance for mobile robots. IEEE Trans. on Rob. and Aut. 7 (3), pp. 278–288. External Links: Document, ISSN Cited by: §I-A1.
  • [5] D.E. Chang and J.E. Marsden (2003) Gyroscopic forces and collision avoidance with convex obstacles. In New Trends in Nonlinear Dynamics and Control and their Applications, W. Kang, C. Borges, and M. Xiao (Eds.), pp. 145–159. Cited by: §II-B.
  • [6] C. I. Connolly, J. B. Burns, and R. Weiss (1990) Path planning using laplace’s equation. In ICRA, Vol. , pp. 2102–2106 vol.3. Cited by: §I-A2.
  • [7] V. Dimitrov et al. (2013-10) Hierarchical navigation architecture and robotic arm controller for a sample return rover. In T-SMC, Vol. , pp. 4476–4481. External Links: Document, ISSN 1062-922X Cited by: §I-A1.
  • [8] D. Fox, W. Burgard, and S. Thrun (1997-03) The dynamic window approach to collision avoidance. RA-M 4 (1), pp. 23–33. External Links: Document, ISSN Cited by: §I-A1.
  • [9] S. S. Ge and Y. J. Cui (2000) New potential functions for mobile robot path planning. IEEE Trans. on Rob. and Aut. 16 (5), pp. 615–620. External Links: Document Cited by: §I-A2.
  • [10] J. Guldner, V. I. Utkin, and R. Bauer (1994-Sep.) Mobile robots in complex environments: a three-layered hierarchical path control system. In IROS, Vol. 3, pp. 1891–1898. External Links: Document, ISSN Cited by: §I-A1.
  • [11] O. Khatib (1985) Real-time obstacle avoidance for manipulators and mobile robots. In ICRA, Vol. , pp. 500–505. Cited by: §I-A2, §I.
  • [12] D. Koditschek (1987) Exact robot navigation by means of potential functions: some topological considerations. In ICRA, pp. 1–6. Cited by: §I-A2.
  • [13] Y. Koren and J. Borenstein (1991) Potential field methods and their inherent limitations for mobile robot navigation. In ICRA, Vol. , pp. 1398–1404. Cited by: §I-A2, §I.
  • [14] S. M. LaValle (2006) Planning algorithms. Cambridge university press. Cited by: §I-A1.
  • [15] E. Lavretsky and K. Wise (2013) Robust and adaptive control. Advanced Textbooks in Control and Signal Processing, Springer. Cited by: §II-D4.
  • [16] G. Li, A. Yamashita, H. Asama, and Y. Tamura (2012) An efficient improved artificial potential field based regression search method for robot path planning. In ICMA, Vol. , pp. 1227–1232. Cited by: §I-A2.
  • [17] A. T. M. Tsardoulias (2014)(Website) External Links: Link Cited by: §III-A.
  • [18] L. Matthies, R. Brockers, Y. Kuwata, and S. Weiss (2014) Stereo vision-based obstacle avoidance for micro air vehicles using disparity space. ICRA, pp. 3242–3249. Cited by: §I-A3.
  • [19] M. Mujahad, D. Fischer, B. Mertsching, and H. Jaddu (2010) Closest gap based (cg) reactive obstacle avoidance navigation for highly cluttered environments. In IROS, Vol. , pp. 1805–1812. Cited by: §I-A3, Fig. 3, §II-A4, §II-A.
  • [20] M. Mujahed, D. Fischer, and B. Mertsching (2013) Safe gap based (sg) reactive navigation for mobile robots. In ECMR, Vol. , pp. 325–330. Cited by: §I-A3.
  • [21] M. Mujahed, H. Jaddu, D. Fischer, and B. Mertsching (2013) Tangential closest gap based (tcg) reactive obstacle avoidance navigation for cluttered environments. In SSRR, Vol. , pp. 1–6. Cited by: §I-A3.
  • [22] M. Mujahed and B. Mertsching (2017) The admissible gap (ag) method for reactive collision avoidance. In ICRA, Vol. , pp. 1916–1921. Cited by: §I-A3.
  • [23] M. Park, J. Jeon, and M. Lee (2001) Obstacle avoidance for mobile robots using artificial potential field approach with simulated annealing. In ISIE, Vol. 3, pp. 1530–1535 vol.3. Cited by: §I-A2.
  • [24] S. Patel, Sang-Hack Jung, J. P. Ostrowski, R. Rao, and C. J. Taylor (2002) Sensor based door navigation for a nonholonomic vehicle. In ICRA, pp. 3081–3086. Cited by: §I.
  • [25] C. Rösmann, F. Hoffmann, and T. Bertram (2015-07) Timed-elastic-bands for time-optimal point-to-point nonlinear model predictive control. In ECC, Vol. , pp. 3352–3357. External Links: Document, ISSN Cited by: §I-A1.
  • [26] J. Saunders, R. Beard, and J. Byrne (2009) Vision-based reactive multiple obstacle avoidance for micro air vehicles. In ACC, Vol. , pp. 5253–5258. Cited by: §I.
  • [27] V. Sezer and M. Gokasan (2012-07) A novel obstacle avoidance algorithm:follow the gap method. In RAS, Vol. 60, pp. 1123–1134. Cited by: §I-A3.
  • [28] J. S. Smith and P. A.Vela (2017-05) PiPS: planning in perception space. In ICRA, Vol. , pp. 6204–6209. External Links: Document, ISSN Cited by: §II-D1.
  • [29] J. S. Smith, R. Xu, and P. Vela (2020) egoTEB: Egocentric, Perception Space Navigation Using Timed-Elastic-Bands. In ICRA, Vol. , pp. 2703–2709. External Links: Document Cited by: §I-A1, §I-A3, §I-A3, §I-A3, §II-A1, §II-A, §II-D1, §III-A.
  • [30] J. S. Smith, S. Feng, F. Lyu, and P. A. Vela (2020) Real-time egocentric navigation using 3d sensing. In Machine Vision and Navigation, O. Sergiyenko, W. Flores-Fuentes, and P. Mercorelli (Eds.), pp. 431–484. External Links: ISBN 978-3-030-22587-2, Document Cited by: §I-A1, §I-A3, §I, §II-C, §II-D1.
  • [31] T. Y. Teck, M. Chitre, and P. Vadakkepat (2010-06) Hierarchical agent-based command and control system for autonomous underwater vehicles. In ICoIAS, Vol. , pp. 1–6. External Links: Document, ISSN Cited by: §I-A1.
  • [32] M. Teiner, I. Rojas, K. Goser, and O. Valenzuela (2003) A hierarchical fuzzy steering controller for mobile robots. In Int. Workshop on Scientific Use of Submarine Cables and Related Technologies, Vol. , pp. 7–12. External Links: Document, ISSN Cited by: §I-A1.
  • [33] T. Urakubo, K. Okuma, and Y. Tada (2004) Feedback control of a two wheeled mobile robot with obstacle avoidance using potential functions. In IROS, pp. 2428–2433. Cited by: §I-A2, §I.
  • [34] R. Xu, S. Feng, and P.A. Vela Potential gap. Note: https://github.com/ivaROS/potential_gap Cited by: §IV.
  • [35] D. J. Zhu and J. -. Latombe (1991)

    New heuristic algorithms for efficient hierarchical path planning

    .
    IEEE Trans. on Rob. and Aut. 7 (1), pp. 9–20. Cited by: §I.