Abstract: In one embodiment, example systems and methods relate to generating an optimal path for a vehicle to avoid an obstacle. As the vehicle travels on a road segment, a polyline representing a path of the vehicle is generated. The polyline may include a plurality of nodes, and each node may have a set of coordinates with respect to the polyline. The vehicle uses sensors to detect one or more obstacles on the road, and lane boundaries of the road. The vehicle uses these obstacles and lane boundaries to calculate lateral constraints for the polyline. The vehicle then uses the lateral constraints and a quadratic program to optimize the polyline. The optimized polyline may be used to determine the optimal path for the vehicle that avoids the one or more obstacles.