Patent Description:
One of the core capabilities of a computer controlled vehicle is path planning, which specifies a configuration of the vehicle over space and time. Such a plan can then be converted into commands to the vehicle's actuation and ultimately tracked and realized in the physical world (subject to uncertainty). The path planning problem is formalized by definition of goals characterizing what an operator wants the vehicle to do and dynamic feasibility characterizing what the vehicle can do. These components may be designed to incorporate constraints as well. The constraints can include physical limitations, operational rules, and safety considerations. Applications of the path planning problem include computing paths to a goal that avoid collision with obstacles of known extent and are minimal with respect to time, distance, or risk. This is itself a NP-complete problem if the system dynamics pose a holonomic constraint.

The current state of the art for path planning relies on rapidly exploring random algorithms. For vehicles in dynamic environments, these algorithms operate on an online basis in soft real time and create a log-sparse graph corresponding to feasible paths that a vehicle can take through space. The planning problem is then posed as search over the space of such graphs, and state of the art algorithms make use of dynamic programming to efficiently determine optimal paths. However, the graph creation step is computationally limiting in practice, as generating large graphs in this manner requires an excessive amount of time. Accordingly, only small and shallow graphs are created, which can result in path planning algorithms that fail to converge to near-optimal solutions and lead to abnormal and unacceptable system behavior.

<CIT> relates to, according to its abstract, systems and methods for autonomous taxi route planning for an aircraft. Clearance communication is received from a ground control station. A planning problem is generated from the clearance communication and sent to a route planner. The route planner receives the planning problem and plans an executable taxi route. Planning the executable taxi route can include generating a complete breadth first search graph from a start pose to a destination, pruning the graph, minimizing the graph, refining the graph, and extracting the shortest path from the graph.

<CIT>, according to its abstract, provides tools and techniques for computing flight plans for unmanned aerial vehicles (UAVs) while routing around obstacles having spatial and temporal dimensions. Methods provided by these tools may receive data representing destinations to be visited by the UAVs, and may receive data representing obstacles having spatial and temporal dimensions. These methods may also calculate trajectories spatial and temporal dimensions, by which the UAV may travel from one destination to another, and may at least attempt to compute flight plans for the UAVs that incorporate these trajectories. The methods may also determine whether these trajectories intersect any obstacles, and at least attempt to reroute the trajectories around the obstacles. These tools may also provide systems and computer-readable media containing software for performing any of the foregoing methods.

The solution is provided by the features of the independent claims. Variations are as described by the features of the dependent claims.

According to various examples, a method of traversing in an environment that includes at least one obstacle, by a mobile autonomous system, to a destination in the environment, is presented. The method includes generating, prior to the mobile autonomous system commencing activity in the environment, a graph comprising a plurality of vertices representing positions in the environment and a plurality of edges between vertices representing feasible transitions by the mobile autonomous vehicle in the environment; annotating the graph with at least one edge connecting a representation of a present position of the mobile autonomous system to a vertex of the graph; determining, based on the graph, a path from the present position of the mobile autonomous system in the environment to the destination; and traversing the environment to the destination, by the mobile autonomous system, based on the path.

Various optional features of the above method include the following. The vertices may store location and direction information. The vertices may store respective costs to the destination. The vertices may store identifications of next vertices in traversing to the destination. The at least one obstacle may include at least one dynamic obstacle. The method may further include invalidating at least one edge in the graph to represent a position of the at least one obstacle at a particular time. The annotating the graph with the at least one edge may include adding to the graph the at least one edge without generating a new memory allocation on an order of a size of the graph. The mobile autonomous system may include an aircraft. The destination may include a plurality of locations, and the graph may represent the plurality of locations by a plurality of vertices. The determining the path may include performing a non-exhaustive search of the graph.

According to various examples, a system for traversing in an environment that includes at least one obstacle, by a mobile autonomous system, to a destination in the environment, is presented. The system includes an electronic processor; electronic persistent memory comprising instructions that, when executed by the electronic processor, configure the electronic processor to perform operations comprising: generating, prior to the mobile autonomous system commencing activity in the environment, a graph comprising a plurality of vertices representing positions in the environment and a plurality of edges between vertices representing feasible transitions by the mobile autonomous vehicle in the environment; annotating the graph with at least one edge connecting a representation of a present position of the mobile autonomous system to a vertex of the graph; and determining, based on the graph, a path from the present position of the mobile autonomous system in the environment to the destination; wherein the mobile autonomous system is configured to traverse the environment to the destination based on the path.

Various optional features of the above system include the following. The vertices may store location and direction information. The vertices may store respective costs to the destination. The vertices may store identifications of next vertices in traversing to the destination. The at least one obstacle may include at least one dynamic obstacle. The operations may further include invalidating at least one edge in the graph to represent a position of the at least one obstacle at a particular time. The annotating the graph with the at least one edge may include adding to the graph the at least one edge without generating a new memory allocation on an order of a size of the graph. The mobile autonomous system may include an aircraft. The destination may include a plurality of locations, and the graph may represent the plurality of locations by a plurality of vertices. The determining the path may include performing a non-exhaustive search of the graph.

The above and/or other aspects and advantages will become more apparent and more readily appreciated from the following detailed description of examples, taken in conjunction with the accompanying drawings, in which:.

Exemplary aspects will now be described more fully with reference to the accompanying drawings. Examples of the disclosure, however, can be embodied in many different forms and should not be construed as being limited to the examples set forth herein. Rather, these examples are provided so that this disclosure will be thorough and complete, and will fully convey the scope to those skilled in the art. In the drawings, some details may be simplified and/or may be drawn to facilitate understanding rather than to maintain strict structural accuracy, detail, and/or scale.

Some examples overcome the computation time and graph size bottlenecks of existing path planning techniques. Some examples are particularly suited for problem instances with a mixed structure that includes both known static hazards and dynamically observed hazards. In such cases, some examples allow for the use of graphs that are orders of magnitude larger (for equivalent computational work) than what are currently used. This allows examples to quickly compute solutions that are both locally and globally performant. Conversely, for a fixed requirement on expressive capacity, some examples provide improved runtime and faster closing of control loop. This allows a vehicle to more readily adapt to new information without losing sight of long-run goals.

Examples may be utilized with a variety of vehicles in a variety of environments. Environments may be three-dimensional, e.g., for airspace or underwater path planning, or two-dimensional, e.g., for ground or sea surface path planning. Example vehicles include aircraft, ships, submarines, automobiles, trucks, factory robots, unmanned ariel, submersible, and terrestrial vehicles, and any other vehicle that is autonomous or that can be controlled by a computer autonomously or semi-autonomously.

Some examples are particularly relevant for scenarios in which the implications of local maneuvering affect long-run mission feasibility. One example of this sort of mixed objective is the safe maneuvering of an energetically constrained vehicle such as an electric vertical takeoff and landing (eVTOL) airplane around hazards such as birds or other aircraft, in which case proximate collision avoidance must be balanced against long run energetic limitations necessary to safely land while avoiding known terrain and hazards. A second example is a strongly underactuated vehicle such as a high-altitude glider, which might need to balance dynamism to harness energy from locally observed wind gusts with station keeping to a sufficient degree for long run mission success.

In general, examples may include an offline portion and an online portion. The offline portion may be executed prior to the vehicle operating in the environment under consideration, and the online portion may be executed during the vehicle's operation in the environment under consideration. The offline portion can include generating an offline graph that represents relations between reachable configurations that avoid static and otherwise known obstacles and satisfy operational constraints. An example offline graph is depicted in <FIG>. The offline portion can further include augmenting the offline graph with vertex-wise information on the minimal cost paths and next vertex information. An example such augmented offline graph is depicted in <FIG>. The online portion can include generating an online graph that includes at least one edge connecting a current position of the vehicle to a vertex in the offline graph. An example such an online graph is depicted in <FIG>. The online portion can also include augmenting the online graph to evaluate if an edge is blocked by one or more previously unobserved obstacles. An example such an augmented online graph is depicted in <FIG>. The online portion can further include searching for an updated minimal path subject to the one or more previously unobserved obstacles. The results of an example such search are depicted in <FIG>.

These and other elements, features, and advantages are shown and described presently in reference to <FIG>.

<FIG> depicts an example offline graph <NUM> according to various examples. Offline graph <NUM> may include a representation of a known starting location and/or a known destination, e.g., by way of non-limiting examples, an origin airport and/or a destination airport. More generally, offline graph is populated with vertices, e.g., vertex <NUM>, that each represent a state that includes both location and direction.

The points used to seed offline graph <NUM>, e.g., the points that are to be used for the location portion of the vertices, can be selected according to any of a variety of techniques. By way of non-limiting examples, such points may be selected randomly, by using a deterministic space filling algorithm, or to align with existing procedural or operational structures (e.g., known fixes, airways, or approaches in an airspace system). Offline graph <NUM> may include, for example, at least <NUM>,<NUM> vertices.

Each vertex in the graph includes both positional and velocity degrees of freedom, representing location and direction with speed, respectively. Various examples may use any mixture of such states, e.g., position, heading, and airspeed for an aircraft. Although offline graph <NUM> represents two-dimensional space, examples are not so limited. Offline graphs (and other graphs disclosed herein) representing three-dimensional space are contemplated. For example, in three dimensions, each vertex may include two three-tuples, e.g., (x<NUM>, y<NUM>, z<NUM>), (x<NUM>, y<NUM>, z<NUM>), with one representing position and another representing orientation. Thus, for three dimensions, each vertex may include two three-dimensional vectors. In two dimensions, each vertex may include two pairs, e.g., (x<NUM>, y<NUM>), (x<NUM>, y<NUM>), with one representing position and another representing orientation; in two dimensions, each vertex may include two two-dimensional vectors. In sum, each vertex in offline graph <NUM> includes both a location and a direction. When clear from context, vertices may be referred to as states, which includes both location and direction, or as locations, which refers to the location portion. Whether two-dimensional or three-dimensional, offline graph <NUM> may include an added dimension (e.g., added to each of the respective three-tuples or pairs) to represent time.

Edges between vertices in offline graph <NUM>, e.g., edge <NUM>, represent feasible transitions between states (e.g., locations and directions) respecting known constraints. Feasible transitions can take into account, for example physical limitations of the vehicle, such as turning radius, and constraints can take into account known obstacles. In general, offline graph <NUM> may be a log sparse graph, with |EDGES| = |VERTICES| logIVERTICESI.

Offline graph <NUM> may be generated offline, prior to the vehicle operating in the space represented by offline graph <NUM>. Offline graph <NUM> may be stored electronically in persistent computer-readable media, for example. In particular, offline graph <NUM>, as well as the other graphs disclosed herein, may be stored in any suitable graph storage data structure, modified to store various data as disclosed herein, e.g., shortest path and successor vertex data as described in reference to <FIG>, in association with the vertices.

<FIG> depicts an example augmented offline graph <NUM>, in particular, an augmentation of offline graph <NUM> of <FIG>, including a minimal path <NUM> from a starting location <NUM> to a destination location <NUM>, according to various examples. Starting location <NUM> and destination location <NUM> may each be represented by a respective vertex in augmented offline graph <NUM>, indicating both location and direction for each. Each vertex in augmented offline graph <NUM> further includes a representation of a cost to destination state <NUM>, e.g., a minimal cost. Thus, each vertex may be augmented with a scalar value representing such a cost. Herein, "cost" may be implemented in a variety of forms and may represent time, distance, fuel expense, or any combination thereof. Further, each vertex may be augmented with an identification of a successor vertex in a path, e.g., a minimal cost path, to destination state <NUM>. The identification of the successor vertex may take any of a variety of forms, e.g., an index of a vertex, where the vertices of augmented offline graph <NUM> are indexed by some enumeration, for example.

As shown, augmented offline graph <NUM> includes minimal cost path <NUM> from starting state <NUM> to destination state <NUM>. Such a path may be obtained using a variety of techniques, e.g., dynamic programming applied to the costs to the destination stored at the vertices. Example suitable dynamic programming techniques include Dijkstra's algorithm and A*.

<FIG> depicts an example online graph <NUM> according to various examples. Online graph <NUM> may be constructed from an augmented offline graph, and is described by way of non-limiting example in reference to augmented offline graph <NUM> of <FIG>. Relative to the offline portion of various examples, for the online portion, when the vehicle is present in the environment under consideration, two things may change. First, the current location <NUM> of the vehicle may be at an arbitrary position, e.g., not at a location represented by a vertex of augmented offline graph <NUM>. Second, one or more previously unknown obstacles, such as obstacle <NUM>, may be present. These changes may be addressed as follows. First, new edges <NUM> that connect the current location <NUM> of the vehicle may be added. Second, edges <NUM> that cross obstacle <NUM> may be considered invalid. Such edges may not be considered when planning a path, but may be retained in the graph, e.g., for use when the obstacle is no longer present.

The current location <NUM> of the vehicle may be determined according to any of a variety of techniques, including, by way of non-limiting examples, GPS, satellite imaging, dead reckoning, and/or triangulation based on any of RADAR, SONAR, or LIDAR. The current location and direction form a state that may be joined to an existing vertex in online graph <NUM>, where the existing vertex may be identified, for example, using a nearest neighbor search.

The new obstacles, such as obstacle <NUM>, may be identified using any of a variety of techniques. Detection techniques include, by way of non-limiting example, ground-based, air-based, sea-based, and/or vehicle-based RADAR, SONAR, and LIDAR. Satellite imaging may be used. Information from any entity, such as an air traffic controller, that may utilize any of the aforementioned detection techniques or a different technique, may further be used to identify any new obstacles that appear in the environment under consideration.

Online graph <NUM> may include an added dimension to represent time. For example, for path planning in three dimensions, each vertex may include a three-dimensional location vector, a three-dimensional direction vector, and a scalar time value, all of which may be represented in seven dimensions, e.g., in <MAT>. Further, each edge may be associated with a traversal duration. When planning in the presence of dynamic obstacles, time as a parameter may be used to evaluate if there is a potential collision. For example, if the current time is T and an edge in the graph is known to have duration D to traverse, then the state that may be reached by following that edge is ([location vector], [direction vector], T+D), which allows for an evaluation of whether there is a potential collision with the dynamic obstacle. The graph may be constructed lazily as-needed (with caching to prevent recalculation).

<FIG> depicts an online graph <NUM>, representing the example online graph <NUM> of <FIG>, and showing a minimal cost path <NUM> from the current location <NUM> of the vehicle to the destination <NUM>, according to various examples. Minimal path <NUM> may be obtained using any of a variety of search algorithms applied to online graph <NUM>, e.g., beam search. In particular, the search may use the cost to destination <NUM> stored at the vertices as a proximate metric to select the minimal cost path <NUM>.

In sum, as shown and described herein in reference to <FIG>, a technique for path planning is presented. The technique includes an offline portion, prior to commencing vehicle operations in the area under consideration, in which an offline graph is generated. The offline graph may be implemented as a large log-sparse mobility graph representing relations between reachable configurations that avoid static and known obstacles and satisfy operational constraints. <FIG> depicts an example such offline graph. The offline portion may further include augmenting the offline graph with vertex-wise information on the minimal cost paths to the destination, e.g., by using dynamic programming. <FIG> depicts an example such augmented offline graph. The technique also includes an online portion, during vehicle activity in the area under consideration, that uses the augmented offline graph to generate an online graph. The online graph includes a connection of a current position of the vehicle to a vertex in the augmented offline graph. <FIG> depicts an example online graph with such a connection. The online graph further includes the ability to evaluate if an edge is blocked by any previously unobserved obstacle. <FIG> depicts an example online graph with such an ability. The online graph is then used to plan a path, e.g., by searching for a minimal path subject to previously unobserved obstacles. <FIG> depicts an example minimal path. <FIG>, described presently, shows a system configured to perform the described technique.

<FIG> is a block diagram of a path planning system <NUM> according to various examples. System <NUM> divides the work required for path planning into actions performed by offline portion <NUM>, which includes actions that may occur prior to vehicle operation in the area of interest, and actions performed by online <NUM> portion, which includes actions that may be performed during vehicle operation in the area of interest. In general, offline portion <NUM> generates an offline graph as a computational representation of mobility in a specific operational context. This can be done long before use is required to solve a specific planning problem from the context of a vehicle, and admits use of hardware parallelism and validation approaches that would not be otherwise computationally tractable in the runtime of planning. Online portion <NUM> generally concerns the enrichment and consumption of the offline graph within the area of operation during operation.

Offline portion <NUM> includes build offline graph component <NUM>. Build offline graph component <NUM> provides an offline graph representing feasible connections between a set of states and a set of goals, subject to known dynamics and static time-invariant constraints. Build offline graph component <NUM> may augment each vertex of the offline graph with a minimal cost to go to the destination. Persisting such cost-to-go information over each vertex allows for the use of purely local O(<NUM>) greedy heuristics about reachability. Further, build offline graph component <NUM> may augment the offline graph with geometric information in each vertex representing path connections, e.g., a next vertex identification, to improve the speed at which an edge can be evaluated as intersecting a known obstacle. Once constructed by build offline graph component <NUM>, the offline graph may be stored in persistent storage <NUM> for consumption during the online portion.

System <NUM> also includes online portion <NUM>. Online portion <NUM> includes planning scope <NUM>, a runtime environment that supports persistent memory allocation to reuse the offline graph without reloading or reallocating from memory. Planning scope <NUM> may be used in a repeated context, including receding horizon planning or evaluation of multiple scenarios. Planning scope <NUM> may retrieve the offline graph from persistent storage <NUM>, which may be the same or a different persistent storage from persistent storage <NUM>.

Online portion <NUM> also includes add edge component <NUM>, which adds at least one edge connecting a representation of a current location of the vehicle to a vertex in the offline graph. That is, add edge component <NUM> provides a graph interface representing the online graph inclusive of an arbitrary current location (e.g., a current state) connected onto the existing offline graph. The connection may be determined using a nearest neighbor search, for example. Add edge component <NUM> may provide an abstract graph view rather than a new full-size memory allocation in order to avoid memory operations on the order of the size of the graph (e.g., O(|VERTICES|)) during online portion <NUM>. Such an abstract graph view may provide a function that can, for example, identify all successors of an input vertex identification. Such a function may operate on a small portion of the graph without requiring the entire graph to be held in dynamic memory. Further, such a function may store a temporary list of vertices that have previously been evaluated by the function and their successors, such that future evaluations of the function at such vertices may be performed using a fast lookup call.

Online portion <NUM> further includes invalidate blocked edge component <NUM>. Invalidate blocked edge component <NUM> provides a graph interface representing an offline graph inclusive of known dynamic constraints, including moving objects known during the online portion. Invalidate blocked edge component <NUM> may employ halfspace inclusion or polytope intersection together with next vertex information to detect whether a dynamic or static constraint blocks an edge. If so, blocked edge component <NUM> removes the edge from being used in computations at later stages. Blocked edge component <NUM> may keep an electronically stored list of blocked edges in memory according to some examples. Such a list may be checked prior to performing certain actions, such as performed by graph planner <NUM>, and edges represented therein may be removed from the actions. Similar to add edge component <NUM>, invalidate blocked edge component <NUM> may provide an abstract graph view rather than a new memory allocation in order to avoid memory operations on the order of the size of the graph during online portion <NUM>.

Online portion <NUM> further includes graph planner <NUM>. Graph planner <NUM> provides trajectories, paths, or plans, given goals. Graph planner <NUM> may implement any of a variety of algorithms, e.g., bounding (A*, D*) random search (e.g., Monte Carlo tree search), or heuristic search algorithms (e.g., beam search). Because the removing blocked edges from being used can increase the cost-to-go for vertices in a non-local manner, graph planner <NUM> may employ backtracking heuristic modifications. According to some examples, a beam search heuristic is used to balance runtime and optimality considerations. In particular, a logarithmically scaling beam width may be used, e.g., the beam width may be selected as c log|VERTICES|, where c is some constant. Graph planner <NUM> outputs a path from the current location of the vehicle to the destination.

Some examples minimize the amount of runtime work performed by online portion <NUM> through the use of various features. Such features may include (<NUM>) creating abstract view graphs instead of reallocating memory, and (<NUM>) updating cost-to-go lazily based on backtracking instead of exhaustively checking edges and performing computational work that scales extensively with number of edges. These two aspects in particular may provide performant and accurate planning with relatively low requirements on dynamic memory allocation and computational complexity at runtime.

<FIG> is a flowchart for a method <NUM> of path planning according to various examples. Method <NUM> may include a mobile autonomous system traversing an environment that includes at least one obstacle to a destination in the environment. Method <NUM> may be implemented by system <NUM> as shown and described in reference to <FIG> using hardware <NUM> as shown and described in reference to <FIG>.

At <NUM>, method <NUM> generates an offline graph. The offline graph may be generated prior to the mobile autonomous system commencing activity in the environment. The offline graph may include a plurality of vertices representing positions in the environment and a plurality of edges between vertices representing feasible transitions by the mobile autonomous vehicle in the environment. The actions of <NUM> may include any, or any combination, of actions as shown and described herein in reference to <FIG> and <FIG> and in reference to build offline graph component <NUM> of <FIG>.

At <NUM>, method <NUM> includes annotating the graph with at least one edge connecting a representation of a present position of the mobile autonomous system to a vertex of the graph. The actions of <NUM> may include any, or any combination, of actions as shown and described in reference to <FIG> and in reference to add edge component <NUM> of <FIG>.

At <NUM>, method <NUM> includes determining, based on the graph, a path from the present position of the mobile autonomous system in the environment to the destination. The actions of <NUM> may include any, or any combination, of actions as shown and described in reference to <FIG> and in reference to graph planner <NUM> of <FIG>.

At <NUM>, method <NUM> includes traversing the environment to the destination, by the mobile autonomous system, based on the path. The actions of <NUM> may include the mobile autonomous system physically traversing in the environment to the destination using the path as a traversal route.

<FIG> is a block diagram of example hardware <NUM> for implementing various examples. For example, <FIG> illustrates various hardware, software, and other resources that can be used in implementations of method <NUM> as shown and described herein in reference to <FIG>. Further, system <NUM> may implement build offline graph component <NUM>, persistent storage <NUM>, persistent storage <NUM>, add edge component <NUM>, invalidate blocked edge component <NUM>, graph planner <NUM>, and/or planning scope <NUM>.

System <NUM> includes offline portion computer <NUM> and online portion computer <NUM>. Offline portion computer <NUM> may perform offline portion actions, e.g., as shown and described herein in reference to <FIG>, <FIG>, and <FIG>. Online portion computer <NUM> may perform online portion actions, e.g., as shown and described herein in reference to <FIG>, <FIG>, and <FIG>. Online portion computer <NUM> may be deployed aboard the vehicle at issue, e.g., aircraft <NUM>. Offline portion computer <NUM> and online portion computer <NUM> may be communicatively coupled by way of one or more networks <NUM>, e.g., the internet.

Either of offline portion computer <NUM> or online portion computer <NUM> may be implemented as any of a desktop computer, a laptop computer, can be incorporated in one or more servers, clusters, or other computers or hardware resources, or can be implemented using cloud-based resources. Offline portion computer <NUM> includes volatile memory <NUM> and persistent memory <NUM>, the latter of which can store computer-readable instructions, that, when executed by electronic processor <NUM>, configure offline portion computer <NUM> to at least partially perform an offline portion of methods, e.g., method <NUM>, as shown and described herein. Offline portion computer <NUM> includes network interface <NUM>, which communicatively couples offline portion computer <NUM> to online portion computer <NUM> via network <NUM>. Online computer <NUM> includes volatile memory <NUM> and persistent memory <NUM>, the latter of which can store computer-readable instructions, that, when executed by electronic processor <NUM>, configure online computer <NUM> to at least partially perform an online portion of methods, e.g., method <NUM>, as shown and described herein. Online portion computer <NUM> includes network interface <NUM>, which communicatively couples online portion computer <NUM> to offline portion computer <NUM> via network <NUM>. Other configurations of system <NUM>, associated network connections, and other hardware, software, and service resources are possible.

Claim 1:
A method (<NUM>) of traversing in an environment that includes at least one obstacle (<NUM>), by a mobile autonomous system, to a destination (<NUM>) in the environment, the method comprising:
generating, prior to the mobile autonomous system commencing activity in the environment, a graph comprising a plurality of vertices (<NUM>) representing positions in the environment and a plurality of edges (<NUM>) between vertices representing feasible transitions by the mobile autonomous vehicle in the environment, wherein the graph is a log-sparse graph with |EDGES| = |VERTICES| log |VERTICES|;
annotating the graph with at least one edge connecting a representation of a present position of the mobile autonomous system to a vertex of the graph;
determining, based on the graph, a path from the present position of the mobile autonomous system in the environment to the destination; and
traversing the environment to the destination, by the mobile autonomous system, based on the path.