# US 2022/0032925 A1 · Vehicle Trajectory Dynamics Validation and Interpolation

> Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt

- Number: US 2022/0032925 A1
- Document ID: US20220032925A1
- Status: Published application
- Kind: A1
- Application number: 17/005,667
- Filed: 2020-08-28
- Published: 2022-02-03
- Earliest claimed: 2020-07-31
- Assignees: Uatc, LLC
- Applicants: Uatc, LLC
- Inventors: Chenggang Liu; David McAllister Bradley
- Web page: https://patentgrep.com/patent/US20220032925A1

## CPC

- B60W 40/10: related to vehicle motion
- B60W 60/0013: {specially adapted for occupant comfort}
- B60W 50/0098: {Details of control systems ensuring comfort, safety or stability not otherwise provided for}
- B60W 50/06: Improving the dynamic response of the control system, e.g. improving the speed of regulation or avoiding hunting or overshoot
- B60W 2050/002: {Integrating means}
- B60W 2520/06: Direction of travel
- B60W 2520/105: Longitudinal acceleration
- B60W 2520/14: Yaw
- B60W 2552/30: Road curve radius
- B60W: CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT

## Abstract

Systems, devices, and methods for validating vehicle trajectories are provided. A vehicle controller can obtain a motion plan indicative of a trajectory including one or more state variables and one or more control variables. The vehicle controller can determine 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. The vehicle controller can further determine the motion plan is dynamically consistent based at least in part on the dynamics defect value, generate one or more vehicle system control signals, and control the autonomous vehicle based at least in part on the one or more vehicle system control signals.

## Claims

### Claim 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; and
controlling the autonomous vehicle based at least in part on the one or more vehicle system control signals.

### Claim 2 (depends on 1)

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.

### Claim 3 (depends on 2)

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

### Claim 4 (depends on 1)

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.

### Claim 5 (depends on 1)

The vehicle controller of claim 1, wherein generating 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.

### Claim 6 (depends on 5)

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

### Claim 7 (depends on 5)

The vehicle controller of claim 5, 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.

### Claim 8 (depends on 5)

The vehicle controller of claim 5, 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.

### Claim 9 (depends on 1)

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

### Claim 10 (depends on 1)

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.

### Claim 11 (depends on 1)

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.

### Claim 12 (depends on 1)

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.

### Claim 13

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; and
controlling the autonomous vehicle based at least in part on the one or more vehicle system control signals.

### Claim 14 (depends on 13)

The method of claim 13, 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.

### Claim 15 (depends on 14)

The method of claim 14, wherein determining, by the computing system, the trapezoidal integration comprises determining the dynamics defect value according to the following equation:
dynamics defect=xk+0.5*hk*[ƒ(xk+1,uk+1)+ƒ(xk,uk)]−xk+1;
wherein (xk, uk) and (xk+1, uk+1) are the one or more state variables x and the one or more control variables u at times tk and tk+1, respectively;
wherein hk is the duration between tk and tk+1; and
wherein ƒ( ) is the continuous-time dynamics function of the one or more state variables x and the one or more control variables u.

### Claim 16 (depends on 13)

The method of claim 13, 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.

### Claim 17 (depends on 13)

The method of claim 13, wherein generating 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; and
wherein interpolating the one or more state variables and the one or more control variables to a particular time step comprises performing at least one of linear interpolation, cubic spline interpolation, or dynamics interpolation.

### Claim 18

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; and
controlling the autonomous vehicle based at least in part on the one or more vehicle system control signals.

### Claim 19 (depends on 18)

The autonomous vehicle of claim 18, wherein determining the dynamics defect value comprises determining the dynamics defect value according to the following equation:
dynamics defect=xk+0.5*hk*[ƒ(xk+1,uk+1)+ƒ(xk,uk)]−xk+1;
wherein (xk, uk) and (xk+1, uk+1) are the one or more state variables x and the one or more control variables u at times tk and tk+1, respectively;
wherein hk is the duration between tk and tk+1; and
wherein ƒ( ) is the continuous-time dynamics function of the one or more state variables x and the one or more control variables u.

### Claim 20 (depends on 18)

The autonomous vehicle of claim 18, wherein generating 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; and
wherein interpolating the one or more state variables and the one or more control variables to a particular time step comprises performing linear interpolation, cubic spline interpolation, or dynamics interpolation.
