A method of navigating an autonomous driving vehicle (ADV) includes determining a target function for an open space model based on one or more obstacles and map information within a proximity of the ADV, then iteratively performing first and second quadratic programming (QP) optimizations on the target function. Then, generating a second trajectory based on results of the first and second QP optimizations to control the ADV autonomously using the second trajectory. The first QP optimization is based on fixing a first set of variables of the target function. The second QP optimization is based on maximizing a sum of the distances from the ADV to each of the obstacles over a plurality of points of the first trajectory, and minimizing a difference between a target end-state of the ADV and a determined final state of the ADV using the first trajectory.