Unstructured vehicle path planner

Abstract

An autonomous vehicle guidance system that generates a path for controlling an autonomous vehicle based at least in part on a data structure generated based at least in part on sensor data that may indicate occupied space in an environment surrounding an autonomous vehicle. The guidance system may receive a grid and generate a grid associated with the grid and the data structure. The guidance system may additionally or alternatively sub-sample the grid (latterly and/or longitudinally) dynamically based at least in part on characteristics determined from the data structure. The guidance system may identify a path based at least in part on a set of precomputed motion primitives, costs associated therewith, and/or a heuristic cost plot that indicates a cheapest cost to move from one pose to another.

What is claimed is: 
     
       1. A method comprising:
 receiving a route associated with a start position and an end position in an environment; 
 receiving sensor data from a sensor; 
 determining, based at least in part on the sensor data, a data structure indicating whether space in the environment is occupied or unoccupied; 
 determining, based at least in part on the route, a first grid comprising one or more layers disposed at intervals along the route and defining a plurality of nodes associated with different locations in the environment, wherein the first grid overlays the data structure, the plurality of nodes being regularly spaced along the route; 
 determining a subset of nodes based at least in part on the data structure; 
 determining, based at least in part on the subset of nodes, a cost plot, and a set of potential motions, a path between the start position and at least one of the end position or an end layer, the cost plot comprising a set of values indicative of costs to move from a range of positions and orientations to a desired position and orientation; and 
 controlling an autonomous vehicle based at least in part on the path. 
 
     
     
       2. The method of  claim 1 , wherein a potential motion of the set of potential motions is associated with a motion cost,
 the method further comprising:
 determining, based at least in part on the motion cost, a connection between a first node and a second node of the subset of nodes, wherein the path comprises the connection; and 
 determining, based at least in part on the second node and the cost plot, a first cost. 
 
 
     
     
       3. The method of  claim 1 , wherein a potential motion of the set of potential motions is precomputed based at least in part on:
 generating a second grid comprising a plurality of cells, wherein a first cell of the plurality of cells represents a first pose of the autonomous vehicle and a second cell of the plurality of cells represents a second pose of the autonomous vehicle; and 
 determining a curve which, when followed by the autonomous vehicle from the first cell to the second cell, will cause the autonomous vehicle to align with the second pose. 
 
     
     
       4. The method of  claim 1 , wherein determining the subset of nodes based at least in part on the data structure comprises at least one of:
 determining, as a sample layer, a first layer of the one or more layers based at least in part on determining that a first number of nodes of the first layer associated with first occupied space of the data structure differs from a second number of nodes of a second layer previous to the first layer associated with second occupied space of the data structure; or 
 determining, as sample nodes, one or more nodes of the first layer based at least in part on a sample rate. 
 
     
     
       5. The method of  claim 4 , wherein:
 determining the path comprises a search; 
 the search is a first search, the sample layer is a first sample layer of a first set of sample layers, the sample nodes are first sample nodes, and the sample rate is a first sample rate; and 
 the method further comprises:
 determining that, within a threshold amount of time or a threshold number of iterations, the first search is unable to identify a feasible path; 
 determining at least one of a second set of sample layers or second sample nodes to increase a total number of sample nodes; and 
 determining, based at least in part on a second search over the second sample nodes, a second path. 
 
 
     
     
       6. The method of  claim 5 , wherein the path is a first path and the search comprises:
 determining a set of paths from at least one of the end position or the end layer to the start position, wherein the set of paths is less than all possible paths between the end position or the end layer and the start position; 
 determining that the set of paths comprises a first group of paths and a second group of paths, based at least in part on determining that first distances between the first group of paths are less than a threshold distance and second distances between the second group of paths are less than the threshold distance, wherein the first group comprises the first path; and 
 outputting the first path as a primary path and a second path from the second group as a contingent path. 
 
     
     
       7. A system comprising:
 one or more processors; and 
 a memory storing processor-executable instructions that, when executed by the one or more processors, cause the system to perform operations comprising:
 receiving a route associated with a start position and an end position in a region of an environment; 
 receiving sensor data from a sensor; 
 determining, based at least in part on the sensor data, an indication of occupied portions of the region; 
 determining, based at least in part on the route and the occupied portions, a grid comprising one or more layers along the route, wherein:
 an individual layer of the one or more layers is associated with a portion of the route and comprises a plurality of nodes and the individual layer is perpendicular to the portion of the route, and 
 the one or more layers are disposed at intervals along the route in unoccupied portions of the region; 
 
 determining, as sample nodes and based at least in part on the grid and the occupied portions, one or more nodes of the one or more layers; and 
 determining, based at least in part on a search for a set of connections between the sample nodes, a first path between at least one of the end position or an end layer associated with the end position and the start position, wherein the search is based at least in part on the occupied portions and a set of values associated with one or more motion primitives. 
 
 
     
     
       8. The system of  claim 7 , wherein the one or more motion primitives and the set of values are precomputed and the search comprises:
 determining, based at least in part on the set of values, at least one of a first sample node of a first sample layer or one or more sample nodes of one or more sample layers succeeding the first sample layer; 
 determining, as a first cost, a value of the set of values associated with a motion primitive which connects a second sample node of the one or more sample nodes to the first sample node; 
 determining that at least one of the first cost is less than a threshold cost or less than first costs associated with one or more other nodes of the one or more sample layers; 
 determining, based at least in part on the one or more motion primitives, the first sample node, the second sample node, and the occupied portions, that at least one motion primitive is collision-free and connects the first sample node to the second sample node; and 
 determining a second cost associated with the at least one motion primitive and the first sample node. 
 
     
     
       9. The system of  claim 8 , wherein determining the second cost is based at least in part on at least one of:
 a third cost associated with at least one of a current position or a current pose of an autonomous vehicle; 
 a fourth cost associated with a curvature of the at least one motion primitive; 
 a fifth cost associated with a first distance from at least a portion of the at least one motion primitive to a portion of the occupied portions; 
 a sixth cost associated with a first difference between the at least one motion primitive and a second motion primitive of a previous connection in the first path; or 
 a seventh cost associated with a second distance of the at least one motion primitive from a lane associated with at least one of the start position, the end position, or a target position. 
 
     
     
       10. The system of  claim 7 , wherein determining the sample nodes comprises at least one of:
 determining, as a sample layer, a first layer of the one or more layers based at least in part on:
 determining a difference between a first characterization of occupied space indicated by the occupied portions associated with the first layer and a second characterization of occupied space indicated by the occupied portions associated with a previous layer, and 
 determining that the difference meets or exceeds a change threshold; or 
 
 determining one or more of the sample nodes from the first layer based at least in part on a sample rate. 
 
     
     
       11. The system of  claim 7 , wherein:
 the search is a first search and the sample nodes are first sample nodes; 
 determining the sample nodes is based at least in part on at least one of a first sample rate or a first sensitivity; and 
 the operations further comprise:
 determining a first total cost associated with the first path; 
 determining that, within a threshold amount of time or a threshold number of iterations, the first search is unable to identify a feasible path; 
 determining, based at least in part on determining that the first search is unable to identify a feasible path, second sample nodes based at least in part on at least one of a second sample rate or a second sensitivity, wherein the second sample nodes are greater in number than the first sample nodes; and 
 determining, based at least in part on a second search over the second sample nodes, a second path. 
 
 
     
     
       12. The system of  claim 11 , wherein the feasible path is collision free and associated with a second cost that is less than a second cost threshold. 
     
     
       13. The system of  claim 7 , wherein the search comprises:
 determining a set of paths from at least one of the end position or the end layer to the start position, wherein the set of paths is less than all possible paths between the end position or the end layer to the start position; 
 determining that the set of paths comprises a first group of paths and a second group of paths, based at least in part on determining that first distances between the first group of paths are less than a threshold distance and second distances between the second group of paths are less than the threshold distance, wherein the first group comprises the first path; and 
 outputting the first path as a primary path and a second path from the second group as a contingent path. 
 
     
     
       14. The system of  claim 13 , wherein the operations further comprise identifying the first path as the primary path and the second path as the contingent path based at least in part on determining that at least one of:
 the first path is at least one of shorter than the second path or is associated with a lower change in curvature than the second path; 
 the first path is associated with a first total cost that is less than first total costs of other paths of the first group; 
 the second path is associated with a second total cost that is less than second total costs of other paths of the second group; or 
 the first total cost is less than the second total cost. 
 
     
     
       15. A non-transitory computer-readable medium storing processor-executable instructions that, when executed by one or more processors, cause the one or more processors to perform operations comprising:
 receiving a route associated with a start position and an end position in an environment; 
 receiving a data structure associated with the environment, the data structure identifying occupied space in the environment; 
 determining, based at least in part on the route, a grid comprising one or more layers, the grid being overlaid over the data structure and shaped based on the route; 
 determining, as sample nodes and based at least in part on the data structure, one or more nodes of the one or more layers; and 
 determining, based at least in part on a search for a set of contiguous connections between the sample nodes, a first path from at least one of the end position or an end layer associated with the end position to the start position, wherein the search is based at least in part on the data structure, a first set of values associated with differences between a range of positions and orientations and a desired position and orientation, and a second set of values associated with one or more motion primitives. 
 
     
     
       16. The non-transitory computer-readable medium of  claim 15 , wherein the one or more motion primitives and the second set of values are precomputed and the search comprises:
 determining, based at least one of a first sample node of a first sample layer and one or more sample nodes of one or more sample layers succeeding the first sample layer, a motion primitive of the one or more motion primitives; 
 determining, based at least in part on the motion primitive and the first set of values, a first cost associated with a second sample node of the one or more sample nodes; 
 determining that at least one of the first cost is less than a threshold cost or less than first costs associated with one or more other nodes of the one or more sample layers; 
 determining, based at least in part on the one or more motion primitives, the first sample node, the second sample node, and the data structure, that at least one motion primitive is collision-free and connects the first sample node to the second sample node; and 
 determining a second cost based at least in part on the first sample node, the second sample node, and the second set of values. 
 
     
     
       17. The non-transitory computer-readable medium of  claim 16 , wherein determining the second cost is based at least in part on at least one of:
 a third cost associated with at least one of a current position or a current pose of an autonomous vehicle; 
 a fourth cost associated with a curvature of the at least one motion primitive; 
 a fifth cost associated with a first distance from at least a portion of the at least one motion primitive to a portion of the data structure identified as being occupied; 
 a sixth cost associated with a first difference between the at least one motion primitive and a second motion primitive of a previous connection in the first path; or 
 a seventh cost associated with a second distance of the at least one motion primitive from a lane associated with at least one of the start position, the end position, or a target position. 
 
     
     
       18. The non-transitory computer-readable medium of  claim 15 , wherein selecting the sample nodes comprises at least one of:
 determining, as a sample layer, a first layer of the one or more layers based at least in part on:
 determining a difference between a first characteristic of space indicated by the data structure associated with the first layer and a second characterization of space indicated by the data structure associated with a previous layer, and 
 determining that the difference meets or exceeds a change threshold; or 
 
 determining one or more of the sample nodes from the first layer based at least in part on a sample rate. 
 
     
     
       19. The non-transitory computer-readable medium of  claim 15 , wherein:
 the search is a first search and the sample nodes are first sample nodes; 
 determining the sample nodes is based at least in part on at least one of a first sample rate or a first sensitivity; and 
 the operations further comprise:
 determining a first total cost associated with the first path; 
 determining that, within a threshold amount of time or a threshold number of iterations, the first search is unable to identify a feasible path; 
 determining, based at least in part on determining that the first search is unable to identify a feasible path, second sample nodes based at least in part on at least one of a second sample rate or a second sensitivity, wherein the second sample nodes are greater in number than the first sample nodes; and 
 determining, based at least in part on a second search over the second sample nodes, a second path. 
 
 
     
     
       20. The non-transitory computer-readable medium of  claim 15 , wherein the search comprises:
 determining a set of paths from at least one of the end position or the end layer to the start position, wherein the set of paths is less than all possible paths between the end position or the end layer to the start position; 
 determining that the set of paths comprises a first group of paths and a second group of paths, based at least in part on determining that first distances between the first group of paths are less than a threshold distance and second distances between the second group of paths are less than the threshold distance, wherein the first group comprises the first path; and 
 outputting the first path as a primary path and a second path from the second group as a contingent path. 
 
     
     
       21. The non-transitory computer-readable medium of  claim 20 , wherein the operations further comprise identifying the first path as the primary path and the second path as the contingent path based at least in part on determining that at least one of:
 the first path is at least one of shorter than the second path or is associated with a lower change in curvature than the second path; 
 the first path is associated with a first total cost that is less than first total costs of other paths of the first group; 
 the second path is associated with a second total cost that is less than second total costs of other paths of the second group; or 
 the first total cost is less than the second total cost.