A trajectory planning system for an autonomous vehicle fits a jerk profile including a plurality of phases within a set of acceptable parameters, a jerk value being constant within each phase of the jerk profile. The system parameterizes the jerk profile based on an initial velocity, an initial acceleration, a final velocity, and a final acceleration for the first segment. The system then integrates the jerk profile to determine a first trajectory function, the first trajectory function including a speed for the vehicle at a given time. The system guides the vehicle along the first segment of the path according to the first trajectory function. The system detects an unplanned obstacle along the first segment of the path. The system plans a second trajectory function for a second segment of the path between a current location of the vehicle and a location on the path before the unplanned obstacle.