System, method and controller for graph-based path planning for a host vehicle

1. A method of path planning for a host vehicle, comprising:

receiving a host vehicle heading H HV , a host vehicle speed S HV , a host vehicle location L HV , environmental information E, and a respective obstacle heading H VO , obstacle speed S VO and obstacle location L VO for each obstacle within a predetermined radius about the host vehicle;

calculating one or more projected host vehicle locations L i using L i =f k1 (i, L HV , H HV , H DEV , S HV , S DEV , T H , E, G), where f k1 is a first kinematic function for generating the one or more projected host vehicle locations L i , i is a step number, H DEV and S DEV are allowable deviations for the host vehicle heading H HV and the host vehicle speed S HV , respectively, T H is a hypothesis time step, and G is a goal;

computing a respective projected obstacle location O i for each obstacle using O i =f k2 (i, L VO , H VO , S VO , T H , E), where f k2 is a second kinematic function for generating the respective projected obstacle location O i for each obstacle;

determining a respective collision potential between each projected host vehicle location L i and each projected obstacle location O i , wherein each collision potential is associated with a respective projected host vehicle location L i ;

until a predetermined maximum number of steps i max , is reached, and while at least one projected host vehicle location L i is a qualified projected host vehicle location L i defined as a projected host vehicle location L i in which each of its associated collision potentials is below a collision threshold, repeating the steps of:

incrementing the step number i;

repeating the calculating step for each respective qualified projected host vehicle location L i to generate one or more additional projected host vehicle locations L i where the respective qualified projected host vehicle location is used for L HV ,

repeating the computing step to generate an additional projected obstacle location O i for each respective obstacle where the current projected obstacle location for each respective obstacle is used for L VO , and

repeating the determining step between each additional projected host vehicle location L i and each additional projected obstacle location O i ; and

controlling at least one of a steering subsystem, a throttle subsystem and a braking subsystem of the host vehicle based on an optimum sequence from the host vehicle location L HV through one or more of the qualified projected host vehicle locations.

2. A method according to claim 1 , further comprising:

ignoring duplicate projected host vehicle locations L i .

3. A method according to claim 1 , further comprising:

selecting the optimum sequence based on minimizing an aggregate collision potential using a Dijkstra algorithm.

4. A method according to claim 1 , wherein the collision potential is defined by CP=1−{1/[1+e{circumflex over ( )}−(d−D SAFE /2)]}, where CP is the collision potential, d is a respective distance between each projected host vehicle location L i and each projected obstacle location O i , and D SAFE is a respective predetermined safe threshold distance.

5. Method according to claim 4 , wherein at least one correction factor relating to environmental information E is applied to the respective predetermined safe threshold distance D SAFE .

6. A method according to claim 1 , wherein the respective obstacle speed S VO and obstacle heading H VO for each obstacle are maintained for all steps.

7. A method according to claim 1 , further comprising, as part of the calculating step:

adjusting the host vehicle speed S HV by up to S DEV at each of the one or more projected host vehicle locations according to one or more predetermined velocity profiles.

8. A method according to claim 7 , wherein each of the predetermined velocity profiles is one or more of an acceleration profile, a deceleration profile, a constant speed profile, a coasting profile, a regenerative braking profile, an emergency acceleration profile and emergency stop profile.

9. A method according to claim 1 , wherein the predetermined radius is a predetermined span of zones about the host vehicle.

10. Method according to claim 1 , wherein each obstacle is a neighboring vehicle or object.

11. A method according to claim 1 , wherein:

the environmental information E includes one or more of: number and arrangement of lanes and shoulders; traffic flow direction for each lane and shoulder; road surface type of each lane and shoulder; location, message/command and effective area of traffic signs/traffic signals; speed limits; local weather conditions; and proximity, speed and heading of each obstacle; and

the goal G includes one or more of: going straight, turning left, turning right, accelerating, decelerating, maintaining a current speed, maintaining a vehicle speed at or above a minimum safe speed, maintaining the vehicle speed at or below a maximum safe speed, maintaining a minimum following distance or more behind another vehicle, merging into an adjacent lane, passing another vehicle, avoiding a passing lane when not passing, avoiding one or both shoulders, stopping at a designated location, and taking evasive action to avoid obstacles.

12. A method of graph-based path planning for a host vehicle, comprising:

receiving a host vehicle heading H HV , a host vehicle speed S HV , a host vehicle location L HV , environmental information E, and a respective obstacle heading H VO , obstacle speed S VO and obstacle location L VO for each obstacle within a predetermined radius about the host vehicle;

calculating one or more projected host vehicle locations L i using L i =f k1 (i, L HV , H HV , H DEV , S HV , S DEV , T H , E, G), where f k1 is a first kinematic function for generating the one or more projected host vehicle locations L i , i is a step number, H DEV and S DEV are allowable deviations for the host vehicle heading H HV and the host vehicle speed S HV , respectively, T H is a hypothesis time step, and G is a goal;

computing a respective projected obstacle location O i for each obstacle using O i =f k2 (i, L VO , H VO , S VO , T H , E), where f k2 is a second kinematic function for generating the respective projected obstacle location O i for each obstacle;

determining a respective collision potential between each projected host vehicle location L i and each projected obstacle location O i , wherein each collision potential is associated with a respective projected host vehicle location L i and wherein the collision potential is defined by CP=1−{1/[1+e{circumflex over ( )}−(d−D SAFE /2)]}, where CP is the collision potential, d is a respective distance between each projected host vehicle location L i and each projected obstacle location O i , and D SAFE is a respective predetermined safe threshold distance;

until a predetermined maximum number of steps i max is reached, and while at least one projected host vehicle location L i is a qualified projected host vehicle location L i defined as a projected host vehicle location L i in which each of its associated collision potentials is below a collision threshold, repeating the steps of:

incrementing the step number i;

repeating the calculating step for each respective qualified projected host vehicle location L i to generate one or more additional projected host vehicle locations L i where the respective qualified projected host vehicle location is used for L HV ,

repeating the computing step to generate an additional projected obstacle location O i for each respective obstacle where the current projected obstacle location for each respective obstacle is used for L VO , and

repeating the determining step between each additional projected host vehicle location L i and each additional projected obstacle location O i ; and

controlling at least one of a steering subsystem, a throttle subsystem and a braking subsystem of the host vehicle based on an optimum sequence from the host vehicle location L HV through one or more of the qualified projected host vehicle locations.

13. A method according to claim 12 , further comprising:

ignoring duplicate projected host vehicle locations L i .

14. A method according to claim 12 , further comprising:

selecting the optimum sequence based on minimizing an aggregate collision potential using a Dijkstra algorithm.

15. A method according to claim 12 , wherein at least one correction factor relating to environmental information E is applied to the respective predetermined safe threshold distance D SAFE .

16. A method according to claim 12 , further comprising, as part of the calculating step:

adjusting the host vehicle speed S HV by up to S DEV at each of the one or more projected host vehicle locations according to one or more predetermined velocity profiles.

17. A controller for planning a path for a host vehicle, the controller configured to execute the steps of:

receiving a host vehicle heading H HV , a host vehicle speed S HV , a host vehicle location L HV , environmental information E, and a respective obstacle heading H VO , obstacle speed S VO and obstacle location L VO for each obstacle within a predetermined radius about the host vehicle;

calculating one or more projected host vehicle locations L i using L i =f k1 (i, L HV , H HV , H DEV , S HV , S DEV , T H , E, G), where f k1 is a first kinematic function for generating the one or more projected host vehicle locations L i , i is a step number, H DEV and S DEV are allowable deviations for the host vehicle heading H HV and the host vehicle speed S HV , respectively, T H is a hypothesis time step, and G is a goal;

computing a respective projected obstacle location O i for each obstacle using O i =f k2 (i, L VO , H VO , S VO , T H , E), where f k2 is a second kinematic function for generating the respective projected obstacle location O i for each obstacle;

determining a respective collision potential between each projected host vehicle location L i and each projected obstacle location O i , wherein each collision potential is associated with a respective projected host vehicle location L i ;

until a predetermined maximum number of steps i max is reached, and while at least one projected host vehicle location L i is a qualified projected host vehicle location L i defined as a projected host vehicle location L i in which each of its associated collision potentials is below a collision threshold, repeating the steps of:

incrementing the step number i;

repeating the calculating step for each respective qualified projected host vehicle location L i to generate one or more additional projected host vehicle locations L i where the respective qualified projected host vehicle location is used for L HV ,

repeating the computing step to generate an additional projected obstacle location O i for each respective obstacle where the current projected obstacle location for each respective obstacle is used for L VO , and

repeating the determining step between each additional projected host vehicle location L i and each additional projected obstacle location O i ; and

controlling at least one of a steering subsystem, a throttle subsystem and a braking subsystem of the host vehicle based on an optimum sequence from the host vehicle location L HV through one or more of the qualified projected host vehicle locations.

18. A controller according to claim 17 , wherein the collision potential is defined by CP=1−{1/[1+e{circumflex over ( )}−(d−D SAFE /2)]}, where CP is the collision potential, d is a respective distance between each projected host vehicle location L i and each projected obstacle location O i , and D SAFE is a respective predetermined safe threshold distance.

19. A controller according to claim 17 , wherein the controller is further configured to execute the step of:

selecting the optimum sequence based on minimizing an aggregate collision potential using a Dijkstra algorithm.

20. A controller according to claim 17 , wherein the controller is further configured to execute, as part of the calculating step, the step of:

adjusting the host vehicle speed S HV by up to S DEV at each of the one or more projected host vehicle locations according to one or more predetermined velocity profiles.