In one embodiment, method performed by an autonomous driving vehicle (ADV) that determines, within a driving space, a plurality of routes from a current location of the ADV to a desired location. The method determines, for each route of the plurality of routes, an objective function to control the ADV autonomously along the route and, for each of the objective functions, performs Differential Dynamic Programming (DDP) optimization in view of a set of constraints to produce a path trajectory. The method determines whether at least one of the path trajectories satisfies each constraint and, in response to a path trajectory satisfying each of the constraints, selects the path trajectory for navigating the ADV from the current location to the desired location.