A method in an autonomous vehicle (AV) is provided. The method includes determining, from vehicle sensor data and road geometry data, a plurality of range measurements and obstacle velocity data; determining vehicle state data wherein the vehicle state data includes a velocity of the AV, a distance to a stop line, a distance to a midpoint of an intersection, and a distance to a goal; determining, based on the plurality of range measurements, the obstacle velocity data and the vehicle state data, a set of discrete behavior actions and a unique trajectory control action associated with each discrete behavior action; choosing a discrete behavior action and a unique trajectory control action to perform; and communicating a message to vehicle controls conveying the unique trajectory control action associated with the discrete behavior action.
展开▼