Patent Description:
The problem of dynamically sensing and avoiding other moving entities is a central challenge to path planning for computer controlled vehicles today in multiple domains (e.g., land, sea surface, undersea, and air). Path planning specifies a configuration of the vehicle over space and time, and such a plan can then be converted into commands to the vehicle's actuation and ultimately realized in the physical world. An application of the path planning problem includes computing a path to a destination that avoids collision with static and dynamic obstacles of known extent and is minimal with respect to time, distance, or risk. This is itself a NP-complete problem if the system dynamics pose a holonomic constraint. The avoiding part of this problem is an instance of the general planning problem. Assurance and verification of planning for avoidance are critical technical challenges for many autonomous systems currently under development.

There is a significant amount of prior work concerning obtaining proofs of sufficiency of control systems. Such proofs generally take a known algebraic and numeric representation of continuous dynamics inclusive of a plant model and a controller and show that within the confines of a such a model the system is stable, controllable, and responsive. In the study of hybrid systems, this is extended to include mixed discrete degrees of freedom to represent phenomena including mode switching which might be present in configurable cyber-physical systems.

In the aerospace domain, the Federal Aviation Administration's Aircraft Collision Avoidance System, ACAS-X, addresses the sense and avoid problem by issuing advisories for a pilot to climb or descend at variable rates based on the scenario geometry. Verification of this system has been studied formally using an automatic theorem prover. However, this work is significantly limited because the ACAS-X planner is extremely confined and capable of only one of a small family of discrete suggestions, while more modern planners cover exponentially large choice spaces for which these verification techniques do not scale.

The paper from <NPL>, according to ist abstract, addresses the problem of path planning in environments in which some of the obstacles can change their positions. It uses the popular PRM method for navigating a robot through an environment. One of the key features of PRM is that it moves the major part of the calculations involved in the path planning process to the preprocessing phase. After that, paths can be extracted very quickly (in a query phase) usually without any noticeable delay. While very successful in many applications, doing most of the work in a preprocessing phase restricts the environment to be static i.e. obstacles are not allowed to change their configurations after the preprocessing phase. This paper describes and evaluates an algorithm based on PRM that does allow obstacles to change their configuration after preprocessing while still allowing for a quick query phase.

According to the present disclosure, a method and a system as defined in the independent claims are provided. Further embodiments of the claimed invention are defined in the dependent claims. Although the claimed invention is only defined by the claims, the below embodiments, examples, and aspects are present for aiding in understanding the background and advantages of the claimed invention.

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 provide an obstacle avoidance sufficiency proof for a path planner for which it is desirable to treat the path planner itself as an arbitrary black box. Unlike prior work in control or ACAS-X, no assumption is required on how the path planner works, nor is there any limitation on the support of paths that it can generate. This allows for verification of complex path planning techniques that do not translate to concise algebraic representations, or for which the algebraic representation would be prohibitively large and computationally intractable.

Some examples make formal statements about a path planner based on the expressivity of feasible paths it can produce. Some examples represent the sufficiency of a collection of paths to avoid any of combination of a fixed number of obstacles in a known family of obstacles as a Boolean satisfiability problem. Once this has been done, a Boolean satisfiability solver will either prove that the problem is unsatisfiable, in which case the example provides a guarantee that the path planner is sufficiently expressive that it cannot be defeated by any set of obstacles in the problem class, or it will prove that the problem is satisfiable with a particular assignment of variables that corresponds to a specific set of obstacles that block all known paths. This set of obstacles may then be fed back into the path planner, which will either declare that it is unable to solve the given problem instance (an existence proof that the path planner is insufficient to cover the problem class), or create a new path that can be added to the set of paths and treated as an additional conjunction in the Boolean satisfiability problem for the next iteration. This process proceeds until one of the two stop conditions occur, which is guaranteed to happen because the decision space is finite and bounded. In many cases, this set of operations completes far faster than the exhaustive upper bound on time.

Examples may have a number of features and advantages. Some examples codify a verification problem for a sufficiency of a path planner to generate a path that avoids obstacles in a mathematically precise geometric union as a Boolean satisfiability problem for a propositional logic expression in conjunctive normal form. Some examples utilize a path planner and Boolean satisfiability solver in adversarial roles to guide efficient search for either positive or negative proofs of path planner sufficiency. In particular, some examples utilize an algorithm for Boolean satisfiability modified to increase runtime performance in the context of repeated queries as are made by the path planner validation process when those repeated queries have an incremental structure in which each successive query adds an additional conjunctive clause to the expression. For example, some examples utilize a Boolean satisfiability algorithm modified as an incremental computational form to avoid rework in the context of an application with an adversarial constraint generator. According to some examples, such a modified Boolean satisfiability algorithm greatly enhances the runtime performance and viability of examples in handling practical problems of interest.

<FIG> is a schematic diagram <NUM> of path planning in the aerospace domain according to various examples. As shown, aircraft <NUM> includes onboard path planner <NUM>, which generates paths, such as path <NUM>, that avoid known obstacles, such as dynamic obstacle <NUM>, in reaching a destination <NUM>. Examples may be used to determine whether or not path planner <NUM> is sufficiently flexible to avoid obstacles, such as dynamic obstacle <NUM>, from a defined finite set of arbitrary obstacles. Examples may be used with any vehicle, including by way of non-limiting example, 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. Examples may be used with any type of path planner, and the internal workings of the path planner need not be known. That is, examples may be applied to any "black box" path planner.

<FIG> is a schematic diagram of a system <NUM> for establishing the sufficiency of a path planner to avoid obstacles in planning a path from a starting location to a destination location. System <NUM> may be implemented using hardware such as that shown and described herein in reference to <FIG>, for example.

System <NUM> includes geometrical partitioner <NUM>. Geometrical partitioner <NUM> decomposes a problem class describing a set of physical scenarios that could occur involving up to a fixed number of obstacles of specified geometry being encountered into countable unions over a discrete finite basis of obstacles. For example, geometric partitioning <NUM> may virtually partition the space into a number of partially overlapping space-filling polytope parts. The space may be located as being generally between a starting location of the vehicle (e.g., a present location of the vehicle) and a destination location of the vehicle (e.g., a final destination or a destination specified as some location further along the vehicle's intended route), or located within a set radius from the vehicle, by way of non-limiting examples. In two dimensions, the parts may be implemented as, by way of non-limiting examples, squares, triangles, rectangles, or hexagons. In three dimensions, such parts may be implemented as, by way of non-limiting examples, tetrahedrons, cubes, parallelepipeds, or icosahedrons. The parts of the partition may partially overlap. For example, the parts may overlap in order to ensure that any arbitrarily positioned obstacle always lies entirely within at least one part. Thus, the area (for two dimensions) or volume (for three dimensions) of overlap of the parts may be selected to enclose the profile (for two dimensions) or the entirety (for three dimensions) of a typical obstacle. Obstacles may be specified in absolute position or relative position, depending on the relevance of interaction with other constraints in the problem.

The original problem class may be specified in terms of a continuous statement that is converted into a discrete cover by geometrical partitioner <NUM>. For example, the problem class may specify a maximum number of obstacles and a volume (in three dimensions) or an area (in two dimensions) in which the obstacles may be located. An example problem class is the statement "up to N airplanes each occupying a cylindrical volume with radius R and height Z are detected between distances D<NUM> and D<NUM> away". Note in this example, the problem class includes a statement over the real numbers (any position between distances D<NUM> and D<NUM> away), while the output of geometrical partitioner <NUM> will be discrete (an overlapping mesh of some polytope) such that paths that are not members of the partition obstacles are guaranteed not to be members of any set of obstacles in the original (continuous) problem class. For example, the output may be such that, for every set of obstacles in the original (continuous) problem class, there exists a collection of members of the partition of equal number such that any point contained in an obstacle of the original (continuous) problem is contained in one of the partition members of the resulting (discrete) problem. This implies that for any collection of paths for which there does not exist a single choice of partition members (of appropriate number) such that every path of the collection intersects at least one of the chosen partition members, there cannot exist a set of obstacles in the original continuous problem class that simultaneously intersects each path in the collection. Accordingly, an infeasibility result in the discrete version of the problem is a sufficient guarantee for infeasibility for the original continuous problem.

In sum, geometrical partitioner <NUM> reduces the continuous space of vehicle operation between a starting location and a destination location to discrete space, where various obstacles may be identified with the parts of the overlapping partition in which they are located.

System <NUM> also includes path planner <NUM>. Path planner <NUM> may be an arbitrary path planner capable of taking in a starting location, a destination location, and a characterization of a set of obstacles to avoid, and either creating at least one path (trajectory) around them or declaring that the problem is infeasible within the search space and bounds of path planner <NUM>. Path planner <NUM> may be algebraic based and/or make use of mobility graphs according to various non-limiting examples. Path planner may include or be implemented in hardware such as that shown and described herein in reference to <FIG>. Path planner <NUM> may be deployed aboard the vehicle or may be deployed at a different location.

System <NUM> further includes membership interpreter <NUM>. Membership interpreter <NUM> accepts as input a path, e.g., as produced by path planner <NUM>, and a partition, e.g., as produced by geometric partitioner <NUM>. Membership interpreter <NUM> outputs a discrete union of parts of the partition that the path impinges on. Membership interpreter <NUM> may utilize any of a variety of techniques to perform the membership determination, by way of non-limiting examples, explicit comparison (e.g., halfspace inequality testing for polytope obstacles) or other geometric methods (e.g., k-d trees, Voronoi diagrams, etc.).

System <NUM> further includes incremental SAT solver <NUM>. Incremental SAT solver <NUM> determines whether there exists a set of obstacles in the partition, subject to constraints such as a maximum number of obstacles, that would block all known paths (e.g., contain at least one member of the output of membership interpreter <NUM> for all known paths). This makes use of a reduction of the geometrical collision problem to a representative Boolean satisfiability problem for a particular propositional logic formula. Incremental SAT solver <NUM> then determines whether the particular propositional logic formula is satisfiable using Boolean satisfiability techniques, such as backtracking or conflict driven search (e.g., the Davis-Putnam-Logemann-Loveland algorithm). That is, incremental SAT solver <NUM> determines whether the particular propositional logic formula is unsatisfiable, or whether there is an assignment of propositional variables corresponding to permissible obstacles that satisfies it. Its satisfaction indicates that the path is blocked by a set of obstacles corresponding to the assignment of propositional variables, and its unsatisfiability indicates that path planner <NUM> is capable of routing around the obstacles.

While a standard propositional logic satisfiability algorithm may be used for incremental SAT solver <NUM>, some examples utilize a modification of a standard SAT solver that is particularly efficient for incremental inquiries. That is, in the context of the establishing the sufficiency of a path planner problem in which repeated SAT queries will be made with additional paths added sequentially, some examples include a modification of a standard SAT algorithm to support incremental addition to the set of clauses in the propositional logic formula without requiring rebuilding any satisfiability tree rollouts. This may be accomplished by noting that since tree nodes (corresponding to propositional logic variable assignments) known to be infeasible or indeterminate stay infeasible or indeterminant (respectively), while there is at most one open node that specifies a set of propositional logic variable assignments that satisfies the problem that can be directly checked and treated as the starting point for further iteration on the appending of a clause.

System <NUM> provides elements that may be used to execute an iterative method. Briefly, according to various examples, geometrical partitioner <NUM> defines a space over which validation is possible, and then path planner <NUM> and incremental SAT solver <NUM> are the anchors in an iteration that creates additional clauses and proves (or disproves) the sufficiency of path planner <NUM>. Execution of path planner <NUM> is responsible for enlarging the set of clauses incremental SAT solver <NUM> must satisfy, while execution of incremental SAT solver <NUM> is responsible for specifying a path planning problem for path planner <NUM> to solve within the class created by geometrical partitioner <NUM>. These and other actions are described in detail presently in reference to <FIG>.

<FIG> is a flow diagram of a method <NUM> for establishing the sufficiency of a path planner to avoid obstacles in planning a path from a starting location to a destination location. Method <NUM> may be implemented by system <NUM> as shown and described herein in reference to <FIG>, e.g., using hardware <NUM> as shown and described herein in reference to <FIG>. The actions of method <NUM> may be performed in any order that enable operation.

At <NUM>, method <NUM> may begin. Method <NUM> may proceed to iterate actions <NUM>, <NUM>, <NUM>, <NUM>, and <NUM> until one of a plurality of predetermined stopping conditions occur at <NUM>. Example stopping conditions are described further below. If a stopping condition occurs, then control passes to <NUM>. Otherwise, if no stopping condition occurs, then control passes to <NUM>.

At <NUM>, method <NUM> attempts to obtain, from a path planner, such as path planner <NUM>, a path from the starting location to the destination location. The path planner may accept a start location, an end location, and description of possible obstacles (e.g., a problem class) and generate a path from the start location to the end location that either avoids the obstacles or declare that no path is possible. The starting location may be the location of the start of a trip or may be a current location of the vehicle during the trip, by way of non-limiting examples. The destination location may be the final location of the end of the trip, or may be a location at some point further along the trip than a present location of the vehicle. At the first iteration of method <NUM>, the set of obstacles provided as constraints to the path planner may be empty; that is, no obstacles may be considered by the path planner in the first iteration. Each iteration after the first may add zero or more additional obstacles as constraints to the path planner, as described below in reference to <NUM> and <NUM>. The path may be held in electronic memory and/or stored in persistent storage, for example. If the path planner cannot route around the obstacles for a given iteration, then control passes to <NUM>. Otherwise, control passes to <NUM>.

At <NUM>, method <NUM> represents the path from the starting location to the destination location as a disjunction of logical terms. According to some examples, method <NUM> may first apply a geometric partitioner, e.g., geometric partitioner <NUM>, to the space generally between the starting location and the destination location to obtain a partition of partially overlapping parts. According to such examples, method <NUM> may then apply a membership interpreter, e.g., membership interpreter <NUM>, to the path obtained at <NUM> with respect to the partition of partially overlapping parts. The membership interpreter outputs identifications of the parts in the partition that intersect the path. By way of non-limiting example, the identifications may be in the form of numerals corresponding to the respective parts for some enumeration of the parts in the partition. By way of non-limiting example, there may be hundreds, thousands, tens of thousands, hundreds of thousands, or millions of parts that intersect a particular path.

Method <NUM> then represents each part in the partition that intersects the path as a propositional variable, e.g., a literal. If the propositional variable is true, then the corresponding part is considered blocked by some obstacle; if false, then the corresponding part is considered traversable. By way of non-limiting illustrative example, if a path from the starting location to the destination location intersects parts x, y, and z, then the corresponding disjunction of logical terms may be represented as, e.g., XVYVZ, where part x corresponds to propositional variable X, part y corresponds to propositional variable Y, and part z corresponds to propositional variable Z. The disjunction of propositional variables "XVYVZ" may be interpreted as "X or Y or Z". If any of parts x, y, or z are blocked, then the corresponding propositional variable X, Y, or Z is considered true. Thus, the disjunction XVYVZ is true if, and only if, at least one of X, Y, or Z is true, which occurs if, and only if, at least one of parts x, y, and z is blocked.

At <NUM>, method <NUM> conjoins the disjunction of terms from <NUM> to a conjunction of disjunctions of terms representing previously considered paths from the starting location to the destination location. For example, for the first iteration of method <NUM>, there may be no previously considered paths, in which case the actions of <NUM> include retaining the disjunction of terms from <NUM>. As another example, for iterations after the first iteration, there may be previously considered paths from the actions of <NUM> from the previous iterations, and corresponding disjunctions of terms, in which case the disjunction from the present iteration is conjoined to the conjunction of disjunctions of terms representing previously considered paths from the starting location to the destination location. In symbols, each iteration i > <NUM> may produce a conjunction of terms, denoted <MAT> Xi,<NUM> ∨Xi,<NUM> ∨ X. ∨Xi,ni, where each Xi,j for j from <NUM> to ni is a propositional variable representing a part in the partition that intersects the path obtained at <NUM> from the i-th iteration of method <NUM>, and ni represents the number of terms in the conjunction (corresponding to the number of parts from the partition in the respective path). Then at each iteration i, the disjunction of terms Φi from that iteration is conjoined to the disjunctions from previous iterations Φi-<NUM>, Φi-<NUM>,. , Φ<NUM>, Φ<NUM> as, for example, <MAT>. The conjunction of terms may be stored in dynamic or persistent memory.

At <NUM>, method <NUM> determines a satisfiability condition, e.g., "satisfiable" or "not satisfiable", of the conjunction of terms Ψi. According to some examples, method <NUM> may apply a Boolean solver, e.g., incremental SAT solver <NUM> as shown and described herein in reference to <FIG>, to the conjunction of terms Ψi. The Boolean solver may accept as input a description of constraints, such as a maximum number of obstacles. The Boolean solver then determines whether an assignment of TRUE to a set of propositional variables in the conjunction of terms Ψi, consistent with the constraints such as a maximum number of obstacles, satisfies the conjunction of terms Ψi.

For examples that utilize an incremental SAT solver, such as incremental SAT solver <NUM>, the satisfiability determination of Ψi = Ψi-<NUM>ΛΦi may be accomplished by using the past satisfiability result for Ψi-<NUM> (and/or a corresponding past satisfiability tree representing propositional variable truth assignments) and determining the satisfiability of Φi, e.g., using backtracking or conflict driven search such as the Davis-Putnam-Logemann-Loveland algorithm. In this manner, the incremental SAT solver uses results from past iterations to expedite satisfiability determinations in a present iteration.

For a negative satisfiability condition at <NUM>, control returns to <NUM>.

For a positive satisfiability condition at <NUM>, control passes to <NUM>. At <NUM>, method <NUM> adds at least one corresponding obstacle of the plurality of obstacles as a constraint to the path planner. In more detail, as described above in reference to <NUM>, a positive satisfiability result for the conjunction <MAT> occurs if, and only if, some assignment of TRUE/FALSE to its propositional variables satisfies it. The propositional variables that are assigned TRUE in such an assignment correspond to parts in the partition that are then provided to the path planner as additional constraints at <NUM>. Control then returns to <NUM>.

At <NUM>, method determines whether one of a plurality of stopping conditions has occurred. According to some examples, two predetermined stopping conditions are possible, where a first stopping condition indicates that the path planner is sufficiently flexible to route around the obstacles described in the original problem class, and a second stopping condition indicates that the path planner is incapable of routing around the obstacles. The first stopping condition, which indicates that the path planner is sufficiently flexible to route around the obstacles, may occur when the satisfiability condition of the conjunction of terms Ψi is negative, i.e., unsatisfiable, at some stage i of the iteration. The second stopping condition, which indicates that the path planner is incapable of routing around obstacles constrained by the original problem description, may occur when the path planner fails to provide a path at <NUM> at some stage i of the iteration.

For examples in which the number of obstacles is finite, one of the two stopping conditions described above is guaranteed to occur. Iterations of method <NUM> may result in the creation of at least one new disjunction of terms at <NUM> corresponding to one new obstacle configuration, and there are a bounded number of such choices when the number of obstacles is finite, for example. In practice, method <NUM> has been shown to terminate far more quickly than the naive exhaustive bound because the adversarial nature of the interactions between the path planner and the SAT solver is exercised in increasingly challenging situations as the SAT solver learns to avoid what the path planner can solve.

For problems classes in which the number of obstacles is infinite or the volume in which they appear is unbounded, method <NUM> can still generate valid results, but it may not be formally guaranteed to result in a stopping condition as described herein. In such cases, e.g., depending on the origin of the unbounded space, literals may be lazily read off membership as they are implicitly found by the execution of the path planner, rather than attempting to explicitly allocate or pre-declare literals. Method <NUM> may terminate for such problem classes after a fixed predetermined number of iterations, for example.

At <NUM>, method <NUM> provides an indication of sufficiency of the path planner to avoid the obstacles in planning a path from the starting location to the destination location based on the stopping condition determined at <NUM>. For example, for the first stopping condition described above, method <NUM> may output an indication that the path planner is sufficient to avoid the obstacles in planning a path from the starting location to the destination location. As another example, for the second stopping condition described above, method <NUM> may output an indication that the path planner is not sufficient to avoid the obstacles in planning a path from the starting location to the destination location. Method <NUM> may output the indication in any of a variety of manners, such as displaying the indication on a computer monitor.

<FIG> is a schematic diagram of example hardware <NUM> suitable for implementing various examples. Hardware <NUM> includes vehicle <NUM>, which may be any type of computer-controllable vehicle. Vehicle <NUM> utilizes path planner <NUM>, which may be located on board vehicle <NUM> or remote from vehicle <NUM>.

Path planner <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. Path planner <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 path planner <NUM> as shown and described herein.

Path planner <NUM> includes network interface <NUM>, which communicatively couples path planner <NUM> to computer <NUM> via network <NUM>. Network <NUM> may include one or more computer networks, and may include the internet or portions thereof. According to some examples, path planner <NUM> is coupled directly to computer <NUM>. According to some examples, path planner <NUM> and computer <NUM> are implemented on the same computer.

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 computer <NUM> to at least partially perform methods disclosed herein, e.g., method <NUM>, as shown and described herein in reference to <FIG>. Computer <NUM> includes network interface <NUM>, which communicatively couples computer <NUM> to path planner <NUM> via network <NUM>. Other configurations of hardware <NUM> are possible.

Variations, modifications, and use cases for disclosed examples are many and diverse. For example, some examples may be used as part of a multi-step path planning process, in which the first step generates a bundle of high diversity paths, and the second step selects or derives a path out of that bundle. This may be accomplished by using a path planner and a SAT solver in conjunction to guide efficient exploration of the expressive space of a path planner subject to appropriate constraints.

As another example, some examples may be used as part of building a lookup table of maneuvers. In particular, when the SAT solver returns a result of "unsatisfiable", the collection of paths implicitly associated with the respective propositional logical conjunctive formula constitutes a sufficient basis such that at least one of them can be used to avoid any scenario in the problem class. The paths can then be explicitly persisted to memory, e.g., hard disk, and used at the time of vehicle operation as part of a lookup based strategy, e.g., for conflict avoidance.

Claim 1:
A method (<NUM>) of establishing a sufficiency of a path planner (<NUM>) to avoid a plurality of obstacles in planning a path from a starting location to a destination (<NUM>) location, the method comprising:
iterating, until a stopping condition of a plurality of predetermined stopping conditions occurs:
obtaining (<NUM>), from the path planner, a path (<NUM>) from the starting location to the destination location;
partitioning a space between the starting location and the destination location into a plurality of partially overlapping parts; and
representing the obstacles as a subset of the plurality of partially overlapping parts,
representing (<NUM>) the path from the starting location to the destination location as a disjunction of logical terms, wherein each of the logical terms corresponds to a part of the plurality of partially overlapping parts;
conjoining (<NUM>) the disjunction of terms to a conjunction of terms representing previously considered paths from the starting location to the destination location;
determining (<NUM>) a satisfiability condition of the conjunction of terms; and
for a positive satisfiability condition, adding (<NUM>) at least one corresponding obstacle of the plurality of obstacles to the path planner; and
providing (<NUM>) an indication of sufficiency of the path planner to avoid the obstacles in planning a path from the starting location to the destination location based on the stopping condition,
wherein the plurality of predetermined stopping conditions comprise:
a stopping condition comprising that the satisfiability condition of the conjunction of terms is negative at some stage in the iterating; and
a stopping condition comprising that the path planner fails to provide a path at some stage in the iterating.