A system and method for generating a target path for a vehicle and controlling the movement of the vehicle includes a processor and a control system in communication with the processor. The control system is configured to control the movement of the vehicle. The processor is configured to determine a target path, determine a cost of the target path using a cost function, wherein the cost function utilizes a criteria that comprises a smoothness of the target path, determine a constraint that the target path is allowed to exist within, and optimize the target path for the vehicle by reducing the cost of the target path using the cost function and the constraint in which the target path is allowed to exist within.