System and method for controlling autonomous vehicles

• US 10,012,984 B2
• Filed: 12/14/2015
• Issued: 07/03/2018
• Est. Priority Date: 12/14/2015
• Status: Active Grant

• Current Assignee: Mitsubishi Electric Research Laboratories, Inc. (Mitsubishi Electric Corporation)
• Original Assignee: Mitsubishi Electric Research Laboratories, Inc. (Mitsubishi Electric Corporation)
• Inventors: Berntorp, Karl, Arslan, Oktay
• Primary Examiner(s): Camby, Richard M
• Application Number:
  US14/968,848
• Publication Number:
  US 20170168485A1
• Time in Patent Office: 932 Days
• CPC Class Codes: B60W 30/00 Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
• CPC Class Codes: B60W 60/0011 involving control alternatives for a single driving scenario, e.g. planning several paths to avoid obstacles
• CPC Class Codes: B60W 60/00276 for two or more other traffic participants
• CPC Class Codes: G01C 21/20 Instruments for performing navigational calculations G01C21/24, G01C21/26 take precedence
• CPC Class Codes: G01C 21/26 specially adapted for navigation in a road network
• CPC Class Codes: G01C 21/3453 Special cost functions, i.e. other than distance or default speed limit of road segments
• CPC Class Codes: G05D 1/0217 in accordance with energy consumption, time reduction or distance reduction criteria
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30

Abstract

A method determines iteratively a motion of the vehicle from an initial location and a target location. An iteration of the method determines a location between the initial location and the target location that satisfies spatial constraints on locations of the vehicle and determines state transitions of the vehicle moved to the location from a set of neighboring locations determined during previous iterations. The method selects a neighboring location resulting in an optimal state transition of the vehicle and updates a graph of state transitions of the vehicle determined during previous iterations with the optimal state transition. The motion of the vehicle is determined a sequence of state transitions connecting the initial location with the target location and the vehicle is controlled according to the determined motion.

Claims

1. A method for controlling a vehicle, comprising:

determining iteratively a motion of the vehicle from an initial location and a target location, wherein the motion is defined by state transitions connecting states of the vehicle, each state includes a location, a velocity and a heading of the vehicle, wherein the motion is determined iteratively until a termination condition is met, wherein an iteration comprises;

determining a location between the initial location and the target location, wherein the location satisfies spatial constraints on locations of the vehicle;

determining state transitions of the vehicle moved to the location from a set of neighboring locations determined during previous iterations, wherein each state transition satisfies constraints on a motion of the vehicle;

selecting a neighboring location resulting in an optimal state transition of the vehicle from the state at the neighboring location to the state at the location; and

updating a graph of state transitions of the vehicle determined during previous iterations with the optimal state transition;

determining the motion of the vehicle as a sequence of state transitions connecting the initial location with the target location; and

controlling the vehicle according to the determined motion, wherein steps of the method are performed by a processor of the vehicle.

2. The method of claim 1, wherein the determining the current location comprises:

sampling a space of locations between the initial location and the target location to produce a sampled location;

connecting the sampled location with a previously sampled location that is closest to the current location; and

selecting the sampled location as the current location if a line segment connecting the sampled location with the previously sampled location is free from a collision.

3. The method of claim 2, further comprising:

determining a length of the line segment; and

replacing the sampled location with an intermediate location on the line segment at a predetermined distance along the line segment from the previously sampled location, if the length is greater than the predetermined distance.

4. The method of claim 1, wherein the determining state transitions comprises:

selecting the set of neighboring locations within a distance from the current location;

determining a state transition of the vehicle from the state of each neighboring location in the set to the current location to produce a set of state transitions, wherein each state transition is determined by emulating responses of a vehicle controller moving the vehicle using a model of the vehicle controller.

5. The method of claim 4, wherein the constraints on the motion of the vehicle define that the state transitions are free from the collision, comprising:

testing whether the state transition is free from the collision; and

removing the state transition from the set of the state transition, if the state transition results in a collision.

6. The method of claim 5, further comprising:

determining a position of an obstacle between the initial location and the target location as a function of time;

determining the state transition as a function of time; and

testing whether the position of the obstacle and the state transition intersects in both the time and the space.

7. The method of claim 4, further comprising:

determining the distance as a function of a total number of previously sampled locations.

8. The method of claim 1, wherein the selecting comprises:

determining a cost of each state transition in the set according to a cost function; and

selecting the optimal state transition corresponding to a minimal or a maximal cost.

9. The method of claim 1, further comprising:

updating the graph of state transitions of the vehicle with a new state transition from the current location to a previously determined location, if the new state transition reduces a cost of reaching the previously sampled location through the graph.

10. The method of claim 1, further comprising:

determining the sequence of state transitions using a cost function optimizing a parameter of the motion.

11. The method of claim 10, wherein the cost function minimizes deviation of the parameter of the motion.

12. The method of claim 1, further comprising:

determining an intermediate target location between the initial location and the target location;

determining a first motion between the initial location and the intermediate target location;

controlling the vehicle to reach the intermediate target location using the first motion;

determining a second motion between the intermediate target location and the target location;

controlling the vehicle to reach the target location using the second motion.

13. The method of claim 1, further comprising:

determining a period of time allowed for determining the motion;

determining a maximal distance allowing to compute the motion within the period of time; and

determining a location at the maximal distance from the initial location toward a final destination as the target location.

14. A control system of a vehicle, comprising:

a navigation system to determine an initial location and a target location of the vehicle;

a vehicle controller to control a motion of the vehicle; and

a motion-planning system including a processor and a memory storing the initial location and the target location, wherein the motion-planning system iteratively determines state transitions moving the vehicle to a location from a plurality of neighboring locations having states determined during previous iterations to produce a graph of state transitions of the vehicle, wherein the motion-planning system determines the motion of the vehicle as a sequence of state transitions connecting the initial location with the target location, and submits the motion to the vehicle controller.

15. The control system of claim 14, wherein the motion-planning system determines that each state transition is free from a collision.

16. The control system of claim 15, wherein the motion-planning system is configured to

determine a position of an obstacle between the initial location and the target location as a function of time;

determine the state transition as a function of time; and

test whether the position of the obstacle and the state transition intersects in both the time and the space.

17. The control system of claim 15, wherein the motion-planning system is configured to determine iteratively a motion of the vehicle from the initial location and the target location, wherein the motion is defined by state transitions connecting states of the vehicle, each state includes a location, a velocity and a heading of the vehicle, wherein the motion is determined iteratively until a termination condition is met, wherein an iteration comprises:

determining a location between the initial location and the target location, wherein the location satisfies spatial constraints on locations of the vehicle;

determining state transitions of the vehicle moved to the location from a set of neighboring locations determined during previous iterations, wherein each state transition satisfies constraints on a movement of the vehicle;

selecting a neighboring location resulting in an optimal state transition of the vehicle from the state at the neighboring location to the state at the location; and

updating the graph of state transitions of the vehicle determined during previous iterations with the optimal state transition connecting the selected neighboring location with the location.

18. The control system of claim 14, wherein the motion-planning system determines that each state transition by emulating responses of the vehicle controller with a model of the vehicle controller.

19. The control system of claim 14, wherein the motion-planning system determines the sequence of state transitions using a cost function optimizing a parameter of the motion.

20. The control system of claim 14, wherein the motion-planning system updates the graph of state transitions of the vehicle with a new state transition from the current location to a previously determined location, if the new state transition reduces a cost of reaching the previously sampled location through the graph.