Safe autonomous overtaking with intention estimation
The present disclosure provides a method in a data processing system that includes at least one processor and at least one memory. The at least one memory includes instructions executed by the at least one processor to implement a vehicle overtaking system. The method includes causing a vehicle control system in an ego vehicle to execute at least a portion of an input control sequence, receiving, from a first plurality of sensors coupled to the ego vehicle, lead vehicle data about a lead vehicle, estimating an intention of the lead vehicle based on the lead vehicle data, and causing the vehicle control system to perform a vehicle maneuver based on the intention of the lead vehicle.
1. A method implemented by a data processing system comprising at least one processor and at least one memory, the at least one memory comprising a plurality of dynamics models and instructions executed by the at least one processor to implement a vehicle overtaking system, the method comprising:
determining, at a first time, an input control sequence for an ego vehicle, the input control sequence comprising a plurality of acceleration values of the ego vehicle, the plurality of acceleration values being determined to ensure that an observed trajectory of a lead vehicle over a finite time horizon is only consistent with one dynamics model of the plurality of dynamics models, wherein the first time is prior to the finite time horizon, wherein the input control sequence is designed to ensure that output trajectories of each of the plurality of dynamics models differ by a predetermined threshold in at least one time instance within a testing period. wherein the testing period is prior to the finite time horizon;
causing a vehicle control system in the ego vehicle to execute at least a first acceleration value of the plurality of acceleration values of the input control sequence;
receiving, from a first plurality of sensors coupled to the ego vehicle, lead vehicle data about the lead vehicle;
estimating an intention of the lead vehicle by selecting a dynamics model predictive of a maneuver of the lead vehicle that includes a physics-based parameter based on the lead vehicle data;
generating a desired location ahead of the lead vehicle based on the dynamics model corresponding to the estimated intention;
calculating, based on the desired location, a trajectory for the ego vehicle, the trajectory calculated to move the ego vehicle toward the desired position; and
causing the vehicle control system to perform a vehicle maneuver based on the calculated trajectory.
2. The method of claim 1 further comprising:
receiving, from a second plurality of sensors coupled to the ego vehicle, oncoming vehicle data about an oncoming vehicle; and
receiving, from a third plurality of sensors coupled to the ego vehicle, ego vehicle data about the ego vehicle.
3. The method of claim 2 , wherein at least one sensor of the first plurality of sensors belongs to the second plurality of sensors and the third plurality of sensors.
4. The method of claim 2 , wherein the input control sequence comprises a plurality of discrete time points and at least a portion of the ego vehicle data, lead vehicle data, and the oncoming vehicle data is associated with one of the discrete time points.
5. The method of claim 2 , wherein the vehicle maneuver comprises:
determining the ego vehicle is unable to overtake the lead vehicle while keeping the ego vehicle within a predetermined set of safety constraints based on at least one of the ego vehicle data, lead vehicle data, oncoming vehicle data, or the estimated intention of the lead vehicle; and
piloting the ego vehicle to follow behind the lead vehicle within the predetermined set of safety constraints.
6. The method of claim 5 , wherein the predetermined set of safety constraints comprises a predetermined headway separation distance between the ego vehicle and the lead vehicle.
7. The method of claim 2 , wherein the vehicle maneuver comprises:
determining the ego vehicle is able to overtake the lead vehicle while keeping the ego vehicle within a predetermined set of safety constraints based on at least one of the ego vehicle data, lead vehicle data, oncoming vehicle data, or the estimated intention of the lead vehicle; and
piloting the ego vehicle along a trajectory to overtake the lead vehicle within the predetermined set of safety constraints.
8. The method of claim 7 , wherein the method further comprises calculating the trajectory based on an ego vehicle speed, a lead vehicle speed, an ego vehicle location, and a lead vehicle location, wherein each of the ego vehicle speed, the lead vehicle speed, the ego vehicle location, and the lead vehicle location are based on at least one of the ego vehicle data and the lead vehicle data.
9. The method of claim 7 , wherein the determining the ego vehicle is able to overtake the lead vehicle while keeping the ego vehicle within the predetermined set of safety constraints is further based on at least one of an oncoming vehicle speed and an oncoming vehicle location, each of the oncoming vehicle speed and the oncoming vehicle location based on the oncoming vehicle data.
10. The method of claim 1 , wherein determining the intention of the lead vehicle comprises:
providing the lead vehicle data to a model selection algorithm;
receiving an output of the model selection algorithm;
determining a most likely intention of the lead vehicle based on the output of the model selection algorithm.
11. The method of claim 10 , wherein the lead vehicle data comprises a lead vehicle speed.
12. A driving control system for an ego vehicle, the driving control system comprising:
a first plurality of sensors coupled to the ego vehicle; and
a controller in electrical communication with the first plurality of sensors, the controller being configured to execute a program stored in the controller to:
determine, at a first time, an input control sequence for the ego vehicle, the input control sequence comprising a plurality of acceleration values of the ego vehicle, the plurality of acceleration values being determined to ensure that an observed trajectory of a lead vehicle is consistent with only one dynamics model of a plurality of dynamics models, wherein the input control sequence is designed to ensure that output trajectories of each of the plurality of dynamics models differ by a predetermined threshold in at least one time instance within a testing period;
for each dynamics model of the plurality of dynamics models, calculate at least one physics-based parameter based on at least a portion of the input control sequence;
cause a vehicle control system in the ego vehicle to execute at least a portion of the input control sequence;
receive, from a first plurality of sensors coupled to the ego vehicle, lead vehicle data about the lead vehicle;
estimate an intention of the lead vehicle by selecting a dynamics model from the plurality of dynamics models, the dynamics model being selected based on the lead vehicle data and the at least one physics-based parameter corresponding to the dynamics model;
generate a desired location based on the lead vehicle data and the selected dynamics model;
generate, based on the selected dynamics model and the desired location, a velocity vector field for the ego vehicle, the velocity vector field generated to guide the ego vehicle around the lead vehicle to the desired location; and
cause the vehicle control system to perform a vehicle maneuver based on the velocity vector field and the estimated intention of the lead vehicle.
13. The system of claim 12 further comprising:
a second plurality of sensors coupled to the ego vehicle; and
a third plurality of sensors coupled to the ego vehicle, and wherein the controller is further configured to:
receive, from the second plurality of sensors coupled to the ego vehicle, oncoming vehicle data about an oncoming vehicle; and
receive, from the third plurality of sensors coupled to the ego vehicle, ego vehicle data about the ego vehicle.
14. The system of claim 13 , wherein at least one sensor of the first plurality of sensors belongs to the second plurality of sensors and the third plurality of sensors.
15. The system of claim 13 , wherein the input control sequence comprises a plurality of discrete time points and at least a portion of the ego vehicle data, lead vehicle data, and the oncoming vehicle data is associated with one of the discrete time points.
16. The system of claim 13 , wherein the vehicle maneuver comprises additional steps in the program and the controller is further configured to:
determine the ego vehicle is unable to overtake the lead vehicle while keeping the ego vehicle within a predetermined set of safety constraints based at least one of the ego vehicle data, lead vehicle data, oncoming vehicle data, or the estimated intention of the lead vehicle; and
pilot the ego vehicle to follow behind the lead vehicle within the predetermined set of safety constraints.
17. The system of claim 16 , wherein the predetermined set of safety constraints comprises a predetermined headway separation distance between the ego vehicle and the lead vehicle.
18. The system of claim 13 , the vehicle maneuver comprises additional steps in the program and the controller is further configured to:
determine the ego vehicle is able to overtake the lead vehicle while keeping the ego vehicle within a predetermined set of safety constraints based on at least one of the ego vehicle data, lead vehicle data, oncoming vehicle data, or the estimated intention of the lead vehicle; and
pilot the ego vehicle along a trajectory to overtake the lead vehicle within the predetermined set of safety constraints.
19. The system of claim 18 , wherein the ego vehicle data comprises an ego vehicle speed and an ego vehicle location, the lead vehicle data comprises a lead vehicle speed and a lead vehicle location, wherein the controller is further configured to calculate the trajectory based on the ego vehicle speed, the lead vehicle speed, the ego vehicle location, and the lead vehicle location, and wherein the trajectory includes a desired location located ahead of the lead vehicle determined based on dynamics of the lead vehicle corresponding to the estimated intention.
20. The method of claim 1 , wherein the physics-based parameter includes a longitudinal velocity.
21. The method of claim 1 , wherein the physics-based parameter includes a longitudinal acceleration.
22. The method of claim 1 , wherein the physics-based parameter includes a lateral velocity.
23. The method of claim 1 , wherein the physics-based parameter includes a coefficient of drag.
24. The system of claim 12 , wherein the physics-based parameter includes at least one of a longitudinal velocity, a longitudinal acceleration, a lateral velocity, a coefficient of drag, or combinations thereof.
25. A driving control system for an ego vehicle, the driving control system comprising:
a first plurality of sensors coupled to the ego vehicle; and
a controller in electrical communication with the first plurality of sensors, the controller being configured to execute a program stored in the controller to:
determine, at a first time, an input control sequence for the ego vehicle, the input control sequence comprising a plurality of acceleration values of the ego vehicle, the plurality of acceleration values being determined to ensure that an observed trajectory of a lead vehicle is consistent with only one intention model of a plurality of intention models, wherein the input control sequence is designed to ensure that output trajectories of each of the plurality of intention models differ by a predetermined threshold in at least one time instance within a testing period;
for each intention model of a plurality of intention models, calculate at least one output trajectory for the lead vehicle based at least in part on the input control sequence;
cause a vehicle control system in the ego vehicle to execute at least a portion of the input control sequence;
receive, from the first plurality of sensors coupled to the ego vehicle, lead vehicle data about the lead vehicle;
estimate the intention of the lead vehicle by selecting an intention model from the plurality of intention models, the intention model being selected based on the lead vehicle data and the at least one output trajectory corresponding to the intention model;
generate a desired location for the ego vehicle based on the selected intention model;
generate, based on the desired location and the selected intention model, a vector field configured to guide the ego vehicle around the lead vehicle to the desired location; and
cause the vehicle control system to perform a vehicle maneuver based on the generated vector field.