T*ε – Bounded-Suboptimal Efficient Motion Planning for Minimum-Time Planar Curvature-Constrained Systems

We consider the problem of finding collision-free paths for curvature-constrained systems in the presence of obstacles while minimizing execution time. Specifically, we focus on the setting where a planar system can travel at some range of speeds with unbounded acceleration. This setting can model many systems, such as fixed-wing drones. Unfortunately, planning for such systems might require evaluating many (local) time-optimal transitions connecting two close-by configurations, which is computationally expensive. Existing methods either pre-compute all such transitions in a preprocessing stage or use heuristics to speed up the search, thus foregoing any guarantees on solution quality. Our key insight is that computing all the time-optimal transitions is both (i) computationally expensive and (ii) unnecessary for many problem instances. We show that by finding bounded-suboptimal solutions (solutions whose cost is bounded by 1+ε times the cost of the optimal solution for any user-provided ε) and not time-optimal solutions, one can dramatically reduce the number of time-optimal transitions used. We demonstrate using empirical evaluation that our planning framework can reduce the runtime by several orders of magnitude compared to the state-of-the-art while still providing guarantees on the quality of the solution.

READ FULL TEXT VIEW PDF

page 1

page 6

page 7

06/15/2022

State Supervised Steering Function for Sampling-based Kinodynamic Planning

Sampling-based motion planners such as RRT* and BIT*, when applied to ki...
11/10/2017

Anytime Motion Planning on Large Dense Roadmaps with Expensive Edge Evaluations

We propose an algorithmic framework for efficient anytime motion plannin...
08/09/2020

Neural Manipulation Planning on Constraint Manifolds

The presence of task constraints imposes a significant challenge to moti...
03/07/2022

A Fast Approach to Minimum Curvature Raceline Planning via Probabilistic Inference

Finding a racing path that allows minimum lap time is the first and fore...
12/29/2020

Alternative Paths Planner (APP) for Provably Fixed-time Manipulation Planning in Semi-structured Environments

In many applications, including logistics and manufacturing, robot manip...
07/09/2020

Computing High-Quality Clutter Removal Solutions for Multiple Robots

We investigate the task and motion planning problem of clearing clutter ...
11/20/2017

Bayesian Active Edge Evaluation on Expensive Graphs

Robots operate in environments with varying implicit structure. For inst...

I Introduction

In this work, we study the problem of finding minimal-time collision-free paths for curvature-constrained systems with variable speed. Curvature constraints are prevalent in a variety of systems (see, e.g., [12, 28, 20, 6]). Unfortunately, determining whether a collision-free curvature-constrained path exists is NP-hard even for a planar system [1]. As a result, this continuous problem can be discretized into a graph data structure using sampling-based [29] or search-based approaches [24], which is then queried to obtain a discrete path representing a curvature-constrained solution in the continuous space. The graph’s vertices correspond to robot configurations (i.e., -dimensional points that uniquely describe the robot’s position and orientation) and edges correspond to local motions taken by the robot.

(a)
(b)
(c)
Fig. 1: LABEL:sub@fig:transitions_compare-a, LABEL:sub@fig:transitions_compare-b The time-optimal and bounded-suboptimal path obtained by T* [22] and by T* using an approximation factor of , respectively. The red and green segments correspond to settings where the system travels at the minimum and maximum speed, respectively. The colored arrows show the orientation of each configuration in the solution. T* finds a solution whose cost is  larger than the cost of the optimal solution found by T*, but the runtime of T* is faster by a factor of roughly . LABEL:sub@fig:transitions_compare-c Given a start orientation (diagonal on the left or horizontal on the right) in an eight-connected grid, there are 68 unique transitions in total (after taking into account symmetry and rotation) to the adjacent grid cells. The speedup of T* is obtained by computing only a small subset of the time-optimal transitions that are all computed by T*. Here, T* computes only five transitions, four of which are used in the found path. Figure best viewed in color.

Interestingly, the computational bottleneck in these search algorithms is a frequent computation of local time-optimal transitions between neighboring vertices, obtained using numerical optimization. Our key insight, depicted in Fig. 1, is that computing all the time-optimal transitions is both (i) computationally expensive and (ii) unnecessary for many problem instances. We introduce a novel algorithmic framework, which we call T*, that allows finding bounded-suboptimal solutions while reducing planning times by orders of magnitude compared to the state-of-the-art.

Our framework consists of three algorithmic components. The first component assumes that we have an efficient-to-compute lower bound on the cost of the optimal transition between two configurations. In Sec. VI, we demonstrate two such bounds—when the environment does not and does contain dynamics such as wind currents. The second algorithmic component is the A*-based search algorithm [17] that uses these lower bounds on optimal transitions, together with a user-provided approximation factor , to choose which edges to consider. Roughly speaking, the search attempts to use only transitions for which the true (computationally expensive) cost was computed while guaranteeing the bound on the quality of the solution obtained. Finally, the third component is a heuristic approach to pre-compute a small set of transitions that are likely to be used in an optimal path.

Using efficient-to-compute lower bounds on the cost of local transitions is a common technique to speed up motion-planning algorithms (see, e.g., [2, 10, 3, 21, 15]). These are used to minimize the computation time taken to compute the cost of an edge or transition, also known as an edge evaluation. Typically, edge evaluation corresponds to computing if a robot intersects an obstacle while performing some local motion (also known as collision detection [14]). Thus, the computationally expensive operation of edge evaluation is applied to each edge individually. Indeed, it can be shown that under some mild assumptions, these approaches allow minimizing the number of edges evaluated [7].

In our case, we plan in a discretized space; thus, the setting is somewhat different. There is a fixed set of transitions that can be taken from any given configuration. Since these transitions are computationally expensive to compute, it is natural to try and re-use transitions that have already been computed. The challenge is how this should be done while ensuring that the cost of the path found is within a given multiplicative bound of the cost of an optimal path.

As we demonstrate in simulation (Sec. VI), our planning framework allows to compute only a small fraction of transitions, which, in turn, allows us to reduce the runtime by several orders of magnitude compared to the state-of-the-art, especially in dynamic conditions.

Ii Related Work

We now continue to review related work on planning for minimum-time planar curvature-constrained systems. When the system is constrained to travel at a single speed, an optimal path connecting two configurations (i.e., two planar locations, each associated with its angular heading) can be computed analytically [4]. In this setting, an optimal path, also known as Dubins path, is one of six types: RSR, RSL, LSR, LSL, RLR, and LRL, where R and L refer to right and left turns, respectively, and S refers to going straight. As the system travels at a single speed, the shortest and the time-optimal paths are identical.

The kinematic model considered in this work is the setting where the system can travel at some range of speeds but with unbounded acceleration (i.e., transitioning between low and high-speed can be done instantaneously). It is not hard to see that, in such a setting, the shortest path is attained by computing the optimal Dubins path, assuming that the system travels at the minimum speed (as the system can travel using the smallest turning radius possible and can thus better maneuver). Computing the time-optimal path requires alternating between low-speed  (to allow for tighter turns) and high-speed motions (to allow for faster progress).

Wolek et al. [27] showed that it is sufficient to consider only the two extreme speeds and identified a sufficient set of 34 candidate paths (in contrast to the six-candidate paths when the system travels at a single speed). Each candidate path contains circular arcs and straight-line segments on which the vehicle can travel at either of the two extreme speeds. However, in contrast to Dubins paths, which can be computed analytically, computing these time-optimal paths for a variable-speed system requires a numerical optimization that is (i) much more computationally expensive and (ii) may return locally optimal solutions (and not globally optimal ones). Kučerová et al. [13] showed an efficient heuristic approach to find high-quality paths when considering time as the cost function by using multiple turning radii. However, there is no guarantee regarding the quality of these paths.

For each model mentioned above, motion-planning algorithms that account for environmental obstacles were introduced. These include both sampling-based approaches such as the work by Wilson et al. [26], search-based methods such as the work by Song et al. [22], and hybrid methods that borrow ideas from both motion-planning disciplines [19]. Of specific interest to the presented work is T* [22], a time-optimal risk-aware motion-planning algorithm that obtains a time-optimal solution but requires a time-consuming preprocessing phase. As our work builds upon the algorithmic foundation of T*, we describe it in Sec. IV. Following T*, Wilson et al. [25] developed a fast motion-planning algorithm that uses Dubins paths of various speeds to reduce the computational effort and runtime of T*. For the settings evaluated, the costs of solutions obtained by this algorithm are near-optimal, but there are no guarantees regarding the quality of solutions.

The aforementioned kinematic model can be extended to account for external dynamic changes such as wind or ocean currents. For such systems, a minimum-time path can be computed similarly to the setting where no wind exists [23]. However, only two candidate path types have analytic solutions (RSR and LSL). Mittal et al. [16] used those two types to compute a path between two vehicle poses under ocean currents. These kinematic models were used not only in the context of single-goal motion planning problems but also in the settings where the objective is to reach multiple goals [5] and further extended to account for some notion of rewards [18].

Iii Problem Statement

Our problem formulation follows Song et al. [22]. Specifically, we assume a variable-speed curvature-constrained planar robotic system with unbounded acceleration. The system’s dynamics can be described by

(1)

Here,  is the robot’s placement and orientation,  is the robot’s speed (which is considered as a control input) and  is the second control input dictating the system’s turning rate via maximal lateral acceleration  that is determined for the specific system111For example, for fixed-wing vehicles, where is the gravitational acceleration and the maximum allowed bang angle. as

(2)

The speed is limited by some minimum and maximum values denoted by and , respectively. W.l.o.g., we assume that . Thus, the system is constrained to travel between two extreme radii:

(3)

We assume that the continuous workspace is discretized into a grid according to a predefined resolution. Each cell can be categorized as free or forbidden corresponding to locations that the system can and cannot occupy, respectively. In addition, we assume that every robot motion starts and ends at the center of a cell. Specifically, at each step, the robot transitions to one of its eight neighbors and with one of eight possible orientations.

A path  is a sequence of configurations where the transition between them obeys the system’s dynamics and constraints (Eq. 1). It is said to be collision free if it only occupies free cells. The cost  of a path  is the step-wise cost of the transitions between any consecutive configurations in . It can be computed using the system velocity along . Namely,

(4)

where  refers to the speed along the path segment .

Given start and goal configurations , a collision-free path  is said to be optimal if it (i) starts at  and ends at ; and (ii) there is no path  connecting  and  such that . Similarly, it is said to be bounded-suboptimal for some approximation factor  if it (i) starts at  and ends at ; and (ii) for any path  connecting  and .

While previous works (see, e.g., [22]) were concerned with finding optimal paths, in this work, we are interested in finding bounded-suboptimal paths for a user-provided approximation factor . As we will see, the extra flexibility obtained by finding bounded-suboptimal paths allows us to reduce running times by orders of magnitude with little compromise on the path quality in practice.

Iv Algorithmic Background

As both A* [17] and T* [22] serve as the algorithmic foundation of our work, we provide a brief description of these two algorithms.

Iv-a A* A-star epsilon)

A* (also known as Focal search) [17] is a bounded-suboptimal search algorithm based on the celebrated A* algorithm [9]. Like A*, it searches a graph by continuously expanding nodes, starting from the start node until the goal node is reached. To ensure optimality, A* orders nodes in a priority list called Open. Nodes in the Open list are ordered according to their -value that is the sum of the cost to reach the node from the source (also known as the cost-to-come or -value and denoted by for a node

) added to a conservative estimate of the cost to reach the goal (also known as the cost-to-go or 

-value and denoted by for a node ). Namely for a node , its -value is

(5)

Unlike A*A* also uses a so-called Focal list containing all nodes from the Open list whose -values are no larger than  times the smallest -value in the Open list. It can be shown that expanding any sequence of nodes from the Focal list ensures that the cost of the final solution will indeed be bounded by a factor of  when compared to the cost of the optimal solution.

Iv-B T* (T-star)

T* [22] builds on the approach by Wolek et al. [27] that allows finding an optimal222 To be precise, the solutions computed are only an approximation of the time-optimal solutions since the computation is based on numerical optimizations, which may not converge to a global optimum. Thus, the optimality of T* and the bounded-suboptimality of T* is only with respect to (w.r.t.) the quality of the local-optimization. solution for the time-optimal transition between any two configurations in an obstacle-free space. These transitions are used to compute all possible motions for an agent in a discretized eight-connected grid. In this space, there is a finite set of possible transition types. For example, a transition can be moving from configuration  to configuration , which corresponds to moving diagonally in the NE direction while changing the system’s heading.

To this end, there are a total of  possible transitions (eight possible start and target orientations and eight neighboring cells). However, after accounting for symmetry and rotation, there are only 68 unique transitions.

After pre-computing all possible transitions, an A*-like search is used to find a minimal-cost path. In their original work, Song et al. [22] consider both minimal-time paths and a cost-function that balances the time and the risk of colliding with an obstacle. However, we consider only minimum-time paths in this work and refer to T* as the search algorithm that optimizes this criterion.

It is important to note that in many settings it is not possible to pre-compute all transitions in an offline phase. This is because optimization-related parameters might be available only when a query is provided. For example, in the presence of wind conditions, which affect the system’s dynamics and hence the transitions’ computation, wind parameters are only available to the planner when a query is provided. For further details, see Sec. VI.

Interestingly, after extensive empirical evaluation, we noticed that: (i) pre-computing all possible transitions last two orders of magnitude more than the A*-like search; (ii) only a small fraction of all possible time-optimal transitions are part of an optimal path found by T*; and (iii) Dubins path computed using the minimal speed  (whose cost is a lower bound on the length of the time-optimal path) is often in the same homotopy class as the optimal path and shares a significant portion of its transitions. These observations are key to our algorithmic framework described in the following section.

V Algorithmic framework—T*sec:tstareps

V-a Preliminaries

Let  denote a set of possible transitions. By a slight abuse of notation, we assume that we have access to two functions  and . The first, which is expensive-to-compute, corresponds to evaluating the true cost of the transition according to Eq. 4. The second, which is fast-to-compute, corresponds to evaluating a lower bound on the transition cost. Namely, . Once the cost of a transition  is evaluated using , the value is stored in a cache-like data structure for later use. Thus, there is no need to compute it again when evaluating the same transition later.

To this end, let  be an indicator function corresponding to the cases where the cost of a transition  (i) has not been evaluated using , i.e.,  or (ii) has been evaluated using , i.e., . We start our algorithm with the setting that .333The reason we chose to define  in this manner is that we start with  for all nodes and order nodes lexicographically in the Focal list where lower means better.

V-B Algorithmic Description

Input:
Output: bounded-suboptimal path connecting
S1. compute  for transitions in shortest path
  minimal-length collision-free Dubins path on map connecting to   using  AlgoLine0.1
for each transition  do AlgoLine0.2
  compute , cache result and set  AlgoLine0.3
S2. A*-like search
run A* from  (node with state ) with Open and Focal ordered according to Eq. 5 and 7, respectively AlgoLine0.4
when expanding a node  doAlgoLine0.5
  if incoming transition  was not computed () thenAlgoLine0.6
    compute , cache result, set AlgoLine0.7
    continueAlgoLine0.8
  if configuration associated with the node  is  thenAlgoLine0.9
    return path associated with AlgoLine0.10
  for each succesor node  of  in map do AlgoLine0.11
     use Eq. 6 to evaluate the -value of  AlgoLine0.12
Algorithm 1 T*
12

12

12

12

12

12

12

12

12

12

12

12

Our algorithmic framework, summarized in Alg. 1, consists of the following two main steps.

  • Heuristically computing the true cost for a small set of transitions.

  • Finding a bounded-suboptimal solution while trying to minimize the number of calls to .

In the first step S1 (lines LABEL:tstareps-shortestLABEL:tstareps-update_kappa), we start by finding the shortestcollision-free Dubins path  (line LABEL:tstareps-shortest) from  to  that uses the minimum speed . For every transition  taken along this path, we compute its true cost , cache it, and set  (lines LABEL:tstareps-for_each_transitionLABEL:tstareps-update_kappa).

In the second step S2 (lines LABEL:tstareps-astarepsLABEL:tstareps-set_g), we run an A*-like search to find a bounded-suboptimal path from  to  given some approximation factor . We base our search algorithm on A* because we can use it to heuristically guide the search to expand nodes for which the incoming time-optimal transition has already been computed.

Formally, each node  considered by the search is associated with a parent node  except for the start node  associated with , which has no parent. In addition, each node stores the time-optimal transition  used to get from  to . When expanding a node  (namely, computing successor nodes and inserting them into the OPEN list), the true cost  of the time-optimal transition leading to  is computed. However, the cost used to reach a successor node  via transition  is (i)  if  and (ii)  if . Note that for some nodes in the Open list, the true cost of the time-optimal transition associated with them may not have been computed. However, for all expanded nodes, the true cost of the incoming time-optimal transition is computed at some point.

To this end, if we set  to be the cost-to-come (-value) of the start node, then the cost-to-come of any other node  in the Open list is

(6)

The -value of the node  (denoted by  and used to prioritize nodes in the Open list) is defined in Eq. 5. can be any admissible estimate of the cost to reach the goal. In our setting, we use the length of the minimum speed Dubins path divided by the vehicle’s maximum speed.

Recall (Sec. IV) that A* uses a Focal list that contains all nodes whose -value is at most  times the -value of the minimal-cost node in the Open list. In our setting, nodes in Focal are lexicographically sorted using the following key (from low to high):

(7)

Thus, any node  for which  (i.e., the true cost  of the transition leading to  has been computed) is always prioritized before any node  for which  (i.e., the true cost  of the transition leading to  has not been computed), regardless of their respective -values. Among all nodes with identical values of , nodes with smaller -values are prioritized. Once a node  is chosen for expansion, the true cost of  is computed if it has not been computed beforehand. Finally, a path is found once a node associated with the goal configuration  is removed from the Open list.

Following the theoretic properties of A* [17] and using the fact that  we have the following Corollary.

Corollary 1

Let  and  be some function that bounds from below the true cost  of any time-optimal transition. T*, using  is bounded-suboptimal with an approximation factor of .

Vi Evaluation

In this section, we report on empirical evaluation of our approach in a simulated environment inspired by Song et al. [22]. We start (Sec. VI-A) by evaluating our heuristic approach for computing the true cost for a small set of transitions (step S1). Then we continue (Sec. VI-B) to demonstrate how the lower bound  needs to be both informative (i.e., as close as possible to the real cost ) and computationally cheap-to-compute (as its main purpose is to save computation time) by evaluating several different lower bounds that balance these two traits. We then move to compare our motion-planning approach with T* in static environmental conditions (Sec. VI-C), i.e., when the system dynamics are known before the query is provided and are independent of the specific location at which the robot resides, and in dynamic environmental conditions such as when wind currents exist (Sec. VI-D). Finally, we briefly discuss and qualitatively compare T* with alternative approaches to find paths that attempt to minimize the execution time (Sec. VI-E).

All experiments were run on a Intel Core i9 processor with of memory. Benchmarks, system parameters, and C++ implementation of all the algorithms used in our work are publicly available444 https://github.com/CRL-Technion/tstar-epsilon.. Throughout Sec. VI-AVI-C, we test the performance of our algorithm across  randomly generated scenarios of cells and using 

, where for a given scenario, each cell has a probability of 

of being blocked, and the start and goal configurations are chosen uniformly at random while ensuring that a solution exists. When reporting algorithmic attributes, e.g., solution cost or runtime, the values represent the average across the 

scenarios. In some cases, we also report confidence intervals that include two standard deviations from the mean. In Sec. 

VI-D, we provide additional information on the experiment setup.

Vi-a Evaluating the Efficacy of step S1

Fig. 2: Average number of calls to by T* as a function of  with (blue) or without (purple) step S1, respectively. Recall that there are at most  possible transitions.

To evaluate the efficacy of the step S1, wherein we provide a heuristic approach for computing the true cost for a small set of transitions, we report the number of calls to  by T* as a function of the approximation factor  with and without this initial step (Fig. 2).

Observe that for values of  larger than , using S1 allows reducing the average number of calls to  by roughly . Moreover, in this case, for large approximation factors, the average calls to  converges to the number of transitions in . When the step S1 is not invoked, T* is not “bootstrapped” with a good set of pre-computed time-optimal and thus evaluates unnecessary transitions.

Vi-B Computing Lower Bounds—Balancing Computation Time with Informative Lower Bounds

Recall that in Sec. LABEL:sec:tstareps, we assumed that we have access to —an efficient-to-compute tight lower bound on , the true cost of the time-optimal transition. However, there is an inherent tradeoff between how informative a lower bound is (which immediately corresponds to how accurately it can guide the search) and its computation time (which can negate the effectiveness of the lower bound).

We evaluated T* using three different lower bounds that demonstrate this behavior: (i) Euclidean distance divided by ; (ii) length of minimum-speed Dubins path divided by ; and (iii) length of minimum-speed Dubins path divided by  while accounting for environment obstacles. Note that all lower bounds are divided by , thus always underestimating the true cost of a transition. In addition, they are ordered from low to high according to their computation time and how informative they are.

(a)
(b)
Fig. 3: Comparing the effect of different approaches for lower bounds ( in Eq. 6) for several values of the approximation factor . (a) Average number of calls to  and (b) Average running time of T*, respectively.
(a)
(b)
(c)
(d)
Fig. 4: Comparison of T* and T* in static environmental conditions over  randomly chosen start and goal configurations. LABEL:sub@fig:no_wind_results-a Solution found by T* and by T* with  for one representative experiment. The beginning of the solution found by both algorithms is identical, while the end differs and is highlighted on the right-hand side. LABEL:sub@fig:no_wind_results-b Average solution cost of T* compared to T* as a function of . LABEL:sub@fig:no_wind_results-c Breakdown of the running time of T* and T* for different values of . LABEL:sub@fig:no_wind_results-d Speedup in running time of T* over T* obtained with and without pre-computing all time-optimal transitions.

We compare (Fig. 3) the three lower bounds w.r.t. the number of time-optimal transitions computed by T* and its running time. As expected, the Euclidean distance is the least-informative lower bound, thus resulting in the maximal number of time-optimal transitions computed for all values of . Moreover, even though it is efficient-to-compute, it does a poor job in guiding the search, and thus the runtime is high. Computing Dubins path while accounting for obstacles is more informative but also more expensive-to-compute. It does result in the smallest number of calls to  but has longer running times when compared to using Dubins paths that do not account for obstacles. The latter also has a comparable number of calls to . Thus, we use Dubins paths without obstacles as the lower bound in the rest of the evaluation.

Vi-C Motion Planning in Static Environmental Conditions

We compare T* with T* in static environmental conditions, i.e., where the system dynamics are invariant to the robot’s placement and orientation in the scenario. As we can see in Fig. (c)c, computing time-optimal transitions dominate the running time of all algorithms. If the system dynamics are known in advance, these can be pre-computed in an offline step by T*, and it outperforms T* for all by a factor of between  and . However, if we account for this preprocessing time, T* dramatically reduce the run time by a factor ranging between and . It is worth noting that in either case, T* finds a solution with an average cost that is much lower than the guaranteed suboptimality bound.

Vi-D Motion Planning in Dynamic Environmental Conditions

(a)
(b)
(c)
Fig. 5: Comparing T* and T* in dynamic environmental conditions where the magnitude and direction of wind currents (blue arrows) are given at the query time. LABEL:sub@fig:wind_results_large_benchmark-a Representative solution obtained on one randomly-generated map with randomly-generated start and goal configurations, wind conditions and minimum velocity . LABEL:sub@fig:wind_results_large_benchmark-b Running time breakdown of T* and T* for the selected values of . LABEL:sub@fig:wind_results_large_benchmark-c Average solution cost compared to the cost of the optimal solution and speedup in running time as a function of .

In the previous section, we considered static environmental conditions and showed that using an approximation factor allows reducing T*’s planning times dramatically. However, if the system’s dynamics are known in advance, T* may perform all its computationally-expensive operations in a preprocessing stage. In contrast, this is not the case when the system’s dynamics are not known in advance. It can be due to changes in the maximal speed that the system can take (due to, e.g., payload changes of the system or regulatory changes happening just before a query is received) or due to dynamic environmental conditions such as wind currents whose magnitude and direction is known only when a query is received. We use the latter case to demonstrate the efficacy of T*. In particular, we assume that the magnitude of wind currents is uniform across a given scenario.

It is worth noting that this dynamic setting, analyzed by Techy and Woolsey [23], can be seen as a generalization of the setting described in Sec. III and by Mittal et al. [16]. To account for this more-involved dynamic setting, we first note that one cannot use rotation and symmetry between transitions to reduce the total amount of unique transitions. Thus, in this setting, the total number of time-optimal transitions is 512. Moreover, this requires an adaptation of how time-optimal transitions are computed (see the Appendix for a description of the new model). This adaptation, based on the code for computing time-optimal transitions [27], is publicly available555https://github.com/CRL-Technion/Variable-speed-Dubins-with-wind. The second notable change is that computing lower bounds (namely, when calling , requires some care, and the modifications are detailed in the Appendix.

For dynamic environmental conditions, we used the same randomly generated scenarios as described earlier. For each such scenario, we generated instances with varying wind and minimum velocity values. In particular, we sampled wind conditions from and minimum velocity values . In total, we ran experiments and the averaged results appear in  Fig. 5. The most computationally demanding component in both T* and T* is still computing the time-optimal transitions, which requires two orders of magnitude more time than any other algorithmic component. However, we observe that the approximation factor allows for a dramatic reduction of the running time with little compromise on the quality of the solution. For example, as it can be seen in Fig (c)c, using , T* guarantees a solution whose cost is at most twice  the cost of the optimal solution, but returns a path whose average cost is no more than . It is done while obtaining a speedup in run time by an average factor of . In general, as depicted in Fig. (c)c, increasing results in a dramatic running-time improvement at the cost of slightly lower solution quality.

Vi-E Discussion—Alternative Approaches

Wilson et al. [25] reduced computational effort by allowing the system to follow Dubins paths but using several radii, and evaluated this approach when using three different radii. This heuristic approach reduces runtime, produces near-optimal solutions but with no guarantees regarding their quality. From our comparison of T* and Wilson’s model in static environments, we found that running times are faster by several orders of magnitude, but path costs are larger by an average of . However, when evaluating their approach in windy conditions, the speedup in running time is comparable to T* (as local transitions cannot be computed analytically and a computationally expensive root-finding problem needs to be solved [23]), and the quality of paths remain higher (again, with no formal guarantees).

An alternative approach is to compare our work with the approach by Mittal et al. [16] that specifically accounted for efficient computation of paths in dynamic environments. However, the main objective of their approach is online motion-planning under ocean currents, and no importance was given to the quality of paths. When comparing T* with their approach on representative environments, the path quality obtained by T* was higher (lower execution times) by a factor of more than .

Vii Conclusions and Future Work

In this work, we present T*, a novel algorithmic framework for efficiently finding bounded-suboptimal solutions for time-optimal motion-planning. This is done by minimizing the number of computations of time-optimal transitions used by the system. When one cannot pre-compute the time-optimal transitions in advance, this reduces the planning times by orders of magnitude compared to the state-of-the-art.

Future work includes adapting our work for efficient re-planing (e.g., when the wind changes while the system is in motion or dramatic payload changes during trajectory execution changing the system’s constraint). Here, we plan to adapt LPA* [11] to account for minimizing the number of transitions used.

Another natural extension is to account for settings when the approximation factor  cannot be determined a priori by the user. Here, it is natural to use an anytime [8] approach. Instead of providing a fixed , the user would provide an upper bound for the algorithm’s running time. We suggest starting with an initially high value of the approximation factor  to compute an initial solution quickly and then progressively decreases as the time permits while reusing transitions computed in earlier search episodes.

Appendix

Fig. 6: Average ratio between each lower bound and the time-optimal cost as a function of the wind’s modulus (solid lines). Colored regions represent a  non-parametric confidence interval of the true value.

To consider the setting where we need to account for wind currents, we detail the updated dynamics model and present two approaches to compute lower bounds on transitions cost-efficiently.

Vii-a Updated Model

We assume that there is a constant wind  that changes the original system dynamics (Eq. 1) to

(8)

Furthermore, we assume that regardless of the magnitude of the wind, the vehicle will move forward. It is ensured by the additional assumption that the wind magnitude is smaller than the minimum speed of the system , i.e.,

(9)

Vii-B Updated Lower Bounds

The wind component may significantly influence the turning capabilities (in the reference frame of the ground). The smallest turning radius is obtained when flying directly against the wind. We denote it by , the lower bound radius, and have that

(10)

To this end, a lower bound on the true length of the path between two configurations  and  can be determined based on the length  of Dubins path with the lower bound radius . Now, if this value is divided by an upper bound on the effective maximal speed  in the reference frame of the ground, it provides a lower bound on the true cost (i.e., the travel time).

We suggest two lower bounds based on how  is determined. The first approach assumes that . Namely, the effective maximal speed is the sum of the system’s maximum speed and the wind magnitude. Thus, the first lower bound  is defined as

(11)

The second lower bound  considers the direction between the start and end configurations ( and , respectively) by computing the ground speed  in the given direction . The direction is determined based on the configurations  and  as , where  stands for the  coordinates of the configuration . The ground speed vector can be expressed by the system’s aerial speed  and wind  as .

Following Eq. 9, the maximum ground speed occurs at the system’s maximum speed, i.e., . The angle between  and  can be written as  Subsequently, the ground speed can be expressed using the cosine rule as  Finally, the second lower bound  is defined as

(12)

Fig. 6 shows the average ratio between each lower bounds and the true cost as a function of the wind force. For both lower bounds, their estimation of the true cost becomes looser as the wind force increases. Notice that  is more informative than  because accounting for the direction enables reducing the upper bound on the ground speed. In addition, notice that both lower bounds reduce to the system’s minimum-speed Dubins path divided by the maximum speed in static environmental conditions. Thus, we used  as lower bound for dynamic environmental conditions.

References

  • [1] P. K. Agarwal, T. Biedl, S. Lazard, S. Robbins, S. Suri, and S. Whitesides (2002) Curvature-constrained shortest paths in a convex polygon. SIAM J. Comput. 31 (6), pp. 1814–1851. Cited by: §I.
  • [2] R. Bohlin and L. E. Kavraki (2000) Path planning using lazy PRM. In ICRA, Vol. 1, pp. 521–528. Cited by: §I.
  • [3] C. Dellin and S. Srinivasa (2016) A unifying formalism for shortest path problems with expensive edge evaluations via lazy best-first search over paths with edge selectors. In ICAPS, Vol. 26. Cited by: §I.
  • [4] L. E. Dubins (1957) On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. Am. J. Math. 79 (3), pp. 497–516. Cited by: §II.
  • [5] J. Faigl, P. Váňa, M. Saska, T. Báča, and V. Spurnỳ (2017) On solution of the dubins touring problem. In ECMR, pp. 1–6. Cited by: §II.
  • [6] B. Garau, M. Bonet, A. Alvarez, S. Ruiz, and A. Pascual (2009) Path planning for autonomous underwater vehicles in realistic oceanic current fields: application to gliders in the western mediterranean sea. J. Marit. Res. 6 (2), pp. 5–22. Cited by: §I.
  • [7] N. Haghtalab, S. Mackenzie, A. Procaccia, O. Salzman, and S. Srinivasa (2018) The provable virtue of laziness in motion planning. In ICAPS, Vol. 28. Cited by: §I.
  • [8] E. A. Hansen and R. Zhou (2007) Anytime heuristic search. J. Artif. Intell. Res. 28, pp. 267–297. Cited by: §VII.
  • [9] P. E. Hart, N. J. Nilsson, and B. Raphael (1968) A formal basis for the heuristic determination of minimum cost paths. IEEE Trans. Syst. Man Cybern. 4 (2), pp. 100–107. Cited by: §IV-A.
  • [10] K. Hauser (2015) Lazy collision checking in asymptotically-optimal motion planning. In ICRA, pp. 2951–2957. Cited by: §I.
  • [11] S. Koenig, M. Likhachev, and D. Furcy (2004) Lifelong planning A*. Artif. Intell. 155 (1-2), pp. 93–146. Cited by: §VII.
  • [12] L. B. Kratchman, M. M. Rahman, J. R. Saunders, P. J. Swaney, and R. J. Webster (2011) Toward robotic needle steering in lung biopsy: a tendon-actuated approach. In Medical Imaging: Image-Guided Procedures, Vol. 7964, pp. 79641I. Cited by: §I.
  • [13] K. Kučerová, P. Váňa, and J. Faigl (2021) Variable-speed traveling salesman problem for vehicles with curvature constrained trajectories. In IROS, pp. 4714–47194714–4719. External Links: Document Cited by: §II.
  • [14] S. M. LaValle (2006) Planning algorithms. Cambridge University Press. External Links: Link Cited by: §I.
  • [15] A. Mandalika, S. Choudhury, O. Salzman, and S. Srinivasa (2019) Generalized lazy search for robot motion planning: interleaving search and edge evaluation via event-based toggles. In ICAPS, Vol. 29, pp. 745–753. Cited by: §I.
  • [16] K. Mittal, J. Song, S. Gupta, and T. A. Wettergren (2020) Rapid path planning for dubins vehicles under environmental currents. Robotics Auton. Syst. 134, pp. 103646. Cited by: §II, §VI-D, §VI-E.
  • [17] J. Pearl and J. H. Kim (1982) Studies in semi-admissible heuristics. IEEE Trans. Pattern Anal. Mach. Intell (4), pp. 392–399. Cited by: §I, §IV-A, §IV, §V-B.
  • [18] R. Pěnička, J. Faigl, and M. Saska (2019) Physical orienteering problem for unmanned aerial vehicle data collection planning in environments with obstacles. RA-L 4 (3), pp. 3005–3012. Cited by: §II.
  • [19] E. Plaku (2013) Robot motion planning with dynamics as hybrid search. In AAAI, Cited by: §II.
  • [20] D. Sadigh, S. Sastry, S. A. Seshia, and A. D. Dragan (2016) Planning for autonomous cars that leverage effects on human actions. In RSS, Vol. 2, pp. 1–9. Cited by: §I.
  • [21] O. Salzman and D. Halperin (2016) Asymptotically near-optimal RRT for fast, high-quality motion planning. IEEE Trans. Robotics 32 (3), pp. 473–483. Cited by: §I.
  • [22] J. Song, S. Gupta, and T. A. Wettergren (2018) T: time-optimal risk-aware motion planning for curvature-constrained vehicles. RA-L 4 (1), pp. 33–40. Cited by: Fig. 1, §II, §III, §III, §IV-B, §IV-B, §IV, §VI.
  • [23] L. Techy and C. A. Woolsey (2009) Minimum-time path planning for unmanned aerial vehicles in steady uniform winds. J. Guid. Control Dyn. 32 (6), pp. 1736–1746. Cited by: §II, §VI-D, §VI-E.
  • [24] N. Wang, D. Zhang, L. Zhou, and Q. Liu (2010) Near optimal path planning for vehicle with heading and curvature constraints. In WCICA, pp. 4514–4519. Cited by: §I.
  • [25] J. P. Wilson, K. Mittal, and S. Gupta (2019) Novel motion models for time-optimal risk-aware motion planning for variable-speed AUVs. In OCEANS MTS/IEEE, pp. 1–5. Cited by: §II, §VI-E.
  • [26] J. P. Wilson, Z. Shen, S. Gupta, and T. A. Wettergren (2020) T-lite: a fast time-risk optimal motion planning algorithm for multi-speed autonomous vehicles. In OCEANS MTS/IEEE, pp. 1–6. Cited by: §II.
  • [27] A. Wolek, E. M. Cliff, and C. A. Woolsey (2016) Time-optimal path planning for a kinematic car with variable speed. J. Guid. Control Dyn. 39 (10), pp. 2374–2390. Cited by: §II, §IV-B, §VI-D.
  • [28] H. Xiang and L. Tian (2011) Development of a low-cost agricultural remote sensing system based on an autonomous unmanned aerial vehicle (UAV). Biosystems Engineering 108 (2), pp. 174–190. Cited by: §I.
  • [29] Z. Zeng, L. Lian, K. Sammut, F. He, Y. Tang, and A. Lammas (2015) A survey on path planning for persistent autonomy of autonomous underwater vehicles. O.E. 110, pp. 303–313. Cited by: §I.