System and method for controlling autonomous or semi-autonomous vehicle

• US 9,568,915 B1
• Filed: 02/11/2016
• Issued: 02/14/2017
• Est. Priority Date: 02/11/2016
• 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, Di Cairano, Stefano
• Primary Examiner(s): Alharbi, Adam
• Application Number:
  US15/041,639
• Time in Patent Office: 369 Days
• Field of Search: None
• US Class Current: 1/1
• 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 30/095 Predicting travel path or likelihood of collision
• CPC Class Codes: B60W 50/0097 Predicting future conditions
• CPC Class Codes: G05D 1/0214 in accordance with safety or protection criteria, e.g. avoiding hazardous areas monitoring the location of vehicles within a certain area, e.g. forbidden or allowed areas, in traffic control systems for road vehicles G08G1/13
• CPC Class Codes: G06N 20/00 Machine learning
• CPC Class Codes: G06N 3/006 based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
• CPC Class Codes: G06N 5/01 Dynamic search techniques; Heuristics; Dynamic trees; Branch-and-bound
• CPC Class Codes: G06N 7/01 Probabilistic graphical models, e.g. probabilistic networks

Abstract

A method controls a motion of a vehicle using a model of the motion of the vehicle that includes an uncertainty. The method samples a control space of possible control inputs to the model of the motion of the vehicle to produce a set of sampled control inputs and determines a probability of each sampled control input to move the vehicle into state satisfying constraints on the motion of the vehicle. The method determines, using the probabilities of the sampled control inputs, a control input having the probability to move the vehicle in the state above a threshold. The control input is mapped to a control command to at least one actuator of the vehicle to control the motion of the vehicle.

Claims

1. A method for controlling a motion of a vehicle, comprising:

sampling a control space of possible control inputs to a model of the motion of the vehicle to produce a set of sampled control inputs, wherein the model of the motion of the vehicle includes an uncertainty;

determining, using the model of the motion of the vehicle, a probability of each sampled control input to move the vehicle into a state satisfying constraints on the motion of the vehicle, comprising;

determining an initial state of the vehicle;

submitting the initial state and the sampled control input to the model of the motion of the vehicle to estimate a transition of the vehicle from the initial state to a next state;

selecting a value of a probability distribution function (PDF) over states of the vehicle at a point corresponding to the next state as the probability of the sampled control input;

determining a probability of the next state to intersect with an uncertainty region of an obstacle; and

assigning a zero probability to the sampled control input when the probability of the next state is above a collision threshold;

determining, using the probabilities of the sampled control inputs, a control input having the probability to move the vehicle in the state above a threshold;

mapping the control input to a control command to at least one actuator of the vehicle; and

controlling the motion of the vehicle according to the control command, wherein steps of the method are performed using a processor of the vehicle.

2. The method of claim 1, wherein the model of the motion of the vehicle is a function describing transitions of states of the vehicle, wherein the function includes noise acting on the state, wherein the state includes a location of the vehicle, a velocity of the vehicle and a heading of the vehicle, and wherein the noise acting on the state includes the uncertainty formed by one or combination of an uncertainty on accuracy of a dynamics of the vehicle described by the function and an uncertainty on accuracy of parameters of the vehicle used by the function.

3. The method of claim 1, wherein the determining the control input comprises:

aggregating the sampled control inputs having the probability above the threshold using a weighted average function to produce the control input, wherein a weight for each sampled control input in the weighted average function is its corresponding probability.

4. The method of claim 1, wherein the determining the control input comprises:

selecting a sampled control input having the highest probability among the probabilities of the sampled control inputs above the threshold as the control input.

5. The method of claim 1, wherein the constraints on the motion of the vehicle include one or combination of a bound on a deviation of a location of the vehicle from a middle of a road, a bound on a change from a current acceleration and a heading angle of the vehicle, a bound on a deviation from a desired velocity profile of the vehicle, and a bound on a minimal distance to an obstacle on the road.

6. The method of claim 1, wherein the constraints on the motion of the vehicle include one or combination of a probability of a deviation of a location of the vehicle from a middle of a road, a probability of a deviation from a current acceleration and a heading angle of the vehicle, a probability on a deviation from a desired velocity profile of the vehicle, and a probability on violating a minimal distance to an obstacle on the road.

7. The method of claim 1, further comprising:

determining the PDF using the constraints on the motion of the vehicle and the uncertainty of the model of the motion of the vehicle.

8. The method of claim 1, wherein the initial state is a current state of the vehicle.

9. The method of claim 1, further comprising:

determining iteratively a sequence of control inputs specifying the motion of the vehicle from the initial state of the vehicle to a target state of the vehicle, wherein the initial state is the state corresponding to the control input determined during a previous iteration of the method.

10. The method of claim 9, wherein the PDF is determined for each iteration of the method, and wherein the PDF for at least one iteration includes multiple discrete sections with values above the threshold, the iteration comprising:

determining a set of control inputs, wherein there is one control for each discrete section of the PDF to produce the set of motions connecting the current state of the vehicle with the target state of the vehicle; and

selecting from the set of motions the motion optimizing a cost function.

11. The method of claim 9, further comprising:

determining the target state based on an objective of the motion and a computational power of the processor.

12. A control system of a vehicle, comprising:

a motion-planning system including a processor and a memory storing a model of the motion of the vehicle that includes an uncertainty of the motion and constraints on the motion of the vehicle, wherein the motion-planning system samples a control space of possible control inputs to the model of the motion of the vehicle to produce a set of sampled control inputs;

determines, using the model of the motion of the vehicle, a probability of each sampled control input to move the vehicle into a state satisfying constraints on the motion of the vehicle; and

determines, using the probabilities of the sampled control inputs, a control input having the probability above a threshold,

wherein the motion-planning system determines the probability of the sampled control input by selecting a value of a probability distribution function (PDF) over states of the vehicle as the probability of the sampled control input at a point corresponding to a next state transitioned from an initial state according to the sampled control input;

a sensor to determine a position of an obstacle as a function of time, wherein the motion-planning system determines a probability of the next state to intersect with an uncertainty region of the obstacle and assigns a zero probability to the sampled control input when the probability of the next state to intersect with the uncertainty region of the obstacle is above a collision threshold; and

a vehicle controller to map the control input to a control command to at least one actuator of the vehicle and to control the motion of the vehicle using the control command to the actuator of the vehicle.

13. The control system of claim 12, wherein the model of the motion of the vehicle is a function describing transitions of states of the vehicle, wherein the function includes noise acting on the state, wherein the state includes a location of the vehicle, a velocity of the vehicle and a heading of the vehicle, and wherein the noise acting on the state includes the uncertainty formed by one or combination of an uncertainty on accuracy of a dynamics of the vehicle described by the function and an uncertainty on accuracy of parameters of the vehicle used by the function.

14. The control system of claim 12, wherein the motion-planning system aggregates the sampled control inputs having the probability above the threshold using a weighted average function to produce the control input, wherein a weight for each sampled control input in the weighted average function is its corresponding probability.

15. The control system of claim 12, wherein the constraints on the motion of the vehicle include one or combination of a probability of a deviation of a location of the vehicle from a middle of a road, a probability of a deviation from a current acceleration and a heading angle of the vehicle, a probability on a deviation from a desired velocity profile of the vehicle, a probability on violating a minimal distance to an obstacle on the road.

16. The control system of claim 12, further comprising:

a navigation system to determine a current state of the vehicle and a target state of the vehicle, wherein the motion-planning system determines iteratively a sequence of control inputs specifying the motion of the vehicle from the current state of the vehicle to the target state of the vehicle, wherein the initial state is the current state or the state corresponding to the control input determined during a previous iteration of the method.