Lane selection

1. A system for lane selection, comprising a memory, and a processor coupled to the memory, and a controller, the processor executing instructions stored in the memory, the instructions comprising:

receiving a current state of an ego vehicle, a current state of a traffic participant vehicle, and a goal position for the ego vehicle within an operating environment;

projecting the ego vehicle and the traffic participant vehicle onto a graph network based on the current state of the ego vehicle and the current state of the traffic participant vehicle, wherein one or more nodes of the graph network are indicative of discretized space within the operating environment;

determining a current node for the ego vehicle within the graph network; and

determining a subsequent node for the ego vehicle based on the following:

identifying one or more adjacent nodes which are adjacent to the current node;

calculating one or more travel times associated with each of the one or more adjacent nodes;

calculating one or more step costs associated with each of the one or more adjacent nodes;

calculating one or more heuristic costs associated with each of the one or more adjacent nodes; and

predicting a position of the traffic participant vehicle based on the one or more travel times, the one or more step costs, the one or more heuristic costs, and a distance to the goal position,

wherein the controller controls one or more vehicle systems to steer the ego vehicle from the current node to the subsequent node.

2. The system for lane selection of claim 1 , wherein the instructions include determining the subsequent node for the ego vehicle based on calculating one or more total costs associated with each of the one or more adjacent nodes.

3. The system for lane selection of claim 2 , wherein one or more total costs associated with each of the one or more adjacent nodes is a sum of one or more of the step costs associated with each of the one or more adjacent nodes and one or more of the heuristic costs associated with each of the one or more adjacent nodes, respectively.

4. The system for lane selection of claim 1 , wherein one or more of the step costs associated with each of the one or more adjacent nodes is indicative of an immediate cost of transitioning from the current node to each of the one or more adjacent nodes, respectively.

5. The system for lane selection of claim 1 , wherein one or more of the heuristic costs associated with each of the one or more adjacent nodes is indicative of an estimated optimal cost-to-go to the goal position.

6. The system for lane selection of claim 1 , wherein one or more of the step costs associated with each of the one or more adjacent nodes includes a control effort, a travel time, a risk, and a switching cost.

7. The system for lane selection of claim 6 , wherein the control effort is determined based on a Euclidean distance between the current node and the corresponding adjacent node onto a horizontal axis and a vertical axis.

8. The system for lane selection of claim 6 , wherein the travel time is determined based on the current state of the ego vehicle including a current velocity of the ego vehicle.

9. The system for lane selection of claim 6 , wherein the risk is determined based on the current state of the traffic participant vehicle relative to the current state of the ego vehicle and an amount of vacillation associated with the traffic participant vehicle.

10. The system for lane selection of claim 6 , wherein the switching cost is determined based on a function of divergence of an initial position of the ego vehicle from a previous target lane.

11. A system for lane selection, comprising:

a memory;

a processor coupled to the memory, the processor executing instructions stored in the memory, the instructions comprising:

receiving a current state of an ego vehicle, a current state of a traffic participant vehicle, and a goal position for the ego vehicle within an operating environment;

projecting the ego vehicle and the traffic participant vehicle onto a graph network based on the current state of the ego vehicle and the current state of the traffic participant vehicle, wherein one or more nodes of the graph network are indicative of discretized space within the operating environment;

determining a current node for the ego vehicle within the graph network; and

determining a subsequent node for the ego vehicle based on the following:

identifying one or more adjacent nodes which are adjacent to the current node;

calculating one or more travel times associated with each of the one or more adjacent nodes;

calculating one or more step costs associated with each of the one or more adjacent nodes;

calculating one or more heuristic costs associated with each of the one or more adjacent nodes;

predicting a position of the traffic participant vehicle based on the one or more travel times, the one or more step costs, the one or more heuristic costs, and a distance to the goal position; and

a controller controlling one or more vehicle systems to steer the ego vehicle from the current node to the subsequent node.

12. The system for lane selection of claim 11 , wherein one or more of the step costs associated with each of the one or more adjacent nodes includes a control effort, a travel time, a risk, and a switching cost.

13. The system for lane selection of claim 12 , wherein the control effort is determined based on a Euclidean distance between the current node and the corresponding adjacent node onto a horizontal axis and a vertical axis.

14. The system for lane selection of claim 12 , wherein the travel time is determined based on the current state of the ego vehicle including a current velocity of the ego vehicle.

15. The system for lane selection of claim 12 , wherein the risk is determined based on the current state of the traffic participant vehicle relative to the current state of the ego vehicle and an amount of vacillation associated with the traffic participant vehicle.

16. The system for lane selection of claim 12 , wherein the switching cost is determined based on a function of divergence of an initial position of the ego vehicle from a previous target lane.

17. A computer-implemented method for lane selection, comprising:

receiving a current state of an ego vehicle, a current state of a traffic participant vehicle, and a goal position for the ego vehicle within an operating environment;

projecting the ego vehicle and the traffic participant vehicle onto a graph network based on the current state of the ego vehicle and the current state of the traffic participant vehicle, wherein one or more nodes of the graph network are indicative of discretized space within the operating environment;

determining a current node for the ego vehicle within the graph network; and

determining a subsequent node for the ego vehicle based on the following:

identifying one or more adjacent nodes which are adjacent to the current node;

calculating one or more travel times associated with each of the one or more adjacent nodes;

calculating one or more step costs associated with each of the one or more adjacent nodes;

calculating one or more heuristic costs associated with each of the one or more adjacent nodes;

predicting a position of the traffic participant vehicle based on the one or more travel times, the one or more step costs, the one or more heuristic costs, and a distance to the goal position; and

controlling one or more vehicle systems to steer the ego vehicle from the current node to the subsequent node.

18. The method for lane selection of claim 17 , comprising determining the subsequent node for the ego vehicle based on calculating one or more total costs associated with each of the one or more adjacent nodes.

19. The method for lane selection of claim 18 , comprising determining one or more total costs associated with each of the one or more adjacent nodes as a sum of one or more of the step costs associated with each of the one or more adjacent nodes and one or more of the heuristic costs associated with each of the one or more adjacent nodes, respectively.