TWI875723B - Apparatus, method and article to facilitate motion planning in an environment having dynamic objects - Google Patents

Abstract

A motion planner of a computer system of a primary agent, e.g., an autonomous vehicle, uses reconfigurable collision detection architecture hardware to perform a collision assessment on a planning graph for the primary agent prior to execution of a motion plan. For edges on the planning graph, which represent transitions in states of the primary agent, the system sets a probability of collision with another agent, e.g., a dynamic object, in the environment based at least in part on the collision assessment. Depending on whether the goal of the primary agent is to avoid or collide with a particular dynamic object in the environment, the system then performs an optimization to identify a path in the resulting planning graph with either a relatively low or relatively high potential of a collision with the particular dynamic object. The system then causes the actuator system of the primary agent to implement a motion plan with the applicable identified path based at least in part on the optimization.

Description

Devices, methods and articles for facilitating movement planning in an environment with dynamic objects

Invention Field

The present disclosure relates generally to motion planning, and more particularly to systems and methods for facilitating motion planning for an agent, such as an autonomous vehicle or other robotic agent, in an environment with dynamic objects.

Invention Background

Motion planning is a fundamental problem in robotics. Motion planning can be used to control the motion of an autonomous vehicle or to control the motion of other types of robots or parts of robots (e.g., accessories). For example, a motion plan specifies a path that an autonomous vehicle or robot or part thereof can follow from a first assembled state (e.g., a starting pose) to a target state (e.g., an ending pose) and typically without colliding with any obstacles in the operating environment or with a reduced probability of colliding with any objects in the operating environment. However, in some cases, it may be necessary to interact with objects in the operating environment in order to detect objects, collect information from objects, exchange information with objects, or even collide with objects, such as in a game. There are generally four main components involved in creating a motion plan: perception, map (also referred to herein as a motion plan map) construction, collision detection, and path finding. Each component presents challenges to overcome within the environment surrounding an autonomous vehicle or robot, which includes static objects, and specifically includes dynamic objects that move within the environment. The future movement of dynamic obstacles may also be unknown or uncertain. Such dynamic objects may move in opposition to the goal of the autonomous vehicle or other type of robot. Therefore, it is advantageous for an autonomous vehicle or other type of robot to perform motion planning to keep up with their changes in real time to avoid collisions or intercept such objects to achieve the target state.

Summary of the invention

The motion planner system may receive sensory information representing the environment in which a primary agent (e.g., an autonomous vehicle, other type of robot) operates. The motion planner system performs collision assessment on the planning graph for the primary agent before performing motion planning, which collision assessment considers the actions of other agents in the environment, including how they other agents may react to actions taken by the primary agent and to each other.

Each edge of the planning diagram represents a transition of a principal agent from one state to another in the principal agent's combination space and has an inherent or operating cost associated with the transition. The inherent or operating cost may reflect various operating parameters of the principal agent, such as fuel and/or energy usage and/or time. Each edge may have an initial weight corresponding to the respective inherent or operating cost.

For edges on the planning graph that represent state transitions of the primary agent, the system determines the probability of collision with a dynamic object in the environment based at least in part on the collision evaluation, and then modifies or adjusts the initial weight of the edge based on the probability of collision with the dynamic object. For example, the system may apply a cost function to each edge to perform a mathematical operation based on the initial weight of that edge (i.e., the weight corresponding to the inherent cost) to obtain a modified weight. This may be done by adding an additional weight to the initial assigned weight based on the collision probability, multiplying the initial assigned weight by a collision probability factor, or applying some other function or formula involving the collision probability and the initial weight corresponding to the inherent cost. As described herein, collision evaluation advantageously considers the reactions of other agents in the environment to the actions of the primary agent, as well as to each other. In addition to collision probability, the system can assign object-specific costs that are independent of collision probability, such as costs that reflect the relative importance of the objects. For example, the cost of colliding with a person can be assigned a significantly higher cost than the cost of colliding with a tree.

For example, in an instance where the goal of the primary agent is to avoid collisions with dynamic objects in the primary agent's environment, if individual edges of the planning graph have a relatively high individual probability of colliding with one or more dynamic objects, then the system may assign weights with relatively large positive values to those edges. If individual edges of the planning graph have a relatively low individual probability of colliding with one or more dynamic objects in the environment, then the system may assign weights with relatively small positive values to those edges. The system then performs optimization to identify paths in the resulting planning graph that have a relatively low probability of colliding with one or more dynamic objects in the environment in which the primary agent operates. The system then optionally causes the primary agent's actuator system to implement a motion plan that has a relatively low probability of colliding with such dynamic objects based at least in part on optimization.

Also for example, in an instance where the goal of the primary agent is to collide with a dynamic object in the primary agent's environment, the system may assign weights with relatively low positive values to individual edges of the planning graph if those edges have relatively high individual probabilities of colliding with one or more dynamic objects, and assign weights with relatively high positive values to those edges if those edges have relatively low individual probabilities of colliding with one or more dynamic objects in the environment. The system then performs optimization to identify paths in the resulting planning graph that have a relatively high probability of colliding with one or more dynamic objects in the environment in which the primary agent operates. The system then optionally causes the primary agent's actuator system to implement a motion plan that has a relatively high probability of colliding with such dynamic objects based at least in part on the optimization.

In the disclosed implementation, there is a computational strategy where every edge in the grid is initialized to "no collision". Sample the intentions of other agents, such as dynamic objects. For example, a behavioral model for each agent can be developed that treats the agent's intentions as modeled potential strategies or goals, rather than simple trajectories. The potential strategies or goals can be in a form that can be sampled to determine how the agent will react to other agent trajectories. Each agent's intention provides a trajectory t , which produces a set of trajectories S. For each sample future trajectory t in S: determine the edges in the grid that collide with t (this can be done in parallel); and increment the cost of the edge to reflect the probability of collision (e.g., if 10% of the trajectories collide with edge E, then E has a 10% chance of collision). Performs a minimum cost path search (after applying one or more cost functions that include a cost term for the probability of collision) to find a plan. The cost of an edge does not necessarily have to be a linear function of the edge's collision probability.

In an example where the primary agent's goal is to avoid collisions with specific dynamic objects, a motion planner performs optimization to identify paths in the resulting planning graph that provide a motion plan (e.g., a path of travel) for the primary agent that has a relatively low probability of colliding with such dynamic objects in the environment in which the primary agent operates. The system then causes an actuator system of the primary agent (e.g., an autonomous vehicle) to implement the motion plan with a relatively low probability of colliding with one or more objects based at least in part on the optimization.

In instances where the primary agent's goal is to collide with dynamic objects, a motion planner performs optimization to identify paths in the resulting planning graph that provide a motion plan (e.g., a path of travel) for the primary agent that has a relatively high probability of colliding with such dynamic objects in the environment in which the primary agent operates. The system then causes an actuator system of the primary agent (e.g., an autonomous vehicle) to implement the motion plan with a relatively high probability of colliding with one or more objects based at least in part on the optimization.

A motion planning method operating in a processor-based system to perform motion planning via planning graphs is described, wherein each planning graph comprises a plurality of nodes and edges, each node implicitly or explicitly representing time and variables characterizing a state of a principal agent operating in an environment including one or more other agents, and each edge represents a transition between a respective pair of nodes. The method can be summarized as including: for a current node in a first planning graph, for each trajectory in a set of trajectories representing actual or expected trajectories of at least one of one or more other agents, determining which edges of the first planning graph collide with the respective trajectory if any of the edges collides with the respective trajectory; applying a cost function to one or more of the respective edges to reflect at least one of the determined collision or its absence; for each of a plurality of candidate nodes in the first planning graph, the candidate node is any node in the first planning graph that is directly coupled to the current node in the first planning graph by a respective single edge of the first planning graph, Finding a minimum cost path from a current node to a target node in a first planning graph, the minimum cost path passing from the current node directly to respective candidate nodes and then to the target node, with or without a number of intervening nodes successively along corresponding paths between the respective candidate nodes and the target node; after finding the minimum cost path for each of the candidate nodes with respect to a trajectory in the set of trajectories, for each of the candidate nodes, calculating a respective value based at least in part on a respective cost associated with each minimum cost path across all trajectories with the respective candidate nodes; and selecting one of the candidate nodes based at least in part on the calculated respective values.

Applying a cost function to one or more of the respective edges to reflect at least one of a determined collision or its absence may include: for any of the edges determined to have collided with at least one trajectory, increasing the cost of the respective edge to a relatively high value to reflect the determined collision, wherein the relatively high value is relatively higher than a relatively low value reflecting the absence of a collision with at least one other edge.

Applying a cost function to one or more of the respective edges to reflect at least one of a determined collision or its absence may include: for any of the edges determined not to collide with at least one trajectory, increasing the cost of the respective edge to a relatively high value to reflect the determined absence of a collision, wherein the relatively high value is relatively higher than a relatively low value reflecting a collision with at least one other edge.

The method may further include: sampling each of at least one of the other agents in the environment to determine a respective expected trajectory of the other agents; and forming the set of trajectories from the determined respective actual or expected trajectories of each of the other agents.

The method may further include selecting a candidate node in the first planning graph from other nodes in the first planning graph based on the candidate node being any node in the first planning graph that is directly coupled to a current node in the first planning graph by a respective single edge of the first planning graph.

Calculating respective values based at least in part on respective costs associated with each minimum cost path across all trajectories to respective candidate nodes may include calculating an average of the respective costs associated with each minimum cost path extending from a current node to a target node through the respective candidate node and, where present, through all intervening nodes.

Selecting one of the candidate nodes based at least in part on the calculated respective values may include selecting one of the candidate nodes having a respective calculated value that is a minimum of all calculated values.

The method may further include updating the trajectory of the primary agent based on the selection of the candidate nodes.

The method may further include initializing the first planning map before applying the cost function to the respective edges to reflect the determined collisions. Initializing the first planning map may include: for each edge in the first planning map, performing a collision evaluation on the edge with respect to each of a plurality of static objects in the environment to identify a collision between the respective edge and the static object if present. Initializing the first planning map may include: for each edge evaluated to collide with at least one of the static objects, applying the cost function to the respective edge to reflect the evaluated collision or removing the edge from the first planning map. Initializing the first planning graph may include: for each node in the first planning graph, calculating the cost from the node to the target node; and logically associating the calculated costs with the respective nodes.

The method may further include: assigning a selected one of the candidate nodes as a new current node in the first planning graph; for the new current node in the first planning graph, for each trajectory in a set of trajectories representing actual or expected trajectories of at least one of one or more other agents, determining which edges of the first planning graph collide with the respective trajectory if any of the edges collides with the respective trajectory; applying a cost function to one or more of the respective edges to reflect at least one of the determined collision or its absence; and for each of a plurality of new candidate nodes in the first planning graph, the new candidate node is a node in the first planning graph that is directly coupled to the node in the first planning graph by a respective single edge of the first planning graph. any node of the new current node of the first planning graph, finding a minimum cost path from the new current node to the target node in the first planning graph, the minimum cost path being transmitted from the new current node directly to the respective new candidate nodes and then to the target node, with or without a number of intervening nodes continuously along the corresponding path between the respective new candidate nodes and the target node; after finding the minimum cost path for each of the new candidate nodes with respect to the trajectory in the set of trajectories, for each of the new candidate nodes, calculating a respective value based at least in part on the respective cost associated with each minimum cost path across all trajectories with the respective new candidate nodes; and selecting one of the new candidate nodes based at least in part on the calculated respective values.

A processor-based system for executing motion planning via planning graphs is described, wherein each planning graph comprises a plurality of nodes and edges, each node implicitly or explicitly representing time and variables characterizing a state of a primary agent operating in an environment including one or more other agents, and each edge representing a transition between a respective pair of nodes. The system may be summarized as comprising: at least one processor; and at least one non-transitory processor-readable medium storing at least one of processor-executable instructions or data that, when executed by the at least one processor, cause the at least one processor to perform any of the methods summarized above.

A method is described for operating in a motion planning system that employs a graph having nodes representing states and edges representing transitions between states. The method can be summarized as including: for each available next node relative to the current node in the first graph, calculating, via at least one processor, a respective associated representative cost of reaching a target node from the current node via the respective next node, the respective associated representative cost reflecting the respective representative cost associated with each available path from the current node to the target node via the respective next node in view of an evaluation of the probability of collision with one or more agents in the environment based on the non-deterministic behavior of each of the one or more agents in the environment, the agents may change any one or more of position, speed, trajectory, path of travel or shape over time; selecting, via at least one processor, a next node based on the calculated respective associated representative cost of each available next node; and commanding movement, via at least one processor, based at least in part on the selected next node.

Calculating the respective associated representative costs from the current node to the target node via the respective next nodes may include: for each expected path between the current node and the target node via the respective next nodes, determining the respective associated representative costs for each edge along the respective expected path between the current node and the target node; for each edge along the respective expected path between the current node and the target node, assigning the determined respective associated representative costs of each edge to the respective edges; determining the minimum cost path of the respective next node from the respective expected path between the current node and the target node via the respective next nodes based at least in part on the assigned determined respective associated representative costs; and assigning a value representing the determined minimum cost path to the respective next node.

Determining a minimum cost path to a respective next node based at least in part on respective expected paths between a current node and a target node through respective next nodes based at least in part on the assigned determined respective associated representative costs may include determining a minimum cost path including a cost of traversing from the current node to the respective next node.

Determining a respective associated representative cost for each edge along the respective expected path between the current node and the target node may include: for each edge along the respective expected path between the current target and the target node, and evaluating the risk of collision with one or more agents in the environment based on one or more probability functions that respectively represent non-deterministic behavior of each of the one or more agents in the environment.

Evaluating the risk of a collision with one or more agents in the environment based on one or more probability functions that respectively represent the non-deterministic behavior of each of the one or more agents in the environment may include sampling the probability functions that respectively represent the non-deterministic behavior of each of the one or more agents in the environment given a sequence of actions represented by a respective one of each edge between a respective next node and each successive node along a respective expected path.

Evaluating the risk of a collision with one or more agents in the environment based on one or more probability functions that respectively represent the non-deterministic behavior of each of the one or more agents in the environment may include: given a sequence of actions represented by a respective one of each edge between a respective next node and each consecutive node along a respective expected path leading to a current node, sampling the probability function that respectively represents the non-deterministic behavior of each of the one or more agents in the environment, the current node being another node along the respective expected path reached during the evaluation of the collision risk.

Evaluating the risk of a collision with one or more agents in the environment based on one or more probability functions that respectively represent non-deterministic behavior of each of the one or more agents in the environment may include: for each of the agents, repeatedly sampling a respective probability function that respectively represents the non-deterministic behavior of the respective agent. Repeatedly sampling the respective probability function that respectively represents the non-deterministic behavior of the respective agent may include repeatedly sampling the respective probability function for a number of iterations, the total number of iterations being based at least in part on an amount of time available before a command must occur.

Evaluating the risk of a collision with one or more agents in the environment based on one or more probability functions that respectively represent the non-deterministic behavior of each of the one or more agents in the environment may include: for each of the agents, repeatedly sampling the probability function that respectively represents the non-deterministic behavior of each of the one or more agents in the environment given a sequence of actions represented by a respective one of each edge between a respective next node and each successive node along a respective expected path.

Evaluating the risk of a collision with one or more agents in the environment based on one or more probability functions that respectively represent non-deterministic behavior of each of the one or more agents in the environment may include: for each of the agents, given a sequence of actions represented by each of the edges between the respective next node and each successive node along the respective expected path leading to a current node, repeatedly sampling the probability function that respectively represents the non-deterministic behavior of each of the one or more agents in the environment, the current node being another node along the respective expected path reached during the evaluation of the collision risk. The evaluation of the collision risk involves simulation of traversal of the respective expected path.

Evaluating the risk of a collision with one or more agents in the environment based on one or more probability functions that respectively represent non-deterministic behavior of each of the one or more agents in the environment may include: evaluating the collision risk based at least on the probabilistically determined respective trajectories of each of the one or more agents in the environment via dedicated risk evaluation hardware, wherein the respective associated representative costs are based at least in part on the evaluated collision risk.

Evaluating the risk of a collision with one or more agents in the environment based on one or more probability functions that respectively represent the non-deterministic behavior of each of the one or more agents in the environment may include evaluating the risk of a collision with an agent in the environment based on one or more probability functions that respectively represent the non-deterministic behavior of at least a minor one of the one or more agents in the environment, a major one of the agents being the agent for which motion planning is performed.

The method may further include: initializing the first graph before calculating respective associated representative costs to reach a target node from a current node via respective next nodes. Initializing the first graph may include: performing static collision evaluation to identify any collisions with one or more static objects in the environment; for each node in the first graph, calculating a respective cost to reach the target node from the respective node; and for each node in the first graph, logically associating the respective calculated cost to reach the target node with the respective node.

A processor-based system for performing motion planning is described that employs a graph having nodes representing states and edges representing transitions between states. The system may be summarized as comprising: at least one processor; and at least one non-transitory processor-readable medium storing at least one of processor-executable instructions or data that, when executed by the at least one processor, causes the at least one processor to perform any of the methods described above.

A method operating in a motion planning system is described that employs a graph having nodes representing states and edges representing transitions between states to generate a motion plan for a primary agent. The method can be summarized as comprising: initializing a step counter T to a starting value (T = 0); initialize the first graph; perform a simulation, the simulation comprising: starting at a current node N in the first graph instead of a target node G in the first graph: for one or more sampling iterations, for each of the one or more secondary agents in the environment, sampling from a probability function the actions that the respective secondary agent will take when the step counter increments from the starting value of the step counter to the current value (T+1, i.e., the next step length), the probability function representing the actions taken by the primary agent and by the one or more secondary agents; determining any edges of the first graph that collide with the next action; for any edges that collide with the next action The method comprises the steps of: determining an edge of the first graph and applying a respective cost function to the edge to reflect the existence of a collision condition; for each node in the first graph of a set of nodes directly connected to the current node, calculating a value representing a minimum cost path, the minimum cost path traversing one or more paths from the current node to a target node via one or more expected paths, the one or more expected paths being through respective nodes directly connected to the current node; determining whether to perform another sampling iteration; in response to determining not to perform another sampling iteration, selecting a node having a minimum cost from the nodes in the set of nodes directly connected to the current node; incrementing a step counter (T = T+1); determining whether the simulation is at a target node, in response to a determination that the simulation is not at the target node, setting a selected node as a new current node without commanding the primary agent and continuing the simulation; in response to a determination that the simulation is at the target node, selecting a node with a minimum cost from the set of nodes directly connected to the current node; and providing an identification of the selected node, the selected node having a minimum cost to command movement of the primary agent.

Sampling the actions to be taken by respective secondary agents as the step counter increments from a starting value to a current value of the step counter from probability functions representing actions taken by one or more secondary agents and the primary agent may include sampling probability functions representing non-deterministic behavior of each of the one or more secondary agents in the environment in view of a series of actions taken by the primary agent represented by respective ones of each edge between respective nodes directly connected to the current node and each successive node along a path to a target node.

Sampling a probability function that represents the non-deterministic behavior of each of one or more agents in the environment, respectively, given the sequence of actions represented by a respective one of each edge may include: for each of the agents, repeatedly sampling a respective probability function that represents the non-deterministic behavior of the respective agent, respectively.

The primary agent may be a primary autonomous vehicle. The method may further include: receiving sensory information representing an environment in which the primary autonomous vehicle operates; and a resulting motion plan implemented by the primary autonomous vehicle. Receiving the sensory information may include receiving sensory information representing a position and trajectory of at least one dynamic object in the environment. Receiving the sensory information may include receiving sensory information at the motion planner, the sensory information collected via one or more sensors carried by the primary autonomous vehicle and representing a position or trajectory of at least one other vehicle in the environment.

The method may further include identifying, by the object detector, at least a first dynamic object in the environment from sensory information collected via the one or more sensors.

A motion planning system is described that employs a graph having nodes representing states and edges representing transitions between states to generate motion plans for a primary agent. The motion planning system can be summarized as comprising: at least one processor; at least one non-transitory processor-readable medium storing processor-executable instructions that, when executed by the at least one processor, cause the at least one processor to perform any of the methods described above.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

In the following description, certain specific details are set forth in order to provide a thorough understanding of the various disclosed embodiments. However, one skilled in the relevant art will recognize that the embodiments may be practiced without one or more of these specific details, or using other methods, components, materials, etc. In other cases, well-known structures, actuator systems, and/or communication networks associated with computer systems have not been shown or described in detail to avoid unnecessarily obscuring the description of the embodiments. In other cases, well-known computer vision methods and techniques for generating sensory data and volume representations of one or more objects and the construction of occupancy grids and the like are not described in detail to avoid unnecessarily obscuring the description of the embodiments.

Unless the context requires otherwise, throughout this specification and the following claims, the word "comprise" and variations thereof, such as "comprises/comprising", should be construed to have an open, inclusive meaning, i.e., "including but not limited to".

References throughout this specification to "one implementation" or "an implementation" or "an embodiment" or "an embodiment" mean that the specific features, structures, or characteristics described in conjunction with the embodiment are included in at least one implementation or at least one embodiment. Therefore, the phrases "one implementation" or "an implementation" or "in an embodiment" or "in an embodiment" appearing in different places in this specification do not necessarily all refer to the same implementation or the same embodiment. In addition, specific features, structures, or characteristics may be combined in any suitable manner in one or more implementations or embodiments.

As used in this specification and the appended claims, the singular forms "a", "an", and "the" include plural references unless the context clearly dictates otherwise. It should also be noted that the term "or" is generally used in its sense including "and/or" unless the context clearly dictates otherwise.

References throughout this specification to a "primary agent" or "a primary agent" means the agent for which a respective motion plan is made or generated (e.g., a semi-autonomous or fully autonomous vehicle, a robot with or without movable appendages). References throughout this specification to "other agents" or "another agent" or "secondary agents" or "a secondary agent" means agents other than the primary agent for which a respective motion plan is made or generated (e.g., a semi-autonomous or fully autonomous vehicle, a robot with or without movable appendages). In some cases, other instances of motion planning may occur for these other or secondary agents, but their motion planning is not for the primary agent.

The titles and abstracts of the disclosures provided herein are for convenience only and do not interpret the scope or meaning of the embodiments.

FIG. 1 shows a dynamic operating environment 100 in which a primary agent (e.g., an autonomous vehicle, a robot with or without removable appendages) 102 can operate according to one illustrated embodiment. For the sake of brevity, the dynamic operating environment 100 is referred to herein as an environment. Although generally described with respect to autonomous vehicles, various implementations described herein are applicable to robots or portions thereof, e.g., a robot operable to navigate an environment and/or a robot with one or more removable appendages.

The environment represents a two- or three-dimensional space in which a primary agent (e.g., an autonomous vehicle) 102 can operate and move. The primary agent 102 can be a car, airplane, boat, drone, or any other vehicle, or can be another type of robot that can autonomously or semi-autonomously (i.e., at least partially autonomously) operate and move along a route or path in the space represented by the environment 100. Environment 100 is a two-dimensional or three-dimensional space in which the vehicle operates and is distinct from the "assembly space" (commonly referred to as "C-space") of the vehicle, which assembly space is referred to below with respect to the motion planning diagrams of FIGS. 4A to 5B and as explained in: International Patent Application No. PCT/US2017/036880, filed on June 9, 2017, entitled "MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS," which is hereby incorporated by reference in its entirety; and International Patent Application No. PCT/US2017/036880, filed on January 5, 2016, entitled "SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MANUFACTURE AND USE THEREOF," filed on January 5, 2016. MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME)" International Patent Application Publication No. WO 2016/122840, which is also hereby incorporated by reference in its entirety. The assembly space is typically multi-dimensional (i.e., greater than 3 dimensions). Referring to Figure 1, environment 100 may include obstacle collision regions. These obstacle collision regions may be attributed to static objects (e.g., buildings, trees, rocks, etc.) or dynamic objects (e.g., other air or ground vehicles, people, animals, rolling rocks, birds, etc.), which objects may be collectively referred to as "agents" or "other agents" in environment 100. For example, static object C 108 represents an object that is not moving in environment 100 and that forms a collision zone in environment 100 such that vehicle 102 may collide with the static object if the vehicle and static object C 108 attempt to occupy the same space in environment 100 at the same time. In various embodiments, there may be fewer or additional static objects than those shown in FIG. 1 .

In addition to static objects, there may also be dynamic objects, including those that represent objects moving in known/certain trajectories (e.g., falling bricks, rolling cans), those controlled by conscious beings (e.g., cyclists, pedestrians, drivers, pilots, birds, etc.), and those controlled by other autonomous systems such as in the case of other autonomous vehicles or robots. Due to these dynamic objects, the challenges for motion planning involve the ability to perform motion planning at extremely fast speeds and the ability to analyze the uncertainty of how a dynamic object may move. The environment 100 around the vehicle 102 can change quickly, and it is advantageous for the vehicle 102 to perform motion planning to keep up with those changes. For example, as shown in FIG1 , an agent, such as dynamic object A 104, is currently moving away from the vehicle 102 along a track 110. However, there may be instances where the vehicle 102 needs to follow or intercept dynamic object A 104 in order to detect dynamic object A 104, collect information from dynamic object A 104, exchange information with dynamic object A 104, or even collide with dynamic object A 104 in a game.

1 , dynamic object B 112 is currently moving toward vehicle 102 along track 106. There may be instances where vehicle 102 needs to avoid colliding with dynamic object B 112 or avoid approaching the dynamic object in order to reach a target destination without collision, avoid damage from such collision, or avoid contact with dynamic object B 112, such as in a game. In one embodiment, the goal of vehicle 102 is to maximize the time without colliding with dynamic object B 112, such that, for example, dynamic object B 112 runs out of fuel before colliding with vehicle 102. In one example embodiment, a goal of the vehicle 102 is to minimize the probability of colliding with the dynamic object B 112 between the current time and the time when the vehicle 102 reaches a desired destination or achieves a specific goal, or between the current time and the time when the dynamic object B 112 runs out of fuel. There may be fewer or additional dynamic objects in the environment 100 than shown in FIG1 . Additionally, in some cases, the environment may have boundaries corresponding to the range of the vehicle 102, which may be determined at least in part by the current fuel or energy available to the vehicle 102.

1 illustrates a representative environment 100, a typical environment may include many additional agents, including objects corresponding to other manned and autonomous vehicles and a variety of other natural or artificial static and dynamic objects and obstacles. The concepts taught herein may be used in a similar manner for environments that are more populated than the illustrated environment.

FIG. 2 and the following discussion provide a brief general description of a suitable controller in the form of a computer system 200 in which the various described motion planning systems and methods may be implemented.

Although not required, many embodiments will be described in the general context of computer executable instructions, such as application modules, objects, or macros stored on a computer or processor readable medium and executed by a computer or processor and dedicated vehicle motion planning hardware that can perform collision assessment and motion planning operations. Such motion planning operations may include performing collision assessments on edges of a plan graph, determining and setting collision probabilities, performing optimizations to identify paths in the plan graph to avoid collisions with or cause collisions with objects in the environment by finding the minimum cost path within the plan graph and implementing such motion planning.

Motion planning via a motion planner typically includes collision detection and finding a minimum cost path. Collision detection, minimum cost path finding, or both may be implemented, for example, on one or more field programmable gate arrays (FPGAs), thereby advantageously allowing easy reconfigurability. Collision detection, minimum cost path finding, or both may be implemented, for example, on one or more application specific integrated circuits (ASICs), thereby advantageously allowing fast processing while still allowing some reconfigurability.

When representing an agent, a vehicle (e.g., an autonomous vehicle or robot) or an object in the environment (e.g., a static or dynamic obstacle) can have its surface represented as stereo pixels (3D pixels) or a mesh of polygons (usually triangles). Each discretized spatial region is called a "stereo pixel," which is equivalent to a 3D (volume) pixel. In some cases, it is advantageous to represent objects as boxes (rectangular prisms) instead. Since objects are not randomly shaped, there can be a lot of structure in how the stereo pixels are organized; many stereo pixels in an object are adjacent to each other in 3D space. Therefore, representing an object as a box may require far fewer bits (i.e., two opposing corners of a box may only require x, y, z coordinates). Additionally, the complexity of intersection testing for boxes is comparable to the complexity of intersection testing for voxels. Various other data structures can be used to represent the 3D surface of an object, such as Euclidean distance fields, binary space partitioning trees, etc.

In one embodiment, collision assessment is performed by first streaming all dynamic object 3D pixels (or boxes) to a processor (e.g., FPGA, ASIC). Edge information for each edge of the map for the vehicle 102 is then streamed from a memory dedicated to the map. Each edge has a certain number of 3D pixels (or boxes) corresponding to a volume in 3D space that is swept by the vehicle 102 when making a transition in the map from one state to another represented by that edge. Those 3D pixels or boxes swept by the vehicle 102 when a transition is made in the map from one state to another state represented by that edge are stored in memory for each edge of the map. For each edge 3D pixel (or box), if it collides with any of the obstacle 3D pixels (or boxes) as it is streamed from the swept volume of the edge, the system 200 determines that a collision has occurred with that edge in the map. For example, if an edge 3D pixel collides with any of the obstacle 3D pixels (or boxes) as it is streamed from the swept volume of edge x of the map, the system notes a collision with edge x. This implementation improves the art of collision assessment because it enables much larger maps to be used for collision assessment compared to other designs that perform collision assessment on all edges of the planning map in parallel. In particular, this helps overcome the shortcomings of other designs regarding the limited amount of map information that can be stored on-chip circuitry. However, using the collision assessment method described herein, on-chip memory is typically sufficient to store all obstacle boxes (but may be less using stereo pixels). This provides the ability to store large maps and/or multiple maps in less expensive off-chip memory, such as dynamic random access memory (DRAM).

In various implementations, such operations may be performed entirely in hardware circuitry, or as software stored in memory storage such as system memory 214 and executed by one or more hardware processors 212a such as one or more microprocessors, digital signal processors (DSPs), field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), graphics processing unit (GPU) processors, programmable logic controllers (PLCs), electrically programmable read-only memories (EEPROMs), or as a combination of hardware circuitry and software stored in memory storage. For example, performing optimization to identify paths in a plan graph to avoid collisions with objects in the environment or to cause collisions with them by finding the minimum cost path in the plan graph can be performed by the optimizer 292. In an exemplary embodiment, when the path optimizer 292 is implemented by hardware, the topology of the plan graph can also be mapped to a reconfigurable mesh architecture of hardware units to enable rapid determination of the minimum cost path. This mapping involves programming each physical node using the addresses and edge weights of its logical neighbors. This allows the architecture to be reconfigured into different plan graph topologies. Other implementations may use a small processor implemented on an FPGA.

In an alternative embodiment, collision assessment may be performed in parallel for each of the edges of the resulting planning graph for vehicle 102 by dedicated motion planning hardware, such as the reconfigurable collision detection architecture and other embodiments described in: International Patent Application No. PCT/US2017/036880, entitled “MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS,” filed on June 9, 2017, and International Patent Application No. PCT/US2017/036880, entitled “SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MANUFACTURE AND USE THEREOF,” filed on January 5, 2016. For example, all or part of such dedicated motion planning hardware may be incorporated into or form part of the motion planner 280 and the collision evaluator 288. In addition, implementations of various related aspects of perception, planning graph construction, collision detection, and path finding are also described in the following: International Patent Application No. PCT/US2017/036880, filed on June 9, 2017, entitled "MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS", and International Patent Application Publication No. WO 2016/122840, filed on January 5, 2016, entitled "SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME". Those skilled in the relevant art will appreciate that the described implementations and other implementations may be implemented with other system configurations and/or other computing system configurations, including configurations of robots, handheld devices, multiprocessor systems, microprocessor-based or programmable consumer electronic devices, personal computers ("PCs"), network-connected PCs, minicomputers, mainframe computers, and the like. The implementations or portions thereof (e.g., at design time, assembly time, pre-runtime) may be implemented in a distributed computing environment in which tasks or modules are executed by remote processing devices linked via a communications network. In a distributed computing environment, programmed modules may be located in both local and remote memory storage devices or media. However, it is important for the vehicle 102 to have efficient computing capabilities to allow the vehicle 102 to respond to the changing environment in real time. Commonly deployed solutions to this problem fail on both the performance and power front ends. They are too slow to allow high-degree-of-freedom vehicles and robots to respond to the environment in real time, and burden the system with powering several CPUs or GPUs. To address this problem, the computer system 200 shown in the example implementation of FIG. 2 includes a motion planner 280 with a collision evaluator 288 on the vehicle 102 that uses a fully retargetable collision detection microarchitecture, such as an FPGA 290. However, in various alternative embodiments, other programmable collision detection micro-architectures, such as ASIC architectures, may be used, including an array of programmable logic blocks and a reconfigurable interconnect hierarchy. In the programming stage, the collision detection micro-architecture can be applied to any vehicle planning problem. The collision evaluator 288 can be used to achieve collision avoidance with specific objects and/or to attempt collisions with other objects. The use of a reconfigurable processor as the collision evaluator 288 effectively removes the limitation of the design being completely dedicated to a single vehicle/map pair. The minimum cost path module allows the minimum cost path to be quickly calculated, for example using a distributed Bellman-Ford strategy.

As mentioned above, some pre-processing activities may be performed prior to the execution phase, and thus, in some embodiments, such operations may be performed by remote processing devices that are linked to the vehicle 200 via the communications network via the network interface 260. For example, the programming phase allows the processor to be configured for the problem of interest. In such embodiments, extensive pre-processing is utilized to avoid execution phase calculations. Pre-calculated data about volumes in 3D space is sent to the collision evaluator 288 of the motion planner 280, which is swept by the vehicle 102 as a transition is made in the map from one state to another state represented by an edge in the map. The topology of the map can also be mapped onto a reconfigurable mesh architecture of a computing unit such as an FPGA 290 to enable rapid determination of the minimum cost path. The mapping step includes programming each physical node using the addresses and edge weights of the logical neighbors of each physical node of the reconfigurable mesh architecture of the computing unit. This allows the architecture to target different map topologies. During the execution phase, the sensor 282 sends sensor data to the motion planner 280. The sensor data is a stream of stereo pixels or boxes (described in more detail below) that exist in the current environment. The collision evaluator 288 calculates which movements are likely to involve collisions and which are unlikely, and upon completion, the results are used by the planning optimizer 292 to determine the minimum cost path. This can advantageously occur without further communication with the sensors 282 or other external components. Depending on whether the goal of the vehicle 102 is to avoid or attempt a collision with a particular object in the environment, the motion planner 280 modifies the costs associated with the map accordingly during the execution phase based on the environment. The motion planner 280 then executes and returns the resulting path to the actuator system 266. Figure 2 shows a computer system 200 such as for use with the autonomous vehicle 102, the computer system including the motion planner 280 and one or more associated non-transitory machine-readable storage media, such as system memory 214 and computer-readable media 226 associated with the disk drive 224. The associated non-transitory computer or processor readable storage media, including the system memory 214 and the computer readable media 226 associated with the disk drive 224, are communicatively coupled to the motion planner 280 via one or more communication channels such as the system bus 216. The system bus 216 can adopt any known bus structure or architecture, including a memory bus with a memory controller, a peripheral bus and/or a local bus. One or more sensors 282, object detectors 284, object behavior predictors 286 and actuator systems 266 are also communicatively coupled to the motion planner 280 via the system bus 216. One or more of these components may also or alternatively communicate with each other via one or more other communication channels, such as one or more parallel cables, serial cables, or wireless network channels capable of high-speed communication, such as Universal Serial Bus (“USB”) 3.0, Peripheral Component Interconnect Express (PCIe), or ^{Thunderbolt®} .

The computer system 200 may also be communicatively coupled to remote systems, such as desktop computers, laptop computers, ultraportable computers, tablet computers, smart phones, wearable computers (not shown), which are directly communicatively coupled or indirectly communicatively coupled to various components of the computer system 200 via the network interface 260. In implementations, the computer system 200 itself or a portion thereof may be remote. These remote systems may be used to program, configure, control, or otherwise interface with or input data to the computer system 200 and various components within the computer system 200. This connection may be via one or more communication channels, such as one or more wide area networks (WANs), such as the Internet using the Internet Protocol. As mentioned above, pre-runtime calculations (e.g., initial map generation) may be performed by a system separate from the vehicle 102 or other type of robot, while run-time calculations may be performed on the vehicle 102, since it is important for the system to be able to update or change vehicle speed in real-time or near-real-time (microseconds) and react to changing operating environment 100.

Some aspects of the construction and operation of the various blocks shown in FIG. 2 are described in the following: International Patent Application No. PCT/US2017/036880, filed on June 9, 2017, entitled “MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS”, and International Patent Application Publication No. WO 2016/122840, filed on January 5, 2016, entitled “SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME”. As a result, such blocks do not require further detailed description as they will be understood by those skilled in the relevant art in view of the references incorporated herein by reference.

The computer system 200 may include one or more processing units 212a, 212b (collectively 212), a system memory 214, and a system bus 216 coupling various system components including the system memory 214 to the processing unit 212. The processing unit 212 may be any logical processing unit, such as one or more central processing units (CPUs) 212a, digital signal processors (DSPs) 212b, application specific integrated circuits (ASICs), field programmable gate arrays (FPGAs), etc. Such ASICs and FPGAs may be used instead of or in addition to the FPGA 290 of the collision evaluator 288 to perform collision assessment on the edges of the planning graph for the vehicle 102. System memory 214 may include read-only memory ("ROM") 218 and random access memory ("RAM") 220. Basic input/output system ("BIOS") 222 may form part of ROM 218 and contains basic routines that help transfer information between components within computer system 200, such as during startup.

The computer system 200 may include a disk drive 224, which may be, for example, a hard disk drive for reading from and writing to a hard disk, a flash memory drive for reading from and writing to a flash memory device, an optical disk drive for reading from and writing to a removable optical disk, or a disk drive for reading from and writing to a magnetic disk. In various different embodiments, the computer system 200 may also include any combination of such disk drives. The disk drive 224 may communicate with the processing unit 212 via the system bus 216. The disk drive 224 may include an interface or controller (not shown) coupled between such a drive and the system bus 216, as is known to those skilled in the relevant art. The disk drive 224 and its associated computer-readable medium 226 provide non-volatile storage of computer-readable instructions, data structures, programmable modules, and other data for the computer system 200. Those skilled in the relevant art will appreciate that other types of computer-readable media that can store data that can be accessed by a computer may be used, such as WORM drives, RAID drives, magnetic tape cartridges, digital video disks ("DVDs"), Bernoulli cartridges, RAM, ROM, smart cards, etc.

Program modules may be stored in the system memory 214, such as an operating system 236, one or more applications 238, other programs or modules 240, and program data 242. The applications 238 may include instructions for causing the processor 212 to perform collision evaluation on edges of a plan graph corresponding to the environment 100, determine and set a collision probability for each edge of the plan graph, and perform optimization to identify paths in the plan graph that avoid collisions with or cause collisions with agents (e.g., dynamic object B 112) in the environment 100. The optimization for identifying paths in the plan graph may include finding a minimum cost path within the plan graph. The application 238 may include instructions that then cause the processor 212 to send signals to the actuator system 266 to move the vehicle 102 according to the motion plan as described herein. The application 238 may additionally include one or more machine-readable instructions that cause the processor 212 to perform other operations of perception (via the sensor 282), map construction, collision detection, and path finding as described herein and in the references incorporated herein by reference.

The application 238 may further include one or more machine-readable instructions for performing the following operations: causing the processor 212 to receive sensor information representing the environment 100 in which the vehicle 102 is operating from the sensor 282; causing the motion planner 280 to perform a collision assessment on each of two or more of the edges of the resulting planning graph for the vehicle 102 using the reconfigurable collision detection architecture hardware of the collision evaluator 288; for each of the two or more edges of the resulting planning graph, setting a collision probability based at least in part on the collision assessment; performing optimization to identify one or more other agents in the resulting planning graph that have a relatively high probability of being associated with one or more other agents in the environment 100 in which the vehicle 102 is operating (e.g., dynamic object A); 104); and causing the actuator system 266 to implement a motion plan that has a relatively high probability of colliding with one or more other agents (e.g., dynamic object A 104) in the environment 100 in which the vehicle 102 operates based at least in part on the optimization. The reconfigurable collision detection architecture hardware can be, for example, an FPGA 290. However, in various alternative embodiments, other programmable collision detection microarchitectures including an array of programmable logic blocks and a reconfigurable interconnect hierarchy, such as an ASIC architecture, can be used.

The application 238 may further include one or more machine-readable instructions that cause the processor 212 to perform the following operations on the planning map based at least in part on an evaluation of the probability of collision with one or more dynamic objects (104, 112) in the environment 100 in which the vehicle 102 operates: if a respective edge has a relatively low respective probability of collision with one or more dynamic objects (104, 112) in the environment 100, then it will have a probability of collision equal to or greater than Assigning a weight having a value of zero to each edge of the planning graph; assigning a weight having a value less than zero to each edge of the planning graph if the respective edge has a relatively high respective probability of colliding with one or more dynamic objects (104, 112) in the environment 100; and performing optimization to identify paths in the resulting planning graph that have a relatively high probability of colliding with one or more agents in the environment 100, such as dynamic objects (104, 112), in which the vehicle 102 is operating.

The application 238 may further include one or more machine-readable instructions for: causing the processor 212 to receive sensor information representing the environment 100 in which the vehicle 102 operates via the sensor 282; causing the motion planner 280 to perform a collision assessment on each of two or more of the edges of the planning graph using the reconfigurable collision detection architecture hardware of the collision evaluator 288; ; for each of two or more edges of the planning graph, setting a collision probability based at least in part on the collision evaluation; performing optimization to identify a path in the resulting planning graph that provides a longest travel path for the vehicle 102 in two-dimensional or three-dimensional space, the travel path being specified by a path that has a relatively low probability of colliding with one or more dynamic objects (e.g., dynamic object B 112) in the environment 100 in which the vehicle 102 operates; and implementing motion planning that has a relatively low probability of colliding with one or more dynamic objects (e.g., dynamic object B 112) in the environment 100 in which the vehicle 102 operates based at least in part on the optimization.

The application 238 may additionally include one or more machine-readable instructions for causing the processor 212 to perform various other methods described herein, including but not limited to the methods illustrated in FIGS. 6-13 .

Although shown in FIG. 2 as being stored in system memory 214 , operating system 236 , applications 238 , other programs/modules 240 , and program data 242 may be stored on associated computer-readable media 226 of disk drive 224 .

The processing unit 212 may be any logic processing unit, such as one or more central processing units (CPUs), digital signal processors (DSPs), ^application specific integrated circuits (ASICs), field programmable gate arrays (FPGAs), etc. Non-limiting examples of commercially available computer systems include, but are not limited to: Celeron, Core, Core 2, Itanium, and Xeon series microprocessors provided by Intel ^® Corporation; K8, K10, Bulldozer, and Bobcat series microprocessors provided by Advanced Micro Devices; A5, A6, and A7 series microprocessors provided by Apple Computer; Snapdragon series microprocessors provided by Qualcomm Inc; and SPARC series microprocessors provided by Oracle Corp. Unless otherwise described, the construction and operation of the various blocks shown in Figure 2 have known designs. Therefore, there is no need to further describe such blocks in detail herein, as will be understood by those skilled in the relevant art. The reconfigurable collision detection architecture hardware of the collision evaluator 288 of the motion planner 280 may be one of the architectures described in international patent application No. PCT/US2017/036880 filed on June 9, 2017, entitled “MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS”, such as providing an array of “edge modules” having storage mechanisms and comparators for each edge, wherein the storage mechanisms and comparators are connected in parallel to logic gates to perform an “OR” of the outputs of parallel lines to generate a collision result.

FIG3 is a block diagram showing an example data flow 300 between various components in the computer system of FIG2 according to one illustrated embodiment. One or more sensors 282, such as cameras, laser sensor devices, audio sensors, etc., incorporated into or in operable communication with the primary agent 102, collect sensory information 302 and communicate this sensory information to the object detector 284 to generate a model of the environment 100. The object detector 284 extracts trajectory information about the detected movement of agents such as dynamic object A 104 and dynamic object B 112 in the environment 100, and communicates this trajectory information 308 to the object behavior predictor 286. Based at least in part on the current detected trajectory of the dynamic objects (104, 112) in the environment 100 as indicated by the trajectory information 308, the object behavior predictor 286 generates one or more predicted trajectories for the dynamic objects (104, 112) and communicates this information to the motion planner 280 as part of the predicted trajectory information 306. For example, if the trajectory information 308 indicates that the dynamic object A 104 is currently on a trajectory toward a particular direction, the object behavior predictor 286 may predict that there is a 40% probability that the dynamic object A 104 will continue its current trajectory and a 60% probability that it will perform another behavior.

Various factors may affect the determination of the object behavior predictor 286 of the predicted trajectory of a dynamic object (104, 112) in the environment 100. For example, in some implementations, it may be indicated or determined that a dynamic object (104, 112) has a goal that will affect its future movement within the environment 100. As one example, it may be indicated or determined that dynamic object A 104, which is detected as currently on a trajectory directly away from the primary agent 102, has a goal of moving away from (and remaining away from) the primary agent 102. Thus, the object behavior predictor 286 may take this into account when predicting the movement of dynamic object A 104. On the other hand, it may be indicated or determined that the dynamic object B 112 detected as currently on a trajectory directly toward the primary agent 102 has a goal of colliding with the primary agent 102. Therefore, the object behavior predictor 286 may take this into account when predicting the movement of the dynamic object B 112.

Additionally, the movement of other agents, such as dynamic objects (104, 112), may be affected by changes in the trajectory of the primary agent 102. Therefore, the object behavior predictor 286 may consider planned and not yet implemented or executed changes in the current trajectory of the primary agent 102 when determining the predicted trajectory of the dynamic objects (104, 112), and include this data in the predicted trajectory information 306 provided to the motion planner 280. For example, if it is indicated or determined that dynamic object B 112 detected as currently on a trajectory directly toward primary agent 102 has a goal of colliding with primary agent 102, it can be predicted that if primary agent 102 changes its trajectory, dynamic object B 112 can make corresponding changes to its trajectory to track primary agent 102. Therefore, if the primary agent 102 has a goal of reaching a destination within the environment 100 without colliding with the dynamic object B 112 (i.e., attempting to collide with the primary agent 102), the motion planner 280 can plan a path to the destination to avoid colliding with the dynamic object B 112, taking into account that the dynamic object B 112 can make corresponding changes to the trajectory of the dynamic object to track the primary agent 102 as the primary agent 102 changes its trajectory to reach the destination.

In general, the system performs perception using a combination of sensors 282 and processing performed by object detectors 284 and object behavior predictors 286 to generate a model of environment 100. In one implementation, sensors 282 generate an occupancy grid. The occupancy grid is a data structure that represents which spatial regions and times in a discretized view of an environment, such as environment 100, contain obstacles. Each discretized spatial region is called a "stereo pixel," which is equivalent to a 3D (volume) pixel. In some cases, it is advantageous to represent objects as boxes (rectangular prisms) instead. The spatial region defined by objects in the environment, including dynamic object A 104, dynamic object B 112, and static object C 108, is represented by such volumetric representations. The volumetric representations of one or more dynamic objects (e.g., dynamic object A 104 and dynamic object B 112) and the volumetric representations of the associated static objects are communicated from the object detector 284 to the motion planner 280. The construction of occupancy grids is described in a large amount of published literature available and known to those of ordinary skill in computer vision and sensing techniques.

The motion planner 280 receives sensory data including volumetric representations of dynamic and static objects from the object detector 284 and receives predicted trajectory information from the object behavior predictor. The motion planner 280 then adjusts the collision probability along each edge in the planning graph that collides with an obstacle in the sensory data to take into account the predicted trajectory, determines a path that takes into account cost and collision probability, and outputs the path to the computing system.

The motion planner may include a hardware processor and memory storage as part of the collision evaluator 288 within the motion planner 280. For example, an FPGA 290 or other programmable logic block array may store a planning map, also referred to herein as a "map" (see, e.g., FIGS. 4A-5B ). In some implementations, the motion planner 280 includes hardware collision detection circuitry, such as an FPGA 290, for performing collision detection. In some implementations, the motion planner 280 includes a reconfigurable collision detection accelerometer. Data regarding the volume swept by the primary agent 102 in 2D or 3D space when a transition is made in the map from one state to another state represented by an edge in the map may be stored at the memory storage of the collision evaluator 288 of the motion planner 280, so that during motion planning, when sensory data including predicted trajectory information is received, the sensory data is compared by the hardware processor of the collision evaluator 288 with the data stored in the memory storage of the collision evaluator 288 (or the local system memory 214 of the computer system 200) to determine collisions. During run-time operations, information may be assigned to the edges of the planning graph based on one or more variables. For example, in the case where the primary agent 102 aims to collide with the dynamic object A 104, the motion planner 280 will generate a motion plan for the primary agent 102 to collide with the dynamic object A 104 based on the prediction of the direction of the dynamic object A 104 according to the predicted trajectory information 306. To this end, the collision evaluator 288 evaluates the likelihood of all edges in the plan graph colliding with the dynamic object A 104. It should be noted that the environment 100 is the two-dimensional or three-dimensional space in which the primary agent 102 operates, and is different from the "assembly space" of the primary agent referenced below with respect to the motion plan graphs shown in Figures 4A to 5B. The master agent combination space is the space of all combinations of master agents 102 that characterize the state of the master agent, and is typically a multi-dimensional space, for example, having more than three dimensions. The edges in the planning diagrams 400 and 500 shown in Figures 4A to 5B represent transitions between combinations of master agents 102. The edges of the planning diagram 400 do not necessarily represent actual movements in Cartesian coordinates, but in some embodiments they may. The edges of the planning diagram 400 may also include speed changes, etc.

Each edge of the plans 400 and 500 represents a transition of a primary agent from one state to another and has an inherent or operating cost associated with the transition. For example, the inherent or operating cost may be related to fuel usage, time to perform the associated action, wear and tear associated with the action, and/or other factors. Each edge may be assigned an initial weight corresponding to the inherent or operating cost.

The system adjusts the cost of the edge during the run phase to represent the probability of collision with a dynamic object (104, 112) in the environment based at least in part on the collision evaluation. The system can perform the cost adjustment by modifying the initial assigned weight of each edge based on the collision probability. For example, the system can apply a cost function to each edge to perform a mathematical operation based on the initial weight of that edge (i.e., the weight corresponding to the inherent cost) to obtain a modified weight. This can be done by adding an additional weight to the initial assigned weight based on the collision probability, multiplying the initial assigned weight by a collision probability factor, or applying some other function or formula involving the collision probability and the initial weight corresponding to the inherent cost.

The inherent or operational costs assigned to edges can also be adjusted during runtime to reflect object-specific costs that represent the relative importance and/or severity of avoiding a collision or achieving a collision with an object. These object-specific costs are independent of the inherent or operational costs and independent of the probability of collision. For example, the object-specific cost associated with a collision with a person can be set to be significantly higher than the object-specific cost associated with a collision with an inanimate object.

For simplicity of explanation, in FIGS. 4A to 5B , all initial weights corresponding to the intrinsic cost of each edge have been set to zero and adjusted by adding an additional cost indicative of the probability of collision. Thus, in one implementation where the goal of the primary agent 102 is to collide with a dynamic object in the environment (e.g., dynamic object A 104), an initial weight of 0 combined with a collision probability of zero results in an edge weight of 0, while a greater collision probability results in an edge weight with a greater negative value (i.e., a negative number with a greater absolute value). In another implementation where the goal of the primary agent 102 is to avoid collisions with dynamic objects in the environment (e.g., dynamic object B 112), a greater collision probability may result in an adjusted edge weight with a greater positive value.

Once all edge weights of the plan map have been adjusted, the path optimizer 292 executes a minimum cost path algorithm from the current position of the master agent 102 indicated in the plan map to all possible end points where the master agent 102 has exhausted fuel/power. The smallest (most negative) path in the plan map is then selected by the motion planner 280.

Once the path optimizer 292 identifies the path within the planning graph, the motion planner immediately communicates the identified path 310 to the actuator system 266 of the main agent 102 in real time to generate signals corresponding to the various motors or motion systems of the main agent 102, thereby causing the physical movement of the main agent 102 to implement the motion plan.

FIG. 4A is an example motion planning graph 400 of the primary agent 102 of FIG. 1 according to an illustrated embodiment, when the primary agent 102's goal is to collide with the dynamic object A 104 of FIG. 1 that may attempt to avoid the primary agent 102. The planning graph 400 includes a plurality of nodes connected by edges. For example, node 408b and node 408c are connected by edge 410a. Each node implicitly or explicitly represents time and variables that characterize the state of the primary agent 102 in the primary agent's combination space. In this example, the primary agent's combination space (commonly referred to as C-space) is the space of combinations of the primary agents represented in the planning graph 400, which combinations characterize the state of the primary agents. The edges in the planning graph 400 represent transitions between these configurations of the primary agent 102. The edges of the planning graph 400 do not represent actual movement in Cartesian coordinates. For example, each node may represent a configuration of the primary agent, which may include but is not limited to the current position, pose, velocity, and orientation of the primary agent 102. In some embodiments, the acceleration of the primary agent 102 is also represented by a node in the planning graph 400.

Each edge of the plan diagram 400 represents a transition of an object 102 between a respective pair of nodes. For example, edge 410a represents a transition of an object such as a primary agent 102 between two nodes. Specifically, edge 410a represents a transition between the state of the primary agent 102 in a particular combination associated with node 408b and the state of the primary agent 102 associated with node 408c. For example, the primary agent 102 may currently be in a particular combination associated with node 408a. Although the nodes are shown as being at various distances from each other, this is for illustrative purposes only and is not related to any physical distance and there is no limit to the number of nodes in the plan diagram 400. However, the more nodes that are used in the planning graph 400, the more accurately and precisely the motion planner 280 can determine the best path based on the goals of the primary agent 102 since there are more paths from which the minimum cost path can be selected.

There may be instances where the primary agent 102 needs to follow or intercept dynamic object A 104 in order to detect dynamic object A 104, collect information from dynamic object A 104, exchange information with dynamic object A 104, or even collide with dynamic object A 104 during gameplay. FIG4A shows how the planning graph is used by the motion planner 280 to identify a path for the primary agent 102 if the primary agent 102's goal is to collide with dynamic object A 104. At this point, the motion planner 280 has received sensory information representing the environment 100 in which the primary agent 102 operates. As described above, collision detection may use 3D pixels or boxes to represent objects in the environment to the motion planner 280, including the main agent 102 and the dynamic object A 104. However, it should be understood that other object representation forms may be used.

In one implementation, the environment is discretized into 3D regions of stereo pixels or boxes. Then, all possible collisions between each motion volume swept by the primary agent 102 in the environment 100 and the stereo pixels or boxes in the discretized space are estimated. Examples of such collision assessments are described in International Patent Application No. PCT/US2017/036880, filed on June 9, 2017, entitled “MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS,” and International Patent Application Publication No. WO 2016/122840, filed on January 5, 2016, entitled “SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME.”

Since the dynamic object A 104 is moving in the environment 100, the motion planner 280 also determines a collision evaluation of the primary agent 102 and the dynamic object A 104 for two or more edges in the planning graph 400 based on the prediction of where the dynamic object A 104 is heading. For each of these edges in the planning graph 400, the motion planner 280 sets a probability of the primary agent 102 colliding with the dynamic object A 104 at a particular future time based at least in part on the collision evaluation. For example, based on the perception information, the dynamic object A 104 is detected to be at a particular location in the environment 100. Based on the current trajectory 110 of the dynamic object A 104, the motion planner 280 determines that the dynamic object A 104 will be at a particular location in the environment 100. For nodes in the planning graph 400, where direct movement between those nodes has a probability of causing a collision with the dynamic object A 104, the motion planner assigns weights to the edges of the planning graph 400 that transition between those nodes (edges 410b, 410c, 410d, 410e, 410f, 410g, 410h, 410i, 410j, 410k), the weights indicating the probability of a collision with the dynamic object A 104. In the example shown in FIG. 4A , this is labeled as diagram portion 414 , but does not correspond to a physical area.

For example, for each of the plurality of edges in the planning graph 400 whose respective probability of colliding with the dynamic object A 104 is lower than the defined threshold collision probability, the motion planner 280 may assign a weight having a value equal to or greater than zero. In the present example, the motion planner 280 has assigned a weight of zero to those edges in the planning graph 400 that, based on the current trajectory of the dynamic object A 104, do not have any (or have a very small) probability of colliding with the dynamic object A 104. For example, as shown on the planning graph 400, the motion planner 280 has assigned a weight of zero to the edge 410a because there is no (or very little) probability of collision with the dynamic object A 104 at the edge 410a according to the current trajectory 110 of the dynamic object A 104. For each of the plurality of edges in the planning graph 400 whose respective probability of collision with the dynamic object A 104 in the environment 100 is higher than the defined threshold collision probability, the motion planner 280 then assigns a weight having a value less than zero. In the present example, the motion planner 280 has assigned weights less than zero to those edges in the planning graph 400 that have a higher probability of colliding with the dynamic object A 104 based on the current trajectory of the dynamic object A 104. The specific threshold value for the probability of collision can vary. For example, the threshold value can be 40%, 50%, 60% or lower or higher probability of collision. In addition, the motion planner 280 assigning weights having values less than zero can include assigning negative weights having magnitudes corresponding to the respective probability of collision. For example, as shown in planning diagram 400, the motion planner has assigned a weight of -3 to edges 410b, 410c, 410d, and 410e, but has assigned a negative weight having a lower magnitude of -2 to edge 410f, and has assigned a weight having a higher magnitude of -5 to edge 410g. The assigned weights need not be integers.

4B is an example motion planning graph 400 of the primary agent 102 of FIG. 1 when the primary agent 102's goal is to collide with the dynamic object A 104 of FIG. 1 that may attempt to avoid the primary agent 102, and an example path 412 (including the bolded edge of the graph 400 connecting nodes 408a to 408d) identified in the planning graph 400 of the primary agent 102 to collide with the dynamic object A 104 according to an illustrated embodiment. After the motion planner 280 sets the probability of the primary agent 102 colliding with the dynamic object A 104 based at least in part on the collision evaluation, the motion planner 280 performs optimization to identify paths 412 in the resulting planning graph 400 that have a relatively high probability of colliding with the dynamic object A 104 .

For example, once all edge weights of the planning graph 400 have been assigned as shown in Figures 4A and 4B, the motion planner 280 may execute a minimum cost path algorithm from the current state of the master agent 102 in the planning graph 400 to all possible end points where the master agent 102 has exhausted its fuel. The minimum (most negative) path in the planning graph 400 is then selected by the motion planner 280. In this example, the current state of the master agent 102 in the planning graph is at node 408a, and this minimum (most negative path) is depicted as path 412 in the planning graph 400. Although shown as a path with many sharp turns in the planning graph 400, such turns do not represent corresponding physical turns in the route, but rather logical transitions between states of the primary agent 102. For example, each node in the identified path 412 may represent a change in state relative to the physical configuration of the primary agent 102 in the environment 100, but does not necessarily represent a change in orientation of the primary agent 102 corresponding to the angle of the path 412 shown in FIG. 4B .

Various procedures for determining the minimum cost path may be used, including those implementing the Bellman-Ford algorithm, but other procedures may be used, including but not limited to any such procedure that determines the minimum cost path as the path between two nodes in the planning graph 400 that minimizes the sum of the weights of the constituent edges of the planning graph. This procedure improves the techniques used by a master agent, such as an autonomous vehicle, for motion planning for collisions with dynamic objects (104, 112) by using the planning graph and collision detection to increase the efficiency and response time of finding the best path to collide with the desired object. In addition, some implementations use the same procedure to identify paths of the master agent 102 that have a relatively high probability of colliding with one or more static objects in the environment in which the master agent 102 operates. In the case of an attempted collision with such a static object, the motion planner 280 may assign weights with large negative values to the edges of the planning graph 400 that have respective probabilities of collision with static objects in the environment 100. In this way, when the motion planner selects the minimum cost path during optimization, such paths will be more likely to be selected. However, in such implementations, the velocity, trajectory, or acceleration of the static objects need not be considered.

In some implementations, when the primary agent 102 attempts to collide with the dynamic object A 104, there may be static objects in the environment that the primary agent 102 should avoid colliding with. In this case, setting the collision probability of the edges of the planning graph 400 based on the collision evaluation includes assigning weights (e.g., by modifying/adjusting the initial weights) to avoid collisions with such static objects accordingly. For example, for each of a number of edges of the planning graph 400 that have a respective probability of colliding with a static object in the environment 100, the motion planner 280 assigns a weight having an infinite value. In this way, when the motion planner chooses minimum-cost paths during optimization, such paths with edge weights set to infinite will be avoided, since such paths will result in collisions with static objects in the event of a traversal.

The motion planner 280 may perform optimization to identify a path in the resulting plan map 400 that has the highest probability of colliding with dynamic object A 104 along the entire route of the primary agent 102. In some implementations, the length of the route may be defined at least in part by when the primary agent 102 runs out of fuel/power. A variable indicating the "remaining fuel" of the primary agent 102 may be stored by the computer system 200. In some implementations, the motion planner 280 may perform optimization to identify a path in the resulting plan map 400 that has a relatively high probability of colliding with one or more objects in the environment in which the primary agent 102 is operating in the shortest relative amount of time. Alternatively, in some implementations, the motion planner 280 may perform optimization to identify the path in the resulting planning graph 400 having the longest travel duration, where the travel duration is specified by the path having a relatively high probability of colliding with the dynamic object A 104.

The paths in the planning map 400 may also be identified based on changes or predicted changes in the trajectory 110 of the dynamic object A. The collision assessment and optimization process may be re-executed in real time or near real time each time the trajectory 110 of the dynamic object A 104 changes or is predicted to change. In addition, the resulting planning map 400 may have or store associated data representing the physical or performance constraints of the master agent 102 and/or the dynamic objects (104, 112), the acceleration, pitch, roll, and yaw of the master agent 102, and in some implementations, the acceleration, pitch, roll, and yaw of the dynamic object A 104. Optimization to identify the path may then be performed based on these variables. For example, if the pitch, roll, and/or yaw of the primary proxy dynamic object A 104 changes, this may indicate a change in the trajectory of the dynamic object A 104 (or result in a predicted change).

FIG. 5A is an example motion planning graph 500 of the primary agent 102 of FIG. 1 according to an illustrated embodiment, when the primary agent 102's goal is to avoid collision with the dynamic object B 112 of FIG. 1 that is close to the primary agent 102. Similar to the planning graph 400, the planning graph 500 includes a plurality of nodes connected by edges. Each node implicitly or explicitly represents a time and variable that characterizes the state of the primary agent 102. For example, each node may represent a combination of the primary agent, which may include but is not limited to the current position, posture, velocity, and orientation of the primary agent 102. In some embodiments, the acceleration of the primary agent 102 is also represented by a node in the planning graph 500.

There may be instances where the primary agent 102 is required to avoid dynamic object B 112 in order to avoid collision with dynamic object B 112. FIG. 5A shows how the planning graph is used by the motion planner 280 to identify the path of the primary agent 102 in a situation where the primary agent 102's goal is to avoid collision with dynamic object B 112 or to avoid dynamic object B 112 and dynamic object B 112 attempts to collide with the primary agent 102, such as in a game. At this point, the motion planner 280 has received sensory information representing the environment 100 in which the primary agent 102 operates. As described above, collision detection may use stereo pixels or boxes to represent objects in the environment, including dynamic object B 112. Stereopixels or boxes may also be used to represent the primary agent 102 to the motion planner 280. However, it should be understood that other objects may be used to represent the morphology. In one implementation, the environment is discretized into 3D regions of stereo pixels or boxes. All possible collisions between each motion volume swept by the primary agent 102 in the environment 100 in the planning map 500 and the stereo pixels or boxes in the discretized space are then estimated.

Since the dynamic object B 112 is moving in the environment 100, the motion planner 280 also determines a collision evaluation of the primary agent 102 and the dynamic object B 112 for two or more edges in the planning graph 500 based on the prediction of where the dynamic object B 112 is heading. For each of these edges in the planning graph 500, the motion planner 280 sets a probability of the primary agent 102 colliding with the dynamic object B 112 at a particular future time based at least in part on the collision evaluation. For example, based on the perception information, the dynamic object B 112 is detected to be at a particular location in the environment 100. Based on the current trajectory 106 of the dynamic object B 112 , the motion planner 280 determines that the dynamic object B 112 will be at a specific location in the environment 100 . For nodes in the planning graph 500 where direct movement between those nodes has a chance of causing a collision with dynamic object B 112, the motion planner assigns weights to the edges of the planning graph 500 that transition between those nodes (edges 510a, 510b, 510c, 510d, 510e, 510f, 510g, 510h, 510i, 510j, 510k, 510l, 510m, 510n, 510o, and 510p) that indicate the chance of a collision with dynamic object B 112. In the example shown in FIG5A , this is labeled as graph portion 514, but does not correspond to a solid area.

For example, for each of the plurality of edges in the planning graph 500 whose respective probability of colliding with the dynamic object B 112 is higher than the defined threshold collision probability, the motion planner 280 may assign a weight having a value greater than zero. In the present example, the motion planner 280 has assigned a weight of zero to those edges in the planning graph 500 that, based on the current trajectory of the dynamic object B 112, do not have any (or have a very small) probability of colliding with the dynamic object B 112. For each of the plurality of edges of the planning graph 500 whose respective probability of colliding with the dynamic object B 112 in the environment 100 is higher than the defined threshold collision probability, the motion planner 280 then assigns a weight having a value greater than zero. In the present example, the motion planner 280 has assigned weights greater than zero to those edges in the planning graph 500 that have a higher probability of colliding with the dynamic object B 112 based on the current trajectory of the dynamic object B 112. The specific threshold value for the collision probability may vary. For example, the threshold value may be 40%, 50%, 60% or lower or higher collision probability. In addition, the motion planner 280 assigning weights with values greater than zero may include assigning negative weights with magnitudes greater than zero, which correspond to respective collision probabilities. For example, as shown in the planning diagram 500, the motion planner has assigned a weight of 5 to edges 510f and 510i with higher collision probabilities, but has assigned a weight with a lower magnitude of 1 to edges 510p and 510g, which the motion planner 280 has determined to have much lower collision probabilities.

5B is an example motion planning graph 500 of the primary agent 102 of FIG. 1 when the primary agent 102's goal is to avoid collision with dynamic object B 112 of FIG. 1 that is close to the primary agent, and an example path 512 (including the bolded edge of the graph 500 connecting nodes 508a to 508b) identified in the planning graph 500 of the primary agent 102 to avoid collision with dynamic object B 112 according to an illustrated embodiment. After the motion planner 280 sets the probability of the primary agent 102 colliding with the dynamic object B 112 based at least in part on the collision evaluation, the motion planner 280 performs optimization to identify a path 512 in the resulting planning graph 500 that provides the longest travel path for the primary agent 102, which is specified by a path having a relatively low probability of colliding with the dynamic object B 112.

In one implementation, once all edge weights of the planning graph 500 have been assigned as shown in Figures 5A and 5B, the motion planner 280 may perform calculations to determine the longest path of travel so that the dynamic object B 112 will run out of fuel before colliding with the primary agent 102. For example, once all edge weights of the planning graph 500 have been assigned as shown in Figures 5A and 5B, the motion planner 280 may perform a minimum cost path algorithm from the current state of the primary agent 102 in the planning graph 500 to all possible end points where the primary agent 102 has run out of fuel/power. The longest route (e.g., in time or distance) in the plan graph 500 with the minimum cost (closest to zero) path is then selected by the motion planner 280. However, the longest route and the minimum cost (closest to zero) path in the plan graph 500 often compete. In the case where the longest route is required, finding the minimum cost path in the plan graph 500 does not have the same high priority as selecting the path with the minimum collision probability. In this example, the current state of the main agent 102 in the plan graph is at node 508a, and this path is depicted as path 512 in the plan graph 500.

In some implementations, the primary agent 102 may have a secondary goal of reaching a specific destination (while avoiding collisions with dynamic object B 112). In this case, optimization may include executing a minimum cost path algorithm from the current state of the primary agent 102 in the planning graph 500 to the desired destination. In one embodiment, the primary agent 102's goal is to maximize the time without colliding with dynamic object B 112, such that, for example, dynamic object B 112 runs out of fuel before colliding with the primary agent 102. In one example embodiment, the primary agent 102's goal is to minimize the probability of colliding with dynamic object B 112 between the current time and the time the primary agent 102 reaches the desired destination or achieves a specific goal, or between the current time and the time dynamic object B 112 runs out of fuel. This process improves upon techniques for motion planning for avoiding collisions with dynamic objects (104, 112) by using a planning graph and collision detection to increase the efficiency and response time of finding the best path to avoid collisions with dynamic objects (104, 112) that may attempt to collide with the autonomous primary agent. Additionally, some implementations use the same process to identify paths of the primary agent 102 that have a zero probability of colliding with one or more static objects in the environment in which the primary agent 102 operates. In situations where collisions with such static objects should be avoided, the motion planner 280 assigns a weight having an infinite value to each of a plurality of edges of the planning graph 500 that have a respective probability of colliding with a static object in the environment 100. In this way, when the motion planner chooses the minimum cost path during optimization, it will avoid such paths with edge weights set to infinity, since crossing that edge will definitely result in a collision with a static object. In these implementations, the velocity or trajectory of the static objects is not considered.

In some implementations, there may be multiple other agents, such as dynamic objects (104, 112), with the primary agent 102 having a goal of avoiding some of the other agents and the primary agent 102 having a goal of intercepting or colliding with others of the other agents. In such implementations, the procedures described herein for the primary agent 102 to collide with the dynamic objects (104, 112) and the procedures described herein for the primary agent 102 to avoid collisions with the dynamic objects (104, 112) may be implemented simultaneously, synchronously, or otherwise in conjunction with one another. For example, some objects may be identified as objects to collide with and other objects may be identified as objects to avoid collision with. The motion planner 280 then performs optimization as described herein, taking into account trajectory and sensor information corresponding to dynamic and static objects and whether such objects should be collided with or avoided. In this case, setting the collision probability of the edges of the planning graph based on the collision evaluation includes assigning weights (e.g., by modifying/adjusting initial weights) to collide or avoid collision accordingly.

The motion planner 280 may perform optimization to identify the path in the resulting plan map 500 that has the lowest probability of colliding with the dynamic object B 112 along the entire route of the primary agent 102. In some implementations, the length of the route may be defined at least in part by when the primary agent 102 runs out of fuel/power. A variable indicating the remaining fuel or power of the primary agent 102 may be stored by the computer system 200. In some implementations, the motion planner 280 may perform optimization to identify the path in the resulting plan map 500 that has the longest travel duration, which is specified by the path having a relatively low probability of colliding with the dynamic object B 112. The path may also be identified based on changes or predicted changes in the trajectory 106 of the dynamic object B 112. The collision assessment and optimization process may be re-executed in real time or near real time with each change or predicted change in the trajectory 106 of the dynamic object B 112. In addition, the resulting planning map 500 may have data representing the physical or performance constraints of the master agent and/or dynamic objects, the acceleration, pitch, roll, and yaw of the master agent 102, and in some implementations, the acceleration, pitch, roll, and yaw of the dynamic object B 112. Optimization to identify the path may then be performed based on these variables. For example, if the pitch, roll, and/or yaw of dynamic object B 112 changes, this may indicate a change in the trajectory of dynamic object B 112 (or result in a predicted change).

The motion planner 280 can be programmed for use with a wide range of autonomous vehicles and robots (with and without attachments) and anticipated mission scenarios. The motion planner 280 can be reused or reprogrammed for different vehicles or robots, or the motion planner 280 can be designed for use with a specific vehicle or robot. One type of robot is an autonomous vehicle, such as the autonomous vehicles described herein.

6 is a schematic diagram of an environment 100 in which a primary agent 102 (e.g., an autonomous vehicle, a robot with or without attachments, etc.) is operable and other agents, such as dynamic object A 104 and dynamic object B 112, have known trajectories (e.g., _tA 110 and _tB 106, respectively), according to one illustrated embodiment. In this scenario, the trajectory of the primary agent 102 can be planned taking into account the intent of the other agents in the environment 100, i.e., their changing trajectories over time, rather than just their current trajectories. This in turn allows conditional actions to be taken on portions of the primary agent 102 in response to the changing trajectories of other agents, such as dynamic object A 104 and dynamic object B 112. For example, in the environment 100 of the primary agent 102, if a person runs into the road, the action to be taken relative to the trajectory of the primary agent 102 will depend on whether the person continues running or stops. In other words, knowing the trajectory of an agent (e.g., a person in this example) allows for scenarios of changing circumstances to be considered, rather than planning a complete path through the grid independently of what other agents in the environment 100 may be doing. Additionally, this approach avoids double counting of collisions, which may occur if an entire set of trajectories is applied to the edges of a motion graph before planning a path.

FIG7 is an example motion planning graph 700 for the master agent 102 of FIG6. In an embodiment, each node in the grid (e.g., _n0 , _n1 , _n2, ...) has an associated value (i.e., cost) based on the cost associated with the edge of the grid (e.g., _c0,4 , _c0,5 , etc.) between the node and the goal (i.e., final state) of the master agent 102.

The grid is initialized by performing static background collision detection to find edges that collide with static objects (e.g., static object C 108). In this case, a cost may be assigned (or a cost function may be applied) to edges that have been determined to cause collisions with static objects (e.g., the edge between n ₁₄ and n ₁₅ ), resulting in a relatively high cost. For example, the cost may be set to be infinite, thereby effectively preventing the trajectory of the primary agent 102 from including edges identified as colliding with static objects. In a second aspect of the initialization of the grid, a cost to a target node (e.g., n ₁₅ ) is determined for each node based on, for example, the minimum cost path from the node in question to the target node. For example, the cost of node _n13 can be determined by the cost of the edge between _n13 and _n16 ( _c13,16 ) and the cost of the edge between _n16 and _n15 ( _c16,15 ).

Motion planning for the primary agent 102 may be performed using the graph 700 depicted in FIG. 7 beginning at time T=i at a node denoted n (e.g., n ₀ ). As explained above, motion planning takes into account the intentions of other agents in the environment 100 of the primary agent 102, such as the intentions of dynamic objects ( 104 , 112 ). The intentions are sampled, for example using a behavior model based on a probability function, to generate a trajectory t for each agent A _j , thereby generating a set of trajectories S . As explained in further detail below, the minimum cost path is determined when each trajectory t is applied to the graph 700 individually, and then an averaging of the costs is performed. This is in contrast to methods that apply the entire set of trajectories to the motion planning graph before determining the minimum cost path.

For each trajectory t in S , a determination is made as to which edges (if any) in the motion planning graph 700 collide with the trajectory, i.e., as to which edges will cause the primary agent 102 to collide with another agent corresponding to trajectory t. The cost values of the edges are modified, for example, by applying a cost function that determines the cost associated with the collision, such as a function that causes high values to be assigned to edges in collision.

After the cost of the edges of the graph 700 has been modified based on the trajectory t , a cost is calculated for each of the candidate nodes n', i.e., nodes reachable from the current node n (e.g., _n0 ) within a single time step (i.e., at time T=i+1). The costs of the candidate nodes n' (e.g., _n3 , _n4 , _n5 , and _n1 ) are calculated by finding the minimum cost path from the current node n (e.g., n0) to the target (e.g., n15) that passes through the candidate nodes n' _{. FIG} . 7 shows an example of a first minimum cost path 710 from node _{n0 to} the target (node _n15 ) that passes through candidate node _n4 and a second minimum cost path 720 from node _n0 to the target that passes through candidate node _n5 . In these examples, for trajectory t , the cost of node n4 will be the sum of the edges along the first path (e.g., c _0,4 , c _4,9 , c _9,13 , c _13,16 , c _16,15 ).

The cost of candidate node n ' is calculated in the above manner for each trajectory ( _t1 , _t2 , ... _tm ) in the set of trajectories S, each trajectory corresponding to agent _Aj ( j = 1 to m ), where m is the number of other agents. The costs are averaged over the set of trajectories S to provide an average cost for each candidate node n'. The candidate node n ' with the lowest average cost is selected as the next node of the primary agent. Therefore, at time T = i +1, the candidate node n ' with the lowest average cost becomes the current node n for the next time step T = i +2. This situation continues until the primary agent 102 reaches the target node (e.g., _n15 ), that is, reaches the state represented by the target node.

FIG8A is a flow chart showing a method 800 of identifying paths of a primary agent through a planning graph that traverse candidate nodes with the lowest average cost taking into account known trajectories of other agents, according to one illustrated embodiment. At 805, the system performs static background collision detection. At 810, a cost to a goal is calculated for each node. As discussed above with respect to FIG7, each node in the grid (e.g., _n0 , _n1 , _n2 ...) has an associated value (i.e., cost) that is based on the cost associated with the edge of the grid (e.g., _c0,4 , _c0,5 , etc.) between the node and the goal (i.e., final state) of the primary agent 102. In particular, the costs associated with the edges of the grid are determined based on the inherent costs (e.g., fuel and/or energy costs) associated with movement between two nodes along the edge in question. In an implementation, the cost to the target node (e.g., n ₁₅ ) is determined for each node based on the minimum cost path from the node in question to the target node. At 815 , the system determines the trajectories t of the other agents A _j in the environment 100 of the primary agent. At 820 , for the current node n , i.e., the current position of the primary agent in the motion planning graph 700 , the system calculates the average cost of each candidate node n ' on a set of trajectories S, as further explained in FIG. 8B and its corresponding description below. At 825, the state (eg, posture) of the primary agent 102 in the motion planning graph 700 is moved from node n to the candidate node n' with the lowest average cost. At 830, time is incremented and the method 800 is repeated for the next time step.

FIG. 8B is a flow chart showing a method 850 that can be used to calculate the cost of each candidate node, which is averaged over a set of known trajectories in the method of FIG. 8A (see block 820), according to one illustrated embodiment. At 855, for t = 1 to m , a loop is started to consider each trajectory t in the set of trajectories S, where m is the number of trajectories. At 860, if present, the system determines which edges of the motion planning graph 700 collide with trajectory t . At 865, if present, the system applies the cost function to the value of the edge determined to collide with trajectory t . At 870, the system determines the cost of each candidate node n' based on the minimum cost path from node n to the target through each candidate node n' . At 875, the index t identifying the track is incremented and method 850 is repeated until all tracks have been processed.

FIG. 9 is a schematic diagram of an environment 100 in which a primary agent 102 (e.g., an autonomous vehicle, a robot with or without attachments, etc.) can operate and in which the primary agent 102 and other agents (e.g., dynamic object A 104 and dynamic object B 112) have interdependent trajectories according to one illustrated embodiment. The trajectories (e.g., _XA and _XB ) of the dynamic objects (104, 112) can be probabilistically modeled. In the disclosed embodiment, dynamic object A 104 and dynamic object B 112 can react to the movements of both the primary agent 102 and all other agents in the environment (including each other). Thus, a behavioral model of each agent is developed that treats the agent's intentions as modeling potential strategies or goals rather than simple trajectories. Potential strategies or goals are in a form that can be sampled to determine how agents will react to other agent trajectories. When the primary agent 102 is at node n at the current time T, the system attempts to determine where the other agents will be in the future. First, the strategies of the other agents are simulated forward to the current time T based on the primary agent's path from its starting node to node n and considering the probabilistic reactions of the secondary agents to the actions of the primary agent and the actions of all secondary agents. Therefore, the probability function given to the secondary agent represents at least some of the actions of the primary and secondary agents up to the current time. This produces a result that indicates the space occupied by the other agents at the current time T. This is because the location of another agent at the current time T depends on the trajectory followed by all other agents and the primary agent 102 up to the current time T.

FIG10 is an example motion planning graph 1000 for the primary agent 102 of FIG9 showing examples of a first minimum cost path 1010 and a second minimum cost path 1020. According to one illustrated embodiment, the second minimum cost path 1020 is determined based on the probabilistically determined trajectories of other agents calculated after the primary agent 102 has moved along the first minimum cost path 1010 from a current node (e.g., _n0 ) to a candidate node (e.g., _n4 ).

In an embodiment, each node in the grid (e.g., _n0 , _n1 , _n2, ...) has an associated value (i.e., cost) that is based on the cost associated with the edges of the grid between the node and the target of the primary agent. The grid is initialized by performing static background collision detection to find edges that collide with static objects (e.g., static object C 108). In this case, a cost may be assigned (or a cost function may be applied) to the edges that have been determined to cause collisions with static objects (e.g., the edge between _n14 and _n15 ), resulting in a relatively high cost. For example, the cost may be set to be infinite, thereby effectively preventing the trajectory of the primary agent 102 from including edges identified as colliding with static objects. In a second aspect of initialization of the grid, a cost to a target is determined for each node based on, for example, the minimum cost path from the node in question to the target node (e.g., _n15 ).

In an embodiment, the other agents _Aj are modeled so that each has its own probabilistic behavior model. For each agent _Aj , the next motion planning step is given by the probability function _Xj of the actions taken by all other agents and the main agent 102 since time T=0. The probability function _Xj can be sampled k times based on factors such as standard deviation, variance, and processing cost/time.

Motion planning for the primary agent 102 may be performed using the graph 1000 depicted in FIG. 10 beginning at time T=0 at a node denoted n (e.g., _n0 ). For each agent _Aj , the system determines which grid edges, if any, collide with each agent moving to the node where the primary agent would be at time T=1. The values of the colliding edges are modified using a cost function that measures the cost of predicted actual collisions and assigns high values to edges in collision. Based on the modified edge costs, the value of each candidate node n ' (e.g., _n3 , _n4 , _n5 , and _n1 ) is calculated. The minimum cost path from the current node (e.g., _n0 ) to the target (e.g., _n15 ) is determined through each of the candidate nodes n ' .

For planning purposes, it is assumed that the primary agent 102 moves to the candidate node n ' with the smallest value, which in the example depicted in FIG. 10 is node _n4 . The minimum cost path 1010 through node n4 may be, for example, a path that passes through nodes _n4 , _n8 , _n12 , _n16 and ends at node _n15 (i.e., the goal). As explained in further detail below, this is only a planned path, not the actual path of the primary agent 102 in the motion planning graph 1000. In other words, this planned move does not result in a motion command being sent to the primary agent 102 by the motion planning system.

The planned movement of the master agent 102 to node _n4 , i.e., the hypothetical movement, affects the paths of the other agents, as determined by the probability model. This means that the minimum cost path 1010 determined at time T=0 where the master agent ₁₀₂ is at node _n0 may no longer be the minimum cost path at time T=1. The calculations described above are repeated under the assumption that node n4 is the current node of the master agent 102, and a new minimum cost path 1020 is determined from node _n4 , such as a path that passes through nodes _n9 , _n13 , _n16 and ends at the target node _n15 . The calculations are repeated until the planned route of the master agent 102 reaches the target, such as node _n15 .

After mapping the planned movement of the primary agent 102 in this manner, the motion planning graph 1000 will have edges whose costs have been determined based on the planned route, which in turn is based on the probability function used to model other agents in the primary agent's environment. The candidate node n' with the lowest value (ie, cost) is determined, and the primary agent 102 moves from the current node n ₀ to the candidate node n' (eg, n ₃ , n ₄ , n ₅ , or n ₁ ). After this actual movement of the primary agent 102 in the motion planning graph 1000, the procedure described above is repeated at the next time step T=T+1 from the new current node n (eg, n ₃ , n ₄ , n ₅ or n ₁ ).

FIG. 11A is a flow chart showing a method 1100 of identifying paths of a primary agent 102 through a planning graph that traverse candidate nodes with the lowest average cost considering the planned path of the primary agent 102 to a target and the probabilistically determined paths of other agents, according to one illustrated embodiment. At 1105, the system performs static background collision detection, as described above. At 1110, the cost to the target is calculated for each node of the motion planning graph 1000. At 1115, the system determines a probability model for the next step size X _j for each agent A _j to generate a set of models S. At 1120, the system calculates the value of each candidate node n ' based on the planned path of the primary agent 102 from the current node n to the target and the probabilistically determined paths of the other agents, as further explained in FIG. 11B and its corresponding description below. At 1125, the primary agent 102 moves from its current node n to the candidate node n' with the lowest value (i.e., cost). At 1130, the method 1100 is repeated in the next time step T=T+1.

FIG. 11B is a flow chart showing a method 1135 for calculating the value of each candidate node based on the planned path of the primary agent 102 to the target and the probabilistically determined paths of the other agents, which is applicable to the method 1100 (block 1120) of FIG. 11A according to one illustrated embodiment. At 1140, the system determines the value of each candidate node n ' based on the sampled subsequent step lengths _xj of the other agents _Aj based on a set of models S , as further explained in FIG. 11C and its corresponding description below. At 1142, the system specifies the next planned position of the primary agent 102 based on the movement from the current node _n0 to the candidate node n' with the lowest value at time T+1. At 1144, if the primary agent 102 is not yet at the target node, the time is incremented (at 1146) and the method 1135 is repeated at the next time step. If the primary agent 102 is at the target node, the process returns to the method 1100 (after block 1120).

FIG. 11C is a flow chart showing a method 1150 for determining the value of each candidate node n ' at the next time step _T +1 based on edge collision cost from the sampled subsequent steps Xj of other agents _Aj based on a set of probability models S , according to an illustrated embodiment, which is applicable to the method 1135 (block 1140) of FIG. 11B. At 1155, iterate based on the number of samples per agent p . At 1160, iterate based on the number of other agents j . At 1165, the system determines the kth sample of the probability next step _Xj of agent _Aj based on the set of models S. At 1170, the system determines which edges of the motion planning graph 1000 collide with the determined probability step length Xj of agent _Aj at time T+1, if any. At 1175, the system applies the cost function to the values of the edges determined to collide with the probability step length _Xj of agent _Aj at time T+1, if any. At 1180, the iterative loop for other agents is repeated until all other agents have been processed. At 1185, after the iterative loop for other agents has been completed j times, the iterative loop for the sample is repeated until all samples have been completed, i.e., k times.

As explained above, in implementation, the probabilistic behavior model of agents _Aj is interdependent because the trajectory of each agent _Aj depends on the trajectory of all other agents and the master agent. Therefore, the current and past positions of all agents _Aj and the master agent are provided as input to the probabilistic behavior model to determine the predicted next step length _Xj for each of the agents _Aj . In this case, the window or history or look back (looking back) is usually limited. Therefore, the innermost loop of the process (depicted in Figure 11C) is the loop used to determine the predicted next step length _Xj for all agents _Aj before repeated sampling. In this way, all samples are performed based on the same current position of all agents _Aj and the master agent.

The foregoing detailed description has been illustrated by using block diagrams, schematics, and examples to illustrate various embodiments of devices and/or programs. To the extent that such block diagrams, schematics, and examples contain one or more functions and/or operations, those skilled in the art should understand that each function and/or operation within such block diagrams, flow charts, or examples can be implemented individually and/or collectively by a wide range of hardware, software, firmware, or any combination thereof. In one embodiment, the subject matter of the present invention can be implemented via an application specific integrated circuit (ASIC) and/or FPGA. However, those skilled in the art will recognize that the embodiments disclosed herein may be implemented in whole or in part in a variety of different implementations in standard integrated circuits as one or more computer programs running on one or more computers (e.g., one or more programs running on one or more computer systems), one or more programs running on one or more controllers (e.g., microcontrollers), one or more programs running on one or more processors (e.g., microprocessors), firmware, or any combination thereof, and that designing circuit systems and/or writing program code for software and/or firmware based on the present invention will be well within the skills of those skilled in the art.

Those skilled in the art will recognize that many of the methods or algorithms described herein may use additional actions, may omit some actions, and/or may perform the actions in an order different from the order specified.

In addition, those skilled in the art will appreciate that the mechanisms taught herein can be distributed as program products in a variety of forms, and that the exemplary embodiments apply equally regardless of the specific type of signal-carrying media used to actually implement the distribution. Examples of signal-carrying media include, but are not limited to, the following: recordable media such as hard drives, CD ROMs, and computer memory.

The various embodiments described above may be combined to provide other embodiments. All generally assigned U.S. patent application publications, U.S. patent applications, foreign patents, and foreign patent applications mentioned in this specification and/or listed in the application data sheet are incorporated herein by reference in their entirety, including but not limited to: Apparatus, method, and article to facilitate motion planning of an autonomous vehicle in an environment having dynamic objects, filed on January 12, 2018, entitled "APPARATUS, METHOD, AND ARTICLE TO FACILITATE MOTION PLANNING OF AN AUTONOMOUS VEHICLE IN AN ENVIRONMENT HAVING DYNAMIC and U.S. Patent Application No. 62/616,783, entitled “MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS,” filed on June 9, 2017; and International Patent Application Publication No. WO 016/122840, entitled “SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME,” filed on January 5, 2016. These and other changes can be made to the embodiments in light of the above detailed description. In general, in the following claims, the terms used should not be interpreted as limiting the claims to the specific embodiments disclosed in this specification and the claims, but should be interpreted to include all possible embodiments together with the full range of equivalents to which the claims are entitled. Accordingly, the claims are not limited by the present disclosure.

100: Dynamic operating environment 102: Main agent/vehicle 104: Dynamic object A 106,110,XA,XB: Track 108: Static object C 112: Dynamic object B 200: Computer system 212a: Hardware processor/processing unit/central processing unit 212b: Processing unit/digital signal processor 214: System memory 216: System bus 218: Read-only memory 220: Random access memory 222: Basic input/output system 224: Disk drive 226: Computer readable media 236: Operating system 238: Application program 240: Other programs/modules 242: Program data 260: Network interface 266: Actuator system 280: Motion planner 282: Sensor 284: Object detector 286: Object behavior predictor 288: Collision evaluator 290: Field programmable gate array (FPGA) 292: Path optimizer 300: Data stream 302: Perception information 306: Predicted trajectory information 308: Trajectory information 310: Identified path 400, 500, 700, 1000: Motion planning graph 408a, 408b, 408c, 408d, 508a, 508b, n ₀ , n ₁ , n ₂ , n ₅ , n ₆ , n ₇ , n ₈ , n ₉ , n ₁₀ , n ₁₁ , n ₁₂ , n ₁₃ , n ₁₄ , n ₁₅ , n ₁₆ : Node 410a, 410b, 410c, 410d, 410e, 410f, 410g, 410h, 410i, 410j, 410k, 510a, 510b, 510c, 510d, 510e, 510f, 510g, 510h, 510i, 510j, 510k, 510l, 510m, 510n, 510o, 510p: Edge 412, 512: Path 414, 514: Graph portion 710, 1010: First minimum cost path 720, 1020: Second minimum cost path 800, 850, 1100, 1135, 1150: Method 820, 1120, 1140: Block n ₄ :Candidate Node

In the drawings, the same reference numerals identify similar elements or actions. The size and relative positions of the elements in the drawings are not necessarily drawn to scale. For example, the shapes and angles of various elements are not drawn to scale, and some of these elements are arbitrarily enlarged and positioned to improve drawing readability. In addition, the specific shapes of the drawn elements are not intended to convey any information about the actual shape of the specific element, but have simply been selected for easy identification in the drawings.

1 is a schematic diagram of an environment in which a primary agent (e.g., an autonomous vehicle, a robot with or without attachments, etc.) may operate according to one illustrated embodiment.

2 is a functional block diagram of a computer system associated with a primary agent (e.g., an autonomous vehicle, a robot with or without removable appendages, etc.) that may operate in the environment of FIG. 1 according to one illustrated embodiment.

3 is a block diagram showing example data flow between various components in the computer system of FIG. 2 according to one illustrated embodiment.

4A is an example motion planning diagram of the primary agent of FIG. 1 when the primary agent's goal is to collide with a dynamic object of FIG. 1 that may attempt to avoid the primary agent, according to one illustrated embodiment.

4B is an example motion planning graph of the primary agent of FIG. 1 when the primary agent's goal is to collide with a dynamic object of FIG. 1 that may attempt to avoid the primary agent, and example paths identified in the primary agent's planning graph to collide with the dynamic object, according to one illustrated embodiment.

5A is an example motion planning diagram of the primary agent of FIG. 1 when the primary agent's goal is to avoid collision with a dynamic object that is close to the primary agent of FIG. 1 according to one illustrated embodiment.

5B is an example motion planning graph of the primary agent of FIG. 1 when the primary agent's goal is to avoid collision with a dynamic object that is close to the primary agent of FIG. 1 and an example path identified in the primary agent's planning graph to avoid collision with the dynamic object according to one illustrated embodiment.

6 is a schematic diagram of an environment in which a primary agent (e.g., an autonomous vehicle, a robot with or without attachments, etc.) may operate and in which other agents have known trajectories, according to one illustrated embodiment.

7 is an example motion planning graph of the primary agent of FIG. 6 showing examples of minimum cost paths through each of two candidate nodes, where the costs are determined based on the known trajectories of other agents, according to one illustrated embodiment.

8A is a flow chart showing a method for identifying paths of a primary agent through a planning graph that traverse candidate nodes with the lowest average cost taking into account known trajectories of other agents according to one illustrated embodiment.

8B is a flow chart showing a method that may be used to calculate the cost of each candidate node, which is averaged over a set of known trajectories in the method of FIG. 8A, according to one illustrated embodiment.

9 is a schematic diagram of an environment in which a primary agent (e.g., an autonomous vehicle, a robot with or without attachments, etc.) may operate and in which the primary agent and other agents have interdependent trajectories according to one illustrated embodiment.

Figure 10 is an example motion planning diagram for the primary agent of Figure 9 according to an illustrated embodiment, showing examples of a first minimum cost path and a second minimum cost path determined based on the trajectories of other agents calculated from a probability model after the primary agent's planned movement from a current node to a candidate node along the first minimum cost path.

11A is a flow chart showing a method for identifying paths of a primary agent through a planning graph that traverse candidate nodes with the lowest average cost considering the planned path of the primary agent to a target and the paths of other agents determined using a probability model according to one illustrated embodiment.

11B is a flow chart showing a method for calculating the value of each candidate node based on the planned path of the primary agent to the target and the paths of other agents determined from the probability model, which is applicable to the method of FIG. 11A , according to one illustrated embodiment.

11C is a flow chart showing a method for determining the value of each candidate node at the next time step based on edge collision costs from sampled subsequent steps of other agents based on a probability model, according to an illustrated embodiment, which method is applicable to the method of FIG. 11B .

100: Dynamic operating environment

102:Main Agent/Vehicle

104: Dynamic Object A

106,110: Track

108: Static Object C

112: Dynamic Object B

Claims (66)

A motion planning method operating in a processor-based system to perform motion planning via planning graphs, wherein each planning graph comprises a plurality of nodes and edges, each node implicitly or explicitly representing time and variables characterizing a state of a primary agent operating in an environment including one or more other agents, and each edge representing a transition between a respective pair of the nodes, the method comprising: for a first planning graph a current node in the first planning graph, for each trajectory in a set of trajectories representing actual or expected trajectories of at least one of the one or more other agents, if any of the edges in the first planning graph collides with the respective trajectory, determining which edges collide with the respective trajectory; applying a cost function to one or more of the respective edges to reflect at least one of the determined collision or the absence thereof; for a plurality of candidate nodes in the first planning graph, For each of the points, the candidate node is any node in the first planning graph that is directly coupled to the current node in the first planning graph by a respective single edge in the first planning graph, finding a minimum cost path from the current node to a target node in the first planning graph, the minimum cost path being transmitted directly from the current node to the respective candidate node and then to the target node, between the respective candidate node and the target node along a pair of The method further comprises: determining a path that has or does not have a plurality of intervening nodes in succession; after finding the minimum cost path for each of the candidate nodes with respect to the trajectories in the set of trajectories, calculating a respective value for each of the candidate nodes based at least in part on a respective cost associated with each minimum cost path across all of the trajectories and the respective candidate node; and selecting one of the candidate nodes based at least in part on the calculated respective values. The motion planning method of claim 1, wherein applying a cost function to one or more of the individual edges to reflect at least one of the determined collision or its non-existence comprises: for any of the edges determined to have collided with at least one trajectory, increasing a cost of the individual edge to a relatively high value to reflect the determined collision, wherein the relatively high value is relatively higher than a relatively low value, the relatively low value reflecting the non-existence of one of the collisions with at least one other edge. The motion planning method of claim 1, wherein applying a cost function to one or more of the individual edges to reflect at least one of the determined collision or its non-existence comprises: for any of the edges determined not to collide with at least one trajectory, increasing a cost of the individual edge to a relatively high value to reflect the determined non-existence of the collision, wherein the relatively high value is relatively higher than a relatively low value, the relatively low value reflecting a collision with at least one other edge. The motion planning method of claim 1 further comprises: for each of at least one of the other agents in the environment, sampling to determine the respective expected trajectory of the other agent; and forming the set of trajectories from the determined respective actual or expected trajectory of each of the other agents. The motion planning method of claim 1 further comprises: based on the waiting node being any node in the first planning graph directly coupled to the current node in the first planning graph by a respective single edge of the first planning graph, selecting the waiting node in the first planning graph from the other nodes of the first planning graph. A motion planning method as claimed in claim 1, wherein calculating a respective value based at least in part on a respective cost associated with each minimum cost path across all of the trajectories and the respective candidate node comprises calculating an average of the respective costs associated with each minimum cost path extending from the current node to the target node via the respective candidate node and, if present, via all of the intervening nodes. A motion planning method as claimed in claim 1, wherein selecting one of the candidate nodes based at least in part on the calculated respective values comprises selecting the candidate node having a respective calculated value that is the minimum of all calculated values among the candidate nodes. The motion planning method of claim 1 further comprises: updating a trajectory of the main agent based on the selected one of the waiting nodes. The method of claim 1, further comprising: initializing the first planning graph before applying the cost function to the respective edges to reflect the determined collisions. The method of claim 9, wherein initializing the first planning graph includes: for each edge in the first planning graph, performing a collision evaluation on the edge with respect to each of a plurality of static objects in the environment to identify collisions between the respective edge and the static objects, if any. The method of claim 10, wherein initializing the first planning graph further comprises: for each edge evaluated to collide with at least one of the static objects, applying a cost function to the respective edge to reflect the evaluated collision or removing the edge from the first planning graph. A method as in any one of claims 9 to 11, wherein initializing the first planning graph further comprises: for each node in the first planning graph, calculating a cost from the node to the target node; and logically associating the calculated costs with the respective nodes. The method of any one of claims 1 to 11, further comprising: assigning the selected one of the waiting nodes as a new current node in the first planning graph; For the new current node in the first planning graph, for each trajectory in a set of trajectories representing the actual or expected trajectory of at least one of the one or more other agents, any one of the edges in the first planning graph In the case of a collision with the respective trajectory, determining which edges collide with the respective trajectory; applying a cost function to one or more of the respective edges to reflect at least one of the determined collision or its absence; and for each of a plurality of new candidate nodes in the first planning graph, the new candidate nodes are directly coupled to the respective single edge of the first planning graph by a respective single edge of the first planning graph. any node of the new current node in the first planning graph, finding a minimum cost path from the new current node to a target node in the first planning graph, the minimum cost path being directly transmitted from the new current node to the respective new candidate node and then to the target node, with or without a plurality of intervening nodes continuously along a corresponding path between the respective new candidate node and the target node; After finding the minimum cost path to each of the new candidate nodes with respect to the trajectories in the set of trajectories, for each of the new candidate nodes, calculating a respective value based at least in part on a respective cost associated with each minimum cost path across all of the trajectories to the respective new candidate node; and selecting one of the new candidate nodes based at least in part on the calculated respective value. A processor-based system for executing motion planning via planning graphs, wherein each planning graph comprises a plurality of nodes and edges, each node implicitly or explicitly representing time and variables representing a state of a primary agent operating in an environment including one or more other agents, and each edge representing a transition between a respective pair of such nodes, the system comprising: at least one processor; and at least one non-transitory processor-readable medium storing processor-executable instructions or data, the processors may execute instructions or data that, when executed by the at least one processor, cause the at least one processor to perform the following operations: for a current node in a first planning graph, for each trajectory in a set of trajectories representing actual or expected trajectories of at least one of the one or more other agents, if any of the edges in the first planning graph collides with the respective trajectory, determine which edges collide with the respective trajectory; apply a cost function to the respective edges; to reflect at least one of a determined collision or its non-existence; for each of a plurality of candidate nodes in the first planning graph, the candidate node is any node in the first planning graph that is directly coupled to the current node in the first planning graph by a respective single edge of the first planning graph, finding a minimum cost path from the current node to a target node in the first planning graph, the minimum cost path being directly transmitted from the current node to the respective candidate node and then to the target node, successively with or without a plurality of intervening nodes along a corresponding path between the respective candidate node and the target node; after finding the minimum cost path for each of the candidate nodes with respect to the trajectories in the set of trajectories, for each of the candidate nodes, calculating a respective value representing an average of a respective cost associated with each minimum cost path to the respective candidate node across all of the trajectories; and selecting one of the candidate nodes based at least in part on the calculated respective value. A processor-based system as claimed in claim 14, wherein in order to apply a cost function to one or more of the respective edges to reflect at least one of the determined collision or its absence, the at least one processor performs the following operations: for any of the edges determined to collide with at least one trajectory, increase a cost of the respective edge to a relatively high value to reflect the determined collision, the relatively high value being relatively higher than a relatively low value, the relatively low value reflecting the absence of a collision for at least one other edge. A processor-based system as claimed in claim 14, wherein to apply a cost function to one or more of the respective edges to reflect at least one of the determined collision or its absence, the at least one processor performs the following operations: for any of the edges determined not to collide with at least one trajectory, increase a cost of the respective edge to a relatively high value to reflect the determined absence of a collision, the relatively high value being relatively higher than a relatively low value, the relatively low value reflecting a collision with at least one other edge. A processor-based system as claimed in claim 14, wherein the at least one processor further performs the following operations: for each of at least one of the other agents in the environment, sampling the intentions of the other agents to determine the respective expected trajectories of the other agents; and forming the set of trajectories from the determined respective actual or expected trajectories of each of the other agents. A processor-based system as claimed in claim 14, wherein the at least one processor further performs the following operation: based on the waiting selected node being any node in the first planning graph directly coupled to the current node in the first planning graph by a respective single edge of the first planning graph, selecting the waiting selected node in the first planning graph from the other nodes of the first planning graph. A processor-based system as claimed in claim 14, wherein to calculate a respective value based at least in part on a respective cost associated with each minimum cost path across all of the trajectories to the respective candidate node, the at least one processor calculates an average of the respective costs associated with each minimum cost path extending from the current node to the target node through the respective candidate node and, if present, through all of the intervening nodes. A processor-based system as claimed in claim 14, wherein in order to select one of the candidate nodes based at least in part on the calculated respective values, the at least one processor selects the candidate node among the candidate nodes having the respective calculated value that is the minimum of all calculated values. A processor-based system as claimed in claim 14, wherein the at least one processor further performs the following operations: updating a trajectory of the primary agent based on the selected one of the waiting nodes. A processor-based system as claimed in claim 14, wherein the at least one processor further performs the following operations: initializing the first planning graph before applying the cost function to the respective edges to reflect the determined collisions. A processor-based system as claimed in claim 22, wherein to initialize the first planning graph, the at least one processor performs the following operations: for each edge in the first planning graph, performing a collision evaluation on the edge with respect to each of a plurality of static objects in the environment to identify collisions between the respective edge and the static objects, if any. A processor-based system as claimed in claim 23, wherein to initialize the first planning graph, the at least one processor further performs the following operations: for each edge evaluated as colliding with at least one of the static objects, applying a cost function to the respective edge to reflect the evaluated collision or removing the edge from the first planning graph. A processor-based system as claimed in any one of claims 22 to 24, wherein to initialize the first planning graph, the at least one processor further performs the following operations: for each node in the first planning graph, calculating a cost from the node to the target node; and logically associating the calculated costs with the respective nodes. A processor-based system as in any of claims 14 to 24, wherein the at least one processor further performs the following operations: assigning the selected one of the waiting nodes as a new current node in the first planning graph; for the new current node in the first planning graph, for each trajectory in a set of trajectories representing actual or expected trajectories of at least one of the one or more other agents, In the event that any of the edges of a first planning graph collides with the respective trajectory, determining which edges collide with the respective trajectory; applying a cost function to one or more of the respective edges to reflect at least one of the determined collision or its absence; and for each of a plurality of new candidate nodes in the first planning graph, the new candidate nodes are selected from a respective one of the first planning graph and the first candidate node. A single edge is directly coupled to any node of the new current node in the first planning graph, finding a minimum cost path from the new current node to a target node in the first planning graph, the minimum cost path passing from the new current node directly to respective new candidate nodes and then to the target node, with or without a number of intervening nodes successively along a corresponding path between the respective new candidate nodes and the target node node; after finding the minimum cost path to each of the new candidate nodes with respect to the trajectories in the set of trajectories, for each of the new candidate nodes, calculating a respective value based at least in part on a respective cost associated with each minimum cost path across all of the trajectories to the respective new candidate node; and selecting one of the new candidate nodes based at least in part on the calculated respective values. A method operating in a motion planning system employing a graph having nodes representing states and edges representing transitions between states, the method comprising: for each available next node relative to a current node in a first graph, calculating, by at least one processor, a respective associated representative cost of reaching a target node from the current node via the respective next node, the respective associated representative cost being a cost based on a non-deterministic behavior of each of one or more agents in an environment with respect to the target node in the environment; An evaluation of a probability of collision of one or more agents reflecting a respective representative cost associated with each available path from the current node to the target node via the respective next node, the agents being able to change any one or more of a position, a velocity, a trajectory, a path of travel, or a shape over time; selecting a next node via at least one processor based on the calculated respective associated representative cost of each available next node; and commanding a move via at least one processor based at least in part on the selection of the next node. The method of claim 27, wherein calculating a respective associated representative cost for reaching a target node from the current node via the respective next node comprises: for each expected path between the current node and the target node via the respective next node, determining a respective associated representative cost for each edge along the respective expected path between the current node and the target node; and determining a respective associated representative cost for each edge along the respective expected path between the current node and the target node. Assigning a determined respective associated representative cost of each edge to each edge of the respective expected path; determining a minimum cost path to the respective next node from the respective expected paths between the current node and the target node through the respective next node based at least in part on the assigned determined respective associated representative costs; and assigning a value representing the determined minimum cost path to the respective next node. The method of claim 28, wherein determining a minimum cost path to the respective next node based at least in part on the respective expected paths between the current node and the target node through the respective next node based on the respective determined associated representative costs assigned comprises: determining a minimum cost path including a cost traversed from the current node to the respective next node. The method of claim 29, wherein determining a respective associated representative cost for each edge between the current node and the target node along the respective expected path comprises: for each edge between the current target and the target node along the respective expected path, and evaluating a risk of a collision with the one or more agents in the environment based on one or more probability functions representing the non-deterministic behavior of each of the one or more agents in the environment. The method of claim 30, wherein evaluating a risk of a collision with one or more agents in the environment based on one or more probability functions representing the non-deterministic behavior of each of the one or more agents in the environment comprises: sampling the probability functions representing the non-deterministic behavior of each of the one or more agents in the environment, respectively, given a sequence of actions represented by each of the edges along the respective expected paths between the respective next node and each successive node. The method of claim 30, wherein evaluating a risk of a collision with one or more agents in the environment based on one or more probability functions representing the non-deterministic behavior of each of the one or more agents in the environment comprises: sampling the probability functions representing the non-deterministic behavior of each of the one or more agents in the environment, respectively, given a sequence of actions represented by each of the edges between the respective next node and each successive node along the respective expected path to a current node, the current node being another node along the respective expected path reached during the evaluation of the risk of collision. The method of claim 30, wherein evaluating a risk of a collision with one or more agents in the environment based on one or more probability functions that respectively represent the non-deterministic behavior of each of the one or more agents in the environment comprises: for each of the agents, repeatedly sampling a respective probability function that respectively represents the non-deterministic behavior of the respective agent. The method of claim 33, wherein the repetitive sampling of the respective probability functions representing the non-deterministic behavior of the respective agents comprises repetitive sampling of the respective probability functions for a plurality of iterations, a total number of the iterations being based at least in part on an amount of time available before the command must occur. The method of claim 30, wherein evaluating a risk of a collision with one or more agents in the environment based on one or more probability functions representing the non-deterministic behavior of each of the one or more agents in the environment comprises: for each of the agents, given a sequence of actions represented by each of the edges along the respective expected paths between the respective next node and each successive node, repeatedly sampling the probability functions representing the non-deterministic behavior of each of the one or more agents in the environment. The method of claim 30, wherein evaluating a risk of a collision with one or more agents in the environment based on one or more probability functions representing the non-deterministic behavior of each of the one or more agents in the environment comprises: for each of the agents, given a sequence of actions represented by each of the edges between the respective next node and each successive node along the respective expected path to a current node, repeatedly sampling the probability functions representing the non-deterministic behavior of each of the one or more agents in the environment, the current node being another node along the respective expected path reached during the evaluation of the risk of collision. The method of claim 30, wherein the assessment of the risk of collision involves simulation of traversal of the respective expected paths. The method of claim 30, wherein evaluating a risk of a collision with one or more agents in the environment based on one or more probability functions representing the non-deterministic behavior of each of the one or more agents in the environment comprises: evaluating a risk of collision via dedicated risk evaluation hardware based at least on a probabilistically determined respective trajectory of each of the one or more agents in the environment, wherein the respective associated representative costs are based at least in part on the evaluated risk of collision. The method of claim 30, wherein evaluating a risk of a collision with one or more agents in the environment based on one or more probability functions representing the non-deterministic behavior of each of the one or more agents in the environment comprises: evaluating a risk of a collision with the one or more agents in the environment based on one or more probability functions representing the non-deterministic behavior of at least one of the one or more agents in the environment, the primary of the agents being an agent for which motion planning is performed. The method of claim 27 further comprises: initializing the first graph before calculating a respective associated representative cost from the current node to a target node via the respective next node. The method of claim 40, wherein initializing the first graph comprises: performing a static collision evaluation to identify any collisions with one or more static objects in the environment; for each node in the first graph, calculating a respective cost to reach a target node from the respective node; and for each node in the first graph, logically associating the respective calculated cost to reach a target node with the respective node. A processor-based system for performing motion planning employing a graph having nodes representing states and edges representing transitions between states, the system comprising: at least one processor; and at least one non-transitory processor-readable medium storing at least one of processor-executable instructions or data that, when executed by the at least one processor, cause the at least one processor to perform the following operations: for each available next node relative to a current node of a primary agent in a first graph for which the motion planning is performed, calculate a respective associated representative of a path from the current node to a target node via the respective next node; The respective associated representative costs reflect a respective representative cost associated with each available path from the current node to the target node via the respective next node in view of an evaluation of a probability of the primary agent colliding with the one or more other agents in the environment based on a non-deterministic behavior of each of the one or more other agents in the environment, the non-deterministic behavior including changing any one or more of a position, a velocity, a trajectory, a path of travel, or a shape over time; selecting a next node based on the calculated respective associated representative costs of each available next node; and commanding a move based at least in part on the selected next node. A processor-based system as in claim 42, wherein in order to calculate a respective associated representative cost of reaching a target node from the current node via the respective next node, the at least one processor performs the following operations: for a set of edges including at least the edges along any expected path between the current node and the target node, determining a respective associated representative cost for each of the edges ; assigning the determined respective associated representative costs of each edge to the respective edges in the set of edges; determining a minimum cost path to the respective next node based at least in part on the respective expected paths between the current node and the target node through the respective next node based at least in part on the assigned determined respective associated representative costs; and assigning a value representing the determined minimum cost path to the respective next node. The system of claim 43, wherein in order to determine a minimum cost path to the respective next node based at least in part on the respective expected paths between the current node and the target node through the respective next node based on the respective determined associated representative costs assigned, the at least one processor determines a minimum cost path including a cost to traverse from the current node to the respective next node. The system of claim 43, wherein to determine a respective representative cost associated with each edge in the set of edges, the at least one processor estimates a risk of a collision between the primary agent and the one or more other agents in the environment based on one or more probability functions representing the non-deterministic behavior of each of the one or more other agents in the environment. The system of claim 45, wherein in order to evaluate a risk of a collision between the primary agent and the one or more other agents in the environment based on one or more probability functions respectively representing the non-deterministic behavior of each of the one or more other agents in the environment, the at least one processor performs the following operations: given a series of actions represented by each of the edges along the respective expected paths between the respective next node and each successive node, the probability functions respectively representing the non-deterministic behavior of each of the one or more other agents in the environment are sampled. The system of claim 45, wherein in order to evaluate a risk of a collision between the primary agent and the one or more other agents in the environment based on one or more probability functions respectively representing the non-deterministic behavior of each of the one or more other agents in the environment, the at least one processor performs the following operations: given a series of actions represented by each of the edges between the respective next node and each successive node along the respective expected path leading to a current node, the probability functions respectively representing the non-deterministic behavior of each of the one or more other agents in the environment are sampled, the current node being another node along the respective expected path reached during the evaluation of the risk of collision. A system as claimed in claim 45, wherein in order to evaluate a risk of a collision between the primary agent and the one or more other agents in the environment based on one or more probability functions respectively representing the non-deterministic behavior of each of the one or more other agents in the environment, the at least one processor performs the following operations: for each of the one or more other agents, repeatedly sampling a respective probability function respectively representing the non-deterministic behavior of the respective agent. The system of claim 48, wherein to repeatedly sample the respective probability functions representing the non-deterministic behavior of the respective agents, the at least one processor repeatedly samples the respective probability functions for a plurality of iterations, a total number of the iterations being based at least in part on an amount of time available before the command must occur. The system of claim 45, wherein in order to evaluate a risk of a collision between the primary agent and the one or more other agents in the environment based on one or more probability functions respectively representing the non-deterministic behavior of each of the one or more other agents in the environment, the at least one processor performs the following operations: for each of the one or more other agents, given a series of actions represented by each of the edges along the respective expected paths between the respective next node and each successive node, the probability functions respectively representing the non-deterministic behavior of each of the one or more other agents in the environment are repeatedly sampled. A system as claimed in claim 45, wherein in order to evaluate a risk of a collision between the primary agent and the one or more other agents in the environment based on one or more probability functions respectively representing the non-deterministic behavior of each of the one or more other agents in the environment, the at least one processor performs the following operations: For each of the one or more other agents, given a series of actions represented by each edge between the respective next node and each successive node along the respective expected path leading to a current node, the probability functions respectively representing the non-deterministic behavior of each of the one or more other agents in the environment are repeatedly sampled, the current node being another node along the respective expected path reached during the evaluation of the risk of collision. The system of claim 45, wherein the assessment of the risk of collision involves simulation of traversal of the respective expected paths. The system of claim 45, wherein in order to evaluate a risk of a collision between the primary agent and the one or more other agents in the environment based on one or more probability functions representing the non-deterministic behavior of each of the one or more other agents in the environment, the at least one processor performs the following operations: evaluating a risk of collision via dedicated risk evaluation hardware based at least on a probabilistically determined respective trajectory of each of the one or more other agents in the environment, wherein the respective associated representative costs are based at least in part on the evaluated risk of collision. The system of claim 45, wherein in order to evaluate a risk of a collision between the primary agent and the one or more other agents in the environment based on one or more probability functions representing the non-deterministic behavior of each of the one or more other agents in the environment, the at least one processor performs the following operations: evaluating a risk of collision via dedicated risk evaluation hardware based at least on a probabilistically determined respective trajectory of each of the one or more other agents in the environment, wherein the respective associated representative costs are based at least in part on the evaluated cost of the collision. A system as claimed in claim 42, wherein the at least one processor further performs the following operations: initializing the first graph before calculating a respective associated representative cost from the current node to a target node via the respective next node. The system of claim 55, wherein to initialize the first graph, the at least one processor performs the following operations: performing a static collision evaluation to identify any collisions of the primary agent with one or more static objects in the environment; for each node in the first graph, calculating a respective cost for reaching a target node for each respective node; and for each node in the first graph, logically associating the respective calculated cost for reaching a target node with the respective node. A method operating in a motion planning system, which uses a graph having nodes representing states and edges representing transitions between states to generate a motion plan for a primary agent, the method comprising: initializing a step counter (T) to a starting value (T=0); initializing a first graph; performing a simulation, the simulation comprising: starting at a current node (N) in the first graph instead of a target node (G) in the first graph: for one or more sampling iterations, for one or more secondary agents in an environment; for each secondary agent in the first graph, sampling from a probability function an action that the respective secondary agent will take when the step counter increments from the starting value of the step counter to a current value (T+1, i.e., the next step), the probability function representing the action taken by the primary agent and by the one or more secondary agents; determining any edge of the first graph that collides with the next action; for any edge that collides with the next action, applying a respective cost function to the edge to reflect the existence of a collision condition; for straight for each node in the first graph of a set of nodes connected to the current node, calculating a value representing a minimum cost path from the current node to the target node via one or more expected paths traversing one or more paths via respective nodes directly connected to the current node; determining whether to perform another sampling iteration; and in response to determining not to perform another sampling iteration, selecting from the set of nodes directly connected to the current node a node having a minimum cost. the node of the present invention; incrementing the step counter (T=T+1); determining whether the simulation is at the target node, in response to a determination that the simulation is not at the target node, setting the selected node as a new current node without commanding the primary agent and continuing the simulation; in response to a determination that the simulation is at the target node, selecting a node having a minimum cost from the set of nodes directly connected to the current node; and providing an identification of the selected node having the minimum cost to command the movement of the primary agent. The method of claim 57, wherein sampling an action to be taken by the respective secondary agent as the step counter increments from the starting value of the step counter to a current value from a probability function representing actions taken by the one or more secondary agents and the primary agent comprises: sampling the probability function representing a non-deterministic behavior of each of the one or more secondary agents in the environment in view of a series of such actions represented by each of the edges between the respective node directly connected to the current node and each successive node along a path to the target node taken by the primary agent. The method of claim 57, wherein sampling the probability functions representing the non-deterministic behavior of each of the one or more agents in the environment in view of the sequence of actions represented by each of the edges comprises: for each of the agents, repeatedly sampling the respective probability functions representing the non-deterministic behavior of the respective agents. The method of claim 57, wherein the primary agent is a primary autonomous vehicle, and the method further comprises: receiving sensory information representing the environment in which the primary autonomous vehicle operates; and implementing a resulting motion plan by the primary autonomous vehicle. The method of claim 60, wherein receiving sensory information includes receiving sensory information representing a position and a trajectory of at least one dynamic object in the environment. The method of claim 60, wherein receiving sensory information includes receiving sensory information at a motion planner, the sensory information being collected via one or more sensors carried by the primary autonomous vehicle and representing a position or a trajectory of at least one other vehicle in the environment. The method of claim 62 further comprises: identifying at least one first dynamic object in the environment by an object detector from the sensing information collected by the one or more sensors. A motion planning system that uses a graph having nodes representing states and edges representing transitions between states to generate a motion plan for a primary agent, the system comprising: at least one processor; at least one non-transitory processor-readable medium storing processor-executable instructions that, when executed by the at least one processor, cause the at least one processor to perform the following operations: initialize a step counter to a starting value; initialize a first graph; and execute a simulation, the simulation comprising: at a current node in the first graph; Starting at a target node in the first graph but not at the target node in the first graph: for one or more sampling iterations, for each of one or more secondary agents in an environment, sampling from a probability function an action that the respective secondary agent will take when the step counter increases from the starting value of the step counter to a current value, the probability function representing actions taken by the primary agent and by the one or more secondary agents; determining any edges of the first graph that collide with the next action; for any edges that collide with the next action, applying a respective cost function for the edge to reflect the existence of a collision condition; for each node in the first graph of a set of nodes directly connected to the current node, calculating a value representing a minimum cost path, the minimum cost path traversing one or more paths from the current node to the target node via one or more expected paths, the one or more expected paths being through respective nodes directly connected to the current node; determining whether to perform another sampling iteration; in response to a determination not to perform another sampling iteration, selecting the set of nodes from the set of nodes directly connected to the current node; The method includes: selecting the node with a minimum cost from the nodes in the target node; incrementing the step counter; determining whether the simulation is at the target node, setting the selected node as a new current node without commanding the primary agent and continuing the simulation in response to a determination that the simulation is at the target node; selecting a node with a minimum cost from the set of nodes directly connected to the current node; and providing an identification of the selected node, the selected node having the minimum cost to command movement of the primary agent. The system of claim 64, wherein in order to sample from a probability function an action that the respective secondary agent will take when the step counter is incremented, the at least one processor samples the probability function representing the non-deterministic behavior of each of the one or more secondary agents in the environment in view of a series of such actions represented by a respective one of each edge between the respective node directly connected to the current node and each successive node along a path to the target node. A system as in claim 64, wherein in order to sample the probability functions representing the non-deterministic behavior of each of the one or more secondary agents in the environment in view of the series of actions represented by each of the edges, the at least one processor performs the following operations: for each of the secondary agents, repeatedly sample the respective probability functions representing the non-deterministic behavior of the respective agent.