US-12623670-B2 - Route-relative trajectory generation and optimization computations incorporating vehicle sideslip

Abstract

Techniques are discussed for generating and optimizing trajectories for autonomous vehicles using route-relative numerical integration relative to a reference path. A planner component of an autonomous vehicle, for example, can receive or generate a reference path corresponding to a route through an environment. The current vehicle state, sideslip, and vehicle dynamics can be represented in a system of equations, such that the planner component can substantially simultaneously solve for subsequent trajectory states in a single convergence operation. In various examples, the subsequent trajectory states can be used to evaluate and determine candidate trajectories to control the autonomous vehicle to traverse the environment.

Inventors

• Sriram Narayanan
• Liam Gallagher
• Marin Kobilarov

Assignees

• Zoox, Inc.

Dates

Publication Date

20260512

Application Date

20221130

Claims (19)

1. 1 . A system comprising: one or more processors; and one or more non-transitory computer-readable media storing computer-executable instructions that, when executed, cause the system to perform operations comprising: determining a current vehicle state associated with a vehicle, on a current driving path of the vehicle in an environment, the current vehicle state including a vehicle position and a vehicle orientation; receiving a reference path associated with the vehicle, wherein the reference path is different from the current driving path, the reference path including a reference point offset from the current driving path; determining a sideslip angle of the vehicle, wherein the sideslip angle is associated with the current vehicle state; determining a candidate vehicle state associated with the vehicle, based at least in part on the current vehicle state, the reference point on the reference path, and the sideslip angle of the vehicle, wherein determining the candidate vehicle state comprises executing a root finding algorithm to determine, substantially simultaneously: a candidate vehicle position associated with the candidate vehicle state; a candidate vehicle orientation associated with the candidate vehicle position; and a candidate arc length of a segment between the vehicle position and the candidate vehicle position; determining, using a loss function, a loss value associated with the candidate vehicle state, based at least in part on the candidate vehicle position, the candidate vehicle orientation, and the candidate arc length; and determining, based at least in part on the loss value, a candidate trajectory including the candidate vehicle state as a control trajectory for controlling the vehicle to traverse the environment.
2. 2 . The system of claim 1 , wherein determining the candidate vehicle state is based at least in part on a dynamics model that permits a non-zero sideslip.
3. 3 . A method comprising: determining, for a vehicle, a current vehicle state on a current driving path of the vehicle, the current vehicle state including a vehicle position and a vehicle orientation; receiving a reference path for the vehicle to follow, wherein the reference path is different from the current driving path, the reference path including a reference point offset from the current driving path; determining, based at least in part on the current vehicle state, a sideslip angle of the vehicle; determining a candidate trajectory based at least in part on the current vehicle state, the reference point on the reference path, and the sideslip angle of the vehicle, wherein determining the candidate trajectory includes: determining a predicted vehicle state of the candidate trajectory, including a predicted vehicle position and predicted vehicle orientation of the predicted vehicle state; and determining, based at least in part on a root finding algorithm, a curvature of a segment between the vehicle position and the predicted vehicle position, and an arc length of the segment; and controlling the vehicle based at least in part on the candidate trajectory.
4. 4 . The method of claim 3 , wherein determining the curvature of the segment comprises: determining an estimated arc length of the segment, based at least in part on at least one of: an arc length of a previous segment associated with the current driving path; or an arc length of a reference segment associated with the reference path; and determining the curvature of the segment, based at least in part on the estimated arc length and the sideslip angle of the vehicle.
5. 5 . The method of claim 3 , wherein determining the predicted vehicle state comprises: executing an optimization algorithm to solve a system of equations substantially simultaneously, wherein solving the system of equations includes solving for: the predicted vehicle position; the predicted vehicle orientation; and the arc length of the segment between the vehicle position and the predicted vehicle position.
6. 6 . The method of claim 5 , wherein executing the optimization algorithm comprises: determining an initial estimated curvature associated with the segment; and executing, based at least in part on the initial estimated curvature, a differential dynamic programming (DDP) algorithm to determine a single convergence.
7. 7 . The method of claim 3 , wherein determining the candidate trajectory comprises: determining a plurality of candidate trajectories for the vehicle; optimizing a candidate trajectory of the plurality of candidate trajectories to determine: a position associated with the candidate trajectory, relative to the reference path; an orientation associated with the candidate trajectory, relative to the reference path; and an arc length associated with the candidate trajectory.
8. 8 . The method of claim 3 , wherein: determining the predicted vehicle position includes determining a lateral distance between the predicted vehicle state and a second reference point on the reference path; and determining the predicted vehicle orientation includes determining an orientation difference between the predicted vehicle state and a second reference orientation associated with the second reference point.
9. 9 . The method of claim 3 , wherein determining the predicted vehicle state is based at least in part on a dynamics model that permits a non-zero sideslip.
10. 10 . One or more non-transitory computer-readable media storing processor-executable instructions that, when executed by one or more processors, cause the one or more processors to perform operations comprising: determining, for a vehicle, a current vehicle state on a current driving path of the vehicle, the current vehicle state including a vehicle position and a vehicle orientation; receiving a reference path for the vehicle to follow, wherein the reference path is different from the current driving path, the reference path including a reference point offset from the current driving path; determining, based at least in part on the current vehicle state, a sideslip angle of the vehicle; determining a candidate trajectory based at least in part on the current vehicle state, the reference point on the reference path, and the sideslip angle of the vehicle, wherein determining the candidate trajectory includes: determining a predicted vehicle state of the candidate trajectory, including a predicted vehicle position and predicted vehicle orientation of the predicted vehicle state; and determining, based at least in part on a root finding algorithm, a curvature of a segment between the vehicle position and the predicted vehicle position, and an arc length of the segment; and controlling the vehicle based at least in part on the candidate trajectory.
11. 11 . The one or more non-transitory computer-readable media of claim 10 , wherein determining the predicted vehicle state comprises: executing an optimization algorithm to solve a system of equations substantially simultaneously, wherein solving the system of equations includes solving for: the predicted vehicle position; the predicted vehicle orientation; and the arc length of the segment between the vehicle position and the predicted vehicle position.
12. 12 . The one or more non-transitory computer-readable media of claim 11 , wherein executing the optimization algorithm comprises: determining an initial estimated curvature associated with the segment; and executing, based at least in part on the initial estimated curvature, a differential dynamic programming (DDP) algorithm to determine a single convergence.
13. 13 . The method of claim 3 , wherein determining the candidate trajectory is based at least in part on: a lateral offset between the current vehicle state and the reference point on the reference path; and an angular offset between the current vehicle state and the reference point.
14. 14 . The method of claim 3 , wherein determining the candidate trajectory further comprises: determining a second reference point on the reference path, based at least in part on an intersection between the predicted vehicle position and a reference vector perpendicular to the reference path at the second reference point; determining a lateral offset between the predicted vehicle state and the second reference point; and determining an angular offset between the predicted vehicle state and the second reference point.
15. 15 . The method of claim 3 , wherein receiving the reference path comprises: determining a roadway associated with the current vehicle state; and receiving from a map server, the reference path associated with the roadway.
16. 16 . The method of claim 3 , wherein determining the candidate trajectory comprises: determining the root finding algorithm as an optimization algorithm to be executed, based at least in part on determining that the sideslip angle of the vehicle is a non-zero sideslip angle.
17. 17 . The method of claim 3 , wherein determining the candidate trajectory further comprises: determining a plurality of candidate driving paths for the vehicle; determining a candidate vehicle state associated with each of the plurality of candidate driving paths; determining, for each candidate vehicle state: a candidate vehicle position; a candidate vehicle orientation; and a candidate arc length; and evaluating each candidate driving path of the plurality of candidate driving paths, based at least in part on the candidate vehicle position, the candidate vehicle orientation, and the candidate arc length of the candidate vehicle state associated with the candidate driving path.
18. 18 . The one or more non-transitory computer-readable media of claim 10 , wherein determining the candidate trajectory comprises: determining the root finding algorithm as an optimization algorithm to be executed, based at least in part on determining that the sideslip angle of the vehicle is a non-zero sideslip angle.
19. 19 . The one or more non-transitory computer-readable media of claim 10 , wherein determining the candidate trajectory further comprises: determining a plurality of candidate driving paths for the vehicle; determining a candidate vehicle state associated with each of the plurality of candidate driving paths; determining, for each candidate vehicle state: a candidate vehicle position; a candidate vehicle orientation; and a candidate arc length; and evaluating each candidate driving path of the plurality of candidate driving paths, based at least in part on the candidate vehicle position, the candidate vehicle orientation, and the candidate arc length of the candidate vehicle state associated with the candidate driving path.

Description

BACKGROUND Autonomous vehicles use various systems and techniques to navigate through driving environments including static and dynamic objects. For instance, autonomous vehicles may utilize route planning methods to guide the vehicles through congested areas with other moving vehicles (autonomous or otherwise), moving people, stationary buildings, etc. In some examples, generating routes for an autonomous vehicle can include evaluating and/or optimizing candidate trajectories based on vehicle accelerations and steering angles associated with candidate vehicle states, which can be challenging and inefficient in resource-limited or time-limited computing environments. BRIEF DESCRIPTION OF THE DRAWINGS The detailed description is described with reference to the accompanying figures. In the figures, the left-most digit(s) of a reference number identifies the figure in which the reference number first appears. The use of the same reference numbers in different figures indicates similar or identical components or features. FIG. 1 illustrates an example technique of generating a trajectory for a vehicle to traverse a driving environment, relative to a reference path, in accordance with one or more examples of the disclosure. FIGS. 2A and 2B illustrate example vehicle state diagrams including non-zero vehicle sideslip angles, in accordance with one or more examples of the disclosure. FIG. 3 illustrates an example driving environment including a determined vehicle trajectory relative to a reference path, in accordance with one or more examples of the disclosure. FIG. 4 depicts an example planning component for generating a vehicle trajectory, in accordance with one or more examples of the disclosure. FIG. 5 depicts a pictorial flow diagram illustrating an example process for determining a trajectory for a vehicle relative to a reference path, and controlling the vehicle based on the trajectory, in accordance with one or more examples of the disclosure. FIG. 6 illustrates an example process for determining subsequent vehicle state data and evaluating candidate trajectories based on the subsequent vehicle state data, in accordance with one or more examples of the disclosure. FIG. 7 depicts a block diagram of an example system for implementing various techniques described herein. DETAILED DESCRIPTION The techniques discussed herein relate to generating and/or optimizing trajectories for autonomous vehicles using route-relative numerical integration relative to a reference path. In some examples, a planning component of an autonomous vehicle can receive or generate a reference path representing a desired route for the vehicle to traverse an environment, such as the center of a lane or other efficient driving route taking into account the various vehicles and other obstacles in the environment. To determine a trajectory for the vehicle to follow relative to the reference path, the planning component may evaluate a number of candidate trajectories based on the current state of the vehicle relative to the reference path (e.g., a lateral position offset and orientation offset), and the lateral and longitudinal dynamics of the vehicle (e.g., steering angle, velocity and/or acceleration). To evaluate a candidate trajectory, the planner component may initially determine subsequent vehicle state data associated with the candidate trajectory, such as the future position and orientation of the vehicle at a subsequent step in the candidate trajectory, as well as the arc length and/or curvature of the path that will drive to reach the subsequent trajectory step. In order to determine the subsequent vehicle state data for a candidate trajectory, various techniques are described herein including representing the current vehicle state data, vehicle sideslip angle and/or direction, and the lateral and longitudinal dynamics of the vehicle, into a system of equations. The planning component may use a root finding algorithm or other optimization technique to solve the system of equations, thereby substantially simultaneously determining the subsequent vehicle state data for the candidate trajectory. The planning component then may use the subsequent vehicle state data to evaluate the candidate trajectory with respect to one or more loss functions, and may determine a trajectory for controlling the vehicle based on the evaluations of the candidate trajectories. For a particular trajectory (e.g., a candidate trajectory in a trajectory optimization process), for the planning component to evaluate the trajectory it may initially determine one or more subsequent vehicle states of the trajectory. In some examples, the current vehicle state and/or subsequent vehicle states of a trajectory can be determined relative to a reference path. For instance, a current vehicle state can include a lateral offset (or error) value representing the lateral distance between the current position of the vehicle and a corresponding point on the reference path, a