Method for decoupling trajectory planning and tracking

Abstract

A vehicle, and a system and method of navigating the vehicle. The system includes a trajectory planning module and a trajectory tracking module. The trajectory planning module operates at a processor of the vehicle to generate a trajectory for the vehicle. The trajectory tracking module operates at the processor to track the trajectory to navigate the vehicle. The trajectory planning module and the trajectory tracking module operate asynchronously from one another.

Description

Technical Field

[0001] This topic relates to autonomous vehicles, and more specifically to systems and methods for navigating autonomous vehicles using asynchronous trajectory planning and trajectory tracking. Background Technology

[0002] Autonomous vehicles comprise modules or programs tasked with navigating the vehicle based on a planned trajectory. A single optimization problem is solved to plan and track the trajectory. This approach typically introduces latency and system delays into the tracking process, which can allow for reliability issues. Therefore, it is desirable to provide a system and method for trajectory tracking that reduces these latency problems. Summary of the Invention

[0003] In one exemplary embodiment, a method for navigating a vehicle is disclosed. A trajectory planning module running at the vehicle's processor generates a trajectory for the vehicle. A trajectory tracking module running at the processor tracks the trajectory to navigate the vehicle. The trajectory planning module and the trajectory tracking module operate asynchronously to each other.

[0004] In addition to one or more features described herein, the trajectory planning module solves a first optimization problem to generate a trajectory, and the trajectory tracking module uses the trajectory to solve a second optimization problem to generate actuation signals for navigating the vehicle, the first and second optimization problems being solved asynchronously. The method also includes running one of the trajectory planning and trajectory tracking modules at different frequencies, and at least one of the trajectory planning and trajectory tracking modules is event-driven. The method further includes adjusting a first frequency of the trajectory planning module relative to a second frequency of the trajectory tracking module based on a state estimate provided from the trajectory tracking module to the trajectory planning module. The state estimate indicates the trajectory tracking module's ability to track a trajectory. The method also includes receiving a trajectory at the trajectory tracking module, generating an updated trajectory based on the time difference between trajectory generation and receiving a positioning message at the trajectory tracking module, and tracking the updated trajectory at the trajectory tracking module. The method also includes providing an anchor reference frame with the trajectory from the trajectory planning module to the trajectory tracking module.

[0005] In another exemplary embodiment, a system for navigating a vehicle is disclosed. The system includes a trajectory planning module and a trajectory tracking module. The trajectory planning module operates at the vehicle's processor to generate a trajectory for the vehicle. The trajectory tracking module operates at the processor to track the trajectory for navigating the vehicle. The trajectory planning module and the trajectory tracking module operate asynchronously to each other.

[0006] In addition to one or more features described herein, the trajectory planning module solves a first optimization problem to generate a trajectory, and the trajectory tracking module uses the trajectory to solve a second optimization problem to generate actuation signals for navigating the vehicle, the first and second optimization problems being solved asynchronously. Various embodiments of the system include one of the trajectory planning module and the trajectory tracking module operating at different frequencies, and at least one of the trajectory planning module and the trajectory tracking module being event-driven. The trajectory planning module operates at a first frequency, and the trajectory tracking module operates at a second frequency, and the trajectory planning module adjusts the first frequency based on a state estimate provided from the trajectory tracking module to the trajectory planning module. The state estimate indicates the trajectory tracking module's ability to track the trajectory. The trajectory tracking module receives the trajectory, generates an updated trajectory based on the time difference between the trajectory generation and the receipt of a positioning message at the trajectory tracking module, and tracks the updated trajectory at the trajectory tracking module. The trajectory planning module provides the trajectory tracking module with an anchor reference frame having the trajectory.

[0007] In yet another exemplary embodiment, a vehicle is disclosed. The vehicle includes a trajectory planning module and a trajectory tracking module. The trajectory planning module operates at the vehicle's processor to generate a trajectory for the vehicle. The trajectory tracking module operates at the processor to track the trajectory for navigating the vehicle. The trajectory planning module and the trajectory tracking module operate asynchronously to each other.

[0008] In addition to one or more features described herein, the trajectory planning module solves a first optimization problem to generate a trajectory, and the trajectory tracking module uses the trajectory to solve a second optimization problem to generate actuation signals for navigating the vehicle, the first and second optimization problems being solved asynchronously. Various embodiments of the vehicle include those in which one of the trajectory planning module and the trajectory tracking module operates at different frequencies, and at least one of the trajectory planning module and the trajectory tracking module is event-driven. The trajectory planning module operates at a first frequency, and the trajectory tracking module operates at a second frequency, and the trajectory planning module adjusts the first frequency based on a state estimate provided from the trajectory tracking module, wherein the state estimate indicates the trajectory tracking module's ability to track a trajectory. The trajectory tracking module receives the trajectory, generates an updated trajectory based on the time difference between the generation of the trajectory and the receipt of a positioning message at the trajectory tracking module, and tracks the updated trajectory at the trajectory tracking module. The trajectory planning module provides the trajectory tracking module with an anchor reference frame having the trajectory.

[0009] The above-described features and advantages, as well as other features and advantages of this disclosure, will become apparent when taken in conjunction with the accompanying drawings and the following detailed description. Attached Figure Description

[0010] Other features, advantages, and details appear by way of example only in the following detailed specification, which refers to the accompanying drawings, wherein:

[0011] Figure 1 An autonomous vehicle according to an illustrative embodiment is shown;

[0012] Figure 2 A top view showing the illustrative traffic conditions through which the vehicle navigates;

[0013] Figure 3 A schematic diagram of a computing system for performing navigation of a master vehicle is shown in an illustrative embodiment;

[0014] Figure 4 Interpolation of trajectory points for different rates and / or offsets is shown;

[0015] Figure 5 A flowchart illustrating trajectory tracking using interpolated trajectories is shown; and

[0016] Figure 6 A top view illustrating the movement of the main vehicle relative to the anchor reference frame is shown. Detailed Implementation

[0017] The following description is exemplary in nature only and is not intended to limit this disclosure, its application, or use. It should be understood that throughout the drawings, corresponding reference numerals denote the same or corresponding parts and features. As used herein, the term module refers to processing circuitry, which may include application-specific integrated circuits (ASICs), electronic circuitry, processors (shared, dedicated, or clustered) and memories executing one or more software or firmware programs, combinational logic circuitry, and / or other suitable components that provide the described functionality.

[0018] According to an exemplary embodiment, Figure 1 An autonomous vehicle 10 is illustrated. In an exemplary embodiment, the autonomous vehicle 10 is a so-called Level Four or Level Five automation system. A Level Four system signifies "high automation," referring to the driving mode-specific performance of the automated driving system on all aspects of a dynamic driving task, even if the human driver does not appropriately respond to requests for intervention. A Level Five system signifies "full automation," referring to the full-time execution of all aspects of a dynamic driving task by the automated driving system under all road and environmental conditions that can be managed by a human driver. It should be understood that the systems and methods disclosed herein can also be used with autonomous vehicles operating at any of Levels One through Five.

[0019] Autonomous vehicle 10 typically includes at least a navigation system 20, a propulsion system 22, a transmission system 24, a steering system 26, a braking system 28, a sensor system 30, an actuator system 32, and a controller 34. Navigation system 20 determines road-level route planning for autonomous vehicle 10. Propulsion system 22 provides power to autonomous vehicle 10 to generate prime drive, and in various embodiments, propulsion system 22 may include an internal combustion engine, an electric motor such as a traction motor, and / or a fuel cell propulsion system. Transmission system 24 is configured to transmit power from propulsion system 22 to two or more wheels 16 of autonomous vehicle 10 according to a selectable speed ratio. Steering system 26 affects the position of two or more wheels 16. Although depicted as including a steering wheel 27 for illustrative purposes, in some embodiments contemplated within the scope of this disclosure, steering system 26 may not include a steering wheel 27. Braking system 28 is configured to provide braking torque to two or more wheels 16.

[0020] Sensor system 30 includes means for sensing objects in the external environment of autonomous vehicle 10 and determining various parameters of the objects for locating their position and relative velocity with respect to autonomous vehicle 10. These parameters are provided to controller 34. In various embodiments, sensor system 30 includes one or more of radar, lidar, digital camera, etc.

[0021] The controller 34 constructs a trajectory for the autonomous vehicle 10 based on the output of the sensor system 30 and determines actions at the vehicle location to track the trajectory. The controller 34 may provide actuation signals to the actuator system 32 to control the propulsion system 22, transmission system 24, steering system 26 and / or braking system 28 to navigate the autonomous vehicle 10 based on the trajectory.

[0022] The controller 34 includes a processor 36 and a computer-readable storage device or computer-readable storage medium 38. The storage medium includes a program or instructions 39 that, when executed by the processor 36, operate the autonomous vehicle 10 based on sensor system outputs. The computer-readable storage medium 38 may also include a program or instructions 39 that, when executed by the processor 36, constructs a trajectory for navigating the autonomous vehicle 10 through its environment and for tracking a trajectory to actuate the vehicle's motion according to the trajectory.

[0023] Figure 2A top view 200 illustrates an illustrative traffic situation through which an autonomous vehicle 10 navigates. The illustrative traffic situation includes a master vehicle 202 (such as autonomous vehicle 10), a first object 204, and a second object 206. The master vehicle 202 plans a trajectory 208 that allows it to navigate safely between the first object 204 and the second object 206. The trajectory 208 includes multiple trajectory points 208a, ..., 208e. Each trajectory point 208a, ..., 208e indicates the expected position of the master vehicle 202 at a given time as it travels along the trajectory 208. The master vehicle 202 follows the trajectory 208 by sequentially following each of the trajectory points 208a, ..., 208e.

[0024] Each trajectory point 208a, ..., 208e has associated boundary regions 210a, ..., 210e indicating a general area for the safe movement of the master vehicle 202. For a selected trajectory point, the vehicle moves safely while within the boundary regions. The shape of the boundary regions can be selected to suit the specific surrounding environment of their associated trajectory points and any limitations imposed by tracking errors of the expected trajectory. For example, the shape of the boundary regions can be adjusted to prevent the master vehicle 202 from colliding with either the first object 204 or the second object 206 while it remains within the boundary regions. As an illustrative example, boundary regions 210C and 210D are elliptical due to the presence of the first object 204 and the second object 206, while boundary regions 210A, 210B, and 210E are more circular.

[0025] The master vehicle 202 tracks a given trajectory point by moving itself such that vehicle point 212, representing the master vehicle 202, passes as close to the trajectory point as possible. Under good tracking conditions, vehicle point 212 crosses the trajectory point or passes through its associated boundary region. Under poor tracking conditions, given the current trajectory plan, vehicle point 212 can estimate that it will have to pass outside the boundary region. The master vehicle 202 can generate a state estimate indicating its ability to track the trajectory point relative to its boundary region. In various embodiments, the state estimate may be an error value indicating the minimum distance achieved between vehicle point 212 and the trajectory point during tracking.

[0026] Figure 3A schematic diagram of a computing system 300 for performing navigation of a master vehicle 202 is shown in an illustrative embodiment. In various embodiments, the computing system 300 may run on a processor 36 of a controller 34. The computing system 300 includes a trajectory planning module (also referred to herein as "TP module 302") and a trajectory tracking module (also referred to herein as "TT module 304"). The TP module 302 selects, creates, or generates a trajectory for the master vehicle 202, and the TT module 304 tracks the trajectory and generates actuation signals for navigating the master vehicle 202. The actuation signals may be sent to an actuator system 32 for implementation at the master vehicle 202. The actuation signals may be steering signals, speed signals, acceleration signals, braking signals, etc.

[0027] The computing system 300 also includes a positioning information buffer 306 and an environment information buffer 308. The positioning information buffer 306 provides positioning messages (LM) about the vehicle's local coordinates, including but not limited to its attitude (i.e., position, orientation), velocity, and acceleration within its environment. The environment information buffer 308 provides environmental information, including but not limited to the position and velocity of objects within the environment, local speed limits, weather conditions, etc. The positioning information buffer 306 can provide positioning messages to both the TP module 302 and the TT module 304. The environment information buffer 308 provides environmental information to the TP module 302.

[0028] The TP module 302 creates or generates a trajectory (including trajectory points and associated boundary areas) based on location messages, environmental information, and the target or destination of the master vehicle 202. The location messages provide information about the master vehicle 202 necessary to create a trajectory consistent with its current location parameters. The environmental information provides information about objects in the environment (such as first object 204 and second object 206), helping to define the locations of trajectory points and boundary areas. Additionally, trajectory planning can take into account the master vehicle's status, such as whether the trailer is attached to it.

[0029] TP module 302 sends a trajectory message (TM) to TT module 304, which includes a trajectory (i.e., trajectory points and associated boundary regions). The trajectory can be a set of discrete trajectory points and a set of discrete boundaries, where each trajectory point has an associated boundary. Alternatively, the trajectory can be a continuous trajectory, and the boundaries are continuous boundaries associated with the continuous trajectory. The trajectory is generated based on environmental and positioning information. As described below, the trajectory message may also include a timestamp indicating the time it was generated. As further discussed below, the trajectory message may also include an anchor reference frame, which can be used at TT module 304 to maintain a common reference frame between modules.

[0030] In the illustrative embodiment, the trajectory message includes at least the fields shown below or their projections in different coordinate systems:

[0031]

[0032] Where T is the time for planning the trajectory, and x a and ψ a These are the position and orientation vectors of the anchor reference frame, respectively. {.} is a tuple, and [.] is an ordered tuple of elements; t p x is the expected future time to reach point p. p and ψ p It is the desired position and orientation vector of point p. and Let b be the first and second derivatives of the position and orientation of point p, and b be the first and second derivatives of the position and orientation of point p. p It is a boundary area.

[0033] The TT module 304 receives a trajectory message (TM) and tracks the trajectory to generate signals for moving the master vehicle 202 along the trajectory. The TP module 302 generates the trajectory by performing a first set of calculations, and the TT module 304 tracks the trajectory by performing a second set of calculations asynchronously with the first set of calculations. Specifically, the TP module 302 generates the trajectory by solving a first optimization problem using location information, environmental information, and a known destination or target. The TT module 304 uses trajectory points, boundaries, and location information to solve a second optimization problem independently of the first optimization problem to generate actuation signals for the master vehicle 202. In various embodiments, the TT module 304 may adjust the gain or constraints of the second optimization problem for selected tracking points to keep the vehicle within the associated boundaries of the selected tracking points.

[0034] The TT module 304 also tracks the state estimate indicating its ability to track or follow a trajectory provided by the TP module 302. The TT module 304 sends the state estimate to the TP module 302. The TP module 302 can adjust its future planned trajectory to suit the TT module 304 based on the state estimate. The future planned trajectory can then be generated based on environmental information, location information, and the latest state estimate from the TT module 304.

[0035] In various embodiments, TP module 302 and TT module 304 operate asynchronously. In other words, TP module 302 and TT module 304 operate simultaneously but at different rates. TP module 302 may operate or run at the TP frequency or a first frequency, and TT module 304 may operate or run at the TT frequency or a second frequency, wherein the TP frequency and TT frequency are different from each other. Typically, the TP frequency is lower than the TT frequency. TP module 302 may adjust its planned trajectory or planning frequency based on state estimation 310.

[0036] The TT module 304 can provide an up-to-date estimate of its tracking capabilities and periodically send this estimate back to the TP module 302. The TP module 302 then creates a collision-free trajectory that the TT module 304 can track. The trajectory is created based on the up-to-date knowledge of the TT module 304's tracking capabilities. In various embodiments, the new trajectory can have a completely different shape. For example, the new trajectory may allow the vehicle to bypass obstacles instead of walking between them.

[0037] Furthermore, since one or both modules are event-driven, TP module 302 and TT module 304 can be asynchronous. For example, TP module 302 can initiate the solution of its first optimization problem when it receives new positioning data, and TT module 304 can initiate the solution of its second optimization problem when it receives new positioning data. Because positioning information can arrive at TP module 302 and TT module 304 at different times, the operations of TP module 302 and TT module 304 are asynchronous.

[0038] When the module is not event-driven, positioning information can be consumed at the TP module 302 at a first frequency (Freq A or TP frequency) and at the TT module 304 at a second frequency (Freq B or TT frequency). The TP module 302 consumes environmental information at a third frequency (Freq C). The TP module 302 outputs the trajectory (including boundary areas) to the TT module 304 at a fourth frequency (Freq D). The TT module 304 outputs an actuation signal to the actuator system 32 at a fifth frequency (Freq E) and sends a state estimate 310 to the TP module 302 at a sixth frequency (Freq F).

[0039] Figure 4 Interpolation 400 of trajectory points at different rates and / or offsets is shown. The first trajectory S is the original trajectory generated at TP module 302. The second trajectory S * The second trajectory S is an interpolated trajectory generated by the TT module 304 for tracking purposes. * It is derived from the first trajectory S and includes receiving information for tracking at the TT module relative to any delay between the generation of the first trajectory S at the TP module 302 and any desired change in the frequency of the points for tracking.

[0040] Point 402 indicates a first trajectory point of the first trajectory S generated by the TT module 304. Point 402 is located at the actual position of the vehicle at the time of trajectory generation. However, the location information may not arrive at the TT module 304 until later. Point 404 can indicate the interpolation of trajectory S at a first time related to tracking (i.e., the time when the location information is received). The TT module 304 determines the difference between the time of trajectory generation and the time when the location information is received at the TT module 304, and interpolates the trajectory points of the original trajectory via this time difference to generate the second trajectory S. * Interpolation compensates for any time delay between trajectory generation and trajectory tracking. Second trajectory S * It is the input to the second optimization problem at TT module 304 for tracking.

[0041] Figure 5 The interpolated trajectory S is shown. * Flowchart 500 for trajectory tracking. In block 502, the most recent trajectory message TM is retrieved from the communication buffer of TP module 302, and the most recent positioning message LM is retrieved from the positioning information buffer 306.

[0042] In box 504, the time alignment value t is calculated relative to the most recent trajectory message TM and the most recent positioning message LM. * As shown in equation (1):

[0043] t * =LM.t0-TM.t0 (1)

[0044] Where LM.t0 is the creation time of the most recent location message, and TM.t0 is the creation time of the most recent trajectory message. In box 506, if the first trajectory S is a set of discrete points, the method proceeds to box 508, where the trajectory is continuously reconstructed from the discrete points. Then, the method proceeds to box 510. Returning to box 506, if the first trajectory S is not a set of discrete points (i.e., the first trajectory S is continuous), the method proceeds directly to box 510. In box 510, the second trajectory S is interpolated from the first trajectory S and the time delay. * As shown in equation (2):

[0045] S * =(s * (t * ),s * (t * +d t ),s * (t * +2d t (2)

[0046] Where s * (t *) is t * The interpolation trajectory point at s * (t * +dt) is t * The interpolation trajectory point at +dt, where d t This refers to the sampling resolution of the TT module 304, etc. In box 512, the second trajectory S is used. * We use points to solve the trajectory tracking optimization problem.

[0047] Figure 6 A top view 600 illustrating the movement of the master vehicle 202 relative to an anchor reference frame 606 is shown. The anchor reference frame 606 is a global reference frame accessible by both the TP module 302 and the TT module 304. A first vehicle position 602 indicates the position of the master vehicle 202 at the time when the trajectory is planned at the TP module 302. A second vehicle position 604 indicates the position of the master vehicle 202 at a later time t+ε, at which the trajectory is tracked at the TT module 304.

[0048] Due to various delays, the first positioning message received at the TP module 302 when the master vehicle 202 is at the first vehicle position 602 may differ from the second positioning message received at the TT module 304 when the master vehicle 202 is at the second vehicle position 604. The TP module 302 uses the first positioning message to generate both a trajectory and an anchor reference frame 606, and includes both in the trajectory message sent to the TT module 304. When the second positioning message is received at the TT module 304 (i.e., when the master vehicle 202 is at the second vehicle position 604), the TT module can reference the anchor reference frame 606 to track the trajectory.

[0049] While the foregoing disclosure has been described with reference to exemplary embodiments, those skilled in the art will understand that various changes can be made and elements can be substituted with equivalents without departing from its scope. Furthermore, many modifications can be made to adapt particular situations or materials to the teachings of this disclosure without departing from the basic scope of this disclosure. Therefore, this disclosure is not intended to be limited to the specific embodiments disclosed, but will include all embodiments falling within its scope.

Claims

1. A method for navigating a vehicle, comprising: The trajectory of the vehicle is generated at the trajectory planning module running at the processor of the vehicle. The trajectory is tracked at a trajectory tracking module running on the processor to navigate the vehicle, wherein the trajectory planning module and the trajectory tracking module operate asynchronously to each other; and Based on the state estimate provided to the trajectory planning module from the trajectory tracking module, the first frequency of the trajectory planning module is adjusted relative to the second frequency of the trajectory tracking module, wherein the state estimate indicates the ability of the trajectory tracking module to track the trajectory.

2. The method of claim 1, wherein, The trajectory planning module solves a first optimization problem to generate the trajectory, and the trajectory tracking module uses the trajectory to solve a second optimization problem to generate actuation signals for navigating the vehicle. The first optimization problem and the second optimization problem are solved asynchronously.

3. The method of claim 1, further comprising: (i) running the trajectory planning module and the trajectory tracking module at different frequencies; and (ii) at least one of the trajectory planning module and the trajectory tracking module being event-driven.

4. The method of claim 1, further comprising: The trajectory is received at the trajectory tracking module, an updated trajectory is generated based on the time difference between the generation of the trajectory and the receipt of the positioning message at the trajectory tracking module, and the updated trajectory is tracked at the trajectory tracking module.

5. A system for navigating a vehicle, comprising: A trajectory planning module, which operates at the processor of the vehicle to generate the trajectory of the vehicle; as well as A trajectory tracking module, which operates at the processor to track the trajectory for navigating the vehicle, wherein the trajectory planning module and the trajectory tracking module operate asynchronously to each other; The trajectory planning module operates at a first frequency, and the trajectory tracking module operates at a second frequency. The trajectory planning module adjusts the first frequency based on a state estimate provided to the trajectory planning module from the trajectory tracking module, wherein the state estimate indicates the ability of the trajectory tracking module to track the trajectory.

6. The system of claim 5, wherein, The trajectory planning module solves a first optimization problem to generate the trajectory, and the trajectory tracking module uses the trajectory to solve a second optimization problem to generate actuation signals for navigating the vehicle. The first optimization problem and the second optimization problem are solved asynchronously.

7. The system of claim 5, wherein one of the following is true: (i) the trajectory planning module and the trajectory tracking module operate at different frequencies; and (ii) at least one of the trajectory planning module and the trajectory tracking module is event-driven.

8. The system of claim 5, wherein, The trajectory tracking module receives the trajectory, generates an updated trajectory based on the time difference between the generation of the trajectory and the receipt of the positioning message at the trajectory tracking module, and tracks the updated trajectory at the trajectory tracking module.

Images