Path planning system and method for a vehicle
A method for path planning for a vehicle may include determining a repulsive potential at each of a plurality of location points in an environment surrounding the vehicle using a vehicle perception sensor. The method further may include determining an attractive potential at each of the plurality of location points in the environment surrounding the vehicle using the vehicle perception sensor. The method further may include calculating a potential field representing the environment surrounding the vehicle based at least in part on the attractive potential at each of the plurality of location points and the repulsive potential at each of the plurality of location points. The potential field quantifies a suitability of each of the plurality of location points in the environment for inclusion in a path for the vehicle. The method further may include generating the path for the vehicle based at least in part on the potential field.
1 . A method for path planning for a vehicle, the method comprising:
determining a repulsive potential at each of a plurality of location points in an environment surrounding the vehicle using a vehicle perception sensor;
determining an attractive potential at each of the plurality of location points in the environment surrounding the vehicle using the vehicle perception sensor;
calculating a potential field representing the environment surrounding the vehicle based at least in part on the attractive potential at each of the plurality of location points and the repulsive potential at each of the plurality of location points, wherein the potential field quantifies a suitability of each of the plurality of location points in the environment for inclusion in a path for the vehicle, wherein calculating the potential field further comprises:
determining a repulsive force at each of the plurality of location points based at least in part on the repulsive potential at each of the plurality of location points;
determining an attractive force at each of the plurality of location points based at least in part on the attractive potential at each of the plurality of location points; and
calculating the potential field based at least in part on the repulsive force at each of the plurality of location points and the attractive force at each of the plurality of location points, wherein a value of the potential field at each of the plurality of location points is equal to a sum of the repulsive force at each of the plurality of location points and the attractive force at each of the plurality of location points, and wherein the potential field at each of the plurality of location points is defined by a potential field function:
F
(
s
)
=
{
∑
{
k
r
(
1
ρ
i
-
1
ρ
0
,
i
)
1
ρ
i
2
s
-
s
0
ρ
i
if
ρ
i
≤
ρ
0
,
i
0
if
ρ
i
>
ρ
0
,
i
}
+
{
-
2
k
a
(
s
-
s
d
)
if
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
≤
d
a
-
2
k
a
d
a
s
-
s
d
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
if
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
>
d
a
}
wherein F(s) is the potential field function, s is a vector describing a location of one of the plurality of location points, k r is a predetermined repulsive constant, ρ i is a distance between the location of the one of the plurality of location points and an ith obstacle of a plurality of obstacles, ρ 0,i is a minimum allowed distance between the vehicle and the ith obstacle of the plurality of obstacles, s 0 is a vector describing a location of one of the plurality of obstacles which is closest to the one of the plurality of location points s, a summation operator Σ indicates a summation over each of the plurality of obstacles, k a is a predetermined attractive constant, s d is a vector describing a goal location, and d a is a predetermined attractive threshold; and
generating the path for the vehicle based at least in part on the potential field.
2 . The method of claim 1 , wherein determining the repulsive potential at each of the plurality of location points further comprises:
detecting a plurality of obstacles using the vehicle perception sensor;
measuring a distance between each of the plurality of location points and each of the plurality of obstacles using the vehicle perception sensor; and
calculating the repulsive potential at each of the plurality of location points based at least in part on the distance between each of the plurality of location points and each of the plurality of obstacles.
3 . The method of claim 2 , wherein detecting the plurality of obstacles further comprises:
detecting the plurality of obstacles using the vehicle perception sensor, wherein at least one of the plurality of obstacles is a marker, barrier, or road sign indicating a construction zone.
4 . The method of claim 2 , wherein calculating the repulsive potential at each of the plurality of location points further comprises:
calculating the repulsive potential at each of the plurality of location points, wherein the repulsive potential at each of the plurality of location points is defined by a repulsive potential function:
U
rep
(
s
)
=
∑
{
1
2
k
r
(
1
ρ
i
-
1
ρ
0
,
i
)
2
if
ρ
i
≤
ρ
0
,
i
0
if
ρ
i
>
ρ
0
,
i
wherein U rep (s) is the repulsive potential function, s is a vector describing a location of one of the plurality of location points, k r is a predetermined repulsive constant, ρ i is a distance between the location of the one of the plurality of location points and an ith obstacle of the plurality of obstacles, ρ 0,i is a minimum allowed distance between the vehicle and the ith obstacle of the plurality of obstacles, and a summation operator Σ indicates a summation over each of the plurality of obstacles.
5 . The method of claim 1 , wherein determining the attractive potential at each of the plurality of location points further comprises:
determining a goal location in the environment;
determining a distance between each of the plurality of location points and the goal location; and
calculating the attractive potential at each of the plurality of location points based at least in part on the distance between each of the plurality of location points and the goal location.
6 . The method of claim 5 , wherein calculating the attractive potential at each of the plurality of location points further comprises:
calculating the attractive potential at each of the plurality of location points, wherein the attractive potential at each of the plurality of location points is defined by an attractive potential function:
U
att
(
s
)
=
{
k
a
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
2
if
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
≤
d
a
k
a
(
2
d
a
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
-
d
a
2
)
if
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
>
d
a
wherein U att (s) is the attractive potential function, s is a vector describing a location of one of the plurality of location points, k a is a predetermined attractive constant, S d is a vector describing the goal location, and d a is a predetermined attractive threshold.
7 . The method of claim 1 , wherein generating the path for the vehicle further comprises:
generating a plurality of candidate points, wherein each of the plurality of candidate points describes a possible location of the vehicle after driving for a predetermined length of time;
determining a plurality of feasible candidate points, wherein the plurality of feasible candidate points includes a subset of the plurality of candidate points, and wherein a value of the potential field at each of the plurality of feasible candidate points is less than or equal to a predetermined potential field value threshold;
determining a plurality of optimal feasible candidate points, wherein the plurality of optimal feasible candidate points includes a subset of the plurality of feasible candidate points, wherein the plurality of optimal feasible candidate points is determined using an optimization algorithm, and wherein the plurality of optimal feasible candidate points is determined based at least in part on a value of the potential field at each of the plurality of feasible candidate points; and
generating the path for the vehicle based at least in part on the plurality of optimal feasible candidate points, wherein the path for the vehicle includes at least the plurality of optimal feasible candidate points.
8 . The method of claim 7 , wherein determining the plurality of optimal feasible candidate points further comprises:
determining the plurality of optimal feasible candidate points such as to minimize a cost function, wherein the cost function is determined based at least in part on the potential field function.
9 . A system for path planning for a vehicle, the system comprising:
a vehicle perception sensor;
a controller in electrical communication with the vehicle perception sensor, wherein the controller is programmed to:
determine a repulsive potential at each of a plurality of location points in an environment surrounding the vehicle using the vehicle perception sensor, wherein to determine the repulsive potential, the controller is further programmed to:
detect a plurality of obstacles using the vehicle perception sensor, wherein at least one of the plurality of obstacles is a marker, barrier, or road sign indicating a construction zone;
measure a distance between each of the plurality of location points and each of the plurality of obstacles using the vehicle perception sensor; and
calculate the repulsive potential at each of the plurality of location points, wherein the repulsive potential at each of the plurality of location points is defined by a repulsive potential function:
U
rep
(
s
)
=
∑
{
1
2
k
r
(
1
ρ
i
-
1
ρ
0
,
i
)
2
if
ρ
i
≤
ρ
0
,
i
0
if
ρ
i
>
ρ
0
,
i
wherein U rep (s) is the repulsive potential function, s is a vector describing a location of one of the plurality of location points, k r is a predetermined repulsive constant, ρ i is a distance between the location of the one of the plurality of location points and an ith obstacle of the plurality of obstacles, ρ 0,i is a minimum allowed distance between the vehicle and the ith obstacle of the plurality of obstacles, and a summation operator Σ indicates a summation over each of the plurality of obstacles;
determine an attractive potential at each of the plurality of location points in the environment surrounding the vehicle using the vehicle perception sensor, wherein to determine the attractive potential, the controller is further programmed to:
determine a goal location in the environment;
determine a distance between each of the plurality of location points and the goal location; and
calculate the attractive potential at each of the plurality of location points, wherein the attractive potential at each of the plurality of location points is defined by an attractive potential function:
U
att
(
s
)
=
{
k
a
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
2
if
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
≤
d
a
k
a
(
2
d
a
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
-
d
a
2
)
if
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
>
d
a
wherein U att (s) is the attractive potential function, s is the vector describing a location of one of the plurality of location points, k a is a predetermined attractive constant, s d is a vector describing the goal location, and d a is a predetermined attractive threshold;
calculate a potential field representing the environment surrounding the vehicle based at least in part on the attractive potential at each of the plurality of location points and the repulsive potential at each of the plurality of location points, wherein the potential field quantifies a suitability of each of the plurality of location points in the environment for inclusion in a path for the vehicle; and
generate the path for the vehicle based at least in part on the potential field.
10 . The system of claim 9 , wherein to calculate the potential field, the controller is further programmed to:
determine a repulsive force at each of the plurality of location points based at least in part on the repulsive potential at each of the plurality of location points;
determine an attractive force at each of the plurality of location points based at least in part on the attractive potential at each of the plurality of location points; and
calculate the potential field based at least in part on the repulsive force at each of the plurality of location points and the attractive force at each of the plurality of location points, wherein a value of the potential field at each of the plurality of location points is equal to a sum of the repulsive force at each of the plurality of location points and the attractive force at each of the plurality of location points, and wherein the potential field at each of the plurality of location points is defined by a potential field function:
F
(
s
)
=
F
r
(
s
)
+
F
a
(
s
)
wherein F(s) is the potential field function, F r (s) is a repulsive force function, and F a (s) is an attractive force function.
11 . The system of claim 10 , wherein to determine the repulsive force, the controller is further programmed to:
determine the repulsive force at each of the plurality of location points, wherein the repulsive force at each of the plurality of location points is defined by the repulsive force function:
F
r
(
s
)
=
-
∇
U
rep
(
s
)
=
∑
{
k
r
(
1
ρ
i
-
1
ρ
0
,
i
)
1
ρ
i
2
s
-
s
0
ρ
i
if
ρ
i
≤
ρ
0
,
i
0
if
ρ
i
>
ρ
0
,
i
wherein F r (s) is the repulsive force function, ∇ is a gradient operator, s is the vector describing a location of one of the plurality of location points, k r is the predetermined repulsive constant, ρ i is the distance between the location of the one of the plurality of location points and the ith obstacle of a plurality of obstacles, ρ 0,i is the minimum allowed distance between the vehicle and the ith obstacle of the plurality of obstacles, s 0 is the vector describing a location of one of the plurality of obstacles which is closest to the one of the plurality of location points s, and the summation operator Σ indicates a summation over each of the plurality of obstacles.
12 . The system of claim 11 , wherein to determine the attractive force, the controller is further programmed to:
determine the attractive force at each of the plurality of location points, wherein the attractive force at each of the plurality of location points is defined by the attractive force function:
F
a
(
s
)
=
-
∇
U
att
(
s
)
=
{
-
2
k
a
(
s
-
s
d
)
if
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
≤
d
a
-
2
k
a
d
a
s
-
s
d
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
if
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
>
d
a
wherein F a (s) is the attractive force function, ∇ is the gradient operator, s is the vector describing a location of one of the plurality of location points, k a is the predetermined attractive constant, s d is the vector describing the goal location, and d a is the predetermined attractive threshold.
13 . The system of claim 12 , wherein to generate the path for the vehicle, the controller is further programmed to:
generate a plurality of candidate points, wherein each of the plurality of candidate points describes a possible location of the vehicle after driving for a predetermined length of time;
determine a plurality of feasible candidate points, wherein the plurality of feasible candidate points includes a subset of the plurality of candidate points, and wherein a value of the potential field at each of the plurality of feasible candidate points is less than or equal to a predetermined potential field value threshold;
determine a plurality of optimal feasible candidate points, wherein the plurality of optimal feasible candidate points includes a subset of the plurality of feasible candidate points, wherein the plurality of optimal feasible candidate points is determined using an optimization algorithm, wherein the plurality of optimal feasible candidate points is determined based at least in part on a value of the potential field at each of the plurality of feasible candidate points, wherein the plurality of optimal feasible candidate points is determined such as to minimize a cost function, and wherein the cost function is determined based at least in part on the potential field function; and
generate the path for the vehicle based at least in part on the plurality of optimal feasible candidate points, wherein the path for the vehicle includes at least the plurality of optimal feasible candidate points.
14 . A method for path planning for a vehicle, the method comprising:
detecting a plurality of obstacles using a vehicle perception sensor;
measuring a distance between each of a plurality of location points and each of a plurality of obstacles in an environment surrounding the vehicle using the vehicle perception sensor;
calculating a repulsive potential at each of the plurality of location points based at least in part on the distance between each of the plurality of location points and each of the plurality of obstacles;
determining a goal location in the environment;
determining a distance between each of the plurality of location points and the goal location;
calculating an attractive potential at each of the plurality of location points based at least in part on the distance between each of the plurality of location points and the goal location;
calculating a potential field representing the environment surrounding the vehicle based at least in part on the attractive potential at each of the plurality of location points and the repulsive potential at each of the plurality of location points, wherein the potential field quantifies a suitability of each of the plurality of location points in the environment for inclusion in a path for the vehicle, wherein calculating the potential field further comprises:
determining a repulsive force at each of the plurality of location points, wherein the repulsive force at each of the plurality of location points is defined by a repulsive force function:
F
r
(
s
)
=
-
∇
U
rep
(
s
)
=
∑
{
k
r
(
1
ρ
i
-
1
ρ
0
,
i
)
1
ρ
i
2
s
-
s
0
ρ
i
if
ρ
i
≤
ρ
0
,
i
0
if
ρ
i
>
ρ
0
,
i
wherein F r (s) is the repulsive force function, ∇ is a gradient operator, s is a vector describing a location of one of the plurality of location points, k r is a predetermined repulsive constant, ρ i is a distance between the location of the one of the plurality of location points and an ith obstacle of a plurality of obstacles, ρ 0,i is a minimum allowed distance between the vehicle and the ith obstacle of the plurality of obstacles, s 0 is a vector describing a location of one of the plurality of obstacles which is closest to the one of the plurality of location points s, and a summation operator Σ indicates a summation over each of the plurality of obstacles;
determining an attractive force at each of the plurality of location points, wherein the attractive force at each of the plurality of location points is defined by an attractive force function:
F
a
(
s
)
=
-
∇
U
att
(
s
)
=
{
-
2
k
a
(
s
-
s
d
)
if
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
≤
d
a
-
2
k
a
d
a
s
-
s
d
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
if
❘
"\[LeftBracketingBar]"
s
-
s
d
❘
"\[RightBracketingBar]"
>
d
a
wherein F a (s) is the attractive force function, ∇ is the gradient operator, s is the vector describing a location of one of the plurality of location points, k a is a predetermined attractive constant, s d is a vector describing a goal location, and d a is a predetermined attractive threshold;
calculating the potential field based at least in part on the repulsive force at each of the plurality of location points and the attractive force at each of the plurality of location points, wherein a value of the potential field at each of the plurality of location points is equal to a sum of the repulsive force at each of the plurality of location points and the attractive force at each of the plurality of location points, and wherein the potential field at each of the plurality of location points is defined by a potential field function:
F
(
s
)
=
F
r
(
s
)
+
F
a
(
s
)
wherein F(s) is the potential field function, F r (s) is the repulsive force function, and F a (s) is the attractive force function; and
generating the path for the vehicle based at least in part on the potential field.
15 . The method of claim 14 , wherein generating the path for the vehicle further comprises:
generating a plurality of candidate points, wherein each of the plurality of candidate points describes a possible location of the vehicle after driving for a predetermined length of time;
determining a plurality of feasible candidate points, wherein the plurality of feasible candidate points includes a subset of the plurality of candidate points, and wherein a value of the potential field function at each of the plurality of feasible candidate points is less than or equal to a predetermined potential field value threshold;
determining a plurality of optimal feasible candidate points, wherein the plurality of optimal feasible candidate points includes a subset of the plurality of feasible candidate points, wherein the plurality of optimal feasible candidate points is determined using an optimization algorithm, wherein the plurality of optimal feasible candidate points is determined based at least in part on a value of the potential field function at each of the plurality of feasible candidate points, wherein the plurality of optimal feasible candidate points is determined such as to minimize a cost function, and wherein the cost function is determined based at least in part on the potential field function; and
generating the path for the vehicle based at least in part on the plurality of optimal feasible candidate points, wherein the path for the vehicle includes at least the plurality of optimal feasible candidate points.
16 . The method of claim 5 , wherein determining the goal location further comprises determining the goal location based on a destination location in a navigation system of the vehicle.
17 . The method of claim 7 , wherein generating the path for the vehicle further comprises:
generating the path for the vehicle by linking the plurality of optimal feasible candidate points with a series of quintic spline curves.
18 . The system of claim 9 , wherein the vehicle perception sensor includes at least one of: a camera and a light detection and ranging (LiDAR) sensor.
19 . The system of claim 13 , wherein to determine the plurality of feasible candidate points, the controller is further programmed to:
determine the plurality of feasible candidate points by excluding candidate points which would require the vehicle to violate vehicle kinematic/dynamic constraints, wherein the vehicle kinematic/dynamic constraints include at least one of: a maximum longitudinal acceleration, a maximum lateral acceleration, a maximum yaw rate, and a maximum speed.
20 . The method of claim 14 , further comprising:
controlling at least one of a brake system, a propulsion system, and a steering system of the vehicle to drive the vehicle along the generated path.