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

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

• receiving a host vehicle heading HHV, a host vehicle speed SHV, a host vehicle location LHV, environmental information E, and a respective obstacle heading HVO, obstacle speed SVO and obstacle location LVO for each obstacle within a predetermined radius about the host vehicle;
• calculating one or more projected host vehicle locations Li using Li=fk1(i, LHV, HHV, HDEV, SHV, SDEV, TH, E, G), where fk1 is a first kinematic function for generating the one or more projected host vehicle locations Li, i is a step number, HDEV and SDEV are allowable deviations for the host vehicle heading HHV and the host vehicle speed SHV, respectively, TH is a hypothesis time step, and G is a goal;
• computing a respective projected obstacle location Oi for each obstacle using Oi=fk2(i, LVO, HVO, SVO, TH, E), where fk2 is a second kinematic function for generating the respective projected obstacle location Oi for each obstacle;
• determining a respective collision potential between each projected host vehicle location Li and each projected obstacle location Oi, wherein each collision potential is associated with a respective projected host vehicle location Li and wherein the collision potential is defined by CP=1−{1/[1+e{circumflex over ( )}−(d−DSAFE/2)]}, where CP is the collision potential, d is a respective distance between each projected host vehicle location Li and each projected obstacle location Oi, and DSAFE is a respective predetermined safe threshold distance;
• and
• until a predetermined maximum number of steps imax is reached, and while at least one projected host vehicle location Li is a qualified projected host vehicle location Li defined as a projected host vehicle location Li 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 Li to generate one or more additional projected host vehicle locations Li where the respective qualified projected host vehicle location is used for LHV,
• repeating the computing step to generate an additional projected obstacle location Oi for each respective obstacle where the current projected obstacle location for each respective obstacle is used for LVO, and
• repeating the determining step between each additional projected host vehicle location Li and each additional projected obstacle location Oi.

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

• receiving a host vehicle heading HHV, a host vehicle speed SHV, a host vehicle location LHV, environmental information E, and a respective obstacle heading HVO, obstacle speed SVO and obstacle location LVO for each obstacle within a predetermined radius about the host vehicle;
• calculating one or more projected host vehicle locations Li using Li=fk1(i, LHV, HHV, HDEV, SHV, SDEV, TH, E, G), where fk1 is a first kinematic function for generating the one or more projected host vehicle locations Li, i is a step number, HDEV and SDEV are allowable deviations for the host vehicle heading HHV and the host vehicle speed SHV, respectively, TH is a hypothesis time step, and G is a goal;
• computing a respective projected obstacle location Oi for each obstacle using Oi=fk2(i, LVO, HVO, SVO, TH, E), where fk2 is a second kinematic function for generating the respective projected obstacle location Oi for each obstacle;
• determining a respective collision potential between each projected host vehicle location Li and each projected obstacle location Oi, wherein each collision potential is associated with a respective projected host vehicle location Li;
• and
• until a predetermined maximum number of steps imax is reached, and while at least one projected host vehicle location Li is a qualified projected host vehicle location Li defined as a projected host vehicle location Li 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 Li to generate one or more additional projected host vehicle locations Li where the respective qualified projected host vehicle location is used for LHV,
• repeating the computing step to generate an additional projected obstacle location Oi for each respective obstacle where the current projected obstacle location for each respective obstacle is used for LVO, and
• repeating the determining step between each additional projected host vehicle location Li and each additional projected obstacle location Oi.