System and method for controlling semi-autonomous vehicles

Patent metadata

Field	Value
Publication number	US-9821801-B2
Application number	US-201514753162-A
Country	US
Kind code	B2
Filing date	Jun 29, 2015
Priority date	Jun 29, 2015
Publication date	Nov 21, 2017
Grant date	Nov 21, 2017

Abstract

A method for controlling a semi-autonomous vehicle modifies a current path for the vehicle desired by a driver of the vehicle. The current path starts at a current position of the vehicle and ends in a target position of the vehicle and the method modifies the current path while preserving the current position and the target position of the vehicle in the modified path. The method overrides the actions of the driver to control a movement of the vehicle according to the modified path.

First claim

We claim:

1. A method for controlling a semi-autonomous vehicle, comprising:
determining a current path for the vehicle in response to actions received from a driver of the vehicle, wherein the actions of the driver include an operation of a steering wheel of the vehicle, pedals of the vehicle, or both, wherein the current path starts at a current position and orientation of the vehicle and ends in a target position and orientation of the vehicle and includes a sequence of points, wherein each point represents coordinates of position of the vehicle and an orientation of the vehicle;
modifying the current path for the vehicle while preserving the current position and the target position of the vehicle in a modified path; and
overriding the actions of the driver to control a movement of the vehicle according to the modified path, wherein steps of the vehicle are performed using a processor of the vehicle, wherein the modifying and the overriding steps are performed in response to detecting an obstacle on the current path of the vehicle, and wherein the modifying comprises:
determining a coarse path defined by a first set of nodes with coarse separation connecting a node representing the current position and orientation with a node representing the target position and orientation, wherein each node represents values of the position and the orientation of the vehicle;
refining the coarse path to produce a refined path formed by a second set of nodes with fine separation, such that the refined path is close to the determined coarse path;
removing at least one redundant node of the refined path that increase the value of a cost function without being useful for avoiding obstacles to produce a pruned path, wherein the cost function balances a combination of a deviation of the modified path from the current path and a difference between a curvature of the modified path and a curvature of the current path; and
smoothing a trajectory connecting the nodes of the pruned path to produce the modified path.

2. The method of claim 1 , wherein the modifying comprises:
determining a set of paths from the current position to the target position of the vehicle, wherein each path avoids the obstacle while satisfying driving constraints;
determining, for each path in the set of paths, a metric of performance as a function of a difference between the current path and each path in the set of paths; and
selecting a path from the set of paths as the modified path according to the metric of performance of the path.

3. The method of claim 2 , wherein the metric of performance is
a total position difference between the current path and each path in the set of paths, such that the path selected as the modified path has the metric of performance
corresponding to a minimal total position difference.

4. The method of claim 2 , wherein the metric of performance of the path is a cost function balancing a combination of a deviation of the path from the current path and a difference between a curvature of the path and the curvature of the current path, such that the path selected as the modified path has the metric of performance corresponding to a minimal value of the cost function.

5. The method of claim 1 , wherein the modifying comprises:
determining the modified path by optimizing the cost function reducing the deviation of the modified path avoiding the obstacle from the current path.

6. The method of claim 1 , wherein the modifying comprises:
determining the modified path avoiding the obstacle by optimizing the cost function balancing the combination of the deviation of the modified path from the current path and the difference between the curvature of the modified path and the curvature of the current path.

7. The method of claim 1 , wherein the determining of the coarse path comprises:
generating a tree of nodes such that links connecting the nodes form a set of coarse paths connecting the current position with the target position;
placing a new node in the tree if a coarse path passing through the new node is more optimal according to the cost function than another coarse path not passing through the new node.

8. The method of claim 7 wherein the generating the tree of nodes comprises:
sampling a random node in a drivable space of the vehicle;
determining the new node on a path connecting the random node and the nearest node from the tree of nodes;
detecting a collision with obstacles along the path between the new node and the nearest node; and when the collision is not detected
adding the new node to an existing node in the tree of nodes that reduces a cost of reaching the new node from a root of the tree of nodes, wherein the cost includes a sum of a cost to reach the existing tree node from the root node and a cost of reaching the new node from the existing tree node determined according to the cost function.

9. The method in claim 8 , further comprising in response to adding the new node to the tree of nodes:
replacing an old link of reaching a neighboring node with a new link of reaching the neighboring node from the new node, when the cost of reaching the neighboring node through new link is less that a cost of reaching the neighboring node through the old link.

10. The method of claim 1 , wherein the refining comprises:
placing new nodes with higher density than those in the coarse path in an area centered around the coarse path.

11. The method of claim 1 , further comprising:
determining the set of points of the coarse path backwards from the target position to the current position; and
updating the set of nodes of the coarse path corresponding to the modified path forwards from the current position to the target position in response to a change of the current position of the vehicle while preserving a portion of the modified path approaching the target position.

12. A method for controlling a semi-autonomous vehicle, comprising:
determining a current path for the vehicle in response to actions received from a driver of the vehicle, wherein the actions of the driver include an operation of a steering wheel of the vehicle, pedals of the vehicle, or both, wherein the current path starts at a current position and orientation of the vehicle and ends in a target position and orientation of the vehicle and includes a sequence of points, wherein each point represents coordinates of position of the vehicle and an orientation of the vehicle;
modifying the current path for the vehicle, in response to detecting an obstacle on the current path of the vehicle, to produce a modified path avoiding the obstacle, wherein the modifying includes optimizing a cost function of a deviation of the modified path from the current path; and
overriding the actions of the driver to control a movement of the vehicle according to the modified path, wherein steps of the vehicle are performed using a processor of the vehicle, wherein the modifying and the overriding steps are performed in response to detecting an obstacle on the current path of the vehicle, and wherein the modifying comprises:
determining a coarse path defined by a first set of nodes with coarse separation connecting a node representing the current position and orientation with a node representing the target position and orientation, wherein each node represents values of the position and the orientation of the vehicle;
refining the coarse path to produce a refined path formed by a second set of nodes with fine separation, such that the refined path is close to the determined coarse path;
removing at least one redundant node of the refined path that increase the value of a cost function without being useful for avoiding ob

Assignees

• Mitsubishi Electric Res Laboratories Inc

Inventors

• Di Cairano Stefano
• Lan Xiaodong

Classifications

• G01C21/3453

  Special cost functions, i.e. other than distance or default speed limit of road segments · CPC title

• B60W30/09 Primary

  Taking automatic action to avoid collision, e.g. braking and steering · CPC title

• B60W50/0097

  Predicting future conditions · CPC title

• G01C21/3407

  specially adapted for specific applications · CPC title

• G01C21/34

  Route searching; Route guidance · CPC title