System and method for adaptive path planning

• US 20050216181A1
• Filed: 03/26/2004
• Published: 09/29/2005
• Est. Priority Date: 03/26/2004
• Status: Active Grant

• Current Assignee: Raytheon Company (Rtx Corporation)
• Original Assignee: Raytheon Company (Rtx Corporation)
• Inventors: Estkowski, Regina I., Tinker, Peter A.
• Granted Patent: US 7,447,593 B2
• Time in Patent Office: Days
• US Class Current: 701/200
• CPC Class Codes: G01C 21/20 Instruments for performing navigational calculations G01C21/24, G01C21/26 take precedence
• CPC Class Codes: G05D 1/0212 with means for defining a desired trajectory involving a plurality of land vehicles G05D1/0287
• CPC Class Codes: G08G 1/164 Centralised systems, e.g. external to vehicles
• CPC Class Codes: G08G 1/166 for active traffic, e.g. moving vehicles, pedestrians, bikes

Abstract

A path planning system and method for an object, such as a vehicle, provides a randomized adaptive path planning that handles real-time path planning for a vehicle operating under kinodynamic constraints in dynamically changing and uncertain environments with probabilistic knowledge of vehicle and obstacle movement.

Claims

1. A method of planning at least one path for an object in a state space from a starting position to a goal position to avoid a plurality of static and/or dynamic objects, comprising:

(a) associating predetermined attributes with the plurality of static objects and/or the plurality of dynamic objects located in the state space;

(b) generating a probabilistic tree in the state space including a plurality of branches extending from the starting position of the vehicle towards the goal position located in the state space; and

(c) extending the plurality of branches of the probabilistic tree towards the goal position located in the state space based on at least one of a plurality of random tree extension rules and a plurality of deterministic tree extension rules until satisfying a predetermined stopping condition; and

(d) evaluating at least a first branch of the plurality of branches of the probabilistic tree for determining whether the first branch of the plurality of branches of the probabilistic tree satisfies predetermined trajectory path constraints.

2. The method of claim 1, wherein if the first branch of the plurality of branches of the probabilistic tree conforms to the predetermined trajectory path constraints, the method further includes:

(e) declaring the first branch of the plurality of branches of the probabilistic tree as the at least one preferred trajectory path for the vehicle in the state space; and

(f) controlling the vehicle to follow the at least one preferred trajectory path in the state space for moving the vehicle from the starting position towards the goal position in the state space.

3. The method of claim 2, wherein if the first branch of the plurality of branches of the probabilistic tree fails to conform to the predetermined trajectory path constraints, the method further includes:

(g) extending the plurality of branches of the probabilistic tree further based on the at least one of the plurality of random tree extension rules and the plurality of deterministic tree extension rules until at least one branch of the plurality of branches of the probabilistic tree satisfies the predetermined stopping condition and conforms to the predetermined trajectory path constraints.

4. The method of claim 3, further including:

(h) declaring the at least one branch of the plurality of branches of the probabilistic tree that couples the starting position to the goal position and that conforms to the predetermined trajectory path constraints as the at least one preferred trajectory path for the vehicle in the state space; and

(i) controlling the vehicle to follow the at least one preferred trajectory path in the state space for moving the vehicle from the starting position towards the goal position in the state space.

5. The method of claim 4, further including:

(j) repeating one or more of steps (a)-(i) at predetermined time intervals as the vehicle moves along the at least one preferred trajectory path toward the goal for updating the at least one preferred trajectory path to compensate for motion of the plurality of dynamic objects.

6. The method of claim 1, wherein satisfying the predetermined stopping condition includes at least one of satisfying a predetermined time constraint and satisfying a predetermined travel distance constraint.

7. The method of claim 1, wherein associating predetermined attributes with the plurality of static objects and the plurality of dynamic objects located in the state space includes associating at least one of a position value, a velocity value, a direction value, an acceleration value and a time value.

8. The method of claim 1, wherein generating the probabilistic tree in the state space further includes:

extending each of a first plurality of edges a first predetermined distance and direction from the starting position in the state space to each of a corresponding first plurality of nodes based on the plurality of random tree extension rules and the plurality of deterministic tree extension rules for forming first segments of each of the plurality of branches of the probabilistic tree.

9. The method of claim 8, further including:

extending at least one of a next successive plurality of edges from each of the first plurality of nodes a second predetermined distance and direction in the state space to each of a corresponding next successive plurality of nodes based on the plurality of random tree extension rules and the plurality of deterministic tree extension rules for forming next successive segments of each branch of the plurality of branches of the probabilistic tree.

10. The method of claim 9, further including:

repeating cyclically extension of each branch of the plurality of branches of the probabilistic tree until at least the first branch of the plurality of branches of the probabilistic tree satisfies the stopping condition.

11. The method of claim 10, further including:

evaluating whether extension of one or more branches of the plurality of branches of the probabilistic tree violate object avoidance constraints; and

suspending further extension of the one or more branches of the plurality of branches if a determination is made that extension of the one or more branches of the plurality of branches of the probabilistic tree violate the object avoidance constraints.

12. The method of claim 11, further including:

resuming extension of the one or more branches of the plurality of branches if a determination is made that extension of the one or more branches of the plurality of branches of the probabilistic tree no longer violates the object avoidance constraints.

13. The method of claim 1, wherein extending the plurality of branches of the probabilistic tree based on the plurality of random tree extension rules includes at least one of extending each branch into the state space that is void of the plurality of static objects and the plurality of dynamic objects and extending each branch into the state space that is void of other branches of the plurality of branches of the probabilistic tree.

14. The method of claim 1, wherein extending the plurality of branches of the probabilistic tree based on the plurality of deterministic tree extension rules includes at least one of extending each branch towards the goal and extending each branch in a straight line with respect to a previous extension of each branch.

15. The method of claim 1, wherein determining whether the first branch of the plurality of branches of the probabilistic tree satisfies the predetermined trajectory path constraints includes determining whether the first branch of the plurality of branches of the probabilistic tree satisfies at least one of a maximum travel distance value, a maximum turn angle value, a minimum distance value to the plurality of static objects and the plurality of dynamic objects.

16. A path planning method for a vehicle, comprising:

defining a state space including an initial start position and a goal position;

generating a plurality of paths from the start position to the goal position over time on a node by node basis based upon a set of rules including at least one of a deterministic rule, a randomness rule, and a probabilistic rule;

assigning locations to objects in the state space over time based upon respective probability distributions; and

selecting a first one of the generated plurality of paths.

17. The method according to claim 16, further including terminating ones of the plurality of paths that are not feasible at a given node in the state space.

18. The method according to claim 17, further including terminating paths based upon one or more of impact with an object, region avoidance, g-force limitations, sensor information, path distance, path time, number of turns, altitude change limitations, straight path desirability, object location confidence level, turning radius limitations, and turning penalties.

19. The method according to claim 16, further assigning a confidence level to object locations.

20. The method according to claim 16, further including assigning object state information including one or more of position, heading, velocity, acceleration, turning radius, acceleration limit, velocity limit, g-force limit, and location confidence level.

21. The method according to claim 20, further including assigning a probability distribution to one or more components of the object state information.

22. A method of adaptive path planning for a vehicle, comprising:

defining a state space for the vehicle and a plurality of objects;

setting a root node to initial state for the vehicle;

generating a plurality of paths comprising node-to-nod branches from a vehicle start location to a goal location;

examining each of the branches to determine whether stopping conditions are satisfied;

generating first ones of the branches using deterministic rules;

generating second ones of the branches using random extension rules;

determining whether first ones of the plurality of branches should terminated; and

selecting a first one of the plurality of paths that extend to the goal location.

23. The method according to claim 22, further including assigning state information to the plurality of objects including one or more of position, heading, velocity, acceleration, turning radius, acceleration limit, velocity limit, g-force limit, and location confidence level.

24. A system to plan a path to a vehicle, comprising:

a workstation including

a processor coupled to a memory containing instructions to enable the steps of;

defining a state space for the vehicle and a plurality of objects;

setting a root node to initial state for the vehicle;

generating a plurality of paths comprising node-to-nod branches from a vehicle start location to a goal location;

examining each of the branches to determine whether stopping conditions are satisfied;

generating first ones of the branches using deterministic rules;

generating second ones of the branches using random extension rules;

determining whether first ones of the plurality of branches should terminated; and

selecting a first one of the plurality of paths that extend to the goal location.

25. The system according to claim 24, further including program instructions to provide state information to the plurality of objects including one or more of position, heading, velocity, acceleration, turning radius, acceleration limit, velocity limit, g-force limit, and location confidence level.

26. The system according to claim 24, further including program instructions to download the selected path to the vehicle.