Vehicle trajectory dynamics validation and interpolation

A method for controlling an autonomous vehicle, comprising:

• obtaining, by a computing system comprising one or more computing devices, a motion plan, the motion plan indicative of a trajectory for an autonomous vehicle, the motion plan comprising one or more state variables and one or more control variables;
• determining, by the computing system, a dynamics defect value for the motion plan based at least in part on the one or more state variables, the one or more control variables, and a continuous-time dynamics function comprising a function indicative of a change in the one or more state variables and the one or more control variables over time;
• determining, by the computing system, whether the motion plan is dynamically consistent based at least in part on the dynamics defect value;
• in response to determining the motion plan is dynamically consistent, generating one or more vehicle system control signals, wherein generating the one or more vehicle system control signals comprises interpolating the one or more state variables and the one or more control variables to a particular time step by performing at least one of linear interpolation, cubic spline interpolation, or dynamics interpolation;
• and
• controlling the autonomous vehicle based at least in part on the one or more vehicle system control signals.

An autonomous vehicle, comprising:

• a vehicle autonomy system comprising one or more processors, the vehicle autonomy system configured to generate a motion plan, the motion plan indicative of a trajectory for the autonomous vehicle, the motion plan comprising one or more state variables and one or more control variables;
• and
• a vehicle controller comprising
• one or more processors;
• and
• a memory comprising one or more tangible non-transitory computer-readable media, the media storing computer-readable instructions that when executed by the one or more processors cause the one or more processors to perform operations comprising:
• obtaining, from the vehicle autonomy system, the motion plan;
• determining a dynamics defect value for the motion plan based at least in part on the one or more state variables, the one or more control variables, and a continuous-time dynamics function comprising a function indicative of a change in the one or more state variables and the one or more control variables over time;
• validating the motion plan based at least in part on the dynamics defect value;
• in response to validating the motion plan, generating one or more vehicle system control signals, wherein generating the one or more vehicle system control signals comprises interpolating the one or more state variables and the one or more control variables to a particular time step by performing at least one of linear interpolation, cubic spline interpolation, or dynamics interpolation;
• and
• controlling the autonomous vehicle based at least in part on the one or more vehicle system control signals.