Task-Motion Planning for Safe and Efficient Urban Driving

Abstract

Autonomous vehicles need to plan at the task level to compute a sequence of symbolic actions, such as merging left and turning right, to fulfill people's service requests, where efficiency is the main concern. At the same time, the vehicles must compute continuous trajectories to perform actions at the motion level, where safety is the most important. Task-motion planning in autonomous driving faces the problem of maximizing task-level efficiency while ensuring motion-level safety. To this end, we develop algorithm Task-Motion Planning for Urban Driving (TMPUD) that, for the first time, enables the task and motion planners to communicate about the safety level of driving behaviors. TMPUD has been evaluated using a realistic urban driving simulation platform. Results suggest that TMPUD performs significantly better than competitive baselines from the literature in efficiency, while ensuring the safety of driving behaviors.

Claims

1. A method of operating a vehicle, comprising: automatically planning motion of the vehicle with an automated processor, comprising computing motion trajectories of an action to incrementally advance the vehicle toward a goal with an associated incremental utility, based on at least a safety of the motion trajectories with respect to an environment of operation of the computed motion trajectories; and automatically planning the task for the vehicle with the automated processor, comprising defining the goal and a sequence of the actions to advance the vehicle toward the goal, selectively dependent an optimization of an aggregate prospective utility of the task and the safety of the motion trajectories to advance the vehicle toward the goal.

2. The method according to claim 1, wherein said planning a task comprises: specifying a task planning domain by D.sup.t, including a set of states, S, and a set of actions, A; providing a factored state space such that each state s∈S is defined by values of a fixed set of variables, and each action a∈A is defined by its preconditions and effects; and defining a utility function which maps a state transition to a real number, which takes both a cost function Cost(s,a,s′) and a safety function Safe(s,a,s′) of conducting action a in state s into account.

3. The method according to claim 2, further comprising: computing a plan p∈P, given domain D.sup.t and a task planning problem, is computed, starting from an initial state s.sup.init∈S and finishing in a goal state s.sup.g∈S; representing a plan P, consisting of a sequence of transitions represented as p=s.sub.0, a.sub.0, . . . , s.sub.N-1, a.sub.N-1, s.sub.N, where s.sub.0=s.sup.init,s.sup.N=s.sup.g and P denotes a set of satisfactory plans; and producing an optimal plan p* with a task planner P.sup.t among all the satisfactory plans, where γ is a constant coefficient and γ>0, $p^{*}=\underset{p\in P}{\arg \min }\underset{.Math.s,a,s^{\prime }.Math.\in p}{.Math.}\left[\mathrm{Cost}\left(.Math.s,a,s^{\prime }.Math.\right)+\frac{\gamma }{1+e^{\mathrm{Safe}\left(.Math.s,a,s^{\prime }.Math.\right)-1}}\right].$

4. The method according to claim 1, further comprising conducting a search directly in a two-dimensional Cartesian space such that a position and an orientation of the vehicle is uniquely represented as a pose, denoted by x and constrained by the urban road network, wherein some parts of the space are designated as free space, and remaining parts are designated as obstacles, and a motion planning domain is specified by D.sup.m, wherein given domain D.sup.m, a motion planning problem is specified by an initial pose x.sup.i and a goal pose x.sup.g.

5. The method according to claim 4, further comprising planning the motion by a motion planner P.sup.m consisting of a path planner and a tracking planner into two phases, wherein: in a first phase, the path planner computes a collision-free trajectory ξ connecting the initial pose x.sup.i and the goal pose x.sup.g taking into account any motion constraints on the part of the vehicle with a minimal trajectory length; in a second phase, computing control signals with a tracking controller to drive the vehicle to follow the computed trajectory; and mapping a systolic state s with a state mapping function, ƒ:X=ƒ(s), into a set of feasible poses X in a continuous space as available options for the motion planner, wherein availability of at least one pose x∈X is assumed in each state s, such that if state s is feasible, the vehicle is in a free space of D.sup.m, and if state s is infeasible, the vehicle is not in a free space of D.sup.m.

6. The method according to claim 1, further comprising: computing a safety level, Safe (s,a,s′), of a motion-level implementation of a symbolic action s,a,s′, wherein the safety level enables the task planner to incorporate a road condition into a process of sequencing high-level actions toward accomplishing complex driving tasks; and computing a sequence of continuous control signals to perform symbolic action s,a,s′, comprising an acceleration δ∈Δ and steering angle θ∈Θ, to drive the vehicle following a trajectory, while ensuring no collision on the road, wherein sets Δ and Θ denote an operation specification of the tracking controller, wherein U.sub.s(t)⊂Δ×Θ specifies a safe control set at time t, in which all elements, denoted by u(t)=(δ,θ), are safe for the vehicle to perform at time t, such that a probability of elements sampled from set Δ×Θ being located in the safe set U.sub.s represents the safety value of action s,a,s′.

7. The method according to claim 1, further comprising: receiving an input which includes a symbolic action s,a,s′, stating mapping function ƒ, motion planner P.sup.m consisting of path planner and tracking controller, and a tracking controller's operation specification sets Δ and Θ; obtaining short-period trajectories of the vehicle and surrounding vehicles, where V.sub.i,i∈[1, . . . , N], is the ith vehicle within a sensing range of the vehicle; iteratively: computing a safety estimation between the vehicle and the surrounding vehicles V.sub.i, where i∈[1, . . . , N] given that the vehicle is performing action s,a,s′ at a motion level; computing a safe control set U.sub.i.sup.s(t) that includes all safe control signals with regard to the vehicle V.sub.i at time t; randomly sampling M elements from the set Δ×Θ, and computing a probability o.sub.i(t) of the sampled elements falling in set U.sub.i.sup.s(t); converting a list of values of safety estimation {o.sub.i(t)} into a single value o*.sub.i according to $o_i^{*}=\frac{{\max }_{t\in T}\left\{o_i\left(t\right)\right\}+{\mathrm{mean}}_{t\in }\left\{o_i\left(t\right)\right\}}{2};$ and selecting a minimum value, o*.sub.i,i∈[1, . . . , N], as an overall safety value, where σ=t.sub.1+ω×i, $0\le i\le \frac{\left(t_2-t_2\right)}{\omega };$ and producing an output of an estimated safety value Safe(s,a,s′)∈[0.0,1.0].

8. The method according to claim 1, further comprising: receiving inputs: symbolic action s,a,s′, state mapping function ƒ, motion planner P.sup.m, and control operation sets Δ and Θ; sampling initial and goal poses x←ƒ(s) and x′←ƒ(s′), given action s,a,s′, and ƒ; computing a collision-free trajectory, ξ.sup.E, using P.sup.m, where ξ.sup.E(t.sub.1)=x,ξ(t.sub.2)=x′; and [t.sub.1,t.sub.2] is the horizon; predicting a trajectory ξ.sub.i.sup.s for an ith surrounding vehicle V.sub.i, where i∈[1, . . . , N], and [t.sub.1,t.sub.2] is the horizon; for each surrounding vehicle V.sub.i: computing a safe control set U.sub.i.sup.s(t) between the vehicle and vehicle V.sub.i at time t∈[t.sub.1,t.sub.2] where U.sub.i.sup.s(t)⊂Δ×Θ and $t=t_1+\omega \times i,i\le \frac{\left(t_2-t_2\right)}{\omega }$ sampling M elements δ,θ, randomly from set Δ×Θ and computing a probability o.sub.i(t) of the elements falling in set U.sub.i.sup.s(t); and converting a list of estimated safety values, {o.sub.i(t)}, into a scalar value o*.sub.i using $o_i^{*}=\frac{{\max }_{t\in T}\left\{o_i\left(t\right)\right\}+{\mathrm{mean}}_{t\in }\left\{o_i\left(t\right)\right\}}{2};$ and selecting a minimum value, o*.sub.i,i∈[1, . . . , N], as a safety value.

9. The method according to claim 1, further comprising: computing both costs and safety values of the vehicle's navigation actions with a motion planner P.sup.m to produce motion trajectories, and generating control signals to move the vehicle; computing a sequence of symbolic actions with a task planner P.sup.t, employing a cost function Cost, and a safety estimation function Safe; and mapping symbolic states to 2D coordinates in continuous spaces using a state mapping function ƒ.

10. The method according to claim 1, further comprising: initializing a cost function and a safety estimation function; computing an optimal task plan, p*=s.sup.init,a.sub.0,s.sub.1, . . . ,s.sup.g wherein s.sup.init corresponds to an initial pose and s.sup.g corresponds to a goal pose; estimating a safety level, μ, of action s,a,s′; updating the safety estimation function using μ and the cost function using p*; and computing a new optimal plan p′.

11. The method according to claim 10, further comprising: computing and executing a desired control signal δ,θ repeatedly until the vehicle reaches the goal pose with a motion planner having an output P.sup.m; receiving inputs comprising: an initial state s.sup.i, a goal specification s.sup.g, a task planner P.sup.t, a state mapping function ƒ, and a safety estimator; initializing a cost function Cost with sampled poses
x∈ƒ(s):Cost(s,a,s′)←A*(x,x′) initializing a safety estimation function Safe with Safe(s,a,s′)←1.0; computing an optimal task plan p using Cost and Safe functions:
p←P.sup.t(s.sup.init,s.sup.g,Cost,Safe) where p=s.sup.inita.sub.0,s.sub.1,a.sub.1, . . . ,s.sup.g until plan p is not empty: extracting a first action of p′s,a,s′, and computing a safety value μ; updating the Safe function: Safe(∈a,s′)←μ and the Cost function: Cost(s,a,s′)←A*(x,x′) generating a new plan: p′←P.sup.t(s, s.sup.g, Cost, Safe); and if p′==p then x′←ƒ(s′), and while x!=x′, calling the motion planner δ,θ←P.sup.m(x,x′), executing a control signal δ,θ, and updating the vehicle's current pose x; removing a tuple s,a from plan p; else updating current plan p←p′.

12. The method according to claim 1, wherein the task is planned with a task planner P.sup.t, implemented using Answer Set Programming (ASP).

13. The method according to claim 1, wherein the environment of operation comprises surrounding vehicles, wherein the surrounding vehicles are in motion.

14. The method according to claim 1, wherein the utility optimization comprises minimizing with a travel distance of the vehicle while maintaining a margin of safety.

15. The method according to claim 1, wherein the vehicle is an autonomous vehicle, and the task comprises navigating a route, further comprising determining a safety with respect to the environment of the vehicle dependent on real-time conditions of operation, said planning the task comprising selecting motion trajectory options consistent with the task that are safe according to a risk tolerance criterion, wherein the selected options are responsive to a cost of a maneuver, a benefit of the maneuver, and a determined safety of the maneuver; further comprising controlling the autonomous vehicle according to the planned motion.

16. The method according to claim 1, further comprising automatically updating the planning of motion and planning of the task in real time dependent on real-time conditions of operation of the vehicle.

17. The method according to claim 1, wherein the safety is automatically statistically determined by the automated processor based on a predicted risk, comprising a risk of collision.

18. The method according to claim 1, further comprising: receiving data relating to a relationship of the vehicle with respect to the environment of operation; determining the safety of the motion trajectories comprising a motion and environment-dependent safety of the vehicle within the environment of operation dependent on the received data; said automatically planning the task comprising continuously planning a utility-optimized route for the vehicle along a path toward the goal having execution options within the route, updated dependent on the determined motion and environment-dependent safety of the autonomous vehicle; the utility-optimized route comprising a selection of the execution options which alter a relation of the vehicle with the environment of operation, that meet at least one safety criterion with respect to the determined a motion and environment-dependent safety; and controlling the vehicle according to the utility-optimized route and automatically planned motion, to thereby achieve safe and efficient advancement of the vehicle toward the goal.

19. A non-transitory computer readable medium containing a program for operating a vehicle, comprising: instructions for planning motion of the vehicle, comprising computing motion trajectories of an action to incrementally advance the vehicle toward a goal with an associated incremental utility, based on at least a safety with respect to an environment of operation of the computed motion trajectories; and instructions for planning the task for the vehicle, comprising defining the goal and a sequence of the actions to advance the vehicle toward the goal, selectively dependent an optimization of an aggregate prospective utility of the task and the safety of the motion trajectories to advance the vehicle toward the goal.

20. A system for operating a vehicle, comprising: a sensor configured to receive information about an environment of operation of the vehicle; at least one automated processor, configured to: implement a motion planner to plan a motion of the vehicle, by computation of motion trajectories to implement an action to incrementally advance the vehicle toward a goal with an associated incremental utility, based on at least a safety of the motion trajectories with respect to an environment of operation of the vehicle; and implement a task planner to plan the task for the vehicle, comprising defining the goal and a sequence of the actions to advance the vehicle toward the goal, selectively dependent an optimization of an aggregate prospective utility of the task and the safety of the motion trajectories to advance the vehicle toward the goal; and an output configured to control the vehicle according to the planned motion.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0244] FIGS. 1A and 1B show a risky situation for the vehicle to merge left due to the busy traffic (FIG. 1A), and a safe situation for the vehicle to merge left (FIG. 1B).

[0245] FIG. 2 shows an overview of the TMPUD algorithm that consists of four components, the task planner, plan manager, motion planner and safety estimator.

[0246] FIG. 3 shows an illustrative example of operation of the system, where the vehicle is tasked with driving from the very left to the top-right area.

[0247] FIGS. 4A-4D show results of an abstraction simulation which compare the overall performances of TMPUD and two baseline methods.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

Example 1

[0248] FIG. 2 shows an overview of algorithm TMPUD that consists of four components, i.e., task planner, plan manager, motion planner and safety estimator. The task planner includes components of goal specification, action description, and utility function, where users' service requests are received by the goal specification component. The task planner computes a sequence of symbolic actions that are passed to the plan manager. The plan manager generates navigation goals (i.e., a pair of two poses) to path planner, which is then used for computing a continuous trajectory for connecting 2D poses. The trajectory will be used in the two components of safety estimator and tracking controller. Safety estimator uses this trajectory to estimate actions' safety levels, and then the utility function in task planner can be updated accordingly. Tracking controller computes the desired control signals to drive the vehicle to follow the trajectory from the path planner.

[0249] FIG. 3. An illustrative example, where the vehicle is tasked with driving from the very left to the top-right area. The vehicle needs to compute plans at both task and motion levels. The vehicle starts with executing Plan A (blue color). While it is getting close to Area 1 (a red circle), the motion-level safety estimator reports a low safety value based on the local road condition. This computed safety value is incorporated into task planner's cost function. Using the updated cost function, the task planner re-computes an optimal plan (Plan B), and suggests the vehicle to go straight and merge left in Area 2. The interaction between task and motion levels, supported by TMPUD, enables the vehicle to dynamically adjust its high-level task plans to avoid unsafe behaviors.

[0250] TMPUD starts with using an optimal task planner to compute Plan A. The vehicle takes the first symbolic action from Plan A (trajectory in blue color), and executes the action using the motion planner. Getting close to Area 1, the vehicle plans to merge left. However, the safety estimator at the motion level reports a low safety value in Area 1. This computed safety value is incorporated into task planner, where the task planner integrates the safety value into its cost function, and re-computes an optimal plan, Plan B. Different from Plan A, Plan B suggests the vehicle to go straight, and merge left in Area 2. In this trial, the vehicle was able to follow Plan B all the way to the goal. TMPUD enabled the vehicle to avoid the risky behavior of merging left in Area 1 without introducing extra motion cost. See, youtu.be/8NHQYUqMyoI.

[0251] Experiments conducted in CARLA are referred as being in full simulation. All vehicles move at a constant speed (20 km/h) on average. In full simulation, ego vehicle performs the whole plan at the task level in the presence of other vehicles. Different numbers of vehicles (200 and 120) are spawned, and traffic of the two environments referred to as being heavy and normal respectively.

[0252] Running full simulation using CARLA is time-consuming, preventing conducting of large-scale experiments. For instance, results are based on tens of thousands of experimental trials, and full simulation in this scale would have required months of computation time. To conduct large numbers of experimental trials, an abstract simulation platform was developed, where action outcomes are sampled from pre-computed probabilistic world models. Parameters of the world models (for abstract simulation) are learned by repeatedly spawning the ego and surrounding vehicles in a small area, and statistically analyzing the results of the vehicles' interaction.

[0253] In particular, a large effort was made in analyzing the outcomes of “merging lane” actions due to its significant potential risks. The probabilities of the three different outcomes of “merging lane” actions were empirically computed, including “merge”, “collide”, and “stop”. Two domain factors were introduced into the abstract simulation platform, including density and acceleration. In high-density environments, the ego vehicle is surrounded by three vehicles, while this number is reduced to one in low-density environments. In high-acceleration environments, surrounding vehicles' acceleration (in m/s2) is randomly sampled in [−1.0, 1.0], while this range is [−0.5, 0.5] in low-acceleration environments.

[0254] The goal of TMPUD is to improve task-completion efficiency (to reduce traveling distance), while guaranteeing safety. So, the two most important evaluation metrics are traveling distance and the number of unsafe behaviors, where unsafe behaviors cause either collisions or force at least one surrounding vehicle to stop (to avoid collisions).

[0255] The two baseline methods are selected from the literature, and referred to as No-communication (No-com), and Threshold-based (Th-based). The No-com baseline [13] forces the vehicle to execute all task-level actions at the motion level, while driving behaviors' safety values are not considered. The Th-based baseline [8] enables the motion planner to “reject” a task-level action when its safety value is lower than a threshold β, where a higher (lower) β threshold makes a vehicle more conservative (aggressive). In case of an action being rejected, the task planner will compute a new plan to avoid the risky action. Three versions of the Th-based baseline were developed with different β values (0.1, 0.3, and 0.5).

TABLE-US-00001 TABLE I FULL SIMULATION: TRAVELING DISTANCE AND NUMBER OF COLLISIONS AND STOPS FOR THREE ALGORITHMS UNDER DIFFERENT TRAFFIC CONDITIONS (NORMAL AND HEAVY TRAFFIC). Num. of Travelling collisions Algorithm Distance (m) and stops Normal Traffic TMPUD 514 0 Th-based β = 0.5 537 0 β = 0.3 513 5 β = 0.1 478 24 No-com 426 48 Heavy Traffic TMPUD 530 2 Th-based β = 0.5 545 2 β = 0.3 528 7 β = 0.1 497 35 No-com 426 54

[0256] Results from Full Simulation

[0257] Table I presents the results in comparing TMPUD to the two baseline methods. As shown in the table, in both road conditions, TMPUD achieved the lowest traveling distance, in comparison to those methods that produced compared safety levels (in terms of the number of collisions and stops). For instance, under normal traffic, only the Th-based baseline with β=0.5 was able to completely avoid collisions and stops, but it produced an average traveling distance of 537 m. In comparison, TMPUD required only 514 m, while completely avoided collisions and stops. Under heavy traffic, TMPUD (again) produced the best performance in safety (based on the number of collisions and stops), while requiring less traveling distance in comparison to the only baseline (Th-based with β=0.5) that produced comparable performance in safety. The experimental trials (200 for each approach) from full simulation took eight full workdays.

[0258] FIGS. 4A-4D show abstraction simulation of the overall performances of TMPUD and two baseline methods. The x-axis represents the average traveling distance, and the y-axis represents the total number of collisions and stops. FIGS. 4A-4D correspond to four different road conditions. The road conditions, low-density and low-acceleration (FIG. 4A), low-density and high-acceleration (FIG. 4B), high-density and low-acceleration (FIG. 4C), high-density and high-acceleration (FIG. 4D). Under each road condition, each algorithm was evaluated using 4000 trials. Batch-based evaluations were employed with four batches for significance analysis, where each batch includes 1000 trials.

[0259] Results from Abstract Simulation

[0260] FIGS. 4A-4D present the performances of TMPUD and the baseline methods in both traveling distance and the number of unsafe behaviors. The x-axis corresponds to the average traveling distance, and y-axis corresponds to the total number of collisions and stops (both are considered failure cases of driving behaviors). From FIGS. 4A-4D, TMPUD is shown to be the most efficient (x-axis) among those methods that produced comparable performances in safety (y-axis), except that Th-based (β=0.5) produced slightly less unsafe behaviors (but it performed poorly in efficiency).

[0261] There are a few side observations. Not surprisingly, No-com produced the worst performance of in safety (y-axis), though its traveling distance remains the lowest. This is because, using No-com, the vehicle blindly executes task-level actions while unrealistically believing driving behaviors are always safe. The Th-based baseline's performance depends on its safety threshold (β), where a greater value produces safer but less efficient behaviors. The results show that TMPUD improves vehicles' task-completion efficiency, while ensuring safety in different road conditions.

[0262] Focusing on urban driving scenarios, both a safety evaluation algorithm, and a task-motion planning algorithm, called TMPUD, are provided for autonomous driving. TMPUD bridges the gap between task planning and motion planning in autonomous driving. TMPUD was extensively evaluated using a 3D urban driving simulator (CARLA) and an abstract simulator. Results suggest that TMPUD improves the task-completion efficiency in different road conditions, while ensuring the safety of driving behaviors.

[0263] TMPUD may also be implemented using different task and motion planners, and these in turn may be evaluated in different testing platforms (e.g., using simulators with a physics engine) under different conditions. The technology may be applied to various autonomous mobile platforms, such as robots, drones.

[0264] A phrase such as an “aspect” does not imply that such aspect is essential to the subject technology or that such aspect applies to all configurations of the subject technology. A disclosure relating to an aspect may apply to all configurations, or one or more configurations. An aspect may provide one or more examples. A phrase such as an aspect may refer to one or more aspects and vice versa. A phrase such as an “embodiment” does not imply that such embodiment is essential to the subject technology or that such embodiment applies to all configurations of the subject technology. A disclosure relating to an embodiment may apply to all embodiments, or one or more embodiments. An embodiment may provide one or more examples. A phrase such an embodiment may refer to one or more embodiments and vice versa. A phrase such as a “configuration” does not imply that such configuration is essential to the subject technology or that such configuration applies to all configurations of the subject technology. A disclosure relating to a configuration may apply to all configurations, or one or more configurations. A configuration may provide one or more examples. A phrase such a configuration may refer to one or more configurations and vice versa.

[0265] Certain units described in this specification have been labeled as modules in order to more particularly emphasize their implementation independence. A module is “[a] self-contained hardware or software component that interacts with a larger system.” Alan Freedman, “The Computer Glossary” 268 (8th ed. 1998). A module may include a machine- or machines-executable instructions. For example, a module may be implemented as a hardware circuit including custom VLSI circuits or gate arrays, off-the-shelf semiconductors such as analog circuits, quantum computers, microprocessors, logic chips, transistors, or other discrete components. A module may also be implemented in programmable hardware devices such as field programmable gate arrays, programmable array logic, programmable logic devices or the like.

[0266] Modules may also include software-defined units or instructions, that when executed by a processing machine or device, transform data stored on a data storage device from a first state to a second state. An identified module of executable code may, for instance, include one or more physical or logical blocks of computer instructions that may be organized as an object, procedure, or function. Nevertheless, the executables of an identified module need not be physically located together, but may include disparate instructions stored in different locations that, when joined logically together, include the module, and when executed by the processor, achieve the stated data transformation. A module of executable code may be a single instruction, or many instructions, and may even be distributed over several different code segments, among different programs, and/or across several memory devices. Similarly, operational data may be identified and illustrated herein within modules, and may be embodied in any suitable form and organized within any suitable type of data structure. The operational data may be collected as a single data set, or may be distributed over different locations including over different storage devices. The absence of a module reflects the inability of system including the module to execute in given circumstances to perform the function of the respective module, and not that its physical or logical constituents are excluded, that is, the module is unavailable. In the foregoing description, numerous specific details are provided, such as examples of programming, software modules, user selections, network transactions, distributed ledgers, blockchains, smart contracts, database queries, database structures, hardware modules, hardware circuits, hardware chips, etc., to provide a thorough understanding of the present embodiments. One skilled in the relevant art will recognize, however, that the invention requires a specific implementation that requires special purpose technology for implementation, that generic hardware alone will not achieve the objectives set forth herein.

[0267] As used herein, various terminology is for the purpose of describing particular implementations only and is not intended to be limiting of implementations. For example, as used herein, an ordinal term (e.g., “first,” “second,” “third,” etc.) used to modify an element, such as a structure, a component, an operation, etc., does not by itself indicate any priority or order of the element with respect to another element, but rather merely distinguishes the element from another element having a same name (but for use of the ordinal term). The term “coupled” is defined as connected, although not necessarily directly, and not necessarily mechanically; two items that are “coupled” may be unitary with each other, but in that case the unitary element must meet established criteria for each item. The terms “a” and “an” are defined as one or more unless this disclosure explicitly requires otherwise. The term “substantially” is defined as largely but not necessarily wholly what is specified (and includes what is specified; e.g., substantially 90 degrees includes 90 degrees and substantially parallel includes parallel), as understood by a person of ordinary skill in the art.

[0268] The phrase “and/or” means “and” or “or”. To illustrate, A, B, and/or C includes: A alone, B alone, C alone, a combination of A and B, a combination of A and C, a combination of B and C, or a combination of A, B, and C. In other words, “and/or” operates as an “inclusive or”. Similarly, the phrase “A, B, C, or a combination thereof” or “A, B, C, or any combination thereof” includes A alone, B alone, C alone, a combination of A and B, a combination of A and C, a combination of B and C, or a combination of A, B, and C.

[0269] As used herein, whether in the above description or the following claims, the terms “comprising,” “including,” “carrying,” “having,” “containing,” “involving,” and the like are to be understood to be open-ended, that is, to mean including but not limited to. The terms “comprise” (and any form of comprise, such as “comprises” and “comprising”), “have” (and any form of have, such as “has” and “having”), and “include” (and any form of include, such as “includes” and “including”). As a result, an apparatus that “comprises,” “has,” or “includes” one or more elements possesses those one or more elements, but is not limited to possessing only those one or more elements. Likewise, a method that “comprises,” “has,” or “includes” one or more steps possesses those one or more steps, but is not limited to possessing only those one or more steps.

[0270] Some implementations are described herein in connection with thresholds. As used herein, satisfying a threshold may refer to a value being greater than the threshold, more than the threshold, higher than the threshold, greater than or equal to the threshold, less than the threshold, fewer than the threshold, lower than the threshold, less than or equal to the threshold, equal to the threshold, etc.

[0271] The terms “about” or “approximately” are intended to denote a range for a quantitative parameter that achieves substantially the same result in the same manner, with either a predictable relation between input parameter and behavior, a statistically insignificant change in response with respect to the change between a nominal input parameter and another input parameter within the stated range of “about” or “approximately”. Thus, a feature would be outside a range of “about” or “approximately” if the result is achieved in a substantially different manner, a substantially different result is achieved, within the range statistically significant and meaningful differences in output response are achieved based on differences between the nominal parameter and the putative one which is “about” or “approximately” the same, or the result is unpredictable to an extent that the output response unpredictable deviates from the benchmark established. “Substantially” and “significant” are interpreted according to the understanding of persons of ordinary skill in the art, dependent on the context, and are intended to represent a reasonable range of quantitative difference which may be ignored or compensated without change in cause or effect.

[0272] It will be apparent that systems and/or methods, described herein, can be implemented in different forms of hardware, software, or a combination of hardware and software. The actual specialized control hardware or software code used to implement these systems and/or methods is not limiting of the implementations. Thus, the operation and behavior of the systems and/or methods are described herein without reference to specific software code, it being understood that software and hardware can be designed to implement the systems and/or methods based on the description herein. It is also understood that the algorithms are not limited by particular expressions, and rather are intended to encompass functional equivalents regardless of expression. Further, as is known and well understood, semantic expressions relating inputs or available data and output or action are themselves algorithms.

[0273] Even though particular combinations of features are recited in the claims and/or disclosed in the specification, these combinations are not intended to limit the disclosure of possible implementations. In fact, many of these features can be combined in ways not specifically recited in the claims and/or disclosed in the specification. Although each dependent claim listed below may directly depend on only one claim, the disclosure of possible implementations includes each dependent claim in combination with every other claim in the claim set.

[0274] No element, act, or instruction used herein should be construed as critical or essential unless explicitly described as such. Also, as used herein, the articles “a” and “an” are intended to include one or more items, and may be used interchangeably with “one or more.” Furthermore, as used herein, the term “set” is intended to include one or more items (e.g., related items, unrelated items, a combination of related and unrelated items, etc.), and may be used interchangeably with “one or more.” Where only one item is intended, the term “one” or similar language is used. Also, as used herein, the terms “has,” “have,” “having,” and/or the like are intended to be open-ended terms. Further, the phrase “based on” is intended to mean “based, at least in part, on” unless explicitly stated otherwise.

[0275] Any embodiment of any of the systems, methods, and article of manufacture can “consist of” or “consist essentially of”, rather than “comprise”, “have”, or “include”, any of the described steps, elements, and/or features. Thus, in any of the claims, the term “consisting of” or “consisting essentially of” can be substituted for any of the open-ended linking verbs recited above, in order to change the scope of a given claim from what it would otherwise be using the open-ended linking verb. Thus, the transitional phrases “consisting of” and “consisting essentially of,” respectively, shall be considered exclusionary transitional phrases, as set forth, with respect to claims, in the United States Patent Office Manual of Patent Examining Procedures. Additionally, the terms “wherein” or “whereby” may be used interchangeably with “where”.

[0276] Further, a device or system that is configured in a certain way is configured in at least that way, but it can also be configured in other ways than those specifically described. The feature or features of one embodiment may be applied to other embodiments, even though not described or illustrated, unless expressly prohibited by this disclosure or the nature of the embodiments.

[0277] The phrase “configured to” means a specification or clarification of the structure or composition of an element defining what the element is, by way of a specific description of its configuration and interface with other elements or an external constraint. The phrase “adapted to” means a specification or clarification of a function or relationship of an element defining what the element does, by way of a specific description of its adaptation and interface with other elements or an external constraint. Functional language within such a specification of an element within a claim is taken to be an affirmative limitation, and not a mere intended use. Functional language or context within a claim preamble is to be considered non-limiting and outside of the claim scope, unless integrated by specific reference and inclusion by the express claim scope.

[0278] The claims hereinbelow are to be construed as excluding abstract subject matter as judicially excluded from patent protection, and the scope of all terms and phrases is to be constrained to only include that which is properly encompassed. By way of example, if a claim phrase is amenable of construction to encompass either patent eligible subject matter and patent ineligible subject matter, then the claim shall be interpreted to cover only the patent eligible subject matter. The scope of the claims shall be considered definite in accordance with the ability of a judicial or administrative court or tribunal to make this determination, regardless of any retroactive or ex post facto changes in interpretation by such court or tribunal. The various disclosure expressly provided herein, in conjunction with the incorporated references, are to be considered to encompass any combinations, permutations, and subcombinations of the respective disclosures or portions thereof, and shall not be limited by the various exemplary combinations specifically described herein.

REFERENCES

[0279] [1] J.-F. Bonnefon, A. Shariff, and I. Rahwan, “The social dilemma of autonomous vehicles,” Science, vol. 352, no. 6293, pp. 1573-1576, 2016. [0280] [2] A. Geiger, P. Lenz, and R. Urtasun, “Are we ready for autonomous driving? the kitti vision benchmark suite,” in 2012 IEEE Conference on Computer Vision and Pattern Recognition. IEEE, 2012, pp. 3354-3361. [0281] [3] M. Maurer, J. C. Gerdes, B. Lenz, H. Winner et al., “Autonomous driving,” Berlin, Germany: Springer Berlin Heidelberg, vol. 10, pp. 978-3, 2016. [0282] [4] C. J. Haboucha, R. Ishaq, and Y. Shiftan, “User preferences regarding autonomous vehicles,” Transportation Research Part C: Emerging Technologies, vol. 78, pp. 37-49, 2017. [0283] [5] P. Koopman and M. Wagner, “Autonomous vehicle safety: An interdisciplinary challenge,” IEEE Intelligent Transportation Systems Magazine, vol. 9, no. 1, pp. 90-96, 2017. [0284] [6] P. Cao, Z. Xu, Q. Fan, and X. Liu, “Analysing driving efficiency of mandatory lane change decision for autonomous vehicles,” IET Intelligent Transport Systems, vol. 13, no. 3, pp. 506-514, 2018. [0285] [7] B. Paden, M. Cáp, S. Z. Yong, D. Yershov, and E. Frazzoli, “A survey of motion planning and control techniques for self-driving urban vehicles,” IEEE Transactions on intelligent vehicles, vol. 1, no. 1, pp. 33-55, 2016. [0286] [8] S. Srivastava, E. Fang, L. Riano, R. Chitnis, S. Russell, and P. Abbeel, “Combined task and motion planning through an extensible plannerindependent interface layer,” in 2014 IEEE international conference on robotics and automation (ICRA). IEEE, 2014, pp. 639-646. [0287] [9] B. Kim, L. P. Kaelbling, and T. Lozano-Pérez, “Learning to guide task and motion planning using score-space representation,” in 2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017, pp. 2810-2817. [0288] [10] C. R. Garrett, T. Lozano-Perez, and L. P. Kaelbling, “Ffrob: Leveraging symbolic planning for efficient task and motion planning,” The International Journal of Robotics Research, vol. 37, no. 1, pp. 104-136, 2018. [0289] [11] S.-Y. Lo, S. Zhang, and P. Stone, “Petlon: planning efficiently for tasklevel-optimal navigation,” in Proceedings of the 17th International Conference on Autonomous Agents and MultiAgent Systems (AAMAS), 2018, pp. 220-228. [0290] [12] A. Dosovitskiy, G. Ros, F. Codevilla, A. Lopez, and V. Koltun, “Carla: An open urban driving simulator,” in Conference on Robot Learning, 2017, pp. 1-16. [0291] [13] C. Chen, A. Gaschler, M. Rickert, and A. Knoll, “Task planning for highly automated driving,” in 2015 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2015, pp. 940-945. [0292] [14] C. Liu and M. Tomizuka, “Control in a safe set: Addressing safety in human-robot interactions,” in ASME 2014 Dynamic Systems and Control Conference. American Society of Mechanical Engineers Digital Collection, 2014. [0293] [15] C. Liu, and M. Tomizuka, “Safe exploration: Addressing various uncertainty levels in human robot interactions,” in 2015 American Control Conference (ACC). IEEE, 2015, pp. 465-470. [0294] [16] J. Chen, B. Yuan, and M. Tomizuka, “Model-free deep reinforcement learning for urban autonomous driving,” in 2019 IEEE Intelligent Transportation Systems Conference (ITSC), 2019, pp. 2765-2771. [0295] [17] J. Chen, S. E. Li, and M. Tomizuka, “Interpretable end-to-end urban autonomous driving with latent deep reinforcement learning,” arXiv preprint arXiv:2001.08726, 2020. [0296] [18] J. Chen, W. Zhan, and M. Tomizuka, “Autonomous driving motion planning with constrained iterative lqr,” IEEE Transactions on Intelligent Vehicles, vol. 4, no. 2, pp. 244-254, 2019. [0297] [19] C. Liu and M. Tomizuka, “Enabling safe freeway driving for automated vehicles,” in 2016 American Control Conference (ACC). IEEE, 2016, pp. 3461-3467. [0298] [20] J. Bi, V. Dhiman, T. Xiao, and C. Xu, “Learning from interventions using hierarchical policies for safe learning,” in Proceedings of the Thirty-Fourth AAAI Conference on Artificial Intelligence, 2020. [0299] [21] C. Paxton, V. Raman, G. D. Hager, and M. Kobilarov, “Combining neural networks and tree search for task and motion planning in challenging environments,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 6059-6066. [0300] [22] K. B. Lim, S. Park, S. Kim, J. M. Jeong, and Y.-S. Yoon, “Behavior planning of an unmanned ground vehicle with actively articulated suspension to negotiate geometric obstacles,” in 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2009, pp. 821-826. [0301] [23] C. Xiu and H. Chen, “A behavior-based path planning for autonomous vehicle,” in International Conference on Intelligent Robotics and Applications. Springer, 2010, pp. 1-9. [0302] [24] J. Wei, J. M. Snider, T. Gu, J. M. Dolan, and B. Litkouhi, “A behavioral planning framework for autonomous driving,” in 2014 IEEE Intelligent Vehicles Symposium Proceedings. IEEE, 2014, pp. 458-464. [0303] [25] C. Chen, M. Rickert, and A. Knoll, “Combining task and motion planning for intersection assistance systems,” in 2016 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2016, pp. 1242-1247. [0304] [26] S. Cambon, R. Alami, and F. Gravot, “A hybrid approach to intricate motion, manipulation and task planning,” The International Journal of Robotics Research, vol. 28, no. 1, pp. 104-126, 2009. [0305] [27] J. Wolfe, B. Marthi, and S. Russell, “Combined task and motion planning for mobile manipulation,” in Twentieth International Conference on Automated Planning and Scheduling, 2010. [0306] [28] D. S. Nau, T.-C. Au, O. Ilghami, U. Kuter, J. W. Murdock, D. Wu, and F. Yaman, “Shop2: An htn planning system,” Journal of artificial intelligence research, vol. 20, pp. 379-404, 2003. [0307] [29] E. Erdem, K. Haspalamutgil, C. Palaz, V. Patoglu, and T. Uras, “Combining high-level causal reasoning with low-level geometric reasoning and motion planning for robotic manipulation,” in 2011 IEEE International Conference on Robotics and Automation. IEEE, 2011, pp. 4575-4581. [0308] [30] A. Houenou, P. Bonnifait, V. Cherfaoui, and W. Yao, “Vehicle trajectory prediction based on motion model and maneuver recognition,” in 2013 IEEE/RSJ international conference on intelligent robots and systems. IEEE, 2013, pp. 4363-4369. [0309] [31] S. Ammoun and F. Nashashibi, “Real time trajectory prediction for collision risk estimation between vehicles,” in 2009 IEEE 5th International Conference on Intelligent Computer Communication and Processing. IEEE, 2009, pp. 417-422. [0310] [32] V. Lifschitz, “Answer set programming and plan generation,” Artificial Intelligence, vol. 138, no. 1-2, pp. 39-54, 2002. [0311] [33] S. Amiri, S. Bajracharya, C. Goktolgal, J. Thomason, and S. Zhang, “Augmenting knowledge through statistical, goal-oriented humanrobot dialog,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019, pp. 744-750. [0312] [34] Y.-q. Jiang, S.-q. Zhang, P. Khandelwal, and P. Stone, “Task planning in robotics: an empirical comparison of pddl- and asp-based systems,” Frontiers of Information Technology & Electronic Engineering, vol. 20, no. 3, pp. 363-373, 2019. [0313] [35] M. T. Emirler, I. M. C. Uygan, B. Aksun Gu{umlaut over ( )} venc, and L. Gu{umlaut over ( )} venc, “Robust pid steering control in parameter space for highly automated driving,” International Journal of Vehicular Technology, 2014. [0314] [36] S. Shah, D. Dey, C. Lovett, and A. Kapoor, “Airsim: High-fidelity visual and physical simulation for autonomous vehicles,” in Field and service robotics. Springer, 2018, pp. 621-635. [0315] [37] N. Koenig and A. Howard, “Design and use paradigms for gazebo, an open-source multi-robot simulator,” in 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)(IEEE Cat. No. 04CH37566), vol. 3. IEEE, 2004, pp. 2149-2154. [0316] [38] Rajasekaran, Sanguthevar, and Suneeta Ramaswami. “Optimal mesh algorithms for the Voronoi diagram of line segments and motion planning in the plane.” Journal of Parallel and Distributed Computing 26, no. 1 (1995): 99-115. [0317] [39] Ahmed, Nizam. “Robot Motion Planning.” (1997). [0318] [40] Hopcroft, John E., and Gordon T. Wilfong. “Reducing multiple object motion planning to graph searching.” SIAM Journal on Computing 15, no. 3 (1986): 768-785. [0319] [41] Tang, Kai. “On computing contact configurations of a curved chain.” Graphical Models and Image Processing 61, no. 6 (1999): 341-361. [0320] [42] Garrett, Caelan Reed, Tomis Lozano-Pdrez, and Leslie Pack Kaelbling. “FFRob: An efficient heuristic for task and motion planning.” In Algorithmic Foundations of Robotics XI, pp. 179-195. Springer, Cham, 2015. [0321] [43] Kaelbling, Leslie Pack, and Tomás Lozano-Pérez. “Integrated task and motion planning in belief space.” The International Journal of Robotics Research 32, no. 9-10 (2013): 1194-1227. [0322] [44] Garrett, Caelan Reed, Rohan Chitnis, Rachel Holladay, Beomjoon Kim, Tom Silver, Leslie Pack Kaelbling, and Tomis Lozano-Pdrez. “Integrated task and motion planning.” arXiv preprint arXiv:2010.01083 (2020). [0323] [45] Dantam, Neil T., Zachary K. Kingston, Swarat Chaudhuri, and Lydia E. Kavraki. “An incremental constraint-based framework for task and motion planning.” The International Journal of Robotics Research 37, no. 10 (2018): 1134-1151. [0324] [46] Rao, Nageswara S. V. “An Algorithmic Framework for Robot Navigation in Unknown Terrains.” (1988). [0325] [47] Ramaswami, Suneeta. “Algorithmic Motion Planning and Related Geometric Problems on Parallel Machines (Dissertation Proposal).” (1993). [0326] [48] Guibas, Leonidas J., Micha Sharir, and Shmuel Sifrony. “On the general motion-planning problem with two degrees of freedom.” Discrete & Computational Geometry 4, no. 5 (1989): 491-521. [0327] [49] Halperin, Dan. “On the complexity of a single cell in certain arrangements of surfaces related to motion planning.” Discrete & Computational Geometry 11, no. 1 (1994): 1-33. [0328] [50] Srivastava, Siddharth, Eugene Fang, Lorenzo Riano, Rohan Chitnis, Stuart Russell, and Pieter Abbeel. “Combined task and motion planning through an extensible planner-independent interface layer.” In 2014 IEEE international conference on robotics and automation (ICRA), pp. 639-646. IEEE, 2014. [0329] [51] Kedem, Klara, and Micha Sharir. “An efficient motion-planning algorithm for a convex polygonal object in two-dimensional polygonal space.” Discrete & Computational Geometry 5, no. 1 (1990): 43-75. [0330] [52] Lagriffoul, Fabien, Neil T. Dantam, Caelan Garrett, Aliakbar Akbari, Siddharth Srivastava, and Lydia E. Kavraki. “Platform-independent benchmarks for task and motion planning.” IEEE Robotics and Automation Letters 3, no. 4 (2018): 3765-3772. [0331] [53] Garrett, Caelan Reed, Tomis Lozano-Pdrez, and Leslie Pack Kaelbling. “FFRob: An efficient heuristic for task and motion planning.” In Algorithmic Foundations of Robotics XI, pp. 179-195. Springer, Cham, 2015. [0332] [54] Halperin, Dan, Mark H. Overmars, and Micha Sharir. “Efficient motion planning for an L-shaped object.” SIAM Journal on Computing 21, no. 1 (1992): 1-23. [0333] [55] Kaelbling, Leslie Pack, and Tomis Lozano-Pdrez. “Hierarchical task and motion planning in the now.” In 2011 IEEE International Conference on Robotics and Automation, pp. 1470-1477. IEEE, 2011. [0334] [56] Dantam, Neil T., Zachary K. Kingston, Swarat Chaudhuri, and Lydia E. Kavraki. “Incremental Task and Motion Planning: A Constraint-Based Approach.” In Robotics: Science and systems, vol. 12, p. 00052. 2016. [0335] [57] Koltun, Vladlen. “Pianos are not flat: Rigid motion planning in three dimensions.” In Symposium on Discrete Algorithms: Proceedings of the sixteenth annual ACM-SIAM symposium on Discrete algorithms, vol. 23, no. 25, pp. 505-514. 2005. [0336] [58] Lagriffoul, Fabien, Dimitar Dimitrov, Julien Bidot, Alessandro Saffiotti, and Lars Karlsson. “Efficiently combining task and motion planning using geometric constraints.” The International Journal of Robotics Research 33, no. 14 (2014): 1726-1747. [0337] [59] van der Stappen, A. Frank, Mark H. Overmars, Mark de Berg, and Jules Vleugels. “Motion planning in environments with low obstacle density.” Discrete & Computational Geometry 20, no. 4 (1998): 561-587. [0338] [60] Kornev, Ivan I., Vladislav I. Kibalov, and Oleg Shipitko. “Local path planning algorithm for autonomous vehicle based on multi-objective trajectory optimization in state lattice.” In Thirteenth International Conference on Machine Vision, vol. 11605, p. 116051I. International Society for Optics and Photonics, 2021. [0339] [61] Seccamonte, Francesco, Juraj Kabzan, and Emilio Frazzoli. “On Maximizing Lateral Clearance of an Autonomous Vehicle in Urban Environments.” In 2019 IEEE Intelligent Transportation Systems Conference (ITSC), pp. 1819-1825. IEEE, 2019. [0340] [62] Broadhurst, Adrian, Simon Baker, and Takeo Kanade. A prediction and planning framework for road safety analysis, obstacle avoidance and driver information. Carnegie Mellon University, the Robotics Institute, 2004. [0341] [63] Sisbot, Emrah Akin, Aurdlie Clodic, Rachid Alami, and Maxime Ransan. “Supervision and motion planning for a mobile manipulator interacting with humans.” In Proceedings of the 3rd ACM/IEEE international conference on Human robot interaction, pp. 327-334. 2008. [0342] [64] Best, Andrew, Sahil Narang, Daniel Barber, and Dinesh Manocha. “Autonovi: Autonomous vehicle planning with dynamic maneuvers and traffic constraints.” In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 2629-2636. IEEE, 2017.