Vehicle trajectory dynamics validation and interpolation

1. A vehicle controller, comprising:

one or more processors;

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 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 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 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.

2. The vehicle controller of claim 1 , wherein determining the dynamics defect value for the motion plan comprises determining a trapezoidal integration of the continuous-time dynamics function.

3. The vehicle controller of claim 2 , wherein determining the trapezoidal integration comprises multiplying a time duration parameter (h k ) by 0.5 and the summation of a continuous-time dynamics function at a time k+1 (f(x k+1 , u k+1) )) and the continuous-time dynamics function at a time k (f(x k , u k )), adding the state variable at the time k (x k ), and subtracting the state variable at the time k+1 (x k+1 ), where x k comprises the one or more state variables at the time k, and u k comprises the one or more control variables at the time k.

4. The vehicle controller of claim 1 , wherein determining the motion plan is dynamically consistent based at least in part on the dynamics defect value comprises determining that the dynamics defect value is less than a threshold.

5. The vehicle controller of claim 1 , wherein interpolating the one or more state variables and the one or more control variables to the particular time step comprises performing linear interpolation.

6. The vehicle controller of claim 1 , wherein interpolating the one or more state variables and the one or more control variables to the particular time step comprises performing cubic spline interpolation.

7. The vehicle controller of claim 1 , wherein interpolating the one or more state variables and the one or more control variables to the particular time step comprises performing dynamics interpolation; and

wherein performing dynamics interpolation comprises quadratic interpolation.

8. The vehicle controller of claim 1 , wherein the motion plan comprises a smoothed polynomial motion plan.

9. The vehicle controller of claim 1 , wherein the one or more state variables comprise at least one of a position, velocity, acceleration, yaw, curvature, odometer, or a change in curvature.

10. The vehicle controller of claim 1 , wherein the one or more control variables comprise at least one of a jerk or ddcurv, the second derivative of curvature.

11. The vehicle controller of claim 1 , wherein controlling the autonomous vehicle based at least in part on the one or more vehicle system control signals comprises sending the one or more vehicle system control signals to one or more vehicle control systems.

12. 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.

13. The method of claim 12 , wherein determining, by the computing system, the dynamics defect value for the motion plan comprises determining, by the computing system, a trapezoidal integration of the continuous-time dynamics function.

14. The method of claim 13 , wherein determining, by the computing system, the trapezoidal integration comprises determining the dynamics defect value according to the following equation:

dynamics defect= x k +0.5* h k *[ f ( x k+1 ,u k+1 )+ f ( x k ,u k )]− x k+1 ;

wherein (x k , u k ) and (x k+1 , u k+1 ) are the one or more state variables x and the one or more control variables u at times t k and t k+1 , respectively;

wherein h k is a duration between t k and t k+1 ; and

wherein f( ) is the continuous-time dynamics function of the one or more state variables x and the one or more control variables u.

15. The method of claim 12 , further comprising:

in response to determining the motion plan is not dynamically consistent, providing, by the computing system, a command to provide a different motion plan.

16. 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.

17. The autonomous vehicle of claim 16 , wherein determining the dynamics defect value comprises determining the dynamics defect value according to the following equation:

dynamics defect= x k +0.5* h k *[ f ( x k+1 ,u k+1 )+ f ( x k ,u k )]− x k+1 ;

wherein (x k , u k ) and (x k+1 , u k+1 ) are the one or more state variables x and the one or more control variables u at times t k and t k+1 , respectively;

wherein h k is a duration between t k and t k+1 ; and

wherein f( ) is the continuous-time dynamics function of the one or more state variables x and the one or more control variables u.