System and method for hierarchical planning in autonomous vehicles

A system for determining a trajectory within an operating space for an autonomous vehicle, the system comprising a processing unit configured to execute instructions to implement:

• a target state generator configured to generate a set of candidate target end states for a behaviour decision based on an estimated state of the vehicle, the estimated state of the vehicle including one or more of:
• a location of the vehicle, an environment of the vehicle, or one or more kinodynamic parameters of the vehicle;
• a candidate trajectory generator configured to generate a set of candidate trajectories corresponding to the set of candidate target end states based on the estimated state of the vehicle;
• a target state evaluator configured to determine a suitability of each of the candidate target end states based on the estimated state of the vehicle;
• and
• a trajectory selector configured to select a trajectory to implement the behaviour decision from the set of candidate trajectories based on the determined suitability of the candidate target end states.

A non-transitory computer-readable medium storing instructions, which when executed by a processor of an autonomous vehicle, cause the autonomous vehicle to:

• generate a set of candidate target end states for a behaviour decision based on an estimated state of the vehicle, the estimated state of the vehicle including one or more of:
• information about vehicle position, an environment of the vehicle, or one or more kinodynamic parameters of the vehicle;
• generate a set of candidate trajectories corresponding to the set of candidate target end states based on the estimated state of the vehicle;
• determine a suitability of each of the candidate target end states based on the estimated state of the vehicle;
• and
• select a trajectory to implement the behaviour decision from the set of candidate trajectories based on the determined suitability of the candidate target end states.