Motion planning in curvilinear coordinates for autonomous vehicles
Methods for planning a trajectory for an autonomous vehicle are disclosed. A vehicle motion planning system will determine a reference curve that represents a path via which the vehicle may travel. The system will detect an actor that is moving in the environment. The system will segment the reference curve according to time intervals. For each of the time intervals, the system will identify a bounding geometry for the actor, predict a lateral offset distance between the reference curve and the actor, and use the predicted lateral offset distance to determine whether to alter a planned trajectory for the autonomous vehicle.
1. A method of planning a trajectory for an autonomous vehicle, the method comprising:
by a motion planning system of an autonomous vehicle:
determining a reference curve that represents a path via which the autonomous vehicle may travel in an environment over a time horizon;
receiving, from a perception system of the autonomous vehicle, data representative of an actor that is moving in the environment;
segmenting the time horizon into a plurality of time intervals;
for each of the time intervals:
defining a coarse bounding geometry for the actor over the time interval, wherein the coarse bounding geometry encompasses the actor and includes possible position of the actor during the time interval,
defining a coarse bounding geometry for the autonomous vehicle over the time interval, wherein the coarse bounding geometry encompasses the autonomous vehicle,
determining whether the coarse bounding geometry for the actor will intersect with the coarse bounding geometry of the autonomous vehicle over a distance interval on the reference curve,
subdividing the time interval into time subintervals in response to the coarse bounding geometry for the actor intersecting the coarse bounding geometry of the autonomous vehicle,
determining a plurality of fine bounding geometries for the actor over the time subintervals and a plurality of fine bounding geometries for the autonomous vehicle over distance subintervals, and
predicting a lateral offset distance between the reference curve and the actor based on the plurality of fine bounding geometries of the actor and the plurality of fine bounding geometries for the autonomous vehicle; and
using the predicted lateral offset distances to determine a trajectory for the autonomous vehicle.
2. The method of claim 1 further comprising, by the motion planning system:
storing the predicted lateral offset distances in a lookup table,
wherein using the predicted lateral offset distances to determine a trajectory for the autonomous vehicle comprises retrieving the predicted lateral offset distances from the lookup table when the moving actor is in proximity to the autonomous vehicle.
3. The method of claim 1 , wherein:
using the predicted lateral offset distance to determine a trajectory for the autonomous vehicle comprises determining, from the predicted lateral offset distance, one or more regions of configuration space for the autonomous vehicle that are occupied by the actor, and one or more regions of configuration space for the autonomous vehicle that are free of the actor;
the method further comprises determining whether a planned trajectory for the autonomous vehicle will conflict with any of the occupied regions of a configuration space; and
the method further comprises, when the planned trajectory for the autonomous vehicle will conflict with any of the occupied regions of the configuration space, altering the planned trajectory to be fully within a free configuration space for the autonomous vehicle and outside of an occupied configuration space for the autonomous vehicle.
4. The method of claim 1 , wherein using the predicted lateral offset distance to determine a trajectory for the autonomous vehicle comprises:
determining one or more regions of configuration space for the autonomous vehicle that are occupied and one or more regions of configuration space for the autonomous vehicle that are free; and
determining a planned trajectory that is fully within a region of free configuration space for the autonomous vehicle and outside of all regions of occupied configuration space for the autonomous vehicle.
5. The method of claim 1 , wherein
predicting the lateral offset distances further comprises determining lateral offset distances in each location where the fine bounding geometries of the actor and the autonomous vehicle intersect.
6. The method of claim 1 , further comprising:
for each of the time subintervals, determining a fine bounding geometry for the actor to obtain the plurality of fine bounding geometries for the actor;
determining a distance interval that corresponds to the time interval, and subdividing the distance interval on the reference curve into a plurality of distance subintervals;
for each of the distance subintervals, determining a fine bounding geometry for the autonomous vehicle; and
identifying a plurality of pairs of the time intervals and corresponding distance subintervals and, for each time subinterval and distance subinterval pair:
determining whether the fine bounding geometry for the actor intersects with the fine bounding geometry for the autonomous vehicle,
if the fine bounding geometry for the actor does not intersect with the fine bounding geometry for the autonomous vehicle, continuing to a next time subinterval, and
if the fine bounding geometry for the actor intersects with the fine bounding geometry for the autonomous vehicle, predicting the lateral offset to a bounding geometry of the actor over the time subinterval at sample locations on the reference curve within the distance subinterval.
7. The method of claim 1 , wherein determining the coarse bounding geometry for the autonomous vehicle further includes:
determining a bounding box for the autonomous vehicle; and
dilating the bounding box for the autonomous vehicle laterally by a threshold.
8. The method of claim 7 , wherein predicting the lateral offset distance between the autonomous vehicle and the moving actor comprises determining a distance between a bounding box for the autonomous vehicle and a convex hull of the actor.
9. The method of claim 1 , further comprises:
determining a maximum distance interval length to guarantee a detection range with respect to a reference point on the reference curve determining the distance interval δ detect =p−r turn , in which:
p is the diagonal of a parallelogram p=√{square root over (a 2 +ab 2 +2ab cos(Δθ))}, where a and b are sides of the parallelogram:
a
=
d
front
/
sin
(
Δθ
)
b
=
d
rear
/
sin
(
Δθ
)
d front and d rear are distances from the reference point to front and rear of the autonomous vehicle, respectively, and
r turn is the turn radius of the reference curve, calculated as r turn =Δs/Δθ;
for each of the distance subintervals, determining a fine bounding geometry for the autonomous vehicle; and
determining whether the fine bounding geometry for the actor intersects with the fine bounding geometry for the autonomous vehicle in any time subinterval and distance subinterval pair.
10. An autonomous vehicle, comprising:
a perception system comprising one or more sensors; and
a computing device comprising a processor and memory containing programming instructions that are configured to cause the computing device to act as a motion planning system by:
determining a reference curve that represents a path via which the autonomous vehicle may travel in an environment over a time horizon,
receiving, from the perception system, data representative of an actor that is moving in the environment,
segmenting the time horizon into a plurality of time intervals, and
for each of the time intervals:
define a coarse bounding geometry for the actor over the time interval, wherein the coarse bounding geometry encompasses the actor and includes possible position of the actor during the time interval,
define a coarse bounding geometry for the autonomous vehicle over the time interval, wherein the coarse bounding geometry encompasses the autonomous vehicle,
determine whether the coarse bounding geometry for the actor will intersect with the coarse bounding geometry of the autonomous vehicle over a distance interval on the reference curve,
subdivide the time interval into time subintervals in response to the coarse bounding geometry for the actor intersecting the coarse bounding geometry of the autonomous vehicle,
determine a plurality of fine bounding geometries for the actor over the time subintervals and a plurality of fine bounding geometries for the autonomous vehicle over distance subintervals, and
predict a lateral offset distance between the reference curve and the actor based on the plurality of fine bounding geometries of the actor and the plurality of fine bounding geometries of the autonomous vehicle; and
use the predicted lateral offset distances to determine a trajectory for the autonomous vehicle.
11. The autonomous vehicle of claim 10 further comprising additional instructions that are configured to cause the motion planning system to store the predicted lateral offset distances in a lookup table,
wherein the instructions to use the predicted lateral offset distances to determine a trajectory for the autonomous vehicle comprise instructions to retrieve the predicted lateral offset distances from the lookup table when the moving actor is in proximity to the autonomous vehicle.
12. The autonomous vehicle of claim 10 , wherein:
the instructions to use the predicted lateral offset distance to determine a trajectory for the autonomous vehicle comprise instructions to determine, from the predicted lateral offset distance, one or more regions of configuration space for the autonomous vehicle that are occupied by the actor, and one or more regions of configuration space for the autonomous vehicle that are free of the actor; and
the memory also comprises instructions to:
determine whether a planned trajectory for the autonomous vehicle will conflict with any of the occupied regions of a configuration space, and
when the planned trajectory for the autonomous vehicle will conflict with any of the occupied regions of the configuration space, alter the planned trajectory to be fully within a free configuration space for the autonomous vehicle and outside of an occupied configuration space for the autonomous vehicle.
13. The autonomous vehicle of claim 10 , wherein the instructions to use the predicted lateral offset distance to determine a trajectory for the autonomous vehicle comprise instructions to:
determine one or more regions of configuration space for the autonomous vehicle that are occupied and one or more regions of configuration space for the autonomous vehicle that are free; and
determine a planned trajectory that is fully within a region of free configuration space for the autonomous vehicle and outside of all regions of occupied configuration space for the autonomous vehicle.
14. The autonomous vehicle of claim 10 , wherein the instructions to predict the lateral offset distances further comprises
determine lateral offset distances in each location where the fine bounding geometries of the actor and the autonomous vehicle intersect.
15. The autonomous vehicle of claim 10 , wherein the instructions further comprises:
for each of the time subintervals, determine a fine bounding geometry for the actor to obtain the plurality of fine bounding geometries for the actor;
determine a distance interval that corresponds to the time interval, and subdivide the distance interval on the reference curve into a plurality of distance subintervals;
for each of the distance subintervals, determine a fine bounding geometry for the autonomous vehicle; and
identify a plurality of pairs of the time intervals and corresponding distance subintervals and, for each time subinterval and distance subinterval pair:
determine whether the fine bounding geometry for the actor intersects with the fine bounding geometry for the autonomous vehicle,
if the fine bounding geometry for the actor does not intersect with the fine bounding geometry for the autonomous vehicle, continue to a next time subinterval, and
if the fine bounding geometry for the actor intersects with the fine bounding geometry for the autonomous vehicle, predict the lateral offset to a bounding geometry of the actor over the time subinterval at sample locations on the reference curve within the distance subinterval.
16. The autonomous vehicle of claim 10 , wherein the instructions to determine the coarse bounding geometry for the autonomous vehicle further includes:
determining a bounding box for the autonomous vehicle; and
dilating the bounding box for the autonomous vehicle laterally by a threshold.
17. The autonomous vehicle of claim 16 , wherein the instructions to predict the lateral offset distance between the autonomous vehicle and the actor comprise instructions to determine a distance between a bounding box for the autonomous vehicle and a convex hull of the moving actor.
18. The autonomous vehicle of claim 10 wherein the instructions further include:
determine a maximum distance interval length to guarantee a detection range with respect to a reference point on the reference curve determining the distance interval δ detect =p−r turn , in which:
p is the diagonal of a parallelogram p=√{square root over (a 2 +ab 2 +2ab cos(Δθ))}, where a and h are sides of the parallelogram:
a
=
d
front
/
sin
(
Δθ
)
b
=
d
rear
/
sin
(
Δθ
)
d front and d rear are distances from the reference point to front and rear of the autonomous vehicle, respectively, and
r turn is the turn radius of the reference curve, calculated as r turn =Δs/Δθ;
for each of the distance subintervals, determine a fine bounding geometry for the autonomous vehicle; and
determine whether the fine bounding geometry for the actor intersects with the fine bounding geometry for the autonomous vehicle in any time subinterval and distance subinterval pair.
19. A method of planning a trajectory for an autonomous vehicle, the method comprising:
determining a reference curve that represents a path via which the autonomous vehicle may travel in an environment over a time horizon,
receiving, from a perception system of the autonomous vehicle, data representative of an actor that is moving in the environment,
segmenting the time horizon into a plurality of time intervals, and
for each of the time intervals:
defining a coarse bounding geometry for the actor over the time interval, wherein the coarse bounding geometry encompasses the actor and includes possible position of the actor during the time interval,
defining a coarse bounding geometry for the autonomous vehicle over the time interval, wherein the coarse bounding geometry encompasses the autonomous vehicle,
determining whether the coarse bounding geometry for the actor will intersect with the coarse bounding geometry of the autonomous vehicle over a distance interval on the reference curve,
subdividing the time interval into time subintervals in response to the coarse bounding geometry for the actor intersecting the coarse bounding geometry of the autonomous vehicle,
determining a plurality of fine bounding geometries for the actor over the time subintervals and a plurality of fine bounding geometries for the autonomous vehicle over distance subintervals, and
predict a lateral offset distance between the reference curve and the actor based on the plurality of fine bounding geometries of the actor and the plurality of fine bounding geometries of the autonomous vehicle; and
use the predicted lateral offset distances to determine a trajectory for the autonomous vehicle by:
determining one or more regions of configuration space for the autonomous vehicle that are occupied and one or more regions of configuration space for the autonomous vehicle that are free; and
determining a planned trajectory that is fully within a region of free configuration space for the autonomous vehicle and outside of all regions of occupied configuration space for the autonomous vehicle.
20. The method of claim 19 further comprising:
storing the predicted lateral offset distances in a lookup table,
wherein using the predicted lateral offset distances to determine a trajectory for the autonomous vehicle comprises retrieving the predicted lateral offset distances from the lookup table when the moving actor is in proximity to the autonomous vehicle.