Systems and methods are provided for controlling a vehicle. In one embodiment, a method includes receiving vehicle state data, high-definition map data, and vehicle object environment data, generating a trajectory path that is optimal with respect to the received data, determining whether to update the trajectory path using Bézier curves based on the received data, performing an assessment of the trajectory using properties of Bézier curves, and generating an updated trajectory.