SYSTEM AND METHOD OF AN ALGORITHMIC SOLUTION TO GENERATE A SMOOTH VEHICLE VELOCITY TRAJECTORY FOR AN AUTONOMOUS VEHICLE WITH SPATIAL SPEED CONSTRAINTS

Methods and systems for piecewise sinusoid trajectory planning including: receiving, by a processing unit disposed in an ego vehicle, point data defining a current path plan for the ego vehicle; fetching, by the processing unit, a current ego position along the current path plan; calculating, by the processing unit, a current velocity and an acceleration for the current ego position which is based in part on a velocity and acceleration derived from a previous planned trajectory, the planned trajectory is calculated by: setting a graph with a grid of nodes in the velocity-time domain of the search space connected by edges; evaluating the graph with a shortest path algorithm wherein the validity and cost with respect to desired constraints of vehicle motion and system limitations of each edge are determined by use of the piecewise sinusoidal path length, velocity, and acceleration profiles which are parameterized to connect pairs of nodes; setting, by the processing unit, the optimal vehicle trajectory equal to the set of connected edges, and thereby piecewise sinusoid velocity profiles, which minimize the objective cost function over some duration planned into the future.