Cooperative driving and collision avoidance by distributed receding horizon control

• US 9,669,828 B2
• Filed: 06/01/2012
• Issued: 06/06/2017
• 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): Tran, Khoi
• Assistant Examiner(s): NGUYEN, ROBERT T
• Application Number:
  US13/486,598
• Publication Number:
  US 20130325306A1
• Time in Patent Office: 1,831 Days
• Field of Search: 701 1, 701 2, 701 24, 701 25, 701 26, 701 28, 701300, 701301
• 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 for controlling a first coordinating vehicle, the method comprising:

receiving 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;

calculating an assumed trajectory for the first coordinating vehicle by solving an optimal control problem, the optimal control problem not including an avoidance constraint;

detecting a conflict based on the received trajectory information and the calculated assumed trajectory; and

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

wherein 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.

2. A controller for a first coordinating vehicle, the controller 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; and

a computer processor configured to execute instructions stored on a non-transitory memory, the instructions including

calculating an assumed trajectory for the first coordinating vehicle by solving an optimal control problem, the optimal control problem not including an avoidance constraint,

detecting a conflict based on the received trajectory information and the calculated assumed trajectory, and

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

wherein 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.

3. The controller according to claim 2, wherein the conflict is detected by determining, based on the received trajectory information and the calculated assumed trajectory, whether a first avoidance boundary of the first coordinating vehicle and a second 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 2, 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 optimization 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 a non-coordinating vehicle within a predetermined detection range, wherein

the processor determines trajectory information for the non-coordinating vehicle based on the detected position and speed information, and

the processor detects a conflict between the first coordinating vehicle and the non-coordinating vehicle based on the determined trajectory information and the assumed trajectory.

9. 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.

10. The controller according to claim 5, 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.

11. The controller according to claim 5, 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.

12. The controller according to claim 5, wherein the processor is further configured to, during each of the 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.

13. 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, of the second coordinating vehicles, which is determined to be in conflict with the first coordinating vehicle.

14. 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.

15. The controller according to claim 8, wherein the processor sets a third avoidance boundary for the non-coordinating vehicle, the third avoidance boundary being smaller in size than the first and second avoidance boundaries.

16. 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.

17. 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.

18. The controller according to claim 2, wherein the computer processor is further configured to control the first coordinating vehicle based upon the optimized trajectory.

19. 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 execute instructions stored on a non-transitory memory, the instructions including;

calculating an assumed trajectory by solving an optimal control problem, the optimal control problem not including an avoidance constraint,

for each received trajectory message, detecting a conflict with a corresponding vehicle based on the received trajectory information and the calculated assumed trajectory, and

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

wherein 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.