Methods and systems for topological planning in autonomous driving
Systems and methods of maneuvering an autonomous vehicle in a local region using topological planning, while traversing a route to a destination location, are disclosed. The system includes an autonomous vehicle including one or more sensors and a processor. The processor is configured to determine the local region on the route and receive real-time information corresponding to the local region. The processor performs topological planning to identify on or more topologically distinct classes of trajectories, compute a constraint set for each of the one or more topologically distinct classes of trajectories, optimize a trajectory to generate a candidate trajectory for each constraint set, and select a trajectory for the autonomous vehicle to traverse the local region from amongst the one or more candidate trajectories. Each of the one or more topologically distinct classes is associated with a plurality of trajectories that take the same combination of discrete actions with respect to objects in the local region.
1. A method of maneuvering an autonomous vehicle while traversing a route to a destination location, the method comprising:
determining, for an autonomous vehicle while traversing a route to a destination location, a local region that surrounds the autonomous vehicle;
receiving real-time information corresponding to the local region;
performing topological planning to identify one or more topologically distinct classes of trajectories, wherein each of the one or more topologically distinct classes is associated with a plurality of trajectories that take the same combination of discrete actions with respect to objects in the local region;
computing a constraint set for each of the one or more topologically distinct classes of trajectories, the constraint set for a topologically distinct class defining a bounded area in curvilinear space that confines the plurality of trajectories associated with that topologically distinct class;
for each constraint set, generating a candidate trajectory; and
selecting, from amongst one or more candidate trajectories, a trajectory for the autonomous vehicle to traverse the local region by:
assigning a score to each of the one or more candidate trajectories, and
selecting a candidate trajectory that has a best score as the trajectory for the autonomous vehicle to traverse the local region.
2. The method of claim 1 , wherein the real-time information comprises at least one of the following: perception information corresponding to the one or more objects in the local region, trajectory predictions relating to the one or more objects in the local region, information relating to an environment of the autonomous vehicle in the local region, or current state information of the autonomous vehicle.
3. A system comprising:
an autonomous vehicle comprising one or more sensors;
a processor; and
a non-transitory computer readable medium comprising one or more programming instructions that when executed by the processor, will cause the processor to:
determine, for the autonomous vehicle while traversing a route to a destination location, a local region that surrounds the autonomous vehicle;
receive real-time information corresponding to the local region;
perform topological planning to identify one or more topologically distinct classes of trajectories, wherein each of the one or more topologically distinct classes is associated with a plurality of trajectories that take the same combination of discrete actions with respect to objects in the local region;
compute a constraint set for each of the one or more topologically distinct classes of trajectories, the constraint set for a topologically distinct class defining a bounded area in curvilinear space that confines the plurality of trajectories associated with that topologically distinct class;
for each constraint set, generate a candidate trajectory; and
select, from amongst one or more candidate trajectories, a trajectory for the autonomous vehicle to traverse the local region
assigning a score to each of the one or more candidate trajectories, and
selecting a candidate trajectory that has a best score as the trajectory for the autonomous vehicle to traverse the local region.
4. The method of claim 1 , wherein assigning the score to each of the one or more candidate trajectories further comprises assigning a score based on at least one of the following: a risk of collision associated with that trajectory, or compliance of that trajectory with traffic rules, or a comfort level associated with that trajectory.
5. The method of claim 1 , wherein assigning the score to each of the one or more candidate trajectories comprises biasing the score in favor of temporal consistency of the selected trajectory between successive trajectory selection cycles.
6. The method of claim 1 , further comprising discarding a constraint set of the one or more constraint sets corresponding to the one or more topologically distinct classes of trajectories upon determining that the constraint set is at least one of the following: infeasible, redundant, or heuristically expensive.
7. The method of claim 1 , wherein each constraint set comprises one or more constraints associated with at least one of the following: longitudinal autonomous vehicle actions associated with perceived static or dynamic objects in the local region; longitudinal autonomous vehicle actions associated with restricted map areas; lateral autonomous vehicle actions associated with perceived static or dynamic objects in the local region; or lateral autonomous vehicle actions associated with restricted map areas.
8. The method of claim 1 , wherein computing the constraint set comprises, with respect to a moving object, determining one or more constraints associated with longitudinal autonomous vehicle actions before determining one or more constraints associated with lateral autonomous vehicle actions.
9. The method of claim 1 , further comprising updating the determined candidate trajectory based on at least one of the following: real-time information corresponding to the local region; or predictions updated based on an intent of the autonomous vehicle.
10. The method of claim 1 , wherein the trajectory for the autonomous vehicle to traverse the local region comprises a lane change trajectory, and the method further comprises identifying a time interval for executing the lane change.
11. The method of claim 1 , wherein the local region is a region around the autonomous vehicle that includes a first area in front of a current location of the autonomous vehicle and a second area behind the current location of the autonomous vehicle.
12. The system of claim 3 , wherein assigning the score to each of the one or more candidate trajectories comprises biasing the score in favor of temporal consistency of the selected trajectory between successive trajectory selection cycles.
13. The system of claim 3 , wherein the real-time information comprises at least one of the following: perception information corresponding to the one or more objects in the local region, trajectory predictions relating to the one or more objects in the local region, information relating to an environment of the autonomous vehicle in the local region, or current state information of the autonomous vehicle.
14. The system of claim 3 , wherein the local region is a region around the autonomous vehicle that includes a first area in front of a current location of the autonomous vehicle and a second area behind the current location of the autonomous vehicle.
15. The system of claim 3 , wherein the one or more programming instructions that when executed by the processor, will cause the processor to assign the score to each of the one or more candidate trajectories further comprise programing instructions to cause the processor to assign a score based on at least one of the following: a risk of collision associated with that trajectory, or compliance of that trajectory with traffic rules, or a comfort level associated with that trajectory.
16. The system of claim 3 , wherein the trajectory for the autonomous vehicle to traverse the local region comprises a lane change trajectory, and the system further comprises programming instructions to cause the processor to identify a time interval for executing the lane change.
17. The system of claim 3 , further comprising programming instructions that when executed by the processor, will cause the processor to discard a constraint set of the one or more constraint sets corresponding to the one or more topologically distinct classes of trajectories upon determining that the constraint set is at least one of the following: infeasible, redundant, or heuristically expensive.
18. The system of claim 3 , wherein each constraint set comprises one or more constraints associated with at least one of the following: longitudinal autonomous vehicle actions associated with perceived static or dynamic objects in the local region; longitudinal autonomous vehicle actions associated with restricted map areas; lateral autonomous vehicle actions associated with perceived static or dynamic objects in the local region; or lateral autonomous vehicle actions associated with restricted map areas.
19. The system of claim 3 , wherein the one or more programming instructions that when executed by the processor, will cause the processor to compute the constraint set further comprise programming instructions to determine, with respect to a moving object, one or more constraints associated with longitudinal autonomous vehicle actions before determining one or more constraints associated with lateral autonomous vehicle actions.
20. The system of claim 3 , further comprising programming instructions that when executed by the processor, will cause the processor to update the determined candidate trajectory based on at least one of the following: real-time information corresponding to the local region; or predictions updated based on an intent of the autonomous vehicle.