Abstract: A method includes inputting a source into a weighted and undirected graph having a plurality of nodes and edges, inputting a target, inputting a plurality of objectives, searching the graph using a rapidly exploring random tree algorithm to determine a solution path that meets an existence constraint and an order constraint, determining a plurality of paths between the source and the target that intersects with each one of the plurality of objectives at least once that conforms with the existence constraint, assigning a travel cost for each of the determined plurality of paths, determining a visiting order of the plurality of objectives that reduces the assigned travel cost by the RRT* algorithm asymptotically decreasing with an allocated computation time by rewiring each one of the plurality of objectives and outputting the solution path to form a connected graph having the plurality of nodes.

Abstract: System, methods, and other embodiments described herein relate to vehicle maneuvering using predictive control with automated driving and contingency planning to preserve safety and increase comfort. In one embodiment, a method includes receiving, by a controller, an operator command associated with a vehicle maneuver while automated driving is engaged. The method also includes adjusting a motion command associated with the vehicle maneuver by applying a predictive control according to motion estimates outputted from the automated driving and the operator command, the predictive control using motion constraints that are constant between time intervals for the vehicle maneuver. The method also includes controlling, by the controller, a vehicle using the motion command for a time step during the time intervals.