Cooperative driving and collision avoidance by distributed receding horizon control

• US 10,679,501 B2
• Filed: 05/03/2017
• Issued: 06/09/2020
• Est. Priority Date: 06/01/2012
• Status: Active Grant

• Current Assignee: Toyota Jidosha Kabushiki Kaisha (Toyota Motor Corporation)
• Original Assignee: Toyota Motor Engineering & Manufacturing North America Incorporated (Toyota Motor Corporation)
• Inventors: Caveney, Derek Stanley, Dunbar, William Bruce
• Primary Examiner(s): Nguyen, Robert T
• Application Number:
  US15/585,273
• Publication Number:
  US 20170330461A1
• Time in Patent Office: 1,133 Days
• CPC Class Codes: B60W 30/0953 the prediction being responsive to vehicle dynamic parameters
• CPC Class Codes: B60W 30/0956 the prediction being responsive to traffic or environmental parameters
• CPC Class Codes: G05D 1/0289 with means for avoiding collisions between vehicles vehicle fittings for automatically controlling speed including means for detecting potential obstacles B60K31/0008; avoiding obstacles by action on the steering system B62D; radar, sonar, lidar systems designed for anti-collision purposes G01S13/93, G01S15/93, G01S17/93
• CPC Class Codes: G05D 1/0295 by at least one leading vehicle of the fleet
• CPC Class Codes: G05D 1/104 involving a plurality of aircrafts, e.g. formation flying traffic control systems for aircraft G08G5/00
• CPC Class Codes: G08G 1/052 with provision for determining speed or overspeed speed measuring in general G01P
• CPC Class Codes: G08G 1/161 Decentralised systems, e.g. inter-vehicle communication
• CPC Class Codes: G08G 1/166 for active traffic, e.g. moving vehicles, pedestrians, bikes
• CPC Class Codes: G08G 1/22 Platooning, i.e. convoy of communicating vehicles

Abstract

Distributed control of vehicles with coordinating cars that implement a cooperative control method, and non-coordinating cars that are presumed to follow predictable dynamics. A cooperative control method can combine distributed receding horizon control, for optimization-based path planning and feedback, with higher level logic, to ensure that implemented plans are collision free. The cooperative method can be completely distributed with partially synchronous execution, and can afford dedicated time for communication and computation, features that are prerequisites for implementation on real freeways. The method can test for conflicts and can calculate optimized trajectories by adjusting parameters in terminal state constraints of an optimal control problem.

Claims

1. A method comprising:

performing a first task including calculating an assumed trajectory for a first coordinating vehicle by solving an optimal control problem; and

perform a second task including detecting a conflict based on trajectory information for a non-coordinating vehicle and the calculated assumed trajectory using a first avoidance boundary of the first coordinating vehicle and a second avoidance boundary of the non-coordinating vehicle, wherein

when a conflict is detected, terminal state constraints in the optimal control problem are adjusted and an optimized trajectory for the first coordinating vehicle is calculated with the adjusted constraints in the optimal control problem such that the detected conflict is resolved,

the optimal control problem includes cost terms including a move suppression (MS) term indicating an amount that the optimized trajectory may deviate from the assumed trajectory, and

the method further comprises controlling the first coordinating vehicle based upon at least one of the assumed trajectory and the optimized trajectory.

2. A controller for a first coordinating vehicle, the controller comprising:

a computer processor configured to;

perform a first task including calculating an assumed trajectory for the first coordinating vehicle by solving an optimal control problem, and

perform a second task including detecting a conflict based on trajectory information for a non-coordinating vehicle and the calculated assumed trajectory using a first avoidance boundary of the first coordinating vehicle and a second avoidance boundary of the non-coordinating vehicle, wherein

when a conflict is detected, terminal state constraints in the optimal control problem are adjusted and an optimized trajectory for the first coordinating vehicle is calculated with the adjusted constraints in the optimal control problem such that the detected conflict is resolved,

the optimal control problem includes cost terms including a move suppression (MS) term indicating an amount that the optimized trajectory may deviate from the assumed trajectory, and

the computer processor is further configured to control the first coordinating vehicle based upon at least one of the assumed trajectory and the optimized trajectory.

3. The controller according to claim 2, further comprising:

a communication terminal configured to receive trajectory messages from a plurality of second coordinating vehicles in a communication range, the trajectory messages including vehicle trajectory information for a predetermined update interval,

wherein the conflict is detected by determining, based on the received trajectory information and the calculated assumed trajectory, whether the first avoidance boundary of the first coordinating vehicle and a third avoidance boundary of any one of the second coordinating vehicles intersect during the update interval.

4. The controller according to claim 2, wherein:

the terminal state constraints include a velocity term and a vehicle spacing term; and

when a conflict is detected, the processor adjusts the velocity term and/or the vehicle spacing term in the optimal control problem such that the detected conflict is resolved.

5. The controller according to claim 3, wherein during each of successive update intervals, the computer processor is further configured to:

recursively detect conflicts between the first coordinating vehicle and each of the second coordinating vehicles that will occur during the update interval; and

calculate the optimized trajectory for each of the recursively detected conflicts.

6. The controller according to claim 2, wherein the computer processor is further configured to:

classify the detected conflict based on a predetermined rule set; and

adjust the terminal state constraints based on the detected conflict classification.

7. The controller according to claim 2, wherein the assumed trajectory for the first coordinating vehicle is calculated by solving the optimal control problem with terminal constraints modified by a high-level maneuver plan.

8. The controller according to claim 2, further comprising:

a sensor configured to detect a position and speed information for the non-coordinating vehicle within a predetermined detection range, wherein

the computer processor is configured to determine the trajectory information for the non-coordinating vehicle based on the detected position and speed information.

9. The controller according to claim 2, wherein the conflict is detected by determining, based on the trajectory information for the non-coordinating vehicle and the calculated assumed trajectory, whether the first avoidance boundary of the first coordinating vehicle and the second avoidance boundary of the non-coordinating vehicle intersect.

10. The controller according to claim 5, wherein during each of the successive update intervals, the communication terminal is further configured to:

transmit the optimized trajectory to the second coordinating vehicles; and

receive updated trajectory messages from the second coordinating vehicles.

11. The controller according to claim 2, wherein the assumed trajectory for the first coordinating vehicle in a current update interval is initially set to the calculated optimized trajectory from an immediately preceding update interval.

12. The controller according to claim 2, wherein the assumed trajectory for the first coordinating vehicle in a current update interval is initially set, in the absence of a high-level maneuver plan, by extrapolating the optimized trajectory from an immediately preceding update interval.

13. The controller according to claim 2, wherein the computer processor is further configured to, during each of successive update intervals, calculate the optimized trajectory for the detected conflict with an earliest loss-of-separation that requires action by the first coordinating vehicle.

14. The controller according to claim 6, wherein the conflict classification is based on a position of the first coordinating vehicle relative to a conflicting vehicle which is determined to be in conflict with the first coordinating vehicle.

15. The controller according to claim 2, wherein when a conflict is detected, the MS term is set such that the amount from which the optimized trajectory may deviate from the assumed trajectory is increased.

16. The controller according to claim 2, wherein the second avoidance boundary is smaller in size than the first avoidance boundary.

17. The controller according to claim 2, wherein the optimal control problem does not include at least one of (1) an avoidance constraint between the first coordinating vehicle and another vehicle and (2) an avoidance constraint between the first coordinating vehicle and a road boundary.

18. The controller according to claim 2, wherein the optimal control problem does not include an avoidance constraint between the first coordinating vehicle and another vehicle and does not include an avoidance constraint between the first coordinating vehicle and a road boundary.

19. The controller according to claim 2, wherein the second task is discrete from the first task.

20. A vehicle coordination system comprising a plurality of coordinating vehicles, each vehicle (i=1, 2, 3, . . . , N) having a controller including:

a communication terminal configured to receive trajectory messages from each vehicle, of the plurality of coordinating vehicles, in a communication range, the trajectory messages including vehicle trajectory information for a predetermined update interval; and

a computer processor configured to;

perform a first task including calculating an assumed trajectory by solving an optimal control problem, and

perform a second task including detecting conflicts with corresponding vehicles based on the received trajectory information, trajectory information for a non-coordinating vehicle, and the calculated assumed trajectory using a first avoidance boundary of a first coordinating vehicle, a second avoidance boundary of a second coordinating vehicle, and a third avoidance boundary of the non-coordinating vehicle, wherein

when a conflict is detected, terminal state constraints in the optimal control problem are adjusted and an optimized trajectory is calculated with the adjusted constraints in the optimal control problem such that the detected conflict is resolved,

the optimal control problem includes cost terms including a move suppression (MS) term indicating an amount that the optimized trajectory may deviate from the assumed trajectory, and

the computer processor is further configured to control the first coordinating vehicle based upon at least one of the assumed trajectory and the optimized trajectory.