A method of route planning for a vehicle proceeding from a current location to a destination in a planning space is provided. The method includes generating a destination-rooted tree from global information that provides cost-to-go routing to the destination from multiple locations in the planning space, generating a vehicle-rooted tree using local information from the current location out to a sensing horizon and determining a local destination at the sensing horizon. The local destination corresponds to minimal cost-to-go routing obtained from the destination-rooted tree.