Apparatus, method and article to facilitate motion planning in an environment having dynamic objects

Patent metadata

Field	Value
Publication number	US-12204336-B2
Application number	US-201917299574-A
Country	US
Kind code	B2
Filing date	Dec 4, 2019
Priority date	Dec 4, 2018
Publication date	Jan 21, 2025
Grant date	Jan 21, 2025

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.

First claim

The invention claimed is:

1. A motion planning method of operation in a processor-based system to perform motion planning via planning graphs, where each planning graph respectively comprises a plurality of nodes and edges, each node which represents, implicitly or explicitly, time and variables that characterize a state of a primary agent, which operates in an environment that includes one or more other agents, and each edge represents a transition between a respective pair of the nodes, the method comprising:
for a current node in a first planning graph,
for each trajectory in a set of trajectories that respectively represent actual or prospective trajectories of at least one of the one or more other agents,
determining which edges of the first planning graph collide with the respective trajectory, if any of the edges collide with the respective trajectory;
applying a cost function to one or more of the respective edges to reflect at least one of a determined collision or absence thereof; and
for each of a number of candidate nodes in the first planning graph, the candidate nodes being 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 least cost path from the current node to a goal node in the first planning graph that passes from the current node directly to the respective candidate node and then to the goal node, with or without a number of intervening nodes successively between the respective candidate node and the goal node along a corresponding path; and

after finding the least cost path for each of the candidate nodes with respect to the trajectories of the set of trajectories,
for each of the candidate nodes, computing a respective value based at least in part on a respective cost associated with each least cost path for the respective candidate node across all of the trajectories; and
selecting one of the candidate nodes based at least in part on the computed respective values.

2. The motion planning method of claim 1 wherein applying a cost function to one or more of the respective edges to reflect at least one of the determined collision or absence thereof includes:
for any of the edges that are determined to collide with at least one trajectory, increasing a cost of the respective edge to a relatively high magnitude to reflect the determined collision, wherein the relatively high magnitude is relatively higher than a relatively low magnitude that reflects an absence of collision for at least one other edge.

3. The motion planning method of claim 1 wherein applying a cost function to one or more of the respective edges to reflect at least one of the determined collision or absence thereof includes:
for any of the edges that are determined not to collide with at least one trajectory, increasing a cost of the respective edge to a relatively high magnitude to reflect the determined absence of collision, wherein the relatively high magnitude is relatively higher than a relatively low magnitude that reflects a collision for at least one other edge.

4. The motion planning method of claim 1 , further comprising:
for each of at least one of the other agents in the environment, sampling to determine the respective prospective trajectory of the other agent; and
forming the set of trajectories from the determined respective actual or prospective trajectory of each of the other agents.

5. The motion planning method of claim 1 , further comprising:
selecting the candidate nodes in the first planning graph from the other nodes of the first planning graph based on the candidate nodes being 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.

6. The motion planning method of claim 1 wherein computing a respective value based at least in part on a respective cost associated with each least cost path for the respective candidate node across all of the trajectories, includes computing an average value of the respective cost associated with each least cost path that extends from the current node to the goal node via the respective candidate node and via all of the intervening nodes, if any.

7. The motion planning method of claim 1 wherein selecting one of the candidate nodes based at least in part on the computed respective values includes selecting the one of the candidate nodes which has the respective computed value that is a smallest of all of the computed values.

8. The motion planning method of claim 1 , further comprising: updating a trajectory of the primary agent based on the selected one of the candidate nodes.

9. 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.

10. The method of claim 9 wherein initializing the first planning graph includes:
for each edge in the first planning graph performing a collision assessment for the edge relative to each of a number of static objects in the environment to identify collisions, if any, between the respective edge and the static objects.

11. The method of claim 10 wherein initializing the first planning graph further includes:
for each edge that is assessed as colliding with at least one of the static objects, applying a cost function to the respective edge to reflect the assessed collision or removing the edge from the first planning graph.

12. The method of claim 9 wherein initializing the first planning graph further includes:
for each node in the first planning graph, computing a cost to the goal node from the node; and
logically associating the computed cost with the respective node.

13. The method of claim 1 , further comprising:
assigning the selected one of the candidate nodes to be a new current node in the first planning graph;
for the new current node in a first planning graph,
for each trajectory in a set of trajectories that respectively represent actual or prospective trajectories of at least one of the one or more other agents,
determining which edges of the first planning graph collide with the respective trajectory, if any of the 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 absence thereof; and
for each of a number of new candidate nodes in the first planning graph, the new candidate nodes being any node in the first planning graph that is directly coupled to the new current node in the first planning graph by a respective single edge of the first planning graph, finding a least cost path from the new current node to a goal node in the first planning graph that passes from the new current node directly to the respective new candidate node and then to the goal node, with or without a number of intervening nodes successively between the respective new candidate node and the goal node along a corresponding path; and

after finding the least cost path for each of the new candidate nodes with respect to the trajectories of the set of trajectories,
for each of the new candidate nodes, computing a respective value based at least in part on a respective cost associated with each least cost path for the respective new candidate node across all of the trajectories; and
selecting one of the new candidate nodes based at least in part on the computed respective values.

14. A processor-based system to perform motion planning via planning graphs, where each planning gra

Assignees

• Univ Duke
• Univ Brown

Inventors

• Sorin Daniel
• Floyd-Jones William
• Murray Sean
• Konidaris George
• Walker William

Classifications

• G01C21/20

  Instruments for performing navigational calculations (G01C21/24, G01C21/26 take precedence) · CPC title

• G05D1/617

  Safety or protection, e.g. defining protection zones around obstacles or avoiding hazards (arrangements for controlling the position or course of two or more vehicles for avoiding collisions therebetween G05D1/693; arrangements for reacting to or preventing system or operator failure G05D1/80) · CPC title

• G05D1/644

  Optimisation of travel parameters, e.g. of energy consumption, journey time or distance · CPC title

• G05D1/648

  Performing a task within a working area or space, e.g. cleaning · CPC title

• G05D1/227

  Handing over between remote control and on-board control; Handing over between remote control arrangements · CPC title