Bidirectional path optimization in a grid
Provided are methods, systems, and computer program products for bidirectional path optimization in a grid. An example method may include: receiving vehicle environment data associated with an environment of a vehicle; determining an occupancy map of the environment using the vehicle environment data, the occupancy map identifying at least one obstacle in the environment; generating a reference path to a destination location for the vehicle; determining a travel path to an intermediate location in the occupancy map based at least in part on the reference path and a longitudinal direction variable, wherein the intermediate location is proximate a point on the reference path; and generating actuation commands for the vehicle based at least in part on the travel path.
1. A method, comprising:
receiving vehicle environment data associated with an environment of a vehicle;
determining an occupancy map of the environment using the vehicle environment data, the occupancy map identifying at least one obstacle in the environment;
generating a reference path to a destination location for the vehicle;
determining a travel path to an intermediate location in the occupancy map based at least in part on the reference path and a longitudinal direction variable, wherein the intermediate location is proximate a point on the reference path, and wherein determining the travel path comprises:
determining a plurality of potential paths based at least in part on at least one physical constraint of the vehicle and variations to a plurality of variable control inputs;
determining a cost associated with each of the plurality of potential paths based at least in part on a weighting policy; and
selecting the travel path from the plurality of potential paths based at least in part on the determined cost associated with each of the plurality of potential paths; and
generating actuation commands for the vehicle based at least in part on the travel path.
2. The method of claim 1 , further comprising:
determining a physical location of the vehicle in a physical coordinate system; and
determining an occupancy map location of the vehicle by transforming the physical location of the vehicle from the physical coordinate system to a coordinate system of the occupancy map,
wherein determining the travel path to the intermediate location comprises determining the travel path to the intermediate location using the occupancy map location of the vehicle.
3. The method of claim 2 , further comprising:
transforming the reference path from the physical coordinate system to the coordinate system of the occupancy map, wherein determining the travel path to the intermediate location in the occupancy map based at least in part on the reference path and the longitudinal direction variable comprises determining the travel path to the intermediate location in the occupancy map using the reference path in the coordinate system of the occupancy map; and
transforming the travel path from the coordinate system of the occupancy map to the physical coordinate system, wherein generating the actuation commands for the vehicle based at least in part on the travel path comprises generating the actuation commands for the vehicle using the travel path in the physical coordinate system.
4. The method of claim 1 , wherein the longitudinal direction variable comprises a range of continuous numbers.
5. The method of claim 4 , wherein the continuous numbers that satisfy a number threshold represent a first longitudinal direction and the continuous numbers that do not satisfy the number threshold represent a second longitudinal direction.
6. The method of claim 5 , wherein the first longitudinal direction is forward and the second longitudinal direction is backward.
7. The method of claim 1 , further comprising generating at least one differentiable occupancy map evaluation using the occupancy map, wherein determining the travel path to the intermediate location comprises determining the travel path to the intermediate location using the at least one differentiable occupancy map evaluation.
8. The method of claim 7 , wherein generating the at least one differentiable occupancy map evaluation comprises applying at least one third order B-spline basis function to the occupancy map.
9. The method of claim 1 , wherein the at least one physical constraint of the vehicle comprises an angle of operation of a steering wheel of the vehicle.
10. The method of claim 1 , wherein each of the plurality of potential paths comprises a plurality of unit-distant steps, and wherein each unit-distant step of the plurality of unit-distant steps is associated with at least one variable control input.
11. The method of claim 1 , wherein the at least one physical constraint constrains a cartesian solution space of the travel path to within a region a predetermined distance from the reference path.
12. The method of claim 1 , wherein the at least one physical constraint constrains a vehicle heading to within a predetermined range of headings with respect to a given point on the reference path.
13. The method of claim 1 , wherein determining the cost associated with each of the plurality of potential paths based at least in part on the weighting policy includes determining a collision-free motion cost, a comfort cost, and a tracking cost for each of the plurality of potential paths.
14. The method of claim 13 , wherein the collision-free motion cost is based on an accumulation of costs, for all prediction steps, for each of a set of points of a vehicle footprint with respect to the at least one obstacle in the environment.
15. The method of claim 13 , wherein the comfort cost is based on, at each prediction step, at least a change rate of the longitudinal direction variable and a change rate of a steering angle.
16. The method of claim 13 , wherein the tracking cost is based on, at each prediction step, a first difference from a reference heading of the reference path at a tracking target location and a second difference from a reference location of the reference path at the tracking target location.
17. The method of claim 13 , wherein the weighting policy includes a plurality of weights for the collision-free motion cost, the comfort cost, and the tracking cost, and wherein a set of weights of the plurality of weights for the tracking cost include an intermediate weight for an intermediate prediction step before a final prediction step, and a final weight for the final prediction step, wherein the intermediate weight is substantially smaller than the final weight.
18. A system, comprising:
at least one processor, and
at least one non-transitory storage media storing instructions that, when executed by the at least one processor, cause the at least one processor to:
receive vehicle environment data associated with an environment of a vehicle;
determine an occupancy map of the environment using the vehicle environment data, the occupancy map identifying at least one obstacle in the environment;
generate a reference path to a destination location for the vehicle;
determine a travel path to an intermediate location in the occupancy map based at least in part on the reference path and a longitudinal direction variable, wherein the intermediate location is proximate a point on the reference path, and wherein to determine the travel path, the instructions, when executed by the at least one processor, cause the at least one processor to:
determine a plurality of potential paths based at least in part on at least one physical constraint of the vehicle and variations to a plurality of variable control inputs;
determine a cost associated with each of the plurality of potential paths based at least in part on a weighting policy; and
select the travel path from the plurality of potential paths based at least in part on the determined cost associated with each of the plurality of potential paths; and
generate actuation commands for the vehicle based at least in part on the travel path.
19. At least one non-transitory storage media storing instructions that, when executed by at least one processor, cause the at least one processor to:
receive vehicle environment data associated with an environment of a vehicle;
determine an occupancy map of the environment using the vehicle environment data, the occupancy map identifying at least one obstacle in the environment;
generate a reference path to a destination location for the vehicle;
determine a travel path to an intermediate location in the occupancy map based at least in part on the reference path and a longitudinal direction variable, wherein the intermediate location is proximate a point on the reference path, and wherein to determine the travel path, the instructions, when executed by the at least one processor, cause the at least one processor to:
determine a plurality of potential paths based at least in part on at least one physical constraint of the vehicle and variations to a plurality of variable control inputs;
determine a cost associated with each of the plurality of potential paths based at least in part on a weighting policy; and
select the travel path from the plurality of potential paths based at least in part on the determined cost associated with each of the plurality of potential paths; and
generate actuation commands for the vehicle based at least in part on the travel path.