FEASIBLE LANE ROUTING

Systems and method are provided for controlling a vehicle. The systems and methods calculate lane plan data including a set of lane plans defining a route from a start location to a destination location, solve a motion planning algorithm to produce solved lane plan data defining a solved lane plan and a trajectory therefor, receive forthcoming distance data representing a forthcoming distance, determine a feasible lane based on the solved lane plan data within the forthcoming distance, remove a lane plan from the lane plan data to produce feasible lane plan data including a feasible lane plan defining a route from the start location to the destination location, and control motion of the vehicle based on the feasible lane plan data.